直线二级倒立摆这种东西，只要在控制方向做过的人，基本都绕不开。一开始我也觉得，不就是小车上面接两根杆子，想办法让它们竖着别倒就行了吗？真上手才发现，经典PID压不住这种系统，换成LQR状态反馈之后，又一头扎进Q矩阵、R矩阵那六个权重加一个控制权重的试凑黑洞里，一组满意的参数往往能磨掉好几天。这也是我写这篇内容的直接原因：把粒子群算法（PSO）和LQR设计绑在一起，让程序自己去搜最优权重，把“凭手感试凑”变成“按指标搜索”。我会把整个项目过程从头到尾梳理一遍，包含建模方法、LQR设计、PSO寻优的完整套路，以及我从仿真到实验踩过的坑，希望能让准备做类似课题的人少走点弯路。
1. 为什么二级倒立摆这么难：先看清控制难点在哪
1.1 系统结构与被试对象定位
直线二级倒立摆的物理结构很直观：一辆小车在直线导轨上运动，小车上串联两根摆杆，下摆通过铰链连接小车，上摆通过铰链连接下摆。控制目标就是让两根摆杆在竖直向上的平衡点附近保持稳定，同时让小车在导轨上不跑飞。听起来简单，但它作为控制算法验证平台之所以长盛不衰，是因为它把控制理论里最难啃的几个特性全集中在一个小装置上了。
和一级倒立摆相比，二级摆的最大变化在于自由度增加了，而输入还是只有一个。小车的位置算一个自由度，下摆角度是一个，上摆角度又是一个，单输入控制三个自由度，系统的耦合关系也更复杂。一级摆的控制经验很难平移到二级摆上，因为上摆的运动是依靠下摆传递的，下摆既要平衡自己，又要为上层摆杆提供支持，控制器的设计自由度被大幅压缩。
1.2 核心控制难点拆解
第一个难点是欠驱动。系统的控制输入只有小车受到的驱动力，而我们需要同时调节小车位移和两级摆杆角度，这是一个典型的欠驱动系统。直白地说，你没有一个独立的“电机”直接去推摆杆，只能通过推小车间接地影响摆杆，这要求控制器对动力学关系有非常准确的把握。
第二个难点是开环不稳定。倒立平衡点本质上是系统的鞍点，任何微小的扰动都会让摆杆加速倒下。更麻烦的是，这个不稳定不是慢吞吞地偏离，而是指数级的发散，留给控制器的反应时间窗口非常短，控制周期稍微慢一点，摆杆就倒下去了。
第三个难点是强非线性和强耦合。摆杆角度一大，系统的运动学方程就会出现明显的非线性项，两级摆杆之间还会互相“拖累”。控制器的设计如果只在小角度线性模型上较真，到了实际装置上很容易因为非线性效应而失控。
第四个难点是非最小相位特性。系统的传递函数存在右半平面零点，意味着系统在响应初期会表现出“反向”的运动趋势，直观说就是小车需要先把摆杆往一个方向推，才能最终往另一个方向移动。这种特性让基于直觉的调节方法经常失效。
需要说明的是，本项目的仿真模型参数取自某实验室的常规配置，不同装置的机械参数差异很大，后面的参数只作为参考，复现时务必按自己的实际平台重新辨识。
2. 建模与LQR基线控制器：先让系统“能稳定”，再说“稳得好”
2.1 从拉格朗日方程到状态空间模型
设计LQR之前，必须先拿到被控对象的数学模型。二级倒立摆的建模主流有两种路线：牛顿力学方法和拉格朗日方法。我个人更推荐拉格朗日方法，因为它在处理多刚体系统的耦合关系时不需要逐个分析每个铰点上的约束力，出错概率更低。
建模时先定义广义坐标：小车位移、下摆角度、上摆角度，然后写出系统动能和势能的表达式。这里有一个关键选择：要不要考虑摩擦。仿真模型如果完全不考虑摩擦，控制器设计出来会偏激进，实际装置上反而容易抖动；如果摩擦系数取得太大，又会让仿真结果过于悲观。我在仿真里采用的是一组带线性阻尼的简化模型，把导轨摩擦和铰链阻尼统一折算成等效粘性阻尼。
在竖直向上平衡点附近做小角度线性化处理，忽略高阶非线性项之后，系统可以写成标准的连续时间状态空间形式：
ẋ = Ax + Bu
其中状态向量取为：
x = [小车位移p, 小车速度ṗ, 下摆角度θ1, 下摆角速度θ̇1, 上摆角度θ2, 上摆角速度θ̇2]ᵀ
输入u为小车驱动力。系统的A矩阵结构上有明显的规律，大致可以表示成下面的形式：
A = [0 1 0 0 0 0; a21 0 a23 0 a25 0; 0 0 0 1 0 0; a41 0 a43 0 a45 0; 0 0 0 0 0 1; a61 0 a63 0 a65 0]B矩阵的形式则和驱动力如何作用于小车相关。这里不把各个系数的完整表达式铺开写，因为不同教材的符号约定差别很大，直接抄公式容易抄错。我仿真测试用的一组代表性参数为：小车质量2.1kg，下摆质量0.12kg，上摆质量0.18kg，下摆长度0.2m，上摆长度0.25m，重力加速度取9.81，等效粘性阻尼取0.1。算出来的A、B矩阵可以直接丢进MATLAB的lqr函数使用。
提示：如果目标是复现这个项目，建议先用符号推导工具把拉格朗日方程展开，再代入参数数值化，不要手算这些耦合系数。手算不仅容易出错，而且后期调整参数时整个流程要重来。
2.2 LQR控制器的设计逻辑
有了状态空间模型之后，LQR的设计思路就非常清晰了。LQR全称是线性二次型调节器，它的核心是找一个状态反馈控制律u = -Kx，使得下面这个无限时间性能指标最小化：
J = ∫₀^∞ (xᵀQx + uᵀRu) dt
这里的Q是半正定对称矩阵，对状态变量进行加权惩罚；R是正定对称矩阵，对控制量进行惩罚。性能指标的经济学含义很直白：Q决定了你有多在乎“状态误差收敛快不快”，R决定了你有多在乎“控制力度不能太大”。某个状态对应的Q权重越大，控制器就越拼命把这个状态压到零，代价是控制能量变大甚至出现震荡；R越大，控制器就越“抠门”，输出控制力越小，代价是收敛变慢。
最优反馈增益矩阵K通过求解代数黎卡提方程得到：
AᵀP + PA - PBR⁻¹BᵀP + Q = 0
解出正定矩阵P之后，K = R⁻¹BᵀP。这个方程在MATLAB里直接调用lqr(A, B, Q, R)就能算出来，核心工作量不在解方程，而在怎么选Q和R。
2.3 手调Q/R的真实痛点
说实话，LQR的理论部分不难，难就难在那个Q矩阵上。六个状态权重加一个R，等于七个自由参数。而且这些参数之间是相互制约的：调大下摆角度的权重，摆杆是稳了，但小车位移的收敛可能变慢；加大小车位移权重，又可能诱发上摆角度的超调。更头疼的是，这些权重差着数量级，经常要在1到1000之间来回跳，完全没有任何直觉可以依赖。
我最初用试凑法调这组权重的时候，流程是典型的“改参数、跑仿真、看曲线、再改参数”，每一步只能靠经验判断往哪个方向调。调了两天，得到一组可用参数，但你没法知道它到底是不是最优的。更现实的问题是，这种手工试凑得到的LQR，往往对某个性能指标（比如摆杆角度超调）过分保守，或者对另一个指标（比如控制能量）浪费严重。
这也解释了为什么我后来转向粒子群算法：LQR权重的寻优本质上是一个连续参数优化问题，目标函数可以通过仿真来评估，正好是PSO这类群体智能算法的擅长领域。写到这里，核心问题就变成了：怎么把这个寻优问题设计得合理且高效。
3. PSO优化LQR权重：把“试凑”变成“搜索”
3.1 PSO的基本思想
粒子群算法是我个人很喜欢的一种优化方法，因为它的逻辑足够直观。想象有一群鸟在某个区域里找食物，每只鸟知道自己的当前位置有多好，也知道整个群体目前找到的最优位置在哪里。每只鸟的运动方向取决于三个因素：自己当前的速度（惯性）、自己历史最佳位置的吸引力（个体认知）、群体历史最佳位置的吸引力（社会认知）。
用数学语言表达就是，每个粒子i有一个位置向量xi和一个速度向量vi，在每次迭代中按照下面两个公式更新：
vᵢ ← w·vᵢ + c₁·r₁·(pbestᵢ - xᵢ) + c₂·r₂·(gbest - xᵢ)
xᵢ ← xᵢ + vᵢ
w是惯性权重，控制着粒子保持原有运动方向的程度；c₁和c₂是学习因子，控制粒子向自身历史最优和群体历史最优学习的强度；r₁和r₂是[0,1]之间的随机数，用来维持搜索的随机性。这个算法最大的好处是：不需要计算梯度，对目标函数没有连续、可导的要求，只要你能给出一个“位置好不好”的评分，它就能搜。
3.2 编码方案与约束处理
用PSO优化LQR的第一步，是决定粒子位置向量对应什么。我采用的方案是粒子位置x直接编码Q矩阵对角线元素和R：
x = [q₁, q₂, q₃, q₄, q₅, q₆, r]
其中q₁到q₆分别对应六个状态权重，r对应控制权重。这里有一个非常关键的小技巧：直接搜原始权重值效果很差，因为各个权重跨度很大，比如q₃可能需要到几百甚至上千，而q₂可能只需要个位数，高维空间中不同量纲的维度会让欧氏距离失真，粒子在某个维度的搜索步长很难同时适应所有尺度。
我的做法是对权重取对数后再搜索。粒子位置向量实际编码的是：
x = [log₁₀(q₁), log₁₀(q₂), ..., log₁₀(q₆), log₁₀(r)]
这样每个维度都落在大致相近的数量级范围内，搜索速度均匀得多，收敛也更快。每次评估粒子时，先把对数值还原成实际权重，再构造Q = diag(q₁, ..., q₆)和R = r，喂给LQR求解。
约束处理也不能省。首先，LQR要求Q半正定、R正定，这在代码里必须显式检查，不满足条件的粒子直接赋予一个极大的惩罚适应度值。其次，即使Q和R满足正定性，解出的K也不一定能让闭环系统稳定，一定要检查闭环矩阵A - BK的特征值是否全部落在左半平面。这两个条件任何一个不满足，粒子的适应度直接判为无穷大，让PSO自动把搜索方向引导到可行域内。
3.3 适应度函数的设计
适应度函数是整个优化过程的指挥棒，它的设计水平直接决定了最终控制器的性格。如果适应度只包含稳定时间，优化出来的控制器可能各种超调；如果只包含超调量，又可能收敛慢到没法用。我采用的是多指标加权组合，包含以下几个部分：
- 小车位移的调节时间ts
- 下摆角度的最大绝对超调max|θ₁|
- 上摆角度的最大绝对超调max|θ₂|
- 控制量的均方根值u_rms
适应度函数写成：
F = α₁·ts + α₂·max|θ₁| + α₃·max|θ₂| + α₄·u_rms
四个系数α₁到α₄表示你对各指标的重视程度。我在项目里取的是α₁=5，α₂=30，α₃=50，α₄=0.1。你会发现我把摆杆角度超调的权重放得特别大，这是因为在二级倒立摆系统中，摆杆角度一旦超过某个阈值（比如20度以上），线性模型的假设就不再成立，仿真结果和真实装置会严重背离，所以优先保证小角度是绝对底线。
注意：适应度函数里各项指标的量纲不一样，时间单位是秒，角度单位是弧度，控制力单位是牛顿，必须通过系数把它们的数量级拉到一个可比范围内，否则结果会被某个数值上特别大的分项主导，优化出来的控制器会严重偏向某一个指标。
3.4 PSO参数配置经验
PSO本身有几个超参数需要设置：种群规模、迭代次数、惯性权重、学习因子。我长期使用的一套配置是：种群规模30，迭代次数100，惯性权重从0.9线性递减到0.4，c₁和c₂都取1.5，速度使用限幅处理。惯性权重线性递减是我比较推荐的做法：前期w大，粒子探索范围广，不容易陷进局部最优；后期w小，粒子精细搜索当前最优区域。
我还会加上速度限幅，限制每个维度上速度的最大绝对值，防止某个粒子飞得太远导致位置越界。种群规模和迭代次数不需要特别大，因为每次适应度评估内部包含一次完整的LQR求解和闭环仿真，计算成本不低，过大的种群会拖慢速度。
4. 完整实操流程：从代码结构到结果对比
4.1 仿真环境搭建
我是在MATLAB/Simulink环境里做的，原因是lqr函数和数值仿真工具链成熟，写起来省事。整个项目拆成三个模块：模型初始化脚本、PSO主程序、Simulink闭环仿真模型。模型初始化脚本负责定义系统参数、计算A和B矩阵、设置PSO超参数；PSO主程序负责种群初始化、迭代更新、适应度评估；Simulink模型负责在每次评估时对给定K做一次闭环扰动仿真。
闭环仿真有一个细节值得说：初始扰动怎么设。我的做法是给下摆一个3度的初始偏角，上摆保持竖直，让控制器从静止状态把系统拉回平衡点。这个初始条件代表了最常规的抗扰场景，也是比较严格的一种测试方式，因为两级摆杆的初始偏差会通过耦合层层放大。
4.2 关键代码结构
适应度评估函数的核心逻辑如下，这段代码的思路可以直接复用到其他控制任务：
function F = pso_lqr_fitness(x) % 输入的x是对数编码: [log10(q1)~log10(q6), log10(r)] q = 10.^x(1:6); r = 10^x(7); Q = diag([q(1), q(2), q(3), q(4), q(5), q(6)]); % 正定约束检查 if min(eig(Q)) <= 0 || r <= 0 F = 1e7; return; end % 求解LQR增益 [K, ~, ~] = lqr(A, B, Q, r); % 闭环稳定性约束检查 if any(real(eig(A - B * K)) >= 0) F = 1e7; return; end % 运行闭环仿真, 提取性能指标 simOut = sim('double_pendulum_lqr', 5); t = simOut.tout; y = simOut.yout; ts = calc_settling_time(t, y(:, 1)); % 位移调节时间 th1max = max(abs(y(:, 2))); % 下摆最大超调 th2max = max(abs(y(:, 3))); % 上摆最大超调 urms = rms(simOut.u); % 控制量均方根 % 加权组合 F = 5 * ts + 30 * th1max + 50 * th2max + 0.1 * urms; endPSO主循环是标准的粒子更新过程，加上每组搜索结束后记录全局最优粒子、更新惯性权重即可。这里强调一个实操习惯：每次优化运行结束后，把全局最优的Q、R和K保存到单独的文件里，并且记录完整的迭代收敛曲线。这个文件既是你的实验结果凭证，也是后续调参的基准线。
4.3 优化结果对比
我把手工试凑的LQR参数和PSO优化后的参数做了对比。手工试凑得到的Q对角权重大致是[100, 1, 500, 10, 800, 20]，R=1；PSO搜索到的一组对数解码结果约为[158, 0.8, 620, 15, 950, 25]，R=0.7。两者在数值上看着接近，但闭环响应差别很直观，对比结果整理如下：
| 性能指标 | 手工试凑LQR | PSO优化LQR |
|---|---|---|
| 小车位移调节时间 | 2.7s | 2.1s |
| 下摆最大角度偏差 | 4.3度 | 3.5度 |
| 上摆最大角度偏差 | 2.1度 | 1.7度 |
| 控制量均方根 | 8.6N | 7.9N |
| 优化耗时 | 两天人肉调试 | 约15分钟程序搜索 |
这组数据说明，PSO找出来的参数不是简单地“某个指标更好”，而是整体性能更均衡：收敛快了，角度偏差小了，控制能量反而更省。手工试凑的瓶颈在于你只能凭感觉在几个方向之间反复权衡，而PSO可以在高维空间里同时搜索整个可行域，找到的往往是更接近帕累托前沿的解。
我还做了一次延展测试：改变初始扰动幅度，从3度增加到8度，PSO优化出的控制器依然能稳定系统，而手工试凑的参数在超过5度之后开始出现明显的摇摆往复。这再次印证了适应度函数里对角度超调的重惩罚是有效的，它在优化时自动保证了控制器对小扰动之外还有一定的裕度。
5. 常见问题与避坑指南
5.1 PSO早熟收敛或者一直不收敛
早熟收敛是PSO最经典的毛病，表现为迭代曲线早早进入平台期，但gbest对应的控制器在仿真里表现很一般。我遇到这种情况的第一反应不是改迭代次数，而是检查适应度函数的区分度。如果大量粒子算出来适应度都一样，说明你的性能指标不够敏感，或者某个惩罚项把所有不合格粒子都压成了同一个极大值，搜索梯度信息就消失了。
另一个常见原因是Q权重的搜索范围给得不够。如果初雪范围只在某几个数量级内，粒子很难跳到真正有希望的区域。我的做法是初始种群在[log₁₀(0.1), log₁₀(1000)]这个范围内均匀撒点，覆盖面足够大，避免一开始就把搜索空间限制死了。
5.2 约束惩罚值怎么选
约束惩罚值的设置是个容易被忽视但非常重要的细节。惩罚值太小，比如只有1万，不合格粒子有可能混在正常粒子里影响gbest的判断；惩罚值太大，又可能导致所有不合格粒子的适应度都等于同一个巨大数值，粒子不知道该往哪个方向飞。我实际使用的惩罚值是1e7，比任何正常控制器的最差适应度都高一个数量级，确保不合格粒子永远不会污染最优解集合。检查标准很简单：如果优化过程中gbest连续多轮迭代没有变化，先去看看gbest对应的是不是惩罚值而不是真实适应度。
5.3 仿真和实物之间的差距
从仿真走向实物时有三处最容易翻车。第一处是状态测量：LQR要求全状态反馈，仿真里所有状态都是完美的，实物上小车速度、摆杆角速度都得靠编码器差分或陀螺仪获得，微分信号噪声极大，不滤波控制器会抖得没法看。第二处是执行器饱和：仿真里的驱动力可以是任意值，实物电机的推力有限，我在适应度函数里加了控制量rms惩罚项，其实就是变相压制控制输入，让它不至于超出执行器能力。第三处是模型不确定性：摩擦、齿隙、摆杆重心偏差都会让实际系统偏离理想模型，所以仿真结果要留出至少30%的稳定裕度，不要把参数优化得过于极限。
5.4 离散化和采样周期
如果要在嵌入式平台或实时控制器上运行，连续时间K不能直接用。我建议在仿真阶段就顺带验证离散化的影响：选择采样周期T，用零阶保持离散化得到离散状态方程，再计算离散LQR或者直接把连续K离散映射到控制周期上。采样周期选多大取决于系统闭环动态的带宽，二级倒立摆的动态比较快，我测试时用1ms和5ms采样周期都能稳住系统，但10ms时明显感觉到控制质量的下降。趁仿真阶段把这个问题摸清楚，能省掉后面在实验平台上的很多调试时间。
最后再分享一个个人体会。把PSO和LQR结合的这个流程，本质上是把“控制器设计问题”拆成了两半：先由LQR把控制结构定下来，再由优化算法处理参数选择。这样的分工让每一步都足够清晰，也方便日后扩展——比如从二级摆换到多级摆，从单输入换到多输入，只需要改模型和适应度函数，优化框架完全不用动。如果你准备在自己的平台上复现这个项目，我建议从单级倒立摆或者纯仿真模型开始，把整套代码跑通之后，再切换到二级摆实际系统上验证，这样排查问题时能少很多干扰项。
