基于射线模型的改进全局路径规划算法

蒋林，方东君，周和文，黄惠保

1 引言

路径规划算法［1］的目标是为移动机器人在障碍物地图中规划出一条可行的移动路径，该路径对移动机器人导航效果有着至关重要的作用. 目前主流的路径规划算法有Dijkstra［2］、A*［3］（A Star）、人工势场法［4］、遗传算法［5］、粒子群算法［6］、视觉图法［7］等算法.

其中，A*作为目前最为经典的路径搜索算法广泛应用于各种移动机器人导航场景中［8］，但由A*得到的路径存在大量冗余节点、拐点多、拐角大、折线多且离障碍物较近，这些缺陷严重降低了路径质量，且不利于移动机器人的控制，甚至会发生碰撞. 故此，在漫长的移动机器人路径规划算法发展史中，国内外学者提出了很多路径规划算法［9］，在以A*为基础的8 大变种中，以JPS［10］（Jump Point Search）搜索效果最优，用跳点搜索的方式实现了高效计算，但随着地图复杂度增加，递归层次加深会导致计算量呈指数增长. 张哲等［11］在A*的基础上引入大于1 的加权因子以增加算法搜索深度和改变单步扩展搜索方式来保证最优解，但合适的单步扩展形状又取决于机器人前面的障碍物环境，这会导致每次改变位姿都要重新求解单步扩展形状，无法适应复杂的环境. 王洪斌等［12］提出结合A*和人工势场算法对局部路径进行优化，提高路径的平滑性，但二次A*算法平滑处理和B 样条曲线优化［13］类似，在实际应用中对机器人的控制要求很高. 王中玉等［14］用不同的搜索矩阵处理不同的位置关系，提高了工作效率和平滑性，但该算法效果主要依赖搜索矩阵的效果，需要引入采样函数［15］进行辅助，且不适用于复杂或者差异较大的环境.

针对上述所提到的问题，本文提出一种基于射线模型的全局路径搜索算法. 该搜索算法以射线模型作为主要搜索方案，更快速、精确地朝目标点进行搜索，此外，还对每个路径节点根据射线模型进行逆向优化，以代价最小的方式实现路径的整体优化，实现了拥有更少折点、更小转角、路程更短、更安全的移动路径规划.

2 基于射线模型的路径搜索

该全局路径规划算法是基于已知环境障碍物信息的环境地图运行的，在此基础上，该算法以射线模型为核心，从起点开始不断向终点发射射线，以更高效的搜索方式实现更优质的路径搜索.

2.1 地图的预处理

由于栅格地图［16］拥有简单实用且容易实现的特点，本文采用栅格地图对目标环境进行建模. 栅格地图由多个代表地图点的小栅格组成，普通搜索算法按照此地图进行搜索时，容易忽视小栅格的尺寸，从而导致实际机器人本体在行驶到某栅格点时与附近障碍物栅格点发生碰撞. 图1 是常见栅格地图上的路径简图，蓝圆表示起点，绿圆表示终点，红色表示路径，黑色表示障碍物，从图1 可以看出，红色路径从栅格顶角处跨越了黑色障碍物，在实际环境中，由于移动机器人本体有一定尺寸，故在执行该段路径时必然会与障碍物发生碰撞. 此外，在狭窄的巷道区域会存在仅能通过机器人本体的环境，若笼统地对障碍物边缘进行扩展会导致原本可通过的路径不可通过.

图1 常见栅格路径示意图

针对上诉问题，本文算法采用的是结合机器人尺寸先对地图进行形态学处理，将部分极危险区域变成不可行区域，然后根据距离对临障区域进行代价估值.该方法不同于普遍的对整幅地图做腐蚀处理，将前面所述的障碍物邻近区域运行成本增加，使得此区域是“不建议走”而非“不能走”，从而限制在仅当其余路径成本过高的情况下才走这部分与障碍物相近但不会发生触碰的区域，这样既可以提高远离障碍物的栅格的搜索优先性，也可以确保机器人能在狭窄的巷道中搜索出可移动路径.

2.2 射线模型

图2表达了当障碍物存在时射线的状态，其中蓝圆表示发射点，绿点表示接收点，从图2可以看出，当无障碍物时，绿点可以直接接收到射线；当有障碍物阻挡时，射线会被障碍物挡住，得到碰撞点的位置.

图2 射线模型简图

这种方法可以在无障碍的两点之间找出最短的路径，只保留起点和终点. 本文提出的路径规划算法将该模型应用于各子路径搜索，最终快速搜索出简捷、平滑的完整路径.

模型应用：从当前点向目标点发出射线，若无障碍则直行，若有障碍则返回碰撞点并向周围扩散，直到到达下一个障碍物或者目标点.

图3简要表达了该搜索模型的应用，其中①—⑤表示在起点与目标点之间有障碍物的环境下搜索过程，⑥表示在起点与终点之间无障碍的环境下搜索过程.在有障碍时，经过路径①到达碰撞点，再以碰撞点为中心向周围未搜索过的方向扩散出②③④个路径，仅存在路径④可以离开障碍物，然后以路径⑤到达目标点，最终得到路径为①→④→⑤，在无障碍时，可以从起点直接到达目标点，最终路径为⑥.

图3 射线搜索示意图

2.3 路径搜索

射线模型为点到点之间的搜索提供了便利，但当射线遇到障碍后需要从周围的区域找到合适的避障点，此处需要根据符合条件的栅格节点的成本大小进行辨识，从而找出更合适的路线. 本文在A*的代价函数［17］基础上引入一个碰撞估值，具体成本计算方法如下：

其中，λ为地图中无障碍栅格与最近障碍物栅格之间的曼哈顿距离，τ是设定的估值函数覆盖范围，C为地图中栅格的碰撞估值，F为总成本，G为已有成本，H为启发成本，x、y为过程节点的坐标，xgoal、ygoal为目标点的坐标.

栅格地图中每个栅格的碰撞估值均在预处理中完成，当无障碍栅格离障碍栅格越近时，由式（1）计算出的估值越高，反之越小，当与障碍物的距离超过设定的覆盖距离τ后，估值为0，由此可为障碍物附近的区域设置梯度估值.

当路径搜索至障碍物附近的栅格时，若该栅格碰撞估值不为0，但小于255，则表明该区域可走但附近有障碍物，且值越高，离障碍物越近，此时非0估值的加入会提高该处的行走成本，仅当其他路径的成本均高于该处成本时，才走该区域. 这种方式可以按照需求对临近障碍物的区域进行梯度赋值，以提高行走代价的方式保留该区域的可行性和远离障碍物的优先性.

该成本计算方法充分考虑了远离障碍物和可穿越狭窄区域这两类情形，解决了常见路径算法在避障和通过狭窄区域之间只能满足一项的问题，使路径规划更灵活，能满足机器人在复杂环境下的路径搜索.

射线模型的应用需要遍历一条射线经过的所有栅格值，并从起点开始直到障碍物或者目标点，这才满足搜索的严谨性，在本文中使用的直线函数以及遍历步长计算如下：

其中，x、y为节点坐标，Δx、Δy分别为横纵坐标的差值，d为遍历步长. 步长d的选择对能否成功遍历该射线所涉及的每个栅格值有至关重要的作用，而且应进行归一化后投入函数计算. 当d为纵坐标差值时，应先计算纵坐标，然后根据新的纵坐标结合直线函数计算出新的坐标点，反之则先计算横坐标，在该处不能混淆步长的计算，否则会导致跨越障碍物、误判等问题. 该计算方式能提升路径搜索的判断精确性，确保路径的每个节点是安全可行的.

图4 简要展示了射线搜索策略从蓝色起点到绿色目标点的搜索过程，其归纳如下：

步骤1：从起点向终点发射一条射线，若存在障碍物则到达首个碰撞点，反之直接抵达目标点，如图4 中过程①；

步骤2：以当前碰撞点为中心并将其标记为路径节点，检索八邻域非障碍物且未确定为路径节点的子节点，采用曼哈顿距离计算各子节点的成本；

步骤3：选择邻域中成本最小的子节点作为新的路径节点并向终点进行延伸，若存在障碍物则重复步骤2、步骤3，如图4过程②；

步骤4：直到当前搜索节点与终点之间没有障碍物时，到达目标点，如图4中过程③.

在上述过程中，每个碰撞点会被标记，用于解决在某一障碍物处反复碰撞导致迷路的搜索问题；在计算上，除了对必要的路径节点进行父节点和成本等计算外，仅需要对当前点和目标点之间形成的射线所经过的栅格判断是否为障碍物，并启发式地由当前点开始向目标点检索，这可以大幅减少搜索过程中的计算损耗，从而提高路径搜索的速度.

图4 射线搜索过程图

3 逆向路径优化

常见的路径规划算法所求出的路径总会存在很多不必要的拐点，于图4 而言，若能直接从起点到达障碍物下方的拐点再到达终点会更有利于机器人的控制，也能节省不少运动成本，在这将提出一种逆向优化算法，旨在进一步减少移动路径中不必要的拐点，以更安全、平滑的路径和更少的转向控制节点来提高机器人移动效率.

