当前位置：首页 > news >正文

【MATLAB代码】扩展卡尔曼滤波估计pmsm的位置误差

news 来源：原创 2025/5/24 8:58:40

在这里插入图片描述

基于扩展卡尔曼滤波（EKF）估计永磁同步电机（PMSM）位置误差的 MATLAB 实现和说明。代码分为系统建模、EKF 算法实现和仿真验证三部分。

文章目录

系统建模
- 核心算法解析
- - 1. 系统建模
  - 2. 噪声配置
  - 3. EKF实现流程
  - 4. 输入信号与仿真
MATLAB 代码
运行结果

本代码实现了基于**扩展卡尔曼滤波（EKF）**的永磁同步电机转子位置与速度的无传感器估计。通过建立PMSM的动力学模型，结合电压输入与电流观测，算法能够实时预测电机的转子位置（θ）、转速（ω）及交直轴电流（id/iq），有效解决了传统传感器依赖带来的成本和可靠性问题。

系统建模

建立 PMSM 的状态方程和输出方程，并进行离散化处理。

状态方程和离散化
- 使用非线性状态方程 $f (x, u)$ 模拟 PMSM 的动态行为，并在 EKF 中离散化处理。
噪声建模
- 过程噪声 $Q$ 和测量噪声 $R$ 是 EKF 的关键参数，需根据实际系统调整。

核心算法解析

1. 系统建模

状态变量：包含转子位置（θ）、转速（ω）、d轴电流（id）、q轴电流（iq），构成4维状态向量x = [θ, ω, id, iq]^T。

状态方程：基于PMSM电磁与机械动力学方程，离散化后描述状态演化：

f = @(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（此处为恒定矩阵，实际应用中需动态计算状态依赖的雅可比矩阵）：
```
F = [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 代码

% PMSM估计
% 2025-05-23/Ver1clc;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 * Tsx(2) + Ts * (3*P/2*(lambda_f*x(4) + (Ld-Lq)*x(3)*x(4))/J - B*x(2)/J); % omegax(3) + Ts * (u(1) - Rs*x(3) + x(2)*Lq*x(4))/Ld; % idx(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;