2. 滑模观测器转子位置估算与误差补偿

任务目标

本案例实现永磁同步电机（PMSM）无传感器控制中的转子位置估算：构建滑模观测器（SMO）从静止坐标系下的电流与电压中估算反电动势，进而提取转子位置；针对滑模抖振引起的估算误差，设计锁相环（PLL）对位置信号进行滤波与补偿，输出平滑的转子位置与速度。

核心原理与实现路径

滑模观测器基于PMSM在αβ轴系下的电压方程构建。通过滑模面切换函数逼近反电动势分量，再经低通滤波得到连续的反电动势估算值。转子位置由反电动势的反正切求得，但滑模抖振与滤波延迟会引入相位误差，因此采用PLL对位置误差进行闭环修正，PLL的输出即为补偿后的转子位置与电角速度。

关键完整代码

// smo_pll_estimator.c
#include <math.h>

#define LSMO_GAIN    100.0f   // 滑模增益
#define LPF_CUTOFF   100.0f   // 低通滤波截止频率(Hz)
#define POLE_PAIRS   4        // 电机极对数
#define TS           0.0001f  // 控制周期(s)

typedef struct {
    float ialpha, ibeta;      // 静止坐标系电流
    float valpha, vbeta;      // 静止坐标系电压
    float ealpha_lpf, ebeta_lpf; // 滤波后反电动势
    float theta_pll, omega_pll;  // PLL输出位置与速度
} SMO_PLL_State;

// 低通滤波系数
static float lpf_coeff(float dt, float fc) {
    float rc = 1.0f / (2.0f * M_PI * fc);
    return dt / (rc + dt);
}

// 滑模观测器 + PLL 位置估算（单步执行）
void smo_pll_step(SMO_PLL_State *s, float ialpha, float ibeta,
                  float valpha, float vbeta, float dt) {
    // 1. 电流误差滑模面
    float err_alpha = ialpha - s->ialpha;
    float err_beta  = ibeta  - s->ibeta;

    // 2. 滑模控制律（符号函数）
    float zalpha = LSMO_GAIN * (err_alpha > 0 ? 1.0f : -1.0f);
    float zbeta  = LSMO_GAIN * (err_beta  > 0 ? 1.0f : -1.0f);

    // 3. 电流观测器方程（忽略电阻与电感压降，仅保留反电动势项）
    float dialpha = (valpha - s->ealpha_lpf - zalpha) * dt;
    float dibeta  = (vbeta  - s->ebeta_lpf  - zbeta)  * dt;
    s->ialpha += dialpha;
    s->ibeta  += dibeta;

    // 4. 反电动势由滑模输出经低通滤波获得
    float k = lpf_coeff(dt, LPF_CUTOFF);
    s->ealpha_lpf += k * (zalpha - s->ealpha_lpf);
    s->ebeta_lpf  += k * (zbeta  - s->ebeta_lpf);

    // 5. PLL 提取位置与速度
    //    反电动势矢量角 = atan2(-ealpha, ebeta)（与转子位置相关）
    float theta_raw = atan2f(-s->ealpha_lpf, s->ebeta_lpf);

    //    位置误差（角度差归一化到 [-pi, pi]）
    float err = theta_raw - s->theta_pll;
    while (err > M_PI)  err -= 2.0f * M_PI;
    while (err < -M_PI) err += 2.0f * M_PI;

    //    PI 调节器（比例+积分）
    float kp = 200.0f, ki = 2000.0f;
    s->omega_pll += ki * err * dt;
    s->theta_pll += (s->omega_pll + kp * err) * dt;

    //    角度归一化
    while (s->theta_pll > M_PI)  s->theta_pll -= 2.0f * M_PI;
    while (s->theta_pll < -M_PI) s->theta_pll += 2.0f * M_PI;
}

运行与验证

环境依赖：需要数学库 libm，编译时链接 -lm。将上述代码保存为 smo_pll_estimator.c，并添加一个测试主函数：

// test_main.c
#include <stdio.h>
#include <math.h>

// 模拟电机模型：给定真实位置，生成电流与电压
void motor_model(float theta_e, float omega_e, float dt,
                 float *ialpha, float *ibeta,
                 float *valpha, float *vbeta) {
    float R = 1.0f, L = 0.005f, flux = 0.1f;
    float ia = 2.0f * sinf(theta_e);
    float ib = 2.0f * cosf(theta_e);
    // 反电动势
    float ea = -flux * omega_e * sinf(theta_e);
    float eb =  flux * omega_e * cosf(theta_e);
    // 电压方程（简化）
    *valpha = R * ia + L * (ia - 0) / dt + ea;
    *vbeta  = R * ib + L * (ib - 0) / dt + eb;
    *ialpha = ia;
    *ibeta  = ib;
}

int main() {
    SMO_PLL_State smo = {0};
    float theta_real = 0.0f, omega_e = 100.0f; // 电角速度 100 rad/s
    float dt = TS;
    float ia, ib, va, vb;

    for (int i = 0; i < 5000; i++) {
        theta_real += omega_e * dt;
        while (theta_real > 2*M_PI) theta_real -= 2*M_PI;

        motor_model(theta_real, omega_e, dt, &ia, &ib, &va, &vb);
        smo_pll_step(&smo, ia, ib, va, vb, dt);

        if (i % 500 == 0) {
            float err = theta_real - smo.theta_pll;
            while (err > M_PI)  err -= 2*M_PI;
            while (err < -M_PI) err += 2*M_PI;
            printf("t=%.3f 真实位置=%.3f 估算位置=%.3f 误差=%.3f rad\n",
                   i*dt, theta_real, smo.theta_pll, err);
        }
    }
    return 0;
}

编译运行：

gcc -o test_smo smo_pll_estimator.c test_main.c -lm
./test_smo

通过条件：程序输出中，估算位置与真实位置的误差在0.5s后稳定收敛至±0.05 rad以内，且不随运行时间发散。若误差持续偏大或振荡，可调整LSMO_GAIN（增大以增强收敛）或LPF_CUTOFF（降低以平滑反电动势，但会增加相位延迟，需PLL补偿）。

误差补偿说明

滑模观测器输出的反电动势经低通滤波后存在相位滞后，直接反正切求取的位置会滞后于真实位置。本案例中的PLL环路通过积分作用自动消除该稳态相位误差：当theta_raw因滤波滞后而小于真实位置时，PLL的误差项持续为正，积分器提升omega_pll，使theta_pll加速追赶，最终收敛到真实位置。因此PLL同时实现了滤波与相位补偿双重功能。