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