徐 培1，马铁军1,2，林丽红1
(1.华南理工大学机械与汽车工程学院，广东 广州 510640)(2.广州华工百川科技股份有限公司，广东 广州 510530)
摘要：提出一种对做PTP运动的6-R机械臂求出其所有关节空间逆解，并优选出相对于起始点行程最短的一组逆解的算法。首先通过D-H方法建立PUMA560的数学模型，然后采用解析法求关节逆解；其次通过计算起点与终点逆解向量之差的p范数来表示关节行程大小，取关节行程最小的一组解作为终点的最短行程逆解；最后采用MATLAB Robotics工具箱做PUMA560关节逆解仿真计算，验证所求行程最短逆解的正确性。结果表明：该算法所求的逆解结果正确，与数值迭代算法求解结果相比，计算结果更优，适合作为最少转角的机械臂逆解优选算法。
关键词：运动学；最短行程；逆解；6-R机械臂
逆运动学在机器人学中占有非常重要的地位，是机器人轨迹规划和运动控制的基础，直接影响着控制的快速性与准确性[1-2]。目前，求解串联机器人位置逆解的主要方法有球面三角法[3]、齐次坐标变换矩阵法[4-6]、四元数法[7-8]等，但对机器人逆运动学多重解问题的研究较少。一般情况下，连杆的非零参数越多，达到某一特定目标的方式也越多。对于一个全部为旋转关节的6自由度操作臂来说，可能多达16种解[9-10]。对于3个相邻的轴交于一点的6自由度操作臂，最多有8 组逆解[11]。但在实际工作中，机器人系统最终只能选择一种最优解来执行任务[12]。这就提出了一个问题，对于多重逆解的情况该如何选择。常用的解决方法是在满足避障的条件下，选择行程最短的逆解作为最优逆解。研究实时性的逆解优选算法对机器人的自动轨迹规划具有重要意义。
1.1 D-H矩阵及运动学方程
D-H矩阵[13]是1995年由DENAVIT和HARTENBERG提出的一种为关节链中的每一杆件建立坐标系的矩阵方法。机械臂的第(i-1)根连杆可以用4个参数ai-1,αi-1,di,θi来描述，其中2个参数ai-1和αi-1用于描述连杆本身的特征，其数值的大小是由Zi-1和Zi两轴之间的距离和夹角来决定的；另外2个参数di和θi用于描述连杆之间的连接关系，其数值的大小是由Xi-1和Xi两轴之间的距离和夹角来决定的(其中i为关节编号，取值为1,2,3,4,5,6)。相邻的两个关节坐标系变换矩阵为
(1)
式中：cθi表示cosθi，sθi表示sinθi，余者类推。采用D-H矩阵法建立PUMA560操作臂坐标系分布图，如图1、图2所示。
图1 PUMA560运动参数和坐标系分布
图2 PUMA560前臂运动参数和坐标系分布
关节i取值1～6，分别可得相邻杆件的齐次变换矩
(2)
1.2 运动学逆解
对于通用6-R关节机器人，在结构设计上满足存在封闭解的条件，即相邻的3个关节轴线相交于一点，可采用解析法求解所有运动学逆解。数值迭代算法需要计算机械臂雅克比逆矩阵，计算量大，不适合作为在线求解运动学逆解的算法。
这里采用解析法求PUMA560机械臂的逆解，求解结果用关节坐标矢量An(θ1,θ2,θ3,θ4,θ5,θ6)表示，其中θi表示第i个关节的角度值。机械臂末端运动轨迹上某一点的位姿矩阵
(3)
式中：PX，PY，PZ分别为机械臂末端关节坐标原点在机械臂基座坐标系中X，Y，Z轴上的坐标；
在不考虑关节转角范围约束的条件下，通过逆运动学求解可以求出8组逆解，而实际运动中机械臂末端运动到某一点时，每个关节转动的角度是确定的。这8组逆解理论上都能使机械臂末端达到同一个位姿，只是不同的解到达终点所需的各关节转角不同。通过计算终点与起始点关节角矢量之差的p范数来描述关节空间行程的大小。设末端运动起始点的关节坐标矢量
‖A0-An‖p=
(4)
式中:A0为已知量；p取值1,2,3,…；An为待求终点逆解矢量，分别取值
将机械臂运动终点的8组关节逆解分别代入式(4)，计算‖A0-An‖p的值，‖A0-An‖p越小，则两矢量间距越小。
3.1 PUMA 560逆解优选
通过对Unimation PUMA 560机械臂的仿真试验来说明本文算法的功能。程序在MATLAB上编写。PUMA 560的D-H连杆参数见表1。
表1 PUMA 560机械臂连杆参数[6]
关节αi-1/(°)ai-1/mmdi/mmθi/(°)100002-9000030431.8-13.404-90-20.3433.605900006-90000
当一个通用6轴关节机械臂的D-H参数和机械臂末端位姿矩阵给定后，通过解析法可以求出8组关节逆解。选取机械臂末端运动轨迹上的两个点，起始点关节坐标矢量为A0，起始点位姿矩阵为T0；终点关节坐标矢量为An，终点位姿矩阵为Tn。
A0=(0,0,0,0,0,0)
将位姿矩阵Tn输入MATLAB逆解求解算法中，可求出8组关节逆解
表2 PUMA560逆运动学求解结果
kθ1θ2θ3θ4θ5θ6‖Akn-A0‖211.440176-0.991270-0.1152802.0362901.643961-1.4640203.4731022-1.7448002.625656-0.115280-1.3748301.152387-0.1093203.63067031.4401760.515937-3.1198801.6888861.113977-0.0114604.0208624-1.744800-2.150140-3.119880-1.1341601.718981-1.5320404.89789751.440176-0.991270-0.115280-1.105120-1.6439601.6775743.1317976-1.7448002.625656-0.1152801.766766-1.1523903.0322734.85755271.4401760.515937-3.119880-1.452520-1.1139803.1301345.0221938-1.744800-2.150140-3.1198802.007436-1.7189801.6095525.193881
由表2可知,第5组逆解处‖
3.2 PUMA 560逆解验证
通过Robotics tools建立PUMA560运动学模型，验证表2中的求解结果。机械臂创建代码如下
L1=link([0 0 0 0 0],′modified');
L2=link([-pi/2 0 0 0 0],′modified');
L3=link([0 0.4318 0 -0.0134 0],′modified′);
L4=link([-pi/2 -0.0203 0 0.4336 0],′modified′);
L5=link([pi/2 0 0 0 0],′modified′);
L6=link([-pi/2 0 0 0 0],′modified′);
r=robot({L1 L2 L3 L4 L5 L6});
r.name=′PUMA560′;
drivebot(r);
其中L1，L2，…，L6分别为机械臂的6个关节向量。
Robotics工具箱的逆运动学求解函数“ikine”采用的是数值迭代法求逆解，正运动学求解函数“fkine”采用齐次变换矩阵代数运算求机械臂末端位姿矩阵。调用工具箱命令
验证第k组解的正确性，其中Tr为机械臂末端执行器位姿矩阵。结果表明，在误差允许的范围内，以上8组逆解均为正确的逆解。调用命令
Q=ikine(r,Tn,A0)
可求出局部最优逆解，其中Q为待求解的机械臂关节角向量。求解结果为
Q=(1.440 2,-0.991 2,-0.115 2,2.036 3,1.644 0,-1.462 4)
Q与表2中
本文提出的6自由度机械臂逆解优选算法通过了实例验证，结果表明此方法可以从机械臂的多重逆解中快速获取关节转动量最少的一组逆解，比雅克比迭代算法求解结果更优。实现了只需输入机械臂末端的位置和姿态，就可以自动求解各关节角变量，并通过逆解优选选出相对于起始点的最短行程的最优解，可作为关节机器人实时性逆解优选算法，为下一步最短行程的关节轨迹规划提供了重要参考依据。在下一阶段，本研究将通过该算法对连续的笛卡尔轨迹求出一系列关节逆解，并通过样条插值获取最短行程的关节轨迹。
参考文献：
[1] 叶上高,刘电霆.机器人运动学逆解及奇异和多解的处理[J].机床与液压,2014(3):27-29.
[2] 刘华山,朱世强,吴剑波,等.基于奇异摄动理论的输入有界机器人轨迹跟踪控制[J].控制理论与应用,2009,26(12):1371-1377.
[3] 王晓星.用球面向量法解机械手逆运动学问题[J].机器人,1988(1):1-8,13.
[4] 张翔,李立君,易春峰,等.基于 D-H 法油茶果采摘机器人正运动学分析[J].农机化研究,2014(4):25-28.
[5] 哈乐,王东辉.基于D-H参数方法的微创手术机器人运动仿真研究[J].医疗卫生装备,2014,35(11):19-21.
[6] 梁安阳,王兰美.基于D-H坐标系法的移动喷漆机器人运动学分析[J].机械工程与自动化,2011(5):125-127.
[7] 倪振松,廖启征,吴莘馨.基于四元数矩阵与Groebner基的6R机器人运动学逆解算法[J].清华大学学报(自然科学版),2013(5):683-687.
[8] MARCHIAFAVA S,PICCINNI P,PONTECORVO M.Quatemionic Structures in Mathematics and Physics[M].Rome,Italy: World Scientific,2001.
[9] MANOCHA D,CANNY J F.Efficient inverse kinematics for general 6R manipulators[J].IEEE Transactions on Robotics & Automation,1994,10(5):648-657.
[10] TSAI L W,MORGAN A P.Solving the kinematics of the most general six- and five -degree- of-freedom manipulators by continuation methods[J].Journal of Mechanical Design,1985,107(2):189-200.
[11] WAMPLER C W.Manipulator inverse kinematic solutions based on vector formulations and damped least-squares methods[J].IEEE Transactions on Systems Man & Cybernetics,1986,16(1):93-101.
[12] 熊有伦.机器人技术基础[M].武汉:华中科技大学出版社，1996.
[13] HARTENBERG R S,DENAVIT J.A kinematic notation for lower pair mechanisms based on matrices[J] .Journal of Applied Mechanics,1955,77:215-221.
[14] CRAIG J J.Introduction to robotics mechanics & control[J].Automatica,1986,14(1):125-126.
XU Pei1，MA Tiejun1,2，LIN Lihong1
(1.School of Mechanical and Automotive Engineering, South China University of Technology,Guangdong Guangzhou,510640,China)(2.Guangzhou SCUT Bestry Technology Co.,Guangdong Guangzhou,510530,China)
Abstract：It proposes an algorithm for obtaining all solutions of inverse kinematics of a general 6-R manipulator,and optimizes the inverse solutions with shortest distance of the starting point.It establishes the mathematical model of PUMA560 through the D-H method,uses the analytic method to obtain the inverse solution.Based on the “P norm” of the difference between the starting point and the end point of the inverse solution vector,it provides the best solution such as the joint angle vector with the minimum “P norm”.It uses MATLAB Robotics toolbox to verify the inverse solutions through simulation of PUMA560.The results show that the inverse solution is correct and better than that of numerical iterative algorithm.
Key words：kinematics; shortest path; inverse solution; 6-R manipulator
DOI：10.3969/j.issn.2095-509X.2017.01.006
收稿日期：2016-11-22
基金项目：国家自然科学基金资助项目(51278201)
作者简介：徐培(1991—)，男，湖北随州人，华南理工大学硕士研究生，主要从事机器人系统及自动化装备方面的研究。
中图分类号：TP242
文献标识码：A
文章编号：2095-509X(2017)01-0028-04
联系客服
