MATLAB实战：5分钟搞定倒立摆LQR控制（附完整代码）

MATLAB实战5分钟搞定倒立摆LQR控制附完整代码倒立摆作为经典的控制系统教学案例一直是学习自动控制的必经之路。但很多初学者在面对复杂的数学推导和代码实现时往往感到无从下手。本文将带你用最短的时间跳过繁琐的理论推导直接上手实现一个完整的倒立摆LQR控制系统。1. 准备工作与环境配置在开始之前我们需要确保MATLAB环境已经准备就绪。建议使用MATLAB R2018b或更高版本因为后续代码中会用到一些较新的绘图函数。首先创建一个新的MATLAB脚本文件命名为inverted_pendulum_lqr.m。我们将在这个文件中完成所有代码的编写。为了后续调试方便建议在脚本开头添加以下清理命令clc; % 清空命令窗口 clear all; % 清空工作区变量 close all; % 关闭所有图形窗口接下来我们需要定义倒立摆的物理参数。这些参数将直接影响系统的动态特性% 系统物理参数 M 2.0; % 小车质量(kg) m 0.1; % 摆杆质量(kg) l 0.5; % 摆杆半长(m) b1 0.1; % 小车摩擦系数 b2 0.1; % 摆杆摩擦系数 g 9.8; % 重力加速度(m/s^2)2. 建立状态空间模型LQR控制需要基于系统的状态空间模型进行设计。对于倒立摆系统我们通常选择以下四个状态变量小车位置x小车速度ẋ摆杆角度θ摆杆角速度θ̇基于这些状态变量我们可以建立系统的状态空间方程。下面是完整的建模代码% 计算中间参数 L 2*l; % 摆杆全长 J (1/3)*m*L^2; % 摆杆转动惯量 JJ J m*l^2; % 修正转动惯量 N (Mm)*JJ - m^2*l^2; % 系统参数组合 % 状态空间矩阵A A [0 1 0 0; 0 -b1*JJ/N -m^2*g*l^2/N b2*m*l/N; 0 0 0 1; 0 b1*m*l/N (Mm)*m*g*l/N -b2*(Mm)/N]; % 输入矩阵B B [0; JJ/N; 0; -m*l/N]; % 输出矩阵C C [1 0 0 0; % 输出小车位置 0 0 1 0]; % 输出摆杆角度 % 直接传递矩阵D D [0; 0];3. LQR控制器设计LQR控制的核心是选择合适的权重矩阵Q和R。这两个矩阵决定了系统对不同状态变量和控制输入的重视程度。% 设计LQR控制器 Q diag([1000, 1, 100, 1]); % 状态权重矩阵 R 1; % 控制输入权重 % 计算最优反馈增益矩阵K K lqr(A, B, Q, R); % 计算闭环系统矩阵 Ac A - B*K;这里有几个关键点需要注意Q矩阵对角元素对应四个状态变量的权重R矩阵标量值控制输入信号的权重lqr函数会自动计算最优反馈增益4. 系统仿真与结果分析现在我们可以对闭环系统进行仿真了。我们将使用脉冲响应来测试系统的稳定性。% 仿真时间设置 t 0:0.01:15; % 0到15秒步长0.01秒 % 创建开环和闭环系统对象 sys_open ss(A, B, C, D); sys_closed ss(Ac, B, C, D); % 计算脉冲响应 [y_open, t_open] impulse(sys_open, t); [y_closed, t_closed] impulse(sys_closed, t); % 提取开环和闭环响应数据 x_open y_open(:,1); % 开环小车位置 theta_open y_open(:,2); % 开环摆杆角度 x_closed y_closed(:,1); % 闭环小车位置 theta_closed y_closed(:,2); % 闭环摆杆角度为了更直观地观察控制效果我们可以绘制响应曲线% 绘制位置响应曲线 figure(1); plot(t_closed, x_closed, LineWidth, 2); grid on; grid minor; xlabel(时间(s)); ylabel(小车位置(m)); title(LQR控制下的小车位置响应); set(gca, FontSize, 12); % 绘制角度响应曲线 figure(2); plot(t_closed, theta_closed*180/pi, LineWidth, 2); grid on; grid minor; xlabel(时间(s)); ylabel(摆杆角度(°)); title(LQR控制下的摆杆角度响应); set(gca, FontSize, 12);5. 参数调优与性能提升LQR控制器的性能很大程度上取决于Q和R矩阵的选择。下面是一些调优建议小车位置权重增大Q(1,1)会使系统更关注位置控制摆杆角度权重增大Q(3,3)会增强摆杆稳定性控制输入权重增大R会限制控制输入的大小我们可以通过以下代码快速测试不同参数组合% 测试不同Q矩阵 Q_tests {diag([1000,1,100,1]), diag([500,1,200,1]), diag([2000,1,50,1])}; legends {Q1,Q2,Q3}; figure(3); hold on; for i 1:length(Q_tests) K_test lqr(A, B, Q_tests{i}, R); Ac_test A - B*K_test; sys_test ss(Ac_test, B, C, D); y_test impulse(sys_test, t); plot(t, y_test(:,2)*180/pi, LineWidth, 2); end grid on; grid minor; xlabel(时间(s)); ylabel(摆杆角度(°)); title(不同Q矩阵下的角度响应对比); legend(legends); set(gca, FontSize, 12);6. 完整代码整合将所有代码整合到一个文件中方便直接运行clc; clear all; close all; % 1. 系统参数设置 M 2.0; m 0.1; l 0.5; b1 0.1; b2 0.1; g 9.8; % 2. 状态空间建模 L 2*l; J (1/3)*m*L^2; JJ J m*l^2; N (Mm)*JJ - m^2*l^2; A [0 1 0 0; 0 -b1*JJ/N -m^2*g*l^2/N b2*m*l/N; 0 0 0 1; 0 b1*m*l/N (Mm)*m*g*l/N -b2*(Mm)/N]; B [0; JJ/N; 0; -m*l/N]; C [1 0 0 0; 0 0 1 0]; D [0; 0]; % 3. LQR控制器设计 Q diag([1000, 1, 100, 1]); R 1; K lqr(A, B, Q, R); Ac A - B*K; % 4. 系统仿真 t 0:0.01:15; sys ss(Ac, B, C, D); y impulse(sys, t); % 5. 结果可视化 figure(1); plot(t, y(:,1), LineWidth, 2); grid on; title(小车位置响应); xlabel(时间(s)); ylabel(位置(m)); figure(2); plot(t, y(:,2)*180/pi, LineWidth, 2); grid on; title(摆杆角度响应); xlabel(时间(s)); ylabel(角度(°));7. 实际应用中的注意事项在实际应用中有几点需要特别注意传感器噪声实际系统中的传感器测量都存在噪声需要在控制器中加入滤波环节执行器饱和小车电机或舵机都有输出限制需要在仿真中考虑饱和非线性采样时间数字控制系统的采样时间会影响性能通常选择系统带宽的5-10倍下面是一个考虑执行器饱和的修改示例% 修改后的仿真考虑执行器饱和 u_max 10; % 最大控制输入 t_sim 15; dt 0.01; t_vec 0:dt:t_sim; x0 [0; 0; 0.1; 0]; % 初始角度0.1弧度 % 手动仿真 x x0; x_history zeros(4, length(t_vec)); for i 1:length(t_vec) u -K*x; % 计算控制输入 u max(min(u, u_max), -u_max); % 饱和限制 x x dt*(A*x B*u); % 欧拉积分 x_history(:,i) x; end % 绘制结果 figure(3); subplot(2,1,1); plot(t_vec, x_history(1,:)); title(考虑饱和的小车位置); xlabel(时间(s)); ylabel(位置(m)); subplot(2,1,2); plot(t_vec, x_history(3,:)*180/pi); title(考虑饱和的摆杆角度); xlabel(时间(s)); ylabel(角度(°));这个示例展示了如何在仿真中加入执行器饱和限制使仿真结果更接近实际情况。