CN108803350A - 基于轴不变量的动基座多轴机器人动力学建模与解算方法 - Google Patents

Abstract

本发明公开了一种基于轴不变量的动基座多轴机器人动力学与解算方法，给出了动基座刚体系统的Ju‑Kane动力学方程建模及解算的方法。并通过2个实施例的建模及解算过程表明：应用Ju‑Kane动力学原理建立多轴系统动力学显式模型简洁、方便、高效，适宜高自由度的多轴系统动力学显式建模与数值建模。同时表明，通过显式动力学建模与计算，可以通过多轴系统的运动检测及视觉检测，计算系统与环境的接触作用力，为多轴系统力位控制及环境感知提供了新的技术途径。

Description

基于轴不变量的动基座多轴机器人动力学建模与解算方法

技术领域

本发明涉及一种动基座多轴机器人动力学与解算方法，属于机器人技术领域。

背景技术

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

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

发明内容

本发明所要解决的技术问题是提供一种基于轴不变量的动基座多轴机器人动力学与解算方法。

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

一种基于轴不变量的动基座多轴机器人动力学建模与解算方法，其特征是，

给定多轴刚体移动系统D＝{A,K,T,NT,F,B}，惯性系记为F^[i]， A为轴序列，F为杆件参考系序列，B为杆件体序列，K为运动副类型序列，NT为约束轴的序列即非树；轴序列为ⁱA_c＝(i,c1,c2,c3,c4,c5,c]，轴类型序列为ⁱK_c＝(X,R,R,R,P,P,P]，该运动链为ⁱl_c＝(i,c1,c2,c3,c4,c5,c]；除了重力外，作用于轴u的合外力及力矩在上的分量分别为及轴k的质量及质心转动惯量分别为m_k及轴k的重力加速度为驱动轴u的双边驱动力及驱动力矩在上的分量分别为及环境i对轴l的作用力及作用力矩分别为及ⁱτ_l；作用于轴c上的合力及合力矩分别为及记 则有以下动基座刚体系统的Ju-Kane动力学方程

则有

其中：表示c的开子树，且有：

式中，及是3×3的分块矩阵，及是3D矢量；k_I表示杆k质心I；轴k的质量及质心转动惯量分别记为m_k及 为转动轴u的惯性矩阵；为平动轴u的惯性矩阵；h_R为转动轴u的非惯性矩阵；h_P为平动轴u的非惯性矩阵；为关节角速度。

由式(150)确定本体c的笛卡尔体系F^[c]三个转动轴的序列，在建立动力学方程后，通过积分完成动力学仿真，直接得到所期望的姿态。

动基座刚体系统的Ju-Kane动力学方程根据闭链刚体系统的Ju-Kane动力学方程建立。

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

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

式中：及是3×3的分块矩阵，及是3D矢量；k_I表示杆k质心I；为转动轴u的惯性矩阵；为平动轴u的惯性矩阵；h_R为转动轴u的非惯性矩阵；h_P为平动轴u的非惯性矩阵。

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

其中：

式中：及是3×3的分块矩阵，及是3D矢量；k_I表示杆k质心I；轴k的质量及质心转动惯量分别记为m_k及 为转动轴u的惯性矩阵；为平动轴u的惯性矩阵；h_R为转动轴u的非惯性矩阵；h_P为平动轴u的非惯性矩阵；为平动关节角速度；为转动关节角速度。

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

树链Ju-Kane规范型方程

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

并且，

式中，k_I表示杆k质心I；轴k的质量及质心转动惯量分别记为m_k及 为转动轴u的惯性矩阵；为平动轴u的惯性矩阵；h_R为转动轴u的非惯性矩阵；h_P为平动轴u的非惯性矩阵；作用于轴u的合外力及力矩在上的分量分别记为及 作用于轴u的合外力及力矩在上的分量分别记为及驱动轴u的双边驱动力及驱动力矩在上的分量分别记为及环境i对轴l的作用力及作用力矩分别为及ⁱτ_l；^ll_k为取由轴l至轴k的运动链，^uL表示获得由轴u及其子树构成的闭子树。

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

