基于改进RRT*算法的超冗余度机器人末端避障路径规划*

（赵 盼，杜兆才，刘 江，王明阳

（1.中国航空制造技术研究院，北京 100024；2.北京科技大学，北京 100083）

随着科学技术的不断发展，机器人技术在社会各个领域得到了广泛应用。超冗余度机器人具有长径比大、自由度高等特点，因此在空间狭窄、运动受限、复杂危险环境下具有超高的避障能力。在航空航天领域，航天器、飞机等设施内部结构复杂，可供作业的区域非常狭小，因此在进行监测、维护、清理、检测等任务时十分困难。超冗余度机器人的灵活性决定了其十分适合这类工作，如代替人工进行飞机油箱的检测[1]、狭小空间飞机部件的装配工作等。但由于超冗余度机器人关节数量较多，其逆运动学求解也变得十分困难。为了避免或简化求解过程，目前很多学者专家采用路径跟随运动的方式对超冗余度机器人进行运动规划。路径跟随运动一般指使整个超冗余度机器人机械臂部分能够跟随某个指定的路径，其特点是在运动受限的区域内，超冗余度机器人的连杆和关节沿路径移动，机械臂看起来始终跟随已有路径动[2]。要实现路径跟随运动，则首先要对机器人末端避障路径进行规划。

末端避障路径规划的主要任务是找到一条从起始点到目标点连续的无碰撞路径[3]。路径规划问题广泛存在于各个领域，如工业机器人、移动机器人、无人机等，吸引了国内外学者的广泛研究，大量的路径规划算法也因此被提出：基于建立引力斥力场的人工势场法[4]在局部避障中具有较好的性能，但在障碍物复杂的场景中容易陷入局部最优；基于计算智能的蚁群算法[5]具有较强的鲁棒性，但计算量大，收敛速度慢；基于搜索的A*算法[6]虽然搜索能力强，但不适合高维状态空间的路径规划。

基于随机采样的路径规划方法[7]的提出，为解决高维度、复杂环境的路径规划问题提供出了一种很好的解决思路。快速探索随机树（Rapidly exploring random tree，RRT）算法[8]搜索能力强，且不需要将搜索空间栅格化，十分适合应用在高维环境下。但RRT 算法也具有搜索效率低、路径曲折、产生路径不稳定等缺点。并且RRT 算法为单纯的随机采样，没有对所求得解的成本进行考虑，因此不能得到最优路径。为解决这个问题，Karaman 等[9]将渐进最优的思想引入标准RRT 算法中，提出了RRT*算法。RRT*算法在RRT 的基础上增加了父节点重新选择与剪枝重新布线的操作，利用这个过程来实现渐进最优。虽然RRT*引入了渐进最优思想，但其仍然具有很多缺点，如收敛速度慢、路径较为曲折等。因此国内外学者在RRT*算法的基础上，提出了很多改进措施。Jordan 等[10]根据RRT 的改进算法RRT–Connect 的双向搜索优势，提出了B–RRT*算法，利用RRT*中的父节点重选与重新布线操作来优化两棵树的搜索方式，同时引入了贪婪搜索策略，得到了最优解；张伟民等[11]通过在采样上引入目标偏向思想、新点扩展上给采样点和目标点分配权重等策略对RRT*算法进行改进，加快了收敛速度，提升了规划效率；曹凯等[12]将人工势场法与RRT*算法结合，提出了一种由人工势场（APF）引导RRT*进行路径规划的方法，利用人工势场引导采样节点进行采样，减少了算法运行时间，加快收敛速度。

上述方法对比标准RRT*算法而言取得了一定的优化效果，但由于RRT*算法的随机性，生成的路径仍然存在一定的曲折，路径转角大小随机性较大，因此生成的路径不一定每次都适合超冗余度机器人的路径跟随运动。本文对超冗余度机器人结构及相邻关节跟随运动过程进行分析，对障碍物进行了膨胀化处理，通过引入超冗余度机器人相关约束，对RRT*算法进行改进。利用改进后的RRT*算法进行超冗余度机器人的路径规划，能够在三维空间中规划出更加适合超冗余度机器人跟随运动的路径，且能保证一次成功率。

1 超冗余度机器人分析及障碍物处理

1.1 超冗余度机器人分析

超冗余度机器人结构如图1 所示，主要由进给底座、驱动底座、机械臂3 部分构成。机械臂部分主要由连杆构成，两个相邻连杆由万向关节进行连接，因此每个关节具有两个旋转自由度。

