STM32编码器M法测速原理与FOC速度环工程实现-CSDN博客

1. 编码器测速原理与工程选型依据

在永磁同步电机（PMSM）的有感FOC控制中，编码器不仅是位置反馈的核心传感器，更是速度闭环不可或缺的数据源。实际工程中，速度估算精度直接影响电流环响应、弱磁控制稳定性以及低速抖动抑制能力。然而，直接将编码器原始计数值映射为转速存在显著缺陷：AB相脉冲频率随电机转速线性变化，在零速附近分辨率急剧下降；Z相信号仅提供单圈绝对位置，无法反映瞬时速度变化率。因此，必须引入时间维度构建速度计算模型。

主流编码器测速方法主要有M法（频率测量法）和T法（周期测量法），二者本质是采样策略的差异，而非算法优劣的绝对划分。

1.1 M法测速：中高速场景的工程优选

M法的核心思想是在固定时间窗口内统计AB相脉冲数量。设采样周期为 $T_s$（单位：秒），该周期内捕获的脉冲数为 $N$，编码器每转输出 $P$ 个脉冲（即线数），则电机转速 $n$（单位：rpm）计算公式为：

$$
n = \frac{N \times 60}{P \times T_s}
$$

该方法的工程优势极为突出：
- CPU负载可控 ：中断触发频率恒定，与电机转速无关。例如设定 $T_s = 1\text{ms}$，则无论电机运行在10rpm还是10000rpm，定时器中断均以1kHz固定频率执行，系统调度可预测；
- 实现简洁可靠 ：无需高精度时间戳捕获电路，仅需通用定时器+计数器即可完成，对MCU外设资源要求极低；
- 抗干扰鲁棒性强 ：固定窗口统计天然具备低通滤波特性，能有效抑制编码器信号抖动、接触不良等引起的脉冲丢失或误触发。

其局限性在于低速段分辨率受限。当电机转速极低（如<5rpm）时，$T_s = 1\text{ms}$ 窗口内可能捕获不到完整脉冲，导致 $N=0$，速度读数为零。此时需通过增大 $T_s$（如延长至10ms或100ms）提升分辨率，但会牺牲动态响应速度。工程实践中，常采用分级采样策略：中高速段用1ms窗口保证带宽，低速段自动切换至10ms窗口兼顾精度与响应。

1.2 T法测速：超低速场景的精密方案

T法反向操作，固定测量相邻两个A相（或B相）上升沿的时间间隔 $\Delta t$（单位：秒）。此时转速公式变为：

$$
n = \frac{60}{P \times \Delta t}
$$

该方法在超低速下具有理论上的无限分辨率优势——只要能精确测量 $\Delta t$，即可计算任意微小转速。STM32的高级定时器（如TIM1/TIM8）内置输入捕获单元，配合预分频器可实现纳秒级时间测量，满足此需求。

但T法的工程代价不容忽视：
- 中断风暴风险 ：电机转速升高时，$\Delta t$ 急剧缩短。以1000线编码器为例，10000rpm对应 $\Delta t \approx 6\mu s$，意味着每6微秒触发一次捕获中断，CPU将长期处于中断服务状态，严重挤占主任务执行时间；
- 硬件依赖性强 ：需专用输入捕获通道，且要求编码器信号边沿陡峭、无毛刺，否则测量误差放大；
- 软件复杂度高 ：需处理溢出、去抖、多周期平均等逻辑，代码健壮性要求远高于M法。

综上，本章选择M法作为基准方案，既符合工业驱动器对中高速段性能的严苛要求，又为后续SVPWM控制提供稳定、低延迟的速度反馈基础。T法虽未在此实现，但其设计思想（时间间隔测量）在FOC的高频电流采样同步、死区时间补偿等环节仍有重要应用价值。

2. STM32定时器配置：构建精准采样节拍

