融合切线段的hRRT 狭窄空间路径规划算法

周飞龙，甘 屹

（上海理工大学机械工程学院，上海 200093）

0 引言

相对于其他算法，RRT 算法因其在多约束、高维路径规划问题中具有概率完备性和渐近最优性［13］而越来越受欢迎。针对RRT 算法收敛速度较慢的问题，Urmson 等［14］更改节点采样方式，提出具有节点偏向目标概率的hRRT（Heuristically RRT）算法；Wang［15］从多随机树方面提出双向随机树；刘成菊等［16］提出基于RRT 的变步长搜索以提高搜索效率。

路径规划算法的目的是找到可达路径，然而一些基于RRT 的算法在狭窄复杂环境中并不能求解出路径。这是由于在狭窄空间中选取节点是困难的，狭窄通道体积小，任何基于容量的抽样分布都可能失败［17］。Rodriguez 等［18］从多向随机树角度提出了在OB-RRT（Obstacle Based RRT）中通过狭窄通道区域建立根节点，让随机树更快地寻找到可行路径，但需要预先确定狭窄空间位置；Informed-RRT*与RABIT*通过局部优化器，提高路径中的狭窄区域采样概率，但是其效率有待提高［19-20］；文献［13］提供一种桥梁检测算法提高对狭窄空间的采样概率，但会在有凹三角形轮廓障碍物附近产生多余采样节点，增加了算法时间。

本文提出一种融合切线段的hRRT 狭窄空间路径规划算法（An hRRT Narrow Space Path Planning Algorithm Fused with Tangent Segment，T-hRRT）。先使用hRRT 算法搜索到障碍物附近，再利用切线段寻找狭窄空间入口并构建障碍物轮廓的可通过狭窄空间，由于最短路径的解一定是由切线及其轮廓组成［8］，因此，T-hRRT 算法在狭窄空间环境中可以得到相对较短的路径，并且算法效率大幅提高。

1 狭窄空间定义

全局通行度是在地图上的所有可通行空间内，除去机器人或车辆所占空间外，自由空间面积的最小值与该点自由空间总面积的百分比［21］。在二维的管道与洞穴环境中，将自由空间面积转化为水平截面宽度化简计算，如式（1）所示。

式（1）中，p为全局通行度，wmin为全局地图可通行的自由空间的最小水平截面宽度，wrobot为移动机器人宽度。当p>0 时，机器人能从狭窄通道通过，当p≤0 时，该路段不可通行。本文将通行度为50% 以下的空间定义为狭窄空间，50%以上的称为普通空间。本文主要以狭窄空间为基础对hRRT 路径规划算法进行研究。

2 hRRT 算法原理

hRRT 算法优先向目标方向扩展低代价路径，同时保持对探索的合理偏向。hRRT 算法以起始点qstɑrt作为随机树RRTree 的根节点。按照特定概率函数F 从自由空间Sfree中选择随机点qrɑnd或者目标点qgoɑl作为采样点qsɑmple。在随机树RRTree 中寻找距离采样点qsɑmple最近的节点qneɑr，以节点qneɑr为原点向采样点qsɑmple方向延长步长ε获得新节点qnew。若新节点qnew路径未经过障碍物空间Sobs，则将新节点qnew加入随机树RRTree 中，若经过障碍物空间Sobs，则搜索失败，重新选取采样点qsɑmple。当树中的节点扩散到目标点qgoɑl时，完成整个搜索，返回起始点qstɑrt到目标点qgoɑl的路径节点，hRRT 算法原理如图1 所示。

我们来梳理一下这三个步骤，第一步通过写作建立人物形象，这是“以写促读”；第二步对照原文修改写作片段，这是“以读促写”；第三步学习联想，用联想的方法再次修改写作片段，这又是“以读促写”。在不断修改的过程中，加深学生对“全神贯注”一词及文章主旨的认识。

hRRT 算法采用启发式成本低的目标偏置节点进行扩展，缩短计算时间，但是当环境中存在狭窄通道时，其性能会出现大幅度下降。其原因主要是由于hRRT 算法采用基于概率函数的随机采样策略，相对于整个环境空间而言，窄道所占面积比例很小，导致窄道内采样概率很低，随机树难以在狭窄通道中扩展，从而狭窄通道两端无法连通，最终导致路径无解。

Fig.1 Schematic diagram of hRRT algorithm principle图1 hRRT 算法原理示意图

3 融合切线段的hRRT 路径规划算法

切线算法基于障碍物边缘间的切线构成最短无碰撞路径原理，应用Dijkstra 算法遍历所有障碍物切点，在二维地图中能得到最短路径，并且其环境适应性好，在狭窄通道环境中也能搜索到最短路径。缺点是时间复杂度与空间复杂度较高，随着地图中障碍物切点数目的增多，算法搜索效率逐渐下降。

本文采用hRRT 算法与切线算法相结合的方法，利用hRRT 算法避免切线算法的全局搜索，利用切线算法通过狭窄通道，加快路径搜索速度，两者相辅相成。

3.1 地图栅格化

任意地图环境都可以被量化成具有一定分辨率的栅格，而栅格大小直接影响信息存储量的大小和建模精度两个问题［21］。由于原始地图信息过大，切线算法时间和空间复杂度较高，为了提高效率、降低资源占用，本文使用栅格法进行环境建模，简化地图。

对于狭窄空间，较大的障碍栅格尺寸会使通路变得更窄，甚至使通路封闭而影响结果，而较小的栅格尺寸会增加算法计算量。因此设计合理的栅格大小很重要，本文定义栅格化尺度为移动机器人体积的一半，保证地图狭窄通道的原有信息，又减少了计算量，如式（2）所示。

式（3）中，Gxy为栅格取值，1 为障碍栅格，0 为自由栅格；glimit为栅格划分阈值，当附近的障碍面积大于此值，代表此栅格为障碍物点。式（4）中，g(x,y)为原始地图(x,y)处的值，障碍空间数值为1，自由空间数值为0；∑gɑround为栅格点附近原始地图中的取值之和，代表了附近栅格的障碍面积。

3.2 障碍物凸点检测

地图中任意障碍物经过栅格化处理可以形成多边形模型，自由空间Sfree内任意一点与障碍物多边形模型的切点一定在其凸点［8］。如图2 所示，黑色区域为障碍物栅格化后的多边形模型，红色点为障碍物多边形模型的凸点（彩图扫OSID 码可见）。

Fig.2 Rasterized maps图2 栅格化地图

因为路径规划时将移动机器人设为质点，但机器人体积不可忽略，故需要在障碍物周围设置安全距离dsɑfe。本文设置的安全距离为障碍物向外一个栅格的距离，即dsɑfe=，如图2 所示的灰色区域内为移动机器人碰撞区域，在移动机器人运动过程中需要避开该区域。

针对安全距离问题，如图2 所示，将灰色包围区域作为最终障碍物模型，使用蓝色凸点作为本文规划算法中所使用的凸点，从而保证算法能够规划出一条无碰撞路径。搜索地图环境中所有凸点，将凸点位置信息放入凸点矩阵bumps{[xi yi]}。凸点检测的伪代码如下：

3.3 切点搜索

在搜索切点时，本文通过有限长度的切线段搜索切点。连接自由空间任意一点xfree与所有凸点，并且两端延长e=wrobot，如果生成的线段中没有经过障碍物空间，则该生成线段的凸点为xfree对障碍物模型的切点。如图3 所示，A点为xfree对障碍物模型的一个切点，用来寻找狭窄空间入口处的节点。

根据上述方法，还可以规划障碍物模型轮廓路径，如图3 所示，以障碍物模型上的凸点A 为原点搜索切点，点B满足要求。A、B 两点连接构成障碍物模型的一段轮廓路径，确保随机树在狭窄空间中有效扩展，切点搜索伪代码如下：

对于有公切线的障碍物，切线段会多次往复搜索，陷入局部收敛。本文将重复的切点只在RRTree 中记录一次，减少随机树节点数，并且使得算法快速跳出局部收敛。

Fig.3 Tangent section of point to obstacle图3 点对障碍物切线段

3.4 算法实现

算法实现过程如下：

Step1：输入起始点qstɑrt、目标点qgoɑl、地图环境的信息。

Step2：栅格化地图，并查找地图中障碍物凸点bumps。

Step3：设置概率函数，本文采用简单概率函数F，利用函数在0 到1 区间随机取值，当大于0.5 时，进入随机采样，当小于等于0.5 时，偏向目标采样。

