6R工业机器人运动学算法的改进

本资料包含pdf文件1个，下载需要1积分

Improvement of kinematics algorithm of 6 R industrial robotWU Ting，WU Guo-kui，WU Hai-bin(School of Mechanical Engineering and Automation，Fuzhou University，Fuzhou 350108，China)Abstract：Aiming at the intensive computation of kinematics module exist in the real-time control of the industrial robot，inverse kinematicsolutions of vertical-six-joint industrial robot were calculated and analyzed.Then，an algorithm of searching the proper inverse kinematic SO-lution was presented based on the characteristics of the solutions and the geometric structure of the robot.After that，the curve interpolationalgorithm in cartesian coordinates was discussed.In order to make the robot easier to be used，the arbitrary user coordinates was built andthe gesture rotation representation was presented.The algorithms were applied to a 6R manipulator in the laboratory to verify the effective-ness of the proposed algorithms.The results indicate that the algorithm can make the robotic joints move smoothly and avoid mutation whenthe robot is running.especialy in the process of cunre interpolation.The algorithms can make the motion of robot smoother and more accu-rate。

Key words：robot kinematics；inverse optimization；cunre interpolation algorithm；user coordinatesU 引 吾机器人运动学在机器人运动控制中扮演着重要角色。机器人特别是工业机器人主要用于完成-定的作业任务，在完成任务过程中不可避免地要进行关节坐标空间与笛卡尔等坐标空间的相互转换，不可避免地要进行正运动学、逆运动学的求解以及插补算法等运动学方法的实现。机器人运动学的作用不言而喻。

机器人运动学涵盖面较广，本研究只研究其中较为重要的运动学逆解及其优化选娶插补算法及-些常用坐标系的建立方式。目前，运动学逆解的方法主要有数值迭代法、几何法 。]，神经网络法 及遗传算法 等方法。机器人运动学逆解算法复杂，数值迭收稿 日期：2013-02-05基金项目：国家自然科学基金资助项目(51175074)作者简介：吴 挺(1987-)，男，辅莆田人，主要从事机器人技术方面的研究.E-mail：wuting04###yahoo.CB通信联系人：吴海彬，男，博士，教授.E-mail：wuhb###fzu.edu.cn第 7期 吴 挺，等：6R工业机器人运动学算法的改进代法依赖于初始点，收敛速度较慢。几何法依赖于机器人的机构模型。遗传算法涉及大量个体的计算，难以满足控制系统实时性的要求。在对逆解的选取时有学者提出-种基于最短行程”的方法 J，但该方法忽略了机械手当前状态，且需要求出机器人的所有解 ，实时性不高。在插补算法方面也有不少学者提出-些插补算法 ，但-般只介绍位置插补的实现，对于姿态插补却没有给出明确的算法。

本研究采用分离变量法求取逆解，提出-种基于机器人几何结构及解的特点的逆解优化方法，该方法无需求出机器人的所有逆解便可选择出较好的逆解，可节约机时，提高实时性，在插补算法中同时综合位置与姿态插补，以满足轨迹规划中对位置和姿态的要求。

1 工业机器人运动学逆解的求法1.1 逆解求解过程本研究研究的6自由度工业机器人如图 1所示，根据D.H法8 对机器人建立坐标系。为了计算方便，这里在机器人末端三轴交汇处建立了坐标系 Z -‰，并在执行器末端建立了与 - 固连的坐标系 -， 。 因此，坐标系7并非按照 D-H法建立。机器人 D。

