直流无刷电机 FOC 控制算法详解

等幅值 Clarke 变换的最终结果为：

3.2.2 Clarke 逆变换公式推导

为了恢复三相电流，我们需要进行 Clarke 逆变换。已知 Iα, Iβ，结合基尔霍夫定律即可推导出 Ia, Ib, Ic 的表达式。

3.3 FOC 算法 Park 变换

Park 变换将静止坐标系下的 Iα, Iβ 转换为随转子旋转的 Id, Iq 坐标系。在这个旋转坐标系下，原本时变的交流量变成了直流量，便于控制。

3.3.1 Park 变换公式推导

角度 θ 是 d 轴与 α 轴的夹角。通过三角投影关系，可以得出 Id 和 Iq 的计算公式。其中 d 轴与转子磁链重合，q 轴垂直于磁链。

经过这一步变换，匀速旋转向量在该坐标系下变为定值，实现了控制变量的线性化。

3.3.2 Park 逆变换公式推导

逆变换则是将计算出的电压指令从旋转坐标系还原回静止坐标系，以便生成 PWM。

3.4 FOC 算法 SVPWM 原理及实现

SVPWM（Space Vector Pulse Width Modulation）空间矢量脉宽调制，旨在模拟三相正弦波电流形成的电压矢量圆，产生接近圆形的磁链轨迹。

算法核心步骤包括：

1. 基本矢量电压分解。
2. 扇区判断（将 360 度分为六个扇区）。
3. 计算相邻两个基本矢量电压的作用时间。
4. 计算三路 PWM 占空比。

3.4.1 SVPWM 原理及实现

SVPWM 通过逆变器 6 个功率管的开关组合，合成所需的电压矢量。

3.4.2 七段式 SVPWM 和五段式 SVPWM 的区别

七段式使用七个向量，效率更高但计算复杂；五段式仅用五个向量，计算简单但谐波略大。本设计采用七段式 SVPWM。

3.4.3 至 3.4.6 矢量与占空比计算

根据扇区不同，基本矢量的作用时间分配也不同。例如在第一扇区，通过计算 Ta 和 Tb，结合 PWM 周期 Tpwm，可以得出三路 PWM 的 CCR 值。

计算公式示例（以扇区一为例）： value_a = (tpwm - Ta - Tb)/4; value_b = value_a + Ta/2; value_c = value_b + Tb/2;

3.5 FOC 矢量控制 C 语言代码

下面是核心的算法实现代码，包含反 Park 变换、扇区判断、矢量作用时间计算及 CCR 配置。

3.5.1 foc.c 文件代码

#include"foc.h"
// 反 Park 变换，将静止两相坐标系 D/Q 转换为旋转两相坐标系 Valpha/Vbeta
void RevParkOperate(float vd,float vq,float theta,float*valpha,float*vbeta){
    *valpha = vd *cos(theta)- vq *sin(theta);
    *vbeta = vd *sin(theta)+ vq *cos(theta);
}

// 扇区判断函数，通过 valpha 和 vbeta 判断扇区
// 返回值：315462 对应扇区 123456
unsigned char SectorJude(float valpha,float vbeta){
    float A = vbeta;
    float B = valpha * sqrt3/2.0f- vbeta/2.0f;
    float C =-valpha * sqrt3/2.0f- vbeta/2.0f;
    unsigned char N =0;
    if(A >0){ N = N+1;}
    if(B >0){ N = N+2;}
    if(C >0){ N = N+4;}
    return N;
}

// 矢量作用时间计算
void VectorActionTime(uint8_t n,float valpha,float vbeta,uint32_t udc,uint32_t tpwm,float*ta,float*tb){
    float X =(sqrt3 * tpwm)/ udc * vbeta;
    float Y =(sqrt3 * tpwm)/ udc *((sqrt3/2.0f)*valpha - vbeta/2.0f);
    float Z =(sqrt3 * tpwm)/ udc *(-(sqrt3/2.0f)*valpha - vbeta/2.0f);
    if(n ==3)//第一扇区
        {*ta = Y;*tb = X;}
    if(n ==1)//第二扇区
        {*ta =-Y;*tb =-Z;}
    if(n ==5)//第三扇区
        {*ta = X;*tb = Z;}
    if(n ==4)//第四扇区
        {*ta =-X;*tb =-Y;}
    if(n ==6)//第五扇区
        {*ta = Z;*tb = Y;}
    if(n ==2)//第六扇区
        {*ta =-Z;*tb =-X;}
}

