Proceedings VCM 2012 93 định vị cho robot di động sử dụng camera và vật mốc

Tuyển tập công trình Hội nghị Cơ điện tử toàn quốc lần thứ 6 679
Mã bài: 149
Định vị cho Robot di động sử dụng camera và vật mốc
Localization for mobile robot using camera and landmarks
Lê Hoàng Anh
Trường Đại học Lạc Hồng,
Ngô Văn Thuyên
Trường Đại học Sư Phạm Kỹ Thuật TP.HCM,

Tóm tắt
Để có thể di chuyển trong môi trường làm việc theo quỹ đạo mong muốn, robot di động phải có khả năng
xác định vị trí của nó. Bài báo này trình bày thuật toán định vị cho robot di động dựa trên nhận dạng vật mốc
nhân tạo sử dụng camera. Thuật toán tìm đường đi tối ưu A* được áp dụng để robot có thể di chuyển từ điểm
xuất phát về điểm đích mà không va chạm với vật cản.Vị trí của robot được xác định từ tọa độ của các vật mốc
trong môi trường làm việc sử dụng phương pháp hình học. Bộ lọc Kalman được sử dụng để kết hợp thông tin
từ mô hình động học của robot và thông tin đo đạc từ bên ngoài để ước lượng vị trí của robot chính xác hơn.
Thuật toán được thực hiện trên mô hình robot thực di chuyển theo kiểu lái vi sai, sử dụng camera Kinect và các
hàm thị giác máy tính trong thư viện OpenCV. Kết quả thực nghiệm cho thấy robot có thể bám theo quỹ đạo
mong muốn nhờ xác định được vị trí của nó. Thuật toán định vị dùng bộ lọc Kalman ước lượng vị trí của robot
tốt hơn.
Abstract
To follow a desired trajectory in a working environment, a mobile robot must be able to localize itself. This
paper presented a localization algorithm for mobile robots based on recognition of artificial landmarks with a
camera. The path finding algorithm A* was applied to find an optimal path for the robot to move from its
initial position to its goal without colliding with obstacles. A geometric method was used to determine the
position of the robot from the coordinate of artificial landmarks in the environment. The Kalman filter
combines the estimation of robot internal states based on its kinematic model and external sensory information
to better estimate the robot positions. The algorithm has been implemented on a real mobile robot platform
with two differential drive wheels, a Kinect camera and computer vision functions in OpenCV library. The
experimental results showed that the robot can use its estimated position to follow its desired trajectory
closely. The Kalman filter provided a better estimation of robot positions.

Key words: Mobile robot localization, Kalman localization, landmark.

1. Đặt vấn đề
Định vị đóng vai trò then chốt trong sự thành
công của một hệ robot độc lập. Nếu robot không
biết được vị trí hiện tại của nó trong môi trường
thì robot sẽ rất khó khăn để quyết định phải làm
việc gì tiếp theo. Các robot hầu như phải có một
vài nhận biết về vị trí của nó trong môi trường
để có thể hành động một cách chính xác [1].
Những vấn đề về định vị mà robot di động cần
thực hiện đó là theo dấu vị trí (position tracking)
tức là phải đảm bảo robot bám theo được vị trí
của nó khi đang di chuyển, tự xác định vị trí ban
đầu (wake-up) tức là robot phải xác định vị trí
ban đầu của nó trong môi trường làm việc và
cuối cùng là nhảy cóc vị trí (kidnapped) tức là
robot đang định vị thì vị trí của nó bất ngờ bị
thay đổi [2].
Sự khác nhau chủ yếu giữa các phương pháp
định vị đã thực hiện đó chính là độ tin cậy của
robot về vị trí hiện tại mà vấn đề này phụ thuộc
vào loại thông tin thu được từ cảm biến. So với
phương pháp định vị sử dụng các loại cảm biến
khác như siêu âm, laser thì camera có khả năng
cung cấp nhiều thông tin hơn, ngoài dữ liệu ảnh
RGB thu được từ môi trường thì camera còn có
khả năng cung cấp thông tin về giá trị độ sâu của
từng điểm trong khung ảnh, hình dáng của vật
thể. Thông tin về độ sâu của từng điểm ảnh là

