1. 二级倒立摆这颗“试金石”，究竟在验证什么
二级倒立摆这个题目，在控制领域几乎是“算法试金石”级别的存在。很多人第一次接触它是在课堂或实验室里，抱着“不就一根杆子立起来吗”的态度上手，结果发现它比一级倒立摆难出一个量级。正因为如此，用PID和LQR去控制同一个二级倒立摆模型，再做对比仿真，是我认为性价比最高的一类练手项目：它不要求你有多深的数学功底，但能把现代控制理论里最核心的几个问题——建模、线性化、状态反馈、权重整定、鲁棒性——全部串起来。这篇文章就是把我自己做这组仿真时的思路、参数、踩坑和排查方法完整拆开，给准备碰或者正在被它折磨的人一个可以直接照做的参考。
这个仿真适合谁？如果你正在学经典控制理论，想搞清楚“PID到底为什么调起来这么玄”；或者你在学现代控制理论，想知道LQR那堆矩阵计算在真实被控对象上表现如何；又或者你只是要交付一个课程设计或实验室报告，需要一套能跑通、能解释、能对比的结果——这篇文章的内容大概率都够用。我默认你至少会用基本操作，但不要求你背下状态空间方程的推导过程，更不要求你熟练摆弄黎卡提方程。我会尽量把每一步背后的“为什么”也讲清楚，毕竟光会点按钮，换个模型就废了。
我先说结论：在我复现的这组对比里，LQR不仅更快把摆稳定下来，而且对模型参数偏差的容忍度明显高于精心调过的串级PID。但这不意味着PID没有意义——恰恰相反，我是在把PID调到几乎极限之后，才真正看懂LQR那套数学在替我解决什么麻烦。下面我从头拆解。
2. 系统建模：仿真之前，先把被控对象“说清楚”
2.1 物理模型和状态变量：二级倒立摆到底“长什么样”
二级倒立摆的物理结构不复杂：一辆可以在水平轨道上运动的小车，车轴上连接一根可以转动的下摆，下摆末端再连接一根可以在同一平面内转动的上摆。你唯一能控制的输入是小车的水平驱动力，而你要稳住的是两个摆都不倒下来，同时尽量让小车回到某个指定的位置。这个系统最讨厌的性质有两个：第一是“欠驱动”——你只有一个控制力，却要同时管小车位移、下摆角度、上摆角度以及它们各自的速度；第二是“本质不稳定”——在竖直向上的平衡点附近，如果没有控制，任何微小的偏差都会迅速被重力放大。
建模时，我习惯把状态向量定义成：
- x：小车位移
- θ1：下摆与竖直方向的夹角
- θ2：上摆与竖直方向的夹角
- dx、dθ1、dθ2：对应的三个速度
也就是说系统有6个状态。输入u是作用在小车上的水平外力F。如果严格用拉格朗日方程去推，会得到一个带惯量矩阵、科氏力项和重力项的非线性微分方程组，形式上比较吓人，但这个方程组才是Simulink非线性仿真里真正跑的东西。很多同学一开始直接用简化到极点的线性模型搭仿真，那实际上是在“自己哄自己”——控制器设计和分析可以用线性模型，但最终验证必须在非线性模型上做，否则你在Matlab里算得再漂亮，放到实物或完整模型上还是一碰就倒。
2.2 为什么要做线性化，以及线性化之后得到什么
LQR设计的前提是线性时不变系统。虽然二级倒立摆非线性，但我们的工作点非常明确：上摆、下摆都处于竖直向上，所有角度和角速度为零，小车位置和速度为零。在这个工作点附近，可以用泰勒展开把系统近似成状态空间方程：
- 状态空间形式的完整记号：ẋ = A·x + B·u，y = C·x + D·u
- 其中 A 是6×6的系统矩阵，B 是6×1的输入矩阵，C 根据你要观测哪些状态来定，D 通常为零矩阵
这里有一个特别容易踩的认知误区：PID控制器在设计时可以不显式使用A和B，但不代表系统没有A和B。你的PID每一个增益，本质上都是在隐式地“猜测”这个系统的动态特性；而LQR是明明白白地把A和B拿出来，让算法帮你按性能指标求解反馈增益。所以后续对比仿真时，我会让PID和LQR共用同一个非线性模型，只是控制器的“内部信息量”不同。
拿到A和B的途径有很多。你可以自己推拉格朗日方程再求雅可比，也可以用工具。我实际用的办法是：在Simulink里搭好完整的非线性机械模型，然后用linmod或线性化分析工具在默认工作点自动求取A、B矩阵。这不是偷懒，而是符合工程惯例——手工推导容易错，而且换个物理参数就要全部重推，工具线性化能把误差风险压到最低。
2.3 执行器限幅和测量噪声：仿真不能太“理想化”
很多对比仿真看起来很漂亮，是因为它们活在真空里：控制力不限幅、传感器无噪声、初始偏差小得离谱。等做完报告，拿去接实物，发现根本不是那么回事。我在这个项目里从一开始就加了两个基本约束：
- 控制力限幅：小车能输出的力是有限的，我用 |u| ≤ 20 N 作为公共约束。这很重要，因为LQR算出来的控制量如果不限幅，初始时刻可能会非常大，直接掩盖算法真实水平。
- 传感器噪声：给小车的位移、两个摆角都加上适量测量白噪声。虽然LQR状态反馈默认所有状态可测，但在仿真实操中加一点噪声能更真实地暴露两种控制器的差异。
这里有个经验供参考：噪声强度不要加得过大，否则会干扰你对核心结论的判断，也容易让模型数值刚性变强。我的做法是先用噪声为零的理想模型做参数整定，整定完成后再把噪声加上去观察鲁棒性，这样每一步的出问题点都清晰可查。
3. 控制方案拆解：PID的“手工硬调”和LQR的“数学整定”
3.1 二级倒立摆里的PID，为什么不是“三个参数”的事
初学PID时会以为事情很简单：位置误差比例放大，速度误差微分抑制，完了。但二级倒立摆只有一路控制力，却要同时稳住两个摆角和小车位置，单环PID根本无从下手。实际可行的结构是串级PID，也叫主从回路：
- 内环（从环）：负责稳住角度相关状态，采样周期最短，增益最高
- 外环（主环）：负责小车位置和速度，采样周期可以稍慢，输出修正后作为内环的角度给定或前馈补偿
我调PID时的顺序大致如下，已经在多个模型上验证过有效：
- 先把内环单独拿出来，只让下摆角θ1闭环，上摆和位置回路暂时断开或冻结，尝试用 PD 让θ1在竖直附近稳定。
- 调好θ1内环后，接入上摆角θ2的内环，让两摆都先稳住，此时控制量会明显变大，要注意限幅是否频繁触发。
- 最后把位置外环接上，用小车的位移误差去修正角度给定，实现“摆稳了再回中”的效果。
- 每个环都从低增益往上加，直到系统开始出现高频抖动，再往回调20%左右作为稳定裕度。
这套流程的最大特点是“慢和耗”。三个回路的增益相互牵扯，调θ2时θ1会抖，调位置环时两个摆又会晃。我最开始总共花了三个多小时，才在某一组参数下让系统稳定住。而且发现一个非常典型的坑：给内环加积分项后，摆杆会越调越疯——因为积分会把执行器限幅时的误差不断累积，形成“积分饱和”。所以我在倒立摆里几乎不用PI，只用PD，或者用带抗积分饱和的智能积分。这个细节普通教材不会强调。
3.2 LQR状态反馈：把多变量耦合直接丢给矩阵
LQR的思路完全不同。它不关心你分多少个环，而是直接问一个问题：在系统状态偏离零点的“代价”和消耗控制能量的“代价”之间，怎么选一个最优折中？数学上就是最小化性能指标：
J = ∫₀^∞ ( xᵀQx + uᵀRu ) dt
其中Q是6×6的半正定状态权重矩阵，R是1×1的正定控制权重矩阵。这个积分越小，代表系统既能快速回到平衡点，又不用太大的控制力。求解这个最优控制问题的核心是黎卡提方程，Matlab里直接一行K = lqr(A, B, Q, R)就得到最优状态反馈增益K，控制律为 u = -K·x。
实际用下来，Q和R的选取才是真正体现“设计师经验”的地方。我的参数设定逻辑是这样的：
- 角度偏差比位置偏差更危险，所以θ1、θ2对应的权重最大，至少是位置权重的10倍以上
- 速度状态可以给较小权重，因为它们本身是误差的导数，惩罚太狠容易导致控制过于激进
- R是控制力的“价格”，太小会让控制器动作猛烈，太大则系统回正太慢
我最终用得比较顺的一组是：Q对角线上，位置权重100，两个角度权重1000，三个速度权重1，R取0.1。这个组合没有经过严格优化，但已经能让系统在约1秒内回稳。你可以把它当起点，不要一上来就迷信某个“最优解”。
3.3 同一套模型，两种控制器的公平对比口径
对比仿真的意义在于“公平”，否则结论没有说服力。我给自己定的对比规则就这么几条：
- 同一个Simulink非线性模型，物理参数完全一致
- 初始状态一致，比如从θ1 = 0.1 rad、θ2 = -0.1 rad开始，位置和速度初值为0
- 执行器限幅都是20N，传感器噪声一致
- 积分步长、求解器类型一致
这个阶段有个容易被忽视的点：初始偏差不能取得太大。二级倒立摆工作点邻域是有限的，如果一开始就给θ1 = 0.5 rad，系统已经严重脱离线性化条件，再好的线性控制器也可能救不回来。这不是控制器不行，是仿真条件不合理。我的经验是初始角度控制在0.05~0.15 rad之间比较合适，既能看出对比差异，也不至于逼着控制器做不可能完成的任务。
4. 仿真实操：从Matlab脚本到Simulink模型，一步步跑通
4.1 先用Matlab脚本完成状态反馈设计
虽然最终验证在Simulink里做，但我建议先把设计阶段放在Matlab脚本里，这样调Q、R参数时不用反复开模型，速度快很多。我的脚本框架大致如下：
% 物理参数 M = 1.0; % 小车质量 kg m1 = 0.2; % 下摆臂质量 kg m2 = 0.1; % 上摆臂质量 kg l1 = 0.2; % 下摆质心到转轴距离 m l2 = 0.2; % 上摆质心到转轴距离 m g = 9.81; % 在Simulink非线性模型中，用工作点线性化得到A和B % 也可以用你自己的推导结果替换 [A, B, C, D] = linmod('double_pendulum_nonlinear'); % 检查系统可控性 rank(ctrb(A, B)); % 应等于6 % LQR权重 Q = diag([100, 1000, 1000, 1, 1, 1]); R = 0.1; K_lqr = lqr(A, B, Q, R); % 输出反馈增益，供Simulink中的矩阵增益块使用 K_lqr这里要强调一下，linmod这个名字看起来过时，但胜在稳定和简单。如果你用的是新版，也可以去“线性分析”工具里操作，得到的A、B矩阵是一样的。可控性检查很多人会跳过，我劝你每次换物理参数后都顺手跑一下，如果秩小于6，说明你当前模型本身在数学意义上存在不可控模态，下面所有优化都是白费劲。
PID参数我用的是工作区变量传给Simulink，整定过程比较“手工”，但有一点值得说：刻意记录每一组参数对应的现象。比如控制器增益达到某个临界值时，哪个状态先开始振荡，这能帮你判断是角度回路刚度不够，还是速度回路阻尼溢出。
4.2 Simulink模型里最容易出问题的三个环节
接下来是Simulink搭建。整个模型的核心结构可以分成三段：被控对象非线性方程、控制器逻辑、信号监测与限幅。我重点说三个实操细节：
第一个是控制器与被控对象之间的采样和求解器设置。我在连续模型里用ode15s或ode23tb这类刚性求解器，而不是默认的ode45。原因很简单：倒立摆模型在平衡点附近既有快速角度动态，又有较慢的位置动态，时间常数差异很大，用ode45会出现步长收不住、仿真速度极慢甚至结果抖动的问题。这个看似细节的选择，能直接改变你仿真能不能跑完。
第二个是PID控制器内部的“导数滤波”。理论上PD中的D项是纯微分，但仿真模型里的测量信号一旦带噪声，纯微分会把噪声放大到离谱。我的做法是每个微分环节都加一个一阶低通滤波器，时间常数取0.01~0.02秒。这会让高频噪声被压下去，而摆杆的固有频率通常远低于这个滤波频率，稳态性能不会明显受损。
第三个是代数环。如果你按“输出直接反馈到控制器，再直接作用于输入”的方式搭建，Simulink会报代数环问题或迭代变慢。破解办法是在反馈回路里插入很小的纯延迟，比如一个单位延迟或者一个低通环节，用来打破输入输出的瞬时耦合。注意延迟选择要小，比如采样步长的1/10，太大则会改变闭环特性。
4.3 跑出来的数据怎么读：稳定时间、超调、控制能量
仿真结束后，不要只看“最后稳没稳”，要把过程指标全部记录下来。我常用三个指标来对比：
- 稳定时间：从初始偏差到所有状态进入平衡点附近±5%范围，并且不再逃逸的时间
- 最大超调：θ1、θ2在调节过程中的最大绝对值，反映动态过程的“凶险程度”
- 控制能量：对u²在时间上积分，衡量控制付出的代价
在我跑的这组典型结果中，PID和LQR的表现差距很明显，我整理成一张简表方便你对照参考：
| 对比指标 | 串级PID（手工整定的较优结果） | LQR（Q/R经验整定） |
|---|---|---|
| 稳定时间 | 约3.2秒 | 约1.4秒 |
| θ2最大超调 | 约0.28 rad | 约0.10 rad |
| 控制量峰值 | 接近限幅20 N | 约14 N |
| 控制能量积分 | 约96 J | 约61 J |
| 加入参数+20%扰动 | 明显振荡，接近临界 | 仍能稳定回正 |
这些数值的具体大小会随物理参数变化，但趋势在多数合理参数下是一致的：LQR对多变量耦合的处理效率更高，动态过程中角度超调更小；PID如果肯花时间调，也能稳住，但它的鲁棒性明显依赖“工作点”，一旦模型参数偏差变大，最容易先出问题。
4.4 增加一组扰动和噪声测试：鲁棒性才是真正分水岭
只比无扰动情况不够过瘾，我给仿真加了一组“压力测试”：在第6秒时给上摆一个瞬时冲击力矩，同时保持传感器噪声开启。这时你会看到PID系统会有一段明显的摆动恢复过程，角度超调可能达到0.3 rad以上，而LQR系统的恢复时间更短，超调也更小。
这背后的原因值得说透：PID的每个回路只看到自己的误差信号，回路之间的协调完全靠整定形成的“默契”；而LQR的每一路反馈都同时利用全部状态信息，相当于针对系统动力学做了一次全局协调。没有哪个控制器是万能的，但在这个被控对象上，LQR的结构优势非常明显。给报告或答辩做分析时，突出这个维度比罗列一堆波形更有说服力。
5. 仿真中不踩不行的坑：我的排查顺序和解决方案
5.1 仿真一开跑就发散？先别急着怀疑算法
仿真发散的排查顺序很重要。很多人的第一反应是“是不是LQR算错了”，实际我遇到的情况里，最常见的原因是初始角度超出线性化邻域。我曾在一次实验中直接给θ2设成0.3 rad，结果两个控制器全部发散，当时吓一跳，后来把初始角度降到0.1 rad就完全正常。其次是控制量限幅参数写错，如果限幅值设得太小，控制器输出被钳制，无法产生足够的恢复力矩，系统当然发散。排查发散问题请按这个顺序来：
- 检查初始状态是否在工作点邻域
- 检查控制量是否频繁撞限幅
- 检查求解器是否适合刚性系统
- 检查代数环是否导致了隐式迭代异常
- 最后才回到控制器参数本身
5.2 PID加积分项后越调越疯，是普通教材没讲的事
我在前面已经预告过这一点，这里再展开一次。给角度内环加积分项，理论上可以消除稳态误差，但在存在执行器限幅的倒立摆系统里，一旦控制量饱和，积分项会不断累积误差，变成“越错越多，输出越偏”。这导致的典型现象就是摆杆开始低频大幅振荡，周期往往好几秒，跟高频抖动完全两回事。解决之道有三：不用积分项，只靠位置外环的积分去消除最终位置误差；或者给积分器接一个限幅器，限制积分量大小；再或者用条件积分法，只在控制量未饱和时积分。这三种方法我都试过，在课程设计场景里最省事的就是第一种：内环纯PD，位置环用普通PI。
5.3 LQR明明在理论上最优，为什么仿真结果还是不好
这类问题十有八九出在Q、R矩阵和模型匹配上。你给位置状态权重太高，控制器会拼命拉小车回位，结果导致摆杆角度被牵连放大；你给R设得太小，初始时刻控制量接近限幅，系统变成一个“开关型”控制行为，LQR的线性最优意义就消失了。另一个容易被忽略的因素是，Simulink里的被控对象往往是非线性模型，而K是在线性化模型上解出来的。当状态偏离工作点较远时，实际闭环特性会和设计时不一致。
我的调整建议是：把Q中角度权重调高，观察角度超调是否下降；如果控制量依然触限，就增大R，通常每增加一个数量级试一次，找到“控制不触限”和“响应够快”的平衡点。这个调参过程看着不“现代”，但工程上确实没有免费午餐。
5.4 状态初值对仿真结果影响极大，别忽略归一化
最后一个我印象很深的坑来自一个看似不起眼的问题：我直接在Simulink状态空间模型里给6个状态设初值，但总是出现“初始时刻有巨大跳变”的曲线。后来发现是因为把角度单位直接用了弧度，而把位置单位用了米，各自的数值尺度相差近10倍，初始状态在控制器看来就像一个大偏置。这不是系统不稳定，而是量纲没有做归一化处理。
实际项目里，我一般先把位置除以某个参考长度，把角度除以某个参考角度，让状态都落在相近的数量级，然后再设计Q矩阵。这样做的另一个好处，是Q矩阵每个对角元素可以被直接理解成“这个状态偏离零点有多么不可接受”，而不是一个没有量纲感的抽象数字。这个经验在后续做其他多变量系统时同样适用。
6. 最后说一点我做这组仿真之后的真实体会
整组仿真做完，我最大的感受不是“LQR比PID强”，而是“对比仿真最大的价值，是逼你把每个环节都搞清楚”。单纯调一个控制器，你可以在参数上碰运气；但要你同时解释两种控制器为什么表现不同，你就必须回到A矩阵、可控性、频率特性这些底层概念去。也正因为如此，如果你现在正卡在调试里，我建议你少钻“再调一下增益”的牛角尖，多花点时间把模型线性化结果打印出来，逐项看一遍每个状态之间的耦合系数。很多参数调不动的问题，根源其实都在模型层面，而不在控制器。
另外还有一个非常实用的小技巧：不要只记录最终那一组“好用”的参数，把你调参过程中每一版参数和对应的现象像实验记录一样保存。因为你在报告中或者答辩时被问到的第一个问题，往往是“为什么从这组参数改成那组参数”。能现场调出几组对比图并解释变化趋势，比你说再多理论都有说服力。二级倒立摆这个题目本身不新鲜，新鲜的永远是你对系统、对算法的理解深度，而这恰恰是这套仿真真正想检验的东西。
