文献阅读之室内机器人通过窄区域的局部路径规划方法

大家好，欢迎大家关注我的知乎专栏- 慢慢悠悠小马车。

---

原文《Automatic Waypoint Generation to Improve Robot Navigation Through Narrow Spaces》。

一、摘要与简介

本文以家庭\室内服务机器人为研究对象，为增强其通过窄区域的能力，提出了一种在线的局部路径调整方法，主要分为2步：

1. 沿全局路径方向自动探测过窄难以通过的区域
2. 然后根据环境特征生成辅助点，作为引导机器人通过的路径点。

方法的优势在于计算快速、通用性强，可以大幅提高机器人通过窄通道、窄门、拐弯等复杂困难区域。

窄区域难以通过，主要原因是传感器的观测噪声、标定误差、定位误差、轨迹跟踪误差等。已有的方法很多建立在对门、走廊通道等特征的主动识别上，然后制定合适的策略通过，泛化能力较差，无法在复杂多样的家居环境中通用。

图1

如图1，本方法是建立在全局路径规划成功的基础上，当局部路径规划失败时，开启窄区域检测和辅助点生成，调整局部路径的部分路径点，一般不会也不必更改全局路径。若有必要，当通过窄区域后，可进行全局重规划。

本文中有2个重要概念：

• critical navigation point，CNP，关键导航路径点，可以简单的认为1个窄区域对应1个CNP。
• auxiliary navigation point，ANPs，辅助导航路径点，1个CNP会衍生出2个ANP。

初始生成的1个CNP和2个ANP是沿着全局路径、靠近窄区域中心通道、共线的关系。之后再根据环境特征、上下文等微调位置。机器人跟踪这3个点，就可以无碰平滑的通过窄区域。

二、核心算法

3.1 窄区域检测，即CNP的生成

算法1

如算法1和图2，算法输入是

• 机器人位姿
• 代价地图
• 预设的窗口大小

算法输出1个CNP。

算法步骤：

1. 根据机器人位姿和窗口大小，在安全膨胀后的代价地图中限定考察的窗口区域；
2. 提取窗口中的边，即障碍物占据区域和可通行区域的交界线；
3. 双层for循环遍历这些边，寻找具有最近欧式距离的1对点，这对点的中点就是CNP。

因为考察窗口足够小，该算法会非常快，且没有必要做严格的计算过程优化。

图2

 3.2 ANPs的生成

算法2

如算法2和图3，算法输入是

• 算法1输出的CNP
• 代价地图
• 半径增长步长
• 角度增长步长
• 角度阈值（共线程度）
• 预设的ANP位置微调窗口大小

算法输出和CNP匹配的2个ANP。

算法步骤：

1. 外层while循环，以CNP为圆心，不断增长检查的半径，直到找到2个ANP。
2. 内层while循环，不断增长检查的朝向角，获得圆周上的1个端点。如果该端点未被障碍物占据，就跳过；如果被占据，就添加到待考察的点集中。
3. 若待考察的点集容量是2，就检查这2个端点是否与CNP近似共线且位于两侧（即落在直径上）。若不共线，将第2个端点从点集中删除，继续2；若共线，计算ANP，即跳到4。如图3(a-c)。
4. 计算2个端点连线的垂线，从ANP沿垂线的2个方向延伸一定距离，获得初始的ANP点。如图3(d)。在以初始ANP点为中心的小窗口中，根据环境特征计算代价值，获取具有最小代价的微调后的ANP点，作为算法的输出结果。如图3(e-f)。

图3

三、实验和展望

本文采用了4幅室内地图、4种移动机器人，进行了仿真测试和实际跑机测试。采用导航成功率（navigation success rate，NSR）来评价算法带来的性能提升。对比证明了该方法可以大幅提高室内拥挤狭窄场景的导航成功率。

本文提出了2点展望：

•  在地图中保存ANPs以便长期利用这些数据，不必一直更新计算。我觉得这取决于时效和环境地图的变化程度。如果限定机器人仍处在观测窗口中不更新ANPs的话，应该可以在计算资源、刷新频率、路径平滑程度等方面取得平衡。如果时间间隔久，室内环境很容易发生变化，如开关门、桌椅腿挪动等，历史数据是不可靠的。
• 相邻ANPs的合并。我认为这是很有必要做的。因为相邻的ANPs更容易导致机器人无意义的在小区域里往复折返，降低运动效率。如图7中红框所示。

图7

四、问题与优化

• 本文通过窄区域的导航点计算思路，和《Path Planning for Autonomous Vehicles in Unknown Semi-structured Environments》中固定Hybrid A*节点作为Anchors、优化平滑其他的路径点，有神似之处。具体可参见 文献阅读之自主泊车APA的路径规划 - 知乎。可惜本文没有介绍微调时的代价函数。
• 图2给的示例比较理想，2条边恰好处于相对的位置，2条边的夹缝也基本和机器人的行进方向一致。我认为算法1只考虑了最短距离去搜索最狭窄的可通过处，会有不适应的场景。如图8所示。如果也考虑机器人的朝向、下一步路径的行进方向，会更合理一些。

图8

• 本文没有提及连续帧的CNP、ANP如何更新。机器人是每个局部规划周期计算，还是每个窗口覆盖范围计算一次？如果要通过连续的窄通道，又要如何处理多个临近的ANP的关系？这个问题和展望2是关联的。我认为有2种优化方式，1是基于全局地图和全局路径，动态调整窗口大小，也不一定非要正方形窗口。一次更新可以计算出多个CNP及对应的ANPs。2是对相邻的ANPs做平滑和合并。
• 算法2在确定了第1个端点后，就不会删除它了。导致如果后面一直搜索不到与第1点匹配共线的第2个端点的话，当前半径轮次就无法搜索到结果了。其实可以将符合条件的点全部暂存，在一个半径值搜索完成后，逐对检查是否有1组符合要求的ANPs。计算量会大幅增加，但更合理，计算过程可以优化。
• 本文没有提及过窄区域前后的全局路径是否需要重规划、局部路径是否平滑、是否要机器人停下原地旋转。理论上，全局规划的路径必然穿过窄区域，机器人进出窄区域的局部路径是会贴近全局路径的，没必要重规划，也不会有大的朝向变化，应该可以不停下平滑通过。
• 对于非圆形底盘的机器人，在窄区域旋转是很容易发生碰撞的。可能在进出窄区域时需要特殊处理，提前调整好朝向，即限定ANP的位置尽量远离窄区域。如图9所示。

图9