Thuật toán dẫn đường cho uav dựa trên hệ tọa độ Serret-Frenet

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 51 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 sOx tiếp tuyến với đường cong quỹ đạo hướng theo hướng quỹ đạo, trục sOy vuông góc với trục sOx , hướng của trục sOx là hướng quỹ đạo t [6]. ( ) EE E x O y : 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; ( )S sx Oy : Hệ tọa độ Serret frenet (SF); t : Góc hướng quỹ đạo được tạo bởi sOx 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. Kỹ thuật điều khiển & Tự động hóa P. T. P. Anh, N. Vũ, P. T. Lai, "Thuật toán dẫn đường cho UAV tọa độ Serret-Frenet." 52 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  0 0l t  đế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à ,x ye e và  với xe là tọa độ theo trục x , ye 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 . y . ψ V cos 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: 1 . os .sin . x y t t e V c l e V g u V k l                     (2) Ở đó: l là vận tốc theo cung dài của gốc tọa độ; k là hệ số đường cong; 1 tanu  ;  là góc nghiêng của UAV trên hệ tọa độ S-F. Hình 1. Hệ tọa độ Serret-Fernet và các tọa độ của UAV. 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 53 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ố 0xe  , khi đó ye 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 k l  (3) 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  0 0 0, ,x ye e  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: . ys k e  (4) Chọn . 2 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ả của UAV. Để  không vượt quá 2  ; ye được xác định như sau: y y y y r khi e r e e khi e r     (5) 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ả , , 0y xe e  . 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à , ,x ye e  thì các biến điều khiển sẽ là 1u tg và 2u l . Khi đó hệ phương trình (2) được viết lại như sau: 2 1 2 .sin .cos . y x l e V e V u g u k u V                  (6) Áp dụng chế độ trượt cho hệ (6) với mặt trượt: 1 2 . x s y es s k es              (7) 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 1s . Kỹ thuật điều khiển & Tự động hóa P. T. P. Anh, N. Vũ, P. T. Lai, "Thuật toán dẫn đường cho UAV tọa độ Serret-Frenet." 54 1 2 2 d.cos 0 .cosx ts e V u u V         (8) 1 1 2. . .( .cos )x x xs s e e e V u    (9) Chọn: 2 2 d 1.sgnt xu u e  (10) Với 2u chọn theo (10) ta có 1 1. 0s s  với 0xe   Chế độ trượt sẽ hội tụ. Ngoài ra, khi 1 2 d 20,u .coste V u   , (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: 1 .sin .cos y l e V g u k V            (12) Xác định tín hiệu điều khiển 1u bảo đảm chế độ trượt: 2 .s ys k e  2 2 2 1. .( .V.cos .V.sin )l s g s s s u k k V      (13) 1 1 d 2 2.sgnstu u   (14) Với: 2 1 d ( .cos .sin )t l s V u k k g     (15) Với 1u và 2u đượ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 2u 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 1sgns bằng 1xk s với xk 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 2 dtu : 2 2 d 1t xu u k s  (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: 2 1 2 .sin .cos . y x l e V e V u g u k u V                  Với vector điều khiển được chọn theo (14) và (16). 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 55 1 1 d 2 2 2 2 d 1 sgn s .s t t x u u u u k      (17) 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 wv và hướng w của gió. Khi đó, với giá trị không tốc aV 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: .sinax a aV V  w w w.sinxV V  .cosay a aV V  w w w.cosyV V  Từ đó ta có: wx ax xV V V  wy ay yV V V  Trong khi đó: arctan 2( , )y xV V  Do đó: w w w warctan 2( .sin .sin , .cos .sin )a a a aV V V V       (18) Do wV và w không đổi lấy đường vecto wV  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 wOx hướng theo hướng gió, trục wOy 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ó: w w.cos( )ax a aV V    w w.sin ( )ay a aV V    Vecto địa tốc khi chiếu lê hệ tọa độ gió sẽ có các thành phần là: w w wgx axV V V  w wgy ayV V Do đó giá trị địa tốc được xác định là: 2 2 xw wg gyV V V  Hay :       2 2 w w w.cos .sina a a aV V V V        (19) Kỹ thuật điều khiển & Tự động hóa P. T. P. Anh, N. Vũ, P. T. Lai, "Thuật toán dẫn đường cho UAV tọa độ Serret-Frenet." 56 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: *( sin ) *(1 os ) x a t t y a c t      (20)  * 1 ost *sin x a c y a t       (21)   2 2 2 0 0 00 1 ost sin 2a sin 2 2a sin 4a.cos 4a.cos 4 2 2 2 t t tt t l a c tdt dt t t t dt              (22) Góc pháp tuyến của đường cong: sin sin arctan 1 ost 1 ost t t tg c c       (23)       2 2 2 2 2 2 2 1 cost sin .sin * 1 cost 1 costsin 1 1 ost 1 cost ost-1 . 1 cost4*sin 2 2.sin 12 24.sin 2 t t t c c t t t                        (24) ' 2a.sinl t Độ cong tại điểm t : 2arcos 4a l t          1 2.2a.sin ar 4a k l l c            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ố ,x ye e , vẽ đồ thị đồng thời vẽ trên graph x,y đường quỹ đạo mong muốn cũng như quỹ đạo bám. 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 57 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 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). 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. Kỹ thuật điều khiển & Tự động hóa P. T. P. Anh, N. Vũ, P. T. Lai, "Thuật toán dẫn đường cho UAV tọa độ Serret-Frenet." 58 b) Với điểm ban đầu của UAV 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ị 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). 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 59 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 Địa chỉ: 1 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.