机器人运动学、动力学基础上利用MATLAB进行PID控制仿真

这是我的第一次写博客，不足之处还请谅解

进入正题

因为用SIMLINK做PID控制时，根据力矩反求加速度，再将得到的角度和角速度反馈回去继续重复计算，存在代数环问题仿真不出来，我一直没有找到解决办法，所以自己慢慢琢磨，只能用matlab代码直接进行PID控制仿真了，可能不是很合理，供参考

根据自己的结构推导出运动学方程和动力学方程

给定理想轨迹

x= 10*t*cos(5*t)+886;
y= 10*t*sin(5*t);
z= 20*t+700;

clc;clear;

for i=0:1:900
t=i/100;
x (i+1)= 10*t*cos(5*t)+886;
y (i+1)= 10*t*sin(5*t);
z (i+1)= 20*t+700;

end
plot3(x,y,z,'r-','LineWidth',1.5);
hold on;

xlabel('X/mm');
ylabel('Y/mm');
zlabel('Z/mm');
grid on;

 

推导出的关节角与时间t的关系

关节角1：q1 =atan((10*t*sin(5*t))/(10*t*cos(5*t) + 886));

关节角2：q2 =- acos(((20*t + 700)^2 + (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2 + 19404)/(2800*((20*t + 700)^2/4 + (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2/4)^(1/2))) - atan((20*t + 700)/(2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100));


关节角3：q3 =pi/2 - acos(4901/4900 - (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2/960400 - (20*t + 700)^2/960400);

生成理论上的关节角与时间的变化曲线

clc;
clear;
%生成理论上的角度曲线
for i=0:1:900
t = 0.01*i
x= 10*t*cos(5*t)+886;
y= 10*t*sin(5*t);
z= 20*t+700;
OB4=[x,y,z];  
q1=atan(y/x);
q2=-(acos((700^2+(sqrt(x^2+y^2)-100)^2+z^2-686^2)/(2*700*sqrt((sqrt(x^2+y^2)-100)^2+z^2)))+atan(z/(sqrt(x^2+y^2)-100)));
q3=pi/2-acos((700^2+686^2-(sqrt(x^2+y^2)-100)^2-z^2)/(2*700*686));
s1(i+1)=q1; 
s2(i+1)=q2; 
s3(i+1)=q3; 
end
j=0:0.01:9;
plot(j,s1)
plot(j,s2)
plot(j,s3)
hold on

利用动力学公式，给定力矩，求解此时刻的角加速度

qdd=inv(M)*(t_0-H);%反求给定力矩，角度，角速度，此时的加速度是多少

进而可以得到下一时刻的角速度和角度，再代会循环计算

根据PID原理，计算此时求得的角度以及角速度与理论上的角度和角速度的差，作为误差和误差变化率，输入PID控制器，输出力矩

clc;
clear;
%给初始值
t=0;
q1 =atan((10*t*sin(5*t))/(10*t*cos(5*t) + 886));
q2 =- acos(((20*t + 700)^2 + (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2 + 19404)/(2800*((20*t + 700)^2/4 + (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2/4)^(1/2))) - atan((20*t + 700)/(2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100));
q3 =pi/2 - acos(4901/4900 - (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2/960400 - (20*t + 700)^2/960400);
q1d =((10*sin(5*t))/(10*t*cos(5*t) + 886) + (50*t*cos(5*t))/(10*t*cos(5*t) + 886) - (10*t*sin(5*t)*(10*cos(5*t) - 50*t*sin(5*t)))/(10*t*cos(5*t) + 886)^2)/((100*t^2*sin(5*t)^2)/(10*t*cos(5*t) + 886)^2 + 1);
q2d =((800*t + (2*(2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)*(50*t*sin(5*t)^2 + ((10*cos(5*t) - 50*t*sin(5*t))*(10*t*cos(5*t) + 886))/2 + 250*t^2*cos(5*t)*sin(5*t)))/((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) + 28000)/(2800*((20*t + 700)^2/4 + (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2/4)^(1/2)) - ((200*t + ((2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)*(50*t*sin(5*t)^2 + ((10*cos(5*t) - 50*t*sin(5*t))*(10*t*cos(5*t) + 886))/2 + 250*t^2*cos(5*t)*sin(5*t)))/(2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2)) + 7000)*((20*t + 700)^2 + (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2 + 19404))/(5600*((20*t + 700)^2/4 + (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2/4)^(3/2)))/(1 - ((20*t + 700)^2 + (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2 + 19404)^2/(1960000*(20*t + 700)^2 + 1960000*(2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2))^(1/2) - (20/(2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100) - ((20*t + 700)*(50*t*sin(5*t)^2 + ((10*cos(5*t) - 50*t*sin(5*t))*(10*t*cos(5*t) + 886))/2 + 250*t^2*cos(5*t)*sin(5*t)))/(((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2)*(2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2))/((20*t + 700)^2/(2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2 + 1);
q3d =-((2*t)/2401 + ((2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)*(50*t*sin(5*t)^2 + ((10*cos(5*t) - 50*t*sin(5*t))*(10*t*cos(5*t) + 886))/2 + 250*t^2*cos(5*t)*sin(5*t)))/(480200*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2)) + 10/343)/(1 - ((20*t + 700)^2/960400 + (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2/960400 - 4901/4900)^2)^(1/2);
Ee=0;%定义一个误差的总和
for i = 0:1:900
    t=0.01*i;

q_1 =atan((10*t*sin(5*t))/(10*t*cos(5*t) + 886));
q_2 =- acos(((20*t + 700)^2 + (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2 + 19404)/(2800*((20*t + 700)^2/4 + (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2/4)^(1/2))) - atan((20*t + 700)/(2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100));
q_3 =pi/2 - acos(4901/4900 - (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2/960400 - (20*t + 700)^2/960400);
q_1d =((10*sin(5*t))/(10*t*cos(5*t) + 886) + (50*t*cos(5*t))/(10*t*cos(5*t) + 886) - (10*t*sin(5*t)*(10*cos(5*t) - 50*t*sin(5*t)))/(10*t*cos(5*t) + 886)^2)/((100*t^2*sin(5*t)^2)/(10*t*cos(5*t) + 886)^2 + 1);
q_2d =((800*t + (2*(2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)*(50*t*sin(5*t)^2 + ((10*cos(5*t) - 50*t*sin(5*t))*(10*t*cos(5*t) + 886))/2 + 250*t^2*cos(5*t)*sin(5*t)))/((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) + 28000)/(2800*((20*t + 700)^2/4 + (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2/4)^(1/2)) - ((200*t + ((2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)*(50*t*sin(5*t)^2 + ((10*cos(5*t) - 50*t*sin(5*t))*(10*t*cos(5*t) + 886))/2 + 250*t^2*cos(5*t)*sin(5*t)))/(2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2)) + 7000)*((20*t + 700)^2 + (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2 + 19404))/(5600*((20*t + 700)^2/4 + (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2/4)^(3/2)))/(1 - ((20*t + 700)^2 + (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2 + 19404)^2/(1960000*(20*t + 700)^2 + 1960000*(2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2))^(1/2) - (20/(2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100) - ((20*t + 700)*(50*t*sin(5*t)^2 + ((10*cos(5*t) - 50*t*sin(5*t))*(10*t*cos(5*t) + 886))/2 + 250*t^2*cos(5*t)*sin(5*t)))/(((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2)*(2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2))/((20*t + 700)^2/(2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2 + 1);
q_3d =-((2*t)/2401 + ((2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)*(50*t*sin(5*t)^2 + ((10*cos(5*t) - 50*t*sin(5*t))*(10*t*cos(5*t) + 886))/2 + 250*t^2*cos(5*t)*sin(5*t)))/(480200*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2)) + 10/343)/(1 - ((20*t + 700)^2/960400 + (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2/960400 - 4901/4900)^2)^(1/2);
                                                                                                                                                                                                                    
e=[q_1-q1;q_2-q2;q_3-q3]; 
ec=[q_1d-q1d;q_2d-q2d;q_3d-q3d];
Ee=Ee+e;
KP=600;
KI=30;
KD=2;
 
t_0=KP*e+KI*Ee+KD*ec;%PID控制输出力矩
    
M =[(241529529139*cos(2*q2))/400000000000 - (26897521*sin(2*q2 + q3))/100000000 + (2669931*sin(2*q2))/500000000 - (85345834133*cos(2*q2 + 2*q3))/2000000000000 - (3842503*sin(q2 + q3))/50000000 + (5091931*cos(q2))/10000000 + (99*sin(q2))/25000 - (26897521*sin(q3))/100000000 + 1920346974018873/2251799813685248,                                                                 0,                                                                 0;
                                                                                                                                                                                                                                                                                                                0,   5823838324126837/4503599627370496 - (26897521*sin(q3))/50000000, 6149815468784047/72057594037927936 - (26897521*sin(q3))/100000000;
                                                                                                                                                                                                                                                                                                                0, 6149815468784047/72057594037927936 - (26897521*sin(q3))/100000000,                                6149815468784047/72057594037927936];
H =[(99*q1d*q2d*cos(q2))/25000 - (26897521*q1d*q3d*cos(q3))/100000000 - (5091931*q1d*q2d*sin(q2))/10000000 - (26897521*q1d*q2d*cos(2*q2 + q3))/50000000 - (26897521*q1d*q3d*cos(2*q2 + q3))/100000000 + (2669931*q1d*q2d*cos(2*q2))/250000000 - (241529529139*q1d*q2d*sin(2*q2))/200000000000 + (85345834133*q1d*q2d*sin(2*q2 + 2*q3))/1000000000000 + (85345834133*q1d*q3d*sin(2*q2 + 2*q3))/1000000000000 - (3842503*q1d*q2d*cos(q2 + q3))/50000000 - (3842503*q1d*q3d*cos(q2 + q3))/50000000;
   (1263543455108301*sin(q2 + q3))/335544320000000 - (6697588617537127*cos(q2))/268435456000000 - (4851*sin(q2))/25000 - (99*q1d^2*cos(q2))/50000 - (26897521*q3d^2*cos(q3))/100000000 + (5091931*q1d^2*sin(q2))/20000000 + (26897521*q1d^2*cos(2*q2 + q3))/100000000 - (2669931*q1d^2*cos(2*q2))/500000000 + (241529529139*q1d^2*sin(2*q2))/400000000000 - (85345834133*q1d^2*sin(2*q2 + 2*q3))/2000000000000 + (3842503*q1d^2*cos(q2 + q3))/100000000 - (26897521*q2d*q3d*cos(q3))/50000000;
                                                                                                                                                                                                                        (1263543455108301*sin(q2 + q3))/335544320000000 + (26897521*q1d^2*cos(q3))/200000000 + (26897521*q2d^2*cos(q3))/100000000 + (26897521*q1d^2*cos(2*q2 + q3))/200000000 - (85345834133*q1d^2*sin(2*q2 + 2*q3))/2000000000000 + (3842503*q1d^2*cos(q2 + q3))/100000000];
 

qdd=inv(M)*(t_0-H);%反求给定力矩，角度，角速度，此时的加速度是多少
q1dd=qdd(1);
q2dd=qdd(2);
q3dd=qdd(3);
%得到下一时刻的角度和角速度                                                                                                                                                                                                                 
    q1d=q1d+q1dd*0.01;
    q2d=q2d+q2dd*0.01;
    q3d=q3d+q3dd*0.01;
    q1=q1+q1d*0.01+q1dd*0.01*0.01;
     q2=q2+q2d*0.01+q2dd*0.01*0.01;
      q3=q3+q3d*0.01+q3dd*0.01*0.01;                                                                                                                                                                                                                    
                                                                                                                                                                                                                    
                                                                                                                                                                                                                    
                                                                                                                                                                                                             
end

 通过调节PID的参数，减小轨迹跟踪误差

这里是关节2的跟踪效果，还不好，还要继续调节参数

 后续，还可以利用模糊控制，进行模糊PID控制提高控制性能。