Hoạch định và bám đường đi cho mobile robot ứng dụng điều khiển hồi tiếp ảnh

TẠP CHÍ KHOA HỌC CÔNG NGHỆ GIAO THÔNG VẬN TẢI SỐ 27+28 – 05/2018 
39 
HOẠCH ĐỊNH VÀ BÁM ĐƯỜNG ĐI CHO MOBILE ROBOT ỨNG 
DỤNG ĐIỀU KHIỂN HỒI TIẾP ẢNH 
Le Duc Hanh1, Nguyen Duy Anh1 
1Ho Chi Minh City University of Technology, Vietnam National University Ho Chi Minh City 
ldhanh@hcmut.edu.vn 
Tóm tắt: Bài báo này trình bày phương pháp tìm đường đi ngắn nhất cho mobile robot trong môi 
trường có vật cản, và điều khiển mobile robot bám theo đường đi (ảo) đã vạch ra bằng cách ứng dụng 
tiêu chuẩn ổn định Lyaponive kết hợp với camera hồi tiếp vị trí thực sự hiện tại của robot. Đầu tiên vị 
trí của robot và các điểm đầu, cuối và vật cản được xác địng thông qua cameara đặt trên cao. Sau đó 
giải thuật tìm đường đi ngắn nhất A* sẽ được ứng dụng để tính toán đường đi tối ưu cho robot để đến 
được điểm đích. Vị trí thực của đường ảo sẽ được chuyển sang tọa độ thực thông qua quá trình calib 
camera. Cuối cùng Robot sẽ bám theo đường đi đã hoạch định đó. Quá trình thực nghiệm được xem là 
đạt kết quả tốt khi sai số đường đi thực sự của robot và đường đi đã hoạch định là nhỏ và robot đến 
được đích 
Từ khóa: Điều khiển hồi tiếp, hoạch định đường đi, ổn định lyaponov, xử lý ảnh, hồi tiếp ảnh. 
Chỉ số phân loại: 1.4 
Abstract: This paper presents a visual feedback control algorithm to find a shortest path for a 
task and control robot to follow that calculated direction by using Lyapunov algorithm combining with 
fixed camera. Firstly by using image processing algorithm, the starting, targets, obstacle positions are 
determined. Secondly the shortest path A* algorithm applied to find the shortest path for mobile robot 
to reach the target. The position of the virtual line according to the world coordinate will be planning 
based on calibration process. Finaly, posisiton based visual control algorithm is applied to control 
robot to follow that path. The succession is determnied when robot approachs the target and have 
small error between the calculated path and the real working path of robot. 
Keywords: Visual tracking, Shortest path, Camera calibration, classical control, Image 
processing 
Classification number: 1.4 
1. Giới Thiệu 
Ngày nay robot được sử dụng trong rất 
nhiều lĩnh vực như an ninh quốc phòng, tìm 
kiếm cứu nạn, giúp con người làm việc ở 
những nơi nguy hiểm hay những nơi con 
người không thể tiếp cận được. Robot phục 
vụ trong nhà hiện nay cũng đang là một 
hướng nghiên cứu đầy hứa hẹn. Tuy nhiên, 
hầu hết các robot phục vụ trong nhà hiện nay 
vẫn chưa thể tự động hoàn toàn, việc trang bị 
cho nó một cảm biến đủ mạnh để có thể né 
tránh vật cản hay nhận biết vị trí cần đến 
đang được rất nhiều nghiên cứu trên thế giới 
quan tâm. Các phương pháp dò line truyền 
thống sử dụng đường line cố định mang lại 
nhiều hạn chế trong việc ứng dụng, đường 
line sau một thời gian sử dụng sẽ bị mờ và 
không rõ, dễ bị tác động của môi trường 
xung quanh, và rất phức tạp khi muốn thay 
đổi hướng di chuyển. Việc ứng dụng camera 
vào trong việc điều khiển robot để bám theo 
đường đi đặt ra một nhu cầu thực tế và cấp 
thiết, nó có thể giải quyết những hạn chế của 
việc dò theo line truyền thống và linh động 
trong việc điều khiển hướng đi, nếu có yêu 
cầu của người sử dụng. Đồng thời việc sử 
dụng camera để quy hoạch đường đi còn giúp 
cho việc di chuyển của robot được tối ưu. 
Hiên nay các nghiên cứu về tìm đường đi của 
robot cũng đang được nhiều nhà nghiên cứu 
quan tâm. U. Farooq et al. [1], sử dụng 
phương pháp điều khiển thông minh fuzzy 
kết hợp với các cảm biến hồng ngoại điều 
khiển robot bám theo đường line cho trước. 
J.H. Su [2] cũng sử dụng phương pháp dò 
line bằng các cảm biến quang và phương 
pháp điều khiển on-off để điều khiển robot 
dò theo line. 
Ngày nay camera đóng một vai trò rất 
quan trọng trong sự phát triển của nền công 
nghiệp, dựa vào sức mạnh và tốc độ ngày 
càng tăng của máy tính, camera đóng vai trò 
như một cảm biến hiệu quả giúp cho robot 
nhận diện, phát hiện vật dụng xung quanh 
40 
Journal of Transportation Science and Technology, Vol 27+28, May 2018 
nó và thực hiện một tác vụ theo yêu cầu. Để 
nhận dạng được vật, thông tin thu thập từ 
camera sẽ được xử lý để nhận dạng những 
đặc điểm cốt lõi của vật và truyền tọa độ của 
vật về cho camera. P.T.L. Hải et al. [3] sử 
dụng phương pháp phân đoạn ảnh cùng 
với phép toán hình thái học (morphology) để 
phân biệt, xác định vị trí và tính vận tốc của 
từng robot. Phương pháp này sử dụng phần 
mềm Matlab nên không đảm bảo được tính 
thời gian thực củng như sẽ khó khăn trong 
việc giao tiếp với các robot tay máy có trên 
thị trường không dùng Matlab. Hay H.Đ. 
Chiến et al. [4] sử dụng phương pháp nhận 
dạng vật theo màu sắc sử dụng phần mềm 
Halcon. Phương pháp này sử dụng một phần 
mềm thương mại sẵn có để thực hiện sẽ gây 
khó khăn trong việc phát triển các chức năng 
và kết nối với các thiết bị ngoại vi khác như 
cánh tay robot. N.V. Khanh et al [5] sử dụng 
một camera gắn trực tiếp trên robot để điều 
khiển robot bám theo đường hàn cho trước 
bằng cách sử dụng phương pháp điều khiển 
sử dụng tọa độ điểm ảnh của camera hay A. 
Cherubini et al [6] sử dụng phương pháp điều 
khiển hồi tiếp ảnh để điều khiển một mobile 
robot bám theo đường line màu trắng. 
Các phương pháp dò theo line cho trước 
sử dụng camera và các cảm biến có hạn chế 
đó là chỉ có thể chạy theo một đường line cho 
trước và cố định vì thế nó không được linh 
hoạt trong sử dụng. Việc ứng dụng một 
phương pháp điều khiển ở đó các đường line 
bám theo có thể thay đổi được là một hướng 
ứng dụng thực tiễn trong cuộc sống đặc biệt 
là những nơi những vật cản có thể thay đổi, 
vì thế đường đi cũng phải linh hoạt thay đổi 
theo. Để có thể tạo ra các đường đi ảo đó 
nhiều nhà nghiên cứu đã tập trung nghiên 
cứu vào việc quy hoạch đường đi. Z. 
Miljkovic et al. [7] ứng dụng giải thuật Triz 
và Maz để quy hoạch đường đi cho robot qua 
các trạm dừng và đi đến điểm đích cần đến. 
S. Kloder et al. [8] sử dụng giải thuật tìm 
đường đi cho nhiều robot để cùng thực hiện 
một tác vụ yêu cầu, bằng cách sử dụng định 
nghĩa về hoán vị bất biến nghiên cứu đã giải 
bài toán về va chạm đường đi của nhiều 
robot từ đó hoạch định ra đường đi cho robot. 
Tuy nhiên các nghiên cứu có giới hạn là tập 
trung vào việc nghiên cứu tìm đường đi và 
mô phỏng trên máy tính, việc ứng dụng để 
robot bám theo đường đi đã hoạch định ra 
vận chưa được nghiên cứu sâu. 
Từ những vấn đề thảo luận ở trên, sử 
dụng camera và xử lý ảnh là một phương 
pháp rất hữu dụng cho robot có thể quy 
hoạch đường đi và giúp robot bám theo 
đường đi đã vạch ra trong nhà hay kho 
xưởng. Ý tưởng của đề tài là từ một điểm 
đầu, điểm đích đến và chướng ngại vật, bằng 
cách sử dụng giải thuật thông minh để tìm 
đường đi ngắn nhất, sau đó sử dụng chương 
trình lập trình thông dụng kết hợp với camera 
và mã nguồn mở xử lý ảnh để thực hiện tác 
vụ điều khiển cho robot bám theo đường đi 
đã vạch ra đó. 
2. Tinh Chỉnh Camera 
Camera calibration (hiệu chỉnh camera) 
là một phương pháp tính toán và thực nghiệm 
nhằm tìm ra các thông số của camera cho 
việc tái tạo không gian 3D của một cảnh nào 
đó trong thực tế bằng những ảnh mà camera 
đó chụp lại được. Để có những thông số đó, 
các tính toán sau chủ yếu dựa vào mô hình 
camera thông dụng nhất hiện nay là mô hình 
Pinhold [9] như hình 1. 
Hình 1. Mô hình Pinhold. 
Trong mô hình Pinhold thì một điểm 
 TM X Y Z trong không gian 3D sẽ 
được chuyển đổi về không gian điểm ảnh 2D, 
 Tm x y . Ta có: 
cot
0
sin
1 0 0 1
x x x
x
y
f f cx X
fZ y c Y
Z


 (1) 
Hay công thức (2) có thể viết lại: 
 Zm KM (2) 
TẠP CHÍ KHOA HỌC CÔNG NGHỆ GIAO THÔNG VẬN TẢI SỐ 27+28 – 05/2018 
41 
Với ma trận K được gọi là ma trận thông 
số nội của camera. Để đơn giản ta có thể viết 
lại ma trận thông số nội dưới dạng: 
0
0 0 1
x x
y y
f s c
K f c
với s là tham số nghiêng. Trong thực thế 
thì đối tượng thật có thể biểu diễn bởi hệ tọa 
độ 3D bất kì mà không phải là tọa độ camera 
như ta giả sử. Để tính toán được trên các hệ 
tọa độ này, cách đơn giản nhất là ta chuyển 
nó về hệ tọa độ camera như H.2. Giả sử rằng 
wM là một điểm bất kì trong hệ tọa độ w bất 
kì, cM là một điểm trong hệ tọa độ camera. 
Khi đó, wM có thể chuyển về cM bằng một 
phép quay R và một phép tịnh tiến t. Ta có: 
w cM RM t (3) 
với ma trận R và vector t mô tả hướng, vị 
trí tương đối giữa hệ tọa độ camera và hệ tọa 
độ thực tế. Ma trận R chứa 3 phép quay (theo 
x, y, z) và vector t chứa 3 phép tịnh tiến. 
Những thông số chứa trong R và t được gọi 
là những thông số ngoại của camera. 
Hình 2 Chuyển đổi tọa độ pixel về tọa độ thực 
Như vậy từ công thức (2) và (3), ta có 
thể viết lại: 
 wp cZm KM K RM t (4) 
