基于均匀概率的目标启发式RRT机械臂路径规划方法

左国玉，陈国栋，刘月雷，龚道雄，李剑锋

(1.北京工业大学信息学部，北京 100124；2.北京工业大学材料与制造学部，北京 100124)

在机械臂将末端或目标物体安全平稳地移动到目标位置并且能够有效地避开障碍物的过程中，机械臂的机构设计及运动规划方法对机械臂实际作业场景中的有效应用就显得非常重要，提高机械臂的运动规划效率是机器人技术研究中的重要课题[1].运动规划分为路径规划和轨迹规划2种[2-4].在运动规划研究中，连接起点位置和终点位置的序列点或曲线称之为路径，构成路径的策略称之为路径规划.在路径规划的基础上加入时间序列信息称之为轨迹规划[5]，路径规划是运动规划中的主要研究内容之一.国内外学者提出了一些经典的路径规划方法，主要有A*法[6]、人工势场法[7]、快速扩展随机树(rapidly-exploring random trees，RRT)法[8]等.A*算法主要应用于二维平面的运动规划问题中，并且在此过程中需要花费大量的时间与精力构建可视图；人工势场法在对环境进行精确建模后，通过将目标点的引力场与障碍物之间的斥力场进行叠加得到人工势场，在该势场的作用下进行路径规划.但是，人工势场法难以获得环境的精确建模，人工势场构建难度大，并且存在容易陷入局部极小值的缺点.

RRT是一种基于随机采样策略的运动规划算法.相对于其他全局路径规划算法，它不需要对环境进行精确建模，可以减少大量的空间建模工作，从而更加适用于高维空间下的路径规划，并且具有较快的扩展和搜索速度，因此，被研究者广泛关注[9-10].然而，由于工作空间中RRT方法采用的是随机抽样和全局抽样策略，所以RRT方法具有较大的随机性和盲目性[11]，并且很难在短时间内生成更优的路径[12].针对快速扩展随机树的上述缺点，许多学者基于不同的研究对象和性能要求提出了一些改进的RRT算法，如RRT-Connect[13]、RRT*[14]、S-RRT[15]、NoD-RRT[16]、LBT-RRT[17]等.除此之外，李玮等[18]通过使用启发式随机路径近似方法来规划机械臂的运动轨迹.Cao等[19]在RRT算法中加入了目标重力的思想，并且采用遗传算法对路径进行平滑处理等优化，能够缩短路径长度，并通过六自由度机械臂的拾取仿真实验验证了该方法的有效性.Huh等[20]提出了基于高斯混合模型的RRT算法，利用高斯混合模型部分随机抽样，大大减少了碰撞检测的次数，可以有效地提高碰撞检测的效率.Zhang等[21]通过在RRT中引入回归机制来减少在配置空间中的过度搜索，并且使用自适应扩展机制避免在笛卡儿空间中不必要的迭代，规划效率得到了显著的提高.

根据以上研究成果，为提高机械臂的运动规划效率，本文提出了基于均匀概率的目标启发式RRT(target heuristic RRT based on uniform probability, PH-RRT)机械臂运动规划方法.一方面，通过对RRT路径规划方法加入均匀概率分配机制，改善传统RRT在冗余空间中的盲目扩展，并基于目标偏差和目标重力的目标启发式节点拓展方法，提高了路径规划的探索效率.另一方面，针对所规划出来的运动路径，引入广度优先搜索的思想进行平滑处理.仿真实验表明，本文提出的方法可以有效地减少路径规划的路径长度和规划时间.

1 路径规划算法

1.1 基本RRT路径规划

