基于轴不变量的多轴机器人正运动学计算方法

摘要

本发明公开了一种基于轴不变量的多轴机器人正运动学计算方法，使用轴集合来对应描述多轴机器装置，以自然坐标系为基础，利用轴集合的轴对应的轴不变量来计算多轴机器装置的控制参数；利用轴不变量的不变性建立基于轴不变量的迭代式运动学方程，并且所述迭代式运动学方程的符号对应到伪代码，反映多轴机器装置运动链的拓扑关系与链序关系；计算运动链的迭代式正运动学数值；计算基于轴不变量的迭代式偏速度。本方法通过简洁的运动链符号系统，保证了系统实现的可靠性及机械化演算；具有基于轴不变量的迭代式，保证计算的实时性；实现坐标系、极性及系统结构参量的完全参数化建模，保证模型的通用性。

权利要求

1.一种基于轴不变量的多轴机器人正运动学计算方法，其特征是，多轴机器人装置包含杆件序列与关节序列，将树链中的关节序列转换成对应的轴序列及其父轴序列，所述轴序列的轴为平动轴或转动轴；

使用轴集合来对应描述多轴机器装置，以自然坐标系为基础，利用轴集合的轴对应的轴不变量来计算多轴机器装置的控制参数；

利用轴不变量的不变性建立基于轴不变量的迭代式运动学方程，并且所述迭代式运动学方程的符号对应到伪代码，反映所述多轴机器人运动链的拓扑关系与链序关系；

计算运动链的迭代式正运动学数值；

计算基于轴不变量的迭代式偏速度。

2.根据权利要求1所述的基于轴不变量的多轴机器人正运动学计算方法，其特征是，惯性空间记为i，给定由i至杆件n的运动链iln，杆件l,n,j均∈A，n>l，s是杆件l上的任一点，A为轴序列；当转动矢量 有测量噪声时，运动链iln的迭代式正运动学数值计算步骤包括：【1】链节 正运动学计算步骤；

运动副 对应的运动链 通过区间符表示为：

其中：是l的前继即父，l是 的后继即子； 为链节，是运动链中的一个基本环节；

【2】运动链iln的位形计算步骤；

【3】运动链iln的速度及加速度计算步骤。

3.根据权利要求2所述的基于轴不变量的多轴机器人正运动学计算方法，其特征是，链节 正运动学计算步骤为：【1-1】已知转动矢量 根据欧拉四元数公式计算欧拉四元数【1-2】由欧拉四元数计算旋转变换阵

【1-3】由下式计算链节速度：

式中，运动副 表示连接杆件 及杆件l的运动副；转动副R，棱柱副P；沿轴 的线位置绕轴 的角位置 轴矢量 角速度 线速度 表达形式投影符|□表示矢量对参考基的投影矢量，参考基为惯性空间i； 为i到 的旋转变换矩阵， 为转动速度 对参考基i的投影矢量； 为平动速度 对参考基i的投影矢量；

【1-4】由下式计算链节加速度：

式中，转动加速度 平动加速度 表示角速度 的导数即角加速度， 表示线速度 的导数即线加速度。

4.根据权利要求2所述的基于轴不变量的多轴机器人正运动学计算方法，其特征是，运动链iln的位形计算步骤为：【2-1】由欧拉四元数的链关系，将四元数乘法运算用其共轭矩阵运算替代，计算欧拉四元数序列 为欧拉四元数；

【2-2】用欧拉四元数表示定轴转动；旋转变换阵的计算等价于链式四元数的矩阵计算，计算旋转变换阵序列{iQj|j∈A}，iQj为i到j的旋转变换矩阵；

【2-3】由下式计算位置矢量

式中，轴矢量 沿轴 的线位置 的含义是坐标矢量 对参考基i的投影矢量，为在自然坐标系下由杆件 到杆件l的坐标矢量；nS表示杆件n中的点S， 为由i至nS的运动链；表达形式投影符|□表示矢量对参考基的投影矢量，参考基为惯性空间i； 为零位时的平动矢量 对参考基i的投影矢量， 为轴矢量 对参考基i的投影矢量。

