七自由度仿人机械臂的双层拟人运动规划方法技术

七自由度仿人机械臂的双层拟人运动规划方法，属于机械臂控制领域，解决了现有基于Bi‑RRT*算法的七自由度仿人机械臂路径规划方法不适用于仿人机械臂的任务空间规划，且无法同时优化仿人机械臂的末端执行器的路径和机械臂构型的问题。所述方法包括为末端执行器规划无碰撞路径、对无碰撞路径进行平滑处理、计算所有可用机械臂构型、根据人臂的运动学特征选择最优机械臂构型集和控制仿人机械臂依次完成最优机械臂构型集中所有臂构型的步骤。在规划无碰撞路径的步骤中采用基于臂构型描述的末端新生长路径点有效性判断方法替代现有Bi‑RRT*算法中的碰撞检测方法。本发明专利技术所述方法适用于七自由度仿人机械臂的双层拟人运动规划。

【技术实现步骤摘要】
七自由度仿人机械臂的双层拟人运动规划方法
本专利技术涉及机械臂控制领域，具体地说，涉及一种复杂环境下七自由度仿人机械臂的双层拟人运动规划方法。
技术介绍
目前，基于随机采样的避障路径规划方法被广泛地用于七自由度仿人机械臂的运动规划中。该类方法通过碰撞检测模块提供采样点的可行性信息，连接一系列从自由空间采样的点，构建一张可行轨迹的图，从这张图中搜索起始点到目标点的可行路径。该类方法不需要在仿人机械臂的关节空间中对障碍物进行精确建模，因而计算量小，即使在高维空间中规划仿人机械臂的运动轨迹，其效率也相当高。Bi-RRT*(BidirectionalRapidly-ExploringRandomTreeStar，双向快速扩展随机树星)算法是基于随机采样的避障路径规划方法中被广泛使用的方法。该方法将双向搜索策掠引入现有的RRT*(Rapidly-ExploringRandomTreeStar，快速扩展随机树星)算法中，并引入一个启发函数。与RRT*算法相比，Bi-RRT*算法具有更强的避障路径规划能力。但是，现有基于Bi-RRT*算法的七自由度仿人机械臂路径规划方法因Bi-RRT*算法采用碰撞检测方法判断采样点的可行性使得该路径规划方法只适用于仿人机械臂的关节空间而不适用于仿人机械臂的任务空间，且无法同时优化仿人机械臂的末端执行器的路径和机械臂构型。
技术实现思路
本专利技术为解决现有基于Bi-RRT*算法的七自由度仿人机械臂路径规划方法不适用于仿人机械臂的任务空间规划，且无法同时优化仿人机械臂的末端执行器的路径和机械臂构型的问题，提出了一种七自由度仿人机械臂的双层拟人运动规划方法。本专利技术所述的七自由度仿人机械臂的双层拟人运动规划方法包括第一层运动规划和第二层运动规划，第一层运动规划为仿人机械臂在复杂环境下的任务空间的拟人路径规划，第二层运动规划为仿人机械臂沿末端无碰撞路径的可用臂构型空间的拟人臂构型规划；所述方法包括：步骤一、在所述仿人机械臂的任务空间内，为所述仿人机械臂的末端执行器规划出一条从其初始位置至目标位置的拟人的无碰撞路径；步骤二、采用基于平滑样条的路径平滑方法对所述无碰撞路径进行平滑处理，形成所述末端执行器的任务空间路径规划；步骤三、在所述末端执行器能够自其初始位置经平滑处理后的无碰撞路径到达目标位置情形下，计算并保存所有满足关节限位关系和避碰撞约束的可用机械臂构型；步骤四、根据人臂的运动学特征，在沿末端执行器无碰撞路径的所有可用机械臂构型中选择出最优机械臂构型集；步骤五、控制所述仿人机械臂依次完成所述最优机械臂构型集中的所有臂构型，使所述末端执行器完成所述平滑处理后的无碰撞路径；步骤一和步骤二构成仿人机械臂在复杂环境下的任务空间的拟人路径规划，步骤三和步骤四构成仿人机械臂沿末端无碰撞路径的可用臂构型空间的拟人臂构型规划。作为优选的是，步骤一包括：步骤一一、在所述仿人机械臂的任务空间内，采用随机采样的方式，以初始点为基础构建第一树的节点集，以目标点为基础构建第二树的节点集；初始点和目标点分别为所述末端执行器的初始位置和目标位置；在六维任务空间内通过随机采样的方式获得的多个机械臂末端点相互独立，恒等分布；步骤一二、在机械臂的任务空间内分别以初始点和目标点为根节点扩展第一树和第二树，所述第一树和第二树均为启发式快速扩展随机树，二者以相同的扩展方式相对交替扩展，直至二者相遇；步骤一三、当第一树和第二树相遇时，计算出此时从初始点至目标点的路径；步骤一四、采用基于三角不等式的路径优化方法对步骤一三所述路径进行局部优化处理，并得到局部优化路径；比较步骤一三所述路径和所述局部优化路径的代价，并保存代价较小的机械臂末端路径，该路径为无碰撞机械臂末端路径。进一步的是，步骤一二包括：步骤一二一、在所述仿人机械臂的任务空间内，采用随机采样的方式获取一个机械臂末端点，并采用基于臂构型描述的末端新生长路径点有效性判断方法对该点进行有效性判断，如果该点被判断为有效，将该点作为第一树的新生长路径点，并执行步骤一二二；否则，执行步骤一二一；步骤一二二、在第一树的节点集中选择出第一树的新生长路径点的最近节点；步骤一二三、在第一树的新生长路径点的附近节点集中，采用路径代价算法选择出路径代价最小的附近节点，并将该节点作为第一树的新生长路径点的父节点；依次连接第一树的新生长路径点的最近节点、第一树的新生长路径点的父节点和第一树的新生长路径点，完成第一树的一次扩展；步骤一二四、判断第一树的新生长路径点是否与第二树相遇，如果二者相遇，执行步骤一三，否则，执行步骤一二五；第一树的新生长路径点与第二树是否相遇的判断方法为：在第二树的节点集中选择出第一树的新生长路径点的最近节点，如果该节点与第一树的新生长路径点的间距小于一个步长，第一树的新生长路径点与第二树相遇，否则，二者未相遇；步骤一二五、完成第二树的一次扩展，并判断第二树的新生长路径点是否与第一树相遇，如果二者相遇，执行步骤一三，否则，执行步骤一二六；第二树的新生长路径点与第一树是否相遇的判断方法为：在第一树的节点集中选择出第二树的新生长路径点的最近节点，如果该节点与第二树的新生长路径点的间距小于一个步长，第二树的新生长路径点与第一树相遇，否则，二者未相遇；步骤一二六、完成第二树的一次扩展，并判断第二树的新生长路径点是否与第一树相遇，如果二者相遇，执行步骤一三，否则，执行步骤一二七；步骤一二七、以扩展两次第一树和扩展两次第二树的方式循环扩展，直至第一树和第二树相遇，二者相遇后，执行步骤一三。进一步的是，步骤一二一所述的基于臂构型描述的末端新生长路径点有效性判断方法包括：步骤A、将仿人机械臂的任务空间内的随机采样点假设为仿人机械臂末端新生长路径点，计算仿人机械臂在该新生长路径点下的满足仿人机械臂关节限位关系的可用臂构型集；该步骤的具体过程为：以第二关节为坐标原点建立三维基坐标，给定所述仿人机械臂末端新生长路径点相对于所述三维基坐标的期望位置和期望姿态，通过建立基于臂角的仿人机械臂运动学模型获得各个关节角与臂角的关系；第一关节至第七关节依次分布在所述仿人机械臂的基座与末端执行器之间；根据各个关节角与臂角的关系，将第二关节和第六关节的运动范围分别分为正负两个区域，进而形成四个臂角区域；所述四个臂角区域分别为：第一臂角区域，满足第二关节角和第六关节角同时小于0；第二臂角区域，满足第二关节角小于0，第六关节角大于0；第三臂角区域，满足第二关节角大于0，第六关节角小于0；第四臂角区域，满足第二关节角和第六关节角同时大于0；分别计算第一臂角区域、第二臂角区域、第三臂角区域和第四臂角区域内所述仿人机械臂的可用臂角范围，从而计算出所述仿人机械臂在该新生长路径点下的满足仿人机械臂关节限位关系的可用臂构型集；步骤B、建立仿人机械臂在该新生长路径点下的基于球扫掠凸体的碰撞检测模型，计算出该新生长路径点下同时满足仿人机械臂关节限位和避碰撞约束的可用臂构型集；该步骤的具体过程为：建立仿人机械臂在关节限位下自运动形成的扫掠体的基于球扫掠凸体的碰撞检测模型：当仿人机械臂自运动时，其上臂和下臂分别形成圆锥面扫掠体，根据第四关节自运动形成的轨迹，采用n个球扫掠凸体构建所述圆锥面扫掠体；上臂包括第一关节至第四关节，下臂包括第四本文档来自技高网...

