Motor Foc Control

FOC 磁场定向控制深度指南 v2.1

面向电机工程实践，系统讲解 FOC 磁场定向控制的原理、实现、调参要点。 新增：PI自动整定脚本、弱磁深度控制、故障诊断完整流程。

目录

1. FOC 基本原理
2. SVPWM 原理与实现
3. MTPA（最大转矩电流比）控制
4. 弱磁控制
5. 磁链观测器
6. PI 参数整定指南
7. 工程参数速查
8. 常见问题排查
9. 参考资源

FOC 基本原理

坐标变换

三相静止坐标系 (ABC)
        ↓ Clarke 变换
两相静止坐标系 (α, β)
        ↓ Park 变换  
两相旋转坐标系 (d, q)

关键公式：
Clarke:  Iα = Ia
         Iβ = (Ia + 2Ib)/√3

Park:    Id = Iα·cos(θ) + Iβ·sin(θ)
         Iq = -Iα·sin(θ) + Iβ·cos(θ)

逆变换：
Iα = Id·cos(θ) - Iq·sin(θ)
Iβ = Id·sin(θ) + Iq·cos(θ)

三相重构：
Ia = Iα
Ib = -Iα/2 + Iβ·√3/2
Ic = -Iα/2 - Iβ·√3/2

FOC 控制框图

        ┌──────────────────────────────────────────┐
        │                                          │
Iq_ref ──→ ┌────┐    ┌────┐    ┌────┐    ┌─────┐ │
           │ PI │───→│ SVPWM │──→│ 逆变器 │──→│ 电机 │ │
           └────┘    └────┘    └─────┘    │     │ │
           ↑ PI      ↑            ↑             │     │
Id_ref ───→┌────┐    │            │             ↓     ↓
           │ PI │────┘            │        ┌────────┐ │
           └────┘                 │        │ 三相输出 │ │
           ↑                      │        └────────┘ │
        0 ───→┌────┐             │             ↑       │
           ┌─→│ −ωLqIq │←───────┘             │       │
           │  └────┘                          │       │
      ┌────┐│                                 │       │
 θe ←│ PLL│←─────────────────────────────────┘       │
      └────┘                                          │
      (磁链观测器)                                      │
                                                     │
     ←←←←←← 转速环 ←←←←←←←←←←←←←←←←←←←←←←←←←←←←←←←←←←

SVPWM 原理与实现

空间矢量定义

// 8个基本空间矢量（6个非零 + 2个零矢量）
// U0(000): 000, U1(100): 100, U2(110): 110, ...
// U3(010): 010, U4(011): 011, U5(001): 001
// U6(101): 101, U7(111): 111

typedef struct {
    float alpha;
    float beta;
} SpaceVector;

// 合成任意方向电压矢量
SpaceVector sv_ref = {
    .alpha = Uref * cos(theta_e),
    .beta  = Uref * sin(theta_e)
};

SVPWM 扇区判断

// 判断参考电压矢量所在扇区
int sv_sector(SpaceVector *sv) {
    float a = sv->beta;
    float b = sv->alpha * 0.866 - sv->beta * 0.5;
    float c = -sv->alpha * 0.866 - sv->beta * 0.5;

    int N = 0;
    if (a > 0) N |= 1;
    if (b > 0) N |= 2;
    if (c > 0) N |= 4;

    // N → 扇区号 (1~6)
    const int sector_table[8] = {0, 2, 6, 1, 4, 3, 5, 0};
    return sector_table[N];
}

SVPWM 时间计算

// 在扇区 k 中，计算相邻矢量作用时间
// T0 = (T - Ta - Tb) / 2
// Ta, Tb 根据扇区查表

typedef struct {
    float Ualpha;
    float Ubeta;
    float T;        // PWM周期
    float Udc;      // 母线电压
} SVPWM_Handle;

