CN106737671B - 七自由度仿人机械臂的双层拟人运动规划方法 - Google Patents

Abstract

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

Description

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

技术领域

本发明涉及机械臂控制领域，具体地说，涉及一种复杂环境下七自由度仿人机械臂的双层拟人运动规划方法。

背景技术

目前，基于随机采样的避障路径规划方法被广泛地用于七自由度仿人机械臂的运动规划中。该类方法通过碰撞检测模块提供采样点的可行性信息，连接一系列从自由空间采样的点，构建一张可行轨迹的图，从这张图中搜索起始点到目标点的可行路径。该类方法不需要在仿人机械臂的关节空间中对障碍物进行精确建模，因而计算量小，即使在高维空间中规划仿人机械臂的运动轨迹，其效率也相当高。

Bi-RRT*(Bidirectional Rapidly-Exploring Random Tree Star，双向快速扩展随机树星)算法是基于随机采样的避障路径规划方法中被广泛使用的方法。该方法将双向搜索策掠引入现有的RRT*(Rapidly-Exploring Random Tree Star，快速扩展随机树星)算法中，并引入一个启发函数。与RRT*算法相比，Bi-RRT*算法具有更强的避障路径规划能力。

但是，现有基于Bi-RRT*算法的七自由度仿人机械臂路径规划方法因Bi-RRT*算法采用碰撞检测方法判断采样点的可行性使得该路径规划方法只适用于仿人机械臂的关节空间而不适用于仿人机械臂的任务空间，且无法同时优化仿人机械臂的末端执行器的路径和机械臂构型。

发明内容

本发明为解决现有基于Bi-RRT*算法的七自由度仿人机械臂路径规划方法不适用于仿人机械臂的任务空间规划，且无法同时优化仿人机械臂的末端执行器的路径和机械臂构型的问题，提出了一种七自由度仿人机械臂的双层拟人运动规划方法。

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

所述方法包括：

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

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

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

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

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

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

作为优选的是，步骤一包括：

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

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

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

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

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

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

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

进一步的是，步骤一二包括：

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

否则，执行步骤一二一；

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

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

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

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

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

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

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

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

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

进一步的是，步骤一二一所述的基于臂构型描述的末端新生长路径点有效性判断方法包括：

步骤A、将仿人机械臂的任务空间内的随机采样点假设为仿人机械臂末端新生长路径点，计算仿人机械臂在该新生长路径点下的满足仿人机械臂关节限位关系的可用臂构型集；

该步骤的具体过程为：

以第二关节为坐标原点建立三维基坐标，给定所述仿人机械臂末端新生长路径点相对于所述三维基坐标的期望位置和期望姿态，通过建立基于臂角的仿人机械臂运动学模型获得各个关节角与臂角的关系；

第一关节至第七关节依次分布在所述仿人机械臂的基座与末端执行器之间；

根据各个关节角与臂角的关系，将第二关节和第六关节的运动范围分别分为正负两个区域，进而形成四个臂角区域；

所述四个臂角区域分别为：

第一臂角区域，满足第二关节角和第六关节角同时小于0；

第二臂角区域，满足第二关节角小于0，第六关节角大于0；

第三臂角区域，满足第二关节角大于0，第六关节角小于0；

第四臂角区域，满足第二关节角和第六关节角同时大于0；

分别计算第一臂角区域、第二臂角区域、第三臂角区域和第四臂角区域内所述仿人机械臂的可用臂角范围，从而计算出所述仿人机械臂在该新生长路径点下的满足仿人机械臂关节限位关系的可用臂构型集；

步骤B、建立仿人机械臂在该新生长路径点下的基于球扫掠凸体的碰撞检测模型，计算出该新生长路径点下同时满足仿人机械臂关节限位和避碰撞约束的可用臂构型集；

该步骤的具体过程为：

建立仿人机械臂在关节限位下自运动形成的扫掠体的基于球扫掠凸体的碰撞检测模型：

当仿人机械臂自运动时，其上臂和下臂分别形成圆锥面扫掠体，根据第四关节自运动形成的轨迹，采用n个球扫掠凸体构建所述圆锥面扫掠体；

上臂包括第一关节至第四关节，下臂包括第四关节至第七关节，n为正整数；

所述球扫掠凸体由球半径和五个顶点构成，该球扫掠凸体定义为：

其中，r为球扫掠凸体V₁的半径，球扫掠凸体V₁的五个顶点分别为至为第一关节、第二关节与第三关节的旋转轴线的交点或者第五关节、第六关节与第七关节的旋转轴线的交点，为第四关节的位置， 是自运动旋转ψ_l/2n的点，是自运动旋转ψ_l/n的点，ψ_l为自运动旋转的角度；

或者，所述球扫掠凸体由球半径和四个顶点构成，该球扫掠凸体定义为：

其中，球扫掠凸体V₂的四个顶点分别为至 是自运动旋转ψ_l/n的点；

分别建立仿人机械人中除机械臂外的其他部件和周围静止环境的基于球扫掠凸体的碰撞检测模型；

采用基于球扫掠凸体的碰撞检测方法检测仿人机械臂自运动形成的扫掠体的基于球扫掠凸体的碰撞检测模型与仿人机械人的其他部件以及周围静止环境的基于球扫掠凸体的碰撞检测模型是否发生碰撞以及是否存在可能的碰撞点，剔除可能产出碰撞的臂构型，获得仿人机械臂在该新生长路径点下同时满足仿人机械臂关节限位关系和避碰撞约束的可用臂构型集；

步骤C、计算所述四个臂角区域中任意两个臂角区域之间的过渡条件；

步骤D、根据仿人机械臂在该新生长路径点下同时满足仿人机械臂关节限位关系和避碰撞约束的可用臂构型集以及所述任意两个臂角区域之间的过渡条件判断该新生长路径点的有效性。

进一步的是，步骤D包括：

步骤a、判断第四关节是否超限，如果第四关节超限，则新生长路径点无效，否则，执行步骤b；

步骤b、判断可用臂构型集是否为空，如果可用臂构型集为空，则新生长路径点无效，否则，执行步骤c；

步骤c、判断该新生长路径点与前一个生长路径点是否在同一个臂角区域内，如果在同一个臂角区域内，则新生长路径点有效，否则，执行步骤d；

步骤d、判断新生长路径点所在的臂角区域与前一个生长路径点所在的臂角区域是否存在过渡点，如果存在过渡点，则新生长路径点有效，否则，新生长路径点无效。

优选的是，步骤四采用冗余解优化方法在所有可用机械臂构型中选择出最优机械臂构型集，所述冗余解优化方法包括：

步骤四一、选择最优臂角区域；

步骤四二、在优化臂角区域内优化拟人臂角；

步骤四三、处理不可用优化臂角；

步骤四四、臂角区域过渡；

步骤四五、采用基于臂角的解析逆运动学方法计算各关节角度。

本发明所述的七自由度仿人机械臂的双层拟人运动规划方法包括双层拟人运动规划，第一层拟人运动规划为仿人机械臂在复杂环境下的任务空间的拟人路径规划，该规划通过实施步骤一和步骤二实现，第二层拟人运动规划为仿人机械臂的可用臂构型空间的拟人臂构型规划，该规划通过实施步骤三和步骤四实现。所述双层拟人运动规划解决了现有基于Bi-RRT*算法的七自由度仿人机械臂路径规划方法无法同时优化仿人机械臂的末端执行器的路径和机械臂构型的问题。

本发明所述的七自由度仿人机械臂的双层拟人运动规划方法采用基于臂构型描述的七自由度仿人机械臂末端点有效性判断方法替代现有Bi-RRT*算法中的碰撞检测方法，使得该运动规划方法适用于仿人机械臂的任务空间规划。

附图说明

在下文中将基于实施例并参考附图来对本发明所述的七自由度仿人机械臂的双层拟人运动规划方法进行更详细的描述，其中：

图1是实施例一提及的参考平面、臂平面和臂角的示意图；

图2是实施例一提及的基于臂角的仿人机械臂运动学模型的示意图；

图3是实施例一提及的仿人机械臂自运动形成的扫略体以及由两个包含一个半径和五个顶点的球扫略凸体构建的碰撞检测模型示意图；