图 1 机器人 D.H表示法参考坐标表 1 机器人 D.H参数表依照D-H表示法原理，可得出相邻坐标系间的变化矩阵，记为：A 、A 、A。、A 、A 、A 。6个变换矩阵可按下式计算：Al：Rot(z， 1)×Trans(a1，0，0)×Rot(x，7r/2)A2Rot(z， )×Trans(a2，0，0)A3Rot(z， 3)×Trans(a3，0，0)×Rot(x，1T./2)A4Rot(z，04)×Trans(0，0，d4)×Rot(x，-1T/2)A5Rot(z， 5)×Rot(x，1T/2)A6Rot(z， 6)(1)由于执行器形状的不同，坐标系7的姿态和位置也会不同。但对于-定执行器，坐标系6和坐标系 7的关系是固定的。坐标系7可由坐标系6经过平移变换d(d7 ， ，d7 )，再依次绕自身坐标系的 ，y， 轴旋转3个角度 ， ， ：得到，这些参数可以由执行器的几何结构得到。故变换矩阵A 可表示为：A7Trans(d7 ，d7 ，d )Rot(x， )Rot(y，咖 )Rot(z，咖 )给定末端位姿矩阵 ：Tax pxay PYaz pzO l(2)解方程 TA A A，A A A A 的过程即为求逆解过程。注 意 到 A 为 常 量 矩 阵，可 在 方 程 T A A A3A A A A 两边同时右乘A 的逆矩阵，得到：T1： ax PXay PYaz Pz0 1(3)式(3)中的参数均由 计算得来，这里为了书写方便，将其各用-个参数替代。对A，处理后，相当于将图1中执行器末端坐标系原点等效到了机器人末端三轴交汇处。于是逆解的过程也转化为求 T A A A，A A A 的过程，不必再考虑坐标系Z7- 。

易知手腕 3个转 动关节轴线交 于-点，满 足Pieper给出的具有封闭解的条件 J，故可用如下方法求逆解。由于运动方程中有许多角度的耦合，研究者不能从矩阵中提取足够的元素来求解单个的正弦和余弦来计算角度。为使角度解耦 ，可以用矩阵 左乘矩阵A ，进行变量分离，再对比左右矩阵求得相应角度。

