KUKA机器人运动学分析及simmulink仿真

1、 KUKA KR40PA机器人运动学分析及simmulink仿真1 Kuka KR40PA码垛机器人简介Kuka KR40PA机器人是一种有四个自由度的码垛机器人，有四个驱动器，很好地运用了平行四边形机构，固定其姿态从而大大简化了控制难度，并且提高了精度及寿命，本文所用kuka码垛机器人如下图所示：2、 机构简图，及其简化。1、 机构简图如下：第一步简化原因：第一步我们简化了两个平行四边形机构，在此我们分析，这两个平行四边形机构的作用是约束末端执行器在XZ平面的姿态，即：使末端执行器始终竖直向下。在此我们人为的默认末端执行器始终竖直向下，不随前面传递构件的影响。此时便可以将两组平行四边形机构去

2、除而不影响末端执行器的姿态和位移。第一步简化后机构简图第二步简化原因：在此我将主动杆1及连杆4去除。杆1 2 3 4组成了一个平行四边形机构，因此3=2-1.所以我们将杆1杆4去除，只要使3=2-1便不影响末端执行器的位置和姿态。第二步简化后的图形第三步简化原因：为了使参数更简洁，便于计算。我们将杆2的第一个关节与第一个旋转轴相交，这样简化的模型更好计算。不影响总体机构的功能。最终简化后的机构简图3、 建立连杆坐标系。如下图：4、 D-H参数表 i i-1 ai-1 di i 10 0 0 1 2 -90 0 0 2 3 0 l2 0 3 4 0 l3 0 4 5 90 0 0 55、 求正运

3、动学公式 10T=c1-s1s1c0121T=c2-s-s2-c32T=c3-s3s3c30l43T=c4-s4s4c40l54T=c5-s50000-10s5c10T=c1-s1s1c01 20T= 10T*21T=c1c2-c1s2s1c2c01 30T= 20T*32T= c1c2-c1s2s1c2c01 40T= 30T*43T= 40Rl3c1c23+c1c2l2l3s1c23+s1c2l200-l3s23-s2l201 由于平行四边形机构的存在使得 41R= - 40R= 10R * 41R= c1-s1s1c * -1000 00 110 = -c10-s10-s1c1 0 10

4、 所以 40T= 40Rl3c1c23+c1c2l2l3s1c23+s1c2l200-l3s23-s2l201= -c1 0-s1l3c1c23+c1c2l2-s1 0c1l3s1c23+s1c2l20 1 0-l3s23-s2l20 0 01 50T= 40T* 54T= -c(1-5)-s(1-5)0l3c1c23+c1c2l2-s(1-5)c(1-5)0l3s1c23+s1c2l20 0 -1-l3s23-s2l20 0 01 雅克比矩阵： 50J=50JV50Jw=-l3s1c23-s1c2l2-l3c1s23-c1s2l2-l3c1s2300l3c1c23+c1c2l2-l3s1s2

5、3-s1s2l2-l3s1s23000-l3c23-l2c2-l3c23000-s1-s1-s100c1c1c-1 至此正解完成。 六、运动学逆解在此只对位置逆解进行分析，姿态逆解只与1 5 有关因此很简单就可以计算出来。假设我们给出目标位置在0坐标系表示坐标为( X , Y , Z ) ,由变换矩阵我们可以得出：X= l3c1c23+c1c2l2 Y= l3s1c23+s1c2l2 Z= -l3s23-s2l2 用 式除以 式可以得到 tan1 =Y/X 利用2幅角反正切公式可以得到 1 =Atan2（y , x）。 式与 式可以写成如下形式：X/c1= l3c23+c2l2 -Z= l3s

6、23+s2l2 两式左右两边分别平方相加得到下式：(X/c1)2+z2= l22+l32+2l2l3c3 又因为 tan1=Y/X 可解得c3=(X2+Y2+Z2-l22- l32)/2l2l3 S3=1-c32 (在此期望值S3大于0，因此取正)再次利用2幅角反正切公式可以得到 3 =Atan2（s3 , c3）。 可写成如下形式X/c1=K2c2-K3s2 -z= K2s2+K3c2 式中 K2= l2+l3c3 K3= l3s3r= K22+K32 =Atan2（K3，K2） 那么 K2=rcos K3=rsin 式可以写成X/(rc1)=c(+2)Z/(-r)=s(+2)所以 +2 =

7、Atan2（-zc1，X）2= Atan2（-zc1，X）-Atan2（K3，K2） 至此逆解完成 七、轨迹规划：加速度表达式是一个三次多项式，速度表达式是一个四次多项式，位移表达式是一个五次多项式。设S为无量纲的运动位移，为无量纲的运动总时间，V、A为无量纲的运动速度及加速度，那么3-4-5多项式运动曲线可以简单表示如下： （3-1） （3-2） （3-3）根据如下边界条件：(1) 当时，；(2) 当时，。将以上边界条件代入上述多项式表达式，可解出多项式的系数，。于是，我们得到3-4-5多项式运动曲线的表达式：（3-4）（3-5）（3-6）由加速度多项式可解出。为了得到带有量纲的多项式运动曲

8、线表达式，我们令其加速度的最大值为，运行总时间为，运行瞬时时刻，有 （3-7）将上式对积分得： （3-8）同理有： （3-9）由此，当时： （3-10）八、基于matlab simmechanics工具箱的运动学仿真在matlab命令窗口中输入 simulink命令回车 弹出simulink library browser如下图： 新建一个model ，在左侧 libraries 中选择simscape 接着选择simmechanics 将我们见立模型的构建分别拖到model并组成下图形式 ，其中 各模块参数输入如下： 环境模块 关节模块 Body0 的参数 Body1的参数 Body2的参数

9、 Joint actuator的参数驱动函数模块的编程：1、基于末端执行器x,y,z分别按照轨迹规划路径的函数编程function c =sita(t,T,a1,b1,c1,a2,b2,c2)s1=1;s2=1;s3=1;sxmax=abs(a2-a1);axmax=sxmax*5.774/T2;if a2=a1 s1=1;endx=a1+s1*(axmax/5.774)*(6*(t5)/T3-15*(t4)/T2+10*(t3)/T);symax=abs(b2-b1);aymax=symax*5.774/T2;if b2=b1 s2=1;endy=b1+s2*(aymax/5.774)*(6

10、*(t5)/T3-15*(t4)/T2+10*(t3)/T);sxmax=abs(c2-c1);axmax=sxmax*5.774/T2;if c2=c1 s3=1;endz=c1+s3*(axmax/5.774)*(6*(t5)/T3-15*(t4)/T2+10*(t3)/T);m12=atan2(y,x); %关节1终点时旋转角度l2=850;l3=850;cs32=(x2+y2+z2-l22-l32)/(2*l2*l3);ss32=sqrt(1-cs322);m32=atan2(ss32,cs32); %关节3终点旋转角度k22=l2+l3*cs32;k32=l3*ss32;m22=at

11、an2(-z*cos(m12),x)-atan2(k32,k22); %关节2终点旋转角度sita1=m12*180/pi; %转换成角度制sita2=m22*180/pi;sita3=m32*180/pi; c=sita1;sita2;sita3;2、基于关节旋转角度分别按照轨迹规划路径的函数编程：function c =sita(t,T,a1,b1,c1,a2,b2,c2)m12=atan2(b2,a2);l2=850;l3=850;cs32=(a22+b22+c22-l22-l32)/(2*l2*l3);ss32=sqrt(1-cs322);m32=atan2(ss32,cs32);k2

12、2=l2+l3*cs32;k32=l3*ss32;m22=atan2(-c2*cos(m12),a2)-atan2(k32,k22);m11=atan2(b1,a1);cs31=(a12+b12+c12-l22-l32)/(2*l2*l3);ss31=sqrt(1-cs312);m31=atan2(ss31,cs31);k21=l2+l3*cs31;k31=l3*ss31;m21=atan2(-c1*cos(m12),a1)-atan2(k31,k21); s1=1;s2=1;s3=1;sxmax=abs(m12-m11);axmax=sxmax*5.774/T2;if m12=m11 s1=

13、1;endm1=m11+s1*(axmax/5.774)*(6*(t5)/T3-15*(t4)/T2+10*(t3)/T);symax=abs(m22-m21);aymax=symax*5.774/T2;if m22=m21 s2=1;endm2=m21+s2*(aymax/5.774)*(6*(t5)/T3-15*(t4)/T2+10*(t3)/T);szmax=abs(m32-m31);azmax=szmax*5.774/T2;if m32=m31 s3=1;endm3=m31+s3*(azmax/5.774)*(6*(t5)/T3-15*(t4)/T2+10*(t3)/T);sita1=

14、m1*180/pi;sita11=sita1;if sita11180 sita1=sita11-360;elseif sita11180 sita2=sita22-360;elseif sita22180 sita3=sita33-360;elseif sita33(-180) sita3=sita33+360;endc=sita1;sita2;sita3;运动学模型建成的效果如下：我们输入初始位移坐标，以及终点位移坐标，末端会根据规划的轨迹由初始位置，移动到末端位置。九、Matlab动力学的仿真（1）对kuka机器人用solidworks进行建模建成模型如下：（2）安装simmechani

15、cslink插件安装方法：安装方法步骤如下：1.选择对应的solidworks及matlab版本的插件下载（mathwork公司免费），不需要解压然后你打开MATLAB；2. 将下载的两个文件所在目录置为MATLAB当前路径；3. 在MATLAB命令窗口输入install_addon()命令,引号内的是你下载的压缩文件的名字；4. 然后MATLAB就会将这个插件装上的;5.最后在关联solidworks，主要分两步：1）在matlab命令窗口运行smlink_linksw，提示成功；2）打开solidworks，点击工具，选择插件，再选择 SimMechanics Link，将会看到SimMechanics Link新的菜单（注意需要在打开装配体*.SLDASM时才能看到）。（3）将solidworks建成的模型导入到simmechanic