当前位置：首页 > 科技创新 > 正文

零知派——STM32智能小车系列教程（四）：红外对射式测速模块与直线行驶PID闭环控制原理及调试

​引言：智能小车要实现的功能很多——避障、跟随、贴边、灭火、直线行驶，每一个模块单独调试起来都有自己的难点。如果一开始就把所有模块塞进一个工程里联调，出了问题很难分清到底是传感器硬件的问题、电平极性的问题，还是上层控制算法的问题

所以这一系列教程的思路是：先把每个功能模块单独拎出来，配一个不依赖其他硬件的最小测试程序，把模块本身调通调透，再回头看它在完整项目里是怎么被使用的

目录

一、引言

二、测速模块硬件原理

2.1 槛型红外对射模块工作原理

2.2 原理图讲解

三、硬件接线说明

3.1引脚定义

3.2 接线方案表

3.3 连接示意图

四、M/T测速法原理

4.1 传统M法测速的局限

4.2 M/T测速法改进

五、PID控制原理

5.1 PID控制器数学意义

5.2 PID系统框图

六、继电器反馈自整定

6.1继电器反馈法

6.2齐格勒-尼科尔斯法

6.3 中位值平均滤波

七、独立测试程序

7.1 命令速查表

7.2VOFA+可视化调参

八、常见问题解答（FAQ）

Q1：测速值跳变严重，PID输出在两个值之间来回切换？

Q2：自整定完成后参数不理想怎么办？

一、引言

在前三篇教程里，我们分别调通了循迹、超声波云台和红外双目跟随三个感知模块，这一篇要讲的是这台小车唯一一个闭环反馈控制的运动模块——直线行驶PID

前面三个模块本质上都是"感知即决策"：读到偏差值就转向，读到距离就调速，属于开环或者简单的比例响应。而直线行驶不一样，它要解决的问题是：

两个独立的直流电机，即使给相同的PWM占空比也不会转得一样快（电机个体差异、齿轮箱摩擦力、轮胎接触面不均，随便一个因素都会导致左右轮实际转速有差异）

如果不做闭环反馈修正，小车走起来是一条弧线而不是直线

这篇教程会把测速原理、槛型光电传感器的电路结构、脉冲计数与转速换算公式、PID闭环设计，以及项目中集成的继电器法PID自整定功能完整讲一遍，

这个系列教程拆成以下几篇：

篇号 主题 核心硬件
第一篇（已完成） 五路循迹模块 TCRT5000 + LM393/LM339
第二篇（已完成） HC-SR04超声波测距+SG90舵机云台 HC-SR04 + SG90
第三篇（已完成） 红外双目跟随模块 红外避障对管
第四篇（本文） 红外对射式测速模块与直线行驶PID闭环 槛型红外计数模块 + 码盘 + PID
第五篇（待更新） DHT11温湿度、火焰传感器、风扇灭火模块 DHT11 + 火焰传感器
第六篇（待更新） 蓝牙/串口双通道通信协议设计 HC-05/BT06
终篇（待更新） 整车项目架构总览 全模块整合

二、测速模块硬件原理

2.1 槛型红外对射模块工作原理

模块正面是一个U形槛口，两侧分别是红外发射管和接收管，中间由LM393比较器把连续变化的接收电压整形成干脆的数字电平

wKgZO2pwRNWAY2MQAAAaJkhYsuE974.png

工作方式：电机转动带动装在输出轴上的码盘同步转动，码盘边缘均匀分布的齿槛依次经过槛口，每经过一个齿槛就遮断一次光路，输出一个脈冲，单位时间内的脈冲数就能反推转速

透光槽转到发射管和接收管之间 → 红外光穿过 → 接收管导通 → 输出低电平

不透光齿转到发射管和接收管之间 → 红外光被遮挡 → 接收管截止 → 输出高电平

接收管输出的信号经 LM393 比较器整形后，通过 DO 引脚输出稳定的方波脉冲

与霍尔编码器的区别：

对比项 红外对射式测速模块 霍尔编码器
工作原理 光学遮断（透光/挡光） 磁电转换（霍尔效应）
输出信号 单路方波脉冲 A/B两路正交方波

2.2 原理图讲解

①单路 TCRT5000 检测电路

典型的 TCRT5000 + LM393测速模块单路原理图

元件 作用
180Ω限流电阻 限制 TCRT5000 发射管电流，通常 100Ω ~ 200Ω
10KΩ分压电阻 与接收管内阻组成分压电路，决定接收管输出的电压变化范围
10KΩ上拉电阻 LM393 集电极开路输出需要上拉到 VCC，通常 10kΩ
VR1电位器 调节比较器参考阈值电压，通常 10kΩ 3296 型

②测速逻辑

电机转动带动装在输出轴上的码盘同步转动，码盘边缘均匀分布的齿槛依次经过槛口，每经过一个齿槛就遮断一次光路，输出一个脈冲，单位时间内的脈冲数就能反推转速

三、硬件接线说明

3.1引脚定义

本项目测速模块的引脚定义（与pinsdefine.h一致）：

// ── 测速（红外对射式）───────────────────────────────────────────
#define SM1Pin          29   // 左轮
#define SM2Pin          28   // 右轮

3.2 接线方案表

模块引脚 零知派迷你板引脚 说明
VCC 5V 模块供电
GND GND 共地
DO D29（左轮）/ D28（右轮） 脉冲信号输出
AO 不接（可选） 模拟信号输出

3.3 连接示意图

四、M/T测速法原理

4.1 传统M法测速的局限

传统M法测速是在固定时间窗口内统计脉冲个数，然后换算成转速

转速n=(60×ft)/(Z×M2)（r/min）

误差率δ = 1/M1 × 100%转速越高，误差越小

4.2 M/T测速法改进

M/T测速法不再“数固定时间窗口里有几个脉冲”，而是精确记录第一个脉冲和最后一个脉冲各自到达的时间戳（用微秒级的micros()），转速直接用“脉冲数÷两个时间戳之间的真实时间差”来算

wKgZPGpwRJGASunoAAEapmlcBxI687.png

误差来源从“半个脉冲的窗口对齐误差”降低到“微秒级的时间戳测量误差”，分辨率能提升一到两个数量级，而且不需要靠拉长窗口来换取平滑，响应速度反而更快

代码实现：

volatile unsigned long g_firstEdgeUs = 0;
volatile unsigned long g_lastEdgeUs  = 0;
volatile uint16_t      g_edgeCount   = 0;

void pulseISR()
{
    unsigned long now = micros();
    if (g_edgeCount == 0) g_firstEdgeUs = now;
    g_lastEdgeUs = now;
    g_edgeCount++;
}

// 在采样函数中：
if (count >= 2) {
    unsigned long elapsedUs = tLast - tFirst;
    rawRpm = (double)(count - 1) * 60000000.0 / (g_pulsesPerRev * (double)elapsedUs);
}

五、PID控制原理

5.1 PID控制器数学意义

PID控制器由比例单元（P）、积分单元（I）和微分单元（D）三部分组成。它把收集到的数据和一个参考值进行比较，然后把这个差别用于计算新的输入值

wKgZPGpwRISASjFWAAAwqcNTpRM666.png

① 比例环节（P）

输出与当前误差成正比，误差越大响应越强，是系统响应速度的主要来源，但增益过大会导致系统超调甚至发散振荡

wKgZPGpwRHiAIoViAAI4-75iL_4160.png

可以看出若比例增益太大（Ki和Kd维持定值），会使系统不稳定，如紫色K=1.6曲线系统振荡

② 积分环节（I）

把历史误差累积起来，专门用来消除纯比例控制无法消除的稳态残差

wKgZO2pwRGqAUPfgAACEswnK3a0840.png

积分增益越大，趋近设定值的速度越快，因为积分控制会累计过去所有的误差，可能会使回授值出现过冲的情形。PID_v1库内部专门做了输出限幅同步限制outputSum

③ 微分环节（D）

根据偏差的变化率进行调节，偏差变化越快，微分作用越强。微分系数 Kd 越大，系统对偏差变化越敏感，能提前响应、抑制超调，但过大会导致系统振荡

