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

Hà Nội, ngày 25 tháng 2 năm 2021
Điểm

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 peerto-peer 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
y
I

Ma trận cosin chỉ hướng
cos( )
R( ) sin( )

cos( )
0

0

Ma trận vận tốc của robot trong hệ qui chiếu R
r

2
R

r

1

2
0

r
1

2l

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

2


123docz

8


r

r

1

2

2

2

r

r

cos( )

2

1

2

IR()R

2

r

sin( )

r

1

2l

2

2l

Phương pháp Euler
y '(t ) f (t , y (t ))

Cho y (t 0 ) y0

t

n 1

t

n

=>

yn 1 yn hf (tn , yn )

h

Á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

x

x (t

i 1

y
I

i

t
i 1

r

)

r

1

i

2

i 1
r

i

(t

ti )

i 1

2
r

1

2

ti)

)

2

2

r
i(t i 1

)

cos(

i

i 1

y

2

sin(

i

r
2

1

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
xI
y

xI
y

Q

Q
I

xI

cos( )
yP

sin( )

Q
I

là tọa độ điểm Q theo hệ qui chiếu I

I

là tọa độ điểm P theo hệ qui chiếu I

Q

x

P

sin( )qx

PI

cos( )

q

y

với


yPI

Đạo hàm 2 vế ta có:
q

x

I

I

x

Q
I

P

sin( )

cos( )

I

q
y

Q

y

cos( )

sin( )

P

9


I

v cos( )

x

P

q x sin( ) qy cos( ) v

y

I
với y

q x cos( ) qy sin( )
vcos( )
sin( )

q

sin( ) q
x

cos( )

1

y
y

q x cos( ) qy sin( )

q

v

q

Q
I

q
y

sin( )

sin( )
q

x

1

cos( )

x

sin( )
x

xQI
yI

1
q

vsin( )

Q

y

cos( )

xI

P

Q

q

cos( )
x

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 tố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