Proceedings VCM 2012 61 dẫn đường và tránh vật cản cho robot di động

Tuyển tập công trình Hội nghị Cơ điện tử toàn quốc lần thứ 6 451
Mã bài: 107
Dẫn đường và tránh vật cản cho robot di động
dựa trên ảnh laser 3D và siêu âm
Navigation and obstacle avoidance for mobile robot
using the 3D-laser image and ultrasonic signal
Trần Thuận Hoàng, Phùng Mạnh Dương, Đặng Anh Việt và Trần Quang Vinh
Trường Đại học Công nghệ, Đại học Quốc gia Hà nội
e-Mail:
Tóm tắt
Bài báo trình bày việc sử dụng hệ đo đa cảm biến với các sensor đo xa laser và siêu âm để thực hiện việc
dẫn đường và tránh vật cản cho robot di động tự trị trong nhà. Hệ thống có khả năng thu nhận được hình ảnh
3D của môi trường gồm cả chiều cao của không gian. Từ đám mây dữ liệu các điểm ảnh ba chiều đó, một giải
thuật ép ảnh và phát hiện vật cản IPaBD (3D-to-2D Image Pressure and Barriers Detection) đã được phát triển
và áp dụng, cho phép tạo lập được một bản đồ toàn cục 2D phát hiện vật cản trên mặt phẳng chuyển động của
robot. Bản đồ này làm cơ sở dữ liệu tốt cho các bước thiết kế quỹ đạo bằng giải thuật tìm kiếm A* hay phương
pháp đồ thị Voronoir với hàm điều khiển Lyapunov. Chương trình tránh vật cản bằng phương pháp trường thế
trong không gian cục bộ cũng được phát triển để đưa robot tới đích an toàn.
Abstract
Paper presents the use of multi-sensor measurement system with laser range finder and ultrasonic sensors to
navigate and avoid obstacles for an in-door autonomous mobile robot. The system is capable to acquisit the 3D
image of environment included the high dimension of objects. From the data cloud of 3D image points, an
algorithm of 3D-to-2D image pressure and barriers detection (IPaBD) was developed and applied to create a
global map in the moving plane of robot. This map is the good data for trajectory design approach using the
A* algorithm or the Voronoir graph using the Lyapunov controlling function. The obstacles avoidancing
program using the voltage field method was also developed in order to guide robot to destination safely.
Keyword: 3D-laser image, 2D-laser scanner, localization, navigation, obstacles avoidance.

1. Phần mở đầu
Dẫn đường là hoạt động điều khiển robot đi tới
đích một cách tin cậy và hiệu quả, dựa trên các dữ

liệu nhận biết về môi trường và các thông tin đọc
được từ các cảm biến theo thời gian thực. Cần có
2 năng lực cơ bản cho việc thiết kế dẫn đường
một robot di động tự trị. Đó là việc khi có được
một bản đồ môi trường cùng với vị trí hiện tại của
robot và đích, phải vạch ra được một quỹ đạo để
đi đến đích. Đây là bài toán mang tính toàn cục vì
robot phải quyết định chuyển động thế nào trong
suốt thời gian dài tới đích. Năng lực thứ hai cũng
quan trọng nhưng mang tính cục bộ. Đó là, dựa
trên các số liệu đọc được từ các cảm biến theo
thời gian thực, robot phải điều chỉnh được quỹ
đạo trên trong một vùng cục bộ để tránh va chạm
với các vật cản đột xuất mà vẫn giữ được hướng
đi toàn cục tới đích.
Như vậy, quá trình dẫn đường cho robot đi động
tự trị có thể chia làm các bước: lập bản đồ môi
trường, định vị robot, thiết kế quỹ đạo và điều
khiển chuyển động, tránh vật cản.
Với môi trường có cấu trúc, các thông tin nhận
biết trước đó cho phép tạo ra bản đồ môi trường.
Nhưng với môi trường phi cấu trúc (không biết
trước) robot cần có khả năng tự quan sát môi
trường để có thể xây dựng được một bản đồ trong
bộ nhớ của nó để có thể thiết kế một quỹ đạo dẫn
nó tới đích. Với yêu cầu giới hạn của bài toán di
chuyển trong một không gian trên mặt phẳng sàn
nhà, nhiệm vụ này có thể được thực hiện khá
nhanh chóng trên một cảm biến đo xa laser 2D
hiện đại để có được một đám mây các điểm ảnh

