第260篇 重定位技术——机器人在已知地图中重新定位

机器人开机后发现自己在哪这件事听起来理所当然实际上是个不小的技术挑战。SLAM解决的是一边走一边定位的问题前提是机器人从某个已知起点出发。但如果机器人被搬到了一个新位置、或者系统崩溃重启、或者从休眠中恢复它就完全不知道自己在哪里了。这时候需要重定位——在已有地图中确定机器人的全局位姿。重定位和SLAM中的定位有本质区别。SLAM中的定位是相对定位知道上一步在哪估计这一步在哪误差累积但有连续性。重定位是全局定位对当前位置完全未知要在整张地图中搜索匹配是一个大海捞针的问题。基于特征匹配的重定位最直观的重定位方法是拿当前传感器的观测数据和地图中所有位置做匹配找到最像的那个位置。对于视觉SLAM重定位通常用词袋模型。ORB-SLAM2的做法是提取当前帧的ORB特征和数据库中所有关键帧的词袋向量比较找出最相似的几个候选帧。然后用特征匹配和PnPPerspective-n-Point计算当前帧相对于候选帧的位姿。如果几何验证通过重定位成功。# 视觉重定位流程 def visual_relocalization(frame, database): bow_vec compute_bow(frame.features) candidates database.query(bow_vec, top_k5) for candidate in candidates: matches match_features(frame, candidate) if len(matches) min_matches: pose solve_pnp(matches, candidate.landmarks) if reprojection_error(pose) threshold: return pose return None # 重定位失败对于激光SLAM重定位可以用NDT或ICP把当前扫描和地图做配准。问题是初始位姿完全未知直接用ICP/NDT可能收敛到错误解。解决办法是多初始值策略——从地图中采样多个初始位姿分别跑ICP/NDT取配准误差最小的那个。分支定界加速——Cartographer的做法Cartographer在重定位方面用了一个很巧妙的加速方法分支定界Branch and Bound。这个方法让全局搜索的速度快了几个数量级。思路是把搜索空间组织成一棵金字塔式的树。底层是高分辨率匹配顶层是低分辨率匹配。先在低分辨率下快速扫描整个搜索空间找到得分还不错的候选区域。然后在这些区域逐步细化到高分辨率匹配。低分辨率匹配的好处是速度快、对位姿误差容忍度高因为分辨率低偏差一点也能匹配上。缺点是精度差可能把很多不太好的位置也标记为候选。高分辨率匹配精度高但速度慢。两者结合保证不遗漏正确解又不用在每个位置都做高分辨率匹配。# 分支定界搜索的核心思路 def branch_and_bound_search(scan, map_pyramid): candidates [(coarse_pose, score) for coarse_pose in map_pyramid.top_level] candidates.sort(keylambda x: -x[1]) # 按分数降序 best_score -inf for pose, upper_bound in candidates: if upper_bound best_score: break # 剪枝 refined refine(pose, map_pyramid, levelhigh_res) if refined.score best_score: best_score refined.score best_pose refined.pose return best_poseMCL蒙特卡洛定位MCLMonte Carlo Localization是另一种经典的全局定位方法特别适合2D激光雷达场景。它用粒子滤波来估计机器人的位姿——在整张地图中撒很多粒子每个粒子代表一个候选位姿根据传感器观测更新每个粒子的权重权重高的粒子被保留权重低的被淘汰。MCL的优势是可以处理多模态分布。如果机器人在一个对称的走廊里可能有两个位置看起来很像MCL可以同时维护两个候选随着机器人移动两个候选的权重会分化最终收敛到正确位置。# MCL的核心步骤 def mcl_update(particles, scan, map_grid): for p in particles: p.weight likelihood(scan, p.pose, map_grid) # 重采样保留高权重粒子 particles resample(particles) # 加入随机扰动保持多样性 for p in particles: p.pose random_noise() return particlesMCL的局限是粒子数量。粒子太少覆盖不了整个搜索空间粒子太多计算量太大。对于大场景全局定位可能需要几千甚至上万个粒子。AMCLAdaptive MCL会根据定位的不确定度自适应调整粒子数量不确定时多撒粒子确定后减少粒子。粗定位到精定位的两阶段策略工程上常用的重定位方案是粗定位精定位两阶段。粗定位阶段用快速但不太精确的方法找到大致位置。比如用Scan Context描述子快速比较找到最相似的几个子地图或者用低分辨率NDT做全局搜索。粗定位给出一个大致正确的位姿误差在1-2米以内。精定位阶段用ICP或高分辨率NDT在粗定位结果的基础上精确对齐。因为初始位姿已经比较准了ICP/NDT可以正确收敛到精确解误差在厘米级别。# 两阶段重定位 def two_stage_relocalization(scan, map_manager): # 阶段一粗定位 coarse_poses coarse_search(scan, map_manager) best_coarse None for pose in coarse_poses: score evaluate_ndt(scan, pose) if score best_coarse.score: best_coarse pose # 阶段二精定位 fine_pose icp_refine(scan, best_coarse) return fine_pose面试追问环节面试官重定位和回环检测有什么区别回环检测是我好像来过这里是在连续运行过程中检测到的。系统有一个大致的位姿估计回环检测只是确认并给出精确的相对位姿。重定位是我在哪系统完全不知道自己的位置需要从头搜索。回环检测是确认重定位是搜索。不过两者的底层技术有很多重叠——都需要场景匹配和位姿估计。面试官ORB-SLAM2的重定位为什么这么可靠ORB-SLAM2的重定位做了几层保障。第一层是词袋模型快速筛选候选关键帧。第二层是特征匹配加几何验证PnPRANSAC排除误匹配。第三层是局部关键帧组验证不只靠一帧还要和候选帧附近的关键帧都能匹配上。第四层是在Essential Graph中传播确认重定位结果和全局地图一致。多层验证使得ORB-SLAM2的重定位很少出错。面试官AMCL在ROS中怎么用的AMCL是ROS navigation栈的标准定位模块。订阅/map获取地图订阅/scan获取激光扫描发布map到odom的tf变换。需要设置初始位姿、粒子数量通常500-2000、更新频率等参数。AMCL自动处理粒子退化和重采样使用体验很顺畅。面试官重定位失败怎么处理重定位失败意味着机器人不知道自己在哪里。处理方式有几种一是让机器人原地不动等待人工干预遥控到已知位置。二是让机器人做探索运动移动一段距离后重新尝试重定位新视角可能提供更多特征。三是切换到建图模式暂时不做定位新建一张小地图之后用这张小地图和全局地图做匹配。在实际部署中重定位失败是一个需要报警的严重事件。面试官kidnapped robot problem是什么绑架机器人问题是重定位的极端场景机器人运行中被搬到另一个位置系统完全不知道。里程计给出的位姿是错的重定位需要检测到异常并触发全局搜索。检测方法是对比里程计预测和传感器观测差异突然变大就可能是被绑架了。重定位是SLAM系统从连续运行到可靠运行的关键一步。开机初始化、系统恢复、异常处理都需要重定位能力。从词袋模型到分支定界再到粒子滤波不同方法各有适用场景。工程上最可靠的重定位方案是粗定位精定位的两阶段策略兼顾速度和精度。多层验证和多模态维护是提高重定位鲁棒性的关键。上一篇第259篇 大规模地图管理——子地图、分层地图和地图压缩下一篇我们聊多传感器融合SLAM看看视觉、激光和IMU怎么组合使用。如果这篇文章对你有帮助欢迎点赞支持一下你的鼓励是我持续更新的动力