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

lõi bởi nĩ cung cấp thơng tin về mơi trường hoạt động cho robot. Hệ thống nhận thức mơi trường cĩ nhiệm vụ bản đồ hố đồng thời tự định vị robot trong mơi trường hoạt động (SLAM - Simultaneous Localization and Mapping) và phát hiện vật cản trong quá trình robot di chuyển. Bài báo trình bày về thiết kế và 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 trên thuật tốn EKF-SLAM và hệ điều hành lập trình robot ROS (Robot Operating System). Các kết quả được mơ phỏng trên phần mềm Gazebo, thực nghiệm dựa trên 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ị và bản đồ hĩa đồng thời (SLAM); Robot Omni; EKF-SLAM. 1. MỞ ĐẦU Tính thơng minh trong những hệ thống robot tự hành này phụ thuộc vào việc robot cĩ thể tự tính tốn ra quỹ đạo phù hợp với mơi trường nĩ hoạt động. Bởi vậy, những nhận thức về mơi trường xung quanh bao gồm cả những thơng tin cố định hay sự thay đổi của mơi trường là yếu tố tiên quyết, ảnh hưởng trực tiếp tới việc chuyển động của robot. Do đĩ, một hệ thống nhận thức cung cấp những thơng tin về mơi trường cho robot cĩ vai trị quan trọng trong các 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 2 phần chính: hệ thống lập quỹ đạo và hệ thống điều khiển bám quỹ đạo. Trong đĩ, hệ thống lập quỹ đạo chuyển động rất cần các thơng tin về mơi trường hoạt động. Việc thiết kế một hệ thống cĩ khả năng cung cấp thơng tin về mơi trường đã được tập trung nghiên cứu trong [4-7]. Các nghiên cứu này tập trung cho hệ robot nonholonomic, các hệ robot cĩ bánh lái hoặc khơng cĩ bánh lái. Tuy nhiên, những hệ thống này chưa được thiết kế cho hệ thống robot đa hướng dạng holonomic. Bản đồ hố và ước lượng vị trí của robot trong mơi trường hoạt động (SLAM) là hai thơng tin quan trọng nhất khi robot hoạt động trong mơi trường khơng biết trước. Vấn đề SLAM phát sinh trong việc điều hướng robot di động thơng qua các mơi trường khơng xác định mà khơng cĩ bản đồ [3, 4, 6, 9]. Các kỹ thuật sử dụng xác suất robotic được nghiên cứu để đưa ra kỹ thuật xử lý vấn đề SLAM trong [12] và [13]. Khi hệ điều hành ROS ra đời, việc xây dựng các hệ thống robot sử dụng kỹ thuật SLAM để bản đồ hố và định vị ngày càng phổ biến như trong [3, 6, 7] và [8]. Các kỹ thuật xử lý vấn đề SLAM lấy giá trị thơng tin đầu vào từ các cảm biến độ sâu, cảm biến khoảng cách, các điểm ảnh 3D và từ đĩ tính tốn ra các giá trị cho việc bản đồ hố và định vị robot. Bên cạnh đĩ, các tín hiệu từ cảm biến thu về trong quá trình di chuyển cũng được sử dụng để phát hiện những vật cản động bất ngờ xuất hiện. Bài báo này trình bày về việc thiết kế một hệ thống nhận thức cho robot Omni đa hướng dựa trên kỹ thuật SLAM và nền tảng ROS [14]. Nội dung bài báo được chia làm 5 phần chính. Mơ hình động học cho robot Omni được trình bày trong phần 2. Tiếp đĩ, đĩng gĩp chính của bài báo là thiết kế hệ thống nhận thức dựa trên kỹ thuật EKF-SLAM và nền 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 Hội thảo Quốc gia FEE, 10 - 2020 31 tảng ROS trong phần 3. Phần 4 là các kết quả mơ phỏng và thực nghiệm để kiểm chứng tính khả thi và hiệu quả của hệ thống. Cuối cùng, phần 5 là kết luận và hướng nghiên cứu. 2. 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 của robot sẽ được sử dụng để đưa vào thuật tốn EKF-SLAM để tính tốn độ dời vị trí cho robot tại mỗi thời điểm. Mơ hình động học cho robot Omni bốn bánh được xây dựng dựa trên mơ hình với bánh xe Omni được bố trí đặt cách nhau một gĩc 900 như trên hình 1.  = T q x y  là véc tơ tọa độ của robot trong hệ tọa độ tồn cục Oxy , trong đĩ, x và y lần lượt là toạ độ của robot theo phương Ox và Oy ,  là gĩc quay của robot so với chiều dương của trục Ox . T x yv v v     là véc tơ vận tốc của robot trong 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à vận tốc gĩc của robot. Hình 1. Hệ trục tọa độ của robot tự hành bốn bánh đa hướng. Xét mối quan hệ hình học trong hình 1, ta xác định được biểu thức mối quan hệ giữa vận tốc của robot trong hệ tọa độ tồn cục và hệ tọa độ động. Phương trình biểu diễn mối quan hệ này cũng là phương trình động học của robot cos sin 0 . sin cos 0 . 0 0 1 q H v v               (1) Theo hình 1, các bánh xe được đặt hợp với trục xv gĩc / 4, cos( ) sin( ) 2 / 2      . Khi cĩ vận tốc mỗi bánh xe, ta cũng cĩ thể suy ra giá trị vận tốc của robot và từ đĩ tính tốn được vị trí robot thơng qua phương trình động học. 1 1 2 2 2 3 3 4 4 2 2 2 2 2 2 2 2 4 x y v r v H d d d d                                                 (2) với d là khoảng cách từ tâm robot tới các bánh và r là bán kính bánh xe. 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.” 32 3. HỆ THỐNG NHẬN THỨC CHO ROBOT TỰ HÀNH 3.1. Xây dựng thuật tốn EKF-SLAM cho robot Omni Hệ thống nhận thức của robot được thiết kế dựa trên các kỹ thuật để xử lý vấn đề bản đồ hố mơi trường và định vị đồng thời (SLAM). Trong lĩnh vực robotics, vấn đề định vị đồng thời xây dựng bản đồ SLAM là một trong những vấn đề quan trọng nhất và đĩng vai trị then chốt trong điều hướng robot. Ngồi ra, tín hiệu từ các cảm biến tầm nhìn cũng được sử dụng để phát hiện vật cản xuất hiện trong quá trình robot di chuyển. SLAM được định nghĩa là một quá trình mà ở đĩ robot cĩ thể xây dựng bản đồ của mơi trường, đồng thời sử dụng bản đồ đĩ để ước tính vị trí của robot cũng như vị trí vật cản trong mơi trường hoạt động. Định nghĩa vector trạng thái ty bao gồm trạng thái của robot và toạ độ các điểm mốc trong bản đồ.   T q x y  là vector trạng thái của robot và  1, 1, 2, 2, , ,... T x y x y N x N ym m m m m m m là vector chứa toạ độ của N điểm mốc là toạ độ của các vật cản tĩnh trong mơi trường. Từ đĩ, ta cĩ yt = (q/m) với ,x y và  biểu diễn tọa độ và hướng của robot tại thời điểm t , ,i xm , ,i ym là tọa độ điểm mốc thứ i , với 1,2...,i N . Số chiều của vector trạng thái là 2 3N  , với N biểu diễn số điểm mốc trên bản đồ. Từ đĩ, thuật tốn EKF-SLAM sử dụng để tính tốn phân phối xác suất: 1: 1:( | , )t t tp y z u (3) Thuật tốn EKF-SLAM cũng bao gồm hai bước đệ quy theo dạng chuẩn của vấn đề SLAM: bước dự đốn (Prediction) và 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), ty được viết lại thành: 1 ( )cos ( )sin ( )sin ( )cos ( ) x y T t t x x y v t v t y y F v t v t t t                    (4) với t là thời gian trích mẫu và 1 0 0 0 ... 0 0 1 0 0 ... 0 0 0 1 0 ... 0 xF            là ma trận cĩ kích thước 3 (2 3)N  . Chuyển động của robot chỉ ảnh hưởng tới các giá trị của trạng thái robot cịn các điểm mốc vẫn giữ khơng đổi nên các giá trị cập nhật cho các điểm mốc đều bằng 0. Từ đĩ, giá trị kỳ vọng mong muốn cho phân phối xác suất ở bước dự đốn được tính bởi: 1, 1, 1 1, 1, cos sin sin cos x t y t T t t x x t y t v v F v v t                              (5) Mơ hình chuyển động đầy đủ khi cĩ nhiễu được mơ tả bởi: 1 ( )cos ( )sin ( )sin ( )cos (0, ) ( ) x y T T t t x x y x t x v t v t y y F v t v t t F R F t                     N (6) 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 Hội thảo Quốc gia FEE, 10 - 2020 33 Với thành phần nhiễu (0, )Tx t xF R FN tuân theo phân phối chuẩn đa biến cĩ kỳ vọng bằng 0 và tR là ma trận hiệp phương sai. T x t xF R F là dạng mở rộng ma trận hiệp phương sai đến các chiều của ma trận trạng thái. (6) được viết lại dưới dạng: 1( , ) (0, ) T t t t x t xy g u y F R F N (7) Với: 1 1 ( )cos ( )sin ( , ) ( )sin ( )cos ( ) x y T t t t x x y v t v t g u y y F v t v t t t                     là thành phần phi tuyến trong mơ hình trạng thái. Sử dụng chuỗi Taylor bậc nhất mở rộng để tuyến tính hố xấp xỉ hàm 1( , )t tg u y  : 1 1 1 1( , ) ( , ) ( )t t t t t t tg u y g u G x       (8) Với, 1t  là giá trị kỳ vọng của trạng thái 1ty  ngay trước đĩ. Ma trận Jacobi 1( , )t t tG g u   đạo hàm theo 1ty  . Từ đĩ, ta tính được biểu thức cập nhật cho ma trận hiệp phương sai 1 T T t t t t x t xG G F R F    (9) 3.1.2. Bước chỉnh sửa (Correction) Mơ hình đo lường được biểu diễn bởi vector   T i i i t t tz r  với i tr là khoảng cách từ cảm biến và it là gĩc quay của robot so với điểm mốc thứ i . 2 2 , , , , 0( ) ( ) (0, ) 0tan 2( , ) i ri x i yi t t i t i y i x m x m yr z a m y m x                           +N (10) ở đĩ, , ,( , ) T i x i ym m là toạ độ của điểm mốc thứ i đo được tại thời điểm ,t 0 (0, ) 0 r          N là nhiễu đo lường bất định. Đặt: 2 2 , , , , ( ) ( ) ( , ) tan 2( , ) i x i y t i y i x m x m y h y i a m y m x             và 0 0 r tQ           . Hàm ( , )th y i được xấp xỉ bằng chuỗi Taylor bậc nhất mở rộng: ( , ) ( , ) ( ) i t t t t th y i h i H y    (11) Ở đĩ, itH là đạo hàm của ( , )th i theo t . Hàm ( , )th i chỉ phụ thuộc vào 2 trạng thái của vector trạng thái, là vị trí robot tq và vị trí của điểm mốc thứ i là im . Tiếp đến vị trí mong muốn được tính tốn từ tư thế và các biến đo lường tại điểm mốc: , , , , , , cos( ) sin( ) i i x t x t ti t i i y t y t t r                               (12) Với ,i x và ,i y lần lượt là giá trị kỳ vọng mong muốn của điểm mốc im theo phương 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.” 34 Ox và Oy . Trong thuật tốn EKF-SLAM, giá trị khoảng cách Mahalanobis sẽ được sử dụng để liên kết giá trị đo lường với điểm mốc đã biết hoặc tạo điểm mốc mới tuỳ vào việc so sánh các khoảng cách này với giá trị ngưỡng chọn trước  . Khoảng cách Mahalanobis được tính: 1ˆ ˆ( ) ( )i k T i kk t t k t tz z z z     (13) Với i tz là phép đo lường thực hiện tới điểm mốc thứ i và ˆ k tz là giá trị đo lường mong muốn của các điểm mốc đã xác định, 1,.., 1k N  với 1N  là giá trị đánh dấu để tạo điểm mốc mới nếu phép đo liên kết trả về nĩ là giá trị khơng nằm trong giá trị ngưỡng. k là ma trận hiệp phương sai được cập nhật theo phép đo: T k k k t t t tH H Q      (14) 3.2. Xây dựng hệ thống nhận thức cho robot Omni dựa trên nền tảng ROS Hệ điều hành ROS là một nền tảng linh hoạt cho việc lập trình các phần mềm cho hệ thống robot như hình 2. Nĩ bao gồm các cơng cụ và thư viện nhằm đơn giản hố việc xây dựng các hệ thống robot phức tạp bằng việc kết hợp các nền tảng robot với nhau. Hơn thế nữa, ROS được xây dựng để tạo điều kiện cho việc phát triển và kết hợp các phần mềm robot với nhau một cách thuận lợi. Hình 2. Hệ thống nhận thức trên nền tảng ROS và cấu hình phần cứng của hệ thống robot. Thuật tốn EKF-SLAM sẽ được thực thi bởi 2 node là bản đồ (Map) và vị trí robot (Robot Pose), xuất ra dữ liệu về bản đồ và vị trí robot. Hình 3. Giao diện thực thi thuật tốn EKF-SLAM trên ROS và vùng bao quanh robot chứa thơng tin vật cản (costmap). 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 Hội thảo Quốc gia FEE, 10 - 2020 35 Hệ thống nhận thức cịn cĩ chức năng phát hiện vật cản dựa vào tín hiệu cảm biến khoảng cách và các điểm ảnh 3D (point cloud 3D). Một vùng bao quanh robot được gọi là vùng costmap cục bộ. Vùng costmap cục bộ được chọn là một vùng hình vuơng với robot ở vị trí trung tâm. Kích thước của hình vuơng được chọn sao cho đảm bảo được rằng robot cĩ thể phát hiện được vật cản từ khoảng cách an tồn. Bên cạnh đĩ, giá trị các điểm ảnh 3D cũng được giới hạn vùng tác động bên trong vùng costmap như hình 3. 4. KẾT QUẢ MƠ PHỎNG VÀ THỰC NGHIỆM 4.1. Kết quả mơ phỏng Mơ phỏng được thực hiện trên phần mềm mơ phỏng Gazebo của ROS. Các vùng màu trắng là các vùng mơi trường đã được nhận thức. Các đường màu đen là các vật cản mà robot nhận dạng được và bản đồ hố sử dụng kỹ thuật EKF-SLAM. Bộ EKF-SLAM liên tục tính tốn và bản đồ hố mơi trường dựa vào các dữ liệu này đồng thời định vị robot trong bản đồ đĩ. Sau khi di chuyển đến các vị trí đủ để thu thập dữ liệu cho việc bản đồ hố tồn bộ mơi trường như hình 4, dữ liệu bản đồ này cĩ thể được lưu lại để tái sử dụng trong những lần kế tiếp mà khơng cần phải tiến hành bản đồ hố lại mơi trường. Hình 4. Quá trình SLAM của robot và kết quả bản đồ hố mơi trường. 4.2. Kết quả thực nghiệm Hình 5. Mơ hình robot Omni (trái) và mơi trường thực nghiệm (phải). Thử nghiệm kỹ thuật EKF-SLAM trong mơi trường thực tế, kết quả thu được như trong hình 5. Ban đầu, robot chưa cĩ bất cứ thơng tin nào về mơi trường này. Trong hình 5, ở 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.” 36 gĩc dưới bên trái là ảnh màu thu được từ camera để theo dõi quá trình di chuyển của robot. Vùng màu trắng là phần mơi trường đã được bản đồ hố, trong khi đĩ, các đường màu đen được bao quanh bởi các đường màu xanh dương là vật cản mơi trường đã được bản đồ hố. Vật cản cũng được hiển thị dưới dạng 3D point cloud bởi phần Rviz. 5. KẾT LUẬN Bài báo đã thiết kế và 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 trên thuật tốn EKF-SLAM và hệ điều hành ROS. Hệ thống được thử nghiệm trên cả mơ hình mơ phỏng và mơ hình thật của robot. Hệ thống nhận thức của robot được xây dựng dựa trên dữ liệu thu được từ Astra Camera và Rplidar A2 trong quá trình chuyển động của robot. Sau đĩ, mơi trường sẽ được bản đồ hố và robot cĩ thể đồng thời tự định vị được trong mơi trường đĩ. Hệ thống nhận thức cịn cung cấp thơng tin về vật cản xuất hiện trong quá trình robot di chuyển. Các kết quả đạt được cho thấy tính hiệu quả thực tế của 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. 4 (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). 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 Hội thảo Quốc gia FEE, 10 - 2020 37 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 bài ngày 29 tháng 7 năm 2020 Hồ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; 2Viện Vật lý, Viện Hàn Lâm KH&CN Việt Nam; 3Viện Tự động hĩa KTQS, Viện KHCNQS; 4Đại học Bách khoa Hà Nội. * E-mail: dqhiep@uneti.edu.vn.