图5展示了一条路径的逆向优化过程，其中①—③指蓝色起点和绿色当前点之间的中间节点，红色路径表示优化前（后）的路径，归纳如下：

步骤1：以当前点为中心，向起点发射射线，图中路径显示有障碍，如图5（a）的浅蓝虚线；

步骤2：以当前点为中心，向节点①发射射线，图中所示无障碍，如图5（a）的浅蓝实线；

步骤3：删除中间节点②、③，新的路径为：起点→①→当前点，至此完成该段路径的路径优化，如图5（b）中红色路径.

图5 逆向优化过程图

综合步骤1~3可将逆向优化总结为：以当前节点为中心，按顺序依次向当前节点之前的节点逐个发射射线检测是否可直行，直到检测到当前节点的前一节点，在这一过程中若发现存在可直行的直线则删除被检测到的可行节点与当前点之间的中间节点，新的路径为：起点到被检测出的可行节点的路径加上当前节点.

将该优化加入射线搜索策略中，可以在求解路径的同时将路径优化，保证整体最优性. 该逆向优化是在已求解出的路径基础上依靠射线函数和检测途经栅格是否为障碍物进行实现，若某段路径的节点数为n，最大检测数为m，其计算关系如式（8）所示.

m=n⋅(n+ 1) /2 （8）

由于该优化与路径搜索同时进行，单次优化需要检测的过程节点定然不会太多，故能避免增加额外的计算负担. 此外，该优化还为前文的路径搜索添加闭环检测，解决局部最优问题［18］. 经过这两者的融合，本文算法在路径搜索过程中不用再对地图上明显不是障碍物的节点进行成本计算以及对连接顺序的更替判断，取代的是仅读取该节点的地图值（0/1），无论是搜索效率还是路径效果上都有很大的提升.

图6 所示路径为图4 障碍环境经完整的本文算法搜索过后的路径效果，从该效果可以看出，本文所实现的全局路径规划算法不仅计算量小，而且得到的路径更简洁，没有多余的拐点，更利于机器人安全、高效地移动.

图6 路径搜索结果展示

综上所述，本文所提出的全局路径规划算法整体流程归纳如图7所示. 此外，A*由于始终以八邻域扩展方式从一个栅格节点中心向邻近栅格点的中心搜索，这类“井”字格局必然会限制A*得到的路径拐角一定是45°的倍数，这不仅局限了搜索的方向还降低了整体路径的灵活性，而本文算法则打破了这种45°转角限制，以点到点的方式实现任意转角，解决了在栅格地图中以栅格为基求解的算法在角度上对路径的局限问题，更全面地提升路径搜索的效率和路径质量. 下文将进行多组仿真实验和实际机器人运行检测该算法的搜索效果.

图7 路径搜索总流程图

4 实验与结果

为在多环境下验证本文算法的路径规划效果，该实验采用常规房间地图以及AIID 2010 STARCRAFT［19］竞赛中使用的三个STAR CRAFT 地图，如图8 所示，结合opencv3 与Vistudio2015 对地图进行预处理，并模拟出机器人在对应环境地图中用本文搜索算法得到的移动路径，最后提炼出时间、路程、拐点等多个参数进行数据分析比对.

4.1 本文路径规划效果

图8 原始实验地图

在进行预处理后，对同一地图划分出简单、一般以及复杂3 个等级的路径情况，分别在4 张地图中验证该搜索算法的路径效果、适应性以及稳定性. 其中，简单路况是指起点和终点间障碍物较少且路径较短的环境；一般路况指两点之间路径较长、障碍物较少的环境；复杂路况则是指路径长、障碍物多的环境. 本文算法在上述3 种环境下的路径搜索效果如表1所示.

表1 三种复杂程度下各地图中的路径效果

由表1可以看出，无论是路径复杂程度的变化还是环境的变化，本文算法都能有效地搜索出可行的移动路径，而且从红色路径效果上看，该路径全程仅保留了必要的拐点，其余均以直线的方式行驶，由起点十分简洁、高效地到达目标点.

故此，本文算法可以适应多种环境，适应性和稳定性强，且得出的路径简洁、安全，全程以最简便的方式直行，仅保留必要的转向节点.

为进一步分析本文算法的搜索效果，对其搜索出的路径进行参数提炼，如表2 所示. 其中，时间指路径搜索时间；路程指得出的路径距离；节点指得出的路径中需要停顿调整的控制点数；拐点指路径中需要拐弯的控制点数；角度是指机器人在走该路径过程中需要的最大转弯角度.

结合表1 和表2 可以看出，本文的全局路径规划算法搜索速度快，可以适应多种不同复杂度的地图和搜索环境，所得到的路径短且没有多余的路径控制节点，转弯节点少且全程保持着安全距离，每个节点的转向角均为锐角，便于移动机器人转向控制.

