CN108241339A - 仿人机械臂的运动求解和构型控制方法 - Google Patents

Abstract

本发明公开了一种仿人机械臂的运动求解和构型控制方法。本发明提出的运动求解方法利用该类机械臂的结构特点，在对应的人类手臂上求出机械臂的整体运动，然后映射回机械臂的关节空间进行执行。本发明提出的运动求解方法不依赖于仿人机械臂的实际关节布置，对于具有不同关节布置的、但是大臂和小臂的臂长相同的仿人机械臂，能产生一致的运动；同时保留了运动优化能力；从对任务的处理方式上模仿人类，能更好地产生类人的动作。本发明基于运动求解方法中的理论，提出了一种构型控制方法，能控制仿人机械臂的完整构型，并且保证了仿人机械臂从运动规划到运动控制的一致性。

Description

仿人机械臂的运动求解和构型控制方法

技术领域

本发明属于机器人领域，具体涉及一种仿人机械臂的运动求解和构型控制方法。

背景技术

仿人机械臂是一类特殊的机械臂，它是模仿人类手臂的结构进行设计的，具有S-R-S(球副-旋转副-球副)的结构，通常由7个旋转关节组成，如图1和图2所示。图中，S表示肩关节中心，对应肩关节处三个机械关节轴线的交点；E表示肘关节中心；W表示腕关节中心，对应腕关节处三个机械关节轴线的交点；z_i(i＝1,…,7)表示7个机械臂关节的轴线方向；l₁和l₂分别为大臂和小臂的臂长；{S}和{W}分别为肩关节和腕关节坐标系，其原点分别位于肩关节和腕关节中心。其中，前3个关节的轴线相交于一点，构成一个球副，是机械臂的肩关节；第4个关节作为机械臂的肘关节；最后3个关节的轴线也相交于一点，同样构成一个球副，是机械臂的腕关节。S-R-S的结构可以有不同的实施方式，即机械臂的关节布置可以不同。然而，由于模仿的都是人类手臂，具有不同关节布置的、但是大臂和小臂臂长相同的仿人机械臂应该被看作是相同的机械臂。所以，在对仿人机械臂进行操作空间轨迹规划时，运动求解方法需要保证得到的运动对于相同的机械臂是一致的。另外，一些场景要求仿人机械臂能严格地执行规划的运动，即需要对机械臂进行构型控制，完全地跟踪规划的运动。例如，仿人机械臂在与人协作时，会在规划阶段完成避碰验证；为了确保无碰撞，机械臂需要严格按照规划的运动运行。由于仿人机械臂的特殊性，机械臂在规划和调整运动时最好能保持前后一致性，以符合人类的动作特性。所以，规划过程中用到的理论应该能应用于构型控制中。

运动求解是机械臂操作空间轨迹规划中的中心问题之一，需要根据末端执行器的运动求解出对应的各关节的运动。由于仿人机械臂具有7个自由度，而操作空间内的任务需求是6个自由度，机械臂具有一个冗余自由度，因此存在无穷多组解。对于仿人机械臂而言，难点不在于求出一组可行的解来，而是在于求出能保证相同的机械臂的运动一致的解。这里的一致是指末端执行器的运动是相同的，同时机械臂臂形的变化也是相同的。目前，对机械臂的运动求解方法有很多种，这些方法均是通用的方法，可以应用到仿人机械臂上。一种方法是广义逆方法，即建立机械臂的雅克比矩阵，利用雅克比矩阵的广义逆来求解机械臂的关节运动，如参考文献[1]：CHAN T F,DUBEY R V.A weighted least-norm solutionbased scheme for avoiding joint limits for redundant joint manipulators[J].IEEE transactions on Robotics and Automation,1995,11(2):286–292。这种方法能保证末端执行器按照任务需求运动。但是，雅克比矩阵是与机械臂的实际关节布置紧密相关的，对于不同关节布置的仿人机械臂，它们的雅克比矩阵各不相同。因此，仿人机械臂会产生不同的臂形变化曲线，即无法满足运动的一致性。另一种方法是引入额外的参数，消除机械臂的冗余性，如参考文献[2]：TONDU B.A closed-form inverse kinematic modellingof a 7R anthropomorphic upper limb based on a joint parametrization[C]Proceedings of the 6th IEEE-RAS International Conference on Humanoid Robots,2006,5:390–397中采用了大臂伸展角和臂形角两种参数化方式。由于机械臂被完全限制住，这种方法能保证运动的一致性。但是，这种方法需要根据引入的参数定义额外的任务需求，因而增加了运动规划的工作量。另外，它也使得机械臂丧失了运动优化的能力。

