Báo cáo dự án thiết kế mô phỏng robot tự hành với thuật toán slam, amcl, rrt trong matlab và ros

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI
VIỆN CƠ KHÍ
BỘ MƠN CƠ ĐIỆN TỬ

BÁO CÁO DỰ ÁN THIẾT KẾ

HỌC PHẦN: ROBOT TỰ HÀNH
MÃ HỌC PHẦN: ME4512
Sinh viên thực hiện:

Bùi Đức Huy

20184479

Phùng Minh Quang

20184592

Giảng viên hướng dẫn: TS. Mạc Thị Thoa
TS. Phạm Đức An
TS. Nguyễn Xuân Thuận


2


Đánh giá của giảng viên hướng dẫn
……………………………………………………………………………………
……………………………………………………………………………………
……………………………………………………………………………………
……………………………………………………………………………………

……………………………………………………………………………………
……………………………………………………………………………………
……………………………………………………………………………………
……………………………………………………………………………………
……………………………………………………………………………………
……………………………………………………………………………………
……………………………………………………………………………………
……………………………………………………………………………………
……………………………………………………………………………………
……………………………………………………………………………………
……………………………………………………………………………………
……………………………………………………………………………………
……………………………………………………………………………………
……………………………………………………………………………………
……………………………………………………………………………………
……………………………………………………………………………………
……………………………………………………………………………………
……………………………………………………………………………………
……………………………………………………………………………………
……………………………………………………………………………………

Kết quả đánh giá
Họ và tên

Điểm

Hà Nội, ngày 25 tháng 2 năm 2021
Giảng viên hướng dẫn

Bùi Đức Huy

Phùng Minh Quang

3


Lời cảm ơn
Được sự hướng dẫn và giúp đỡ nhiệt tình của 3 thầy cơ TS. Mạc Thị Thoa,TS.
Nguyễn Xn Thuần và TS. Phạm Đức An, chúng em đã hoàn thành được bài tập
lớn được giao “Mô phỏng Robot tự hành với thuật toán SLAM, AMCL, RRT
trong Matlab và ROS”. Bởi thời gian và kiến thức có giới hạn, sẽ khơng tránh
khỏi những sai sót ngồi ý muốn. Do vậy chúng em rất mong được sự chỉ bảo và
sự đóng góp ý kiến của thầy cơ để đề tài được hoàn thành.
Chúng em xin chân thành cảm ơn sự hướng dẫn của TS.Mạc Thị
Thoa,TS.Nguyễn Xuân Thuần và TS.Phạm Đức An đã giúp chúng em hoàn
thành đề tài.

4


MỤC LỤC
CHƯƠNG 1. GIỚI THIỆU CHUNG ................................................................. 1
1.1

Lý do chọn đề tài ........................................................................................ 1

1.2

Giới thiệu các sản phẩm liên quan ............................................................. 1
Kích thước và khối lượng ........................................................... 2
Thành phần cấu tạo TurtleBot3................................................... 3

1.3

ROS ............................................................................................................ 3
Mơ hình ROS .............................................................................. 4
Tầng ROS Filesystem ................................................................. 4
Tầng ROS Computation Graph ................................................... 5
Tầng ROS Community................................................................ 7

CHƯƠNG 2. CƠ SỞ LÝ THUYẾT VỀ ĐỘNG HỌC, SLAM,
NAVIGATION ..................................................................................................... 8
2.1

Động học Thuận ......................................................................................... 8

2.2

Động học nghịch ........................................................................................ 9

2.3

SLAM ....................................................................................................... 10

2.4

Costmap.................................................................................................... 11

2.5

DWA ........................................................................................................ 13

2.6

AMCL ...................................................................................................... 14

CHƯƠNG 3. KẾT QUẢ MÔ PHỎNG............................................................. 15
3.1

ĐỘNG HỌC ............................................................................................. 15

3.2

SLAM ....................................................................................................... 16

3.3

NAVIGATION ........................................................................................ 18

CHƯƠNG 4. TÀI LIỆU THAM KHẢO .......................................................... 19

5


