1.算法仿真效果
matlab2022a仿真结果如下：
2.算法涉及理论知识概要 倒立摆控制，使摆杆尽快达到平衡位置，且无大的振荡和过大的角度和速度的控制系统。当摆杆到达期望位置后，系统能克服随机扰动而保持稳定。该控制系统的输入为小车的位移（即位置）和摆杆的倾斜角度期望值，计算机在每一个采样周期中采集来自传感器的小车与摆杆的实际位置信号，与期望值进行比较后，通过控制算法得到控制量，再经数模转换驱动直流电机实现倒立摆的实时控制。按摆杆数量，分为一级、二级、三级倒立摆等。该系统是一个复杂的、不稳定的非线性系统，是进行控制理论研究的理想实验平台。对倒立摆系统的研究能有效地反映控制中的许多典型问题，如非线性问题、鲁棒性问题、镇定问题、随动问题以及跟踪问题等。
倒立摆，Inverted Pendulum ，是典型的多变量、高阶次 ，非线性、强耦合、自然不稳定系统。倒立摆系统的稳定控制是控制理论中的典型问题 ，在倒立摆的控制过程中能有效反映控制理论中的许多关键问题 ，如非线性问题、鲁棒性问题、随动问题、镇定、跟踪问题等。因此倒立摆系统作为控制理论教学与科研中典型的物理模型 ，常被用来检验新的控制理论和算法的正确性及其在实际应用中的有效性。从 20 世纪 60 年代开始 ，各国的专家学者对倒立摆系统进行了不懈的研究和探索。
倒立摆控制系统是一个复杂的、不稳定的、非线性系统，是进行控制理论教学及开展各种控制实验的理想实验平台。对倒立摆系统的研究能有效的反映控制中的许多典型问题：如非线性问题、鲁棒性问题、镇定问题、随动问题以及跟踪问题等。通过对倒立摆的控制，用来检验新的控制方法是否有较强的处理非线性和不稳定性问题的能力。同时，其控制方法在军工、航天、机器人和一般工业过程领域中都有着广泛的用途，如机器人行走过程中的平衡控制、火箭发射中的垂直度控制和卫星飞行中的姿态控制等。
本课程，我们的倒立摆模型如下：
3.MATLAB核心程序
global lp;
global mp;
global g;
LEN= 10000;
%离散
ts = 0.01;
mp = 0.17;
r = 0.16;
Jb = 0.0024;
R = 8.3;
Km = 0.023;
Kg = 120/16;
lp = 0.2;
g = 9.8;
%根据
a1 = -mp*r*g/Jb;
a2 = -Kg^2*Km^2/R/Jb;
a3 = g*(Jb + mp*r^2)/lp/Jb;
a4 = r*Kg*Kg*Km*Km/R/lp/Jb;
b1 = Kg*Km/R/Jb;
b2 = -r*Kg*Km/R/lp/Jb;
A = [0,0 ,1 ,0;
0,0 ,0 ,1;
0,a1,a2,0;
0,a3,a4,0];
B = [0;
0
b1;
b2];
[F,G] = c2d(A,B,ts);
Q = diag([1 1 0 0]);
R = [1];
[K,p,e] = dlqr(F,G,Q,R);
F = F-G*K;
%初始条件
x1_0 = 0;
x2_0 = pi/2;
x3_0 = 0;
x4_0 = pi/2;
u_0 = 10;
for k=1:1:LEN
k
.........................................................................
%PF
X(k) = func_ukf_filter(y);
u_0 = X(k);
x1_0 = x1(k);
x2_0 = x2(k);
x3_0 = x3(k);
x4_0 = x4(k);
end
