STM32智能小车循迹：从if-else到PID，我的代码优化与性能提升实战-程序员充电站

STM32智能小车循迹：从if-else到PID的代码演进与性能优化实战

去年夏天调试实验室的第四代智能小车时，发现初版循迹代码在复杂赛道上会出现明显的"蛇形走位"。这个现象促使我重新思考嵌入式系统中控制算法的实现方式——如何用更优雅的代码结构替代原始的if-else判断，同时提升实时响应性能。本文将分享从基础状态判断到完整PID控制器的代码重构过程，重点解析嵌入式C在资源受限环境下的优化技巧。

1. 基础循迹实现与性能瓶颈分析

实验室常见的五路红外循迹模块，本质上是通过 TCRT5000 红外对管检测地面反射率差异。原始实现通常直接读取GPIO状态进行电机控制：

if(READ_SENSOR_1 && !READ_SENSOR_2) { motor_set_speed(LEFT, 70); motor_set_speed(RIGHT, 30); } else if(!READ_SENSOR_1 && READ_SENSOR_2) { motor_set_speed(LEFT, 30); motor_set_speed(RIGHT, 70); }

这种实现存在三个典型问题：

实时性损失：每个条件判断都需要完整读取所有传感器状态
控制阶跃：速度突变导致电机抖动和机械损耗
可维护性差：新增传感器需要修改所有条件分支

通过逻辑分析仪抓取波形发现，原始方案每次控制周期耗时约 2.1ms，其中 GPIO 读取就占用了 1.3ms。这对于要求 100Hz 以上更新率的循迹系统显然不够理想。

2. 硬件接口优化与状态编码

首先优化传感器读取方式。STM32 的 GPIO 端口寄存器支持原子读取整个端口状态：

#define SENSOR_PORT GPIOA->IDR #define SENSOR_MASK 0x00A0 // PA5-PA7 uint8_t read_sensors() { return (SENSOR_PORT & SENSOR_MASK) >> 5; }

这种读取方式将5个传感器的采样时间从 1.3ms 缩短到 0.2μs。接着引入状态编码机制：

二进制编码	十进制	轨迹位置
00011	3	偏右
00110	6	严重偏右
11000	24	严重偏左
01100	12	居中

状态机实现示例：

typedef enum { TRACK_LOST = 0, TRACK_FAR_LEFT, TRACK_NEAR_LEFT, TRACK_CENTER, TRACK_NEAR_RIGHT, TRACK_FAR_RIGHT } TrackState; TrackState get_track_state(uint8_t sensors) { static const uint8_t state_map[] = {0, 0, 0, 3, 2, 0, 1, 0, 4, 0, 0, 0, 5}; return (sensors < 13) ? state_map[sensors] : TRACK_LOST; }

3. PID控制器的嵌入式实现技巧

在资源受限的STM32F103上实现浮点PID需要考虑以下优化点：

3.1 定点数优化

当CPU没有FPU时，使用Q格式定点数：

typedef int32_t Q16_t; #define Q16_FROM_FLOAT(x) ((Q16_t)((x)*65536.0f)) #define Q16_TO_FLOAT(x) (((float)(x))/65536.0f) Q16_t pid_update(Q16_t error) { static Q16_t integral = 0; static Q16_t last_error = 0; integral += error; Q16_t derivative = error - last_error; last_error = error; return (Kp * error + Ki * integral + Kd * derivative) >> 16; }

3.2 抗积分饱和处理

增加积分限幅防止电机堵转：

if(abs(error) < ERROR_THRESHOLD) { integral += error; integral = CLAMP(integral, -INTEGRAL_MAX, INTEGRAL_MAX); } else { integral = 0; }

3.3 参数整定经验值

参数	起步值	调整方向	影响特征
Kp	1.0	增大减少静差	响应速度加快但可能振荡
Ki	0.01	微调消除稳态误差	过大会引起超调
Kd	0.5	抑制超调和振荡	对噪声敏感

实际调试时建议先用Ziegler-Nichols方法确定大致范围，再通过赛道实测微调。

4. 系统级优化与实时性保障

4.1 定时器中断调度

利用STM32的TIM2定时器产生100Hz的中断：

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { if(htim == &htim2) { uint8_t sensors = read_sensors(); TrackState state = get_track_state(sensors); float control = pid_update(state_to_error(state)); motor_adjust_speed(control); } }

4.2 内存访问优化

将频繁访问的变量定义到CCM RAM：

__attribute__((section(".ccmram"))) PID_Controller main_pid;

4.3 串口调试输出优化

避免直接使用printf，采用DMA传输：

uint8_t debug_buf[64]; void debug_printf(const char *fmt, ...) { va_list args; va_start(args, fmt); int len = vsnprintf(debug_buf, sizeof(debug_buf), fmt, args); HAL_UART_Transmit_DMA(&huart1, debug_buf, len); va_end(args); }