Step4：寻找随机树RRTree 中距离采样点qsɑmple最近的点qneɑr，按照采样点qsɑmple方向定步长ε扩展生成新节点qnew，判断该路径间是否有障碍，若存在障碍，则进入Step5，若无障碍，将新节点qnew加入到随机树中RRTree。

Step5：以当前距离采样点qsɑmple最近的点qneɑr做障碍物切线段，将切点加入随机树RRTree 中，删除重复切点。

Step6：当随机树中的节点与目标点qgoɑl没有障碍时，路径搜索完成。

4 实验分析

为了验证T-hRRT 路径规划算法性能，本文在3 种地图环境中对hRRT 算法、切线算法以及T-hRRT 算法进行仿真，地图中黑色几何图形代表障碍物，空白区域为自由空间，蓝色细短线表示分支，黑色粗线表示最终生成的路径。算法运行环境为：64bit Windows10 操作系统；MATLAB 2019a；处理Intel（R）Core（TM）i5-9300H；主频2.4GHz；内存8GB。

4.1 普通空间环境

地图一为普通空间环境，大小为500*500 分辨率，全局通行度大于50%，起始点与目标点位置分布为（10，10）与（490，490）。该地图用于验证3 种算法在起点与终点之间包含多障碍物时的搜索能力，如图4（a）、4（b）、4（c）分别为切线算法、hRRT 算法、T-hRRT 算法在地图一环境下的一次实验结果，其中hRRT 算法的步长为30。

Fig.4 Results of map 1 experiment图4 地图一中实验结果

在地图一普通空间环境中对3 种算法经过20 次试验，其规划时间、路径长度与节点数如表1 所示。

Table 1 Simulation results of three algorithms in ordinary space environment表1 普通空间环境下3 种算法仿真结果

由表1 可知，在地图一环境中T-hRRT 算法获得相对于hRRT 算法的较短路径，平均节点数减少了87%，搜索时间减少了90%；相对于切线算法减少了99%的平均规划时间。结果表明，T-hRRT 算法在普通空间环境中表现良好。

4.2 狭窄空间环境

地图二与地图三为狭窄空间环境，大小为500*500 分辨率，全局通行度小于50%，其中地图二只为直线型狭窄空间环境，地图三为不规则轮廓型狭窄空间环境。起始点与目标点位置分布为（10，10）与（490，490）。用于验证3 种算法在起点与终点之间包含不同狭窄空间时的搜索能力，图5（a）、5（b）、5（c）、6（a）、6（b）、6（c）为两种狭窄空间环境中切线算法、hRRT 算法、T-hRRT 算法的一次实验结果。

在地图二直线型狭窄空间环境与地图三不规则轮廓型狭窄空间环境中对3 种算法经过20 次试验，其规划时间、路径长度与节点数如表2 和表3 所示。

Fig.5 Results of map 2 experiment图5 地图二中实验结果

Fig.6 Results of map 3 experiment图6 地图三中实验结果

Table 2 Simulation results of three algorithms under linear narrow space environment表2 直线型狭窄空间环境下3 种算法仿真结果

表2 与表3 中的值为无穷，表示该算法在此环境中失效，而对于切线算法无节点数，步长为30 的hRRT 算法在地图二与地图三狭窄空间环境中失效，显然狭窄环境影响了随机树的有效扩展，达到节点最大搜索次数时无解。切线算法与T-hRRT 算法都能通过狭窄通道，但是T-hRRT 算法在地图二直线型狭窄空间环境与地图三不规则轮廓型狭窄空间环境中平均规划时间相比于切线算法减少了89%，而平均路径长度不超过切线算法规划最短路径长度的5%。

Table 3 Simulation results of three algorithms in narrow space with irregular contour表3 不规则轮廓型狭窄空间环境下3 种算法仿真结果

5 结语

针对狭窄空间下hRRT 算法路径无解问题，本文结合切线算法与hRRT 算法，提出T-hRRT 算法，利用切线算法加快hRRT 算法节点扩展与通过狭窄空间，利用hRRT 算法随机扩展节点避免切线算法的全局计算，相互弥补缺点从而获得规划时间较少并且长度较短的路径。T-hRRT 算法完成了hRRT 算法不能解决的狭窄通道问题，并且其路径长度与切线算法规划的最短路径长度相差不到5%，而相对于切线算法的规划时间减少了约89%。在后续研究中，将进一步提升算法在三维空间与未知、动态环境中的规划能力。