môi trường 3D quanh robot. Vấn đề là cần trích
xuất được các thông tin của hình ảnh 3D để hình
thành một bản đồ 2D trên mặt sàn nhà với đầy đủ
các vị trí bị chiếm chỗ bởi các vật cản có các
chiều cao nhỏ hơn hoặc bằng chiều cao của robot.
Có một số tác giả như trong [1][2] đã đề ra các
phương pháp xác lập bản đồ trong đó chủ yếu dựa
trực tiếp vào các phương pháp phân vùng 3D
phức tạp và có giá tính toán rất cao. Trong bài
báo này, chúng tôi đề xuất một giải thuật cho
phép xây dựng bản đồ 2D này một cách đơn giản
dựa trên đặc điểm của thiết bị đo xa laser và yêu
452 Trần Thuận Hoàng, Phùng Mạnh Dương, Đặng Anh Việt và Trần Quang Vinh
VCM2012
cầu tránh vật cản của một robot di động cụ thể.
Giải thuật được gọi là IPaBD, có nghĩa là “ép ảnh
3D thành 2D và phát hiện vật cản” (3D-to-2D
image pressure and barrier detection). Nội dung
này sẽ được trình bày trong phần 2 của báo cáo.
Dựa trên bản đồ 2D được xác lập, việc thiết kế
quỹ đạo toàn cục đã được thử nghiệm bằng 2
phương pháp: phương pháp tìm đường theo giải
thuật A* với không gian được rời rạc hóa thành
các “ô chiếm giữ” [3] và phương pháp tìm đường
bằng đồ thị Voronoi [4] với điều khiển đường đi
liên tục bằng hàm Lyapunov [5][10]. Một số cơ
sở lý thuyết liên quan đến các thử nghiệm này sẽ
được trình bày trong phần 3. Đối với nhiệm vụ
tránh vật cản gần trong vùng cục bộ, chúng tôi đã
áp dụng phương pháp trường thế năng [6] với một

hệ các cảm biến đo xa siêu âm, phát triển chương
trình điều khiển cho phép tránh được các vật cản
trong vùng từ 0,3m đến 4m. Các kết quả tránh vật
cản trong quá trình dẫn đường cũng được trình
bày trong phần 3 của báo cáo. Phần 4 trình bày
các kết quả đo đạc thực nghiệm cho thấy hiệu quả
của việc tổng hợp dữ liệu từ các cảm biến cho dẫn
đường robot di động tự trị, nó cho phép mở ra
những khả năng ứng dụng với các xe tự hành
thông minh trong thực tế.
2. Xây dựng bản đồ 2D từ đám mây các điểm
ảnh 3D của môi trường
2.1. Thu số liệu từ các cảm biến laser và siêu
âm.
Hình 1 là ảnh cảm biến đo xa laser LMS-211 và
các cảm biến siêu âm được gắn lên một robot di
động tự trị được thiết kế. Việc đo khoảng cách
đến một điểm trên vật cản dựa trên nguyên tắc
xác định thời gian đi-về của một xung laser phản
xạ từ vật cản với các góc quét lệch ngang (yaw)
khác nhau. Để nhận được hình ảnh 3D của môi
trường, máy đo xa laser được gắn lên một đế có
thể quay ngẩng lên-xuống quanh một trục nằm
ngang như mô tả chi tiết tại [7]. Trong nghiên cứu
này, chúng tôi đã điều chỉnh các thông số của hệ
đo laser sao cho với mỗi lần quay ngẩng lên
(pitching up), cảm biến thu nhận được 94 khung
ảnh 2D của môi trường, ứng với từng góc ngẩng
trong dải từ -5 đến +20, với bước tăng góc
ngẩng là (5+20)/94 = 0,266. Mỗi khung ảnh

2D nhận được ứng với 1 góc ngẩng 
j
là do thu
thập số liệu khi tia laser được quét ngang với các
góc quét 
k
trong dải quét 100 (từ 40 đến 140),
độ phân giải 1. Như vậy, một ảnh 3D thu được
sẽ là một đám mây với 10194 = 9.494 điểm ảnh.

