Bài viết đề 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.
Nghiên cứu khoa học cơng nghệ THUẬT TỐ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 dẫn đường bám theo quỹ đạo trơn có độ cong Nhờ sử dụng hệ tọa độ Serret-Frenet toán xác định khoảng cách từ UAV đến đường quỹ đạo giải phóng, làm giảm phức tạp của thuật toán bám đường quỹ đạo, đồng thời loại bỏ điểm kỳ dị thuật toán bám đường UAV nằm tâm cung tròn Trong báo, chế độ trượt với mặt trượt phi tuyến 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 MỞ ĐẦU Có nhiều thuật tốn dẫn đường cho UAV bám theo đường quỹ đạo xây dựng trước, thuật toán dẫn đường phi tuyến[1], thuật toán dẫn đường sở trường véc tơ [2], thuật tốn bám đường sở điều chỉnh tồn phương tuyến tính [3], thuật tốn bám đuổi 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 cao, sử dụng trượt phi tuyến Tuy nhiên, toán dẫn đường cần xác định khoảng cách từ UAV đến đường quỹ đạo mà điều khó khăn quỹ đạo đường cong có bán kính thay đổi Ngồi toán dẫn đường gặp phải điểm kỳ dị, UAV nằm tâm đường quỹ đạo Để khắc phục nhược điểm này, hệ tọa độ Serret Fernet áp dụng, toán dẫn đường giải toán bám quỹ đạo với tốc độ quỹ đạo thay đổi với giả thiết UAV bay độ cao khơng đổi Việc đưa tốn dẫn đường hệ tọa độ Serret Fernet đề cập cụ thể báo BIỂU DIỄN CÁC THÔNG SỐ BÁM QUỸ ĐẠO QUA HỆ TỌA ĐỘ SERRET-FRENET Hệ Serret Frenet (SF) không gian hai chiều hệ tọa độ có điểm gốc tọa độ O chuyển động dọc theo đường cong quỹ đạo l tốc độ di chuyển l , trục Oxs tiếp tuyến với đường cong quỹ đạo hướng theo hướng quỹ đạo, trục Oys vng góc với trục Oxs , hướng trục Oxs hướng quỹ đạo t [6] ( xE OE yE ) : Hệ tọa độ trái đất; x, y, : C: V: ( xS Oys ) : t : O: Vị trí hướng UAV hệ tọa độ trái đất; tâm UAV; Vận tốc UAV; Hệ tọa độ Serret frenet (SF); Góc hướng quỹ đạo tạo Oxs trục x Gốc tọa độ hệ SF O hình chiếu trực giao 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 Hệ tọa độ Serret-Fernet tọa độ UAV Trong mặt phẳng nằm ngang đường quỹ đạo mô tả quỹ đạo điểm gốc tọa độ O hệ tọa độ FS trượt theo đường quỹ đạo Giả sử l độ dài đường quỹ đạo dọc theo đường mong muốn Độ dài đường cong quỹ đạo hiểu khoảng cách từ điểm gốc ban đầu l t0 đến vị trí O , để từ xác định vị trí mong muốn để UAV bám theo DẪN ĐƯỜNG TRONG HỆ TỌA ĐỘ SF Tọa độ UAV hướng UAV hệ tọa độ S-F ký hiệu ex , ey với ex tọa độ theo trục x , ey tọa độ theo trục y hiệu góc hướng UAV góc hướng quỹ đạo t (1) Theo [5] mơ hình động học UAV sau: x V cos y V sin ψ u Trên hệ tọa độ S-F phương trình động học hệ bám đường sau: ex V cos l ey V sin g u1 t V t k l Ở đó: l vận tốc theo cung dài gốc tọa độ; k hệ số đường cong; u1 tan ; góc nghiêng UAV hệ tọa độ S-F 52 (2) 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 hệ (2) khác với toán dẫn đường sử dụng sai số khoảng cách sai số hướng chỗ tốn bám theo quỹ đạo Tuy nhiên, vấn đề đặt bám theo đường nên phụ thuộc vào thời gian cần phải loại bỏ Điều giải cách đưa tốc độ gốc tọa độ trở thành tín hiệu điều khiển, nhằm mục đích đưa sai số ex , ey sai số khoảng cách k (l ) độ cong đường quỹ đạo điểm gốc tọa độ bán kính đường cong là: (3) R k (l ) Hệ (2) có trạng thái ban đầu, gốc tọa độ điểm khởi đầu đường quỹ đạo, tọa độ UAV ex , ey , vị trí hướng ban đầu UAV 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 Chọn k (4) r ; r bán kính đường tròn với tốc độ góc quay lớn hiệu UAV Để không vượt ; ey xác định sau: r ey r ey (5) ey ey r Giả sử tồn chế độ trượt mặt (4), cần chọn l cho hệ thống bám ổn định, hay nói cách khác , ey , ex Như l trở thành thành phần vecto điều khiển Nếu biến trạng thái hệ thống ex , ey , biến điều khiển u1 tg u2 l Khi hệ phương trình (2) viết lại sau: ey V sin ex V c os u2 g u1 kl u2 V Áp dụng chế độ trượt cho hệ (6) với mặt trượt: s ex s 1 s2 ks ey (6) (7) Xác định điều khiển tương đương cho vector điều khiển, trước tiên 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 u2td V cos s1.s1 ex ex ex (V cos u2 ) (8) (9) u2 u2td 1.sgn ex (10) Với u2 chọn theo (10) ta có s1.s1 với ex Chế độ trượt hội tụ Ngoài ra, e1 0, u 2td V cos u2 , (11) Chọn: Đặt (11) vào (6) rút gọn phương trình (6), hệ phương trình (6) trở thành phương trình: ey V sin (12) g 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 ) V (13) u1 u1td sgns2 (14) V2 (kl cos ks sin ) (15) g Với u1 u2 chọn theo (10) (14) tồn chế độ trượt siêu mặt trượt (7) hệ (6) Chú ý hệ (6) tín hiệu điều khiển u2 l khơng phải tín hiệu vật lý mà tín hiệu mong muốn tốn học nên l khơng giới hạn Do Với: u1td biểu thức (10) thay 1 sgns1 kx s1 với kx lớn tùy ý Khi chế độ trượt tín hiệu điều khiển tín hiệu u2td : u2 u2td kx s1 (16) Các kết phát triển thành bổ đề sau: Bổ đề: Đối với quỹ đạo đường cong 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 đường quỹ đạo điểm gốc, toán bám đường UAV thực dựa Serret-Frenet Frame có phương trình động học sau: ey V sin ex V c os u2 g u1 kl u2 V Với vector điều khiển chọn theo (14) (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 sgn s (17) u2 u2td k x s1 với điều khiển tương đương chọn theo (8) (15) Bổ đề sở để tổng hợp hệ điều khiển dẫn đường cho UAV chế độ bám theo đường với độ cao không đổi 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] xác định vận tốc vw hướng w gió Khi đó, với giá trị khơng tốc Va góc hướng không tốc a giá trị địa tốc UAV, ký hiệu V góc hướng địa tốc xác đị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 đó: arctan 2(Vy ,Vx ) Do đó: arctan 2(Va sin a Vw sin w ,Va cos a Vw sin w ) (18) Do Vw w không đổi lấy đường vecto Vw đường sở để tính địa tốc UAV Đặt hệ tọa độ góc có gốc tọa độ O , trục Ox w hướng theo hướng gió, trục Oyw vng góc hướng gió Khi chiếu vecto khơng tốc UAV lên trục ta có: Vaxw Va cos( a w ) Vayw Va sin ( a w ) Vecto địa tốc chiếu lê hệ tọa độ gió có thành phần là: Vgxw Vaxw Vw Vgyw Vayw Do giá trị địa tốc xác định là: V Vg2xw Vgy2 w Hay : V V w Va c os a w Va sin a w Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san TĐH, 04 - 2019 (19) 55 Kỹ thuật điều khiển & Tự động hóa MƠ PHỎNG Để kiểm chứng kết trên, tiến hành mô bám theo đường quỹ đạo đường cong cycloid sau: x a *(t sin t ) y a * (1 cost ) (20) x a * 1 cost y a *sin t (21) t t t l a 1 cost sin tdt 2a sin dt 0 t t (22) t t t 2a sin dt 4a.cos 4a.c os 20 Góc pháp tuyến đường cong: sin t sin t arctan cost cost cost sin t.sin t * cost 1 cost 2 sin t 1 cost tg 1 cost cost-1 (23) (24) t 1 cost 2 4*sin t 2.sin 2 1 t 4.sin 2 ' l 2a.sin t Độ cong điểm t : l t 2arcos 4a k l l 2.2a.sin arc 4a Như giá trị l cho trước, độ cong hồn tồn xác định Đặt k l vào 6c; t xác định theo (25) tiến hành mơ với cá vị trí ban đầu UAV qua xác định giá trị lệnh điều khiển sai số ex , ey , vẽ đồ thị đồng thời vẽ graph x,y đường quỹ đạo mong muốn 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 mô thể hình 2a, 2b, hình 3, hình 4a, 4b hình Kết mơ cho thấy khoảng 1s hệ dò tìm tọa độ x hệ tọa độ S.F Sau hệ thống ổn định bám theo quỹ đạo cong cho trước với sai số 0,5m a) Với điểm ban đầu UAV (0;0) Hình 2a Quỹ đạo đặt dạng Cycloid Hình 2b Quỹ đạo thực UAV bám theo quỹ đạo đặt Hình Kết mơ dẫn đường sử dụng chế độ trượt với mặt trượt phi tuyến dựa 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 UAV 20;50 Hình 4a Quỹ đạo đặt dạng Cycloid Hình 4b Quỹ đạo thực UAV bám theo quỹ đạo đặt Hình Kết mơ dẫn đường sử dụng chế độ trượt với mặt trượt phi tuyến dựa hệ tọa độ Serret-Frenet có điểm đầu (20;50) KẾT LUẬN Qua kết mô nhận thấy dẫn đường sở chế độ trượt với mặt trượt phi tuyến dựa hệ tọa độ Serret-Frenet giúp toán xác định khoảng cách từ UAV đến đường quỹ đạo giải phóng, làm giảm phức tạp của thuật tốn bám đường quỹ đạo, đồng thời loại bỏ đ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ệ thuật toán bám đường UAV nằm tâm cung tròn, hệ thống ổn định nhanh hoạt động chế độ Đây sở để điều khiển UAV thực nhiệm vụ bám theo quỹ đạo đặt trước TÀI LIỆU THAM KHẢO [1] [2] [3] [4] [5] [6] 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 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 A Ratnoo, P Sujit, and M Kothari, “Optimal path following for high wind flights,” Proc of the IFAC World Congress, Aug 2011 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 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ố 55, tháng 6/2018) 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 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 năm 2019 Địa chỉ: Viện Tự động hóa KTQS; Cục Khoa học quân sự; Viện Khoa học Công nghệ Quân * Email: ptpanh2003@yahoo.com Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san TĐH, 04 - 2019 59 ... gốc tọa độ; k hệ số đường cong; u1 tan ; góc nghiêng UAV hệ tọa độ S-F 52 (2) 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ệ... "Thuật toán dẫn đường cho UAV … tọa độ Serret-Frenet. " Nghiên cứu khoa học cơng nghệ thuật tốn bám đường UAV nằm tâm cung tròn, hệ thống ổn định nhanh hoạt động chế độ Đây sở để điều khiển UAV. .. thuật điều khiển & Tự động hóa Hình Hệ tọa độ Serret-Fernet tọa độ UAV Trong mặt phẳng nằm ngang đường quỹ đạo mô tả quỹ đạo điểm gốc tọa độ O hệ tọa độ FS trượt theo đường quỹ đạo Giả sử l độ