CN113799141A - 六自由度机械臂避障路径规划方法 - Google Patents

Abstract

本发明提出一种六自由度机械臂避障路径规划方法，在已知起始位置，目标位置和障碍物信息的情况下，利用双树结构探索空间，辅以目标偏置和metropolis接受准则优化随机采样点的生成，加快算法收敛于目标位置，采用动态步长，根据节点扩展情况来评判节点周围障碍物情况，依据障碍物多少动态调节节点扩展步长，使其适应不同的障碍物环境，并采用势力场修正策略，加快算法远离起始点，收敛目标位置，有效提高了算法避障路径规划的实时性，最后用重选父节点优化路径节点，减少路径长度。前述设计的路径规划算法可以很好的适用于六自由度机械臂路径规划问题，在不同的障碍物环境中都可以完成路径规划任务。

Description

六自由度机械臂避障路径规划方法

技术领域

本发明属于机械臂运动规划技术领域，涉及一种六自由度机械臂避障路径规划方法，包括一种用于六自由度机械臂在复杂障碍物环境下避障路径规划的改进RRT-Connect算法。。

背景技术

机械臂应用是现代工业自动化、智能化的一个重要体现，而作为机械臂应用过程中所要解决的一个重要问题——路径规划问题一直颇受人们的重视。机械臂路径规划方法的好坏直接影响机械臂的整体工作性能及效率。

目前，解决机械臂路径规划问题的常用的方法有：A^*算法、人工势场法、快速随机树算法、RRT-Connect算法、RRT*算法等。其中，A^*算法和人工势场法主要在移动机器人路径规划中具有较好的表现，但随着维度的增加，空间环境复杂度的提升将导致前述算法的计算量陡增，并可能会导致算法陷入局部极小值问题。基于采样的快速随机树算法虽可减少障碍物描述以及用搜索树来探索工作空间，适用于不同维度的机械臂路径规划问题，但该算法采取完全随机的采样方式来扩展搜索树新节点，算法的实时性和路径质量无法得到良好地保证。传统RRT-Connect(双向快速扩展随机树)算法在起点和终点分别构建搜索树，通过两棵搜索树的交互扩展来搜索可行路径，极大地加快了算法速度，但RRT-Connect算法的探索方向依旧是随机的，且路径质量也没有明显得到改善。RRT*针对路径质量优化问题提出了重选父节点和重布线随机树，在节点扩展的过程中选择代价小的父节点和重新布线来减小路径代价，因此RRT*算法在有限的迭代次数中可以获取相对较优的路径，但随着搜索树扩展，节点数增加，算法计算量会随之增大，算法的收敛时间成了突出问题。因此，研究在复杂障碍物环境下良好实现六自由度机械臂避障路径规划问题是具有十分重要的现实意义的。

发明内容

针对现有技术存在的缺陷和不足，本发明的目的在于提供一种六自由度机械臂避障路径规划方法，与传统RRT-Connect算法相比，该方案在RRT-Connect算法的基础上，将目标偏置策略、metropolis接受准则、势力场修正策略、基于动态步长的双树扩展策略及重选父节点策略进行有机融合，不仅可有效提高避障路径规划的实时性，而且还可对路径的规划质量进行必要地改善。

本发明在已知起始位置，目标位置和障碍物信息的情况下，利用双树结构探索空间，辅以目标偏置和metropolis接受准则优化随机采样点的生成，加快算法收敛于目标位置，采用动态步长，根据节点扩展情况来评判节点周围障碍物情况，依据障碍物多少动态调节节点扩展步长，使其适应不同的障碍物环境，并采用势力场修正策略，加快算法远离起始点，收敛目标位置，有效提高了算法避障路径规划的实时性，最后用重选父节点优化路径节点，减少路径长度。前述设计的路径规划算法可以很好的适用于六自由度机械臂路径规划问题，在不同的障碍物环境中都可以完成路径规划任务。

本发明具体采用以下技术方案：