本发明给出了动基座刚体系统的Ju-Kane动力学方程建模及解算的方法。并通过2个实施例的建模及解算过程表明：应用Ju-Kane动力学原理建立多轴系统动力学显式模型简洁、方便、高效，适宜高自由度的多轴系统动力学显式建模与数值建模。同时表明，通过显式动力学建模与计算，可以通过多轴系统的运动检测及视觉检测，计算系统与环境的接触作用力，为多轴系统力位控制及环境感知提供了新的技术途径。

附图说明

图1自然坐标系与轴链；

图2固定轴不变量；

图3三轮移动系统示意图；

图4CE3月面巡视器移动系统；

图5副臂移动系统树链结构简图。

具体实施方式

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

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

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

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

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

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

轴不变量与坐标轴具有本质区别：

(1)坐标轴是具有零位及单位刻度的参考方向，可以描述沿该方向平动的位置，但不能完整描述绕该方向的转动角度，因为坐标轴自身不具有径向参考方向，即不存在表征转动的零位。在实际应用时，需要补充该轴的径向参考。例如：在笛卡尔系F^[l]中，绕lx转动，需以ly或lz为参考零位。坐标轴自身是1D的，3个正交的1D参考轴构成3D的笛卡尔标架。

(2)轴不变量是3D的空间单位参考轴，其自身就是一个标架。其自身具有径向参考轴，即参考零位。空间坐标轴及其自身的径向参考轴可以确定笛卡尔标架。空间坐标轴可以反映运动轴及测量轴的三个基本参考属性。

已有文献将无链指标的轴矢量记为并称之为欧拉轴(Euler Axis)，相应的关节角称为欧拉角(Euler Angle)。本申请之所以不再沿用欧拉轴，而称之为轴不变量，是因为轴不变量具有以下属性：

【1】给定旋转变换阵因其是实矩阵，其模是单位的，故其有一个实特征值λ₁及两个互为共轭的复特征值λ₂＝e^iφ及λ₃＝e^-iφ；其中：i为纯虚数。因此，|λ₁|·||λ₂||·||λ₃||＝1，得λ₁＝1。轴矢量是实特征值λ₁＝1对应的特征矢量，是不变量；

【2】是3D参考轴，不仅具有轴向参考方向，而且具有径向参考零位，将在3.3.1节予以阐述。

【3】在自然坐标系下：即轴不变量是非常特殊的矢量，它对时间的导数也具有不变性，且有非常优良的数学操作性能；

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

【4】在自然坐标系统中，通过轴矢量及关节变量可以直接描述旋转坐标阵没有必要为除根之外的杆件建立各自的体系。同时，以唯一需要定义的根坐标系为参考，可以提高系统结构参数的测量精度；

【5】应用轴矢量的优良操作，将建立包含拓扑结构、坐标系、极性、结构参量及力学参量的完全参数化的统一的多轴系统运动学及动力学模型。

因基矢量e_l是与F^[l]固结的任一矢量，基矢量是与固结的任一矢量，又是F^[l]及共有的单位矢量，故是F^[l]及共有的基矢量。因此，轴不变量是F^[l]及共有的参考基。轴不变量是参数化的自然坐标基，是多轴系统的基元。固定轴不变量的平动与转动与其固结的坐标系的平动与转动等价。

在系统处于零位时，以自然坐标系为参考，测量得到坐标轴矢量在运动副运动时，轴矢量是不变量；轴矢量及关节变量唯一确定运动副的转动关系。

因此，应用自然坐标系统，当系统处于零位时，只需确定一个公共的参考系，而不必为系统中每一杆件确定各自的体坐标系，因为它们由轴不变量及自然坐标唯一确定。当进行系统分析时，除底座系外，与杆件固结的其它自然坐标系只发生在概念上，而与实际的测量无关。自然坐标系统对于多轴系统(MAS)理论分析及工程作用在于：

(1)系统的结构参数测量需要以统一的参考系测量；否则，不仅工程测量过程烦琐，而且引入不同的体系会引入更大的测量误差。

(2)应用自然坐标系统，除根杆件外，其它杆件的自然坐标系统由结构参量及关节变量自然确定，有助于MAS系统的运动学与动力学分析。

