Hợp nhất Odometry: kết hợp Encoder, IMU, và RF2O trong một node

Ba nguồn ước lượng chuyển động đã giới thiệu riêng lẻ: Encoder (bài Differential Drive), IMU (bài Sensor Fusion), và RF2O (bài trước). Bài này ghép cả ba vào một cấu hình robot_localization thực tế — không phải chỉ 2 nguồn như ví dụ đơn giản ở bài Sensor Fusion.

Khái niệm chính

Nguyên tắc cốt lõi khi hợp nhất nhiều nguồn: mỗi nguồn chỉ đóng góp đúng phần nó mạnh nhất, không phải toàn bộ dữ liệu nó có.

Encoder (wheel_odom)  → chính xác cho x, y KHI KHÔNG trượt bánh
RF2O                   → chính xác cho yaw/vyaw, KHÔNG phụ thuộc trượt bánh
IMU (gyroscope)         → chính xác cho vyaw tức thời, phản ứng nhanh với thay đổi đột ngột

Nguyên lý hoạt động

ekf_local:
  ros__parameters:
    odom0: /wheel_odom
    odom0_config: [true,  true,  false,
                    false, false, false,
                    false, false, false,
                    false, false, false,
                    false, false, false]   # chỉ tin x, y từ encoder

    odom1: /rf2o/odom
    odom1_config: [false, false, false,
                    false, false, true,
                    false, false, false,
                    false, false, true,
                    false, false, false]   # tin yaw, vyaw từ RF2O

    imu0: /imu/data
    imu0_config: [false, false, false,
                   false, false, false,
                   false, false, false,
                   false, false, true,
                   false, false, false]     # tin vyaw (gyroscope) từ IMU

Đây là ứng dụng trực tiếp nguyên tắc đã nêu ở bài Sensor Fusion: không có nguồn nào đáng tin ở mọi khía cạnh, EKF cân trọng số động dựa trên độ không chắc chắn của từng nguồn, phần cấu hình _config chỉ định trước trường dữ liệu nào mỗi nguồn được phép đóng góp.

Tóm lại: Có nhiều hơn 2 nguồn không phải để "an toàn hơn" một cách mơ hồ — mỗi nguồn thêm vào giải quyết đúng điểm mù của các nguồn còn lại. RF2O bù cho encoder khi trượt bánh; IMU bù cho cả hai khi cần phản ứng tức thời với thay đổi hướng đột ngột. Thêm nguồn không đóng góp gì mới (trùng điểm mạnh với nguồn đã có) chỉ làm hệ thống phức tạp hơn mà không cải thiện độ chính xác.

Kiến thức kỹ thuật

Công thức

Mảng _config có đúng 15 phần tử, ứng với 15 chiều trạng thái mà robot_localization theo dõi — nắm được cấu trúc này giúp tránh nhầm lẫn khi cấu hình:

Thứ tự 15 chiều: [x, y, z, roll, pitch, yaw,
                   vx, vy, vz, vroll, vpitch, vyaw,
                   ax, ay, az]

VD: odom0_config chỉ bật vị trí [true, true, false, ...] (phần tử 1,2)
    → nghĩa là chỉ tin x, y từ encoder, bỏ qua mọi thành phần khác
    kể cả khi topic /wheel_odom có publish đầy đủ dữ liệu vận tốc

Thông số

Nguồn	Trường được tin	Lý do
Encoder (`odom0`)	x, y	Chính xác khi không trượt bánh, nhưng không đo trực tiếp hướng đáng tin
RF2O (`odom1`)	yaw, vyaw	Không phụ thuộc trượt bánh, tốt cho hướng
IMU (`imu0`)	vyaw	Phản ứng tức thời với thay đổi hướng đột ngột

Ứng dụng thực tế

Kiểm tra chất lượng fusion bằng cách so sánh với từng nguồn riêng là bước xác nhận quan trọng trước khi tin tưởng cấu hình:

ros2 topic echo /wheel_odom      # chỉ encoder
ros2 topic echo /rf2o/odom       # chỉ RF2O
ros2 topic echo /odometry/filtered  # kết quả sau EKF

Khi robot đi thẳng trên sàn phẳng, ba nguồn nên cho kết quả gần khớp nhau. Khi robot đi qua đoạn sàn trơn (nghi ngờ trượt), /wheel_odom và /rf2o/odom sẽ bắt đầu lệch nhau — quan sát trực tiếp độ lệch này qua ros2 topic echo là cách kiểm chứng thực tế xem sensor fusion có đang hoạt động đúng như kỳ vọng, không chỉ tin vào cấu hình lý thuyết. Kiến trúc robot_localization hỗ trợ số lượng nguồn odom/imu/pose/twist tuỳ ý (đánh số odom0, odom1, odom2...) — ba nguồn ở đây chỉ là ví dụ điển hình cho một AMR không GPS. Robot ngoài trời có thể thêm GPS (pose source), robot có camera có thể thêm visual odometry — cùng nguyên tắc, chỉ khác số lượng và loại nguồn.

Ví dụ thực tế

Kịch bản debug thực tế: robot báo vị trí "nhảy" đột ngột trong log

Bước 1: echo /wheel_odom -> thấy vx, vy hợp lý, không có gì bất thường
Bước 2: echo /rf2o/odom -> thấy vyaw đột biến lớn tại đúng thời điểm nghi vấn
Bước 3: echo /odometry/filtered -> kết quả sau EKF cũng "nhảy" theo

Kết luận: RF2O đang cho dữ liệu nhiễu tại thời điểm đó — có thể do đi qua
khu vực thiếu đặc trưng hình học (đã học ở bài RF2O) — cần giảm trọng số
tin cậy RF2O hoặc kiểm tra lại covariance của topic /rf2o/odom

Lỗi thường gặp

Bật tin toàn bộ trường dữ liệu của mọi nguồn thay vì chỉ phần mạnh nhất — như đã nêu ở phần Tóm lại, làm hệ thống phức tạp hơn mà không cải thiện độ chính xác, thậm chí có thể làm nhiễu lẫn nhau nếu 2 nguồn cùng đóng góp cùng 1 trường với độ tin cậy khác nhau đáng kể.
Nhầm thứ tự phần tử trong mảng _config 15 chiều — như ở mục Công thức, đặt sai vị trí true khiến EKF tin nhầm trường dữ liệu, khó phát hiện lỗi này qua log thông thường.
Không so sánh trực tiếp các nguồn riêng lẻ khi debug — như ví dụ trên, bỏ qua bước xác nhận thực tế mà chỉ tin vào kết quả EKF cuối cùng khiến khó xác định nguồn nào đang gây nhiễu.
Thêm nguồn dữ liệu trùng điểm mạnh với nguồn đã có — không mang lại lợi ích tương xứng độ phức tạp cấu hình thêm vào, cần xác định rõ mỗi nguồn mới bù đắp điểm yếu cụ thể nào trước khi thêm.

Kết luận

Hợp nhất odometry không phải "càng nhiều nguồn càng chính xác" một cách vô điều kiện — nguyên tắc cốt lõi là mỗi nguồn chỉ đóng góp đúng phần nó mạnh nhất, bù đắp điểm mù của các nguồn còn lại. Cấu hình _config 15 chiều là công cụ để hiện thực hoá nguyên tắc này một cách tường minh. Bài cuối cùng của chuyên mục Localization, TF Tree, đi vào cách debug cây hệ toạ độ khi có bất kỳ lỗi nào trong toàn bộ chuỗi các nguồn dữ liệu vừa học.