Về thuật toán bám đường cho UAV - Phạm Thị Phương Anh

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.