Odometry cho AMR: pipeline thực tế từ xung encoder tới /odom

Bài Encoder ở chuyên mục Hardware AMR đã học cách đọc xung từ cảm biến. Bài Odometry trong Localization và Động học Differential Drive đã học nguyên lý dead reckoning và công thức tích phân vị trí. Bài này không lặp lại các phần đó — nó ráp chúng thành một pipeline firmware hoàn chỉnh, chạy thực tế trên MCU của một AMR, với 3 tần số khác nhau hoạt động song song.

Khái niệm chính

Pipeline odometry hoàn chỉnh gồm 4 bước nối tiếp, mỗi bước chạy ở một lớp phần cứng/tần số khác nhau:

• Đếm xung (ISR) — ngắt phần cứng, tần số cao nhất, chỉ tăng biến đếm, không tính toán
• Chuyển Δticks thành khoảng cách/vận tốc — chạy định kỳ (50-100Hz), dùng thông số hình học bánh xe
• Tính v, ω từ vận tốc từng bánh — áp dụng công thức Differential hoặc Mecanum đã học ở 2 bài trước
• Tích phân thành x, y, θ và publish — tần số thấp hơn (20-50Hz), đúng chu kỳ ROS2 cần

Nguyên lý hoạt động

Ba bước đầu chạy trên MCU, độc lập với ROS2 — đây là lý do odometry vẫn "sẵn có ngay từ giây đầu tiên" như đã nêu ở bài Odometry trong Localization, không phụ thuộc độ trễ khởi động của tầng phần mềm cao hơn:

1. ISR (ngắt phần cứng, mỗi lần cạnh xung encoder xuất hiện):
   tick_L++ hoặc tick_L-- (tuỳ chiều, xác định qua kênh A/B — đã học ở bài Encoder)

2. Mỗi chu kỳ Δt (VD: 20ms, 50Hz):
   Δtick_L = tick_L_hiện_tại − tick_L_lần_trước
   khoảng_cách_bánh_trái = Δtick_L × khoảng_cách_mỗi_tick
   v_L = khoảng_cách_bánh_trái / Δt

3. Áp dụng công thức Differential/Mecanum đã học:
   v, ω (hoặc vx, vy, ω) từ v_L, v_R (hoặc v1..v4)

4. Tích phân (tần số có thể thấp hơn bước 2-3, VD: 30Hz):
   θ += ω × Δt;  x += v×cos(θ)×Δt;  y += v×sin(θ)×Δt
   → đóng gói thành message nav_msgs/Odometry, publish lên topic /odom

Kiến thức kỹ thuật

Công thức

Hằng số chuyển đổi "khoảng cách mỗi tick" — mắt xích nối dữ liệu thô của encoder với công thức odometry, tính một lần khi thiết kế phần cứng:

khoảng_cách_mỗi_tick = (π × Đường_kính_bánh_xe) / (PPR × Tỉ_số_truyền × 4)

(hệ số ×4 áp dụng khi dùng quadrature encoder đếm cả 2 cạnh lên/xuống của cả 2 kênh —
đã học ở bài Encoder)

VD: đường kính bánh 6.5cm, encoder gốc 11 PPR, tỉ số truyền hộp số 1:34
khoảng_cách_mỗi_tick = (π × 6.5) / (11 × 34 × 4) ≈ 0.0136 cm/tick

Thông số

Bước trong pipeline	Tần số điển hình	Chạy ở đâu
Đếm xung (ISR)	Vài kHz (ngắt phần cứng)	MCU, ngắt cứng
Tính v, ω + PID	50-100Hz	MCU, vòng lặp chính
Publish /odom	20-50Hz	MCU → ROS2, hoặc trực tiếp trên MPU nếu driver chạy ở đó

Ứng dụng thực tế

Message nav_msgs/Odometry publish lên topic /odom là đầu vào trực tiếp cho package robot_localization (đã nhắc ở bài IMU trong AMR) — nơi EKF hợp nhất dữ liệu này với IMU để bù trừ trượt bánh, và sau đó là đầu vào cho AMCL/Nav2 ở tầng Navigation. Một chi tiết dễ bị bỏ sót: message này cần có trường covariance (độ không chắc chắn) hợp lý, không chỉ để mặc định — EKF dùng chính giá trị này để quyết định "tin odometry bao nhiêu phần trăm" so với các nguồn dữ liệu khác khi hợp nhất.

