Ý tưởng chính của giải thuật này là xác định những vị trí khả thi của robot trong không gian làm việc, sử dụng tín hiệu của cảm biến siêu âm như là một thiết bị xác định khoảng cách v
Trang 1ROBOT DI ĐỘNG TỰ ĐỊNH VỊ KHÔNG DÙNG CỘT MỐC
Đoàn Hiệp (+)
TS Nguyễn Văn Giáp (++)
(+) Sinh viên thuộc Chương trình Đào tạo Kỹ sư chất lượng cao tại Việt Nam (++) Bộ môn Cơ Điện Tử , Khoa Cơ Khí, Trường Đại học Bách Khoa TPHCM
TÓM TẮT
Công việc khó khăn nhất trong điều khiển một
robot di động tự trị, đó là việc robot tự định vị chính
xác vị trí và hướng của nó trong không gian làm
việc Bài báo này nhằm giới thiệu một phương pháp
định vị robot bằng giải thuật định vị không dùng cột
mốc hiện Ý tưởng chính của giải thuật này là xác
định những vị trí khả thi của robot trong không gian
làm việc, sử dụng tín hiệu của cảm biến siêu âm như
là một thiết bị xác định khoảng cách và hướng của
robot đối với các chướng ngại vật cố định, để từ đó
suy ra vị trí của robot bằng cách so sánh bản đồ
toàn cục và bản đồ cục bộ quanh robot.
Từ khóa: Định vị, Dò đường, Cột mốc hiện, Robot di động
tự trị
ABSTRACT
An important challenge of an autonomous mobile
robot is finding its current pose (position and
orientation) within working environment This paper
represents a method for self-localization without
explicit landmarks The main idea of this algorithm
comes from finding feasible poses using ultrasonic
sensors as a point-and-shoot ranging device These
poses are then computed to estimate robot’s pose by
correlating the global and local maps
Keyword: Localization, Navigation, Explicit landmark,
Autonomous mobile robot
1 GIỚI THIỆU
Bài báo này giới thiệu một phương pháp định vị
và thăm dò bản đồ dựa trên hệ thống thuật toán
chồng chất cảm biến, được đề xuất bởi Hans P
Moravec [Moravec89]
Bài báo gồm 4 phần chính: phần đầu tiên sẽ trình
bày các khái niệm cơ sở được sử dụng, phần thứ hai
trình bày nội dung các thuật toán, phần thứ ba sẽ
trình bày các kết quả mô phỏng đạt được, và phần
kết luận là những đánh giá về khả năng thực hiện
cũng như đề xuất thực hiện một robot thí nghiệm
phục vụ cho việc học tập và nghiên cứu
2 ĐẶT VẤN ĐỀ
Môi trường hoạt động của robot là một căn phòng
có mặt sàn phẳng, được giới hạn bởi các bức tường
đã biết Các chướng ngại vật được xem là chướng
ngại vật hai chiều [Latombe91] Mặt sàn được chia
thành các vùng rời rạc, gọi là các ô lưới Robot sử dụng các cảm biến siêu âm, được đặt trên một vòng
tròn bao quanh robot, sẽ cập nhật các xác suất bị chiếm (occupied) hoặc còn trống (empty) của các ô
lưới Từ những đánh giá này robot vẽ lại bản đồ chướng ngại vật Dựa vào bản đồ tìm được, robot sẽ xác định vị trí của nó bằng việc đi tìm những vị trí khả thi
3 MỘT SỐ KHÁI NIỆM CƠ SỞ 3.1 Robot di động tự trị và cột mốc
Robot di động tự trị là robot có tất cả những gì cần thiết tích hợp bên trong robot. [Marsland02]
Nghĩa là robot có các pin, cảm biến, máy tính và chương trình điều khiển để nó không được kết nối với bất kỳ sự hỗ trợ bên ngoài nào Chương trình điều khiển cũng phải có khả năng điều khiển robot mà không cần sự trợ giúp của con người Như vậy,
cột mốc (landmark) [Borenstein96] cũng là một hình thức hỗ trợ thông tin từ bên ngoài, được sử dụng khá phổ biến trong nhiều ứng dụng khác Trong đề tài này, cột mốc không được sử dụng
3.2 Vectơ định vị
Sóng siêu âm truyền đi trong không khí với vận tốc khoảng 343 m/s Cảm biến siêu âm phát ra và thu về các mặt sóng siêu âm, sau đó khuếch đại cường độ của sóng với hàm mũ theo thời gian Cảm biến xác định thời gian sóng di chuyển trong không gian, để suy ra khoảng cách từ nó đến chướng ngại vật gần nhất, nằm trong vùng quét của cảm biến
[Thuan03]. Vectơ định vị đối với một cảm biến đo khoảng cách khi nó ghi nhận một thông số s là một vectơ có độ lớn bằng s và có chiều là chiều của cảm biến đó (xem hình 1) [Brown00]
3.3 Vị trí khả thi
Ý tưởng cơ bản nhất của thuật toán định vị với nhiều cảm biến trong bài báo này được thể hiện dưới dạng đi tìm vị trí khả thi của robot, là những vị trí mà robot có thể đứng tại đó và ghi nhận được thông số đã nhận được từ cảm biến (xem hình 1)
Vị trí khả thi của robot đối với môi trường có chướng ngại vật M và vectơ định vị z là vị trí mà tại đó ta có thể đặt gốc của vectơ định vị, sao cho ngọn của vectơ nằm trên chướng ngại vật M, mà thân của
Trang 2vectơ không cắt qua bất kỳ chướng ngại vật
nào.[Brown00]
Hình 1: (a) là vị trí khả thi,
(b) và (c) không phải là vị trí khả thi
3.4 Vùng quét của cảm biến siêu âm
Trong không gian hai chiều, búp hướng (beam
patern) của cảm biến siêu âm có thể được đơn giản
hoá thành một hình quạt với góc mở từ 15 đến 45
độ, tuỳ loại Các búp bên (side lobe) sẽ được loại
bỏ nhờ các thiết bị hỗ trợ phần cứng, hoặc không
được tính toán trong phần mềm
Hình 2: Búp hướng của cảm biến Polaroid 6500
Phạm vi trong góc mở và khoảng cách xa nhất mà
cảm biến có thể xác định được chướng ngại vật,
chúng ta gọi đó là vùng quét của cảm biến (xem
hình 3)
4 CÁC THUẬT TOÁN
4.1 Thuật toán thăm dò bản đồ
Hình 3: Vùng quét của cảm biến siêu âm
Vùng quét của cảm biến siêu âm được chia thành
3 vùng chính Vùng I, là vùng mà cảm biến ghi
nhận có vật cản, vùng II là vùng từ vùng I đến cảm
biến, dường như không có vật cản nào, vùng III là
vùng phía sau vật cản, là vùng chưa biết, trong
thuật toán không quan tâm đến vùng này (xem hình
3)
Gọi s là khoảng cách đo được của cảm biến, H là sự kiện ô lưới đã bị chiếm và –H là sự kiện ô lưới còn trống Phân bố xác suất trên vùng quét của cảm biến siêu âm là một phân bố Gauss theo phương hướng kính, phân bố này sẽ được đơn giản hoá thành một cung tròn trong thuật toán Vì vậy, nếu cho trước một thông số cảm biến, chúng ta có thể tính được xác suất cảm biến ghi nhận được vật cản nằm tại một ô lưới, nếu đã thực sự có một vật cản nào đó nằm trong vùng quét của cảm biến siêu âm,
[Murphy00],[Moravec89] theo công thức sau:
r R H s
2
) ( ) ( )
| ( = − + ββ−α (1)
Vùng II:
2
) ( ) ( 1 )
|
α
β− +
−
−
r R H
s
Vùng III: P(s|H) không quan tâm
Nhưng ở đây yếu tố cần xác định là xác suất đã
bị chiếm của một ô lưới khi cảm biến ghi nhận được thông số s, có nghĩa là cần tính P(H|s) Aùp dụng công thức toàn phần Bayes [Diep99], như sau:
) ( )
| ( ) ( )
| (
) ( )
| ( )
| (
H P H s P H P H s P
H P H s P s
H P
−
−
−
trong đó, P(H) là xác suất của sự kiện H, đây là thời điểm lần đầu tiên ô lưới được quét, giá trị khởi tạo là xác suất ngẫu nhiên:
5 0 ) ( 1 ) ( ) (H =P −H = −P H =
Một vấn đề khác lại được đặt ra là nếu có nhiều cảm biến cùng quét qua một ô lưới, hay là sau một thời gian di chuyển, robot lại quét qua ô lưới đó
Như vậy, việc cập nhật cho các ô lưới phải dựa vào thông tin có trước của ô lưới đó, như sau:
)
| ( )
| ( )
| ( )
| (
)
| ( )
| ( )
| (
1 1
1
−
−
−
−
− +
=
n n
n n
n n
n
s H P H s P s H P H s P
s H P H s P s
H
Hình 4 dưới dây mô tả quá trình thăm dò bản đồ của robot Vùng đen đậm là vùng không gian trống, vùng màu xám là vùng chưa biết Các vùng màu còn lại là vùng có các giá trị xác suất thay đổi
Hình 4: Bản đồ đang được vẽ lại
22.5 độ
Búp bên
z
a b
c
M
Trang 3Vấn đề kỹ thuật khó khăn nhất trong phần này là
việc dùng cảm biến siêu âm để xác định khoảng
cách từ robot đến các chướng ngại vật Bài toán đặt
ra là làm cách nào để giải quyết hiện tượng đọc
chéo (crosstalk) của các cảm biến siêu âm Hiện
tượng đọc chéo là hiện tượng mà cảm biến này ghi
nhận sóng siêu âm được phát ra từ cảm biến khác
sau một hay nhiều lần phản xạ trên các chướng ngại
vật Hiện tượng này đã được J Borenstein giải
quyết bằng việc đặt lệch các thời gian phát của mỗi
cảm biến, và sử dụng một chu kỳ phát phù hợp với
số lượng cảm biến được sử dụng [Borenstein95] Do
giới hạn của bài báo, chúng tôi không trình bày nội
dung chi tiết của phương pháp này
4.2 Thuật toán định vị
Thay vì sử dụng khái niệm vị trí khả thi bằng hình
học như phần trên đã trình bày, để đưa vào chương
trình tính toán, hình ảnh vị trí khả thi có thể được rút
ra từ bản đồ được chia lưới theo công thức toán học
sau:
)) , 0 ( ( ) (
)
,
Trong đó, FP thay cho chữ feasible poses có
nghĩa là vị trí khả thi, M là tập hợp các điểm nằm
trên biên chướng ngại vật, và z là vectơ định vị mà
robot xác định được, ∂ ( X ) là ký hiệu cho tập hợp
các điểm biên của tập hợp X, Θlà ký hiệu của
phép trừ Minkowskii và l(x,y) là ký hiệu của một
đoạn thẳng hở nối hai điểm x và y, nhưng không kể
hai điểm x và y [Brown00]
Tổng Minkowskii là tổng của một tập hợp với
một vectơ, kết quả của phép cộng này là một tập
hợp được xác định bằng việc cộng tất cả các phần
tử của tập hợp với vectơ đó Hình ảnh sau đây mô
tả phép trừ Minkowskii (hình 7) và các vị trí khả thi
tìm được dùng công thức (6) (hình 8)
Hình 7: Hiệu Minkowskii
Ở đây có thể nhận thấy rằng, phần nằm trong của
tập hợp M Θ zcũng sẽ nằm trong tập hợp
)
,
0
( z
l
M Θ , do đó, chúng ta có thể đơn giản công
thức (6) thành dạng
)) , 0 ( ( ) (
)
,
Hình 8: Tìm vị trí khả thi ứng với chướng ngại vật M và vectơ định vị z
Cho rằng trong không gian chướng ngại vật M, robot ghi nhận được nhiều vectơ định vị và gọi tập hợp các vectơ định vị là Z Bằng cách giao tất cả những vị trí khả thi, vị trí hiện tại của robot sẽ có thể được tìm thấy bằng công thức sau:
) , ( )
,
FP
Z
z∈
Hình 9 mô tả vị trí khả thi ứng với các vectơ định
vị x, y, và z, đồng thời tìm ra vị trí thực của robot, nếu robot ghi nhận được 3 vectơ định vị x, y, z đó
Hình 9: Tìm vị trí khả thi bằng vectơ định vị
Tuy nhiên, một vấn đề được đặt ra là cảm biến siêu âm luôn trả về các giá trị với một sai số nhất định Chính vì vậy, thay vì tính toán vị trí khả thi với vectơ định vị lý tưởng như trên, robot sẽ phải tính toán vị trí khả thi với một sai số ε nào đó Sai số
ε này được xác định dựa trên phân bố của cảm biến siêu âm Bán kính ε là bán kính sao cho xác suất xuất hiện chướng ngại vật trong phạm vi bán kính đó là 90% Vì vậy, công thức (7) sẽ được viết lại như sau:
))) 1 ( , 0 ( ( ) B (z ( ) , (
z z l M M
z M
ε
o
o
Q
o
-Q
P
Q
PΘ
o
Trang 4Trong đó, Bε là quả cầu bán kính ε Hình 10 mô
tả vị trí khả thi của robot với 3 vectơ định vị có sai
số
Hình 10: Định vị có tính đến sai số
Như vậy, sai số ε này vào khoảng bao nhiêu, và
làm cách nào để hạn chế sai số? Xem cảm biến
siêu âm với búp hướng được thu gọn lại trong một
hình quạt bán kính bằng tầm quét xa nhất của cảm
biến (khoảng 10m đối với cảm biến Polaroid 6500
và 6m đối với SRF08); đồng thời, phân bố Gauss
của cảm biến cũng được xem như phân phố là một
đường tròn theo góc mở (hình 3) Như vậy, độ dài
cung tròn ở khoảng cách 10m, với góc mở 25 độ là
) ( 36 4 180
25 10
)
10
Bán kính ε sẽ có độ lớn lên đến vài mét Sai số
này là sai số không thể chấp nhận được Một giải
pháp đặt ra là dùng nhiều cảm biến cho việc ghi
nhận một vectơ định vị như hình 11 [Elfes85]
Hình 11: Dùng nhiều cảm biến
để ghi nhận một vectơ định vị
Góc θ1 giới hạn ở đây bằng bao nhiêu thì tốt
nhất, vì góc này càng lớn thì càng ổn định, nhưng
nó phụ thuộc vào khoảng cách từ cảm biến đến
chướng ngại vật chưa biết Ở đây chúng tôi chọn
o
30
θ Điều đó có nghĩa là, trong quá trình di
chuyển, robot sẽ lưu lại các “vệt quét” của cảm
biến siêu âm trước đó, nếu “vệt quét” cũ giao với
vệt quét mới tại một điểm hợp với vị trí mới và cũ
của cảm biến một góc lớn hơn 30o thì ta xác nhận
điểm đó là chướng ngại vật, và chọn vectơ định vị
đi qua điểm đó Trong trường hợp góc ghi nhận không tốt, ta sẽ phải dùng phương pháp ATM (Arc Transversal Median) [CNL03] để tìm ra điểm trọng tâm trong các điểm nghi ngờ và chọn vectơ định vị
đi qua điểm đó
Hình 12: Tìm góc giao hai cảm biến
Xét chướng ngại vật nằm ở khoảng cách 6 mét cách cảm biến như hình 12 trên, đồng thời, sai số vùng I là 10 cm:
cm cm
cm Len
cm cm
Arc
o
3 37 ) 30 tan(
10 ) 30 sin(
10 ) 30 (
314 180
30 600 ) 600 (
= +
=
=
(10)
Trong đó, Arc ký hiệu cho cung tròn là những vị trí mà cảm biến ghi nhận có thể có vật cản và Len
là chiều dài đoạn giao của hai cảm biến Kết quả cho thấy độ chính xác của cảm biến sẽ tăng lên
3 37
314≈ lần Với kết quả này, bán kính sai số ε sẽ vào khoảng 15 cm đến 20 cm Đây là một kết quả lý tưởng
Cuối cùng, cho dù có hạn chế được sai số của cảm biến, trong một môi trường chưa biết, không có một sự khẳng định nào rằng robot sẽ ghi nhận các vectơ định vị như khi thăm dò bản đồ (hình 13) Hay nói cách khác, tập hợp vị trí khả thi của robot như trong công thức (8) sẽ có thể là một tập rỗng, hoặc một tập hợp mà các phần tử không liên thông (có nhiều vị trí khả thi của robot nằm nhiều nơi cách xa nhau trên bản đồ)
Hình 13: Chướng ngại vật di động
Bằng cách đưa vào một hàm đặc trưng như sau, việc tìm ra vị trí khả thi có thể được thực hiện dễ dàng
1 θ
θ
) sin(
10
θ
) tan(
10 θ
θ
10
600
10
Trang 5
∉
∈
=
FP p
FP p p
z
M
FP
: 0
: 1 )
(
)
,
(
Lại tiếp tục đưa vào hàm rnk() là tổng giá trị các
hàm đặc trưng của p như sau:
∑
∈
=
Z z
z M
FP p p
Z
M
Vậy, vị trí của robot là vị trí phù hợp với nhiều
vectơ định vị được robot ghi nhận nhất:
=
∈ ( ( , , )) sup
) , , (
|
)
,
(M Z p rnk M Z p rnk M Z q
Loc
C q
(13)
Trong đó, C là tập hợp tất cả các ô lưới trong bản
đồ Mặc dù vậy, Loc(M,Z) có thể sẽ là một tập hợp
các điểm liên thông Để tìm ra một điểm duy nhất,
ta sẽ tính tổng của các hàm rnk() của các điểm lân
cận, điểm có giá trị cao nhất sẽ là vị trí tức thời của
robot Nếu lấy tổng các điểm lân cận gần nhất vẫn
không tìm ra được điểm duy nhất, chúng ta sẽ lấy
tổng các điểm lân cận tiếp theo
Ngoài ra, để hạn chế vùng tính toán vị trí của
robot và nâng cao độ chính xác, robot có thể được
hỗ trợ thuật toán dead reckoning dựa vào việc tính
toán vị trí theo sự điều khiển mong muốn và đưa
vào một sai số tích luỹ theo quãng đường di chuyển
của robot Sai số của phương pháp dead reckoning
sẽ được kiểm tra và sửa lỗi bằng phương pháp
UMBmark [Borenstein94] Chúng tôi đã viết một
chương trình tính toán UMBmark Calculator giúp
người thí nghiệm mô tả lại kết quả thí nghiệm với
UMBmark, và tính toán nhanh các sai số để hiệu
chỉnh
5 CHƯƠNG TRÌNH MÔ PHỎNG
Nhiệm vụ của chương trình mô phỏng là kiểm tra
các kết quả của thuật toán, đồng thời cũng là một
chương trình nhằm giới thiệu về hệ thống thuật toán
này cho sinh viên
Chương trình được viết theo lập trình hướng đối
tượng bằng ngôn ngữ Visual C++ NET
Ngoài ra, trong phần Help (giúp đỡ), chúng tôi
trình bày tất cả các lý thuyết về thuật toán, cũng
như các lý thuyết về bài toán động học cho robot di
động mô hình NEWT, các lý thuyết cảm biến siêu
âm, hồng ngoại, la bàn,… sẽ hỗ trợ cho việc giảng
dạy và nghiên cứu về sau
Dưới đây là giao diện chương trình mô phỏng
CabSim và chương trình tính toán UMBmark
Calculator (hình 14, 15, 16)
Hình 14: Chương trình UMBmark Calculator
Hình 15: Bản đồ chướng ngại vật
(a)
(b)
© Hình 16: (a) Robot bắt đầu quét bản đo (b) Robot đang thăm dò bản đồ (c)Robot hoàn thành việc quét bản đồ
Trang 66 KẾT LUẬN
Một cách sơ lược, bài báo đã phân tích các vấn
đề giải thuật, tạo cơ sở cho việc thiết kế và chế tạo
một robot thực có khả năng tự trị trong một môi
trường chưa biết Chúng tôi đã tiến hành việc mô
phỏng thuật toán thăm dò bản đồ, và đang tiến hành
phần thuật toán định vị
Hiện nay, việc sử dụng hệ thống giải thuật chồng
chất cảm biến này đã và đang được phát triển rất
mạnh tại các phòng thí nghiệm ở các nước trên thế
giới Sản phẩm được sản xuất hàng loạt đầu tiên sử
dụng hệ thống giải thuật này là robot hút bụi
Trilobite của hãng Electrolux 2003 Những sản
phẩm đơn chiếc nổi tiếng khác như robot thăm dò
sao Hoả, và robot dẫn đường cho người mù…
Những ứng dụng từ công nghiệp, đến những ứng
dụng đơn giản hay vô cùng phức tạp kể trên cho
thấy rằng đây là một hướng đi cần được quan tâm
Hơn thế, việc sử dụng cảm biến rẻ tiền như cảm
biến siêu âm luôn là hướng nghiên cứu phát triển
rất mạnh
Khó khăn lớn nhất hiện nay cho việc chế tạo thử
nghiệm một robot là kinh phí Để thử nghiệm một
robot di động, các phòng thí nghiệm trên thế giới
thường phải bỏ ra từ 5000 USD đến hàng trăm
nghìn USD hoặc lớn hơn nhiều Trong khi đó, ở
Việt Nam, con số này là một con số không tưởng
Trước mắt, chúng tôi đã thực hiện đề tài này ở
mức độ chương trình mô phỏng, như trình bày ở
trên, nhằm mục đích phục vụ cho việc học tập,
giảng dạy và nghiên cứu Ngoài ra, sau khi phân
tích sơ bộ kinh phí thực hiện đề tài, chúng tôi mong
muốn có được một khoản tài trợ 2000 USD cho việc
thực hiện một mô hình robot hoàn chỉnh để có thể
tiếp tục thí nghiệm và phát triển đề tài
Hướng phát triển của đề tài sẽ là việc phát triển
các thuật toán thăm dò bản đồ, định vị và tìm đường
đi cho robot thông qua chương trình mô phỏng
Nâng cấp thuật toán để robot có thể hoạt động
trong không gian ba chiều
TÀI LIỆU THAM KHẢO
[Brown00] R.G.BROWN and B.R.DONALD,
“Mobile Robot Self Localization without Explicit
Landmarks”, p 515- 559, Algorithmica 2000
[Borenstein94] J BORENSTEIN and L FENG,
“UMBmark – A Method for Measuring, Comparing,
and Correcting Dead Reckoning Errors in Mobile
Robots”, Technical Report, University of Michigan,
Dec 1994
[Borenstein95] J BORENSTEIN and Y KOREN,
“Error Eliminating Rapid Ultrasonic Firing for
Mobile Robot Obastacle Avoidance”, IEEE Transactions on Robotics and Automation, 1995
[Borenstein96] J BORENSTEIN, L FENG and H
R EVERETT, “Where Am I? – Sensors and Methods for Mobile Robot Positioning”, Technical Report, University of Michigan, Apr 1996
[CNL03] HOWIE CHOSET, KEIJI NAGATANI, NICOLE A LAZAR, “The Arc Transversal Algorithm: Approach to Increasing Ultrasonic Sensor Azimuth Accuracy”, IEEE Transaction on Robotics and Automation, Vol 19,
No 3, June 2004
[Diep99] TRẦN TUẤN ĐIỆP, LÝ HOÀNG TÚ, “Lý thuyết xác suất và thông kê toán học”, In lần thứ 2, NXB Thống kê, 1999
[Elfes85] ALBERTO ELFES, “Ultrasonic Sensors”, Carnegie Mellon University, 1985
[Latombe91] JEANS CLAUDE LATOMBE,
“Robot Motion Planning”, Kluwer Academic Publisher, 1991
[Marsland02] STEPHEN MARSLAND,
“CS3451 Laboratory Exercises: Guide to the Robots”, 2002
[Moravec85] HANS P MORAVEC and ALBERTO ELFES, “High Resolution Maps from Wide Angle Sonar”, In Proc 1985 IEEE
International Conference on Robotics and Automation, p 116 – 121, St Louis, MO, 1985
[Moravec89] HANS P MORAVEC, “Sensor Fusion in Certainty Grid for Mobile Robots”, Carnegie Mellon University, Sensor Devices and Systems for Robotics, Springer Verlag, NY,1989
[Murphy00] ROBIN R MURPHY,
“Introduction to AI Robotics”, Massachusetts Institute of Technology, 2000
[Muir88] PATRICK FRED MUIR,
“Modeling and Control of Wheeled Mobile Robot”, PhD thesis at Carnegie Mellon University, 1988
[Thuan03] NGUYỄN ĐỨC THUẬN, NGUYỄN VŨ SƠN, TRẦN ANH VŨ, “Cơ sở kỹ thuật siêu âm”, NXB Khoa học và Kỹ Thuật, 2003