5.根据权利要求2所述的基于轴不变量的多轴机器人正运动学计算方法，其特征是，运动链iln的速度及加速度计算步骤为：【3-1】计算绝对角速度：

式中，角速度 轴不变量 为由 至k转动矢量，k为属于运动链iln的杆件，是k的前继即父，表达形式投影符|□表示矢量对参考基的投影矢量，参考基为惯性空间i，为轴不变量 对参考基i的投影矢量， 为转动速度 对参考基i的投影矢量；

【3-2】计算绝对角加速度：

式中，角加速度 轴矢量 为转动加速度 对参考基i的投影矢量；

【3-3】计算绝对平动速度：

式中，线速度 轴不变量 nS表示杆件n中的点S，由n至nS的坐标矢量 为由参考基i至杆件n的角速度叉乘矩阵， 为矢量 对参考基i的投影矢量； 为 的导数；

【3-4】计算绝对平动加速度：

式中，线速度 轴不变量 表达形式投影符|□表示矢量对参考基的投影矢量，参考基为惯性空间i， 为轴不变量 对参考基i的投影矢量； 为零位时由原点 至原点Ol的平动矢量对参考基i的投影矢量， 为由参考基i至 的角速度叉乘矩阵； 为 的2次幂；

其中： —转动加速度，其中 向心加速度； —哥氏加速度，是平动与转动的耦合加速度。

6.根据权利要求2所述的基于轴不变量的多轴机器人正运动学计算方法，其特征是，基于轴不变量的迭代式偏速度计算步骤包括：定义使能函数：

式中，k为属于运动链iln的杆件；

由使能函数，

【1】计算绝对角速度对角速度的偏速度；

【2】计算绝对平动速度矢量对关节平动速度的偏速度；

【3】计算绝对转动矢量对关节角度的偏速度；

【4】计算绝对位置矢量对关节位移的偏速度；

【5】计算绝对位置矢量对关节角度的偏速度；

【6】计算绝对平动速度矢量对角速度的偏速度。

7.根据权利要求6所述的基于轴不变量的多轴机器人正运动学计算方法，其特征是，绝对角速度对角速度的偏速度计算公式为：式中，角速度 轴不变量 表达形式投影符|□表示矢量对参考基的投影矢量，参考基为惯性空间i， 为轴不变量 对参考基i的投影矢量；iφn′为绝对角速度。

8.根据权利要求6所述的基于轴不变量的多轴机器人正运动学计算方法，其特征是，绝对平动速度矢量对关节平动速度的偏速度计算公式为：式中，线速度 轴不变量 表达形式投影符|□表示矢量对参考基的投影矢量，参考基为惯性空间i， 为轴不变量 对参考基i的投影矢量， 为绝对平均速度矢量；

绝对平动速度矢量对角速度的偏速度计算公式为：

式中，角速度 是轴不变量 的叉乘矩阵，表达形式投影符|□表示矢量对参考基的投影矢量，参考基为惯性空间i，nS表示杆件n中的点S，由k至nS的位置矢量

9.根据权利要求6所述的基于轴不变量的多轴机器人正运动学计算方法，其特征是，绝对转动矢量对关节角度的偏速度计算公式为：式中，角位置 轴不变量 kln为杆件k至杆件n的运动链，表达形式投影符|□表示矢量对参考基的投影矢量，参考基为惯性空间i， 为轴不变量 对参考基i的投影矢量，为转动矢量 对参考基i的投影矢量。

10.根据权利要求6所述的基于轴不变量的多轴机器人正运动学计算方法，其特征是，绝对位置矢量对关节位移的偏速度计算公式为：式中，线位置 轴不变量 表达形式投影符|□表示矢量对参考基的投影矢量，参考基为惯性空间i， 为轴矢量 对参考基i的投影矢量；nS表示杆件n中的点S，由i至nS的位置矢量绝对位置矢量对关节角度的偏速度计算公式为：

