本发明涉及一种多轴机器人正运动学计算方法,属于机器人技术领域。
背景技术
机器人是目前非常热门的领域。这个领域过去几十年已经投入了大量的科学与工程人力,而且研究了多年。然而,一旦遇到轴的数目与自由度增加到一定数量的时候,按照现有的教科书以及已知的观察、建模、计算与控制方式,往往会陷入复杂失控,甚至无法求解的难题。
首先,过去的做法缺少一般化的能力。对于不同的机器人往往需要重新研究,建立对应的运动学与力学模型。
其次,过去的做法在建模的过程,所用的图示以及语言通常是不精确也不完整的。这导致许多的参数在建模的初期并没有被考虑进去。后续的整个建模,包括程序化代码的编写时,就必须个别考虑跟解决之前没有考虑的参数与细节。这对于复杂系统,例如自由度更高的机器人应用时,往往意味着大量的隐藏臭虫(bug)会藏在整个建模的系统里头。这会影响整个系统开发的效率,并且透过这种没有完整考量下开发出来的系统,往往有很多稳定性问题难以解决。
此外,过去的做法在遇到复杂度比较高的时候,运算量会大幅增加或甚至找不出解答,以及计算精度大受影响。换言之,对于需要即时运算控制以达成自主控制的机器人来说,就成了重大的缺陷。
因此,虽然现在已经有很多的机器人相关的理论,但是却仍然缺少一个完整有效的设计框架与对应的运算与控制方法,可以在各种不同机器人实际开发过程中,方方面面地解决建模,模型内的运算结构与规则,到正运动学,逆运动学,以及力学计算的相关问题。
技术实现要素:
本发明所要解决的技术问题是提供一种基于轴不变量的多轴机器人正运动学计算方法。
为解决上述技术问题,本发明采用以下技术方案:
一种基于轴不变量的多轴机器人正运动学计算方法,其特征是,
多轴机器人装置包含杆件序列与关节序列,将树链中的关节序列转换成对应的轴序列及其父轴序列,所述轴序列的轴为平动轴或转动轴;
使用轴集合来对应描述多轴机器装置,以自然坐标系为基础,利用轴集合的轴对应的轴不变量来计算多轴机器装置的控制参数;
利用轴不变量的不变性建立基于轴不变量的迭代式运动学方程,并且所述迭代式运动学方程的符号对应到伪代码,反映所述多轴机器装置运动链的拓扑关系与链序关系;
计算运动链的迭代式正运动学数值;
计算基于轴不变量的迭代式偏速度。
惯性空间记为i,给定由i至杆件n的运动链iln,杆件l,n,j∈a,n>l,s是体l上的任一点,a为轴序列;当转动矢量
【1】链节
运动副
其中:
【2】运动链iln的位形计算步骤;
【3】运动链iln的速度及加速度计算步骤。
链节
【1-1】已知转动矢量
【1-2】由欧拉四元数计算旋转变换阵
【1-3】由下式计算链节速度:
式中,运动副
【1-4】由下式计算链节加速度:
式中,转动加速度
运动链iln的位形计算步骤为:
【2-1】由欧拉四元数的链关系,将四元数乘法运算用其共轭矩阵运算替代,计算欧拉四元数序列
【2-2】用欧拉四元数表示定轴转动;旋转变换阵的计算等价于链式四元数的矩阵计算,计算旋转变换阵序列{iqj|j∈a};
【2-3】由下式计算位置矢量
式中,轴矢量
运动链iln的速度及加速度计算步骤为:
【3-1】计算绝对角速度:
式中,角速度
【3-2】计算绝对角加速度:
式中,角加速度
【3-3】计算绝对平动速度:
式中,线速度
【3-4】计算绝对平动加速度:
式中,线速度
其中:
基于轴不变量的迭代式偏速度计算步骤包括:
定义使能函数:
式中,k为属于运动链iln的杆件;
由使能函数,
【1】计算绝对角速度对关节角速度的偏速度;
【2】计算绝对平动速度矢量对关节平动速度的偏速度;
【3】计算绝对转动矢量对关节角度的偏速度;
【4】计算绝对位置矢量对关节位移的偏速度;
【5】计算绝对位置矢量对关节角度的偏速度;
【6】计算绝对平动速度矢量对关节角速度的偏速度。
绝对角速度对关节角速度的偏速度计算公式为:
式中,角速度
绝对平动速度矢量对关节平动速度的偏速度计算公式为:
式中,线速度
绝对平动速度矢量对关节角速度的偏速度计算公式为:
式中,角速度
绝对转动矢量对关节角度的偏速度计算公式为:
式中,角位置
绝对位置矢量对关节位移的偏速度计算公式为:
式中,线位置
绝对位置矢量对关节角度的偏速度计算公式为:
式中,角位置
本发明所达到的有益效果:
本发明的方法提出并证明了基于轴不变量的迭代式运动学实时数值建模方法,包含:基于轴不变量的迭代式的位置矢量、转动矢量、速度矢量、加速度矢量及偏速度矢量的计算方法。具有简洁的运动链符号系统,具有伪代码的功能,具有迭代式结构,保证了系统实现的可靠性及机械化演算;具有基于轴不变量的迭代式,保证计算的实时性;实现坐标系、极性及系统结构参量的完全参数化建模,保证模型的通用性,避免了与系统接口与用户接口的转换,通过轴不变量构建了内在紧凑系统,提高了运动学计算的实时性及功能复用性能;轴运动矢量的统一表达及简洁的结构化层次模型不仅有助于简化多轴系统运动学建模过程,而且为基于轴不变量的多轴系统动力学建模奠定了基础。
附图说明
图1自然坐标系与轴链;
图2固定轴不变量;
图3偏速度的含义示意图。
具体实施方式
下面结合附图对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案,而不能以此来限制本发明的保护范围。
定义1自然坐标轴:称与运动轴或测量轴共轴的,具有固定原点的单位参考轴为自然坐标轴,亦称为自然参考轴。
定义2自然坐标系:如图1所示,若多轴系统d处于零位,所有笛卡尔体坐标系方向一致,且体坐标系原点位于运动轴的轴线上,则该坐标系统为自然坐标系统,简称自然坐标系。
自然坐标系优点在于:(1)坐标系统易确定;(2)零位时的关节变量为零;(3)零位时的系统姿态一致;(4)不易引入测量累积误差。
由定义2可知,在系统处于零位时,所有杆件的自然坐标系与底座或世界系的方向一致。系统处于零位即
由上式知,
轴不变量与坐标轴具有本质区别:
(1)坐标轴是具有零位及单位刻度的参考方向,可以描述沿该方向平动的位置,但不能完整描述绕该方向的转动角度,因为坐标轴自身不具有径向参考方向,即不存在表征转动的零位。在实际应用时,需要补充该轴的径向参考。例如:在笛卡尔系f[l]中,绕lx转动,需以ly或lz为参考零位。坐标轴自身是1d的,3个正交的1d参考轴构成3d的笛卡尔标架。
(2)轴不变量是3d的空间单位参考轴,其自身就是一个标架。其自身具有径向参考轴,即参考零位。空间坐标轴及其自身的径向参考轴可以确定笛卡尔标架。空间坐标轴可以反映运动轴及测量轴的三个基本参考属性。
已有文献将无链指标的轴矢量记为
【1】给定旋转变换阵
【2】是3d参考轴,不仅具有轴向参考方向,而且具有径向参考零位,将在3.3.1节予以阐述。
【3】在自然坐标系下:
对轴不变量而言,其绝对导数就是其相对导数。因轴不变量是具有不变性的自然参考轴,故其绝对导数恒为零矢量。因此,轴不变量具有对时间微分的不变性。有:
【4】在自然坐标系统中,通过轴矢量
【5】应用轴矢量
因基矢量el是与f[l]固结的任一矢量,基矢量
在系统处于零位时,以自然坐标系为参考,测量得到坐标轴矢量
因此,应用自然坐标系统,当系统处于零位时,只需确定一个公共的参考系,而不必为系统中每一杆件确定各自的体坐标系,因为它们由轴不变量及自然坐标唯一确定。当进行系统分析时,除底座系外,与杆件固结的其它自然坐标系只发生在概念上,而与实际的测量无关。自然坐标系统对于多轴系统(mas)理论分析及工程作用在于:
(1)系统的结构参数测量需要以统一的参考系测量;否则,不仅工程测量过程烦琐,而且引入不同的体系会引入更大的测量误差。
(2)应用自然坐标系统,除根杆件外,其它杆件的自然坐标系统由结构参量及关节变量自然确定,有助于mas系统的运动学与动力学分析。
(3)在工程上,可以应用激光跟踪仪等光学测量设备,实现对固定轴不变量的精确测量。
(4)由于运动副r及p、螺旋副h、接触副o是圆柱副c的特例,可以应用圆柱副简化mas运动学及动力学分析。
定义3不变量:称不依赖于一组坐标系进行度量的量为不变量。
定义4转动坐标矢量:绕坐标轴矢量
定义5平动坐标矢量:沿坐标轴矢量
定义6自然坐标:以自然坐标轴矢量为参考方向,相对系统零位的角位置或线位置,记为ql,称为自然坐标;称与自然坐标一一映射的量为关节变量;其中:
定义7机械零位:对于运动副
故关节
定义8自然运动矢量:将由自然坐标轴矢量
自然运动矢量实现了轴平动与转动的统一表达。将由自然坐标轴矢量及关节确定的矢量,例如
定义9关节空间:以关节自然坐标ql表示的空间称为关节空间。
定义10位形空间:称表达位置及姿态(简称位姿)的笛卡尔空间为位形空间,是双矢量空间或6d空间。
定义11自然关节空间:以自然坐标系为参考,通过关节变量
如图2所示,给定链节
轴矢量
定义12自然坐标轴空间:以固定轴不变量作为自然参考轴,以对应的自然坐标表示的空间称为自然坐标轴空间,简称自然轴空间。它是具有1个自由度的3d空间。
如图2所示,
给定多轴系统d={t,a,b,k,f,nt},在系统零位时,只要建立底座系或惯性系,以及各轴上的参考点ol,其它杆件坐标系也自然确定。本质上,只需要确定底座系或惯性系。
给定一个由运动副连接的具有闭链的结构简图,可以选定回路中任一个运动副,将组成该运动副的定子与动子分割开来;从而,获得一个无回路的树型结构,称之为span树。t表示带方向的span树,以描述树链运动的拓扑关系。
i为结构参数;a为轴序列,f为杆件参考系序列,b为杆件体序列,k为运动副类型序列,nt为约束轴的序列即非树。
描述运动链的基本拓扑符号及操作是构成运动链拓扑符号系统的基础,定义如下:
【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中,若
定义以下表达式或表达形式:
轴与杆件具有一一对应性;轴间的属性量
约定:“□”表示属性占位;若属性p或p是关于位置的,则
本申请中约定:在运动链符号演算系统中,具有偏序的属性变量或常量,在名称上包含表示偏序的指标;要么包含左上角及右下角指标,要么包含右上角及右下角指标;它们的方向总是由左上角指标至右下角指标,或由右上角指标至右下角指标,本申请中为叙述简便,有时省略方向的描述,即使省略,本领域技术人员通过符号表达式也可以知道,本申请中采用的各参数,对于某种属性符,它们的方向总是由偏序指标的左上角指标至右下角指标,或由右上角指标至右下角指标。例如:
本申请的符号规范与约定是根据运动链的偏序性、链节是运动链的基本单位这两个原则确定的,反映了运动链的本质特征。链指标表示的是连接关系,右上指标表征参考系。采用这种符号表达简洁、准确,便于交流与书面表达。同时,它们是结构化的符号系统,包含了组成各属性量的要素及关系,便于计算机处理,为计算机自动建模奠定基础。指标的含义需要通过属性符的背景即上下文进行理解;比如:若属性符是平动类型的,则左上角指标表示坐标系的原点及方向;若属性符是转动类型的,则左上角指标表示坐标系的方向。
(1)ls-杆件l中的点s;而s表示空间中的一点s。
(2)
(3)
(4)
(5)
(6)
(7)左下角指标为0时,表示机械零位;如:
(8)0-三维零矩阵;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的运动链;
给定运动链
(16)rodrigues四元数表达形式:
欧拉四元数表达形式:
不变量的四元数(也称为轴四元数)表达形式
运动链是一个偏序的链;而运动副
显然,全序及偏序是一个对象自身的属性。而力学及机器人理论上尚未出现相应的符号系统。
借鉴集合论的链理论,将运动副
其中:
在span树中,简单运动链
故有
因有序集合的子集也是有序的集合,故定义由
记
同样,因有序集合的子集也是有序的集合,故有
ili=(i,i],|ili|=0。(7)
称ili为空链或平凡链。惯性空间(环境)记为i,平凡链ili总是存在的。
1.基于轴不变量的迭代式运动学计算方法
给定运动链iln,轴l,n∈a,n>l,s是体l上的任一点,a为轴序列。当转动矢量lφl有测量噪声时,运动链iln的迭代式正运动学数值计算步骤为:
【1】链节
【1-1】已知转动矢量
定义四元数
四元数
因4d复数的矢部参考基是唯一的自然参考基,故四维复数的左上角的参考指标仅表明运动关系,已失去投影参考系的含义,具有不同左上角指标的4d复数可以进行代数运算。尽管参考指标在4d复数中无意义,但不表明指标关系无意义,因为复数的乘除运算与复数的作用顺序密切相关。
【1-2】由式(10)计算旋转变换阵
显然,有
【1-3】由式(12)计算链节速度:
运动副
【1-4】由式(13)计算链节加速度:
转动加速度
【2】运动链iln的位形计算步骤
【2-1】由式(19)计算欧拉四元数序列
由欧拉四元数的链关系,四元数
其中:
且有
当给定角度
由式(15)及式(16),结合欧拉四元数,得
式(14)应用计算机编程实现时,可用下式替代。
式(18)仅包含16个乘法运算及12个加法运算。而
由式(18)得
式(14)表示的是位置矢量转动算子,即表示的是转动。因此,欧拉四元数乘积运算对应旋转变换阵的乘积运算。因此旋转变换链等价于定轴转动链,即
由上可知,欧拉四元数可以唯一确定旋转变换阵;旋转变换阵也可以唯一确定欧拉四元数,即欧拉四元数与旋转变换阵等价。转动矢量与规范四元数一一对应,即四元数表示定轴转动;旋转变换阵的计算等价于链式四元数的矩阵计算。
【2-2】因式(10)较式(21)计算复杂度高,故由式(21)计算旋转变换阵序列{iqj|j∈a};
式(21)是关于
【2-3】由式(22)计算位置矢量
【3】运动链iln的速度及加速度步骤
【3-1】由式(23)计算绝对角速度
上式可由式(24)
得
绝对导数
【3-2】由式(25)计算绝对角加速度
上式可由式(26)
得
转动加速度
【3-3】由式(27)计算绝对平动速度
上式可由式(28)求得,
称式(28)为“正序的绝对求导式”,牵连项
【3-4】由式(29)计算绝对平动加速度
上式可由式(30)得
其中:
由式(30)可知,平动加速度
2.基于轴不变量的偏速度计算方法
现有技术中通常采用雅克比矩阵的计算方法,但均未对结论进行证明且结论不全面。在运动学及动力学分析时,将雅克比矩阵称为偏速度更合适。因为雅克比矩阵泛指偏导数,不一定具有可加性;而在运动学及动力学中偏速度特指矢量对关节变量的偏导数,具有可加性。偏速度是对应速度的变换矩阵,是对单位方向矢量的矢量投影。在运动学分析及动力学分析中,偏速度起着关键性的作用,偏速度的计算是动力学系统演算的基本前提。
首先,定义使能(enable)函数,
式中,k为属于运动链ill的杆件;
式(31)的特殊形式为
下面,说明基于轴不变量的迭代式偏速度计算步骤:
【1】根据式(33)计算绝对角速度对关节角速度的偏速度,
上式可由式(23)得
【2】根据式(34)计算绝对平动速度矢量对关节平动速度的偏速度,
即
【3】根据式(35)计算绝对转动矢量对关节角度的偏速度,
上式可由式(23)得
【4】根据式(36)计算绝对位置矢量对关节位移的偏速度,
上式可由式(22)得
【5】根据式(37)计算绝对位置矢量对关节角度的偏速度,
上式可由式(27)得
即
故有
【6】根据式(38)计算绝对平动速度矢量对关节角速度的偏速度,
上式可由式(27)得
将上述结论,以专利中对应的式(39)、(40)、(41)统一表述,称之为偏速度定理。
若给定运动链运动链iln,则有
当
由式(37)及(38)得式(40)。因
式(35)至式(38)对运动学及动力学分析具有非常重要的作用。它们不仅物理意义清晰,还可以简化运动学及动力学方程的表达。
如图3所示,一方面,从几何角度看,式(39)中的偏速度即为对应的轴不变量,式(40)表示的是位置矢量对轴不变量的一阶距,即轴矢量
由式(42)左序叉乘与转置的关系式
可知
式(43)表明:
式(43)中
3.轴不变量对时间微分的不变性
由式(24)及式(26)可知
故有
式(45)表明:对轴不变量而言,其绝对导数就是其相对导数。因轴不变量是具有不变性的自然参考轴,故其绝对导数恒为零矢量。因此,轴不变量具有对时间微分的不变性。
由式(39)及式(45)得
由式(28)及式(45)得
由上式得
即
由式(47)可知:偏速度对时间t的导数仍是轴不变量的迭代式。轴不变量
式(48)中左式表示:转动链ilc的dcm对该链全部关节角的偏速度之和;式(48)中右式表示:转动链ilc的轴不变量之和。因此,运动链的dcm对关节角的偏速度具有不变性。
对于mas系统的树
表明:mas系统的轴不变量对时间是不变的,即刚体系统的自然参考轴具有不变性。由式(48)可知:系统的关节变量与自然参考轴一一映射,体的关节变量数由其独立的运动维度确定,但不改变自然参考轴对时间微分的不变性。
4.树形运动链的变分计算步骤
将函数自变量的导数称为微商,以d表示。与微分相对应,将自变量函数的增量称为变分,以δ表示;但变分不考虑时间t的增量δt,即δt≡0。正是因为不考虑时间增量δt,故线位移及角位移的变分理解为同一时刻t可能的运动量变化,即虚位移。
【1】转动矢量的变分
上式可由式(33)得
【2】平动矢量的变分
上式可由式(37)及式(38)得
以上所述仅是本发明的优选实施方式,应当指出,对于本技术领域的普通技术人员来说,在不脱离本发明技术原理的前提下,还可以做出若干改进和变形,这些改进和变形也应视为本发明的保护范围。