Đị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, lehoanganh.lhu@gmail.com Ngô Văn Thu
Trang 1Đị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, lehoanganh.lhu@gmail.com
Ngô Văn Thuyên
Trường Đại học Sư Phạm Kỹ Thuật TP.HCM, nvthuyen@gmail.com
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
Trang 2lự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 x( A,y A)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(C A)và(C B)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 x 0,
0
y , xn, ymnê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 P x y( ,1 1)
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ụcy( )r 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
) , (x A y A A
) , (x B y B B
) , (x1 y1 P
) , (x2 y2
A
r
B
r
m
y
) (C A
) (C B
Hình 1 Xác định tọa độ robot bằng phương
pháp hình học
)
( r
x
)
(r
y
) , (x1 y1 P
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 P x y( ,1 1)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
1
B
y y và x B x1
hoặc
1
B
y y và x B x1
1 1
tan( B )
B
a
1
B
y y và x B x1
hoặc
1
B
y y và x B x1
1
180 tan( B )
B
a
1
B
90
1
B
180
1
B
y y và x B x1 00
Trang 3) ,
A
) , (x B y B
B
) , (x1 y1 P
)
,
(x2 y2
C
) (C A
) (C B
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ì:
( )r 900( )r (1)
Nếu tọa độ tâm vật mốc ≤ 320 thì:
90
với
0 ( ) ( 320 )57
640
vm
của vật mốc đang xét so với tâm khung ảnh theo
trục x, x vm 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), 570 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
) ,
(x vmA y vmA
A
) , (x vmB y vmB B
Cam
era
)
(r
y
)
(r
x
)
(r
Ảnh thu được qua camera Tọa độ tâm khung ảnh (320,240)
0
5
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 x( )r 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
) , (x1 y1 P
)
(r
) , ( xB yB B
) , ( xA yA A
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
2 Ước lượng ma trận covariance kế tiếp
1
T
PAP A Q
Cập nhật giá trị đo (Hiệu chỉnh) 1.Tính độ lợi Kalman
1
k
k
covariance
P k (IK H P k ) k
và P k1
Trang 43.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:
2
f x x s (5)
2
f y y s (6)
1
k
f (7)
Trong đó
2
s
, s r, s l là quãng
đường đi được của bánh xe bên phải và bánh xe
b
hai bánh xe Do đó vectơ trạng thái của hệ thống
T
T
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
k k
k
x
s
(8)
k
k
B
=
k
x
(9) Vectơ đo đạc của hệ thống z k x y Tgồ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:
H
Độ lệch tiêu chuẩn nhiễu vị trí của hệ thống đối với tọa độ x, y là x y0.01m (phương sai
10
0,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
0 0 7, 62.10
Q
(11)
4 4
5
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
Trang 5Hì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
420
X [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
420
X [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 420
X [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 420
X [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
8 Chiều cao đặt camera so với mặt sàn
43 cm
Trang 6dụ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
