二级倒立摆LQR控制仿真全流程：从非线性建模到Simulink闭环实现

二级倒立摆，这个课题我在学生时代折腾过很久，工作之后再看身边的同事做机器人腿部平衡、无人机吊舱稳定这类项目，本质上都绕不开同一套东西：非线性建模、局部线性化、状态反馈控制、仿真闭环验证。这篇文章就把我实际做过的“二级倒立摆仿真”全过程拆开讲一遍，从物理建模到LQR控制器设计，再到Simulink里的完整仿真实现，所有步骤和踩过的坑都如实写明，希望能让你少走点弯路。

适合谁来读？一类是做控制理论课程的课题设计，另一类是刚接触倒立摆、想搞清楚“从方程到控制器到底怎么串起来”的工程师。懂一点自动控制原理和MATLAB基础就够了，我会尽量把每一步的“为什么这样做”也讲清楚，而不是只丢一堆公式。

1. 项目概述与整体思路

倒立摆是控制理论里最经典的实验平台之一，一级倒立摆大家都听过：一辆小车，一根摆杆，目标是通过移动小车让摆杆竖直不倒。二级倒立摆就是在摆杆顶端再串联一根摆杆，让两根摆杆同时保持竖直向上。别小看加的这一根杆，系统从四阶变成六阶，耦合关系、非线性程度、控制难度完全是另一个量级。

这个项目要解决的问题很明确：对一个物理上天生不稳定的系统，先写出它的运动方程，再设计控制器让它在竖直平衡点附近稳定下来，最后用仿真验证整个方案是否可行。实际价值也很直接——机器人双足平衡、火箭垂直着陆、起重机的防摆控制，底层数学结构和倒立摆如出一辙，把这个项目吃透，换到任何一个工程对象上你都能快速上手。

我给自己定的技术路线是这样的：

1. 用拉格朗日方程建立二级倒立摆的非线性动力学模型
2. 在竖直平衡点附近做线性化，得到标准状态空间方程
3. 对线性系统做可控性分析
4. 设计LQR状态反馈控制器，求解反馈增益矩阵
5. 在Simulink里搭建非线性被控对象模型，接入控制器形成闭环
6. 通过仿真调参，验证鲁棒性和动态性能

这套流程是控制工程里非常标准的“建模—分析—设计—验证”链路。之所以选择LQR而不是传统PID，原因后面会专门展开，这里先提一句：二级倒立摆有6个状态变量，多回路PID的参数整定会让你调到怀疑人生，而LQR在一个统一框架下解决了多变量协同控制问题。

2. 物理建模：从拉格朗日方程到状态空间

2.1 系统描述与参数定义

先明确物理对象。小车在水平轨道上运动，受到外部驱动力F，小车上方铰接着摆杆1（也就是第一级），摆杆1末端又铰接摆杆2（第二级）。为了简化分析，通常做以下几个假设：

• 小车、摆杆都是刚体
• 摆杆质量均匀分布，质心在几何中心
• 忽略铰链处的摩擦，但可以考虑小车与轨道间的黏性摩擦
• 所有运动限制在竖直平面内

我用的参数定义如下表所示：

这里有个容易搞混的细节：建模时用的l到底是摆杆全长还是质心到铰点的距离。我习惯统一用“摆杆全长”，在动能表达式里会乘以1/2，比如质心位置是l/2处，计算转动惯量时也要对应调整。这个约定从头到尾保持一致就行，最怕写着写着混用，后面线性化出来的矩阵符号全是错的。

系统的广义坐标选为：

• x：小车在轨道上的位移
• θ1：摆杆1与竖直方向的夹角，顺时针为正
• θ2：摆杆2相对摆杆1的夹角，同样按照约定方向取正

状态变量选6个：x、θ1、θ2以及它们各自的一阶导数。这里角度方向的正负约定非常重要，方向取反会让后面线性化得到的矩阵符号出错，控制器输出方向反过来，仿真一开始就发散。

2.2 非线性动力学方程推导

推导方法我直接用拉格朗日方程：

[ \frac{d}{dt}\left(\frac{\partial L}{\partial \dot{q}_i}\right) - \frac{\partial L}{\partial q_i} = Q_i ]

其中L是拉格朗日量，等于系统动能T减去势能V，q_i是广义坐标，Q_i是对应的广义力。这个方法的优势是不用像牛顿法那样考虑每个铰链的内力，直接通过能量计算就能得到方程。

先写动能。小车的动能很简单：

[ T_c = \frac{1}{2}M\dot{x}^2 ]

摆杆1的动能稍微复杂一点，因为它既有随质心的平动又有绕质心的转动。质心位置可以通过x和θ1表达出来，求导就是质心速度，然后加上绕质心转动那一项。摆杆2更麻烦，它的质心位置取决于x、θ1、θ2三个广义坐标，速度表达式里会交叉出现θ1和θ2的耦合项。

势能部分相对好写，就是两根摆杆质心高度乘以重力。写出完整表达式后，把L = T - V代入拉格朗日方程，整理成标准的二阶动力学形式式：

[ M(q)\ddot{q} + C(q,\dot{q})\dot{q} + G(q) = B u ]

其中M(q)是3×3质量矩阵，C(q,q˙)包含了科氏力和离心力项，G(q)是重力项，B是输入矩阵，u是驱动力。这里我不得不说实话：二级倒立摆的M、C、G符号表达式非常长，手工推导极其容易漏项。我第一次推导的时候卡了快一周，后来用符号计算工具辅助验证，才发现有些交叉项漏掉了。

所以我强烈建议你也用符号计算工具求一遍，拿来跟手推结果对比。实际操作很简单，把动能和势能表达式输进去，让工具自动算d/dt项和偏导数，再化简。这样你能把精力放在理解物理结构上，而不是困在代数计算里。

2.3 平衡点线性化与状态空间模型

非线性模型可以直接用于仿真，但LQR控制器设计必须在线性模型基础上进行，所以要找到平衡点做线性化。

二级倒立摆的竖直向上状态是我们要稳定的平衡点，对应：

[ q^* = [0, 0, 0]^T, \quad \dot{q}^* = [0, 0, 0]^T ]

在这个点附近，令Δq、Δq˙为小偏差量，对非线性方程做泰勒展开，保留一阶项并忽略高阶小量，就能得到线性状态空间模型：

[ \dot{X} = A X + B u ]

其中状态向量X=[Δx, Δθ1, Δθ2, Δẋ, Δθ̇1, Δθ̇2]^T。矩阵A和B的具体形式是通过质量矩阵的逆、重力项的Jacobian等计算出来的，实际操作中你会先用符号工具算出M(q)和G(q)的表达式，再把平衡点代入，数值计算出A矩阵。

我当时为了验证线性化结果是否可信，做了一个简单检查：把线性模型和原始非线性模型放在同一初始条件下仿真，如果初始角度很小（比如0.01 rad），两条响应曲线应该高度重合。一旦初始角度增大到0.3 rad以上，误差就会明显暴露，这也提醒我控制器的工作范围是“平衡点附近”，不要指望LQR能处理大幅度摆动。

2.4 建模中的常见陷阱

建模阶段我踩过的坑，集中说三个。

第一个是转动惯量取值错误。均匀细杆绕质心的转动惯量是ml²/12，绕端点才是ml²/3，两个值差了4倍。建模时如果用的是绕质心的转动惯量，那么平动动能里的质心速度已经是相对地面的，转动动能里就要用绕质心那一项，不能混着用。这个错非常隐蔽，因为一组参数下系统可能是渐近稳定的，换了参数就发散，排查起来特别费劲。

第二个是角度正负方向不统一。我建议在一张纸上明确画好坐标系，标出θ1、θ2的正方向，然后所有势能表达式、质心位置表达式全部对照这张图写。如果不这么做，线性化后B矩阵的正负号错了，控制器给出的力方向就是反的，仿真结果直接飞掉。

第三个是忘记摩擦力。如果你的目标是跟实物对照，那摩擦力不能忽略。小车和轨道之间至少加一个线性阻尼项，比如F_friction = f_c * ẋ，f_c取0.1到1之间的小值。虽然这对控制器设计本身影响不大，但会让仿真结果更接近真实系统，尤其是观察稳态误差时。