H.1. Robot đa cảm biến.
Ảnh này có được tính tuyến tính và độ chính xác
theo chiều cao là do tốc độ quét ngẩng là không
đổi. Sự ổn định tốc độ cỡ ±5% này được duy trì là
nhờ một bộ điều khiển nhúng lắp trên vi xử lý với
giải thuật điều khiển PID [7]. Do cảm biến được
đặt ở độ cao so với mặt sàn 0,4m nên ở góc ngẩng
cực tiểu -5, cảm biến có thể phát hiện ra các
vật có chiều cao từ 0m trở lên ở khoảng cách tối
thiểu 4,58 m. Các vật cản cách robot một khoảng
nhỏ hơn sẽ được phát hiện nhờ một hệ thống gồm
8 cảm biến đo xa siêu âm đã được lắp đặt điều
khiển bằng vi xử lý. Tổng hợp số liệu từ 2 hệ
thống cảm biến sẽ cho robot một hình ảnh toàn
cục bản đồ môi trường phía trước.
2.2. Xây dựng bản đồ toàn cục từ số liệu ảnh
laser 3D phía trước robot.
Tại một góc quét ngẩng

j

(với j = 1 94) robot
sẽ thu được 1 khung ảnh 2D gồm các điểm ảnh
được xác định bởi các góc quét ngang

k
(k = 1
101) và khoảng cách đến vật R
k
tương ứng. Từ đó
có thể xác định được tọa độ Descarter của mỗi
điểm ảnh tạo nên hình ảnh 3D của môi trường
[7]:
jkkj
kjkkj
kjkkj
Rz
Ry
Rx




sin
sincos
coscos
,
,
,




(1)
Do robot di chuyển trên mặt phẳng sàn nhà, nên
hình chiếu điểm ảnh các vật lên mặt phẳng (x,y)
song song với sàn nhà sẽ được dùng để xây dựng
bản đồ 2D. Thực hiện phép hợp (U) tất cả các
điểm ảnh có tọa độ (x,y), ta được kết quả “ép” các
khung trong ảnh 3D của môi trường theo phương
z thành một bản đồ 2D duy nhất trên mặt phẳng
x-y như hình 2.
Tuyển tập công trình Hội nghị Cơ điện tử toàn quốc lần thứ 6 453
Mã bài: 107

H.2. Ép các khung trong ảnh 3D thành bản đồ
2D.
Ảnh bản đồ 2D thu được như trên là một ma trận
các điểm ảnh, mỗi điểm được biểu diễn bằng một
cặp thông số (, R), trong đó 
k
là góc quét của
tia laser thứ k và R
k
là khoảng cách đo được đến
điểm ảnh. Sự phụ thuộc giữa hai thông số này là
không đơn trị như trong mỗi khung quét laser 2D
thông thường. Tức là, ứng với một góc quét
ngang  có thể có một hoặc nhiều điểm có R khác
nhau (ứng với các độ cao z khác nhau). Trong các
xử lý dẫn đường thông thường, các bước cần tiến
hành là phân vùng ảnh để tách ra các biên ảnh 3D

cần thiết, từ đó xác định được khoảng không gian
tự do cho robot di động tới đích. Tuy nhiên, nhận
thấy bản đồ 2D thu nhận được qua quá trình ép
ảnh kể trên thực ra chỉ là hình ảnh bề mặt các vật
cản hướng về phía robot. Do đó chỉ cần xác định
phần biên các vùng ảnh trước “tầm nhìn” (thị
trường) của robot bằng việc lựa chọn các điểm
ảnh (hình tròn) có khoảng cách đến robot gần
nhất trong số các điểm ảnh cùng chung một góc
quét laser như hình 3 chỉ ra.







H.3. Phát hiện các điểm ảnh có khoảng cách
cực tiểu (các điểm tròn trên đường liền
nét).
Các điểm ảnh còn lại trên cùng góc quét không
được chọn (các điểm hình vuông) là ở các cao độ
z khác nhưng có khoảng cách xa hơn khoảng cách
các điểm được chọn (hình tròn).
Sau bước xử lý này, sự phụ thuộc (, R) trở nên
đơn trị và có thể dễ dàng áp dụng các thuật toán
phân đoạn phổ biến hiện nay. Chúng tôi đã sử
dụng phương pháp PDBS với giải thuật đơn giản
dựa trên khoảng cách Euclidean [8] cho quá trình
phân đoạn này.Với mục đích cụ thể của bài toán,

cần phát hiện và tránh các vật cản đứng trên sàn
nhà phẳng, có chiều cao z thấp hơn một giá trị
ngưỡng (ở đây là chiều cao của robot, bằng 1,2
m); bản đồ 2D trở nên đơn giản hơn nữa và lộ ra
phần không gian tự do cho robot có chiều cao xác
định khi được loại bỏ tất cả các điểm ảnh có giá
trị độ cao lớn hơn độ cao robot (z
k
> 1,2 m) và
các điểm ảnh có z
k
= 0m đại diện cho sàn nhà.
Như vậy toàn bộ các bước xây dựng bản đồ 2D
được tiến hành lần lượt theo các bước của giải
thuật được chúng tôi gọi là IPaBD (3D-to-2d
image pressure and barrier detection) như sau:
Giải thuật 3D-to-2D IPaBD
 Bước 1: Xuất phát từ tập hợp số liệu đám
mây điểm ảnh 3D, thực hiện phép hợp tất
cả các điểm ảnh lên mặt phẳng tọa độ
(x,y).
 Bước 2: Với mỗi góc quét ngang 
k
, tìm
và chọn giá trị R
k
= R
min
.
 Bước 3: Loại bỏ các điểm có z > z

ngưỡng

và z = 0.
 Bước 4: Thực hiện giải thuật phân đoạn
ảnh (nếu cần).

3. Dẫn đường và tránh vật cản cho robot
3.1. Dẫn đường robot
Bước tiếp theo là tìm đường đi tối ưu cho robot
tới đích (path planning). Chúng tôi đã thử nghiệm
2 phương pháp: tìm đường bằng giải thuật tìm
kiếm A* dựa trên việc số hóa không gian thành
các ô chiếm giữ và tìm đường tối ưu theo đồ thị
liên tục Voronoir với việc áp dụng luật điều khiển
theo hàm Lyapunov.
Trong thực nghiệm thứ nhất, không gian mặt sàn
được rời rạc hóa thành một ma trận M(j,k) ô chữ
nhật, mỗi ô có kích thước (a  a) cm, gọi là ô
chiếm giữ. Giá trị a được chọn bằng 1/3 đường
kính thiết diện ngang của robot. Như vậy tâm một
ô chiếm giữ M(j,k) sẽ có tọa độ là (j.a+a/2,
k.a+a/2) và chiếm một vùng tọa độ x từ j.a đến
j(a+1) và vùng tọa độ y từ k.a đến k(a+1). Nếu
điểm ảnh trên bản đồ có tọa độ nằm trong vùng
tọa độ ô nào thì ô đó được gọi là bị chiếm giữ và
được gán giá trị logic “1”, các ô còn lại thuộc về
không gian tự do, có giá trị “0”. Từ số liệu các
vùng có các ô bị chiếm giữ, cần dùng giải thuật
“dãn ảnh” (dilation) để dãn ra một ô nữa, tạo
vùng không gian ngăn robot va chạm với mép

vùng bị chiếm giữ. Từ các dữ liệu bản đồ các ô bị
chiếm giữ bởi điểm ảnh các vật cản, cùng các giá
trị của 2 ô xuất phát S(x
S
, y
S
) và ô đích D(x
D
,y
D
)
có thể áp dụng giải thuật A* để vạch ra đường đi
ngắn nhất qua các ô tự do để đến ô đích.
Trong thực nghiệm thứ hai, từ tọa độ các điểm
đặc trưng được trích chọn từ các đoạn ảnh, đồ thị
Voronoir được xây dựng cho phép tìm ra một
đường đi tối ưu qua các điểm nút, là điểm giữa
của các cửa tự do có kích thước rộng hơn đường

k

Robot

x
y

y
454 Trần Thuận Hoàng, Phùng Mạnh Dương, Đặng Anh Việt và Trần Quang Vinh
VCM2012
kính robot. Xuất phát từ tư thế (tọa độ và hướng)

