本发明涉及一种非理想关节机器人动力学建模与解算方法,属于机器人技术领域。
背景技术
拉格朗日在研究月球天平动问题时提出了拉格朗日方法,是以广义坐标表达动力学方程的基本方法;同时,也是描述量子场论的基本方法。应用拉格朗日法建立动力学方程已是一个烦琐的过程,尽管拉格朗日方程依据系统能量的不变性推导系统的动力学方程,具有理论分析上的优势;但是在工程应用中,随着系统自由度的增加,方程推导的复杂性剧增,难以得到普遍应用。凯恩方程建立过程与拉格朗日方程相比,通过系统的偏速度、速度及加速度直接表达动力学方程。故凯恩动力学方法与拉格朗日方法相比,由于省去了系统能量的表达及对时间的求导过程,极大地降低了系统建模的难度。然而,对于高自由度的系统,凯恩动力学建模方法也是难以适用。
拉格朗日方程及凯恩方程极大地推动了多体动力学的研究,以空间算子代数为基础的动力学由于应用了迭代式的过程,计算速度及精度都有了一定程度的提高。这些动力学方法无论是运动学过程还是动力学过程都需要在体空间、体子空间、系统空间及系统子空间中进行复杂的变换,建模过程及模型表达非常复杂,难以满足高自由度系统建模与控制的需求,因此,需要建立动力学模型的简洁表达式;既要保证建模的准确性,又要保证建模的实时性。没有简洁的动力学表达式,就难以保证高自由度系统动力学工程实现的可靠性与准确性。同时,传统非结构化运动学及动力学符号通过注释约定符号内涵,无法被计算机理解,导致计算机不能自主地建立及分析运动学及动力学模型。
技术实现要素:
本发明所要解决的技术问题是提供一种基于轴不变量的非理想关节机器人动力学建模与解算方法。
为解决上述技术问题,本发明采用以下技术方案:
一种基于轴不变量的非理想关节机器人动力学建模与解算方法,其特征是,
给定多轴刚体系统d={a,k,t,nt,f,b},惯性系记为f[i],
设运动轴u的广义内摩擦及粘滞的合力及合力矩分别为
【1】轴u及轴u′的ju-kane动力学规范方程分别为
式中:
【2】非树约束副uku′的约束代数方程为
闭链刚体系统的ju-kane动力学方程:
【1】轴u及轴u′的ju-kane动力学规范方程分别为
其中:
【2】非树约束副uku′的约束代数方程为
其中:
式中:
应用式(129)至式(134)计算径向约束力大小
上式表示运动轴矢量与运动轴约束力具有自然正交补的关系;
若
记
其中:
由式(130)得到关节约束力大小
若记运动轴径向力大小为
至此,完成了轴径向约束广义力的计算。
由式(130)至式(134)计算运动轴u的径向约束力大小
考虑广义内摩擦力及粘滞力的基于轴不变量的约束力求解步骤为:
在完成轴径向约束广义力的计算后,得到运动轴u的径向约束力大小
其中:sk[u]─运动轴u的内摩擦系数,ck[u]─运动轴u的粘滞系数;sign()表示取正或负符号;
记广义内摩擦力及粘滞力的合力及合力矩分别为
闭链刚体系统的ju-kane动力学方程根据树链ju-kane规范型方程建立。
树链ju-kane规范型方程
其中:
并且,
式中,ki表示杆k质心i;轴k的质量及质心转动惯量分别记为mk及
本发明所达到的有益效果:
对于非理想约束系统,建立了闭链刚体非理想约束系统的ju-kane动力学方程。
【1】在基于笛卡尔坐标轴链的牛顿欧拉动力学中,非树运动副uku′∈p约束不能表达齿条与齿轮、蜗轮与蜗杆等约束。而本申请建立的非树约束副uku′的约束代数方程可表达任一种约束类形,并且物理内涵明晰;
【2】在基于笛卡尔坐标轴链的牛顿欧拉动力学当中,非树运动副代数约束方程是6d的;而本申请建立的非树约束副的约束代数方程表示是3d非树运动副代数约束方程,从而降低了系统方程求解的复杂度;
【3】在基于笛卡尔坐标轴链的牛顿欧拉动力学当中,非树运动副代数约束方程是关于6d矢量空间绝对加速度的,是关于关节坐标、关节速度的迭代式,具有累积误差;而本申请建立的非树约束副的约束代数方程是关于关节加速度的,保证了约束方程的准确性。
附图说明
图1自然坐标系与轴链;
图2固定轴不变量;
图3、图4为运动轴的内摩擦力及粘滞力示意图。
具体实施方式
下面对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案,而不能以此来限制本发明的保护范围。
闭链刚体系统具有非常广泛的应用;比如,ce3巡视器的摇臂移动系统是具有差速器的闭链,重载机械臂通常是具有四连杆的闭链系统。同时,实际的运动轴通常包含内摩擦力及粘滞力。因此研究闭链刚体系统的ju-kane动力学建模非常必要。
描述运动链的基本拓扑符号及操作是构成运动链拓扑符号系统的基础,定义如下:
【1】运动链由偏序集合(]标识。
【2】a[l]为取轴序列a的成员;因轴名l具有唯一的编号对应于a[l]的序号,故a[l]计算复杂度为o(1)。
【3】
【4】
【5】llk为取由轴l至轴k的运动链,输出表示为
【6】ll为取轴l的子。该操作表示在
【7】ll表示获得由轴l及其子树构成的闭子树,ll为不含l的子树;递归执行ll,计算复杂度为
【8】支路、子树及非树弧的增加与删除操作也是必要的组成部分;从而,通过动态span树及动态图描述可变拓扑结构。在支路llk中,若
计算复杂度o()表示计算过程的操作次数,通常指浮点乘与加的次数。以浮点乘与加的次数表达计算复杂度非常烦琐,故常采用算法循环过程中的主要操作次数;比如:关节位姿、速度、加速度等操作的次数。
定义1自然坐标轴:称与运动轴或测量轴共轴的,具有固定原点的单位参考轴为自然坐标轴,亦称为自然参考轴。
定义2自然坐标系:若多轴系统d处于零位,所有笛卡尔体坐标系方向一致,且体坐标系原点位于运动轴的轴线上,则该坐标系统为自然坐标系统,简称自然坐标系。
自然坐标系优点在于:(1)坐标系统易确定;(2)零位时的关节变量为零;(3)零位时的系统姿态一致;(4)不易引入测量累积误差。
定义3不变量:称不依赖于一组坐标系进行度量的量为不变量。
由定义2可知,在系统处于零位时,所有杆件的自然坐标系与底座或世界系的方向一致。系统处于零位即
由上式知,
对轴不变量而言,其绝对导数就是其相对导数。因轴不变量是具有不变性的自然参考轴,故其绝对导数恒为零矢量。因此,轴不变量具有对时间微分的不变性。有:
定义4转动坐标矢量:绕坐标轴矢量
定义5平动坐标矢量:沿坐标轴矢量
定义6自然坐标:以自然坐标轴矢量为参考方向,相对系统零位的角位置或线位置,记为ql,称为自然坐标;称与自然坐标一一映射的量为关节变量;其中:
定义7机械零位:对于运动副
故关节
定义8自然运动矢量:将由自然坐标轴矢量
自然运动矢量实现了轴平动与转动的统一表达。将由自然坐标轴矢量及关节确定的矢量,例如
定义9关节空间:以关节自然坐标ql表示的空间称为关节空间。
定义10位形空间:称表达位置及姿态(简称位姿)的笛卡尔空间为位形空间,是双矢量空间或6d空间。
定义11自然关节空间:以自然坐标系为参考,通过关节变量
给定多轴系统d={t,a,b,k,f,nt},在系统零位时,只要建立底座系或惯性系,以及各轴上的参考点ol,其它杆件坐标系也自然确定。本质上,只需要确定底座系或惯性系。
给定一个由运动副连接的具有闭链的结构简图,可以选定回路中任一个运动副,将组成该运动副的定子与动子分割开来;从而,获得一个无回路的树型结构,称之为span树。t表示带方向的span树,以描述树链运动的拓扑关系。
i为结构参数;a为轴序列,f为杆件参考系序列,b为杆件体序列,k为运动副类型序列,nt为约束轴的序列即非树。
定义以下表达式或表达形式:
轴与杆件具有一一对应性;轴间的属性量
约定:
本申请中约定:在运动链符号演算系统中,具有偏序的属性变量或常量,在名称上包含表示偏序的指标;要么包含左上角及右下角指标,要么包含右上角及右下角指标;它们的方向总是由左上角指标至右下角指标,或由右上角指标至右下角指标,本申请中为叙述简便,有时省略方向的描述,即使省略,本领域技术人员通过符号表达式也可以知道,本申请中采用的各参数,对于某种属性符,它们的方向总是由偏序指标的左上角指标至右下角指标,或由右上角指标至右下角指标。例如:
本申请的符号规范与约定是根据运动链的偏序性、链节是运动链的基本单位这两个原则确定的,反映了运动链的本质特征。链指标表示的是连接关系,右上指标表征参考系。采用这种符号表达简洁、准确,便于交流与书面表达。同时,它们是结构化的符号系统,包含了组成各属性量的要素及关系,便于计算机处理,为计算机自动建模奠定基础。指标的含义需要通过属性符的背景即上下文进行理解;比如:若属性符是平动类型的,则左上角指标表示坐标系的原点及方向;若属性符是转动类型的,则左上角指标表示坐标系的方向。
(1)ls-杆件l中的点s;而s表示空间中的一点s。
(2)
krl-
(3)
(4)
krs-
(5)
(6)
(7)左下角指标为0时,表示机械零位;如:
(8)0-三维零矩阵;03=[000]t;1-三维单位矩阵;
(9)约定:“\”表示续行符;
幂符
叉乘符运算的优先级高于投影符
(10)单位矢量在大地坐标系的投影矢量
(11)
(12)iql,相对绝对空间的旋转变换阵;
(13)以自然坐标轴矢量为参考方向,相对系统零位的角位置或线位置,记为ql,称为自然坐标;关节变量
(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的运动链;
给定运动链
1.建立多轴系统的拉格朗日方程
应用链符号系统建立关节空间的拉格朗日方程,考虑质点动力学系统d={a,k,t,nt,f,b},首先根据牛顿力学推导自由质点
保守力
得
式(2)应用系统的能量及广义坐标建立系统的方程。关节变量
保守力与惯性力具有相反的链序。拉格朗日系统内的约束既可以是质点间的固结约束,又可以是质点系统间的运动约束;刚体自身是质点系统
给定刚体多轴系统d={a,k,t,nt,f,b},惯性空间记为i,
【1】系统能量
动力学系统d能量
其中:
【2】多轴系统拉格朗日方程
由式(2)得多轴系统拉格朗日方程,
式(6)为轴u的控制方程,即在轴不变量
2.建立ju-kane动力学预备方程:
基于多轴系统拉格朗日方程(6)推导居―凯恩(ju-kane)动力学预备定理。先进行拉格朗日方程与凯恩方程的等价性证明;然后,计算能量对关节速度及坐标的偏速度,再对时间求导,最后给出ju-kane动力学预备定理。
【1】拉格朗日方程与凯恩方程的等价性证明
证明:考虑刚体k平动动能对
考虑刚体k转动动能对
证毕。
因
动力学系统d的平动动能及转动动能分别表示为
考虑式(4)及式(5),即有
式(7)及式(8)是居―凯恩动力学预备定理证明的依据,即居―凯恩动力学预备定理本质上与拉格朗日法是等价的。同时,式(8)右侧包含了多轴系统凯恩方程;表明拉格朗日法与凯恩法的惯性力计算是一致的,即拉格朗日法与凯恩法也是等价的。式(8)表明:在拉格朗日方程(4)中存在
【2】能量对关节速度及坐标的偏速度
【2-1】若
【2-2】若
至此,已完成能量对关节速度及坐标的偏速度计算。
【3】求对时间的导数
【3-1】若
【3-2】若
至此,已完成对时间t的求导。
【4】ju-kane动力学预备定理
将式(11)、式(14)、式(15)及式(16)代入式(8),
给定多轴刚体系统d={a,k,t,nt,f,b},惯性系记为f[i],
式(17)具有了树链拓扑结构。ki表示杆k质心i。因闭子树ul中的广义力具有可加性;因此闭子树的节点有唯一一条至根的运动链,因此运动链iln可以被运动链ul替换。
下面,针对ju-kane动力学预备方程,解决式(17)右侧dfk及dtk的计算问题,从而建立树链刚体系统ju-kane动力学方程。
3.建立树链刚体系统ju-kane动力学模型
给定轴链
对给定轴链
左序叉乘与转置的关系为:
根据运动学迭代式,有:
3.1外力反向迭代
给定由环境i中施力点is至轴l上点ls的双边外力
其中:
【1】若k∈ill,则有
即
式(26)中
【2】若k∈ill,则有
即有
式(26)及式(27)表明环境作用于轴k的合外力或力矩等价于闭子树kl对轴k的合外力或力矩,将式(26)及式(27)合写为
至此,解决了外力反向迭代的计算问题。在式(28)中,闭子树对轴k的广义力具有可加性;力的作用具有双重效应,且是反向迭代的。所谓反向迭代是指:
3.2共轴驱动力反向迭代
若轴l是驱动轴,轴l的驱动力及驱动力矩分别为
【1】由式(18)、式(19)及式(29)得
即
若轴u与轴
因
【2】由式(19)、式(18)及式(29)得
即
若轴u与
至此,完成了共轴驱动力反向迭代计算问题。
3.3树链刚体系统ju-kane动力学显式模型的建立:
下面,先陈述树链刚体系统ju-kane动力学方程,简称ju-kane方程;然后,给出建立步骤。
给定多轴刚体系统d={a,k,t,nt,f,b},惯性系记为f[i],
其中:[·]表示取行或列;
其中,记
记
上述方程的建立步骤为:
记
ex的能量为
由式(26)、式(27)、式(31)、式(33)及式(41)得式(40)。
将偏速度计算公式式(19),式(18)及式(20)代入ju-kane动力学预备方程(17)得
由式(21)得
考虑式(43),则有
同样,考虑式(43),得
将式(43)至式(45)代入式(42)得式(34)至式(39)。
实施例1
给定如图3所示的通用3r机械臂,a=(i,1:3];应用本发明的方法建立树链ju-kane动力学方程,并得到广义惯性矩阵。
步骤1建立基于轴不变量的迭代式运动方程。
由式(46)基于轴不变量的转动变换矩阵
得
运动学迭代式:
二阶张量投影式:
由式(48)及式(47)得
由式(49),式(47)及式(55)得
由式(50)及式(55)得
由式(51)、式(55)及式(57)得
由式(52)及式(55)得
由式(53)及式(55)得
步骤2建立动力学方程。先建立第1轴的动力学方程。由式(37)得
由式(39)得
由式(61)及式(62)得第1轴的动力学方程,
建立第2轴的动力学方程。由式(37)得
由式(39)得
由式(64)及式(65)得第2轴的动力学方程,
最后,建立第3轴的动力学方程。由式(37)得
由式(39)得
由式(67)及式(68)得第3轴的动力学方程,
由式(61),式(63)及式(67)得广义质量阵。
由此可知,只要程式化地将系统的拓扑、结构参数、质惯量等参数代入式(36)至式(40)就可以完成动力学建模。通过编程,很容易实现ju-kane动力学方程。因后续的树链ju-kane规范方程是以ju-kane动力学方程推导的,树链ju-kane动力学方程的有效性可由ju-kane规范型实例证明。
3.4树链刚体系统ju-kane动力学规范型
在建立系统动力学方程后,紧接着就是方程求解的问题。在动力学系统仿真时,通常给定环境作用的广义力及驱动轴的广义驱动力,需要求解动力学系统的加速度;这是动力学方程求解的正问题。在求解前,首先需要得到式(71)所示的规范方程。
规范化动力学方程,
其中:rhs–右手侧(righthandside)
显然,规范化过程就是将所有关节加速度项进行合并的过程;从而,得到关节加速度的系数。将该问题分解为运动链的规范型及闭子树的规范型两个子问题。
3.4.1运动链的规范型方程
将式(36)及式(37)中关节加速度项的前向迭代过程转化为反向求和过程,以便后续应用;显然,其中含有6种不同类型的加速度项,分别予以处理。
【1】给定运动链
上式的推导步骤为:
【2】给定运动链
上式的推导步骤为:因
【3】给定运动链
上式可由下式而得,因
【4】给定运动链
上式的推导步骤为:考虑
【5】给定运动链
上式的推导步骤为:考虑
【6】给定运动链
上式的推导步骤为:因
3.4.2闭子树的规范型方程
因闭子树ul中的广义力具有可加性;因此闭子树的节点有唯一一条至根的运动链,式(73)至式(77)的运动链iln可以被ul替换。由式(73)得
由式(74)得
由式(75)得
由式(76)得
由式(77)得
至此,已具备建立规范型的前提条件。
3.5树链刚体系统ju-kane动力学规范方程
下面,建立树结构刚体系统的ju-kane规范化动力学方程。为表达方便,首先定义
然后,应用式(78)至式(82),将式(36)及式(37)表达为规范型。
【1】式(36)的规范型为
上式的具体建立步骤为:由式(24)及式(36)得
由式(52)及式(85)得
将式(80)代入式(85)右侧前一项得
将式(79)代入式(86)右侧后一项得
将式(87)及式(88)代入式(86)得
对于刚体k,有
上式的具体建立步骤为:由式(37)得
将式(78)代入式右侧前一项(91)得
将式(81)代入式(91)右侧后一项得
将式(82)代入式(91)右侧中间一项得
将式(92),式(93)及式(94)代入式(92)得
对于刚体k,有
【3】应用式(84)及式(90),将ju-kane方程重新表述为如下树链ju-kane规范型方程:
给定多轴刚体系统d={a,k,t,nt,f,b},惯性系记为f[i],
其中:
式中,ki表示杆k质心i;轴k的质量及质心转动惯量分别记为mk及
4.闭链刚体系统的ju-kane动力学方程建立
下面,先陈述闭链刚体系统的居―凯恩(简称ju-kane)动力学方程;然后,给出具体建模过程。
给定多轴刚体系统d={a,k,t,nt,f,b},惯性系记为f[i],
【1】轴u及轴u′的ju-kane动力学规范方程分别为
【2】非树约束副uku′的约束代数方程为
其中:
式中:
具体建模过程如下:
非树约束副
由式(114)得
轴u对轴u′在约束轴方向上的广义约束力
由式(115)及式(116)得
由式(115)得
δ表示增量;
由式(18)及式(118)得
故有
由式(110)及式(122)得式(105)。由式(19)及式(119)得
由式(111)及式(123)得式(106)。由式(19)及式(120)得
由式(112)及式(124)得式(107)。由式(19)及式(121)得
由式(113)及式(125)得(108)。由式(18),式(116)及式(110)得
广义约束力
根据轴u的ju-kane动力学规范方程得式(103)及式(104)。
以关节空间自然轴链为基础的ju-kane闭链刚体动力学克服了笛卡尔坐标轴链空间的局限:
【1】在基于笛卡尔坐标轴链的牛顿欧拉动力学中,非树运动副uku′∈p约束不能表达
【2】在基于笛卡尔坐标轴链的牛顿欧拉动力学当中,非树运动副代数约束方程是6d的;而式(105)至式(108)表示是3d非树运动副代数约束方程,从而降低了系统方程求解的复杂度;
【3】在基于笛卡尔坐标轴链的牛顿欧拉动力学当中,非树运动副代数约束方程是关于6d矢量空间绝对加速度的,是关于关节坐标、关节速度的迭代式,具有累积误差;而式(105)至式(108)是关于关节加速度的,保证了约束方程的准确性。
5.基于轴不变量的约束力求解
对于无功率损耗的运动轴u,记其约束力及约束力矩矢量分别为
由式(96)及式(139)计算得
若
记
其中:
在完成前向动力学正解后,根据已计算的关节加速度
由式(130)可以得到关节约束力大小
若记运动轴径向力大小为
至此,完成了轴径向约束广义力的计算。
树链刚体系统对应的关节加速度序列记
将根据运动轴类型及自然参考轴表达的刚体运动链广义惯性矩阵称为轴链刚体广义惯性矩阵,简称轴链广义惯性矩阵。
定义正交补矩阵
给定多轴刚体系统d={a,k,t,nt,f,b},
由式(136)得
其中,
由式(136)得
6.广义内摩擦力及粘滞力计算
在完成轴径向约束广义力的计算后,得到运动轴u的径向约束力大小
故有
其中:sk[u]─运动轴u的内摩擦系数,ck[u]─运动轴u的粘滞系数;sign()表示取正或负符号。
记广义内摩擦力及粘滞力的合力及合力矩分别为
运动轴的广义内摩擦力及粘滞力是运动轴的内力,因为它们仅存在于运动轴向上,与轴径向约束力总是正交的。当运动轴轴向动态作用力平衡时,无论广义内摩擦力及粘滞力是否存在或大小如何,都不影响动力学系统的运动状态;故而,不影响运动轴的径向约束力。因此,由式(130)至式(134)计算运动轴u的径向约束力大小
7.建立闭链刚体非理想约束系统的ju-kane动力学显式模型
设运动轴u的广义内摩擦及粘滞的合力及合力矩分别为
【1】轴u及轴u′的ju-kane动力学规范方程分别为
【2】非树约束副uku′的约束代数方程为
其它,参见式(103)至式(113)、式(97)至式(102)。
建立过程为:
运动轴u的内摩擦及粘滞合力