Về thuật toán bám đường cho UAV

Nghiên cứu khoa học công nghệ 
Tạp chí Nghiên cứu KH&CN quân sự, Số 55, 06 - 2018 35
VỀ THUẬT TOÁN BÁM ĐƯỜNG CHO UAV 
Phạm Thị Phương Anh1*, Nguyễn Vũ2, Phan Tương Lai3 
Tóm tắt: Bám theo đường là một nhiệm vụ cơ bản của UAV. Một vài thuật toán 
đã được phát triển nhằm đảm bảo chất lượng bám theo đường cho UAV. Bài báo 
này sẽ trình bày một số thuật toán bám theo đường, đồng thời tiến hành so sánh các 
thuật toán đó thông qua công cụ mô phỏng. 
Từ khóa: UAV; Dẫn đường; Điều khiển trượt. 
1. MỞ ĐẦU 
Hiện nay, UAV được ứng dụng rộng rãi trong các lĩnh vực kinh tế xã hội cũng như 
quốc phòng an ninh. Để thực hiện nhiệm vụ của mình, bài toán bám theo đường trên một 
độ cao ổn định là một trong những bài toán cơ bản trong điều khiển UAV, kể cả trong 
nhiệm vụ quét theo vùng được xác định bằng các điểm là đỉnh của đa giác. Đường phải 
theo thường là những đường thẳng hoặc những cung tròn. Nhiệm vụ chung đặt ra cho bài 
toán bám theo đường là đảm bảo độ chính xác trong quá trình bám theo đường, đồng thời 
bảo đảm tính bền vững dưới tác động của nhiễu bên ngoài. 
Các thuật toán bám theo đường được phát triển dựa trên giả thiết tốc độ UAV không 
đổi, được phát triển cơ bản theo các hướng thuật toán carrot, thuật toán dẫn đường phi 
tuyến [1], thuật toán dẫn đường trên cơ sở trường vecto [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]. Trong bài báo này, bằng cách sử dụng mô hình động hình học và công cụ 
mô phỏng, các ưu điểm và nhược điểm của các phương pháp này sẽ được xem xét và phân 
tích, đặc biệt là sự ảnh hưởng của tham số biến đổi. 
Trước tiên bài toán được xây dựng như sau: 
Cho đường quỹ đạo, với vị trí của UAV là ,xM x y và góc hướng quỹ đạo của nó là 
 bài toán bám theo đường là cần xác định góc hướng mong muốn của UAV d sao cho 
khi bám theo góc hướng này, UAV sẽ tiến dần tới và bám theo đường quỹ đạo đã cho. 
Đường quỹ đạo thông thường là các đường thẳng và các cung tròn. Các đường thẳng 
thường được xác định bởi chuỗi điểm dấu, góc của đường thắng nối hai điểm dấu liên tiếp 
là góc đường ngắm  . Đối với quỹ đạo là cung tròn, các tham số của nó được xác định 
bởi tọa độ tâm O và bán kính r của nó. Khoảng cách từ UAV đến đường quỹ đạo là sai số 
ngang. Đối với quỹ đạo cung tròn, góc  được xác định là góc của tiếp tuyến với cung 
tròn tại điểm giao của đường thẳng nối tọa độ UAV với tâm đường tròn O gần với UAV 
nhất. Nhiệm vụ của thuật toán bám theo đường là làm cho sai số 0d cũng như sai số 
góc 0  trong suốt thời gian thực hiện nhiệm vụ t . 
2. CÁC THUẬT TOÁN BÁM THEO ĐƯỜNG 
 2.1. Đặt vấn đề 
Giả thiết đường quỹ đạo cần bám của UAV nằm trên mặt phẳng ngang, đồng thời vận 
tốc của UAV không đổi. Mặt phẳng nằm ngang được thể hiện bẳng tọa độ 2 chiều Oxy. 
Khi đó mô hình động hình học của UAV được xây dựng như sau: 
a ω ω g
a ω ω g
x v v ψ v 
y v v ψ v 
φ
cos cos cos
sin sin sin
u
 
 



 (1) 
Kỹ thuật điều khiển & Điện tử 
P. T. P. Anh, N. Vũ, P. T. Lai, “Về thuật toán bám đường cho UAV.” 36 
Trong đó, av : Vận tốc của UAV;  : Góc hướng của UAV;  : Tốc độ góc hướng; v : 
Vận tốc gió;  : Hướng gió; u : Tập lệnh điều khiển. 
Mô hình động học ở đây tương đương với mô hình xe trên mặt phẳng nằm ngang [5]. 
Trong trường hợp không có gió mô hình động học của UAV có dạng sau: 
a
a
x v
y v
ψ
cos
sin
u





 (2) 
