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

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 đồ 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
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 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 đồ 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 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 q 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

30

Đ. Q. Hiệp, …, P. X. Minh, “Xây dựng hệ thống nhận thức … và hệ điều hành ROS.”


Nghiên cứu khoa học công nghệ

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 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. 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

v  vx

vy

 

T

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
q  H .v   sin 
 0

Theo

hình

1,

các

bánh

xe

 sin 
cos 
0

được

0
0  .v
1 

đặt

hợp

(1)
với

trục

vx

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.
 2
 vx 
  r
 vy   4  2

 
 d
 


 2

2

 2
d

 2
d


 1 
2   1 
 
 


2  2   H2  2 
 3 
 
d  3 
 
  4 
 4 

(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.

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

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
đồ 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 q 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 yt bao gồm trạng thái của robot và toạ độ các điểm mốc

trong

bản

m   m1, x

m1, y

đồ.
m2, x

q  x

y 

T

m2, y

... mN , x

là

vector

trạng

thái

của

robot

và

mN , y  là vector chứa toạ độ của N điểm mốc là
T

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 , mi , x , mi , y là tọa độ điểm mốc thứ i , với
i  1,2..., N . Số chiều của vector trạng thái là 2 N  3 , 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:

p( yt | z1:t , u1:t )

(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), yt được 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)

 1 0 0 0 ... 0 


với t là thời gian trích mẫu và Fx   0 1 0 0 ... 0  là ma trận có kích thước
 0 0 1 0 ... 0 


3  (2 N  3) . 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:
 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 đủ khi có nhiễu được 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 … và 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
bằng 0 và Rt là ma trận hiệp phương sai. FxT Rt Fx 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:
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 là thành phần phi tuyến trong



 (t )


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
g (ut , yt 1 ) :
T
x

g (ut , yt 1 )  g (ut , t 1 )  Gt ( xt 1  t 1 )

(8)

Với, t 1 là giá trị kỳ vọng của trạng thái yt 1 ngay trước đó. Ma trận Jacobi
Gt  g (ut , t 1 ) đạo hàm theo yt 1 .
Từ đó, ta tính được 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 được biểu diễn bởi vector zti   rti ti  với rti là khoảng cách từ
T

cảm biến và ti là góc quay của robot so với điểm mốc thứ i .
 r i   (mi , x  x)2  (mi , y  y ) 2 

 +N(0,  r
zti   t i   

0
 t   a tan 2(mi , y  y, mi , x  x)   

0
)
  

 r
ở đó, (mi , x , mi , y )T là toạ độ của điểm mốc thứ i đo được tại thời điểm t , N(0, 
0
 ( m  x) 2  ( m  y ) 2 
i,x
i, y
 và
là nhiễu đo lường bất định. Đặt: h( yt , i)  
 a tan 2(m  y, m  x)   
i
,
y
i
,
x


 r 0 
Qt  
 . Hàm h( yt , i) được xấp xỉ bằng chuỗi Taylor bậc nhất mở rộng:
 0  

h( yt , i)  h( t , i)  Hti ( yt  t )

(10)
0
)
  

(11)

Ở đó, H ti là đạo hàm của h( t , i) theo t . Hàm h( t , i) chỉ phụ thuộc vào 2 trạng thái
của vector trạng thái, là vị trí robot qt và vị trí của điểm mốc thứ i là mi . 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:
 i , x   t , x  i  cos(ti  t , ) 



  rt 
i
 i , y   t , y 
 sin(t  t , ) 

(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 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 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:

 k  ( zti  zˆtk )T  k1 ( zti  zˆtk )

(13)

Với zti là phép đo lường thực hiện tới điểm mốc thứ i và zˆtk là giá trị đo lường mong
muốn của các điểm mốc đã xác định, k  1,.., N  1 với N  1 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  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 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 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).

34

Đ. Q. Hiệp, …, P. X. Minh, “Xây dựng hệ thống nhận thức … và 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 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 vng với robot
ở vị trí trung tâm. Kích thước của hình vng đượ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 đồ hoá 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. Q 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, ở

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


Kỹ 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 đồ 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 đồ
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 đồ 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).

36

Đ. Q. Hiệp, …, P. X. Minh, “Xây dựng hệ thống nhận thức … và 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 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;
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:

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