基于C语言的FOC算法核心模块实现

一、基础数学运算模块

// Clark变换（3相→2相静止坐标系）
void Clarke_Transform(float ia, float ib, float ic, float *i_alpha, float *i_beta) {
    *i_alpha = ia;  // 假设ia为中性点参考
    *i_beta = (2.0f/3.0f) * (ib - ic) * 0.8660254f;  // sqrt(3)/2 ≈ 0.866
}

// Park变换（静止→旋转坐标系）
void Park_Transform(float i_alpha, float i_beta, float theta, float *i_d, float *i_q) {
    *i_d = i_alpha * cosf(theta) + i_beta * sinf(theta);
    *i_q = -i_alpha * sinf(theta) + i_beta * cosf(theta);
}

// 逆Park变换（旋转→静止坐标系）
void InvPark_Transform(float v_d, float v_q, float theta, float *v_alpha, float *v_beta) {
    *v_alpha = v_d * cosf(theta) - v_q * sinf(theta);
    *v_beta = v_d * sinf(theta) + v_q * cosf(theta);
}

// 逆Clark变换（2相→3相静止坐标系）
void InvClarke_Transform(float v_alpha, float v_beta, float *va, float *vb, float *vc) {
    *va = v_alpha;
    *vb = -0.5f * v_alpha + 0.8660254f * v_beta;
    *vc = -0.5f * v_alpha - 0.8660254f * v_beta;
}

---

二、PI控制器模块

typedef struct {
    float Kp;
    float Ki;
    float integral;
    float prev_error;
    float output_min;
    float output_max;
} PI_Controller;

// PI控制器初始化
void PI_Init(PI_Controller *pi, float Kp, float Ki, float out_min, float out_max) {
    pi->Kp = Kp;
    pi->Ki = Ki;
    pi->integral = 0.0f;
    pi->prev_error = 0.0f;
    pi->output_min = out_min;
    pi->output_max = out_max;
}

// PI计算
float PI_Compute(PI_Controller *pi, float setpoint, float feedback, float dt) {
    float error = setpoint - feedback;
    pi->integral += error * dt;
    float derivative = (error - pi->prev_error) / dt;
    pi->prev_error = error;
    return fminf(fmaxf(pi->Kp * error + pi->Ki * pi->integral, pi->output_min), pi->output_max);
}

---

三、SVPWM生成模块

// 扇区判断（输入α-β坐标）
uint8_t Get_Sector(float alpha, float beta) {
    float u1 = beta;
    float u2 = (sqrtf(3.0f)*alpha - beta)/2.0f;
    float u3 = (-sqrtf(3.0f)*alpha - beta)/2.0f;
    
    uint8_t sector = 0;
    if(u1 > 0) sector |= 0x01;
    if(u2 > 0) sector |= 0x02;
    if(u3 > 0) sector |= 0x04;
    
    static const uint8_t sector_map[8] = {6,1,3,2,5,4,6,0}; // 扇区映射表
    return sector_map[sector];
}

// SVPWM生成（归一化电压范围[-1,1]）
void SVPWM_Generate(float va, float vb, float vc, float Vdc, float *dutyA, float *dutyB, float *dutyC) {
    float T1, T2, T0;
    float alpha = va;
    float beta = vb;
    
    // 计算基本矢量作用时间
    T1 = (sqrtf(3.0f)*beta - alpha) / Vdc;
    T2 = (sqrtf(3.0f)*alpha + beta) / Vdc;
    T0 = 1.0f - T1 - T2;
    
    // 根据扇区分配占空比（示例扇区1）
    uint8_t sector = Get_Sector(alpha, beta);
    switch(sector) {
        case 1: // 0°~60°
            *dutyA = (T0/2 + T1 + T2/2);
            *dutyB = (T0/2 + T2/2);
            *dutyC = (T0/2);
            break;
        // 其他扇区实现类似...
    }
    
    // 限幅处理
    *dutyA = fminf(fmaxf(*dutyA, 0.0f), 1.0f);
    *dutyB = fminf(fmaxf(*dutyB, 0.0f), 1.0f);
    *dutyC = fminf(fmaxf(*dutyC, 0.0f), 1.0f);
}

---

四、完整FOC控制流程

#define PWM_FREQ 20000    // 20kHz
#define DT 0.0000001      // 100ns采样周期

// 全局变量
PI_Controller id_pi, iq_pi;
float theta = 0.0f;       // 转子电角度
float id_ref = 0.0f;      // d轴电流参考
float iq_ref = 10.0f;     // q轴电流参考

// 初始化
void FOC_Init() {
    PI_Init(&id_pi, 1.0f, 100.0f, -1.0f, 1.0f);
    PI_Init(&iq_pi, 1.0f, 100.0f, -1.0f, 1.0f);
}

// 主控制循环
void FOC_Control(float ia, float ib, float ic) {
    // 1. 电流采样（假设已通过ADC获取）
    float i_alpha, i_beta;
    Clarke_Transform(ia, ib, ic, &i_alpha, &i_beta);
    
    // 2. Park变换
    float id, iq;
    Park_Transform(i_alpha, i_beta, theta, &id, &iq);
    
    // 3. 电流环控制
    float vd = PI_Compute(&id_pi, id_ref, id, DT);
    float vq = PI_Compute(&iq_pi, iq_ref, iq, DT);
    
    // 4. 逆Park变换
    float valpha, vbeta;
    InvPark_Transform(vd, vq, theta, &valpha, &vbeta);
    
    // 5. SVPWM生成
    float dutyA, dutyB, dutyC;
    SVPWM_Generate(valpha, vbeta, 12.0f, &dutyA, &dutyB, &dutyC);
    
    // 6. PWM输出（假设使用STM32 HAL库）
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, dutyA*1000);
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, dutyB*1000);
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, dutyC*1000);
}

// 位置更新（示例使用编码器）
void Update_Position() {
    theta += Get_Encoder_Speed() * DT;  // 电角度累加
    theta = fmodf(theta, 2.0f*M_PI);    // 角度归一化
}

---

五、关键调试参数建议

参数类型	推荐范围	调试方法
电流环KP/KI	5-20 / 50-200	观察电流波形超调量
速度环KP/KI	0.1-1 / 10-50	阶跃响应测试
SVPWM死区时间	1-5μs	示波器观察PWM重叠
电流采样频率	≥10kHz	确保满足Nyquist采样定理

---

六、扩展功能实现

1. 无感FOC观测器（扩展卡尔曼滤波）

typedef struct {
    float angle;      // 估计电角度
    float speed;      // 估计转速
    float K[2][2];    // 观测器增益
} EKF_Observer;

void EKF_Init(EKF_Observer *ekf, float angle0, float speed0) {
    ekf->angle = angle0;
    ekf->speed = speed0;
    // 初始化增益矩阵（需根据电机参数计算）
    ekf->K[0][0] = 0.1f; ekf->K[0][1] = 0.01f;
    ekf->K[1][0] = 0.01f; ekf->K[1][1] = 0.1f;
}

void EKF_Update(EKF_Observer *ekf, float ia, float ib, float iq_ref) {
    // 状态预测
    float id_pred = 0.0f;  // 假设d轴电流为0
    float iq_pred = ekf->speed * Lq / (R + Lq*ekf->speed);
    
    // 观测更新
    float y_measured = iq_ref;
    float y_pred = iq_pred;
    float K_gain = ekf->K[0][0] * (y_measured - y_pred);
    
    ekf->angle += (ekf->speed + K_gain) * DT;
    ekf->speed += K_gain * DT;
}

2. 弱磁控制实现

float WeakMagnet_Control(float iq_ref, float speed) {
    float id_ref = 0.0f;
    if(speed > 0.8f*max_speed) {
        id_ref = -0.5f * (1.0f - speed/max_speed);  // 弱磁补偿
    }
    return id_ref;
}

---

七、调试工具建议

1. 电流波形监控：使用示波器观察ia/ib/ic三相电流，应呈现正弦波形且谐波畸变率<5%
2. Park矢量轨迹：在Park坐标系下观察id/iq轨迹，理想状态为圆形
3. 扇区切换测试：记录SVPWM生成的占空比变化，验证扇区判断逻辑
4. 转子位置跟踪：对比编码器反馈与观测器估计角度，误差应<0.1°

---

八、硬件配置参考（STM32）

// 定时器配置（TIM1用于PWM输出）
TIM_HandleTypeDef htim1;
void TIM1_Init() {
    htim1.Instance = TIM1;
    htim1.Init.Prescaler = 84-1;    // 1MHz时钟
    htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
    htim1.Init.Period = 1000-1;     // 1kHz PWM频率
    htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
    HAL_TIM_PWM_Init(&htim1);
    
    // 通道配置
    TIM_OC_InitTypeDef sConfigOC = {0};
    sConfigOC.OCMode = TIM_OCMODE_PWM1;
    sConfigOC.Pulse = 0;
    sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
    HAL_TIM_PWM_ConfigChannel(&htim1, TIM_CHANNEL_1, &sConfigOC);
    // 同理配置CHANNEL2/3
}

// ADC配置（用于电流采样）
ADC_HandleTypeDef hadc1;
void ADC1_Init() {
    hadc1.Instance = ADC1;
    hadc1.Init.Resolution = ADC_RESOLUTION_12B;
    hadc1.Init.ScanConvMode = DISABLE;
    hadc1.Init.ContinuousConvMode = ENABLE;
    HAL_ADC_Init(&hadc1);
}

参考代码 关于FOC算法的纯代码实现 www.youwenfan.com/contentcsp/97763.html

九、性能优化

1. 查表法加速：预先计算sin/cos值存入数组
2. Q格式定点数：使用Q15格式替代浮点运算
3. DMA传输：配置DMA实现ADC-PWM数据直通
4. 中断优先级：设置TIM中断为最高优先级（NVIC优先级0）