(3)在工程上，可以应用激光跟踪仪等光学测量设备，实现对固定轴不变量的精确测量。

(4)由于运动副R及P、螺旋副H、接触副O是圆柱副C的特例，可以应用圆柱副简化MAS运动学及动力学分析。

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

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

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

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

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

故关节的控制量为

定义8自然运动矢量：将由自然坐标轴矢量及自然坐标q_l确定的矢量称为自然运动矢量。其中：

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

定义9关节空间：以关节自然坐标q_l表示的空间称为关节空间。

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

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

如图2所示，给定链节原点O_l受位置矢量约束的轴矢量为固定轴矢量，记为其中：

轴矢量是关节自然坐标的自然参考轴。因是轴不变量，故称为固定轴不变量，它表征了运动副的结构关系，即确定了自然坐标轴。固定轴不变量是链节结构参数的自然描述。

定义12自然坐标轴空间：以固定轴不变量作为自然参考轴，以对应的自然坐标表示的空间称为自然坐标轴空间，简称自然轴空间。它是具有1个自由度的3D空间。

如图2所示，及不因杆件Ω_l的运动而改变，是不变的结构参考量。确定了轴l相对于轴的五个结构参数；与关节变量q_l一起，完整地表达了杆件Ω_l的6D位形。给定时，杆件固结的自然坐标系可由结构参数及关节变量唯一确定。称轴不变量固定轴不变量关节变量及为自然不变量。显然，由固定轴不变量及关节变量构成的关节自然不变量与由坐标系至F^[l]确定的空间位形具有一一映射关系，即

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

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

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

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

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

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

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

【4】为取轴序列的成员；计算复杂度为O(1)。

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

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

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

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

定义以下表达式或表达形式：

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

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

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

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

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

(1)l_S－杆件l中的点S；而S表示空间中的一点S。

(2)－杆件k的原点O_k至杆件l的原点O_l的平动矢量；

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

(3)－原点O_k至点l_S的平动矢量；

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

(4)－原点O_k至点S的平动矢量；

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

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

－运动副的轴矢量；

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

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

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

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

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

－平动轴的机械零位，

－转动轴的机械零位；

(8)0－三维零矩阵；1－三维单位矩阵；

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

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

表示的转置，表示对集合转置，不对成员执行转置；如：

为投影符，表示矢量或二阶张量对参考基的投影矢量或投影序列，即坐标矢量或坐标阵列，投影即是点积运算“·”；如：位置矢量在坐标系F^[k]中的投影矢量记为

为叉乘符；如：是轴不变量的叉乘矩阵；给定任一矢量的叉乘矩阵为叉乘矩阵是二阶张量。

叉乘符运算的优先级高于投影符的优先级。投影符的优先级高于成员访问符或成员访问符优先级高于幂符

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

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

(12)ⁱQ_l，相对绝对空间的旋转变换阵；

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

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

(15)ⁱl_j表示由i到j的运动链；^ll_k为取由轴l至轴k的运动链；

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

(16)Rodrigues四元数表达形式：

欧拉四元数表达形式：

不变量的四元数(也称为轴四元数)表达形式

一、1.建立多轴系统的拉格朗日方程

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

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

得

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

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

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

【1】系统能量

动力学系统D能量表达为

其中：

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

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

式(6)为轴u的控制方程，即在轴不变量上的力平衡方程；是合力^i|Df_u在上的分量，是合力矩^i|Dτ_u在上的分量。

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

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

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

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

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

证毕。

因_gε_k ⁱ与不相关，由式(7)及多轴系统拉格朗日方程(6)得

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

考虑式(4)及式(5)，即有

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

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

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

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

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

【3】求对时间的导数

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

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

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

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

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

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

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

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

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

给定轴链k∈ⁱl_n，有以下偏速度计算公式：

对给定轴链 ^|il_l|≥2，有以下加速度迭代式：

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

根据运动学迭代式，有：

3.1外力反向迭代

给定由环境i中施力点i_S至轴l上点l_S的双边外力及外力矩ⁱt_l，它们的瞬时轴功率p_ex表示为

