Xem mẫu
- Nghiên cứu khoa học công nghệ
THUẬT TOÁN DẪN ĐƯỜNG CHO UAV DỰA TRÊN
HỆ TỌA ĐỘ SERRET-FRENET
Phạm Thị Phương Anh1*, Nguyễn Vũ2, Phan Tương Lai3
Tóm tắt: Bài báo đề xuất sử dụng hệ tọa độ Serret-Frenet trong dẫn đường
bám theo quỹ đạo trơn có độ cong bất kỳ. Nhờ sử dụng hệ tọa độ Serret-Frenet
bài toán xác định khoảng cách từ UAV đến đường quỹ đạo được giải phóng, làm
giảm sự phức tạp của của thuật toán bám đường quỹ đạo, đồng thời cũng loại bỏ
được điểm kỳ dị trong thuật toán bám đường khi UAV nằm trên tâm của cung
tròn. Trong bài báo, chế độ trượt với mặt trượt phi tuyến được sử dụng để mang
lại chất lượng cao cho hệ thống dẫn.
Từ khóa: Điều khiển UAV; Bám đường; Serret-Fernet.
1. MỞ ĐẦU
Có rất nhiều thuật toán dẫn đường cho UAV bám theo đường quỹ đạo được xây
dựng trước, thuật toán dẫn đường phi tuyến[1], thuật toán dẫn đường trên cơ sở
trường véc tơ [2], thuật toán bám đường trên cơ sở bộ điều chỉnh toàn phương
tuyến tính [3], thuật toán bám đuổi và thuật toán bám theo đường ngắm [4], dẫn
đường áp dụng chế độ trượt [5]. Chế độ trượt mang lại hiệu quả cao, nhất là khi sử
dụng trượt phi tuyến. Tuy nhiên, trong mọi bài toán dẫn đường cần xác định được
khoảng cách từ UAV đến đường quỹ đạo mà điều này còn khó khăn đối với quỹ
đạo là đường cong có bán kính thay đổi. Ngoài ra bài toán dẫn đường cũng gặp
phải các điểm kỳ dị, khi UAV nằm đúng tâm của đường quỹ đạo. Để khắc phục
nhược điểm này, hệ tọa độ Serret Fernet được áp dụng, bài toán dẫn đường được
giải quyết như bài toán bám quỹ đạo với tốc độ trên quỹ đạo được thay đổi với giả
thiết UAV bay ở độ cao không đổi. Việc đưa bài toán dẫn đường về hệ tọa độ
Serret Fernet sẽ được đề cập cụ thể trong bài báo này.
2. BIỂU DIỄN CÁC THÔNG SỐ BÁM QUỸ ĐẠO QUA
HỆ TỌA ĐỘ SERRET-FRENET
Hệ Serret Frenet (SF) trong không gian hai chiều là hệ tọa độ có điểm gốc tọa
độ O chuyển động dọc theo đường cong quỹ đạo l và tốc độ di chuyển là l , trục
Oxs tiếp tuyến với đường cong quỹ đạo hướng theo hướng quỹ đạo, trục Oys
vuông góc với trục Oxs , hướng của trục Oxs là hướng quỹ đạo t [6].
( xE OE yE ) : Hệ tọa độ trái đất;
x, y, : Vị trí và hướng của UAV trong hệ tọa độ trái đất;
C: tâm của UAV;
V: Vận tốc của UAV;
( xS Oys ) : Hệ tọa độ Serret frenet (SF);
t : Góc hướng quỹ đạo được tạo bởi Oxs và trục x .
O: Gốc tọa độ hệ SF và O là hình chiếu trực giao của C lên đường cong
quỹ đạo.
Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san TĐH, 04 - 2019 51
- Kỹ thuật điều khiển & Tự động hóa
Hình 1. Hệ tọa độ Serret-Fernet và các tọa độ của UAV.
Trong mặt phẳng nằm ngang đường quỹ đạo được mô tả là quỹ đạo của điểm
gốc tọa độ O của hệ tọa độ FS trượt theo đường quỹ đạo. Giả sử l là độ dài của
đường quỹ đạo dọc theo đường mong muốn. Độ dài đường cong quỹ đạo được
hiểu là khoảng cách từ điểm gốc ban đầu l t0 0 đến vị trí O , để từ đó xác định
được vị trí hiện tại mong muốn để UAV bám theo.
3. DẪN ĐƯỜNG TRONG HỆ TỌA ĐỘ SF
Tọa độ UAV và hướng của UAV trong hệ tọa độ S-F ký hiệu là ex , ey và với
ex là tọa độ theo trục x , ey là tọa độ theo trục y và là hiệu giữa góc hướng của
UAV và góc hướng quỹ đạo.
t (1)
Theo [5] mô hình động học của UAV như sau:
x V .cos
y V .sin
ψ u
Trên hệ tọa độ S-F phương trình động học của hệ bám đường như sau:
ex V .cos l
ey V .sin
g (2)
u1 t
V
t k .l
Ở đó: l là vận tốc theo cung dài của gốc tọa độ;
k là hệ số đường cong;
u1 tan ;
là góc nghiêng của UAV trên hệ tọa độ S-F.
52 P. T. P. Anh, N. Vũ, P. T. Lai, "Thuật toán dẫn đường cho UAV … tọa độ Serret-Frenet."
- Nghiên cứu khoa học công nghệ
Bài toán dẫn đường trong hệ (2) khác với bài toán dẫn đường sử dụng sai số
khoảng cách và sai số hướng ở chỗ nó là bài toán bám theo quỹ đạo. Tuy nhiên, vì
vấn đề đặt ra là bám theo đường nên sự phụ thuộc vào thời gian cần phải loại bỏ.
Điều này giải quyết bằng cách đưa tốc độ của gốc tọa độ trở thành tín hiệu điều
khiển, nhằm mục đích đưa sai số ex 0 , khi đó ey chính là sai số khoảng cách và
k (l ) chính là độ cong của đường quỹ đạo tại điểm gốc tọa độ và bán kính của
đường cong khi đó là:
1
R (3)
k (l )
Hệ (2) sẽ có trạng thái ban đầu, khi gốc tọa độ là điểm khởi đầu của đường quỹ
đạo, tọa độ UAV ex 0 , ey 0 , 0 là vị trí và hướng ban đầu của UAV trong hệ tọa độ
S.F.
Sử dụng luật dẫn đường với mặt trượt tuyến tính:
s k .e y (4)
Chọn k .r ; r là bán kính đường tròn với tốc độ góc quay lớn nhất hiệu quả
2
của UAV.
Để không vượt quá ; ey được xác định như sau:
2
r khi ey r
ey (5)
ey khi ey r
Giả sử tồn tại chế độ trượt trên mặt (4), cần chọn l sao cho hệ thống bám ổn
định, hay nói cách khác cả , ey , ex 0 .
Như vậy l trở thành một trong các thành phần của vecto điều khiển. Nếu các
biến trạng thái của hệ thống là ex , ey , thì các biến điều khiển sẽ là u1 tg và
u2 l .
Khi đó hệ phương trình (2) được viết lại như sau:
ey V .sin
ex V .c os u2 (6)
g
u1 kl .u2
V
Áp dụng chế độ trượt cho hệ (6) với mặt trượt:
s ex
s 1 (7)
s2 ks .ey
Xác định điều khiển tương đương cho vector điều khiển, trước tiên đối với mặt
trượt s1 .
Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san TĐH, 04 - 2019 53
- Kỹ thuật điều khiển & Tự động hóa
s1 ex V .cos u2 0 u2td V .cos (8)
s1.s1 ex .ex ex .(V .cos u2 ) (9)
Chọn: u2 u2td 1.sgn ex (10)
Với u2 chọn theo (10) ta có s1.s1 0 với ex 0 Chế độ trượt sẽ hội tụ.
Ngoài ra, khi e1 0, u 2td V .cos u2 , (11)
Đặt (11) vào (6) và rút gọn phương trình (6), hệ phương trình (6) trở thành 2
phương trình:
ey V .sin
g (12)
V u1 kl .c os
Xác định tín hiệu điều khiển u1 bảo đảm chế độ trượt:
s2 k s .e y
g
s2 .s2 s2 .( u1 kl .V.co s k s .V.sin ) (13)
V
u1 u1td 2 .sgns2 (14)
V2
Với: u1td (kl .cos ks .sin ) (15)
g
Với u1 và u2 được chọn theo (10) và (14) sẽ tồn tại chế độ trượt trên siêu mặt
trượt (7) đối với hệ (6).
Chú ý rằng đối với hệ (6) tín hiệu điều khiển u2 là l không phải là tín hiệu vật
lý mà chỉ là tín hiệu mong muốn về toán học nên l không giới hạn. Do đó trong
biểu thức (10) có thể thay 1 sgns1 bằng kx s1 với kx lớn tùy ý. Khi đó khi ở chế độ
trượt tín hiệu điều khiển sẽ chính là tín hiệu u2td :
u2 u2td kx s1 (16)
Các kết quả có thể phát triển thành các bổ đề sau:
Bổ đề:
Đối với quỹ đạo đường cong bất kỳ đã biết độ cong theo chiều dài cung tính từ
điểm gốc, đồng thời biết góc hướng ban đầu của đường quỹ đạo tại điểm gốc, bài
toán bám đường của UAV được thực hiện dựa trên Serret-Frenet Frame có phương
trình động học như sau:
ey V .sin
ex V .c os u2
g
u1 kl .u2
V
Với vector điều khiển được chọn theo (14) và (16).
54 P. T. P. Anh, N. Vũ, P. T. Lai, "Thuật toán dẫn đường cho UAV … tọa độ Serret-Frenet."
- Nghiên cứu khoa học công nghệ
u1 u1td 2 sgn s 2
(17)
u2 u2td k x .s1
với các điều khiển tương đương được chọn theo (8) và (15).
Bổ đề là cơ sở để tổng hợp hệ điều khiển dẫn đường cho UAV trong chế độ bám
theo đường với độ cao không đổi.
4. DẪN ĐƯỜNG TRONG ĐIỀU KIỆN GIÓ
Giả sử gió thay đổi chậm. Sử dụng phương pháp đánh giá gió [3] có thể xác
định được vận tốc vw và hướng w của gió.
Khi đó, với giá trị không tốc Va và góc hướng không tốc a giá trị địa tốc của
UAV, ký hiệu là V và góc hướng của địa tốc là được xác định như sau:
Vax Va .sin a
Vwx Vw .sin w
Vay Va .c os a
Vwy Vw .c os w
Từ đó ta có:
Vx Vax Vwx
Vy Vay Vwy
Trong khi đó:
arctan 2(Vy ,Vx )
Do đó:
arctan 2(Va .sin a Vw .sin w ,Va .cos a Vw .sin w )
(18)
Do Vw và w không đổi lấy đường vecto Vw là đường cơ sở để tính địa tốc của
UAV. Đặt hệ tọa độ góc có gốc tọa độ là O , trục Ox w hướng theo hướng gió, trục
Oyw vuông góc hướng gió. Khi đó chiếu vecto không tốc của UAV lên 2 trục này
ta có:
Vaxw Va .cos( a w )
Vayw Va .sin ( a w )
Vecto địa tốc khi chiếu lê hệ tọa độ gió sẽ có các thành phần là:
Vgxw Vaxw Vw
Vgyw Vayw
Do đó giá trị địa tốc được xác định là:
V Vg2xw Vgy2 w
Hay :
2 2
V V
w Va .c os a w Va .sin a w (19)
Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san TĐH, 04 - 2019 55
- Kỹ thuật điều khiển & Tự động hóa
5. MÔ PHỎNG
Để kiểm chứng kết quả trên, tiến hành mô phỏng bám theo đường quỹ đạo
đường cong cycloid như sau:
x a *(t sin t )
(20)
y a * (1 cost )
x a * 1 cost
(21)
y a *sin t
t t
2 t
l a 1 cost sin 2 tdt 2a sin 2 dt
0 0
2
t
(22)
t
t t t
2a sin dt 4a.cos 4a.c os 4
0
2 20 2
Góc pháp tuyến của đường cong:
sin t sin t
tg arctan (23)
1 cost 1 cost
1 cost sin t.sin t
2
*
sin t 1 cost 1 cost 2
1
1 cost
2
1 cost .
cost-1
(24)
2 t 1 cost 2
4*sin
2
t
2.sin 2
2 1
t 2
4.sin 2
2
'
l 2a.sin t
Độ cong tại điểm t :
l
t 2arcos
4a
1
k l
l
2.2a.sin arc
4a
Như vậy mỗi giá trị l cho trước, độ cong của nó hoàn toàn được xác định.
Đặt k l vào 6c; t xác định theo (25) tiến hành mô phỏng với cá vị trí ban đầu
của UAV qua đó xác định giá trị các lệnh điều khiển cũng như sai số ex , ey , vẽ đồ
thị đồng thời vẽ trên graph x,y đường quỹ đạo mong muốn cũng như quỹ đạo bám.
56 P. T. P. Anh, N. Vũ, P. T. Lai, "Thuật toán dẫn đường cho UAV … tọa độ Serret-Frenet."
- Nghiên cứu khoa học công nghệ
Kết quả mô phỏng như thể hiện trong các hình 2a, 2b, hình 3, hình 4a, 4b và
hình 5. Kết quả mô phỏng cho thấy trong khoảng 1s đầu tiên hệ dò tìm tọa độ x
trên hệ tọa độ S.F. Sau đó hệ thống ổn định bám theo quỹ đạo cong cho trước với
sai số là 0,5m.
a) Với điểm ban đầu của UAV là (0;0)
Hình 2a. Quỹ đạo đặt dạng Cycloid. Hình 2b. Quỹ đạo thực của UAV bám
theo quỹ đạo đặt.
Hình 3. Kết quả mô phỏng dẫn đường sử dụng chế độ trượt với mặt trượt phi
tuyến dựa trên hệ tọa độ Serret-Frenet với điểm đầu (0;0).
Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san TĐH, 04 - 2019 57
- Kỹ thuật điều khiển & Tự động hóa
b) Với điểm ban đầu của UAV là 20;50
Hình 4a. Quỹ đạo đặt dạng Cycloid. Hình 4b. Quỹ đạo thực của UAV bám
theo quỹ đạo đặt.
Hình 5. Kết quả mô phỏng dẫn đường sử dụng chế độ trượt với mặt trượt phi
tuyến dựa trên hệ tọa độ Serret-Frenet có điểm đầu là (20;50).
6. KẾT LUẬN
Qua các kết quả mô phỏng ở trên nhận thấy dẫn đường trên cơ sở chế độ
trượt với mặt trượt phi tuyến dựa trên hệ tọa độ Serret-Frenet giúp bài toán xác
định khoảng cách từ UAV đến đường quỹ đạo được giải phóng, làm giảm sự phức
tạp của của thuật toán bám đường quỹ đạo, đồng thời cũng loại bỏ được điểm kỳ dị
58 P. T. P. Anh, N. Vũ, P. T. Lai, "Thuật toán dẫn đường cho UAV … tọa độ Serret-Frenet."
- Nghiên cứu khoa học công nghệ
trong thuật toán bám đường khi UAV nằm trên tâm của cung tròn, hệ thống ổn
định nhanh và có thể hoạt động trong mọi chế độ. Đây là cơ sở để điều khiển UAV
thực hiện nhiệm vụ bám theo quỹ đạo đặt trước.
TÀI LIỆU THAM KHẢO
[1]. S. Park, J. Deystt, and J. How, “Performance and lyapunov stability of a
nonlinear path-following guidance method” Journal of Guidance, Control,
and Dynamics, vol. 30, no. 6, pp. 1718–1728, 2007.
[2]. D. Nelson, D. Barber, T. McLain, and R. Beard, “Vector field path following
for miniature air vehicles,” IEEE Transactions on Robotics, pp. 519–529,
June 2007.
[3]. A. Ratnoo, P. Sujit, and M. Kothari, “Optimal path following for high wind
flights,” Proc. of the IFAC World Congress, Aug 2011.
[4]. M. Kothari, I. Postlethwaite, and D. Gu, “A suboptimal path planning
algorithm using rapidly-exploring random trees” International Journal of
Aerospace Innovations, vol. 2, no. 1, pp. 93–104, 2010.
[5]. Phạm Thị PhươngAnh, Nguyễn Vũ, Phan Tương Lai, “Về thuật toán bám
đường cho UAV” Nghiên cứu KH&CN quân sự (số 55, tháng 6/2018)
[6]. Liao Yu-Lei, ZHang Ming-jun, Wan Lei, “Serret−Frenet frame based on
path following control for underactuated unmanned surface vehicles with
dynamic uncertainties”, Central South University Press and Springer-Verlag
Berlin Heidelberg, Vol .22, pp. 214–223, 2015.
ABSTRACT
ALGORITHM OF PATH FOLLOWING FOR UAV BASING ON THE
COORDINATE SYSTEM OF SERRET-FRENET
The article proposes using the coordinate system of Serret-Fernet in path
following tracking slippery orbit, which has got any flexure. Thanks to using the
coordinate system of Serret-Fernet, the problem defines distance from UAV to
orbit that is launched. It also reduces the complexity of algorithm of path
following orbit, at the same time wiping out the singularities in algorithm of path
following UAV area in the center of arc. In this report, the sliding mode with
non-linear sliding was used to give the high quality navigation.
Keywords: UAV; Sliding mode; Path following.
Nhận bài ngày 15 tháng 01 năm 2019
Hoàn thiện ngày 22 tháng 02 năm 2019
Chấp nhận đăng ngày 15 tháng 3 năm 2019
1
Địa chỉ: Viện Tự động hóa KTQS;
2
Cục Khoa học quân sự;
3
Viện Khoa học và Công nghệ Quân sự.
*
Email: ptpanh2003@yahoo.com.
Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san TĐH, 04 - 2019 59
nguon tai.lieu . vn
