CN106584461A - 多约束条件下七自由度仿人机械臂的逆运动学拟人臂构型优化方法 - Google Patents

Abstract

多约束条件下七自由度仿人机械臂的逆运动学拟人臂构型优化方法，属于机械臂控制领域，解决了现有基于位置的逆运动学方法无法完整描述仿人机械臂的可用臂构型、末端可达位置减少、运动易跳变和无法获得满足多约束条件下拟人臂构型的问题。所述方法首先计算出沿给定轨迹下避关节限位和碰撞的所有可用臂构型集，随后在所有可用臂构型集中采用操作椭球优化获得拟人臂构型，引入机械臂自运动处理不可用优化臂角和臂角区域间的过渡，将一个受六个等式和多个不等式约束的七维关节空间中寻找优化关节的问题转化为一个一维的优化问题，本发明用于七自由度仿人机械臂的拟人臂构型优化。

Description

多约束条件下七自由度仿人机械臂的逆运动学拟人臂构型优 化方法

技术领域

本发明涉及一种多约束条件下七自由度仿人机械臂的逆运动学拟人臂构型优化方法，属于机械臂控制领域。

背景技术

七自由度仿人机械臂在执行任务的过程中会受到关节限位、周围环境、自碰撞以及动态平衡等多个约束条件的制约，如何获得同时满足以上约束条件的最优的冗余解是七自由度机械臂运动学领域急需解决的重要问题，也是机械臂运动学研究中的一个难点。

目前，国内外众多研究机构都对多约束条件下七自由度仿人机械臂的逆运动学展开了研究，该研究可以分为两个方向：基于速度的逆运动学方法和基于位置的逆运动学方法。

基于速度的逆运动学方法主要包括梯度投影法、阻尼最小二乘法、加权最小范数法、广义加权最小范数法和变权法。梯度投影法可以通过嵌套使用的方式来解决多约束条件的问题，然而该方法只能在主任务的零空间内局部优化子任务，无法保证子任务的有效性。加权最小范数法只能够处理关节限位的约束，无法处理多约束条件的情况。广义加权最小范数法能够在仿人机械臂跟踪给定轨迹的过程中赋予其避障和满足关节限位的能力，并且试图将多个不同时发生的任务结合为一个子任务，进而增加处理多约束问题的能力，然而，该方法在通常情况下很难判断子任务是否会同时发生。变权法克服了广义加权最小范数法很难判断子任务是否会同时发生的问题，该方法在仿人机械臂的运动过程中只考虑主任务和活动的约束，但是该方法只能处理一些简单的碰撞问题。基于速度的逆运动学方法通常会受到误差和奇异的困扰，既无法对仿人机械臂的零空间进行完整的描述，也无法处理复杂环境下的碰撞问题，因此，该类方法更适用于简单环境下七自由度仿人机械臂的实时控制。

基于位置的逆运动学方法采用几何分析的方式直接获得特定构型机械臂的解析解，与基于速度的逆运动学方法相比，该类方法的计算量小，通用性较差，但是在仿人机械臂上存在优势。于2008年提出的封闭条件下仿人机械臂逆运动学解法给出了一种定义关节限位下可用臂构型的求解方法，但是该方法较复杂，不易实现，同时未考虑碰撞的影响。该方法虽然指出基于位置的逆运动学方法存在末端可达位置少和运动易跳变的问题，但是并未给出有效的解决办法。2010年提出了一种有关节偏置的七自由度机械臂Barrett WAM逆运动学方法，指出正切类的关节对其零空间的限制是无法获得的。2013年提出了一种使用运动基元来描述仿人机械臂运动的方法，并给出了关于运动基元的逆运动学公式，使用人臂三角获得唯一的冗余解，但该方法并未考虑关节限位和碰撞的影响。现有基于位置的逆运动学方法已经明确地给出基于臂角参数的运动学方程，正试图解决关节限位和碰撞约束对逆解的影响，然而其存在无法完整描述可用臂构型、末端可达位置减少、运动易跳变和无法获得满足多约束条件下拟人臂构型的问题。