【技术保护点】
七自由度仿人机械臂的双层拟人运动规划方法，其特征在于，所述方法包括第一层运动规划和第二层运动规划，第一层运动规划为仿人机械臂在复杂环境下的任务空间的拟人路径规划，第二层运动规划为仿人机械臂沿末端无碰撞路径的可用臂构型空间的拟人臂构型规划；所述方法包括：步骤一、在所述仿人机械臂的任务空间内，为所述仿人机械臂的末端执行器规划出一条从其初始位置至目标位置的拟人的无碰撞路径；步骤二、采用基于平滑样条的路径平滑方法对所述无碰撞路径进行平滑处理，形成所述末端执行器的任务空间路径规划；步骤三、在所述末端执行器能够自其初始位置经平滑处理后的无碰撞路径到达目标位置情形下，计算并保存所有满足关节限位关系和避碰撞约束的可用机械臂构型；步骤四、根据人臂的运动学特征，在沿末端执行器无碰撞路径的所有可用机械臂构型中选择出最优机械臂构型集；步骤五、控制所述仿人机械臂依次完成所述最优机械臂构型集中的所有臂构型，使所述末端执行器完成所述平滑处理后的无碰撞路径；步骤一和步骤二构成仿人机械臂在复杂环境下的任务空间的拟人路径规划，步骤三和步骤四构成仿人机械臂沿末端无碰撞路径的可用臂构型空间的拟人臂构型规划。

【技术特征摘要】
1.七自由度仿人机械臂的双层拟人运动规划方法，其特征在于，所述方法包括第一层运动规划和第二层运动规划，第一层运动规划为仿人机械臂在复杂环境下的任务空间的拟人路径规划，第二层运动规划为仿人机械臂沿末端无碰撞路径的可用臂构型空间的拟人臂构型规划；所述方法包括：步骤一、在所述仿人机械臂的任务空间内，为所述仿人机械臂的末端执行器规划出一条从其初始位置至目标位置的拟人的无碰撞路径；步骤二、采用基于平滑样条的路径平滑方法对所述无碰撞路径进行平滑处理，形成所述末端执行器的任务空间路径规划；步骤三、在所述末端执行器能够自其初始位置经平滑处理后的无碰撞路径到达目标位置情形下，计算并保存所有满足关节限位关系和避碰撞约束的可用机械臂构型；步骤四、根据人臂的运动学特征，在沿末端执行器无碰撞路径的所有可用机械臂构型中选择出最优机械臂构型集；步骤五、控制所述仿人机械臂依次完成所述最优机械臂构型集中的所有臂构型，使所述末端执行器完成所述平滑处理后的无碰撞路径；步骤一和步骤二构成仿人机械臂在复杂环境下的任务空间的拟人路径规划，步骤三和步骤四构成仿人机械臂沿末端无碰撞路径的可用臂构型空间的拟人臂构型规划。2.如权利要求1所述的七自由度仿人机械臂的双层拟人运动规划方法，其特征在于，步骤一包括：步骤一一、在所述仿人机械臂的任务空间内，采用随机采样的方式，以初始点为基础构建第一树的节点集，以目标点为基础构建第二树的节点集；初始点和目标点分别为所述末端执行器的初始位置和目标位置；在六维任务空间内通过随机采样的方式获得的多个机械臂末端点相互独立，恒等分布；步骤一二、在机械臂的任务空间内分别以初始点和目标点为根节点扩展第一树和第二树，所述第一树和第二树均为启发式快速扩展随机树，二者以相同的扩展方式相对交替扩展，直至二者相遇；步骤一三、当第一树和第二树相遇时，计算出此时从初始点至目标点的路径；步骤一四、采用基于三角不等式的路径优化方法对步骤一三所述路径进行局部优化处理，并得到局部优化路径；比较步骤一三所述路径和所述局部优化路径的代价，并保存代价较小的机械臂末端路径，该路径为无碰撞机械臂末端路径。3.如权利要求2所述的七自由度仿人机械臂的双层拟人运动规划方法，其特征在于，步骤一二包括：步骤一二一、在所述仿人机械臂的任务空间内，采用随机采样的方式获取一个机械臂末端点，并采用基于臂构型描述的末端新生长路径点有效性判断方法对该点进行有效性判断，如果该点被判断为有效，将该点作为第一树的新生长路径点，并执行步骤一二二；否则，执行步骤一二一；步骤一二二、在第一树的节点集中选择出第一树的新生长路径点的最近节点；步骤一二三、在第一树的新生长路径点的附近节点集中，采用路径代价算法选择出路径代价最小的附近节点，并将该节点作为第一树的新生长路径点的父节点；依次连接第一树的新生长路径点的最近节点、第一树的新生长路径点的父节点和第一树的新生长路径点，完成第一树的一次扩展；步骤一二四、判断第一树的新生长路径点是否与第二树相遇，如果二者相遇，执行步骤一三，否则，执行步骤一二五；第一树的新生长路径点与第二树是否相遇的判断方法为：在第二树的节点集中选择出第一树的新生长路径点的最近节点，如果该节点与第一树的新生长路径点的间距小于一个步长，第一树的新生长路径点与第二树相遇，否则，二者未相遇；步骤一二五、完成第二树的一次扩展，并判断第二树的新生长路径点是否与第一树相遇，如果二者相遇，执行步骤一三，否则，执行步骤一二六；第二树的新生长路径点与第一树是否相遇的判断方法为：在第一树的节点集中选择出第二树的新生长路径点的最近节点，如果该节点与第二树的新生长路径点的间距小于一个步长，第二树的新生长路径点与第一树相遇，否则，二者未相遇；步骤一二六、完成第二树的一次扩展，并判断第二树的新生长路径点是否与第一树相遇，如果二者相遇，执行步骤一三，否则，执行步骤一二七；步骤一二七、以扩展两次第一树和扩展两次第二树的方式循环扩展，直至第一树和第二树相遇，二者相遇后，执行步骤一三。4.如权利要求3所述的七自由度仿人机械臂的双层拟人运动规划方法，其特征在于，步骤一二一所述的基于臂构型描述的末端新生长路径点有效性判断方法包...

【专利技术属性】
技术研发人员：夏晶，
申请(专利权)人：西安科技大学，
类型：发明
国别省市：陕西,61