运动规划完成后，机械臂的运动实际上已经完全确定了。一些场景要求仿人机械臂能严格地执行规划的运动，即需要对机械臂进行构型控制，完全地跟踪规划的运动。关节空间能完整描述机械臂的运动，因此构型控制可以在关节空间中进行。这种构型控制方法是根据关节的误差来控制机械臂的运动的。因为关节误差与实际需要的操作空间的任务需求没有直接的关系，所以末端执行器的运动跟踪性能无法得到有效的保障。另外，由于运动求解不能在关节空间中进行，因此无法保证运动规划和控制的前后一致性。另一种方法是引入额外参数以描述机械臂的完整运动，通过末端执行器的位姿参数和额外参数与关节空间的严格数学关系来设计控制器，实现构型控制，如参考文献[3]：SERAJIH.Configuration control of redundant manipulators:Theory and implementation[J].IEEE Transactions on Robotics and Automation,1989,5(4):472–490。这种方法没有考虑仿人机械臂的结构特点；另外，如上所述，它所对应的运动求解方法具有增加运动规划工作量和使机械臂丧失优化能力等缺点。

发明内容

针对现有方法中存在的问题，本发明提出一种仿人机械臂的运动求解和构型控制方法。本发明提出的运动求解方法不是从机械臂的运动学模型出发，而是利用机械臂的结构特点，将操作空间中的运动需求与机械臂所对应人类手臂的生理关节关联起来，在对应的人类手臂上完成运动的求解，然后映射回机械臂的关节空间中。因此，本发明提出的运动求解方法对于不同关节布置的仿人机械臂能产生一致的运动。另外，本发明提出的运动求解方法保留了机械臂的运动优化能力。本发明进而将运动求解方法中的理论应用于仿人机械臂的构型控制中，能完全跟踪机械臂的运动，同时保持了运动规划和控制的前后一致性。

一种仿人机械臂的运动求解方法，包括以下步骤：

步骤一：将操作空间中的任务需求与机械臂的生理关节进行关联。

人类在控制末端工具的运动时，注意力聚焦在工具坐标系上，一般希望能直接影响到工具坐标系的位置和姿态。因此，在肩关节处建立虚拟关节Δ-P，Δ-A_s和Δ-L，构成一个球型机器人，以控制工具坐标系的位置。其中，Δ-P和Δ-A_s为转动副，Δ-L为移动副。同时，由于工具坐标系和腕关节坐标系的相对位姿是固定的，所以对工具坐标系的速度要求V_T可以转换到对腕关节坐标系的速度要求V_W上。这样，轨迹规划的任务需求与机械臂的生理关节关联起来了。

步骤二：对腕关节的线速度进行分解，确定肩关节和肘关节的运动。

保留步骤一中的肩关节处虚拟关节Δ-P和Δ-A_s的影响。Δ-P和Δ-A_s贡献了腕关节线速度v_W的一部分，剩余的部分v_W,rem由大臂和小臂实现。将剩余线速度v_W,rem沿肩-腕连线SW进行分解，其中，垂直分量由肩关节处的虚拟关节Δ-M实现；平行分量有肩关节和肘关节处的虚拟关节Δ-S和Δ-E共同实现。

此时，仿人机械臂的肩关节和肘关节的角速度确定了。肩关节的角速度ω_shoulder来源于：Δ-P，Δ-A_s，Δ-M和Δ-S；肘关节的角速度ω_elbow来源于：Δ-E。

步骤三：确定腕关节的运动。

步骤一到步骤二确定了肩关节和肘关节的运动，实现了腕关节坐标系的线速度需求。腕关节坐标系的角速度需求ω_W由肩、肘和腕关节共同决定。因此，排除肩关节和肘关节运动的影响，可求得腕关节的运动，即有

ω_Wrist＝ω_W-ω_shoulder-ω_elbow

