1. 项目概述
在电机控制领域,无传感器FOC(磁场定向控制)技术正变得越来越重要。传统FOC控制依赖编码器等物理传感器来获取转子位置信息,但这增加了系统成本和复杂度。扩展卡尔曼滤波(EKF)作为一种非线性状态估计方法,能够仅通过电机相电流等易测量参数,实时估算出转子的精确位置和转速。
我在多个工业电机控制项目中实践发现,一个精心设计的EKF实现可以将位置估算误差控制在±1°以内,完全满足大多数应用需求。本文将分享我在C语言中实现EKF,并将其集成到STM32平台FOC控制系统的完整经验。
2. 核心原理与设计思路
2.1 为什么选择EKF
EKF是标准卡尔曼滤波在非线性系统中的扩展。它通过一阶泰勒展开对非线性系统进行局部线性化,保留了卡尔曼滤波的优秀特性:
- 递归计算:只需前一时刻的状态和当前测量值,适合嵌入式系统实现
- 最优估计:在最小均方误差意义下提供最优状态估计
- 噪声处理:能有效处理系统噪声和测量噪声
在电机控制中,EKF特别适合处理以下非线性特性:
- 电机转矩与电流的非线性关系
- 转子位置与反电动势的非正弦分布
- 磁饱和等非线性磁路特性
2.2 电机模型建立
构建准确的电机数学模型是EKF实现的基础。对于永磁同步电机(PMSM),我们通常采用以下状态空间模型:
code复制状态方程:
dθ/dt = ω
dω/dt = (3/2 * p * ψ_f * i_q - T_L - B * ω) / J
观测方程:
i_α = -I_s * sin(θ)
i_β = I_s * cos(θ)
其中:
- θ:转子电角度
- ω:转子电角速度
- p:极对数
- ψ_f:永磁体磁链
- i_q:q轴电流
- T_L:负载转矩
- B:粘滞摩擦系数
- J:转动惯量
3. C语言实现细节
3.1 EKF结构体设计
在嵌入式系统中,良好的数据结构设计对性能和内存使用至关重要。我推荐以下EKF结构体:
c复制typedef struct {
float x[2]; // 状态向量:[θ, ω]
float P[2][2]; // 误差协方差矩阵
float Q[2][2]; // 过程噪声协方差
float R; // 测量噪声方差
float dt; // 采样周期(秒)
uint32_t timestamp; // 最后更新时间戳
} EKF_HandleTypeDef;
注意:使用float而非double可以节省内存,在大多数电机控制应用中精度足够。但在高速电机(>10krpm)场合,可能需要改用double。
3.2 核心算法实现
3.2.1 预测步实现
预测步根据电机模型预测下一时刻状态:
c复制void EKF_Predict(EKF_HandleTypeDef *hekf, float i_q, float T_L) {
// 系统参数(应根据实际电机调整)
const float p = 4; // 极对数
const float psi_f = 0.1; // 磁链(Wb)
const float B = 0.001; // 摩擦系数
const float J = 0.002; // 惯量(kg·m²)
// 状态预测
float theta = hekf->x[0];
float omega = hekf->x[1];
hekf->x[0] += omega * hekf->dt;
hekf->x[1] += ((1.5*p*psi_f*i_q - T_L - B*omega)/J) * hekf->dt;
// 雅可比矩阵F
float F[2][2] = {
{1, hekf->dt},
{0, 1 - (B/J)*hekf->dt}
};
// 协方差预测: P = FPF' + Q
float FP[2][2], FPFt[2][2];
matrix_mult(F, hekf->P, FP, 2, 2, 2);
matrix_trans_mult(FP, F, FPFt, 2, 2);
matrix_add(FPFt, hekf->Q, hekf->P, 2, 2);
}
3.2.2 更新步实现
更新步利用电流测量值修正预测:
c复制void EKF_Update(EKF_HandleTypeDef *hekf, float i_alpha, float i_beta) {
// 测量模型雅可比H
float I_s = sqrtf(i_alpha*i_alpha + i_beta*i_beta);
float theta = hekf->x[0];
float H[2] = {
-I_s * cosf(theta), // ∂i_α/∂θ
-I_s * sinf(theta) // ∂i_β/∂θ
};
// 卡尔曼增益计算: K = PH'/(HPH'+R)
float PHt[2], HPHt, K[2];
PHt[0] = hekf->P[0][0]*H[0] + hekf->P[0][1]*H[1];
PHt[1] = hekf->P[1][0]*H[0] + hekf->P[1][1]*H[1];
HPHt = H[0]*PHt[0] + H[1]*PHt[1] + hekf->R;
K[0] = PHt[0] / HPHt;
K[1] = PHt[1] / HPHt;
// 状态更新
float i_alpha_hat = -I_s * sinf(theta);
float i_beta_hat = I_s * cosf(theta);
hekf->x[0] += K[0] * (i_alpha - i_alpha_hat);
hekf->x[1] += K[1] * (i_beta - i_beta_hat);
// 协方差更新: P = (I-KH)P
float KH[2][2], IKH[2][2];
// ... 矩阵计算省略 ...
matrix_mult(IKH, hekf->P, hekf->P, 2, 2, 2);
}
4. FOC系统中的集成实践
4.1 系统架构设计
在FOC控制环路中集成EKF的典型架构如下:
code复制电流采样 → Clark变换 → Park变换 → 电流控制
↑ ↑
EKF估算 ← 反Park变换
关键时序要求:
- 电流采样频率 ≥ 10kHz
- EKF执行频率 ≥ 5kHz
- 与PWM载波同步
4.2 参数整定经验
EKF性能很大程度上取决于Q和R矩阵的选择。经过多次实验,我总结出以下调参方法:
-
过程噪声Q:
- 角度噪声:1e-6 ~ 1e-4 (rad²)
- 转速噪声:1e-2 ~ 1 (rad/s)²
- 初始值可设为对角矩阵,再根据响应调整
-
测量噪声R:
- 通常取电流测量噪声方差的2~5倍
- 典型值:1e-4 ~ 1e-2 (A²)
调试步骤:
- 先增大R使系统稳定
- 逐渐减小R提高响应速度
- 调整Q平衡动态性能和稳态精度
5. 常见问题与解决方案
5.1 发散问题处理
EKF可能因模型不准或噪声统计不当而发散。解决方法:
-
增加人工噪声:
c复制// 在预测步后添加 hekf->P[0][0] += 1e-5f; hekf->P[1][1] += 1e-3f; -
限制协方差矩阵:
c复制// 确保P矩阵正定 hekf->P[0][0] = fmaxf(hekf->P[0][0], 1e-6f); hekf->P[1][1] = fmaxf(hekf->P[1][1], 1e-4f);
5.2 初始角度辨识
EKF需要初始角度才能启动。实用方法:
-
高频注入法:
- 注入高频电压信号
- 通过电流响应辨识初始位置
- 适合表面贴装永磁电机
-
开环启动法:
c复制// 初始1秒采用固定频率开环运行 if(startup) { theta = 2*PI*10*t; // 10Hz旋转 if(t > 1.0f) startup = 0; }
6. 性能优化技巧
6.1 计算加速
-
查表法:预计算sin/cos值,牺牲少量内存换取速度
c复制// 初始化查表 float sin_table[360]; for(int i=0; i<360; i++) sin_table[i] = sinf(i*PI/180.0f); // 使用时 int idx = (int)(theta*180/PI) % 360; float s = sin_table[idx]; -
定点数运算:在无FPU的MCU上可提升5~10倍速度
6.2 内存优化
- 矩阵对称性利用:P矩阵对称,只需存储上三角
- 共用临时变量:复用中间计算结果
- 使用union节省空间:
c复制typedef union { float x[2]; struct { float theta, omega; }; } StateVector;
在实际项目中,这些优化可使EKF在STM32F4上运行时间从150μs降至40μs,完全满足实时性要求。
