发明内容
有鉴于此,本申请提供了一般三支链全参数标定方法,可以提高一般三支链并联机构的精度。
本发明提供的技术方案如下:
一般三支链并联机构全参数标定方法,用于标定一般三支链并联机构的全部结构参数,所述一般三支链并联机构包括静平台、动平台和支链,所述动平台上设置有靶标,该方法包括:
建立所述一般三支链并联机构的数学模型;
建立所述数学模型的全参数雅可比矩阵,确定所述并联接机构的全部结构参数与误差的映射关系;
对所述动平台上的靶标进行测量,获取所述动平台的任意位姿参数;
根据所述任意位姿参数利用辨识算法对所述全参数雅可比矩阵进行辨识,以对所述一般三支链并联机构的全部结构参数进行标定;
根据标定后的全部结构参数对所述一般三支链并联机构进行误差补偿。
进一步的,所述一般三支链并联机构包括多个支链、动平台和静平台,多个所述支链的一端与所述动平台相连接,多个所述支链的另一端与静平台相连接,所述建立所述一般三支链并联机构的数学模型的步骤包括:
建立每个所述支链的数学模型,公式为:
其中,dj2表示是第j个支链上,沿第2个坐标系Z方向的平移,是主动驱动变量,θj1,θj2,θj3,θj4为被动转动参数,其他结构参数为常数,B表示静平台,P表示动平台。
进一步的,所述支链通过球铰与所述动平台相连接,所述球铰包括铰链座,所述建立所述一般三支链并联机构的数学模型的步骤还包括:
建立所述支链的基坐标0系相对于静平台B系的齐次变换矩阵,公式为:
其中,B0x,B0y,θ0,α0四个参数表示所述铰链座相对于所述静平台的位姿关系;
建立支链4系相对于所述动平台P系的齐次变换矩阵,计算公式为:
其中,所述rp,α5,θ5三个参数表示动平台铰链座相对于静平台铰链座的位姿关系。
进一步的,所述建立所述一般三支链并联机构的数学模型的步骤还包括:
建立所述支链的DH矩阵模型,公式为:
其中,i=1,2,3,4。
进一步的,所述铰链座包括静平台铰链座和动平台铰链座,所述建立所述数学模型的全参数雅可比矩阵的步骤包括:
建立标准DH参数误差模型;
建立所述静平台铰链座的误差模型;
建立所述动平台铰链座的误差模型;
建立所述支链的误差映射;
建立所述一般三支链并联机构全参数雅可比矩阵。
进一步的,所述建立标准DH参数误差模型的步骤包括:
将所述支链模型的DH矩阵分为横轴和立轴方向上的两个螺旋运动,公式为:
Tiz=Transz(si)Rotz(θi),Tix=Transx(ai)Rotx(αi);
对螺旋运动公式分别进行全微分,公式为:
其中,i=1,2,3,4,dTiz=TizΔTiz,dTix=TixΔTix;
根据全微分后的螺旋运动公式计算所述支链模型在横轴和立轴方向上的位姿误差参数,公式为:
其中,i=1,2,3,4;
将所述支链模型在横轴和立轴方向上的位姿误差参数公式中的旋量用六维矢量表达,公式为:
其中,ddi,dθi,dai,dαi为第i个所述关节上的DH四参数的误差参数。
进一步的,所述建立所述静平台铰链座的误差模型的公式为:
其中,
进一步的,所述建立所述动平台铰链座的误差模型的公式为:
其中,
进一步的,所述建立所述支链的误差映射的步骤包括:
建立所述支链的误差映射公式,公式为:
将所述误差映射公式转换为:
其中,J是一般三支链并联机构全误差参数的雅可比矩阵,dE=[de1,de2,de3]T是去掉被动变量的所有参数,是所述动平台的六维旋量误差。
进一步的,所述根据所述任意位姿参数利用辨识算法对所述全参数雅可比矩阵进行辨识的步骤包括:
选用线性最小二乘法、非线性最小二乘法或LM算法对所述进行系统参数辨识。
本申请可以实现对一般三支链并联机构全部结构参数的标定,标定方法定位精度提高效果明显。由于一般三支链并联机构的组成零部件非常多,如果提高每个零部件的设计及制造精度,不仅成本高,而且在工程上不可能达到的。通过对参数进行标定的方法可以减少系统误差,提高一般三支链并联机构的定位精度。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚,下面将结合附图,对本发明实施例中的技术方案进行清楚、完整的描述。显然,所描述的实施例是本发明的一部件实施例,而不是全部的实施例。
因此,以下对本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围,而是仅仅表示本发明的部件实施例。基于本发明中的实施例,本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
需要说明的是,在不冲突的情况下,本发明中的实施例及实施例中的特征可以相互组合。
应注意到:相似的标号和字母在下面的附图中表示类似项,因此,一旦某一项在一个附图中被定义,则在随后的附图中不需要对其进行进一步定义和解释。
目前,一般三支链并联机构的精度问题可以有两个方法进行解决,一种方法是提高设计和制造的精度。另一种方法是标定技术。提高设计和制造的精度成本很高,实际工程上还是做不到零公差。使用标定技术进行机械零部件设计时,仅考虑如间隙和变形等因素,避免产生随机误差。由于常数结构参数误差仅产生系统误差,需要按照理论尺寸和合理公差进行设计制造即可,大大减低了制造成本。待装配好后进行标定,减少系统误差到允许范围即可。通过标定提高一般三支链并联机构定位精度的方法成本低且可行。
本申请实施例提供了一般三支链并联机构全参数标定方法,不仅可以实现空间三自由度一般三支链并联机构的参数标定问题,还可以推广解决一般的三支链并联机构的参数标定问题。
如图1所示,该方法包括:
步骤101,建立一般三支链并联机构的数学模型。
为描述方便,本申请实施例中的并联机构采用一般三支链并联机构进行描述,但本申请实施例中的计算方法也可以应用至其他形式的并联结构。一般3RPS并联机构相当于三个五自由度一般串联机构,由动平台将它们的末端连接在一起,这里“一般”的意思是一般三支链并联机构的结构参数是任意的,是通用的意思。
3-RPS机构是由上、下平台皆为等边三角形的动平台和静平台以及连接上下两个平台的3个支链构成。其中3个支链与动平台相连的运动副为球铰副,与静平台相连的为转动副。在球铰副与转动副之间的为移动副。固定坐标系固连在静平台上,原点在静平台中心处。
在进行全参数标定前,有必要将本申请需要的数学算法进行简单介绍。本申请是标定方法是基于李群李代数、螺旋理论和LM(Levenberg-Marquardt列文伯格-马夸尔特)算法进行的。
刚体运动群SE(3)及其李代数se(3)的计算方法:其中,用SE(3)可以表达刚体的位姿。R∈SO(3)用三阶方向余旋矩阵表达刚体的姿态。用刚体运动群的李代数se(3)表达误差旋量。
上式中,三阶方向余旋矩阵的李代数是反对称矩阵,如下:
由于公式(2)用4阶方阵表示的旋量与公式(4)用6维向量表示的旋量同构,用6维向量表达误差旋量更紧凑。其中,姿态误差矢量ω的量纲是弧度,位置误差v的量纲是米。定义位姿误差评价指标是六维矢量的模||(ωT,vT)||,该数是一个无量纲的量。这个数越小表示位置和姿态的误差都越小。
另外,刚体运动群的伴随变换为
SE(3)在其李代数上的伴随变换,用6维向量形式的表达为下式:
在基坐标系中,实测动平台位姿为Ta,名义动平台位姿为Tn,ΔT=δTT-1∈se(3)表示从Ta到Tn的位姿误差。
n阶螺旋系必然存在一个6-n阶的反旋量系螺旋系和其反螺旋的关系可以看成是齐次线性方程组的零空间映射。本文中,由旋量4系利用QR分解法求解它的反旋量,组合4个运动旋量构成的6*4阶矩阵A。A矩阵的秩为4,对A矩阵进行QR分解,得到正交矩阵Q,Q矩阵的第5列和第6列是反螺旋。
在本申请实施例中,支链的数学模型如下:
在本申请实施例中,一般三支链并联机构的每个支链是一般五自由度串联机构,并且支链共有一个动平台。表示是第j个支链上,沿第2个坐标系Z方向的平移。是主动驱动变量,求运动学正解,即在输入锁定的情况下,可以认为它是常数。是被动转动参数。其它的结构参数均是常数。B表示静平台,P表示动平台。
在本申请实施例中,通过DH(Denavit-Hartenberg迪纳威特-哈坦伯格)算法对一般三支链并联机构进行数学模型的建立。在数学模型的建立过程中,为描述方便仅讨论一个支链的情况,所以可以忽略掉支链编号j。下面分别进行铰链座和支链的DH矩阵的建立。
本申请实施例中的支链分别通过球铰与静平台和动平台相连接,球铰包括铰链座,本申请实施例中可以采用虎克铰。虎克铰铰链座模型建立如下:
其中,B0x,B0y,θ0,α0四个参数表示所述铰链座相对于所述静平台的位姿关系。上述矩阵是支链的基坐标0系相对于定平台B系的齐次变换矩阵为了方便技术,公式中的符号下标一律使用了数字。
同理,还可以得到支链4系相对动平台P系的齐次变换矩阵,如下:
其中,rp,α5,θ5三个参数表示动平台铰链座相对于静平台铰链座的位姿关系。
进一步的,所述建立所述一般三支链并联机构的数学模型的步骤还包括:
建立所述支链的DH矩阵模型,公式为:
其中,i=1,2,3,4。由于B0z就是支链参数s1,所以在静平台铰链座的参数中不包括B0z。以一般三支链并联机构为例,一个支链总共包括23个参数,其中被动驱动参数4个,需要辨识的参数19个。整个机构三个支链,共57个参数。一般3RPS运动学正解问题是现有技术,这里不再赘述。
步骤102,建立所述数学模型的全参数雅可比矩阵,确定所述并联接机构的全部结构参数与误差的映射关系。
如图2所示,具体包括:步骤1021,建立标准DH参数误差模型;步骤1022,建立所述静平台铰链座的误差模型;步骤1023,建立所述动平台铰链座的误差模型;步骤1024,建立所述支链的误差映射;步骤1025,建立所述一般三支链并联机构的全参数雅可比矩阵。
其中,本申请实施例中的一般三支链并联机构中的支链包括四个关节,可以分别用标准DH参数表达。铰链座包括静平台铰链座和动平台铰链座。将公式(11)分为Z轴方向和X轴方向的两个螺旋运动。
具体包括
其中Tiz=Transz(si)Rotz(θi),
Tix=Transx(ai)Rotx(αi)
并分别进行全微分,得到如下公式:
其中,i=1,...,4,由于dTiz=TizΔTiz,dTix=TixΔTix (13)
得到:
公式(14)中旋量用六维矢量表达为:
其中,ddi,dθi,dai,dαi是i关节上的DH参数的误差参数。
进一步的,建立静平台铰链座误差模型,具体包括:
对公式(9)进行全微分再左乘得:
在公式(20)中
更进一步的,建立动平台铰链座误差模型包括:
对公式(10)进行全微分再左乘得到下式:
其中:
另外,还可以建立整条支链的误差映射,包括:
首先将公式(8)式j支链的模型表达为更简捷的形式,具体如下:
在公式(31)中,T1z=T2,以此类推。
得到上式的误差映射为:
略去公式(32)中的高阶无穷小项,得到:
再将dTi=TiΔTi代入(33)式,得到:
令得到:
由公式(6)可得:
将支链中的误差参数写成向量为:
der=[dBx,dBy,drp,dθ0,dθ1,...,dθ4,dθ5,dd1,...,dd4,dα0,...,dα5,da1,...,da4]T。
进一步的,一般三支链并联机构具有三个支链,可以将公式(36)改写成:
其中j=1,2,3,j表示第j个支链。
在公式(36)中,主动驱动锁定状态和其它结构参数一样是常量,被动驱动的值取决于动平台的位姿,从一条支链上看它们给动平台提供四个运动螺旋和两个约束螺旋每个约束螺旋是与四个运动螺旋互异积均为零的反螺旋,3个支链各给动平台提供2个约束螺旋,6个约束螺旋线性无关的情况下,动平台就被约束住了,瞬时处于静止状态,如果6个约束螺旋线性相关,动平台没能约束6个自由度,系统就处于奇异状态。
进一步的,将约束螺旋左乘在公式(38)的两侧,可以消去被动驱动,得到:
其中,j=1,2,3公式(39)中dej是J支链去掉4个被动驱动参数后的19个误差参数,是这19个误差参数对应的螺旋量构成的矩阵。
进一步的,由于动平台是三个支链共有的,得到动平台的误差参数,具体为:
其中,
dE=[de1,de2,de3]T是去掉被动变量的所有参数,总共3×19=57个参数的误差参数,是动平台6维旋量误差,J是一般3RPS并联机构全误差参数6行57列雅可比矩阵。
通过上述算法获得了全部结构参数的雅可比矩阵。
步骤103,对所述动平台上的靶标进行测量,获取所述动平台的任意位姿参数;
在本申请实施例中,可以使用激光跟踪仪测量动平台上三个以上的靶标,再通过SVD算法可以计算出动平台的任意位姿,得到任意位姿参数。
步骤104,根据所述任意位姿参数对所述全参数雅可比矩阵进行辨识,以对所述一般三支链并联机构的全部结构参数进行标定,。
在对全参数进行辨识时,可以采用线性最小二乘、非线性最小二乘和LM算法。
具体包括,联机构全参数运动学正解,是高维非线性函数,即:
Tj=f(xj,β) (41)
其中,j=1,2,...,m。
公式(41)中,β=β0+δβ,xj=xj0+δx,β是实际结构参数,不包括驱动。β0是名义结构参数。δβ是结构参数误差。m是驱动指令总数。j是驱动指令编号。Tj是对应的实际动平台位姿参数。xj是对应的实际驱动变量,xj0是对应的名义驱动参数,δx是驱动参数误差。yi表示实测动平台位姿,用yi-f(xi,β)表示位姿误差。
辨识系统参数β的目标是使m个不同位姿,误差的平方和最小。
参数的微小变化量δz在运动空间局部呈线性变化,Jj的j是驱动指令编号对应的全参数雅可比
δTj=f(xi+δx,β+δβ)-f(xi,β)=Jjδz (43)
其中,j=1,2,...,m。需要辨识的参数是δz=(δxT,δβT)T,并将驱动原点误差和结构参数误差统一在一起表达,这是一个与驱动指令无关的系统误差。
选用线性最小二乘可以得到δz的解:
δz=[(JTJ)-1JT]δT (44)
式中:
J=[J1,J2,...,Jm]T
δT=[δT1,δT2,...,δTm]T
但由于正解Tj=f(xj,β)在运动空间是非线性的,所以改用高斯-牛顿方程迭代法求解非线性最小二乘问题可以得到精确性更好的解。
zk+1=zk-(J(zk)TJ(zk))-1J(zk)TδT(zk) (45)
由于J(zk)TJ(zk)必须非奇异。通过引进μk,克服J(xk)TJ(xk)奇异给高斯牛顿迭代法带来的困难。因此最终选用了Levenberg-Marquardt算法
选取恰当的参数μk,LM算法可以保证J(xk)TJ(xk)+μkI非奇异并且能够避免出现过大的||dk||。
过大的||dk||造成迭代不收敛。μk越大,||dk||越小,迭代步长太小则没有意义,如||dk||=10-10,需要的迭代步也过多,程序运行很慢。控制maxdk<0.001mm可以满足工程精度需要,迭代次数适中。
通过上述辨识方法实现了对参数的辨识,实现了对全参数的标定。
步骤105,根据标定后的全部结构参数对所述一般三支链并联机构进行误差补偿。
下面,本申请实施例以实际的一般三支链并联机构验证上述算法误差补偿的精度。采用支链与静平台链接使用R副,与动平台链接使用复合球铰链的三支链并联机构。由于复合球铰链名义结构参数要求轴与轴垂直相交。但工程实际中做不到0误差。本申请提出的算法可以解决实际结构参数中,距离不为0、角度参数不为90度情况下的解。
首先按DH方法建立一般三支链并联机构支链的坐标系,根据坐标系得到支链上各构件参数见表1。3个支链的名义参数是相同的,实际参数是不同的,这里仅以一个支链为例说明。
表1:
关节序号 |
aj/mm |
αj/° |
dj/mm |
θj/° |
1 |
0 |
-90 |
0 |
θ1 |
2 |
0 |
90 |
l |
θ2 |
3 |
0 |
-90 |
0 |
θ3 |
4 |
0 |
90 |
0 |
θ4 |
表中aj为zj-1轴与zj轴的公法线的长度,αj为两轴的夹角,dj为轴xj-1与轴xj沿zj轴向的长度,θj为轴xj-1与轴xj之间的夹角。
根据支链中各个坐标系的定义,用对偶四元数做出支链j从坐标系i-1到坐标系i的变换,即为静平台铰链位置和方向见表2。动平台铰链位置和方向见表3。
表2:
支链 |
x |
y |
z |
X方向 |
Y方向 |
Z方向 |
1 |
495 |
0 |
0 |
1,0,0 |
0,0,1 |
0,-1,0 |
2 |
-247.5 |
428.683 |
0 |
-0.5,0.866025,0 |
0,0,1 |
0.866025,0.5,0 |
3 |
-247.5 |
-428.683 |
0 |
-0.5,-0.866025,0 |
0,0,1 |
-0.866025,0.5,0 |
表3:
工作空间中可以有无穷多组驱动指令,由于3RPS一般三支链并联机构仅有3个自由度,将3个影响因素,每个因素分成10个水平,也会产生1000组,试验量太多。这里利用正交试验设计的方法,构成了100组驱动指令[12],从中选择前80组用于参数辨识,后20组仅用于效验辨识结果。
横坐标是100组位姿的指令驱动编号,纵坐标是综合误差评价指标||(ωT,vT)||,姿态误差矢量ω的量纲是弧度,位置误差v的量纲是米。该指标是一个无量纲的量。星型表示标定前的位姿误差,菱型表示标定后的位姿误差,标定前综合误差指标均值是0.012125,标定后综合误差指标均值是0.0001705。定位精度提高71倍。如图4所示,图中星形为标定前的误差,菱形为标定后的误差。以驱动指令1的为例,标定前位姿误差分量为:
标定后位姿误差分量为:
标定后位姿误差较原有误差减小很多,表明采用本申请中的全参数标定方法的标定效果有效。
综上所述,本申请可以实现对一般三支链并联机构全部结构参数的标定,标定方法定位精度提高效果明显。由于一般三支链并联机构的组成零部件非常多,如果提高每个零部件的设计及制造精度,不仅成本高,而且在工程上不可能达到的。通过对参数进行标定的方法可以减少系统误差,提高一般三支链并联机构的定位精度。
在一般三支链并联机构设计和制造的过程中,应努力避免如间隙、变形等随机误差,对于常值的结构参数,他们仅会产生系统误差,没必要苛求它的精度,待装配完成后进行标定即可。本申请的方法具有一般性,可以推广到其它的一般三支链并联机构进行全参数标定。
以上所述仅为本发明的优选实施例而已,并不用于限制本发明,对于本领域的技术人员来说,本发明可以有各种更改和变化。凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。应注意到:相似的标号和字母在下面的附图中表示类似项,因此,一旦某一项在一个附图中被定义,则在随后的附图中不需要对其进行进一步定义和解释。