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

无刷电机有 3 根电机线按顺序依次叫 U 相线 (一般为黄色)，V 相线 (一般为绿色) 和 W 相线 (一般为蓝色)，3 组漆包线绕组的一端连接在一起，另外一端引出即为 UVW 相线，所以任意两根相线通电都可以导通这两个线圈。

2.3.2 直流无刷电机驱动原理

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

直流无刷电机驱动电路由三个半桥控制电路组成，通过控制三个半桥的上下桥导通和关断，实现逆变控制，将直流电变成交流电，简单点理解就是可以实现流过电机三相线圈的电流的流向控制，是通过使用三相逆变电路来实现驱动电机，如下图所示； 所谓的三相逆变电路就是由三个半桥构成的电路，如上图中的 A+与 A-为一个半桥，B+与 B-以及 C+与 C-各自又为一个半桥，共三个半桥；这三个半桥各自控制对应的 A、B、C 三相绕组；当控制 A 的上桥臂 A+导通时，此时 A 相绕组接到电源正，当控制 B 的下桥臂 B-导通时，此时 B 相绕组接到电源负，所以此时电流由 A 流向 B。

由于有感直流无刷电机的运行方式为绕组两两导通，所以三相线圈的导通组合只有 6 种通电情况，根据合理的顺序依次切换通电顺序即可让转子跟着磁场转起来。如下图是六步换相导通的情况：

想要控制无刷电机绕组的极性，只需要控制绕组对应半桥的'上桥臂导通'或者'下桥臂导通'就可以实现控制该相连接至'正极'或者'负极'了，但是要注意不可以同侧半桥上下桥臂同时导通，否则会导致短路，烧毁电机！要实现 6 步换相控制无刷电机转起来，就可以通过三相逆变电路来实现，如下图；

无刷电机依靠传感器提供转子位置信息进行驱动的方式我们称之为有感驱动方式，直流无刷电机六步换相控制需要通过霍尔传感器判断电机转子的位置，无刷电机的传感器一般为霍尔传感器，根据霍尔器件可检测磁场的变化的特性再搭配一定的电路将磁场方向变化信号转化成不同的高低电平信号输出； 霍尔传感器安装的以 120°电角度所安装，如下图所示；

下图为电机旋转时三个霍尔传感器输出的波形，所对应的扇区组合。通过三个霍尔传感器输出的波形就可以判断当前转子的具体位置，同样满足 6 步一周期。

六步换向需要依赖霍尔传感器反馈的转子位置，其相对应的就是三相逆变电路的上下桥臂导通情况，如下表所示。

下图为直流无刷电机六步换相控制动态图；

下图为有感驱动无刷电机整体框图；

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

我们从原理上简单的讲解了下 FOC 和六步换向的区别，而在实际控制中，FOC 与六步换相还存在以下差别： （1）控制信号区别：FOC 采用正弦波驱动，六步换相采用方波驱动； （2）控制方式区别：FOC 控制中三个半桥的 MOS 采用三三导通，而六步换相采用两两导通。

3. 无刷电机 FOC 控制算法

3.1 FOC 控制算法整体流程

下面是 FOC 控制算法的流程图：

FOC 控制算法的实现流程，主要分为以下几大步： （1）采集电机三相相电流 Ia Ib Ic （2）将三相相电流通过 Clark 变换得到 Iα Iβ （3）将 Iα Iβ 通过 Park 变换得到 Id Iq （4）根据 Id Iq 和目标 Id_target Iq_target 进行 PID 计算得到 Uq Ud (电流环) （5）将 Uq Ud 通过反 Park 变换得到 Uα Uβ （6）将 Uα Uβ 作为 SVPWM 的输入，得到三路半桥的 PWM 输出定时器的比较寄存器值 CCR

3.2 FOC 算法 Clarke 变换

Clarke 变换也就是将无刷电机静止的三相相电流 Ia Ib Ic 三相坐标系转化为静止的 α-β 两相直角坐标系。 下图为三相相电流 Ia Ib Ic 三相坐标系的动态图：

下图为转换成 α-β 两相直角坐标系的动态图：

3.2.1 Clarke 变换公式推导

通过 ADC 采样得到电机的 Ia 和 Ib 两项电流信息，由于基尔霍夫电流定律(公式 Ia + Ib + Ic = 0)，同一个节点流入电流值与流出电流相等，我们可以计算出 Ic，三个电流的相位差为 120°，如下图所示；

通过 Clark 变换，将三相定子坐标系（三个轴互为 120°，Ia , Ib , Ic）转化为两相的定子直角坐标系（i α , i β）这个过程有点类似于力的矢量分解，把三相映射到两相的坐标轴之上，如下图所示。

根据上图把三相映射到两相的坐标轴之上，可以得出 Clarke 变换的推导公式为：

将上式写成矩阵形式：

上式中，系数 K = 2/3，变换前后，幅值不变，即合成矢量的大小和方向相等，等幅值变换其实就是将 Iα =Ia，将系数 K = 2/3 代入上式得：

再根据基尔霍夫电流定律 Ia + Ib + Ic = 0 得出等幅值 Clarke 变换的最终结果为：

3.2.2 Clarke 逆变换公式推导

