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.
- 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)

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án | Công thức |
|---|---|
| Thuận: từ tốc độ bánh ra vận tốc xe | v = (vR + vL) / 2 ; ω = (vR − vL) / L |
| Nghịch: từ lệnh ra tốc độ bánh | vR = 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.
2. Ackermann (xe có cơ cấu lái)

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.
| Differential | Ackermann | |
|---|---|---|
| Quay tại chỗ | Có | Không |
| Ổn định tốc độ cao, tải nặng | Trung bình | Tốt |
| Cơ khí | Đơn giản | Phức tạp hơn (cơ cấu lái, cảm biến góc) |
| Planner Nav2 gợi ý | NavFn / Smac 2D | Smac 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ế
- 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).
- 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.
- Lặp lại theo cả hai chiều quay để phát hiện hai bánh có đường kính lệch nhau.
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ý.