1. 从能站住到站得稳直线型一阶倒立摆软件设计到底在解决什么问题很多人做倒立摆硬件搭完、电机能转、编码器能读数就觉得离成功只差写几行PID了。真上手才发现摆杆要么纹丝不动地倒下去要么疯狂来回抽搐要么勉强立住两秒然后猛地飞出去。问题几乎从来不在硬件而在软件设计这一层——控制周期怎么定、状态量怎么滤波、控制量怎么限幅、什么时候该切模式这些决策才是决定倒立摆能站住还是站得稳的分水岭。直线型一阶倒立摆的软件设计本质上是把一套物理模型翻译成实时可执行的代码逻辑。它要解决的核心问题有三个第一如何在一个固定的控制周期内稳定地采集小车位置、小车速度、摆杆角度、摆杆角速度这四个状态量第二如何用这些状态量算出一个合适的电机驱动力让摆杆角度收敛到零、小车位置也收敛到目标点第三如何在启动、扰动、超限这些边界情况下保证系统不失控、不损坏机械结构。这套软件适合谁参考如果你正在做控制类课程设计、参加相关学科竞赛或者单纯想搞明白一个真实的实时控制系统长什么样那这篇内容就是给你写的。它不假设你有很深的控制理论背景但会尽量把每个设计决策背后的原因讲清楚让你不只是抄代码而是知道为什么这么写。下面我会按照一个实际项目的推进顺序从软件架构、状态采集、控制算法、模式管理到调试手段逐层拆开讲。2. 软件整体架构为什么我坚持定时器中断 状态机这套组合2.1 主循环轮询方案为什么在倒立摆上会翻车刚接触倒立摆的人最容易写出的结构是这样的主循环里依次读编码器、读角度传感器、算PID、输出PWM然后靠延时或者干脆不延时地循环下去。这个结构在跑马灯、读温度这种慢速任务上没问题但放到倒立摆上会立刻暴露两个致命缺陷。第一个缺陷是控制周期不确定。主循环里如果加了串口打印、屏幕刷新这类耗时操作控制周期就会从1毫秒变成5毫秒甚至10毫秒而且这个变化是不规律的。倒立摆是一个开环不稳定系统控制周期的抖动会直接反映成控制量的抖动摆杆就会开始高频抖动甚至发散。第二个缺陷是状态量读取不同步。你在主循环里先读小车位置再读摆杆角度这中间隔了几十微秒对于快速运动的摆杆来说这两个量对应的根本不是同一个时刻的状态用它们算出来的控制量天然就带误差。我实测过一个反面案例某次为了图省事把角度传感器的读取放在主循环里结果摆杆在平衡点附近始终有大约±0.5度的高频振荡怎么调PID都压不下去。后来把读取挪进定时器中断振荡立刻降到±0.1度以内。这不是PID参数的问题是采样时序的问题。2.2 定时器中断负责硬实时主循环负责慢任务正确的架构应该是分层的。定时器中断以固定频率触发比如1kHz也就是每1毫秒进一次中断。中断服务函数里只做三件事读取所有状态量、执行控制算法、更新PWM输出。这三件事必须在一个控制周期内完成所以中断里的代码要尽可能短不能有浮点除法以外的复杂运算更不能有打印、延时、等待这类阻塞操作。主循环则负责所有不要求硬实时的任务按键扫描、OLED显示刷新、串口数据上传、参数在线调整、故障日志记录。这些任务慢一点没关系几十毫秒的延迟人眼根本看不出来但它们绝对不能干扰中断的时序。这里有个关键细节主循环和中断之间共享的变量比如目标位置、PID参数、模式标志位必须做原子性保护。最简单的做法是在主循环写这些变量时先关中断写完再开中断。如果用的是32位单片机读写一个32位变量本身是原子的但如果是读-改-写操作比如target step就必须保护。我一般会把这些共享变量集中放在一个结构体里主循环更新时整体关中断拷贝一份中断里只读这份拷贝避免竞态。2.3 状态机把启动-平衡-复位串成一条清晰的线倒立摆不能一上电就直接进平衡控制因为初始时刻摆杆大概率是倒着的直接算PID会输出一个巨大的控制量小车猛地一冲机械结构都可能受损。所以软件里必须有一个状态机来管理运行阶段。我通常设计四个状态待机态、起摆态、平衡态、故障态。待机态下电机不输出等待用户指令起摆态下通过特定的摆动策略把摆杆从下垂位置甩到接近竖直平衡态下切换到LQR或PID进行稳定控制故障态下立即切断电机输出并记录故障原因。状态之间的切换条件要写得非常明确比如摆杆角度进入±10度且角速度小于某阈值才允许从起摆态切到平衡态。这个状态机的价值在于它把什么时候该做什么这件事从控制算法里剥离出来了。控制算法只管给定当前状态输出多少力至于这个力该不该输出、输出给谁由状态机决定。这样调试的时候你可以单独测试起摆策略也可以单独测试平衡控制互不干扰。3. 状态量采集编码器和角度传感器的数据怎么变成可用的速度信号3.1 角度传感器选型与原始数据处理直线型一阶倒立摆的角度测量常见方案有电位器、增量式编码器、绝对式编码器、磁编码器几种。电位器便宜但有磨损和噪声增量式编码器精度高但需要零位标定绝对式编码器省事但成本高。我一般推荐用磁编码器比如基于霍尔原理的那类体积小、无接触磨损、上电即知绝对角度省掉了找零位的麻烦。拿到原始角度数据后第一件事是零位标定。摆杆竖直向上的位置对应的传感器读数就是角度零点。这个零点必须在软件里做成可配置的参数因为每次装配后机械零位都会有细微差异。标定方法很简单手动把摆杆扶到竖直读取传感器值写入零点参数。但要注意标定时小车必须静止否则传感器读数会受振动影响。第二件事是角度解算。如果用的是增量式编码器读出来的是脉冲计数需要乘以一个系数转换成角度。这个系数等于360度除以编码器一圈的总脉冲数。如果编码器经过齿轮传动还要乘以传动比。这里有个容易踩的坑传动比的方向。齿轮传动可能让编码器计数方向与摆杆实际转动方向相反如果不处理角度符号就是反的控制量也会反摆杆会直接飞出去。我一般会在软件里加一个方向标志位标定时如果发现方向反了改一下标志位就行不用重新接线。3.2 速度信号为什么不能直接对位置做差分小车速度和摆杆角速度这两个量理论上可以通过对位置和角度做差分得到。但直接差分有个严重问题噪声放大。假设位置信号上叠加了幅度0.1毫米的随机噪声采样周期是1毫秒那么差分出来的速度噪声幅度就是0.1毫米除以1毫秒等于0.1米每秒。这个噪声水平已经和真实速度相当了控制量会被噪声主导电机就会发出刺耳的啸叫。解决办法是加低通滤波器。最常用的是一阶低通公式是y y alpha * (x - y)其中alpha是滤波系数越小滤波越强但延迟越大。alpha的选取要在噪声抑制和相位延迟之间折中。我一般先用一个经验值比如0.1到0.3之间然后看实际波形调整。如果速度信号还是太毛糙可以再串一级滤波但要注意两级滤波会带来更大的相位滞后可能影响控制稳定性。另一个方案是用状态观测器比如卡尔曼滤波或者龙伯格观测器直接从位置和角度估计出速度。观测器的好处是能在滤波的同时补偿相位延迟但实现复杂度高参数整定也麻烦。对于一阶倒立摆这种模型明确的系统我建议先用低通滤波跑通等有精力再上观测器。实测下来一阶低通配合合理的控制周期已经能把倒立摆控制得很稳。3.3 采样时序四个状态量必须在同一时刻读取前面提到过四个状态量必须对应同一时刻。具体实现上我会在中断服务函数的最开头用一个标志位或者直接顺序读取的方式把四个量一次性读进来存到局部变量里后续所有计算都用这组局部变量。读取顺序也有讲究先读角度和角速度再读位置和速度因为摆杆的动态比小车快角度信息的时效性更重要。如果角度传感器和位置传感器走的是不同的通信接口比如一个走SPI一个走I2C那读取时间差可能达到几十微秒。这种情况下要么选同一种接口要么在软件里做时间戳对齐。不过对于1kHz的控制周期来说几十微秒的偏差通常可以接受真正要避免的是毫秒级的偏差。4. 控制算法落地从LQR到PID代码里到底怎么算4.1 为什么我最终选了LQR而不是纯PID倒立摆的控制目标有两个摆杆角度归零、小车位置归零。这是一个多变量控制问题两个目标之间存在耦合——你把摆杆往左推小车会往右跑你把小车往左拉摆杆会往右倒。纯PID很难同时处理好这两个目标的耦合关系调参时经常是角度稳了位置飘位置稳了角度抖。LQR线性二次型调节器天然适合这种多变量问题。它的核心思想是定义一个代价函数同时惩罚状态偏差和控制量大小然后求解使代价函数最小的控制律。对于倒立摆状态向量是[位置, 速度, 角度, 角速度]控制量是电机力。LQR会算出一个4维的反馈增益向量K控制量就等于-K乘以状态向量。LQR的优势在于你只需要调整代价函数里的权重矩阵Q和R就能在角度控制优先和位置控制优先之间做权衡。Q的对角线元素越大对应状态的偏差被惩罚得越重控制器就越倾向于快速消除这个偏差。比如你把角度对应的Q元素调大摆杆角度就会被控制得更紧但小车位置可能会波动更大。这种权衡关系非常直观比PID三个参数互相纠缠要好调得多。4.2 LQR增益的计算与代码实现LQR增益K的计算通常是在电脑上用MATLAB或者Python的control库离线算好然后把K的四个数值硬编码到单片机程序里。计算时需要倒立摆的线性化模型包括小车质量、摆杆质量、摆杆长度、摩擦系数这些参数。这些参数不需要非常精确因为LQR有一定的鲁棒性参数偏差20%以内通常还能稳住。代码实现上控制量计算就是一行向量点乘float K[4] {k1, k2, k3, k4}; // 离线算好的增益 float state[4] {pos, vel, angle, ang_vel}; float control 0; for (int i 0; i 4; i) { control - K[i] * state[i]; }这里有个细节状态量必须做归一化或者单位统一。位置用米、速度用米每秒、角度用弧度、角速度用弧度每秒这样算出来的K才是有意义的。如果角度用度、位置用毫米K的数值会差好几个数量级虽然理论上也能工作但调参时非常不直观。4.3 控制量限幅与死区补偿算出来的控制量不能直接丢给电机。首先要做限幅因为电机驱动力有上限而且过大的控制量会损坏机械结构。限幅值一般取电机额定力矩的80%左右留一点余量。限幅之后还要做死区补偿因为电机在低电压下可能转不动存在一个死区。如果不补偿小控制量会被死区吃掉摆杆在平衡点附近就会有小幅度的极限环振荡。死区补偿的做法是如果控制量绝对值小于死区阈值就把它映射到死区边界如果大于阈值就正常输出。更精细的做法是加一个前馈补偿量但一阶倒立摆用简单的死区映射就够了。我实测下来加了死区补偿之后摆杆在平衡点的残余振荡能从±0.3度降到±0.1度以内。4.4 积分项要不要加一个容易引发争议的设计决策LQR本身是不带积分项的这意味着如果系统存在稳态误差比如小车轨道有轻微倾斜摆杆可能稳定在一个非零角度上。要消除这个稳态误差可以在LQR基础上加一个积分项对角度偏差做积分然后把积分量作为一个额外的状态反馈。但积分项是把双刃剑。加得好稳态误差消除加得不好积分饱和会导致系统大幅超调甚至振荡。我的经验是如果机械装配精度足够轨道水平度调得好可以先不加积分靠LQR本身的鲁棒性就能把角度控制在±0.1度以内。如果确实需要加积分系数要从很小的值开始试并且一定要加积分限幅防止积分量无限累积。5. 模式管理与安全逻辑让倒立摆摔得优雅5.1 起摆策略怎么把下垂的摆杆甩上来倒立摆上电时摆杆通常是自然下垂的。起摆的目标是把摆杆从下垂位置角度180度甩到接近竖直角度接近0度。常见的起摆策略有两种能量法和往复摆动法。能量法的思路是通过控制小车往复运动给摆杆注入能量直到摆杆的总能量动能加势能达到竖直位置对应的势能。具体实现时根据摆杆当前角度和角速度计算能量然后让小车朝某个方向加速加速方向与摆杆运动方向相关。能量法的优点是起摆快通常两三秒就能甩上来缺点是对参数敏感能量计算不准时可能甩过头。往复摆动法更简单粗暴让小车按固定频率和幅度来回运动摆杆在共振作用下幅度逐渐增大当角度进入竖直附近时切换到平衡控制。这个方法的优点是实现简单、鲁棒性好缺点是起摆时间较长可能需要五六秒。我一般先用往复摆动法跑通因为它对参数不敏感容易调试。等系统稳定了再换成能量法提升起摆速度。切换条件要设得保守一点比如角度进入±15度且角速度小于某个阈值才切换避免在摆杆快速穿过竖直位置时切换导致失控。5.2 故障检测与紧急停机倒立摆运行过程中可能出现多种故障摆杆角度超出安全范围、小车撞到轨道限位、电机电流过大、编码器信号丢失。这些故障如果不及时处理轻则控制失效重则损坏机械。软件里必须有一个故障检测与处理机制。我的做法是在中断里做快速检测比如角度绝对值超过60度、小车位置超过轨道长度的80%、电机电流超过额定值任何一个条件满足就立即把状态机切到故障态电机输出置零。故障态下会记录故障类型和发生时刻方便后续排查。这里有个细节故障检测的阈值不能设得太紧否则正常控制时的瞬态波动会误触发。比如摆杆在受到扰动时可能瞬间冲到30度如果阈值设成25度就会误报。我一般把角度故障阈值设在45到60度之间位置故障阈值设在轨道限位的90%处留足安全余量。5.3 平滑启动与软停止电机从零直接跳到目标输出会产生一个冲击电流对电机驱动器和机械结构都不友好。所以启动时要加一个斜坡让控制量在几十毫秒内从零逐渐增加到目标值。停止时同理让控制量逐渐降到零而不是直接切断。这个斜坡在代码里就是一个简单的限幅每个控制周期控制量的变化量不超过某个最大值。比如最大变化量设为满量程的1%那么100个周期也就是100毫秒内控制量能从零加到满量程。这个时间常数要根据机械结构的承受能力来定太短了冲击大太长了响应慢。6. 调试手段没有好的调试工具倒立摆就是玄学6.1 串口波形把状态量实时传出来看调倒立摆最忌讳的就是盲调——改一个参数看摆杆动两下再改一个参数再看两下。这种方式效率极低而且根本不知道内部状态发生了什么。正确的做法是通过串口把关键状态量实时传到电脑上用波形工具画出来看。我一般会传四个状态量加上控制量一共五个通道。串口波特率至少115200控制周期1毫秒的话每毫秒传五个浮点数数据量是20字节115200波特率下每毫秒能传约11字节不够用。所以要么降低上传频率比如每10个控制周期传一次要么用二进制格式而不是ASCII格式减少数据量。二进制格式下五个float是20字节115200波特率下每毫秒能传约5.7组还是不够。实际做法是每5到10毫秒传一组对于观察趋势来说足够了。看波形时重点关注几个东西角度是否收敛、有没有稳态误差、控制量是否饱和、速度信号是否毛糙。如果角度波形有高频振荡说明控制周期或者滤波有问题如果角度收敛但速度信号很脏说明滤波不够如果控制量频繁饱和说明增益太大或者限幅太紧。6.2 参数在线调整不用反复烧录每次改参数都要重新编译烧录调试效率会非常低。所以软件里应该支持参数在线调整。实现方式很简单主循环里解析串口收到的命令比如P 1.5表示把某个参数设为1.5然后更新对应的全局变量。中断里读这些全局变量时注意原子性保护。我一般会把所有可调参数放在一个结构体里串口命令通过参数编号来索引。这样加新参数时只需要在结构体里加一个字段命令解析部分不用改。在线调整配合波形显示调参效率能提升好几倍。6.3 常见问题排查表调试过程中遇到的问题大部分都能归到几类里。下面这个表是我自己总结的排查清单遇到问题时按顺序过一遍基本能定位到原因。现象可能原因排查方法摆杆高频抖动控制周期不稳定、速度信号噪声大检查中断是否被其他任务打断、加大滤波系数摆杆缓慢倒向一侧零位标定不准、轨道不水平重新标定零位、调整轨道水平度小车持续朝一个方向跑位置反馈符号反了、目标位置设置错误检查位置编码器方向、确认目标位置起摆甩不上去起摆幅度不够、切换条件太严加大起摆幅度、放宽切换角度阈值平衡后突然飞车故障检测误触发、积分饱和检查故障阈值、加积分限幅电机发出啸叫控制量高频振荡、PWM频率与电机共振降低控制增益、调整PWM频率6.4 从能跑到跑得好几个提升控制品质的细节系统能稳住之后还可以做一些优化让控制品质更好。第一个是前馈补偿如果知道小车轨道有固定坡度可以在控制量里加一个恒定的前馈项抵消坡度的影响这样LQR的反馈项就不用去补偿这个固定偏差控制会更从容。第二个是增益调度在起摆阶段用一组增益在平衡阶段用另一组增益。起摆阶段需要快速响应增益可以大一点平衡阶段需要精细控制增益可以小一点。切换增益时要做平滑过渡避免突变引起振荡。第三个是扰动观测如果系统经常受到外部扰动比如有人碰一下摆杆可以加一个扰动观测器估计扰动力的大小并前馈补偿。这个实现起来复杂一些但对于提升抗扰能力效果明显。7. 写在最后一些踩过坑之后才明白的事倒立摆的软件设计最难的不是写控制算法而是处理各种边界情况和时序问题。我踩过的最大的坑是在中断里调用了浮点除法结果中断执行时间从20微秒涨到200微秒控制周期直接崩了。后来把所有除法都改成乘以倒数并且把倒数在初始化时算好问题才解决。另一个坑是串口打印。调试时为了看数据在中断里加了printf结果串口输出阻塞了中断摆杆直接飞出去。后来改成把数据存到缓冲区主循环里再打印才解决。这个教训让我明白中断服务函数里绝对不能有任何可能阻塞的操作。还有一个经验是先仿真再上实物。在MATLAB或者Python里把控制算法仿真跑通确认参数范围合理再往单片机里移植。仿真里可以随便试参数不用担心摔坏硬件。实物调试时先用仿真得到的参数作为起点微调一下就能用能省很多时间。最后说一个心态问题。倒立摆调试过程中失败是常态成功是偶然。可能调了一整天都稳不住第二天换个思路突然就成了。关键是每次失败都要有记录、有分析知道是哪个环节出了问题而不是盲目地改参数。把调试过程当成一个系统性的排查过程而不是碰运气这样才能真正把倒立摆做稳、做好。