void svpwm_calc(SVPWM_Handle *h, float *Ta, float *Tb) {
    float X = h->Ubeta;
    float Y = h->Ualpha * 0.866 + h->Ubeta * 0.5;
    float Z = -h->Ualpha * 0.866 + h->Ubeta * 0.5;

    int sector = sv_sector((SpaceVector*)h);

    switch(sector) {
        case 1: *Ta = Z; *Tb = Y; break;
        case 2: *Ta = Y; *Tb = -X; break;
        case 3: *Ta = -Z; *Tb = X; break;
        case 4: *Ta = -X; *Tb = Z; break;
        case 5: *Ta = X; *Tb = -Y; break;
        case 6: *Ta = -Y; *Tb = -Z; break;
    }

    // 过调制处理
    float T_sum = *Ta + *Tb;
    if (T_sum > h->T) {
        *Ta = *Ta * h->T / T_sum;
        *Tb = *Tb * h->T / T_sum;
    }

    // 零矢量分配（7段式对称PWM）
    float T0 = (h->T - *Ta - *Tb) / 2;
    float T7 = T0;

    // 计算三相占空比
    // DPA, DPB, DPC 存入比较寄存器
}

占空比计算（7段式对称SVPWM）

// 各扇区占空比计算
// Tcm1, Tcm2, Tcm3 → 对应 ABC 三相比较值

void svpwm_duty(SVPWM_Handle *h, float *Tcm) {
    float Ta, Tb;
    svpwm_calc(h, &Ta, &Tb);
    float T0 = (h->T - Ta - Tb) / 2;

    int sector = sv_sector((SpaceVector*)h);

    switch(sector) {
        case 1:  // U4(100), U6(110), U0(000)
            Tcm[0] = (h->T + Ta + Tb) / 2;  // A: 先断后通
            Tcm[1] = (h->T - Ta + Tb) / 2;  // B
            Tcm[2] = (h->T - Ta - Tb) / 2;  // C
            break;
        case 2:  // U6(110), U2(010), U0(000)
            Tcm[0] = (h->T - Ta + Tb) / 2;
            Tcm[1] = (h->T + Ta + Tb) / 2;
            Tcm[2] = (h->T - Ta - Tb) / 2;
            break;
        case 3:  // U2(010), U3(011), U0(000)
            Tcm[0] = (h->T - Ta - Tb) / 2;
            Tcm[1] = (h->T + Ta + Tb) / 2;
            Tcm[2] = (h->T + Ta - Tb) / 2;
            break;
        case 4:  // U3(011), U1(001), U0(000)
            Tcm[0] = (h->T - Ta - Tb) / 2;
            Tcm[1] = (h->T - Ta + Tb) / 2;
            Tcm[2] = (h->T + Ta + Tb) / 2;
            break;
        case 5:  // U1(001), U5(101), U0(000)
            Tcm[0] = (h->T + Ta - Tb) / 2;
            Tcm[1] = (h->T - Ta - Tb) / 2;
            Tcm[2] = (h->T + Ta + Tb) / 2;
            break;
        case 6:  // U5(101), U4(100), U0(000)
            Tcm[0] = (h->T + Ta + Tb) / 2;
            Tcm[1] = (h->T - Ta - Tb) / 2;
            Tcm[2] = (h->T + Ta - Tb) / 2;
            break;
    }
}

MTPA（最大转矩电流比）控制

原理

对于内嵌式 PMSM（IPMSM），存在 d轴磁阻转矩，利用 MTPA 可在相同电流下获得更大转矩。

电磁转矩方程：
T = 1.5 × np × [Ψm × Iq + (Ld - Lq) × Id × Iq]

定义电流幅值：
|I| = √(Id² + Iq²)

MTPA 轨迹：固定 |I|，寻找使 T 最大的 (Id, Iq) 组合

MTPA 数学推导

令 ∂T/∂Id = 0，约束条件 |I| = constant

拉格朗日函数：L = T - λ(√(Id²+Iq²) - I_ref)

∂L/∂Id = 1.5×np×[(Ld-Lq)×Iq] - λ×Id/|I| = 0
∂L/∂Iq = 1.5×np×[Ψm + (Ld-Lq)×Id] - λ×Iq/|I| = 0

联立求解（化简后）：

Id_MTPA = -(Ψm / (2×ΔL)) + √[(Ψm/(2ΔL))² + Iq²]

其中 ΔL = Lq - Ld > 0

当 ΔL 很小时（表贴式），Id_MTPA ≈ 0 → 退化为 Id=0 控制

MTPA 实现

// MTPA 查表法（实时性最好）
// 预计算 MTPA 曲线，运行时查表 + 线性插值

static const float mtpa_table_Iq[] = {
    0.0, 2.0, 4.0, 6.0, 8.0, 10.0, 12.0, 15.0, 18.0, 20.0
};
static const float mtpa_table_Id[] = {
    0.0, -0.5, -1.1, -1.8, -2.5, -3.2, -4.0, -5.3, -6.6, -7.8
};

float mtpa_interpolate(float Iq) {
    int n = sizeof(mtpa_table_Iq) / sizeof(mtpa_table_Iq[0]);
    // 边界
    if (Iq <= mtpa_table_Iq[0]) return mtpa_table_Id[0];
    if (Iq >= mtpa_table_Iq[n-1]) return mtpa_table_Id[n-1];
    // 查表
    for (int i = 0; i < n-1; i++) {
        if (Iq >= mtpa_table_Iq[i] && Iq < mtpa_table_Iq[i+1]) {
            float t = (Iq - mtpa_table_Iq[i]) /
                       (mtpa_table_Iq[i+1] - mtpa_table_Iq[i]);
            return mtpa_table_Id[i] + t * (mtpa_table_Id[i+1] - mtpa_table_Id[i]);
        }
    }
    return 0;
}

// MTPA 解析公式法（适合在线计算）
float mtpa_Id_formula(float Iq, float Psi_m, float Ld, float Lq) {
    float delta_L = Lq - Ld;
    if (delta_L < 1e-6) return 0;  // 表贴式，无 MTPA 效益

    float k = Psi_m / (2.0f * delta_L);
    float Id = -k + sqrtf(k * k + Iq * Iq);
    return -Id;  // Id 必须为负（去磁）
}

弱磁控制

原理

当转速超过基速时，反电动势 E > Vdc，电压饱和。弱磁通过增加 Id（去磁电流）来降低有效磁链，从而在恒功率区扩展转速。

电压极限椭圆（d-q 平面）：
Vd² + Vq² ≤ Vdc² / 2   （SVPWM线性调制区最大输出）

反电动势约束：
V = ω_e × (Ψm - Ld×Id)  ≈ ω_e × Ψm_eff

弱磁的本质：在电压极限椭圆内重新分配 Id、Iq

弱磁深度等级

弱磁控制器实现

typedef enum {
    MTPA_MODE,         // 最大转矩电流比
    FW_MODE,            // 弱磁模式
    SIXSTEP_MODE        // 六步方波
} FluxMode;

typedef struct {
    float Vdc;          // 母线电压
    float V_lim;        // 电压极限（Vdc/sqrt(2)）
    float V_th;         // 弱磁启动阈值（建议 0.9×V_lim）
    float Id_fw_min;    // 最大去磁电流（负值）
    float gamma;         // 弱磁PI积分系数
    float Kp_fw;        // 弱磁比例增益
    float Ki_fw;        // 弱磁积分增益
} FluxWeaken_Handle;

void flux_weaken_update(FluxWeaken_Handle *h,
                        float V_mag, float Id_ref,
                        float *Id_fw_out) {
    // V_mag = sqrt(Vd² + Vq²)
    // Id_ref = MTPA 给定的 Id

    static float integral = 0;
    float error = V_mag - h->V_lim * 0.95;  // 提前 5% 介入

    if (error > 0) {
        // 电压饱和，启动弱磁
        integral += h->gamma * error;
        integral = fminf(integral, 0);  // Id_fw 为负，积分限幅
        *Id_fw_out = h->Id_fw_min;       // 直接给最大去磁
    } else {
        // 电压裕量足够，逐渐减小弱磁
        integral *= 0.95;  // 缓慢衰减
        *Id_fw_out = Id_ref + integral;
    }

    // 与 MTPA 叠加
    *Id_fw_out = fmaxf(*Id_fw_out, h->Id_fw_min);
}

