4 Kết quả mô phỏng trên trên phần mềm Gazebo

Khoảng cách k sẽ được sử dụng để so sánh với giá trị ngưỡng để từ đó

quyết định xem giá trị đo lường này sẽ được liên kết với điểm mốc nào, hay tạo giá trị điểm mốc mới nếu khoảng cách này lớn hơn ngưỡng

Giải thuật EKF-SLAM

Giải sử tại thời điểm t , các giá trị sau đã biết:

 Giá trị kỳ vọng của trạng thái vị trí robot và điểm mốct-1

 Ma trận hiệp phương sai của trạng thái ở thời điểmt-1

 Tín hiệu đo lường zt

 H k

 Số điểm mốc đã xác định Nt-1

Các bước giải thuật được thực hiện như sau: 1 Cập nhật giá trị số

2 Mốc quan sát được tại thời điểm t : Nt Nt-1

3

 1 0 0 0 0

0 1 0 0 0

 0 0 1 0 0

4 Tính tốn giá trị kỳ vọng mong muốn cho bước dự đoán

 vx,t cost1, v y ,t sint1, T x t T  0 0vx,t sint1, v y ,t cost1, gt  6 Tính tốn ma trận hiệp phương sai mong muốn của bước dự đốn

T T

7 Tính Qt  0

0



8 Với tất cả các tập quan sát được z i r i

T

9 Tăng số lượng các điểm mốc thêm 1 để thêm vị trí chứa điểm mốc mới nếu được tạo Giá trị điểm mốc thứ N +1 là giá trị điểm mốc mới

10 Cho chỉ số k = 1 đến Nk1  T Fx   vx,t sint1, v y ,t cost1,  5 Tính tốn ma trận Gt I Fx gt Fx với vx,t t1, v y ,t t1,  0 0 cos sin  0 0 0  t Gtt1 Gt Fx RFx Kết thúc bước dự đoán  r t t  k , xk , xt , x  k , yk , y t , y

11 Đặt δk là véc tơ tính tốn giá trị kỳ vọng của khoảng cách từ vị trí robot đến các điểm mốc đã biết vàk k k

12 Tính giá trị đo lường mong muốn ứng với các giá trị kỳ vọng 



T

14 Tính các giá trị khoảng cách Mahalanobis k (zt zk )T1(zi z k ) giữa tín hiệu đo lường và giá trị đo lường kỳ vọng của các điểm mốc đã xác định 15 Xác định chỉ số trong các tín hiệu đo kỳ vọng của các điểm mốc đã xác định

có khoảng cách Mahalanobis nhỏ nhất j(i) arg min k

k

16 So sánh giá trị này với giá trị ngưỡng

17 Nếu giá trị j(i) arg min k nhỏ hơn ngưỡng :

k

18 Tính hệ số Kalman theo giá trị khoảng cách nhỏ nhất j(i) :

Ktit [ H t j (i ) ]j (1i ) ,

19 Tính giá trị kỳ vọng và ma trận hiệp phương sai được cập nhật lại ở bước chỉnh

sửa theo hệ phương trình i

20 Nếu giá trị j(i) arg min k lớn hơn giá trị ngưỡng :

k

21 Vị trí điểm mốc mới thứ N +1 được đưa thêm vào, khi đó kỳ vọng tại điểm

i i

 t t , y t t ,



Giải thuật EKF-SLAM liên tục tính tốn vị trí của robot, xác định vị trí các điểm mốc và liên kết với giá trị đo lường có liên quan nhất, đồng thời cập nhật thêm các điểm mốc mới nếu giá trị điểm mốc đó lớn hơn ngưỡng liên kết với các giá trị đo

 a tan 2( k , x k , y t ,

zˆ tk, )

13 Cập nhật lại ma trận hiệp phương sai k t t H H k tk Qt

tt Kt (zi ztj (i ) ) t (I Kt H t j (i ) )t

 Nt1, xt , x cos(tt , )

mốc mới được tính bởi: rt N1, y sin( i )

i ˆ t k t ˆ t

tt

t t

