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 常见问题排查表

调试过程中遇到的问题，大部分都能归到几类里。下面这个表是我自己总结的排查清单，遇到问题时按顺序过一遍，基本能定位到原因。

6.4 从"能跑"到"跑得好"：几个提升控制品质的细节

系统能稳住之后，还可以做一些优化让控制品质更好。第一个是前馈补偿：如果知道小车轨道有固定坡度，可以在控制量里加一个恒定的前馈项，抵消坡度的影响，这样LQR的反馈项就不用去补偿这个固定偏差，控制会更从容。

第二个是增益调度：在起摆阶段用一组增益，在平衡阶段用另一组增益。起摆阶段需要快速响应，增益可以大一点；平衡阶段需要精细控制，增益可以小一点。切换增益时要做平滑过渡，避免突变引起振荡。

第三个是扰动观测：如果系统经常受到外部扰动，比如有人碰一下摆杆，可以加一个扰动观测器，估计扰动力的大小并前馈补偿。这个实现起来复杂一些，但对于提升抗扰能力效果明显。

7. 写在最后：一些踩过坑之后才明白的事

倒立摆的软件设计，最难的不是写控制算法，而是处理各种边界情况和时序问题。我踩过的最大的坑，是在中断里调用了浮点除法，结果中断执行时间从20微秒涨到200微秒，控制周期直接崩了。后来把所有除法都改成乘以倒数，并且把倒数在初始化时算好，问题才解决。

另一个坑是串口打印。调试时为了看数据，在中断里加了printf，结果串口输出阻塞了中断，摆杆直接飞出去。后来改成把数据存到缓冲区，主循环里再打印，才解决。这个教训让我明白，中断服务函数里绝对不能有任何可能阻塞的操作。

还有一个经验是：先仿真再上实物。在MATLAB或者Python里把控制算法仿真跑通，确认参数范围合理，再往单片机里移植。仿真里可以随便试参数，不用担心摔坏硬件。实物调试时，先用仿真得到的参数作为起点，微调一下就能用，能省很多时间。

最后说一个心态问题。倒立摆调试过程中，失败是常态，成功是偶然。可能调了一整天都稳不住，第二天换个思路突然就成了。关键是每次失败都要有记录、有分析，知道是哪个环节出了问题，而不是盲目地改参数。把调试过程当成一个系统性的排查过程，而不是碰运气，这样才能真正把倒立摆做稳、做好。