RRT路径规划方法广泛应用于机械臂的路径规划，其基于全局统一随机采样策略，不需要构建机器人环境模型，将传统规划算法中的障碍物信息映射步骤省略，可以节约计算成本.在配置空间中，它从起点开始逐渐扩展形成搜索树，直到目标点成为搜索树的一部分或达到搜索树的设置阈值.只要给定足够的搜索时间，基本RRT方法就可以完成路径规划任务.基本RRT方法保证了扩展随机树的全部概率，具有很好的全局搜索性，但是全局随机抽样方法也带来了随机性和盲目性等局限，并且基本RRT方法很难在路径规划过程中获得更好的路径质量.随着维度的进一步增加，这些缺点将会更加明显.图1所示为基本RRT算法扩展方式.

图1 基本RRT示意图

具体扩展过程描述如下.

首先，将状态空间中的起始状态点qstart作为起始节点.然后，根据全局统一的随机数在空间中生成随机采样点qrand，并在生成的搜索树中找到距离随机采样点qrand最近的节点qnear，通过一定的搜索扩展步长在qrand和qnear之间建立新的节点qnew.此时，将检测qnew是否与障碍物碰撞：如果发生碰撞，则将其舍弃；如果没有发生碰撞，则将qnew添加到搜索树中.检查qnew到目标节点的距离是否达到设定的阈值：如果达到设置的阈值，则找到从起始节点到目标节点的路径；如果未达到设置的阈值，则重复进行上述搜索过程，直到搜索树的节点与目标节点之间的距离小于设置的阈值或最大样本数达到上限为止.基本RRT方法的伪代码如算法1所示.

算法1基本RRT扩展方法

Tree.init(qstart)

for(Dis(qnew,qgoal)> Disset)&&∈[1,N]do

qrand← RANDOM_SAMPLE_POINT(n);

qnear← NEAREST(qrand, Tree);

qnew← Tree_EXTENDqrand, step,qnear);

if CollisionFree(qnear,qnew)then

Tree.add.point(qnew);

end if

