ĐẠI HỌC QUỐC GIA THÀNH PHỐ HỒ CHÍ MINH
TRƢỜNG ĐẠI HỌC BÁCH KHOA
KHOA ĐIỆN - ĐIỆN TỬ
BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG

LUẬN VĂN TỐT NGHIỆP

TRIỂN KHAI THUẬT TOÁN ĐỊNH HƢỚNG TRÊN ROS
CHO ROBOT TỰ HÀNH TRONG NHÀ GVHD : ThS. Huỳnh Văn Kiểm
STUDENT : Nguyễn Phạm Nhất Thiên Minh 40901569
Hoàng Việt Hà 40900700
Thành phố Hồ Chí Minh, tháng 12, 2013

BỘ GIÁO DỤC VÀ ĐÀO TẠO CỘNG HÕA XÃ HỘI CHỦ NGHĨA VIỆT NAM
TRƢỜNG ĐẠI HỌC BÁCH KHOA Độc lập – Tự do – Hạnh phúc
Thành phố Hồ Chí Minh o0o
KHOA: ĐIỆN-ĐIỆN TỬ NHIỆM VỤ LUẬN VĂN TỐT NGHIỆP
BỘ MÔN: ĐIỀU KHIỂN TỰ ĐỘNG
HỌ VÀ TÊN: NGUYỄN PHẠM NHẤT THIÊN MINH MSSV: 40901569
HOÀNG VIỆT HÀ MSSV: 40900700
NGÀNH: ĐIỀU KHIỂN TỰ ĐỘNG LỚP: DD09KSTD
Đầu đề luận văn:
Triển khai thuật toán định hƣớng trên ROS cho robot tự hành trong nhà
1- Nhiệm vụ (yêu cầu về nội dung và số liệu ban đầu)

Chuyên ngành: Điều khiển tự động
2- Đề tài luận văn:
Triển khai thuật toán định hƣớng trên ROS cho robot tự hành trong nhà
3- Họ tên ngƣời hƣớng dẫn: Th.S. HUỲNH VĂN KIỂM
4- Tổng quát về bản thuyết minh:
Số trang: Số chƣơng:
Số bảng số liệu: Số hình vẽ:
Số tài liệu tham khảo: Phần mềm tính toán:
Sản phẩm:
5- Tổng quát về các bản vẽ:
Số bản vẽ: bản A1 bản A2 Khổ khác:
Số bản vẽ tay: Số bản vẽ máy tính:
6- Những ƣu điểm chính của luận văn tốt nghiệp: 7- Những khuyết điểm chính của luận văn tốt nghiệp: 8- Đề nghị: Đƣợc bảo vệ : □ Bổ sung thêm để bảo vệ: □ Không đƣợc bảo vệ: □
9- Ba câu hỏi sinh viên phải trả lời trƣớc hội đồng:
a.
b.
c.
10- Đánh giá chung (bằng chữ: giỏi, khá, trung bình): Điểm: /10
Ký tên (ghi rõ họ tên)

BỘ GIÁO DỤC VÀ ĐÀO TẠO CỘNG HÕA XÃ HỘI CHỦ NGHĨA VIỆT NAM
TRƢỜNG ĐẠI HỌC BÁCH KHOA Độc lập – Tự do – Hạnh phúc
BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG o0o
Ngày…… tháng …… năm……….


NHẬN XÉT CỦA GIÁO VIÊN HƢỚNG DẪN

NHẬN XÉT CỦA GIÁO VIÊN PHẢN BIỆN
những ứng dụng thực tiễn thú vị. Ngoài ra, còn có bạn bè luôn sẵn sàng đồng hành giúp đỡ chúng
tôi khi khó khăn về học tập và cuộc sống, đặc biệt là các thành viên và ban chủ nhiệm câu lạc bộ
nghiên cứu khoa học Pay It Forward. Nhân đây, xin gửi đến thầy cô và các bạn lời cảm ơn chân
thành nhất.
Cuối cùng, chúng tôi xin cảm ơn Cha Mẹ cùng gia đình đã luôn đứng về phía chúng tôi,
truyền dạy, động viên và tạo điều kiện cho chúng tôi trong suốt quá trình học tập và nghiên cứu
vừa qua.
Tp. Hồ Chí Minh, ngày 18 tháng 12 năm 2013