图1 超冗余度机器人结构Fig.1 Super redundant robot structure

关节转角的定义，如图2 中θ所示。关节的弯曲角度θ受臂段直径D和间距h0限制，满足tan （θ/2≤h0/D）。由于关节角度存在限制，因此在进行路径跟随运动时，末端路径的角度也应满足一定限制条件，即最终生成的路径，转角应尽量小于该角度。

图2 关节转角定义Fig.2 Joint angle definition

针对超冗余度机器人跟随末端路径运动，对两个相邻连杆的路径跟随运动进行分析。由于关节转角受到关节结构的限制，转角具有极限大小θmax，因此考虑路径转角为极限转角时两臂段的跟随运动。如图3 所示，在第1 节连杆头部通过路径拐点后，在前进过程中，第1 节连杆会扫掠过一个区域。这种情况下，若障碍物在这个扫掠区域内，从路径角度来看没有发生碰撞，但在机械臂路径跟随过程中，由于连杆为刚性，所以在运动过程中，臂段会与路径外的障碍物发生碰撞。因此，在设计避障算法时，需预留安全距离。

图3 路径跟随运动Fig.3 Path following motion

由于该扫掠区域在计算时较为复杂，且与路径转角有关，因此将此区域扩大，利用该扫掠区域的最大宽度来设计避障算法，使障碍物距离路径至少为该最大值。对于该值，如图4 所示：路径为FOH，选取第1 段连杆AB运动过程中3 个阶段进行分析，分别为A′B′到AB再到A″B″。其中，当连杆AB运动至路径拐点O与连杆AB的数学关系为OA=OB时，即图4 中AB处，此时为整个路径跟随运动过程的中间时刻，由A′B′和A″B″可知，在此中间时刻，之前和之后的运动过程扫掠区域对称，若将路径扩展至F′O′H′处后再进行路径规划，则在原路径FOH上进行路径跟随运动时，障碍物在任何位置均不会发生碰撞。将路径进行移动扩展，扩展的距离为O′N。该距离可扩展至障碍物上，再进行路径规划。该距离求解方式为

图4 扫掠区域分析Fig.4 Analysis of swept area

式中，l为连杆长度；h0为连杆间关节间距；θmax为极限转角大小。

1.2 障碍物模型处理及碰撞检测

在本研究路径规划算法设计中，障碍物用球体来代替。由于该路径用于超冗余度机器人路径跟随运动，因此在规划路径时需考虑超冗余度机器人机械臂部分在该路径上运动时的碰撞问题。

首先由于机器人臂段为圆柱形，对于机器人连杆与障碍物两个实体之间的空间位置关系，可以简化为直线段和实体之间的相对位置关系，将原有球形障碍物沿半径方向扩大与臂段圆柱半径等长距离，在此基础上进行路径规划。

同时，通过上文相邻关节的路径跟随运动分析，在路径跟随运动时会扫掠过一个区域。对于为避免该区域的碰撞而预留的安全距离，同样可以类比至障碍物上，将障碍物沿半径方向扩大该最大值的距离。

针对碰撞检测算法，在进行设计时，不仅要考虑障碍物球心到直线的最小距离，还要考虑线段与球形障碍物的位置关系。设障碍物球心O到连杆AB的垂直距离为dmin，根据连杆AB与球形障碍物的位置关系，可分为以下两种情况。

（1）障碍物球心O到连杆AB的距离大于障碍物的半径，此时机械臂与障碍物不发生碰撞，即dmin≥r，如图5（a）所示。

（2）障碍物球心O到连杆AB的距离小于障碍物的半径，此时分为以下几种情况进行讨论。

a. 连接障碍物球心与连杆两端点AB，若||OA||

图5 杆与球形障碍物的位置关系Fig.5 Position relationship between rod and spherical obstacle

b. 连接障碍物球心O与连杆两端点A、B，若||OA||>r且||OB||>r，则继续分为以下情况进行讨论。

如图6 所示，判断A、B两点在同侧还是异侧，只需判断两个角α、β是否全部小于90°。若其中有任一角度大于90°，则A、B两点在O点同侧；若均小于90°，则A、B两点分布于O点异侧。根据余弦定理

图6 杆端点相对位置Fig.6 Relative position of rod end point

判断两角是否均小于90°，则只需判断f是否大于0即可。

若f>0，则A、B两点位于障碍物球心O两侧，发生碰撞，如图7（a）所示；若f≤0，则A、B两点位于障碍物球心O同侧，不发生碰撞，如图7（b）所示。

