3.2.1. Sơ đồ khối mạch điều khiển
Hình 3.6: Sơ đồ khối hệ thống điều khiển
Để xây dựng một con robot có khả năng di chuyển định vị và tạo lập bản đồ trong phòng nhóm đồ án chúng em đã nghiên cứu tìm hiểu tài liệu để xây dựng lên một sơ đồ điều khiển như trên hình. Và được xây dựng dựa trên thành phần cơ bản:
- Raspberry Pi 3 Module B: nhóm đã chọn Raspberry Pi 3 Model B những ưu điểm như công suất cao, tiêu thụ năng lượng thấp, bộ nhớ lớn, khả năng lập trình dễ dàng, tốc độ xử lý cao.
- Arduino Uno: Được nạp chương trình thông qua Raspberry Pi 3 Module B xuất tín hiệu điều khiển tới module điều khiển mạch động cơ L298. - Module điều khiển động cơ L298: được kết nối với Arduino Uno thông
qua các chân IO, L298 thu nhận tín hiệu điều khiển từ Arduino điều khiển hoạt động của động cơ. Mạch điều khiển động cơ DC L298 dễ sử dụng, chi phí thấp, dễ lắp đặt, là sự lựa chọn tối ưu trong tầm giá.
- Cảm biến RPLIDAR A1: RPLIDAR A1 kết nối với Raspberry thông qua cầu UART và đầu cắm chuyển đổi USB.
53
54
55
3.2.2. Lựa chọn thiết bị điều khiển
3.2.2.1. Chọn mạch vi điều khiển Arduino Uno R3
Ngoài ra, nhóm em có sử dụng thêm Ardunio uni R3 để điều khiển động cơ. Aduino uno R3 là một mạch vi điều khiển sử dụng phổ biến dễ dàng lập trình giá cả hợp lí. Bảng mạch được trang bị các bộ chân đầu vào/ đầu ra Digital và Analog có thể giao tiếp với các bảng mạch mở rộng khác nhau.
Hình 3.9: Arduino uno R3
Thông số kỹ thuật:
Vi điều khiển: ATmega328 họ 8 bit
Điện áp hoạt động: 5V DC (chỉ sử dụng nguồn cấp qua cổng USB) Tần số hoạt động: 16 MHz Dòng điện tiêu thụ: khoảng 30mA Điện
áp đầu vào khuyên dùng: 7-12V DC Điện áp vào giới hạn: 6 -20V DC
Số chân Digital I/O: 14(6 chân hardware PWM) Số chân Analog: 6 (độ phân giải 10 bit)
Dòng tối đa trên mỗi chân I/O: 30mA Dòng ra tối đa (nguồn 5V): 500mA
56
Bộ nhớ Flash: 32KB (ATmega328) với 0.5KB dùng bởi bootloader
SRAM: 2KB (ATmega328) EEPROM: 1KB(ATmega328)
3.2.2.2. Lựa chọn mạch công suất
Mạch điều khiển động cơ DC L298 có khả năng điều khiển 2 động cơ DC, dòng tối đa 2A mỗi động cơ, mạch tích hợp diod bảo vệ và IC nguồn 7805 giúp cấp nguồn 5VDC cho các module khác.
Hình 3.10: L298-Mạch điều khiển động cơ
Thông số kĩ thuật:
– IC chính: L298 – Mạch điều khiển động cơ – Điện áp đầu vào: 5~30VDC
– Dòng tối đa cho mỗi cầu H là: 2A, công suất tối đa: 25W/ 1 cầu – Mức điện áp logic: Low -0.3V~1.5V, High: 2.3V~Vss
57 3.2.2.3. Mạch phát hạ áp Buck DC-DC 5A XL4015 Hình 3.11: Mạch phát hạ áp Buck DC-DC 5A XL4015 Thông số kỹ thuật: IC sử dụng XL4015 TO263 Điện áp vào 8 - 36V DC Điện áp ra 1.25 - 32V DC Dòng điện ra Max 5A Hiệu suất 96%
Thay đổi điện áp đầu ra: xoay biến trở (adsbygoogle = window.adsbygoogle || []).push({});
Kích thước 54x23x18mm
3.2.2.4. Pin 3S-12V Li-ion 18650
Chúng em sử dụng pin 3S-12V Li-ion 18650 4000mAh để cấp nguồn cho động cơ.
58
3.2.3. Tính toán và lập trình điều khiển động cơ
Hình 3.13: Sơ đồ hệ thống điều khiển động cơ
Luật điều khiển thường được chọn trên cơ sở đã xác định được mô hình toán học của đối tượng phải phù hợp với đối tượng cũng như thỏa mãn yêu cầu của bài toán thiết kế.
Phương trình toán học: ( ) ( ) p ( ) I ( ) D de t U t K e t K e t dt K dt (3.5) Ảnh Laplace: ( ) I P C d K G s K K s s (3.6)
59
Hình 3.14: Sơ đồ khối bộ điều khiển PID
Khâu tỉ lệ, tích phân, vi phân được cộng lại với nhau để tính toán đầu ra của bộ điều khiển PID. Định nghĩa rằng u(t) là đầu ra của bộ điều khiển, biểu thức cuối cùng của giải thuật PID là:
0 ( ) ( ) ( ) ( ) ( ) e p I d de t u t MV t K e t K e t dt K dt (3.7)
Trong đó các thông số điều chỉnh là:
Độ lợi tỉ lệ, Kp: giá trị càng lớn thì đáp ứng càng nhanh do đó sai số càng lớn, bù khâu tỉ lệ càng lớn. Một giá trị độ lợi tỉ lệ quá lớn sẽ dấn đến quá trình mất ổn định và dao động.
Độ lợi tích phân, Ki: giá trị càng lớn kéo theo sai số ổn định bị khử càng nhanh. Đổi lại là độ vọt lố càng lớn: bất kỳ sai số âm nào được tích phân trong suốt đáp ứng quá độ phải được triệt tiêu tích phân bằng sai số dương trước khi tiến tới trạng thái ổn định.
Độ lợi vi phân, Kd: giá trị càng lớn càng giảm độ vọt lố, nhưng lại làm chậm đáp ứng quá độ và có thể dẫn đến mất ổn định do khuếch đại nhiễu tín hiệu trong phép vi phân sai số.
Có một số phương pháp để chọn thông số PID như là:
1.Trước tiên bạn cài đặt hệ số P trước. từ nhỏ tới lớn đến khi nào đạt được độ cứng cần thiết của hệ thống.
60
2.Thứ 2 cài đặt tới hệ số I. Từ nhỏ tới lớn, quan sát đáp ứng của hệ thống khi các thông số thay đổi. Khi nào đạt được khi thôi
3.Cuối cùng chỉnh tới thông số D. Khi hệ số I tăng thì đáp ứng của hệ thống nhanh nhưng rễ mất ổn định, bị dao động. Bạn cũng tăng từ từ hệ số D lên.
Nhóm sử dụng phương pháp thực nghiệm trên Arduino và động cơ để chọn thông số PID:
– Đưa các giá trị Ki = Kd đồng thời quy chúng về giá trị = 0. Sau đó; ta hiệu chỉnh giá trị Kp tăng dần đến khi nào hệ thống dao động tuần hoàn ổn định thì ngưng.
– Hiệu chuẩn time tích phân tương ứng với chu kỳ dao động tuần hoàn. – Kiểm soát lại giá trị Kp vừa khớp với yêu cầu bài toán. Trong qua trình
hiệu chỉnh; nếu xảy ra hiện tượng thay đổi do dao động đột biến thì ta hiệu chuẩn thêm giá trị Kd.
Mạch điều khiển động cơ dùng cầu H
Mạch cầu H là nguyên lý khá phổ biến trong điều khiển đông cơ. Cấu trúc mạch cầu H là một giải pháp đơn giản có thể tạo ra điện áp lưỡng cực từ nguồn đơn cực.
Hình vẽ trên mô tả một mạch cầu H đơn giản dùng bốn tranzito. Bằng cách đóng mở các cặp tranzito khác nhau (Q1-Q3) hoặc (Q2-Q4), có thể có điện áp lưỡng cực qua tải từ một nguồn cấp đơn cực. Trong nhiều ứng dụng, các cặp tranzito trong cầu H có thể được điều khiển trực tiếp bởi đầu ra của một thiết bị số (TTL hoặc CMOS).
61
Nhóm thực hiên điều khiển động cơ qua ROS với gói “teleop_twist_keyboard” trên bàn phím máy tính. (adsbygoogle = window.adsbygoogle || []).push({});
Hình 3.16: Điều khiển động cơ bằng bàn phím kết hợp Rviz
Thuật toán được lập trình điều khiển trên Arduino Uno R3. Tham khảo phần code chi tiết tại phụ lục 1.
Sơ đồ hệ thống điều khiển robot tham khảo tại bản vẽ 4.
62
3.3. Thiết kế hệ thống định vị và dẫn đường 3.3.1. Xây dựng thuật toán quét bản đồ 3.3.1. Xây dựng thuật toán quét bản đồ
Hình 3.17: Lưu đồ thuật toán SLAM
Robot có thể thực hiện các gói phần mềm SLAM sử dụng các loại thuật toán khác nhau dựa trên nền tảng ROS như Gmapping, Cartographer, hector, karto, tuy nhiên sử dụng phổ biến nhất vẫn là Hector mapping. Với dữ liệu cảm biến laser quét 360 độ, robot vừa có thể tạo bản đồ, vừa có khả năng tự định vị mình trong bản đồ đó. Đối với thuật toán Hector mapping, robot sử dụng bộ lọc để tìm ra tọa độ gần với vị trí thực tế của robot trên bản đồ. Hình 3.6 biểu diễn lưu đồ thuật toán SLAM cho quy trình vừa định vị vừa cập nhật bản đồ vào cơ sở dữ liệu. Quá trình này diễn ra liên tục mỗi khi có dữ liệu từ cảm biến trả về, robot vừa định vị bản thân, vừa cập nhật bản đồ vào cơ sở dữ liệu tại cuối quá trình.
63
3.3.1.1. Thuật toán xây dựng bản đồ (mapping):
a) Cơ sở lý thuyết:
Thuật toán sử dụng để xây dựng bản đồ từ dữ liệu laser phổ biến là Gmapping. Bản đồ được tạo ra là bản đồ dạng lưới, là dạng bản đồ tiêu chuẩn trong các ứng dụng robot. Thuật toán này được đề xuất đầu tiên trong cuốn Rao- Blackwellized Particle Filtering for Dynamic Bayesian Networks của Kevin Murphy và Stuart Russell. Ý tưởng chính là sử dụng bộ lọc hạt Rao- Blackwelliezed. Sau đó chúng được cải tiến bằng cách tối ưu hóa các bản phân phối đề xuất và giới thiệu tính năng lấy mẫu lại thích ứng, làm cho thuật toán phù hợp hơn cho các ứng dụng thực tế. Sau đó nó được gọi là Gmapping (G trong grid có nghĩa là lưới). Trong bài tập lần này chúng em không đề cập sâu tới thuật toán này mà chỉ đề cập tới khái niệm và cách sử dụng các package để tái tạo bản đồ trên nền ROS.
Hình 3.18: Tập hợp điểm cảm biến laser quét được
b) Gmapping và Laser Scan Matcher trong ROS:
Đối với gmapping thì nó yêu cầu một dữ liệu odometry và một cảm biến laser. Trong Ros thì để ứng dụng Gmapping ta sử dụng node slam_gmapping. Để sử dụng slam_gmapping ta cần cung cấp dữ liệu Odometry và dữ liệu mà
64
cảm biến laser publish lên topic /scan. Đầu ra của package này là một bản đồ 2D (nav_msgs/OccupancyGrid), có thể nói như là mặt sàn của môi trường đang dựng. Tương tự như các package khác, chúng ta cần thiết lập launch file cho gmapping. Trong đó, chúng ta có các thông số quan trọng sau:
<param name="map_udpate_interval" value="2.0"/> <param name="delta" value="0.02"/>
<param name="xmin" value="-20"/> <param name="xmax" value="20"/> <param name="ymin" value="-20"/> <param name="ymax" value="20"/>
map_udpate_interval là thời gian giữa hai lần cập nhật map tính bằng giây. delta là độ phân giải của map (đơn vị là mét trên một khối occupancy grid). xmin, xmax, ymin, ymax là quy định kích thước của bản đồ (đơn vị là mét).
Xác định tọa độ của robot so với frame gốc ban đầu
Để tính được dữ liệu Odometry hay xác định vị trí của robot so với frame gốc ban đầu phục vụ cho build map nhóm sử dụng package có tên laser scan matcher để xác định tọa độ dựa vào các tín hiệu laser scan liên tiếp nhau. Hình dưới là map được tạo khi sử dụng gmapping và tf(chuyển đổi tọa độ) tĩnh không sử dụng đến laser scan matcher. Ta có thể thấy rằng nếu chỉ sử dụng gmapping, khi xoay lidar đi 1 góc thì map cũng bị di chuyển theo hướng của lidar. Laser scan matcher sẽ giải quyết vấn đề đó, đồng thời chúng có thể kết hợp với 1 số loại cảm biến khác như encoder, IMU để định vị vị trí tốt hơn cho robot trong môi trường. Tuy nhiên trong bài tập này, chúng em chỉ sử dụng 1 loại cảm biến duy nhất để lập bản đồ đó là lidar. Một số tham số cấu hình khi sử dụng laser scan matcher:
<param name="fixed_frame" value = "odom"/> <param name="base_frame" value="base_frame"/>
65 <param name="use_odom" value="false"/> <param name="use_imu" value="false"/> <param name="publish_pose" value="true"/> <param name="publish_tf" value="true"/>
Các thông số cần cấu hình cho laser_scan_matcher:
Biến fixed_frame dùng để xác định đâu là frame cố định của hệ thống để package có thể tính được vị trí của base frame so với fixed_frame.
Biến use_odom để xác định xem chúng ta có cung cấp nguồn dữ liệu khác là cảm biến đo lường odometry hay không.
Biến use_imu giống như biến use_odom là xác định là chúng ta có đưa thêm cảm biến IMU vào hay không.
Biến publish_pose, publish_tf dùng khai báo xem package này có publish các thông số tf và pose vào môi trường ROS hay không.
Để xây dựng được bản đồ ta cần di chuyển robot xung quanh map 1 lần để robot có thể quét được tất cả các vị trí. Chi tiết tham khảo thêm ở video đi kèm với báo cáo
66 (adsbygoogle = window.adsbygoogle || []).push({});
3.3.1.2. Thuật toán định vị (localization):
a) Cơ sở lý thuyết:
Để thực hiện việc định vị cho robot ta sử dụng phương pháp định vị Monte Carlo thích nghi – AMCL (adaptive Monte Carlo localization).
Monte Carlo Localization (MCL) là một thuật toán định vị một robot bằng cách sử dụng một bộ lọc hạt. Thuật toán yêu cầu một bản đồ đã biết và nhiệm vụ là ước tính tư thế (vị trí và hướng) của robot trong bản đồ dựa trên chuyển động và cảm nhận của robot. Thuật toán bắt đầu với tin tưởng ban đầu về phân bố xác suất của tư thế robot, được biểu thị bằng các hạt được phân phối theo độ tin tưởng đó. Các hạt này được lan truyền theo mô hình chuyển động của robot mỗi khi tư thế của robot thay đổi. Khi nhận được các chỉ số cảm biến mới, mỗi hạt sẽ đánh giá độ chính xác của nó bằng cách kiểm tra khả năng nó sẽ nhận được các chỉ số cảm biến như vậy ở vị trí hiện tại của nó. Tiếp theo, thuật toán sẽ phân phối lại (lấy mẫu lại) các hạt thành các hạt có độ chính xác hơn. Tiếp tục lặp lại các bước chuyển động, cảm biến và lấy mẫu lại này và tất cả các hạt sẽ hội tụ về một cụm duy nhất gần tư thế thực của robot nếu quá trình định vị thành công.
AMCL là một biến thể của MCL. Trong đó AMCL tự động điều chỉnh số hạt dựa trên khoảng cách để đảm bảo rằng phân bố hạt hội tụ về phân bố thực của trạng thái robot dựa trên tất cả các phép đo chuyển động và cảm biến trong quá khứ với xác suất cao.
67
Hình 3.20: Ví dụ bản đồ đã có
AMCL xác định giới hạn dưới và giới hạn trên về số lượng các hạt sẽ được tạo ra trong quá trình lấy mẫu lại. Việc cho phép tạo ra nhiều hạt hơn có thể cải thiện cơ hội hội tụ về tư thế robot thực sự, nhưng có tác động đến tốc độ tính toán và các hạt có thể mất nhiều thời gian hơn hoặc thậm chí không thể hội tụ. Nếu robot biết vị trí ban đầu của nó, thông tin đó có thể giúp AMCL định vị robot nhanh hơn với số lượng hạt ít hơn, có thể sử dụng giá trị nhỏ hơn của giới hạn trên.
Trong trường hợp không có ước tính tư thế robot ban đầu, AMCL sẽ cố gắng định vị robot mà không cần biết vị trí ban đầu của robot. Thuật toán ban đầu giả định rằng robot có xác suất ngang nhau khi ở bất kỳ đâu trong không gian trống của khu vực và tạo ra các hạt phân bố đồng đều bên trong không gian đó. Lúc này sẽ AMCL đòi hỏi nhiều hạt hơn đáng kể so với định vị với ước tính đặt ra ban đầu.
68
Hình 3.21: Lần cập nhật đầu tiên của AMCL
Sau khoảng 8 lần cập nhật, các hạt bắt đầu hội tụ đến các khu vực có khả năng xảy ra cao hơn:
Hình 3.22: Lần cập nhật thứ 8 của AMCL
Sau 60 lần cập nhật, tất cả các hạt sẽ hội tụ đến đúng tư thế của robot và các lần quét laze phải căn chỉnh chặt chẽ với đường viền bản đồ:
69
Hình 3.23: Lần cập nhật thứ 60 của AMCl
b) Amcl trong ROS:
Amcl đưa vào một bản đồ dựa trên laser có sẵn đã được thực hiện ở phần mapping, quét laser và chuyển đổi thông báo, và kết quả đầu ra đưa ra các ước tính. Khi khởi động, amcl khởi tạo bộ lọc hạt của nó theo các thông số được cung cấp. Lưu ý rằng, do mặc định, nếu không có tham số nào được đặt, trạng thái bộ lọc ban đầu sẽ là một đám mây hạt (particlecloud) có kích thước vừa phải tập trung vào khoảng (0,0,0).
Các hạt sau khi được tọa từ dữ liệu cảm biến, sẽ được gán cho một trọng số nhất định, sau quá trình lấy mẫu lại, các hạt có trọng số cao hơn sẽ được giữ