Từ công thức (4), ta có công thức tổng 
quát cuối cùng là: 
 1 1w wRp pZm K RM t K Zm t M (5) 
Với: 
-  1 Tpm x y : tọa độ pixel 
-  w
TM X Y Z 
 : tọa độ thực 
3. Giải Thuật A* 
Trong khoa học máy tính, A* là một 
thuật toán tìm kiếm trong đồ thị. Thuật toán 
này tìm một đường đi từ một nút khởi đầu 
đến một nút đích cho trước [10]. Thuật toán 
này sử dụng một đánh giá “heuristic” để xếp 
loại từng nút theo ước lượng về quãng đường 
tốt nhất đi qua nút đó. Thuật toán sẽ duyệt 
các nút theo thứ tự của đánh giá heuristic 
này. Heuristic có thể hiểu đơn giản là một 
giải thuật để giải các thuật toán, nó là một 
phương pháp được tạo ra dựa trên những 
kinh nghiệm, trực giác của con người. Mục 
đích của thuật giải Heuristic là giải các thuật 
toán phức tạp nhanh chóng, đưa ra được kết 
quả so với giải thuật thông thường, vì vậy chi 
phí thấp hơn. Thuật toán A* ứng dụng hàm 
heuristic để tìm kiếm đường đi. Từ trạng thái 
hiện tại, A* xây dựng tất cả các đường đi có 
thể đi dùng hàm ước lượng khoảng cách ( 
hàm Heuristic) để đánh giá đường đi tốt nhất 
có thể đi. Thứ tự ưu tiên gán cho một đường 
đi x được quyết định bởi hàm f(x) = g(x) + 
h(x) . Trong đó g(x): là chi phí đường đi tính 
từ điểm xuất phát tới điểm x hiện tại, h(x) là 
khoảng cách từ điểm x tới điểm đích. 
Hàm f(x) có giá trị càng thấp thì độ ưu 
tiên của x càng cao. Để hiểu rõ hơn về các 
giá trị trong thuật toán, ta cần làm rõ hai định 
nghĩa sau: Khoảng cách giữa 2 điểm A, B là 
độ dài đoạn thẳng ngắn nhất nối 2 điểm đó 
lại với nhau (có thể cắt ngang qua vật cản). 
Chi phí giữa 2 điểm A, B là độ dài đường đi 
cần thực hiện để đi từ điểm A đến điểm B 
(không được cắt ngang qua vật cản). 
Giả thiết môi trường hoạt động robot 
được chia thành mạng lưới các ô vuông với 
ma trận 4x4 như hình 3. 
Hình 3. Áp dụng thuật toán A* trên ma trận 4x4. 
42 
Journal of Transportation Science and Technology, Vol 27+28, May 2018 
Vị trí xuất phát có tọa độ là A(1,1), vị trí 
đích là B(1,4), các chấm tròn đen là vật cản. 
Trong trường hợp này bước tiếp theo phải đi 
từ A chỉ có thể là node (2,1). Khi đi tới node 
(4,2) sẽ có 2 node liền kề có thể được lựa 
chọn làm node kế tiếp. Quá trình lựa chọn 
dựa vào hàm f(x) được tính toán như phương 
trình (1) trong đó: 
g(4,3) là chi phí từ node A(1,1) → (2,1) 
→ (3,1) → (4,2) → (4,3), h(4,3) là khoảng 
cách giữa node (4,3) và node B 
g(3,3) là chi phí từ node A(1,1) → (2,1) 
→ (3,1) → (4,2) → (3,3), h(3,3) là khoảng 
cách giữa node (3,3) và node B 
4,3 4,3 4,3 4.414 3.162 7.576
3,3 3,3 3,3 4.828 2.236 7.064
f g h
f g h
Theo kết quả tính toán được node (3,3) 
có chi phí f(x) nhỏ hơn, vì vậy sẽ chọn node 
(3,3) là node đi tiếp theo. Thuật toán sẽ tiếp 
tục xét các node liền kề với node (3,3) cho 
đến khi đạt được node mong muốn 
4. Thiết kế bộ điều khiển 
Mô hình động học của robot đã được 
giới thiệu nhiều trong các nghiên cứu. Mô 
hình này bao gồm các điểm quan trọng. Điểm 
R: điểm tham chiếu cho robot; Điểm M: 
trung điểm của hai bánh chủ động; Điểm C: 
Điểm tracking của robot. Mô hình này được 
thể hiện trên H.4 
Hình 4. Mô hình hóa Robot. 
Phương trình động học của robot tại M 
cos 0
sin 0
0 1
x
v
y




 (5) 
