六轴联动机械臂运动学及动力学求解分析
V0.5版
第五讲
为了保证内容的连续性，第五讲包含了前面几讲的内容，修正了一点笔误，文档结构作了些调整。
作者
朱森光
Email zsgsoft@hotmail.com
完成时间 2016-02-06
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 当前位置值为-60 度；O3O4 两点距离为 x3，关节 5 绕图中的 Z4 轴旋转的角度定义为θ4,
图中的θ4 当前位置值为-60 度；O4O5 两点距离为 x4，关节 6 绕图中的 X5 轴旋转的角度定义为θ5, 图中的θ5 当前位置值为+60 度。以上定义中角度正负值定义符合
右手法则。符号定义好了，接下来描述齐次变换矩阵。
定义 R0 为关节 1 绕 Y 轴的旋转矩阵
R0=[c0 0 s0 0 //c0 = cosθ0 s0 = sinθ0
0 1 0 0
-s0 0 c0 0
0 0 0 1]
定义 T0 为坐标系 O1X1Y1Z1 相对坐标系 OXYZ 的平移矩阵
T0=[1 0 0 x0
0 1 0 y0
0 0 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
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)+s0s3s4
Ny= s1(c2c4-c3s2s4)+c1(s2c4+c2c3s4)
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)