3. 控制器设计：为什么首选LQR以及具体实现

3.1 为什么不直接用PID，而是先做LQR

一级倒立摆用单回路PID还能凑合，但二级倒立摆有6个状态，输入只有一个驱动力，这是典型的多输入单输出系统中的“多变量处理”问题。你当然可以设计两个PID回路分别稳定θ1和θ2，再考虑让小车归位，但三个回路同时作用在一个输入上，回路之间强耦合，调参顺序稍微不对就满盘皆输。

LQR的思维方式完全不同。它把6个状态放在一起衡量，要最小化一个综合的性能指标：

[ J = \int_0^\infty \left( X^T Q X + u^T R u \right) dt ]

这个性能指标可以理解为：状态偏离平衡点的程度，和控制力消耗的能量，两者加权总和。Q矩阵决定你对不同状态偏差的“容忍度”，R矩阵决定你对控制能量有多“吝啬”。

LQR的另一个好处是：只要系统可控，求解Riccati方程可以得到唯一的最优状态反馈矩阵K。这意味着有明确的数学工具保证稳定性，而不是靠经验不断试错。实际工程中你仍然需要调Q和R，但调参维度从“多个回路PID参数”降到了“一个权重矩阵和一个输入权重”，并且每个参数都有直观的物理含义。

3.2 LQR控制器设计过程

LQR设计的基本前提是系统可控。我在代码里直接用可控性矩阵判断：

co = ctrb(A, B); if rank(co) < 6 error('系统不完全可控，请检查模型参数'); end

这一步千万别省。我之前偷懒跳过它，结果换了另一组参数后仿真一直发散，查了快半天才意识到那个参数组合下系统不完全可控。

可控性检查通过后，就可以求解反馈增益K。MATLAB里一行命令：

K = lqr(A, B, Q, R);

控制律形式是：

[ u = -KX ]

这里K是1×6的行向量，对应6个状态的增益系数。实现时要注意：把K与状态向量X的排列顺序完全对应，比如X(1)是位移，X(2)是θ1，X(3)是θ2，X(4)是速度，X(5)是θ1角速度，X(6)是θ2角速度。增益顺序一旦错位，等于控制律所基于的状态完全搞错了，仿真必炸。

求解K之后，务必检查闭环系统的极点：

eig(A - B*K)

所有特征值的实部都应该为负，且没有靠近虚轴的极点。如果发现有一对极点离虚轴很近（比如实部绝对值小于0.05），系统收敛会很慢，需要在Q矩阵里加重对应状态的惩罚。

3.3 权重矩阵整定的经验法则

Q矩阵和R矩阵的整定是整个项目最耗时也最体现经验的地方。我最初的一组权重是这样的：

[ Q = \text{diag}([100, 200, 200, 20, 20, 20]), \quad R = 1 ]

这组权重的含义是：对θ1和θ2的角度偏差惩罚最重（200），位移也压得比较好（100），速度项的权重相对小一些（20）。实际调出来之后系统能稳定，但我发现小车回中速度偏慢，于是把第一个元素从100提到500，小车回收明显加快，代价是控制力变化更剧烈了一点点。

关于整定经验，我总结几条规律：

1. 开始调参时先保持R不变，只调Q。R太小会让控制力狂野，仿真容易因为数值问题发散。
2. 哪个状态你觉得收敛得太慢，就加大Q里对应的对角元素。比如θ2老是振，就先把Q中对应θ2的权重翻倍，而不是动θ1。
3. 如果控制力出现高频振荡，试着把R从1调到10，通常会顺滑很多。代价是收敛变慢、超调略大。
4. 常用的出发点可以是Q=diag([100,100,100,10,10,10])再根据响应逐步调整。不要一上来就追求最优，先把系统稳定住，再一点点优化性能。

我记得第一次跑通闭环的时候，θ1能稳住但θ2缓慢漂移，后来把θ2的惩罚权重从100加到500，这个问题才解决。复盘原因也很简单：θ2在动力学上是受θ1间接影响的，如果不给它足够高的控制权重，控制器就会优先照顾θ1，θ2成了“被遗忘的孩子”。

