Xây dựng hệ thống nhận thức môi trường cho robot tự hành omni bốn bánh dựa trên thuật toán EKF SLAM và hệ điều hành ROS

Kỹ thuật Điều khiển – Tự động hóa Đ Q Hiệp, , P X Minh, “Xây dựng hệ thống nhận thức và hệ điều hành ROS ” 30 XÂY DỰNG HỆ THỐNG NHẬN THỨC MÔI TRƯỜNG CHO ROBOT TỰ HÀNH OMNI BỐN BÁNH DỰA TRÊN THUẬT TOÁN.

Kỹ thuật Điều khiển – Tự động hóa XÂY DỰNG HỆ THỐNG NHẬN THỨC MÔI TRƯỜNG CHO ROBOT TỰ HÀNH OMNI BỐN BÁNH DỰA TRÊN THUẬT TOÁN EKF-SLAM VÀ HỆ ĐIỀU HÀNH ROS Đỗ Quang Hiệp1*, Ngô Mạnh Tiến2, Nguyễn Mạnh Cường2, Lê Trần Thắng3, Phan Xuân Minh4 Tóm tắt: Lập quỹ đạo chuyển động toán quan trọng điều khiển chuyển động cho robot tự hành, đó, hệ thống nhận thức mơi trường đóng vai trị cốt lõi cung cấp thơng tin mơi trường hoạt động cho robot Hệ thống nhận thức môi trường có nhiệm vụ đồ hố đồng thời tự định vị robot môi trường hoạt động (SLAM - Simultaneous Localization and Mapping) phát vật cản trình robot di chuyển Bài báo trình bày thiết kế xây dựng hệ thống nhận thức môi trường hoạt động cho robot Omni bốn bánh dựa thuật tốn EKF-SLAM hệ điều hành lập trình robot ROS (Robot Operating System) Các kết mô phần mềm Gazebo, thực nghiệm dựa phần cứng robot tự hành Omni bốn bánh tự xây dựng Từ khóa: Hệ điều hành robot (ROS); Gazebo; Rviz; Định vị đồ hóa đồng thời (SLAM); Robot Omni; EKF-SLAM MỞ ĐẦU Tính thơng minh hệ thống robot tự hành phụ thuộc vào việc robot tự tính tốn quỹ đạo phù hợp với mơi trường hoạt động Bởi vậy, nhận thức môi trường xung quanh bao gồm thông tin cố định hay thay đổi môi trường yếu tố tiên quyết, ảnh hưởng trực tiếp tới việc chuyển động robot Do đó, hệ thống nhận thức cung cấp thông tin môi trường cho robot có vai trị quan trọng hệ thống robot tự hành [1, 2] Một hệ thống robot tự hành hồn chỉnh thơng thường gồm phần chính: hệ thống lập quỹ đạo hệ thống điều khiển bám quỹ đạo Trong đó, hệ thống lập quỹ đạo chuyển động cần thông tin môi trường hoạt động Việc thiết kế hệ thống có khả cung cấp thông tin môi trường tập trung nghiên cứu [4-7] Các nghiên cứu tập trung cho hệ robot nonholonomic, hệ robot có bánh lái khơng có bánh lái Tuy nhiên, hệ thống chưa thiết kế cho hệ thống robot đa hướng dạng holonomic Bản đồ hoá ước lượng vị trí robot mơi trường hoạt động (SLAM) hai thông tin quan trọng robot hoạt động môi trường trước Vấn đề SLAM phát sinh việc điều hướng robot di động thông qua môi trường không xác định mà đồ [3, 4, 6, 9] Các kỹ thuật sử dụng xác suất robotic nghiên cứu để đưa kỹ thuật xử lý vấn đề SLAM [12] [13] Khi hệ điều hành ROS đời, việc xây dựng hệ thống robot sử dụng kỹ thuật SLAM để đồ hoá định vị ngày phổ biến [3, 6, 7] [8] Các kỹ thuật xử lý vấn đề SLAM lấy giá trị thông tin đầu vào từ cảm biến độ sâu, cảm biến khoảng cách, điểm ảnh 3D từ tính tốn giá trị cho việc đồ hố định vị robot Bên cạnh đó, tín hiệu từ cảm biến thu q trình di chuyển sử dụng để phát vật cản động bất ngờ xuất Bài báo trình bày việc thiết kế hệ thống nhận thức cho robot Omni đa hướng dựa kỹ thuật SLAM tảng ROS [14] Nội dung báo chia làm phần Mơ hình động học cho robot Omni trình bày phần Tiếp đó, đóng góp báo thiết kế hệ thống nhận thức dựa kỹ thuật EKF-SLAM 30 Đ Q Hiệp, …, P X Minh, “Xây dựng hệ thống nhận thức … hệ điều hành ROS.” Nghiên cứu khoa học công nghệ tảng ROS phần Phần kết mô thực nghiệm để kiểm chứng tính khả thi hiệu hệ thống Cuối cùng, phần kết luận hướng nghiên cứu MƠ HÌNH ROBOT TỰ HÀNH OMNI BỐN BÁNH VÀ HỆ ĐIỀU HÀNH ROS Mơ hình động học robot sử dụng để đưa vào thuật tốn EKF-SLAM để tính tốn độ dời vị trí cho robot thời điểm Mơ hình động học cho robot Omni bốn bánh xây dựng dựa mơ hình với bánh xe Omni bố trí đặt cách góc 900 hình q =  x y   véc tơ tọa độ robot hệ tọa độ tồn cục Oxy , đó, x y toạ độ robot theo phương Ox Oy ,  góc quay robot so với chiều dương trục Ox T v  vx vy   T véc tơ vận tốc robot hệ trục tọa độ động gắn vào tâm robot bao gồm vận tốc tịnh tiến vận tốc góc robot Hình Hệ trục tọa độ robot tự hành bốn bánh đa hướng Xét mối quan hệ hình học hình 1, ta xác định biểu thức mối quan hệ vận tốc robot hệ tọa độ toàn cục hệ tọa độ động Phương trình biểu diễn mối quan hệ phương trình động học robot cos q  H v   sin   Theo hình 1, bánh xe  sin  cos  0  v  đặt hợp (1) với trục vx góc    / 4, cos( )  sin(  )  / Khi có vận tốc bánh xe, ta suy giá trị vận tốc robot từ tính tốn vị trí robot thơng qua phương trình động học   vx    r  vy        d     2  d  d   1             H2    3    d       4   4  (2) với d khoảng cách từ tâm robot tới bánh r bán kính bánh xe Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san Hội thảo Quốc gia FEE, 10 - 2020 31 Kỹ thuật Điều khiển – Tự động hóa HỆ THỐNG NHẬN THỨC CHO ROBOT TỰ HÀNH 3.1 Xây dựng thuật toán EKF-SLAM cho robot Omni Hệ thống nhận thức robot thiết kế dựa kỹ thuật để xử lý vấn đề đồ hố mơi trường định vị đồng thời (SLAM) Trong lĩnh vực robotics, vấn đề định vị đồng thời xây dựng đồ SLAM vấn đề quan trọng đóng vai trị then chốt điều hướng robot Ngồi ra, tín hiệu từ cảm biến tầm nhìn sử dụng để phát vật cản xuất trình robot di chuyển SLAM định nghĩa trình mà robot xây dựng đồ mơi trường, đồng thời sử dụng đồ để ước tính vị trí robot vị trí vật cản mơi trường hoạt động Định nghĩa vector trạng thái yt bao gồm trạng thái robot toạ độ điểm mốc m   m1, x m1, y đồ m2, x q  x y  T m2, y mN , x vector trạng thái robot mN , y  vector chứa toạ độ N điểm mốc T toạ độ vật cản tĩnh mơi trường Từ đó, ta có yt = (q/m) với x, y  biểu diễn tọa độ hướng robot thời điểm t , mi , x , mi , y tọa độ điểm mốc thứ i , với i  1,2 , N Số chiều vector trạng thái N  , với N biểu diễn số điểm mốc đồ Từ đó, thuật tốn EKF-SLAM sử dụng để tính toán phân phối xác suất: p( yt | z1:t , u1:t ) (3) Thuật toán EKF-SLAM bao gồm hai bước đệ quy theo dạng chuẩn vấn đề SLAM: bước dự đoán (Prediction) bước chỉnh sửa (Correction) 3.1.1 Bước dự đốn (Prediction) Từ biểu thức phương trình động học (1), yt viết lại thành:  vx (t )cos  v y (t )sin     yt  yt 1  F  vx (t )sin   v y (t )cos   t    (t )   T x (4)  0    với t thời gian trích mẫu Fx   0  ma trận có kích thước  0     (2 N  3) Chuyển động robot ảnh hưởng tới giá trị trạng thái robot điểm mốc giữ không đổi nên giá trị cập nhật cho điểm mốc Từ đó, giá trị kỳ vọng mong muốn cho phân phối xác suất bước dự đốn tính bởi:  vx cos t 1,  v y sin t 1,  t  t 1  F  vx sin t 1,  v y cos t 1,  t      T x (5) Mơ hình chuyển động đầy đủ có nhiễu mơ tả bởi:  vx (t )cos  v y (t )sin     yt  yt 1  F  vx (t )sin   v y (t )cos   t  N(0, FxT Rt Fx )    (t )   T x 32 (6) Đ Q Hiệp, …, P X Minh, “Xây dựng hệ thống nhận thức … hệ điều hành ROS.” Nghiên cứu khoa học công nghệ Với thành phần nhiễu N(0, FxT Rt Fx ) tuân theo phân phối chuẩn đa biến có kỳ vọng Rt ma trận hiệp phương sai FxT Rt Fx dạng mở rộng ma trận hiệp phương sai đến chiều ma trận trạng thái (6) viết lại dạng: yt  g (ut , yt 1 )  N(0, FxT Rt Fx ) (7)  vx (t )cos   v y (t )sin     Với: g (ut , yt 1 )  yt 1  F  vx (t )sin   v y (t )cos   t thành phần phi tuyến    (t )   mơ hình trạng thái Sử dụng chuỗi Taylor bậc mở rộng để tuyến tính hố xấp xỉ hàm g (ut , yt 1 ) : T x g (ut , yt 1 )  g (ut , t 1 )  Gt ( xt 1  t 1 ) (8) Với, t 1 giá trị kỳ vọng trạng thái yt 1 trước Ma trận Jacobi Gt  g (ut , t 1 ) đạo hàm theo yt 1 Từ đó, ta tính biểu thức cập nhật cho ma trận hiệp phương sai t  Gt t 1 GtT  FxT Rt Fx (9) 3.1.2 Bước chỉnh sửa (Correction) Mô hình đo lường biểu diễn vector zti   rti ti  với rti khoảng cách từ T cảm biến ti góc quay robot so với điểm mốc thứ i  r i   (mi , x  x)2  (mi , y  y )    +N(0,  r zti   t i    0  t   a tan 2(mi , y  y, mi , x  x)    0 )     r đó, (mi , x , mi , y )T toạ độ điểm mốc thứ i đo thời điểm t , N(0,  0  ( m  x)  ( m  y )  i,x i, y  nhiễu đo lường bất định Đặt: h( yt , i)    a tan 2(m  y, m  x)    i , y i , x    r  Qt    Hàm h( yt , i) xấp xỉ chuỗi Taylor bậc mở rộng:    h( yt , i)  h( t , i)  Hti ( yt  t ) (10) 0 )    (11) Ở đó, H ti đạo hàm h( t , i) theo t Hàm h( t , i) phụ thuộc vào trạng thái vector trạng thái, vị trí robot qt vị trí điểm mốc thứ i mi Tiếp đến vị trí mong muốn tính tốn từ tư biến đo lường điểm mốc:  i , x   t , x  i  cos(ti  t , )       rt  i  i , y   t , y   sin(t  t , )  (12) Với i , x i , y giá trị kỳ vọng mong muốn điểm mốc mi theo phương Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san Hội thảo Quốc gia FEE, 10 - 2020 33 Kỹ thuật Điều khiển – Tự động hóa Ox Oy Trong thuật toán EKF-SLAM, giá trị khoảng cách Mahalanobis sử dụng để liên kết giá trị đo lường với điểm mốc biết tạo điểm mốc tuỳ vào việc so sánh khoảng cách với giá trị ngưỡng chọn trước  Khoảng cách Mahalanobis tính:  k  ( zti  zˆtk )T  k1 ( zti  zˆtk ) (13) Với zti phép đo lường thực tới điểm mốc thứ i zˆtk giá trị đo lường mong muốn điểm mốc xác định, k  1, , N  với N  giá trị đánh dấu để tạo điểm mốc phép đo liên kết trả giá trị không nằm giá trị ngưỡng  k ma trận hiệp phương sai cập nhật theo phép đo: T  k  H tk t  H tk   Qt (14) 3.2 Xây dựng hệ thống nhận thức cho robot Omni dựa tảng ROS Hệ điều hành ROS tảng linh hoạt cho việc lập trình phần mềm cho hệ thống robot hình Nó bao gồm cơng cụ thư viện nhằm đơn giản hoá việc xây dựng hệ thống robot phức tạp việc kết hợp tảng robot với Hơn nữa, ROS xây dựng để tạo điều kiện cho việc phát triển kết hợp phần mềm robot với cách thuận lợi Hình Hệ thống nhận thức tảng ROS cấu hình phần cứng hệ thống robot Thuật toán EKF-SLAM thực thi node đồ (Map) vị trí robot (Robot Pose), xuất liệu đồ vị trí robot Hình Giao diện thực thi thuật toán EKF-SLAM ROS vùng bao quanh robot chứa thông tin vật cản (costmap) 34 Đ Q Hiệp, …, P X Minh, “Xây dựng hệ thống nhận thức … hệ điều hành ROS.” Nghiên cứu khoa học cơng nghệ Hệ thống nhận thức cịn có chức phát vật cản dựa vào tín hiệu cảm biến khoảng cách điểm ảnh 3D (point cloud 3D) Một vùng bao quanh robot gọi vùng costmap cục Vùng costmap cục chọn vùng hình vng với robot vị trí trung tâm Kích thước hình vng chọn cho đảm bảo robot phát vật cản từ khoảng cách an tồn Bên cạnh đó, giá trị điểm ảnh 3D giới hạn vùng tác động bên vùng costmap hình KẾT QUẢ MÔ PHỎNG VÀ THỰC NGHIỆM 4.1 Kết mô Mô thực phần mềm mô Gazebo ROS Các vùng màu trắng vùng môi trường nhận thức Các đường màu đen vật cản mà robot nhận dạng đồ hoá sử dụng kỹ thuật EKF-SLAM Bộ EKF-SLAM liên tục tính tốn đồ hố môi trường dựa vào liệu đồng thời định vị robot đồ Sau di chuyển đến vị trí đủ để thu thập liệu cho việc đồ hố tồn mơi trường hình 4, liệu đồ lưu lại để tái sử dụng lần mà không cần phải tiến hành đồ hố lại mơi trường Hình Q trình SLAM robot kết đồ hố mơi trường 4.2 Kết thực nghiệm Hình Mơ hình robot Omni (trái) môi trường thực nghiệm (phải) Thử nghiệm kỹ thuật EKF-SLAM môi trường thực tế, kết thu hình Ban đầu, robot chưa có thông tin môi trường Trong hình 5, Tạp chí Nghiên cứu KH&CN qn sự, Số Đặc san Hội thảo Quốc gia FEE, 10 - 2020 35 Kỹ thuật Điều khiển – Tự động hóa góc bên trái ảnh màu thu từ camera để theo dõi trình di chuyển robot Vùng màu trắng phần môi trường đồ hố, đó, đường màu đen bao quanh đường màu xanh dương vật cản mơi trường đồ hố Vật cản hiển thị dạng 3D point cloud phần Rviz KẾT LUẬN Bài báo thiết kế xây dựng hệ thống nhận thức môi trường cho robot tự hành đa hướng bốn bánh Omni dựa thuật toán EKF-SLAM hệ điều hành ROS Hệ thống thử nghiệm mơ hình mơ mơ hình thật robot Hệ thống nhận thức robot xây dựng dựa liệu thu từ Astra Camera Rplidar A2 trình chuyển động robot Sau đó, mơi trường đồ hố robot đồng thời tự định vị mơi trường Hệ thống nhận thức cịn cung cấp thơng tin vật cản xuất trình robot di chuyển Các kết đạt cho thấy tính hiệu thực tế hệ thống nhận thức xây dựng TÀI LIỆU THAM KHẢO [1] Hashemi, Ehsan, Maani Ghaffari Jadidi, and Navid Ghaffari Jadidi, "Model-based PI– fuzzy control of four-wheeled omni-directional mobile robots," Robotics and Autonomous Systems, (2011), pp 930-942 [2] L a S H Lin, "Modeling and Adaptive Control of an Omni-Mecanum-Wheeled Robot," Intelligent Control and Automation, vol (2013), pp 166-179 [3] R L e a Guimarães, "ROS navigation: Concepts and tutorial," Springer, Cham, (2016), pp 121-160 [4] A a P A Pajaziti, "SLAM–map building and navigation via ROS," International Journal of Intelligent Systems and Applications in Engineering, vol 2(4) (2014), pp 71-75 [5] A I A a E M Buyval, "Comparative analysis of ROS-based monocular SLAM methods for indoor navigation," Ninth International Conference on Machine Vision, vol 10341 (2016) [6] Z e a An, "Development of Mobile Robot SLAM Based on ROS," International Journal of Mechanical Engineering and Robotics Research, (2016), pp 47-51 [7] R e a Giubilato, "An evaluation of ROS-compatible stereo visual SLAM methods on a nVidia Jetson TX2," Measurement, (2019), pp 161-170 [8] R N a M K B Darmanin, "Autonomous Exploration and Mapping using a Mobile Robot Running ROS," ICINCO, 2016 [9] R K e a Megalingam, "ROS based autonomous indoor navigation simulation using SLAM algorithm," Int J Pure Appl Math, (2018), pp 199-205 [10] P e a Marin-Plaza, "Global and local path planning study in a ROS-based research platform for autonomous vehicles," Journal of Advanced Transportation, (2018) [11] C F H a T B Rösmann, "Online trajectory planning in ROS under kinodynamic constraints with timed-elastic-bands," Robot Operating System (ROS) Springer, Cham, (2017), pp 231-261 [12] F e a Albers, "Online Trajectory Optimization and Navigation in Dynamic Environments in ROS," Robot Operating System (ROS) Springer, Cham, (2019), pp 241-274 [13] S Thrun, "Probabilistic robotics," Communications of the ACM, (2002), pp 52-57 [14] S W B a D F Thrun, Probalistic robotics, (2006) 36 Đ Q Hiệp, …, P X Minh, “Xây dựng hệ thống nhận thức … hệ điều hành ROS.” Nghiên cứu khoa học công nghệ ABSTRACT CONSTRUCTING AN ENVIRONMENTAL PERCEPTION SYSTEM FOR A FOUR-WHEELED OMNIDIRECTIONAL MOBILE ROBOT BASED ON EKF-SLAM AND ROS Trajectory motion planning is an important problem in motion control for autonomous mobile robots, in which the environmental perception system plays a core role because it provides information about the operating environment for robots The environmental perception system has the task of mapping the operating environment and simultaneously positioning the robot in the map (SLAM Simultaneous Localization and Mapping) and detecting obstacles In the paper, the design and construction of an environmental perception system for a four-wheeled Omni robot based on EKF-SLAM algorithm and the ROS (Robot Operating System) are presented The results were simulated on Gazebo software, based on self-built four-wheel Omni robot hardware Keywords: Robot Operating System (ROS); Gazebo; Rviz; Simultaneous Localization and Mapping (SLAM); Omni robot; EKF-SLAM Nhận ngày 29 tháng năm 2020 Hoàn thiện ngày 05 tháng 10 năm 2020 Chấp nhận đăng ngày 05 tháng 10 năm 2020 Địa chỉ: 1Khoa Điện, Trường Đại học KT-KT Công nghiệp; Viện Vật lý, Viện Hàn Lâm KH&CN Việt Nam; Viện Tự động hóa KTQS, Viện KHCNQS; Đại học Bách khoa Hà Nội * E-mail: dqhiep@uneti.edu.vn Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san Hội thảo Quốc gia FEE, 10 - 2020 37 ... xây dựng hệ thống nhận thức môi trường cho robot tự hành đa hướng bốn bánh Omni dựa thuật toán EKF- SLAM hệ điều hành ROS Hệ thống thử nghiệm mơ hình mơ mơ hình thật robot Hệ thống nhận thức robot. .. tk t  H tk   Qt (14) 3.2 Xây dựng hệ thống nhận thức cho robot Omni dựa tảng ROS Hệ điều hành ROS tảng linh hoạt cho việc lập trình phần mềm cho hệ thống robot hình Nó bao gồm cơng cụ thư... TỰ HÀNH 3.1 Xây dựng thuật toán EKF- SLAM cho robot Omni Hệ thống nhận thức robot thiết kế dựa kỹ thuật để xử lý vấn đề đồ hố mơi trường định vị đồng thời (SLAM) Trong lĩnh vực robotics, vấn đề