扩展卡尔曼滤波器（EKF）

扩展卡尔曼滤波器（EKF）

扩展卡尔曼滤波器（Extended Kalman Filter, EKF）是卡尔曼滤波器的一种扩展，专门用于处理非线性系统的状态估计问题。在电机控制领域，EKF常用于磁链观测器的设计，以提高对转子位置和速度的估算精度。相比于传统的磁链观测器，EKF能够更有效地处理系统的非线性特性，并对噪声和参数不确定性具有更好的鲁棒性。

1. 基本原理

卡尔曼滤波器是一种递归算法，用于线性系统的状态估计。它通过对系统的动态模型进行预测，并结合测量数据进行校正，得到最优的状态估计值。然而，许多实际系统是非线性的，扩展卡尔曼滤波器通过对非线性系统进行线性化处理，扩展了卡尔曼滤波器的应用范围。

非线性系统模型

一个典型的非线性系统可以描述为：

状态方程：
$${\mathbf{x}}_{k+1}=\mathbf{f}({\mathbf{x}}_k,{\mathbf{u}}_k)+{\mathbf{w}}_k$$

观测方程：
$${\mathbf{z}}_k=\mathbf{h}({\mathbf{x}}_k)+{\mathbf{v}}_k$$

其中：

• ${\mathbf{x}}_k$ 是系统状态向量。
• ${\mathbf{u}}_k$ 是控制输入向量。
• ${\mathbf{z}}_k$ 是测量向量。
• $\mathbf{f}(\cdot )$ 是状态转移函数（非线性）。
• $\mathbf{h}(\cdot )$ 是测量函数（非线性）。
• ${\mathbf{w}}_k$ 和 ${\mathbf{v}}_k$ 分别是过程噪声和测量噪声，通常假设它们是零均值的高斯白噪声，协方差分别为 ${\mathbf{Q}}_k$ 和 ${\mathbf{R}}_k$。

2. 扩展卡尔曼滤波器的步骤

EKF 的算法可以分为两个主要步骤：预测和更新。由于系统的非线性特性，这两个步骤中的状态和观测方程需要进行线性化处理。

1. 预测步骤

首先，利用系统的非线性状态方程对下一个时刻的状态进行预测：

$${\hat{\mathbf{x}}}_{k+1|k}=\mathbf{f}({\hat{\mathbf{x}}}_k,{\mathbf{u}}_k)$$

然后，计算预测状态的协方差矩阵：

$${\mathbf{P}}_{k+1|k}={\mathbf{F}}_k{\mathbf{P}}_k{\mathbf{F}}_k^T+{\mathbf{Q}}_k$$

其中，${\mathbf{F}}_k$ 是状态转移矩阵的雅可比矩阵（对 $\mathbf{f}$ 关于 $\mathbf{x}$ 的偏导数矩阵）：

$${\mathbf{F}}_k=\frac{\partial \mathbf{f}({\mathbf{x}}_k,{\mathbf{u}}_k)}{\partial {\mathbf{x}}_k}{|}_{{\hat{\mathbf{x}}}_k}$$

2. 更新步骤

当有新的测量 ${\mathbf{z}}_{k+1}$ 时，首先计算测量残差：

$${\mathbf{y}}_{k+1}={\mathbf{z}}_{k+1}-\mathbf{h}({\hat{\mathbf{x}}}_{k+1|k})$$

接着，计算测量残差的协方差：

$${\mathbf{S}}_{k+1}={\mathbf{H}}_{k+1}{\mathbf{P}}_{k+1|k}{\mathbf{H}}_{k+1}^T+{\mathbf{R}}_{k+1}$$

其中，${\mathbf{H}}_{k+1}$ 是测量矩阵的雅可比矩阵（对 $\mathbf{h}$ 关于 $\mathbf{x}$ 的偏导数矩阵）：

$${\mathbf{H}}_{k+1}=\frac{\partial \mathbf{h}({\mathbf{x}}_{k+1})}{\partial {\mathbf{x}}_{k+1}}{|}_{{\hat{\mathbf{x}}}_{k+1|k}}$$

然后，计算卡尔曼增益：

$${\mathbf{K}}_{k+1}={\mathbf{P}}_{k+1|k}{\mathbf{H}}_{k+1}^T{\mathbf{S}}_{k+1}^{-1}$$

最后，利用测量数据更新状态估计值和协方差矩阵：

$${\hat{\mathbf{x}}}_{k+1}={\hat{\mathbf{x}}}_{k+1|k}+{\mathbf{K}}_{k+1}{\mathbf{y}}_{k+1}$$

$${\mathbf{P}}_{k+1}=(\mathbf{I}-{\mathbf{K}}_{k+1}{\mathbf{H}}_{k+1}){\mathbf{P}}_{k+1|k}$$

3. 电机模型

在永磁同步电机中，$d$-$q$轴下的电机模型可以用以下状态方程描述：

状态方程：
$$\begin{array}{rl}\frac{d{\lambda }_d}{dt}& =v_d-R_s\cdot i_d+\omega \cdot {\lambda }_q\\ \frac{d{\lambda }_q}{dt}& =v_q-R_s\cdot i_q-\omega \cdot {\lambda }_d\\ \frac{d\omega }{dt}& =\frac{T_e-T_L}{J}\end{array}$$

其中：

• ${\lambda }_d$ 和 ${\lambda }_q$ 是$d$轴和$q$轴的磁链。
• $v_d$ 和 $v_q$ 是$d$轴和$q$轴的电压。
• $i_d$ 和 $i_q$ 是$d$轴和$q$轴的电流。
• $\omega$ 是电机转子电角速度。
• $R_s$ 是定子电阻。
• $T_e$ 是电磁转矩，可以由磁链和电流计算得到。
• $T_L$ 是负载转矩，通常假设已知或为常数。
• $J$ 是电机转动惯量。

观测方程（测量方程）为：
$$\begin{array}{rl}{i}_d& =\frac{{\lambda }_d}{L_d}\\ i_q& =\frac{{\lambda }_q}{L_q}\end{array}$$

其中，$L_d$ 和 $L_q$ 是电机的电感。

补充 电磁转矩公式

在永磁同步电机（PMSM）的$d$-$q$坐标系中，电磁转矩的计算公式通常为：

$$T_e=\frac{3}{2}\cdot p\cdot ({\lambda }_d\cdot i_q-{\lambda }_q\cdot i_d)$$

其中：

• $T_e$ 是电磁转矩，单位通常为牛·米（Nm）。
• $p$ 是电机的极对数。
• ${\lambda }_d$ 和 ${\lambda }_q$ 分别是$d$轴和$q$轴的磁链，单位通常为韦伯（Wb）。
• $i_d$ 和 $i_q$ 分别是$d$轴和$q$轴的电流，单位为安培（A）。

公式解释

• ${\lambda }_d\cdot i_q$：这是产生电磁转矩的一个主要项，$d$轴磁链与$q$轴电流的乘积。
• ${\lambda }_q\cdot i_d$：这是另一个项，$q$轴磁链与$d$轴电流的乘积。对于常见的永磁同步电机，通常希望$d$轴电流$i_d$接近零，以优化电磁转矩的产生。
• $\frac{3}{2}\cdot p$：这是一个常数因子，它考虑了电机极对数和电机的相数（永磁同步电机通常为三相）。

4. C语言实现

#include <stdio.h>
#include <math.h>

// 电机参数定义
#define Rs 1.0f     // 定子电阻，单位：欧姆
#define Ld 0.005f   // d轴电感，单位：亨
#define Lq 0.005f   // q轴电感，单位：亨
#define J  0.01f    // 电机转动惯量，单位：kg·m²
#define B  0.001f   // 阻尼系数，单位：N·m·s
#define Ts 0.001f   // 采样时间，单位：秒
#define P  2        // 极对数