可得如下方程：Af T1A2A3A4A5A6 (4)比较方程左右两边，由两边矩阵中元素(3，4)相等得到：latan2(-Py，- )或 1atan2(PY，PX)(5)· 884· 机 电 工 程 第 30卷考虑式(4)左右两边矩阵中元素(1，4)和(2，4)相等，并计算化简可得：03atan2(W，±√1- )-atan2(a3，d4) (6)(PXC1-a1P ) Pz -a -口；-d；202√0 d考虑式 A A T A A A A 左、右两边矩阵元素(1，4)和(2，4)相等，可得：02-an2( 专 ，(03Js3- C3)Pz(PXC1-n1P，S1)(口3C3 S3a2)、pz2(PXC1-01PYS1) /(7)考虑式 A A A A T。A A 左右两边矩阵中元素(3，3)相等，可得：04atan2(-ayC1axS1，C23(axC1ayS1)$23 )(8)或者：04atan2(-(-ayC1axS1)，-(C23(axC1ayS1)S23az)) (9)考虑式 A A A A T。A A 左右两边矩阵中元素(1，3)和(2，3)相等，可得：05：atan2(C4(C23(axC1ayS1)azS23)54(-ayC1axS1)，523(axC1ayS1)-azC23) (10)考虑式 A A A A T1A5A6左右两边矩阵中元素(3，1)和(3，2)相等可得：06：atan2(C4(-nyC1nxS1)-S4(C23(nxClnyS1)nzS23)，C4(-oyC1oxS1)-54(C23(oxC1oyS1)ozS ，)) (11)1.2 逆解优化选取从以上求取逆解的过程可以看出，对应于某-位置，可以有8组不同的封闭解。但实际的机器人运动控制只能选择其中-组解，并且关节角转角是有范围限制的，同时，在轨迹运动时，如何使各关节变化平缓，减少机器人工作过程中角度突变，也对解的选择提出了要求。

在进行轨迹跟踪时，为了减小关节运动的时间，使各关节变化平稳，应使目标关节角与当前关节角旧能接近∩以采用下式来选取逆解：K∑ l 0 l-0 I (12) 5 、式中：0igoal-第 i关节角的目标关节角，Oi.ow-第 关节角的当前角度。

选取关节角度时，所要的关节角应满足使K值最校但该法需要算出机器人的所有封闭解，从中选择出使 K值为最小的解，这样会浪费机时去求无用解，且算法较为复杂，效率不高。

可以从机器人结构及解的特性来简化算法。在运动中为使机器人有较好的轨迹跟踪精度，则必然要求每次转过的关节角较校从上文求出的0 来看，0 对应的两个解相差 180。，从中可以得到-个与当前 0 较接近的解，另-个解可以直接舍弃。选定了 后，0只有两个解且 0 的两个解相差的角度依赖于 0 的大小，0 完全依赖于0 和 0 ，因此可以求出这两组解，采用下式来选取使 .，最小的解：3I，∑ l 0igoal-0 l (13)i20 的两个解也相差 180。，选择与当前角度相差较小的解，否则从图 1中工业机器人的结构可以看出后面的两个关节角将产生突变，这是不允许的。0 和 0完全撒于前面的解。至此，已选出-组较好的解。

该法可以避免去解不符合要求的解，提高效率，简化算法。具体流程如图2所示。

2 插补算法图2 逆解选取流程度轨迹规划在机器人运动控制中具有重要的作用，对机器人的作业有着重要的影响。本研究所述轨迹规划是在笛卡尔坐标系下进行的，必须运用插补算法并反复求解逆运动学方程来计算关节角。插补算法在轨迹规划中具有重要的地位，因此研究它具有重要的意义。在笛卡尔坐标空问中描述机器人末端执行器，包括位置描述和姿态描述，因此需要对位置进行插补，同时也要对姿态进行插补。

总体原则是：在进行插补时，将起点和终点间的变换分解为-个平移(位置插补)和-个绕空间中某根轴 k的旋转(姿态插补)。平移实现末端执行器坐标系原点的移动，旋转实现末端执行器姿态的改变，两变换同时进行。

2.1 位置插补算法空间曲线的位置插补可以近似地等效为由直线段第 7期 吴 挺 ，等：6R工业机器人运动学算法的改进和圆弧段所组成，对应的位置插补就是直线插补和圆弧插补。

对直线插补，设直线的起点坐标为A( ， ， )，终点坐标为 ( ， ， )，选定插补次数Ⅳ，则有：t (、xB-XA、)/N△y( - )/Ⅳ (14) (、z B-zA /N于是直线的插补点( ，Y ， )可以表示为：， XAlAXY y (15) ： 其中：0≤ ≤Ⅳ。

对圆弧插补，设该圆弧为由空间中任意不共线的3点A、B、C所组成的，示意图如图3所示。

图3 圆弧插补示意图易得线段 A 的中垂面、线段 BC的中垂面如下：( -( A 口)/2)( 口- A)(Y-(YaYB)/2)( - )(z-( )/2)( -zA)： (16)( -( c 口)/2)( c- 日)(Y-(YcyB)/2)(Yc-YB)( -(zcz口)/2)(zc-z口)0l - c y-yc - c ll 。- Y。-Y。 z。- l0 (17)1 1 l6- Yb-Yc Zb-z l联立以上 3个方程，解方程便可得到圆弧的圆心坐标 x。，y。， )，进而可得到圆浑径 和圆心角 0。

给定插补次数 Ⅳ，则可得步距角6O/N。为求插补点坐标可按图3所示过 点做切线 P P ，进而得到圆弧上的点 P ，该递推公式已有学者推出6 J。

2.2 姿态插补算法根据曲线特点可将姿态插补分为有中间点约束的姿态插补(如圆弧插补)和无中间点约束的姿态插补(如直线插补)。

对无中间点约束的插补，设起点到终点的姿态转换矩阵为 ，并将 扩充为-个齐次矩阵R。设 为：R a 0ay 0a 00 1(18)则可将该旋转变换 (矩阵 )转化为绕空间中某个 k轴旋转 0角得到，得到的 k轴单位向量 ( ， ，)和转角 0可表示如下 m ：r 1(O -a )/2sin07 M2(口 -n；)/2sin0 (19) 3(n -O )/2sin00：arctanf(0：-a ) (a -n ) (n -0 )此外，可以得到绕 k轴转过某个角度 0 得到的转厂 cos01(1-M ) /X1u2(1-cos01)-U3sin01 1 3(1-cosO1)M2sin01]T(01)I 1 2(1-cosO1)U3sin01 u；cosO1(1- ) U3M2(1-cosO1)- l sin0l I (21)L 1 3(1-cosO1)-U2sin01 U3u2(1-cosO1)U1 sin01 ；cosO1(1- ；) j于是姿态的插补便可转化为转角 0的插补，插补 0 2aresin((( - ) (Y -YA) ( -次数为Ⅳ，则有：AO：0/N (22)插补点的姿态可以表示为： ( ) (23)式中：0≤ ≤Ⅳ， -直线初始点的姿态。

将插补点的位置与姿态结合起来，便可得到插补点的位姿矩阵，再利用逆运动学方程便可解出各个关节角。

对有中间点约束的姿态插补，如图3中的圆弧插补，考虑中间点 的姿态，可按下述方法进行。由下式可得AB弧所对圆心角 OA ：) ) /(2R)) (24)对 NOA /0取整，可得AB弧的插补次数 ，BC弧插补次数 。N- 。由此便可分别对AB弧和BC圾行姿态插补，方法同直线插补。

