Tài liệu Robot di động tự định vị không dùng cột mốc docx

1
ROBOT 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
2
vectơ 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:
Vùng I:
Max
R
rR
HsP *
2
)()(
)|(
β
αβ
−

+
−
=
(1)
Vùng II:
2
)()(
1)|(
β
αβ
−
+
−
−=
R
rR
HsP
(2)
Vùng III:
)|( HsP
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
)|( sHP
. p dụng
công thức toàn phần Bayes [Diep99], như sau:
)()|()()|(
)()|(
)|(

HPHsPHPHsP
HPHsP
sHP
−−−
=
(3)
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)()(
=−=−=
HPHPHP
(4)
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:
)|()|()|()|(
)|()|(
)|(
11
1
−−
−
−−+
=
nnnn
nn
n

sHPHsPsHPHsP
sHPHsP
sHP
(5)
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
3
Vấ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( () (),( zlMzMzMFP Θ−Θ∂=
(6)
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
zM Θ
cũng sẽ nằm trong tập hợp
),0( zlM Θ
, do đó, chúng ta có thể đơn giản công
thức (6) thành dạng
)),0( () (),( zlMzMzMFP Θ−Θ=
(7)

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:
),(),( zMFPZMFP
Zz∈
= I
(8)
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
zlMMzMFP
ε
εε
−Θ−⊕Θ=
(9)
o
o
Q
o
-Q
P
QP Θ
o
4
Trong đó,
ε
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( mmArc ==
π

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
1
=
θ
. Đ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 30
o
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
cmcm
Len
cmcmArc
o
3.37
)30tan(
10
)30sin(
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
khoảng
5.8
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

5



∉
∈
=
FPp
FPp
p
zMFP
: 0
: 1
)(
),(
χ
(11)
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:
∑
∈
=
Zz
zMFP
ppZMrnk )(),,(

),(
χ
(12)
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),,(|),( qZMrnkpZMrnkpZMLoc
Cq
(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 đồ