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à

ĐẠ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)

2- Ngày giao nhiệm vụ luận án:
3- Ngày hoàn thành nhiệm vụ:
4- Họ tên ngƣời hƣớng dẫn Phần hƣớng dẫn

Nội dung và yêu cầu LVTN đã đƣợc thông qua bộ môn
Ngày ……tháng 12 năm 2013
Chủ nhiệm bộ môn Ngƣời hƣớng dẫn chính
(Ký và ghi rõ họ tên) (Ký và ghi rõ họ tên)




PHẦN DÀNH CHO KHOA, BỘ MÔN
Ngƣời duyệt (Chấm sơ bộ):
Đơn vị:
Ngày bảo vệ:
Điểm tổng kết:
Nơi lƣu trữ luận vă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……….
PHIẾU CHẤM ĐIỂM BẢO VỆ LVTN
(Dành cho ngƣời hƣớng dẫn)
1- Họ và tên sinh viên: NGUYỄN PHẠM NHẤT THIÊN MINH MSSV: 40901569
HOÀNG VIỆT HÀ MSSV: 40900700

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……….

PHIẾU CHẤM ĐIỂM BẢO VỆ LVTN
(Dành cho ngƣời phản biện)
11- Họ và tên sinh viên: NGUYỄN PHẠM NHẤT THIÊN MINH MSSV: 40901569
HOÀNG VIỆT HÀ MSSV: 40900700
Chuyên ngành: Điều khiển tự động
12- Đề 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à

13- Họ tên ngƣời phản biện:
14- 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:
15- 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:
16- Những ƣu điểm chính của luận văn tốt nghiệp:


17- Những khuyết điểm chính của luận văn tốt nghiệp:


18- Đề nghị: Đƣợc bảo vệ : □ Bổ sung thêm để bảo vệ: □ Không đƣợc bảo vệ: □
19- Ba câu hỏi sinh viên phải trả lời trƣớc hội đồng:
a.
b.
c.
20- Đánh giá chung (bằng chữ: giỏi, khá, trung bình): Điểm: /10
Ký tên (ghi rõ họ tên)

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

i

LỜI CẢM ƠN
Lời đầu tiên chúng tôi xin gửi tới thầy Thạc sĩ Huỳnh Văn Kiểm lời biết ơn sâu sắc nhất vì
những chỉ dẫn, đánh giá, định hƣớng và những chia sẻ kinh nghiệm bổ ích của thầy từ đồ án tới
luận văn, tạo điều kiện để chúng tôi có thể hoàn thành tốt luận văn này.
Bên cạnh đó, phải kể đến sự chỉ dạy tận tình, tâm huyết của các thầy cô đang giảng dạy tại
bộ môn điều khiển tự động, tại khoa Điện-Điện tử, trƣờng đại học Bách Khoa thành phố Hồ Chí
Minh. Nhờ các thầy cô, chúng tôi mới đƣợc tiếp cận với những kiến thức mới hết sức bổ ích và

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

thích hợp với các vật cản chƣa biết để tìm đƣờng đi đến một vị trí đích đã chọn. Từ tính năng cơ
bản này, trong tƣơng lai, ta có thể khai thác sâu hơn các tính năng của ROS, tích hợp các kỹ thuật
định vị tiên tiến giúp cho robot có thể di chuyển chính xác hơn, phạm vi hoạt động rộng nhằm
phục vụ cho các ứng dụng trong đời sống.
iii