弱磁调度逻辑

float select_Id_ref(float Id_MTPA, float Id_FW, float V_mag,
                    float V_lim, float speed) {
    float Id_ref;

    if (V_mag < V_lim * 0.85) {
        // 电压充裕，MTPA 控制
        Id_ref = Id_MTPA;
    } else if (V_mag < V_lim * 0.95) {
        // 电压接近饱和，过渡区
        float alpha = (V_mag - V_lim * 0.85) / (V_lim * 0.1);
        Id_ref = Id_MTPA + alpha * (Id_FW - Id_MTPA);
    } else {
        // 深度弱磁
        Id_ref = Id_FW;
    }

    return Id_ref;
}

磁链观测器

滑模观测器（SMO）估测转子位置

typedef struct {
    float I_alpha;
    float I_beta;
    float Ia;
    float Ib;
    float Ic;
    float R;
    float L;
    float emf_alpha;
    float emf_beta;
    float angle;
    float speed;
    float prev_angle;
    float k_gain;       // 滑模增益
    float filter_bw;     // 低通截止频率 rad/s
} SMO_Handle;

// sign 函数（连续近似，避免抖振）
float sign_f(float x, float delta) {
    if (x > delta)  return 1.0f;
    if (x < -delta) return -1.0f;
    return x / delta;  // |x|<delta: 线性近似
}

#define K_SMO 0.5f    // 滑模增益（过大=抖振，过小=延迟）

void smo_update(SMO_Handle *smo, float V_alpha, float V_beta,
               float Ia, float Ib, float Ic, float dt) {

    float e_alpha = smo->I_alpha - Ia;
    float e_beta  = smo->I_beta  - Ib;

    // 滑模控制量
    float z_alpha = smo->k_gain * sign_f(e_alpha, 0.05f);
    float z_beta  = smo->k_gain * sign_f(e_beta,  0.05f);

    // 电流更新（欧拉法）
    smo->I_alpha += dt * (-smo->R * Ia + V_alpha - z_alpha) / smo->L;
    smo->I_beta  += dt * (-smo->R * Ib + V_beta  - z_beta ) / smo->L;

    // 反电动势提取（低通滤波）
    float alpha_lpf = 2.0f * M_PI * smo->filter_bw;
    smo->emf_alpha += dt * alpha_lpf * (z_alpha - smo->emf_alpha);
    smo->emf_beta  += dt * alpha_lpf * (z_beta  - smo->emf_beta);

    // 角度提取
    smo->angle = atan2f(smo->emf_alpha, -smo->emf_beta);

    // 速度计算（微分）
    float d_angle = smo->angle - smo->prev_angle;
    // 角度归一化（处理 -π 到 +π 跳变）
    if (d_angle > M_PI)  d_angle -= 2.0f * M_PI;
    if (d_angle < -M_PI) d_angle += 2.0f * M_PI;
    smo->speed = d_angle / dt;
    smo->prev_angle = smo->angle;
}

PLL（锁相环）角度提取

// 对 SMO 估算的反电动势做 PLL
// emf_d = -ω_e × Ψm × sin(Δθ) ≈ -ω_e × Ψm × Δθ（小角度时）
// emf_q =  ω_e × Ψm × cos(Δθ) ≈  ω_e × Ψm

typedef struct {
    float Kp;
    float Ki;
    float integral;
    float angle;
    float speed;
    float lock_threshold;  // 锁定阈值 rad/s
    int   locked;         // 锁定标志
} PLL_Handle;

void pll_update(PLL_Handle *pll, float emf_d, float emf_q, float dt) {
    // d轴反电动势应接近0（正确对齐时）
    // q轴反电动势 = ω_e × Ψm

    float error = emf_d;  // PLL 误差

    // PI 积分
    pll->integral += pll->Ki * error * dt;
    // 积分限幅
    pll->integral = fmaxf(fminf(pll->integral, 1000.0f), -1000.0f);

    pll->speed = pll->Kp * error + pll->integral;
    pll->angle += pll->speed * dt;

    // 角度归一化 0~2π
    while (pll->angle < 0)      pll->angle += 2.0f * M_PI;
    while (pll->angle > 2.0f*M_PI) pll->angle -= 2.0f * M_PI;

    // 锁定判断
    if (fabsf(error) < pll->lock_threshold && fabsf(pll->speed) > 10.0f) {
        pll->locked = 1;
    }
}

