简介非线性控制是机器人控制中的经典难题倒立摆自摆起则是理解非线性系统特性的极佳实例。与常见的线性平衡控制不同自摆起任务要求系统在没有外力助推的条件下通过小车往复运动为摆杆注入能量使其从自然下垂状态荡过180度进入竖直平衡区域。这一过程涉及能量反馈、状态切换、线性化建模等核心控制思想。能量控制法通过构造摆杆机械能与目标能量的差并利用方向项实现类秋千式节能激励克服了模型偏差与摩擦力影响成为工程上最稳健的起摆策略。当系统进入平衡点附近小角度范围后LQR控制器接管利用状态空间模型和二次型最优指标实现多变量协同镇定。基于MATLAB/Simulink与嵌入式平台的实践表明控制周期选择、切换平滑处理及参数辨识水平直接决定实物系统能否稳定运行。本文结合直线倒立摆与旋转倒立摆工程实践系统梳理自摆起控制器设计、参数整定及仿真到移植的关键环节为自动化课程设计及实验平台调试提供可复用的参考路径。 倒立摆自摆起说白了就是让一根倒立的杆子自己从“垂下”的状态“站起来”然后在竖直位置稳住。很多第一次接触倒立摆的人以为控制目标就是“别让杆子倒”把LQR调好就大功告成。但真正做过实物的人都知道最折磨人的根本不是平衡而是“起摆”——没有外力助推只靠小车来回移动产生的惯性让摆杆从自然下垂位置荡过180度、进入平衡点附近再交给LQR接管。整个过程把非线性控制、能量控制、状态切换、线性化建模全部串起来了可以说一个自摆起项目做下来本科自动控制原理里的知识能活一半。本文写给我的同行可能是正在做课程设计的自动化专业学生也可能是想把倒立摆实验平台跑通的工程师。我会结合自己做直线倒立摆和旋转倒立摆两套平台的实际经历把起摆控制器的设计思路、LQR平衡控制的参数整定、仿真到实物移植的细节以及我栽过的跟头全部摊开来讲。如果你也卡在“杆子摆不起来”或者“起来了但LQR接管瞬间就倒”这个阶段这篇文章应该能帮你少走不少弯路。1. 为什么“自摆起”比“平衡控制”更坑起摆问题的本质1.1 平衡控制只是“守城”自摆起才是“攻城”如果把倒立摆的整个控制任务分成两段前半段是“把杆子从下面送到上面”后半段是“在竖直位置附近稳住”。后半段的本质是线性控制问题只要摆杆在竖直位置附近小角度摆动sinθ就约等于θcosθ就约等于1原本的非线性微分方程可以线性化成状态空间模型LQR、极点配置、PID都能管用。但前半段完全不同。摆杆要从θπ竖直向下荡到θ0竖直向上角度变化接近180度整个过程强非线性线性化模型完全失效。更关键的是机器人能直接控制的只有小车的水平运动摆杆本身没有电机它只能通过小车加速/减速时产生的惯性力被“带动”起来。这个过程和荡秋千的原理几乎一模一样秋千本身没有动力人要靠身体重心变化给系统注入能量让摆幅越来越大最终达到能荡过最高点的程度。所以你看自摆起这个问题的核心矛盾在于“控制不了摆杆只能控制小车”和“要让摆杆获得足够大的能量”之间的矛盾。很多新手一上来就盯着角度试图让小车跟踪摆杆角度的变化曲线结果杆子越摆越乱小车反而一路冲到了导轨尽头。原因就是搞错了控制目标——起摆阶段要控制的不是角度而是能量。1.2 起摆的核心是能量反馈不是角度跟踪我在实验室第一次调起摆时想当然地写了一个PID控制器把摆杆角度当作参考输入小车位置当输出结果摆杆被小车拖着晃了两下然后慢慢停下来根本没有继续往上荡的趋势。后来查了文献才明白应该用能量反馈。倒立摆系统的机械能包含摆杆的动能和势能。自然下垂时势能最低竖直向上时势能最高两者之间相差 2mglm是摆杆质量l是摆杆质心到转轴的距离。目标能量就是竖直向上时的能量E_target mgl以转轴为零势能点竖直向上时势能取mgl动能为0。只要让系统当前能量向目标能量逼近摆杆就会逐渐获得足够的角速度冲过最高点。一个最常用的能量控制器长这样u sat( k_E * (E - E_target) * sign(θ_dot * cosθ) )其中u是小车加速度或者驱动力sat是饱和限幅函数。这个式子看着简单但里面的sign(θ_dot * cosθ) 是个非常关键的方向项。它的作用相当于荡秋千时的“节拍器”只有当摆杆角速度的方向和当前姿态配合起来小车加速能够增加摆杆能量的时候才朝这个方向注入力如果节拍反了作用就会变成“刹车”把好不容易积累的能量又抽回去。我见过有人把方向项去掉只用能量差乘以固定增益结果摆幅长到一定程度就上不去了半天都冲不过最高点。原因就是在某些阶段控制器在错误的时间点输出了错误的力的方向抵消了摆杆自己的动能。所以千万别小看这个sign项它决定了能量法能不能收敛。1.3 “大力出奇迹”为什么不可行行程限位与到达状态也有初学者觉得起摆不就是猛地推小车一下把杆子弹上去吗理论上如果电机推力无穷大确实可以直接把杆子从下垂位置“弹”到竖直位置。但实际系统里有两个残酷的限制。第一是电机力矩上限。直线倒立摆的小车电机通常只有几十瓦最大加速度受限根本不可能瞬间把杆子弹上去。第二是导轨行程有限。猛推小车一下小车会很快撞上限位开关而杆子可能才刚晃了一下。我最初调试时试过满占空比猛推结果是小车“哐”一声撞到导轨末端摆杆倒是弹起来一点但完全不可控接着就因为撞限位没了行程只能手动复位。更关键的是即使你真有这么大的力把杆子“弹”到竖直位置到达时的角速度也一定非常大。LQR平衡控制器能镇定的前提是状态在平衡点附近的小邻域内如果角速度太大超出了线性化模型的适用范围LQR根本稳不住。所以“自摆起”的三个字重点在“自”它讲究的是通过多次小幅蓄能、逐步提升摆幅把系统状态平滑送入平衡控制器的“捕获区”而不是一次性暴力到达。2. 起摆控制器选型能量法是最稳的入场券2.1 为什么不用轨迹规划模型偏差和摩擦力让人抓狂有人会问能不能用最优控制或轨迹规划离线算好一条从下垂到竖直的完美轨迹然后让小车跟踪这条轨迹把摆杆带到平衡点理论上确实有这类方法比如时间最优起摆、LQR规划轨迹等。但我在实物上试过的感受是理论很丰满实现很骨感。轨迹规划对模型的精确度要求太高了。你需要精确知道摆杆质量、质心位置、转动惯量、导轨摩擦力、电机时间常数……而这些东西在实际试验台上很难精确测量。摩擦力更是非线性且随温度、润滑状态变化。离线规划出来的轨迹在仿真里完美无缺一到实物上就是另一回事一点模型偏差跟踪误差就会累积轨迹一旦偏离控制器就得拼命纠偏最后出现振荡甚至发散。能量法的好处恰恰在于它对模型误差不敏感。它需要的只是摆杆能量表达式里那几个参数而且这几个参数即使偏差个20%控制器依然能工作只是收敛速度有变化。因为它本质上是一个带方向判断的标量反馈不像轨迹跟踪那样依赖精确的逐点状态匹配。2.2 能量反馈控制器的完整实现步骤下面给出一个可以照抄的实现流程基于直线倒立摆。第一步建立摆杆能量估计。计算当前摆杆相对于转轴的机械能E 0.5 * J * θ_dot² - m * g * l * cosθ其中J是摆杆绕转轴的转动惯量θ0为竖直向上位置θπ为自然下垂位置。这里我刻意省略了小车速度带来的牵连项因为对于绝大多数一阶倒立摆摆杆自身角速度对能量的贡献占主导牵连项影响很小省掉反而让控制器更简洁、更稳健。如果你做的是旋转倒立摆需要把旋转臂的角速度也考虑进去但核心逻辑一样。第二步计算能量差。目标能量是E_target m * g * l当摆杆正好竖直向上、且角速度为零时E E_target。第三步构造控制器并限幅u clamp( k_E * (E - E_target) * sign(θ_dot * cosθ), -u_max, u_max )u_max根据电机和导轨行程设定我建议先取一个比较保守的值比如小车加速度不超过2 m/s²然后逐步增大。第四步设置切换条件。当 |θ| 25°~30° 且 |θ_dot| 小于一定阈值比如0.5 rad/s时从能量控制器切换为LQR平衡控制器。这部分我会在第4章详细展开。2.3 起摆增益k_E的整定先慢后快注意位置漂移k_E这个增益既不能太小也不能太大。太小的结果摆杆的摆幅增长极慢可能好几十秒都冲不过最高点实验室里的耐心会被耗尽。太大的结果小车加速度频繁饱和不仅对电机冲击大还会导致摆杆在某个位置被过度激励摆幅忽大忽小甚至出现混沌感。我的经验是先给一个较小的k_E比如0.5观察摆杆能不能在5到10秒内把摆幅逐步增加起来。如果行再逐步增大到1.0、1.5直到找到“摆幅增加速度令人满意且不撞限位”的区间。对于我后来用的直线倒立摆k_E1.2左右就比较合适。另一个非常容易踩的坑是能量控制器只管摆杆能量不管小车位置。但在直线倒立摆上小车来回运动时间一长会慢慢漂移到导轨一端最后撞限位开关。所以一定要在能量控制器里叠加一个位置保护项最简单就是一个比例项u 能量控制项 - k_p * x其中x是小车偏离导轨中心的位置k_p取一个不大的值比如0.5~1.0只要保证小车不会持续漂向一端。这个保护项不能太大否则会干扰能量注入效果起摆变慢。理想情况是起摆过程中小车在导轨中段来回运动偏离中心不超过整个导轨行程的40%。3. 摆起来之后的接管LQR平衡控制在平衡点附近干的事3.1 为什么选LQR而不是继续用PID当摆杆被能量控制器送进平衡点附近后系统的“主角”就从能量控制器变成了LQR。有人可能觉得平衡控制用PID也能做确实很多低成本倒立摆玩具用PID也能稳住但它需要花大量时间去整定四个甚至更多的参数而且PID的结构是单输入单输出很难同时兼顾摆杆角度和小车位置。倒立摆在平衡点附近是一个四阶系统状态包括小车位置x、小车速度x_dot、摆杆角度θ、摆杆角速度θ_dot。这4个状态互相耦合你不可能用一两个PID环路完美处理。LQR的思路则完全不同它把4个状态放在同一个二次型性能指标里通过权重矩阵Q和R一次性算出最优状态反馈增益矩阵K。你不需要去思考“每个回路怎么独立调”只需要想清楚“我更在乎哪个状态”其余的都交给代数求解。这个特性在实际调试中非常省心。我在从PID转向LQR后最大的感受是LQR不需要一点点摸索比例、积分、微分的参数而是通过调整Q和R对角元的相对大小就能直观地改变系统行为。比如想让小车回中更快就加大x对应的权重想让杆子更硬朗就加大θ对应的权重。这种“直觉化调参”对新手极其友好。3.2 线性模型与状态方程参数辨识比公式推导向更重要LQR设计必须基于线性状态方程。状态向量取x [x, x_dot, θ, θ_dot]ᵀ输入u为作用在小车上的驱动力或电压换算后的力。用拉格朗日方程建模线性化后可以得到如下形式x_ddot ( -m * g * l / denom ) * θ ( (J m * l²) / denom ) * u θ_ddot ( (M m) * m * g * l / denom ) * θ - ( m * l / denom ) * u其中 denom J * (M m) M * m * l²。这里的M是小车质量m是摆杆质量l是摆杆质心到转轴距离J是摆杆绕转轴的转动惯量。如果你的平台是旋转倒立摆公式稍有不同核心是加入旋转臂的等效质量但流程一致。很多文章会直接把一组理论参数代入公式算出K然后上电调试。但我劝你别偷懒花十分钟做一下参数辨识。尤其是J不要直接用细杆模型公式0.333mL²来算因为实际的摆杆上往往固定了编码器码盘、配重块、连接件这些附加质量会让质心位置和惯量都发生变化。一个简单的辨识方法把摆杆当作物理摆让它自由摆动用秒表或编码器记录摆动周期T然后反算JT 2π * sqrt( J / (m * g * l) )也就是 J m * g * l * (T / 2π)²。这个方法比任何理论计算都准因为T可以直接测得。我在实际项目里遇到过理论计算的J比辨识结果偏小30%以上导致LQR增益偏大系统高频颤抖后来换成辨识值才稳定。3.3 Q、R矩阵怎么选先保守再慢慢收紧LQR的代价函数是J ∫(xᵀQx uᵀRu) dtQ是对角矩阵每个对角元代表你对对应状态的重视程度R是控制输入权重越大代表你越不希望控制作用过猛。Q和R的相对大小决定了控制器“该多用力”。我给出一个从零开始的整定建议。初始取Q diag([10, 1, 10, 1]) R 0.1这个组合是比较保守的控制输出不会太猛烈系统应该能稳住但响应偏慢。然后逐步增大θ的权重让杆子更硬。比如 Q diag([50, 0.5, 80, 0.1])R 0.02这组参数在我的直线倒立摆上可以获得很紧的平衡效果。注意增大Q对应状态权重会让控制增益K变大控制力度更猛但过猛会激发高频噪声甚至让系统不稳定。一个经验法则R越大系统越“软”抗扰性差但不易振荡R越小系统越“硬”抗扰性强但容易抖。我建议在实物上从较软的参数开始确保系统能稳定然后逐步减小R直到看到高频抖动再稍微回调一点。这个过程有点像调音乐均衡器找到那个“临界点”后平衡效果会非常漂亮。4. 从仿真到实物MATLAB/Simulink与嵌入式实现细节4.1 先用仿真验证完整状态机别只仿真LQR我见过很多人拿到倒立摆平台第一件事就是写LQR的仿真看到曲线收敛就很兴奋直接开始写嵌入式代码。结果上电后能量控制器和LQR之间的切换逻辑一塌糊涂杆子根本起不来。这里我强烈建议仿真阶段必须把“能量起摆-切换-LQR平衡”三个环节全部做进去而且要把切换条件、状态机逻辑、输出斜坡全部写进仿真模型。在Simulink里可以搭一个S函数来表达倒立摆动力学然后用Stateflow或者普通MATLAB Function写状态机。状态机至少包含两个状态SwingUp和Balance。当 |θ| 0.4 rad 且 |θ_dot| 0.5 rad/s 时从SwingUp切换到Balance如果切换后0.5秒内|θ|仍大于0.5 rad则切换回SwingUp重新起摆。仿真中的一个技巧故意把模型参数改成偏离设计值比如质量增加20%惯量增加15%看整体闭环是否还能工作。这个鲁棒性测试能提前暴露“切换边界过于敏感”的问题。我在仿真里发现过如果切换阈值取得太小比如5度那么在有模型偏差时摆杆很难进入切换区域但如果阈值取得太大比如60度LQR在大角度下根本镇不住。所以阈值应该取在20到30度之间这个区间比较中庸。4.2 传感器角度/角速度处理差分噪声和零漂都要处理实物系统的角度来源一般是编码器。编码器给出的是角度但控制器需要角速度。最简单的办法是差分θ_dot (θ_now - θ_prev) / T但这样做噪声很大直接输入LQR会让输出高频抖动。我的做法是第一步对编码器原始角度做一个一阶低通滤波截止频率大约50~80Hz。 第二步对滤波后的角度做中心差分(θ[k1] - θ[k-1]) / (2T)这样比一阶差分滞后小一些。 第三步对角速度再做一次滑动平均滤波窗口长度5~10个采样周期。这套组合在1ms控制周期下效果很好相位延迟大约几毫秒完全够用。如果你用的是MPU6050之类的陀螺仪来测角速度那还能获得更平滑的角速度信号但陀螺仪有零飘在长时间平衡时会让零点缓慢漂移所以要用互补滤波高频分量用陀螺仪角速度积分低频分量用编码器角度校准。常见形式angle 0.98 * (angle gyro_dt * gyro_rate) 0.02 * encoder_angle但我的经验是对于固定倒立摆实验台编码器差分低通已经足够稳定没必要给自己增加卡尔曼滤波的难度。卡尔曼滤波在低成本惯性导航里是真香但在这种高精度编码器系统里是多余的复杂度。4.3 控制周期选择1ms、500Hz、10ms的区别太大了控制周期是决定倒立摆能不能稳住的隐形因素。我刚开始用Arduino跑控制循环主循环里到处是delay和各种串口打印实际控制周期可能到了10ms~20ms杆子根本稳不住。后来把控制代码移到定时器中断里强制到5ms周期才勉强稳住但角度略大时还是会发抖。最后换到STM32F407用1ms定时器中断效果非常理想。倒立摆的自然频率通常在1~3Hz理论上采样频率只要大于奈奎斯特频率6Hz就行但实际控制需要的带宽远高于此。我的经验是控制频率至少是系统自然频率的20到50倍也就是对于1~3Hz的倒立摆控制周期应该在1ms上下。10ms周期意味着每个控制周期内在开环状态下摆杆已经转过好几个毫弧度对阻尼和抗扰都会有明显滞后。所以如果条件允许尽量上1ms周期。如果单片机性能不足把控制频率降到500Hz同时减少滤波阶数也比10ms好太多。4.4 切换逻辑输出不要突变给LQR一个“软启动”过程能量控制器和LQR控制器算出的控制量在切换瞬间往往差异巨大。能量控制器输出的力可能很大而LQR初始输出接近0如果直接切换小车加速度会产生一个阶跃这个冲击可能直接打断摆杆的动态让本来已经进入平衡区域的系统瞬间失控。解决方案是给输出加一个斜坡过渡让LQR的输出在0.1~0.2s内从0逐渐增长到正常值。具体实现u_balance_cmd 0; // 计算出的LQR控制量 u_smooth smooth_value; // 在前100ms内从0线性上升到1 u_final u_smooth * u_balance_cmd;当然能量控制输出也做一个类似的斜坡从1降到0但通常能量控制器输出在切换点已经接近0所以只需关注LQR侧。这个斜坡函数极其简单但能显著降低切换抖动。我的经验是一定要在仿真里验证这个过渡过程因为实物的冲击在仿真里看不见但一旦上电实测很容易发现切换瞬间杆子会“哆嗦”一下。5. 我在实际调车过程中踩过的坑和最终参数参考5.1 摆杆换向点判断错误导致摆不起来有一段时间我的起摆控制器始终让摆杆摆到某个角度就弹回去反复振荡完全无法冲过最高点。我一度以为是能量增益不够把k_E从1.0加到2.5没有任何改善。之后我打印了θ和θ_dot的波形手动推着摆杆观察能量变化才发现问题出在编码器零点方向与模型定义的方向相反。也就是说代码里的θ正方向和模型里定义的θ正方向差了180度。结果sign(θ_dot * cosθ) 这个方向项也一起反了控制器在应该注入能量的时候反而在抽走能量。这个问题在仿真里不可能出现因为仿真模型的方向完全一致只有到了硬件接线和机械装配阶段才会暴露。解决方法是在控制代码之前先写一个简单的开环测试把小车的力方向和编码器角度的正方向对应起来确认“小车向右移动时摆杆的角度变化方向是什么”然后在代码里统一。5.2 LQR切换瞬间的抖动问题起摆终于成功后新的问题来了每次从能量控制切换到LQR小车都会高频抖动一下有时候甚至把杆子抖倒。这个问题的根源我排查了很久最后发现有两个因素叠加。第一个因素是切换瞬间的阶跃输出。能量控制器的输出和LQR的输出在切换点没有对齐产生冲击。第二个因素是LQR增益里对小车位置的权重太大。因为起摆过程中小车往往偏离导轨中心切换时x的值可能很大LQR为了尽快把x拉回零点会输出一个很大的控制力这个力在杆子还没完全稳定时又会加剧角度振荡。我的解决办法一方面增加输出斜坡过渡另一方面把Q中x的权重从100降到50同时增大R从0.01到0.02让控制动作变平滑。调整之后切换过程顺滑了很多杆子基本不抖。5.3 一个能直接用的参考参数组合最后给出我在直线倒立摆平台上调通的一组完整参数供你作为起点参考。设备参数小车质量M1.2kg摆杆质量m0.12kg摆杆质心到转轴距离l0.18m摆杆惯量J0.0018kg·m²辨识值重力加速度g9.8m/s²。控制器参数能量法起摆增益k_E1.2位置保护增益k_p0.6加速度限幅u_max2.0m/s²切换条件为|θ|0.4rad且|θ_dot|0.3rad/s。LQR权重Qdiag([50, 0.5, 80, 0.1])R0.02。由MATLAB的lqr函数计算得到反馈增益大约为K [-22.4, -14.6, 63.3, 10.7]注意这套参数只对应我的特定机械平台。你换一台设备光是摩擦力和电机特性变化K就可能要整体缩放。正确的做法是把我的参数当成“中间值”然后在它附近小范围搜索而不是直接照抄。你可以用MATLAB的lqr函数批量生成几组不同Q/R下的K然后实测比较响应。故障现象可能原因处理建议摆幅长时间不增长能量控制器方向项符号反了检查编码器零点方向打印θ和θ_dot波形确认小车撞限位位置保护项缺失或太弱叠加位置比例项k_p限制小车漂移切换瞬间抖动LQR输出阶跃加输出斜坡过渡增大R减小控制器增益杆子在平衡点附近高频颤抖LQR增益过大或角速度噪声大增大R或对θ_dot加强滤波最后再分享一个我自己的调试习惯拿到新的倒立摆平台我一定先用手把摆杆扶到竖直位置让LQR单独接管确认它能稳定一分钟以上才敢去试自摆起。这个顺序能帮你把“平衡”和“起摆”两个子问题完全分开。如果LQR连平衡都稳不住起摆做得再漂亮也没用。等你把一个平台的这两个环节都跑顺了再换到旋转倒立摆、环形倒立摆或者二级倒立摆你会发现核心思路完全一样变的只是模型方程和参数辨识方法而已。本文还有配套的精品资源点击获取 倒立摆自摆起：从能量控制到LQR平衡的完整实现指南
简介非线性控制是机器人控制中的经典难题倒立摆自摆起则是理解非线性系统特性的极佳实例。与常见的线性平衡控制不同自摆起任务要求系统在没有外力助推的条件下通过小车往复运动为摆杆注入能量使其从自然下垂状态荡过180度进入竖直平衡区域。这一过程涉及能量反馈、状态切换、线性化建模等核心控制思想。能量控制法通过构造摆杆机械能与目标能量的差并利用方向项实现类秋千式节能激励克服了模型偏差与摩擦力影响成为工程上最稳健的起摆策略。当系统进入平衡点附近小角度范围后LQR控制器接管利用状态空间模型和二次型最优指标实现多变量协同镇定。基于MATLAB/Simulink与嵌入式平台的实践表明控制周期选择、切换平滑处理及参数辨识水平直接决定实物系统能否稳定运行。本文结合直线倒立摆与旋转倒立摆工程实践系统梳理自摆起控制器设计、参数整定及仿真到移植的关键环节为自动化课程设计及实验平台调试提供可复用的参考路径。 倒立摆自摆起说白了就是让一根倒立的杆子自己从“垂下”的状态“站起来”然后在竖直位置稳住。很多第一次接触倒立摆的人以为控制目标就是“别让杆子倒”把LQR调好就大功告成。但真正做过实物的人都知道最折磨人的根本不是平衡而是“起摆”——没有外力助推只靠小车来回移动产生的惯性让摆杆从自然下垂位置荡过180度、进入平衡点附近再交给LQR接管。整个过程把非线性控制、能量控制、状态切换、线性化建模全部串起来了可以说一个自摆起项目做下来本科自动控制原理里的知识能活一半。本文写给我的同行可能是正在做课程设计的自动化专业学生也可能是想把倒立摆实验平台跑通的工程师。我会结合自己做直线倒立摆和旋转倒立摆两套平台的实际经历把起摆控制器的设计思路、LQR平衡控制的参数整定、仿真到实物移植的细节以及我栽过的跟头全部摊开来讲。如果你也卡在“杆子摆不起来”或者“起来了但LQR接管瞬间就倒”这个阶段这篇文章应该能帮你少走不少弯路。1. 为什么“自摆起”比“平衡控制”更坑起摆问题的本质1.1 平衡控制只是“守城”自摆起才是“攻城”如果把倒立摆的整个控制任务分成两段前半段是“把杆子从下面送到上面”后半段是“在竖直位置附近稳住”。后半段的本质是线性控制问题只要摆杆在竖直位置附近小角度摆动sinθ就约等于θcosθ就约等于1原本的非线性微分方程可以线性化成状态空间模型LQR、极点配置、PID都能管用。但前半段完全不同。摆杆要从θπ竖直向下荡到θ0竖直向上角度变化接近180度整个过程强非线性线性化模型完全失效。更关键的是机器人能直接控制的只有小车的水平运动摆杆本身没有电机它只能通过小车加速/减速时产生的惯性力被“带动”起来。这个过程和荡秋千的原理几乎一模一样秋千本身没有动力人要靠身体重心变化给系统注入能量让摆幅越来越大最终达到能荡过最高点的程度。所以你看自摆起这个问题的核心矛盾在于“控制不了摆杆只能控制小车”和“要让摆杆获得足够大的能量”之间的矛盾。很多新手一上来就盯着角度试图让小车跟踪摆杆角度的变化曲线结果杆子越摆越乱小车反而一路冲到了导轨尽头。原因就是搞错了控制目标——起摆阶段要控制的不是角度而是能量。1.2 起摆的核心是能量反馈不是角度跟踪我在实验室第一次调起摆时想当然地写了一个PID控制器把摆杆角度当作参考输入小车位置当输出结果摆杆被小车拖着晃了两下然后慢慢停下来根本没有继续往上荡的趋势。后来查了文献才明白应该用能量反馈。倒立摆系统的机械能包含摆杆的动能和势能。自然下垂时势能最低竖直向上时势能最高两者之间相差 2mglm是摆杆质量l是摆杆质心到转轴的距离。目标能量就是竖直向上时的能量E_target mgl以转轴为零势能点竖直向上时势能取mgl动能为0。只要让系统当前能量向目标能量逼近摆杆就会逐渐获得足够的角速度冲过最高点。一个最常用的能量控制器长这样u sat( k_E * (E - E_target) * sign(θ_dot * cosθ) )其中u是小车加速度或者驱动力sat是饱和限幅函数。这个式子看着简单但里面的sign(θ_dot * cosθ) 是个非常关键的方向项。它的作用相当于荡秋千时的“节拍器”只有当摆杆角速度的方向和当前姿态配合起来小车加速能够增加摆杆能量的时候才朝这个方向注入力如果节拍反了作用就会变成“刹车”把好不容易积累的能量又抽回去。我见过有人把方向项去掉只用能量差乘以固定增益结果摆幅长到一定程度就上不去了半天都冲不过最高点。原因就是在某些阶段控制器在错误的时间点输出了错误的力的方向抵消了摆杆自己的动能。所以千万别小看这个sign项它决定了能量法能不能收敛。1.3 “大力出奇迹”为什么不可行行程限位与到达状态也有初学者觉得起摆不就是猛地推小车一下把杆子弹上去吗理论上如果电机推力无穷大确实可以直接把杆子从下垂位置“弹”到竖直位置。但实际系统里有两个残酷的限制。第一是电机力矩上限。直线倒立摆的小车电机通常只有几十瓦最大加速度受限根本不可能瞬间把杆子弹上去。第二是导轨行程有限。猛推小车一下小车会很快撞上限位开关而杆子可能才刚晃了一下。我最初调试时试过满占空比猛推结果是小车“哐”一声撞到导轨末端摆杆倒是弹起来一点但完全不可控接着就因为撞限位没了行程只能手动复位。更关键的是即使你真有这么大的力把杆子“弹”到竖直位置到达时的角速度也一定非常大。LQR平衡控制器能镇定的前提是状态在平衡点附近的小邻域内如果角速度太大超出了线性化模型的适用范围LQR根本稳不住。所以“自摆起”的三个字重点在“自”它讲究的是通过多次小幅蓄能、逐步提升摆幅把系统状态平滑送入平衡控制器的“捕获区”而不是一次性暴力到达。2. 起摆控制器选型能量法是最稳的入场券2.1 为什么不用轨迹规划模型偏差和摩擦力让人抓狂有人会问能不能用最优控制或轨迹规划离线算好一条从下垂到竖直的完美轨迹然后让小车跟踪这条轨迹把摆杆带到平衡点理论上确实有这类方法比如时间最优起摆、LQR规划轨迹等。但我在实物上试过的感受是理论很丰满实现很骨感。轨迹规划对模型的精确度要求太高了。你需要精确知道摆杆质量、质心位置、转动惯量、导轨摩擦力、电机时间常数……而这些东西在实际试验台上很难精确测量。摩擦力更是非线性且随温度、润滑状态变化。离线规划出来的轨迹在仿真里完美无缺一到实物上就是另一回事一点模型偏差跟踪误差就会累积轨迹一旦偏离控制器就得拼命纠偏最后出现振荡甚至发散。能量法的好处恰恰在于它对模型误差不敏感。它需要的只是摆杆能量表达式里那几个参数而且这几个参数即使偏差个20%控制器依然能工作只是收敛速度有变化。因为它本质上是一个带方向判断的标量反馈不像轨迹跟踪那样依赖精确的逐点状态匹配。2.2 能量反馈控制器的完整实现步骤下面给出一个可以照抄的实现流程基于直线倒立摆。第一步建立摆杆能量估计。计算当前摆杆相对于转轴的机械能E 0.5 * J * θ_dot² - m * g * l * cosθ其中J是摆杆绕转轴的转动惯量θ0为竖直向上位置θπ为自然下垂位置。这里我刻意省略了小车速度带来的牵连项因为对于绝大多数一阶倒立摆摆杆自身角速度对能量的贡献占主导牵连项影响很小省掉反而让控制器更简洁、更稳健。如果你做的是旋转倒立摆需要把旋转臂的角速度也考虑进去但核心逻辑一样。第二步计算能量差。目标能量是E_target m * g * l当摆杆正好竖直向上、且角速度为零时E E_target。第三步构造控制器并限幅u clamp( k_E * (E - E_target) * sign(θ_dot * cosθ), -u_max, u_max )u_max根据电机和导轨行程设定我建议先取一个比较保守的值比如小车加速度不超过2 m/s²然后逐步增大。第四步设置切换条件。当 |θ| 25°~30° 且 |θ_dot| 小于一定阈值比如0.5 rad/s时从能量控制器切换为LQR平衡控制器。这部分我会在第4章详细展开。2.3 起摆增益k_E的整定先慢后快注意位置漂移k_E这个增益既不能太小也不能太大。太小的结果摆杆的摆幅增长极慢可能好几十秒都冲不过最高点实验室里的耐心会被耗尽。太大的结果小车加速度频繁饱和不仅对电机冲击大还会导致摆杆在某个位置被过度激励摆幅忽大忽小甚至出现混沌感。我的经验是先给一个较小的k_E比如0.5观察摆杆能不能在5到10秒内把摆幅逐步增加起来。如果行再逐步增大到1.0、1.5直到找到“摆幅增加速度令人满意且不撞限位”的区间。对于我后来用的直线倒立摆k_E1.2左右就比较合适。另一个非常容易踩的坑是能量控制器只管摆杆能量不管小车位置。但在直线倒立摆上小车来回运动时间一长会慢慢漂移到导轨一端最后撞限位开关。所以一定要在能量控制器里叠加一个位置保护项最简单就是一个比例项u 能量控制项 - k_p * x其中x是小车偏离导轨中心的位置k_p取一个不大的值比如0.5~1.0只要保证小车不会持续漂向一端。这个保护项不能太大否则会干扰能量注入效果起摆变慢。理想情况是起摆过程中小车在导轨中段来回运动偏离中心不超过整个导轨行程的40%。3. 摆起来之后的接管LQR平衡控制在平衡点附近干的事3.1 为什么选LQR而不是继续用PID当摆杆被能量控制器送进平衡点附近后系统的“主角”就从能量控制器变成了LQR。有人可能觉得平衡控制用PID也能做确实很多低成本倒立摆玩具用PID也能稳住但它需要花大量时间去整定四个甚至更多的参数而且PID的结构是单输入单输出很难同时兼顾摆杆角度和小车位置。倒立摆在平衡点附近是一个四阶系统状态包括小车位置x、小车速度x_dot、摆杆角度θ、摆杆角速度θ_dot。这4个状态互相耦合你不可能用一两个PID环路完美处理。LQR的思路则完全不同它把4个状态放在同一个二次型性能指标里通过权重矩阵Q和R一次性算出最优状态反馈增益矩阵K。你不需要去思考“每个回路怎么独立调”只需要想清楚“我更在乎哪个状态”其余的都交给代数求解。这个特性在实际调试中非常省心。我在从PID转向LQR后最大的感受是LQR不需要一点点摸索比例、积分、微分的参数而是通过调整Q和R对角元的相对大小就能直观地改变系统行为。比如想让小车回中更快就加大x对应的权重想让杆子更硬朗就加大θ对应的权重。这种“直觉化调参”对新手极其友好。3.2 线性模型与状态方程参数辨识比公式推导向更重要LQR设计必须基于线性状态方程。状态向量取x [x, x_dot, θ, θ_dot]ᵀ输入u为作用在小车上的驱动力或电压换算后的力。用拉格朗日方程建模线性化后可以得到如下形式x_ddot ( -m * g * l / denom ) * θ ( (J m * l²) / denom ) * u θ_ddot ( (M m) * m * g * l / denom ) * θ - ( m * l / denom ) * u其中 denom J * (M m) M * m * l²。这里的M是小车质量m是摆杆质量l是摆杆质心到转轴距离J是摆杆绕转轴的转动惯量。如果你的平台是旋转倒立摆公式稍有不同核心是加入旋转臂的等效质量但流程一致。很多文章会直接把一组理论参数代入公式算出K然后上电调试。但我劝你别偷懒花十分钟做一下参数辨识。尤其是J不要直接用细杆模型公式0.333mL²来算因为实际的摆杆上往往固定了编码器码盘、配重块、连接件这些附加质量会让质心位置和惯量都发生变化。一个简单的辨识方法把摆杆当作物理摆让它自由摆动用秒表或编码器记录摆动周期T然后反算JT 2π * sqrt( J / (m * g * l) )也就是 J m * g * l * (T / 2π)²。这个方法比任何理论计算都准因为T可以直接测得。我在实际项目里遇到过理论计算的J比辨识结果偏小30%以上导致LQR增益偏大系统高频颤抖后来换成辨识值才稳定。3.3 Q、R矩阵怎么选先保守再慢慢收紧LQR的代价函数是J ∫(xᵀQx uᵀRu) dtQ是对角矩阵每个对角元代表你对对应状态的重视程度R是控制输入权重越大代表你越不希望控制作用过猛。Q和R的相对大小决定了控制器“该多用力”。我给出一个从零开始的整定建议。初始取Q diag([10, 1, 10, 1]) R 0.1这个组合是比较保守的控制输出不会太猛烈系统应该能稳住但响应偏慢。然后逐步增大θ的权重让杆子更硬。比如 Q diag([50, 0.5, 80, 0.1])R 0.02这组参数在我的直线倒立摆上可以获得很紧的平衡效果。注意增大Q对应状态权重会让控制增益K变大控制力度更猛但过猛会激发高频噪声甚至让系统不稳定。一个经验法则R越大系统越“软”抗扰性差但不易振荡R越小系统越“硬”抗扰性强但容易抖。我建议在实物上从较软的参数开始确保系统能稳定然后逐步减小R直到看到高频抖动再稍微回调一点。这个过程有点像调音乐均衡器找到那个“临界点”后平衡效果会非常漂亮。4. 从仿真到实物MATLAB/Simulink与嵌入式实现细节4.1 先用仿真验证完整状态机别只仿真LQR我见过很多人拿到倒立摆平台第一件事就是写LQR的仿真看到曲线收敛就很兴奋直接开始写嵌入式代码。结果上电后能量控制器和LQR之间的切换逻辑一塌糊涂杆子根本起不来。这里我强烈建议仿真阶段必须把“能量起摆-切换-LQR平衡”三个环节全部做进去而且要把切换条件、状态机逻辑、输出斜坡全部写进仿真模型。在Simulink里可以搭一个S函数来表达倒立摆动力学然后用Stateflow或者普通MATLAB Function写状态机。状态机至少包含两个状态SwingUp和Balance。当 |θ| 0.4 rad 且 |θ_dot| 0.5 rad/s 时从SwingUp切换到Balance如果切换后0.5秒内|θ|仍大于0.5 rad则切换回SwingUp重新起摆。仿真中的一个技巧故意把模型参数改成偏离设计值比如质量增加20%惯量增加15%看整体闭环是否还能工作。这个鲁棒性测试能提前暴露“切换边界过于敏感”的问题。我在仿真里发现过如果切换阈值取得太小比如5度那么在有模型偏差时摆杆很难进入切换区域但如果阈值取得太大比如60度LQR在大角度下根本镇不住。所以阈值应该取在20到30度之间这个区间比较中庸。4.2 传感器角度/角速度处理差分噪声和零漂都要处理实物系统的角度来源一般是编码器。编码器给出的是角度但控制器需要角速度。最简单的办法是差分θ_dot (θ_now - θ_prev) / T但这样做噪声很大直接输入LQR会让输出高频抖动。我的做法是第一步对编码器原始角度做一个一阶低通滤波截止频率大约50~80Hz。 第二步对滤波后的角度做中心差分(θ[k1] - θ[k-1]) / (2T)这样比一阶差分滞后小一些。 第三步对角速度再做一次滑动平均滤波窗口长度5~10个采样周期。这套组合在1ms控制周期下效果很好相位延迟大约几毫秒完全够用。如果你用的是MPU6050之类的陀螺仪来测角速度那还能获得更平滑的角速度信号但陀螺仪有零飘在长时间平衡时会让零点缓慢漂移所以要用互补滤波高频分量用陀螺仪角速度积分低频分量用编码器角度校准。常见形式angle 0.98 * (angle gyro_dt * gyro_rate) 0.02 * encoder_angle但我的经验是对于固定倒立摆实验台编码器差分低通已经足够稳定没必要给自己增加卡尔曼滤波的难度。卡尔曼滤波在低成本惯性导航里是真香但在这种高精度编码器系统里是多余的复杂度。4.3 控制周期选择1ms、500Hz、10ms的区别太大了控制周期是决定倒立摆能不能稳住的隐形因素。我刚开始用Arduino跑控制循环主循环里到处是delay和各种串口打印实际控制周期可能到了10ms~20ms杆子根本稳不住。后来把控制代码移到定时器中断里强制到5ms周期才勉强稳住但角度略大时还是会发抖。最后换到STM32F407用1ms定时器中断效果非常理想。倒立摆的自然频率通常在1~3Hz理论上采样频率只要大于奈奎斯特频率6Hz就行但实际控制需要的带宽远高于此。我的经验是控制频率至少是系统自然频率的20到50倍也就是对于1~3Hz的倒立摆控制周期应该在1ms上下。10ms周期意味着每个控制周期内在开环状态下摆杆已经转过好几个毫弧度对阻尼和抗扰都会有明显滞后。所以如果条件允许尽量上1ms周期。如果单片机性能不足把控制频率降到500Hz同时减少滤波阶数也比10ms好太多。4.4 切换逻辑输出不要突变给LQR一个“软启动”过程能量控制器和LQR控制器算出的控制量在切换瞬间往往差异巨大。能量控制器输出的力可能很大而LQR初始输出接近0如果直接切换小车加速度会产生一个阶跃这个冲击可能直接打断摆杆的动态让本来已经进入平衡区域的系统瞬间失控。解决方案是给输出加一个斜坡过渡让LQR的输出在0.1~0.2s内从0逐渐增长到正常值。具体实现u_balance_cmd 0; // 计算出的LQR控制量 u_smooth smooth_value; // 在前100ms内从0线性上升到1 u_final u_smooth * u_balance_cmd;当然能量控制输出也做一个类似的斜坡从1降到0但通常能量控制器输出在切换点已经接近0所以只需关注LQR侧。这个斜坡函数极其简单但能显著降低切换抖动。我的经验是一定要在仿真里验证这个过渡过程因为实物的冲击在仿真里看不见但一旦上电实测很容易发现切换瞬间杆子会“哆嗦”一下。5. 我在实际调车过程中踩过的坑和最终参数参考5.1 摆杆换向点判断错误导致摆不起来有一段时间我的起摆控制器始终让摆杆摆到某个角度就弹回去反复振荡完全无法冲过最高点。我一度以为是能量增益不够把k_E从1.0加到2.5没有任何改善。之后我打印了θ和θ_dot的波形手动推着摆杆观察能量变化才发现问题出在编码器零点方向与模型定义的方向相反。也就是说代码里的θ正方向和模型里定义的θ正方向差了180度。结果sign(θ_dot * cosθ) 这个方向项也一起反了控制器在应该注入能量的时候反而在抽走能量。这个问题在仿真里不可能出现因为仿真模型的方向完全一致只有到了硬件接线和机械装配阶段才会暴露。解决方法是在控制代码之前先写一个简单的开环测试把小车的力方向和编码器角度的正方向对应起来确认“小车向右移动时摆杆的角度变化方向是什么”然后在代码里统一。5.2 LQR切换瞬间的抖动问题起摆终于成功后新的问题来了每次从能量控制切换到LQR小车都会高频抖动一下有时候甚至把杆子抖倒。这个问题的根源我排查了很久最后发现有两个因素叠加。第一个因素是切换瞬间的阶跃输出。能量控制器的输出和LQR的输出在切换点没有对齐产生冲击。第二个因素是LQR增益里对小车位置的权重太大。因为起摆过程中小车往往偏离导轨中心切换时x的值可能很大LQR为了尽快把x拉回零点会输出一个很大的控制力这个力在杆子还没完全稳定时又会加剧角度振荡。我的解决办法一方面增加输出斜坡过渡另一方面把Q中x的权重从100降到50同时增大R从0.01到0.02让控制动作变平滑。调整之后切换过程顺滑了很多杆子基本不抖。5.3 一个能直接用的参考参数组合最后给出我在直线倒立摆平台上调通的一组完整参数供你作为起点参考。设备参数小车质量M1.2kg摆杆质量m0.12kg摆杆质心到转轴距离l0.18m摆杆惯量J0.0018kg·m²辨识值重力加速度g9.8m/s²。控制器参数能量法起摆增益k_E1.2位置保护增益k_p0.6加速度限幅u_max2.0m/s²切换条件为|θ|0.4rad且|θ_dot|0.3rad/s。LQR权重Qdiag([50, 0.5, 80, 0.1])R0.02。由MATLAB的lqr函数计算得到反馈增益大约为K [-22.4, -14.6, 63.3, 10.7]注意这套参数只对应我的特定机械平台。你换一台设备光是摩擦力和电机特性变化K就可能要整体缩放。正确的做法是把我的参数当成“中间值”然后在它附近小范围搜索而不是直接照抄。你可以用MATLAB的lqr函数批量生成几组不同Q/R下的K然后实测比较响应。故障现象可能原因处理建议摆幅长时间不增长能量控制器方向项符号反了检查编码器零点方向打印θ和θ_dot波形确认小车撞限位位置保护项缺失或太弱叠加位置比例项k_p限制小车漂移切换瞬间抖动LQR输出阶跃加输出斜坡过渡增大R减小控制器增益杆子在平衡点附近高频颤抖LQR增益过大或角速度噪声大增大R或对θ_dot加强滤波最后再分享一个我自己的调试习惯拿到新的倒立摆平台我一定先用手把摆杆扶到竖直位置让LQR单独接管确认它能稳定一分钟以上才敢去试自摆起。这个顺序能帮你把“平衡”和“起摆”两个子问题完全分开。如果LQR连平衡都稳不住起摆做得再漂亮也没用。等你把一个平台的这两个环节都跑顺了再换到旋转倒立摆、环形倒立摆或者二级倒立摆你会发现核心思路完全一样变的只是模型方程和参数辨识方法而已。本文还有配套的精品资源点击获取