图4是实施例一提及的仿人机械臂自运动形成的扫略体以及由两个包含一个半径和四个顶点的球扫略凸体构建的碰撞检测模型示意图；

图5是实施例一提及的仿人机械臂的下臂上端自运动形成的轨迹和包裹下臂上端的由一个半径和四个顶点组成的球扫掠凸体的示意图；

图6是实施例一提及的仿人机械臂的下臂上端自运动形成的轨迹和包裹下臂上端的由一个半径和三个顶点组成的球扫掠凸体的示意图。

在附图中，相同的部件使用相同的附图标记。附图并未按照实际的比例。

具体实施方式

下面将结合附图对本发明所述的七自由度仿人机械臂的双层拟人运动规划方法作进一步说明。

实施例一：下面结合图1至图6详细地说明本实施例。本实施例所述的七自由度仿人机械臂的双层拟人运动规划方法包括第一层运动规划和第二层运动规划，第一层运动规划为仿人机械臂在复杂环境下的任务空间的拟人路径规划，第二层运动规划为仿人机械臂沿末端无碰撞路径的可用臂构型空间的拟人臂构型规划；

所述方法包括：

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

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

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

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

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

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

所述步骤一包括：

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

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

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

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

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

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

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

所述步骤一二包括：

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

否则，执行步骤一二一；

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

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

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

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

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

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

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

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

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

步骤一二一所述的基于臂构型描述的末端新生长路径点有效性判断方法包括：

步骤A、将仿人机械臂的任务空间内的随机采样点假设为仿人机械臂末端新生长路径点，计算仿人机械臂在该新生长路径点下的满足仿人机械臂关节限位关系的可用臂构型集；

该步骤的具体过程为：

以第二关节为坐标原点建立三维基坐标，给定所述仿人机械臂末端新生长路径点相对于所述三维基坐标的期望位置和期望姿态，通过建立基于臂角的仿人机械臂运动学模型获得各个关节角与臂角的关系；

第一关节至第七关节依次分布在所述仿人机械臂的基座与末端执行器之间；

根据各个关节角与臂角的关系，将第二关节和第六关节的运动范围分别分为正负两个区域，进而形成四个臂角区域；

所述四个臂角区域分别为：

第一臂角区域，满足第二关节角和第六关节角同时小于0；

第二臂角区域，满足第二关节角小于0，第六关节角大于0；

第三臂角区域，满足第二关节角大于0，第六关节角小于0；

第四臂角区域，满足第二关节角和第六关节角同时大于0；

分别计算第一臂角区域、第二臂角区域、第三臂角区域和第四臂角区域内所述仿人机械臂的可用臂角范围，从而计算出所述仿人机械臂在该新生长路径点下的满足仿人机械臂关节限位关系的可用臂构型集；

步骤B、建立仿人机械臂在该新生长路径点下的基于球扫掠凸体的碰撞检测模型，计算出该新生长路径点下同时满足仿人机械臂关节限位和避碰撞约束的可用臂构型集；

该步骤的具体过程为：

建立仿人机械臂在关节限位下自运动形成的扫掠体的基于球扫掠凸体的碰撞检测模型：

当仿人机械臂自运动时，其上臂和下臂分别形成圆锥面扫掠体，根据第四关节自运动形成的轨迹，采用n个球扫掠凸体构建所述圆锥面扫掠体；

上臂包括第一关节至第四关节，下臂包括第四关节至第七关节，n为正整数；

所述球扫掠凸体由球半径和五个顶点构成，该球扫掠凸体定义为：

其中，r为球扫掠凸体V₁的半径，球扫掠凸体V₁的五个顶点分别为至为第一关节、第二关节与第三关节的旋转轴线的交点或者第五关节、第六关节与第七关节的旋转轴线的交点，为第四关节的位置， 是自运动旋转ψ_l/2n的点，是自运动旋转ψ_l/n的点，ψ_l为自运动旋转的角度；

或者，所述球扫掠凸体由球半径和四个顶点构成，该球扫掠凸体定义为：

其中，球扫掠凸体V₂的四个顶点分别为至 是自运动旋转ψ_l/n的点；