Q trình đánh giá thu nhận tín hiệu bản đồ được tiến hành mơ phỏng trên Matlab, từ đó sử dụng trên phần mềm Gazebo giả lập môi trường hoạt động nhằm đánh giá hoạt động của robot như trong môi trường thực, sau cùng tiến hành trực quan hóa trên phần mềm Rviz trong môi trường thực tế nhằm đánh giá chất lượng và sai lệch của thuật toán EKF-SLAM

Hệ thống nhận thức của robot được xây dựng dựa trên dữ liệu thu được từ Astra Camera và Rplidar A2 trong quá trình chuyển động của robot Sau đó, mơi trường sẽ được bản đồ hố và robot có thể đồng thời tự định vị được trong mơi trường đó Các kết quả đạt được cho thấy tính hiệu quả thực tế của hệ thống nhận thức đã xây dựng

2 4 1 Mơ phỏng thuật tốn EKF-SLAM trên Matlab

Phần mềm Matlab được sử dụng để mơ phỏng kiểm chứng độ chính xác của thuật tốn EKF-SLAM Giao diện mơ phỏng được xây dựng như trong Hình 2 6

Giao diện mơ phỏng thuật tốn EKF-SLAM được xây dựng gồm 3 phần chính: · Mơi trường ảo: Phần mơi trường ảo được xây dựng trên hệ trục toạ độ 2 chiều Oxy Đường màu đen là bản đồ chính xác của mơi trường Đường màu đỏ là quá trình di chuyển của robot và các điểm đặt vật cản trong môi trường hoạt động

· Các nút điều khiển: Gồm các nút thêm/bớt điểm đặt, điểm mốc và vật cản để xây dựng các kịch bản xây dựng bản đồ cho robot

· Phần hiển thị bản đồ: Bản đồ thu được sẽ hiển thị phần bên phải với nền đen thể hiện phần bản đồ thu được và phần trắng thể hiện đường bao bên ngoài

Tham số động học mơ phỏng với mơ hình robot (Bảng 1 1): · Tốc độ lớn nhất theo phương x và y: 1,5 m/s

· Tốc độ góc lớn nhất: 0,5 rad/s · Bán kính robot: 0,3 m

· Bán kính bánh xe: 0,06 m

· Nhiễu mơ hình theo phương x và y: kỳ vọng µ= 0, phương sai σ = 0,02 m Tham số cho cảm biến lidar:

· Pham vi quét nhỏ nhất: 0,2 m · Độ phân giải: 1o , Góc quét: 360o

· Nhiễu với kỳ vọng µ = 0, phương sai σ = 0,03 m

Hình 2 6 Giao diện mơ phỏng thuật tốn EKF-SLAM

Q trình khởi chạy mơ phỏng được xây dựng trên cơ sở các đường bao (màu đen) thể hiện như các bức tường trong thực tế, các vật cản có thể mơ tả như các hình elip xuất hiện ngẫu nhiên trong môi trường hoạt động Tiến hành chạy thuật toán với các đường màu xanh lá cây là các tín hiệu thu được từ cảm biến (Hình 2 7)

Kết thúc q trình SLAM, bản đồ mơi trường đã được thu lại sau khi robot đã di chuyển đến các vị trí xung quanh và kết thúc quỹ đạo di chuyển Bản đồ thu được với đường màu đỏ là vị trí robot, các đường màu xanh dương đậm là vị trí chính xác của robot Phần bản đồ hố mơi trường được hiển thị phần bên phải (Hình 2 8)

Hình 2 8 Kết quả bản đồ thu được

Tiến hành đánh giá độ chính xác của bản đồ đã lập trên cơ sở phép đo độ lệch chuẩn làm căn cứ đánh giá Có thể nhận thấy độ lệch chuẩn bản đồ hố mơi trường được biểu diễn trong Hình 2 9

Độ lệch chuẩn lớn nhất khi bản đồ hố mơi trường trong q trình robot di chuyển là khoảng 0,43 mét Độ lệch chuẩn trung bình vị trí các điểm mốc dao động quanh giá trị 0,19 mét

Bảng 2 1 Bảng đánh giá độ lệch chuẩn bản đồ

