孙灵硕

（武汉理工大学 物流工程学院，武汉430000）

随着机器人技术的发展，焊接机器人逐渐替代了传统的手工焊接[1-3]，在港口大型机械装备的制造中发挥着越来越重要的作用[4]。其中，焊接机器人的路径规划问题作为研究的重点，受到了广泛关注[5-7]，其基本任务是快速找到一条从上一焊缝终点到下一焊缝起点的无碰撞路径。目前可用于避障路径规划的算法很多，比如人工势场法[8]、A* 算法[9-10]、Dijkstra 算法[11]等。其中，人工势场法算法简单、实时性好、安全性高，但易于陷入局部最优陷阱和出现路径振荡现象；A* 算法和Dijkstra 算法都是求解最优路径的经典算法，但是需要对障碍环境进行精确建模，在焊接机器人路径规划这样的高维空间中计算量会剧增。RRT-Connect[12]算法是RRT 系列算法中的一种改进型算法，通过随机采样的方式搜索路径，避免了对环境的精确建模，在高维空间的路径规划问题中具有良好的性能，适合解决焊接机器人的避障路径规划问题。

尽管RRT-Connect算法相较于RRT 算法有了显著的性能提升，但也存在一些不足之处。在焊接机器人采用RRT-Connect算法进行路径规划时，当工件存在较多凹形障碍区域时算法性能会出现明显下降，具体表现为：路径搜索次数增多、路径质量欠佳。本文针对RRT-Connect算法在焊接机器人路径规划中存在的问题，提出了改进的RRT-Connect算法，改进算法的主要创新点为：

（1）检测并标记出位于凹形障碍区域的树节点及其附近区域。

（2）根据标出的区域缩减自由状态空间，修剪随机树上处于凹形障碍区域的树节点。

通过上述做法，不断增强随机树规避凹形障碍区域的能力，避免随机树陷入凹形障碍区域，将随机树导向更有价值的区域。最后通过仿真实验验证了改进算法的优势。

1 问题分析

1.1 RRT-Connect算法

RRT-Connect算法通过不断扩展快速探索随机树T（V，E）来探索状态空间X，直到得到一条可行的路径。状态空间X 指的是路径规划的工作空间。有障碍物的状态空间记为Xobs，其余的记为自由状态空间Xfree，Xfree=X-Xobs。T（V，E）是一种树形数据结构，V 代表随机树上的树节点，E 表示树节点之间所形成的边。RRT-Connect算法的流程如下：

（1）首先算法在起点qinit和目标点qgoal各初始化一棵快速探索随机树，分别记为Ta和Tb。

（2）算法进入搜索过程，算法在自由状态空间Xfree中均匀随机地选取一个采样点，记为qrand。

（3）Ta执行Extend 过程，Extend 过程如图1所示。

图1 Extend 过程示意图Fig.1 Diagram of Extend process

Extend 过程首先遍历Ta上的树节点，找到距离qrand最近的树节点qnear，而后向qrand搜索一固定步长，产生新树节点qnew并进行碰撞检测。

（4）随后Tb执行Connect 过程，过程如图2所示。

图2 Connect 过程示意图Fig.2 Diagram of Connect process

Connect 过程本质上是迭代执行Extend 过程。首先遍历Tb上的树节点，找到距离qnew最近的树节点qnear，随后向着qnew尽可能多地前进多个步长，产生多个新的树节点，直到发生碰撞或与qnew发生连接。

（5）若最新的树节点Sn没有与qnew发生连接，则交换两棵随机树Ta和Tb，进入下一次搜索过程。

RRT-Connect算法引入了启发性的贪婪策略，显著提高了算法的收敛速度，但有时也会将随机树导入低价值区域，造成算法性能下降。

1.2 焊接机器人路径规划

焊接机器人在进行避障路径规划时，若待焊接工件上有较多凹形障碍区域，RRT-Connect算法的搜索次数增多、算法速度变慢、路径质量偏差。凹形障碍区域在几何上直观表现为一个由工件围成的“口袋型”的区域。工件及其形成的凹形障碍区域如图3所示。

图3 工件形成的凹形障碍区域Fig.3 Concave area formed by obstacle

由于在算法的路径搜索过程中，两棵随机树交替执行Connect 过程，则在开始的几次搜索过程中，两棵随机树都极易陷入凹形障碍区域中。陷入凹形障碍区域的随机树如图4所示，以随机树Tb上的最新树节点S 为例，图中灰色区域均为树节点S 的Voronoi图区域。而随机树Ta接下来通过Extend 过程产生的新的树节点一定在此区域内，因此当随机树Tb接着执行Connect 过程必然会选择树节点S 作为最近树节点。但是由于树节点S 距离障碍物太近，几乎不可能向着随机树Ta的方向产生新的树节点，只会白白的消耗搜索次数，算法性能此时出现退化。

图4 陷入凹形障碍区域的随机树Fig.4 Random trees trapped by concave area

2 改进的RRT-Connect算法

根据前文所述，本文提出了一种改进的RRTConnect 算法，在Extend 过程和Connect 过程中加入检测模块，用来检测每次路径搜索中产生的新树节点是否位于凹形障碍区域。若检测到该树节点位于凹形障碍区域，则将此树节点所在位置及其附近区域标记为凹形障碍区域Xcon，并将此凹形障碍区域从自由状态空间Xfree中删去，同时补充到障碍状态空间Xobs中去。最后将随机树上位于凹形障碍区域的树节点删去。接下来对凹形障碍区域的定义以及改进算法的主要工作进行详细介绍。

2.1 凹形障碍区域

在几何学中，一个几何图形可分为凸或凹的。如图5所示，图5（a）为凸多边形；图5（b）是凹多边形。凸几何图形是指内部为凸集的几何图形，凹体的定义也同理。下面分别给出凸几何图形和凹几何图形的定义：

定义1任意2 个属于图形上的点所连成的线段全部位于几何图形的内部或边界的图形称为凸几何图形。

定义2存在2 个属于图形上的点所连成的线段不完全位于几何图形的内部或边界的图形称为凹几何图形。

类似的，也可以推出凹体的定义。凸几何图形和凹几何图形的等价条件如图5（c）和图5（d）所示。基于上述定义可以进一步推导出凹形障碍区域的定义：

定义3所有2 个属于图形上的点所连成的线段中不位于几何图形的内部或边界部分的全体集合称为该图形所形成的凹形障碍区域。

凹形障碍区域的定义将为接下来的算法改进提供理论基础。

图5 凸多边形、凹多边形及其等价条件Fig.5 Convex polygons，concave polygons and their judgment

2.2 凹形障碍区域检测与标记

为避免随机树陷入凹形障碍区域，首先考虑将自由状态空间中的凹形障碍区域检测并标记出来。直接对整个自由状态空间Xfree检测凹形障碍区域这一做法计算量过大，而考察Xfree内的某一点是否位于凹形障碍区域则较为简单。所以改进的RRTConnect 算法通过检测路径搜索过程中产生的新树节点是否位于凹形障碍区域的方式来标记凹形障碍区域。以二维平面为例，根据前文所述凹形障碍区域的定义，那么可以推导出图形外某一点是否位于该图形所形成的凹形障碍区域的等价条件为：

定理1存在过此点所在位置的一条直线，以此点为分界点，若该直线在此分界点的两端均与图形存在交点，则说明此点位于凹形障碍区域。

凹形障碍区域检测的具体做法如图6所示，过新产生的树节点S2随机地做若干条直线，判断其中是否存在一条直线L1在此树节点两端均与障碍物存在交点。若存在上述直线L1，则说明此树节点所在位置位于凹形障碍区域。

图6 凹形障碍区域检测示意图Fig.6 Detection of concave area

若树节点被检测出位于凹形障碍区域，则标记此树节点所在位置及其附近区域为已识别的凹形障碍区域Xcon，如图7所示。

图7 凹形障碍区域标记示意图Fig.7 Mark of concave area

2.3 缩减自由状态空间与修剪随机树

在得出已识别的凹形障碍区域Xcon后，将此区域纳入到障碍状态空间Xobs中去，与之相对的自由状态空间Xfree则减去此区域。随机树在接下来的路径搜索过程中就会避免在此次标记出的凹形障碍区域内产生采样点以及新的树节点。在后续的搜索过程中，算法会不断积累检测和标记出凹形障碍区域，使得随机树规避凹形障碍区域的能力不断增强，最终达到规避凹形障碍区域的目的。

根据前文所述，改进的算法在Extend 过程和Connect 过程中加入了凹形区域识别模块。这里以执行完Connect 过程后的随机树为例，介绍随机树修剪的具体过程。由于Connect 过程在一次搜索过程中可以产生多个新的树节点，为了尽可能多的识别出凹形障碍区域，在随机树通过Connect 过程产生并添加了多个新的树节点之后，凹形障碍区域检测模块再对新的树节点进行检测。随机树修剪过程如图8所示，算法按照最新添加到随机树上的树节点最先检测的顺序，直到检测出某一新的树节点不位于凹形障碍区域或者遍历完所有新的树节点时结束，随后对随机树进行修剪，将随机树上位于凹形障碍区域的树节点删去。这使得算法在后续的路径搜索过程中可以不受陷入凹形障碍区域的树节点产生的负面影响，从而提高算法的收敛速度。

2.4 算法实现

根据上述两点算法改进内容，改进的RRT-Connect算法流程如图9所示。

图8 随机树修剪过程示意图Fig.8 Trimming process of random tree

图9 改进的RRT-Connect算法流程Fig.9 Improved RRT-Connect algorithm

