算法eq2

第25卷第6期2008年12月控制理论与应用ControlTheory&ApplicationsVol.
25No.
6Dec.
20086R机机机器器器人人人实实实时时时逆逆逆运运运动动动学学学算算算法法法研研研究究究刘松国,朱世强,李江波,王宣银(浙江大学流体传动及控制国家重点实验室,浙江杭州310027)摘要:提出一套解决各类6R机器人逆运动学问题的实时算法.
一般算法通过矢量计算和16阶矩阵分解得到一般6R机器人的最多16组逆运动学解.
封闭解法直接提取运动学等式求出关节变量的解析解.
组合算法将封闭解法或一般算法的结果作为初始值,采用牛顿-拉夫森方法迭代出逆运动学精确解,适用于所有接近满足封闭解条件或一般算法条件的6R机器人.
求解实验结果表明,整套算法最大算法时间约为2.
03ms,为任意几何结构的6R机器人应用于强实时系统提供了逆运动学解决方案.
关键词:6R机器人;逆运动学;实时算法;组合算法中图分类号:TP242.
2文献标识码:AResearchonreal-timeinversekinematicsalgorithmsfor6RrobotsLIUSong-guo,ZHUShi-qiang,LIJiang-bo,WANGXuan-yin(StateKeyLabofFluidPowerTransmissionandControl,ZhejiangUniversity,HangzhouZhejiang310027,China)Abstract:Asetofreal-timealgorithmsforinversekinematicsofalltypeof6Rrobotsisproposed.
Thegeneralalgorithmobtains16inversekinematicssolutionsintotalforgeneral6Rrobotsbasedonvectoroperationsandeigenvalue-decompositionofa16ordertargetmatrix.
Theclosed-formalgorithmselectsproperkinematicsequationsdirectlyandsolvesforthejointvariablesanalytically.
TheobtainedresultsareemployedbythecombinedalgorithmastheinitialvaluesintheiterativeNewton-Raphsonmethodforndingtheexactsolutionsoftheinversekinematics,whichcanbeusedfor6Rrobotsthatapproximatelymeettherequirementsoftheclosed-formalgorithmorthegeneralalgorithm.
Experimentalresultsshowthattheproposedsetofalgorithmssolvestheinversekinematicsproblemof6Rrobotswithanygeometrycongurationin2.
03ms,andprovideseffectivesolutionsfortheinversekinematicsproblemof6Rrobotsappliedinstrongreal-timesystems.
Keywords:6Rrobots;inversekinematics;real-timealgorithm;combinedalgorithm文文文章章章编编编号号号:10008152(2008)061037051引引引言言言(Introduction)逆运动学算法是6R机器人轨迹规划与控制的基础,其实时性能对机器人控制系统具有至关重要的意义.
当6R机器人的几何结构满足Pieper准则[1],即3个相邻关节轴交于一点或者3个相邻关节轴相互平行时,可以采用封闭解法[2]求解逆运动学问题,PUMA560等广泛使用的工业机器人均采用这种结构.
对于满足Pieper准则但又存在结构误差的6R机器人,通常采用补偿算法[3]修正结构误差对逆运动学解的影响,然而补偿算法都针对特定的机器人系统,通用性和实时性无法保证.
当6R机器人的几何结构不满足Pieper准则时,成为一般6R机器人,其逆运动学问题是机器人领域密切关注的难题[510].
Raghavan和Roth通过矢量运算由6个逆运动学等式构造14个基础方程[5],消元运算后得到一元24次方程,求出最多16组逆运动学解,但存在8个增根.
Manocha采用24阶矩阵特征分解方法对Raghavan的算法进行改进[6],提高了逆运动学解算的稳定性和精度.
文献[7,8]为解决空间7R机构的位移分析难题,分别采用复数方法和矩阵运算构造10个基础方程,进而得到一元16次方程,消除了增根,两种方法对一般6R机器人的逆运动学求解具有借鉴意义.
本文提出一套解决各类6R机器人逆运动学问题的实时算法.
采用封闭解法求解满足Pieper准则的6R机器人的逆运动学问题.
一般算法通过矢量计算和符号运算将Manocha得到的目标矩阵从24阶降低到16阶,并以矩阵特征分解方法提高一般6R机器人逆运动学求解的效率和稳定性.
将封闭解法和一般算法分别与牛顿-拉夫森迭代算法[4]组合,解决满足Pieper准则但又存在结构误差的6R机器人和不满收稿日期:20070518;收修改稿日期:20080101.
1038控制理论与应用第25卷足一般算法条件的一般6R机器人的逆运动学问题,具有通用和高效的特点.
采用C++语言实现整套算法,得到各类6R机器人的最大逆运动学算法时间约为2.
03ms.
2逆逆逆运运运动动动学学学算算算法法法(Inversekinematicsalgo-rithms)根据D-H参数和连杆坐标系,6R机器人的运动学方程描述为TEnd=T1T2T3T4T5T6,(1)其中:Ti=Rz(θi)Tz(di)Tx(ai)Rx(αi)(i=1,2,6),ai为连杆长度,αi为连杆扭角,di为连杆偏置,θi为关节变量,TEnd为机器人工具中心的位姿矩阵.
6R机器人的逆运动学问题就是在已知连杆参数di,ai和αi以及给定TEnd的条件下,求解关节变量θi.
2.
1一一一般般般算算算法法法(Generalalgorithm)一般算法用于解决无封闭解的一般6R机器人的逆运动学问题.
文献[6]构造了24*24矩阵,通过矩阵特征分解求出关节变量,存在两个问题:24*24矩阵的获取和求根算法复杂;有8个纯虚数增根.
对其采用的两个矢量等式P和L作变换,得到P=1000λ2202λ2c2s20s2c20001h1h2h3a20d2=c3s30s3c30001f1f2f3,L=1000λ2202λ2c2s20s2c20001n1n2n3=c3s30s3c30001r1r2r3.
其中:i=sinαi,λi=cosαi,hv=FSC(c1,s1,c2,s2),fv=FSC(c4,s4,c5,s5),nv=FSC(c1,s1,c2,s2),rv=FSC(c4,s4,c5,s5),v=1,2,3,i=1,2,5,FSC表示关节变量正余弦函数的组合,si=sinθi,ci=cosθi.
采用符号运算对矢量等式P和L作乘法,由P,L,P·P,P·L,P*L和(P·P)L(2P·L)P,得到共14个逆运动学方程:EQ1:PL1(θ3,θ4,θ5)=PR1(θ1,θ2);EQ2:PL2(θ3,θ4,θ5)=PR2(θ1,θ2);EQ3:PL3(θ4,θ5)=PR3(θ1,θ2);EQ4:LL1(θ3,θ4,θ5)=LR1(θ1,θ2);EQ5:LL2(θ3,θ4,θ5)=LR2(θ1,θ2);EQ6:LL3(θ4,θ5)=LR3(θ1,θ2);EQ7:(P·P)L(θ4,θ5)=(P·P)R(θ1,θ2);EQ8:(P·L)L(θ4,θ5)=(P·L)R(θ1,θ2);EQ9:(P*L)L1(θ3,θ4,θ5)=(P*L)R1(θ1,θ2);EQ10:(P*L)L2(θ3,θ4,θ5)=(P*L)R2(θ1,θ2);EQ11:(P*L)L3(θ4,θ5)=(P*L)R3(θ1,θ2);EQ12:[(P·P)L(2P·L)P]L1(θ3,θ4,θ5)=[(P·P)L(2P·L)P]R1(θ1,θ2);EQ13:[(P·P)L(2P·L)P]L2(θ3,θ4,θ5)=[(P·P)L(2P·L)P]R2(θ1,θ2);EQ14:[(P·P)L(2P·L)P]L3(θ4,θ5)=[(P·P)L(2P·L)P]R3(θ1,θ2).
式中:下标L和R分别表示矢量等式的左边和右边,1,2和3是矢量中元素的标号.
可见,EQ3,EQ6,EQ7,EQ8,EQ11和EQ14共6个方程左右两边与θ3无关,移项并组合成矩阵形式,可得L16*11[s4s5s4c5c4s5c4c5s4c4s5c5s2c21]T=R16*6[s1s2s1c2c1s2c1c2s1c1]T.
(2)式中:L16*11和R16*6为常系数矩阵.
令x3=tan(θ3/2),代入含有θ3的8个等式中,线性变换后得到8个新的等式:EQ1L+x3EQ2L=EQ1R+x3EQ2R,EQ2Lx3EQ1L=EQ2Rx3EQ1R,EQ4L+x3EQ5L=EQ4R+x3EQ5R,EQ5Lx3EQ4L=EQ5Rx3EQ4R,EQ9L+x3EQ10L=EQ9R+x3EQ10R,EQ10Lx3EQ9L=EQ10Rx3EQ9R,EQ12L+x3EQ13L=EQ12R+x3EQ13R,EQ13Lx3EQ12L=EQ13Rx3EQ12R.
移项并组合成矩阵形式,得L28*11(x3)[s4s5s4c5c4s5c4c5s4c4s5c5s2c21]T=R28*6[s1s2s1c2c1s2c1c2s1c1]T.
(3)式中:R28*6为常数矩阵,L28*11(x3)的各项元素为x3的一次二项式或常数.
当rank(R16*6)=6时,式(2)两边左乘R116*6,并第6期刘松国等:6R机器人实时逆运动学算法研究1039令xi=tan(θi/2),i=4,5,代入式(3),每个等式左右两边分别乘以(1+x24)(1+x25),可得L38*11(x3)x24x25x24x5x4x25x4x5x24x4x25x5s2kc2k1]T=08*1.
(4)式中k=(1+x24)(1+x25),L38*11(x3)与L28*11(x3)具有相似的形式.
将式(4)中各式乘以x4,得到8个新的方程,并与式(4)中的8个方程组合,可得L416*16(x3)[x34x25x34x5x24x25x24x5x34x24x4x25x4x5x4s2kx4c2kx4x25x5s2kc2k1]T=016*1.
为表述方便,简单表示为L4(x3)(V245)=0.
(5)其中:L4(x3)=Ax3+B,A,B∈R16*16为常数矩阵,V245为包含θ4和θ5的半角正切以及θ2的正余弦函数的向量.
使方程(5)有解的必要条件是det[L4(x3)]=0.
(6)采用矩阵特征分解方法求解关节变量.
当A为非奇异矩阵时,由方程L4(x3)=0可得Ix3+A1B=0.
(7)其中I为16*16单位矩阵.
令M=A1B,由矩阵计算理论可知,矩阵M的16个特征值即为x3的16个解,对应的特征向量即为V245,由V245中的相关元素可以计算出x4,x5,s2和c2,从而解出θ2,θ3,θ4和θ5,代入式(2)中,可以解出θ1,将求出的5个关节变量代入式(1),即可解出θ6.
当6R机器人的3个相邻关节轴相互平行时,有R16*6=02*6R4*6.
当6R机器人3个相邻关节轴相交于一点时,有A=R16*12016*4.
即当6R机器人的几何结构满足Pieper准则时,关键矩阵无法求逆,因此一般算法不适合满足Pieper准则的6R机器人的逆运动学求解.
对于满足Pieper准则但有误差的6R机器人的逆运动学求解,一般算法也适用.
2.
2封封封闭闭闭解解解法法法(Closeformalgorithm)当6R机器人的几何结构满足Pieper准则时,可以得到封闭解.
不满足Pieper准则的一般6R机器人只有在零连杆参数数目较多时,才可能有封闭解.
目前广泛使用的工业机器人都力求满足Pieper准则以得到封闭解,如PUMA560类型机器人腕部三轴相交于一点.
封闭解法对于不同类型的满足Pieper准则的6R机器人和具有封闭解的一般6R机器人,仅需选择不同的运动学等式.
以研制的PUMA560类型机器人为例,D-H参数如表1所示.
令L(i,j)和R(i,j)分别表示4*4矩阵L和R的第i行第j列个元素,求解其逆运动学问题的步骤为:由L=T11TEnd=T2T3T4T5T6=R,L(3,4)=R(3,4),L(1,4)=R(1,4),L(2,4)=R(2,4)(8)可分别求出θ1和θ3在[–180,180]区间内的两个解.
同样,由L=(T1T2T3)1TEnd=T4T5T6=R,L(1,4)=R(1,4),L(3,4)=R(3,4),L(1,3)=R(1,3),L(2,3)=R(2,3)(9)可求出θ2在[–180,180]区间内的一个解和θ4在区间内的两个解.
由L=(T1T2T3T4)1TEnd=T5T6=R,L(1,3)=R(1,3),L(2,3)=R(2,3)(10)可求出θ5在[–180,180]区间内的一个解.
由L=(T1T2T3T4T5)1TEnd=T6=R,L(1,3)=R(1,3),L(2,3)=R(2,3)(11)可求出θ6在[–180,180]区间内的一个解.
可见θ1,θ3和θ4分别有两个解,θ2,θ5和θ6分别有一个解,因此,PUMA560类型机器人最多有8组逆运动学解.
表1PUMA560机器人D-H参数Table1D-HparametersofPUMA560robot关节θ/()d/mma/mmα/()1θ1211150–90.
02θ2055003θ30175–90.
04θ4650090.
05θ50090.
06θ60002.
3组组组合合合算算算法法法(Combinedalgorithm)6R机器人某关节坐标系下的广义速度可以描述为线速度ν和角速度ω组成的6维列矢量,微分运动可以描述为微分平移d和微分旋转δ组成的6维列矢量D,且νω=limt→01tdδ=limt→01tD.
(12)1040控制理论与应用第25卷同时,6R机器人笛卡尔空间的广义速度和关节空间的关节速度可以通过6*6机器人雅可比矩阵J相互转换:νω=J(θ)dθdt.
(13)由式(12)和式(13)可得limt→01tD=J(θ)dθdt,即D=J(θ)dθ.
6R机器人从当前位姿TCur运动到目标位姿TEnd可以描述为微分运动形式:TEnd=TCur(I+TCur),则有TCur=T1CurTEndI.
TCur即为微分平移和微分旋转变换,且有TCur=0δzδydxδz0δxdyδyδx0dz0001,其中:dx,dy和dz为微分平移矢量d中的元素,δx,δy和δz为微分旋转矢量δ中的元素.
当J(θ)可逆时,可以求得dθ=J1(θ)D.
因此,可采用牛顿-拉夫森迭代法求解6R机器人的逆运动学问题,具体步骤如下:Step1估计一个邻近的关节矢量θCur,将θCur代入式(1)中,可得当前位姿;Step2根据当前位姿TCur和目标位姿TEnd计算微分运动矩阵:TCur=T1CurTEndI;Step3由微分运动矩阵中的相应元素得到微分运动矢量D,进一步求出dθ=J1(θCur)D;Step4当||dθ||ξ时,迭代结束并输出θCur,ξ是满足精度需要的自定义充分小数;当迭代次数大于N时,迭代结束并输出迭代失败信息,N为满足预计迭代次数要求的自定义充分大数;当||dθ||>ξ且迭代次数小于N时,令θCur=θCur+dθ,并计算TCur=T(θCur),重复Step2至Step4,直至迭代退出条件满足.
2.
3.
1组组组合合合算算算法法法I(CombinedalgorithmI)对于修正误差后满足Pieper准则的6R机器人,可以先忽略几何结构误差,采用封闭解法求出逆运动学解初始值,然后考虑结构误差,以牛顿-拉夫森算法迭代出精确解.
该组合算法融合了封闭解算法高效和迭代算法局部收敛速度快的优势,是一种通用高效的误差修正算法.
极少数情况下迭代算法不能收敛到真实解时,只需将修正误差后满足Pieper准则的6R机器人当作一般6R机器人,采用一般算法即可求出逆运动学解.
2.
3.
2组组组合合合算算算法法法II(CombinedalgorithmII)当6R机器人有部分连杆参数为零而又不满足Pieper准则时,可能导致一般算法的关键矩阵不可逆,且不存在封闭解.
此时令所有零连杆参数的部分或者全部为小数据,使一般算法关键矩阵可逆,首先采用一般算法求出逆运动学解初始值,然后恢复零连杆参数,以牛顿–拉夫森算法迭代出精确解.
3实实实验验验与与与性性性能能能(Experimentsandperformance)在配置为PentiumIV2.
4GCPU,512MRAM,Windows2000操作系统的计算机平台上检验算法的有效性和实时性能,采用VC++编译和直接调用NEWMAT和CLAPCAK进行数值和矩阵运算.
表28组逆运动学解Table2Eightsolutionsofinversekinematics组号关节θ/()关节θ/()1–89.
066006602–74.
8564945113179.
72280354–170.
8453051594.
479412346116.
36253371–88.
554294212–75.
1530887623–179.
605998149.
0609460305–94.
773399286–64.
18684275193.
0483583826.
53520027533160.
39061674–71.
318104435–171.
86293646–172.
5755194193.
2345869826.
72560130043160.
58857544105.
94825655172.
0180762610.
00521334190.
000000002–140.
00000005349.
9999999949.
999999999580.
000000016120.
0000000190.
475890372–139.
33559736349.
299150954–170.
09328845–79.
958456856–60.
492311741–85.
243089562165.
06346927329.
931087154–60.
101485995171.
40327936–5.
4524778811–85.
555983312165.
02697278330.
247604944121.
52961075–171.
18705776173.
2035008第6期刘松国等:6R机器人实时逆运动学算法研究1041以有误差的PUMA560机器人作为求解实例,D-H参数如表1,且θ1=90,θ2=140,θ3=50,θ4=10,θ5=80及θ6=120,有误差的连杆参数为:d2=d3=d5=d6=a4=a5=a6=2,α2=α6=1.
采用组合算法I和一般算法均可求出8组逆运动学解,如表2.
以各类6R机器人测试提出的逆运动学算法的有效性,得到算法的适用范围和实时性能如表3.
以相同的硬件平台和程序设计方法实现Manocha的算法,算法时间为4.
6ms,而本文的一般算法的算法时间为1.
37ms,前者是计算机性能提高的结果,后者是算法优化的结果.
表3算法适用范围及实时性能Table3Applicabilityandreal-timeperformanceofalgorithmsPieper准则类型算法时间/ms满足且PUMA560封闭解法0.
11无误差其他封闭解法≈0.
11PUMA560组合算法I0.
77满足但一般算法1.
37有误差其他组合算法I≈0.
77一般算法1.
37有封闭解封闭解法≈0.
11无封闭解但满足一般算法1.
37不满足一般算法条件无封闭解且不满组合算法II≈2.
03足一般算法条件求解实验和算法实时性能分析结果表明,整套算法可求出各类6R机器人的逆运动学解,并具有良好的实时性能,为满足Pieper准则但有结构误差的6R机器人提供了通用实时误差修正算法,为一般6R机器人的实时逆运动学求解提供了解决方案.
4结结结论论论(Conclusion)提出一套解决各类6R机器人逆运动学问题的实时算法.
当6R机器人的几何结构满足Pieper准则,或者不满足Pieper准则但由于零连杆参数数目较多而具有封闭解时,通过选择不同的运动学等式可以得到封闭解.
对于不满足Pieper准则且无封闭解的一般6R机器人,采用一般算法可求出最多16组逆运动学解.
组合算法I和组合算法II分别为满足Pieper准则但有误差的6R机器人和不满足一般算法条件的一般6R机器人提供了通用逆运动学求解方案.
采用C++实现整套算法,算法时间最大约为2.
03ms,最小约为0.
11ms,因此为各类6R机器人应用于强实时系统提供了逆运动学解决方案.
参参参考考考文文文献献献(References):[1]熊有伦.
机器人技术基础[M].
武汉:华中科技大学出版社,1996.
(XIONGYoulun.
FundamentalsofRobotTechniques[M].
Wuhan:HuazhongUniversityofScienceandTechnologyPress,1996.
)[2]PAULRP,ZHANGH.
Computationalefcientkinematicsformanip-ulatorswithsphericalwristsbasedonhomogeneoustransformationrepresentation[J].
InternationalJournalofRoboticsResearch,1986,5(2):30–42.
[3]DOLINSKYJU,JENKINSONID,COLQUHOUNGJ.
Applica-tionofgeneticprogrammingtothecalibrationofindustrialrobots[J].
ComputersinIndustry,2007,58(3):255–264.
[4]ANGELESJ.
Onthenumericalsolutiontotheinversekinematicsproblem[J].
InternationalJournalofRoboticsResearch,1985,4(2):21–37.
[5]RAGHAVANM,ROTHB.
Kinematicanalysisofthe6Rmanipulatorofgeneralgeometry[C]//InternationalJournalofRoboticsResearch.
Tokyo:MITPress,1989:314–320.
[6]MANOCHAD,CANNYJF.
Efcientinversekinematicsforgeneral6Rmanipulators[J].
IEEETransactiononRoboticsandAutomation,1994,5(9):648–657.
[7]廖启征,梁崇高,张启先.
空间7R机构位移分析的新研究[J].
机械工业学报,1986,22(3):1–9.
(LIAOQizheng,LIANGChonggao,ZHANGQixian.
Anovelap-proachtothedisplacementanalysisofgeneralspatial7Rmecha-nism[J].
ChineseJournalofMechanicalEngineering,1986,22(3):1–9.
)[8]LEEHY,LIANGCG.
Displacementanalysisofthegeneralspa-tial7-link7Rmechanism[J].
MechanismandMachineTheory,1988,23(3):219–226.
[9]HUSTYML,PFURNERM,SCHROCKERHP.
Anewandefcientalgorithmfortheinversekinematicsofageneralserial6Rmanipula-tor[J].
MechanismandMachineTheory,2007,42(1):66–81.
[10]CHAPELLEF,BIDAUDP.
Aclosedformforinversekinemat-icsapproximationofgeneral6Rmanipulatorsusinggeneticpro-gramming[C]//Proceedingsofthe2001IEEEInternationalConfer-enceonRobotics&Automation.
Seoul,USA:IEEEPress,2001:3364–3369.
作者简介:刘刘刘松松松国国国(1980—),男,博士生,目前研究方向为智能机器人与嵌入式系统,E-mail:sgliu@zju.
edu.
cn;朱朱朱世世世强强强(1966—),男,教授,目前研究方向为智能机器人与机电控制系统,E-mail:sqzhu@zju.
edu.
cn;李李李江江江波波波(1983—),男,硕士生,目前研究方向为智能机器人与机器视觉,E-mail:ernist@zju.
edu.
cn;王王王宣宣宣银银银(1966—),男,教授,目前研究方向为机器人与图像信息技术,E-mail:xywang@zju.
edu.
cn.