简介一个基于 STM32F10x 的直线倒立摆控制工程面向倒立摆实验与嵌入式实时控制学习场景完整覆盖传感器数据读取、控制算法计算和电机驱动链路的实现。压缩包解压后可见 BALANCE、HARDWARE、SYSTEM、USER、STM32F10x_FWLib、OBJ 等目录BALANCE 存放平衡算法HARDWARE 封装 GPIO、ADC、PWM 等外设驱动SYSTEM 负责时钟与中断等系统初始化USER 为应用层代码STM32F10x_FWLib 提供标准外设库OBJ 保存中间编译结果。控制部分不只包含 PD、PID 基础算法还涉及微分先行、梯形积分、抗积分饱和、积分分离等优化策略可对照源码理解每一项在防超调、消稳态误差和抑制积分饱和上的作用。资源共 214 个文件核心为 55 个头文件和 48 个 C 源文件另有 Keil 工程文件、编译中间文件、hex/axf 可执行镜像以及 map、sct 等链接描述文件整体约 3.79MB已有 818 人学习/下载。目录分层清晰、代码与标准库分离适合准备倒立摆实验或学习 STM32 实时控制的中高级开发者。1. 直线倒立摆 STM32 控制程序先看它在做什么第一次见到直线倒立摆实物时第一反应是这东西怎么敢叫“直线”——一截轨道上一辆小车驮着摆杆来回跑杆子直立不倒看着确实像违反物理直觉。等自己上手调一次就明白了能在 1 秒内把自由下落的摆杆稳稳“立”起来再顶着外力扰动自动回正背后靠的是 STM32 在 1 kHz 中断里反复执行“读编码器、算偏差、算 PID、输出 PWM”这一整套闭环。这份资源不是仿真是一份能直接烧进板子的裸机工程适合两类人一类是刚学完 STM32 外设、想找一个带真实控制闭环的综合性项目的学生另一类是已经在做平衡小车或云台、想倒回去看经典倒立摆控制结构怎么写、怎么调参的开发者。全文从硬件接线、PID 核心实现到避坑调试都会讲到照着烧录、接好编码器和电机驱动就能跑起来。2. 系统架构与硬件选型轨道、小车、摆杆与主控是怎么连成一体的2.1 机械结构决定控制难度直线倒立摆和常见的平衡小车的最大区别在于小车的运动被限制在一根直线轨道上摆杆通过旋转编码器安装在小车顶部电机通过同步带或齿轮驱动小车沿轨道左右移动。控制目标是让摆杆在垂直位置附近保持平衡同时让小车不要冲出轨道限位。这个机械结构直接决定了控制策略的维度。单级直线倒立摆是一个典型的四阶非线性系统状态量包括小车位置、小车速度、摆杆角度、摆杆角速度。但因为轨道限制了 Y 轴和旋转自由度系统的控制输入只有“小车加速度”这个单通道所以它比二级摆、环形摆简单又比纯仿真复杂——真实世界里的摩擦力、电机死区、编码器噪声全都叠在控制回路上。这也是这个项目最适合作为嵌入式控制入门的原因数学上能算清楚物理上又能让你老老实实调 PID。2.2 主控与外设分工从工程角度说这份资源的核心主控是 STM32F103 系列属于 Cortex-M3 内核72 MHz 主频。在倒立摆这种需要强实时性的控制场景里这个主频已经足够——控制周期 1 kHz每个周期内要完成的动作并不多但要求中断响应确定、时序稳定。使用定时器触发 ADC 或编码器采样而不是在主循环里去轮询这是整个程序实时性的关键。外设层面摆杆角度通过旋转编码器读取常见配置是 500 线增量式编码器配合 STM32 的定时器编码器模式4 倍频后每圈 2000 个脉冲。小车位移则通过轨道上的直线编码器或电机自带的霍尔编码器获得。电机驱动部分通常使用 TB6612 或 DRV8870 这类带逻辑电平输入的驱动器STM32 输出 PWM 控制电机转速和方向。显示和交互部分一般预留了 OLED 屏幕和按键用于运行时切换 PID 参数。2.3 控制回路整体数据流整个程序的数据流可以画成一条直线编码器中断读取角度和位置经过滤波和微分计算速度送入 PID 控制器计算出控制量控制量映射为 PWM 占空比和方向引脚电平输出到电机驱动。这条链路每 1 ms 循环一次中间不能有阻塞操作也不能有动态内存分配这类不确定行为。中断服务函数时序伪代码: 定时器每 1ms 触发一次 - 读编码器计数 - 换算角度/位置 - 计算偏差 - PID 计算 - 更新 PWM 寄存器 - 清除中断标志这里有一个嵌入式新手特别容易忽视的点PID 的积分项、微分项必须在定时中断里连续计算不能漏周期。哪怕主循环里正在处理串口数据或刷新 OLED 屏幕中断依然要准时触发。所以这份工程把整个控制算法全部写在定时器中断服务函数里主循环只做参数显示和状态上报。中断的优先级、频率和初始化顺序我放在后面核心代码里说。3. 核心模块代码拆解编码器采集到 PWM 输出的完整链路3.1 定时器编码器模式的初始化STM32 的定时器编码器模式是倒立摆项目里第一个值得学习的点。用 TIM4 的 CH1 和 CH2 分别接编码器的 A、B 相硬件上自动完成正交解码和计数不需要外部中断去手动判断相位。这种方式的优势是计数过程不占用 CPU 时间而且 4 倍频直接提高了角度分辨率。// 编码器模式初始化 - TIM4, 通道1/2接增量式编码器A/B相 void Encoder_Init(void) { GPIO_InitTypeDef GPIO_InitStructure; TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_ICInitTypeDef TIM_ICInitStructure; RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); GPIO_InitStructure.GPIO_Pin GPIO_Pin_6 | GPIO_Pin_7; // PB6A相, PB7B相 GPIO_InitStructure.GPIO_Mode GPIO_Mode_IN_FLOATING; GPIO_Init(GPIOB, GPIO_InitStructure); TIM_TimeBaseStructure.TIM_Period 0xFFFF; TIM_TimeBaseStructure.TIM_Prescaler 0; TIM_TimeBaseStructure.TIM_ClockDivision TIM_CKD_DIV1; TIM_TimeBaseStructure.TIM_CounterMode TIM_CounterMode_Up; TIM_TimeBaseInit(TIM4, TIM_TimeBaseStructure); TIM_ICInitStructure.TIM_Channel TIM_Channel_1; TIM_ICInitStructure.TIM_ICPolarity TIM_ICPolarity_Rising; TIM_ICInitStructure.TIM_ICSelection TIM_ICSelection_DirectTI; TIM_ICInitStructure.TIM_ICPrescaler TIM_ICPSC_DIV1; TIM_ICInitStructure.TIM_ICFilter 0x0A; // 输入滤波抗抖动 TIM_ICInit(TIM4, TIM_ICInitStructure); TIM_ICInitStructure.TIM_Channel TIM_Channel_2; TIM_ICInitStructure.TIM_ICPolarity TIM_ICPolarity_Rising; TIM_ICInitStructure.TIM_ICSelection TIM_ICSelection_IndirectTI; TIM_ICInitStructure.TIM_ICPrescaler TIM_ICPSC_DIV1; TIM_ICInitStructure.TIM_ICFilter 0x0A; TIM_ICInit(TIM4, TIM_ICInitStructure); TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising); TIM_SetCounter(TIM4, 0); TIM_Cmd(TIM4, ENABLE); }配置的要点在于 TIM_EncoderMode_TI12 这一项它让定时器同时检测两路输入并在两个边沿都计数实现 4 倍频。TIM_ICFilter 加了一个小的输入滤波值因为编码器在电机启停瞬间会产生机械抖动直接不滤波的话计数器会出现毛刺角度读数在静止时乱跳PID 的微分项会被这种噪声放大得很厉害。读计数值时用 TIM_GetCounter 直接取之后要立刻把计数器清零或者记录上次值做差分保证角度变化量是准确的。3.2 角度与速度换算拿到定时器计数值之后需要把它换算成物理量。以 500 线编码器为例4 倍频后每圈 2000 个脉冲摆杆从垂直位置向一侧摆动 90 度对应 500 个脉冲。换算代码通常是这样// 从编码器计数值换算摆杆角度单位弧度 float Encoder_GetAngle(int16_t encoder_count) { // PULSE_PER_REV 2000 (500线 * 4倍频) // ANGLE_RANGE 2 * PI float angle (float)encoder_count * (2.0f * 3.14159f) / PULSE_PER_REV; return angle; }角速度则不直接对角度做微分因为编码器计数本身有量化误差直接差分会导致速度值一抖一抖的。常见做法是对连续多次的角度值做一阶低通滤波再计算差分或者直接用定时器捕获两个相邻中断之间的脉冲数作为速度的近似值。这个细节直接关系到后面 PID 的 D 项能不能稳定工作。3.3 PID 控制在定时器中断里的实现直线倒立摆使用两组 PID 控制一组负责摆杆角度环另一组负责小车位置环。角度环的输出是目标加速度位置环修正这个目标值避免小车缓慢漂移到轨道尽头。两个环在同一个 1 kHz 中断里级联执行顺序是先算位置环再算角度环。// 1ms定时中断 - 倒立摆核心控制逻辑 void TIM6_IRQHandler(void) { if (TIM_GetITStatus(TIM6, TIM_IT_Update) ! RESET) { TIM_ClearITPendingBit(TIM6, TIM_IT_Update); // 1. 读取编码器原始值 int16_t angle_count TIM_GetCounter(TIM4); int16_t position_count TIM_GetCounter(TIM5); // 2. 换算物理量 float angle (float)angle_count * ANGLE_TO_RAD; float position (float)position_count * PULSE_TO_MM; // 3. 计算角速度带低通滤波 angle_speed (angle - last_angle) / DT; angle_speed_filtered FILTER_ALPHA * angle_speed (1 - FILTER_ALPHA) * angle_speed_filtered; // 4. 位置环PID输出一个角度修正量 position_error 0.0f - position; // 目标位置是轨道中央 position_out pos_kp * position_error pos_kd * (0.0f - position_speed_filtered); // 5. 角度环PID目标是摆杆垂直叠加位置环修正 angle_error (0.0f - angle) position_out; integral angle_error * DT; if (integral INTEGRAL_LIMIT) integral INTEGRAL_LIMIT; if (integral -INTEGRAL_LIMIT) integral -INTEGRAL_LIMIT; differential angle_speed_filtered; output ang_kp * angle_error ang_ki * integral ang_kd * differential; // 6. 输出限幅 if (output PWM_MAX) output PWM_MAX; if (output -PWM_MAX) output -PWM_MAX; // 7. 写入PWM寄存器 Motor_SetOutput((int16_t)output); // 8. 更新状态 last_angle angle; last_position position; } }这段代码是整份工程的核心逻辑顺序值得讲清楚。位置环在前它根据小车偏离轨道中央的距离计算出这个时刻角度环的“目标角度应该偏移多少”。也就是说当小车往右跑的时候位置环会临时让摆杆往右倾斜一点利用重力分量产生向左的加速度把小车道回来。这就是倒立摆控制里常见的“重心偏移法”比直接在小车速度上做 PID 更符合物理直觉。角度环的 D 项用的是滤波后的角速度而不是角度偏差的差分目的是减少编码器量化噪声的放大。INTEGRAL_LIMIT 限幅是必须的没有限幅的积分项在摆杆长时间受静摩擦卡住时会积累到一个很大的值等摩擦力突然消失小车会被这一下积分输出弹出轨道。3.4 PWM 输出与电机方向控制PID 输出是一个有符号数正值表示一个方向负值表示另一个方向。电机驱动接口需要把符号拆成方向引脚和占空比两部分// 电机输出value为正时正向转为负时反向转 void Motor_SetOutput(int16_t value) { uint16_t duty (value 0) ? value : (-value); if (duty 1000) duty 1000; if (value 0) { GPIO_SetBits(GPIOA, GPIO_Pin_4); // DIR1 1 GPIO_ResetBits(GPIOA, GPIO_Pin_5); // DIR2 0 } else { GPIO_ResetBits(GPIOA, GPIO_Pin_4); // DIR1 0 GPIO_SetBits(GPIOA, GPIO_Pin_5); // DIR2 1 } TIM_SetCompare1(TIM3, duty); // PWM占空比 }死区问题需要注意。电机在占空比很小时比如 0 到 50/1000可能根本转不动但 PID 输出的死区会造成“输出在积累、车却不动”的假象然后占空比突然越过静摩擦阈值小车猛地窜一下。处理办法是给输出加死区补偿当 PID 输出绝对值小于某个阈值时直接给一个最小的启动力矩。这个值通常靠实测每个电机的启动电压不太一样不能靠公式算出来。4. 初始化流程与主循环设计上电后程序按什么顺序跑4.1 外设初始化顺序的讲究初始化顺序在倒立摆项目里有讲究顺序不对会出现上电瞬间摆杆砸下去的失控情况。这份工程里推荐顺序是先初始化时钟和 GPIO再初始化编码器接口接着关闭电机输出然后初始化定时中断最后才开启主循环。原因很简单电机输出必须在控制系统准备好之前处于安全状态否则上电瞬间一个随机占空比就会让小车冲出去。更具体一点PWM 的初始占空比应为 0并且方向引脚要预设为刹车状态。编码器要先清零保证第一次读到的角度是可信的。定时中断最后开启因为一旦开启1 kHz 的控制循环就开始跑了如果前面的外设状态还没稳定等于带着脏数据进入闭环。int main(void) { // 1. 时钟与基础GPIO SystemInit(); GPIO_Config(); // 2. 编码器接口初始化TIM4/TIM5 Encoder_Init(); // 3. 电机驱动引脚与PWM初始化初始占空比0 Motor_Init(); Motor_SetOutput(0); // 4. 传感器校准读取摆杆垂直静止时的编码器偏移 Calibrate_ZeroPoint(); // 5. OLED / 串口初始化 OLED_Init(); UART_Init(115200); // 6. 最后开启控制定时中断 TIM6_Init(); // 1kHz中断此时控制闭环开始运行 TIM_Cmd(TIM6, ENABLE); while (1) { // 主循环只做非实时任务刷新屏幕处理按键串口上报参数 OLED_ShowPIDParam(); UART_SendStatus(); } }4.2 零点校准为什么关键摆杆的垂直位置并不是天然对应编码器读数为 0。安装误差、摆杆重心不完全在轴线上都会导致编码器零点与物理垂直方向有偏差。如果不做校准PID 的目标角度就带着一个固定偏差结果是摆杆稳定时小车会一直往一个方向跑位置环拉都拉不住。校准代码通常会这么做程序启动后让摆杆自然垂下读取此时编码器读数作为零点。但倒立摆的摆杆启动前是悬垂的要立起来需要“起摆”操作——要么手动扶起摆杆到垂直位置再按校准键要么程序里写一套能量控制的起摆逻辑。这份工程里如果带起摆功能它的思路是把摆杆来回甩动让摆杆逐渐靠近垂直位置在某个能量阈值处切入平衡控制。这部分代码天然独立接在 Calibrate_ZeroPoint 之后、TIM6 启动之前。4.3 主循环任务的优先级思想很多初学者容易犯的错是把 PID 计算写进主循环这是完全错误的。主循环里的 OLED 刷新一次可能要 5 到 10 毫秒串口发送碰上 DMA 又要等这些都会让控制周期抖动。这份工程的处理方式是主循环只干“慢活”控制全部走定时中断两者之间通过全局变量交换参数。调参时在中断里改 PID 结构体OLED 只负责显示当前值不参与实时数据流。这种“慢任务与快任务分离”的写法在做任何嵌入式控制系统时都值得沿用。5. 避坑手册PID 调不拢、小车乱窜、编码器丢步的排查记录5.1 一上电小车就全速往一端冲过去现象程序烧录后电机立刻全速运转小车瞬间冲出轨道根本来不及断电。原因最常见的是 PWM 初始化为高电平有效但电机驱动是低电平有效。STM32 定时器输出比较极性配置成 high 时初始占空比 0 对应的引脚其实是低电平对低电平有效的驱动器来说就是全速输出。这属于硬件极性不匹配纯看代码看不出问题必须对照电机驱动模块手册检查。解决把 TIM_OCInitStructure.TIM_OCPolarity 改成 TIM_OCPolarity_Low同时把 Motor_SetOutput(0) 之后用万用表量一下方向引脚和 PWM 引脚的实际电平确认输出为零。从那以后我每次初始化电机驱动都会强制先测一遍空闲电平再往下写。5.2 摆杆能平衡但小车缓慢漂移直到撞到限位现象摆杆立住了但小车在以很慢的速度朝一个方向移动几分钟后撞上轨道端部。原因位置环没调好。角度环 PD 确实能让摆杆平衡但它不关心小车在哪里。位置环的 P 项太弱或者位置环只在角度环之外做叠加但方向反了都会出现这种“摆杆立住但整体漂移”的现象。另一个可能原因是编码器零点偏差没有被校准导致摆杆保持一个极小的倾斜角产生恒定推力。解决先校准零点排除机械偏差。然后单独加大位置环 P 值观察小车是否会在轨道中央来回振荡如果振荡就加位置环 D 值。一个快速判断方法是用手把小车往右推看摆杆是否向右倾斜正确响应如果向左倾斜说明位置环反馈极性反了。5.3 摆杆在平衡点附近高频抖动声音滋滋响现象摆杆看起来立住了但高频抖动电机声音尖锐电流明显偏大。原因微分项被编码器噪声放大。直接对角度差分得到角速度会让 D 项输出带有大量高频分量加上 PWM 输出的死区补偿就形成了持续的高频振荡。这个问题的本质不是 PID 参数不行而是信号质量不行。解决第一步先给编码器输入加 TIM_ICFilter 滤波第二步对角度速度做一阶低通滤波滤波系数可以先从 0.5 试起。调完滤波再加 D 项通常会发现之前不敢加大的 D 值现在可以安全地加大稳定性反而提升。5.4 手动扶起摆杆到垂直位置一松手立刻倒下现象摆杆扶正后松手瞬间没有控制力电机才反应但已经倒了。原因控制周期太长或者执行器响应太慢。如果控制中断不是 1 kHz 而是用 SysTick 写成了 10 ms那么每次松手后的第一个控制周期已经落后太多摆杆加速度太大电机追不上。另一个原因是 PWM 频率太低电机电流纹波大力矩不足。解决检查定时中断的更新中断频率是否确实为 1 kHz用示波器或者 IO 翻转法测一下周期。PWM 频率设置在 10 kHz 到 20 kHz 为宜TB6612 在这种频率下响应最好。如果确实是机械力矩不足只能换电机或减速比这是硬件层面绕不过去的物理限制。6. 二次开发思路从单级摆到自起摆与上位机联调拿到这份工程第一步不是改代码而是先给原来的控制链路加一个“旁路监测”。具体做法是在定时中断的 PID 计算前后各记录一个时间戳通过串口把两次时间戳的间隔上报确认中断负载率。正常情况下 1 ms 内部应该有至少 60% 的空闲如果发现中断执行时间超过 500 us就要优化代码——把浮点运算换成查表或者把滤波拆成定点整数运算。这个验证方法能直接判断你的电路板有没有继承到足够的实时余量再往下改才有底。接下来值得改的第一个功能是自动起摆。当前工程如果依赖手扶摆杆到垂直位置那只能算半自动。起摆的核心思路是能量控制来回摆动摆杆让摆杆累积能量同时监测摆杆角度和角速度组成的“能量值”当这个值大于某个门槛且摆杆位于垂直附近时立即切换到平衡 PID。能量控制与平衡控制在代码里是两套独立的控制函数切换条件写在一个状态机里。我个人习惯是把状态机设计成三个状态SWING、BALANCE、STOP每个状态有独立的进入条件和中止条件避免 PID 输出在两个模式间跳变时产生突变。再往后可以加一个简单的上位机联调通道。用串口把角度、角速度、PID 输出和 PWM 占空比以 CSV 格式上报上位机端直接画四条波形曲线观察参数调整前后的响应变化。我一般的调参思路是先把积分离散化输出打印出来看它是不是长期处于限幅状态——如果积分项一直在限幅边缘说明系统存在未被补偿的静摩擦或重力偏置这时候加大 Ki 不是出路应该回头检查零点校准和死区补偿。从那以后我每次调倒立摆这类自平衡系统都强制走一遍“测零漂、测死区、看积分输出、最后动 PID”的顺序能少走一半弯路。希望这篇笔记能帮你在自己的板子上少翻几次车。本文还有配套的精品资源点击获取