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

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

Info

Publication number
CN108803350B
CN108803350B CN201810933652.9A CN201810933652A CN108803350B CN 108803350 B CN108803350 B CN 108803350B CN 201810933652 A CN201810933652 A CN 201810933652A CN 108803350 B CN108803350 B CN 108803350B
Authority
CN
China
Prior art keywords
axis
formula
equation
kane
force
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
CN201810933652.9A
Other languages
English (en)
Other versions
CN108803350A (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 CN201810933652.9A priority Critical patent/CN108803350B/zh
Priority to PCT/CN2018/112616 priority patent/WO2020034401A1/zh
Publication of CN108803350A publication Critical patent/CN108803350A/zh
Application granted granted Critical
Publication of CN108803350B publication Critical patent/CN108803350B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
    • G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
    • G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
    • G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance

Landscapes

  • Engineering & Computer Science (AREA)
  • Health & Medical Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Computation (AREA)
  • Medical Informatics (AREA)
  • Software Systems (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Management, Administration, Business Operations System, And Electronic Commerce (AREA)
  • Manipulator (AREA)

Abstract

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

Description

基于轴不变量的动基座多轴机器人动力学建模与解算方法
技术领域
本发明涉及一种动基座多轴机器人动力学与解算方法，属于机器人技术领域。
背景技术
拉格朗日在研究月球天平动问题时提出了拉格朗日方法，是以广义坐标表达动力学方程的基本方法；同时，也是描述量子场论的基本方法。应用拉格朗日法建立动力学方程已是一个烦琐的过程，尽管拉格朗日方程依据系统能量的不变性推导系统的动力学方程，具有理论分析上的优势；但是在工程应用中，随着系统自由度的增加，方程推导的复杂性剧增，难以得到普遍应用。凯恩方程建立过程与拉格朗日方程相比，通过系统的偏速度、速度及加速度直接表达动力学方程。故凯恩动力学方法与拉格朗日方法相比，由于省去了系统能量的表达及对时间的求导过程，极大地降低了系统建模的难度。然而，对于高自由度的系统，凯恩动力学建模方法也是难以适用。
拉格朗日方程及凯恩方程极大地推动了多体动力学的研究，以空间算子代数为基础的动力学由于应用了迭代式的过程，计算速度及精度都有了一定程度的提高。这些动力学方法无论是运动学过程还是动力学过程都需要在体空间、体子空间、系统空间及系统子空间中进行复杂的变换，建模过程及模型表达非常复杂，难以满足高自由度系统建模与控制的需求，因此，需要建立动力学模型的简洁表达式；既要保证建模的准确性，又要保证建模的实时性。没有简洁的动力学表达式，就难以保证高自由度系统动力学工程实现的可靠性与准确性。同时，传统非结构化运动学及动力学符号通过注释约定符号内涵，无法被计算机理解，导致计算机不能自主地建立及分析运动学及动力学模型。
发明内容
本发明所要解决的技术问题是提供一种基于轴不变量的动基座多轴机器人动力学与解算方法。
为解决上述技术问题，本发明采用以下技术方案：
一种基于轴不变量的动基座多轴机器人动力学建模与解算方法，其特征是，
给定多轴刚体移动系统D＝{A,K,T,NT,F,B}，惯性系记为F[i]，
Figure BDA00017672476300000116
Figure BDA0001767247630000011
A为轴序列，F为杆件参考系序列，B为杆件体序列，K为运动副类型序列，NT为约束轴的序列即非树；
Figure BDA0001767247630000012
轴序列为iAc＝(i,c1,c2,c3,c4,c5,c]，轴类型序列为iKc＝(X,R,R,R,P,P,P]，该运动链为ilc＝(i,c1,c2,c3,c4,c5,c]；除了重力外，作用于轴u的合外力及力矩在
Figure BDA0001767247630000013
上的分量分别为
Figure BDA0001767247630000014
及
Figure BDA0001767247630000015
轴k的质量及质心转动惯量分别为mk及
Figure BDA0001767247630000016
轴k的重力加速度为
Figure BDA0001767247630000017
驱动轴u的双边驱动力及驱动力矩在
Figure BDA0001767247630000018
上的分量分别为
Figure BDA0001767247630000019
及
Figure BDA00017672476300000110
环境i对轴l的作用力及作用力矩分别为
Figure BDA00017672476300000111
及iτl；作用于轴c上的合力及合力矩分别为及
Figure BDA00017672476300000113
记
Figure BDA00017672476300000114
Figure BDA00017672476300000115
则有以下动基座刚体系统的Ju-Kane动力学方程
Figure BDA0001767247630000021
Figure BDA0001767247630000022
则有
Figure BDA0001767247630000023
其中：
Figure BDA0001767247630000024
表示c的开子树，
Figure BDA0001767247630000025
且有：
Figure BDA0001767247630000026
Figure BDA0001767247630000027
Figure BDA0001767247630000028
Figure BDA0001767247630000029
式中，
Figure BDA00017672476300000210
及
Figure BDA00017672476300000211
是3×3的分块矩阵，
Figure BDA00017672476300000212
及
Figure BDA00017672476300000213
是3D矢量；kI表示杆k质心I；轴k的质量及质心转动惯量分别记为mk及
Figure BDA00017672476300000214
Figure BDA00017672476300000215
为转动轴u的惯性矩阵；
Figure BDA00017672476300000216
为平动轴u的惯性矩阵；hR为转动轴u的非惯性矩阵；hP为平动轴u的非惯性矩阵；
Figure BDA00017672476300000217
为关节角速度。
由式(150)确定本体c的笛卡尔体系F[c]三个转动轴的序列，在建立动力学方程后，通过积分完成动力学仿真，直接得到所期望的姿态。
动基座刚体系统的Ju-Kane动力学方程根据闭链刚体系统的Ju-Kane动力学方程建立。
闭链刚体系统的Ju-Kane动力学方程为：
【1】轴u及轴u′的Ju-Kane动力学规范方程分别为
Figure BDA00017672476300000218
Figure BDA00017672476300000219
式中：
Figure BDA00017672476300000220
及
Figure BDA00017672476300000221
是3×3的分块矩阵，
Figure BDA00017672476300000222
及
Figure BDA00017672476300000223
是3D矢量；kI表示杆k质心I；
Figure BDA00017672476300000224
为转动轴u的惯性矩阵；
Figure BDA0001767247630000031
为平动轴u的惯性矩阵；hR为转动轴u的非惯性矩阵；hP为平动轴u的非惯性矩阵。
【2】非树约束副uku′的约束代数方程为
Figure BDA0001767247630000032
Figure BDA0001767247630000033
Figure BDA0001767247630000034
Figure BDA0001767247630000035
其中：
Figure BDA0001767247630000036
Figure BDA0001767247630000037
Figure BDA0001767247630000038
Figure BDA0001767247630000039
Figure BDA00017672476300000310
式中：
Figure BDA00017672476300000311
及
Figure BDA00017672476300000312
是3×3的分块矩阵，
Figure BDA00017672476300000313
及
Figure BDA00017672476300000314
是3D矢量；kI表示杆k质心I；轴k的质量及质心转动惯量分别记为mk及
Figure BDA00017672476300000315
Figure BDA00017672476300000316
为转动轴u的惯性矩阵；
Figure BDA00017672476300000317
为平动轴u的惯性矩阵；hR为转动轴u的非惯性矩阵；hP为平动轴u的非惯性矩阵；
Figure BDA00017672476300000318
为平动关节角速度；
Figure BDA00017672476300000319
为转动关节角速度。
闭链刚体系统的Ju-Kane动力学方程根据树链Ju-Kane规范型方程建立。
树链Ju-Kane规范型方程
Figure BDA00017672476300000320
式中：
Figure BDA00017672476300000321
及
Figure BDA00017672476300000322
是3×3的分块矩阵，
Figure BDA00017672476300000323
及
Figure BDA00017672476300000324
是3D矢量；
Figure BDA00017672476300000325
为轴u的合外力在
Figure BDA00017672476300000326
上的分量，
Figure BDA00017672476300000327
为轴u的合力矩在
Figure BDA00017672476300000328
上的分量；
并且，
Figure BDA00017672476300000329
Figure BDA00017672476300000330
Figure BDA0001767247630000041
Figure BDA0001767247630000042
Figure BDA0001767247630000043
Figure BDA0001767247630000044
式中，kI表示杆k质心I；轴k的质量及质心转动惯量分别记为mk及
Figure BDA0001767247630000045
Figure BDA0001767247630000046
为转动轴u的惯性矩阵；
Figure BDA0001767247630000047
为平动轴u的惯性矩阵；hR为转动轴u的非惯性矩阵；hP为平动轴u的非惯性矩阵；作用于轴u的合外力及力矩在
Figure BDA0001767247630000048
上的分量分别记为
Figure BDA0001767247630000049
及
Figure BDA00017672476300000410
Figure BDA00017672476300000411
作用于轴u的合外力及力矩在
Figure BDA00017672476300000412
上的分量分别记为
Figure BDA00017672476300000413
及
Figure BDA00017672476300000414
驱动轴u的双边驱动力及驱动力矩在
Figure BDA00017672476300000415
上的分量分别记为
Figure BDA00017672476300000416
及
Figure BDA00017672476300000417
环境i对轴l的作用力及作用力矩分别为
Figure BDA00017672476300000418
及iτl；llk为取由轴l至轴k的运动链，uL表示获得由轴u及其子树构成的闭子树。
本发明所达到的有益效果：
本发明给出了动基座刚体系统的Ju-Kane动力学方程建模及解算的方法。并通过2个实施例的建模及解算过程表明：应用Ju-Kane动力学原理建立多轴系统动力学显式模型简洁、方便、高效，适宜高自由度的多轴系统动力学显式建模与数值建模。同时表明，通过显式动力学建模与计算，可以通过多轴系统的运动检测及视觉检测，计算系统与环境的接触作用力，为多轴系统力位控制及环境感知提供了新的技术途径。
附图说明
图1自然坐标系与轴链；
图2固定轴不变量；
图3三轮移动系统示意图；
图4CE3月面巡视器移动系统；
图5副臂移动系统树链结构简图。
具体实施方式
下面对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。
定义1自然坐标轴：称与运动轴或测量轴共轴的，具有固定原点的单位参考轴为自然坐标轴，亦称为自然参考轴。
定义2自然坐标系：如图1所示，若多轴系统D处于零位，所有笛卡尔体坐标系方向一致，且体坐标系原点位于运动轴的轴线上，则该坐标系统为自然坐标系统，简称自然坐标系。
自然坐标系优点在于：(1)坐标系统易确定；(2)零位时的关节变量为零；(3)零位时的系统姿态一致；(4)不易引入测量累积误差。
由定义2可知，在系统处于零位时，所有杆件的自然坐标系与底座或世界系的方向一致。系统处于零位即
Figure BDA0001767247630000051
时，自然坐标系
Figure BDA0001767247630000052
绕轴矢量
Figure BDA0001767247630000053
转动角度
Figure BDA0001767247630000054
将
Figure BDA0001767247630000055
转至F[l]；
Figure BDA0001767247630000056
在
Figure BDA0001767247630000057
下的坐标矢量与
Figure BDA0001767247630000058
在F[l]下的坐标矢量
Figure BDA0001767247630000059
恒等，即有
Figure BDA00017672476300000510
由上式知，
Figure BDA00017672476300000511
或
Figure BDA00017672476300000512
不依赖于相邻的坐标系
Figure BDA00017672476300000513
及F[l]；故称
Figure BDA00017672476300000514
或
Figure BDA00017672476300000515
为轴不变量。在不强调不变性时，可以称之为坐标轴矢量(简称轴矢量)。
Figure BDA00017672476300000516
或
Figure BDA00017672476300000517
表征的是体
Figure BDA00017672476300000518
与体l共有的参考单位坐标矢量，与参考点
Figure BDA00017672476300000519
及Ol无关。体
Figure BDA00017672476300000520
与体l即为杆件或轴。
轴不变量与坐标轴具有本质区别：
(1)坐标轴是具有零位及单位刻度的参考方向，可以描述沿该方向平动的位置，但不能完整描述绕该方向的转动角度，因为坐标轴自身不具有径向参考方向，即不存在表征转动的零位。在实际应用时，需要补充该轴的径向参考。例如：在笛卡尔系F[l]中，绕lx转动，需以ly或lz为参考零位。坐标轴自身是1D的，3个正交的1D参考轴构成3D的笛卡尔标架。
(2)轴不变量是3D的空间单位参考轴，其自身就是一个标架。其自身具有径向参考轴，即参考零位。空间坐标轴及其自身的径向参考轴可以确定笛卡尔标架。空间坐标轴可以反映运动轴及测量轴的三个基本参考属性。
已有文献将无链指标的轴矢量记为
Figure BDA0001767247630000061
并称之为欧拉轴(Euler Axis)，相应的关节角称为欧拉角(Euler Angle)。本申请之所以不再沿用欧拉轴，而称之为轴不变量，是因为轴不变量具有以下属性：
【1】给定旋转变换阵
Figure BDA0001767247630000062
因其是实矩阵，其模是单位的，故其有一个实特征值λ1及两个互为共轭的复特征值λ2＝eiφ及λ3＝e-iφ；其中：i为纯虚数。因此，|λ1|·||λ2||·||λ3||＝1，得λ1＝1。轴矢量
Figure BDA0001767247630000063
是实特征值λ1＝1对应的特征矢量，是不变量；
【2】是3D参考轴，不仅具有轴向参考方向，而且具有径向参考零位，将在3.3.1节予以阐述。
【3】在自然坐标系下：
Figure BDA0001767247630000064
即轴不变量
Figure BDA0001767247630000065
是非常特殊的矢量，它对时间的导数也具有不变性，且有非常优良的数学操作性能；
对轴不变量而言，其绝对导数就是其相对导数。因轴不变量是具有不变性的自然参考轴，故其绝对导数恒为零矢量。因此，轴不变量具有对时间微分的不变性。有：
Figure BDA0001767247630000066
【4】在自然坐标系统中，通过轴矢量
Figure BDA0001767247630000067
及关节变量
Figure BDA0001767247630000068
可以直接描述旋转坐标阵
Figure BDA0001767247630000069
没有必要为除根之外的杆件建立各自的体系。同时，以唯一需要定义的根坐标系为参考，可以提高系统结构参数的测量精度；
【5】应用轴矢量
Figure BDA00017672476300000610
的优良操作，将建立包含拓扑结构、坐标系、极性、结构参量及力学参量的完全参数化的统一的多轴系统运动学及动力学模型。
因基矢量el是与F[l]固结的任一矢量，基矢量
Figure BDA00017672476300000611
是与
Figure BDA00017672476300000612
固结的任一矢量，又
Figure BDA00017672476300000613
是F[l]及
Figure BDA00017672476300000614
共有的单位矢量，故
Figure BDA00017672476300000615
是F[l]及
Figure BDA00017672476300000616
共有的基矢量。因此，轴不变量
Figure BDA00017672476300000617
是F[l]及
Figure BDA00017672476300000618
共有的参考基。轴不变量是参数化的自然坐标基，是多轴系统的基元。固定轴不变量的平动与转动与其固结的坐标系的平动与转动等价。
在系统处于零位时，以自然坐标系为参考，测量得到坐标轴矢量
Figure BDA00017672476300000619
在运动副
Figure BDA00017672476300000620
运动时，轴矢量
Figure BDA00017672476300000621
是不变量；轴矢量
Figure BDA00017672476300000622
及关节变量
Figure BDA00017672476300000623
唯一确定运动副
Figure BDA00017672476300000624
的转动关系。
因此，应用自然坐标系统，当系统处于零位时，只需确定一个公共的参考系，而不必为系统中每一杆件确定各自的体坐标系，因为它们由轴不变量及自然坐标唯一确定。当进行系统分析时，除底座系外，与杆件固结的其它自然坐标系只发生在概念上，而与实际的测量无关。自然坐标系统对于多轴系统(MAS)理论分析及工程作用在于：
(1)系统的结构参数测量需要以统一的参考系测量；否则，不仅工程测量过程烦琐，而且引入不同的体系会引入更大的测量误差。
(2)应用自然坐标系统，除根杆件外，其它杆件的自然坐标系统由结构参量及关节变量自然确定，有助于MAS系统的运动学与动力学分析。
(3)在工程上，可以应用激光跟踪仪等光学测量设备，实现对固定轴不变量的精确测量。
(4)由于运动副R及P、螺旋副H、接触副O是圆柱副C的特例，可以应用圆柱副简化MAS运动学及动力学分析。
定义3不变量：称不依赖于一组坐标系进行度量的量为不变量。
定义4转动坐标矢量：绕坐标轴矢量
Figure BDA0001767247630000071
转动到角位置
Figure BDA0001767247630000072
的坐标矢量
Figure BDA0001767247630000073
为
Figure BDA0001767247630000074
定义5平动坐标矢量：沿坐标轴矢量
Figure BDA0001767247630000075
平动到线位置
Figure BDA0001767247630000076
的坐标矢量
Figure BDA0001767247630000077
为
Figure BDA0001767247630000078
定义6自然坐标：以自然坐标轴矢量为参考方向，相对系统零位的角位置或线位置，记为ql，称为自然坐标；称与自然坐标一一映射的量为关节变量；其中：
Figure BDA0001767247630000079
定义7机械零位：对于运动副
Figure BDA00017672476300000710
在初始时刻t0时，关节绝对编码器的零位
Figure BDA00017672476300000711
不一定为零，该零位称为机械零位；
故关节
Figure BDA00017672476300000712
的控制量
Figure BDA00017672476300000713
为
Figure BDA00017672476300000714
定义8自然运动矢量：将由自然坐标轴矢量
Figure BDA00017672476300000715
及自然坐标ql确定的矢量
Figure BDA00017672476300000716
称为自然运动矢量。其中：
Figure BDA00017672476300000717
自然运动矢量实现了轴平动与转动的统一表达。将由自然坐标轴矢量及关节确定的矢量，例如
Figure BDA00017672476300000718
称为自由运动矢量，亦称为自由螺旋。显然，轴矢量
Figure BDA00017672476300000719
是特定的自由螺旋。
定义9关节空间：以关节自然坐标ql表示的空间称为关节空间。
定义10位形空间：称表达位置及姿态(简称位姿)的笛卡尔空间为位形空间，是双矢量空间或6D空间。
定义11自然关节空间：以自然坐标系为参考，通过关节变量
Figure BDA00017672476300000720
表示，在系统零位时必有
Figure BDA00017672476300000721
的关节空间，称为自然关节空间。
如图2所示，给定链节
Figure BDA0001767247630000081
原点Ol受位置矢量
Figure BDA0001767247630000082
约束的轴矢量
Figure BDA0001767247630000083
为固定轴矢量，记为
Figure BDA0001767247630000084
其中：
Figure BDA0001767247630000085
轴矢量
Figure BDA0001767247630000086
是关节自然坐标的自然参考轴。因
Figure BDA0001767247630000087
是轴不变量，故称
Figure BDA0001767247630000088
为固定轴不变量，它表征了运动副
Figure BDA0001767247630000089
的结构关系，即确定了自然坐标轴。固定轴不变量
Figure BDA00017672476300000810
是链节
Figure BDA00017672476300000811
结构参数的自然描述。
定义12自然坐标轴空间：以固定轴不变量作为自然参考轴，以对应的自然坐标表示的空间称为自然坐标轴空间，简称自然轴空间。它是具有1个自由度的3D空间。
如图2所示，
Figure BDA00017672476300000812
及
Figure BDA00017672476300000813
不因杆件Ωl的运动而改变，是不变的结构参考量。
Figure BDA00017672476300000814
确定了轴l相对于轴
Figure BDA00017672476300000815
的五个结构参数；与关节变量ql一起，完整地表达了杆件Ωl的6D位形。给定
Figure BDA00017672476300000816
时，杆件固结的自然坐标系可由结构参数
Figure BDA00017672476300000817
及关节变量
Figure BDA00017672476300000818
唯一确定。称轴不变量
Figure BDA00017672476300000819
固定轴不变量
Figure BDA00017672476300000820
关节变量
Figure BDA00017672476300000821
及
Figure BDA00017672476300000822
为自然不变量。显然，由固定轴不变量
Figure BDA00017672476300000823
及关节变量
Figure BDA00017672476300000824
构成的关节自然不变量
Figure BDA00017672476300000825
与由坐标系
Figure BDA00017672476300000826
至F[l]确定的空间位形
Figure BDA00017672476300000827
具有一一映射关系，即
Figure BDA00017672476300000828
给定多轴系统D＝{T,A,B,K,F,NT}，在系统零位时，只要建立底座系或惯性系，以及各轴上的参考点Ol，其它杆件坐标系也自然确定。本质上，只需要确定底座系或惯性系。
给定一个由运动副连接的具有闭链的结构简图，可以选定回路中任一个运动副，将组成该运动副的定子与动子分割开来；从而，获得一个无回路的树型结构，称之为Span树。T表示带方向的span树，以描述树链运动的拓扑关系。
I为结构参数；A为轴序列，F为杆件参考系序列，B为杆件体序列，K为运动副类型序列，NT为约束轴的序列即非树。
Figure BDA00017672476300000829
为取轴序列
Figure BDA00017672476300000832
的成员。转动副R，棱柱副P，螺旋副H，接触副O是圆柱副C的特例。
描述运动链的基本拓扑符号及操作是构成运动链拓扑符号系统的基础，定义如下：
【1】运动链由偏序集合(]标识。
【2】A[l]为取轴序列A的成员；因轴名l具有唯一的编号对应于A[l]的序号，故A[l]计算复杂度为O(1)。
【3】
Figure BDA00017672476300000830
为取轴l的父轴；轴
Figure BDA00017672476300000831
的计算复杂度为O(1)。计算复杂度O()表示计算过程的操作次数，通常指浮点乘与加的次数。以浮点乘与加的次数表达计算复杂度非常烦琐，故常采用算法循环过程中的主要操作次数；比如：关节位姿、速度、加速度等操作的次数。
【4】
Figure BDA0001767247630000091
为取轴序列
Figure BDA0001767247630000092
的成员；
Figure BDA0001767247630000093
计算复杂度为O(1)。
【5】llk为取由轴l至轴k的运动链，输出表示为
Figure BDA0001767247630000094
且
Figure BDA0001767247630000095
基数记为|llk|。llk执行过程：执行
Figure BDA0001767247630000096
若
Figure BDA0001767247630000097
则执行
Figure BDA0001767247630000098
否则，结束。llk计算复杂度为O(|llk|)。
【6】ll为取轴l的子。该操作表示在
Figure BDA0001767247630000099
中找到成员l的地址k；从而，获得轴l的子A[k]。因
Figure BDA00017672476300000910
不具有偏序结构，故ll的计算复杂度为
Figure BDA00017672476300000911
【7】lL表示获得由轴l及其子树构成的闭子树，
Figure BDA00017672476300000912
为不含l的子树；递归执行ll，计算复杂度为
Figure BDA00017672476300000913
【8】支路、子树及非树弧的增加与删除操作也是必要的组成部分；从而，通过动态Span树及动态图描述可变拓扑结构。在支路llk中，若
Figure BDA00017672476300000914
则记
Figure BDA00017672476300000915
Figure BDA00017672476300000916
即
Figure BDA00017672476300000917
表示在支路中取成员m的子。
定义以下表达式或表达形式：
轴与杆件具有一一对应性；轴间的属性量
Figure BDA00017672476300000918
及杆件间的属性量
Figure BDA00017672476300000919
具有偏序性。
约定：
Figure BDA00017672476300000933
表示属性占位；若属性p或P是关于位置的，则
Figure BDA00017672476300000920
应理解为坐标系
Figure BDA00017672476300000921
的原点至F[l]的原点；若属性p或P是关于方向的，则
Figure BDA00017672476300000922
应理解为坐标系
Figure BDA00017672476300000923
至F[l]。
Figure BDA00017672476300000924
及
Figure BDA00017672476300000925
应分别理解为关于时间t的函数
Figure BDA00017672476300000926
及
Figure BDA00017672476300000927
且
Figure BDA00017672476300000928
及
Figure BDA00017672476300000929
是t0时刻的常数或常数阵列。但是正体的
Figure BDA00017672476300000930
及
Figure BDA00017672476300000931
应视为常数或常数阵列。
本申请中约定：在运动链符号演算系统中，具有偏序的属性变量或常量，在名称上包含表示偏序的指标；要么包含左上角及右下角指标，要么包含右上角及右下角指标；它们的方向总是由左上角指标至右下角指标，或由右上角指标至右下角指标，本申请中为叙述简便，有时省略方向的描述，即使省略，本领域技术人员通过符号表达式也可以知道，本申请中采用的各参数，对于某种属性符，它们的方向总是由偏序指标的左上角指标至右下角指标，或由右上角指标至右下角指标。例如：
Figure BDA00017672476300000932
可简述为(表示由k至l)平动矢量；rl k表示(由k至l的)线位置；krl表示(由k至l的)平动矢量；其中：r表示“平动”属性符，其余属性符对应为：属性符φ表示“转动”；属性符Q表示“旋转变换矩阵”；属性符l表示“运动链”；属性符u表示“单位矢量”；属性符w表示“角速度”；角标为i表示惯性坐标系或大地坐标系；其他角标可以为其他字母，也可以为数字。
本申请的符号规范与约定是根据运动链的偏序性、链节是运动链的基本单位这两个原则确定的，反映了运动链的本质特征。链指标表示的是连接关系，右上指标表征参考系。采用这种符号表达简洁、准确，便于交流与书面表达。同时，它们是结构化的符号系统，包含了组成各属性量的要素及关系，便于计算机处理，为计算机自动建模奠定基础。指标的含义需要通过属性符的背景即上下文进行理解；比如：若属性符是平动类型的，则左上角指标表示坐标系的原点及方向；若属性符是转动类型的，则左上角指标表示坐标系的方向。
(1)lS－杆件l中的点S；而S表示空间中的一点S。
(2)
Figure BDA0001767247630000101
－杆件k的原点Ok至杆件l的原点Ol的平动矢量；
Figure BDA0001767247630000102
在自然坐标系F[k]下的坐标矢量，即由k至l的坐标矢量；
(3)
Figure BDA0001767247630000103
－原点Ok至点lS的平动矢量；
Figure BDA0001767247630000104
在F[k]下的坐标矢量；
(4)
Figure BDA0001767247630000105
－原点Ok至点S的平动矢量；
Figure BDA0001767247630000106
在F[k]下的坐标矢量；
(5)
Figure BDA0001767247630000107
－连接杆件
Figure BDA0001767247630000108
及杆件l的运动副；
Figure BDA0001767247630000109
－运动副
Figure BDA00017672476300001010
的轴矢量；
Figure BDA00017672476300001011
及
Figure BDA00017672476300001012
分别在
Figure BDA00017672476300001013
及F[l]下的坐标矢量；
Figure BDA00017672476300001014
是轴不变量，为一结构常数；
Figure BDA00017672476300001015
为转动矢量，转动矢量/角矢量
Figure BDA00017672476300001016
是自由矢量，即该矢量可自由平移；
(6)
Figure BDA00017672476300001017
－沿轴
Figure BDA00017672476300001018
的线位置(平动位置)，
Figure BDA00017672476300001019
－绕轴
Figure BDA00017672476300001020
的角位置，即关节角、关节变量，为标量；
(7)左下角指标为0时，表示机械零位；如：
Figure BDA00017672476300001021
－平动轴
Figure BDA00017672476300001022
的机械零位，
Figure BDA00017672476300001023
－转动轴
Figure BDA00017672476300001024
的机械零位；
(8)0－三维零矩阵；1－三维单位矩阵；
(9)约定：“\”表示续行符；
Figure BDA00017672476300001030
表示属性占位；则
幂符
Figure BDA00017672476300001025
表示
Figure BDA00017672476300001031
的x次幂；右上角角标∧或
Figure BDA00017672476300001032
表示分隔符；如：
Figure BDA00017672476300001026
或
Figure BDA00017672476300001027
为
Figure BDA00017672476300001028
的x次幂。
Figure BDA00017672476300001033
表示
Figure BDA00017672476300001034
的转置，表示对集合转置，不对成员执行转置；如：
Figure BDA00017672476300001029
Figure BDA00017672476300001035
为投影符，表示矢量或二阶张量对参考基的投影矢量或投影序列，即坐标矢量或坐标阵列，投影即是点积运算“·”；如：位置矢量
Figure BDA0001767247630000111
在坐标系F[k]中的投影矢量记为
Figure BDA0001767247630000112
Figure BDA0001767247630000113
为叉乘符；如：
Figure BDA0001767247630000114
是轴不变量
Figure BDA0001767247630000115
的叉乘矩阵；给定任一矢量
Figure BDA0001767247630000116
的叉乘矩阵为
Figure BDA0001767247630000117
叉乘矩阵是二阶张量。
叉乘符运算的优先级高于投影符
Figure BDA00017672476300001130
的优先级。投影符
Figure BDA00017672476300001131
的优先级高于成员访问符
Figure BDA00017672476300001132
或
Figure BDA00017672476300001133
成员访问符
Figure BDA00017672476300001134
优先级高于幂符
Figure BDA0001767247630000118
(10)单位矢量在大地坐标系的投影矢量
Figure BDA0001767247630000119
单位零位矢量
Figure BDA00017672476300001110
(11)
Figure BDA00017672476300001111
－零位时由原点
Figure BDA00017672476300001112
至原点Ol的平动矢量，且记
Figure BDA00017672476300001113
表示位置结构参数。
(12)iQl，相对绝对空间的旋转变换阵；
(13)以自然坐标轴矢量为参考方向，相对系统零位的角位置或线位置，记为ql，称为自然坐标；关节变量
Figure BDA00017672476300001114
自然关节坐标为φ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 BDA00017672476300001115
若n表示笛卡尔直角系，则称
Figure BDA00017672476300001116
为笛卡尔轴链；若n表示自然参考轴，则称
Figure BDA00017672476300001117
为自然轴链。
(16)Rodrigues四元数表达形式：
Figure BDA00017672476300001118
欧拉四元数表达形式：
Figure BDA00017672476300001119
不变量的四元数(也称为轴四元数)表达形式
Figure BDA00017672476300001120
一、1.建立多轴系统的拉格朗日方程
应用链符号系统建立关节空间的拉格朗日方程，考虑质点动力学系统D＝{A,K,T,NT,F,B}，首先根据牛顿力学推导自由质点
Figure BDA00017672476300001121
的拉格朗日方程；然后，推广至受约束的质点系统。
保守力
Figure BDA00017672476300001122
相对质点惯性力
Figure BDA00017672476300001123
具有相同的链序，即
Figure BDA00017672476300001124
具有正序，质点
Figure BDA00017672476300001125
的合力为零。质点
Figure BDA00017672476300001126
的能量记为
Figure BDA00017672476300001127
根据广义坐标序列
Figure BDA00017672476300001128
与笛卡尔空间位置矢量序列
Figure BDA00017672476300001129
关系
Figure BDA0001767247630000121
得
Figure BDA0001767247630000122
式(2)应用系统的能量及广义坐标建立系统的方程。关节变量
Figure BDA0001767247630000123
与坐标矢量irl的关系如式(1)所示，称式(1)为关节空间与笛卡尔空间的点变换。
保守力与惯性力具有相反的链序。拉格朗日系统内的约束既可以是质点间的固结约束，又可以是质点系统间的运动约束；刚体自身是质点系统
Figure BDA0001767247630000124
质点能量具有可加性；刚体动能量由质心平动动能及转动动能组成。下面，就以简单运动副R/P分别建立拉格朗日方程，为后续进一步推出新的动力学理论奠定基础。
给定刚体多轴系统D＝{A,K,T,NT,F,B}，惯性空间记为i，
Figure BDA0001767247630000125
轴l的能量记为εl i，其中平动动能为vεl i，转动动能为wεl i，引力势能为gεl i；轴l受除引力外的外部合力及合力矩分别为Dfl及
Figure BDA0001767247630000126
轴l的质量及质心转动惯量分别为ml及
Figure BDA0001767247630000127
轴u的单位轴不变量为
Figure BDA0001767247630000128
环境i作用于lI的惯性加速度记为
Figure BDA0001767247630000129
重力加速度
Figure BDA00017672476300001210
链序由i至lI；
Figure BDA00017672476300001211
链序由lI至i；且有
Figure BDA00017672476300001212
【1】系统能量
动力学系统D能量
Figure BDA00017672476300001213
表达为
Figure BDA00017672476300001214
其中：
Figure BDA00017672476300001215
【2】多轴系统拉格朗日方程
由式(2)得多轴系统拉格朗日方程，
Figure BDA0001767247630000131
式(6)为轴u的控制方程，即在轴不变量
Figure BDA0001767247630000132
上的力平衡方程；
Figure BDA0001767247630000133
是合力i|Dfu在
Figure BDA0001767247630000134
上的分量，
Figure BDA0001767247630000135
是合力矩i|Dτu在
Figure BDA0001767247630000136
上的分量。
2.建立Ju-Kane动力学预备方程：
基于多轴系统拉格朗日方程(6)推导居―凯恩(Ju-Kane)动力学预备定理。先进行拉格朗日方程与凯恩方程的等价性证明；然后，计算能量对关节速度及坐标的偏速度，再对时间求导，最后给出Ju-Kane动力学预备定理。
【1】拉格朗日方程与凯恩方程的等价性证明
Figure BDA0001767247630000137
证明：考虑刚体k平动动能对
Figure BDA0001767247630000138
的偏速度对时间的导数得
Figure BDA0001767247630000139
考虑刚体k转动动能对
Figure BDA00017672476300001310
的偏速度对时间的导数得
Figure BDA00017672476300001311
证毕。
因gεk i与
Figure BDA0001767247630000141
不相关，由式(7)及多轴系统拉格朗日方程(6)得
Figure BDA0001767247630000142
动力学系统D的平动动能及转动动能分别表示为
Figure BDA0001767247630000143
考虑式(4)及式(5)，即有
Figure BDA0001767247630000144
式(7)及式(8)是居―凯恩动力学预备定理证明的依据，即居―凯恩动力学预备定理本质上与拉格朗日法是等价的。同时，式(8)右侧包含了多轴系统凯恩方程；表明拉格朗日法与凯恩法的惯性力计算是一致的，即拉格朗日法与凯恩法也是等价的。式(8)表明：在拉格朗日方程(4)中存在
Figure BDA0001767247630000145
重复计算的问题。
【2】能量对关节速度及坐标的偏速度
【2-1】若
Figure BDA0001767247630000146
并考虑
Figure BDA0001767247630000147
Figure BDA0001767247630000148
及
Figure BDA0001767247630000149
仅与闭子树uL相关，由式(4)及式(5)，得
Figure BDA00017672476300001410
Figure BDA00017672476300001411
Figure BDA00017672476300001412
【2-2】若
Figure BDA0001767247630000151
并考虑
Figure BDA0001767247630000152
Figure BDA0001767247630000153
及
Figure BDA0001767247630000154
仅与闭子树uL相关，由式(4)及式(5)，得
Figure BDA0001767247630000155
Figure BDA0001767247630000156
Figure BDA0001767247630000157
至此，已完成能量对关节速度及坐标的偏速度计算。
【3】求对时间的导数
【3-1】若
Figure BDA0001767247630000158
由式(7)、式(9)及式(10)得
Figure BDA0001767247630000159
【3-2】若
Figure BDA00017672476300001510
由式(7)、式(12)及式(13)得
Figure BDA00017672476300001511
至此，已完成对时间t的求导。
【4】Ju-Kane动力学预备定理
将式(11)、式(14)、式(15)及式(16)代入式(8)，
给定多轴刚体系统D＝{A,K,T,NT,F,B}，惯性系记为F[i]，
Figure BDA00017672476300001512
除了重力外，作用于轴u的合外力及力矩分别记为i|Dfu及i|Dτu；轴k的质量及质心转动惯量分别记为mk及
Figure BDA00017672476300001513
轴k的重力加速度为
Figure BDA00017672476300001514
则轴u的Ju-Kane动力学预备方程为
Figure BDA0001767247630000161
式(17)具有了树链拓扑结构。kI表示杆k质心I。因闭子树uL中的广义力具有可加性；因此闭子树的节点有唯一一条至根的运动链，因此运动链iln可以被运动链uL替换。
下面，针对Ju-Kane动力学预备方程，解决式(17)右侧Dfk及Dτk的计算问题，从而建立树链刚体系统Ju-Kane动力学方程。
3.建立树链刚体系统Ju-Kane动力学模型
给定轴链
Figure BDA0001767247630000162
k∈iln，有以下偏速度计算公式：
Figure BDA0001767247630000163
Figure BDA0001767247630000164
Figure BDA0001767247630000165
对给定轴链
Figure BDA0001767247630000166
|ill|≥2，有以下加速度迭代式：
Figure BDA0001767247630000167
左序叉乘与转置的关系为：
Figure BDA0001767247630000168
根据运动学迭代式，有：
Figure BDA0001767247630000171
Figure BDA0001767247630000172
3.1外力反向迭代
给定由环境i中施力点iS至轴l上点lS的双边外力
Figure BDA0001767247630000173
及外力矩itl，它们的瞬时轴功率pex表示为
Figure BDA0001767247630000174
其中：
Figure BDA0001767247630000175
及iτl不受
Figure BDA0001767247630000176
及
Figure BDA0001767247630000177
控制，即
Figure BDA0001767247630000178
及iτl不依赖于
Figure BDA0001767247630000179
及
Figure BDA00017672476300001710
【1】若k∈ill，则有
Figure BDA00017672476300001711
由式(19)及式(18)得
Figure BDA00017672476300001712
即
Figure BDA00017672476300001713
式(26)中
Figure BDA00017672476300001714
与式(21)中
Figure BDA00017672476300001715
的链序不同；前者是作用力，后者是运动量，二者是对偶的，具有相反的序。
【2】若k∈ill，则有
Figure BDA00017672476300001716
由式(22)及式(25)得
Figure BDA00017672476300001717
即有
Figure BDA00017672476300001718
式(26)及式(27)表明环境作用于轴k的合外力或力矩等价于闭子树kL对轴k的合外力或力矩，将式(26)及式(27)合写为
Figure BDA0001767247630000181
至此，解决了外力反向迭代的计算问题。在式(28)中，闭子树对轴k的广义力具有可加性；力的作用具有双重效应，且是反向迭代的。所谓反向迭代是指：
Figure BDA0001767247630000182
是需要通过链节位置矢量迭代的；
Figure BDA0001767247630000183
的序与前向运动学
Figure BDA0001767247630000184
计算的序相反。
3.2共轴驱动力反向迭代
若轴l是驱动轴，轴l的驱动力及驱动力矩分别为
Figure BDA0001767247630000185
及
Figure BDA0001767247630000186
则驱动力
Figure BDA0001767247630000187
及驱动力矩
Figure BDA0001767247630000188
产生的功率pac表示为
Figure BDA0001767247630000189
【1】由式(18)、式(19)及式(29)得
Figure BDA00017672476300001810
即
Figure BDA00017672476300001811
若轴u与轴
Figure BDA0001767247630000191
共轴，则有
Figure BDA0001767247630000192
记
Figure BDA0001767247630000193
Figure BDA0001767247630000194
Figure BDA0001767247630000195
因
Figure BDA0001767247630000196
与
Figure BDA0001767247630000197
无关，由式(30)得
Figure BDA0001767247630000198
因
Figure BDA0001767247630000199
与
Figure BDA00017672476300001910
共轴，故有
Figure BDA00017672476300001911
【2】由式(19)、式(18)及式(29)得
Figure BDA00017672476300001912
即
Figure BDA00017672476300001913
若轴u与
Figure BDA00017672476300001914
共轴，则有
Figure BDA00017672476300001915
记
Figure BDA00017672476300001916
Figure BDA00017672476300001917
由式(32)得
Figure BDA00017672476300001918
至此，完成了共轴驱动力反向迭代计算问题。
3.3树链刚体系统Ju-Kane动力学显式模型的建立：
下面，先陈述树链刚体系统Ju-Kane动力学方程，简称Ju-Kane方程；然后，给出建立步骤。
给定多轴刚体系统D＝{A,K,T,NT,F,B}，惯性系记为F[i]，
Figure BDA0001767247630000201
除了重力外，作用于轴u的合外力及力矩在
Figure BDA0001767247630000202
上的分量分别记为
Figure BDA0001767247630000203
及
Figure BDA0001767247630000204
轴k的质量及质心转动惯量分别记为mk及
Figure BDA0001767247630000205
轴k的重力加速度为
Figure BDA0001767247630000206
驱动轴u的双边驱动力及驱动力矩在
Figure BDA0001767247630000207
上的分量分别记为
Figure BDA0001767247630000208
及
Figure BDA0001767247630000209
环境i对轴l的力及力矩分别为
Figure BDA00017672476300002010
及iτl；则轴u树链Ju-Kane动力学方程为
Figure BDA00017672476300002011
其中：[·]表示取行或列；
Figure BDA00017672476300002012
及
Figure BDA00017672476300002013
是3×3的分块矩阵，
Figure BDA00017672476300002014
及
Figure BDA00017672476300002015
是3D矢量，q为关节空间。且有，
Figure BDA00017672476300002016
Figure BDA00017672476300002017
Figure BDA00017672476300002018
Figure BDA00017672476300002019
Figure BDA00017672476300002020
Figure BDA00017672476300002021
其中，记
Figure BDA00017672476300002022
Figure BDA00017672476300002023
记
Figure BDA00017672476300002024
Figure BDA00017672476300002025
Figure BDA00017672476300002026
上述方程的建立步骤为：
记
Figure BDA0001767247630000211
故有
Figure BDA0001767247630000212
ex的能量为
Figure BDA0001767247630000213
pex为瞬时轴功率；pac为驱动轴的驱动力及驱动力矩产生的功率。
由式(26)、式(27)、式(31)、式(33)及式(41)得式(40)。
将偏速度计算公式式(19)，式(18)及式(20)代入Ju-Kane动力学预备方程(17)得
Figure BDA0001767247630000214
由式(21)得
Figure BDA0001767247630000215
考虑式(43)，则有
Figure BDA0001767247630000216
同样，考虑式(43)，得
Figure BDA0001767247630000217
将式(43)至式(45)代入式(42)得式(34)至式(39)。
实施例1
给定如图3所示的通用3R机械臂，A＝(i,1:3]；应用本发明的方法建立树链Ju-Kane动力学方程，并得到广义惯性矩阵。
步骤1建立基于轴不变量的迭代式运动方程。
由式(46)基于轴不变量的转动变换矩阵
Figure BDA0001767247630000221
得
Figure BDA0001767247630000222
运动学迭代式：
Figure BDA0001767247630000223
Figure BDA0001767247630000224
Figure BDA0001767247630000225
Figure BDA0001767247630000226
Figure BDA0001767247630000227
二阶张量投影式：
Figure BDA0001767247630000228
Figure BDA0001767247630000229
由式(48)及式(47)得
Figure BDA00017672476300002210
由式(49)，式(47)及式(55)得
Figure BDA00017672476300002211
由式(50)及式(55)得
Figure BDA0001767247630000231
由式(51)、式(55)及式(57)得
Figure BDA0001767247630000232
由式(52)及式(55)得
Figure BDA0001767247630000233
由式(53)及式(55)得
Figure BDA0001767247630000234
步骤2建立动力学方程。先建立第1轴的动力学方程。由式(37)得
Figure BDA0001767247630000235
由式(39)得
Figure BDA0001767247630000236
由式(61)及式(62)得第1轴的动力学方程，
Figure BDA0001767247630000237
建立第2轴的动力学方程。由式(37)得
Figure BDA0001767247630000238
由式(39)得
Figure BDA0001767247630000241
由式(64)及式(65)得第2轴的动力学方程，
Figure BDA0001767247630000242
最后，建立第3轴的动力学方程。由式(37)得
Figure BDA0001767247630000243
由式(39)得
Figure BDA0001767247630000244
由式(67)及式(68)得第3轴的动力学方程，
Figure BDA0001767247630000245
由式(61)，式(63)及式(67)得广义质量阵。
Figure BDA0001767247630000246
由此可知，只要程式化地将系统的拓扑、结构参数、质惯量等参数代入式(36)至式(40)就可以完成动力学建模。通过编程，很容易实现Ju-Kane动力学方程。因后续的树链Ju-Kane规范方程是以Ju-Kane动力学方程推导的，树链Ju-Kane动力学方程的有效性可由Ju-Kane规范型实例证明。
3.4树链刚体系统Ju-Kane动力学规范型
在建立系统动力学方程后，紧接着就是方程求解的问题。在动力学系统仿真时，通常给定环境作用的广义力及驱动轴的广义驱动力，需要求解动力学系统的加速度；这是动力学方程求解的正问题。在求解前，首先需要得到式(71)所示的规范方程。
规范化动力学方程，
Figure BDA0001767247630000251
其中：RHS–右手侧(Right hand side)
显然，规范化过程就是将所有关节加速度项进行合并的过程；从而，得到关节加速度的系数。将该问题分解为运动链的规范型及闭子树的规范型两个子问题。
3.4.1运动链的规范型方程
将式(36)及式(37)中关节加速度项的前向迭代过程转化为反向求和过程，以便后续应用；显然，其中含有6种不同类型的加速度项，分别予以处理。
【1】给定运动链
Figure BDA0001767247630000252
则有
Figure BDA0001767247630000253
上式的推导步骤为：
Figure BDA0001767247630000254
【2】给定运动链
Figure BDA0001767247630000255
则有
Figure BDA0001767247630000256
上式的推导步骤为：因
Figure BDA0001767247630000257
故得
Figure BDA0001767247630000261
【3】给定运动链
Figure BDA0001767247630000262
则有
Figure BDA0001767247630000263
上式可由下式而得，因
Figure BDA0001767247630000264
故有
Figure BDA0001767247630000265
【4】给定运动链
Figure BDA0001767247630000266
则有
Figure BDA0001767247630000267
上式的推导步骤为：考虑
Figure BDA0001767247630000268
将式(72)代入式(75)左侧得
Figure BDA0001767247630000271
【5】给定运动链
Figure BDA0001767247630000272
则有
Figure BDA0001767247630000273
上式的推导步骤为：考虑
Figure BDA0001767247630000274
将式(72)代入式(76)左侧得
Figure BDA0001767247630000275
【6】给定运动链
Figure BDA0001767247630000276
则有
Figure BDA0001767247630000277
上式的推导步骤为：因
Figure BDA0001767247630000278
故有
Figure BDA0001767247630000279
Figure BDA0001767247630000281
3.4.2闭子树的规范型方程
因闭子树uL中的广义力具有可加性；因此闭子树的节点有唯一一条至根的运动链，式(73)至式(77)的运动链iln可以被uL替换。由式(73)得
Figure BDA0001767247630000282
由式(74)得
Figure BDA0001767247630000283
由式(75)得
Figure BDA0001767247630000284
由式(76)得
Figure BDA0001767247630000285
由式(77)得
Figure BDA0001767247630000286
至此，已具备建立规范型的前提条件。
3.5树链刚体系统Ju-Kane动力学规范方程
下面，建立树结构刚体系统的Ju-Kane规范化动力学方程。为表达方便，首先定义
Figure BDA0001767247630000291
然后，应用式(78)至式(82)，将式(36)及式(37)表达为规范型。
【1】式(36)的规范型为
Figure BDA0001767247630000292
上式的具体建立步骤为：由式(24)及式(36)得
Figure BDA0001767247630000293
由式(52)及式(85)得
Figure BDA0001767247630000294
将式(80)代入式(85)右侧前一项得
Figure BDA0001767247630000295
将式(79)代入式(86)右侧后一项得
Figure BDA0001767247630000296
将式(87)及式(88)代入式(86)得
Figure BDA0001767247630000297
对于刚体k，有
Figure BDA0001767247630000298
由式(35)、式(83)及式(89)得式(84)。【2】式(37)的规范型为
Figure BDA0001767247630000301
上式的具体建立步骤为：由式(37)得
Figure BDA0001767247630000302
将式(78)代入式右侧前一项(91)得
Figure BDA0001767247630000303
将式(81)代入式(91)右侧后一项得
Figure BDA0001767247630000304
将式(82)代入式(91)右侧中间一项得
Figure BDA0001767247630000305
将式(92)，式(93)及式(94)代入式(92)得
Figure BDA0001767247630000306
对于刚体k，有
Figure BDA0001767247630000311
由式(35)，式(83)及式(95)得式(90)。
【3】应用式(84)及式(90)，将Ju-Kane方程重新表述为如下树链Ju-Kane规范型方程：
给定多轴刚体系统D＝{A,K,T,NT,F,B}，惯性系记为F[i]，
Figure BDA0001767247630000312
除了重力外，作用于轴u的合外力及力矩在
Figure BDA0001767247630000313
上的分量分别记为
Figure BDA0001767247630000314
及
Figure BDA0001767247630000315
轴k的质量及质心转动惯量分别记为mk及
Figure BDA0001767247630000316
轴k的重力加速度为
Figure BDA0001767247630000317
驱动轴u的双边驱动力及驱动力矩在
Figure BDA0001767247630000318
上的分量分别记为
Figure BDA0001767247630000319
及
Figure BDA00017672476300003110
环境i对轴l的作用力及力矩分别为
Figure BDA00017672476300003111
及iτl；则轴u的Ju-Kane动力学规范方程为
Figure BDA00017672476300003112
其中：
Figure BDA00017672476300003113
及
Figure BDA00017672476300003114
是3×3的分块矩阵，
Figure BDA00017672476300003115
及
Figure BDA00017672476300003116
是3D矢量。并且，
Figure BDA00017672476300003117
Figure BDA00017672476300003118
Figure BDA00017672476300003119
Figure BDA00017672476300003120
Figure BDA00017672476300003121
Figure BDA0001767247630000321
式中，kI表示杆k质心I；轴k的质量及质心转动惯量分别记为mk及
Figure BDA0001767247630000322
Figure BDA0001767247630000323
为转动轴u的惯性矩阵；
Figure BDA0001767247630000324
为平动轴u的惯性矩阵；hR为转动轴u的非惯性矩阵；hP为平动轴u的非惯性矩阵；作用于轴u的合外力及力矩在
Figure BDA0001767247630000325
上的分量分别记为
Figure BDA0001767247630000326
及
Figure BDA0001767247630000327
Figure BDA0001767247630000328
作用于轴u的合外力及力矩在
Figure BDA0001767247630000329
上的分量分别记为
Figure BDA00017672476300003210
及
Figure BDA00017672476300003211
驱动轴u的双边驱动力及驱动力矩在
Figure BDA00017672476300003212
上的分量分别记为
Figure BDA00017672476300003213
及
Figure BDA00017672476300003214
环境i对轴l的作用力及作用力矩分别为
Figure BDA00017672476300003215
及iτl。
若多轴刚体系统D＝{A,K,T,NT,F,B}仅包含转动轴，
Figure BDA00017672476300003216
则式(101)可简化为
Figure BDA00017672476300003217
4.闭链刚体系统的Ju-Kane动力学方程建立
下面，先陈述闭链刚体系统的居―凯恩(简称Ju-Kane)动力学方程；然后，给出具体建模过程。
给定多轴刚体系统D＝{A,K,T,NT,F,B}，惯性系记为F[i]，
Figure BDA00017672476300003218
Figure BDA00017672476300003219
Figure BDA00017672476300003220
除了重力外，作用于轴u的合外力及力矩在
Figure BDA00017672476300003221
上的分量分别记为
Figure BDA00017672476300003222
及
Figure BDA00017672476300003223
轴k的质量及质心转动惯量分别记为mk及
Figure BDA00017672476300003224
轴k的重力加速度为
Figure BDA00017672476300003225
驱动轴u的双边驱动力及驱动力矩在
Figure BDA00017672476300003226
上的分量分别记为
Figure BDA00017672476300003227
及
Figure BDA00017672476300003228
环境i对轴l的作用力及作用力矩分别为
Figure BDA00017672476300003229
及iτl；轴u对轴u′的广义约束力记为
Figure BDA00017672476300003230
则有闭链刚体系统的Ju-Kane动力学方程：
【1】轴u及轴u′的Ju-Kane动力学规范方程分别为
Figure BDA00017672476300003231
Figure BDA00017672476300003232
【2】非树约束副uku′的约束代数方程为
Figure BDA0001767247630000331
Figure BDA0001767247630000332
Figure BDA0001767247630000333
Figure BDA0001767247630000334
其中：
Figure BDA0001767247630000335
Figure BDA0001767247630000336
Figure BDA0001767247630000337
Figure BDA0001767247630000338
Figure BDA0001767247630000339
式中：
Figure BDA00017672476300003310
及
Figure BDA00017672476300003311
是3×3的分块矩阵，
Figure BDA00017672476300003312
及
Figure BDA00017672476300003313
是3D矢量；kI表示杆k质心I；轴k的质量及质心转动惯量分别记为mk及
Figure BDA00017672476300003314
Figure BDA00017672476300003315
为转动轴u的惯性矩阵；
Figure BDA00017672476300003316
为平动轴u的惯性矩阵；hR为转动轴u的非惯性矩阵；hP为平动轴u的非惯性矩阵；
Figure BDA00017672476300003317
为平动关节角速度；
Figure BDA00017672476300003318
为转动关节角速度。
具体建模过程如下：
非树约束副
Figure BDA00017672476300003319
保持约束点uS及u′S一致，故有
Figure BDA00017672476300003320
由式(115)得
Figure BDA00017672476300003321
轴u对轴u′在约束轴方向上的广义约束力
Figure BDA00017672476300003322
及轴u′对轴u在约束轴方向上的广义约束力
Figure BDA00017672476300003323
的功率分别为
Figure BDA00017672476300003324
由式(116)及式(117)得
Figure BDA00017672476300003325
由式(116)得
Figure BDA0001767247630000341
Figure BDA0001767247630000342
Figure BDA0001767247630000343
Figure BDA0001767247630000344
δ表示增量；
由式(18)及式(119)得
Figure BDA0001767247630000345
故有
Figure BDA0001767247630000346
由式(111)及式(123)得式(106)。由式(19)及式(120)得
Figure BDA0001767247630000347
由式(112)及式(124)得式(107)。由式(19)及式(121)得
Figure BDA0001767247630000348
由式(113)及式(125)得式(108)。由式(19)及式(122)得
Figure BDA0001767247630000349
由式(114)及式(126)得(109)。由式(18)，式(117)及式(111)得
Figure BDA00017672476300003410
Figure BDA00017672476300003411
广义约束力
Figure BDA0001767247630000351
及
Figure BDA0001767247630000352
是矢量，由式(127)及式(128)得式(110)。由此可知，偏速度主要应用于力的反向迭代。广义约束力
Figure BDA0001767247630000353
及
Figure BDA0001767247630000354
视为外力。
根据轴u的Ju-Kane动力学规范方程得式(104)及式(105)。
以关节空间自然轴链为基础的Ju-Kane闭链刚体动力学克服了笛卡尔坐标轴链空间的局限：
【1】在基于笛卡尔坐标轴链的牛顿欧拉动力学中，非树运动副uku′∈P约束不能表达
Figure BDA0001767247630000355
及
Figure BDA0001767247630000356
或
Figure BDA0001767247630000357
及
Figure BDA0001767247630000358
的情形，即不能表达齿条与齿轮、蜗轮与蜗杆等约束。而本申请的非树约束副uku′的约束代数方程式(106)至式(109)可表达任一种约束类形，并且物理内涵明晰；
【2】在基于笛卡尔坐标轴链的牛顿欧拉动力学当中，非树运动副代数约束方程是6D的；而式(106)至式(109)表示是3D非树运动副代数约束方程，从而降低了系统方程求解的复杂度；
【3】在基于笛卡尔坐标轴链的牛顿欧拉动力学当中，非树运动副代数约束方程是关于6D矢量空间绝对加速度的，是关于关节坐标、关节速度的迭代式，具有累积误差；而式(106)至式(109)是关于关节加速度的，保证了约束方程的准确性。
5.基于轴不变量的约束力求解
对于无功率损耗的运动轴u，记其约束力及约束力矩矢量分别为
Figure BDA0001767247630000359
Figure BDA00017672476300003510
显然，有
Figure BDA00017672476300003511
由式(96)及式(140)计算得
Figure BDA00017672476300003512
式(129)表示运动轴矢量与运动轴约束力具有自然正交补的关系。
若
Figure BDA00017672476300003513
及
Figure BDA00017672476300003514
为运动副
Figure BDA00017672476300003515
的两个正交约束轴，且约束轴与运动轴正交，即
Figure BDA00017672476300003516
记
Figure BDA00017672476300003517
为约束轴轴矢量，
Figure BDA00017672476300003518
替换式(96)中
Figure BDA00017672476300003519
重新计算得
Figure BDA00017672476300003520
其中：
Figure BDA00017672476300003521
Figure BDA00017672476300003522
在完成前向动力学正解后，根据已计算的关节加速度
Figure BDA00017672476300003523
由式(131)可以得到关节约束力大小
Figure BDA00017672476300003524
约束力矩大小
Figure BDA0001767247630000361
当
Figure BDA0001767247630000362
时，由式(131)得
Figure BDA0001767247630000363
且
Figure BDA0001767247630000364
式(131)中同一时刻具有相同的运动状态及内外力。仅在运动轴向上出现力及力矩的平衡；而在约束轴向，动力学方程不满足，即力与力矩不一定平衡。
由式(131)可以得到关节约束力大小
Figure BDA0001767247630000365
及
Figure BDA0001767247630000366
约束力矩大小
Figure BDA0001767247630000367
及
Figure BDA0001767247630000368
若记运动轴径向力矢量
Figure BDA0001767247630000369
及力矩矢量
Figure BDA00017672476300003610
则有
Figure BDA00017672476300003611
若记运动轴径向力大小为
Figure BDA00017672476300003612
及力矩大小为
Figure BDA00017672476300003613
由式(134)得
Figure BDA00017672476300003614
至此，完成了轴径向约束广义力的计算。
树链刚体系统对应的关节加速度序列记
Figure BDA00017672476300003615
可根据下述步骤计算：
将根据运动轴类型及自然参考轴表达的刚体运动链广义惯性矩阵称为轴链刚体广义惯性矩阵，简称轴链广义惯性矩阵。
定义正交补矩阵
Figure BDA00017672476300003616
及对应的叉乘矩阵
Figure BDA00017672476300003617
Figure BDA00017672476300003618
给定多轴刚体系统D＝{A,K,T,NT,F,B}，
Figure BDA00017672476300003619
将系统中各轴动力学方程(96)按行排列；将重排后的轴驱动广义力及不可测的环境作用力记为fC，可测的环境广义作用力记为fi；将系统对应的关节加速度序列记为
Figure BDA00017672476300003620
将重排后的
Figure BDA00017672476300003621
记为h；考虑式(136)；则该系统动力学方程为
Figure BDA00017672476300003622
由式(137)得
Figure BDA00017672476300003623
其中，
Figure BDA00017672476300003624
由式(137)得
Figure BDA00017672476300003625
6.广义内摩擦力及粘滞力计算
在完成轴径向约束广义力的计算后，得到运动轴u的径向约束力大小
Figure BDA0001767247630000371
及约束力矩大小
Figure BDA0001767247630000372
如图3、图4所示，记运动轴u的内摩擦力大小及内摩擦力矩大小分别为
Figure BDA0001767247630000373
及
Figure BDA0001767247630000374
运动轴u的粘滞力及粘滞力矩大小分别为
Figure BDA0001767247630000375
及
Figure BDA0001767247630000376
故有
Figure BDA0001767247630000377
Figure BDA0001767247630000378
其中：sk[u]─运动轴u的内摩擦系数，ck[u]─运动轴u的粘滞系数；sign()表示取正或负符号。
记广义内摩擦力及粘滞力的合力及合力矩分别为
Figure BDA0001767247630000379
由式(141)及式(142)得
Figure BDA00017672476300003710
运动轴的广义内摩擦力及粘滞力是运动轴的内力，因为它们仅存在于运动轴向上，与轴径向约束力总是正交的。当运动轴轴向动态作用力平衡时，无论广义内摩擦力及粘滞力是否存在或大小如何，都不影响动力学系统的运动状态；故而，不影响运动轴的径向约束力。因此，由式(131)至式(135)计算运动轴u的径向约束力大小
Figure BDA00017672476300003711
及约束力矩大小
Figure BDA00017672476300003712
时，可以不考虑运动轴的广义内摩擦力及粘滞力。
7.建立闭链刚体非理想约束系统的Ju-Kane动力学显式模型
设运动轴u的广义内摩擦及粘滞的合力及合力矩分别为
Figure BDA00017672476300003713
闭链刚体系统的Ju-Kane动力学方程建立后，计算关节加速度
Figure BDA00017672476300003720
后，应用式(130)至式(135)计算径向约束力大小
Figure BDA00017672476300003714
及
Figure BDA00017672476300003715
约束力矩大小
Figure BDA00017672476300003716
及
Figure BDA00017672476300003717
再建立如下闭链刚体非理想约束系统的Ju-Kane动力学方程：
【1】轴u及轴u′的Ju-Kane动力学规范方程分别为
Figure BDA00017672476300003718
Figure BDA00017672476300003719
【2】非树约束副uku′的约束代数方程为
Figure BDA0001767247630000381
Figure BDA0001767247630000382
Figure BDA0001767247630000383
Figure BDA0001767247630000384
其它，参见式(104)至式(114)、式(97)至式(102)。
建立过程为：
运动轴u的内摩擦及粘滞合力
Figure BDA0001767247630000385
及合力矩
Figure BDA0001767247630000386
是运动轴u的外力，故有式(144)；运动轴u′的内摩擦及粘滞合力
Figure BDA0001767247630000387
及合力矩
Figure BDA0001767247630000388
是运动轴u′的外力，故有式(145)。其它过程与闭链刚体系统的Ju-Kane动力学方程建模步骤相同。
二、动基座刚体系统的Ju-Kane动力学规范方程
动基座刚体系统应用领域越来越广泛，包含：空间机械臂、星表巡视器、双足机器人等。下面，先陈述动基座刚体系统的Ju-Kane动力学定理；然后，予以证明；最后，给出三轮移动系统及CE3巡视器动力学建模示例。
1.动基座刚体系统的Ju-Kane动力学方程
给定多轴刚体移动系统D＝{A,K,T,NT,F,B}，惯性系记为F[i]，
Figure BDA0001767247630000389
Figure BDA00017672476300003810
Figure BDA00017672476300003811
轴序列为iAc＝(i,c1,c2,c3,c4,c5,c]，轴类型序列为iKc＝(X,R,R,R,P,P,P]，该运动链为ilc＝(i,c1,c2,c3,c4,c5,c]；除了重力外，作用于轴u的合外力及力矩在
Figure BDA00017672476300003812
上的分量分别为
Figure BDA00017672476300003813
及
Figure BDA00017672476300003814
轴k的质量及质心转动惯量分别为mk及
Figure BDA00017672476300003815
轴k的重力加速度为
Figure BDA00017672476300003816
驱动轴u的双边驱动力及驱动力矩在
Figure BDA00017672476300003817
上的分量分别为
Figure BDA00017672476300003818
及
Figure BDA00017672476300003819
环境i对轴l的作用力及作用力矩分别为
Figure BDA00017672476300003820
及iτl；作用于体c上的合力及合力矩分别为
Figure BDA00017672476300003821
及i|Dτc，记
Figure BDA00017672476300003822
Figure BDA00017672476300003823
且有
Figure BDA00017672476300003824
Figure BDA00017672476300003825
则有
Figure BDA00017672476300003826
其中：
Figure BDA0001767247630000391
表示c的开子树，
Figure BDA0001767247630000392
且有：
Figure BDA0001767247630000393
Figure BDA0001767247630000394
Figure BDA0001767247630000395
Figure BDA0001767247630000396
其它，参见式(99)至式(102)。
下面给出具体过程：显然,有
Figure BDA0001767247630000397
由式(150)及式(151)可知，它们确定了轴c的笛卡尔直角坐标系，但三个转动轴序列存在12种。由式(19)得
Figure BDA0001767247630000398
由式(158)得
Figure BDA0001767247630000399
由式(159)得
Figure BDA00017672476300003910
故有
Figure BDA00017672476300003911
Figure BDA0001767247630000401
由式(98)及式(160)得
Figure BDA0001767247630000402
由式(161)及式(163)得(155)。由式(101)及式(160)得
Figure BDA0001767247630000403
由式(162)及式(164)得式(156)。
由动基座刚体系统的Ju-Kane动力学方程可知，可以根据需要由式(150)确定本体c的笛卡尔体系F[c]三个转动轴的序列，在建立动力学方程后，通过积分完成动力学仿真，直接可以得到所期望的姿态。
实施例1：基于Ju-Kane的10轴三轮移动系统动力学建模及逆解
给定三轮移动系统D＝{A,K,T,NT,F,B}，如图3所示，轴1、轴2及轴3驱动车轮，轴3驱动舵机；轴序列为A＝(i,c1,c2,c3,c4,c5,c,1:4]，父轴序列为
Figure BDA0001767247630000405
轴l的质量及质心转动惯量分别为ml及
Figure BDA0001767247630000404
l∈[c,1:4]。应用本发明的方法建立各轴的动力学方程。
步骤1显然，|A|＝4，|B|＝5，|NT|＝|O|＝3，由轮式多轴系统的自由度DOF(D)＝6+|A|-|NT|+|O|，计算轮式多轴系统的自由度，得DOF(D)＝10。
根据轮式多轴系统移动维度准则，计算得轮式多轴系统移动维度DOM(D)为3，由于，当DOM(D)>3时，系统在自然地形上是静不定的；当DOM(D)＝3时，系统在自然地形上是静定的；当DOM(D)＝2时，系统在人工地形上是静定的。故该系统D在自然路面上静定。
步骤2基于轴不变量的正向运动学计算
由式(46)得
Figure BDA0001767247630000411
由式(48)及式(46)计算
Figure BDA0001767247630000412
由式(49)计算
Figure BDA0001767247630000413
由式(50)计算
Figure BDA0001767247630000414
考虑
Figure BDA0001767247630000415
及
Figure BDA0001767247630000416
其中l＝[c,1:4]；由式(55)计算
Figure BDA0001767247630000417
由式(53)计算
Figure BDA0001767247630000418
步骤3建立Ju-Kane动力学规范方程
由式(155)及式(156)得
Figure BDA0001767247630000419
Figure BDA00017672476300004110
由式(99)及式(100)分别计算
Figure BDA00017672476300004111
及
Figure BDA00017672476300004112
代入式(152)得
Figure BDA00017672476300004113
由式(103)，式(100)及式(96)得
Figure BDA00017672476300004114
至此，获得全部10个轴的动力学方程。
步骤4进行力反向迭代
对于给定轴链
Figure BDA00017672476300004115
根据轮土矢量矢学及移动维度，轮土作用的牵引力为
Figure BDA0001767247630000421
其中，
Figure BDA0001767247630000422
Figure BDA0001767247630000423
—前向库仑摩擦系数；
Figure BDA0001767247630000424
—侧向库仑摩擦系数；
Figure BDA0001767247630000425
—轮土正压力；
Figure BDA0001767247630000426
－牵引力；
Figure BDA0001767247630000427
－取
Figure BDA0001767247630000428
的正或负符号；
当包含侧向库仑摩擦力及轮土侧向力
Figure BDA0001767247630000429
时，
Figure BDA00017672476300004210
由式(173)、式(174)及(102)得
Figure BDA00017672476300004211
Figure BDA00017672476300004212
若仅考虑轮土作用力及主动轴驱动力，则由式(102)得
Figure BDA00017672476300004213
步骤5计算动力学方程逆解
将式(175)至式(177)写为整体形式
f10×1＝B10×10·u10×1； (178)
其中：
Figure BDA0001767247630000431
给定
Figure BDA0001767247630000432
由式(171)及式(172)计算f。若B-1存在，由式(178)得
u＝B-1·f。 (180)
由式(179)及式(180)可知：
【1】控制力矩
Figure BDA0001767247630000433
及
Figure BDA0001767247630000434
与轮土作用力为
Figure BDA0001767247630000435
及
Figure BDA0001767247630000436
存在耦合；
【2】完成动力学逆解计算后，不仅得到驱动轴控制力矩
Figure BDA0001767247630000437
及
Figure BDA0001767247630000438
而且可以得到轮土作用力
Figure BDA0001767247630000439
及
Figure BDA00017672476300004310
故该逆解作用在于：
【2-1】计算驱动轴期望控制力矩
Figure BDA00017672476300004311
及
Figure BDA00017672476300004312
【2-2】通过运动状态(位姿、速度及加速度)实现轮土作用力
Figure BDA00017672476300004313
及
Figure BDA00017672476300004314
的间接测量。
实施例2：基于Ju-Kane的20轴巡视器移动系统动力学建模及逆解
给定图4所示的CE3月面巡视器移动系统D＝{A,K,T,NT,F,B}，该系统Span树如图5所示；标识符及缩略符如下：
c―Chassis/车箱；i―Inertial Space/惯性空间(或导航系)；
rr―Right Rocker/右主臂(副臂)；rb―Right Bogie/右副臂；
lr―Left Rocker/左主臂(副臂)；lb―Left Bogie/左副臂；
rfd―Right Front Direction/右前方向机；rrd―Right Rear Direction/右后方向机；
lfd―Left Front Direction/左前方向机；lrd―Left Rear Direction/左后方向机；
rfw―Right Front Wheel/右前轮；lfw―Left Front Wheel/左前轮；
rmw―Right Middle Wheel/右中轮；lmw―Left Middle Wheel/左中轮；
rrw―Right Rear Wheel/右后轮；lrw―Left Rear Wheel/左后轮；
rfc―Right Front Wheel-Earth Contractor/右前轮地接触点；
lfc―Left Front Wheel-Earth Contractor/左前轮地接触点；
rmc―Right Middle Wheel-Earth Contractor/右中轮地接触点；
lmc―Left Middle Wheel-Earth Contractor/左中轮地接触点；
rrc―Right Rear Wheel-Earth Contractor/右后轮地接触点；
lrc―Left Rear Wheel-Earth Contractor/左后轮地接触点；
结合系统结构参数如图2；建立该系统的动力学方程。
步骤1显然，|A|＝14，|NT|＝7，|O|＝6；轮式多轴系统移动维度DOM(D)为3，故多轴系统D能适应自然路面。轴链A、父轴链
Figure BDA00017672476300004411
及非树集合NT分别为
A＝(i,c1,c2,c3,c4,c5,c,rr,rb,rrd,rrw,rmw,rfd,rfw,lr,lb,lrd,lrw,lfd,lfw,lmw]，
Figure BDA0001767247630000441
Figure BDA0001767247630000442
步骤2：基于轴不变量的多轴系统正运动学计算
由式(46)得
Figure BDA0001767247630000443
由式(48)及式(181)计算
Figure BDA0001767247630000444
由式(49)及式(182)计算
Figure BDA0001767247630000445
由式(50)计算
Figure BDA0001767247630000446
记l∈(c,rb,rr,rfd,rfw,rmw,rrd,rrw,lb,lr,lfd,lfw,lmw,lrd,lrw],因是刚体系统，故有
Figure BDA0001767247630000447
and
Figure BDA0001767247630000448
由式(51),(182)及式(184)计算
Figure BDA0001767247630000449
由式(53)计算
Figure BDA00017672476300004410
步骤3建立动力学方程
由式(155)及式(156)得
Figure BDA0001767247630000451
Figure BDA0001767247630000452
由式(99)及式(100)分别计算
Figure BDA0001767247630000453
及
Figure BDA0001767247630000454
代入式(152)得
Figure BDA0001767247630000455
由式(103)，式(100)及式(96)得
Figure BDA0001767247630000456
其中：u∈[rfd,rfw,rmw,rrd,rrw,lfd,lfw,lmw,lrd,lrw]。
由式(106)，式(100)，式(104)及式(110)得
Figure BDA0001767247630000457
Figure BDA0001767247630000458
由式(111)及式(106)得
Figure BDA0001767247630000459
其中，差速轴初始角度为
Figure BDA00017672476300004510
由式(103)，式(100)及式(96)得
Figure BDA00017672476300004511
至此，获得19轴动力学方程及1个非树约束副3D代数方程；其中，包含19轴外力矩标量及1轴约束力矩矢量。
步骤4进行力反向迭代
由式(40)得
Figure BDA00017672476300004512
Figure BDA00017672476300004513
Figure BDA00017672476300004514
Figure BDA0001767247630000461
Figure BDA0001767247630000462
Figure BDA0001767247630000463
Figure BDA0001767247630000464
Figure BDA0001767247630000465
步骤5计算动力学方程逆解
增加四个舵机驱动力矩约束，
Figure BDA0001767247630000466
增加驱动轮力矩约束，
Figure BDA0001767247630000467
记
Figure BDA0001767247630000468
将式(194)至式(201)写为整体形式
f20×1＝B20×20·u20×1 (205)
其中：
Figure BDA0001767247630000469
B[1:6][1:4]＝06×4， (207)
Figure BDA0001767247630000471
Figure BDA0001767247630000472
Figure BDA0001767247630000473
Figure BDA0001767247630000474
Figure BDA0001767247630000475
Figure BDA0001767247630000476
Figure BDA0001767247630000477
Figure BDA0001767247630000478
记k∈[rb,rr,rfd,rfw,rmw,rrd,rrw,lb,lr,lfd,lfw,lmw,lrd,lrw]，由式(187)至式(192)求逆解得
Figure BDA0001767247630000479
及
Figure BDA00017672476300004710
共计21个标量；从而，由式(205)得u。
由上求解过程可知：
【1】由于该系统存在6个驱动轴及4个舵机轴，而该移动系统移动自由度为3，故存在7个冗余的控制轴。通过式(202)及式(203)人为地加入7个约束，保证了逆解存在的唯一性；
【2】通过动力学计算，不仅可以计算该系统的控制力矩，也唯一求解了六个轮土作用力；通过该移动系统运动状态的检测，应用动力学逆解，实现了轮土作用力的间接测量。
以上给出了基于Ju-Kane的20轴巡视器移动系统动力学建模及逆解、基于Ju-Kane的10轴三轮移动系统动力学建模及逆解的实施例。实施例表明：应用Ju-Kane动力学原理建立多轴系统动力学显式模型简洁、方便、高效，适宜高自由度的多轴系统动力学显式建模与数值建模。同时表明，通过显式动力学建模与计算，可以通过多轴系统的运动检测及视觉检测，计算系统与环境的接触作用力，为多轴系统力位控制及环境感知提供了新的技术途径。
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。

Claims (4)

1.一种基于轴不变量的动基座多轴机器人动力学建模与解算方法，其特征是，
给定多轴刚体移动系统D＝{A，K，T，NT，F，B}，惯性系记为F[i]，
Figure FDA00024879728900000125
u′，k，l∈A，
Figure FDA00024879728900000126
A为轴序列，F为杆件参考系序列，B为杆件体序列，x为运动副类型序列，T表示带方向的span树，NT为约束轴的序列即非树；
Figure FDA0002487972890000013
轴类型序列为iKc＝(x，R，R，R，P，P，P]，X、R、P均表示轴类型，其中，R为转动副，P为棱柱副，剩余的轴类型归结为X；除了重力外，作用于轴u的合外力及力矩在
Figure FDA0002487972890000014
上的分量分别为
Figure FDA0002487972890000015
及
Figure FDA0002487972890000016
轴k的质量及质心转动惯量分别为mk及
Figure FDA0002487972890000017
轴k的重力加速度为
Figure FDA0002487972890000018
驱动轴u的双边驱动力及驱动力矩在
Figure FDA0002487972890000019
上的分量分别为
Figure FDA00024879728900000110
及
Figure FDA00024879728900000111
环境i对轴l的作用力及作用力矩分别为
Figure FDA00024879728900000112
及iτl；记
Figure FDA00024879728900000113
则有以下动基座刚体系统的Ju-Kane动力学方程
Figure FDA00024879728900000114
Figure FDA00024879728900000115
则有
Figure FDA00024879728900000116
其中：cQi表示由 c到i的旋转变换阵； c L表示c的开子树，cL-c＝ c L，且有：
Figure FDA00024879728900000117
Figure FDA00024879728900000118
Figure FDA00024879728900000119
Figure FDA00024879728900000120
式中，
Figure FDA00024879728900000121
及
Figure FDA00024879728900000122
是3D矢量；kI表示轴k质心；
Figure FDA00024879728900000123
为转动轴的惯性矩阵；
Figure FDA00024879728900000124
为平动轴的惯性矩阵；hR为转动轴的非惯性矩阵；hP为平动轴的非惯性矩阵；
闭链刚体系统的Ju-Kane动力学方程根据树链Ju-Kane规范型方程建立；
树链Ju-Kane规范型方程
Figure FDA0002487972890000021
式中：
Figure FDA0002487972890000022
及
Figure FDA0002487972890000023
是3D矢量；
并且，
Figure FDA0002487972890000024
Figure FDA0002487972890000025
Figure FDA0002487972890000026
Figure FDA0002487972890000027
Figure FDA0002487972890000028
式中，
Figure FDA0002487972890000029
为转动轴的惯性矩阵；
Figure FDA00024879728900000210
为平动轴的惯性矩阵；
Figure FDA00024879728900000211
l1k为取由 轴l至轴k的运动链，uL表示获得由 轴u及其子树构成的闭子树。
2.根据权利要求1所述的基于轴不变量的动基座多轴机器人动力学建模与解算方法，其特征是，
由 式(150)确定本体c的笛卡尔体系F[c]三个转动轴的序列，在建立动力学方程后，通过积分完成动力学仿真，直接得到所期望的姿态。
3.根据权利要求1所述的基于轴不变量的动基座多轴机器人动力学建模与解算方法，其特征是，
动基座刚体系统的Ju-Kane动力学方程根据闭链刚体系统的Ju-Kane动力学方程建立。
4.根据权利要求3所述的基于轴不变量的动基座多轴机器人动力学建模与解算方法，其特征是，闭链刚体系统的Ju-Kane动力学方程为：
【1】轴u及轴u′的Ju-Kane动力学规范方程分别为
Figure FDA0002487972890000031
Figure FDA0002487972890000032
【2】非树约束副uku′的约束代数方程为
Figure FDA0002487972890000033
Figure FDA0002487972890000034
Figure FDA0002487972890000035
Figure FDA0002487972890000036
其中：
Figure FDA0002487972890000037
Figure FDA0002487972890000038
Figure FDA0002487972890000039
Figure FDA00024879728900000310
Figure FDA00024879728900000311
式中：
Figure FDA00024879728900000312
为
Figure FDA00024879728900000313
的二阶导数，
Figure FDA00024879728900000314
为
Figure FDA00024879728900000315
的二阶导数。
CN201810933652.9A 2018-08-16 2018-08-16 基于轴不变量的动基座多轴机器人动力学建模与解算方法 Active CN108803350B (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN201810933652.9A CN108803350B (zh) 2018-08-16 2018-08-16 基于轴不变量的动基座多轴机器人动力学建模与解算方法
PCT/CN2018/112616 WO2020034401A1 (zh) 2018-08-16 2018-10-30 基于轴不变量的动基座多轴机器人动力学建模与解算方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810933652.9A CN108803350B (zh) 2018-08-16 2018-08-16 基于轴不变量的动基座多轴机器人动力学建模与解算方法

Publications (2)

Publication Number Publication Date
CN108803350A CN108803350A (zh) 2018-11-13
CN108803350B true CN108803350B (zh) 2020-06-23

Family

ID=64080308

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810933652.9A Active CN108803350B (zh) 2018-08-16 2018-08-16 基于轴不变量的动基座多轴机器人动力学建模与解算方法

Country Status (2)

Country Link
CN (1) CN108803350B (zh)
WO (1) WO2020034401A1 (zh)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102073280A (zh) * 2011-01-13 2011-05-25 北京科技大学 一种复杂挠性航天器模糊奇异摄动建模与姿态控制方法
CN103760816A (zh) * 2013-12-30 2014-04-30 哈尔滨工业大学深圳研究生院 基于任务极坐标系的伺服系统轮廓控制方法
CN104573255A (zh) * 2015-01-22 2015-04-29 天津理工大学 一种基于改进多体系统传递矩阵的机械臂建模与求解方法
CN105739513A (zh) * 2016-02-05 2016-07-06 北京航空航天大学 一种四旋翼飞行机器人非线性轨迹跟踪控制器及其跟踪控制方法
CN107529630A (zh) * 2017-06-23 2018-01-02 西北工业大学 一种空间机器人建立动力学模型的方法

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH10217173A (ja) * 1997-02-07 1998-08-18 Yaskawa Electric Corp ロボットの非干渉化制御装置
CN106249616B (zh) * 2016-07-22 2020-06-05 上海航天控制技术研究所 一种在轨服务机械臂动力学建模方法和系统
CN106547989A (zh) * 2016-11-23 2017-03-29 北京邮电大学 具有关节柔性/臂杆柔性机械臂的位置内环阻抗控制算法
CN107953324A (zh) * 2017-12-29 2018-04-24 华南理工大学 基于旋量理论和凯恩方法的蛇形机器人动力学分析方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102073280A (zh) * 2011-01-13 2011-05-25 北京科技大学 一种复杂挠性航天器模糊奇异摄动建模与姿态控制方法
CN103760816A (zh) * 2013-12-30 2014-04-30 哈尔滨工业大学深圳研究生院 基于任务极坐标系的伺服系统轮廓控制方法
CN104573255A (zh) * 2015-01-22 2015-04-29 天津理工大学 一种基于改进多体系统传递矩阵的机械臂建模与求解方法
CN105739513A (zh) * 2016-02-05 2016-07-06 北京航空航天大学 一种四旋翼飞行机器人非线性轨迹跟踪控制器及其跟踪控制方法
CN107529630A (zh) * 2017-06-23 2018-01-02 西北工业大学 一种空间机器人建立动力学模型的方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
一种新的机器人动力学算法;徐向荣 等;《华东冶金学院学报》;19901231;第7卷;16-26 *
多刚体系统运动仿真中选择自然坐标的自适应调节方法;张可维 等;《中国机械工程》;20110930;第22卷(第18期);2143-2147 *
崎岖地形环境中月球车的动力学建模与仿真;彭丽媛 等;《系统科学与数学》;20110630;第31卷(第6期);742-750 *

Also Published As

Publication number Publication date
WO2020034401A1 (zh) 2020-02-20
CN108803350A (zh) 2018-11-13

Similar Documents

Publication Publication Date Title
US11491649B2 (en) Axis-invariant based multi-axis robot kinematics modeling method
WO2020034420A1 (zh) 一种基于轴不变量的多轴机器人逆运动学建模与解算方法
Orin et al. Centroidal momentum matrix of a humanoid robot: Structure and properties
WO2020034422A1 (zh) 一种基于轴不变量的多轴机器人系统正运动学建模与解算方法
WO2020034421A1 (zh) 一种基于轴不变量的多轴机器人系统建模与控制方法
Wang et al. A multi-objective approach for the trajectory planning of a 7-DOF serial-parallel hybrid humanoid arm
CN107529630A (zh) 一种空间机器人建立动力学模型的方法
CN108959829B (zh) 基于轴不变量的非理想关节机器人动力学建模方法
CN109117451B (zh) 基于轴不变量的树链机器人动力学建模与解算方法
CN108803350B (zh) 基于轴不变量的动基座多轴机器人动力学建模与解算方法
CN109015641B (zh) 基于轴不变量的通用6r机械臂逆解建模与解算方法
CN109086544B (zh) 基于轴不变量的闭链机器人动力学建模与解算方法
Li et al. Dynamic modeling and analysis for 6-DOF industrial robots
CN108942943B (zh) 基于轴不变量的多轴机器人正运动学计算方法
Song et al. Analysis of dynamic modeling and solution of 3-RPS parallel mechanism based on conformal geometric algebra
CN109079850B (zh) 基于轴不变量多轴机器人d-h系及d-h参数确定方法
Guo et al. An innovative joint-space explicit dynamics symbolic computation model for closed-chain mechanisms
CN109291047B (zh) 基于轴不变量及dh参数1r/2r/3r逆解建模方法
CN108959828B (zh) 基于轴不变量的通用3r机械臂逆解建模与解算方法
WO2020034416A1 (zh) 基于轴不变量的通用7r机械臂逆解建模与解算方法
Mehrafrooz et al. Dynamic analysis of an n-revolute planar serial manipulator and sensitivity analysis based on sobol's method
Zhang Robot Basics: Representation, Rotation and Velocity
Yu et al. Inverse dynamics analysis of 4-DOF stacking robot with closed chain structure
Dmitriev et al. Representation of a robotic manipulator mechanical subsystem as a component circuit with vector links
Alexiou Projective articulated dynamics

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