二阶倒立摆控制器设计（连续+离散极点配置、连续+离散观测器设计、lqr最优控制）

以下是matlab的全部代码，分为很多块，运行时应按照顺序。在运行代码前应已经设置好ABCD四个状态空间矩阵。

%全部的代码都在这里了,在验证时先导入工作空间中的所有数据（这样ABCD四个矩阵就进来了），
%再分别将各个代码块复制到命令行运行实现其功能.
%首先矩阵ABCD的输入就省略了

%连续系统和离散系统的建立
sys = ss(A,B,C,D);
Ts = 0.01;%默认采样时间为0.01s
sysd = c2d(sys,Ts);
F = sysd.A;
G = sysd.B;
Cd = sysd.C;
D = 0;

%1 矩阵的能控性、能观性
Gc = ctrb(A,B);
disp(rank(Gc));%看一看能控性矩阵的秩，为6，则系统完全能控
Go = obsv(A,C);
disp(rank(Go));%看一看能观性矩阵的秩，为6，则系统完全能观。

% 连续系统转离散系统
Gcd = ctrb(sysd);
disp(rank(Gcd));%看一看离散系统能控性矩阵的秩，为6，完全能控
God = obsv(sysd);
disp(rank(God));%看一看离散系统能观性矩阵的秩，为6，完全能观；

%系统稳定性
pzmap(sys);%看连续系统极点分布，有正极点，系统不稳定
pzmap(sysd);%看离散系统极点分布，有极点在单位圆外部，系统不稳定
step(sys);%看连续系统阶跃响应，发散，不稳定；
step(sysd);%离散系统阶跃响应，发散，不稳定；

%连续系统极点配置
P = [-2.34+1.59j -2.34-1.59j -28.3 -28.3 -28.3 -28.3];%要配置的极点信息
K = acker(A,B,P);%利用acker()函数，计算出反馈矩阵K;
A1 = A-B*K;%极点配置后的"A"矩阵A1
sys1 = ss(A1,B,C,D);%极点配置后的新系统
step(sys1);%观察配置极点后新系统的阶跃响应，稳定.
pzmap(sys1);

%离散系统极点配置
Pd = [0.9767+0.0155i 0.9767-0.0155i 5.12e-13 5.12e-13 5.12e-13 5.12e-13];%将之前的s平面的极点投射到z平面上来。
Kd = acker(F,G,Pd);
F1 = F-G*Kd;
sysd1 = sysd;
sysd1.A = F1;%sysd1为配置了极点后的离散系统
step(sysd1);%观察其阶跃响应，系统稳定
%pzmap(sysd1);

%连续系统观测器设计
V = [-7.02 -7.02 -28 -29 -27 -28.1]; %状态观测器的极点信息设计，由于acker方法此处无法使用且place方法在此处要求不得出现2重以上重根，故进行这样的极点设置。
L = place(A',C',V)';
A0 = A-B*K-L*C;
B0 = L;
C0 = -K;

%离散系统状态观测器设计
Pd = [0.9767+0.0155i 0.9767-0.0155i 5.12e-13 5.12e-13 5.12e-13 5.12e-13];
Kd = acker(F,G,Pd);
Vd = [exp(-7.0200) exp(-7.0200) exp(-28.0000) exp(-29.0000) exp(-27.0000) exp(-28.1000)];%同样将离散
Ld = place(F',Cd',Vd)';
F0 = F-G*Kd-Ld*Cd;
G0 = Ld;
C0 = -Kd;

%LQR最优控制设计
q11 = 2500;    %代表小车位置的权重
q22 = 500;    %代表摆杆 1 角度的权重
q33 = 500;    %代表摆杆 2 角度的权重
q44 = 0;    %小车速度的权重
q55 = 0;    %摆杆 1 角速度的权重
q66 = 0;    %摆杆 2 角速度的权重
Q = [q11 0   0   0   0   0   ;
     0   q22 0   0   0   0   ;
     0   0   q33 0   0   0   ;
     0   0   0   q44 0   0   ;
     0   0   0   0   q55 0   ;
     0   0   0   0   0   q66];
R = 0.1;      %控制能量的限制
Klqr = lqr(A,B,Q,R);
Alqr = A - B*Klqr;
Blqr = B;
Clqr = C;
Dlqr = 0;
syslqr = ss(Alqr,Blqr,Clqr,Dlqr);
step(syslqr);