式中，角位置 是轴不变量 的叉乘矩阵，由杆件k至nS的位置矢量

说明书

基于轴不变量的多轴机器人正运动学计算方法

技术领域

[0001] 本发明涉及一种多轴机器人正运动学计算方法，属于机器人技术领域。

背景技术

[0002] 机器人是目前非常热门的领域。这个领域过去几十年已经投入了大量的科学与工程人力，而且研究了多年。然而，一旦遇到轴的数目与自由度增加到一定数量的时候，按照现有的教科书以及已知的观察、建模、计算与控制方式，往往会陷入复杂失控，甚至无法求解的难题。

[0003] 首先，过去的做法缺少一般化的能力。对于不同的机器人往往需要重新研究，建立对应的运动学与力学模型。

[0004] 其次，过去的做法在建模的过程，所用的图示以及语言通常是不精确也不完整的。这导致许多的参数在建模的初期并没有被考虑进去。后续的整个建模，包括程序化代码的编写时，就必须个别考虑跟解决之前没有考虑的参数与细节。这对于复杂系统，例如自由度更高的机器人应用时，往往意味着大量的隐藏臭虫(bug)会藏在整个建模的系统里头。这会影响整个系统开发的效率，并且透过这种没有完整考量下开发出来的系统，往往有很多稳定性问题难以解决。

[0005] 此外，过去的做法在遇到复杂度比较高的时候，运算量会大幅增加或甚至找不出解答，以及计算精度大受影响。换言之，对于需要即时运算控制以达成自主控制的机器人来说，就成了重大的缺陷。

[0006] 因此，虽然现在已经有很多的机器人相关的理论，但是却仍然缺少一个完整有效的设计框架与对应的运算与控制方法，可以在各种不同机器人实际开发过程中，方方面面地解决建模，模型内的运算结构与规则，到正运动学，逆运动学，以及力学计算的相关问题。

发明内容

[0007] 本发明所要解决的技术问题是提供一种基于轴不变量的多轴机器人正运动学计算方法。

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

[0009] 一种基于轴不变量的多轴机器人正运动学计算方法，其特征是，

[0010] 多轴机器人装置包含杆件序列与关节序列，将树链中的关节序列转换成对应的轴序列及其父轴序列，所述轴序列的轴为平动轴或转动轴；

[0011] 使用轴集合来对应描述多轴机器装置，以自然坐标系为基础，利用轴集合的轴对应的轴不变量来计算多轴机器装置的控制参数；

[0012] 利用轴不变量的不变性建立基于轴不变量的迭代式运动学方程，并且所述迭代式运动学方程的符号对应到伪代码，反映所述多轴机器装置运动链的拓扑关系与链序关系；

[0013] 计算运动链的迭代式正运动学数值；

[0014] 计算基于轴不变量的迭代式偏速度。

[0015] 惯性空间记为i，给定由i至杆件n的运动链iln，杆件l,n,j∈A，n>l，s是体l上的任一点，A为轴序列；当转动矢量 有测量噪声时，运动链iln的迭代式正运动学数值计算步骤包括：

[0016] 【1】链节 正运动学计算步骤；

[0017] 运动副 对应的运动链 通过区间符表示为：

[0018] 其中：是l的前继即父，l是 的后继即子； 为链节，是运动链中的一个基本环节；

[0019] 【2】运动链iln的位形计算步骤；

[0020] 【3】运动链iln的速度及加速度计算步骤。

[0021] 链节 正运动学计算步骤为：

[0022] 【1-1】已知转动矢量 根据欧拉四元数公式计算欧拉四元数

[0023] 【1-2】由欧拉四元数计算旋转变换阵

[0024] 【1-3】由下式计算链节速度：

[0025]

[0026] 式中，运动副 表示连接杆件l及杆件l的运动副；转动副R，棱柱副P；沿轴 的线位置 绕轴 的角位置 轴矢量 角速度 线速度

[0027] 【1-4】由下式计算链节加速度：

[0028]

[0029] 式中，转动加速度 平动加速度

[0030] 运动链iln的位形计算步骤为：

[0031] 【2-1】由欧拉四元数的链关系，将四元数乘法运算用其共轭矩阵运算替代，计算欧拉四元数序列

[0032] 【2-2】用欧拉四元数表示定轴转动；旋转变换阵的计算等价于链式四元数的矩阵计算，计算旋转变换阵序列{iQj|j∈A}；

[0033] 【2-3】由下式计算位置矢量

[0034]

[0035] 式中，轴矢量 沿轴 的线位置

[0036] 运动链iln的速度及加速度计算步骤为：

[0037] 【3-1】计算绝对角速度：

[0038]

[0039] 式中，角速度 轴矢量

[0040] 【3-2】计算绝对角加速度：

[0041]

[0042] 式中，角加速度 轴矢量

[0043] 【3-3】计算绝对平动速度：

[0044]

[0045] 式中，线速度 轴矢量

[0046] 【3-4】计算绝对平动加速度：

[0047]

[0048] 式中，线速度 轴矢量

[0049] 其中： —转动加速度，其中 向心加速度； —哥氏加速度，是平动与转动的耦合加速度。

[0050] 基于轴不变量的迭代式偏速度计算步骤包括：

[0051] 定义使能函数：

[0052]

[0053] 式中，k为属于运动链iln的杆件；

[0054] 由使能函数，

[0055] 【1】计算绝对角速度对关节角速度的偏速度；

[0056] 【2】计算绝对平动速度矢量对关节平动速度的偏速度；

[0057] 【3】计算绝对转动矢量对关节角度的偏速度；

[0058] 【4】计算绝对位置矢量对关节位移的偏速度；

[0059] 【5】计算绝对位置矢量对关节角度的偏速度；

[0060] 【6】计算绝对平动速度矢量对关节角速度的偏速度。

[0061] 绝对角速度对关节角速度的偏速度计算公式为：

[0062]

[0063] 式中，角速度 轴矢量

[0064] 绝对平动速度矢量对关节平动速度的偏速度计算公式为：

[0065]

[0066] 式中，线速度 轴矢量

[0067] 绝对平动速度矢量对关节角速度的偏速度计算公式为：

[0068]

[0069] 式中，角速度 是轴不变量 的叉乘矩阵。

[0070] 绝对转动矢量对关节角度的偏速度计算公式为：

[0071]

[0072] 式中，角位置 轴矢量

[0073] 绝对位置矢量对关节位移的偏速度计算公式为：

[0074]

[0075] 式中，线位置 轴矢量

[0076] 绝对位置矢量对关节角度的偏速度计算公式为：

[0077]

[0078] 式中，角位置 是轴不变量 的叉乘矩阵。

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

[0080] 本发明的方法提出并证明了基于轴不变量的迭代式运动学实时数值建模方法，包含：基于轴不变量的迭代式的位置矢量、转动矢量、速度矢量、加速度矢量及偏速度矢量的计算方法。具有简洁的运动链符号系统，具有伪代码的功能，具有迭代式结构，保证了系统实现的可靠性及机械化演算；具有基于轴不变量的迭代式，保证计算的实时性；实现坐标系、极性及系统结构参量的完全参数化建模，保证模型的通用性，避免了与系统接口与用户接口的转换，通过轴不变量构建了内在紧凑系统，提高了运动学计算的实时性及功能复用性能；轴运动矢量的统一表达及简洁的结构化层次模型不仅有助于简化多轴系统运动学建模过程，而且为基于轴不变量的多轴系统动力学建模奠定了基础。

附图说明

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

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

[0083] 图3偏速度的含义示意图。

具体实施方式

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

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

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

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

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

[0089]

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

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

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

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

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

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

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

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

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

[0099]

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

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

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

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

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

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

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

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

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

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

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

[0111]

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

[0113]

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

[0115]

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

[0117] 故关节 的控制量 为

[0118]

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

[0120]

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

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

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

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

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

[0126]

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

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

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

[0130]

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

[0163] －平动轴 的机械零位，

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

[0180] 欧拉四元数表达形式：

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

[0182] 运动链是一个偏序的链；而运动副 既表示由杆件 至杆件l的连接，又表示由杆件l至杆件 的连接，故运动副 具有全序；故有

[0183]

[0184] 显然，全序及偏序是一个对象自身的属性。而力学及机器人理论上尚未出现相应的符号系统。

[0185] 借鉴集合论的链理论，将运动副 对应的简单运动链 通过区间符表示为

[0186]

[0187] 其中：是l的前继即父，l是 的后继即子；称 为链节，是运动链中的一个基本环节。

[0188] 在Span树中，简单运动链 与l一一映射，即

[0189]

[0190] 故有

[0191]

[0192] 因有序集合的子集也是有序的集合，故定义由 至κ的运动链 为

[0193]

[0194] 记 为 的前继(Predecessor)。故有

[0195]

[0196] 同样，因有序集合的子集也是有序的集合，故有

[0197] ili＝(i,i],|ili|＝0。   (7)

[0198] 称ili为空链或平凡链。惯性空间(环境)记为i，平凡链ili总是存在的。

[0199] 1.基于轴不变量的迭代式运动学计算方法

[0200] 给定运动链iln，轴l,n∈A，n>l，s是体l上的任一点，A为轴序列。当转动矢量 有测量噪声时，运动链iln的迭代式正运动学数值计算步骤为：

[0201] 【1】链节 正运动学计算步骤

[0202] 【1-1】已知转动矢量 根据式(9)计算欧拉四元数

[0203] 定义四元数 及保证模不变的共轭四元数

[0204]

[0205] 四元数 的虚部与实部表示的是不变量，故左上角指标不表示参考系，而仅表示链的作用关系。因此， 可视为四维空间的复数，其中 是实部， 是虚部。通过研究四维空间复数，人们认识了欧拉四元数。 前三个数构成矢量，对应基i的坐标，最后一个是实部，即有

[0206] 因4D复数的矢部参考基是唯一的自然参考基，故四维复数的左上角的参考指标仅表明运动关系，已失去投影参考系的含义，具有不同左上角指标的4D复数可以进行代数运算。尽管参考指标在4D复数中无意义，但不表明指标关系无意义，因为复数的乘除运算与复数的作用顺序密切相关。

[0207]

[0208] 【1-2】由式(10)计算旋转变换阵

[0209]

[0210] 显然，有

[0211]

[0212] 【1-3】由式(12)计算链节速度：

[0213]

[0214] 运动副 表示连接杆件 及杆件l的运动副；转动副R，棱柱副P；关节转动角度矢量 位置矢量 轴矢量 角速度 线速度 角速度

[0215] 【1-4】由式(13)计算链节加速度：

[0216]

[0217] 转动加速度 平动加速度

[0218] 【2】运动链iln的位形计算步骤

[0219] 【2-1】由式(19)计算欧拉四元数序列

[0220] 由欧拉四元数的链关系，四元数 乘法运算可用其共轭矩阵 运算替代，有[0221]

[0222] 其中：

[0223]

[0224] 且有 称 为 的共轭矩阵。同时，因为四元数是四维空间复数，矢部对参考基的矢量投影应相对于同一个参考基。称式(14)为四元数串接性运算，与齐次变换相对应。因此，序列姿态运算具有运动链串接性；与矢量叉乘运算相似，四元数乘可应用相应的共轭矩阵替代。

[0225] 当给定角度 后，其正、余弦 及其半角的正、余弦Sl、Cl均是常数；为方便表达，记

[0226]

[0227] 由式(15)及式(16)，结合欧拉四元数，得