DANH MỤC HÌNH VẼ
Hình 1 - TurtleBot3 Burger và TurtleBot3 Waffle Pi ............................................ 1
Hình 2 - Kích thước TurtleBot3-Burger ................................................................ 2
Hình 3 - TurtleBot3 Waffle Pi ............................................................................... 2
Hình 4 - Cấu tạo Turtlebot3 Burger ....................................................................... 3
Hình 5 - Mơ hình ROS Filesystem ......................................................................... 5
Hình 6 - Tầng ROS Computation Graph ............................................................... 5

Hình 7 - Giao tiếp giữa các nodes .......................................................................... 7
Hình 8 - Hệ toạ độ Robot ....................................................................................... 8
Hình 9 – Quan hệ giữa khoảng cách đến chướng ngại vật và giá trị Costmap .... 12
Hình 10 - Biểu diễn Costmap ............................................................................... 12
Hình 11 – Hệ toạ độ v - w .................................................................................... 13
Hình 12 – Vận tốc dài và vận tốc góc .................................................................. 14
Hình 13 – Q trình sử dungj AMCL .................................................................. 15
Hình 14 – Mơ phỏng trên Matlab ......................................................................... 15
Hình 15 – Mơ phỏng trên Simulink ..................................................................... 16
Hình 16 - Robot trong mơi trường gazebo ........................................................... 17
Hình 17 - Cảm biến lazer quét khi bắt đầu .......................................................... 17
Hình 18 - Bản đồ được lưu................................................................................... 17
Hình 19 - Dùng 2D pose đặt robot ở vị trí ngẫu nhiên ........................................ 18
Hình 20 - Robot di chuyển theo đường đi mà thuật tốn chọn ............................ 18
Hình 21 - Kết quả cuối cùng…………………………………………………….19

6


CHƯƠNG 1. GIỚI THIỆU CHUNG
1.1 Lý do chọn đề tài
1.2 Giới thiệu các sản phẩm liên quan
Khơng tìm thấy mục nhập nào của bảng hình minh họa.

Hình 1- TurtleBot3 Burger và TurtleBot3 Waffle Pi

TurtleBot là một robot nền tảng tiêu chuẩn ROS. Turtle có nguồn gốc từ robot
Turtle, được điều khiển bởi ngơn ngữ lập trình máy tính giáo dục Logo vào
năm 1967. Ngoài ra, xuất hiện lần đầu trong hướng dẫn cơ bản của ROS, là
một chương trình bắt chước hệ thống lệnh của Logo rùa chương trình . Nó

cũng được sử dụng để tạo biểu tượng Rùa như một biểu tượng của ROS. Chín
chấm được sử dụng trong logo ROS có nguồn gốc từ mai sau của con
rùa. TurtleBot, có nguồn gốc từ Logo con rùa, được thiết kế để dễ dàng dạy
những người mới làm quen với ROS thơng qua TurtleBot cũng như dạy ngơn
ngữ lập trình máy tính sử dụng Logo. Kể từ đó TurtleBot đã trở thành nền
tảng tiêu chuẩn của ROS, là nền tảng phổ biến nhất giữa các nhà phát triển và
sinh viên.
TurtleBot3 là một robot di động nhỏ, giá cả phải chăng, có thể lập trình, dựa
trên ROS để sử dụng trong giáo dục, nghiên cứu, sở thích và tạo mẫu sản
phẩm. Mục tiêu của TurtleBot3 là giảm đáng kể kích thước của nền tảng và
hạ giá thành mà không phải hy sinh chức năng và chất lượng của nó, đồng
thời cung cấp khả năng mở rộng. TurtleBot3 có thể được tùy chỉnh theo
nhiều cách khác nhau tùy thuộc vào cách bạn tái tạo các bộ phận cơ khí và sử
dụng các bộ phận tùy chọn như máy tính và cảm biến. Ngoài ra, TurtleBot3
được phát triển với SBC hiệu quả về chi phí và kích thước nhỏ, phù hợp với
hệ thống nhúng mạnh mẽ, cảm biến khoảng cách 360 độ và công nghệ in 3D.
Công nghệ cốt lõi của TurtleBot3 là SLAM , Điều hướng và Thao tác , làm
cho nó phù hợp với các robot dịch vụ gia đình. TurtleBot có thể chạy các
thuật tốn SLAM (bản địa hóa và lập bản đồ đồng thời) để xây dựng bản đồ
và có thể lái xe xung quanh phịng của bạn. Ngồi ra, nó có thể được điều
khiển từ xa từ máy tính xách tay, bàn di chuột hoặc điện thoại thơng minh
chạy Android. TurtleBot cũng có thể theo dõi chân của một người khi họ đi


