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.
Trang 1Kỹ 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 là bài toán quan trọng trong điều khiển
chuyển động cho robot tự hành, trong đó, hệ thống nhận thức môi trường đóng vai trò cốt 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 đồ hoá đồ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 toá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 toá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 hoà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 đồ hoá 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 đồ hoá 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 toán ra các giá trị cho việc bản đồ hoá 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
Trang 2tả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 toán EKF-SLAM để
tính toá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 q=x y T là véc tơ
tọa độ của robot trong hệ tọa độ toà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
v 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 độ toà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
(1)
Theo hình 1, các bánh xe được đặt hợp với trục v x 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 toán được vị trí robot thông qua phương trình động học
2
4
x y
v r
(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
Trang 3Kỹ thuật Điều khiển – Tự động hóa
3 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 của robot được thiết kế dựa trên các kỹ thuật để xử lý vấn đề bản
đồ hoá 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 Ngoà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 y bao gồm trạng thái của robot và toạ độ các điểm mốc t
trong bản đồ T
q x y là vector trạng thái của robot và
1,x 1,y 2,x 2,y N x, N y, T
m 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ó y t = (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 , m , i x, m i y, 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à 2N3, với N biểu diễn số điểm mốc trên
bản đồ Từ đó, thuật toán EKF-SLAM sử dụng để tính toán phân phối xác suất:
1: 1:
( t| t, t)
Thuật toá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ự đoán (Prediction) và bước chỉnh sửa (Correction)
3.1.1 Bước dự đoán (Prediction)
Từ biểu thức phương trình động học (1), y được viết lại thành: t
1
( ) cos ( )sin ( )sin ( ) cos
( )
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
x
F
là ma trận có kích thước
3 (2 N3) 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ự đoán được tính bởi:
cos sin sin cos
T
(5)
Mô hình chuyển động đầy đủ khi có nhiễu được mô tả bởi:
1
( ) cos ( )sin ( )sin ( ) cos (0, )
( )
t
Trang 4Với thành phần nhiễu (0, T )
x t x
F R F
N tuân theo phân phối chuẩn đa biến có kỳ vọng bằng 0 và R là ma trận hiệp phương sai t F R F là dạng mở rộng ma trận hiệp phương x T t x
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 )
( ) cos ( )sin ( , ) ( )sin ( ) cos
( )
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 hoá xấp xỉ hàm
1
( ,t t )
g u y :
( ,t t ) ( ,t t ) t( t t )
Với, t1 là giá trị kỳ vọng của trạng thái y t1 ngay trước đó Ma trận Jacobi
1
( , )
G g u đạo hàm theo y t1
Từ đó, ta tính được biểu thức cập nhật cho ma trận hiệp phương sai
1
t G t t G t F R F x t x
3.1.2 Bước chỉnh sửa (Correction)
Mô hình đo lường được biểu diễn bởi vector i i iT
z r với r là khoảng cách từ t i
cảm biến và i
t
là góc quay của robot so với điểm mốc thứ i
0
0 tan 2( , )
i
r
r z
ở đó, (m i x, ,m i y, )T 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:
( , )
tan 2( , )
t
h y i
và
0
0
r
t
Q
Hàm ( , )h y i t được xấp xỉ bằng chuỗi Taylor bậc nhất mở rộng:
( , )t ( t, ) t i( t t)
Ở đó, i
t
H là đạo hàm của ( h t, )i theo t Hàm (ht, )i chỉ phụ thuộc vào 2 trạng thái
của vector trạng thái, là vị trí robot q và vị trí của điểm mốc thứ i là t m Tiếp đến vị trí i
mong muốn được tính toán từ tư thế và các biến đo lường tại điểm mốc:
cos( ) sin( )
i
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 m theo phương i
Trang 5Kỹ thuật Điều khiển – Tự động hóa
Ox và Oy
Trong thuật toá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 k)
k z t z t k z t z t
Với i
t
z là phép đo lường thực hiện tới điểm mốc thứ i và ˆ z là giá trị đo lường mong t k
muốn của các điểm mốc đã xác định, k1, ,N1 với N1 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
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 hoá 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 toá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 toán EKF-SLAM trên ROS
và vùng bao quanh robot chứa thông tin vật cản (costmap)
Trang 6Hệ 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 toà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 đồ hoá sử dụng kỹ thuật EKF-SLAM
Bộ EKF-SLAM liên tục tính toán và bản đồ hoá 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 đồ hoá toà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 đồ hoá lại
môi trường
Hình 4 Quá trình SLAM của robot và kết quả bản đồ hoá 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, ở
Trang 7Kỹ thuật Điều khiển – Tự động hóa
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 đồ hoá, 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 đồ hoá 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 toá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 đồ hoá 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)
Trang 8ABSTRACT
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 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ỉ: 1 Khoa Điện, Trường Đại học KT-KT Công nghiệp;
2 Viện Vật lý, Viện Hàn Lâm KH&CN Việt Nam;
3 Việ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