分别建立仿人机械人中除机械臂外的其他部件和周围静止环境的基于球扫掠凸体的碰撞检测模型；

采用基于球扫掠凸体的碰撞检测方法检测仿人机械臂自运动形成的扫掠体的基于球扫掠凸体的碰撞检测模型与仿人机械人的其他部件以及周围静止环境的基于球扫掠凸体的碰撞检测模型是否发生碰撞以及是否存在可能的碰撞点，剔除可能产出碰撞的臂构型，获得仿人机械臂在该新生长路径点下同时满足仿人机械臂关节限位关系和避碰撞约束的可用臂构型集；

步骤C、计算所述四个臂角区域中任意两个臂角区域之间的过渡条件；

步骤D、根据仿人机械臂在该新生长路径点下同时满足仿人机械臂关节限位关系和避碰撞约束的可用臂构型集以及所述任意两个臂角区域之间的过渡条件判断该新生长路径点的有效性；

步骤四采用冗余解优化方法在所有可用机械臂构型中选择出最优机械臂构型集，所述冗余解优化方法包括：

步骤四一、选择最优臂角区域；

步骤四二、在优化臂角区域内优化拟人臂角；

步骤四三、处理不可用优化臂角；

步骤四四、臂角区域过渡；

步骤四五、采用基于臂角的解析逆运动学方法计算各关节角度。

本实施例选用臂角作为冗余参数来描述仿人机械臂的自运动。图1是参考平面、臂平面和臂角的示意图，如图1所示，P_s、P_e和P_w分别为仿人机械臂的肩关节、肘关节和腕关节，ψ为臂角。图2是基于臂角的仿人机械臂运动学模型的示意图，如图2所示，q₁至q₇分别为仿人机械臂的第一关节至第七关节，第一关节q₁至第三关节q₃、第四关节q₄和第五关节q₅至第七关节q₇分别对应肩关节P_s、肘关节P_e和腕关节P_w，坐标原点O、X轴、Y轴和Z轴构成三维基坐标。由肩关节P_s、肘关节P_e和腕关节P_w构成的平面为臂平面。当仿人机械臂的末端执行器的位姿不变时，由于仿人机械臂包含一个冗余自由度，肘关节P_e能够以肩关节P_s和腕关节P_w共同的轴线为轴旋转，进而形成多个角度不同的臂平面。定义第三关节q₃为0度时的臂平面为参考平面，参考平面和臂平面的夹角为臂角ψ。

仿人机械臂自运动的范围会受到关节限位和障碍物位置的影响，当仿人机械臂自运动的范围为空时，表明仿人机械臂受关节限位和障碍物位置的限制无法到达该位置。本实施例基于该原理提出基于臂构型描述的末端新生长路径点有效性判断方法。

在给定末端位姿下，各个关节角与臂角ψ的关系为：

θ₁＝a tan2{sign(sinθ₂)(-a_s22sinψ-b_s22cosψ-c_s22),

sign(sinθ₂)(-a_s12sinψ-b_s12cosψ-c_s12)}

θ₂＝±a cos(-a_s32sinψ-b_s32cosψ-c_s32)

θ₃＝a tan2{sign(sinθ₂)(a_s33sinψ+b_s33cosψ+c_s33),

sign(sinθ₂)(-a_s31sinψ-b_s31cosψ-c_s31)}

θ₅＝a tan2{sign(sinθ₆)(a_w23sinψ+b_w23cosψ+c_w23),

sign(sinθ₆)(a_w13sinψ+b_w13cosψ+c_w13)}

θ₆＝±a cos(a_w33sinψ+b_w33cosψ+c_w33)

θ₇＝a tan2{sign(sinθ₆)(a_w32sinψ+b_w32cosψ+c_w32),

sign(sinθ₆)(-a_w31sinψ-b_w31cosψ-c_w31)}

其中，θ₁至θ₇分别为第一关节角至第七关节角，a_sij,b_sij,c_sij,a_wij,b_wij,c_wij分别为矩阵A_s,B_s,C_s,A_w,B_w，C_w的(i,j)元素，⁰x_sw是肩关节P_s到腕关节P_w的向量，d_se是肩关节P_s到肘关节P_e的向量，d_ew是肘关节P_e到腕关节的向量，d_se和d_ew是常向量。

根据上述各个关节角与臂角ψ的关系可知：第四关节角θ₄只与仿人机械臂的末端执行器的位姿有关，第二关节角θ₂和第六关节角θ₆与仿人机械臂的末端执行器的位姿和臂角ψ有关，第一关节角θ₁、第三关节角θ₃、第五关节角θ₅和第七关节角θ₇除了与仿人机械臂的末端执行器的位姿和臂角ψ有关，还与第二关节角θ₂和第六关节角θ₆的正负有关。

假设第i关节角的范围为(i＝1,2,…,7)θ_i∈[-π,π]，其中，和分别是第i关节的下限和上限。为了在[-π,π]的范围内确定各个关节角与臂角ψ的唯一对应关系，将第二关节角θ₂和第六关节角θ₆分别分为正负两个区域，从而形成四个臂角区域：

第一臂角区域：即θ₂<0,θ₆<0，所述仿人机械臂在该臂角区域内的可用臂角范围为：

第二臂角区域：即θ₂<0,θ₆>0，所述仿人机械臂在该臂角区域内的可用臂角范围为：

第三臂角区域：即θ₂>0,θ₆<0，所述仿人机械臂在该臂角区域内的可用臂角范围为：

第四臂角区域：即θ₂>0,θ₆>0，所述仿人机械臂在该臂角区域内的可用臂角范围为：

其中，(i＝1,3,5,7)是第二关节角或第六关节角为正时，第i关节的有效臂角范围，(i＝1,3,5,7)是第二关节角或第六关节角为负时，第i关节的有效臂角范围，ψ_j<0，(j＝2,6)为第j关节在关节运动范围内的有效臂角范围，ψ_j>0，(j＝2,6)为第j关节在关节运动范围内的有效臂角范围；

所述仿人机械臂末端点在该新生长路径点下满足关节限位关系的臂构型集为；

随后建立各个关节限位和臂角的关系，对于第二关节角θ₂和第六关节角θ₆，其逆解和臂角ψ的关系可描述为如下函数：

cosθ_i＝a sinψ+b cosψ+c

a、b和c均为系数，从而建立如下不等式解得ψ_j<0和ψ_j>0：

对于第一关节角θ₁、第三关节角θ₃、第五关节角θ₅和第七关节角θ₇在各个臂角区域内，其角度与臂角一一对应，可描述为如下正切函数：

a_n、b_n、c_n、a_d、b_d、c_d均为系数，tanθ_i在[-π,π]范围内为周期函数，为了建立臂角ψ和关节上下限的关系，需要根据关节运动范围分段建立不等式关系。例如时，将分为三段建立不等式组：

a)正切函数单调连续，分子、分母小于零，因此第i关节，i∈(1,3,5,7)在该范围的可用臂角可以通过如下不等式组获得:

其中+和-符号的选择取决于对应余弦关节的正负。

b)θ_i∈[-π/2,π/2],正切函数分母大于零，因此第i关节在该范围的可用臂角满足如下不等式组

c)在该范围的可用臂角满足如下不等式组

最终第i关节，i∈(1,3,5,7)在关节限位下的可用臂角范围为

为了剔除碰撞对仿人机械臂的自运动的影响引入碰撞检测技术。传统的离散碰撞检测将仿人机械臂的自运动范围分成若干个离散臂构型来判断是否发生碰撞，计算量大，会遗漏两检测点间的碰撞，产生“隧道效应”。因此本实施例采用球扫略凸体构建仿人机械臂自运动形成的扫略体，可以有效地提高了碰撞检测算法的实时性，解决“隧道效应”。

球扫掠凸体是由半径为r球体与凸体的Minkowski和,定义如下：其中是由点集形成的凸体。

采用球扫掠凸体构建仿人机械臂的上臂和下臂的碰撞检测模型，仿人机械臂的上臂包括第一关节q₁至第四关节q₄，仿人机械臂的下臂包括第四关节q₄至第七关节q₇。图3是仿人机械臂自运动形成的扫略体以及由两个包含一个半径和五个顶点的球扫略凸体构建的碰撞检测模型示意图。图4是仿人机械臂自运动形成的扫略体以及由两个包含一个半径和四个顶点的球扫略凸体构建的碰撞检测模型示意图。

当所述仿人机械臂自运动时，其上臂和下臂分别形成圆锥面扫掠体，采用多个球扫掠凸体近似所述圆锥面扫掠体。假设近似允许的误差为ε，首先计算需要的球扫掠凸体个数n＝min{1,2,3,....}，即满足下式中变量n的最小值：

图5是仿人机械臂的下臂上端自运动形成的轨迹和包裹下臂上端的由一个半径和四个顶点组成的球扫掠凸体的示意图，如图5所示，所述仿人机械臂下臂的上端自运动形成的轨迹可用两个球扫掠体近似，每个球扫掠凸体可由一个半径和四个顶点组成，因此下臂形成的扫掠体可以表示为：

其中， 是点自运动旋转ψ_l/4的点；是点自运动旋转ψ_l/2的点；同理可以获得球扫掠凸体的各顶点。

图6是仿人机械臂的下臂上端自运动形成的轨迹和包裹下臂上端的由一个半径和三个顶点组成的球扫掠凸体的示意图，如图6所示，每个球扫掠凸体还可由一个半径和三个顶点组成，此时仿人机械臂下臂自运动形成的扫掠体可以表示为：

其中，

球扫掠凸体可以使用更少的点构建出精确的碰撞检测模型，调整球扫掠体凸体的点集和半径以最小的体积包含机器人各构件三维模型表面上的所有点，能够有效地增加碰撞检测的实时性。

由于仿人机械臂的可用臂角范围被分成了四个臂角区域，每个臂角区域都是独立的，只有在满足一定条件的过渡点上，仿人机械臂才能从一个臂角区域过渡到另一个臂角区域。

第一臂角区域与第二臂角区域之间的过渡条件和第三臂角区域与第四臂角区域之间的过渡条件相同，所述过渡条件为在第一臂角区域与第二臂角区域或第三臂角区域与第四臂角区域的重叠部分存在一个满足如下关系的可用臂角ψ_cond2：

1＝a_w33sinψ_cond2+b_w33cosψ_cond2+c_w33

在该点处对应的两组逆运动学解中第六关节角θ₆均为0度，从而使第六关节能连续地从负值运动到正值，不会产生关节角度的跳变。

第一臂角区域与第三臂角区域之间的过渡条件和第二臂角区域与第四臂角区域之间的过渡条件相同，涉及到第二关节角θ₂的正负转换，所述过渡条件是在第一臂角区域与第三臂角区域或第二臂角区域与第四臂角区域的重叠部分存在一个满足如下关系的可用臂角ψ_cond1：

1＝-a_s32sinψ_cond1-b_s32cosψ_cond1-c_s32

同理，第一臂角区域与第四臂角区域，第二臂角区域与第三臂角区域的过渡条件是在两个区域的重叠部分存在一个同时满足上面两个过渡条件的可用臂角。

通过实施步骤一得到的无碰撞路径为分段的线性路径，成锯齿状，不光滑，可能导致机械臂在运行中出现停顿，降低工作效率。步骤二对步骤一中的无碰撞路径进行平滑处理，能够有效地解决该问题。

使用C++语言和open inventor编程实施本发明的复杂环境下仿人机器人臂双层拟人运动规划系统用于一个拥有19个构件的仿人机器人上，其具体步骤如下：

(1)将仿人机器人和复杂环境的pre模型转换成iv格式导入程序，open inventor读取模型建立仿人机器人和复杂环境的虚拟场景。根据读入的三维模型构建复杂环境的基于球扫掠凸体的碰撞检测模型。保存仿人机械臂各关节的运动范围；

(2)输入仿人机械臂的初始构型和目标位姿，采用改进的RRT*算法在笛卡尔空间内搜索新的生长点；

(3)计算新的生长点下仿人机械臂满足关节限位约束的自运动范围，采用多个臂角区域描述该运动范围；

(4)采用本发明给出的方法构建仿人机械臂在关节限位下自运动形成的扫略体，该实例中采用多个包含5个点和1个半径的球扫略凸体表示自运动范围的碰撞检测模型；

(5)根据正运动学和机器人三维模型构建仿人机器人其他部分的基于球扫掠凸体碰撞检测模型，建立碰撞检测对；

(6)采用基于球扫掠凸体的碰撞检测方法计算各个碰撞检测对之间的最小距离以及可能的碰撞点，根据碰撞点计算出引起碰撞的机械臂自运动臂构型，剔除相应的臂角，获得在生长点处满足关节限位和避碰撞的所有可用臂构型，即可用臂角范围；

(7)根据获得的可用的臂角范围和过渡条件确定新生长点的有效性，如果新生长点可用，加入树中；否则，重新采样。从而获得一点到达期望位姿的安全末端轨迹。对于采样、双向搜索以及局部路径优化等步骤；

(8)引入基于平滑样条的路径平滑方法进行路径平滑处理，解决RRT*规划的路径是分段的线性路径,存在锯齿和不光滑的问题。完成仿人机械臂在任务空间的仿人路径规划；

(9)保存末端路径对应的可用臂构型集，将在该空间内进行臂构型的优化；

(10)根据机械臂初始构型和任务需要从获得的可用臂构型集中选择优化臂角区域；

(11)采用操作椭球优化在优化臂角区域内获得拟人的优化臂角；

(12)处理不可用的优化臂角，防止机械臂运动的不连续；

(13)如果需要臂角区域的过渡，则进行两个臂角区域过渡，修改过渡点附近的优化臂角；

(14)根据获得优化臂角，代入基于臂角参数的七自由度仿人机械臂解析逆运动学方程，获得连续的各个关节角度。

实施例二：本实施例是对本实施例一所述的七自由度仿人机械臂的双层拟人运动规划方法作进一步限定。

本实施例所述的七自由度仿人机械臂的双层拟人运动规划方法，步骤D包括：

步骤a、判断第四关节是否超限，如果第四关节超限，则新生长路径点无效，否则，执行步骤b；

步骤b、判断可用臂构型集是否为空，如果可用臂构型集为空，则新生长路径点无效，否则，执行步骤c；

步骤c、判断该新生长路径点与前一个生长路径点是否在同一个臂角区域内，如果在同一个臂角区域内，则新生长路径点有效，否则，执行步骤d；

步骤d、判断新生长路径点所在的臂角区域与前一个生长路径点所在的臂角区域是否存在过渡点，如果存在过渡点，则新生长路径点有效，否则，新生长路径点无效。

虽然在本文中参照了特定的实施方式来描述本发明，但是应该理解的是，这些实施例仅是本发明的原理和应用的示例。因此应该理解的是，可以对示例性的实施例进行许多修改，并且可以设计出其他的布置，只要不偏离所附权利要求所限定的本发明的精神和范围。应该理解的是，可以通过不同于原始权利要求所描述的方式来结合不同的从属权利要求和本文中所述的特征。还可以理解的是，结合单独实施例所描述的特征可以使用在其他所述实施例中。

Claims (5)

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

所述方法包括：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

否则，执行步骤一二一；

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

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

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

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

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

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

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

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

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

3.如权利要求2所述的七自由度仿人机械臂的双层拟人运动规划方法，其特征在于，步骤一二一所述的基于臂构型描述的末端新生长路径点有效性判断方法包括：

步骤A、将仿人机械臂的任务空间内的随机采样点假设为仿人机械臂末端新生长路径点，计算仿人机械臂在该新生长路径点下的满足仿人机械臂关节限位关系的可用臂构型集；

该步骤的具体过程为：

以第二关节为坐标原点建立三维基坐标，给定所述仿人机械臂末端新生长路径点相对于所述三维基坐标的期望位置和期望姿态，通过建立基于臂角的仿人机械臂运动学模型获得各个关节角与臂角的关系；

第一关节至第七关节依次分布在所述仿人机械臂的基座与末端执行器之间；

根据各个关节角与臂角的关系，将第二关节和第六关节的运动范围分别分为正负两个区域，进而形成四个臂角区域；

所述四个臂角区域分别为：

第一臂角区域，满足第二关节角和第六关节角同时小于0；

第二臂角区域，满足第二关节角小于0，第六关节角大于0；

第三臂角区域，满足第二关节角大于0，第六关节角小于0；

第四臂角区域，满足第二关节角和第六关节角同时大于0；

分别计算第一臂角区域、第二臂角区域、第三臂角区域和第四臂角区域内所述仿人机械臂的可用臂角范围，从而计算出所述仿人机械臂在该新生长路径点下的满足仿人机械臂关节限位关系的可用臂构型集；

步骤B、建立仿人机械臂在该新生长路径点下的基于球扫掠凸体的碰撞检测模型，计算出该新生长路径点下同时满足仿人机械臂关节限位和避碰撞约束的可用臂构型集；

该步骤的具体过程为：

建立仿人机械臂在关节限位下自运动形成的扫掠体的基于球扫掠凸体的碰撞检测模型：

当仿人机械臂自运动时，其上臂和下臂分别形成圆锥面扫掠体，根据第四关节自运动形成的轨迹，采用n个球扫掠凸体构建所述圆锥面扫掠体；

上臂包括第一关节至第四关节，下臂包括第四关节至第七关节，n为正整数；

所述球扫掠凸体由球半径和五个顶点构成，该球扫掠凸体定义为：

其中，r为球扫掠凸体V₁的半径，球扫掠凸体V₁的五个顶点分别为至为第一关节、第二关节与第三关节的旋转轴线的交点或者第五关节、第六关节与第七关节的旋转轴线的交点，为第四关节的位置， 是自运动旋转/2n的点，是自运动旋转/n的点， 为自运动旋转的角度；

或者，所述球扫掠凸体由球半径和四个顶点构成，该球扫掠凸体定义为：

其中，r为球扫掠凸体V₂的半径，球扫掠凸体V₂的四个顶点分别为至 是自运动旋转/n的点，为自运动旋转的角度；

分别建立仿人机械人中除机械臂外的其他部件和周围静止环境的基于球扫掠凸体的碰撞检测模型；

采用基于球扫掠凸体的碰撞检测方法检测仿人机械臂自运动形成的扫掠体的基于球扫掠凸体的碰撞检测模型与仿人机械人的其他部件以及周围静止环境的基于球扫掠凸体的碰撞检测模型是否发生碰撞以及是否存在可能的碰撞点，剔除可能产出碰撞的臂构型，获得仿人机械臂在该新生长路径点下同时满足仿人机械臂关节限位关系和避碰撞约束的可用臂构型集；

步骤C、计算所述四个臂角区域中任意两个臂角区域之间的过渡条件；

步骤D、根据仿人机械臂在该新生长路径点下同时满足仿人机械臂关节限位关系和避碰撞约束的可用臂构型集以及所述任意两个臂角区域之间的过渡条件判断该新生长路径点的有效性。

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

步骤a、判断第四关节是否超限，如果第四关节超限，则新生长路径点无效，否则，执行步骤b；

步骤b、判断可用臂构型集是否为空，如果可用臂构型集为空，则新生长路径点无效，否则，执行步骤c；

步骤c、判断该新生长路径点与前一个生长路径点是否在同一个臂角区域内，如果在同一个臂角区域内，则新生长路径点有效，否则，执行步骤d；

步骤d、判断新生长路径点所在的臂角区域与前一个生长路径点所在的臂角区域是否存在过渡点，如果存在过渡点，则新生长路径点有效，否则，新生长路径点无效。

5.如权利要求1所述的七自由度仿人机械臂的双层拟人运动规划方法，其特征在于，步骤四采用冗余解优化方法在所有可用机械臂构型中选择出最优机械臂构型集，所述冗余解优化方法包括：

步骤四一、选择优化臂角区域；

步骤四二、在优化臂角区域内优化拟人臂角；

步骤四三、处理不可用拟人臂角；

步骤四四、臂角区域过渡；

步骤四五、采用基于臂角的解析逆运动学方法计算各关节角度。