Trước tiên xem xét mô hình trong điều kiện không có gió và áp dụng các thuật toán 
bám theo đường để đánh giá hiệu quả của từng thuật toán. Để đánh giá thuật toán sử dụng 
mô hình với các tham số sau: av 50m / s ; tốc độ góc tối đa 
0
maxω 30 / s ; bán kính 
cung tròn r 400m ; 
2.2. Thuật toán bám theo đường ngắm ảo 
Để xây dựng thuật toán bám theo đường ngắm ảo, xem xét các ký hiệu trong hình 1 
Hình 1. Sơ đồ UAV bám theo đường thẳng (a) và đường tròn (b) 
 theo thuật toán bám điểm ảo. 
Đối với trường hợp bám theo đường thẳng như hình 1a, các tham số cần quan tâm là 
khoảng cách đến đường và vị trí của điểm ngắm ảo. Giả sử chọn điểm ngắm ảo là điểm T 
nằm trên đường thẳng quỹ đạo, có khoảng cách đến hình chiếu của UAV lên đường này 
điểm ( , )P PP x y là  về hướng chuyển động theo quỹ đạo, còn khoảng cách từ UAV đến 
quỹ đạo là d , góc đường ngắm từ UAV (M) đến điểm T( , )t tx y là d vị trí của UAV là 
( , )M x y , góc hướng của UAV là  . Mục tiêu của thuật toán này là luôn đảm bảo cho 
góc hướng của UAV trùng với góc đường ngắm từ UAV đến điểm ngẳm ảo d (hình 1a). 
Góc hướng của UAV được xác định bằng các phương tiện đo, còn góc hướng mong 
muốn được xác định theo thuật toán sau: 
 d t tψ atan2 x x , y y (3) 
Để xác định d cần phải biết ,x y được xác định bằng các thiết bị đo gắn trên UAV và 
( , )t tx y được xác định theo thuật toán sau: 
Đặt P riM khi đó T r δiM 
Nghiên cứu khoa học công nghệ 
Tạp chí Nghiên cứu KH&CN quân sự, Số 55, 06 - 2018 37
Giả sử góc của đường thẳng 1i iM M là , góc của đường iM M là m , độ dài iM M là s 
Khi đó 
t
t
x r δ
y r δ
cos
sin


 (4) 
Ở đó và r được xác định như sau: 
 i 1 i i 1 iθ atan2 x x , y y (5) 
 2 2mr s cos θ θ s d (6) 
Với 
2 2
i is x x y y (7) 
Và m i iθ atan2 x x , y y (8) 
Thuật toán được thực hiện theo các công thức (3) đến (8) cho phép xác định góc ngắm 
từ UAV tới điểm ảo. Tuy nhiên, qua các công thức này chưa thấy nổi bật ảnh hưởng của 
sai số bám d tới độ lệch góc giữa đường thẳng ngắm với góc quỹ đạo . 
Để làm rõ ảnh hưởng này, sử dụng thuật toán chuyển hệ tọa độ. Hệ tọa độ mới có tâm 
là điểm ( , )i i iM x y , có trục Ox trùng với đường thẳng 1i iM M , tức là quay đi 1 góc  
Mỗi điểm trong hệ tọa độ cũ được chuyển sang hệ tọa độ mới thông qua phép tịnh tiến 
và phép quay tổng hợp lại trong phương trình sau: 
m
k i k ik
m
k i k ik
cos θ sin θ x x x xcosθ sinθx
sin θ cos θ y y y ysinθ cosθy
 (9) 
Như vậy, sai số bám d sẽ chính là ym 
 m i id y x x sinθ y y cosθ (10) 
Góc mong muốn sẽ chính là ψ  = atan2(δ, d) (11) 
Và đây cũng chính là góc giữa đường MT với đường 1i iM M , trong một số mô tả khác, 
đây chính là góc lệch với góc chuẩn, tức là góc của đường 1i iM M 
Với cách thể hiện như trong (11) dưới góc độ hình học rất trực quan và thể hiện rõ ảnh 
hưởng của d và  đến việc lựa chọn tín hiệu điều khiển. 
Góc hướng ban đầu của UAV khi đó sẽ là 
m
0 0ψ ψ  (12) 
Và lệnh điều khiển được chọn sẽ là: 
 m m mdu ψ k ψ ψ k(atan2 δ,d ψ)  (13) 