// EKF状态变量
typedef struct {
    float lambda_d;  // d轴磁链
    float lambda_q;  // q轴磁链
    float omega;     // 转子电角速度
    float theta;     // 转子电角度
    float P[3][3];   // 状态协方差矩阵
} EKF_State;

// 电机输入结构
typedef struct {
    float v_d;       // d轴电压
    float v_q;       // q轴电压
    float i_d;       // d轴电流
    float i_q;       // q轴电流
} Motor_Input;

// 定义系统噪声协方差矩阵Q和测量噪声协方差矩阵R
float Q[3][3] = {
    {0.001f, 0.0f, 0.0f},
    {0.0f, 0.001f, 0.0f},
    {0.0f, 0.0f, 0.01f}
};

float R[2][2] = {
    {0.01f, 0.0f},
    {0.0f, 0.01f}
};

// 电磁转矩计算
float calculate_electromagnetic_torque(float lambda_d, float lambda_q, float i_d, float i_q) {
    return (3.0f / 2.0f) * P * (lambda_d * i_q - lambda_q * i_d);
}

// EKF预测步骤
void EKF_Predict(EKF_State *state, Motor_Input *input, float T_L) {
    // 计算电磁转矩
    float T_e = calculate_electromagnetic_torque(state->lambda_d, state->lambda_q, input->i_d, input->i_q);

    // 状态预测
    float lambda_d_dot = input->v_d - Rs * input->i_d + state->omega * state->lambda_q;
    float lambda_q_dot = input->v_q - Rs * input->i_q - state->omega * state->lambda_d;
    float omega_dot = (T_e - T_L - B * state->omega) / J;

    state->lambda_d += Ts * lambda_d_dot;
    state->lambda_q += Ts * lambda_q_dot;
    state->omega += Ts * omega_dot;

    // 计算雅可比矩阵F
    float F[3][3] = {
        {1.0f, Ts * state->omega, Ts * state->lambda_q},
        {-Ts * state->omega, 1.0f, -Ts * state->lambda_d},
        {0.0f, 0.0f, 1.0f}
    };

    // 更新状态协方差矩阵 P = F * P * F^T + Q
    float P_new[3][3];
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            P_new[i][j] = 0.0f;
            for (int k = 0; k < 3; k++) {
                P_new[i][j] += F[i][k] * state->P[k][j];
            }
            P_new[i][j] += Q[i][j];
        }
    }

    // 更新协方差矩阵
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            state->P[i][j] = P_new[i][j];
        }
    }
}

// EKF更新步骤
void EKF_Update(EKF_State *state, Motor_Input *input) {
    // 计算测量残差 y = z - h(x)
    float y[2];
    y[0] = input->i_d - state->lambda_d / Ld;
    y[1] = input->i_q - state->lambda_q / Lq;

    // 计算测量矩阵H
    float H[2][3] = {
        {1.0f / Ld, 0.0f, 0.0f},
        {0.0f, 1.0f / Lq, 0.0f}
    };

    // 计算测量残差协方差矩阵 S = H * P * H^T + R
    float S[2][2];
    for (int i = 0; i < 2; i++) {
        for (int j = 0; j < 2; j++) {
            S[i][j] = 0.0f;
            for (int k = 0; k < 3; k++) {
                S[i][j] += H[i][k] * state->P[k][j];
            }
            S[i][j] += R[i][j];
        }
    }

    // 计算卡尔曼增益矩阵 K = P * H^T * S^(-1)
    float K[3][2];
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 2; j++) {
            K[i][j] = 0.0f;
            for (int k = 0; k < 3; k++) {
                K[i][j] += state->P[i][k] * H[j][k];
            }
            K[i][j] /= S[j][j];
        }
    }

    // 更新状态估计 x = x + K * y
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 2; j++) {
            state->lambda_d += K[0][j] * y[j];
            state->lambda_q += K[1][j] * y[j];
            state->omega +=

 K[2][j] * y[j];
        }
    }

    // 更新状态协方差矩阵 P = (I - K * H) * P
    float P_new[3][3];
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            P_new[i][j] = state->P[i][j];
            for (int k = 0; k < 2; k++) {
                P_new[i][j] -= K[i][k] * H[k][j];
            }
        }
    }

    // 更新协方差矩阵
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            state->P[i][j] = P_new[i][j];
        }
    }
}