步骤四：根据具体的关节布置，将生成的运动映射回机械臂的关节空间上。

在忽略机械臂的具体关节布置的前提下，步骤一到步骤四已经完全确定了机械臂的运动。根据机械臂的具体关节布置，将该运动映射回机械臂的关节空间上，以便于机械臂执行所需的规划任务。

所述的运动求解方法在步骤二与步骤三之间，可以添加一个可选的步骤，用于优化机械臂的运动。

机械臂具有7个自由度，对于只有6个自由度的任务需求而言，机械臂具有1个冗余的自由度，可以用于优化。根据额外的任务需要，如避障等，可以让机械臂的大臂和小臂绕肩-腕连线SW转动，即引入虚拟关节Δ-A_ψ。这是机械臂的自运动，能调整机械臂的臂形，但不影响腕关节的线速度。

引入虚拟关节Δ-A_ψ后，会影响肩关节的角速度ω_shoulder，其来源变为：Δ-P，Δ-A_s，Δ-M，Δ-A_ψ和Δ-S。

所述的运动求解方法先将操作空间内末端执行器的运动映射到仿人机械臂所对应的人类手臂的生理关节的运动上；然后将生理关节的运动映射到仿人机械臂的关节空间上用于执行。

所述的运动求解方法通过在肩关节处建立虚拟球型机器人来控制工具坐标系的位置，用球坐标来代替笛卡尔坐标描述工具坐标系的线速度需求。

所述的运动求解方法在步骤一中，虚拟关节Δ-P的旋转方向w_P由末端工具的当前位置p_T和线速度v_T确定，w_P垂直于肩-工具连线ST和v_T所确定的平面；虚拟关节Δ-L的平移方向v_T沿着肩-工具连线SW，v_T垂直于w_P；虚拟关节Δ-A_s的旋转方向w_A,S与v_T和w_P保持正交关系。

所述的运动求解方法在步骤二中，虚拟关节Δ-M的旋转方向w_M由腕关节的当前位置p_W和剩余线速度的垂直分量确定，w_M垂直于肩-腕连线SW和所确定的平面；虚拟关节Δ-L的平移方向v_T沿着肩-工具连线SW，v_T垂直于w_P；虚拟关节Δ-S和Δ-E的旋转方向均垂直于大臂和小臂确定的平面，即与仿人机械臂的第4个关节的旋转轴平行。

所述的运动求解方法在步骤一确定了一组先导运动，即虚拟关节Δ-P和Δ-A_s对应的运动；步骤二及以下需要在保证先导运动的情况下进行。

所述的运动求解方法通过手臂瞬时的、绕肩-腕连线旋转的自运动来调整手臂的臂形。

一种仿人机械臂的构型控制方法，包含以下步骤：

步骤一、利用仿人机械臂的运动求解方法中提出的变量来描述动力学方程。

通常，机械臂的动力学方程由关节量来描述，即有

式中：M表示广义质量，C表示离心力和科氏力项，N表示重力项，τ为机械臂的关节力矩；

可以等价地用权利要求说明1和2所述的方法中提出的变量和来描述该动力学方程，即有

因而，可以采用与变量有关的量来设计控制器，以控制仿人机械臂的运动。

步骤二、参数化冗余自由度，建立误差模型。

运动规划得到的运动实际包含了仿人机械臂的所有运动。采用臂形角ψ参数化仿人机械臂的冗余自由度。这样，对于机械臂的构型控制变成了对末端工具的位姿和臂形角的控制。根据权利要求1所述的方法，利用球坐标来表示位置误差；姿态误差采用指数坐标来表示；臂形角误差为实际值和期望值之差。

步骤三、根据步骤一中的动力学方程和步骤二中的误差模型，对机械臂进行动力学控制。

根据新的动力学方程，基于计算力矩方法，设计出以下控制率

式中：分别表示末端期望的加速度(表示在球坐标上)和角加速度以及虚拟关节Δ-A_ψ期望的加速度；ω为末端当前的角速度；ω_d为末端期望的角速度；R为末端当前的姿态矩阵；R_d为末端期望的姿态矩阵；k_p,χ，k_p,ω，k_p,ψ，k_d,χ，k_d,ω，k_d,ψ为控制器的参数；χ_e，ζ_e，ψ_e分别为末端的位置误差(表示在球坐标上)和姿态误差以及臂形角的误差；为对应的速度误差。

