基于自适应目标偏置系数的机械臂路径规划算法

吴文迎， 蔡锦达， 高朋帅

(上海理工大学 出版印刷与艺术设计学院， 上海 200093)

机械臂的路径规划问题一直是人工智能领域研究的热点。路径规划的目的是使机械臂在一定约束条件下，从起始状态到目标状态，在三维空间中搜索无碰撞路径进行操作[1]。在对机械臂做路径规划的同时，要满足3个原则：①可行性，即路径规划必须能够实际用于真实机械臂，而不是“纸上谈兵”；②最优解，避障问题是指在有障碍物的环境中，从起点到目标点规划一条不发生碰撞的最优路径[2]；③防碰撞，机械臂投入使用后维修检测都将会耗费人力财力，因此在做路径规划时应最大程度地避免机械臂运动中与障碍物的碰撞。

为了找出能够同时满足以上3个原则的路径规划方法，LaValle[3]提出了快速搜索随机树(rapid-exploration random tree，RRT)算法，该算法可以对高维空间快速搜索，通过在随机采样点向空白区域搜索避开模拟障碍物从而高效地解决复杂高维空间的路径规划问题。由于RRT算法的实际应用效率较高，越来越多的路径规划研究基于该算法，于是涌现出了大量RRT的改进算法。Kuffner等[4]提出了基于双向扩展平衡的连结型双树RRT-connect算法，该算法在RRT算法的基础上同时发展2棵随机树，分别从起始点和目标点生长，并运用贪婪策略将2棵树连接起来，以减少搜索时间。Sertac Karaman等[5]将基于随机采样的路径规划算法与随机几何理论相结合，提出了一种新的快速寻优随机图算法(rapidly exploring random graph，RRG)，并将该算法的树型版本称为RRT*算法。

课题组在RRT算法的基础上，综合RRT-connect和RRT*算法的优势，提出了一种基于自适应目标偏置系数的机械臂路径规划算法，该算法将自适应偏置系数添加到节点的拓展过程中，从而减少了随机采样点的数目，减少了算法运行时间。在路径树生成后，加入了剪枝算法来进一步优化路径树。与RRT算法及其已有的改进算法相比，该算法可以有效地提高搜索效率和路径质量。

1 机械臂的运动学分析

课题组将工作末端为无影灯的6自由度机械臂作为研究平台。建立6自由度机械臂D-H参数连杆坐标系如图1所示，然后根据坐标系计算各关节的D-H参数。

图1 D-H参数连杆坐标系Figure 1 D-H parametric linkage coordinate system

由D-H参数坐标系得出各个关节的D-H参数如表1所示。

表1 机械臂的D-H参数表Table 1 D-H coordinate parameters of manipulator

由于课题组对机械臂所做的路径规划基于给定的初始点及目标点在坐标系中的关节坐标，因此不对机械臂做逆运动学求解。

2 路径规划方法

RRT算法及其改进算法是一种基于随机采样的查询步进式算法，在高维空间和复杂约束下的路径规划应用中具备良好的使用性能[6]。

2.1 基础RRT算法

RRT算法即快速搜索随机树算法，原理是随机发展出一棵路径树T。RRT算法主要步骤如流程图2所示。首先初始化参数，起始点S、目标点G，扩展步长SS、偏差b等。

图2 RRT算法流程图Figure 2 RRT algorithm flowchart

路径规划问题是对所有τ∈[0,1]，寻找一条从初始配置σ(0)=S开始到达目标区域σ(1)∈G且满足σ(τ)∈xrand的无碰撞路径σ:[0，1]→xrand[7]。如图3所示，这棵路径树从起始点S开始生长，在地图空间中随机采样取点，寻找这棵路径树中与随机采样点Xrand最接近的点Xnear，以Xnear为初始点沿着Xnear-Xrand方向延长步长Δ得到新点Xnew，且此点需要与随机采样点不触碰障碍地连接起来，于是对点Xnew做碰撞检测，即检验Xnew和Xrand2点之间的路径是否存在障碍。通过碰撞检测的新点加入路径树中。搜索过程中不断地在路径树中加入随机采样点，生成随机路径树，直到探索到目标点G，于是生成一条从起始点到目标点的唯一路径。

图3 RRT算法路径规划的过程Figure 3 RRT algorithm path planning process

RRT算法伪代码。

T.add(S)

fori=1 toNset

xrand←Sample( )

xnear←Nearest(T,xrand)

xnew←Extend(xnear,xrand,StepSize)

if Collectionfree(xnew,world)

T.add(xnew)

else continue

if ‖xnew-G‖≤bri

returnT, flag=ture

else flag=false

returnT, flag

其中Sample( )函数在六维空间中完成随机点的采样。Extend( )函数完成节点的扩展，Collectionfree( )函数完成采样过程中的碰撞检测。

图4所示为RRT算法在二维障碍空间和三维障碍空间中的路径规划结果。RRT算法适用于多障碍物情况下6自由度机械臂的运动规划，但该算法虽然降低了路径规划的计算成本，却因为采样点的随机性导致收敛速度慢，路径生成时间长，且未对生成的路径做后处理，因此不是路径规划的最优解。

图4 RRT算法路径规划结果Figure 4 RRT algorithm path planning results

2.2 RRT-connect算法

RRT算法从起始点发展一棵路径树到目标点，由于采样点的选择具有随机性，因此该算法的收敛速度较慢，路径生成效率不高。RRT-connect算法在RRT算法的基础上提出了双树的构想，即从起始点区域和目标点区域同时搜索状态空间通过随机采样点来各自发展一棵路径树。如图5所示，在每一次迭代中，发展一棵树的新点后，将此点作为另一棵树的目标点进行生长，2棵树不断地往对方的方向交替扩展，直到2棵树规划出的路径支点相连。不同于RRT算法在生成样本并插入到路径树中时扩展最大扩展长度，在没有障碍物碰撞的情况下，路径树继续向目标方向扩展，因此可以更快地规划路径[8]。

图5 RRT-connect算法路径规划的过程Figure 5 RRT-connect algorithm path planning process

RRT-connect算法伪代码。

TaTb

fori=1 toNset

xrand←Sample( )

xnear←Nearest(Ta,xrand)

xnew←Extend(xnear,xrand, StepSize)

Ta.add(xnew)

if Getit(xnew)

flag=ture

return(Ta,Tb, flag)

else

swap(Ta,Tb)

flag=false

return(flag)

RRT-connect算法路径规划结果如图6所示。

图6 应用贪婪策略的RRT-connect算法Figure 6 RRT-Connect algorithm with greedy strategy

RRT-connect算法在处理节点时，只要一棵路径树上生成的新节点与另一棵路径树的距离比指定的阈值小，则连接2点，这容易导致路径的连接处转角过大，甚至出现180°的转角[9]。较RRT算法而言，RRT-connect算法提高了搜索速度和搜索效率，2棵树的快速生长带有启发性且能够有效避免陷入局部最优，路径树生长的搜索过程更加贪婪和明确。

2.3 RRT*算法

RRT算法中，选取的随机采样点总是和距离该点最近的点连接，而不是最优的点。RRT*算法针对这一问题在RRT算法的基础上做出改进，在随机采样点加入路径树后，RRT*算法没有直接将最近邻节点作为其父节点, 而是以该点周围半径为r的区域为目标区域寻找一个使其代价值最小的节点作为父节点[10]，即重置父节点操作。如图7所示，标准是加入路径树后能够使得该点到起点的距离更短。如果重置后的父节点可以减少路径代价，则将新的节点加入路径树连接起来，摒弃原来的连接线，即重布线操作。重布线操作对每一个新点的父节点选择增加一个优化，减少每一代的路径距离[11]。

图7 重置父节点操作示意图Figure 7 Resetting parent node operation schematic

RRT*算法伪代码。

T

fori=1 toNset

xrand←Sample( )

xnear←Nearest(T,xrand)

xnew←Extend(xnear,xrand, StepSize)

T←xnew

xnear←Findnear(xnew,T,r)

Chooseparent(xnew,xnear)

Rewrite(xnew,xnear)

if Getit(xnew)

flag=true

else

flag=false

returnT, flag

RRT*需要大量的节点回溯来判断约束条件，从而影响收敛速度，难以应用于具有刚性实时性要求的系统[12]，在通道大且障碍物多的地图环境中效率较低。

2.4 改进RRT算法

综合RRT-connect算法和RRT*算法的优点，课题组提出了一种基于自适应目标偏置系数的改进RRT算法，改进后的RRT算法基于随机采样的方式，应用了双树的构想，不应用贪婪算法。在起始点区域和目标点区域各自生长一棵路径树，将对方作为交替生长的方向，当双树连接后即终止对地图空间的搜索，将2棵路径树整合成为1颗从起始点到目标点的路径树，最后对合成的路径树做剪枝后处理。

