THIẾT KẾ BỘ ĐIỀU KHIỂN TỐI ƯU H∞ CHO HỆ MÁY BAY KHÔNG NGƯỜI LÁI (UAV) ỨNG DỤNG TRONG PHÁT HIỆN CHÁY RỪNG
Lê Duy Tùng, Đỗ Trọng Tấn*, Nguyễn Văn Đưa Trung tâm Công nghệ Vi điện tử và Tin học, viện Ứng dụng Công nghệ
* Tác giả liên hệ: [email protected] Ngày tòa soạn nhận được bài báo: 03/09/2020
Ngày phản biện đánh giá và sửa chữa: 02/12/2020 Ngày bài báo được duyệt đăng: 25/03/2021 Tóm tắt:
Với sự phát triển mạnh mẽ về công nghệ, các thiết bị bay không người lái đang được ứng dụng ngày càng rộng rãi, hỗ trợ con người làm việc, hoạt động tại các môi trường nguy hiểm,… Cùng với đó là yêu cầu về các thuật toán điều khiển ngày càng tinh vi hơn, để hỗ trợ các thiết bị này thực hiện chính xác nhiệm vụ được giao. Bài báo trình bày về phương pháp thiết kế bộ điều khiển tối ưu, kết hợp bám quỹ đạo cho thiết bị bay không người lái phục vụ cho việc phát hiện cháy rừng. Các kết quả tính toán được tiến hành mô phỏng trên Matlab và đạt được các kết quả tốt.
Từ khóa: Thiết bị bay không người lái, điều khiển tối ưu.
1. Đặt vấn đề
Trong hai thập kỷ qua, ngoại trừ trong ngành năng lượng chưa có nhiều đột phá, về phần cứng như: các thiết bị cảm biến và vi xử lý có tốc độ, độ chính xác ngày càng cao trong khi giá thành giảm, công nghệ viễn thông và định vị cực kỳ phát triển và đã ứng dụng vào dân sự như truyền dữ liệu tốc độ cao & GPS, công nghệ chế tạo phôi với thành công tiêu biểu là dân sự hóa máy in 3D, giá đang ở mức hợp lý và đang giảm đã khiến cho việc đưa UAV vào áp dụng thực tế chỉ còn bước thiết kế bộ điều khiển & lập trình. Cũng chính vì điều này, một lượng lớn các nghiên cứu về điều khiển UAV được công bố, tập trung chủ yếu vào việc giữ thăng bằng ổn định trên không và chuyển động tới điểm mong muốn. Các kỹ thuật điều khiển tiêu biểu có thể kể đến: Các bộ điều khiển phản hồi đơn giản như PD, PID [1], điều khiển trượt [2], điều khiển dự báo [3], [4], điều khiển thích nghi bền vững [5], [6], hoặc các bộ điều khiển LQR cho mô hình đã được tuyến tính hóa [7],…. Đặc điểm các bộ điều khiển này là dễ dàng áp dụng vào thực tế nhưng chất lượng có thể không tốt do nhiễu ngoại lực và nhiễu bất định. Các bộ điều khiển nâng cao sử dụng lý thuyết ổn định Lyapunov và điều khiển trượt kết hợp back-stepping nhằm giúp hệ kín ổn định tiệm cận [8]. Đặc điểm của những bộ điều khiển loại này là có thể đạt tới điểm đặt, có tính bền vững (với điều khiển trượt) nhưng yêu cầu tính toán cao trong thiết kế và áp dụng thực tiễn. Các bộ điều khiển nâng cao sử dụng điều khiển thích nghi bền vững nhằm nâng cao khả năng chịu nhiễu của UAV như tác động của gió, ngoại lực khác,.... Các bộ điều khiển khác sử dụng lý thuyết Điều khiển mờ, Mạng nơron hay Trí thông minh nhân tạo (AI) [9]. Các phương pháp điều khiển đều đã đạt được kết quả ổn định.
Trong bài báo này, chúng tôi nghiên cứu bộ điều khiển tối ưu H∞ cho dòng UAV quadrotor nhằm đảm bảo sự ổn định và bám theo quỹ đạo đã thiết
kế. Bên cạnh đó, đối với UAV loại nhỏ (miniature) có nội lực tác động lên quadrotor là khá nhỏ so với lực do tín hiệu điều khiển tác động, do vậy trong mô hình động học của UAV loại nhỏ hoàn toàn có thể không xét tới sự thiếu hụt cơ cấu chấp hành mà bộ điều khiển thiết kế trên mô hình đó vẫn đảm bảo chất lượng động học. Nội dung nghiên cứu được trình bày dưới đây và được thể hiện qua các kết quả mô phỏng đáng tin cậy.
2. Nội dung nghiên cứu 2.1. Xây dựng mô hình UAV
Hình 2.1. Mô hình thu gọn của UAV Coi máy bay là một khối cứng hình chữ thập.
Đặt B=
{
B B B1, ,2 3}
là hệ tọa độ gắn liền với khung máy bay, trong đó B1 là trục nằm trên hướng bay bình thường của máy bay, B2vuông góc với B1 và và hướng sang phải, B3 có chiều thẳng đứng hướng lên trên, vuông góc mặt phẳng B OB1 2. Hệ quy chiếu quán tính I ={
E E Ex, ,y z}
gắn với trái đất.Vector ξ =
[
x y z]
T biểu diễn cho vị trí của tâm khối máy bay trong hệ tọa độ trái đất. Định hướng máy bay thông qua một ma trận chuyển RI :B→I,với RI∈SO(3) là một ma trận trực giao. Ma trận chuyển có thể thu được thông qua ba phép quay liên tiếp quanh ba trục của hệ tọa độ máy bay. Trong bài báo này, các góc Euler XYZ cố định được sử dụng để mô tả sự quay của UAV đối với mặt đất. Các góc này cần thỏa mãn điều kiện:
Góc roll, φ :
2 2
π φ π
− < <
Góc pitch, θ :
2 2
π θ π
− < <
Góc yaw, ψ :
(
− < <π ψ π)
Từ ba phép quay trên, ma trận chuyển hệ tọa độ từ B→I là:
11 12 13
21 22 23
31 32 33
I
R R R
R R R R
R R R
=
(1)
Với:
11 12 13 21 22 23 31 32 33
cos cos
cos sin sin sin cos cos sin cos sin sin sin cos
sin sin sin cos cos sin sin cos cos sin
sin cos sin cos cos R
R R R R R R R R
ψ θ
ψ θ ϕ ψ φ
ψ θ φ ψ φ
ψ θ
ψ θ φ ψ φ
ψ θ φ ψ φ
θ
θ φ
θ φ
=
= −
= +
=
= +
= −
= −
=
=
Phương trình động học của chuyển động quay và tịnh tiến có được bằng ma trận chuyển. Động học tịnh tiến có thể được viết như sau:
I I. B
v =R v (2) Với vI =
[
u0 v0 w0]
T và vB =[
uL vL wL]
Tcác vận tốc tuyến tính của tâm khối máy bay biểu diễn trong hệ tọa độ trái đất và hệ tọa độ máy bay.
Các động học quay có được như sau:
Wη1
η= −ω (3) 1 sin tan cos tan
0 cos sin
0 sin sec cos sec p q r
φ φ θ φ θ
θ φ φ
ψ φ θ φ θ
= −
Trong đó η=
[
φ θ ψ]
T và ω=[
p q r]
Tlà các vận tốc góc trong hệ tọa độ máy bay.Các phương trình động học máy bay có thể được biểu diễn bằng công thức Euler - Lagrange dựa trên động năng và thế năng:
i i
f d L L
dt q q
ξ
τη
∂ ∂
= −
∂ ∂
(4) ( , ) Ctrans Crot p
L q q =E +E −E (5) Trong đó:
L là hàm Lagrange của mô hình máy bay
Ctrans
E là động năng chuyển động tịnh tiến
ECrotlà động năng chuyển động quay Ep là tổng thế năng
[ ]
T 6q= ξ η′ ′ ∈ℜ là vector các tọa độ
η 3
τ ∈ℜ biểu diễn các mômen roll, pitch, yaw
I ˆ T
fξ =R f +α là lực tịnh tiến đặt lên máy bay do đầu vào điều khiển chính U1 hướng trục z với
3 12
I ˆ Ie
R f R U= .
T Ax Ay Az
α = là các vector lực khí động học, có thành phần trên các trục E E Ex, ,y z. Các lực khí động học được coi như là nhiễu loạn bên ngoài.
Do hàm Lagrange không chứa các thành phần động năng có sự kết hợp của η và ξ, nên phương trình Euler - Lagrange có thể được chia thành 2 phần là động học tịnh tiến và động học quay. Chuyển động tịnh tiến có thể được biểu diễn bởi phương trình sau:
mξ + mge3 = fξ (6) Phương trình (5) có thể biểu diễn bằng vector trạng thái ξ:
( )
( )
( )
1
1
1
1 cos sin cos sin sin 1 sin sin cos cos sin
1 cos cos
x
y
z
x U A
m m
y U A
m m
z g U A
m m
ψ θ φ ψ φ
ψ θ φ ψ φ
θ φ
= + +
= − +
= − + +
(7)
Trong đó: m là khối lượng máy bay và g là gia tốc trọng trường. Phương trình động năng quay biểu diễn các chuyển động quay là hàm của η. Đặt Wη
là ma trận Jacobian từ ω sang η trong (2), ta có:
( ) Wη η′JWη
ℑ = ℑ = (8) Trong đó: J là momen quán tính.
Phương trình động năng quay:
1
Crot 2
E = η η ′ℑ (9) Từ biểu diễn trên, phương trình chuyển động quay Euler-Lagrange theo η có thể viết như sau:
( ) ( , )
M η η+Cη η η τ = η (10) Với: M( )η = ℑ( )η :
11 12 13
21 22 23
31 32 33
( )
M M M
M M M M
M M M
η
=
Trong đó:
( )
( )
11 12 13 21
2 2
22 23 31 32
2 2 2 2 2
33
0 sin 0
cos sin
cos sin cos sin
cos sin cos
sin sin cos cos cos
xx
xx
yy zz
yy zz
xx
yy zz
xx yy zz
M I
M
M I
M
M I I
M I I
M I
M I I
M I I I
θ
φ φ
φ φ θ
θ
φ φ θ
φ φ θ φ θ
=
=
= −
=
= +
= −
= −
= −
= + +
và:
11 12 13
21 22 23
31 32 33
( , )
C C C
C C C C
C C C
η η
=
Trong đó:
11 12 2
2 13 2
21 2
2
22 23
0
( )( cos sin sin cos )
( ) cos cos cos
( ) cos sin cos
( )( cos sin sin cos )
( ) cos cos cos
( ) cos sin
yy zz
zz yy xx
zz xx
zz yy
yy zz xx
zz yy
xx
C
C I I
I I I
C I I
C I I
I I I
C I I
C I
θ φ φ ψ φ θ
ψ φ θ ψ θ
ψ φ φ θ
θ φ φ ψ φ θ
ψ φ θ ψ θ
φ φ φ
=
= − +
+ − −
= −
= − +
+ − +
= −
= −
2 2
sin cos sin cos sin cos cos sin
yy zz
I I
ψ θ θ ψ φ θ θ
ψ φ θ θ
+ +
31 ( yy zz) cos cos sin2 xx cos C = I −I ψ θ φ φ−I θ θ
32 2
2
2 2
( )( cos sin sin sin cos )
( ) cos cos sin cos
sin sin cos cos cos sin
zz yy
yy zz xx
yy zz
C I I
I I I
I I
θ φ φ θ φ φ θ
φ φ θ ψ θ θ
ψ φ θ θ ψ φ θ θ
= − +
+ − +
− −
2 2
33
2
( ) cos sin cos sin cos sin cos cos sin cos sin
yy zz yy
zz xx
C I I I
I I
φ φ φ θ θ φ θ θ
θ φ θ θ θ θ θ
= − −
− +
Do đó, mô hình toán học (sử dụng cho tổng hợp bộ điều khiển) mô tả chuyển động quay của quadrotor thông qua phương trình Euler - Lagrange là [9]:
( )
( )1 ( , )
M η C
η= η − τ − η η η (11) 2.2. Thiết kế bộ điều khiển
Để có được quỹ đạo bám cho quadrotor thì cần phải kết hợp 2 yếu tố đó là khả năng điều khiển dưới sự tác động của nhiễu ngoài, tham số bất định và sự không mô hình động lực học được. Đề xuất sách lược điều khiển dựa trên cấu trúc phân tán của quadrotor được mô tả ở Hình 2.2.
Hình 2.2. Cấu trúc điều khiển UAV
Đầu tiên, quỹ đạo đặt cho chuyển động tịnh tiến được cung cấp từ thông tin của khu vực có nhiệt độ bất thường thông qua các camera nhiệt được đặt trên các đỉnh núi của Khu di tích lịch sử Đền Hùng. Từ thông số có được của camera, máy tính
nhúng tiến hành tính toán và xây dựng quỹ đạo tối ưu cho UAV di chuyển đến khu vực bị nghi ngờ.
Quỹ đạo này được truyền từ máy tính nhúng xuống UAV thông qua module Telemetry Radio. Sau đó, dựa trên đường đi x y zr, ,r rmong muốn của chuyển động tịnh tiến cùng các đạo hàm của chúng ta sẽ tính được các tín hiệu đặt cho đầu vào điều khiển
1r, ,xr yr
U u u . Góc yaw đặt xét sau, không liên quan gì đến vòng điều khiển ngoài. Quỹ đạo này được tạo ra với giả thiết: không có nhiễu ngoài nào tác động vào thiết bị ảo và trạng thái máy bay là ổn định.
Một bộ điều khiển dự báo được dùng để điều khiển chuyển động tịnh tiến của quadrotor ở vòng ngoài, sử dụng tín hiệu được cung cấp bởi khối Quỹ đạo khởi tạo. Bộ điều khiển dự báo không gian trạng thái dựa trên sai lệch mô hình (E-SSPC) [10] cũng bao gồm thành phần tích phân của sai lệch vị trí trong vector trạng thái để đạt được trạng thái ổn định khi các nhiễu loạn liên tục tác động.
Điều khiển chuyển động tịnh tiến được thực hiện trong hai giai đoạn. Đầu tiên điều khiển độ cao z của quadrotor và tổng lực đẩy U1 là tín hiệu tác động, giai đoạn hai, tín hiệu đặt của các góc pitch và roll (tương ứng θ φr, r) được tính thông qua hai đầu vào ảo theo yêu cầu của chuyển động theo tọa độ
( )
x y, . Trong bước hai này, biến điều khiển U1 được sử dụng như là một tham số biến đổi theo thời gian.Cuối cùng, bộ điều khiển H∞cho hệ con chuyển động quay được sử dụng ở vòng trong để làm ổn định quadrotor. Vị trí và tốc độ góc được điều khiển trong vòng này thông qua các momen xoắn trên ba trục tọa độ, τηa = τφa τθa τψa′là các biến tác động. Để đạt được trạng thái ổn định sai lệch không trong khi có tác động liên tục của nhiễu ngoài, tích phân của sai lệch vị trí góc cũng được xét đến. Do cấu trúc nối tầng của sách lược này và có tính đến hiệu suất vòng kín đạt được bởi vòng điều khiển H∞, các góc Euler có thể được xét đến như các tham số biến đổi theo thời gian trong thiết kế của bộ điều khiển tịnh tiến.
a. Điều khiển phi tuyến H∞cho hệ con chuyển động quay
Mô hình động học chuyển động quay có được từ công thức Euler - Lagrange được sử dụng để phát triển bộ điều khiển phi tuyến H∞, khi đó, τη có thể được viết thành:
a d
η η η
τ =τ +τ (12) Trong đó τηa là vector momen xoắn và τηd là biểu diễn cho tổng ảnh hưởng của sai lệch mô hình hệ thống cùng các nhiễu ngoài.
Xét vector sai lệch:
( )
r r r
x
dt dt
η
η η η
η η η
η η η
−
= = −
−
∫
∫
(13)
Trong đó ηr và ηr∈ℜn là quỹ đạo mong muốn và
vận tốc tương ứng, để ý rằng một thành phần tích phân đã được thêm vào trong vector sai lệch. Thành phần này sẽ cho phép đạt được trạng thái ổn định sai lệch không khi các thành phần nhiễu tác động liên tục lên hệ thống.
Ta xem xét cấu trúc điều khiển sau cho hệ con chuyển động quay [4]:
( )
1 1
1 1
( ) ( , ) ( ) ( , )
a M C T M Tx C Tx T u
η η η
τ = η η+ η η η − − η + η η + −
Ta thấy τηa có thể được chia làm 3 phần: phần thứ (14) nhất bao gồm hai biểu thức đầu, là phần động học của hệ thống. Phần thứ hai có 2 thành phần chứa vector sai lệch xη và đạo hàm của nó xη. Giả sử
d 0
τη ≡ thì hai thành phần này của luật điều khiển cho phép bám quỹ đạo đạo đặt 1 cách hoàn hảo.
Cuối cùng, phần thứ 3 gồm một vector u, biểu diễn cho điều khiển để loại bỏ nhiễu.
Ma trận T có thể được viết thành:
[
1 2 3]
T = T T T
trong đó, T1=ρI, với ρ là dương, vô hướng và I là ma trận đơn vị bậc n.
Thay luật điều khiển (13) vào (11) và kí hiệu
1 1
( ) ( ) d
d M= ηT M− η τη , ta được:
( ) ( , )
M η Txη+Cη ηTxη = +u d (15) Công thức trên biểu diễn cho phương trình động học của sai lệch hệ thống. Nhìn vào phương trình phi tuyến này, vấn đề điều khiển phi tuyến H∞có thể được đặt ra như sau:
“Tìm một luật điều khiển u t( ) mà tỷ lệ giữa năng lượng của biến chi phí ζ =Wh x uT( )η TT và năng lượng của các tín hiệu nhiễu d nhỏ hơn mức độ suy giảm γ cho trước”.
Từ việc xem xét ma trận W, ta xét cấu trúc cho ma trận Q và S:
1 12 13
12 2 23
13 23 3
Q Q Q
Q Q Q Q
Q Q Q
=
, 12
3
S
S S
S
=
Để áp dụng các kết quả lý thuyết, ta cần viết lại phương trình động học phi tuyến của sai lệch theo mẫu tiêu chuẩn của điều khiển phi tuyến H∞thành dạng sau:
( , ) ( , ) ( , )
xη = f x tη +g x t u k x t dη + η (16) Trong đó:
1
1 1 1 1
0 1 1 2 1 2 3 0
( ) ( , ) 0 0
( , ) ( )
0
M C
f x t T T I T T I T T T T x
I I
η η
η − η η
− − − −
−
= − − + −
−
1 01
( )
( , ) ( , ) 0
0 M g x tη k x tη T
η −
−
= =
với:
1 2 3
0 0
0 0 T T T
T I I
I
=
(17)
Như đã nói ở trên, nghiệm của phương trình HJB [10] phụ thuộc vào việc lựa chọn hàm chi phí ζ mà cụ thể là lựa chọn hàm h x( )η . Ở đây, ta chọn
( )
h xη =xη. Sau đó, việc tính toán luật điều khiển u sẽ yêu cầu chọn hàm Lyapunov V x t
( )
η, .Định lý sau sẽ giúp ta thực hiện điều này:
Định lý: Cho hàm vô hướng V x t
( )
η, :0 0
( ) 0 0
( , ) 1 0
2 0
T T
M
V x t x T Y X Y T x
X Y Z Y
η η η
η
= −
− +
(18)
Trong đó X Y Z, , ∈ℜn n× là các ma trận hằng, đối xứng xác định dương sao cho Z XY X− −1 +2X >0 và T0 được định nghĩa ở (16). T là ma trận trong (14). Nếu các ma trận này thỏa mãn:
1 2
2 2
2
1 T ( T )T ( T ) 0
O Y X
Y X Z X Q
X Z X O
T T S T R S T γ
−
+ +
+
+ − + + =
(19)
Thì khi đó thì hàm V x t
( )
η, ở (25) sẽ là 1 nghiệm của phương trình HJB với một giá trị của γ . Định lý này có được từ [2].Chứng minh:
Xét hệ:
( ) ( , ) d
M q q N q q+ = +τ τ Với: N q q( , ) =C q q q F q G q( , ) + ( ) + ( )
Trong đó q R∈ n là biến trạng thái, q là đạo hàm của nó. Giả sử 2 vector này đều có thể đo được.
Vector τ là tín hiệu đầu vào của hệ thống, τd biểu diễn tổng nhiễu và sai lệch hệ thống. Ma trận quán tính M(q) đối xứng, xác định dương, C q q q( , ) là vector hướng tâm và Coriolis, F q( ) biểu diễn ma sát và G(q) là trọng lực.
Như ta đã biết thì ma trận C q q( , ) không phải là duy nhất nên để cho tiện, chúng ta có thể viết C q q( , ) thành:
( , ) 1 ( , ) ( , ) C q q = 2M q q N q q + Trong đó M q q( , ) và N q q( , ) được tính:
ij ij
1
ij n ij
k k k
M M
M d M q q
dt q = q
∂ ∂
= = =
∂
∑
∂
1
1 2
n ik ij
ij k
k j i
M M
N q
q q
=
∂ ∂
=
∑
∂ − ∂ Xét hàm:0 0
0 0
( , ) 1 0
2 0
T T
M
V x t x T Y X Y T x X Y Z Y
= −
− +
Hàm V(x,t) xác định dương khi và chỉ khi:
0 0
0 0
0 M
Y X Y
X Y Z Y
− >
− +
Khi ma trận quán tính M đối xứng, xác định dương và giả thiết các ma trận X Y Z R, , ∈ n n* là các ma trận hằng, đối xứng, xác định dương thì bất đẳng thức trên thỏa mãn khi và chỉ khi:
( ) 1( ) 0
Z Y+ − X Y Y X Y− − − >
Hay: Z XY X− −1 +2X >0 chính là giả thiết.
Tiếp theo, ta cũng thấy rằng V x t
( )
, là 1 nghiệm cho phương trình Hamilton-Jacobi:( )
11 2
1
, 1 ( )( ) ( )
2
1 1 ( , ) ( , ) ( , ) ( , ) 2
( , ) ( ) 0
T T T
T T T
T
V V f x t h x Q SR S h x t x
V k x t k x t g x t R g x t x
V g x t R S h x x
γ
−
−
−
∂ +∂ + −
∂ ∂
+ ∂∂ −
×∂ =
∂
Vector vi phân theo x của V x t
( )
, :[ ]
0 1* 1*
0 0
( , ) 0 1 0 0
0 2
T T T
n n
V x t x T M Y X Y T
x X Y Z Y
∂ ∂ = − +− + Ω
Trong đó Ω ∈R1*n bằng:
1
0 0 0 0
0 0 0 0
0 0 0 ,...., 0 0 0
0 0 0 0 0 0
T T
n
T T T T
M M
q q
x T T x x T T x
∂ ∂
∂ ∂
Dễ dàng thấy:
[
01*nΩ01*n][
g x t u k x t( , ) + ( , )ω]
=0 Hệ quả là:[ ]
0 0 1* 1*
0 0
( , ) 0 1 0 0
0 2
T T T
n n
V x t f x T M Y X Y T f
x X Y X Y
∂ ∂ = − −+ + Ω
0 0 1* 1*
1* 1*
0 0 1* 1*
0 0
0 1[0 0 ]
0 2
1 [0 0 ]( )
2
0 0
0 1[0 0 ]
0 2
T T n n
n n
T T n n
M
x T Y X Y T f f
X Y X Y gu k M
x T Y X Y T f x
X Y X Y ω
= − + Ω
− +
+ Ω +
= − + Ω
− +
Thay vào ta có:
0 0 0
1
2 3 2 0
0 0 0 0
0 0
0 0
(1 ) 0 0
12 (1 ) 1( )
0
T T T T
M M
x T Y X Y T f x T Y X Y
X Y X Y X Y X Y
M M N
I I T I T T T x
I I
ρ ρ ρ
−
− = −
− + − +
− +
× − − − −
−
2 3 2 0
2 3 2
(1 ) 0 0
21 1 1 ( )
1( ) 1( ) ( ) 1( )( )
T T o
M N
x T X YT X Y T T T x
X Y X Z X Y T X Z X Y T T
ργ ρ ρ
ρ ρ ρ
− +
= − − − −
− + − − − + − − −
Và:
( )
1* 1* 1* 1*
1
0 0
1[0 0 ] 1[0 0 ] 1
2 2 2
0 0
1 0 0 0
2 0 0 0
n n n n
n T
k rk
k k
T T
e
x e e
e M q q q
x T T x
=
Ω = Ω = Ω
∂ −
∂
=
∑
Đạo hàm V x t
( )
, theo thời gian:1
0 0
0 0
1 0 0 0
2 0 0 0
n T
k k rk
T T
qM q
V x T T x
t
=
∂
∂
∂ =
∂
∑
Thay vào biểu thức bên trên ta được:
( , ) V TV x t f
t x
∂ +∂
∂ ∂
0 0 1* 1*
0 0
0 1[0 0 ]
0 2
T T n n
V x T M Y X Y T f x
t X Y Z Y
∂
= ∂ + − +− + Ω
0 0
1
0 0
0 0
0 0
0 0
1 0 0 0
2 0 0 0
T T
n T
k
k k
T T
M
x T Y X Y T f
X Y Z Y qM q
x T T x
=
= −
− +
∂
∂
+
∑
0 2 3 2 0
2 3 2
0 0
1 1 1 ( )
1( ) 1( ) ( ) (1 )( )
T T
N
x T Y X YT X Y T T T x
X Y X Z X Y T X Z X Y T T
ρ ρ ρ
ρ ρ ρ
−
= − − − −
− + − − − + − − −
Tính toán thấy N là ma trận nghiêng đối xứng, và do cấu trúc đặc biệt của T0, ta có :
0 0
0 0
0 0 0 0
0 0 0
T T
N
x T T x
−
=
Thay phương trình trên ta được:
( , ) V TV x t f
t x
∂ +∂
∂ ∂
0 2 3 2 0
2 3 2
0 0
1 1 1 ( )
1( ) 1( ) ( ) (1 )( )
T T
N
x T Y X YT X Y T T T x
X Y X Z X Y T X Z X Y T T
ρ ρ ρ
ρ ρ ρ
−
= − − − −
− + − − − + − − −
0 0 0 0
1 0 0 2
2 2 0 0 0 0
1 0 2
2 2 0
T
T
Y X
x Y X X X Z x
X X Z
Y X
x Y X Z X x
X X Z
= + +
+
= +
+
Biểu thức g x tT( , ) V x t( , ) x
∂
∂ được tính bởi:
1 1
0 0 0
0
0 0
( , )
( , ) [ 0 0](T ) 0 0 [I 0 0]T
T T T
V x t M
g x t M T Y X Y T x
x X Y Z Y
x Tx
− −
∂ ∂ = − +−
= =
Thêm vào đó, ta có g x t
( )
, =k x t( )
, nên:1 2
1 2
1 1 ( , ) ( , ) ( , ) ( , ) 2
1 1
2
T T T
T T
V k x t k x t g x t R g x t V
x x
x T I R Tx γ
γ
−
−
∂ ∂
−
∂ ∂
= −
Cuối cùng, thay thế biểu thức đã tính được ở trên vào phương trình Hamilton-Jacobi ở đầu với điều kiện h x
( )
=x, ta được:1 2
1 1
1 0 2 1 1
2 2
2 0
1 ( ) 0
2
T T T
T T T T T
Y X
x Y X Z X x x T I R Tx
X X Z
x T R S x x Q SR S x γ
−
− −
+ + −
+
− + − =
Đơn giản hóa biểu thức này, ta có được điều phải
chứng minh. Trở lại với bài toán quadrotor:
Ma trận T=
[
T T T1 2 3]
được tính bằng cách giải một số phương trình toán học Riccati sau:Tính T1:
1 1 1 1
1 2 1 1 1 1 1 1 1 1
(1 ) 0
T T T T
T I R T S R T T R S S R S Q γ
− − − −
− − − − + =
Tính T3:
1 1 1 1
3 2 3 3 3 3 3 3 3 3
(1 ) 0
T T T T
T γ I R T S R T T R S S R S Q
− − − −
− − − − + =
Tính X:
1 1 1 1
1T 12 3 1 3 1T 3T 1 3T 13
X T γ R T S R T T R S S R S Q
− − − −
= − − − − − +
Tính T2:
1 1 1 1
2 2 2 2 2 2 2 2 2 2
1 2 0
T T T T
T γ I R T S R T T R S− − − S R S Q− X
− − − − + + =
Thay V x t
( )
η, vào (14), luật điều khiển u* tương ứng với chỉ số tối ưu H∞,γ là:* 1( )
u = −R S T x− ′+ η (20) Cuối cùng, thay luật điều khiển trên vào (17), cùng với một vài biến đổi, luật điều khiển tối ưu trở thành:
( )
( )
*a M( ) r C( , ) M KD KP KI dt τη = η η + η η η − η η+ η−
∫
η Trong đó: (21)1 1 1 1
1 2 1 1 1
1 1 1 1
1 3 2 2 2
1 1 1 1
1 3 3 3
( ( ) ( , ) ( ) ( ))
( ( ) ( , ) ( ) ( ))
( ( ) ( , ) ( ) ( ))
D P I
K T T M C T M R S T
K T T M C T M R S T
K T M C T M R S T
η η η η
η η η η
η η η η
− − − −
− − − −
− − − −
= + + ′+
= + + ′+
′
= + +
Trường hợp đặc biệt khi các thành phần của W’W thỏa mãn:
2 2 2 2
1 1 2 2 3 3
12 13 23 1 2 3
, , , ,
,
Q I Q I Q I R uI
Q Q Q O S S S O
ω ω ω ω
= = = =
= = = = = = (22)
Khi đó:
22 1 3 1
1 2
2 ( ) ( , ) 1
D
u
K ω ω ω I M C I
η η η
ω ω
+ −
= + +
22 1 3 1
3 2
1 1
1
3 2
1
2 ( ) ( , ) 1
( ) ( , ) 1
P
u
I
u
K I M C I
K M C I
ω ω ω
ω η η η
ω ω ω
ω η η η
ω ω
−
−
+
= + +
= +
Trong đó, các tham số ω ω ω1, ,2 3 và ωu có thể được chỉnh định.
Các biểu thức này có một đặc điểm quan trọng:
chúng không phụ thuộc vào tham số γ, vậy nên chúng ta có thể có được một biểu thức đại số để tính toán giải pháp tối ưu cho trường hợp cụ thể này. Bài mô phỏng bên dưới đây của chúng em sẽ mô phỏng cho trường hợp này.
b. E-SSPC cho bám quỹ đạo
Trong phần này, luật điều khiển được thiết kế để giải quyết vấn đề bám quỹ đạo đặt. Xét một không gian trạng thái MPC tuyến tính dựa trên sai lệch mô hình
(E-SSPC), từ sai lệch mô hình, hai bộ điều khiển dự báo được tổng hợp. Bộ thứ nhất điều khiển độ cao thông qua U1 còn bộ thứ hai sử dụng tín hiệu này tạo ra một tham số theo thời gian trong các chuyển động tuyến tính theo trục x và y để tính hai đầu vào ảo u ux, y.
Ta viết lại (7) trong không gian trạng thái theo mẫu:
( )t f( ( ), ( ))t u tξ
ξ = ξ
để thiết kế bộ điều khiển, với
[
0 0 0]
( )t x t u t( ) ( ) y t( ) v t( ) z t( ) w ( )t T
ξ = là
vector không gian trạng thái của hệ, u0(t), v0(t), w0(t) là các thành phần cấu thành nên vận tốc tuyến tính của tâm quadrotor trong biểu diễn hệ tọa độ nội.
Từ (7) và vector không gian trạng thái mới, phương trình động học của hệ dùng để thiết kế điều khiển có thể được viết thành:
0 1
1
0 1
( ) ( ( ), ( )) ( )
( ) ( ) ( )
( ) ( ) ( ) ( )
w ( )
( ) ( ) (cos ( )cos ( ))
x x
o y y
z
t f t u t u t
A t u t U t
m m
v t U t A t
u t m m
t
U t A t
g t t
m m
ξ ξ ξ
θ φ
=
+
=
+
− + +
(23)
Với:
( ) cos ( )sin ( )cos ( ) sin ( )sin ( ) ( ) sin ( )sin ( )cos ( ) cos ( )sin ( )
x y
u t t t t t t
u t t t t t t
ψ θ φ ψ φ
ψ θ φ ψ φ
+
−
(24)
Phương trình (7) cho thấy rằng chuyển động thông qua các trục x và y phụ thuộc vào đầu vào điều khiển U1. Thực tế thì U1 là độ lớn tổng lực đẩy được thiết kế để đạt được chuyển động tuyến tính mong muốn, trong khi u ux, y có thể được xem như là các hướng của U1 gây ra các chuyển động theo các trục x và y.
Mục tiêu của cách tiếp cận này là đảm bảo rằng UAV bám theo một quỹ đạo đặt định trước và giảm thiều tối đa sai lệch. Tuy nhiên, do thực tế các tọa độ đặt thay đổi theo thời gian nên ta cần một thiết bị ảo có mô hình giống với quadrotor:
( ( ), ( ))
r f r t u tξr
ξ = ξ (25) Trong đó:
[
0 0 0]
( ) ( ) ( ) ( ) ( ) ( ) w ( )
r t x t u tr r y t v t z tr r r r t
ξ = ′
Và
yr 1
r( ) xr r
u tξ = u u U ′
là các trạng thái đặt và đầu vào điều khiển tương ứng. Giả thiết không có các nhiễu ngoài trong thiết bị ảo. Thiết bị ảo này giúp ta có được các đầu vào điều khiển đặt cho các chuyển động tịnh tiến với giả thiết rằng độ cao của quadrotor đã ổn định. Do đó, trong trường hợp này, các giá trị đặt là:
( )
1r . r
U =m z +g
1 1
. , .
r r
r r
r r
x y
x m y m
u u
U U
= =
Bằng cách trừ các tham chiếu ảo (24) cho (22) ta có sai lệch mô hình của chuyển động tịnh tiến như sau:
( )t A t( ). ( )t B t u t( ). ( )ξ
ξ = ξ + (26) Trong đó, ξ( )t =ξ( )t −ξr( )t là vector sai lệch và
( ) ( ) r( )
u tξ =u t u tξ − ξ là sai lêch đầu vào điều khiển.
Các ma trận A(t) và B(t) là các ma trận Jacobi của hệ thống (22). Hơn nữa, thành phần tích phân của sai lệch vị trí cũng đã được gộp vào trong vector sai lệch để thực hiện bám quỹ đạo khi bị tác động liên tục của các nhiễu loạn.
Xét vector sai lệch:
0 0 0
0 0 0
0 0 0
( ) ( ) ( )
( ) ( ) ( )
( ) ( ( ) ( ))
( ) ( ) ( )
( ) ( ) ( )
( )
( ) ( ( ) ( ))
( ) ( ) ( )
w ( ) w ( ) w ( )
( ) ( ( )
r r r
r r r r r r
x t x t x t
u t u t u t
x t dt x t x t dt y t y t y t v t v t v t x t
y t dt y t y t dt z t z t z t
t t t
z t dt x t z
ξ
−
−
−
−
−
= =
−
−
−
−
∫ ∫
∫ ∫
∫
( ))t dt
∫
(27)
Sử dụng phương pháp Euler, ta có mô hình tuyến tính rời rạc theo thời gian:
( 1) . ( ) ( ). ( )
x kξ + =A x kξ +B k u kξ (28) Đầu vào điều khiển U t1( ) được xem như là một tham số phụ thuộc thời gian cho chuyển động đặt theo x và y. Hơn nữa, do cấu trúc điều khiển phân cấp, các góc roll, pitch và yaw cũng được coi là các tham số phụ thuộc thời gian.
Sai lệch mô hình (25) có thể được chia thành hai hệ con: sai lệch cao độ và sai lệch chuyển động theo x và y. Các ma trận A B, cho mỗi hệ con biểu diễn như sau:
1 0 0
0 1 0 , cos ( )cos ( )
0 1 0
z z
t t
A B k k
t m
θ φ
∆ ∆
= =
∆
(29)
1
1
0 0
1 0 0 0 0
( ) 0
0 1 0 0 0 0
0 1 0 0 0 0 0
0 0 0 1 0 , 0 0
0 0 0 0 1 0 0 ( )
0 0 0 0 1
0 0
xy xy
t tU k
t m
A B
t
tU k t m
∆ ∆
∆
= ∆ =
∆
∆
(30)
Với ∆t là thời gian lấy mẫu được chọn đủ nhỏ để có thể giữ lại được tất cả sai lệch của chuyện động tịnh tiến nhưng cũng phải đủ lớn để chuyển động quay ở trạng thái ổn định.
Dựa trên những phân tích này, vấn đề bám quỹ đạo cho UAV có thể được hiểu là: tìm các đầu vào điều khiển trong khoảng giá trị giới hạn có thể để điều khiển các biến trạng thái trong (25) từ một vị trí đầu xξ0 đến gốc tọa độ. Cho nên, từ sai lệch độ cao và di chuyển ngang, các luật điều khiển được thiết kế để khiến cho hệ buộc phải bám theo quỹ đạo đặt.
Luật điều khiển đầu tiên tính toán đầu vào U1 sao cho hàm chi phí sau nhỏ nhất:
(
2 2)
ˆ ˆ ˆ ˆ
ˆ ˆ ˆ ˆ
ˆ ( | ) ˆ ( | )
T T
z z rz z z rz z rz z z rz
z z rz z
J x x Q x x u u R u u x k N k x k N k
ξ ξ ξ ξ ξ ξ ξ ξ
ξ ξ
= − − + − −
+Ω + − +
Trong đó, các ma trận khối lượng Q Rz, z là các ma (31) trận đường chéo, xác định dương, N2z là cửa sổ dự báo và Ω là chi phí trạng thái cuối:
(
2 2)
2 2
2 2
ˆ ( | ) ˆ ( | )
ˆ ( | ) ˆ ( | )
ˆ ( | ) ˆ ( | )
z z rz z
T
z z rz z
z z z rz z
x k N k x k N k x k N k x k N k P x k N k x k N k
ξ ξ
ξ ξ
ξ ξ
Ω + − +
= + − +
× + − +
Với Pz ≥0.
Các dự báo của đầu ra mô hình x k j kˆξz( + | ) được tính bằng cách tuyến tính hóa mô hình không gian trạng thái theo thời gian của quadrotor bởi các phương trình (25) và (26), ta được:
ˆz z( | ). ( | )z z( | ).ˆz
xξ =P k k x k kξ +H k k uξ (32) Trong đó u k kξz( | )=U k U k1( )− 1r( ) và x kξz( ) là vector trạng thái sai lệch độ cao.
Các vector độ cao đặt là:
2
1 1
1 1
( 1| ) ( | ) ˆ
( 1| ) ( | )
( | ) ( 1| ) ˆ
( 1| ) ( 1| )
rz rz
rz
rz z r