2.3 位置和姿态插补综合轨迹插补需要有位置和姿态信息才能进行，需要将插补点的位置与姿态结合起来，得到插补点的位姿矩阵，再用逆运动学方程便可解出各个关节角。由上文可以得到插补点的位置P ( ，Y ， ) ，及姿态矩阵· 886· 机 电 工 程 第30卷T ，由此便可得到插补点的位姿矩阵R ：Ri。 。 (25 00 01 J )3 不同位姿表达方式之间的转换工业机器人的运动学控制，位姿状态的表述通常采用位姿矩阵，但是位姿矩阵中姿态参数较多且不相互独立，在做速度或力矩控制时，往往由于雅克比矩阵无法从位姿矩阵直接通过计算得到而带来麻烦。而如果采用相互独立的6参数表示机器人末端执行器的位姿状态，并建立该 6参数分别与 6个关节角之间的函数关系式，则可以较方便地得到雅克比矩阵。这 6个参数分别是： ， ， 表示执行器坐标系原点坐标，3个角度 。， ， ，分别表示绕参考坐标系的 ，)，， 轴转过的角度。它可以较为形象地表达机器人的位姿。

3.1 由旋转参数转化为位姿矩阵记 Rot(d， )表示为绕 d轴旋转 角得到的位姿矩阵。则按顺序依次绕参考坐标系 ，Y， 轴旋转后所得的的矩阵R可按下式计算：R：Rot(z， 1)Rot(Y， 2)Rot( ， 3) (26)3个位置参数可以看成，旋转后再相对于参考坐标系平移(d ，d ，d )，于是最终的位姿可按下式计算：RTrans(P ，P ，P：)Rot(z， 1)Rot(Y， 2)Rot( ， 3)(27)3.2 由位姿矩阵转化为旋转参数设机器人位姿矩阵为 ：Tn on y O，n ： 0z0 0a Pay Pya P：0 1(28)则有 TR，比较矩阵两边元素，可得：fd pI dypyI d pI atan2(T(3，2)， (3，3))l 2atan2(-T(3，1)，(T(1，1)cosq3r(2，1)sinq3)) 3atan2( (2，1)，T(1，1))(29)式中：T(a，b)-矩阵 中第 a行第 b列元素。

此外，矩阵 可由机器人正运动学得到，设 6个关节角分别为 ，， ， ，64， ，06，则矩阵 中元素均可表示为 ， ， ，，04， ， 的函数～其代人式(25)中，可得：d ( l， 2， 3， 4， 5， 6)d ( 1， 2， 3， 4， 5，06)z ( 1， 2， 3，04，05，06) f301 ( 1， 2， 3， 4，05， 6) 2 ( 1， 2， 3， 4， 5， 6)3厂6( l， 2， 3， 4， 5，06)对式(26)求微分，则可直接得到雅克比矩阵，并可用于机器人的速度控制。

4 用户坐标系的建立对用户来说，工业机器人参考坐标系的建法未必很明确和实用，参考坐标系往往不方便于空间定位，且随着环境改变，描述操作对象的坐标系也应随之而变。

