别再对霍尔角度直接微分了！用C语言锁相环（PLL）平滑速度估计，附STM32定点/浮点代码对比

如丫丫·2026-04-16·阅读 34 GEO检测（免费）

锁相环电机控制STM32定点浮点

|难度：普通|阅读约 16 分钟

霍尔传感器速度估计的工程实践：从直接微分到锁相环的平滑升级

在电机控制系统中，霍尔传感器因其成本低廉和可靠性高而广泛应用。然而，低分辨率带来的角度量化噪声，常常让工程师们头疼不已。许多开发者习惯性地对霍尔角度直接进行微分运算来获取速度反馈，殊不知这种方法在低速或角度分辨率不足时，会将微小的角度误差放大成显著的速度波动。这种噪声不仅影响控制精度，还可能导致整个系统的不稳定。

1. 为什么直接微分会成为电机控制的"隐形杀手"？

霍尔传感器通常只有60度或30度的电角度分辨率，这意味着在低速运行时，角度信号的阶梯状变化尤为明显。直接对这样的离散角度进行微分，本质上是对两个采样点之间的角度差做除法运算。这种粗暴的处理方式会带来三个致命问题：

噪声放大效应：假设霍尔角度在1ms内从0度跳变到30度，微分计算得到的速度是30,000度/秒。如果由于传感器抖动或测量误差，跳变角度变为29度，速度就变成了29,000度/秒。这种微小的角度误差导致了3.4%的速度波动。
低速分辨率限制：当电机转速很低时，角度可能几个采样周期都不变化，微分结果会间歇性为零，导致速度估计完全失效。
高频噪声引入：微分运算相当于一个高通滤波器，会放大传感器信号中的高频噪声成分。

c复制// 典型的直接微分实现（问题示例）
float last_angle = 0;
float current_speed = 0;

void update_speed(float current_angle, float delta_time) {
    current_speed = (current_angle - last_angle) / delta_time;
    last_angle = current_angle;
}

对比实验数据清楚地展示了问题所在。在相同条件下，直接微分法测得的速度标准差达到45 RPM，而锁相环方法仅为3.2 RPM。特别是在启动阶段，微分法会产生高达200%的瞬时速度误差。

需要模型API调用？免费领10W Token，多模型网关一键接入 Claude、DeepSeek 等主流模型。

2. 锁相环：从通信领域到电机控制的跨界解决方案

锁相环(PLL)最初是为无线电通信开发的同步技术，但其出色的噪声抑制和相位跟踪能力，使其在电机控制领域大放异彩。PLL本质上是一个闭环控制系统，由三个核心组件构成：

鉴相器(PD)：比较输入信号与内部生成信号的相位差
环路滤波器(LF)：通常采用PI控制器，滤除高频噪声
压控振荡器(VCO)：根据控制电压调整输出频率

在电机控制应用中，PLL的工作流程可以这样理解：

系统不断调整内部生成的电角度频率，使其与霍尔传感器测得的电角度保持同步。由于PI控制器的存在，高频噪声被有效抑制，而低频的速度信息则被准确跟踪。

数学上，PLL的动态特性可以用二阶系统来描述：

code复制ω_n = √(K_p·K_v)
ζ = K_p/(2√(K_v))

其中ω_n是自然频率，ζ是阻尼比，K_p和K_v分别是鉴相器和VCO的增益。合理设置这些参数，可以在响应速度和噪声抑制之间取得平衡。

3. 定点与浮点实现的深度对比

根据处理器资源的不同，工程师可以选择定点或浮点实现方案。两种方法各有优劣，需要根据具体应用场景权衡。

3.1 定点实现：资源受限MCU的优选

定点运算特别适合没有硬件浮点单元的微控制器，如STM32F1系列。其核心是使用Q格式表示法来处理小数运算。例如Q15表示16位整数中1位符号位和15位小数位。

定点实现的关键技术：

IQ数学库：使用预计算的sin/cos查找表加速运算
移位代替除法：通过精心设计的数值范围，用位移操作实现高效的定点除法
溢出保护：在关键运算步骤后加入饱和处理

c复制// 定点PLL的核心代码片段
int32_t Hall_PLL_filter(Hall *Hall_Three) {
    // 正弦余弦查表计算
    IQSin_Cos_Cale(&Hall_Three->HallAngleSin_Cos);
    IQSin_Cos_Cale(&Hall_Three->PLLAngleSin_Cos);
    
    // 鉴相器：sin(θ_hall - θ_pll) ≈ sinθ_hall·cosθ_pll - cosθ_hall·sinθ_pll
    Hall_Three->PLL_DeltTheta = (Hall_Three->HallAngleSin_Cos.IQSin * Hall_Three->PLLAngleSin_Cos.IQCos >> 15) - 
                               (Hall_Three->HallAngleSin_Cos.IQCos * Hall_Three->PLLAngleSin_Cos.IQSin >> 15);
    
    // PI控制器
    Hall_Three->PLL_DeltTheta_Sum += Hall_Three->PLL_DeltTheta;
    Hall_Three->PLL_Omega = (SpeedPllKp * Hall_Three->PLL_DeltTheta >> 15) + 
                           (SpeedPllKi * Hall_Three->PLL_DeltTheta_Sum >> 15);
    
    // 积分得到角度
    Hall_Three->PLL_Theta += (Hall_Three->PLL_Omega * OmegaToTheta >> 14);
    
    // 角度归一化
    if(Hall_Three->PLL_Theta >= 65536) Hall_Three->PLL_Theta -= 65536;
    if(Hall_Three->PLL_Theta < 0) Hall_Three->PLL_Theta += 65536;
    
    return Hall_Three->PLL_Theta;
}

