Điều khiển tốc độ (Velocity Control): từ /cmd_vel tới PWM động cơ

Bài ros2 topic đã dùng lệnh ros2 topic pub /cmd_vel geometry_msgs/msg/Twist để gửi lệnh vận tốc. Nhưng gửi lệnh chỉ là mong muốn — driver động cơ phải tự tìm cách đạt đúng tốc độ đó trong thực tế, bất kể tải nặng/nhẹ, sàn nhám/trơn, hay pin đang yếu dần. Đây chính xác là bài toán velocity control.

Khái niệm chính

Cách đơn giản nhất — không dùng phản hồi gì cả — là ánh xạ trực tiếp tốc độ mong muốn sang một giá trị PWM cố định, dựa trên thông số động cơ lý thuyết:

PWM = tốc_độ_mong_muốn / tốc_độ_tối_đa × 255

Vấn đề: PWM 50% trên sàn phẳng cho tốc độ X, nhưng cùng PWM 50% đó trên sàn nghiêng hoặc khi robot chở thêm tải sẽ cho tốc độ thấp hơn X. Đây gọi là điều khiển open-loop (vòng hở) — không có cách nào tự phát hiện và sửa sai lệch.

Nguyên lý hoạt động

Velocity control đúng nghĩa cần một vòng phản hồi: đọc tốc độ thực tế từ encoder, so sánh với tốc độ mong muốn, dùng PID liên tục chỉnh PWM để triệt tiêu sai lệch:

loop (tần số cố định, ví dụ 100Hz):
    toc_do_thuc_te = doc_encoder()
    error = toc_do_mong_muon − toc_do_thuc_te
    pwm = pid.compute(error, dt)
    set_pwm(pwm)

Tóm lại: Open-loop chỉ đúng khi mọi điều kiện đều lý tưởng và không đổi. Closed-loop (dùng encoder + PID) là cách duy nhất đảm bảo tốc độ thực tế bám sát tốc độ mong muốn khi điều kiện vận hành thay đổi liên tục.

Hai tầng velocity control trong một robot ROS2:

Tầng cao (ROS2, trên MPU): /cmd_vel (v, ω) → inverse kinematics → v_L, v_R mong muốn
Tầng thấp (MCU): vòng PID velocity control cho từng bánh, chạy 100Hz-1kHz

Kiến thức kỹ thuật

Công thức

Sai số chấp nhận được (steady-state error) cho velocity control trên AMR thường tính theo phần trăm tốc độ mục tiêu, không phải giá trị tuyệt đối cố định:

Sai số (%) = |Tốc độ thực tế − Tốc độ mục tiêu| / Tốc độ mục tiêu × 100%

VD: mục tiêu 0.5 m/s, sau khi PID ổn định đo được 0.48 m/s
Sai số = |0.48 − 0.5| / 0.5 × 100% = 4%

Ngưỡng chấp nhận điển hình cho AMR: dưới 2-5% ở trạng thái ổn định

Thông số

Trong hệ sinh thái ROS2, không phải ai cũng tự viết vòng velocity control từ đầu — package ros2_control cung cấp sẵn kiến trúc chuẩn hoá cho việc này:

Thành phần ros2_control	Vai trò
diff_drive_controller	Nhận /cmd_vel, tự tính inverse kinematics ra tốc độ từng bánh
Hardware Interface	Lớp trừu tượng giao tiếp với driver động cơ thật (đọc encoder, ghi lệnh)
Controller Manager	Quản lý vòng đời và tần số chạy của các controller

Ứng dụng thực tế

Phần lớn dự án AMR dùng ROS2 hiện đại không tự viết lại toàn bộ pipeline /cmd_vel → PID → PWM từ đầu — ros2_control với diff_drive_controller (hoặc tương đương cho Mecanum) đã chuẩn hoá sẵn kiến trúc này, chỉ cần viết đúng "Hardware Interface" kết nối với driver động cơ cụ thể của robot (đọc encoder thật, ghi giá trị PWM/dòng điện thật). Cách tiếp cận này tách biệt rõ phần logic điều khiển chuẩn (dùng lại được giữa nhiều robot khác nhau) khỏi phần driver phần cứng cụ thể (khác nhau tuỳ robot), giảm đáng kể lượng code cần tự viết và debug so với triển khai thủ công hoàn toàn từ đầu.

Ví dụ thực tế

$ ros2 topic pub /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.5}}" --once
$ ros2 topic echo /wheel_velocities

Output theo thời gian (mô phỏng, robot leo dốc nhẹ giữa chừng):
t=0.0s: target=0.50, actual=0.00   (vừa nhận lệnh)
t=0.5s: target=0.50, actual=0.47   (đang tăng tốc, PID đang chỉnh)
t=1.0s: target=0.50, actual=0.49   (gần ổn định, sai số ~2%)
t=1.5s: target=0.50, actual=0.41   (bắt đầu leo dốc, tụt tốc độ)
t=2.0s: target=0.50, actual=0.49   (PID đã bù lại, tốc độ ổn định trở lại)

Chuỗi số liệu này minh hoạ đúng hành vi closed-loop đã nói ở phần Nguyên lý hoạt động — tốc độ thực tế tụt xuống ngay khi gặp dốc (t=1.5s), nhưng PID tự động phát hiện sai lệch và bù lại trong vòng nửa giây, không cần biết trước độ dốc là bao nhiêu.

Lỗi thường gặp

• Nhầm đơn vị giữa /cmd_vel (m/s, rad/s) và PWM thô — quên quy đổi đúng hệ số giữa đơn vị vật lý chuẩn ROS2 và giá trị PWM 0-255/0-1000 nội bộ của driver, gây robot chạy sai tốc độ dù message gửi đúng giá trị.
• Không xử lý timeout khi /cmd_vel ngừng publish — nếu node điều khiển cấp cao (Nav2, teleop) bị crash hoặc mất kết nối, robot phải tự dừng nếu không nhận lệnh mới trong một khoảng thời gian nhất định — thiếu cơ chế này là lỗ hổng an toàn nghiêm trọng, robot có thể tiếp tục chạy theo lệnh cuối cùng vô thời hạn.
• Thiếu giới hạn (clamp) tốc độ tối đa — nhận thẳng giá trị từ /cmd_vel mà không kiểm tra giới hạn an toàn của robot, một lệnh sai (do bug ở tầng trên hoặc dữ liệu hỏng) có thể ra lệnh tốc độ vượt xa khả năng an toàn của phần cứng.
• Độ trễ phản hồi encoder qua mạng làm PID không ổn định — nếu vòng velocity control chạy trên máy khác với nơi đọc encoder (qua mạng thay vì cục bộ trên MCU), độ trễ mạng cộng thêm vào vòng phản hồi có thể gây dao động — đây là lý do vòng PID tốc độ luôn nên chạy cục bộ trên MCU gần động cơ nhất, không qua mạng.

Kết luận

Velocity control biến lệnh /cmd_vel từ "mong muốn" thành tốc độ thực tế đáng tin cậy, bất kể tải trọng hay địa hình thay đổi — nhờ vòng phản hồi closed-loop dùng encoder + PID, và trong thực tế thường được chuẩn hoá qua ros2_control thay vì tự viết từ đầu. Bài tiếp theo trong chuyên mục nói về Giới hạn gia tốc — vì sao đạt đúng tốc độ mục tiêu ngay lập tức chưa chắc đã là điều robot nên làm.