发明内容

本发明为解决现有基于位置的逆运动学方法无法完整描述仿人机械臂的可用臂构型、末端可达位置减少、运动易跳变和无法获得满足多约束条件下拟人臂构型的问题，提出了一种多约束条件下七自由度仿人机械臂的逆运动学拟人臂构型优化方法。

本发明的原理：通过观察人类手臂的运动，发现任意一个给定末端位姿，手臂都能绕腕关节和肩关节的连线旋转形成自运动，产生多个可用臂型。而自运动的范围取决于手臂关节限位和人体的碰撞，也就是说已知手臂的关节运动范围、人体的形状以及周围环境，沿期望轨迹下手臂的所有自运动范围是可以确定的。在该自运动范围内优化可获得同时满足关节限位和避碰撞两个约束的最优臂构型。将这种思路引入仿人机械臂的解析逆运动学中，提出了一种多约束条件下七自由度仿人机械臂的逆运动学拟人臂构型优化方法，如图1所示。

常用的仿人机械臂冗余解问题通过优化罚函数来确定唯一的解，因此先前的大多数方法使在速度域中解决该问题，而本实施例希望在位置域上解决该问题。在位置域上基于罚函数的多约束冗余解问题总体可以描述为:

Minimize f(θ)

fk(θ)＝x^d

Subject to d(θ)≥0

θ^l≤θ≤θ^u

其中f(θ)是罚函数，fk(θ)是仿人机械臂的正运动学，x^d是期望的末端位姿，d(θ)≥0表示仿人机械臂和其他物体的最小距离大于0，θ^l≤θ≤θ^u表示关节限位。可见这个问题是在一个受6个等式和多个不等式约束的7维关节空间中寻找优化关节的问题。解决这种问题是非常困难的。本发明将多约束条件下仿人机械臂的冗余解问题简化为如下形式：

Minimine f(ψ)

Subject to ψ∈Ψ

其中Ψ是关节限位下和避碰撞的所有可用臂角范围，因此解冗余解只需要解决这个一维的优化问题，这明显简单于传统的冗余解法。

本发明所述的多约束条件下七自由度仿人机械臂的逆运动学拟人臂构型优化方法包括可用臂构型描述部分和拟人臂构型冗余解优化部分；

可用臂构型描述部分包括：

步骤一、根据仿人机械臂末端执行器的运动轨迹计算满足关节限位条件的仿人机械臂自运动的范围；

该步骤的具体过程为：

以第二关节为坐标原点建立三维基坐标，沿着所述运动轨迹给定仿人机械臂末端执行器相对于所述三维基坐标的期望位置和期望姿态，通过建立基于臂角的仿人机械臂运动学模型获得第一至第七关节角与臂角的关系；

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

根据第一至第七关节角与臂角的关系，将第二关节和第六关节的运动范围分别分为正负两个区域，进而将整个臂角区域分为四个臂角区域：

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

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

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

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

分别计算第一臂角区域、第二臂角区域、第三臂角区域和第四臂角区域内所述仿人机械臂的可用臂角范围，从而计算出所述仿人机械臂在所述运动轨迹下满足关节限位条件的自运动范围；

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

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

步骤四、采用基于球扫掠凸体的碰撞检测方法检测仿人机械臂自运动形成的扫掠体的基于球扫掠凸体的碰撞检测模型与仿人机械人的其他部件以及周围静止环境的基于球扫掠凸体的碰撞检测模型之间是否发生碰撞以及是否存在可能的碰撞点，剔除发生碰撞和可能发生碰撞的臂构型，获得可用臂构型集；

拟人臂构型冗余解优化部分包括：

步骤五、在整个臂角区域内选择优化臂角区域，优化臂角区域为连续性最佳，范围最大和过渡次数最少的区域；