其中：及ⁱτ_l不受及控制，即及ⁱτ_l不依赖于及

【1】若k∈ⁱl_l，则有由式(19)及式(18)得

即

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

【2】若k∈ⁱl_l，则有由式(22)及式(25)得

即有

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

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

3.2共轴驱动力反向迭代

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

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

即

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

因与共轴，故有

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

即

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

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

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

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

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

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

其中，记

记

上述方程的建立步骤为：

记故有

ex的能量为p_ex为瞬时轴功率；p_ac为驱动轴的驱动力及驱动力矩产生的功率。

由式(26)、式(27)、式(31)、式(33)及式(41)得式(40)。

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

由式(21)得

考虑式(43)，则有

同样，考虑式(43)，得

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

实施例1

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

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

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

得

运动学迭代式：

二阶张量投影式：

由式(48)及式(47)得

由式(49)，式(47)及式(55)得

由式(50)及式(55)得

由式(51)、式(55)及式(57)得

由式(52)及式(55)得

由式(53)及式(55)得

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

由式(39)得

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

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

由式(39)得

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

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

由式(39)得

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

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

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

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

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

规范化动力学方程，

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

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

3.4.1运动链的规范型方程

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

【1】给定运动链则有

上式的推导步骤为：

【2】给定运动链则有

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

【3】给定运动链则有

上式可由下式而得，因故有

【4】给定运动链则有

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

【5】给定运动链则有

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

【6】给定运动链则有

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

3.4.2闭子树的规范型方程

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

由式(74)得

由式(75)得

由式(76)得

由式(77)得

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

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

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

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

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

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

由式(52)及式(85)得

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

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

将式(87)及式(88)代入式(86)得

对于刚体k，有由式(35)、式(83)及式(89)得式(84)。【2】式(37)的规范型为

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

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

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

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

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

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

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

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

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

式中，k_I表示杆k质心I；轴k的质量及质心转动惯量分别记为m_k及 为转动轴u的惯性矩阵；为平动轴u的惯性矩阵；h_R为转动轴u的非惯性矩阵；h_P为平动轴u的非惯性矩阵；作用于轴u的合外力及力矩在上的分量分别记为及 作用于轴u的合外力及力矩在上的分量分别记为及驱动轴u的双边驱动力及驱动力矩在上的分量分别记为及环境i对轴l的作用力及作用力矩分别为及ⁱτ_l。

若多轴刚体系统D＝{A,K,T,NT,F,B}仅包含转动轴，则式(101)可简化为

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

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

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

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

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

其中：

式中：及是3×3的分块矩阵，及是3D矢量；k_I表示杆k质心I；轴k的质量及质心转动惯量分别记为m_k及 为转动轴u的惯性矩阵；为平动轴u的惯性矩阵；h_R为转动轴u的非惯性矩阵；h_P为平动轴u的非惯性矩阵；为平动关节角速度；为转动关节角速度。

具体建模过程如下：

非树约束副保持约束点u_S及u′_S一致，故有

由式(115)得

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

由式(116)及式(117)得

由式(116)得

δ表示增量；

由式(18)及式(119)得

故有

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

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

由式(113)及式(125)得式(108)。由式(19)及式(122)得

由式(114)及式(126)得(109)。由式(18)，式(117)及式(111)得

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

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

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

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

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

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

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

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

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

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

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

其中：

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

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

若记运动轴径向力大小为及力矩大小为由式(134)得

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

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

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

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

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

由式(137)得

其中，

由式(137)得

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

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

故有

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

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

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

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

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

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

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

其它，参见式(104)至式(114)、式(97)至式(102)。

建立过程为：

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

二、动基座刚体系统的Ju-Kane动力学规范方程

动基座刚体系统应用领域越来越广泛，包含：空间机械臂、星表巡视器、双足机器人等。下面，先陈述动基座刚体系统的Ju-Kane动力学定理；然后，予以证明；最后，给出三轮移动系统及CE3巡视器动力学建模示例。

1.动基座刚体系统的Ju-Kane动力学方程

给定多轴刚体移动系统D＝{A,K,T,NT,F,B}，惯性系记为F^[i]， 轴序列为ⁱA_c＝(i,c1,c2,c3,c4,c5,c]，轴类型序列为ⁱK_c＝(X,R,R,R,P,P,P]，该运动链为ⁱl_c＝(i,c1,c2,c3,c4,c5,c]；除了重力外，作用于轴u的合外力及力矩在上的分量分别为及轴k的质量及质心转动惯量分别为m_k及轴k的重力加速度为驱动轴u的双边驱动力及驱动力矩在上的分量分别为及环境i对轴l的作用力及作用力矩分别为及ⁱτ_l；作用于体c上的合力及合力矩分别为及^i|Dτ_c，记 且有

则有

其中：表示c的开子树，且有：

其它，参见式(99)至式(102)。

下面给出具体过程：显然,有

由式(150)及式(151)可知，它们确定了轴c的笛卡尔直角坐标系，但三个转动轴序列存在12种。由式(19)得

由式(158)得

由式(159)得

故有

由式(98)及式(160)得

由式(161)及式(163)得(155)。由式(101)及式(160)得

由式(162)及式(164)得式(156)。

由动基座刚体系统的Ju-Kane动力学方程可知，可以根据需要由式(150)确定本体c的笛卡尔体系F^[c]三个转动轴的序列，在建立动力学方程后，通过积分完成动力学仿真，直接可以得到所期望的姿态。

实施例1：基于Ju-Kane的10轴三轮移动系统动力学建模及逆解

给定三轮移动系统D＝{A,K,T,NT,F,B}，如图3所示，轴1、轴2及轴3驱动车轮，轴3驱动舵机；轴序列为A＝(i,c1,c2,c3,c4,c5,c,1:4]，父轴序列为轴l的质量及质心转动惯量分别为m_l及l∈[c,1:4]。应用本发明的方法建立各轴的动力学方程。

步骤1显然，|A|＝4，|B|＝5，|NT|＝|O|＝3，由轮式多轴系统的自由度DOF(D)＝6+|A|-|NT|+|O|，计算轮式多轴系统的自由度，得DOF(D)＝10。

根据轮式多轴系统移动维度准则，计算得轮式多轴系统移动维度DOM(D)为3，由于，当DOM(D)>3时，系统在自然地形上是静不定的；当DOM(D)＝3时，系统在自然地形上是静定的；当DOM(D)＝2时，系统在人工地形上是静定的。故该系统D在自然路面上静定。

步骤2基于轴不变量的正向运动学计算

由式(46)得

由式(48)及式(46)计算

由式(49)计算

由式(50)计算

考虑及其中l＝[c,1:4]；由式(55)计算

由式(53)计算

步骤3建立Ju-Kane动力学规范方程

由式(155)及式(156)得 由式(99)及式(100)分别计算及代入式(152)得

由式(103)，式(100)及式(96)得

至此，获得全部10个轴的动力学方程。

步骤4进行力反向迭代

对于给定轴链根据轮土矢量矢学及移动维度，轮土作用的牵引力为

其中，

—前向库仑摩擦系数；—侧向库仑摩擦系数；—轮土正压力；－牵引力；－取的正或负符号；

当包含侧向库仑摩擦力及轮土侧向力时，

由式(173)、式(174)及(102)得

若仅考虑轮土作用力及主动轴驱动力，则由式(102)得

步骤5计算动力学方程逆解

将式(175)至式(177)写为整体形式

f_10×1＝B_10×10·u_10×1； (178)

其中：

给定由式(171)及式(172)计算f。若B^-1存在，由式(178)得

u＝B^-1·f。 (180)

由式(179)及式(180)可知：

【1】控制力矩及与轮土作用力为及存在耦合；

【2】完成动力学逆解计算后，不仅得到驱动轴控制力矩及而且可以得到轮土作用力及故该逆解作用在于：

【2-1】计算驱动轴期望控制力矩及

【2-2】通过运动状态(位姿、速度及加速度)实现轮土作用力及的间接测量。

实施例2：基于Ju-Kane的20轴巡视器移动系统动力学建模及逆解

