直线二级倒立摆这种东西只要在控制方向做过的人基本都绕不开。一开始我也觉得不就是小车上面接两根杆子想办法让它们竖着别倒就行了吗真上手才发现经典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矩阵对角线元素和Rx [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.4c₁和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]R1PSO搜索到的一组对数解码结果约为[158, 0.8, 620, 15, 950, 25]R0.7。两者在数值上看着接近但闭环响应差别很直观对比结果整理如下性能指标手工试凑LQRPSO优化LQR小车位移调节时间2.7s2.1s下摆最大角度偏差4.3度3.5度上摆最大角度偏差2.1度1.7度控制量均方根8.6N7.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把控制结构定下来再由优化算法处理参数选择。这样的分工让每一步都足够清晰也方便日后扩展——比如从二级摆换到多级摆从单输入换到多输入只需要改模型和适应度函数优化框架完全不用动。如果你准备在自己的平台上复现这个项目我建议从单级倒立摆或者纯仿真模型开始把整套代码跑通之后再切换到二级摆实际系统上验证这样排查问题时能少很多干扰项。
直线二级倒立摆这种东西只要在控制方向做过的人基本都绕不开。一开始我也觉得不就是小车上面接两根杆子想办法让它们竖着别倒就行了吗真上手才发现经典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矩阵对角线元素和Rx [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.4c₁和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]R1PSO搜索到的一组对数解码结果约为[158, 0.8, 620, 15, 950, 25]R0.7。两者在数值上看着接近但闭环响应差别很直观对比结果整理如下性能指标手工试凑LQRPSO优化LQR小车位移调节时间2.7s2.1s下摆最大角度偏差4.3度3.5度上摆最大角度偏差2.1度1.7度控制量均方根8.6N7.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把控制结构定下来再由优化算法处理参数选择。这样的分工让每一步都足够清晰也方便日后扩展——比如从二级摆换到多级摆从单输入换到多输入只需要改模型和适应度函数优化框架完全不用动。如果你准备在自己的平台上复现这个项目我建议从单级倒立摆或者纯仿真模型开始把整套代码跑通之后再切换到二级摆实际系统上验证这样排查问题时能少很多干扰项。
