摘 要：给出了一种基于指数积公式的串联机器人运动学参数标定方法．根据关节旋量坐标的理论值和实际值之间的伴随变换关系将指数积公式改写成包含有关节约束条件的等价形式．对运动学方程取微分得到末端执行器误差与关节旋量误差及零位旋量误差间的线性化模型．给出一种基于最小二乘法的串联机器人运动学参数标定模型，并通过伴随变换的方法实现运动学参数识别过程中关节旋量坐标的更新．一种6自由度串联机器人的标定仿真实验表明参数标定过程能够快速地收敛到稳定值，标定结果能有效地补偿末端执行器的位姿误差．
关键词：串联机器人；指数积；参数标定
A Calibration Method for Serial Robots Based on POE Formula
Abstract:A kinematics parameter calibration method for serial robots based on the product of exponential (POE) formulais presented. According to the adjoint transformation between the theoretical value and actual value of joint twists, thePOE formula is transformed into an equivalent form including the joint constraints. A linearized model describing therelationship between the errors in the end-effector and the errors in the joint twists and in the zero position twist, is obtainedby differentiating the kinematics equation. A least-squares kinematics calibration model for serial robots is given and thejoint twists are updated through the adjoint transformation in the kinematic parameters identification procedure. A calibrationsimulation experiment with a 6-DOF (degree of freedom) serial robot shows that the calibration process can quickly convergeto the stable values and the calibration result can compensate for the pose errors in the end-effector effectively.
Keywords:serial robot; POE (product of exponential); parameter calibration
　　1 前言（Introduction）
　　目前，在虚拟环境中实现离线编程已成为机器人应用技术的一个发展趋势．机器人的实际运动学模型和仿真环境中使用的理论模型间的偏差将导致机器人产生较大的定位误差．精确的运动学模型是实现离线编程的基础，参数标定是获得准确的运动学模型的一种经济有效的方法[1-3]．
　　机器人运动学参数的标定研究大多基于D-H参数法，原因是这种方法需要的参数量最少．D-H参数法的主要不足之处是当相邻关节的轴线接近平行时，机器人会出现奇异性问题．一些学者采用增加参数的方法来解决奇异性问题，带来了模型建立过程不直接、形式复杂、缺少通用性等新问题[4-5]．为了克服D-H参数法的不足，Park[6]首先提出了基于指数积公式的运动学参数标定方法，原因有：(1)指数积模型实现了转动关节和移动关节的统一描述，具有通用性的优点；(2)当相邻关节轴线接近平行时，运动学模型相对关节运动是光滑变化的，不会出现奇异性问题[7-8]．Park给出的标定模型中包含有限积分项，使用过程中并不方便．He[9]对Park的模型进行了改进，给出了一种显式的标定模型．
　　文[9]给出的基于指数积公式的标定方法需在迭代计算过程中的每一步进行一次正交化处理，以保证关节旋量坐标更新后仍满足关节约束条件的要求．正交化处理过程将导致关节旋量坐标中实际更新的误差量不能严格地符合线性化方程对参数误差的识别结果，不但降低了标定结果的精度，甚至会影响标定过程的稳定性．为避免正交化处理过程，基于指数积公式，本文给出了一种包含关节约束条件的显式形式的串联机器人运动学参数标定模型．
　　2 运动学模型变换（Kinematics model trans-formation）
　　2.1 误差分析
　　n自由度串联机器人的运动学模型的指数积公式为[7]f (q) =eˆx1q1eˆx2q2¢ ¢ ¢eˆxnqnfst(0) (1)式中，q为关节变量向量，ˆxi为i 号关节旋量，qi 为i 号关节变量，fst(0)为各关节处于零位时末端执行器坐标系相对于基础坐标系的位姿矩阵．fst(0)可以写成某个常旋量ˆG2se(3)的指数形式．式(1)可以改写成f (q) =eˆx1q1eˆx2q2¢ ¢ ¢eˆxnqneˆG(2)机器人的误差模型可以通过对运动学方程取微分的方式获得[6]df f¡1=µ¶f¶Y YdY+¶f¶ G GdG+¶f¶ qqdq¶f¡1(3)式中，Y= [x1x2¢ ¢ ¢ xn] 2R6£n，G2R6．由式(3)可知机器人的参数误差与旋量坐标误差(dxi;dG)及关节的零位偏置误差（dq）相对应．文[10]指出关节的零位偏置误差可以归结为旋量误差，不必单独识别．本节对这一结论给出了更为合理的解释．假设模型中只有旋量误差，旋量坐标的实际值为(x0ai;G0a)，此时运动学模型为f0a(q) =eˆx0a1q1eˆx0a2q2¢ ¢ ¢eˆx0anqneˆG0a(4)实际运动学模型中关节零位偏置误差是存在的，设为dq．代入式(4)可得运动学模型fa(q)=eˆx0a1(q1+dq1)eˆx0a2(q2+dq2)¢ ¢ ¢eˆx0an(qn+dqn)eˆG0a=eˆx0a1q1eˆx0a1dq1eˆx0a2q2³e¡ˆx0a1dq1eˆx0a1dq1´eˆx0a2dq2¢ ¢ ¢eˆx0anqn³e¡ˆx0an¡1dqn¡1¢ ¢ ¢e¡ˆx0a2dq2e¡ˆx0a1dq1eˆx0a1dq1eˆx0a2dq2¢ ¢ ¢eˆx0an¡1dqn¡1´eˆx0andqneˆG0a=eˆx0a1q1³eˆx0a1dq1eˆx0a2q2e¡ˆx0a1dq1´¢ ¢ ¢³eˆx0a1dq1eˆx0a2dq2¢ ¢ ¢eˆx0an¡1dqn¡1eˆx0anqne¡ˆx0an¡1dqn¡1¢ ¢ ¢e¡ˆx0a2dq2e¡ˆx0a1dq1´¢³eˆx0a1dq1eˆx0a2dq2¢ ¢ ¢eˆx0andqneˆG0a´(5)根据恒等式eˆxx q=geˆx0qg¡1=eg³ˆx0q´g¡1[7]，式(5)可以改写为fa(q) =eˆxa1q1eˆxa2q2¢ ¢ ¢eˆxanqneˆGa(6)式中，eˆxa1q1=eˆx0a1q1eˆxa2q2=eˆx0a1dq1eˆx0a2q2e¡ˆx0a1dq1=eeˆx0a1dq1(ˆx0a2q2)e¡ˆx0a1dq1.eˆxanqn=eˆx0a1dq1eˆx0a2dq2¢ ¢ ¢eˆx0an¡1dqn¡1eˆx0anqne¡ˆx0an¡1dqn¡1¢ ¢ ¢e¡ˆx0a2dq2e¡ˆx0a1dq1=eeˆx0a1dq1eˆx0a2dq2¢¢¢eˆx0an¡1dqn¡1(ˆx0anqn)e¡ˆx0an¡1dqn¡1e¡ˆx0a2dq2e¡ˆx0a1dq1eˆGa=³eˆx0a1dq1eˆx0a2dq2¢ ¢ ¢eˆx0andqn´eˆG0a由式(6)可知，关节的零位偏置误差可以看作是旋量误差的一个误差源，在运动学参数的标定过程中仅需识别和补偿旋量误差，式(3)可以简化为df f¡1=µ¶f¶Y YdY+¶f¶ G GdG¶f¡1(7)
　　2.2 模型变换
　　关节旋量坐标x=£wTvT¤T的理论值和实际值都应满足相应的关节约束条件[10]．转动关节旋量坐标的理论值xn和实际值xa之间的关系可用某个刚体变换eˆ h=24RR pp0 135的伴随矩阵表示xa=Ad³eˆ h´xn=24R 0ˆ ppR RR RR3524wnvn35=24Rwnˆ ppR Rwn+RRvvn35(8)转动关节应满足的关节约束条件为kwk=1; wTv=0 (9)易知，转动关节的旋量坐标经伴随变换之后仍满足关节约束条件要求，即kwak=kRwnk=kwnk=1 (10)(wa)Tva= (Rwn)T(ˆ ppR Rwn+RRvvn) =0 (11)移动关节旋量坐标的理论值xn和实际值xa之间的关系也可用某个刚体变换eˆ h=24RR pp0 135的伴随矩阵表示158 机 器 人 2013年3月xa="0va#=Ad³eˆ h´xn="R 0ˆ ppR RR RR#"0vn#="0RRvvn#(12)移动关节的旋量坐标x=£0 vT¤T应满足的约束条件为kvk=1 (13)易知，移动关节的旋量坐标经过伴随变换之后仍满足关节约束条件要求，即kvak=kRRvvnk=kvnk=1 (14)由式(8)、(12) 可知，运动学方程(6)可写成理论关节旋量的伴随变换形式fa(q) =eeˆ h1ˆxn1q1e¡ˆ h1eeˆ h2ˆxn2q2e¡ˆ h2¢ ¢ ¢eeˆ hnˆxnnqne¡ˆ hneˆGa(15)
　　3 标定模型（Calibration model）
　　3.1 运动学模型线性化将运动学方程(15)代入方程(7)，化简可得¡df ¢ f¡1¢_=³d³eeˆ h1ˆxn1q1e¡ˆ h1eeˆ h2ˆxn2q2e¡ˆ h2¢ ¢ ¢eeˆ hnˆxnnqne¡ˆ hneˆGa´¢³eeˆ h1ˆxn1q1e¡ˆ h1eeˆ h2ˆxn2q2e¡ˆ h2¢ ¢ ¢eeˆ hnˆxnnqne¡ˆ hneˆGa´¡1´_=³d³eˆ h1eˆxn1q1e¡ˆ h1eˆ h2eˆxn2q2e¡ˆ h2¢ ¢ ¢eˆ hneˆxnnqne¡ˆ hneˆGa´¢³e¡ˆGaeˆ hne¡ˆxnnqne¡ˆ hn¢ ¢ ¢eˆ h2e¡ˆxn2q2e¡ˆ h2eˆ h1e¡ˆxn1q1e¡ˆ h1´´_=³d³eˆ h1eˆxn1q1e¡ˆ h1´³eˆ h1e¡ˆxn1q1e¡ˆ h1´´_+Ad³eˆ h1eˆxn1q1e¡ˆ h1´³d³eˆ h2eˆxn2q2e¡ˆ h2´¢³eˆ h2e¡ˆxn2q2e¡ˆ h2´´_+¢ ¢ ¢+AdÃn¡1Õi=1eˆ hieˆxniqie¡ˆ hi!¢³d³eˆ hneˆxnnqne¡ˆ hn´³eˆ hne¡ˆxnnqne¡ˆ hn´´_+AdµnÕi=1eˆ hieˆxniqie¡ˆ hi¶³³deˆGa´e¡ˆGa´_(16)由恒等式d¡eˆ h¢e¡ˆ h+eˆ hd¡e¡ˆ h¢=0，可得³d³eˆ hieˆxniqie¡ˆ hi´³eˆ hie¡ˆxniqie¡ˆ hi´´_=³³deˆ hi´eˆxniqie¡ˆ hieˆ hie¡ˆxniqie¡ˆ hi´_+³eˆ hieˆxniqi³de¡ˆ hi´eˆ hie¡ˆxniqie¡ˆ hi´_=³deˆ hie¡ˆ hi´_+³eˆ hieˆxniqi³e¡ˆ hieˆ hi´³de¡ˆ hi´¢eˆ hie¡ˆxniqie¡ˆ hi´_=³deˆ hie¡ˆ hi´_¡Ad³eˆ hieˆxniqie¡ˆ hi´³deˆ hie¡ˆ hi´_=³I¡Ad³eˆ hieˆxniqie¡ˆ hi´´³deˆ hie¡ˆ hi´_(17)利用式(17)可将式(16)化简为¡df ¢ f¡1¢_=³I¡Ad³eˆ h1eˆxn1q1e¡ˆ h1´´³deˆ h1e¡ˆ h1´_+Ad³eˆ h1eˆxn1q1e¡ˆ h1´³I¡Ad³eˆ h2eˆxn2q2e¡ˆ h2´´¢³deˆ h2e¡ˆ h2´_+¢ ¢ ¢+AdÃn¡1Õi=1eˆ hieˆxniqie¡ˆ hi!¢³I¡Ad³eˆ hneˆxnnqne¡ˆ hn´´³deˆ hne¡ˆ hn´_+AdµnÕi=1eˆ hieˆxniqie¡ˆ hi¶³³deˆGa´e¡ˆGa´_(18)式(18)中¡d¡eˆ hi¢e¡ˆ hi¢_的显式表达式为[9;11]³d³eˆ hi´e¡ˆ hi´_=³I6+4¡aisinai ¡4 cosai2a2iWi+4ai ¡5 sinai +aicosai2a3iW2i+2¡aisinai ¡2 cosai2a4iW3i+2ai ¡3 sinai +aicosai2a5iW4i´dhi=Kidhi(19)式中，ai =kwik=¡w21i+w22i+w23i¢1=2，hi=24wivi35，Wi =24ˆ wi0ˆ viˆ wi35．式(18) 中³³deˆG´e¡ˆG´_的显式形式与式(19)相同，即³³deˆG´e¡ˆG´_=KstdG (20)将式(19)、(20)代入式(18)可得运动学方程线性化模型的显式表达式¡df ¢ f¡1¢_=³I¡Ad³eˆ h1eˆxn1q1e¡ˆ h1´´K1dh1+Ad³eˆ h1eˆxn1q1e¡ˆ h1´¢³I¡Ad³eˆ h2eˆxn2q2e¡ˆ h2´´K2dh2+¢ ¢ ¢+AdÃn¡1Õi=1eˆ hieˆxniqie¡ˆ hi!³I¡Ad³eˆ hneˆxnnqne¡ˆ hn´´¢Kndhn+AdµnÕi=1eˆ hieˆxniqie¡ˆ hi¶KstdG (21)df f¡1为末端执行器坐标系相对于基础坐标系的空间速度．当执行器的实际位姿fa和名义位姿fn非常接近时，有df f¡1= (fa¡fn)(fn)¡1=fa(fn)¡1¡I4(22)第35卷第2期 高文斌，等：一种基于指数积的串联机器人标定方法 159
3.2 参数标定模型线性化方程(22)可以写成如下矩阵形式y=Hx (23)式中，y=¡df ¢ f¡1¢_2R6(24)x=hdhT1; dhT2;¢ ¢ ¢;dhTn;dGTiT2R6(n+1)(25)H= [J1; J2;¢ ¢ ¢Jn; Jn+1] 2R6£6(n+1)(26)H的各列如下Ji =8><>:³I¡Ad³eˆ h1eˆxn1q1e¡ˆ h1´´K1; i =1AdÃi¡1Õk=1³eˆ hkeˆxnkqke¡ˆ hk´!¢³I¡Ad³eˆ hieˆxniqie¡ˆ hi´´Ki; 1<i 6nAdÃi¡1Õk=1³eˆ hkeˆxnkqke¡ˆ hk´!Kst; i =n+1控制机械臂运动到工作空间中k个标定位姿．得到k组式(23)所示误差方程，联立各方程可得标定方程Y=LLxx (27)式中，Y=264y1y2.yk375，L=264H1H2.Hk375．x可通过最小二乘法求解获得，x=¡LTL¢¡1LTY (28)3.3 标定流程运动学参数标定识别流程如图1所示．控制机械臂运动到一系列标定位姿，计算末端执行器位姿的名义值并记录对应的各关节变量值．通过测量设备获得末端执行器位姿的实际值．标定模型中误差量dhi 和dG可通过最小二乘法（式(28)）获得．在参数的迭代识别过程中，从k¡1步到k步的关节旋量坐标和零位旋量坐标的更新方法如式(29)、(30)所示：xi;k=Ad³edˆ hi;k¡1´xi;k¡1(29)Gk=Gk¡ 图3 迭代过程中末端执行器误差值Fig.3 Errors of the end-effector in the iterative procedure在机械臂的关节空间内随机产生50组测试点，根据表2所示旋量坐标的“名义值”、“实际值”和“识别结果”，利用式(6)分别计算末端执行器位姿的“理论值”、“实际值”、“补偿值”．分别计算测试位置上末端执行器位置量和姿态量的“理论值”和“补偿值”与“实际值”间误差向量的模．如图4所示，在随机产生的测试点上末端执行器的姿态误差和位置误差都得到了明显的降低．可知在机械臂参数误差和测量噪声均较大时，标定结果仍能有效地补偿末端执行器的位姿误差．5 结论（Conclusion）基于包含有关节约束条件的指数积公式的等价形式，给出了一种新型的串联机器人运动学参数标定模型．模型为显式表达式形式，结构清晰，使用方便，避免了基于D-H参数法的标定模型缺少通用性的不足，具有更好的数学一致性，可以应用于任何拓扑结构形式的串联机器人的标定．标定表2 旋量坐标值Tab.2 Twist coordinate values关节旋量 名义值 实际值 识别结果x10 0.0050 0.00500 0.0020 0.00201 1.0000 1.00000 0.0292 0.02780 0.0169 0.01290 ¡0.0002 ¡0.0002x20 ¡0.0189 ¡0.0189¡1 ¡0.9998 ¡0.99980 ¡0.0049 ¡0.00480 0.0205 0.01990 ¡0.0005 ¡0.00040 0.0165 0.0072x30 0.0286 0.0286¡1 ¡0.9996 ¡0.99961 0.0015 0.00150 0.1559 0.15320 ¡0.1485 ¡0.1484¡100 ¡100.1249 ¡100.1367x40 0.0243 0.02430 ¡0.0009 ¡0.0010¡1 ¡0.9997 ¡0.9997¡50 ¡57.3451 ¡57.3363250 248.3065 248.30920 ¡1.6270 ¡1.6310x50 0.0027 0.0026¡1 ¡0.9999 ¡0.99991 0.0158 0.0158¡20 ¡16.1706 ¡16.18430 ¡4.0015 ¡4.0040¡250 ¡250.1718 ¡250.1717x60 0.0108 0.01080 0.0131 0.0130¡1 ¡0.9999 ¡0.9999¡50 ¡52.3448 ¡52.3338250 249.4292 249.42770 2.6900 2.6880G0 0.0200 0.02000 ¡0.0100 ¡0.01000 0.0100 0.0100250 249.0000 248.993750 51.0000 50.9964¡20 ¡20.6000 ¡20.6129第35卷第2期 高文斌，等：一种基于指数积的串联机器人标定方法 16110 20 30 40 50 00.020.040.060.080.10.12 图4 末端执行器误差Fig.4 Errors of the end-effector模型中包含了关节约束条件：对于转动关节满足kwk=1，wTv=0，对于移动关节满足kvk=1，在参数的迭代标定过程中关节旋量坐标通过伴随变换的方式实现更新，避免了为满足关节约束条件要求在迭代计算中的每一步都对关节旋量坐标的更新值进行归一化处理的不足．该模型能够保证标定过程中每步获得的关节旋量坐标更新值都严格地符合线性化方程，提高了标定过程的稳定性和标定结果的准确性．最后，通过一种6自由度PUMA机械臂的标定仿真实验验证了运动学参数标定方法的有效性和稳定性．
    参考文献（References）[1] Dolinsky J U, Jenkinson I D, Colquhoun G J. Application ofgenetic programming to the calibration of industrial robots[J].Computers in Industry, 2007, 58(3): 255-264.[2] Chen H P, Fuhlbrigge T, Choi S, et al. Practical industrialrobot zero offset calibration[C]//IEEE Conference on Automa-tion Science and Engineering. Piscataway, NJ, USA: IEEE,2008: 516-521.[3] Mustafa S K, Tao P Y, Yang G L, et al. A geometrical ap-proach for online error compensation of industrial manipula-tors[C]//IEEE/ASME International Conference on AdvancedIntelligent Mechatronics. Piscataway, NJ, USA: IEEE, 2010:738-743.[4] Veitschegger W K, Wu C H. Robot accuracy analysis based onkinematics[J]. IEEE Journal of Robotics and Automation, 1986,2(3): 171-179.[5] Zhuang H Q, Roth Z S, Hamano F. A complete and paramet-rically continuous kinematic model for robot manipulators[J].IEEE Transactions on Robotics and Automation, 1992, 8(4):451-463.[6] Okamura K, Park F C. Kinematic calibration using the productof exponentials formula[J]. Robotica, 1996, 14(4): 415-421.[7] Park F C. Computational aspects of the product-of-exponentialsformula for robot kinematics[J]. IEEE Transactions on Auto-matic Control, 1994, 39(3): 643-647.[8] Chen I M, Yang G L, Tan C T, et al. Local POE model forrobot kinematic calibration[J]. Mechanism and Machine The-ory, 2001, 36(11/12): 1215-1239.[9] He R B, Zhao Y J, Yang S N. Kinematic-parameter identifica-tion for serial-robot calibration based on POE formula[J]. IEEETransactions on Robotics, 2010, 26(3): 411-423.[10] Lou Y J, Chen T N, Wu Y Q, et al. Improved and modifiedgeometric formulation of POE based kinematic calibration ofserial robots[C]//IEEE/RSJ International Conference on Intelli-gent Robots and Systems. Piscataway, NJ, USA: IEEE, 2009:5261-5266.[11] Selig J M. Geometric fundamentals of robotics[M]. 2nd ed. NewYork, USA: Springer-Verlag, 2004.