Nguyễn Phạm Nhất Thiên Minh
Hoàng Việt Hà
ii

TÓM TẮT LUẬN VĂN
Hệ điều hành robot – Robot Operation System là một nền tảng mã nguồn mở dành cho việc
thiết kế phần mềm robot. Tiền thân của ROS đƣợc phát triển đầu tiên bởi Phòng thí nghiệm Trí
tuệ Nhân tạo của Đại học Stanford vào năm 2007. Kể từ năm 2008, ROS đƣợc phát triển và mở
rộng tại phòng nghiên cứu Willow Garage cùng một mạng lƣới liên kết nhiều viện nghiên cứu lớn
trên thế giới. Willow Garage hiện là đơn vị duy trì các thƣ viện mã nguồn mở rất phổ biến là
OpenCV và Point Cloud Library, chính vì vậy, ROS đƣợc tích hợp đầy đủ các thƣ viện trên cùng
với vô số các công cụ và chƣơng trình ứng dụng robot đƣợc đóng góp xây dựng bởi cộng đồng
đông đảo các nhà nghiên cứu từ khắp nơi trên thế giới, giúp cho việc thử nghiệm, chia sẻ các thuật
toán trở nên dễ dàng hơn. Trong thời gian ngắn kể từ khi xuất hiện, ROS đã phát triển nhanh
chóng và trở thành một công cụ phổ biến, chuẩn mực đƣợc sử dụng rộng rãi bởi các đơn vị nghiên
cứu và chế tạo robot.
Định hướng cho robot di động là một vấn đề có nhiều ứng dụng to lớn và liên quan tới
nhiều lĩnh vực. Đây cũng là một hƣớng ứng dụng trọng tâm của ROS. Trong phạm vi luận văn,
bài toán định hƣớng đƣợc nghiên cứu trên một robot tự hành trong nhà ứng dụng các lý thuyết và
công cụ tiên tiến đƣợc phát triển trên nền tảng ROS. Bằng việc ứng dụng thành công ROS, robot
đã có khả năng tự định vị trong một môi trƣờng gồm các đặc điểm biết trƣớc đồng thời phản ứng

CHƢƠNG IV – LẬP TRÌNH TÍCH HỢP NAVIGATION STACK TRÊN MÔ HÌNH
ROBOT 56
4.1. Serial Server cho Mobile Base 56
4.2. Transform Broadcaster. 60
4.3. Dựng Static Map 63
CHƢƠNG V – THỰC NGHIỆM VÀ ĐÁNH GIÁ 67
iv

5.1. Bố trí và quan sát: 67
5.2. Đánh giá khả năng định hƣớng di động 76
5.3. Kết luận 76
5.4. Hƣớng phát triển 77
Tài liệu tham khảo 78
Phụ lục 1 – HƢỚNG DẪN LẬP TRÌNH ROS TRÊN IDE ECLIPSE. 79
Phụ lục 2 – TẠO LAUNCH FILE CHO NODE DEPTHIMAGE_TO_LASERSCAN 84 v

DANH MỤC HÌNH ẢNH
Hình 2.1: Mô hình robot lái bằng hiệu vận tốc 5
Hình 2.2: Minh họa về quá trình định vị theo phƣơng pháp xác suất 10
Hình 2.3: Minh họa mô hình xác suất của phƣơng trình động học 11
Hình 2.4: Mô hình odometry mô tả chuyển động của robot trong khoảng thời gian (t-1,t] 12
Hình 2.5: Mô hình theo odometry với các thiết lập thông số nhiễu khác nhau. 13
Hình 2.6: Phân bố xác suất của biến ngẫu nhiên a, phƣơng sai b, kì vọng bằng 0 14
Hình 2.7: Ví dụ lấy mẫu ngẫu nhiên theo mô hình odometry. 16
Hình 2.8: Mô hình xác suất của phép đo từ cảm biến khoảng cách chùm tia 16
Hình 2.9: Minh họa trƣờng hợp kidnapping 18