bộ trong phịng. Ngồi ra, TurtleBot3 có thể được sử dụng như một trình điều
khiển di động có khả năng điều khiển một đối tượng bằng cách gắn một trình
điều khiển như OpenMANIPULATOR. OpenMANIPULATOR _có ưu điểm
là tương thích với Waffle TurtleBot3 và Waffle Pi. Thơng qua khả năng
tương thích này có thể bù đắp cho sự thiếu tự do và có thể có sự hồn thiện
hơn như một robot dịch vụ với SLAM và khả năng điều hướng mà

TurtleBot3 có.
Kích thước và khối lượng

Hình 2: Kích thước TurtleBot3-Burger

Hình 3-TurtleBot3 Waffle Pi

2


Thành phần cấu tạo TurtleBot3

Hình 4 Cấu tạo Turtlebot3 Waffle

1.3 ROS
ROS là một hệ thống framework dùng cho các ứng dụng robot. Về cơ
bản, ROS có những đặc tính thiết yếu của một hệ điều hành như khả năng
thực hiện các tác vụ (task) song song, giao tiếp, trao đổi dữ liệu với nhau giữa
các tác vụ, quản lý dữ liệu… ROS còn được phát triển riêng biệt về các thư
viện, công cụ hỗ trợ việc vẽ đồ thị, lưu dữ liệu, hiển thị model 3D của robot…
ROS được phát triển dưới dạng module. Từ đó một nhóm nghiên cứu có thể
chỉ chuyên sâu vào một lĩnh vực như vẽ bản đồ hay định vị robot vẫn có thể
xây dựng một hệ thống robot hoàn chỉnh khi tái sử dụng những module đã
được phát triển trước đó. 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 cung cấp nhiều module giúp người dùng tuỳ biến cho phù hợp với
yêu cầu phát triển từng loại robot.
• Những module hoạt động độc lập với nhau và trao đổi thơng qua các gói
dữ liệu. Vì vậy dễ phát hiện và sửa lỗi cho từng module.
3

• ROS có mã nguồn mở được cộng động nghiên cứu về lĩnh vực robot phát
triển. Lượng người dùng lớn giúp cho hệ thống ROS ngày càng đa dạng và
phong phú về tính năng và dữ liệu. Những lỗi phát sinh trong quá trình sử dụng
cũng được cộng đồng người dùng hỗ trợ xử lý nhanh chóng.
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 Linix. Phần mềm cho ROS chủ
yếu được thử nghiệm trên Ubuntu và Mac OS X. Cho đến nay cộng đồng
ROS cũng đã bắt đầu xây dựng cho Fedora, Gentoo, Arch Linux và các nền
tảng Linux khác. ROS chưa hỗ trợ trên Microsoft Windows.
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.
Mơ hình ROS
Mơ hình ROS được chia làm ba tầng: Filesystem, Computation Graph, và
Community.
Tầng ROS Filesystem
Filesystem là nguồn tài nguyên ROS được lưu trữ trên bộ nhớ hệ thống, bao
gồm những thành phần như:
•

Packages: chứa các lệnh thực thi của ROS (các nodes), một thư viện phụ
thuộc ROS, tập dữ liệu, các file cấu hình, hoặc các dữ liệu cần thiết khác
trong hệ thống.

•

Manifests (manifests.xml): cung cấp cơ sở dữ liệu về một package, bao
gồm license, cờ 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 (vd: Navigation stack).

•

