Nav2 xuất lệnh /cmd_vel kiểu geometry_msgs/Twist: vận tốc thẳng linear.x (m/s) và vận tốc góc angular.z (rad/s). Driver phải biến cặp số này thành tốc độ từng bánh (động học nghịch), và ngược lại từ encoder tính ra xe đã đi đâu (động học thuận, odometry). Bài này đi qua hai kiểu xe phổ biến nhất trong nhà máy.

Sau bài này bạn sẽ:
  • Tính được tốc độ bánh từ lệnh v, ω và ngược lại.
  • Hiểu giới hạn của xe Ackermann so với xe vi sai khi điều hướng.
  • Viết hàm tích phân odometry từ xung encoder.

1. Differential drive (hai bánh vi sai)

Hình 1. Xe vi sai: vận tốc hai bánh quyết định vận tốc thẳng v và vận tốc góc ω của base_link.
Hình 1. Xe vi sai: vận tốc hai bánh quyết định vận tốc thẳng v và vận tốc góc ω của base_link.

Hai bánh chủ động đặt đối xứng hai bên, cách nhau L (track width), bán kính bánh r, thêm bánh tự lựa (caster) giữ thăng bằng. Đây là cấu hình phổ biến nhất của AGV kéo/kệ và AMR vì đơn giản và xoay được tại chỗ.

Bài toánCông thức
Thuận: từ tốc độ bánh ra vận tốc xev = (vR + vL) / 2 ; ω = (vR − vL) / L
Nghịch: từ lệnh ra tốc độ bánhvR = v + ω·L/2 ; vL = v − ω·L/2
Tốc độ quay bánhφ̇ = vbánh / r (rad/s) ; rpm = φ̇ · 60 / 2π

Ví dụ số: L = 0,50 m, r = 0,075 m, lệnh v = 0,6 m/s, ω = 0,4 rad/s. Khi đó vR = 0,6 + 0,4·0,25 = 0,70 m/s, vL = 0,50 m/s; tốc độ bánh 9,33 và 6,67 rad/s, tức khoảng 89,1 và 63,7 vòng/phút. Qua hộp số 1:20, động cơ phải quay khoảng 1783 và 1273 vòng/phút – đây là con số dùng để chọn động cơ và kiểm tra driver có bị bão hòa không.

Bão hòa tốc độ: nếu một bánh vượt giới hạn mà driver cắt đơn giản, tỷ lệ vR/vL bị méo và xe đi sai cung. Đúng cách là chia cả hai bánh cho cùng một hệ số để giữ nguyên bán kính quay.

2. Ackermann (xe có cơ cấu lái)

Hình 2. Mô hình xe đạp cho Ackermann: góc lái δ và chiều dài cơ sở L quyết định bán kính quay.
Hình 2. Mô hình xe đạp cho Ackermann: góc lái δ và chiều dài cơ sở L quyết định bán kính quay.

Xe kéo, xe nâng tự hành dạng ô tô dùng bánh lái phía trước. Mô hình "xe đạp" gộp hai bánh trước thành một bánh lái với góc δ, chiều dài cơ sở L (khoảng cách trục trước–sau):

  • ω = v · tan δ / L, suy ra góc lái cần: δ = atan(L · ω / v).
  • Bán kính quay R = L / tan δ; có Rmin ứng với δmax, nên không xoay tại chỗ được – khi v = 0 thì không tạo được ω.
  • Ví dụ: L = 1,2 m, v = 0,6 m/s, ω = 0,4 rad/s → R = 1,5 m, δ ≈ 38,7°.

Hệ quả cho điều hướng: với xe Ackermann phải dùng planner tôn trọng bán kính quay (Smac Hybrid-A* hoặc Smac State Lattice trong Nav2) và controller không đòi quay tại chỗ (Regulated Pure Pursuit tắt use_rotate_to_heading). Dùng NavFn + DWB mặc định sẽ sinh đường xe không đi nổi.

