发明内容

针对现有技术存在的缺陷和不足，本发明的目的在于提供一种六自由度机械臂避障路径规划方法，与传统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接受准则，可使搜索树具备向目标位置快速靠近的能力的同时，能接受远离目标点的情况出现，保留绕开障碍物的能力，以确保机械臂无障路径的快速、有效规划。新节点扩展环节中的势力场修正操作，可使路径搜索加快远离起始点并向目标点靠拢，同样提升了算法的收敛速度。基于动态步长的双树扩展策略可让算法在简单的障碍物环境中，以更快的速度收敛；在复杂的障碍物环境中，可以根据障碍物分布的不同，采用不同的步长扩展，避免因步长过大而使算法无法避开障碍物或因步长过小而使算法的收敛速度变慢。引入重选父节点策略在搜索树每次成功扩展新节点后，重新选择代价最小的父节点，通过缩短搜索树添加新节点增加的路径代价来优化路径，可以获取相对较优的路径。