六轴运动机器人运动学求解分析

六轴联动机械臂运动学及动力学求解分析
V0.9版
随着版本的不断更新，旧版本文档中的一些笔误得到了修正，同时文档内容更丰富，仿真程序更完善。

作者朱森光
Email zsgsoft@
完成时间 2016-02-28
1引言
笔者研究六轴联动机械臂源于当前的机器人产业热，平时比较关注当前热门产业的发展方向。

笔者从事的工作是软件开发，工作内容跟机器人无关，但不妨碍研究机器人运动学及动力学，因为机器人运动学及动力学用到的纯粹是数学和计算机编程知识，学过线性代数和计算机编程技术的人都能研究它。

利用业余时间翻阅了机器人运动学相关资料后撰写此文，希望能够起到抛砖引玉的作用引发更多的人发表有关机器人技术的原创性技术文章。

本文内容的正确性经过笔者编程仿真验证可以信赖。

2机器建模
既然要研究机器人，那么首先要建立一个机械模型，本文将以典型的六轴联动机器臂为例进行介绍，图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轴旋转。

这儿采用关节这个词可能有点不够精确，先这么意会着理解吧。

3运动学分析
3.1齐次变换矩阵
齐次变换矩阵是机器人技术里最重要的数学分析工具之一，关于齐次变换矩阵的原理很多教科书中已经描述在此不再详述，这里仅针对图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当前位置值为0度；O3O4两点距离为x3，关节5绕图中的Z4轴旋转的角度定义为θ4, 图中的θ4当前位置值为-60度；O4O5两点距离为x4，关节6绕图中的X5轴旋转的角度定义为θ5, 图中的θ5当前位置值为0度。

以上定义中角度正负值定义符合右手法则,所有角度定义值均为本关节坐标系相对前一关节坐标系的相对旋转角度值（一些资料上将O4O5两点重合在一起即O4O5两点的距离x4退化为零，本文定义x4大于零使得讨论时更加不失一般性）。

符号定义好了，接下来描述齐次变换矩阵。

