Bài đăng kỹ thuật
Triển khai và phân tích hệ thống
Nâng cấp robot mecanum (dự án #7) với lidar 2D: cài driver RPLIDAR, lập bản đồ bằng slam_toolbox, định vị AMCL mô hình omni, tự hành bằng Nav2 bộ điều khiển MPPI chế độ Omni, cộng thêm lớp lọc an toàn dừng/giảm tốc theo vùng quét lidar.
Lidar 2D quét 360° quanh robot là cảm biến chủ lực của robot tự hành trong nhà. Kết hợp với ROS 2 và Nav2, robot mecanum có thể tự lập bản đồ, tự tìm đường đến đích và né vật cản – đồng thời tận dụng khả năng đi ngang để luồn qua chỗ hẹp mà robot vi sai không làm được.
1. Bài toán thực tế
Điều khiển từ xa không phù hợp cho vận hành thực tế. Robot cần tự biết mình ở đâu trên bản đồ, tự lập kế hoạch đường đi, và quan trọng nhất là không va vào người hay vật cản xuất hiện bất ngờ.
2. Mục tiêu dự án
- Đưa dữ liệu lidar lên ROS 2 (
/scan), khai báo đúng TFbase_link → laser. - Lập bản đồ bằng slam_toolbox, lưu bản đồ.
- Cấu hình Nav2 cho robot đa hướng: AMCL dùng
OmniMotionModel, MPPI vớimotion_model: Omni. - Node an toàn độc lập: giảm tốc khi vật cản
3. Phần cứng sử dụng
| Thiết bị | Thông số chính | Vai trò |
|---|---|---|
| Robot mecanum + ROS 2 từ dự án #7 | Đã có /cmd_vel và /odom | Nền tảng |
| Lidar 2D (RPLIDAR A1/A2/C1 hoặc tương đương) | Tầm quét ~ 8–12 m, 5–10 Hz | Quét môi trường |
| IMU (tùy chọn, ví dụ BNO055) | — | Hỗ trợ odometry khi xoay |
| Raspberry Pi 5 / Jetson Orin Nano | ≥ 4 GB RAM | Chạy SLAM và Nav2 |
4. Sơ đồ khối và đấu nối

Luồng dữ liệu: lidar → /scan → slam_toolbox (khi lập bản đồ) hoặc AMCL (khi định vị trên bản đồ đã lưu) → Nav2 planner + controller → /cmd_vel_nav → node lọc an toàn → /cmd_vel → firmware mecanum. Node an toàn đặt cuối chuỗi nên vẫn có tác dụng cả khi điều khiển tay.
Bảng tham chiếu
| Thành phần Nav2 | Tham số quan trọng cho mecanum |
|---|---|
| AMCL | robot_model_type: "nav2_amcl::OmniMotionModel" |
| Controller (MPPI) | motion_model: "Omni", có giới hạn vy_max, vy_std |
| Velocity smoother | Giới hạn gia tốc cho cả 3 trục x, y, θ |
| Costmap | robot_radius hoặc footprint đúng kích thước thật, lớp obstacle từ /scan |
5. Nguyên lý và thuật toán
SLAM. slam_toolbox ghép các lần quét lidar với nhau (scan matching) kết hợp odometry để dựng bản đồ lưới chiếm chỗ (occupancy grid) và tự sửa sai khi robot quay lại chỗ cũ (loop closure).
Mô hình chuyển động đa hướng. Với robot omni, AMCL phải dùng mô hình omni để mô tả nhiễu theo cả trục y; bộ điều khiển phải được phép xuất vận tốc ngang. Nếu để cấu hình vi sai mặc định, robot vẫn chạy được nhưng không bao giờ đi ngang.
Lớp an toàn phản xạ. Nav2 đã tránh vật cản qua costmap, nhưng có độ trễ vài trăm ms. Node an toàn đơn giản làm việc trực tiếp trên /scan: chỉ xét các tia nằm trong cung ±30° quanh hướng vận tốc hiện tại, tìm khoảng cách nhỏ nhất và giới hạn vận tốc theo đó. Nav2 cũng có sẵn collision_monitor làm nhiệm vụ tương tự với các vùng đa giác.
6. Chương trình mẫu
Node lọc an toàn (Python, rclpy):
import math, rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
STOP, SLOW = 0.30, 0.60 # m
CONE = math.radians(30)
class SafetyFilter(Node):
def __init__(self):
super().__init__('safety_filter')
self.scan = None
self.create_subscription(LaserScan, 'scan', self.on_scan, 10)
self.create_subscription(Twist, 'cmd_vel_nav', self.on_cmd, 10)
self.pub = self.create_publisher(Twist, 'cmd_vel', 10)
def on_scan(self, msg): self.scan = msg
def nearest_in(self, heading):
s, best = self.scan, float('inf')
for i, r in enumerate(s.ranges):
if not (s.range_min < r < s.range_max): continue
a = s.angle_min + i * s.angle_increment # góc trong frame lidar
d = math.atan2(math.sin(a - heading), math.cos(a - heading))
if abs(d) < CONE: best = min(best, r)
return best
def on_cmd(self, cmd: Twist):
v = math.hypot(cmd.linear.x, cmd.linear.y)
if self.scan is not None and v > 0.01:
dmin = self.nearest_in(math.atan2(cmd.linear.y, cmd.linear.x))
k = 0.0 if dmin < STOP else 0.4 if dmin < SLOW else 1.0
cmd.linear.x *= k; cmd.linear.y *= k
self.pub.publish(cmd)
def main():
rclpy.init(); rclpy.spin(SafetyFilter())Đoạn mã giả định lidar lắp quay đúng chiều trục x của base_link (góc 0 hướng về phía trước). Nếu lidar lắp xoay, cộng thêm góc lệch từ TF. Khi remap, cho Nav2 xuất ra cmd_vel_nav.
7. Các bước triển khai và kiểm thử
- Chạy driver lidar, xem
/scantrong RViz2 với Fixed Frame = laser; kiểm tra hướng phía trước đúng. - Khai báo URDF/TF
base_link → laserđúng vị trí lắp. - Lập bản đồ:
ros2 launch slam_toolbox online_async_launch.py, lái tay đi hết khu vực, lưu bản đồ bằngmap_saver_cli. - Chạy Nav2 với bản đồ, đặt 2D Pose Estimate và Nav2 Goal trong RViz2.
- Thử đi ngang qua khe hẹp, thử người bước ra chắn đường: robot phải giảm tốc rồi dừng.
8. Lỗi thường gặp và cách xử lý
- Bản đồ bị nhòe, tường đôi: odometry sai (thường do trượt mecanum) hoặc TF lidar sai góc.
- Robot không đi ngang: Nav2 vẫn dùng cấu hình vi sai (motion model DiffDrive, vy_max = 0).
- Robot dừng giữa chừng không lý do: lidar thấy chính thân robot/cột đỡ – lọc bỏ các tia dưới bán kính thân robot.
9. Hướng mở rộng
- Dùng
collision_monitorcủa Nav2 với vùng đa giác thay cho node tự viết. - Kết hợp camera độ sâu để thấy vật thấp hơn mặt phẳng quét của lidar.
- Giao nhiệm vụ vận chuyển giữa các trạm (dự án #69).