图7 同侧和异侧碰撞检测结果Fig.7 Same side and different side collision detection results

2 RRT* 算法改进

2.1 RRT*算法

定义整个搜索空间为Q，搜索空间Q由障碍物空间Qobs和自由空间Qfree组成。在搜索空间中，xstart为起始点，xgoal为目标点。路径规划的定义为，在自由空间Qfree内寻找出一条连接xstart与xgoal的路径，使得路径无碰撞且满足以下约束条件：规划出的路径应尽可能短，且不与障碍物发生碰撞；考虑到超冗余度机器人后续路径跟随问题，规划出的路径不应包含过大转角。

基于随机采样原理的快速搜索随机树方法（RRT）具有概率完备性、计算量小、计算难度低等特点，它对解决高维状态规划问题有着较强的适应性，这一能力是目前很多规划算法所不具备的，因此非常适合高维运动状态规划的实际情况。鉴于 RRT*算法不仅加快了原RRT 算法的收敛速度，还增强了算法的最优性能，因此选取 RRT*作为路径规划的基础方法。

RRT*算法的具体工作流程如下。

（1）将系统的初始状态（末端位置）作为树的第1个顶点xstart，将目标点作为树的最终节点xgoal。

（2）在搜索空间Q内，按照目标偏向思想，随机生成一个采样点xrand，搜索随机树T上距离随机点xrand距离最近的节点xnearest，并拓展一个固定步长生成新的节点xnew，检测节点xnearest与节点xnew的连线间是否有障碍物，如没有障碍物则将节点xnew添加到搜索树T上，并计算其价值函数。

（3）重选父节点：在生成的新节点规定的邻域R范围内搜索节点集合xnear，计算将集合内点xnear–i作为xnew父节点xparent的价值函数，并与之前以xnearest作为父节点xparent的价值函数相比较。如果以xnear–i作为xnew父节点xparent的价值函数小于原价值函数cost，并且检测连线后无碰撞，则将xnearest与节点xnew的连线断开，连接xnear–i与xnew，并更新价值函数。

（4）剪枝重新布线：遍历集合xnear内所有节点xnear–i，若xnear–i与xnew连线不与障碍物碰撞，且xnear–i与xnew的价值函数加上xnew到起始节点xstart的价值函数小于xnear–i到xstart的原价值函数，即断开xnear–i与原父节点的连接，并将xnew作为xnear–i的父节点，连接xnear–i与xnew，并更新价值函数。

（5）重复以上过程，经过有限次迭代优化求解，直至搜索树T存在一点与目标点xgoal之间的距离小于所规定的阈值，停止搜索。

（6）由目标点向起始点检索，生成完整的 RRT*路径。

2.2 改进RRT＊算法

标准RRT*算法为随机采样搜索算法，所以较大概率产生的路径太过曲折、无用节点数过多。因此本文提出以下4 种改进策略。

（1）路径生成方式改进。

传统的RRT*算法中引入了目标偏向思想，具体流程为随机树在随机获取采样点时，首先按照均匀概率分布获取一个概率值p，若p小于设定的阈值，则xrand=xgoal；若p大于阈值，则按照原有的随机采样方式获取随机点。目标偏向思想的引入，可以减少随机树在扩展时的随机性，减少损耗。但随机点生成后路径的产生方式上并没有任何改变，直接按照固定步长向随机点方向扩展。因此本文对路径产生方式进行改进，将目标点信息进行再次引入：首先，在采样空间内随机生成一个点xrand0，连接xnear与xrand0确定单位向量a，连接xnear与xgoal确定单位向量b，随机生成两个0 ～ 1 之间的随机数p1、p2，随机点的长度与方向按照式（4）确定。

式中，σ为拓展步长。

（2）路径角度约束。

根据前文分析得知，由于超冗余度机器人关节结构特点，生成路径中的转角应小于θmax，因此在生成新节点时引入角度约束。对于图8 所示路径中连续的3 个节点x1、x2、x3及夹角角度φ，计算公式为

图8 路径转角Fig.8 Path corner

根据式（5）判断扩展的新节点xnew和xnearest的连线与xnearest和其父节点xnear–parent的连线之间的夹角是否超过限定值。若大于限定角度，则放弃节点xnew；同样的，在重选父节点和剪枝过程中也应遵守此规则。对于重选父节点过程，当选定父节点xnear–i和xnew的连线与xnear–i和其父节点的连线夹角大于限定角度时，放弃xnear–i作为xnew的新父节点。在剪枝过程中，判断xnear–i和xnew连线与xnew和其父节点连线的夹角是否大于限定角度。若大于限定角度，则放弃本次剪枝过程。