một trong những thông tin dữ liệu đầu vào quan
trọng của thuật toán định vị. Bên cạnh đó
phương pháp sử dụng camera còn có thể tận
dụng được khả năng xử lý tính toán mạnh của
máy tính. Trong các công trình đã thực hiện [3],
[4], [5] và [6] thì camera là loại cảm biến được
680 Lê Hoàng Anh, Ngô Văn Thuyên
VCM2012
lựa chọn để xây dựng thuật toán định vị. Tuy
nhiên, các loại camera này chỉ cung cấp ảnh
RGB, không có thông tin về dữ liệu độ sâu của
từng điểm ảnh. Giới hạn này làm cho các thuật
toán định vị không đạt hiệu quả cao.
Bài báo này trình bày thuật toán định vị bằng
phương pháp hình học dựa trên việc nhận dạng
các vật mốc nhân tạo có màu sắc khác nhau
được bố trí sẵn trong môi trường làm việc sử
dụng kỹ thuật thị giác máy tính. Phương pháp
xác định tọa độ và hướng của robot được mô tả
chi tiết trong phần II. Phần III thảo luận về việc
sử dụng bộ lọc Kalman trong thuật toán định vị
và xây dựng bộ lọc Kalman cho mô hình robot
di động thực. Phần IV và V của bài báo trình
bày kết quả thực nghiệm và kết luận.
2. Thuật toán định vị bằng phương pháp hình
học
Khi đề cập đến vị trí có nghĩa là đề cập đến tọa
độ
,
x y

và hướng (góc

) của robot trong hệ tọa
độ của môi trường. Đối với trường hợp sử dụng
phương pháp hình học trong thuật toán định vị
thì chỉ cần nhận dạng được hai vật mốc trong
khung ảnh [3]. Sau đó thông tin về tọa độ tâm
của hai vật mốc này được sử dụng để xác định
khoảng cách từ vật mốc đến camera, đó cũng
chính là khoảng cách từ vật mốc đến robot. Giá
trị khoảng cách này được xác định dựa vào dữ
liệu độ sâu các điểm ảnh thu được từ camera
Kinect một thiết bị của hãng Microsoft dùng cho
các máy trò chơi.
Giả sử hai vật mốc nhận dạng có vị trí trong hệ
tọa độ môi trường lần lượt là
( , )
A A
A x y
và
( , )
B B
B x y
.Tọa độ của robot được xác định chính
là giao điểm của hai đường tròn
( )
A
C
và
( )

B
C
như
trong Hình 1.
Các vật mốc được bố trí tại những đường thẳng
giới hạn không gian di chuyển của robot
0
x

,
0
y

,
x n

,
y m

nên tọa độ của robot được
xác định chính là giao điểm thứ nhất của hai
đường tròn
1 1
( , )
P x y
.
Hướng của robot được xác định bởi góc

như
trong Hình 2, đó là góc lệch giữa trục

( )
r
y
của hệ
tọa độ đặt trên robot và trục
x
của hệ tọa độ môi
trường.
Góc

