báo cáo đồ án ii đề tài slam trong mobile robot

Bạn đang xem bản rút gọn của tài liệu. Xem và tải ngay bản đầy đủ của tài liệu tại đây (5.8 MB, 32 trang )

Trang 1<div class="page_container" data-page="1">

ĐẠI HỌC BÁCH KHOA HÀ NỘI TRƯỜNG ĐIỆN – ĐIỆN TỬ

BÁO CÁO ĐỒ ÁN II

Đề tài: SLAM trong MOBILE ROBOT

Sinh viên th c hi n: Nguyự ệ ễn Văn TrungMSSV: 20185933

Lớp: CTTN - ĐKTĐ K63

Giảng viên hướng d n: TS.ẫ Dương Minh Đức

</div>Trang 2<div class="page_container" data-page="2">

Lời nói đầu

Mobile Robot là Robot có khả năng tự di chuyển, tự vận động theo một quỹ đạo được xác định trước hoặc chưa biết trước và được thực hiện một công việc được giao. Robot có thể hoạt động ở nhiều mơi trường khác nhau như trên không, trên mặt đất, dưới nước hay thậm chí là ngồi vũ trụ. Mobile Robot có ứng dụng to lớn trên nhiều lĩnh vực từ dân sự, quân sự, vũ trụ và đang được nghiên c u phát tri n r ng rãi t các cơng ty l n, nh ng phịng ứ ể ộ ừ ớ ữnghiên c u khứ ở ắp nơi thế ớ gi i. M t trong nh ng y u t quan tr ng cộ ữ ế ố ọ ủa Robot là có th t ể ựgiải quyết các vấn đề ằ n m ngồi dự tính như khả năng nhận diện, phát hiện và tránh vật cản hoặc đi vào môi trường chưa biết

Để Robot có th t định hướng và xác định vị trí hi n t i, ta c n ph i mô ph ng l i b n ể ự ệ ạ ầ ả ỏ ạ ảđồ kho ng không gian di chuy n c a Robot vi c này ta g i là Mapping. T việc có được ả ể ủ – ệ ọ ừbản đồ của khoảng không gian di chuyển, ta cần phải xác định vị trí của nó trong khoảng không gian ấy – vi c này g i là Localization. Sau nh ng công vi c trên, ta s ệ ọ ữ ệ ẽ xác định đường đi cho Robot và điều khiển để Robot đi theo đúng quỹ đạo của đường đi mà ta sẽ vẽ - việc này g i là path-planning. Bài toán cho 2 vọ ấn đề Localization và Mapping g i là bài toán ọSimultaneous Localization and Mapping SLAM. –

Bài báo cáo được chia thành 4 ph n. ầ

Phần 1 gi i thiớ ệ ổu t ng quan v tài. ề đề

Phần 2 trình bày bài tốn SLAM và các thu t toán ậ SLAM như: Gmapping, Fast-Slam và RTAB-Map.

Phần 3 trình bày cơ bản về ề ả n n t ng Robot Operating System (Ros).

Phần 4 mô phỏng các thu t toán Gmapping, Fast-Slam, RTAB-Map trên Ros. ậEm xin gửi lời cảm ơn đến TS. Dương Minh Đức, phó trưởng bộ mơn Tự Động Hóa Cơng Nghiệp, Trường Điện Điện tử, Đại Học Bách Khoa Hà Nội đã hướng dẫn, cung cấp - tài liệu và tạo điều kiện hết mức để em hoàn thành báo cáo này. Đồ án của em chưa nghiên cứu kĩ nên chắc khơng tránh khỏi những thiếu xót. Em mong được thầy chỉ bảo thêm để giúp em hoàn thiện hơn nữa.

Em xin chân thành cảm ơn thầy!

</div>Trang 3<div class="page_container" data-page="3">

2.2.1. Monte Carlo Localization (MCL) ... 9

2.2.2. Thuật toán MCL tăng cường (Augmented MCL) ... 10

3.1.2. ROS Computation Graph Level ... 31

3.1.3. ROS Community Level ... 32

3.2. Môi trường mô phỏng trong ROS ... 33

3.2.1. Gazebo ... 33

3.2.2. Rviz ... 33

CHƯƠNG 4: MÔ PHỎNG ... 35

</div>Trang 4<div class="page_container" data-page="4">

DANH M C VIỤ ẾT TẮT

</div>Trang 5<div class="page_container" data-page="5">

CHƯƠNG 1. ỔNG QUAN ĐỀT TÀI

SLAM (Simulatanous Localization and Mapping) là thu t toán th c hi n ậ ự ệ đồng th i nh ờ địvị và xây dựng bản đồ.

Bài toán SLAM được định nghĩa như sau:

Đặt Robot vào 1 môi trường không xác định. Robot sẽ di chuyển trong môi trường đấy và c m nhả ận môi trường thông qua các c m biả ến đượcc đặt trên Robot. Bài toán SLAM là bài tốn xây d ng bự ản đồ mơi trường đồng thời xác định v trí cị ủa Robot so v i bớ ản đồ này.

Để Robot có th tự định hướng và xác định vị trí hi n t i, ta c n ph i mô ph ng l i b n ể ệ ạ ầ ả ỏ ạ ảđồ kho ng không gian di chuy n c a Robot vi c này ta g i là Mapping. T vi c có ả ể ủ – ệ ọ ừ ệđược bản đồ ủa kho ng không gian di chuy n, ta c n ph c ả ể ầ ải xác định vị trí c a nó trong ủkhoảng khơng gian ấy – việc này gọi là Localization. Sau những công việc trên, ta sẽ xác định đường đi cho Robot và điều khiển để Robot đi theo đúng quỹ đạo của đường đi mà ta s v - vi c này g i là Path-planning. Bài toán cho 2 vẽ ẽ ệ ọ ấn đề Localization và Mapping g i là bài toán SLAM. ọ

Robot s v a di chuyẽ ừ ển và xác định được nó đang ở đâu và khơng gian xung quanh nó

Hình 1: M i quan h gi a các bài toán trong Mobile Robot ố ệ ữ

</div>Trang 6<div class="page_container" data-page="6">

• Tư thế xt được đặc trưng bởi 2 bi n v trí trên khơng gian 2D và góc quay cế ị ủa Robot. Sau đấy chuỗi các tư thế lại thành 1 tập Xt={ , ... }x x1 2 xt

• utlà đại lượng đặc trưng cho chuyển động giữa thời gian t - 1 và thời gian t; dữ liệu đó có thể được lấy từ bộ mã hóa bánh xe của rô bốt hoặc từ các bộ điều khiển được cung cấp cho các động cơ đó. Sau đó, chuỗi các dữ liệ ạu l i thành Ut={ , ... }u u1 2 ut

đặc trưng cho chuyển động của Robot

•

m

là bản đồ ủa mơi trường. Mơi trườ c ng có thể bao gồm các điểm mốc, vật thể… và

m

mơ t v trí c a chúng. Bả ị ủ ản đồ môi trường

m

thường được giả định là b t bi n ấ ếtheo th i gian ờ

• ztlà phép đo của các c m bi n. ả ế Các phép đo của rô b t thi t lố ế ập các tính năng thơng tin được tính bằng

m

và v trí rơ bị ốt Xt. Chuỗi các phép đo được đưa ra như sau:

12{z , ... }

Bây gi bài toán Slam tr thành bài tốn xây dờ ở ựng và tính tốn các đại lượng chưa biết gồm bản đồ môi trường m và tư thế ủa Robot Xt. c

Ý tưởng chính: Bài tốn SLAM tính tốn x p xấ ỉ trạng thái xtvà bản đồ ủa môi trườ c ng

m

khi bi t các dế ữ liệu v tín hiề ệu điều khi n ể u1:tvà dữ liệu c m bi n ả ế z1:t trong t p thậ ời gian r i rờ ạc: p m x z( , t| 1: 1t−,u1:t)

Phần ti p theo của đồ án sẽ trình bày chi ti t vế ế ề SLAM g m các b l c, thu t toán ồ ộ ọ ậLocalization, Mapping, SLAM, Path-Planning.

</div>Trang 7<div class="page_container" data-page="7">

CHƯƠNG 2: SLAM

2.1. Các b lộ ọc cơ bản

2.1.1. B l c Extended Kalman Filter ộ ọ

Trong th c t , h u h t hàm chuy n ự ế ầ ế ể trạng thái và phép đo của Robot u là các hàm phi đềtuyến.

Đầu tiên, chúng ta gi s hàm chuy n tr ng thái và ả ử ể ạ phép đo là các hàm phi tuyến xác định bởi công thức:

</div>Trang 8<div class="page_container" data-page="8">

Đầu ra tr v kì v ng và ma tr n hiả ề ọ ậ ệp phương sai của trạng thái

x

t.

2.1.2. B l c Particle Filter ộ ọ

Ý tưởng chính của bộ lọc Particle là đại diện cho các trạng thái dự báo bởi một tập hợp các m u tr ng thái ng u nhiên rẫ ạ ẫ ồi sau đó tính tồn và loại bỏ d n các Particle có xác suầ ất thấp.

Trong b lộ ọc Particle, các m u có phân b ẫ ố posterior được g i là Particle: ọ

</div>Trang 9<div class="page_container" data-page="9">

Localization là vấn đề xác định tư thế củ Robot trong 1 bản đồ môi trường nhất định.a Localization có thể đc xem là vấn đề chuyển đổi giữa các hệ tọa độ. Bản đồ môi trường đc mô tả trong hệ tọa độ global, độc lập hoàn toàn với tư thế củ Robot. Localization là a quá trình thiết lập sự tương ứng giữa hệ tọa độ bản đồ và hệ tọa độ cục bộ của Robot. Biết được phép biến đổi tọa độ này cho phép Robot ể hiện vị trí của các đối tượng quan thtâm trong khung tọa độ của chính nó — điều kiện tiên quyết cần thiết cho việc điều hướng Robot.

Khó khăn cơ bản của Localization là Robot khơng thể đo trực tiếp tư thế của nó (𝑥,𝑦,𝜃)𝑇

từ 1 cảm biến mà phải tích hợp dữ liệu của nhiều cảm biến qua thời gian để ước lượng tư thế của nó.

Thuật tốn Localization đặc trưng nhất có thể kể đến là thuật toán Monte Carlo Localization dựa trên bộ lọc Particle Filter.

2.2.1. Monte Carlo Localization (MCL)

MCL còn gọi là bản địa hóa của bộ lọc Particle.

Thuật tốn đưa ra bản đồ của mơi trường và sẽ dự đốn hướng, vị trí củ Robot khi di a chuyển.

Đồng thời bước lấy mẫu sẽ sử dụng mơ hình chuyển động của Robot. Tính hệ số weight bằng mơ hình đo lường.

</div>Trang 10<div class="page_container" data-page="10">

❖ Ví dụ:

Hình 6: Ví d thu t tốn MCL ụ ậ

2.2.2. Thuật toán MCL tăng cường (Augmented MCL)

*Vấn đề: Nếu chẳng may ở bước lấy mẫu lại chúng ta vô tình loại bỏ các Particle gần vị trí thực củ Robot hoặc vị trí thực củ Robot bị thay đổi đột ngột (kidnapped)?a a

Giải pháp: thêm các Particle ngẫu nhiên vào tập hợp Particle

*Thuật toán:

</div>Trang 11<div class="page_container" data-page="11">

Hình 7: Thuật tốn MCL tăng cường

Trích mẫu từ mơ hình chuyển động. Tính trọng số weight từ mơ hình đo lường.

Tính tốn weight trung bình và weight slow, weight fast.

Mấu chốt của MCL tăng cường nằ ở dịng 13: Trong q trình lấy mẫu lại, thuật toán m sẽ thêm vào 1 mẫu ngẫu nhiên nếu Particle có xác suất nằm trong khoảng cho bởi cơng thức trong thuật tốn. Nếu khơng sẽ thực hiện lấy mẫu như MCL.

*Ví d : ụ

Hình trịn đỏ đại diện cho trung bình của các Particle. Hình trịn trắng là vi trí thực của Robot.

</div>Trang 13<div class="page_container" data-page="13">

Hình h là k t qu c a b lế ả ủ ộ ọc MCL tăng cường, ước tính v trí trung bình các Particle và ịvị trí thực của Robot đã trùng nhau trên bản đồ.

RBPF s d ng Sampling Importannce Resampling (SIR) gử ụ ồm các bước sau:

-Trích mẫu: M i t p các particle ỗ ậ {x( )i} được t o ra t ạ ừ ( )1

{xi−} b ng cách trích m u t mơ hình ằ ẫ ừchuyển động

-Tính tr ng sọ ố: M i tr ng s ỗ ọ ố ( )i

w được tính theo cơng th c: ứ( )

( )1:1:1: 1( )1:1:1: 1

-Tái trích mẫu: Các particle được trích m u l i theo tr ng sẫ ạ ọ ố. Sau bước này, tr ng s c a các ọ ố ủparticle m i s b ng nhau. ớ ẽ ằ

-Ước lượng bản đồ: V i mớ ỗi particle, xác suất ước lượng bản đồ ( )( )1:1:

tính d a vào qu o các particle ự ỹ đạ ( )1:

x và t p d ậ ữ liệu c m bi n ả ế z1:t.

2.4. SLAM

SLAM được chia ra làm 2 d ng: ạ

• Online Slam: x p x ấ ỉ trạng thái t c th i cùng v i bứ ờ ớ ản đồ môi trường

</div>Trang 14<div class="page_container" data-page="14">

• Full Slam: x p x tồn bấ ỉ ộ trạng thái của Robot trong t p th i gian cùng b n ậ ờ ảđồ môi trường:

1:1: 11:( , t| t , t)

Với Full Slam thì chúng ta có thể hình dung được quỹ đạo của Robot.

Phần ti p theo cế ủa đồ án s trình bày 2 thuẽ ật tốn đặc trưng của Online Slam và Full Slam là Gmapping và FastSLam. C 2 thuả ật toán này đều phát tri n t b l c Particle Filter. ể ừ ộ ọ

2.4.1. Gmapping

Gmapping là thu t toán Online Slam d a trên thu t toán Rao-Blackwellized Particle Filter ậ ự ậ(RBPF) cũng sử ụng các bướ d c SIR:

- Trích m u: M i t p các particle ẫ ỗ ậ {x( )i} được t o ra t ạ ừ ( )1

{xi−} b ng cách trích m u t mơ ằ ẫ ừhình chuyển động

- Tính tr ng s : M i tr ng s ọ ố ỗ ọ ố ( )i

w được tính theo cơng th c: ứ( )

( )1:1:1: 1( )1:1:1: 1

- Tái trích mẫu: Các particle được trích m u l i theo tr ng sẫ ạ ọ ố. Sau bước này, tr ng s cọ ố ủa các particle m i s b ng nhau. ớ ẽ ằ

</div>Trang 15<div class="page_container" data-page="15">

- Ước lượng bản đồ: V i m i particle, xác suớ ỗ ất ước lượng bản đồp m( |z1:t,x1:t) sẽ được tính d a vào qu o các particle ự ỹ đạ ( )

( )1:1:1: 1

(

( )

) (

1:( ) 1: 1

) (

1

(

( ) ( )1

) ()

11: 1( ) 1: 11: 2

)

1:1:1: 1

động, ta được:

(

( )

) (

( )( )

)

1:1: 1111( )

|

i

,,

i

|

i

,

( )

=

</div>Trang 16<div class="page_container" data-page="16">

sánh s ẽ tăng lên. Điều này làm cho việc đóng vịng l p m t nhi u th i gian. RTAB-Map ặ ấ ề ờ được tối ưu hóa cho SLAM quy mơ lớn (large-scale) và dài h n (long-term) b ng cách s d ng ạ ằ ử ụnhiều phương pháp tối ưu để cho phép thực hiện việc đóng vịng lặp trong thời gian thực. Q trình đóng vịng lặp phải di n rễ a đủ nhanh để có th ể thu được kết quả trước khi thu được các hình nh camera ti p theo. ả ế

Các bước xử lý của RTAB-Map:

2.6.1. GrapSlam

GraphSLAM là một thuật toán SLAM đầy đủ. Cốt lõi c a GraphSLAM là tủ ối ưu hóa đồ thị (Graph optimization). Không giống như FastSLAM, sử dụng các Particle để ước tính tư thếcó kh ả năng xảy ra nh t cấ ủa robot, GraphSLAM làm vi c vệ ới t t c d u cùng mấ ả ữ liệ ột lúc để tìm ra giải pháp tối ưu.