步骤六、基于操作椭球优化法在优化臂角区域内选择拟人臂构型；

步骤七、引入仿人机械臂自运动处理不可用优化臂角；

步骤八、当存在臂角区域的过渡时，在过渡点的两侧采用过渡点臂角的优化方法优化过渡点处的臂角；

步骤九、根据最优臂角区域的优化臂角曲线以及基于臂角参数的仿人机械臂解析逆运动学公式计算各个关节的解析逆解。

作为优选的是，步骤二采用球扫掠凸体构建仿人机械臂在关节限位下自运动形成的扫掠体的碰撞检测模型，当仿人机械臂自运动时，其上臂和下臂分别形成圆锥面扫掠体，根据第四关节自运动形成的轨迹，采用n个球扫掠凸体构建所述圆锥面扫掠体；

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

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

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

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

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

作为优选的是，步骤六采用操作椭球融合优化指标c在t_k+1时刻的优化的臂角范围为

其中dt是[t_k,t_k+1]的时间间隔。和分别是在t_k+1时刻优化臂角区域Ψ_regioni的上下限,ψ_current是t_k时刻的臂角，为最大臂角速度；

在t_k+1时刻的优化臂角具体步骤如下：

步骤1、设置臂角和dψ＝0.5°；

步骤2、计算臂角为ψ_k+1时的融合优化指标c；

步骤3、设置ψ_k+1＝ψ_k+1+dψ，再返回步骤2，迭代运行直到时停止迭代；

步骤4、在结果中寻找使融合优化指标c最大的臂角ψ_k+1为优化臂角。

作为优选的是，步骤七中当可用臂角范围的边界出现臂角变化较大的情况时，即机械臂以最大的臂角速度旋转也无法跟上边界的变化产生不可用臂角采用机械臂末端位置不动，机械臂肘关节绕肩关节和腕关节的连线转动调整臂角，使机械臂末端运动连续，能到达更远的位置。

作为优选的是，步骤八所述的过渡点臂角的优化方法为采用和过渡条件相关的余弦关节中最接近0°的臂角作为优化臂角，所述和过渡条件相关的余弦关节包括第二关节和第六关节；

或者，步骤八所述的过渡点臂角的优化方法包括：

步骤A、在实施步骤七后获得的优化臂角曲线上选择过渡的开始点和结束点，并计算开始点、过渡点和结束点对应的关节角；

步骤B、当实施步骤A所得的关节角对应的关节为正切关节时，将该关节作为规划关节，并采用线性插值法生成该关节在开始点、过渡点以及结束点之间的中间关节，该中间关节为规划后的关节；

步骤C、根据规划后的关节的角度反算出臂角，并将该臂角作为过渡区域的优化臂角。

作为优选的是，步骤六还能够根据运动捕捉系统实时采集的志愿者在复杂环境下手臂的运动数据并利用最小二乘理论确定人臂与臂角之间的关系，将该关系映射到仿人机械臂的优化臂角区域内选择拟人臂构型；

或者步骤六还能够根据能量最小原理在优化臂角区域内确定拟人臂构型。

本发明所述的多约束条件下七自由度仿人机械臂的逆运动学拟人臂构型优化方法通过步骤一至步骤四实现了仿人机械臂可用臂构型的描述，能够完整地获得沿仿人机械臂末端执行器运动轨迹的满足关节限位和碰撞约束的所有可用臂构型，解决了现有基于位置的逆运动学方法无法完整描述仿人机械臂的可用臂构型的问题；通过实施步骤五至步骤九实现了拟人冗余解优化，在可用臂构型中确定最优拟人臂构型，解决了现有基于位置的逆运动学方法存在的末端可达位置减少和运动易跳变的问题。

附图说明

在下文中将基于实施例并参考附图来对本发明所述的多约束条件下七自由度仿人机械臂的逆运动学拟人臂构型优化方法进行更详细的描述，其中：

图1是多约束条件下七自由度仿人机械臂的逆运动学拟人臂构型优化方法的框图，SSCH碰撞模型为基于球扫掠凸体的碰撞检测模型；

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

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

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

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

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

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

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

具体实施方式

下面将结合附图对本发明所述的多约束条件下七自由度仿人机械臂的逆运动学拟人臂构型优化方法作进一步说明。

实施例一：下面结合图2至图7详细地说明本实施例。本实施例所述的多约束条件下七自由度仿人机械臂的逆运动学拟人臂构型优化方法包括可用臂构型描述部分和拟人臂构型冗余解优化部分；

可用臂构型描述部分包括：

步骤一、根据仿人机械臂末端执行器的运动轨迹计算满足关节限位条件的仿人机械臂自运动的范围；

该步骤的具体过程为：

以第二关节为坐标原点建立三维基坐标，沿着所述运动轨迹给定仿人机械臂末端执行器相对于所述三维基坐标的期望位置和期望姿态，通过建立基于臂角的仿人机械臂运动学模型获得第一至第七关节角与臂角的关系；

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

根据第一至第七关节角与臂角的关系，将第二关节和第六关节的运动范围分别分为正负两个区域，进而将整个臂角区域分为四个臂角区域：

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

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

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

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

分别计算第一臂角区域、第二臂角区域、第三臂角区域和第四臂角区域内所述仿人机械臂的可用臂角范围，从而计算出所述仿人机械臂在所述运动轨迹下满足关节限位条件的自运动范围；

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

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

步骤四、采用基于球扫掠凸体的碰撞检测方法检测仿人机械臂自运动形成的扫掠体的基于球扫掠凸体的碰撞检测模型与仿人机械人的其他部件以及周围静止环境的基于球扫掠凸体的碰撞检测模型之间是否发生碰撞以及是否存在可能的碰撞点，剔除发生碰撞和可能发生碰撞的臂构型，获得可用臂构型集；

拟人臂构型冗余解优化部分包括：

步骤五、在整个臂角区域内选择优化臂角区域，优化臂角区域为连续性最佳，范围最大和过渡次数最少的区域；

步骤六、基于操作椭球优化法在优化臂角区域内选择拟人臂构型；

步骤七、引入仿人机械臂自运动处理不可用优化臂角；

步骤八、当存在臂角区域的过渡时，在过渡点的两侧采用过渡点臂角的优化方法优化过渡点处的臂角；

步骤九、根据最优臂角区域的优化臂角曲线以及基于臂角参数的仿人机械臂解析逆运动学公式计算各个关节的解析逆解；

步骤二采用球扫掠凸体构建仿人机械臂在关节限位下自运动形成的扫掠体的碰撞检测模型，当仿人机械臂自运动时，其上臂和下臂分别形成圆锥面扫掠体，根据第四关节自运动形成的轨迹，采用n个球扫掠凸体构建所述圆锥面扫掠体；

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

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

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

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

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

步骤六采用操作椭球融合优化指标c在t_k+1时刻的优化的臂角范围为

其中dt是[t_k,t_k+1]的时间间隔。和分别是在t_k+1时刻优化臂角区域Ψ_regioni的上下限,ψ_current是t_k时刻的臂角，为最大臂角速度；

在t_k+1时刻的优化臂角具体步骤如下：

步骤1、设置臂角和dψ＝0.5°；

步骤2、计算臂角为ψ_k+1时的融合优化指标c；

步骤3、设置ψ_k+1＝ψ_k+1+dψ，再返回步骤2，迭代运行直到时停止迭代；

步骤4、在结果中寻找使融合优化指标c最大的臂角ψ_k+1为优化臂角；

