单级倒立板的控制——PID和LQR

博大广阔

function a=handstand()
%单级倒立板控制——PID和LQR
clear all
close all
global A
global B
%%%%%%%%%%%%%%%%%%%%%%%%%%construcr system%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
g=9.8; M=1;m=0.1;  %car mass and link mass
L=0.5;             %the half length of link
uc=0.0005; up=0.000002;%rub cofficient
I=(1/12)*m*L^2; l=0.5*L;
den=(M+m)*I+M*m*l^2;
t1=m*(m+M)*g*l/den;
t2=-m^2*g*l^2/den;
t3=-m*l/den;
t4=(I+m*l^2)/den;
A=[0 1 0 0;
    t1 0 0 0;
    0 0 0 1;
    t2 0 0 0];
B=[0 t3 0 t4].';
Q=[100 10 1 1];Q=diag(Q);%100 10 1 1 expreass importance of theta dtheta x dx
R=[0.1];                  %energy
Ke=lqr(A,B,Q,R);        %LQG gain :this state that A ,B is right
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%initinal value and system parameter%%%%%
x=[-10/57.3 ,0,0.2, 0].'; %initial value or initial state
ts=0.02; tmax=10; %simulation time and time step
Kp=[-50 -10 -10 -10];Kd=[-10 -10 -10 -10];
Mm=2;
%%%%%%%%%%%%%%%%%%%%%%%%%loop needed parameters%%%%%%%%%%%%%%%%%
dE=[0 0 0 0].';E_1=dE;
u_1=0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%simulation begin%%%%%%%%%%%%%%%%%%%%%%%
for k=1:1:1000
%%%%%%%%%%%%%%%%%%%%%%%%%control model%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Tspan=[0 ts];
%[t,x]=ode45('fun',Tspan,xk,[],u_1)
X=x;uu=u_1;h=ts;
K1=A*X+B*uu;K2=A*(X+h*K1/2)+B*uu;            %fouth_order this is every good!!
K3=A*(X+h*K2/2)+B*uu; K4=A*(X+h*K3)+B*uu;
X=X+(h/6)*(K1+K2*2+2*K3+K4);
R=[0 0 0 0].';
E=R-X;
if Mm==1;u=Ke*E;end
if Mm==2;u=Kp*E+Kd*dE;end
if u>=10;u=10;end;if u<=-10;u=-10;end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%parameter update%%%%%%%%%%%%%%%%%%%%%
dE=E-E_1; u_1=u;x=X;
E_1=E;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%save output%%%%%%%%%%%%%%%%%%%%%%
time(k)=ts*k; YY(:,k)=X;
end
plot(time,YY,'r');grid on
end