Thời gian (s) 32 34 35 37 40 60 70
Sai lệch (10-1 m) 0 5 0 0 0 0 0
Trong Chương 3, luận án tiến hành thiết kế hệ điều hướng cho robot trên cơ sở xây dựng thuật toán Lập kế hoạch di chuyển tồn cục A∗ , từ đó xây dựng thuật toán TEB nhằm điều hướng quỹ đạo cục bộ tránh vật cản động cho FWOMR với yêu cầu đáp ứng nhanh khi tránh vật cản, giảm thời gian quá độ khi robot di chuyển ở quỹ đạo cục bộ nhưng vẫn bám theo đường quỹ đạo toàn cục. Với yêu cầu thời gian đáp ứng nhanh của bộ điều khiển đã lựa chọn địi hỏi robot đảm bảo thích ứng với việc phát hiện vật cản tĩnh và động phát sinh trên quỹ đạo tồn cục đặt trước, khi đó việc ưu tiên là lập kế hoạch di chuyển cục bộ nhằm tránh vật cản động đồng thời bám nhanh vào quỹ đạo đặt trước. Vì vậy các tình huống đặt ra với trường hợp này là xây dựng quỹ đạo di chuyển sao cho robot di chuyển linh hoạt và ổn định.
Tiến hành so sánh các thuật toán lựa chọn nhằm đánh giá chất lượng bộ điều khiển được đặt ra thể hiện thuật tốn MPC tính đáp ứng nhanh khi gặp vật cản động cụ thể như sau:
- Thuật toán SMC bám quỹ đạo sau thời gian đáp ứng khoảng 4 giây, hơn nữa vì thuật tốn SMC có hiện tượng q điều khiển khi mất khoảng 2,5 giây quá điều chỉnh sau khi bám quỹ đạo đặt.
- Thuật toán DSC bám quỹ đạo sau thời gian đáp ứng khoảng 3,5 giây, nhưng thuật tốn DSC có hiện tượng dao động khoảng 2,5 giây sau khi bám quỹ đạo đặt dẫn đến sự kém ổn định, giảm chất lượng bám quỹ đạo theo đề xuất.
Thuật toán MPC bám quỹ đạo sau thời gian đáp ứng khoảng 2,5 giây, hơn nữa khơng có hiện tượng dao động mà giảm dần giá trị bám theo quỹ đạo đặt cho thấy chất lượng bám quỹ đạo đáp ứng yêu cầu đề xuất.
Kết quả mô phỏng được kiểm chứng, so sánh với thuật toán đã sử dụng cho thấy tính hiệu quả khi robot di chuyển tránh vật cản, kết quả được thể hiện trong công bố [CT3]1, [CT4], [CT5]2 của luận án.
Để kiểm chứng chất lượng bộ điều khiển đề xuất, ở chương 4 luận án sẽ tiến hành thực nghiệm robot trong môi trường thực tế trên phần cứng đã lựa chọn với các kịch bản có vật cản tĩnh và động trong môi trường bất định.
1https://youtu.be/DRSf4KmiNWs
104
Chương 4
THỰC NGHIỆM THUẬT TỐN ĐIỀU KHIỂN
Với mơ hình hệ thống cho FWOMR sử dụng thuật toán nhận thức mơi trường dựa trên thuật tốn EKF-SLAM ở Chương 2, các thuật tốn điều hướng tồn cục và cục bộ cho robot kết hợp thuật toán điều khiển bám quỹ đạo được xây dựng trong Chương 3, từ đó luận án tiến hành thực nghiệm robot trong mơi trường thực tế nhằm kiểm chứng cơ sở lý thuyết đã đề xuất.
Để tiến hành thực nghiệm hệ thống robot, luận án tiến hành thiết kế hoàn chỉnh và xây dựng các kịch bản thử nghiệm với mơ hình robot (Hình 4.1):
•Mơ hình robot Omni
•Cảm biến lidar
•3D Camera
•Máy tính nhúng Jetson TX2
•Vi mạch nhúng STM32F
•Cơ cấu chấp hành
Về cấu trúc bộ phận di chuyển là 4 bánh Omni đa hướng, giúp cho việc di chuyển của robot dễ dàng và linh hoạt hơn.
105
Mơ hình Robot được xây dựng với 3 thành phần chính: Phần xử lý trung tâm, phần cảm biến và phần điều khiển cơ cấu chấp hành. Kiến trúc phần cứng robot FWOMR được mô tả như sơ đồ Hình 4.2.
Hình 4.2 Cấu trúc phần cứng điều khiển FWOMRTổng quan về kiến trúc phần cứng cho FWOMR như: Tổng quan về kiến trúc phần cứng cho FWOMR như:
• Máy tính nhúng Jetson-TX2 là vi xử lý hiệu năng cao kết nối xử lý thông tin với hệ thống cảm biến, trực tiếp xử lý thông tin từ một loạt các cảm biến bao gồm máy ảnh Astra, Lidar sau đó truyền lệnh vào một bộ vi điều khiển.
• Camera Astra như mắt của robot bao gồm máy ảnh RGB và máy ảnh chiều sâu. Độ phân giải hình ảnh RGB lên đến1280 x 720 @ 30 khung hình / giây, chụp ảnh từ mơi trường. Với độ phân giải hình ảnh sâu lên đến 640 x 480 @ 30 khung hình, hình ảnh có độ sâu được chuyển đổi thành dữ liệu qt laze thơng báo về mơi trường.
• PRLidar là một cảm biến đo ánh sáng phản xạ từ vật thể khi PRLidar chiếu ánh sáng laze tới những các mục tiêu. Dữ liệu của camera Astra và PRLidar được hiển thị qua Gazebo và Rviz để tạo bản đồ 2D và 3D.
• Mạch điều khiển STM32F103C8T6 sẽ là phần nhận tín hiệu điều khiển từ Jetson TX2 và sau đó truyền tín hiệu điều khiển đến mạch cầu MOSFET để vận hành bốn động cơ. HỆ ĐIỀU HÀNH ROBOT (ROS) VI ĐIỀU KHIỂN STM32 VI XỬ LÝ HIỆU NĂNG CAO JETSON-TX2 CẢM BIẾN LIDAR CAMERA ASTRA BỘ MÃ HÓA TRỤC QUAY 4 BỘ MÃ HÓA TRỤC QUAY 3 BỘ MÃ HÓA TRỤC QUAY 2 BỘ MÃ HÓA TRỤC QUAY 1 ĐỘNG CƠ 4 ĐỘNG CƠ 3 ĐỘNG CƠ 2 ĐỘNG CƠ 1
106
• Màn hình cảm ứng được sử dụng để theo dõi tồn bộ q trình điều hướng của robot bao gồm tốc độ, thơng tin về PRLidar và Astra và trực quan hóa các tác vụ như SLAM hoặc điều hướng.
Để thiết lập một hệ thống bản địa hóa cho robot di chuyển ở chế độ 2D, cần thực hiện cách tiếp cận đưa vào một bản đồ dựa trên tín hiệu qt laser, chuyển đổi thơng báo và kết quả đầu ra là các ước tính.
4.2.1. Thiết lập khung điều hướng
Thuật toán EKF-SLAM sẽ được thực thi bởi 2 node là bản đồ (Map) và vị trí robot (Robot Pose), xuất ra dữ liệu về bản đồ và vị trí robot. Lập bản đồ và lập kế hoạch di chuyển là rất quan trọng để điều hướng cho robot tự động.
Hình 4.3. Nút lập bản đồ và vị trí sử dụng trong ROS
Trong ROS, với các nút và chủ đề đầy đủ, ngăn xếp điều hướng được hiển thị như Hình 4.3 cung cấp robot hỗ trợ thơng tin tồn diện để điều hướng. Để đạt được mục tiêu này, các thuật tốn đã được phát triển với mục đích trích xuất bản đồ tạo điều kiện cho chuyển động chính xác của robot. Khối Gmapping thực hiện nhiệm vụ thu nhận tín hiệu và lập bản đồ môi trường hoạt động trong nhà (hoặc ngoài trời). Kỹ thuật này bao gồm xây dựng bản đồ của môi trường và đồng thời sử dụng bản đồ này để tính tốn vị trí của nó. Khi q trình tập hợp thơng tin từ mơi trường thông qua các cảm biến và việc tạo bản đồ đã hoàn thành, bản đồ được tạo có thể được lưu để sử dụng trong ngăn xếp điều hướng (Hình 4.4).
RpLidar
Gmapping
107
Bằng cách nhận thơng tin bao gồm vận tốc và vị trí của robot để tạo đường dẫn cục bộ, ROS sử dụng phép biến đổi tọa độ (tf) mô tả Oxyz tọa độ của cảm biến từ phép đo độ dời vị trí của robot. Các nút “Sensor_msgs / LaserScan” hoặc
“sensor_msgs / PointCloud” lưu trữ thông tin cảm biến từ camera Astra và cảm
biến Lidar. Omni_core Omni_teleop Odom Geometry_msgs/Twist tf/tfMessage Sensor_msg/LaserScan tf scan Base_footprint Base_link map Nav_msgs/OccupancyGrid Map-server (map-saver) Base_scan Map.pgm& map.yaml
Hình 4.4. Cấu trúc thiết lập gói Gmapping
Bên cạnh đó, chuyển động của robot có thể được điều khiển thuận tiện trong mơi trường khơng xác định do bản đồ được xây dựng bằng cách sử dụng thuật toán EKF-SLAM.
4.2.2. Thiết lập quỹ đạo
Để thực thi phương pháp lập kế hoạch di chuyển toàn cục, ROS cung cấp 3 giao diện bao gồm: carrot_planner, navfn, và global_planner. Trong đó lập kế hoạch di chuyển tồn cục (global_planner) là giao diện được sử dụng để thực thi thuật toán A∗ . Trong phương pháp này, việc lựa chọn tham số cost_factor cần được tính tốn phù hợp. Tham số này là một hệ số nhân với giá trị thu được từ bản đồ chi phí tồn cục, nó thể hiện mức độ ảnh hưởng của các giá trị bản đồ chi phí (costmap) tới việc thực hiện tính tốn quỹ đạo tồn cục (Hình 4.5).
Tiến hành thử nghiệm cho robot với giá trị cost_factor ban đầu là 0,55 rồi giảm dần từng khoảng 0,55 đến 0,01. Sau q trình thực nghiệm có thể thấy khi cost_factor
= 0,55 (Hình 4.6a) lúc này robot sẽ xa với vật cản dẫn đến giá trị costmap ít ảnh hưởng đến bản đồ tồn cục.
(a) (b)
Hình 4.5. Quỹ đạo toàn cục
(a) Giá trị tham số cost_factor = 0,55; (b) Giá trị tham số cost_factor = 0,01 Giảm dần các giá trị đến cost_factor = 0,01, trong trường trường hợp này robot gần vật cản hơn so với khi cost_factor = 0,55, do đó quỹ đạo tồn cục của robot trong trường hợp thứ hai sẽ gần với vật cản hơn trường hợp thứ nhất. Việc tính tốn lựa chọn cost_factor liên quan đến kiến trúc phần cứng, hiệu năng bộ xử lý của từng robot nên việc chọn việc chọn tham số hợp lý sẽ đảm bảo thời gian và tốc độ xử lý
robot.
Thuật tốn TEB trình bày ở trên cũng được ROS cung cấp một giao diện thực thi để dễ dàng xây dựng một hệ lập kế hoạch di chuyển cục bộ cho robot phù hợp với thuật toán này. Các ràng buộc động học về vận tốc và gia tốc của robot cần được thiết lập và là một trong những giá trị ảnh hưởng nhiều nhất đến kết quả tính tồn quỹ đạo cục bộ.
Sử dụng phần mềm Gazebo được tích hợp để có thể sử dụng trong ROS, môi trường giả lập hoạt động của robot được tối ưu sao cho các điều kiện vật lý giống với môi trường thực tế với các tham số mô phỏng và thực nghiệm giống nhau:
.
Acc_lim_y (Tốc độ lớn nhất theo phương y): 1,5 m/s Acc_lim_theta (Tốc độ góc lớn nhất): 0,5 rad/s Bán kính robot: 0,3 mét
Bán kính bánh xe: 0,06 mét
Min_obstacle_dist (khoảng cách ngắn nhất đến vật cản): 0,5 mét
Include_costmap_obstacles: thêm các vật cản phát hiện được từ costmap vào Include_dynamic_obstacle (có xét đến cả vật cản động)
Tham số cho cảm biến lidar: Phạm vi quét lớn nhất: 0,2 ÷10 mét, Độ phân giải: 1o, góc quét: 360o
Việc chọn thông số phù hợp cho bản đồ chi phí là rất quan trọng vì chỉnh định các tham số này ảnh hưởng đáng kể đến hiệu năng của việc lập quỹ đạo cho robot. Hơn nữa, việc thiết lập các lớp trong bản đồ chi phí giúp cho việc phân luồng dữ liệu đến việc tính tốn quỹ đạo tồn cục và quỹ đạo cục bộ. Lớp gia tăng trong bản đồ chi phí được lựa chọn sao cho robot có thể vừa tránh được vật cản nhưng cũng ko bị lệch quá xa quỹ đạo toàn cục.
Ban đầu, lựa chọn thử nghiệm tham số lớp gia tăng inflation_radius = 1,5. Với tham số lớp đã chọn làm tăng khoảng an toàn đến vật cản vì vậy robot di chuyển với bán kính lớn dẫn đến lệch xa khỏi quỹ đạo tồn cục, tăng thời gian và tính hiệu quả khi di chuyển đến đích (Hình 4.6).
110
Tiến hành thử nghiệm với việc giảm dần các giá trị tham số lớp gia tăng. Sau các thử nghiệm, luận án chọn tham số inflation_radius= 0,55. Hệ số này cho thấy robot di chuyển với khoảng cách an toàn nhưng vẫn đảm bảo cho robot không lệch xa khỏi quỹ đạo tồn cục, đáp ứng thời gian di chuyển đến đích (Hình 4.7).
Hình 4.7. Thơng số inflation_radius=0,55
Từ các kết quả đã được trình bày ở Chương 2 và Chương 3, luận án tiến hành thử nghiệm thuật tốn với các kịch lấy thơng tin bản bản đồ và xác định vị trí robot. Sau đó, thực hiện bài tốn điều hướng tránh vật cản (tĩnh và động) theo quỹ đạo cục bộ với yêu cầu đảm bảo bám theo quỹ đạo toàn cục ban đầu.
(a) (b)
Hình 4.8. Mơi trường thực nghiệm cho robot
111
Mơi trường thực tế được dùng thử nghiệm với các kịch bản là địa hình sảnh chính của tịa nhà (hình 4.8a) với kích thước 6 mét x 8 mét và dãy hành lang chung (hình 4.8b) trong phạm vi 12 mét x 2,4 mét có đoạn rẽ gấp khúc qua cửa lối đi chính. Q trình thực nghiệm sẽ được tiến hành theo các mục dưới đây.
4.3.1. Thực nghiệm robot lấy thơng tin bản đồ
Hình 4.9 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 đồ. Bản đồ được chia thành các ô lưới thể hiện tọa độ đã được tạo ra.
Hình 4.9. Bản đồ ảo được xây dựng trên Gazebo
Hình 4.10a 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. Các đường bao bên ngồi màu tím là các vật cản (bờ tường) đã thu nhận được khi quét bản đồ. Hình 4.10b là bản đồ được quan sát trực quản bởi Rviz giống môi trường được tạo trên Gazebo. Đường màu đỏ là dữ liệu quét của Camera.
(a) (b)
Hình 4.10. Bản đồ quan sát trên Rviz
112
Sau khi bản đồ khu vực được tạo, vị trí bắt đầu của robot được xác định trên bản đồ đó bằng cách sử dụng kết hợp gói AMCL và thơng tin hình học thu nhận được. Việc quét bản đồ có thể được thực hiện thủ công khi điều khiển cho robot di chuyển lần lượt qua các vị trí trên bản đồ hoặc thực hiện tự động. Hình 4.10b là ảnh thu nhận của cảm biến khi quét các vị trí robot cần lấy bản đồ và các vật cản trong môi trường hoạt động.
Kết quả bản đồ thực tế sau khi thực hiện bản đồ hóa hồn chỉnh như hình 4.11.
Hình 4.11. Bản đồ ơ lưới thu được
4.3.2. Thực nghiệm robot tránh vật cản tĩnh
Quá trình điều hướng được thực hiện bằng việc lập kế hoạch đường đi của robot. Quỹ đạo toàn cục được sử dụng để tạo ra con đường ngắn nhất đến mục tiêu thể hiện qua đường màu xanh.
Quỹ đạo tồn cục được lập dựa trên tính tốn đường đi ngắn nhất cho robot. Khi quỹ đạo toàn cục đã được tạo ra, quỹ đạo cục bộ sẽ chuyển con đường này thành các lệnh vận tốc cho động cơ robot. Việc tránh vật cản phát sinh trong quá trình di chuyển phụ thuộc vào quỹ đạo của robot, khi xuất hiện vật cản trên đường di chuyển, robot sẽ chuyển sang quỹ đạo cục bộ nhằm tính tốn thơng số trên gói Gmapping để lựa chọn lệnh vận tốc và hướng nhằm thay đổi quỹ đạo di chuyển tránh vật cản phù hợp. Hình 4.12 cho thấy mơi trường hoạt động với các vật cản tĩnh là các vách tường, các khối hộp khác nhau và bất kỳ vật cản nào đã quét được từ bước tạo bản đồ. Khi hoạt động, các cảm biến cập nhật bản đồ với các vật cản mới phát hiện được. Quỹ
113
đạo cục bộ căn cứ vào vật cản gặp phải và xuất ra kế hoạch di chuyển với khoảng cách có thể thay đổi.
Hình 4.12. Điều hướng cho robot
Trên nền ROS, luận án đã xây dựng một bảng điều khiển System Control bao gồm các vị trí 1,2,3,4 tương ứng với các điểm đích cần đi đến của robot. Hình 4.12 mô tả trạng thái ban đầu của robot đang ở vị trí bất kỳ, khi nhận lệnh điểm đến số 4, hệ thống sẽ tính tốn lập ra quỹ đạo tồn cục (đường màu xanh lá cây) và quỹ đạo cục bộ (đường màu đỏ) phát sinh khi robot gặp vật cản động (vật cản mới phát sinh). Sau khi tránh vật cản, robot sẽ lại di chuyển bám theo quỹ đạo toàn cục trong thời gian ngắn nhất để đi tới mục tiêu (Goal) đã định.
114
Kết quả thực nghiệm tham số cost_factor= 0,01; inflaction_radius= 1,5 cho thấy với bộ tham số đã chọn, vật cản tĩnh (là người đứng yên phát sinh sau khi thiết lập bản đồ) robot lựa chọn quỹ đạo cục bộ tối ưu (đường màu đỏ). Tuy nhiên, với giá trị inflaction_radius= 1,5 tăng bán kính an tồn xung quanh vật cản, do đó làm hẹp khoảng cách của hành lang, vì vậy robot khơng thể thực hiện theo quỹ đạo ban đầu.
Hình 4.14. Robot khơng thể di chuyển theo quỹ đạo cục bộ
Thực nghiệm tương tự với tham số cost_factor= 0,01; inflaction_radius= 0,55,