文末获取Matlab源码源文件+完整建模设计报告
一、状态空间建模
忽略空气阻力、各种摩擦之后，可将倒立摆系统抽象为小车和匀质杆组成的系统。单级倒立摆模型结构如图2.1所示。其中，小车质量M（kg），摆杆质量m（kg），小车摩擦系数b（N/m/sec），摆杆转动轴心到杆质心的长度（m），摆杆惯量I（kg*m2），加在小车上的力F（N），小车位置x（m），摆杆与垂直向上方向的夹角φ（rad），摆杆与垂直向下方向的夹角θ（rad）。设小车与摆杆相互作用力的水平和垂直方向的分量为N和P。分析小车水平方向所受的合力，可以得到以下方程：
由摆杆水平方向的受力进行分析可以得到下面等式：
联立上式，得到系统的第一个运动方程：
对摆杆垂直方向上的合力进行分析，得到下面方程：
力矩平衡方程：
联立以上方程，得到第二个运动方程：
设θ=φ+π（φ是摆杆与垂直向上方向之间的夹角），假设φ与1rad相比很小，即φ<<1，则可以进行近似处理：
用u来代表被控对象的输入力F，线性化后得到两个运动方程：
进一步变换得到系统的状态空间方程：
二、模型离散化
对模型中各个参数进行现实取值：小车质量M=1.096kg，摆杆质量m=0.109kg，小车摩擦系数b=0.1 N/m/sec，摆杆转动轴心到杆质心的长度=0.25m，摆杆惯量I=0.0034kg*m2。将上述参数代入状态空间方程得到：
（其余完整详见文末）
三、模型实现与实验
使用Matlab建立模型，使用LQR算法控制算法实现倒立摆实验，设置初态为[0; 2; 0; 1]，设置输入为0，得到角度和位移曲线如图1所示。
提取其中的位移曲线，如图2所示。假设位移信息读取受到严重噪声干扰，加入均值为0、方差为0.4的高斯噪声以模拟此干扰。受到干扰后的位移曲线如图3所示。
合理设置参数，使用卡尔曼滤波处理被噪声干扰的信号，滤波效果如图4所示。
分析卡尔曼滤波效果图可得，卡尔曼滤波有效地减少了噪声的影响。
四、附录
实验代码（其余完整详见文末）
clc;
clear;
% 输入状态空间建模信息
A = [0 1 0 0;
0 -0.0883 0.6293 0;
0 0 0 1;
0 -0.2357 27.8285 0];
B = [0;
0.8832;
0;
2.3566];
C = [1 0 0 0;
0 0 1 0];
D = 0;
% 模型离散化
T = 0.02;
[AA,BB] = c2d(A, B, T);
% 倒立摆引入LQR控制算法
Q = [1 0 0 0;
0 1 0 0;
0 0 2 0;
0 0 0 2];
R = 1;
K = lqr(AA, BB, Q, R);
Ac = AA - BB*K;
% 确定模型初态、模型输入，并求解模型运动状态
x0=[0;
2;
0;
1];
t = 0:0.01:9.99;
u = zeros(size(t));
[y, x] = lsim(Ac, BB, C, D, u, t, x0);
figure(1);
plot(t, y(:, 1), "b", "LineWidth", 1);
hold on;
plot(t, y(:, 2), "r", "LineWidth", 1);
legend('位移', '角度');
xlabel('时间/s');
figure(2);
% 提取位移信息
y_position = y(:,1);
plot(t, y_position, "LineWidth", 1);
xlabel('时间/s');
% 假设位移信息读取受到严重噪声干扰，加入高斯噪声
Z = y_position;
for i = 1:length(Z)
Z(i) = y_position(i) + normrnd(0,0.4);
end
figure(3);
plot(t, Z);
xlabel('时间/s');
r = 0.017;
D = Z-y_position;
X = Z(1);
T = 0.03;
V = 0.1;
Q = T^2*V^2;
R = r^2;
P = X^2;
A = 1;
H = 1;
Xn = KalmanFilter(X, Z, A, Q, H, R, P, y_position);