定义R0为关节1绕Y轴的旋转矩阵
=cosθ0 s0 = sinθ0
//c0
R0
=[c0 0 s0 0
0 1 0 0
0 c0 0
-s0
0 0 0 1]
定义T0为坐标系O1X1Y1Z1相对坐标系OXYZ的平移矩阵
T0=[1 0 0 x0
0 1 0 y0
00 1 0
0 0 0 1]
定义R1为关节2绕Z1轴的旋转矩阵
R1=[c1 –s1 0 0
s1 c1 0 0
0 0 1 0
0 0 0 1]
定义T1为坐标系O2X2Y2Z2相对坐标系O1X1Y1Z1的平移矩阵
T1=[1 0 0 x1
0 1 0 0
0 0 1 0
0 0 0 1]
定义R2为关节3绕Z2轴的旋转矩阵
R2=[c2 –s2 0 0
s2 c2 0 0
0 0 1 0
0 0 0 1]
定义T2为坐标系O3X3Y3Z3相对坐标系O2X2Y2Z2的平移矩阵
T2=[1 0 0 x2
0 1 0 0
0 0 1 0
0 0 0 1]
定义R3为关节4绕X3轴的旋转矩阵
R3=[1 0 0 0
0 c3 –s3 0
0 s3 c3 0
0 0 0 1]
定义T3为坐标系O4X4Y4Z4相对坐标系O3X3Y3Z3的平移矩阵
T3=[1 0 0 x3
0 1 0 0
0 0 1 0
0 0 0 1]
定义R4为关节5绕Z4轴的旋转矩阵
R4=[c4 –s4 0 0
s4 c4 0 0
0 0 1 0
0 0 0 1]
定义T4为坐标系O5X5Y5Z5相对坐标系O4X4Y4Z4的平移矩阵
T4=[1 0 0 x4
0 1 0 0
0 0 1 0
0 0 0 1]
定义R5为关节6绕X5轴的旋转矩阵
R5=[1 0 0 0
0 c5 –s5 0
0 s5 c5 0
0 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*R5
3.2正运动学求解
正运动学求解就是求出3.1节中齐次变换矩阵C的解析表达式，下面求解。

C=R0*T0*R1*T1*R2*T2*R3*T3*R4*T4*R5=[Nx Ox Ax Px
Ny Oy Ay Py
Nz Oz Az Pz
0001]
这里要注意矩阵乘法满足结合律但不满足交换律，可以先单独求出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)+s0s3s4
=s1(c2c4-c3s2s4)+c1(s2c4+c2c3s4)
Ny
Nz
=-s0c1(c2c4-c3s2s4)+s0s1(s2c4+c2c3s4)+c0s3s4
Ox= 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)
=c0c1(c2s4s5+c3c4s2s5+c5s2s3)-c0s1(s2s4s5-c2c3c4s5-c2c5s3)+s0(-c4s3s5+c3c5)
Ax
Ay= s1(c2s4s5+c3c4s2s5+c5s2s3) +c1(s2s4s5-c2c3c4s5-c2c5s3)
Az
=-s0c1(c2s4s5+c3c4s2s5+c5s2s3)+s0s1(s2s4s5-c2c3c4s5-c2c5s3)+c0(-c4s3s5+c3c5)
=c0c1(x4c2c4-x4c3s2s4+x3c2+x2c2)-c0s1(x4c4s2+x4c2c3s4+x3s2+x2s2)+x1c0c1+x4s0s3s4+x0c0
Px
=s1(x4c2c4-x4c3s2s4+x3c2+x2c2)+x1s1+c1(x4c4s2+x4c2c3s4+x3s2+x2s2)+y0
Py
Pz=-s0c1(x4c2c4-x4c3s2s4+x3c2+x2c2)+s0s1(x4c4s2+x4c2c3s4+x3s2+x2s2)- x1s0c1+ x4c0s3s4-x0s0
矩阵C就是最终的六轴联动机械臂的齐次变换矩阵，如果机械手末端相对于坐标系O5X5Y5Z5的相对坐标为U(a,b,c)，那么末端U在大地坐标系OXYZ中的坐标为：
图3-1
以上就是机器人正运动学的求解，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 Ax
Ny Oy Ay
Nz Oz Az]
反应的就是末端坐标系O5X5Y5Z5的姿态，子矩阵M实际上就是轴O5X5、O5Y5、O5Z5在大地坐标系OXYZ下的方向余弦矩阵，而(Px,Py,Pz)就是点O5在大地坐标系OXYZ下的绝对坐标。

3.3逆运动学求解
机器人逆运动学求解是根据末端位姿矩阵C反求六个关节的旋转角θ0、θ1、θ2、θ3、θ4、θ5的问题。

为了便于求解，这儿对C=R0*T0*R1*T1*R2*T2*R3*T3*R4*T4*R5等式进行变换，令S0=R0*T0，然后将等式两边同时左乘S0的逆S0-1得到：
S0-1*C=R1*T1*R2*T2*R3*T3*R4*T4*R5
0 -s0
-x0
=[c0
其中S0-1
0 1 0 -y0
s0 0 c0 0
0 0 0 1]
等式左边S0-1*C=[c0Nx-s0Nz c0Ox-s0Oz c0Ax-s0Az c0Px-s0Pz-x0
Ny Oy Ay Py-y0
S0Nx+c0Nz s0Ox+c0Oz s0Ax+c0Az s0Px+c0Pz
0 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)+x1c1
S1(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 x4s3s4
0 0 0 1]
等式左右两个矩阵内对应元素相等于是就得到如下方程组：
1c1(c2c4-c3s2s4)-s1(s2c4+c2c3s4)= c0Nx-s0Nz
2s1(c2c4-c3s2s4)+c1(s2c4+c2c3s4)= Ny
3s3s4= S0Nx+c0Nz
4c1(-s4c2c5-s2c3c4c5+s2s3s5) -s1(-s2s4c5+c2c3c4c5-c2s3s5)= c0Ox-s0Oz
5s1(-s4c2c5-s2c3c4c5+s2s3s5) +c1(-s2s4c5+c2c3c4c5-c2s3s5)= Oy
6s3c4c5+c3s5= s0Ox+c0Oz
7c1(c2s4s5+c3c4s2s5+c5s2s3) -s1(s2s4s5-c2c3c4s5-c2c5s3)=c0Ax-s0Az
8s1(c2s4s5+c3c4s2s5+c5s2s3) +c1(s2s4s5-c2c3c4s5-c2c5s3)=Ay
9-c4s3s5+c3c5= s0Ax+c0Az
10c1(x4c2c4-x4c3s2s4+x3c2+x2c2) -s1(x4c4s2+x4c2c3s4+x3s2+x2s2)+x1c1= c0Px-s0Pz-x0
11s1(x4c2c4-x4c3s2s4+x3c2+x2c2)+x1s1+c1(x4c4s2+x4c2c3s4+x3s2+x2s2)= Py-y0
12x4s3s4= 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的简写。