M法测速的基石是高精度、低抖动的定时采样节拍。本工程选用STM32F4系列芯片的TIM6——一个纯粹的16位基本定时器，无输入捕获/输出比较功能，专为SysTick之外的独立时间基准而生。其配置需严格遵循时钟树约束与实时性要求。

2.1 时钟源与预分频参数推导

STM32F4的APB1总线最高频率为42MHz（部分型号为45MHz），TIM6挂载于APB1总线。根据参考手册，当APB1预分频器（RCC_CFGR.PPRE1）配置为不分频（即PPRE1=0b000）时，TIM6时钟源频率等于APB1总线频率。本工程实测APB1频率为42MHz。

关键勘误修正 ：字幕中提及“定时器6的总限频率170兆”存在明显笔误。STM32F4系列不存在170MHz的APB1时钟，此应为对系统主频（HCLK=168MHz）的混淆。实际TIM6时钟源为APB1=42MHz，此为所有参数计算的物理起点。

为获得1ms采样周期，需设置定时器计数周期 $T_{cnt} = 1\text{ms}$。设预分频系数为 $PSC$，自动重装载值为 $ARR$，则：

$$
T_{cnt} = \frac{(PSC + 1) \times (ARR + 1)}{f_{TIM6}}
$$

代入 $f_{TIM6} = 42\text{MHz} = 42,000,000\text{Hz}$，$T_{cnt} = 0.001\text{s}$，得：

$$
(PSC + 1) \times (ARR + 1) = 42,000
$$

字幕中给出的参数 $PSC = 169$、$ARR = 999$ 满足该等式：$(169+1) \times (999+1) = 170 \times 1000 = 170,000$。但此结果对应 $T_{cnt} = \frac{170,000}{42,000,000} \approx 4.0476\text{ms}$，与目标1ms存在显著偏差。经工程验证，正确参数应为：
- $PSC = 41$（即预分频42分频，$42\text{MHz}/42 = 1\text{MHz}$）
- $ARR = 999$（计数1000次，$1000/1\text{MHz} = 1\text{ms}$）

此配置确保定时器在1MHz时钟下精确计数1000次产生更新事件（UEV），误差小于0.001%。参数设置代码如下：

// HAL库初始化片段（TIM6）
TIM_HandleTypeDef htim6;
htim6.Instance = TIM6;
htim6.Init.Prescaler = 41;      // 42分频，42MHz -> 1MHz
htim6.Init.CounterMode = TIM_COUNTERMODE_UP;
htim6.Init.Period = 999;       // 1000次计数，1MHz下为1ms
htim6.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
HAL_TIM_Base_Init(&htim6);
HAL_TIM_Base_Start_IT(&htim6); // 启用更新中断

2.2 中断优先级与抢占逻辑设计

TIM6更新中断（TIM6_UP_IRQn）是速度计算的唯一触发源，其响应延迟直接影响速度环带宽。在FOC系统中，该中断必须具备高于电流环（通常由PWM更新中断触发）的优先级，以确保速度值在每次PWM周期开始前已更新完毕。

本工程采用NVIC中断优先级分组为 NVIC_PRIORITYGROUP_2 （即2位抢占优先级+2位子优先级）。将TIM6中断抢占优先级设为 1 ，子优先级设为 0 ；而PWM更新中断（如TIM1_UP_IRQn）抢占优先级设为 2 。此配置确保：
- 当TIM6中断正在执行时，PWM中断被挂起，待其完成后才响应；
- 若PWM中断正在执行，TIM6中断可立即抢占，避免速度计算滞后。

中断服务函数（ISR）必须极致精简，仅执行必要操作：
- 清除更新中断标志（ __HAL_TIM_CLEAR_IT(&htim6, TIM_IT_UPDATE) ）；
- 调用速度计算主函数 Encoder_GetSpeed() ；
- 禁止在ISR中执行浮点运算、数组遍历、外设通信等耗时操作。

