4 Thử nghiệm quỹ đạo chuyển động của FWOMR sử dụng MPC trên cơ sở

Ý tưởng cốt lõi là thích nghi và tối ưu TEB với hai tập hợp trên bởi các hàm tối ưu đa mục tiêu trong thời gian thực Hàm mục tiêu:

f B kk B k B* arg min f B B (3 5) (3 6) Trong đó f B là hàm mục tiêu tồn cục, được định nghĩa bằng tổng của các

hàm mục tiêu với trọng số tương ứng cùng với một hằng số phạt cho việc vượt qua giới hạn động học B là giá trị tối ưu của B trong thuật toán TEB, đây cũng là quỹ đạo cục bộ tối ưu cho robot

Mục tiêu của thuật tốn TEB là tìm ra giá trị B để sinh ra đường đi tối ưu cho robot Các hàm mục tiêu f k B được tính tốn nhằm giới hạn được vận tốc và gia

tốc của robot qua đó tối ưu hóa được quãng đường di chuyển đến đích của robot với quãng đường đi ngắn nhất và trong thời gian nhanh nhất đồng thời không va chạm với các vật cản trong môi trường

Ta định nghĩa hàm mục tiêu f k B dựa trên hàm mất mát e để xấp xỉ các hàm giới hạn để có thể sử dụng cho hệ điều hành ROS:

e d , d r , , S , n

  0

r

(3 7)

Trong đó dr là giá trị giới hạn; S, n thể hiện độ chia và bậc của đa thức; là hằng số phạt (thường chọn là số dương nhỏ)

 d dn

 ; d d r  S

Hình 3 6 minh họa các điểm đặt mong muốn và vật cản trên quãng đường di chuyển của robot, dựa vào việc tối ưu các hàm mục tiêu thuật toán TEB sinh ra đường đi tốt nhất cho robot bám theo quỹ đạo mong muốn đồng thời tránh các vật cản trên đường đi

Dải đàn hồi thời gian được định nghĩa là một quỹ đạo tính tốn cho robot từ vị trí hiện tại tới vị trí mong muốn trong vùng đặt trước Các vị trí mong muốn này tham chiếu từ quỹ đạo toàn cục để đảm bảo cho robot di chuyển hướng tới vị trí đích Dải đàn hồi này sẽ thay đổi linh hoạt để vừa tránh các vật cản mà vẫn đảm bảo việc robot sẽ đi qua những điểm đặt mong muốn (Waypoint) là các điểm ký hiệu WP1, WP2, WP3, và WP4

Hình 3 6 Quỹ đạo tính tốn bởi TEB

Để tìm ra được các điểm đặt mong muốn (WP) và tránh được các vật cản thuật toán TEB sử dụng 2 hàm mục tiêu là:

 path e dmin , rwp max , , S , n

ob edmin ,ro min , , S , n

(3 8) (3 9) Trong đó dmin là khoảng cách nhỏ nhất từ vị trí của robot đến điểm đặt hoặc vật cản, rwp max là giới hạn khoảng cách lớn nhất của robot so với điểm đặt, ro min giới hạn khoảng cách an toàn của robot đến vật cản Thuật toán TEB sinh ra quỹ đạo tránh các vật cản động cũng như vật cản tĩnh trong quá trình di chuyển của robot đồng thời tính ra quỹ đạo ngắn nhất cho robot di chuyển đến đích Cả hai hàm mục tiêu có dạng xấp xỉ giống nhau

Với hàm mục tiêu quãng đường, các điểm đặt mong muốn sẽ thu hút dải đàn hồi trong khi các vật cản làm nó phải thay đổi hình dạng để có thể sinh ra quỹ đạo tránh vật cản đó Hàm mục tiêu dựa trên khoảng cách tối thiểu d min, j giữa dải dàn hồi

với các điểm mong muốn hoặc vật cản j như Hình 3 7

Hình 3 7 Khoảng cách ngắn nhất giữa TEB với vật cản (hoặc điểm đặt) Thuật tốn TEB tính ra đường đi nhanh nhất cho robot thơng qua việc tính tốn tối ưu cực tiểu min ftime với:

B time n  i1 2 (3 10) Bên cạnh đó việc tính tốn vận tốc dài và vận tốc góc của robot dựa vào khoảng cách giữa 2 vị trí qk và qk1 trong khoảng thời gian di chuyển làTk

vk 1 xk+1 xk

Tk yk+1 yk (3 11)

k   k+1    k

Tk (3 12)

Tương tự ta có gia tốc của robot có dạng

ak 2 vk+1   v k 

TkTk+1

(3 13) Tương tự ta tính được gia tốc góc của robot bằng việc thay thế vận tốc dài vk

bằng vận tốc góc ωk , với r là bán kính của bánh xe  Ti

Hình 3 8 là cấu trúc điều khiển cho FWOMR sử dụng thuật toán TEB Tại bước khởi tạo, thuật tốn TEB điều chỉnh độ phân giải về khơng gian và thời gian cho quỹ đạo của robot thông qua các trạng thái của robot, sau đó liên kết các điểm đặt trên quỹ đạo đường đi của robot và vật cản trong môi trường với các từng trạng thái của thuật tốn TEB

Hình 3 8 Cấu trúc thuật tốn lập kế hoạch di chuyển cục bộ TEB

Dựa vào việc nhận thức về môi trường và đường đi của robot, các hàm mục tiêu được hình thành và liên kết với nhau, việc tối ưu các hàm mục tiêu sẽ hình thành nên quỹ đạo đường đi tốt nhất cho robot đồng thời có thể tránh các vật cản tĩnh và động trong suốt quá trình di chuyển

Dựa vào mơ hình động học của robot, các trạng thái của robot tại mỗi thời điểm sẽ chuyển thành các tín hiệu điều khiển về vận tốc và gia tốc cho robot Sau mỗi vịng lặp, vị trí mới của robot và vật cản trong bản đồ toàn cục được cập nhật lại để thuật tốn hình thành đường đi tiếp theo cho robot

Bộ điều khiển dự báo MPC được thiết kế dựa trên thuật tốn đã được trình bày trong mục 1 2 4 Một bộ điều khiển MPC được đề xuất cho bài toán điều hướng quỹ đạo của robot di động đa hướng dựa trên tuyến tính hóa mơ hình động học để dự đốn các trạng thái của hệ thống trong tương lai và hạn chế về tín hiệu điều khiển được tính đến [77] Alborzi và cộng sự trình bày một khái niệm về phương tiện ảo và bộ điều khiển dự báo mơ hình được kết hợp để đảm bảo rằng robot di động có thể đi theo quỹ đạo mong muốn đồng thời đáp ứng các hạn chế về chuyển động [8] Điều hướng là nhiệm vụ chính nhưng quan trọng đối với bất kỳ hệ thống robot di động tự động nào Luận án xem xét vấn đề thiết kế bộ điều khiển hiệu quả cho robot di động đi theo quỹ đạo đã thiết kế trước đó của hệ thống dẫn đường có khả năng tránh vật cản tĩnh và động Nhờ khả năng dự đoán trạng thái tương lai của robot, bộ điều khiển MPC đặc biệt thích hợp cho các vấn đề tránh chướng ngại vật trong mơi trường bất định (Hình…)

Do đó, robot có thể xác định chướng ngại vật và tính tốn các tín hiệu điều khiển tối ưu để đổi hướng nhanh hơn so với các thuật toán trước đây [76], [79] MPC dựa trên ngăn xếp điều hướng đề xuất thực thi dựa trên nền tảng hệ điều hành ROS cho FWOMR được thiết kế hoàn toàn trực quan để hoạt động tương tự như hệ thống thực tế và được đặt trong môi trường mô phỏng bằng phần mềm Gazebo, giám sát trực quan trên Rviz với nhiều loại vật cản

3 2 1 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 quá trình tối ưu hóa, dẫn đến cải thiện tính hiệu quả của toà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 toá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: