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.. Bài toán SLAM là bài toá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ớ ả
SLAM
Các b l ộ ọc cơ bả n
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:
= + Ý tưởng chính làm cơ sở cho bộ lọc EKF là tuyến tính hóa Chúng ta sẽ sử dụng công thức Taylor b c nhậ ất để tuy n tính hóa: ế
Từ đây, chúng ta cũng sẽ rút ra được các công th c sau: ứ
• Xác su t chuy n tr ng thái: ấ ể ạ
*Thu ậ t toán: Đầ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 toà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: ọ
Trong đó: x t k là Particle i di n cho 1 gi thuy t tr ng thái t i thđạ ệ ả ế ạ ạ ời điểm t
Dữ liệu vào: Trạng thái trước (tập h p mợ ẫu Particle trước), tín hiệu điều khi n, d ể ữliệu cả biến
Hình 4: Thu t toán Particle Filter ậ Các bước thực hiện bộ lọc:
Trích mẫu từ mô hình chuyển động
Tính trọng số weight của từng particle
Tạo tập Particle mới từ ọng số weight đã tính ở bước trướtr c
Dữ liệu ra: Trạng thái sau (tập h p mợ ẫu Particle sau)
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 th tâ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 toá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
Hình 5: Thu t toán MCL ậ MCL còn gọi là bản địa hóa của bộ lọc Particle
Thuật toán đưa ra bản đồ của môi trường và sẽ dự đoá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
Hình 6: Ví d thu t toá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
Hình 7: Thuật toá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 toá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 quá 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 toán Nếu không sẽ thực hiện lấy mẫu như MCL
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
Hình 8: Ví d thuụ ật toán MCL tăng cường
4 ảnh đầu tiên là s d ng MCL ử ụ
Hình e, xảy ra trường h p Robot b t ng b ợ ấ ờ ị thay đổ ịi v trí (Robot b b t cóc) ị ắ
Hình f và g s dử ụng MCL tăng cường t o ng u nhiên các Particle trên bạ ẫ ản đồ
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 đồ.
Mapping
Mapping là vấn đề tích h p thông tin thu thợ ập được v i các c m bi n cớ ả ế ủa Robot thành một tập d ữliệu
Thuật toán Mapping đặc trưng nhất có thể kể đến Rao-Blackwellized Particle Filter (RBPF) Các bước của thuật toán thực hiện như sau: Ý tưởng chính của RBPF là ước lượng xác suất p x(1: t ,m z| 1: t ,u 1: 1 t − ) bằng ước lượng quỹ đạo chuyển động của Robot p x(1: t |z 1: t ,u 1: 1 t − ) và tính toán bản đồ ự d a vào qu o ỹ đạ p m z( | 1: t ,x 1: t )
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 t ( ) i } được t o ra t ạ ừ{x t ( ) i − 1 } 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 ỗ ọ ốw t ( ) i được tính theo công th c: ứ
-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 đồ p m( ( ) i |z 1: t ,x 1: ( ) i t ) sẽ được tính d a vào qu o các particle ự ỹ đạ 1: ( ) i x t và t p d ậ ữliệu c m bi n ả ế z 1:t
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
• Full Slam: x p x toàn bấ ỉ ộ trạng thái của Robot trong t p th i gian cùng b n ậ ờ ả đồ môi trường:
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 toá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 ể ừ ộ ọ
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 t ( ) i } được t o ra t ạ ừ{x t ( ) i − 1 } 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 ọ ố ỗ ọ ốw t ( ) i được tính theo công th c: ứ
- 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 đồp m( ( ) i |z 1: t ,x 1: ( ) i t ) sẽ được tính d a vào qu o các particle ự ỹ đạ ( ) 1: i x t và t p d ậ ữliệu c m bi n ả ế z 1:t
Ta có phân ph i gi thuy t có d ng: ố ả ế ạ
Trọng s ốweight được tính theo công thức:
= , thành mô hình chuy n ể động, ta được:
− − − 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 Quá 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:
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:t và 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 ề
• 1 tư thế n i v i 1 feature b ng ràng buố ớ ằ ộc đo lường (measurement constraint), trên hình là các nét đứt
0 0 0 x T x là ràng buộc ban đầu
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
Hình 14: Biểu đồ Graph-SLAM Để giải phương trình tuyến tính, GraphSlam sử ụ d ng ma tr n thông tin ậ và vecto thông tin để lưu trữ thông tin v các ràng buề ộc.
Ràng bu c chuyộ ển động, đo lường được điền vào 4 ô trong ma tr n thông tin và 2 ô trong ậ vecto thông tin
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 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 ạ ỏ Ở đâ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, ụ
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:
Các ràng bu c phi tuy n có th ộ ế ể được tuy n tính hóa b ng cách s d ng công th c tuy n ế ằ ử ụ ứ ế tính Taylor b c nhậ ất, nhưng sẽ gây ra sai s ố Để ả gi m sai s này, vi c tuy n tính hóa mố ệ ế ọi ràng bu c ph i di n ra càng g n càng t t v i v trí th c cộ ả ễ ầ ố ớ ị ự ủa tư thế hoặc phép đo liên quan đến ràng bu c ộ Đểthực hiện điều này, m t gi i pháp l p lộ ả ặ ại đượ ử ụng, trong đó điểm c s d tuyến tính hóa được cải thiện với mỗi lần lặp lại Sau nhiều lần lặp lại, kết quả, , tr μ ở thành một ước tính h p ợ lý hơn nhiều cho v trí th c c a t t c ị ự ủ ấ ả các tư thế và tính năng của robot
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 ậ ữliệu, t o biạ ể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
2.6.2 Đóng vòng lặp (Loop Closures) Đóng vòng lặp là quá trình tìm kiếm sự phù hợp giữa vị trí hiện tại và vị trí đã truy cập độ 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 toà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 toàn c c cùng v i các k thuụ ớ ỹ ật khác để đảm bảo rằng quá trình đóng vòng lặp diễn ra trong thời gian thực
Hình 17: Đóng vòng lặp trong RTAB-Map
Tầm quan trọng của việc đóng vòng lặp được hi u rõ nh t b ng cách xem k t qu bể ấ ằ ế ả ản đồ mà không có nó
Khi tính năng đóng vòng lặp bị vô hiệu hóa, bạn có thể thấy các phần của đầu ra bản đồ đượ ặ ạc l p l i và bản đồ kết qu trông có v l n xả ẻ ộ ộn hơn rất nhi u Nó không ph i là mề ả ột đại diện chính xác của môi trường Điều này là do Robot không sử dụng tính năng đóng vòng lặp để so sánh hình ảnh và v trí m i v i nh ng hình nh và vị ớ ớ ữ ả ị trí đã được xem trước đó, thay vào đó, nó đăng ký chúng dưới dạng v trí mị ới Khi tính năng đóng vòng lặp được bật, bản đồ sẽ mượt mà hơn đáng kể và là sự thể ệ hi n chính xác của căn phòng.
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
Hình 18: T m quan tr ng cầ ọ ủa đóng vòng lặp
RTAB-Map
Kiến trúc ROS có ba c p khái ni m: Filesystem, Computation Graph và Community ấ ệ
• Cấp th nh t 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
• Cấp th hai ứ –Computation Graph: nơi giao tiếp giữa các quá trình và hệ thống Trong c p khái ni m này, ta s ph i thi t l p hấ ệ ẽ ả ế ậ ệ thống, qu n lý các quá trình, giao tiả ếp giữa nhiều máy tính v i nhau, ớ
• Cấp th ba Community: giứ – ải thích/ hướng d n các công c và các khái niẫ ụ ệm để chia s ẻ kiến thức, thuật toán chương trình từ bất kỳ nhà phát triển nào Đây là cấp độ quan tr ng ọ vì nó ảnh hưởng đến s phát tri n l n m nh c a cự ể ớ ạ ủ ộng đồng ROS
Filesystem ch y u là các nguủ ế ồn tài nguyên ROS được th c thi trên b nh ự ộ ớ lưu trữ h ệthống, bao gồm:
Hình 20: C u trúc tấ ầng thư mục trong ROS
• Packages: là đơn vị chính để ổ chứ t c phần mềm trên ROS M t package có ộ thể chứa các nodes (ROS runtime processes), các thư viện đặc thù của ROS, các file cài đặt hoặc bất cứ file nào cho việc tổ chức
Mục đích của package là để tạo ra t p hậ ợp chương trình có kích thước nh ỏnhất để có thể ễ d dàng sử d ng l ụ ại.
ROS
Môi trường mô phỏng trong ROS
3.2 Môi trường mô ph ng trong ROS ỏ
Gazebo là trình mô ph ng 3D cung cỏ ấp Robot, c m biả ến, mô hình môi trường để mô ph ng ỏ Robot và cung c p mô ph ng th c t v i công c v t lý c a nó Gazebo là m t trong nh ng ấ ỏ ự ế ớ ụ ậ ủ ộ ữ trình mô ph ng ph bi n nh t cho mã ngu n, nó là m t trình mô ph ng r t ph bi n trong ỏ ổ ế ấ ồ ộ ỏ ấ ổ ế lĩnh vực robot vì hi u suệ ất cao Hơn nữa, Gazebo được phát tri n và phân ph i b i Open ể ố ở Robotics ph ụ trách ROS nên tương thích với ROS
Gazebo có những đặc điểm sau:
• Mô phỏng động, v i quy n truy c p vào nhi u công c v t lý hi u su t cao bao gớ ề ậ ề ụ ậ ệ ấ ồm ODE, Bullet, Simbody và DART
• Đồ h a 3D nâng cao, b ng cách s d ng OGRE, có th tọ ằ ử ụ ể ạo môi trường và hi n th các ể ị hình d ng, k t c u, ánh sáng, bóng t i th c t ạ ế ấ ố ự ế
• Cảm bi n và ti ng n, bạn có th nhế ế ồ ể ận được dữ liệu r t thú vị ấ
• bổ sung, để các nhà phát triển có thể tùy chỉnh Robot, cảm biến và môi trường điều khiển nh API cờ ủa nó
• Nhiều mô hình robot, bao g m PR2, Pioneer2 DX, iRobot Create, TurtleBot ho c xây ồ ặ dựng của riêng bạn bằng cách sử dụng SDF
Rviz vi t t t c a ROS Visualization, là m t công cế ắ ủ ộ ụ trực quan hóa 3D cho các ng d ng ứ ụROS Nó cung c p chấ ế độ xem mô hình Robot, thu th p thông tin c m bi n t c m biậ ả ế ừ ả ến Robot và phát l i d ạ ữliệu đã thu thập Nó có th hi n th d ể ể ị ữliệ ừu t máy ảnh, tia laser, t các ừ thiết b 3D và 2D ị được cung cấp
MÔ PHỎNG
4.1 Mô phỏng thuật toán Gmapping trên n n t ng Ros ề ả Đối tượng mô phỏng: Robot TurtleBot3
Hình 26: Mô hình Robot TurtleBot 3
Môi trường mô phỏng: Stage_3 trên Ros
Hình 28: Bản đồ 2D khi s d ng Gmapping ử ụ Điều hướng Robot thành công
4.2 Mô phỏng thuật toán RTAB-Map trên n n t ng Ros ề ả
Môi trường mô phỏng: Gazebo
Hình 29: Môi trường mô ph ng thu t toán RTAB-Map ỏ ậ
Hình 30: Bản đồ 2D khi s d ng RTAB-Map ử ụRTAB-Map 3D:
