STM32直线倒立摆控制程序：从硬件选型到PID/LQR调参全解析

简介：一套基于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) ≈ theta，cos(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 的 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，分频后计数时钟为 1MHz；TIM_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 度以内。这套方法我现在每个新参数组必跑一遍。

调倒立摆最忌没有章法地乱试。从那以后我每次拿到新的控制程序，都会强制走一遍零点校准、控制周期验证、编码器噪声排查这三件事，再碰任何参数。希望帮到你，剩下的交给时间和耐心。

本文还有配套的精品资源，点击获取