DifferentialAckermann
Quay tại chỗCóKhông
Ổn định tốc độ cao, tải nặngTrung bìnhTốt
Cơ khíĐơn giảnPhức tạp hơn (cơ cấu lái, cảm biến góc)
Planner Nav2 gợi ýNavFn / Smac 2DSmac Hybrid-A* / State Lattice

3. Odometry từ encoder

Encoder 1024 xung/vòng đọc ở chế độ x4 cho 4096 count mỗi vòng động cơ; qua hộp số 1:20 là 81 920 count mỗi vòng bánh. Mỗi count ứng với quãng đường 2π·0,075 / 81 920 ≈ 5,75 µm. Trong mỗi chu kỳ (ví dụ 20 ms), lấy số count tăng thêm của mỗi bánh, đổi ra quãng đường, rồi cộng dồn:

import math


class DiffOdom:
    def __init__(self, wheel_radius=0.075, track=0.50, counts_per_wheel_rev=4096 * 20):
        self.m_per_count = 2 * math.pi * wheel_radius / counts_per_wheel_rev
        self.L = track
        self.x = self.y = self.th = 0.0

    def update(self, d_left_counts, d_right_counts, dt):
        dl = d_left_counts * self.m_per_count
        dr = d_right_counts * self.m_per_count
        ds = (dr + dl) / 2.0               # quãng đường tâm xe
        dth = (dr - dl) / self.L           # góc quay thêm
        # tích phân dùng góc giữa chu kỳ (chính xác hơn Euler khi đang quay)
        mid = self.th + dth / 2.0
        self.x += ds * math.cos(mid)
        self.y += ds * math.sin(mid)
        self.th = math.atan2(math.sin(self.th + dth), math.cos(self.th + dth))
        return ds / dt, dth / dt          # v, w để điền vào nav_msgs/Odometry.twist


def inverse(v, w, track=0.50, wheel_radius=0.075, max_wheel=1.0):
    vr, vl = v + w * track / 2, v - w * track / 2
    scale = max(1.0, abs(vr) / max_wheel, abs(vl) / max_wheel)   # giữ nguyên bán kính quay
    return vl / scale / wheel_radius, vr / scale / wheel_radius   # rad/s từng bánh


odo = DiffOdom()
print(odo.update(800, 1000, 0.02))   # ≈ (0.259 m/s, 0.115 rad/s)
print(inverse(0.6, 0.4))             # ≈ (6.67, 9.33) rad/s

Kết quả update được điền vào nav_msgs/Odometry (pose + twist kèm covariance) và phát TF odom → base_link như bài #5.

4. Hiệu chỉnh thông số thực tế

  1. Bán kính bánh: cho xe chạy thẳng 10 m đo bằng thước; rđúng = rcũ · (quãng đo thật / quãng odom báo).
  2. Track width: cho xe xoay tại chỗ 10 vòng; Lđúng = Lcũ · (góc odom báo / góc thật). Bánh cao su bị nén nên L hiệu dụng thường khác L đo bằng thước.
  3. Lặp lại theo cả hai chiều quay để phát hiện hai bánh có đường kính lệch nhau.
Mẹo: gộp IMU (con quay hồi chuyển) với encoder bằng EKF của package robot_localization giúp góc quay chính xác hơn hẳn khi bánh trượt trên sàn epoxy ướt.

5. Lỗi thường gặp

  • Đảo dấu một bánh: hai động cơ lắp đối xứng nên một bánh phải đảo chiều trong driver; quên sẽ khiến lệnh tiến thành xoay tại chỗ.
  • Dùng sai đơn vị: ROS dùng m/s và rad/s; driver động cơ thường nhận rpm hoặc count/s.
  • Encoder tràn số 16 bit: luôn tính hiệu count có xử lý tràn (ép kiểu int16 khi trừ).
  • Covariance để 0: EKF và AMCL hiểu là odom "hoàn hảo" và tin tuyệt đối – đặt giá trị hợp lý.

Tài liệu tham khảo