TRƯỜNG ĐẠI HỌC VINH

VIỆN KỸ THUẬT VÀ CÔNG NGHỆ

----------------

ĐỒ ÁN TỐT NGHIỆP ĐẠI HỌC

ĐỀ TÀI:

LẬP BẢN ĐỒ VÀ ĐIỀU HƯỚNG CHUYỂN ĐỘNG

TRÁNH VẬT CẢN CHO ROBOT DI ĐỘNG TRÊN
NỀN TẢNG HỆ ĐIỀU HÀNH ROS1

Sinh viên thực hiện : VÕ VĂN HỢP

Lớp : K59 – KTĐK&TĐH
Giảng viên hướng dẫn : THS. HỒ SỸ PHƯƠNG

NGHỆ AN, 05 – 2023

 TRƯỜNG ĐẠI HỌC VINH
VIỆN KỸ THUẬT VÀ CÔNG NGHỆ

----------------

ĐỒ ÁN TỐT NGHIỆP ĐẠI HỌC

ĐỀ TÀI:

LẬP BẢN ĐỒ VÀ ĐIỀU HƯỚNG CHUYỂN ĐỘNG

TRÁNH VẬT CẢN CHO ROBOT DI ĐỘNG TRÊN
NỀN TẢNG HỆ ĐIỀU HÀNH ROS1

Sinh viên thực hiện : VÕ VĂN HỢP

Lớp : K59 – KTĐK&TĐH
Giảng viên hướng dẫn : THS. HỒ SỸ PHƯƠNG
Giảng viên phản biện : TS. MAI THẾ ANH

NGHỆ AN, 05 – 2023

 TRƯỜNG ĐẠI HỌC VINH CỘNG HOÀ XÃ HỘI CHỦ NGHĨA VIỆT
VIỆN KỸ THUẬT VÀ CÔNG NGHỆ NAM
Độc lập - Tự do - Hạnh phúc
______________________________________________________

___________________________________________________________________________

Bộ môn Kỹ thuật điều khiển & Tự động hóa

GIẤY XÁC NHẬN

NỘI DUNG ĐỒ ÁN TỐT NGHIỆP ĐÃ ĐƯỢC CHỈNH SỬA
THEO YÊU CẦU CỦA HỘI ĐỒNG ĐÁNH GIÁ
Đề
tài: .................................................................................................................................
...... ................................................................................................................................
.......
Là đồ án tốt nghiệp đại học của sinh viên: ...................................................................
Mã số sinh viên:............................................................................................................
Lớp: ..............................................................................................................................
Giảng viên hướng dẫn: .................................................................................................
Đồ án tốt nghiệp này đã được bảo vệ tại hội đồng đánh giá đồ án tốt nghiệp
của Bộ môn Kỹ thuật Điều khiển & Tự động hóa, Viện Kỹ thuật và Công nghệ,
Trường Đại học Vinh vào ngày ...... tháng ....... năm 20......
Thành phần Hội đồng đánh giá đồ án tốt nghiệp đại học gồm: (ghi rõ học
hàm, học vị, họ và tên)
1. .......................................................................... - Chủ tịch Hội đồng
2............................................................................ - Ủy viên - Thư ký
3............................................................................ - Ủy viên - Phản biện
4............................................................................ - Ủy viên
5............................................................................ - Ủy viên
Đồ án tốt nghiệp này đã được chỉnh sửa theo ý kiến đóng góp của giảng viên
phản biện và Hội đồng đánh giá.
Nghệ An, ngày ...... tháng ... năm 20......

CÁN BỘ HƯỚNG DẪN CÁN BỘ PHẢN BIỆN

(Ký, ghi rõ họ tên) (Ký, ghi rõ họ tên)

TRƯỞNG BỘ MÔN
(Ký, ghi rõ họ tên)
 LỜI MỞ ĐẦU
Ngày nay, cùng với sự phát triển nhanh chóng của khoa học và công nghệ,
lĩnh vực Robot thông minh đang là một xu hướng được nghiên cứu và phát triển của
nhiều quốc gia trên thế giới. Sự ra đời của Robot thông minh đã có những đóng
góp to lớn trong các ngành công nghiệp và xã hội giai đoạn vừa qua. Hiện nay, có
rất nhiều ứng dụng của hệ thống robot thông minh được tìm thấy trong nhiều lĩnh
vực như hệ thống điều khiển, hệ thống quản lý sản xuất, AGV (phương tiện tự
động dẫn đường), chống thảm hoạ và hỗ trợ y tế... Tuy nhiên vấn đề ở đây là việc
làm sao để robot tự hành có thể di chuyển, nhận biết môi trường và thực
hiện được nhiệm vụ đặt ra. Vấn đề đầu tiên là di chuyển, robot tự hành nên di
chuyển thế nào và cơ cấu di chuyển nào là sự lựa chọn tối ưu nhất. Điều hướng là
vấn đề cốt lõi trong nghiên cứu và phát triển Robot tự hành.
Từ các thực tế trên, mục tiêu của đề tài là xây dựng robot tự hành dựa trên
nền tảng ROS, robot có khả năng lập bản đồ ở một không gian chưa xác định, định
vị và di chuyển đến địa điểm khác hoàn toàn tự động. Thông qua đó có thể trợ giúp
cho con người trong các công việc đòi hỏi tính chính xác cao, tần suất làm việc lớn
cũng như các công việc khó khăn, nguy hiểm.
Tuy thời gian có hạn, song với tinh thần ham học và nỗ lực của bản thân
chúng em đã hoàn thành đề tài đúng với mục tiêu đặt ra. Bên cạnh đó cũng không
tránh được sự sai sót. Do vậy, chúng em rất mong có những góp ý chân thành của
các thầy, cô để đề tài có kết quả cao nhất.
Nhân đây, chúng em xin gửi lời cảm ơn sâu sắc nhất đến ThS. Hồ Sỹ Phương
người đã tận tình hướng dẫn, truyền đạt các kiến thức chuyên môn trong suốt quá
trình thực hiện đồ án. Thầy đã tạo môi trường làm việc thuận lợi nhất giúp chúng
em hoàn thành tốt những mục đã đề ra. Ngoài ra, chúng em cũng thật sự biết ơn
những thầy cô giáo trong khoa Kỹ Thuật Điều Khiển và Tự Động Hóa đã giảng dạy
chu đáo, cung cấp những kiến thức chuyên sâu góp phần quan trọng để chúng em có
thể hoàn thiện đề tài này.

1
 TÓM TẮT ĐỒ ÁN
Đề tài được xây dựng trên nền tảng hệ điều hành dành cho robot (ROS –
Robot Operating System), đây là một hệ điều hành mã nguồn mở được tích hợp rất
nhiều công cụ, thư viện lớn của đông đảo các nhà nghiên cứu trên khắp thế giới,
giúp cho việc thử ngiệ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.
Điều 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 đề tài này, bài toán định hướng được nghiên cứu trên một robot tự hành 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 có những đặc điểm biết trước đồng thời phản
ứng thích hợp với những vật cản chưa xác định nhằm tìm đường di chuyển đến một
vị trí đích đã định. Từ tính năng cơ bản này, trong tương lai gần 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 con người.
Sau khi hoàn thành đồ án, nhóm đã rút ra các kết quả là robot có thể vẽ lại
một môi trường chưa xác định với độ chính xác cao. Bên cạnh đó, robot còn có khả
năng tự định vị và di chuyển đến đích khá tốt với quãng đường đi là ngắn nhất.
Trong quá trình điều hướng, robot sẽ tự động tránh các vật cản cố định cũng như
phản ứng tránh các vật cản di động.

2
 ABSTRACT
The topic is built on the basis of operating system for robots (ROS - Robot
Operating System), this is an open source operating system that integrates a lot of
tools, a large library of researchers around the world, making it easier to test and
share algorithms. In the short time since its appearance, ROS has grown rapidly and
become a popular tool, standard widely used by robotics research and
manufacturing units.
Navigating mobile robots is a matter of enormous applications and involving
many fields. This is also a central application direction of ROS. In this topic, the
orientation problem is studied on an autonomous robot developed on the ROS
platform. By successfully applying ROS, the robot was able to locate itself in an
environment with known characteristics and react appropriately to unknown
obstacles in order to find a way to move to a target location determined. From this
basic feature, in the near future, we can further exploit the features of ROS,
integrate advanced positioning techniques to help the robot move more accurately,
have a wide operating range to serving applications in human life.
After completing the project, the team has drawn the results that the robot
can redraw an unknown environment with high accuracy. Besides, the robot is also
capable of self-locating and moving to the destination quite well with the shortest
distance. During navigation, the robot will automatically avoid fixed obstacles as
well as react to avoid moving obstacles.

3
 MỤC LỤC
GIẤY XÁC NHẬN..................................................................................................3
LỜI MỞ ĐẦU..........................................................................................................1
TÓM TẮT ĐỒ ÁN..................................................................................................2
ABSTRACT.............................................................................................................3
DANH MỤC HÌNH VẼ...........................................................................................7
DANH MỤC BẢNG..............................................................................................10
DANH MỤC TỪ VIẾT TẮT................................................................................11
CHƯƠNG 1. GIỚI THIỆU...................................................................................12
1.1. TỔNG QUAN VỀ ĐỀ TÀI..............................................................12
1.2. MỤC ĐÍCH CỦA ĐỒ ÁN...............................................................13
1.3. PHƯƠNG PHÁP THỰC HIỆN......................................................13
1.4. PHẠM VI ÁP DỤNG.......................................................................14
1.5. SƠ LƯỢC VỀ NỘI DUNG ĐỒ ÁN................................................15
CHƯƠNG 2. CỞ SỞ LÝ THUYẾT.....................................................................16
2.1. GIỚI THIỆU TỔNG QUAN HỆ ĐIỀU HÀNH ROS....................16
2.2. TÍNH NĂNG CỦA ROS..................................................................17
2. 3. CẤU TRÚC ROS............................................................................17
2.3.1. Tầng ROS Filesystem................................................................17
2.3.2. Tầng ROS Computation Graph...............................................18
2.3.3. Tầng ROS Community Level...................................................20
2.4. CÁC NODE CHÍNH TRONG ROS...............................................21
2.4.1. Node Key Teleop.......................................................................21
2.4.2. Node Twist to motor.................................................................22
2.4.3. Serial Node................................................................................23
2.4.4. Node Odometry.........................................................................23
2.4.5. Node SLAM/AMCL..................................................................24
4
 2.4.6. Node Rplidar.............................................................................25
2.4.7. Node Robot State Publisher.....................................................25
2.4.8. Node Map Server......................................................................26
2.4.9. Node Move Base........................................................................26
2.5. PHÉP BIẾN ĐỔI TỌA ĐỘ TRONG ROS....................................26
CHƯƠNG 3. PHÂN TÍCH VÀ THIẾT KẾ PHẦN CỨNG CHO ROBOT......28
3.1. TỔNG QUAN VỀ PHẦN CỨNG...................................................28
3.1.1. Sơ đồ khối của hệ thống............................................................28
3.1.2. Sơ đồ đấu dây............................................................................29
3.2. THIẾT KẾ PHẦN CỨNG...............................................................31
3.2.1. Lựa chọn vật liệu và chế tạo khung robot...............................31
3.2.2. Phân tích và lựa chọn động cơ DC Servo................................32
3.2.3. Mạch điều khiển động cơ BTS7960.........................................35
3.2.4. Nguồn điện.................................................................................37
3.2.5. Cảm biến RPLIDAR A1...........................................................38
3.3. MÔ HÌNH ĐỘNG HỌC CỦA ROBOT.........................................38
3.4. ĐIỀU KHIỂN ĐỘNG DC CƠ SỬ DỤNG THUẬT TOÁN PID...40
3.4.1. Khái niệm bộ điều khiển PID...................................................40
3.4.2. Cân chỉnh hệ số PID cho động cơ............................................42
3.5. PHƯƠNG PHÁP ĐỌC ENCODER BẰNG NGẮT NGOÀI........42
CHƯƠNG 4. PHÂN TÍCH VÀ XÂY DỰNG PHẦN MỀM DỰA TRÊN NỀN
TẢNG ROS............................................................................................................45
4.1. THUẬT TOÁN VẼ BẢN ĐỒ HECTOR SLAM............................45
4.1.1. Tổng quan..................................................................................45
4.1.2. Xử lý Hector Slam.....................................................................47
4.1.3. Scan Matching...........................................................................49
4.2. HỆ THỐNG ĐỊNH VỊ CHO ROBOT............................................52
4.2.1. Mô hình chuyển động...............................................................52

5
 4.2.2. Mô hình quan sát......................................................................53
4.2.3. Particle filter..............................................................................53
4.2.4. Xác định vị trí robot với thuật toán AMCL............................55
4.3. ĐIỀU HƯỚNG CHO ROBOT........................................................57
4.3.1. ROS Navigation Stack..............................................................57
4.3.2. Bản đồ trọng số (costmap)........................................................59
4.3.3. Thuật toán Dijkstra cho global planner..................................60
4.3.4. Thuật toán Dynamic Window Approach để tránh vật cản cho
local planner...................................................................................................64
CHƯƠNG 5: KẾT QUẢ THỰC NGHIỆM.........................................................67
5.1. MÔI TRƯỜNG THỬ NGHIỆM....................................................67
5.2. KẾT QUẢ ĐẠT ĐƯỢC...................................................................68
5.2.1. Quá trình mapping và kết quả bản đồ....................................68
5.2.2. Nhận xét và kết luận.................................................................70
5.3. Định vị robot trong bản đồ..............................................................70
5.4. Điều hướng cho robot dùng thuật toán Dijsktra...........................71
5.4.1. Kết quả.......................................................................................71
5.4.2. Đánh giá kết quả........................................................................71
5.5. Tránh vật cản trên đường di chuyển dùng thuật toán DWA.......72
5.5.1. Kết quả.......................................................................................72
5.5.2. Đánh giá kết quả.......................................................................73
5.6. Những vấn đề phát sinh trong việc thực hiện đề tài......................73
CHƯƠNG 6. KẾT LUẬN.....................................................................................75
TÀI LIỆU THAM KHẢO.....................................................................................77
PHỤ LỤC............................................................................................................... 78

6
 DANH MỤC HÌNH VẼ