一种六自由度机械臂避障路径规划方法，其特征在于：在已知起始位置，目标位置和障碍物信息的情况下，利用双树结构探索空间，辅以目标偏置和metropolis接受准则优化随机采样点的生成，以加快算法收敛于目标位置；并依据势力场修正策略，调整节点扩展位置，加快算法远离起始点，收敛目标位置；采用动态步长，根据节点扩展情况评判节点周围障碍物情况，依据障碍物多少动态调节节点扩展步长，使其适应不同的障碍物环境；最后采用重选父节点优化路径节点，减少路径长度。

若对算法的操作具体化，则包括了以下步骤：

步骤S1：设六自由度机械臂末端操作器在系统基坐标系O-xyz下的位置矢量为P＝(x,y,z)^T；其末端操作器连体坐标系O_e-x_ey_ez_e的姿态通过基坐标系O-xyz进行Z-Y-X欧拉角q＝(γ,β,α)^T旋转操作确定，即以基坐标系O-xyz为起始坐标系，先绕基坐标系O-xyz的z轴旋转α角，获得坐标系O'-x'y'z'；再绕旋转后的坐标系O'-x'y'z'的y'轴旋转β角，获得坐标系O”-x”y”z”；最后绕坐标系O”-x”y”z”的x”轴旋转γ角，最终确定出机械臂末端操作器的连体坐标系O_e-x_ey_ez_e；由此，将机械臂末端操作器位姿相对于系统基坐标系O-xyz的齐次变换矩阵

描述为：

其中，

为末端操作器连体坐标系O_e-x_ey_ez_e姿态相对于基坐标系O-xyz的旋转变换矩阵，O＝(0,0,0)为零向量，

为机械臂连杆i连体坐标系相对于连杆i-1连体坐标系的齐次变换矩阵，θ_i表示机械臂连杆i相对于连杆i-1的关节转角；

利用上式及反变换法对系统进行运动学反解，可求得机械臂各连杆关节角θ_i(i＝1,2,…,6)；

利用符号

表示机械臂在位置*时的系统节点；其中，

表示机械臂各连杆在位置*时所对应的关节角；将系统起始位置节点用符号Q_init表示，目标位置节点用Q_goal表示，相应的以机械臂关节角行程作为系统的路径代价度量；

在此基础上，获取包括尺寸、形状、起始位置Q_init、目标位置Q_goal的机械臂信息及包括障碍物的尺寸、形状及位置的机械臂工作环境信息；

步骤S2：以机械臂起始位置Q_init作为根节点构建起始搜索树StartT，以机械臂目标位置Q_goal作为根节点构建目标搜索树GoalT；所述起始搜索树是一个以Q_init为根节点且可以不断扩展的节点集motionS；所述目标搜索树是一个以Q_goal为根节点且可以不断扩展的节点集motionG；起始搜索树的扩展步长StartT_step和目标搜索树的扩展步长GoalT_step初始时都设为相同的步长值init_step；

步骤S3：采用目标偏置和metropolis接受准则在工作空间筛选随机采样点Q_rand；设rand为在[0,1]内随机生成的数值，P_goal为目标偏置概率；若rand＜P_goal，则直接将目标点Q_goal作为随机采样点Q_rand；若rand≥P_goal，则采用metropolis接受准则来筛选随机采样点Q_rand，即若采样点满足P＝1，则接受此采样点，继续执行步骤S4；若P＝0，则此采样点不被算法接受，重新进行随机采样，直至采样点满足P＝1的情况；其中，metropolis接受准则如下：

其中，

Q_nearest表示起始搜索树StartT中距离采样点Q_rand最近的节点，Q_end表示起始搜索树StartT上一次扩展时节点集motionS添加的节点，符号d(Q_A,Q_B)表示节点A和节点B的路径代价，即

λ为采样点接受范围调节参数；λ值越大，则采样点接受范围越小；λ值越小，则采样点接受范围越大；

步骤S4：在起始搜索树StartT上搜寻节点集motionS中距离Q_rand最近的节点Q_nearest；设起始搜索树StartT的步长为StartT_step，若d(Q_nearest,Q_rand)≥StartT_step，

则依据：

扩展出新节点Q_{new_0}；若d(Q_nearest,Q_rand)＜StartT_step，则节点扩展失败，执行步骤S9；

步骤S5：新节点Q_{new_0}初步扩展成功后，考虑新节点Q_{new_0}受起始节点斥力和目标节点引力影响，对新扩展节点Q_{new_0}的位置作如下势力场修正：

其中，Q_new为势力场修正后的新节点，b为斥力调节参数，c为引力调节参数，且b、c分别满足：

步骤S6：对新节点Q_new重新选择父节点Q_Parent；设radius＝k_rrg_*log(cardDbl)；k_rrg_＝e+e/dimDbl，cardDbl＝StartT.size+1；其中，StartT.size表示当前StartT的节点数，e是自然对数的底，dimDbl是关节空间维度；之后以Q_new为圆心，radius为半径在起始搜索树StartT上搜寻半径radius范围内的节点，并逐个将其记为Q_nears[i]，i＝1,2，…，p；p表示满足搜索条件的节点数；将Q_nears[i]依次作为Q_new的备选父节点，计算从Q_init途径Q_nears[i]并运动到Q_new的路径总代价：

Q_new[i].cost＝Q_nears[i].cost+d(Q_nears[i]，Q_new)(i＝1,2，…，p)

其中，Q_near[i].cost表示Q_init运动到Q_nears[i]的路径代价；在不发生碰撞的前提下，将满足路径总代价最小即，

Q_new.cost＝min{Q_new[1].cost,Q_new[2].cost,…,Q_new[p].cost}

的备选父节点拟作为Q_new最终的父节点Q_Parent；

步骤S7：再次判断机械臂从Q_Parent运动至Q_new过程中是否发生碰撞，若不发生碰撞，则扩展成功，将Q_new加入StartT的节点集motionS中，并继续执行步骤S8；若扩展失败则跳转至步骤S9；

步骤S8：重设起始搜索树的步长StartT_step＝StartT_step+init_step且λ＝λ+1，执行步骤S10；

步骤S9：StartT_step设置为初始步长值init_step，λ＝1，返回步骤S3；

步骤S10：判断StartT，GoalT是否满足连接条件即d(Qnew,QGoalT_nearest)＝Thr；其中，Q_{GoalT_nearest}为目标搜索树GoalT中距离Q_new最近的节点，Thr为连接阈值；

若满足连接条件，则起始搜索树的根节点到目标搜索树的根节点之间经过的全部节点即为规划的路径；若不满足连接条件，则切换至目标搜索树GoalT开始进行反向空间搜索；执行步骤S11；

步骤S11：目标搜索树GoalT以Q_new为反向目标扩展方向，扩展步长设置为GoalT_step，若搜索树GoalT向Q_new反向扩展时并未遇到障碍物且d(Q_{GoalT_nearest},Q_new)≥GoalT_step时，依据：

GoalT_step＝GoalT_step+init_step

拓展反向新节点Q_{GoalT_new}，并将Q_{GoalT_new}加入到GoalT的节点集motionG中；其中，Q_{GoalT_nearest}表示目标搜索树GoalT的节点集motionG中距离Q_new最近的节点；

之后，目标搜索树GoalT继续以Q_new为扩展方向扩展；若目标搜索树GoalT扩展过程中遇到障碍物或d(Q_{GoalT_nearest},Q_new)＜GoalT_step，则目标搜索树GoalT停止扩展，GoalT_step设置为初始步长值init_step后，执行步骤S12；

步骤S12：判断StartT，GoalT是否满足连接条件，即是否满足d(Q_new,Q_{GoalT_nearest})＝Thr：

若满足，则起始搜索树的根节点到目标搜索树的根节点之间经过的全部节点即为规划的路径；

