Professional Documents
Culture Documents
Xây dựng thuật toán điều khiển UAV
Xây dựng thuật toán điều khiển UAV
1. MỞ ĐẦU
Máy bay không người lái UAV dạng Tri-rotors là một chủng loại UAV có
cấu tạo đơn giản, xong điều khiển nó lại có tính phức tạp hơn so với các chủng loại
UAV khác. Trong các công trình [1,2] đã xây dựng mô hình động học cho chuyển
động của Tri-rotors. Trong công trình [3] nhóm tác giả đã đưa hệ phương trình mô
tả quá trình điều khiển bay Tri-rotors về dạng affine. Trong công trình này chúng
tôi sẽ áp dụng lý thuyết điều khiển Backstepping để tổng hợp lệnh điều khiển
chuyển động cho UAV dạng Tri-rotors.
2. XÂY DỰNG THUẬT TOÁN ĐIỀU KHIỂN
CHUYỂN ĐỘNG TRI-ROTORS
Trên các hình từ H.1 đến H.3 mô tả UAV dạng Tri-rotor và các hệ tọa độ
dùng để khảo sát chuyển động của Tri-rotor trong không gian.
Hình 1. Mô hình của Tri- Hình 2. Hệ tọa độ sử dụng Hình 3. Góc nghiêng
rotors. trong mô hình toán UAV. động cơ cánh quạt.
Trong [3] đã xây dựng mô hình mô tả chuyển động cho tri-rotors theo quan
điểm điều khiển với các tham số đặc trưng sau:
x x1 x4
X 1 y x2 ; X 2 x5
(1a)
z x3 x6
p x7 u x10
X 3 q x8 ; X 4 v x11
(1b)
r x9 w x12
Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san TĐH, 04 - 2019 152
Tên lửa & Thiết bị bay
Trong đó là véc tơ vận tốc tâm khối Tri-rotors trong hệ tọa độ mặt đất;
là véc tơ tốc độ quay của Tri-rotors với các thành phần p, q, r ; , , là ba góc
tư thế của Tri-rotors so với hệ tọa độ mặt đất; x , y , z là tọa độ tâm khối Tri-
rotors trong hệ tọa độ mặt đất. Hệ phương trình mô tả chuyển động tâm khối và
chuyển động quay được đưa về dạng affine như sau:
X RX 1 4 (2)
X 2 X 3 (3)
X 3 F3 ( X 3 ) B3U (4)
X 4 F4 ( X 2 , X 3 , X 4 ) B4U (5)
trong đó:
s ( x4 )c( x5 ) s ( x5 ) s ( x4 )c( x5 )
R c( x4 ) s ( x5 )c( x6 ) s ( x4 ) s ( x6 ) c( x5 )c( x6 ) s ( x4 ) s ( x5 )c( x6 ) c( x4 ) s ( x6 ) (6)
c( x4 ) s ( x5 ) s ( x6 ) s ( x4 ) s ( x6 ) c( x5 ) s ( x6 ) s ( x4 ) s ( x5 ) s ( x6 ) c( x4 )c( x6 )
I 2 I3
x8 x9
I
s ( x5 )c( x4 ) s ( x5 ) s ( x4 ) c( x5 ) f 31 ( X 3 ) 1
1 I I
c( x5 ) s ( x4 ) c( x5 )c( x4 ) 0 ; F3 ( X 3 ) f 32 ( X 3 ) 3 1 x7 x9 (7)
x5 I
c( x4 ) s ( x4 ) 0 f 33 ( X 3 ) 2
I1 I 2
x7 x8
I3
f 41 ( X 2 , X 3 , X 4 )
F4 ( X 2 , X 3 , X 4 ) f 42 ( X 2 , X 3 , X 4 ) (8)
f 43 ( X 2 , X 3 , X 4 )
f 41 ( X 2 , X 3 , X 4 ) x9 x11 x8 x12 g s( x6 ) s( x4 ) gc( x6 )c( x4 ) s( x5 ) (9)
f 42 ( X 2 , X 3 , X 4 ) x7 x12 x9 x10 gc( x4 ) s ( x6 ) gc( x6 ) s ( x4 ) s ( x5 ) (10)
f 43 ( X 2 , X 3 , X 4 ) x8 x10 x7 x11 gc( x6 )c( x5 ) (11)
k 3l k f 3l
0 0 0 0 f
2 I1 2 I1
k l kfl kfl
B3 0 0 0 f (12)
I2 2I2 2I2
kfl kfl kfl 0 0 0
I3 I3 I3
153 Đ. V. Thành, T. Đ. Thuận, “Ứng dụng giải thuật backstepping … UAV dạng tri-rotors.”
Nghiên cứu khoa học công nghệ
k 3 kf 3
0 f 0 0 0
2m 2m
k 3 k 3 kf 3
f f 0 0 0
B4 m 2m 2m (13)
kf kf kf
0 0 0
m m m
2
u1 1 sin(1 )
u 2 sin( )
2 2 2
u3 3 sin( 3 )
2
U 2 (14)
4 1 cos (1 )
u
u 2
5 2 cos ( )
2
u6 3 cos ( 3 )
2
Ở đây c(.) , s (.) là hàm số cos (.) và sin(.) ; I1 , I 2 , I 3 -mô men quán tính của
Tri-rotors theo các trục 1, trục 2, trục 3; l -khoảng cách từ tâm cánh quạt đến tâm
Tri-rotors; 1 , 2 , 3 -tốc độ quay cánh quạt Tri-rotors; k f -hệ số tỉ lệ thể hiện
quan hệ giữa lực tạo ra của một cánh quạt với tốc độ quay của cánh quạt, tức là:
Fi k f i2 , i 1, 2, 3 (15)
Ở đây coi ba cánh quạt tương đương nhau, nên các hệ số tỉ lệ bằng nhau. Vì vậy
hệ số k f dùng chung cho cả 3 cánh quạt; m -khối lượng Tri-rotors; các góc 1 ,
2 , 3 là ba góc nghiêng của động cơ cánh quạt, có ba động cơ điện để thay đổi
các góc này. Để thay đổi các thành phần U1, U2, U3, U4, U5, U6 của véc tơ điều
khiển U có thể thay đổi ba góc nghiêng này cùng với tốc độ quay 1 , 2 , 3 của ba
cánh quạt.
Việc điều khiển ở đây chính là xác định quy luật thay đổi véc tơ U để tâm khối
và tư thế Tri-rotors bám theo quy luật mong muốn, tức là làm cho X 1 và X 2 bám
theo véc tơ đặt trước X 1d , X 2d , tức là:
X 1 X 1d ; X 2 X 2d hoặc ( X 1 X 1d ) 0 ; ( X 2 X 2d ) 0 (16)
Với cách đặt biến như (1a), (1b) và từ các biểu thức (2), (3), (4), (5), (6), (7) cho
thấy: hệ động học (2) và (3) không phụ thuộc trực tiếp vào véc tơ điều khiển U ,
mà phụ thuộc vào hai véc tơ trạng thái X 4 và X 3 của hai hệ động học sau. Từ đặc
điểm này cho phép sử dụng giải thuật backstepping để tổng hợp lệnh điều khiển
U sao cho đạt yêu cầu (16).
Hai phương trình (2), (3) có thể được viết dưới dạng véc tơ mở rộng sau:
X 1 0 R( X 2 ) X 3
(17)
X 2 ( X 2 ) 0 X 4
Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san TĐH, 04 - 2019 154
Tên lửa & Thiết bị bay
ở đây 0 là ma trận kích thước (3 X 3) với tất cả phần tử đều băng 0 . Từ (17) cho
tấy véc tơ trạng thái ( X 3 X 4 )T có thể coi là véc tơ điều khiển (véc tơ đầu vào) của
hệ động học (17). Định nghĩa véc tơ sai lệch bám như sau:
Z11 X 1 X 1d
Z1 2 d , tức là: Z11 X 1 X 1d , Z12 X 2 X 2d (18)
Z1 X 2 X 2
Tiến hành phép lấy đạo hàm đối với véc tơ sai lệch bám Z1 nhận được:
X 1 X 1d 0 R( X 2 ) X 3 X 1d
Z1 d (19)
0 X 4 X 2d
X 2 X 2 ( X 2 )
Đặt điều khiển ảo kích thước (4X1) như sau:
1 (20)
2
Véc tơ sai lệch vec tơ điều khiển ảo khi đó sẽ là:
X3 X 3
Z2 1 (21)
X4 X 4 2
Khi đó véc tơ điều khiển ảo sẽ được tính như sau:
X X
1 3 Z2 ; 3 Z2 1 Z2 (22)
2 X 4 X4 2
Xây dựng hàm Lyapunov cho hệ động học (19) như sau:
1
V1 Z1T Z1 (23)
2
Tiến hành phép lấy đạo hàm theo biến thời gian đối với hàm V1 có:
X 1 X 1d
V1 Z1T Z1 Z1T ( d ) (24)
X 2 X 2
Thay đạo hàm Z1 bằng vế phải biểu thức (19) nhận được:
0 R( X 2 ) X 3 X 1d
V1 Z1T ( ) (25)
( X 2 ) 0 X 4 X 2d
Không thay đổi giá trị V1 khi thêm và bớt vào biểu thức (25) véc tơ c1Z1 (trong
đó c1 là hằng số dương được chọn):
T
0 R( X 2 ) X 3 X 1d
V1 Z1 (c1Z1 c1Z1 ) (26)
( X 2 ) 0 X 4 X 2d
Thay véc tơ ( X 3 X 4 )T tính theo biểu thức (22) vào (26) và triển khai tiếp
nhận được:
155 Đ. V. Thành, T. Đ. Thuận, “Ứng dụng giải thuật backstepping … UAV dạng tri-rotors.”
Nghiên cứu khoa học công nghệ
0 R( X 2 ) X 1d
T T
V1 c Z Z1 Z (c1Z1 (Z 2 ) d )
1 1 1
( X 2 ) 0 X 2
T T
0 R( X 2 )
c1Z1 Z1 Z1 Z2 (27)
( X 2 ) 0
T
0 R( X 2 ) X 1d
Z1 (c1Z1 d )
( X 2 ) 0 X 2
Nếu điều khiển ảo được xác định sao cho véc tơ thứ hai trong tích của số
hạng thứ ba ở vế phải (27) bằng 0, tức là:
0 R( X 2 ) X 1d
c1Z1 d 0 (28)
( X 2 ) 0 X 2
hoặc:
0 R( X 2 ) X 1d
d c1Z1 (29)
( X 2 ) 0 X 2
thì:
0 R( X 2 )
V1 c1Z1T Z1 Z1T Z2 (30)
( X 2 ) 0
Triển khai phương trình (29) với cách phân chia biến theo biểu thức (18), (20) có:
R( X 2 ) 2 X 1d z11 (31)
( X ) X z
2 1 2
d
1
2
(32)
Vậy điều khiển ảo được xác định như sau:
1 1 ( X 2 )( X 2d z12 ) 1 ( X 2 )( X 2 z1 )
1 d 2
;
1 (33)
2 R 1 ( X 2 )( X 1d z11 ) 2 R ( X 2 )( X 1 z1 )
d 1
Tiếp tục xây dựng hệ động học cho sai lệch vec tơ điều khiển ảo:
X 3
Z 2 (34)
X 4
Xây dựng hàm Lyapunov cho cả hai hệ động học (19) và (34) như sau:
1
V2 V1 Z 2T Z 2 (35)
2
Tiến hành phép lấy đạo hàm theo biến thời gian đối với hàm V2 có:
V V Z T Z
2 1 2 2 (36)
Thay V1 bằng vế phải phương trình (30), thay Z 2 bằng vế phải phương trình
(34) vào phương trình (36) nhận được:
0 R( X 2 ) X 3
V2 c1Z1 Z1 Z1 Z 2 Z 2 ( )
T T T
(37)
( X 2 ) 0 X 4
Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san TĐH, 04 - 2019 156
Tên lửa & Thiết bị bay
Không thay đổi giá trị V2 khi thêm và bớt vào biểu thức (37) véc tơ c2 Z 2 (trong
đó c2 là hằng số dương được chọn) nhận được:
0 R( X 2 ) X 3
V2 c1Z1 Z1 Z1 Z 2 Z 2 (c2 Z 2 c2 Z 2 )
T T T
( X 2 ) 0 X 4
(38)
0 R ( X ) X
Z 2 Z 2 (c2 Z 2 )
2 3
c1Z1T Z1 c2 Z 2T Z 2 Z1T T
( X 2 ) 0
X 4
Thay véc tơ ( X X )T trong biểu tức (38) bằng vế phải hai phương trình (4) và
3 4
V2 0 (43)
Chứng minh: Ký hiệu các phần tử của hai véc tơ Z1 và Z 2 như sau:
Z1 ( z1(1) z2(1) z3(1) z4(1) z5(1) z6(1) )T ; Z 2 ( z1(2) z2(2) z3(2) z4(2) z5(2) z6(2) )T (44)
Tiếp tục ký hiệu các phấn tử cúa hai ma trận R( X 2 ) và ( X 2 ) như sau:
157 Đ. V. Thành, T. Đ. Thuận, “Ứng dụng giải thuật backstepping … UAV dạng tri-rotors.”
Nghiên cứu khoa học công nghệ
0 0 0 r11 r12 r13 z1(2) r11 z4(2) r12 z5(2) r13 z6(2)
0 0 0 r r r z r z r z r z
21 22 23 2(2) 21 4( 2) 22 5(2) 23 6(2)
0 R( X 2 ) 0 0 0 r31 r32 r33 3(2) z r z
31 4(2) r z
32 5(2) r z
33 6(2)
Z2
( X 2 ) 0
11 12 13 0 0 0 z
4(2) z
11 1(2) z
12 2(2) z
13 3(2)
0 0 0 z
21 22 23 21 z1(2) 22 z2(2) 23 z3(2)
5(2)
31 32 33 0 0 0 z6(2) 31 z1(2) 32 z2(2) 33 z3(2)
Khi đó:
0 R ( X 2 ) 0 R( X 2 )
Z1T Z 2 ( z1(1) z2(1) z3(1) z4(1) z5(1) z6(1) ) Z2
( X 2 ) 0 ( X 2 ) 0
r11 z4(2) r12 z5(2) r13 z6(2)
r21 z4(2) r22 z5(2) r23 z6(2) (48)
r z r z r z
31 4(2) 32 5(2) 33 6(2)
( z1(1) z2(1) z3(1) z4(1) z5(1) z6(1) )
3
11 z1(2) 12 z2(2) 13 z3(2)
21 z1(2) 22 z2(2) 23 z3(2)
z z z
31 1(2) 32 2(2) 33 3(2)
Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san TĐH, 04 - 2019 158
Tên lửa & Thiết bị bay
159 Đ. V. Thành, T. Đ. Thuận, “Ứng dụng giải thuật backstepping … UAV dạng tri-rotors.”
Nghiên cứu khoa học công nghệ
Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san TĐH, 04 - 2019 160
Tên lửa & Thiết bị bay
Hình 4. Tín hiệu điều khiển U1. Hình 5. Sự thay đổi góc (chúc góc) .
Hình 6. Tín hiệu điều khiển U2. Hình 7. Sự thay đổi góc (hướng).
Hình 8. Tín hiệu điều khiển U3. Hình 9. Sự thay đổi góc (cren).
161 Đ. V. Thành, T. Đ. Thuận, “Ứng dụng giải thuật backstepping … UAV dạng tri-rotors.”
Nghiên cứu khoa học công nghệ
Tọa độ đk - m
Hình 10. Tín hiệu điều khiển U4. Hình 11. Tâm khối theo trục x.
Tọa độ đk - m
Hình 12. Tín hiệu điều khiển U5. Hình 13. Tâm khối theo trục y.
Tọa độ đk - m
Hình 14. Tín hiệu điều khiển U6. Hình 15. Tâm khối theo trục z.
4. KẾT LUẬN
Bằng giải thuật cuốn chiếu Backstepping đã xác định được luật điều khiển tư
thế và tâm khối Tri-rotors bám theo quỹ đạo mong muốn. Bằng diễn giải toán học
đã xác định được các luật điểu khiển, xác định các hằng số trong luật điều khiển
Backstepping. Các kết quả mô phỏng cho thấy sự hợp lý của thuật toán đề xuất.
TÀI LIỆU THAM KHẢO
[1]. Hoàng Quang Chính, Nguyễn Công Toàn, “Nghiên cứu xây dựng mô hình
toán và mô phỏng UAV tri-rotor”, Tuyển tập công trình Hội nghị toàn quốc
lần thứ 2 về Điều khiển và Tự động hóa- VCCA-2013, tr 556-563.
Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san TĐH, 04 - 2019 162
Tên lửa & Thiết bị bay
[2]. Đặng Văn Thành, Trần Đức Thuận, "Khảo sat chuyển động UAV tri-rotor trong
trường hợp một động cơ thay đổi tốc độ và góc nghiêng", Tạp chí Nghiên cứu
KH&CN quân sự, số 52, 12-2017.
[3]. Dong-Wan Yoo, “Dynamic Modeling and Control System Design for Tri-
rotor UAV”, Proceedings of the 2010 3rd International Symposium on
System and Control in Aeronautics and Astronautics, 2010.
[4]. Nguyễn Doãn Phước, “Phân tích và điều khiển hệ phi tuyến”, NXB Bách
khoa, 2012.
[5]. Nguyễn Doãn Phước, “Lý thuyết điều khiển nâng cao”, NXB Khoa học và Kỹ
thuật, 2009.
ABSTRACT
APPLICATION SOLUTIONS BACKSTEPPING BUILDING ALTERNATIVES
OF MOTION CONTROL FOR THE UAV FORM-ROTORS
Presentation of the analysis and transformation of Tri-rotor kinematic
models when building an affine control system model to help apply the modern
control theory of Tri-rotor control law.
Keywords: UAV; Tri-rotor; Affine.
163 Đ. V. Thành, T. Đ. Thuận, “Ứng dụng giải thuật backstepping … UAV dạng tri-rotors.”