đồ án ii tự động tạo lập bản đồ và định vị cho mobile robot 4 bánh mecanum

Cùng với đó,khi xét đến những ứng dụng rộng lớn hơn, khả năng tạo dựng bản đồ xungquanh mà không cần đến sự can thiệp của con người giúp giảm bớt nguồnlực, nguy hiểm cũng là một vấn đề đ

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

Lời mở đầu

Ngày nay, chúng ta đang tiến tới một xã hội công nghiệp hóa hiện đại, tiệnnghi Đóng vai trò lớn trong cách mạng công nghệ hiện nay không thể khôngnhắc tới sự phát triển của mobile robot Đặc biệt, với các ứng dụng kho bãi,nhà máy, nơi có sự xuất hiện của nhiều hàng hóa máy móc, sự linh động, khảnăng thay đổi trạng thái của mobile robot là rất quan trọng Cùng với đó,khi xét đến những ứng dụng rộng lớn hơn, khả năng tạo dựng bản đồ xungquanh mà không cần đến sự can thiệp của con người giúp giảm bớt nguồnlực, nguy hiểm cũng là một vấn đề đang nhận được rất nhiều sự quan tâm.Nhận biết được các vấn đề quan trọng trên, nhóm chúng em gồm 2 sinh viênđã thực hiện đề tài : “Tự động tạo lập map và định vị cho mobile robot 4bánh mecanum” Đồ án của chúng em được mô phỏng kiểm chứng bằng môphỏng trên ROS – một phần mềm chuyên dụng cho lập trình robot.

Qua đây, chúng em xin cảm ơn thầy Nguyễn Mạnh Linh đã luôn định hướng,giúp đỡ cũng như giải đáp các thắc mắc của chúng em trong suốt quá trìnhthực hiện, tạo điều kiện cho chúng em hoàn thành đồ án này.

Mục lục

1.1 Lý thuyết xác suất: các khái niệm và ký hiệu được sử dụng 3

1.2 Lập bản đồ lưới chiếm dụng 4

1.3 Thuật toán SLAM 8

1.4 Thuật toán explore_lite 9

1.5 Mô hình động học mobile robot 11

2 Ứng dụng thuật toán SLAM dựa trên ROS 152.1 Giới thiệu cơ bản về ROS 15

2.2 Các package phụ thuộc trong ROS 17

2.2.1 Package explore_lite 17

2.2.2 Package gmapping 18

2.3 Kết quả mô phỏng SLAM trên phần mềm Gazebo, Rviz 19

Thuật toán cho phép thực hiện quá trình trên được gọi là thuật toán SLAM(Simultaneous Localization and Mapping)và đây là chủ đề chính của chươngnày Dựa theo chương 1,8,9,13 trong sách Probabilistic Robotics [1].

1.1Lý thuyết xác suất: các khái niệm và ký hiệu được sửdụng

Các ký hiệu khác được sử dụng trong báo cáo như sau:

robot bị giới hạn trong môi trường phẳng, pose (vị trí và hướng) thườngđược đưa ra bởi ba biến số, hai tọa độ vị trí của nó trong mặt phẳng vàhướng di chuyển của nó (yaw) Pose của robot thường được gọi là trạngthái động học.

môi trường Nếu các phép đo được thực hiện trong khoảng thời gian từ

mobile robot, một ví dụ điển hình về dữ liệu điều khiển là vận tốc củarobot.

Dữ liệu điều khiển và đo lường thường bị nhiễu bởi một số tác nhân gây nhiễu.

p(x xt|0:t−1,z1:t−1 1:t,u ) (1.1)Nếu trạng thái hoàn tất, tức là kiến thức về các trạng thái, đo lường hoặckiểm soát trong quá khứ không cung cấp thêm thông tin để giúp dự đoántương lai, thì phân phối xác suất trở thành:

p(xt|x0:t−1,z1:t−1 1:t,u )= (x xp t| t−1,ut) (1.2)

Khái niệm quan trọng cuối cùng được báo cáo trong sách Probabilistic Roboticslà niềm tin Từ quan điểm xác suất, niềm tin được thể hiện dưới dạng phânphối xác suất có điều kiện, tức là xác suất sau đối với các biến trạng thái dựatrên dữ liệu có sẵn:

bel(xt)=p(xt|z1:t,u1:t) (1.3)

Đây là tất cả các công cụ được sử dụng để giải thích việc hình thức hóa toánhọc của các thuật toán được triển khai trong thực tế để khiến robot xây dựngbản đồ trong khi định vị chính nó trong đó.

1.2Lập bản đồ lưới chiếm dụng

Lập bản đồ là vấn đề tạo ra bản đồ về môi trường mà robot đang ở từ dữ liệuđo lường không chắc chắn và với giả định rằng pose của robot đã biết.Các thuật toán xử lý loại vấn đề này được gọi là thuật toán lập bản đồ.Các thuật toán này có thể dựa trên các tính năng, tức là các đối tượng có thểnhận dạng được trong môi trường hoặc trên lưới chiếm chỗ, trong đó bản đồđược biểu thị dưới dạng lưới có khoảng cách đều nhau Trong báo cáo này,chỉ có cách tiếp cận dựa trên lưới sẽ được thảo luận.

Mỗi lưới có thể tương ứng với một chướng ngại vật hoặc một không giantrống, một giá trị nhị phân được gán tương ứng Các thuật toán lập bản đồlưới chiếm dụng tính toán các ước tính gần đúng về sau cho các biến ngẫunhiên này.

Phía sau trên các bản đồ được cung cấp dữ liệu là

trong đó m là bản đồ.

Bản đồ lưới chiếm dụng phân vùng không gian thành các ô lưới:

Vì số lượng lưới mô tả bản đồ theo thứ tự hàng chục nghìn, phân vùng lưới

(1.4) cho mỗi bản đồ đơn lẻ là không khả thi.

Do đó, thuật toán đơn giản hóa vấn đề bằng cách ước tính riêng

Công cụ tìm phạm vi laser phát ra tín hiệu 2 chiều và ghi lại tín hiệu dộitừ chướng ngại vật Tín hiệu phát ra là một chùm ánh sáng tập trung Quá

phép đối phó với độ không chắc chắn trong mô hình cảm biến Các tham số

hai được thể hiện bằng độ (Hình 1.1) Sự tồn tại của một tham số độ phângiải được chứng minh bằng thực tế là các công cụ tạo ra toàn bộ các phạm

zt= z1t, ,zN

Hình 1.1: Sơ đồ tham số hóa của công cụ tìm phạm vi laser

được coi là một phiên bản thích nghi của Bayes’ filter:

Thuật toán 1.1: Thuật toán lưới chiếm dụng, pseudo code

chặn sự không ổn định về số lượng cho các xác suất gần đến 0 hoặc 1:

lt,i= log p(mi|z1:t,x1:t)

1 − p(mi|z1:t,x1:t) (1.9)Thuật toán 1.1 lặp qua tất cả các ô và chỉ cập nhật những thứ quan tâm bởi

nón của cảm biến Đối với các ô này, giá trị chiếm dụng được cập nhật theohàm inverse_sensor_model:

inverse sensor model_ _ (mi,x ,zt t)=log p(mi|z1 − p(mt,xt)

l0= logp(mi=1)p(mi=0)= log

1 − p(mi) (1.11)Một cách để thực hiện mô hình đo lường nghịch đảo là Thuật toán 1.2

Thuật toán 1.2: Hàm mô hình cảm biến nghịch đảo, pseudo code

Hình 1.2: Sơ đồ gán giá trị chiếm dụng cho một ô

Thuật toán 1.2 hoạt động theo các giả định rằng pose của robot được đưa rabởi xt=[ ; y; θ]x tvà cảm biến được tập trung vào robot Ở nơi đầu tiên, trong

tâm của ô đến vị trí của robot, được tính theo dòng 4 Khoảng cách này là

phạm vi của ô Trong dòng 5, định hướng của khoảng cách như vậy đối với

Thuật toán lưới chiếm dụng được trình bày trong phần này phụ thuộc vàosự phân tách mạnh mẽ của phương trình (1.7) Giả định này nhằm đơn giảnhóa việc trình bày ánh xạ lưới chiếm dụng, nhưng trong thực tế, các dẫn xuấtnâng cao hơn của thuật toán này được sử dụng.

Hình 1.3: Bản đồ thu được với lập bảnd đồ lưới chiếm dụng (phải) từ dữliệu cảm biến thô (trái) chỉ dựa trên phương pháp đo (nguồn: [1]).

1.3Thuật toán SLAM

Như đã nói ở trên, tạo lập một bản đồ là công việc rất quan trọng khi tasử dụng robot nói riêng hay mobile robot nói chung, tuy nhiên việc tạo bảnđồ bằng chính robot lại yêu cầu robot biết được vị trí hiện tại của nó, muốnbiết được vị trí hiện tại thì lại cần vấn đề Lúc này vấn đề bị đẩy lên thànhtính nhân quả “trước-sau”, vì vậy để giải quyết vấn đề này chúng ta sử dụngthuật toán SLAM (simultaneous localization and mapping) để đồng thời tạolập map và định vị Và Doucet đã công bố một thuật toán hiệu quả xử lý vấnđề này đó là “ Rao-Blackwellized particle filters”.

p(x1:t,m|z1:t 0:t,u )=p(m|x1:t,z1:t) (xp 1:t|z1:t,u0:t) (1.12)

Để có thể ước lượng P, RBPF sử dụng một bộ lọc hạt mà trong đó, mỗi mộtlần lấy mẫu được gắn với một bản đồ riêng biệt Mỗi bản đồ được xây dựng

1 Lấy mẫu:2.

t |z1:t,u0:t)

1.4Thuật toán explore_lite

Khám phá và tạo lập map là các vấn đề cơ bản đối với các mobile robot tựđộng hoạt động trong môi trường ban đầu không biết hoặc động Việc khámphá liên quan đến các vấn đề đã được biết đến rộng rãi trong lĩnh vực hìnhhọc tính toán, đó là vấn đề được gọi là “art gallery” Vì vấn đề “art-gallery”là NP-complete [] và yêu cầu kiến thức hoàn chỉnh về môi trường, việc khámphá một môi trường không biết thường được thực hiện theo phương pháp“interactive” hoặc “greedy” Thay vì lên kế hoạch trước tất cả các vị trí màrobot cần thu thập thông tin, phương pháp khám phá greedy chỉ lên kế hoạchmột bước tiếp theo bằng cách xác định một góc nhìn tiếp theo tốt nhất cungcấp thông tin mới về môi Trong những thập kỷ qua, đã xuất hiện các chiếnlược khám phá khác nhau [2, 3, 4, 5] Đồ án này của chúng em tập trung vàophương pháp khám phá dựa trên biên - Frontier-based Exploration Strategies[].

Phương pháp Frontier-based exploration thường được thực hiện trên bản đồgrid Ý tưởng của phương pháp này đó là hướng mobile robot đến đường biêncủa những ô kẻ trống và những ô chưa có thông tin Cụ thể hơn, robot tìmkiếm các vùng có thể đi qua trong bản đồ đã được xây dựng cho đến thờiđiểm đó và kề với các vùng chưa được mô hình hóa Lúc này, vấn đề tìm ragóc nhìn tối ưu sẽ được rút gọn về là tìm ra đường biên gần nhất, chúng ta sẽđánh giá độ dài trên đường đi đến các biên và tiến đến biên gần nhất Điềunày tương đương với việc ta giả định là lượng thông tin dự kiến thu được lànhư nhau với các đường biên khác nhau.

Frontier-based Exploration

1 Xác định tập hợp các điểm có thể di chuyển T2 Xác định tập hợp các điểm có thể di chuyển đến R3 Xác định tập hợp các điểm có thể di chuyển và chạm đến

C = c[xy]| c[xy]∈ T ∩ R

4 Xác định tập hợp đường biên F bằng cách kiểm tra mỗi điểm là phần tửcủa C với một ô chưa biết:

F = c[xy]|c[xy]∈ C, cm ∈ [−1;1] n ∈ [−1;1][(x+m y+n)])( : p( c[(x+m y+n)()]) = 0.5

r = (r rx y)Tlà vị trí hiện tại của robot):

n = argminL((xy)T, r)