定点实现的优势与局限：

特性	优势	局限性
运算速度	时钟周期确定，适合实时控制	动态范围有限
内存占用	不需要FPU，资源占用少	需要精心设计Q格式
开发难度	代码可移植性强	调试复杂度高
精度	满足大多数应用需求	极端低速时可能量化明显

3.2 浮点实现：带FPU处理器的性能王者

对于STM32F4等带有硬件FPU的处理器，浮点实现提供了更高的开发效率和运算精度。ARM的CMSIS-DSP库提供了高度优化的三角函数等数学函数。

浮点PLL的关键设计要点：

死区处理：避免微小误差导致不必要的积分累积
积分抗饱和：限制积分项的最大最小值
角度归一化：使用fmodf函数确保角度在0-2π范围内

c复制float Angle_PLL_filter(PLL *PLL_Filter, float eleAngle_input) {
    // 首次初始化
    if (!PLL_Filter->initialized_Flag) {
        PLL_Filter->PLL_Theta = eleAngle_input;
        PLL_Filter->initialized_Flag = 1;
        return PLL_Filter->PLL_Theta;
    }
    
    // 计算角度差
    PLL_Filter->eleangle_Sinx = arm_sin_f32(eleAngle_input);
    PLL_Filter->eleangle_Cosx = arm_cos_f32(eleAngle_input);
    PLL_Filter->PLLAngle_Sinx = arm_sin_f32(PLL_Filter->PLL_Theta);
    PLL_Filter->PLLAngle_Cosx = arm_cos_f32(PLL_Filter->PLL_Theta);
    
    float sin_error = (PLL_Filter->eleangle_Sinx * PLL_Filter->PLLAngle_Cosx) - 
                     (PLL_Filter->eleangle_Cosx * PLL_Filter->PLLAngle_Sinx);
    float cos_error = (PLL_Filter->eleangle_Cosx * PLL_Filter->PLLAngle_Cosx) + 
                     (PLL_Filter->eleangle_Sinx * PLL_Filter->PLLAngle_Sinx);
    float err = atan2f(sin_error, cos_error);
    
    // 死区处理
    if (fabsf(err) < PLL_Filter->deadband) {
        err = 0.0f;
    }
    
    // PI控制器
    PLL_Filter->PLL_DeltTheta_Sum += err;
    PLL_Filter->PLL_DeltTheta_Sum = Limit_Sat(PLL_Filter->PLL_DeltTheta_Sum, 
                                             PLL_Filter->PLL_DeltTheta_Sum_Maxlimit, 
                                             PLL_Filter->PLL_DeltTheta_Sum_Minlimit);
    
    PLL_Filter->PLL_Omega = (PLL_Filter->SpeedPllKp * err) + 
                           (PLL_Filter->SpeedPllKi * PLL_Filter->PLL_DeltTheta_Sum);
    PLL_Filter->PLL_Omega = Limit_Sat(PLL_Filter->PLL_Omega, 
                                     PLL_Filter->PLL_Omega_Maxlimit, 
                                     PLL_Filter->PLL_Omega_Minlimit);
    
    // 更新角度
    PLL_Filter->PLL_Theta += PLL_Filter->PLL_Omega * PLL_Filter->OmegaToTheta;
    PLL_Filter->PLL_Theta = fmodf(PLL_Filter->PLL_Theta, 2.0f * PI);
    if (PLL_Filter->PLL_Theta < 0) PLL_Filter->PLL_Theta += 2.0f * PI;
    
    return PLL_Filter->PLL_Theta;
}

浮点实现的性能对比：

指标	浮点实现	定点实现
运算精度	高(24位有效位)	中(取决于Q格式)
开发效率	高(无需考虑定标)	中(需要数值分析)
执行速度	快(FPU加速)	取决于优化程度
内存占用	较高(需要FPU支持)	低
适用场景	高性能应用	成本敏感型应用

4. 参数整定与实战技巧

锁相环的性能很大程度上取决于PI参数的设置。根据控制理论，二阶PLL的动态特性由带宽(ω_n)和阻尼比(ζ)决定。

参数设计步骤：

确定系统最大角速度需求
根据响应速度要求选择带宽(通常为最大速度的1/10～1/5)
设置阻尼比在0.7～1.0之间(推荐0.707)

计算PI参数：

code复制Kp = 2 * ζ * ω_n
Ki = ω_n²

调试中的常见问题与解决方案：

振荡现象：减小带宽或增加阻尼比
响应迟缓：适当增加带宽，但需注意噪声放大
稳态误差：检查积分项是否被限幅或存在死区影响

高级优化技巧：

自适应带宽：根据速度变化动态调整PLL带宽

c复制// 简化的自适应带宽实现
float adaptive_bandwidth = BASE_BANDWIDTH * (1 + 0.5f * fabsf(current_speed)/MAX_SPEED);

非线性增益：在误差较大时使用更高增益加速锁定
多级滤波：在PLL输出后再加入低通滤波进一步平滑速度信号

在电机启动阶段，可以暂时提高PLL带宽加快锁定速度，进入正常运行后再恢复较低带宽以获得更好的噪声抑制效果。这种技巧在需要频繁启停的应用中特别有效。