wKgZPGpwRFyARZjVAAB79Tdon1Q979.png

不同微分增益Kd下，受控变数对时间的变化（Kp和Ki维持定值）

5.2 PID系统框图

PID控制器可以根据历史数据和差别的出现率来调整输入值，使系统更加准确而稳定

wKgZO2pwRFOAAvLLAAFk2Ao-Ras218.png

人工调试

先将Ki及Kd设为零，增加Kp一直到回路输出震荡为止

将Kp设定为“1/4振幅衰减”，增加Ki直到一定时间后的稳态误差可被修正为止

若Ki可能会造成不稳定，最后若有需要，可以增加Kd

调整PID参数对系统的影响如下

调整方式 (on) 上升时间 超调量 安定时间 稳态误差 稳定性
↑ Kp 减少 ↓ 增加 ↑ 小幅增加 ↗ 减少 ↓ 变差 ↓
↑ Ki 小幅减少 ↘ 增加↑ 增加 ↑ 大幅减少↓↓ 变差↓
↑ Kd 小幅减少 ↘ 减少↓ 减少↓ 变动不大→ 变好 ↑

pYYBAGDUtgWARxH6AAZFqfr9QMo686.png

六、继电器反馈自整定

6.1继电器反馈法

手动一个个试Kp/Ki/Kd效率很低，工程上经典做法是齐格勒-尼科尔斯临界比例度法：只保留比例作用，从小到大逐步增大Kp，直到系统输出出现持续等幅振荡，记录下这个临界增益Ku​和振荡周期Pu​，再用经验公式反推PID三项

wKgZO2pwRDeAS--fAAAPBVfczPk446.png

自整定的核心思路是：让系统主动施加一些激励信号，观察响应，然后根据响应特征反推出合适的PID参数

6.2齐格勒-尼科尔斯法

让被控对象在两个固定输出值之间强制切换，系统会自发产生一段稳定振荡，，分析这段波形的峰峰值和周期就能算出幅值A ​和周期T_u​

从振荡波形中提取两个关键参数：

幅值 A（峰峰值的一半）

周期 T_u（相邻两个峰值的时间间隔）

然后计算临界增益 K_u：K_u = 4 × d / (π × A)其中d是输出步长

最后套用Ziegler-Nichols 整定公式：

控制器类型 Kp Ki Kd
P 0.5 × K_u — —
PI 0.45 × K_u 0.54 × K_u / T_u —
PID 0.6 × K_u 1.2 × K_u / T_u 0.075 × K_u × T_u

官方自整定库的优势

手写自整定代码虽然实现了同样的公式，但缺少几个关键保护机制：

SetNoiseBand()：自动忽略小于阈值的信号抖动

SetLookbackSec()：用可调的历史窗口找波峰波谷，抗干扰能力强

因此本项目直接接入了官方PID_AutoTune_v0库

6.3 中位值平均滤波

中位值平均滤波法（又称防脉冲干扰平均滤波法）相当于“中位值滤波法”+“算术平均滤波法”的组合

连续采样 N 个数据，去掉一个最大值和一个最小值，然后计算 N-2 个数据的算术平均值

为什么有效：

中值部分：负责踢掉单次突刺（比如测量噪声导致的异常高值），排序后天然被挤到两端去掉

平均部分：负责抹平持续性的随机抖动

两者互补，正好应对测速数据中“既有普通抖动、偶尔也有明显偏离的点”这个特征

代码实现（窗口 N=5）

#define MED_WINDOW 5

double medianAvgFilter(double newVal, double *buf, uint8_t *idx, bool *filled)
{
    buf[*idx] = newVal;
    *idx = (*idx + 1) % MED_WINDOW;
    if (*idx == 0) *filled = true;

    double sorted[MED_WINDOW];
    memcpy(sorted, buf, sizeof(sorted));
    // 插入排序
    for (uint8_t i = 1; i < MED_WINDOW; i++) {
        double key = sorted[i];
        int8_t j = i - 1;
        while (j >= 0 && sorted[j] > key) { sorted[j+1] = sorted[j]; j--; }
        sorted[j+1] = key;
    }
    // 去掉最小值和最大值，其余取平均
    double sum = 0;
    for (uint8_t i = 1; i < MED_WINDOW - 1; i++) sum += sorted[i];
    return sum / (MED_WINDOW - 2);
}