所述的构型控制方法利用了之前提到的运动求解方法，动力学方程从原来的通过关节量描述变为通过一组新的变量来描述，即由和描述。

所述的构型控制方法通过球坐标来表示机械臂末端的位置误差；对于末端位置的控制，是通过控制在肩关节处建立的虚拟球型机械臂的运动来实现的。

所述的构型控制方法通过瞬时的、绕肩-腕轴线转动的自运动来控制手臂的臂形角ψ。

本发明具有以下优点：

1、本发明提出的运动求解方法求得的运动与仿人机械臂的具体关节布置无关，对于具有不同关节布置、但是大臂和小臂臂长相同的仿人机械臂，能产生一致的运动。

2、本发明提出的运动求解方法保留了仿人机械臂的优化能力。在满足操作空间运动要求的前提条件下，还可以针对额外的任务需求，对机械臂的运动进行优化。

3、本发明提出的运动求解方法通过模仿人类处理类似问题的方式，将任务需求与机械臂的生理关节关联起来，因此，产生的运动具有一定的类人动作特性。

4、本发明提出的构型控制方法能控制仿人机械臂的完整运动，从而保证了实际执行的运动与规划出来的运动是一致的。

5、本发明提出的构型控制方法利用了运动求解方法中的理论，使得仿人机械臂在规划和调整自身运动时能保持前后一致性。

6、本发明提出的构型控制方法考虑了人类调整臂形的方式，通过绕肩-腕连线的自运动来控制手臂臂形，更符合人类的行为特点。

附图说明

图1：一种仿人机械臂的关节布置示意图；

图2：人类手臂的结构示意图；

图3：本发明提出的运动求解方法和传统广义逆方法的路线图；

图4：末端运动与生理关节的关联示意图；

图5：末端运动在人类手臂上的实现示意图；

图6：本发明提出的运动求解方法产生的末端轨迹和终止构型；

图7：传统广义逆方法产生的末端轨迹和终止构型；

图8：本发明提出的运动求解方法和传统广义逆方法在两种不同关节布置仿人机械臂上产生的臂形角变化曲线；

图9：仿人机械臂由运动规划到构型控制的流程图；

图10：仿人机械臂的误差模型；

图11：阶跃响应中末端位置的变化曲线；

图12：阶跃响应中末端姿态的变化曲线；

图13：阶跃响应中臂形角的变化曲线；

图14：本发明提出的构型控制方法跟踪圆弧轨迹的实际轨迹图；

图15：轨迹跟踪中末端位置误差的变化曲线；

图16：轨迹跟踪中末端姿态误差的变化曲线；

图17：轨迹跟踪中臂形角误差的变化曲线。

具体实施方式

下面结合附图对本发明进行详细说明。

如图3所示，运动求解的目的是为了找到对应操作空间运动的一组关节空间解。传统的广义逆方法是通过关节空间到操作空间的雅克比矩阵的广义逆来求解的。由于雅克比矩阵依赖于机械臂的关节布置，因此对于不同关节布置的仿人机械臂，这种方法无法保证运动规划的一致性。本发明提出的运动求解方法首先将操作空间中的运动映射到人类手臂上，然后映射回关节空间中。因为运动在人类手臂上已经完全确定，所以求解的运动是与仿人机械臂的实际关节布置是无关的，从而能得到一致的运动。

本发明提出的运动求解方法通过以下步骤实现：

步骤一：将操作空间中的任务需求与机械臂的生理关节关联起来。

图4所示为人类手臂在操作空间中画出一条轨迹。其中，S、E和W分别表示手臂的肩、肘和腕关节中心，T表示末端工具中心；{S}为肩关节坐标系，是手臂的基坐标系，它的X轴指向前方，Z轴竖直向上，如图2所示；{W}为腕关节坐标系，它的X轴指向手指伸直方向，Z轴垂直于手掌面；{T}为末端工具坐标系。人类在画线时，注意力聚焦在工具坐标系{T}上，一般希望能直接影响到工具坐标系的位置和姿态。因此，在肩关节处建立虚拟关节Δ-P，Δ-A_s和Δ-L，构成一个球型机器人，以控制工具坐标系的位置，如图3所示。其中，Δ-P和Δ-A_s为旋转副，它们的轴线均通过肩关节中心，用单向箭头表示；Δ-L为平移副，它的平移方向也通过肩关节中心，用双向箭头表示。Π_P表示工作平面，由肩-工具连线ST和末端的线速度v_T确定。