Hình 3.29: Picture robot 51
Hình 3.30: Giải thuật điều khiển trên vi điều khiển 51
Hình 3.31: Sơ đồ chức năng các ngoại vi trên MCU 52
Hình 3.32: Sơ đồ khối của Timer điều khiển nâng cao của STM32F4 54
Hình 3.33: Giản đồ xung giải mã quadrature encoder của Encoder Interface trên STM32F4 55
Hình 3.34: Khối XOR ngõ vào giúp tăng đôi tần số encoder và các ngõ ra PWM giúp điều khiển
đảo chiều động cơ 55

Hình 4.1: Ví dụ về hai hệ tọa độ trên robot 60
Hình 4.2: Giá trị của điểm trong hệ tọa độ cảm biến, cây tọa độ đơn giản và giá trị chuyển đổi
trong hệ tọa độ của mobile base. 61
Hình 4.3: Sơ đồ cây các hệ tọa độ chính của robot tự hành 61
Hình 4.4: Laser scan 2D trích từ point cloud 3D 63
Hình 4.5: Một số bản đồ 2D đƣợc dựng bằng slam_gmapping 2D 64
Hình 4.6: Static map đƣợc đo vẽ 64
Hình 4.7: 3D map và hình chiếu 2D 66

Hình 5.1: Khu vực thử nghiệm 67
Hình 5.2: Khu vực khảo sát khả năng lập lộ trình và tránh vật cản của robot. 68
Hình 5.3: Chọn tọa độ đích cho robot 69
Hình 5.4: robot di chuyển tránh vật cản 69
Hình 5.5: Quá trình mapping và path-planning của robot 71
Hình 5.6: Robot ở tọa độ đích 72
Hình 5.7: Quá trình di chuyển của robot 74
Hình 5.8: Lộ trình ƣớc lƣợng của robot khi đi đến một điểm và trở về 75

vii

DANH MỤC BẢNG
Bảng 2.1: Giải thuật motion_model_odommetry 13

SLAM) là một bài toán đặc biệt luôn đi kèm với bài toán định hƣớng. SLAM đƣợc định nghĩa là
việc xây dựng hoặc cập nhật liên tục một bản đồ, đồng thời định vị vị trí của robot trong bản đồ
đó. Trong SLAM, hai tác vụ này không thể tách rời, khi biết chính xác vị trí của mình thì robot
mới có thể đƣa các cập nhật mới vào bản đồ một cách hợp lý. Đồng thời, khi bản đồ đƣợc cập
nhật chính xác, robot mới có thể đánh giá đƣợc sự dịch chuyển của mình so với môi trƣờng.
CHƯƠNG 1: TỔNG QUAN VỀ ĐỊNH HƯỚNG CHO ROBOT DI ĐỘNG
2
Trong bài toán định hƣớng, robot sẽ phải có khả năng thực hiện SLAM ở nhiều mức độ. Nếu ta
cung cấp cho robot một bản đồ đƣợc đo đạc chính xác, robot vẫn phải sử dụng SLAM tại những
vị trí có vật cản chƣa biết trƣớc hoặc những vị trí mà giá trị cảm biến trả về không đủ xác định vị
trí của robot, ví dụ nhƣ tại một hành lang dài có bối cảnh không thay đổi. Các vấn đề lý thuyết
định hƣớng và SLAM sẽ đƣợc đề cập sâu hơn trong chƣơng sau.
1.2. Mục tiêu luận văn và phƣơng pháp thực hiện
Mục tiêu luận văn:
Thiết kế một robot di động, sử dụng cảm biến khoảng cách Kinect và các cảm biến khác
cùng với thuật toán định hƣớng trên ROS để xây dựng bản đồ có đặc tính bằng phẳng và tự di
chuyển trong bản đồ đó tới một vị trí đặt trƣớc, đồng thời xử lý tránh vật cản chƣa biết. Với mục
đích phát triển sâu xa là trở thành một robot cá nhân có thể thực hiện nhiều tác vụ nhằm giảm tải
công việc cho con ngƣời nhƣ quét dọn, hút bụi, chuyển đồ,… trong phạm vi nhà ở hoặc trong các
văn phòng, bệnh viện
Phƣơng pháp thực hiện
Triển khai thuật toán định hƣớng trên ROS và các ứng dụng ROS hỗ trợ việc định hƣớng
cho robot tự hành trong nhà với sự hỗ trợ thu thập dữ liệu từ Kinect (thu thập dữ liệu và vẽ bản đồ
cho robot định hƣớng cho) và các cảm biến khác nhƣ encoder, cảm biến gia tốc góc (IMU)… để
robot tự thực hiện đƣợc các chức năng nhƣ mong đợi.
1.3. Sơ lƣợc về nội dung luận văn
Nội dung luận văn bao gồm 5 chƣơng và 1 phụ lục
 Chƣơng 1: Tổng quan về định hướng cho robot di động: Giới thiệu sơ lƣợc về bài
