写一个离散LQR对汽车悬架在受到道路干扰主动控制的MATLAB代码

首先需要定义汽车悬架的动态模型，这里假设汽车悬架的状态量为 $x = [z, z', \theta, \theta']^T$，其中 $z$ 表示车身的垂直位移，$z'$ 表示车身的垂直速度，$\theta$ 表示车轮的倾斜角度，$\theta'$ 表示车轮的倾斜角速度。汽车悬架的控制输入量为 $u$，表示车身上的外部控制力。则动态方程可以表示为：

$$
\begin{aligned}
\dot{x} &= \begin{bmatrix}
0 & 1 & 0 & 0\\
\frac{k_2}{m_2} & -\frac{(k_1+k_2)}{m_2} & \frac{k_1}{m_2} & 0\\
0 & 0 & 0 & 1\\
\frac{k_1}{m_1l} & 0 & -\frac{(k_1+k_2)}{m_1l} & 0
\end{bmatrix}x + \begin{bmatrix}
0\\ \frac{1}{m_2}\\ 0\\ \frac{1}{m_1l}
\end{bmatrix}u\\
y &= \begin{bmatrix}
1 & 0 & 0 & 0\\
0 & 0 & 1 & 0
\end{bmatrix}x
\end{aligned}
$$

其中 $m_1$ 和 $m_2$ 分别表示车轮和车身的质量，$k_1$ 和 $k_2$ 分别表示车轮和车身的弹性系数，$l$ 表示车轮到车身的距离。控制目标为使车身的垂直位移和车轮的倾斜角度尽可能地接近给定值。

然后就可以编写离散LQR控制器的MATLAB代码：

% 设计汽车悬架的动态模型
m1 = 250; % 车轮质量
m2 = 1000; % 车身质量
k1 = 80000; % 车轮弹性系数
k2 = 160000; % 车身弹性系数
l = 2; % 车轮到车身的距离

A = [0, 1, 0, 0; k2/m2, -(k1+k2)/m2, k1/m2, 0; 0, 0, 0, 1; k1/(m1*l), 0, -(k1+k2)/(m1*l), 0];
B = [0; 1/m2; 0; 1/(m1*l)];
C = [1, 0, 0, 0; 0, 0, 1, 0];
D = 0;

% 设计控制目标
Q = diag([10, 1, 10, 1]); % 状态权重矩阵
R = 0.1; % 控制输入权重

% 计算LQR控制器的增益矩阵
[K, ~, ~] = dlqr(A, B, Q, R);

% 仿真汽车悬架的控制效果
t = 0:0.01:10; % 时间范围
x0 = [0; 0; 0; 0]; % 初始状态
r = [0.1*ones(1,length(t)); 0.05*sin(2*pi*t)]; % 目标状态
u = zeros(1,length(t)); % 初始控制输入

for i = 2:length(t)
    x = A*x0 + B*u(i-1); % 系统动态方程
    y = C*x; % 系统输出方程
    
    % 计算离散LQR控制器的控制输入
    u(i) = -K*(x-r(:,i));
    
    % 更新状态
    x0 = x;
end

% 画出控制效果图像
figure;
subplot(2,1,1);
plot(t, r(1,:), '-r', t, C*x, '-b');
xlabel('Time (s)');
ylabel('Vertical Displacement (m)');
legend('Reference', 'Response');
title('Vertical Displacement Control');
subplot(2,1,2);
plot(t, r(2,:), '-r', t, C*x, '-b');
xlabel('Time (s)');
ylabel('Wheel Tilt Angle (rad)');
legend('Reference', 'Response');
title('Wheel Tilt Angle Control');