改进的RRT-Connect算法首先在起点处qinit和目标点处qgoal各初始化一棵快速探索随机树Ta与Tb；然后进入搜索过程，在自由状态空间Xfree内选取采样点qrand，在2 棵随机树执行完各自的路径搜索过程并产生新的树节点后，检测新产生的树节点是否位于凹形障碍区域，将位于凹形障碍区域的树节点所在位置及其附近区域标记为已识别的凹形障碍区域Xcon；然后更新自由状态空间Xfree和障碍状态空间Xobs。其中自由状态空间Xfree需要减去已识别的凹形障碍区域Xcon部分，障碍状态空间Xobs则加上已识别的凹形障碍区域Xcon部分；最后修剪掉随机树上位于已识别的凹形障碍区域Xcon的树节点。若两棵随机树未发生连接，则交换2 棵随机树的身份进行下一次搜索，直到返回一条可行路径或者搜索次数耗尽时结束。

3 实验与分析

为了对改进的RRT-Connect算法进行性能测试，本仿真实验采用港口机械装备中常见的H 型钢截面作为待焊接工件进行避障。H 型钢如图10所示，由1 块腹板拼接在2 块翼板的翼缘中间组成，常用作港口起重运输机械中的承载结构。

图10 H 型钢工件Fig.10 The H-beam

本仿真实验利用图11所示的地图进行仿真实验，地图中的空白区域代表自由状态空间，黑色区域代表待焊接H 型钢工件的截面所形成的障碍物状态空间，图中灰色正方形和三角形所在位置分别代表路径规划的起点和目标点。

图11 仿真实验地图Fig.11 Environment map of simulation

在仿真实验中，分别以RRT 算法和RRT-Connect算法作为改进的RRT-Connect算法的比较对象，通过比较这3 种算法在搜索次数、路径长度以及路径平滑度等指标来证明改进的RRT-Connect算法性能的优越性。图12 为改进的RRT-Connect算法的仿真结果，如图12（a）所示，图中灰色的折线段代表改进的RRT-Connect算法在起点和目标点处扩展出的随机树，黑色的折线段代表规划出的路径；图12（b）中的灰色区域代表算法在路径搜索过程中标记出的凹形障碍区域，从仿真结果可以看出，生成的路径成功地避开了标记出的凹形障碍区域。

图12 改进的RRT-Connect算法仿真实验结果Fig.12 Simulation results of the improved RRT-Connect algorithm

RRT-Connect算法和RRT 算法的仿真结果分别如图13 和图14所示。图中黑色的折线段代表算法规划出的路径，灰色的折线段表示算法生成的随机树。由于算法搜索次数越多，随机树探索的空间越大，通过与图12（a）的对比可以看出，相比于其它两种算法，改进的RRT-Connect算法在搜索次数上具有明显的优势，而且所生成的路径长度更短。

图13 RRT-Connect算法仿真实验结果Fig.13 Simulation results of the RRT-Connect algorithm

图14 RRT 算法仿真实验结果Fig.14 Simulation results of RRT algorithm

在仿真实验中，分别对上述3 种算法进行100 次仿真实验，分别从算法搜索次数、路径长度以及路径平滑度3 个指标对这3 种算法进行分析对比。3 种算法在100 次重复实验中的搜索次数、路径长度与路径平滑度的平均值如表1所示，从数据可以看出，改进的RRT-Connect算法在搜索次数上具有突出的优势，在路径长度以及路径平滑度上也取得了一定的改进成果。相比于RRT-Connect算法，改进的算法在算法搜索次数上降低了47.30%，路径长度降低了9.96%，路径平滑度也有明显改善。其中搜索次数大幅降低的原因归功于改进的算法有效地避免了随机树陷入凹形障碍区域，将随机树的搜索方向导向价值更高的区域，加快了路径搜索的速度。此外，由于随机树成功地规避了凹形障碍区域，减少了路径的弯折，所以改进的算法生成的路径在长度上和路径平滑度上也具有一定的优势。

表1 仿真实验结果比较Tab.1 Comparison of simulation experiment result s

4 结语

本文针对RRT-Connect算法在凹形障碍区域内出现性能下降的问题，提出了一种改进的RRT-Connect算法。通过在路径搜索过程中不断检测和标记出凹形障碍区域，不断更新自由状态空间与障碍状态空间，并对随机树上陷入凹形障碍区域的树节点进行修剪，有效地避免了随机树在路径搜索过程中陷入凹形障碍区域所造成的算法性能下降的问题。最后通过仿真实验，验证了改进后的算法相对于RRT-Connect算法在搜索次数上具有突出优势，在路径长度及路径平滑度上取得了一定的改进成果。

本文所述改进的RRT-Connect算法目前主要用于只有单个待焊接工件的环境，下一步将提高算法的泛化能力，考虑针对多个工件所形成的复杂环境进行进一步的改进，以便更好地服务于工程实际问题。