六轴运动机器人运动学求解分析_第一讲.doc

六轴联动机械臂运动学求解分析第一讲作者朱森光Emailzsgsoft@1引言笔者研究六轴联动机械臂源于当前的机器人产业热，平时比较关注当前热门产业的发展方向笔者工作主要从事软件开发跟机器人毫无关系，利用业余时间研究整 理机器人技术相关的文章，希望能够起到抛砖引玉的作用引发更多的人发表有关机器人技术的原创性技术资料本系列文章的所有文字、图片及相关资料均为原创，内 容正确性经过笔者亲自编程仿真验证可以信赖2机器建模2.1坐标系既然要研究机器人，那么首先要建立一个机械模型，本文将以典型的六轴联动机器臂为例进行介绍，图 2-1为笔者使用3D技术建立的一个简单模型首先建立一个大地坐标系，一般教科书上都是以大地为 XY平面，垂直于大地向上方向为 Z轴，本文为了跟教科书上有所区别同时不失一般性，将以水平向右方向为 X轴，垂直于大地向上方向为 Y轴，背离机器人面向人眼的方向为 Z轴，移到电脑屏幕上那就是屏幕水平向右为 X轴，屏幕水平向上为 Y轴，垂直于屏幕向外为 Z轴，之所以建立这样不合常规的坐标系是希望能够突破常规的思维定势训练在任意空间建立任意坐标系的能力图2-1图2-1中的机械臂，灰色立方体为机械臂底座，定义为关节 1,它能绕图中Y轴旋转；青色为关节 2，它能绕图中的Z1轴旋转；蓝色为关节 3,它能绕图中的Z2轴旋转；绿色为关节 4，它能绕图中的X3轴旋转；红色为关节 5，它能绕图中的Z4轴旋转；黄色为关节 6，它能绕图中的X5轴旋转。

2.2齐次变换矩阵齐次变换矩阵是机器人技术里最重要的数学分析工具之一，关于齐次变换矩阵的原理很多教科书中已经描述在此不再详述，这里仅针对图 2-1的机械臂写出齐次变换矩阵的生成过程首先定义一些变量符号，关节 1绕图中Y轴旋转的角度定义为B 0,当B 0=0 时，O1点在OXYZ坐标系内的坐标是（x0,y0,0）;关节 2绕图中的Z1轴旋转的角度定义为B 1,图中的B1 当前位置值为+90度；定义O1O2两点距离为x1,关节3绕图中的Z2轴旋转的角度定义为B 2,图中的B2 当前位置值为-90度； O2O3两点距离为x2,关节4绕图中的X3轴旋转的角度定义为B 3,图中的B3 当前位置值为-60度；O3O4两点距离为x3,关节5绕图中的Z4轴旋转的角度定义为B 4,图中的B4当前位置值为-60度；O4O5两点距离为x4,关节6绕图中的X5轴旋转的角度定义为B 5,图中的B5 当前位置值为+60度以上定义中角度正负值定义符合 右手法则符号定义好了，接下来描述齐次变换矩阵定义R10为关节1绕Y轴的旋转矩阵R0=[c00 s00 〃c0 = cos 0 0 s0 = sin 0 001 00-s00 c0000 0 1]定义T0为坐标系O1X1Y1Z1相对坐标系OXYZ的平移矩阵T0=[1 0 0 x00 1 0 y00 0 1 00 0 0 1]定义R1为关节2绕Z1轴的旋转矩阵R1=[c1 — s1 0 0s1 c1 0 00 0 1 0 0 0 0 1]定义T1为坐标系O2X2Y2Z2相对坐标系 01X1Y1Z啲平移矩阵T1=[1 0 0 x11 1 0 01 0 1 00 0 0 1]定义R2为关节3绕Z2轴的旋转矩阵R2=[c2 - s2 0 0s2 c2 0 00 0 1 00 0 0 1]定义T2为坐标系O3X3Y3Z3相对坐标系O2X2Y2Z2的平移矩阵 T2=[1 0 0 x20 1 0 00 0 1 00 0 0 1]定义R3为关节4绕X3轴的旋转矩阵R3=[1 0 0 00 c3 - s00 s3 c3 00 0 0 1]定义T3为坐标系O4X4Y4Z4相对坐标系O3X3Y3Z3的平移矩阵 T3=[1 0 0 x30 1 0 00 0 1 00 0 0 1]定义R4为关节5绕Z4轴的旋转矩阵R4=[c4 - s4 0 0s4 c4 0 00 0 1 0 0 0 0 1]O4X4Y4Z4勺平移矩阵定义T4为坐标系 O5X5Y5Z5相对坐标系T4=[1 0 0 x40 1 0 00 0 1 00 0 0 1]定义R5为关节6绕X5轴的旋转矩阵R5=[1 0 0 00 c5 - s00 s5 c5 00 0 0 1]以上矩阵定义中 cO、cl、c2、c3、c4、c5 分别为 cos 0 0、cos0 1、cos 0 2、cos 0 3、cos0 4、cos 0 5 的简写，s0、si、s2、s3、s4、s5分别为 sin 0 0、sin 0 1、sin 0 2、sin 0 3、sin 0 4、sin 0 5 的简写。