Trong quá trình hoạt động, góc hướng sẽ được đo bằng các thiết bị phù hợp. Trong điều 
kiện lý tưởng: 
t
m
d
0
ψ udt (14) 
Tín hiệu điều khiển (13) được đưa vào (2) để tiến hành mô phỏng. Không mất tính tổng 
quát, trong mô phỏng coi các tham số đưa vào đã được thực hiện sau phép chuyển tọa độ, 
với động học (2) và tham số UAV: av 50m / s ; 
0
maxu 30 / s 
Kỹ thuật điều khiển & Điện tử 
P. T. P. Anh, N. Vũ, P. T. Lai, “Về thuật toán bám đường cho UAV.” 38 
d y 
 2( , )d atan d  
  : Góc hướng UAV 
 Góc quỹ đạo là / 4 
 0 : Góc ban đầu / 3 
 0d : Sai lệch ban đầu 600m 
 m mdu ψ k ψ ψ .( 2 , )k atan d   
Kết quả mô phỏng như sau: 
Hình 2. Kết quả mô phỏng bám theo đường thẳng 
sử dụng thuật toán ngắm điểm ảo. 
Kết quả mô phỏng ta thấy sau 50s thì hệ thống ổn định, sai số bám là 0.8m, sai số góc 
0.04 rad; tốc độ góc dao động nhiều. 
Khi thay đổi hệ số  ta thấy nếu chọn  nhỏ thì hệ thống mất nhiều thời gian để ổn 
định nhưng sai số nhỏ, nếu lớn hệ thống ổn định nhanh nhưng sai số lớn. Khi sử dụng 
phương pháp này phụ thuộc vào cách chọn hệ số  
Đối với trường hợp bám theo đường tròn vì tính đối xứng qua tâm của đường nên ta sẽ 
không thực hiện phép quay tọa độ mà chỉ thực hiện phép tịnh tiến. Không mất tính tổng 
quát nếu coi tâm của đường tròn là gốc tọa độ khi đó: 
Hình chiếu của UAV xuống đường quỹ đạo là giao của đường thẳng OM với đường 
quỹ đạo – Điểm P. 
Điểm ngắm ảo là điểm nằm trên đường tròn và sau điểm P một dây cung có góc chặn 
là  . 
Đường tròn có bán kính R. 
Tương tự như trong chế độ đường thẳng, cần xác định góc hướng mong muốn 
 d mψ θ π  (15) 
Với λ atan2(Rsinε, (R Rcosε d)) (16) 
Và mθ atan2(x, y) (17) 
Tổng hợp lại ta nhận được 
Nghiên cứu khoa học công nghệ 
Tạp chí Nghiên cứu KH&CN quân sự, Số 55, 06 - 2018 39
 dψ (atan2 x, y π) atan2(Rsinε, (R 1 cosε d)) (18) 
Theo phương trình (18) góc mong muốn của UAV phụ thuộc vào tọa độ của nó, giá trị 
góc dây cung đón trước (điểm ngắm ảo) và khoảng cách từ UAV đến đường tròn d . Khi 
d đủ lớn atan2(Rsinε, (R 1 cosε d)) sẽ tiến gần đến 0, khi đó, hướng mong muốn 
sẽ xấp xỉ hướng đường thẳng MO, có nghĩa rằng UAV sẽ nhắm thẳng tâm của đường để 
bay tới. 
Khi UAV nằm trong đường tròn 0d , các công thức trên vẫn được sử dụng. Tuy 
nhiên, trong trường hợp đặc biệt, khi , 0d R x y và d không xác định. Trong 
trường hợp này với một điều kiện biên nhất định, ví dụ x , y  
Khi d sẽ xác định trong mọi điều kiện. 
Hình 3. Kết quả mô phỏng bám theo đường tròn sử dụng thuật toán ngắm điểm ảo. 
Kết quả mô phỏng với điều kiện ban đầu như sau: 
R=160m 
Góc hướng ban đầu : / 3 
Vị trí ban đầu: (0,190) 
Kết quả mô phỏng ta thấy sau 8s thì hệ thống ổn định, sai số bám là 1m. Khi thay đổi 
hệ số  ta thấy nếu chọn  nhỏ thì hệ thống mất nhiều thời gian để ổn định nhưng sai số 
nhỏ, nếu  lớn hệ thống ổn định nhanh nhưng sai số lớn. Khi sử dụng phương pháp này 
phụ thuộc vào cách chọn hệ số  . 
2.3. Thuật toán bám đuổi và bám theo đường ngắm 
Hình 4. Biễu diến hình học thuật toán bám đuổi và bám theo. 
Kỹ thuật điều khiển & Điện tử 
P. T. P. Anh, N. Vũ, P. T. Lai, “Về thuật toán bám đường cho UAV.” 40 
Một số ký hiệu trong thuật toán: 
u : Góc của đường iM M đường thẳng nối điểm đầu của đường quỹ đạo với UAV; 
d : Góc của đường 1iMM đường thẳng nối với UAV với điểm cuối của đường 
quỹ đạo; 
 : góc hướng của UAV; 
 : góc của đường quỹ đạo 1i iM M . 