机器人逆运动求解就是根据以上12个方程求出θ0、θ1、θ2、θ3、θ4、θ5，逆运动学求解时末端位姿信息是已知的。

你可能会认为12个方程求解6个未知数显然冗余了肯定能求出，呵呵果真如此吗？答案是当然能求出但是很复杂！实际上这12个方程式只描述了6个线性无关的量，也就是说有冗余表达关系，3.2节中说到C的子矩阵M实际上是轴O5X5、O5Y5、O5Z5在大地坐标系OXYZ下的方向余弦矩阵，而我们知道一个坐标系相对另一个坐标系的姿态只要3个线性无关的量（比如欧拉角表示法）表示就足够了，再加上Px,Py,Pz这3个量共6个线性无关的量。

所以最终的问题是根据6个线性无关的已知值求解θ0、θ1、θ2、θ3、θ4、θ5。

人类的智力是无法求解这种非线性方程组的，因为这种非线性方程组很难写出θ等于什么的解析表达式，所以只有借助于计算机来求解了，计算机求解最笨的方法就是枚举各种可能的值判断是否满足这12个方程式，比如以0.01度为最小单位枚举所有可能的θ0、θ1、θ2、θ3、θ4、θ5，由于角度范围是0-360度，所以当以0.01度为最小单位枚举时将有36000的6次方种组合，由于实数存在无穷尽的特性，比如以0.01度为最小单位势必会丢掉更小的数如0.001度、0.0001度的值，所以是不能保证方程等式的两边绝对相等的，只能判断等式两边差的绝对值足够小就认为等式成立。

36000的6次方种组合即使耗尽地球上的所有计算资源也不能保证实时性，况且精度还只有0.01度，如果精度要求更高呢？所以肯定存在其它更好的方法了，下面就来介绍一种更好的解法。

z通过仔细观察发现联立方程3和方程12将可以求出θ0=arctan((Pz-x4*Nz)/(x4*Nx-Px))
z方程1可以化简为c4*cos(θ1+θ2)-c3s4*sin(θ1+θ2)= c0Nx-s0Nz
z方程2可以化简为c4*sin(θ1+θ2)+c3s4*cos(θ1+θ2)= Ny
z方程5可以化简为(c3c4c5-s3s5)cos(θ1+θ2)-s4c5 sin (θ1+θ2)=Oy
z方程8可以化简为s4s5sin(θ1+θ2)-(c3c4s5+c5s3)cos(θ1+θ2)=Ay
z方程10可以化简为(x2+x3+ x4c4)cos(θ1+θ2)-x4c3s4sin(θ1+θ2)+x1c1= c0Px-s0Pz-x0
z方程11可以化简为(x2+x3+ x4c4)sin (θ1+θ2)+x4c3s4cos(θ1+θ2) +x1s1=Py-y0
按照此思路去寻找规律，最终就能大大降低计算机的计算强度从而快速求出解。

3.4姿态控制
末端机械手的姿态可以用欧拉角来表示，欧拉角可以描述一个坐标系相对于另一个坐标系的姿态，运用到这儿就是描述末端机械手坐标系O5X5Y5Z5相对于大地坐标系OXYZ的姿态。