七、独立测试程序

调整代码前建议先用不依赖整车其他模块的独立程序验证测速和PID闭环本身。这份沙盒集成了 M/T测速、中位值平均滤波、官方自整定库、以及和VOFA+滑块组件直接兼容的文本命令协议

/**************************************************************************************
 * 文件: IR_Encoder_PID_Test.ino
 * 功能：红外对射式测速 + PID闭环 + 官方自整定库 独立测试沙盒
 *
 * 串口命令：SP= KP= KI= KD= PPR= AT AC M= P R STOP
 **************************************************************************************/

#include < PID_v1.h >
#include < PID_AutoTune_v0.h >

#define SM2_PIN     28
#define MOTOR_IN1   11
#define MOTOR_IN2   10

int      g_pulsesPerRev = 20;
uint32_t g_sampleMs     = 100;

// ── M/T测速法：记录脉冲时间戳 ──────────────────────────────────
volatile unsigned long g_firstEdgeUs = 0;
volatile unsigned long g_lastEdgeUs  = 0;
volatile uint16_t      g_edgeCount   = 0;

// ── 中位值平均滤波 ──────────────────────────────────────────────
#define MED_WINDOW 5
double  g_medBuf[MED_WINDOW] = {0};
uint8_t g_medIdx = 0;
bool    g_medFilled = false;

double g_input    = 0;
double g_output   = 0;
double g_setpoint = 0;

double g_kp = 0.5, g_ki = 0.01, g_kd = 0.0;
PID    g_pid(&g_input, &g_output, &g_setpoint, g_kp, g_ki, g_kd, DIRECT);
PID_ATune g_aTune(&g_input, &g_output);
bool   g_tuning = false;

// ── 中断服务 ──────────────────────────────────────────────────────
void pulseISR()
{
    unsigned long now = micros();
    if (g_edgeCount == 0) g_firstEdgeUs = now;
    g_lastEdgeUs = now;
    g_edgeCount++;
}

// ── 电机驱动 ──────────────────────────────────────────────────────
void motorWrite(int pwm)
{
    pwm = constrain(pwm, 0, 255);
    int inv = 255 - pwm;
    analogWrite(MOTOR_IN1, 255);
    analogWrite(MOTOR_IN2, inv);
}

// ── 中位值平均滤波 ──────────────────────────────────────────────
double medianAvgFilter(double newVal)
{
    g_medBuf[g_medIdx] = newVal;
    g_medIdx = (g_medIdx + 1) % MED_WINDOW;
    if (g_medIdx == 0) g_medFilled = true;

    double sorted[MED_WINDOW];
    memcpy(sorted, g_medBuf, sizeof(sorted));
    for (uint8_t i = 1; i < MED_WINDOW; i++) {
        double key = sorted[i];
        int8_t j = i - 1;
        while (j >= 0 && sorted[j] > key) { sorted[j+1] = sorted[j]; j--; }
        sorted[j+1] = key;
    }
    double sum = 0;
    for (uint8_t i = 1; i < MED_WINDOW - 1; i++) sum += sorted[i];
    return sum / (MED_WINDOW - 2);
}

// ── 自整定 ──────────────────────────────────────────────────────
void startAutoTune()
{
    g_tuning = true;
    g_output = g_output > 0 ? g_output : 80;
    g_aTune.SetOutputStep(30);
    g_aTune.SetControlType(1);
    g_aTune.SetLookbackSec(5);
    g_aTune.SetNoiseBand(3);
}

void updateAutoTune()
{
    int val = g_aTune.Runtime();
    if (val != 0) {
        g_tuning = false;
        g_kp = g_aTune.GetKp();
        g_ki = g_aTune.GetKi();
        g_kd = g_aTune.GetKd();
        g_pid.SetTunings(g_kp, g_ki, g_kd);
        Serial.println("[AUTOTUNE] Done");
    }
}

