RRPR机械臂simulink建模及仿真

                                  RRPR机械臂建模及仿真

（1）机器人建模

在solidworks中绘制机械臂的简单模型，将零件文件导入simulink中，完成坐标系变换的设置，完成机械臂的搭建。

 

                                                                     Simulink机械臂搭建图

 

完成搭建后使用改进DH方法建立连杆坐标系，完成DH参数表，并求解工具坐标系到基坐标系的变换矩阵。

                           

                                                                       DH模型图

DH参数表为：

通过程序fk_test求得工具坐标系到基坐标系的变换矩阵为：

最后设置机械臂材料的密度为2.7g/cm3 ，完成建模。

 

                                        

                                                                 Simulink 仿真结果图

（2）反解模块

function [theta1,theta2,d,theta3] = my_ik(x,y,z,fi)

%输入x,y，z以及抓手的旋转角度

%输出关节空间的位置

p=sqrt(x^2+y^2);

u0=sqrt(1-((p^2+49/400-1/16)/((7/10)*p))^2); %也可取负值

u1=(p^2+49/400-1/16)/(7/10*p);

theta1=atan2(u0,u1)+atan2(y,x);

theta2=atan2(4*(cos(theta1)*y-sin(theta1)*x),4*(cos(theta1)*x+sin(theta1)*y-7/20));

d=1/8-z;

theta3=theta1+theta2-atan2(sin(fi),cos(fi));

end

（3）轨迹规划模块

采用关节空间的规划方法，根据教材上介绍的具有中间点的路径的三次多项式方法，当中间点速度已知时，可以得到

function [sdd,sd,s] = my_traj(Q,tf,t)

%输入关节经过的位置Q，Q为各个位置构成的向量，tf为位置点间的间隔时间，t为自然时间（仿真时间）

%输出轨迹，轨迹速度，轨迹加速度

n=length(Q)-1;

qd=[0];

 

for i=1:n-1

    qd=[qd my_vmid(Q(i),Q(i+1),Q(i+2),tf)]; %求中间点速度

end

qd0=qd;

qdf=[qd(2:end) 0];

if(t==0)   %跳过对0时间点的运算，否则仿真不成功

    i=1;

else

    i=ceil(t/tf); %向上取整

end

    X = [0 0 0 1;

        tf^3 tf^2 tf 1;

        0 0 1 0;

        3*tf^2 2*tf 1 0

    ];

    coeffs = (X \ [Q(i) Q(i+1) qd0(i) qdf(i)]')';

 

    coeffs_d = coeffs(1:3) .* (3:-1:1);

    coeffs_dd = coeffs_d(1:2) .* (2:-1:1);

 

    p = polyval(coeffs, t-(i-1)*tf);

    pd = polyval(coeffs_d, t-(i-1)*tf);

    pdd = polyval(coeffs_dd, t-(i-1)*tf);

   

    s=p;

    sd=pd;

    sdd=pdd;

end

   

    function v=my_vmid(x1,x2,x3,tf)

        v1=(x2-x1)/tf;

        v2=(x3-x2)/tf;

        if(v1*v2>=0)

            v=(v1+v2)/2;

        else

            v=0;

        end

end　

（4）单关节控制器

f为输出的力矩，q为位置反馈，w为速度反馈；qdd_in，qd_in，q_in分别为轨迹规划模块产生的期望的加速度，速度，位置的值。

通过仿真调节PID参数，得到

关节1控制器的Kp=74500，Kv=332，Ki=230000；位置误差曲线为

 

关节2控制器的Kp=89000，Kv=75，Ki=500000；位置误差曲线为

 

关节3控制器的Kp=365000，Kv=490，Ki=490000；位置误差曲线为

 

关节4控制器的Kp=99000，Kv=130，Ki=350000；位置误差曲线为

综上，四个控制器的误差总体控制在1%左右。

 

机械臂simulink仿真模型图

这是笔者完成的一个课程大作业，源文件已上传，于（2020.6.12）

链接：https://download.csdn.net/download/weixin_42075625/12518458