欧拉角有三个角分别是俯仰角α、偏航角β和横滚角γ，在本文建立的机器模型相对坐标系中，欧拉角与齐次变换矩阵C的子矩阵M的关系如下：M=[cosαcosβsinβsinγ-sinαcosBcosγsinβcosγ+ sinαcosβsinγ
sinαcosαcosγ-cosαsinγ
-cosαsinβsinαsinβcosγ+cosβsinγcosβcosγ-sinαsinβsinγ]
可见欧拉角与变换矩阵C是有对应关系的，所以改变欧拉角就相当于改变了变换矩阵C，再根据变换矩阵C进行逆运动学求解就可以得出各关节的角度实现控制了。

图3-2示意了末端机械手的两种姿态，两种姿态之间转换时保持机械手末端顶点的位置不变，仅仅改变机械手的俯仰角α，那么这种机械手本体绕末端顶点作俯仰动作的运动就是一种姿态控制运动。

图3-2
图3-3
图3-3则示意了一种平移运动，此时姿态角α、β、γ均保持不变，仅改变机械手顶点位置。

姿态控制还有其它几种类型，比如沿空间任意曲线运动、机械手本体自转、绕末端顶点作圆锥运动等等，原理就是通过改变欧拉角以及Px,Py,Pz坐标来实现的，按照想要的规律控制α、β、γ和Px,Py,Pz坐标就能实现机械手的运动控制了。

特别注意的是欧拉角控制姿态时如果俯仰角α为正负90度时将会导致奇异，此时可以依次轮流冻结最近的β、γ来求另一个角直到α偏离正负90度为止。

除了可以用欧拉角法来控制姿态外还可以用四元数法控制姿态，由于四元数法可以绕任意矢量轴旋转所以在某些情况下比欧拉角法更方便。

3.5轨迹计算
3.5.1两点确定直线
这个很简单，没什么好讨论的，仿真效果如图3-4所示。

图3-4
3.5.2三点确定空间圆弧
首先根据三点确定三点所在平面的方程，联立三点到圆心的距离相等可以解得圆心坐标和半径，在三点所在平面上建立坐标系O6X6Y6Z6，Z6轴为平面法向量方向，设坐标系O6X6Y6Z6在大地坐标系OXYZ下的方向余弦矩阵为B（可以根据三点之间的连线向量算出），那么三点在坐标系O6X6Y6Z6中的相对坐标就是用B的逆矩阵左乘三点在大地坐标系OXYZ中的坐标，计算出三点在坐标系O6X6Y6Z6中的相对坐标后就可以得到X6Y6平面内的二维圆弧方程了，对二维圆弧进行插补计算然后再用矩阵B左乘插补点变换到大地坐标系OXYZ中即可，仿真效果如图3-5所示。

图3-5
4动力学分析
4.1速度分析
设机械手质心相对O5X5Y5Z5坐标系的相对坐标为(R,S,T)，则机械手质心在大地坐标系OXYZ下的绝对坐标为：
=R*Nx+S*Ox+T*Ax+Px
X
=R*Ny+S*Oy+T*Ay+Py
Y
=R*Nz
+S*Oz+T*Az+Pz
Z
又设Ѱx, Ѱy, Ѱz为机械手在大地坐标系OXYZ下的姿态角，表达式可以由3.4节中欧拉角与矩阵M的关系推导得到，下面写出机械手的速度雅可比矩阵。