// CCR 时间计算
void CCRCalculate(uint8_t n,float ta,float tb,uint32_t tpwm,uint32_t*ccr1,uint32_t*ccr2,uint32_t*ccr3){
    /*限制 ta 和 tb，ta+tb 不能超过 tpwm*/
    float temp = ta + tb;
    if(temp > tpwm){ ta = ta / temp*tpwm; tb = tb / temp*tpwm;}
    float value1 =(tpwm - ta - tb)/4.0f;
    float value2 = value1 + ta/2.0f;
    float value3 = value2 + tb/2.0f;
    switch(n){
        case3:*ccr1 = value1;*ccr2 = value2;*ccr3 = value3;break;
        case1:*ccr1 = value2;*ccr2 = value1;*ccr3 = value3;break;
        case5:*ccr1 = value3;*ccr2 = value1;*ccr3 = value2;break;
        case4:*ccr1 = value3;*ccr2 = value2;*ccr3 = value1;break;
        case6:*ccr1 = value2;*ccr2 = value3;*ccr3 = value1;break;
        case2:*ccr1 = value1;*ccr2 = value3;*ccr3 = value2;break;
    }
}

// 通过 Vd,Vq,THETA,Udc,Tpwm 计算每一步的 CCR 值并写入定时器
void SvpwmAlgorithm(float vd,float vq,float theta,uint32_t udc,uint32_t tpwm){
    float valpha =0;
    float vbeta =0;
    RevParkOperate(vd,vq,theta,&valpha,&vbeta);
    unsigned char n = SectorJude(valpha,vbeta);
    float ta =0;
    float tb =0;
    VectorActionTime(n,valpha,vbeta,udc,tpwm,&ta,&tb);
    uint32_t ccr1 =0;
    uint32_t ccr2 =0;
    uint32_t ccr3 =0;
    CCRCalculate(n,ta,tb,tpwm,&ccr1,&ccr2,&ccr3);
    __HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_1,ccr1);
    __HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_2,ccr2);
    __HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_3,ccr3);
}

3.5.2 foc.h 文件代码

#ifndef__FOC_H__
#define__FOC_H__
#include"tim.h"
#include<math.h>
#include<stdio.h>
#define sqrt3 1.7320508f
void SvpwmAlgorithm(float vd,float vq,float theta,uint32_t udc,uint32_t tpwm);
void Clark_Park(float Electronic_Angle);
float motor_ramp(float t_value,float acc_time);
#endif

4. STM32CUBEMX 配置

4.1 时钟原配置

4.2 调试接口配置

4.3 时钟配置

4.4 高级定时器配置

用于 FOC 算法产生 PWM，需配置为互补输出模式。

4.5 ADC 配置

用于电机三相电流采样。

5. 代码讲解

5.1 foc.c 文件逻辑

这里再次展示核心算法文件的完整实现，重点在于 Clark_Park 变换和中断回调中的闭环控制逻辑。

#include"foc.h"
#include"spi.h"
#include"adc.h"
FOC_CURRENT foc_current;

void RevParkOperate(float vd,float vq,float theta,float*valpha,float*vbeta){
    *valpha = vd *cos(theta)- vq *sin(theta);
    *vbeta = vd *sin(theta)+ vq *cos(theta);
}

unsigned char SectorJude(float valpha,float vbeta){
    float A = vbeta;
    float B = valpha * sqrt3/2.0f- vbeta/2.0f;
    float C =-valpha * sqrt3/2.0f- vbeta/2.0f;
    unsigned char N =0;
    if(A >0){ N = N+1;}
    if(B >0){ N = N+2;}
    if(C >0){ N = N+4;}
    return N;
}

void VectorActionTime(uint8_t n,float valpha,float vbeta,uint32_t udc,uint32_t tpwm,float*ta,float*tb){
    float X =(sqrt3 * tpwm)/ udc * vbeta;
    float Y =(sqrt3 * tpwm)/ udc *((sqrt3/2.0f)*valpha - vbeta/2.0f);
    float Z =(sqrt3 * tpwm)/ udc *(-(sqrt3/2.0f)*valpha - vbeta/2.0f);
    if(n ==3){*ta = Y;*tb = X;}
    if(n ==1){*ta =-Y;*tb =-Z;}
    if(n ==5){*ta = X;*tb = Z;}
    if(n ==4){*ta =-X;*tb =-Y;}
    if(n ==6){*ta = Z;*tb = Y;}
    if(n ==2){*ta =-Z;*tb =-X;}
}