Stack Manifests (stack.xml): cung cấp cơ sở dữ liệu về một stack, bao
gồm license và các thơng số phụ thuộc vào những stack khác.

•

Message(msg): là cấu trúc dữ liệu cho việc truyền nhận trong ROS.
4


•

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.

Hình 5 -Mơ hình ROS Filesystem

Tầng ROS Computation Graph

Hình 6-Tầng ROS Computation Graph

“Computation Graph”, gọi là lược đồ tính tốn, là một mạng peer-topeer của ROS trong đó các dữ liệu được xử lý với nhau. Computation Graph
cơ bản gồm các thành phần: Nodes, Master, Parameter Server, Messages,
Services, Topics, và Bags, tất cả các thành phần này đều cung cấp dữ liệu cho
Graph bằng những phương thức khác nhau.
•

Nodes: là đơn vị thực hiện các lệnh tính tốn, xử lý dữ liệu. Hệ điều
hành ROS được thiết kế mơ-đun hố, các mơ-đun được xây dựng rất
5


chi tiết và chuyên biệt, một hệ thống điều khiển robot thường sẽ bao
gồm nhiều nodes. Ví dụ, một node điều khiển hệ thống cảm biến, một
node điều khiển động cơ bánh xe, một node thực hiện việc tác vụ định
vị, một node hoạch định đường đi, một node vẽ quỹ đạo của hệ
thống… Mỗi node ROS được xây dựng nhờ sử dụng một ROS client
library, chẳng hạn như roscpp hoặc rospy.
•

Master: cung cấp tên đăng ký và tra cứu đến các node cịn lại. Nếu
khơng có Master, các node sẽ khơng thể tìm thấy nhau, trao đổi thơng
tin, hay gọi services đều khơng thực hiện được.

•

Parameter Server: là một phần của Master.

•

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

•

Topics: Messages được định tuyến thơng qua một hệ thống trao đổi
(transport system), trong đó phân loại các thông điệp thành 2 dạng:
publish (đưa tin) và subcribe (đă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ủ đề). Tên
của topic được dùng để quy định nội dung của message.

•

Service: thực hiện các yêu cầu tương tác theo kiểu request/reply.

•

Bags: là một định dạng để lưu và phát lại dữ liệu từ các ROS
messages. Bags là một cơ chế quan trọng để lưu trữ dữ liệu, chẳng hạn
như dữ liệu cảm biến.

6


Hình 7 Giao tiếp giữa các nodes

Tầng ROS Community
Tầng này được định nghĩa là tài nguyên về phần mềm và kiến thức được
chia sẽ bởi cộng đồng phát triển ROS bao gồm: Distributions, Respositories,

The ROS Wiki, Mailing lists.

7


CHƯƠNG 2. CƠ SỞ LÝ THUYẾT VỀ ĐỘNG HỌC, SLAM,
NAVIGATION

2.1 Động học Thuận

Hình 8 Hệ toạ độ robot

Xét 2 hệ qui chiếu cố định I và hệ qui chiếu gắn liền với robot R
Vị trí của robot trong hệ qui chiếu cố định
x
 I   y 
 

Ma trận cosin chỉ hướng
cos( )  sin( ) 0 
R( )   sin( ) cos( ) 0 
 0
0
1 

Ma trận vận tốc của robot trong hệ qui chiếu R
 r1 r2 
 2  2 



R  
0

 r r 
2
 1

2l 
 2l

Ma trận vận tốc của robot trong hệ qui chiếu cố định
8


 r1 r2 

 2  2  cos( ) 







r

r




I  R( )R    1  2  sin( ) 
2 
 2



r1  r2



2l
2l



Phương pháp Euler
 y '(t )  f (t , y(t ))

Cho  y(t0 )  y0
=>
t  t  h
 n 1 n

yn 1  yn  hf (tn , yn )

Áp dụng phương pháp Euler, ta xác định được vị trí bất kì của robot trong
hệ qui chiếu cố định tại thời điểm ti+1 thơng qua vị trí tại thời điểm ti


 r1 r2 

 xi  (ti 1  ti )  2  2  cos(i ) 



 xi 1  




