Sau khi có bản đồ (bài #10), mỗi lần vận hành AGV cần biết mình đang ở đâu trên bản đồ đó. Trong Nav2, việc này do AMCL (Adaptive Monte Carlo Localization) đảm nhận: nó so dữ liệu lidar với bản đồ có sẵn để ước lượng vị trí, rồi phát TF map → odom.

Sau bài này bạn sẽ:
  • Hiểu bộ lọc hạt hoạt động thế nào và vì sao cần initial pose.
  • Biết ý nghĩa các tham số quan trọng của nav2_amcl.
  • Có quy trình hiệu chỉnh và kiểm tra chất lượng định vị.

1. Bộ lọc hạt

Hình 1. Chu trình bộ lọc hạt của AMCL: dự đoán theo odom, cập nhật trọng số theo lidar, lấy mẫu lại.
Hình 1. Chu trình bộ lọc hạt của AMCL: dự đoán theo odom, cập nhật trọng số theo lidar, lấy mẫu lại.

AMCL giữ hàng trăm tới vài nghìn "hạt" – mỗi hạt là một giả thuyết (x, y, θ) về vị trí xe. Khi xe đi, mọi hạt được dịch theo odom kèm nhiễu (vì odom không chính xác). Sau đó với từng hạt, AMCL hỏi: nếu xe ở đây thì lidar sẽ thấy gì, và có khớp với bản quét thật không? Hạt khớp tốt được trọng số cao và được nhân bản, hạt khớp kém bị loại. Chữ "Adaptive" nghĩa là số hạt tự giảm khi đã chắc chắn (thuật toán KLD), tiết kiệm CPU.

Hình 2. Đám hạt thu hẹp dần khi xe di chuyển và lidar thấy thêm đặc điểm của bản đồ.
Hình 2. Đám hạt thu hẹp dần khi xe di chuyển và lidar thấy thêm đặc điểm của bản đồ.

2. Đặt vị trí ban đầu

AMCL không tự biết xe đang ở đâu khi mới bật. Cách đơn giản là nút 2D Pose Estimate trong RViz. Trong vận hành thật, xe thường khởi động tại vị trí sạc cố định, nên phần mềm tự phát initial pose:

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseWithCovarianceStamped
import math


class InitPose(Node):
    def __init__(self, x, y, yaw):
        super().__init__('init_pose')
        pub = self.create_publisher(PoseWithCovarianceStamped, 'initialpose', 10)
        msg = PoseWithCovarianceStamped()
        msg.header.frame_id = 'map'
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.pose.pose.position.x = x
        msg.pose.pose.position.y = y
        msg.pose.pose.orientation.z = math.sin(yaw / 2)
        msg.pose.pose.orientation.w = math.cos(yaw / 2)
        cov = [0.0] * 36
        cov[0] = cov[7] = 0.25 ** 2        # sai số x, y ~ 0,25 m
        cov[35] = math.radians(10) ** 2    # sai số góc ~ 10°
        msg.pose.covariance = cov
        self.create_timer(1.0, lambda: pub.publish(msg))   # chờ AMCL kết nối rồi phát


def main():
    rclpy.init()
    rclpy.spin(InitPose(1.20, -0.50, 0.0))     # vị trí trạm sạc trên bản đồ

Với Nav2 có thể đặt sẵn trong tham số AMCL: set_initial_pose: true và initial_pose: {x: 1.2, y: -0.5, z: 0.0, yaw: 0.0}.

3. Các tham số quan trọng

amcl:
  ros__parameters:
    base_frame_id: base_footprint
    odom_frame_id: odom
    global_frame_id: map
    scan_topic: scan
    robot_model_type: nav2_amcl::DifferentialMotionModel   # OmniMotionModel cho mecanum
    min_particles: 500
    max_particles: 2000
    alpha1: 0.2     # nhiễu quay do quay
    alpha2: 0.2     # nhiễu quay do tịnh tiến
    alpha3: 0.2     # nhiễu tịnh tiến do tịnh tiến
    alpha4: 0.2     # nhiễu tịnh tiến do quay
    alpha5: 0.2     # chỉ dùng cho mô hình omni
    laser_model_type: likelihood_field
    laser_max_range: 20.0
    max_beams: 60
    z_hit: 0.5
    z_rand: 0.5
    sigma_hit: 0.2
    update_min_d: 0.25
    update_min_a: 0.2
    resample_interval: 1
    transform_tolerance: 1.0
    recovery_alpha_slow: 0.0
    recovery_alpha_fast: 0.0
NhómTham sốTăng lên thì…
Số hạtmin/max_particlesBền hơn khi mơ hồ, tốn CPU hơn
Mô hình chuyển độngalpha1–4Ít tin odom hơn: đám hạt tỏa rộng, chịu được trượt bánh nhưng vị trí "rung" hơn
Mô hình lidarsigma_hit, z_hit, z_randz_rand cao: chịu được nhiều vật lạ không có trong bản đồ (người, pallet)
Tần suất cập nhậtupdate_min_d, update_min_aCập nhật thưa hơn, nhẹ CPU, phản ứng chậm hơn
Số tia dùngmax_beamsChính xác hơn, chậm hơn (60 là đủ cho lidar 2D)

4. Quy trình hiệu chỉnh

  1. Odom tốt trước đã: hiệu chỉnh bán kính bánh, track width (bài #6). AMCL không cứu được odom tệ.
  2. Ghi rosbag một vòng chạy có các đoạn quay tại chỗ và hành lang dài (bài #8).
  3. Phát lại với AMCL, trong RViz hiển thị /particle_cloud và LaserScan: tia lidar phải trùng khít tường trên bản đồ.
  4. Nếu đám hạt tỏa quá rộng khi quay → giảm alpha1; nếu vị trí "kẹt" sai mà không tự sửa → tăng nhẹ alpha, tăng số hạt.
  5. Kiểm tra định lượng: đặt xe tại các điểm mốc đã đo, so /amcl_pose với tọa độ thật (mục tiêu ±5 cm, ±2°).
Hành lang đồng nhất là kẻ thù: dãy kệ dài giống nhau khiến lidar không phân biệt được vị trí dọc hành lang. Giải pháp: thêm đặc điểm (tấm phản quang, vách ngăn), dùng lidar tầm xa hơn, hoặc kết hợp marker (QR sàn, phản quang) cho những đoạn quan trọng.

5. Theo dõi chất lượng định vị khi chạy

Hiệp phương sai trong /amcl_pose cho biết AMCL đang tự tin đến đâu. Một node giám sát nên cảnh báo hoặc dừng xe khi độ lệch chuẩn vượt ngưỡng:

import math
from geometry_msgs.msg import PoseWithCovarianceStamped


def on_amcl_pose(self, msg: PoseWithCovarianceStamped):
    sx = math.sqrt(msg.pose.covariance[0])
    sy = math.sqrt(msg.pose.covariance[7])
    syaw = math.degrees(math.sqrt(msg.pose.covariance[35]))
    if max(sx, sy) > 0.15 or syaw > 5.0:
        self.get_logger().warn(f'Dinh vi kem: sx={sx:.2f} sy={sy:.2f} syaw={syaw:.1f} do')

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

  • Không có TF map → odom: chưa đặt initial pose, hoặc scan_topic/frame sai.
  • Vị trí nhảy khi xe đứng yên: bình thường AMCL không cập nhật khi đứng yên; nếu nhảy, kiểm tra có hai nguồn phát TF odom.
  • Định vị trôi trong kho đông người: tăng z_rand, giảm z_hit, đảm bảo bản đồ không chứa vật di động.
  • "Kidnapped robot" (bị khiêng đi chỗ khác): gọi service /reinitialize_global_localization rải hạt toàn bản đồ, rồi cho xe đi vài mét để hội tụ.

Tài liệu tham khảo