1. 项目概览为什么非要做自起摆直线一级倒立摆算是控制领域最经典的入门级硬件了大学实验室里几乎人手一套。可千万别小看它这个看似简单的装置串联了建模、状态估计、非线性控制、实时系统一大堆知识点。我最早接触它是在做课程设计那会儿当时的任务和大多数人一样——小车能稳定平衡就行。但真正上手之后才发现最有挑战性的不是“稳住”而是“怎么让它站起来”。理论上说倒立摆的稳态控制已经非常成熟线性二次型调节器LQR一上调好权重几秒钟就能让摆杆稳定在竖直位置。但问题是摆杆在竖直向下的自然状态时你没法直接让它倒立。电机功率有限轨道长度有限如何利用有限的行程把摆杆从下垂位置“甩”起来在合适的时间和位置进入稳态控制范围就是自起摆要解决的问题。这篇文章我会从系统建模开始讲重点拆解两段控制逻辑能量注入式的自起摆设计以及LQR稳态平衡的实现。中间会穿插我实际调试时遇到过的坑和总结出来的经验包括参数整定、限幅处理、状态切换等细节。文中所用的参数基于我手上的直线导轨小车倒立摆平台摆杆长度约30cm小车质量约1kg理解思路之后你完全可以迁移到自己的硬件上。这套方案的价值在于它不依赖高精度的模型参数起摆阶段只要求“摆起来就行”稳态阶段通过LQR保证鲁棒性。整体实现难度适中适合有自动控制理论基础、想动手做完整闭环的读者参考。2. 系统建模把物理装置翻译成数学语言2.1 拉格朗日方程建模过程倒立摆的核心是一个经典的两体系统小车在水平导轨上运动摆杆通过转轴固定在小车上可以在竖直平面内自由转动。先明确我用的坐标定义x为小车水平位移θ为摆杆与竖直向上方向的夹角。**θ0表示摆杆竖直向上目标平衡位置θπ表示摆杆自然下垂。**这个定义很多教材不统一如果看别人代码时符号对不上多半就是初始角度的定义问题。建模用拉格朗日方程最清晰不需要拆解复杂的牛顿力分析。系统的动能包括小车的平动动能1/2·M·ẋ²摆杆质心的平动动能1/2·m·(vx² vy²)摆杆绕质心的转动动能1/2·J·θ̇²其中摆杆质心位置为(x l·sinθ, l·cosθ)注意这里l是质心到转轴的距离如果是均质摆杆则l L/2。联立动能减去势能得到拉格朗日量代入拉格朗日方程整理后会得到两个耦合的非线性微分方程。完整推导过程教科书里都有我直接给实际控制时用的关键结果。小车运动方程 (M m)·ẍ m·l·θ̈·cosθ - m·l·θ̇²·sinθ F摆杆转动方程 m·l·ẍ·cosθ (J m·l²)·θ̈ - m·g·l·sinθ 0这里的F是电机施加在小车上的力。注意方程里有一项m·l·θ̇²·sinθ这是摆杆旋转带来的离心力项。很多人建模完之后忽略这一项在摆动幅度大或者摆速快的时候就会明显感觉到模型和实际行为对不上。2.2 平衡点线性化与状态空间方程非线性方程没法直接设计线性控制器但好在我们的控制目标就是让摆杆稳定在θ0附近的小邻域。在平衡点附近做小角度近似sinθ≈θcosθ≈1θ̇²项视为高阶小量可以舍弃。把这两个方程联立消元整理成标准的状态空间形式。取状态向量X [x, ẋ, θ, θ̇]ᵀ输入u F输出我取全部四个状态方便观测。线性化后的状态矩阵A和输入矩阵B如下以我实际系统的参数带入A [0, 1, 0, 0; 0, 0, -m·g/M, 0; 0, 0, 0, 1; 0, 0, (Mm)·g/(M·l), 0]B [0; 1/M; 0; -1/(M·l)]如果摆杆是均质杆转动惯量J (1/3)·m·L²质心到转轴距离l L/2。代入这些之后A矩阵第四行第三列的值约等于(Mm)·g/(M·l)这个值是系统的关键特征参数直接决定了系统的自然频率和所需的控制增益量级。**一个非常重要的检查点**A矩阵特征值里必然有一个正实部特征值——这是倒立摆的“内在不稳定性”来源表明系统就是开环不稳定的。如果在仿真里发现特征值全部在左半平面那一定是你符号定义或者参数代入出错了。2.3 模型参数获取与实测校准建模不是写几个公式就完了参数不准再好的控制器也白搭。我的做法分成三步第一直接测量法。小车质量M用电子秤称摆杆质量m同样称重。摆杆长度L用游标卡尺量质心位置l对均质杆就是L/2。但实际买来的摆杆可能一端有配重块或者传感器质心位置要重新估算简单的方法是把摆杆水平放置、用棱柱支撑找平衡点那个点就是质心。第二摩擦力辨识。导轨和转轴的摩擦力模型其实相当重要尤其是低速段。我实测发现静摩擦和动摩擦差别很大简单线性摩擦模型不够用。更准确的方法是给小车一个恒定力输入测稳态速度改变输入力大小记录多组数据拟合出摩擦系数。实际操作中如果不做摩擦补偿会出现稳态误差——摆杆看似平衡了但小车会缓慢漂移一直撞击限位开关。第三在仿真里验证模型行为。把实际参数代入非线性模型对摆杆做个自由摆锤仿真对比实测摆动周期的衰减曲线。如果衰减速率和频率都对得上说明模型基本可用。这一步别省后面用能量法设计起摆时模型的准确性直接影响起摆成功率。3. 稳态控制与自起摆的完整方案设计3.1 控制策略选型为什么是LQR加能量法倒立摆稳态控制可以从多种方法中选方案经典PID、极点配置、LQR、滑模控制、自抗扰控制等。我最终选择LQR加能量注入式的起摆策略核心原因有三第一LQR在设计流程上非常清晰物理意义明确。四个状态权重矩阵Q的取值直接对应“我多在乎某个状态的偏差”不需要像PID那样三个参数对着响应曲线反复试探。尤其对于多输入多输出系统LQR能天然处理状态间的耦合关系。第二从起摆到稳态的切换非常自然。起摆阶段我关注的是“总能量”稳态阶段关注的是“状态偏差”两者可以通过一个统一的能量/角度切换条件衔接逻辑清晰。第三LQR的鲁棒性在工程上足够用。尽管我们用的是线性化模型但LQR本身具有一定幅值和相位裕度对模型参数误差、摩擦力等未建模动态有一定容忍度。实测中我用名义参数设计的增益换到不同摆杆上依然能稳定只是起摆和平衡的响应速度略有差异。3.2 自起摆原理能量从一个势能谷爬向势能峰理解自起摆之前先想明白一个物理事实摆杆在竖直下垂位置时系统处于势能最低点。要让摆杆到达竖直向上的倒立位置需要注入足够的能量让摆杆能翻越水平位置的“势能山坡”。起摆过程本质上是一个能量控制问题。理想目标不是精确控制某个角度轨迹而是单调增加摆杆的机械能让摆杆摆幅逐步增大。摆杆能量表达式为E 1/2·(J m·l²)·θ̇² m·g·l·(cosθ - 1)注意这里我把势能零点定义在竖直向上位置θ0所以目标平衡状态对应的能量是E 0而自然下垂状态θπ的能量是E -2·m·g·l。起摆的全部意义就是让总能量从负值逐步逼近零。那么小车运动如何影响摆杆能量对摆杆方程两端乘以θ̇并积分可以得到能量变化率dE/dt -m·l·ẍ·θ̇·cosθ这个式子非常关键。它告诉我们**小车加速度带来的惯性力通过摆杆等效长度做功从而注入能量。**控制策略非常简单粗暴当摆杆向下摆动时θ̇·cosθ的符号为某一方向让小车产生恰当的加速度给摆杆“推一把”当摆杆向上摆动时不要再注入能量甚至可以制动一下防止能量过冲。这样每次摆动都叠加一点能量摆幅不断变大。3.3 起摆控制律设计与限幅策略实际使用的起摆控制律我采用了一个简化的能量反馈加摆动同步项u_swing k_E·(E0 - E)·sign(θ̇·cosθ) k_d·ẋ这里E0是目标能量接近0但略小于0比如-0.05Jsign函数的作用是让力的方向始终和摆杆运动的“可注入能量方向”一致。第二项k_d·ẋ是速度阻尼项防止小车在有限导轨上越冲越远。第一版实现我踩过一个大坑如果不加速度阻尼小车会为了给摆杆注入能量而越来越快地满导轨乱跑经常撞到限位。后来我把小车速度反馈的权重调大起摆过程才可控。经验值k_E取0.5~1.5之间k_d取2~5具体数值和摆杆质量相关需要实测调整。此外还有两个防呆限幅控制量u限制在±15N以内对应电机PWM限幅约±70%小车位置x限制在±0.38m导轨有效行程的一半再加一点安全余量如果小车距离限位小于0.05m强制切换为“先回中”的控制模式起摆结束条件我用的是能量阈值加角度阈值双重判断当估计能量E -0.05J且摆杆角度连续50ms落在±20°范围内立即切换为LQR稳态控制。注意切换瞬间有控制量的阶跃跳变建议做一阶低通滤波过渡避免电机电流冲击。3.4 LQR稳态控制器的权重设计与效果预期稳态控制的核心是设计状态反馈控制器u -K·X通过Riccati方程求解最优增益矩阵K。维度上我的Q矩阵是4×4对角阵Q diag(q_x, q_v, q_theta, q_omega)实际调参过程中我发现对控制效果影响最大的比例关系是q_theta和q_x的相对大小。如果q_theta远大于q_x控制器会优先保证摆杆垂直但小车可能缓慢漂移反过来则小车稳如磐石但摆杆抗扰能力变差。我最终用的权重组合Q diag(500, 10, 200, 5)R 1对应的LQR增益大约是K [-22.36, -31.62, 38.73, 5.19]具体数值和系统参数强相关不建议直接照抄但可以作为调参的初始参考。有人可能会问为什么Q里x的权重比θ大那么多却还是优先稳摆杆因为状态量的物理单位不同x以米为单位本身数值就小θ以弧度为单位摆个5°就是0.087rad从对u的贡献来看增益矩阵会把单位差异自动平衡回来。LQR的美妙之处就在这里它根据你给的相对权重自动找到在物理尺度差异下最优的增益分配。4. 实操从仿真到实物平台的关键环节4.1 仿真平台搭建与验证动手调实物之前先用MATLAB/Simulink或者Python搭建仿真永远是省时间的第一步。我用的是Python环境配合scipy的solve_continuous_are函数求解Riccati方程整个系统不超过200行代码。仿真的价值主要在三个方面第一验证模型的正确性。把参数代入非线性模型跑开环自由摆动仿真看角度变化曲线和真实摆杆摆动周期是否一致。精确匹配不太可能但周期误差小于10%基本可以接受。第二验证起摆控制律的收敛性。仿真的时间尺度可以随意加速一分钟内能跑几十次起摆流程快速摸索出合理的k_E和k_d范围。然后再上实物细调至少节约两三个小时的现场调试时间。第三验证切换逻辑的稳定性。仿真里故意加入摆杆初始角度扰动检查在极限情况下起摆切换到稳态的流程会不会卡死或者振荡。**有一个特别容易被忽视的坑仿真里的微分环节对采样时间的敏感性。**实物控制器的采样周期一般是1ms~5ms仿真如果你用连续求解器算出来的控制器表现会比实物好很多建议仿真里务必按实物的采样周期做离散化这样才能暴露离散化带来的相位延迟问题。4.2 硬件配置与传感器数据处理我用的平台构成非常简单直流有刷电机带编码器驱动小车摆杆转轴处有另一个编码器测角度DC-DC降压模块给控制板供电主控是STM32F407。硬件上值得注意的细节主要是编码器接口电路和电源滤波。角度和速度的采集处理是整个控制闭环的命门。编码器直接读到的原始计数需要做几件事第一角度归一化。因为摆杆可以360°任意旋转编码器计数会在绕一圈后发生跳变。处理方式把原始角度映射到[-π, π]区间同时维护一个累计转角变量用于起摆阶段的摆幅判断。我用一个小技巧——在中断里做增量换算每次取当前读数与上次读数之差如果差值的绝对值大于半个周期就认为是越界自动调整累计值。第二速度估计。直接用位置差分算速度会引入大量噪声尤其在编码器分辨率不高的时候差分出来的速度信号几乎是毛刺。实测下来效果最好的是中心差分加滑动平均滤波窗口取5~8个采样点。代价是引入了约两个采样周期的延迟但换来的是稳定可靠的速度反馈信号对平衡控制来说很值。第三零位标定。每次上电时不知道摆杆的绝对角度让摆杆自然下垂静止读取当前编码器计数把该位置标定为θπ。这个操作看起来简单但很多人忘了做直接用默认零位会导致起摆阶段能量计算错误。4.3 控制周期选择与执行代码结构我的控制主循环跑在定时器中断里周期设为2ms500Hz。选择这个值的原因系统开环不稳定极点对应的模态频率通常在1~3Hz左右控制频率至少是系统带宽的20倍以上才能保证较好的抑制效果。2ms的周期对STM32来说绰绰有余还能留出量做通信和显示。控制代码的主结构我会拆成几个阶段所有逻辑放在一个有限状态机里状态IDLE系统启动等待摆杆可自由下垂状态SWING_UP执行能量注入起摆状态BALANCE执行LQR稳态平衡状态FAIL检测到超限或异常停止电机并报警每个状态的切换条件集中在同一个函数里方便维护和调试。我最开始把切换逻辑散写在各个控制函数内部后来发现排查问题的时候要把整个代码看一遍才能定位非常痛苦。集中管理之后每次调试只需打印当前状态编号和切换变量就能快速判断问题所在。4.4 起摆到稳态的软过渡处理起摆阶段最后摆杆的角速度通常还比较大如果直接切换到LQR控制控制器会瞬间输出一个很大的反向力矩轻则引起小车突兀一顿重则让摆杆再次倒下。解决办法是在切换时加一个过渡增益曲线。我的实现方式#define SWITCH_BLEND_TIME 0.5f // 过渡时间0.5秒 float blend_ratio (float)(current_tick - switch_start_tick) / (SWITCH_BLEND_TIME * CONTROL_FREQ); if (blend_ratio 1.0f) blend_ratio 1.0f; float control_out blend_ratio * u_balance (1.0f - blend_ratio) * u_swing_last;用一个线性插值把起摆控制量平缓过渡到稳态控制量。实测这样处理后切换瞬间的冲击电流减少了80%以上。还有一种做法是在过渡期间把LQR增益整体乘以blend_ratio效果类似但线上性插值控制量更直观。5. 调试实录常见问题与排查心得5.1 起摆不起来或越摆越弱是怎么回事这是最让人抓狂的问题。摆杆在小车跑动下轻微晃动但摆幅不增加或者越摆越小通常不是控制律问题而是能量注入相位错了。一个排查技巧把估计的摆杆能量E和起摆控制量u_swing同时录下来看u_swing变化和θ̇符号的关系。正确的行为应该是每当θ̇的符号翻转后u_swing也同步改变方向如果两者出现滞后或者反向就说明反馈信号有问题最常见的原因是编码器A、B相接反导致θ̇符号反了。另一个常见原因小车导轨摩擦太大起摆时小车本身移动距离不足能量注入效率低。判断方法是看起摆过程中小车振幅是否够大——如果摆杆摆幅已经接近45°了但小车位移不到10cm说明k_E太小或者速度阻尼k_d太大调整比例即可。5.2 稳态时小车漂移不停怎么办这个问题的根源九成是摩擦模型不准。LQR设计中假设系统是理想的无摩擦模型但实际导轨的静摩擦会让小车的微小位置误差无法被修正表现为缓慢的单向漂移直到撞到限位开关。我尝试过三种方案最后结合使用效果最好在控制量中叠加一个低通滤波后的速度补偿项在位置上添加一个死区逻辑当位置偏差小于1cm且速度趋近于零时停止修正但不切断LQR输出更彻底的做法是在仿真中把Stribeck摩擦模型加入被控对象重新优化Q权重关于死区逻辑有一个要注意的点死区过大容易引起极限环振荡死区过小又起不到作用。我用1mm的死区区段加3mm的迟滞范围实测稳定性和抑制漂移都满意。5.3 切换瞬间摆杆掉下来这个问题主要出现在起摆能量刚好卡在阈值附近的时候。能量阈值设得太高切换时摆杆还没真正进入线性化区间LQR输出不够撑住摆杆阈值设得太低则摆杆仍在大力甩动切换瞬间冲击过大。我的解法是双条件判定角度偏差小于25°且角速度小于80°/s两个条件同时满足才切换。同时把目标能量E0从0改为-0.03J让切换发生在摆杆过冲回落的过程中此时角速度天然比较小。经验总结一句话起摆结束的理想位置是在摆杆接近竖直且速度接近零的时刻而不是能量刚好到零的时刻。5.4 高频抖动与电机发热问题如果LQR增益过高或采样噪声过大控制量会出现高频振荡表现为小车高频抖动电机发热严重。这个问题的根源通常是速度反馈噪声被增益放大。解决方法优先级从低到高增加速度滤波窗口长度降低LQR中速度项权重q_v在控制量输出前加一阶低通滤波截止频率20~50Hz减小编码器倍频避免高频噪声混入注意第4条踩过坑我以为编码器4倍频分辨率越高越好结果4倍频后的低频噪声反而更明显。后来折中成2倍频配合滑动平均滤波效果最好。5.5 问题排查速查表为了方便参考我把调试过程中最常见的问题和对应排查方向整理成一个表格现象可能原因优先排查项起摆无效果编码器方向反、反馈相位错误检查θ̇符号与控制量方向是否匹配起摆周期长k_E过小或k_d过大增大k_E 30%观察摆幅增长速度起摆小车撞限位k_d不足、导轨摩擦小加大速度阻尼增加位置限幅平衡稳定但缓慢漂移未建模摩擦影响增加死区或摩擦补偿项切换时掉摆切换条件不匹配检查角度、角速度双重阈值高频抖动速度噪声或增益过大滤波、降q_v、输出低通滤波电机发热严重PWM频率过高或控制量振荡检查PWM频率设置和分析控制量波形6. 经验总结和进阶方向整套系统从建模到跑通我陆陆续续花了大概两个星期。回头看最值得分享的体会是倒立摆的控制难点从来不在控制器本身而在边界条件——起摆的能量管理、切换的平滑处理、实体硬件带来的噪声和摩擦。这些在教科书上不会写只有亲手调过才知道。有一点我想特别强调仿真和实物的差距是巨大的但仿真依然是调试过程中最值得投入时间的地方。我们在仿真里暴露的问题越多上实物之后翻车的概率就越低。每次调整算法之后先在仿真里跑一百次随机初始条件的测试再上实物验证这个习惯帮我避开了无数低级错误。如果看完这篇你还是觉得不过瘾可以尝试这几个方向第一把稳态控制从LQR换成模型预测控制MPC。MPC的优点是可以显式处理约束条件小车位置限位、电机力矩限幅在同等稳定裕度下可以把起摆和平衡统一到一个控制框架里省去切换逻辑。第二加入状态观测器。我们现在假设四个状态都可直接测量但实际工程中角度可以测小车位置也可以测角速度的位置差分噪声较大。设计一个降维观测器或者卡尔曼滤波器把速度和角速度的估计做得更干净控制品质还能再上一个台阶。第三尝试模糊自适应整定。把摆杆长度视为可变参数在线估计等效摆长再用增益调度法实时更新LQR增益可以让同一套系统适应不同的摆杆配置。直线倒立摆实验的价值在于它涵盖了从建模到工程实现的全链路做完这个项目你对现代控制理论的理解会有一个质的飞跃。希望这篇文章能帮你顺着坑少走一点弯路也欢迎在评论区交流各自的调试心得。 自起摆倒立摆：能量注入与LQR稳态控制实战解析
1. 项目概览为什么非要做自起摆直线一级倒立摆算是控制领域最经典的入门级硬件了大学实验室里几乎人手一套。可千万别小看它这个看似简单的装置串联了建模、状态估计、非线性控制、实时系统一大堆知识点。我最早接触它是在做课程设计那会儿当时的任务和大多数人一样——小车能稳定平衡就行。但真正上手之后才发现最有挑战性的不是“稳住”而是“怎么让它站起来”。理论上说倒立摆的稳态控制已经非常成熟线性二次型调节器LQR一上调好权重几秒钟就能让摆杆稳定在竖直位置。但问题是摆杆在竖直向下的自然状态时你没法直接让它倒立。电机功率有限轨道长度有限如何利用有限的行程把摆杆从下垂位置“甩”起来在合适的时间和位置进入稳态控制范围就是自起摆要解决的问题。这篇文章我会从系统建模开始讲重点拆解两段控制逻辑能量注入式的自起摆设计以及LQR稳态平衡的实现。中间会穿插我实际调试时遇到过的坑和总结出来的经验包括参数整定、限幅处理、状态切换等细节。文中所用的参数基于我手上的直线导轨小车倒立摆平台摆杆长度约30cm小车质量约1kg理解思路之后你完全可以迁移到自己的硬件上。这套方案的价值在于它不依赖高精度的模型参数起摆阶段只要求“摆起来就行”稳态阶段通过LQR保证鲁棒性。整体实现难度适中适合有自动控制理论基础、想动手做完整闭环的读者参考。2. 系统建模把物理装置翻译成数学语言2.1 拉格朗日方程建模过程倒立摆的核心是一个经典的两体系统小车在水平导轨上运动摆杆通过转轴固定在小车上可以在竖直平面内自由转动。先明确我用的坐标定义x为小车水平位移θ为摆杆与竖直向上方向的夹角。**θ0表示摆杆竖直向上目标平衡位置θπ表示摆杆自然下垂。**这个定义很多教材不统一如果看别人代码时符号对不上多半就是初始角度的定义问题。建模用拉格朗日方程最清晰不需要拆解复杂的牛顿力分析。系统的动能包括小车的平动动能1/2·M·ẋ²摆杆质心的平动动能1/2·m·(vx² vy²)摆杆绕质心的转动动能1/2·J·θ̇²其中摆杆质心位置为(x l·sinθ, l·cosθ)注意这里l是质心到转轴的距离如果是均质摆杆则l L/2。联立动能减去势能得到拉格朗日量代入拉格朗日方程整理后会得到两个耦合的非线性微分方程。完整推导过程教科书里都有我直接给实际控制时用的关键结果。小车运动方程 (M m)·ẍ m·l·θ̈·cosθ - m·l·θ̇²·sinθ F摆杆转动方程 m·l·ẍ·cosθ (J m·l²)·θ̈ - m·g·l·sinθ 0这里的F是电机施加在小车上的力。注意方程里有一项m·l·θ̇²·sinθ这是摆杆旋转带来的离心力项。很多人建模完之后忽略这一项在摆动幅度大或者摆速快的时候就会明显感觉到模型和实际行为对不上。2.2 平衡点线性化与状态空间方程非线性方程没法直接设计线性控制器但好在我们的控制目标就是让摆杆稳定在θ0附近的小邻域。在平衡点附近做小角度近似sinθ≈θcosθ≈1θ̇²项视为高阶小量可以舍弃。把这两个方程联立消元整理成标准的状态空间形式。取状态向量X [x, ẋ, θ, θ̇]ᵀ输入u F输出我取全部四个状态方便观测。线性化后的状态矩阵A和输入矩阵B如下以我实际系统的参数带入A [0, 1, 0, 0; 0, 0, -m·g/M, 0; 0, 0, 0, 1; 0, 0, (Mm)·g/(M·l), 0]B [0; 1/M; 0; -1/(M·l)]如果摆杆是均质杆转动惯量J (1/3)·m·L²质心到转轴距离l L/2。代入这些之后A矩阵第四行第三列的值约等于(Mm)·g/(M·l)这个值是系统的关键特征参数直接决定了系统的自然频率和所需的控制增益量级。**一个非常重要的检查点**A矩阵特征值里必然有一个正实部特征值——这是倒立摆的“内在不稳定性”来源表明系统就是开环不稳定的。如果在仿真里发现特征值全部在左半平面那一定是你符号定义或者参数代入出错了。2.3 模型参数获取与实测校准建模不是写几个公式就完了参数不准再好的控制器也白搭。我的做法分成三步第一直接测量法。小车质量M用电子秤称摆杆质量m同样称重。摆杆长度L用游标卡尺量质心位置l对均质杆就是L/2。但实际买来的摆杆可能一端有配重块或者传感器质心位置要重新估算简单的方法是把摆杆水平放置、用棱柱支撑找平衡点那个点就是质心。第二摩擦力辨识。导轨和转轴的摩擦力模型其实相当重要尤其是低速段。我实测发现静摩擦和动摩擦差别很大简单线性摩擦模型不够用。更准确的方法是给小车一个恒定力输入测稳态速度改变输入力大小记录多组数据拟合出摩擦系数。实际操作中如果不做摩擦补偿会出现稳态误差——摆杆看似平衡了但小车会缓慢漂移一直撞击限位开关。第三在仿真里验证模型行为。把实际参数代入非线性模型对摆杆做个自由摆锤仿真对比实测摆动周期的衰减曲线。如果衰减速率和频率都对得上说明模型基本可用。这一步别省后面用能量法设计起摆时模型的准确性直接影响起摆成功率。3. 稳态控制与自起摆的完整方案设计3.1 控制策略选型为什么是LQR加能量法倒立摆稳态控制可以从多种方法中选方案经典PID、极点配置、LQR、滑模控制、自抗扰控制等。我最终选择LQR加能量注入式的起摆策略核心原因有三第一LQR在设计流程上非常清晰物理意义明确。四个状态权重矩阵Q的取值直接对应“我多在乎某个状态的偏差”不需要像PID那样三个参数对着响应曲线反复试探。尤其对于多输入多输出系统LQR能天然处理状态间的耦合关系。第二从起摆到稳态的切换非常自然。起摆阶段我关注的是“总能量”稳态阶段关注的是“状态偏差”两者可以通过一个统一的能量/角度切换条件衔接逻辑清晰。第三LQR的鲁棒性在工程上足够用。尽管我们用的是线性化模型但LQR本身具有一定幅值和相位裕度对模型参数误差、摩擦力等未建模动态有一定容忍度。实测中我用名义参数设计的增益换到不同摆杆上依然能稳定只是起摆和平衡的响应速度略有差异。3.2 自起摆原理能量从一个势能谷爬向势能峰理解自起摆之前先想明白一个物理事实摆杆在竖直下垂位置时系统处于势能最低点。要让摆杆到达竖直向上的倒立位置需要注入足够的能量让摆杆能翻越水平位置的“势能山坡”。起摆过程本质上是一个能量控制问题。理想目标不是精确控制某个角度轨迹而是单调增加摆杆的机械能让摆杆摆幅逐步增大。摆杆能量表达式为E 1/2·(J m·l²)·θ̇² m·g·l·(cosθ - 1)注意这里我把势能零点定义在竖直向上位置θ0所以目标平衡状态对应的能量是E 0而自然下垂状态θπ的能量是E -2·m·g·l。起摆的全部意义就是让总能量从负值逐步逼近零。那么小车运动如何影响摆杆能量对摆杆方程两端乘以θ̇并积分可以得到能量变化率dE/dt -m·l·ẍ·θ̇·cosθ这个式子非常关键。它告诉我们**小车加速度带来的惯性力通过摆杆等效长度做功从而注入能量。**控制策略非常简单粗暴当摆杆向下摆动时θ̇·cosθ的符号为某一方向让小车产生恰当的加速度给摆杆“推一把”当摆杆向上摆动时不要再注入能量甚至可以制动一下防止能量过冲。这样每次摆动都叠加一点能量摆幅不断变大。3.3 起摆控制律设计与限幅策略实际使用的起摆控制律我采用了一个简化的能量反馈加摆动同步项u_swing k_E·(E0 - E)·sign(θ̇·cosθ) k_d·ẋ这里E0是目标能量接近0但略小于0比如-0.05Jsign函数的作用是让力的方向始终和摆杆运动的“可注入能量方向”一致。第二项k_d·ẋ是速度阻尼项防止小车在有限导轨上越冲越远。第一版实现我踩过一个大坑如果不加速度阻尼小车会为了给摆杆注入能量而越来越快地满导轨乱跑经常撞到限位。后来我把小车速度反馈的权重调大起摆过程才可控。经验值k_E取0.5~1.5之间k_d取2~5具体数值和摆杆质量相关需要实测调整。此外还有两个防呆限幅控制量u限制在±15N以内对应电机PWM限幅约±70%小车位置x限制在±0.38m导轨有效行程的一半再加一点安全余量如果小车距离限位小于0.05m强制切换为“先回中”的控制模式起摆结束条件我用的是能量阈值加角度阈值双重判断当估计能量E -0.05J且摆杆角度连续50ms落在±20°范围内立即切换为LQR稳态控制。注意切换瞬间有控制量的阶跃跳变建议做一阶低通滤波过渡避免电机电流冲击。3.4 LQR稳态控制器的权重设计与效果预期稳态控制的核心是设计状态反馈控制器u -K·X通过Riccati方程求解最优增益矩阵K。维度上我的Q矩阵是4×4对角阵Q diag(q_x, q_v, q_theta, q_omega)实际调参过程中我发现对控制效果影响最大的比例关系是q_theta和q_x的相对大小。如果q_theta远大于q_x控制器会优先保证摆杆垂直但小车可能缓慢漂移反过来则小车稳如磐石但摆杆抗扰能力变差。我最终用的权重组合Q diag(500, 10, 200, 5)R 1对应的LQR增益大约是K [-22.36, -31.62, 38.73, 5.19]具体数值和系统参数强相关不建议直接照抄但可以作为调参的初始参考。有人可能会问为什么Q里x的权重比θ大那么多却还是优先稳摆杆因为状态量的物理单位不同x以米为单位本身数值就小θ以弧度为单位摆个5°就是0.087rad从对u的贡献来看增益矩阵会把单位差异自动平衡回来。LQR的美妙之处就在这里它根据你给的相对权重自动找到在物理尺度差异下最优的增益分配。4. 实操从仿真到实物平台的关键环节4.1 仿真平台搭建与验证动手调实物之前先用MATLAB/Simulink或者Python搭建仿真永远是省时间的第一步。我用的是Python环境配合scipy的solve_continuous_are函数求解Riccati方程整个系统不超过200行代码。仿真的价值主要在三个方面第一验证模型的正确性。把参数代入非线性模型跑开环自由摆动仿真看角度变化曲线和真实摆杆摆动周期是否一致。精确匹配不太可能但周期误差小于10%基本可以接受。第二验证起摆控制律的收敛性。仿真的时间尺度可以随意加速一分钟内能跑几十次起摆流程快速摸索出合理的k_E和k_d范围。然后再上实物细调至少节约两三个小时的现场调试时间。第三验证切换逻辑的稳定性。仿真里故意加入摆杆初始角度扰动检查在极限情况下起摆切换到稳态的流程会不会卡死或者振荡。**有一个特别容易被忽视的坑仿真里的微分环节对采样时间的敏感性。**实物控制器的采样周期一般是1ms~5ms仿真如果你用连续求解器算出来的控制器表现会比实物好很多建议仿真里务必按实物的采样周期做离散化这样才能暴露离散化带来的相位延迟问题。4.2 硬件配置与传感器数据处理我用的平台构成非常简单直流有刷电机带编码器驱动小车摆杆转轴处有另一个编码器测角度DC-DC降压模块给控制板供电主控是STM32F407。硬件上值得注意的细节主要是编码器接口电路和电源滤波。角度和速度的采集处理是整个控制闭环的命门。编码器直接读到的原始计数需要做几件事第一角度归一化。因为摆杆可以360°任意旋转编码器计数会在绕一圈后发生跳变。处理方式把原始角度映射到[-π, π]区间同时维护一个累计转角变量用于起摆阶段的摆幅判断。我用一个小技巧——在中断里做增量换算每次取当前读数与上次读数之差如果差值的绝对值大于半个周期就认为是越界自动调整累计值。第二速度估计。直接用位置差分算速度会引入大量噪声尤其在编码器分辨率不高的时候差分出来的速度信号几乎是毛刺。实测下来效果最好的是中心差分加滑动平均滤波窗口取5~8个采样点。代价是引入了约两个采样周期的延迟但换来的是稳定可靠的速度反馈信号对平衡控制来说很值。第三零位标定。每次上电时不知道摆杆的绝对角度让摆杆自然下垂静止读取当前编码器计数把该位置标定为θπ。这个操作看起来简单但很多人忘了做直接用默认零位会导致起摆阶段能量计算错误。4.3 控制周期选择与执行代码结构我的控制主循环跑在定时器中断里周期设为2ms500Hz。选择这个值的原因系统开环不稳定极点对应的模态频率通常在1~3Hz左右控制频率至少是系统带宽的20倍以上才能保证较好的抑制效果。2ms的周期对STM32来说绰绰有余还能留出量做通信和显示。控制代码的主结构我会拆成几个阶段所有逻辑放在一个有限状态机里状态IDLE系统启动等待摆杆可自由下垂状态SWING_UP执行能量注入起摆状态BALANCE执行LQR稳态平衡状态FAIL检测到超限或异常停止电机并报警每个状态的切换条件集中在同一个函数里方便维护和调试。我最开始把切换逻辑散写在各个控制函数内部后来发现排查问题的时候要把整个代码看一遍才能定位非常痛苦。集中管理之后每次调试只需打印当前状态编号和切换变量就能快速判断问题所在。4.4 起摆到稳态的软过渡处理起摆阶段最后摆杆的角速度通常还比较大如果直接切换到LQR控制控制器会瞬间输出一个很大的反向力矩轻则引起小车突兀一顿重则让摆杆再次倒下。解决办法是在切换时加一个过渡增益曲线。我的实现方式#define SWITCH_BLEND_TIME 0.5f // 过渡时间0.5秒 float blend_ratio (float)(current_tick - switch_start_tick) / (SWITCH_BLEND_TIME * CONTROL_FREQ); if (blend_ratio 1.0f) blend_ratio 1.0f; float control_out blend_ratio * u_balance (1.0f - blend_ratio) * u_swing_last;用一个线性插值把起摆控制量平缓过渡到稳态控制量。实测这样处理后切换瞬间的冲击电流减少了80%以上。还有一种做法是在过渡期间把LQR增益整体乘以blend_ratio效果类似但线上性插值控制量更直观。5. 调试实录常见问题与排查心得5.1 起摆不起来或越摆越弱是怎么回事这是最让人抓狂的问题。摆杆在小车跑动下轻微晃动但摆幅不增加或者越摆越小通常不是控制律问题而是能量注入相位错了。一个排查技巧把估计的摆杆能量E和起摆控制量u_swing同时录下来看u_swing变化和θ̇符号的关系。正确的行为应该是每当θ̇的符号翻转后u_swing也同步改变方向如果两者出现滞后或者反向就说明反馈信号有问题最常见的原因是编码器A、B相接反导致θ̇符号反了。另一个常见原因小车导轨摩擦太大起摆时小车本身移动距离不足能量注入效率低。判断方法是看起摆过程中小车振幅是否够大——如果摆杆摆幅已经接近45°了但小车位移不到10cm说明k_E太小或者速度阻尼k_d太大调整比例即可。5.2 稳态时小车漂移不停怎么办这个问题的根源九成是摩擦模型不准。LQR设计中假设系统是理想的无摩擦模型但实际导轨的静摩擦会让小车的微小位置误差无法被修正表现为缓慢的单向漂移直到撞到限位开关。我尝试过三种方案最后结合使用效果最好在控制量中叠加一个低通滤波后的速度补偿项在位置上添加一个死区逻辑当位置偏差小于1cm且速度趋近于零时停止修正但不切断LQR输出更彻底的做法是在仿真中把Stribeck摩擦模型加入被控对象重新优化Q权重关于死区逻辑有一个要注意的点死区过大容易引起极限环振荡死区过小又起不到作用。我用1mm的死区区段加3mm的迟滞范围实测稳定性和抑制漂移都满意。5.3 切换瞬间摆杆掉下来这个问题主要出现在起摆能量刚好卡在阈值附近的时候。能量阈值设得太高切换时摆杆还没真正进入线性化区间LQR输出不够撑住摆杆阈值设得太低则摆杆仍在大力甩动切换瞬间冲击过大。我的解法是双条件判定角度偏差小于25°且角速度小于80°/s两个条件同时满足才切换。同时把目标能量E0从0改为-0.03J让切换发生在摆杆过冲回落的过程中此时角速度天然比较小。经验总结一句话起摆结束的理想位置是在摆杆接近竖直且速度接近零的时刻而不是能量刚好到零的时刻。5.4 高频抖动与电机发热问题如果LQR增益过高或采样噪声过大控制量会出现高频振荡表现为小车高频抖动电机发热严重。这个问题的根源通常是速度反馈噪声被增益放大。解决方法优先级从低到高增加速度滤波窗口长度降低LQR中速度项权重q_v在控制量输出前加一阶低通滤波截止频率20~50Hz减小编码器倍频避免高频噪声混入注意第4条踩过坑我以为编码器4倍频分辨率越高越好结果4倍频后的低频噪声反而更明显。后来折中成2倍频配合滑动平均滤波效果最好。5.5 问题排查速查表为了方便参考我把调试过程中最常见的问题和对应排查方向整理成一个表格现象可能原因优先排查项起摆无效果编码器方向反、反馈相位错误检查θ̇符号与控制量方向是否匹配起摆周期长k_E过小或k_d过大增大k_E 30%观察摆幅增长速度起摆小车撞限位k_d不足、导轨摩擦小加大速度阻尼增加位置限幅平衡稳定但缓慢漂移未建模摩擦影响增加死区或摩擦补偿项切换时掉摆切换条件不匹配检查角度、角速度双重阈值高频抖动速度噪声或增益过大滤波、降q_v、输出低通滤波电机发热严重PWM频率过高或控制量振荡检查PWM频率设置和分析控制量波形6. 经验总结和进阶方向整套系统从建模到跑通我陆陆续续花了大概两个星期。回头看最值得分享的体会是倒立摆的控制难点从来不在控制器本身而在边界条件——起摆的能量管理、切换的平滑处理、实体硬件带来的噪声和摩擦。这些在教科书上不会写只有亲手调过才知道。有一点我想特别强调仿真和实物的差距是巨大的但仿真依然是调试过程中最值得投入时间的地方。我们在仿真里暴露的问题越多上实物之后翻车的概率就越低。每次调整算法之后先在仿真里跑一百次随机初始条件的测试再上实物验证这个习惯帮我避开了无数低级错误。如果看完这篇你还是觉得不过瘾可以尝试这几个方向第一把稳态控制从LQR换成模型预测控制MPC。MPC的优点是可以显式处理约束条件小车位置限位、电机力矩限幅在同等稳定裕度下可以把起摆和平衡统一到一个控制框架里省去切换逻辑。第二加入状态观测器。我们现在假设四个状态都可直接测量但实际工程中角度可以测小车位置也可以测角速度的位置差分噪声较大。设计一个降维观测器或者卡尔曼滤波器把速度和角速度的估计做得更干净控制品质还能再上一个台阶。第三尝试模糊自适应整定。把摆杆长度视为可变参数在线估计等效摆长再用增益调度法实时更新LQR增益可以让同一套系统适应不同的摆杆配置。直线倒立摆实验的价值在于它涵盖了从建模到工程实现的全链路做完这个项目你对现代控制理论的理解会有一个质的飞跃。希望这篇文章能帮你顺着坑少走一点弯路也欢迎在评论区交流各自的调试心得。