速度雅可比矩阵J=[ðX / ðθ0 ðX / ðθ1 ðX / ðθ2 ðX / ðθ3 ðX / ðθ4 ðX / ðθ5
ðY/ ðθ0 ðY / ðθ1 ðY / ðθ2 ðY / ðθ3 ðY / ðθ4 ðY / ðθ5
ðZ/ ðθ0 ðZ / ðθ1 ðZ / ðθ2 ðZ / ðθ3 ðZ / ðθ4 ðZ / ðθ5
ðѰx/ ðθ0 ðѰx / ðθ1 ðѰx / ðθ2 ðѰx / ðθ3 ðѰx / ðθ4 ðѰx / ðθ5
ðѰy/ ðθ0 ðѰy / ðθ1 ðѰy / ðθ2 ðѰy / ðθ3 ðѰy / ðθ4 ðѰy / ðθ5
ðѰz/ ðθ0 ðѰz / ðθ1 ðѰz / ðθ2 ðѰz / ðθ3 ðѰz / ðθ4 ðѰz / ðθ5]
（ð为偏导数符号）
则机械手质心速度[Vx,Vy,Vz,Wx,Wy,Wz]T=J*[ω0,ω1,ω2,ω3,ω4,ω5] T，这就是根据各关节角速度求机械手质心速度和角速度的公式。

反过来如果知道机械手质心速度和角速度，如何求各关节角速度呢，那就是[ω0,ω1,ω2,ω3,ω4,ω5] T=J-1*[Vx,Vy,Vz,Wx,Wy,Wz]T，其中J-1为矩阵J的逆。

4.2力矩分析
力矩分析分两部分：一部分是忽略各关节杆件质量和机械手的质量，仅分析作用在机械手上的环境外力和各关节抵抗力矩之间的关系；另一部分是根据拉格朗日方
程推导出的运动产生的惯性力、离心力科氏力以及自身重力共同作用产生的力矩关系。

本文档只讨论第一部分即忽略各关节杆件质量和机械手的质量，只分析机械手上的环境外力和各关节抵抗力矩之间的关系。

设作用在机械手质心上的环境外力产生的等效力和力矩的合并向量为[Fx,Fy,Fz,Mx,My,Mz]T，各关节抵抗力矩向量为[Ⱦ0, Ⱦ1, Ⱦ2, Ⱦ3, Ⱦ4, Ⱦ5] T，由于力雅可比矩阵与速度雅可比矩阵是有对应关系的，力雅可比矩阵实际上是速度雅可比矩阵的转置J T，所以[Ⱦ0, Ⱦ1, Ⱦ2, Ⱦ3, Ⱦ4, Ⱦ5] T=J T*[Fx,Fy,Fz,Mx,My,Mz]T，反过来[Fx,Fy,Fz,Mx,My,Mz]T=( J T) -1*[Ⱦ0, Ⱦ1, Ⱦ2, Ⱦ3, Ⱦ4, Ⱦ5] T。

5编程实现
通过前面的分析发现机器人运动学方程组是一组非线性方程组，需要通过计算机编程才能求解，一般采用C/C++编程语言来进行求解比较高效，很多人使用MATLAB 编程，MATLAB比较适合用来进行部分知识点的前期验证，要想做出可以演示的一整套东西建议还是直接使用C/C++编程语言比较好，PC机上的编程在工程上一般使用Visual C++开发工具。

图5-1
图5-1是笔者使用Visual C++开发工具开发的仿真验证软件，采用OpenGL编程技术进行3D建模仿真，实现了以下功能：
1.通过界面输入六个关节角度进行正运动学求解得到末端机械手位姿的方向余弦变换矩阵C。

2.通过界面输入末端机械手位姿的方向余弦变换矩阵C进行逆运动学求解得到六个关节角度。

3.通过界面控制末端机械手的俯仰动作。

4.通过界面控制末端机械手做上下平移运动。

5.通过界面控制末端机械手做前后平移运动。

6.通过界面控制末端机械手做左右平移运动。

6总结
本文通过建立合适的机器模型和相对坐标系，借助于齐次变换矩阵技术演算得出了机器人正逆运动学求解的一系列公式，最终通过编程仿真验证了这一系列公式的正确性，从而论证了机器人运动学的求解问题。

可以看到解决机器人运动学求解问题用到了三角函数知识、几何学中的坐标系变换知识、线性代数中的矩阵知识以及计算机编程知识等，由此可见研究复杂问题时数学建模能力和计算机编程能力的重要性。

文中讨论的机械臂有6个自由度，解决了6个自由度的运动学问题后再去解低于6个自由度的运动学问题就没有任何技术障碍了。