được xác định bằng cách tính toán góc
lệch của vật mốc so với hệ tọa độ môi trường và
góc lệch của vật mốc so với hệ tọa độ đặt trên
robot.
(0,0)
x
y
),(
AA
yxA
),(
BB
yxB
),(
11
yxP
),(
22
yx
A

r
B
r
m
y

)(
A
C
)(
B
C

Hình 1. Xác định tọa độ robot bằng phương
pháp hình học
)
(
r
x
)
(
r
y
),(
11
yxP




Hình 2. Các hệ tọa độ tham chiếu
Góc lệch của vật mốc so với hệ tọa độ môi
trường

được xác định dựa vào vị trí hiện tại
của robot
1 1
( , )
P x y
và tọa độ của một trong hai
vật mốc theo hệ tọa độ môi trường đã nhận dạng
như Hình 3.
Giả sử vật mốc
B
được chọn để thực hiện việc
tính toán. Tùy thuộc vào vị trí của robot so vật
mốc mà góc

có thể được xác định như trình
bày trong Bảng 1.
Bảng 1. Góc lệch của vật mốc so với hệ tọa độ
môi trường
Vị trí của robot so
với vật mốc
Góc lệch
1
B
y y

và

1
B
x x


hoặc
1
B
y y

và
1
B
x x


1
1
tan( )
B
B
y y
a
x x





1

B
y y

và
1
B
x x


hoặc
1
B
y y

và
1
B
x x


0
1
1
180 tan( )
B
B
y y
a
x x



 


1
B
y y

và
1
B
x x


0
90


1
B
y y

và
1
B
x x


0
180



1
B
y y

và
1
B
x x


0
0




Tuyển tập công trình Hội nghị Cơ điện tử toàn quốc lần thứ 6 681
Mã bài: 149
),(
AA
yxA
),(
BB
yxB
),(
11
yxP
),(

22
yx

C
)(
A
C
)(
B
C
m
y


Hình 3. Góc lệch của vật mốc so với hệ tọa độ môi trường
Góc lệch của vật mốc so với hệ tọa độ đặt trên
robot
( )
r

được xác định dựa vào tọa độ tâm của
vật mốc trên ảnh thu được từ camera như trong
Hình 4. Tùy thuộc vào tọa độ tâm của vật mốc
theo trục
x
mà góc lệch này được xác định như
sau:
 Nếu tọa độ tâm vật mốc > 320 thì:

( ) 0 ( )

90
r r
 
  (1)
 Nếu tọa độ tâm vật mốc ≤ 320 thì:

( ) 0 ( )
90
r r
 
  (2)
với
0
( )
( 320 )57
640
vm
r
x


 là góc lệch giữa tâm
của vật mốc đang xét so với tâm khung ảnh theo
trục
x
,
vm
x
là tọa độ tâm vật mốc đang xét theo
trục

x
trên khung ảnh (pixel). Hằng số 320 là
tọa độ tâm khung ảnh theo trục
x
và 640 là giá
trị giới hạn độ lớn của ảnh thu được từ camera
(pixel),
0
57

 là giới hạn góc nhìn của camera
Kinect theo chiều ngang.
640x480 pixel
x
y
(0,0)
0
57

),(
vmAvmA
yxA
),(
vmBvmB
yxB
Camera
)
(
r
y

)
(
r
x
)(r

Ảnh thu được qua camera
Tọa độ tâm khung ảnh
(320,240)
0
57


a)
b)
)(r

)(r


Hình 4. Góc lệch của vật mốc so với hệ tọa độ đặt trên robot

Hướng của robot trong môi trường làm việc
(Hình 5) được xác định:

0
90
 
 
(3)

Với

là góc lệch giữa trục
( )
r
x
của hệ tọa độ
đặt trên robot và trục
x
của hệ tọa độ môi
trường được xác định bởi công thức:

( )
r
  
  (4)

)
(
r
x
)
(
r
y

),(
11
yxP


)(r

),(
BB
yxB
),(
AA
yxA


Hình 5. Các góc biểu diễn hướng của robot
trong hệ tọa độ môi trường
3. Thuật toán định vị sử dụng bộ lọc Kalman
3.1 Định vị sử dụng bộ lọc Kalman
Thuật toán định vị sử dụng phương pháp hình
học dựa vào việc nhận dạng hai vật mốc cho kết
quả sai lệch khi giá trị khoảng cách từ robot đến
hai vật mốc thu được bởi camera không chính
xác do ảnh hưởng bởi các yếu tố như cường độ
chiếu sáng, kích thước vật mốc và góc nhìn…Do
đó bộ lọc Kalman được sử dụng để kết hợp
thông tin ước lượng nội và thông tin đo đạc trực
tiếp từ cảm biến để tăng độ chính xác cho thuật
toán định vị.
Hình 6 mô tả quy trình tổng quát của bộ lọc
Kalman [7].


Hình 6. Quy trình tổng quát của bộ lọc Kalman

Cập nhật thời gian
(Dự đoán)
1. Ước lượng giá trị kế
tiếp
 
1
1
k
A Bu
x x



 
k
k

2. Ước lượng ma trận
covariance kế tiếp
1
T
k k
P AP A Q


 

Cập nhật giá trị đo
(Hiệu chỉnh)
1.Tính độ lợi Kalman

1
( )
k
T T
k k
P H HP H R
K
  

2.Cập nhật lại giá trị đo
k
z

( )
kk
k
k
x
x
Hx K z
 
 
 


3.Cập nhật lại ma trận
covariance

( )
k k

k
P I K H
P

 
Khởitạo

1
k
x

và
1
k
P


682 Lê Hoàng Anh, Ngô Văn Thuyên
VCM2012
3.2 Xây dựng bộ lọc Kalman cho mô hình
robot di động thực
Tọa độ vị trí của robot trong môi trường
x
,
y

và hướng góc

được sử dụng như là các biến
trạng thái của bộ lọc Kalman [2]. Giá trị quãng

đường di chuyển của hai bánh xe và tọa độ của
robot xác định bởi thuật toán định vị được xem
như là ngõ vào của hệ thống. Phương trình động
học mô tả vị trí và hướng của robot trong môi
trường được xác định dựa vào mô hình di
chuyển theo kiểu lái vi sai:
1
cos( )
2
x k k
f x x s




     (5)
1
sin( )
2
y k k
f y y s




     (6)
1k
f

  


   
(7)

Trong đó
2
r l
s s
s
  
  ,
r
s

,
l
s

là quãng
đường đi được của bánh xe bên phải và bánh xe
bên trái,
r l
s s
b

  
  ,
b
là khoảng cách giữa
hai bánh xe. Do đó vectơ trạng thái của hệ thống

có thể được viết lại như sau:
T
k k k k
X x y


 
 
với vectơ ngõ vào
T
k r l
u s s
  
 
 
và hàm hệ thống
( )
T
x y
f x f f f

 

 
trong đó các hàm thành
viên của
( )
f x
là các phương trình (6), (7) và
(8).

Ma trận Jacobian
A
hệ thống và
B
ngõ vào của
mô hình robot di động này được xác định như
sau:

1 0 sin( )
2
0 1 cos( )
2
0 0 1
k
k
x x x
k k k
y y y
k
k k k
x
k k k
x
f f f
s
x y
f f f
A s
x y
f f f

x y
  







  
 

 
 
 
  
 
 
 
 
  

 
   
 
 
  
 
 
 

  
 
 
 
 
  
 
 

(8)

k
x x
l r
y y
k
l r
l r
x
f f
s s
f f
B
s s
f f
s s
 
 
 
 

 
 
 
 

 
 
 
 
 
 
 
 
 
=

1 1
cos( ) sin( ) cos( ) sin( )
2 2 2 2 2 2 2 2
1 1
sin( ) cos( ) sin( ) cos( )
2 2 2 2 2 2 2 2
1 1
k
x
s s
b b
s s
b b
b b

   
   
   
   
     
 
     
 
 
     
 
     
 
 

 
 
 

(9)
Vectơ đo đạc của hệ thống
 
T
k
z x y

 gồm
ba thành phần và trực tiếp có được từ thuật toán
định vị dựa vào các vật mốc nên ma trận
Jacobian của vectơ đo

H
:

1 0 0
0 1 0
0 0 1
H
 
 

 
 
 
(10)
Độ lệch tiêu chuẩn nhiễu vị trí của hệ thống đối
với tọa độ
x
,
y
là
0.01
x y
m
 
  (phương sai
2 2 4 2
10
x y
m
 


  ) và độ lệch tiêu chuẩn nhiễu
về hướng
0
0,5


 (phương sai
2 5
7,62.10




2
rad
) do đó ma trận hiệp phương sai số nhiễu
hệ thống và phép đo được chọn:

4
4
5
10 0 0
0 10 0
0 0 7,62.10
Q



 

 

 
 
 
 
(11)

4
4
5
10 0 0
0 10 0
0 0 7,62.10
R



 
 

 
 
 
 
(12)

4. Kết quả thực nghiệm
Thuật toán định vị dựa trên việc nhận
dạng các vật mốc trong môi trường phụ thuộc

vào nhiều yếu tố như cường độ chiếu sáng, kích
thước vật mốc, màu sắc, vị trí đặt vật mốc và
góc nhìn… Để có thể đạt được hiệu quả tốt nhất
thì thuật toán phải được thực hiện trong môi
trường có ánh sáng đều, kích thước vật mốc phải
đủ lớn để robot có thể phát hiện ra vật mốc ở
khoảng cách cho phép của camera, màu sắc của
vật mốc phải tương phản với màu nền và không
trùng với các màu có trong môi trường. Kết quả
thực nghiệm thuật toán định vị được thực hiện
bằng cách điều khiển robot di chuyển với quỹ
đạo được trình bày trong Hình 7. Kết quả này
được so sánh với đường di chuyển thực của
robot trong môi trường và thuật toán định vị có
sử dụng bộ lọc Kalman như được trình bày trong
Hình 8, Hình 9, Hình 10 và Hình 11.

Tuyển tập công trình Hội nghị Cơ điện tử toàn quốc lần thứ 6 683
Mã bài: 149

Hình 7. Không gian môi trường thực nghiệm
thuật toán định vị
0 60 120 180 240 300 360
0
60
120
180
240
300
360

420
X [cm]
Y [cm]


Goals
Desired trajectory
Localization
Hình 8. Thuật toán đinh vị với quỹ đạo gồm 10
điểm đích
0 60 120 180 240 300 360
0
60
120
180
240
300
360
420
X [cm]
Y [cm]


Goals
Desired trajectory
Ground truth
Localization


Hình 9. So sánh thuật toán định vị với đường di

chuyển thực của robot
0 60 120 180 240 300 360
0
60
120
180
240
300
360
420
X [cm]
Y [cm]


Goals
Desired trajectory
Ground truth
Kalman Localization

Hình 10. So sánh thuật toán định vị sử dụng bộ
lọc Kalman với đường di chuyển thực
0 60 120 180 240 300 360
0
60
120
180
240
300
360
420

X [cm]
Y [cm]


Goals
Desired trajectory
Ground truth
Localization
Kalman Localization
Hình 11. Kết quả so sánh tổng hợp thuật toán
định vị
Kết quả thực nghiệm cho thấy robot có khả năng
định vị dựa trên nhận dạng các vật mốc bằng
camera. Nhờ xác định được vị trí mà robot có
thể bám theo quỹ đạo mong muốn tạo ra từ thuật
toán A*. Khi thuật toán lọc Kalman được áp

Các thông số kỹ thuật của mô hình robot di
động
1 Cơ cấu di chuyển Lái vi sai
2 Khối lượng 5kg
3 Khoảng cách hai bánh xe 45 cm
4 Đường kính bánh xe 8 cm
5 Tốc độ di chuyển tối đa 0.2 m/s
6 Công suất động cơ 20w
7 Khả năng tải nặng 10kg
8 Chiều cao đặt camera so với
mặt sàn
43 cm
684 Lê Hoàng Anh, Ngô Văn Thuyên

VCM2012
dụng, robot có thể xác định vị trí của nó chính
xác hơn.
5. Kết luận
Camera Kinect có giá thành hợp lý nhưng
lại có khả năng cung cấp thông tin về độ sâu của
từng điểm trong khung ảnh tương đối chính xác
vì vậy ngày càng được dùng nhiều trong các ứng
dụng định vị cho robot di động. Thuật toán hình
học có thể được sử dụng để xác định vị trí của
robot nhờ việc nhận dạng và xác định vị trí các
vật mốc nhân tạo trong môi trường làm việc.
Việc sử dụng thuật toán lọc Kalman để kết hợp
ước lượng dựa trên mô hình và giá trị đo đạc bên
ngoài cho kết quả định vị chính xác hơn.






Hình 12. Mô hình robot thực tế

Bảng 2. Thông số kỹ thuật mô hình robot di
động


Tài liệu tham khảo
1. Roland, S. and N. Illah R, Introduction to
Autonomous Mobile Robots, 1st ed, 2004,

London: The MIT Press.
2. Negenborn, R., Robot Localization and Kalman
Filters, in Institute of Information and Computing
Sciences2003, UTRECHT UNIVERSITY.
3. Bais, A. and R. Sablatnig, Landmark Based Global
Self-localization of Mobile Soccer Robots, ACCV
2006: p. 842-851.
4. Tsukiyama, T., Mobile Robot Localization from
Landmaek Bearings, XIX IMEKO World
Congress, 2009: p. 2109-2112.
5. Stroupe, A.W., K. Sikorski, and a.T. Balch,
Constraint-Based Landmark Localization,
Submitted to Proc IROS 2002.
6. Jang, G., et al., Robust Self-localization of Mobile
Robots using Artificial and Natural Landmarks,
IEEE 2003, CIRA 2003: p. 412-417.
7. Welch, G. and G. Bishop, An Introduction to the
Kalman Filter, 2006, ACADEMIC PRESS, INC:
New York. p. 42.

Lê Hoàng Anh nhận bằng tốt
nghiệp kỹ sư ngành Điện Tử
Viễn Thông tại trường Đại học
Lạc Hồng năm 2006 và nhận
bằng thạc sĩ ngành Kỹ Thuật
Điện Tử tại trường Đại học Sư
Phạm Kỹ Thuật TP.HCM năm
2012. Hướng nghiên cứu chính
là robot di động. Hiện nay anh
đang công tác tại khoa Điện-

Điện Tử trường Đại học Lạc Hồng.

Ngô Văn Thuyên tốt nghiệp
kỹ sư ngành Điện năm 1999 tại
Đại học Sư Phạm Kỹ Thuật
TP.HCM, nhận bằng thạc sĩ
ngành Kỹ Thuật Điện tại Đại
học Bách Khoa TP.HCM năm
2003 và nhận bằng tiến sĩ tại
Đại học Công nghệ Sydney
(Australia) năm 2008. Hướng
nghiên cứu chính là quy hoạch
quỹ đạo, định vị cho robot di động và điều khiển
thông minh. Hiện nay tiến sĩ Ngô Văn Thuyên đang
công tác tại khoa Điện-Điện Tử trường Đại học Sư
Phạm Kỹ Thuật TP.HCM.