MỤC LỤC Trang
LỜI CẢM ƠN i
TÓM TẮT LUẬN VĂN ii
MỤC LỤC iii
DANH MỤC HÌNH ẢNH v
DANH MỤC BẢNG vii
CHƢƠNG I – TỔNG QUAN VỀ ĐỊNH HƢỚNG CHO ROBOT DI ĐỘNG 1
1.1. Định hƣớng cho robot tự hành. 1
1.2. Mục tiêu luận văn và phƣơng pháp thực hiện 2
1.3. Sơ lƣợc về nội dung luận văn 2
CHƢƠNG II – CƠ SỞ LÝ THUYẾT ĐỊNH HƢỚNG ROBOT 4
2.1. Ƣớc lƣợng tọa độ từ encoder 4
2.2. Chuyển động học xác suất và phƣơng pháp định vị Monte Carlo thích nghi 8
CHƢƠNG III – CÁC THÀNH PHẦN CHÍNH CỦA ROBOT TỰ HÀNH TRONG NHÀ 20
3.1. Hệ điều hành robot – ROS 20
3.1.1. Tổng quan về hệ điều hành robot- Robot Operating System 20
3.1.2. Cấu Trúc ROS 23
3.1.3. ROS Navigation Stack 36
3.2. Camera khoảng cách Kinect 42
3.2.1. Công nghệ và đặc tính 42
3.2.2. Package driver và điều khiển cho Kinect 46
3.3. Mobile base và Điều khiển 48
3.3.1. Thiết kế mô hình di động 48
3.3.2. Thiết kế chƣơng trình nhúng 52

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.1: Các robot ứng dụng thành công ROS PR2, Kobuki, Jaguar, Turtlebot 21
Hình 3.2: Cộng đồng các trung tâm tham gia đóng góp phát triển cho ROS trên thế giới 21
Hình 3.3: So sánh khối lƣợng công việc phải làm khi dùng và không dùng ROS 22
Hình 3.4: Mối quan hệ giữa Stack và các package 24
Hình 3.5: Ví dụ về quan hệ giữa Stack, Package và các file mô tả theo dạng thƣ mục 24
Hình 3.6: Mô tả cơ chế quản lý parameter trên Master. 26
Hình 3.7: Mô tả hoạt động của service 27
Hình 3.8: Mô hình giao tiếp cơ bản trong ROS 28
Hình 3.9: ROS Repository vaa repository trong toàn tài nguyên ROS 28
Hình 3.10: Các hệ tọa độ gắn với các phần tử chuyển động trên robot 29
Hình 3.11: Các hệ tọa độ của robot và chuyển động trong không gian. 30
Hình 3.12: Giao tiếp giữa action client và server 30
Hình 3.13: Mô hình robot đƣợc mô tả bằng URDF 32
Hình 3.14: Remapping topic 34
Hình 3.15: Computation Graph của Navigation Stack 36
Hình 3.16: Costmap function 39
Hình 3.17: Recovery Behavior của move_base 42
Hình 3.18: Kinect 42
Hình 3.19: Những thành phần chính của Kinect 43
Hình 3.20: Vùng nhìn thấy của camera khoảng cách trên Kinect. 44
Hình 3.21: Hình ảnh thực tế từ Kinect trên mobile base 45
Hình 3.22: Khoảng cách từ tâm mobile base tới vật cản 45
Hình 3.23: Kinect adaptor, connector và supply cable 46
Hình 3.24: Ứng dụng của openni_launch trong ccny_rgbd 47
Hình 3.25: Logo của point cloud 48
Hình 3.26: Sơ đồ chi tiết mạch điều khiển mobile base 49
vi

Hình 3.27: Thiết khung robot 50
Hình 3.28: Động cơ có hộp số planet 50

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

Bảng 2.2: Giải thuật prob_normal_distribution và giải thuật prob_triangular_distribution 14
Bảng 2.3: Giải thuật sample_motion_model_odometry 15
Bảng 2.4: Giải thuật sample_normal_distribution và giải thuật sample_triangular_distribution 15
Bảng 2.5: Giải thuật beam_range_finder_model 17
Bảng 2.6: Giải thuật Augm ented_MCL 18
CHƯƠNG 1: TỔNG QUAN VỀ ĐỊNH HƯỚNG CHO ROBOT DI ĐỘNG
1
CHƢƠNG I – TỔNG QUAN VỀ ĐỊNH HƢỚNG CHO ROBOT DI ĐỘNG
1.1. Định hƣớng cho robot tự hành.
Robot tự hành là một robot có khả năng tự di chuyển, tự vận động (có thể lập trình lại) dƣới
sự điều khiển tự động để thực hiện tốt những công việc đƣợc giao. Môi trƣờng hoạt động của
robot có thể là mặt đất, nƣớc, không khí, không gian hay là sự tổ hợp của các môi trƣờng đó. Các
mô hình Robot di động khác nhau đƣợc nghiên cứu với theo môi trƣờng trong đó robot chuyển
động, ví dụ nhƣ xe tự hành trên mặt đất AGV (Autonomous Guided Vehicles), robot tự hành dƣới
nƣớc AUV (Autonomous Underwater Vehicles), robot tự hành trên không UAV (Unmanned Arial
Vehicles) và robot vũ trụ (Space Robot). Những khả năng cần có của một robot tự hành là dự trữ
năng lƣợng đảm bảo cho robot tự hành trong thời gian yêu cầu, khả năng phát hiện và nhận biết
vật cản, tránh va chạm với các vật thể và khả năng xử lý, tính toán các chƣơng trình giải thuật
phức tạp trong thời gian thực để đáp ứng kịp thời với những tác động của môi trƣờng .v.v. Trong
đó khả năng định hƣớng của robot tự hành là vấn đề quan trọng nhất. Khả năng định hƣớng đƣợc
định nghĩa là kết hợp của ba khả năng:
1. Định vị bản thân – Self-localization
2. Lập lộ trình – Path-planning
3. Dựng và diễn dịch bản đồ – Map building and interpretation
Self-localization là khả năng robot nhận biết tọa độ và định hƣớng không gian (orientation)
của mình trong một hệ tọa độ. Path-planning là việc xác định các tọa độ hiện thời và các tọa độ
tiếp theo mà robot cần đạt đƣợc để đi đến tọa độ mục tiêu. Map building là công việc nhận diện
môi trƣờng qua từng phép đo đạc từ cảm biến và kết hợp các phép đo này thành một mô hình
trong hệ tọa độ của robot từ đó làm cơ sở cho việc định vị và lập lộ trình.
Bài toán định vị và lập bản đồ đồng thời (Simultaneous Localization and Mapping –

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

robot, giải bài toán ƣớc lƣợng từ tín hiệu encoder thu về. Sơ lƣợc về mô hình chuyển
động xác suất học của robot và phƣơng pháp định vị Monte Carlo thích nghi.
 Chƣơng 3: Các thành phần chính của robot tự hành trong nhà: Tổng quan về hệ
điều hành robot ROS, camera khoảng cách Kinect. Cấu tạo robot trong luận văn, và
chƣơng trình nhúng điều khiển động cơ trên Kit STM32F4 Discovery.
CHƯƠNG 1: TỔNG QUAN VỀ ĐỊNH HƯỚNG CHO ROBOT DI ĐỘNG
3
 Chƣơng 4: Lập trình tích hợp Navigation Stack trên mô hình robot: Thiết kế, thao
tác các node đặc thù hỗ trợ cho Navigation Stack cho mô hình robot trong đề tài.
 Chƣơng 5: Thực nghiệm và đánh giá: Trình bày các kết quả đạt đƣợc trong quá
trình thực nghiệm, đánh giá sai số và đề ra phƣơng hƣớng cải thiện phát triển.
 Phụ lục 1: Cách tạo 1 package mới trong ROS và sử dụng Môi trƣờng Phát triển
Tích hợp (Intergrated Development Environment – IDE) để lập trình cho ROS.
 Phụ lục 2: Cách tạo file .launch cho depthimage_to_laserscan.
CHƯƠNG 2: CƠ SỞ LÝ THUYẾT ĐỊNH HƯỚNG ROBOT
4
CHƢƠNG II – CƠ SỞ LÝ THUYẾT ĐỊNH HƢỚNG ROBOT
2.1. Ƣớc lƣợng tọa độ từ encoder
Có nhiều phƣơng pháp để ƣớc lƣợng tọa độ của robot. Trong đó, phƣơng pháp đơn giản
nhất là ƣớc lƣợng tọa độ từ giá trị hồi tiếp của encoder và có thể kết hợp thêm cảm biến quán tính
(inertial measurement unit). Ngoài ra ROS còn hỗ trợ việc ước lượng tọa độ bằng thị giác - visual
odometry. Đây là phƣơng pháp xử lý ảnh màu point cloud RGBXYZ để tìm các điểm đặc trƣng
tƣơng ứng trong các khung ảnh và ƣớc lƣợng độ dịch chuyển của robot dựa vào thông số không
gian của các điểm đó. Phƣơng pháp này có độ chính xác phụ thuộc vào môi trƣờng, ví dụ nhƣ một
căn phòng có nhiều vật dụng sẽ tìm đƣợc nhiều điểm đặc trƣng hơn một hành lang, ngoài ra, khối
lƣợng tính toán từ phƣơng pháp này rất lớn.
Trở về với bài toán định hƣớng, để thực hiện việc định vị cho robot, Navigation Stack sử
dụng phương pháp định vị Monte Carlo thích nghi trong package amcl (adaptive Monte Carlo
localization), phƣơng pháp này sử dụng giá trị đo đạc khoảng cách từ cảm biến để xác định môi
trƣờng xung quanh robot trên một mặt phẳng, so sánh giá trị này với một bản đồ 2D cho trƣớc và

ƣớ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 :


 


 



    


   

Hình 2.1: Mô hình robot lái bằng hiệu vận tốc
Sau khoảng thời gian  góc  thay đổi một lƣợng , tọa độ mới của 

sau  là




, sử dụng lại phƣơng trình



và  để tìm quan hệ giữa  và điểm này, ta có quan hệ
giữa tọa độ mới và tọa độ cũ:


 







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










 












 

CHƯƠNG 2: CƠ SỞ LÝ THUYẾT ĐỊNH HƯỚNG ROBOT
6
Ta sẽ tìm quan hệ giữa các đại lƣợng , ,  với giá trị thu đƣợc từ encoder và thay vào
phƣơng trình trên. Sử dụng quan hệ giữa vận tốc dài và vận tốc góc của một vật thể trong chuyển
động tròn, ta có các phƣơng trình sau:


 

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

 
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

hai bánh và vận tốc góc:







Lấy phƣơng trình



cộng phƣơng trình  vế theo vế, ta có:





 



Thay  từ



vào phƣơng trình trên ta có quan hệ giữa vận tốc tịnh tiến và vận tốc
hai bánh:







 



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 phƣơng trình  và



với 

và 

là độ dài mỗi
bánh dịch chuyển đƣợc trong thời gian . Thay lại  và  trong các biểu thức này vào phƣơng
trình . Ta có:








 


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





 





  










 
Phƣơng trình trên cho ta quan hệ chính xác của giữa chuyển động của hai bánh với tọa độ
của robot. Ta có thể xây dựng giải thuật xấp xỉ phƣơng trình trên để cập nhật tọa độ của robot với
đầu vào ở mỗi thời điểm cập nhật là 

và 

, là số đếm tăng thêm từ encoder của mỗi bánh, và
tọa độ , ,  cuối chu kỳ trƣớc. Khi đó ta gán giá trị 


 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

Trong phần 2.1, mặc dù ta có đƣợc mô tả toán học chính xác chuyển động của robot nhƣng
biểu thức  chứa các đại lƣơng vi phân chỉ có thể xấp xỉ bằng các phƣơng pháp tính. Đồng
thời giá trị trả về từ encoder lại cho ta giá trị rời rạc, cho nên tọa độ ƣớc lƣợng vẫn hàm chứa một
số sai số qua mỗi bƣớc lặp và sai số sẽ tích lũy dần dần và càng ngày tọa độ ƣớc lƣợng đƣợc càng
có độ tin cậy thấp hơn.
Trong mọi trƣờng hợp, đối với một một robot cụ thể đƣợc thiết kế theo một mô hình toán
học cho trƣớc nào đó, ta có thể suy ra đƣợc quỹ đạo, vị trí của robot từ mô hình của nó. Tuy
nhiên, một vấn đế tất yếu đó là ta không bao giờ tìm đƣợc mô hình chính xác của robot, mà chỉ có
thể tìm đƣợc một họ các mô hình có các hệ số trong phạm vi chính xác nhất định. Ngoài ra, khi
hoạt động, tác động của môi trƣờng, đo lƣờng, hao mòn lại làm xuất hiện sai số và kết quả điều
khiển không đúng nhƣ mô hình. Do đó cần phải quan tâm đến các yếu tố không chắn chắn này.
Trong mô hình động học xác suất, một trong những định lý quan trọng nhất của xác suất
học đó là định lý Bayes. Định lý Bayes cho phép tính xác suất của một biến ngẫu nhiên , khi biết
sự kiện liên quan ,  đã xảy ra. Trong phƣơng pháp định vị Monte Carlo thích nghi,  là tọa độ
của robot và  là giá trị đo đƣợc từ cảm biến và thông tin bản đồ đã biết. Giả sử ta biết trƣớc
trong một hành lang có ba cửa phòng và robot đang ở một vị trí bất kì trong hành lang. Robot có
trang bị một cảm biến và có thể cho ta một biến ngẫu nhiên z cho biết robot đang ở cạnh một cánh
cửa với xác suất của z phân bố theo hàm Gauss. Tại thời điểm đầu tiên, robot không biết đƣợc nó
đang ở đâu trong đoạn hành lang. Hàm cảm tính  của robot có dạng phân bố đều trên toàn
bộ chiều dài của hành lang nhƣ đồ thị đầu tiên trong hình 2.2. Khi khởi động hành trình, cảm biến
trên robot cho tín hiệu robot đang ở cạnh một cánh cừa. Phân bố xác suất , là xác suất để
robot nhận đƣợc một tín hiệu cảm biến nhƣ vậy trên toàn hành lang theo định lý Bayes, phân bố
CHƯƠNG 2: CƠ SỞ LÝ THUYẾT ĐỊNH HƯỚNG ROBOT
9
tập trung quanh vị trí các cánh cửa. Tại thời điểm đó,  của robot sẽ có dạng tƣơng tự nhƣ
. Nói cách khác gần đúng, robot biết rằng nó đang ở gần một trong ba cánh cửa trong hành
lang. Đồ thị thứ hai và thứ ba trong hình minh họa hai phân bố xác suất  và  tại thời
điểm này. Khi robot di chuyển một đoạn so với vị trí mà nó vừa cảm nhận đƣợc cánh cửa, dựa
vào odometry của mình, robot sẽ dự đoán vị trí hiện thời, do đó  cũng dịch chuyển theo
hƣớng di chuyển. Tuy nhiên do odometry có sự không chắc chắn nên đỉnh tại các vùng xác suất

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 


và 

là trạng thái của robot tại thời điểm  và , 

là giá trị điều khiển tại
thời điểm .
Phƣơng trình này mô tả hàm phân bố xác suất hậu nghiệm, với điều kiện ràng buộc phụ
thuộc vào trạng thái động học của robot, cần xét khả năng robot đạt trạng thái 

với điều kiện giá
trị điều khiển là 

và biết robot đang ở trạng thái 

.
Trong ứng dụng cụ thể, 

có thể đƣợc xác định nhờ odometry, tuy nhiên khi phân tích lý
thuyết, ta sẽ xem nhƣ 

là giá trị điều khiển.

Hình 2.3: Minh họa mô hình xác suất của phƣơng trình động học
Ta giả sử trong quá trình di chuyển tịnh tiến trên một quãng đƣờng, sẽ xảy ra hiện tƣợng
trƣợt hoặc có sự chênh lệch nhất thời vận tốc trên hai bánh, robot sẽ có một phân bố xác suất vị trí
ở cuối quãng đƣờng, đám mây minh họa phân bố này dày ở các điểm gần đƣờng thẳng và thƣa
dần ra xung quanh, biểu thị độ chắc chắn trong một lân cận gần điểm cuối lý tƣởng. Hình thứ hai
minh họa độ chắc chắn trong một quãng đƣờng phức tạp hơn. Ta thấy độ chắc chắn này phân bố
trải đều hơn.

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à


.


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]. Đƣợc xấp xỉ với chuyển động quay δ
rot1
, chuyển động tịnh tiến δ
trans
,
và chuyển động quay δ
rot2
. Các chuyển động quay và tịnh tiến này bị ảnh hƣởng
bởi nhiễu.
Giải thuật trình bày trong bảng 2.1 đƣợc dùng để tính giá trị của 









với giá trị
ngõ vào là trạng thái của robot tại thời điểm  là 



  


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à 


, chuyển
động tịnh tiến 

và chuyển động quay thứ hai 

. Mỗi cặp vị trí 

 (chuyển động từ 
tới

) đƣợc diễn tả bằng vector thông số duy nhất








và những thông số