Δ-P的轴线w_P垂直于平面Π_P，

Δ-L的移动方向v_L沿着肩-工具连线ST，

式中，表示肩-工具连线ST的单位向量。易知，w_P和v_L相互垂直。

Δ-A_s的轴线w_A,S同时垂直于w w_P和v v_L，即有

w_A,S＝w_P×v_L

虚拟关节Δ-P，Δ-A_s和Δ-L的参数确定后，可以建立它们的角速度与末端工具坐标系的线速度之间的关系。

式中，J_χ为雅克比矩阵；v_T为工具坐标系的期望线速度；为虚拟球型机器人的关节角速度，其中，和分别表示虚拟关节Δ-P，Δ-A_s和Δ-L的关节速度。

同时，由于工具坐标系和腕关节坐标系的相对位姿是固定的，所以对工具坐标系的速度要求V_T可以转换到对腕关节坐标系的速度要求V_W上。于是有

式中，v_W和ω_W为腕关节坐标系的线速度和角速度；ω_T为工具坐标系的期望角速度；p_WT为工具坐标系和腕关节坐标系的相对位置，在基坐标系下表示。

这样，对于工作坐标系的任务需求与机械臂的生理关节关联了起来。

步骤二：对腕关节的线速度进行分解，确定肩关节和肘关节的运动。

保留步骤一中的肩关节处虚拟关节Δ-P和Δ-A_s的影响。Δ-P和Δ-A_s贡献了腕关节线速度v_W的一部分，另一部分，即腕关节剩余线速度v_W,rem，由大臂和小臂实现。

v_W,rem＝v_W-(ω_P+ω_A,S)×SW

式中，ω_P和ω_A,S分别对应虚拟关节Δ-P和Δ-A_s的角速度；SW为从肩关节中心指向腕关节中心的矢量。

如图5所示，将剩余线速度v_W,rem沿肩-腕连线SW进行分解。其中，垂直分量由肩关节处的虚拟关节Δ-M实现。虚拟关节Δ-M的轴线垂直于肩-腕连线SW和垂直分量所确定的平面，虚拟关节Δ-M的关节角速度为

平行分量有肩关节和肘关节处的虚拟关节Δ-S和Δ-E共同实现。Δ-S和Δ-E均垂直于由大臂和小臂确定的人臂平面，Δ-S和Δ-E的角速度为

式中，ω_S和ω_E分别为虚拟关节Δ-S和Δ-E的关节角速度；SE^||表示从肩关节中心指向肘关节中心的矢量SE平行于SW的分量；EW为从肘关节中心指向腕关节中心的矢量；SW表示矢量SW对应的叉乘矩阵。

此时，仿人机械臂的肩关节和肘关节的角速度确定了。肩关节的角速度ω_shoulder来源于：Δ-P，Δ-A_s，Δ-M和Δ-S；肘关节的角速度ω_elbow来源于：Δ-E。根据角速度的叠加性，有

式中，为表达操作空间任务需求的一组新的变量，J_S,a为表示与生理肩关节角速度ω_shoulder之间关系的雅可比矩阵；J_E,a为表示与生理肘关节角速度ω_elbow之间关系的雅可比矩阵。

步骤三：确定腕关节的运动。

步骤一到步骤二确定了肩关节和肘关节的运动，实现了腕关节坐标系的线速度需求。腕关节坐标系的角速度需求ω_W由肩、肘和腕三个关节共同决定。因此，排除肩关节和肘关节运动的影响，即可求得腕关节的运动。

式中，J_W,a为表示与生理腕关节角速度ω_Wrist之间关系的雅可比矩阵，ω_shoulder、ω_elbow和ω_Wrist分别表示肩、肘和腕三个生理关节的角速度；

步骤四：根据具体的关节布置，将生成的运动映射回机械臂的关节空间上。