Hình 1.1. Robot tự hành trên nền tảng ROS............................................................13
Hình 1.2. Quy trình tự động xây dựng bản đồ.........................................................14
Hình 2.1. Mô hình ROS File System level..............................................................17
Hình 2.2. Mô hình tầng ROS Computation Graph...................................................19
Hình 2.3. Một số phiên bản của ROS ở từng giai đoạn khác nhau..........................20
Hình 2.4. Sơ đồ cấu trúc hệ thống trên ROS............................................................21
Hình 2.5. Vận tốc ứng với lệnh điều khiển..............................................................21
Hình 2.6. Các giá trị vận tốc của robot....................................................................22
Hình 2.7. Serial node...............................................................................................23
Hình 2.8. Odometry thực hiện cập nhật vị trí mới của robot...................................24
Hình 2.9. SLAM/AMCL.........................................................................................25
Hình 2.10. Node Rplidar thực hiện quét dữ liệu môi trường...................................25
Hình 2.11. Map Server............................................................................................26
Hình 2.12. Các hệ tọa độ trực quan hóa...................................................................27
Hình 3.1. Sơ đồ khối của hệ thống..........................................................................28
Hình 3.2. Sơ đồ đấu dây của khối nguồn và khối Lidar...........................................29
Hình 3.3. Sơ đồ đấu dây của khối động cơ..............................................................29
Hình 3.4. Sơ đồ đấu dây của khối encoder..............................................................30
Hình 3.5. Sơ đồ đấu dây của khối vi điều khiển......................................................30
Hình 3.6. Mặt trái của mẫu thiết kế 3D của robot....................................................31
Hình 3.7. Mặt phải của mẫu thiết kế 3D của robot..................................................32
Hình 3.8. Động cơ DC giảm tốc JBG 37-520 kèm encoder.....................................32
Hình 3.9. Encoder tương đối gồm 2 kênh A và B....................................................34
Hình 3.10. Nguyên tắc hoạt động của Encoder........................................................34
Hình 3.11. Mạch điều khiển động cơ BTS7960.......................................................35
Hình 3.12. Sơ đồ các chân của BTS7960................................................................37
7
Hình 3.13. Pin 18650...............................................................................................37
Hình 3.14. Cảm biến RPLIDAR A1........................................................................38
Hình 3.15. Bộ điều khiển PID.................................................................................40
Hình 3.16. Giải thuật PID trong chương trình.........................................................42
Hình 4.1. Các node của Hector SLAM....................................................................45
Hình 4.2. Sơ đồ quá trình vẽ bản đồ........................................................................46
Hình 4.3. Bản đồ được vẽ từ các điểm Lidar thu được............................................46
Hình 4.4. Sử dụng Scan Matching để mở rộng bản đồ............................................47
Hình 4.5. Toàn bộ bản đồ đã được vẽ......................................................................47
Hình 4.6. Một điểm bất kì trên bản đồ lưới 2D........................................................48
Hình 4.7. Bản đồ lưới chiếm dụng và các dẫn xuất không gian...............................49
Hình 4.8. Các bản đồ bị chồng lên nhau do quá trình xoay Lidar............................49
Hình 4.9. Dữ liệu của Laser không khớp với bản đồ...............................................50
Hình 4.10. Dữ liệu của Laser khớp với bản đồ........................................................50
Hình 4.11. Tóm tắt quá trình lập bản đồ dùng Hector SLAM.................................52
Hình 4.12. Trạng thái chuyển động của robot theo mô hình xác suất......................52
Hình 4.13. 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...........................................................................55
Hình 4.14. Computation Graph Navigation Stack...................................................57
Hình 4.15. Các giá trị của bản đồ trọng số ảnh hướng tới hoạt động của robot.......59
Hình 4.16. Tóm tắt giải thuật Dijkstra.....................................................................60
Hình 4.17. Thuật toán Dijkstra................................................................................61
Hình 4.18. Chọn node..............................................................................................61
Hình 4.19. Khoảng cách ngắn nhất..........................................................................61
Hình 4.20. Khoảng cách nhỏ nhất...........................................................................62
Hình 4.21. Khoảng cách đường đi...........................................................................62
Hình 4.22. Xét tiếp node F......................................................................................62
Hình 4.23. Kết quả của thuật toán...........................................................................63

8
Hình 4.24. Hoạch định đường đi dùng thuật toán Dijkstra......................................63
Hình 4.25. Vận tốc cho phép Va trong DWA...........................................................64
Hình 4.26. Vận tốc trong cửa sổ động Vd trong DWA............................................65
Hình 4.27. Heading của robot trong DWA..............................................................65
Hình 4.28. Lưu đồ thuật toán của thuật toán DWA.................................................66
Hình 5.1. Mô hình robot thử nghiệm.......................................................................67
Hình 5.2. Phòng thử nghiệm robot..........................................................................67
Hình 5.3. Không gian thực tế để dựng bản đồ.........................................................68
Hình 5.4. Bắt đầu quá trình dựng bản đồ.................................................................68
Hình 5.5. Hoàn thành bản đồ...................................................................................69
Hình 5.6. Sử dụng bộ lọc AMCL định vị robot.......................................................70
Hình 5.7. Hoạch định đường đi cho robot...............................................................71
Hình 5.8. Di chuyển tới điểm đích theo định hướng................................................71
Hình 5.9.Hoạch định đường đi ban đầu...................................................................72
Hình 5.10. Vẽ đường đi mới khi có vật cản xuất hiện.............................................72
Hình 5.11. Robot đi đến điểm đích..........................................................................73
Hình 5.12. Bản đồ bị chồng hình.............................................................................74

9
 DANH MỤC BẢNG
Bảng 3.1.Thông số kỹ thuật của động cơ DC..........................................................32
Bảng 3.2. Thông số kỹ thuật của Encoder...............................................................34
Bảng 3.3. Sơ đồ chân của encoder:..........................................................................35
Bảng 3.4. Chân vi điều khiển (Dòng thấp):.............................................................36
Bảng 3.5. Chân động cơ (Dòng điện cao)................................................................36
Bảng 3.6. Số lượng ngắt trên các loại arduino.........................................................43
Bảng 5.1. So sánh bản đồ thu được và thực tế.........................................................69

10
 DANH MỤC TỪ VIẾT TẮT

ROS Robot Operating System Hệ thống vận hành robot

PID Proportional Integral Derivative Bộ điều khiển vi tích phân tỷ lệ

SLAM Simultaneous Localization And Định vị và vẽ bản đồ đồng thời

Mapping

MOSFE Metal-Oxide Semiconductor Field Bán dẫn hiệu ứng trường oxi
T Effect Transist kim loại

AMCL Adaptive Monte Carlo Localization Định vị Monte – Carlo thích

nghi

LIDAR Light Detection And Ranging Xác định ánh sáng và phạm vi

DWA Dynamic Window Approach Thuật toán tránh vật cản

MCU Multipoint Control Unit Thiết bị điều khiển đa điểm

API Application Programming Interface Phương thức, giao thức kết nối
với các thư viện và ứng dụng
khác

PWM Pulse Width Modulation Điều Chế Độ Rộng Xung

DC Direct Current Điện Một Chiều

OMG Occupancy Gird Maps Bản đồ lưới chiếm dụng

UART Universal Asynchronous Giao Thức Gói Tin Người

Receivers/Transmitter Dùng

PF Particle Filter Bộ lọc không tham số 

11
 CHƯƠNG 1. GIỚI THIỆU
1.1. TỔNG QUAN VỀ ĐỀ TÀI
Khoa học và kỹ thuật tiến bộ không ngừng và có tính bước ngoặt như hiện tại
thì trong tương lai gần, mỗi một chúng ta sẽ có bên người một robot riêng hơn là
một chiếc PC hay điện thoại. Vì thế robot đã và đang giữ vai trò rất lớn như là một
bộ phận không thể nào thiếu trong đời sống. Trong tương lai gần robot sẽ là một bộ
phận các cánh tay đắc lực hỗ trợ cho loài người trong mọi công việc.
Song song với ra đời của ngành kỹ thuật điều khiển và tự động hóa, robot
cũng dần được hoàn chỉnh thêm và ngày càng sử dụng rộng rãi trong các ngành sản
xuất, giáo dục, y tế, khoa học công nghệ... và mang lại nhiều lợi ích đối với cuộc
sống con người như thay thế dần lao động của con người trong các điều kiện môi
trường độc hại nguy hiểm, tăng nhanh năng suất lao động, và đặc biệt nó đóng góp
tích cực cho tiến trình công nghiệp hoá của đất nước ta nói riêng và nhân loại nói
chung.
Cũng chính những lợi ích vô cùng to lớn mà robot đem tới cho con người
nên ngày nay việc sử dụng robot ngày càng trở nên phổ biến. Robot tự hành đa dạng
về kích cỡ, hình dáng và có thể hoạt động theo nhiều cách thức khác nhưng có cùng
một mục đích chung là giúp ích cho con người. Robot có thể sử dụng nhiều phương
thức điều hướng khác nhau, ví dụ như nhận biết mã vạch trên nền đất, chụp hình
cảnh quan chung quanh hoặc sử dụng công nghệ quét laser. Song, chúng chỉ có ba
điều quan trọng nhất phải được ưu tiên giải quyết. Trước hết, robot phải biết rõ toạ
độ của mình trong hệ thống toạ độ cơ sở. Thứ hai, robot phải tạo cho chúng một bản
đồ chỉ hướng di chuyển và né tránh vật cản chính xác. Cuối cùng là do quá trình
hoàn thành hai công việc trên nên đòi hỏi robot phải cảm nhận rõ thế giới quanh
robot bằng các giác quan, đặc biệt là quá trình thị giác.
Ở Việt Nam chúng ta hiện tại, việc ứng dụng xe tự động không phải khá mới
mẻ nhưng có nhiều hạn chế về việc áp dụng công nghệ cao. Nhằm phục vụ yêu cầu
của xã hội thì việc nghiên cứu và sử dụng robot tự động phục vụ cuộc sống cần phải
đẩy mạnh hơn. Chính vì vậy nên nhóm đã lựa chọn đề tài "Lập bản đồ và điều
hướng chuyển động tránh vật cản cho robot di động trên nền tảng hệ điều hành
ROS1" để nghiên cứu và thực hiện, qua đó học được nhiều kiến thức cũng như trau

12
dồi nhiều kỹ năng với mong muốn sẽ mở rộng lĩnh vực xe tự hành trong tương
nhằm nâng cao đời sống của con người và phát triển đất nước hơn nữa.

Hình 1.1. Robot tự hành trên nền tảng ROS

1.2. MỤC ĐÍCH CỦA ĐỒ ÁN
Mục đích chủ yếu của đồ án là phát triển robot tự hành có khả năng lập bản
đồ và di chuyển trúng đích mà không bị va chạm với vật cản. Robot di động này có
ứng dụng và tiềm năng hết sức đa dạng nhưng đồ án này chủ yếu tập trung chung
cho một vài chức năng chính được phát triển trên cơ sở ROS:

 Hiểu được cơ bản các thuật toán trong điều hướng cho xe tự hành dựa trên
bản đồ đã dựng từ môi trường thực tế.

 Mô phỏng và điều hướng robot trên Gazebo và Rviz.

 Thiết kế và chế tạo robot có khả năng vẽ bản đồ trong một không gian
chưa xác định.

 Robot có khả năng định vị và hoạch định đường đi ngắn nhất, đi theo
đường đã hoạch định.

 Bên cạnh đó trong quá trình di chuyển robot có thể tránh các vật cản cố
định đồng thời phản ứng thích hợp để né các vật cản di động.
1.3. PHƯƠNG PHÁP THỰC HIỆN
ROS hỗ trợ đa dạng trong các bài toán định vị: định vị cục bộ (Simultaneous
localization) và định vị toàn cục (Simultaneous globalization) dựa trên các gói hàm
thư viện đóng gói (package). Global_planner là thư viện hỗ trợ giải quyết bài toán
13
toàn cục. Sử dụng thuật toán Dijkstra để tìm đường đi ngắn nhất.
Dwa_local_planner (Dynamic Window Approach - DWA) [1] là hàm thư viện cung
cấp bộ điều khiển robot di chuyển trong mặt phẳng. Bộ điều khiển này kết nối kết
quả của bài toán tìm đường đến robot. Khi có bản đồ, bộ tìm đường sẽ tạo quỹ đạo
động học để robot đi từ vị trí đầu đến đích. Trên đường đi, bộ tìm đường sẽ tạo một
bản đồ lưới nhỏ xung quanh robot và điều khiển robot di chuyển theo các ô lưới
này. Công việc của bộ điều khiển là xác định vận tốc dài và vận tốc góc để gửi tới
robot. Sau khi tinh chỉnh các thông số cho hai hàm thư viện toàn cục và cục bộ, quá
trình SLAM của robot được bắt đầu. Lúc này robot sẽ vừa di chuyển vừa tính toán
các bài toán vận tốc, định vị và xây dựng bản đồ. Quá trình xây dựng bản đồ của
robot được thiết kế như Hình 1.2.
Quy trình xây dựng bản đồ tự động được thực hiện như sau: đầu tiên robot sẽ
thu thập toàn bộ môi trường chưa xác định thông qua cảm biến Lidar. Các tọa độ
này sẽ được sử dụng để vẽ bản đồ hiện tại bằng phương pháp Gmapping [2], từ đây
những vị trí còn thiếu trên bản đồ là vị trí Robot cần di chuyển đến để hoàn thiện
bản đồ. Robot sẽ tự động di chuyển đến vị trí được xác định trong suốt giai đoạn di
chuyển, dữ liệu được cập nhật thời gian thực về máy tính, từ đó người quan sát có
thể kiểm tra được tình hình Robot. Khi đến vị trí mới, quá trình thu thập dữ liệu mới
sẽ được thực hiện. Quá trình này chỉ kết thúc khi Lidar không thể thu thập được dữ
liệu mới, hay nói cách khác bản đồ đã được hoàn thiện. Lúc này, quá trình SLAM
đã kết thúc, người điều khiển có thể điều khiển Robot đến bất kỳ vị trí nào trong
bản đồ theo yêu cầu.

Hình 1.2. Quy trình tự động xây dựng bản đồ

1.4. PHẠM VI ÁP DỤNG
Trong kỷ nguyên cách mạng công nghiệp lần thứ tư, nhu cầu sử dụng robot
làm công việc cá nhân thay cho con người rất lớn, một robot tự hành có khả năng
thu thập dữ liệu từ môi trường của nó và hoạt động trong một thời gian dài mà
không cần có sự tham gia của con người là cần thiết.

14
 Nghiên cứu, thiết kế và chế tạo những robot tự hành, xe tự hành không người
lái đang là hướng nghiên cứu trọng tâm của giới khoa học và công nghệ trên khắp
thế giới. Dựa trên những công cụ phần cứng mạnh mẽ, robot giờ đây sử dụng nhiều
loại hệ cảm biến mắt máy (camera 3D), cảm biến quét Laser (Lidar), cảm biến siêu
âm... và đặc biệt là công nghệ trí tuệ nhân tạo và xử lí hình ảnh cho việc nhận dạng,
nhận biết và điều hướng qua những cảm biến đó. Với những công cụ và phần mềm
điều khiển thông thường thì việc xử lí phức tạp, đủ các loại thông tin và độ phân
giải cao như thế này là vô cùng khó, và ROS là phần mềm lập trình có thể giúp
robot xử lý tốt bài toán này. ROS hiện nay được sủ dụng rộng rãi và gần như hoàn
hảo vào việc thiết kế và lập trình trên các loại robot, robot tự hành, và xe tự lái trên
khắp thế giới.
Robot được tạo ra trên nền tảng ROS, kết hợp hoàn chỉnh với 3 chức năng
của robot là định vị, tạo bản đồ và lập kế hoạch đường đi nhằm mục đích chính là
tạo lập bản đồ từ một không gian chưa xác định, phục vụ các nhu cầu của con
người, trang bị cho các bệnh viện, phục vụ và khử khuẩn cách ly, phục vụ cho các
nhà máy công nghiệp, dịch vụ dẫn đường tự động, vận chuyển hàng hóa trong kho
bãi…
1.5. SƠ LƯỢC VỀ NỘI DUNG ĐỒ ÁN
Nôi dung đồ án bao gồm:

 Chương 1: Giới thiệu: Giới thiệu sơ lược về vấn đề đồ án cần giải quyết,