r

r



 I   yi 1    yi  (ti 1  ti )  1  2  sin(i ) 
2 
 2

i 1  




r


r

 1

2 

 i  (ti 1  ti ) 
 
2l  
 2l


2.2 Động học nghịch
q 

Xét 1 điểm Q trên thân robot có tọa độ theo hệ quy chiếu R là  x 
q


y



Tọa độ của điểm Q theo hệ qui chiếu cố định I
 xQI   xPI  cos( )  sin( )   qx 
 I  I
   với
 yQ   yP   sin( ) cos( )   q y 
 xQI 
 I  là tọa độ điểm Q theo hệ qui chiếu I
 yQ 
 xPI 
 I  là tọa độ điểm P theo hệ qui chiếu I
 yP 

Đạo hàm 2 vế ta có:
 xQI   xPI    sin( )  cos( )   qx 
 I I 
 

 yQ   y P    cos( )  sin( )   q y 

9


 xPI  v cos( ) 
 xQI  cos( ) qx sin( )  qy cos( )  v 
 I 

 I   
   với  y P    v sin( ) 
sin(

)
q
cos(

)

q
sin(

)


y
x
y
 Q  
  
     
 
v 

 cos( )  qx sin( )  q y cos( ) 
qx cos( )  q y sin( ) 


   
sin( )

 

1

 xQI 
 I
 yQ 



 qy 
 qy 
 cos( )    sin( ) sin( )    cos( )  I
v 

 qx 
 qx 
  xQ 
    
  y I 
1
1
  
  Q 
   sin( )
  cos( )


 qx 
 qx 

2.3 SLAM
Simultaneous localization and mapping (SLAM) là bài tốn tính tốn xây dựng hoặc cập nhật bản
đồ của một môi trường không xác định đồng thời theo dõi vị trí của tác nhân bên trong nó. Mặc dù
ban đầu đây có vẻ là một bài tốn con gà và quả trứng nhưng có một số thuật tốn được biết đến để
giải nó, ít nhất là khoảng thời gian có thể giải quyết được cho một số mơi trường nhất định. Các
phương pháp giải gần đúng phổ biến bao gồm bộ lọc hạt, bộ lọc Kalman mở rộng , giao điểm hiệp
phương sai và GraphSLAM . Các thuật tốn SLAM dựa trên các khái niệm trong hình học tính
tốnvà thị giác máy tính , và được sử dụng trong điều hướng robot , lập bản đồ robot và đo
đường cho thực tế ảo hoặc thực tế tăng cường .

Một số thuật toán SLAM: Gmapping, Kalman filter,…
 Gmapping:
Particle filter là thuật toán phổ biến nhất trong việc object tracking. Ví
dụ điển hình là thuật tốn Monte Carlo localization sử dụng particle filter. Bộ

lọc Kalman trước đây chỉ đảm bảo độ chính xác cho một hệ thống tuyến tính
và một hệ thống mà nhiễu gassian được áp dụng. Hầu hết các vấn đề trong thế
giới thực là hêj thống phi tuyến.
Bởi vì robot và cảm biến cũng là phi tuyến, Particle filter thường được
sử dụng để pose estimation. Nếu bộ lọc Kalman là một phương pháp phân
tích giả định hệ thống là tuyến tính có các tham số là tuyến tính, Particle filter
là một kỹ thuật để dự đốn thơng qua mô phỏng bằng phương pháp thử và sai.
Particle filter được đặt tên đó vì giá trị ước tính được tạo ra bởi phân bố xác
suất trong hệ thống được biểu diễn như 1 particle. Đây được gọi là phương
pháp Sequential Monte Carlo.
Particle filter giống như các thuật toán pose estimation khác, ước tính
pose của vât thể với giả định rằng sai số đưuocj bao gốm trong input. Khi sử
dụng SLAM, giá trị đo lường của robot và giá trị đo bằng cảm biến khoảng
cách được sử dụng để ước tính pose hiện tại của robot.
Trong phương pháp Particle filter, vị trí khơng chắc chắn được mơ tả
bằng một loạt các particle được gọi là mẫu. Chúng tôi di chuyển các hạt đến
vị trí và hướng ước tính mới dựa trên mơ hình chuyển động và xác suất của
10


robot, đồng thời đo trọng lượng của từng particle theo giá trị đo thực tế và
giảm dần nhiễu để ước tính tư thế chính xác. Trong trường hợp rơ bốt di
động, mỗi hạt được biểu diễn dưới dạng hạt = pose (x, y, i), trọng lượng và
mỗi hạt là một particle tùy ý đại diện cho vị trí ước tính và hướng của rô bốt
được biểu thị bằng x, y, và i của robot và độ ảnh hưởng của mỗi particle.
Bộ lọc này trải qua 5 quy trình sau đây. Ngoại trừ lần khởi chạy ở bước
1, các bước 2 ~ 5 được thực hiện lặp đi lặp lại để ước tính tư thế của rơ bốt.
Nói cách khác, đó là phương pháp ước tính tư thế của robot bằng cách cập
nhật sự phân bố của các particle thể hiện xác suất của robot trên mặt phẳng
tọa độ X, Y dựa trên giá trị cảm biến đo được.

1. Initialization
Vì tư thế ban đầu của robot (vị trí, hướng) khơng xác định, các
particle được sắp xếp ngẫu nhiên trong phạm vi mà tư thế có thể thu
được với N particle. Mỗi hạt ban đầu nặng 1 / N, và tổng trọng lượng
của các hạt là 1. N được xác định theo kinh nghiệm, thường là hàng
trăm. Nếu vị trí ban đầu được biết, các hạt được đặt gần robot.
2.
3.
4.
5.

Prediction
Update
Pose estimation
Resampling

2.4 Costmap
Tư thế của robot được ước tính dựa trên odemetry thu được từ encoder và
IMU. Và khoảng cách giữa robot và chướng ngại vật có được nhờ cảm biến
khoảng cách gắn trên robot. Tư thế của rô bốt và cảm biến, thông tin về
chướng ngại vật và bản đồ lưới thu được từ SLAM được sử dụng để tải bản
đồ và tối ưu các khu vực có chướng ngại vật, khu vực trống và khu vực
không xác định để điều hướng.
Trong Navigation, costmap tính tốn khu vực chướng ngại vật, khu vực có
thể va chạm và khu vực có thể di chuyển của rơ bốt dựa trên bốn yếu tố nói
trên. Tùy thuộc vào loại điều hướng,costmap có thể được chia thành hai loại.
Một là ‘global_costmap’, thiết lập một kế hoạch đường đi để điều hướng
trong khu vực toàn cầu bản đồ. Loại còn lại là ‘local_costmap’ được sử dụng
để lập kế hoạch đường đi và tránh chướng ngại vật trong khu vực giới hạn
xung quanh rô bốt. Mặc dù mục đích của chúng khác nhau, nhưng cả hai

costmap đều được biểu diễn theo cùng một cách.
Costmap được biểu diễn dưới dạng giá tị từu 0 đến 255. Các giá trị này
được tóm tắt như sau:






000: Khu vưcj robot có thể di chuyển hoàn toàn tự do
001 – 127: Khu vực có khả năng va chạm thấp
128 – 252: Khu vwcj có khả năng va chạm cao
253 – 254: Khu vực chắc chắn xảy ra va chạm
255: Khu vực có chướng ngại vật, robot knoong thể di chuyển
11


Hình 9: Quan hệ giữa khoảng cách đến chướng ngại vật và giá trị costmap

Hình 10: Biểu diễn Costmap

12


2.5 DWA
Dynamic Window Approach (DWA) là một phương pháp phổ biến để lập
kế hoạch tránh chướng ngại vật và tránh chướng ngại vật. Đây là một
phương pháp lựa chọn tốc độ có thể nhanh chóng đến điểm mục tiêu trong
khi tránh các chướng ngại vật có thể va chạm với robot trong khơng gian tìm
kiếm vận tốc. Trong ROS, Trajectory planner được sử dụng để lập kế hoạch

