直线倒立摆的STM32实时控制实现：从传感、驱动到闭环算法
在自动化控制领域，有些系统天生“反直觉”——比如一个竖立在小车上的杆子，靠不断移动底座来维持自身不倒。这正是直线倒立摆的魅力所在：它看似简单，实则集成了非线性、强耦合与内在不稳定等典型控制难题。正因如此，它成了检验控制理论和嵌入式工程能力的“试金石”。
而当这套系统跑在一块STM32芯片上时，问题就变成了如何用有限的资源，在毫秒甚至微秒级时间内完成感知、计算与执行的精密配合。这不是单纯的代码堆砌，而是对硬件特性、控制逻辑与实时性能的综合考验。
我们不妨从一个实际场景切入：假设你正在调试一台刚组装好的直线倒立摆装置。电机轻微抖动，摆杆微微倾斜，但始终无法真正站稳。这时候你知道，问题可能出在任何一个环节——是角度测量有噪声？PWM响应太慢？还是PID参数没调好？要解决这些问题，必须深入每一个模块的设计细节。
STM32为何成为倒立摆控制的核心平台？
为什么大多数高校实验平台和科研原型都选择STM32作为主控？答案并不只是“便宜”或“资料多”，而是它的架构恰好契合了这类高动态系统的控制需求。
以STM32F407为例，168MHz主频、浮点运算单元（FPU）、多个高级定时器和编码器接口，这些都不是摆设。在倒立摆中，控制器需要每1~2ms完成一次完整的采样—计算—输出流程。如果使用传统51单片机，光是处理一次卡尔曼滤波就可能耗去数毫秒，根本来不及响应快速变化的姿态；而STM32凭借其强大的算力，可以在几百微秒内完成包括传感器数据融合、双环PID计算在内的全部任务。
更重要的是，它的外设高度集成且可配置性强。比如：
- 定时器支持
编码器模式
，无需CPU干预即可自动计数A/B相信号；
- 高级定时器能生成带
死区时间的互补PWM
，直接驱动H桥避免短路；
- I²C/SPI接口配合DMA，让MPU6050的数据读取几乎不占用CPU时间。
这种“硬件替软件干活”的设计哲学，正是实现高实时性的关键。你在写代码的时候会发现，很多原本需要轮询或中断处理的任务，其实已经被硬件默默完成了。
角度与位置检测：精度来自细节处理
倒立摆的第一步不是控制，而是准确知道“我现在在哪”。这里有两条独立但又相互关联的反馈路径： 摆杆角度 和 小车位置 。
摆杆角度怎么测？别只看MPU6050原始数据
很多人一上来就接MPU6050，读出加速度和角速度，然后简单积分得到角度。结果往往是数据跳变剧烈，稍微震动一下就读出十几度偏差。问题出在哪？陀螺仪存在零偏漂移，加速度计受振动干扰严重——原始数据不能直接用。
正确的做法是做 传感器融合 。常用的方法有两种：
-
互补滤波 ：
快速响应 + 低频稳定。公式很简单：
$$
\theta = 0.98 \times (\theta_{prev} + \omega \cdot dt) + 0.02 \times \theta_{acc}
$$
其中第一项来自陀螺仪积分，第二项是根据加速度反推的角度。系数可以根据系统响应调整，通常把权重更多给陀螺仪。 -
卡尔曼滤波 ：
更复杂但也更优，尤其在存在外部扰动时表现更好。虽然实现起来稍麻烦，但在STM32F4上跑一个离散卡尔曼滤波完全没问题，还能通过串口实时观察估计误差。
我见过太多项目失败的原因就是忽略了这一点：你以为你读到了真实角度，其实是噪声在跳舞。
小车位置靠编码器，但你怎么保证不丢脉冲？
光电编码器通常标称1000 PPR（每转脉冲数），配合30:1减速比和直径5cm的轮子，理论上可以达到约4μm的位置分辨率。听起来很美，但如果布线不当或中断优先级设置错误，很容易出现“丢脉冲”现象——明明走了10cm，系统却显示只有9.8cm。
解决方案有两个层面：
- 硬件层面 ：使用差分信号传输（如RS422编码器）或至少加上拉电阻、屏蔽线；
- 软件层面 ：利用STM32定时器的 编码器接口模式 （Encoder Mode），而不是自己用外部中断计数。
后者尤为关键。以下这段初始化代码看似普通，实则决定了系统的稳定性基础：
void Encoder_Init(void) {
__HAL_RCC_TIM2_CLK_ENABLE();
__HAL_RCC_GPIOA_CLK_ENABLE();
GPIO_InitTypeDef GPIO_InitStruct = {0};
GPIO_InitStruct.Pin = GPIO_PIN_0 | GPIO_PIN_1;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_PULLUP;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF1_TIM2;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
htim2.Instance = TIM2;
htim2.Init.Prescaler = 0;
htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
htim2.Init.Period = 0xFFFF;
htim2.EncoderMode = TIM_ENCODERMODE_TI12;
HAL_TIM_Encoder_Start(&htim2, TIM_CHANNEL_ALL);
}
一旦启用该模式，TIM2就会自动根据A/B相的相位关系进行增减计数，CNT寄存器的值就是当前位置。整个过程由硬件完成，即使CPU正在处理其他任务也不会丢失任何脉冲。
你可以随时通过
__HAL_TIM_GET_COUNTER(&htim2)
获取当前计数值，效率极高。
电机驱动不只是“通电就行”：PWM与H桥的协同艺术
有了准确的状态信息，下一步是施加控制力。这里的执行机构通常是直流电机 + H桥驱动芯片（如DRV8876或L298N）。但要注意， 驱动方式直接影响系统的动态响应能力 。
PWM频率该怎么选？
常见误区是认为PWM频率越高越好。实际上，频率过高会导致开关损耗增加，MOSFET发热严重；过低则会产生明显的“嗡嗡”声，甚至引起机械共振。
对于倒立摆系统，推荐将PWM频率设定在 10–20kHz之间 ，既避开了人耳听觉范围，又能保证足够的控制分辨率。例如，在16MHz定时器时钟下，若周期设为1000，则PWM频率为16kHz，占空比调节步进为0.1%。
下面是典型的PWM初始化代码：
void PWM_Motor_Init(void) {
__HAL_RCC_TIM1_CLK_ENABLE();
__HAL_RCC_GPIOA_CLK_ENABLE();
GPIO_InitTypeDef GPIO_InitStruct = {0};
GPIO_InitStruct.Pin = GPIO_PIN_8;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Alternate = GPIO_AF1_TIM1;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
htim1.Instance = TIM1;
htim1.Init.Prescaler = 84 - 1; // 1MHz @ 84MHz APB2
htim1.Init.Period = 1000 - 1; // 1kHz → 实际可用更高
htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
}
注意这里用了TIM1——高级定时器，支持互补输出和死区插入。如果你用的是半桥或全桥驱动方案，这点至关重要：上下桥臂不能同时导通，否则会造成电源短路。
如何实现双极性控制？
倒立摆需要双向驱动力。常见的做法是使用一个方向引脚（DIR）加一个PWM信号。但更优雅的方式是采用 双路互补PWM ，通过改变两路信号的占空比差值来控制方向和大小。
不过在实际应用中，为了简化逻辑，多数仍采用如下函数封装：
void Set_Motor_Speed(int16_t speed) {
if (speed > 0) {
__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, speed);
HAL_GPIO_WritePin(DIR_GPIO, DIR_PIN, GPIO_PIN_SET);
} else {
__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, -speed);
HAL_GPIO_WritePin(DIR_GPIO, DIR_PIN, GPIO_PIN_RESET);
}
}
其中
speed
范围一般限制在0~1000之间，对应0%~100%占空比。加入符号判断后，形成“双极性输出”效果。
控制算法落地：PID不只是公式套用
现在我们有了状态反馈，也有了执行手段，接下来是最核心的部分： 怎么算出该给电机多大的指令？
虽然现代控制方法如LQR、滑模控制在理论上更具优势，但对于初学者和教学平台来说， 双环PID仍是首选 。原因很简单：直观、易调、见效快。
双环结构的设计思想
单一PID很难兼顾角度稳定和位置控制。因此普遍采用 串级控制结构 ：
- 内环（角度环） ：目标是快速抑制摆杆倾角，响应频率要求高（建议控制周期≤2ms），通常采用PD或带弱积分的PID；
- 外环（位置环） ：调节小车位置至期望点（通常是原点），响应较慢，可用PI控制器，其输出作为角度环的目标值。
举个例子：你想让小车停在中间，但它偏右了。位置环检测到偏差后，会“命令”角度环：“你现在允许有一点向左的倾斜”，于是小车向左加速，直到回到中心位置再恢复垂直。
这个“以倾斜换位移”的策略，正是倒立摆能够平衡的关键机制。
PID代码实现中的“坑”
下面是一个典型的离散PID实现：
typedef struct {
float Kp, Ki, Kd;
float error, last_error, prev_error;
float integral, output;
} PID_Controller;
float PID_Calculate(PID_Controller *pid, float setpoint, float feedback) {
pid->error = setpoint - feedback;
pid->integral += pid->error;
// 积分限幅，防饱和
if (pid->integral > 1000) pid->integral = 1000;
else if (pid->integral < -1000) pid->integral = -1000;
float derivative = pid->error - pid->last_error;
pid->output = pid->Kp * pid->error +
pid->Ki * pid->integral +
pid->Kd * derivative;
pid->prev_error = pid->last_error;
pid->last_error = pid->error;
return pid->output;
}
这段代码看着没问题，但有几个隐藏风险：
- 没有抗积分饱和措施 ：即使做了限幅，一旦系统长时间无法回到设定值（如机械卡死），积分项还是会累积到极限，导致恢复时剧烈超调。更好的做法是引入“积分分离”或“条件积分”。
-
微分项未滤波
：原始误差的微分对噪声极其敏感。建议对微分项做一阶低通滤波：
c derivative = 0.7 * derivative + 0.3 * (pid->error - pid->last_error); - 未考虑采样周期一致性 ：所有计算都默认周期恒定。若中断被延迟，应记录实际dt并动态调整。
系统整合与调试实战
最终的控制循环通常放在一个高优先级的定时器中断中，例如TIM6配置为1ms触发：
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
if (htim == &htim6) {
angle = Get_Angle_From_MPU6050();
pos = __HAL_TIM_GET_COUNTER(&htim2);
// 内环：角度稳定
float angle_output = PID_Calculate(&angle_pid, 0.0f, angle);
// 外环：位置调节（可选）
float pos_setpoint = 0.0f;
float pos_output = PID_Calculate(&pos_pid, pos_setpoint, pos);
angle_pid.setpoint = pos_output; // 外环输出作为内环目标
Set_Motor_Speed((int16_t)angle_output);
}
}
在这个框架下，你可以通过串口向上位机发送
angle
,
pos
,
PWM
等变量，用Python或LabVIEW绘图观察动态过程。在线修改PID参数也是必备功能，否则每次改完都要重新烧录程序，效率极低。
常见问题及应对策略
| 问题 | 根本原因 | 解决方案 |
|---|---|---|
| 摆杆起不来 | 初始角度过大或无起摆策略 | 使用“摆动起摆”法，先左右摆动积累能量 |
| 电机持续抖动 | Kd不足或信号噪声大 | 提高微分增益，增加软件滤波 |
| 控制延迟明显 | 中断优先级低或任务过重 | 提升定时器中断优先级，关闭非必要中断 |
| 数据跳变频繁 | 未滤波或电源干扰 | 对MPU6050使用卡尔曼滤波，加去耦电容 |
特别提醒一点： 机械结构本身的质量往往比算法更重要 。导轨是否平直？轴承是否有间隙？轮子是否打滑？这些问题哪怕最完美的控制器也无法弥补。
写在最后：从实验室走向更广阔的应用
当你第一次看到那个摇摇晃晃的杆子终于稳稳立住时，那种成就感是难以言喻的。而这套基于STM32的控制系统所体现的思想—— 高精度传感、高效外设利用、实时闭环控制 ——远不止用于教学演示。
同样的技术架构可以迁移到：
- 自平衡机器人（如Segway原理）
- 云台姿态稳定系统
- 工业机械臂末端补偿
- 甚至无人机飞行控制的地面验证平台
未来，也可以尝试将PID替换为LQR（线性二次型调节器），利用MATLAB设计最优反馈增益矩阵，进一步提升系统鲁棒性和抗扰能力。或者探索基于强化学习的自适应控制，在未知负载变化下仍能保持稳定。
但无论如何演进，起点始终是一样的：一块STM32，一组传感器，一段精心打磨的控制代码。正是这些看似简单的组件，构成了智能控制世界的基石。
创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考
扫一扫
7101
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
