Thuật toán lập quỹ đạo di chuyển nhanh và đơn giản cho thiết bị tự hành

KHOA HỌC CƠNG NGHỆ P-ISSN 1859-3585 E-ISSN 2615-9619 THUẬT TỐN LẬP QUỸ ĐẠO DI CHUYỂN NHANH VÀ ĐƠN GIẢN CHO THIẾT BỊ TỰ HÀNH A FAST AND SIMPLE TRAJECTORY PLANNING ALGORITHM FOR AUTOMOTIVE DEVIES Nguyễn Ngọc Tuấn1,*, Tăng Thanh Lâm1, Lại Tiến Đệ1, Trần Xn Tình2 TĨM TẮT Xác định quỹ đạo di chuyển từ vị trí A đến B môi trường động, đảm bảo tránh vật cản nhiệm vụ phức tạp thiết bị tự hành Bài báo đề xuất thuật toán lập quỹ đạo di chuyển (trajectory planning) tốc độ cao, yêu cầu cài đặt đơn giản, đảm bảo an tồn cho thiết bị tự hành có phần cứng khơng cao Từ khóa: Lập quỹ đạo di chuyển, tìm đường ngắn nhất, xe tự lái, thiết bị tự hành ABSTRACT Determining the trajectory moving from position A to B, while avoiding obstacles as well as changing the environment is a simple task for humans but very complex for autonomous devices Typically, autonomous devices will use sensors to build maps of the surrounding environment, through which to determine the trajectory of movement as well as appropriate control action to reach the desired destination This article proposes a high-speed trajetory planning algorithm that requires simple settings to ensure safety for selfpropelled equipment with low hardware requirements Keywords: Motion Planning, shortest path searching, self-driving car, autonomous equipment Học viện Kỹ thuật Qn Học viện Phịng khơng Khơng qn * Email: ngoctuanhvhn@gmail.com Ngày nhận bài: 29/10/2020 Ngày nhận sửa sau phản biện: 10/12/2020 Ngày chấp nhận đăng: 26/02/2021 GIỚI THIỆU Nhưng năm gần đây, thiết bị tự hành dần đưa vào ứng dụng thực tiễn nhiều thay hoạt động môi trường chuyên biệt dây chuyền sản xuất cơng nghiệp Từ đặt u cầu thiết bị tự hành có khả làm việc linh hoạt trước thay đổi môi trường xung quanh Do đố, giải toán lập quỹ đạo di chuyển an toán, tránh vật cản tĩnh động thiết bị tự hành có vai trị đặc biệt quan trọng Các thuật tốn thường gọi thuật toán lập kế hoạch di chuyển (path planning) hay thuật tốn tìm đường vấn đề nghiên cứu lâu dạng toán miền liên thơng, tốn tìm đường ngắn đồ thị Nhiều thuật toán giải trọn vẹn tốn với kết lộ trình tối ưu với nhiều phương pháp tiếp cận khác Trong thực tế, môi trường xung quanh lúc biết cách đầy đủ hay mơ tả cách xác đồ thị Ngoài ra, thiết bị tự hành, vấn đề tính tốn thời gian thực trước thay đổi môi trường yêu cầu bắt buộc Bài báo đề xuất thuật toán lập quỹ đạo nhanh, đơn giản, độ phức tạp thấp nhằm giải vấn đề TỔNG QUAN VỀ CÁC GIẢI THUẬT LẬP QUỸ ĐẠO DI CHUYỂN ĐÃ ĐƯỢC NGHIÊN CỨU Lập quỹ đạo di chuyển áp dụng đồ môi trường biết trước cách đầy đủ phần Do đó, robot phải có khả định vị lập đồ thời gian thực lập áp dụng thuật toán xây dựng quỹ đạo di chuyển tối ưu [1, 2, 3] Có nhiều cách phân loại thuật toán lập quỹ đạo di chuyển, nhiên, ta chia chúng làm hai loại lập quỹ đạo di chuyển sở hình học khơng gian (geometric-based planner) lập quỹ đạo di chuyển sở điều khiển (control-based planner) [4] 2.1 Phương pháp lập quỹ đạo di chuyển sở hình học khơng gian (geometric - based planner) Các quỹ đạo di chuyển lập dựa phương pháp quan tâm đến ràng buộc hình học Người ta cho đường khả thi hình học chuyển đổi quỹ đạo khả thi động học [5] Kết phương pháp đường không va chạm điểm ban đầu mục tiêu cuối thiết bị với số lượng điểm tham chiếu tối thiểu, mơ ta hình học chuyển động thiết bị, tọa độ (x, y, z) điểm nối lộ trình Lập quỹ đạo sở hình học chia làm hai nhóm: Muli-query planners Single-query plannesr, đó: Muli-query planners: Nhiều lộ trình tính tốn đồng thời với thuật tốn dừng lại lộ trình đến đích, tương tự thuật tốn tìm kiếm theo chiều rộng (BFS) Thuật toán tiêu biểu theo phương pháp là: Probability Roadmap Method (PRM) Single-query plannesr: Các hướng di chuyển tiềm thêm vào định duyệt theo ưu tiên kết thúc thỏa mãn điều kiện ràng 34 Tạp chí KHOA HỌC VÀ CÔNG NGHỆ ● Tập 57 - Số (02/2021) Website: https://tapchikhcn.haui.edu.vn SCIENCE - TECHNOLOGY P-ISSN 1859-3585 E-ISSN 2615-9619 buộc Khi tất quỹ đạo chuyển động xây dựng, thuật toán đánh giá quỹ đạo đến đích đưa kết quỹ đạo có chi phí di chuyển thấp Hình So sánh kỹ thuật rời rạc hóa sở hình học (trái) sở điều khiển (phải) Hình Ví dụ kết lập quỹ đạo di chuyển thuật toán lập quỹ đạo di chuyển sở hình học Có thể thấy, lộ trình mà thuật tốn phương pháp đưa đường gấp khúc nối từ điểm đầu đến điểm cuối lộ trình cần chuyển đổi sang quỹ đạo di chuyển mượt cho phù hợp với dạng di chuyển thiết bị Tuy nhiên, trình chuyển đổi làm tăng độ phức tập tính tốn thuật tốn làm tối ưu ban đầu lộ trình 2.2 Phương pháp lập quỹ đạo di chuyển sở điều khiển (control-based planner) Việc tính tốn quỹ đạo kiểm tra quỹ đạo có xảy va chạm hay khơng vấn đề khó khăn làm tăng độ phức tạp phương pháp xây dựng quỹ đạo di chuyển dựa sở điều khiển ĐỀ XUẤT THUẬT TOÁN LẬP QUỸ ĐẠO DI CHUYỂN NHANH, ĐƠN GIẢN CHO THIẾT BỊ TỰ HÀNH 3.1 Cơ sở thuật toán Thuật toán lập quỹ đạo di chuyển nhóm tác giả đề xuất báo thuộc phương pháp lập quỹ đạo di chuyển sở điều khiển Để giảm thiếu số quỹ đạo cần tính tốn, nhóm tác giả đề xuất xây dựng thuật toán sở kết hợp ý tưởng hai nhóm Muli-query planners Single-query plannesr Ngồi nhóm tác giả đưa phương pháp tính nhanh quỹ đạo kiểm tra va chạm cho quỹ đạo nhanh làm giảm độ phức tạp thuật tốn Hình Quỹ đạo di chuyển đưa theo phương pháp lập quỹ đạo sở điều khiển Phương pháp lập quỹ đạo di chuyển sử dụng điều kiện động lực học hay điều khiển xét đến trình xây dựng quỹ đạo di chuyển Trong lập quỹ đạo di chuyển dựa sở hình học chuỗi điểm biểu diễn cấu hình hình học hay tọa độ điểm nối khơng gian lộ trình phương pháp lập quỹ đạo sở điều khiển đưa kết chuỗi quỹ đạo di chuyển biểu diễn quỹ đạo thực tế lộ trình di chuyển cần thiết Hình Chế độ điều khiển move (trái) chế độ điều khiển drive (phải) drone Có thể thấy, so với phương pháp lập quỹ đạo di chuyển sở hình học, phương pháp cho kết lộ trình trực quan, xác sử dụng để đưa định điều khiển Mặc dù vậy, tính toán thực tế, thuật toán dựa sở hình học thường sử dụng kỹ thuật rời rạc không gian thành dạng lưới miền liên thơng từ giảm đáng kể chi phí tính toán mà đảm độ tối ưu cao [6] Trong đó, thuật tốn dựa sở điều khiển rời rạc hóa tốn cách rời rạc hóa quỹ đạo điều khiển kết hợp với rời rạc hóa thời gian tính quỹ đạo độ phức tạp tính tốn lớn có q nhiều quỹ đạo phù hợp cần xét đến Website: https://tapchikhcn.haui.edu.vn Hình Bài tốn lập quỹ đạo di chuyển cho drone đến đích (màu đen minh họa vật cản, màu vàng vùng nguy hiểm) Để thuận tiện cho việc trình bày thuật tốn, ta xét ví dụ cụ thể tốn điều khiển drone đến mục Vol 57 - No (Feb 2021) ● Journal of SCIENCE & TECHNOLOGY 35 KHOA HỌC CƠNG NGHỆ tiêu, tránh vật cản khơng gian 2D Giả sử drone trang bị cảm biến cần thiết định vị lập đồ mơi trường xung quanh bán kính giới hạn Rmax Ta xét drone trường hợp điều khiển theo chế độ lái (drive) - với tốc độ tiến Vx (m/s) tốc độ góc quay ω (rad/s) Trong trường hợp chế độ điều khiển move (di chuyển tịnh tiến) kết hợp hai chế độ ta tiến hành tương tự P-ISSN 1859-3585 E-ISSN 2615-9619 dùng để kiểm tra va chạm, mask tao lưu trữ trước Drone định vị đồ (environment map) vị trí góc xoay, vùng đồ phía trước drone crop để kiểm tra va trạm Vùng map crop resize kích thước mask quỹ đạo, để kiểm tra quỹ đạo có xảy va chạm hay không, ta cần dùng hàm XOR ảnh, có pixel trùng ảnh, ta biết quỹ đạo va chạm 3.2 Mợt số tḥt tốn phụ cần thiết Thuật tốn 1: Tìm quỹ đạo di chuyển trạng thái drone ứng với vận tốc tiến Vx (m/s) tốc độ vận tốc quay ω (rad/s) Ta có: R = (2π/ω)*Vx/2π = Vx/ω Có thể thấy quỹ đạo R drone phụ thuộc vào tỉ lệ Vx/ω ngược lại Vì thế, nhắc đến quỹ đạo, ta coi nói đến tỉ số Vx/ω (Vx, ω có dấu) Giả sử vị trí drone (X0,,Y0), hướng tiến drone hợp với trục Ox góc A0 (rad) Trạng thái drone sau khoảng thời gian dt là: A1 = A0 + ω*dt; X1 = X0 + R*(cosA0 – cosA1); Y1 = Y0 + R*(sinA0 + sinA1) Hình Tính quỹ đạo chuyển động R theo lệnh điều khiển Vx, ω Thuật toán 2: Đề xuất quỹ đạo di chuyển: Hình Ví dụ kiểm tra va chạm cho quỹ đạo với trạng thái drone 3.3 Thuật toán lập quỹ đạo di chuyển Tại trạng thái drone, để tìm quỹ đạo di chuyển đến mục tiêu, tư tưởng thuật toán sau: Tại thời điểm, để tìm đường tốt nhất, drone thực tính tốn quỹ đạo tạo M quỹ đạo (tính trước M bước) Nếu mục tiêu xa vị trí drone, quỹ đạo đưa thuật toán quỹ đạo có khả tốt hướng đến mục tiêu chưa hoàn thành quỹ đạo hoàn chỉnh đến mục tiêu Điều phù hợp thực tế phạm vi cảm biến để xây dựng đồ mơi trường hạn chế, nên mục tiêu nằm ngồi phạm vi Hơn nữa, mơi trường mơi trường động, tính tốn trước q nhiều bước, kết bước cuối (tương lai xa) thiếu xác hay khơng có nhiều ý nghĩa Tại bước đầu tiên, từ trạng thái drone, ta chọn N quỹ đạo tốt theo số tiêu chí (các quỹ đạo không xảy va chạm) Từ N quỹ đạo đó, ta tính tốn N trạng thái drone Trong K-1 bước tiếp theo, ta thực tương tự với trạng thái tính tốn bước trước, ta tiếp tục chọn N quỹ đạo tốt Tại M-K bước cuối cùng, với trạng thái tính tốn bước trước, ta chọn quỹ đạo tốt Hình Ví dụ quỹ đạo di chuyển xét đến Để rời rạc toán, quỹ đạo di chuyển drone rời rạc hóa Trong trình kiểm tra quỹ đạo thỏa mãn, ta xem xét quỹ đạo Các quỹ đạo rời rạc tính tốn trước chúng không đổi nên giảm thiểu độ phức tạp thuật toán chạy Thuật toán 3: Kiểm tra va chạm nhanh sử dụng phương pháp xử lý ảnh Các quỹ đạo tính tốn trước thuật tốn 3, đồng thời, thuật toán tương ứng với mask Như ta có tất NK quỹ đạo, quỹ đạo xây dựng từ M quỹ đạo Ta đánh giá quỹ đạo theo số tiêu chí mong muốn tổng quãng đường ngắn nhất, lộ trình mượt nhất, trạng thái cuối drone để đến đích thuận tiện (gần đích nhất, hướng tiến done hướng tiến đích tạo thành góc nhỏ nhất, ) Cuối cùng, Drone chọn quỹ đạo có điểm số tốt từ đưa định điều khiển tương ứng Với tham số trên, ta viết tắt cấu hình config(M,N,K) Nếu K = M, hay bước ta xem xét N quỹ đạo con, ta phải tính tốn tổng cộng NM quỹ 36 Tạp chí KHOA HỌC VÀ CÔNG NGHỆ ● Tập 57 - Số (02/2021) Website: https://tapchikhcn.haui.edu.vn SCIENCE - TECHNOLOGY P-ISSN 1859-3585 E-ISSN 2615-9619 đạo Trong thử nghiệm thực tế, K nhỏ nhiều so với M mà không làm giảm khả tránh vật cản thuật tốn Ví dụ cấu hình config(5, 3, 2) Nếu K = M, ta cần tính tốn 35 = 243 quỹ đạo, với K = 2, ta cần tính 32 = quỹ đạo Với M, N lớn hơn, số quỹ đạo cần tính tốn K = M lớn tăng theo hàm mũ Hình Ví dụ quỹ đạo tính tốn với cấu hình config(4, 3, 1) đồng thời giảm thời gian lấy mẫu để quỹ đạo di chuyển drone tối ưu Với cấu hình thuật tốn config(4, 5, 1) thử nghiệm cấu hình phần cứng chíp Intel core i5 8400, đạt fps 180; raspberry đạt fps 58 Như vậy, thuật tốn hồn tồn đáp ứng thời gian thực với cấu hình phần cứng thấp Đặc biệt, ta cải thiện thuật toán thêm cách sử dụng kỹ thuật xử lý đa luồng dùng GPU tính tốn cơng đoạn xử lý ảnh Hình 13 Mơ thuật tốn khơng gian hẹp (đường hầm) KẾT QUẢ MƠ PHỎNG KẾT LUẬN Thuật tốn lập quỹ đạo di chuyển với toán điều khiển drone phần mô ngôn ngữ Python kết hợp với sử dụng thư viện xử lý ảnh OpenCV Cấu hình thuật tốn config(4, 5, 1) Bài báo đề xuất thuật toán dùng để lập quỹ đạo di chuyển nhanh đơn giản, đảm bảo an tồn điều kiện mơi trường thay đổi cho thiết bị tự hành Thuật tốn có độ phức tạp thấp, lập quỹ đạo thời gian thực máy tính nhúng giá rẻ Raspberry Pi, Jetson Nano Kết mơ tốn điều khiển drone cho kết tốt nhiều dạng địa hình phức tạp Quỹ đạo mà thuật tốn tìm ra trực tiếp thực điều khiển đến drone mà trải qua trình mềm hóa quỹ đạo, giúp cho việc thiết lập điều khiển đơn giản Hình 10 Mơ thuật tốn tránh vật cản kích thước lớn (điều kiện giống ngồi trời) Hình 11 Mơ thuật tốn tránh vật cản kích thước nhỏ (điều kiện giống nhà) TÀI LIỆU THAM KHẢO [1] Gregor Klancar, 2017 Wheeled Mobile Robotics Elsevier Inc, pp 161-206 [2] Spyros G Tzafestas, 2014 Introduction to Mobile Robot Control Elsevier Inc, pp 429-478 [3] V.I Zhulev, V.S Leushkin, T.N Nguyen, 2013 Real-time trajectory planning for unmanned ground vehicle Вестник РГРТУ, pp 18-22 [4] The Open Motion Planning Library, Rice University [Online] Available: https://ompl.kavrakilab.org [5] Ioan A Șucan, Mark Moll, Lydia E Kavraki, 2012 The Open Motion Planning Library IEEE Robotics & Automation Magazine, pp 72-82 [6] Zachary Kingston, Mark Moll, Lydia E, 2019 Exploring Implicit Spaces for Constrained Sampling-Based Planning International Journal of Robotics Research, Houston, pp 1151-1178 AUTHORS INFORMATION Hình 12 Mơ thuật tốn tránh nhiều vật cản kích thước nhỏ (điều kiện giống rừng cây) Nguyen Ngoc Tuan1, Tang Thanh Lam1, Lai Tien De1, Tran Xuan Tinh2 Le Quy Don University (Military Technical Academy) Air Defence - Air Force Academy Ở tất kiểm tra, thuật tốn ln tìm đường an tồn đến mục tiêu Có thể tăng tham số M, N, K, Website: https://tapchikhcn.haui.edu.vn Vol 57 - No (Feb 2021) ● Journal of SCIENCE & TECHNOLOGY 37 ... NHANH, ĐƠN GIẢN CHO THIẾT BỊ TỰ HÀNH 3.1 Cơ sở thuật toán Thuật tốn lập quỹ đạo di chuyển nhóm tác giả đề xuất báo thuộc phương pháp lập quỹ đạo di chuyển sở điều khiển Để giảm thiếu số quỹ đạo. .. Tính quỹ đạo chuyển động R theo lệnh điều khiển Vx, ω Thuật toán 2: Đề xuất quỹ đạo di chuyển: Hình Ví dụ kiểm tra va chạm cho quỹ đạo với trạng thái drone 3.3 Thuật toán lập quỹ đạo di chuyển. .. OpenCV Cấu hình thuật tốn config(4, 5, 1) Bài báo đề xuất thuật toán dùng để lập quỹ đạo di chuyển nhanh đơn giản, đảm bảo an toàn điều kiện môi trường thay đổi cho thiết bị tự hành Thuật tốn có