基于C语言的FOC算法核心模块实现，包含坐标变换、PI控制、SVPWM生成等关键部分
一、基础数学运算模块
// 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;
}
七、调试工具建议
- 电流波形监控：使用示波器观察ia/ib/ic三相电流，应呈现正弦波形且谐波畸变率<5%
- Park矢量轨迹：在Park坐标系下观察id/iq轨迹，理想状态为圆形
- 扇区切换测试：记录SVPWM生成的占空比变化，验证扇区判断逻辑
- 转子位置跟踪：对比编码器反馈与观测器估计角度，误差应<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/contentali/97763.html
九、性能优化
- 查表法加速：预先计算sin/cos值存入数组
- Q格式定点数：使用Q15格式替代浮点运算
- DMA传输：配置DMA实现ADC-PWM数据直通
- 中断优先级：设置TIM中断为最高优先级（NVIC优先级0）
