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 
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 
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 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.  =
T
q x y  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
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 độ 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 .
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 toá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 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 
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 toán EKF-SLAM sử dụng để tính toán phân phối xác suất: 
 1: 1:( | , )t t tp y z u (3) 
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), 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ự đoá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 hoá 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 toá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 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 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 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). 
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 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, ở 
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 đồ 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). 
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 
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; 
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.