KUKA机器人运动学分析及simmulink仿真(共16页).docx

精选优质文档-----倾情为你奉上 KUKA KR40PA机器人运动学分析及simmulink仿真1． Kuka KR40PA码垛机器人简介Kuka KR40PA机器人是一种有四个自由度的码垛机器人，有四个驱动器，很好地运用了平行四边形机构，固定其姿态从而大大简化了控制难度，并且提高了精度及寿命，本文所用kuka码垛机器人如下图所示：2、 机构简图，及其简化1、 机构简图如下：第一步简化原因：第一步我们简化了两个平行四边形机构，在此我们分析，这两个平行四边形机构的作用是约束末端执行器在XZ平面的姿态，即：使末端执行器始终竖直向下在此我们人为的默认末端执行器始终竖直向下，不随前面传递构件的影响此时便可以将两组平行四边形机构去除而不影响末端执行器的姿态和位移第一步简化后机构简图第二步简化原因：在此我将主动杆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、 求正运动学公式 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 所以 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-l3s1s23-s1s2l2-l3s1s23000-l3c23-l2c2-l3c23000-s1-s1-s100c1c1c-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)/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 =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）由加速度多项式可解出为了得到带有量纲的多项式运动曲线表达式，我们令其加速度的最大值为，运行总时间为，运行瞬时时刻，有 （3-7）将上式对积分得： （3-8）同理有： （3-9）由此，当时： （3-10）八、基于matlab simmechanics工具箱的运动学仿真在matlab命令窗口中输入 simulink命令回车 弹出simulink library browser如下图： 新建一个model ，在左侧 libraries 中选择simscape 接着选择simmechanics 将我们见立模型的构建分别拖到model并组成下图形式 ，其中 各模块参数输入如下： 环境模块 关节模块 Body0 的参数 Body1的参数 Body2的参数 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/T^2;if a2=a1 s1=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;if b2=b1 s2=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;if c2=c1 s3=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、基于关节旋转角度分别按照轨迹规划路径的函数编程：function c =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;if m12=m11 s1=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/。