[0228]

[0229] 式(14)应用计算机编程实现时，可用下式替代。

[0230]

[0231] 式(18)仅包含16个乘法运算及12个加法运算。而 需要进行27个乘法运算及18个加法运算。在得到 后，计算 及 再由式(21)计算 是4·4的矩
阵，其构成如下：第4列为右手序的四元数 第4行为左手序的四元数 即 左上3×
3包含为 其中： 的右上三角为右手序的矢量 的左下三角为左手序的
矢量 即 的主对角为 的第4个元素。

[0232] 由式(18)得

[0233]

[0234] 式(14)表示的是位置矢量转动算子，即表示的是转动。因此，欧拉四元数乘积运算对应旋转变换阵的乘积运算。因此旋转变换链等价于定轴转动链，即

[0235]

[0236] 由上可知，欧拉四元数可以唯一确定旋转变换阵；旋转变换阵也可以唯一确定欧拉四元数，即欧拉四元数与旋转变换阵等价。转动矢量与规范四元数一一对应，即四元数表示定轴转动；旋转变换阵的计算等价于链式四元数的矩阵计算。

[0237] 【2-2】因式(10)较式(21)计算复杂度高，故由式(21)计算旋转变换阵序列{iQj|j∈A}；

[0238]

[0239] 式(21)是关于 和 的多重线性方程，是轴不变量 的二阶多项式。给定自然零位矢量 作为 的零位参考，则 及 分别表示零位矢量及径向矢量。式
(21)即为 对称部分 表示零位轴张量，反对称部分
表示径向轴张量，分别与轴向外积张量 正交，从而确定三维自然轴空间；
式(21)仅含一个正弦及余弦运算、6个积运算及6个和运算，计算复杂度低；同时，通过轴不变量 及关节变量 实现了坐标系及极性的参数化。

[0240] 【2-3】由式(22)计算位置矢量

[0241]

[0242] 【3】运动链iln的速度及加速度步骤

[0243] 【3-1】由式(23)计算绝对角速度

[0244]

[0245] 上式可由式(24)

[0246]

[0247] 得

[0248]

[0249] 绝对导数 表示对投影坐标系i求绝对导数；角速度 轴矢量 式(24)表明：绝对角速度与相对角速度是等价的。

[0250] 【3-2】由式(25)计算绝对角加速度

[0251]

[0252] 上式可由式(26)

[0253]

[0254] 得

[0255]

[0256] 转动加速度

[0257] 【3-3】由式(27)计算绝对平动速度

[0258]

[0259] 上式可由式(28)求得，

[0260]

[0261] 称式(28)为“正序的绝对求导式”，牵连项 是由投影参考系i至测量参考系l的角速度叉乘矩阵； 结果以投影坐标系i为参考，所有和项的投影参考系具有一致性。

[0262] 【3-4】由式(29)计算绝对平动加速度

[0263]

[0264] 上式可由式(30)得

[0265]

[0266] 其中： —平动加速度； —转动加速度，其中 向心加速度； —哥氏加速度，是平动与转动的耦合加速度。

[0267] 由式(30)可知，平动加速度 是矢量，具有可加性。

[0268] 2.基于轴不变量的偏速度计算方法

[0269] 现有技术中通常采用雅克比矩阵的计算方法，但均未对结论进行证明且结论不全面。在运动学及动力学分析时，将雅克比矩阵称为偏速度更合适。因为雅克比矩阵泛指偏导数，不一定具有可加性；而在运动学及动力学中偏速度特指矢量对关节变量的偏导数，具有可加性。偏速度是对应速度的变换矩阵，是对单位方向矢量的矢量投影。在运动学分析及动力学分析中，偏速度起着关键性的作用，偏速度的计算是动力学系统演算的基本前提。

[0270] 首先，定义使能(Enable)函数，

[0271]

[0272] 式中，k为属于运动链ill的杆件；

[0273] 式(31)的特殊形式为

