文章编号:1002-0446(2001)02-0137-05一种基于优化算法的机械手运动学逆解X马化一　张艾群　张竺英(中国科学院沈阳自动化研究所　沈阳　110015)摘　要:本文给出一种基于优化算法的机械手运动学逆解的方法,这种优化方法基于信赖域方法,,具有超线性的收敛速率.这种方法不仅具有牛顿方法的快速收敛性,又具有理想的总体收敛性.这种方法较CCD&BF S有明显的优点,可以在一般的PC机上实现实时求解.在P II400上仅需不到10ms就可以求得最优解.关键词:逆解;信赖域;总体收敛.中图分类号:　T P24 文献标识码:　BA INVERSE SOLUTION FOR MANIPULATOR KINEMATICSBASED ON OPTIMAL ALGORITHMM A Hua-yi　ZHANG Ai-qun　ZHANG Zhu-ying(S henyang I nstitute of A utomation,Ch inese A cad emy of S cienc es,S heny ang　110015)　Abstract:T his paper pr oposes an appr oach to the inver se solutio n for the manipulato r kinem atics based on the o ptimal a lg or ithm.T his optima l appr oach,based on co nfidence threshold algo rithm,possesses the super linear con-ver gence ra te.T his appr oach has not o nly t he r apid conv erg ence for New to n's metho d,but also t he desirable over-all co nv erg par ed w ith CCD&BF S,this appr oach has the obvious adv ant ages and implem ents a real time solutio n o n PC Computer g enerally.T he optim al so lutio n ca n be solved on PÊ400,using less than10ms.　Keywords:inver se solut ion,co nfidence thr esho ld,o ver all conv erg ence1　引言(Introduction)机械手逆解是机械手正解的逆过程,即给定机械手末端操纵器需要达到的位置和姿态,求解机械手各关节的关节变量.机械手运动学逆解在机器人中占有重要地位,直接关系到运动分析,离线编程和轨迹规划等.随着机器人应用范围的扩展,很多场合要求机械手离线逆解是不能满足要求的.对于简单结构(clo sed-form)的机械手,直接解可以很方便就得到(Puma机械手).但是由于一些原因,有些机械手的结构并不这样简单,比如在设计过程中,驱动机构如果是液压式的,做成简单结构就会影响机械手的性能指标,;在加工、制造和装配的过程中都会出现一些误差,这种误差的存在,就会使得机械手结构变得不简单(clo sed-form).虽然可以在控制器中加入一些补偿措施,但这必将加大技术人员的工作量.对于具有复杂结构的机械手,大体上有两类方法解机械手逆解问题.第一类方法是基于代数求解的方法.这种方法对于六DOF机械手是很好的一种求解方法.这类方法主要是基于特征多项式连续法[13,14]和消去法[2,3].这种算法是非常有效的,并且可以解出所有的解.但这种方法复杂,而且只适用于六自由度机械手问题.另一类算法是数值算法,数值解法可以分为直接解法和间接解法两类.直接解法包括牛顿法和牛顿-拉普森法[9].这种算法如果雅克比矩阵为奇异,则问题不可解;如果初始点不是充分接近目标点,问题也不可解.间接解法是基于优化的方法,这种解法主要包括CCD法、BFS法、CCD&BFS法和遗传算法等.CCD法,即最速下降法,具有全局收敛的特性,但只具有线性的搜索速度.BFS算法虽具有超线性的收敛速率,但在远离目标点时不收敛于最优解. CCD&BFS算法虽克服了二者的缺点,但仍不能用于实时逆解.本文提出一种基于信赖域的优化算法.这种算第23卷第2期2001年3月机器人　R OBOT Vo l.23,N o.2　M ar ch,2001X收稿日期:2000-09-05法不仅具有拟牛顿法的收敛速率,而且具有理想的总体收敛特性.这种算法不仅稳定,而且非常简单,执行速率又很快,可以被应用于在线的运动学逆解.本文组织如下:第二部分,讨论问题的描述.第三部分,将讨论信赖域法及其算法,第四部分,给出一些实际应用的例子,第五部分.将给出本文的结论.2　问题的描述(Description of problem )基于优化方法求解机械手运动学逆解属于间接的方法,首先需要建立关于目标问题的数学模型,然后针对具体问题利用从模型中得到的信息进行优化求解.2.1　连杆坐标系的建立本文利用D-H 法建立连杆坐标系,图1显示了机械手连杆坐标系各个参数的定义和坐标系的建立方法.图1　D-H 连杆坐标系参数定义及建立F ig.1　D efinition and establishment of D-H link coo rdinate par ameter s2.2　建立目标函数假定目标末端操纵器相对于基坐标系的位形矩阵为:H d =R dP d[0]1.其中R d =[d 1,d 2,d 3]t为末端操纵器的目标点的方向矩阵;P d 为末端操纵器目标点的位置向量.当前点的末端操纵器相对于基坐标系的位形矩阵为H c =R cP c[0]1,其中R c =[h 1,h 2,h 3]为末端操纵器当前点的方向矩阵;P c 为末端操纵器的当前点位置.机械手的运动学逆解问题可以被描述为:给定目标位姿矩阵H d ,求解q 1,q 2,…,q n ,并且q Ii ≤q i ≤q Ui(0≤i ≤n ),使得当前位姿矩阵H c 与目标位姿矩阵拟合,即‖P d -P h (q )‖=0,d j h j (q *)=1,j =1～3,‖·‖为U clidean 范数,(·)为向量点积.于是可以得出关于机械手运动学逆解目标函数的最优化模型E (q )=$p (q )+$O (q )$P (q )=D P (q )D P (q )=(P d -P h )2$O =∑3j =1(djh j -1)2(1)其中E (q )为总的误差,$P 为位置误差,$O 为方向误差.对于本最优化问题就是得到q *,使得E (q *)的值为小到允许的误差范围.在实际应用中$O 远远小于$P ,并且对于三个方向的要求也不同,可以分别给予权重E (q )=w p $P (q )+w o $O (q )$O =∑3j =1woj(d j h j -1)2(2)应该指出权重非常重要,通常权重的选择与对于位置和方向的允许误差有关.但是如果方向向量的权如果不是0,那么就应该大于10000.3　算法的导出(Alorithm )信赖预算法应用梯度和二阶导数的信息.梯度可以直接导出,但是对于二阶导数若直接导出则相当麻烦,.可以应用由BFGS 修正的Hessian 矩阵来代替二阶导数.3.1　目标函数的梯度和二阶导数梯度可以被定义为如下$E (q )=5E (q )5q 1,5E (q )5q 2,…,5E (q )5q n(3)138 机　器　人2001年3月 根据目标函数,一阶导数的每一项可以导出为5E(q) 5q i=2z i[(P d-P h(q))×P ih+∑3j=1(d j h j(q)-1)(h j(q)×d j) 如果关节i是转动关节2z i(P d-P h(q)) 如果关节i是移动关节(4) 直接求解二阶导数不仅麻烦,而且非常浪费机时,可应用BFGS法修正H k+1=I-s k y T ks T k y kH k I-s k y T ks T k y k+s k s T ks T k y k(5)y k是最后两次迭代梯度的差,s k为搜索方向s k+1=H K+1s k(6) 3.2　信赖域算法拟牛顿算法具有超线性的收敛速度,但是对于高度非线性的方程,,在最优点的邻域内收敛域于最优解.而信赖域算法不仅具有超线性的收敛速率,而且可以具有全局的收敛速率.它的主要思想:利用二次函数q k=f(x k)+g k s+0.5s T B k s逼近目标函数f(x),给定第k步迭代x k的一个邻域8k, 8k={x‖x-x k‖≤h k},使在这个邻域内q k(s)与目标函数一致;然后利用这个n维二次模型确定搜索方向s k,信赖域的模型为min q(k)(s)=f(x k)+g T k s+0.5s T G k ss.t.‖s‖≤h k(7)‖·‖为范数.可以应用任意范数.方程中G k是二阶导数,用BFGS修正的Hessian矩阵G k≈B k= H-1k代替;于是可得q k=f(x k)+g k s+12s T B k s=f(x k)+g k H g k+12g T k H k g k(8)3.3　算法Step1:初始化机械手结构数:偏距a(a1,a2,…,a n),连杆长度d(d1,d2,…,d n),扭角A(A1,A2,…,A n),给定每一个关节变量的初始角q(q1,q2,…,q n),机械手的结构限制q I i和q U i(i=1～n),目标函数精度E,初始步长限制h0,初始化Hessian矩阵;Step2:解每一个关节和末端操纵器的位姿矩阵;Step3:解目标函数的梯度5E(q)5q i;Step4:解目标函数的搜索方向s i;Step5:令A=1,if s i+q i≥q U i,A=(q U i-q i)/s i如果s i+q i≤q I i,A=(q I i-q i)/s i;i=1～n;Step6:q i=q i+A.s i,i=1～n;Step7:解E(q k)和r k;Step8:如果r k<0.75　令　h k+1=h k/4如果r k>0.75　令　h k+1=2h k否则 令　h k+1=h kStep9:如果r k≤0,令x k+1=x k 否则x k+1=x k+s k;Step10:返回步2.4　例子(Examples)本方法最初是为解决水下监控机械手在线运动学反解而导出的一种方法.在作业时,机械手末端操纵器被要求垂直于导管架焊缝并实时跟踪于它.本方法具有超线性的收敛速度,可以在线求解.以下的几个例子是分别基于Schilling机械手和Pum a机械手的.所有的程序都是在PII400计算机上进行的,编程语言为V isue C++.表1　机械手结构参数(Table1　Architecture parameters of manipulator)关节i a i(mm)A i(rad)d i(mm)H i(rad)范围(rad) 194P/2186.700～-P/2 2311-P/2-58.8P/2-P/4～P/4 3307022.50-P/2～P/6 412.8-P/20-P/2-P/2～P/6 50000-P3/4～P3/4 例1　图2为被应用于水下机器人的Shilling 机械手,它为五自由度的机械手,并且其结构也为非简单型的(Closed-fo rm).由于解存在的不确定性,求解矩阵方程的高度非线性,使这样一个问题变得复杂化.在跟踪导管架的作业中跟踪的位姿误差为不大于0.1mm,角度误差为不小于0.8°.我们给定目标函数的权值为:w p=1,w o1=w o2=0,w o3=100000.表2为根据对目标函数精度不同10-1～10-8而得到139第23卷第2期马化一等:　一种基于优化算法的机械手运动学逆解表2　用信赖域方法解机械手运动学逆解Table 2　Inverse solution for the manipulator kinematics with conf idence thresholdE t T 精确解-0.78570-0.5236-0.52360q 1q 2q 3q 4q 51e -1332-0.7867-1.7e -3-0.5272-0.5163 5.2e -31e -2436-0.7872-1.7e -3-0.5269-0.5166 5.3e -31e -3439-0.7871-1.7e -3-0.5270-0.5165 5.3e -31e -4444-0.7870-1.6e -3-0.5270-0.5164 5.3e -31e -5444-0.7870-1.6e -3-0.5270-0.5164 5.3e -31e -6878-0.7866-3.9e -4-0.5257-0.5190 2.0e -41e -7995-0.7860-2.1e -4-0.5247-0.5212 1.4e -41e -810103-0.7857-1.4e -4-0.5242-0.52211.0e -4图2　Schilling 机械手结构参数F ig.2　A rchitectur e par ameter s of Schiling manipulator的运动学反解及耗时.t ,T 分别为执行的时间和重复的次数,E 为目标函数要求的精度.初始角度变量可以随机选择.为q 1=-0.1,q 2=-0.3,q 3=-0.5,q 4=0.1,q 5=0.6.本算法对于一般的精度要求E =0.01,只花了6ms ,重复40次;对于精确解只花了11m s,软件也更加鲁棒.例2　本例子是本算法的运算时间与CCD&BFGS 法,其他的方法,参考文章[1].随机100次给出目标位姿和方向,比较信赖域算法于CCD -BFS 算法.图3是比较结果曲线,从图中可以看出信赖域算法明显好于CCD-BFS 算法.. 例3　在本例子中,说明可以用本方法求解机械手的所有解,如果多解存在,给出不同的初始角度变量,经过几次循环,就可以求出全部机械手运动学逆解.本例是基于PU MA 机械手.目标位姿为:d 1=[0.5650,0.6896,0.4530]t ;d 2=[-0.4753,0.7208,-0.5045]t ;d 3=[-0.6745,0.0697,0.7350]t ;P =[-330.412,56.2007,744.957]给出一个初始的精确解为q =(20°,-80°,30°,60°,20°,-30°).表3为PUM A 机械手结构参数;表4为用信赖域法解决的所有运动学反解;I II ～VIII 为8组解,其中q 1,q 2,…q n 为机械手的6个关节变量,A 、T 分别为精确解和信赖域法求得出的解.图3　信赖域与CCD -BF S 平均执行时间比较Fig.3　Com par ed w it h co nfidence thr esho ldand CCD -BF S tim e140 机　器　人2001年3月表3　Puma机械手参数(Table3　Parameters of PUMA manipulator)关节i H i(度)A i a i(度)d i范围(度)1900-900-160～16020431.80149.09-225～453900900-45～225400-90433.07-110～170500900-100～100600056.25-266～266表4　用信赖域方法解PUMA机械手全部解与精确解比较Table4　Compared with the overall solutions and the precise solutions of PUMA manipulatorI II III IV V VI VII VIIIq1A20202020155.48155.48155.48155.48 T19.99819.97819.87620.108155.463155.491155.394155.501q2A-80-38.47-142.53100-80-38.47-142.53-80 T-80.001-38.453-14.34199.982-79.952-38.461-142.612-79.918q3A30301501503030150150 T30.01229.861149.973150.15229.96730.191150.057150.107q4A60149.105157.07418.2312.50416.22621.7871155.313 T60.036149.132157.13518.19612.62716.22321.721155.391q5A2035.230549.497371.230891.133150.775935.66731.2175 T20.06735.22449.51771.207.91.23150.78235.68931.287q6A-30-125.518-136.20622.3841-117.764-128.443-136.00488.4451 T-29.978-125.629-136.10622.352-117.754-128.451-136.10788.4675　结论(Conclusion)本篇文章得出了一个解决机械手运动学反解的最优化方法.本最优化方法是基于信赖域法,超线性收敛于最优解点.由于其收敛速度很快,故可以被应用再线求解机械手运动学反解.由于本算法没有直接用到二阶导数,故不存在奇异解的问题.本方法用于解决监控机械手在线垂直跟踪导管架.经实践证明,可以很好地应用于这样一个系统,也可以被用于连续轨迹的跟踪问题.参考文献　(References)1　L i-Ch un Tom my Wang.A Combined Optimization M ethod for Solvin g T he In verse Kinematics Problem of M ech anical M anipula-tors.IEEE T rans of Rob otics and Autom ation,1991,7(4):489 -4992　XinHu a Zhao.A Success ive App roximation Algorith m for Th e Invers e Position An alys is of Serial M anipulator.Robotica,1999, 17:487-4893　Din esh M anoca.Efficien t Invers e Kinematics for General M anip-ulator s.IEE E T rans of Robotics an d Au tomation,1994,10(5)4　James N iels en.On the Kinematics Analysis of Rob otic M echa-nism s.Th e International Journal of Robotics Resear ch,1999,18(12):1147-11605　袁亚湘,孙文瑜.最优化理论与方法.科学出版社,19976　Burke J,M or J J.Convergence Property Of T rus t Region M eth-ods for Linear and Convex Con strain s.M ath,pr og.,1990,32:305-3367　C on n A R,Gou ld N I M.Global Convergen ce of a C las s of T rus t Region Alg or ith ms for Optimiz ation With S imple Bou nds,SIAM J.N umer.Anal,1988,25:433-4608　蒋新松.机器人导论.辽宁科学技术出版社.19949　An geles J.On Th e Numerical Solu tion for The In vers e Kine-matics Problem.International Jorunal of Rob otics Res earch, 1985,4(2):21-3710刘永超.遗传算法解机械手运动学逆解.机器人,1998,20(6): 421-42611Kazeronian.K.On T he Nu merical Invers e Kin ematics of Robotics M anipu lator.ASM E j M ech anisms T rans mis sions Automation Des ign,1987,109:120-12612　C ox D,Little J,O'Shea D.Ideals,Varieties,and Algorithm (s econ d edition).New York Ver lag13　Wamper C,M organ A,Sommese A.Numerical Continuation M ethods for Solvin g Polynomial Sys tems Arising in Kin ematics.AS M E J.M ech.Design.1990,112:59-6814　Gill P E,M urray W.Quasi New ton M ethods for Unconstrained Optimization.J.Ins t.M ath.Applns,1972,9:91-108作者简介:　马化一(1974-),硕士研究生.研究领域:水下监控式机械手.　张艾群(1959-),男,研究员,研究室主任,硕士生导师.研究领域:水下机器人.　张竺英(1964-),副研究员,硕士生导师.研究领域:水下机器人.141第23卷第2期马化一等:　一种基于优化算法的机械手运动学逆解。