一种基于轴不变量的多轴机器人系统正运动学建模与解算方法

摘要

本发明提出了一个基于轴不变量的多轴系统正运动学建模与解算方法。该方法实现了内在紧凑的、实时及功能复用及精简的层次化及完全参数的建模与实时解算，具有伪代码及符号分析的功能，可以设置成电路、代码，直接或间接，部分或全部在多轴机器系统内部执行。此外，本发明也包括在这些原理上建构的分析验证系统，用来设计跟验证多轴机器系统。

说明书

技术领域

本发明关于一种机器人、机器人自主控制系统与机器人自主控制系统内运用的方法，且特别关于一种多轴机器人，多轴机器人自主控制系统与多轴机器人自主控制系统内运用的方法。

背景技术

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

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

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

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

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

发明内容

因此，本发明提出了一个可使用在各种不同的多轴机器人设计中，从建模、正运动学、逆运动学以及力学方程式各个不同角度都能完整、有效解决工程实际要面对的估算与控制问题的方法。

对于熟悉机器人设计领域的人员，他们可以透过本发明文件的说明，使用不同的编程工具、电路，设计出符合本发明概念的各种多轴机器人、用在这些多轴机器人内的自主控制系统，以及在这些自主控制系统内执行的运算方法。

换言之，对于机器人设计领域的技术人员来说，他们可以用下列的发明，设计出一个完整的机器人，或是针对一个已经存在的机器人，修改或替换内部的运算与控制电路，或是撰写根据本发明的程序代码，载入已经存在的机器人的控制系统，让这些程序代码透过跟这个控制系统进行结合而达成本发明的技术效果。这些不同的切入方式，只要符合本发明文件所介绍的精神，都应该被认为属于本发明所涵盖的范围。

必须说明的是，在目前的电子与机械领域中，软件以及硬件的关联与组合变化非常多元。换言之本发明可以实现成一个机器人，本发明也可以是一组可被机器人远端下载的程序代码，或甚至透过网路远端连线，将运算一部分或全部由机器人实体以外的电脑或伺服器来加以执行。实现技术手段的多样性，不应该影响本发明应该被保护的范围，也不应该认为本发明可能实现方法包括程序代码或看起来涉及相当多的数学代号，就被归类为科学原理或数学方法。由于本发明的一系列概念可以落实为具体的技术方案，解决实际的技术问题，应该被认为确实属于专利法所要保护的范围。

以下所说明的内容，虽然会涉及相当多的数学，但是这些数学是基于工程技术观点对于实际多轴机器人进行观察所建立的模型。并且，这些数学代号很大比例可以转换成可编写程序的伪代码。这些伪代码，包括所设计的矩阵运算、方程式求解，都可以透过针对不同处理器或电路，编写成对应的程序，或全部或部分设计成对应的电路以加速运算。

这些数学代号背后代表的都是实际上各种机器人各个关节的马达输入或输出数值，各个手臂段的质量、角度、位置、力矩，轮子与地面的摩擦力、变形、转动角度等等。换言之，这个发明能够达成并且希望被保护的是根据这一系列完整的发明创新概念，所衍生出来的机器人、机器人自主控制系统以及机器人自主控制系统的方法，而非表面上的数学或物理原理，应该是属于可以被专利保护的标的。

由于这整个发明比较复杂，这份专利说明文件会尽可能地将各个环节的步骤加以说明。并且，由于整个发明涉及的方法本身就是在处理一个复杂系统，相关的说明初步看起来会相当复杂。但是，一旦对其理解后，就会发现这些看起来复杂的表述以及跟过去不同的运算方式，可以大幅度降低复杂度。

透过以下所说明的方法，首先可以完整地覆盖从各种看起来不同的探勘车辆、工具机器手臂、仿生机器人，用同样的逻辑跟语言建立模型。并且，透过以下说明的方法，不管是机器人设计领域中最重要的正运动学、逆运动学或力学方程式求解，都能够有清楚简单的操作步骤，并根据这些操作步骤编写成对应的代码或电路，完成具体的机器人技术方案。而且，这些计算由于发明人全面性地观察了多轴机器人背后技术问题的本质，找出这个非常多人研究却没有人特别找出的关键点，包括从轴不变量来建构模型、建构正运动学、逆运动学、运动方程式以及求解。察觉他人未察觉的问题、针对技术问题提出可以解决技术问题的手段，并且这个技术手段可以产生意想不到的功效，这些都应该属于非显而易见，进而足以让本发明具有专利法上的进步性。

以下先针对本发明背后原理主要的特征加以说明，设计机器人领域中的技术人员可以参考实施方式里头附带的完整说明，在理解后根据各种不同的设计需求，设计出符合本发明概念的多轴机器人、多轴机器人的自主控制系统，或是在多轴机器人控制系统上面执行的方法。

首先，本发明针对多轴机器人提出了一个完整的观察方式，针对这个观察方式，可以将各种不同的多轴机器人的不同元件，甚至看起来不同的关节，用同构(isomorphic)的完整方式，以一套清晰的图示符号与语言加以描述。

换言之，给定一个具体的多轴机器人，例如探勘车、工具机台的机器手臂、仿生机器人，设计者可以快速并且完整地描述这个多轴机器人。这套符号跟语言有助于描述整个机器人运动学跟力学涉及的相关问题。然而，必须说明的是，发明人利用了这套图示符号跟语言以及对应的运动学与力学，成功地设计了多种不同的多轴机器人自主控制系统。一般设计者也可以直接利用这套图示符号与语言。但是，即使使用者运用看起来不同的符号名称，只要符合本发明所揭露的发明概念，应该也属于本发明的范围。

透过这套清晰的图示符号与语言，设计者可以将一个多轴机器人的各种不同的关节，转换成等价的多个平动以及转动轴。这些轴之间可以构成有向Span树以及偏序运动链。以下的发明详细说明会解释这些参考现代集合论设计的符号以及语言，不但跟所关注的机器人系统产生同构的关系，而且本身构成了一个具有完备性的模型。为了证明跟显示这套语言系统的完备性，我们在专利说明文件甚至提供了相关公理的推导与证明。

这个由运动链构成的拓扑空间，可以进行下述说明的运动轴符号与操作，并且这些操作会符合一个对应真实世界机器人的物理特性。举例来说，对于有三个杆件透过两个转动轴连接的多轴机器人，当这些转动轴内部的马达转动一个设定角度时，这三个杆件彼此的相对位置会彼此牵连。此时，如何计算这三个杆件的末端会从哪个位置移动到哪个位置，就属于机器人学里头的正运动学。这会涉及到多个轴坐标系之间的迭代计算。设计者可以在下面的对应章节，可以看到这些符号系统跟本发明提出的技术特征，例如应用轴不变量的特性，如何用在正运动学的各种技术问题进行运作与计算。设计者在看完这些说明后，可以根据不同的设计需求，编写出对应的程序代码或设计对应的电路。这些编写程序代码的细节或电路设计的不同方法，设计者应该可以在一般的电子电路设计与编程参考书籍中取得相关的信息，本发明文件不会对于这些细节的程序编码或电路设计进行累赘的描述。

此外，如果已经设定对应的结果，例如希望将多轴机器人的末端吸盘，移动到多轴机器人利用摄像头等机器视觉所判断的某个特定位置以拾取某个物件，这时候如何设定每个轴关节马达的转动或移动角度，就构成逆运动学的主要问题。

当然，除了计算多轴机器人不同杆件的特定位置对应的轴操作量，有些杆件本身具有限制条件，例如在移动过程当移动到某个相对坐标角度会被某个壳体挡住。或是，工作环境有一些位置属于某些杆件不能进入的区域。这些限制可以作为逆运动学求解过程中的限制参数。

这些运算会涉及大量的坐标迭代计算以及求解。举例来说，下列的实施方式章节会说明如何用本发明基于轴不变量，更有效地计算机器人学里头很关键的D-H参数。并且，本发明的方法可以有效地计算高阶多元方程的对应解。这些解对应实际上各个轴关节的操作位移或转动角度。因此，并非单纯的科学原理或是数学公式，而是属于真实能应用、也是发明人希望能够保护的技术方案。

透过本发明的语言符号系统，轴不变量下建构的相关矩阵运算，以及针对目标位置的求解方程式的操作，这些逆运动学都可以相较于现行的逆运动学找出更有效率的运算方法。如上所述，这些运算方法可以编写成对应的程序代码或电路。

必须说明的是，设计者即使使用名称不同但实质意义相同的运算方法来设计对应的控制代码或电路时，应该仍然属于本发明的范围。再次强调，本发明范围并不限制于所提供的语言符号。

透过这样的正运动学、逆运动学，发明人并且提供了一个具体的探勘车实施例子，以证明这整套方法的具体可行。

接着透过下列的实施说明，发明人进一步基于运动学以及这套语言符号系统，以及相关的轴不变量的特性，建构了一整套的多轴机器人的力学方程。有了这套完整的力学方程，只要给定这套力学方程里头一部分已知变数数值，就可以透过对于这套力学方程的求解，找出其他变数的数值。例如，在这个力学方程中，输入多轴机器人杆件的质量、各个轴关节的移动、转动速度、加速度等，就可以对于多轴机器人不同位置的受力进行估算。此外，如同逆运动学所要解决的问题，这套力学方程也可以用来计算，当多轴机器人某个端点希望受力或施力是多少的目标值时，应该如何去操作各个轴关节的马达输出功率。

虽然过去已经有一些用来计算机器人领域的力学方程，但是本发明针对多轴机器人所提出的力学方程不管在计算效率或是能够求解的维度上，都相较于传统的力学方程有明显的优势。

此外，本发明提出的运动学或力学运算，设计者可以从下列的说明发现，有非常大量的运算是基于简洁的语言符号跟对应的矩阵运算。这些语言符号与对应的矩阵运算的特性使得这些运算可以很方便的对应到伪代码。甚至，设计者会发现大部分的伪代码背后的运算很大比例可以平行计算。换言之，使用目前的多处理器电路，搭配对应的编码，可以让整个运动学或力学的计算在很短的时间内达成。这对于需要即时得到结果，以便运用这些结果来进行多轴机器人参数设定，例如对于马达的控制电流设定的应用，具有非常实际的意义。

举例来说，即使过去的力学方程可以针对自主控制系统所需要解决问题，算出相同的解答。但是，如果不能在硬件电路的计算时间限制内即时算出，也无法顺利构成一个有用的机器人自主控制系统。

因此，这个发明可以实做成各种不同的形式，并且因此具有各种不同的范围。实际的范围请参照权利要求章节所定义的范围，以及针对这个发明的相关专利案件。

对于机器人控制系统来说，能够完整地掌握各个参数跟细节是非常有价值。举例来说，投放到灾区或甚至月球表面的探勘车，如果因为设计上的缺陷造成判断错误，造成的损失可能是非常高昂的代价。

此外，对于能够用统一的符号系统以及对应的运动学、力学方程来面对跟设计看起来可能完全不同的机器人系统，本身就是一件非常有价值的事情。举例来说，用在探勘车上的机器人控制电路，只要做一些参数的设定跟调整，就可以直接安置在各种不同工具机的机器手臂上。这对于处理芯片的制作成本跟复杂度，有非常大的好处。

并且，透过下面的说明，设计者可以理解到本发明针对机器人设计会遇到的技术问题所提出的技术方案，不管在计算效率或是稳定性，都有着比现有设计框架更好的优点。

本发明以现代集合论的链理论及张量分析理论为基础，建立了适应现代计算机符号演算的、具有严谨运动链指标系统及运动链坐标系统的链拓扑演算符号系统；构建了基于链拓扑演算符号系统的多体运动学、动力学建模及自主行为控制的理论。

自主行为控制是以多轴系统的运动学及动力学、现代计算机等为基础。实现各种机器人系统的自主行为必须遵从系统自身及外在环境的规律。然而，不同领域的符号系统各不相同。例如，不存在一致的机械制图语言、3D模型语言、专业符号语言，集成这些符号系统存在巨大障碍。由于缺少一致的工程技术规范及技术语言，不同领域的信息交换存在障碍，难以保证各种机器人研制的质量。

这些机器人的设计需要仰赖现代数学、力学、天文学及计算机等学科的融合。换言之，要设计一个可靠的控制系统，需要建立一套适应现代计算机技术特点的多轴系统自主建模及控制的理论。

在本发明中，多轴系统运动学及动力学建模及自主行为控制既实现了系统拓扑、坐标系、结构参量及动力学参量的参数化，又保证了计算及测量的精确性与实时性。从而，实现建模与控制的模块化，提高系统集成的效率与工程应用的可靠性及继承性。

同时，基于轴不变量的运动学及动力学模型实现了对于系统拓扑(连接关系)、参考系、极性、结构及力学参量的参数化，是具有通用性的、高效率的、可直接翻译为计算机代码的机器人运动学与动力学系统，是实现机器人自主控制的基础。由于该系统具有计算机可理解的符号系统，计算机可以进行运动学及动力学分析，并具有自主判别推理结论正确性的机制，可以极大提升机器人对于运动学及动力学问题分析与求解的智能。

无论是复杂的天体系统，还是复杂的航天器系统，它们均具有客观的拓扑结构，该拓扑结构基础是树链。通过树链，将力学、天文学及计算机理论进行整合，不仅可以统一复杂大系统的符号表示及演算，而且可以产生诸多创新性的理论及工程技术成果。

本发明遵从同构的哲学原理，以拓扑的序不变性、度量的张量不变性与对偶性、机械化的建模与演算为原则，建立了适应现代计算机进行符号演算的，具有运动链符号演算系统的，基于轴不变量的多轴系统运动学及动力学方程。一方面，实现了系统拓扑、坐标系、结构参数及动力学参数的参数化。另一方面，是三维(3D)矢量空间的轴不变量的迭代式，保证了计算精度与实时性。同时，具有计算机伪代码的功能，保证工程实现的正确性与可靠性。以之为基础，开展多轴系统自主行为控制研究。由计算机自主地完成多轴系统的运动学与动力学的数值与显式建模，正逆运动学及正逆动力学的解算。本发明可以间接地感知多轴系统与环境的作用状态，间接地感知多轴系统与环境的作用力。本发明可以提高系统绝对定位精度、动态响应，抑制振动，实现系统轻量化。从而，本发明可以提高多轴系统自主行为能力，提供友好、高效、优质的作业与服务。

现代集合论的链(Chain)理论是偏序的集合理论，包含树链系统在内的客观世界的普遍规律。链理论不仅适用于运动链的作用关系，也适用于动作链的作用关系。张量分析理论是关于连续介质粒子及场论研究的数学工具，具有爱因斯坦指标系统。本发明借鉴了现代集合论的链(Chain)理论及张量(Tensor)分析理论，建立了运动链符号演算系统，将传统依赖于自然语言注释与数学语言描述的多体系统运动学及动力学理论转化为以链符号系统为基础的3D操作代数理论体系。用来建构本发明的理论具有以下特点：

首先，本发明提供具有以链序为核心的运动链符号演算系统。一方面，通过具有链指标的符号系统规范，准确地表达运动学与动力学物理属性内涵。另一方面，简洁地表达了运动链的运动学与动力学内在的序作用规律。

其次，本发明具有链指标的3D操作代数系统。操作代数与算子代数的不同在于：操作代数既包含数值计算的矩阵操作，又包含树链拓扑操作。从而，使得多轴系统具有变拓扑结构的适应性、空间运算的紧凑性及操作的可理解性。避免了6D空间算子代数依赖系统拓扑的整体性、空间运算及分解的复杂性及空间算子的抽象性。适用于可变结构的多轴系统运动学及动力学建模。

另外，本发明提供了具有自然轴链的3D操作代数系统。运动的前向迭代及力作用的反向迭代是树链拓扑的基本特征。多体系统本质是多轴系统。运动副是轴与轴的约束副。运动链是轴链。运动副的公共单位轴不变量是自然参考轴，具有自然不变性，故称该单位轴不变量为轴不变量。轴不变量及对应的自然坐标唯一确定对应轴的自然坐标系。基于笛卡尔直角坐标系的运动链本质上是笛卡尔坐标轴链系统，它是自然轴链的3D操作代数的特例。

此外，本发明具有固定轴不变量的结构参数及自然坐标系统。一方面，解决了多轴系统结构参数精确的方法问题。另一方面，也解决了基于固定轴不变量自动确定D-H系与D-H参数的问题。同时，解决了通用6轴可逆解的运动学方程实时建立及实时逆解计算问题。

并且，本发明建构的多轴系统的运动学及动力学方程具有不变量的迭代式。本发明通过轴不变量统一了3D空间的定轴转动、4D空间的罗格里德四元数、欧拉四元数及对偶四元数，解决了6D空间的双矢量姿态、运动旋量及力旋量。本发明建立了基于轴不变量的位移、速度、加速度、偏速度的迭代式。本发明针对有根树链及闭链、无根树链与闭链，建立了基于轴不变量的居─凯恩动力学方程。多轴系统的运动学及动力学方程是关于多轴系统拓扑、坐标系与极性、结构参量及运动参量的参数化的方程，具有正逆解的精确性与实时性。它们具有链指标的伪代码功能，是建立于拓扑公理与度量公理基础上的公理化理论体系，保证了复杂系统建模的正确性与可靠性。

这整套技术可以用来制作成机器人的控制系统，包括一部分或全部在机器人的控制电路里头执行，或是局部或全部在远端的伺服器或外部电脑装置进行运算。此外，这整套技术可以用来制作分析工具，提供给设计者用来分析、验证所设计机器系统的正确性，或进行迭代设计的参考。换言之，这整套技术可以制作成一套完整的工具。此外，透过下面的说明将可以理解，这整套系统所提到的模型具有极好的可扩充性。换言之，多个基于这整套系统建构的机器人模组可以叠加组合在一起，仍然可以用同一个技术方案来加以控制与分析。另外，由于这整套技术方案具有配套的简洁符号系统，而这套符号系统可以设计成对应的程序代码与函数库。这整套系统除了容易建模，即使系统有任何地方出错，仍然可以透过感测器回馈或是工程人员调整设定后，继续保持一定的运作能力。换言之，这个技术方案具有很好的容错能力。

根据本发明的一个实施例提供一种控制方法，用于控制多轴机器装置(multi-axis robot device)。所述多轴机器装置包含杆件集合(link set)与关节集合(jointset)，所述杆件集合中之杆件透过所述关节集合的关节结合，此控制方法包含下列步骤。

将所述关节集合转换成对应的轴集合。关节集合中的一个关节对应成所述轴集合的子轴集合。所述轴集合的轴为平动轴或转动轴。

使用所述轴集合来对应描述所述多轴机器装置。利用所述轴集合的轴对应的轴不变量(axis invariant)来计算所述多轴机器装置的控制参数。其中，对于一个轴的两个杆件，这个轴的轴不变量不会随着对应的关节运动而改变。并且，使用计算出的控制参数来控制所述多轴机器装置。

在一些实施例中，这个方法可以更包含将所述关节集合转换成对应的轴集合对应的数据存在控制电路的存储器中。

在一些实施例中，这个方法可以更包含将所述轴集合来对应所述多轴机器装置的描述对应的数据存在所述控制电路的所述存储器中，并且透过修改所述描述对应的数据，直接调整所述控制方法的更新计算。换言之，跟传统只要设定变更就需要重新建模并加以复杂的除错验算不同，透过这个系统，只需要直接提供调整后的参数即可直接建立新的模型。

这个特征能够大量制造机器设备的零件以降低成本，但同时可以保持组装的多轴机器装置的弹性。任何调整都可以快速反应并且准确的完成新设定的建模与相关的计算。

在一些实施例中，使用所述轴集合来对应描述所述多轴机器装置，并且利用所述轴集合的轴对应的轴不变量(axis invariant)来计算所述多轴机器装置的控制参数，其中对于一个轴的两个杆件，这个轴的轴不变量不会随着对应的关节运动而改变。

这个方法也可以更包含针对所述关节集合的关节的感测器量测数据，结合所述轴集合的轴不变量来计算所述多轴机器装置的至少一正运动学参数，来预测所述多轴机器装置的运动轨迹。

此外，这个控制方法克更包含根据针对所述关节集合与所述杆件对应的所述运动轨迹，对照预设规则，自主操作所述多轴机器装置。

在另外的实施例中，这个控制方法可更包含针对所述多轴机器装置的给定运动轨迹对应的运动方程式求解，找出对应所述给定运动轨迹对应的关节集合的的关节的所述控制参数。以下的内容会具体举例说明相关运动方程式的计算。

此外，这个控制方法也可以包含针对所述关节集合的关节的感测器量测数据，结合所述轴集合的轴不变量来计算所述多轴机器装置的至少一力学参数，来预测所述多轴机器装置各部位的受力情况。

在另一些实施例中，这个控制方法也可包含针对所述多轴机器装置的给定力学参数对应的力学方程式求解，找出对应所述给定力学参数对应的关节集合的的关节的所述控制参数。

另外，所述轴集合可对应成一个有向展开树结构。并且，在对应所述有向展开树结构时，如果有些轴之间有非树连结关系，记录成一个非树连结结合。

这个多轴机器装置整体透过1对1的同构对应方式对应到轴集合，达成同构(isomorphic)的对应关系。换言之，在多轴机器装置进行一定的运动时，这个轴集合建立的模型也可以找到完全对应的运作。反之，在这个轴集合建立的模型中进行运算，也可以对应到所述多轴机器装置的运动。

除了运动学，也可以进一步在运动学的基础上建立力学模型，并且进行对应的计算或求解。在一些实施例中，所述杆件集合进一步扩张成与轴集合一对一对应的虚拟杆件集合，使得每个虚拟杆件集合的杆件与所述轴集合的一个轴刚好一对一对应。

根据本发明的另一实施例，提供一种多轴机器装置。这种多轴机器装置包含多个关节，构成关节集合；多个杆件，构成杆件集合，所述杆件集合中的杆件透过所述多个关节连接；以及控制电路，用来控制所述多个关节的运动，并且所述控制电路将所述关节集合转换成对应的轴集合，关节集合中的一个关节对应成所述轴集合的子轴集合，所述轴集合的轴包含平动轴与转动轴两种类型。

所述控制电路使用所述轴集合来对应描述所述多轴机器装置，并且利用所述轴集合的轴对应的轴不变量(axis invariant)来计算所述多轴机器装置的控制参数，其中对于一个轴的两个杆件，这个轴的轴不变量不会随着对应的关节运动而改变。所述控制电路使用计算出的控制参数来控制所述多轴机器装置的关节。

以下会介绍一套完整的符号以及对应的计算系统，用来对应跟描述各种多轴机器装置。

在一些实施例中，所述控制电路针对所述关节集合的关节的感测器量测数据，结合所述轴集合的轴不变量来计算所述多轴机器装置的至少一正运动学参数，可用来预测所述多轴机器装置的运动轨迹。

在一些实施例中，其中所述控制电路针对所述关节集合与所述杆件对应的所述运动轨迹，对照预设规则，自主操作所述多轴机器装置。

在一些实施例中，所述控制电路针对所述多轴机器装置的给定运动轨迹对应的运动方程式求解，找出对应所述给定运动轨迹对应的关节集合的关节的所述控制参数。

在一些实施例中，其中所述控制电路针对所述关节集合的关节的感测器量测数据，结合所述轴集合的轴不变量来计算所述多轴机器装置的至少一力学参数，来预测所述多轴机器装置各部位的受力情况。

在一些实施例中，所述控制电路针对所述多轴机器装置的给定力学参数对应的力学方程式求解，找出对应所述给定力学参数对应的关节集合的的关节的所述控制参数。

除了作为控制方法或是多轴机器装置本身，本发明也可以基于相同的技术特征，制作成设计系统，用来设计与验证多轴机器装置。这种设计系统可将各种多轴机器系统对应成以轴不变量建构起来的运动以及/或力学模型，来分析与验证各种设计的可行性、正确性与完整性。

所被分析的多轴机器装置的杆件具有杆件集合(link set)与关节集合(jointset)，所述杆件集合中之杆件透过所述关节集合的关节结合。这个此设计系统可包含输入单元，供设计者将所述关节集合转换成对应的轴集合，关节集合中的一个关节对应成所述轴集合的子轴集合，所述轴集合的轴包含平动轴与转动轴两种类型。举例来说，设计者可用XML档案作为参数输入的方式，或是跟电脑辅助设计工具(CAD)结合，在里头透过互动式命令列或图像操作界面来输入新哪管参数。基本的数据输入的设计可以参照目前一半的技术文件，在此不再详细赘述。

这个分析系统还包括处理单元，使用所述轴集合来对应描述所述多轴机器装置，并且利用所述轴集合的轴对应的轴不变量(axis invariant)来计算所述多轴机器装置的控制参数。对于一个轴的两个杆件，这个轴的轴不变量不会随着对应的关节运动而改变。

这个分析系统还包括分析单元，使用计算出的控制参数来分析验证所述多轴机器装置的设计。

这里提到的处理单元与分析单元可以一部分用硬件完成，也可以用软件与硬件结合的方式来制作。实际上也可以将相关的逻辑编写成代码，全部或部分在使用者的电脑上执行或是在远端的伺服器执行。

在实际建立这个系统时，所述输入单元可包含针对所述关节集合的关节设定参数，并结合所述轴集合的轴不变量来计算所述多轴机器装置的至少一正运动学参数，来预测所述多轴机器装置的运动轨迹。

在实际建立这个系统时，所述处理单元更包含利用对于使用所述轴集合建构的力学方程式，计算所述多轴机器装置的力学参数。

根据本发明的另一个实施例，提供一种控制方法，用于控制多轴机器装置(multi-axis robot device)。所述多轴机器装置包含杆件集合(link set)与关节集合(jointset)，所述杆件集合中之杆件透过所述关节集合的关节结合，此控制方法包含下列步骤。

将所述关节集合转换成对应的轴集合，关节集合中的一个关节对应成所述轴集合的子轴集合，所述轴集合的轴包含平动轴与转动轴两种类型。

使用所述轴集合来对应描述所述多轴机器装置，并且利用所述轴集合来建立多种不同的动力学方程，这些动力学方程里头使用所述轴集合对应的轴不变量。

根据本发明的另一实施例提供一种控制方法，用于控制多轴机器装置(multi-axis robot device)。所述多轴机器装置包含杆件集合(link set)与关节集合(jointset)，所述杆件集合中之杆件透过所述关节集合的关节结合，此控制方法包含下列步骤。

将所述关节集合转换成对应的轴集合，关节集合中的一个关节对应成所述轴集合的子轴集合，所述轴集合的轴包含平动轴与转动轴两种类型。

使用所述轴集合来对应描述所述多轴机器装置，并且利用所述轴集合的轴计算对应的轴不变量(axis invariant)。对于一个轴的两个杆件，这个轴的轴不变量不会随着对应的关节运动而改变。

利用轴不变量的不变性建立基于轴不变量的迭代式运动学方程。并且，所述迭代式运动学方程的符号对应到伪代码，可清晰反应所述多轴机器装置运动链的拓扑关系与链序关系。使用计算出的控制参数来控制所述多轴机器装置。

在一些实施例中，在所述迭代式运动学方程里头，使用具有正交归一的四元数来做迭代计算，取代旋转变换矩阵，以增加所计算参数的精确性。相较于传统直接用旋转变换矩阵进行迭代计算，这种做法可以避免错误的累加，而大幅增加整体系统的精确性跟稳定性。

此外，在所述迭代式运动学方程中，可应用轴不变量，将转动表达为转动矢量。

在一些实施例中，所述迭代式运动学方程具有自然零位轴及系统零位轴，轴不变量在3D空间及4D空间下具有优良的操作性能，满足运动链公理及度量公理，并且具有伪代码的功能，物理含义准确。

在一些实施例里头，在所述迭代式运动方程用来计算旋转运动参数的旋转变换矩阵可用轴不变量来表示。

根据本发明提供的另一实施例，提供一种控制方法，用于控制多轴机器装置(multi-axis robot device)。所述多轴机器装置包含杆件集合(link set)与关节集合(joint set)。所述杆件集合中之杆件透过所述关节集合的关节结合。这个控制方法包含下列步骤。

将所述关节集合转换成对应的轴集合，关节集合中的一个关节对应成所述轴集合的子轴集合，所述轴集合的轴包含平动轴与转动轴两种类型。

使用所述轴集合来对应描述所述多轴机器装置，并且利用所述轴集合的轴计算对应的轴不变量(axis invariant)，其中对于一个轴的两个杆件，这个轴的轴不变量不会随着对应的关节运动而改变。

透过轴不变量建构这个多轴机器装置的运动学模型，并且将计算的结果直接跟另一个连接的多轴机器装置进行叠加运算，以计算组合的多轴机器装置的运动参数。

换言之，一个复杂的系统在轴不变量的架构下，可以分割成多个局部来计算。另一方面，这些分开计算的局部机器模组也可以将运算的结果组合起来构成整体的复杂结果。此外，也可以透过下列各种不同的方法来一起计算。

举例来说，这个方法可提供一个硬件或软件的接口(interface)，让所述多轴机器装置与另一个多轴机器装置的参数进行叠加，以直接计所述组合的多轴机器装置的运动参数。

在另外的实施例中，所述多轴机器装置与所述另一个多轴机器装置可以在分离与组合之间切换，并且在组合后透过同一个控制电路加以控制。

另一种操作方式，也包括所述多轴机器装置与所述另一个多轴机器装置在分离与组合之间切换，并且在组合后透过位于两个多轴机器装置的多个控制电路透过协同运算加以控制。

或者，所述的控制方法可更包含侦测所述多轴机器装置是否与另一个多轴机器装置组合，并且自动启动组合的叠加计算。

在一些实施例中，所述控制方法可更包含侦测所述多轴机器装置的实际运动参数跟推算的运动参数之间的误差，并且用所述误差来重新调整所述运动学模型。

这个对于自主运作的机器装置特别重要。因为所有的机器设备都有可能随着制造时候的误差，使用过程的零件老旧或破损而产生各种难以预期的变化。这些变化可以透过针对一些位置的侦测加以测量。这些测量的结果可以跟透过轴不变量建立的模型所做的计算做比较。比较发现的误差可以回馈去调整轴不变量的模型，以达成整个系统具有一定的容错能力，加强整体的系统稳定性。

换言之，当所述多轴机器装置出现局部故障或变形时，透过所述重新调整，利用调整后的所述运动学模型来计算相关的运动学参数。

此外，所述运动学模型里头对应的符号，透过下面的介绍可以发现这些符号都可以对应伪代码，进而编写成对应的软件或是电路。因此，整个运动学模型的运算可透过呼叫对应伪代码的的电路或代码进行运算。

此外，在一些实施例中，控制方法可包含接口，用来跟应用层的代码沟通，以完成自主机器操作的功能。

机器人的用途非常多，其中一种就是用于工厂的制造用机器人的控制。这时候可以利用所述运动学模型搭配外部量测器的量测参数，代入所要组装设备的操作，完成控制所述多轴机器装置达成所述制造用机器人的功能。

这种控制方法也可以利用运动参数代入力学方程，计算所述多轴机器装置各部件所承受的力量。换言之，可以从运动学的参数，推算所承受的力量，进而可以更精密有效地控制所述多轴机器装置。这一点对于机器人能够更有效运作有很大的帮助。目前的机器系统通常需要固定基座或是很重的重量，因为无法计算复杂的力学参数。

相对地，本发明基于轴不变量可以建立起简洁可运算的运动学模型、力学模型，以计算各种控制或分析所需的参数。

因此，透过本发明可以建构出过去难以建立的机器人控制系统的底层。并且，这样的底层具有非常简洁的表示模式，可直接转换成对应的代码或电路，并且加以优化。

图示说明

图1为运动副的结构示范图。

图2为棱柱副的结构示范图。

图3为系统内运动副的标识符、所属类型及简图比较图。

图4为系统外运动副的标识符、所属类型及简图比较图。

图5为基本结构简记符比较图。

图6a为柱面机械臂示范图。

图6b为球面机械臂示范图。

图6c为回转机械臂示范图。

图7为摇臂式六轮机器人的轮系差速机构结构简图。

图8为摇臂式六轮机器人的杆系差速机构结构简图。

图9为闭链类型机器人示范图。

图10为CE3月面巡视器的轴链有向Span树图。

图11为坐标轴与基矢量参考图。

图12为矢量的投影矢量图。

图13为矢量在笛卡尔直角坐标系的投影矢量图。

图14为基矢量与基架关系图。

图15为三维二阶张量的基分量图。

图16为自然坐标系与轴不变量图。

图17为固定轴不变量参考图。

图18为由双矢量确定的转动图。

图19为D-H转动图。

图20为解耦机械手结构图。

图21a、图21b为两组坐标系关系图。

图22为径向投影及自然零位图。

图23为矢量的镜像变换图。

图24为正射镜像图。

图25a为矢量做定轴转动前矢量图。

图25b为图25a做定轴转动后矢量图。

图26为Cayley参数含义图。

图27为二维空间下极点图。

图28为运动螺旋图。

图29为轴不变量精测原理。

图30为固定轴不变量的原点确定图。

图31为偏速度的含义图。

图32为固定轴不变量与位形对偶四元数的关系图。

图33为螺旋径向不变量图。

图34为自然坐标系与D-H系的关系图。

图35为月面巡视器太阳翼坐标系示意图。

图36为全向天线与右翼、全向天线与太阳翼干涉图。

图37为月面巡视器2DOF桅杆示意图。

图38为CE3巡视器机械臂D-H系图。

图39为轴不变量的导出不变量图。

图40为高精度通用机械臂(左6R，中7R，右8R)。

图41为类人手臂(左7R右8R)。

图42为人的手臂结构。

图43机器人柔性加工中心(左6R3F，右6R4F)。

图44为平面2R机械臂示意图。

图45为理想的弹簧质量摆。

图46为多轴系统的闭子树。

图47为通用3R机械臂示意图。

图48为内摩擦力及粘滞力示意图。

图49为三轮移动系统图。

图50为CE3月面巡视器移动系统。

图51为基于线性化补偿器的多轴系统跟踪控制图。

图52为基于逆模补偿器的多轴系统力位控制图。

图53为模糊变结构控制框图。

图54为模糊变结构控制控制律图。

具体实施方式

由于这个发明概念包括一连串相互关联的内容，为了便于设计者理解本发明的内容，并能够基于这样的理解而实施本发明的内容，以下将分章节搭配图示针对本发明加以说明。设计者可以从其他部分的说明找到对应的实施细节，进而利用技术领域中熟悉的电子电路或程序编程，将本发明落实成各种不同的技术方案。

第一部分。运动链符号演算系统Equation Chapter 1 Section 1

机器人运动系统是机器人系统的重要组成部分，是由机器人关节、机器人内传感器、外传感器及控制软件模块构成的有机整体。

零件(Part)：组成机电系统不可拆分的单个制件；零件包含：凸轮、螺栓、板金等。

构件(Component)：组成机电系统的、彼此间无相对运动的基本单元；机械构件包含：连杆、机架等。

连杆(Link)：常常由单独加工的连杆体、连杆头、轴瓦、轴套、螺栓、螺母、开口销等零件组成的一个构件。在机器人领域，通常将构件统称为异型杆件，简称为杆件。无论构件的外形如何，把机器人本体中起主要支撑作用的构件均称为杆件。机架通常由桁架、板金及紧固件组成。

部件(Modular)：机械或电气装配过程的独立功能模块，具有独立机械、电器等装配接口。机械部件包含：减速器、联轴器、制动器等；电气部件包含：电机、轴编码器等。

机构(Linkage)：由一组机械零件、机械构件及运动副组成完成确定的机械运动装置；包含：车轮驱动机构、车轮方向机构、旋翼机构、扑翼机构、回转机构、单轴/双轴云台机构。机构的主要功能在于：形成和释放机器人部件的连接或紧固状态；使机器人部件展开到所需位置与姿态；通过机构间的相对运动产生的内力、机构与环境的相对运动产生的外力，共同产生对机器人运动状态的改变。

总成(Assembly)：机械或电气部件构成的不依赖其它机械部件的独立功能模块，具有独立机械、电器等装配接口。总成包含：一体化的机器人关节、方向机、轮系等。

结构是支撑科学仪器和其他分系统的骨架，机构是机器人产生动作的部件，机构和结构都属于机械系统，在设计上存在着相同或相似之处。结构主要功能在于：为机器人携带的仪器设备和其它分系统提供安装空间、安装位置和安装方式；为仪器设备提供有效的电磁防护、粉尘防护、力学保护；为特定仪器设备提供所需的刚性条件，保证高增益天线、光学部件和传感器所需的位姿精度；为特定仪器设备或其他分系统提供所需的物理性能；例如：热辐射或绝热性能、导电或绝缘性能。在机器人分析时，机架也视为杆件。

机器人运动行为：机器人通过综合电子系统对自身及环境状态的感知，协调执行序列的动作，实现自身及环境状态变迁的过程(Procedure)；包含：蓄能行为、越障行为、抓取行为等。机器人运动技能是指机器人拥有的运动行为能力，是未受激励的运动行为。

机器人运动学(Kinematics)研究机器人自身及环境运动状态表征及状态间相互作用的过程。机器人动力学(Dynamics)研究机器人自身及环境作用力与机器人运动状态的作用过程。

机器人关节(Joints)是机器人重要组成部分，决定着机器人运动学与动力学行为能力。

自然环境即自然的空间是三维(3D)的，这是一个客观的量或不变量。相应地，空间中任一点有3个独立平动自由度(DOF)。任意三个独立的点固结成为一个刚体，具有三个独立方向，故刚体姿态具有3个独立转动自由度；刚体姿态是其任三个独立点的导出状态。三个独立的平动自由度及三个转动自由度对应三个独立平动轴(Translational axis)及三个独立的转动轴(Rotational axis)。独立的平动轴或转动轴是指任两个轴不共轴线即不共轴。

机器人关节(Joint)通常由由高功率密度力矩电机、减速器、高精度绝对编码器、抱闸及电机驱动器组成。有的力控制关节也装配力传感器。

机器人关节区别于普通机电系统中的关节，高性能的机器人关节具有以下基本特点：关节质量轻、结构紧凑、功率密度大，关节输出力矩要足够大；减速器及编码器精度高，通常优于3角分或更高，通常编码器精度要优于减速器背隙(Backlash)精度4至6倍；电机及减速器的效率高，热量少，高可靠性，通常需要可靠8000hrs或更高。电机驱动器具有电流、速度及位置三环控制，通常采用EtherCAT通信；采用中空轴结构设计，提高管线可靠性；编码器通过减速器的馈轴在电机侧安装，提高关节位置检测精度；电机转子、抱闸及减速器入轴(Shaft)一体化设计；减速器出轴与绝对编码器转子一体化设计；关节底座与机器人杆件一体化设计；从而，提高关节可靠性，降低关节质量。

关节通过减速器法兰与父杆件固结，通过输出轴法兰与子杆件l固结；通过关节，实现相邻杆件的相对运动控制。

霍尔效应：当载流导体的电流I受与之正交的定向磁场B作用时，在该磁场方向产生霍尔电势V_H。霍尔电势V_H(V)与电流I(Amp)及电流正交的磁场强度B(Gauss)成正比；

V_H∝I·B。>

基于霍尔效应的敏感器具有以下特点：是真正的固态器件，可以满足300亿次的操作，具有高可靠及长寿命的特点；具有10万次/秒的操作速度，满足高动态响应需求；可满足―40至+150℃宽温需求；霍尔敏感器常用于无刷电机的电子换向器件；当极化的电机转子通过霍尔敏感器时，产生交变的转子位置信号，用于控制电机功率模块的导通状态。

无刷直流(Blushless DC/BLDC)电机由定子、转子及电子换向器组成；驱动器由换向控制逻辑单元、功率模块，以及未画出的电机控制模块及通信模块等组成。

电机转子由永磁材料“钕-铁-硼”或“钐钴”的磁钢构成；在径向上形成极性交替的磁场。机器人电机需要较高的磁场强度，才能保证电机具有较轻的质量。

电子换向器检测转子与定子的相对位置，通常由三个霍尔传感器构成。位置信号输入给换向控制逻辑单元，产生控制功率模块的时序；从而，建立与旋转电磁磁场(Electromagnetic Field/EMF)，拖动转子运行，产生功率输出。

用电子开关器件代替传统的接触式换向器和电刷，无换向火花降低了电磁干扰、提高了可靠性，降低了机械噪声。

交流同步电机转子极对数P给定时，电机驱动器通过控制三相电源频率f(Hz)，可以调节电机转速N(RPM)，

N＝60·f/P。 (1.2)

显然，电源频率一定量，转子极数越多，电机转速越低；通过增加极对数，提高电机输出力矩；极对数多的电机，呈扁平的结构，称为力矩电机。低速的力矩电机的可靠性通常更高，易与减速器匹配；适应提高极对数(3至6对)，也可降低电机及减速器的总质量，提高力控机器人的动态性能。极对数过高时，电机的质量将偏大，在机器人工程上难以应用。

给定电流参考时，直流无刷电机通过控制三相电压的占空比，控制电机电流；通过单位时间内检测的过零次数计算电机转动的速度，由PID速度驱动器产生期望的电流；经电流环实现速度控制。同样，通过速度积分可以得到位置，由位置环驱动器产生期望的速度，再由速度环及电流环实现位置及力的控制。

在通信上，通常遵从串口、CAN/CANOpen、EtherCAT/COE协议。对于力控电机由于要实现多轴协调控制，需要根据机器人动力学过程实时地控制关节电机的电流；需要保证电流环的控制速度；通常需要EtherCAT总线(100Mbps)，通常CAN总线(1Mbps)难以满足力控制机器人应用需求。具有EtherCAT总线的驱动器通常开放电流环，而CAN总线的驱动器通常不会开放电流环，主要因为CAN总线速率不够，易造成机电事故。

行星轮减速器由太阳轮、行星轮及行星架、外齿圈组成。因为齿圈固定，电机驱动输入端的太阳轮，行星轮及行星架绕太阳轮产生公转。

由于该减速器依赖齿轮的啮合传动，行星轮公转速度难以降低；减速比通常为3～10。传动效率达97～98％。精密行星轮减速器背隙可控制至1Rad Min。行星轮减速器由于结构上具有对称性，使得行星轮受力均匀。若需求大减速比，可以串接为多级行星轮减速器，通常至多4级；在结构上通常呈圆柱形，空间利用率不高，效率大大衰减。同时，由于行星轮减速器啮合的齿数少，导致负载能力差。在机器人工程中，行星轮通常用于减速器第1级。

摆线针轮减速器(Cycloid reducer)由摆线齿轮、针齿及针齿套、转臂等构成。曲柄转一周，摆线齿轮走一齿；从而，实现具有大减速比的差动齿轮减速。由于硬齿面且多齿啮合，该减速器输出力矩较大，过载能力强，抗冲击。摆线针轮减速器同时具有传动效率高，体积小，重量轻，故障少，寿命长，运转平稳可靠、噪音小，拆装方便，容易维修，结构简单及力比特性好的优点。精密摆线针轮减速器精度通常可达1Rad Min或更高。

RV(Rotate Vector)传动是二级封闭行星轮系，第一级渐开线圆柱齿轮行星减速机构和第二级摆线针轮减速机构两部分组成。该减速器具有行星减速器及摆线针轮减速器的双重优点。目前，RV减速器是机器人减速器的主流减速器。

谐波减速器(Harmonic reducer)由刚轮、柔轮及波发生器组成。波发生器外部是椭圆形的椭轮；椭轮外齿数目比刚轮内齿数目少2个，即每半周少1个。输入轴通过轴承与波生器连接。当输入轴转动时，椭轮将力传递给柔轮，柔轮外齿与刚轮通过内齿啮合；输入轴转动半周，柔轮外齿相对刚轮内齿相对移动一个齿，实现齿轮差速传动。

单级谐波减速器的传动比达60～300或更大。齿轮齿副总数中有25～30％同时处于啮合，运动精度可达10～60Rad Sec。由于使用的材料要少50％，其体积及重量至少减少1/3，具有高精度、高承载力等优点。但由于柔轮外齿与刚轮内齿采用弹性啮合且需要有一定的预应力，故谐波减速器不抗冲击，力比特性差，可靠性也不及RV减速器。由于谐波减速器精度最高，在机器人关节中得到了广泛使用。

旋转变压器简称旋变(Resolver)，通常由圆柱形的定子与转子组成；是一种输出电压与转子转角保持一定函数关系的感应式微电机。它是一种将角位置转换为电信号的传感器，也是能进行坐标换算和函数运算的解算元件。

XF称作旋变发送机，XB称作旋变变压器。旋变发送机发送一个与机械转角有关的、作一定函数关系变化的电气信号；旋变变压器接受这个信号、并产生和输出一个与双方机械转角之差有关的电气信号。伺服放大器接受旋变变压器的输出信号，作为伺服电动机的控制信号。经放大，驱动伺服电动机旋转，并带动接受方旋转变压器转轴l及其它相连的机构，直至达到和发送机方一致的角位置旋变发送机的初级，一般在转子上设有正交的两相绕组，其中一相作为励磁绕组，输入单相交流电压；另一相短接，以抵消交轴磁通，改善精度。次级也是正交的两相绕组。旋变变压器的初级一般在定子上，由正交的两相绕组组成；次级为单项绕组，没有正交绕组。

作为旋变发送机它的励磁绕组是由单相电压供电，电压可以写为以下形式：

其中：—励磁电压的幅值，ω—励磁电压的角频率。励磁绕组的励磁电流产生的交变磁通，在次级输出绕组中感生出电动势。当转子转动时，由于励磁绕组和次级输出绕组的相对位置发生变化，因而次级输出绕组感生的电动势也发生变化。又由于次级输出的两相绕组在空间成正交的90°电角度，因而两相输出电压如下式：

其中：u_x—正弦相的输出电压，u_y—余弦相的输出电压，—次级输出电压的幅值；α—励磁方和次级输出方电压之间的相位角，—发送机转子的转角。

可以看出，励磁方和输出方的电压是同频率的，但存在着相位差。正弦相和余弦相在电的时间相位上是同相的，但幅值彼此随转角分别作正弦和余弦函数变化。通过一定的解算芯片和算法，就能将电机的转子角位置和转速计算出来。

旋变精度适中，通常可达3Rad Min，抗震性好，价格较低。但由于输出的是模拟信号，需要特定的旋变接口板。

光学绝对编码器由LED光源、棱镜、码盘(光栅盘)及光敏感器组成。光源经棱镜形成平行于光轴的平行光，投射至码盘。码盘是在不透明的基底上按格雷码制成透明的栅格；同时，也包含二进制的增量栅格。由于码盘与电动机同轴，电动机旋转时，码盘与电动机同速旋转，经发光二极管等电子元件组成的检测装置检测输出若干脉冲信号。光线通过码盘由光敏感器转换成电信号。通过光敏感器读取格雷码，并转换为绝对角度；连续读取二进制的增量栅格，可计算码盘转动的速度。

编码器码盘的材料有玻璃、金属、塑料；玻璃码盘是在玻璃上沉积很薄的刻线，其热稳定性好，精度高，易碎，成本高；金属码盘刻有光路通断的栅格，不易碎，但由于金属有一定的厚度，精度就有限制，易变形，其热稳定性就要比玻璃的差一个数量级；塑料码盘是经济型的，其成本低，不易碎及变形，但精度、热稳定性及寿命均要差一些。因此，光学绝对编码器的精度取决于码盘材料及光刻的精度。

高精度的光学绝对编码器精度可达1Rad Sec，比其它类型的绝对编码器精度要高很多。在电接口上，通常遵从SSI、EnDat、BiSS等协议。

无论是电机、减速器、绝对编码器，还是抱闸，共轴安装是一体化关节的基本保证；否则，会导致结构的破坏。一组传感器及执行器具有共轴的特性称为共轴性(Coaxality)；即共轴安装的两根轴等价为一根轴。

运动副(Kinematic pair)是机械系统的关节在运动学上的抽象。机器人运动分析是运动系统设计及控制的基础。从运动分析与综合的角度，将机器人运动系统视为由杆件与运动副组成的运动链(Kinematic chain/KC)。杆件代表与该杆件固结的空间。

请参照图1。图1为运动副的结构示范图。所述运动副是由第一杆件l 101与第二杆件102组成，形成具有相对运动的简单机构；這種简单机构使所述第一杆件l 101与所述第二杆件102具有确定的运动，是所述第一杆件l 101与所述第二杆件102间既直接接触又有相对运动的联接。运动副是既包含两杆件的相对运动，又包含两杆件相对运动的约束；称自由运动的维度为自由度(DOF)，约束的维度为约束度(Degree of constraint/DOC)。

将运动副根向的构件称为定子；将运动副叶方向的构件称为动子。定子与动子是相对的。

记组成任一个运动副k的定子及动子分别为及l，记该运动副为表示连接杆件及l的运动副类或运动副簇。因运动副表示定子与动子l的连接，故它表示的是双向连接关系。将由至l且由l至的有序连接，称为全序的连接；将由至l或由l至的有序连接，称为偏序的连接；偏序及全序反映连接是否有存在方向。

请再度参照图1。图1所示的转动副在其运动轴(Motion axle)上有一个转动自由度，存在由三个平动约束轴(Constraint axes)及两个转动约束轴构成的约束度。

除了图1给出的一种转动副示范图，所述转动副还可以有多种可能的变化，包含棱柱副、螺旋副、圆柱副、球副、接触副、球销副等多种可能的结构。

请参照图2。图2为棱柱副的结构示范图。所述棱柱副由第一杆件l 201与第二杆件202组成，所述棱柱副在其运动轴上有一个平动自由度，存在两个平动约束轴及三个转动约束轴。

螺旋副在其运动轴上存在一个转动自由度，该轴转动时产生轴向位移。存在三个独立的平动约束轴及两个转动约束轴。圆柱副在其运动轴上具有一个平动自由度及一个转动自由度；存在两个平动约束轴及两个转动约束轴。球副存在三个转动轴，即具有三个转动自由度；具有三个平动约束轴。接触副有且仅有一个理想的接触点，仅存在三个轴向转动及两个轴向平动；存在一个轴向的单边平动约束。单边约束意即轴的一个方向受约束；对应于默认的双边约束，意即轴的两个方向均受约束。球销副存在两个独立的转动轴，即具有两个转动自由度；存在三个平动约束轴及一个转动约束轴。

根据运动副所引入的约束度分类：把有且有一个约束度的运动副称为I级副；把有且有两个约束度的运动副称为Ⅱ级副，依此类推。

运动副的两构件直接接触构成接触点、接触线或接触面。构件与构件之间为面接触的运动副称为低副，其接触部分的压强较低；构件与构件之间为点、线接触的运动副称为高副，其接触部分的压强较高。

请参照图3。图3为系统内运动副的标识符、所属类型及简图比较图。常见的运动副类型有球面副301、球销副302、圆柱副303、螺旋副304、棱柱副305与转动副306。

除图3给出的机器人系统内的运动副，图4补充了三个系统外运动副。请参照图4。其中，轮地(wheel-terrain)接触运动副O 401是地面/无限小平面与轮接触点位置约束的运动副。对于自然环境下轮式机器人而言，轮地间不同接触位置对应不同的接触副，因为轮地接触点位置及接触面法向不同。固结副描述底座固定安装的机器人与环境间的关系，该大地符亦可表示空间机器人安装的飞行器本体。系统外运动副对于移动机器人运动分析具有重要的作用。

请参照图5。图5为基本结构简记符比较图。图5中，增加了惯性中心(Inertialcenter)符I，因为质心是空间机器人动力学建模的基本物理属性，离开杆件质心，谈杆件的动能、动量等物理量毫无意义。因此，杆件质心是机器人机械简图的基本要素。大地或惯性空间(Inertial Space)标识符记为i。由后续章节可知，惯性中心I与惯性空间i构成自然的回路或闭链。

从运动学及动力学分析的角度，将运动副划分为两大类：

R/P副，即转动副或棱柱副；它们是构成其它复合运动的基本运动副，任何复合副都可以用一定数量的R/P来等价，且它们的运动轴是相互独立的。有的R/P副能够输出动力，是执行器的输出副；例如：旋转电机及减速器的输出轴等价于R；直线电机的输出轴等价于P副；虚副V等价于三轴的R副及三轴的P副。棱柱副或转动副的约束轴约束了线位置及角位置，称其为完整约束；由初始时刻至任一时刻的位形是确定的，可积的。因简单运动副是由两个共轴线的轴构成，且任一轴与一个杆件固结；故在运动关系上轴与杆件等价，即轴与杆件可混用。

O副，即接触副；理想的轮地接触可视为接触副。由于该约束副仅约束了关节速度而不是位形；由于存在相对滑动，由初始时刻至任一时刻的位形是不确定的，不可积的；该约束副为非完整约束副。

由上可知，基本的运动副R及P、螺旋副H、接触副O可以视为圆柱副C的特例；同时，运动副R及P可以组合为其它复合运动副。故将圆柱副用于运动副的通用模型。简单运动副即R或P的定子与转子具有共轴性，分别与不同杆件固结，杆件间的运动本质上运动轴间的相对运动。因此，在运动学上，圆柱副C是运动副的基元，具有完备性。。

称至少两个简单运动副按一定次序连接构成的运动机构(Kinematic Linkage)为运动链(KC)；运动链是运动机构在运动学层次的抽象。简单运动副是构成运动链的基本单位，称之为链节(Chaining Pair)。表达运动链的拓扑关系及度量关系是运动链分析的基本前提。

约束副是两个关节间所受的约束在运动学上的抽象。常以表示约束副。

在简单运动副内部，除了运动轴(Motion axis)的自由度之外，也存在轴向的约束。运动副的运动轴数即自由度，约束轴数即约束度是运动副自身的属性。运动副内部的约束轴仅约束轴方向的运动，不构成约束副，即运动副与约束副、约束轴是不同的概念。

与运动副的理论抽象一样，需要确定约束副的基元，以简化运动学分析。因为通过运动副可以描述所有的空间运动，因此可以通过约束不同运动轴的相对位置或速度来描述所有空间的约束；所以，不同运动副轴间的相对位置或速度约束是约束副的基元，具有运动学上的完备性。在理论上，将约束副区分为完整约束及非完整约束，双边约束及单边约束。

位置约束是指两约束轴上的点的相对线位置或两轴相对转动角度受控的约束。速度约束是指两约束轴上点的相对线速度或两轴相对转动的角速度受控的约束。完整约束是指相对速度可积的约束，位置约束必为完整约束；若相对速度不可积，则为非完整约束。双边约束是指运动轴两个方向都受控的约束；而单边约束是指运动轴单个方向受控的约束。

通过以上运动副及约束副分析可知：由运动轴、约束轴及测量轴是运动系统的基元。关节及测量单元的自然轴是空间运动的自然参考轴，又称为空间参考轴(Spacialaxis)，具有以下基本属性：

(1)共轴性及方向性是执行器及传感器参考的基本属性：一方面，控制量及检测量是对特定轴向(Axial direction)而言的；另一方面，关节的一个自由度对应于一个独立的运动轴。因此，在连接及运动关系上，轴是关节的基本单位，也是构成多体系统的基本单位。更重要的是：控制量及检测量是共轴线的两轴间相对的运动量(Kinematic Quantities)；要么是轴向的平动(Axial Translation)，要么是轴向的转动(Axial Rotation)。否则，在物理上是不可控的，也是不可测的；可控及可测的运动量在结构上必须存在相应的轴向连接。

极性是执行器及传感器的另一个基本属性：关节角位置或线位置是具有正负的标量；通常，遵从右手法则(Right-hand Rule)时，为正；遵从左手法则时，为负。

(2)零参考是执行器及传感器参考的又一基本属性：传感器与减速器的共轴连接具有机械零位；相应地，电机驱动器通常具有电子零位。空间参考关系是运动副及杆件运动的基础。零位本质上是转动的径向参考轴。

(3)轴位置空间的三个维度是轴实体的客观属性：对于由可数个运动副连接而成的机械系统，忽视全部与距离及角度表示的度量，仅从拓扑关系上看，就是一个轴与轴的连接的系统。从度量的角度上看，一个连续的三维空间与一个抽象的三维轴空间一一映射；轴的位置及方向自然由相应的度量系统表征。因此，无论是线型的轴，还是弯曲的轴，在拓扑上，只要是连续的一个体，就表示是一个轴。

运动副的共轴性、极性与零位表明：(1)杆件与轴具有一一对应性；(2)轴间的属性量及杆件间的属性量具有偏序性；(3)轴间的属性量具有共轴安装的直接可检测性；(4)因杆件的结构参量在工程上直接可以测量，故杆件间的属性量本质上也具有直接可检测性。(5)运动学及动力学需要适应多轴系统的拓扑结构、结构参数、参考系及极性的参数化需求，才能保证理论系统及软件系统的易用性与可靠性。

机器人拓扑系统(Topological System)是指忽略杆件的尺寸、仅考虑运动副及杆件相互连接构成的系统；同一类拓扑系统具有相同类型的连接关系；当杆件尺寸连续变化时，拓扑关系或结构保持不变。按拓扑关系将机器人运动链分为：串链、树链及闭链三种类型。

串链类型：请同时参照图6a、图6b、图6c。图6a为柱面机械臂示范图。图6b为球面机械臂示范图。图6c为回转机械臂示范图。

图6a中，所述柱面机械臂包含机底座601、回转6R机械臂本体与拾取器(Gripper)。所述回转6R机械臂本体主要由第一杆件及转动副⁰R₁>1R₂>2R₃>3R₄>4R₅>5R₆>

图6b中，所述球面机械臂包含底座6011、回转6R机械臂本体与拾取器。所述回转6R机械臂本体主要由第一杆件及转动副⁰R₁>1R₂>2R₃>3R₄>4R₅>5R₆>

图6c中，所述回转机械臂包含底座60111、回转6R机械臂本体与拾取器。所述回转6R机械臂本体主要由第一杆件及转动副⁰R₁>1R₂>2R₃>3R₄>4R₅>5R₆>

所述柱面机械臂、所述球面机械臂除了转动副外还有棱柱副。其中，所述拾取器三个转动副轴共交于一点，称之为腕心。将拾取器拾取物体时期望位置称为拾取点(PickPoints)；拾取点总是位于以腕心为球心的球面上。故控制这样的机械臂拾取物体时，可分为三个步骤：首先，由拾取器相对世界的期望姿态计算三轴姿态；然后，根据期望姿态及球面半径，计算期望的腕心位置；最后，根据期望的腕心(Wrist Center)位置，确定三轴角度。将这种位置控制与姿态控制独立进行的机械臂称为解耦机械臂。对于柱面机械臂，腕心位于机械臂工作空间的柱面上；对于球面机械臂，腕心位于机械臂工作空间的球面上。

机械臂杆件通过运动的连接确定了一个简单运动链，称之为单链或链。该链是有序的杆件集合。记为[0,1,2,3,4,5,6]，其中7个序号表示7个杆件，两相邻杆件通过运动副相连接。

树链类型：CE3巡视器(Rover)移动系统是六轮独立驱动的摇臂系统，由摇臂机构、四个舵机、六个驱动轮组成。

摇臂机构由右主臂/摇臂(Rocker)、左主臂/摇臂、右副臂(Bogie)、左副臂、差速机构组成。右主臂、左主臂分别与差速机构右轴及左轴固结。左副臂及右副臂分别通过转动副与左主臂及右主臂连接。左前及右前方向机构分别通过转动副与左主臂及右主臂连接；左后及右后方向机构分别与左副臂及右副臂通过转动副连接。通过主臂与副臂的摇动、差速机构的差速作用，保证车体悬挂于左、右主臂的角平分面上。故将该结构称为摇臂悬架，左右对称的部分分别称为左悬架与右悬架。摇臂机构是一个树形结构，称之为树形运动链或树链(Tree-Typed KC)。

请参照图7。图7為摇臂式六轮机器人的轮系差速机构结构简图。右主臂701、左主臂702分别与差速机构703的差速机构右轴704及差速机构左轴705固结。所述差速机构右轴704带动右斜齿轮706转动，所述差速机构左轴705带动左斜齿轮707转动，驱动与壳体710固结的前斜齿轮708、后斜齿轮709绕主臂轴实现差速转动；保证车箱底板711位于右主臂701与左主臂702的角平分线上。

请参照图8。图8为摇臂式六轮机器人的杆系差速机构结构简图。左主臂801与左差动臂803固结、右主臂802与右差动臂804固结。左差动臂803通过两个球副及左差动连杆与平均连杆连接。同样，右差动臂通过两个球副及右差动连杆与平均连杆连接。左主臂801及右主臂802分别通过转动副与横轴805连接。因此，当左主臂801与右主臂802运动时，横轴805与平均连杆在同一平面内，且该平面是左主臂801及右主臂802的角平分面。左主臂801与右主臂802分别通过转动副与左副臂806及右副臂807连接。

图7与图8所示的移动系统是一个具有回路的闭链；差速器等价为具有约束的两个转动副，该约束使这两个转动副构成了回路。但因该系统的主要拓扑结构是树形结构，故习惯上称其为树链结构。

闭链类型：请参照图9。图9为闭链类型机器人示范图。该机器人由6个移动副901、12个球副902、14个杆件903组成。通过运动副连接的杆件903构成回路。它是一个带回路的结构，称为并联运动链(Parallel KC)或闭链(Closed-loop KC)。闭链总可分解为树链。

由上可知，单链是树链的特殊情形，闭链可以分解为若干个树链；分析树链机器人的运动学及动力学具有非常重要的意义。树链拓扑是树链机器人最基本的结构约束，在机器人运动学分析、动力学分析及情景演算中，机器人拓扑关系是最基本约束条件。

为方便机器人运动学及动力学分析，在绘制结构简图时需要约定组成机器人运动系统的标识符及缩略标识符。

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/左后轮地接触点；

根据机器人结构关系，应用图3至系统外运动副对于移动机器人运动分析具有重要的作用。

给定一个由运动副连接的具有闭链的结构简图，可以选定回路中任一个运动副，将组成该运动副的定子与动子分割开来；从而，获得一个无回路的树型结构，称之为Span树，亦称为生成树。称被分割的支路为非树弧(Non-tree arcs)或弦。Span树是对应图的支撑集。

机器人运动学与动力学依赖于机器人拓扑，拓扑关系即连接关系作用：反映杆件间的连接关系，反映杆件间运动量的参考关系，也反映杆件间运动量的作用关系。偏序(Partial ordering)即单向连接关系是全序(Full ordering)即双向连接的基础。

公理1.1：树型轴链(Tree-typed Axis-chain/AC)以偏序为核心，描述树型运动系统的三大基本事实：

事实1：当根向部件运动时，必然带动叶向的部件运动；即根部件的位置、速度及加速度由根向叶传递并叠加；表明运动具有由根至叶的偏序关系。简称为运动的前向(正向)链迭代(Motion Iteration of Forward Chaining)。

事实2：当叶向部件受外部作用力时，必然引起根向的杆件的力等效作用；即叶部件的力由叶向根传递并叠加；即表明力作用具有由叶至根的偏序关系。简称为力的反向(逆向)链迭代(Force Iteration of Backward Chaining)。

事实3：为了表征机器人运动学及动力学的行为过程，需要建立运动学及动力学度量的参考关系；该参考关系既包含作用过程的次序关系，又包含测量量与控制量的坐标参考关系。有向Span树为机器人运动学及动力学过程提供了拓扑次序参考基准。简称为拓扑与度量的链序参考(Chaining Reference of Topologies and Metrics)。

上述树型轴链(Tree-typed Axis-chain/AC)公理的三大基本事实是多轴系统运动学及动力学理论的基础，是后续运动链拓扑公理及度量公理的基石。因此，需要建立有向Span树，以描述树链运动的拓扑关系。

上述的偏序性是多轴系统(Multi-axis System/MAS)的基本特征，将贯穿本书的运动学及动力学的各个章节。通过对树中的节点进行编号，将Span树表述为偏序的拓扑系统。给定一个Span树，按如下流程对各节点进行编号：

【1】选取任一节点作为根，根编号为0；

【2】由根至叶选取任一支路l，令l＝1，依次向叶编号至k_l；

【3】若存在未编号的节点，则选取任一剩余的支路l+1，将该支路的根编号为k_l+1，依次向叶编号至k_l+1；否则，结束编号。

至此，任一节点l或杆件Ω_l及轴A_l具有了唯一的编号；依编号将杆件缩略名记为运动轴序列A。运动轴序列简称为轴序列(Axis>

【1】除根之外，任一节点或杆件均具有唯一的父节点或杆件；故连接杆件及杆件Ω_l的运动副与杆件Ω_l一一映射；若所有运动副仅有一个运动轴(复合运动副由数个简单运动副串接等价)，则运动副杆件Ω_l及第l个运动轴A^[l]两两之间一一映射。

【2】由一个根向节点至一个叶向节点的路径是唯一的。

【3】由根向节点至叶节点方向定义为前向或正向，反之为反向或逆向。

【4】由N+1个杆件构成的有向Span树，其N个杆件或运动轴与自然数(NaturalNumber)集(0,1,…,N]一一映射；故一个有向Span树的拓扑关系与一个自然数集的拓扑关系等价。

【5】表示l的父，且有偏序关系即叶向杆件的序总大于其根向杆件的序。

因杆件缩略名是唯一的，任一杆件有唯一的编号，杆件缩略名的次序由其对应编号的次序确定。相应地，杆件的结构参数及运动参数的标识号与该杆件编号一一映射。

有向Span树反映的是机器人拓扑关系。机器人行为不仅与机器人拓扑相关，而且与机器人运动学及动力学过程密切相关；需要应用拓扑空间及矢量空间的数学理论，分析机器人的运动学及动力学过程。因偏序集合是与自然数一一映射的集合，是现代集合论研究的重要内容；因此，首先需要了解现代集合论的数学基础知识；以之为基础，再进一步研究多轴系统运动学及动力学理论及工程问题。

以下通过公理化集合论的有益借鉴，提出运动链基础技术符号，明确多轴系统运动学及动力学研究的思路与目标。首先，简述现代集合论的基础，阐明偏序、链等基本概念；从而，提出运动链基础技术符号。然后，简述矢量空间基本概念；从而，确定建立以动作为核心的、基于代数几何的、拓扑符号参与显式建模的运动链符号演算系统的研究目标。

任一门类的数学都是研究特定数学空间的理论。数学空间包含两个方面：

(1)空间成员的形式化(Formalization)及空间成员关系的形式化。空间是一个集合，习惯上将集合成员统称为点，可能是真实世界空间点，亦可能是抽象的数学空间点；将空间点服从的基本运算关系称为空间结构(Spacial Structures)。比如，矢量空间具有代数乘、矢量和的可加性结构。空间的形式化就是如何正确地表示空间中的点及空间点的基本关系；

(2)空间关系的演算，即如何依据空间点的结构揭示同类空间内在规律与关系。

策梅洛(Zermelo)和弗伦克尔(Fraenkel)等提出了ZF集合论的公理化系统，以之为基础提出了ZFC公理系统。已经证明，公理化的数学理论就是ZFC集合论的公理系统。数学理论是研究一切自然科学的基础，对于机器人运动链符号系统也不例外。现代集合论统一了不同门类的数学。任一门类的数学是一个与其研究对象同构的数学符号系统。

若两个数学空间之间存在一一映射，那么这两个空间被叫做同构空间。同构也是基本的哲学思想。一个符号系统要与被研究系统等价，就需要保证符号系统与被研究系统的同构，从而，符号系统的演算才能与被研究系统的演化过程等价，即符号推理的分析过程才能反映被研究对象的演化规律。同构反映的是规律的客观性。不以同构思想为基础，探究被研究对象及对应的符号系统规律，极易产生符号演算的错误。

经常应用一个熟知的数学空间，研究与该空间同构的另一个空间的运动；同构不仅是数学研究的基础，也是计算机软件、电子技术等实现的基础。

以现代集合论为基础，在同构哲学观点下，建立运动链符号演算系统，将运动链拓扑系统及运动链度量系统统一起来，构建机器人多轴系统建模与控制的统一理论框架。

集合论基础

集合是具有某种属性对象的总和；这些对象称为集合的元素或点。集合成员也可能是集合；但一个集合不可能是其自身的成员；这是系统层次可分的反映。

习惯上，数域(集合)表示为：自然数整数实数复数其中：

任意一个集合I＝{i₀,i₁,…,i_n,…}的成员具有唯一性，即i_k≠i_j，其中

空集：不含任何元素的集合，用符号表示；空集是任何集合的成员。

集合论是研究集合的数学理论，包含集合、元素和成员关系等基本数学概念。在现代数学中，集合论和一阶谓词逻辑共同构成了数学的公理化基础，以未定义的“集合”与“集合成员”等术语来形式化地建构不同门类的数学空间；从而，以现代集合论统一了所有门类的数学理论。任一数学门类均是现代集合论的一个实例化。

现代集合论与运动链符号演算

公理化集合论是建立数学符号系统的基础系统。它由公理化集合论符号系统内部原子、一阶谓词/判断、函子/函数组成。以之为基础，增加应用域的原子及谓词，可以建立相应的应用符号系统。故将公理化集合论符号系统简称为元系统(Meta-system)。

原子符：指被研究系统的基本对象符(Object Symbols)、对象的主属性符(MajorProperty Symbols)及子属性符(Minor Property Symbols)。例如：电阻R₁、电容C₂、能量E、动作A₁等。任一系统总是可分的、总是有结构的。被研究系统的原子符与该系统的研究层次是相对应的。

由原子集合成员构成新的对象符及属性符，称为复合对象符及复合属性符。它们是被研究对象的基本存在，故将原子符、复合对象符、复合属性符统称为原子符。

谓词符：表示被研究系统中明确的判断符号，即要么成立要么不成立；它由谓词关系符及数个占位符构成，形如其中：P是谓词关系符，()是界定符，表示数个占位符。谓词表示一个明确的判断。

函子符：表示被研究系统的函数关系，它是由函数关系符及数个占位符构成，形如其中：f是谓词关系符号，()是界定符，表示数个占位符。函子表示的是一个函数关系；与谓词不同，它不是一个二值判断。

原子符与被研究系统的对象或属性存在一一映射关系；谓词符及函子符与被研究系统的对象或属性的作用关系存在一一映射。谓词判断及函子运算统称为系统的属性操作，简称为操作。由原子符及属性操作符构成的符号演算系统与被研究系统存在一一映射的关系，即符号系统与被研究系统是等价的；通过自然的(Natural)及结构化的(Structural)符号语言(Symbolic Language)描述自然的系统，才能反映被研究对象的运动过程。将谓词判断及函子运算的符号统称为属性操作符，包含连续的度量关系符及连接的拓扑关系符。

集合论常用的基本原子符包含：

【1】表示存在空集；

【2】I表示全集；

【3】[]表示有序的集合、序列或矩阵；例如：[x,y,z]。

【4】{}表示集合标识符；例如：{x,y,z}，它的成员不分次序。

【5】,表示元素/项的分隔符；

【6】()仅当左边存在谓词时表示符号作用域的界定符；例如：P(x,y,z)。

【7】表示属性占位符；例如：

【8】表示任意个数占位符；例如：

集合论常用的基本一阶谓词/关系符号如下：

【1】表示存在；表示唯一存在；例如：

【2】表示任意；例如：

【3】＝表示相等；例如：x＝y。

【4】∈表示包含；例如：a∈b。

【5】∧表示且关系；∨表示或关系；表示反关系；例如：∧x，∨x，

【6】表示能够推得/蕴含；例如：

【7】表示双向推得/蕴含，例如：

【8】表示赋值操作；例如：

【9】∪表示集合合取操作；例如：∪C＝{x|x∈s,s∈C}＝∪{s|s∈C}，A∪B＝{x|x∈A∨x∈B}。

【10】∩表示集合析取操作；例如：A∩B＝{x|x∈A∧x∈B}。

【11】-表示集合减操作；例如：

【12】→表示多对一映射或映射；例如：A→B

【13】表示一一映射；例如：

【14】其左侧除分隔符外无其它符号时表示无名谓词，仅当括号中各项为真时，该无名谓词输出为真。

【15】·表示点乘或点积；×表示叉乘或叉积；*表示复数积；·表示代数积；

因为集合中的成员也是集合，集合及成员可用大写或小写字母表示。

符右侧字母表示变量；谓词表示的是规律，仅当谓词P为真时的输出才能构成其值域的成员。由谓词表达式表示的是判断，两个匹配的谓词表达式是一个肯定的判断。为书写方便，一元及二元谓词有左操作及右操作的表示形式，是的变体，例如：x＝y表示相等操作，表示取反操作。

以元系统为基础，定义新的操作，可以构成新的符号系统。元系统是ZF公理系统的基础。

元系统由原子组成；原子是元系统组成单位，以最少的原子、函子及谓词构成复杂的理论系统，才能保证理论的简洁性。数学元系统为不同门类数学分支提供了统一的语言与逻辑操作。

ZF中唯一的对象是集合，集合中的成员是集合，集合任一组成员也是集合。ZF公理是现代集合论的基石。下面几个公理的集合表示有点晦涩，可以忽略它们，不防碍后续的阅读。

外延公理(Axiom of extensionality)：集合x的任一成员z是集合y的成员，且集合y的任一成员z是集合x的成员。简言之：若两集合成员相等，则两集合相等；即有

正则公理/基础公理(Axiom of regularity/axiom of foundation)：对任一非空集合x存在一个成员y，使x∩y为空集；即有简言之：任一非空集合中存在一个成员与该集合无交集。这样的集合称为良基集。

例如，不允许出现x属于x的情况。显然，不是良基集。

空集存在公理(Axiom of empty set)：即存在一个集合它没有任何元素。

配对公理(Axiom of pairing)：任给两集合x及y，存在另一个集合z＝{x,y}；即有简言之：任何集合与其它集合可组合成新的集合。

并集公理(Axiom of union)：将任一集合C的任一组成员Y合并为一个集合A。简言之：任一集合的成员组是可归并的。

幂集公理(Axiom of power set)：任一集合x的所有子集构成一新的集合y；即简言之：对任一集合成员集合分解，直至所有成员集合为空集，得到的所有集合称为幂集。

如果|x|＝n，则|Power(x)|＝2ⁿ。故任一集合是可划分的。

无穷公理(Axiom of infinity)：存在一个集合x，空集是其元素，且任意元素y∪{y}也是其元素；即有简言之：对无穷集合而言，一定存在包含该集合的集合。

根据皮亚诺公理系统对自然数的描述，存在一个包含所有自然数的集合。

关于分离公理模式、替换公理模式、AC选择公理请参阅文献。对不同集合引入一定的基本运算，形成一个特定的数学门类。

现代集合论意义在于：在元系统层次上，证明了当今不同数学门类的关系，也说明了不同门类的科学是可以通过现代集合论的符号系统来得到统一；从而，真正地实现不同学科的交叉。为多轴系统建模与控制理论的元系统，首先需要建立基本的集合表示方法：

【1】在集合论中，| |表示有限集合的基数，即元素的个数，数乘·表示排列。例如：

但有即有2·3＝3·2。但二者的含义不同。代数乘是矩阵乘的特殊形式。符号重载即依赖于运算对象完成的计算功能不同是非常普遍及必要的。因此，符号运算需要根据属性量进行定义。

【2】无次序的集合即组合总可以通过有序的组合表示，组合是排列的特殊形式，排列是存在的一般形式。李的三个儿子a,b,c表达为Li′s sons＝[a,b,c]，[a,b,c]是有序的集合即向量，它是{a,b,c}的一个实例。因此，在本书后续部分，[]表示有序的集合；{}表示无序的集合。

【3】集合名称表示一类特定的集合；例如，在李的三个儿子Li′s sons＝[a,b,c]中，集合名称Li′s sons是有结构的关系，而[a,b,c]是的一个实例而已。结构化的名称具有清晰的内涵，是属性量表达的基本形式。在定义集合成员后，成员的关系均应表现为操作或作用关系，而函子表示成员间的映射关系；以谓词表征的动作或操作有助于理解符号的物理内涵。

现代集合论是一个严谨的公理化理论体系，而目前的机器人运动学与动力学理论不是一个严谨的公理化体系，存在多种理论分支，需要将已证实的理论统一起来，才能构建包含最少属性符及最少操作的理论系统。

目前，力学理论不是一个具有最少属性符及操作的公理化系统。比如，平动速度用v表示，而平动加速度用a表示，速度v对时间t求导用v′表示；经常将力表示为这些表示存在歧义：比如，既不知道该力的作用点，也不知道力的施力点，不适应计算机处理的需求。这些非结构化的符号代表的属性需要通过注释才能理解，难以适应高自由度的机器人运动学及动力学分析的需求。

通过一阶谓词表征作用关系，不仅具有可读性，而且无歧义；例如，谓词fetch(A,B)表示A取回B；同样，作为多轴系统建模与控制理论的元系统，也将所有的对象视为集合，也需要建立公理化的理论体系。因为多轴系统建模与控制理论是针对机器人工程或机械电子工程的，是具有明确应用背景的学科，其任何属性及属性间的关系是精确的、明晰的；所有的关系具有谓词或函子的结构。比如，定义表示连接杆件及杆件l的运动副，这是谓词的表达形式。

在运动链符号演算系统中，系统属性变量及常量均采用简洁的、物理意义清晰的名称，它们均通过有序排列的集合即矩阵表示；正体的名称表示常量，斜体的名称表示关于时间t的变量。建立以运动链公理为核心的运动链符号演算系统为多轴系统建模与控制理论提供元系统。

由上可知，计算机语言是对现代集合论符号语言的有益补充，符号系统直接转化为计算机伪代码是解决系统工程实现的一个重要方面。

偏序集及皮亚诺的自然数集

由前述的基本事实可知：偏序是机器人系统的基本属性，链理论正是偏序的理论抽象。

链(Chain)是具有偏序的集合。它是现代集合论的重要研究内容。因为现代集合论的链理论是包含运动链的客观世界普遍遵循的基本规律。以之为基础，可以在拓扑层次上来指导运动链的理论分析。

设a为集合，称a∪{a}为a的后继(Successor)，记为a+或S(a)。a的后继即为与a最邻近的且≥a的元素。

设A是集合，若A满足下列条件，称A为归纳集：

(1)自然数集合是所有归纳集的并集。因为：记记记记

归纳可证之。

故有，皮亚诺的自然数公理：零是自然数；每个自然数都有一个后继；零不是任何自然数的后继；不同的自然数有不同的后继。设由自然数组成的某个集合含有零，且每当该集合含有某个自然数时，则同时含有这个数的后继，那么该集合包含有全部自然数。

自然数集是一个0生1、1生2、2生3的如此往复的归纳过程。

自然数集又称为核(Null)，是原子符，表示空；表示空位的内容，表示一个可以占有1个空位的对象；表示占有2个空位的对象；等等。因此，自然数是自然实体位置的一一映射。自然数集具有：传递性，a∈a⁺；正序关系，a＜a⁺；

给定集合X，称集合X至集合X×X的子集R的映射为关系R。如果(a,b)∈R，则记为^aR_b。因此，集合X的关系R是X×X的子集。

若a,b∈X，表示a中至少存在一个元素小于b中的元素。序运算符通常表示为≤、＜、当然，这里的小于的标准可以是不同的，既可以是任何形式的泛数，又可以是其它的约定。

传递性(Transitive)：若^aR_b且^bR_c；则有^aR_c。

自反性(Reflexive)：若则有^aR_a。

反对称性(Anti-symmetric)：若^aR_b且^bR_a；则有a＝b。

对称性(Symmetric)：若^aR_b，则有^bR_a；反之亦然。

偏序集：集合X的序运算≤具有传递性、自反性及反对称性，称(X,≤)为偏序集(Partially Set)。

将序运算≤及其传递性、自反性及反对称性统称为链序关系(Chaining orderrelations)。

链：对于集合A，若k≤i或k＞i；则(A,≤)为线性序集，称之为链。简言之，链就是有序的集合。在本书中，将运动链之外的其它有序集合称为序列(Sequence)。

显然，连续的区间是偏序集合，是具有无限多个成员的链，通常以区间符表示之。即有：{x∈L|k＜x＜i}＝(k,i)；{x∈X|k≤x＜i}＝[k,i)；{x∈X|k＜x≤i}＝(k,i]；{x∈X|k≤x≤i}＝[k,i]。

例如：偏序集合[1,2,3]是一个链。若集合{x,y,z}，其成员满足x＜y且y＜z；则偏序集合[x,y,z]是一个链。显然，矢量及矩阵的成员是有序的排列，在以排列序号作为排序的准则时，矢量及矩阵也是链。链是数学研究的基本对象，是自然存在的基本形式之一。

对于自然数集合若则有i＜i⁺。自然数集合具有传递性；若i＜i⁺，且i⁺＜k，则i＜k。故自然数集合是一个链。

给定集合X，且有一一映射f:X→Y，则有称两集合是同构的。

同构链：对于一个链X及像链Y，函数f:X→Y；若有f(a)≤f(b)，则称链X及像链Y互为同构链；并记为

在现实世界中，链关系是最基本的关系。例如：代数乘“·”具有拓扑关系运算；集合A＝{a₁,a₂}与集合B＝{b₁,b₂,b₃}进行排列得到二维集合A·B＝{a₁·b₁,a₁·b₂,a₁·b₃,a₂·b₁,a₂·b₂,a₂·b₃}；其基数即成员的个数记为|A·B|＝6；即有2·3＝6。本质上表示一个排列a₁·b₁表示a₁与b₁有序的拓扑即连接关系。因为代数乘运算是链的基本运算，所以链关系自然是不同学科的基本关系。

以ZFC公理系统为基础，记记1＝0+，2＝1+等等，即形式化表示了自然数集合的表示。在自然数集中增加一阶谓词：加+、乘·、小于等于≤、小于＜；它们构成了自然数空间的基本结构。在自然数域中，引入一阶谓词：函子/操作-，便发现新的运算结果不一定是自然数集的元素，即在自然数域中减-运算不是自闭的，于是需要拓广(Extend)自然数集，从而引入整数集在整数集中，引入一阶谓词：除/运算，便发现该运算也不是自闭的，于是产生了实数集R。在实数集上引入开方运算，发现了复数集特定的矢量空间运算与复数空间运算具有同构关系。例如：二维笛卡尔矢量空间的转动可以映射为三维复数空间的积运算；三维笛卡尔矢量空间的转动可以映射为四维复数空间的积运算。

数学空间正是空间运算不自闭(Non-closure)而逐步拓广的，数学空间也因此不断延伸，扩大了人类的视野；矢量空间是基本的数学空间。它们本质上要么是偏序的、要么是全序的空间。链的实例包含：动作链(Action chain)、行为链(Behavior chain)、运动链(Kinematic chain/KC)等，它是普遍的存在形式。链本质是有序的拓扑关系；一方面，系统拓扑决定系统运动学及动力学行为过程；另一方面，到目前为止，拓扑符号与系统行为过程建模与分析是分离的，即拓扑符号未参与多体运动学及动力学建模过程，难以揭示复杂系统的基本规律。

但是，现有的多体运动学及动力学缺少拓扑符号系统。链节间的次序具有不变性。在系统建模时，不仅需要表达系统的拓扑及链序，而且需要保证运动学及动力学过程的链序不变性(Invariants of Chaining Order)，只有这样才能保证符号系统的客观性。

向量空间又称为线性空间，是线性代数的中心内容和基本概念之一。设是一个数域(Field)及一个向量空间和两个运算：

【1】向量加法：记作v+w，

【2】标量乘法：记作a·v，

其中：·表示代数乘或矩阵乘；+表示代数加或矩阵加。

矢量空间是具有正交基架的线性空间，且满足内积、叉积运算。矢量空间分析是机器人系统的基础，因为在矢量空间变换下，具有距离及角度的不变性。离开距离及角度的不变性，就无从谈论机器人运动学与动力学。空间任一点的位置、速度、加速度及作用力均是矢量。

然而，关节运动是关节的动子与定子的相对运动；称该相对运动的关节坐标即关节线位置(Linear Position)及角位置(Angular Position)为自然坐标(NaturalCoordinates)，是以关节坐标轴(Joint Axis)为参考的标量；由于存在转动，关节坐标与位移(Displacement)、速度(Velocity)、加速度(Acceleration)具有非线性关系。

称由两个三维矢量合并的一个六维矢量为螺旋；故螺旋又称为双矢量(DoubleVectors)。将由三维转动速度矢量与平动速度矢量构成的双矢量称为运动旋量；将由三维力矩矢量力矢量构成的双矢量称为力旋量。

记r及p分别为三维的转动速度矢量及平动速度矢量，记r及p为两个实数；则旋量有及双数s＝r+p·ε。

引入双数单位(Dual number unit)符号ε，且有

0·ε＝0,1·ε＝ε,ε²＝ε*ε＝0；>

存在以下点积与叉积运算：

双变量及s＝r+p·ε的函数定义为

式中ε²＝0反映串链中两个位移矢量的运算对运动链总位移无影响的物理事实。

以双矢量概念为基础，形成了在6D空间算子代数。在机器人动力学建模中具有重要应用。同样，也有双四元数，在6R机械臂运动学逆解中具有非常重要的作用。

张量不变量反映的是：事物属性的不变性；要度量事物的属性就必须有参考对象；相对不同参考对象的度量存在必然的联系，即规律的客观性。事物属性的表征量必须指明它的参考对象。

“张量分析”是由爱因斯坦最初提出的，以之为基础，建立了电磁场及力场等理论。“张量分析”被广泛应用于连续介质粒子系统的研究。“张量不变性(Invariants ofTensors)”是自然界最基本的规律，是事物客观性的反映。

矢量空间具有“加(Addition)及标量积(Scalar Product)”的基本代数结构(Fundamental Algebraic Structures)，也包含额外结构：

【1】一个实数或复数矢量空间加上范数(Norms)结构则成为赋范矢量空间；【2】一个实数或复数矢量空间加上内积(Inner Product)结构则成为内积空间即酉空间；

【3】一个矢量空间加上极限(Limits)则成为拓扑矢量空间(Topological VectorSpaces)；

【4】一个矢量空间加上双线性算子(Bilinear Operator)则成为域代数(FieldAlgebra)；

【5】在笛卡尔直角坐标系下，线性空间是一个保距、保角(Preserving Distancesand Angles)的等积投影(Isometric Projection)空间，即矢量的范数及矢量间的角度保持不变。在机器人运动学及动力学研究中，一般使用笛卡尔直角坐标系。

上述不同的数学空间，对于机器人研究者，常全序及偏序是一个对象自身的属性常感到过于抽象。在数学系统建立过程中，由于数学家更专注于数学理论的形式化及逻辑的严谨性，常常忽视其背后的应用。矢量空间及双矢量空间是机器人运动学与动力学研究的基础，由于缺乏由原子及操作构成的元系统、缺乏严谨的公理化理论体系及运动链符号系统，致使复杂机器人系统研究存在严重困难。比如，尽管具有拉格朗日、凯恩等分析动力学方法，但是难以建立简洁的高自由度多刚体动力学系统方程。

矢量属于几何的范畴，具有直观的可理解性，但缺乏代数几何的可分析性；需要将二者有机地结合。同时，客观世界是通过一组动作的执行，由一组状态牵移至另一组状态的过程；通过空间的动作表述空间的运动，不仅易于理解，而且易于计算机软件工程实现。因此，本章提出运动链符号系统，在保证拓扑不变性及度量不度性的基础上，修正及发展(Revise and Develop)经典的矢量空间理论，以建立以动作为核心的、基于代数几何的、拓扑符号参与显式建模(Explicit/Analytical Modelling)的运动链符号演算系统。

一方面，计算机系统自身是以地址为索引的矩阵阵列；信息的存贮及表示也需要以阵列的方式保存。另一方面，有向Span树的偏序性是多轴系统的基本特点。因此，(1)链或偏序的集合是符号化理论的基础，全序的集合是对应偏序集合的特殊形式；(2)将事物的属性统一用矩阵表示，不仅表达简洁，而且适应数值计算机的结构特点。

首先，借鉴现代集合论的偏序集表征运动链。记称ⁱl_l为运动链。空集表示空位。若空位具有父集合的地址，则集合概念拓广为链的概念；若空位无父集合地址，则为空集。

实矩阵或复矩阵是偏序的集合。记k至j的位置矢量为显然，表示由k至j位置的谓词。

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

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

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

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

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

故有

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

记为的前继(Predecessor)。故有

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

ⁱl_i＝(i,i],|ⁱl_i|＝0。>

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

在运动链符号演算系统中，具有偏序的属性变量或常量，在名称上包含表示偏序的指标(Index/Indices)；要么包含左上角及右下角指标，要么包含右上角及右下角指标；它们的方向总是由左上角指标至右下角指标，或由右上角指标至右下角指标。例如：表示由k至l的位置矢量；其中：r是径向(Radial)的平动属性符；r_l^k表示由k至l的线位置。

给定运动链若n表示笛卡尔直角系，则称为笛卡尔轴链(Cartesian AC/CAC)；若n表示自然的运动轴，则称为自然轴链(Natural AC/NAC)。

链{L,≤}是有序的集合，其中：≤确定了链的序结构。序结构是拓扑系统及度量系统的基本结构之一。

在定义坐标系时，习惯使用右手序(RHS Order)，即由右手法则确定坐标轴的次序，例如[x,y,z]^T；故默认的坐标遵从右手序。相应地，也就确定了数的升序为右手序；例如[1,2,3]。链的连接次序简称链序，是由被研究对象本来的序结构确定的。在书写[x,y,z]、[1,2,3]等有序的集合时，习惯上由左至右书写，这样的书写次序称之为右手序。对应地，将基分量写成行的形式，称为左手序(LHS>

将运动链由根至叶的次序称为前向序(Forward Order)，反之称之为逆序(Backward Order)；与之对应的根标识号总是小于叶的标识号；与之对应的由根至叶的“缩放—转动—平动”过程，称为正运动。这一运动过程本质上源于正运动的参考是自我参考(Self Reference)的：以自身的体系为参考，首先进行形状的缩放，再以自身的体系参考进行转动，最后以自身的体系参考进行平动。例如：机器人要到达目标，首先需要调整姿态，至期望的姿态后，再进行平动；如此反复，直到到达目标。

在本书中，前向序、正序、右手序是对应的或等价的；反向序、逆序、左手序是对应的或等价的。前向与反向是相对于Span树连接方向而言的；正序与逆序是相对于参考轴或参考方向而言的；右手序及左手序则是相对于书写方向而言的。

给定两个索引集r及c，按“词法”(Lexicographic Order)次序比较：若且r^[1]＝c^[1]、…，r^[k]＝c^[k]，r^[k+1]≠c^[k+1]；则有r^[k+1]＞c^[k+1]。“词法正”是指字典中词汇编排次序。因此，无论被研究系统是否有序，只要被符号化，就一定有“词法”次序。

例如：若r＝[1,4,3,2]，c＝[1,2,4,3]；则有

计算机在矩阵运算时，经常需要通过索引引用该矩阵中的分块矩阵，也需要对矩阵中的元素进行重新排列；索引集合及引用是计算机符号处理的基本表示方法及基本操作方法。

常记[n,…,m]为n:m，其中：[·]表示取行或列；*表示任意符号或常数。若|r|＝|0|，索引集则称r为“词法正”的；若索引集则称r为“词法负”的。

高维度的树形运动链分析需要借助于现代计算机的符号演算与计算；以此为基础，才能开发相应的树形运动链运动学及动力学分析与建模的工具。运动链的符号系统需要与现代计算机理论相结合。建立适应现代计算机的符号操作需要，有助于理解复杂运动链的运动学及动力学的内在规律，并完成计算机软件开发。

在计算机系统中，无论是内存还是外存都是存贮阵列。引入索引符号作用在于：

多轴系统理论的技术实现需要通过软件编程实现，期望多轴系统的运动学及动力学方程具有伪代码(Pseudo-code)的功能。

机器人智能的基础在于软件模块的通用性，多轴系统运动学及动力学要实现拓扑、坐标系、运动学及动力学参量的参数化，在理论分析时，需要将它们作为参量处理。通过索引符号可以简洁地表征属性量与其子属性的关系。

【1】索引指标

给定有序的集合r＝[1,4,3,2]^T，记r^[k]表示取集合r的第k行的元素。常记[x]、[y]、[z]、[w]表示取第1、2、3、4列的元素。这些索引指标主要为了分析矩阵成员间的关系。例如：

【2】索引集

用于取元素序号的指标集合称之为索引集。例如，给定集合l＝[1,3,5,7]，索引集r＝[1,4,3,2]^T及c＝[1,2,4,3]；则有l^[r]＝[1,7,5,3]，l^[c]＝[1,3,7,5]。进一步，给定矩阵Q，其中：

则有

【3】成员的幂符号

记或为标量的k次幂；其中：右上角角标∧或表示分隔符。

运动链拓扑空间

首先，以公理化集合论为基础，提出基于运动轴的有向Span树；然后，建立有向Span树的符号系统及基本运算，为运动链符号系统的构建奠定基础。然而，现代集合论的链符号不是专门针对运动链分析的，有必要根据运动链的基本特点对链符号进行适应性修改。

轴链有向Span树

任何复合运动副可由两个基本运动副组成，即转动副R与棱柱副P。在有向Span树T中，子杆件Ω_l仅有一个父杆件且杆件Ω_l与运动轴A^[l]或运动副是一一映射的，即杆件Ω_l或轴A^[l]或运动副在对应关系上等价。故有

记多轴系统为D＝{T,A,B,K,F,NT}；其中：为有向Span树，A为轴序列，F＝{F^[l]|l∈A}为树链坐标系序列，B＝{B^[l]|l∈A}为杆件动力学体(简称体)序列，为运动副类型序列，NT为约束轴序列即非树。显然，运动轴序列A与体序列B、运动副类型序列K、参考系序列F是一一映射的关系；即

轴序列A是多轴系统D＝{T,A,B,K,F,NT}所有轴构成的轴链；T,B,K,F分别与A一一映射，是关于轴A的序列；NT与T构成了多轴系统D的拓扑结构即图G。

请参照图10。图10为CE3月面巡视器的轴链有向Span树图。其中：虚副ⁱk_c＝(i,c1,c2,c3,c4,c5,c]，即虚副ⁱk_c等价于一个轴链ⁱl_c。同样，其它复合运动副都可以通过一个轴链等价。

【1】图10中，在拓扑上，每个节点代表一个唯一的运动轴，所有运动轴按正序排列构成轴序列A；

【2】由实有向线段表示的运动副连接关系，确定轴序列A的父序列给定l∈A，则有：

【3】给定运动副按正序排列构成运动副类型序列K；

【4】给定虚线非树弧表示约束副存放于非树约束序列NT，即且有NT＝{^lC_k|l∈A,k∈CA,^lC_k∈{R,P,H,O}}；CA为约束副类型序列。

【5】系统D有|A|-|NT|个自由度，其中：|A|及|NT|表示轴序列A及父轴序列NT的基数。

在MAS系统D中，所有运动副分为两类：由轴序列A及其父轴序列确定的运动副、由非树约束集NT成员确定的约束副。轴序列A、父轴序列及非树约束集NT可以完整地反映一个图的连接关系。

请再度参照图10。由图10得轴序列及非树弧序列

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

由(1.18)及(1.19)，得父轴序列

运动链符号及基本操作如下：

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

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

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

【4】为取轴序列的成员；由式(1.18)可知，故计算复杂度为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树及动态图(Dynamic Graph)描述可变拓扑结构(Variable Topology Structure)。在支路^ll_k中，若则记即表示在支路中取成员m的子。

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

由(1.18)、(1.19)、及(1.21)得：及故有^cl_lb＝(c,lr,lb]，|^cl_lb|＝2，

由式(1.21)得中成员为lb的地址为17及21，从而由A得到lb的子为lrd及lmw，递归得lrd的子为lrw；故得^lbL＝{lb,lmw,lrd,lrw}。

运动链拓扑公理

对于运动链有以下运动链拓扑公理(Topology>

【1】ⁱl_n具有半开属性，即

【2】ⁱl_n空链或平凡链ⁱk_i的存在性，即

ⁱk_i∈ⁱl_n,|ⁱk_i|＝0。>

【3】ⁱl_n运动链具有串接性(可加性或可积性)，即

ⁱl_n＝ⁱl_l+^ll_n，>

ⁱl_n＝ⁱl_l·^ll_n。>

【4】^ll_n具有可逆性，即

^ll_n＝-ⁿl_l。>

由式(1.26)可知：由l至n的链^ll_n与n至l的链ⁿl_l是可逆的。运动链的偏序被称为链序(Chaining>

由式(1.24)及式(1.26)可知运动链具有不变性：

【1】运动链链序的一致性，即组成链的各项链序必须一致；

【2】运动链传递的串接性，即相邻两项的链指标_l+^l或_l·^l满足对消法则；

以上运动链四个拓扑公理反映了链序(上下指标的次序)的不变性是运动链运动学及动力学行为的基本准则。因此，链符号在高自由度运动学及动力学分析中具有以下作用：

【1】链符号是系统结构参数、运动参数及动力学参数表征的基础；

【2】明确运动参数间的依赖关系，是运动学及动力学分析的基础；

【3】表征运动学及动力学方程的链序不变性，保证方程的正确性。

轴链度量空间

多自由度(Multi-DoF)的机器人是典型的多轴系统(MAS)，记为D＝{T,A,B,K,F,NT}；其中：为有向Span树，A为轴序列，F为杆件参考系序列，B为杆件体序列，K为运动副类型集合，NT为约束轴的集合即非树。F为笛卡尔直角坐标系统。

要对系统属性进行距离及角度的度量(Metric)，就需要建立由参考点、参考轴或笛卡尔系构成的运动链度量系统(Metric System)。度量系统不仅决定系统属性的描述形式，也影响系统属性间的计算精度与复杂性。比如：以地磁北向为参考，需考虑地磁方向的精确性问题，因为不同时间及地点的地磁方向与大小大不相同。在矢量空间下，参考轴是最基本的参考单位，参考系可以由一组独立的参考轴构成。在工程上，坐标系选择需要考虑测量手段、测量精度及应用习惯。与理论坐标系不同，精密机械工程的坐标系要具有可测量的光学特征(Optical Speckle/Spots)；否则，既不能被人感知，又不能被光学设备检测。本节，首先建立轴链度量系统，再研究该系统的空间元素及关系，即度量空间(MetricSpaces)。

轴链完全Span树

通过对多体系统的分析，去除所有通过距离及角度进行度量的实体及关系，仅保留系统的连接关系，得到了以轴为基元的轴链有向Span树。

任一轴有其各自的3D点空间，称该空间为轴空间，且任一轴空间有且仅有一个自由度。因此，轴链有向Span树本质上表征了由一组运动轴构成的运动空间。复合运动副由一组串接的简单运动副等价，构成一个描述该复合运动副的轴链，表征的是该复合运动副的运动空间，该空间的自由度数与复合运动副的运动轴数相等。

为研究多体系统的运动学及动力学，需要在轴链有向Span树中增加通过距离及角度进行度量的实体及关系。点、线及体是3D轴空间构成要素。

【1】轴链有向Span树的点

杆件l的惯性中心或质心I记为l_I，表示惯性中心I是杆件l的子。通常情况下，任意点记为S；而l_S表示杆件l上的任意一点S，点l_S是杆件l的子；F^[l]表示由任意点l_S构成的3D笛卡尔空间。因此，任何点是树链空间下的点，是Span树的组成要素。

【2】轴链有向Span树中的轴

将轴l中的x轴、y轴及z轴分别记为lx，ly及lz；以表明x轴、y轴及z轴的父为l轴。显然，x,y,z是多轴系统的专用符号，不能用作轴名。

【3】轴链有向Span树中的体

体序列B＝{B^[l]|l∈A}，其中：若则体l的质量为零，其中，l_S是任意点，Ω_l表示杆件l的几何体。动力学体B^[l]是几何体Ω_l的从属属性。

对于复合运动副仅轴A^[l]与动力学体B^[l]固结，其质量和转动惯量非零，而其它轴无质量与惯量；任一轴均固结几何体Ω_k；显然，即Ω_k是Ω_l的子空间。

【4】轴链有向Span树中的力

环境i中的任一点i_S作用于B^[l]上点l_S的力和力矩分别记为固定矢量(FixedVector)和环境中的点i_S通过作用力与体上的点l_S通过力及力矩构成了回路。

因匀速或绝对不动的惯性空间不存在，惯性空间总是相对的，故这种惯性空间无实际操作上的意义；轴链Span树根据被研究系统的范围确定该系统共有的根，该根即惯性空间。因此，MAS系统的轴链Span树是轴链完全Span树(Axis Chain Full-Span Tree)。树根i表示世界，包含作用力的施力点i_S；惯性空间i由被研究的MAS及环境施力点共同确定。

投影矢量及张量不变性

运动链属性的度量总要相对于一定的坐标轴；否则，将无法度量运动链的属性。请参照图11，图11为坐标轴与基矢量参考图。如图11所示，一维坐标轴l 1101由原点O_l>l1103构成，是具有刻度的方向参考线；它是构成参考系的基元(Primitive)。一维坐标轴l>

整体形式的基矢量e_l>即由三个独立的有序符号组成，表示三个独立的自由度。

笛卡尔直角坐标系(笛卡尔系)由三个两两正交的坐标轴构成。在笛卡尔坐标下的角度及距离具有不变性，即保角及保距的特性；它们是机器人系统分析的基础。同时，符合人们对事物的认知习惯。笛卡尔直角坐标系即参考标架(Reference Frame)F^[l]＝O_l-x_ly_lz_l由原点O_l及基矢量(Base>l构成；其中：称为基架(Base Frame)，包含三个独立的符号，表示三个独立的维度。在数学中基矢量表示空间中一组独立的单位矢量；在工程中还需要考虑基矢量对应的度量单位(Metric Units)。基矢量e_l是客观的；比如，刚体的瞬时转动轴。而基架既是客观的，又是主观的；一方面，是基架与基矢量是等价，另一方面是基架是人为构造的。

笛卡尔空间是具有内积(点积)“·”及叉积“×”运算的空间。两个矢量的点积运算及叉积运算前提是矢量；两个坐标矢量的点积运算及叉积运算前提是同一参考系下坐标向量，可以统一为代数乘运算“·”。

请参照图12，图12为矢量的投影矢量图。如图12所示，由原点O_l1201至点l_S>1203对一维坐标轴l 1204或基矢量e_l>1205称为矢量对轴l坐标。记对参考轴l投影矢量(简称投影)为且有其中，r是平动3D矢量，其元素确定矢量的方向及大小。投影矢量依赖于单位坐标轴或单位基，该参考基又称为投影基(Projective Bases)。

显然，投影变换即是点积运算“·”，即

由式(1.27)可知：坐标轴可以作为平动的参考；但不能完整地作为转动的参考：要表达转动，要么增加另一个矢量，通过二者所张的角度表达；要么在坐标轴径向增加另一个零位参考矢量，通过绕该轴的转动角度表达。原因在于：坐标轴本质上是1D的。

投影符的优先级高于成员访问符或成员访问符优先级高于幂符引入投影符的作用在于：

【1】区分链节属性量，因为链节属性量反映相邻杆件(轴)的运动量通常是可以直接测量的；而不同链节间的运动量难以直接测量；

【2】在运动学及动力学方程中，保证运动链的链序关系正确，即链序的不变性；

【3】保证运动学及动力学方程书写简洁，以关注重要的运算关系；

【4】投影符与链指标一样，存在相应的运算法则，可以保证方程的正确性。

请参照图13，图13为矢量在笛卡尔直角坐标系的投影矢量图，包含原点O_l、正交基矢量e_l、坐标矢量位置矢量在笛卡尔三个坐标轴上的投影矢量为且有由于左上角指标指明了参考系，坐标矢量既间接表示了位移矢量又直接表示了位移坐标矢量，即具有矢量及坐标矢量的双重作用。笛卡尔坐标系F^[l]的正交坐标轴记为x_l、y_l及z_l。

位置矢量在坐标系F^[k]中的投影矢量记为且有

矢量即一阶张量是基矢量与坐标矢量的代数积。具有不变性的矢量表示为

其中：基e_l总写成行序(逆序)的形式；坐标矢量具有列序(正序)的形式。如同硬币的正反面关系一样，基e_l与坐标矢量具有对偶关系(Duality Relation)。

给定正交基矢量e_l，则有

||e_l||＝1。>

证明：基的度量需要以一个单位矢量为参考。请参照图14。图14为基矢量与基架关系图，包含原点O_l、正交基矢量e_l、单位轴矢量第一转动角度第二转动角度第三转动角度如图14所示，由基架的三个方向矢量分别转至任一单位矢量e_l的三个角度分别记为第一转动角度第二转动角度第三转动角度因其中：C＝cos。故这三个角度中只有两个独立的量。

因为基矢量e_l是三维空间的单位基，是一个独立的符号；同时具有基分量即有同时，基的相互关系通过投影标量即坐标来表达。故有

证毕。

由上证明可知，基矢量e_l是与F^[l]固结的任意单位矢量；转动基矢量e_l与转动F^[l]等价；基的度量依赖于坐标。用单位轴矢量替代e_l，则转动F^[l]与转动等价。因轴l的3D空间坐标轴矢量记为故将与轴l固结的三个有序的单位轴分别记lx、ly及lz；表明这三个坐标轴属于轴l。基矢量e_l与有序的基分量等价。坐标轴矢量与其上固结的三个有序单位坐标轴[lx,ly,lz]等价。

坐标矢量是固定矢量，有固定起点与终点；同时，指明了参考系，即参考基。因给定l∈A时，对于平动坐标矢量左上角l表示F^[l]的原点O_l。

记三维二阶张量在k系下的坐标阵列为

其中：的左上角k表示参考系，即是k系下的坐标阵列；的方向是由k系的原点O_k指向l系中的点S。以两个相同的坐标基e_k的阵列为参考；请参照图15，图15为三维二阶张量的基分量图。如图15所示，包含由两个基矢量的笛卡尔积(CartesianProduct)得到的9个二阶基分量；大写字母J表示3×3的与基分量对应的坐标阵列。具有不变性的二阶张量表示为

即二阶张量的基分量与对应坐标的乘积之和具有不变性。二阶张量的坐标阵列即坐标矩阵的6个非对角元素表示六面体法向对应的坐标，3个对角元素表示三个轴向对应的坐标。在力学中，转动惯性张量、应变及应力张量等均是二阶张量。

以不同参考系度量的属性量存在内在的关系，需要保证属性量的不变性：坐标与基相互参考，基与坐标的代数积保持不变；否则，不同的度量存在矛盾，不能保证属性量的客观性。故有，矢量及二阶张量的不变性关系：

显然，参在机器人逆运动学计算中具有非常重要的作用考基e_l不是运动链拓扑结构的要素，而是运动链度量的参考要素。矢量及二阶张量的左上角及右下角指标首先表示的是拓扑关系即连接关系，其左上角指标也表明参考系。

运动链度量规范

运动链不仅具有链序不变性，而且具有张量不变性；运动链的属性量通过链指标反映该属性量具有的链序关系。记主属性符为p或P。

【1】记矢量为满足矢量(一阶张量)不变性，

其中：及是3×1的坐标矢量，是在F^[l]下的表示；是在F^[k]下的表示。式(1.34)表明：矢量坐标与基矢量的一阶矩保持不变，即矢量具有不变性。三维矢量是三个基分量的线性组合。一个三维空间点由三维矢量刻画。

【2】记二阶张量满足二阶张量不变性，

其中：及是3×3的坐标阵列，是在下的表示；是在F^[k]下的表示。式(1.35)表明：二阶坐标张量与基的二阶多项式保持不变，即二阶张量具有不变性。二阶张量是9个二阶基分量的线性组合。张量表示事物的客观性，是代数系统建立的基础，即代数系统的基础公式通常需要通过张量的不变性予以证明。

两个三维空间点的耦合作用由三维二阶张量刻画。即

故称式(1.36)中由两个坐标矢量构成的并矢(Dyad)为二阶坐标张量。

【3】的零阶属性标量p记为

【4】若属性p或P是关于位置的，则应理解为坐标系的原点至F^[l]的原点；若属性p或P是关于方向的，则应理解为坐标系至F^[l]。

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

【6】称为投影符，矢量或二阶张量对参考基的投影矢量或投影序列，即坐标矢量或坐标阵列。

给定运动链根据上述规范约定：

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

【2】－原点O_k至原点O_l的平动矢量，在F^[k]下的坐标矢量；

【3】－原点O_k至点l_S的平动矢量，在F^[k]下的坐标矢量；

【4】－原点O_k至点S的平动矢量，在F^[k]下的坐标矢量；

【5】－运动副的轴矢量，及分别在及F^[l]下的坐标矢量；

【6】－沿轴的线位置，－绕轴的角位置；

【7】－平动轴的机械零位，－转动轴的机械零位；

【8】0－三维零矩阵；1－三维单位矩阵；

【9】转置表示对集合转置，不对成员执行转置；例如，

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

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

自然坐标系与轴不变量

在机器人工程中，先定义笛卡尔坐标系统；再通过工程测量确定坐标系间的关系；最后，以该坐标系统为参考，进行机器人运动学及动力学分析。杆件间的关系需要通过与它们固结的坐标系进行度量。然而，笛卡尔系的坐标轴两两正交是一个非常强的约束，它只发生在理论上。在历史上，笛卡尔系从未受到挑战。

将笛卡尔坐标系与体固结，即标记原点及坐标轴方向；借助光学特征，才能应用现代光学设备(比如，激光跟踪仪)测量坐标系间的相互关系；具有一定大小的一组光学特征难以满足笛卡尔坐标轴两两正交的精度需求，导致过大的测量误差。对于精密机器人工程而言，即使微小的角度，比如10角秒，也会通过杆件将位置误差放大至不可接受的程度。同时，笛卡尔系与杆件固结受到杆件实际空间限制，在杆件内部及外部均导致无法测量。因此，在研究精密机器人系统时，期望间接地确定及应用笛卡尔系。首先，在勿需三轴两两正交约束下，测量一组具有极小光学特征的测点的空间位置；然后，依据一定准则，通过计算间接确定笛卡尔坐标系统。应用现代光学设备，由于测点的精度易于满足工程精度需求，计算引起的误差可以忽略不记；从而，保证笛卡尔坐标系统精度。这是一个先测量后定义的过程；与传统的先定义笛卡尔坐标系统后测量的过程相反。

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

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

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

由定义1可知，在系统处于零位时，所有杆件的关节坐标系与底座或世界系的方向是一致的。

请参照图16。图16为自然坐标系与轴不变量图，包含转动角度1601、轴矢量1602。如图16所示，系统处于零位即时，关节坐标系绕轴矢量1602转动角度1601将转至F^[l]；在下的坐标矢量与在F^[l]下的坐标矢量恒等，即有

由式(1.37)知，或不依赖于相邻的坐标系故称或为轴不变量；在不强调不变性时，简称为坐标轴矢量或轴矢量。或表征的是体与体l共有的参考单位坐标矢量，与及O_l无关。在第二部分，将对式(1.37)予以证明。

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

(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】式(1.37)表明在自然坐标系下：即轴不变量是非常特殊的矢量，且有非常优良的数学操作性能，在后续章节中将予以分析与应用；

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

【5】在后续章节中，应用轴矢量的优良操作，将建立包含拓扑结构、坐标系、极性、结构参量及动力学参量的统一的多轴系统运动学及动力学方程。

因基矢量e_l是与F^[l]固结的任一矢量，基矢量是与固结的任一矢量，又是F^[l]及共有的单位矢量，故是F^[l]及共有的基矢量。因此，轴不变量是F^[l]及共有的参考基。轴不变量是参数化的自然坐标基(Natural Base Coordinates)，是多轴系统的基元。

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

因此，应用自然坐标系统，当系统处于零位时，本质上只需确定一个公共的参考系，而不必为系统中每一杆件确定各自的体坐标系，因为它们可由轴矢量及自然坐标唯一确定。当系统分析时，与杆件固结的自然坐标系只发生在概念上。自然坐标系统对于MAS理论分析及工程作用在于：

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

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

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

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

【5】同时，轴不变量在理论上具有非常优良的操作性能，例如，轴不变量与轴内力是正交的，故轴不变量是轴内力的解耦自然正交补(Decoupled NaturalOrthogonal Complement)；可以建立基于自然不变量(Natural Invariant)的迭代式的运动学与动力学方程，既保证建模的精确性与简洁性，又可以保证计算的实时性。

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

定义3：转动坐标矢量：绕坐标轴矢量转动角度的坐标矢量为

定义4：平动坐标矢量：沿坐标轴矢量平动位置的平动矢量为

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

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

故关节的控制量为

定义7：自然运动矢量：相对坐标轴矢量平动或转动q_l的运动坐标矢量为

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

定义8：关节空间：以关节坐标q_l表示的空间称为关节坐标空间，简称关节空间(Joint>

定义9：位形空间：称表达位置及姿态(简称位姿/Position and Orientation)的两个笛卡尔空间为位形空间(Configuration Space/CS)，是双矢量空间(Dual VectorSpace)或6D空间。

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

请参照图17。图17为固定轴不变量参考图。如图17所示，有杆件1701与杆件l1702，给定链节原点O_l受位置矢量1703约束的轴矢量1704为固定轴矢量(FixedAxis-vector)，记为其中：

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

固定轴不变量是自然运动(平动与转动)的单位参考轴，称之为自然坐标轴(Natural Coordinate-axis/NCA)；以之为参考的坐标为自然坐标或关节坐标描述自然的平动与转动。

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

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

显然，固定轴不变量是运动轴l的自然坐标轴。与笛卡尔坐标轴不同在于：笛卡尔系具有三个正交的笛卡尔坐标轴构成；而自然坐标轴有且只有一个坐标轴。显然，三个独立的自然坐标轴，既可以定义一个三维空间的斜坐标系，又可以定义一个笛卡尔直角坐标系。在一个自由体上，可以定义三个独立的自然坐标轴作为该体的平动坐标系；又可以定义另三个独立的自然坐标轴作为该体的转动坐标系；即在一个自由体上定义六个独立的自然坐标轴作为该体平动及转动的六维空间参考。因此，以自然坐标轴为基础的自然参考系具有笛卡尔直角坐标系不具有的灵活性。

【1】当时，则有

将3D转动矢量及3D平动矢量合写为6D的形式称之为固定运动螺旋(Fixed Motion Screw)或运动矢量，它具有一个平动自由度及一个转动自由度。相应地，称为固定螺旋轴，简称为螺旋轴；显然，是结构螺旋轴，它是螺旋轴的特例。因固定螺旋是螺旋轴的原点受约束的3D螺旋，故称固定螺旋为3D螺旋。与运动螺旋相对应，分别称及为速度及加速度螺旋。与固定运动螺旋相对的是3D力螺旋(Fixed Force Screw)。显然，螺旋轴具有5个独立参量，包含3P位置及2R姿态。

【2】当时，p_l为螺旋步距，则有

式(1.46)与式(1.47)等价，且有

【3】当则有

显然，称仅含一个转动自由度的为转动矢量，是运动矢量的特例。

【4】当则有

显然，称仅含一个平动自由度的为平动矢量，是固定运动螺旋的特例。

关节坐标又称为自然坐标；同样有关节速度、关节加速度；它们合称为自然运动量。自然轴空间是由一组独立自然坐标轴构成的自然坐标轴链(Natural Coordinate-axisChain)系统，简称自然轴链(Natural Axis Chain/NAS)系统。式(1.30)表明：与一个轴空间固结的轴不变量等价于该空间的单位基矢量；平移及转动一个固定轴不变量等价于平移及转动与之固结的自然系；固定轴不变量的平动与转动就是一个螺旋运动；自然坐标轴链与实际运动链的运动序列具有同构关系；运动链就是由一组有序的固定轴不变量构成的空间关系链。

式(1.46)至式(1.50)中的if语句间是“或”关系，以保证后续多轴系统运动学及动力学方程数目与自由度相对应。在算法上，式(1.46)至式(1.50)自然地(Naturally/In anatural way)分配轴运动的计算空间；这与经典的运动螺旋具有本质区别。由自然坐标系、自然不变量及自然螺旋构成了运动链度量系统；它与链拓扑系统构成运动链符号演算系统。自然表达常常意味着简单、精确、优雅及清晰("Natural expression"often implysimplicity,accuracy,elegance and clarity)；进一步，揭示多轴系统的本质；从而，提升系统计算精度、速度及可靠性。

由式(1.45)可知，固定轴不变量的关节坐标与坐标系及F^[l]的位姿关系是一一映射的。三维空间下的线位移及角位移均是参考轴的螺旋运动因是不变量，即该坐标矢量是不变的，故称轴l的转动为定轴转动。由至F^[l]的运动路径有无穷个，但存在唯一最短路径的运动，该运动即为定轴的螺旋运动。

请参照图18。图18为由双矢量确定的转动图。如图18所示，原点O_l>0的单位矢量1801经转动角度1804至t的单位矢量1802的转动等价为绕轴矢量1803经1804角度的转动。这表明：有向线段至有向线段的转动本质是绕轴的转动。称为零位矢量至当前矢量的轴矢量或螺旋轴。零位矢量向当前矢量方向转动1/4周的矢量称为至的径向矢量。这表明：由双矢量确定的转动与螺旋运动等价，其中：

因此，沿轴的平动、绕轴的转动及其复合运动都是螺旋运动。

因圆柱副的运动是螺旋运动，故称圆柱副的轴为螺旋轴(Screw axis)。由式(1.46)可知，棱柱副、转动轴副、螺旋副是螺旋运动的特例；将螺旋运动的螺旋轴作为一般的运动轴或约束轴，有助于简化系统的实现。相应地，称以螺旋运动轴为基础的轴链为螺旋轴链，通过螺旋轴链的实例化，可以获得由平动轴、转动轴、螺旋轴构成的轴链。固定轴不变量本质上是螺旋轴不变量；由式(1.46)可知，固定轴不变量是运动矢量的参考轴。

以运动轴表征的运动链系统称为自然轴链系统；以笛卡尔坐标轴表征的运动链系统则称为笛卡尔轴链系统。后者是前者的特殊情形。由式(1.46)至式(1.50)是以平动矢量及转动矢量为基础，描述了以螺旋轴不变量为核心的螺旋运动；它的空间维度可以根据运动轴或约束轴的类别自适应地分配；从而，保证多轴系统分析的灵活性。故将式(1.46)至式(1.50)称为自然螺旋，它与现有文献的6D即双矢量的螺旋不同。由式(1.46)得6D运动矢量与固定轴矢量的关系如下：

式(1.52)中对时间的导数即在6D空间(双矢量/Dual vectors)算子代数中是以整体形式合写为运动旋量(Twist)，将力与力矩两矢量合写为力旋量(Wrench)。它们是螺旋(Screw)的两种实例。

【1】多轴系统运动学及动力学理论感兴趣的是3D自然轴空间；仅在阐明与双矢量空间关系时，采用6D运动旋量及6D力旋量。因6D空间操作代数过于抽象，且6D空间操作代数运动学与动力学计算复杂度过大，违背多轴系统实时建模与控制理论建立的初衷。

【2】刚体的运动是6D的，该空间下的基元是体，即系统的层次是刚体；任一体是由三个独立的点经两两相对位置约束等价的；3个独立的点有9个维度，两两约束即约束了3个维度，故任一体具有6个维度。6D空间是双3D矢量空间，如式(1.5)至(1.8)所示。

6D运动矢量的速度及加速度也是运动矢量。3D运动矢量或及它们的速度与加速度也是运动矢量。通常，将6D运动矢量或3D运动矢量统称为运动矢量。轴链控制过程，就是通过平动及转动固定轴不变量，控制与系统固接的运动矢量与期望运动矢量对齐(Alignment)的过程。运动矢量由定点双矢量表示，其实线矢量表示位置矢量，虚线矢量表示转动矢量；当期望运动矢量与杆件l运动矢量重合时，则表示两个运动矢量对齐。

D-H系与D-H参数

D-H(Denavit and Hartenberg)系在机器人逆运动学计算中具有非常重要的作用。给定一个多轴系统，以任一状态作为系统零位。底座系根据实际需要确定，通常其原点置于第1根轴上。自然坐标系对应的D-H系记为根据D-H坐标系统的编号习惯，运动副对应的轴记为即D-H系统中的指标习惯遵从父指标，这一点与自然坐标系统的编号遵从子指标不同。

给定链节除叶杆件外，建立D-H系的过程如下：

【1】令及z_l′分别在及上。

【2】作及z_l′的公垂线与及z_l′的交点分别为及O_l′。其中：作为中间参考系的原点，O_l′作为F^[l′]的原点；即由运动副及的轴线共同确定与杆件固结的点O_l′。

【3】选取由至z_l′的方向定义为

【4】若选择任一公垂线作为x_l′。若及z_l′相交，选其交点作为F^[l′]的原点O_l′。

【5】根据右手法则，补足y_l′。

建立D-H系后，可以确定杆件的D-H参数：

【1】是由点至点的轴偏距；平移后与重合。

【2】是轴至轴l′x的转角；初态绕轴转动后，轴与轴l′x对齐，至中间坐标系

【3】是由点至点O_l′的轴距；平移后与O_l′重合。

【4】是由轴至轴l′z的扭角；转动后，轴与轴l′z对齐，至终态F^[l′]；

由上可知，运动副具有三个D-H结构参数及一个运动参数，比刚体少了两个维度；因为D-H系依赖于公垂线，存在两个约束。在关节坐标系统，存在五个结构参数，一个运动参数，对应于刚体的六个维度。

在自然坐标系统下，运动副根杆件运动必导致叶杆件l运动，D-H系及D-H参数确定过程遵从由根至叶的次序；因子节点具有唯一的标识，叶杆件l的结构参数及运动参量指标与对应的杆件指标l相对应；运动参数及结构参数的度量以其父轴为参考。但是，对于D-H坐标系统，因为D-H系不是自然坐标系且结构参数表达是不自然的：D-H系及D-H参数确定过程比较繁琐；是运动参数，结构参数的指标是遵从父指标，违反了树链系统指标遵从子指标的约定。因此，D-H系及D-H参数确定过程不适用于树链系统。解决这一矛盾的方法是：运动副对应的轴记为l，杆件由1开始编号，令D-H编号遵从子指标。

同时，可知：杆件及l的D-H系分别记为及F^[l′]。D-H运动链等价于笛卡尔轴链记轴类型序列或运动序列为记轴不变量序列为及由杆件的初始位形，经4个运动序列后，至杆件l的终态位形。

最后一个杆件l的D-H确定方法如下：

【1】期望点C(腕心)与杆件l固结；引入虚副^lV_c，过点C作z_c′，z_l′||z_c′；

【2】作z_l′及z_c′的公垂线，且与轴z_l′及z_c′分别相交于及O_c′；选取任一方向，定义为x_c′；

【3】按右手法则，补足y_c′。

相应地，确定杆件l的D-H参数：

【1】是由O_l′至的轴偏距。

【2】是由至C的轴偏距；是由至O_c′的轴距。

【3】α_C＝0，是由轴l′z至轴c′z的扭角；a_C＝0，是由O_c′至C的轴距

D-H建立过程需要以自然坐标系为基础，即系统零位时，参考系方向保持一致；绕轴转动的过程是将轴与轴对齐的过程；绕轴转动的过程是将轴与轴l′z对齐的过程。称为关节角参考零位，为扭角参考零位。在D-H参数中，称及为结构参数；称及为参考零位。记转动副的D-H关节坐标为θ_l，自然关节坐标为e_l；则有

要保证D-H参数运算的正确性，就必须保证初始参考的一致性，即需要以自然坐标系为参考，以自然关节坐标进行计算。否则，要么产生计算错误，要么增加系统计算的复杂性。轴不变量与第二部分将阐述的螺旋径向不变量可以确定D-H系。D-H系与D-H参数确定过程较繁琐，表达也不自然；因为正交的笛卡尔系在工程上易于引入测量误差，难以满足精密多轴系统的工程需求。但其结构参数较少，在逆运动学计算时有一定优势。

上述D-H系确定规则只是诸多方法的一种。只要保证结构参数为3个及运动参数为1个的坐标，均可视为D-H系统。应用D-H系的目的主要是为了简化逆运动学的分析与求解。

一方面，不以一定的参考对象为基准，就无法认识事物的属性。另一方面，相对不同参考基准的属性量之间存在共有的、不变的量即不变量；否则，就不能反映事物的客观性。

参考对象通常包含：参考点、参考轴、参考系等。不变性是事物属性及属性间作用规律的客观性。序不变性、张量不变性及对偶性是系统研究的基本准则。

张量不变性是指所有基分量与坐标分量的代数积之和具有不变性。包含标量、矢量及二阶张量的张量不变性正是系统属性的客观性反映；等价于不同度量单位与度量数量的乘积要保持不变；否则，会导致人为的度量错误。

除了轴不变量固定轴不变量自然坐标δ^[i][k]及等自然不变量之外，客观世界存在其它多种不变量；比如，圆周率π，指数e，光速c等。发现及应用不变量来解决问题是科学研究的永恒主题。

客观的自然空间是3D的，该空间下的基元是点，即该系统的最小粒度是空间点；3D是自然空间的不变量。给定一个惯性单元，三轴加速度具有三个独立的维度；任何加装的其它加速度计检测的加速度与这三个独立的三轴加速度是相关的；实际的加速度在同一时刻仅能有三个独立的加速度等价。同样，轴不变量是多轴系统运动学及动力学系统的基元，通过它可以建立精确的、实时的、具有通用性的仿真分析系统及控制系统。

对于不同的运动副，自由度及约束度是运动副的不变量，它们对应于不同的子空间。以自然坐标系为参考时，坐标轴矢量是不依赖于相邻杆件体系的坐标矢量，是一个特定的不变量，其依赖的系统层次是运动副，反映了相邻杆件的共轴性，即是一个公共的参考轴。

不变量是一类事物或系统属性的反映。同样，由运动副及杆件构成的多轴系统，自然存在该系统层次下的诸多不变量。

序不变性是指系统连接次序不因参考基准不同而发生改变。无论空间系统有多复杂，它总以点为基元；在不同的系统层次下，点与点之间存在序的作用关系，包含：相邻点的位置关系、质点间的运动传递关系、质点间的内力传递关系等。序不变性是理论系统及计算系统正确性的基本保证。

对偶性是指相互依存的两个属性具有相对的作用及相互依存的共同属性；比如：力与运动具有对偶性、基与坐标具有对偶性。与不变性及对偶性密切相关的是不变量，它是指不依赖于所有或部分参考基而存在的量。

正如我们认识世界一样：首先，从自我的角度理解问题；然后，才能从他人的角度去思考。显然，自我参考(Ego Reference)是认识事物的前提；超我参考(SuperegoReference)是合作共存的基本保证。正确认识多轴系统运动特性，也需要自我参考及超我参考。建立参考标架是认识事物的基本前提。

【1】在多轴系统，必须区分物理量是否可测与是否可控；传统的运动学与动力学理论总是置可控性与可测性于不顾，易导致物理上的不可实现及理论分析的无意义；只有具有可控制性的运动量，在工程上才能被控制；只有具有可观测性的运动量，在工程上才能被测量。

【2】在多轴系统当中，可控的及可测的运动量在结构上必须存在相应的轴向连接；即只有同一个运动副的运动量是可控的及可测的，不同运动副的运动量是不可控的及不可测的；

由式(1.46)可知，自我参考的角位移及线位移是直接可测的及可控制的；同样，自我参考的角速度及线速度是直接可测的及可控制的；自我参考的转动速度及平动也是可控的及可测的，因为它们的结构参数及运动参数是可测量的。因此，称直接可测运动量的参考系为度量坐标系(Measure Reference Frame)。若及通常是不直接可控及可测的；及也是不直接可控及可测的。因此，投影算子区分了不可测、不可控制的运动量。概念上的投影仅保证度量在数量上的等价，而不具有物理的可控性及可测性。故称间接测量的参考系为投影参考系(Projection ReferenceFrame)。

链节的运动量在物理上是可控制的及可测的，链节是运动链串接过程的基本单位，是不可逾越的；否则，违反运动链的传递性。

运动链度量公理

公理1.2给定运动链^kL，以运动链拓扑空间算子为基础的三维矢量空间算子具有运动链度量公理(Metric>

【1】运动矢量的偏序性

根据运动链拓扑公理及参考的一致性，得

式(1.54)至(1.55)表明：链节的关节角位置及线位置转动矢量及平动矢量是运动链的基本属性；具有正序及逆序的等价性即具有全序性。

【2】运动链逆向运动的传递性及可加性：

式(1.56)至(1.57)表明：属性p的坐标矢量ⁱp_l、属性P的二阶坐标张量ⁱP_l对于闭子树^kL的可加性；反映叶向作用力对根向作用力的传递性及可加性。任一和项的参考指标需一致，任一积项的指标满足对消原则，即前一个积项的右下角指标与后一个积项的左上角指标相同且可对消。等式两侧的链序一致，即运动链的连接次序保持不变。

【3】运动链前向运动的传递性及可加性：

式(1.58)及(1.59)表明：属性p的坐标矢量属性P的二阶坐标张量对于运动链ⁱl_j的串接性；具有迭代的关系式。反映根向运动对叶向运动的串接性及可加性。

正因为运动链前向运动的传递性及可加性、运动链逆向作用力的传递性及可加性，产生运动副运动的全序性及运动链运动的全序性。

由运动链符号系统规范、自然坐标系统、运动链公理构建了运动链符号演算系统；它为基于轴不变量的多轴系统建模与控制提供了元系统。

首先，以运动链拓扑空间算子为基础，建立三维矢量空间操作代数；进一步，为建立基于轴不变量的多轴系统运动学理论奠定基础。

笛卡尔直角坐标空间是三维点积空间或酉空间。相对于不同笛卡尔直角坐标系(简称笛卡尔系)的距离及角度具有不变性。在点积空间下，除了具有加法及标量乘运算外，矢量还具有点积及叉积运算。笛卡尔系是三维矢量空间的基本要素或基元。

然而，笛卡尔系是两两正交的三个自然坐标轴构成的系统。因此，笛卡尔坐标轴系统是自然坐标轴系统的特例。

尽管矢量的点积及叉积运算的几何含义非常清晰；但是，难以揭示复杂矢量空间运算的规律。需要建立公理化的矢量空间操作代数(Operation Algebra)系统，将几何的矢量分析方法转化为代数的分析方法。不仅可以进一步揭示复杂矢量空间下的规律，而且适应数值计算机的结构特点。从而，为树链多轴系统的运动学与动力学分析奠定基础。

三维矢量空间操作代数是一个公理化的系统：真实三维空间的点是三维矢量空间的基元，任两个独立的空间点构成线，任三个独立的空间点构成体。三维是客观世界的自然本征量。应用高维空间理论解决低维空间的问题，必然会带来计算效率及可理解性方面的问题。六维空间算子代数正是以六维矢量空间的理论解决真实三维空间问题的典型例证。

算子是数学概念，操作是计算机概念。之所以称为三维矢量空间操作代数，是因为该代数系统是适应计算机处理的：一方面，符号系统是简洁、精确，结构化的，不仅易于理解，也易于计算机操作；另一方面，操作是机械性的过程，是以简洁的基本操作为基础的，过程具有简洁的迭代式结构，在技术实现上是简洁的及高效率的。更重要的是：操作既包含函数运算，又包含逻辑判断。一方面，三维空间操作代数以树链拓扑符号系统为基础，取父、取运动链及取闭子树等拓扑操作是关于轴序列及父轴序列的离散操作过程；另一方面，该空间操作代数是以符号演算、动作(投影、对齐及转矩等)及矩阵操作为核心的。在计算机自动建模与分析时，需要判别属性符号的构成与指标，需要执行阵列元素的访问操作。

笛卡尔直角坐标系F^[l]＝O_l-x_ly_lz_l由原点O_l及三个正交的坐标轴构成；坐标轴的单位矢量称为坐标基，记为及是三个独立的符号，且有

式表示：1^[x]、1^[y]及1^[z]是三个独立的3D坐标基矢量；同时，由式(1.60)可知：单位基矢量e_l与单位坐标基1一一映射，即转动单位基矢量e_l等价于转动与之固结的任一单位坐标基1，即有

显然，坐标基1张成了一个单位立方体；转动刚体上任一单位立方体1与基矢量e_l等价。

基与坐标在序上具有对偶性(Duality)，它们的点积(代数积)具有不变性，位置矢量与基坐标关系为

由式可知，矢量是基矢量e_l及坐标矢量的点积，是关于基分量的一次式；矢量是一阶张量，是不变量；即

基矢量e_l与坐标构成“基架-坐标”对。基矢量e_l与坐标或坐标矢量是对偶的；前者服从左手序即以行向量表示，后者服从右手序即以列向量表示；套用唯物辩证法的语言表述为：基矢量e_l与坐标矢量分别表示位置矢量中互为对偶的两个方面，它们相互依存，是不可分割的整体。

因故有

由式可知，转置符号不改变上下指标的次序即链序，仅改变数据的排列次序。

由式可知：矩阵积符号即为代数积符号“·”；在本书中，该符号起到间隔不同属性符号的作用，使表达式更加清晰。

记两个矢量的外积或外部积符号为计算过程如下：

其计算结果为矩阵，即两个1阶矢量外积后，结果的阶次为2阶。显然，两个1阶矢量内积后的阶次为0，即标量。

【1】基的外标量积

由基矢量e_l与基矢量(一阶张量)e_l外积是基矢量e_l的并矢，即按标量积运算“·”构成二阶张量，即基矢量e_l(任意一个单位置矢量)相对于自身的投影；

故有

由得δ^[i][k]，

称δ^[i][k]为克罗内克符号。式中δ^[i][k]与线性代数中的定义相比，除了指标表示方法不同外，其它完全一致。

【2】基的外矢量积

基矢量e_l并矢即外矢量积，按叉积运算构成二阶张量，故有

并定义基矢量的叉乘矩阵，

是由e_l组成的二阶张量，其元素对应于单位立方体的六个面的法向，表示具有反对称性的空间旋转；式服从逆序。的螺旋轴矢量即轴矢量表示如下：

式中，是一个反对称矩阵，与e_l具有一一映射的关系，即转动基矢量与平动单位基e_l是对偶的。的右上三角元素遵从左手序(反序)，即

基矢量e_l的左手序具有不变性，其中i,j∈{1,2,3}。显然，上标的波浪符是一个衍生符，因为它仅改变的排列形式。称e_l是的逆序轴矢量。由式可知

其中：为李奇符号，且有

式中除指标表示不同外与线性代数中的定义完全一致。由式可知，基矢量e_l服从左手序。与e_l具有一一映射的关系；与e_l方向相反，服从左手序。式(1.31)中的二阶基分量与其一阶基分量的叉乘矩阵相对应。因故与e_l正交。式中的{x,y,z}的右手序包含：[x,y,z]、[y,z,x]及[z,x,y]；其它为左手序。由式及式可知：基的外矢量积表示基的螺旋二阶张量。

【3】基矢量的矢量积

因是任意一个单位基矢量，故有叉积，亦称矢量积，

式表明，在笛卡尔直角坐标系下，表示空间旋转，e_l表示空间平移；因e_l等价于共轴线的平动与转动不存在耦合，即空间螺旋线上的平动与转动互不影响。

矢量积运算

【1】坐标矢量的外代数积

称为外代数积，它是由及两个一阶矢量张成的二阶张量，即基矢量e_l位于外侧的矩阵代数积运算，

故定义其坐标形式为

因此，外积运算是一种特定的矩阵运算。

【2】坐标矢量的外标量积

称为坐标矢量的外标量积，是基矢量e_l位于内侧的外点积，

故其坐标形式为

由式可知：坐标矢量内积运算可以用矩阵乘“·”即代数乘表示。

【3】矢量的矢量积

称为矢量积，即基矢量e_l位于内侧的矢量积运算，

证明：

故式成立。证毕。

矢量的转动

基矢量及基矢量e_l的外点积定义为

则有

证明：因

则有

由式(1.79)得

证毕。

显然，是关于及e_l二次多项式，即是二阶张量。式表明该式两边的链符号是一致的。式表明：矢量空间下的转动具有度量的不变性。由式得

由式及式得

即

由式得

即

式及为基变换公式；显然，指标_l·^l满足对消法则。由式得

由式及式得

由上可知：

由式可知，表示由到F^[l]的姿态，即由转动至l，链序由左上指标至右下指标确定。

由式可知，是以系为参考的坐标阵列，是e_l的三个基分量分别对三个基分量的投影矢量；故是投影矢量序列。因笛卡尔轴是正交的，故是正射投影(OrthogonalProjection)，具有是“保角”及“保距”的属性。

由式可知，是方向余弦矩阵(DCM)，即有

由式可知，坐标基的外点积是基分量间的投影标量；由式(1.29)可知，基矢量的内积是基矢量间的标量投影。由式可知，虽然方向余弦是全序的，但方向余弦矩阵是偏序的。

由式可知：故

因是3·3的实矩阵，故有三个单位特征值；其中一个必为实数，记为另两个为共轭复数与且又因故有因此，必有特征值由第3章可知，的特征值1对应的特征矢量即为轴不变量

【2】矢量的投影

若矢量与坐标基e_l固结，矢量对坐标基的投影矢量记为矢量对坐标基e_l的投影矢量为则有

将式代入式得

故有

由式及式得

式及式为坐标变换即投影矢量公式。式为投影矢量的右序形式，即将位于投影算子右侧的相对F^[l]系表示的矢量投影至该算子左侧的系；式为投影矢量的左序形式，即将位于投影算子的相对系表示的矢量投影至该算子右侧的F^[l]系。

与同序的表示转动，与反序的表示投影。通过转动将方向矢量与方向矢量对齐，通过平动实现位置矢量对齐，通过运动实现定点方向矢量对齐。因此，式物理含义为：当系统处于零位时，给定任意两个坐标矢量及此时即期望两矢量与趋向对齐；经过转动后，即将与对齐。

同样，含义表述为：当系统处于零位时，给定任意期望坐标矢量及与杆件固结的坐标矢量此时，有即期望两矢量与趋向对齐；经过转动后两方向逐渐趋向对齐；再次转动后，即与已对齐。这是由根对叶的正向转移过程。

显然有，

故有

矢量的一阶螺旋矩

“右手法则”本质上用于确定右手螺旋，大姆指的指向是“螺旋矩”(Screw Moment)的方向。平动与转动是螺旋运动的两个对偶方面，力与力矩是力螺旋的两个对偶方面。“螺旋矩”简称为“矩”是固定矢量平移过程产生的转动效应(Turning Effect)。

如图18所示，零位矢量作用(Action)于当前矢量的“螺旋矩”是在它们轴矢量上产生的转动效应。根据右手法则可以确定零位矢量至当前矢量的轴矢量即通过两个矢量的叉乘得到它们的轴矢量。“螺旋矩”应视为的动作，其过程为：先向径向矢量投影与再绕零位矢量转动1/4周。

记3D矢量及3D转动速度矢量运算，则有

其中：

证明：由式得

其中：为续行符。又

故有

证毕。

由式(1.92)可知：

【1】由矢量^lω_k唯一确定了该矢量的叉乘矩阵将^lω_k称为的轴矢量；

【2】坐标矢量的“叉乘运算”可应用对应“叉乘矩阵”替代，后续通常不再使用叉乘符号；

【3】同时，表示转动与平动的耦合，参考基为e_l。因是e_l的切空间或切标架，故表示由^lω_j至^lr_k的切向量。因与e_l正交，故式(1.92)中表示^l|jr_k的螺旋变换，即是由坐标矢量^lω_j至坐标矢量^l|jr_k的正交坐标矢量；

【4】因根方向的转动牵连叶方向的平动，满足_l·^l链序的对消法则，体现平动与转动的对偶性。投影符清晰表达了链序的作用关系，书写也简单方便。

“叉乘”表示坐标矢量由右至左的矢量积运算，由左至右的“螺旋矩”表示运动状态的变更，具有链序关系，二者互为对偶。对产生的螺旋矩为速度，对产生的“矩”为力矩(Torque/Force Moment)。由与可知：转动^i|lω_k与力作用的序是对偶的。将不区分链序关系的叉乘称为矩，将具有n个叉乘的运算称为n阶矩，比如2阶惯性矩因运动状态与力的作用是对偶的，故常把它们分别表示在方程的两侧；否则，需要加“-”，进行序的变更。运动状态变更的原因是力的作用；前者的角速度位于左侧，遵从左手序；后者的力位于右侧，遵从右手序。

同时，有下式成立。

证明：因

又

故式成立。

对比式及式(1.93)可知：坐标基的叉乘矩阵与坐标矢量的叉乘矩阵具有相反的链序关系，反映了坐标基与坐标矢量的对偶性。式(1.93)的作用在于：将几何形式的叉积运算转换为代数式的叉乘矩阵运算。

绕坐标轴的简单转动仅仅是转动的特殊形式。事实上：刚体在空间下绕任一轴转动，由转动前至转动后的状态即旋转变换阵可以由该转动轴矢量及转动角度唯一确定。坐标矢量存在唯一对应的矩阵，该坐标矢量为其对应矩阵的轴矢量(Axis Vector)。

定义12：称坐标矢量^lω_j就是其叉乘矩阵的轴矢量。并记为

任一矩阵^kM_l可表示为其中：为对称阵，为反对称阵。由上可知，存在轴矢量。

定义13：称矩阵^kM_l的反对称阵的轴矢量为矩阵^kM_l的轴矢量。

根据上面定义得

故有

给定任一矢量及矢量^lr_k，则有

证明：是反对称矩阵，是矩阵的反对称阵部分。对(*表示任何符号)，有

由式得

故有

考虑式(1.97)的轴矢量定义，可知式(1.99)成立。证毕。

由式(1.99)可知：右式为由左至右的叉乘即螺旋矩，左式为由右至左的坐标运算，二者具有对偶的关系。

二阶张量投影

将在j系下的表示(投影)记为因二阶张量是不变量，则有

因及是基的二次式，具有标量的形式，是不变量。

由式得

故

式是二阶张量坐标阵列的变换公式，即相似变换公式。简单说：相似变换即是二阶张量的坐标变换即为投影。同样，二阶张量的投影可以清晰地表达链序关系，书写也很简洁。

运动链的前向迭代与逆向递归

给定运动链则有

即

由式及式得

故有

式的物理含义表述为：系统处理零位时，先将与杆件l固结的位矢平移后将转动从而，使与对齐。考虑运动链⁰l_3C＝(0:3,3C]，运动过程是由根向叶的正向迭代，故有运动链计算的逆向递归过程

⁰r₁+⁰Q₁·(¹r₂+¹Q₂·(²r₃+²Q₃·³r_3C))＝⁰r_3C，

其等价为运动链的前向迭代过程

⁰r₁+^0|1r₂+^0|2r₃+^0|3r_3C＝⁰r_3C；

递归过程的物理含义表述为：系统处理零位时其中：l∈[1:3]；先将与杆件3固结的位矢平动⁰r₁，再转动⁰Q₁；接着平动¹r₂，再转动¹Q₂；最后，平动²r₃，再转动²Q₃；这是使与对齐的正向转移过程。在数值计算时，递归过程对内存访问的次数少，故其要比对应的迭代过程快得多。将式重新表示为

其中：

称及分别为及的齐次(Homogeneous)坐标；称为链节的转移矩阵(Transformation Matrix)，表示至l的螺旋运动。

【1】齐次逆变换

由式及式(1.55)得

将式(1.108)重新表示为

其中：

【2】齐次变换阵的串接性

证明：

证毕。

式(1.111)表明齐次变换阵具有串接性。

【3】缩放齐次变换的链符号

记体l的同名体为l′，即l＝l′。记由体l至其同名体l′缩放矢量为^lc_l′；其对应的缩放矩阵记为^lC_l′，称^lc_l′为^lC_l′的轴矢量；其中：

故有缩放变换

矢量表示是由矢量经^lC_l′缩放得到的。显然有

记缩放齐次变换阵为

故有

因为

故有

即

式(1.112)右侧链序为表明物理含义为：是“先平动—再转动—后缩放”的正向转移过程。

因

故有

即

式(1.113)右侧链序为表明物理含义为：是“先缩放—再转动—后平动”的逆向转移过程。

缩放齐次变换在3D系统中具有广泛应用。机器人中，体l′一般是一个异形杆件。在机械制图时，制图单位与机器人运动分析及导航控制时采用的单位可能不一致，或者机器人杆件自身具有三轴形变，常常需要将杆件l′上的任一点l′_s按其体系l′进行三轴缩放，缩放后的体为其同名体l。在机器视觉测量时，镜头实现了对场景的物点l′_s与像点l_s的变缩放变换，在机器视觉计算时也常常应用到缩放齐次变换。因此，缩放齐次变换是机器人运动链分析的基本运算方法之一。

【4】无穷远点的齐次坐标

若时，则有

由式(1.114)可知：无限远的矢量只有方向，而位置无法确定；齐次坐标构成的空间是四维的；但齐次坐标不是矢量，而是数组。

由式(1.114)可知：只有定位矢量才存在齐次变换，它反映了运动链节间存在运动的牵连；而对于自由矢量，例如角速度矢量及转动矢量等，不存在齐次变换；即自由矢量在运动链中不存在运动的牵连。

转动矢量与螺旋矩

记角速度角速度的叉乘矩阵也是二阶张量的定轴转动轴矢量为当是常矢量时，则有

定义

称为转动矢量，其中：φ是转动属性符。转动矢量/角矢量是自由矢量；即该矢量可自由平移。

有转动矢量的叉乘矩阵为

即是的轴矢量。

因故是标量，是自然坐标或关节坐标，表示转动的幅度或大小。是转动矢量与轴矢量的内积/投影即角度。转动矢量表示绕单位转动轴角度为的转动。

在运动链符号系统中，对于转动属性的描述采用转动矢量对于平动属性的描述采用平动矢量它们分别表征转动状态与平动状态。

转动矢量是绕一个固定轴的转动，又称定轴转动。机器人系统中转动副的运动量是以固定轴矢量或及角位置或来表示的，转动矢量或是转动最自然的表示形式；棱柱副运动量是以固定轴矢量或及线位置或来表示的，平动矢量或是平动自然的表示形式。

正向运动具有自我参考：对于转动而言，由初态绕轴转动角度后，至终态l；相应的转动矢量为对于平动而言，由初态沿轴平动后，至终态l；相应的平动矢量为对于同一个体而言，其运动次序通常为：先缩放，再转动，后平移。

自然坐标系、自然不变量及自然坐标是自然空间的自然表示：转动矢量与方向方向余弦矩阵等价，具有3D自然空间维度的不变性；自然轴不变量相对相邻的自然坐标系及l的坐标不变，具有自然坐标的不变性；自然轴不变量与自然坐标或无关，具有自然参考轴的不变性。

叉乘符运算的优先级低于投影运算的优先级。给定任一矢量的叉乘矩阵是二阶张量，即

则有

证明：由式(1.97)得

又

即是反对称阵。由式(1.95)得其中：n∈[x,y,z]，故有

故式(1.115)成立，叉乘矩阵的任一列是对应坐标轴的螺旋矩，叉乘矩阵具有螺旋距的不变性。

角速度的叉乘矩阵也是二阶张量，即

对求偏导数得

由式(1.121)可知是由位置矢量空间至平动速度空间的变换阵，是对的梯度。

由式(1.120)式(1.121)可知的意义在于：是角速度矢量^lω_k的二阶张量，满足二阶张量的坐标变换即相似变换；是位置矢量^lr_k至平动速度矢量的坐标变换阵。

根据运动链坐标变换运算有

-^lω_k＝^l|kω_l＝^lQ_k·^kω_l。>

由式(1.120)及式(1.122)得

故有

一方面，因是二阶张量，它是由^lω_k衍生得到的，它的两组基均为e_l。在进行相似变换时，如式(1.123)可表示为

因此，叉乘矩阵在运算时，通过等价指标可以清晰地表达与其它属性量的指标关系。由链符号的矢量运算关系，可知

因为叉乘矩阵表示的是叉乘运算，故有

叉乘或的物理含义是：先将杆件k固结的矢量投影至的径向，再转动1/4周，指向由^lω_k至的螺旋轴方向。。

矢量的二阶螺旋矩

【1】基本矢量关系

在力学中，矢量积又称为矩。给定矢量a，b，c，由线性代数可知

称(1.127)为“右侧优先双矢量积”公式，因为叉乘右侧优先(RHS Priority)计算。同时，有

称式(1.128)为“左侧优先双矢量积”公式，因为叉乘左侧优先(LHS Priority)计算。

矩的不变量主要包括矢量的矩不变量和二阶张量的矩不变量(MomentInvariant)。下面两个等式表示了二阶矩(Second Order Moment)公式：

式(1.129)表示三个矢量张成的棱柱体积等价关系，满足顺序轮换法则。

双矢量积(Double vector product)操作是多轴系统的基本操作，反映运动链中的广义作用力之间及运动状态之间的耦合；将它们转换为代数运算，是建立代数几何学(Algebraic Geometry)的一个重要环节，以方便后续运动学及动力学的分析。

【2】右侧优先的双矢量积操作

给定坐标矢量则有

证明：由式(1.130)，*代表任意点，得

因^kr_*是任意矢量，可得式(1.130)。

【3】左侧优先的双矢量积操作

给定坐标矢量则有

证明：由(1.127)，*代表任意点，故得

因^kr_*代表任意矢量，可得式(1.130)。

由式(1.131)得

即有

式(1.132)表明，双矢量积运算与外积运算的内在关系。

称式(1.130)及式(1.131)为二阶矩公式，因为它们是由两个矢量构成的矢量积。式(1.92)、式(1.130)及式(1.131)是将3D空间几何转化为3D空间操作代数的基本公式，它们既具有代数运算，又具有空间拓扑操作。因此，3D空间操作代数有分析代数及几何拓扑的双重优点，是以点积与叉积为基本运算的“保角(角度)”及“保距(距离、一阶及二阶距)”的系统。

考虑运动链用轴不变量及替换式(1.130)中的位置矢量分别得

用轴不变量及替换式(1.131)及式(1.132)中的位置矢量分别得

因为关节转动矢量、转动速度及加速度都是关于轴不变量的多重线性型，所以上面4个关于轴不变量的关系式在后续运动学及动力学分析中具有重要作用。习惯上，运动链^kl_l的矢量方程一般以运动链的根坐标系F^[k]为参考。

笛卡尔轴链运动学及问题

运动链符号系统为运动学及动力学分析奠基于三个基础：运动链的拓扑不变性，通过链指标清晰反映运动量及运动属性操作的作用关系。运动链的张量不变性反映运动属性的客观性。矩阵初等操作的不变性反映是二阶坐标张量在不同参考空间的等价性。

运动链分析包含两个过程：前向的运动传递过程及逆向的外力传递过程。以坐标轴表征的运动链系统则称为笛卡尔轴链(Cartesian axis chain)系统。三维矢量空间操作代数既可以用于笛卡尔轴链系统的分析，又可用于自然轴链系统的分析。笛卡尔轴链只是轴链的特殊情形，不具备自然轴链系统的性质。下面，先阐述笛卡尔轴链的应用；然后，讨论笛卡尔轴链存在的问题。

【1】D-H转动

请参照图19。给定运动副初始时坐标系与F^[l′]方向一致；先绕坐标轴转动角度至再绕坐标轴x_l′转动角度至F^[l′]，两次转动关系请参照图19。

为书写方便，在本书中约定：记D-H参数指标遵从父指标，与自然坐标系统下的参数遵从子指标不同。定义

由式(1.133)及式得

即有

显然，式(1.134)第3行不含运动参量这一特征在3R机械臂位置逆运动学计算中将得到应用。第3行表示的是基分量在基e_l′下的投影；坐标轴的转动不影响基分量在基e_l′下的投影。且有

即

显然，

【2】D-H齐次变换

对于运动链由式(1.135)得

即

式(1.136)表明：既包含运动参数φ_l又包含结构参数a_l及c_l。

对于运动链有

即

式(1.137)表明：与运动参量φ_l无关；这一特征同样在3R机械臂位置逆运动学计算中将得到应用。表示的是由F^[l′]看的位置，当坐标轴转动时，对位置无任何影响。

由式(1.137)得

由式(1.138)得

式(1.138)及(1.139)表明及的模具有不变性。

应用笛卡尔轴链分析D-H系变换关系，只因为D-H系自身是特殊的笛卡系。笛卡尔轴链适用范围很窄，它存在许多问题。下面，予以阐述。

任一刚体姿态可以通过绕三个笛卡尔坐标轴{x,y,z}的转动序列确定；其中任意相邻的两个坐标轴应是独立的，即不能出现共轴的情况。{x，y，z}排列{[m，n，p]|m，n，p∈{x，y，z}}共27种，其中：存在m＝n＝p的排列有3种；存在m＝n≠p及m≠n＝p的排列各6种。故三轴转动序列共有27-3-6-6＝12种。在12种转动序列中，仅有“1-2-3”序列与笛卡尔坐标轴[x，y，z]序列等价。笛卡尔坐标轴{x,y,z}序列仅保证姿态等价，不保证转动过程序列等价，笛卡尔轴链与运动链不是严格意义上的同构关系。

由参考系经转动至体系l的转动由绕三个转动轴矢量转动序列等价，且若^l1n_l2＝1^[y]，^l2n_l＝1^[x]，则为“3-2-1”转序；若^l1n_l2＝1^[x]，^l2n_l＝1^[z]，则为“3-1-3”转序。上述的两种转动序列是常用的；比如，用于移动机器人本体姿态的描述。

【1】全姿态角

全姿态角范围为(-π，π]，在姿态角求解时使用θ＝atan(y，x)非常方便，与atan(x)对应的重载函数atan(y，x)计算过程如下：

显然，有

atan(y,x)＝atan(cy,cx),if c＞0。 (1.141)

由式(1.140)可知，θ∈(-π，π]，x＝0为θ＝atan(y，x)的奇异点。由集合论可知，+∝及-∝也是数，这里的“奇异点”在理论上不成立。但由于计算机浮点位数有限，存在现实上的计算精度问题。在工程上，只要增加浮点位数，总能满足期望的精度要求，故“奇异点”在工程上亦不存在。将连续函数作用域上有限点出现奇异的情形称为“单点奇异”，故“单点奇异”总可以通过连续性补足，故仅影响计算误差，而并不导致无解，即函数的行为是确定的。

在姿态角计算时，应采用或计算；若应用及计算姿态角，则需要检查是否可以完整描述所有可能的姿态。

将连续函数作用域上连续区间出现奇异的情形称为“区间奇异”，由于不能通过连续性补足，在工程上导致无解，即函数的具有不确定性，在工程上导致系统行为无法控制或状态无法确定。

【2】“3-2-1”转动序列

考虑球副由参考系经“3-2-1”转动序列(Rotation order of“3-2-1”)至体系l运动链角序列记为三个转动轴矢量为^l1n_l2＝1^[y]，^l2n_l＝1^[x]。其中：l1及l2为中间坐标系。给定时，试求

解：由于正余弦计算的复杂度较高，需要通常先计算，后使用。将计算的正余弦表示为

则有

故有

常称为卡尔丹角，其极性由确定。不满足可加性，常称之为伪坐标。的任一元素是姿态角正余弦的多重线性表示，又称多重线性型。上述过程：已知姿态角，计算旋转变换阵；这是姿态正问题。

式(1.142)在本书中使用非常普遍，在以后的应用中，不再予以提示或说明。

若体轴x指向前方，轴y指向左侧，轴z指向上方，则的物理含义为：—偏航角，—俯仰角，—横滚角。该“3-2-1”姿态角常用于描述机器人、飞机及导弹的姿态。

由式(1.142)得

由式(1.144)可知，该姿态角范围需满足：否则，不能完整描述体l的全部姿态。初始时，体系l与参考系重合；当时，体l经“3-2”转序后，可以将体l上的任一方向与环境中的任一方向对齐；在完成方向对齐后，经转序“1”，当时，可将的任一径向矢量与的任一径向矢量对齐。

显然，是的逆运动序列；不仅需要改变转动的极性，而且需要颠倒转动的次序。以角度序列描述姿态时，仅表示起止状态的等价，不表示转动过程的等价。上述过程是：由旋转变换阵求解姿态角；这是姿态的逆问题。

【3】“3-1-3”转动序列

示例2.1请参照图20。图20为解耦机械手结构图。如图20所示，解耦机械手由第一转动副2001、第二转动副2002、第三转动副2003构成，並包含四根杆件串接构成；三个转动轴共交于一点O 2004，称之为“腕心”，且R₁轴2005与R₂轴2006正交，R₂轴2006与R₃轴2007正交。显然，对于解耦机械手而言，拾取点S>经转动至体系l的转动链三个转动轴矢量为角序列记为若^l1n_l2＝1^[x]，^l2n_l＝1^[z]，则为“3-1-3”转序。给定时，试求

解：

由式(1.145)得

由式(1.146)可知，可以描述全姿态。

用“3-1-3”角序列描述天体姿态时，称为进动角，为章动角，为自转角。

由于姿态序列存在12种，为每一个转动序列，列写各自的姿态逆解是非常烦琐的。对于更一般的情况，即给定确定绕三个转动轴矢量且^l1n_l2≠^l2n_l转动的角度。姿态逆的通解问题需要进一步解决。

笛卡尔轴链的偏速度问题

请再次参照图20，在此示例中，若将解耦机械手抓取工件的位置记为S，则有

对于式(1.147)而言，有

称式(1.148)为解耦机械手或球副的运动约束方程，反映的是空间距离约束的不变性。

转动副、球销副均是球副的特例。记S是球副中心，故对运动副存在约束方程：

其中：表示球副中心在系下的位置矢量，c为常数。

由式(1.149)得

显然，

其中：

称为雅克比矩阵，它反映的是速度与关节速度的关系。由式(1.151)知，即速度是关于关节速度的线性型。当然，关节速度不是矢量，因为各成员的参考不一致，且不满足可加性，故称之为关节速度。

将式(1.151)代入式(1.150)得

考虑球副轴矢量序列其中：^l1n_l2＝1^[y]，^l2n_l＝1^[z]；角序列记为显然，这是“1-2-3”的转动序列。则有

即

故有

由式(1.154)可知，角速度是关于关节速度的线性函数，即角速度是关节速度的线性型。显然，关节速度不是矢量。将称为偏角速度，并记其为即

由式(1.155)可知

尽管线速度或角速度是关节坐标的非线性函数，但线速度或角速度是关节速度的线性函数。

示例2.2给定轴序列A＝(i,c1,c2,c3,c4,c5,c]，父轴序列轴类型序列记为K＝(F,R,R,R,P,P,P]，关节坐标序列记为q_(i,c]＝(φ_c1,φ_c2,φ_c3,r_c4,r_c5,r_c]；故该运动链记为ⁱl_c＝(i,c1,c2,c3,c4,c5,c]。且有

ⁱn_c1＝^c5n_c＝1^[z]，^c1n_c2＝^c4n_c5＝1^[y]，^c2n_c3＝^c3n_c4＝1^[x]。>

该运动链表达的是：先执行“3-2-1”转动，再执行“1-2-3”的平动。则有

由式(1.157)～(1.160)，得

显然，

故有

由式(1.163)得

由式(1.164)得

其中：

对于精密机电系统而言，由于存在机加工及装配的误差，正交的运动轴或测量轴是不存在的。例如：惯性单元的三个体轴方向安装有加速度计及角速度陀螺，分别检测三个轴向的平动加速度及转动角速度；有时，出于可靠性考虑，通常斜装一只加速度计及一只速度陀螺，用作备份。工程上，斜装运动轴及测量轴的情况是普遍存在的。因此，需要进一步研究螺旋轴链系统的运动学与动力学问题。

给定运动链且k∈ⁱl_n，由式(1.166)得

同样，由式(1.167)得

式(1.167)及式(1.168)的偏速度在机器人运动学及动力学分析中具有非常重要的地位，偏速度问题需要进一步解决。

由式(1.147)得

式(1.169)需要计算由式(1.169)可知，是的函数，且是时间t的函数；故直接计算是非常麻烦的事。旋转变换阵的求导问题需要进一步解决。

示例2.3：相机体系c相对巡视器本体系r的安装关系由两系坐标轴间的夹角确定：其中：表示由轴x_r至轴x_c的角度，其它亦然。

求^rQ_c。

解：相机坐标轴x在巡视器体系下的投影为相机坐标轴y在巡视器体系下的投影为相机坐标轴z在巡视器体系下的投影为故有

解毕。

此示例应用方向余弦计算旋转变换阵，在原理上是正确的。但在工程上存在一个重要的缺点：由于九个角度测量存在误差，致使旋转变换阵的“正交归一”约束被破坏。示例如下：

示例2.4：续示例2.3，经工程测量得

由式计算得

由计算结果可知^rQ_c病态的，精度仅有6位。

应用式(1.144)或式(1.146)计算姿态角在理论上是成立的；其前提是：旋转变换阵必须满足“正交归一”约束，当这一约束不能完成满足时，的计算误差可能较大。对于病态式(1.144)及式(1.146)未充分利用各分量，导致姿态角序列精度比余弦角的测量精度差。

除工程测量误差外，由于计算机存在数字截断误差，也导致旋转变换阵的病态。对于运动链^k1_j，由于存在一定的病态，导致^kQ_j误差不断累积；但在实际应用时，需要对进行“正交归一化”。对式(1.171)进行“正交归一化”结果如下：

“正交归一化”处理后的精度达到8位。因此，通过方向余弦计算旋转变换阵时，一方面需要提高测量精度，另一方面需要对旋转变换阵进行“正交归一化”处理。否则，将导致运动链计算精度逐级衰减。如何对旋转变换阵进行“正交归一化”是需要进一步解决的问题。

极性参考与线性约束求解问题

在机器人运动学及动力学分析时，关心的是运动量间的相互关系，即关心的是张量坐标阵列的相互关系；而不必关心参考基间的相互关系。张量坐标阵列可以通过矩阵表示，包含标量、列矢量或行矢量、矩阵。高阶张量的坐标阵列可以表示为矩阵的向量、矩阵的矩阵。矩阵是信息有序排列方式，不仅适应人们对于事物的认知方式，也适应现代数值计算机进行信息处理的内在机理。

【1】坐标轴序的初等变换

一个立方体的6个面单位法向可以确定6个不同的参考轴；可以建立120种坐标系；任两个坐标系存在等价关系。由于极性定义不同，经常需要对以不同极性定义的坐标进行转换。

示例2.5：续示例2.4，定义与坐标系r、c分别对应的坐标系r′、c′，相互关系如图21a、图21b所示，求^rQ_c。

解法1，由坐标系的方向余弦关系得

解法2，先由^rQ_c初等变换列变换，即交换x_c及y_c、z_c取反，得^rQ_c'

再由^rQ_c′初等变换行变换，即y_r取反、z_r取反，得^r′Q_c′。

解毕。

由示例2.4求解过程可知，通过“行交换”、“列交换”、“行取反”、“列取反”的初等变换本质上是改变了参考基的次序与极性，但不改变矩阵表示的本征运动关系。因此，对不同序的坐标系定义，它们的变换关系可以通过“行交换”、“列交换”、“行取反”、“列取反”的初等变换操作即确定。

【2】坐标变换的初等变换

由式可知^i|kr_l＝ⁱQ_k·^kr_l，即矩阵右乘变换

式(1.173)等价为

对(1.174)进行“初等行变换操作”。例如：交换式(1.174)中x_i行及y_i行，对z_i行乘常数c_i，其中：得

显然式(1.174)与式(1.175)是等价的。将ⁱT_k称为ⁱQ_k的“初等行变换矩阵”。关系阵ⁱQ_k及ⁱT_k的“初等行变换操作”与方程求解的输入^i|kr_l及输出^kr_l的成员及成员的排序次序是无关的或者说独立的，即不影响求解的结果与解的排列次序。

对于式即矩阵左乘变换存在初等列变换及存在“初等列变换矩阵”。

由线性代数理论可知，通过初等行变换或初等列变换可以使ⁿQ_k变换为单位阵1，则相应有从而有

同时，初等变换是矩阵的逆阵、三角化、对角化处理的基本操作。其中，枢轴操作是矩阵操作中非常重要的操作，在机器人动力学计算中具有广泛的应用。

线性方程是非线性方程的基础，复数空间奠基于实数空间；正确理解复数及非线性空间的参考基是保证拓扑不变性及度量不变性的基础。由第3章可知，轴不变量是实空间及复空间(Complex Space)运动链的基元，是解决高自由度及高维度空间问题的基石。

给定运动链由式计算得到。由式(1.59)及式得

由式(1.176)完成运动链ⁱl_n的姿态计算。一方面，由于工程上存在病态，导致ⁱQ_n病态加剧；另一方面，由式计算不通用，仅适用于笛卡尔轴链，而通常的转动轴并不与坐标轴一致。

由式(1.46)计算及由式(1.59)及式(1.116)得

由式(1.58)及式得

由式(1.178)完成运动链的位置计算。

由式(1.178)得

由式(1.179)得

显然，相对转动速度及相对平动速度是矢量；相对转动加速度及相对平动加速度也是矢量。

定义

由式(1.182)知：求导符优先级高于投影符称该求导运算为相对求导，并没有考虑基矢量相对惯性基矢量e_i的运动带来的影响。

由式(1.182)得

由式(1.183)及式(1.184)分别完成运动链的相对平动速度及相对平动加速度的计算；显然，它们并未考虑运动链各杆件的牵连速度。

由式(1.177)得

由式(1.185)及式(1.186)分别完成运动链的相对转动速度及相对转动加速度计算；显然，它们并未考虑运动链各杆件的牵连加速度。

尽管相对速度及加速度具有理论上的意义，但不表示运动链的真实速度及加速度。需要考虑运动链各杆件间的牵连效应，才能正确表征系统的速度、加速度、能量等基本属性。

后续內容，针对精密多轴系统的需求，以运动链符号演算系统为基础，建立基于轴不变量的多轴系统理论，主要考虑的因素有：

【1】测量的精确性问题，需要考虑多轴系统加工、装配误差，最大限度地降低测量误差及计算误差。在自然关节空间下，通过精密的光学设备测量固定轴不变量表征的结构参数。一方面，包含了系统加工及装配的误差；另一方面，考虑了精度光学设备测量空间位置点的精确性的特点，具有工程可实现性。固定轴不变量是多轴系统的自然不变量，易于工程测量。

【2】计算的实时性与精确性问题，需要考虑高自由度多轴系统带来的计算复杂性。一方面，需要建立迭代式的运动学方程及动力学方程；另一方面，需要建立与系统自由度一致的运动属性描述方法及最小的属性量的操作。以运动链符号演算系统为基础，建立的基于自然不变量(包含：固定轴不变量自然坐标及)的多轴系统理论，可以建立基于轴不变量的迭代式的运动学方程与动力学方程，可以保障多轴系统计算的实时性与精确性。

【3】工程开发的效率问题，高自由度多轴系统不仅带来计算复杂性，也带来工程实现的复杂性。一方面，需要为工程技术人员提供精确、简洁、结构化的符号语言，既包含属性量的准确描述，又包含属性量间的作用关系，还需要考虑现代数值计算机的矩阵运算特点；另一方面，应用结构化的符号系统，提高工程实现的效率，既需要直接以结构化的运动学与动力学符号方程替代编程实现所需的伪代码，又需要应用计算机软件实现运动学与动力学的建模与分析功能。运动链符号演算系统是满足上述需求的结构化的符号语言。基于轴不变量的运动学与动力学方程，具有伪代码的功能及迭代的计算过程，易于工程实现。

【4】不同理论方法的兼容性问题，在现有多体系统理论中，存在不同的理论分支，具有各自的应用特点，需要统一不同的理论方法，既是理论研究的需要，又是系统实现的实时性与精确性的需要。因此，以运动链符号演算系统为基础，建立多轴系统的公理化理论体系。以螺旋轴为基元的螺旋轴链系统可以统一3D及6D矢量空间、复数空间、四元数及双四元数空间的理论。

基于轴不变量的多轴系统内涵在于：

【1】以运动链符号演算系统为基础，轴不变量为核心，建立多轴系统建模与控制理论；

【2】多轴系统是自然坐标轴系统，它以自然坐标轴作为系统的参考基元。

【3】多轴系统模型是具有链符号系统的，以轴不变量(自然参考轴)及自然坐标表征的代数方程；多轴系统理论是研究自然轴空间的、具有树链拓扑操作的代数系统。

多轴系统建模与控制理论研究思路：

【1】先分析轴不变量基本属性，再研究以自然轴为参考的3D空间操作代数；

【2】进一步，研究空间点及质点，再研究刚体；

【3】进而，研究链节运动学关系，再研究运动链运动学关系；

【4】最后，研究树链系统的动力学，建立基于轴不变量的多轴系统建模理论。

第二部分 基于轴不变量的多轴系统正运动学Equation Chapter 2 Section 1

【1】由第一部分可知，运动链具有以下基本公理：

【1.1】ⁱl_n具有半开属性，即

【1.2】ⁱl_n存在一个空链或平凡链ⁱk_i，即

ⁱk_i∈ⁱl_n,|ⁱk_i|＝0；>

【1.3】ⁱl_n运动链具有串接性(可加性或可积性)，即

ⁱl_n＝ⁱl_l+^ll_n，>

ⁱl_n＝ⁱl_l·^ll_n；>

【1.4】ⁱl_n具有可逆性，即

^ll_n＝-ⁿl_l。>

【2】对于轴链有以下基本结论：

【3】若k，l∈A，存在以下二阶矩关系：

【4】左序叉乘与转置的关系

证明：因

故式(2.14)成立。

【5】空间操作代数

尽管多体动力学已得到广泛的研究，但缺乏运动链符号系统，也未建立基于轴不变量的空间代数系统。提出的空间操作代数与传统的空间算子代数具有下列不同点：

【5.1】操作是指空间中的基本动作或计算机执行的运算；操作既包含地址访问、矩阵的行列置换、拓扑关系的访问，也包含函数的计算；故操作是算子概念的推广；多轴系统运动学与动力学既与系统拓扑相关，又需要通过计算机实现，自然需要建立与之相应的操作代数；

【5.2】一方面，空间操作或计算机操作更直接，易于理解；通过系统操作执行系统状态的变更，在新的变更状态下，再执行相应的操作，完成系统的演化与计算；另一方面，易于计算机软件实现，计算机系统自身就是基于一组基本操作的计算系统；包含地址访问、枢轴操作、LU及LDL^T分解等的矩阵运算是计算机数值计算的基础；

【5.3】空间操作代数主体上通过空间或计算机操作序列表征空间运动关系，链序是空间操作的基本特征；空间运算既需要保证序不变性(拓扑不变性)、张量不变性(度量不变性)及对偶性，又需要保证测量及数值计算的精确性与实时性，它们是空间操作代数的基本特征；

【5.4】空间操作代数需要确定空间操作的基元，以保证复杂空间操作的效率；自然参考轴及以自然坐标系为基础的3D关节空间轴不变量是空间操作的基元；通过一组自然参考轴可以建立笛卡尔直角坐标系，D-H系及其它所需的坐标系系统；通过3D关节空间轴不变量及自然坐标实现体系的参数化，保证工程测量的精确性及空间参考的灵活性。

总之，3D空间操作代数是以运动链符号系统为基础，以符号演算、动作及矩阵操作为主体的3D空间正运动学计算系统，有别于传统的双矢量6D空间算子系统。

基于轴不变量的3D矢量空间操作代数

以固定轴不变量表征系统的结构参量，结构参数间的代数运算结果仍为结构参量；以自然坐标即关节变量表征系统的关节变量，关节变量间的代数运算结果仍为关节变量。由系统结构参量构成的3D矢量，称为结构矢量(Vector of Structural Parameters)；由关节变量构成的标量称为运动标量(Motion Scalars)。基于轴不变量的3D矢量空间操作代数系统既是以轴不变量为核心的3D空间操作代数系统，又是关于结构矢量与关节变量(标量)的二阶多项式系统。

运动副的坐标轴矢量表示运动轴的单位方向，是自然的参考轴，其单位方向由其成员确定；坐标轴矢量具有如下不变性：

由式(2.15)可知，坐标轴矢量是全序的矢量，即其连接次序是双向的，负号-不能改变连接次序，但可改变的坐标分量，故轴矢量又称为轴不变量。轴矢量作为杆件及杆件Ω_l的公共参考轴，在系统演算(Calculus)前确定；在系统演算过程中，不能被人为更改；否则会导致参考不一致。

轴不变量可以方便地确定零位轴系，具有优良的空间操作性能(Spatialoperating performance)，应用轴不变量可以有效地解决许多理论及工程问题。

本章以轴链为基础，建立基于轴不变量的多轴系统运动学理论；从而，根据多轴系统的轴数灵活地建立多轴系统的运动学及动力学方程。

基于轴不变量的零位轴系

请参照图22。图22为径向投影及自然零位图，如图22所示，给定运动副的轴矢量2201及具有单位长度的零位(Zero position)矢量2202，且点S2203位于单位球面上；称轴矢量2201向零位矢量2202方向转动90转后的单位矢量为自然零轴(Naturalzero axis)矢量，记为由轴矢量2201及自然零轴按右手系可以确定自然径向轴(Radial axis)矢量将系统初始时刻的自然零轴及径向轴分别记为初始时刻的自然零轴2206及初始时刻的径向轴2205，并分别称为系统零位轴矢量及系统零位径向轴矢量。

则有：零位矢量对轴矢量投影标量即坐标为零位矢量对轴矢量投影矢量为零位矢量对零位轴矢量投影矢量为故得零位矢量的径向投影变换及系统零位投影变换分别为

轴矢量对的矩矢量(Moment Vector)为零位矢量表达为

【1】若给定零位矢量则系统零位轴矢量系统零位径向轴矢量及轴矢量构成零位轴系，该轴系由系统零位轴矢量及轴矢量唯一确定；一般情况下，该轴与自然坐标系F^[l]方向不一致。

【2】给定径向约束力因其与轴矢量正交，则有

称为自然正交补(Decoupled Natural OrthogonalComplement)矩阵。

【3】零位投影变换具有对称性，即

证明：由式(2.17)得

证毕。

轴不变量可以方便地实现矢量的镜像。请参照图23。图23为矢量的镜像变换图。如图23所示，点S′(2302)是点S(2301)的镜像，矢量镜面轴矢量记为点S″(2303)是点S(2301)的逆像。且有点S′(2302)至点S(2301)记为

易得

且有

记

称为像变换，为逆像变换。显然，有

即是自逆的矩阵：

应用镜像变换可以解决光的反射、折射、映像等诸多问题，在光学系统中具有广泛的应用。

示例2.1：请参照图24，图24为正射镜像图。3D世界系为正射镜像即是由镜面法向观察场景的像，其中：则有正射镜像变换

证明：由图24可知由式可知：

证毕。

下面探讨轴不变量的叉乘矩阵幂零(Nilpotent)特性，它们是后续研究的基础。

【1】轴矢量叉乘矩阵的幂

给定运动副的轴矢量则有轴矢量的二阶幂零特性：

证明：是单位矢量，利用数学归纳法证明如下。

当p＝1时，由式(2.32)可知式(2.25)成立。由式(2.26)得

当p＝k时，假设式(2.25)和式(2.26)成立，即

则当p＝k+1时，由式(2.28)及(2.29)得

故式(2.25)及式(2.26)成立。

因此，称为一阶径向变换或一阶矩变换，具有反对称性；相应地，分别称及为二阶及三阶径向变换；如式(2.28)及式(2.29)所示，具有周期性。

因是二阶坐标张量，得

由式(2.30)得

对于轴不变量由式(2.17)和式(2.11)得

由式(2.11)得

【2】轴四元数及其性质

轴不变量以自然坐标系为参考，转动角以系统零位轴矢量为参考，它们构成4D空间。下面，分别定义轴不变量的四元数(称为轴四元数)左叉乘矩阵及右叉乘矩阵

由式(2.28)，(2.29)及式(2.34)得

给定轴不变量并记

则有

证明：由单位矢量得当p＝1时由式(2.37)可知

即

由式(2.39)得

即

当p＝m时，式(2.37)及式(2.38)成立，即

则p＝m+1时，由式(2.39)及式(2.41)得

由式(2.36)及式(2.42)得

由数学归纳法可知，式(2.37)及式(2.38)成立。

由式(2.37)得

由式(2.38)得

式(2.43)及式(2.44)表明角速度叉乘矩阵的幂具有周期性。

【3】转动矢量

应用轴不变量，可以将转动表达为转动矢量。下面，对之进行阐述。

称体l由初始姿态绕单位轴矢量转动角度后，至终止姿态l的过程为定轴转动(Fixed-axis Rotation)。显然，相对坐标系的单位轴矢量是常矢量，即定轴。单位转动轴确定了定轴转动的方向，角度确定了该定轴转动的幅度或大小。故定义转动矢量(Rotation Vector)或罗德里格参数(Rodrigues Parameters)如下：

在自然坐标系统下，故有

其中：

因故有

因是二阶坐标张量，故有

基于轴不变量的定轴转动

请参照图25a。图25a为矢量做定轴转动前矢量图。如图25a所示，给定轴矢量及与轴矢量固结的单位矢量在转动前，对于单位矢量对系统零位轴的投影矢量为对系统径向轴(2505)的矩矢量为径向矢量为

请参照图25b。图25b为图25a做定轴转动后矢量图。由式(2.15)可知，轴矢量相对于杆件及Ω_l或自然坐标系及F^[l]是固定不变的，故称该转动为定轴转动。单位矢量绕轴转动角度后，转动后的零位矢量对系统零位轴的投影矢量为转动后的零位矢量对系统径向轴的矩矢量为轴向分量为故得具有链指标的Rodrigues矢量方程

因矢量是任意的且得具有链指标的Rodrigues转动方程

若由式(2.51)，得若即坐标系与F^[l]的方向一致，由式(2.51)可知：反对称部分必有因此，系统零位是自然坐标系与F^[l]重合的充分必要条件，即初始时刻的自然坐标系方向一致是系统零位定义的前提条件。利用自然坐标系可以很方便地分析MAS系统运动学和动力学。

显然：是自然轴上的坐标，是系统零位轴上的坐标。固结于体自然坐标系F^[l]的单位矢量与一一映射，即等价。自然零位轴及自然坐标轴分别是四维复数空间的实轴及三个虚轴。式(2.51)中，右式前两项是关于角度的对称矩阵，故有最后一项是关于角度的反对称矩阵，故有因此，由及唯一确定，即由矢量及标量唯一确定，这是后续诸多问题讨论的基础。

式(2.51)中任意项的链序保持一致。由式(2.51)得

同时，由式(2.51)得

即

式(2.53)表明：一方面，在相邻自然坐标系下，相邻杆件l和的轴矢量具有相同的坐标；另一方面，轴矢量由原点指向的外侧，轴矢量由O_l指向O_l外侧，它们具有相同的坐标，即轴不变量具有全序关系，它的正序与逆序无区别。因此

若由式(2.53)得将之代入式(2.51)得

即有

由式(2.52)及式(2.55)可知：对轴矢量数值取负与对关节角取逆序都可得到的逆。轴矢量是自由矢量，其方向总由坐标系原点指向的外侧；显然，数值取负与拓扑(连接)次序取反是两个不同的概念。在多轴系统理论中，因轴矢量用作关节执行器及传感器的参考轴，是系统参考规范，故式恒成立，即轴矢量是不变量。

定轴转动的幂零多项式

由式(2.51)得

易得

考虑的泰勒展开

将式(2.36)带入式(2.57)得

即

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

示例2.2：

当即故

则有

示例2.3：

当时，即故

由式(2.59)得

示例2.4：

当时，即故

由式(2.59)得

示例2.5：已知求

解：

由式(2.60)得

解毕。

定轴转动的指数

因DCM矩阵一定存在为1的特征值，其对应特征向量是由Cayley-Hamilton理论，式(2.50)可表示为

式(2.60)表示的是一般转动规律，指数e是自然界普适常量。

由式(2.60)及式(2.6)得

故

对于定轴转动，由式(2.60)可得性质具有非常重要的作用，需要分析其基本性质。

由式(2.63)得

由式(2.54)及式(2.64)得

由式(2.60)得

即有

由式(2.63)至式(2.66)可知，应用式(2.60)求自然角度非常方便，具有优良的空间操作性能；式(2.59)比式(2.60)计算复杂度低，更适合于数值计算。

基于轴不变量的Cayley变换

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

由式(2.67)得

定义

故有

【1】定轴转动的Cayley正变换

由式(2.69)必有

证明：由式(2.59)及(2.69)得

因

将式(2.73)代入式(2.72)，

故有

又

故有

由式(2.74)及式(2.75)得(2.71)成立。

由式(2.71)得

故为正交旋转变换阵。

于1846年Cayley表达了没有链指标的Cayley变换。称式(2.76)中的τ_l为Cayley参数，其含义如图26所示，它是切向矢量与径向矢量的正切。且有

由式(2.76)可知，与径向矢量及切向矢量是线性关系，称为“Rodrigues线性不变量”。通常称即为Rodrigues或Gibbs矢量，而将称为修改的Rodrigues参数(MRPs)。

【2】Cayley逆变换

由式(2.71)得

证明：由式(2.26)、(2.59)及式(2.73)得

故有

另一方面，

故有

由式(2.78)及式(2.79)得式(2.77)成立。

证毕。

由式(2.77)可知，Gibbs矢量DCM矩阵转动矢量一一映射，即

因此，轴不变量叉乘矩阵的性质具有非常重要的作用，需要分析其基本性质。

由式(2.77)得：

证明：由式(2.77)得

因为即故

因是任意的，有故式(2.81)得证。式(2.81)表明：是二阶张量，同时具有反对称性。称式(2.77)为“Cayley逆变换”。

因由式(2.81)得

将式(2.82)与对比，可知：轴矢量是不变量，即正因轴矢量具有不依赖于相邻参考系的不变性，故轴矢量称为轴不变量。

基于轴不变量的3D矢量位姿方程

下面，阐述3D矢量位姿定理，并予以证明。

定理2.1给定运动链ⁱl_n，则有基于轴不变量的3D矢量姿态方程

其中：

证明：由式(2.8)及式(2.59)得则是及的多重线性型，其中：l∈ⁱl_k。式(2.59)可表示为。

称(2.85)为改进的Cayley变换。即有

给定运动链由式(2.6)及式(2.85)得式。

式是关于的n维2阶多项式方程。将式(2.86)代入式(2.8)得

即得式。

式(2.83)及式(2.84)表明：姿态及位置矢量是关于τ_k的6个“n维2阶”多项式方程。式(2.83)及式(2.84)是关于结构矢量及关节变量的矢量方程，称定理2.1为3D矢量位姿定理。式(2.84)所示的位置逆问题就是当给定期望位置时如何求解该多项式方程的关节变量τ_l及的问题，其中：l∈ⁱl_n。

式(2.83)及式(2.84)表明：因相关的结构矢量可以事先计算，且可以表示为逆向递归过程，具有线性的计算复杂度，故可以带来计算速度的提升。又因在结构参数归一化后，ⁱQ_n的“正交归一”性由两个正交的矩阵即及得到保证，且与τ_l无关，其中：l∈ⁱl_n，故式(2.83)及式(2.84)的计算精度不会因为数字截断误差而累积。从而，保证了矢量位姿方程的计算精度。

因此，基于轴不变量的3D矢量位姿方程不仅方程数与3D空间的位姿维度相等，而且具有计算速度及计算精度的优势。表明Gibbs矢量可以表示姿态。

示例2.6：请再度参照图20，对于解耦机械手而言，拾取点S(2008)位于以腕心为球心的球面上。由参考系经转动至体系l的转动链转动轴链为角序列记为

故有转动矢量

因定轴转动的轴矢量序列是自然不变量，故由式(2.87)得角速度得

由式(2.88)得雅克比矩阵即偏速度表示为

由式(2.89)得出的角速度矢量的雅克比矩阵可知，它仅与运动副的结构参量即轴矢量相关，而与运动参量无关。

由式(2.63)得

由式(2.31)及式(2.90)得旋转变换阵的偏速度

由式(2.91)得

式(2.92)可知：旋转变换阵的偏速度就是角速度的偏速度。显然，以自然不变量表达的转动可以解决偏速度的计算问题。

由本节可知，轴不变量具有优良的空间操作性能，给转动分析与计算带来了极大方便。因为轴不变量是转动及平动的本征量。正因如此，需要建立基于轴不变量的多轴系统运动学及动力学理论，以揭示多轴系统内在的规律。

基于轴不变量的四元数演算

Rodrigues四元数

定轴转动通过轴不变量及转动角度唯一确定，即通过转动矢量唯一表示。将转动矢量的变体形式称为“Rodrigues四元数(Quaternion)”，亦称“Rodrigues参数”。四元数意为四个数，前三个是矢量，最后一个是标量。显然，“Rodrigues四元数”不满足可加性，故“Rodrigues四元数”是四维数组。

将“Rodrigues四元数”表示为

并记其中：称为“Rodrigues四元数”矢部，称为“Rodrigues四元数”实部或标部。显然，有即模为1。式(2.93)是美式“Rodrigues四元数”。

由式(2.93)得

由式(2.94)可知：不能覆盖一周，故与不是一一映射的关系。式(2.94)的角度范围与文献[2]不同，该文献中的转动轴方向是可重定义的；本文中的轴矢量是不变量，它作为运动链的自然参考轴，方向是先于系统分析确定的，之后不可重新定义。

欧式“Rodrigues四元数”习惯表示为

由式(2.93)知

为的逆四元数，即共轭四元数；显然，是转动的逆过程。给定一个Rodrigues四元数时，只能确定定轴转动的角度范围为[0，π]。故称定轴转动的Rodrigues四元数为有限转动四元数。注意，为的负/反四元数，且有

若给定未归一化的显然，则由式(2.51)得对应的旋转变换阵

对于规范Rodrigues四元数有及由式(2.99)得

即

由式(2.101)及得

由式(2.102)及式(2.103)计算时，充分应用了矩阵每一元素，对于病态矩阵具有鲁棒能力。

定轴转动是树形运动链系统的基本问题，由于角度测量噪声及计算机有限字长的截断误差致使旋转变换阵的正交约束被破坏，经树链旋转变换造成误差放大，难以满足工程需求。需要通过Rodrigues四元数来表征旋转变换阵，保证了旋转变换阵的正交归一化。应用基于轴不变量的转动可以降低测量及数字截断误差。

四维空间复数

有限转动四元数的转动角度范围为[0，π)，它有下面基本特点：

【1】对于定轴转动，有限转动四元数可以唯一确定旋转变换阵；反之，则不成立。即

【2】有限转动四元数及有如下关系：前者是标量，称为标量部分；后者是转动轴矢量，称为矢量部分。

对于定轴转动的逆问题，由旋转变换阵计算有限转动四元数时有一个特别重要的约束条件：而机器人或航天器或其机构在定轴转动时常常要求在[0，2π)内实现连续转动，显然不能满足要求，这正是该四元数命名为有限转动四元数的原因。对于定轴转动的逆问题，有限转动四元数与对应的旋转变换阵并不完全等价，这反映了有限转动四元数的局限性。

尽管如此，因为通过有限转动四元数可以表示旋转变换阵，且有限转动四元数表示定轴转动是自然的，所以在表示定轴转动时常常用它作为人机交互的接口来计算旋转变换阵。因为在以自然坐标系为基础的定轴转动中，通过转动矢量、Rodrigues四元数可以确定相邻两个坐标系的关系；此时，除根坐标系外，其它体系不必定义；即在多轴系统中，只需要关注底座坐标基。在机器人或航天器及其机构规划与控制时需要通过欧拉四元数表示，欧拉四元数与对应的旋转变换阵完全等价。

2D转动是3D转动的特例。请再度参照图25a、图25b。以转动的零位平面为参考，若记则即不考虑轴矢量由式(2.32)可知：故有i²＝-1。因此，2D平面的复数基分量为[i，1]。显然，i是纯单位虚数，具有实在的物理含义。由式(2.51)及(2.60)得被誉为“上帝方程(God’s>

由式(2.104)得由平面复数或2D复数可知，复数的纯虚数轴与实数轴正交，复数乘积运算表示的对应矢量的转动。在2D空间下，应用复数来解决转动问题是非常简单的。2D复数解决的是1DOF的转动问题。因此，进一步研究四维复数，以解决3DOF的转动问题。

由于转动轴矢量是自然不变量，对于转动链而言，转动轴矢量视为参考轴，在工程上，仅需要确定世界系或惯性系F^[i]；多轴系统处于零位时，即该参考系统为自然坐标系统。笛卡尔直角坐标系是理想的坐标系统，它应该在理想的情况下进行应用，即应用第2.5.4节的关节坐标及自然坐标系，以固定轴矢量为参考，建立通用的运动学方程及动力学方程。在自然坐标系统下，只需定义世界系或惯性系统即根杆件的体系；其它杆件的体系只发生在理想的概念上，因为它们可以由固定轴矢量及关节坐标确定。因此，仅需定义世界系或惯性系，树链系统共用一组基，故有自然坐标系的公共参考基i，

i＝[i_x,i_y,i_z]。>

记“*”为复数乘，故有

i_x*i_y＝i_z,i_y*i_z＝i_x,i_z*i_x＝i_y。>

由式(2.106)定义4D复数空间的单位基下面的点表示新增一个独立的维度。

且i满足

其中：3D复数乘“*”是在式(2.107)约束下的满足式(2.106)3D矢量乘“*”运算，即

因此

1+i_x*i_x＝0,1+i_y*i_y＝0,1+i_z*i_z＝0。>

因i_x、i_y及i_z是三个独立的符号，故视为三个纯虚数；由式(2.109)3D虚数i的三个分量满足：

即相关的纯虚数复数乘与3D笛卡尔空间点积一一映射。由上可知，将坐标基[i_x，i_y，i_z]增加一个维度并引入约束(2.108)后，仍具有三个独立的维度，是一个独立的3D实空间。显然，4D复数空间与3D笛卡尔空间同构即等价。由式(2.107)得

与式(2.69)相似，定义

式(2.111)和式(2.112)表明4D复数空间具有矩阵不变性和内积不变性。与式(2.70)类似得

由式(2.107)得：故有

i²＝-1。>

显然，3D笛卡尔空间是新的4D复数空间的子空间，复数乘既具有式(2.106)矢量乘运算又具有式(2.110)、(2.114)代数乘运算。很自然地，可以应用4D复数空间规律研究3D笛卡尔空间的规律。如同2D复数，在3D空间下姿态表示与运算是复杂的，但在4D复数空间下它们具有简单的表示及运算，这是破解姿态计算难题的关键。

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

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

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

记4D复数为且有任一常数c，具有如下复数加“+”、数乘“·”及共轭及复数“*”运算：

将式(2.116)写成数组形式：

称式(2.118)为四元数的代数加公式。将式(2.118)写成数组形式：

称式(2.119)为四元数的标量乘公式。下面，分析复数乘“*”的计算规律。

即

将式(2.120)写成伪坐标(Pseudo-coordinates)或数组形式，

即

另一方面，

故式(2.121)中的复数乘“*”可以转化为数乘“·”运算，即有

故定义

称为四元数的叉乘(共轭)矩阵。由式(2.122)易得

由式(2.124)及上式得

式(2.125)及式(2.126)作用在于：四元数的乘法运算可用四元数的共轭矩阵运算替换。与矢量叉乘运算相似，四元数乘可应用相应的共轭矩阵替代；称式(2.126)为4D复数乘公式。

【1】欧拉四元数定义

定义

其中：称为Euler-Rodrigues四元数或欧拉四元数；显然，它是模为1的四元数，又称为规范四元数。基于Rodrigues四元数，Euler首次采用了半角表示的Rodrigues四元数。式(2.127)是美式欧拉四元数表示法；与Rodrigues四元数一样，欧式欧拉四元数表示为

在本书中，仅使用式(2.127)美式欧拉四元数表示法。

【2】由欧拉四元数表示旋转变换阵

由式(2.51)可知

因此，欧拉四元数表示了定轴转动，并确定了旋转变换阵考虑式(2.25)得

由式(2.45)知由式(2.48)知式(2.93)及式(2.127)关系为

由(2.129)式可知旋转变换阵为

显然，有

由式(2.127)及式(2.93)可知，欧拉四元数与Rodrigues四元数区别在于：前者是而后者是而式(2.132)计算复杂度相对式(2.59)要高一些。

【3】由旋转变换阵计算欧拉四元数

下面，讨论旋转变换阵求四元数的问题。由式(2.131)可知

由式(2.134)得

即

将式(2.135)代入式(2.137)m∈{1，2，3，4}，m≠n。式(2.133)的角度范围与文献不同，该文献中的转动轴方向是可重定义的；而本文中的轴矢量是不变量，它作为运动链的自然参考轴，方向是先于系统分析确定的，在以后的运算过程中不应该对之重新定义。可以完整地覆盖一周，欧拉四元数与姿态具有一一映射的关系。

由式(2.134)及式(2.137)计算时，充分应用了矩阵元素的信息，它们由及确定。与笛卡尔坐标轴链的姿态计算相比，欧拉四元数对病态旋转变换矩阵具有更强的鲁棒能力。由式(2.132)可知，与分别描述的是旋转变换阵及由式(2.133)及式(2.138)唯一确定及轴不变量即有将式(2.128)重新表示为

将[0，2π)的姿态角称为最短路径全姿态角。欧拉四元数能区分最短路径全姿态角，而Rodrigues四元数只能区分[0，π)内的姿态角。

【4】四元数的逆

由式(2.130)及式(2.132)可知：

故有即

由式(2.130)知它是的反或逆，且有

注意：单位四元数的共轭四元数即有因故共轭四元数表示的是的逆转动。由式(2.141)可知及等价。

由式(2.125)及式(2.126)可知，四元数乘法运算可用其共轭矩阵运算替代，故有

其中：由式(2.125)得

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

由式(2.143)及式(2.67)，得

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

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

由式(2.145)得

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

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

因为空间中独立的三个点可以唯一确定转动，转动是平动的导出量，所以位置矢量也可以用四元数来表征。定义位置矢量的四元数

位置四元数将3D矢量空间增广为4D空间，其中：为任意实数。在物理意义上，与刚体固结的位置矢量既表示该刚体的位置也表示刚体的姿态，位置四元数与姿态四元数在数学形式上一致。它们具有对偶关系：

其中：

证明：考虑式(2.32)、式(2.34)、式(2.143)及式(2.149)中定义的得

考虑式(2.126)、式(2.143)、式(2.146)及式(2.149)中定义的得

由式(2.59)、式(2.143)及式(2.149)中定义的得

故式(2.149)成立。

由式(2.146)及式(2.149)得

因欧拉四元数是特定的位置四元数，由式(2.146)及式(2.149)及得

对比上式及式(2.145)可知：与不同，仍是四元数，但不再是欧拉四元数。故有

位置矢量的坐标变换对应于位置四元数对姿态四元数的“共轭变换”，与矩阵的相似变换相对应。由第2章可知，转动矢量与平动矢量在数学形式上是统一的；由本节可知，位置四元数与转动四元数在数学形式上也是统一的。通常将姿态四元数及位置四元数互称为对偶四元数或合称为双四元数(Dual-Quaternions)。对偶四元数对于通用机械臂位姿逆解非常重要。

【1】基于双矢量的定姿四元数确定原理

由初始单位矢量至目标单位矢量的姿态，等价于绕轴转动角度其中：则有双矢量姿态(Double vector attitude)确定过程：

由式(2.151)得

由式(2.127)，得

其中：| |用于防止数值计算时的溢出。由式(2.153)可知在许多软件(例如Coin3D)中，双矢量定姿算法对用户来说非常不方便，因为它们要求初始矢量至目标矢量的角度范围仅为[0，π)。有双矢量姿态确定过程表明：欧拉四元数本质上统一(Unify)了双矢量叉乘与点乘运算，表达了覆盖(-π,π]的整周(Complete Cycle)转动。

【2】基于轴不变量的正交三轴姿态

示例2.7：记巡视器体系o_c-x_cy_cz_c，惯性系为o_i-x_iy_iz_i；给定轴链ⁱ1_c＝(i，c1，c2，c]，轴不变量序列为角序列记为即初始体系与导航系一致，分别绕ⁱn_c1、^c1n_c2、^c2n_c旋转至巡视器当前姿态。其中：c1、c2为中间坐标系。则有：

证明：由式(2.127)分别得

由式(2.147)得

其中：故式(2.154)成立。由式(2.131)可知

故式(2.155)成立。“3-2-1”姿态角由式(2.144)得

在已知ⁱQ_c时，分别由式(2.145)及式(2.155)计算存在重要区别：由式(2.143)计算ⁱQ_c时，ⁱQ_c1·^c1Q_c2·^c2Q_c中任一项在一定程度上违反了“正交归一化”，致使ⁱQ_c具有明显的“病态”；而由式(2.155)计算ⁱQ_c时，式(2.156)分别得到相应的四元数并“单位化”后得到ⁱQ_c，其精度主要由计算机字长确定，具有极高的精度。因此，通过四元数计算DCM是树链系统运动学计算的基本准则。

示例2.8：在示例2.6中，已知机械手姿态转动的期望姿态为则有：

解：给定被抓工件轴向及转动角度为时，由式(2.127)得欧拉四元数

由式(2.131)可知

由式(2.147)得

故式(2.158)成立。

同样，因存在测量误差，在由式(2.158)计算过程中，应用了四元数并在“单位化”后得到“正交归一化”的其精度主要由计算机字长确定，具有极高的精度。

基于轴不变量的运动微分演算

首先，定义绝对求导符号：

由式(2.159)知：求导符及优先级低于投影符称该求导运算为绝对求导，考虑了基矢量相对惯性基矢量e_i的运动带来的影响。

相对求导运算符是衍生操作符，它的优先级高于投影符即先进行求导运算再进行投影运算。绝对导数也是衍生操作符，它的优先级低于投影符即先进行投影运算再进行求导运算。

【1】角速度的叉乘矩阵

因则

即显然，与互为反对称阵。

将式(2.66)代入式(2.160)得

角速度叉乘矩阵是反对称阵；由式(2.66)及式(2.161)可知：角速度方向由求导的旋转变换阵方向确定，且有

因此，角速度及是矢量，具有可加性。

另一方面，因

则显然，是的轴矢量，即对于任一矢量有服从矩阵运算，服从矢量运算。同样，亦服从矢量运算，即是矢量。故有

同理有：

角速度叉乘矩阵是反对称阵。式(2.168)及式(2.169)是简单刚体速度方程。

因及则

由式(2.166)知，角速度叉乘矩阵是二阶不变量，或服从相似变换。

【2】绝对导数

给定链节l_s是l系中的点，l系在初始时刻方向与系方向一致，l系经时间t后相对系的姿态记为显然，是时间t的函数，位置矢量经无穷次、时间间隔为无穷小的转动至位置矢量则有

式(2.167)是运动链的运动方程，对之求导得

考虑式(2.161)及式(2.168)，即有运动链的速度公式：

由运动链的偏序性可知，根向转动与叶向平动牵连。因此，链指标满足_l·^l对消运算法则。

若为常数时，即则有

若即无平动速度，由式(2.169)得

对比式(2.167)及式(2.169)的链指标关系，令

则式(2.169)表达为

式(2.173)满足链序不变性，是矢量，具有可加性。

称式(2.172)中为正序绝对导数，因为式(2.172)中满足正链序，与的投影坐标系至度量坐标系l的次序一致，且左上角的投影坐标系位于根方向，是正序；由式(2.169)及式(2.166)得

同样，定义

则式(2.174)表达为

式(2.176)满足链序不变性，是矢量，具有可加性；称式(2.175)中为逆序绝对导数，因为式(2.175)中满足逆链序，与的度量坐标系l至投影坐标系的次序一致，且右下角的投影坐标系位于根方向，是逆序。显然，式(2.172)及式(2.173)的正序绝对导数更易使用，默认的绝对求导总是对投影坐标系的求导。式(2.175)及式(2.176)的逆序绝对导数需要在右下角表明求导的参考系。逆序绝对导数运算规律不如正序绝对导数直观，较少使用。由式(2.173)得

由式(2.176)得

【1】基的绝对导数

若由基至动基e_l的姿态为绝对导数表示对投影坐标系i求绝对导数，则有

证明：将被求导项的参考指标转化为求导时间的参考指标，对于任意有限长度的固定矢量或定位矢量考虑式(2.172)有

即

另一方面，

根据矢量绝对求导不变性得

由式(2.180)、式(2.181)及式(2.182)得式(2.179)。

证毕。

式(2.179)特点如下：满足链指标运算律；是投影参考系i至度量参考系l的角速度叉乘矩阵；^i|e′_l求导结果以投影参考系i为参考。

同样有

证明：将被求导项的参考指标转化为求导时间的参考指标，对于任意有限长度的固定矢量或定位矢量考虑式(2.175)有

即

另一方面，

根据矢量绝对求导不变性得

由式(2.184)、式(2.185)及式(2.186)得式(2.183)。证毕。

【2】绝对求导公式

由式(2.180)得

称式(2.187)为“正序的绝对求导公式”，特点如下：与相对求导不同之处在于：增加了牵连项所有和项与积项满足链指标运算律；是由投影参考系i至测量参考系l的角速度叉乘矩阵；结果以投影坐标系i为参考，所有和项的投影参考系具有一致性。

由式(2.184)得

称式(2.188)为“逆序的绝对求导公式”，特点如下：与相对求导不同之处在于：增加了牵连项所有和项与积项满足链指标运算律。是由测量参考系l至绝对求导参考系i的角速度叉乘矩阵。结果以度量参考系l为参考，所有和项的投影参考系具有一致性。

由式(2.161)得

故有

式(2.189)表明：绝对角速度与相对角速度是等价的。

示例2.9：已知则

证明：应用式(2.187)得

证毕。

式(2.190)是著名的欧拉方程。该式等价为

证明1：由式(2.190)得

故

证毕。

证明2：因应用式(2.188)得

证毕。

称式(2.191)为“相对空间欧拉方程”；称式(2.190)为“绝对空间欧拉方程”。尽管式(2.190)与式(2.191)是等价的，这种等价是建立在各运动量无噪声的理想条件基础上的。在工程上，二者有着重要的区别：相对空间欧拉方程中不需要相对绝对空间的姿态，可以通过速率陀螺等惯性器件直接测量得到；而绝对空间欧拉方程需要相对绝对空间的旋转变换阵ⁱQ_l，而ⁱQ_l含有测量噪声。

加速度

由式(2.169)可知对之进行相对求导得

即

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

由式(2.192)可知，平动加速度是矢量，具有可加性。由运动链的偏序性可知，根向转动与叶向平动牵连。因此，及链指标满足_l·^l对消运算法则。记角加速度为则有

其中：是反对称阵，是对称阵。由式(2.193)得

由式(2.194)可知：角加速度是矢量，具有可加性。同时，有

故有

式(2.189)表明：绝对角加速度与相对角加速度等价。对比式(2.189)及式(2.195)可知：转动矢量的绝对导数与相对导数等价。

旋转变换阵的二阶导数

由式(2.66)得

或

由式(2.197)得

故有

对比式(2.161)与式(2.198)或式(2.199)可知：DCM的一阶导数及二阶导数分别对应角速度的一阶矩及角加速度的一阶矩

齐次速度与齐次速度变换

给定链节齐次坐标及且

则有

其中：

证明：

证毕。

基于轴不变量的欧拉四元数微分演算

欧拉四元数微分方程

由式(2.32)可知

由式(2.12)和式(2.13)得

定义右手序(正序)矩阵和左手序(逆序)矩阵分别为：

故

考虑式(2.202)，式(2.204)及式(2.205)，则有

即右手序矩阵和左手序矩阵相互可逆，

因

故

应用式(2.204)及式(2.205)可知

因右手序矩阵和左手序矩阵独立于则有

应用式(2.202)，式(2.204)及式(2.205)得

同理，得

即右手序矩阵和左手序矩阵是与姿态无关的不变量

由式(2.202)，(2.204)及(2.202)得

由式(2.210)，(2.211)得

由式(2.202)，(2.204)及(2.202)得

由式(2.213)，(2.214)得

由式(2.206)得

由式(2.209)得

由式(2.212)，(2.215)及(2.217)得

由式(2.209)，(2.206)及(2.218)得

即

由式(2.220)得

由式(2.32)、式(2.203)及式(2.221)得

即

由式(2.32)、式(2.203)及式(2.223)得

即

由式(2.222)及式(2.224)可知：和分别独立于

将式(2.209)代入式(2.165)第一式得

将式(2.207)代入式(2.228)第一式得

由式(2.229)及式(2.208)得

将式(2.225)带入式(2.230)得

由式(2.231)得

将式(2.206)带入式(2.232)中，可得正序的四元数微分方程

由式(2.204)及式(2.233)得

定义

及其逆序共轭矩阵(叉乘矩阵)，

由式(0.0)及式(0.0)可得四元数导数的正序方程

即

将式(2.238)称作欧拉四元数的微分方程，前四个方程与线性相关，最后一个方程为约束方程。

由式(2.233)得

将式(2.209)代入式(2.239)得

由式(2.206)及(2.240)得四元数导数的逆序方程

同理，由式(2.233)得四元数导数的逆序方程

由式(2.206)得正序四元数的二阶导数

同理，由式(2.243)及式(2.236)得

四元数微分方程求解

因式(2.238)的前4个方程在t_k至t_k+1时间段内，是与线性相关的微分方程，其中t_k+1＝t_k+δt，其指数运算形式为：

由式(2.245)得：

式(2.247)计算复杂度较高，进一步简化得

其中：

式(2.248)即为

其中：

证明：由式(2.37)及式(2.38)可知

显然

证毕。

由角速度矢量积分得转动矢量通过式(2.250)可迭代求解四元数再应用式(2131)得旋转变换阵式(2.250)较式(2.245)计算复杂度要小很多。考虑式(2.250)，当时，有

对于高动态情形,需要提高采样频率，同时需要对数据进行平滑处理，以防止引入高频噪声。详细的四元数微分方程求解可参考文献。式(2.253)主要用于惯性导航及牛顿欧拉动力学积分等。

基于轴不变量的运动旋量与螺旋演算

运动旋量

以γ表示6D空间旋量属性符；相应地，其左上角坐标系应理解为6D的空间；由6D空间至位形空间l的相对位形表示为

对式(2.254)求导得

称为运动旋量(Twist)。

运动旋量转移矩阵

给定运动链得

式(2.256)及式(2.257)表达的是相对平动速度链关系。

由式(2.173)及(2.256)得

由式(2.177)及(2.257)得

由式(2.255)及式(2.258)得

由式(2.255)及式(2.259)得

其中：

称式(2.260)及(2.261)为正序旋量传递矩阵。

【1】运动旋量转移矩阵的串接性

记运动旋量转移矩阵为

由式(2.264)及式(2.263)得

故有

式(2.265)说明运动旋量转移矩阵具有串级性。

【2】运动旋量转移矩阵的逆

记

由式(2.264)及式(2.266)得

故有

故与是互逆的，即有

运动螺旋

【1】空间直线Plücker坐标

给定运动副轴矢量上任一点l_S的参数方程表示为

其中：s—直线参量。称为Plücker坐标；其前三个坐标为该直线的单位方向矢量后三个坐标为与单位方向矢量的矩。空间直线具有两个独立的变量，即具有两个自由度；因此，在直线的Plücker坐标中，必存在4个约束，即因此，Plücker坐标不是矢量，而是数组；服从矩阵运算。记

因

故有

其中：

称式(2.271)为Plücker坐标转移公式。因

故有

在运动学上，无穷远处的一条直线是一条无方向的直线，它的矩是有方向的，且独立于这个点的测度。记由式(2.270)得

【2】三维空间极点

请参照图27。图27为二维空间下极点图。如图所示，在二维空间下，体2707转动角度至体l 2702时，总存在一点S，满足即点S相对两系的坐标不变，将点S称为极点。

试证：如图28所示，给定运动副时，三维空间l中一定存在极点l_S。

证明：若3D空间l中螺旋线(Spiral Line)的极点存在，即满足

则有

将式(2.71)及式(2.275)代入式(2.276)得

故有

即

亦即

故

即

故式(2.277)成立。称为螺旋线极距矢量。

证毕。

试证：给定运动副在杆件l定轴转动时，刚体上任一点l_S的位置矢量在其转轴方向上的分量相等，且该分量为

证明：如图28所示，由式(2.276)得

由式(2.59)及式(2.278)得

且有

表明与转动角度无关，且该分量为

证毕。

由上证明可知，给定杆件l上点l_S，则有不变量点l_S运动螺旋步距p_S为

故

由式(2.276)得螺旋线方程

由式(2.280)得运动螺旋的极距矢量及步距表达式。

基于轴不变量的多轴系统正运动学演算

固定轴不变量的精确测量原理

因为多轴系统的机加工及装配过程(Machining and assembly processes)不可避免地导致设计结构参数(Design Structure Parameters)存在误差，所以需要解决多轴系统的工程结构参数(Engineering Structure Parameters)精确测量的问题。下面，阐述应用自动激光跟踪仪(Automatic Laser Tracking System)精确测量多轴系统的工程结构参数的方法。

多轴系统D＝{T，A，B，K，F，NT}的自然关节空间是以自然坐标系统F为参考的。自然坐标系统F即为原点位于关节轴线且在系统复位(System Reset)时坐标系方向一致。多轴系统结构参数为构型空间表示为

请参照图29。固定轴不变量的测量如图29所示，应用激光跟踪仪测量杆件l上的测点l_S′及l_S。首先，获得轴l定轴转动角度后的测点位置为及然后，获得轴l定轴转动角度后的测点位置为及最后，计算得到单位位置矢量及测量过程总是由根杆件向叶杆件依次进行。

当系统处于零位时，固定轴不变量可由应用激光跟踪仪或3D坐标机测量得到。相对公共参考系F^[i]进行固定轴不变量的测量，可以消除测量误差的累积效应。为考虑加工及装配误差，经常将测量棱镜与被测杆件l固结，通过激光跟踪仪i跟踪测量棱镜中心l_S的位置，得到对应的位置矢量从而，获得与被测杆件l固结的单位矢量

轴不变量计算过程：首先，应用式(2.127),确定及其次，因为已知量，应用式(2.142)得接着，将代入式(2.153)，得最后，由式(2.151)，得请参照图30。图30为固定轴不变量的原点确定图。

如图30所示，将测点l_S′及测点l_S的中点l_S*至轴作垂线得到的交点定义为轴的固定点O_l。

则有

由(2.284)得

由式(2.285)得

解得^lr_S*，

将式(2.287)代入式(2.285)得ⁱr_l，

或

由上可知，条件比正交基e_l更容易满足。该方法有助于精确测量包含加工及装配误差的轴不变量。

在工程测量时，通常由系统根部杆件开始，直至所有的叶；每测量一个杆件后，即可将之制动。选定所有杆件被制动后的状态为零位状态，由关节传感器测量的关节坐标记为称为机械零位。且有

至此，获得了系统结构参数及机械零位将多轴系统控制量记为同时，以非自然坐标系为参考的关节坐标存在参考零位式(2.282)中的关节坐标q_l与控制量q^Δ、机械零位及参考零位关系如下

多轴系统正运动学计算就是在给定结构参数关节坐标q_l、关节速度及关节加速度时，完成期望的位形、速度、加速度及偏速度的计算过程。

由上可知，以自然坐标系为基础，工程上可以精确测量固定轴不变量可以达到关节的重复精度，避免了以笛卡尔直角坐标系为参考导致的结构参数测量误差过大问题，为精密多轴系统的研制奠定了基础。

理想树形运动链位形计算流程

给定树形运动链ⁱl_n，轴l，n∈A，n＞l，s是体l上的任一点；当测量无测量噪声时，运动链ⁱl_n正运动学计算流程为：

【1】链节正运动学计算

【1-1】若则由结构参数及运动参数根据式(2.130)计算欧拉四元数或由式(2.93)计算

【1-2】若则由式(2.131)计算旋转变换阵或由(2.99)式计算旋转变换阵若则有

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

【2-1】由式(2.6)计算齐次变换阵ⁱQ_n；

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

在理想正运动学计算流程中，既可以应用欧拉四元数又可以应用有限转动四元数计算链节旋转变换阵。

上述理想正运动学计算流程在Open Inventor、Coin3D等3D软件中得到广泛应用。但在有测量噪声时，所有的旋转变换阵计算均应使用式(2.131)及式(2.147)所示的过程，一方面，需要降低测量噪声导致的旋转变换阵的“病态”；另一方面，阻止“病态”旋转变换阵在串接性运动时的进一步恶化。

基于轴不变量的迭代式运动学计算流程

给定树形运动链ⁱl_n，轴l,n∈A，n＞l，S是体l上的任一点。当有测量噪声时，建立运动链ⁱl_n迭代式(Iterative)正运动学(Forward>

【1】链节正运动学计算流程

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

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

【1-3】由式(2.9)计算链节速度；

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

【2】运动链ⁱl_n的位形计算流程

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

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

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

【3】运动链ⁱl_n的速度及加速度流程

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

证明：由式(2.189)得

证毕。

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

证明：由式(2.195)得

证毕。

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

证明：由式(2.187)得

证毕。

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

证明：由式(2.192)得

证毕。

基于轴不变量的偏速度计算原理

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

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

式(2.296)的特殊形式为

下面，证明基于轴不变量的迭代式偏速度公式。

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

证明：由式(2.292)得

证毕。

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

证明：

证毕。

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

证明：由式(2.292)得

证毕。

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

证明：由式(2.8)得

证毕。

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

证明：由式(2.294)得

即

故有

证毕。

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

证明：由式(2.294)得

证毕。

将上述结论，以定理2.2统一表述，称之为偏速度定理。

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

证明：当时，由式(2.300)，(2.298)，(2.301)及(2.299)可得式(2.304)。

由式(2.302)及(2.303)得式(2.305)。因ⁱφ_n和与和无关，得式(2.306)。证毕。

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

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

由式(2.14)可知

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

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

轴不变量对时间的不变性

由式(2.189)及式(2.195)可知

故有

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

由式(2.304)及式(2.309)得

由式(2.187)及式(2.309)得

由上式得

即

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

由式(2.65)得

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

对于MAS系统的树由式(2.309)及式(2.312)得

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

树形运动链变分计算原理

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

【1】转动矢量的变分

证明：由式(2.298)得

证毕。

【2】平动矢量的变分

证明：由式(2.302)及式(2.303)得

证毕。

自然坐标轴链与笛卡坐标轴链的关系

给定轴序列ⁱ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]；记

则该特定的自然轴链与笛卡尔轴链等价。将该运动链简记为ⁱF_c,即由自然坐标系F^[i]至自然坐标系F^[c]的笛卡尔轴链。显然，姿态序列可以根据工程需求进行设置。

由式(2.304)及式(2.315)得

由式(2.317)得

由式(2.317)得

给定笛卡尔轴链ⁱF_c，作用于体c一点S的作用力为位置矢量为则有

由式(2.301)得

故得

由式(2.305)得

由式(2.299)、式(2.323)得

故得

由式(2.322)及式(2.325)可知：作用于体c一点S的作用力对于原点O_c的作用效应包含作用力矢量及力矩其中该结论与传统力学的力作用效应结论一致。

由上可知：当三轴平动序列及三轴转动序列为笛卡尔直角坐标系的坐标轴时，笛卡尔直角坐标系的叉乘运算才成立。笛卡尔轴链是自然轴链的特例。基于轴不变量的轴链运动学具有以下特点：

【1】以简洁的自然坐标系为参考，保证了固定轴不变量结构参数测量的精确性，不仅包含加工误差，而且包含装配误差；

【2】具有简洁、精确、统一的轴链符号系统，运动学方程含义清晰、准确；

【3】基于轴不变量的迭代式的运动学方程是关于轴不变量的迭代式，保证了运动学计算的准确性及实时性；

【4】基于轴不变量的迭代式的运动学方程自身具有伪代码的功能，保证了工程实现的可靠性。

基于轴不变量的对偶四元数演算

给定运动链ⁱl_c，l，k∈ⁱl_c，且l≠k，由式(2.7)及(2.8)表示的位形计算可知：链节平动量与的运算对运动链总位移ⁱr_c无影响。双数及对偶四元数就是这类关系的抽象。

双数

双数(Dual numbers)由Clifford提出，定义双数单位符号ε及双数基

0·ε＝0,1·ε＝ε,ε²＝ε*ε＝0，>

双数算子ε具有二阶幂零特性。双数基通常写成行的形式，它与空间参考基无关，仅表示双数间的相关性，不存在绝对导数。

定义一个双数不变量及其保证模不变的共轭数

其中：称为双数的伪坐标，通常写成列的形式。双数的数可代指任意数的阵列。其中：φ表示转动属性，r表示平动属性。称φ及r分别为双数的主部与副部。

【1】双数的基本运算

给定双数序列满足加法“+”及乘法“*”运算，由式(2.326)得

由式(2.328)及式(2.330)得

由式(2.331)得

【2】双数的导数

转动属性φ与平动属性r通常是时间t的函数。若ρ为常数，由式(0.0)对时间t求导得

将双数的函数展开为ε的Taylor级数，得

即有

其中：f(φ)为函数主部，与双数主部φ相对应。同时，对式(2.334)两边求导得

【3】双数的正余弦函数

由式(2.334)得

应用式(2.335)对式(2.336)求导或直接对式(2.336)求导得

即有

由式(2.336)得

即

通过对双数主部φ及副部r的实例化，可以得到相应的双数实例，如：广义对偶四元数、位形对偶四元数及螺旋对偶四元数等。

由式(2.328)及式(2.336)得

基于轴不变量的欧拉四元数迭代式

由式(2.124)得

由式(2.145)得姿态四元数迭代式：

由式(2.141)及式(2.341)得

由式(2.341)及式(2.342)得

由式(2.145)及式(2.341)得

即

由式(2.344)可知：欧拉四元数的相似变换是其轴不变量的投影，标部S_l及矢部的大小C_l具有不变性，因为轴矢量及关节角是不变量；由式(2.60)可知：DCM矩阵的相似变换对应其转动矢量投影后的叉乘矩阵且有

显然，式(2.341)、式(2.342)及(2.344)均具有迭代式的形式；它们是后续应用对偶四元数建立运动方程的基础。

给定三维坐标矢量a、b及c，则有

a^T·b·c＝c·a^T·b。>

证明：

证毕。

由式(2.346)得

由式(2.124)及式(2.347)得

即有

由式(2.348)可知：的矢部是对应矢部的投影，其标部保持不变。式(2.344)及式(2.348)表明：姿态四元数及位置四元数的相似变换的本质是投影变换。

基于轴不变量的对偶四元数

【1】对偶四元数及基本运算

定义对偶四元数如下：

其中：―姿态四元数，―位置四元数，ε―满足式(2.326)的双数算子。为对偶四元数的伪坐标。位置四元数表征的是原点O_l至体l上任一固结点S的位置四元数。

定义单位欧拉四元数及零位置四元数

当时，称对偶四元数为纯转动对偶四元数；当时，称对偶四元数为纯平动对偶四元数。一般情况下，对偶四元数既表示平动又表示转动。

给定对偶四元数及满足式(2.329)中的加法“+”运算，得

即有伪坐标的形式，

给定对偶四元数及满足式(2.330)中乘法“*”运算，考虑式(2.326)得

由式(2.330)得

即有

上式表明：不一定为0。因此，式(2.353)对应的伪坐标形式为

【3】共轭对偶四元数

定义对偶四元数的共轭(Conjugate)对偶四元数及如下：

由式(2.349)得

证明：考虑式(2.354)及式(2.355)，得

故式(2.356)成立。考虑式(2.354)及式(2.355)，得

故式(2.357)成立。证毕。

由式(2.122)、式(2.354)及(2.355)得

即

由式(2.126)、式(2.354)及(2.355)得

即

【4】正交平动四元数的性质

当时，有称为正交平动四元数。由式(2.358)及正交平动四元数的正交性得

定义正交平动四元数的逆满足

由式(2.360)得

当时，有考虑式(2.122)及式(2.148)，由式(2.361)及式(2.353)得

因此，当时，为正交平动四元数，其逆为

由式(2.350)及式(2.363)得

由式(2.349)及式(2.363)得

当时，将式(2.150)代入式(2.365)得

基于轴不变量的位形对偶四元数

由式(2.86)可知，轴链的位置方程是关于{τ_l|l∈ⁱl_n}的多元二次方程。由于求解多元二次的解非常困难；需要简化式(2.86)所示的定位方程，以便较容易地获得逆解。考虑轴链则有该式是迭代式运动学的基本形式。若要简化运动链的定位方程，则要将式以更加紧凑的形式表示。

【1】双四维复数基

首先定义双四维复数基

由式(2.114)及式(2.332)得

故有

【2】单边对偶四元数

定义为位形对偶四元数类别。考虑式(2.150)及式(2.350)，将平动及转动组合为式(2.369)所示的单边位形对偶四元数(Unilateral Dual-quaternions)

其中：为位置四元数，定义如式(2.148)所示,且d_l＝0。位形对偶四元数的伪坐标为

由式(0.0)得

显然，表示先转动后平动的位置四元数，具有由根至叶的正序。考虑式(2.144)、式(2.148)及式(2.370)得

由式(2.372)可知：

由式(2.148)、式(2.355)及式(2.370)得

即有

显然，及表示先平动后转动的位置四元数，具有由叶至根的逆序。由式(2.361)可知，式(2.370)中单边位形对偶四元数的逆对偶四元数为

由式(2.370)及式(2.375)得

式(2.149)及上式得对位形对偶四元数的模不变性方程：

其中：为单位对偶四元数，

【3】双边对偶四元数

定义位形双边对偶四元数(Bilateral Dual-quaternions)

式(2.378)是式(2.370)的特例，又称之为位置对偶四元数。由式(2.148)、式(2.355)及(2.378)得

且有

因为式(2.370)中位形对偶四元数的结构参量正是固定轴不变量请参照图32，图32为固定轴不变量与位形对偶四元数的关系图，图32包含轴不变量轴不变量位置矢量如图32所示，所以轴链位形对偶四元数描述的是位置矢量3203绕轴不变量3201的螺旋运动(Screw motion)。

【4】对偶四元数的链关系

给定轴链双边对偶四元数及单边对偶四元数则有对偶四元数由叶至根的逆序迭代式

证明：由式(2.370)及式(2.375)至式(2.378)得

故式(2.381)成立。由式(2.370)及式(2.375)至式(2.378)得

故式(2.382)成立。

证毕。

给定轴链双边对偶四元数及单边对偶四元数则有对偶四元数由根至叶的正序迭代式：

证明：由式(2.370)及式(2.375)至式(2.378)得

故式(2.383)成立。由式(2.370)及式(2.375)至式(2.378)得

故式(2.384)成立。

证毕。

当给定轴链l∈ⁱl_n时，由式(2.381)至式(2.384)分别得双边位置对偶四元数及单边位形对偶四元数的迭代式：

式(2.386)及式(2.388)不仅进行了位置矢量的坐标变换，而且保证了运动链位置矢量的不变性，分别用于建立轴链及轴链的定位方程；而式(2.385)及(2.387)是平凡的，无实际用处。在式(2.386)及式(2.388)中，通常或表示期望的位置对偶四元数；或表示终端效应器(End-effector)相对其体系的位置对偶四元数。虽然式(2.385)至(2.388)形式较简单，但当轴数较多时，计算过程很复杂，需要建立有效的迭代过程。

下面，进一步简化位形对偶四元数的表达形式，以得到由平动及转动统一描述的、更简洁的螺旋对偶四元数。首先，提出由D-H参数表征的螺旋径向不变量；进而，提出基于轴不变量的螺旋对偶四元数。

【1】螺旋径向不变量

由第一部分可知：位形对偶四元数本质上表示的是螺旋运动。下面，进一步将式(2.372)表达为更加简洁的形式，以反映螺旋运动的特点。请再度参照图32，由D-H参数定义得

显然，有平凡事实：

及式(2.32)得

故有

其中：

由式(2.394)得

故有

由式(2.394)可知，总存在依赖于系统结构参量c_l及关节角的螺旋径向不变量且与正交。称为的螺旋径向不变量。它是螺旋对偶四元数的前提条件。下面，阐述螺旋对偶四元数。

【2】螺旋对偶四元数

定义为螺旋对偶四元数类别。令任一体的自然坐标原点与其D-H系统原点一致，参考系仍为自然坐标系。由式(2.394)得

当

时，由式(2.373)得

由式(2.395)及式(2.398)得

当时，由式(2.373)得单边对偶四元数，

由式(2.400)可知：是关于C_l、S_l及c_l的线性型，将平动与转动以紧凑的形式表达了出来。因式(2.400)的螺旋对偶四元数是特定的位形对偶四元数，故式(2.385)至式(2.388)仍成立。

以螺旋径向不变量为基础，建立螺旋对偶四元数的作用在于：应用式(2.395)、式(2.396)可以减少对偶四元数的计算量，易于揭示螺旋对偶四元数的规律，从而简化运动学求解过程。请参照图33，由式(2.395)、式(2.396)可知：螺旋径向不变量与轴不变量及正交，如图33所示。

考虑式(2.373)，当时，得单边螺旋对偶四元数。

对比式(2.400)及式(2.401)，同样可知：螺旋对偶四元数是特定的位形对偶四元数。因此，位形对偶四元数的性质同样适用于螺旋对偶四元数。当应用D-H参考及螺旋四元数的机械臂运动学建模时，不得不通过两个螺旋四元数表达一个关节的运动，其符号演算及数值计算量比应用一个位形四元数高得多。

【3】螺旋对偶四元数的不变性

由于螺旋对偶四元数是位形对偶四元数的特例，可以将之表达得更为紧凑。考虑具有双数基及双4维复数基的螺旋对偶四元数，由式(2.67)、式(2.349)、式(2.369)、式(2.336)及式(2.400)得螺旋对偶四元数不变量，即伪坐标与基的整体形式：

即有

其中：

称为对偶螺旋轴(Dual screw axis)，它由螺旋径向矢量及螺旋径向不变量构成。

由式(2.358)得

即有

式(2.407)表明：对偶螺旋轴是单位的双3D矢量，螺旋对偶四元数是单位四元数。应用式(2.404)至式(2.406)，将式(2.402)重新表示为

其中：

对比式(2.409)及式(2.127)，二者具有一样的单位轴及正余弦的数学结构；因此，螺旋对偶四元数具有欧拉公式的所有空间操作(Spatial operators)。螺旋对偶四元数是平动及转动四元数的对立统一(Unity of opposites)，式(2.395)及式(2.409)分别表明二者的对立与统一。

将式(2.104)中的i及分别用及代换，则有，

即有螺旋对偶四元数的伪坐标形式

显然，式(2.410)中的指数运算比对偶四元数的复数乘法运算具有更优良的操作性能，为复杂运动学分析提供了有效的技术途径。

考虑式(2.368)，由式(2.410)得

【4】螺旋对偶四元数的幂与微分

由式(2.409)得的ρ次幂，

显然，式(2.413)是式(2.409)的插值(Interpolation)过程。由式(2.413)及式(2.237)分别得欧拉四元数的插值及微分公式：

由式(2.400)得的ρ次幂，

同样，由式(2.409)得的ρ次方，

记双3D转动速度叉乘矩阵及双3D单位矩阵表示为

由式(2.336)及式(2.408)得

由式(2.337)及上式得

将(2.419)表示为双四元数的矩阵形式，得

其中：

由式(2.420)可知：总是关于及的函数。

对偶四元数的迭代式

一方面，建立高自由度的显式规范方程计算复杂度极高，难以手工完成，必须借助于Maple等符号计算软件完成该运动学建模；但是，Maple等符号计算软件难以集成于机器人控制系统当中。另一方面，变结构的机器人需要实时地自动建立规范的运动学显式方程。因此，需要解决建立基于对偶四元数的迭代式运动学方程的问题。

由式(2.341)及式(2.342)可知：

式(2.422)中的^i|nλ_n是关于的多重线性型，即四元数表示的姿态方程是关于的多重线性多项式。

式(2.386)及式(2.388)的位置四元数可以表示为如下迭代式：

证明：由式(2.344)得

式(2.386)及式(2.388)表明：的矢部是对应矢部的投影；的矢部是对应矢部的投影。由式(2.382)及式(2.384)分别得

显然

由式(2.386)、式(2.388)、式(2.348)及式(2.425)至(2.428)得式(2.423)及式(2.424)。

证毕。

因螺旋对偶四元数是位形对偶四元数的特例，式(2.423)及式(2.424)同样适用于螺旋对偶四元数，应用式(2.401)可以进一步简化式(2.423)及式(2.424)。

由式(2.422)及式(2.424)可知：它们是关于的一阶多项式方程；式(2.422)中是关于的多重线性多项式；因此，运动链ⁱl_n的运动学方程是Cayley参数的多项式方程。给定或时计算是逆运动学求解问题；而如何求解关于的多项式方程是逆运动学研究的核心问题。

由式(2.422)及式(2.424)可知：基于对偶四元数得到的机械臂运动学方程是平凡的，它们是基于轴不变量的运动学方程；即使不应用任何对偶四元数的知识，也能得到式(2.422)及式(2.424)。显然，相对四元数而言，对偶四元数概念并没有增加新的信息，难以得到新的结论。

轴不变量概念的作用

本章提出了轴不变量的概念，本章建立了基于轴不变量的多轴系统正运动学。现在，总结一下轴不变概念的作用：

【1】轴不变量是轴及轴l的公共参考轴，与关节坐标一起，通过罗格里德四元数及欧拉四元数实现自然坐标系F^[l]的参数化及轴l极性的参数化；轴不变量是既表示轴及轴l的具有不变性的结构参量，又表示轴与轴l的不变的轴链，通过拓扑操作实现了多轴系统拓扑结构的参数化。

【2】以轴不变量及关节坐标为基础的欧拉四元数保证了与转动矢量的一一映射，保证了自然参考轴定义的不变性；从而，保证多轴系统运算的可靠性。

【3】轴不变量及具有不变性，具有优良的操作性能，其衍生的具有反对称性、具有对称性、当时，具有幂零特性。

【4】由式(2.309)及式(2.312)分别可知：轴不变量具有对时间的不变性，运动链的DCM对关节角的偏速度具有轴不变量的不变性，轴不变量就是基的坐标矢量。

【5】轴不变量是3D空间操作代数的基元；具有拓扑操作的3D空间操作代数，不仅物理意义清晰，而且计算简单，可以适应变拓扑系统的应用需求；既有别于传统的3D矢量代数，又有别于6D双矢量算子代数。

【6】通过轴不变量可以将3D矢量转化为6D矢量、四元数及位形对偶四元数等不同数学空间，并减小相应的计算复杂度，建立了结构化的、内在紧凑的多轴运动学系统。

【7】基于轴不变量的运动学方程是轴不变量的二阶多变量多项式方程，也是关于轴不变量的迭代式方程；不仅统一了运动学方程的形式，而且提高了运动学计算的效率与精度；

【8】轴不变量可以通过激光跟踪仪等光学设备实现精确测量。

【9】通过轴不变量，实现了包含坐标系、极性及系统结构参量的完全参数化建模。

轴不变量是实现坐标系、极性、结构参数及动力学参数的完全参数化的关键。在基于轴不变量的多轴系统动力学建模中，可以直接列写任一轴的动力学方程，该方程同样可以表示为轴不变量的迭代式，且广义惯量是3×3的矩阵，可以极大地降低正逆动力学计算的复杂度。因此，将轴矢量命名为轴不变量不仅是必要的，而且反映了轴不变量本征属性。

轴不变量既是多轴系统结构的基元，又是多轴系统运动参考的基元；同时，轴不变量既具有优良的数学操作性能，又具有通过激光跟踪仪进行精密测量的优点；因此，轴不变量是构建结构化的、内在紧凑的多轴系统理论的基石。

通俗地说，轴不变量对于多轴系统建模的作用与二进位字对于信息处理的作用一样：它们都是系统的基元，是真实世界的自然描述，具有最简的操作性能；因而，以此构建的系统具有灵活性及高效性。多轴系统运动学具有与自然数同构的链符号系统，它们为多轴系统提供了作用方向及度量基准参考，保证了系统内部作用关系的客观性、紧凑性及层次性。

第三部分>Equation Chapter 3 Section 1

基础公式

【1】由式(2.85)得规范的姿态方程

【2】由式(2.86)得规范的定位方程

【3】当k∈ⁱl_n时，由式(2.304)至式(2.306)可知

多轴系统逆运动学特点

考虑式(3.1)，若ⁱQ_n表示需确定的姿态，仅三个独立的自由度；则当|ⁱl_n|＝3时，存在定姿逆解。给定单位矢量由式(3.1)得

若表示需要确定的方向，则当|ⁱl_n|＝2时，存在定向逆解。给定单位矢量及由式(3.1)得

若表示期望的投影标量，则当|ⁱl_n|＝1时，存在投影逆解。

考虑式(3.2)，若表示期望确定的位置，则当|ⁱl_n|＝3时，存在位置逆解，当|ⁱl_n|＝6时，存在位姿逆解。

式(3.1)及式(3.2)是关于Cayley参数的n维2阶多项式方程，计算逆解的困难在于：需要通过复杂的消元过程，获得仅含有单个Cayley参数的高阶多项式方程；进而求解一元高阶多项式方程。目前，基理论是解决多元多项式方程求解的一种可能途径，但其计算复杂度通常极高。对于机器人逆运动学(Inverse Kinematics/IK)问题，应用多重线性运动学方程计算逆运动学显式解(Symbolic/Analytic Solution)是一个更高效的途径。

下面，讨论一元高次多项式方程及多元多项式方程(Multivariate PolynomialEquations)消元原理，为后续进行逆运动学计算奠定基础。16世纪意大利数学家卡当(Candano)及其助手先后给出了一元三次和四次方程的解析解。法国青年数学家伽罗华(Galois)利用“可解群”理论证明一般五次以上方程不存在解析解。为方便读者阅读，下面对一元三次和四次方程的解法进行整理，以便后续引用。

一元三次方程解析解

给定一元三次方程a·x³+b·x²+c·x+d＝0，其中：且a≠0；i为一维单位纯虚数；证明该方程3个解分别为

其中：

证明：第一步，转化为标准三次式。记

x³+α·x²+β·x+γ＝0，>

则有，α＝b/a，β＝c/a，γ＝d/a。

第二步，消去平方项。令x＝y-λ，代入式(3.10)得

(y-λ)³+α·(y-λ)²+β·(y-λ)+γ＝0，

对之展开得y³-3λy²+3λ²y-λ³+αy²-2·αλy+αλ²+βy-βλ+γ＝0。故有

y³+(α-3·λ)·y²+(3·λ²-2·α·λ+β)·y-(λ³-α·λ²+β·λ-γ)＝0。>

令λ＝α/3，式(3.11)中平方项消除。得x＝y-λ＝y-α/3。则式(3.11)可化简为y³+(-α²/3+β)·y+(2/27·α³-1/3·α·β+γ)＝0，并表示为

y³+p·y+q＝0；>

其中：

第三步，化为二次平方式。令y＝u+v，代入(3.12)式得

(u+v)³+p·(u+v)+q＝0。>

显然，有下式成立，

(u+v)³-3·u·v·(u+v)-(u³+v³)＝0。>

对比式(3.13)及式(3.14)系数得

将u³,v³视为一元二次方程的根，故有z²+q·z-p³/27＝0，解u³及v³得

则

其中：

第四步，得标准三次式的解。因故y＝u+v存在以下三种可能：

令

第五步，还原解。

用x＝y-λ＝y-α/3＝y-b/(3·a)换元得式a·x³+b·x²+c·x+d＝0的解为式(3.8)及式(3.9)。证毕。

一元四次方程解析解

给定一元四次方程a·x⁴+b·x³+c·x²+d·x+e＝0，其中：且a≠0；证明该方程4个解分别为

其中：

证明：第一步，规范化。由ax⁴+bx³+cx²+dx+e＝0得记其为

x⁴+α·x³+β·x²+γ·x+μ＝0；>

其中：α＝b/a，β＝c/a，γ＝d/a，μ＝e/a。

第二步，去除三次项。令x＝y-α/4，代入式(3.17)得

即

令式(3.18)等价为

y⁴+p·y²+q·y+r＝0；>

其中：

第三步，化为二次平方式。显然，式y⁴+p·y²+q·y+r可转化二次平方式。

令y⁴+p·y²+q·y+r＝y⁴+z·y²+z²/4，将式(3.19)代入之得

y⁴+z·y²+z²/4＝z·y²+z²/4-p·y²-q·y-r＝0，即有

(y²+z/2)²＝(z-p)·y²-q·y+(z²/4-r)＝0。>

故式(3.20)之判别式，亦称为一元四次方程的预解式：q²-4·(z-p)·(z²/4-r)＝0，即

q²-(z-p)·(z²-4·r)＝0，>

式(3.21)即为

z³-p·z²-4·r·z+(4·r·p-q²)＝0。>

利用式(3.8)至式(3.9)，求式(3.22)的实数根z′：

将该实数根z₁代回式(3.20)得

由式(3.24)得两个二次式

解式(3.25)得

再代入x＝y-α/4＝y-b/(4·a)，故

证毕。

基于友阵的一元高阶多项式方程求解

一元n阶多项式p(x)＝a₀+a₁x+…a_n-1x^n-1+xⁿ具有n个解。若能找到一个矩阵A，满足|A-λ_l·1_n|·v_l＝0，其中：l∈[1:n]，λ_l为该矩阵的特征值，v_l为对应的特征矢量。若矩阵A的特征方程为则称该矩阵为多项式p(x)的友矩阵(Companion Matrix，简称友阵，因此，多项式方程p(λ_l)＝0的解为其友阵A的特征方程|A-λ_l·1_n|＝0的解。

若多项式p(x)的友阵为

则由矩阵A的特征向量构成的矩阵为范德蒙德(Vandermonde)矩阵为

且有

p(λ_l)＝|A-λ_l·1_n|＝0。>

证明：记特征值λ_l对应的特征向量为v_l，由特征值定义可知A·v_l＝λ_l·v_l，故有

故式(3.27)成立。因A·v_l＝λ_l·v_l，则有

因v_l≠0_n，故有

|(A-λ_l·1_n)·v_l|＝|A-λ_l·1_n|·|v_l|＝|p(λ_l)|·||[0,…,0,1]^T||。

因此，式(3.28)。证毕。

■

示例3.1求解多项式p(x)＝x³-10x²+31x-30。

解：由式(3.28)及式(3.28)分别得

解得全部特征根λ＝[2,3,5]，即为该多项式方程的全部解。

示例3.2求解多项式p(x)＝x²+1。

解：由式(3.26)及式(3.28)分别

解得全部特征根±i，即为该多项式方程的全部解。

“n个n元N阶”多项式系统消元与求解

考虑具有2个单变量多项式(Univariate Polynomials)方程f(x)及g(x)构成的多项式系统F₂(x)，并引入辅助变量(Auxiliary>

是关于变量x及y的对称(Symmetric)多项式；其中：Det表示行列式(Determinant)操作。这是Cayley于1865年提出的Cayley-Bézout方法。若

f(x)＝0,g(x)＝0， (3.30)

则对任意y必有Dixon多项式

显然，x-y必然被消去。故式(3.31)是方程组(3.30)有解的必要条件(Necessarycondition)。因该系统存在非独立的方程，故该系统Dixon多项式恒为零(Identicallyzero)，它是该系统可解的必要条件。因此，有必要将Dixon多项式推广至(Generalize to)“n个n元N阶”多项式系统，即由“n个n元N阶”多项式方程构成的多项系统。

“n个n元1阶”多项式系统

一个代数环(Algebraic Ring)是指一个满足加及乘运算的集合。其中：加法满足交换律(Commutative)、结合(Associative)律、逆运算及加法单位性(AdditiveIdentity)；乘法对加法满足左分配律(Left Distributivity)或右分配(RightDistributivity)律、乘法单位性(Multiplicative Identity)。二进制代数系统(BinaryAlgebra)及多项式符号系统(Polynomial Symbolic Systems)是典型的代数环系统，它们是互为同构的系统。

记多项式独立变量(Independent Variables)记为X_n＝[x₁，x₂，…，x_n]，n位二进位字(Binary>n，以行向量表示，多项式组合变量(Composite>是独立变量的二进位字幂积(Power Product)，以列向量表示。

其中：独立变量数为n；n位二进位字α_n共有2ⁿ个二进位字实例，与组合变量一一映射，故有组合变量的维度由上可知：组合变量的系数(Variable Coefficients)矩阵记为与二进位字一一映射。

示例3.3：为书写方便，记当n＝3时，有

分块矩阵的高维行列式计算原理

记<1:n>表示自然数[1:n]的全排列，共有n！个实例。给定属于数域的大小为n×n的矩阵M，其j行i列元素记为根据行列式定义得

其中：I[i1,…in]表示排列<i1,…in>的逆序个数。式计算复杂度为：n！次n个数积及n！次加法，具有指数计算复杂度，只能适用于维度较小的行列式。对于维度较大的行列式，通常应用Laplace公式进行递规运算，记为的伴随矩阵(Adjugate Matrix)，则有

更简单的算法通常应用高斯消去法或LU分解法，先通过初等变换将矩阵变为三角阵或三角阵的乘积，后计算行列式。上述针对数域的行列式计算方法不适用于高维度的多项式矩阵，需要引入分块矩阵的行列式计算方法。计算矢量多项式(Vector Polynomial)的行列式是一个特定的分块矩阵行列式的计算问题，它在矢量层次上表达了矢量与行列式的内在联系。而分块矩阵行列式计算则从矩阵层次上表达分块矩阵与行列式的内在规律。

若给定矢量多项式其中：及为3D坐标矢量，为多项式变量序列；若约定

则有

证明：因

故式(3.39)成立。证毕。

■

式(3.38)及式(3.39)可以推广至n维空间。式(3.38)有助于从矢量层次上分析行列式的内在规律；比如，当任两矢量平行或三矢量共面时，对应的行列式为零。式(3.39)表明：矢量多项式的行列式易导致“组合爆炸”。

示例3.4给定2个2维行矢量多项式及一方面，由式(3.39)得

另一方面，

上面的结果验证了式(3.39)的正确性。

下面，先陈述分块矩阵(Block matrix)的行列式计算定理，然后再予以证明。

定理3.1若记大小为(n+m)·(n+m)的方阵为M，大小为n·n的矩阵是方阵M的前n行及任意n列元素构成的子矩阵，大小为m·m的矩阵是方阵M后m行及剩余m列元素构成的子矩阵；由升序排列的矩阵列序号构成的序列cn及cm是序列[1:m+n]的子集，[cn,cm]∈<1:n+m>，且有cm∪cn＝[1:m+n]；则方阵M行列式与分块矩阵及的行列式关系为

证明：因行列式由矩阵元素的全排列(Full permutation)确定，故子矩阵及的全排列与方阵M元素的全排列等价。[cn,cm]共有(n+m)！/(n！m！)种。因方阵M由子矩阵及构成，故方阵M元素的全排列可以被分成(n+m)！/(n！m！)类；其中：的元素排列有n！种，的元素排列有m！，每一类包含n！m！种排列。因此，方阵M的行列式表示为：

证毕。

■

式右侧的每一项需要执行n·n！+m·m！+1次积运算及和运算，总运算次数为(n+m)！(n·n！+m·m！+1)/(n！m！)，远比未分块计算时的总运算次数(n+m)(n+m)！小得多，当n和m较大时尤其明显。当矩阵较大时，可以采用逐步分块的方法进一步降低计算量。对于16·16的方阵而言，其行列式计算复杂度可以由原先的16·16！降至逐步分块后的10,992,267，计算速度可以得到显著提高。

示例3.5根据Laplace公式，计算如下方阵M的行列式，

易得Det(M)＝6。选择2·2分块矩阵，即n＝m＝2。应用式计算过程如下：

两种方法的计算结果一致，验证了式的正确性。

为简化行列式计算与分析，约定表示在满足条件con下的相等。例如：若且y₂的阶次α[2]＜2；则有

对行列式进行行阶梯化计算原理：

对于S×S矩阵，其每一项是关于τ₁的n阶多项式。计算该矩阵的行列式时，可通过初等行变换将原行列式变为上三角行列式，再将非零的对角线元素相乘，得到行列式的多项式表达式。该式为0，得到τ₁的所有解。

行阶梯化的具体方法为，先对行列式第一列的最高阶次由高到低进行排序，再进行最多(S-1)×n次初等行变换消元，得到第一列只有第一个元素不为0的行列式。再对该行列式第1行及1列的余子式进行初等行变换消元，依次迭代求解。

基于N进位字的N阶多项式系统

【1】基于N进位字的N阶多项式系统

若“n个n元1阶”多项式幂积中独立变量重复出现N次，则得到n个n元N阶多项式系统n元N阶多项式系统与n位N进位字同构。

示例3.6：由式(3.35)及式(3.43)得3维1阶多项式

N进位字乘积及多项式组合变量乘积运算如下：

a_m·a_n＝Power(α[1]·α[2]…α[m]·α[m+1]·α[m+2]…α[m+n]，α^[·]∈[0：N]；(3.45)

相应地，有组合变量系数(Variable coefficients)表示为

【2】n个“种元N阶”多项式系统的Dixon多项式

引入辅助变量[y₂,y₃,…,y_n]，且有

在多元多重多项式(3.43)中，用辅助变量Y_m的前m个依次替换原变量(OriginalVariables)X_n中的m个变量，记“|”为替换操作符，得到增广的(Extended)多项式

由式(3.41)及式得

其中：

定义可分离组合变量及如下：

由式(3.52)及式(3.53)可知：替换式是关于及的双重线性型。相应地，用辅助变量替换的多项式系统记为

给定n个“定元N阶”多项式系统定义其Dixon多项式为

由式得

式(3.53)中分离变量与其他文献不同：原变量X_n-1被辅助变量Y_n-1替换的次序不同，Dixon多项式也不同。考虑式及式得该多项式的Dixon行列式

在笛卡尔空间下，由位置矢量或转动矢量构成的行列式表示矢量张成空间的容积(Volume)；在不同笛卡尔空间下具有容积的不变性。其中：

n个“n元N阶”多项式的Dixon行列式的阶次及替换变量项数分别为：

n个“n元N阶”Dixon矩阵的存在性

定理3.2：给定n个“定元N阶”多项式系统F_n(Y_n-1|X_n-1)，n≥2；存在与消去变量x₂,…,x_n无关的Dixon矩阵_SΘ_S(x₁)，其Dixon多项式表示为分离变量(Separable Variables)及的双重线性型(Bilinear Form)：

α[l]∈[0,N·(n-l+1)-1],l∈[2:n]。 (3.61)

Dixon矩阵大小为S×S，其第[i][j]成员为单变量x₁的N阶多项式：

其中：

若

则有

证明：考虑式(3.58)，记A^[1][c]为第1行第c列的伴随矩阵，得

由式(3.51)、式(3.52)、式(3.53)及式(3.59)可知：是及的双重线性型。由式(3.59)得式(3.63)。要保证式成立，则要求

故得式(3.51)。因Dixon矩阵_SΘ_S的每个成员有N+1个待定系数，故共有(N+1)·S×S个待定系数。因Order(Det(_SΘ_S(x₁)),x₁)＝S·N≥N，分离变量不影响式(3.60)右侧x₁阶次，故得式(3.62)。由式(3.60)及式(3.67)可知：式(3.60)两边的及阶次相等，两边的x₁阶次相等。应用独立辅助变量替换原变量，得到增广的多项式；因行列式即容积不取决于变量的维度及形式，故增维后行列式与原行列式相等。故式(3.60)至式(3.62)成立。

记l,l′∈[2:n]，由式(3.64)可知：式(3.60)中若存在则存在因变量替换不改变多项式系数，故及的系数与多项式项的系数相同，且Dixon矩阵具有对称性。因此式(3.65)成立。证毕。

称定理3.2为n个“n元N阶”Dixon矩阵存在性定理。该定理与Dixon消元方法不同在于：(1)式(3.58)的Dixon多项式计算过程不同；(2)式(3.60)的消元过程不同，适用于n个“n元N阶”多项式系统；(3)具有N进位字的符号系统。式(3.60)将多项式求解问题转化Dixon矩阵表示及线性方程求解问题。式(3.56)是Cayley-Bézout多项式结式(Resultant ofPolynomials)的推广(Generalization)。但是，由于式(3.67)中阶次高，易导致“组合爆炸”。

n个“n元N阶”多项式方程求解

考虑式(3.60)，若故得

Det(_SΘ_S(x₁))＝0。>

称式(3.68)中“n个n元”为Dixon消元必要条件，从而获得局部可行解(LocalFeasible solution)。若_SΘ_S存在零行或零列向量，则无法建立x₁的多项式方程；此时，通过除标量积之外的初等变换，将_SΘ_S变为行阶梯(Row>SΘ_S)；在计算该矩阵的杻轴(Pivot)的积之后得方阵即在_SΘ_S中选取S′个独立的列向量。

任一个n个“一元N阶”多项式系统的实例(简称多项式)记为其中：且有根据的多项式确定Dixon矩阵、分离变量及选取及满足

确定双线性型

其中：中与对应的各列线性独立。因由式(3.60)及式(3.63)得

称其为结式或消去式。式(3.72)是单变量x₁的多项式方程；消去了n-1个未知量；从而，可以获得单变量x₁的可行解。若x₁同时满足

则x₁为正确解。将已解的x₁代入式(3.74)，因式(3.72)成立且任意，故得

即有

若有必要条件

成立，解式(3.75)，得被消去变量的解；否则，需要结合式得到全部解。考虑式(3.54)，因式(3.60)两边的x₁阶次相等，故必有

若同时满足

则由式能解得中n-1个互不相同的组合变量；从而，得到所有独立变量的解。

给定n个“n元N阶”多项式Dixon矩阵计算步骤如下：

①确定系统结构：方程数及独立变量数记为n；独立变量记为X_n；多项式复合变量记为替换变量记为Y_n-1，替换变量数为n-1；大小为S·S的Dixon矩阵记为其成员系数如式(3.62)所示，其中：S由式(3.72)确定；待消去变量为x₁。

②由式(3.43)得x^α与对应关系，表达式(3.49)中至多有S项。

③根据式(3.57)及Sarrus规则，计算Dixon(F_n(Y_n-1|X_n-1))；根据对应的N进位字运算结果，完成多项式合并。

④Dixon矩阵成员如式(3.72)所示，由式(3.72)计算Dixon矩阵_SΘ_S的(n+1)·S²个系数。

⑤当满足式及式直接解判别准则时，由式及式得全部数值解。

示例3.7求多项式系统的解。

解：显然，由式可知，这是2个“2元1阶”多项式系统，满足Dixon消元条件。由式(3.57)及式(3.60)，得

其中：

由式(3.74)及式(3.80)得：将之代入式得τ_l＝0。显然，因变量满足式(3.64)，式(3.80)所示的Dixon矩阵对称。

示例3.8求多项式系统的解。

解：该是2个“2元1阶”多项式系统，满足Dixon消元条件。由式(3.57)及式(3.60)，得

其中：

由式(3.74)及式(3.80)得将之代入式(3.81)得τ_l＝1。

示例3.9对多项式系统进行Dixon消元。

解：式形式上表现为2阶多项式，但本质上是“多个3元”1阶多项式系统，满足Dixon消元条件。由式(3.57)及式(3.60)，得

其中：

由式(3.74)及式(3.84)得将它们代入式(3.83)得对应的显然，因变量满足式(3.64)，式(3.84)所示的Dixon矩阵是对称的。

示例3.10对多项式系统(3.85)进行Dixon消元。

解：该式是4个“4元1阶”多项式系统，满足Dixon消元条件。由式及式，得

其中：

由式及式得5个解：

其中：不是该方程组的解。将其它解分别代入式(3.75)。当时，由式得

解得：τ₃＝1，τ₄＝-2。将τ₃及τ₄代入式得τ₂＝1。同样，可得其他三组解。显然，因变量不满足式(3.64)，式(3.86)所示的Dixon矩阵不对称。该例表明Dixon行列式为零对于多重线性多项式系统是充分的。

示例3.11对多项式系统进行Dixon消元。

解：由式(3.87)可知，这是2个“2元2阶”多项式系统，满足Dixon消元条件。由式(3.57)及式(3.60)，得

其中：

由式(3.74)及式(3.88)得：τ₁＝-2，τ₁＝-1及τ₁＝3，分别代入式(3.87)得该方程组的解：

显然，因组合变量满足式(3.64)，式(3.88)所示的Dixon矩阵是对称的。

示例3.12计算多项式系统(3.89)的Dixon行列式。

解：由式(3.57)及式(3.60)得

其中：

取₆Θ₁₀(x₁)中关于x₁独立的6列向量组成则有

应用式(3.40)计算式(3.91)的行列式得

显然存在解代入式(3.89)得式(3.90)中的7至9列与中各列相关。尽管但是及不存在，因为将它们代入式(3.90)后，不能满足式(3.73)。故当|α|≠|β|不满足时，式(3.68)的解为可行解，需要代入系统方程验证是否正确。该例表明Dixon行列式为零对于一般的多项式系统是必要条件，但不是充分条件。

示例3.13计算多项式系统(3.92)的Dixon行列式。

解：由式(3.57)及式(3.60)，得

其中：

其中，对式(3.93)应用式(3.40)得

显然，存在解代入式得

示例3.14通过矩阵的初等行变换，得到式(3.86)的行阶梯矩阵。

解：rk代表第k行。由式(3.86)，得

则得

基于固定轴不变量的D-H系及D-H参数确定

在应用名义D-H系及D-H参数计算运动学逆解时，由于存在机加工及装配误差，导致系统绝对定位及定姿精度远低于系统的重复精度；同时，D-H系建立及D-H参数确定过程较烦琐，当系统自由度较高时，手工完成这一过程可靠性低。因此，需要解决由计算机完成D-H系及D-H参数的确定问题。同时，高精度的D-H系及D-H参数是机器人进行精确作业的基础，也是“示教-再现”(Teaching and Playback)机器人向自主机器人发展的基础。

基于固定轴不变量的D-H系确定

请参照图34。图34为自然坐标系与D-H系的关系图。图34包含D-H系原点O_l′、中间点轴不变量轴不变量如图34所示，A＝(0，1，…，k]，F＝{F^[l]|l∈A}，且有首先确定中间点及D-H系原点O_l′。

【1】令和z_l′分别经过轴不变量和轴不变量且

【2】定义为到n_l的公垂线，包括三种情况。

【2.1】若则⁰r_l′可用轴不变量和轴不变量表示。

因且得

解式(3.96)得

将式(3.97)带入式(3.94)得

且

对于自然坐标系，有

将式(3.100)，式(3.98)和式(3.99)改写为：

且

由式(3.100)，式(3.102)可以表示为如下形式

通常，用来表示的零位方向。

【2.2】显然，若且得到且

【2.3】若且则且

由式(3.105)和得

【3】得到

【4】由式，得

至此，通过固定轴不变量确定了D-H系。

基于固定轴不变量的D-H参数确定

请再度参照图34。如图34所示，是轴的单位坐标矢量。

令由第一部分轴扭角的定义得

令由第一部分关节转动角定义得

其中：a_l和c_l分别为轴到轴l的轴距和偏距，α_l为轴到轴l的扭角，为轴的零位。

综上所述，通过固定轴不变量和可以方便地表达D-H参数及同时可以表达零位

基于固定轴不变量的D-H系及D-H参数确定原理具有如下作用：

【1】解决了D-H系及D-H参数工程可实现性问题；由于D-H系及D-H参数确定过程需要借助于光学特征才能实施，所需的特征通常位于杆件内部及外部，工程上无法精确测量。而固定轴不变量可以借助于激光跟踪仪等光学测量设备间接地进行测量。

【2】保证了D-H系及D-H参数的精确性；由于D-H系及D-H参数确定过程需要满足正交性要求，工程上难以满足。在多轴系统设计时，根据图纸确定的D-H系及D-H参数与工程D-H系及D-H参数存在较大差异，需要考虑机械加工及系统装配引起的误差。通过工程测量的固定轴不变量在测量设备精度得到保证的前提下，可以得到精确的、以固定轴不变量表征的结构参数，从而保证了D-H系及D-H参数的精确性。

基于轴不变量及D-H参数的1R/2R/3R姿态逆解

在工程应用中，自然坐标系不仅简单、方便；而且有助于提高工程测量精度，增强建模的通用性。同时，多轴系统的运动学及动力学建模的困难主要是因为存在转动，而转动的描述的关键在于转动轴。本节基于自然坐标系，研究1R的投影、2R的定向及3R的定姿问题。主要目的是为后续阐述基于轴不变量的多轴系统逆运动学奠定基础。

基于轴不变量的1R姿态逆解

投影是旋转矢量在线性空间下的度量。给定链节控制关节角使固结的单位矢量^lu_S与期望单位矢量的投影最优；其中：为^lu_S与的夹角。称该问题为投影逆解问题。由式(2.83)及式(3.7)得

即

其中：

若解式(3.113)得

若式(3.113)退化为一次式：

由式(3.116)得

记式(3.115)根号部分

因τ_l是关于的连续函数。因故式(3.113)关于凸函数。当取边界条件时，由式(3.114)及式(3.118)得

由式(3.119)得

此时，满足最小的解为

基于轴不变量的CE3太阳翼姿态逆解

请参照图35。图35为月面巡视器太阳翼坐标系示意图。如图35所示CE3巡视器太阳翼(Sun panel)体系p，O_p位于转动副^cR_p轴线中心，x_p过转动副^cR_p并指向巡视器前向，y_p指向巡视器左侧，z_p由右手规则确定，即指向+Y光敏元件阵列法向。巡视器体系记为c。

其中：—太阳翼转动角，S_r—太阳翼前外侧点；S_l—太阳翼后外侧点；^cr_p—巡视器体系原点O_c至太阳翼体系原点O_p位置矢量在巡视器体系下坐标；—巡视器至太阳的单位矢量在导航系n下坐标。

由式(2.83)得

太阳翼上任一点S在其体系下坐标记为则有齐次坐标变换

记巡视器相对导航系的旋转变换阵为ⁿQ_c，则有ⁿQ_p＝ⁿQ_c·^cQ_p，故有

^cu_S＝^cQ_n·ⁿu_S，>

^pu_S＝^pQ_n·ⁿu_S。>

记器日矢量相对太阳翼高度角为其由式(3.125)确定

记由太阳翼法向至太阳单位矢量夹角记为则有

CE3巡视器的太阳翼控制包含两种模式：

①太阳翼调节控制

太阳翼调节控制是指：给定的最小阈值控制既要保证太阳翼产生足够的功率，又要保证太阳翼由于太阳辐射不致过热，即由式(3.115)或(3.117)可解得τ_p。显然，

②太阳翼最优控制

太阳翼最优控制是指：控制保证太阳翼最大的发电量。由式(3.121)可解得τ_p，显然，下面通过特例验证式(3.115)、式(3.117)及式(3.121)的正确性。若

^pu_S＝[0>T，^cn_p＝[1>T，>

将式(3.128)代入式(3.120)得

将式(3.128)代入式(3.121)得

当时，由式(3.129)得由式(3.130)得

当时，由式(3.129)得由式(3.130)得

当时，由式(3.129)得由式(3.130)得

显然，上述结果与直观的物理含义一致，证明了基于轴不变量的1R投影逆解原理的正确性。

由上述太阳翼逆解可知，存在两组最优解。由于太阳翼转动角度受结构约束、太阳翼温度约束、太阳翼与数传天线或全向天线可能存在机械干涉，需要对太阳翼工作区间进行限定。在允许的工作区间内控制太阳翼，保证发电量的最大化。

请参照图36。图36为全向天线与右翼、全向天线与太阳翼干涉图。图36包含测控站3601、数据接收站3602。如图36所示，因太阳翼距数传天线及全向天线较近，易遮挡电磁波的传输，致使数传通信或全向通信中断或功率衰减；称之为巡视器数传天线与太阳翼或全向天线与太阳翼机械干涉。避免机械干涉是巡视器任务规划、桅杆控制、太阳翼控制的基本约束条件。

请参照图36。判断巡视器数传天线与太阳翼或全向天线与太阳翼机械干涉的方法如下：记全向发射天线顶点为S_l>r>l>r>cn_t，参数为t，其对应的点记为^cr_t，在巡视器体系c下射线参数方程为

^cr_t＝^cr_A+^cn_t·t，>

即

记太阳翼前内侧角点为B，太阳翼法向为^cn_p，射线与太阳翼平面任一交点记为^cr_t。太阳翼平面方程为

(^cr_t-^cr_B)^T·^cn_p＝0，>

即

将式(3.133)代入式(3.131)得

式(3.135)中时，说明射线与太阳翼法向正交，显然不存在干涉，即

因将式(3.133)代入式(3.131)可得射线与太阳翼平面交点^cr_t。

若则检测射线与太阳翼干涉。当然，工程实现时，需要进行更多的射线检测，并考虑干涉阈度。

太阳翼行为控制通过3D场景显示，可以直观地反映“日地月”及地面站、巡视器姿态、太阳翼运动状态。不仅让用户准确地把握巡视器在轨时的情景状态，而且有助于提高软件的可靠性。在仿真测试时，可以用来分析探测区域、月面地貌、探测时间区间、太阳翼及左太阳翼发电性能等与月面巡视探测任务的适应性，可以优化巡视器电源系统的设计。

基于轴不变量及D-H参数的2R及3R姿态逆解

一方面，对于任一个杆件，D-H参数仅有3个结构参数及1个关节变量；有利于简化姿态逆运动学的消元过程。另一方面，D-H参数通常是名义的，难以得到准确的工程参数；需要通过固定轴不变量的精确测量，并通过计算获得相应的准确的D-H系及D-H参数。因此，基于轴不变量的2R指向与3R姿态问题可以转化为基于D-H参数的2R指向与3R姿态问题。

给定2R转动链由初始单位矢量指向期望单位矢量求及φ_l。这是定向逆解(Inverse>

若令D-H参数指标遵从子指标，因故用D-H参数表示得

由式(3.138)最后一行得

故有

即有

其中：

故有

由式(3.138)第一行得

故有

即

其中：

因式(3.140)及式(3.144)不一定满足式(3.138)的第2行，由式(3.140)及式(3.144)获得的及φ_l只是可能解；故需将可能解代入式(3.138)的第2行；若仍成立，才可得到真实解。

给定3R转动链及期望姿态轴不变量序列求关节角序列这是3R姿态逆解(Inverse solution of attitude)问题。

解：由式(3.140)及式(3.144)得由得故有

至此，解决了第2章基于笛卡尔轴链的姿态逆解方法缺乏通用性(Generality)的问题。由式(3.144)及式(3.146)可知可能存在两组解。

基于D-H参数的CE3数传机构指向逆解

请参照图37。图37为月面巡视器2DOF桅杆示意图。如图37所示，CE3巡视器的数传机构转动链为^cl_m＝(c，d，m]，轴不变量序列为[^cn_d，^dn_m]。地面数据接收站单位矢量为^cu_S。求其角度序列[φ_d，φ_m]。

如基于D-H参数的2R定向与3R姿态逆解节中所示，若经过精测获得轴不变量表达的结构参数

将式(3.147)代入式(3.111)及式(3.112)得桅杆D-H参数，如表3-1所示。

表3-1桅杆D-H参数

将表中参数分别代入式(3.141)及式(3.145)得

将式(3.150)代入式(3.142)得

将式(3.151)代入式(3.142)得

因需要代入式(3.138)的第2行检验才能得到真实解，故φ_l最多存在两组解。

考虑式(3.148)及式(3.149)，通过特例验证式(3.152)及式(3.153)的正确性：

上式的物理含义间接地表明第一部分原理的正确性。在数值计算时由于存在数字截断误差，可能导致无解；此时，需要将加上一个微小增量，重新再计算，以保证解的存在性。

CE3数传机构控制模块包含以下功能：桅杆正逆运动学计算、数传行为控制、任务规划系统内部通信、任务支持系统间通信、3D显示控制、调试接口、输入输出转换等功能。对桅杆进行控制时，首先检查测控可见性、太阳可见性、是否受太阳遮挡等基本约束条件。

基于轴不变量及D-H参数的3R机械臂位置逆解

由于D-H系是理想的坐标系统，需要作相邻两轴的公垂线，且三个坐标轴需两两正交。一方面，理想的正交在工程上不存在；公垂点在机械臂结构表面的可能性几乎为零。因缺乏高精度单位方向及公垂点的视觉特征，导致工程上无法精确确定D-H系及D-H参数。另一方面，传统的3R机械臂位置逆解通常根据理论D-H参数进行计算；或者以理论D-H为基础，应用激光跟踪仪的精测结果，对D-H参数进行优化，由于D-H参数与机械臂末端位置的强非线性，优化的效果十分有限，致使机械臂的绝对定位精度远低于相对定位精度，在精密机械臂应用中，难以实现机械臂的自主控制功能。

在第一部分中，根据精确测量的固定轴不变量可以唯一确定一组D-H系及D-H参数。由于精确测量的固定轴不变量既包含了机械加工误差又包含了装配误差，保障了确定的D-H系及D-H参数的精度，可以大幅度提升机械臂的绝对定位精度。在3R机械臂逆解过程中，尽管直接使用的是D-H参数，但本质上是依赖于固定轴不变量表征的结构参数。虽然文献给出了3R机械臂的逆解结果，但并未公布相应的证明过程，且求解过程不很准确。下面对之补充，以方便读者深入了解该原理及其工程应用。

基于D-H方法的3R机械臂位置逆解(Inverse Position Solution)过程如下：①应用固定轴不变量的精测原理，完成机械臂结构参数的精确测量；

②应用基于固定轴不变量的D-H系及D-H参数确定原理，自动建立D-H系及D-H参数；

③应用基于D-H参数的3R机械臂位置逆解为基础，计算3R机械臂逆解。

将3R机械臂轴链记为A＝(0,1:4]，固定于虚轴4的腕心记为C，运动副序列记为R＝(⁰R₁，¹R₂，²R₃，³V₄]，运动链记为^0′l_4′C＝(0′,1′:4′,4′C]，D-H系序列记为F＝(F^[0′]，F^[1′]，F^[2′]，F^[3′]]。记拾取点为C′，腕心期望位置及姿态四元数分别为

则有

该机械臂正运动学方程表示为

故位置方程为

式(3.156)是““个3元2阶”多项式系统。当给定及时，由式(3.155)得由式(3.157)解得逆解从而得^0′Q_3′；进而，由式(3.155)得应用第一部分姿态逆原理可解得因此，6R解耦机械臂逆解可以分解为3R位置逆解及3R姿态逆解两个子问题。因此，6R解耦机械臂由3R机械臂与3R解耦机构(Decoupled Mechanism)串接构成。从逆运动学角度看，6R解耦机械臂本质上是3R机械臂。

解耦机构的常见构型，RBR型最经典，其后三轴共点；BBR及3R型俗称偏置型，其后三轴的前两轴及后两轴分别共点。结构上的三轴及两轴共点是非常强的约束，需要精密加工及装配得以保证。

对于RBR型，给定拾取点P的期望位置^0′r_7′P及第6轴期望姿态^0′Q_6′，则腕心4′C的期望位置表示为

对于偏置型，给定P点期望位置^0′r_7′P、第6轴及第7轴的期望双矢量^0′|5′n_6′及^0′|6′n_7′，则腕心4′C的期望位置表示为

腕心D-H参数及基本关系

若最后一个运动轴为l，将腕关节轴矢量^3′n_4′的腕心记为C。令D-H参数指标遵从子指标，由第一部分D-H变换可知：

a_C＝0，>

故由式(3.160)得

由式(3.161)和式(3.162)得

基于D-H参数的3R机械臂位置逆解

因第4轴为虚轴，故3′C≡4′C。为简化3R位置逆解计算，令

^0′r_1′＝0₃，^0′Q_1′＝1，>

由式(3.165)，将式(3.157)表示为

将式(3.166)称为基于D-H系的3R机械臂的位置方程。应用式(3.160)、式(3.163)及式(3.164)，将式(3.166)表示为

式(3.166)或式(3.167)具有以下3个特点。

特点1：式(3.167)左侧包含φ₂和φ₃，且是[C(φ₁)，S(φ₁)]^T和[C(φ₃)，S(φ₃)]^T的多重线性型；因DCM矩阵的欧几里德范数恒为1，即与φ₂无关，故左侧欧几里德范数可以通过φ₃表示。因右式不包含φ₂和φ₃，其欧几里德范数可以由φ₁表示。故欧几里德范数等式表示为如下形式；

AC(φ₁)+BS(φ₁)+CC(φ₃)+DS(φ₃)+E＝0。>

其中：A，B，C，D，E由运动链的结构参数确定。式(3.166)中右式范数表示为：

即

其中：为续行符。式(3.167)左式范数为

即

由式(3.169)和式(3.170)得

即

对比式(3.171)和式(3.168)，得到衍生结构参数：

特点2：因式(3.167)的第三行无φ₂，得

其表示为如下形式：

FC(φ₁)+GS(φ₁)+HC(φ₃)+IS(φ₃)+J＝0；>

式(3.174)中衍生结构参数为

特点3：衍生结构参数间的基本关系如下：

联立式(3.168)和式(3.174)得

即

第一步：求第三轴显式解

由式(3.178)，得到第三轴的解，即由式(3.176)知

若Δ₁₁≠0，即(由式(3.176))得

即

利用C²(φ₁)+S²(φ₁)＝1简化式(3.180)，得到第三轴等式

即

将式(3.181)表示为

KC²(φ₃)+LS²(φ₃)+MC(φ₃)·S(φ₃)+NC(φ₃)+PS(φ₃)+Q＝0。>

式中衍生参数表示为：

利用式(3.176)简化式(3.183)得

式(3.183)即为

下面对式(3.182)进行求解。首先，将式(3.182)表示为

即

将式(3.186)表示为

其中，衍生参数为：

即

式(3.187)为以τ₃为项的四阶方程，其求解方法参见第0节，最多有4个解。

第一奇异

若Δ₁₁＝0，即则需考虑三种情况，将它们称作第一奇异。若表明腕心在第一轴上，τ₁任意；与上一节的第一奇异情况相同。

【1】若a₁＝0，μ₁≠0或由式(3.172)，A＝B＝0，式(3.168)表示为

CC(φ₃)+DS(φ₃)+E＝0。>

若C≠0，即式(3.189)可表示为得到

若C＝0，D≠0，利用式(3.189)得

若C≠0，D＝0则

φ₃＝±acos(E/C)。>

【2】a₁≠0，μ₁＝0，由式(3.175)，F＝G＝0，式(3.174)表示为

HC(φ₃)+IS(φ₃)+J＝0。>

若H≠0，则式(3.194)表示为可得

若H＝0，I≠0，则

【3】若a₁＝0且μ₁＝0，表明机械臂的第一轴与第二轴同轴；因无法计算φ₃，故无法确定φ₁。该结构设计存在问题。

第二步：求第一轴显式解

由式(3.168)及式(3.174)得

【1】若Δ₁₁＝AG-BF≠0，解式(3.198)得

因φ₃最多有4个解，由式(3.199)得[C(φ₁)，S(φ₁)]有对应4组解；又φ₁＝atan(S(φ₁)，C(φ₁))，故φ₁最多有4个解。

【2】若Δ₁₁＝CI-DH＝0，则将C(φ₃)和S(φ₃)分别代入式(3.168)式(3.174)求解。以代入式(3.174)为例，得

y_Cμ₁C(φ₁)-x_Cμ₁S(φ₁)+μ₂Z+W＝0，>

其中：

由式(3.202)及式(3.200)得

①若则

将式(3.204)两组解代入式(3.168)检验，一定仅有一组解满足式(3.168)。

②若W+μ₂·Z-y_C·μ₁＝0且x_C·μ₁≠0，则式(3.203)退化为一次式，即有

-2x_Cμ₁τ₁+(W+μ₂Z+y_Cμ₁)＝0；解得

③若W+μ₂·Z-y_C·μ₁＝0，x_C·μ₁＝0且W+μ₂Z+y_Cμ₁＝0，故有

φ₁＝(-π，π]。>

将以上三组解合写为

第三步：求第二轴显式解

令

利用式(3.167)的前两个方程组得

令

若Δ₂₂≠0，则

即有

φ₂＝atan(S(φ₂)，C(φ₂))。(3.212)

因φ₁及φ₃最多有4组解，且φ₂由φ₁及φ₃唯一确定，故φ₂至多有4个非奇异解。由式(3.187)、式(3.199)及式(3.212)可知：已获得[φ₁，φ₂，φ₃]的4组非奇异解。

显然，τ₂还存在其它求解方法。例如，当φ₁及φ₃求解后，将式(3.167)中第1式或第2式分别转化为τ₂的二次式，分别求解可得2组不同的局部可行非奇异解；故[φ₁，φ₂，φ₃]存在16组局部非奇异可行解；但它们需要同时满足式(3.167)三个等式，由式(3.209)获得的4组解是16个局部可行解中同时满足第1式及第2式的非奇异解。故有3R位置逆解结论：3R机械臂位置逆存在16个局部可行非奇异解及4组非奇异解。

第二奇异

若Δ₂₂＝0，即A₁₁＝0且A₁₂＝0；将该情形称为第二奇异。若λ₂≠0，由式(3.208)得

【1】若

即

a₃＝c_C·μ₃＝0。>

此时φ₃解存在，由式(3.158)，a_C＝0,a₃＝μ₃＝0且c_C＝0，即腕心在第三轴上。该情况下，第三轴的旋转无法控制机械臂腕心位置，表示该结构设计错误。

【2】若λ₂＝0，由式(3.208)，得A₁₂＝0，则式(3.209)表示为

得

基于D-H参数的3R机械臂工作空间

3R机械臂工作空间Ω是指机械臂腕心位置[x_C,y_C,z_C]构成的空间，因角序列[φ₁,φ₂,φ₃]及运动学方程连续，故Ω的最大包络Env连续且满足：

式(3.218)表示Env上的点[x_C,y_C,z_C]与任一点[x,y,z]的直线距离要么极大要么极小，即Env是Ω的凸包(Convex>1,φ₂,φ₃]范围不受限时的凸包计算问题。

由式(3.171)得

由式(3.219)得

由式(3.220)可知：当给定φ₁时，左式存在关于τ₃的极大与极小值。由式(3.218)及式(3.220)得

若令

则有

欲使τ₃存在，则val必须满足

4·val·(val-a₂a₃+c₂c_Cμ₂μ₃)-(a₂c_Cμ₃+c₂a₃μ₂)²≤0。>

由式(3.223)及式(3.224)得

其中：

由式(3.221)及式(3.222)得

因此，由式(3.225)可得式(3.226)的极大与极小值；过[a₁C(φ₁),a₁S(φ₁),c₁]的凸包截面为同心球环；[a₁C(φ₁),a₁S(φ₁),c₁]自身是个圆环；故式(3.226)所示的凸包是同心球环。

基于D-H参数的CE3机械臂位置逆解示例

CE3机械臂由底座(base)、肩(should)、臂(arm)、腕(wrist)及X光谱仪组成。记该机械臂轴链为A＝(b，s，a，w，C]。请参照图38，图38为CE3巡视器机械臂D-H系图。图38包含第一根轴s/1、第二根轴a/2、第三根轴w/3、虚轴C/4。

CE3月面巡视器机械臂名义D-H参数如表3-2所示。

在D-H参数中，结构参数a_l、c_l及α_l与零位时的自然关节坐标相对应。自然关节坐标即为关节坐标θ_l；故有

从而应用第一部分基于轴不变量的D-H参数确定方法获得工程D-H参数，如表3-3所示。

该机械臂腕心C为X光谱仪中心轴与其探测面交点。应用第二部分基于轴不变量的结构参数测量方法，通过激光跟踪仪测量得到轴不变量序列其中：

^cn₁＝[0.01898,0.01548,0.99970]^T，¹n₂＝[0.99983,0.00998,0.01550]^T，

²n₃＝[0.99991,0.00988,0.00908]^T，³n_C＝[0.01484,-0.99980,0.01341]^T，

^cr₁＝[0.5330,0.0386,-0.1461]^Tm，¹r₂＝[0.4227,0.0013,-0.2241]^Tm，

表3-2 CE3机械臂名义D-H参数

表3-3基于轴不变量的工程D-H参数

机械臂正逆运动学计算结果如表3-4所示，正逆互验精度为千分之一度，即3.6"；因此，运动学计算误差可以忽略不计。CE3巡视器机械臂由地面遥操作控制，由于被探测地面及岩石的非结构化，需要保证机械臂的重复定位精度达0.2mm与绝对定位精度达0.3mm。被探测地面及岩石的3D场景由巡视器避障双目立体视觉获得，场景的图像传输至控制中心进行三维重构，重构精度约为2mm。X光谱仪敏感面与被探测对象平均距离为7mm。故被测对象与X光谱仪探测面最小距度为3.5mm，在环境重构误差及机械臂绝对定位误差内，能够保证X光谱仪有效探测。

表3-4 CE3机械臂正逆运动学互验

经系统测试，机械臂重复定位精度为0.2mm；应用名义D-H参数计算的定位精度为0.86mm；应用工程D-H参数的定位精度提高至0.25mm。测试表明：基于轴不变量的工程D-H参数可以较大幅度地提高机械臂的绝对定位精度，有助于机械臂自主能力的提高。根据环境重构的地形及巡视器各部件的三角面，应用AABB碰撞检测技术，实现CE3机械臂碰撞检测功能。为了指导操作人员确定可行的探测区域，需要绘制机械臂工作空间与探测空间。工作空间是指机械臂拾取点可以接触位置的集合，探测空间是工作空间与地形表面的交集。

机械臂携带X光谱仪进行探测，还需要满足其它条件：比如，光照是否适宜、探测区域是否平整等。当操作人员在探测空间内点击鼠标时，系统会自动评估该区域是否可以探测并提示该局部区域的相关信息。操作人员选择期望的探测区域后，执行机械臂的运动规划模块，规划机械臂展开的运动序列。

基于轴不变量的通用机械臂逆运动学

自主机器人研究的一个重要方面是需要解决变拓扑结构机器人的运动学建模问题。在MAS中，具有动态的图结构(Dynamic Graph Structure)，可以动态地建立基于运动轴的有向Span树，为研究可变拓扑结构(Variable Topology Structure)的机器人建模与控制奠定了基础。为此，需要提出基于轴不变量的通用机械臂逆解原理，既要建立包含坐标系、极性、结构参数、关节变量的完全参数化的正运动学模型，又要实时地计算位姿方程；一方面，可以提高机器人的自主性，另一方面，可以提高机器人位姿控制的绝对精度。

6R解耦机械臂在结构上存在共点约束：要么4至6轴共点，要么4轴与5轴共点且5轴与6轴共点。对于高精度的机械臂而言，由于存在机加工及装配误差，该假设不成立。由于通用6R机械臂不存在共点约束，其逆解计算十分困难，在工程上不得不屈从于解耦约束，该约束既增加了机械臂加工及装配难度，又降低了机械臂绝对定位精度。只有突破通用6R机械臂的逆解原理，才能满足机械臂进行精密作业的需求，自主机器人理论才能得到完善。

对于6R通用机械臂，需要解决分析逆解(Analytical Inverse Solution)的可操作性(Operability)问题：一方面，通过固定轴不变量表征工程结构参数，保证多轴系统的绝对定位精度；另一方面，需要解决运动方程的降维问题及应用变量消元法(VariableElimination Method)进行逆解的可计算性(Computability)问题。

自然空间的平动轴及转动轴数各有3个，其中平动轴可以由转动轴替代。记6R运动链中的平动轴及转动轴分别为显然，可以将该运动链划分为三大类别：纯平动类(3种)、纯转动类(6种)以及既有转动又有平动的复合类(12种)，合计21种。其中，3种纯平动链是平凡运动学问题，不需讨论。因此，非平凡轴链运动学逆解(IK Solutions of NontrivialAC)的存在条件(Existence Conditions)为：

当|ⁱl_n|＝6时，则要求即至少需要3个转动副才能满足位姿对齐需求。

人工推导6R运动链的运动学方程非常繁琐且易出错，难以保证建模的可靠性；一方面，需要建立迭代式的方程，以满足计算机自动建立多轴系统符号模型(SymbolicModels)的需求；另一方面，需要应用更少轴数的运动链进行等价。运动学方程存在很多等价的形式，只有特定结构的运动学方程才具有逆解的可行性，既要求正运动学方程具有阶次最小、方程数最少及独立变量数最少；又要求逆解过程不存在数值计算导致的奇异性。

因自然空间的位姿有6个维度，故要建立仅含6个关节变量的6个位姿方程。显然，基于欧拉四元数或对偶四元数的位姿方程不满足方程数最小的要求；尽管式(3.1)及式(3.2)是2阶多项式系统，但是方程仅有6个。包含平动及转动的运动矢量本质上是自然螺旋，机械臂的最后一轴总要与期望方向对齐，才能执行所需的操作；在前5轴控制第6轴与期望的位置及指向的对齐之后，再控制第6轴满足径向对齐；因此，对于通用6R机械臂，只需要建立包含前5个关节变量的位姿方程。

考虑运动链ⁱl_n，式(3.1)及式(3.2)是关于Cayley参数或Gibbs矢量的方程。为此，提出“居-吉布斯”(Gibbs)姿态四元数，目的是：通过前5轴完成对齐，以消去第4轴及第5轴的关节变量，为后续的逆解奠定基础。

“居-吉布斯”四元数与类DCM矩阵

考虑运动链ⁱl_n，一方面，式(3.2)定位方程是Gibbs矢量方程；另一方面，正余弦的三角函数可以转化为相应Gibbs参量的方程。为此，提出“居-吉布斯”(Gibbs)姿态四元数；从而，将位姿逆运动学问题转化为Gibbs参量的多项式方程求解的问题。

【1】“居-吉布斯”四元数

首先，定义与欧拉四元数同构的“居-吉布斯”规范四元数

其中：为Gibbs矢量。由式(2.115)得Gibbs共轭四元数

其中：

显然，为模的平方。由式(2.124)得Ju-Gibbs四元数的积仍为Ju-Gibbs四元数。因为Ju-Gibbs四元数是四元数，满足四元数乘法运算

其中：

由式(3.233)得

习惯上，单关节及运动链的期望姿态以规范的即标部为1的Ju-Gibbs四元数表示，由它们积运算表达的Ju-Gibbs四元数通常是不规范的，即其标部不是规一化的。由式(3.234)可知：给定仅当轴l及的规范Ju-Gibbs四元数，且两轴正交时，为Ju-Gibbs规范四元数。

由式(3.234)得

证明：由式(3.234)得

或由四维复数性质得

证毕。

记为单位Ju-Gibbs四元数，由式(3.233)得

故为单位Ju-Gibbs四元数。

由式(3.229)、式(3.230)及式(3.236)得

证明：由式(3.234)得

故式(3.238)成立。证毕。

由式(3.231)、式(3.238)及式(3.235)得

【2】类DCM及性质

由式(3.1)得

其中：

由式(3.241)可知：ⁱQ_n及是关于τ_k的n重2阶多项式。显然，为模的平方，由式(3.240)可知：与类似，故称之为Gibbs类DCM(quasi-DCM)。由式(3.242)得

显然，类DCM可以通过Ju-Gibbs四元数表达。故式(3.1)姿态方程及式(3.2)位置方程本质上是关于Ju-Gibbs四元数的表达式。根据的二阶幂零特性，易得

由式(3.244)得：

【3】分块方阵的逆

若给定可逆方阵K、B及C，且

则有

证明：记

则有

故有

将式代入式得故得

将之代入式得

将式代入式得

将之代入式得

证毕。

基于轴不变量的6R解耦机械臂指向逆解原理

6R解耦机械臂由3R机械臂及3R解耦机构组成。6R解耦机构姿态逆解是指：给定6R串接运动链结构参数、期望姿态及前3个关节的姿态，计算第4轴及第5轴的关节变量，先对齐第5轴的指向，后对齐第6轴的径向，即实现姿态对齐。

下面，证明指向对齐时的Ju-Gibbs四元数的存在性。称定理3.3为Ju-Gibbs四元数指向对齐定理。

【1】基于Ju-Gibbs四元数的指向对齐原理

定理3.3考虑轴链若使单位轴矢量与期望单位轴矢量对齐，则至少存在一个多轴旋转的Ju-Gibbs四元数

其中

证明：由式(3.71),得

由式(3.251)得

且

因轴矢量与期望矢量是单位矢量，假定且得

式表明和相互正交。由式(3.253)和得到最佳的轴矢量

且

由式(3.255)和(3.256)得，若或由式(3.251)得

由式得

因由式(3.258)得

■

且

由式(3.259)及式(3.260)分别得式(3.249)及式(3.250)。证毕。

定理3.3表明：至少存在一个期望的Ju-Gibbs四元数使单位矢量与期望单位矢量对齐。

示例3.15考虑轴链ⁱl₆，由定理3.3得

【2】基于Ju-Gibbs四元数的2R机械人腕关节指向逆解原理

以Ju-Gibbs四元数指向对齐为基础，阐述6R解耦机械臂指向逆解定理，再予以证明。

定理3.4若给定6R转动链ⁱl₆＝(i,1:6]，记第5轴关节Ju-Gibbs四元数期望为及第3轴关节Ju-Gibbs规范四元数为则有指向对齐时的逆解

其中：

Ju-Gibbs方向四元数满足

证明：首先考虑基于欧拉四元数的姿态对齐，由式(2.145)得

由式(3.266)得

其中：

由式(3.246)及式(3.268)得

由式(3.267)得

其中：

由式(3.269)及式(3.270)得

由式(3.272)得

若式(3.272)与式(3.273)两边对应相除得

其次，考虑Ju-Gibbs四元数的指向对齐。因故得。由式(3.239)得

以规范Ju-Gibbs四元数表征关节变量，由式(3.234)得

由式(3.276)得

因³n₄及⁴n₅独立，由式(3.264)可知，³E₅必存在。显然，由³n₄及⁴n₅唯一确定。将式(3.276)、式(3.277)及式(3.269)代入式(3.275)得

若由式(3.278)第1行得

将式(3.279)代入式(3.278)得

由式(3.274)及式(3.280)可知两种原理等价。由式(3.280)第2、3行得

由式(3.274)可知式(3.261)成立。因式(3.280)存在4个等式，2个独立变量，由式(3.281)及式(3.280)中第4行得约束方程

若由式(3.273)得C₄C₅＝0；由式(3.267)得

由式(3.283)得

显然，当时，若由式(3.282)得

若由式(3.282)得

由式(3.286)及式(3.281)可知，式(3.262)成立。

当时，若则式(3.261)亦成立。

为了保证指向对齐，当时，³n₄或⁴n₅的方向需要和保持一致。显然,当且由式(3.285)得

因此，式(3.262)成立。

Ju-Gibbs方向四元数

由式(3.261),(3.262)和(3.279)得

且

把代入以上两个方程中得到式(3.265)，它显示的是特定的Ju-Gibbs四元数，称为Ju-Gibbs方向四元数。

■

示例3.16继示例3.15，考虑轴链ⁱl₆，且有³n₄＝1^[x]，⁴n₅＝1^[y]，由式得³E₅＝1。由式(3.262)得

定理3.3表明DCM至少存在一个与之等价的Ju-Gibbs四元数；定理3.4证明过程表明Ju-Gibbs四元数与欧拉四元数同构；同时，式(3.243)表明以Ju-Gibbs四元数表示的类DCM与DCM同构。因此，应用Ju-Gibbs四元数可以完整表达位姿关系。

若给定运动链ⁱl_n，k∈ⁱl_n，期望规范Ju-Gibbs四元数及期望位置矢量考虑式(3.2)及式；则位置及指向对齐关系表示为

且具有模不变性

与欧拉四元数及对偶四元数相比，Ju-Gibbs四元数表征的位姿对齐不存在冗余方程；通过指向对齐，可以求解第4轴及第5轴的关节变量，为6R及7R机械臂逆解奠定了基础。

基于轴不变量的运动链Dixon行列式

本节基于轴不变量提出径向不变量及运动链Dixon行列式性质，为基于轴不变量的机器人逆运动学分析与解算奠定基础。

【1】轴不变量

首先，轴不变量与坐标轴具有本质区别：坐标轴是具有零位及单位刻度的参考方向，可以描述沿该方向平动的位置，但不能完整描述绕该方向的转动角度，因为坐标轴自身不具有径向参考方向，即不存在表征转动的零位。在实际应用时，需要补充该轴的径向参考。坐标轴自身是1D的，3个正交的1D参考轴构成3D的笛卡尔标架；轴不变量是3D的空间单位参考轴，具有径向参考轴，即参考零位。空间坐标轴及其径向参考轴确定相应的笛卡尔系。空间坐标轴可以反映运动轴及测量轴的三个基本参考属性。

其次，轴不变量与欧拉轴具有本质区别：旋转变换阵是实矩阵，轴矢量是特征值1对应的特征矢量，是不变量；轴不变量是3D参考轴，不仅具有轴向参考方向，也具有径向参考方向即零位；在自然坐标系下，轴不变量不依赖地相邻固结的自然坐标，即在相邻固结的自然坐标下具有不变的自然坐标，且有非常优良的数学操作性能；在自然坐标系统中，通过轴矢量及关节坐标，可以直接描述旋转坐标阵；没有必要为每一个杆件建立各自的体系，可以极大地简化建模的工作量。

同时，以唯一需要定义的根坐标系为参考，测量轴不变量，可以提高结构参数的测量精度。基于轴矢量的优良操作，可以建立包含拓扑结构、坐标系、极性、结构参量及动力学参量的统一的多轴系统运动学及动力学方程。

由式(3.1)及式(3.2)可知：多轴系统的姿态及位置方程本质上是多元多项式方程，其逆解本质上归结于多元多项式消元问题，包含Dixon矩阵及Dixon行列式计算两个子问题。由式(3.2)表征3R机械臂位置方程，是“3个3元2阶”多项式。应用Dixon消元方法计算逆解，有两个替换变量；在计算8×8的Dixon行列式时，最大可能的阶次为16。由式(3.39)可知：行列式计算是一个排列过程，面临着“组合爆炸”的难题。

多元多项式消元过程通常被认为是NP问题。所有的非确定性多项式时间可解的判定问题构成NP类问题。非确定性算法将问题分解成猜测和验证两个阶段：算法的猜测阶段具有非确定性，算法的验证阶段具有确定性，通过验证确定猜测解是否正确。假如可以在多项式时间内计算出来，就称为多项式非确定性问题。通常应用基进行多元多项式消元，不得不求助于启发式的猜测和验证来解决问题。

由式(3.293)及式(3.294)可知，任意三个结构参数矢量对同一个轴的零位不变量或径向不变量的行列式为零；任意两个参数矢量对同一个轴的轴向不变量的行列式为零故零位。由轴不变量导出的不变量可以简化Dixon矩阵行列式计算。在应用Dixon消元时，Mathematica及Maple等软件不知道如何应用D-H运动链的域知识处理严重的组合爆炸问题。

【2】径向不变量

结构参数及是链节l的结构参量，它们在系统零位时可以测量得到。在第二部分阐述了依赖于轴不变量的零位矢量、径向矢量及轴向矢量，请参照图39，图39为轴不变量的导出不变量图，如图39所示，它们是与转动角无关的不变量。其中，零位矢量3901是特定的径向矢量3902。

任一个矢量可以分解为零位矢量3901及轴向矢量3903，故有

其中：

考虑链节其D-H参数有

显然，是轴l及的公垂线或公共径向矢量，是轴l的轴向矢量。由式(3.289)可知：任一个结构参数矢量可分解为与坐标系为无关的零位不变量及轴向不变量记任一结构参数矢量由轴不变量导出的径向矢量为结构参数矢量及轴不变量唯一确定径向坐标系，具有2个独立维度。若由轴不变量导出的两个轴向不变量及共线，则记为

若由导出的两个零位不变量及与任两个径向不变量及共面，则记为

因此，称式(3.290)所示的轴向不变量及零位不变量是结构参数矢量对自然轴的分解。

由轴不变量导出的零位矢量、径向矢量及轴向矢量具有以下关系：

证明：由式(3.290)得

式(3.295)得证。由式(3.290)得

式(3.296)得证。由式(3.290)得

式(3.297)得证。

称式(3.295)为零位矢量的反转公式；称式(3.296)为零位矢量与径向矢量的互换公式；称式(3.297)为径向矢量不变性公式。由式(3.289)、式(3.295)至式(3.297)得

由式(3.298)得

因是对称部分的结构常数，故称式(3.298)为矢量的对称分解。因是反对称部分的结构常数，故称式(3.299)为矢量的反对称分解。称式(3.300)为归零等式。

【3】运动链Dixon行列式性质

定义

由式(3.233)得

其中：

考虑式(3.302)，若M为4·4的矩阵，则有

且有

由式(3.242)及式(3.290)得

由式(3.306)易证得

由式(3.307)得

式(3.307)可以将及可以转化为关于的多重线性型。同时，对y_l及τ_l具有对称(轮换)性。由式(3.291)、式(3.298)及式(3.299)得

式(3.309)由三个导出的独立结构参量及一个运动变量τ_l构成。由式(3.309)得

由式(3.307)及式(3.311)得

由式(3.307)及式(3.312)得

显然，直接计算比应用式(3.313)计算要复杂得多。因行列式三列不同时存在故式(3.313)中y₂的阶次为3，τ₂及τ₃的阶次为3，y₃的阶次为1。

【4】基于类DCM的2R方向逆解

对于2R轴链实现方向对齐，因Ju-Gibbs方向四元数不能全部表示，故定理3.4不能获得2R方向对齐时的全部逆解，需要借助于类DCM才能解决这一问题，其表述为定理3.5。

定理3.5给定6R轴链ⁱl₆＝(i,1:6]，轴矢量³n₄及⁴n₅，期望第5轴的DCM为期望第3轴的DCM为方向矢量⁵l₆与期望方向对齐的逆解需要满足以下方程：

证明：方向矢量⁵l₆与期望方向对齐需满足

由式(3.240)得

即

式(3.317)被重新表达为式(3.314)。

基于轴不变量的3R机械臂位置逆解原理与推广

3R机械臂位置逆解是指：给定3R串接机械臂结构参数及期望位置时，计算3个关节角，使串链位置与期望位置对齐的过程。第一部分阐述了基于D-H参数的3R机械臂位置逆解问题，但存在如下缺点：建立D-H系及D-H参数的过程不自然，应用繁琐；需要处理计算方法导致的奇异性问题；在应用时，易引入系统测量误差；该方法难以推广至通用6R机械臂的逆解问题。研究基于轴不变量的3R机械臂位置逆解原理，目的是阐述应用Dixon消元原理实现通用机械臂位姿逆解的步骤。

【1】根据n元3D矢量位姿方程，获得n个“元2阶”多项式方程。

由式(3.2)得3R运动学方程

由式(3.318)得

由(3.319)式得

若记

则由式(3.241)及式得(3.321)

由式(3.320)及式(3.321)得

下面，通过定理3.5表述3R机械臂运动学多项式系统的Dixon行列式结构特点，并予以证明。称该定理为3R机械臂Dixon行列式结构定理。

定理3.6：由式(3.323得3R运动学多项式方程

多项式系统F₃(Y₂|T₂)；则有

其中：

证明：由式及式(3.324)得

由式及式(3.328)可知式(3.326)成立。由式(3.307)及式(3.321)得

由式(3.321)、式(3.329)及式(3.330)得

其中：应用式(3.313)计算

显然，式(3.331)中的y₂阶次β2∈[0:3]及y₃阶次β3∈[0:1]。考虑式(3.328)后三项：中的y₂阶次β2∈[0:3]及y₃阶次β3∈[0:1]；中的y₂阶次β2∈[0:2]及y₃阶次β3∈[0:1]；中的y₂的阶次β2∈[0:3]及y₃的阶次β3∈[0:1]。由上可知：式(3.328)中的y₂阶次β2∈[0:3]及y₃的阶次β3∈[0:1]。故有S＝8。

由式(3.321)、式(3.328)至式(3.331)可知：中组合变量系数为独立的列向量，故选取的系数构成方阵剩余列向量一定与各列相关。故式(3.327)成立。证毕。

【2】应用“基于轴不变量的Dixon行列式计算”方法，“分块矩阵的高维行列式计算”方法或者“对行列式进行行阶梯化计算”方法简化行列式计算。

由式(3.307)及式(3.321)得

由式(3.332)得

由式(3.333)得

由式(3.334)得

由式(3.328)得

将式(3.335)至式(3.337)代入式(3.338)得

【3】应用n个“用元N阶”多项式的Dixon消元与求解原理完成位姿逆解计算，其中：根据Dixon矩阵的行列式为0，得到一元高阶多项式方程，应用基于友阵的一元高阶多项式方程求解一元高阶多项式方程的解。

由式(3.68)、式(3.326)及式(3.327)得

因S＝8，应用式计算的复杂度为8·8！＝322560；应用式，执行二次分块行列式计算，其中：2·2分块矩阵计算复杂度为4！(2·2！+2·2！+1)/(2！2！)＝30，4·4分块为矩阵计算复杂度为8！(30+30+1)/(4！4！)＝4270。一般情况下，式(3.340)是关于τ₁的16阶单项式方程。应用数值法解得第一轴可行解将第一轴可行解分别代入式得对应的第二、三轴可行解。将可行解分别代入式(3.324)验证，若成立，则为正确解。

基于轴不变量的3R机械臂位置逆解原理具有如下作用：由于轴不变量可以精确测量，有助于提高机械臂的绝对定位精度；由于关节范围覆盖完整的一周，消除了D-H计算原理导致的奇异性；与D-H方法相比，求解过程具有通用性，可以获得系统全部逆解。该原理证明过程表明：整体与局部、复杂与简单是对立统一的关系；式(3.39)将矢量多项式的行列式计算转化为三个矢量的行列式，在该原理证明当中起到了决定性的作用；轴不变量及其导出不变量有助于理解行列式及该原理的内涵。

基于轴不变量的通用6R机械臂位姿逆解

当自由度较高时，人工推导运动学方程非常繁杂且易出错，难以保证建模的可靠性。需要建立迭代式形式，以满足计算机自动建立多轴系统符号模型(Symbolic Models)的需求；同时，需要应用系统内在规律，简化系统方程。相同的系统可能存在不同形式的方程，它们计算复杂度、物理内含、系统阶次及奇异性等也不尽相同。当然，理想的系统模型需要满足最小阶次及无计算奇异性的应用需求。运动学方程存在很多等价的形式，只有特定结构的运动学方程是高自由度多轴系统逆运动学求解的基础。

6R解耦机械臂在结构上存在以下共点约束：要么4至6轴共点，要么4轴与5轴共点且5轴与6轴共点。对于高精度的机械臂而言，由于存在机加工及装配误差，该假设不成立。通用6R机械臂不存在共点约束，但逆解计算非常困难，在工程上不得不屈从于解耦约束。

【1】基于轴不变量的通用6R机械臂位姿逆解预备定理

本节研究拾取点位于第6轴上的通用6R机械臂逆解问题，该机械臂特点在于：给定期望位置及期望姿态的逆解问题与给定期望位置及期望姿态的逆解问题等价；第5轴实现定点指向对齐，第6轴实现无限转动。下面先阐述通用6R机械臂逆解预备定理，再予以证明。

定理3.7：若给定6R轴链ⁱl₆＝(i，1∶6]，ⁱl₁＝0₃，期望位置矢量及Ju-Gibbs四元数分别为及则由轴不变量表征的6R机械臂运动学多项式方程为：

其中：

系统结构参数及期望Ju-Gibbs姿态四元数构成的矩阵表示为

证明：期望与姿态对齐，由式(3.287)得进而，由式(3.239)得

由式(3.233)得式(3.342)，由式(3.347)得

由式(3.234)得

其中：

由式(3.348)及式(3.349)得

由式(3.246)得

将式(3.352)代入式(3.348)得

其中：

由式(3.354)得式(3.344)。由式(3.344)及式(3.353)得

式(3.355)是关于期望姿态及4轴、5轴结构参数的约束方程。由式(3.242)得

一方面，由式(3.355)、式(3.356)及式(3.357)得

另一方面，由式(3.347)、(3.355)及式(3.358)得

故得

由式(3.356)得(3.346)及

由式(3.355)及式(3.346)得

式(3.355)至式(3.361)用于后续方程简化，C是结构常数矩阵。考虑式(3.353)两边2范数得

考虑当ⁱl₁＝0₃时的位置矢量对齐关系得

由式(3.2)及式(3.364)得

进而，得

即有

显然，有

由式(3290)、式(3.347)及式(3.366)得式(3.365)之左式

由式(3.290)、式(3.356)、式(3.361)及式(3.359)得

由式(3.361)及式(3.368)得

由式(3.355)、式(3.361)及式(3.369)得

将式(3.367)及式(3.370)代入(3.365)，且消去两边的得式(3.341)。证毕。

定理3.7作用在于：消去τ₄及τ₅后的位置方程(3.341)是3个“3元2阶”多项式方程，为实时计算通用6R轴机械臂逆解奠定了基础。一方面，将有利于提高6R机械臂的绝对定位精度；另一方面，使传统解耦机械臂的第4轴及第5轴向根方向移动，不仅有利于6R机械臂结构的优化，而且有利于提高6R机械臂避让障碍的灵活性。

【2】基于轴不变量的通用6R机械臂Dixon矩阵结构

下面，以定理3.7的通用6R机械臂运动学方程为基础，证明定理3.8，阐述该方程的Dixon矩阵的结构特点。

定理3.8：若给定6R轴链ⁱl₆＝(i，1：6]，ⁱl₁＝0₃；期望位置矢量及Ju-Gibbs四元数分别为及则式(3.341)构成多项式系统F₃(Y₂|T₂)的Dixon矩阵具有如下结构：

其中：

证明：记

由式(3.341)、式(3.374)至式(3.375)得

由式(3.376)得

其中：由式(3.304)及式(3.374)得

由式(3.305)及式(3.374)得

由式(3.304)、式(3.305)及式(3.375)得

由式(3.378)至式(3.381)可知：是关于y₂的1阶及y₃的0阶多项式；关于y₂的2阶及y₃的1阶多项式。同时，因(3.376)是矢量多项式，当任意两列的结构参数矢量相同时，对应的行列式为零，故式(3.371)是关于y₂的3阶及y₃的1阶多项式，故式(3.373)成立。又因式(3.376)是关于τ₁的2阶多项式，故式(3.372)成立。

【3】基于轴不变量的通用6R机械臂逆解示例

根据定理3.5，3.7及3.8开发通用6R机械臂逆解软件，在主频为2.8G的笔记本电脑上运行。下面逆解示例求解时间小于700ms，逆解个数取决于机械臂结构的对称性，但至多有16组解。

示例4.17 6R机械臂的结构参数如下：ⁱn₁＝1^[z]，¹n₂＝1^[y]，²n₃＝1^[y]，³n₄＝1^[x]，⁴n₅＝1^[y]，⁵n₆＝1^[x]；ⁱl₁＝0₃m，若给定期望位置及期望方向，则存在如下8组逆解：

φ_[1][*]＝[-76.69657,170.546093,-20,33.69583,-16.915188]Deg，

φ_[2][*]＝[-76.69657,170.546093,-20,-146.30417,16.915188]Deg，

φ_[3][*]＝[-76.69657,150,20,-16.44416,-34.76538]Deg，

φ_[4][*]＝[-76.69657,150,20,-163.55584,34.76538]Deg，

φ_[5][*]＝[-90,30,-20,30,40]Deg，φ_[6][*]＝[-90,30,-20,-150,-40]Deg，

φ_[7][*]＝[90,9.4539,-20,-130.008225,-24.80936]Deg，

φ_[8][*]＝[90,9.4539,-20,49.99178,24.80936]Deg。

示例3.18 6R机械臂的结构参数如下：ⁱn₁＝1^[z]，¹n₂＝1^[y]，²n₃＝1^[y]，³n₄＝1^[x]，⁴n₅＝1^[y]，⁵n₆＝1^[x]；ⁱl₁＝0₃m，⁵l₆＝0₃m。(a)若给定期望位置及期望方向则仅存在一组解φ_[1][*]＝[90,30,-20,30,40]Deg。(b)若给定期望位置及期望方向则存在如下两组逆解：

φ_[1][*]＝[90,30,-20,0,0]Deg>[2][*]＝[90,9.45391,20,0,-19.45391]Deg。

通用6R机械臂的实时逆解作用在于：不仅有助于提高机械臂的绝对定位精度，而且可以进一步优化机械臂的结构，降低系统的重量。

基于轴不变量的通用7R机械臂位姿逆解

由于7R通用机械臂第5轴实现定点指向对齐及第7轴实现无限转动，第6轴可以用于避碰调节，具有类人手臂的结构。当拾取点位于上时，前6轴实现位姿对齐，第7轴极可以无限转动。因此，7R通用机械臂比6R机械臂具有更强的空间作业灵活性。

本节研究名义拾取点位于第7轴上的通用7R机械臂逆解问题，该机械臂特点在于：给定期望位置及期望姿态的逆解问题与给定期望位置及期望姿态的逆解问题等价，即7R机械臂本质上是6R轴链问题。下面，先阐述通用7R机械臂逆解预备定理，再予以证明。

定理3.9：若给定7R轴链ⁱl₇＝(i，1：7]，ⁱl₁＝0₃，期望位置矢量及Ju-Gibbs四元数分别为及则由轴不变量表征的7R机械臂运动学多项式方程为：

其中：

系统结构参数及期望Ju-Gibbs姿态四元数构成的矩阵表示为

证明：期望与姿态对齐，由式(3.287)得进而，由式(3.239)得

由式(3.233)得式(3.383)，由式(3.388)得

由式(3.234)得

其中：

由式(3.389)及式(3.390)得

由式(3.246)得

将式(3.393)代入式(3.392)得

其中：

由式(3.395)得式(3.385)。由式(3.385)及式(3.394)得

式(3.396)是关于姿态及5轴、6轴结构参数的约束方程。由式(3.385)得(3.387)及

由式(3.396)及式(3.387)得

式(3.396)至式(3.399)用于后续方程简化，C是结构常数矩阵。考虑式(3.398)两边2范数得

由式(3.242)得

一方面，由式(3.398)、式(3.397)及式(3.401)得

另一方面，由式(3.388)、(3.396)及式(3.402)得

故得

由式(3.404)得式(3.382)中的姿态方程。

考虑当ⁱl₁＝0₃时的位置矢量对齐关系得

由式(3.w)及式(3.405)得

进而，得

即有

显然，有

由式(3.388)及式(3.407)得式(3.406)之左式

由式(3.290)、式(3.397)、式(3.398)及式(3.403)得

由式(3.398)及式(3.409)得

由式(3.396)、式(3.399)及式(3.410)得

将式(3.408)及式(3.411)代入(3.406)，且消去两边的得(3.382)中位置方程。证毕。

定理3.9作用在于：消去τ₅及τ₆后的位姿方程(3.341)是4个“4元2阶”多项式方程。显然，式(3.341)的Dixon行列式中的y_[2：4]阶次依次至少为[5,3,1]，应用式该行列式的计算复杂度至少48·48！，在现代技术条件下难以实现。

Ju-Gibbs增量四元数及性质

由基于轴不变量的通用6R机械臂位姿逆解和基于轴不变量的通用7R机械臂位姿逆解两节可知：通用机械臂位姿逆解的计算复杂度较高，需要解决实时计算的技术问题。在工程上，计算精度是一个相对的概念，需要保证数值计算精度远高于系统结构参数精度即能满足工程要求。式(3.1)所示的姿态方程及式(3.2)所示的位置方程本质上是Ju-Gibbs四元数的表达式。只要保证式(3.1)及式(3.2)具有足够的计算精度即能满足工程精度要求。下面，首先提出“居-吉布斯”增量四元数(Delta-quaternion)，再建立通用6R机械臂增量位姿方程并进行实时逆解计算。

【1】“居-吉布斯”增量四元数定义

定义“居-吉布斯”增量四元数

其中：

显然，“居-吉布斯”增量四元数是四维复数，且有

【2】“居-吉布斯”增量四元数性质

由式(3.229)及式(3.412)得

由式(3.232)得

由式(3.416)得

由式(3.231)得

由式(3.418)及(3.235)得

由式(3.242)得

基于轴不变量的通用7R机械臂运动规划

通用7R机械臂逆解由于计算复杂度极高，在现有技术条件下无法实现。但是，通常位于第7轴上的拾取点与第6轴的距离很小。因此，在第6轴取距拾取点较近的点为名义拾取点，先计算通用6R机械臂的逆解；以之为基础，再应用数值迭代法，完成通用7R机械臂的运动规划与逆解计算。下面，探讨通用7R机械臂的增量(Delta)位姿方程建立及逆解的问题。

【1】通用7R轴链的增量位姿方程

下面，先陈述由Gibbs增量四元数表征的通用6R轴链增量位姿方程，并予以证明；然后，进行求其逆解。

定理3.10：若给定6R轴链ⁱl₇＝(i，1：7]，ⁱl₁＝0₃，位置矢量及Ju-Gibbs增量四元数分别为及则由Ju-Gibbs增量四元数表征的通用6R机械臂增量位姿方程表示为：

其中：由系统结构参数行四元数构成的矩阵表示为

下面，分析通用7R机械臂增量位姿逆解。显然，式(3.421)是关于{ε_l|l∈[1：4}的线性方程。将式(3.421)重新表示为

A·[ε₁>2>3>4]^T＝b；>

若A^-1存在，解式(3.427)得

[ε₁>2>3>4>-1·b。>

由式(3.417)及式(3.430)得由式(3.397)得ε₅及ε₆。至此，逆解计算完毕。

通用6R机械臂增量位姿逆解作用在于：可以通过增量位置矢量及Gibbs增量四元数应用逐步逼近的算法使通用机械臂增量位姿到达期望的位姿。该原理本质上第一部分提及原理一致，二者具有一样的收敛性。同样，当收敛至目标状态时，通常仅有一组逆解。

【2】基于偏速度迭代的通用7R机械臂运动规划

记运动链为l∈(i,1:6]，由式(3.3)及式(3.4)得

记记期望位姿分别为及且有将式合写为

由式应用梯度(Gradient Descent Method/GDM)下降法得

其中：步长Step>0，Step→0。显然有

选择Step步长，由初态开始迭代，直至终态基于偏速度的迭代优化步骤如下：

(1)确定目标函数

显然，Goal表示及的方差(Variance)。

(2)选择步长

一方面，应用构造法得确定步长

则由式及式可知：仅当时，Step→0，Goal→0。

另一方面，由式(3.413)及式得

其中：ε_(i,6]＝[ε₁>2 …>6]^T。由式(3.437)及式确定步长

(3)迭代过程

一方面，若取式(3.438)之步长Step，由式完成迭代计算：

其中：当时，迭代过程结束。对于式的迭代过程，则必有

δGoal≤0， (3.440)

即式的迭代过程一定收敛。

证明：由式及式得

由式、式及式得

因式与式(3.438)在理论上等价，故可用式(3.438)替代式。但是式与式(3.438)的计算过程不同：因计算机字长有限，当时，前者精度越来越差，而后者起来越高；同时，后者计算量相对较小。因此，在工程应用时，取式(3.438)步长更好。

当时，得到稳态解φ_(i,6]，即为通用机械臂的位姿逆解。证毕。

■

基于偏速度的通用7R机械臂运动规划特点在于：通过迭代，逼近期望的位姿，可以得到一条由初始位姿至期望位姿的路径。因为该方法是趋向目标的优化过程，所以实时性较差。若在迭代过程中，控制关节增量，则可以满足关节速度的约束；因此，在完成运动规划的同时，也获得对应期望位姿的一组逆解。

轴不变量理论与自主行为机器人学

基于轴不变量的通用6R机械臂位姿逆解预备定理奠定了通用机械臂逆解的理论基础。因式(3.336)具有迭代式的形式，故又称之为基于轴不变量的通用5R迭代式运动学建模定理；通用6R机械臂方程逆解结构定理表明6R机械臂具有实时逆解，即5R迭代式运动学存在实地逆解。这两个定理表明：

(1)以固定轴不变量表征的刚体位形不仅简单、直观，而且对于逆解计算具有重要意义；

(2)基于轴不变量的通用机械臂运动学方程不仅表达简洁，而且具有实时逆解。

表示3D空间下的5轴及2轴螺旋的固定及自由轴不变量是机器人理论的基元，具有建模与求解的普适性。基于轴不变量的正逆运动不仅是机器人理论的重大突破，而且会带来机器人工程的革命。如图40及图41所示，通用6R机械臂与1R/2R/3R解耦末端操作机构串接，研制高精度的7R/8R/9R的类人手臂(Humanoid Arm)。

高精度机械臂底座具有多个(≥3)激光跟踪球球座，以建立精确的机械臂底座坐标系；各轴结构参数(固定轴不变量)可以精确测量，不存在理想的笛卡尔直角坐标系的测量约束及结构上的解耦约束，可以将机械臂的绝对定位精度提升至与重复定位精度相当的水平。

请参照图42。图42包含根食指3R 4201、中食指1R 4202、叶食指1R 4203、根大拇指3R 4204、中大拇指1R 4205、叶大拇指1R 4206、腕2R 4207、肘2R 4208、肩3R 429。对于人类手臂，由根至叶的运动链由5R臂(Limb)、2R手腕与5R手指(Finger)构成，是12个自由度及6折(Fold)的空间结构；若将手腕的两轴分别计入臂与手指，则该运动链是两个通用6R臂的串接与解耦的系统，手是5支链(Chain)6R手指的系统(简记为5C-6R)。因此，类人手臂由6R机械臂(Manipulator)及5C-6R灵巧手(Dexterous Hand)串接构成，实现分层级(Level)的运动控制。第1层级的6R机械臂与5C-6R灵巧手具有12R的轴链，执行大范围的空间运动。第2层级的5C-6R灵巧手具有变拓扑(Variable Topology)轴链，能够使用工具，实现对受控对象的精密操作。

(1)DH0模式，操作0DOF受控对象，比如：握持(Hold)；

(2)DH1模式，操作1DOF受控对象，比如：扳拧(Wrench)，推动(Push)与拉动(Pull)；

(3)DH2模式，操作2DOF受控对象，比如：摆动(Sway)，旋拧(Screw)与拖拉(Drawing)；

(4)DH3模式，操作3DOF受控对象，比如：平移(Translate)与旋转(Rotate)；

(5)DH4模式，操作4DOF受控对象，比如：平衡(Balance)，对接(Attach)与分离(Detach)；

(6)DH5模式，操作5DOF受控对象，比如：穿越(Pass)，触碰(Touch)与切割(Cut)；

(7)DH6模式，操作6DOF受控对象，比如：搅动(Stir)，摆放(Set)与雕刻(Engrave)。

两通用机械臂的分层串接、5C-6R的变拓扑及多折(Multi-Fold)的结构是类人手臂的三大特征。

如图43所示，6R4F(Folding)类人手臂具有复杂空间操作的优势。因其前五轴突破了解耦约束，故具有更优化及更简洁的结构。研制用于特种加工与作业的类人手臂，实现多机器人协作的柔性加工系统是自主机器人的发展趋势。

“轴不变量理论”之所以能将现有的运动学及动力学原理统一起来，根本原因在于：一方面，“轴不变量理论”是一个在拓扑上以轴为基元及在度量上以3D螺旋为基元的多轴系统建模与控制理论。另一方面，多轴系统本质上是自治系统(Autonomous System)，即由不显含时间的常微分方程(ODE)表达的系统；自治系统具有行为的确定性；奠基于自治模型的“自主机器人”，无论在结构上还是在行为上，具有有序性、精确性、可控性及实时性。而偏微分系统的结构及行为，常常具有分岔及混沌的不确定性；多轴系统的运动学及动力学方程都是3D矢量空间操作代数系统。

第四部分>Equation Chapter 4Section 1

基础公式

【1】给定运动链则有

ⁱk_i∈ⁱl_n，|ⁱk_i|＝0，>

^kl_n＝-ⁿl_k，>

ⁱl_n＝ⁱl_l+^ll_n，ⁱl_n＝ⁱl_l·^ll_n。>

【2】自然不变量

【3】基于轴不变量的转动

其中：

【4】运动学迭代式

给定轴链有以下速度及加速度迭代式：

其中：及|ⁱl_l|为轴链ⁱl_l的轴数。

由式(4.2)得

其中：

显然，有

若l为刚体，由式(4.22)及式(4.19)得

【5】二阶张量投影

【6】给定运动链则有惯性坐标张量

【7】给定轴链有以下偏速度计算公式：

【8】给定轴链k，l∈ⁱl_n，有以下二阶矩公式：

【9】左序叉乘与转置的关系

【10】轮土矢量矢学及移动维度

FD(W_S)＝3>

FD(W_NS)＝2>

DOF(D)＝6+|A|-|NT|+|O| (4.43)

DOM(D)＝DOF(D)-n_NS·FD(W_NS)-n_S·FD(W_S)(4.44)

多轴系统的拉格朗日方程推导与应用

1764年，拉格朗日(Lagrange)在研究月球天平动问题时提出了拉格朗日方法(Lagrangian)，是以广义坐标表达动力学方程的基本方法；同时，也是描述量子场论的基本方法。本节推导拉格朗日方程，并应用链符号系统对之重新表述。

下面考虑质点动力学系统D＝{A，K，T，NT，F，B}，根据牛顿力学推导自由质点的拉格朗日方程；然后，推广至受约束的质点系统。对于一组受保守力作用的质点，在牛顿惯性空间的笛卡尔直角坐标系下，有

式(4.45)中的保守力相对质点惯性力具有相同的链序，即具有正序，质点的合力为零。质点的能量记为由动能及势能组成，即有

由式(4.46)得，动量ⁱp_D，且有

由式(4.47)得

称式(4.48)为笛卡尔矢量空间的拉格朗日方程。

广义坐标序列与笛卡尔空间位置矢量序列{ⁱr_l|l∈T}关系记为

由式(4.49)得

显然，有

由式(4.48)及式(4.49)得

由式(4.50)及式(4.51)得

对于任一函数f(…，ⁱr_l，…；t)，其时间微分为

由式(4.54)的莱布尼兹(Leibnitz)规则及式(4.53)第2项得

由式(4.50)得

另一方面，由偏导数链规则可知

由式(4.56)及式(4.57)得

将式(4.55)及式(4.58)代入式(4.48)得

若非奇异,则存在。故得关节空间的拉格朗日方程，

式(4.60)应用系统的能量及广义坐标建立系统的方程，与式(4.49)具有结构上的不变性。式(4.60)中，给定关节空间点的标量与描述该点的广义坐标是相互独立的；式(4.49)坐标更换称为空间点变换(point transformation)。

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

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

【1】系统能量

系统D能量表达为

其中：

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

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

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

示例4.1：请参照图44。图44为平面2R机械臂示意图。图44包含杆件1的长度l₁4401、杆件1的质心与轴1原点的距离杆件2的质心与轴2原点的距离如图44所示平面2R机械臂系统，A＝(i，1，2]，关节坐标序列杆件自然坐标系序列{F^[l]|l∈A}，惯性系记为F^[i]；质心位置矢量重力加速度矢量质量序列{m_l|l∈A}，质心转动惯量序列关节驱动力矩分别为及应用式(4.64)建立该系统的拉格朗日方程。

解：记且记

其中*表示未知量；应用式(4.11)正余弦简记符。

步骤1：表达系统的能量。杆件1的动能，

杆件1的重力势能，

杆件2的动能，

杆件2的重力势能，

系统重力势能，

系统动能，

步骤2：获得系统能量对关节速度的偏导数，

步骤3：获得系统能量对关节角度的偏导数，

步骤4：获得偏速度对时间t的导数，

故得

步骤5：由式(4.71)、式(4.70)及式(4.64)得拉格朗日动力学方程：

由示例4.1可知：对于2DOF的平面机械臂，应用拉格朗日法建立动力学方程已是一个烦琐的过程；随着系统自由度的增高，计算复杂度也剧增。原因在于：

【1】首先，平动速度及转动速度的计算复杂度为O(N)；

【2】进而，通过平动速度及转动速度表达系统能量的计算复杂度为O(N²)；

【3】接着，由系统能量计算偏速度的复杂度为O(N³)；

【4】最后，由偏速度对时间求导的复杂度为O(N⁴)。

尽管拉格朗日方程依据系统不变性的能量推导系统的动力学方程，具有理论分析上的优势；但是在工程应用中，随着系统自由度的增加，方程推导的复杂性剧增，难以得到普遍应用。

多轴系统凯恩方程推导与应用

首先分析由Thomas R.Kane提出的凯恩方程，并应用链符号系统对之重新表述。

给定多轴系统D＝{A，K，T，NT，F，B}，考虑由N＝|B|个刚体构成的多轴系统，任一刚体受到外力及关节内力作用。惯性系记为F^[i]；体k的质心k_I所受的合外力及力矩坐标矢量分别记为及ⁱτ_k，k∈[1，2，…，N]。关节内力及力矩矢量分别记为及应用达朗贝尔(D’Alembert)原理建立体k的力平衡方程，

其中：为体k的惯性力；惯性力表示为

记质心k_I的虚位移为它是时间δt→0的位移增量；根据虚功原理得虚功δW，k∈[1，2，…，N]。

在关节内力不引起功率损失的假设下，有

故有

由式(4.75)得

因位置矢量为

故有

显然，有

因虚位移是任意的，故由式(4.76)得

由式(4.74)及式(4.78)得

相似地，应用虚功原理得

其中：及分别为外力矩及惯性力矩坐标矢量。且有

其中：体k的质心转动惯量。由式(4.80)及式(4.81)得

将式(4.79)及式(4.82)组合在一起得多轴系统凯恩方程

由式(4.83)可知,多轴系统凯恩方程的建立步骤如下：

步骤1：标识质心、力的作用点等关键点；

步骤2：选取独立的一组关节坐标并得到方向余弦矩阵(DCM)；

步骤3：通过关节坐标及关节速度表达平动速度、平动加速度、转动速度及转动加速度；

步骤4：如下表所示，计算偏速度；

步骤5：将计算出的偏速度、速度及加速度代入式(4.83)得系统的动力学方程；

步骤6：将动力学方程写成标准形式，即获得规范化动力学方程，

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

示例4.2：如图45所示，是一个理想的弹簧质量摆，应用凯恩方法及拉格朗日方法分别建立该系统的动力学方程。图45包含：弹簧弹性形变即关节1的位移弹簧弹性系数k、质量摆绳长l₂、质量摆摆动的角度

解法1：凯恩建模方法如下：

步骤1：如图45所示质心位置标识为*；

步骤2：得到DCM。

步骤3：选取关节坐标

计算转动速度及加速度：

计算平动速度及加速度，如下表所示。

步骤4：构建偏速度表。

步骤5：将计算出的偏速度、速度及加速度代入式(4.83)得系统动力学方程各项。

步骤6：将动力学方程写成标准形式，即得到规范化动力学方程。

解法2：拉格朗日方法如下：

步骤1：表达系统的能量

质点1动能

选取地面为0势能面，质点1势能

质点2动能

质点2势能

系统总能量

步骤2：获取系统能量对速度的偏导数

步骤3：获取系统能量对位移、角度的偏导数

步骤4：获得偏速度对时间t的导数

步骤5：将各项带入拉格朗日动力学方程，整理得

显然，上式与式(4.83)一致。

由上可知，凯恩方程建立过程与拉格朗日相比，通过系统的偏速度、速度及加速度直接表达动力学方程。相对系统自由度N，首先计算速度及加速度的复杂度为O(N)；然后，计算偏速度的复杂度为O(N)。故凯恩动力学建模的复杂度为O(N²)。

凯恩动力学方法与拉格朗日方法相比，由于省去了系统能量的表达及对时间的求导过程，极大地降低了系统建模的难度。然而，对于高自由度的系统，凯恩动力学建模过程仍难以适用。因为凯恩动力学建模过程还需解决以下问题：需要解决建立迭代式运动学方程的问题；需要解决偏速度求解的问题；需要解决建立式示例4.2所示的规范化动力学方程的问题。

多体分析动力学研究的局限性

拉格朗日方程及凯恩方程极大地推动了多体动力学的研究，以空间算子代数为基础的动力学由于应用了迭代式的过程，计算速度及精度都有了一定程度的提高。这些动力学方法无论是运动学过程还是动力学过程都需要在体空间、体子空间、系统空间及系统子空间中进行复杂的变换，建模过程及模型表达非常复杂，难以满足高自由度系统建模与控制的需求。主要表现于：

【1】对于高自由度的系统，由于缺乏规范的动力学符号系统，导致技术交流成为严重障碍。虽然文献[1-20]各自有相应的符号，但它们是示意性的，不能准确反映物理量的内涵，未能体现运动链的本质；需要专业人员具有长期的动力学建模经验，否则难以保证建模过程的工程质量及普遍应用。

【2】在系统结构参数、质惯量参数给定时，尽管存在多种动力学分析及计算方法，但由于动力学建模过程复杂，不能清晰地表达统一的动力学模型。难以适应动力学控制的需求。同时，无论拉格朗日动力学方程还是凯恩动力学方程建模过程都表明3D空间的动力学方程可以表达多体动力学过程。

【3】6D空间(位姿空间)算子代数的物理含义非常抽象，建立的动力学算法缺乏严谨的公理化证明，复杂的计算过程由于缺乏简洁、准确的符号系统；在一定程度上牺牲了计算速度。文献[13-15]各自以数百页篇幅介绍6D空间算子代数的多体动力学原理，几乎没有通过完整的示例来阐述原理的应用过程，即使一个低自由度的系统，建模过程也很冗长。根本原因在于该6D空间是双3D矢量空间。

因此，需要建立动力学模型的简洁表达式；既要保证建模的准确性，又要保证建模的实时性。没有简洁的动力学表达式，就难以保证高自由度系统动力学工程实现的可靠性与准确性。同时，传统非结构化运动学及动力学符号通过注释约定符号内涵，而不能被计算机理解；导致计算机不能自主地建立及分析运动学及动力学模型。

居―凯恩动力学预备定理

3D笛卡尔空间的基元是自然参考轴即轴不变量。将多体运动学理论与链拓扑论、计算机理论相结合，遵从张量不变性、序不变性及轴不变性的基本原理，建立运动链符号系统及基于轴不变量的多轴系统运动学与动力学的理论，根本目的是通过3D自然轴空间代数及拓扑操作、建立简洁的基于轴不变量的迭代式运动学与动力学方程。

在第一部分，应用链符号系统，将传统的拉格朗日方程及凯恩方程分别表达为式(4.60)及式(4.83)。下面，进一步阐述基于轴不变量的正向与反向迭代式对于多轴系统动力学建模的作用。

基于轴不变量的正反向迭代

下面，先阐明运动轴轴向、运动学的前向迭代及动力学的反向迭代三个重要准则，为提出Ju-Kane动力学原理奠定基础。请参照图46，图46为多轴系统的闭子树，如图46所示，将的两个轴“切开”，轴运动链为轴l的闭子树为^lL，且令D′＝^lL。

【1】运动轴轴向平衡方程：是运动轴方向的惯性力与外力平衡的方程。若系统D′对轴l施加约束力记为^D′f_l及约束力矩记为^D′τ_l；则有

其中：^D′f_l─系统D′作用于轴l的合力；^D′τ_l─系统D′作用于轴l的合力矩。

式(4.86)表明，尽管施加约束力^D′f_l及约束力矩^D′τ_l未知，在运动轴轴向的分量总是零；动力学方程本质上是运动轴轴向力或力矩的平衡方程；轴约束力、力矩与自然运动轴正交，又称自然正交补。

【2】运动学的前向迭代：无论是有根系统还是无根系统，根的位形、速度及加速度视为已知量，由根至叶的运动学计算，可以确定任一轴的位形、速度及加速度。这是由式(4.4)所示系统拓扑的串接性决定的。由第3部分分析可知，任一轴的相对惯性空间的位形、速度及加速度是关于轴不变量的迭代式。

【3】动力学的反向迭代：力的传递由叶向根反向传递，力的作用具有双重效应，由闭子树^lL至轴l的作用力^D′f_l及作用力矩^D′τ_l是闭子树成员的惯性力及外力在轴l上的等价作用力及力矩。由运动学的前向迭代确定了闭子树成员的相对惯性空间的位形、速度及加速度，也同样确定了闭子树成员的惯性力及力矩。

运动轴轴向平衡方程的建立依赖于轴不变量的基本性质；运动学的前向迭代依赖于运动链的拓扑操作及基于轴不变量的迭代式运动学计算；动力学的反向迭代依赖于闭子树的拓扑操作及迭代式的偏速度计算。在第2部分及第3部分已对这三个问题进行了系统阐述。下面，通过示例4.3，应用式(4.8)至式(4.20)运动学迭代过程证明式(4.32)至式(4.34)的偏速度迭代过程的正确性。

示例4.3：继示例4.1，应用式(4.8)至式(4.20)的完成运动学迭代计算；通过实例证明式(4.32)至式(4.34)偏速度迭代过程的正确性。

解：显然，结构参数为

ⁱn₁＝1^[z],¹n₂＝1^[z]>

引力常数为

【1】平动与转动

旋转变换阵：由式(4.8)得

由式(4.8)及式(4.90)得

转动矢量：

由式(4.15)得

位置矢量：显然，有

由式(4.16)得

转动速度矢量：显然，有

由式(4.17)得

平动速度矢量：显然，有

由式(4.18)得

由式(4.18)并考虑式(4.104)得

转动加速度矢量：由式(4.19)得

平动加速度矢量：由式(4.20)得

由式(4.107)得

由式(4.20)得

即

由式(4.20)得

即

【2】偏速度验证

由式(4.99)得

故有

由式(4.100)得

故有

由式(4.111)及式(4.112)的特例验证了式(4.33)的正确性。

由式(4.96)得

故有

由式(4.98)得

故有

由式(4.113)及式(4.114)之特例验证了式(4.32)之正确性。由式(4.102)得

故有

由式(4.105)得

故有

由式(4.115)及式(4.116)之特例验证了式(4.32)之正确性。

对于链ⁱl_k而言，由式(4.12)至式(4.20)中运动矢量迭代计算的复杂度为O(|ⁱl_k|)。上述表明推导偏速度是非常烦琐的过程；由式(4.32)至式(4.34)的偏速度迭代计算复杂度为O(1)。将式(4.32)至式(4.34)的偏速度迭代式应用于凯恩方程(4.83)，可以将凯恩动力学建模的复杂度降至线性复杂度。下面，推导Ju-Kane动力学预备定理，以之为基础，再解决多轴系统建模与正逆计算的其它问题。

居―凯恩动力学预备定理证明

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

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

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

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

证毕。

因与不相关，由式(4.117)及多轴系统拉格朗日方程(4.64)得

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

考虑式(4.62)及式(4.63)，即有

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

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

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

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

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

【3】求对时间的导数

【3-1】若由式(4.117)、式(4.119)及式(4.120)得

【3-2】若由式(4.117)、式(4.122)及式(4.123)得

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

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

将式(4.121)、式(4.124)、式(4.125)及式(4.126)代入式(4.118)得定理4.1，表述如下：

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

尽管式(4.127)是根据多轴系统拉格朗日方程(4.64)推导的，式(4.127)与凯恩方程(4.83)有相似之处。因此，称定理4.1为Ju-Kane动力学预备定理。虽然式(4.127)形式上是凯恩方程对两种基本运动副的不同表示，但是二者存在本质的不同；因为式(4.127)具有了树链拓扑结构。

Ju-Kane动力学预备定理应用

示例4.4：继示例4.1及示例4.3，应用Ju-Kane预备定理建立该机械臂动力学模型。

解：由式(4.111)得

式表示(4.128)表示杆1惯性力在方向的投影。由式(4.112)及式(4.106)得

由式(4.115)及式(4.107)得

故有

由式(4.116)及式(4.108)得

即

由式(4.116)及式(4.109)得

即

由式(4.131)及式(4.132)得

式(4.133)表示杆2质心加速度在自然坐标方向的投影。由式(4.113)得

由式(4.116)得

由式(4.134)及式(4.135)得

式(4.136)表示重力在自然坐标方向的投影。将式(4.128)、式(4.129)、式(4.130)、式(4.132)、式(4.133)及式(4.136)代入式(4.127)得

对比式(4.72)及式(4.137)可知，它们完全一致。该例间接证明了Ju-Kane预备定理的正确性。解毕。

示例4.5：继示例4.2，应用Ju-Kane预备定理建立该系统的动力学方程。

解：考虑质点1：

故有

考虑质点2：

故有

将上述结果代入Ju-Kane动力学方程(4.127)得

显然，上式与式(4.85)一致。

树链刚体系统Ju-Kane动力学显式模型

下面，针对Ju-Kane预备定理，解决式(4.127)右侧的^Df_k及^Dτ_k计算问题，并将式(4.32)至式(4.34)的偏速度代入式(4.127)中；从而建立树链刚体系统Ju-Kane动力学方程。

外力反向迭代

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

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

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

即

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

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

即有

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

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

共轴驱动力反向迭代

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

【1】由式(4.32)、式(4.33)及式(4.142)得

即

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

因与共轴，故有

【2】由式(4.33)、式(4.32)及式(4.142)得

即

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

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

树链刚体系统Ju-Kane动力学显式模型

下面，先陈述树链刚体系统Ju-Kane动力学定理，简称Ju-Kane定理；然后，对之证明。

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

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

证明：记故有

由式(4.139)、式(4.140)、式(4.144)、式(4.146)及式(4.154)得式(4.153)。将式(4.33)，式(4.32)及式(4.34)代入Ju-Kane动力学预备方程(4.127)得

由式(4.20)得

考虑式(4.156)，则有

同样，考虑式(4.156)，得

将式(4.156)至式(4.158)代入式(4.155)得式(4.147)至式(4.152)。证毕。

对于纯转动轴系统，由式(4.152)得

由式(4.159)可知，对于纯转动轴系统，相对质心轴的转动(自然转动)通过陀螺力矩可以交换但不消耗系统能量。

树链刚体系统Ju-Kane动力学建模示例

示例4.6：请参照图47，给定如图47所示的通用3R机械臂，A＝(i，1，2，3]；应用树链Ju-Kane动力学定理建立其动力学方程，并得到广义惯性矩阵。

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

由式(4.8)得

由式(4.12)及式(4.160)得

由式(4.16)，式(4.160)及式(4.161)得

由式(4.17)及式(4.161)得

由式(4.18)、式(4.161)及式(4.163)得

由式(4.19)及式(4.161)得

由式(4.25)及式(4.161)得

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

由式(4.152)得

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

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

由式(4.152)得

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

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

由式(4.152)得

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

由式(4.167)，式(4.169)及式(4.173)得广义质量阵。

解毕。

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

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

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

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

运动链的规范型

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

【1】给定运动链则有

证明：

证毕。

【2】给定运动链则有

证明：因故得

证毕。

【3】给定运动链则有

证明：因故有

证毕。

【4】给定运动链则有

证明：考虑将式(4.177)代入式(4.180)左侧得

证毕。

【5】给定运动链则有

证明：考虑将式(4.177)代入式(4.181)左侧得

证毕。

【6】给定运动链则有

证明：因故有

证毕。

闭子树的规范型

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

由式(4.179)得

由式(4.180)得

由式(4.181)得

由式(4.182)得

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

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

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

然后，应用式(4.183)至(4.187)，将式(4.149)及式(4.150)表达为规范型。

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

证明:由式(4.22)及式(4.149)得

由式(4.19)及式(4.190)得

将式(4.185)代入式(4.190)右侧前一项得

将式(4.184)代入式(4.191)右侧后一项得

将式(4.192)及式(4.193)代入式(4.191)得

对于刚体k，有由式(4.148)、式(4.188)及式(4.194)得式(4.189)。

证毕。

【2】式(4.150)的规范型为

证明：由式(4.150)得

将式(4.183)代入式右侧前一项(4.196)得

将式(4.186)代入式(4.196)右侧后一项得

将式(4.187)代入式(4.196)右侧中间一项得

将式(4.197)，式(4.198)及式(4.199)代入式(4.197)得

对于刚体k，有由式(4.148)，式(4.188)及式(4.200)得式(4.195)。

证毕。

【3】应用式(4.189)及式(4.195)，将Ju-Kane定理重新表述为如下树链Ju-Kane规范型定理。

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

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

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

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

示例4.7：应用Ju-Kane规范型定理建立示例4.1所示的平面2R机械臂动力学方程；并证明两种方程的等价性。

步骤1建立基于轴不变量的迭代式运动学方程。分别建立轴不变量，DCM，位置、平动速度及转动速度表达式，参见式(4.87)至式(4.105)；步骤2由式(4.65)，式(4.87)至式(4.105)及式(4.208)得

由式(4.209)及式(4.210)得

由式(4.205)得

■

证明：由(4.87)，式(4.96)，式(4.99)得

由式(4.65)，(4.87)，式(4.99)及式(4.100)得

由(4.87)，式(4.99)，式(4.101)至式(4.105)得

由(4.87)，式(4.98)及式(4.89)得

由(4.87)，式(4.96)及式(4.89)得

将式(4.214)至式(4.225)代入式(4.212)得

由(4.87)，式(4.95)，式(4.101)至式(4.105)得

由(4.87)，式(4.95)，(4.97)，式(4.101)至式(4.105)得

由(4.87)，式(4.65)，式(4.100)得

由(4.87)，式(4.89)，式(4.95)得

将式(4.222)至式(4.225)代入式(4.213)得

由式(4.201)，式(4.211)，式(4.213)及式(4.226)得该系统动力学方程

对比式(4.72)及式(4.227)，两组方程一样。显然，证明过程冗长，原因在于该2R机械臂具有特定的结构参数；Ju-Kane动力学规范方程是针对通用构型与结构参数的。证毕。

示例4.8：应用Ju-Kane规范型定理建立示例4.2所示系统的动力学模型，并证明两种方程的等价性。

解：步骤1建立基于轴不变量的迭代式动力学方程DCM，位置，平动速度及转动速度表达式，参见示例4.2。步骤2由式(4.202)至式(4.206)可得

步骤3由式(4.207)求合外力及合外力矩

步骤4由式(4.201)整理可得，

显然，上式与式(4.85)一致。

示例4.8：继示例4.6，应用Ju-Kane动力学规范方程得该系统的广义质量矩阵，并判别是否与应用Ju-Kane定理得到的广义质量矩阵一样。

解：由式(4.208)得

由式(4.228)，式(4.229)及式(4.230)得(4.176)。间接证明了Ju-Kane规范型的正确性。

示例4.9：给定6R机械臂系统D＝{A，K，T，NT，F，B}，A＝(i，1，2，3，4，5，6]，不可检测的环境作用力记为各轴驱动力力矩为应用式(4.207)计算各轴的外部作用力矩

解：因不可检测，在力位控制中，需要通过驱动轴的力控制予以抵消；故期望通过动力方程进行解算得到驱动轴的总控制力矩。由式(4.207)得

其中：为u轴合力矩，为驱动轴的驱动力矩。解毕。

轴链刚体惯性矩阵

将根据运动轴类型及3D自然坐标系表达的刚体运动链广义惯性矩阵称为轴链刚体惯性矩阵，简称为轴链惯性矩阵。由式(4.244)及式(4.247)得

由式(4.232)及式(4.233)可知，上述轴链惯性矩阵是3×3的矩阵,其大小比与传统的6×6广义惯性矩阵小了4倍；相应地，求逆的复杂度也比传统的惯性矩阵小4倍。

闭子树^uL的能量表达为

若l∈^uL，则由式(4.32)至式(4.34)及式(4.234)得

若l∈^uL，，则由式(4.32)至式(4.34)及式(4.234)得

令

且有

因此，M^[u][k]可记为

式中(4.242)M^[u][k]是3×3的轴链惯性矩阵(AGIM)，称δ_k为运动轴属性符。

轴链刚体广义惯性矩阵特点

给定多轴刚体系统D＝{A，K，T，NT，F，B}，ⁱl_n＝(i，…，l，…，u，…n]，k∈^uL；该系统轴链刚体惯性矩阵在所有运动副类型相同的情况下具有对称性，即有

证明：显然，有u≥l。

若由式(4.203)得

由式(4.244)及式(4.245)得

若由式(4.206)得，若得

由式(4.247)，式(4.248)及式若得

记|A|＝a，将轴数为a的系统广义惯性矩阵记为M_3a×3a。由式(4.243)得

式(4.250)中轴链刚体惯性矩阵M_3a×3a具有对称性，其元素即轴链惯性矩阵是3×3的矩阵；给定多轴刚体系统D＝{A，K，T，NT，F，B}，轴链刚体惯性矩阵元素具有以下特点：

【1】若由式(4.203)可知及是对称矩阵；

【2】若由式(4.206)可知及是对称矩阵；

【3】若由式(4.206)可知是反对称矩阵；

由上可知，轴链惯性矩阵的元素不一定具有对称性。

给定运动链笛卡尔坐标轴序列记为其中：为转动轴序列，为平动轴序列，且有自然坐标序列为由式(4.203)得

显然，有u＝^uL，m_l＝0,由上式得

显然，刚体坐标轴惯性矩阵与6D惯性矩阵不同，但二者等价。

轴链刚体系统广义惯性矩阵

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

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

由式(4.252)得

考虑由式(4.35)得到的及式(4.255)得

显然，是对称矩阵。

由式(4.250)得

式(4.257)表明具有对称性；称之为轴链广义惯性矩阵。

由式(4.244)及式(4.245)、式(4.247)及式(4.248)，的计算复杂度与闭子数^kL的轴数成正比。故有

对于轴链广义惯性矩阵，由式(4.258)及式(4.250)可得如下结论：

【1】若由单颗CPU计算则有

【2】若由a颗CPU或GPU并行计算则有

树链刚体系统Ju-Kane动力学方程正解

现在探讨如何得到树链刚体系统Ju-Kane动力学方程正解。动力学方程的正解是给定驱动力时根据动力学方程求关节加速度或惯性加速度。

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

由式(4.259)得

其中，

由式(4.259)得

关键是如何计算(4.262)中的轴链广义惯性矩阵的逆即若应用枢轴方法蛮力计算显然，即使对于轴数不是很多的多轴系统，计算代价也极大。故该方法不宜使用。

由式(4.257)知轴链广义惯性矩阵是对称矩阵，且因系统能量大于零，故其是正定矩阵。有效的计算过程如下：

【1】首先，对其进行LDL^T分解，

其中，是唯一存在的下三角矩阵，D_a×a是对角矩阵。

【1-1】若由单颗CPU计算进行LDL^T分解，则分解复杂度为O(a²)；

【1-2】若由a颗CPU或GPU并行分解则分解复杂度为O(a)；

【2】应用式(4.264)计算

将式(4.264)代入式(4.262)得

至此，得到树链刚体系统Ju-Kane动力学方程正解。它具有以下特点：

【2-1】基于Ju-Kane规范型的式(4.263)中轴链广义惯性矩阵大小仅是6D双矢量空间的广义惯性矩阵的1/4，的LDL^T分解速度得到大幅度提升。同时，式(4.265)中f^c、fⁱ及h都是关于轴不变量的迭代式，可以保证求解的实时性与精确性；Ju-Kane规范型具有公理化的理论基础，物理内涵清晰；而基于6D空间操作算子的多体系统动力学以整体式的关联矩阵为基础，无论是建模过程还是正解过程较Ju-Kane规范型系统建模与求解过程都很抽象。特别是借鉴卡尔曼滤波及平滑的理论建立的动力学迭代方法，缺乏严谨的公理化分析证明。

【2-2】式(4.263)中的轴链广义惯性矩阵式(4.265)中f^c、fⁱ及h都可以根据系统结构动态更新，可以保证工程应用的灵活性。

【2-3】式(4.263)中轴链广义惯性矩阵及式(4.265)中f^c、fⁱ及h具有简洁、优雅的链指标系统；同时，具有软件实现的伪代码功能，可以保证工程实现的质量。

【2-4】因坐标系及轴的极性可以根据工程需要设置，动力学仿真分析的输出结果不必做中间转换，提高了应用方便性与后处理的效率。

树链刚体系统Ju-Kane动力学方程逆解

动力学方程的逆解是指已知动力学运动状态、结构参数及质惯性，求解驱动力或驱动力矩。考虑式(4.201)及式(4.207)得

当已知关节位形、速度及加速度时，由式(4.147)得^i|Df_u及^i|Dτ_u。进一步，若外力及外力矩已知，则由式(4.266)求解驱动力及驱动力矩显然，动力学方程的逆解计算复杂度正比于系统轴数|A|。

尽管动力学逆解计算很简单，但它对于多轴系统实时力控制具有非常重要的作用。当多轴系统自由度较高时，实时动力学计算常常是一个重要瓶颈，因为力控制的动态响应通常要求比运动控制的动态响应的频率高5至10倍。一方面，由于轴链惯性矩阵不仅对称，而且大小仅是传统的体链惯性矩阵的1/4，由式(4.261)计算轴链广义惯性矩阵时计算量要小很多。另一方面，由式(4.260)计算运动轴轴向惯性力的计算量仅是牛顿欧拉法的1/36。

闭链刚体系统的Ju-Kane动力学符号模型

前面讨论了刚体系统的动力学建模问题，它们是以理想约束副及树链拓扑为前提的。

闭链刚体系统也具有非常广泛的应用；比如，CE3巡视器的摇臂移动系统是具有差速器的闭链，重载机械臂通常是具有四连杆的闭链系统。同时，实际的运动轴通常包含内摩擦力及粘滞力。本节首先研究闭链刚体系统的Ju-Kane动力学；然后，解决运动轴约束力求解问题，再讨论运动轴内摩擦力及粘滞力问题；最后，建立闭链刚体非理想约束系统的Ju-Kane动力学方程。

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

下面，先陈述闭链刚体系统的Ju-Kane动力学定理；然后，予以证明。

定理4.4：给定多轴刚体系统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′的约束代数方程为

其中：

其它，参见式(4.202)至式(4.207)。

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

由式(4.278)得

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

由式(4.279)及式(4.280)得

由式(4.279)得

由式(4.32)及式(4.282)得

故有

由式(4.274)及式(4.286)得式(4.269)。由式(4.33)及式(4.283)得

由式(4.275)及式(4.287)得式(4.270)。由式(4.33)及式(4.284)得

由式(4.276)及式(4.288)得式(4.271)。由式(4.33)及式(4.285)得

由式(4.277)及式(4.289)得(4.272)。由式(4.32)，式(4.280)及式(4.274)得

广义约束力及是矢量，由式(4.290)及式(4.291)得式(4.273)。由此可知，偏速度主要应用于力的反向迭代。广义约束力及视为外力，由定理4.3得式(4.267)及式(4.268)。

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

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

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

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

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

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

由式(4.201)及式(4.262)计算得式(4.292)表示运动轴矢量与运动轴约束力具有自然正交补(Natural Orthogonal Complement)的关系。

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

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

其中：

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

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

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

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

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

在完成轴径向约束广义力的计算后，得到运动轴u的径向约束力大小及约束力矩大小请参照图48，记运动轴u的内摩擦力大小及内摩擦力矩大小分别为及运动轴u的粘滞力及粘滞力矩大小分别为及

故有

其中：_sk^[u]─运动轴u的内摩擦系数，_ck^[u]─运动轴u的粘滞系数。

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

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

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

定理4.5：给定多轴刚体系统D＝{A，K，T，NT，F，B}，惯性系记为F^[i]，除了重力外，作用于轴u的合外力及力矩在上的分量分别记为及轴k的质量及质心转动惯量分别记为m_k及轴k的重力加速度为驱动轴u的双边驱动力及驱动力矩在上的分量分别记为及环境i对轴l的作用力及作用力矩分别为及轴u对轴u′的广义约束力记为运动轴u的广义内摩擦及粘滞的合力及合力矩分别为则有

【1】闭链刚体系统的Ju-Kane动力学方程，参见式(4.267)至式(4.277)、式(4.202)至式(4.207)；

【1-1】应用式(4.262)至式(4.265)计算关节加速度

【1-2】应用式(4.293)至式(4.298)计算径向约束力大小及约束力矩大小及

【2】建立如下闭链刚体非理想约束系统的Ju-Kane动力学方程：

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

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

其它，参见式(4.267)至式(4.277)、式(4.202)至式(4.207)。

证明：运动轴u的内摩擦及粘滞合力及合力矩是运动轴u的外力，故有式(4.302)；运动轴u′的内摩擦及粘滞合力及合力矩是运动轴u′的外力，故有式(4.303)。其它证明过程与定理4.4相似。证毕。

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

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

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

定理4.6给定多轴刚体移动系统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|Df_c及合力矩为^i|Dτ_c，记且有

则有

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

其它，参见式(4.204)至式(4.207)。

证明：显然,有

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

由式(4.33)得

由式(4.316)得

由式(4.317)得

故有

由式(4.203)及式(4.318)得

由式(4.319)及式(4.321)得(4.313)。由式(4.206)及式(4.318)得

由式(4.320)及式(4.322)得式(4.314)。证毕。

由定理4.6可知，可以根据需要由式(4.308)确定本体c的笛卡尔体系F^[c]三个转动轴的序列，在建立动力学方程后，通过积分完成动力学仿真，直接可以得到所期望的姿态。同时，对于除本体的其它轴，定理4.4、定理4.5同样适应。

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

本节阐述基于Ju-Kane的三轮移动系统动力学建模及逆解问题。

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

解：步骤1显然，|A|＝4，|B|＝5，|NT|＝|O|＝3，代入式(4.43)，得DOF(D)＝10。由式(4.41)、式(4.42)及式(4.44)得DOM(D)＝10-2·FD(W_NS)-1·FD(W_S)＝3。故该系统D在自然路面上静定。

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

由式(4.8)得

由式(4.12)及式(4.8)计算

由式(4.16)计算

由式(4.17)计算

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

由式(4.25)计算

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

由式(4.313)及式(4.314)得由式(4.204)及式(4.205)分别计算及代入式(4.310)得

由式(4.208)，式(4.205)及式(4.201)得

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

步骤4进行力反向迭代

由式(4.39)、式(4.40)及(4.207)得

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

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

将式(4.331)至式(4.333)写为整体形式

f_10×1＝B_10×10·u_10×1；>

其中：

给定由式(4.329)及式(4.330)计算f。若B^-1存在，由式(4.334)得

u＝B^-1·f。>

由式(4.335)及式(4.336)可知：

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

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

【2.1】计算驱动轴期望控制力矩及

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

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

本节阐述基于Ju-Kane的CE3月面巡视器移动系统动力学建模及逆解问题。

示例4.11：给定图50所示的CE3月面巡视器移动系统D＝{A，K，T，NT，F，B}，该系统Span树如图17所示；标识符及缩略符参见示例1.1；系统结构参数如图17；建立该系统的动力学方程。

步骤1：显然，|A|＝14，|NT|＝7，|O|＝6；由式(4.41)、式(4.42)及式(4.44)得DOM(D)＝19-2·FD(W_NS)-4·FD(W_S)＝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：基于轴不变量的多轴系统正运动学计算

由式(4.8)得

由式(4.12)及式(4.337)计算

由式(4.16)及式(4.338)计算

由式(4.17)计算

记l∈(c，rb，rr，rfd，rfw，rmw，rrd，rrw，lb，lr，lfd，lfw，lmw，lrd，lrw]，因是刚体系统，故有和由式(4.18)，(4.338)及式(4.340)计算

由式(4.25)计算

步骤3：建立动力学方程

由式(4.313)及式(4.314)得由式(4.204)及式(4.205)分别计算及代入式(4.310)得

由式(4.208)，式(4.205)及式(4.201)得

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

由式(4.269)，式(4.205)，式(4.267)及式(4.273)得

由式(4.274)及式(4.269)得

其中，差速轴初始角度为

由式(4.208)，式(4.205)及式(4.201)得

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

步骤4：进行力反向迭代

由式(4.153)得

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

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

增加驱动轮力矩约束，

记

将式(4.350)至式(4.357)写为整体形式

f_20×1＝B_20×20·u_20×1>

其中：

B_{[1：6][1：4]}＝0_6×4，>

记k∈{rb，rr，rfd，rfw，rmw，rrd，rrw，lb，lr，lfd，lfw，lmw，lrd，lrw}，由式(4.343)至式(4.348)求逆解得^i|Df_c,及共计21个标量；从而，由式(4.361)得u。

由上求解过程可知：

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

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

基于轴不变量的多轴系统力位控制

本节讨论多轴系统的力位控制伺服问题。首先，讨论力位控制的需求及必要条件，再探讨多轴力位控制的原理。

【1】力位控制需求

力位控制在多轴系统特别是机器人系统、精密加工中心当中具有非常重要的作用。

【1-1】提高作业节拍。在生产线特别是装配、机加工等作业过程中，要求提高机器人及加工中心的作业节拍，从而提高生产效率、降低生产成本；在提高多轴系统作业节拍的同时，需要考虑多轴系统动力学过程的影响。

【1-2】防止多轴系统结构的损坏，实现柔顺控制。在机器人装配、机加工过程中，包含拾取机构、切削机构、钻取机构末端执行器(End Effector)与作业对象发生硬撞击，易导致末端执行器及多轴系统的损伤，需要实现柔顺控制，即当受到过载的作用力时，系统能够具有一定的柔性，不至于产生硬撞击。因此，要求驱动轴的控制力可以根据环境的作用力实时及柔性地调节。示例4.9中的式(4.231)考虑不可测的环境作用力，目的是为通过动力学系统的力位柔顺控制奠定基础。

【1-3】提高人机协作的安全性；机器人与人合理分工，完成各自的作业劳动，需要保障人机协作的安全性。在高节拍的机器人生产线上，每年都会产生多例机器人伤人事件。一方面，需要减少人机交互的场景；另一方面，需要提升机器人根据环境对象实现柔性力位跟踪控制。例如，从技术上实现机械臂自主控制，不再需要人对机器的示教过程；从而，减少人机交互的必要性。通过视觉检测，识别人机接触的位置，通过力传感器检测人机交互的作用力；柔性地调整机器人各轴控制力。但是，目前的力传感器质量及体积过大，成本很高，系统应用复杂。需要通过动力学建模与控制实现机器人的力位柔顺控制。

【2】基于多轴系统动力学模型的力位控制前提条件

【2-1】实现环境作业力间接测量。由示例4.10及本节阐述基于Ju-Kane的CE3月面巡视器移动系统动力学建模及逆解问题。

示例4.11可知，应用多轴系统动力学逆解不仅可以计算驱动器的期望控制力矩或控制力，而且可以间接测量多轴系统与环境的作用力。因此，为多轴系统力位控制提供了原理支撑。

【2-2】紧凑型力控关节。由于力控过程的动态响应通常要比位置伺服控制的响应速度高5到10倍；要求关节驱动器具有更高的通信速率与可靠性，EtherCAT通信可以满足力控的通信需求。需要电机及减速器具有良好的力特性，通过电机驱动电流检测计算关节驱动力矩，既需要电机负载与电流的模型准确，又需要减速器力特性达到一定的精度；力矩电机及RV减速器更适应力控的要求。

【2-3】机器人控制器需要实现多轴系统运动学及动力学的实时解算，通常需要达到300～500Hz以上；即当机器人与环境作用的运动速度为300～500mm/s时，位置控制精度可以达到mm量级，为保证人身安全奠定了技术基础。

多轴刚体系统动力学方程的结构

给定系统D＝{A，K，T，NT，F，B}，|NT|＝c，|A|＝a,不可测的环境作用力独立维度为记为e，驱动轴维度记为d；则未知作用力维度为w＝d+e；系统方程数为n＝a+3c。

将由式(4.274)至式(4.277)的非树约束副代数方程写成整体形式

而驱动轴控制广义力及不可测环境作用力记为u_n；将及非树约束力合写为由式(4.260)动力学方程及非树约束副代数方程(4.372)写成整体形式

其中：u_n为驱动轴轴向广义控制力分量及未知环境作用力，B_n×n为驱动轴广义控制力及未知环境作用力的反向传递矩阵。通常B_n×n可逆，将式(4.373)表达为

其中：

由于在多轴系统动力学建模时，存在不准确的结构参数及质惯量参数，故将式(4.374)称为多轴动力学系统的名义模型或理论模型。而对应的系统工程模型或实际模型记为

因是轴向惯性力，在系统方程中是主项，故常常需要考虑的上下界，记为

其中，是下界常数，是上界常数，I是单位矩阵。

将式(4.375)及式(4.376)称多轴刚体系统动力学控制方程，属于仿射性方程组；在结构上具有以下特点：

【1】控制输入u既包含轴驱动广义力又可能包含环境作用力，这与传统的系统控制模型不同；

【2】是关于控制输入u的线性方程；

【3】是关于系统状态q及的非线性方程；

【4】关节加速度及控制输入u具有相同的维度。

在了解多轴系统控制方程结构特点的基础上，开展针对性的控制律设计，以达到多轴系统力位控制的目标。

基于线性化补偿器的多轴系统跟踪控制

给定仿射性系统

请参照图51，图51为基于线性化补偿器的多轴系统跟踪控制图。构造全局线性化补偿器^[3]：

及稳定的伺服反馈控制器^[3]：

若伺服反馈控制器(4.380)稳定，则系统(4.378)可实现状态对期望状态的跟踪控制。

其中：M′(q)及分别是M(q)及的名义模型，δq＝q_d-q。

证明：如图51所示，控制对象自身完成的是正动力学计算，线性化补偿器完成的是逆动力学计算。

【1】通过线性化补偿器实现全局线性化

将式(4.378)代入式(4.379)得

因M′(q)及分别是M(q)及的名义模型，故有将之代入式(4.381)得

式(4.382)表明：控制对象被全局线性化。

【2】通过PD伺服反馈控制消除模型不确定性带来的扰动

但实际上名义模型并不无限逼近系统模型，故由式(4.380)及式(4.382)知

式(4.383)是一个二阶n维线性系统。通过调节PD控制器参数k₀及k₁，使该系统渐近稳定，即

证毕。

由式(4.380)可知，该方法不能直接对进行控制；从而，不能实现力位跟踪控制。

基于逆模补偿器的多轴系统力位控制

阻抗控制是为消除环境作用阻力对系统产生的影响而进行的控制。环境阻抗可分为可检测的及不可检测的两大类。

记阻抗作用的位置矢量为由式(4.32)得

其中：

由虚功原理“在任意时刻，阻抗作用的功率与其作用于运动副的等广义力uⁱ所产生的功率是相等的”可知

将式(4.384)代入式(4.385)得

因是任意的，故由式(4.386)得

uⁱ是系统(4.378)外部阻抗作用于运动轴方向的等效广义力。

请参照图52，图52为基于逆模补偿器的多轴系统力位控制图。如图52所示，给定仿射性系统

构造逆模补偿器：

及稳定的伺服反馈控制器：

若伺服反馈控制器(4.390)稳定，则系统(4.388)可实现状态对期望状态的跟踪控制。

其中：M′(q)及分别是M(q)及的名义模型，δq＝q_d-q。

证明：

将式(4.388)代入式(4.389)得

因M′(q′)及分别是M(q)及的名义模型，u′ⁱ是uⁱ的名义模型；故有及由式(4.391)得

其中：δM(q)＝M′(q)-M(q)＝M₀，

将式(4.390)代入式(4.392)得

由式(4.393)得

若二阶n维线性系统(4.394)稳定，且k₂I_n×n＞＞M₀，k₁I_n＞＞h₁，k₀I_n＞＞h₀；则由式(4.394)得因此，实现跟踪控制。同时，因故基于逆模补偿器的多轴系统是一个全局线性化的系统。其中：＞＞表示远大于。

因系统(4.394)稳定，故受系统(4.390)结构参数控制，由式(4.388)得δu同样受系统(4.390)结构参数控制。因此，实现了外力增量δu的伺服控制。

当uⁱ的名义模型u′ⁱ已知时，直接通过逆模补偿器消除扰动u_f；当u′ⁱ未知时，但已知时，可以通过动力学逆解计算出外作用力u′ⁱ；当u′ⁱ未知时，且亦未知时，如图52所示，扰动力uⁱ引起系统关节加速度变更，通过PID控制器自动调节减小该扰动力对系统的影响。

基于模糊变结构的多轴系统力位控制

基于全局线性化补偿器的PD阻抗控制适用于较精确的被控对象的模型及可以检测的环境作用阻抗。当被控对象的模型不精确或环境作用阻抗不可检测时，为保证系统的控制性能，需要设计鲁棒控制器。下面，先给出预备知识，再陈述基于模糊滑模的变结构控制定理，然后予以证明。

定义4.1：一个n×n实对称矩阵称作正定，如果对于一个所有元素均非0的实向量且x≠0；如果x^TBx＞0，那么B是正定矩阵^[4,5]。如果上面的严格不等式被弱化为那么B称为半正定矩阵。

定义4.2：对于所有的使标量函数f(x)：R→R正定的条件是^[4,5]：

(1)f(0)＝0；(2)f(x)＞0；(3)f(x)连续；(4)连续；如果上面的条件(2)被弱化为f(x)≥0，那么f(x)是半正定的。

定义4.3：假设有A，B∈R^n×n是实对称矩阵，那么当且仅当矩阵是A-B正定时，A＞B；类似地，A-B是半正定时，A≥B^[4,5]。

定义4.4：当存在一个正数α＞0时，如果B(t)≥α·I，那么时变矩阵B(t)∈R^n×n是一致正定的。

推论4.1：当且仅当特征值是正数(或非负)时，实对称矩阵B∈R^n×n是正定的(或半正定的)。

推论4.2：当且仅当逆矩阵B^-1是正定时，非奇异矩阵B∈R^n×n是正定的。

推论4.3：如果A，B∈R^n×n均为实对称矩阵并且A≥B，那么对于任意向量x∈Rⁿ均有

x^T·A·x≥x^T·B·x>

定理4.7：如果矩阵A∈R^n×n是非奇异并具有独立特征值，那么就存在一个相似变换A＝V^-1·δ(A)·V，δ(A)是由A的特征值构成的对角矩阵，而V是由特征向量组成的非奇异矩阵^[4,5]。

定理4.8：如果A，B∈R^n×n是n×n的具有独立特征值的非奇异矩阵，且A和B可以互换，即AB＝BA，那么他们有相同的特征向量满足A＝V^-1·δ(A)·V和B＝V^-1·δ(B)·V。其中δ(A)和δ(B)分别是A和B关于特征向量V的对角矩阵^[4,5]。

定理4.9：有一个n×n的正定矩阵B∈R^n×n和两个任意向量x，y∈Rⁿ，下面的不等式称为“普遍Cauchy-Schwarz不等式”：

根据上面的定理，有下面结论：

推论4.4：如果A，B∈R^n×n是正定的或半正定的，且A和B互换，即A·B＝B·A，那么矩阵A·B也是正定的或半正定的^[4,5]。

证明：如果A和B互换，根据定理4.8，有：A·B＝V^-1·δ(A)·V·V^-1·δ(B)·V＝V^-1·δ(A·B)·V

其中δ(A·B)＝δ(A)·δ(B)，A和B的特征值是A和B的特征值的乘积。根据推论4.1，如果A和B是正定或半正定的，那么它们的特征值均为正数(非负)，因而，它们的乘积也是正数(非负)。由此，证明了A·B是正定的或半正定的。

定理4.10：假设有一个n×n的正定矩阵B∈R^n×n且存在一个正实数b＞0使得b·I＞B，I是n×n的单位矩阵。假设任一向量y∈Rⁿ且||y||≤ρ，那么对于任意向量x∈Rⁿ有下面不等式^[4，5]：

x^T·B·y≤b·ρ·||x||>

证明：因为b·I-B是半正定的，那么对于任意向量x，y∈Rⁿ有：

x^T·(b·I-B)·x≥0

y^T·(b·I-B)·y≥0

故有

x^T·B·x≤b·||x||²

y^T·By≤b·||y||²。

由||y||＜ρ及式(4.395)，得

y^T·B·y≤b·ρ²>

根据定理4.9，在有界向量y和任意向量x，有：

由式(4.399)及式(4.398)得

因而

定理4.11：考虑M∈R^n×n是n×n正定矩阵且K∈R^n×n是n×n对角正定矩阵。如果存在一个正实数m＞0且m·I≥M，那么对于任意向量x∈Rⁿ有^[4，5]：

m·x^T·M^-1·K·x≥x^T·K·x>

证明：m·I≥M意味着m·I-M是半正定的。根据推论4.2，如果M是正定的，那么M^-1也是正定的。因为K是对角型，那么M^-1·K可以互换，根据推论4.4，M^-1·K也是正定的。因而有

(m·I-M)·M^-1·K＝M^-1·K·(m·I-M)

根据推论4.4，(mI-M)·M^-1·K半正定的条件是：

(mI-M)·M^-1·K＝M^-1·K·(m·I-M)或

根据推论4.3，对于任意向量x∈R^n×n有：

或m·x^T·M^-1·K·x≥x^T·K·x

亦即

模糊变结构控制

定理4.12：请参照图53，图53为模糊变结构控制框图。如图53所示，考虑输入为u＝[u_[1]，…u_[i]，…u_[n]]^T、状态为q＝[q_[1]，…q_[i]，…q_[n]]^T的仿射性动力学系统。

【1】系统结构与参数包含：

【1.1】仿射性系统模型记为

【1.2】名义仿射性系统模型记为

其中：q∈Rⁿ，及分别表示M′的上确界与下确界且为正定函数，M′∈R^n×n,h′∈R^n×n，u′∈Rⁿ。

【1.3】令两系统模型误差满足

【1.4】系统偏差记为

e＝q-q_d；>

其中：q_d为系统控制目标。

【1.5】广义误差记为

滑模超平面为

其中：

P＝2Λ，Q＝Λ²，>

Λ为n×n正定对角阵，λ_[i]为Λ的对角元素，且λ_[i]＞0。

【1.6】参考加速度记为

【1.7】滑模(Sliding mode)边界厚度记为φ_[i]，φ_[i]表示广义误差控制边界，且φ_[i]＞0；广义误差控制边界内的状态q_[i]的集合为

N_[i](s_[i]，φ_[i])＝{q_[i]∈R：|s_[i]|≤φ_[i]}；>

称N_[i](s_[i]，φ_[i])之为滑模面的邻域。

【2】控制目标与控制律

若期望该闭环控制系统由初态q(0)到滑模面的控制过程满足Lyapunov-like稳定，即

其中：η_[i]＞0，φ_[i]＞0；则该系统的模糊滑模控制律为^[4，5]：

请参照图54，图54为模糊变结构控制控制律图。如图54所示，且第i个控制输入满足：

【2.1】当s_[i]＞0时，

【2.2】当s_[i]＜0时，

【2.3】连续单调，当s_[i]＝0时

证明：由式(4.407)及式(4.408)知：滑模面的特征根为-λ_[i]，又λ_[i]＞0，故按指数趋向稳定。故任意非零初始状态q(0)能够到达滑模面。

由式(4.411)可知，对于在式(4.410)表示的滑模面邻域N_[i](s_[i]，φ_[i])之外状态的广义误差满足渐近稳定，从而避免在滑模控制中的抖颤效应。

由式(4.405)及式(4.407)得

由式(4.374)得

将式(4.417)代入(4.416)得

将式(4.409)代入(4.418)式得

将式(4.412)代入式(4.419)得

由式(4.404)及式(4.420)得

其中：为系统不确定向量，控制量u_c是针对系统不稳定性的补偿输入。

由式(4.421)知

由式(4.404)、式(4.422)、式(4.377)及定理4.10可知

因

及式(4.422)得

由式(4.425)，选择u_c满足如下条件

以保证即保证系统广义误差s_[i]→0。式(4.426)即为

选择作为si的函数，满足以下条件：

【1】是连续函数；【2】当0＜|s_i|＜φ_i时，是单调递减的；【3】

因为：当时，式(4.427)成立；当0＜|s_i|＜φ_i时，单调递减，式(4.427)必成立。所以，取为

u_c＝-G(s_i)·Ω(s_i)·s；>

其中：G是一个n×n的正定对角矩阵，其中g_i(s_i)是G的对角元素，是正定函数；Ω也是n×n的正定对角线矩阵，1/|s_i|是对角元素；

当s_i＝0时，G(s_i)·Ω(s_i)的对角元素为0。显然，式(4.428)能保证式(4.427)成立。

定义

将式(4.428)代入式(4.427)得

因为G(s_i)·Ω(s_i)至少是半正定的，根据定理4.11得

由式(4.431)，考虑式(4.431)及式(4.430)，若令

成立，又因s_i·dsgn(s_i)＝|s_i|，则需

成立；式(4.433)是式(4.428)中控制输入u_c的约束条件。

式(4.428)等价为

即当s_i＞0时，

当s_i＜0时

由式(4.435)及式(4.436)分别得式(4.413)、式(4.414)成立。

证毕。

基于滑模的鲁棒控制较传统滑模控制具有以下优点：【1】对不精确的动力学模型具有鲁棒控制能力；【2】由于滑模控制律使用了软切换，较传统滑模控制的硬切换，可以大大降低“震颤”，减小了对系统的硬冲击；基于滑模的鲁棒控制仅适用于系统输入与系统独立状态数一致的系统；对于系统输入与系统独立状态数不一致的欠控制系统或冗余控制系统不适用；欠控制系统或冗余控制系统不能通过简单的控制完成，它们本质上是具有在线规划功能的控制问题。

多轴系统力位控制示例

应用示例4.11的CE3月面巡视器动力学方程作为巡视器名义模型，应用第一部分基于模糊变结构的多轴系统力位控制，应用第一部分的巡视器动力学仿真环境作为巡视器系统模型，进行仿真验证。

巡视器各轴坐标为期望控制为仿真表明：

【1】基于Ju-Kane方法的巡视器动力学计算耗时仅为牛顿―欧拉方法动力学的1/16至1/20；

【2】基于模糊变结构的多轴控制原理正确，满足巡视器控制工程需求；

【3】基于Ju-Kane方法的巡视器动力学不仅可以应用于实时控制，也可以实时计算巡视器与环境的作用力。

第四部分。补充说明

根据上述的说明，可以理解，本发明提供了一整套建立多轴机器装置的运动与力学模型，以及对应的计算机制。透过证明跟举例，可清楚知道这套技术方案可以实做成各种的控制与分析系统。

除了这边所举例的系统跟方法范例，其他的变形或替换应该也视为落入本发明的保护范围。

去获取专利，查看全文>