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

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

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