GraphSlam được mơ tả như sau:

Giả ử s chúng ta được cung cấp một tập hợp các phép đo

z

1:t v i các biớ ến tương ứng

c

1:tvà một t p h p các dậ ợ ữ liệu điều khiển

u

1:t. GraphSLAM bi n nh ng dế ữ ữ liệu này thành bi u ểđồ.

Hình dưới có 5 tư thế

x

0

−x

4đại diện cho 5 tư thế t i 5 thạ ời điểm

t

0

−t

4 và 2 feature

m m

1

,

2 Các tư thế và feature được nối v i nhau b i các ràng bu c (constraint), các ràng bu c là các ớ ở ộ ộhàm phi tuy n b c 2 ế ậ

• 2 tư thế được nối v i nhau bớ ằng ràng bu c chuyộ ển động (motion constraint), trên hình là các nét li n. ề

</div>Trang 17<div class="page_container" data-page="17">

Hàm m c tiêu c a GraphSlam là t ng c a các ràng bu c và thuụ ủ ổ ủ ộ ật toán s t i thi u hóa hàm ẽ ố ểmục tiêu để ối ưu hóa bản đồ và đường đi củ t a Robot.

</div>Trang 18<div class="page_container" data-page="18">

Hình 15: Ma tr n thông tin, vecto thông tin trong Graph-SLAM ậ

Sau khi điền đầy đủ, có thể tính được vecto kì vọng. Vecto kì vọng được xác định bởi các tư thế và feature nên nó chính là ước tính tốt nhất cho các tư thế.

Loạ ỏ ếi b bi n

Loại b bi n có th ỏ ế ể được áp d ng lụ ặp đi lặ ại để lo i b t t c các ràng bu c theo chu k . p l ạ ỏ ấ ả ộ ỳĐiều này có th được thực hi n b ng cách lo i b hoàn toàn biể ệ ằ ạ ỏ ến sau đó điều chỉnh các liên k t hi n có ho c thêm các liên k t mế ệ ặ ế ới để phù h p v i nh ng liên k t s b ợ ớ ữ ế ẽ ị loạ ỏi b . Quá trình này được thể hiện trong hình sau:

Hình 16: Ma trận, vecto thông tin khi lo i b feature m1 ạ ỏ

</div>Trang 19<div class="page_container" data-page="19">

Ở đây sẽ loại b feature ỏ

m

1. Trong quá trình này, ma tr n thơng tin s ậ ẽ có năm ô được đặt lại về 0 (được biểu th bị ằng màu đỏ) và bốn ô s ẽ được điều ch nh giá tr cỉ ị ủa chúng (được biểu th bị ằng màu xanh lá cây) để phù h p vợ ới vi c loệ ạ ỏ ếi b bi n còn l i các ô khác gi ạ ữnguyên giá tr . ị Tương tự, vectơ thơng tin sẽ có m t ô b ộ ị loạ ỏ và hai ô được điều chỉnh.i b Quá trình này được l p lặ ại cho t t c các feature và cu i cùng, ma ấ ả ố trận được xác định trên tất c ả các tư thế c a robot. T i thủ ạ ời điểm này, quy trình tương tự như trước đây có thể được áp d ng, ụ

Ràng bu c phi tuy n ộ ế

Trong th c t , các ràng bu c h u hự ế ộ ầ ết đều là các hàm phi tuyến. Do đó, các ràng buộc chuyển động và đo lường phải được tuyến tính hóa trước khi chúng có thể được thêm vào ma trận thơng tin và vectơ.

Tuyến tính hóa các ràng buộc đo lường và chuyển động theo Taylor b c nhậ ất:

Quy trình làm vi c cho GraphSLAM v i các ràng bu c phi tuyệ ớ ộ ến được tóm tắt dưới đây:• Thu th p d u, t o biậ ữ liệ ạ ểu đồ ề v các ràng bu c ộ

• Tuyến tính hóa tất cả các ràng buộc và thêm các ràng buộc tuyến tính hóa vào ma trận thơng tin và vectơ thơng tin

• Tính vecto kì v ng: ọ

=

−1

</div>Trang 20<div class="page_container" data-page="20">