步骤七中当可用臂角范围的边界出现臂角变化较大的情况时，即机械臂以最大的臂角速度旋转也无法跟上边界的变化产生不可用臂角采用机械臂末端位置不动，机械臂肘关节绕肩关节和腕关节的连线转动调整臂角，使机械臂末端运动连续，能到达更远的位置；

步骤八所述的过渡点臂角的优化方法为采用和过渡条件相关的余弦关节中最接近0°的臂角作为优化臂角，所述和过渡条件相关的余弦关节包括第二关节和第六关节；

或者，步骤八所述的过渡点臂角的优化方法包括：

步骤A、在实施步骤七后获得的优化臂角曲线上选择过渡的开始点和结束点，并计算开始点、过渡点和结束点对应的关节角；

步骤B、当实施步骤A所得的关节角对应的关节为正切关节时，将该关节作为规划关节，并采用线性插值法生成该关节在开始点、过渡点以及结束点之间的中间关节，该中间关节为规划后的关节；

步骤C、根据规划后的关节的角度反算出臂角，并将该臂角作为过渡区域的优化臂角。

本实施例选用臂角作为冗余参数来描述仿人机械臂的自运动。图2是参考平面、臂平面和臂角的示意图，如图2所示，P_s、P_e和P_w分别为仿人机械臂的肩关节、肘关节和腕关节，ψ为臂角。图3是基于臂角的仿人机械臂运动学模型的示意图，如图3所示，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₇。图4是仿人机械臂自运动形成的扫略体以及由两个包含一个半径和五个顶点的球扫略凸体构建的碰撞检测模型示意图。图5是仿人机械臂自运动形成的扫略体以及由两个包含一个半径和四个顶点的球扫略凸体构建的碰撞检测模型示意图。

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

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

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

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

其中，

步骤三：球扫掠凸体可以使用更少的点构建出精确的碰撞检测模型，调整球扫掠体凸体的点集和半径以最小的体积包含机器人各构件以及周围环境的三维模型表面上的所有点。对于非凸体的构件将被分割成几个凸体来表示，能够有效地增加碰撞检测的实时性。

步骤四：基于球扫略凸体的碰撞检测方法通过采用GJK迭代算法计算仿人机械臂自运动形成的扫掠体的基于球扫掠凸体的碰撞检测模型与仿人机械人的其他部件以及周围静止环境的基于球扫掠凸体的碰撞检测模型之间的最短距离来确定是否发生碰撞。

GJK算法是一种基于单形体的下降算法，它输入任意两个凸体的顶点集，计算凸体间的欧式距离和最近的点。由凸体convP_i与半径r_i定义的球扫略凸体V(r_i；P_i)和由凸体convP_j与半径r_j定义的球扫略凸体V(r_j；P_j)的最短距离等于凸体convP_i和凸体convP_j的最短距离减去半径r_i和r_j。

dis(V(r_i；P_i),V(r_j；P_j))＝GJK_r(r_i；P_i；r_j；P_j)＝GJK(P_i；P_j)-r_i-r_j

球扫略凸体V(r_i；P_i)和V(r_j；P_j)上的最近点p_i,p_j为：

当两球扫掠凸体的距离dis(V(r_i；P_i),V(r_j；P_j))小于等于零时，认为两物体发生碰撞。剔除可能产出碰撞的臂构型，获得可用臂构型集。

步骤五：由于整个臂角区域被分成多个臂角区域，在臂角区域重叠部分一个臂角仍会解得多组冗余解。这就意味着即使通过冗余优化方法获得一个优化的臂角也有可能出现冗余多解的现象，这将引起选解的混乱，也是产生关节突变的一个原因。因此，本实施例首次将臂角区域作为参数引入逆运动学计算来进一步确定唯一的冗余解。优化臂角区域的选择直接影响到仿人机械臂的连续运动和运动范围。应尽可能地选择连续的、大范围的臂角区域作为优化臂角区域，同时也必须考虑机械臂当前所处的臂角区域，减少臂角区域间的过渡。

