简介一套基于STM32F10x的直线倒立摆控制程序面向嵌入式控制初学者、自动化专业学生以及机电一体化开发者核心解决倒立摆的实时平衡控制问题。工程在经典PD、PID基础上引入微分先行、梯形积分、抗积分饱和与积分分离等优化策略并配置了完整的系统初始化、硬件外设驱动GPIO、ADC、PWM和传感器数据处理代码。包内目录划分清晰BALANCE模块负责平衡控制算法HARDWARE为底层驱动SYSTEM包含时钟与中断配置USER存放应用逻辑便于按模块阅读和复用。资源包共214个文件包括55个头文件、48个C源文件、26个d和26个o等编译中间文件、Keil工程文件uvprojx/uvopt及hex可执行文件另有htm说明文档和txt说明整体约3.79MB。目前已有818人学习浏览适合课程设计、毕业设计或竞赛作品参考也可作为平衡控制算法验证平台。通过阅读该工程还能理解STM32标准外设库的调用方式、控制算法在嵌入式环境中的离散化实现以及模块化工程组织方法是学习实时控制与嵌入式开发的实用参考。1. 直线倒立摆不是玄学STM32控制程序到底能帮你省掉什么第一次接触直线倒立摆的人十有八九是在课程设计或实验室里被它折磨过的摆杆装上去了电机也转了但小车就是不能把摆杆稳稳立在竖直位置。很多人下载了现成的STM32控制程序烧进去发现摆杆疯狂抖动两秒然后砸下来第一反应是代码有问题第二反应是硬件坏了。实际上直线倒立摆是一个典型的“系统匹配”问题底层机制、传感器选型、控制周期和算法参数缺一不可。这份STM32控制程序把机械结构、编码器接口、PWM驱动、角度环和速度环的完整逻辑都打包好了我拆开看过工程结构清晰核心控制代码带注释适合想快速复现并在这个基础上调参数的人。省掉的不是写代码的时间而是你从零摸清“倒立摆为什么能站起来”这条思路的时间。新手能照着接好线、烧写、调参熟手可以直接改控制律拿它当算法验证平台。2. 控制对象与硬件选型从机械结构到STM32外设的匹配很多嵌入式项目可以先写代码再考虑硬件但直线倒立摆不行。它的控制效果很大程度取决于机械精度和传感器分辨率如果硬件选型不对后续算法再怎么调都救不回来。这一章先讲清楚控制对象是什么再说明程序里每一个外设为什么这样配。2.1 直线倒立摆的物理模型与状态量直线倒立摆的经典结构是一条水平导轨导轨上有一个小车小车上固定一根可以自由旋转的摆杆。电机带动小车在导轨上左右移动通过给小车施加水平力来控制摆杆的角度。理想情况下不考虑摩擦和空气阻力摆杆的运动方程可以写成(M m) * x_ddot m * L * theta_ddot * cos(theta) - m * L * theta_dot^2 * sin(theta) F m * L * theta_ddot * cos(theta) m * L^2 * theta_ddot - m * g * L * sin(theta) 0其中 M 是小车质量m 是摆杆质量L 是摆杆质心到旋转轴的距离x 是小车位移theta 是摆杆与竖直方向的夹角。在摆杆稳定在竖直位置附近时theta 很小可以近似 sin(theta) ≈ thetacos(theta) ≈ 1并把高阶项去掉得到一个线性状态空间方程。这个过程是整个控制程序的理论基础——最终控制律都是在这个线性化模型上设计的。程序里读到的状态量通常有四个小车位置 x、小车速度 x_dot、摆杆角度 theta、摆杆角速度 theta_dot。前两个来自导轨一端的编码器或拉线传感器后两个来自摆杆旋转轴上的编码器。这个资源中默认使用的是增量式编码器通过 STM32 的定时器编码器模式直接读取不需要额外搭解码电路。2.2 电机、编码器、驱动器与STM32的接口设计选电机时这个程序针对的是直流减速电机带有霍尔编码器或光电编码器。减速比一般在 30:1 以上否则小车加速太快控制精度跟不上。电机的功率不需要太大但扭矩要能拖动小车和摆杆。驱动部分用的是典型的 H 桥驱动模块比如常见的 L298N 或者 TB6612。STM32 输出 PWM 信号控制电机转速输出两个 GPIO 信号控制正反转。编码器的接口是这个项目里最容易接错的地方。如果用的是霍尔增量式编码器它有 A、B 两路输出直接接 STM32 的定时器通道。以某常用定时器为例PA0 和 PA1 可以配置成 TIM2_CH1 和 TIM2_CH2开启编码器模式后定时器计数器值会随电机转动自动增减不需要中断轮询。这个程序里默认编码器分辨率为 13 线/圈经过 4 倍频后电机轴每转一圈产生 52 个脉冲再乘上减速比就是轮子实际转动带来的位移分辨率。如果发现位置反馈跳动优先检查编码器 A/B 相接反没有接反会导致方向判断混乱控制正反馈摆杆会越调越倒。电机驱动部分的接线可以按下面的引脚对应关系来确认信号STM32引脚说明PWM-IN1PA8定时器1通道1输出PWMPWM-IN2PA9定时器1通道2输出反向PWMDIR-1PB0方向控制高电平正转DIR-2PB1方向控制低电平反转ENCODER-APA0定时器2通道1ENCODER-BPA1定时器2通道2不要直接拿 STM32 的 3.3V 引脚驱动电机驱动模块的逻辑端很多 H 桥模块需要 5V 逻辑电平。程序里默认在初始化 GPIO 时把引脚设置为复用开漏输出并外接上拉电阻这个细节很容易被复制代码的人忽略结果就是 PWM 信号出来了但电机不动。2.3 程序框架定时器、PWM与中断优先级安排整个控制程序跑起来依赖几个关键外设一个定时器产生 PWM一个定时器工作在编码器模式一个定时器产生固定周期的控制中断。倒立摆控制的本质是一个周期性任务每个控制周期读取编码器计算角度和位置通过控制律得到控制量更新 PWM 占空比。控制周期越短系统可以处理的角速度范围越大但对传感器分辨率和计算开销也越苛刻。这个程序选的是 1kHz 控制频率也就是 1ms 一个中断周期。中断优先级安排是常见的隐藏坑。PWM 中断和编码器中断如果优先级设置不当会导致控制周期抖动。这个程序把控制周期定时器中断优先级设为最高抢占优先级为 0子优先级为 0编码器更新中断设为次高。因为编码器工作在硬件计数模式其实不需要中断只有定时器溢出时才需要读取。串口中断优先级最低避免收发数据打断控制周期。控制循环的骨架大致是这样的void ControlTASK_Init(void) { TIM_TimeBaseInitTypeDef TIM_InitStruct; RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); TIM_InitStruct.TIM_Prescaler 84 - 1; // 84MHz时钟分频到1MHz TIM_InitStruct.TIM_Period 1000 - 1; // 1kHz中断频率 TIM_InitStruct.TIM_ClockDivision TIM_CKD_DIV1; TIM_InitStruct.TIM_CounterMode TIM_CounterMode_Up; TIM_TimeBaseInit(TIM3, TIM_InitStruct); TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE); NVIC_InitTypeDef NVIC_InitStruct; NVIC_InitStruct.NVIC_IRQChannel TIM3_IRQn; NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority 0; NVIC_InitStruct.NVIC_IRQChannelSubPriority 0; NVIC_InitStruct.NVIC_IRQChannelCmd ENABLE; NVIC_Init(NVIC_InitStruct); TIM_Cmd(TIM3, ENABLE); } void TIM3_IRQHandler(void) { if (TIM_GetITStatus(TIM3, TIM_IT_Update) ! RESET) { TIM_ClearITPendingBit(TIM3, TIM_IT_Update); Encoder_Read(); // 读取编码器计数值 Control_Update(); // 计算控制量 Motor_SetPWM(); // 更新PWM输出 } }参数说明TIM_Prescaler 设为 83 是因为系统主频 84MHz分频后计数时钟为 1MHzTIM_Period 为 999计数 1000 次产生一次更新事件得到 1kHz 中断。如果主频不同这两个参数需要按实际值换算否则控制周期会偏整定好的参数会失效。3. 控制算法落地从PID到LQR的实现与参数整定硬件接口通了以后真正的核心是控制算法。这个资源同时提供了 PID 和 LQR 两种实现并且把切换方法写在注释里。新手用 PID 起步熟手可以改状态反馈矩阵。这一章把两条路都拆开给出可以直接抄的代码以及参数怎么从无到有地调出来。3.1 PID控制为什么能稳住倒立摆倒立摆系统是一个开环不稳定的系统摆杆稍微偏离竖直位置重力就会让它继续倒下。PID 控制器的思路是检测到角度偏差输出一个力让小车加速使摆杆恢复平衡。单环 PID 往往不够用因为只控制角度会让小车越走越偏最后冲出导轨。所以这个程序采用串级 PID 结构内环控制角度外环控制小车位置。角度环的输入是摆杆角度偏差输出是小车速度给定。位置环的输入是位置偏差输出作为角度环的修正量。换句话说当小车偏离导轨中心时角度环的目标值会被修正让摆杆略微倾斜产生一个加速度把小车拉回中心。这种结构在工程里很常用。看一下角度环的实现代码float AnglePID_Calc(float target_angle, float measured_angle) { float error target_angle - measured_angle; float p_out angle_kp * error; float d_out angle_kd * (error - last_angle_error) / dt; last_angle_error error; return p_out d_out; }逻辑说明这里没有积分项是因为倒立摆角度环的主要目标是快速抑制偏差积分作用反而容易引入超调和振荡。实际调试中只需要调整 angle_kp 和 angle_kd 两个参数。参数说明angle_kp 决定恢复力的大小设太小时摆杆软绵绵地倒下设太大时摆杆高频抖动。angle_kd 提供阻尼抑制角速度过大导致的过冲。一般先给 kp 一个较小值让它勉强能撑住然后逐渐加大 kd 消除抖动。位置环也是一个类似的比例加微分控制器输出量叠加到目标角度上float PosPID_Calc(float target_pos, float measured_pos) { float error target_pos - measured_pos; float p_out pos_kp * error; float d_out pos_kd * (error - last_pos_error) / dt; last_pos_error error; return p_out d_out; }整个控制量计算流程是先算位置环输出用这个值修正角度环目标再算角度环输出得到一个速度给定最后把速度给定换算成 PWM 占空比限幅后送给电机。注意这里的两个 PID 都是 PD 形式积分项被故意去掉了因为倒立摆工作在平衡点附近稳态误差主要由摩擦力等非线性因素造成加积分项容易和微分项打架。3.2 LQR控制器状态反馈矩阵的计算与离散化PID 调参需要经验LQR 则提供了更系统的设计方法。LQR 是对线性状态空间模型求解一个最优状态反馈矩阵 K使代价函数 J ∫(x^T Q x u^T R u) dt 最小。Q 矩阵对应各个状态的权重R 矩阵对应控制量的权重。这个程序里给出的方法是先离线算好 K 矩阵再写入代码而不是在 STM32 里做矩阵运算。在 MATLAB 或 Python 里计算 K 矩阵的代码import numpy as np from scipy import linalg # 直线倒立摆线性化后的状态矩阵自行根据物理参数计算 A np.array([[0, 1, 0, 0], [0, 0, -3.0, 0], [0, 0, 0, 1], [0, 0, 30.0, 0]]) B np.array([[0], [1.0], [0], [5.0]]) Q np.diag([100.0, 10.0, 200.0, 20.0]) R np.array([[1.0]]) K linalg.solve_continuous_are(A, B, Q, R) K np.linalg.inv(R).dot(B.T.dot(K)) print(K matrix:, K)逻辑说明A 矩阵的第二行第三列 -3.0 代表摆杆角度对小车的耦合项第四行第三列 30.0 是重力效应项。不同摆杆长度和质量的 A 矩阵数值不同但结构是一样的。Q 矩阵里摆杆角度的权重远大于小车位置因为控制的首要目标是稳定摆杆位置靠后。R 矩阵代表电机出力代价R 越小控制越激进但 PWM 更容易饱和。得到 K [k1, k2, k3, k4] 后控制量 u -K · x。在 STM32 里实现时状态向量就是四个反馈值。因为控制周期固定为 1ms可以离散化后直接算void LQR_Control(void) { float u - (lqr_k1 * state_x lqr_k2 * state_v lqr_k3 * state_theta lqr_k4 * state_omega); if (u PWM_MAX) u PWM_MAX; if (u -PWM_MAX) u -PWM_MAX; Motor_SetPWM((int16_t)u); }参数说明lqr_k1 到 lqr_k4 就是上面算出的 K 矩阵元素。状态量单位要注意如果角度用手算的弧度位置用米那么 K 矩阵也对应这套单位。程序里默认编码器读取的计数脉冲值需要转换成米角度转换成弧度否则控制量数量级不对。3.3 代码结构各模块如何协同打开工程目录核心文件有几个main.c 负责初始化和主循环encoder.c 负责编码器数据处理motor.c 负责 PWM 和方向控制control.c 负责控制算法uart.c 负责与上位机通信。工程文件里还有一份 README说明引脚映射和怎么改物理参数。主循环里没有复杂的逻辑真正的工作都放在中断里int main(void) { SystemInit(); GPIO_Config(); Timer_Init(); Encoder_Config(); Motor_Config(); UART_Init(); Control_Struct_Init(); while (1) { // 主循环仅在必要时执行低优先级任务 if (uart_rx_flag) { uart_rx_flag 0; Param_UpdateFromHost(); // 通过串口修改参数 } } }这段代码的意义是明确分层控制逻辑全部在中断里执行主循环只处理串口通信和参数修改。这样避免了主循环中的任何一个程序分支拖慢控制周期。控制周期里的 Encoder_Read() 函数有两个关键点一个是读取摆杆编码器要处理零点漂移另一个是读取小车编码器要考虑方向反转。程序里通过编码器的计数方向直接判断小车运动方向因此初始化时必须用当前编码器值作为位置零点否则小车一上电就跑偏。4. 调试与避坑编码器噪声、零点漂移、电机死区等常见问题排查写代码可以靠逻辑推演调倒立摆就是纯体力活加眼力活。我拆过这个程序后把最容易出现的四个问题按现象列出来每个都给出了排查路径。你遇到的问题大概率是其中一种。4.1 现象小车往一个方向顶死电机持续发热这是最常见的翻车现场。上电后摆杆还没立起来小车就疯了一样往导轨一端冲直到撞到限位开关。原因有两个一是摆杆编码器的零点没有对准竖直位置。程序默认上电瞬间的角度是零度但如果机械上摆杆此时不是竖直的控制程序会误以为摆杆偏差很大于是朝一个方向猛推小车。二是 PWM 方向接反了导致负反馈变成了正反馈。解决方法是先进入“测试模式”把程序里的控制量输出固定为一个很小的值观察电机转向是否与编码器方向相反。然后手动把摆杆拨到竖直位置在程序里读取当前编码器值把这个值写入 Angle_Zero_Offset。有些程序支持通过按键设置零点下载的这个工程里在串口指令中定义了 ZERO 命令发送后程序会把当前编码器值记录到 Flash。从那以后我每次给摆杆做机械维护后都会强制走一遍零点校准流程。4.2 现象摆杆高频抖动角度数据跳变抖动说明控制回路有噪声或控制增益过高。拿着示波器或通过串口把角度数据打印出来如果看到角度值在 0 附近左右跳变而不是平滑变化基本可以断定是编码器信号噪声。增量式编码器的 A/B 线如果和电机电源线走在同一个线槽里电机换向时的电流冲击会耦合到编码器信号上。解决方法有几个把编码器线单独走远离电机线在编码器电源引脚加 100nF 去耦电容或者在程序里对编码器计数值做滤波。这个程序里提供了一个简单的滑动平均滤波函数适用于角度信号。注意滤波会增加延时滤波窗口太大会让控制相位滞后所以窗口长度不要超过 3 个采样点。我当时调这步时发现光是给编码器线绞合就能消除大半跳变。4.3 现象能直立但抗扰性差一推就倒摆杆能立住但轻轻一碰就大幅度摆动甚至倒掉。这说明控制器的阻尼作用不够。在 PID 模式里优先加大 angle_kd在 LQR 模式里相当于需要在 Q 矩阵中加大角速度的权重。如果你已经用了默认参数但系统仍然很软还有一个隐藏原因是控制周期不对。这个程序默认 1kHz但如果有人把 main.c 里的系统时钟改了比如超频到 168MHz而定时器分频没有相应调整实际控制周期会变短或变长。判断方法很简单程序里有一个毫秒计数变量用秒表对比串口打印的时间戳。如果发现每 1000 个周期的时间明显不是 1 秒需要重新计算定时器分频值。常见做法是在初始化时用逻辑分析仪抓一个 PWM 翻转波形来验证周期。4.4 现象程序烧进去没反应电机不转这个问题多数不是程序逻辑错误而是硬件配置问题。检查点按顺序来先量驱动模块逻辑电源是否 5V再看 STM32 的 PWM 引脚是否有波形输出最后看编码器是否插反。我在拆这个工程时发现它的电机驱动部分默认是共阳接法需要把驱动模块的跳线帽调整到对应模式如果跳线帽不对PWM 信号会被拉到地电机自然不动。另外要注意 GPIO 的复用配置。这个程序用了定时器复用功能如果漏掉了 GPIO_PinRemapConfig或者把引脚占用了PWM 不会从正确的引脚输出。程序里有供电前自检流程如果某个外设初始化失败LCD 或串口会打印错误码。先把自检跑通再碰控制逻辑。5. 进阶让倒立摆更稳的四个技巧与验证方法当你把基础版调通以后摆杆能立起来但总感觉它像站在气球上。我在复现过程中尝试了四个改进效果明显而且改动量都不大。第一个技巧是把编码器角度信号和陀螺仪做互补滤波。纯编码器的分辨率有限在高速摆动时角速度信号会有滞后和噪声。如果摆杆上集成了一颗六轴传感器可以用加速度计估算角度再用陀螺仪估算角速度与编码器融合。互补滤波的代码我写在控制中断里float angle_fused alpha * (last_angle gyro_rate * dt) (1.0f - alpha) * accel_angle;alpha 的物理意义是信任陀螺仪的比例一般在 0.95 到 0.98 之间。alpha 越大融合后的角度越平滑但对加速度计的响应越慢。这个参数需要和编码器噪声水平折中。第二个技巧是加入摩擦力前馈。直线导轨和齿轮箱的静摩擦力会让小车在低速时“卡顿”控制量小于静摩擦阈值时小车不动大于阈值后又突然窜动。在算出控制量后加上一个与运动方向相反的常量补偿if (current_speed 0) u friction_comp; else if (current_speed 0) u - friction_comp;friction_comp 的取值靠实验观察让程序输出一个固定 PWM然后从小逐渐增大记录小车刚好开始移动的 PWM 值乘一个 0.8 的系数作为补偿量。第三个技巧是使用串口上位机在线调参。工程里包含了上位机通信协议格式是“参数名数值”字符串控制中断的快照通过 DMA 发送到串口。我通常用匿名上位机展示角度曲线边推摆杆边观察波形边改参数比反复烧写程序效率高得多。第四个技巧是增加一个“软限位”保护逻辑。导轨两端有机械限位开关但倒立摆惯性大撞上去容易损坏机械结构。程序里在位置环中加入了缓冲区域当小车位置接近导轨末端时自动把位置权重加大强制让小车减速。验证方法我习惯分三步走第一步让摆杆从 30 度偏角释放记录恢复时间好的参数应该在 1 秒内摆回竖直位置并且不出现大的过冲第二步在摆杆稳定后用手拨动观察能在几次摆动内恢复第三步在小车匀速运动过程中给摆杆一个阶跃干扰看角度偏差最大值是否能控制在 3 度以内。这套方法我现在每个新参数组必跑一遍。调倒立摆最忌没有章法地乱试。从那以后我每次拿到新的控制程序都会强制走一遍零点校准、控制周期验证、编码器噪声排查这三件事再碰任何参数。希望帮到你剩下的交给时间和耐心。本文还有配套的精品资源点击获取
