KUKA KR40PA机器人运动学分析及simmulink仿真一．Kuka KR40PA码垛机器人简介Kuka KR40PA机器人是一种有四个自由度的码垛机器人，有四个驱动器，很好地运用了平行四边形机构，固定其姿态从而大大简化了控制难度，并且提高了精度及寿命，本文所用kuka码垛机器人如下图所示：二、机构简图，及其简化。
1、机构简图如下：第一步简化原因：第一步我们简化了两个平行四边形机构，在此我们分析，这两个平行四边形机构的作用是约束末端执行器在XZ平面的姿态，即：使末端执行器始终竖直向下。
在此我们人为的默认末端执行器始终竖直向下，不随前面传递构件的影响。
此时便可以将两组平行四边形机构去除而不影响末端执行器的姿态和位移。
第一步简化后机构简图第二步简化原因：在此我将主动杆1及连杆4去除。
杆1 2 3 4组成了一个平行四边形机构，因此β3=β2-β1.所以我们将杆1杆4去除，只要使β3=β2-β1便不影响末端执行器的位置和姿态。
第二步简化后的图形第三步简化原因：为了使参数更简洁，便于计算。
我们将杆2的第一个关节与第一个旋转轴相交，这样简化的模型更好计算。
不影响总体机构的功能。
最终简化后的机构简图三、建立连杆坐标系。
如下图：四、D-H参数表i αi-1a i-1d iθi1 0 0 0 θ12 -90 0 0 θ23 0 l20 θ34 0 l30 θ45 90 0 0 θ5五、求正运动学公式=======*==*==*=由于平行四边形机构的存在使得== * = * = 所以=== *=雅克比矩阵：==至此正解完成。
六、运动学逆解在此只对位置逆解进行分析，姿态逆解只与有关因此很简单就可以计算出来。
假设我们给出目标位置在0坐标系表示坐标为( X , Y , Z ) ,由变换矩阵我们可以得出：X= ①Y= ②Z= ③用②式除以①式可以得到tan=Y/X利用2幅角反正切公式可以得到=Atan2（y , x）。
①式与③式可以写成如下形式：X/c1= ④-Z= ⑤两式左右两边分别平方相加得到下式：+= ++2c3又因为tan1=Y/X可解得c3=(++--)/2S3=(在此期望值S3大于0，因此取正)再次利用2幅角反正切公式可以得到=Atan2（s3 , c3）。
①③可写成如下形式X/c1=c2-s2 ⑥-z= s2+c2 ⑦式中= += s3r=（，）那么⑥ ⑦式可以写成 X/(rc1)=c( +2) Z/(-r)=s ( +2)所以 + =Atan2（-zc1，X ）= Atan2（-zc1，X ）- （ ， ）至此逆解完成 七、轨迹规划：加速度表达式是一个三次多项式，速度表达式是一个四次多项式，位移表达式是一个五次多项式。
设S 为无量纲的运动位移，Γ为无量纲的运动总时间，V 、A 为无量纲的运动速度及加速度，那么3-4-5多项式运动曲线可以简单表示如下：f e d c b a S +Γ+Γ+Γ+Γ+Γ=2345 （3-1） e d 2c 3b 4a 5V 234+Γ+Γ+Γ+Γ= （3-2） e d 26c 12b a 20A 23++Γ+Γ+Γ=（3-3）根据如下边界条件：(1) 当0=Γ时，，，； (2) 当1=Γ时，，，。
将以上边界条件代入上述多项式表达式，可解出多项式的系数，，，，，。
于是，我们得到3-4-5多项式运动曲线的表达式：345Γ+ΓΓ=1015-6S （3-4） 2343060-30V Γ+ΓΓ=（3-5） Γ+ΓΓ=60180-120A 23（3-6）由加速度多项式可解出774.5max =A 。
为了得到带有量纲的多项式运动曲线表达式，我们令其加速度的最大值为m axa ，运行总时间为T ，运行瞬时时刻T ⨯Γ=t ，有)60180120(774.5a 23maxΓ+Γ-Γ=a（3-7）将上式对t 积分得：0=S 0=V 0=A 1=S 0=V 0=A 6a =15b -=10=c 0=d 0=e 0=f)306030(774.5v 234maxΓ+Γ-Γ=T a （3-8）同理有：)10156(774.5s 3452max Γ+Γ-Γ=T a（3-9）由此，当T t =时：2max 2max max1732.0774.5T a T a s s ⋅===（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<a1s1=-1;elseif a2>=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;if b2<b1s2=-1;elseif b2>=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;if c2<c1s3=-1;elseif c2>=c1endz=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<m11elseif m12>=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;if m22<m21s2=-1;elseif m22>=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;if m32<m31s3=-1;elseif m32>=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;if sita11>180sita1=sita11-360;elseif sita11<(-180)sita1=sita11+360;endsita2=m2*180/pi;sita22=sita2;if sita22>180sita2=sita22-360;elseif sita22<(-180)sita2=sita22+360;endsita3=m3*180/pi;sita33=sita3;if sita33>180sita3=sita33-360;elseif sita33<(-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-on ZIP file name>')命令,引号内的是你下载的压缩文件的名字；4. 然后MATLAB就会将这个插件装上的;5.最后在关联solidworks，主要分两步：1）在matlab命令窗口运行smlink_linksw，提示成功；2）打开solidworks，点击工具，选择插件，再选择 SimMechanics Link，将会看到SimMechanics Link新的菜单（注意需要在打开装配体*.SLDASM时才能看到）。