步骤六：操作椭球和力椭球优化可以使七自由度仿人机械臂获得拟人的臂型，因此引入操作椭球和力椭球优化在机械臂的一维可用臂构型集内优化一组满足多约束的仿人臂型。操作椭球的传递比表示了力和速度的放大倍数，而传递比的倒数表示力和速度的控制精度的放大倍数。在机械臂的操作任务中，可以通过最大化这些传递比来优化冗余机械臂的性能。如果希望沿方向向量u_i,i＝1,2,...,l控制力，沿方向向量u_j,j＝l+1,l+2,...,m控制速度，则定义融合优化指标c为：

式中w_i,w_j——为权值，表示在任务各方向上幅值或者精度的需求量；

公式的前半部分为力传递比的平方，公式的后半部分为速度传递比的平方。+表示任务要求控制沿期望方向的精度，而-表示任务要求控制沿期望方向的幅值。因此在操作任务中确定最优臂构型的问题将转换成在零空间内寻找臂角使融合指标c最大的问题

为了保证机械臂的连续运动，臂角的变化速度不能超过最大臂角速度假设ψ_k是在t_k时刻的臂角，则在t_k+1时刻的可能臂角范围为：

其中dt是[t_k,t_k+1]的时间间隔。和分别是在t_k时刻优化臂角区域Ψ_regioni的上下限，因此在t_k+1时刻的优化臂角可以通过如下步骤迭代获得：

步骤1、设置臂角和dψ＝0.5°；

步骤2、计算臂角为ψ_k+1时的融合优化指标c；

步骤3、设置ψ_k+1＝ψ_k+1+dψ，再返回步骤2，迭代运行直到时停止迭代；

步骤4、在结果中寻找使融合优化指标c最大的臂角ψ_k+1为优化臂角。

步骤七：由于可用臂角范围的边界可能出现臂角变化较大的情况，这样即使机械臂以最大的臂角速度旋转也无法跟上边界的变化，产生不可用臂角这种情况是引起机械臂末端可达位置减少的主要原因，因此在臂角变化较大的位置，将引入冗余机械臂的自运动来调整臂角，防止臂角的突变有效地解决该问题。在日常生活中，人类也会采用这种策略，在某些位置通过自运动来调整臂角，使手能到达更远的位置。最后，优化的臂角曲线应尽可能的平滑，避免过大的臂角变化导致计算的关节速度大于实际各关节的最大跟踪速度；其次，尽可能保证机械臂末端的运动方向的一致性，避免机械臂末端方向变化导致操作者对机械臂运动理解的困惑。

步骤八：为了保证仿人机械臂运动的连续，除了将臂角区域作为参数引入逆运动学计算，要求优化臂角连续外，还需要处理好臂角区域的过渡。要想机械臂保持末端轨迹从一个臂角区域连续运动到另一个臂角区域，必须解决两个问题：

(1)优化的臂角必须经过两臂角区域的过渡点，只有在过渡点才能使两个臂角区域对应的关节角度同时趋近于0°。

(2)关节角度相对于臂角的高变化率通常会出现在两个臂角区域的过渡点附近，原有的优化臂角很容易引起关节角度的跳变，因此在过渡点附近必须改变臂角的优化方法以防止高变化率引起的突变。

因此本实施例提出了两种过渡点臂角的优化方法：

方法1：

选用余弦关节(第二关节或第六关节)最接近0°的臂角作为过渡点附近的优化臂角。也就说在余弦关节大于0°的臂角区域，选择使余弦关节角度最小的臂角；在余弦关节小于0°的臂角区域，选择使余弦关节角度最大的臂角。对于余弦关节，当sinθ_i不为0°时，关节角度为整体最大和最小时的臂角可由如下公式给出：

如果臂角ψ^-和ψ⁺中一个对应余弦关节的角度最大，则另一个臂角对应的关节角度最小。

