├── .gitignore ├── ADRC.m ├── Differentail_Evolution.m ├── LADRC.m ├── Neural.m ├── PD_Controller_3.m ├── PID_controller.m ├── PID_controller_2.m ├── PID_diff_advance.m ├── PMS_Generator.slx ├── README.md ├── Rosenbrock.m ├── Servo_Joint.slx ├── TD_controller.m └── tracer_filter.m /.gitignore: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # 此 .gitignore 文件已由 Microsoft(R) Visual Studio 自动创建。 3 | ################################################################################ 4 | 5 | /.vs 6 | /ADRC.asv 7 | *.asv 8 | -------------------------------------------------------------------------------- /ADRC.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Cking616/Adavance_PID/4d594c991c14b8373751e3c386c34e919d935bfc/ADRC.m -------------------------------------------------------------------------------- /Differentail_Evolution.m: -------------------------------------------------------------------------------- 1 | clear all; 2 | close all; 3 | 4 | Size=30; 5 | CodeL=2; 6 | 7 | MinX(1)=-2.048; 8 | MaxX(1)=2.048; 9 | MinX(2)=-2.048; 10 | MaxX(2)=2.048; 11 | 12 | G=50; 13 | F=0.6; 14 | cr=0.9; 15 | for i=1:1:CodeL 16 | P(:,i)=MinX(i)+(MaxX(i)-MinX(i))*rand(Size,1); 17 | end 18 | 19 | BestS=P(1,:); 20 | for i=2:Size 21 | if (Rosenbrock(P(i,1),P(i,2))>Rosenbrock(BestS(1),BestS(2))) 22 | BestS=P(i,:); 23 | end 24 | end 25 | 26 | fi = Rosenbrock(BestS(1),BestS(2)) 27 | 28 | for kg=1:1:G 29 | time(kg)=kg; 30 | for i=1:Size 31 | r1=1;r2=1;r3=1; 32 | while(r1==r2||r1==r3||r2==r3||r1==i||r2==i||r3==i) 33 | r1=ceil(Size*rand(1)); 34 | r2=ceil(Size*rand(1)); 35 | r3=ceil(Size*rand(1)); 36 | end 37 | 38 | h(i,:)=P(r1,:)+F*(P(r2,:)-P(r3,:)); 39 | 40 | for j=1:CodeL 41 | if h(i,j)MaxX(j) 44 | h(i,j)=MaxX(j); 45 | end 46 | end 47 | 48 | for j=1:1:CodeL 49 | tempr=rand(1); 50 | if(temprRosenbrock(P(i,1),P(i,2))) 58 | P(i,:)=v(i,:); 59 | end 60 | 61 | if(Rosenbrock(P(i,1),P(i,2))>fi) 62 | fi = Rosenbrock(P(i,1),P(i,2)); 63 | BestS=P(i,:); 64 | end 65 | end 66 | Best_f(kg)=Rosenbrock(BestS(1),BestS(2)); 67 | end 68 | BestS 69 | Best_f(kg) 70 | figure(1); 71 | plot(time,Best_f(time),'k','linewidth',2); 72 | 73 | function J=Rosenbrock(x1,x2) 74 | J=100*(x1^2-x2)^2+(1-x1)^2; 75 | end 76 | 77 | -------------------------------------------------------------------------------- /LADRC.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Cking616/Adavance_PID/4d594c991c14b8373751e3c386c34e919d935bfc/LADRC.m -------------------------------------------------------------------------------- /Neural.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Cking616/Adavance_PID/4d594c991c14b8373751e3c386c34e919d935bfc/Neural.m -------------------------------------------------------------------------------- /PD_Controller_3.m: -------------------------------------------------------------------------------- 1 | clear all; 2 | close all; 3 | 4 | ts=0.0005; 5 | sys=tf(1, [0.1,1,0]); 6 | dsys=c2d(sys,ts,'z'); 7 | [num,den]=tfdata(dsys,'v'); 8 | 9 | %sys1=tf(1, [0.5,1]); 10 | %dsys1=c2d(sys1,ts,'tucsin'); 11 | %[num1, den1]=tfdata(dsys1,'v'); 12 | %disp(den1(2)); 13 | %disp(num1(1)); 14 | num1 = 475 / 1024;den1 = 549 / 1024;den2 = 0; 15 | u_1=0.0;u_2=0.0;u_3=0.0; 16 | yd_1=0;yd_2=0;yd_3=0; 17 | y_1=0;y_2=0;y_3=0; 18 | v_1=0;v_2=0;v_3=0; 19 | kp_0=200 * 1000 / 2048;kvff=1;kaff=0; 20 | t_po = 30; 21 | cur_e = 0; 22 | kp=kp_0; 23 | Tc = 3; 24 | 25 | x=[0,0,0]'; 26 | error_1=0; 27 | 28 | for k=1:1:20000 29 | time(k)=k*ts; 30 | 31 | S=4; 32 | if S==1 33 | if mod(time(k),5) < 1 34 | yd(k)=mod(time(k),1); 35 | elseif mod(time(k),5) < 4 36 | yd(k)=1; 37 | else 38 | yd(k)=1-mod(time(k),1); 39 | end 40 | yd(k)=yd(k); 41 | elseif S==2 42 | if mod(time(k),2) < 1 43 | yd(k)=mod(time(k),1); 44 | else 45 | yd(k)=1-mod(time(k),1); 46 | end 47 | yd(k)=yd(k); 48 | elseif S==3 49 | yd(k)=mod(time(k),1.0); 50 | elseif S==4 51 | if mod(time(k),10) < 2 52 | yd(k)=(time(k)) * (time(k)); 53 | elseif mod(time(k),10) < 3 54 | yd(k)= time(k) * 4 - 4; 55 | elseif mod(time(k),10) < 5 56 | yd(k)=12 - (5 - time(k)) * (5 - time(k)); 57 | elseif mod(time(k),10) < 7 58 | yd(k)= 12 - (time(k) - 5) * (time(k) - 5); 59 | elseif mod(time(k),10) < 8 60 | yd(k)= 8 - (time(k) - 7) * 4; 61 | else 62 | yd(k)= (10 - time(k)) * (10 - time(k)); 63 | end 64 | yd(k)=yd(k); 65 | end 66 | 67 | if v_1 > 0 68 | if v_2 > 0 69 | kaff = 2.0; 70 | else 71 | kaff = 1.4; 72 | end 73 | else 74 | if v_2 > 0 75 | kaff = 0; 76 | else 77 | kaff = 1.8; 78 | end 79 | end 80 | 81 | if cur_e > 0 82 | if cur_e > t_po 83 | kp = 1.55 * kp_0; 84 | else 85 | kp = (1.5 * cur_e / t_po + 0.05) * kp_0; 86 | end 87 | else 88 | if cur_e < -t_po 89 | kp = 1.55 * kp_0; 90 | else 91 | kp = (1.5 * (-cur_e) / t_po + 0.05) * kp_0; 92 | end 93 | end 94 | u_1=kp*x(1)+kvff*x(2)+kaff*x(3); 95 | u_1= u_1 * 924 / 1024 + u_2 * 50 / 1024 + u_3 * 50 / 1024; 96 | u(k)=u_1; 97 | 98 | if u(k)>=5 99 | u(k)=5; 100 | end 101 | if u(k)<=-5; 102 | u(k)=-5; 103 | end 104 | 105 | acc(k) = (u(k) - u_2) / ts; 106 | if k > 1 107 | if acc(k) - acc(k - 1) > Tc 108 | acc(k) = acc(k - 1) + Tc; 109 | end 110 | 111 | if acc(k) - acc(k - 1) < -Tc 112 | acc(k) = acc(k - 1) - Tc; 113 | end 114 | else 115 | if acc(k) > Tc 116 | acc(k) = Tc; 117 | end 118 | 119 | if acc(k) < -Tc 120 | acc(k) = - Tc; 121 | end 122 | end 123 | 124 | if acc(k)>=15 125 | acc(k)=15; 126 | end 127 | if acc(k)<=-15 128 | acc(k)=-15; 129 | end 130 | 131 | u(k) = u_2 + ts * acc(k); 132 | u_1 = u(k); 133 | 134 | y(k)=-den(2)*y_1-den(3)*y_2+num(2)*u_1+num(3)*u_2; 135 | error(k)=yd(k)-y(k); 136 | 137 | yd_3=yd_2;yd_2=yd_1;yd_1=yd(k); 138 | y_3=y_2;y_2=y_1;y_1=y(k); 139 | u_3=u_2;u_2=u_1; 140 | v_1 = (yd_1-yd_2) / ts; 141 | 142 | if v_1 > 5 143 | v_1=5; 144 | end 145 | 146 | if v_1 < -5 147 | v_1=-5; 148 | end 149 | 150 | v_1 = den1 * v_2 + num1 * v_1 + den2 * v_3; 151 | 152 | x(1)=error(k); 153 | x(2)= v_1; 154 | x(3)=(v_1 - v_2)/ ts; 155 | if x(3) > 0.1 156 | x(3) = 0.1; 157 | end 158 | if x(3) < -0.1 159 | x(3) = -0.1; 160 | end 161 | v_2 = v_1; 162 | v_3 = v_1; 163 | 164 | error_1=error(k); 165 | error_100(k) = error(k) * 100 * 1000; 166 | cur_e = error_100(k); 167 | end 168 | 169 | figure(1); 170 | plot(time,yd,'r',time,y,'k:','linewidth',2); 171 | legend('Ideal position signal','Position tracking'); 172 | 173 | figure(2); 174 | plot(time,error_100,'r','linewidth',2); 175 | legend('error position'); 176 | 177 | figure(3); 178 | plot(time,u,'r','linewidth',2); 179 | legend('speed'); 180 | 181 | figure(4); 182 | plot(time,acc,'r','linewidth',2); 183 | legend('acceleration'); 184 | -------------------------------------------------------------------------------- /PID_controller.m: -------------------------------------------------------------------------------- 1 | clear all; 2 | close all; 3 | 4 | ts = 0.001; 5 | sys = tf(5.235e005, [1, 87.35, 1.047e004, 0]); 6 | dsys = c2d(sys, ts, 'Z'); 7 | [num, den]=tfdata(dsys,'v'); 8 | 9 | u_1=0.0;u_2=0.0;u_3=0.0; 10 | y_1=0.0;y_2=0.0;y_3=0.0; 11 | x=[0,0,0]; 12 | error_1=0 13 | for k=1:1:500 14 | time(k)=k*ts; 15 | 16 | S=1; 17 | if S==1 18 | kp=0.50;ki=0.001;kd=0.001; 19 | yd(k)=1; 20 | elseif S==2 21 | kp=0.50;ki=0.001;kd=0.001; 22 | yd(k)=sign(sin(2*2*pi*k*ts)); 23 | elseif S==3 24 | kp=1.5;ki=1.0;kd=0.01; 25 | yd(k)=0.5*sin(2*2*pi*k*ts); 26 | end 27 | 28 | u(k)=kp*x(1)+kd*x(2)+ki*x(3); 29 | if u(k)>=10 30 | u(k)=10; 31 | end 32 | if u(k)<=-10 33 | u(k)=10; 34 | end 35 | 36 | y(k)=-den(2)*y_1-den(3)*y_2-den(4)*y_3+num(2)*u_1+num(3)*u_2+num(4)*u_3; 37 | 38 | error(k)=yd(k)-y(k); 39 | 40 | u_3=u_2;u_2=u_1;u_1=u(k); 41 | y_3=y_2;y_2=y_1;y_1=y(k); 42 | 43 | x(1)=error(k); 44 | x(2)=(error(k)-error_1)/ts; 45 | x(3)=x(3)+error(k)*ts; 46 | 47 | error_1=error(k); 48 | end 49 | 50 | figure(1); 51 | plot(time, yd, 'r', time, y, 'k:', 'linewidth', 2); 52 | legend('Ideal position signal','Position tracking'); 53 | -------------------------------------------------------------------------------- /PID_controller_2.m: -------------------------------------------------------------------------------- 1 | clear all; 2 | close all; 3 | 4 | ts=0.001; 5 | sys=tf(5.235e0005,[1,87.35,1.047e004,0]) 6 | dsys=c2d(sys,ts,'Z'); 7 | [num,den]=tfdata(dsys,'v'); 8 | 9 | u_1=0.0;u_2=0.0;u_3=0.0; 10 | yd_1=rand; 11 | y_1=0;y_2=0;y_3=0; 12 | 13 | x=[0,0,0]; 14 | error_1=0; 15 | 16 | for k=1:1:3000 17 | time(k)=ts*k; 18 | 19 | kp=1.0;ki=2.0;kd=0.01; 20 | 21 | S=1; 22 | if S==1 23 | if mod(time(k),2)<1 24 | yd(k)=mod(time(k),1); 25 | else 26 | yd(k)=1-mod(time(k),1); 27 | end 28 | yd(k)=yd(k)-0.5; 29 | end 30 | if S==2 31 | yd(k)=mod(time(k),1.0); 32 | end 33 | if S==3 34 | yd(k)=rand; 35 | dyd(k)=(yd(k)-yd_1)/ts; 36 | while abs(dyd(k))>=5.0 37 | yd(k)=rand; 38 | dyd(k)=abs(yd(k)-yd_1)/ts; 39 | end 40 | end 41 | 42 | u(k)=kp*x(1)+kd*x(2)+ki*x(3); 43 | 44 | if u(k)>=10 45 | u(k)=10; 46 | end 47 | if u(k)<=-10; 48 | u(k)=-10; 49 | end 50 | 51 | y(k)=-den(2)*y_1-den(3)*y_2-den(4)*y_3+num(2)*u_1+num(3)*u_2+num(4)*u_3; 52 | error(k)=yd(k)-y(k); 53 | 54 | yd_1=yd(k); 55 | 56 | u_3=u_2;u_2=u_1;u_1=u(k); 57 | y_3=y_2;y_2=y_1;y_1=y(k); 58 | 59 | x(1)=error(k); 60 | x(2)=(error(k)-error_1)/ts; 61 | x(3)=x(3)+error(k)*ts; 62 | xi(k)=x(3); 63 | 64 | error_1=error(k); 65 | D=0; 66 | if D==1 67 | plot(time, yd, 'b', time, y ,'r'); 68 | pause(0.0000000000); 69 | end 70 | end 71 | figure(1); 72 | plot(time, yd, 'r', time, y , 'k:', 'linewidth', 2); 73 | legend('Ideal position signal', 'Position tracking'); 74 | 75 | -------------------------------------------------------------------------------- /PID_diff_advance.m: -------------------------------------------------------------------------------- 1 | clear all; 2 | close all; 3 | 4 | ts = 20; 5 | sys =tf( 1,[60, 1],'inputdelay',80); 6 | dsys = c2d(sys,ts,'zoh'); 7 | [num,den] = tfdata(dsys,'v') 8 | 9 | u_1=0;u_2=0;u_3=0;u_4=0;u_5=0; 10 | ud_1=0; 11 | y_1=0;y_2=0;y_3=0; 12 | error_1=0;error_2=0; 13 | ei=0; 14 | for k=1:1:400 15 | time(k)=k*ts; 16 | 17 | y(k)=-den(2)*y_1+num(2)*u_5; 18 | 19 | kp=0.36;kd=14;ki=0.0021; 20 | 21 | yd(k)=1.0*sign(sin(0.00025*2*pi*k*ts)); 22 | yd(k)=yd(k) + 0.05*sin(0.03*pi*k*ts); 23 | 24 | error(k)=yd(k) - y(k); 25 | ei=ei + error(k)*ts; 26 | 27 | gama=0.50; 28 | Td=kd/kp; 29 | Ti=0.5; 30 | c1=gama*Td/(gama*Td+ts); 31 | c2=(Td+ts)/(gama*Td+ts); 32 | c3=Td/(gama*Td+ts); 33 | 34 | M=1; 35 | if M==1 36 | ud(k)=c1*ud_1+c2*y(k)-c3*y_1; 37 | u(k)=kp*error(k) + ud(k) + ki*ei; 38 | elseif M==2 39 | u(k)= kp*error(k) + kd*(error(k)-error_1)/ts + ki * ei; 40 | end 41 | 42 | if u(k)>=110 43 | u(k)=110; 44 | end 45 | if u(k)<=-110 46 | u(k)=-110; 47 | end 48 | 49 | u_5=u_4;u_4=u_3;u_3=u_2;u_2=u_1;u_1=u(k); 50 | y_3=y_2;y_2=y_1;y_1=y(k); 51 | 52 | error_2=error_1; 53 | error_1=error(k); 54 | end 55 | 56 | figure(1); 57 | plot(time,yd,'r',time,y,'k:','linewidth',2) 58 | %xlabel('time(s)');ylable('yd,y'); 59 | legend('Ideal position signal','Position tracking'); 60 | figure(2); 61 | plot(time,u,'r','linewidth',2); 62 | %xlable('time(s)');ylable('u'); 63 | -------------------------------------------------------------------------------- /PMS_Generator.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Cking616/Adavance_PID/4d594c991c14b8373751e3c386c34e919d935bfc/PMS_Generator.slx -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Adavance_PID 2 | 学习先进PID控制一书的控制算法，并按照真实系统要求，组合部分算法调节参数以满足真实项目需求。 3 | -------------------------------------------------------------------------------- /Rosenbrock.m: -------------------------------------------------------------------------------- 1 | clear all; 2 | close all; 3 | 4 | x_min=-2.048; 5 | x_max=2.048; 6 | 7 | L=x_max-x_min; 8 | N=101; 9 | for i=1:1:N 10 | for j=1:1:N 11 | x1(i)=x_min+L/(N-1)*(i-1); 12 | x2(j)=x_min+L/(N-1)*(j-1); 13 | fx(i,j)=100*(x1(i)^2-x2(j))^2 + (1-x1(i))^2; 14 | end 15 | end 16 | figure(1); 17 | surf(x1,x2,fx); 18 | 19 | display('Maximum valus of fx='); 20 | disp(max(max(fx))); -------------------------------------------------------------------------------- /Servo_Joint.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Cking616/Adavance_PID/4d594c991c14b8373751e3c386c34e919d935bfc/Servo_Joint.slx -------------------------------------------------------------------------------- /TD_controller.m: -------------------------------------------------------------------------------- 1 | close all; 2 | clear all; 3 | 4 | T=0.001; 5 | y_1=0;yp_1=0; 6 | dy_1=0; 7 | 8 | a=25;b=133; 9 | sys=tf(b,[1,a,0]); 10 | dsys=c2d(sys, T , 'z'); 11 | [num,den]=tfdata(dsys,'v'); 12 | u_1=0;u_2=0; 13 | p_1=0;p_2=0; 14 | 15 | for k=1:1:5000 16 | t=k*T; 17 | time(k)=t; 18 | 19 | yd(k)=sin(t); 20 | dyd(k)=cos(t); 21 | p(k)=-den(2)*p_1-den(3)*p_2+num(2)*u_1+num(3)*u_2; 22 | 23 | d(k)=0.5*sign(rands(1)); 24 | if mod(k,100)==1 || mod(k,100)==2 25 | yp(k)=p(k)+d(k); 26 | else 27 | yp(k)=p(k); 28 | end 29 | 30 | M=2; 31 | if M==1 32 | y(k)=yp(k); 33 | dy(k)=(yp(k)+yp_1)/T; 34 | elseif M==2 35 | alfa=8;nmna=6; 36 | y(k)=y_1+T*(dy_1-nmna*sqrt(abs(y_1-yp(k)))*sign(y_1-yp(k))); 37 | dy(k)=dy_1-T*alfa*sign(y_1-yp(k)); 38 | end 39 | kp=10;kd=0.1; 40 | u(k)=kp*(yd(k)-y(k))+kd*(dyd(k)-dy(k)); 41 | 42 | y_1=y(k); 43 | yp_1=yp(k); 44 | dy_1=dy(k); 45 | 46 | u_2=u_1;u_1=u(k); 47 | p_2=p_1;p_1=p(k); 48 | end 49 | 50 | if M == 1 51 | figure(1); 52 | plot(time, yd, 'k', time, p, 'r:', 'linewidth', 2); 53 | legend('ideal position siganal','position tracking'); 54 | elseif M==2 55 | figure(1); 56 | subplot(211); 57 | plot(time, p, 'k',time, yp, 'r:', time, y, 'b:', 'linewidth',2) 58 | legend('ideal position siganal','position tracking'); 59 | subplot(212); 60 | plot(time,dy,'k','linewidth',2); 61 | legend('speed siganal by TD'); 62 | figure(2); 63 | plot(time, yd,'r', time,p,'k:','linewidth',2); 64 | legend('ideal position siganal','position tracking'); 65 | end 66 | -------------------------------------------------------------------------------- /tracer_filter.m: -------------------------------------------------------------------------------- 1 | clear all; 2 | close all; 3 | 4 | ts = 0.001; 5 | x = [0,0]; 6 | for k=1:1:3000 7 | time(k) = k *ts; 8 | 9 | u0=1.0*sin(1*2*pi*k*ts); 10 | u = u0 + 0.1 * rands(1); 11 | 12 | r =1800; 13 | h = 0.015; 14 | T = ts; 15 | delta = r * h; 16 | delta0 = delta * h; 17 | y = x(1) - u + h * x(2); 18 | a0 = sqrt(delta * delta + 8 *r * abs(y)); 19 | if abs(y) <= delta0 20 | a = x(2) + y / h; 21 | else 22 | a = x(2) + 0.5 *(a0 -delta) *sign(y); 23 | end 24 | if abs(a) <=delta 25 | fst2 = -r * a / delta; 26 | else 27 | fst2 = -r * sign(a); 28 | end 29 | 30 | x(1) = x(1) + T *x(2); 31 | x(2) = x(2) + T * fst2; 32 | rin0(k) = u0; 33 | rin(k) = u; 34 | x1(k) = x(1); 35 | end 36 | figure(1); 37 | plot(time, rin, 'k', time, x1, 'k'); 38 | xlabel('time(s)'); 39 | ylabel('rin,x1'); 40 | figure(2); 41 | plot(time,rin0, 'k', time, x1, 'k'); 42 | xlabel('time(s)'); 43 | ylabel('rin0,x1'); 44 | --------------------------------------------------------------------------------