给定图4所示的CE3月面巡视器移动系统D＝{A,K,T,NT,F,B}，该系统Span树如图5所示；标识符及缩略符如下：

c―Chassis/车箱；i―Inertial Space/惯性空间(或导航系)；

rr―Right Rocker/右主臂(副臂)；rb―Right Bogie/右副臂；

lr―Left Rocker/左主臂(副臂)；lb―Left Bogie/左副臂；

rfd―Right Front Direction/右前方向机；rrd―Right Rear Direction/右后方向机；

lfd―Left Front Direction/左前方向机；lrd―Left Rear Direction/左后方向机；

rfw―Right Front Wheel/右前轮；lfw―Left Front Wheel/左前轮；

rmw―Right Middle Wheel/右中轮；lmw―Left Middle Wheel/左中轮；

rrw―Right Rear Wheel/右后轮；lrw―Left Rear Wheel/左后轮；

rfc―Right Front Wheel-Earth Contractor/右前轮地接触点；

lfc―Left Front Wheel-Earth Contractor/左前轮地接触点；

rmc―Right Middle Wheel-Earth Contractor/右中轮地接触点；

lmc―Left Middle Wheel-Earth Contractor/左中轮地接触点；

rrc―Right Rear Wheel-Earth Contractor/右后轮地接触点；

lrc―Left Rear Wheel-Earth Contractor/左后轮地接触点；

结合系统结构参数如图2；建立该系统的动力学方程。

步骤1显然，|A|＝14，|NT|＝7，|O|＝6；轮式多轴系统移动维度DOM(D)为3，故多轴系统D能适应自然路面。轴链A、父轴链及非树集合NT分别为

A＝(i,c1,c2,c3,c4,c5,c,rr,rb,rrd,rrw,rmw,rfd,rfw,lr,lb,lrd,lrw,lfd,lfw,lmw]，

步骤2：基于轴不变量的多轴系统正运动学计算

由式(46)得

由式(48)及式(181)计算

由式(49)及式(182)计算

由式(50)计算

记l∈(c,rb,rr,rfd,rfw,rmw,rrd,rrw,lb,lr,lfd,lfw,lmw,lrd,lrw],因是刚体系统，故有and由式(51),(182)及式(184)计算

由式(53)计算

步骤3建立动力学方程

由式(155)及式(156)得 由式(99)及式(100)分别计算及代入式(152)得

由式(103)，式(100)及式(96)得

其中：u∈[rfd,rfw,rmw,rrd,rrw,lfd,lfw,lmw,lrd,lrw]。

由式(106)，式(100)，式(104)及式(110)得

由式(111)及式(106)得

其中，差速轴初始角度为

由式(103)，式(100)及式(96)得

至此，获得19轴动力学方程及1个非树约束副3D代数方程；其中，包含19轴外力矩标量及1轴约束力矩矢量。

步骤4进行力反向迭代

由式(40)得

步骤5计算动力学方程逆解

增加四个舵机驱动力矩约束，

增加驱动轮力矩约束，

记

将式(194)至式(201)写为整体形式

f_20×1＝B_20×20·u_20×1 (205)

其中：

B_[1:6][1:4]＝0_6×4， (207)

记k∈[rb,rr,rfd,rfw,rmw,rrd,rrw,lb,lr,lfd,lfw,lmw,lrd,lrw]，由式(187)至式(192)求逆解得及共计21个标量；从而，由式(205)得u。

由上求解过程可知：

【1】由于该系统存在6个驱动轴及4个舵机轴，而该移动系统移动自由度为3，故存在7个冗余的控制轴。通过式(202)及式(203)人为地加入7个约束，保证了逆解存在的唯一性；

【2】通过动力学计算，不仅可以计算该系统的控制力矩，也唯一求解了六个轮土作用力；通过该移动系统运动状态的检测，应用动力学逆解，实现了轮土作用力的间接测量。

