Sơ đồ cấu trúc bộ điều khiển MPC

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

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:

J (X,u) của bài tốn NMPC có thể được

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

⊂ 6

⊂ 3

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 (XN N −r X) + ∑[(X −i r X )iT Q(X −ir X ) + ui T Ru ]i 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ỉ

u

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 X = f (X ,u −r X ) =≈ +∇f (X(k),u (k))(u − u (k)) i 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 = ui−T1 i BTQB u + 2ui T BiTQC + Ci TiQC ;∀i ∈i [1, N ] (3.34) Với: Bi = ∇f (Xi−1(k),ui−1(k));∀i ∈[1, N ]

C =i f T (Xi−1(k),ui−1(k)) − ∇f (Xi−1(k),ui−1(k))ui−1(k) −r X ;∀i ∈i [1, N ] (3.35) Thay (3.24) vào (3.23), kết hợp với (3.34) được biểu thức:

∇u J = ∇∇ uJ ∇J ∇ J  = G

∑ ∑ 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 i−1 iQ i=1 iQ (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 iQ i=1 Với: = uT Hu + 2uT M + N BTQB + R 0 0   1 1  0 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 JSQP 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)

0

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.

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

0

 π T

Xmax =

∞ ∞ ∞ 1, 0 1, 0 4 

umax = [5, 0 5, 0 5, 0]T (3.46) Thử nghiệm với vị trí khởi tạo của robot là

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

x = [0 0 0]T

và quỹ đạo đặt của

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.