Phương trình động học của robot tại 
điểm tracking C 
cos sin
sin cos
c c
c c
c c
x x d x x d
y y d y y d
  
  
 
 (6) 
Với d là khoảng cách từ tâm xe đến điểm 
bám line C. 
Phương trình động học tại điểm tham 
chiếu R 
.cos
.sin
R R R
R R R
R R
x v
y v
 



 (7) 
Trong đó vR là vận tốc mong muốn của 
robot tại điểm tham chiếu. 
Bộ điều khiển được thiết kế cho điểm 
tracking C đến vị trí điểm tham chiếu R với 
vận tốc mong muốn 𝑣𝑣𝑅𝑅. Để làm được điều 
đó, cần phải xác định được các sai số bám 
đường sau: 
1
2
3
cos sin 0
sin cos 0
0 0 1
R c
R c
R c
x xe
e y y
e
 (8) 
Sau khi có được các sai số e1, e2, e3, bộ 
điều khiển tracking trong [11] cho phép xác 
định giá trị vận tốc v và ω cần thiết để điểm 
tracking có thể bám theo điểm tham chiếu: 
3 1 1
2 2 2 3 3
cos
sin
R
R
v v e k e
k v e k e 
 (9) 
5. Thực Nghiệm 
Tiến hành thực nghiệm trong nhiều 
trường hợp khác nhau để đưa ra những đánh 
giá về quá trình tìm đường đi, và khả năng 
bám theo đường đi tìm được của robot. Việc 
đánh giá về khả năng bám đường của robot 
sẽ được đánh giá theo các sai số e1, e2, e3 và 
sai lệch tọa độ tracking của robot so với tọa 
độ đường đi tìm được. Không gian dùng để 
thực nghiệm được bố trí như hình 5. Camera 
được gá đặt cố định phía trên mặt phẳng làm 
việc của robot với phạm vi hoạt động là 1200 
x 900 mm. vị trí của điểm đích đến và điểm 
bắt đầu được xác định trước, vị trí vật cản là 
bất kỳ. Cấu trúc điều khiển của robot được 
tóm tắt như H.6 gồm có 1 vi điều khiển trung 
TẠP CHÍ KHOA HỌC CÔNG NGHỆ GIAO THÔNG VẬN TẢI SỐ 27+28 – 05/2018 
43 
tâm nhận lệnh từ máy tính và điều khiển 2 vi 
điều khiển khác điều khiển động cơ. Đầu tiên 
đường đi ngắn nhất từ điểm đầu đến điểm 
đích được tính toán dựa trên giải thuất A*. 
Kết quả của đường đi ngắn nhất được thể 
hiện trong hình 9. 
Hình 5. Không gian hoạt động của robot. 
MCU 
Master
MCU 
slave
Driver Driver
Left 
Motor
Right 
Motor
Encoder Encoder
MCU 
slave
Camera
Laptop
Xử lý ảnh
Frame Bluetooth
I2C
Hình 6. Cấu trúc điều khiển của robot. 
Để áp dụng thuật toán A* tìm đường đi 
cho robot thì vị trí của robot, vật cản và vị trí 
điểm đích cần phải xác định. Vị trí các điểm 
này được xác định theo tâm của các đối 
tượng bằng xử lý ảnh. Để đưa kích thước vật 
cản vào trong bài toán thì vật cản sẽ được mở 
rộng từ tọa độ tâm theo mọi hướng với bán 
kính 6cm. 
Trên thực tế, robot cũng có kích thước 
nhất định, để đưa kích thước robot vào bài 
toán nhưng không làm bài toán trở nên phức 
tạp, robot sẽ được đơn giản hóa kích thước 
bằng cách lập một đường biên bao xung 
quanh robot. Đường biên là đường tròn, có 
tâm là trung điểm hai bánh chủ động, bán 
kính là khoảng cách từ tâm đến biên ngoài 
cùng của một bánh. Bán kính này có thể 
được coi như là khoảng cách an toàn của 
robot đối với vật cản. Vì vậy, mỗi vật cản sẽ 
được mở rộng thêm từ biên của chúng với 
bán kính bằng bán kính này. Vật cản được sử 
dụng trong đề tài là khối tròn, bán kính 6cm, 
có màu đỏ đồng nhất và mô hình robot kiểm 
nghiệm có bán kính từ trung điểm hai bánh 
dẫn động đến biên ngoài cùng của bánh xe là 
10.5 cm 
Hình 7. Kết quả tìm đường đi ngắn nhất. 
Sau khi đã có đường đi ngắn nhất, robot sẽ 
dựa trên đường đi đó mà tiến đến điểm đích, 
kết quả của quá trình điều khiển này được thể 
hiện trong H.8 với đường màu đỏ là đường 
hoạch định, và đường màu xanh chính là tọa 
độ của robot. 
Hình 8 Kết quả bám theo đường line. 
Hình 9. Sai số e1. 
44 
Journal of Transportation Science and Technology, Vol 27+28, May 2018 
Hình 10. Sai số e2 
Hình 11. Sai số e3 
Dựa vào H.8-H.11, có thể nhận ra rằng 
sự kết hợp giữa camera và giải thuật điều 
khiển đã phát huy tác dụng, giup cho robot 
bám theo đường line như mục tiêu đã đề ra 
với sai số nhỏ. 
6. Kết Luận 
Nghiên cứu đã trình bày một phương án 
hiệu quả giúp cho robot có thể hoạt động 
trong một vùng không gian hẹp như xưởng 
hay trong nhà. Bằng cách kết hợp giữa xử lý 
ảnh và điều khiển, đường đi của robot trở nên 
linh hoạt hơn không còn bị cứng nhắc như dò 
line truyền thống. Trong quá trình robot bám 
đường đi, nhờ camera robot có thể nhận biết 
được vật thể xung quanh và tránh được các 
vật cản. Ngoài ra người dùng có thể tự định 
nghĩa đường line cho robot thông qua giao 
diện camera giúp cho việc thay đổi đường đi 
không còn khó khăn nữa 
Tài liệu tham khảo 
[1] U. Farooq, M. Amar, M. U. Asad, G. Abbas, và A. 
Hanif, “Fuzzy logic Reasoning System for Line 
Following Robot, “ IACSIT International Journal 
of Engineering and Technology, 6 (2014), pp. 
244-248. 
[2] J. H. Su, C. S. Lee, H. H. Huang, S. H. Chuang 
và C. Y. Lin, “An intelligent line following robot 
project for introductory robot courses, “ World 
Transactions on Engineering and Technoloy 
Education, 8 (2010), pp. 455-461. 
[3] P.T.L. Hải, T.T. Hùng, N.V. Khanh và L.T. Hiếu, 
“Ứng Dụng Xử Lý Ảnh Kỹ Thuật Số Trong Việc 
Theo Dõi Sự Chuyển Động Của Các Robot Di 
Động “, Tạp chí Khoa học Trường Đại học Cần 
Thơ, 2015, pp. 39-47. 
[4] H.Đ Chiến, N.P. Cường, Đ.Đ. Duy, “Ứng Dụng 
Xử Lý Ảnh Nhận Dạng Vật Thể Theo Màu Sắc 
Cho Robot Tay Máy”, Khoa Điện –Điện tử Đại 
học Bà Rịa Vũng Tàu, 2015 
[5] N. V. Khanh, N. T. Tien, B. T. Hieu, H. V. 
Hung, “Study on Tracking Control of Welding 
Mobile Robot Using Camera - Straight Welding 
Path Application, “Proceeding of The 
International Symposium on Electrical and 
Electronics Engineering (ISEE 2005), HCMUT, 
2005, pp. 102-107. 
[6] A. Cherubini, F. Chaumetter, G. Oriolo, “An 
image-based visual servoing scheme for 
following paths with nonholonomic mobile 
robots”, 10th International Conference on 
Control, Automation, Robotics and Vision, 2008, 
pp. 220-225 
[7] Z. Miljkovic, P. Milica, B. Babic, Integration of 
Proces Planning, Scheduling, and Mobile Robot 
Navigation Based on TRIZ and Multi-Agent 
Methodology,” FME Transactions 41, 2 (2013), 
pp. 97-106. 
[8] S. Kloder and S. Hutchinson, “Path Planning for 
Permutation-Invariant MultiRobot Formations,” 
IEEE Trans. on Robotics, 22 (2006), pp. 
650-665. 
[9] A. Cherbuini, F. chaumette and G.Oriolo, “A 
position-based visual servoing scheme for 
following paths with nonholonomic mobile 
robots”,IEEE International Conference on 
Intelligent Robots and Systems, 2008, 
pp.101-105. 
[10] C..Zeng (2012). GA-based Global Path Planning 
for Mobile Robot Employing A* Algorithm. 
Journal of Computer, Vol.7, No.2, 470-474. 
[11] L.H.Danh, L.T.D.Hieu, N.T.Tien.et al..Nghiên 
cứu thiết kế bộ điều khiển bám line cho AGV 
qua giao lộ. Hội nghị toàn quốc lần thứ 7 về Cơ 
điện tử - VCM – 2014 , pp 597-601 
 Ngày nhận bài: 6/3/2018 
 Ngày chuyển phản biện: 9/3/2018 
 Ngày hoàn thành sửa bài: 29/3/2018 
 Ngày chấp nhận đăng: 5/4/2018