của robot tại điểm xuất phát và tư thế của robot
cần có tại các điểm nút cũng như điểm đích; các
hàm Lyapunov được áp dụng để dẫn đường robot
đi qua từng cặp điểm nút kề cận nhau cho tới
điểm đích. Phương pháp sử dụng điều khiển bằng
hàm Lyapunov cho một quỹ đạo liên tục và tối
ưu.
Ta biết rằng để dẫn đường cho robot đi từ tư thế
này đến tư thế kia, ngoài những điều kiện ràng
buộc về điều kiện nonholonomic của hệ thống
bánh xe, còn phải đáp ứng điều kiện tối ưu của
luật điều khiển [9].


H.4. Các thông số và thế của robot [10]

Từ hình 4 luật điều khiển có thể được tóm tắt như
sau: với 2 điểm tham chiếu trong mặt phẳng OXY
là O
1
và O
2
mà robot đi qua, thế của robot bao
gồm vị trí và hướng của robot tại O
1
là x
1
,y
1
,θ

1
và
O
2
là x
2
,y
2
,θ
2
. Với ρ là khoảng cách từ O
1
đến O
2
;

là góc tạo bởi hai véc tơ O
1
O
2
với O
2
X
2
; α là
góc tạo bởi hai véc tơ O
1
O
2
với O