以上给出了基于Ju-Kane的20轴巡视器移动系统动力学建模及逆解、基于Ju-Kane的10轴三轮移动系统动力学建模及逆解的实施例。实施例表明：应用Ju-Kane动力学原理建立多轴系统动力学显式模型简洁、方便、高效，适宜高自由度的多轴系统动力学显式建模与数值建模。同时表明，通过显式动力学建模与计算，可以通过多轴系统的运动检测及视觉检测，计算系统与环境的接触作用力，为多轴系统力位控制及环境感知提供了新的技术途径。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。

Claims (6)

1.一种基于轴不变量的动基座多轴机器人动力学建模与解算方法，其特征是，

给定多轴刚体移动系统D＝{A,K,T,NT,F,B}，惯性系记为A为轴序列，F为杆件参考系序列，B为杆件体序列，K为运动副类型序列，NT为约束轴的序列即非树；轴序列为ⁱA_c＝(i,c1,c2,c3,c4,c5,c]，轴类型序列为ⁱK_c＝(X,R,R,R,P,P,P]，该运动链为ⁱl_c＝(i,c1,c2,c3,c4,c5,c]；除了重力外，作用于轴u的合外力及力矩在上的分量分别为及轴k的质量及质心转动惯量分别为m_k及轴k的重力加速度为驱动轴u的双边驱动力及驱动力矩在上的分量分别为及环境i对轴l的作用力及作用力矩分别为及ⁱτ_l；作用于轴c上的合力及合力矩分别为^i|Df_c及^i|Dτ_c，记则有以下动基座刚体系统的Ju-Kane动力学方程

则有

其中： ^c L表示c的开子树，^cL-c＝ ^c L，且有：

式中，及是3×3的分块矩阵，及是3D矢量；k_I表示杆k质心I；轴k的质量及质心转动惯量分别记为m_k及 为转动轴u的惯性矩阵；为平动轴u的惯性矩阵；h_R为转动轴u的非惯性矩阵；h_P为平动轴u的非惯性矩阵；为关节角速度。

2.根据权利要求1所述的基于轴不变量的动基座多轴机器人动力学建模与解算方法，其特征是，

由式(150)确定本体c的笛卡尔体系F^[c]三个转动轴的序列，在建立动力学方程后，通过积分完成动力学仿真，直接得到所期望的姿态。

3.根据权利要求1所述的基于轴不变量的动基座多轴机器人动力学建模与解算方法，其特征是，

动基座刚体系统的Ju-Kane动力学方程根据闭链刚体系统的Ju-Kane动力学方程建立。

4.根据权利要求3所述的基于轴不变量的动基座多轴机器人动力学建模与解算方法，其特征是，闭链刚体系统的Ju-Kane动力学方程为：

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

式中：及是3×3的分块矩阵，及是3D矢量；k_I表示杆k质心I；为转动轴u的惯性矩阵；为平动轴u的惯性矩阵；h_R为转动轴u的非惯性矩阵；h_P为平动轴u的非惯性矩阵。

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

其中：

式中：及是3×3的分块矩阵，及是3D矢量；k_I表示杆k质心I；轴k的质量及质心转动惯量分别记为m_k及 为转动轴u的惯性矩阵；为平动轴u的惯性矩阵；h_R为转动轴u的非惯性矩阵；h_P为平动轴u的非惯性矩阵；为平动关节角速度；为转动关节角速度。

5.根据权利要求4所述的基于轴不变量的动基座多轴机器人动力学建模与解算方法，其特征是，

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

6.根据权利要求5所述的基于轴不变量的动基座多轴机器人动力学建模与解算方法，其特征是，

树链Ju-Kane规范型方程

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

并且，

式中，k_I表示杆k质心I；轴k的质量及质心转动惯量分别记为m_k及 为转动轴u的惯性矩阵；为平动轴u的惯性矩阵；h_R为转动轴u的非惯性矩阵；h_P为平动轴u的非惯性矩阵；作用于轴u的合外力及力矩在上的分量分别记为及 作用于轴u的合外力及力矩在上的分量分别记为及驱动轴u的双边驱动力及驱动力矩在上的分量分别记为及环境i对轴l的作用力及作用力矩分别为及ⁱτ_l；^ll_k为取由轴l至轴k的运动链，^uL表示获得由轴u及其子树构成的闭子树。