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

Abstract

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

Description

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

技术领域

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

背景技术

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

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

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

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

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

发明内容

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

式中，转动加速度平动加速度

运动链ⁱl_n的位形计算步骤为：

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

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

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

式中，轴矢量沿轴的线位置

运动链ⁱl_n的速度及加速度计算步骤为：

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

式中，角速度轴矢量

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

式中，角加速度轴矢量

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

式中，线速度轴矢量

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

式中，线速度轴矢量

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

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

定义使能函数：

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

由使能函数，

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

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

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

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

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

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

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

式中，角速度轴矢量

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

式中，线速度轴矢量

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

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

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

式中，角位置轴矢量

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

式中，线位置轴矢量

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

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

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

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

附图说明

图1自然坐标系与轴链；

图2固定轴不变量；

图3偏速度的含义示意图。

具体实施方式

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

定义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的父轴；轴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 L为不含l的子树；递归执行 ^ll，计算复杂度为

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

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

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

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

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

本申请中约定：在运动链符号演算系统中，具有偏序的属性变量或常量，在名称上包含 表示偏序的指标；要么包含左上角及右下角指标，要么包含右上角及右下角指标；它们的方 向总是由左上角指标至右下角指标，或由右上角指标至右下角指标，本申请中为叙述简便， 有时省略方向的描述，即使省略，本领域技术人员通过符号表达式也可以知道，本申请中采 用的各参数，对于某种属性符，它们的方向总是由偏序指标的左上角指标至右下角指标，或 由右上角指标至右下角指标。例如：可简述为(表示由k至l)平动矢量；表示(由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次幂。[□]^T表示□的转置，表示对集合转置，不对成员执行转置；如：

为投影符，表示矢量或二阶张量对参考基的投影矢量或投影序列，即坐标矢量或坐标 阵列，投影即是点积运算“·”；如：位置矢量在坐标系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四元数表达形式：

欧拉四元数表达形式：

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

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

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

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

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

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

故有

因有序集合的子集也是有序的集合，故定义由至k的运动链为

记为的前继(Predecessor)。故有

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

ⁱl_i＝(i,i],|ⁱl_i|＝0。 (7)

称ⁱl_i为空链或平凡链。惯性空间(环境)记为i，平凡链ⁱl_i总是存在的。

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

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

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

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

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

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

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

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

显然，有

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

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

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

转动加速度平动加速度

【2】运动链ⁱl_n的位形计算步骤

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

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

其中：

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

当给定角度后，其正、余弦及其半角的正、余弦S_l、C_l均是常数；为方便表达，记

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

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

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

由式(18)得

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

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

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

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

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

【3】运动链ⁱl_n的速度及加速度步骤

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

上式可由式(24)

得

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

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

上式可由式(26)

得

转动加速度

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

上式可由式(28)求得，

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

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

上式可由式(30)得

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

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

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

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

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

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

式(31)的特殊形式为

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

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

上式可由式(23)得

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

即

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

上式可由式(23)得

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

上式可由式(22)得

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

上式可由式(27)得

即

故有

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

上式可由式(27)得

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

若给定运动链运动链ⁱl_n，则有

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

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

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

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

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

可知

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

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

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

由式(24)及式(26)可知

故有

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

由式(39)及式(45)得

由式(28)及式(45)得

由上式得

即

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

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

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

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

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

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

【1】转动矢量的变分

上式可由式(33)得

【2】平动矢量的变分

上式可由式(37)及式(38)得

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

Claims (10)

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

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

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

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

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

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

2.根据权利要求1所述的基于轴不变量的多轴机器人正运动学计算方法，其特征是，

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

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

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

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

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

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

3.根据权利要求2所述的基于轴不变量的多轴机器人正运动学计算方法，其特征是，

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

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

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

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

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

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

式中，转动加速度平动加速度

4.根据权利要求2所述的基于轴不变量的多轴机器人正运动学计算方法，其特征是，

运动链ⁱl_n的位形计算步骤为：

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

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

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

式中，轴矢量沿轴的线位置

5.根据权利要求2所述的基于轴不变量的多轴机器人正运动学计算方法，其特征是，

运动链ⁱl_n的速度及加速度计算步骤为：

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

式中，角速度轴矢量

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

式中，角加速度轴矢量

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

式中，线速度轴矢量

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

式中，线速度轴矢量

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

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

定义使能函数：

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

由使能函数，

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

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

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

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

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

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

7.根据权利要求6所述的基于轴不变量的多轴机器人正运动学计算方法，其特征是，绝对角速度对关节角速度的偏速度计算公式为：

式中，角速度轴矢量

8.根据权利要求6所述的基于轴不变量的多轴机器人正运动学计算方法，其特征是，

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

式中，线速度轴矢量

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

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

9.根据权利要求6所述的基于轴不变量的多轴机器人正运动学计算方法，其特征是，

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

式中，角位置轴矢量

10.根据权利要求6所述的基于轴不变量的多轴机器人正运动学计算方法，其特征是，

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

式中，线位置轴矢量

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

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