[发明专利]七自由度仿人机械臂的双层拟人运动规划方法

权利要求书

1.七自由度仿人机械臂的双层拟人运动规划方法，所述方法包括第一层运动规划和第二层运动规划，第一层运动规划为仿人机械臂在复杂环境下的任务空间的拟人路径规划，第二层运动规划为仿人机械臂沿末端无碰撞路径的可用臂构型空间的拟人臂构型规划；

所述方法包括：

步骤一、在所述仿人机械臂的任务空间内，为所述仿人机械臂的末端执行器规划出一条从其初始位置至目标位置的拟人的无碰撞路径；

步骤二、采用基于平滑样条的路径平滑方法对所述无碰撞路径进行平滑处理，形成所述末端执行器的任务空间路径规划；

步骤三、在所述末端执行器能够自其初始位置经平滑处理后的无碰撞路径到达目标位置情形下，计算并保存所有满足关节限位关系和避碰撞约束的可用机械臂构型；

步骤四、根据人臂的运动学特征，在沿末端执行器无碰撞路径的所有可用机械臂构型中选择出最优机械臂构型集；

步骤五、控制所述仿人机械臂依次完成所述最优机械臂构型集中的所有臂构型，使所述末端执行器完成所述平滑处理后的无碰撞路径；

步骤一和步骤二构成仿人机械臂在复杂环境下的任务空间的拟人路径规划，步骤三和步骤四构成仿人机械臂沿末端无碰撞路径的可用臂构型空间的拟人臂构型规划；

其特征在于，步骤一包括：

步骤一一、在所述仿人机械臂的任务空间内，采用随机采样的方式，以初始点为基础构建第一树的节点集，以目标点为基础构建第二树的节点集；

初始点和目标点分别为所述末端执行器的初始位置和目标位置；

在六维任务空间内通过随机采样的方式获得的多个机械臂末端点相互独立，恒等分布；

步骤一二、在机械臂的任务空间内分别以初始点和目标点为根节点扩展第一树和第二树，所述第一树和第二树均为启发式快速扩展随机树，二者以相同的扩展方式相对交替扩展，直至二者相遇；

步骤一三、当第一树和第二树相遇时，计算出此时从初始点至目标点的路径；

步骤一四、采用基于三角不等式的路径优化方法对步骤一三所述路径进行局部优化处理，并得到局部优化路径；

比较步骤一三所述路径和所述局部优化路径的代价，并保存代价较小的机械臂末端路径，该路径为无碰撞机械臂末端路径。

2.如权利要求1所述的七自由度仿人机械臂的双层拟人运动规划方法，其特征在于，步骤一二包括：

步骤一二一、在所述仿人机械臂的任务空间内，采用随机采样的方式获取一个机械臂末端点，并采用基于臂构型描述的末端新生长路径点有效性判断方法对该点进行有效性判断，如果该点被判断为有效，将该点作为第一树的新生长路径点，并执行步骤一二二；

否则，执行步骤一二一；

步骤一二二、在第一树的节点集中选择出第一树的新生长路径点的最近节点；

步骤一二三、在第一树的新生长路径点的附近节点集中，采用路径代价算法选择出路径代价最小的附近节点，并将该节点作为第一树的新生长路径点的父节点；

依次连接第一树的新生长路径点的最近节点、第一树的新生长路径点的父节点和第一树的新生长路径点，完成第一树的一次扩展；

步骤一二四、判断第一树的新生长路径点是否与第二树相遇，如果二者相遇，执行步骤一三，否则，执行步骤一二五；

第一树的新生长路径点与第二树是否相遇的判断方法为：在第二树的节点集中选择出第一树的新生长路径点的最近节点，如果该节点与第一树的新生长路径点的间距小于一个步长，第一树的新生长路径点与第二树相遇，否则，二者未相遇；

步骤一二五、完成第二树的一次扩展，并判断第二树的新生长路径点是否与第一树相遇，如果二者相遇，执行步骤一三，否则，执行步骤一二六；

第二树的新生长路径点与第一树是否相遇的判断方法为：在第一树的节点集中选择出第二树的新生长路径点的最近节点，如果该节点与第二树的新生长路径点的间距小于一个步长，第二树的新生长路径点与第一树相遇，否则，二者未相遇；

步骤一二六、完成第二树的一次扩展，并判断第二树的新生长路径点是否与第一树相遇，如果二者相遇，执行步骤一三，否则，执行步骤一二七；

步骤一二七、以扩展两次第一树和扩展两次第二树的方式循环扩展，直至第一树和第二树相遇，二者相遇后，执行步骤一三。