方法2：根据正切关节(第一关节、第三关节、第五关节或第七关节)来确定过渡点附近的优化臂角，该方法的包括：

步骤A、在实施步骤七后获得的优化臂角曲线上选择过渡的开始点和结束点，并计算开始点、过渡点和结束点对应的关节角；

步骤B、当实施步骤A所得的关节角对应的关节为正切关节时，将该关节作为规划关节，并采用线性插值法生成该关节在开始点、过渡点以及结束点之间的中间关节，该中间关节为规划后的关节；

步骤C、根据规划后的关节的角度反算出臂角，并将该臂角作为过渡区域的优化臂角。

步骤九：将以上步骤获得的优化臂角区域以及优化的臂角曲线代入如下基于臂角的解析逆运动学方程获得各个关节的角：

θ₁＝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)}

实施例二：本实施例是对实施例一所述的多约束条件下七自由度仿人机械臂的逆运动学拟人臂构型优化方法作进一步的限定。

本实施例所述的多约束条件下七自由度仿人机械臂的逆运动学拟人臂构型优化方法，步骤六还能够根据运动捕捉系统实时采集的志愿者在复杂环境下手臂的运动数据并利用最小二乘理论确定人臂与臂角之间的关系，将该关系映射到仿人机械臂的优化臂角区域内选择拟人臂构型；

或者步骤六还能够根据能量最小原理在优化臂角区域内确定拟人臂构型。

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

Claims (6)

1.多约束条件下七自由度仿人机械臂的逆运动学拟人臂构型优化方法，其特征在于，所述方法包括可用臂构型描述部分和拟人臂构型冗余解优化部分；

可用臂构型描述部分包括：

步骤一、根据仿人机械臂末端执行器的运动轨迹计算满足关节限位条件的仿人机械臂自运动的范围；

该步骤的具体过程为：

以第二关节为坐标原点建立三维基坐标，沿着所述运动轨迹给定仿人机械臂末端执行器相对于所述三维基坐标的期望位置和期望姿态，通过建立基于臂角的仿人机械臂运动学模型获得第一至第七关节角与臂角的关系；

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

根据第一至第七关节角与臂角的关系，将第二关节和第六关节的运动范围分别分为正负两个区域，进而将整个臂角区域分为四个臂角区域：

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

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

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

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

分别计算第一臂角区域、第二臂角区域、第三臂角区域和第四臂角区域内所述仿人机械臂的可用臂角范围，从而计算出所述仿人机械臂在所述运动轨迹下满足关节限位条件的自运动范围；

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

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

步骤四、采用基于球扫掠凸体的碰撞检测方法检测仿人机械臂自运动形成的扫掠体的基于球扫掠凸体的碰撞检测模型与仿人机械人的其他部件以及周围静止环境的基于球扫掠凸体的碰撞检测模型之间是否发生碰撞以及是否存在可能的碰撞点，剔除发生碰撞和可能发生碰撞的臂构型，获得可用臂构型集；

拟人臂构型冗余解优化部分包括：

步骤五、在整个臂角区域内选择优化臂角区域，优化臂角区域为连续性最佳，范围最大和过渡次数最少的区域；

步骤六、基于操作椭球优化法在优化臂角区域内选择拟人臂构型；

步骤七、引入仿人机械臂自运动处理不可用优化臂角；

步骤八、当存在臂角区域的过渡时，在过渡点的两侧采用过渡点臂角的优化方法优化过渡点处的臂角；

步骤九、根据最优臂角区域的优化臂角曲线以及基于臂角参数的仿人机械臂解析逆运动学公式计算各个关节的解析逆解。

2.如权利要求1所述的多约束条件下七自由度仿人机械臂的逆运动学拟人臂构型优化方法，其特征在于，步骤二采用球扫掠凸体构建仿人机械臂在关节限位下自运动形成的扫掠体的碰撞检测模型，当仿人机械臂自运动时，其上臂和下臂分别形成圆锥面扫掠体，根据第四关节自运动形成的轨迹，采用n个球扫掠凸体构建所述圆锥面扫掠体；

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

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

