引言
移动机器人一旦离开建图阶段，就要在已知地图上持续定位。这件事看似比 SLAM 简单——地图已经给了，只需回答「我在哪」——但工程上它反而是长期运行中最容易出问题的一环。定位一旦漂移或跳变，规划、控制、任务状态机全部连锁失效，而现场表现往往只是「机器人行为诡异」，很难直接定位到定位模块。
工程上的第一个难点是里程计的不可靠。轮式里程计是所有定位算法的运动先验，它的误差来自轮径标定、轮距标定、打滑、地面材质变化。未标定的里程计误差可达 10%，标定后能压到 1%~2%，这个差距直接决定粒子滤波的收敛速度与稳定性。
第二个难点是「已知地图」这个假设本身会失效。货架移位、新增隔断、门开合都会让静态地图与观测不符。定位算法必须在「地图部分过期」的情况下仍能工作，这要求观测模型对不一致有容忍度，也要求有地图更新的机制。
第三个难点是恢复能力。机器人被抬起搬走、上电时不知道自己在哪、长期失跟后突然恢复——这些场景需要全局重定位，而全局重定位的计算量与误匹配风险都远高于跟踪定位。本文聚焦「已知地图上的定位」，建图本身（SLAM）见 SLAM 与定位建图 ，状态估计的数学基础见传感器融合与状态估计 。
目录
- 已知地图定位的问题定义
- 里程计模型与运动学
- 里程计标定与漂移
- AMCL 粒子滤波原理
- 重采样：低方差与自适应
- 观测模型：likelihood_field 与 beam
- 全局重定位与绑架问题恢复
- 多传感器融合定位
- 地图服务与地图更新
- 多楼层与电梯场景
- 定位质量评估与监控
- 调参与现场排障
1. 已知地图定位的问题定义
定位要估计的是机器人在已知地图中的位姿，其数学形式是一个贝叶斯滤波问题。
贝叶斯滤波递推（定位的核心）：
预测：bel(x_t) = ∫ p(x_t | x_{t-1}, u_t) · bel(x_{t-1}) dx_{t-1}
—— 用运动模型 u_t 把上一时刻的信念推演到当前
更新：bel(x_t) ∝ p(z_t | x_t) · bel(x_t)
—— 用观测 z_t 对预测加权修正
两种实现：
参数化：高斯假设，用卡尔曼滤波（EKF/UKF）——快、单峰、易被多峰骗
非参数化：粒子集，用蒙特卡洛定位（MCL/AMCL）——慢、多峰、鲁棒
已知地图 vs SLAM 的区别：
已知地图：地图 m 是常量，只需估计 x_t
SLAM：同时估计 x_{1:t} 与 m，问题维度大得多
为什么移动机器人普遍用粒子滤波而不是卡尔曼滤波？因为室内环境常有「对称走廊」「相似房间」这类观测歧义，位姿信念是多峰的，高斯假设会强行把它压成单峰，导致定位跳到错误的位置。粒子滤波天然支持多峰表示，是这一场景的正确工具。
2. 里程计模型与运动学
粒子滤波的「预测」步依赖运动模型，对差速轮机器人而言就是里程计运动学。
差速驱动里程计模型（两轮差速）：
给定左右轮位移 Δs_L, Δs_R：
Δs = (Δs_L + Δs_R) / 2 # 中心位移
Δθ = (Δs_R - Δs_L) / L # 转角，L 为轮距（track width）
Δx = Δs · cos(θ + Δθ/2)
Δy = Δs · sin(θ + Δθ/2)
θ += Δθ
关键参数：
L（轮距） ：两驱动轮中心的距离，标定的核心
r（轮半径） ：编码器计数到位移的换算系数
减速比 ：电机编码器到轮子的传动比
// 里程计更新：把编码器增量积分成位姿
void integrateOdom(double d_left, double d_right, double dt) {
const double ds = 0.5 * (d_left + d_right);
const double dtheta = (d_right - d_left) / track_width_;
const double dtheta_half = 0.5 * dtheta;
// 二阶近似，比直接用当前朝向积分更准
x_ += ds * std::cos(theta_ + dtheta_half);
y_ += ds * std::sin(theta_ + dtheta_half);
theta_ += dtheta;
theta_ = std::atan2(std::sin(theta_), std::cos(theta_)); // 归一化到 [-π, π]
}
一个必须避免的错误：用当前朝向积分位移（x += ds·cos(θ) 而不是 cos(θ + Δθ/2)）。当旋转较大时（如原地快速转向），前者会累积显著误差。二阶近似（取中值角度）几乎不增加计算量，却能明显降低转弯时的漂移。
3. 里程计标定与漂移
标定的目标是求出「编码器读数」到「真实位移」的准确映射，核心是轮半径与轮距两个量。
标定方法一：直线标定（定轮半径）
让机器人直线前进 10 m，测量实际距离 d_real
若里程计报告 d_odom，则轮半径修正系数 = d_real / d_odom
重复 3~5 次取平均，消除打滑带来的随机性
标定方法二：原地旋转标定（定轮距）
让机器人原地旋转 10 圈（20π rad），测量实际转角
轮距修正系数 = θ_real / θ_odom
轮距误差是转角漂移的主因，这一项最容易被忽略
标定方法三：UMBmark（系统误差与随机误差分离）
走正方形路径回到起点，测量终点与起点的偏差
顺时针与逆时针各走一次，可分离「系统性误差」（可标定）
与「非系统性误差」（打滑、地面不均，不可标定）
漂移的误差来源与量级：
轮径/轮距标定误差 ：系统性，标定后可降到 < 1%
打滑 ：非系统，湿滑地面可达 5%~20%
地面材质变化 ：地毯 vs 硬地，轮径有效值不同
转弯时轮子侧滑 ：差速轮转弯必然有侧滑，转角误差主要来源
编码器分辨率与噪声 ：小，通常可忽略
标定后的残差主要是非系统性误差，只能靠外部观测（激光、视觉）修正。这就是粒子滤波存在的意义：里程计提供运动先验，激光提供绝对修正，两者互补。如果里程计漂移超过 5%，任何定位算法都会吃力，所以标定是第一步，不能跳过。
4. AMCL 粒子滤波原理
AMCL（Adaptive Monte Carlo Localization）是 ROS 2 的默认定位实现，是 MCL 的自适应版本。
AMCL 的一次迭代：
1. 采样（预测）
对每个粒子，按运动模型 + 里程计增量 + 噪声采样新位姿
噪声来自 alpha1~alpha4 参数（旋转噪声、平移噪声等）
2. 权重更新
对每个粒子，用激光观测与地图计算似然 p(z | x)
地图上的障碍与观测到的障碍越吻合，权重越高
3. 归一化权重
4. 重采样（自适应）
按权重复制高权重粒子、淘汰低权重粒子
粒子数根据 KLD（Kullback-Leibler 距离）自适应增减
5. 发布位姿估计
通常是权重最高的粒子簇的加权均值
amcl:
ros__parameters:
min_particles: 500 # 最小粒子数
max_particles: 2000 # 最大粒子数
alpha1: 0.2 # 旋转噪声：旋转引起
alpha2: 0.2 # 旋转噪声：平移引起
alpha3: 0.2 # 平移噪声：平移引起
alpha4: 0.2 # 平移噪声：旋转引起
alpha5: 0.2 # 仅 omni 模型使用
laser_model_type: "likelihood_field"
update_min_d: 0.25 # 移动 0.25m 才更新，省算力
update_min_a: 0.2 # 转 0.2rad 才更新
resample_interval: 1 # 每次更新都重采样
recovery_alpha_slow: 0.001 # 慢速恢复平均权重
recovery_alpha_fast: 0.1 # 快速恢复平均权重
alpha1~alpha4 是运动模型噪声，它们的取值反映「你对里程计有多信任」。值太小（0.05）会让粒子过度集中在里程计预测处，观测修正能力弱；值太大（0.5）会让粒子云过度发散，定位抖动。默认值 0.2 是保守的起点，标定良好的里程计可以适当调小以提高精度。
5. 重采样：低方差与自适应
重采样是粒子滤波的关键步骤，实现方式直接影响定位质量与计算量。
重采样的目的：
避免粒子退化——权重逐渐集中到少数粒子上，其余粒子失效
做法：按权重复制高权重粒子、淘汰低权重粒子
朴素重采样的问题：
用轮盘赌独立抽取 N 次，计算量 O(N)，且随机性大
低方差重采样（Low-Variance / Systematic）：
用单一随机数 + 等间隔的累积权重比较，一次遍历完成
计算量 O(N)，方差更小，AMCL 的默认实现
自适应粒子数（KLD 采样）：
根据粒子分布的空间离散度动态调整粒子数
分布集中（跟踪良好）→ 粒子少（如 500）
分布发散（全局定位）→ 粒子多（如 2000+）
好处：跟踪时省算力，恢复时保精度
KLD 采样的参数：
kld_err ：近似误差上界，典型 0.01~0.05
kld_z ：置信度，典型 0.99（对应 z 值约 2.33）
min/max_particles：粒子数的上下限
意义：kld_err 越小、kld_z 越高，所需粒子越多，定位越精确但越慢
resample_interval 控制重采样频率。设为 1（每次更新都重采样）会让粒子多样性快速下降，长期运行可能导致「过早收敛到错误位置」；设为 2~3 能保留更多多样性，但收敛慢。在观测歧义大的环境（长走廊）建议设为 2，牺牲一点收敛速度换取抗误收敛能力。
6. 观测模型：likelihood_field 与 beam
观测模型决定「一个粒子与激光观测有多吻合」，是定位精度的直接来源。
beam 模型（原始模型）：
对每条激光束，建模四种情况：
命中障碍、意外障碍、未命中（最大距离）、随机测量
参数：z_hit, z_short, z_max, z_rand
优点：物理意义清晰，能处理动态障碍
缺点：慢（每条光束算多次），参数难调
likelihood_field 模型（推荐）：
预先计算地图上每个格子到最近障碍的距离（距离场）
观测似然 = exp(-d² / (2σ²))，d 为观测点到最近障碍的距离
优点：快（查表）、平滑（对地图误差容忍度高）、参数少（只有 σ）
缺点：对动态障碍的处理不如 beam 模型精细
两者对比：likelihood_field 在大多数场景下又快又稳，是默认选择
amcl:
ros__parameters:
laser_model_type: "likelihood_field"
laser_likelihood_max_dist: 2.0 # 距离场截断距离（米）
laser_max_range: 12.0 # 超过此距离的读数丢弃
laser_min_range: 0.1 # 低于此距离的读数丢弃
sigma_hit: 0.2 # likelihood_field 的 σ
laser_max_range 的设置很关键。设得比雷达实际量程小，能丢弃「打在远处无回波」的读数（这些读数噪声大）；设得比实际量程大，会引入不可靠的远距离观测。室内雷达通常量程 1230 m，但有效可信距离往往只有 812 m，因此 laser_max_range 常设为 8~12。sigma_hit 反映「地图精度」：地图越准，σ 可以越小。
7. 全局重定位与绑架问题恢复
上电时不知道位置、被搬走、长期失跟，都需要全局重定位能力。
三种恢复场景与策略：
1. 上电初始化（初始位姿未知）
用 /initialpose 给出大致位置，或用全局定位搜索
AMCL 支持「无初始位姿」启动，粒子均匀撒在自由空间
2. 绑架问题（Kidnapped Robot）
机器人被抬起搬到别处，但自身不知道
现象：定位持续错误，粒子云聚集在错误位置
对策：recovery_alpha_slow/fast 注入随机粒子
3. 长期失跟后恢复
传感器被遮挡、动态障碍挡住视线
现象：粒子云发散，定位漂移
对策：观测恢复后自动收敛，或触发重定位
AMCL 的恢复机制（Augmented MCL）：
维护两个平均权重：
w_slow：长期平均（alpha_slow，如 0.001，时间常数大）
w_fast：短期平均（alpha_fast，如 0.1，时间常数小）
当 w_fast < w_slow（短期观测变差）时，注入随机粒子：
注入概率 = max(0, 1 - w_fast / w_slow)
这样系统能「察觉自己可能错了」并尝试其他假设
绑架问题恢复的时间是几秒到几十秒，取决于粒子数与注入速率。要求快速恢复的场景不能只靠 AMCL：应该用分支定界全局搜索（如 Cartographer 的纯定位模式）或视觉重定位做初值。AMCL 的恢复是「渐进式纠错」，不是「瞬时重定位」。
# 手动触发重定位：给一个初始位姿
ros2 topic pub --once /initialpose geometry_msgs/msg/PoseWithCovarianceStamped \
'{header: {frame_id: "map"}, pose: {pose: {position: {x: 5.0, y: 3.0},
orientation: {w: 1.0}}}}'
8. 多传感器融合定位
单一激光定位在退化场景（长走廊、空旷大厅）下不可靠，多传感器融合能显著提升鲁棒性。
融合的层次：
1. 观测层融合（紧耦合）
把 IMU、轮式里程计、视觉、激光的观测统一进一个滤波器
代表：robot_localization 的 EKF/UKF 融合轮式 + IMU
优点：精度高；缺点：标定与调参复杂
2. 位姿层融合（松耦合）
各定位源独立输出位姿，再融合
AMCL 输出 map→odom，robot_localization 融合 odom→base_link
优点：模块化、易替换；缺点：无法利用底层信息
3. 地图匹配 + 里程计（AMCL 本身）
里程计给运动先验，激光给绝对修正
# robot_localization：融合轮式里程计与 IMU，输出平滑的 odom→base_link
ekf_filter_node:
ros__parameters:
frequency: 30.0 # 融合频率
two_d_mode: true # 平面运动，忽略 z/roll/pitch
odom0: /wheel_odom
odom0_config: [true, true, false, # x, y, z
false, false, true, # roll, pitch, yaw
true, true, false, # vx, vy, vz
false, false, true, # vroll, vpitch, vyaw
false, false, false] # 加速度
imu0: /imu/data
imu0_config: [false, false, false,
false, false, true, # 只用 IMU 的 yaw
false, false, false,
false, false, true, # 与 yaw 角速度
false, false, false]
imu0_differential: false
imu0_remove_gravitational_acceleration: true
odom0_config 的 two_d_mode 是最重要的开关。移动机器人在平面上运动，把 z、roll、pitch 全部禁用能避免 IMU 的俯仰漂移污染定位。IMU 的 yaw 与轮式里程计的 yaw 融合是核心：IMU 短期准（无累积漂移的角速度）、轮式长期稳（绝对转角），互补后转角估计显著改善。不要同时用 IMU 的 yaw 和轮式里程计的 yaw 而不设权重，两者会打架，应通过协方差或 differential 开关协调。
9. 地图服务与地图更新
定位依赖地图，地图的质量与新鲜度直接决定定位质量。
地图的两个来源：
1. map_server 加载静态地图（PGM + YAML）
优点：稳定、可版本管理
缺点：环境变化后过期
2. SLAM 在线提供地图（如 slam_toolbox 的定位模式）
优点：能适应缓慢变化
缺点：需要持续的建图计算，且可能与定位循环依赖
地图元数据（YAML）的关键字段：
image: factory.pgm
resolution: 0.05 # 米/像素，必须与建图时一致
origin: [-10.0, -10.0, 0.0] # 地图左下角在 map 坐标系的位置
negate: 0 # 0 = 白底黑障，1 = 黑底白障
occupied_thresh: 0.65
free_thresh: 0.196
地图更新的三种策略：
1. 定期重建：停机重新建图，替换文件，最稳妥
2. 局部修补：人工在旧地图上擦除/添加区域（如删掉已移走的货架）
3. 在线更新：SLAM 定位模式边定位边更新（slam_toolbox localization mode）
风险：误把动态障碍写进地图，形成幽灵障碍
地图版本必须纳入管理并与机器人绑定。现场最常见的问题之一是「用了错版本的地图」，表现为定位持续偏差但算法「看起来没问题」。做法是给地图文件加时间戳与校验和，启动时打印并记录。
10. 多楼层与电梯场景
多楼层场景是定位的特殊难点，因为地图不连续、电梯是「瞬移」。
多楼层定位的三种做法：
1. 分层地图 + 楼层切换
每层一张地图，机器人知道自己所在楼层
跨层时切换 map_server 的地图与 AMCL 的初始位姿
关键：楼层信息必须由任务层显式提供，定位本身无法从单层激光判断楼层
2. 全局 3D 地图
用一张 3D 地图涵盖所有楼层，定位在 3D 中做
优点：无需楼层切换；缺点：内存与算力大，2D 雷达在多层重叠区歧义
3. 楼层标记 + 电梯内重定位
进入电梯后定位失效（空间小、无特征）
出电梯后需快速重定位到新楼层
对策：出电梯时给出初始位姿先验（电梯口位置）
电梯场景的工程要点：进电梯前记录位姿，电梯运行时冻结定位（不更新），到达后根据「电梯停在几楼」的先验恢复定位。在电梯里强行定位会因空间狭小、观测退化而产生跳变，正确做法是主动暂停定位更新，而不是让算法硬撑。
11. 定位质量评估与监控
定位必须可观测，否则「定位坏了」这件事只能在事后从行为异常倒推。
可监控的定位指标：
1. 粒子云离散度：协方差或粒子分布的标准差
发散说明定位不确定，应收敛却发散是告警信号
2. 观测似然：平均权重或似然值
持续偏低说明地图与观测不符（地图过期或定位错误）
3. 重定位触发频率：recovery 机制的激活次数
频繁触发说明环境与地图偏差大或里程计标定差
4. map→odom 变换的跳变率
单次跳变超过阈值（如 0.5 m）应告警，可能是定位跳变
5. 定位与里程计的偏差：map→odom 的累积变化
缓慢增长正常，突增异常
# 监控 map→odom 变换的变化
ros2 run tf2_ros tf2_echo map odom
# 观察 particle_cloud 话题，粒子云发散即定位不确定
ros2 topic echo /particle_cloud --once | head
# 记录位姿用于事后分析
ros2 bag record /amcl_pose /tf /scan /odom -o loc_debug
定位与运动规划与轨迹优化
的耦合在于代价地图的位姿来源：定位跳变会让机器人「以为自己偏了」，从而触发无意义的重规划。这也是为什么导航系统要区分 map→odom（全局修正）与 odom→base_link（局部平滑）——定位跳变只影响前者，不打断局部控制。这一接口在 Nav2 导航栈一篇中有详细展开。
12. 调参与现场排障
调参应遵循固定顺序，避免盲目试错。
调参顺序（从底层到上层）：
1. 先标定里程计（轮径、轮距），用直线与原地旋转验证
2. 再校地图（分辨率、原点、与实际环境对齐）
3. 再调运动模型噪声 alpha1~alpha4（信任里程计就调小）
4. 再调观测模型（laser_max_range、sigma_hit）
5. 最后调粒子数与恢复参数（min/max_particles、recovery_alpha_*）
现场排障的典型症状与根因：
定位持续偏差 → 地图版本错 / 地图与实测不对齐 / 外参错
定位抖动 → alpha 噪声过大 / 粒子数过少 / sigma_hit 过小
定位跳变 → 观测歧义（长走廊）/ 动态障碍 / 恢复机制误触发
上电无法定位 → 无初始位姿 / 粒子数不足 / 环境特征不足
恢复很慢 → 恢复 alpha 参数不当 / 需要全局重定位
转弯后误差累积 → 轮距标定不准 / 里程计二阶近似未用
一个实用的验证方法：录一段包含直线、转弯、回环的数据，离线重放并观察定位轨迹。用 evo 对比定位轨迹与真实轨迹（若有人工测量的地面真值），能定量评估标定与调参效果。定量指标比肉眼观察可靠得多。
权衡取舍
| 决策 | 选 A | 选 B |
|---|---|---|
| 滤波器 | EKF：快、单峰、易被歧义骗 | 粒子滤波：慢、多峰、鲁棒 |
| 观测模型 | beam：物理清晰、慢 | likelihood_field：快、平滑、推荐 |
| 粒子数 | 少（500）：省算力、易失跟 | 多（2000+）：鲁棒、算力高 |
| 重采样间隔 | 1：收敛快、多样性低 | 2~3：多样性高、收敛慢 |
| 融合层次 | 观测层：精度高、复杂 | 位姿层：模块化、易替换 |
| 地图来源 | 静态文件：稳定可版本管理 | SLAM 在线：适应变化、有风险 |
| 多楼层 | 分层地图：简单、需楼层信息 | 3D 地图：统一、算力大 |
核心原则：定位问题先怀疑标定与地图，再怀疑算法参数。里程计标定不准、地图版本错误、地图与实际不对齐，这三类问题占了「定位不准」投诉的绝大多数，而它们都不是调 alpha 参数能解决的。
常见坑清单
- 里程计未标定，转角漂移 5% 以上，任何定位算法都吃力——先做直线与原地旋转标定。
- 里程计积分用当前朝向而非中值朝向，转弯时累积误差——用
θ + Δθ/2的二阶近似。 - 地图版本与现场环境不符，定位持续偏差——地图纳入版本管理并绑定机器人。
- 用了错的地图元数据（origin/resolution），地图与实际错位——核对分辨率与原点。
alpha噪声设得过大，粒子云发散导致定位抖动——信任里程计时调小噪声。- 粒子数过少，长走廊等歧义场景下过早收敛到错误位置——提高粒子数或增大重采样间隔。
- 在电梯内强行定位，空间狭小观测退化导致跳变——进电梯前冻结定位更新。
- IMU 的 yaw 与轮式 yaw 直接相加未协调，两者打架——用协方差或 differential 开关协调。
- 没有定位质量监控，定位坏了只能从行为异常倒推——监控粒子离散度与 map→odom 跳变。
- 要求 AMCL 秒级从绑架问题恢复——AMCL 是渐进纠错，快速恢复需全局搜索或视觉重定位。
小结
已知地图定位的核心是「里程计给先验、激光给修正、粒子滤波融合两者」。AMCL 的调参旋钮分三层：运动模型噪声（信任里程计的程度）、观测模型（地图精度与雷达可信距离）、粒子与恢复参数（精度与鲁棒性的权衡）。这三层的调节顺序不能颠倒，否则会掩盖底层问题。
下一步建议阅读传感器融合与状态估计 ，理解 EKF/ESKF 与 IMU 预积分的数学细节，这是多传感器融合的基础；SLAM 与定位建图一篇讲清地图从哪来，是本文的前置；Nav2 导航栈一篇展示了定位结果如何被导航系统消费。
最后一句经验：定位不准时，先跑一遍标定与地图核对，再动参数。九成的问题在标定与地图，不在滤波器。
继续阅读
探索更多技术文章
浏览归档，发现更多关于系统设计、工具链和工程实践的内容。