为了能够方便地操作机器人，更好地实现工作空间中的定位，更好地规划机械手的运动，有必要找到-个能够建立坐标系并实现其与机器人参考坐标系相互转化的方法。

为了建立这样-个坐标系，用户需要给定 3个适当的点 ，设为 A，B，c，其中点 A为用户要建立的坐标系原点，B为用户建立的坐标系 X轴正向上-点，c为用户建立坐标系的 y平面中的-点。用户坐标系的建立如图4所示。

图4 用户坐标系的建立由此可得用户坐标系的原点为 A点， 轴为向量AB，方向由A指向B，z可由A × C得到，进而可以由之× 得到 。这样便可得到用户坐标系在参考坐标系中的位姿矩阵。

得到用户坐标系后，就可以比较方便地确定空间中的点在该坐标系下的坐标。为了让机器人识别这些点，需要研究用户坐标系与机器人参考坐标系的相互转化。设用户坐标系为 u，机器人参考坐标系为 5，用符号 表示坐标系 相对于坐标系 .s的转化矩阵。

已知用户坐标系下的点 P，可由式 P T，× P转化为机器 人参 考 坐标 系下 的点 的坐 标。同理 由 P× P可将机器人参考坐标系下的点转化为用户坐标系下。若已知用户坐标系下的位姿矩阵 ，可由第 7期 吴 挺 ，等：6R工业机器人运动学算法的改进式 Tn T X 得到其在机器人参考坐标系下的位姿矩阵。由式 TR X 可将机器人参考系下的位姿转化为用户坐标系下的位姿。

5 实 验实验借助-台工作半径为650 mln的垂直6关节工业机械手，将以上算法编成动态链接库，通过VC程序调用动态链接库来控制机械手的运动。在机械手做直线插补过程中，采集空间位置坐标和 6个关节速度，并与原出厂程序控制的机械手的位置和速度进行对比。采集得到的位置坐标如图5所示。本研究算法得出的轨迹如图5(a)所示，机械手的出厂程序得出的轨迹如图5(b)所示。

f/sfa本研究算法下的坐标轨迹t/s(b1原算法下的坐标轨迹图5 空间坐标轨迹对比图从图5可以看出，图5(a)的直线跟踪能力比图5(b)强。由此可得，改进后的逆运动学和插补算法是可行的，并且比原算法具有更强的直线轨迹跟踪能力。

机械手在做直线插补时前 4个关节的速度曲线图如图6所示，采用本研究提出的方法得到的曲线如图6(a)所示，采用原出厂程序得到的曲线如图6(b)所示。

图6(a)、6(b)所对应的笛卡尔空间轨迹是完全- 样的。从图6(b)可以看到，在-段较长时间内，第四轴的速度要比其他三轴速度大很多，因此，第四轴每个插补点所转过的角度比其他轴都大很多，有可能产t/ms(a)本算法下的各轴速度曲线t/ms(b)原算法下的各轴速度曲线图 6 各轴速度曲线对 比图生突变，导致插补的精度下降，同时插补时间也明显增加(约为图6(a)的2倍)。图6(a)中各轴速度较为均匀，不会出现某-轴速度长时间大于其他轴，由此可知各轴插补角度较为均匀，有效地减少了角度突变的可能。由于各插补角度较为均匀，可实现较小步距角的插补，有利于提高精度。

6 结束语本研究针对6自由度工业机器人，采用分离变量法求其运动学逆解，并根据逆解的特点和机器人的几何结构从逆解中选取最优逆解，将曲线插补中的姿态插补转化为对绕空间中的某根轴旋转的角度的插补，并将以上逆解的改进算法运用到插补算法中。实验结果表明，机器人曲线插补更为精确，有效地避免了各关节角度突变。笔者给出了6参数位姿表示法与位姿矩阵之间的变换算法，由此可以得到雅克比矩阵，用于速度或力矩控制。用户坐标系的建立，使机器人运动空间的定位更加符合和方便用户需要，极大方便了用户的使用。

改进后的算法也存在-些缺点，当机械手的第四轴的旋转范围小于(-180。，180。)