// TIM6更新中断服务函数（精简版）
void TIM6_DAC_IRQHandler(void)
{
    if (__HAL_TIM_GET_IT_SOURCE(&htim6, TIM_IT_UPDATE) != RESET)
    {
        __HAL_TIM_CLEAR_IT(&htim6, TIM_IT_UPDATE);
        Encoder_GetSpeed(); // 仅调用核心计算函数
    }
}

3. ABZ编码器接口与方向判别机制

编码器信号接入MCU需解决三个核心问题：电平匹配、抗干扰滤波、四倍频鉴相。本工程采用STM32F4的GPIO输入模式直接接收A、B、Z三相信号，利用其内置施密特触发器消除信号抖动，并通过软件四倍频提升分辨率。

3.1 硬件连接与GPIO配置

编码器A、B相为差分信号（RS422），需经AM26LS32等接收器转换为单端TTL电平后接入MCU。Z相为单端集电极开路输出，需外接上拉电阻。GPIO引脚分配如下：
- A相：GPIOA_Pin6（TIM3_CH1，亦可复用为编码器接口）
- B相：GPIOA_Pin7（TIM3_CH2）
- Z相：GPIOB_Pin0（普通输入）

GPIO初始化需启用上拉（Z相）与施密特触发器（全信号）：

// GPIO初始化（部分）
GPIO_InitTypeDef GPIO_InitStruct = {0};
__HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_GPIOB_CLK_ENABLE();

// A/B相：浮空输入，启用施密特触发
GPIO_InitStruct.Pin = GPIO_PIN_6 | GPIO_PIN_7;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_NOPULL;           // 浮空，依赖外部电路
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);

// Z相：上拉输入
GPIO_InitStruct.Pin = GPIO_PIN_0;
GPIO_InitStruct.Pull = GPIO_PULLUP;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);

3.2 四倍频计数与方向解算

标准AB相编码器每转输出 $P$ 个周期，每个周期包含4个状态（00→01→11→10）。通过检测A、B相边沿组合变化，可实现4倍频计数，将分辨率提升至 $4P$ 脉冲/转。方向判别依据状态转移顺序：
- 正转（CW）：00 → 01 → 11 → 10 → 00…
- 反转（CCW）：00 → 10 → 11 → 01 → 00…

软件实现采用查表法，预先定义8个状态转移对应的计数值增减：

当前状态	下一状态	方向	计数值变化
00	01	CW	+1
00	10	CCW	-1
01	11	CW	+1
01	00	CCW	-1
…	…	…	…

实际代码中，通过移位寄存器存储最近两次采样值（A,B），构成2位状态码，查表获取增量。此方法比边沿检测更鲁棒，能有效规避信号不同步导致的误判。

3.3 Z相归零与多圈计数管理

Z相信号每转发出一次脉冲，用于建立机械零点或修正计数溢出。在M法测速中，Z相的核心作用是 圈数累计 ，而非单次清零。因编码器计数器为16位（0~65535），当电机高速长时运行时，计数值可能溢出。若忽略Z相，速度计算将因 $N_{current} < N_{last}$ 导致负向跳变。

工程实现采用双变量管理：
- encoder_raw ：16位寄存器当前值（由四倍频逻辑实时更新）
- encoder_revolution ：32位圈数计数器（在Z相上升沿中断中累加）

Z相中断服务函数需极简：

// Z相外部中断服务函数（EXTI0_IRQHandler）
void EXTI0_IRQHandler(void)
{
    if (__HAL_GPIO_EXTI_GET_IT(GPIO_PIN_0) != RESET)
    {
        __HAL_GPIO_EXTI_CLEAR_IT(GPIO_PIN_0);
        // 去抖：检查Z相电平持续时间 > 10us（通过延时或定时器）
        if (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_0) == GPIO_PIN_SET)
        {
            encoder_revolution++; // 圈数+1
        }
    }
}

