简单的控制算法，没有用Simulink工具，直接写代码。主要点就是分清楚仿真环境和需要验证的算法。也就是说首先要数学建模，建立一个‘环境’，用来模拟实际环境。然后写自己的控制算法。最后把算法用到‘环境’上，就是在仿真了。
全部代码如下：
%The simulation of the stable pendulum cart
%using PID,poleplacement and LQR
clc;
clear;
%parameter
m = 0.09; % The pendulum mass: m = 0.09kg;
M = 1.52; % The mobile cart mass: M = 1.52kg;
L = 0.24; % The 1/2 length of the pendulum: L = 0.24m;
I = m*(2*L)*(2*L)/12; % The inertia of the pendulum: I = m*(2L)^2/12;
Ts = 0.015; % The sampling time;
g = 9.81; % The gravity accelation: g = 9.81 N/m;
%modeling
temp_den = (m+M)*I+m*M*L*L;
i = m*g*L*(m+M)/temp_den;
j = -m*m*L*L*g/temp_den;
A = [0 1 0 0;
i 0 0 0;
0 0 0 1;
j 0 0 0];
B = [0 -m*L/temp_den 0 (I+m*L*L)/temp_den]';
C = [1 0 0 0;
0 0 1 0];
D =[0 0]';
[G H]=c2d(A,B,Ts);
x0=[-0.2 0 0.2 0]';
t=0:Ts:3;
u=zeros(size(t));
%without control
y0=dlsim(A,B,C,D,u,x0);
subplot(4,1,1);
plot(t,y0);
axis([0 3 -50 50]);
xlabel('t');
ylabel('θ&x');
title('未加控制');
legend('θ','x');
%poleplacement
P=[0.9277,-0.9489,0.9632,0.9704];
K=place(G,H,P);
G1=G-H*K;
y1=dlsim(G1,H,C,D,u,x0);
subplot(4,1,2);
plot(t,y1);
xlabel('t');
ylabel('x&θ');
title('极点配置');
legend('x','θ');
%LQR
Q=[200 0 0 0;
0 1 0 0;
0 0 200 0;
0 0 0 1];
R=0.1;
K=dlqr(G,H,Q,R)
G2=G-H*K;
y2=dlsim(G2,H,C,D,u,x0);
subplot(4,1,3);
plot(t,y2);
xlabel('t');
ylabel('x&θ');
title('LQR');
legend('x','θ');
%PID
y3=zeros(4,size(t,2));
sumx0=[0 0 0 0]';
u=0;
kp1=150;
ki1=3;
kd1=1000;
kp2=80;
ki2=1;
kd2=1500;
for i=1:size(t,2)
lastx0=x0;
x0=G*x0+H*u;
Dx0=x0-lastx0;
sumx0=sumx0+x0;
u=kp1*x0(1)+ki1*sumx0(1)+kd1*Dx0(1)+kp2*x0(3)+ki2*sumx0(3)+kd2*Dx0(3);
y3(:,i)=x0;
end
y3=[y3(1,:);y3(3,:)];
subplot(4,1,4);
plot(t,y3);
xlabel('t');
ylabel('x&θ');
title('PID');
legend('x','θ');