độ không ch c chắ ắn liên quan đến v trí c a Robot. Lo i ti p c n này không thành công n u ị ủ ạ ế ậ ếvị trí ước tính khơng chính xác.

Trong phương pháp tiếp cận vịng lặp tồn cầu, một vị trí mới được so sánh v i các v trí ớ ịđã xem trước đó. Nếu khơng tìm th y k t qu phù h p, v trí m i s ấ ế ả ợ ị ớ ẽ được thêm vào b nhộ ớ. Khi robot di chuy n xung quanh và bể ản đồ phát triển, lượng thời gian để kiểm tra các vị trí mới v i nh ng v ớ ữ ị trí đã thấy trước đó sẽ tăng tuyến tính. N u th i gian c n thiế ờ ầ ết để tìm kiếm và so sánh hình nh m i v i hình nh ả ớ ớ ả được lưu trong bộ nh lớ ớn hơn thời gian thu th p, ậbản đồ sẽ trở nên vô hiệu.

RTAB-Map s d ng viử ụ ệc đóng vịng lặp tồn c c cùng v i các k thuụ ớ ỹ ật khác để đảm bảo rằng q trình đóng vịng lặp diễn ra trong thời gian thực.

Ví dụ: hình bên trái, nơi đóng vịng lặp b t t, b n s ị ắ ạ ẽ thấy được đánh dấu nơi cánh cửa được thể hiện dưới dạng nhi u góc và nhi u ph n c a m t cánh cề ề ầ ủ ộ ửa, trong đó ở bên ph i, bả ạn nhìn th y m t cánh c a duy nhấ ộ ử ất được xác định rõ ràng.

</div>Trang 21<div class="page_container" data-page="21">

Hình 18: T m quan tr ng cầ ọ ủa đóng vòng lặp

2.6.3. Qu n lý b ả ộ nhớ

RTAB-Map s d ng k thu t qu n lý b nh gi i h n s ử ụ ỹ ậ ả ộ ớ để ớ ạ ố lượng vị trí được coi là ứng c ửviên trong quá trình phát hiện đóng vịng lặp. K thu t này là mỹ ậ ột tính năng chính của RTAB-Map và cho phép đóng vịng lặp được th c hi n trong th i gian thự ệ ờ ực.

</div>Trang 22<div class="page_container" data-page="22">

• Khi có được một hình ảnh mới, một node mới sẽ được tạo trong Bộ nhớ ngắn hạn (STM).

• Khi t o m t node, hãy nh l i rạ ộ ớ ạ ằng các tính năng được trích xuất và so sánh với từ vựng để tìm tất cả các từ trong hình ảnh, tạo một bag-of-word cho node này. • Các node đượ ấn địc nh trọng lượng trong STM dựa trên thời gian Robot đã ở tại vị

trí đó trong đó thời gian dài hơn có nghĩa là trọng lượng cao hơn. Nế- u hai hình ảnh liên ti p gi ng nhau, tr ng s c a node th nhế ố ọ ố ủ ứ ất tăng lên một và khơng có node mới nào được tạo cho hình ảnh thứ hai.

• STM có kích thước cố định là S. Khi STM đến các node S, node cũ nhất được chuyển đến WM để được xem xét phát hiện đóng vịng.

• Việc đóng vịng lặp xảy ra trong WM.

• Kích thước WM phụ thuộc vào giới hạn thời gian cố định T. Khi thời gian cần thiết để ử x lý d li u mữ ệ ới đạt đến T, mộ ốt s node c a biểu đồ ủ được chuy n t WM sang ể ừLTM - k t qu ế ả là kích thước WM được gi gữ ần như khơng đổi.

• Các node cũ nhất và ít trọng số hơn trong WM được chuyển sang LTM trước các node khác, vì vậy WM đượ ạc t o thành t ừ các node được nhìn thấy trong khoảng thời gian dài hơn.

• LTM khơng được sử dụng để phát hiện đóng vịng lặp và tối ưu hóa đồ thị. • Nếu việc đóng vịng lặp được phát hiện, các hàng xóm trong LTM của một node cũ

có th ể được chuy n tr l i WM (mể ở ạ ột quá trình được g i là truy xu t). ọ ấ

</div>