// ── 主循环 ──────────────────────────────────────────────────────
void loop()
{
    // 处理串口命令（SP= KP= KI= KD= AT AC STOP等）
    handleSerialCommands();

    unsigned long now = millis();
    if (now - g_lastSampleMs < g_sampleMs) return;

    // M/T测速
    noInterrupts();
    uint16_t count = g_edgeCount;
    unsigned long tFirst = g_firstEdgeUs;
    unsigned long tLast = g_lastEdgeUs;
    g_edgeCount = 0;
    interrupts();

    double rawRpm;
    if (count >= 2) {
        unsigned long elapsedUs = tLast - tFirst;
        rawRpm = (double)(count - 1) * 60000000.0 / (g_pulsesPerRev * (double)elapsedUs);
    } else {
        rawRpm = 0;
    }

    // 中位值平均滤波
    double filtered = medianAvgFilter(rawRpm);
    g_input = filtered;

    // PID计算
    if (g_tuning) {
        updateAutoTune();
        motorWrite((int)g_output);
    } else {
        g_pid.Compute();
        motorWrite((int)g_output);
    }

    // VOFA+ 输出
    Serial.print(g_setpoint, 2); Serial.print(',');
    Serial.print(g_input, 2);    Serial.print(',');
    Serial.print(g_setpoint - g_input, 2); Serial.print(',');
    Serial.println(g_output, 1);

    g_lastSampleMs = now;
}

7.1 命令速查表

命令 效果
H 切换到直线行驶模式
U 触发自整定（先左轮后右轮）
LKP= LKI= LKD= 左轮PID手动调参
RKP= RKI= RKD= 右轮PID手动调参
EA= 调整测速滤波EMA系数
PPR= 调整码盘每圈脈冲数
RESET 左右轮恢复默认参数
INFO 查看当前所有参数

7.2VOFA+可视化调参

①数据协议

本项目使用 VOFA+ 的FireWater 协议——CSV 风格的字符串流，数据之间用逗号隔开，换行打印数据并显示数据曲线

setpoint,input,error,outputn

②控件配置

在 VOFA+ 中配置滑块控件，命令格式直接使用项目已有的文本协议：

控件 命令格式 说明
Kp 滑块 KP=%fn 设置比例系数
Ki 滑块 KI=%fn 设置积分系数
Kd 滑块 KD=%fn 设置微分系数
目标转速滑块 SP=%fn 设置目标转速
启动自整定按钮 ATn 启动继电器法自整定
取消自整定按钮 ACn 取消整定
急停按钮 STOPn 停止电机

③调参流程

第一步：纯比例调节

将 Ki 和 Kd 置 0，逐步增大 Kp 直到系统出现震荡，然后回调一点到震荡刚好消失

wKgZO2pwRA6AelfGAAZdeAwa9ZA868.png

第二步：加入积分

保持 Kp 不变，逐步增大 Ki 直到静差基本消除，注意观察是否出现超调

wKgZPGpwRAaAWeaiAAZbkFiVo14028.png

第三步：加入微分（如需要）

如果系统存在超调或震荡，从 0 开始逐步增加 Kd，直到曲线平滑

八、常见问题解答（FAQ）

Q1：测速值跳变严重，PID输出在两个值之间来回切换？

A：测量噪声被 Kp 放大，形成了“追噪声”现象。使用中位值平均滤波（窗口 N=5）、适当降低 Kp 值

Q2：自整定完成后参数不理想怎么办？

A：通过 VOFA+ 滑块手动微调，先微调 Kp（±0.2），再调 Ki（±0.05），最后调 Kd（±0.02）、重新运行自整定（发送 AT 命令）

资料整合

PID控制器： PID_controller

VOFA+官方文档： VOFA+ 学习文档

PID_AutoTune库： PID-AutoTune-Library

开环控制只能“发命令”，闭环控制才能“看结果”。有了精确的测速和 PID 闭环，小车才能真正做到“指哪打哪”——设定多少转速，就稳定跑多少转速

审核编辑 黄宇

上一篇
民调：韩国总统李在明施政好评率45.9% 创就任以来新低

下一篇
李强签署国务院令 公布修订后的《集成电路布图设计保护条例》

相关文章

推荐文章

最新文章