我们学习了怎么把一个三相时域上的复杂问题用矢量表示，并且用克拉克变换对它进行了降维，得到了降维后的简化表达形式。那么，有没有什么办法能够反过来把降维后的形式重新升维变回原来的 Ia,Ib,Ic 三相电流波形呢？有，这就被称为克拉克逆变换。这个在后续的 FOC 算法中也会很常用到。趁热打铁，我们来看一看怎么进行克拉克逆变换。 我们已经知道了克拉克变换后的 Iα、Iβ，我们在此基础上研究 Clarke 逆变换，首先，Ia 的逆变换我们是不需要研究的，因为我们已经知道了 Iα =Ia。

进一步的，我们研究逆变换 Ib:

最后，还是根据基尔霍夫电流定律：Ia + Ib + Ic = 0，我们得到逆变换 Ic：

则克拉克逆变换三式皆得，总结如下：

3.3 FOC 算法 Park 变换

将电机三相相电流通过 Clark 变化得到 Iα Iβ，然后再将 Iα Iβ 通过变换得到随电机转子转动的 Iq Id 坐标系称为 Park 变换。

3.3.1 Park 变换公式推导

根据上图，角度θ是 d 轴与两相静止坐标系α轴的夹角。根据三角关系推出，Iα在 d 轴上的投影分量为 Iαcosθ，q 轴与β轴的夹角为θ2，可以得出θ2 = θ，因此 iβ在 d 轴上的投影分量为 iβsinθ; iα在 q 轴上的投影分量为 -iαsinθ，因为它的投影分量在 q 轴的负方向上; iβ在 q 轴上的投影分量为 iβscosθ。综合以上，可以得出公式:

这个 id,iq 坐标系是始终跟着转子旋转的； （1）d 轴方向与转子磁链方向重合，又叫直轴； （2）q 轴方向与转子磁链方向垂直，又叫交轴；

经过这一步的变换，我们会发现，一个匀速旋转向量在这个坐标系下变成了一个定值，这个坐标系下两个控制变量都被线性化了，如下图所示；

3.3.2 Park 逆变换公式推导

Vd 在β轴上的投影分量为 Vdsinθ，Vq 在β轴上的投影分量为 Vqcosθ，由此可得出下式：

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

3.4.1 SVPWM 原理及实现

SVPWM (Space Vector Pulse Width Modulation) 即空间矢量脉宽调制。它通过三项逆变器的 6 个功率管输出随时间变化的 PWM 波，模拟三项对称正弦波电流形成的电压矢量园，产生接近圆形的磁链轨迹。SVPWM 的原理如下： （1）基本矢量电压：SVPWM 将三相电压分解为两个正弦波和一个直流分量，这三个分量构成了基本矢量电压。 （2）扇区判断：根据电机的电角度，判断当前所处的扇区。扇区是将电机的 360 度分为六个等分，每个扇区对应一个基本矢量电压。 （3）计算相邻两个基本矢量电压的作用时间：根据当前所处的扇区，计算相邻两个基本矢量电压的作用时间。作用时间是指每个基本矢量电压在一个电周期内的持续时间。 （4）三路 PWM 占空比计算：根据相邻两个基本矢量电压的作用时间，计算三相 PWM 的占空比。占空比是指 PWM 信号高电平的持续时间与一个电周期的比值。 算法流程如下： step1：根据电机的电角度，判断当前所处的扇区。 step2：根据当前所处的扇区，计算相邻两个基本矢量电压的作用时间。 step3：根据相邻两个基本矢量电压的作用时间，计算三相 PWM 的占空比。

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

七段式 SVPWM (Space Vector Pulse Width Modulation) 和五段式 SVPWM 都是一种常用的交流电机控制方法，它们的主要区别在于矢量分解时所使用的向量数不同。 (1) 七段式：七段式 SVPWM 使用七个不同的向量进行矢量分解，可以得到更高的电机效率和更低的谐波失真率。但是，七段式 SVPWM 需要更复杂的计算，因此需要更高的控制精度和计算能力。 (2) 五段式：五段式 SVPWM 只使用五个向量进行矢量分解，计算较为简单，适用于控制较为简单的电机系统。但是，五段式 SVPWM 相对于七段式 SVPWM 的效率和失真率会略差一些。 本设计采用七段式 SVPWM 设计；

七段式 SVPWM 波形图

3.4.3 基本矢量电压

3.4.4 扇区判断

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

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

根据上图 (以扇区一为例) 可以得出三路 PWM 占空比值的公式： value_a = (tpwm - Ta - Tb)/4; value_b = value_a + Ta/2; value_c = value_b + Tb/2;

value_a：占空比占比最多； value_b：占空比占比中等； value_c：占空比占比最少；

每个扇区 A、B、C 占空比占比都不一样，根据 3.4.2 六个扇区的七段式波形图，得出的 value_a、value_b、value_c 代入相对应的扇区；

3.5 FOC 矢量控制 C 语言代码

3.5.1 FOC 矢量控制 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)
    {
        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);
}

3.5.2 FOC 矢量控制 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 文件

#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;
}

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

#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

5.3 主要运行代码

/*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 算法
        }
    }
}