Sai lệch bản đồ nhận được có sự ảnh hưởng của các phép đo và các vị trí khi robot di chuyển có thể kể đến như độ chính xác của giá trị quan sát, cần hiệu chỉnh phương sai để lấy lại các giá trị cần thiết khi tiến hành mơ phỏng

Bên cạnh đó, độ lệch chuẩn vị trí robot lớn nhất theo hai phương khoảng 0,24 mét và giá trị này dao động quanh giá trị trung bình khoảng 0,15 mét trong suốt quá trình robot di chuyển (Hình 2 10)

Hình 2 10 Độ lệch chuẩn vị trí

Từ bảng đánh giá có thể thấy vị trí robot theo các phương gặp sai lệch khi robot di chuyển cho thấy ảnh hưởng của các phép đo khi lấy bản đồ

Bảng 2 2 Bảng đánh giá độ lệch chuẩn vị trí robot:

2 4 2 Kết quả mơ phỏng trên trên phần mềm Gazebo

Mơ hình FWOMR được xây dựng trong môi trường mô phỏng sử dụng phần mềm Gazebo (Hình 2 11) -1 Thời gian (10 s) 50 100 150 250 300 400 Độ lệch chuẩn phương x (m) 0,12 0,17 0,22 0,11 0,11 0,13 Độ lệch chuẩn vị trí phương y (m) 0,14 0,22 0,17 0,11 0,05 0,15 -1 Thời gian (10 s) 50 100 150 250 280 400 Độ lệch chuẩn bản đồ (m) 0,2 0,33 0,22 0,3 0,45 0,2

Hình 2 11 Mơ hình mơ phỏng cho FWOMR

Hình 2 12, mơi trường mô phỏng sẽ giả lập không gian làm việc của robot với các đường bao như bờ tường, vách ngăn chia (vật cản)

Hình 2 12 Mơ hình 3D cho FWOMR trong mơi trường mơ phỏng Việc này có thể tùy chọn và được cấu hình thêm khi robot di chuyển ở các khu vực có các vách kính trong suốt (khó khăn sử dụng cảm biến Lidar) và từ đó hệ thống được bản đồ hóa phục vụ các tác vụ điều hướng, lập quỹ đạo di chuyển [89]

Hình 2 13 là các kết quả của hệ thống nhận thức về môi trường Trên bản đồ chia ô lưới với các vùng màu trắng là các vùng môi trường đã được nhận thức Các đường màu đen là các vật cản mà robot nhận dạng được

Trong phần này, một số mô phỏng được tiến hành trên công cụ Gazebo Kết quả giám sát thu được qua cơng cụ Rviz

Hình 2 13 Quá trình SLAM của robot

Hình 2 14 cho thấy bản đồ được xây dựng trên Gazebo bao gồm những bức tường và những lối đi robot có thể di chuyển xung quanh để có được dữ liệu cần thiết xây dựng bản đồ

Hình 2 14 Bản đồ ảo được xây dựng trên Gazebo

Trong Hình 2 15, các đường màu đỏ là tín hiệu quét laser được tạo ra từ Rplidar và vị trí hiện tại của robot được cập nhật bằng cách sử dụng các phép đo hình học

Hình 2 15 Bản đồ thu được trên Rviz

Trong Hình 2 16, bản đồ ảo được tạo ra trong Rviz giống môi trường được tạo trên Gazebo Sau khi bản đồ khu vực làm việc được tạo bởi gói EKF-SLAM,

Mapserver tiến hành lưu lại bản đồ qua hai file “map gpm” và “map yaml” chứa các thông tin của bản đồ được tạo [8], [12]

Thuật toán EKF-SLAM cho phép liên tục tính tốn và bản đồ hố mơi trường dựa vào các dữ liệu này đồng thời định vị robot trong bản đồ đó Sau khi di chuyển đến các vị trí đủ để thu thập dữ liệu cho việc bản đồ hố tồn bộ mơi trường, dữ liệu bản đồ này có thể được lưu lại để tái sử dụng trong những lần kế tiếp mà không cần phải tiến hành bản đồ hố lại mơi trường