if(‖qnew-qgoal‖

return Tree;

end if

end for

return False;

1.2 PH-RRT路径规划

基本RRT方法采用的是基于全局统一的随机采样策略，即扩展过程中随机采样点在工作空间中完全随机产生，从而导致了较大的随机性和盲目性.为解决以上问题并提高路径规划效率，提出了一种PH-RRT路径规划.

首先，为保证RRT原本具有的全局搜索性的同时减小在随机点采样过程中的盲目性，通过引入基于均匀概率的分配机制，即在随机点生成过程中，设置一个处于0～1的阈值作为采样阈值t，随后随机生成0～1随机采样值n，将随机采样值n和采样阈值t进行比较，根据不同情况采用不同的随机点采样策略.当随机采样值n在0和采样阈值t之间时，将随机点选作目标节点，即qrand=qgoal，以提高随机树探索效率.当随机采样值n在采样阈值t和1之间，仍采用随机采样策略，即在工作空间中随机生成随机点，在该概率区间内保留了基本RRT的全局搜索性.采样阈值t越大表示目标导向性越大，但为了保证较好的全局搜索性和路径规划效果，t的值不宜过大，一般处于0.15～0.30.

然后，在均匀概率的分配机制的采样策略的基础上，当n处于[0，t]的概率区间内时，新节点扩展方式为

(1)

式中qnear为随机搜索树中离目标点最近的节点,在qnear的基础上向着qgoal的方向以特定步长ρ1进行新节点扩展.

当n处于[t，1]的概率区间内时,随机产生采样节点qrand,为保证在该概率区间内进一步提升探索效率,将目标点也融入节点扩展的过程中,体现为随机树扩展方向受到目标点影响,目标点会启发扩展方向向目标点靠近,可理解为目标点对扩展方向具有启发式的重力影响.具体扩展过程如图2所示.

图2 目标重力下的目标启发式节点扩展

具体新节点扩展方式为

qnew=qnear+X1+X2(t≤n<1)

(2)

式中X1、X2分别代表在目标重力影响和随机点影响下的扩展分量，公式为

(3)

(4)

式中：qnear为随机搜索树中与随机点距离最近的节点；ρ2、ρ3分别为目标点和随机点对新节点扩展的分量步长，两者的值越大，代表新节点扩展步长越大，ρ2/ρ3越大代表目标点对节点扩展的目标启发影响越大.

基于均匀概率的目标启发式RRT路径规划的伪代码如算法2所示.

算法2PH-RRT算法伪代码

Tree.init(qstart);

t← SET VALUE OF CRITICAL POINT(0, 1);

for(Dis(qnew,qgoal)>Disset)&&n∈[1,N]do

qrand← RANDOM_SAMPLE_POINT(n);

qnear← NEAREST(qnew, Tree);

qnew← Tree_EXTEND(qnew, step,qnear);

k← RANDOM VALUE(0,1);

if(0<=k<=t)then

qrand=qgoal;

qnew=qnear+ρ(qgoal-qnear/‖qgoal-qnear‖);

end if

else if(t<=k<=1)then

qnew=qnear+X1+X2;

end if

if CollisionFree(qnear,qnew)then

Tree.add.point(qnew);

end if

if(‖qnew-qgoal‖

return Tree;

end if

end for

pathsmooth;

return False;

2 路径优化处理

尽管在均匀概率下的目标偏差和目标重力的作用下可以有效地减少冗余空间中的搜索，但生成的路径中的路径点仍然存在许多冗余点，从而导致机械臂轨迹不平滑，使路径规划的距离、时间和机械臂的能耗也相应增加.因此，在保持原始曲线特性的同时，有必要对轨迹曲线进一步优化.

这里通过引入广度优先搜索的思想来优化路径，如图3所示.其中：p1是公共路径段；p2～p4是不同的优化路径可能性.

图3 路径优化处理示意图

具体优化过程如下：首先，将路径中的所有节点相互连接，并删除与障碍冲突的路径，并且计算节点之间的路径长度作为权重，并记录直接连接到起点的路径的最小权重点i.然后，更新并比较起点的最小权重和最小权重点i的连接点，并循环扩展，直到目标点成为最小权重点.根据该优化方法对图3中路径进行优化可得到优化路径为p1+p3.

3 仿真实验

3.1 二维空间下仿真对比实验

首先，在二维空间进行仿真对比实验，地图范围500×500，随机采样点均在地图内部，内部设置一定障碍物，起始节点坐标为(10,10)，目标节点坐标为(490,490).本实验中针对基本RRT、RRT-Connect与PH-RRT方法分别进行25次实验.其中基本RRT、RRT-Connect方法每步的拓展步长均为15.PH-RRT实验中为保证较好的全局搜索性和路径规划效果，随机阈值t=0.2，为保证实验结果的可比性，取ρ1=15，为保证在目标启发式作用下扩展步长适宜并且使得新节点扩展更倾向于目标点，取ρ2=10，ρ3=20，即ρ2/ρ3=1/2.各方法在采样期间的最大采样数均为10 000.图4分别为3种方法的路径规划和路径优化处理后的一次实验结果.

图4 二维环境下路径规划及路径优化仿真实验对比

如图4所示，在二维空间中可以明显地看出PH-RRT方法能够有效减少在冗余空间中的搜索，大量减少了无用节点的生成，提高了路径规划效率，节省了路径规划时间，并且路径优化算法也能有效地优化路径，提升了路径平滑度并缩短了路径长度.

针对25次实验结果，对3种方法的路径规划时间和所规划出的路径长度进行数据采集，结果如图5、6所示.

图5 路径规划时间对比

图6 路径长度对比

在路径规划时间方面，因为基本RRT方法随机性高，稳定性差，其平均路径规划时间为15.70 s.PH-RRT和RRT-Connect方法在运动路径规划时间上均有一定的改善，PH-RRT的平均路径规划时间为7.38 s，RRT-Connect的平均路径规划时间为8.59 s，由此可见，PH-RRT方法的探索效率高于双向探索的RRT-Connect方法.通过以上数据可以得出，二维空间环境下PH-RRT方法相比于其他2种RRT方法，路径规划时间均有所改善，能够提高路径规划效率，减少规划时间.在规划的路径长度方面，基本RRT的平均路径长度为1 087.4，RRT-Connect平均路径长度为1 003.5，PH-RRT的平均路径长度为851.6.通过以上数据可以看出，PH-RRT因为引入了目标偏差和目标重力的目标启发式思想使得扩展节点更加接近目标点，从而能够得到质量更好的路径.

采用基于广度优先搜索的路径优化方法针对PH-RRT方法所规划出的路径的25次优化结果如图7所示.

图7 优化路径与原路径长度对比

如图7所示，PH-RRT的优化前平均路径长度为851.6，路径平滑后的平均路径长度为754.9，缩短了12.81%.相比于未优化的原路径，经过路径优化处理后大大减少了路径的冗余点，提高了路径的平滑度.

3.2 三维空间下仿真对比实验

3.2.1 三维空间环境下的路径规划仿真对比实验

在二维空间下的仿真对比实验验证了PH-RRT方法在二维空间中的有效性及可靠性，但机械臂工作空间为三维空间，随着空间维度增加计算量和随机性也进一步增加，需要在三维空间中进行仿真对比实验以验证该方法针对机械臂的轨迹规划具有有效性.

为了验证三维空间中规划的有效性及可靠性，本实验设置了2个空间大小为100×100×100的三维地图.一个是无障碍场景地图，在无障碍物情况下，可以明显观察出三维空间中探索随机树的拓展情况；另一个是有9个均匀分布于空间的半径为15的球形作为障碍物的障碍场景地图，用于验证该方法可以在避障的同时完成轨迹规划任务.

本实验中，随机采样点均处于三维空间内部，起始节点坐标为(5, 5, 5)，目标节点坐标为(95, 95, 95).针对基本RRT、RRT-Connect与PH-RRT三种方法分别进行25次仿真实验，其中基本RRT、RRT-Connect方法每步的拓展步长均为5，PH-RRT中相关参数取t=0.2，为保证实验结果的可比性，取ρ1=5.为保证在目标启发式作用下扩展步长适宜并且使得新节点扩展更倾向于目标点，取ρ2=4，ρ3=8，即ρ2/ρ3=1/2.各方法在采样期间的最大采样数均为10 000.图8～10分别为3种方法在无障碍场景下和有障碍场景下的一次实验.

图8 三维空间下基本RRT路径规划

由图8～10对比可以明显得出，在无障碍场景下基本RRT方法的扩展节点几乎遍布整个工作空间，难以在短时间内进行有效的轨迹规划，在三维空间中基本RRT方法盲目探索的随机性和计算冗余性随着维度的提高进一步放大.RRT-Connect方法因为探索空间维度的增加，双向探索的2棵随机探索树相连接的可能性减少，该方法在三维空间中探索效率不如二维空间中的实验结果，而PH-RRT方法就节点数和路径质量而言仍然具有较好的路径规划效果.

为了更加准确客观地评价3种RRT方法在三维空间中的路径规划的性能指标差别，分别记录3种RRT方法在有障碍场景下的25次实验中的平均节点总数、平均路径节点总数、平均路径长度和平均规划时间等数据，结果如表1所示.

图9 三维空间下RRT-Connect路径规划

图10 三维空间下PH-RRT路径规划

表1 有障碍场景下各算法性能指标对比

由实验结果可以得到，在引入了均匀概率原则的分配机制，并且融入了目标偏差和目标力的目标启发式策略后，PH-RRT方法在三维空间搜索有效空间方面显著提升，生成节点总数远远小于基本RRT算法，相较于RRT-Connect算法也有较大提升，在探索过程中生成的总节点数减少会使得探索过程中计算量减少，因此，PH-RRT的平均路径规划时间也明显缩短，相对于其他2种方法具有更好的路径规划效率.此外，PH-RRT方法所规划出的路径的平均路径节点总数和平均路径长度相比于其他2种方法也均明显减小，验证了PH-RRT方法在三维空间中得到的路径质量优于其他传统方法，具有较好的有效性和可靠性.

3.2.2 三维空间环境下的路径规划仿真对比实验

三维空间中PH-RRT路径规划方法可以有效地减少冗余空间中的搜索，但生成的路径点中仍然存在许多冗余点.为验证三维空间中路径优化处理仍然可以进一步优化路径，分别在无障碍场景地图和有障碍场景地图下，针对已通过PH-RRT方法进行路径规划得到的路径进行基于广度优先搜索思想的路径平滑处理方法的仿真实验，实验结果如图11、12所示.

图11 无障碍场景下路径平滑处理仿真实验

图12 有障碍场景下路径平滑处理仿真实验

图中，黑线表示规划出的路径，红线表示优化后路径.在无障碍场景下优化后路径为连接起始点和目标点的直线路径，优化效果明显.在有障碍场景下，处理后路径拐点明显减少，可以有效提高路径平滑度，并且原路径经过路径优化处理后的路径长度明显缩短.有障碍场景下优化前后路径长度如图13所示.

图13 三维空间中优化路径与原路径长度对比

PH-RRT路径规划方法规划出的平均路径长度为196.5，经过路径平滑后的平均路径长度为178.4.原路径经过路径优化处理后，路径长度缩短了9.21%，在三维空间中路径优化处理仍然能够较好地减少路径的冗余点，提高路径的平滑度.

3.3 机器人操作系统(robot operating system，ROS)环境下机械臂仿真实验

为进一步验证本文提出的方法能够很好地适用于机械臂的无碰撞路径规划，安全到达目标点，实验在ROS的仿真环境中对机械臂的不同规划路径方法的规划效果进行了对比分析.

实验中机械臂采用自行设计的仿人移动机器人的仿生机械臂，实验环境为ROS下搭建的仿真环境，机械臂的工作任务为模拟机械臂翻越桌面的任务.实验中自行设计的仿人移动机器人的三维单臂模型如图14所示，其中机器人手臂的所有关节均为旋转关节，右侧是七自由度机械臂的连杆坐标系，其中关节7用于连接机械臂的末端执行器.

图14 仿人机械手的三维模型及连杆坐标系

选取机械臂的空间起始点为(-0.331 8,-0.340 2,-0.340 3, 0.005 9, 0.960 2, 0.250 3,-0.124 1)、机械臂的空间终止点为(-0.429 6,-0.396 3,-0.024 3, 0.330 0, 0.925 7, 0.094 55,-0.158 4)，在同一种起始构型与终止构型情况下对基本RRT、RRT-Connect、PH-RRT运动规划方法进行实验，实验结果分别如图15所示.

图15 ROS环境下仿真实验

图15中，蓝色线表示该方法优化前的轨迹路径，红色线表示该方法优化后的轨迹路径.由实验结果可见，本文的PH-RRT路径规划方法应用于机械臂的效果明显优于传统方法.PH-RRT方法有效地规划出了可行性路径，并使得路径平滑度得到改善，路径长度进一步缩短.

4 结论

1)针对高维空间的机械臂进行路径规划及其优化研究是机械臂性能提升的关键之一，对基本RRT路径规划方法加入均匀概率分配机制和目标偏差与目标重力的目标启发式策略，并引入广度优先搜索思想，对路径进行优化.通过在二维空间、三维空间环境下与传统的RRT、RRT-Connect方法进行对比实验，证明了该方法在减少路径长度与提高路径平滑度方面的有效性.

2)通过基于ROS对所建立的机械臂模型进行相同起始点与终止点的机械臂仿真实验，结果表明，机械臂的运动规划路径长度与平滑度得到明显改善，在机械臂上具有较好的适用性.