在忽略机械臂的具体关节布置的前提下，步骤一到步骤四已经完全确定了机械臂的运动。根据机械臂的具体关节布置，将该运动映射回机械臂的关节空间上，以便于机械臂执行所需的规划任务。这是一个1-1映射。

式中，为仿人机械臂的关节角速度；J_S,m为机械臂在肩关节处的3个关节到肩关节整体的角速度的雅克比矩阵；w₄为机械臂的第4个关节在当前时刻下的轴线方向；J_W,m为机械臂在腕关节处的3个关节到腕关节整体的角速度的雅克比矩阵。

在步骤二与步骤三之间，可以添加一个可选的步骤，以优化机械臂的运动。

机械臂具有7个自由度，对于只有6个自由度的任务需求而言，机械臂具有1个冗余的自由度，可以用于优化。根据额外的任务需要，如避障等，可以让机械臂的大臂和小臂绕肩-腕连线SW转动，即引入虚拟关节Δ-A_ψ，如图5所示。这是机械臂的自运动，能调整机械臂的臂形，但不影响腕关节的线速度。

Δ-A_ψ的轴线方向为

添加虚拟关节Δ-A_ψ后，会影响肩关节的角速度。ω_shoulder的来源变为：Δ-P，Δ-A_s，Δ-M，Δ-A_ψ和Δ-S。因此，有

式中，在基础上添加了一个额外变量，其中为虚拟关节Δ-A_ψ的关节速度。

从而可以得到新的映射关系

因此，可以通过控制对机械臂的运动进行优化。

图6和图7分别为通过本发明提出的运动求解方法和传统广义逆方法(即其中为机械臂的雅克比矩阵的Moore-Penrose广义逆，为机械臂的末端速度)进行圆弧轨迹规划得到的末端轨迹和终止时的构型。从图中可以看出，本发明提出的运动求解方法能实现圆弧路径规划。另外，传统广义逆方法得到的终止构型超出了人类手臂能到达的生理极限，而本发明提出的方法没有。因而，本发明提出的方法更适合进行拟人运动规划。图8为这两种方法应用于2种不同关节布置的仿人机械臂上所产生的臂形角变化曲线。可以看出，通过本发明提出的方法得到的臂形角变化曲线是完全重合的，即得到的机械臂的运动是完全一致的；而通过传统广义逆方法得到的臂形角变化曲线是不一样的，即它对于不同关节布置的仿人机械臂产生了不同的运动。

运动规划完成后，实际上得到了机械臂的完整运动。一些场景要求仿人机械臂能严格地执行规划的运动，即需要对机械臂进行构型控制，完全地跟踪规划的运动。例如，仿人机械臂在与人协作时，会在规划阶段完成避碰验证；为了确保无碰撞，机械臂需要严格按照规划的运动运行。图9所示为仿人机械臂从运动规划到构型控制的流程图。为了描述机械臂的完整构型，引入额外参数——臂形角ψ来参数化机械臂的冗余自由度。

机械臂的动力学方程为

式中，式中：M表示广义质量，C表示离心力和科氏力项，N表示重力项，τ为机械臂的关节力矩。

根据前面对于运动求解的分析，机械臂的关节角与有对应关系，因此动力学方程可以由一组新的变量来等价地表示，即有

基于计算力矩法，可以设计控制率为

式中，u为新的控制输入。带入***的动力学方程，可以得到闭环***的动力学方程。于是有

因而，可以利用与有关的量来设计机械臂的控制器。

下面分别对机械臂的位置、姿态和臂形角的控制器进行设计，然后合并为完整的构型控制器。

如图10所示，根据末端的期望位置p_d和速度v_d，可以确定虚拟关节Δ-P，Δ-A_s和Δ-L的参数。因而，实际位置p可以由一组球坐标表示，即有

l_L＝||ST||-||ST_d||

式中，sign(·)为符号函数，表示括号内变量的正负号；ST_d表示从肩关节中心指向工具坐标系期望位置的矢量；ST表示从肩关节中心指向工具坐标系当前位置的矢量；ST^⊥表示矢量ST垂直于SW的分量；ST^||表示矢量ST平行于SW的分量。

定义末端的位置和线速度误差为

χ_e＝χ-χ_d＝χ

则位置误差的动力学方程为