toán định hƣớng cho robot di động, mục tiêu luận văn và phƣơng pháp thực hiện.
 Chƣơng 2: Cơ sở lý thuyết định hướng robot: Lý thuyết về ƣớc lƣợng tọa độ cho

ƣớc lƣợng tọa độ thích hợp có lân cận trên bản đồ phù hợp với giá trị đo này. Trong một số
trƣờng hợp khi việc ƣớc lƣợng cho ra giá trị không hội tụ, nhƣ khi robot di chuyển trong một khu
vực rộng hơn tầm đo của cảm biến khoảng cách, navigation stack sẽ sử dụng tọa độ ƣớc lƣợng từ
các phƣơng pháp khác để định hƣớng cho robot tiếp tục di chuyển cho đến khi gặp đƣợc một lân
cận quen thuộc. Phần dƣới đây trình bày phƣơng pháp ƣớc lƣợng tọa độ từ encoder đƣợc sử dụng
trong luận văn và lý thuyết cùa phương pháp định vị Monte Carlo thích nghi.
Ƣớc lƣợng tọa độ của robot từ giá trị hồi tiếp từ encoder.
Robot tự hành trong luận văn đƣợc thiết kế theo mô hình robot lái bằng hiệu tốc độ
(differential steered drive). Giả sử robot chuyển động không trƣợt, ta sẽ ƣớc lƣợng tọa độ của
robot dựa vào giá trị hồi tiếp từ encoder. Trong hệ tọa độ , robot chuyển động với vận tốc tịnh
tiến là  và vận tốc xoay ω. Trong khoảng thời gian rất bé ,  và ω không đổi, chuyển động
của robot là chuyển động tròn quanh tâm quay tức thời  với bán kính
r
.
CHƯƠNG 2: CƠ SỞ LÝ THUYẾT ĐỊNH HƯỚNG ROBOT
5
Tại đầu khoảng thời gian , tọa độ tâm 

của robot trong hệ tọa độ  gồm bộ ba 



,
tại điểm này ta có tọa độ của tâm :


 


 

 












 




 


  

 

 







 


 







 
Trong đó:


vận tốc dài quanh trục bánh phải.


vận tốc dài quanh trục bánh trái.
 khoảng cách giữa 2 bánh xe.
 bán kính quay tức thời.

vận tốc quay của robot.
Lấy phƣơng trình



trừ phƣơng trình  vế theo vế ta đƣợc quan hệ giữa vận tốc của





 



Ta suy ra đƣợc bán kính quay tức thời  của robot từ các phƣơng trình  và 
















CHƯƠNG 2: CƠ SỞ LÝ THUYẾT ĐỊNH HƯỚNG ROBOT
7
Thay 


















 





  










 và 

 vào vị trí của 

và 


trong phƣơng trình .
Ngoài ra ta còn có thể xấp xỉ phƣơng trình  và giảm đáng kể khối lƣợng tính toán nếu
việc cập nhật tọa độ đƣợc thực hiện trên hệ nhúng.
Ta có:








 











  


 




 













