不知道你有没有过这种经历调一个直线一级倒立摆看着摆杆垂在导轨上明明知道自己要做的事情就两件事——让它自己摆上去再让它站稳可真到了写代码、调参数的时候问题一个接一个。起摆时小车跑到限位还没摆起来或者好不容易摆到竖起来了却被LQR一接管就甩飞出去。我在这类项目上踩过的坑不算少这篇文章就把我做完整个直线一级倒立摆自起摆与稳态控制项目的思路、建模、算法、调试经验完整写出来希望能让后面做这个项目的人少走点弯路。项目面向自动化、机器人方向的学生和工程师内容覆盖从拉格朗日建模、能量法起摆、LQR稳态控制到实时实现的完整链路读完你至少能搭出一套能跑起来的控制方案。1. 项目目标与控制方案选型1.1 自起摆与稳态控制到底难在哪直线一级倒立摆是典型的欠驱动系统驱动量只有小车的水平力或加速度系统却要同时管住“小车位移”和“摆杆角度”两个输出。更麻烦的是摆杆本身没有独立的执行器你只能通过底座的加减速来间接改变摆杆运动状态这就决定了它必须分两个阶段来处理。自起摆的目标是让摆杆从自然下垂位置通过往复摆动不断积累能量最终摆到倒立位置附近。这里的核心不是“控制”而是“能量注入”如果直接套用线性控制器在摆角较大的区域根本无效因为线性模型在倒立点附近才成立。稳态控制的目标则是在倒立点附近抵抗扰动让摆杆稳定停留在竖直位置。这个阶段系统可以近似为线性时不变系统也是经典状态反馈能充分发挥作用的地方。两个阶段衔接起来才是完整的项目起摆让系统进入稳态控制的捕获范围稳态控制在捕获范围内完成锁定。难点往往不在某一阶段本身而在两阶段切换的那一瞬间。1.2 为什么选“能量法起摆LQR稳态”这套组合自起摆的主流方案有能量法、开关控制bang-bang控制、模糊控制和轨迹规划等方法。我最终选择能量法是因为它不需要精确知道摆杆的初始状态对摩擦等未建模动态也有较强的鲁棒性。bang-bang控制虽然简单但起摆过程比较粗野容易让小车频繁撞击行程限位模糊控制则需要整定大量规则调试效率低。稳态控制方面LQR线性二次型调节器几乎是这个项目最合理的选择。它通过最小化二次型性能指标直接得到状态反馈增益矩阵不需要像极点配置那样反复试凑极点位置还有清晰的权重矩阵可以作为性能调优入口。这套组合在工程上还有一个实际优势能量法起摆的输出和中后期LQR的控制量都是连续值不像bang-bang那样剧烈跳变切换时对执行机构的冲击更小也有利于起摆过程结束后的平滑接管。2. 动力学建模与系统辨识2.1 从拉格朗日方程到状态空间方程直线一级倒立摆的物理模型由小车和摆杆两部分组成。设小车质量为M位移为r受到水平外力u摆杆质量为m质心到铰点距离为l绕质心转动惯量为I摆杆与竖直向上方向的夹角为θ。系统动能包括小车平动动能、摆杆质心平动动能和摆杆绕质心转动动能势能则取倒立位置为零点。写出拉格朗日方程并化简会得到两个耦合的非线性微分方程。在θ0附近做小角度线性化也就是令sinθ≈θ、cosθ≈1并忽略高阶小量整理后得到[ (Mm)\ddot{r} b\dot{r} - ml\ddot{\theta} u ][ ml\ddot{r} (Iml^2)\ddot{\theta} - mgl\theta 0 ]这里b是小车与导轨之间的粘性摩擦系数。取状态变量为 (x [r, \dot{r}, \theta, \dot{\theta}]^T)控制输入为u输出如果想同时观测位置和角度可以简单取 (y [r, \theta]^T)。这一步线性化是后续LQR设计的基础也是整个项目中“为什么在倒立点附近可以放心用状态反馈”的根本原因。2.2 参数测量与辨识方法模型参数直接决定状态空间矩阵和控制增益的数值参数不准会导致仿真效果与实物差异巨大。质量和长度可以用电子秤、游标卡尺直接测量需要多说两句的是另外三个量。摆杆质心到铰点距离l可以用悬挂法确定把摆杆悬挂起来重垂线方向通过质心变换两次悬挂点交点就是质心位置。转动惯量I也可以用双线摆实验估算但对细长摆杆来说先用均质杆公式 (I mL^2/12) 估算就够用误差对LQR增益的影响不大。最需要实测的是摩擦系数b这往往是仿真和实物差距的主要来源。我的做法是让小车以恒定速度运行记录电机电流换算出的稳态推力再用力除以速度得到粘性摩擦系数近似值。实际操作中摩擦力还有库仑摩擦成分但倒立摆平衡阶段小车速度通常很小把摩擦全部折算成粘性项并适当调大能获得更好的控制效果。2.3 一套可以直接用的参考参数与A、B矩阵为了让后面的LQR设计有可复现的数值示例我以自己用过的一套平台参数为例M 1.0 kgm 0.2 kgl 0.3 mI 0.006 kg·m^2b 0.1 N·s/mg 9.81 m/s^2代入线性化方程并整理成状态空间形式得到的A矩阵和B矩阵如下A [0 1 0 0; 0 -0.0952 1.4014 0; 0 0 0 1; 0 0.2381 28.0286 0] B [0; 0.9524; 0; 2.3810]可以看到A矩阵里右下角元素是28.0286它来自 ( mgl(Mm)/D ) 代表重力在摆角上产生的“不稳定”效应数值越大说明系统越容易倾倒。设计控制器时这个值对应的物理意象就是摆杆角度只要有微小偏差重力矩就会把它进一步拉偏控制器的任务就是抵抗这个趋势。3. 自起摆控制的设计与实现3.1 起摆本质把能量“泵”进摆杆如果系统没有阻尼摆杆总机械能是守恒的。摆杆自然下垂时动能和势能都最小倒立时势能最大。起摆的目标就是让摆杆从最低能量状态逐步爬升到最高能量状态本质上是个能量注入过程。小车每次加速或减速都会通过铰点向摆杆传递惯性力这个力可以在摆杆运动的某些相位做正功在其他相位做负功。起摆控制的关键就是选择合适的时机施加力让正功尽量多、负功尽量少。经典的能量法思路是构造一个“能量误差”信号用它来决策施加力的方向和大小。设摆杆总机械能为E倒立静止时的目标能量为E0那么当E小于E0时控制器应该让系统从外部获取能量当E接近E0时控制力应逐渐减小为切换到平衡控制做准备。3.2 一种实用的能量起摆算法我使用的起摆控制律可以写成u sat( K_sw * (E - E0) * sign(theta_dot * cos(theta)), U_max )其中theta是摆杆与竖直向上方向的夹角theta_dot是角速度sat是饱和限幅函数K_sw是需要整定的起摆增益U_max是限幅值。这个式子里的符号判断值得单独解释一下当摆杆还在下方区域摆荡cos(theta)为正如果摆杆正朝倒立方向运动theta_dot也大于零那么整个符号为正控制力朝正方向输出把能量往摆杆里“泵”当摆杆越过倒立位置开始往回摆cos(theta)变负控制方向自动切换避免在摆杆回落时继续无意义地注入能量。饱和函数的作用是限制最大控制力防止起摆过程中小车直接撞到限位。K_sw和U_max的整定有很强的现场性K_sw太小能量注入速度慢起摆要磨蹭十几秒K_sw太大摆杆还没到倒立位小车已经跑到导轨尽头了。我实际调试时一般先从较小的K_sw开始比如5观察摆杆摆幅是否逐次增大再逐步增加。摆幅增大的过程应该是稳定的、单调的如果摆到某一半幅值附近开始“卡住”说明摩擦力超过了单次能量注入的增量需要适当增大U_max或K_sw。3.3 行程保护与起摆阶段的边界条件直线一级倒立摆的小车行程有限起摆过程又是大幅摆动很容易触发限位开关。我在工程实现里加了三层保护。第一层是控制力限幅也就是前面提到的U_max通过限制最大加速度来避免小车被“推飞”。第二层是做摆杆角度阈值判断的“软限位”如果摆杆角度超过设定摆幅就暂时降低甚至反向输出控制力避免摆杆在小车尚未回中时继续把小车拽向一侧。第三层才是硬限位也就是限位开关信号直接切断输出。这三层保护的优先级要从高到低安排硬件限位切断永远第一位软限位其次控制力限幅最后。因为起摆过程中一旦小车撞到限位还没自动停下轻则机械结构受损重则编码器读数错乱导致整个控制失效。另外起摆阶段不要一上来就把目标能量设成E0可以设置一个能量阈值比如先起摆到目标能量的95%略微降低摆杆到达倒立点的速度这样切换冲击会小很多。实测下来这个“留一手”的做法能明显提高起摆切换到稳态的成功率。4. 稳态平衡控制与LQR设计4.1 LQR权重矩阵的选择经验LQR的性能指标是 (J \int (x^T Q x u^T R u) dt)Q和R的取值直接决定反馈增益。很多人卡在这一步是因为不知道Q该怎么选其实核心原则很简单你更在意哪个状态收敛就把对应的Q权重调大。对直线一级倒立摆来说四个状态分别是小车位移、小车速度、摆杆角度、摆杆角速度。一般来说最关心的是摆杆角度其次是角速度再是小车位移最后是速度。所以Q矩阵一个常见的初始取法是Q diag([5, 1, 100, 10]) R 1这里角度权重100明显大于位移权重5意思是允许小车有较大位移来换取摆杆角度的快速回正。R取1表示对控制量不做太强的惩罚。如果现场发现控制量饱和严重就适当增大R如果发现摆杆收敛太慢就先增大Q中角度和角速度对应项。调参时要注意LQR增益不是越大越好。增益太大控制量很容易饱和反而导致系统在极限环上振荡增益太小抗扰能力不足。一个实用的判定标准是在稳态控制下用手轻推摆杆系统能在一到两秒内恢复平衡且控制量不频繁触及限幅。4.2 参考设计与求解代码用Matlab求解LQR非常简单A [0 1 0 0; 0 -0.0952 1.4014 0; 0 0 0 1; 0 0.2381 28.0286 0]; B [0; 0.9524; 0; 2.3810]; Q diag([5, 1, 100, 10]); R 1; K lqr(A, B, Q, R); eig(A - B*K)我在这组参数下算出的反馈增益大致是K [-2.2361, -4.9126, -33.1681, -6.3352]对应的闭环极点都在左半平面说明系统稳定。你会注意到K的第三项角度反馈明显大于其他项这是因为经过Riccati方程求解后系统会自动分配更多的控制权重来抵抗重力带来的发散趋势。需要注意的是这套K值是在前面那套平台参数下算出来的如果你手里的平台参数不同直接套用K值往往不会得到满意效果。正确做法是把平台实测参数代入A、B矩阵重新用lqr求解。4.3 起摆到稳态的切换逻辑与平滑处理起摆和稳态两套控制律都有各自适用的状态区间如何判断什么时候从起摆切到稳态是整个项目中最容易翻车的环节。我采用的切换条件是双阈值切换条件: |theta| 0.3 rad 且 |theta_dot| 0.6 rad/s角度阈值0.3 rad大概17度保证摆杆已经进入LQR的线性化工作区间角速度阈值0.6 rad/s则保证摆杆不是以很大速度冲过倒立点否则即使LQR接管也需要很大控制力才能把角速度“刹住”很容易触发控制饱和。切换瞬间控制量跳变也是常见问题。起摆控制律在切换点可能输出较大的正向力而LQR刚接管时输出是另一个值二者相差过大会造成冲击。我处理这个问题的办法是给输出加一个斜坡限幅切换后的前100毫秒内把LQR输出与上一个控制周期输出做线性插值过渡。实测下来这样做把切换成功率从勉强及格提升到了几乎每次都能稳定接管。5. 实时控制实现要点5.1 硬件平台与控制周期选择直线一级倒立摆的典型硬件组成包括直流伺服电机、同步带驱动的直线导轨、小车、摆杆、两路增量式编码器一路测量小车位置一路测量摆杆角度以及电机驱动器和控制器。控制器选型上有人用Matlab/Simulink实时仿真机有人用带编码器接口的嵌入式板卡。无论哪种控制周期都要尽量短。我的经验是1毫秒控制周期是稳妥起点5毫秒以上起摆会明显“迟钝”能量注入的相位总是慢半拍导致摆幅增长缓慢甚至失败。原因很好理解起摆算法依赖准确的相位判断控制周期越长判断结果的实时性越差能量注入的最佳时机越容易错过。5.2 状态获取与噪声处理LQR需要全部四个状态量其中小车位置和摆杆角度来自编码器但小车速度和摆杆角速度必须通过差分估算。直接对位置差分得到的速度噪声极大控制效果会表现为高频抖动甚至引发驱动器过流保护。我建议用一阶低通滤波配合差分或者更平滑的方法是用跟踪微分器。低通滤波器的截止频率需要权衡截止频率太高速度噪声仍然明显太低则速度估计滞后LQR的阻尼项相当于“慢半拍”系统容易振荡。对1毫秒控制周期速度滤波截止频率放在30到50赫兹效果比较理想。另一个容易踩坑的点是编码器角度归一化。摆杆编码器正反转动时计数会跨过零点如果不做归一化处理角度会在0到2π之间跳变导致角度差计算错误。需要在读取原始编码器值后立即做角度范围映射并保证切换逻辑和LQR使用的角度定义一致。5.3 主循环代码框架实时控制主循环是状态机的经典结构。下面这段伪代码概括了我实际项目中的主循环思路while True: r_raw read_encoder_carriage() theta_raw read_encoder_pendulum() theta normalize_angle(theta_raw) r r_raw * resolution_to_meter # 速度和角速度估计带低通滤波 r_dot lowpass_filter((r - r_prev) / dt) theta_dot lowpass_filter((theta - theta_prev) / dt) if abs(theta) 0.3 and abs(theta_dot) 0.6: control_state BALANCE else: control_state SWING_UP if control_state SWING_UP: E compute_energy(theta, theta_dot) u swingup_control(E, theta, theta_dot) else: u -K[0]*r - K[1]*r_dot - K[2]*theta - K[3]*theta_dot u slope_limit(u, u_prev, max_step) u saturation(u, u_max) output_to_motor(u) r_prev, theta_prev r, theta u_prev u wait_until_next_cycle()这段流程中一定要注意的是切换状态的判定必须在控制量计算之前完成因为起摆控制律和LQR用到的状态量虽然是同一组但控制律本身完全不同一旦状态判断延迟一个周期切换冲击就会被放大。6. 调试记录与常见问题排查6.1 起摆不起或者越摆越低这类问题最直接的现象是摆杆摆幅始终在一个小范围内浮动甚至越来越小。排查思路我一般按下面几步走。先看符号是否正确。包括角度方向正负、控制力方向正负。常见的错误是做能量计算时角度定义与编码器映射不一致导致符号判断完全反了控制力在应该注入能量的时候反而在抽走能量。一个快速验证方法用手把摆杆拨到一侧观察控制器输出力的方向看是不是在帮助摆杆继续往反方向摆。再看增益是否过小。能量注入量不够摩擦损耗抵消了注入能量摆幅自然上不去。这时候需要增大K_sw或U_max。如果在某个摆幅附近反复“卡住”大概率就是这个原因。最后检查小车行程是否被浪费。理想的起摆过程应该在小车行程范围内尽量让摆杆多摆几个来回如果小车总是在一段很短距离内来回移动说明控制力方向切换太频繁能量注入效率很低可以考虑加大限幅和减小K_sw。6.2 切换瞬间“打摆子”摆杆确实到达了倒立点附近但一切到LQR就来回哆嗦甚至飞出去这是最让人头疼的场景之一。先说最常见的原因切换太早。LQR是线性模型设计的控制器如果切换时刻摆角还很大、角速度还很快状态点已经超出了LQR的吸引域系统自然稳定不住。解决办法是收紧切换阈值尤其要收紧角速度阈值。其次是切换时控制量突变。起摆控制律在这个时刻的输出可能仍在较大值而LQR刚算出的控制量是另一个方向两个值直接切换必然造成小车突然加/减速反而把摆杆“抖”倒。解决办法就是前面提到的斜坡限幅过渡。还有一个比较隐蔽的原因切换阈值内角度和角速度虽然满足条件但摆杆其实还在加速冲向倒立点。比如角速度阈值0.6 rad/s如果角加速度很大摆杆在切换后50毫秒内就会超过0.6LQR要花很大力气才能刹住。这种情况可以增加对摆杆角加速度的估算或者直接通过现场观察切换时刻摆杆运动趋势来调整阈值。6.3 LQR参数到底怎么调很多人在调LQR参数时一上来就把Q矩阵所有元素都改一遍这样改了等于白改因为你根本分不清是哪个状态权重起的作用。我推荐的步骤是先固定R1只调Q。第一步把角度权重Q33调大观察摆杆是否更快回正第二步调角速度权重Q44观察阻尼效果第三步调位移权重Q11观察小车是否会漂移太远。每调一个参数都用手扶摆杆或者给摆杆施加一个外部扰动来验证响应不要只看仿真曲线。还有一个判断经验如果系统出现持续高频抖动基本是角速度权重太大或者速度估计噪声太大如果系统出现低频摆动、回复缓慢基本是角度权重不够或者控制增益整体偏小。先分辨频率特征再决定改哪个参数效率会高很多。6.4 一个推荐的调试顺序很多第一次做这个项目的人上来就直接把起摆和稳态一起调一旦出问题根本分不清是哪部分引起的。我建议严格按照下面的顺序来。先把起摆禁用掉用手扶住摆杆到倒立位置附近只调LQR稳态。确认稳态控制能在小扰动下稳定站立并且恢复时间可以接受再进行下一步。这一步的目的很明确先把一个阶段调稳再让另一个阶段去对接它。然后把LQR关闭单独调起摆。观察摆幅是否逐次增大小车是否会在导轨两端撞限位。起摆成功且摆杆能到达倒立点附近后先别急着连上LQR可以设一个较大的切换阈值让起摆阶段把摆杆“送”到LQR更容易接管的状态。最后才是把两者完整联动起来重点观察切换处的行为。按照这个顺序走下来整个项目会清晰很多几乎不会走到“全盘推翻重来”的那一步。这个项目做完之后我对“控制”这件事最大的体会是常规的线性控制方法只要用对场合真的非常可靠但前提是你得弄清楚它什么时候有效、什么时候失效。起摆阶段非线性强就老老实实用能量法倒立点附近线性化误差小才轮到LQR上场。整套方案跑顺之后你还能继续往这个平台上加更多玩法比如加了扰动的小车位置跟踪、摆杆阻尼系数在线辨识甚至是更激进的起摆时间优化。我个人觉得直线一级倒立摆这个项目最珍贵的地方就是它把所有控制环节都浓缩在一个桌面级平台上无论理论还是工程都能快速验证。 
长河君 · 2026/9/29 9:06:16 · 阅读 1.2k 直线一级倒立摆自起摆与LQR稳态控制全流程解析
不知道你有没有过这种经历调一个直线一级倒立摆看着摆杆垂在导轨上明明知道自己要做的事情就两件事——让它自己摆上去再让它站稳可真到了写代码、调参数的时候问题一个接一个。起摆时小车跑到限位还没摆起来或者好不容易摆到竖起来了却被LQR一接管就甩飞出去。我在这类项目上踩过的坑不算少这篇文章就把我做完整个直线一级倒立摆自起摆与稳态控制项目的思路、建模、算法、调试经验完整写出来希望能让后面做这个项目的人少走点弯路。项目面向自动化、机器人方向的学生和工程师内容覆盖从拉格朗日建模、能量法起摆、LQR稳态控制到实时实现的完整链路读完你至少能搭出一套能跑起来的控制方案。1. 项目目标与控制方案选型1.1 自起摆与稳态控制到底难在哪直线一级倒立摆是典型的欠驱动系统驱动量只有小车的水平力或加速度系统却要同时管住“小车位移”和“摆杆角度”两个输出。更麻烦的是摆杆本身没有独立的执行器你只能通过底座的加减速来间接改变摆杆运动状态这就决定了它必须分两个阶段来处理。自起摆的目标是让摆杆从自然下垂位置通过往复摆动不断积累能量最终摆到倒立位置附近。这里的核心不是“控制”而是“能量注入”如果直接套用线性控制器在摆角较大的区域根本无效因为线性模型在倒立点附近才成立。稳态控制的目标则是在倒立点附近抵抗扰动让摆杆稳定停留在竖直位置。这个阶段系统可以近似为线性时不变系统也是经典状态反馈能充分发挥作用的地方。两个阶段衔接起来才是完整的项目起摆让系统进入稳态控制的捕获范围稳态控制在捕获范围内完成锁定。难点往往不在某一阶段本身而在两阶段切换的那一瞬间。1.2 为什么选“能量法起摆LQR稳态”这套组合自起摆的主流方案有能量法、开关控制bang-bang控制、模糊控制和轨迹规划等方法。我最终选择能量法是因为它不需要精确知道摆杆的初始状态对摩擦等未建模动态也有较强的鲁棒性。bang-bang控制虽然简单但起摆过程比较粗野容易让小车频繁撞击行程限位模糊控制则需要整定大量规则调试效率低。稳态控制方面LQR线性二次型调节器几乎是这个项目最合理的选择。它通过最小化二次型性能指标直接得到状态反馈增益矩阵不需要像极点配置那样反复试凑极点位置还有清晰的权重矩阵可以作为性能调优入口。这套组合在工程上还有一个实际优势能量法起摆的输出和中后期LQR的控制量都是连续值不像bang-bang那样剧烈跳变切换时对执行机构的冲击更小也有利于起摆过程结束后的平滑接管。2. 动力学建模与系统辨识2.1 从拉格朗日方程到状态空间方程直线一级倒立摆的物理模型由小车和摆杆两部分组成。设小车质量为M位移为r受到水平外力u摆杆质量为m质心到铰点距离为l绕质心转动惯量为I摆杆与竖直向上方向的夹角为θ。系统动能包括小车平动动能、摆杆质心平动动能和摆杆绕质心转动动能势能则取倒立位置为零点。写出拉格朗日方程并化简会得到两个耦合的非线性微分方程。在θ0附近做小角度线性化也就是令sinθ≈θ、cosθ≈1并忽略高阶小量整理后得到[ (Mm)\ddot{r} b\dot{r} - ml\ddot{\theta} u ][ ml\ddot{r} (Iml^2)\ddot{\theta} - mgl\theta 0 ]这里b是小车与导轨之间的粘性摩擦系数。取状态变量为 (x [r, \dot{r}, \theta, \dot{\theta}]^T)控制输入为u输出如果想同时观测位置和角度可以简单取 (y [r, \theta]^T)。这一步线性化是后续LQR设计的基础也是整个项目中“为什么在倒立点附近可以放心用状态反馈”的根本原因。2.2 参数测量与辨识方法模型参数直接决定状态空间矩阵和控制增益的数值参数不准会导致仿真效果与实物差异巨大。质量和长度可以用电子秤、游标卡尺直接测量需要多说两句的是另外三个量。摆杆质心到铰点距离l可以用悬挂法确定把摆杆悬挂起来重垂线方向通过质心变换两次悬挂点交点就是质心位置。转动惯量I也可以用双线摆实验估算但对细长摆杆来说先用均质杆公式 (I mL^2/12) 估算就够用误差对LQR增益的影响不大。最需要实测的是摩擦系数b这往往是仿真和实物差距的主要来源。我的做法是让小车以恒定速度运行记录电机电流换算出的稳态推力再用力除以速度得到粘性摩擦系数近似值。实际操作中摩擦力还有库仑摩擦成分但倒立摆平衡阶段小车速度通常很小把摩擦全部折算成粘性项并适当调大能获得更好的控制效果。2.3 一套可以直接用的参考参数与A、B矩阵为了让后面的LQR设计有可复现的数值示例我以自己用过的一套平台参数为例M 1.0 kgm 0.2 kgl 0.3 mI 0.006 kg·m^2b 0.1 N·s/mg 9.81 m/s^2代入线性化方程并整理成状态空间形式得到的A矩阵和B矩阵如下A [0 1 0 0; 0 -0.0952 1.4014 0; 0 0 0 1; 0 0.2381 28.0286 0] B [0; 0.9524; 0; 2.3810]可以看到A矩阵里右下角元素是28.0286它来自 ( mgl(Mm)/D ) 代表重力在摆角上产生的“不稳定”效应数值越大说明系统越容易倾倒。设计控制器时这个值对应的物理意象就是摆杆角度只要有微小偏差重力矩就会把它进一步拉偏控制器的任务就是抵抗这个趋势。3. 自起摆控制的设计与实现3.1 起摆本质把能量“泵”进摆杆如果系统没有阻尼摆杆总机械能是守恒的。摆杆自然下垂时动能和势能都最小倒立时势能最大。起摆的目标就是让摆杆从最低能量状态逐步爬升到最高能量状态本质上是个能量注入过程。小车每次加速或减速都会通过铰点向摆杆传递惯性力这个力可以在摆杆运动的某些相位做正功在其他相位做负功。起摆控制的关键就是选择合适的时机施加力让正功尽量多、负功尽量少。经典的能量法思路是构造一个“能量误差”信号用它来决策施加力的方向和大小。设摆杆总机械能为E倒立静止时的目标能量为E0那么当E小于E0时控制器应该让系统从外部获取能量当E接近E0时控制力应逐渐减小为切换到平衡控制做准备。3.2 一种实用的能量起摆算法我使用的起摆控制律可以写成u sat( K_sw * (E - E0) * sign(theta_dot * cos(theta)), U_max )其中theta是摆杆与竖直向上方向的夹角theta_dot是角速度sat是饱和限幅函数K_sw是需要整定的起摆增益U_max是限幅值。这个式子里的符号判断值得单独解释一下当摆杆还在下方区域摆荡cos(theta)为正如果摆杆正朝倒立方向运动theta_dot也大于零那么整个符号为正控制力朝正方向输出把能量往摆杆里“泵”当摆杆越过倒立位置开始往回摆cos(theta)变负控制方向自动切换避免在摆杆回落时继续无意义地注入能量。饱和函数的作用是限制最大控制力防止起摆过程中小车直接撞到限位。K_sw和U_max的整定有很强的现场性K_sw太小能量注入速度慢起摆要磨蹭十几秒K_sw太大摆杆还没到倒立位小车已经跑到导轨尽头了。我实际调试时一般先从较小的K_sw开始比如5观察摆杆摆幅是否逐次增大再逐步增加。摆幅增大的过程应该是稳定的、单调的如果摆到某一半幅值附近开始“卡住”说明摩擦力超过了单次能量注入的增量需要适当增大U_max或K_sw。3.3 行程保护与起摆阶段的边界条件直线一级倒立摆的小车行程有限起摆过程又是大幅摆动很容易触发限位开关。我在工程实现里加了三层保护。第一层是控制力限幅也就是前面提到的U_max通过限制最大加速度来避免小车被“推飞”。第二层是做摆杆角度阈值判断的“软限位”如果摆杆角度超过设定摆幅就暂时降低甚至反向输出控制力避免摆杆在小车尚未回中时继续把小车拽向一侧。第三层才是硬限位也就是限位开关信号直接切断输出。这三层保护的优先级要从高到低安排硬件限位切断永远第一位软限位其次控制力限幅最后。因为起摆过程中一旦小车撞到限位还没自动停下轻则机械结构受损重则编码器读数错乱导致整个控制失效。另外起摆阶段不要一上来就把目标能量设成E0可以设置一个能量阈值比如先起摆到目标能量的95%略微降低摆杆到达倒立点的速度这样切换冲击会小很多。实测下来这个“留一手”的做法能明显提高起摆切换到稳态的成功率。4. 稳态平衡控制与LQR设计4.1 LQR权重矩阵的选择经验LQR的性能指标是 (J \int (x^T Q x u^T R u) dt)Q和R的取值直接决定反馈增益。很多人卡在这一步是因为不知道Q该怎么选其实核心原则很简单你更在意哪个状态收敛就把对应的Q权重调大。对直线一级倒立摆来说四个状态分别是小车位移、小车速度、摆杆角度、摆杆角速度。一般来说最关心的是摆杆角度其次是角速度再是小车位移最后是速度。所以Q矩阵一个常见的初始取法是Q diag([5, 1, 100, 10]) R 1这里角度权重100明显大于位移权重5意思是允许小车有较大位移来换取摆杆角度的快速回正。R取1表示对控制量不做太强的惩罚。如果现场发现控制量饱和严重就适当增大R如果发现摆杆收敛太慢就先增大Q中角度和角速度对应项。调参时要注意LQR增益不是越大越好。增益太大控制量很容易饱和反而导致系统在极限环上振荡增益太小抗扰能力不足。一个实用的判定标准是在稳态控制下用手轻推摆杆系统能在一到两秒内恢复平衡且控制量不频繁触及限幅。4.2 参考设计与求解代码用Matlab求解LQR非常简单A [0 1 0 0; 0 -0.0952 1.4014 0; 0 0 0 1; 0 0.2381 28.0286 0]; B [0; 0.9524; 0; 2.3810]; Q diag([5, 1, 100, 10]); R 1; K lqr(A, B, Q, R); eig(A - B*K)我在这组参数下算出的反馈增益大致是K [-2.2361, -4.9126, -33.1681, -6.3352]对应的闭环极点都在左半平面说明系统稳定。你会注意到K的第三项角度反馈明显大于其他项这是因为经过Riccati方程求解后系统会自动分配更多的控制权重来抵抗重力带来的发散趋势。需要注意的是这套K值是在前面那套平台参数下算出来的如果你手里的平台参数不同直接套用K值往往不会得到满意效果。正确做法是把平台实测参数代入A、B矩阵重新用lqr求解。4.3 起摆到稳态的切换逻辑与平滑处理起摆和稳态两套控制律都有各自适用的状态区间如何判断什么时候从起摆切到稳态是整个项目中最容易翻车的环节。我采用的切换条件是双阈值切换条件: |theta| 0.3 rad 且 |theta_dot| 0.6 rad/s角度阈值0.3 rad大概17度保证摆杆已经进入LQR的线性化工作区间角速度阈值0.6 rad/s则保证摆杆不是以很大速度冲过倒立点否则即使LQR接管也需要很大控制力才能把角速度“刹住”很容易触发控制饱和。切换瞬间控制量跳变也是常见问题。起摆控制律在切换点可能输出较大的正向力而LQR刚接管时输出是另一个值二者相差过大会造成冲击。我处理这个问题的办法是给输出加一个斜坡限幅切换后的前100毫秒内把LQR输出与上一个控制周期输出做线性插值过渡。实测下来这样做把切换成功率从勉强及格提升到了几乎每次都能稳定接管。5. 实时控制实现要点5.1 硬件平台与控制周期选择直线一级倒立摆的典型硬件组成包括直流伺服电机、同步带驱动的直线导轨、小车、摆杆、两路增量式编码器一路测量小车位置一路测量摆杆角度以及电机驱动器和控制器。控制器选型上有人用Matlab/Simulink实时仿真机有人用带编码器接口的嵌入式板卡。无论哪种控制周期都要尽量短。我的经验是1毫秒控制周期是稳妥起点5毫秒以上起摆会明显“迟钝”能量注入的相位总是慢半拍导致摆幅增长缓慢甚至失败。原因很好理解起摆算法依赖准确的相位判断控制周期越长判断结果的实时性越差能量注入的最佳时机越容易错过。5.2 状态获取与噪声处理LQR需要全部四个状态量其中小车位置和摆杆角度来自编码器但小车速度和摆杆角速度必须通过差分估算。直接对位置差分得到的速度噪声极大控制效果会表现为高频抖动甚至引发驱动器过流保护。我建议用一阶低通滤波配合差分或者更平滑的方法是用跟踪微分器。低通滤波器的截止频率需要权衡截止频率太高速度噪声仍然明显太低则速度估计滞后LQR的阻尼项相当于“慢半拍”系统容易振荡。对1毫秒控制周期速度滤波截止频率放在30到50赫兹效果比较理想。另一个容易踩坑的点是编码器角度归一化。摆杆编码器正反转动时计数会跨过零点如果不做归一化处理角度会在0到2π之间跳变导致角度差计算错误。需要在读取原始编码器值后立即做角度范围映射并保证切换逻辑和LQR使用的角度定义一致。5.3 主循环代码框架实时控制主循环是状态机的经典结构。下面这段伪代码概括了我实际项目中的主循环思路while True: r_raw read_encoder_carriage() theta_raw read_encoder_pendulum() theta normalize_angle(theta_raw) r r_raw * resolution_to_meter # 速度和角速度估计带低通滤波 r_dot lowpass_filter((r - r_prev) / dt) theta_dot lowpass_filter((theta - theta_prev) / dt) if abs(theta) 0.3 and abs(theta_dot) 0.6: control_state BALANCE else: control_state SWING_UP if control_state SWING_UP: E compute_energy(theta, theta_dot) u swingup_control(E, theta, theta_dot) else: u -K[0]*r - K[1]*r_dot - K[2]*theta - K[3]*theta_dot u slope_limit(u, u_prev, max_step) u saturation(u, u_max) output_to_motor(u) r_prev, theta_prev r, theta u_prev u wait_until_next_cycle()这段流程中一定要注意的是切换状态的判定必须在控制量计算之前完成因为起摆控制律和LQR用到的状态量虽然是同一组但控制律本身完全不同一旦状态判断延迟一个周期切换冲击就会被放大。6. 调试记录与常见问题排查6.1 起摆不起或者越摆越低这类问题最直接的现象是摆杆摆幅始终在一个小范围内浮动甚至越来越小。排查思路我一般按下面几步走。先看符号是否正确。包括角度方向正负、控制力方向正负。常见的错误是做能量计算时角度定义与编码器映射不一致导致符号判断完全反了控制力在应该注入能量的时候反而在抽走能量。一个快速验证方法用手把摆杆拨到一侧观察控制器输出力的方向看是不是在帮助摆杆继续往反方向摆。再看增益是否过小。能量注入量不够摩擦损耗抵消了注入能量摆幅自然上不去。这时候需要增大K_sw或U_max。如果在某个摆幅附近反复“卡住”大概率就是这个原因。最后检查小车行程是否被浪费。理想的起摆过程应该在小车行程范围内尽量让摆杆多摆几个来回如果小车总是在一段很短距离内来回移动说明控制力方向切换太频繁能量注入效率很低可以考虑加大限幅和减小K_sw。6.2 切换瞬间“打摆子”摆杆确实到达了倒立点附近但一切到LQR就来回哆嗦甚至飞出去这是最让人头疼的场景之一。先说最常见的原因切换太早。LQR是线性模型设计的控制器如果切换时刻摆角还很大、角速度还很快状态点已经超出了LQR的吸引域系统自然稳定不住。解决办法是收紧切换阈值尤其要收紧角速度阈值。其次是切换时控制量突变。起摆控制律在这个时刻的输出可能仍在较大值而LQR刚算出的控制量是另一个方向两个值直接切换必然造成小车突然加/减速反而把摆杆“抖”倒。解决办法就是前面提到的斜坡限幅过渡。还有一个比较隐蔽的原因切换阈值内角度和角速度虽然满足条件但摆杆其实还在加速冲向倒立点。比如角速度阈值0.6 rad/s如果角加速度很大摆杆在切换后50毫秒内就会超过0.6LQR要花很大力气才能刹住。这种情况可以增加对摆杆角加速度的估算或者直接通过现场观察切换时刻摆杆运动趋势来调整阈值。6.3 LQR参数到底怎么调很多人在调LQR参数时一上来就把Q矩阵所有元素都改一遍这样改了等于白改因为你根本分不清是哪个状态权重起的作用。我推荐的步骤是先固定R1只调Q。第一步把角度权重Q33调大观察摆杆是否更快回正第二步调角速度权重Q44观察阻尼效果第三步调位移权重Q11观察小车是否会漂移太远。每调一个参数都用手扶摆杆或者给摆杆施加一个外部扰动来验证响应不要只看仿真曲线。还有一个判断经验如果系统出现持续高频抖动基本是角速度权重太大或者速度估计噪声太大如果系统出现低频摆动、回复缓慢基本是角度权重不够或者控制增益整体偏小。先分辨频率特征再决定改哪个参数效率会高很多。6.4 一个推荐的调试顺序很多第一次做这个项目的人上来就直接把起摆和稳态一起调一旦出问题根本分不清是哪部分引起的。我建议严格按照下面的顺序来。先把起摆禁用掉用手扶住摆杆到倒立位置附近只调LQR稳态。确认稳态控制能在小扰动下稳定站立并且恢复时间可以接受再进行下一步。这一步的目的很明确先把一个阶段调稳再让另一个阶段去对接它。然后把LQR关闭单独调起摆。观察摆幅是否逐次增大小车是否会在导轨两端撞限位。起摆成功且摆杆能到达倒立点附近后先别急着连上LQR可以设一个较大的切换阈值让起摆阶段把摆杆“送”到LQR更容易接管的状态。最后才是把两者完整联动起来重点观察切换处的行为。按照这个顺序走下来整个项目会清晰很多几乎不会走到“全盘推翻重来”的那一步。这个项目做完之后我对“控制”这件事最大的体会是常规的线性控制方法只要用对场合真的非常可靠但前提是你得弄清楚它什么时候有效、什么时候失效。起摆阶段非线性强就老老实实用能量法倒立点附近线性化误差小才轮到LQR上场。整套方案跑顺之后你还能继续往这个平台上加更多玩法比如加了扰动的小车位置跟踪、摆杆阻尼系数在线辨识甚至是更激进的起摆时间优化。我个人觉得直线一级倒立摆这个项目最珍贵的地方就是它把所有控制环节都浓缩在一个桌面级平台上无论理论还是工程都能快速验证。