Để xác định tập các điểm thỏa mãn C và đường biên gần nhất với vị trí hiệntại của robot, chúng ta kiểm tra tất cả các ô trong bản đồ phản chiếu xácsuất mà đồng thời là ô có thể di chuyển được và có thể tiếp cận được Đểđảm bảo an toàn trong quá trình di chuyển và robot có thể tiếp cận đượcvị trí được chọn, ta xác định đường robot có thể di chuyển bằng cách khôngcho phép robot di chuyển qua các vùng có vật cản hoặc các ô mà không

là 0.25) thì đó là ô an toàn Hơn nữa, do kích thước của robot, ta muốnrobot giữ một khoảng cách tối thiểu với các vật cản trong môi trường, nên

Với yêu cầu như trên, ta có thể sử dụng giải thuật Dijkstra Không cầnđến một đích đến cụ thể, nó sẽ cho ta một bản đồ có thể tiếp cận được đầyđủ Bản đồ này cho ta thông tin ứng với mỗi một ô về cả “cost” khi di chuyểnđến đó, cụ thể là quãng đường và các ô tương ứng trên quãng đường đó Khicó được bản đồ này,ta có thể chọn được đoạn đường ngắn nhất thỏa mãn yêucầu.

Hình 1.4: Các loại bản đồ

Để dễ hình dung, hình (a) giả sử là môi trường chúng ta cần khám phá, tronghình (b), tường và các vật cản đã được cộng thêm kích thước của robot vàhình (c) bao gồm các khu vực robot có thể tiếp cận được (phần màu trắng).

1.5Mô hình động học mobile robot

Khi nghiên cứu về động học robot, ta có một số giả thiết như sau :

chuyển động.

tốc trượt được xem như bằng 0.

Môi trường robot di chuyển ở đó được định nghĩa:

Hình 1.5: Sơ đồ tiếp xúc bánh xe và mặt đất lý tưởng

Chúng ta sử dụng 4 bánh mecanum giúp cho mobile robot có thể di chuyểntheo bất kỳ hướng nào mà không cần thay đổi góc quay ban đầu của xe.

Hình 1.6: Điều khiển chuyển động và độ trượt của bánh mecanum

Để có thể tính toán được mô hình động học robot, đầu tiên chúng ta cần tínhtoán được vận tốc tại điểm tiếp xúc của bánh xe với mặt phẳng chuyên động.Với mỗi bánh xe vận tốc tại điểm tiếp xúc có các tham số:

Vận tốc của mỗi bánh xe tại điểm tiếp xúc:

VC = VM + ˙θzm.MC+ ˙βzm AC+ ˙ω yl BC+ ˙ψ yr.B′C (1.14)Trong đó:

1m :

vx− ly˙

θ − (R + r) ˙ω1−√2 2r ˙ψ1=0vy+ lx˙

3m :

vx+ ly˙

θ − (R + r) ˙ω3−√2 2r ˙ψ3=0vy− lx˙θ −√2ψ˙3=0

4m :

vx− ly˙

θ − (R + r) ˙ω4−√2 2r ˙ψ4=0vy− lx˙θ +√

Rút gọn các hệ trên:

1m : vx−vy− (lx+ ly) ˙ θ − (R + r) ˙ψ1=0 2m : vx+vy+(lx+ly) ˙ θ − (R + r) ˙ψ2=03m : vx−vy+(lx+ ly) ˙ θ − (R + r) ˙ψ3=0 4m : vx+vy− (lx+ ly) ˙ θ − (R + r) ˙ψ4=0

( Ràng buộc holonomic )

hàm được biểu diễn bởi 3 ẩn còn lại, hay nói cách khác chỉ có 3 vận tốc quaylà độc lập tuyến tính Suy ra 3 bánh xe cho ta khả năng tạo ra toàn bộ cácvectơ nên số bậc tự do của mobile robot 4 bánh mecanum là 3.

Mô hình động học:

Với: ˙ω =˙ω1

; J = 1R+r

1 −1 −(lx+ ly)1 1 (lx+ ly)1 −1 (lx+ ly)1 1 −(lx+ ly)

; ˙ξ =

−1 1 −1 1

−1l lx+y

1l lx+y

1l lx+y

−1l l

˙θ =R+r4

1l lx+y

−1l lx+y

˙ω4Trong đó:

˙η = J(θ) ˙ξ,J(θ)=

cos(θ) −sin(θ) 0sin(θ) cos(θ) 0

Chương 2

Ứng dụng thuật toán SLAM dựatrên ROS

2.1Giới thiệu cơ bản về ROS

ROS (robot operating system) là 1 phần mềm đóng vai trò trung gian giữaphần cứng tầng điều khiển và robot ROS cung cấp cho chúng ta khả năngchuẩn hóa code khi lập trình robot, giúp cho việc tái sử dụng, nâng cấp vàdebug trở nên dễ dàng hơn rất nhiều.Ngoài ra, ROS có một cộng đồng lậptrình viên lớn, với nhiều mã nguồn mở và các package được lập trình sẵn giúpchúng ta không cần phải "phát minh lại cái bánh xe".

Để làm được điều đó, ROS có cấu trúc như sau:

Hình 2.1: Cấu trúc đơn giản trong ROS

Với ví dụ như Hình 2.1 trên, ta thấy có 3 package (các ô nét đứt), mỗi age này tượng trưng cho một tập hợp các chương trình xử lý một khối côngviệc nhất định, có thể liên quan đến : motion planing, hardware, camera, .Các package này sẽ được giao tiếp với nhau dựa trên các message thông quacác topic, các message và topic này sẽ được xây dựng chuẩn hóa theo quy

pack-chuẩn chung dành cho ROS, giúp việc đọc hiểu trở nên dễ dàng.

Ngoài ra, ROS cung cấp công cụ mô phỏng mạnh mẽ là Gazebo Gazebolà một công cụ mô phỏng thế giới thật với đầy đủ các môi trường, tính chấtkhác nhau.

Hình 2.2: Mô phỏng robot và môi trường thực

Bên cạnh đó là Rviz, đây là một công cụ giúp ta kiểm soát nhận thức củarobot, tức là giúp chúng ta xem một cách trực quan robot đang “thấy” dữliệu gì từ nguồn là phần cứng thật cũng như mô phỏng Gazebo.

Hình 2.3: Robot trên Gazebo và Rviz

2.2Các package phụ thuộc trong ROS2.2.1 Package explore_lite

<include file="$(find gazebo_ros)/launch/empty_world.launch">

<arg name="world_name"value="$(find mcn4_description)/world/maze_1.world"/></include>

<param name="robot_description"command="$(find xacro)/xacro $(findmcn4_description)/urdf/mcn4.xacro"/>

<! Robots state publisher >

<node pkg="robot_state_publisher"type="robot_state_publisher"

<node pkg="gazebo_ros"name="urdf_spawner"type="spawn_model"

args="-urdf -model mcn4 -param robot_description -x 0 -y 0 -z 0.5"/><node pkg="gmapping"type="slam_gmapping"name="gmapping_node"output="log">

<param name="base_frame"value="base_link"/><param name="odom_frame"value="odom"/><param name="delta"value="0.01"/><param name="xmin"value="-5"/><param name="ymin"value="-5"/>

<param name="ymax"value="5"/><param name="maxUrange"value="5"/><param name="map_update_interval"value="1"/><param name="linearUpdate"value="0.05"/><param name="angularUpdate"value="0.05"/><param name="temporalUpdate"value="0.1"/><param name="particles"value="100"/></node>

<node pkg="move_base"type="move_base"respawn="false"name="move_base"

<param name="base_local_planner"value="dwa_local_planner/DWAPlannerROS"/><rosparam file="$(find mcn4_navigation)/param/costmap_common_params_mcn4.yaml"

<rosparam file="$(find mcn4_navigation)/param/costmap_common_params_mcn4.yaml"

<rosparam file="$(find mcn4_navigation)/param/local_costmap_params.yaml"

2.2.2 Package gmapping

Cấu trúc Gmapping : Node Gmapping nhận dữ liệu từ sensor_msgs/LaserScanvà tạo ra bản đồ (nav_msgs/OccupancyGrid), dữ liệu bản đồ có thể truy xuấtqua topic hoặc service.

Các dữ liệu đầu vào :

Các dữ liệu đầu ra:

File launch mcn4_gmapping.launch: