3. 基于扩展卡尔曼滤波的转速和位置估计

任务目标

本案例针对表贴式永磁同步电机（SPMSM），在静止坐标系（α-β轴）下建立电机状态方程，设计扩展卡尔曼滤波器（EKF），利用电机相电流与相电压在线估计转子电角速度与电角度，并与给定实际值进行对比验证。

状态方程与EKF设计

选择状态向量 x = [i_α, i_β, ω_e, θ_e]^T，输入向量 u = [u_α, u_β]^T，测量向量 z = [i_α, i_β]^T。SPMSM在α-β轴下的电压方程为：

di_α/dt = (-R_s·i_α + u_α + ω_e·ψ_f·sinθ_e) / L_s
di_β/dt = (-R_s·i_β + u_β - ω_e·ψ_f·cosθ_e) / L_s
dω_e/dt = 0（假设机械时间常数远大于电时间常数，转速在一个采样周期内近似恒定）
dθ_e/dt = ω_e

对上述连续方程在采样时间 T_s 内进行一阶欧拉离散化，得到非线性离散状态方程 x(k+1) = f(x(k), u(k))。EKF在每个采样周期内执行预测与更新两步：预测阶段计算状态先验估计与误差协方差先验；更新阶段利用测量残差修正状态与协方差。雅可比矩阵 F 与 H 由状态方程对状态向量求偏导获得。

关键完整代码（MATLAB）

%% 参数设置
R_s = 2.875; L_s = 0.0085; psi_f = 0.175;   % 电机参数
T_s = 1e-4;                                 % 采样周期 10kHz
t_end = 0.5; t = 0:T_s:t_end; N = length(t);

% 给定实际转速与位置（斜坡加速至额定转速）
omega_actual = 2*pi*50 * min(t/0.2, 1);     % 电角速度 rad/s
theta_actual = cumsum(omega_actual)*T_s;    % 电角度

% 实际电流（由理想电压方程反推，作为测量真值）
u_alpha = 10*sin(theta_actual); u_beta = 10*cos(theta_actual);
i_alpha_meas = zeros(1,N); i_beta_meas = zeros(1,N);
for k = 1:N-1
    di_a = (-R_s*i_alpha_meas(k) + u_alpha(k) + omega_actual(k)*psi_f*sin(theta_actual(k)))/L_s;
    di_b = (-R_s*i_beta_meas(k)  + u_beta(k)  - omega_actual(k)*psi_f*cos(theta_actual(k)))/L_s;
    i_alpha_meas(k+1) = i_alpha_meas(k) + di_a*T_s;
    i_beta_meas(k+1)  = i_beta_meas(k)  + di_b*T_s;
end
% 添加测量噪声
rng(0);
i_alpha_meas = i_alpha_meas + 0.05*randn(1,N);
i_beta_meas  = i_beta_meas  + 0.05*randn(1,N);

%% EKF初始化
x_est = [0; 0; 0; 0];                       % [i_α, i_β, ω_e, θ_e]
P = diag([0.1, 0.1, 1, 0.1]);               % 初始协方差
Q = diag([1e-4, 1e-4, 0.5, 0.01]);          % 过程噪声
R = diag([0.0025, 0.0025]);                 % 测量噪声（对应0.05^2）

x_log = zeros(4, N); x_log(:,1) = x_est;

%% EKF主循环
for k = 1:N-1
    % --- 预测 ---
    i_a = x_est(1); i_b = x_est(2); w = x_est(3); th = x_est(4);
    % 离散化状态方程 f(x,u)
    f_x = [i_a + T_s*(-R_s*i_a + u_alpha(k) + w*psi_f*sin(th))/L_s;
           i_b + T_s*(-R_s*i_b + u_beta(k)  - w*psi_f*cos(th))/L_s;
           w;
           th + T_s*w];
    % 雅可比矩阵 F = ∂f/∂x
    F = [1 - T_s*R_s/L_s, 0, T_s*psi_f*sin(th)/L_s, T_s*w*psi_f*cos(th)/L_s;
         0, 1 - T_s*R_s/L_s, -T_s*psi_f*cos(th)/L_s, T_s*w*psi_f*sin(th)/L_s;
         0, 0, 1, 0;
         0, 0, T_s, 1];
    x_pred = f_x;
    P_pred = F * P * F' + Q;

    % --- 更新 ---
    H = [1 0 0 0; 0 1 0 0];                 % 测量矩阵（线性）
    z = [i_alpha_meas(k+1); i_beta_meas(k+1)];
    y_res = z - H*x_pred;                   % 测量残差
    S = H*P_pred*H' + R;
    K = P_pred*H' / S;                      % 卡尔曼增益
    x_est = x_pred + K*y_res;
    P = (eye(4) - K*H) * P_pred;

    % 角度归一化到 (-π, π]
    x_est(4) = atan2(sin(x_est(4)), cos(x_est(4)));
    x_log(:,k+1) = x_est;
end

%% 结果对比与可视化
omega_est = x_log(3,:); theta_est = x_log(4,:);
figure;
subplot(2,1,1);
plot(t, omega_actual, 'b-', t, omega_est, 'r--', 'LineWidth', 1.5);
legend('实际转速', 'EKF估计转速'); xlabel('时间 (s)'); ylabel('电角速度 (rad/s)');
title('转速估计对比'); grid on;
subplot(2,1,2);
plot(t, theta_actual, 'b-', t, theta_est, 'r--', 'LineWidth', 1.5);
legend('实际位置', 'EKF估计位置'); xlabel('时间 (s)'); ylabel('电角度 (rad)');
title('转子位置估计对比'); grid on;

% 稳态误差（取后50%数据）
idx = round(N/2):N;
omega_err = rms(omega_est(idx) - omega_actual(idx));
theta_err = rms(angle(exp(1j*(theta_est(idx)-theta_actual(idx)))));
fprintf('稳态转速RMSE: %.4f rad/s\n', omega_err);
fprintf('稳态位置RMSE: %.4f rad\n', theta_err);

运行与验证

运行环境：MATLAB R2019b及以上版本，无需额外工具箱。

运行方式：将上述代码保存为 ekf_pmsm_estimation.m，在MATLAB中直接运行。代码自动生成仿真数据并执行EKF估计。

验证方法：

  1. 运行后观察两个对比子图：转速估计曲线应在约0.1s后紧密跟随实际转速曲线；位置估计曲线应无累积漂移，与实际位置曲线基本重合。
  2. 命令窗口输出的稳态RMSE值应较小（转速RMSE通常小于5 rad/s，位置RMSE小于0.1 rad），数值越小说明估计精度越高。
  3. 可修改R矩阵中的测量噪声方差（增大模拟更差电流传感器），观察估计曲线波动是否相应增大，以验证滤波器对噪声的抑制特性。

关键实现说明

本案例中实际电流由理想电压方程反推生成，仅作为EKF的测量输入，不构成闭环控制。EKF的Q矩阵中转速项（第3个对角元）取值相对较大，以允许转速估计快速收敛；角度项取值较小以平滑位置估计。若电机参数（R_s, L_s, ψ_f）与实际不匹配，稳态估计误差会增大，这是EKF对模型依赖性的直观体现。

← 上一章 📖 返回目录 下一章 →