Ví dụ thực tế

import math

DIST_PER_TICK = 0.0136  # cm/tick, tinh o muc Cong thuc
L = 35  # khoang cach 2 banh (cm)
dt = 0.02  # 50Hz

x, y, theta = 0.0, 0.0, 0.0
tick_L_prev, tick_R_prev = 0, 0

def update_odometry(tick_L, tick_R, x, y, theta):
    global tick_L_prev, tick_R_prev
    d_L = (tick_L - tick_L_prev) * DIST_PER_TICK
    d_R = (tick_R - tick_R_prev) * DIST_PER_TICK
    tick_L_prev, tick_R_prev = tick_L, tick_R

    v = (d_L + d_R) / 2 / dt
    omega = (d_R - d_L) / L / dt

    theta += omega * dt
    x += v * math.cos(theta) * dt
    y += v * math.sin(theta) * dt
    return x, y, theta

# Mo phong 1 chu ky: banh phai quay nhieu hon banh trai (re trai nhe)
x, y, theta = update_odometry(tick_L=50, tick_R=70, x=x, y=y, theta=theta)
print(f"x={x:.4f} y={y:.4f} theta={math.degrees(theta):.2f} do")

Output:
x=0.0136 y=0.0000 theta=0.65 do

Sau đúng 1 chu kỳ 20ms, robot đã dịch chuyển và xoay một góc rất nhỏ — pipeline này lặp lại 50 lần mỗi giây, mỗi lần tích luỹ một bước rất nhỏ, đúng bản chất dead reckoning đã học ở bài Odometry trong Localization.

Lỗi thường gặp

• Quên hệ số ×4 quadrature khi tính khoảng_cách_mỗi_tick — như đã cảnh báo ở bài Encoder, dùng nhầm PPR gốc thay vì PPR hiệu dụng khiến toàn bộ pipeline sai lệch hàng chục lần dù logic code hoàn toàn đúng cấu trúc.
• Tràn số nguyên (integer overflow) của biến đếm tick — nếu dùng kiểu số nguyên nhỏ (VD: 16-bit) cho biến đếm chạy liên tục hàng giờ, biến đếm tràn về 0 gây một bước nhảy sai lệch lớn trong Δtick tại đúng thời điểm tràn số.
• Đọc tick_L và tick_R không cùng một thời điểm — nếu code đọc tick_L rồi thực hiện việc khác rồi mới đọc tick_R, độ trễ giữa 2 lần đọc (dù rất nhỏ) làm lệch cặp giá trị dùng để tính v, ω cùng một chu kỳ — nên đọc cả 2 gần như đồng thời (tắt ngắt tạm thời khi đọc, hoặc đọc atomic).
• Publish /odom với covariance mặc định không phản ánh thực tế — như đã nêu ở phần Ứng dụng, EKF cần giá trị covariance hợp lý để cân đối độ tin cậy giữa các nguồn dữ liệu, không nên để nguyên giá trị mặc định của template code mẫu.

Kết luận

Odometry trong một AMR thực tế không phải một công thức chạy một lần — nó là một pipeline 4 bước chạy liên tục ở 3 tần số khác nhau, từ ngắt phần cứng tốc độ kHz tới publish ROS2 tốc độ vài chục Hz. Nắm đúng pipeline này giúp debug hiệu quả hơn nhiều so với chỉ hiểu công thức toán đơn thuần — phần lớn lỗi odometry thực tế nằm ở khâu chuyển đổi dữ liệu thô (tick, hằng số hình học, tần số đọc), không phải ở công thức tích phân. Đây cũng là bài cuối của chuyên mục AMR Drive System — chuyên mục AMR Software Architecture tiếp theo đặt toàn bộ pipeline này vào đúng vị trí trong 5 tầng phần mềm đã giới thiệu ở bài Kiến trúc AMR.