式中，k_p,p和k_d,p均为正定对角阵。以为上述误差***的状态变量，则该***会稳定在(0,0)，从而机械臂的末端位置误差也会稳定在(0,0)。

定义末端的姿态误差为

采用指数坐标来描述姿态，则有

ζ_e＝log(R_e)

式中，ζ_e为姿态误差矩阵R_e对应的指数坐标。

对上式进行微分，可以得到指数坐标的导数和姿态误差的角速度之间的关系。

式中，J(ζ_e)为末端的角速度到指数坐标微分的雅克比矩阵；表示R_e对应的指数映射的微分。

根据姿态误差矩阵，可以得到姿态误差的角速度为

对上式进行微分，可以得到角加速度。

则姿态误差的动力学***为

式中，k_p,ω和k_d,ω均为正定对角阵。以(ζ_e,ω_e)为上述误差***的状态，则姿态误差会稳定在(0,0)。

臂形角ψ为由大臂和小臂确定的人臂平面和过肩-腕连线SW的参考平面的夹角。定义臂形角为

式中，n_arm为人臂平面的发现方向；n_ref为参考平面的法线方向；为矢量WS对应的单位向量。

对臂形角进行微分，可以得到它与仿人机械臂的关节空间的关系。

式中，J_ψ为从关节空间到臂形角的雅克比矩阵，它是一个行向量。

定义臂形角误差为

ψ_e＝ψ-ψ_d

则臂形角的速度误差为

展开上式，有

式中，a和b均为标量。

臂形角的误差通过虚拟关节Δ-A_ψ对应的角速度来调节，因而臂形角误差的动力学

来调节，因而臂形角误差的动力学方程为

式中，k_p,ψ和k_d,ψ均大于0。以为上述误差***的状态，则臂形角误差会稳定在(0,0)。

综合以上分析，可以设计新的控制输入u为

则完整的控制律为

在该控制率的作用下，闭环***的误差会稳定在零点，即能完全跟踪仿人机械臂的构型变化。

图11、图12和图13为该控制器的阶跃响应曲线，其中图11对应位置，图12对应姿态，图13对应臂形角。从图中可以看出，机械臂最终会稳定在期望构型。图14为跟踪一条圆弧轨迹的实际末端轨迹图。实际轨迹几乎和期望轨迹重合，因此在该控制器的控制下，机械臂能够很好地跟踪期望轨迹。图15、16和17为跟踪圆弧轨迹过程中的误差。其中，图15对应位置误差，位置误差定义为实际末端位置到期望轨迹的距离；图16对应姿态误差，姿态误差定义为ζ_e的模；图17对应臂形角的误差。从图中可以看出，在跟踪过程中，误差较小，说明该控制器能很好地跟踪机械臂的构型变化。

Claims (7)

1.仿人机械臂的运动求解方法，包括以下几个步骤：

步骤一：将操作空间中的任务需求与机械臂的生理关节进行关联；

在肩关节处建立虚拟关节Δ-P，Δ-A_s和Δ-L，构成一个球型机器人，以控制工具坐标系的位置，其中，Δ-P和Δ-A_s为转动副，Δ-L为移动副，对工具坐标系的速度要求V_T能够转换到对腕关节坐标系的速度要求V_W上，则操作空间中的任务需求与机械臂的生理关节进行关联；

步骤二：对腕关节的线速度进行分解，确定肩关节和肘关节的运动；

腕关节线速度v_W的一部分由Δ-P和Δ-A_s实现，剩余部分v_W,rem由大臂和小臂实现，将剩余线速度v_W,rem沿肩-腕连线SW进行分解，其中，垂直分量由肩关节处的虚拟关节Δ-M实现，平行分量由肩关节和肘关节处的虚拟关节Δ-S和Δ-E共同实现；

步骤三：确定腕关节的运动；

腕关节坐标系的角速度需求ω_W由肩、肘和腕三个生理关节共同决定；因此，排除肩关节和肘关节运动的影响，得到腕关节的运动；

ω_Wrist＝ω_W-ω_shoulder-ω_elbow

式中：ω_shoulder、ω_elbow和ω_Wrist分别表示肩、肘和腕三个生理关节的角速度；

步骤四：根据具体的关节布置，将生成的运动映射回机械臂的关节空间上；

