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

直流无刷电机 FOC 控制算法详解 | 极客日志

#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)
    {
        case 3: *ccr1 = value1; *ccr2 = value2; *ccr3 = value3; break;
        case 1: *ccr1 = value2; *ccr2 = value1; *ccr3 = value3; break;
        case 5: *ccr1 = value3; *ccr2 = value1; *ccr3 = value2; break;
        case 4: *ccr1 = value3; *ccr2 = value2; *ccr3 = value1; break;
        case 6: *ccr1 = value2; *ccr2 = value3; *ccr3 = value1; break;
        case 2: *ccr1 = value1; *ccr2 = value3; *ccr3 = value2; break;
    }
}

//通过 Vd,Vq,THETA,Udc,Tpwm 计算每一步的 CCR 值并写入定时器
/* Vd:给电机 Vd 的电压 Vq:给电机 Vq 的电压 theta:电角度 theta:母线电压 tpwm:PWM 周期 */
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);
}

#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

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

//反 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)
    {
        case 3: *ccr1 = value1; *ccr2 = value2; *ccr3 = value3; break;
        case 1: *ccr1 = value2; *ccr2 = value1; *ccr3 = value3; break;
        case 5: *ccr1 = value3; *ccr2 = value1; *ccr3 = value2; break;
        case 4: *ccr1 = value3; *ccr2 = value2; *ccr3 = value1; break;
        case 6: *ccr1 = value2; *ccr2 = value3; *ccr3 = value1; break;
        case 2: *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：电角度 *
Current：Park、Clark 变换之后的电流值 */
void Clark_Park(int APhaseCurrent, int BPhaseCurrent, float Electronic_Angle, FOC_CURRENT *Current)
{
    //Clark 变换 I_alfa = APhaseCurrent; I_beta = 1/sqrt3 *(2*BPhaseCurrent + APhaseCurrent);
    //Park 变换
    foc_current.Id = I_alfa * cos(Electronic_Angle) + I_beta * sin(Electronic_Angle);
    foc_current.Iq = -(I_alfa * sin(Electronic_Angle)) + I_beta * cos(Electronic_Angle);
    foc_current.Id = foc_current.Id / 1600.0;
    foc_current.Iq = foc_current.Iq / 1600.0;
}

#ifndef __FOC_H__
#define __FOC_H__
#include "tim.h"
#include <math.h>
#include <stdio.h>
#define sqrt3 1.7320508f

/*FOC 变换之后的电流值*/
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

/*TIM3 回调函数*/
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
/*进入中断时间为 1ms 秒*/
{
    if (htim->Instance == TIM3)
    {
        if (motor_state == 1)
        {
            Get_Pos(); //编码器多圈数据累加
            Encoder_TypeDef.angle = Get_Elec_Angle(); //获取机械角度
            /*角度补偿 Encoder_TypeDef.angle：电机的机械角度 Encoder_TypeDef.angle_rad_offset：电机零位的编码器的值 */
            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); //PID 输出
            /*速度计算*/
            vel_LPF = LPF_velocity(MT6701_GetVelocity()); //速度计算以及低通滤波处理
            /*速度环*/
            PID_Speed_Out = PID_Function(&pid_speed_TypeDef, PID_Position_Out, vel_LPF); //PID 输出
            /*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); //PID 输出
            SvpwmAlgorithm(0, PID_Out_Value, Encoder_TypeDef.electronic_angle, 12, 3600); //SVPWM 算法
        }
    }
}

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

1. FOC 概述

1.1 FOC 控制算法介绍

2. 无刷电机

2.1 无刷电机介绍

2.2 无刷电机和永磁同步电机的区别

2.3 无刷电机的控制原理

2.3.1 无刷电机工作原理

2.3.2 直流无刷电机驱动原理

2.3.2.1 有感直流无刷电机六步换相驱动原理

2.3.2.2 直流无刷电机 FOC 控制原理

3. 无刷电机 FOC 控制算法

3.1 FOC 控制算法整体流程

3.2 FOC 算法 Clarke 变换

3.2.1 Clarke 变换公式推导

3.2.2 Clarke 逆变换公式推导

3.3 FOC 算法 Park 变换

3.3.1 Park 变换公式推导

3.3.2 Park 逆变换公式推导

3.4 FOC 算法 SVPWM（空间矢量脉冲调制）原理及实现

3.4.1 SVPWM 原理及实现

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

3.4.3 基本矢量电压

3.4.4 扇区判断

3.4.5 相邻两个基本矢量电压的作用时间

3.4.6 根据时间计算三路 PWM 占空比

3.5 FOC 矢量控制 C 语言代码

3.5.1 FOC 矢量控制 foc.c 文件代码

3.5.2 FOC 矢量控制 foc.h 文件代码

4. STM32CubeMX 配置

4.1 时钟原配置

4.2 调试接口配置

4.3 时钟配置

4.4 高级定时器配置 (用于 FOC 算法，产生 PWM)

4.5 ADC 配置 (用于电机三相电流采样)

5. 代码讲解

5.1 foc 算法控制算法代码.c 文件

5.2 foc 算法控制算法代码.h 文件

5.3 主要运行代码

微信扫一扫，关注极客日志

更多推荐文章

相关免费在线工具