简介从经典控制理论出发线性二次型调节器LQR是处理线性系统最优状态反馈的常用工具但在欠驱动非线性系统中单纯依赖LQR无法完成大幅摆起。倒立摆作为自动控制领域的经典对象其自摆起过程需要先通过能量法将摆杆从下垂状态荡至竖直区域再切换至LQR实现稳定。这种“分段控制切换逻辑”的思想在机器人、航天器姿态控制等场景中具有普遍参考价值。本文围绕一阶倒立摆建模、能量反馈起摆策略、LQR平衡控制参数整定及实物调试常见问题展开并结合lqr一阶倒立摆与二阶倒立摆pid的对比帮助自动化方向学习者理解控制策略选择的工程逻辑。 做倒立摆自摆起项目最上头的瞬间不是它稳稳立在竖直位置的时候而是你按下启动按钮摆杆从垂直朝下开始荡秋千幅度越来越大最后“啪”地一下翻上去那一两秒的心态波动比调任何参数都剧烈。这个项目在自动控制领域属于经典欠驱动非线性控制问题核心就两句话让倒立摆自己从下垂位置起摆再用LQR控制把它稳定在倒立平衡点。很多学校实验课上只会做“手动扶起摆杆然后稳住”的版本但真正把自摆起整个流程跑通之后你才会理解什么是能量反馈、什么是局部线性化、什么是控制切换。这篇文章我会把整套方案从建模到仿真再到实物调试踩过的坑全部拆开讲适合自动化专业学生、准备竞赛的控制方向选手以及所有在PID之外想试试LQR的人。1. 自摆起到底在解决什么问题1.1 从“扶上去”到“自己站起来”传统倒立摆实验里大家都默认一个前提摆杆已经处于竖直向上的小角度附近系统只需要做“稳定”这件事。此时模型可以线性化LQR、极点配置、PID串级都能用。但实际工程里没有谁帮你扶着杆子系统上电之后摆杆就是垂在下面的。如果想让设备自动进入平衡状态就必须先解决“起摆”这个问题。自摆起Swing-Up的难点在于摆杆在大角度范围内运动时动力学是非线性的小车对摆杆的作用力方向还不断变化。你不能指望一个线性控制器在这个阶段输出一个恒定的大功率信号把它“顶”上去因为小车只能水平运动对摆杆只有通过摩擦力或者惯性力间接作用。这个过程更像是荡秋千通过小车的往复加速不断给摆杆注入能量让它的摆动幅度逐步增大直到接近竖直位置再切换到平衡控制器。所以自摆起解决的其实是一个“控制策略分段”的问题。起摆阶段用非线性或基于能量的方法平衡阶段用LQR或PID等线性方法。两者之间的切换设计往往比单独设计任何一个控制器都更容易翻车。1.2 为什么不能只靠LQR直接起摆LQR的全称是线性二次型调节器它确实是现代控制理论里非常漂亮的一个工具给定线性系统状态空间模型通过求解Riccati方程就能得到一组状态反馈增益。它的前提是“线性”。在倒立摆平衡点附近也就是角度为零附近系统可以近似为线性定常系统LQR可以给出有最优意义的控制效果。可一旦摆杆转角到了30度、60度、90度甚至超过180度接近朝下系统的非线性效应会越来越明显。你用平衡点附近线性化得到的模型去描述这么大范围的运动误差会大到控制器完全失灵。更关键的是欠驱动系统的状态在远离平衡点时需要的控制策略不是“把它压回去”而是“先让它动起来、加能量”。LQR的状态反馈结构反而会把摆杆往平衡点方向推导致它在中途就掉回来。所以自摆起和LQR稳定是两个阶段、两种逻辑不能混着用。这个认知一定要先建立起来。很多第一次接触这个项目的同学第一反应是“把LQR的K调大一点不就能把摆杆拉上去了吗”。我在仿真里试过结果是小车直接高速冲向导轨一端摆杆毫无起色然后限位报警。因为大角度下线性模型的输入到输出的增益方向都变了不是一个增益大小能解决的问题。1.3 一阶与二阶倒立摆控制难度完全不在一个量级网上搜索“lqr一阶倒立摆”和“二阶倒立摆pid”的热度都很高两个词其实代表了两条难度曲线。一阶倒立摆就是一个小车加一根摆杆这是自摆起项目最经典的载体。因为只有一个旋转关节能量法起摆非常有效LQR只需要设计一个4维状态反馈整个系统调试周期短适合入门。二阶倒立摆则需要在小车上串联两根摆杆更接近真实火箭或者双足机器人的动力学特征。二阶系统的欠驱动模态更多起摆难度大幅上升单纯的能量法需要扩展到多个能量模式而且控制通道少状态多LQR用起来需要更高阶的观测器。现实中很多竞赛题里的“二阶倒立摆pid”指的其实是平衡阶段用PID串级起摆阶段往往还是用能量或轨迹优化方法因为纯PID去直接起摆基本不可能。我的建议是如果你还没把小车的摩擦力、摆杆转动惯量、编码器噪声这些基础问题搞清楚不要一上来就挑战二阶。先把一阶自摆起LQR做到“每次上电都能稳定起来”再去扩展会轻松很多。2. 建模和起摆策略让摆杆荡起来2.1 建立一阶倒立摆的动力学模型设计控制器之前一定要有模型哪怕是很简化的模型。一阶倒立摆的常见物理参数包括小车质量M、摆杆质量m、摆杆长度l通常指质心到转轴的距离、摆杆绕转轴的转动惯量J、小车与导轨的摩擦系数b、摆杆转轴处的阻尼系数c。控制输入是作用在小车上的水平力F状态变量一般取小车位移x、小车速度x_dot、摆杆角度θ和摆杆角速度θ_dot。角度定义建议取竖直向上为0任意方向摆动的正负按右手定则。运动方程可以写成下面的形式我默认读者有基础简化了一点但公式必须能用(M m) * x_ddot b * x_dot m * l * θ_ddot * cos(θ) - m * l * θ_dot² * sin(θ) F(J m * l²) * θ_ddot c * θ_dot m * g * l * sin(θ) -m * l * x_ddot * cos(θ)这套方程看着复杂其实第一行描述小车受力第二行描述摆杆绕转轴转动。注意第二行里的小车加速度x_ddot是耦合项这也是小车能通过水平运动推动摆杆的原因。如果没有这一项小车再怎么动摆杆都不会受到影响。建模的时候有个容易踩的坑很多教材把摆杆质量简化成集中在末端的一个质点这样J就是m*l²公式会清爽很多但实物系统如果摆杆比较粗质点和实际转动惯量的误差会导致控制器增益偏大或偏小。建议用三维建模软件或者实测摆动周期把J校准一下。不过对于自摆起这种强反馈系统参数误差在一定范围内是能容忍的不需要精确到小数。2.2 能量法起摆的原理与控制律起摆的经典做法是能量法。它的思想特别直观把整个摆杆看作一个单摆我们希望它的能量从最低点附近的低能量状态增加到刚好能越过竖直点的高能量状态。定义摆杆的总机械能为E 0.5 * (J m*l²) * θ_dot² m*g*l*(cos(θ) - 1)注意这里的势能零点选在竖直向上位置。当摆杆竖直向上时θ0cosθ1势能是0当摆杆竖直向下时θπcosθ-1势能为-2mgl也就是最低势能。整个系统的目标能量是E_target0即摆杆拥有恰好能“翻上”竖直位置的能量。能量法控制律的形式很多一种常用的是F k_energy * (E - E_target) * sign(θ_dot) * cos(θ)或者更常用的是使系统能量误差收敛的反馈形式F k_energy * (E - E_target) * θ_dot * cos(θ)这两种表达都能用网上资料也各有偏好。核心逻辑是当摆杆能量低于目标时根据摆杆当前的角速度和角度来施加一个小车加速度让这个加速度的效果是“帮摆杆增加能量”当能量高于目标时则反向制动防止摆杆越过能量。就像荡秋千时你在最低点收缩身体提升能量在最高点伸展身体消耗多余能量。实际写控制代码时我更喜欢直接用比较简洁的版本F clamp(K1 * (E_target - E) * sign(θ_dot) -F_max F_max)注意这里我没有用cos(θ)项因为在小角度起摆的初期cosθ符号变化不大但到了接近竖直位置时如果不加限制可能会给反了。为了避免在起摆末段能量已经足够但角度还没到切换阈值时反向推我会再叠加一个位置阻尼项F clamp(K1 * ΔE * sign(θ_dot) - K2 * x_dot -F_max F_max)这个x_dot阻尼项非常重要。如果只做能量反馈小车的平均位置会慢慢漂移最后撞到导轨限位。加上位置阻尼后小车整体保持在导轨中间摆杆照样能荡起来。K1和K2都需要调试K1太大会导致能量过冲小车猛冲K2太大会把起摆的能量都吃掉起摆变慢。我一般的经验是先调K1让摆杆能在5到8个周期内明显越摆越高再加K2把位移漂移压住K2大概是K1的0.1到0.2倍。2.3 起摆到平衡的切换逻辑能量法能让摆杆摆动幅度逐渐接近竖直点但真正进入LQR稳定区域之前必须做切换。切换条件不能只看角度阈值我见过很多方案只写了“θ小于30度时切换到LQR”结果切换瞬间因为角速度太大LQR一上来就饱和直接把摆杆“打飞”。一个比较稳妥的切换条件是同时判断角度和角速度比如|θ| θ_switch且|θ_dot| ω_switch其中θ_switch通常取20到35度ω_switch取1到2 rad/s。切换前如果角速度还很大说明摆杆还在快速上升或下落这时候LQR即使稳住角度角速度反馈也会给出一个很大的控制量容易把小车推过头。更好的做法是当摆杆角度进入预设范围后先切换到一个“阻尼控制”只把角速度降下来等角速度也小了再交给LQR。这样可以避免硬切。还要考虑滞回。如果切换阈值设成一个固定值摆杆可能在临界点附近来回抖动控制器一会儿是能量法一会儿是LQR很容易振荡。我在实物中会把阈值设成两个能量法切换到LQR的条件是角度小于25度并且角速度小于1.5 rad/s如果LQR阶段角度又跑出35度则立刻切回能量法。这样形成一个滞回区间系统不会频繁切换。3. LQR控制器局部模型的“最优”状态反馈3.1 平衡点线性化与能控性当摆杆稳定在竖直向上附近时θ很小可以把sinθ约等于θcosθ约等于1高阶项忽略。代入之前建立的动力学方程整理后可以得到状态空间模型dx/dt A x B F其中状态向量x [x x_dot θ θ_dot]的转置A是4×4矩阵B是4×1向量。具体的表达式不同建模方法会略有差异但都可以通过符号计算得到。我建议你在MATLAB或Python的SymPy里把A、B矩阵符号化推一遍然后随手验证能控性矩阵的秩是否为4。一阶倒立摆在这个平衡点附近通常完全能控这也是LQR能用的前提。很多资料直接给出A、B数值甚至不给推导拿到代码就用。这种做法在标准参数下没问题但换了不同尺寸的摆杆后就会出错因为矩阵里的m、l、J变化会对增益产生明显影响。所以不管你从哪里拿到代码还是要自己推到一遍至少要把质量、长度填进去重新计算。求得A、B之后LQR的目标是找一个状态反馈控制律F -K * x使得二次型性能指标最小J ∫ (x^T * Q * x F^T * R * F) dt这个指标的意义很直观Q描述你对各个状态偏差的在意程度R描述你对控制量大小的在意程度。LQR会自动解出一个稳态最优控制律增益K就是Riccati方程的解。你不用自己去配极点只需要调Q和R上手门槛比极点配置低很多。3.2 Q、R矩阵怎么选才能少走弯路选Q、R是一个让新手非常头疼的事。我最早做的时候直接套网上的Q diag([100 10 1 1])R 1结果小车在平衡点附近剧烈抖动摆杆也不是特别稳。后来我才明白调Q、R是有方法论的不是靠玄学。比较实用的一个出发点是Bryson规则把每个状态量允许的最大偏差取倒数平方作为Q矩阵对角线元素把最大控制量取倒数平方作为R。比如你希望小车位移偏差不超过0.1米速度偏差不超过1 m/s角度偏差不超过0.1弧度角速度偏差不超过1 rad/s控制力不超过20牛顿。那么Q的对角线就是[1/0.01 1/1 1/0.01 1/1] [100 1 100 1]R就是1/400约等于0.0025。但Bryson规则给出来的权重只是一个起点实际还要根据效果微调。我后来采用的经验是先固定R让角度相关权重稍大保证摆杆能稳定再逐步加大位置权重让小车不要漂移。比如Q diag([10 1 100 5])R 0.1这组参数在一阶倒立摆上通常表现不错。角度权重q3比角度速度权重q4大是因为我们更关心摆杆是否竖直而不是它转得多快位置权重q1比速度权重q2大是为了让小车回到导轨中心。R也不能太小R太小意味着控制量不受限制反馈增益会很大系统对噪声特别敏感听上去是“最优”实际上抖得厉害。还有一个经验LQR增益计算出来后先看一下K的数值范围。如果K里角度反馈那一项超过几十甚至上百说明控制量会被角度偏差放大很多需要调大R或者调小Q中对应权重。实际工程里控制力或者电压都有上限不是算出一个K就能无限放大。3.3 仿真代码与关键参数示例为了验证整体方案我推荐先用Python写一个简单的仿真。这里给出一段可以跑通大致流程的伪代码框架重点看控制结构不要直接复制到硬件上。import numpy as np from scipy.integrate import solve_ivp # 物理参数 M 1.0 # 小车质量 kg m 0.1 # 摆杆质量 kg l 0.2 # 摆杆质心到转轴距离 m J 0.01 # 摆杆转动惯量 kg*m^2 g 9.81 b 0.1 # 小车摩擦 c 0.005 # 摆杆阻尼 def dynamics(t, state, F): x, xd, theta, thetad state # 这里用简化动力学方程展开求 xdd 和 thetadd # 具体推导略实际可以调用符号求逆 sinth np.sin(theta) costh np.cos(theta) Mmat np.array([[M m, m * l * costh], [m * l * costh, J m * l * l]]) forcing np.array([F - b * xd m * l * thetad**2 * sinth, -c * thetad - m * g * l * sinth]) xdd, thetadd np.linalg.solve(Mmat, forcing) return [xd, xdd, thetad, thetadd]仿真主循环就是每一时刻计算控制量。先判断当前状态是否满足切换条件不满足就用力学能量法满足就用LQR# LQR增益 K 由 lqr(A, B, Q, R) 计算得到 # K 是 1x4 的行向量 def controller(state): x, xd, theta, thetad state E_target 0.0 E_current 0.5 * (J m * l * l) * thetad**2 m * g * l * (np.cos(theta) - 1) theta_switch np.deg2rad(25) omega_switch 1.5 if abs(theta) theta_switch and abs(thetad) omega_switch: F -K state else: F K1 * (E_target - E_current) * np.sign(thetad) - K2 * xd F np.clip(F, -F_max, F_max) return F这里面K1、K2、F_max都要根据实际系统设定。仿真时可以先不管噪声把起摆过程整段画出来看角度曲线是否呈现“越荡越高”的趋势以及切换后角度是否收敛。如果仿真里都起不来那一定是公式或者控制逻辑写错了不要急着上硬件。4. 实物调试中的坑与排查技巧4.1 起摆不上去或小车撞限位这是最常遇到的问题。我看到很多人的第一版程序都是“起摆时小车来回移动但摆杆角度基本不变”。排查思路是这样先看能量误差是否在变化如果能量误差一直在减小证明控制器正在给摆杆注入能量只是幅度不够如果能量误差根本不怎么变多半是控制方向符号搞反了或者小车加速度太小摆杆根本来不及响应。小车撞限位的原因则多半是能量反馈里的位置阻尼不够。我在起摆控制律里特意加了一项-K2*xd但如果K2设得太小小车依然会漂移。你可以给起摆控制单独加一个位置外环比如在小车位移超过一定范围时强制让F叠加一个指向中心的比例力优先级最高。这样虽然会稍微干扰起摆但至少不会撞坏硬件。还有一个细节起摆过程中如果小车驱动力被限幅过多摆杆很难获得足够能量。建议仿真时把F_max设置成实际驱动器能输出的80%不要到100%留一点余量给LQR平衡阶段的瞬态需求。我见过有人把F_max设得非常大结果起摆阶段小车猛地一冲触发了限位开关整个装置直接停下来。4.2 切换瞬间摆杆直接掉落这个现象在实物调试里非常典型仿真各种完美一上实物切换瞬间就掉。原因主要有两类。第一类是切换条件里的角速度阈值太大切换时摆杆还在快速摆动LQR根本来不及把角速度抑制下来。第二类是状态估计有延迟你看到的θ角度是滤波后的实际角度可能已经偏出很远导致LQR给出的控制方向不对。我推荐在切换前单独做一个几十毫秒的“预稳定”阶段只保留角速度反馈把角速度降到阈值以下再切入完整的LQR。相当于在能量法和LQR之间加了一个缓冲。这个缓冲阶段控制律可以写成F -K_damp * θ_dot这种情况下小车会尽量抑制摆杆的角速度虽然不一定能把角度拉回零但能有效降低切换冲击。等角度和角速度都满足条件了再切入LQR成功率会高很多。注意切换时最好把控制量限幅放宽一点因为LQR刚接手时反馈量可能很大如果限幅太小控制器输出一直在饱和区相当于开环反而稳不住。我一般把切换瞬间的限幅设为正常稳定阶段限幅的1.5倍过了前20毫秒再恢复正常限幅。4.3 平衡区域总在抖如何判断是增益还是噪声如果你发现起摆成功了切换也成功了但摆杆在平衡点附近一直微微抖动甚至发出“嗡嗡”声就要分清楚是控制器增益太高还是传感器噪声导致的反馈抖动。最简单的判断方法是把传感器数据先做一次低通滤波比如一阶低通截止频率设在20到50赫兹看看抖动是否明显减小。如果减小了说明是传感器高频噪声被LQR的高增益放大。如果滤波后依然抖那就是Q、R参数太激进需要把R调大或者把Q中的角速度权重降低。还有一种容易被忽略的情况执行器本身有滞回或者死区尤其使用直流电机配H桥驱动时小控制量可能无法让电机真正转动。解决方法是加一个很小的死区补偿或者改用带速度环的驱动方案。在模型里可以把死区拟合成一个非线性函数仿真更容易复现。另外采样周期和控制器周期是否一致也要检查。如果传感器是100Hz控制周期却跑到了500Hz你相当于在两次有效反馈之间插入了很多次基于老数据的计算系统等效相位裕度会下降容易产生极限环振荡。我建议把控制频率统一比如都固定在1kHz宁可少做几次计算也不要用异步周期。4.4 PID和LQR到底怎么选热词里“lqr一阶倒立摆”和“二阶倒立摆pid”放在一起其实是很多人在选择控制方法时纠结的体现。对于一阶倒立摆的平衡阶段LQR确实比PID更省心。因为系统有四维状态用PID串级往往要调三个环参数多整定时间成倍增加LQR只需要定Q和R自动算出状态反馈增益而且天然考虑了所有状态的耦合。二阶倒立摆就不一样了状态维度更高LQR虽然也能算但实物实现时对状态估计要求很苛刻很多时候需要卡尔曼滤波或者全维观测器。一些人反而会用PID串级因为工程上PID的每个环节物理意义更直观检修容易。但起摆阶段两者都不是万能的二阶级联系统更依赖能量法或轨迹规划。我的个人观点是不要被“最优”两个字迷惑。LQR在数学模型意义下是最优的但在有噪声、限幅、摩擦和离散化误差的实物系统里这只是提供了一个好的初始点。你最终调出来的K往往和理论最优值有偏差。选PID还是LQR取决于你的时间成本和团队技术储备。如果你能熟练建模LQR是首选如果参数辨识困难、传感器质量差PID可能反而更容易调稳。写在最后的调试心得自摆起LQR这个项目最大的价值不是“把LQR公式背下来”而是让你明白控制系统的完整闭环建模、仿真、控制器设计、切换逻辑、实物验证、排障。每走一步都会暴露理论课本上没有的细节。我在第一次实物起摆成功时小车行程只剩不到10厘米差点撞限位但那一刻摆杆稳稳立住的画面还是让我激动了很久。如果你也在做类似项目我的建议是先把仿真里的切换阈值、限幅、增益都跑熟再上实物遇到起摆失败不要急着调K先用示波器或者串口把角度、角速度、能量误差、控制量四条曲线记录下来对着曲线找问题通常10分钟内就能定位。这套流程跑通后再往二阶倒立摆、旋转倒立摆扩展你会觉得思路清晰很多。本文还有配套的精品资源点击获取