[0274]

[0275] 下面，说明基于轴不变量的迭代式偏速度计算步骤：

[0276] 【1】根据式(33)计算绝对角速度对关节角速度的偏速度，

[0277]

[0278] 上式可由式(23)得

[0279]

[0280] 【2】根据式(34)计算绝对平动速度矢量对关节平动速度的偏速度，

[0281]

[0282] 即

[0283]

[0284] 【3】根据式(35)计算绝对转动矢量对关节角度的偏速度，

[0285]

[0286] 上式可由式(23)得

[0287]

[0288] 【4】根据式(36)计算绝对位置矢量对关节位移的偏速度，

[0289]

[0290] 上式可由式(22)得

[0291]

[0292] 【5】根据式(37)计算绝对位置矢量对关节角度的偏速度，

[0293]

[0294] 上式可由式(27)得

[0295]

[0296] 即

[0297]

[0298] 故有

[0299]

[0300] 【6】根据式(38)计算绝对平动速度矢量对关节角速度的偏速度，

[0301]

[0302] 上式可由式(27)得

[0303]

[0304] 将上述结论，以专利中对应的式(39)、(40)、(41)统一表述，称之为偏速度定理。

[0305] 若给定运动链运动链iln，则有

[0306]

[0307]

[0308]

[0309] 当 时，由式(35)，(33)，(36)及(34)可得式(39)。

[0310] 由式(37)及(38)得式(40)。因 和 与 和 无关，得式(41)。

[0311] 式(35)至式(38)对运动学及动力学分析具有非常重要的作用。它们不仅物理意义清晰，还可以简化运动学及动力学方程的表达。

[0312] 如图3所示，一方面，从几何角度看，式(39)中的偏速度即为对应的轴不变量，式(40)表示的是位置矢量对轴不变量的一阶距，即轴矢量 与矢量 的叉乘；另一方面，从力作用关系看， 是 在轴向 的投影。

[0313] 由式(42)左序叉乘与转置的关系式

[0314]

[0315] 可知

[0316]

[0317] 式(43)表明： 完成了力 对轴 作用效应即力矩的计算。

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

[0319] 3.轴不变量对时间微分的不变性

[0320] 由式(24)及式(26)可知

[0321]

[0322] 故有

[0323]

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

[0325] 由式(39)及式(45)得

[0326]

[0327] 由式(28)及式(45)得

[0328]

[0329] 由上式得

[0330]

[0331] 即

[0332]

[0333] 由式(47)可知：偏速度对时间t的导数仍是轴不变量的迭代式。轴不变量 是基el的坐标矢量， 本质上表示基el在参考系i上的投影。若式(45)不成立，则否认了参考基el作为参考的不变性即客观性。由 得

[0334]

[0335] 式(48)中左式表示：转动链ilc的DCM对该链全部关节角的偏速度之和；式(48)中右式表示：转动链ilc的轴不变量之和。因此，运动链的DCM对关节角的偏速度具有不变性。

[0336] 对于MAS系统的树 由式(45)及式(48)得

[0337]

[0338] 表明：MAS系统的轴不变量对时间是不变的，即刚体系统的自然参考轴具有不变性。由式(48)可知：系统的关节变量与自然参考轴一一映射，体的关节变量数由其独立的运动维度确定，但不改变自然参考轴对时间微分的不变性。

[0339] 4.树形运动链的变分计算步骤

[0340] 将函数自变量的导数称为微商，以d表示。与微分相对应，将自变量函数的增量称为变分，以δ表示；但变分不考虑时间t的增量δt,即δt≡0。正是因为不考虑时间增量δt，故线位移及角位移的变分理解为同一时刻t可能的运动量变化，即虚位移。

[0341] 【1】转动矢量的变分

[0342]

[0343] 上式可由式(33)得

[0344]

[0345] 【2】平动矢量的变分

[0346]

[0347] 上式可由式(37)及式(38)得

[0348]

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