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à:

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 tồn của robot đến vật cản. Thuật tố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.

 path  edmin , rwp max , , S, n (3.8)

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

dmin, 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

 n 2

time   Ti 

 i1  (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 v  1 xk+1  xk k T y  y k k+1 k (3.11) k+1  k k Tk (3.12)

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

a k  2vk+1  vk 

T  Tk k+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.

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 toá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 toá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 có hệ phương trình trạng thái:

τd , ta

Ký hiệu

x1

r

là quỹ đạo tham chiếu. Từ đó, xác định được vận tốc tham chiếu theo cơng thức:

x  H 1x

2r 1r (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

X x1 x2T ,

Xr  x1r x2r

T là hệ phương trình trạng thái tham chiếu của hệ 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) (3.16)

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

X  x1 

  Hx2   0   f (X)  g(u) 

x  M 1 Cx  Gsig(x ) M 1τ

 2   2 2    (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 (3.18)

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 :

x1  Hx2 

x  M 1 τ  Cx  Gsig(x )

 2 2 2

 6

 3

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

umin  uk  umax ;uk  , k  0,1,..., Nu

(3.19) Trong đó X  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 định nghĩa như sau:

2 N -1 2 Nu -1 2

J (X,u)  XN Q N   Xi Q   ui R

i1 i1 (3.20)

Trong đó . biểu thị chuẩn Ơ-clit, 2

N QN 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à tương lai là ui ,i  1, Nu .

Xi ,i  1, N , tín hiệu điều khiển dự báo ở miền đầu ra

T

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

1 2 

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

e 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:

r 2 N r 2 Nu 2

J (X,u)  XN  X Q   Xi  Xi   u j

N i1 Q i1 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

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)  (XN  r X)T Q N (XN  r X)  [(X  i r X )i T Q(X  i r X )  ui Ti Ru ]i

i1 (3.23)

Với: Xi1  f (Xi ,ui );i 0, N 1 (3.24)

umin  ui  umax ;i 0, N 1 (3.25) Trong đó X   e X e X ... e X T  6N  1 2 N  u  u u ... u T  3N 1 2 N (3.26) Dạng ma trận của phương trình (3.23) có thể viết lại như sau:

J (X,u)   e X  r XT Q  e X  r X  XTQX  uT Ru N N N i i (3.27) Với QN  Q . R  m  m, Q  n  n là các ma trận đối xứng xác định dương, chọn Với các ma trận: Q 0 0  R 0 0   0 Q 0   0 R 0  Q   ; R         0 0 Q  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 D   I3Nx3N  ; d  umax  I  u   3Nx3N   min  (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ỉ

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 (SQP) 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)

2 u

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

Trong trường hợp hàm mục tiêu được biểu thị như trong (3.23), xác dịnh được: J  2 e XT Q f (Xi ,ui )

 2Ru

ui i1 ui i (3.33)

Xét hàm Hessian 2 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:

f T X (k),u (k) 2 i1 i1 2 2 X  r Xi  f (X ,u  r X )  f X (k),u (k)u  u (k) i Q i1 i1 i Q i1 i1 i1 i1  r X i Q  B u  C 2 i i1 i Q  uTi1 i BTQB u  2ui T BTiQC  Ci TQC ;i i i 1, N  (3.34) Với: Bi  f (Xi1(k),ui1(k));i 1, N 

C  f T (X (k),u (k))  f (X (k),u (k))u (k)  r X ;i 1, N

i i1 i1 i1 i1 i1 i

(3.35)

Thay (3.24) vào (3.23), kết hợp với (3.34) được biểu thức:

u

 J    J  J  J   G

u u

N -1 N -1 J (X,u)  f (X ,u )  r X 2  X 2   f (X ,u )  r X 2  u 2 N 1 N 1 NQN 0Q i1 i1 iQ iQ i1 i1 (3.36)  B u C 2 X 2 N -1  B u C 2 N -1u 2 N N 1 NQN 0Q i i1 i1 iQ i1 iQ Với:  uT Hu  2uT M  N BTQB  R 0 0   1 01 BTQB  R 0  H  2 2       0 0 BT QB  R N N  BTQC 0 0   1 1   0 BTQC 0  2 N 1 T M   2 2 ; N  X0Q   Ci QCi   i1  0 0 BNT QC N  (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ị dạng lại như sau:

 

2 J . Từ đó, sử dụng

phương pháp SQP, QP được định

min J (u)  G(u  u(k))  1 (u  u(k))T H (u 

u(k))

u SQP 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ứ ith của véc tơ.i  .

Hàm Lagrange có thể được thiết lập như sau:

L(u, )  JSQP (u)  T (Du  d ) (3.39) với  là hệ số nhân Lagrange.

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

L ( u , )

 J (u)  D  H (u  u(k))  G  D  0

u SQP

L(u,)

 Du  d  0

 (3.41)

  0 (3.42)

T (Du  d )  0 (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:

để 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.

H (u  u(k))  G 

D

(3.44)

Thuật toán: Giải SQP sử dụng chiến lược active set

1 Đầu vào:khởi tạo X  X[j] X[j] ...X[j]  , u  u[j] u[j] ... u[j]  , j  0

j  1 2 N  j  1 2 N 1  2 While true do

3 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)

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

5 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.

6 SIG ← Dấu của các nhân tử Lagrange

7 If all(SIG) > 0

8 Break

9 Else

10 loại bỏ bất đẳng thức có dấu âm nhất ra khỏi Active set 11 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    , 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); end

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];

% 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,:);