简介：直线倒立摆STM32控制程序是一套基于STM32F10x系列微控制器的经典控制实验源码工程，面向嵌入式开发者和控制理论学习者，用于实现倒立摆系统的实时采集、算法解算与电机驱动平衡。程序整合了PD、PID、微分先行、梯形积分、抗积分饱和、积分分离等多种控制策略，并按BALANCE、HARDWARE等模块拆分，便于定位逻辑、修改参数和二次开发。压缩包共214个文件，大小3.79MB，以C/H源码、O/AXF/HEX编译输出、Uvprojx工程配置、MAP/LST链接调试文件为主，同时包含标准外设库，可直接在Keil环境打开编译。已有818人学习下载，适合需要从零搭建倒立摆实验项目或理解嵌入式实时控制完整流程的读者。通过这份工程，可获得硬件驱动、控制算法、系统初始化及编译配置的一整套参考实现，对学习STM32库函数开发和自动控制原理很有帮助。 第一次接触直线倒立摆，是在做课程设计的时候。实验室里摆着一套导轨、小车加摆杆的装置，老师给的课题就一句话：用STM32实现直线倒立摆控制程序。我当时心想，不就是一根杆子立在车上嘛，接上电机、读个编码器、写个PID，怎么也能立住几秒吧。结果真动起手来，才知道这东西有多“磨人”：先是编码器读数乱跳，接着小车一启动就往轨道尽头冲，好不容易把杆立起来了，稍微碰一下小车就疯狂来回甩，最后直接飞出去。前前后后折腾了两三个星期，踩了无数坑，才把系统调到一个相对稳定的状态。
回头看，直线倒立摆这个项目虽然“岁数”不小，但它把机械、电力电子、传感器、实时控制和嵌入式编程全部串在了一条链路上。做完这一套，你对“闭环控制”这四个字的理解会和只看书本完全不一样。这篇文章我就从硬件接线、控制算法、STM32代码实现、现场调试四个方面，把自己做直线倒立摆控制程序的完整思路和踩坑记录写出来，希望能帮到正在和倒立摆“搏斗”的同学。
1. 为什么这根杆子“看着简单、做起来要命”：控制难点与系统架构
1.1 系统本质：一个持续发散的不稳定对象
倒立摆最大的特点是“天生不稳定”。摆杆在垂直向上的位置是一个不稳定的平衡点，只要角度稍微偏离一点，重力力矩就会让杆继续往同一个方向偏，而且越偏越快。这意味着控制程序必须在杆转过头之前就检测到偏差，并让小车“追”着杆跑，把杆顶回竖直位置。
这个反应有多快？我实际测下来，一根普通的金属摆杆从垂直位置偏离约10度到完全倒下，时间大约在0.3到0.5秒之间。也就是说，控制器必须在几十毫秒甚至十几毫秒内输出有效的修正力矩。所以这类系统对控制周期极其敏感，我最终采用的1kHz控制循环，本质上就是在采样间隔1毫秒的离散控制系统中，和一根“随时要倒”的杆子赛跑。
另一个难点在于，倒立摆是一个典型的“欠驱动”系统，小车只有一个自由度，却要同时控制“小车位置”和“摆杆角度”两个状态，电机推力必须同时兼顾位置调节和角度稳定。这在控制理论上很经典，在工程上却意味着每一毫秒的决策背后都要做优先级权衡。
1.2 机械与传感器带来的“额外难度”
除了系统本身的控制难度，机械件和传感器也会给你“挖坑”。直线导轨的摩擦力、皮带或丝杆的间隙、编码器的安装偏心、电机死区，这些非理想因素叠加在一起，会让理论模型和实际装置差得非常多。我刚开始调参数时，按照仿真模型给的LQR增益直接上机，结果小车直接“抽风”，在轨道上来回撞，差点把限位块都给撞飞。
所以做倒立摆，建议一开始就建立这样一个认知：控制算法只是系统的一半，另一半是机械装配质量、传感器精度和底层驱动的实时性。哪一端偏了，闭环都不可能稳。
2. 搭建平台时的关键取舍：编码器、电机驱动与供电
2.1 两个编码器怎么接进STM32
直线倒立摆至少需要两个位置反馈：一个是小车在导轨上的位置，另一个是摆杆绕铰接点的角度。市面上常见的倒立摆实验台，一般用两个增量式编码器分别测这两个量。增量式编码器输出A、B两相正交脉冲，STM32的定时器有专门的编码器模式，可以硬件自动识别脉冲计数和方向，不需要外部中断去手动数脉冲，这一点建议直接使用。
我在项目里用的是500线增量式编码器，经过4倍频之后，每圈就有2000个计数。摆杆角度编码器直接装在铰接点上，所以角度分辨率大约是2π/2000，也就是约0.18度，这个精度对倒立摆控制来说是够用的。小车位置编码器如果和电机同轴安装，换算关系就要看皮带轮直径或丝杆导程，这个单位换算建议先写在纸上算清楚，不然控制增益的量纲会乱套。
接线方面，STM32的定时器编码器模式需要占用同一个定时器的两个通道。比如我用TIM3的通道1和通道2接摆杆编码器，用TIM4的通道1和通道2接小车位置编码器。CubeMX里选择“Encoder Mode”，再把计数器设置为16位或32位都可以。
注意：编码器模块和STM32之间的电平最好一致。很多编码器或驱动板是5V接口，如果STM32用的是3.3V供电，最好加电平转换，或者用支持3.3V兼容的编码器，否则长期使用有烧引脚的风险。
2.2 电机驱动选型与供电独立性
驱动小车通常用直流减速电机，配套一个H桥驱动板。倒立摆在起摆和平衡过程中，电机会频繁正反转，起步瞬间电流很大。我最早用一个额定电流1A的小驱动模块，结果跑十几秒就过热保护，小车直接失去控制。后来换成了持续电流3A以上的双通道H桥驱动板，这个问题才消失。
供电方面是我踩过的另一个大坑。电机启动和正反转切换时，母线电流突变会造成电压跌落，如果STM32和电机共用同一个电源，单片机会时不时复位，或者编码器读数在电机动作瞬间出现毛刺。后来我把电机电源和逻辑电源完全分开，电机用12V开关电源，STM32用独立的5V/3.3V供电，同时把电机电源线的GND和逻辑板GND单点连接，干扰问题一下就缓解了很多。
2.3 间隙问题：一个非常容易被忽略的机械细节
倒立摆系统对机械间隙极其敏感。皮带传动里有皮带弹性形变和齿间间隙，丝杆传动里有丝杆与螺母的间隙，这些间隙在控制上等效于“死区”。死区会导致杆已经出现微小角度偏差时，电机的修正力要等间隙被“吃掉”之后才传到小车上，这个滞后会让系统产生持续的低频抖动，甚至无法收敛。
如果用的是实验室现成的装置，装配精度通常还行。如果是自己搭建的装置，建议选用预紧滚珠丝杆或张紧度合适的同步带，尽量避免用普通梯形丝杆。实测下来，机械间隙对控制稳定性的影响，有时候比PID参数没调准还致命。
3. 控制算法选择：并联PD上手，LQR做进阶
3.1 简化数学模型
在写程序之前，至少要对被控对象有一个数学模型。设小车质量为M，摆杆质量为m，摆杆长度的一半为l，重力加速度为g，小车位移为x，摆杆相对垂直方向的角度为θ，电机推力为F。在小角度范围内线性化之后，运动方程大致可以写成：
(M+m)·x¨ + m·l·θ¨ = F m·l·x¨ + (J + m·l²)·θ¨ = m·g·l·θ
其中J是摆杆绕铰接点的转动惯量。写成状态空间形式后，状态变量可以取[x, θ, ẋ, θ̇]，输入u就是电机推力F。这个方程不一定要自己手推，许多控制理论教材和网上的开源资料里都有现成的倒立摆模型，但看懂它，对于后面选择控制策略和调试方向很有帮助。
3.2 并联PD控制律与整定顺序
对很多第一次做倒立摆的人来说，一上来就整LQR容易“死得很难看”，因为LQR需要四个状态都能比较准确地估计，而实际传感器里速度量往往是差分出来的，噪声大，直接用LQR容易在增益稍大时把系统激振。我更推荐先用“并联PD”的思路把系统跑通，再考虑更高阶的算法。
所谓并联PD控制律，本质上就是把角度和位置两个闭环叠加起来：
u = Kp_θ·θ + Kd_θ·θ̇ + Kp_x·(x - x_ref) + Kd_x·ẋ
这个公式的含义是：角度偏了，用推力把它“顶”回去；角速度告诉控制器杆正在往哪个方向倒，提前施加阻尼；位置项保证小车不会一直往一边跑；位置速度项让小车移动不那么冲。四个增益各有分工，调试起来直觉很强。
我的整定顺序是：先只保留角度项(Kp_θ、Kd_θ)，把位置项全部置零。这时小车会为了稳定摆杆而不断移动，但这没关系，先让角度能稳定住。用手轻轻拨动摆杆，观察它能不能快速回到竖直位置，如果出现高频抖动，增加Kd_θ；如果杆缓慢地往一边倒、修不回来，增加Kp_θ。当角度环能稳定之后，再逐步加入位置项。加入位置项时从很小的Kp_x开始，让小车缓慢地回到导轨中间，如果小车来回振荡，再加大Kd_x。
这套方法我用下来非常有效，比直接照抄别人的LQR增益靠谱得多，因为不同装置的机械参数差异很大，只有从实际系统出发调整，才能得到真正适合当前装置的参数。
3.3 LQR状态反馈的落地方法
当并联PD已经能把杆立住之后，如果想把系统性能做得更好，可以考虑LQR。LQR的本质是求一个状态反馈增益向量K，使得控制量u = -K·[x, θ, ẋ, θ̇]^T在某个二次型性能指标下最优。这里的Q矩阵反映你对各个状态的重视程度，R矩阵反映你对控制能量的限制。
实际工程中不需要手算K，直接用MATLAB、Python里的控制库，输入状态矩阵A、B，再给定Q和R，就能求出K。例如我当时的初值选择是Q = diag([100, 100, 10, 1])，R = 1，然后根据实际振荡情况微调。但注意，LQR的前提是状态反馈量都“干净”，尤其是速度量，如果差分噪声太大，LQR的优势根本发挥不出来，甚至不如手动调的PD。所以建议在跑LQR之前，先对编码器原始值做平滑滤波，并在代码里把速度量也一起实时查看。
4. STM32程序设计：从编码器计数到PWM输出的完整链路
4.1 定时器的编码器模式配置
在CubeMX里，把TIM3设置成Encoder Mode，选择“Combined CH1 and CH2”作为计数源，并把计数周期设成65535（16位）。这样A相每来一个上升沿，计数器就自动加减，完全不用软件参与。初始化之后，直接读取__HAL_TIM_GET_COUNTER(&htim3)就能拿到底层的原始计数值。
编码器计数溢出问题很关键。16位计数器的范围是0到65535，小车来回跑的时候，计数器会从65535跳回0，如果直接当作有符号数处理，可能会在边界处出现好几万计数的一跳。解决办法是直接用int16_t类型去读取寄存器值，这样65535读出来就是-1，正好是真实方向上的连续值。
角度单位换算参考这样写：
#define ENCODER_PPR 2000.0f // 500线 * 4倍频 #define ANGLE_PER_COUNT (2.0f * 3.1415926f / ENCODER_PPR) #define ANGLE_OFFSET_RAD 0.0f // 机械零点偏移 float GetPendulumAngle(void) { int16_t raw = (int16_t)__HAL_TIM_GET_COUNTER(&htim3); return (float)raw * ANGLE_PER_COUNT + ANGLE_OFFSET_RAD; }位置编码器的换算类似，只是系数从“每计数多少弧度”换成“每计数多少米”。如果你用的是同步带轮，带轮直径为D，每转走π·D米，那么：
#define POS_PER_COUNT (3.1415926f * PULLEY_DIAMETER_M / ENCODER_PPR)4.2 1kHz控制中断的实现细节
倒立摆的控制循环我放在1kHz的定时器中断里执行，也就是每1毫秒跑一次“采样-控制-输出”的完整链路。为什么不放主循环while里跑？因为主循环里一旦有串口发送或者LCD刷新之类的耗时操作，控制周期就会变得极不稳定，倒立摆对这种时序抖动非常敏感。
我用定时器TIM6作为控制节拍，配置为1毫秒溢出一次，开中断。在中断回调函数里执行核心控制逻辑：
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { if (htim->Instance == TIM6) { // 1. 采样 float x = GetCartPosition(); float theta = GetPendulumAngle(); // 2. 计算速度（一阶差分 + 低通） float vx = (x - x_last) / DT; float vth = (theta - theta_last) / DT; vx = 0.8f * vx + 0.2f * vx_filtered; vth = 0.8f * vth + 0.2f * vth_filtered; // 3. 控制律（并联PD） float u = KP_THETA * theta + KD_THETA * vth + KP_POS * (x - X_REF) + KD_POS * vx; // 4. 输出限幅 if (u > MAX_U) u = MAX_U; if (u < -MAX_U) u = -MAX_U; // 5. 转成PWM占空比输出给电机驱动 SetMotorPWM(u); // 6. 保存本次状态 x_last = x; theta_last = theta; vx_filtered = vx; vth_filtered = vth; } }代码里的DT是采样周期，等于0.001秒。速度项如果直接从差分取，噪声会很大，所以我加了一阶低通滤波，让控制量更平滑。实测中这个低通滤波器的系数对稳定性影响挺大，系数太大会滞后太多，系数太小噪声又压不住，一般取在0.6到0.9之间比较合适。
4.3 实时性、优先级和“万一没跑完”的处理
在STM32上做倒立摆，中断优先级的分配非常关键。我把TIM6的控制中断设置成抢占优先级最高，这样即使串口正在发送数据，也绝不允许打扰控制循环。如果串口中断优先级比控制中断还高，就会出现一种诡异现象：程序每输出一帧日志，小车就抖一下。
另外，一旦控制环计算耗时超过1毫秒，定时器中断就会“堆叠”，造成控制节奏混乱。建议在控制代码里避免做浮点库函数调用、除法等耗时操作，能用查表的尽量查表，能用乘法的别用除法。实测STM32F103在72MHz主频下，上面那段代码跑一遍大约是10到20微秒，余量很充足。
还有一件事必须做：安全保护。如果摆杆角度超过一定范围，比如±30度，系统已经没有能力救回来了，这时候要立即封锁PWM输出，让电机停机，否则小车会加速冲向轨道末端，轻则撞坏限位，重则把摆杆摔断。我加了这样一个保护逻辑：
if (fabsf(theta) > THETA_SAFE_LIMIT) { SetMotorPWM(0); control_enabled = 0; // 锁定输出，需要手动复位 }这类“保命逻辑”看着简单，但在实物调试时真的能帮你省下不少换零件的钱。
5. 调试记录：上位机看波形、两步调参、三类常见坑
5.1 用串口把状态变量送出来观察波形
调倒立摆最忌讳“盲调”，就是不知道当前角度、位置、速度是多少，靠感觉去试PID参数。我强烈建议写一个底层调试接口，把角度、位置、控制量等关键变量通过串口以文本或二进制帧的形式发到上位机，然后用串口示波器软件画波形。
对STM32来说，最简单的方式就是重定向printf到USART，定时打印一行：角度、角速度、位置、速度、控制量。波特率115200完全够用。我用的是串口绘图工具，把数据按列存成CSV再画图。当杆子出现持续振荡时，从波形上能明显看到角度的正弦波动，这时应该拉大阻尼项；当小车缓慢漂移不回来时，波形上角度有一个固定偏移，就要加大位置项增益。
提示：调试阶段不要急于追求“完美立住”，先看波形趋势，理解每个参数在波形上体现为什么形态，后面调参会顺畅得多。
5.2 参数整定的实操顺序
我最终把参数整定分成两个阶段。第一个阶段是“角度闭环稳定”：位置项全部为0，只保留Kp_θ和Kd_θ，目标是让杆在受到扰动后能快速回正，即使小车会乱跑也没关系。判断标准是把杆稳定后，用手在摆杆中部轻轻弹一下，杆在1到2个来回内稳定下来，而不是持续振荡。
第二个阶段是“位置闭环回归”：逐步加入Kp_x和Kd_x。先给Kp_x一个很小的值，例如让小车每周漂移几厘米能拉回来，再慢慢加大。注意Kp_x加大的过程中，角度项的Kd_θ通常也需要跟着微调，因为位置环和角度环是耦合的。实测中，如果位置环增益加得太快，会使系统整体出现低频摇摆，这时候不能只加Kp_x，要适当增加Kd_θ或者Kd_x。
5.3 编码器突变、飞车、响应迟钝的排查思路
我遇到过最诡异的问题是编码器读数偶发跳变。波形上表现为角度突然跳一个很大的值，电机瞬间打满，小车猛地一冲。排查下来有两个原因：一是编码器线太长且没有屏蔽，电机正反转时干扰耦合进编码器信号；二是编码器A/B相插头接触不良。
解决办法是：编码器线尽量短，使用屏蔽双绞线，屏蔽层单端接地；接插件选带锁扣的型号，防止振动脱落。如果你用的是延长线，一定要检查是否有虚焊。
还有一类“飞车”问题，是控制程序里的整数溢出造成的。比如角度换算系数写错，导致角度量纲不对，控制器以为杆偏了好几度，实际上偏了不到一度，电机就会输出巨大推力。这时候一定要回到波形界面，确认角度、位置的原始数值量级是否正确。
响应迟钝通常有两个来源：一是控制中断被其他高优先级中断频繁打断，可以通过关掉串口打印观察是否恢复；二是速度滤波系数拉得太大，导致控制环感知到的角速度严重滞后，系统相位余量不足。把滤波系数调小一点，或者改用更高带宽的传感器，问题会明显好转。
6. 让系统真正“立得住”的几条经验
6.1 把机械间隙和摩擦力“喂”给算法
机械间隙没法完全消除时，可以在控制律里加一点“死区补偿”。我的做法是：当计算出的控制量u特别小，比如小于电机启动死区对应阈值时，直接把u设成0；当u大于阈值时，在u上叠加上一个小的固定补偿量，抵消摩擦和间隙带来的死区。这个补偿量要用低速斜坡测试测出来，不能拍脑袋。
这个技巧表面上看起来很低级，但它能让系统在小偏差情况下的极限环振荡明显减小，稳定性上了一个台阶。
6.2 让控制程序更皮实的几个小改动
最后再分享几个让系统“更耐造”的改动：
- 启动时先保持输出0，等编码器上电稳定后再开启控制循环，避免上电瞬间误动作。
- 控制中断里不打印任何调试信息，所有调试数据先缓存到缓冲区，待主循环空闲时再发送。
- 给PWM输出做斜坡限制，防止控制量瞬间从零跳到满值，减小对机械和驱动板的冲击。
- 对速度量使用“中值滤波 + 一阶低通”的组合，编码器偶尔跳一个毛刺时能直接滤掉，不至于让控制器“抽搐”一下。
直线倒立摆这个项目，做到最后你会发现，它真正考验的不是你背了多少控制理论公式，而是你能不能把模型、硬件和代码串成一个“有即时应答能力”的整体。我的感受是，与其纠结于用LQR还是用PD，不如先把最基本的采样链路、数据观测和参数整定流程做扎实。毕竟，一个调试工具完善的并联PD系统，往往比一个没有观测手段的LQR系统立得稳得多。
本文还有配套的精品资源，点击获取
