1. 从一个“怎么推都不倒”的玩具说起倒立摆这东西第一次见的人多半会觉得它像个魔术道具一根杆子立在能左右滑动的小车上你松开手它明明该倒可它偏偏晃两下又自己站稳了你推它一下它退几步又回来甚至你让小车沿着一条曲线走杆子还能全程保持不倒。我第一次在实验室看到这个演示的时候脑子里只有一个念头——这玩意儿到底是怎么算出来的。后来自己动手从零推了一遍模型、写了一遍LQR控制器才发现它其实没有那么玄。倒立摆是控制领域最经典的教学案例之一它把“非线性系统线性化”“状态空间建模”“最优控制”这几个核心概念串成了一条完整的链路。你如果能独立把倒立摆的动力学方程推出来再设计一个LQR控制器让它跟踪一条位置轨迹那说明你对现代控制理论的理解已经过了入门关。这篇文章面向的是正在学自动控制原理、现代控制理论或者准备做倒立摆相关课程设计、毕业设计、竞赛项目的同学。我会从受力分析开始一步步把非线性模型推出来再在平衡点附近线性化然后设计LQR控制器最后加上位置跟踪的外环。整个过程我会把“为什么这么做”讲清楚把参数怎么选、坑在哪里、调试时看什么现象都交代明白。你跟着走一遍应该能自己复现出一套能跑的仿真。需要提前说明的是下面涉及的数值参数都是基于常见实验平台的合理取值你在自己的平台上需要根据实际物理参数重新测量和调整。代码示例用Python写方便你直接跑起来看效果移植到MATLAB或者C也不难。2. 倒立摆建模从受力分析到状态空间2.1 为什么选小车-摆杆这一套结构倒立摆有很多变体一级、二级、环形、旋转式等等。我这里选最基础的一级直线倒立摆也就是小车在水平导轨上运动、摆杆铰接在小车上的那种。原因有三个第一它的动力学方程推导难度适中不会一上来就被复杂的耦合项劝退第二它的状态量少四个LQR的维数低调试起来直观第三绝大多数实验平台都是这个构型你推出来的模型可以直接和实物对照。在开始推导之前先把假设条件列清楚这些假设决定了你的模型和真实系统之间有多大的差距小车和摆杆都视为刚体不考虑弹性变形。摆杆的质量均匀分布质心在杆的中点。导轨摩擦和铰链摩擦先用粘性摩擦近似后面可以再加库仑摩擦项。小车位置通过编码器测量摆杆角度通过角度传感器测量暂时不考虑噪声和延迟。这些假设在低速、小角度场景下是合理的。如果你的摆杆很长很软或者小车加速度很大那模型误差就会明显增大这时候要么细化模型要么在控制器里留足够的鲁棒性余量。2.2 拉格朗日法推导非线性方程推导倒立摆动力学有两种常用方法牛顿-欧拉法和拉格朗日法。牛顿法需要分别对小车和摆杆做受力分析然后处理铰链处的约束力过程比较繁琐。拉格朗日法从能量角度出发不需要关心内部约束力对于这种多体系统更清爽。我选拉格朗日法。先定义广义坐标小车水平位置 (x)摆杆与竖直向上方向的夹角 (\theta)。注意这里 (\theta) 的定义方式很关键——我取竖直向上为零点逆时针为正。这样当摆杆竖直立稳时 (\theta 0)线性化的时候直接在零点附近展开就行。如果你取竖直向下为零点那平衡点就在 (\theta \pi) 附近线性化会多一层坐标变换容易出错。系统的动能包括小车平动动能和摆杆的平动加转动动能。摆杆质心位置为[ x_p x l \sin\theta, \quad y_p l \cos\theta ]其中 (l) 是铰链到摆杆质心的距离对于均匀杆 (l L/2)(L) 是杆全长。对时间求导得到质心速度然后写出总动能[ T \frac{1}{2}(Mm)\dot{x}^2 m l \dot{x}\dot{\theta}\cos\theta \frac{1}{2}\left(\frac{1}{3}mL^2\right)\dot{\theta}^2 ]这里 (M) 是小车质量(m) 是摆杆质量(\frac{1}{3}mL^2) 是均匀杆绕质心的转动惯量。势能以铰链高度为零点[ V mgl\cos\theta ]拉格朗日量 (L T - V)代入拉格朗日方程[ \frac{d}{dt}\frac{\partial L}{\partial \dot{q_i}} - \frac{\partial L}{\partial q_i} Q_i ]对 (x) 和 (\theta) 分别处理得到两个耦合的二阶微分方程。对 (x) 方向[ (Mm)\ddot{x} ml\ddot{\theta}\cos\theta - ml\dot{\theta}^2\sin\theta F - b\dot{x} ]对 (\theta) 方向[ ml\ddot{x}\cos\theta \frac{4}{3}ml^2\ddot{\theta} - mgl\sin\theta -c\dot{\theta} ]其中 (F) 是施加在小车上的水平力(b) 是导轨粘性摩擦系数(c) 是铰链粘性摩擦系数。这两个方程就是倒立摆的完整非线性模型。注意很多教材在推倒立摆时会把摆杆简化为质点集中在杆端那样转动惯量项会变成 (ml^2) 而不是 (\frac{4}{3}ml^2)。两种模型在仿真里都能用但如果你要和实物对比建议用均匀杆模型因为实际摆杆的质量分布更接近均匀杆。这个细节在调试时会体现出来——用质点模型算出来的控制增益放到实物上可能会偏软。2.3 平衡点线性化与状态空间形式非线性模型没法直接用LQR因为LQR要求系统是线性的。所以下一步是在平衡点附近做线性化。倒立摆有两个平衡点竖直向下稳定但不稳定听起来矛盾意思是无控制时它自然停在那里但一旦有扰动就回不来和竖直向上不稳定需要控制才能维持。我们关心的是竖直向上那个也就是 (\theta 0, \dot{\theta} 0) 附近。线性化的方法是在平衡点附近做泰勒展开保留一阶项。对于 (\theta) 很小的情况有近似[ \sin\theta \approx \theta, \quad \cos\theta \approx 1, \quad \dot{\theta}^2 \approx 0 ]把这些代入非线性方程得到线性化后的方程[ (Mm)\ddot{x} ml\ddot{\theta} F - b\dot{x} ][ ml\ddot{x} \frac{4}{3}ml^2\ddot{\theta} - mgl\theta -c\dot{\theta} ]整理成矩阵形式。定义状态向量 (X [x, \dot{x}, \theta, \dot{\theta}]^T)输入 (u F)输出 (y x)我们关心小车位置。经过代数运算可以得到[ \dot{X} AX Bu ][ y CX ]其中矩阵 (A) 和 (B) 的表达式可以通过求解上述方程组得到。我直接给出结果你可以自己验证[ A \begin{bmatrix} 0 1 0 0 \ 0 -\frac{(Mm)b mlc}{D} -\frac{m^2l^2g}{D} \frac{mlc}{D} \ 0 0 0 1 \ 0 \frac{mlb (Mm)c}{D} \frac{(Mm)mgl}{D} -\frac{(Mm)c}{D} \end{bmatrix} ][ B \begin{bmatrix} 0 \ \frac{\frac{4}{3}ml^2}{D} \ 0 \ -\frac{ml}{D} \end{bmatrix} ]其中 (D (Mm)\frac{4}{3}ml^2 - m^2l^2)。这个矩阵看起来吓人但你在代码里只需要把物理参数代进去让计算机算就行。关键是理解每个元素的物理含义(A) 矩阵描述状态之间的耦合关系比如摆杆角度会影响小车加速度小车加速度又会影响摆杆角速度(B) 矩阵描述输入力如何影响各个状态。实操心得我第一次推完这个矩阵的时候用数值代入验证了一下特征值发现有一个正实部特征值说明开环系统确实不稳定这符合预期。如果你算出来的特征值全是负实部那多半是符号或者参数代错了回去检查 (\theta) 的正方向定义和重力项符号。3. LQR控制器设计从代价函数到增益矩阵3.1 LQR到底在优化什么LQR的全称是线性二次型调节器。名字里的“线性”指系统是线性的“二次型”指代价函数是状态和输入的二次型。它的核心思想是你希望系统状态尽快回到零同时又不想让控制输入太大这两者之间有矛盾LQR通过一个代价函数把矛盾量化然后求使代价最小的控制律。代价函数的标准形式是[ J \int_0^\infty (X^T Q X u^T R u) dt ]其中 (Q) 是状态权重矩阵(R) 是输入权重矩阵。(Q) 越大控制器越在意状态偏差回复越快但输入越大(R) 越大控制器越在意输入大小动作越柔和但回复越慢。LQR的最优解是状态反馈[ u -KX ]其中 (K R^{-1}B^T P)(P) 是代数黎卡提方程的解[ A^T P PA - PBR^{-1}B^T P Q 0 ]这个方程在Python里用scipy.linalg.solve_continuous_are可以直接解不需要自己写迭代。3.2 Q和R怎么选从物理意义出发很多人第一次用LQR最头疼的就是Q和R怎么调。网上有些教程直接给一组“万能参数”但换一个平台就不灵了。我的建议是从物理意义出发分步调。先看状态向量 (X [x, \dot{x}, \theta, \dot{\theta}]^T)。四个状态的量纲不同位置是米速度是米每秒角度是弧度角速度是弧度每秒。如果直接给它们相同的权重那量纲大的状态会主导代价函数量纲小的状态被忽略。所以Q矩阵通常取对角阵每个对角元素对应一个状态的权重而且要根据量纲做缩放。我的经验做法是先只惩罚角度和角速度让摆杆能立起来。这时候Q取[ Q \text{diag}(0, 0, 100, 10) ]R取一个较小的值比如0.01。跑仿真看摆杆能不能稳住。如果能稳住但小车漂移再逐步增加位置和速度的权重。最终我常用的一组参数是[ Q \text{diag}(100, 10, 200, 20), \quad R 0.01 ]这组参数的含义是位置偏差100米才和角度偏差1弧度等价速度偏差10米每秒和角速度偏差1弧度每秒等价。实际调试时你可以先把R固定然后按比例放大Q观察响应速度和超调的变化。注意Q和R的绝对大小不重要重要的是它们的比值。你把Q和R同时乘以一个常数最优增益K不变。所以调参时只需要关注相对比例。3.3 用Python求解LQR增益下面是一段可以直接跑的代码从定义参数到算出K矩阵import numpy as np from scipy.linalg import solve_continuous_are # 物理参数根据你的平台修改 M 0.5 # 小车质量 kg m 0.2 # 摆杆质量 kg L 0.6 # 摆杆全长 m l L / 2 # 铰链到质心距离 m g 9.81 # 重力加速度 b 0.1 # 导轨摩擦系数 c 0.01 # 铰链摩擦系数 # 中间变量 D (M m) * (4/3) * m * l**2 - (m * l)**2 # 状态矩阵A A np.array([ [0, 1, 0, 0], [0, -((Mm)*b m*l*c)/D, -(m**2 * l**2 * g)/D, (m*l*c)/D], [0, 0, 0, 1], [0, (m*l*b (Mm)*c)/D, ((Mm)*m*g*l)/D, -((Mm)*c)/D] ]) # 输入矩阵B B np.array([ [0], [(4/3)*m*l**2 / D], [0], [-m*l / D] ]) # LQR权重 Q np.diag([100, 10, 200, 20]) R np.array([[0.01]]) # 求解黎卡提方程 P solve_continuous_are(A, B, Q, R) # 计算增益矩阵K K np.linalg.inv(R) B.T P print(K , K)跑出来K是一个1x4的行向量四个元素分别对应位置、速度、角度、角速度的反馈增益。你可以观察一下通常角度对应的增益绝对值最大因为摆杆角度是最需要快速纠正的状态。3.4 位置跟踪给LQR加一个外环标准LQR是调节器它的目标是让所有状态回到零。但我们的需求是让小车跟踪一个位置指令比如从0米走到1米。这时候不能直接把LQR用在位置误差上因为LQR的反馈是 (u -KX)如果 (X) 里的位置是相对于原点的绝对位置那它只会把小车拉回原点。解决办法是引入参考输入。有两种常见做法一种是前馈加反馈另一种是状态增广。我用的是前馈加反馈结构简单物理意义清晰。具体来说把控制律改成[ u -K(X - X_{ref}) u_{ff} ]其中 (X_{ref} [x_{ref}, 0, 0, 0]^T) 是参考状态(u_{ff}) 是前馈力。对于倒立摆如果小车以恒定速度运动摆杆需要有一个小角度倾斜来提供加速度所以前馈力不能简单设为零。更实用的做法是让参考轨迹平滑变化然后用LQR跟踪误差状态。我在实际项目里的做法是给定一个位置指令 (x_{ref})先通过一个二阶低通滤波器生成平滑的 (x_{ref}(t)) 和 (\dot{x}_{ref}(t))然后构造误差状态[ e [x - x_{ref}, \dot{x} - \dot{x}_{ref}, \theta, \dot{\theta}]^T ]控制律为 (u -Ke)。这样LQR会自动处理跟踪过程中的耦合你不需要单独设计前馈项。滤波器的带宽决定了跟踪的快慢带宽越高跟踪越快但太高会激发摆杆的高频动态导致抖动。实操心得我一开始做位置跟踪时直接给阶跃指令结果小车猛地一冲摆杆差点倒了。后来加了二阶低通滤波器把指令的上升时间从0秒拉到0.5秒跟踪就平稳多了。这个滤波器相当于给参考轨迹做了“柔化”让LQR有足够的时间协调小车和摆杆的运动。4. 仿真实现与参数调试实录4.1 仿真框架搭建仿真用Python的scipy.integrate.solve_ivp做数值积分比手写欧拉法稳定。核心循环是这样的每个仿真步根据当前状态和参考轨迹计算控制力然后把控制力输入到非线性模型里积分得到下一时刻状态。注意这里积分用的是非线性模型不是线性化模型这样才能验证控制器在真实非线性系统上的表现。import numpy as np from scipy.integrate import solve_ivp import matplotlib.pyplot as plt def inverted_pendulum_dynamics(t, state, u, params): x, dx, theta, dtheta state M, m, l, g, b, c params # 非线性方程求解加速度 # 这里用矩阵形式解线性方程组得到ddx和ddtheta # 具体推导见2.2节 # ... return [dx, ddx, dtheta, ddtheta] def reference_trajectory(t, x_target, omega): # 二阶低通滤波生成的平滑参考 # ... return x_ref, dx_ref def simulate(): # 初始化状态、参数、时间 # 循环调用solve_ivp # 记录数据 # 绘图 pass仿真步长建议取0.001秒太小了计算慢太大了积分误差大。对于倒立摆这种动态较快的系统0.001秒是个稳妥的选择。4.2 调试过程中遇到的典型现象第一次跑仿真你可能会遇到几种情况我按出现频率排个序现象一摆杆直接倒下控制力完全不起作用。这通常是K的符号错了。检查一下你的状态定义和K的对应关系。比如你的角度定义是逆时针为正但K里角度增益是负的那控制器就会往错误方向推。解决办法是打印出K的每个元素对照物理意义检查符号。现象二摆杆能立住但小车一直往一个方向跑。这是位置环没起作用。检查Q矩阵里位置和速度的权重是不是太小了或者参考轨迹的生成有问题。我遇到过一种情况是参考轨迹的初始值和系统初始状态不一致导致一开始就有一个大的位置误差LQR拼命纠正反而把摆杆搞倒了。解决办法是让参考轨迹从当前状态平滑过渡。现象三系统高频抖动控制力在正负之间快速切换。这是增益过大或者仿真步长过大导致的。先检查仿真步长如果步长没问题那就是Q/R比值太大控制器太激进。适当增大R或者减小Q让控制力柔和一些。现象四跟踪稳态误差不为零。如果小车最终停在离目标位置差一点的地方可能是摩擦补偿不够。粘性摩擦在LQR里没有显式建模如果摩擦较大稳态时会有一个恒定的位置偏差。解决办法是在控制律里加一个积分项或者把摩擦系数估准一些。4.3 参数调试的实用流程我把自己的调试流程总结成下面这张表你可以照着走步骤目标操作观察指标1摆杆能立住Q只保留角度和角速度项R取0.01摆杆角度是否收敛到02小车不漂移逐步增加位置和速度权重小车位置是否发散3响应速度合适按比例放大Q保持R不变调节时间、超调量4跟踪平滑加入参考轨迹滤波器跟踪误差、控制力幅值5抗扰动在仿真中加脉冲扰动恢复时间、最大偏差每一步只调一个维度不要同时改多个参数否则你分不清是哪个参数起了作用。我见过有同学一次性把Q和R都改了结果系统震荡花了半天才找到原因。注意仿真里调好的参数搬到实物上通常需要再微调。实物的摩擦、传感器噪声、执行器延迟都会影响效果。我的经验是实物上的R要比仿真大20%到50%让控制力柔和一些给传感器噪声留余量。5. 常见问题与排查技巧实录5.1 建模阶段的坑问题线性化后模型和仿真对不上。如果你用线性化模型做仿真发现摆杆角度稍大一点就发散这是正常的因为线性化只在平衡点附近有效。一般来说角度在正负10度以内线性化误差可以接受超过20度非线性项的影响就很明显了。解决办法是仿真用非线性模型控制器设计用线性化模型这样既能保证控制器有解析解又能验证实际效果。问题摩擦项符号搞反。粘性摩擦的方向总是与运动方向相反所以方程里应该是 (-b\dot{x}) 和 (-c\dot{\theta})。如果你写成正号系统会变成负阻尼仿真里会看到振幅越来越大。这个错误很隐蔽因为符号错了系统也能跑只是行为不对。问题转动惯量算错。均匀杆绕质心的转动惯量是 (\frac{1}{12}mL^2)但绕铰链的转动惯量要用平行轴定理是 (\frac{1}{12}mL^2 m(L/2)^2 \frac{1}{3}mL^2)。我在2.2节里用的就是 (\frac{1}{3}mL^2)。如果你用的是质点模型那就是 (mL^2)。这两个值差了三倍对控制效果影响很大。5.2 LQR求解阶段的坑问题黎卡提方程无解。如果solve_continuous_are报错通常是因为系统不是可稳定的或者Q和R不满足正定条件。检查一下A和B是否配对正确Q是否半正定R是否正定。对于倒立摆只要模型正确黎卡提方程一定有解。问题K矩阵数值过大。如果K的元素达到几千几万说明Q/R比值太大了。这时候控制力会非常大仿真里可能直接发散。把Q整体缩小或者R放大让K回到合理范围。一般来说K的元素在个位数到几十之间比较正常。问题状态量纲不统一导致调参困难。前面说过位置和角度的量纲不同直接给相同权重不合理。我的做法是先把角度权重设为1位置权重设为0.01然后根据响应调整。你也可以对状态做归一化让每个状态的变化范围都在正负1之间这样权重就直观了。5.3 跟踪控制阶段的坑问题跟踪阶跃指令时摆杆倒了。阶跃指令意味着无穷大的加速度需求物理上不可能实现。解决办法是加参考轨迹滤波器把阶跃变成斜坡或者S形曲线。滤波器的自然频率建议取系统闭环带宽的1/3到1/5这样跟踪不会太慢也不会激发高频动态。问题跟踪过程中小车和摆杆的运动不同步。这通常是因为LQR只考虑了状态误差没有考虑参考轨迹的加速度。如果你跟踪的轨迹加速度较大可以在控制律里加一个前馈加速度项。具体来说根据参考加速度计算摆杆需要的平衡角度然后把这个角度作为前馈加到参考状态里。问题稳态时小车在目标位置附近来回晃。这是积分饱和或者摩擦补偿过度导致的。如果你加了积分项检查积分限幅是否合理如果没加积分项检查摩擦系数是否估得太大。我的经验是粘性摩擦系数宁可估小一点让LQR自己纠正也不要估大了导致振荡。5.4 常见问题速查表现象可能原因排查方法解决措施摆杆直接倒下K符号错误打印K对照物理意义修正状态定义或K符号小车单向漂移位置权重太小检查Q矩阵增大位置和速度权重高频抖动增益过大或步长过大减小Q/R比值检查步长增大R或减小Q步长取0.001s跟踪有稳态误差摩擦未补偿观察稳态时的控制力加积分项或估准摩擦跟踪时摆杆倒指令变化太快检查参考轨迹上升时间加低通滤波器仿真发散模型参数错误检查A、B矩阵特征值重新推导或核对参数6. 从仿真到实物的几个关键考量仿真跑通了下一步就是往实物上搬。这一步的坑比仿真多得多我挑几个最关键的说说。传感器噪声。仿真里状态是精确已知的实物上角度传感器和编码器都有噪声。噪声进入LQR的反馈通道会被K放大导致控制力抖动。解决办法有两个一是对传感器信号做低通滤波截止频率取系统带宽的2到3倍二是适当增大R降低控制器对噪声的敏感度。我通常两个都用先滤波再调R。执行器延迟。电机从收到指令到出力有一个延迟仿真里通常忽略。如果延迟接近系统的时间常数闭环性能会明显下降。解决办法是在仿真里加入延迟环节重新调参让控制器对延迟有一定容忍度。一般来说延迟小于闭环带宽的1/10影响可以忽略。摩擦非线性。粘性摩擦是线性的但实物上还有库仑摩擦和静摩擦。库仑摩擦在低速时会导致“爬行”现象小车一顿一顿地走。解决办法是在控制律里加摩擦前馈或者用高增益的积分项克服。但积分项太强又会引起振荡需要折中。参数不确定性。仿真里的质量、长度、摩擦系数都是估的实物上可能有偏差。LQR本身有一定的鲁棒性但如果偏差太大性能会下降。我的做法是在仿真里把参数上下调20%看控制器还能不能稳住。如果都能稳住那实物上问题不大。实操心得我第一次把LQR搬到实物上时仿真里调好的参数直接跑结果摆杆抖得厉害。后来发现是角度传感器的噪声比预想的大加了一个截止频率50Hz的低通滤波器再把R从0.01调到0.02就稳多了。这个经历告诉我仿真到实物之间永远要留调试余量。7. 后续可以继续折腾的方向这套东西跑通之后如果你想继续深入有几个方向可以扩展。一是把LQR换成MPCMPC能显式处理输入约束和状态约束对于倒立摆这种输入有限幅的系统更合适。二是加入扰动观测器估计并补偿风扰或者地面不平带来的干扰。三是做二级倒立摆状态量变成六个LQR的维数增加调参难度也上一个台阶但原理是一样的。我个人觉得倒立摆这个项目最大的价值不在于最后那个控制器跑得多好而在于你完整地走了一遍“建模-线性化-控制器设计-仿真-调试”的流程。这个流程在工业界的应用非常广泛从无人机姿态控制到机器人平衡控制底层逻辑都是相通的。你把倒立摆吃透了再去看那些复杂的系统会发现很多地方似曾相识。最后分享一个小技巧如果你在调参时不确定Q和R怎么选可以先随便给一组跑仿真看响应然后根据响应调整。比如超调大了就增大R响应慢了就增大Q。调个五六轮基本就能找到一组可用的参数。不要指望一次就调到最优控制器的参数是在反复迭代中磨出来的。