Thuật toán bám đuổi là duy trì góc hướng của UAV trùng với góc của đường 1i iM M . 
Như vậy, góc đặt tỷ lệ với độ lệch 2 góc: 
P 1 dψ k (θ ψ) (19) 
Thuật toán bám theo đường ngắm đảm bảo góc của đường thẳng nối điểm đầu của 
đường quỹ đạo với UAV u và góc của đường thẳng quỹ đạo  trùng nhau và được xác 
định như sau: 
 l 2 uψ k .d.sin θ θ (20) 
Với d là sai số bám ngang hay là khoảng cách từ UAV đến đường quỹ đạo 
Tổng hợp hai thuật toán ta nhận được: 
 1 2 ). . (d P l d uk k d sin       (21) 
Tương tự như (13) luật điều khiển được chọn 
( )du k    (22) 
Đối với quỹ đạo tròn, thuật toán này được áp dụng dựa theo các tham số xác định như 
sau: 
d : Khoảng cách từ UAV tới đường tròn với UAV. 
d  : Góc tiếp tuyếncủa đường tròn tại giao của đường nối tâm đường tròn với 
UAV 
u  : Góc hướng của UAV 
Và như vậy: 
 1 2 ( )d d dk k dsin     (23) 
Sử dụng công thức (24) để tiến hành mô phỏng với luật điều khiển (23). 
Trong trường hợp UAV nằm ở tâm đường tròn, áp dụng điều kiện 
; ; ; yx x y     để mô phỏng. 
3. THUẬT TOÁN DẪN ĐƯỜNG CHO UAV DỰA TRÊN CHẾ ĐỘ TRƯỢT 
Các thuật toán dẫn đường đề xuất ở trên sử dụng tín hiệu điều khiển liên tục (13) (22). 
Dưới đây sẽ đề xuất thuật toán dẫn đường sử dụng tín hiệu gián đoạn. 
Để xây dựng thuật toán, sử dụng sơ đồ như hình 1, khi UAV bám theo đường nối hai 
điểm 1i iM M . Phương trình động học của UAV theo các trạng thái của , d như sau: 
.sin
Δ
d v
u
 
  

 
 (24) 
Nghiên cứu khoa học công nghệ 
Tạp chí Nghiên cứu KH&CN quân sự, Số 55, 06 - 2018 41
Như vậy, theo hệ phương trình (24), các biến trạng thái là d , khoảng cách từ UAV đến 
đường quỹ đạo;  : sai số góc của UAV và góc đường quỹ đạo; v vận tốc của UAV 
Mục đích của điều khiển là đưa các biến trạng thái của hệ (24) về gốc tọa độ, tức là 
0 d và 0 
Mặt trượt được chọn là: 
 0s d kd  (25) 
Lệnh điều khiển u được tổng hợp như sau: 
 0
 0
max
max
u khi s
u
u khi s
 (26) 
Để tránh hiện tượng rung, lệnh điều khiển được chọn là khâu Role 3 vị trí với histeresic 
như sau: 
 Để xác định u sử dụng thêm hai biến như sau: 
1
1
p khi s
p khi s
p khi s


 
1
1
q khi s
q khi s
q khi s


 
max
max
1
0 1, 1
1
u khi p
u khi p q
u khi q
 (27) 
Trong biểu thức (25), d được xác định theo (24) 
Dưới đây sẽ chứng minh hệ (24) ổn định tiệm cận khi sử dụng bộ điều khiển tổng hợp 
theo (26). 
Chọn hàm Lyapunop: 2
1
0
2
V d (28) 
Khi đó: . V d d  (29) 
Theo (25) khi tồn tại chế độ trượt .d k d  (30) 
Từ (30) ta có 2. 0V k d  
Vậy d ổn định tiệm cận. Khi đó từ (31) d cũng ổn định khi đó: 
0
d
arcsin
v
 

Hệ (24) ổn định tiệm cận 
Mặt khác: 
¨
. .Δ . . d v cos u v cos    
Từ (24) ta có: 
 . . . . . .a as s u v cos k v sin s   
Để s.s 0  các điều kiện cần thỏa mãn 
Kỹ thuật điều khiển & Điện tử 
P. T. P. Anh, N. Vũ, P. T. Lai, “Về thuật toán bám đường cho UAV.” 42 
Khi 0s . . . . 0a as u v cos k v sin   
 .u k tan  sử dụng maxu u 
Khi 0s .u k tan  sử dụng maxu u 
 Vì u là tín hiệu điều khiển có giới hạn max maxu u u nên trong một số trường 
hợp khi tan  quá lớn , không thỏa mãn điều kiên s.s 0  . Tuy nhiên, sau một thời 
gian sai số  sẽ nhỏ dần, và một khi thỏa mãn điều kiện max tan u điều kiện (32) 
sẽ thỏa mãn s.s 0  hệ ổn định tiệm cận. 
Dưới đây sẽ trình bày kết quả mô phỏng theo thuật toán đề xuất với av 50m / s ; 
0
maxu 30 / s với điều kiện ban đầu giống thuật toán bám theo điểm ảo (hình 5). 
Hình 5. Kết quả mô phỏng bám theo đường thẳng sử dụng thuật toán trượt. 
Kết quả mô phỏng ta thấy sau 35 s thì hệ thống ổn định, sai số bám là 0.6 m, sai số góc 
0.005 rad; 
Hình 6. Kết quả mô phỏng bám theo đường tròn sử dụng thuật toán trượt. 
Để sử dụng thuật toán này cho đường tròn, trạng thái 0 trong (28) được thay bằng tín 
hiệu điều khiển danh định bảo đảm tôc độ góc của UAV bằng: 
Nghiên cứu khoa học công nghệ 
Tạp chí Nghiên cứu KH&CN quân sự, Số 55, 06 - 2018 43
 au
v
R
 
Việc nhận biết UAV chuyển vào quỹ đạo được xác định bằng các thuật toán phù hợp . 
Kết quả mô phỏng với điều kiện ban đầu như sau: 
R=160m 
Góc hướng ban đầu : / 3 
Vị trí ban đầu: (0,190) 
Kết quả mô phỏng ta thấy sau 3 s thì hệ thống ổn định, sai số bám là 0.3 m. 
4. KẾT LUẬN 
Qua các kết quả mô phỏng ở trên nhận thấy dẫn đường sử dụng phương pháp ngẳm 
theo điểm ảo thì kết quả phụ thuộc nhiều vào cách chọn hệ số . Khi thay đổi hệ số  ta 
thấy nếu chọn  nhỏ thì hệ thống mất nhiều thời gian để ổn định nhưng sai số nhỏ, nếu  
lớn hệ thống ổn định nhanh nhưng sai số lớn. Trong khi đó, sử dụng thuật toán dẫn đường 
trên cơ sở chế độ trượt thì 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]. L. E. Dubins, “On curves of minimal length with a constraint on average curvature 
and with prescribed initial and terminal positions and tangents,” American Journal of 
Mathematics, vol. 79, p. 497516,1957. 
ABSTRACT 
PATH FOLLOWING ALGORITHM FOR UAV 
 Path following is a basic mission of UAVs. Therefor some algorithms have been 
developed in oder to ensure the quality of UAV’s path following. Some algorithms 
for path following and verifying the results via simulation tool are presented in this 
article. 
Keywords: UAV; Sliding mode; Path following. 
Nhận bài ngày 12 tháng 4 năm 2018 
Hoàn thiện ngày 07 tháng 6 năm 2018 
Chấp nhận đăng ngày 08 tháng 6 năm 2018 
Địa chỉ: 1 Viện Tự động hóa KTQS; 
 2 Cục Khoa học Quân sự/BQP; 
 3 Viện Khoa học và Công nghệ quân sự. 
 * Email: ptpanh2003@yahoo.com.