室内导航系统 08：定位不是启动 AMCL，而是管理重定位与地图生命周期
建完图、启动 AMCL、在 RViz 点一个初始位姿，只能证明定位链路能运行。生产机器人会冷启动、被人搬动、进入电梯、遇到家具改动、跨楼层，并在运行中经历短时传感器退化。
定位系统的目标不是始终输出一个 pose，而是输出可信 pose，并在不可信时明确进入恢复流程。
两种位姿解决两种问题
局部连续位姿
odom → base_link 服务控制，要求：
- 高频；
- 连续；
- 短时准确；
- 不因回环突然跳变。
来源可以是轮速+IMU、VIO 或 LIO。
全局位姿
map → odom 服务地图一致性，允许缓慢修正或在重定位时变化。来源可以是 AMCL、scan matching、视觉地点识别、图优化或人工锚点。
把两者混在一起，回环会让控制器看到机器人瞬移；只保留局部里程计，长期又会漂出地图。
冷启动有四种模式
- 固定站点启动：充电桩、停靠位提供强先验，最可靠。
- 粗略人工初始化：操作员在地图点选区域和方向。
- 全局粒子搜索：AMCL 在全图采样，面积大时收敛慢且可能多解。
- 地点识别 + 几何验证：视觉或扫描先给候选，再用局部匹配确认。
产品应优先创造强先验，而不是把所有问题交给全局搜索。充电桩、AprilTag、反光柱、门口锚点都可以降低冷启动风险。
被搬动后的恢复
“kidnapped robot” 问题要求系统先发现自己错了。可使用：
- 观测与地图残差持续增大；
- 粒子分布多峰或有效粒子骤减；
- 轮速显示静止但 IMU/视觉检测到大运动；
- 定位与已知锚点冲突；
- 控制轨迹长期无法解释实际运动。
检测后不要继续以原速度导航。进入重定位状态：
停止任务运动
→ 扩大候选区域
→ 原地或小范围主动观测
→ 多假设匹配
→ 达到置信阈值
→ 低速恢复并二次验证
如果仍无法恢复，请求人工并保留现场数据。
地图变化分三类处理
短时动态
人、推车、临时箱子进入局部 costmap，快速衰减，不修改静态地图。
可运营变化
门状态、临时封路、限速和施工区进入语义/路线层，可由运营平台设置有效期。
永久结构变化
墙、固定货架、通道和站点变化需要地图版本升级。先在新版本建图和验证，再灰度切换；旧版本保留回滚。
自动 lifelong mapping 可以提出变化候选，但永久写入前要防止把长期停放的移动物体当成建筑结构。
多楼层不应做成一张巨大平面图
推荐：
building frame
├─ floor_01/map
├─ floor_02/map
└─ elevator/stair transition graph
每层拥有自己的几何地图和定位状态；电梯任务由拓扑层管理。进入电梯前锁定入口锚点，轿厢内使用里程计和专用感知，出梯后在目标楼层锚点重定位。
如果把重叠楼层投影到同一 2D 图，定位与路径规划会产生无法解释的交叉。
长期定位需要地图健康度
为每个区域维护：
- 最近成功定位时间；
- 匹配残差分布；
- 重定位耗时；
- 动态占比；
- 失败和人工接管次数；
- 哪些机器人/传感器版本验证过；
- 变化候选。
当某个区域残差长期上升，触发巡检建图，而不是等大量机器人同时失败。
位置不确定性要进入行为
定位模块应输出协方差和健康状态。行为策略可按不确定性：
- 降低速度；
- 增大障碍膨胀；
- 禁止进入窄门和电梯；
- 主动旋转获取更多观测；
- 导航到几何特征更丰富的恢复点；
- 请求人工。
“有 pose”不代表“可以安全运动”。
验收用例
- 20 个随机冷启动点；
- 5 个重复走廊和对称空间；
- 被搬动 1—3 米并改变方向；
- 传感器遮挡 3—10 秒；
- 临时移动 20% 家具；
- 旧地图启动与新版地图切换；
- 电梯或多楼层转换；
- 长时间运行后的内存、漂移和重定位。
定位系统真正成熟的标志，是它知道什么时候自己不知道，并有受控方法恢复。