根据机械臂的具体关节布置，将该运动映射回机械臂的关节空间上，机械臂执行所需的规划任务。

2.根据权利要求1所述的仿人机械臂的运动求解方法，所述的步骤一中，具体为：

虚拟关节Δ-P的旋转方向w_P由末端工具的当前位置p_T和线速度v_T确定，w_P垂直于肩-工具连线ST和v_T所确定的平面；虚拟关节Δ-L的平移方向v_T沿着肩-工具连线ST，v_T垂直于w_P；虚拟关节Δ-A_s的旋转方向w_A,S与v_T和w_P保持正交关系。

3.据权利要求1所述的仿人机械臂的运动求解方法，所述的步骤二中，具体为：

确定仿人机械臂的肩关节和肘关节的角速度，肩关节的角速度ω_shoulder来源于：Δ-P，Δ-A_s，Δ-M和Δ-S；肘关节的角速度ω_elbow来源于：Δ-E；

虚拟关节Δ-M的旋转方向w_M由腕关节的当前位置p_W和剩余线速度的垂直分量确定，w_M垂直于肩-腕连线SW和所确定的平面；虚拟关节Δ-L的平移方向v_T沿着肩-工具连线SW，v_T垂直于w_P；虚拟关节Δ-S和Δ-E的旋转方向均垂直于大臂和小臂确定的平面，即与仿人机械臂的第4个关节的旋转轴平行。

4.根据权利要求1所述的仿人机械臂的运动求解方法，所述的步骤二与步骤三之间，添加优化步骤，具体为：

机械臂具有7个自由度，对于6个自由度的任务，机械臂具有1个冗余的自由度，根据任务需要，让机械臂的大臂和小臂绕肩-腕连线SW转动，即引入虚拟关节Δ-A_ψ；

Δ-A_ψ的轴线方向为

添加虚拟关节Δ-A_ψ后，肩关节的角速度；ω_shoulder的来源为：Δ-P，Δ-A_s，Δ-M，Δ-A_ψ和Δ-S；因此，有

式中，其中为虚拟关节Δ-A_ψ的关节速度。

5.根据权利要求4所述的仿人机械臂的运动求解方法，添加优化步骤后，所述的步骤四中的映射关系为：

式中，为仿人机械臂的关节角速度；J_S,m为机械臂在肩关节处的3个关节到肩关节整体的角速度的雅克比矩阵；w₄为机械臂的第4个关节在当前时刻下的轴线方向；J_W,m为机械臂在腕关节处的3个关节到腕关节整体的角速度的雅克比矩阵。

6.根据权利要求1所述的仿人机械臂的运动求解方法，所述的步骤四中的映射关系为：

式中，为仿人机械臂的关节角速度；J_S,m为机械臂在肩关节处的3个关节到肩关节整体的角速度的雅克比矩阵；w₄为机械臂的第4个关节在当前时刻下的轴线方向；J_W,m为机械臂在腕关节处的3个关节到腕关节整体的角速度的雅克比矩阵。

7.仿人机械臂的构型控制方法，包括以下几个步骤：

步骤一、采用来描述动力学方程；

机械臂的动力学方程由关节量来描述，即有

式中：M表示广义质量，C表示离心力和科氏力项，N表示重力项，τ为机械臂的关节力矩；

采用变量和来描述该动力学方程，即有

步骤二、参数化冗余自由度，建立误差模型；

采用臂形角ψ参数化仿人机械臂的冗余自由度，采用球坐标表示位置误差；采用指数坐标表示姿态误差；臂形角误差为实际值和期望值之差；

步骤三、根据步骤一中的动力学方程和步骤二中的误差模型，对机械臂进行动力学控制；

基于计算力矩方法，根据新的动力学方程，获取控制率为：

式中：分别表示末端期望的加速度和角加速度以及虚拟关节Δ-A_ψ期望的加速度；ω为末端当前的角速度；ω_d为末端期望的角速度；R为末端当前的姿态矩阵；R_d为末端期望的姿态矩阵；k_p,χ，k_p,ω，k_p,ψ，k_d,χ，k_d,ω，k_d,ψ为控制器的参数；χ_e，ζ_e，ψ_e分别为末端的位置误差和姿态误差以及臂形角的误差；为对应的速度误差。