若不满足，则切换至起始搜索树StartT开始搜索空间；返回执行步骤S3。

与现有技术相比，本发明及其优选方案采用改进RRT-Connect算法将目标偏置策略、势力场修正策略、基于动态步长的双树扩展策略及重选父节点策略进行有机融合，旨在有效提高六自由机械臂无障路径的规划速度及规划质量。在随机点采样环节引入目标偏置策略及基于λ参数动态调节的metropolis接受准则，可使搜索树具备向目标位置快速靠近的能力的同时，能接受远离目标点的情况出现，保留绕开障碍物的能力，以确保机械臂无障路径的快速、有效规划。新节点扩展环节中的势力场修正操作，可使路径搜索加快远离起始点并向目标点靠拢，同样提升了算法的收敛速度。基于动态步长的双树扩展策略可让算法在简单的障碍物环境中，以更快的速度收敛；在复杂的障碍物环境中，可以根据障碍物分布的不同，采用不同的步长扩展，避免因步长过大而使算法无法避开障碍物或因步长过小而使算法的收敛速度变慢。引入重选父节点策略在搜索树每次成功扩展新节点后，重新选择代价最小的父节点，通过缩短搜索树添加新节点增加的路径代价来优化路径，可以获取相对较优的路径。

附图说明

下面结合附图和具体实施方式对本发明进一步详细的说明：

图1本发明实施例中MOTOMAN-GP7型号机械臂尺寸参数图。

图2是本发明实施例中基于ROS平台机械臂仿真模型图。

图3是本发明实施例中基于改进RRT-Connect算法的路径规划总流程图。

图4是本发明实施例中采样点Q_rand选取子流程图。

图5是本发明实施例中目标树GoalT扩展子流程图。

图6是本发明实施例中机械臂路径规划效果示意图。

具体实施方式

为让本专利的特征和优点能更明显易懂，下文特举实施例，作详细说明如下：

本实施例基于ROS(Robot Operating System，机器人操作系统)平台，搭建机械臂仿真场景，选用MOTOMAN-GP7型号机械臂作为本实例研究对象，其尺寸参数和运动学参数如图1所示。

MOTOMAN-GP7型号机械臂运动学参数如下表1所示：

以机械臂基座中心构建笛卡尔基坐标系O-xyz，障碍物1的中心位置设为(0.6m,0.4m,0.25m)、障碍物2的中心位置设为(0.6m,0m,0.25m)、障碍物3的中心位置设为(0.6m,-0.4m,0.25m)、障碍物4的中心位置设为(0.6m,0.3m,0.6m)、障碍物5的中心位置设为(0.6m,0.3m,0.1m)、障碍物6的中心位置设为(0.6m,-0.3m,0.6m)、障碍物7的中心位置设为(0.6m,-0.3m,0.1m)；图示长方体障碍物大小均相同，长度0.6m、宽度0.05m、高度0.5m；球体障碍物半径均为0.1m，仿真场景如图2所示。六自由度机械臂末端操作器的起始位置设为P₀＝(0.55m,0m,0.81m)^T，起始姿态以Z-Y-X欧拉角表示设为q0＝(90°，-90°，90°)^T；机械臂的第一、二个目标位置分别设为P₁＝(0.6,-0.25,0.25)^T、P₂＝(0.6,0.25,0.25)^T；机械臂在两个目标位置的末端姿态设为q₁＝q₂＝(180°,0°,180°)。在本实施例中采用改进RRT-Connect算法规划路径，机械臂将会从起始位置运动至第一个目标位置后，再运动至第二个目标位置，最终返回起始位置。

参见图3-图5，本发明的改进RRT-Connect算法的实际应用步骤如下：

步骤S1：设六自由度机械臂末端操作器在系统基坐标系O-xyz下的位置矢量为P＝(x,y,z)^T；其末端操作器连体坐标系O_e-x_ey_ez_e的姿态通过基坐标系O-xyz进行Z-Y-X欧拉角q＝(γ,β,α)^T旋转操作确定，即以基坐标系O-xyz为起始坐标系，先绕基坐标系O-xyz的z轴旋转α角，获得坐标系O'-x'y'z'；再绕旋转后的坐标系O'-x'y'z'的y'轴旋转β角，获得坐标系O”-x”y”z”；最后绕坐标系O”-x”y”z”的x”轴旋转γ角，最终确定出机械臂末端操作器的连体坐标系O_e-x_ey_ez_e。于是，机械臂末端操作器位姿相对于系统基坐标系O-xyz的齐次变换矩阵

可描述为：

其中，

为末端操作器连体坐标系O_e-x_ey_ez_e姿态相对于基坐标系O-xyz的旋转变换矩阵，O＝(0,0,0)为零向量，

为机械臂连杆i连体坐标系相对于连杆i-1连体坐标系的齐次变换矩阵，θ_i表示机械臂连杆i相对于连杆i-1的关节转角。

利用上式及反变换法对系统进行运动学反解，可求得机械臂各连杆关节角θ_i(i＝1,2,…,6)。

为便于后续路径规划改进RRT-Connect算法的设计，利用符号

表示机械臂在位置*时的系统节点；其中，

表示机械臂各连杆在位置*时所对应的关节角。于是，系统起始位置节点可用符号Q_init表示，目标位置节点可用Q_goal表示，相应的以机械臂关节角行程作为系统的路径代价度量。

以上述为基础，获取机械臂信息(包括尺寸、形状、起始位置Q_init、目标位置Q_goal)及其工作环境信息(包括障碍物的尺寸、形状及位置)。

步骤S2：以机械臂起始节点Q_init(即起始位置)作为根节点构建起始搜索树StartT，以机械臂目标节点Q_goal(即目标位置)作为根节点构建目标搜索树GoalT。注意所述起始搜索树实际上是一个以Q_init为根节点且可以不断扩展的节点集motionS；所述目标搜索树实际上是一个以Q_goal为根节点且可以不断扩展的节点集motionG。本实施例中依据机械臂参数设置初始步长值init_step设为7.614，起始搜索树StartT的步长StartT_step和目标搜索树GoalT的步长GoalT_step在初始时都设置为init_step，即StartT_step＝GoalT_step＝init_step＝7.614；

步骤S3：采用目标偏置和metropolis接受准则在工作空间中筛选随机采样点Q_rand，本实施例中目标偏置概率P_goal设置为0.05，rand为在[0,1]内随机生成的数值；若rand＜P_goal，随机采样点Q_rand为目标点Q_goal，若rand≥P_goal，则采用metropolis接受准则来筛选随机采样点Q_rand，若采样点满足P＝1，则接受此采样点，继续执行步骤S4；若P＝0，则此采样点不被算法接受，重新进行随机采样，直至采样点满足P＝1的情况；metropolis接受准则如下：

其中，

Q_nearest表示起始搜索树StartT中距离采样点Q_rand最近的节点，Q_end表示起始搜索树StartT上一次扩展时节点集motionS中最新添加的节点，符号d(Q_A,Q_B)表示节点位置A和节点位置B的路径代价，即

λ为采样点接受范围调节参数；λ值越大，则采样点接受范围越小；λ值越小，则采样点接受范围越大。本实施例中λ初始值设为2；

步骤S4：在起始搜索树StartT上搜寻节点集motionS中距离Q_rand最近的节点Q_nearest；设起始搜索树StartT的步长为StartT_step，若d(Q_nearest,Q_rand)≥StartT_step，则依据：

扩展出新节点Q_{new_0}；若d(Q_nearest,Q_rand)＜StartT_step，则节点扩展失败，执行步骤S9；

步骤S5：新节点Q_{new_0}扩展成功后，考虑新节点Q_{new_0}受起始节点斥力和目标节点引力影响，对新扩展节点Q_{new_0}的位置作如下势力场修正：

其中，Q_new表示为势力场修正后的新节点，b为斥力调节参数，c为引力调节参数，且b、c分别满足：

步骤S6：对新节点Q_new重新选择父节点Q_Parent。设radius＝k_rrg_*log(cardDbl)；k_rrg_＝e+e/dimDbl，cardDbl＝StartT.size+1；StartT.size表示当前StartT的节点数，e是自然对数的底，dimDbl是关节空间维度；本实施例中dimDbl＝6。之后以Q_new为圆心，radius为半径在起始搜索树StartT上搜寻半径radius范围内的节点，并逐个将其记为Q_nears[i](i＝1,2，…，p；p表示满足搜索条件的节点数)；将Q_nears[i]依次作为Q_new的备选父节点，计算从Q_init途径Q_nears[i]并运动到Q_new的路径距离总代价：

Q_new[i].cost＝Q_nears[i].cost+d(Q_nears[i]，Q_new)(i＝1,2，…，p)

其中，Q_near[i].cost表示Q_init运动到Q_nears[i]的路径距离代价。在不发生碰撞的前提下，将满足路径总代价最小即：

Q_new.cost＝min{Q_new[1].cost,Q_new[2].cost,…,Q_new[p].cost}的备选父节点拟作为Q_new最终的父节点Q_Parent。

步骤S7：再次判断机械臂从Q_Parent运动至Q_new过程中是否发生碰撞，若不发生碰撞，则扩展成功，将Q_new加入StartT的节点集motionS中，并继续执行步骤S8；若扩展失败则跳转至步骤S9；

步骤S8：起始搜索树的步长StartT_step＝StartT_step+init_step且λ＝λ+1，执行步骤S10；

步骤S9：StartT_step设置为初始步长值init_step，λ＝1，返回步骤S3；

步骤S10：判断StartT，GoalT是否满足连接条件即d(Qnew,QGoalT_nearest)＝Thr？其中，Q_{GoalT_nearest}为目标搜索树GoalT中距离Q_new最近的节点，Thr为连接阈值；在本实施例中Thr＝0；

若满足连接条件，则起始搜索树的根节点到目标搜索树的根节点之间经过的全部节点即为规划的路径；若不满足连接条件，则切换至目标搜索树GoalT开始进行反向空间搜索；执行步骤S11；

步骤S11：目标搜索树GoalT以Q_new为反向目标扩展方向，扩展步长设置为GoalT_step，若搜索树GoalT向Q_new反向扩展时并未遇到障碍物且d(Q_{GoalT_nearest},Q_new)≥GoalT_step时，依据

GoalT_step＝GoalT_step+init_step

拓展反向新节点Q_{GoalT_new}，并将Q_{GoalT_new}加入到GoalT的节点集motionG中。其中，Q_{GoalT_nearest}表示目标搜索树GoalT的节点集motionG中距离Q_new最近的节点；

之后，目标搜索树GoalT继续以Q_new为扩展方向扩展；若目标搜索树GoalT扩展过程中遇到障碍物或d(Q_{GoalT_nearest},Q_new)＜GoalT_step，则目标搜索树GoalT停止扩展，GoalT_step设置为初始步长值init_step后，执行步骤S12；

步骤S12：判断StartT，GoalT是否满足连接条件，即是否满足d(Q_new,Q_{GoalT_nearest})＝Thr？

若能够连接，则起始搜索树的根节点到目标搜索树的根节点之间经过的全部节点即为规划的路径；图6即为机械臂路径规划效果示意图。

若不能连接，则交换至起始搜索树StartT开始搜索空间；返回执行步骤S3。

本专利不局限于上述最佳实施方式，任何人在本专利的启示下都可以得出其它各种形式的六自由度机械臂避障路径规划方法，凡依本发明申请专利范围所做的均等变化与修饰，皆应属本专利的涵盖范围。

Claims (2)

1.一种六自由度机械臂避障路径规划方法，其特征在于：在已知起始位置，目标位置和障碍物信息的情况下，利用双树结构探索空间，辅以目标偏置和metropolis接受准则优化随机采样点的生成，以加快算法收敛于目标位置；并依据势力场修正策略，调整节点扩展位置，加快算法远离起始点，收敛目标位置；采用动态步长，根据节点扩展情况评判节点周围障碍物情况，依据障碍物多少动态调节节点扩展步长，使其适应不同的障碍物环境；最后采用重选父节点优化路径节点，减少路径长度。

2.根据权利要求1所述的六自由度机械臂避障路径规划方法，其特征在于，包括以下步骤：

步骤S1：设六自由度机械臂末端操作器在系统基坐标系O-xyz下的位置矢量为P＝(x,y,z)^T；其末端操作器连体坐标系O_e-x_ey_ez_e的姿态通过基坐标系O-xyz进行Z-Y-X欧拉角q＝(γ,β,α)^T旋转操作确定，即以基坐标系O-xyz为起始坐标系，先绕基坐标系O-xyz的z轴旋转α角，获得坐标系O'-x'y'z'；再绕旋转后的坐标系O'-x'y'z'的y'轴旋转β角，获得坐标系O”-x”y”z”；最后绕坐标系O”-x”y”z”的x”轴旋转γ角，最终确定出机械臂末端操作器的连体坐标系O_e-x_ey_ez_e；由此，将机械臂末端操作器位姿相对于系统基坐标系O-xyz的齐次变换矩阵

描述为：

其中，

为末端操作器连体坐标系O_e-x_ey_ez_e姿态相对于基坐标系O-xyz的旋转变换矩阵，O＝(0,0,0)为零向量，

为机械臂连杆i连体坐标系相对于连杆i-1连体坐标系的齐次变换矩阵，θ_i表示机械臂连杆i相对于连杆i-1的关节转角；

利用上式及反变换法对系统进行运动学反解，可求得机械臂各连杆关节角θ_i(i＝1,2,…,6)；

利用符号

表示机械臂在位置*时的系统节点；其中，

表示机械臂各连杆在位置*时所对应的关节角；将系统起始位置节点用符号Q_init表示，目标位置节点用Q_goal表示，相应的以机械臂关节角行程作为系统的路径代价度量；

在此基础上，获取包括尺寸、形状、起始位置Q_init、目标位置Q_goal的机械臂信息及包括障碍物的尺寸、形状及位置的机械臂工作环境信息；

步骤S2：以机械臂起始位置Q_init作为根节点构建起始搜索树StartT，以机械臂目标位置Q_goal作为根节点构建目标搜索树GoalT；所述起始搜索树是一个以Q_init为根节点且可以不断扩展的节点集motionS；所述目标搜索树是一个以Q_goal为根节点且可以不断扩展的节点集motionG；起始搜索树的扩展步长StartT_step和目标搜索树的扩展步长GoalT_step初始时都设为相同的步长值init_step；

步骤S3：采用目标偏置和metropolis接受准则在工作空间筛选随机采样点Q_rand；设rand为在[0,1]内随机生成的数值，P_goal为目标偏置概率；若rand＜P_goal，则直接将目标点Q_goal作为随机采样点Q_rand；若rand≥P_goal，则采用metropolis接受准则来筛选随机采样点Q_rand，即若采样点满足P＝1，则接受此采样点，继续执行步骤S4；若P＝0，则此采样点不被算法接受，重新进行随机采样，直至采样点满足P＝1的情况；其中，metropolis接受准则如下：

其中，

Q_nearest表示起始搜索树StartT中距离采样点Q_rand最近的节点，Q_end表示起始搜索树StartT上一次扩展时节点集motionS添加的节点，符号d(Q_A,Q_B)表示节点A和节点B的路径代价，即

λ为采样点接受范围调节参数；λ值越大，则采样点接受范围越小；λ值越小，则采样点接受范围越大；

步骤S4：在起始搜索树StartT上搜寻节点集motionS中距离Q_rand最近的节点Q_nearest；设起始搜索树StartT的步长为StartT_step，若d(Q_nearest,Q_rand)≥StartT_step，

则依据：

扩展出新节点Q_{new_0}；若d(Q_nearest,Q_rand)＜StartT_step，则节点扩展失败，执行步骤S9；

步骤S5：新节点Q_{new_0}初步扩展成功后，考虑新节点Q_{new_0}受起始节点斥力和目标节点引力影响，对新扩展节点Q_{new_0}的位置作如下势力场修正：

其中，Q_new为势力场修正后的新节点，b为斥力调节参数，c为引力调节参数，且b、c分别满足：

步骤S6：对新节点Q_new重新选择父节点Q_Parent；设radius＝k_rrg_*log(cardDbl)；k_rrg_＝e+e/dimDbl，cardDbl＝StartT.size+1；其中，StartT.size表示当前StartT的节点数，e是自然对数的底，dimDbl是关节空间维度；之后以Q_new为圆心，radius为半径在起始搜索树StartT上搜寻半径radius范围内的节点，并逐个将其记为Q_nears[i]，i＝1,2，…，p；p表示满足搜索条件的节点数；将Q_nears[i]依次作为Q_new的备选父节点，计算从Q_init途径Q_nears[i]并运动到Q_new的路径总代价：

Q_new[i].cost＝Q_nears[i].cost+d(Q_nears[i]，Q_new)(i＝1,2，…，p)

其中，Q_near[i].cost表示Q_init运动到Q_nears[i]的路径代价；在不发生碰撞的前提下，将满足路径总代价最小即，

Q_new.cost＝min{Q_new[1].cost,Q_new[2].cost,…,Q_new[p].cost}

的备选父节点拟作为Q_new最终的父节点Q_Parent；

步骤S7：再次判断机械臂从Q_Parent运动至Q_new过程中是否发生碰撞，若不发生碰撞，则扩展成功，将Q_new加入StartT的节点集motionS中，并继续执行步骤S8；若扩展失败则跳转至步骤S9；

步骤S8：重设起始搜索树的步长StartT_step＝StartT_step+init_step且λ＝λ+1，执行步骤S10；

步骤S9：StartT_step设置为初始步长值init_step，λ＝1，返回步骤S3；

步骤S10：判断StartT，GoalT是否满足连接条件即d(Q_new,Q_{GoalT_nearest})＝Thr；其中，Q_{GoalT_nearest}为目标搜索树GoalT中距离Q_new最近的节点，Thr为连接阈值；

若满足连接条件，则起始搜索树的根节点到目标搜索树的根节点之间经过的全部节点即为规划的路径；若不满足连接条件，则切换至目标搜索树GoalT开始进行反向空间搜索；执行步骤S11；

步骤S11：目标搜索树GoalT以Q_new为反向目标扩展方向，扩展步长设置为GoalT_step，若搜索树GoalT向Q_new反向扩展时并未遇到障碍物且d(Q_{GoalT_nearest},Q_new)≥GoalT_step时，依据：

GoalT_step＝GoalT_step+init_step

拓展反向新节点Q_{GoalT_new}，并将Q_{GoalT_new}加入到GoalT的节点集motionG中；其中，Q_{GoalT_nearest}表示目标搜索树GoalT的节点集motionG中距离Q_new最近的节点；

之后，目标搜索树GoalT继续以Q_new为扩展方向扩展；若目标搜索树GoalT扩展过程中遇到障碍物或d(Q_{GoalT_nearest},Q_new)＜GoalT_step，则目标搜索树GoalT停止扩展，GoalT_step设置为初始步长值init_step后，执行步骤S12；

步骤S12：判断StartT，GoalT是否满足连接条件，即是否满足d(Q_new,Q_{GoalT_nearest})＝Thr：

若满足，则起始搜索树的根节点到目标搜索树的根节点之间经过的全部节点即为规划的路径；

若不满足，则切换至起始搜索树StartT开始搜索空间；返回执行步骤S3。

Images