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

KUKAKR40PA机器人运动学分析及simmulink仿真一．KukaKR40PA码垛机器人简介KukaKR40PA机器人是一种有四个自由度的码垛机器人，有四个驱动器，很好地运用了平行四边形机构，固定其姿态从而大大简化了控制难度，并且提高了精度及寿命，本文所用kuka码垛机器人如下图所示：二、机构简图，及其简化。1、机构简图如下：第一步简化原因：第一步我们简化了两个平行四边形机构，在此我们分析，这两个平行四边形机构的作用是约束末端执行器在XZ平面的姿态，即：使末端执行器始终竖直向下。在此我们人为的默认末端执行器始终竖直向下，不随前面传递构件的影响。此时便可以将两组平行四边形机构去除而不影响末端执行器的姿态和位移。第一步简化后机构简图第二步简化原因：在此我将主动杆1及连杆4去除。杆1234组成了一个平行四边形机构，因此β3=β2-β1.所以我们将杆1杆4去除，只要使β3=β2-β1便不影响末端执行器的位置和姿态。第二步简化后的图形第三步简化原因：为了使参数更简洁，便于计算。我们将杆2的第一个关节与第一个旋转轴相交，这样简化的模型更好计算。不影响总体机构的功能。最终简化后的机构简图三、建立连杆坐标系。如下图：四、D-H参数表iαi-1ai-1diθi1000θ12-9000θ230l20θ340l30θ459000θ5五、求正运动学公式T10=[c1−s1s1c1000000001001]T21=[c2−s2000010−s2−c2000001]T32=[c3−s3s3c30l20000001001]T43=[c4−s4s4c40l30000001001]T54=[c5−s50000−10s5c5000001]T10=[c1−s1s1c1000000001001]T20=T10*T21=[c1c2−c1s2s1c2c1000000001001]T30=T20*T32=[c1c2−c1s2s1c2c1000000001001]T40=T30*T43=[R40l3c1c23+c1c2l2l3s1c23+s1c2l200−l3s23−s2l201]由于平行四边形机构的存在使得R41=[−100000110]R40=R10*R41=[c1−s1s1c100001]*[−100000110]=[−c10−s10−s1c1010]所以T40=[R40l3c1c23+c1c2l2l3s1c23+s1c2l200−l3s23−s2l201]=[−c10−s1l3c1c23+c1c2l2−s10c1l3s1c23+s1c2l2010−l3s23−s2l20001]T50=T40*T54=[−c(1−5)−s(1−5)0l3c1c23+c1c2l2−s(1−5)c(1−5)0l3s1c23+s1c2l200−1−l3s23−s2l20001]雅克比矩阵：J50=[JV50Jw50]=[−l3s1c23−s1c2l2−l3c1s23−c1s2l2−l3c1s2300l3c1c23+c1c2l2−l3s1s23−s1s2l2−l3s1s23000−l3c23−l2c2−l3c23000−s1−s1−s100c1c1c101000−1]至此正解完成。六、运动学逆解在此只对位置逆解进行分析，姿态逆解只与θ1θ5有关因此很简单就可以计算出来。假设我们给出目标位置在0坐标系表示坐标为(X,Y,Z),由变换矩阵我们可以得出：X=l3c1c23+c1c2l2①Y=l3s1c23+s1c2l2②Z=−l3s23−s2l2③用②式除以①式可以得到tanθ1=Y/X利用2幅角反正切公式可以得到θ1=Atan2（y,x）。①式与③式可以写成如下形式：X/c1=l3c23+c2l2④-Z=l3s23+s2l2⑤两式左右两边分别平方相加得到下式：(X/c1)2+z2=l22+l32+2l2l3c3又因为tan1=Y/X可解得c3=(X2+Y2+Z2-l22-l32)/2l2l3S3=√1−c32(在此期望值S3大于0，因此取正)再次利用2幅角反正切公式可以得到θ3=Atan2（s3,c3）。①③可写成如下形式X/c1=K2c2-K3s2⑥-z=K2s2+K3c2⑦式中K2=l2+l3c3K3=l3s3r=√K22+K32γ=Atan2（K3，K2）那么K2=rcosγK3=rsinγ⑥⑦式可以写成X/(rc1)=c(γ+2)Z/(-r)=s(γ+2)所以γ+θ2=Atan2（-zc1，X）θ2=Atan2（-zc1，X）-Atan2（K3，K2）至此逆解完成七、轨迹规划：加速度表达式是一个三次多项式，速度表达式是一个四次多项式，位移表达式是一个五次多项式。设S为无量纲的运动位移，为无量纲的运动总时间，V、A为无量纲的运动速度及加速度，那么3-4-5多项式运动曲线可以简单表示如下：fedcbaS2345（3-1）ed2c3b4a5V234（3-2）ed26c12ba20A23（3-3）根据如下边界条件：(1)当0时，，，；(2)当1时，，，。将以上边界条件代入上述多项式表达式，可解出多项式的系数，，，，，。于是，我们得到3-4-5多项式运动曲线的表达式：3451015-6S（3-4）2343060-30V（3-5）60180-120A23（3-6）由加速度多项式可解出774.5maxA。为了得到带有量纲的多项式运动曲线表达式，我们令其加速度的最大值为maxa，运行总时间为T，运行瞬时时刻Tt，有)60180120(774.5a23maxa（3-7）将上式对t积分得：0S0V0A1S0V0A6a15b10c0d0e0f)306030(774.5v234maxTa（3-8）同理有：)10156(774.5s3452maxTa（3-9）由此，当Tt时：2max2maxmax1732.0774.5TaTass（3-10）八、基于matlabsimmechanics工具箱的运动学仿真在matlab命令窗口中输入simulink命令回车弹出simulinklibrarybrowser如下图：新建一个model，在左侧libraries中选择simscape接着选择simmechanics将我们见立模型的构建分别拖到model并组成下图形式，其中各模块参数输入如下：环境模块关节模块Body0的参数Body1的参数Body2的参数Jointactuator的参数驱动函数模块的编程：1、基于末端执行器x,y,z分别按照轨迹规划路径的函数编程functionc=sita(t,T,a1,b1,c1,a2,b2,c2)s1=1;s2=1;s3=1;sxmax=abs(a2-a1);axmax=sxmax*5.774/T^2;ifa2a1s1=-1;elseifa2=a1s1=1;endx=a1+s1*(axmax/5.774)*(6*(t^5)/T^3-15*(t^4)/T^2+10*(t^3)/T);symax=abs(b2-b1);aymax=symax*5.774/T^2;ifb2b1s2=-1;elseifb2=b1s2=1;endy=b1+s2*(aymax/5.774)*(6*(t^5)/T^3-15*(t^4)/T^2+10*(t^3)/T);sxmax=abs(c2-c1);axmax=sxmax*5.774/T^2;ifc2c1s3=-1;elseifc2=c1s3=1;endz=c1+s3*(axmax/5.774)*(6*(t^5)/T^3-15*(t^4)/T^2+10*(t^3)/T);m12=atan2(y,x);%关节1终点时旋转角度l2=850;l3=850;cs32=(x^2+y^2+z^2-l2^2-l3^2)/(2*l2*l3);ss32=sqrt(1-cs32^2);m32=atan2(ss32,cs32);%关节3终点旋转角度k22=l2+l3*cs32;k32=l3*ss32;m22=atan2(-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、基于关节旋转角度分别按照轨迹规划路径的函数编程：functionc=sita(t,T,a1,b1,c1,a2,b2,c2)m12=atan2(b2,a2);l2=850;l3=850;cs32=(a2^2+b2^2+c2^2-l2^2-l3^2)/(2*l2*l3);ss32=sqrt(1-cs32^2);m32=atan2(ss32,cs32);k22=l2+l3*cs32;k32=l3*ss32;m22=atan2(-c2*cos(m12),a2)-atan2(k32,k22);m11=atan2(b1,a1);cs31=(a1^2+b1^2+c1^2-l2^2-l3^2)/(2*l2*l3);ss31=sqrt(1-cs31^2);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/T^2;ifm12m11s1=-1;elseifm12=m11s1=1;endm1=m11+s1*(axmax/5.774)*(6*(t^5)/T^3-15*(t^4)/T^2+10*(t^3)/T);symax=abs(m22-m21);aymax=symax*5.774/T^2;ifm22m21s2=-1;elseifm22=m21s2=1;endm2=m21+s2*(aymax/5.774)*(6*(t^5)/T^3-15*(t^4)/T^2+10*(t^3)/T);szmax=abs(m32-m31);azmax=szmax*5.774/T^2;ifm32m31s3=-1;elseifm32=m31s3=1;endm3=m31+s3*(azmax/5.774)*(6*(t^5)/T^3-15*(t^4)/T^2+10*(t^3)/T);sita1=m1*180/pi;sita11=sita1;ifsita11180sita1=sita11-360;elseifsita11(-180)sita1=sita11+360;endsita2=m2*180/pi;sita22=sita2;ifsita22180sita2=sita22-360;elseifsita22(-180)sita2=sita22+360;endsita3=m3*180/pi;sita33=sita3;ifsita33180sita3=sita33-360;elseifsita33(-180)sita3=sita33+360;endc=[sita1;sita2;sita3];运动学模型建成的效果如下：我们输入初始位移坐标，以及终点位移坐标，末端会根据规划的轨迹由初始位置，移动到末端位置。九、Matlab动力学的仿真（1）对kuka机器人用solidworks进行建模建成模型如下：（2）安装simmechanicslink插件安装方法：安装方法步骤如下：1.选择对应的solidworks及matlab版本的插件下载（mathwork公司免费），不需要解压然后你打开MATLAB；2.将下载的两个文件所在目录置为MATLAB当前路径；3.在MATLAB命令窗口输入install_addon('add-onZIPfilename')命令,引号内的是你下载的压缩文件的名字；4.然后MATLAB就会将这个插件装上的;5.最后在