至此最终的齐次变换矩阵就可以写出来了，那就是： C=R0*T0*R1*T1*R2*T2*R3*T3*R4*T4*R52.3 正运动学求解正运动学求解就是求出 2.2 节中齐次变换矩阵 C 的解析表达式，下面求解C=R0*T0*R1*T1*R2*T2*R3*T3*R4*T4*R5= [Nx Ox Ax Px Ny Oy Ay PyNz Oz Az Pz 0 0 0 1]这里要注意矩阵乘法满足结合律但不满足交换律，所以你可以先单独求出 R4*T4，R3*T3，R2*T2，R1*T1，R0*T0 然后再将它们相乘，即C= (R0*T0)*(R1*T1)*(R2*T2)*(R3*T3)*(R4*T4)*R5最终得出结果如下：Nx=c0c1(c2c4-c3s2s4)-c0s1(s2c4+c2c3s4)+s0s3s4Ny= s1(c2c4-c3s2s4)+c1(s2c4+c2c3s4)Nz= -s0c1(c2c4-c3s2s4)+s0s1(s2c4+c2c3s4)+c0s3s4Ox= C0c1(-s4c2c5-s2c3c4c5+s2s3s5) - C0s1(-s2s4c5+c2c3c4c5-c2s3s5)+s0(s3c4c5+c3s5)Oy= s1(-s4c2c5-s2c3c4c5+s2s3s5) +c1(-s2s4c5+c2c3c4c5-c2s3s5)Oz= -s0c1(-s4c2c5-s2c3c4c5+s2s3s5)+s0s1(-s2s4c5+c2c3c4c5-c2s3s5)+c0(s3c4c5+c3s5) Ax=c0c1(c2s4s5+c3c4s2s5+c5s2s3)-c0s1(s2s4s5-c2c3c4s5-c2c5s3)+s0(-c4s3s5+c3c5) Ay= s1(c2s4s5+c3c4s2s5+c5s2s3) +c1(s2s4s5-c2c3c4s5-c2c5s3)Az=-s0c1(c2s4s5+c3c4s2s5+c5s2s3)+s0s1(s2s4s5-c2c3c4s5-c2c5s3)+c0(-c4s3s5+c3c5)Px=c0c1(x4c2c4-x4c3s2s4+x3c2+x2c2)-c0s1(x4c4s2+x4c2c3s4+x3s2+x2s2)+x1c0c1+x4s0s3s4+x0c0Py= Sl(x4c2c4-x4c3s2s4+x3c2+x2c2)+x1s1+c1(x4c4s2+x4c2c3s4+x3s2+x2s2)+yoPz=-s0c1(x4c2c4-x4c3s2s4+x3c2+x2c2)+s0s1(x4c4s2+x4c2c3s4+x3s2+x2s2)+x1s0c1-x0s0矩阵C就是最终的六轴联动机械臂的齐次变换矩阵，如果机械手末端相对于坐标系 O5X5Y5Z5的相对坐标为 U(a,b,c)，那么末端U在大地坐标系 OXYZ中的坐标为：XuYuZu=Nx Ox Ax PxNy Oy Ay PyXabcNz Oz AzPz10 0 011图2-2以上就是机器人正运动学的求解， Nx、Ny、Nz、Ox、Oy、Oz、Ax、Ay、Az、Px、Py、Pz表达式中的x0、y0、x1、x2、x3、x4为机械固有尺寸，B 0、B 1、0 2、B 3、B 4、B5为六个关节的旋转角。

