基于轴不变量的非理想关节机器人动力学建模方法

摘要

本发明公开了一种基于轴不变量的非理想关节机器人动力学与解算方法，对于非理想约束系统，建立了闭链刚体非理想约束系统的Ju‑Kane动力学方程。以关节空间自然轴链为基础的Ju‑Kane闭链刚体动力学克服了笛卡尔坐标轴链空间的局限：在基于笛卡尔坐标轴链的牛顿欧拉动力学中，非树运动副约束不能表达如齿条与齿轮、蜗轮与蜗杆等约束。而本申请的非树约束副的约束代数方程式可表达任一种约束类形，并且物理内涵明晰；降低了系统方程求解的复杂度；保证了约束方程的准确性。

权利要求

1.一种基于轴不变量的非理想关节机器人动力学建模方法，其特征是，给定 多 轴刚 体系 统D ＝ {A ,K ,T ,NT ,F ,B} ，惯 性 系记 为F [i ] ，A为轴序列，K为运动副类型序列，T表示带方向的span树，以描述树链运动的拓扑关系，NT为约束轴的序列即非树，F为杆件参考系序列，B为杆件体序列；除了重力外，作用于轴u的合外力及力矩在 上的分量分别记为 及 轴k的质量及质心转动惯量分别记为mk及 轴k的重力加速度为 驱动轴u的双边驱动力及驱动力矩在 上的分量分别记为 及 环境i对轴l的作用力及作用力矩分别为i及τl；轴u对轴u′的广义约束力记为

约定：在运动链符号演算系统中，具有偏序的属性变量或常量，在名称上包含表示偏序的指标；要么包含左上角及右下角指标，要么包含右上角及右下角指标；对于某种属性符，它们的方向总是由偏序指标的左上角指标至右下角指标，或由右上角指标至右下角指标；

定义不变量：称不依赖于一组坐标系进行度量的量为不变量；

对于轴l、 在系统处于零位时，所有杆件的自然坐标系与底座或世界系的方向一致；

系统处于零位即转动角度 时，自然坐标系 绕轴矢量 转动角度 将 转至F[l]； 在 下的坐标矢量与 在F[l]下的坐标矢量 恒等，即有[l]

或 不依赖于相邻的坐标系 及F ；故称 或 为轴不变量；

设运动轴u的广义内摩擦及粘滞的合力及合力矩分别为 根据建立的闭链刚体系统的Ju-Kane动力学方程，计算关节加速度 后，计算径向约束力大小 及 约束力矩大小 及 再建立如下闭链刚体非理想约束系统的Ju-Kane动力学方程：【1】轴u及轴u′的Ju-Kane动力学规范方程分别为式中： 及 是3×3的分块矩阵， 及 是3D矢量； 为转动副R下轴u的惯性矩阵； 为平动副P下轴u的惯性矩阵；hR为转动副R下轴u的非惯性矩阵；hP为平动副P下轴u的非惯性矩阵；【2】非树约束副uku′的约束代数方程为

2.根据权利要求1所述的基于轴不变量的非理想关节机器人动力学建模方法，其特征是，闭链刚体系统的Ju-Kane动力学方程：

【1】轴u及轴u′的Ju-Kane动力学规范方程分别为其中： 及 是3×3的分块矩阵， 及 是3D矢量；kI表示杆k质心I；轴k的质量及质心转动惯量分别记为mk及 为转动副R下轴u的惯性矩阵； 为平动副P下轴u的惯性矩阵；hR为转动副R下轴u的非惯性力；hP为平动副P下轴u的非惯性力；

【2】非树约束副uku′的约束代数方程为

其中：

式中： 及 是3×3的分块矩阵， 及 是3D矢量；kI表示杆k质心I；轴k的质量及质心转动惯量分别记为mk及 为转动副R下轴u的惯性矩阵； 为平动副P下轴u的惯性矩阵；hR为转动副R下轴u的非惯性矩阵；hP为平动副P下轴u的非惯性矩阵； 为平动关节角速度； 为转动关节角速度。

3.根据权利要求2所述的基于轴不变量的非理想关节机器人动力学建模方法，其特征是，应用式(129)至式(134)计算径向约束力大小 及 约束力矩大小 及 对于无功率损耗的运动轴u，记其约束力及约束力矩矢量分别为 则有上式表示运动轴矢量与运动轴约束力具有自然正交补的关系；

若 及 为运动副 的两个正交约束轴，且约束轴与运动轴正交，即记 为约束轴轴矢量，有

其中：

由式(130)得到关节约束力大小 及 约束力矩大小 及 若记运动轴径向力矢量及力矩矢量 则有若记运动轴径向力大小为 及力矩大小为 由式(133)得至此，完成了轴径向约束广义力的计算。

4.根据权利要求3所述的基于轴不变量的非理想关节机器人动力学建模方法，其特征是，由式(130)至式(134)计算运动轴u的径向约束力大小 及约束力矩大小 时，不考虑运动轴的广义内摩擦力及粘滞力。

5.根据权利要求3所述的基于轴不变量的非理想关节机器人动力学建模方法，其特征是，考虑广义内摩擦力及粘滞力的基于轴不变量的约束力求解步骤为：在完成轴径向约束广义力的计算后，得到运动轴u的径向约束力大小 及约束力矩大小 记运动轴u的内摩擦力大小及内摩擦力矩大小分别为 及 运动轴u的粘滞力及粘滞力矩大小分别为 及 则[u] [u]

其中：sk ─运动轴u的内摩擦系数，ck ─运动轴u的粘滞系数；sign()表示取正或负符号；

记广义内摩擦力及粘滞力的合力及合力矩分别为 由式(140)及式(141)得

6.根据权利要求1所述的基于轴不变量的非理想关节机器人动力学建模方法，其特征是，闭链刚体系统的Ju-Kane动力学方程根据树链Ju-Kane规范型方程建立。

7.根据权利要求6所述的基于轴不变量的非理想关节机器人动力学建模方法，其特征是，树链Ju-Kane规范型方程其中： 及 是3×3的分块矩阵， 及 是3D矢量； 为轴u的合外力在 上的分量， 为轴u的合力矩在 上的分量；

并且，

式中，kI表示杆k质心I；轴k的质量及质心转动惯量分别记为mk及 为转动副R下轴u的惯性矩阵； 为平动副P下轴u的惯性矩阵；hR为转动副R下轴u的非惯性矩阵；hP为平动副P下轴u的非惯性矩阵；作用于轴u的合外力及力矩在 上的分量分别记为 及驱动轴u的双边驱动力及驱动力矩在 上的分量分别记为 及 环境i对轴l的作用力及作用力矩分别为 及itl；llk为取由轴l至轴k的运动链，uL表示获得由轴u及其子树构成的闭子树。

说明书

基于轴不变量的非理想关节机器人动力学建模方法

技术领域

[0001] 本发明涉及一种非理想关节机器人动力学建模与解算方法，属于机器人技术领域。

背景技术

[0002] 拉格朗日在研究月球天平动问题时提出了拉格朗日方法，是以广义坐标表达动力学方程的基本方法；同时，也是描述量子场论的基本方法。应用拉格朗日法建立动力学方程已是一个烦琐的过程，尽管拉格朗日方程依据系统能量的不变性推导系统的动力学方程，具有理论分析上的优势；但是在工程应用中，随着系统自由度的增加，方程推导的复杂性剧增，难以得到普遍应用。凯恩方程建立过程与拉格朗日方程相比，通过系统的偏速度、速度及加速度直接表达动力学方程。故凯恩动力学方法与拉格朗日方法相比，由于省去了系统能量的表达及对时间的求导过程，极大地降低了系统建模的难度。然而，对于高自由度的系统，凯恩动力学建模方法也是难以适用。

[0003] 拉格朗日方程及凯恩方程极大地推动了多体动力学的研究，以空间算子代数为基础的动力学由于应用了迭代式的过程，计算速度及精度都有了一定程度的提高。这些动力学方法无论是运动学过程还是动力学过程都需要在体空间、体子空间、系统空间及系统子空间中进行复杂的变换，建模过程及模型表达非常复杂，难以满足高自由度系统建模与控制的需求，因此，需要建立动力学模型的简洁表达式；既要保证建模的准确性，又要保证建模的实时性。没有简洁的动力学表达式，就难以保证高自由度系统动力学工程实现的可靠性与准确性。同时，传统非结构化运动学及动力学符号通过注释约定符号内涵，无法被计算机理解，导致计算机不能自主地建立及分析运动学及动力学模型。

发明内容

[0004] 本发明所要解决的技术问题是提供一种基于轴不变量的非理想关节机器人动力学建模与解算方法。

[0005] 为解决上述技术问题，本发明采用以下技术方案：

[0006] 一种基于轴不变量的非理想关节机器人动力学建模方法，其特征是，

[0007] 给定多轴刚体系统D＝{A ,K ,T ,NT ,F ,B}，惯性系记为F[i]，除了重力外，作用于轴u的合外力及力矩在 上的分量分
别记为 及 轴k的质量及质心转动惯量分别记为mk及 轴k的重力加速度为 驱动
轴u的双边驱动力及驱动力矩在 上的分量分别记为 及 环境i对轴l的作用力
及作用力矩分别为 及iτl；轴u对轴u′的广义约束力记为

[0008] 设运动轴u的广义内摩擦及粘滞的合力及合力矩分别为 根据建立的闭链刚体系统的Ju-Kane动力学方程，计算关节加速度 后，计算径向约束力大小 及 约束力矩大小 及 再建立如下闭链刚体非理想约束系统的Ju-Kane动力学方程：

[0009] 【1】轴u及轴u′的Ju-Kane动力学规范方程分别为

[0010]

[0011]

[0012] 式中： 及 是3×3的分块矩阵， 及 是3D矢量； 为转动轴u的惯性矩阵； 为平动轴u的惯性矩阵；hR为转动轴u的非惯性矩阵；hP为平动轴u的非惯性矩阵；

[0013] 【2】非树约束副uku′的约束代数方程为

[0014]

[0015]

[0016]

[0017]

[0018] 闭链刚体系统的Ju-Kane动力学方程：

[0019] 【1】轴u及轴u′的Ju-Kane动力学规范方程分别为

[0020]

[0021]

[0022] 其中： 及 是3×3的分块矩阵， 及 是3D矢量；kI表示杆k质心I；轴k的质量及质心转动惯量分别记为mk及 为转动轴u的惯性矩阵； 为平动轴u的惯性矩阵；hR为转动轴u的非惯性力；hP为平动轴u的非惯性力。

[0023] 【2】非树约束副uku′的约束代数方程为

[0024]

[0025]

[0026]

[0027]

[0028] 其中：

[0029]

[0030]

[0031]

[0032]

[0033]

[0034] 式中： 及 是3×3的分块矩阵， 及 是3D矢量；kI表示杆k质心I；轴k的质量及质心转动惯量分别记为mk及 为转动轴u的惯性矩阵； 为平动轴u的惯性矩阵；hR为转动轴u的非惯性矩阵；hP为平动轴u的非惯性矩阵； 为平动关节角速度；
为转动关节角速度。

[0035] 应用式(129)至式(134)计算径向约束力大小 及 约束力矩大小 及 对于无功率损耗的运动轴u，记其约束力及约束力矩矢量分别为 则有

[0036]

[0037] 上式表示运动轴矢量与运动轴约束力具有自然正交补的关系；

[0038] 若 及 为运动副 的两个正交约束轴，且约束轴与运动轴正交，即

[0039]

[0040] 记 为约束轴轴矢量，有

[0041]

[0042] 其中：

[0043]

[0044]

[0045] 由式(130)得到关节约束力大小 及 约束力矩大小 及 若记运动轴径向力矢量 及力矩矢量 则有

[0046]

[0047] 若记运动轴径向力大小为 及力矩大小为 由式(133)得

[0048]

[0049] 至此，完成了轴径向约束广义力的计算。

[0050] 由式(130)至式(134)计算运动轴u的径向约束力大小 及约束力矩大小 时，不考虑运动轴的广义内摩擦力及粘滞力。

[0051] 考虑广义内摩擦力及粘滞力的基于轴不变量的约束力求解步骤为：

[0052] 在完成轴径向约束广义力的计算后，得到运动轴u的径向约束力大小 及约束力矩大小 记运动轴u的内摩擦力大小及内摩擦力矩大小分别为 及 运动轴u的粘滞力及粘滞力矩大小分别为 及 则

[0053]

[0054]

[0055] 其中：sk[u]─运动轴u的内摩擦系数，ck[u]─运动轴u的粘滞系数；sign()表示取正或负符号；

[0056] 记广义内摩擦力及粘滞力的合力及合力矩分别为 由式(140)及式(141)得

[0057]

[0058] 闭链刚体系统的Ju-Kane动力学方程根据树链Ju-Kane规范型方程建立。

[0059] 树链Ju-Kane规范型方程

[0060]

[0061] 其中： 及 是3×3的分块矩阵， 及 是3D矢量； 为轴u的合外力在上的分量， 为轴u的合力矩在 上的分量；

[0062] 并且，

[0063]

[0064]

[0065]

[0066]

[0067]

[0068]

[0069] 式中，kI表示杆k质心I；轴k的质量及质心转动惯量分别记为mk及 为转动轴u的惯性矩阵； 为平动轴u的惯性矩阵；hR为转动轴u的非惯性矩阵；hP为平动轴u的非惯性矩阵 ；作用于轴u的合外力及力矩在 上的分量分别记为 及作用于轴u的合外力及力矩在 上的分量分别记为 及
驱动轴u的双边驱动力及驱动力矩在 上的分量分别记为 及 环境i对轴l的
作用力及作用力矩分别为 及itl；llk为取由轴l至轴k的运动链，uL表示获得由轴u及其子树构成的闭子树。

[0070] 本发明所达到的有益效果：

[0071] 对于非理想约束系统，建立了闭链刚体非理想约束系统的Ju-Kane动力学方程。

[0072] 【1】在基于笛卡尔坐标轴链的牛顿欧拉动力学中，非树运动副uku′∈P约束不能表达齿条与齿轮、蜗轮与蜗杆等约束。而本申请建立的非树约束副uku′的约束代数方程可表达任一种约束类形，并且物理内涵明晰；

[0073] 【2】在基于笛卡尔坐标轴链的牛顿欧拉动力学当中，非树运动副代数约束方程是6D的；而本申请建立的非树约束副的约束代数方程表示是3D非树运动副代数约束方程，从而降低了系统方程求解的复杂度；

[0074] 【3】在基于笛卡尔坐标轴链的牛顿欧拉动力学当中，非树运动副代数约束方程是关于6D矢量空间绝对加速度的，是关于关节坐标、关节速度的迭代式，具有累积误差；而本申请建立的非树约束副的约束代数方程是关于关节加速度的，保证了约束方程的准确性。

附图说明

[0075] 图1自然坐标系与轴链；

[0076] 图2固定轴不变量；

[0077] 图3、图4为运动轴的内摩擦力及粘滞力示意图。

具体实施方式

[0078] 下面对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。

[0079] 闭链刚体系统具有非常广泛的应用；比如，CE3巡视器的摇臂移动系统是具有差速器的闭链，重载机械臂通常是具有四连杆的闭链系统。同时，实际的运动轴通常包含内摩擦力及粘滞力。因此研究闭链刚体系统的Ju-Kane动力学建模非常必要。

[0080] 描述运动链的基本拓扑符号及操作是构成运动链拓扑符号系统的基础，定义如下：

[0081] 【1】运动链由偏序集合(]标识。

[0082] 【2】A[l]为取轴序列A的成员；因轴名l具有唯一的编号对应于A[l]的序号，故A[l]计算复杂度为O(1)。

[0083] 【3】 为取轴l的父轴；由式 可知，计算复杂度为O(1)。

[0084] 【4】 为取轴序列 的成员；由式 可知，故 计算复杂度为O(1)。

[0085] 【5】llk为取由轴l至轴k的运动链，输出表示为 且 基数记为|llk|。llk执行过程：执行 若 则执行 否则，结束。llk计算复杂度为O(|llk|)。

[0086] 【6】ll为取轴l的子。该操作表示在 中找到成员l的地址k；从而，获得轴l的子A[k]。因 不具有偏序结构，故ll的计算复杂度为

[0087] 【7】lL表示获得由轴l及其子树构成的闭子树，lL为不含l的子树；递归执行ll，计算复杂度为

[0088] 【8】支路、子树及非树弧的增加与删除操作也是必要的组成部分；从而，通过动态Span树及动态图描述可变拓扑结构。在支路llk中，若 则记即 表示在支路中取成员m的子。

[0089] 计算复杂度O()表示计算过程的操作次数，通常指浮点乘与加的次数。以浮点乘与加的次数表达计算复杂度非常烦琐，故常采用算法循环过程中的主要操作次数；比如：关节位姿、速度、加速度等操作的次数。

[0090] 定义1自然坐标轴：称与运动轴或测量轴共轴的，具有固定原点的单位参考轴为自然坐标轴，亦称为自然参考轴。

[0091] 定义2自然坐标系：若多轴系统D处于零位，所有笛卡尔体坐标系方向一致，且体坐标系原点位于运动轴的轴线上，则该坐标系统为自然坐标系统，简称自然坐标系。

[0092] 自然坐标系优点在于：(1)坐标系统易确定；(2)零位时的关节变量为零；(3)零位时的系统姿态一致；(4)不易引入测量累积误差。

[0093] 定义3不变量：称不依赖于一组坐标系进行度量的量为不变量。

[0094] 由定义2可知，在系统处于零位时，所有杆件的自然坐标系与底座或世界系的方向一致。系统处于零位即 时，自然坐标系 绕轴矢量 转动角度 将 转至F[l]； 在 下的坐标矢量与 在F[l]下的坐标矢量 恒等，即有

[0095]

[0096] 由上式知， 或 不依赖于相邻的坐标系 及F[l]；故称 或 为轴不变量。在不强调不变性时，可以称之为坐标轴矢量(简称轴矢量)。 或 表征的是体 与体l共有的参考单位坐标矢量，与参考点 及Ol无关。

[0097] 对轴不变量而言，其绝对导数就是其相对导数。因轴不变量是具有不变性的自然参考轴，故其绝对导数恒为零矢量。因此，轴不变量具有对时间微分的不变性。有：

[0098]

[0099] 定义4转动坐标矢量：绕坐标轴矢量 转动到角位置 的坐标矢量 为

[0100] if

[0101] 定义5平动坐标矢量：沿坐标轴矢量 平动到线位置 的坐标矢量 为

[0102] if

[0103] 定义6自然坐标：以自然坐标轴矢量为参考方向，相对系统零位的角位置或线位置，记为ql，称为自然坐标；称与自然坐标一一映射的量为关节变量；其中：

[0104]

[0105] 定义7机械零位：对于运动副 在初始时刻t0时，关节绝对编码器的零位 不一定为零，该零位称为机械零位；

[0106] 故关节 的控制量 为

[0107]

[0108] 定义8自然运动矢量：将由自然坐标轴矢量 及自然坐标ql确定的矢量 称为自然运动矢量。

[0109] 其中：

[0110]

[0111] 自然运动矢量实现了轴平动与转动的统一表达。将由自然坐标轴矢量及关节确定的矢量，例如 称为自由运动矢量，亦称为自由螺旋。显然，轴矢量 是特定的自由螺旋。

[0112] 定义9关节空间：以关节自然坐标ql表示的空间称为关节空间。

[0113] 定义10位形空间：称表达位置及姿态(简称位姿)的笛卡尔空间为位形空间，是双矢量空间或6D空间。

[0114] 定义11自然关节空间：以自然坐标系为参考，通过关节变量 表示，在系统零位时必有 的关节空间，称为自然关节空间。

[0115] 给定多轴系统D＝{T,A,B,K,F,NT}，在系统零位时，只要建立底座系或惯性系，以及各轴上的参考点Ol，其它杆件坐标系也自然确定。本质上，只需要确定底座系或惯性系。

[0116] 给定一个由运动副连接的具有闭链的结构简图，可以选定回路中任一个运动副，将组成该运动副的定子与动子分割开来；从而，获得一个无回路的树型结构，称之为Span树。T表示带方向的span树，以描述树链运动的拓扑关系。

[0117] I为结构参数；A为轴序列，F为杆件参考系序列，B为杆件体序列，K为运动副类型序列，NT为约束轴的序列即非树。 为取轴序列 的成员。转动副R，棱柱副P，螺旋副H，接触副O是圆柱副C的特例。

[0118] 定义以下表达式或表达形式：

[0119] 轴与杆件具有一一对应性；轴间的属性量 及杆件间的属性量 具有偏序性。

[0120] 约定：“□”表示属性占位；若属性p或P是关于位置的，则 应理解为坐标系 的原点至F[l]的原点；若属性p或P是关于方向的，则 应理解为坐标系 至F[l]。

[0121] 及 应分别理解为关于时间t的函数 及 且 及 是t0时刻的常数或常数阵列。但是正体的 及 应视为常数或常数阵列。

[0122] 本申请中约定：在运动链符号演算系统中，具有偏序的属性变量或常量，在名称上包含表示偏序的指标；要么包含左上角及右下角指标，要么包含右上角及右下角指标；它们的方向总是由左上角指标至右下角指标，或由右上角指标至右下角指标，本申请中为叙述简便，有时省略方向的描述，即使省略，本领域技术人员通过符号表达式也可以知道，本申请中采用的各参数，对于某种属性符，它们的方向总是由偏序指标的左上角指标至右下角指标，或由右上角指标至右下角指标。例如： 可简述为(表示由k至l)平动矢量； 表示(由k至l的)线位置；krl表示(由k至l的)平动矢量；其中：r表示“平动”属性符，其余属性符对应为：属性符φ表示“转动”；属性符Q表示“旋转变换矩阵”；属性符l表示“运动链”；属性符u表示“单位矢量”；属性符ω表示“角速度”；属性符J表示质心转动惯量；J表示偏速度雅克比矩阵；角标为i表示惯性坐标系或大地坐标系；其他角标可以为其他字母，也可以为数字。

[0123] 本申请的符号规范与约定是根据运动链的偏序性、链节是运动链的基本单位这两个原则确定的，反映了运动链的本质特征。链指标表示的是连接关系，右上指标表征参考系。采用这种符号表达简洁、准确，便于交流与书面表达。同时，它们是结构化的符号系统，包含了组成各属性量的要素及关系，便于计算机处理，为计算机自动建模奠定基础。指标的含义需要通过属性符的背景即上下文进行理解；比如：若属性符是平动类型的，则左上角指标表示坐标系的原点及方向；若属性符是转动类型的，则左上角指标表示坐标系的方向。

[0124] (1)lS－杆件l中的点S；而S表示空间中的一点S。

[0125] (2) －杆件k的原点Ok至杆件l的原点Ol的平动矢量；

[0126] 在自然坐标系F[k]下的坐标矢量，即由k至l的坐标矢量；

[0127] (3) －原点Ok至点lS的平动矢量；

[0128] 在F[k]下的坐标矢量；

[0129] (4) －原点Ok至点S的平动矢量；

[0130] － 在F[k]下的坐标矢量；

[0131] (5) －连接杆件 及杆件l的运动副；

[0132] －运动副 的轴矢量；

[0133] 及 分别在 及F[l]下的坐标矢量； 是轴不变量，为一结构常数；

[0134] 为转动矢量，转动矢量/角矢量 是自由矢量，即该矢量可自由平移；

[0135] (6) －沿轴 的线位置(平动位置)，

[0136] －绕轴 的角位置，即关节角、关节变量，为标量；

[0137] (7)左下角指标为0时，表示机械零位；如：

[0138] -平动轴 的机械零位，

[0139] －转动轴 的机械零位；

[0140] (8)0－三维零矩阵；03＝[0 0 0]T；1－三维单位矩阵；

[0141] (9)约定：“\”表示续行符；“□”表示属性占位；则

[0142] 幂符 表示□的x次幂；右上角角标∧或 表示分隔符；如： 或 为 的x次幂。

[0143] [□]T表示□的转置，表示对集合转置，不对成员执行转置；如：

[0144] |□为投影符，表示矢量或二阶张量对参考基的投影矢量或投影序列，即坐标矢量或坐标阵列，投影即是点积运算“·”；如：位置矢量 在坐标系F[k]中的投影矢量记为[0145] 为叉乘符；如： 是轴不变量 的叉乘矩阵；给定任一矢量 的叉乘矩阵为叉乘矩阵是二阶张量。

[0146] 叉乘符运算的优先级高于投影符|□的优先级。投影符|□的优先级高于成员访问符□[□]或□[□]，成员访问符□[□]优先级高于幂符

[0147] (10)单位矢量在大地坐标系的投影矢量 单位零位矢量

[0148] (11) －零位时由原点 至原点Ol的平动矢量，且记 表示位置结构参数。

[0149] (12)iQl，相对绝对空间的旋转变换阵；

[0150] (13)以自然坐标轴矢量为参考方向，相对系统零位的角位置或线位置，记为ql，称为自然坐标；关节变量 自然关节坐标为φl；

[0151] (14)对于一给定有序的集合r＝[1,4,3,2]T，记r[x]表示取集合r的第x行元素。常记[x]、[y]、[z]及[w]表示取第1、2、3及4列元素。

[0152] (15)ilj表示由i到j的运动链；llk为取由轴l至轴k的运动链；

[0153] 给定运动链 若n表示笛卡尔直角系，则称 为笛卡尔轴链；若n表示自然参考轴，则称 为自然轴链。

[0154] 1.建立多轴系统的拉格朗日方程

[0155] 应用链符号系统建立关节空间的拉格朗日方程，考虑质点动力学系统D＝{A,K,T,NT,F,B}，首先根据牛顿力学推导自由质点 的拉格朗日方程；然后，推广至受约束的质点系统。

[0156] 保守力 相对质点惯性力 具有相同的链序，即 具有正序，质点 的合力为零。质点 的能量记为 根据广义坐标序列 与笛卡尔空间位置矢量序列
关系

[0157]

[0158] 得

[0159]

[0160] 式(2)应用系统的能量及广义坐标建立系统的方程。关节变量 与坐标矢量irl的关系如式(1)所示，称式(1)为关节空间与笛卡尔空间的点变换。

[0161] 保守力与惯性力具有相反的链序。拉格朗日系统内的约束既可以是质点间的固结约束，又可以是质点系统间的运动约束；刚体自身是质点系统 质点能量具有可加性；刚体动能量由质心平动动能及转动动能组成。下面，就以简单运动副R/P分别建立拉格朗日方程，为后续进一步推出新的动力学理论奠定基础。

[0162] 给定刚体多轴系统D＝{A,K,T,NT,F,B}，惯性空间记为i， 轴l的能量记为其中平动动能为 转动动能为 引力势能为 轴l受除引力外的外部合力及合力矩分别为Dfl及Dτl；轴l的质量及质心转动惯量分别为ml及 轴u的单位轴不变量为环境i作用于lI的惯性加速度记为 重力加速度 链序由i至lI； 链序由lI至i；
且有

[0163]

[0164] 【1】系统能量

[0165] 动力学系统D能量 表达为

[0166]

[0167] 其中：

[0168]

[0169] 【2】多轴系统拉格朗日方程

[0170] 由式(2)得多轴系统拉格朗日方程，

[0171]

[0172] 式(6)为轴u的控制方程，即在轴不变量 上的力平衡方程； 是合力在 上的分量， 是合力矩 在 上的分量。

[0173] 2.建立Ju-Kane动力学预备方程：

[0174] 基于多轴系统拉格朗日方程(6)推导居―凯恩(Ju-Kane)动力学预备定理。先进行拉格朗日方程与凯恩方程的等价性证明；然后，计算能量对关节速度及坐标的偏速度，再对时间求导，最后给出Ju-Kane动力学预备定理。

[0175] 【1】拉格朗日方程与凯恩方程的等价性证明

[0176]

[0177] 证明：考虑刚体k平动动能对 的偏速度对时间的导数得

[0178]

[0179] 考虑刚体k转动动能对 的偏速度对时间的导数得

[0180]

[0181] 证毕。

[0182] 因 与 不相关，由式(7)及多轴系统拉格朗日方程(6)得

[0183]

[0184] 动力学系统D的平动动能及转动动能分别表示为

[0185]

[0186] 考虑式(4)及式(5)，即有

[0187]

[0188] 式(7)及式(8)是居―凯恩动力学预备定理证明的依据，即居―凯恩动力学预备定理本质上与拉格朗日法是等价的。同时，式(8)右侧包含了多轴系统凯恩方程；表明拉格朗日法与凯恩法的惯性力计算是一致的，即拉格朗日法与凯恩法也是等价的。式(8)表明：在拉格朗日方程(4)中存在 重复计算的问题。

[0189] 【2】能量对关节速度及坐标的偏速度

[0190] 【2-1】若 并考虑 及 仅与闭子树uL相关，由式(4)及式(5)，得

[0191]

[0192]

[0193]

[0194] 【2-2】若 并考虑 及 仅与闭子树uL相关，由式(4)及式(5)，得

[0195]

[0196]

[0197]

[0198] 至此，已完成能量对关节速度及坐标的偏速度计算。

[0199] 【3】求对时间的导数

[0200] 【3-1】若 由式(7)、式(9)及式(10)得

[0201]

[0202] 【3-2】若 由式(7)、式(12)及式(13)得

[0203]

[0204] 至此，已完成对时间t的求导。

[0205] 【4】Ju-Kane动力学预备定理

[0206] 将式(11)、式(14)、式(15)及式(16)代入式(8)，

[0207] 给定多轴刚体系统D＝{A,K,T,NT,F,B}，惯性系记为F[i]， 除了重力外，作用于轴u的合外力及力矩分别记为 及 轴k的质量及质心转动惯量分别记为mk及 轴k的重力加速度为 则轴u的Ju-Kane动力学预备方程为

[0208]

[0209] 式(17)具有了树链拓扑结构。kI表示杆k质心I。因闭子树uL中的广义力具有可加性；因此闭子树的节点有唯一一条至根的运动链，因此运动链iln可以被运动链uL替换。

[0210] 下面，针对Ju-Kane动力学预备方程，解决式(17)右侧Dfk及Dτk的计算问题，从而建立树链刚体系统Ju-Kane动力学方程。

[0211] 3.建立树链刚体系统Ju-Kane动力学模型

[0212] 给定轴链 k∈iln，有以下偏速度计算公式：

[0213]

[0214]

[0215]

[0216] 对给定轴链 |ill|≥2，有以下加速度迭代式：

[0217]

[0218] 左序叉乘与转置的关系为：

[0219]

[0220] 根据运动学迭代式，有：

[0221]

[0222]

[0223] 3.1外力反向迭代

[0224] 给定由环境i中施力点iS至轴l上点lS的双边外力 及外力矩iτl，它们的瞬时轴功率pex表示为

[0225]

[0226] 其中： 及iτl不受 及 控制，即 及iτl不依赖于 及

[0227] 【1】若k∈ill，则有 由式(19)及式(18)得

[0228]

[0229] 即

[0230]

[0231] 式(26)中 与式(21)中 的链序不同；前者是作用力，后者是运动量，二者是对偶的，具有相反的序。

[0232] 【2】若k∈ill，则有 由式(22)及式(25)得

[0233]

[0234] 即有

[0235]

[0236] 式(26)及式(27)表明环境作用于轴k的合外力或力矩等价于闭子树kL对轴k的合外力或力矩，将式(26)及式(27)合写为

[0237]

[0238] 至此，解决了外力反向迭代的计算问题。在式(28)中，闭子树对轴k的广义力具有可加性；力的作用具有双重效应，且是反向迭代的。所谓反向迭代是指： 是需要通过链节位置矢量迭代的； 的序与前向运动学 计算的序相反。

[0239] 3.2共轴驱动力反向迭代

[0240] 若轴l是驱动轴，轴l的驱动力及驱动力矩分别为 及 则驱动力 及驱动力矩 产生的功率pac表示为

[0241]

[0242] 【1】由式(18)、式(19)及式(29)得

[0243]

[0244] 即

[0245]

[0246] 若轴u与轴 共轴，则有 记因 与 无关，由式
(30)得

[0247]

[0248] 因 与 共轴，故有

[0249]

[0250] 【2】由式(19)、式(18)及式(29)得

[0251]

[0252] 即

[0253]

[0254] 若轴u与 共轴，则有 记 由式(32)得

[0255]

[0256] 至此，完成了共轴驱动力反向迭代计算问题。

[0257] 3.3树链刚体系统Ju-Kane动力学显式模型的建立：

[0258] 下面，先陈述树链刚体系统Ju-Kane动力学方程，简称Ju-Kane方程；然后，给出建立步骤。

[0259] 给定多轴刚体系统D＝{A,K,T,NT,F,B}，惯性系记为F[i]， 除了重力外，作用于轴u的合外力及力矩在 上的分量分别记为 及 轴k的质量及质心转动惯量分别记为mk及 轴k的重力加速度为 驱动轴u的双边驱动力及驱动力矩在 上的分量分别记为 及 环境i对轴l的力及力矩分别为 及iτl；则轴u树链Ju-
Kane动力学方程为

[0260]

[0261] 其中：[·]表示取行或列； 及 是3×3的分块矩阵， 及 是3D矢量，q为关节空间。且有，

[0262]

[0263]

[0264]

[0265]

[0266]

[0267]

[0268] 其中，记 记上述方程的建立步骤为：

[0269] 记 故有

[0270]

[0271] ex的能量为 pex为瞬时轴功率；pac为驱动轴的驱动力及驱动力矩产生的功率。由式(26)、式(27)、式(31)、式(33)及式(41)得式(40)。

[0272] 将偏速度计算公式式(19)，式(18)及式(20)代入Ju-Kane动力学预备方程(17)得[0273]

[0274] 由式(21)得

[0275]

[0276] 考虑式(43)，则有

[0277]

[0278] 同样，考虑式(43)，得

[0279]

[0280] 将式(43)至式(45)代入式(42)得式(34)至式(39)。

[0281] 实施例1

[0282] 给定如图3所示的通用3R机械臂，A＝(i,1:3]；应用本发明的方法建立树链Ju-Kane动力学方程，并得到广义惯性矩阵。

[0283] 步骤1建立基于轴不变量的迭代式运动方程。

[0284] 由式(46)基于轴不变量的转动变换矩阵

[0285]

[0286] 得

[0287]

[0288] 运动学迭代式：

[0289]

[0290]

[0291]

[0292]

[0293]

[0294] 二阶张量投影式：

[0295]

[0296]

[0297] 由式(48)及式(47)得

[0298]

[0299] 由式(49)，式(47)及式(55)得

[0300]

[0301] 由式(50)及式(55)得

[0302]

[0303] 由式(51)、式(55)及式(57)得

[0304]

[0305] 由式(52)及式(55)得

[0306]

[0307] 由式(53)及式(55)得

[0308]

[0309] 步骤2建立动力学方程。先建立第1轴的动力学方程。由式(37)得

[0310]

[0311] 由式(39)得

[0312]

[0313] 由式(61)及式(62)得第1轴的动力学方程，

[0314]

[0315] 建立第2轴的动力学方程。由式(37)得

[0316]

[0317] 由式(39)得

[0318]

[0319] 由式(64)及式(65)得第2轴的动力学方程，

[0320]

[0321] 最后，建立第3轴的动力学方程。由式(37)得

[0322]

[0323] 由式(39)得

[0324]

[0325] 由式(67)及式(68)得第3轴的动力学方程，

[0326]

[0327] 由式(61)，式(63)及式(67)得广义质量阵。

[0328]

[0329] 由此可知，只要程式化地将系统的拓扑、结构参数、质惯量等参数代入式(36)至式(40)就可以完成动力学建模。通过编程，很容易实现Ju-Kane动力学方程。因后续的树链Ju-Kane规范方程是以Ju-Kane动力学方程推导的，树链Ju-Kane动力学方程的有效性可由Ju-Kane规范型实例证明。

[0330] 3.4树链刚体系统Ju-Kane动力学规范型

[0331] 在建立系统动力学方程后，紧接着就是方程求解的问题。在动力学系统仿真时，通常给定环境作用的广义力及驱动轴的广义驱动力，需要求解动力学系统的加速度；这是动力学方程求解的正问题。在求解前，首先需要得到式(71)所示的规范方程。

[0332] 规范化动力学方程，

[0333]

[0334] 其中：RHS–右手侧(Right hand side)

[0335] 显然，规范化过程就是将所有关节加速度项进行合并的过程；从而，得到关节加速度的系数。将该问题分解为运动链的规范型及闭子树的规范型两个子问题。

[0336] 3.4.1运动链的规范型方程

[0337] 将式(36)及式(37)中关节加速度项的前向迭代过程转化为反向求和过程，以便后续应用；显然，其中含有6种不同类型的加速度项，分别予以处理。

[0338] 【1】给定运动链 则有

[0339]

[0340] 上式的推导步骤为：

[0341]

[0342] 【2】给定运动链 则有

[0343]

[0344] 上式的推导步骤为：因 故得

[0345]

[0346] 【3】给定运动链 则有

[0347]

[0348] 上式可由下式而得，因 故有

[0349]

[0350] 【4】给定运动链 则有

[0351]

[0352] 上式的推导步骤为：考虑 将式(72)代入式(75)左侧得

[0353]

[0354] 【5】给定运动链 则有

[0355]

[0356] 上式的推导步骤为：考虑 将式(72)代入式(76)左侧得

[0357]

[0358] 【6】给定运动链 则有

[0359]

[0360] 上式的推导步骤为：因 故有

[0361]

[0362] 3.4.2闭子树的规范型方程

[0363] 因闭子树uL中的广义力具有可加性；因此闭子树的节点有唯一一条至根的运动链，式(73)至式(77)的运动链iln可以被uL替换。由式(73)得

[0364]

[0365] 由式(74)得

[0366]

[0367] 由式(75)得

[0368]

[0369] 由式(76)得

[0370]

[0371] 由式(77)得

[0372]

[0373] 至此，已具备建立规范型的前提条件。

[0374] 3.5树链刚体系统Ju-Kane动力学规范方程

[0375] 下面，建立树结构刚体系统的Ju-Kane规范化动力学方程。为表达方便，首先定义[0376]

[0377] 然后，应用式(78)至式(82)，将式(36)及式(37)表达为规范型。

[0378] 【1】式(36)的规范型为

[0379]

[0380] 上式的具体建立步骤为：由式(24)及式(36)得

[0381]

[0382] 由式(52)及式(85)得

[0383]

[0384] 将式(80)代入式(85)右侧前一项得

[0385]

[0386] 将式(79)代入式(86)右侧后一项得

[0387]

[0388] 将式(87)及式(88)代入式(86)得

[0389]

[0390] 对于刚体k，有 由式(35)、式(83)及式(89)得式(84)。

[0391] 【2】式(37)的规范型为

[0392]

[0393] 上式的具体建立步骤为：由式(37)得

[0394]

[0395] 将式(78)代入式右侧前一项(91)得

[0396]

[0397] 将式(81)代入式(91)右侧后一项得

[0398]

[0399] 将式(82)代入式(91)右侧中间一项得

[0400]

[0401] 将式(92)，式(93)及式(94)代入式(92)得

[0402]

[0403] 对于刚体k，有 由式(35)，式(83)及式(95)得式(90)。

[0404] 【3】应用式(84)及式(90)，将Ju-Kane方程重新表述为如下树链Ju-Kane规范型方程：

[0405] 给定多轴刚体系统D＝{A,K,T,NT,F,B}，惯性系记为F[i]， 除了重力外，作用于轴u的合外力及力矩在 上的分量分别记为 及 轴k的质量及质心转动惯量分别记为mk及 轴k的重力加速度为 驱动轴u的双边驱动力及驱动力矩在 上i
的分量分别记为 及 环境i对轴l的作用力及力矩分别为 及τl；则轴u的Ju-
Kane动力学规范方程为

[0406]

[0407] 其中： 及 是3×3的分块矩阵， 及 是3D矢量。并且，

[0408]

[0409]

[0410]

[0411]

[0412]

[0413]

[0414] 式中，kI表示杆k质心I；轴k的质量及质心转动惯量分别记为mk及 为转动轴u的惯性矩阵； 为平动轴u的惯性矩阵；hR为转动轴u的非惯性矩阵；hP为平动轴u的非惯性矩阵 ；作用于轴u的合外力及力矩在 上的分量分别记为 及驱动轴u的双边驱动力及驱动力矩在 上的分量分别记为
及 环境i对轴l的作用力及作用力矩分别为 及iτl；llk为取由轴l至轴k的运
动链，uL表示获得由轴u及其子树构成的闭子树。

[0415] 4.闭链刚体系统的Ju-Kane动力学方程建立

[0416] 下面，先陈述闭链刚体系统的居―凯恩(简称Ju-Kane)动力学方程；然后，给出具体建模过程。

[0417] 给定多轴刚体系统D＝{A ,K ,T ,NT ,F ,B}，惯性系记为F[i]，除了重力外，作用于轴u的合外力及力矩在 上的分量
分别记为 及 轴k的质量及质心转动惯量分别记为mk及 轴k的重力加速度为 驱
动轴u的双边驱动力及驱动力矩在 上的分量分别记为 及 环境i对轴l的作
用力及作用力矩分别为 及iτl；轴u对轴u′的广义约束力记为 则有闭链刚体系统的Ju-Kane动力学方程：

[0418] 【1】轴u及轴u′的Ju-Kane动力学规范方程分别为

[0419]

[0420]

[0421] 【2】非树约束副uku′的约束代数方程为

[0422]

[0423]

[0424]

[0425]

[0426] 其中：

[0427]

[0428]

[0429]

[0430]

[0431]

[0432] 式中： 及 是3×3的分块矩阵， 及 是3D矢量；kI表示杆k质心I；轴k的质量及质心转动惯量分别记为mk及 为转动轴u的惯性矩阵； 为平动轴u的惯性矩阵；hR为转动轴u的非惯性矩阵；hP为平动轴u的非惯性矩阵； 为平动关节角速度；
为转动关节角速度。

[0433] 具体建模过程如下：

[0434] 非树约束副 保持约束点uS及u′S一致，故有

[0435]

[0436] 由式(114)得

[0437]

[0438] 轴u对轴u′在约束轴方向上的广义约束力 及轴u′对轴u在约束轴方向上的广义约束力 的功率分别为

[0439]

[0440] 由式(115)及式(116)得

[0441]

[0442] 由式(115)得

[0443]

[0444]

[0445]

[0446]

[0447] δ表示增量；

[0448] 由式(18)及式(118)得

[0449]

[0450] 故有

[0451]

[0452] 由式(110)及式(122)得式(105)。由式(19)及式(119)得

[0453]

[0454] 由式(111)及式(123)得式(106)。由式(19)及式(120)得

[0455]

[0456] 由式(112)及式(124)得式(107)。由式(19)及式(121)得

[0457]

[0458] 由式(113)及式(125)得(108)。由式(18)，式(116)及式(110)得

[0459]

[0460]

[0461] 广义约束力 及 是矢量，由式(126)及式(127)得式(109)。由此可知，偏速度主要应用于力的反向迭代。广义约束力 及 视为外力。

[0462] 根据轴u的Ju-Kane动力学规范方程得式(103)及式(104)。

[0463] 以关节空间自然轴链为基础的Ju-Kane闭链刚体动力学克服了笛卡尔坐标轴链空间的局限：

[0464] 【1】在基于笛卡尔坐标轴链的牛顿欧拉动力学中，非树运动副uku′∈P约束不能表达 及 或 及 的情形，即不能表达齿条与齿轮、蜗轮与蜗杆等约束。而本申请的非树约束副uku′的约束代数方程式(105)至式(108)可表达任一种约束类形，并且物理内涵明晰；

[0465] 【2】在基于笛卡尔坐标轴链的牛顿欧拉动力学当中，非树运动副代数约束方程是6D的；而式(105)至式(108)表示是3D非树运动副代数约束方程，从而降低了系统方程求解的复杂度；

[0466] 【3】在基于笛卡尔坐标轴链的牛顿欧拉动力学当中，非树运动副代数约束方程是关于6D矢量空间绝对加速度的，是关于关节坐标、关节速度的迭代式，具有累积误差；而式(105)至式(108)是关于关节加速度的，保证了约束方程的准确性。

[0467] 5.基于轴不变量的约束力求解

[0468] 对于无功率损耗的运动轴u，记其约束力及约束力矩矢量分别为 显然，有

[0469]

[0470] 由式(96)及式(139)计算得 式(128)表示运动轴矢量与运动轴约束力具有自然正交补的关系。

[0471] 若 及 为运动副 的两个正交约束轴，且约束轴与运动轴正交，即

[0472]

[0473] 记 为约束轴轴矢量， 替换式(96)中 重新计算得

[0474]

[0475] 其中：

[0476]

[0477]

[0478] 在完成前向动力学正解后，根据已计算的关节加速度 由式(130)可以得到关节约束力大小 约束力矩大小 当 时，由式(130)得 且 式(130)中同一时刻具有相同的运动状态及内外力。仅在运动轴向上出现力及力矩的平衡；而在约束轴向，动力学方程不满足，即力与力矩不一定平衡。

[0479] 由式(130)可以得到关节约束力大小 及 约束力矩大小 及 若记运动轴径向力矢量 及力矩矢量 则有

[0480]

[0481] 若记运动轴径向力大小为 及力矩大小为 由式(133)得

[0482]

[0483] 至此，完成了轴径向约束广义力的计算。

[0484] 树链刚体系统对应的关节加速度序列记 可根据下述步骤计算：

[0485] 将根据运动轴类型及自然参考轴表达的刚体运动链广义惯性矩阵称为轴链刚体广义惯性矩阵，简称轴链广义惯性矩阵。

[0486] 定义正交补矩阵 及对应的叉乘矩阵

[0487]

[0488] 给定多轴刚体系统D＝{A,K,T,NT,F,B}， 将系统中各轴动力学方程(96)按行排列；将重排后的轴驱动广义力及不可测的环境作用力记为fC，可测的环境广义作用力记为fi；将系统对应的关节加速度序列记为 将重排后的 记为h；考虑式(135)；则该系统动力学方程为

[0489]

[0490] 由式(136)得

[0491]

[0492] 其中，

[0493]

[0494] 由式(136)得

[0495]

[0496] 6.广义内摩擦力及粘滞力计算

[0497] 在完成轴径向约束广义力的计算后，得到运动轴u的径向约束力大小 及约束力矩大小 如图3、图4所示，记运动轴u的内摩擦力大小及内摩擦力矩大小分别为 及运动轴u的粘滞力及粘滞力矩大小分别为 及

[0498] 故有

[0499]

[0500]

[0501] 其中：sk[u]─运动轴u的内摩擦系数，ck[u]─运动轴u的粘滞系数；sign()表示取正或负符号。

[0502] 记广义内摩擦力及粘滞力的合力及合力矩分别为 由式(140)及式(141)得

[0503]

[0504] 运动轴的广义内摩擦力及粘滞力是运动轴的内力，因为它们仅存在于运动轴向上，与轴径向约束力总是正交的。当运动轴轴向动态作用力平衡时，无论广义内摩擦力及粘滞力是否存在或大小如何，都不影响动力学系统的运动状态；故而，不影响运动轴的径向约束力。因此，由式(130)至式(134)计算运动轴u的径向约束力大小 及约束力矩大小 时，可以不考虑运动轴的广义内摩擦力及粘滞力。

[0505] 7.建立闭链刚体非理想约束系统的Ju-Kane动力学显式模型

[0506] 设运动轴u的广义内摩擦及粘滞的合力及合力矩分别为 闭链刚体系统的Ju-Kane动力学方程建立后，计算关节加速度 后，应用式(129)至式(134)计算径向约束力大小 及 约束力矩大小 及 再建立如下闭链刚体非理想约束系统的Ju-Kane动力学方程：

[0507] 【1】轴u及轴u′的Ju-Kane动力学规范方程分别为

[0508]

[0509]

[0510] 【2】非树约束副uku′的约束代数方程为

[0511]

[0512]

[0513]

[0514]

[0515] 其它，参见式(103)至式(113)、式(97)至式(102)。

[0516] 建立过程为：

[0517] 运动轴u的内摩擦及粘滞合力 及合力矩 是运动轴u的外力，故有式(143)；运动轴u′的内摩擦及粘滞合力 及合力矩 是运动轴u′的外力，故有式(144)。其它过程与闭链刚体系统的Ju-Kane动力学方程建模步骤相同。