Nếu nhƣ chuyển động xoay của robot rất nhỏ, ta có thể xấp xỉ các phƣơng trình trên trở
thành dạng đơn giản.










CHƯƠNG 2: CƠ SỞ LÝ THUYẾT ĐỊNH HƯỚNG ROBOT
8
Phƣơng trình xấp xỉ trên cũng là  của phƣơng trình gốc khi 

 

, ứng với việc
robot chỉ di chuyển tịnh tiến theo phƣơng có hệ số góc .
Để điều khiển robot, ta có có quan hệ giữa 

, 

với  và  nhƣ sau:





 





 
2.2. Chuyển động học xác suất và phƣơng pháp định vị Monte Carlo thích nghi

cao cũng thấp dần nhƣ đồ thị thứ tƣ trong hình 2.2. Tiếp theo khi cảm biến cho ta một phép đo
mới của z. Cảm tính của robot sẽ có phân bố lớn xung quanh vị trí cánh cửa thứ hai và phân bố ở
các khu vực khác đều giảm mạnh nhƣ trong đồ thị thứ sáu. Từ sự chắc chắn cao tại cánh cửa hiện
tại, trong khoảng từ đó đến cánh cửa kế tiếp, cảm tính của robot dựa vào odometry vẫn cho vị trí
thực tế có giá trị cao mặc dù có giảm và sẽ đƣợc bổ sung lại khi robot đến đƣợc cánh cửa thứ 3.
CHƯƠNG 2: CƠ SỞ LÝ THUYẾT ĐỊNH HƯỚNG ROBOT
10

Hình 2.2: Minh họa về quá trình định vị theo phƣơng pháp xác suất, phân bố
xác suất đƣợc biễu diễn ở dạng rời rạc
CHƯƠNG 2: CƠ SỞ LÝ THUYẾT ĐỊNH HƯỚNG ROBOT
11
Ta thử khảo sát trƣờng hợp khi chƣa có thông tin về bản đồ , tọa độ ƣớc lƣợng của robot
có dạng một phân bố xác suất hậu nghiệm - posterior probability. Xác suất hậu nghiệm của một
biến ngẫu nhiên là xác suất có điều kiện mà nó nhận đƣợc khi một bằng chứng có liên quan đƣợc
xét đến. Phân bố xác suất hậu nghiệm của một biến ngẫu nhiên khi cho trƣớc giá trị của một biến
khác có thể đƣợc tính theo Định lý Bayes. Ta biểu diễn phƣơng trình động học xác suất của robot
dƣới dạng một hàm phân bố nhƣ sau:










.
Với 

CHƯƠNG 2: CƠ SỞ LÝ THUYẾT ĐỊNH HƯỚNG ROBOT
12
Với mô hình odometry, ta sử dụng các số liệu chuyển động tương đối của robot. Trong
khoảng thời gian   robot chuyển từ trạng thái 

sang 

. Thông qua odometry ta nhận
đƣợc sự di chuyển của robot từ 



 sang 








. Trên hình 2.4, đƣờng
thẳng màu đen là thể hiện cho kết quả đo lƣờng odometry trong hệ trục tọa độ gắn liền với robot
(chƣa biết toạ độ tƣơng đối của hệ trục tọa gắn với robot so với hệ trục tọa độ cố định). Yếu tố
quan trọng nhất trong bài toán ƣớc lƣợng trạng thái là xác định sự sai biệt giữa 2 vị trí 

và 


(đƣợc gọi là “vi sai” vị trí), giá trị sai biệt này dùng để ƣớc lƣợng cho khoảng cách thực 


  


và trạng thái tiếp theo











. Chuyển động của robot đƣợc diễn tả bởi 

với 






. Chuyển động


đƣợc tách 3 bƣớc liên tiếp: 1 chuyển động quay, 1 chuyển động thẳng và 1 chuyển động quay
nữa. Hình 2.5 miêu tả sự khai triển này: chuyển động quay đầu tiên đƣợc gọi là 