4. 仿真实现：在Simulink里搭出完整的闭环系统

4.1 仿真环境与整体架构

我用的是MATLAB/Simulink环境。整个仿真模型从架构上分为三块：被控对象模型（非线性动力学）、控制器模块（状态反馈增益）、信号采集与参考输入模块。

为什么被控对象必须用非线性模型而不是线性模型？因为仿真的意义就在于验证控制器在“真实”被控对象上的表现。如果你用线性模型验证线性控制器，那就成了自己跟自己玩，看不出LQR对建模误差和非线性特性的容忍度。我通常的做法是先用非线性模型直接搭被控对象，控制器调试通过后，一切结果才可信。

4.2 用S函数把动力学方程“塞”进Simulink

搭建非线性被控对象有两种常用方案：一种是直接用积分器模块搭出每个微分方程，另一种是写S函数。我的经验是：二级倒立摆的动力学方程太长，用积分器搭会形成错综复杂的连线网络，调试时看不过来，推荐用S函数。

S函数本质是一个符合Simulink接口约定格式的MATLAB函数。我给出一个极简的结构示例：

function [sys, x0, str, ts] = plant_sfunc(t, x, u, flag, M, m1, m2, l1, l2, g) switch flag case 0 sizes = simsizes; sizes.NumContStates = 6; sizes.NumDiscStates = 0; sizes.NumOutputs = 6; sizes.NumInputs = 1; sizes.DirFeedthrough = 0; sizes.NumSampleTimes = 1; sys = simsizes(sizes); x0 = [0; 0; 0; 0; 0; 0]; str = []; ts = [0 0]; case 1 % x(1)=小车位移, x(2)=θ1, x(3)=θ2, x(4)=ẋ, x(5)=θ̇1, x(6)=θ̇2 F = u(1); % 在这里调用你推导出的质量矩�ddy阵和广义力表达式 % 计算出三个加速度 qdd1, qdd2, qdd3 % 例如： % qdd = Mq \ (F_vec - Cq - Gq) qdd = dynamics(x, F, M, m1, m2, l1, l2, g); sys = [x(4); x(5); x(6); qdd(1); qdd(2); qdd(3)]; case {2, 4, 9} sys = []; case 3 sys = x; otherwise error(['Unhandled flag = ', num2str(flag)]); end

这只是一个框架，实际的核心在于dynamics函数，也就是把质量矩阵、科氏力、重力项代进去求解加速度的那一段。如果你推导的时候用了符号计算工具，可以直接把符号表达式转成MATLAB函数，省去手敲长公式的环节。

在Simulink模型里，拖入一个S-Function模块，填入函数名plant_sfunc，参数处把M、m1等传递进去，一个非线性被控对象就搭好了。输出端用Mux或者直接输出6维向量，接到控制器模块。

4.3 控制器封装与闭环连接

控制器的Simulink实现出乎意料的简单：一个Gain模块，增益矩阵设置为K，输入是6维状态向量，输出是控制力u。因为LQR状态反馈本身就是一个线性增益，我不需要写任何额外的控制逻辑。

但这里有个工程细节要说：仿真时假设全状态可测，但实际项目中角度可以用编码器测得，速度往往是差分算出来的，会有噪声。如果想更接近真实，可以在仿真里对速度通道加一个测量噪声和高通滤波，看看控制器的容忍度如何。

闭环连接顺序是：Plant模块输出状态X，进入Gain模块乘以K，取反后作为控制力送回Plant模块的输入。我通常会在控制器输出端加一个Saturation饱和限幅模块，比如限制在±20N，防止仿真过程中数值突变。限幅也会暴露一个现象：如果LQR输出的理想控制力经常超过限幅值，说明控制器太激进，R该调大一点。

建模时的初始化脚本可以这样组织：

% 参数定义 M = 1.0; m1 = 0.5; m2 = 0.5; l1 = 0.3; l2 = 0.3; g = 9.81; % 符号推导过程得到Mq, Cq, Gq，并线性化得到A, B [A, B] = linearize_model(M, m1, m2, l1, l2, g); % 检查可控性 co = ctrb(A, B); assert(rank(co) == 6, '系统不完全可控'); % LQR设计 Q = diag([100, 200, 200, 20, 20, 20]); R = 1; K = lqr(A, B, Q, R); % 闭环极点检查 assert(all(real(eig(A - B*K)) < 0), '闭环系统不稳定');

写好这个脚本后，每次仿真前运行一次，把K加载到工作区，Simulink的Gain模块就会自动识别。

4.4 仿真参数设置与初始条件处理

仿真参数设置这块，有几个关键点：

求解器我建议先用变步长ode45，如果发现系统振动频率较高或者方程刚性明显，再换ode15s。二级倒立摆本身不是特别刚性的系统，ode45通常足够。最大步长限制在0.001秒，太大会漏掉高频动态，仿真结果看起来平稳但实际已经数值发散。

初始条件用向量x0设置。我第一次调试时给了θ2=0.1 rad的初始偏差，结果仿真直接崩溃，当时还以为控制器设计错了，后来改成0.01 rad就一切正常。原因很简单：大偏差已经超出了线性化模型的适用范围，LQR控制器按照“小偏差近似”工作，一开始就处理不了。正确的做法是先从小偏差（比如0.001到0.01 rad）开始验证正确性，再逐步加大初始角度，测试控制器的鲁棒范围。

还有一个小细节，就是仿真时长。我一般先跑5秒，如果快速收敛就在10秒内观察稳态。时间太短看不出收敛趋势，太长又会浪费调试时间。

5. 仿真结果分析与参数调优实录

5.1 典型响应曲线怎么读

仿真跑通了，关键是能正确解读曲线。我一般同时观察四组信号：θ1和θ2的时间响应、小车位移x、控制力F、以及状态误差的平方积分。

正常情况应该是：初始偏差存在时，θ1、θ2迅速朝0收敛，方向单调，没有反复穿越零点的长尾振荡；小车位移会先向一侧移动，再缓慢回到目标位置；控制力在初始阶段有一个较大的脉冲，然后快速衰减到接近0。如果你看到θ2出现高频小抖动，而θ1看上去很平稳，多半是Q矩阵中θ2的权重太低或者测量噪声太强。

我习惯把控制力变化曲线单独截图放大。如果控制力保持在限幅值附近持续几秒，说明控制器饱和了，系统在极限工作条件下可能会失控。这通常意味着R太小，控制策略过于激进，导致实际工程中执行器无法满足。

5.2 调参中的一次实战经历

具体分享一次我调参的完整经历，非常有代表性。初始参数我设Q=diag([100,200,200,20,20,20])、R=1，仿真结果显示θ1在0.8秒内回到0附近，效果还算满意，但θ2一直有0.02 rad左右的稳态波动，始终消不掉。

我第一步把Q中对应θ2的权重从200调到500，θ2波动确实变小了，但又出现了低速漂移。第二步我把位移权重从100调到300，小车回收更快了，但控制力曲线明显更抖。第三步我把R从1调到5，控制力平滑了很多，代价是收敛时间从0.8秒增加到1.5秒。

最终我用的权重是Q=diag([300,400,500,30,30,30])、R=5。这个组合在快速性和控制能量之间取得了较好平衡。整个过程看起来很有逻辑，但实际花了两天，因为每次调整之后都要重新跑仿真、看曲线、判断是哪个状态的问题。

我总结出一条经验：调试时一次只改一个参数，并且做好记录。我用一个表格记录每次修改前后目标变量的变化，比如“θ2最大偏差、稳定时间、控制力峰值”。否则几个参数同时改来改去，最后连自己都不知道哪一版是好的。

5.3 性能评估与鲁棒性测试

调好参数后，可以做几组有说服力的性能测试：

第一组是脉冲扰动测试。在仿真到2秒时给θ2方向加一个短时力矩脉冲（可以用Signal Builder或者直接在Plant输入上叠加一个脉冲信号），观察系统能否在几秒内恢复平衡。这是评估控制器鲁棒性最直观的方式。

第二组是参数摄动测试。把m2从0.5 kg改成0.6 kg，看看控制器是否仍然稳定。LQR有一个特性：对参数摄动有一定容忍度，但超过范围就失效。我在实际测试中发现，m2增到0.8 kg之后系统开始振荡，说明控制器已经接近稳定边界了。

第三组是初始角度极限测试。逐步把θ2初始角从0.01 rad增加到0.05 rad、0.1 rad，记录系统仍能镇定的临界值。这个值就是控制器的有效工作范围，超出之后必须考虑非线性控制器或者切换控制策略，这不是LQR能解决的问题。

6. 常见问题排查与避坑指南

6.1 仿真发散的原因排查

仿真一发散，第一反应不是改控制器参数，而是先检查模型本身。我见过太多次“改了三天参数仍然发散”的情况，最后发现是初始状态定义顺序跟控制器K不对应。

以下是我整理的排查顺序：

第二条提示一点：Q/R权重不当导致的振荡，有时候看起来像数值不稳定。我的判别方法是把仿真步长改小一半重新跑，如果曲线变好，那就是数值问题；如果曲线不变，那就是控制器本身的问题。

6.2 可控性检查常被忽略的前置条件

可控性矩阵满秩是LQR设计的最基本前提，但很多人一上来就是lqr(A,B,Q,R)，完全不检查。我建议把这步写进初始化脚本里，用assert强制校验。另外还有一个容易被忽略的点：可控性结论是针对你选的6个状态而言的。如果把状态向量改成[θ1,θ2,θ1̇,θ2̇]而完全去掉位移x和速度，系统的可控性结论完全不同，因为位移虽然在控制中不参与反馈，但它是被控对象的一部分，线性化模型中输入矩阵B的维度也因此改变。

还有一个实际细节：可控性是一个“非此即彼”的性质，但对于数值矩阵，rank()判断不稳定。你需要先看机器精度，svd分解后看看最小奇异值有多小，如果它在1e-12量级，系统实际上是“病态可控”的，控制器设计出来之后数值上也很难奏效。

6.3 长期未收敛的处理技巧

有时候系统能稳定但收敛特别慢，比如θ2在10秒后还有0.005 rad的残留偏差。这类问题我一般按三个方向排查：

第一个方向是检查是否有积分环节缺失。LQR纯状态反馈相当于PD控制，没有积分项，如果模型里存在常值扰动，就会有稳态误差。可以在控制律里加入对角度误差的积分项，扩展状态向量，变成LQI（线性二次型积分控制）设计。

第二个方向是Q矩阵中对应慢状态的权重不足。把θ2对应元素继续加大，直到收敛时间满足需求，但要注意这会让控制力峰值显著上升。

第三个方向是检查闭环极点分布。用eig(A-B*K)算出来，如果有一对极点实部特别小（比如-0.01），它的时间常数是100秒，你跑10秒当然看不出收敛。这时候直接调整Q权重，让这对极点的实部绝对值增大到0.5以上，响应速度立刻改善。

6.4 最后的提醒：分阶段验证

我最后想特别强调一个工作习惯：分阶段验证，不要一次跨太多步。具体来说是这样：

第一阶段，验证线性模型的闭环响应。给线性模型接上LQR控制器，初始条件0.01 rad，确认曲线符合预期。第二阶段，把控制器接到非线性模型上，初始条件不变，确认两条响应大致一致。第三阶段，逐渐加大初始条件和扰动范围，测试控制器能扛到什么程度。

每完成一个阶段，记录一段仿真曲线和参数快照。这样出问题时你永远有一个“上一阶段是好的”作为参照点，而不是面对一个完全失控的庞大闭环系统无从下手。我后来做任何控制项目都沿用这个习惯，效率提升非常明显。

做这个项目的过程中，我也有一件印象很深的小事。当时调参调了整整一个下午，曲线怎么都压不平，后来发现是我在初始化脚本里定义的θ2正方向，和S函数状态方程推导时的方向约定刚好相反。一个负号的问题，浪费了半天时间。后来我把所有方向约定直接写成了注释放在文件开头，每次新建工程都先复制这一段，再也没有犯过同样的错误。也希望你能在项目一开始就做好这件事。