基于改进RRT*-Connect算法的机械臂多场景运动规划

王怀震 高 明 王建华 房立金 李洪生

(1.浪潮集团山东新一代信息产业技术研究院有限公司，济南 250101;2.东北大学机器人科学与工程学院，沈阳 110819)

0 引言

随着机器人技术的不断发展，机械臂在工业生产和农业采摘领域已经得到广泛应用。机械臂的避障运动规划已经成为机械臂应用的前提。其主要目的是为了实现机械臂在复杂环境中，可以自主地进行搜索并规划出一条从起始位置到目标位置的无碰撞路径[1-3]。目前，机器人规划方法主要有：①基于图搜索的规划方法，如Dijkstra算法和A*算法[4]。由于基于图搜索的方法难以实现高维度环境的机械臂运动规划，因此该类方法多数应用于移动机器人的路径规划。②基于采样的规划方法，如随机路图法(Probabilistic roadmap method,PRM)[5]和快速搜索随机树(Rapidly-exploring random trees，RRT)[6-7]。在运用采样方法进行搜索路径时，其不会存在随环境维数和大小变化而导致的规划效率低和成功率低等问题。RRT算法与PRM算法相比，其更适合解决高维空间中路径搜索问题。因此，RRT算法普遍应用于在机械臂的路径规划领域。

近年来，基于RRT算法的机械臂路径规划得到国内外学者的广泛关注和研究[8-12]。RRT算法是一种随机采样的搜索算法,通过扩展固定搜索步长的随机树进行路径搜索。然而，传统的RRT方法在生成新节点时，随机扩展策略往往会出现路径规划速度较慢和节点数量多的问题。因此，在实际应用时，该算法存在诸多局限性。文献[13]在传统RRT算法的基础上提出了一种双向版本的RRT(RRT-Connect)方法。该方法可以在初始位置和目标位置同时采样并生成扩展随机树，当两棵随机树之间建立连接时，算法停止搜索并输出一条无碰撞的路径。虽然RRT-Connect算法可以更高效地解决机械臂路径规划问题，但由于缺乏优化过程，RRT和RRT-Connect皆无法提供路径的最优解。为了解决上述问题，文献[14]提出了一种渐进最优RRT*算法。RRT*算法相比于RRT方法，改变了父节点的选取方式并增加了剪枝优化过程。该算法在返回第一条路径后，会继续在环境中寻找最优路径，以生成一条渐进最优路径。但是，RRT*算法在全局环境中仍然采用均匀采样的方式，导致路径规划优化效率较低。文献[15]结合RRT-Connect和RRT*算法的研究思想，提出了一种RRT*-Connect算法。该算法能够有效地降低单向RRT*方法在规划过程的时间成本，但并没有解决RRT*算法优化效率较低的问题。为此，JONATHAN等[16]研究了一种更为先进的Informed RRT*(IRRT*)算法。该方法通过设计椭球子集约束对采样区域进行空间配置，缩小了自由空间中节点的采样范围，从而保证路径可以有效地收敛到全局最优解[17]。然而，当目标位置位于狭窄通道或障碍物较为复杂的环境时，IRRT*算法存在适应性较差和收敛精度较低的问题。值得注意的是，以上基于RRT的算法皆采用了固定步长的扩展方式进行路径规划，采样搜索树需要较长时间才能扩展到目标位置，这在一定程度上限制了算法的收敛速度[18]。

本文研究一种自适应步长的启发式RRT*-Connect机械臂运动规划算法。引入目标偏向策略进行椭球子集约束采样，使采样点能够更快地收敛到最优值。在扩展节点时，采用一种自适应步长策略以加快本文算法的收敛速度，并有效缩短规划路径的长度。当搜索树中总节点达到预设值时，通过搜索树优化剪枝策略对搜索树进行剪枝，删除无效的采样点，进一步降低运行时间。并进行仿真与实验验证该算法的优势和适应能力。

1 机械臂运动规划基本原理

1.1 运动规划定义

设集合X⊆R，称为运动规划状态空间。将Xobs⊂X的集合定义为障碍空间，Xfree=XXobs的集合称为无障碍空间，即自由空间。在自由空间中定义xstart∈Xfree为起始位置，定义xgoal∈Xfree为目标位置。运动规划问题是在空间X中寻找一条从初始位置σ(0)=xstart开始，到达目标位置σ(1)=xgoal的无碰撞路径，将其定义为集合σ：[0,1]→Xfree[19]。

定义c(σ)为每条无碰撞路径映射到一个非负实数的成本函数。最优运动规划问题就是寻找一条无碰撞最短路径以解决运动规划问题，设最低路径成本函数为σ*，即

(1)

1.2 碰撞检测技术

串联机械臂本体是由多个关节构成的链式结构。机械臂各关节的角度可以通过末端位置和方向进行逆运动学求解。根据正运动学可求出各连杆坐标系的齐次变换矩阵，进而得到各环节的等效空间线方程。由于机械臂本体固定于底座，因此只需要对各个关节进行检测就可以判断机械臂是否与障碍物发生碰撞。本文采用几何包络法[20]实现机械臂的碰撞检测。为了简化障碍物和机械臂模型，使用圆柱体来描述机械臂连杆，将不规则的障碍物采用球体、长方体进行描述。这样，机械臂的碰撞检测问题就可以转换为圆柱体、球体、长方体之间的碰撞检测。碰撞检测简化模型如图1所示。

图1 碰撞检测模型Fig.1 Collision detection model

2 改进RRT*-Connect算法

KLEMM等[14]针对RRT的缺陷提出了更为高效的RRT*方法，并在RRT-Connect算法原理的基础上，采用RRT*取代RRT，研究了RRT*-Connect算法。该算法虽然提高了RRT*的搜索效率，但在搜索过程中，由于算法在采样时采样点过于随机，采样点的分布过于分散，导致在复杂障碍物环境中仍然存在搜索效率和收敛精度较低的问题。

针对上述问题，提出了一种渐近最优的RRT*-Connect规划算法。采用一种新的启发式采样方法，对RRT*-Connect采样方法进行优化。在扩展节点时运用搜索树优化剪枝策略和自适应步长策略，进一步提高了搜索效率。与常规的RRT*-Connect相比，可以用更少的迭代次数返回比当前更优的路径。

2.1 启发式采样策略

常规的RRT*-Connect采样方法存在采样点过于随机分散的问题，为了提高采样效率，提出一种启发式采样方法，引入目标偏向策略进行椭球子集约束采样，使采样点能够更快地收敛到最优值。

2.1.1目标偏向策略

设定一个阈值α，根据障碍物环境决定α。当环境复杂且障碍物较多时，α设为较小值，反之α设为较大值。在采样时以概率α向目标方向采样，以概率1-α在自由空间内随机采样，这样在保证了搜索目标随机性的同时提高了搜索效率。在搜索过程中，同时从起始点和目标点开始采样生成Tree1和Tree2。随机树的具体采样方式为

(2)

(3)

式中P——随机数，取0～1

InformedSample——椭球子集采样

如式(2)、(3)所示，随机树每次在自由空间采样时，按均匀概率会随机产生一个概率P。如果P小于α，则采样点xrand采用带有目标偏向采样。这时，Tree1将目标点作为目标方向进行采样，而Tree2将起始点作为目标方向进行采样。如果P大于α，则采样点进行椭球子集约束采样。当搜索到一条路径方案之后，椭球子集半径随之缩小，使采样点在不断压缩的椭球范围里进行采样。因此，搜索路径将不断优化，有效提高了规划效率。

2.1.2椭球子集采样

图2 椭球子集采样Fig.2 Ellipsoid subset sampling

为了实现椭球子集内的均匀分布采样，将均匀分布的样本从半径为n的球变换到椭球子集，xellipse～μ(Xellipse)，即

xellipse=Lxball+xcenter

(4)

其中

xcenter=(xf1+xf2)/2

(5)

xball={x∈X|；‖x‖2≤1}

(6)

式中xcenter——超椭球体的中心

xball——椭球内随机采样点

L——变换矩阵

‖x‖2——x的二范数

xf1、xf2——焦点

该变换将通过超椭球矩阵S∈Rn×n的Cholesky分解来计算[21]，即

S≡LLT

(7)

(x-xcenter)TS(x-xcenter)=I

(8)

变换矩阵L使xellipse保持均匀分布。采样点子集x的估计可以通过横截半径和横轴来实现。横轴对角矩阵为

(9)

通过分解运算得

(10)

为了实现超椭球体旋转到世界坐标系，选取旋转矩阵的计算方法为

F=Udiag{1,…,1,det(U)det(V)}VT

(11)

x=FLxball+xcenter

(12)

最后，返回椭球内的采样点xrand。椭球子集采样算法伪代码为

1:ifcbest<∞ then

2:cmin←‖xgoal-xstart‖2

3:xcenter←(xstart+xgoal)/2

4:c←RotationToWorldFrame(xstart,xgoal)

5:r1←cbest/2;

7:L←diag{r1,r2,…,rn}

8:xball←SampleUnitBall

9:xrand←(FLxball+xcenter)∩X

10:else

11:xrand～μ(X)

12:returnxrand

2.2 搜索树优化剪枝策略

常规的RRT*-Connect方法在搜索树中的节点时，存在节点数无限增长的缺点，这在高维空间中将对内存造成计算负担。将采用搜索树优化剪枝策略限制搜索树中的节点数，以降低运行时间。

本文所设计的REWIRE例程与原始的REWIRE不同，当树中的节点数量大于阈值N时，需要检验每个重新布线的节点的原始父节点是否成为叶节点，如果成为叶节点，则从树中移除。在重新布线过程之后，如果树的大小仍然大于N，则执行强制移除，即随机移除一个叶节点。最后，如果重新布线和强制移除都不能使节点数量低于N，则返回到采样前的状态。优化剪枝算法伪代码为

1:procedure REWIRE(T,Xnear,xnew)

2:for ∀xnear∈Xneardo

3:cnear←COST(xnear)

4:cnew←COST(xnew)+MOTIONCOST(xnew,xnear)

5: ifcnew

6: if COLLISIONFREE(xnew,xnear)then

7:xparent← PARENT(xnear)

8: ifxnearhas no brothers∧M

9: REMOVENODE(xparent)

10:E←E{(xparent,xnear)}

11:E←E∪{(T,xnew,xnear)}

12:returnT

其中，xnear为随机树中与xrand邻节的点，xnew为重新布线生成新节点，xparent为父节点，cnew为新节点与父节点之间的距离，cnear为邻节点与父节点之间的距离，E为路径点的集合，T为随机搜索树。

2.3 自适应步长策略

在常规RRT*-Connect算法中搜索树一般采用固定步长进行扩展，生成路径转折较多，路径长度较长。本文提出一种自适应步长RRT*-Connect方法。当Tree1通过随机采样生成随机节点xrand1后，判断得到最近的邻节点xnearest1，以步长d向xrand1方向扩展，即

(13)

生成新节点xnew1点后，判断得到xnew1距离Tree2最近的邻节点xnearest2，然后以步长d向xnew1方向扩展，即

(14)

同理，Tree2采用同样的方式进行扩展。这里，步长d并非固定值，扩展的步长由机械臂和障碍物环境决定。其设计方法如下：

设定参数：小步长d1、大步长d2，设定阈值b1和b2。当距离Dtree小于阈值b1时，采用小步长进行扩展，这样可以增加搜索树之间的连接成功率。当两树之间的距离Dtree大于等于阈值b1时，则采用大步长d2进行扩展。自适应步长优化方法为

(15)

(16)

式中 (x1,y1)——Tree1节点

(x2,y2)——Tree2节点

如果机械臂与障碍物距离Dobs大于等于阈值b2，判定机械臂附近是自由空间没有障碍物，则采用大步长d2进行扩展。反之如果机械臂与障碍物距离小于设定阈值b2时，认为机械臂附近存在障碍物环境，采用小步长d1进行扩展。这样既能保证机械臂的避障效果，又能提高规划路径的效率。

(17)

(18)

式中 (x,y)——Tree节点

(x0,y0)——障碍物圆心

r——半径

最终改进RRT*-Connect算法伪代码为

1:T1←{xstart};T2←{xgoal}

2:fori=1 toNdo

3: ifP<αthen

4:xrand1←xgoal

5:xrand2←xstart

6: else

7:xrand←InformedSample

8:xnearest←NEAREST(T,xrand)

9: ifDobs

10:d=d1

11: else

12:d=d2

13:xnew1←steer(xnearest,xrand,d)

14:xnew2←steer(xnearest,xnew1,d)

15: if ObstacleFree(xnew1)then

16:Xnear←NEAR(xnew1)

17: CHOOSEPARENT(Xnear,xnew1)

18: REWIRE(Xnear,xnew1)

19: FORCEREMOVAL(T1,xnew1)

20: ifDtree

21: if ObstacleFree(xnew2)then

22:Xnear←NEAR(xnew2)

23: CHOOSEPARENT(xnear,xnew2)

24: REWIRE(xnear,xnew2)

25: FORCEREMOVAL(T2,xnew2)

26: ifDtree

27:path=CONNECT(T1,T2)

28:return path

其中，T1、T2分别为从起始点和目标点生成的随机树，xnearest1和xnearest2为随机树中与xrand的最近邻节点，xnew1和xnew2为重新布线生成新节点，b为设定阈值。

为了更好地呈现本文方法的运行过程，文中给出改进RRT*-Connect算法的流程图，如图3所示。本文算法的步骤为：

图3 算法流程图Fig.3 Algorithm flow chart

(1)初始化地图，设置起始位置和终止位置、启发率、采样节点总数、动态步长等参数。

(2)引入目标偏向策略，Tree1以起始位置为起点,以终止位置为目标(Tree2以终止位置为起点,以起始位置为目标),进行椭球子集约束生成随机采样点进行启发式采样。

(3)确定离Tree1/Tree2最近节点，通过自适应步长策略确定动态步长。

(4)生成Tree1/Tree2新节点，并通过碰撞检测判断两树是否满足连接，能连接则跳至步骤(7)，不能连接则进行步骤(5)；未通过碰撞检测则返回步骤(2)重新进行采样。

(5)对新的节点选择父节点并重新布线操作。

(6)当搜索树中总节点数大于预设阈值时，通过搜索树优化剪枝策略对搜索树进行优化剪枝。

(7)判断两树距离是否小于一个步长。如果小于一个步长则通过连接程序将两树连接起来，输出路径信息path。反之则不能连接返回步骤(2)继续进行搜索。

(8)算法迭代完成生成最终的规划路径，规划任务结束。

3 仿真与实验

为了验证本文改进算法的优势和有效性，通过Matlab仿真平台对算法进行验证并分别与RRT*[13]、RRT*-Connect[14]、IRRT*[15]算法在多场景下的规划结果进行数据对比。为了更好地验证本文规划算法的实用性，在Matlab和ROS平台搭建机械臂实验环境，实现本文算法在不同应用场景的机械臂运动规划。

3.1 Matlab仿真对比

3.1.1场景1

设置650 mm×650 mm环境场景1，设(20 mm,20 mm)为起始位置，(630 mm，630 mm)为目标位置，设置启发式概率α=0.15，最小步长为5 mm，最大步长为15 mm，设置固定的最大节点数为1 000，最大的迭代次数为5 000。4种算法的仿真结果如图4所示。进行100组重复实验，4种算法测试实验结果见表1。

图4 场景1中不同算法的规划结果Fig.4 Planning results of different algorithms in scenario 1

由图4可以看出，RRT*-Connect比本文算法的搜索过程更为复杂且路径明显更长。IRRT*同样采用椭圆子集进行采样，但搜索树包含节点更多。对比表1中的测试结果,可得本文算法在相同迭代次数下，具有更短的路径长度、更快的收敛速度且成功率达到98%。

表1 场景1中4种算法测试结果对比Tab.1 Test comparison results of four algorithms in scenario 1

3.1.2场景2

相较于场景1，场景2在同样的起始位置与目标位置间存在一条狭窄通道，其设计目的是为了验证各个算法在狭窄通道内的路径规划能力。4种算法的路径规划结果如图5所示。进行100组重复实验，4种算法测试实验结果见表2。

由图5和表2可以看出，在狭窄通道场景下，RRT*-Connect比RRT*和IRRT*的搜索路径成本较低，搜索时间更短，而RRT*的搜索成功率最低，充分表明了双向搜索RRT*的优势。与RRT*-Connect相比较，本文算法的路径长度更短，体现出自适应步长在狭窄通道内搜索的优势。从全局数据来看，本文算法在狭窄通道内规划路径更短、效率和成功率更高。

表2 场景2中4种算法测试结果对比Tab.2 Test comparison results of four algorithms in scenario 2

3.1.3场景3

相较于场景1和场景2，场景3的环境设置相对复杂，目的是为了验证算法对复杂环境的适应能力。本文算法的实验结果如图6所示。进行100组重复实验，4种算法测试实验结果见表3。

表3 场景3中4种算法测试结果对比Tab.3 Test comparison results of four algorithms in scenario 3

由图6可得，本文算法在复杂环境内同样具备较强的适应能力。通过对比测试数据可以看出，本文算法在相同仿真条件下，实现路径长度更短，平均运行时间更优，提高了路径搜索的成功率。

3.1.4场景4

为了验证本文算法在三维场景中的有效性，利用Matlab在三维空间内进行了规划算法对比实验，结果如图7所示。

图7 三维环境规划结果Fig.7 3D environmental planning results

由图7可以看出，本文算法在三维环境下的搜索树包含节点更少，规划路径更短且相对平滑，有效地证明了本文算法的优越性。

3.2 机械臂实验验证

3.2.1Matlab仿真

为了验证本文算法在机械臂中的有效性，在Matlab中建立了7自由度平面机械臂模型，并将本文算法应用于机械臂运动规划仿真实验。图8～10为不同场景下，机械臂的初始状态、结束状态和规划结果。

图8 不同场景机械臂初始状态Fig.8 Initial state of robot in different scenarios

图9 不同场景机械臂结束状态Fig.9 End state of robot in different scenarios

由图10可以看出，机械臂在不同场景中，由起始点向目标点不断扩展节点进行搜索，运行轨迹较为平滑，能够有效地避开障碍物，完成指定任务。

图10 不同场景机械臂规划结果Fig.10 Robot planning results in different scenarios

3.2.2ROS中机械臂规划实验

Sawyer机械臂是Rethink Robotics公司推出的7自由度智能协作机器人，能够较好地适应狭窄和稠密环境的作业任务。基于Linux系统对Sawyer机械臂进行仿真环境配置，利用ROS(Robot operating system)中的MoveIt!控制Sawyer机械臂进行避障运动规划，将本文算法在Sawyer机械臂上进行验证。Sawyer机械臂是7自由度协作机器人，每个关节可以在位置模式、速度模式和力矩模式下进行控制，其质量为19 kg，臂展为1 260 mm，有效载荷为4 kg，重复定位精度为±0.1 mm，Sawyer机械臂实验平台如图11所示。

图11 Sawyer机械臂实验平台Fig.11 Sawyer robot experiment platform

在Sawyer机械臂实验平台设置障碍物的实验场景，其中摆放长方体盒子为障碍物，左侧平板位置为起始点，将其移动到障碍物另一侧的目标位置方块处。实验分别与前文中RRT*、RRT*-Connect、IRRT*算法在多场景下的规划结果进行数据对比与分析，每种算法进行100 次重复实验。

由表4、5可以得出，RRT*-Connect算法比另外2种已有算法搜索轨迹成本低，能够有效缩短轨迹长度，但成功率低于IRRT*算法。通过对比全局实验数据可以看出，本文算法在不同的实验场景中可以实现轨迹长度更短，平均运行时间更优，提高了轨迹搜索的成功率。实验结果表明，本文算法在不同障碍环境下具有较强的适应能力。

表4 实验场景1中4种算法测试结果对比Tab.4 Test data comparison results of four algorithms in experiment scenario 1

由图12可以看出，机械臂运行过程平稳且流畅，说明了本文算法能够在真实机械臂上较好地实现避障运动规划，完成稠密环境的作业任务。实验结果证明了本文算法的有效性。

表5 实验场景2中4种算法测试结果对比Tab.5 Test data comparison results of four algorithms in experiment scenario 2

图12 本文算法ROS中机械臂的规划结果Fig.12 Planning results of proposed method for robot in ROS

4 结束语

提出了一种采用自适应步长的启发式RRT*-Connect机械臂运动规划算法。在迭代过程中，引入目标偏向策略进行椭球子集约束采样，使采样点能够更快地收敛到最优值。在扩展节点时，设计了一种自适应步长策略以加快本文算法的收敛速度，有效地缩短规划路径的长度。同时，在扩展节点过程，配置树中总节点数的预设值，采用搜索树优化剪枝策略对搜索树进行剪枝，从而降低规划的运行时间。在多场景下进行仿真与实验，并与RRT*、RRT*-Connect和IRRT*算法进行了对比，实验结果充分验证了本文算法的有效性和实用性。