六轴联动机械臂运动学建模求解分析.pdf
- 文档编号:14655094
- 上传时间:2023-06-25
- 格式:PDF
- 页数:6
- 大小:236.46KB
六轴联动机械臂运动学建模求解分析.pdf
《六轴联动机械臂运动学建模求解分析.pdf》由会员分享,可在线阅读,更多相关《六轴联动机械臂运动学建模求解分析.pdf(6页珍藏版)》请在冰点文库上搜索。
六轴联动机械臂运动学求解分析六轴联动机械臂运动学求解分析V0.3版版第三讲第三讲为了保证内容的连续性,第三讲包含了前面几讲的内容,文档结构作了些调整。
作者朱森光Email完成时间2015-12-121引言引言笔者研究六轴联动机械臂源于当前的机器人产业热,平时比较关注当前热门产业的发展方向。
笔者从事的工作是软件开发,工作内容跟机器人无关,但不妨碍研究机器人运动学,因为机器人运动学用到的纯粹是数学和计算机编程知识,学过线性代数和计算机编程技术的人都能研究它。
利用业余时间翻阅了机器人运动学相关资料后撰写此文,希望能够起到抛砖引玉的作用引发更多的人发表有关机器人技术的原创性技术文章。
本文的所有文字、图片及相关资料均为原创,内容正确性经过笔者亲自编程仿真验证可以信赖。
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为关节1绕Y轴的旋转矩阵R0=c00s00/c0=cos0s0=sin00100-s00c000001定义T0为坐标系O1X1Y1Z1相对坐标系OXYZ的平移矩阵定义T0为坐标系O1X1Y1Z1相对坐标系OXYZ的平移矩阵T0=100x0010y000100001定义R1为关节2绕Z1轴的旋转矩阵定义R1为关节2绕Z1轴的旋转矩阵R1=c1s100s1c10000100001定义T1为坐标系O2X2Y2Z2相对坐标系O1X1Y1Z1的平移矩阵定义T1为坐标系O2X2Y2Z2相对坐标系O1X1Y1Z1的平移矩阵T1=100x1110010100001定义R2为关节3绕Z2轴的旋转矩阵定义R2为关节3绕Z2轴的旋转矩阵R2=c2s200s2c20000100001定义T2为坐标系O3X3Y3Z3相对坐标系O2X2Y2Z2的平移矩阵定义T2为坐标系O3X3Y3Z3相对坐标系O2X2Y2Z2的平移矩阵T2=100x2010000100001定义R3为关节4绕X3轴的旋转矩阵定义R3为关节4绕X3轴的旋转矩阵R3=10000c3s300s3c300001定义T3为坐标系O4X4Y4Z4相对坐标系O3X3Y3Z3的平移矩阵定义T3为坐标系O4X4Y4Z4相对坐标系O3X3Y3Z3的平移矩阵T3=100x3010000100001定义R4为关节5绕Z4轴的旋转矩阵定义R4为关节5绕Z4轴的旋转矩阵R4=c4s400s4c40000100001定义T4为坐标系O5X5Y5Z5相对坐标系O4X4Y4Z4的平移矩阵定义T4为坐标系O5X5Y5Z5相对坐标系O4X4Y4Z4的平移矩阵T4=100x4010000100001定义R5为关节6绕X5轴的旋转矩阵定义R5为关节6绕X5轴的旋转矩阵R5=10000c5s500s5c500001以上矩阵定义中c0、c1、c2、c3、c4、c5分别为cos0、cos1、cos2、cos3、cos4、cos5的简写,s0、s1、s2、s3、s4、s5分别为sin0、sin1、sin2、sin3、sin4、sin5的简写。
至此最终的齐次变换矩阵就可以写出来了,那就是:
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=NxOxAxPxNyOyAyPyNzOzAzPz0001这里要注意矩阵乘法满足结合律但不满足交换律,可以先单独求出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)+s0s3s4Ny=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(-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+x0c0Py=s1(x4c2c4-x4c3s2s4+x3c2+x2c2)+x1s1+c1(x4c4s2+x4c2c3s4+x3s2+x2s2)+y0Pz=-s0c1(x4c2c4-x4c3s2s4+x3c2+x2c2)+s0s1(x4c4s2+x4c2c3s4+x3s2+x2s2)+x1s0c1-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=NxOxAxNyOyAyNzOzAz反应的就是末端坐标系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的逆S00得到:
S0*C=0*C=R1*T1*R2*T2*R3*T3*R4*T4*R5其中S0=c00-s0-x0010-y0s00c000001等式左边S0*C0*C=c0Nx-s0Nzc0Ox-s0Ozc0Ax-s0Azc0Px-s0Pz-x0NyOyAyPy-y0S0Nx+c0Nzs0Ox+c0Ozs0Ax+c0Azs0Px+c0Pz0001等式右边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)+x1c1S1(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)S3s4s3c4c5+c3s5-c4s3s5+c3c5x4s3s40001等式左右两个矩阵内对应元素相等于是就得到如下方程组:
1c1(c2c4-c3s2s4)-s1(s2c4+c2c3s4)=c0Nx-s0Nz2s1(c2c4-c3s2s4)+c1(s2c4+c2c3s4)=Ny3s3s4=S0Nx+c0Nz4c1(-s4c2c5-s2c3c4c5+s2s3s5)-s1(-s2s4c5+c2c3c4c5-c2s3s5)=c0Ox-s0Oz5s1(-s4c2c5-s2c3c4c5+s2s3s5)+c1(-s2s4c5+c2c3c4c5-c2s3s5)=Oy6s3c4c5+c3s5=s0Ox+c0Oz7c1(c2s4s5+c3c4s2s5+c5s2s3)-s1(s2s4s5-c2c3c4s5-c2c5s3)=c0Ax-s0Az8s1(c2s4s5+c3c4s2s5+c5s2s3)+c1(s2s4s5-c2c3c4s5-c2c5s3)=Ay9-c4s3s5+c3c5=s0Ax+c0Az10c1(x4c2c4-x4c3s2s4+x3c2+x2c2)-s1(x4c4s2+x4c2c3s4+x3s2+x2s2)+x1c1=c0Px-s0Pz-x011s1(x4c2c4-x4c3s2s4+x3c2+x2c2)+x1s1+c1(x4c4s2+x4c2c3s4+x3s2+x2s2)=Py-y012x4s3s4=s0Px+c0Pz注意:
以上12个方程式中c0、c1、c2、c3、c4、c5分别为cos0、cos1、cos2、cos3、cos4、cos5的简写,s0、s1、s2、s3、s4、s5分别为sin0、sin1、sin2、sin3、sin4、sin5的简写。
机器人逆运动求解就是根据以上12个方程求出0、1、2、3、4、5,逆运动学求解时末端位姿信息是已知的,也就是说这12个方程式的等号右边的值都是已知的。
你可能会认为12个方程求解6个未知数显然冗余了肯定能求出,呵呵果真如此吗?
答案是当然能求出但是很复杂!
实际上这12个方程式只描述了6个线性无关的量,也就是说有冗余表达关系,3.2节中说到C的子矩阵M中Nx、Ny、Nz、Ox、Oy、Oz、Ax、Ay、Az、Px、Py、Pz实际上是轴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度,如果精度要求更高呢?
所以肯定存在其它更好的方法了,下面就来介绍一种更好的解法。
?
通过仔细观察发现联立方程3和方程12将可以求出0=arctan(Pz-x4*Nz)/(x4*Nx-Px)arctan(Pz-x4*Nz)/(x4*Nx-Px)?
方程1可以化简为c4*cos(1+2)-c3s4*sin(1+2)=1+2)-c3s4*sin(1+2)=c0Nx-s0Nz?
方程2可以化简为c4*sin(1+2)+c3s4*cos(1+2)=(1+2)+c3s4*cos(1+2)=Ny?
方程10可以化简为(x2+x3+x4c4)cos(1+2)-x4c3s4sin1+2)-x4c3s4sin(1+2)+x1c1=1+2)+x1c1=c0Px-s0Pz-x0?
方程11可以化简为(x2+x3+x4c4)sin(1+2)+x4c3s4cos1+2)+x4c3s4cos(1+2)1+2)+x1s1=Py-y0=Py-y0?
方程5可以化简为(c3c4c5-s3s5)cos(1+21+2)-s4c5sin(1+2)1+2)=Oy?
方程8可以化简为s4s5sin(1+21+2)-(c3c4s5+c5s3)cos(1+2)1+2)=Ay按照此思路去寻找规律,最终就能大大降低计算机的计算强度从而快速求出解。
4姿态控制姿态控制末端机械手的姿态可以用欧拉角来表示,欧拉角可以描述一个坐标系相对于另一个坐标系的姿态,运用到这儿就是描述末端机械手坐标系O5X5Y5Z5相对于大地坐标系OXYZ的姿态。
欧拉角有三个角分别是俯仰角A、偏航角B和横滚角C,在本文建立的机器模型相对坐标系中,欧拉角与齐次变换矩阵C的子矩阵M的关系如下:
M=cosAcosBsinBsinC-sinAcosBcosCsinBcosC+sinAcosBsinCsinAcosAcosC-cosAsinC-cosAsinBsinAsinBcosC+cosBsinCcosBcosC-sinAsinBsinC可见欧拉角与变换矩阵C是有对应关系的,所以改变欧拉角就相当于改变了变换矩阵C,再根据变换矩阵C进行逆运动学求解就可以得出各关节的角度实现控制了。
图4-1和图4-2中,黄色的长方体示意末端机械手模型,如果保持机械手末端顶点的位置不变,仅仅改变机械手的俯仰角A,那么这种机械手本体绕末端顶点作俯仰动作的运动就是一种姿态控制运动。
图4-1图4-2姿态控制还有其它几种类型,比如俯仰角不变机械手整体平移或上下运动、机械手本体自转、绕末端顶点作圆锥运动等等,原理就是通过改变欧拉角来实现的,按照想要的规律控制俯仰角A、偏航角B和横滚角C就能实现机械手的姿态运动控制了。
5编程实现编程实现通过前面的分析发现机器人运动学方程组是一组非线性方程组,需要通过计算机编程才能求解,一般采用C/C+编程语言来进行求解比较高效,很多人使用MATLAB编程,MATLAB比较适合用来进行部分知识点的前期验证,要想做出可以演示的一整套东西建议还是直接使用C/C+编程语言比较好,PC机上的编程在工程上一般使用VisualC+开发工具。
图5-1图5-1是笔者使用VisualC+开发工具开发的仿真验证软件,采用OpenGL编程技术进行3D建模仿真,实现了以下功能:
1.通过界面输入六个关节角度进行正运动学求解得到末端机械手位姿的方向余弦变换矩阵C。
2.通过界面输入末端机械手位姿的方向余弦变换矩阵C进行逆运动学求解得到六个关节角度。
3.通过界面控制末端机械手的俯仰动作。
6总结总结本文通过建立合适的机器模型和相对坐标系,借助于齐次变换矩阵技术演算得出了机器人正逆运动学求解的一系列公式,最终通过编程仿真验证了这一系列公式的正确性,从而论证了机器人运动学的求解问题。
可以看到解决机器人运动学求解问题用到了三角函数知识、几何学中的坐标系变换知识、线性代数中的矩阵知识以及计算机编程知识等,这再次证明了科学研究过程中数学建模能力和计算机编程能力的重要性。
文中讨论的机械臂有6个自由度,解决了6个自由度的运动学问题后再去解低于6个自由度的运动学问题就没有任何技术障碍了。
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 联动 机械 运动学 建模 求解 分析
![提示](https://static.bingdoc.com/images/bang_tan.gif)