3.1. Phân tích phần cứng, phần mềm hệ thống
3.1.1. Phần cứng 3.1.1.1. Thiết kế khung xe
Thiết kế xe đơn giản, có câu trúc đối xứng, có khả năng phát triển chức năng
và dễ điều khiển là mục tiêu của thiết kế.
Mô hình xe thiết kế gồm 2 tầng:
e Tầng 1: Đặt các mạch điều khiến, động cơ va board xử lý trung tâm.
e Tầng 2: Đặt bộ nguồn cho toàn bộ hệ thống và cảm biến RPLiDAR AI.
Hình 3.2: Mặt cắt trước của khung xe
Hình 3.1: Mặt cắt bên của khung xe
32
Hình 3.3: Mặt cắt trên của khung xe
Hình 3.1, Hình 3.2, Hình 3.3 là hình ảnh mô phỏng các mặt cắt của khung robot được thiết kế và vẽ trên phần mềm SolidWork. Khung robot được vẽ bằng AutoCAD và cắt mica.
Một số thông số kỹ thuật của khung xe:
e Kích thước khung xe: 30x25x15cm
e Bánh mecanum (Hình 3.4) với đường kính 9.6cm được sử dụng nhằm mục
đích phục vụ cho việc thực thi đa hướng của robot.
Hình 3.4: Bánh mecanum
e_ Động cơ DC servo giảm tốc JGB37 178RBM (Hình 3.5) được sử dụng có
tích hợp thêm Encoder hai kênh AB giúp đọc và điều kiến chính xác vị trí,
33
chiêu quay của động cơ trong các ứng dụng cân độ có chính xác cao: điêu
khiển PID, Robot tự hành...
Hình 3.5: Động cơ DC Servo giảm tốc JGB37
e Mạch cầu H XY-160D (Hình 3.6) là mạch điều khiển động cơ DC Motor
Driver XY-160D. Mỗi mạch cầu H được sử dụng để điều khiển 2 động cơ
DC với công suất tối đa 160W mỗi động cơ (nếu chạy liên tục thì nên giới hạn ở mức 70% công suất tôi đa theo thông số nhà sản xuất), mạch được thiết kế chuẩn công nghiệp sử dụng IC Driver bằng MOSFET hiệu suất
cao, tích hợp Opto cách ly và các thiết kế chống nhiễu TVS, ESD.
Hình 3.6: Mạch điều khiển động cơ DC motor driver XY-160D
34
e Cảm biến, GY-521 6DOF IMU MPU6050 (Hình 3.7) được sử dụng dé đo
6 thông số: 3 trục Góc quay (Gyro), 3 trục gia tốc hướng
(Accelerometer) là loại cảm biến gia tốc phổ biến nhất trên thị trường hiện nay.
Hình 3.7: Cảm biến MPU 6050
3.1.1.2. Mạch nguồn
Mạch nguồn (Hình 3.8) được vẽ bởi Altium và lay module nguồn XL4016 8A làm module nguồn chính cho Nvidia Jetson Nano va mạch thứ cấp, bao gồm RPLiDAR AI va Arduino Mega. Sắp tới robot sẽ được nâng cấp bang module có dòng cao hơn dé chạy 6n định hơn vi đòng của XL4016 8A chi vừa đủ cho toàn bộ robot, trong khi module chỉ có thé giữ dòng 8A trong thời gian ngắn. Cu thé, trong
, 0600 50000
, H@©©6 aoeoooe
oO
oa o ủooooo
Hình 3.8: Mach PCB nguồn
35
khóa luận, mạch nguồn sẽ thay thế module DC-DC BUCK CONVERTER STEP- DOWN 20A. Module này có thê tinh chỉnh dòng và áp băng biến trở với dòng ra tối
đa là 20A và áp từ 1.2V đến 36V thuận tiện cho việc sử dụng.
3.1.1.3. Board hệ thống
e_ Board điều khiển Mach Arduino Mega 2560 Atmega là một board vi diéu khién dua trén ATmega2560. Board này có 54 chân I/O (14 chân PWM ), 16 analog đầu hang vào, 4 UARTs (phần cứng công tuần tự), sử dụng thạch anh 16 MHz, kết nối công USB, một Jack cắm điện, chân ICSP, và một nút reset.
Hình 3.9: Sơ đồ khối điều khiển với Arduino Mega 2560
Hình 3.9 là sơ đồ lắp đặt của khối điều khiển. Hệ thống sử dụng 2 bộ nguồn 12V riêng biệt để cấp nguồn cho mỗi bộ động cơ trước-sau của robot. Ngoài ra, còn sử dụng thêm 1 bộ nguồn dé duy trì hoạt động cho Arduino Mega. Cảm biến MPU6050 cũng được kết nối với Arduino Mega qua giao thức I2C.
36
e Board xử lý trung tâm
Dé có thé đáp ứng được khả năng cũng như tốc độ xử lý, chúng ta cần một board có khả năng xử lý đủ mạnh dé chạy các thuật toán điều khiển có độ phức tap cao. Bên cạnh đó, nhằm giảm kích thước và tăng tính linh động cho sản phẩm thì việc
lựa chọn một bộ vi xử lý tích hợp có kích thước nhỏ là một sự lựa chọn hợp lý. Trên
thị trường hiện nay, board nhúng có rất nhiều giá thành khác nhau dẫn đến khả năng
xử lý khác nhau.
Từ đó, việc phát triển ứng dụng cho robot tự hành trong nhà dựa trên board NVIDIA Jetson là một sự lựa chọn tối ưu.
Jetson Nano cung cấp 472 GFLOPS dé chạy các thuật toán AI hiện dai một
cách nhanh chóng, với một quad-core 64-bit ARM CPU, một 128-core được tích hop
trên NVIDIA GPU, cũng như bộ nhớ 4GB LPDDR4. Có thê chạy song song nhiều
mạng neural và xử lý đông thời một sô cảm biên có độ phân giải cao.
37
Dé thuận lợi cho việc nghiên cứu va phat trién, Jetson Nano Developer Kit (Hình 3.11) được thiết kế boi NVIDIA là sự lựa chọn tốt hon bởi chúng ta sẽ không cần phải thiết kế các module kết nói với các thiết bị khác như HDMI, USB....
Thông số kỹ thuật của Jetson Nano Developer kit được mô tả chỉ tiết tại Bảng 3.1
Bảng 3.1: Thông số kỹ thuật Jetson Nano developer Kit
GPU 128-core Maxwell
CPU Quad-core ARM A57 @ 1.43 GHz
Memory 4 GB 64-bit LPDDR4 25.6 GB/s
Storage microSD
Video Encode 4K @ 30 | 4x 1080p @ 30 | 9x 720p @ 30
(H.264/H.265)
38
Video Decode 4K @ 601 2x 4K @ 301 8x 1080p @ 301 18x 720p @
30 (H.264/H.265)
Connectivity Gigabit Ethernet, M.2 Key E
Display HDMI 2.0 and eDP 1.4
USB 4x USB 3.0, USB 2.0 Micro-B
Others GPIO, I2C, I2S, SPI, UART
Mechanical 100 mm x 80 mm x 29 mm
3.1.2. Phan mém
Hệ thống gồm 3 phan riêng biệt va được mô tả ở Hình 3.12:
gmapping May tinh
map server ca nhan IMU 6050 Encoder Dong co
Path_planning : Arduino Mega
Velocity 7
(tính toán giá trị [1 — Bộ PID
AMCL odometrym, tf)UART
UART (odometry, tf)
Hình 3.12: Tổng quan hệ thống
3.1.2.1. Máy tính nhúng đặt trên robot:
e Cài đặt hệ điều hành ROS melodic với các gói package được cung cấp từ
nhà sản xuất (rplidar_ros), lập trình lại và kế thừa từ cộng đồng ROS trên thé giới.
e Kết nối với cảm biến LiDAR thực hiện quét bản đồ và gửi trả các tín hiệu
quét được về máy tính nhúng xử lý.
e Xử lý dữ liệu trả về, tính toán và gửi trả tín hiệu điều khiển cho khối điều
khiên thực hiện nhiệm vụ.
39
Bảng 3.2 là một đoạn chương trình dùng dé cấu hình các thông số cơ bản của cho cảm biến như cổng kết nối, baudrate, ... trên nằm trong file node.cpp
trong gói package rplidar_ros do nhà sản xuất phần cứng cung cap cho
người sử dụng.
Bảng 3.2: File node.cpp trong package rplidar_ros int main(int argc, char * argv[]) {
nh_private.param<std::string>("serial_port", serial_port, "/dev/ttyACMO"); nh_private.param<int>("serial_baudrate", serial_baudrate, 115200);
nh_private.param<std::string>("frame_id", frame_id, "laser_frame");
nh_private.param<bool>("inverted", inverted, false);
nh_private.param<bool>(""angle_compensate", angle_compensate, false);
nh_private.param<std::string>("scan_mode", scan_mode, std::string());
Cac package được lập trình lại cho phù hợp với robot được mô ta tại Error!
Reference source not found.. Ngoài ra đề thực hiện được nhiệm vụ di chuyển tự động và tránh né vật cản, đồ án có kế thừa lại một số package đã được xây dựng và phát triển bởi cộng đồng ROS trên thế giới, cụ thể được mô tả tại Bảng 3.4
Bảng 3.3: Packega lập trình lại Package Chức năng
my_robot_bringup Khởi động robot lên hệ thống ROS bang cách thiết lập
serial_ node và rplidar_ node.
My_robot_ description | Mô tả robot (URDF), cũng như là noi chứa mô tả của
robot.
40
My_robot_slam Cho phép thực hiện SLAM với hai thuật toán là gmapping
và hector_ slam.
My_robot_ navigation Chức năng cho phép thực hiện Navigation Stack.
Bảng 3.4: Package kế thừa
Package/Metapackage | Tác giả Chức năng
rosserial Michael Ferguson [12] | Truyền di liệu từ máy tinh
Jetson Nano đến Arduino Mega bằng UART.
navigation Eitan Marder-Eppstein | Gồm các package hỗ trợ các
[13] thuật toán cho
Stack
Navigation như costmap2d, global_ planner,
dwa_local_planner, move_base, amcl, map_server.
joint_state_publisher David V.Lu va Jackie
Kay [14]
Nhận mô tả URDF dé Publish các JointState lên Rviz nhầm hiển thị trạng thái của robot trên
màn hình.
Bảng 3.5: Chỉ tiết các packages quan trọng trong hệ thống
Package Node Publish Topic | Subscribe
Topic
My_robot_bringup /my_robot_core /joint_states /cmd_vel
/odom
/tf
Joint_state_publisher /my_state_publisher tt /joint_states
41
rplidar_ros /robot_lds /scan Không có
map_ Server /map_ server /map Không có
move_base /move_base /cmd_vel /move_base_
simple/goal
/move_base /odom action phuc vu
cho
Navigation Stack
amcl /amcl /particlecloud | /scan
/tf /initialpose
/tf
Bang 3.5 mô ta chỉ tiết các gói package quan trong trong hệ thống. Chi tiết các publish - subscribe topic (Hình 2.5) mà từng gói package sử dụng.
3.1.2.2. Máy tính cá nhân
e_ Cài hệ điều hành ROS và kết nối với máy tính nhúng qua mang Wifi
e Dùng dé điều khiến robot khi thực hiện nhiệm vụ vẽ ban đồ và hiển thị giao
diện bản đồ số hóa.
Đề điều khiển robot di chuyền, trên máy tính cá nhân ta kế thừa gói package
42
teleop_twist_keyboard. Package này hỗ trợ điều khiển robot di chuyển theo 8
hướng thông qua các phím được mô tả tại Hình 3.13.
Hình 3.13: Bộ phím điều khiển robot [15]
Dé hién thi giao diện ban đồ và chọn điểm đích cho nhiệm vụ tự hành ta kế
thừa gói package rviz.
PFecaCamecs Measure ⁄/200ieEdmxe Z720NwGGl - PubichPone
Shap.
0765398
0490398 7” 1
Bel Beset Left-click: Rotate, Middle-click: Move X/¥, Right-Cllck/Mouse Wheel: Zoom, Shift: More options.
Hình 3.14: Giao diện của Rviz trên ROS [16]
Hình 3.14 là giao diện chi tiết của Rviz khi khởi động. Giao diện dễ sử dụng, khi đã vẽ và lưu thành công bản đỗ. Ta chỉ cần dùng chuột chọn điểm dich
ngày trên giao diện của Rviz đê gửi xuông cho robot thực hiện nhiệm vu.
3.1.2.3. Khối điều khiển:
e_ Thực thi các lệnh nhận được từ máy tính nhúng.
e Error! Reference source not found. là sơ đồ thuật toán trên Arduino Mega
2560 sử dụng thư viện ros.h dé tạo kết nối với rosserial nhầm publish, subscribe các topic lên hệ thống dé máy tính nhúng có thé nhận được các giá trị từ Arduino gửi lên bao gồm: Odomrtry, JoinState, TF. Đồng thời Arduino
43
Mega sẽ nhận các giá tri điêu khién từ máy tính nhúng gửi xuông: vận toc quay
và vận tôc góc đê điêu khiên động cơ hoạt động đúng.
| SetBaudrate(115200)
nodeHanlde initailize cmd_ vel Subsriber initialize odom Publisher initiailize joint_state Publisher initiailize tf_broadcaster initialize
Motor initailize IMU initialize
Ỷ
Set default odom Set default joint_state prev_update_time = millis()
"—...ÔÔỎ. 14. —
t= millis()
(t- time[2}) >= DRIVE_
INFORMATION_PUBLISH_CYCLE
Get Encoder Data rT a
Calculate Robot
Information ae *
Publish Robot
Move_Base wen ———— t-tTimef0] >= CONTROL_ FALSEý -=
“MOTOR SPEED CYCLE i
Personal Computer
AMCL
=
TRUE
i sườn ¿
Subscribe from. Á i=
cmd_velitopic | Get Linear and TRUE _ 1. imel6|>CONTROL_
————————T————* Angular Velocity & w MOTOR TIMEOUT
i tTime[6] = millis() — _ =
] FALSE—
¥
{Set Setpoint = velocity for|
PID tTime[0] = t
Set Setpoint = 0 for
PID tTime[0] = t
i ...FIÐ And Control Motor every 10ms._
Hình 3.15: Lưu đồ thuật toán trên Arduino Mega 2560 Ngoài ra còn dùng thuật toán PID để điều khiển động cơ chính xác phục vụ tính giá trị Odometry và di chuyển.
44
3.2. Lập trình bộ điều khiển PID
PID Controller Saturation
Encoder “
Encoder
Hinh 3.16: Giai thuat diéu khién PID
Bộ điều khiển nhằm mục đích điều khién tốc độ động cơ đáp ứng với tốc độ mong muốn thông qua tính toán và xác định cho các giá trị Kp, K;, Kp. (Hình 3.16)
© com7 = x
aot LI LIVI LCUẨN ỨIL/LNTIHITALTILTIWITI
—— : 11563 =m 11763 11863 —
Hình 3.17: Giản đồ xung tốc độ đáp ứng 10Rpm
Hình 3.17 mô tả giản đồ xung của bộ PID tìm được cho hệ thống. Đầu vào
của hệ thống và 10Rpm và tín hiệu đầu ra của hệ thống giao động trong khoảng 9-10Rpm. Như vậy là đáp ứng được mong muốn. Vì thế bộ PID được chon để sử
dụng.
3.3. Xây dựng giải thuật chuyển động
Dựa vào giá trị linear_x, linear_y, angula_z từ gói packet move_base ta có thé xây dựng được giải thuật chuyén động cho robot đa hướng với bánh mecanum.
45
(3.1)
Trong đó:
Hình 3.18: Cấu hình robot với 4 bánh đa hướng [17]
Hình 3.18 mô tả chỉ tiết các đại lượng của robot theo kiến trúc bánh mecacum. Với cỏc giỏ trị như 1x, ứy là vận tốc theo trục thang và ngang của robot. w là vận tốc
góc của robot. Ngoài ra l¿, ly là khoảng cách của từng bánh xe so với trọng tâm của robot.
Hiện thực giải thuật chuyên động của robot được mô tả chi tiết tại Bảng 3.6
Bang 3.6: File my_robot_mega.ino mt_driver.setSetpointFL((1 / WHEEL_RADIUS) *
(goal_velocity_from_cmd[LINEARX] - goal_velocity_from_cmd[LINEARY] -
(WHEEL_SEPARATION_WIDTH + WHEEL_SEPARATION_LENGTH) *
46
goal_velocity_from_cmd{ANGULAR]) * radtorpm); // vận tốc của bánh trước -
bên trái
mt_driver.setSetpointFR((1 / WHEEL_RADIUS) *
(goal_velocity_from_cmd[LINEARX] + goal_velocity_from_cmd[LINEARY] +
(WHEEL_SEPARATION_WIDTH + WHEEL_SEPARATION_LENGTH) *
goal_velocity_from_cmd[ANGULAR]) * radtorpm); // vận tốc của bánh trước -
bên phải
mt_driver.setSetpointBL((1 / WHEEL_RADIUS) *
(goal_velocity_from_cmd[LINEARX] + goal_velocity_from_cmd[LINEARY] -
(WHEEL_SEPARATION_WIDTH + WHEEL_SEPARATION_LENGTH) *
godl_velocity_from_cmd[ANGULAR]) * radtorpm); // vận tốc của bánh sau -bên
trái
mt_driver.setSetpointBR((1 / WHEEL_RADIUS) *
(goal_velocity_from_cmd{LINEARX] - goal_velocity_from_cmda[LINEARY] +
(WHEEL_SEPARATION_WIDTH + WHEEL_SEPARATION_LENGTH) *
goal_velocity_from_cmd[ANGULAR)]) * radtorpm); // vận tốc của bánh sau -bên
phải
3.4. Tính toán giá trị Odometry
Hầu hết các vấn đề về robot đều dẫn tới một câu hỏi: Robot đang ở đâu? Robot
có thể đi đến nơi khác bằng cách nào khi mà không biết vị trí hiện tại của nó? Một số nghiên cứu về robot hi vọng rằng các mục tiêu, vị trí có thể được xác định bằng các
dữ liệu của các cảm biến khi mục tiêu nằm trong phạm vi của các cảm biến đó, nhưng một cách tốt hơn đề làm việc đó chính là navigation. Một phương pháp điều hướng
cơ bản, được hầu hết tất cả robot hiện nay sử dụng, là Odometry, sử dụng kiến thức
liên quan đên chuyên động của bánh xe đê ước tính chuyên động của xe.
Mục tiêu của phần này là để chỉ ra cách sử dụng đữ liệu dé tính toán Odometry. Một tập dữ liệu thực sẽ được sử dụng rộng rãi dé cho thấy phương pháp này có thé hoạt
47
động ra sao. Giả sử robot bắt đầu từ điểm gốc. Trạng thái của nó là (x, y, 0) = (0, 0, 0). Nếu robot di chuyên thăng trong ba giây với vận tốc 1 m/ s, thì một dự đoán chính
xác là trạng thái của robot sẽ là (3, 0, 0).
Chúng ta sẽ giả định rằng chiếc xe được dẫn động bằng vi sai: nó có một động cơ ở bên trái và một động cơ khác ở phía bên phải. Nếu cả hai động cơ quay về phía trước, robot đi thắng. Nếu động cơ bên phải quay nhanh hơn động cơ bên trái, robot sẽ di chuyên sang trái. Mục tiêu của chúng ta là đo tốc độ quay của động cơ bên trái và bên phải. Từ điều này, chúng ta có thể đo vận tốc và tốc độ rẽ của chúng tôi, sau đó tích hợp các dữ liệu này dé thu được vị trí. Vận tốc động cơ cho chúng ta hai đại lượng:
độ dịch chuyên khi xoay và độ dịch chuyên khi đi thăng., từ đó chúng ta có vị trí hiện tai của robot (x, y, 0). Như vậy chúng ta có tốc độ góc (angular velocity) được lay dit liệu và tính toán từ MPU6050 và tốc độ tuyến tinh (linear velocity) được lay dữ liệu
từ encoder của động cơ.
x(E) = (601 + Wz + 6x + 604) or (3.2) y(£) = (Ta + W2 + 6x — 60x) ` (3.3)
YAWnew = VAWoig + Axis,. step_từme (3.4)
Giá tri odometry được sử dung để ước tinh ước tinh thay đổi vi trí theo thời
gian.
teensy imu_calib imu_madgwick_filter robot_localization
$--- + 4--- + $--- + $--- +
| IMU DRIVER | +--- > | IMU RELAY | +--- > | AHRS ALGORITHM | +--- >| |
tenn --- 2+ --- + . + $o----2--- + | |
“raw_imu' "imu/data_raw" "imu/data' | |
| EKF | +--- >
teensy lino_base_node | | ‘odom tenn --- 2+ --- + . + | |
| ENCODER DRIVER | +--- > | ENCODER RELAY | +--- >| |
$--- + $--- + $--- +
“raw_vel' “rau _odom"
Hình 3.19: Sơ đồ giải thuật tính giá trị Odometry[17]
Để tính toán giá trị odometry ta cần dữ liệu góc quay trả về từ MPU 6050 và tốc
độ trả về từ bộ encoder có sẵn trên động cơ. Lưu đồ giải thuật được trình bày ở
48
Hình 3.19 và hiện thực giải thuật được trình bày chỉ tiết trong file
my robot_megda.ino (Bảng 3.7).
Bảng 3.7: File my_robot_mega.ino theta = yaw * PI/ 180;
delta_theta = theta - last_theta;
w = delta_theta / step_time;
vx = (mt_ driver.getSpeedFL(Q + mt_ driver.getSpeedFRQO + mt_ driver.getSpeedBLQ + mt_driver.getSpeedBRQ) / radtorpm * (WHEEL_RADIUS/4);
vy = (- mt_driver.getSpeedFLQ + mt_ driver.getSpeedFRQ + mt_driver.getSpeedBL()
- mt_driver.getSpeedBRQ) / radtorpm * (WHEEL_RADIUS / 4);
delta_x = (vx * cos(odom_pose[2]) - vy * sin(odom_pose[2])) * step_time;
delta_y = (vx * sin(odom_pose[2]) + vy * cos(odom_pose[2])) * step_time;
Dé kiểm tra giá trị odometry (vận tốc tuyến tinh và vận tốc góc) ta thực hiện 2
phép thử sau:
e _ Điều khiển robot về phía trước và kiểm tra /odom nếu vận tốc tuyến tính theo
trục x xấp xi bang tốc độ được xác định tai teleop_twist_keyboard (0,5 m/s).
Vận tốc tuyến tính phải dương khi bạn lái tới và âm khi bạn lùi. Hình 3.20 trái
49
là giá trị vận tốc tuyến tính ta mong muốn (0.5m/s) và Hình 3.20 phải là giá trị trả về từ hệ thống (4.96). Hai giá trị này sắp sỉ thể hiện hệ thống hoạt động đúng với mong muốn.
e Đối với robot truyền động mecanum, ban cũng cần kiểm tra vận tốc tuyến tính
theo trục y. Trước tiên, bạn cần phải di chuyên robot bằng cách nhấn SHIFT + j dé di chuyền sang trái va SHIFT + ¡ dé di chuyên sang phải. Lan này, hãy kiểm tra vận tốc chuyền động thắng theo trục y. Tốc độ phải xấp xi bằng tốc
độ được xác định tại teleop_twist_keyboard. Di chuyền sang trái sẽ trả về một vận tốc thăng đương và âm khi bẻ lái sang phải. Tiếp theo, xoay robot ngược
chiều kim đồng hồ (nhân ‘j' từ teleop_twist_keyboard) và kiểm tra /odom nêu vận tốc góc gần bằng với tốc độ được xác định tại teleop_twist_keyboard. Hình 3.21 trái là giá trị vận tốc góc mong muốn (Irad/s) Hình 3.21 phải là giá tri vận tốc góc trả về từ hệ thống (0.99). Hai giá trị này sắp sỉ băng nhau thể hiện
hệ thống hoạt động ồn định.
3.5. Tính toán giá trị tf (Transform)
Mỗi giá trị trả về từ cảm biến LiDAR là giá trị tính từ vị trí cảm biến đến vật
cản(base_ laser), nhưng trong các giải thuật, tính toán ta luôn tính từ trọng tâm cua
(0.3, 0.0, 0.0)
base_laser in "base_laserTM
base link
Hinh 3.22: Gia tri tf
50