锁相环电角度滤波（定点\浮点C语言实现）
参考文章：(36 封私信 / 80 条消息) 霍尔角度使用锁相环（PLL）原理的应用 - 知乎(36 封私信 / 80 条消息) 20231221-基于锁相环PLL的电角度滤波 - 知乎使用锁相环对霍尔角度滤波_哔哩哔哩_bilibili红色方框部分即为PLL滤波器，包括鉴相器、PID滤波、震荡器三个部分鉴相器：为正弦鉴相器，由于输入的电角度是锯齿波，因此存在很多谐波，其使用限制是震荡器：类比压控
·
参考文章：
(36 封私信 / 80 条消息) 霍尔角度使用锁相环（PLL）原理的应用 - 知乎
(36 封私信 / 80 条消息) 20231221-基于锁相环PLL的电角度滤波 - 知乎
锁相环跟随电角度原理：
红色方框部分即为PLL滤波器，包括鉴相器、PID滤波、震荡器三个部分
鉴相器：为正弦鉴相器，由于输入的电角度是锯齿波，因此存在很多谐波，其使用限制是
震荡器：类比压控震荡器，通过电压来控制电机的转速，电压越高电机转速越快，电角度的频率越高，按照如下公式递推可以得到锯齿波的周期电角度
角速度积分得到电角度，电角度通过一个Mod取模单元限制角度在0至2*pi范围，组成一个锯齿波的周期电角度。
PI滤波器：滤出高频噪声，输出电角速度；这里其实使用其他滤波器如一阶低通滤波器作为环路滤波器也可以用，但是效果就不太好
锁相环的使用条件和使用场景：
PLL的本质是一个相位跟踪器而不是通用滤波器，因此必须要使用在输入信号具有周期性或准周期性、幅值变化小、谐波少时，而PLL性能之所以比LPF好，是因为锁相环使用了先验信息，即已知滤波器的输出结果是一个正弦波，然后锁相环只需要调节频率、相位差就可以了，而LPF默认输出是随机信号，缺少了关键信息，所以PLL滤波的结果要比LPF更好。
如果将PLL用于非周期输入信号如传感器信号，则需要对PLL结构做出改变：
1、鉴相器 (PD) 的变通：由于传感器信号不是交流频率，而是缓慢变化的直流电压，因此不能使用乘法器等传统的鉴相器。通常采用比例鉴相器，即直接将参考信号（PLL内部VCO生成的正弦/余弦波）与输入信号的差值作为误差信号。误差 = 输入电压 - VCO输出幅值
2、VCO 的变通：VCO（压控振荡器）在这里的角色发生了变化。它的输出频率不再直接代表信号频率，而是其输出信号的幅度被控制来跟踪输入传感器的直流电压值。VCO输出幅值 ≈ 输入电压
此时PLL和直接PI滤波器的结构完全等效，可以理解为应用在非周期信号的PLL会“退化”成PI滤波器；
而PI滤波器和低通滤波器则是通用的，对输入信号无特别要求；
锁相环的使用场景非常多，例如无传感器电机控制中提取电角度和电角速度、电网并网逆变器的相位跟踪（Grid PLL）、载波恢复 / 解调（carrier recovery）、平滑输入的电角度并做角速度估计等等，
这里我们主要介绍平滑输入的电角度并做角速度估计，在低速或角度量化很粗（HALL传感器这类低分辨率传感器）时，PLL 提取电角速度通常明显优于直接对角度做差分（微分），因为差分会放大量化噪声和测量噪声，同时PLL还能平滑输入的电角度；
第一张图为角度微分的速度环控制，第二张图为锁相环提取的速度做速度环控制，速度单位为转每秒；
实际代码：
定点简化版：
.h:
typedef struct {
...
/** 锁相环滤波 */
float pll_kp;
float pll_ki;
float pll_theta; //<锁相环法估算的角度
float pll_omega; //<锁相环法估算的速度
IQSin_Cos HallAngleSin_Cos; //对霍尔电角度的正弦余弦值进行计算
IQSin_Cos PLLAngleSin_Cos; //对锁相环电角度的正弦余弦值进行计算
int32_t PLL_Theta; //PLL滤波后的电角度
int32_t PLL_DeltTheta; //PLL电角度和霍尔电角度的error差值
int32_t PLL_DeltTheta_Sum; //PLL电角度和霍尔电角度的error差值的积分
int32_t PLL_Omega; //PLL预测的角速度
int32_t PLL_Omega_filter; //PLL预测的角速度滤波，因为一般PLL预测的角速度比较噪声
} Hall;
.c:
int32_t Hall_PLL_filter(Hall *Hall_Three)
{
#define SpeedPllBandwidth 250.0f //rad/s
#define SpeedPllKp (int16_t)(2 *SpeedPllBandwidth*707/1000)//2*Wn*阻尼比
#define SpeedPllKi (int16_t)(SpeedPllBandwidth * SpeedPllBandwidth/PWM_FREQ)// Wn*Wn*TS
#define OmegaToTheta (int16_t)(16384*32768.0/PWM_FREQ/PI)
Hall_Three->HallAngleSin_Cos.IQAngle = Hall_Three->Predict_eleangle; //从0-2pi映射到0-65535
IQSin_Cos_Cale((p_IQSin_Cos)&Hall_Three->HallAngleSin_Cos);
Hall_Three->PLLAngleSin_Cos.IQAngle = Hall_Three->PLL_Theta;
IQSin_Cos_Cale((p_IQSin_Cos)&Hall_Three->PLLAngleSin_Cos);
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);
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);
Hall_Three->PLL_Omega_filter += ((Hall_Three->PLL_Omega - Hall_Three->PLL_Omega_filter) >> 2);
//最终输出限幅
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;
}
.h:
typedef struct {
int32_t IQAngle; // 电机磁极位置角度0---65536即是0---360度
int32_t IQSin; // IQ格式正弦参数，-32768---32767 -1到1
int32_t IQCos; // IQ格式余弦参数，-32768---32767 -1到1
} IQSin_Cos , *p_IQSin_Cos;
.c:
// 正余弦数据表 int16_t的0---+32768，对应0到1的Q15定点数
// -1到0的Q15定点数直接再下表对应定点数据前加符号即可
const int16_t IQSin_Cos_Table[256]={\
0x0000,0x00C9,0x0192,0x025B,0x0324,0x03ED,0x04B6,0x057F,\
0x0648,0x0711,0x07D9,0x08A2,0x096A,0x0A33,0x0AFB,0x0BC4,\
0x0C8C,0x0D54,0x0E1C,0x0EE3,0x0FAB,0x1072,0x113A,0x1201,\
0x12C8,0x138F,0x1455,0x151C,0x15E2,0x16A8,0x176E,0x1833,\
0x18F9,0x19BE,0x1A82,0x1B47,0x1C0B,0x1CCF,0x1D93,0x1E57,\
0x1F1A,0x1FDD,0x209F,0x2161,0x2223,0x22E5,0x23A6,0x2467,\
0x2528,0x25E8,0x26A8,0x2767,0x2826,0x28E5,0x29A3,0x2A61,\
0x2B1F,0x2BDC,0x2C99,0x2D55,0x2E11,0x2ECC,0x2F87,0x3041,\
0x30FB,0x31B5,0x326E,0x3326,0x33DF,0x3496,0x354D,0x3604,\
0x36BA,0x376F,0x3824,0x38D9,0x398C,0x3A40,0x3AF2,0x3BA5,\
0x3C56,0x3D07,0x3DB8,0x3E68,0x3F17,0x3FC5,0x4073,0x4121,\
0x41CE,0x427A,0x4325,0x43D0,0x447A,0x4524,0x45CD,0x4675,\
0x471C,0x47C3,0x4869,0x490F,0x49B4,0x4A58,0x4AFB,0x4B9D,\
0x4C3F,0x4CE0,0x4D81,0x4E20,0x4EBF,0x4F5D,0x4FFB,0x5097,\
0x5133,0x51CE,0x5268,0x5302,0x539B,0x5432,0x54C9,0x5560,\
0x55F5,0x568A,0x571D,0x57B0,0x5842,0x58D3,0x5964,0x59F3,\
0x5A82,0x5B0F,0x5B9C,0x5C28,0x5CB3,0x5D3E,0x5DC7,0x5E4F,\
0x5ED7,0x5F5D,0x5FE3,0x6068,0x60EB,0x616E,0x61F0,0x6271,\
0x62F1,0x6370,0x63EE,0x646C,0x64E8,0x6563,0x65DD,0x6656,\
0x66CF,0x6746,0x67BC,0x6832,0x68A6,0x6919,0x698B,0x69FD,\
0x6A6D,0x6ADC,0x6B4A,0x6BB7,0x6C23,0x6C8E,0x6CF8,0x6D61,\
0x6DC9,0x6E30,0x6E96,0x6EFB,0x6F5E,0x6FC1,0x7022,0x7083,\
0x70E2,0x7140,0x719D,0x71F9,0x7254,0x72AE,0x7307,0x735E,\
0x73B5,0x740A,0x745F,0x74B2,0x7504,0x7555,0x75A5,0x75F3,\
0x7641,0x768D,0x76D8,0x7722,0x776B,0x77B3,0x77FA,0x783F,\
0x7884,0x78C7,0x7909,0x794A,0x7989,0x79C8,0x7A05,0x7A41,\
0x7A7C,0x7AB6,0x7AEE,0x7B26,0x7B5C,0x7B91,0x7BC5,0x7BF8,\
0x7C29,0x7C59,0x7C88,0x7CB6,0x7CE3,0x7D0E,0x7D39,0x7D62,\
0x7D89,0x7DB0,0x7DD5,0x7DFA,0x7E1D,0x7E3E,0x7E5F,0x7E7E,\
0x7E9C,0x7EB9,0x7ED5,0x7EEF,0x7F09,0x7F21,0x7F37,0x7F4D,\
0x7F61,0x7F74,0x7F86,0x7F97,0x7FA6,0x7FB4,0x7FC1,0x7FCD,\
0x7FD8,0x7FE1,0x7FE9,0x7FF0,0x7FF5,0x7FF9,0x7FFD,0x7FFE};
/*将当前计算的IQ电角度对应的正余弦值用查表法找出最接近的
这里的pV->IQSin和pV->IQCos值都是Q15格式定点数
规定查表的电角度范围一定是要0-65535，即需要从0-2pi映射到0-65535*/
void IQSin_Cos_Cale(p_IQSin_Cos pV)
{
uint16_t hindex;
hindex = (uint16_t) pV->IQAngle;
hindex >>=6;
/*65536/64 ===1024，此时1024表示360度电角度，则256===90度 ，将65536转为1024就是为了方便查表，
表格中数据有256个，表示0到1的Q15定点数，而每90°区间的正余弦值就覆盖了-1到0或者0到1，即每个90°区间可以分为256段
0-360°有四个区间4*256个数据分别对应1024段，因此需要将输入电角度范围变为1024段，以一一对应四个区间
的正余弦值表格中的256个数据*/
//0x01-0xFF(0-90度) 0X100-0X1FF(90-180度) 0X200-0X2FF 0X300-0X3FF
switch (hindex & SIN_RAD)
{
case U0_90: // 0X300 & 0x01-0xFF = 0x00
pV->IQSin = IQSin_Cos_Table[(uint8_t)(hindex)]; // 0---255 ==0---32766
pV->IQCos = IQSin_Cos_Table[(uint8_t)(0xFF-(uint8_t)(hindex))];
break;
case U90_180: // 0X300 & 0X100-0X1FF = 0x0100
pV->IQSin = IQSin_Cos_Table[(uint8_t)(0xFF-(uint8_t)(hindex))]; // 255---0 == 0---32766
pV->IQCos = -IQSin_Cos_Table[(uint8_t)(hindex)]; //-1到0的定点数
break;
case U180_270: // 0X300 & 0X200-0X2FF = 0X200
pV->IQSin = -IQSin_Cos_Table[(uint8_t)(hindex)];
pV->IQCos = -IQSin_Cos_Table[(uint8_t)(0xFF-(uint8_t)(hindex))];
break;
case U270_360: // 0X300 & 0X300-0X3FF = 0x0300
pV->IQSin= -IQSin_Cos_Table[(uint8_t)(0xFF-(uint8_t)(hindex))];
pV->IQCos = IQSin_Cos_Table[(uint8_t)(hindex)];
break;
default:
break;
}
}
浮点版本：
.h:
#include <arm_math.h>
//锁相环PLL滤波结构体类型定义
typedef struct {
float eleangle_Sinx; //对采样输入电角度的正弦余弦值进行计算
float eleangle_Cosx;
float PLLAngle_Sinx; //对锁相环电角度的正弦余弦值进行计算
float PLLAngle_Cosx;
float PLL_Theta; //PLL滤波后的电角度
float PLL_DeltTheta; //PLL电角度和采样电角度的error差值
float PLL_DeltTheta_Sum; //PLL电角度和采样电角度的error差值的积分
float PLL_DeltTheta_Sum_Maxlimit; //PI环路滤波器积分项上限
float PLL_DeltTheta_Sum_Minlimit; //PI环路滤波器积分项下限
float PLL_Omega; //PLL预测的角速度
float PLL_Omega_Maxlimit; //PLL预测的角速度上限
float PLL_Omega_Minlimit; //PLL预测的角速度下限
uint8_t initialized_Flag; // 是否已初始化（第一次接收）
float FOC_FREQ;
float SpeedPllBandwidth;
float SpeedPllKp;
float SpeedPllKi;
float OmegaToTheta; //电角速度转电角度系数
float deadband; //死区,避免噪声在小误差下触发积分/PI动作
}PLL;
.c:
/**
*PLL滤波器，去除电角度噪声
*@param PLL *PLL_Filter 锁相环滤波结构体参数
* float input 需要滤波的参数的测量值（即传感器的采集值）
*@return 滤波后的值
*/
// #define PWM_FREQ 20000.0f
// #define SpeedPllBandwidth 250.0f //rad/s
PLL* Angle_PLL_filter_init(float FOC_FREQ, float PllBandwidth, float deadband, float Max_Angle_Speed)
{
// 参数合法性检查（可根据需要调整阈值）
if (FOC_FREQ <= 0.0f || PllBandwidth <= 0.0f) {
#ifdef DEBUG
DEBUG_OUT("PLL init param invalid: FOC_FREQ=%f, PllBandwidth=%f", FOC_FREQ, PllBandwidth);
#endif
return NULL;
}
// 分配结构体
PLL *pll = (PLL*)MALLOC(sizeof(PLL));
if (!pll) {
#ifdef DEBUG
DEBUG_OUT("PLL allocation failed\n");
#endif
return NULL;
}
// 清零初始化
memset(pll, 0, sizeof(PLL));
// 基本参数赋值（与原静态 init 行为一致）
pll->FOC_FREQ = FOC_FREQ;
pll->SpeedPllBandwidth = PllBandwidth;
// SpeedPllKp = 2 * Wn * zeta （这里默认 zeta = 1.0）
pll->SpeedPllKp = (2.0f * pll->SpeedPllBandwidth * 1.0f);
// SpeedPllKi = Wn^2 * Ts （Ts = 1 / FOC_FREQ）
pll->SpeedPllKi = (pll->SpeedPllBandwidth * pll->SpeedPllBandwidth) / (pll->FOC_FREQ);
// OmegaToTheta (电角速度 -> 电角度的系数) = 1 / FOC_FREQ （原实现）
pll->OmegaToTheta = 1.0f / pll->FOC_FREQ;
// 经验默认值：可按需调整或由调用者设置
pll->deadband = deadband;
pll->PLL_Omega_Maxlimit = Max_Angle_Speed; //系统最大角速度 rad/s 默认给一个极大值
pll->PLL_Omega_Minlimit = - pll->PLL_Omega_Minlimit;
pll->PLL_DeltTheta_Sum_Maxlimit = pll->PLL_Omega_Maxlimit / pll->SpeedPllKi; // PLL_Omega积分累计最大值，保证PLL_Omega积分项不过大
pll->PLL_DeltTheta_Sum_Minlimit = - pll->PLL_DeltTheta_Sum_Maxlimit;
pll->initialized_Flag = 0;
#ifdef DEBUG
DEBUG_OUT("PLL allocated %p FOC_FREQ=%f Bandwidth=%f Kp=%f Ki=%f",
(void*)pll, pll->FOC_FREQ, pll->SpeedPllBandwidth, pll->SpeedPllKp, pll->SpeedPllKi);
#endif
return pll;
}
static inline float Limit_Sat( float fdata,float fmax, float fmin)
{
fdata = (fdata > fmax) ? fmax : fdata;
fdata = (fdata < fmin) ? fmin : fdata;
return fdata;
}
#define Lpf(a,a_last,lpf) (a * lpf + a_last * (1 - lpf))
float Angle_PLL_filter(PLL *PLL_Filter, float eleAngle_input)
{
/* 1. 首次初始化：直接把 PLL_Theta 设为输入，清积分*/
if (!PLL_Filter->initialized_Flag) {
PLL_Filter->PLL_Theta = eleAngle_input;
PLL_Filter->initialized_Flag = 1;
return PLL_Filter->PLL_Theta;
}
/* 2. 计算角度差 */
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);
// PLL_Filter->PLL_DeltTheta = sin_error; // 直接用 sin_error 作为误差值
// 也可以用 atan2f 把近似相位差sin（a-b）转换成实际相位差a-b，范围 (-pi, pi]
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);
PLL_Filter->PLL_DeltTheta = err;
if (fabsf(PLL_Filter->PLL_DeltTheta) < PLL_Filter->deadband) { // 死区处理，避免噪声在小误差下触发积分/PI动作
PLL_Filter->PLL_DeltTheta = 0.0f;
}
/* 3. 计算角速度和角度 */
PLL_Filter->PLL_DeltTheta_Sum += PLL_Filter->PLL_DeltTheta; //PLL积分项
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 * PLL_Filter->PLL_DeltTheta) + \
(PLL_Filter -> SpeedPllKi * PLL_Filter->PLL_DeltTheta_Sum);
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); //最终输出限幅 角度规范化到 [0,2pi)
if (PLL_Filter->PLL_Theta < 0) PLL_Filter->PLL_Theta += 2.0f * PI;
return PLL_Filter->PLL_Theta;
}
// **清空PLL滤波器**
void Angle_PLL_filter_clear(PLL *PLL_Filter) {
if (!PLL_Filter) {
return; // 安全检查
}
// 重置变量
PLL_Filter->initialized_Flag = 0;
PLL_Filter->PLL_DeltTheta_Sum = 0.0f;
PLL_Filter->PLL_Theta = 0.0f;
PLL_Filter->PLL_Omega = 0.0f;
}
void Angle_PLL_filter_deinit(PLL *pll)
{
if (!pll) return;
#ifdef DEBUG
DEBUG_OUT("PLL deinit free %p", (void*)pll);
#endif
FREE(pll);
}
智能硬件社区聚焦AI智能硬件技术生态，汇聚嵌入式AI、物联网硬件开发者，打造交流分享平台，同步全国赛事资讯、开展 OPC 核心人才招募，助力技术落地与开发者成长。
更多推荐
9
0- 0
已为社区贡献15条内容
所有评论(0)