21 Xây dựng hàm mục tiêu cho FWOMR

Trong phần này, một thuật toán NMPC được xây dựng như một bài tốn giải phương trình trình bậc hai Từ phương trình động học (1 3) và phương trình động lực học (1 16), đặt x1 q là véc tơ tọa độ trong hệ tọa độ Đề-các Oxy và x2 v là véc tơ vận tốc của robot trong hệ tọa độ liên kết, bỏ qua thành phần nhiễu bất định τ d , ta có hệ phương trình trạng thái:

1

 Hx

(3 14) Ký hiệu x1r là quỹ đạo tham chiếu Từ đó, xác định được vận tốc tham chiếu

theo cơng thức:

x2r H1x1r (3 15)

Hệ phương trình trạng thái của hệ thống (3 14) được viết gọn lại thành

T T

thống Phương trình động lực học của FWOMR được xác định bởi hàm số:

X f (X, u)

Dạng ma trận của hệ thống được mô tả như sau:

(3 16)

 x1 Hx2 0

1  f (X) g (u) (3 17)

Do đó, dạng rời rạc của hệ thống thu được bởi phương trình:

Xk1 Xk f (X)dt g (u)dt

Phương trình hệ thống thời gian rời rạc với các ràng buộc cho bởi :

(3 18) x1 2

x2 M τ Cx2 Gsig(x2 )

Xx1 2x , Xrx1r x2r là hệ phương trình trạng thái tham chiếu của hệ

Xmin Xk Xmax ; Xk , k 0,1, , N

umin uk umax ; uk , k 0,1, , Nu (3 19) Trong đó X  6 biểu thị véc tơ trạng thái và u  là véc tơ đầu vào Hàm f ( ) và g ( ) là các hàm phi tuyến, f (0) 0 , N biểu thị tầm dự báo, Nu

biểu thị giới hạn điều khiển, N Nu > 0

NMPC có thể được xây dựng như một quy trình tối ưu hóa nhiều vịng lặp Một hàm mục tiêu được tối ưu hóa để có được một véc tơ đầu vào điều khiển tối ưu Sau đó, tiến hành tính tốn (trực tuyến) lặp lại của một chức năng tối ưu hóa hàm chức năng Ưu điểm của NMPC là nó xem xét các ràng buộc của hệ thống trong q trình tối ưu hóa, dẫn đến cải thiện tính hiệu quả của tồn hệ thống

Dạng chuẩn bậc hai của hàm mục tiêu J (X,u) của bài tốn NMPC có thể được định nghĩa như sau:

J (X, u) X N 2QN N -1 i1  Nu -1 i1 ui 2 R (3 20)

Trong đó biểu thị chuẩn Ơ-clit, X N QN2 là thành phần hàm phạt giá trị cuối;

QN , Q và R là các ma trận đường chéo xác định dương, chọn tùy ý Trạng thái dự

báo ở miền đầu ra tương lai là Xi ,i 1, N , tín hiệu điều khiển dự báo ở miền đầu ra tương lai là ui ,i 1, Nu

Từ véc tơ trạng thái đặt được mơ tả bởi phương trình r X r x1

lệch bám được xác định bởi:

r T

X X r X (3 21)

Từ phương trình (3 16), hàm mục tiêu (3 20) có thể viết lại thành:

J (X, u) X N r X 2 QN N i1  r X 2 Q Nu i1 2 R (3 22)

Trong MPC, tầm dự báo tín hiệu ra N thường được chọn so với tầm dự báo tín hiệu điều khiển Nu như sau: N Nu 1

 Xi 

x2 , sai

Từ mơ hình động lực học của FWOMR, phương pháp NMPC được thực hiện để tìm chuỗi điều khiển u sao cho trạng thái hiện tại X sẽ hội tụ đến giá trị mong muốn Chọn N Nu , hàm mục tiêu trong (3 20) được viết lại như sau:

N -1

J (X, u) (X N N N i i i i i i ]

i1 (3 23)

Với:

Trong đó

Xi1 f (Xi , ui );i0, N 1

umin ui umax ;i0, N 1

(3 24) (3 25) X e X1 uu1 u 2 X2 T T 6 N (3 26) Dạng ma trận của phương trình (3 23) có thể viết lại như sau:

T

(3 27) Với Rm m , Qn n là các ma trận đối xứng xác định dương, chọn

QN Q Với các ma trận: Q 0    0 0 0 R 0 R   Q 0 0 0 0   R (3 28)

Tương tự , phương trình (3 27) có th ể viế t l ạ i ở dạ ng t ổ ng quát hơn:

Du d (3 29)

Với  I3 Nx3 N umax

I3 Nx3 Numin (3 30)

3 2 2 Tính tốn SQP

T ạ i mỗi th ời điể m lấ y mẫ u, b ộ điề u khi ển NMPC thu được các phép đo đầ u vào và đầ u ra hi ệ n t ạ i c ủ a robot, tr ạ ng thái hi ệ n t ạ i c ủa robot và mơ hình robot để tính tốn trình t ự điề u khi ể n t ối ưu trong tương lai trên một mi ề n dự báo xác định nh ằ m tối ưu hóa hàm mục tiêu đồng th ờ i th ỏ a mãn các ràng bu ộ c c ủa h ệ thống Sau đó, chỉ

 r X)T Q (X r X)[(X r X )T Q(X r X ) uT Ru X N e u N 3 N J (X, u) e X N r X QN e X N r X XT QX uT Rui  0 Q Q 0 0; R D ; d

tín hi ệu điề u khi ển đầ u tiên trong chu ỗi đượ c s ử dụng làm đầu vào để điề u khi ể n robot Trong s ố nhi ề u cách ti ế p c ận để gi ả i quyế t v ấn đề NMPC phương pháp lậ p trình tồn phương liên tiếp (SQ P) thường đượ c sử dụng Nó đủ mạ nh cho các v ấn đề thự c tế để x ử lý b ấ t k ỳ mức độ khơng tuyế n tính nào, bao g ồ m c ả sự khơng tuyế n tính trong các ràng bu ộc Ý tưởng chính c ủa SQP là nó gi ả i quyế t trong mỗ i l ầ n l ặ p l ạ i mộ t QP h ạ n ch ế b ất đẳ ng thứ c thu được b ằ ng cách tuyế n tính hóa các hàm mục tiêu và ràng bu ộc [28] Do đó, để áp d ụng phương pháp SQP , hàm m ục tiêu trong (3 23) được tuyến tính hóa để có dạ ng sau:

J (X, u) J (X(k ), u(k ))u J (X(k ), u(k ))(u u(k ))

 1 (u u(k ))T2 J (X(k ), u(k ))(u u(k )

Trong đó Jacobiu J được tính bởi cơng thức:

(3 31)



u0 u1J u N1 (3 32)

Trong trường hợp hàm mục tiêu được biể u thị như trong (3 23), xác d ịnh được:

i1

, u

i (3 33)

Xét hàm Hessianu2 J , vì hàm mục tiêu phi tuyến có dạng bình phương nhỏ

nhấ t, nên phép g ần đúng Gauss -Newton Hessian v ới đặ c tính h ội t ụ tuyế n tính có th ể được sử dụng thay vì giá tr ị chính xác để tránh tính tốn ph ứ c tạ p:

Xi r Xi 2Q  f (Xi1, ui1 r Xi ) Q2

f T Xi1(k ), ui1(k )

f Xi1(k ), ui1(k )ui1 ui1(k )

 r Xi 2 Q (3 34)  Biui1 Ci 2 Q T T T V ới:

Bif (Xi1(k ), ui1(k ));i1, N

Ci f T (Xi1(k ), ui1(k ))f (Xi1(k ), ui1(k ))ui1(k ) r Xi ;i1, N

Thay (3 24) vào (3 23), k ế t h ợ p v ới (3 34) đượ c biể u th ứ c:

(3 35) 2 u

u JJ J G

J 2 e XT Qf (Xi i ) 2Ru

uiui

V ới: N -1 N -1 i1 i1 N -1 N -1 u u i1 i1  uT Hu 2uT M N (3 36)  T 0 QB 0   T T  T 0 B2 QC2 0 0Q  T N1 i1 T (3 37)

Theo phương pháp thự c hiệ n x ấ p x ỉ Gauss-Newton Hessian, ma tr ận H đượ c sử dụng để ước lượng giá trịu2 J Từ đó, sử dụng phương pháp SQP, QP được định

dạ ng lại như sau:

u 2 (3 38)

Thỏa mãn điề u kiệ n: Du d

Bài toán 1: G ọi là t ậ p các ch ỉ số i mà ph ầ n t ử thứ i c ủa véc tơ Du d bằng 0, thì bao gồm các chỉ số của các ràng buộc hoạt động Kí hiệu là véc tơ thu được

bằng cách lấy tất cả hàng thứ i th của véc tơi Hàm Lagrange có thể được thiết lập như sau:

L(u,) J SQP (u) T (Du d )

với là h ệ số nhân Lagrange

Sau đó, các điề u kiệ n c ủa Karush-Kuhn-Tucker được đả m bả o:

(3 39)

L ( u , )

u J SQP (u)D H (u u(k)) GD 0 (3 40)

J (X, u) f (X N1, u N1) r X N Q2 N X0Q2 f (Xi1, ui1) r XiQ2 uiQ2

 BN N1 CN Q2 N X0Q2 Bi i1 CiQ2 uiQ2

 BT QB1 R 0 0 B2 2 R   H BN N R  0 0 QB  B1 1QC 0 0 M  ; N X 2   BN N 0 0 QC  Ci iQC

L ( u , )   Du d 0 (3 41)  0  T (Du d ) 0 (3 42) (3 43) Để tìm giá trị cực tiểu của u của J SQP thỏa mãn các điều kiện Karush-Kuhn-

Tucker này, chi ến lược t ậ p h ợp ho ạt động được đưa vào thự c hi ệ n Gi ả sử r ằ ng t ậ p hoạt động được đưa ra tạ i th ời điể m k tứ c thời và u(k ) là một điể m khả thi

Thuậ t toán SQP sử d ụ ng chi ến lược tậ p hợp ho ạt động ti ến hành như sau:

Bướ c 1 M ứ c t ối thi ể u c ủa đối tượng mục tiêu b ậ c hai (3 38) đối v ới các ràng

buộc trong t ậ p ho ạt động được coi là b ằng nhau được tính b ằ ng cách gi ả i:

H (u u(k )) GD

 Du d 0

(3 44) (3 45) để có được hướng c ậ p nh ậ t p u u(k )

Bướ c 2: Thự c hiện bước lớn nh ấ t có th ể theo hướng p mà không phá vỡ bất kỳ

bất đẳ ng thứ c không ho ạt độ ng nào:

u u(k )p

với 0 1 được chọn để đảm bảo rằng các ràng buộc bất đẳng thức không hoạt động vẫn khả thi

Bước 3: Nếu 0 1 , thêm một số giới hạn của bất đẳng thức vào tập hoạt

động và trả về một Nếu không, hãy thực hiện bước đầy đủ ( 1 ) và xem xét dấu

của các hệ số nhân Lagrange xem nó có thỏa mãn điều kiện (3 42) và (3 43) hay không:

- Nếu tất cả các ràng buộc về bất đẳng thức có số nhân dương, kết thúc thuật tốn

- Nếu khơng, loại bỏ bất đẳng thức có số nhân âm nhất khỏi tập hợp đang hoạt động và quay lại bước 1

Thuật toán: Giải SQP sử dụng chiến lược active set 1 2 3 4 5 6 7 8 9 10 11 [j] [j] [j] [j] [j] [j] While true do

Tối thiểu hàm mục tiêu bậc hai (3 38) thơng qua phương trình (3 42) và (3 43)

Cập nhật p u u(k )

Tính tốn số bước lớn nhất của p sao cho u u(k )p thỏa mãn điều kiện ràng buộc với 0 1

SIG ← Dấu của các nhân tử Lagrange If all(SIG) > 0

Break Else

loại bỏ bất đẳng thức có dấu âm nhất ra khỏi Active set

End If

12 End While 13 Return �̅

Bài toán 2:

- V ề mặ t lý thuyế t, s ố lầ n l ặ p ph ụ thu ộc vào b ộ kích ho ạt ban đầ u N ế u t ậ p hợp hoạt động ban đầu được chọn đúng0 , thì thuật tốn sẽ kết thúc trong lần

lặp đầ u tiên M ặ t khác, n ế u t ậ p h ợp ho ạt động ban đầu khác đáng kể so v ới t ậ p h ợp mong mu ố n, thì c ầ n ph ả i lặ p lạ i nhi ề u lần để có được kế t qu ả tối ưu

- Sau khi kết thúc bước lặp và thu được hệ quả điều khiển tối ưu u , chỉ phần tử đầ u tiên c ủa nó được s ử dụng để điề u khi ể n h ệ th ống cho FWOMR trong th ờ i gian lấ y m ẫ u này

- Sau đó, các biến đầu ra được đo lường và quá trình tương tự được th ự c hi ệ n trong th ời gian t ứ c thì ti ế p theo

Chương trình mơ phỏng được sử dụng b ởi cơng c ụ Matlab v ới các hàm toolbox đã chứ ng minh s ự h ộ i t ụ Qua đó hệ th ống đượ c xây d ự ng nh ằ m ki ể m ch ứ ng quá trình thự c hiệ n

% Nmpc_Code

function y = nmpc( u ) %NMPC cur_time = u(1); state = u(2:4); ref = u(5:10); args p(1:3) = state;

for k = 1:N %new - set the reference to track

t_pred = cur_time + (k-1)*T; pred_ref = reference(t_pred); x_ref = pred_ref(1); y_ref = pred_ref(2); theta_ref = pred_ref(3); vx_ref = pred_ref(4); vy_ref = pred_ref(5); omega_ref = pred_ref(6);

args p(6*k-2:6*k+0) = [x_ref, y_ref, theta_ref]; args p(6*k+1:6*k+3) = [vx_ref, vy_ref, omega_ref];

end

% initial value of the optimization variables

args x0 = [reshape(X0',3*(N+1),1);reshape(u0',3*N,1)];

sol = solver('x0', args x0, 'lbx', args lbx, 'ubx', args ubx, 'lbg', args lbg, 'ubg', args ubg,'p',args p); u = reshape(full(sol x(3*(N+1)+1:end))',3,N)'; y = u(1,:); end

Xét mơ hình robot được mơ tả như phương trình động học (1 3) và phương trình động lực học (1 16), để giải quyết bài toán điều khiển bám quỹ đạo với hàm mục tiêu được mơ tả bởi phương trình (3 22)

Để phân tích q trình điều khiển cho robot ta cần thiết lập điều kiện ràng buộc tín hiệu điều khiển trong khoảng sau:

Xmax  1,0 1,0

T

(3 46)

T

Thử nghiệm với vị trí khởi tạo của robot là x0 [0 0 0]T và quỹ đạo đặt của

robot được xác định bởi:

2 12 2 12 t ) t ) (3 47)

3 4 1 Mô phỏng quỹ đạo chuyển động của FWOMR sử dụng MPC

Trong phần này, luận án sử dụng phần mềm mô phỏng MATLAB nhằm đánh giá chất lượng thuật tốn đề xuất Kết quả mơ phỏng đã được thực hiện cho thấy hiệu quả của bộ điều khiển với tín hiệu điều khiển trong miền dự báo

Kết quả cũng chứng minh chương trình điều khiển hoạt động hiệu quả trong vấn đề bám quỹ đạo của robot với thời gian hữu hạn Trong bài tốn mơ phỏng, luận án lựa chọn miền dự báo khác nhau nhằm đánh giá tính hiệu quả về sự ảnh hưởng của chúng đến hoạt động của robot

Hình 3 10 Quỹ đạo di chuyển của robot với các miền dự báo

Hình 3 10 mô tả quỹ đạo di chuyển của robot với thuật toán điều khiển trong các miền dự báo khác nhau Miền dự báo lớn hơn sẽ cho robot bám nhanh hơn và

yr (t ) 5sin( x r (t ) 5cos(

umax5,0 5,0 5,0

 

mềm mại hơn vào đường quỹ đạo đặt Có thể nhận thấy với N 10 , quỹ đạo di chuyển của robot trơn hơn so với N 5

Hơn nữa, thuật toán điều khiển được thể hiện rõ hơn khi cho thấy miền dự báo lớn dẫn đến tác động điều khiển mịn hơn so với miền dự báo nhỏ (Hình 3 11)

Hình 3 11 Các tham số bộ điều khiển

Tuy nhiên, miền dự báo lớn lại làm tăng khối lượng tính tốn, vì vậy trong thực tế cần lựa chọn phần cứng của robot phù hợp nhằm đáp ứng tốt với thuật toán MPC với miền dự báo Hình 3 11 cho thấy các vận tốc v x , v y và của robot bám theo giá trị đặt trong khi vẫn thỏa mãn các điều kiện ràng buộc Các vận tốc ban đầu phía trên đường giới hạn cho đến khi tiếp cận quỹ đạo mong muốn, chúng hội tụ với các giá trị vận tốc của quỹ đạo đặt

Vận tốc theo phương x có sự thay đổi từ giây thứ 4 với đáp ứng miền thời gian

N 10 cho thấy miền dự báo lớn sẽ bám quỹ đạo gần với giá trị mong muốn hơn

Bảng 3 1 Bảng tham số vận tốc theo x

Thời gian (s) 4 5 10 15 20 25 30 35

Vận tốc (m/s) N=5 1 0,5 0 0,5 -0,8 -0,5 0 0,8

Vận tốc theo hướng y cho thấy sự thay đổi đáng kể đột ngột của robot từ giây thứ 3 xuống -0,8 m/s, tuy nhiên với miền giới hạn lớn vận tốc theo y bám dần theo quỹ đạo đặt mà ko gấp như miền giới hạn nhỏ Sự thay đổi từ giây thứ 10 quỹ đạo của cả 2 miền thời gian trùng nhau đến cuối chu kỳ tính tốn

Bảng 3 2 Bảng tham số vận tốc theo trục y

Với vận tốc góc của robot có sự biến đổi từ giây thứ 3 dao động quanh giá trị 0, nhưng có thể thấy với miền thời gian lớn N 10 quá trình quá độ ngắn hơn và sớm đạt về 0 ở giây thứ 5 so với miền thời gian N 5

Bảng 3 3 Bảng tham số vận tốc theo trục

Sai lệch bám quỹ đạo được xác định bởi công thức:  x x r x

 (3 48)

Sai lệch bám của robot được hiển thị trong Hình 3 12 Có thể thấy rằng sai lệch đang hội tụ về khơng Theo mơ hình động học của robot, sự hội tụ của x hoặc y phụ thuộc vào trạng thái ban đầu và hướng của robot

Hình 3 12 Sai lệch bám của robot trong miền dự báo

Thời gian (s) 3 5 10 15 20 25 30 35 Vận tốc (m/s) N=5 -0,8 -0,8 -0,8 -0,5 0 0,5 0,8 0,5 Vận tốc (m/s) N=10 -0,3 -0,5 -0,8 -0,5 0 0,5 0,8 0,5 Thời gian (s) 2 5 3 4 5 7 25 30 35 Vận tốc (rad/s) N=5 -0,3 0,3 0 -0,1 -0,1 0 0 0 Vận tốc (rad/s) N=10 -0,3 0,3 -0,1 0 0 0 0 0  y yr y

Như trong Hình 3 12, khi robot bắt đầu chuyển động, sai số lớn là do các trạng thái ban đầu và các ràng buộc về vận tốc, và theo thời gian, nó hội tụ về khơng Với miền dự báo lớn hơn, sai lệch bám sẽ hội tụ về 0 nhanh hơn

Sai lệch theo phương x thể hiện từ giây thứ 3 cho thấy chất lượng bám quỹ đạo của cả 2 miền thời gian cơ bản giống nhau Miền thời gian lớn sẽ tiệm cận với giá trị đặt sớm hơn khoảng 1 giây và ổn định ngay so với miền thời gian nhỏ hơn

Bảng 3 4 Sai lệch theo phương x

Sai lệch theo phương y thể hiện từ giây thứ 3 do sai lệch về hướng robot và trạng thái ban đầu nên từ khoảng 1 giây đầu robot gặp sai lệch khoảng -0 1 m, sau đó xảy ra quá trình quá độ khiến quỹ đạo di chuyển của robot trong khoảng 3 giây với biên độ cực đại 0,5 m (N=10) và 0,8m (N=5) với miền thời gian lớn sẽ có q trình q độ ngắn hơn cho thấy chất lượng bám quỹ đạo của cả 2 miền thời gian cơ bản