PI 参数整定指南

自动整定脚本

# 自动计算 PI 参数
python scripts/foc_pi_tuner.py --R 1.23 --L 5.6e-3 --J 1e-3 --Kt 0.15

# 交互模式（引导输入）
python scripts/foc_pi_tuner.py --mode interactive

# 指定目标带宽
python scripts/foc_pi_tuner.py --R 2.1 --L 8e-3 --target_bw 150

# 指定采样周期 + 生成Bode图
python scripts/foc_pi_tuner.py --R 1.23 --L 5.6e-3 --Ts_us 62.5 --plot

电流环整定

目标：带宽 ωbw = R/L × 0.5（典型因子 0.3~0.6）

Kp = L × ωbw
Ki = R / L

示例：
R = 1.23Ω, L = 5.6mH, ωbw = 1.23/0.0056 × 0.5 ≈ 110 rad/s
Kp = 0.0056 × 110 ≈ 0.616
Ki = 1.23/0.0056 ≈ 220 (需根据 Ts 离散化修正)

速度环整定

目标：带宽 ≈ 电流环带宽 / 10

Kp_speed ≈ J × ωbw_speed
Ki_speed ≈ Kp_speed / Tm

示例：
J = 0.001 kg·m²
ωbw_speed = 11 rad/s (电流环1/10)
Kp_speed = 0.001 × 11 = 0.011
Tm = J × R / Kt² = 0.001 × 1.23 / 0.15² ≈ 0.055 s
Ki_speed = 0.011 / 0.055 ≈ 0.2

离散化修正（Tustin 方法）

给定采样周期 Ts：
Kp_d = Kp
Ki_d = Ki × Ts / 2

示例：Ts = 62.5μs (16kHz)
Ki_d = 220 × 31.25e-6 = 0.006875

实现（位置式）：
  integral += Ki_d × error
  output   = Kp_d × error + integral
  限幅：output ∈ [-Vmax, Vmax]

工程参数速查

详见 references/foc-quick-ref.md

常见问题排查

调试流程图

启动抖动？
├─ 初始角度问题 → 重新做初始角辨识（脉冲法/预定位）
├─ Hall错位 → 校正Hall安装位置
└─ 电流环带宽不足 → 提高电流环 Kp

高速失步？
├─ 弱磁不足 → 增强 Id_fw，验证 V_mag > 0.95×V_lim
├─ 母线电压不足 → 提高 Vdc 或加升压
└─ 反电动势估算错误 → 检查速度观测器

振动噪音？
├─ 开关频率共振 → 提高PWM频率或加RC吸收
├─ 机械共振 → 加陷波滤波器
└─ 电流纹波大 → 提高开关频率或加电感

参考资源

• scripts/foc_pi_tuner.py - PI参数自动整定脚本（推荐优先使用）
• references/foc-quick-ref.md - FOC工程参数速查卡（典型值、阈值速查）
• references/pm-materials.md - 永磁体牌号参数表（可选扩展）

快速使用 PI 整定

# 完整参数计算（自动输出离散PI参数）
python scripts/foc_pi_tuner.py --R 1.23 --L 5.6e-3 --poles 8 --J 1e-3 --Kt 0.15 --Ts_us 62.5

# 绘制 Bode 图（验证稳定性）
python scripts/foc_pi_tuner.py --R 1.23 --L 5.6e-3 --plot

# 对比不同带宽参数
python scripts/foc_pi_tuner.py --R 2.1 --L 8e-3 --target_bw 80
python scripts/foc_pi_tuner.py --R 2.1 --L 8e-3 --target_bw 200

注意：脚本输出为理论初始值，实际需在电机上微调。调试顺序：电流环→速度环→位置环，逐级验证带宽和稳定性。