AMCL 收敛 粒子群高度聚拢 观测匹配稳定 不确定性降到阈值以下不是单一条件而是位置、角度、粒子分布、权重、持续稳定性5 个维度同时满足。下面给你可直接工程落地的收敛条件 实测距离 / 角度 / 数值标准。一、收敛的核心定义一句话粒子从全局散乱 → 集中在真实位姿附近的极小高斯团且连续多帧雷达匹配分数高、粒子数量自动下降、位姿输出稳定。二、5 大收敛判断条件带工程实测数据1. 粒子空间分布收敛最直观条件所有粒子集中在极小圆形 / 椭圆区域收敛后粒子X/Y 散布半径≤ 0.2m ~ 0.5m工程常用 0.3m粒子航向角yaw散布≤ 3° ~ 8°工程常用 5°未收敛粒子散在全地图 / 多个团 / 大范围弥散已收敛粒子像一团箭头紧紧抱在机器人周围2. 粒子权重分布收敛算法内部判断条件粒子平均权重持续高于阈值雷达匹配度高有效粒子数 ESSEffective Sample Size接近当前总粒子数ESS 1 / Σ(权重 ²)收敛时ESS ≥ 0.7 × 总粒子数权重最高的一批粒子集中在同一位姿附近3. 自适应粒子数自动下降KLD 收敛AMCL 会根据不确定性自动减少粒子全局搜索800~1500 个收敛稳定自动降到100~300 个甚至更少条件粒子数稳定在最小值附近 2~5 帧不再增加4. 位姿输出稳定可观测收敛连续多帧输出位姿变化极小位置漂移≤ ±0.03m ~ ±0.08m / 帧角度漂移≤ ±0.5° ~ ±1.5° / 帧连续3~10 帧稳定在小范围内无跳变5. 长期平均权重 短期平均权重防绑架 / 防发散AMCL 内部维护两个指数平滑权重ωslow长期平均权重慢更新ωfast短期平均权重快更新收敛条件ωslow ωfast说明当前匹配一直很好没有突然变差一旦ωfast 明显大于 ωslow→ 判定定位失效、发散、被绑架 → 重新全局撒粒子三、收敛完整判断标准工程可直接用同时满足以下全部才算 “已收敛、定位稳定”粒子聚集半径 0.3m角度方差 5°粒子数量稳定在min_particles100~300位姿变化连续 5 帧X/Y ±0.05myaw ±1°权重稳定ωslow ωfast且平均权重 0.6~0.7匹配度高无全局重搜触发连续 1~3 秒没有重新撒全局粒子四、收敛过程从全局到局部示例带时间 / 距离0~0.5s全局撒粒子1000 个→ 散乱全地图0.5~1.5s运动 观测粒子开始向高匹配区域聚拢粒子团半径5m → 2m → 0.5m1.5~2.5s半径 0.3m角度 5°粒子数自动降到200 个位姿误差 ±0.1m / ±2°2.5s 后完全收敛→ 进入局部小范围匹配模式五、发散 / 丢失定位的条件反向理解收敛任一触发即判定未收敛 / 定位丢失粒子团突然散开 1m连续 3~5 帧雷达匹配分数低权重 0.3~0.4ωfast 明显大于 ωslow突然匹配不上位姿跳变 0.5m 或 15°粒子数自动重新暴涨回最大值全局重搜六、一句话总结好记AMCL 收敛 粒子抱成团0.3m、角度不散5°、粒子变少、位姿稳、匹配分高、持续不炸团。
