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

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.

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óm | Tham số | Tăng lên thì… |
|---|---|---|
| Số hạt | min/max_particles | Bền hơn khi mơ hồ, tốn CPU hơn |
| Mô hình chuyển động | alpha1–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 lidar | sigma_hit, z_hit, z_rand | z_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ật | update_min_d, update_min_a | Cập nhật thưa hơn, nhẹ CPU, phản ứng chậm hơn |
| Số tia dùng | max_beams | Chính xác hơn, chậm hơn (60 là đủ cho lidar 2D) |
4. Quy trình hiệu chỉnh
- 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ệ.
- 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).
- Phát lại với AMCL, trong RViz hiển thị
/particle_cloudvà LaserScan: tia lidar phải trùng khít tường trên bản đồ. - 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. - Kiểm tra định lượng: đặt xe tại các điểm mốc đã đo, so
/amcl_posevới tọa độ thật (mục tiêu ±5 cm, ±2°).
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ặcscan_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ảmz_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_localizationrải hạt toàn bản đồ, rồi cho xe đi vài mét để hội tụ.