（3）重选父节点及剪枝过程优化。

在重选父节点及剪枝过程中，对于传统的RRT*算法，需在邻域R的范围内遍历所有节点，对路径进行优化。但该过程随着邻域范围的增大，计算量也会增大。对于该过程，在进行遍历之前，增加一个与起始点xstart连接的过程，然后进行碰撞检测，若无碰撞，则直接将xstart作为新的父节点。若满足无碰撞的条件，则不仅可以大大减少因遍历所有节点而产生的计算量，而且在一定程度上对路径起到优化作用。

（4）碰撞检测后调整。

在传统RRT*算法中，拓展步长为一个定值σ，这个定值需在算法中作为初始量给出。若σ过大，则在扩展过程中容易产生碰撞，生成大量无效节点，扩展效率低；若σ过小，则扩展次数将增大，增大了算法收敛的时间。

针对这一问题，本文采用拓展步长调整策略，即随机树通过拓展新节点，当拓展路径与障碍物发生碰撞时，利用二分法进行调整，具体方式为：取新节点xnew与其父节点xparent连线的中点xmid，将xmid作为新的xnew进行碰撞检测，若仍发生碰撞，则重复进行该过程，重复两次后若仍有碰撞，则放弃该节点。

改进RRT*算法的大体流程如图9 所示。

图9 改进RRT*算法流程图Fig.9 Flow chart of improved RRT* algorithm

3 试验与分析

为检验改进RRT*算法的性能，在MATLAB2018a上进行仿真试验。试验软件环境为：Windows10，硬件配置为：Inter（R）Core（TM）i5–6200UCPU@2.30GHz，内存（RAM）8GB。三维仿真地图尺寸设置为2000×2000×2000，起始点为（10，10，10），终点为（2000，2000，2000）。障碍物设置为球形，具体坐标及大小见表1。设置初始固定步长为400mm，在改进算法中，路径角度约束为20°。

表1 障碍物坐标及大小Table 1 Obstacle coordinates and size

本文算法是在RRT*算法的基础上进行改进后提出的，因此设计试验时，采用标准RRT*作为对比算法进行对比试验。由于扩展随机树算法的随机性，因此在进行试验时，每种对比算法进行50 次试验，将50 次试验的数据取平均值作为对比数据，对比结果数据见表2。

表2 对比试验数据Table 2 Comparison of experimental data

如图10 所示，通过对比试验可以看出，改进后的RRT*算法具有以下特点。

图10 路径规划结果对比Fig.10 Comparison of path planning results

（1）搜索效率方面。改进后的RRT*算法，搜索平均时间为0.702 s，标准RRT*算法的搜索时长为0.534 s，在搜索时长上虽有所增加，但该算法应用于离线轨迹规划，搜索时长为次要因素，且每次规划出的路径均能符合超冗余度机器人的路径跟随运动，无需人工再次进行筛选；标准RRT*算法的平均节点数为101，改进后算法的节点数为75，在搜索效率上与原算法相比，平均迭代次数减小，平均规划时间相差不大。因此可以看出，改进后的算法在增加多种约束后，虽然算法的计算量有所增加，但节点数变少，内存消耗减小。

（2）路径长度方面。改进后的RRT*算法，平均路径长度为3610.2771 mm，相比于原算法的平均路径长度3951.8089 mm 减少了近9%，由该数据可知，改进RRT*算法规划出的路径长度较小，提高了路径的质量。

（3）路径优化方面。由于改进RRT*算法中路径转角约束的引入，所有路径转角均小于20°，且由于碰撞后调整策略的引入，使得路径在空旷区域中以固定步长直接通过；在障碍物较多区域，碰撞后进行调整，路径得到明显优化。

4 结论

本文通过对超冗余度机器人的结构和相邻关节跟随运动时臂杆的跟随运动状态进行分析，一方面对障碍物进行了两次膨胀化处理，避免了机器人运动过程中潜在的干涉碰撞问题；另一方面对RRT*算法进行了改进，提出了4 种改进策略，缩短了规划路径的长度，同时避免了过大关节转角，符合超冗余度机器人在后续路径跟随运动的路径要求，实现了在三维空间内的避障路径优化。