đường đi cục bộ, nhưng DWA đang được thay thế vì hiệu suất vượt trội của
nó.
Đầu tiên, Robot khơng ở trong hệ trục toạ đọ x-y, thay vào đó nó sẽ ở
trọng hệ toạ độ với 2 trục là vận tốc dài và vận tốc góc. Trong hệ toạ độ này
robot có giá trị tối đa của vận tốcbởi vì có hạn chế về phần cứng, gọi là
Dynamic Window

Trong Dynamic Window, Hàm G(v,w) được sử dụng để tính v,w. Giá trị
này sẽ được tối ưu hố để robot có thể tránh được các chướng ngại vật và
đến với địch nhanh nhất có thể.

Hình 11: Hệ toạ độ v-w

13


Hình 12: Vận tốc dài và vận tốc góc

2.6 AMCL
Mục đích của thuật tốn này là để xác định xem robot hiện đang ở đâu
trong bản đồ cho trước. Tức là phải tìm pose của robot. Vì mục đích đó,
thuật tốn sẽ tính tốn xác suất những nơi robot có thẻ đang ở đó. Đầu
tiên, vị trí và hướng (x, y, θ) của rô bốt tại thời điểm t được ký hiệu là xt
và thông tin khoảng cách thu được từ cảm biến khoảng cách cho đến thời
điểm t được ký hiệu là z0t = {z0, z1 , ..., zt}, và thông tin chuyển động thu
được từ encoder cho đến thời điểm t là u0t = {u0, u1, ..., ut}. Sau đó,
chúng ta có thể tính tốn niềm tin (xác suất hậu kỳ sử dụng cơng thức
Bayes) với phương trình sau:

Trong bước dự đốn, vị trí bel '(xt) của robot tại khung thời gian tiếp

theo được tính bằng mơ hình chuyển động p (xt | xt-1, ut-1) của robot và
xác suất bel (xt-1) ở vị trí trước đó và thơng tin chuyển động mà u nhận
được từ encoder:

Sau đây là bước cập nhật. Lần này, mơ hình cảm biến p (zt | xt), xác
suất bel (xt) và hằng số chuẩn hóa eta (ht) được sử dụng để thu được xác
suất chính xác hơn bel '(xt) dựa trên thơng tin cảm biến:

Tiếp theo là quá trình SIR(Sampling Importance weighting Resampling):

14


Hình 13: Quá trình sử dụng AMCL

CHƯƠNG 3. KẾT QUẢ MƠ PHỎNG
3.1 ĐỘNG HỌC

Hình 14: Mơ phỏng trên Matlab

15


Hình 15: Mơ phỏng trên Simulink

3.2 SLAM
1. Khởi tạo mơi trường mô phỏng
export TURTLEBOT3_MODEL=burger
roslaunch turtlebot3_gazebo turtlebot3_world.launch
2. Chạy Slam node

Roslaunch turtlebot3_slam turtlebot3_slam.launch slam_methods:= gmapping
3. Khởi tạo Node để điều khiển
roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
4. Lưu bản đồ
rosrun map_server map_saver -f ~/map

16


Hình 16 - Robot trong mơi trường gazebo

Hình 17 - Cảm biến lazer quét khi bắt đầu

Hình 18- Bản đồ được lưu

17


3.3 NAVIGATION
1. Khởi chạy robot và môi trường
export TURTLEBOT3_MODEL=Burger
roslaunch turtlebot3_gazebo turtlebot3_world.launch
2. Chạy Navigation node
roslaunch turtlebot3_navigation turtlebot3_navigation.launch
map_file:=$HOME/map.yaml
3. Khởi tạo node điều khiển
roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch

Hình 19 - Dùng 2D pose đặt robot ở vị trí ngẫu nhiên

Hình 20-Robot di chuyển theo đường đi mà thuật toán chọn

18


Hình 21: Kết quả cuối cùng

CHƯƠNG 4. TÀI LIỆU THAM KHẢO
1.
2.
3.
4.

Slide Robot tự hành
/> />Download the 'ROS Robot Programming' Book for Free! - Learn Robotics
- Robotsource

19