mô tả phương pháp để giải quyết vấn đề và trình bày mục đích của đồ án,
phạm vi mà đồ án tập trung giải quyết.

 Chương 2: Cơ sở lý thuyết: Trình bày các kiến thức về lý thuyết liên quan
tới đồ án.

 Chương 3: Phân tích và thiết kế phần cứng cho robot: Trình bày tổng
quan về phần cứng, sơ đồ đấu dây và sơ đồ khối của hệ thống.

 Chương 4: Xây dựng phần mềm dựa trên nền tảng ROS: Sử dụng các kiến
thức liên quan để xây dựng hệ thống định vị và dẫn đường cho robot.

 Chương 5: Kết quả: Trình bày các kết qủa đạt được của đồ án.

 Chương 6: Kết luận

15
 CHƯƠNG 2. CỞ SỞ LÝ THUYẾT
2.1. GIỚI THIỆU TỔNG QUAN HỆ ĐIỀU HÀNH ROS
Robot Operating System – ROS là hệ điều hành mã nguồn mở dành cho
robot, là một framework được dùng rất rộng rãi trong lĩnh vực robotic với nhiều ưu
điểm. Nó tạo ra một nền tảng phần mềm có thể hoạt động trên rất nhiều robot khác
nhau mà không cần sự thay đổi quá nhiều trong chương trình phần mềm.
ROS được bắt đầu với ý tưởng tạo ra sự thuận tiện là có thể chia sẻ dễ dàng
và có thể được sử dụng lại trên những phần cứng robot khác nhau mà không cần
phải xây dựng lại từ đầu. Việc xây dựng lại từ đầu cho một nền tảng robot riêng biệt
sẽ tốn rất nhiều thời gian và công sức.
ROS là một hệ điều hành mã nguồn mở nên thu hút sự quan tâm của cộng
đồng, đồng nghĩa với các công cụ, thư viện sẽ được xây dựng và phát triển phong
phú. Hiện nay, mô hình robot đã và đang được xây dựng trên hệ điều hành này với
nhiều ứng dụng có tính thực tiễn cao.
ROS cung cấp, hỗ trợ các dịch vụ (services) như một hệ điều hành như phần
cứng trừu tượng (hardware abtraction), kiểm soát các thiết bị cấp thấp (low-level
device control), thực thi (implementation) các chức năng phổ biến, tin nhãn
(message) qua lại giữa các quá trình, và quản lý gói. Ngoài ra, nó cũng cung cấp
nhiều công cụ và thư viện cho việc tham khảo, biên dịch, viết và chạy chương trình
trên nhiều máy khác nhau [3].
Nhóm đã chọn hệ thống ROS để phát triển robot với khả năng vẽ bản đồ và
định vị dựa trên những ưu điểm sau:

 ROS là hệ điều hành mã nguồn mở.

 Các tài liệu kỹ thuật, tài liệu hướng dẫn và các kênh hỗ trợ đầy đủ.

 Một trong những vẫn đề cốt lõi nhất khiến ROS trở nên mạnh mẽ đó là
tính cộng đồng rất lớn. Nguồn tài nguyên được cộng đồng đóng góp hầu
như được xây dựng, phát triển từ những viện nghiên cứu và những trường
đại học hàng đầu.
16
 Về mặt trao đổi dữ liệu và giao tiếp trong ROS: ROS tích hợp một vài chuẩn
giao tiếp khác nhau, bao gồm giao tiếp đồng bộ theo chuẩn RPC qua các services,
truyền dữ liệu bất đồng bộ qua topics, và lưu trữ dữ liệu trên Parameter Server.
Hiện nay, ROS chỉ chạy trên nền tảng Unix. Các phần mềm được chạy trên
ROS chủ yếu được thử nghiệm trên hệ điều hành Ubuntu và Mac OS X. Bên cạnh
đó, thông qua cộng đồng này, nó còn đang được xây dựng để hỗ trợ cho các nền
tảng khác như Fedora, Gentoo, Arch Linux và các nền tảng Linux khác.
Hệ điều hành ROS cùng với các công cụ và thư viện hỗ trợ thường được phát
hành ở dạng ROS Distribution, tương tự như Linux distribution, cung cấp một bộ
phần mềm để người dùng sử dụng, xây dựng và phát triển.
2.2. TÍNH NĂNG CỦA ROS
ROS cung cấp các gói công cụ được tiêu chuẩn hóa giúp:

 Quản lý phần cứng bằng cách viết trình điều khiển

 Quản lý bộ nhớ và các quy trình

 Quản lý đồng thời, song song và hợp nhất dữ liệu

 Cung cấp các thuật toán lý luận trừu tượng, sử dụng trí tuệ nhân tạo.
2. 3. CẤU TRÚC ROS
Mô hình ROS gồm ba tầng: Filesystem, Computation Graph, và Community.
2.3.1. Tầng ROS Filesystem
Filesystem giải thích về các dạng hình thức bên trong, cấu trúc thư mục và
các tập tin tối thiểu để ROS hoạt động. Nó chủ yếu là các tài nguyên của ROS và
được thực hiện trên đĩa cứng.

17
 Hình 2.1. Mô hình ROS File System level

 Packages: Gói dữ liệu là thành phần quan trọng trong cấu trúc chính của
hệ điều hành ROS. Một package có thể chứa các lệnh thực thi của ROS
(các nodes) và một thư viện phụ thuộc ROS như gói dữ liệu, các file cấu
hình, hoặc những dữ liệu quan trọng khác trong ứng dụng.

 Manifests: Là bảng kê khai thông tin dữ liệu của package (manifest.xml),

cung cấp cơ sở dữ liệu về package đó, bao gồm điều kiện cho phép
(license) và những yếu tố phụ thuộc của gói dữ liệu đó. Manifest còn chứa
thông tin về đặc trưng của ngôn ngữ lập trình ví dụ như các cờ báo (flags)
của trình biên dịch.

 Stacks: Là tập hợp các packages phối hợp với nhau để thực hiện một chức
năng cụ thể, chẳng hạn như “navigation stack” là tập hợp các packages
dẫn hướng cho robot.

 Stack Manifests: (stack.xml) cung cấp cơ sở dữ liệu về một stack, bao

gồm điều kiện cho phép (license) và các thông số phụ thuộc vào những
stack khác.

 Message (msg): Định nghĩa cấu trúc dữ liệu cho các messages được gửi
trong ROS.

 Service (srv): Định nghĩa cấu trúc dữ liệu cho các lệnh truy cập (request)
và các phản hồi (response) của các services trong ROS.
2.3.2. Tầng ROS Computation Graph

18
 Việc tính toán trong ROS được thực hiện bằng cách sử dụng một mạng quy
trình được gọi là các nút ROS. Computation Graph cơ bản gồm các thành phần:
Nodes, Master, Parameter server, Messages, Topics, Services, và Bags. Mỗi thành
phần này đóng góp vào biểu đồ theo những cách khác nhau.

Hình 2.2. Mô hình tầng ROS Computation Graph

 Nodes: Mỗi nút là quy trình thực hiện tính toán. Mỗi nút ROS được xây
dựng bởi các thư viện máy khách ROS như roscpp và rospy. Sử dụng API
thư viện máy khách, có thể triển khai các loại phương thức giao tiếp khác
nhau trong mỗi nút ROS. Trong một robot sẽ có nhiều nút để làm những
loại công việc khác nhau. Sử dụng các phương tiện giao tiếp ROS, nó sẽ
giao tiếp với nhau và trao đổi thông tin.

 Master: ROS Master cung cấp đăng ký tên và tra cứu các nút còn lại. Các
nút sẽ không thể tìm thấy nhau, trao đổi tin nhắn hoặc gọi services nếu
không có ROS Master. Trong một hệ thống phân tán, chúng ta nên chạy
máy chủ trên một máy tính và các nút từ xa khác có thể tìm thấy nhau
bằng cách giao tiếp với máy chủ này.

 Parameter Server: Cho phép dữ liệu được lưu trữ bởi các từ khóa trong
một vị trí trung tâm. Nó là một phần của Master.

 Messages: Các node giao tiếp với nhau thông qua messages. Một message
đơn giản là một cấu trúc dữ liệu, bao gồm các trường được định nghĩa như
integer, floating point, boolean... Messages có thể bao gồm các kiểu cấu
trúc và mảng lồng nhau.

19
  Topics: Messages được định tuyến thông qua hệ thống vận chuyển, trong
đó được phân loại thành 2 dạng publish (đưa tin) và subscribe (đăng ký
nhận thông tin). Một node gửi đi một message bằng việc đưa thông tin tới
một topic (chủ đề). Topic chỉ là một cái tên để nhận dạng nội dung của
message.

 Services: Mô hình publish/subscribe thì rất linh hoạt trong việc giao tiếp
nhưng đặc điểm là truyền được đa đối tượng và một chiều. Nhưng đôi khi
lại không thích hợp cho việc truyền theo dạng request/reply, thường được
dùng trong kiểu hệ thống phân bổ. Do đó, việc truyền nhận theo dạng
request/reply được dùng thông qua services. Service được định nghĩa một
cặp cấu trúc dữ liệu: một cho request và một cho reply.

 Bags: Là một định dạng để lưu trữ và phát lại dữ liệu ROS message. Bags
là một cơ chế quan trọng cho việc lưu trữ dữ liệu; ví dụ như giá trị của các
cảm biến rất khó để thu thập cho sự nghiên cứu, phát triển và kiểm tra
thuật toán. Vì thế việc dùng bags là rất quan trọng trong việc phát triển
robot, đặc biệt là những robot mang tính phức tạp cao.
2.3.3. Tầng ROS Community Level.
ROS Community được định nghĩa là nguồn tài nguyên ROS mà các đơn vị
nghiên cứu có thể trao đổi phần mềm và kiến thức. Các nguồn tài nguyên bao gồm:

 Distributions: Là tổng hợp các phiên bản của stack mà chúng ta có thể cài
đặt. ROS Distributions có vai trò tương tự như Linux Distributions.

Hình 2.3. Một số phiên bản của ROS ở từng giai đoạn khác nhau

20
  Respositories: ROS là nguồn tài nguyên dựa trên cộng đồng mã nguồn
mở, trong đó các viện nghiên cứu, trường đại học có thể cùng phát triển
đồng thời và công bố những mã nguồn trên mô hình robot của riêng họ.

 The ROS Wiki: Gồm các tài liệu về ROS. Bất cứ ai cũng có thể chia sẻ
tài liệu, cung cấp các bản cập nhập, viết các bài hướng dẫn… thông qua tài
khoản mà họ đã đăng ký.
2.4. CÁC NODE CHÍNH TRONG ROS
Để điều khiển robot chuyển động theo quỹ đạo mong muốn và đi đến điểm
đích, tại mỗi thời điểm cần xác định được thông tin về vị trí và hướng của robot. Sơ
đồ cấu trúc của hệ thống trên ROS được phát triển dựa trên các node, mỗi node
được tạo ra có nhiệm vụ và chức năng khác nhau. Các thông tin trao đổi giữa từng
node được giao tiếp qua các tin nhắn, các thông tin truyền nhận theo các giao thức
quy định trước và có chung một chuẩn phục vụ việc giao tiếp với các gói dữ liệu
bên ngoài. Sơ đồ cấu trúc hệ thống được thiết kế trên ROS gồm các node và quá
trình truyền nhận dữ liệu được mô tả như trong Hình 2.4.

Hình 2.4. Sơ đồ cấu trúc hệ thống trên ROS

2.4.1. Node Key Teleop
Node Key Teleop nhận thông tin điều khiển từ người dùng để chuyển thành
vận tốc góc và vận tốc tuyến tính của robot. Đầu vào là thông tin điều khiển của
người dùng từ bàn phím, ngõ ra là các giá trị vận tốc tuyến tính v( m/s) và vận tốc
góc ω (m/s)như Hình 2.5.

21
 Hình 2.5. Vận tốc ứng với lệnh điều khiển
2.4.2. Node Twist to motor.
Node Twist to motor thực hiện chuyển vận tốc tuyến tính và vận tốc góc sang
giá trị vận tốc cho từng động cơ. Mối quan hệ giữa vận tốc tuyến tính v, vận tốc góc
ω và vận tốc động cơ bên trái vl và động cơ bên phải v r như Hình 2.6.

Hình 2.6. Các giá trị vận tốc của robot

 Trong đó:
 l là khoảng cách giữa hai bánh xe (m).
 I là tâm ảo của đường tròn khi robot được chạy theo quỹ đạo cong.

22
  R là khoảng cách từ tâm ảo đến trọng tâm của robot. Khi đó vận tốc của
từng động cơ được tính theo công thức sau:

 Khi đó vận tốc của từng động cơ được tính theo công thức sau:

{
l
v r=v +ω ×
2
l
v l=v−ω ×
2

 Vi điều khiển nhận và xử lý tốc độ nhận được rồi xuất xung PWM để điều
khiển hai động cơ thông qua mạch điều khiển động cơ BTS7960, kết hợp
với thuật toán PID chạy theo vận tốc đã đặt.
2.4.3. Serial Node
Serial Node [4] thực hiện giao tiếp giữa MCU và ROS thông qua giao tiếp
Serial. Hai chức năng chính của Serial Node là:

 Đọc giá trị encoder trái và phải. Những giá trị này được gửi dưới dạng
<encoder trái><encoder phải>. Serial Node sẽ thực hiện xử lí chuỗi để
tách giá trị tương ứng.

 Những giá trị này được xuất sang Node Odometry để ước lượng vị trí
robot. Nhận giá trị vận tốc điều khiển hai động cơ và gửi xuống MCU. Hai
giá trị vận tốc này được gửi từ Node Twist To Motor.

Hình 2.7. Serial node

2.4.4. Node Odometry
Odometry [5] có nhiệm vụ ước lượng vị trí của robot (x, y, θ ) từ thông tin
của encoder. Vị trí của robot được biểu diễn qua bộ ba thông số k ={x , y , θ }. Trong
đó x, y là tọa độ trong hệ tọa độ Decade và θ là hướng của robot so với trục x. Quỹ
đạo của robot sau thời gian t là k 0: t =x 0 , x 1 , x 2 ,… x t . Giả sử robot có vị trí ban đầu (

23
x 0 , y 0 , θ0). Vị trí mới của robot sau một khoảng thời gian t di chuyển được một đoạn
d và xoay góc θ như Hình 2.8 được tính theo công thức:

[][ ][ ] [ ]
'
x cos θ 0 −sin θ 0 0 x x0
'
y = sinθ 0 cos θ 0 0 y + y0
'
θ 0 0 1 θ θ0

Trong đó x, y, θ là khoảng cách di chuyển và xoay góc của robot trong

khoảng ∆ t , được tính theo công thức:

{
x=d∗cos ⁡(θ)
y=−d∗sin ⁡( θ)
θ=ω∗∆ t

Tốc độ góc ω có thể được tính từ encoder rồi quy ra góc quay của robot. Tuy
nhiên trong quá trình thử nghiệm, việc tính tốc độ góc từ encoder có sai số lớn, gây
ảnh hưởng bài toán xác định vị trí. Khoảng cách di chuyển d của robot được tính từ
khoảng cách di chuyển độc lập của hai bánh xe trái và phải thông qua giá trị đọc về
của encoder theo công thức:
¿

Trong đó:

 ∆ encoder ¿, ∆ encoder là số xung encoder đọc được trong khoảng thời gian ∆ t
¿ ¿

(xung).

 d ¿ ¿, d ¿ là khoảng cách hai bánh xe trái và phải đi được (m).

 Tick_meter là số xung encoder khi hai bánh xe đi được một mét (ppm)

Hình 2.8. Odometry thực hiện cập nhật vị trí mới của robot
2.4.5. Node SLAM/AMCL

24
 Node SLAM [6] thực hiện vẽ bản đồ môi trường khi robot nhận được ước
lượng vị trí và dữ liệu từ cảm biến Lidar. Để có thể định vị và tránh vật cản, robot
cần biết bản đồ môi trường. Ở lần đầu tiên, người dùng sẽ điều khiển robot di
chuyển bằng tay để vẽ được bản đồ môi trường. Bản đồ sẽ được vẽ bằng phương
pháp Hector SLAM. Node AMCL thực hiện nhiệm vụ xác định vị trí của robot
trong bản đồ đã vẽ. Do đó node AMCL hoạt động khi robot tự định vị và di chuyển
trong bản đồ. Hai node SLAM và AMCL không hoạt động cùng lúc.

Hình 2.9. SLAM/AMCL

2.4.6. Node Rplidar
Node Rplidar đọc dữ liệu do cảm biến laser Lidar cung cấp về khoảng cách
từ robot đến vật cản trong tầm nhìn rộng đến 12m và góc quét 360 °. Dữ liệu này có
thể dùng để vẽ bản đồ và xác định vị trí của robot và tránh vật cản.

Hình 2.10. Node Rplidar thực hiện quét dữ liệu môi trường
2.4.7. Node Robot State Publisher
25
 Node Robot State Publisher [7] hỗ trợ hiển thị vị trí và hướng xoay robot
trong phần mềm mô phỏng 3D. Mỗi bộ phận của robot sẽ được biểu diễn bởi một
Vector vị trí trong không gian. Ví dụ trạng thái vị trí của robot trong không gian vào
thời điểm t sẽ được biểu diễn bằng Vector như Hình 2.8 Nếu robot có nhiều khớp
nối hoạt động độc lập giống với cánh tay robot 6 trục thì mỗi một trục sẽ được biểu
diễn bởi một Vector. Khi sử dụng phần mềm giả lập sẽ có thể hiển thị sự thay đổi
của các khớp nối với sự di chuyển của các Vector và mối quan hệ của các Vector
2.4.8. Node Map Server
Node Map Server dùng để quản lý bản đồ được robot sử dụng. Map Server
cung cấp công cụ để lưu bản đồ vẽ từ SLAM và tải bản đồ để sử dụng cho AMCL.
Bản đồ được lưu thành hai file khác nhau. Một file YAML dùng để lưu thông số
của bản đồ như: tên bản đồ, độ phân giải, gốc toạ độ ban đầu (thường là (0,0,0)),
mức ngưỡng xác định vật cản. File còn lại là file ảnh của bản đồ, thường là ảnh
xám. Pixel có màu đen là vật cản, màu trắng là không có vật cản, màu xám chỉ vùng
bản đồ chưa có thông tin do chưa được quét

Hình 2.11. Map Server

2.4.9. Node Move Base
Node Move Base [8] sử dụng vị trí robot từ AMCL và bản đồ từ Map Server
để điều khiển robot di chuyển đến đích. Đường đi vạch sẵn sẽ được tìm bằng thuật
toán Dijkstra. Trong quá trình di chuyển, nếu có vật cản xuất hiện, robot sẽ tiến
hành tìm lại đường đi mới đến đích. Thông tin điều khiển bao gồm vận tốc tuyến
tính và vận tốc góc được truyền đến Node Twist To Motor để điều khiển robot.

26
2.5. PHÉP BIẾN ĐỔI TỌA ĐỘ TRONG ROS
Gọi hệ tọa độ gắn liền robot và nằm ở tâm quay của robot là
“base_footprint”. Cảm biến sử dụng trong robot phải được khai báo là nằm ở hệ tọa
độ nào và phép biến đổi tọa độ từ hệ tọa độ “base_footprint” tới hệ tọa độ đó. Các
gói Hector SLAM và Navigation cũng yêu cầu các phép biến đổi hệ tọa độ trên
robot được khai báo đầy đủ. Hình 2.12 trực quan hóa các hệ tọa độ trên mô hình 3D
của robot bằng công cụ Soliword mô tả các phép biến đổi hệ tọa độ trên robot.

Hình 2.12. Các hệ tọa độ trực quan hóa

27
 CHƯƠNG 3. PHÂN TÍCH VÀ THIẾT KẾ PHẦN CỨNG CHO ROBOT
3.1. TỔNG QUAN VỀ PHẦN CỨNG
3.1.1. Sơ đồ khối của hệ thống
Robot tự hành trong nhà là một hệ thống được cấu tạo gồm có khung xe, các
ngoại vi và mạch điều khiển đáp ứng các yêu cầu xử lý cảm biến, tự hành tránh vật
cản và điều khiển từ xa. Từ các yêu cầu nêu trên, phần cứng robot bao gồm:

 Khung cơ khí

 Cảm biến Lidar

 Vi điều khiển giao tiếp với ngoại vi và máy tính Arduino Mega 2560

 Động cơ DC servo

 Mạch điều khiển động cơ BTS7960 để điều khiển động cơ

 Nguồn điện cung cấp vào mạch điều khiển động cơ BTS7960

 Máy tính được cài hệ thống ROS trên hệ điều hành Linux

Hình 3.1. Sơ đồ khối của hệ thống

28
3.1.2. Sơ đồ đấu dây

 Khối nguồn và khối Lidar

Hình 3.2. Sơ đồ đấu dây của khối nguồn và khối Lidar

 Khối động cơ

29
 Hình 3.3. Sơ đồ đấu dây của khối động cơ

 Khối encoder

Hình 3.4. Sơ đồ đấu dây của khối encoder

 Khối vi điều khiển

30
 Hình 3.5. Sơ đồ đấu dây của khối vi điều khiển
3.2. THIẾT KẾ PHẦN CỨNG
3.2.1. Lựa chọn vật liệu và chế tạo khung robot
Robot tự hành có chức năng chính là tự hành tránh các vật cản cố định đồng
thời phản ứng thích hợp với các vật cản di động, trên thân robot được đặt lên laptop
có khối lượng 2,2 kg nên robot được thiết kế với hình dạng là hình chữ nhật, kích
thước 47x30cm. Trong quá trình di chuyển của robot để giảm thiểu tối đa rủi ro
cũng như đảm tính chính xác khi di chuyển nhóm đã lựa chọn vật liệu nhôm định
hình kích thước 2x2cm để làm khung xe. Với tính chắc chắn khi nối các khớp của
nhôm định hình, khi robot di chuyển sẽ giảm thiểu tối đa khả năng sai lệch cũng
như dễ dàng tính toán khoảng cách đến các vật cản ở mọi hướng và dễ dàng tránh
các vật cản cũng như có thể đi qua không gian hẹp.

31
 Hình 3.6. Mặt trái của mẫu thiết kế 3D của robot

Hình 3.7. Mặt phải của mẫu thiết kế 3D của robot

3.2.2. Phân tích và lựa chọn động cơ DC Servo
Động cơ được sử dụng cho robot là động cơ JGB 37-520/ 12V-35RPM.
Động cơ được tích hợp thêm Encoder hai kênh AB giúp đọc và điều kiển chính xác
vị trí, chiều quay của động cơ trong các ứng dụng cần độ có chính xác cao: điều
khiển PID, Robot tự hành.
Động Cơ DC Servo JGB37-520 DC có cấu tạo bằng kim loại cho độ bền và
độ ổn định cao, được sử dụng trong các mô hình robot, xe, thuyền,..., hộp giảm tốc
của động cơ có nhiều tỉ số truyền giúp bạn dễ dàng lựa chọn giữa lực kéo và tốc độ
(lực kéo càng lớn thì tốc độ càng chậm và ngược lại), động cơ sử dụng nguyên liệu
chất lượng cao (lõi dây đồng nguyên chất, lá thép 407, nam châm từ tính mạnh,...)
cho sức mạnh và độ bền vượt trội hơn các loại giá rẻ trên thị trường hiện nay (sử
32
dụng lõi dây nhôm, nam châm từ tính yếu). Thông số kỹ thuật của động cơ như
Bảng 3-1.

Hình 3.8. Động cơ DC giảm tốc JBG 37-520 kèm encoder

Bảng 3.1.Thông số kỹ thuật của động cơ DC

Đại lượng Giá trị

270:1 (Động cơ quay 270 vòng trục

Tỉ số truyền
chính động cơ quay 1 vòng)

Dòng không tải 120mA

Dòng chịu đựng tối đa khi có tải 1A

Tốc độ không tải 35RPM (35 vòng 1 phút)

Tốc độ chịu đựng khi có tải 28RPM (28 vòng 1 phút)

Lực kéo Moment định mức 30KG.CM

Lực kéo Moment tối đa 35KG.CM

Chiều dài hộp số L 26,5mm

Số xung encoder trên mỗi kênh

11x270=2970 xung
trên 1 vòng quay của trục chính

Khi di chuyển tự hành, tốc độ di chuyển của robot không yêu cầu là quá
nhanh bên cạnh đó động cơ DC được yêu cầu có hộp số giảm tốc để tăng moment
xoắn giúp việc kéo tải động cơ dễ dàng hơn. Việc điều khiển vận tốc cũng như vị trí

33
của robot cần có bộ phận phản hồi thông tin về số vòng quay mà bánh xe quay
được, bộ phận đảm nhận chức năng này là Encoder.
Encoder có 2 loại chính đó là Encoder tương đối và Encoder tuyệt đối. Trong
đó Encoder tương đối có ngõ ra là hai kênh tín hiệu A và B lệch pha nhau như Hình
3.9, tín hiệu khe Z là tín hiệu để xác định động cơ quay được một vòng. Số xung
trên mỗi vòng quay được chỉ định bởi nhà sản xuất, pha A sớm hay trễ pha hơn B
cho ta biết chiều quay của động cơ. Bên cạnh đó Encoder tuyệt đối cho tín hiệu ngõ
ra là chuỗi n bit cho biết động cơ đã quay được 1/2n vòng quay, tín hiệu này cho
biết chính xác vị trí của Encoder mà người sử dụng không cần xử lý thêm gì.

Hình 3.9. Encoder tương đối gồm 2 kênh A và B

Nguyên tắc hoạt động của Encoder:

 Nguyên lý cơ bản của Encoder, đó là một đĩa tròn xoay, quay quanh trục.
Trên đĩa có các lỗ (rãnh). Người ta dùng một đèn led để chiếu lên mặt đĩa.
Khi đĩa quay, chỗ không có lỗ (rãnh), đèn led không chiếu xuyên qua
được, chỗ có lỗ (rãnh), đèn led sẽ chiếu xuyên qua. Khi đó, phía mặt bên
kia của đĩa, người ta đặt một con mắt thu. Với các tín hiệu có, hoặc không
có ánh sáng chiếu qua, người ta ghi nhận được đèn led có chiếu qua lỗ hay
không. Số xung đếm được và tăng lên nó tính bằng số lần ánh sáng bị cắt.

 Như vậy là encoder sẽ tạo ra các tín hiệu xung vuông và các tín hiệu xung
vuông này được cắt từ ánh sáng xuyên qua lỗ. Nên tần số của xung đầu ra
sẽ phụ thuộc vào tốc độ quay của tấm tròn đó.

34
 Hình 3.10. Nguyên tắc hoạt động của Encoder
Thông số kỹ thuật và sơ đồ chân của Encoder như Bảng 3-2 và Bảng 3-3.
Bảng 3.2. Thông số kỹ thuật của Encoder

Đại lượng Giá trị

Số xung mỗi kênh/1 vòng quay trục

2970 xung
chính

Điện áp hoạt động 3,3V ~ 5V

Loại encoder Encoder tương đối

Bảng 3.3. Sơ đồ chân của encoder:

M1 Dây cấp nguồn cho động cơ

GND Dây cấp nguồn cho Encoder, 0VDC

C1/A Kênh trả xung A

C2/B Kênh trả xung B

Dây cấp nguồn cho encoder điện áp

VCC
3.3~5VDC

M2 Dây cấp nguồn cho động cơ

3.2.3. Mạch điều khiển động cơ BTS7960

BTS7960 là một mô-dun điều khiển động cơ DC, cầu H dòng cao tích hợp
đầy đủ cho các ứng dụng truyền động động cơ cần sử dụng dòng cao.

35
 Mạch này chứa một MOSFET bên cao kênh p và một MOSFET bên thấp
kênh n, với một vi mạch trình điều khiển tích hợp trong một gói. Việc giao tiếp với
vi điều khiển được thực hiện dễ dàng nhờ vi mạch điều khiển tích hợp có các đầu
vào mức logic, bảo vệ chống quá nhiệt, quá áp, quá dòng và ngắn mạch.

Hình 3.11. Mạch điều khiển động cơ BTS7960

Thông số kỹ thuật:
Bảng 3.4. Chân vi điều khiển (Dòng thấp):

Điện áp cũng cấp 6 ~ 27VDC

Dòng tải cao 45A

Tự động ngắt khi áp dưới 5.5V Mở lại sau khi áp lớn hơn 5.5V

Kích thước 50x50mm

Chân đảo chiều hoặc cấp PWM nửa

RPWM
cầu phải, Mức tích cực cao

Chân đảo chiều hoặc cấp PWM nửa

LPWM
cầu trái, Mức tích cực cao

0 Disable nửa cầu phải, 1 Enable

R_EN
nửa cầu phải

L_EN 0 Disable nửa cầu trái, 1 Enable

nửa cầu trái

36
 R_IS giới hạn dòng nửa cầu phải

L_IS giới hạn dòng nửa cầu trái

VCC 5VDC

Bảng 3.5. Chân động cơ (Dòng điện cao)

M+ Chân dương của động cơ

M- Chân âm của động cơ

B+ Điện áp dương đầu vào

B- Điện áp âm đầu vào

Hình 3.12. Sơ đồ các chân của BTS7960

3.2.4. Nguồn điện
Để robot có thể tự hành, di chuyển một cách linh hoạt trong một không gian
cần nguồn điện độc lập cho hệ thống. Có hai nguồn điện chính để cấp cho hệ thống.
Một nguồn được lấy trực tiếp từ pin của laptop cấp trực tiếp cho vi điều khiển và RP
Lidar. Nguồn còn lại được dùng để cấp nguồn cho động cơ.
Pin 18650 là pin có kích thước 18mm x 65mm. Đây là dòng pin lithium-ion
có thể sạc lại được sản xuất bởi nhiều thương hiệu lớn nổi tiếng khác nhau: sony,
panasonic, akasha, ansmann. Pin 18650 cung cấp hiệu suất của một pin lithium-ion,
công suất trong khoảng 1800mAh đến khoảng 3500mAh và công suất 3,7 volt.
Chúng được sử dụng trong một loạt các thiết bị từ máy tính xách tay đến con trỏ
laser và các phụ kiện máy ảnh như gimbals và thanh trượt, đèn pin…

37
 Điện áp cấp cho động cơ giao từ 4 ~ 24V, do đó nhóm quyết định đấu nối 4
Pin 18650 mỗi pin có điện áp từ 3,7 ~ 4,2V trở thành nguồn có điện áp 14,8V ~
16,8V phù hợp với điện áp nằm trong khoảng cấp cho động cơ.

Hình 3.13. Pin 18650

3.2.5. Cảm biến RPLIDAR A1

Lidar dùng để đo khoảng cách đến các đối tượng bằng cách sử dụng chùm
xung lazer (pulsed laser) phát ra và sau đó đo các xung phản xạ ánh sáng của các
đối tượng bằng cảm biến. Sự khác biệt về thời gian truyền về và bước sóng được
dùng để tính toán và xây dựng mô hình vật thể.
Lidar được sử dụng rất nhiều trong thực tế, một số ứng dụng như: xây dựng
bản đồ số trong ngành địa chất, khảo cổ học, hệ thống dẫn đường bằng laser… Một
trong những ứng dụng nổi bật của Lidar hiện giờ là được dùng trong xe tự hành và
robot tự hành.

Hình 3.14. Cảm biến RPLIDAR A1

3.3. MÔ HÌNH ĐỘNG HỌC CỦA ROBOT
38
 Mô hình động học là nghiên cứu về chuyển động của các hệ thống cơ học mà
không xem xét các lực tác động đến chuyển động. Mục đích chính của mô hình
động học là biểu diễn mối quan hệ giữa vận tốc của robot với vận tốc bánh xe cùng
với các thông số hình học của robot.
Gọi V R ,V L là vận tốc dài của 2 bánh trái và phải. Ta có vận tốc dài của robot:

V R +V L φ∙ + φ∙
V= =R R L
2 2

và vận tốc góc của robot là:

V R−V L φ∙ −φ∙
V= =R R L
2L 2L

Tốc độ của robot trong khung robot bây giờ có thể được biểu diễn dưới dạng
vận tốc của tâm A trong khung robot như sau:

{
r φ ∙R + φ∙L
ẋ =Ra
2
r
ẏ a =0
φ∙R−φ∙L
θ̇=ω=R
2

Như vậy:

[ ][ ][ ]
R R
r
ẋa 2 2 φ∙
R
ẏa = 0
r
0 ∙
θ̇ R R φL
2L 2L

Vận tốc có thể thu được trong khung quán tính như sau:

[ ]
R R
cosθ cosθ

[]
r
ẋ 2 2

[]
φ ∙R
a
R R
q̇ I = ẏ =
r
sinθ sinθ ∙ (19)
a
2L 2L φL
θ̇ R R
2L 2L

Phương trình (19) là đặc trưng cho mô hình động thuận của robot hay mô
hình động học có thể được biểu diễn bằng vận tốc dài và vận tốc góc của robot
trong hệ tọa độ Robot.

39
 [ ] [ ][ ]
r
ẋa cosθ 0
q̇ I = ẏ ra = sinθ 0 V
ω
θ̇ 0 1

Ngoài ra ta còn có ràng buộc động học của robot như sau:
I I
ẋ a sinθ− ẏ a cosθ=0
I I
ẋ a cosθ+ ẏ a sinθ=ωR + Lθ
I I
ẋ a cosθ+ ẏ a sinθ=ωL−Lθ
I I
Giả sử, vị trí ban đầu của robot là( x a , y a , θ0)chuyển động trong thời gian t(s)
0 0

khi đó tọa độ trong khung quán tính của robot như sau:
x Ia ( t )=x (aI ) +V .t .cos ⁡(θ ( t ))
0

y aI ( t ) = y(aI ) +V . t . sin (θ ( t ))
0

V R −V L
θ ( t )=θ0 + .t
2L

Từ hai công thức (15) và (16) ta cũng có phương trình động học ngược của
robot như sau: ω R . R=2 .(V + L . θ̇)
ω L . L=2 .(V −L. θ̇)

3.4. ĐIỀU KHIỂN ĐỘNG DC CƠ SỬ DỤNG THUẬT TOÁN PID

3.4.1. Khái niệm bộ điều khiển PID
Thuật toán PID là một trong những thuật toán quan trọng của đề tài, PID
giúp cho hướng di của robot ổn định hơn, đây cũng là mục tiêu quan trọng của đề
tài.
PID hay còn gọi là bộ vi tích phân tỉ lệ là một cơ chế phản hồi vòng điều
khiển được sử dụng rộng rãi trong các hệ thống điều khiển công nghiệp công nghiệp
– bộ điều khiên PID được sử dụng phổ biến nhất trong các bộ điều khiến phản hồi.
Một bộ điều khiển PID tính toán một giá trị "sai số" là hiệu số giữa giá trị đo thông
số biến đổi và giá trị đặt mong muốn. Bộ điều khiển sẽ thực hiện giảm tối đa sai số
bằng cách điều chỉnh giá trị điều khiển đầu vào. Trong trường hợp không có kiến
thức cơ bản về quá trình, bộ điều khiển PID là bộ điều khiển tốt nhất. Tuy nhiên, để
đạt được kết quả tốt nhất, các thông số PID sử dụng trong tính toán phải điều chỉnh
theo tính chất của hệ thống trong khi kiểu điều khiển là giống nhau, các thông số

40
phải phụ thuộc vào đặc thù của hệ thống. Giá trị tỉ lệ P xác định tác động của sai số
hiện tại, giá trị tích phân I xác định tác động của tổng các sai số quá khứ, và giá trị
vi phân L xác định tác động của tốc độ biến đổi sai số.
Một bộ điều khiển PID cơ bản được biểu diễn như ở Hình 3.15.

Hình 3.15. Bộ điều khiển PID

 Ngõ ra của bộ điều khiển PID chính là tổng của ngõ ra ba khâu này:
PID out =P out + I out + Dout

 Khâu tỉ lệ là tích giữa sai số hiện tại với hằng số độ lợi K p :

Pout = K p × e (t)

 Khâu tích phân là tích giữa tổng sai số tức thời theo thời gian với độ lợi K 1
:
t
I out =K 1 X ∫ e ( τ ) dτ
0

 Khâu vi phân là tích giữa độ dốc sai số theo thời gian với độ lợi vi phân
K D:

d
Dout =K D × e (t)
dt

 Hàm truyền của bộ điều khiển PID liên tục:

KI
G ( s )=K p + + K DS
s

Sự phát triển của các hệ thống số yêu cầu cần có những bộ điều khiển rời rạc.
Do đó thuật toán điều khiển PID rời rạc cũng được sử dụng rộng rãi trên máy tính
và hệ thống nhúng. Chuyển đổi về miền thời gian rời rạc cho từng khâu như sau:

41
  Khâu tỉ lệ:
u ( kT )=K p xe (kT )

 Khâu tích phân:

k
u ( kT )=K I ∑ e ( k ) xT
0

 Khâu vi phân:

e ( kT )−e [(k−1)T ]
u ( kT )=K D
T

 Với T là chu kỳ lấy mẫu (giây). Tác đó cho ta ngõ ra tác động u ( kT ) là:
k
e ( kT ) −e [ (k −1)T ]
u ( kT )=K p xe ( kT ) + K I ∑ e ( k ) xT + K D
0 T

Hình 3.16. Giải thuật PID trong chương trình

3.4.2. Cân chỉnh hệ số PID cho động cơ
Có nhiều phương pháp để cân chỉnh PID cho hệ thống điều khiển như: cân
chỉnh bằng phương pháp thủ công, dùng phương pháp Ziegler-Nichols, dùng phần
mềm hỗ trợ cân chỉnh PID. Tuỳ theo mức độ phức tạp và yêu cầu đặt ra mà mỗi
phương pháp sẽ phù hợp với từng hệ thống khác nhau. Nhóm đã sử dụng phương
pháp cân chỉnh thủ công theo các bước sau:

 Bước 1: Khởi tạo các giá trị K p , K I , K D ban đầu bằng 0.

42
  Bước 2: Tăng dần K p cho đến khi đáp ứng động cơ bắt đầu dao động, khi
đó K p sẽ được chọn bằng 1/2 giá trị bắt đầu dao động.

 Bước 3: Cân chỉnh K D để bộ điều khiển nhanh chóng trở về giá trị đặt khi
bị vọt lố. Trong hệ thống điều khiển động cơ, ở ngõ ra đã có một khâu tích
phân lý tưởng nên giá trị K I , sẽ được bằng 0.
3.5. PHƯƠNG PHÁP ĐỌC ENCODER BẰNG NGẮT NGOÀI
Ngắt (interrupt) là những tên gọi hàm tự động khi hệ thống sinh ra một sự
kiện. Những sự kiện này được nhà sản xuất vi điều khiển lập trình bằng phần cứng
sẽ được cấu hình trong chương trình với các tên gọi khác.
Vì ngắt hoạt động độc lập và tự sinh ra khi được cấu hình nên chương trình
điều khiển sẽ dễ dàng thực hiện. Một ví dụ điển hình về ngắt là hàm millis (). Hàm
này tự động làm việc cùng với chương trình và thu được 1 con số tăng lên theo thời
gian dù chúng ta không cài đặt nó. Việc cài đặt hàm millis () sử dụng để ngắt có thể
được cấu hình tự động bên trong mã chương trình Arduino.
Ngắt giúp chương trình khởi động và xử lí nhanh chóng hơn. Chẳng hạn như
khi xác định 1 nút nhấn có được nhấn hay không, trước hết bạn cần so sánh trạng
thái nút nhấn với hàm digitalRead () trong đoạn chương trình loop (). Với cách cài
đặt ngắt thông thường bạn chỉ cần đưa nút nhấn đến đúng vị trí có khả năng ngắt và
lúc đó cài đặt ngắt sẽ sinh ra khi trạng thái nút nhấn từ HIGH -> LOW. Thêm 1 tên
hàm sẽ gọi khi ngắt sinh ra. Vậy là xong, biến trong đoạn chương trình ngắt sẽ cho
bạn thấy trạng thái nút nhấn. Số lượng nút ngắt phụ thuộc theo các đoạn vi điều
khiển. Với Arduino Uno chỉ có 2 ngắt, Mega 2560 có 6 ngắt và Leonardo có 5 ngắt
[9].
Bảng 3.6. Số lượng ngắt trên các loại arduino

Board Int.0 Int.1 Int.2 Int.3 Int.4 Int.5

Uno, Ethernet 2 3

Mega2560 2 3 21 20 19 18

Leonardo 3 2 0 1 7

Thông số ngắt ngoài:

 ISR: tên hàm sẽ gọi khi có sự kiện ngắt được sinh ra.

43
  mode: kiểu kích hoạt ngắt, bao gồm

 LOW: kích hoạt liên tục khi trạng thái chân digital có mức thấp

 HIGb cnbH: kích hoạt liên tục khi trạng thái chân digital có mức cao.

 RISING: kích hoạt khi trạng thái của chân digital chuyển từ mức điện áp
thấp sang mức điện áp cao.

 FALLING: kích hoạt khi trạng thái của chân digital chuyển từ mức điện
áp cao sang mức điện áp thấp.
Cách đọc Encoder bằng ngắt ngoài: Đây là phương pháp dễ nhưng chính xác
để đọc encoder và cũng là phương pháp được dùng trong bài học này.

 Ý tưởng của phương pháp rất đơn giản, chúng ta nối kênh A của encoder
với 1 ngắt ngoài (INT2 chẳng hạn) và kênh B với một chân nào đó bất kỳ
(không phải chân ngắt). Cứ mỗi lần ngắt ngoài xảy ra, tức có 1 xung xuất
hiện trên ở kênh A thì trình phục vụ ngắt ngoài tự động được gọi.

 Trong trình phục vụ ngắt này chúng ta kiểm tra mức của kênh B, tùy theo
mức của kênh B chúng ta sẽ tăng biến đếm xung lên 1 hoặc giảm đi 1. Tuy
nhiên, cần phải tính toán rất cẩn thận khi sử dụng phương pháp này

44
CHƯƠNG 4. PHÂN TÍCH VÀ XÂY DỰNG PHẦN MỀM DỰA
TRÊN NỀN TẢNG ROS
4.1. THUẬT TOÁN VẼ BẢN ĐỒ HECTOR SLAM
4.1.1. Tổng quan
Hector SLAM là kỹ thuật SLAM 2D được phát triển vào năm 2018. Trong
ROS, hector_slam là một gói nhỏ nhằm cài đặt hector_mapping và các gói liên
quan. Các gói chính bao gồm:

 hector_mapping: Node SLAM dựa trên LiDAR không cần odometry và tài
nguyên tính toán thấp. Trong đó, các tham số của node hector_mapping có
thể phân loại như sau:
- Tham số tf: gồm các tham số để điều chỉnh khung tf.

- Tham số bản đồ: gồm các tham số để thiết lập các thuộc tính bản đồ
như kích thước, vị trí xuất xứ, thời gian xuất bản bản đồ....
- Thông số Laser: gồm các thông số để thiết lập các ngưỡng của máy
quét laser. Các giá trị mặc định khớp với các thông số của cảm biến
LiDAR Hokuyo.

 hector_geotiff: Lưu bản đồ và quỹ đạo robot vào các tệp hình ảnh địa lý.

 hector_trajectory_server: Nơi lưu quỹ đạo dựa trên TF. Hình 4.1 biểu thị
mối liên hệ giữa các node trong Hector SLAM được tích hợp trong ROS.

45
 Hình 4.1. Các node của Hector SLAM
Thuật toán Hector SLAM xác định vị trí robot dựa vào phương pháp scan
matching và sử dụng phép đo hình học (odometry) tính từ xung encoder của 2 bánh
xe. Do đó, Hector SLAM rất phù hợp cho việc lập bản đồ trong điều kiện thông tin
về khung tọa độ (odometry) của robot bị sai lệch, không thể đo được. Thuật toán
này yêu cầu phải sử dụng một thiết bị quét tầm xa có tốc độ cao.
Trong đồ án này nhóm sử dụng cảm biến khoảng cách Lidar RPLIDAR
A1M8 360° Laser Range Scanner sử dụng giao tiếp UART, cảm biến có khả năng
quét xa với khoảng cách 0.15~12m, tốc độ quay 5.5Hz và tần số lấy mẫu lên đến
8000 điểm trên 1 giây để xây dựng bản đồ.

Hình 4.2. Sơ đồ quá trình vẽ bản đồ

Từ hình 4.2 ta có các bước vẽ bản đồ bằng Hector SLAM như sau:

 Bước 1: Cảm biến LIDAR sẽ tiến hành quét môi trường xung quanh. Giá
trị trả về từ cảm biến gồm 360 điểm tương ứng với 360 độ khi cảm biến
quét môi trường xung quanh. Mỗi điểm sẽ có 1 giá trị khoảng cách tính từ
LIDAR đến vị trí có vật cản

46
 Hình 4.3. Bản đồ được vẽ từ các điểm Lidar thu được

 Bước 2: Quá trình tiền xử lý sẽ sử dụng thuật toán Hector SLAM để nội
suy tất cả các điểm trên thành các giá trị tương ứng trên bản đồ và vẽ
chúng lên bản đồ lưới được lập sẵn ở Hình 4.3(b).

 Bước 3: Quá trình Scan Matching sẽ tối ưu hóa liên kết của các điểm đã
vẽ và các điểm mới quét được sao cho giá trị chiếm dụng được giảm tối
thiểu.

Hình 4.4. Sử dụng Scan Matching để mở rộng bản đồ

 Bước 4: Thực hiện di chuyển LIDAR dựa vào sự tính toán của các quá
trình trên ta sẽ thu được một bản đồ của môi trường cần quét.

47
 Hình 4.5. Toàn bộ bản đồ đã được vẽ
4.1.2. Xử lý Hector Slam
Khi khởi động quét môi trường để tạo bản đồ, gốc tọa độ của bản đồ sẽ được
cài đặt ở vị trí ban đầu của robot với trục z hướng lên trên, trục x hướng theo chiều
thuận bánh xe của robot (hướng tiến). Hector SLAM [10] sẽ thiết lập sẵn một bản
đồ chiếm dụng, mỗi giá trị rời rạc trên bề mặt bản đồ có một giới hạn riêng. Khi lập
bản đồ ta không thể truy cập trực tiếp vào các giá trị này mà cần phải tính toán nội
suy ra xác suất chiếm dụng của từng điểm và các dẫn xuất. Ta sử dụng bộ lọc
Bilinear để nội suy giá trị ô lưới con sau đó ước tính xác suất chiếm dụng và các
dẫn xuất trên.
Từ ý tưởng trên, khi thực hiện scan bề mặt môi trường, cảm biến Lidar sẽ trả về vị
trí các điểm cần đánh dấu trên bàn đồ lưới rời rạc mà Hector SLAM đã lập sẵn. Các
điểm trả về sẽ là liên tục ở bất kỳ đâu trên bản đồ. Cho ví dụ, cảm biến Lidar trả về
một điểm Pm bất kỳ sẽ chiếm vị trí trên bản đồ, như Hình 4.7.

48
 Hình 4.6. Một điểm bất kì trên bản đồ lưới 2D
Các điểm P00, P01, P10, P11 là các giá trị rời rạc được định sẵn trên OGM , các
giá trị trên là các tọa độ nguyên gần nhất với Pm. Ta cần tính xem điểm Pm sẽ nằm ở
vị trí nào trên bản đồ. Giá trị chiếm chỗ của Pm là M (Pm ). Nếu giá trị chiếm chỗ
được xấp xỉ bằng cách sử dụng phương pháp Bilinear thì phép nội suy tuyến tính
với tọa độ nguyên gần nhất P00, P01, P10, P11 có thể được biểu diễn dưới dạng :

M ( Pm) ≈
(
y− y 0 x−x 0
y 1− y 0 x 1−x 0
M ( P11 ) +
x 1−x
x1 −x0
M ( P01 )
)
(
+ y 1− y x−x 0
y 1− y 0 x 1−x 0
M ( P10 ) +
x 1−x
x 1−x 0
M ( P00 ) )
Gradient của giá trị chiếm chỗ sẽ là:

∇ M ( Pm ) = ( ∂ M ( P m ) ∂ M ( Pm )
∂x
,
∂y )
Từ đó, các dẫn xuất (derivatives) của bản đồ trong một điểm cụ thể sẽ được
tính như sau:
∂ M ( Pm ) y− y 0 y −y
∂x
≈
y 1− y 0
( M ( P 11 ) + M ( P01 ) ) + 1
y 1− y 0
( M ( P11 ) + M ( P 01) )

∂ M ( Pm ) x −x 0 x1 −x
∂y
≈
x 1−x 0
( M ( P11 ) + M ( P01 ) ) +
x 1−x 0
( M ( P11 ) + M ( P01 ) )

49
 Hình 4.7. Bản đồ lưới chiếm dụng và các dẫn xuất không gian
4.1.3. Scan Matching
Khi cảm biến Lidar di chuyển (xoay, tiến, lùi) trong môi trường cần quét, vị
trí của các điểm mà lidar trả về cũng thay đổi một lượng tương ứng. Việc này dẫn
đến giá trí chiếm dụng của một điểm trên bản đồ OGM cũng không cố định mà sẽ bị
lệch đi một khoảng tương ứng với góc quay của Lidar. Kết quả là bản đồ của chúng
ta sẽ không còn chính xác như Hình 4.9.

Hình 4.8. Các bản đồ bị chồng lên nhau do quá trình xoay Lidar
Scan matching [11] tối ưu hóa sự liên kết giữa các chùm điểm cuối
(endpoints) đã được vẽ trên bản đồ với các chùm điểm cuối vừa mới được cảm biến
50
Lidar quét. Scan matching sẽ đảm bảo cho bản đồ luôn được cập nhập một cách
chính xác cho dù cảm biến Lidar có di chuyển hay xoay bất kì góc nào.

Hình 4.9. Dữ liệu của Laser không khớp với bản đồ

Hình 4.10. Dữ liệu của Laser khớp với bản đồ

Phương pháp Gauss-Newton được sử dụng để dự đoán các tư thế tiếp theo
mà không cần liên kết dữ liệu tìm kiếm giữa các endpoints. Bắt đầu với một tư thế

51
ước tính (estimate pose) ban đầu ξ=( px , py , ψ)T , mục tiêu của scan matching là
giảm tối thiểu lỗi chiếm chỗ của endpoints M ( S i (ξ) ) và bản đồ (giá trị của
M ( S i (ξ) ) =1 nghĩa là vị trí đó có chướng ngại vật tồn tại).

Để giảm thiểu lỗi chiếm chỗ của endpoints ta có thể tính như công thức sau:
n
ξ =arg min ∑ [ 1−M ( S i (ξ) ) ]
¿

i=1

Trong đó Si (ξ ) biểu diễn sự biến đổi của endpoints nhận được khi quét bằng
Lidar trong khung robot sang bản đồ.

[ S p
][ ] [ ]
Si ( ξ )= cos (ψ )−sin (ψ) i , x + x
sin (ψ) cos (ψ) Si , y p y

Giải thuật Gauss- Newton được dùng để giải các bài toán bình phương nhỏ
nhất phi tuyến tính, hàm mục tiêu r i được định nghĩa như sau:
r i=1−M ( S i ( ξ ) )

Giả sử vị trí của robot có một chuyển động rất nhỏ, khi đó Δ𝜉 đủ nhỏ để bỏ
qua, thì vector gradient G của hàm mục tiêu được viết là:

[ ][
n T
∂ ri n ∂ S (ξ )
G=∑ r i =∑ ∇ M ( S i ( ξ ) ) i 1−M ( S i ( ξ ) ) ]
i=1 ∂ ξ i=1 ∂ξ

Hàm H biểu thị cho ma trận Hessian, thu được bằng cách bỏ qua các giá trị
đạo hàm phát sinh bậc 2:

[ ][ ]
n n T
∂r ∂r ∂ S (ξ ) ∂ Si (ξ)
H=∑ i i =∑ ∇ M ( S i ( ξ ) ) i ∇ M ( Si ( ξ ) )
i=1 ∂ξ ∂ ξ i=1 ∂ξ ∂ξ

Trong đó, đạo hàm của Si ( ξ ) được thể hiện bởi ma trận sau:

[ ]
∂ S i ( ξ ) 1 0 −sin ⁡(ψ ) S −cos (ψ) S
= i,x i, y
∂ξ 0 1 cos(ψ )S i , x −sin ⁡(ψ )Si , y

Mối quan hệ lặp lại của phương pháp Newton để giảm thiểu hàm mục tiêu
được trình bày như sau:
−1
ξ t =ξ t−1−H G

Giờ ta sẽ tìm giá trị ∆𝜉 sao cho nhỏ nhất:

−1
∆ ξ=ξ t−1−ξ t =H

52
 Hình 4.11. Tóm tắt quá trình lập bản đồ dùng Hector SLAM
4.2. HỆ THỐNG ĐỊNH VỊ CHO ROBOT
4.2.1. Mô hình chuyển động
Mô hình chuyển động diễn tả xác suất biến đổi trạng thái p( x t | x t−1, ut ). Đây
là bước dự đoán trạng thái trong các bộ lọc. Trạng thái của robot thường được diễn
tả bằng ba biến (x, y, z) trong trục Castesian và ba biến (roll, pitch, yaw) trong góc
Euler. Mô hình robot hoạt động trong mặt phẳng chỉ cần dùng vector sau để diễn tả
trạng thái:
T
X =( x , y , θ)

Trong đó (x, y) là vị trí của robot và θ là hướng xoay của robot tại vị trí đó.
Do ảnh hưởng của những yếu tố đã trình bày, không thể dùng một vector để diễn tả
trạng thái của robot ở thời điểm t. Thay vào đó, trạng thái được diễn tả bằng một
phân phối xác suất như Hình 4.13.

Hình 4.12. Trạng thái chuyển động của robot theo mô hình xác suất
Robot có trạng thái ban đầu là x t−1, sau khi nhận được điều khiển ut sẽ di
chuyển đến vị trí x t (i) là một trong các điểm màu xanh dương. Trong khi đó, robot
chỉ đọc được một giá trị của encoder để suy ra vị trí là điểm màu đỏ. Mỗi vị trí x t (i)
có thể xảy ra được gọi là một particle. Có hai mô hình chuyển động chính. Mô hình

53
thứ nhất xem điều khiển ut là vận tốc điều khiển của động cơ làm cho robot di
chuyển. Mô hình này có lợi cho việc tránh vật cản vì ước lượng chuyển động xảy ra
trước khi lệnh điều khiển được truyền xuống động cơ. Tuy nhiên mô hình này chỉ
hiệu quả khi sai số của vận tốc điều khiển và thực tế là tương đối nhỏ. Mô hình thứ
hai xem điều khiển ut là giá trị đọc về từ encoder của động cơ. Mô hình này có độ
chính xác cao hơn mô hình vận tốc. Tuy nhiên robot phải thực hiện xong việc di
chuyển mới có thể ước lượng giá trị encoder. Trong đề tài này nhóm quyết định
chọn mô hình dự đoán trạng thái từ thông tin encoder.
4.2.2. Mô hình quan sát
Mô hình quan sát biểu diễn quá trình xử lý dữ liệu từ cảm biến Lidar khi có
sự ảnh hưởng của nhiễu, được định nghĩa là một phân phối xác suất có điều kiện
p( z t , x t , m). Trong đó z t là phép đo của cảm biến, x t là vị trí của robot ở thời điểm t
và m là bản đồ của môi trường. Ở thời điểm t, phép đo z t thu được một mảng k phần
tử các giá trị đo khoảng cách z t ( i )với 0 ≤ I ≤ k . Ta có thể biểu diễn:
1 2 k
z t ={z t , z t ,… , z t }

Giả sử các điểm đo là độc lập với nhau, ta có thể xấp xỉ:
k
p ( z i|x t ,m ) =∏ p ( z it|x t , m )
i=1

Bản đồ m có dạng grid map, được chia thành nhiều cell nhỏ. Mỗi ô vuông sẽ
có tọa độ (x , y ) với hai mức giá trị là có vật cản hoặc không có vật cản. Giả sử mi là
cell thứ i, bản đồ m biểu diễn không gian bằng một tệp hữu hạn các cell:
m=∑ mi
i

4.2.3. Particle filter

Particle Filter (PF) [12] là bộ lọc không tham số. PF sử dụng một số hữu hạn
các mẫu để đại diện cho một phân phối xác suất. Vì số lượng mẫu có hạn nên PF có
tính xấp xỉ. Gọi bel(x t ) là phân phối thể hiện nhận thức của chính robot về vị trí của
mình trong không gian. Ý tưởng chính của bộ lọc PF là dùng một tập hợp M các
mẫu ngẫu nhiên để đại diện cho bel( x t ):
[ 1] [2 ] [ n]
bel ( x t ) =xt , x t , … , xt

Mỗi particle x [ti ] với (1 ≤i ≤ M ) là một vị trí trong môi trường thực tế mà robot
có thể đang ở đó. Thuật toán của bộ lọc PF với ngõ vào là trạng thái ở thời điểm
t −1, điều khiển ut và ngõ ra là trạng thái hiện tại:

54
  Bước 1: Tạo tập hợp các particle rỗng:
X t = X t=∅

 Bước 2: Với mỗi particle thứ I thực hiện:

- Lấy mẫu:

x [ti ] p ( xt|ut , x [t−1

i]
)
- Tính trọng số cho từng mẫu:

ω [ti] = p ( z t| x[ti] )

- Cập nhật lại tập particle tạm thời:

[i ] [i ]
X t = X t +¿ x t , ωt > ¿
[m ]
 Bước 3: Thực hiện lấy mẫu x t là từ tập particle tạm thời theo trọng số
trong ứng và cập nhật tập particle.
[m ] [ m]
X t = X t +¿ x t , ωt >¿

Số lượng particle trong bộ lọc càng lớn thì phân phối đại diện bởi tập particle
đó càng chính xác. Tuy nhiên tập particle quá lớn không có lợi cho quá trình tính
toán và hoạt động thời gian thực của robot. Ở bước thứ 2, tập particle tạm thời chính
là dự đoán của robot về vị trí hiện tại dựa trên vị trí trước đó và tín hiệu điều khiển.
Việc lấy mẫu trực tiếp từ p ( x t|ut , x t−1 ) là không thể. Thông thường mô hình chuyển
động ở phần này sẽ được sử dụng để dự đoán tập particle tạm thời. Tập particle tạm
thời nằm lân cận quanh vị trí mà robot thu được qua encoder. Mỗi particle được gán
một trọng số thể hiện sự kết hợp của mô hình quan sát và tập particle. Trọng số của
một particle càng lớn có nghĩa xác suất particle đó là vị trí thực của robot càng lớn.
Bước thứ 3 là bước quan trọng nhất trong PF: lấy mẫu tại các mẫy quan trọng. Tập
particle tạm thời biến đổi thành tập particle chính thức với cùng kích thước M. Các
particle có trọng số thấp được loại bỏ khỏi bộ lọc. Thông thường các trọng số tại
thời điểm t sẽ được cập nhập dựa trên trọng số ở thời điểm t – 1, với giá trị ban đầu
bằng 1:

ω [ti] = p ( z t| x[ti] . ω [t−1

i]
)

4.2.4. Xác định vị trí robot với thuật toán AMCL

4.2.4.1. Thuật toán định vị Monte-Carlo

55
 Monte-Carlo là một bộ lọc dạng PF dùng để xác định vị trí của robot trong
không gian với một bản đồ đã biết trước. Bộ lọc Monte-Carlo có hai bước chính là
lấy mẫu vị trí từ mô hình chuyển động, đánh giá trọng số bằng mô hình quan sát và
tiến hành lấy mẫu lại các mẫu quan trọng. Ví dụ robot sử dụng bộ lọc Monte-Carlo
để xác định vị trí của mình khi di chuyển và quan sát vật cản như hình 10:

Hình 4.13. 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
Lúc khởi tạo vị trí robot là hoàn toàn không chắc chắn khi chưa thực hiện
phép quan sát môi trường xung quanh. Do đó các particle x i được lấy mẫu ngẫu
nhiên và đều trên khắp bản đồ. Sau khi quan sát với phép đo z t , thuật toán MCL
thực hiện tính trọng số cho các các particle đã lấy mẫu trước đó. Hàm belx t thể hiện
nhận thức của robot về vị trí của mình dựa trên tập hợp các particle. Sau đó robot di
chuyển, tập particle x t cũng được di chuyển theo, đồng thời trọng số mỗi particle
cũng được ước lượng qua quan sát z t . Hàm belx t được tính lại và các particle có
trọng số nhỏ được loại bỏ. Quá trình được lặp lại cho những lần tiếp theo với số
mẫu hội tụ và giảm. Sau nhiều lần quan sát, tập particle sẽ hội tụ quanh một lân cận
mà xác suất robot đang ở vị trí đó là cao nhất.
4.2.4.2. Bộ lọc thích nghi AMCL

56
 Thông thường tập particle tại một thời điểm X t = { x[1t ] , x[t2] , … x[tM ]} sẽ nằm trong
lân cận của một điểm. Vì vậy khi robot bị dời đi một khoảng lớn hơn vùng lân cận
hoặc thất bại trong định vị toàn cục thì robot không thể khôi phục lại vị trí từ bộ lọc.
p ( z t|z t−1 , ut , m )

Phân phối trên được xấp xỉ theo trọng số ωi của particle để dễ dàng tính toán
số particle ngẫu nhiên cần thêm vào:
M
1
( z t|z t −1 , ut , m ) ≈ M ∑ ωt
[m ]

m =1

Để giảm ảnh hưởng của nhiễu, các trọng số được ước lượng theo các trọng số
ngắn hạn và dài hạn. Khi đó bộ lọc Monte-Carlo có tính thích nghi (AMCL) gồm
các bước sau:

 Bước 1: Tạo tập hợp các particle rỗng:

X t = X t=∅

 Bước 2: Với mỗi particle, tiến hành lấy mẫu để dự đoán vị trí và đánh giá
các trọng số:
- Lấy mẫu:

x [ti ] p ( xt|ut , x [t−1

i]
)
- Tính trọng số cho từng mẫu:

ω [ti] = p ( z t| x[ti] )

- Cập nhật lại tập particle tạm thời:

[i ] [i ]
X t = X t +¿ x t , ωt > ¿

- Tính trung bình của trọng số:

1
ω avg=ω avg + , ω[ m ]
M t

- Tính trọng số ngắn hạn:

ω slow =ωslow + α slow (ωavg −ω slow )

- Tính trọng số dài hạn:

ω fast =ω fast + α fast (ω avg−ωslow )

57
  Bước 3: Thêm các particle ngẫu nhiên vào tập particle chính thức X t với
xác xuất:
ωfast
max ⁡(0.0,1 .0− )
ω slow
[m ]
 Bước 4: Thực hiện lấy mẫu x t lại từ tập particle tạm thời theo trọng số
tương ứng và cập nhật tập particle:
[m ] [ m]
X t = X t +¿ x t , ωt >¿

Các chỉ số cho trọng số ngắn hạn và dài hạn phải thõa mãn điều kiện:
0 ≤ α slow ≤ α fast

Quá trình thêm các particle ngẫu nhiên xem xét sự khác biệt giữa ω fast và ω slow
khi quan sát vùng lân cận. Khi vùng quan sát ngắn hạn tốt hơn hoặc bằng vùng quan
sát dài hạn thì không cần thêm vào các particle ngẫu nhiên. Khi vùng quan sát ngắn
hạn xấu hơn vùng quan sát dài hạn, nghĩa là robot đã bị lạc thì các particle ngẫu
nhiên sẽ được thêm vào tập particle cần quan sát.
4.3. ĐIỀU HƯỚNG CHO ROBOT
4.3.1. ROS Navigation Stack

Hình 4.14. Computation Graph Navigation Stack

58
 Navigation Stack là một ứng dụng được phát triển đầy đủ nhất của ROS. Ý
tưởng của Navigation Stack khá đơn giản, đó là một chương trình lấy thông tin từ
odometry (tọa độ ước lượng) và các cảm biến, xử lý và xuất lệnh dưới dạng vận tốc
xuống mô hình di động (mobile base). Sơ đồ bên trên biểu diễn tổng quan về các
phần tử của Navigation Stack. Navigation Stack khi triển khai sang cấp computation
graph sẽ có dạng đầy đủ gồm một node move_base và các node amcl và map_server
như trong sơ đồ trên. Khối màu trắng là các phần tử cần thiết đã được triển khai, các
khối màu xám là các khối tùy chọn được hỗ trợ sẵn. Các khối màu xanh là các khối
cần được xây dựng riêng cho từng mô hình robot để có thể ứng dụng được
Navigation Stack trên mô hình. Ta cần thiết kế các node có thể cung cấp các phép
chuyển đổi hệ tọa độ, publish các thông tin từ cảm biến theo đúng kiểu Message và
chuyển đổi lệnh điều khiển cho mobile base. Tiếp theo ta phải thiết lập Navigation
Stack với các thông số liên quan tới cấu trúc và chuyển động của robot. Bên cạnh
đó, có một số yêu cầu bắt buộc khi thiết kế Robot ứng dụng Navigation Stack [13].
Nguyên lý hoạt động:

 Đầu tiên chúng ta cần build map bản đồ môi trường và map sẽ được lưu ở
map_sever sau đó map sẽ được map_sever đưa vào global_costmap để
tạo bản đồ điều hướng.

 Global_costmap sẽ được cung cấp global_planner (hoạch định đường đi

toàn cục) và mong muốn của chúng ta là đi chuyển robot từ vị trí hiện tại
đến vị trí chúng ta mong muốn thì global_planner giúp chúng ta tạo hoạch
định đường đi đến điểm đích đó trên bản đồ map.

 Sau đó global_planner sẽ cung cấp hoạch định đường đi toàn cục cho
local_planner (hoạch định đường đi cục bộ) thì để vạch ra đường đi cục
bộ cần dựa vào môi trương thực tế.

 Giá trị được đưa vào đầu tiên đó là local_costmap, thì local_costmap sẽ
cắt 1 phần nhỏ bản đồ xung quanh robot kết hợp với sensor source (cảm
biến lidar) sẽ quét môi trường rồi update xung quanh vị trí robot có vật
cản mới hay không rồi sau đó sẽ quyết định robot sẽ di chuyển như thế
nào.

 Hoạch định đường đi cục bộ vẫn sẽ cố gắng bám theo hoạch định đường
đi toàn cục ban đầu để di chuyển và nó sẽ kết hợp để tránh vật cản.

 Ta có amcl là một modul để xác định vị trí của robot trên bản đồ map.

59
  Odometry source (Laser scan matcher) dùng để quét map hiển thị tf (vị trí
của robot được hiển thị dưới dạng tọa độ) của robot.
4.3.2. Bản đồ trọng số (costmap)
Trong điều hướng robot, bản đồ trọng số dùng để tính vùng có vật cản, các
vùng có khả năng va chạm và vùng robot có thể di chuyển. Phụ thuộc vào kiểu điều
hướng, bản đồ trọng số có thể chia thành hai phần [14].

 Một là global_costmap, thiết lập kế hoạch di chuyển cho điều hướng

robot trong toàn bộ không gian của bản đồ.

 Còn lại là local costmap được dùng cho kế hoạch di chuyển và tránh vật
cản trong không gian giới hạn xung quanh robot.

 Bản đồ trọng số thể hiện giá trị từ 0 tới 255. Từng giá trị được xác định
nơi robot có thể di chuyển hoặc va chạm với vật cản:

 000: vùng khoảng trống mà robot có thể di chuyển được.

 001 - 127: vùng có xác suất thấp xuất hiện vật cản.

 128 - 252: vùng có xác suất cao xuất hiện vật cản.

 253 - 254: vùng có xuất hiện vật cản.

 255: vùng bị chiếm dụng, robot không thể di chuyển

Hình 4.15. Các giá trị của bản đồ trọng số ảnh hướng tới hoạt động của robot

60
  Cost_lethal: Là giá trị của một cell chứa vật cản. Nếu trọng tâm của
robot đi vào vùng này thì chắc chắn robot sẽ bị va chạm

 Cost_inscribed: Là giá trị của một cell từ vật cản đến bán kính nội tiếp
của robot. Nếu tâm của robot nắm trong vùng này thì robot chắc chắn sẽ
va chạm vào vật cản.

 Cost_possibly_circumscribed: Là giá trị của costmap nó cũng tương tự

như cost_inscribed nhưng đó là giá trị từ vật cản đến bán kính ngoại tiếp
của robot. Nếu tâm của robot đi vào vùng này thì khả năng robot va
chạm vật cản phụ thuộc vào hướng đi của robot

 Cost_freespace: Có giá trị bằng 0, thể hiện rằng không có vật cản nơi
này, robot có thể di chuyển tự do trong vùng này.

 Cost_unknow: Là những cell chưa có thông tin về giá trị của costmap.
4.3.3. Thuật toán Dijkstra cho global planner
Trong điều hướng cho robot để robot đến được vị trí mong muốn, ta phải đưa
ra một hoạch định đường đi từ điểm bắt đầu (vị trí hiện tại của robot) đến điểm kết
thúc (vị trí đích mong muốn). Nhằm tối ưu hóa cho đường đi, ta cần phải lựa chọn
đường đi ngắn nhất giữa hai điểm ấy. Với yêu cầu này, thuật toán Dijkstra [15] là
một trong những thuật toán đơn giản để tìm được đường đi tối ưu nhất Hình.

Hình 4.16. Tóm tắt giải thuật Dijkstra

61
 Bài toán đặt ra cho thuật toán Dijkstra là có một đồ thị G=(V . E) (với V là
tập hợp các đỉnh và E là các cạnh vô hướng hoặc có hướng), mỗi cạnh sẽ có trọng
số luôn dương và một đỉnh nguồn s. Chúng ta cần tính toán được đường đi ngắn
nhất từ đỉnh nguồn s đến mỗi đỉnh trên đồ thị. Để hiểu rõ hơn, ta hãy xem hình sau:

Hình 4.17. Thuật toán Dijkstra.

 Bước 1: Chọn node A là source node, tính khoảng cách từ node A đến các
node liền kề.

Hình 4.18. Chọn node

 Bước 2: Khoảng cách ( A – C ) là ngắn nhất. Xét node C, tính tổng khoảng
cách từ node C đến các node liền kề.

Hình 4.19. Khoảng cách ngắn nhất

62
 Bước 3: Khoảng cách từ A đến D là nhỏ nhất. Xét node D và tính khoảng
cách từ node D đến các node liền kề. Ta tính được, đường đi từ node
( A – D – F )=7+ 14=21> ( A – C – F )=16 . Nên giữ nguyên đường đi ngắn nhất
từ ( A−F) là ( A – C – F).

Hình 4.20. Khoảng cách nhỏ nhất

 Bước 4: Ở node B, khoảng cách từ A đến E có hai đường đi. Khoảng cách
đường từ ( A – C – E )=4+ 17=21> ( A−B−E )=9+11=20. Nên ta chọn đường
đi ngắn nhất từ ( A – E) là ( A – B – E).

Hình 4.21. Khoảng cách đường đi

 Bước 5: Sau đó, ta xét tiếp node F tương tự như quá trình trên.

Hình 4.22. Xét tiếp node F

63
  Bước 6: Ở node E, quãng đường từ A−Zlà
( A – C – E−Z )=4+17+ 5=26> ( A – C – F−Z )=4+12+9=25. Nên ta chọn
đường đi nhỏ nhất từ A−Z là ¿).
Kết quả thuật toán:

Hình 4.23. Kết quả của thuật toán

Qua thuật toán trên ta có thể dễ dàng tìm được đường đi ngắn nhất từ điểm
bắt đầu đến điểm đích.

Hình 4.24. Hoạch định đường đi dùng thuật toán Dijkstra.

64
4.3.4. Thuật toán Dynamic Window Approach để tránh vật cản cho local
planner
Thuật toán Dynamic Window Approach (DWA) dùng để tìm ra một tín hiệu
điều khiển hợp lý gửi xuống robot nhằm mục đích điều khiển nó đến đích an toàn,
nhanh chóng dựa trên global planner đã hoạch định từ trước. Thuật toán này gồm
hai bước chính là cắt giảm không gian tìm kiếm (serarch space) của vận tốc và tìm
được vận tốc tối ưu trong không gian tìm kiếm đó.
Các vận tốc có thể điều khiển được trong không gian tìm kiếm được cắt giảm
theo ba bước sau:

 Quỹ đạo tròn: thuật toán DWA chỉ xét đến quỹ đạo là hình tròn (đường
cong) được xác định duy nhất bởi một cặp vận tốc thẳng và vận tốc xoay
(v, ω).

 Vận tốc cho phép: nhằm tạo ra một quỹ đạo an toàn cho robot để tránh
vật cản. Một cặp vận tốc (v, ω) được cho phép là khi robot có thể dừng
trước vật cản gần nhất mà không có sự va chạm trên đường cong tương
ứng với vận tốc đó. Vận tốc cho phép được định nghĩa như sau:

V a ={(v , ω)∨v ≤ √ 2. dist ( v , ω ) . v b ˄ ω ≤ √ 2. dist ( v , ω ) . ωb

Trong đó:

 V a là chuỗi các giá trị vận tốc ( v , ω ) cho phép robot dừng trước vật cản
mà không có sự va chạm.

 dist(v , ω)là khoảng cách nhỏ nhất mà robot dừng trước vật cản để không
có sự va chạm.

 v b , ω b là gia tốc của vận tốc thẳng và vận tốc xoay tối đa nếu robot di
chuyển sẽ gây va chạm với vật cản.

65
 Hình 4.25. Vận tốc cho phép V a trong DWA
Dynamic window: nhằm hạn chế vận tốc cho phép đối với những vận tốc có
thể đạt được trong khoảng chu kỳ cho trước với gia tốc tối đa của robot. Để ∆ t là
khoảng thời gian mà trong đó gia tốc v, sẽ được thực thi để ( v a , ω a) là vận tốc thực
được gửi xuống robot. Từ đó, vận tốc V a sẽ được định nghĩa như sau:
V d ={(v , ω)∨v ∈[v a−v̇ . ∆t , v a + v̇ . ∆t ]˄ ω ∈[ω ¿ ¿ a−ω̇ . ∆ t , ωb −ω̇ . ∆t ]¿

Hình 4.26. Vận tốc trong cửa sổ động V d trong DWA

Kết thúc ba bước trên thì ta tìm được không gian tìm kiếm
V r =V s ∩V a ∩V d

Tối ưu:
Ta có hàm mục tiêu được định nghĩa như sau:
G ( v , ω )=a . healding ( v , ω ) + βdist ( v , ω ) + γ . vel ( v , ω )

Để có thể tối ưu hóa vận tốc ngõ ra thì hàm mục tiêu phải có giá trị tối đa. Để
thực hiện được điều này, ta thực hiện các bước sau:

 Target heading: heading là giá trị đo tiến độ hướng đến đích của robot.
Giá trị sẽ mang giá trị tối đa khi robot di chuyển trực tiếp về phía đích.

66
 Giá trị của heading ( v , ω )được tính bởi công thức 180 – θ , với θ là góc
giữa hướng của robot và điểm đích.

Hình 4.27. Heading của robot trong DWA

 Không gian trống (clearance): hàm dist( v , ω )thể hiện khoảng cách tính từ
robot đến vật cản gần nhất nằm trên quỹ đạo cong của nó. Giá trị này sẽ
rất lớn nếu không có vật cản nằm trên quỹ đạo cong di chuyển của nó.
Giá trị này càng nhỏ thì việc nó đối mặt với vật cản càng cao, khi đó nó
sẽ di chuyển xung quanh vật cản ấy.

 Vận tốc: hàm vel( v , ω )là vận tốc di chuyển thẳng của robot và hỗ trợ di
chuyển nhanh hơn.

 Các hệ số a , β , γ được chọn sao cho phù hợp với đặc tính của robot và
môi trường hoạt động. Khi hàm mục tiêu có giá trị lớn nhất thì quỹ đạo
tối ưu sẽ được chọn với vận tốc ( v , ω )tốt nhất và là kết quả của thuật toán.

67
 Hình 4.28. Lưu đồ thuật toán của thuật toán DWA

CHƯƠNG 5: KẾT QUẢ THỰC NGHIỆM

Sau quá trình xây dựng mô hình Mobile robot, thiết lập môi trường hệ điều
hành ROS trên board nhúng, cấu hình và lập trình các node cần thiết cho quá trình
trong hệ thống thì chúng ta sẽ kiểm chứng thực nghiệm để cân chỉnh các thông số
và tìm ra các ưu điểm, khuyết điểm của hệ thống.
5.1. MÔI TRƯỜNG THỬ NGHIỆM

68
 Hình 5.1. Mô hình robot thử nghiệm
Để đánh giá độ chính xác và hoạt động của giải pháp đề xuất, hệ thống định
vị được tích hợp lên robot như hình 4. Thử nghiệm được tiến hành trong môi trường
trong nhà kín với diện tích 4,7 × 4,3 m 2, nền lát gạch, địa hình di chuyển phù hợp với
Robot đã thiết kế; nhóm bố trí thêm 1 thùng nhựa để làm vật cản. Để đảm bảo các
yếu tố bên ngoài như thời gian hoạt động, quãng đường di chuyển,… không ảnh
hưởng tới kết quả kiểm thử, với mỗi lần thử nghiệm, robot sẽ được khởi động lại và
di chuyển theo cùng một quỹ đạo được vạch ra trước với tốc độ di chuyển cố định.
Ảnh chụp căn phòng thực tế để thử nghiệm robot:

Hình 5.2. Phòng thử nghiệm robot

5.2. KẾT QUẢ ĐẠT ĐƯỢC
5.2.1. Quá trình mapping và kết quả bản đồ
Việc lập bản đồ sẽ thực hiện bằng cách đưa robot mang lidar đi quét căn
phòng, những nơi cần có robot đi qua. Những nơi mà không gian chưa xác định ta
có thể điều khiển robot từ xa để robot đi lập bản đồ.

69
Hình 5.3. Không gian thực tế để dựng bản đồ

Hình 5.4. Bắt đầu quá trình dựng bản đồ

70
 Hình 5.5. Hoàn thành bản đồ
Sau khi điều khiển robot di chuyển khắp phòng thì nhóm thu được bản đồ
như hình 9. Bản đồ thu được tại hình 9 là một tập hợp các đám mây điểm (point
cloud) do cảm biến Lidar quét và tạo ra. Đây là tập hợp vị trí các chướng ngại vật
(chấm màu đỏ) và vùng không gian trống trong không gian 2D. Đường màu đỏ thể
hiện cho đường biên (boundary) của phòng thí nghiệm, vùng màu trắng là vùng
robot có thể tự do di chuyển.
Bảng 5.1. So sánh bản đồ thu được và thực tế.

Bản đồ vẽ được Kích thước thực tế

Chiều dài phòng (m) 4,6 4,7

Chiều rộng phòng (m) 4,2 4,3

Chiều rộng cửa sau lưng (m) 0,7 0,75

Chiều rộng cửa bên trái (m) 0,75 0,8

Chiều rộng cửa bên phải (m) 0,8 0,85

71
5.2.2. Nhận xét và kết luận
Diện tích vùng quan sát được đo lại trên phần mềm, kết quả cho thấy toàn bộ
diện tích vùng quan sát là 4,6 × 4,2 m 2, có sai lệch so với diện tích thực của phòng.
5.3. Định vị robot trong bản đồ
AMCL đưa vào một bản đồ dựa trên laser có sẵn đã được thực hiện ở phần
mapping, quét laser và chuyển đổi thông báo, và kết quả đầu ra đưa ra các ước tính.
Khi khởi động, AMCL khởi tạo bộ lọc hạt của nó theo các thông số được cung cấp.
Lưu ý rằng, do mặc định, nếu không có tham số nào được đặt, trạng thái bộ lọc ban
đầu sẽ là một đám mây hạt (particlecloud) có kích thước vừa phải tập trung vào
khoảng (0,0,0). Các thông số cấu hình tham khảo trong mục Navigation. Các hạt
sau khi được tọa từ dữ liệu cảm biến, sẽ được gán cho một trọng số nhất định, sau
quá trình lấy mẫu lại, các hạt có trọng số cao hơn sẽ được giữ lại, các hạt có trọng
số thấp hơn sẽ bị loại trừ, từ đó hạt có trọng số lớn nhất sẽ được chỉ định là vị trí
hiện tại của robot. Quá trình này diễn ra liên tục mỗi khi có dữ liệu từ cảm biến trả
về, robot vừa định vị bản thân, vừa cập nhật bản đồ vào cơ sở dữ liệu tại cuối quá
trình. Hình dưới biểu diễn quá trình lấy mẫu hạt khi robot đang trong quy trình thiết
lập bản đồ, như trong hình, các hạt là các mũi tên đỏ sau quá trình lấy mẫu lại thì có
xu hướng hội tụ về vị trí của robot trong thực tế, trong trường hợp này, nếu robot
xoay quanh tại chỗ càng lâu, thì các hạt sẽ càng hội tụ, từ đó cho ra một tọa độ
chính xác nhất. Các mũi tên đỏ sẽ là tư thế có thể của robot trong bản đồ.

Hình 5.6. Sử dụng bộ lọc AMCL định vị robot

72
5.4. Điều hướng cho robot dùng thuật toán Dijsktra
5.4.1. Kết quả
Hoạch định đường đi đến đích cho robot và bắt đầu di chuyển.

Hình 5.7. Hoạch định đường đi cho robot

Di chuyển tới điểm đích theo hoạch định đường đi đã đặt ra trước đó.

Hình 5.8. Di chuyển tới điểm đích theo định hướng

5.4.2. Đánh giá kết quả
Robot đã di chuyển gần đến điểm đích mong muốn theo hoạch định đường đi
được đưa ra.Tuy nhiên,vẫn có một chút chênh lệch nhỏ giữa vị trí robot so với đích
đến và trong quá trình di chuyển robot di chuyển không được ổn định.

73
5.5. Tránh vật cản trên đường di chuyển dùng thuật toán DWA
5.5.1. Kết quả
Hoạch định đường đi ban đầu

Hình 5.9.Hoạch định đường đi ban đầu

Có vật cản xuất hiện robot đã hoạch định một đường đi mới để tránh vật cản

Hình 5.10. Vẽ đường đi mới khi có vật cản xuất hiện

74
 Đi đến điểm đích.

Hình 5.11. Robot đi đến điểm đích

5.5.2. Đánh giá kết quả
Phát hiện kịp thời vật cản để xử lý tạo ra được một hoạch đình đường đi mới
để tránh vật cản và di chuyển tới đích mong muốn.
Tuy nhiên vẫn có sự chênh lệch giứa vị trí robot và đích đến sau khi quá trình
kết thúc.
5.6. Những vấn đề phát sinh trong việc thực hiện đề tài
Các thiết bị thực nghiệm đề tài đa số là các thiết bị được dùng trong công
nghiệp song đó cũng 1 vài thiết bị ít hiện thực trong đời sống nên sẽ có việc sai số
so với thực tế.
Những lần đầu làm đề tài thì còn sai sót linh kiện, linh kiện hư và linh kiện
làm không đúng đề tài rất nhiều gây cản trở việc thực hiện và tốn kém tiền bạc.
Nhưng sau khi tìm hiểu kỹ càng thì nhóm đã tích hợp được những linh kiện
cần thiết để hoàn thành tốt đề tài.
Các thông số cho Robot ban đầu khi hoàn thành rất rối loạn và khó chọn lựa.
Tốn rất nhiều thời gian và ngày giờ để thực hiện và tìm ra được 1 thông số tạm gọi
là hoàn chỉnh cho robot.
Việc thiết kế form mẫu cho robot rất mất thời gian bởi vì nhóm phải học
thêm một phần mềm vẽ 2D mới và canh chuẩn kích thước cho Robot. Đặc biệt là

75
khâu làm hoàn thiện robot không biết nên lựa chọn vật liệu nào. Nên cuối cùng
nhóm chọn vật liệu có sẵn ở nhà là Gỗ. Bởi vì gỗ dễ khoang lỗ vít và thẩm mĩ.
Việc khó khăn nhất có lẻ là việc tìm kiếm tài liệu. Bởi vì đề tài này có quét
bản đồ và định vị trí nên những đề tài tương tự rất hiếm. Đa số chỉ là những bài báo
cáo có sẵn và từ đó thì phải dựng tạo nên từng các bước nối tiếp linh kiện.
Robot sẽ tạo bản đồ nhiễu nếu như tốc độ chạy và tốc độ quay quá nhanh. Để
tránh tình trạng robot bị đứng và nhận thức chậm chạp nên cách khắc phục là đặt
vận tốc chạy và quay bằng một hằng số nhất định và hoạt động chậm thì lúc đấy bản
đồ sẽ không bị chồng và nhiễu.

Hình 5.12. Bản đồ bị chồng hình

76
 CHƯƠNG 6. KẾT LUẬN
Qua thời gian nỗ lực tìm hiểu kết hợp với vốn tri thứ vốn có của bản thân,
cùng với sự giúp đỡ tận tình của ThS. Hồ Sỹ Phương nhóm chúng em đã hoàn thành
và đạt được mục tiêu đề ra ban đầu của đồ án tốt nghiệp này. Qua đó chúng em đã
hiểu sâu hơn về nền tảng lập trình cho robot ROS, đã được biết đến các thuật toán
lập bản đồ, tìm đường đi ngắn nhất và tự động tránh vật cản. Nhóm đã hoàn thành
việc xây dựng một robot di động có khả năng di chuyển đến đích và tránh được vật
cản với các kết quả sau:

 Nhóm đã hoàn thành thiết kế khung robot cùng với hệ thống mạch điều khển
giúp robot có thể di chuyển linh hoạt và ổn định. Tuy nhiên vẫn còn một số
hạn chế sau, do hệ thống chạy bằng nguồn pin nên khả năng đáp ứng điện áp
là không đáng kể. Dùng laptop làm bộ xử lý trung tâm có khả năng xử lý cao
và ổn định tuy nhiên việc dùng laptop làm cho khả năng di chuyển của robot
không được tối ưu nhất.

 Quá trình thu nhận thông tin và lập bản đồ của robot khá chính xác so với
thực tế. Do giải thuật vẽ bản đồ chưa được tối ưu nhất nên trong quá trình vẽ
bản đồ vẫn còn một số hạn chế như robot phải di chuyển chậm để lập bản đồ
chính xác nhất. Các môi trường có vật liệu phản chiếu hay xuyên thấu cũng
ảnh hướng tới khả năng tính toán khoảng cách tới các vật thể của robot.

 Trong quá trình di chuyển, khả năng tránh vật cản của robot là khá chính xác.
Khi gặp vật cản tĩnh hoặc di động robot có khả năng phản ứng thích hợp để
né các vật cản đó. Nhưng do phải đặt laptop nên khả năng di chuyển trong
không gian hẹp và có nhiều vật cản còn gặp nhiều khó khăn.
Dựa trên thành công của đồ án “Lập bản đồ và điều hướng chuyển động
tránh vật cản cho robot di động trên nền tảng hệ điều hành ros1” chúng em tin rằng
hệ thống sẽ được ứng dụng rộng rãi vào thực tiễn nhiều hơn. Để làm được điều đó
robot cần có những hướng phát triển thêm như:

 Thiết kế phần cứng phù hợp trong không gian chật hẹp, nhiều vật cản để
robot có thể vận chuyển hàng cho người dùng. Dùng một máy tính nhúng có
tính linh hoạt cao và kích thước nhỏ gọn như Raspberry hay Jetson nano.

 Tích hợp thêm IOT để điều khiển robot thông qua Internet. Đồng thời cần
phát triển thêm nhiều tính năng tương tác giữa robot với con người.

77
 Trong quá trình di chuyển lập bản đồ có thể tính toán và tăng tốc độ di
chuyển của robot để giữa được độ chính xác.

 Sử dụng camera và các cảm biến siêu âm để giúp rotbot tăng khả năng xác
định vật cản nhờ đó có thể tránh vật cản từ xa.

 Cải thiện thuật toán để tối ưu khả năng điều hướng cho robot.

78
 TÀI LIỆU THAM KHẢO
[1] https://tinyurl.com/2n3bzdvf, truy nhập lần cuối ngày 10/4/2023
[2] https://tinyurl.com/2nymh2mr, truy nhập lần cuối ngày 11/4/2023.
[3] https://tinyurl.com/2facp54b, truy nhập lần cuối ngày 13/4/2023.
[4] https://tinyurl.com/2llpqtvp, truy nhập lần cuối ngày 14/4/2023.
[5] https://tinyurl.com/2ntpvxnf, truy nhập lần cuối ngày 14/4/2023.
[6] https://tinyurl.com/2plth62e, truy nhập lần cuối ngày 22/4/2023.
[7] https://tinyurl.com/2hoopees, truy nhập lần cuối ngày 27/4/2023
[8] https://tinyurl.com/2q3l4eex, truy nhập lần cuối ngày 2/5/2023
[9] https://tinyurl.com/2nxzrsoq, truy nhập lần cuối ngày 4/5/2023
[10] https://tinyurl.com/2z5r9ucd, truy nhập lần cuối ngày 4/5/2023
[11] https://tinyurl.com/2n4gune4, truy nhập lần cuối ngày 5/5/2023
[12] https://tinyurl.com/2zava9aq, truy nhập lần cuối ngày 6/5/2023
[13] https://arxiv.org/pdf/2108.12571.pdf, truy nhập lần cuối ngày 8/5/2023
[14] https://tinyurl.com/2nrlvk9f, truy nhập lần cuối ngày 9/5/2023
[15] https://tinyurl.com/2zwsjxuj, truy nhập lần cuối ngày 9/5/2023

79
 PHỤ LỤC
Tất cả source code và thư mục của dự án:
https://github.com/vovanhop/DATN-MOBILE_ROBOT

80