六轴运动机器人运动学求解分析_第九讲

1、六轴联动机械臂运动学及动力学求解分析V0.9 版随着版本的不断更新，旧版本文档中的一些笔误得到了修正，同时文档内容更丰富，仿真程序更完善作者朱森光Emailzsgsoft完成时间2016-02-281引言笔者研究六轴联动机械臂源于当前的机器人产业热，平时比较关注当前热门产业的发展方向。笔者从事的工作是软件开发，工作内容跟机器人无关，但不妨碍研究 机器人运动学及动力学，因为机器人运动学及动力学用到的纯粹是数学和计算机编程知识，学过线性代数和计算机编程技术的人都能研究它。利用业余时间翻阅了机器 人运动学相关资料后撰写此文，希望能够起到抛砖引玉的作用引发更多的人发表有关机器人技术的原创性技术文章。本

2、文内容的正确性经过笔者编程仿真验证可以信赖。2机器建模既然要研究机器人，那么首先要建立一个机械模型，本文将以典型的六轴联动机器臂为例进行介绍，图2-1为笔者使用3D技术建立的一个简单模型。首先建立一个大地坐标系，一般教科书上都是以大地为XY平面，垂直于大地向上方向为Z轴，本文为了跟教科书上有所区别同时不失一般性，将以水平向右方向为X轴，垂直于大地向上方向为 Y轴，背离机器人面向人眼的方向为Z轴，移到电脑屏幕上那就是屏幕水平向右方向为X轴，屏幕竖直向上方向为 Y轴，垂直于屏幕向外为 Z轴，之所以建立这样不合常规的坐标系是希望能够突破常规的思维定势训练在任意空间建立任意坐标系的能力。图2-1图2-

3、1中的机械臂，底部灰色立方体示意机械臂底座，定义为关节1,它能绕图中Y轴旋转；青色长方体示意关节2,它能绕图中的Z1轴旋转；蓝色长方体示意关节3,它能绕图中的Z2轴旋转；绿色长方体示意关节4，它能绕图中的X3轴旋转；深灰色长方体示意关节5,它能绕图中的Z4轴旋转；末端浅灰色机构示意关节6即最终要控制的机械手，机器人代替人的工作就是通过这只手完成的，它能绕图中的X5轴旋转。这儿采用关节这个词可能有点不够精确，先这么意会着理解吧。3运动学分析3.1齐次变换矩阵齐次变换矩阵是机器人技术里最重要的数学分析工具之一，关于齐次变换矩阵的原理很多教科书中已经描述在此不再详述，这里仅针对图2-1的机械臂写出齐

4、次变换矩阵的生成过程。首先定义一些变量符号，关节1绕图中Y轴旋转的角度定义为B 0,当B 0=0 时，O1点在OXYZ坐标系内的坐标是（x0,y0,0）;关节 2绕图中的Z1轴旋转的角度定义为B 1,图中的B1当前位置值为+90度；定义O1O2两点距离为x1,关节3绕图中的Z2轴旋转的角度定义为B 2,图中的B2当前位置值为-90度；O2O3两点距离为x2,关节4绕图中的X3轴旋转的角度定义为B 3,图中的B3 当前位置值为0度；O3O4两点距离为x3,关节5绕图中的Z4轴旋转的角度定义为B 4,图 中的B4当前位置值为-60度；O4O5两点距离为x4,关节6绕图中的X5轴旋转的角度定义为B

5、5,图中的B5 当前位置值为0度。以上定义中角度正负值定义符合右手 法则,所有角度定义值均为本关节坐标系相对前一关节坐标系的相对旋转角度值（一些资料上将O4O5两点重合在一起即 O4O5两点的距离x4退化为零，本文定义 x4大于零使得讨论时更加不失一般性）。符号定义好了，接下来描述齐次变换矩阵。定义R0为关节1绕Y轴的旋转矩阵R0 =c00 s0 0 c0= cos 0 0 s0 = sin 0 00 10 0-s00 c0 00 0 0 1定义T0为坐标系O1X1Y1Z1相对坐标系OXYZ的平移矩阵T0=1 0 0 x001 0 y000 1 00 0 0 1定义R1为关节2绕Z1轴的旋转矩

6、阵R1=c1 - s1 0 0s1 c1 0 00 0 0 1定义T1为坐标系O2X2Y2Z2相对坐标系 01X1Y1Z啲平移矩阵T1=100x1010000100001定义R2为关节3绕Z2轴的旋转矩阵R2=c2 - s2 0 0s2 c2 0 0 0 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 00c3s030s3c3000 01定义T3为坐标系O4X4Y4Z4相对坐标系O3X3Y3Z3的平移矩阵T3=1 0 0 x301 0

7、 000 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 - S50 s5 c5 00 0 0 1以上矩阵定义中 cO、cl、c2、c3、c4、c5 分别为 cos 0 0、cos0 1、cos 0 2、cos 0 3、cos0 4、cos 0 5 的简写，sO、si、s2、s3、s4、s5分别为 sin 0 0、sin

8、 0 1、 sin 0 2、sin 0 3、sin 0 4、sin 0 5 的简写。至此最终的齐次变换矩阵就可以写出来了，那就是： C=R0*T0*R1*T1*R2*T2*R3*T3*R4*T4*R53.2 正运动学求解正运动学求解就是求出 3.1 节中齐次变换矩阵 C 的解析表达式，C=R0*T0*R1*T1*R2*T2*R3*T3*R4*T4*R5= Nx Ox Ax PxNy Oy Ay Nz Oz Az000 这里要注意矩阵乘法满足结合律但不满足交换律，可以先单独求出C= (R0*T0)*(R1*T1)*(R2*T2)*(R3*T3)*(R4*T4)*R5 最终得出结果如下：下面求解。

9、PyPz1R4*T4 ， R3*T3， R2*T2 ，R1*T1 ，R0*T0 然后再将它们相乘，即Nx=c0c1(c2c4-c3s2s4)-c0s1(s2c4+c2c3s4)+s0s3s4N=y s1(c2c4-c3s2s4)+c1(s2c4+c2c3s4) =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(-s2s4c

10、5+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(

11、-c4s3s5+c3c5)=Px c0c1(x4c2c4-x4c3s2s4+x3c2+x2c2)-c0s1(x4c4s2+x4c2c3s4+x3s2+x2s2)+x1c0c1+x4s0s3s4+x0c0=Py Sl(x4c2c4-x4c3s2s4+x3c2+x2c2)+x1s1+c1(x4c4s2+x4c2c3s4+x3s2+x2s2)+yoPz=-s0c1(x4c2c4-x4c3s2s4+x3c2+x2c2)+s0s1(x4c4s2+x4c2c3s4+x3s2+x2s2)- x1s0c1+ x4c0s3s4-x0s0矩阵C就是最终的六轴联动机械臂的齐次变换矩阵，如果机械手末端相对于坐标系O5

12、X5Y5Z5的相对坐标为 U(a,b,c)，那么末端U在大地坐标系 OXYZ中的坐标为：XuNx Ox AxPxaYuNy Oy AyPyVbZuNz Oz Az-A Pz匚10 0 011图3-1以上就是机器人正运动学的求解，Nx、Ny、Nz、Ox、Oy、Oz、Ax、Ay、Az、Px、Py、Pz表达式中的x0、y0、x1、x2、x3、x4为机械固有尺寸，B 0、B1、0 2、B 3、B 4、B5为六个关节的旋转角。实际上C的子矩阵M= Nx Ox AxNy Oy Ay Nz Oz Az反应的就是末端坐标系 O5X5Y5Z5的姿态，子矩阵M实际上就是轴 O5X5、O5Y5、O5Z5在大地坐标系

13、 OXYZ下的方向余弦矩阵，而(Px,Py,Pz)就是点O5在大地 坐标系OXYZ下的绝对坐标。3.3逆运动学求解机器人逆运动学求解是根据末端位姿矩阵 等式进行变换，令 S0=R0*T0，然后将等式两边同时左乘C反求六个关节的旋转角0 0、C=R0*T0*R1*T1*R2*T2*R3*T3*R4*T4*R5-1S0 *C=R1*T1*R2*T2*R3*T3*R4*T4*R5其中S©-10 2、0 3、0 4、0 5的问题。为了便于求解，这儿对1S0的逆S0-得到：等式左边c00s0-s0-x00-1S0 *C0 c0-y000c0Nx-s0NzNyS0Nx+c0Nz1 c0Ox-s0

14、OzcOAx-sOAzcOPx-sOPz-xOOy s0Ox+c0OzAys0Ax+c0AzPy-y0 s0Px+c0Pz1等式右边C1(c2c4-c3s2s4)-s1(s2c4+c2c3s4)c1(-s4c2c5-s2c3c4c5+s2s3s5)-s1(-s2s4c5+c2c3c4c5-c2s3s5)R1*T1*R2*T2*R3*T3*R4*T4*R5=C1(c2s4s5+c3c4s2s5+c5s2s3)-S1(s2s4s5-c2c3c4s5-c2c5s3)C1(x4c2c4-x4c3s2s4+x3c2+x2c2)-S1(x4c4s2+x4c2c3s4+x3s2+x2s2)+x1c1S1(c

15、2c4-c3s2s4)+c1(s2c4+c2c3s4)s1(-s4c2c5-s2c3c4c5+s2s3s5)+c1(-s2s4c5+c2c3c4c5-c2s3s5)S1(c2s4s5+c3c4s2s5+c5s2s3)+C1(s2s4s5-c2c3c4s5-c2c5s3)S1(x4c2c4-x4c3s2s4+x3c2+x2c2)+x1s1+C1(x4c4s2+x4c2c3s4+x3s2+x2s2)S3s4s3c4c5+c3s5-c4s3s5+c3c5x4s3s4C1(x4c2c4-x4c3s2s4+x3c2+x2c2) -S1(x4c4s2+x4c2c3s4+x3s2+x2s2)+x1c仁 c0

16、Px-s0Pz-x0S1(x4c2c4-x4c3s2s4+x3c2+x2c2)+x1s1+c1(x4c4s2+x4c2c3s4+x3s2+x2s2)= Py-y0 x4s3s4= s0Px+c0Pz101cos0 4、cos 0 5 的简写，s0、s1、s2、s3、s4、s5 分别为 sin 0 0、sin0 0等式左右两个矩阵内对应元素相等于是就得到如下方程组：C1(c2c4-c3s2s4)-s1(s2c4+c2c3s4)= c0Nx-s0NzS1(c2c4-c3s2s4)+c1(s2c4+c2c3s4)= Nys3s4= S0Nx+c0Nzc1(-s4c2c5-s2c3c4c5+s2s3s

17、5) -s1(-s2s4c5+c2c3c4c5-c2s3s5)= c0Ox-s0Ozs1(-s4c2c5-s2c3c4c5+s2s3s5) +c1(-s2s4c5+c2c3c4c5-c2s3s5)= Oy s3c4c5+c3s5= s0Ox+c0OzC1(c2s4s5+c3c4s2s5+c5s2s3)-s1(s2s4s5-c2c3c4s5-c2c5s3)= c0Ax-s0AzS1(c2s4s5+c3c4s2s5+c5s2s3) +c1(s2s4s5-c2c3c4s5-c2c5s3)=Ay-c4s3s5+c3c5= s0Ax+c0Az1112注意：以上 12 个方程式中 c0、c1、c2、c3、

18、c4、c5 分别为 cos 0 0、cos 0 1、cos 0 2、cos 0 3、 0 1、s in 0 2、sin 0 3、sin 0 4、sin 0 5 的简写。机器人逆运动求解就是根据以上12个方程求出00、0 1、0 2、0 3、0 4、0 5,逆运动学求解时末端位姿信息是已知的。你可能会认为12个方程求解6个未知数显然冗余了肯定能求出，呵呵果真如此吗？答案是当然能求出但是很复杂！实际上这12个方程式只描述了 6个线性无关的量，也就是说有冗余表达关系，3.2 节中说到C的子矩阵M实际上是轴O5X5、O5Y5、O5Z5在大地坐标系 OXYZ下的方向余弦矩阵，而我们知道一个坐标系相对另一

19、个坐标系的姿态只要3个线性无关的量(比如欧拉角表示法)表示就足够了，再加上Px,Py,Pz这3个量共6个线性无关的量。所以最终的问题是根据6个线性无关的已知值求解0 0、0 1、0 2、0 3、0 4、0 5。人类的智力是无法求解这种非线性方程组的，因为这种非线性方程组很难写出0等于什么的解析表达式，所以只有借助于计算机来求解了，计算机求解最笨的方法就是枚举各种可能的值判断是否满足这12个方程式，比如以 0.01度为最小单位枚举所有可能的0 0、0 1、0 2、0 3、0 4、0 5,由于角度范围是0-360度，所以当以0.01度为最小单位枚举时将有36000的6次方种组合，由于实数存在无穷尽

20、的特性，比如以 0.01度为最小单位势必会丢掉更小的数如0.001度、0.0001度的值，所以是不能保证方程等式的两边绝对相等的，只能判断等式两边差的绝对值足够小就认为等式成立。36000 的6次方种组合即使耗尽地球上的所有计算资源也不能保证实时性，况且精度还只有0.01度，如果精度要求更高呢？所以肯定存在其它更好的方法了，下面就来介绍一种更好的解法。z通过仔细观察发现联立方程3和方程12将可以求出0 0=arctan(Pz-x4*Nz)/(x4*Nx-Px)z 方程 1 可以化简为 c4*cos( 0 1+ 0 2)-c3s4*sin( 0 1+ 0 2)=c0Nx-s0Nzz方程2 可以化

21、简为 c4*sin ( 0 1+ 0 2)+c3s4*cos( 0 1+ 0 2)=Nyz方程5 可以化简为(c3c4c5-s3s5)cos( 0 1+ 0 2)-s4c5 sin ( 0 1+ 02)=Oyz方程8 可以化简为 s4s5sin( 0 1+ 0 2 )-(c3c4s5+c5s3)cos( 0 1+ 02)=Ayz方程10 可以化简为(x2+x3+ x4c4)cos( 0 1+ 0 2)-x4c3s4sin ( 01+ 0 2)+x1c1=cOPx-sOPz-xOz方程11 可以化简为(x2+x3+ x4c4)sin ( 0 1+ 0 2)+x4c3s4cos ( 01+ 0 2

22、)+x1s仁Py-yO按照此思路去寻找规律，最终就能大大降低计算机的计算强度从而快速求出解。3.4姿态控制末端机械手的姿态可以用欧拉角来表示，欧拉角可以描述一个坐标系相对于另一个坐标系的姿态，运用到这儿就是描述末端机械手坐标系O5X5Y5Z5相对于大地坐标系OXYZ的姿态。欧拉角有三个角分别是俯仰角a偏航角B和横滚角 Y在本文建立的机器模型相对坐标系中，欧拉角与齐次变换矩阵 C的子矩阵M的关系如下:M=cos acos Bsin Bin ysin acosBcos 丫sin Bos y sin acos Bin 丫sin acos aos y-cos asin (3sin ain Bos yc

23、os Bin 丫-cos asin 丫cos Bos Ysin ain Bin Y可见欧拉角与变换矩阵 C是有对应关系的，所以改变欧拉角就相当于改变了变换矩阵C,再根据变换矩阵C进行逆运动学求解就可以得出各关节的角度实现控制了。图3-2示意了末端机械手的两种姿态，两种姿态之间转换时保持机械手末端顶点的位置不变，仅仅改变机械手的俯仰角久那么这种机械手本体绕末端顶点作俯仰动作的运动就是一种姿态控制运动。图3-2图3-3图3-3则示意了一种平移运动，此时姿态角a B 丫均保持不变，仅改变机械手顶点位置。姿态控制还有其它几种类型，比如沿空间任意曲线运动、机械手本体自转、绕末端顶点作圆锥运动等等，原理就

24、是通过改变欧拉角以及Px,Py,Pz坐标来实现的，按照想要的规律控制a B 丫和Px,Py,Pz坐标就能实现机械手的运动控制了。特别注意的是欧拉角控制姿态时如果俯仰角a为正负90度时将会导致奇异，此时可以依次轮流冻结最近的B 丫来求另一个角直到 a偏离正负90度为止。除了可以用欧拉角法来控制姿态外还可以用四元数法控制姿态，由于四元数法可以绕任意矢量轴旋转所以在某些情况下比欧拉角法更方便。3.5轨迹计算3.5.1两点确定直线这个很简单，没什么好讨论的，仿真效果如图3-4所示。图3-43.5.2三点确定空间圆弧首先根据三点确定三点所在平面的方程，联立三点到圆心的距离相等可以解得圆心坐标和半径，在三

25、点所在平面上建立坐标系O6X6Y6Z6 , Z6轴为平面法向量方向，设坐标系 O6X6Y6Z6在大地坐标系 OXYZ下的方向余弦矩阵为 B （可以根据三点之间的连线向量算出），那么三点在坐标系 O6X6Y6Z6中的相对坐标就是用 B的逆矩阵左乘三点在大地坐标系OXYZ中的坐标，计算出三点在坐标系O6X6Y6Z6中的相对坐标后就可以得到X6Y6平面内的二维圆弧方程了，对二维圆弧进行插补计算然后再用矩阵 B左乘插补点变换到大地坐标系OXYZ中即可，仿真效果如图 3-5所示。图3-54动力学分析4.1速度分析设机械手质心相对 O5X5Y5Z5坐标系的相对坐标为（R,S,T），则机械手质心在大地坐标系

26、OXYZ下的绝对坐标为:X = R*Nx+S*Ox+T*Ax+PxY = R*Ny+S*Oy+T*Ay+PyZ = R*Nz +S*Oz+T*Az+Pz又设？ x, ?y, ?z为机械手在大地坐标系 OXYZ下的姿态角，表达式可以由3.4节中欧拉角与矩阵 M的关系推导得到，下面写出机械手的速度雅可比矩阵。速度雅可比矩阵 J=eX / e0 0eX / e0 1eX / e0 2eX / e 0 3eX / e 0 4eX / e 0 5e Y/ e 0 0eY / e0 1eY / e0 2eY/e0 3eY / e 0 4eY / e 0 5e Z/ e 0 0eZ / e 0 1eZ /

27、e 0 2eZ / e 0 3eZ / e 0 4eZ / e 0 5e ? x/ e 0 0e? x / e 0 1e? x / e 0 2e? x / e 0 3e? x / e 0 4e? x / e 0 5e ? y/ e 0 0e? y / e 0 1e? y / e 0 2e? y / e 0 3e? y / e 0 4e? y / e 0 5e ? z/ e 0 0e? z / e 0 1e? z / e 0 2e? z / e 0 3e? z / e 0 4e? z / e 0 5（e为偏导数符号）则机械手质心速度Vx,Vy,Vz,Wx,Wy,Wz T=J*3 0, 3 1, 32, 3 3, 3 4, 35，这就是根据各关节角速度求机械手质心速度和角速度的公式。反过来如果知道机械手质心速度和角速度，如何求各关节角速度呢，那就是 3 0, 3 1, 3 2, 3 3, 3 4, 3 5 T=J-1*Vx,Vy,Vz,Wx,Wy,Wz T，其中J-1为矩阵J的逆。4.2力矩分析力矩分析分两部分：一部分是忽略各关节杆件质量和机械手的质量，仅分析作用在机械手上的环境外力和各关节抵抗力矩之间的关系；另一部分是根据拉格朗日方 程推导出的运动产生的惯性力、离心力科氏力以及自身重力共同作用产生的力矩关系。本文档只讨论第一部分即忽略各关节杆件质