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

六轴联动机械臂运动学及动力学求解分析

V0.9版

随着版本的不断更新，旧版本文档中的一些笔误得到了修正，同时文档内容更丰富，仿真程序更完善。 作者 朱森光

Email zsgsoft@hotmail.com

完成时间 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轴的旋转矩阵

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)

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+x0c0 Py= s1(x4c2c4-x4c3s2s4+x3c2+x2c2)+x1s1+c1(x4c4s2+x4c2c3s4+x3s2+x2s2)+y0

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

其中S0-1=[c0 0 -s0 -x0

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]

等式左右两个矩阵内对应元素相等于是就得到如下方程组：