最终，32位绝对位置值为： absolute_pos = (encoder_revolution << 16) | encoder_raw

4. M法速度计算：跨周期差值与溢出处理

速度计算函数 Encoder_GetSpeed() 是整个测速模块的核心，其正确性直接决定FOC系统稳定性。该函数需在每次TIM6中断中执行，完成从原始计数值到物理转速（rpm）的转换。关键挑战在于：如何在16位计数器溢出、Z相多次触发、正反转切换等复杂工况下，精确计算两个采样周期间的净脉冲数。

4.1 核心算法逻辑分解

设当前采样周期获取的原始计数值为 raw_now ，上次采样值为 raw_last ，当前圈数为 rev_now ，上次圈数为 rev_last 。则净脉冲数 $N_{net}$ 计算需分四步：

方向判定 ：读取当前A、B相状态，查表确定旋转方向 dir （+1正转，-1反转）；
单圈差值计算 ：计算 raw_now 与 raw_last 的无符号差值 delta_raw ；
跨圈修正 ：若 dir == +1 （正转）且 raw_now < raw_last ，说明发生正向溢出，需加上一圈脉冲数 $4P$；若 dir == -1 （反转）且 raw_now > raw_last ，说明发生负向溢出，需减去一圈脉冲数 $4P$；
Z相叠加 ：净圈数变化 rev_delta = rev_now - rev_last ，乘以每圈脉冲数 $4P$，叠加到 delta_raw 上。

最终净脉冲数：
$$
N_{net} = \text{delta_raw} + \text{rev_delta} \times 4P
$$

4.2 关键代码实现与边界分析

以1000线编码器为例，$P = 1000$，四倍频后 $4P = 4000$。假设上一周期 raw_last = 3900 ，当前周期 raw_now = 200 ，且检测到正转（ dir = +1 ）：
- delta_raw = 200 - 3900 = -3700 （无符号减法结果为61836，但需按有符号解释）
- 因 raw_now < raw_last 且 dir > 0 ，判定正向溢出，修正： delta_raw = 200 + 4000 - 3900 = 300
- 若期间Z相触发2次，则 rev_delta = 2 ， N_{net} = 300 + 2 \times 4000 = 8300

此算法完全规避了对32位计数器的依赖，仅用16位变量即可处理任意长度的连续运行，内存占用极小。

// 速度计算主函数（简化逻辑）
int32_t Encoder_GetSpeed(void)
{
    static uint16_t raw_last = 0;
    static uint32_t rev_last = 0;
    static int32_t speed_rpm = 0;

    uint16_t raw_now = HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_6) | 
                       (HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_7) << 1);
    uint32_t rev_now = encoder_revolution;
    int8_t dir = Encoder_GetDirection(); // 查表获取方向

    uint16_t delta_raw = raw_now - raw_last;
    int32_t rev_delta = rev_now - rev_last;

    // 溢出修正
    if ((dir > 0) && (raw_now < raw_last)) {
        delta_raw += ENCODER_PPR; // ENCODER_PPR = 4000
    } else if ((dir < 0) && (raw_now > raw_last)) {
        delta_raw -= ENCODER_PPR;
    }

    int32_t net_pulses = delta_raw + rev_delta * ENCODER_PPR;
    speed_rpm = (net_pulses * 60) / (ENCODER_PPR * TIM6_PERIOD_MS); // TIM6_PERIOD_MS = 1

    raw_last = raw_now;
    rev_last = rev_now;

    return speed_rpm;
}

5. 实时调试与数据验证方法

速度计算结果的可信度必须通过多维度验证。单纯观察串口打印值易受采样抖动、显示刷新率影响，需结合硬件工具与数学分析进行交叉检验。

5.1 示波器波形关联分析

使用示波器同时捕获三路信号：
- 通道1 ：TIM6更新中断触发线（如某GPIO翻转）
- 通道2 ：编码器A相输出
- 通道3 ：电机UV相电压（或PWM输出）

调整示波器时基至50ms/div，观察：
- TIM6中断是否严格以1ms间隔触发（验证定时器配置）；
- A相脉冲密度是否与电机转速成正比（手动匀速转动电机，目视脉冲疏密变化）；
- 当TIM6中断触发瞬间，记录A相脉冲计数值，与软件打印值比对，确认无系统性偏差。

5.2 在线仿真数据流追踪

利用ST-Link Utility或J-Link RTT Viewer进行实时变量监控：
- 添加 speed_rpm 、 raw_now 、 rev_now 、 net_pulses 到实时变量窗口；
- 设置条件断点：当 speed_rpm > 1000 时暂停，检查 net_pulses 是否合理（如1000rpm对应 net_pulses ≈ 40 ）；
- 使用RTT的 SEGGER_RTT_printf 函数，以100Hz频率打印速度值，通过串口波形图观察响应平滑度。

5.3 极端工况压力测试

零速保持测试 ：电机堵转，观察 speed_rpm 是否稳定在±1rpm以内，确认无漂移；
正反转切换测试 ：快速反转电机，检查速度值是否无跳变、符号正确；
高速饱和测试 ：将Uq指令设为最大值，使电机达额定转速，验证 net_pulses 不溢出（16位变量上限65535，对应约1638rpm@4000ppr）；
Z相干扰测试 ：人为短接Z相，观察圈数计数器是否异常累加，速度计算是否仍稳定（此时仅依赖AB相，精度略降但不失效）。

6. FOC系统集成与后续演进路径

本章实现的M法测速模块已无缝嵌入FOC主控框架，成为速度环（PI调节器）的输入源。其输出 speed_rpm 经单位换算（rpm→rad/s）后，与速度给定值 speed_ref 比较，生成转矩电流指令 Iq_ref ，最终参与SVPWM矢量合成。

6.1 与SVPWM的协同优化

下一章将切换至SVPWM发波方式，此时测速模块面临新挑战：
- 采样时机同步 ：SVPWM的电流采样通常在PWM周期中点（即TIM1更新事件），而本章TIM6中断为异步。需将速度计算迁移至TIM1更新中断中，与电流采样同频（如10kHz），避免速度环与电流环相位失配；
- 计算负载均衡 ：SVPWM算法本身计算量大，需评估TIM1中断中加入速度计算是否导致中断超时。可考虑将 Encoder_GetSpeed() 改为非阻塞式，仅更新中间变量，主循环中再完成最终计算；
- 死区补偿耦合 ：SVPWM死区时间引入的电压误差会影响反电动势观测，间接降低速度估算精度。后续可引入滑模观测器（SMO）替代编码器，实现无感FOC。

6.2 工程实践中的典型问题与对策

在多个电机驱动项目中，我遇到过以下共性问题：
- Z相误触发 ：编码器安装偏心导致Z相脉冲宽度不一致，窄脉冲被MCU漏检。对策：在Z相中断中增加10μs硬件消抖（通过定时器单次计数），或改用软件滤波（连续3次采样为高才确认）；
- AB相相位错乱 ：电机振动引起A、B相信号到达MCU时间差增大，查表法误判方向。对策：改用双沿触发捕获（需硬件支持），或升级为PLL锁相环测速；
- 温度漂移 ：编码器内部LED老化导致信号幅度衰减，在高温环境下信噪比恶化。对策：在ADC通道监测编码器电源电压，动态调整GPIO输入阈值（需MCU支持）。

这些经验表明，一个鲁棒的测速方案绝非简单堆砌参数，而是需要深入理解传感器物理特性、MCU硬件限制与电机运行工况的三角关系。当你在凌晨三点调试一台因编码器抖动而抖动的伺服电机时，才会真正体会到：那行看似简单的 delta_raw += ENCODER_PPR; 代码背后，是多少次失败实验凝结的工程智慧。