直流无刷电机 FOC 控制算法详解与 STM32 实现

代码实现

以下是基于 C 语言的 FOC 核心算法实现，包含反 Park 变换、扇区判断及 SVPWM 计算逻辑。

#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);//计算当前 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);//CCR 时间计算（计算占空比值）
    __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);
}
/*FOC-Park 和 Clark 变换*/
/* APhaseCurrent：电机 A 相电流 BPhaseCurrent：电机 B 相电流 Electronic_Angle：电角度 */
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;
}

STM32CubeMX 配置要点

硬件配置是算法运行的基础，以下关键步骤需在 CubeMX 中完成：

1. 时钟配置：确保系统时钟稳定，为定时器和 ADC 提供足够频率。
2. 调试接口：建议关闭 SWD 引脚复用，以便腾出 GPIO 用于电机驱动。
3. 高级定时器：配置为 PWM 模式，用于生成三路互补 PWM 信号，注意死区设置以防炸管。
4. ADC 配置：开启多通道扫描模式，用于实时采样三相电流。

主程序逻辑

在定时器中断中执行 FOC 闭环控制。典型的流程包括读取编码器位置、计算电角度、执行速度环 PID、电流环 PID，最后调用 SVPWM 更新比较寄存器。

/*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);//将电角度限制在 0-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 变换*/
            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);//SVPWM 算法
        }
    }
}