实际上 C的子矩阵M=[Nx Ox AxNy Oy AyNz Oz Az]反应的就是末端坐标系 O5X5Y5Z5的姿态，子矩阵M实际上就是轴 O5X5、O5Y5、O5Z5在大地坐标系 OXYZ下的方向余弦，而(Px,Py,Pz)就是点O5在大地坐标 系OXYZ下的绝对坐标2.4逆运动学求解机器人逆运动学求解是根据末端位姿矩阵C反求六个关节的旋转角0 0、C=R0*T0*R1*T1*R2*T2*R3*T3*R4*T4*R50 2、 0 3、 0 4、 0 5的问题为了便于求解，这儿对S0的逆S0'得到：其中S0'=[C0 0-s0-x00 10-y0s00c000001]等式左边S0' * C=[c0Nx-s0Nzc0Ox-s0Ozc0Ax-s0Azc0Px-s0Pz-x0NyOyAyPy-y0S0Nx+c0Nzs0Ox+c0Ozs0Ax+c0Azs0Px+c0Pz0001]寺式右边R1*T1*R2*T2*R3*T3*R4*T4*R5=等式进行变换，令 S0=R0*T0，然后将等式两边同时左乘S0' *C=R1*T1*R2*T2*R3*T3*R4*T4*R5[C1(c2c4-c3s2s4)-s1(s2c4+c2c3s4)c1(-s4c2c5-s2c3c4c5+s2s3s5)-s1(-s2s4c5+c2c3c4c5-c2s3s5)C1(c2s4s5+c3c4s2s5+c5s2s3)-S1(s2s4s5-c2c3c4s5-c2c5s3)S1(c2c4-c3s2s4)+c1(s2c4+c2c3s4) s1(-s4c2c5-s2c3c4c5+s2s3s5)+c1(-s2s4c5+c2c3c4c5-c2s3s5)S1(c2s4s5+c3c4s2s5+c5s2s3)+C1(s2s4s5-c2c3c4s5-c2c5s3)S3s4s3c4c5+c3s5-c4s3s5+c3c50 0 0等式左右两个矩阵内对应元素相等于是就得到如下方程组：1 C1(c2c4-c3s2s4)-s1(s2c4+c2c3s4)= cONx-sONz2 S1(c2c4-c3s2s4)+c1(s2c4+c2c3s4)= Ny3 s3s4= S0Nx+c0Nz4 c1(-s4c2c5-s2c3c4c5+s2s3s5) -s1(-s2s4c5+c2c3c4c5-c2s3s5)= cOOx-sOOz5 s1(-s4c2c5-s2c3c4c5+s2s3s5) +c1(-s2s4c5+c2c3c4c5-c2s3s5)= Oy6 s3c4c5+c3s5= sOOx+cOOz7 C1(c2s4s5+c3c4s2s5+c5s2s3)-s1(s2s4s5-c2c3c4s5-c2c5s3)= cOAx-sOAz8 S1(c2s4s5+c3c4s2s5+c5s2s3) +c1(s2s4s5-c2c3c4s5-c2c5s3)=Ay9 -c4s3s5+c3c5= s0Ax+c0Az10 C1(x4c2c4。