2.4.1 自适应偏置算法

RRT算法具有随机特性，因此路径树的生长缺乏导向性，在路径树的生长过程中设置1个偏置概率参数可以使得随机采样点更偏向于目标点[13]。

如图8所示，设置的目标偏置系数P决定了新节点Xnew偏向随机采样点Xrand还是目标点G，当P的值越大说明新节点Xnew越偏向目标点G，当P的值越小说明新节点Xnew越偏向随机采样点Xrand。且有

图8 应用目标偏置系数的扩展方法Figure 8 Eextended method of applying target bias coefficient

(1)

式中0

如图9所示，应用目标偏置系数可以使得对地图空间的探索更具目的性，但实验发现当P较小时，路径树的搜索更具随机性，但同时在地图空间的搜索时间更长；当P较大时，路径树的生长更具目的性，但随机性减小，难以处理遇到较大障碍物时的情况，搜索失败率提高。因此在路径树生长过程中引入自适应偏置系数应用于新节点的选择，根据Xnear与离它最近的障碍物的直线距离Len来设置偏置系数P。若Len较大，则增大P的值，使得路径树能够更加快速往对方的方向逼近；若Len较小，则先使得路径树迅速规划路线逃离障碍物，再增大P的值，迅速往目标方向逼近。引入自适应偏置系数的目标偏置算法路径规划结果如图10所示。

图9 目标偏置算法障碍空间中搜索Figure 9 Search in obstacle space of target bias algorithm

图10 引入自适应偏置系数的目标偏置算法Figure 10 Target bias algorithm with adaptive bias coefficient

自适应偏置函数GoalExtend(xrand,xnear,G)使得路径树向另一棵树的方向生长，得到其伪代码。

xrand,xnear, G

d←dist(xnear, world)

P←Bias(d)

xnew←f(P)

returnxnew

2.4.2 重置父节点

RRT*算法利用重置父节点和节点重构2个方法来降低路径树生长成本，创造最优的路径收敛效果，但在每一次的迭代中都存在重置父节点和节点重构会较大地影响路径树的生长速度。且局部采样的步骤提前会使得搜索路径树的随机性降低，甚至有可能降低生成的路径树质量。改进的RRT算法为路径树中生成的新节点扩大重置父节点的范围并重置父节点，不做节点重构。改进的RRT算法中重置父节点范围的过程如下[14]：

1) 生成节点xnew，并找出xnear的点集合；

2) 遍历xnear点集合中的节点，将这些节点的n级父节点加入xnear′的点集合中；

3) 合并xnear与xnear′的点集合，作为xnew重选父节点的范围。

计算并保存生成的新节点到起始点的代价函数Fmin，将生成的新节点Xnew作为中心，遍历路径树上和Xnew的距离小于r且满足函数Collectionfree( )的节点，将这些节点作为Xnew的父节点，把它们与Xnew的距离Hn计算保存下来，计算Xnew的代价函数Fn=Gn+Hn，将Fn的最小值与Fmin作比较，若最小值小于Fmin，则重置父节点，重新选用Fn的最小值所对应的节点为Xnew的父节点。

2.4.3 改进的RRT算法伪代码

得到改进的RRT算法伪代码。

nTTaTb

fori=1 toNset

xrand←Sample( )

xnear←Nearest(Ta,xrand)

xnew←goalExtend(xnear,xrand,Tb, StepSize)

Ta.add(xnew)

xnear←Findnear(xnew,Ta,r)

Chooseparent(xnew,xnear)

flag=false

if Getit(xnew)

flag=true

else

swap(Ta,Tb)

if flag=true

T=Ta+Tb

nT=Trim(T)

return(nT, flag)

return(flag)

图11所示为改进后的RRT算法分别在二维障碍空间和三维障碍空间中搜索路径的结果，可以看出改进RRT算法路径曲率较小，长度较短，质量较高。

图11 改进的RRT算法搜索路径的结果Figure 11 Improved RRT algorithm for searching path results

3 路径后处理

改进后的RRT算法虽然能够有效提高地图空间中的RRT搜索效率，但因其搜索能力更强，也会导致路径的扩展方向变化较大，优化后路径仍有许多冗余点。在有障碍物的复杂地图空间中，冗余点过多会导致无法有效跟踪机械臂的运动[15]。因此对得到的路径做剪枝后处理是必要操作。

