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

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

Info

Publication number
CN108942943B
CN108942943B CN201810933676.4A CN201810933676A CN108942943B CN 108942943 B CN108942943 B CN 108942943B CN 201810933676 A CN201810933676 A CN 201810933676A CN 108942943 B CN108942943 B CN 108942943B
Authority
CN
China
Prior art keywords
vector
axis
invariant
formula
reference base
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201810933676.4A
Other languages
English (en)
Other versions
CN108942943A (zh
Inventor
居鹤华
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Individual
Original Assignee
Individual
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Individual filed Critical Individual
Priority to CN201810933676.4A priority Critical patent/CN108942943B/zh
Priority to PCT/CN2018/112658 priority patent/WO2020034403A1/zh
Publication of CN108942943A publication Critical patent/CN108942943A/zh
Application granted granted Critical
Publication of CN108942943B publication Critical patent/CN108942943B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/1607—Calculation of inertia, jacobian matrixes and inverses

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Automation & Control Theory (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Manipulator (AREA)

Abstract

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

Description

基于轴不变量的多轴机器人正运动学计算方法
技术领域
本发明涉及一种多轴机器人正运动学计算方法，属于机器人技术领域。
背景技术
机器人是目前非常热门的领域。这个领域过去几十年已经投入了大量的科学与工程人力，而且研究了多年。然而，一旦遇到轴的数目与自由度增加到一定数量的时候，按照现有的教科书以及已知的观察、建模、计算与控制方式，往往会陷入复杂失控，甚至无法求解的难题。
首先，过去的做法缺少一般化的能力。对于不同的机器人往往需要重新研究，建立对应的运动学与力学模型。
其次，过去的做法在建模的过程，所用的图示以及语言通常是不精确也不完整的。这导致许多的参数在建模的初期并没有被考虑进去。后续的整个建模，包括程序化代码的编写时，就必须个别考虑跟解决之前没有考虑的参数与细节。这对于复杂系统，例如自由度更高的机器人应用时，往往意味着大量的隐藏臭虫(bug)会藏在整个建模的系统里头。这会影响整个系统开发的效率，并且透过这种没有完整考量下开发出来的系统，往往有很多稳定性问题难以解决。
此外，过去的做法在遇到复杂度比较高的时候，运算量会大幅增加或甚至找不出解答，以及计算精度大受影响。换言之，对于需要即时运算控制以达成自主控制的机器人来说，就成了重大的缺陷。
因此，虽然现在已经有很多的机器人相关的理论，但是却仍然缺少一个完整有效的设计框架与对应的运算与控制方法，可以在各种不同机器人实际开发过程中，方方面面地解决建模，模型内的运算结构与规则，到正运动学，逆运动学，以及力学计算的相关问题。
发明内容
本发明所要解决的技术问题是提供一种基于轴不变量的多轴机器人正运动学计算方法。
为解决上述技术问题，本发明采用以下技术方案：
一种基于轴不变量的多轴机器人正运动学计算方法，其特征是，
多轴机器人装置包含杆件序列与关节序列，将树链中的关节序列转换成对应的轴序列及其父轴序列，所述轴序列的轴为平动轴或转动轴；
使用轴集合来对应描述多轴机器装置，以自然坐标系为基础，利用轴集合的轴对应的轴不变量来计算多轴机器装置的控制参数；
利用轴不变量的不变性建立基于轴不变量的迭代式运动学方程，并且所述迭代式运动学方程的符号对应到伪代码，反映所述多轴机器装置运动链的拓扑关系与链序关系；
计算运动链的迭代式正运动学数值；
计算基于轴不变量的迭代式偏速度。
惯性空间记为i，给定由i至杆件n的运动链iln，杆件l,n,j∈A，n>l，s是体l上的任一点，A为轴序列；当转动矢量
Figure GDA0001783397720000021
有测量噪声时，运动链iln的迭代式正运动学数值计算步骤包括：
【1】链节
Figure GDA0001783397720000022
正运动学计算步骤；
运动副
Figure GDA0001783397720000023
对应的运动链
Figure GDA0001783397720000024
通过区间符表示为：
Figure GDA0001783397720000025
其中：
Figure GDA0001783397720000026
是l的前继即父，l是
Figure GDA0001783397720000027
的后继即子；
Figure GDA0001783397720000028
为链节，是运动链中的一个基本环节；
【2】运动链iln的位形计算步骤；
【3】运动链iln的速度及加速度计算步骤。
链节
Figure GDA0001783397720000029
正运动学计算步骤为：
【1-1】已知转动矢量
Figure GDA00017833977200000210
根据欧拉四元数公式计算欧拉四元数
Figure GDA00017833977200000211
【1-2】由欧拉四元数计算旋转变换阵
Figure GDA00017833977200000212
【1-3】由下式计算链节速度：
Figure GDA00017833977200000213
式中，运动副
Figure GDA00017833977200000214
表示连接杆件l及杆件l的运动副；转动副R，棱柱副P；沿轴
Figure GDA00017833977200000215
的线位置
Figure GDA00017833977200000216
绕轴
Figure GDA00017833977200000217
的角位置
Figure GDA00017833977200000218
轴矢量
Figure GDA00017833977200000219
角速度
Figure GDA00017833977200000220
线速度
Figure GDA00017833977200000221
【1-4】由下式计算链节加速度：
Figure GDA00017833977200000222
式中，转动加速度
Figure GDA00017833977200000223
平动加速度
Figure GDA00017833977200000224
运动链iln的位形计算步骤为：
【2-1】由欧拉四元数的链关系，将四元数乘法运算用其共轭矩阵运算替代，计算欧拉四元数序列
Figure GDA0001783397720000031
【2-2】用欧拉四元数表示定轴转动；旋转变换阵的计算等价于链式四元数的矩阵计算，计算旋转变换阵序列{iQj|j∈A}；
【2-3】由下式计算位置矢量
Figure GDA0001783397720000032
Figure GDA0001783397720000033
式中，轴矢量
Figure GDA0001783397720000034
沿轴
Figure GDA0001783397720000035
的线位置
Figure GDA0001783397720000036
运动链iln的速度及加速度计算步骤为：
【3-1】计算绝对角速度：
Figure GDA0001783397720000037
式中，角速度
Figure GDA0001783397720000038
轴矢量
Figure GDA0001783397720000039
【3-2】计算绝对角加速度：
Figure GDA00017833977200000310
式中，角加速度
Figure GDA00017833977200000311
轴矢量
Figure GDA00017833977200000312
【3-3】计算绝对平动速度：
Figure GDA00017833977200000313
式中，线速度
Figure GDA00017833977200000314
轴矢量
Figure GDA00017833977200000315
【3-4】计算绝对平动加速度：
Figure GDA00017833977200000316
式中，线速度
Figure GDA00017833977200000317
轴矢量
Figure GDA00017833977200000318
其中：
Figure GDA00017833977200000323
—转动加速度，其中
Figure GDA00017833977200000320
向心加速度；
Figure GDA00017833977200000321
—哥氏加速度，是平动与转动的耦合加速度。
基于轴不变量的迭代式偏速度计算步骤包括：
定义使能函数：
Figure GDA00017833977200000322
式中，k为属于运动链iln的杆件；
由使能函数，
【1】计算绝对角速度对关节角速度的偏速度；
【2】计算绝对平动速度矢量对关节平动速度的偏速度；
【3】计算绝对转动矢量对关节角度的偏速度；
【4】计算绝对位置矢量对关节位移的偏速度；
【5】计算绝对位置矢量对关节角度的偏速度；
【6】计算绝对平动速度矢量对关节角速度的偏速度。
绝对角速度对关节角速度的偏速度计算公式为：
Figure GDA0001783397720000041
式中，角速度
Figure GDA0001783397720000042
轴矢量
Figure GDA0001783397720000043
绝对平动速度矢量对关节平动速度的偏速度计算公式为：
Figure GDA0001783397720000044
式中，线速度
Figure GDA0001783397720000045
轴矢量
Figure GDA0001783397720000046
绝对平动速度矢量对关节角速度的偏速度计算公式为：
Figure GDA0001783397720000047
式中，角速度
Figure GDA0001783397720000048
Figure GDA0001783397720000049
是轴不变量
Figure GDA00017833977200000410
的叉乘矩阵。
绝对转动矢量对关节角度的偏速度计算公式为：
Figure GDA00017833977200000411
式中，角位置
Figure GDA00017833977200000412
轴矢量
Figure GDA00017833977200000413
绝对位置矢量对关节位移的偏速度计算公式为：
Figure GDA00017833977200000414
式中，线位置
Figure GDA00017833977200000415
轴矢量
Figure GDA00017833977200000416
绝对位置矢量对关节角度的偏速度计算公式为：
Figure GDA00017833977200000417
式中，角位置
Figure GDA00017833977200000418
Figure GDA00017833977200000419
是轴不变量
Figure GDA00017833977200000420
的叉乘矩阵。
本发明所达到的有益效果：
本发明的方法提出并证明了基于轴不变量的迭代式运动学实时数值建模方法，包含：基于轴不变量的迭代式的位置矢量、转动矢量、速度矢量、加速度矢量及偏速度矢量的计算方法。具有简洁的运动链符号系统，具有伪代码的功能，具有迭代式结构，保证了系统实现的可靠性及机械化演算；具有基于轴不变量的迭代式，保证计算的实时性；实现坐标系、极性及系统结构参量的完全参数化建模，保证模型的通用性，避免了与系统接口与用户接口的转换，通过轴不变量构建了内在紧凑系统，提高了运动学计算的实时性及功能复用性能；轴运动矢量的统一表达及简洁的结构化层次模型不仅有助于简化多轴系统运动学建模过程，而且为基于轴不变量的多轴系统动力学建模奠定了基础。
附图说明
图1自然坐标系与轴链；
图2固定轴不变量；
图3偏速度的含义示意图。
具体实施方式
下面结合附图对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。
定义1自然坐标轴：称与运动轴或测量轴共轴的，具有固定原点的单位参考轴为自然坐标轴，亦称为自然参考轴。
定义2自然坐标系：如图1所示，若多轴系统D处于零位，所有笛卡尔体坐标系方向一致，且体坐标系原点位于运动轴的轴线上，则该坐标系统为自然坐标系统，简称自然坐标系。
自然坐标系优点在于：(1)坐标系统易确定；(2)零位时的关节变量为零；(3)零位时的系统姿态一致；(4)不易引入测量累积误差。
由定义2可知，在系统处于零位时，所有杆件的自然坐标系与底座或世界系的方向一致。系统处于零位即
Figure GDA0001783397720000051
时，自然坐标系
Figure GDA0001783397720000052
绕轴矢量
Figure GDA0001783397720000053
转动角度
Figure GDA0001783397720000054
将
Figure GDA0001783397720000055
转至F[l]；
Figure GDA0001783397720000056
在
Figure GDA0001783397720000057
下的坐标矢量与
Figure GDA0001783397720000058
在F[l]下的坐标矢量
Figure GDA00017833977200000521
恒等，即有
Figure GDA00017833977200000510
由上式知，
Figure GDA00017833977200000511
或
Figure GDA00017833977200000512
不依赖于相邻的坐标系
Figure GDA00017833977200000513
及F[l]；故称
Figure GDA00017833977200000514
或
Figure GDA00017833977200000515
为轴不变量。在不强调不变性时，可以称之为坐标轴矢量(简称轴矢量)。
Figure GDA00017833977200000516
或
Figure GDA00017833977200000517
表征的是体
Figure GDA00017833977200000518
与体l共有的参考单位坐标矢量，与参考点
Figure GDA00017833977200000519
及Ol无关。体
Figure GDA00017833977200000520
与体l即为杆件或轴。
轴不变量与坐标轴具有本质区别：
(1)坐标轴是具有零位及单位刻度的参考方向，可以描述沿该方向平动的位置，但不能完整描述绕该方向的转动角度，因为坐标轴自身不具有径向参考方向，即不存在表征转动的零位。在实际应用时，需要补充该轴的径向参考。例如：在笛卡尔系F[l]中，绕lx转动，需以ly或lz为参考零位。坐标轴自身是1D的，3个正交的1D参考轴构成3D的笛卡尔标架。
(2)轴不变量是3D的空间单位参考轴，其自身就是一个标架。其自身具有径向参考轴，即参考零位。空间坐标轴及其自身的径向参考轴可以确定笛卡尔标架。空间坐标轴可以反映运动轴及测量轴的三个基本参考属性。
已有文献将无链指标的轴矢量记为
Figure GDA0001783397720000061
并称之为欧拉轴(Euler Axis)，相应的关节角称为欧拉角(Euler Angle)。本申请之所以不再沿用欧拉轴，而称之为轴不变量，是因为轴不变量具有以下属性：
【1】给定旋转变换阵
Figure GDA0001783397720000062
因其是实矩阵，其模是单位的，故其有一个实特征值λ1及两个互为共轭的复特征值λ2＝eiφ及λ3＝e-iφ；其中：i为纯虚数。因此，|λ1|·||λ2||·||λ3||＝1，得λ1＝1。轴矢量
Figure GDA0001783397720000063
是实特征值λ1＝1对应的特征矢量，是不变量；
【2】是3D参考轴，不仅具有轴向参考方向，而且具有径向参考零位，将在3.3.1节予以阐述。
【3】在自然坐标系下：
Figure GDA0001783397720000064
即轴不变量
Figure GDA0001783397720000065
是非常特殊的矢量，它对时间的导数也具有不变性，且有非常优良的数学操作性能；
对轴不变量而言，其绝对导数就是其相对导数。因轴不变量是具有不变性的自然参考轴，故其绝对导数恒为零矢量。因此，轴不变量具有对时间微分的不变性。有：
Figure GDA0001783397720000066
【4】在自然坐标系统中，通过轴矢量
Figure GDA0001783397720000067
及关节变量
Figure GDA0001783397720000068
可以直接描述旋转坐标阵
Figure GDA0001783397720000069
没有必要为除根之外的杆件建立各自的体系。同时，以唯一需要定义的根坐标系为参考，可以提高系统结构参数的测量精度；
【5】应用轴矢量
Figure GDA00017833977200000610
的优良操作，将建立包含拓扑结构、坐标系、极性、结构参量及力学参量的完全参数化的统一的多轴系统运动学及动力学模型。
因基矢量el是与F[l]固结的任一矢量，基矢量
Figure GDA00017833977200000611
是与
Figure GDA00017833977200000612
固结的任一矢量，又
Figure GDA00017833977200000613
是F[l]及
Figure GDA00017833977200000614
共有的单位矢量，故
Figure GDA00017833977200000615
是F[l]及
Figure GDA00017833977200000616
共有的基矢量。因此，轴不变量
Figure GDA00017833977200000617
是F[l]及
Figure GDA00017833977200000618
共有的参考基。轴不变量是参数化的自然坐标基，是多轴系统的基元。固定轴不变量的平动与转动与其固结的坐标系的平动与转动等价。
在系统处于零位时，以自然坐标系为参考，测量得到坐标轴矢量
Figure GDA00017833977200000619
在运动副
Figure GDA00017833977200000620
运动时，轴矢量
Figure GDA00017833977200000621
是不变量；轴矢量
Figure GDA00017833977200000622
及关节变量
Figure GDA00017833977200000623
唯一确定运动副
Figure GDA00017833977200000624
的转动关系。
因此，应用自然坐标系统，当系统处于零位时，只需确定一个公共的参考系，而不必为系统中每一杆件确定各自的体坐标系，因为它们由轴不变量及自然坐标唯一确定。当进行系统分析时，除底座系外，与杆件固结的其它自然坐标系只发生在概念上，而与实际的测量无关。自然坐标系统对于多轴系统(MAS)理论分析及工程作用在于：
(1)系统的结构参数测量需要以统一的参考系测量；否则，不仅工程测量过程烦琐，而且引入不同的体系会引入更大的测量误差。
(2)应用自然坐标系统，除根杆件外，其它杆件的自然坐标系统由结构参量及关节变量自然确定，有助于MAS系统的运动学与动力学分析。
(3)在工程上，可以应用激光跟踪仪等光学测量设备，实现对固定轴不变量的精确测量。
(4)由于运动副R及P、螺旋副H、接触副O是圆柱副C的特例，可以应用圆柱副简化MAS运动学及动力学分析。
定义3不变量：称不依赖于一组坐标系进行度量的量为不变量。
定义4转动坐标矢量：绕坐标轴矢量
Figure GDA0001783397720000071
转动到角位置
Figure GDA0001783397720000072
的坐标矢量
Figure GDA0001783397720000073
为
Figure GDA0001783397720000074
定义5平动坐标矢量：沿坐标轴矢量
Figure GDA0001783397720000075
平动到线位置
Figure GDA0001783397720000076
的坐标矢量
Figure GDA0001783397720000077
为
Figure GDA0001783397720000078
定义6自然坐标：以自然坐标轴矢量为参考方向，相对系统零位的角位置或线位置，记为ql，称为自然坐标；称与自然坐标一一映射的量为关节变量；其中：
Figure GDA0001783397720000079
定义7机械零位：对于运动副
Figure GDA00017833977200000710
在初始时刻t0时，关节绝对编码器的零位
Figure GDA00017833977200000711
不一定为零，该零位称为机械零位；
故关节
Figure GDA00017833977200000712
的控制量
Figure GDA00017833977200000713
为
Figure GDA00017833977200000714
定义8自然运动矢量：将由自然坐标轴矢量
Figure GDA00017833977200000715
及自然坐标ql确定的矢量
Figure GDA00017833977200000716
称为自然运动矢量。其中：
Figure GDA00017833977200000717
自然运动矢量实现了轴平动与转动的统一表达。将由自然坐标轴矢量及关节确定的矢量，例如
Figure GDA0001783397720000081
称为自由运动矢量，亦称为自由螺旋。显然，轴矢量
Figure GDA0001783397720000082
是特定的自由螺旋。
定义9关节空间：以关节自然坐标ql表示的空间称为关节空间。
定义10位形空间：称表达位置及姿态(简称位姿)的笛卡尔空间为位形空间，是双矢量空间或6D空间。
定义11自然关节空间：以自然坐标系为参考，通过关节变量
Figure GDA0001783397720000083
表示，在系统零位时必有
Figure GDA0001783397720000084
的关节空间，称为自然关节空间。
如图2所示，给定链节
Figure GDA0001783397720000085
原点Ol受位置矢量
Figure GDA0001783397720000086
约束的轴矢量
Figure GDA0001783397720000087
为固定轴矢量，记为
Figure GDA0001783397720000088
其中：
Figure GDA0001783397720000089
轴矢量
Figure GDA00017833977200000810
是关节自然坐标的自然参考轴。因
Figure GDA00017833977200000811
是轴不变量，故称
Figure GDA00017833977200000812
为固定轴不变量，它表征了运动副
Figure GDA00017833977200000813
的结构关系，即确定了自然坐标轴。固定轴不变量
Figure GDA00017833977200000814
是链节
Figure GDA00017833977200000815
结构参数的自然描述。
定义12自然坐标轴空间：以固定轴不变量作为自然参考轴，以对应的自然坐标表示的空间称为自然坐标轴空间，简称自然轴空间。它是具有1个自由度的3D空间。
如图2所示，
Figure GDA00017833977200000816
及
Figure GDA00017833977200000817
不因杆件Ωl的运动而改变，是不变的结构参考量。
Figure GDA00017833977200000818
确定了轴l相对于轴
Figure GDA00017833977200000819
的五个结构参数；与关节变量ql一起，完整地表达了杆件Ωl的6D位形。给定
Figure GDA00017833977200000820
时，杆件固结的自然坐标系可由结构参数
Figure GDA00017833977200000821
及关节变量
Figure GDA00017833977200000822
唯一确定。称轴不变量
Figure GDA00017833977200000823
固定轴不变量
Figure GDA00017833977200000824
关节变量
Figure GDA00017833977200000825
及
Figure GDA00017833977200000826
为自然不变量。显然，由固定轴不变量
Figure GDA00017833977200000827
及关节变量
Figure GDA00017833977200000828
构成的关节自然不变量
Figure GDA00017833977200000829
与由坐标系
Figure GDA00017833977200000830
至F[l]确定的空间位形
Figure GDA00017833977200000831
具有一一映射关系，即
Figure GDA00017833977200000832
给定多轴系统D＝{T,A,B,K,F,NT}，在系统零位时，只要建立底座系或惯性系，以及各轴上的参考点Ol，其它杆件坐标系也自然确定。本质上，只需要确定底座系或惯性系。
给定一个由运动副连接的具有闭链的结构简图，可以选定回路中任一个运动副，将组成该运动副的定子与动子分割开来；从而，获得一个无回路的树型结构，称之为Span树。T表示带方向的span树，以描述树链运动的拓扑关系。
I为结构参数；A为轴序列，F为杆件参考系序列，B为杆件体序列，K为运动副类型序列，NT为约束轴的序列即非树。
Figure GDA0001783397720000091
为取轴序列
Figure GDA0001783397720000092
的成员。转动副R，棱柱副P，螺旋副H，接触副O是圆柱副C的特例。
描述运动链的基本拓扑符号及操作是构成运动链拓扑符号系统的基础，定义如下：
【1】运动链由偏序集合(]标识。
【2】A[l]为取轴序列A的成员；因轴名l具有唯一的编号对应于A[l]的序号，故A[l]计算复杂度为O(1)。
【3】
Figure GDA0001783397720000093
为取轴l的父轴；轴
Figure GDA0001783397720000094
的计算复杂度为O(1)。计算复杂度O()表示计算过程的操作次数，通常指浮点乘与加的次数。以浮点乘与加的次数表达计算复杂度非常烦琐，故常采用算法循环过程中的主要操作次数；比如：关节位姿、速度、加速度等操作的次数。
【4】
Figure GDA0001783397720000095
为取轴序列
Figure GDA0001783397720000096
的成员；
Figure GDA0001783397720000097
计算复杂度为O(1)。
【5】llk为取由轴l至轴k的运动链，输出表示为
Figure GDA0001783397720000098
且
Figure GDA0001783397720000099
基数记为|llk|。llk执行过程：执行
Figure GDA00017833977200000910
若
Figure GDA00017833977200000911
则执行
Figure GDA00017833977200000912
否则，结束。llk计算复杂度为O(|llk|)。
【6】ll为取轴l的子。该操作表示在
Figure GDA00017833977200000913
中找到成员l的地址k；从而，获得轴l的子A[k]。因
Figure GDA00017833977200000914
不具有偏序结构，故ll的计算复杂度为
Figure GDA00017833977200000915
【7】lL表示获得由轴l及其子树构成的闭子树， l L为不含l的子树；递归执行ll，计算复杂度为
Figure GDA00017833977200000916
【8】支路、子树及非树弧的增加与删除操作也是必要的组成部分；从而，通过动态Span树及动态图描述可变拓扑结构。在支路llk中，若
Figure GDA00017833977200000917
则记
Figure GDA00017833977200000918
Figure GDA00017833977200000919
即
Figure GDA00017833977200000920
表示在支路中取成员m的子。
定义以下表达式或表达形式：
轴与杆件具有一一对应性；轴间的属性量
Figure GDA00017833977200000921
及杆件间的属性量
Figure GDA00017833977200000922
具有偏序性。
约定：
Figure GDA00017833977200000927
表示属性占位；若属性p或P是关于位置的，则
Figure GDA00017833977200000923
应理解为坐标系
Figure GDA00017833977200000924
的原点至F[l]的原点；若属性p或P是关于方向的，则
Figure GDA00017833977200000925
应理解为坐标系
Figure GDA00017833977200000926
至F[l]。
Figure GDA0001783397720000101
及
Figure GDA0001783397720000102
应分别理解为关于时间t的函数
Figure GDA0001783397720000103
及
Figure GDA0001783397720000104
且
Figure GDA0001783397720000105
及
Figure GDA0001783397720000106
是t0时刻的常数或常数阵列。但是正体的
Figure GDA0001783397720000107
及
Figure GDA0001783397720000108
应视为常数或常数阵列。
本申请中约定：在运动链符号演算系统中，具有偏序的属性变量或常量，在名称上包含表示偏序的指标；要么包含左上角及右下角指标，要么包含右上角及右下角指标；它们的方向总是由左上角指标至右下角指标，或由右上角指标至右下角指标，本申请中为叙述简便，有时省略方向的描述，即使省略，本领域技术人员通过符号表达式也可以知道，本申请中采用的各参数，对于某种属性符，它们的方向总是由偏序指标的左上角指标至右下角指标，或由右上角指标至右下角指标。例如：
Figure GDA0001783397720000109
可简述为(表示由k至l)平动矢量；
Figure GDA00017833977200001010
表示(由k至l的)线位置；krl表示(由k至l的)平动矢量；其中：r表示“平动”属性符，其余属性符对应为：属性符φ表示“转动”；属性符Q表示“旋转变换矩阵”；属性符l表示“运动链”；属性符u表示“单位矢量”；属性符w表示“角速度”；角标为i表示惯性坐标系或大地坐标系；其他角标可以为其他字母，也可以为数字。
本申请的符号规范与约定是根据运动链的偏序性、链节是运动链的基本单位这两个原则确定的，反映了运动链的本质特征。链指标表示的是连接关系，右上指标表征参考系。采用这种符号表达简洁、准确，便于交流与书面表达。同时，它们是结构化的符号系统，包含了组成各属性量的要素及关系，便于计算机处理，为计算机自动建模奠定基础。指标的含义需要通过属性符的背景即上下文进行理解；比如：若属性符是平动类型的，则左上角指标表示坐标系的原点及方向；若属性符是转动类型的，则左上角指标表示坐标系的方向。
(1)lS－杆件l中的点S；而S表示空间中的一点S。
(2)
Figure GDA00017833977200001011
－杆件k的原点Ok至杆件l的原点Ol的平动矢量；
Figure GDA00017833977200001012
在自然坐标系F[k]下的坐标矢量，即由k至l的坐标矢量；
(3)
Figure GDA00017833977200001025
－原点Ok至点lS的平动矢量；
Figure GDA00017833977200001014
在F[k]下的坐标矢量；
(4)
Figure GDA00017833977200001015
－原点Ok至点S的平动矢量；
Figure GDA00017833977200001016
在F[k]下的坐标矢量；
(5)
Figure GDA00017833977200001017
－连接杆件
Figure GDA00017833977200001018
及杆件l的运动副；
Figure GDA00017833977200001019
－运动副
Figure GDA00017833977200001020
的轴矢量；
Figure GDA00017833977200001021
及
Figure GDA00017833977200001022
分别在
Figure GDA00017833977200001023
及F[l]下的坐标矢量；
Figure GDA00017833977200001024
是轴不变量，为一结构常数；
Figure GDA0001783397720000111
为转动矢量，转动矢量/角矢量
Figure GDA0001783397720000112
是自由矢量，即该矢量可自由平移；
(6)
Figure GDA0001783397720000113
－沿轴
Figure GDA0001783397720000114
的线位置(平动位置)，
Figure GDA0001783397720000115
－绕轴
Figure GDA0001783397720000116
的角位置，即关节角、关节变量，为标量；
(7)左下角指标为0时，表示机械零位；如：
Figure GDA0001783397720000117
－平动轴
Figure GDA0001783397720000118
的机械零位，
Figure GDA0001783397720000119
－转动轴
Figure GDA00017833977200001110
的机械零位；
(8)0－三维零矩阵；1－三维单位矩阵；
(9)约定：“\”表示续行符；
Figure GDA00017833977200001139
表示属性占位；则
幂符
Figure GDA00017833977200001111
表示
Figure GDA00017833977200001112
的x次幂；右上角角标∧或
Figure GDA00017833977200001140
表示分隔符；如：
Figure GDA00017833977200001113
或
Figure GDA00017833977200001114
为
Figure GDA00017833977200001115
的x次幂。
Figure GDA00017833977200001116
表示
Figure GDA00017833977200001117
的转置，表示对集合转置，不对成员执行转置；如：
Figure GDA00017833977200001118
Figure GDA00017833977200001119
为投影符，表示矢量或二阶张量对参考基的投影矢量或投影序列，即坐标矢量或坐标阵列，投影即是点积运算“·”；如：位置矢量
Figure GDA00017833977200001120
在坐标系F[k]中的投影矢量记为
Figure GDA00017833977200001121
Figure GDA00017833977200001122
为叉乘符；如：
Figure GDA00017833977200001123
是轴不变量
Figure GDA00017833977200001124
的叉乘矩阵；给定任一矢量
Figure GDA00017833977200001125
的叉乘矩阵为
Figure GDA00017833977200001126
叉乘矩阵是二阶张量。
叉乘符运算的优先级高于投影符
Figure GDA00017833977200001127
的优先级。投影符
Figure GDA00017833977200001128
的优先级高于成员访问符
Figure GDA00017833977200001129
或
Figure GDA00017833977200001130
成员访问符
Figure GDA00017833977200001131
优先级高于幂符
Figure GDA00017833977200001132
(10)单位矢量在大地坐标系的投影矢量
Figure GDA00017833977200001133
单位零位矢量
Figure GDA00017833977200001134
(11)
Figure GDA00017833977200001135
－零位时由原点
Figure GDA00017833977200001136
至原点Ol的平动矢量，且记
Figure GDA00017833977200001137
表示位置结构参数。
(12)iQl，相对绝对空间的旋转变换阵；
(13)以自然坐标轴矢量为参考方向，相对系统零位的角位置或线位置，记为ql，称为自然坐标；关节变量
Figure GDA00017833977200001138
自然关节坐标为φl；
(14)对于一给定有序的集合r＝[1,4,3,2]T，记r[x]表示取集合r的第x行元素。常记[x]、[y]、[z]及[w]表示取第1、2、3及4列元素。
(15)ilj表示由i到j的运动链；llk为取由轴l至轴k的运动链；
给定运动链
Figure GDA0001783397720000121
若n表示笛卡尔直角系，则称
Figure GDA0001783397720000122
为笛卡尔轴链；若n表示自然参考轴，则称
Figure GDA00017833977200001227
为自然轴链。
(16)Rodrigues四元数表达形式：
Figure GDA0001783397720000124
欧拉四元数表达形式：
Figure GDA0001783397720000125
不变量的四元数(也称为轴四元数)表达形式
Figure GDA0001783397720000126
运动链是一个偏序的链；而运动副
Figure GDA0001783397720000127
既表示由杆件
Figure GDA0001783397720000128
至杆件l的连接，又表示由杆件l至杆件
Figure GDA0001783397720000129
的连接，故运动副
Figure GDA00017833977200001210
具有全序；故有
Figure GDA00017833977200001211
显然，全序及偏序是一个对象自身的属性。而力学及机器人理论上尚未出现相应的符号系统。
借鉴集合论的链理论，将运动副
Figure GDA00017833977200001212
对应的简单运动链
Figure GDA00017833977200001213
通过区间符表示为
Figure GDA00017833977200001214
其中：
Figure GDA00017833977200001215
是l的前继即父，l是
Figure GDA00017833977200001216
的后继即子；称
Figure GDA00017833977200001217
为链节，是运动链中的一个基本环节。
在Span树中，简单运动链
Figure GDA00017833977200001218
与l一一映射，即
Figure GDA00017833977200001219
故有
Figure GDA00017833977200001220
因有序集合的子集也是有序的集合，故定义由
Figure GDA00017833977200001221
至κ的运动链
Figure GDA00017833977200001222
为
Figure GDA00017833977200001223
记
Figure GDA00017833977200001224
为
Figure GDA00017833977200001225
的前继(Predecessor)。故有
Figure GDA00017833977200001226
同样，因有序集合的子集也是有序的集合，故有
ili＝(i,i],|ili|＝0。 (7)
称ili为空链或平凡链。惯性空间(环境)记为i，平凡链ili总是存在的。
1.基于轴不变量的迭代式运动学计算方法
给定运动链iln，轴l,n∈A，n>l，s是体l上的任一点，A为轴序列。当转动矢量
Figure GDA0001783397720000131
有测量噪声时，运动链iln的迭代式正运动学数值计算步骤为：
【1】链节
Figure GDA0001783397720000132
正运动学计算步骤
【1-1】已知转动矢量
Figure GDA0001783397720000133
根据式(9)计算欧拉四元数
Figure GDA0001783397720000134
定义四元数
Figure GDA0001783397720000135
及保证模不变的共轭四元数
Figure GDA0001783397720000136
Figure GDA0001783397720000137
四元数
Figure GDA0001783397720000138
的虚部与实部表示的是不变量，故左上角指标不表示参考系，而仅表示链的作用关系。因此，
Figure GDA0001783397720000139
可视为四维空间的复数，其中
Figure GDA00017833977200001310
是实部，
Figure GDA00017833977200001311
是虚部。通过研究四维空间复数，人们认识了欧拉四元数。
Figure GDA00017833977200001312
前三个数构成矢量，对应基i的坐标，最后一个是实部，即有
Figure GDA00017833977200001313
因4D复数的矢部参考基是唯一的自然参考基，故四维复数的左上角的参考指标仅表明运动关系，已失去投影参考系的含义，具有不同左上角指标的4D复数可以进行代数运算。尽管参考指标在4D复数中无意义，但不表明指标关系无意义，因为复数的乘除运算与复数的作用顺序密切相关。
Figure GDA00017833977200001314
【1-2】由式(10)计算旋转变换阵
Figure GDA00017833977200001315
Figure GDA00017833977200001316
显然，有
Figure GDA0001783397720000141
【1-3】由式(12)计算链节速度：
Figure GDA0001783397720000142
运动副
Figure GDA0001783397720000143
表示连接杆件
Figure GDA0001783397720000144
及杆件l的运动副；转动副R，棱柱副P；关节转动角度矢量
Figure GDA0001783397720000145
位置矢量
Figure GDA0001783397720000146
轴矢量
Figure GDA0001783397720000147
角速度
Figure GDA0001783397720000148
线速度
Figure GDA0001783397720000149
角速度
Figure GDA00017833977200001410
【1-4】由式(13)计算链节加速度：
Figure GDA00017833977200001411
转动加速度
Figure GDA00017833977200001412
平动加速度
Figure GDA00017833977200001413
【2】运动链iln的位形计算步骤
【2-1】由式(19)计算欧拉四元数序列
Figure GDA00017833977200001414
由欧拉四元数的链关系，四元数
Figure GDA00017833977200001415
乘法运算可用其共轭矩阵
Figure GDA00017833977200001416
运算替代，有
Figure GDA00017833977200001417
其中：
Figure GDA00017833977200001418
且有
Figure GDA00017833977200001419
称
Figure GDA00017833977200001420
为
Figure GDA00017833977200001421
的共轭矩阵。同时，因为四元数是四维空间复数，矢部对参考基的矢量投影应相对于同一个参考基。称式(14)为四元数串接性运算，与齐次变换相对应。因此，序列姿态运算具有运动链串接性；与矢量叉乘运算相似，四元数乘可应用相应的共轭矩阵替代。
当给定角度
Figure GDA00017833977200001422
后，其正、余弦
Figure GDA00017833977200001423
及其半角的正、余弦Sl、Cl均是常数；为方便表达，记
Figure GDA0001783397720000151
由式(15)及式(16)，结合欧拉四元数，得
Figure GDA0001783397720000152
式(14)应用计算机编程实现时，可用下式替代。
Figure GDA0001783397720000153
式(18)仅包含16个乘法运算及12个加法运算。而
Figure GDA0001783397720000154
需要进行27个乘法运算及18个加法运算。在得到
Figure GDA0001783397720000155
后，计算
Figure GDA0001783397720000156
及
Figure GDA0001783397720000157
再由式(21)计算
Figure GDA0001783397720000158
是4·4的矩阵，其构成如下：第4列为右手序的四元数
Figure GDA0001783397720000159
第4行为左手序的四元数
Figure GDA00017833977200001510
即
Figure GDA00017833977200001511
左上3×3包含为
Figure GDA00017833977200001512
其中：
Figure GDA00017833977200001513
的右上三角为右手序的矢量
Figure GDA00017833977200001514
的左下三角为左手序的矢量
Figure GDA00017833977200001515
即
Figure GDA00017833977200001516
Figure GDA00017833977200001517
的主对角为
Figure GDA00017833977200001518
的第4个元素。
由式(18)得
Figure GDA00017833977200001519
式(14)表示的是位置矢量转动算子，即表示的是转动。因此，欧拉四元数乘积运算对应旋转变换阵的乘积运算。因此旋转变换链等价于定轴转动链，即
Figure GDA00017833977200001520
由上可知，欧拉四元数可以唯一确定旋转变换阵；旋转变换阵也可以唯一确定欧拉四元数，即欧拉四元数与旋转变换阵等价。转动矢量与规范四元数一一对应，即四元数表示定轴转动；旋转变换阵的计算等价于链式四元数的矩阵计算。
【2-2】因式(10)较式(21)计算复杂度高，故由式(21)计算旋转变换阵序列{iQj|j∈A}；
Figure GDA00017833977200001521
式(21)是关于
Figure GDA00017833977200001522
和
Figure GDA00017833977200001523
的多重线性方程，是轴不变量
Figure GDA00017833977200001524
的二阶多项式。给定自然零位矢量
Figure GDA00017833977200001525
作为
Figure GDA00017833977200001526
的零位参考，则
Figure GDA00017833977200001527
及
Figure GDA00017833977200001528
分别表示零位矢量及径向矢量。式(21)即为
Figure GDA00017833977200001529
对称部分
Figure GDA00017833977200001530
表示零位轴张量，反对称部分
Figure GDA0001783397720000161
表示径向轴张量，分别与轴向外积张量
Figure GDA00017833977200001618
正交，从而确定三维自然轴空间；式(21)仅含一个正弦及余弦运算、6个积运算及6个和运算，计算复杂度低；同时，通过轴不变量
Figure GDA0001783397720000163
及关节变量
Figure GDA0001783397720000164
实现了坐标系及极性的参数化。
【2-3】由式(22)计算位置矢量
Figure GDA0001783397720000165
Figure GDA0001783397720000166
【3】运动链iln的速度及加速度步骤
【3-1】由式(23)计算绝对角速度
Figure GDA0001783397720000167
上式可由式(24)
Figure GDA0001783397720000168
得
Figure GDA0001783397720000169
绝对导数
Figure GDA00017833977200001610
表示对投影坐标系i求绝对导数；角速度
Figure GDA00017833977200001611
轴矢量
Figure GDA00017833977200001612
式(24)表明：绝对角速度与相对角速度是等价的。
【3-2】由式(25)计算绝对角加速度
Figure GDA00017833977200001613
上式可由式(26)
Figure GDA00017833977200001614
得
Figure GDA00017833977200001615
转动加速度
Figure GDA00017833977200001616
【3-3】由式(27)计算绝对平动速度
Figure GDA00017833977200001617
上式可由式(28)求得，
Figure GDA0001783397720000171
称式(28)为“正序的绝对求导式”，牵连项
Figure GDA0001783397720000172
Figure GDA0001783397720000173
是由投影参考系i至测量参考系l的角速度叉乘矩阵；
Figure GDA0001783397720000174
结果以投影坐标系i为参考，所有和项的投影参考系具有一致性。
【3-4】由式(29)计算绝对平动加速度
Figure GDA0001783397720000175
上式可由式(30)得
Figure GDA0001783397720000176
其中：
Figure GDA0001783397720000177
—平动加速度；
Figure GDA0001783397720000178
—转动加速度，其中
Figure GDA0001783397720000179
向心加速度；
Figure GDA00017833977200001710
—哥氏加速度，是平动与转动的耦合加速度。
由式(30)可知，平动加速度
Figure GDA00017833977200001711
是矢量，具有可加性。
2.基于轴不变量的偏速度计算方法
现有技术中通常采用雅克比矩阵的计算方法，但均未对结论进行证明且结论不全面。在运动学及动力学分析时，将雅克比矩阵称为偏速度更合适。因为雅克比矩阵泛指偏导数，不一定具有可加性；而在运动学及动力学中偏速度特指矢量对关节变量的偏导数，具有可加性。偏速度是对应速度的变换矩阵，是对单位方向矢量的矢量投影。在运动学分析及动力学分析中，偏速度起着关键性的作用，偏速度的计算是动力学系统演算的基本前提。
首先，定义使能(Enable)函数，
Figure GDA0001783397720000181
式中，k为属于运动链ill的杆件；
式(31)的特殊形式为
Figure GDA0001783397720000182
下面，说明基于轴不变量的迭代式偏速度计算步骤：
【1】根据式(33)计算绝对角速度对关节角速度的偏速度，
Figure GDA0001783397720000183
上式可由式(23)得
Figure GDA0001783397720000184
【2】根据式(34)计算绝对平动速度矢量对关节平动速度的偏速度，
Figure GDA0001783397720000185
即
Figure GDA0001783397720000191
【3】根据式(35)计算绝对转动矢量对关节角度的偏速度，
Figure GDA0001783397720000192
上式可由式(23)得
Figure GDA0001783397720000193
【4】根据式(36)计算绝对位置矢量对关节位移的偏速度，
Figure GDA0001783397720000194
上式可由式(22)得
Figure GDA0001783397720000195
【5】根据式(37)计算绝对位置矢量对关节角度的偏速度，
Figure GDA0001783397720000196
上式可由式(27)得
Figure GDA0001783397720000201
即
Figure GDA0001783397720000202
故有
Figure GDA0001783397720000203
【6】根据式(38)计算绝对平动速度矢量对关节角速度的偏速度，
Figure GDA0001783397720000204
上式可由式(27)得
Figure GDA0001783397720000211
将上述结论，以专利中对应的式(39)、(40)、(41)统一表述，称之为偏速度定理。
若给定运动链运动链iln，则有
Figure GDA0001783397720000212
Figure GDA0001783397720000213
Figure GDA0001783397720000214
当
Figure GDA0001783397720000215
时，由式(35)，(33)，(36)及(34)可得式(39)。
由式(37)及(38)得式(40)。因
Figure GDA0001783397720000216
和
Figure GDA0001783397720000217
与
Figure GDA0001783397720000218
和
Figure GDA0001783397720000219
无关，得式(41)。
式(35)至式(38)对运动学及动力学分析具有非常重要的作用。它们不仅物理意义清晰，还可以简化运动学及动力学方程的表达。
如图3所示，一方面，从几何角度看，式(39)中的偏速度即为对应的轴不变量，式(40)表示的是位置矢量对轴不变量的一阶距，即轴矢量
Figure GDA00017833977200002110
与矢量
Figure GDA00017833977200002111
的叉乘；另一方面，从力作用关系看，
Figure GDA00017833977200002112
是
Figure GDA00017833977200002113
在轴向
Figure GDA00017833977200002114
的投影。
由式(42)左序叉乘与转置的关系式
Figure GDA00017833977200002115
可知
Figure GDA0001783397720000221
式(43)表明：
Figure GDA0001783397720000222
完成了力
Figure GDA0001783397720000223
对轴
Figure GDA0001783397720000224
作用效应即力矩的计算。
式(43)中
Figure GDA0001783397720000225
与式(27)中
Figure GDA0001783397720000226
(即
Figure GDA0001783397720000227
)的链序不同；前者是作用力，后者是运动量，二者是对偶的，具有相反的序。
3.轴不变量对时间微分的不变性
由式(24)及式(26)可知
Figure GDA0001783397720000228
故有
Figure GDA0001783397720000229
式(45)表明：对轴不变量而言，其绝对导数就是其相对导数。因轴不变量是具有不变性的自然参考轴，故其绝对导数恒为零矢量。因此，轴不变量具有对时间微分的不变性。
由式(39)及式(45)得
Figure GDA00017833977200002210
由式(28)及式(45)得
Figure GDA00017833977200002211
由上式得
Figure GDA00017833977200002212
即
Figure GDA0001783397720000231
由式(47)可知：偏速度对时间t的导数仍是轴不变量的迭代式。轴不变量
Figure GDA0001783397720000232
是基el的坐标矢量，
Figure GDA0001783397720000233
本质上表示基el在参考系i上的投影。若式(45)不成立，则否认了参考基el作为参考的不变性即客观性。由
Figure GDA0001783397720000234
得
Figure GDA0001783397720000235
式(48)中左式表示：转动链ilc的DCM对该链全部关节角的偏速度之和；式(48)中右式表示：转动链ilc的轴不变量之和。因此，运动链的DCM对关节角的偏速度具有不变性。
对于MAS系统的树
Figure GDA0001783397720000236
由式(45)及式(48)得
Figure GDA0001783397720000237
表明：MAS系统的轴不变量对时间是不变的，即刚体系统的自然参考轴具有不变性。由式(48)可知：系统的关节变量与自然参考轴一一映射，体的关节变量数由其独立的运动维度确定，但不改变自然参考轴对时间微分的不变性。
4.树形运动链的变分计算步骤
将函数自变量的导数称为微商，以d表示。与微分相对应，将自变量函数的增量称为变分，以δ表示；但变分不考虑时间t的增量δt,即δt≡0。正是因为不考虑时间增量δt，故线位移及角位移的变分理解为同一时刻t可能的运动量变化，即虚位移。
【1】转动矢量的变分
Figure GDA0001783397720000238
上式可由式(33)得
Figure GDA0001783397720000239
【2】平动矢量的变分
Figure GDA0001783397720000241
上式可由式(37)及式(38)得
Figure GDA0001783397720000242
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。

Claims (10)

1.一种基于轴不变量的多轴机器人正运动学计算方法，其特征是，
多轴机器人装置包含杆件序列与关节序列，将树链中的关节序列转换成对应的轴序列及其父轴序列，所述轴序列的轴为平动轴或转动轴；
使用轴集合来对应描述多轴机器装置，以自然坐标系为基础，利用轴集合的轴对应的轴不变量来计算多轴机器装置的控制参数；
利用轴不变量的不变性建立基于轴不变量的迭代式运动学方程，并且所述迭代式运动学方程的符号对应到伪代码，反映所述多轴机器人运动链的拓扑关系与链序关系；
计算运动链的迭代式正运动学数值；
计算基于轴不变量的迭代式偏速度。
2.根据权利要求1所述的基于轴不变量的多轴机器人正运动学计算方法，其特征是，
惯性空间记为i，给定由i至杆件n的运动链iln，杆件l,n,j均∈A，n>l，s是杆件l上的任一点，A为轴序列；当转动矢量
Figure FDA0002358217580000011
有测量噪声时，运动链iln的迭代式正运动学数值计算步骤包括：
【1】链节
Figure FDA0002358217580000012
正运动学计算步骤；
运动副
Figure FDA0002358217580000013
对应的运动链
Figure FDA0002358217580000014
通过区间符表示为：
Figure FDA0002358217580000015
其中：
Figure FDA0002358217580000016
是l的前继即父，l是
Figure FDA0002358217580000017
的后继即子；
Figure FDA0002358217580000018
为链节，是运动链中的一个基本环节；
【2】运动链iln的位形计算步骤；
【3】运动链iln的速度及加速度计算步骤。
3.根据权利要求2所述的基于轴不变量的多轴机器人正运动学计算方法，其特征是，
链节
Figure FDA0002358217580000019
正运动学计算步骤为：
【1-1】已知转动矢量
Figure FDA00023582175800000110
根据欧拉四元数公式计算欧拉四元数
Figure FDA00023582175800000111
【1-2】由欧拉四元数计算旋转变换阵
Figure FDA00023582175800000112
【1-3】由下式计算链节速度：
Figure FDA00023582175800000113
式中，运动副
Figure FDA00023582175800000114
表示连接杆件
Figure FDA00023582175800000115
及杆件l的运动副；转动副R，棱柱副P；沿轴
Figure FDA00023582175800000116
的线位置
Figure FDA00023582175800000117
绕轴
Figure FDA00023582175800000118
的角位置
Figure FDA00023582175800000119
轴矢量
Figure FDA00023582175800000120
角速度
Figure FDA00023582175800000121
线速度
Figure FDA00023582175800000122
表达形式投影符|□表示矢量对参考基的投影矢量，参考基为惯性空间i；
Figure FDA00023582175800000123
为i到
Figure FDA00023582175800000124
的旋转变换矩阵，
Figure FDA00023582175800000125
为转动速度
Figure FDA00023582175800000126
对参考基i的投影矢量；
Figure FDA00023582175800000127
为平动速度
Figure FDA0002358217580000021
对参考基i的投影矢量；
【1-4】由下式计算链节加速度：
Figure FDA0002358217580000022
式中，转动加速度
Figure FDA0002358217580000023
平动加速度
Figure FDA0002358217580000024
Figure FDA0002358217580000025
表示角速度
Figure FDA0002358217580000026
的导数即角加速度，
Figure FDA0002358217580000027
表示线速度
Figure FDA0002358217580000028
的导数即线加速度。
4.根据权利要求2所述的基于轴不变量的多轴机器人正运动学计算方法，其特征是，
运动链iln的位形计算步骤为：
【2-1】由欧拉四元数的链关系，将四元数乘法运算用其共轭矩阵运算替代，计算欧拉四元数序列
Figure FDA0002358217580000029
Figure FDA00023582175800000210
为欧拉四元数；
【2-2】用欧拉四元数表示定轴转动；旋转变换阵的计算等价于链式四元数的矩阵计算，计算旋转变换阵序列{iQj|j∈A}，iQj为i到j的旋转变换矩阵；
【2-3】由下式计算位置矢量
Figure FDA00023582175800000211
Figure FDA00023582175800000212
式中，轴矢量
Figure FDA00023582175800000213
沿轴
Figure FDA00023582175800000214
的线位置
Figure FDA00023582175800000215
Figure FDA00023582175800000216
的含义是坐标矢量
Figure FDA00023582175800000217
对参考基i的投影矢量，
Figure FDA00023582175800000218
为在自然坐标系下由杆件
Figure FDA00023582175800000219
到杆件l的坐标矢量；nS表示杆件n中的点S，
Figure FDA00023582175800000220
为由i至nS的运动链；表达形式投影符|□表示矢量对参考基的投影矢量，参考基为惯性空间i；
Figure FDA00023582175800000221
为零位时的平动矢量
Figure FDA00023582175800000222
对参考基i的投影矢量，
Figure FDA00023582175800000223
为轴矢量
Figure FDA00023582175800000224
对参考基i的投影矢量。
5.根据权利要求2所述的基于轴不变量的多轴机器人正运动学计算方法，其特征是，
运动链iln的速度及加速度计算步骤为：
【3-1】计算绝对角速度：
Figure FDA00023582175800000225
式中，角速度
Figure FDA00023582175800000226
轴不变量
Figure FDA00023582175800000227
Figure FDA00023582175800000228
为由
Figure FDA00023582175800000229
至k转动矢量，k为属于运动链iln的杆件，
Figure FDA00023582175800000230
是k的前继即父，表达形式投影符|□表示矢量对参考基的投影矢量，参考基为惯性空间i，
Figure FDA00023582175800000231
为轴不变量
Figure FDA00023582175800000232
对参考基i的投影矢量，
Figure FDA00023582175800000233
为转动速度
Figure FDA00023582175800000234
对参考基i的投影矢量；
【3-2】计算绝对角加速度：
Figure FDA00023582175800000235
式中，角加速度
Figure FDA00023582175800000236
轴矢量
Figure FDA00023582175800000237
Figure FDA00023582175800000238
为转动加速度
Figure FDA00023582175800000239
对参考基i的投影矢量；
【3-3】计算绝对平动速度：
Figure FDA0002358217580000031
式中，线速度
Figure FDA0002358217580000032
轴不变量
Figure FDA0002358217580000033
nS表示杆件n中的点S，由n至nS的坐标矢量
Figure FDA0002358217580000034
Figure FDA0002358217580000035
为由参考基i至杆件n的角速度叉乘矩阵，
Figure FDA0002358217580000036
为矢量
Figure FDA0002358217580000037
对参考基i的投影矢量；
Figure FDA0002358217580000038
为
Figure FDA0002358217580000039
的导数；
【3-4】计算绝对平动加速度：
Figure FDA00023582175800000310
式中，线速度
Figure FDA00023582175800000311
轴不变量
Figure FDA00023582175800000312
表达形式投影符|□表示矢量对参考基的投影矢量，参考基为惯性空间i，
Figure FDA00023582175800000313
为轴不变量
Figure FDA00023582175800000314
对参考基i的投影矢量；
Figure FDA00023582175800000315
为零位时由原点
Figure FDA00023582175800000316
至原点Ol的平动矢量对参考基i的投影矢量，
Figure FDA00023582175800000317
为由参考基i至
Figure FDA00023582175800000318
的角速度叉乘矩阵；
Figure FDA00023582175800000319
为
Figure FDA00023582175800000320
的2次幂；
其中：
Figure FDA00023582175800000321
—转动加速度，其中
Figure FDA00023582175800000322
向心加速度；
Figure FDA00023582175800000323
—哥氏加速度，是平动与转动的耦合加速度。
6.根据权利要求2所述的基于轴不变量的多轴机器人正运动学计算方法，其特征是，
基于轴不变量的迭代式偏速度计算步骤包括：
定义使能函数：
Figure FDA00023582175800000324
式中，k为属于运动链iln的杆件；
由使能函数，
【1】计算绝对角速度对角速度的偏速度；
【2】计算绝对平动速度矢量对关节平动速度的偏速度；
【3】计算绝对转动矢量对关节角度的偏速度；
【4】计算绝对位置矢量对关节位移的偏速度；
【5】计算绝对位置矢量对关节角度的偏速度；
【6】计算绝对平动速度矢量对角速度的偏速度。
7.根据权利要求6所述的基于轴不变量的多轴机器人正运动学计算方法，其特征是，
绝对角速度对角速度的偏速度计算公式为：
Figure FDA00023582175800000325
式中，角速度
Figure FDA00023582175800000326
轴不变量
Figure FDA00023582175800000327
表达形式投影符|□表示矢量对参考基的投影矢量，参考基为惯性空间i，
Figure FDA00023582175800000328
为轴不变量
Figure FDA00023582175800000329
对参考基i的投影矢量；iφn′为绝对角速度。
8.根据权利要求6所述的基于轴不变量的多轴机器人正运动学计算方法，其特征是，
绝对平动速度矢量对关节平动速度的偏速度计算公式为：
Figure FDA00023582175800000330
式中，线速度
Figure FDA0002358217580000041
轴不变量
Figure FDA0002358217580000042
表达形式投影符|□表示矢量对参考基的投影矢量，参考基为惯性空间i，
Figure FDA0002358217580000043
为轴不变量
Figure FDA0002358217580000044
对参考基i的投影矢量，
Figure FDA0002358217580000045
为绝对平均速度矢量；
绝对平动速度矢量对角速度的偏速度计算公式为：
Figure FDA0002358217580000046
式中，角速度
Figure FDA0002358217580000047
Figure FDA0002358217580000048
是轴不变量
Figure FDA0002358217580000049
的叉乘矩阵，表达形式投影符|□表示矢量对参考基的投影矢量，参考基为惯性空间i，nS表示杆件n中的点S，由k至nS的位置矢量
Figure FDA00023582175800000410
9.根据权利要求6所述的基于轴不变量的多轴机器人正运动学计算方法，其特征是，
绝对转动矢量对关节角度的偏速度计算公式为：
Figure FDA00023582175800000411
式中，角位置
Figure FDA00023582175800000412
轴不变量
Figure FDA00023582175800000413
kln为杆件k至杆件n的运动链，表达形式投影符|□表示矢量对参考基的投影矢量，参考基为惯性空间i，
Figure FDA00023582175800000414
为轴不变量
Figure FDA00023582175800000415
对参考基i的投影矢量，
Figure FDA00023582175800000416
为转动矢量
Figure FDA00023582175800000417
对参考基i的投影矢量。
10.根据权利要求6所述的基于轴不变量的多轴机器人正运动学计算方法，其特征是，
绝对位置矢量对关节位移的偏速度计算公式为：
Figure FDA00023582175800000418
式中，线位置
Figure FDA00023582175800000419
轴不变量
Figure FDA00023582175800000420
表达形式投影符|□表示矢量对参考基的投影矢量，参考基为惯性空间i，
Figure FDA00023582175800000421
为轴矢量
Figure FDA00023582175800000422
对参考基i的投影矢量；nS表示杆件n中的点S，由i至nS的位置矢量
Figure FDA00023582175800000423
绝对位置矢量对关节角度的偏速度计算公式为：
Figure FDA00023582175800000424
式中，角位置
Figure FDA00023582175800000425
Figure FDA00023582175800000426
是轴不变量
Figure FDA00023582175800000427
的叉乘矩阵，由杆件k至nS的位置矢量
Figure FDA00023582175800000428
CN201810933676.4A 2018-08-16 2018-08-16 基于轴不变量的多轴机器人正运动学计算方法 Active CN108942943B (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN201810933676.4A CN108942943B (zh) 2018-08-16 2018-08-16 基于轴不变量的多轴机器人正运动学计算方法
PCT/CN2018/112658 WO2020034403A1 (zh) 2018-08-16 2018-10-30 基于轴不变量的多轴机器人正运动学计算方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810933676.4A CN108942943B (zh) 2018-08-16 2018-08-16 基于轴不变量的多轴机器人正运动学计算方法

Publications (2)

Publication Number Publication Date
CN108942943A CN108942943A (zh) 2018-12-07
CN108942943B true CN108942943B (zh) 2020-03-17

Family

ID=64470083

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810933676.4A Active CN108942943B (zh) 2018-08-16 2018-08-16 基于轴不变量的多轴机器人正运动学计算方法

Country Status (2)

Country Link
CN (1) CN108942943B (zh)
WO (1) WO2020034403A1 (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113246133B (zh) * 2021-05-28 2022-09-27 北京世冠金洋科技发展有限公司 机械臂多关节的旋转指令计算方法、旋转控制方法及系统

Family Cites Families (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE10305693B4 (de) * 2003-02-12 2006-02-16 Jürgen Michael Knapp Vorrichtung zum Positionieren und/oder Bewegen eines chirurgischen Instrumentes
EP2345512A1 (en) * 2010-01-14 2011-07-20 Syddansk Universitet Method of finding feasible joint trajectories for an n-dof robot with rotation invariant process (N>5)
JP2014014920A (ja) * 2012-06-15 2014-01-30 Kumamoto Univ 機構制御装置、プログラム及び機構制御方法
CN102982653B (zh) * 2012-11-07 2014-10-15 中南大学 基于加速度和高度信息的人体跌倒监测方法及装置
CN103035008B (zh) * 2012-12-15 2015-08-12 北京工业大学 一种多相机系统的加权标定方法
CN103802656B (zh) * 2014-03-05 2016-06-22 北京工业大学 一种四轮行星车移动系统
CN105269565B (zh) * 2015-10-30 2017-04-05 福建长江工业有限公司 一种六轴磨抛工业机器人离线编程及修正方法
CN105773620B (zh) * 2016-04-26 2017-09-12 南京工程学院 基于倍四元数的工业机器人自由曲线的轨迹规划控制方法
CN106625671B (zh) * 2016-12-27 2019-02-19 西北工业大学 一种空间机器人抓捕翻滚目标的最优轨迹规划方法
CN107791248B (zh) * 2017-09-28 2021-04-30 浙江理工大学 基于不满足Pieper准则的六自由度串联机器人的控制方法
CN107932504B (zh) * 2017-11-13 2020-11-13 浙江工业大学 基于PyQt的机械臂运行控制系统
CN108081256A (zh) * 2017-12-07 2018-05-29 南京航空航天大学 检测制动一体化机器人关节驱动器

Also Published As

Publication number Publication date
CN108942943A (zh) 2018-12-07
WO2020034403A1 (zh) 2020-02-20

Similar Documents

Publication Publication Date Title
CN108972558B (zh) 一种基于轴不变量的多轴机器人动力学建模方法
CN108942942A (zh) 一种基于轴不变量的多轴机器人逆运动学建模与解算方法
CN108983705A (zh) 一种基于轴不变量的多轴机器人系统正运动学建模与解算方法
Chirikjian et al. Pose changes from a different point of view
CN109079784A (zh) 一种基于轴不变量的多轴机器人系统建模与控制方法
Müller Approximation of finite rigid body motions from velocity fields
Singh et al. Kinematic modeling of robotic manipulators
CN108942943B (zh) 基于轴不变量的多轴机器人正运动学计算方法
Löw et al. Geometric algebra for optimal control with applications in manipulation tasks
Strandberg Robot path planning: an object-oriented approach
CN109015641B (zh) 基于轴不变量的通用6r机械臂逆解建模与解算方法
Pradeep et al. On the use of dual-matrix exponentials in robotic kinematics
CN109079850B (zh) 基于轴不变量多轴机器人d-h系及d-h参数确定方法
CN112084592A (zh) 折叠式桁架动力学分析系统、方法、装置和存储介质
CN109117451B (zh) 基于轴不变量的树链机器人动力学建模与解算方法
CN109033688B (zh) 基于轴不变量的通用7r机械臂逆解建模与解算方法
CN109291047B (zh) 基于轴不变量及dh参数1r/2r/3r逆解建模方法
CN108959828B (zh) 基于轴不变量的通用3r机械臂逆解建模与解算方法
CN108959829B (zh) 基于轴不变量的非理想关节机器人动力学建模方法
CN109086544B (zh) 基于轴不变量的闭链机器人动力学建模与解算方法
CN108803350B (zh) 基于轴不变量的动基座多轴机器人动力学建模与解算方法
CN108927825B (zh) 基于轴不变量的多轴机器人结构参数精测方法
Zhang Robot Basics: Representation, Rotation and Velocity
CN111967099B (zh) 多自由度机械臂矢量多项式系统最优求解方法
Qi Theory and Algorithms for Simulating Multibody Dynamic Systems Using Modern C++

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant