简介直线倒立摆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。在小角度范围内线性化之后运动方程大致可以写成(Mm)·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”作为计数源并把计数周期设成6553516位。这样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系统立得稳得多。本文还有配套的精品资源点击获取