六轴联动机械臂运动学求解分析第一讲作者朱森光Email zsgsoft@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轴旋转的角度定义为θ0，当θ0=0时，O1点在OXYZ坐标系内的坐标是(x0,y0,0)；关节2绕图中的Z1轴旋转的角度定义为θ1，图中的θ1当前位置值为+90度；定义O1O2两点距离为x1,关节3绕图中的Z2轴旋转的角度定义为θ2，图中的θ2当前位置值为-90度；O2O3两点距离为x2，关节4绕图中的X3轴旋转的角度定义为θ3, 图中的θ3当前位置值为-60度；O3O4两点距离为x3，关节5绕图中的Z4轴旋转的角度定义为θ4, 图中的θ4当前位置值为-60度；O4O5两点距离为x4，关节6绕图中的X5轴旋转的角度定义为θ5, 图中的θ5当前位置值为+60度。
以上定义中角度正负值定义符合右手法则。
符号定义好了，接下来描述齐次变换矩阵。
定义R0为关节1绕Y轴的旋转矩阵cosθ0 s0 = sinθ0=//c0R0=[c0 0 s0 00 1 0 00 c0 0-s00 0 0 1]定义T0为坐标系O1X1Y1Z1相对坐标系OXYZ的平移矩阵T0=[1 0 0 x00 1 0 y000 1 00 0 0 1]定义R1为关节2绕Z1轴的旋转矩阵R1=[c1 –s1 0 0s1 c1 0 00 0 1 00 0 0 1]定义T1为坐标系O2X2Y2Z2相对坐标系O1X1Y1Z1的平移矩阵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 –s3 00 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 00 0 0 1]定义T4为坐标系O5X5Y5Z5相对坐标系O4X4Y4Z4的平移矩阵T4=[1 0 0 x40 1 0 00 0 1 00 0 0 1]定义R5为关节6绕X5轴的旋转矩阵R5=[1 0 0 00 c5 –s5 00 s5 c5 00 0 0 1]以上矩阵定义中c0、c1、c2、c3、c4、c5分别为cosθ0、cosθ1、cosθ2、cosθ3、cosθ4、cosθ5的简写，s0、s1、s2、s3、s4、s5分别为sinθ0、sinθ1、 sinθ2、sinθ3、sinθ4、sinθ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 PxNy Oy Ay PyNz Oz Az Pz0001]这里要注意矩阵乘法满足结合律但不满足交换律，所以你可以先单独求出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)+s0s3s4s1(c2c4-c3s2s4)+c1(s2c4+c2c3s4)Ny=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+x0c0s1(x4c2c4-x4c3s2s4+x3c2+x2c2)+x1s1+c1(x4c4s2+x4c2c3s4+x3s2+x2s2)+y0Py=Pz=-s0c1(x4c2c4-x4c3s2s4+x3c2+x2c2)+s0s1(x4c4s2+x4c2c3s4+x3s2+x2s2)+x1s0c1-x0s0矩阵C就是最终的六轴联动机械臂的齐次变换矩阵，如果机械手末端相对于坐标系O5X5Y5Z5的相对坐标为U(a,b,c)，那么末端U在大地坐标系OXYZ中的坐标为：图2-2以上就是机器人正运动学的求解，Nx、Ny、Nz、Ox、Oy、Oz、Ax、Ay、Az、Px、Py、Pz表达式中的x0、y0、x1、x2、x3、x4为机械固有尺寸，θ0、θ1、θ2、θ3、θ4、θ5为六个关节的旋转角。
实际上C的子矩阵M=[Nx Ox AxNy Oy AyNz Oz Az]反应的就是末端坐标系O5X5Y5Z5的姿态，子矩阵M实际上就是轴O5X5、O5Y5、O5Z5在大地坐标系OXYZ下的方向余弦，而(Px,Py,Pz)就是点O5在大地坐标系OXYZ下的绝对坐标。
2.4逆运动学求解机器人逆运动学求解是根据末端位姿矩阵C反求六个关节的旋转角θ0、θ1、θ2、θ3、θ4、θ5的问题。
为了便于求解，这儿对C=R0*T0*R1*T1*R2*T2*R3*T3*R4*T4*R5等式进行变换，令S0=R0*T0，然后将等式两边同时左乘S0的逆S0′得到：S0′*C=R1*T1*R2*T2*R3*T3*R4*T4*R5其中S0′=[c0 0 -s0 -x00 1 0 -y0s0 0 c0 00 0 0 1]c0Ox-s0Oz c0Ax-s0Az c0Px-s0Pz-x0[c0Nx-s0Nz等式左边S0′* C=Ny Oy Ay Py-y0S0Nx+c0Nz s0Ox+c0Oz s0Ax+c0Az s0Px+c0Pz0 0 0 1]等式右边R1*T1*R2*T2*R3*T3*R4*T4*R5=[c1(c2c4-c3s2s4)-s1(s2c4+c2c3s4) c1(-s4c2c5-s2c3c4c5+s2s3s5) c1(c2s4s5+c3c4s2s5+c5s2s3) c1(x4c2c4-x4c3s2s4+x3c2+x2c2)-s1(-s2s4c5+c2c3c4c5-c2s3s5) -s1(s2s4s5-c2c3c4s5-c2c5s3) -s1(x4c4s2+x4c2c3s4+x3s2+x2s2)+x1c1S1(c2c4-c3s2s4)+c1(s2c4+c2c3s4) s1(-s4c2c5-s2c3c4c5+s2s3s5) s1(c2s4s5+c3c4s2s5+c5s2s3) s1(x4c2c4-x4c3s2s4+x3c2+x2c2)+x1s1+c1(-s2s4c5+c2c3c4c5-c2s3s5) +c1(s2s4s5-c2c3c4s5-c2c5s3) +c1(x4c4s2+x4c2c3s4+x3s2+x2s2)S3s4 s3c4c5+c3s5 -c4s3s5+c3c5 x4s3s40 0 0 1]等式左右两个矩阵内对应元素相等于是就得到如下方程组：1c1(c2c4-c3s2s4)-s1(s2c4+c2c3s4)= c0Nx-s0Nz2s1(c2c4-c3s2s4)+c1(s2c4+c2c3s4)= Ny3s3s4= S0Nx+c0Nz4c1(-s4c2c5-s2c3c4c5+s2s3s5) -s1(-s2s4c5+c2c3c4c5-c2s3s5)= c0Ox-s0Oz5s1(-s4c2c5-s2c3c4c5+s2s3s5) +c1(-s2s4c5+c2c3c4c5-c2s3s5)= Oy6s3c4c5+c3s5= s0Ox+c0Oz7c1(c2s4s5+c3c4s2s5+c5s2s3) -s1(s2s4s5-c2c3c4s5-c2c5s3)= c0Ax-s0Az8s1(c2s4s5+c3c4s2s5+c5s2s3) +c1(s2s4s5-c2c3c4s5-c2c5s3)=Ay9-c4s3s5+c3c5= s0Ax+c0Az10c1(x4c2c4-x4c3s2s4+x3c2+x2c2) -s1(x4c4s2+x4c2c3s4+x3s2+x2s2)+x1c1= c0Px-s0Pz-x011s1(x4c2c4-x4c3s2s4+x3c2+x2c2)+x1s1+c1(x4c4s2+x4c2c3s4+x3s2+x2s2)= Py-y012x4s3s4= s0Px+c0Pz注意：以上12个方程式中c0、c1、c2、c3、c4、c5分别为cosθ0、cosθ1、cosθ2、cosθ3、cosθ4、cosθ5的简写，s0、s1、s2、s3、s4、s5分别为sinθ0、sin θ1、s inθ2、sinθ3、sinθ4、sinθ5的简写。