简介倒立摆自适应动态规划ADPMATLAB仿真资料包面向控制理论、强化学习与MATLAB编程学习者重点演示如何用ADP算法实现单级倒立摆的稳定控制适合作为自动化、机器人或电气相关专业的课程设计与毕业设计参考。压缩包共4个文件约1.72MB两个m脚本分别承担主仿真流程与倒立摆动力学模型一个CAJ文档提供基于近似动态规划的倒立摆控制参考论文另有JPG图片展示随机起始状态下的控制效果。资源已有1635人学习下载说明该实例在相关学习者中有较高热度。源码以完整可运行的MATLAB仿真为主体包含系统建模、ADP迭代求解、仿真循环与结果可视化使用者可直接修改模型参数观察控制器在不同初始条件下逐步将摆杆稳定至垂直平衡的过程CAJ文献进一步补充算法推导与结果分析帮助读者将仿真代码与理论方法对照理解快速复现ADP倒立摆控制实验。1. 倒立摆 MATLAB 仿真一条从动力学到闭环控制的完整链路倒立摆是最典型的欠驱动非线性系统小车加摆杆两个自由度却只有一个控制输入。教材讲它是为了引出状态空间和 LQR工程上做自平衡小车、机器人运控的人也先用倒立摆仿真验证算法再碰真实硬件。用 MATLAB 写倒立摆仿真程序本质是把「建模—线性化—设计控制律—验证」这条链路完整跑通。程序的价值不在复现课本公式而在每一环都变成可调、可看、可出错的代码。曲线发散、符号写反、权重设错每个问题都会逼你把背后的理论再想一遍这正是它作为入门项目至今没被替代的原因。2. 建立倒立摆动力学模型用 ode45 跑通开环 MATLAB 仿真2.1 小车-摆杆模型的运动方程怎么来常见做法取「小车 摆杆」二自由度模型这也是自平衡小车、双轮机器人的通用等价模型。小车质量 M摆杆质量 m摆杆长度 l重力加速度 g状态取四个小车位移 p、速度 ṗ、摆杆角度 θ、角速度 θ̇。θ 从直立位置算起θ 0 是竖直向上θ π 是自然下垂外力 F 沿水平方向作用在小车质心。用拉格朗日方程推出两个耦合的二阶方程(Mm) p̈ m l θ̈ cosθ − m l θ̇² sinθ F m l² θ̈ m l p̈ cosθ − m g l sinθ 0θ̇² 项是摆杆旋转带来的向心加速度贡献是非线性的主要来源之一线性化时会被直接忽略但它决定了开环大角度扰动下系统行为的真实性。θ 定义在直立点这一点很关键教材从下垂点建模时符号会差一个负号后面线性化、算 LQR 增益都会联动出错建议在代码注释里固定这个约定。2.2 把方程写成 ode45 能直接计算的函数ode45 要求把方程改写成 dstate/dt f(t, state) 的形式。上面的方程对 p̈ 和 θ̈ 是耦合的手解显式表达式容易抄错我一般用质量矩阵法写成 M(q)·acc rhs在函数里用左除解加速度。% cart_pole_dynamics.m function dstate cart_pole_dynamics(t, state, params, F) % state [p; p_dot; theta; theta_dot] M params(1); m params(2); l params(3); g params(4); p state(1); p_dot state(2); theta state(3); theta_dot state(4); Mq [M m, m * l * cos(theta); m * l * cos(theta), m * l^2]; rhs [F - m * l * theta_dot^2 * sin(theta); m * g * l * sin(theta)]; acc Mq \ rhs; % 解出 [p_ddot; theta_ddot] dstate [p_dot; acc(1); theta_dot; acc(2)]; end逻辑说明Mq \ rhs是求解线性方程组Mq * acc rhs的标准写法比inv(Mq) * rhs数值上更稳尤其摆杆接近水平、矩阵条件数变大的区间左除几乎不放大误差。刻意不展开 p̈ 和 θ̈ 的显式表达式也是为了避免手写长公式时符号出错。参数说明params顺序固定为[M, m, l, g]调用处不要调换F是外力开环仿真先给 0。角度单位统一用弧度绘图时再用rad2deg转。如果看到角度曲线一开始就往负方向走说明初始状态符号与模型约定不一致先检查约定不要急着调控制器。2.3 开环仿真先让摆杆倒下验证模型物理行为初始状态我习惯取[0; 0; 0.1; 0]摆杆偏离直立约 5.7°小车静止。零外力仿真两秒摆杆应在重力作用下倒下小车被反作用力推向另一侧。这一步是检验模型符号正确性的最快路径摆杆往正方向倒小车应该往负方向移动才符合动量守恒。% run_open_loop.m params [1.0, 0.2, 0.5, 9.81]; % M1kg, m0.2kg, l0.5m tspan [0, 2]; state0 [0; 0; 0.1; 0]; % 轻微偏离直立位 [t, states] ode45((t, s) cart_pole_dynamics(t, s, params, 0), ... tspan, state0); figure; plot(t, rad2deg(states(:,3)), LineWidth, 1.2); xlabel(时间 (s)); ylabel(摆杆角度 (deg)); grid on;参数说明tspan只给起点和终点时MATLAB 自动决定输出点密度后面做动画再改成linspace(0, 2, 2000)强制均匀输出。states(:,3)取角度整列rad2deg只用于显示。跑完应看到角度从 0.1 rad 增长到接近 π如果曲线变平或出现台阶多半是误差容限下没采够点用odeset(RelTol, 1e-6)作为第三个参数传入ode45再跑。后续仿真统一用这张物理参数表M、m、l、g 固定后剩下的变量只有 Q、R 和初始角度参数含义数值单位M小车质量1.0kgm摆杆质量0.2kgl摆杆长度0.5mg重力加速度9.81m/s²θ0初始摆角相对直立0.1rad3. 倒立摆状态空间线性化与 LQR 控制器设计Q、R 决定控制风格3.1 在直立平衡点做线性化写出 A、B 矩阵闭环控制的目标是把摆杆维持在 θ 0 附近。在这个平衡点做小角度近似cosθ ≈ 1sinθ ≈ θθ̇² 作为高阶小量忽略。代入上面的非线性方程并整理成 ẋ Ax Bu 标准形式A [0 1 0 0; 0 0 -m*g/M 0; 0 0 0 1; 0 0 (Mm)g/(Ml) 0]B [0; 1/M; 0; -1/(M*l)]x [p; ṗ; θ; θ̇]输出取小车位移和摆杆角度时 C [1 0 0 0; 0 0 1 0]D 0。注意 A(4,3) (Mm)g/(Ml)这一项随 l 增大而减小特征值越靠近虚轴系统越接近临界状态这也解释了为什么实物摆杆越长越难控制。线性化模型只负责设计控制器仿真验证必须回到非线性模型两者不要混用。3.2 用 lqr() 算反馈增益Q、R 按这个顺序调LQR 极小化 J ∫(xᵀQx uᵀRu)dtQ 是状态权重对角阵对应 [p, ṗ, θ, θ̇]R 是控制力惩罚标量。我一般先定 θ 的权重控制核心目标再定 p 的权重限制小车行程速度项初始都给 1最后微调 R。权重对应状态典型初值增大后的效果Q(1,1)小车位移 p10小车更快回中行程变小Q(2,2)小车速度 ṗ1运动更平滑阻尼感增强Q(3,3)摆杆角度 θ100立得更直但控制力峰值变大Q(4,4)角速度 θ̇1抑制摆杆高频抖动R控制力 F0.1越大越省力响应越慢代码直接用 Control System Toolbox 的lqr()函数% design_lqr.m M 1.0; m 0.2; l 0.5; g 9.81; A [0 1 0 0; 0 0 -m*g/M 0; 0 0 0 1; 0 0 (Mm)*g/(M*l) 0]; B [0; 1/M; 0; -1/(M*l)]; Q diag([10, 1, 100, 1]); R 0.1; K lqr(A, B, Q, R); fprintf(反馈增益 K [%.4f, %.4f, %.4f, %.4f]\n, K);参数说明diag把四个权重排成对角阵顺序必须与状态定义一致否则相当于对错误的物理量加权。lqr默认求解连续时间代数黎卡提方程返回 1×4 行向量。第一次运行建议打印出来看符号K(3) 必须为正摆角才能被拉回零如果为负优先检查 A 矩阵里 θ 的符号约定而不是调权重。提示没有 Control System Toolbox 时可以用care()求解黎卡提方程得到 P再按 K R⁻¹BᵀP 手算反馈增益结果与lqr()一致。3.3 把 u −Kx 接回非线性模型跑出真正的闭环响应控制器基于线性模型设计但必须作用在cart_pole_dynamics这个非线性函数上否则验证毫无意义。闭环脚本只用一个匿名函数包住反馈律就能复用第 2 章的动力学函数% run_closed_loop.m params [1.0, 0.2, 0.5, 9.81]; K [-3.1623, -3.1769, 41.0955, 8.8022]; % 占位增益以 lqr 输出为准 tspan [0, 5]; state0 [0; 0; 0.1; 0]; [t, states] ode45((t, s) cart_pole_cl(t, s, params, K), tspan, state0); plot(t, rad2deg(states(:,3))); xlabel(时间 (s)); ylabel(摆杆角度 (deg)); grid on; function dstate cart_pole_cl(t, state, params, K) u -K * state; % 状态反馈u 是标量控制力 dstate cart_pole_dynamics(t, state, params, u); end逻辑说明匿名函数把时间、状态、参数一次性传入闭环函数内部先算u -K * state再调用非线性动力学。K 的第 3 列直接作用于摆角所以它的符号和数值决定了控制方向与力度。运行后角度应在 12 秒内收敛到 0 附近并保持小车位移同时被拉回原点附近。参数说明示例 K 只为了让脚本能直接跑实际请用design_lqr.m的输出替换。把state0的第三行改成 0.5 再跑会发现收敛明显变慢甚至失败——这不是代码问题而是线性控制律在大角度区间的固有局限也是后续进阶到滑模或能量控制swing-up的出发点。4. 把倒立摆仿真搬进 Simulink并输出一个摆杆动画4.1 用 S-Function 在 Simulink 里复用同一个动力学函数脚本仿真适合调参和跑批量数据但演示控制逻辑时 Simulink 更直观。常见做法是搭一个环形结构S-Function 封装非线性动力学Mux 把状态汇总Gain 矩阵乘状态得到控制力Scope 看曲线。很多人纠结要不要在 Simulink 里重写一遍动力学其实不用直接复用cart_pole_dynamics.m注册成 Level-2 S-Function模型逻辑和脚本完全一致避免两套模型对不上。Simulink 模块关键设置作用S-Function名称cart_pole_sfun参数params计算四个连续状态的导数Mux4 输入 1 输出汇总状态向量Gain矩阵-K乘法模式计算控制力 u −KxScope无观察角度、位移曲线Level-2 S-Function 有固定模板核心是三个回调InitializeConditions里把初值写入连续状态Derivatives里调用动力学函数Outputs里把状态透传出去。完整模板用edit sfuntmpl生成按下面这个骨架填% cart_pole_sfun.m (Level-2 S-Function 回调核心) function cart_pole_sfun(block) setup(block); function setup(block) block.NumInputPorts 0; block.NumOutputPorts 1; block.NumContStates 4; block.NumDialogPrms 2; % 初值向量、物理参数 block.SetPreCompOutPortParamToDynamic; block.SampleTimes [0 0]; % 连续采样 block.RegBlockMethod(InitializeConditions, Init); block.RegBlockMethod(Derivatives, Deriv); block.RegBlockMethod(Outputs, Output); function Init(block) block.ContStates.Data block.DialogPrm(1).Data; % 初值向量 function Output(block) block.OutputPort(1).Data block.ContStates.Data; function Deriv(block) params block.DialogPrm(2).Data; % [M m l g] F 0; % 开环测试 block.Derivatives.Data cart_pole_dynamics(0, ... block.ContStates.Data, params, F);参数说明DialogPrm是 S-Function 的 mask 参数这里约定第一个存初值[0;0;0.1;0]第二个存[M,m,l,g]顺序不要和动力学函数里的params混。SampleTimes [0 0]声明连续系统Simulink 求解器才能用变步长 ode45 驱动它。上面骨架是开环版本做闭环时把NumInputPorts改为 1在Derivatives里读block.InputPort(1).Data作为 F 即可。4.2 用 patch line 写一个可复用的小车摆杆动画动画是报告和答辩里最能说清楚问题的输出。思路很简单每画一帧删除上一帧的小车矩形和摆杆线段循环滚动。MATLAB 的patch适合画矩形line画摆杆delete做擦除。% animate_cart_pole.m figure(Color, w); hold on; axis([-3 3 -0.6 1.8]); axis equal; grid on; car_w 0.4; car_h 0.1; step max(1, floor(length(t) / 500)); % 把总帧数压到 500 左右 % 这里的 states 来自 run_closed_loop.m 的输出 for k 1:step:length(t) p states(k,1); th states(k,3); car patch([p-car_w/2 pcar_w/2 pcar_w/2 p-car_w/2], ... [0 0 car_h car_h], [0.55 0.75 1.0]); pole line([p, p l*sin(th)], ... [car_h, car_h l*cos(th)], ... LineWidth, 4, Color, k); drawnow; pause(0.02); delete(car); delete(pole); end逻辑说明patch用四个顶点画小车矩形line从车顶中心画摆杆摆杆末端由l*sin(th)和l*cos(th)从角度换算得到drawnow强制刷新画布不加它动画会攒到最后一次性显示。delete删掉旧帧等效于擦除重画比clf整体清屏快得多画面也不闪烁。参数说明step控制抽帧密度总帧数压到 500 左右避免状态点太多导致动画卡顿pause(0.02)控制帧间间隔改小就是加速播放。脚本里l用params(3)取出不要硬编码角度和位移的单位保持一致绘制前统一换算好。提示动画速度不等于仿真速度。tspan的物理时间、pause的墙钟时间、drawnow的实际刷新率三者独立验收只看动画行为不要试图严格对齐时间轴。5. 倒立摆仿真排错三招先看发散再看符号最后验极点5.1 三个最常踩的坑仿真发散、曲线不收敛绝大多数时候不是控制器不行而是模型层的问题。下面三个坑按排查优先级排好照表检查比反复调 Q、R 更快现象原因处理角度直接变成 NaN求解器发散步长过大或初始角过大缩小RelTol初值降到 0.3 rad 内小车朝一个方向匀速漂移有稳态力残留θ 符号大概率反了检查 A 矩阵第三行确认 K(3) 0闭环能保持直立但摆动幅度大权重不匹配Q(3,3) 过小或 R 过大按 3.2 表格增大角度权重、减小 R5.2 用闭环极点位置和扰动峰值做验证验证 LQR 设计是否合理有两个不依赖肉眼观察的办法。一是直接读闭环极点eig(A - B*K)应全部落在左半平面主导极点实部对应时间常数 τ ≈ 1/|Re(λ)|收敛时间约为 35τ可以反过来检验 Q、R 是否过保守。二是给小车加一个持续 0.2 秒的阶跃力扰动记录摆角最大偏差和控制力峰值和物理执行器的极限做对比。% verify.m eig(A - B*K) % 所有特征值负实部即收敛 [max_dev, idx] max(abs(states(:,3))); % 扰动峰值 fprintf(最大摆角偏差 %.3f rad, 时间 %.2f s\n, max_dev, t(idx));参数说明eig的结果若出现共轭复根虚部对应振荡频率实部对应衰减速率max返回峰值和位置峰值超过 0.35 rad 说明增益偏弱回到第 3 章的表加大 Q(3,3)。固定 M、m、l、g 四个物理参数只动 Q、R是保证发散时可定位问题的前提。仿真收敛后这套 K 可以直接搬到实物控制器上换掉ode45为周期采样循环即可算法本体不用改。本文还有配套的精品资源点击获取 倒立摆MATLAB仿真：从动力学建模到LQR闭环控制完整实践
简介倒立摆自适应动态规划ADPMATLAB仿真资料包面向控制理论、强化学习与MATLAB编程学习者重点演示如何用ADP算法实现单级倒立摆的稳定控制适合作为自动化、机器人或电气相关专业的课程设计与毕业设计参考。压缩包共4个文件约1.72MB两个m脚本分别承担主仿真流程与倒立摆动力学模型一个CAJ文档提供基于近似动态规划的倒立摆控制参考论文另有JPG图片展示随机起始状态下的控制效果。资源已有1635人学习下载说明该实例在相关学习者中有较高热度。源码以完整可运行的MATLAB仿真为主体包含系统建模、ADP迭代求解、仿真循环与结果可视化使用者可直接修改模型参数观察控制器在不同初始条件下逐步将摆杆稳定至垂直平衡的过程CAJ文献进一步补充算法推导与结果分析帮助读者将仿真代码与理论方法对照理解快速复现ADP倒立摆控制实验。1. 倒立摆 MATLAB 仿真一条从动力学到闭环控制的完整链路倒立摆是最典型的欠驱动非线性系统小车加摆杆两个自由度却只有一个控制输入。教材讲它是为了引出状态空间和 LQR工程上做自平衡小车、机器人运控的人也先用倒立摆仿真验证算法再碰真实硬件。用 MATLAB 写倒立摆仿真程序本质是把「建模—线性化—设计控制律—验证」这条链路完整跑通。程序的价值不在复现课本公式而在每一环都变成可调、可看、可出错的代码。曲线发散、符号写反、权重设错每个问题都会逼你把背后的理论再想一遍这正是它作为入门项目至今没被替代的原因。2. 建立倒立摆动力学模型用 ode45 跑通开环 MATLAB 仿真2.1 小车-摆杆模型的运动方程怎么来常见做法取「小车 摆杆」二自由度模型这也是自平衡小车、双轮机器人的通用等价模型。小车质量 M摆杆质量 m摆杆长度 l重力加速度 g状态取四个小车位移 p、速度 ṗ、摆杆角度 θ、角速度 θ̇。θ 从直立位置算起θ 0 是竖直向上θ π 是自然下垂外力 F 沿水平方向作用在小车质心。用拉格朗日方程推出两个耦合的二阶方程(Mm) p̈ m l θ̈ cosθ − m l θ̇² sinθ F m l² θ̈ m l p̈ cosθ − m g l sinθ 0θ̇² 项是摆杆旋转带来的向心加速度贡献是非线性的主要来源之一线性化时会被直接忽略但它决定了开环大角度扰动下系统行为的真实性。θ 定义在直立点这一点很关键教材从下垂点建模时符号会差一个负号后面线性化、算 LQR 增益都会联动出错建议在代码注释里固定这个约定。2.2 把方程写成 ode45 能直接计算的函数ode45 要求把方程改写成 dstate/dt f(t, state) 的形式。上面的方程对 p̈ 和 θ̈ 是耦合的手解显式表达式容易抄错我一般用质量矩阵法写成 M(q)·acc rhs在函数里用左除解加速度。% cart_pole_dynamics.m function dstate cart_pole_dynamics(t, state, params, F) % state [p; p_dot; theta; theta_dot] M params(1); m params(2); l params(3); g params(4); p state(1); p_dot state(2); theta state(3); theta_dot state(4); Mq [M m, m * l * cos(theta); m * l * cos(theta), m * l^2]; rhs [F - m * l * theta_dot^2 * sin(theta); m * g * l * sin(theta)]; acc Mq \ rhs; % 解出 [p_ddot; theta_ddot] dstate [p_dot; acc(1); theta_dot; acc(2)]; end逻辑说明Mq \ rhs是求解线性方程组Mq * acc rhs的标准写法比inv(Mq) * rhs数值上更稳尤其摆杆接近水平、矩阵条件数变大的区间左除几乎不放大误差。刻意不展开 p̈ 和 θ̈ 的显式表达式也是为了避免手写长公式时符号出错。参数说明params顺序固定为[M, m, l, g]调用处不要调换F是外力开环仿真先给 0。角度单位统一用弧度绘图时再用rad2deg转。如果看到角度曲线一开始就往负方向走说明初始状态符号与模型约定不一致先检查约定不要急着调控制器。2.3 开环仿真先让摆杆倒下验证模型物理行为初始状态我习惯取[0; 0; 0.1; 0]摆杆偏离直立约 5.7°小车静止。零外力仿真两秒摆杆应在重力作用下倒下小车被反作用力推向另一侧。这一步是检验模型符号正确性的最快路径摆杆往正方向倒小车应该往负方向移动才符合动量守恒。% run_open_loop.m params [1.0, 0.2, 0.5, 9.81]; % M1kg, m0.2kg, l0.5m tspan [0, 2]; state0 [0; 0; 0.1; 0]; % 轻微偏离直立位 [t, states] ode45((t, s) cart_pole_dynamics(t, s, params, 0), ... tspan, state0); figure; plot(t, rad2deg(states(:,3)), LineWidth, 1.2); xlabel(时间 (s)); ylabel(摆杆角度 (deg)); grid on;参数说明tspan只给起点和终点时MATLAB 自动决定输出点密度后面做动画再改成linspace(0, 2, 2000)强制均匀输出。states(:,3)取角度整列rad2deg只用于显示。跑完应看到角度从 0.1 rad 增长到接近 π如果曲线变平或出现台阶多半是误差容限下没采够点用odeset(RelTol, 1e-6)作为第三个参数传入ode45再跑。后续仿真统一用这张物理参数表M、m、l、g 固定后剩下的变量只有 Q、R 和初始角度参数含义数值单位M小车质量1.0kgm摆杆质量0.2kgl摆杆长度0.5mg重力加速度9.81m/s²θ0初始摆角相对直立0.1rad3. 倒立摆状态空间线性化与 LQR 控制器设计Q、R 决定控制风格3.1 在直立平衡点做线性化写出 A、B 矩阵闭环控制的目标是把摆杆维持在 θ 0 附近。在这个平衡点做小角度近似cosθ ≈ 1sinθ ≈ θθ̇² 作为高阶小量忽略。代入上面的非线性方程并整理成 ẋ Ax Bu 标准形式A [0 1 0 0; 0 0 -m*g/M 0; 0 0 0 1; 0 0 (Mm)g/(Ml) 0]B [0; 1/M; 0; -1/(M*l)]x [p; ṗ; θ; θ̇]输出取小车位移和摆杆角度时 C [1 0 0 0; 0 0 1 0]D 0。注意 A(4,3) (Mm)g/(Ml)这一项随 l 增大而减小特征值越靠近虚轴系统越接近临界状态这也解释了为什么实物摆杆越长越难控制。线性化模型只负责设计控制器仿真验证必须回到非线性模型两者不要混用。3.2 用 lqr() 算反馈增益Q、R 按这个顺序调LQR 极小化 J ∫(xᵀQx uᵀRu)dtQ 是状态权重对角阵对应 [p, ṗ, θ, θ̇]R 是控制力惩罚标量。我一般先定 θ 的权重控制核心目标再定 p 的权重限制小车行程速度项初始都给 1最后微调 R。权重对应状态典型初值增大后的效果Q(1,1)小车位移 p10小车更快回中行程变小Q(2,2)小车速度 ṗ1运动更平滑阻尼感增强Q(3,3)摆杆角度 θ100立得更直但控制力峰值变大Q(4,4)角速度 θ̇1抑制摆杆高频抖动R控制力 F0.1越大越省力响应越慢代码直接用 Control System Toolbox 的lqr()函数% design_lqr.m M 1.0; m 0.2; l 0.5; g 9.81; A [0 1 0 0; 0 0 -m*g/M 0; 0 0 0 1; 0 0 (Mm)*g/(M*l) 0]; B [0; 1/M; 0; -1/(M*l)]; Q diag([10, 1, 100, 1]); R 0.1; K lqr(A, B, Q, R); fprintf(反馈增益 K [%.4f, %.4f, %.4f, %.4f]\n, K);参数说明diag把四个权重排成对角阵顺序必须与状态定义一致否则相当于对错误的物理量加权。lqr默认求解连续时间代数黎卡提方程返回 1×4 行向量。第一次运行建议打印出来看符号K(3) 必须为正摆角才能被拉回零如果为负优先检查 A 矩阵里 θ 的符号约定而不是调权重。提示没有 Control System Toolbox 时可以用care()求解黎卡提方程得到 P再按 K R⁻¹BᵀP 手算反馈增益结果与lqr()一致。3.3 把 u −Kx 接回非线性模型跑出真正的闭环响应控制器基于线性模型设计但必须作用在cart_pole_dynamics这个非线性函数上否则验证毫无意义。闭环脚本只用一个匿名函数包住反馈律就能复用第 2 章的动力学函数% run_closed_loop.m params [1.0, 0.2, 0.5, 9.81]; K [-3.1623, -3.1769, 41.0955, 8.8022]; % 占位增益以 lqr 输出为准 tspan [0, 5]; state0 [0; 0; 0.1; 0]; [t, states] ode45((t, s) cart_pole_cl(t, s, params, K), tspan, state0); plot(t, rad2deg(states(:,3))); xlabel(时间 (s)); ylabel(摆杆角度 (deg)); grid on; function dstate cart_pole_cl(t, state, params, K) u -K * state; % 状态反馈u 是标量控制力 dstate cart_pole_dynamics(t, state, params, u); end逻辑说明匿名函数把时间、状态、参数一次性传入闭环函数内部先算u -K * state再调用非线性动力学。K 的第 3 列直接作用于摆角所以它的符号和数值决定了控制方向与力度。运行后角度应在 12 秒内收敛到 0 附近并保持小车位移同时被拉回原点附近。参数说明示例 K 只为了让脚本能直接跑实际请用design_lqr.m的输出替换。把state0的第三行改成 0.5 再跑会发现收敛明显变慢甚至失败——这不是代码问题而是线性控制律在大角度区间的固有局限也是后续进阶到滑模或能量控制swing-up的出发点。4. 把倒立摆仿真搬进 Simulink并输出一个摆杆动画4.1 用 S-Function 在 Simulink 里复用同一个动力学函数脚本仿真适合调参和跑批量数据但演示控制逻辑时 Simulink 更直观。常见做法是搭一个环形结构S-Function 封装非线性动力学Mux 把状态汇总Gain 矩阵乘状态得到控制力Scope 看曲线。很多人纠结要不要在 Simulink 里重写一遍动力学其实不用直接复用cart_pole_dynamics.m注册成 Level-2 S-Function模型逻辑和脚本完全一致避免两套模型对不上。Simulink 模块关键设置作用S-Function名称cart_pole_sfun参数params计算四个连续状态的导数Mux4 输入 1 输出汇总状态向量Gain矩阵-K乘法模式计算控制力 u −KxScope无观察角度、位移曲线Level-2 S-Function 有固定模板核心是三个回调InitializeConditions里把初值写入连续状态Derivatives里调用动力学函数Outputs里把状态透传出去。完整模板用edit sfuntmpl生成按下面这个骨架填% cart_pole_sfun.m (Level-2 S-Function 回调核心) function cart_pole_sfun(block) setup(block); function setup(block) block.NumInputPorts 0; block.NumOutputPorts 1; block.NumContStates 4; block.NumDialogPrms 2; % 初值向量、物理参数 block.SetPreCompOutPortParamToDynamic; block.SampleTimes [0 0]; % 连续采样 block.RegBlockMethod(InitializeConditions, Init); block.RegBlockMethod(Derivatives, Deriv); block.RegBlockMethod(Outputs, Output); function Init(block) block.ContStates.Data block.DialogPrm(1).Data; % 初值向量 function Output(block) block.OutputPort(1).Data block.ContStates.Data; function Deriv(block) params block.DialogPrm(2).Data; % [M m l g] F 0; % 开环测试 block.Derivatives.Data cart_pole_dynamics(0, ... block.ContStates.Data, params, F);参数说明DialogPrm是 S-Function 的 mask 参数这里约定第一个存初值[0;0;0.1;0]第二个存[M,m,l,g]顺序不要和动力学函数里的params混。SampleTimes [0 0]声明连续系统Simulink 求解器才能用变步长 ode45 驱动它。上面骨架是开环版本做闭环时把NumInputPorts改为 1在Derivatives里读block.InputPort(1).Data作为 F 即可。4.2 用 patch line 写一个可复用的小车摆杆动画动画是报告和答辩里最能说清楚问题的输出。思路很简单每画一帧删除上一帧的小车矩形和摆杆线段循环滚动。MATLAB 的patch适合画矩形line画摆杆delete做擦除。% animate_cart_pole.m figure(Color, w); hold on; axis([-3 3 -0.6 1.8]); axis equal; grid on; car_w 0.4; car_h 0.1; step max(1, floor(length(t) / 500)); % 把总帧数压到 500 左右 % 这里的 states 来自 run_closed_loop.m 的输出 for k 1:step:length(t) p states(k,1); th states(k,3); car patch([p-car_w/2 pcar_w/2 pcar_w/2 p-car_w/2], ... [0 0 car_h car_h], [0.55 0.75 1.0]); pole line([p, p l*sin(th)], ... [car_h, car_h l*cos(th)], ... LineWidth, 4, Color, k); drawnow; pause(0.02); delete(car); delete(pole); end逻辑说明patch用四个顶点画小车矩形line从车顶中心画摆杆摆杆末端由l*sin(th)和l*cos(th)从角度换算得到drawnow强制刷新画布不加它动画会攒到最后一次性显示。delete删掉旧帧等效于擦除重画比clf整体清屏快得多画面也不闪烁。参数说明step控制抽帧密度总帧数压到 500 左右避免状态点太多导致动画卡顿pause(0.02)控制帧间间隔改小就是加速播放。脚本里l用params(3)取出不要硬编码角度和位移的单位保持一致绘制前统一换算好。提示动画速度不等于仿真速度。tspan的物理时间、pause的墙钟时间、drawnow的实际刷新率三者独立验收只看动画行为不要试图严格对齐时间轴。5. 倒立摆仿真排错三招先看发散再看符号最后验极点5.1 三个最常踩的坑仿真发散、曲线不收敛绝大多数时候不是控制器不行而是模型层的问题。下面三个坑按排查优先级排好照表检查比反复调 Q、R 更快现象原因处理角度直接变成 NaN求解器发散步长过大或初始角过大缩小RelTol初值降到 0.3 rad 内小车朝一个方向匀速漂移有稳态力残留θ 符号大概率反了检查 A 矩阵第三行确认 K(3) 0闭环能保持直立但摆动幅度大权重不匹配Q(3,3) 过小或 R 过大按 3.2 表格增大角度权重、减小 R5.2 用闭环极点位置和扰动峰值做验证验证 LQR 设计是否合理有两个不依赖肉眼观察的办法。一是直接读闭环极点eig(A - B*K)应全部落在左半平面主导极点实部对应时间常数 τ ≈ 1/|Re(λ)|收敛时间约为 35τ可以反过来检验 Q、R 是否过保守。二是给小车加一个持续 0.2 秒的阶跃力扰动记录摆角最大偏差和控制力峰值和物理执行器的极限做对比。% verify.m eig(A - B*K) % 所有特征值负实部即收敛 [max_dev, idx] max(abs(states(:,3))); % 扰动峰值 fprintf(最大摆角偏差 %.3f rad, 时间 %.2f s\n, max_dev, t(idx));参数说明eig的结果若出现共轭复根虚部对应振荡频率实部对应衰减速率max返回峰值和位置峰值超过 0.35 rad 说明增益偏弱回到第 3 章的表加大 Q(3,3)。固定 M、m、l、g 四个物理参数只动 Q、R是保证发散时可定位问题的前提。仿真收敛后这套 K 可以直接搬到实物控制器上换掉ode45为周期采样循环即可算法本体不用改。本文还有配套的精品资源点击获取