表2 三种复杂程度下各地图中的路径参数

4.2 与A*路径搜索算法比对

本文算法与A*在相同条件下搜索出的路径效果对比如表3 所示. 其中红色代表本文算法，蓝色代表A*算法，在进行多次试验后，取在相同条件下运行多次的参数平均值，并在表4 中对两种路径的参数进行对比.

表3 本文算法与A*路径效果对比

表4 本文算法与A*路径参数对比

结合表3和表4可以看出，A*得到的路径在任意环境下均有贴行障碍物的路段，实际机器人执行该段路径时极易发生碰撞，而且整体路径蜿蜒曲折，十分耗费机器人控制资源. 而本文算法得到的路径相比之下要更安全，除了在必要的拐点外，大部分区域是与障碍物相隔了一定距离，而且路径更平滑，减小了机器人的控制负担. 此外，在相同条件下本文算法的搜索速度比A*快约50%，路程减少约数十个像素点，全程需调整位姿的控制节点数极少，且本文路径突破了45°倍数角的约束，转向角比A*小.

由此可得，本文算法得到的路径比A*更为简洁、安全且易于控制. 而且在路程、控制节点数以及路径平滑性等方面比A*拥有明显的优势.

5 扫地机器人实验

5.1 机器人实验场地介绍及试验

在本实验中使用一台直径0.32 m、高0.09 m且配有摄像头和雷达等传感器的扫地机器人作为测试设备，测试环境为长50 m、宽40 m 的室内环境，设置有各类障碍物和巷道，足以考验路径规划算法的各方面性能，环境如图9（a）所示. 测试步骤如下：（1）将本算法程序烧录至扫地机器人，使其按照本算法进行路径规划；（2）实现测试环境的地图构建，图9（b）所示为该实验环境经过地图建模后得到的栅格环境地图，栅格大小为0.05 m×0.05 m；（3）在地图中指定起点和终点，让移动机器人搜索路径并执行；（4）整理实验数据进行分析.

图9 实验环境

基于上述环境，使机器人从当前点移动到目标点，并记录规划出的路径. 图10展示了6种路径，其中红点表示机器人当前所在位置，黄标表示目标点，黄色的虚线表示机器人规划出来的整体路径.

从图10 中可以看出，本文算法搜索出的路径始终与障碍物保持着安全的距离，拐点少，整体路径简洁.为了证明本文算法的灵活性，在图10（f）中添加了两处虚拟障碍墙进行测验，结果显示本文算法拥有较强的适应性，可以在多种复杂程度的环境中成功搜索适合机器人执行的移动路径.

图10 机器人实际运行展示

5.2 真实环境中与A*运行对比

以上述运行环境为基础，此处将提取局部路径搜索细节，更细致地展现本文算法与A*在实际运行中的效果差异. 如图11 所示，橙色点为起点，绿色点为终点，青色点为在路径中需要停顿、调整位姿或在搜索过程中占用计算较大的节点.

图11 本文算法与A*在搜索过程中的细节对比

由图11 可见，A*得到的路径中占用计算较大的点和需要机器人改变位姿的点明显比本文算法得到的路径结果多. 本文仅使用了两个过渡节点和三段直线即完成了从起点到终点的规划任务，而A*在图中的青色节点近乎遍布了整体路径的90%，故本文算法得到的路径相对更加简洁且执行时更为简便.

A*虽然考虑了路径最短以及方向两个问题［20］，但是其对每个节点进行八邻域搜索必然会进行许多不必要的计算，浪费计算资源. 此外，在障碍物边缘进行八邻域搜索必然导致得到的路径节点与障碍物较近，而本文算法主要以射线模型为基础进行搜索，没有局限于八领域搜索和相邻节点之间的运动，以检索射线所经过的栅格标记值代替直接对各栅格的成本等计算，在路径安全性、简洁性以及搜索效率等多方面相比于A*有明显的优势.

5.3 导航过程效果观察

图12 和图13 展示了图10（f）路径的机器人实际运行情况，从中可以看出，机器人在移动过程中没有触碰障碍物，即便在拐角处也保留了一定的安全距离，证明经本文算法求解出的移动路径实际可行且安全可靠.

图12 路线图

图13 实际机器人导航图

6 结论

本文针对传统路径规划算法得到的路径平滑性低且搜索速度慢等问题，通过射线搜索提高了搜索速度，结合碰撞估值使机器人既可优先远离障碍物行走也可通过狭窄的巷道区域，并用逆向优化策略对路径进一步优化，解决局部最优问题，增强路径的平滑性，提高了路径的整体质量和算法的搜索效率.