1
X
1
.







),(2tan
),(2tan
)()(
22
222
2
2
2
2
xxyya
xxyya
yyxx
(2)
Luật điều khiển ràng buộc vận tốc để đảm bảo
quỹ đạo của robot đi từ điểm ban đầu O
1
đến đích
O
2

với các biến giá trị điều khiển (ρ, α,

), được
gọi là các biến dẫn đường, và chúng sẽ hội tụ về
zero khi robot tiến về đích. Mô hình động học của
robot di động có thể được biểu diễn như sau:





sin cos yx (3)
Trong đó,  và v lần lượt là vận tốc góc và vận
tốc dài của robot. Mô hình động học của robot
được mô tả qua các biến dẫn đường (

,

,

) như
sau:
α/ρνφα/ρ ν-ωα sinsin cos 




(4)
Hàm Lyapunov được xây dựng như sau:






/))(/sincos(
cos
2/)(2/
2
2
2
21
hkk
k
hVVV
gg



(5)
Trong đó k
v
và k
α
là các hệ số vận tốc, theo [9-10]
để đáp ứng điều kiện tối ưu của luật điều khiển
thì đạo hàm bậc nhất của V
g1
và V
g2
luôn luôn có

giá trị âm do đó các biến dẫn đường sẽ hội tụ về
không ở tại tọa độ đích.
Gọi tọa độ thật của robot là
ˆ
ˆ ˆ
( , , )
X Y

, tọa độ đo
được của robot là (X,Y,).
Đặt 
X
, 
Y
, 

là nhiễu đo của phép đo vị trí
(
ˆ
ˆ ˆ
, ,
X Y

) của robot. Các giá trị đo của vị trí và
hướng X, Y,  như sau:
ˆ
X
X X

 

,
ˆ
Y
Y Y

 
,
ˆ

  
 
.
Trong đó
X X
 
 ,
Y Y
 
 ,
 
 
 .
Đặt
, ,
  
  
lần lượt là các thành phần nhiễu của
các biến navigation:
   
   

   
   
2 2
2 2
2 2 2 2
2 2
2 2
2 2 2 2
ˆ ˆ
ˆ ˆ
X X Y Y X X Y Y
X X Y Y X X Y Y


  


  
       
       
 
(6)
Giá trị thật của các biến navigation là:

   
 
 
2 2
2 2
2 2 2

2 2
ˆ ˆ
ˆ
ˆ ˆ ˆ
atan2 ,
ˆ
ˆ ˆ
ˆ
atan2 ,
X X Y Y
Y Y X X
Y Y X X

 
 
   
   
   
(7)
Các biến này xác định các yếu tố đầu vào  và v
để điều khiển robot đi theo quỹ đạo (5). Các
nhiễu đo (
X
, 
Y
, 
θ
) ảnh hưởng đáng kể đến hiệu
quả của mô hình điều khiển, bộ lọc Kalman mở
rộng được sử dụng để tổng hợp dữ liệu từ các

cảm biến và chuyển động học của robot để ước
lượng chính xác hơn trạng thái của hệ thống như
biểu diễn trên hình 5.


H.5. Sơ đồ khối của vòng điều khiển

3.2. Tránh vật cản bằng phương pháp trường
thế với các cảm biến siêu âm
Trên đường đi, robot phải có khả năng phát hiện
và tránh được các vật cản đột xuất.Với các trường
hợp này, một hệ thống cảm biến phát hiện vật thể
gần (proximity sensor) được sử dụng, đó là 8 cảm
biến siêu âm đo khoảng cách được bố trí trên
robot như hình 6, cho phép phát hiện các vật cản
phía trước và hai bên cạnh robot.
Tuyển tập công trình Hội nghị Cơ điện tử toàn quốc lần thứ 6 455
Mã bài: 107

H.6. Sơ đồ bố trí các cảm biến siêu âm.
Phương pháp trường thế [6] đã được áp dụng để
phát triển chương trình điều khiển dẫn đường,
tránh vật cản. Theo cách tiếp cận này, các vật cản
được coi là sinh ra các lực đẩy robot F
r
và đích
tác động lực hút F
t
lên robot như biểu diễn trên
hình 6. Tổng các lực này tạo nên lực R xác định

góc hướng và vận tốc chuyển động của robot tại
thời điểm hiện tại.








H.7. Phương pháp trường thế [6].
Gọi tọa độ của mục tiêu là ),(
goalgoalgoal
yxq  ,
tọa độ của robot là ),( yxq . Lực tạo ra từ mục tiêu






att att att goal
F q U q k q q     tạo ra vận tốc
dài và vận tốc góc hướng về mục tiêu là:


goalgoalgoalgoalattatt
yyxxkqV

sin)(cos)()( 



att goal
k
  
   (8)
Với ),(2tan xxyya
goalgoalgoal


là góc hướng
của đường thẳng nối từ robot tới mục tiêu;

là
góc chỉ hướng tức thời của robot trong hệ tọa độ
Decaster OXY.
Khi robot đi vào vùng lân cận (khoảng cách tới
vật cản nhỏ hơn d
0
) vật cản thứ i và vật cản nằm
trong phạm vi


60 , 60
o o
 
  trong hệ tọa độ
cực có gốc tọa độ là tâm của robot, thì vận tốc dài
và vận tốc góc robot là:

 
 
 
 
 
0
2
0
0
1 1 1 1
2
0
i i
obst obst
i
i
i
obst
rep
obst
i
obst
k d q d
dd q
V q
d
d q d

 
 


 

 

 




(9)


i i
att obs
k
  
   (10)
Trong đó, nếu vật cản obs
i
ở phía trước, bên phải
robot thì:



atan2 , 60
i o
obs goal goal
Y Y X X


    , nếu vật cản
obs
i
ở phía trước, bên trái robot thì


atan2 , 60
i o
obs goal goal
Y Y X X

   
Trong ứng dụng này, chúng tôi lựa chọn các tham
số như sau:
0
0,7 10 10
i
att rep
d m k k
  

3. Thực nghiệm và thảo luận
3.1. Kết quả xây dựng bản đồ bằng giải thuật
IPaBD
Với các thông số của thiết bị được chọn như trình
bày trên, ảnh 3D thu được tại hiện trường như
hình 8 với điểm xuất phát S(0,0) và đích D(-
2.200,6.800).

H.8. Ảnh chụp 3D môi trường toàn cục.

Robot có chiều cao là 1,2 m. Tại hiện trường, có
một cổng (A) với thanh dầm nằm ở vị trí thấp hơn
chiều cao của robot và một cổng (B) có thanh
dầm nằm cao hơn robot. Ngoài ra có một hành
lang (C) với chiều cao không hạn chế nhưng ở vị
trí bên cạnh. Robot cần chọn cho mình một
đường đi từ S tới đích D ngắn nhất mà không va
chạm với các vật cản.
Hình 9 là bản đồ ảnh 2D sau khi được ép lên mặt
phẳng x-y với tất cả 9.494 điểm ảnh.
Hình 10 là bản đồ các điểm cản phía trước tầm
nhìn của robot được phát hiện qua giải thuật
IPaBD.
H.9. Bản đồ 2D với toàn bộ điểm ảnh 3D
được ép trên mặt phẳng x-y.
Biểu đồ lưới
Các giá trị
chiếm giữ

456 Trần Thuận Hoàng, Phùng Mạnh Dương, Đặng Anh Việt và Trần Quang Vinh
VCM2012

H.10. Kết quả bản đồ thu được do giải thuật
IPaBD.

3.2. Tìm đường bằng giải thuật A*
Kết quả sử dụng giải thuật tìm kiếm A* cho
đường đi số tối ưu qua các lưới chiếm giữ cho
trên hình 11. Ở đây không gian bản đồ được chia
thành 4040 ô. Do kích thước của robot là

(8080 cm), mỗi ô được chọn là (2020 cm). Ta
thấy bản đồ của các vùng bị chiếm giữ bao gồm
cả phần dãn ảnh bằng bán kính của robot 40cm
(có độ sáng đậm). Kết quả tìm đường cho thấy,
robot không đi qua cổng (A) do phát hiện có vật
cản là thanh dầm (do không gian 3 chiều) và cũng
không đi qua hành lang (C) vì có đường đi dài
hơn mà sẽ đi qua cổng (B) mặc dù cũng có thanh
dầm nhưng lại nằm ở độ cao hơn chiều cao của
robot.


H.11. Dẫn đường robot bằng giải thuật A*.

3.3. Tìm đường bằng đồ thị Voronoi và điều
khiển robot bằng hàm Lyapunov
Từ bản đồ hình 10, xây dựng được đồ thị Voronoi
với tất cả các đường đi qua các nút là điểm giữa
các khoảng không gian tự do cho robot có thể di
chuyển. Từ đó có thể chọn ra một đường đi ngắn
nhất là đường S-B-D như hình 12 chỉ ra.
Quá trình điều khiển robot đơn giản được thực
hiện như sau: từ điểm xuất phát, robot quay tại
chỗ cho tới đúng hướng SB, sau đó chuyển động
thẳng về B. Đến B, robot quay tại chỗ cho tới
đúng hướng BD rồi chuyển động thẳng về D.
Trước tiên, chúng tôi đánh giá khả năng có thể sử
dụng được của bộ điều khiển dùng hàm
Lyapunov được áp dụng cho robot tự trị.

H.12. Bản đồ Voronoi với đường đi tối ưu qua
nút B (đường đậm nét)

Thử nghiệm kiểm tra dẫn đường robot từ điểm
đầu có tọa độ và góc hướng x,y, là S(0,0,0),
điểm cuối D(2,2,30
0
), (2,2,60
0
) và (2,2,90
0
) cho
kết quả đạt yêu cầu như hình 13(a) chỉ ra với sự
thay đổi vận tốc dài và vận tốc góc của robot tới
giá trị ổn định ở điểm cuối như kết quả trên hình
13(b) và 13(c).


H.13. Đường điều khiển Lyapunov (a) và kết quả
điều khiển ổn định (b)(c).

Phần mềm điều khiển bằng hàm Lyapunov sau đó
đã được áp dụng và kết quả khả quan thu được
trên hình 14 là quỹ đạo thực của robot đạt được
trong thực nghiệm.
Tuyển tập công trình Hội nghị Cơ điện tử toàn quốc lần thứ 6 457
Mã bài: 107

H.14. Quỹ đạo dẫn đường robot bằng đồ thị

Voronoi và hàm Lyapunov.
3.4. Tránh vật cản bằng hệ các cảm biến siêu
âm.
Thực nghiệm được tiến hành bằng thao tác đưa
vào một vật cản O đột xuất trong quá trình di
chuyển của robot trên quỹ đạo định trước. Hình
15 cho kết quả khả quan khi robot có khả năng
tránh được vật cản đó theo phương pháp trường
thế và tiếp tục bám theo quỹ đạo toàn cục để về
tới đích.

H.15. Tránh vật cản đột xuất trên quỹ đạo robot.
Hình 16 là ảnh toàn cảnh môi trường thực nghiệm
và robot.

H.16. Môi trường thực nghiệm.
4. Kết luận
Bài báo đã trình bày quá trình nghiên cứu dẫn
đường cho một robot di động hoạt động tự quản
trị qua các giai đoạn lập bản đồ toàn cục, vạch
đường đi và tránh vật cản trong không gian 3
chiều. Căn cứ vào đặc điểm của các số liệu thu
được từ cảm biến laser, vào yêu cầu chuyển động
của robot trên mặt sàn phẳng cũng như kích thước
robot, một giải thuật lập bản đồ 2D từ số liệu 3D
gọi là IPaBD đã được phát triển. Bản đồ cho được
dữ liệu tin cậy để từ đó phát triển được các phần
mềm tìm đường đi tối ưu cho robot tới đích theo
bản đồ số rời rạc bằng phương pháp tìm kiếm A*
hay bằng điều khiển liên tục qua các nút đường đi

trên đồ thị Voronoi với luật điều khiển Lyapunov.
Phương pháp hàm Lyapunov tuy có thể đòi hỏi
một quãng đường đi dài hơn phương pháp trước
nhưng bù lại sẽ có được một quỹ đạo liên tục, đáp
ứng được cả góc hướng của robot tại điểm đích.
Phần mềm điều khiển robot tránh vật cản theo
phương pháp trường thế cũng được phát triển cho
được kết quả tốt trong phạm vi không gian cục
bộ. Các kết quả nghiên cứu này có thể được phát
triển cho việc nghiên cứu dẫn đường các xe tự
hành trong môi trường ngoài nhà.
Tài liệu tham khảo
[1] P. de la Puente et al Extraction of
Geometrical Features in 3D Environments for
Service Robotic Applications. Springer-Verlag
Berlin Heidelberg 2008.
[2] Johannes Strom, Andrew Richardson, Edwin
Olson. Graph-based Segmentation for Colored
3D Laser Point Clouds. The 2010 IEEE/RSJ
International Conference on Intelligent Robots
and Systems October 18-22, 2010, Taipei,
Taiwan.
[3] J. Borenstain, H.R. Everette, and L. Feng.:
Where I am? Sensor and Methods for Mobile
Robot Positioning. University of Michigan,
1996
[4] Priyadarshi Bhattachrya and Marina L.
Gavrilova. Roadmap-Based Path Planning
Using the Voronoi Diagram for a Clearance-
Based Shortest Path. IEEE Robotics &

Automation Magazine, June 2008.
[5] Augie Widyotriatmo, Keum-Shik Hong2, and
Lafin H. Prayudhi,:Robust stabilization of a
wheeled vehicle: Hybrid feedback control
design and experimental validatio. Journal of
Mechanical Science and Technology 24 (2)
(2010) 513~520
[6] Y. Koren, J. Borenstein,:Potential Field
Methods and Their Inherent Limitations for
Mobile Robot Navigation. Proceedings of the
IEEE Conference on Robotics and
Automation. Sacramento, California, April 7-
12, 1991, pp. 1398-1404
[7] Trần Thuận Hoàng, Đặng Anh Việt, Trần
Quang Vinh,: Xây dựng hệ đo xa 3D sử dụng
cảm biến laser dùng cho robot di động tự trị.
Hội nghị toàn quốc về Điều khiển và Tự động
458 Trần Thuận Hoàng, Phùng Mạnh Dương, Đặng Anh Việt và Trần Quang Vinh
VCM2012
hoá - VCCA-2011, 257-262, Hà Nội, Việt
Nam.
[8] Cristiano Premebida and Urbano Nunes,
“Segmentation and geometric primitives
extraction from 2D laser range data for
mobile robot applications”. Robótica 2005 –
Actas do Encontro Cienistisfico Coimbra, 29
de Abril de 2005.
[9] A. Widyotriatmo, B. H. Hong and K S.
Hong,: Predictive navigation of an
autonomous vehicle with nonholonomic and

minimum turning radius constraints. J. Mech.
Sci. Technol., 23 (2) (2009) 381-388.
[10] Thuan Hoang TRAN, Manh Duong PHUNG,
Van Tinh NGUYEN and Quang Vinh TRAN,
“A Path Following Algorithm for Wheeled
Mobile Robot Using Extended Kalman Filter
,” IEICE Proceeding of the 3th international
conference on Integrated Circuit Design ICDV
(IEICE 2012), August 2012 Vietnam.