GIỚI THIỆU
Lý do chọn đề tài
Với sự tiến bộ của khoa học kỹ thuật và nhu cầu ngày càng cao của con người, việc cải tiến sản phẩm là điều cần thiết Xây dựng robot tự hành để thay thế con người trong các công việc nặng nhọc, đặc biệt trong môi trường độc hại như hóa chất, khu vực hạn chế tầm nhìn hay phóng xạ, cũng như trong các công việc hàng ngày như lau nhà, hút bụi và cắt cỏ, trở thành một yêu cầu cấp bách.
Trong lĩnh vực robot di động, hoạch định di chuyển là một bài toán quan trọng, giúp robot di chuyển từ điểm bắt đầu đến mục tiêu một cách tối ưu Hiện nay có nhiều phương pháp hoạch định như trường thế năng, bản đồ đường, thuật toán di truyền GA và phương pháp bầy đàn PSO ROS cũng cung cấp các gói để hỗ trợ hoạch định di chuyển cho robot Mỗi phương pháp có ưu và nhược điểm riêng, tùy thuộc vào ứng dụng cụ thể Cảm biến LiDAR, công nghệ tiên tiến, cung cấp thông tin 3D chính xác về môi trường, hỗ trợ robot trong việc định vị, phát hiện và đo khoảng cách Thuật toán di chuyển sẽ tận dụng thông tin từ cảm biến LiDAR để giúp robot xác định vị trí, tránh va chạm và điều hướng hiệu quả Do đó, nhóm đã chọn đề tài Thuật toán di chuyển cho mobile robot sử dụng cảm biến LiDAR.
Mục tiêu nghiên cứu của đề tài
Mục tiêu của nhóm khi thực hiện đề tài:
- Thiết kế mô hình mobile robot di chuyển linh hoạt
- Tính toán, lựa chọn động cơ, cơ cấu phù hợp cho mobile robot
- Xây dựng phương trình động học phù hợp cho cơ cấu robot đã thiết kế
- Thiết kế mạch điều khiển cho robot
- Thiết kế bộ điều khiển tốc độ động cơ cho robot
Nghiên cứu thuật toán di chuyển cho robot tập trung vào việc tránh vật cản và tìm đường đến các vị trí đã định trước Thuật toán này sử dụng cảm biến LiDAR để thu thập dữ liệu môi trường, kết hợp với hệ thống ROS (Robot Operating System) nhằm tối ưu hóa quá trình điều khiển và điều hướng của robot Việc áp dụng công nghệ này không chỉ nâng cao khả năng tự động hóa mà còn cải thiện hiệu suất di chuyển của robot trong các không gian phức tạp.
- Xây dựng lại bản đồ những vùng robot hoạt động, di chuyển và định vị chính nó trong bản đồ đã tạo.
Đối tượng và phạm vi nghiên cứu
- Nền tảng ROS (Robot Operating System)
- Thuật toán vẽ bản đồ Hector SLAM
- Nghiên cứu, thiết kế mobile robot sử dụng 3 bánh Omni-wheel
- Xây dựng bản đồ cho robot hoạt động trong môi trường trong nhà
- Hoạch định đường đi cho robot di chuyển trong bản đồ đã tạo
Phương pháp nghiên cứu
- Dùng cơ cấu 3 omni – wheel cho mobile robot
- Sử dụng LiDAR để quét map và định vị cho robot
- Xây dựng bản đồ môi trường với Hector SLAM
- Định vị robot trong bản đồ với Adaptive Monte Carlo Localization (AMCL)
- Hoạch định đường đi cho robot với thuật toán Dijkstra (ROS stack navigation)
Kết cấu đồ án
Báo cáo đồ án nhóm xây dựng được chia làm 6 chương, với nội dung như sau:
CHƯƠNG 1: GIỚI THIỆU: Nêu tính cấp thiết và lý do hình thành đề tài, mục tiêu nghiên cứu, đối tượng, phạm vi nghiên cứu của đề tài, kết cấu luận văn và phân định nội dung của vấn đề nghiên cứu
CHƯƠNG 2: TỔNG QUAN ĐỀ TÀI: Giới thiệu các vấn đề liên quan về đề tài, các nghiên cứu liên quan
CHƯƠNG 3: THIẾT KẾ PHẦN CỨNG: Thiết kế phần cứng cho robot bao gồm thiết kế cơ khí: lựa chọn động cơ, cơ cấu phù hợp và thiết kế phần điện
CHƯƠNG 4: ĐỘNG HỌC – ĐIỀU KHIỂN: Tính toán động học cho robot, xây dựng hệ thống điện – điều khiển cho robot hoạt động ổn định
CHƯƠNG 5: XÂY DỰNG PHẦN MỀM TRÊN ROS: Giới thiệu về hệ điều hành cho robot Xây dựng bản đồ môi trường robot hoạt động, kết hợp với ROS nghiên cứu thuật toán hoạch định di chuyển cho robot và giúp robot di chuyển đến mục tiêu bằng đường di chuyển ngắn nhất
CHƯƠNG 6: KẾT LUẬN, HƯỚNG PHÁT TRIỂN: Kiểm nghiệm lại kết quả và đúc kết lại quá trình làm đồ án.
THIẾT KẾ PHẦN CỨNG HỆ THỐNG
Thiết kế cơ khí
3.1.1 Lựa chọn cơ cấu di chuyển
Hiện nay, mobile robot sử dụng nhiều cơ cấu di chuyển khác nhau, mỗi loại có ưu nhược điểm riêng Việc lựa chọn cơ cấu di chuyển phù hợp phụ thuộc vào nhu cầu và ứng dụng cụ thể Dưới đây là một số cơ cấu di chuyển phổ biến trong mobile robot.
3.1.1.1 Cơ cấu di chuyển sử dụng bánh xe dạng đĩa
Hình 3.1 Bánh xe dạng đĩa
Bánh xe dạng đĩa là loại bánh xe lâu đời và phổ biến trong lĩnh vực robot cũng như nhiều lĩnh vực khác Hiện nay, robot phục vụ trong nhà hàng, robot y tế, và các dự án như robot dò line hay robot tránh vật cản vẫn sử dụng bánh xe truyền thống Cấu trúc phổ biến cho bánh xe dạng đĩa thường bao gồm 2 bánh chủ động và 1 bánh điều hướng.
Hình 3.2 Cơ cấu sử dụng 2 bánh chủ động và 1 bánh điều hướng Ưu điểm:
Việc sử dụng bánh xe dạng đĩa mang lại nhiều lợi ích như giá thành rẻ, cấu tạo đơn giản và thuận tiện trong việc tính toán chuyển động của robot Điều này không chỉ giúp dễ dàng di chuyển mà còn đảm bảo tính linh hoạt cho robot Hơn nữa, lực kéo của động cơ được tối ưu hóa so với các loại cơ cấu di chuyển khác, nâng cao hiệu suất hoạt động của robot.
- Với cơ cấu này thì sẽ sử dụng ít động cơ hơn
Cơ cấu di chuyển bằng bánh xe dạng đĩa chỉ cho phép di chuyển theo một trục cố định trên thân robot, thiếu tính linh hoạt so với các loại bánh xe đa hướng.
Để đảm bảo xe vào cua ổn định, cần sử dụng bộ vi sai cho hai bánh sau; nếu không, hiện tượng trượt bánh sẽ xảy ra, phụ thuộc vào động cơ được sử dụng cho hai bánh sau hoặc bánh trước.
3.1.1.2 Cơ cấu di chuyển sử dụng bánh xe Omni wheel
Bánh xe omni wheel là loại bánh xe đa hướng với cấu trúc đặc biệt, bao gồm các bánh trượt tự do được sắp xếp đối xứng theo hình dạng bánh xe truyền thống Thiết kế này cho phép kết hợp chuyển động quay và chuyển động tịnh tiến một cách độc lập Bánh omni thường được ứng dụng trong hệ thống băng chuyền, dây chuyền điện tử và các loại robot di động linh hoạt Đối với robot di động sử dụng bánh omni, hệ thống thường bao gồm ba bánh omni được đặt cách nhau 120 độ.
Hình 3.4 Cơ cấu robot sử dụng 3 bánh omni Ưu điểm:
- Cấu trúc tổng thể nhỏ gọn, tính thẩm mĩ cao
Robot với bánh xe omni sử dụng kỹ thuật truyền động đa hướng cho phép di chuyển linh hoạt và ổn định Nhờ vào thiết kế này, robot có thể hoạt động hiệu quả ngay cả trong những không gian chật hẹp.
- Khả năng sửa chữa cũng như thay thế khó khăn
3.1.1.3 Cơ cấu di chuyển sử dụng bánh xe Mecanum
Bánh xe Mecanum là loại bánh xe đa hướng có cấu trúc tương tự như bánh Omni, nhưng với các bánh trượt được bố trí ở góc 45° so với mô-men vectơ dao động Thiết kế này cho phép bánh Mecanum không chỉ di chuyển thẳng như bánh xe truyền thống mà còn có khả năng định hướng ở góc 45° so với hướng di chuyển ban đầu nhờ vào hệ thống bánh trượt.
Robot sử dụng cơ cấu di chuyển bằng bánh xe Mecanum thường sử dụng hệ 4 bánh, các bánh được bố trí theo hình chữ nhật ở phía 4 góc (xem hình 3.6 [28])
Hình 3.6 Cơ cấu sử dụng 4 bánh mecanum Ưu điểm:
- Có khả năng di chuyển linh hoạt và ổn định giống như bánh Omni
- Mô-men kéo không cao
- Dễ bị mòn, tuổi thọ không cao
Nhóm nghiên cứu sẽ phát triển một robot di động linh hoạt cho môi trường trong nhà, sử dụng cơ cấu chuyển động với 3 bánh omni được bố trí lệch nhau 120° Các ưu và nhược điểm của thiết kế này đã được trình bày chi tiết trong mục 3.1.1.2, cùng với hình ảnh minh họa trong hình 3.7.
Hình 3.7 Cơ cấu robot sử dụng 3 bánh omni 3.1.2 Tính toán, lựa chọn động cơ
Dựa trên các thông số đầu vào có sẵn và các ước lượng, chúng tôi tiến hành tính toán các thông số cần thiết của động cơ nhằm đáp ứng các yêu cầu ban đầu, như được trình bày trong bảng 3.1.
Bảng 3.1 Thông số đầu vào
STT Thông số Kí hiệu Giá trị Đơn vị
1 Vận tốc lớn nhất của robot νmax 1 m/s
2 Thời gian gia tốc mong muốn - 1 s
3 Khối lượng bánh omni wheel m 0,35 kg
4 Bán kính bánh omni wheel R 0,039 m
5 Khối lượng robot ước tính M 10 kg
3.1.2.2 Cơ sở lý thuyết để lựa chọn động cơ Động cơ của robot đóng vai trò cung cấp momen cho bánh xe để thực hiện chuyển động Quá trình chuyển động này phụ thuộc đáng kể vào khối lượng của robot và ma sát giữa bánh xe và mặt đường Mô hình toán cho một bánh xe được biểu diễn như trong hình 3.8:
Hình 3.8 Mô hình toán học cho bánh xe chủ động
I: momen quán kính (kgm 2 ) m: khối lượng bánh xe (kg) R: bán kính bánh xe(m) τ: momen quay của động cơ (Nm) γ: gia tốc góc (rad/s 2 )
Fms: lực ma sát (N) μ: hệ số ma sát
M: khối lượng robot (kg) a: gia tốc của robot (m/s 2 ) ω: vận tốc góc (rad/s) v: vận tốc (m/s) P: công suất động cơ (W) n: tốc độ của động cơ (vòng/phút) k: hệ số an toàn
- Momen quán tính của bánh xe được tính gần đúng như sau:
- Phương trình cân bằng momen quanh tâm bánh xe: τ - F R = Iγ ms (3.2)
- Để robot không bị trượt, momen của động cơ phải thỏa mãn phương trình sau:
- Phương trình định luật II Newton:
- Từ phương trình (3.3) và phương trình (3.5) ta có:
- Tốc độ lớn nhất của động cơ:
- Công suất của động cơ:
Từ cơ sở lý thuyết tính toán ở mục 3.1.2.2, ta xác định được các thông số cần thiết của động cơ như sau:
Với các thông số như trên ta chọn động cơ Planet 600rpm (hình 3.9) có hộp giảm tốc, 24V 30w encoder 12ppr với các thông số ở bảng 3.2:
Bảng 3.2 Thông số động cơ Planet
Tốc độ không tải 600 (vòng/phút) Công suất động cơ 30 (W)
Dòng tiêu thụ 3600 (mA) Điện áp 24 (VDC)
Khả năng tải trọng 25 (kg) Encoder 12 xung, 2 kênh A B, nguồn 5VDC
Khung và vỏ của robot di động đóng vai trò thiết yếu trong việc đảm bảo hoạt động ổn định và đáp ứng các yêu cầu kỹ thuật Chúng không chỉ kết nối, cố định và sắp xếp hợp lý các bộ phận như bánh xe, linh kiện, bảng mạch và pin, mà còn giúp robot duy trì sự ổn định và độ vững chắc trong quá trình vận hành Hơn nữa, khung và vỏ còn bảo vệ robot khỏi các tác động bên ngoài và nâng cao tính thẩm mỹ của thiết bị.
Nhóm đã đặt ra yêu cầu chế tạo robot di chuyển linh hoạt mà không cần tải trọng, với tiêu chí nhẹ, bền, đẹp và giá thành hợp lý Sau khi tham khảo tài liệu, nhóm quyết định sử dụng công nghệ in 3D với nhựa PLA – FS để thiết kế và chế tạo khung vỏ robot, nhờ vào tính nhẹ, thẩm mỹ cao và chi phí hợp lý của vật liệu này.
Mô hình robot được nhóm thiết kế như hình 3.10 và được hoàn thiện như hình 3.11:
Hình 3.10 Mô hình tổng quan mobile robot 1
1 Phụ lục: Bản vẽ cơ khí
Thiết kế hệ thống điện
3.2.1 Các thiết bị điện – điện tử
Cảm biến LiDAR (Laser Imaging Detection and Ranging) là công nghệ tiên tiến trong robot học và xử lý hình ảnh, cung cấp thông tin chi tiết về môi trường xung quanh Thiết bị này sử dụng tia laser để đo khoảng cách và phát hiện các vật thể một cách chính xác và đáng tin cậy Nguyên lý hoạt động của cảm biến LiDAR dựa trên việc phát ra tia laser và ghi nhận thời gian phản hồi khi tia laser quay lại sau khi chạm vào các vật thể Bằng cách quét laser trong một góc rộng hoặc 360 độ, cảm biến LiDAR tạo ra bản đồ chi tiết về môi trường xung quanh robot.
Cảm biến LiDAR nổi bật với khả năng đo khoảng cách chính xác, quét 3D và hoạt động hiệu quả trong nhiều điều kiện ánh sáng Nó giúp robot định vị, phát hiện và tránh vật thể, đồng thời cung cấp thông tin cần thiết cho việc điều hướng và lập bản đồ Ứng dụng của LiDAR rất đa dạng, từ robot tự lái đến các hệ thống robot thông minh, nâng cao hiệu suất, an toàn và tương tác với môi trường Với tính linh hoạt, độ chính xác và khả năng tương tác cao, cảm biến LiDAR đã trở thành công cụ quan trọng trong robot học và ngày càng được áp dụng rộng rãi trong thực tế.
Bảng 3.3 Thông số kỹ thuật cảm biến LiDAR
Tên Thông số Điện áp 5VDC
Phương pháp phát hiện vật cản Laser Khoảng cách phát hiện vật tối đa 12m
Tốc độ lấy mẫu tối đa 8000 Samples/ time
Tần số quét tối đa 10Hz
Raspberry Pi là một dòng máy tính nhỏ gọn, được thiết kế và phát triển bởi Raspberry
Raspberry Pi là một vi xử lý mạnh mẽ, nhỏ gọn và giá cả phải chăng, được phát triển bởi Pi Foundation tại Vương quốc Anh nhằm khuyến khích việc học lập trình và ứng dụng công nghệ trong giáo dục, đặc biệt trong lĩnh vực STEM Thiết bị này được trang bị nhiều cổng kết nối như HDMI, âm thanh, USB và hỗ trợ kết nối mạng qua Ethernet hoặc Wi-Fi Ngoài ra, Raspberry Pi còn có khe cắm thẻ nhớ microSD để lưu trữ hệ điều hành và dữ liệu, sử dụng hệ điều hành Linux và có khả năng chạy đa dạng phần mềm, từ ứng dụng văn phòng đến trình duyệt web và các ứng dụng nhúng cũng như điều khiển robot.
Hình 3.13 Raspberry pi 4 model B Ưu điểm:
- Hỗ trợ ngoại vi rộng rãi
- Hỗ trợ tất cả các loại ngôn ngữ lập trình
- Bộ xử lý nhanh hơn
- Có thể được sử dụng như một máy tính di động nhỏ gọn
- Thiếu bộ nhớ trong eMMC
- Bộ xử lý đồ họa bị thiếu
- Không thực tế bằng máy tính để bàn
- Quá nhiệt khi xử lý nhiều tác vụ cùng lúc
Kit Blue Pill sử dụng chip STM32F103C8T6, một vi điều khiển phổ thông của STM, nổi bật với đầy đủ ngoại vi cơ bản, bộ nhớ hợp lý và giá thành rẻ STM32F103 thuộc họ F1 của STMicroelectronics, trang bị lõi ARM Cortex-M3, hoạt động ở tốc độ tối đa 72MHz So với các vi điều khiển tương tự, STM32F103 có mức giá hợp lý, cùng với nhiều lựa chọn cho mạch nạp và công cụ lập trình, giúp người dùng dễ dàng nạp chương trình và phát triển ứng dụng.
Hình 3.14 Kit Phát Triển STM32F103C8T6
Bảng 3.4 Thông số kỹ thuật kit Phát Triển STM32F103C8T6
Microcontroller STM32F103C8T6 Điện áp hoạt động 3.3 V
Nguồn từ các chân I/O 6 mA
Chuẩn giao tiếp I2C, UART, CAN, USB
3.2.1.4 Mạch điều khiển động cơ DC BTS7960
Hình 3.15 Mạch điều khiển động cơ DC BTS7960
Module điều khiển động cơ DC BTS7960 - 43A được thiết kế để giao tiếp dễ dàng với vi điều khiển, tích hợp driver trong IC và cung cấp các tính năng bảo vệ như cảm biến dòng, tạo dead time, và bảo vệ quá nhiệt, quá áp, quá dòng, sụt áp và ngắn mạch Để đảm bảo an toàn, thiết bị tự động tắt nguồn khi điện áp đầu vào dưới 5.5V và sẽ tự động mở lại khi điện áp vượt quá mức này, giúp bảo vệ module và động cơ khỏi hoạt động không ổn định Ngoài ra, BTS7960 còn có cảm biến nhiệt bên trong để ngắt đầu ra khi nhiệt độ vượt quá ngưỡng an toàn, ngăn ngừa hư hỏng và đảm bảo an toàn cho hệ thống.
Module Điều Khiển Động Cơ DC BTS7960 - 43A là một giải pháp tối ưu cho các ứng dụng điều khiển động cơ DC, mang lại hiệu suất cao và tính năng bảo vệ an toàn Sản phẩm này được ưa chuộng nhờ vào khả năng đáp ứng các yêu cầu khắt khe về hiệu suất và độ tin cậy trong quá trình hoạt động.
Bảng 3.5 Thông số kỹ thuật BTS7960
Dòng điện tải mạch 43A Tần số điều khiển tối đa 25 KHz
3.2.2 Lựa chọn nguồn cho robot Để robot có thể tự hành, di chuyển một cách linh hoạt thì cần nguồn điện cấp riêng cho hệ thống Cung cấp nguồn cho động cơ và hệ thống vi điều khiển, các driver điều khiển động cơ, nguồn phải đáp ứng được những yêu cầu như: điện áp cấp đủ cho các thiết bị, dòng của nguồn phải chịu tải tốt…
Thông số dòng tiêu thụ của các thiết bị được thể hiện trong bảng 3.6:
Bảng 3.6 Thông số dòng tiêu thụ của các thiết bị
Thiết bị Dòng tiêu thụ
LiDAR 1 1 1 5 Để robot có thể tự hành, di chuyển một cách linh hoạt thì cần nguồn điện cấp riêng cho hệ thống Trong đề tài này, nhóm nghiên cứu dùng hai nguồn điện chính để cấp cho hệ thống Một nguồn được lấy trực tiếp từ cục sạc dự phòng dành cho Smartphone, cấp trực tiếp cho hoạt động của Raspberry Pi 4 model B Nguồn còn lại là nguồn 12 𝑉𝐷𝐶 (Dòng tối đa 3A) được dùng để cấp nguồn cho BTS7960 điều khiển động cơ Điện áp cấp cho mỗi động cơ là 12 − 24 𝑉𝐷𝐶 Dòng tiêu thụ tối đa của động cơ là 1,25
A, ở đây nhóm sử dụng 3 động cơ cùng với dòng tiêu thụ từ vi điều khiển và ngoại vi nên dòng tối đa có thể sẽ lên đến hơn 3,75 A Do đó pin LIPO 6S 6500mAh (22,2V) (Hình 3.16) được chọn để cấp nguồn cho việc điều khiển động cơ Pin LIPO 6S được cấu tạo từ 6 cell pin mắc nối tiếp, mỗi cell có dung lượng tối đa khi sạc đầy là 4.2V Pin LIPO nhóm sử dụng có dòng xả tối đa gấp 45 lần so với dòng cho phép, dung lượng lên đến 6000mAh do đó có thể đảm bảo đủ công suất cấp cho hệ robot hoạt động ổn định Ngoài ra, để đảm bảo tuổi thọ cho pin, nhóm sử dụng thêm module đo và cảnh báo dung lượng pin khi dưới ngưỡng cho phép
Thông số pin LIPO được hiển thị ở bảng 3.7
Bảng 3.7 Thông số pin LIPO
Mạch giảm áp đóng vai trò quan trọng trong việc cung cấp nguồn điện ổn định cho các thiết bị trong hệ thống, đảm bảo dòng điện đủ theo yêu cầu Chức năng chính của mạch giảm áp là tạo ra điện áp thấp hơn so với điện áp đầu vào và duy trì mức điện áp này, bất chấp sự thay đổi của điện áp đầu vào.
Trong đồ án này, nhóm đã sử dụng mạch giảm áp Buck DC-DC LM2596 để giảm áp từ nguồn pin LIPO Cụ thể, mạch giảm áp này được sử dụng để cung cấp điện cho các thiết bị ngoại vi với điện áp 5V và cho BTS7960 với điện áp 12V Thông số kỹ thuật của mạch giảm áp Buck DC-DC LM2596 được trình bày trong bảng 3.8.
Hình 3.17 Mạch giảm áp Buck DC-DC LM2596
Thông số Giá trị Điện áp đầu vào 4V - 30V Điện áp đầu ra 1.5V - 28.5V
Dòng ngõ ra tối đa 3A
3.2.4 Sơ đồ nguyên lý mạch điều khiển
Sơ đồ nguyên lý mạch điều khiển của mobile robot mà nhóm thiết kế như hình 3.18:
Hình 3.18 Sơ đồ nguyên lý mạch điều khiển 2
ĐỘNG HỌC – ĐIỀU KHIỂN
Động học của mobile robot
Đế robot là cơ cấu chấp hành cho phép robot di chuyển trong không gian 2 chiều dưới sự điều khiển của Board mạch chủ Bài viết này tập trung vào việc di chuyển của robot trên sàn nhà, không tính đến các yếu tố bên ngoài ảnh hưởng đến chuyển động Động học đóng vai trò quan trọng trong việc xác định phạm vi các tư thế mà robot có thể thực hiện trong môi trường thực.
Mô hình động học của đế robot sử dụng hệ phương trình để xác định vị trí và hướng di chuyển của robot, bao gồm chuyển động tịnh tiến và chuyển động xoay Chuyển động tịnh tiến liên quan đến sự dịch chuyển của gốc tọa độ trên đế robot theo trục x và y, trong khi chuyển động xoay liên quan đến sự quay quanh trục z Bánh xe là phần tử cơ bản để điều khiển đế robot, với các tham số động học như bán kính bánh xe và khoảng cách từ tâm bánh xe đến gốc tọa độ Mô hình động học đơn giản với 3 bậc tự do (DoF) sử dụng 3 bánh đa hướng Omni, cho phép trạng thái của robot thay đổi theo vận tốc và góc quay của các bánh khi di chuyển từ vị trí hiện tại đến vị trí yêu cầu.
4.1.1 Mô hình hóa động học robot
Hình 4.1 Mô hình hóa động học đế robot
Cấu trúc hình học của robot được thể hiện ở hình 4.1, gồm có:
- Ba bánh xe đa hướng Omni có cùng bán kính R, được kéo bởi động cơ motor DC và cách đều nhau một góc 120 0
- Tâm dịch chuyển của bánh đa hướng trùng với tâm của robot tại O
- Khoảng cách từ tâm tới bánh xe là L
- OXY là hệ tọa độ tham chiếu toàn cục
- MVnV là hệ tọa độ tham chiếu cục bộ gắn liền với robot
Ma trận quay R θ chuyển từ hệ tọa độ toàn cục Oxy sang hệ tọa độ gắn liền trên khung robot:
Các vectơ vận tốc trong hệ toàn cục VO và hệ cục bộ trên khung robot VM:
Từ các phương trình (4.1), (4.2) ta có mối liên hệ như sau:
Tọa độ tâm robot trong hệ tọa độ toàn cục tại thời điểm t2 = t1 + t được tính như sau:
Mối quan hệ giữa tốc độ bánh xe v 1 , v 2 và v 3 có liên quan với tốc độ của robot V, Vn và ω như được mô tả như sau:
Từ phương trình trên, sử dụng động học nghịch robot, ta xác định được tốc độ robot từ tốc độ bánh xe như sau:
Trong bài toán động học, chúng ta đã xác định được các phương trình liên kết giữa vận tốc của động cơ và quỹ đạo chuyển động của robot.
(Các công thức tính toán trong mục 4.1.2 được tham khảo tài liệu [8], [9], [15])
Trong nghiên cứu động học, nhóm đã xác định mối liên hệ giữa vận tốc của động cơ và quỹ đạo chuyển động của robot Robot sẽ hoạt động trong môi trường trong nhà, do đó, nó cần một bản đồ cùng với vector vận tốc toàn cục để điều hướng hiệu quả trong không gian đó.
Để chuyển đổi vận tốc từ bản đồ sang vận tốc của robot, chúng ta cần thực hiện quy trình chuyển vị Hệ tọa độ của bản đồ và hệ tọa độ của robot được minh họa trong hình 4.2.
Hình 4.2 Chuyển vị vận tốc cho robot
Với v map là vector vận tốc trong hệ trục tọa độ bản đồ, vận tốc v và v n của robot khi chuyển vị từ hệ trục bản đồ sang hệ trục robot được tính bằng cách xoay theo trục z với góc ∝.
_ _ cos sin sin cos x map y map n x map y map v v v v v v
4.1.4 Kết quả mô phỏng động học
Ban đầu, các vectơ vận tốc v và v n của robot cùng phương với trục O x và O y như hình 4.3:
Dựa trên các phương trình động học và động lực học đã được nêu rõ, việc mô phỏng trên Matlab (xem phụ lục mô phỏng động học – động lực học) đã cho ra những kết quả đáng chú ý.
- Cho robot chạy với vận tốc các bánh lần lượt: v 1 = -1,7 (m/s); v 2 = 2,5 (m/s); v 3 = 2 (m/s)
Robot di chuyển theo quỹ đạo hình tròn theo chiều dương (ngược chiều kim đồng hồ), và đồ thị thể hiện vị trí cũng như sự thay đổi góc xoay θ được minh họa trong hình 4.4.
Hình 4.4 Vị trí robot với quỹ đạo đường tròn
Khi cho robot di chuyển với tốc độ các bánh lần lượt là v1 = -5 (m/s), v2 = 2,5 (m/s) và v3 = 2,5 (m/s), quỹ đạo di chuyển của robot sẽ là một đường thẳng, ngược hướng với trục vn Góc xoay θ của robot không thay đổi theo thời gian, và đồ thị vị trí được thể hiện trong hình 4.5.
Hình 4.5 Vị trí robot với quỹ đạo đường thẳng theo chiều âm
Khi cho robot di chuyển với vận tốc các bánh lần lượt là v1 = 0 (m/s), v2 = -2 (m/s) và v3 = 2 (m/s), quỹ đạo di chuyển của robot sẽ tạo thành một đường thẳng, ngược hướng so với trục vn Góc xoay θ của robot sẽ không thay đổi theo thời gian, và đồ thị vị trí được thể hiện trong hình 4.6.
Hình 4.6 Vị trí robot với quỹ đạo đường thẳng theo chiều dương
Với vận tốc các bánh xe lần lượt là v1 = -1 m/s, v2 = 0 m/s, và v3 = 1 m/s, quỹ đạo di chuyển tạo thành một đường thẳng ngang, song song với trục v Góc xoay θ giữ nguyên theo thời gian và bằng 0 Đồ thị vị trí được thể hiện trong hình 4.7.
Hình 4.7 Vị trí robot với quỹ đạo đường thẳng ngang
Với vận tốc các bánh xe lần lượt là v1 = 1 (m/s), v2 = -2 (m/s) và v3 = 1 (m/s), quỹ đạo di chuyển tạo thành một đường thẳng dọc, song song với trục vn Góc xoay θ giữ nguyên theo thời gian và bằng 0, như thể hiện trong đồ thị vị trí ở hình 4.8.
Hệ thống điều khiển robot
Mạch điều khiển robot được phát triển để thực hiện các chức năng như điều khiển động cơ, đọc tín hiệu từ encoder và giao tiếp qua cổng serial port bằng giao thức UART, nhằm nhận và truyền dữ liệu với máy tính Hệ thống robot được minh họa trong sơ đồ tổng quan ở hình 4.9.
Để đảm bảo điều khiển động cơ ổn định ở tốc độ mong muốn, nhóm đã sử dụng một driver riêng cho mỗi động cơ.
Encoder gồm có hai pha A và B đặt lệch pha nhau thường là 90° để xác định số vòng quay được và chiều quay hiện tại của động cơ như hình 4.10 [2]:
Hình 4.10 Giản đồ xung Clock Encoder
Khi động cơ quay theo chiều thuận, pha A sẽ sớm hơn pha B và ngược lại Encoder có ba chế độ đọc xung: x1, x2 và x4 Chế độ x1 chỉ đọc cạnh lên của pha A, trong khi chế độ x2 đọc cả cạnh lên và cạnh xuống của pha A Chế độ x4, với độ phân giải cao nhất, sẽ đọc cạnh lên và cạnh xuống của cả pha A và pha B.
Pha A luôn sớm hơn pha B, với A ở cạnh lên và B ở cạnh xuống, và ngược lại Khi quay thuận, biến đếm ở các chế độ sẽ tăng lên một tại các thời điểm đọc được, và ngược lại Theo lý thuyết đọc Encoder, việc đọc xung từ các Encoder có 𝑃𝑃𝑅 lớn bằng phương pháp sử dụng ngắt ngoài để bắt cạnh hai pha A và B sẽ tốn thời gian xử lý Ngoài ra, các yếu tố như nhiễu trên đường truyền và méo dạng xung cũng có thể gây ra sai số trong quá trình đọc Để khắc phục những khuyết điểm này, nhóm đã quyết định sử dụng timer để đọc xung Encoder, cho phép việc đọc xung hoạt động độc lập với quá trình xử lý của CPU thông qua các thanh ghi của timer.
4.2.1.2 Điều khiển vận tốc động cơ bằng thuật toán PID
Hình 4.11 Sơ đồ thuật toán PID
PID (Proportional-Integral-Derivative) là thuật toán điều khiển phổ biến được áp dụng trong các hệ thống như nhiệt độ, áp suất và vị trí động cơ Thuật toán này hoạt động dựa trên việc so sánh giá trị đầu ra y của hệ thống với giá trị đặt y0 để xác định sai số e Bằng cách xử lý sai số e, PID tạo ra tác động u giúp giá trị ngõ ra y theo sát giá trị đặt ban đầu, từ đó đảm bảo hiệu quả trong điều khiển các đại lượng cần thiết.
Bộ điều khiển PID bao gồm ba thành phần chính: tỉ lệ (P), tích phân (I) và vi phân (D) Ngõ ra của bộ điều khiển PID được tính bằng tổng của ngõ ra từ ba thành phần này.
Khâu tỉ lệ là tích giữa sai số hiện tại với độ lợiK P :
- P out : thừa số tỉ lệ của đầu ra
Khâu tích phân là tích giữa tổng sai số tức thời theo thời gian với độ lợi K I :
- I out : thừa số tích phân của đầu ra
- 𝜏: biến tích phân trung gian
Khâu vi phân là tích giữa độ dốc sai số theo thời gian với độ lợi vi phânK D :
- D out : thừa số vi phân của đầu ra
Vậy ngõ ra tác động là:
Hàm truyền của bộ điều khiển PID liên tục:
Sự phát triển của các hệ thống điều khiển số yêu cầu cần có những bộ điều khiển rời rạc
Thuật toán điều khiển PID rời rạc được ứng dụng phổ biến trong máy tính và hệ thống nhúng Việc chuyển đổi về miền thời gian rời rạc cho từng khâu là một bước quan trọng trong quá trình thiết kế và tối ưu hóa hệ thống điều khiển.
Với T là chu kì lấy mẫu theo thời gian (giây) Từ đó cho ta ngõ ra tác động u(kT) là:
4.2.1.3 Phương pháp xác định bộ thông số PID a Tìm hàm truyền
Trong đồ án này, nhóm sử dụng động cơ Planet 600RPM 24V 30W encoder 12 PPR
Để xác định hàm truyền xấp xỉ của động cơ do thiếu các thông số cụ thể, cần tiến hành khảo sát mối quan hệ giữa nguồn cấp và tốc độ quay của động cơ Việc này giúp chúng ta hiểu rõ hơn về sự liên kết giữa hai đại lượng này, từ đó xây dựng được hàm truyền động cơ chính xác.
Hình 4.12 Khảo sát đầu vào và đầu ra hệ thống
Khi tín hiệu PWM đầu vào của động cơ đạt đến mức tối đa là PWMmax = 0,5, động cơ sẽ đạt được tốc độ tối đa Để thu thập dữ liệu tốc độ, cần thiết lập đầu vào PWM ở mức 20% của PWMmax, tương đương với PWM = 0,1.
Với U(s) là giá trị PWM đầu vào và ω(s) là tốc độ đầu ra, hàm truyền động cơ có dạng:
Nếu ω(s) có bậc m và U(s) có bậc n, hàm truyền động cơ xác định khi m ≤ n
Chọn U(s) là hàm bậc 2, ω(s) là hàm bậc 0, sử dụng công cụ systemIdentification của Matlab, ta tìm được hàm truyền xấp xỉ của hệ thống như sau:
Hàm truyền xấp xỉ của hệ thống có độ chính xác là 95,48%, phù hợp để sử dụng cho tính toán thông số PID điều khiển tốc độ động cơ
Mặc dù sử dụng cùng một loại động cơ với các thông số giống nhau, nhưng đáp ứng của từng động cơ vẫn có sự sai lệch nhỏ, ảnh hưởng đến hàm truyền động cơ Để xác định hàm truyền của từng động cơ, nhóm nghiên cứu đã tiến hành mô phỏng bằng cách cấp điện áp riêng cho từng động cơ Sử dụng công cụ MATLAB, nhóm đã tìm ra hàm truyền và so sánh, khảo sát sự khác biệt giữa các hàm truyền và đồ thị đáp ứng của mỗi động cơ.
Hình 4.13 Khảo sát đầu vào và đầu ra của động cơ thứ nhất
Hàm truyền của động cơ thứ nhất là:
Hình 4.14 Khảo sát đầu vào và đầu ra của động cơ thứ hai
Hàm truyền của động cơ thứ hai là:
Hàm truyền của động cơ thứ 3 là:
Hình 4.16 So sánh ba động cơ
Dựa vào đồ thị 4.16, có thể nhận thấy rằng mặc dù cả ba bánh xe đều thuộc cùng một hãng sản xuất và có thông số động cơ giống nhau, nhưng hàm truyền và đồ thị vẫn có sự chênh lệch nhỏ Bên cạnh đó, cần tiến hành tìm bộ thông số PID để tối ưu hóa hiệu suất.
Từ hàm truyền hệ thống (4.25), ta có a1, 216 10 5 b33, 47 c244,8
Hàm truyền tổng của hệ thống có dạng:
Từ phương trình 4.29, ta suy ra được phương trình đặc trưng:
Xét điều kiện để hệ thống ổn định theo tiêu chuẩn Routh với phương trình đặc trưng
0 n n n a s a s a s , bảng Routh tổng quát như sau:
Bảng 4.1 Bảng Routh tổng quát s n c 11 a 0 c 12 a 2 c 13 a 4 …
Với hàm đặc trưng như phương trình 4.30, ta lập được bảng Routh như bảng 4.2:
Bảng 4.2 Tính thông số cho bảng Routh s 3 1 c aK P s 2 b aK D aK I 1
Để hệ thống ổn định, tất cả các nghiệm của phương trình đặc trưng phải nằm bên trái mặt phẳng, tức là các phần tử trong cột đầu tiên của bảng Routh phải đều dương Do đó, điều kiện cần và đủ để đảm bảo sự ổn định của hệ thống là các yếu tố này phải được thỏa mãn.
K b b aK a c aK b aK c aK aK K b aK a aK K
Trong bài toán điều khiển vận tốc, nhóm chỉ sử dụng bộ điều khiển PI, không sử dụng khâu D nên chọn K D 0
Như vậy, với K P 0, 267 và 0 K I 9, 004, nhóm tiến hành mô phỏng đáp ứng để lựa chọn bộ PI tối ưu nhất, các kết quả mô phỏng ở hình 4.17
Hình 4.17 Khảo sát đáp ứng hệ thống
Khi thực nghiệm với giá trị K I = 0,75, đồ thị đáp ứng cho thấy thời gian lâu và có nhiều giao động, không phù hợp Với K I = 2,5 và K I = 4, mặc dù đáp ứng nhanh, nhưng độ nhấp nhô và sai lệch lớn khiến động cơ không ổn định trong thời gian dài và mất nhiều thời gian để đạt trạng thái cân bằng Do đó, nhóm nghiên cứu quyết định chọn K I = 1,5, vì mặc dù thời gian đáp ứng hơi lâu, nhưng độ sai lệch và nhiễu ít hơn, cho thấy tính ổn định cao hơn.
Từ đó, ta có được đồ thị đáp ứng vận tốc như hình 4.18:
Hình 4.18 Đáp ứng vận tốc với PID
Với các thông số PID đã được lựa chọn, đường biểu diễn tốc độ đáp ứng của động cơ thể hiện sự ổn định cao và bám sát chính xác với đường vẽ tốc độ mong muốn.
(Các công thức trong mục 4.2.1.3 được tham khảo từ Giáo trình Điều khiển tự động trường Đại học Sư Phạm Kỹ thuật thành phố Hồ Chí Minh)
4.2.2 Phương thức giao tiếp phần cứng và điều khiển robot
Vì trong đề tài này, nhóm chỉ sử dụng 1 bộ UART trên Raspberry nên phương thức truyền dữ liệu qua UART để điều khiển vận tốc robot như sau:
Ba chân TX của ba vi điều khiển STM32 sẽ được kết nối chung với nhau và nối vào chân RX của Raspberry Đồng thời, ba chân RX của các vi điều khiển sẽ được kết nối chung và nối vào chân TX của Raspberry.
- Raspberry gửi lần lượt 3 giá trị vận tốc của động cơ cho 3 STM32.
XÂY DỰNG PHẦN MỀM TRÊN ROS
Giới thiệu nền tảng ROS
Hình 5.1 Logo hệ điều hành ROS
ROS (Robot Operating System) là nền tảng mã nguồn mở chuyên dụng cho robot, cung cấp các dịch vụ như hệ điều hành Nó hỗ trợ giao tiếp phần cứng, điều khiển thiết bị cấp thấp, thực hiện các chức năng thông dụng, truyền thông giữa các tiến trình và quản lý gói tin.
ROS là hệ điều hành dành cho robot, nổi bật với khả năng thực hiện các tác vụ song song, giúp robot thực hiện nhiều công việc như điều hướng, xử lý dữ liệu và giao tiếp với cảm biến một cách linh hoạt Hệ thống này đi kèm với nhiều thư viện và công cụ hỗ trợ, bao gồm vẽ đồ thị, lưu trữ dữ liệu và hiển thị mô hình 3D Với thiết kế module, ROS cho phép các nhóm nghiên cứu tập trung vào lĩnh vực cụ thể như bản đồ hóa hoặc định vị, đồng thời tận dụng khả năng tái sử dụng các module đã phát triển để xây dựng hệ thống robot hoàn chỉnh hiệu quả.
ROS có những ưu điểm nổi trội sau:
- Gọn nhẹ: ROS được thiết kế nhỏ gọn, vì vậy có thể tích hợp ROS vào các phần mềm robot
Ngôn ngữ lập trình của ROS bao gồm nhiều lựa chọn hiện đại như Python, C++, và Lisp, cùng với các thư viện thực nghiệm trong Java và Lua.
- Dễ dàng kiểm tra: ROS hỗ trợ các bộ công cụ hỗ trợ việc kiểm tra và giám sát quá trình truyền nhận thông tin
- ROS cung cấp nhiều module giúp người dùng tùy biến để phù hợp với yêu cầu xây dựng từng loại robot
5.1.2 Cấu trúc thư mục và một số thuật ngữ trong ROS
5.1.2.1 Cấu trúc thư mục trong ROS
Trong ROS, các package đóng vai trò là đơn vị tổ chức phần mềm chính, bao gồm các thành phần như nodes (chương trình), thư viện ROS, tập dữ liệu, tệp cấu hình và các dữ liệu cần thiết khác Mục đích chính của các package là cung cấp chức năng cho phép phần mềm khác tái sử dụng.
Cấu trúc của một thư mục package thông thường bao gồm:
- Tệp package manifest (package.xml): Cung cấp thông tin ngắn gọn về package như tên package, tác giả, phiên bản, mô tả và các gói phụ thuộc
- Các message type (msg): Định nghĩa cấu trúc dữ liệu để gửi và nhận thông tin qua topic trong ROS
- Các service type (srv): Định nghĩa cấu trúc dữ liệu để gửi và nhận dữ liệu thông qua service trong ROS
ROS hỗ trợ các nhà phát triển bằng cách sử dụng các packages, cho phép họ tập trung vào việc xây dựng các thành phần riêng lẻ mà không cần phải viết lại các chức năng đã có Điều này giúp tăng tốc quá trình phát triển phần mềm trong hệ thống ROS.
5.1.2.2 Một số thuật ngữ trong ROS
ROS Master đóng vai trò là máy chủ kết nối các node trong mạng lưới, cho phép chúng giao tiếp và tìm thấy nhau Trong một hệ thống phân tán, ROS Master nên được chạy trên một máy tính chủ, giúp các node từ xa có thể tương tác thông qua nó Giao tiếp giữa Master và các node slave được thực hiện thông qua XMLRPC, một giao thức không duy trì kết nối dựa trên HTTP Điều này có nghĩa là các node slave chỉ có thể truy cập ROS Master khi cần đăng ký thông tin hoặc yêu cầu dữ liệu từ các node khác.
Node trong ROS là đơn vị xử lý nhỏ nhất, giao tiếp với master qua XMLRPC và giữa các nút thông qua XMLRPC hoặc TCPROS5 của giao thức TCP/IP Mỗi Node được phát triển bằng các thư viện ROS client như roscpp và rospy, và cần được thiết kế chi tiết cho từng nhiệm vụ cụ thể Trong một robot, sẽ có nhiều Node để thực hiện các chức năng khác nhau, với mục tiêu xây dựng các quy trình đơn giản thay vì một quy trình lớn ROS khuyến nghị tạo một Node riêng cho từng mục đích, dễ dàng tái sử dụng, chẳng hạn như Node điều khiển động cơ, Node định vị robot, và Node hoạch định di chuyển cho robot di động.
Package là đơn vị cơ bản trong ROS, chứa tệp cấu hình cần thiết để khởi chạy các packages hoặc nodes khác Nó bao gồm tất cả các tệp cần thiết cho việc vận hành, như thư viện phụ thuộc ROS, quy trình, bộ dữ liệu và các tệp cấu hình khác.
A metapackage is a collection of packages that share a common purpose An example of a metapackage is the ROS navigation stack, which consists of ten packages, including AMCL, DWA, EKF, and map_server.
- Message: Một node gửi hoặc nhận thông tin giữa các node khác thông qua message
The message data types can include integer, floating point, and boolean The TCPROS and UDPROS communication protocols are employed for message transmission Topics facilitate one-way message delivery, while services are utilized for two-way communication.
Trong ROS, mỗi message được trao đổi thông qua một hệ thống gọi là Topic, tương tự như chủ đề của một cuộc hội thoại Khi một node gửi messages qua một topic, nó được xem là đang xuất bản một topic, trong khi node nhận messages qua topic đó được coi là đang đăng ký nhận Điều đặc biệt là node gửi và node nhận không biết về sự tồn tại của nhau, và có thể đăng ký nhận một topic ảo mà không cần có node gửi nào.
Service refers to the two-way synchronous communication between a client service that requests a specific task and a server service that is responsible for responding to those requests.
Service Server là một máy chủ trong giao tiếp thông qua service message, nơi nhận yêu cầu (request) như đầu vào và truyền phản hồi (respond) như đầu ra Cả yêu cầu và phản hồi đều được biểu diễn dưới dạng messages Khi nhận service request, server thực hiện dịch vụ đã chỉ định và gửi kết quả cho service client dưới dạng phản hồi.
Dịch vụ khách hàng được hiểu là một khách hàng trong quá trình giao tiếp qua tin nhắn dịch vụ, trong đó yêu cầu dịch vụ từ máy chủ và nhận phản hồi như một đầu vào Khách hàng gửi yêu cầu đến máy chủ dịch vụ và nhận lại phản hồi từ hệ thống.
- Publisher: Publish là hành động gửi gói tin (message) vào một topic tương ứng mà
Mỗi Publisher node trong ROS cần đăng ký với master bằng cách cung cấp thông tin như tên node và topic Khi đã đăng ký, các publisher gửi gói tin (message) qua topic đã đăng ký Các subscriber có cùng topic với publisher sẽ nhận thông tin mà publisher gửi.
Điều hướng cho mobile robot
Điều hướng và điều khiển quỹ đạo di chuyển là bài toán quan trọng trong lĩnh vực robot di động Quá trình này liên quan đến việc chỉ đạo và kiểm soát hoạt động của robot để di chuyển và tương tác với môi trường xung quanh Robot có khả năng tự tính toán quỹ đạo đường đi tối ưu đến điểm đích, đồng thời tránh các vật cản trong quá trình di chuyển Bài toán điều hướng robot bao gồm bốn vấn đề chính.
- Xây dựng bản đồ môi trường hoạt động
- Định vị robot trong bản đồ đã xây dựng
- Hoạch định quỹ đạo di chuyển
5.2.1 Xây dựng bản đồ môi trường
Trong lĩnh vực robot di động, việc lập bản đồ môi trường là rất quan trọng để robot có thể hoạt động hiệu quả Robot cần học bản đồ của môi trường chưa biết để xác định vị trí, hoạch định đường đi và thực hiện các tác vụ theo quỹ đạo đã định Để đạt được điều này, nhóm đã sử dụng công nghệ Hector SLAM để xây dựng bản đồ cho môi trường hoạt động của robot.
5.2.1.1 Tổng quan về thuật toán Hector SLAM
SLAM (Simultaneous Localization and Mapping) là một thuật toán mã nguồn mở dùng để tạo bản đồ 2D cho môi trường thực tế bằng cảm biến Laser Scan, trong dự án này nhóm sử dụng Rp LiDAR A1M8 Thuật toán Hector SLAM áp dụng phương pháp Scan Matching kết hợp với dữ liệu từ phép đo hình học của xung Encoder từ ba bánh xe để xác định vị trí của robot.
Hector SLAM là giải pháp lý tưởng cho việc lập bản đồ trong những điều kiện mà thông tin về khung tọa độ của robot không chính xác, không thể đo đạc và có dung sai lớn Để thực hiện thuật toán này, cần có một thiết bị quét tầm xa với tốc độ cao, và Rp LiDAR A1 đáp ứng đầy đủ yêu cầu đó với tốc độ quét 5,5 Hz và tầm quét lên đến 12 m, giúp quét hiệu quả môi trường cần lập bản đồ.
Như hình 5.8 [20], thuật toán SLAM được định nghĩa là việc ước tính vị trí vật cản môi trường và vị trí của robot trong môi trường đó [20]
Dưới dạng xác suất, thuật toán SLAM được biểu diễn dạng phân bố xác suất như sau:
Trong bài viết này, chúng ta xem xét tập hợp các giá trị đầu vào điều khiển từ thời điểm bắt đầu đến thời điểm k, ký hiệu là u 0:k = {u 0, u 1, , u k} Đồng thời, m = {m 1, m 2, , m n} đại diện cho các điểm mốc vị trí vận cản, và z 0:k = {z 0, z 1, , z k} là tập các điểm mốc quan sát được Phân bố xác suất mô tả mối liên hệ giữa vị trí mốc trên bản đồ m và trạng thái của robot x k tại thời điểm k, dựa trên các giá trị quan sát z 0:k và tín hiệu đầu vào điều khiển u 0:k Để tính toán phân bố xác suất này, cần sử dụng mô hình chuyển trạng thái, mô tả cách tín hiệu điều khiển tác động lên sự chuyển đổi giữa các trạng thái của robot, cùng với mô hình quan sát, xác định xác suất thực hiện giá trị quan sát z t khi vị trí robot và vị trí điểm mốc đã được xác định.
Giả định rằng vị trí của robot và bản đồ đã được xác định, các giá trị quan sát là độc lập có điều kiện Dưới giả định này, mô hình chuyển động của robot được mô tả thông qua phân bố xác suất chuyển trạng thái.
Phân bố này giả định tuân theo tiêu chuẩn Markov, trong đó trạng thái x_k chỉ phụ thuộc vào trạng thái ngay trước đó x_{k-1} và tín hiệu điều khiển u_k Ngoài ra, x_k là độc lập với các giá trị quan sát được và bản đồ m.
Thuật toán SLAM được thực thi với hai bước tuần tự lặp lại bao gồm dự đoán (prediction) và chỉnh sửa (correction)
Bước dự đoán được mô tả bởi phân phối:
Do trạng thái của robot x x k , k 1 và bản đồ m là độc lập nên ta có thể viết lại như sau:
Vì x t có tính chất Markov nên (5.5) được viết lại thành:
Sau khi tính toán bước dự đoán, bước chỉnh sửa được thực hiện theo phân bố:
Biểu thức (5.6) và (5.7) trình bày quy trình tính toán đệ quy để xác định xác suất hậu nghiệm cho trạng thái robot x k và bản đồ m tại thời điểm t Quá trình này dựa trên tập quan sát z 0:k và tín hiệu điều khiển đầu vào u 0:k.
(Các công thức trong 5.2.1.1 được tham khảo từ tài liệu [20])
5.2.1.2 Tiền xử lý Hector SLAM
Khi khởi động LiDAR để quét bản đồ, gốc tọa độ của bản đồ sẽ được đặt ở vị trí ban đầu của robot như hình 5.9:
Hình 5.9 Gốc tọa độ cảm biến LiDAR
Thuật toán Hector SLAM sử dụng một bản đồ lưới chiếm dụng (occupancy grid map)
Bản đồ lưới chiếm dụng trong hệ thống Hector SLAM đại diện cho môi trường xung quanh robot, được chia thành các ô vuông nhỏ với kích thước cố định Mỗi ô vuông trên bản đồ biểu thị một vị trí cụ thể và có thể mang một giá trị rời rạc để chỉ ra trạng thái của nó, như ô đó có thể được coi là bị chiếm (occupied), không bị chiếm (free), hoặc không xác định (unknown) Các giá trị này có giới hạn riêng để phân biệt các trạng thái khác nhau, giúp robot xác định và điều hướng trong môi trường một cách hiệu quả.
Hình 5.10 Một điểm bất kì trên bản đồ lưới 2D
Để xác định xác suất chiếm dụng tại từng điểm trên bản đồ, chúng ta không thể trực tiếp thay đổi các giá trị mà phải nội suy từ các dữ liệu đo được từ cảm biến Lidar Bộ lọc Bilinear được áp dụng để nội suy giá trị trong các ô con trên lưới, giúp ước tính các giá trị trên bề mặt liên tục tại mọi điểm Ví dụ, một điểm P m được LiDAR trả về trong bản đồ liên tục (hình 5.10 [21]) Gradient của chỗ chiếm dụng được tính toán theo một công thức cụ thể.
Giá trị chiếm dụng được tính gần đúng thông qua phương pháp Bilinear, sử dụng nội suy tuyến tính với các tọa độ nguyên gần nhất P00, P10, P01 và P11.
Từ đó, các dẫn xuất (derivatives) có thể xấp xỉ như sau:
(Các công thức trong 5.2.1.2 được tham khảo từ tài liệu tham khảo [21])
Quá trình scan matching là việc so khớp và căn chỉnh dữ liệu quét laser mới với dữ liệu đã có hoặc bản đồ hiện tại Mục tiêu chính của quá trình này là xác định sự tương quan tốt nhất giữa các dữ liệu quét laser, giúp xác định vị trí của robot trong môi trường hoặc cập nhật bản đồ một cách chính xác.
Khi robot di chuyển, LiDAR cũng di chuyển theo, dẫn đến sự dịch chuyển của các điểm LiDAR trong môi trường quét Điều này làm cho giá trị chiếm dụng của một điểm trên bản đồ bị lệch tương ứng với sự dịch chuyển của LiDAR, khiến cho bản đồ trở nên không chính xác Để khắc phục vấn đề này, quá trình scan matching trong SLAM được áp dụng nhằm tối ưu hóa sự tương quan giữa các điểm cuối đã được vẽ trên bản đồ và các điểm cuối mới nhất thu thập từ cảm biến LiDAR Quá trình scan matching đảm bảo rằng bản đồ luôn được cập nhật chính xác khi robot di chuyển.
Phương pháp Gauss-Newton cho phép ước lượng tư thế tiếp theo mà không cần liên kết dữ liệu giữa các điểm cuối Bằng cách sử dụng tư thế ước tính ban đầu là P P x , y , T, quá trình scan matching sẽ tối thiểu hóa lỗi chiếm chỗ của các điểm cuối.
M S (khi M S i = 1 tức là vị trí đó tồn tại vật cản) Để tối thiểu hóa lỗi chiếm chỗ của điểm cuối, ta dùng công thức:
S i biểu thị sự biến đổi của các điểm kết thúc (endpoints) thu được từ quét Lidar Góc ψ đại diện cho sự thay đổi giữa hai điểm endpoints so với gốc tọa độ gắn trên robot Công thức tính S i được xác định dựa trên các yếu tố này.