$V_1(r,\{p_1^0,{\left\{p_1^i\right\}}_{i=1}^4\left\}\right)$

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

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

$V_2(r,\{p_1^0,{\left\{p_1^i\right\}}_{i=1}^3\left\}\right)$

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

3.如权利要求1所述的多约束条件下七自由度仿人机械臂的逆运动学拟人臂构型优化方法，其特征在于，步骤六采用操作椭球融合优化指标c在t_k+1时刻的优化的臂角范围为

$\begin{array}{c}{\mathrm{\&Psi;}}_{k+1}=\&lsqb;{\mathrm{\&psi;}}_{k+1}^l,{\mathrm{\&psi;}}_{k+1}^u\&rsqb;=\\ \&lsqb;{\mathrm{\&psi;}}_{current}-{\stackrel{\&CenterDot;}{\mathrm{\&psi;}}}_{max}dt,{\mathrm{\&psi;}}_{current}+{\stackrel{\&CenterDot;}{\mathrm{\&psi;}}}_{max}dt\&rsqb;\&cap;\&lsqb;{\mathrm{\&psi;}}_{regioni}^l,{\mathrm{\&psi;}}_{regioni}^u\&rsqb;\end{array}$

其中dt是[t_k,t_k+1]的时间间隔。和分别是在t_k+1时刻优化臂角区域Ψ_regioni的上下限,ψ_current是t_k时刻的臂角，为最大臂角速度；

在t_k+1时刻的优化臂角具体步骤如下：

步骤1、设置臂角和dψ＝0.5°；

步骤2、计算臂角为ψ_k+1时的融合优化指标c；

步骤3、设置ψ_k+1＝ψ_k+1+dψ，再返回步骤2，迭代运行直到时停止迭代；

步骤4、在结果中寻找使融合优化指标c最大的臂角ψ_k+1为优化臂角。

4.如权利要求1所述的多约束条件下七自由度仿人机械臂的逆运动学拟人臂构型优化方法，其特征在于，步骤七中当可用臂角范围的边界出现臂角变化较大的情况时，即机械臂以最大的臂角速度旋转也无法跟上边界的变化产生不可用臂角采用机械臂末端位置不动，机械臂肘关节绕肩关节和腕关节的连线转动调整臂角，使机械臂末端运动连续，能到达更远的位置。

5.如权利要求1所述的多约束条件下七自由度仿人机械臂的逆运动学拟人臂构型优化方法，其特征在于，步骤八所述的过渡点臂角的优化方法为采用和过渡条件相关的余弦关节中最接近0°的臂角作为优化臂角，所述和过渡条件相关的余弦关节包括第二关节和第六关节；

或者，步骤八所述的过渡点臂角的优化方法包括：

步骤A、在实施步骤七后获得的优化臂角曲线上选择过渡的开始点和结束点，并计算开始点、过渡点和结束点对应的关节角；

步骤B、当实施步骤A所得的关节角对应的关节为正切关节时，将该关节作为规划关节，并采用线性插值法生成该关节在开始点、过渡点以及结束点之间的中间关节，该中间关节为规划后的关节；

步骤C、根据规划后的关节的角度反算出臂角，并将该臂角作为过渡区域的优化臂角。

6.如权利要求1所述的多约束条件下七自由度仿人机械臂的逆运动学拟人臂构型优化方法，其特征在于，步骤六还能够根据运动捕捉系统实时采集的志愿者在复杂环境下手臂的运动数据并利用最小二乘理论确定人臂与臂角之间的关系，将该关系映射到仿人机械臂的优化臂角区域内选择拟人臂构型；

或者步骤六还能够根据能量最小原理在优化臂角区域内确定拟人臂构型。