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

技术领域

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

背景技术

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

目前，解决机械臂路径规划问题的常用的方法有：A

发明内容

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

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

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

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

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

步骤S1：设六自由度机械臂末端操作器在系统基坐标系O-xyz下的位置矢量为P＝(x,y,z)

其中，

利用上式及反变换法对系统进行运动学反解，可求得机械臂各连杆关节角θ

利用符号

在此基础上，获取包括尺寸、形状、起始位置Q

步骤S2：以机械臂起始位置Q

步骤S3：采用目标偏置和metropolis接受准则在工作空间筛选随机采样点Q

其中，

步骤S4：在起始搜索树StartT上搜寻节点集motionS中距离Q

则依据：

扩展出新节点Q

步骤S5：新节点Q

其中，Q

步骤S6：对新节点Q

Q

其中，Q

Q

的备选父节点拟作为Q

步骤S7：再次判断机械臂从Q

步骤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开始进行反向空间搜索；执行步骤S11；

步骤S11：目标搜索树GoalT以Q

GoalT_step＝GoalT_step+init_step

拓展反向新节点Q

之后，目标搜索树GoalT继续以Q

步骤S12：判断StartT，GoalT是否满足连接条件，即是否满足d(Q

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

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

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

附图说明

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

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

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

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

图4是本发明实施例中采样点Q

图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

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

步骤S1：设六自由度机械臂末端操作器在系统基坐标系O-xyz下的位置矢量为P＝(x,y,z)

其中，

利用上式及反变换法对系统进行运动学反解，可求得机械臂各连杆关节角θ

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

以上述为基础，获取机械臂信息(包括尺寸、形状、起始位置Q

步骤S2：以机械臂起始节点Q

步骤S3：采用目标偏置和metropolis接受准则在工作空间中筛选随机采样点Q

其中，

步骤S4：在起始搜索树StartT上搜寻节点集motionS中距离Q

扩展出新节点Q

步骤S5：新节点Q

其中，Q

步骤S6：对新节点Q

Q

其中，Q

Q

步骤S7：再次判断机械臂从Q

步骤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开始进行反向空间搜索；执行步骤S11；

步骤S11：目标搜索树GoalT以Q

GoalT_step＝GoalT_step+init_step

拓展反向新节点Q

之后，目标搜索树GoalT继续以Q

步骤S12：判断StartT，GoalT是否满足连接条件，即是否满足d(Q

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

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

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