剪枝处理为路径树剔除冗余点，将剩下的关键拐点连接起来生成更优的路径树。本研究的剪枝处理算法Trim(T)应用了贪婪策略，路径树是由位姿点集nT形成的一条连接起始点和目标点的路径。首先将路径树生长过程中生成的节点全部考虑，然后检查相邻节点的连接线是否接触地图空间中布置的障碍物。只要选取的2端节点的连线和障碍物没有交叉,则删除2端节点之间的其他节点,2端节点可以直接连接作为一个新的路径,直到发生碰撞,将碰撞点的父节点作为新的起点,然后碰撞点再次被持久化，重复以上的步骤直到达到目标点[16]。如图12所示，经过剪枝处理后的路径在机械臂的实际应用中更为合理，减少了冗余点和路径的长度。

图12 剪枝处理前后得到的路径Figure 12 Path obtained before and after pruning

4 实验结果与分析

4.1 对比实验

基于对6自由度机械臂运动过程中速度和稳定性的要求，利用MATLAB仿真软件对课题组提出的改进RRT算法进行仿真，并将改进算法中的关键参数与不同算法产生的效果进行比较[17]，来判定算法的质量。

在二维的确定障碍空间内进行对比实验，将搜索时间作为判断路径搜索速度的指标；将路径长度和最大曲率作为判断路径质量的指标。考虑到其他因素对实验的影响，取各算法运行100次的平均值作为结果参数，设置搜索步长SS=0.5。由于RRT*算法具有渐进最优的特性，因此实验中发现RRT*算法搜索20 000次后生成的路径基本一致，课题组将RRT*算法搜索20 000次后生成的路径长度为最短路径。实验采用Intel Xeon(至强)W-2255@3.70 GHz的10核处理器，实验结果如表2所示。

表2 实验结果对比Table 2 Comparison of experimental results

对比实验结果显示，RRT算法搜索速度相对较慢，路径质量也较差；RRT-connect算法因为应用了贪婪算法，因此每一次迭代都会生成多步搜索，路径规划的单次时间最短，搜索速度最快，但RRT-connect算法生成的路径最大曲率相对另外2种RRT改进算法要大得多，且生成的路径长度最大，超过RRT*算法生成的路径长度37.5%，路径质量较差；RRT*算法产生的路径长度最小，最大曲率也仅仅略高于本研究中改进的RRT算法，路径质量较好，但单次搜索时间远高于其他3种算法，搜索速度太慢。

改进的RRT算法仅在单次运行时间上略长于RRT-connect算法，但该算法生成的路径最大曲率相对较小，路径长度略高于RRT*算法，超过RRT*算法规划的路径长度的2.6%，实验结果显示改进的RRT算法在保证搜索效率的基础上，大幅提高了生成的路径质量。

4.2 基于ROS的仿真模拟

在ROS平台的Rviz仿真软件中模拟手术室的工作环境[18]，将基于自适应目标偏置系数的RRT算法应用在6自由度机械臂上，通过机械臂避障规划与控制实验来验证算法的有效性。图13所示为6自由度机械臂在2种不同的环境中做路径规划的仿真过程[19]。

图13 机械臂在变化环境中的路径规划Figure 13 Path planning of manipulator in changing environment

图14所示为6自由度机械臂分别在这2种模拟环境中工作时各个关节的角度随时间变化的曲线。通过曲线可以看出不同环境中关节的运动存在差异，但各关节变化的曲线都较为平滑，展现了6自由度机械臂模拟工作时稳定性较好，且冗余的路径长度被有效减少。

图14 仿真过程中各关节弧度变化曲线Figure 14 Curve of each joint change during simulation

5 结语

课题组提出了一种基于自适应目标偏置系数的机械臂路径规划算法，利用MATLAB对该算法进行仿真实验，将实验结果与已有的RRT算法及其改进算法的路径规划结果作对比分析，并在基于ROS的仿真平台进行验证。实验结果表明：应用自适应目标偏置系数的改进RRT算法兼顾搜索效率和路径质量，且应用贪婪策略的剪枝后处理算法有效减少了冗余节点和路径长度，该算法有较好的避障性能和寻优能力，符合在实际应用中对机械臂工作稳定性的要求。课题组改进的RRT算法模拟仿真的地图环境中只有静态障碍物，没有考虑到有动态障碍物的地图环境。接下来的研究应围绕如何将应用场景扩大到添加了动态障碍物的地图环境中对算法作进一步改进，提高算法的实用性。