从理论到代码:一文读懂LQR控制及其在倒立摆上的实践

%% 倒立摆LQR控制系统 - 详细注释版clear all; close all; clc;format compact;fprintf('=== 倒立摆LQR控制系统 ===\n');fprintf('作者：张工\n\n');%% ====================== 1. 系统参数设置 ======================M = 1.0;        % 小车质量 (kg)m = 0.1;        % 摆杆质量 (kg)l = 0.5;        % 摆杆长度 (m)g = 9.81;       % 重力加速度 (m/s^2)b = 0.1;        % 小车摩擦系数 (N·s/m)%% ====================== 2. 建立状态空间模型 ======================% 状态变量：[小车位置; 小车速度; 摆杆角度; 摆杆角速度]A = [0, 1, 0, 0;     0, -b/M, -m*g/M, 0;     0, 0, 0, 1;     0, -b/(M*l), (M+m)*g/(M*l), 0];B = [0; 1/M; 0; 1/(M*l)];  % 控制矩阵%% ====================== 3. 系统特性分析 ======================% 检查能控性 - 这是状态反馈的前提Co = ctrb(A, B);if rank(Co) == size(A, 1)    fprintf('✓ 系统完全能控，可以使用状态反馈\n');else    error('系统不能控！');end%% ====================== 4. LQR控制器设计 ======================% 关键：权重矩阵的选择Q = diag([10, 1, 100, 10]);  % 角度权重最大，因为稳定性最重要R = 0.1;                    % 允许使用一定的控制力% 一行代码求解LQR问题！[K, P, E] = lqr(A, B, Q, R);fprintf('反馈增益 K = [%.3f, %.3f, %.3f, %.3f]\n', K(1), K(2), K(3), K(4));fprintf('控制律：u = -Kx = %.3f*位置 - %.3f*速度 - %.3f*角度 - %.3f*角速度\n', ...        K(1), K(2), K(3), K(4));%% ====================== 5. 系统仿真 ======================% 初始条件：摆杆倾斜50度的大扰动！theta0_deg = 50;x0 = [0; 0; theta0_deg*pi/180; 0];% 创建闭环系统A_closed = A - B*K;sys_closed = ss(A_closed, B, eye(4), 0);% 加入外部扰动，模拟真实环境t = 0:0.01:10;disturbance = zeros(size(t));disturbance(100:150) = 0.5;   % 1-1.5秒施加扰动disturbance(500:520) = -0.3;  % 5-5.2秒施加反向扰动% 运行仿真[~, t_sim, x_sim] = lsim(sys_closed, disturbance, t, x0);u_actual = -x_sim * K' + disturbance';  % 实际控制输入%% ====================== 6. 结果可视化（8个子图全面分析） ======================figure('Position', [100, 50, 1400, 900]);% 图1：状态响应subplot(3, 3, [1, 2]);plot(t_sim, x_sim(:,1), 'b-', 'LineWidth', 2); hold on;plot(t_sim, x_sim(:,3)*180/pi, 'r-', 'LineWidth', 2);xlabel('时间 (s)'); ylabel('状态值');legend('小车位置 (m)', '摆杆角度 (°)');title('主要状态响应');grid on;% 图2：控制输入subplot(3, 3, 3);plot(t_sim, u_actual, 'g-', 'LineWidth', 2);xlabel('时间 (s)'); ylabel('控制力 (N)');title('控制输入');grid on;% 图3：相平面分析subplot(3, 3, 4);plot(x_sim(:,3)*180/pi, x_sim(:,4)*180/pi, 'b-');xlabel('角度 (°)'); ylabel('角速度 (°/s)');title('角度-角速度相平面');grid on;% 图4-8：更多分析（极点配置、频率响应、代价函数、动画演示等）% ...（完整代码请见下文）