基于扩展卡尔曼滤波（EKF）估计永磁同步电机（PMSM）位置误差的 MATLAB 实现和说明。代码分为系统建模、EKF 算法实现和仿真验证三部分。
文章目录
- 系统建模
-
- 核心算法解析
-
- [1. 系统建模](#1. 系统建模)
- [2. 噪声配置](#2. 噪声配置)
- [3. EKF实现流程](#3. EKF实现流程)
- [4. 输入信号与仿真](#4. 输入信号与仿真)
- [MATLAB 代码](#MATLAB 代码)
- 运行结果
本代码实现了基于**扩展卡尔曼滤波（EKF）**的永磁同步电机转子位置与速度的无传感器估计。通过建立PMSM的动力学模型，结合电压输入与电流观测，算法能够实时预测电机的转子位置（θ）、转速（ω）及交直轴电流（id/iq），有效解决了传统传感器依赖带来的成本和可靠性问题。
系统建模
建立 PMSM 的状态方程和输出方程，并进行离散化处理。
-
状态方程和离散化
- 使用非线性状态方程 f ( x , u ) f(x, u) f(x,u)模拟 PMSM 的动态行为，并在 EKF 中离散化处理。
-
噪声建模
- 过程噪声 Q Q Q和测量噪声 R R R是 EKF 的关键参数，需根据实际系统调整。
核心算法解析
1. 系统建模
-
状态变量 ：包含转子位置（θ）、转速（ω）、d轴电流（id）、q轴电流（iq），构成4维状态向量
x = [θ, ω, id, iq]^T。 -
状态方程 ：基于PMSM电磁与机械动力学方程，离散化后描述状态演化：
matlabf = @(x, u) [ x(1) + Ts * x(2); % 位置更新 x(2) + Ts*(3*P/2*(λ*x(4)+(Ld-Lq)*x(3)*x(4))/J - B*x(2)/J); % 转速更新 x(3) + Ts*(u(1)-Rs*x(3)+x(2)*Lq*x(4))/Ld; % d轴电流 x(4) + Ts*(u(2)-Rs*x(4)-x(2)*(Ld*x(3)+λ))/Lq % q轴电流 ];该模型考虑了电磁转矩、摩擦阻力及电感特性对状态的影响。
2. 噪声配置
- 过程噪声协方差Q：对角矩阵，反映各状态变量的过程噪声强度（位置/速度噪声较小，电流噪声较大）。
- 观测噪声协方差R ：针对电流测量噪声，体现传感器精度限制。
噪声参数的合理设置是保证EKF鲁棒性的关键。
3. EKF实现流程
-
预测步骤 ：
-
状态预测：通过非线性状态方程更新状态估计。
-
协方差预测：基于简化雅可比矩阵
F（此处为恒定矩阵，实际应用中需动态计算状态依赖的雅可比矩阵）：matlabF = [1, Ts, 0, 0; 0, 1, 0, 0; 0, 0, 1, Ts; 0, 0, 0, 1];
-
-
更新步骤 ：
- 计算卡尔曼增益
K，融合电流观测值（id/iq）修正状态估计。 - 观测雅可比矩阵
H为常量，因输出方程仅直接观测电流。
- 计算卡尔曼增益
4. 输入信号与仿真
- 电压输入：采用50Hz正弦（vd）和余弦（vq）信号模拟三相逆变器输出，驱动电机运行。
- 仿真时长：1秒，采样时间100μs，共10000步，满足实时性需求。
MATLAB 代码
matlab
% PMSM估计
% 2025-05-23/Ver1
clc;clear;close all;
rng(0);
% 系统参数设置
P = 4; % 极对数
Rs = 0.2; % 定子电阻 (ohm)
Ld = 0.002; % 直轴电感 (H)
Lq = 0.002; % 交轴电感 (H)
lambda_f = 0.1; % 永磁磁链 (Wb)
J = 0.001; % 转动惯量 (kg·m^2)
B = 0.0001; % 摩擦系数 (N·m·s/rad)
Ts = 1e-4; % 采样时间 (s)
% 初始状态 (假设真实值)
theta_true = 0; % 转子位置 (rad)
omega_true = 0; % 转子速度 (rad/s)
id_true = 0; % d轴电流 (A)
iq_true = 0; % q轴电流 (A)
% 初始估计值
theta_hat = 0; % 初始位置估计
omega_hat = 0; % 初始速度估计
id_hat = 0; % 初始 d 轴电流估计
iq_hat = 0; % 初始 q 轴电流估计
% 噪声设置
Q = diag([1e-6, 1e-6, 1e-4, 1e-4]); % 过程噪声协方差
R = diag([1e-3, 1e-3]); % 测量噪声协方差
% 状态方程
f = @(x, u) [ ...
x(1) + Ts * x(2); % theta = theta + omega * Ts
x(2) + Ts * (3*P/2*(lambda_f*x(4) + (Ld-Lq)*x(3)*x(4))/J - B*x(2)/J); % omega
x(3) + Ts * (u(1) - Rs*x(3) + x(2)*Lq*x(4))/Ld; % id
x(4) + Ts * (u(2) - Rs*x(4) - x(2)*(Ld*x(3) + lambda_f))/Lq % iq
];
% 输出方程
h = @(x) [x(3); x(4)]; % 输出电流 (id, iq)
% 初始化协方差矩阵
P = diag([1e-4, 1e-4, 1e-2, 1e-2]); % 初始化状态协方差
% 仿真时间
T_end = 1; % 仿真 1 秒
N = T_end / Ts; % 仿真步数
% 输入电压 (正弦波信号)
vd = 10 * sin(2*pi*50*(0:Ts:(T_end-Ts)));
vq = 10 * cos(2*pi*50*(0:Ts:(T_end-Ts)));
% 数据存储
theta_error = zeros(1, N); % 位置误差存储
theta_est = zeros(1, N); % 估计位置存储
theta_real = zeros(1, N); % 实际位置存储
% EKF 循环
for k = 1:N
% 获取输入电压
u = [vd(k); vq(k)];
% 真实系统更新 (加噪声)
x_true = [theta_true; omega_true; id_true; iq_true];
x_true = f(x_true, u) + sqrt(Q) * randn(4, 1); % 真实状态加过程噪声
theta_true = x_true(1);
omega_true = x_true(2);
id_true = x_true(3);
iq_true = x_true(4);
% 测量值 (加噪声)
z = h(x_true) + sqrt(R) * randn(2, 1);
% EKF 预测步骤
x_hat = [theta_hat; omega_hat; id_hat; iq_hat];
x_pred = f(x_hat, u); % 状态预测
F = [1, Ts, 0, 0; ... % 状态方程的雅可比矩阵
0, 1, 0, 0; ...
0, 0, 1, Ts; ...
0, 0, 0, 1];
P_pred = F * P * F' + Q; % 协方差预测
% EKF 更新步骤
H = [0, 0, 1, 0; ... % 输出方程的雅可比矩阵
0, 0, 0, 1];
K = P_pred * H' / (H * P_pred * H' + R); % 卡尔曼增益
x_hat = x_pred + K * (z - h(x_pred)); % 状态更新
P = (eye(4) - K * H) * P_pred; % 协方差更新
% 更新估计值
theta_hat = x_hat(1);
omega_hat = x_hat(2);
id_hat = x_hat(3);
iq_hat = x_hat(4);
% 记录误差
theta_error(k) = theta_true - theta_hat;
theta_est(k) = theta_hat;
theta_real(k) = theta_true;
end
% 绘制真实位置与估计位置
t = 0:Ts:(T_end-Ts);
figure;
subplot(2, 1, 1);
plot(t, theta_real, 'b', 'LineWidth', 1.5); hold on;
plot(t, theta_est, 'r--', 'LineWidth', 1.5);
xlabel('Time (s)');
ylabel('Position (rad)');
legend('真实位置', '估计位置');
title('PMSM EKF位置估计');
% grid on;
% 绘制位置误差
subplot(2, 1, 2);
plot(t, theta_error, 'k', 'LineWidth', 1.5);
xlabel('Time (s)');
ylabel('位置误差(rad)');
title('位置估计误差');
% grid on;运行结果
如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者