// 转子角度计算
void Calculate_Rotor_Angle(EKF_State *state) {
    state->theta += Ts * state->omega;
    if (state->theta > 2 * M_PI) {
        state->theta -= 2 * M_PI;
    } else if (state->theta < 0) {
        state->theta += 2 * M_PI;
    }
}

// 主函数
int main() {
    // 初始化 EKF 状态
    EKF_State ekf_state = {
        .lambda_d = 0.0f,
        .lambda_q = 0.0f,
        .omega = 0.0f,
        .theta = 0.0f,
        .P = {
            {1.0f, 0.0f, 0.0f},
            {0.0f, 1.0f, 0.0f},
            {0.0f, 0.0f, 1.0f}
        }
    };

    // 初始化电机输入 (假数据)
    Motor_Input motor_input = {1.0f, 0.5f, 0.2f, 0.1f};

    // 负载转矩 (假设已知)
    float T_L = 0.1f;

    // 执行 EKF 预测和更新步骤
    for (int i = 0; i < 1000; i++) {
        EKF_Predict(&ekf_state, &motor_input, T_L);
        EKF_Update(&ekf_state, &motor_input);
        Calculate_Rotor_Angle(&ekf_state);

        // 输出转子角度
        printf("Step %d: Rotor Angle (rad): %f, Speed (rad/s): %f\n", i, ekf_state.theta, ekf_state.omega);
    }

    return 0;
}

5. 代码详细说明

1. 电机模型：

   • 使用$d$-$q$坐标系下的电机模型，状态变量包括${\lambda }_d$、${\lambda }_q$和$\omega$。

2. EKF_Predict 函数：

   • 执行预测步骤，基于当前状态和输入电压预测下一个时刻的状态。

3. EKF_Update 函数：

   • 使用当前的测量电流值更新状态估计，计算卡尔曼增益并修正预测结果。

4. Calculate_Rotor_Angle 函数：

   • 根据估算的电角速度$\omega$来计算转子电角度$\theta$。

5. 主函数：

   • 模拟运行1000步，逐步更新状态估计和转子角度。

6. 扩展卡尔曼滤波器的优缺点

优点：

• 处理非线性系统：能够有效处理电机模型中的非线性特性。
• 高精度状态估计：通过卡尔曼滤波算法，能够提供较为精确的状态估计。

缺点：

• 计算复杂度高：涉及矩阵运算和逆运算，在实时系统中可能需要优化。
• 对初始条件敏感：初始状态估计和协方差矩阵的设定对滤波效果有较大影响。

7. 实际应用注意事项

• 初始化：状态向量和协方差矩阵的初始化非常重要，应根据实际电机参数设定合理的初始值。
• 实时性：确保算法在目标嵌入式系统上能够实时运行，可以考虑对矩阵运算进行优化。
• 噪声建模：合理设定过程噪声和测量噪声的协方差矩阵$Q$和$R$，这将显著影响EKF的估计精度。

如果你有任何疑问或需要进一步解释，请随时告诉我！

8. 总结

扩展卡尔曼滤波器是一种强大的状态估计工具，尤其适用于非线性系统。在电机控制中，EKF 可以用来估算转子位置、速度和磁链等关键参数，从而实现高性能的无传感器控制。然而，由于其计算复杂性，在实际应用中需要仔细设计和优化，确保系统的实时性和精度。