void CCRCalculate(uint8_t n,float ta,float tb,uint32_t tpwm,uint32_t*ccr1,uint32_t*ccr2,uint32_t*ccr3){
    float temp = ta + tb;
    if(temp > tpwm){ ta = ta / temp*tpwm; tb = tb / temp*tpwm;}
    float value1 =(tpwm - ta - tb)/4.0f;
    float value2 = value1 + ta/2.0f;
    float value3 = value2 + tb/2.0f;
    switch(n){
        case3:*ccr1 = value1;*ccr2 = value2;*ccr3 = value3;break;
        case1:*ccr1 = value2;*ccr2 = value1;*ccr3 = value3;break;
        case5:*ccr1 = value3;*ccr2 = value1;*ccr3 = value2;break;
        case4:*ccr1 = value3;*ccr2 = value2;*ccr3 = value1;break;
        case6:*ccr1 = value2;*ccr2 = value3;*ccr3 = value1;break;
        case2:*ccr1 = value1;*ccr2 = value3;*ccr3 = value2;break;
    }
}

void SvpwmAlgorithm(float vd,float vq,float theta,uint32_t udc,uint32_t tpwm){
    float valpha =0;
    float vbeta =0;
    RevParkOperate(vd,vq,theta,&valpha,&vbeta);
    unsigned char n = SectorJude(valpha,vbeta);
    float ta =0;
    float tb =0;
    VectorActionTime(n,valpha,vbeta,udc,tpwm,&ta,&tb);
    uint32_t ccr1 =0;
    uint32_t ccr2 =0;
    uint32_t ccr3 =0;
    CCRCalculate(n,ta,tb,tpwm,&ccr1,&ccr2,&ccr3);
    __HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_1,ccr1);
    __HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_2,ccr2);
    __HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_3,ccr3);
}

void Clark_Park(int APhaseCurrent,int BPhaseCurrent,float Electronic_Angle,FOC_CURRENT *Current){
    // Clark 变换
    int I_alfa = APhaseCurrent;
    int I_beta =1/sqrt3 *(2*BPhaseCurrent + APhaseCurrent);
    // Park 变换
    Current->Id = I_alfa*cos(Electronic_Angle)+ I_beta*sin(Electronic_Angle);
    Current->Iq =-(I_alfa *sin(Electronic_Angle))+ I_beta*cos(Electronic_Angle);
    Current->Id = Current->Id/1600.0;
    Current->Iq = Current->Iq/1600.0;
}

5.2 foc.h 头文件定义

#ifndef__FOC_H__
#define__FOC_H__
#include"tim.h"
#include<math.h>
#include<stdio.h>
#define sqrt3 1.7320508f
typedef struct{
    float Id;
    float Iq;
}FOC_CURRENT;
extern FOC_CURRENT foc_current;
void SvpwmAlgorithm(float vd,float vq,float theta,uint32_t udc,uint32_t tpwm);
void Clark_Park(int APhaseCurrent,int BPhaseCurrent,float Electronic_Angle,FOC_CURRENT *Current);
#endif

5.3 主要运行代码

在中断回调中执行完整的控制环路，包括位置获取、PID 运算及 FOC 变换。

/*TIM3 回调函数*/
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim){
    if(htim->Instance==TIM3){
        if(motor_state ==1){
            Get_Pos();
            Encoder_TypeDef.angle =Get_Elec_Angle();
            if(Encoder_TypeDef.angle>=Encoder_TypeDef.angle_rad_offset){
                Encoder_TypeDef.angle = Encoder_TypeDef.angle - Encoder_TypeDef.angle_rad_offset;
            }else{
                Encoder_TypeDef.angle =6.2831852f- Encoder_TypeDef.angle_rad_offset + Encoder_TypeDef.angle;
            }
            Encoder_TypeDef.electronic_angle = Encoder_TypeDef.angle*7;
            Encoder_TypeDef.electronic_angle =fmod(Encoder_TypeDef.electronic_angle,6.2831852f);
            PID_Position_Out =PID_Function(&pid_position_TypeDef,target_location,encoder.rad_comulative);
            vel_LPF =LPF_velocity(MT6701_GetVelocity());
            PID_Speed_Out =PID_Function(&pid_speed_TypeDef,PID_Position_Out,vel_LPF);
            Clark_Park(PhaseACurrent,PhaseBCurrent,Encoder_TypeDef.electronic_angle,&foc_current);
            PID_Out_Value =PID_Function(&pid_current_Typedef,PID_Speed_Out,foc_current.Iq);
            SvpwmAlgorithm(0,PID_Out_Value,Encoder_TypeDef.electronic_angle,12,3600);
        }
    }
}