EKF trong Nav2: mô hình 2 tầng Local và Global

Bài EKF (chuyên mục Robotics Fundamentals) đã giải thích thuật toán predict-update tổng quát. Trong kiến trúc Nav2 thực tế, package robot_localization thường được triển khai theo một pattern cụ thể: hai EKF riêng biệt, mỗi cái phụ trách đúng một đoạn trong TF tree đã học ở bài Hệ toạ độ trong Robot.

Khái niệm chính

EKF cục bộ (Local) — chỉ fusion odometry + IMU, publish transform odom → base_link
EKF toàn cục (Global) — thêm cả pose từ AMCL, publish transform map → odom

Hai EKF không dư thừa nhau — mỗi cái publish đúng một đoạn TF khác nhau, ghép lại đúng chuỗi map → odom → base_link hoàn chỉnh.

Nguyên lý hoạt động

ekf_local:
  ros__parameters:
    frequency: 30.0
    odom0: /wheel_odom
    imu0: /imu/data
    world_frame: odom
    publish_tf: true    # publish transform odom → base_link

EKF này không nhận dữ liệu từ AMCL — chỉ kết hợp odometry bánh xe và IMU (đúng bài Sensor Fusion), publish transform odom → base_link. Đặc điểm: mượt, tần số cao, liên tục — nhưng vẫn trôi dần theo thời gian vì không có nguồn tuyệt đối nào hiệu chỉnh.

ekf_global:
  ros__parameters:
    frequency: 30.0
    odom0: /wheel_odom
    imu0: /imu/data
    pose0: /amcl_pose      # thêm nguồn AMCL
    world_frame: map
    publish_tf: true       # publish transform map → odom

EKF thứ hai nhận thêm pose từ AMCL làm nguồn "neo" tuyệt đối, publish transform map → odom — đúng đoạn transform "cập nhật thưa hơn, hiệu chỉnh định kỳ" đã nói ở bài Hệ toạ độ trong Robot và Odometry trong Localization.

Nếu chỉ dùng một EKF publish thẳng map → base_link, các node cần dữ liệu mượt tần số cao (như controller bám path) sẽ nhận dữ liệu "giật" mỗi khi AMCL cập nhật thưa thớt — đúng vấn đề "robot giật mỗi khi AMCL hiệu chỉnh lại" đã nhắc ở bài Hệ toạ độ. Tách 2 tầng cho phép mỗi loại dữ liệu (mượt tần số cao vs chính xác dài hạn) tồn tại độc lập trong đúng đoạn TF của nó, không xung đột nhau.

Tóm lại: Tầng local đảm bảo dữ liệu mượt, tần số cao cho các node cần phản hồi nhanh (như controller_server); tầng global đảm bảo vị trí tuyệt đối không trôi vô hạn, cho các node cần độ chính xác dài hạn (như planner_server).

Kiến thức kỹ thuật

Công thức

Tần số cập nhật của TF odom→base_link (từ EKF local) so với map→odom (từ EKF global, giới hạn bởi tần số AMCL) — độ trễ hiệu dụng cho node cần vị trí tuyệt đối bằng tổng độ trễ cả 2 đoạn:

Độ_trễ_vị_trí_trong_map = Độ_trễ_odom→base_link + Độ_trễ_map→odom

VD: EKF local chạy 30Hz (độ trễ ~33ms), AMCL cập nhật 2Hz (độ trễ ~500ms)
Độ_trễ_vị_trí_trong_map ≈ 33ms + 500ms ≈ 533ms

Nhưng vị trí trong odom frame (chỉ cần EKF local) có độ trễ chỉ ~33ms
→ đây là lý do controller bám path nên dùng odom frame, không phải map frame trực tiếp

Thông số

Tầng EKF	Nguồn dữ liệu	Transform publish	Đặc điểm
Local	Odometry + IMU	`odom → base_link`	Mượt, tần số cao, trôi dần theo thời gian
Global	Odometry + IMU + AMCL pose	`map → odom`	Cập nhật thưa hơn, hiệu chỉnh sai số trôi định kỳ

Ứng dụng thực tế

Kiến trúc 2 tầng EKF này đúng khớp với 2 nhu cầu khác nhau trong toàn bộ pipeline Nav2: controller_server cần dữ liệu vị trí tương đối mượt tần số cao để bám path chính xác (dùng đoạn odom→base_link), trong khi planner_server cần vị trí tuyệt đối chính xác trong bản đồ để tính đường đi đúng tới đích (dùng đoạn đầy đủ map→odom→base_link). Tách 2 tầng cho phép mỗi node trong Nav2 lấy đúng loại dữ liệu nó cần mà không cần biết chi tiết cách EKF phía sau vận hành.

Ví dụ thực tế

# Debug: kiểm tra TF publish đúng đoạn
ros2 run tf2_ros tf2_echo map odom       # nên thấy cập nhật thưa, từ ekf_global
ros2 run tf2_ros tf2_echo odom base_link  # nên thấy cập nhật liên tục, từ ekf_local

Kết quả mong đợi:
tf2_echo odom base_link -> cập nhật ~30 lần/giây (khớp frequency: 30.0 của ekf_local)
tf2_echo map odom -> cập nhật thưa hơn nhiều, phụ thuộc tần số AMCL (~2-5Hz)

Nếu map odom không cập nhật gì cả:
-> kiểm tra ekf_global có publish_tf: true chưa
-> kiểm tra pose0: /amcl_pose có đang nhận dữ liệu từ AMCL không
-> kiểm tra world_frame: map đã đặt đúng instance chưa (nhầm lẫn phổ biến
   nhất là gán world_frame: map cho ekf_local, phá vỡ toàn bộ cấu trúc 2 tầng)

Nếu một trong hai lệnh trên không trả về dữ liệu, kiểm tra lại publish_tf: true và world_frame đã cấu hình đúng cho từng instance EKF chưa — nhầm lẫn cấu hình giữa hai instance là lỗi phổ biến nhất khi mới triển khai pattern 2 tầng này.

Lỗi thường gặp

Gán nhầm world_frame giữa 2 instance EKF — như ví dụ trên, phá vỡ toàn bộ cấu trúc TF 2 tầng, khiến hệ thống publish transform sai đoạn.
Dùng chung 1 EKF cho cả map và odom — như đã nêu ở phần Nguyên lý hoạt động, gây "giật" dữ liệu cho các node cần vị trí mượt tần số cao mỗi khi AMCL cập nhật thưa thớt.
Không kiểm tra publish_tf: true ở đúng cả 2 instance — một trong hai đoạn TF không được publish, gây lỗi "could not transform" ở các node phía sau phụ thuộc đoạn đó.
Nhầm lẫn node nào nên dùng odom frame và node nào nên dùng map frame — dùng sai frame khiến controller nhận dữ liệu trễ hơn cần thiết, hoặc planner nhận dữ liệu không đủ chính xác dài hạn.

Kết luận

Kiến trúc 2 tầng EKF (local cho odom→base_link, global cho map→odom) là cách Nav2 giải quyết đồng thời hai nhu cầu đối lập: dữ liệu mượt tần số cao cho điều khiển tức thời, và vị trí tuyệt đối chính xác dài hạn cho lập kế hoạch. Đây là ứng dụng thực tế trực tiếp của nguyên lý phân tầng đã gặp xuyên suốt kiến trúc AMR. Bài tiếp theo trong chuyên mục đi vào RF2O — một nguồn odometry khác hẳn encoder, hữu ích chính xác khi bánh xe trượt.