FOC核心公式工程实践：从Clarke变换到SVPWM的代码实现

1. 项目概述：从零到一，拆解FOC核心公式的工程实践

搞电机控制，尤其是永磁同步电机（PMSM）和直流无刷电机（BLDC），FOC（磁场定向控制）是绕不开的一道坎。很多朋友，包括我自己刚入门那会儿，面对一堆坐标变换、空间矢量调制（SVPWM）的公式，感觉就像在看天书。网上的资料要么过于理论，满篇微积分和矩阵推导；要么就是直接给代码，知其然不知其所以然，调参数全靠蒙。这个项目，就是想做一件“笨”事：把FOC实现过程中那些最关键、最核心的公式，从它们诞生的物理意义出发，到最终如何落地成一行行代码，进行一次彻底的梳理和“翻译”。

这不是一篇学术论文，而是一份来自工程一线的实战笔记。我会假设你已经有了一些电机控制的基础概念，比如知道三相电流、知道PWM，但被Clarke、Park变换搞得晕头转向，对SVPWM的七段式、五段式实现感到困惑。我们的目标很明确：打通从理论公式到嵌入式C代码的最后一公里。你会看到每个公式的“前世今生”——它为什么被需要，它解决了什么问题，以及最关键的，在单片机的内存和算力限制下，我们如何对它进行简化、定点化和优化。无论是用STM32的电机库，还是自己从零手搓FOC，这份公式整理都能帮你建立起清晰的实现地图，避免在调试的泥潭里打转。

2. FOC整体框架与核心思路拆解

在深入公式之前，我们必须先站在高处，看一眼FOC的完整作战地图。FOC，又称矢量控制，它的核心思想可以用一句话概括：模仿直流电机的控制方式。直流电机为什么好控制？因为它的励磁磁场和电枢磁场在空间上是自然解耦的，调节电枢电流就能直接、线性地控制转矩。而交流电机的三相绕组产生的磁场是耦合的、旋转的，直接控制三相电流非常困难。

FOC的智慧就在于，它通过一套“数学魔术”，把我们在静止坐标系（ABC三相）下观测到的耦合的、交流的物理量，变换到一个与转子磁场同步旋转的坐标系（dq坐标系）下。在这个旋转的坐标系里，原本交流的量变成了直流量，并且被解耦成了两个分量：直轴分量（Id）和交轴分量（Iq）。Id专门用来产生磁通（类似于直流电机的励磁电流），Iq专门用来产生转矩（类似于直流电机的电枢电流）。这样一来，我们就能像控制直流电机一样，通过分别控制Id和Iq的参考值，来独立控制电机的磁场和转矩，实现了精准的动态控制。

整个FOC的闭环控制流程，可以概括为以下五个关键步骤，这也是我们公式梳理的主线：

  1. 测量与采样：通过ADC采样得到电机的三相电流（Ia, Ib, Ic）和直流母线电压（Udc）。这是所有控制的源头，采样精度和实时性至关重要。
  2. Clarke变换：将三相静止坐标系（ABC）下的电流，变换到两相静止坐标系（αβ）下。这一步将三个变量减少为两个，并且消除了三相系统中的零序分量（如果三相平衡，则零序分量为0）。
  3. Park变换：将两相静止坐标系（αβ）下的电流，变换到两相旋转坐标系（dq）下。这个变换需要转子当前的电角度（θ）。变换后，我们便得到了解耦的直轴电流Id和交轴电流Iq。
  4. 电流环PI控制：将测量得到的Id、Iq与它们的给定值（Id_ref, Iq_ref）进行比较。Id_ref通常设为0（对于表贴式永磁电机，采用Id=0控制），Iq_ref则由外环（速度环或位置环）给出。PI控制器计算出在dq旋转坐标系下需要施加的电压矢量Vd和Vq。
  5. 反Park变换与SVPWM：将旋转坐标系下的电压指令Vd、Vq，通过反Park变换还原到静止的αβ坐标系，得到电压矢量Vα和Vβ。SVPWM算法则负责将这个电压矢量，通过逆变桥的六个开关管，合成出对应的PWM波，最终驱动电机。

接下来，我们就沿着这条主线，逐一拆解每个环节的核心公式、工程实现要点和那些容易踩坑的细节。

3. Clarke变换：从三相到两相的降维与简化

3.1 公式推导与物理意义

我们直接从三相电流Ia, Ib, Ic开始。在理想的三相对称系统中，有Ia + Ib + Ic = 0。Clarke变换的目标，是将这三个互成120度、在时间上相位互差120度的交流量，映射到一个二维的静止直角坐标系（α-β坐标系）中。

最经典的等幅值变换公式如下：

Iα = Ia Iβ = (Ia + 2*Ib) / sqrt(3)

或者写成更通用的矩阵形式：

[ Iα ] [ 1, -1/2, -1/2 ] [ Ia ] [ Iβ ] = [ 0, sqrt(3)/2, -sqrt(3)/2 ] [ Ib ] [ I0 ] [ 1/2, 1/2, 1/2 ] [ Ic ]

其中I0是零序分量，在三相平衡且无中线时为零，通常我们只关心Iα和Iβ。

为什么需要这个变换？

  1. 降维与简化：将三个相互依赖的变量（因为和为0）转换为两个独立的变量，降低了后续处理的复杂度。
  2. 为旋转变换做准备：α-β坐标系是一个静止的二维坐标系，它是通往旋转的d-q坐标系的必经桥梁。所有空间矢量的合成与分解都在这个平面内进行，非常直观。

3.2 工程实现与定点化技巧

在嵌入式系统中，我们很少直接使用浮点数进行sqrt(3)的运算。常见的优化方法有两种：

方法一：近似系数法由于sqrt(3) ≈ 1.7320508，1/sqrt(3) ≈ 0.57735027。在保证一定精度的前提下，可以使用定点分数来近似。例如，采用Q15格式（16位有符号整数，小数点在第15位之后），我们可以将系数放大2^15=32768倍。

  • 1/sqrt(3)的Q15值约为0.57735 * 32768 = 18918。
  • 那么Iβ的计算可以近似为：Iβ = (Ia + 2*Ib) * 18918 >> 15（先乘后右移15位，实现定点除法）。

方法二：忽略系数法（适用于某些库）在一些对绝对幅值要求不严格，更关注矢量方向的场合（比如某些SVPWM算法），为了追求极致的速度，会使用一种“变幅”的Clarke变换，即忽略1/sqrt(3)这个系数：

Iα = Ia Iβ = (Ia + 2*Ib) / 3 * 2 // 注意，这里用了2/3，是另一种常见形式，但幅值已变。

或者更简单地：

Iα = Ia Iβ = (Ia + 2*Ib) // 直接忽略系数，此时Iβ的幅值被放大。

注意：使用忽略系数的变换后，后续的Park变换、PI参数乃至SVPWM的输入范围都需要重新标定和调整，不能混用不同标准的变换系数。ST的电机库（MCSDK）早期版本就使用了这种简化，需要特别注意。

实操心得：

  • 采样通道的校准：Ia, Ib, Ic的采样值通常存在偏置（零点漂移）。必须在每次上电或定期进行偏置校准。一个简单的方法是在PWM输出为50%占空比（即零电压矢量）时，采样多组电流值并求平均，将此平均值作为该通道的偏置值，在后续采样中减去。
  • 克拉克变换的验证：可以用信号发生器给电机三相注入一组平衡的正弦波（或直接让电机低速空转），然后用示波器或软件抓取变换后的Iα和Iβ。它们应该是相位相差90度的两路正弦波，并且幅值相等。这是验证采样和变换是否正确的重要一步。

4. Park变换及其反变换：解耦的关键旋转门

4.1 公式推导与角度依赖

得到了静止的(Iα, Iβ)矢量后，我们需要知道转子磁场的实时位置（电角度 θ），才能通过Park变换将其“旋转”到与转子同步的d-q坐标系中。d轴（直轴）通常定义在转子永磁体磁极的方向上，q轴（交轴）超前d轴90度电角度。

Park变换公式如下：

Id = Iα * cosθ + Iβ * sinθ Iq = -Iα * sinθ + Iβ * cosθ

反Park变换（将d-q坐标系下的电压指令Vd, Vq变换回 α-β 坐标系）公式如下：

Vα = Vd * cosθ - Vq * sinθ Vβ = Vd * sinθ + Vq * cosθ

这两个变换是FOC解耦的灵魂。经过Park变换后，如果转子位置θ是准确的，那么：

  • Id就代表了与转子磁场方向一致的电流分量，控制它就能控制主磁场强弱（用于弱磁控制）。
  • Iq就代表了产生转矩的电流分量，控制它就能直接、线性地控制电机的电磁转矩。

4.2 工程实现：查表法与CORDIC算法

在实时控制中，计算sinθ和cosθ是主要开销。有几种主流方法：

方法一：查表法这是最常用、最快捷的方法。预先将一个正弦周期（0~2π）的sin值（或同时计算cos）以一定分辨率（如256点、512点）制成表格，存储在Flash或RAM中。

  • 角度归一化：将实际电角度θ（通常是0~2π的浮点数）映射到查表索引。例如，表长N=512，则索引index = (int)(θ / (2π) * N) % N。
  • 获取正弦/余弦值：sin_val = sin_table[index]；cos_val = sin_table[(index + N/4) % N]（因为cosθ = sin(θ+π/2)）。
  • 定点处理：查表值通常也是定点数（如Q15）。整个变换过程就是一系列的定点乘加运算。

方法二：CORDIC算法这是一种通过迭代移位和加减来计算三角函数和反三角函数的硬件友好型算法。在没有硬件浮点单元（FPU）且追求高精度和灵活角度分辨率的场合，用CORDIC实现Park变换是一个不错的选择。它不需要大容量的查找表，但需要若干次迭代循环。许多现代单片机的DSP指令集甚至直接支持CORDIC运算。

方法三：硬件FPU如果MCU带有硬件FPU（如STM32F4/F7/H7系列），直接使用浮点数运算库（如arm_sin_cos_f32）也非常方便，代码简洁，精度高。

注意事项：

  • 角度θ的来源：对于有感FOC，θ来自编码器或旋转变压器的解算。对于无感FOC，θ来自观测器（如滑模观测器、锁相环、扩展卡尔曼滤波）的估算。无论来源如何，确保输入Park变换的θ是准确且连续的（避免360°跳变）至关重要。
  • 计算顺序与溢出：定点乘加运算要注意中间结果的位数扩展，防止溢出。例如，两个Q15数相乘，结果是Q30，需要根据情况进行舍入和移位，再存储回Q15格式。
  • 归一化的处理：有些算法会将电流、电压值归一化到标幺值系统（例如，以最大相电流峰值为基值，以直流母线电压为基值）。这样可以使PI控制器的参数设计更具通用性，不受具体电机参数和电压等级的影响。如果采用标幺值系统，Park变换的输入输出也应是标幺值。

5. 电流环PI控制器：动态性能的调节器

5.1 离散化公式与参数整定

在dq旋转坐标系下，我们得到了电流误差e_d = Id_ref - Id,e_q = Iq_ref - Iq。PI控制器的作用就是根据这个误差，计算出需要施加的电压Vd和Vq。

连续域的PI公式为：V(t) = Kp * e(t) + Ki * ∫ e(t) dt

在数字系统中，我们需要将其离散化。最常用的是位置式PI：

V[k] = Kp * e[k] + Ki * sum(e[j]) * Ts (j从0到k)

其中Ts是控制周期（电流环采样周期）。

为了避免积分饱和和方便编程，更常用的是增量式PI：

ΔV[k] = Kp * (e[k] - e[k-1]) + Ki * Ts * e[k] V[k] = V[k-1] + ΔV[k]

增量式PI只与最近两次的误差有关，计算量小，且自带抗积分饱和特性（因为输出是增量，天然受限），在实际工程中应用更广。

参数整定思路（经验法）：

  1. 将Ki设为0，先调节Kp。逐渐增大Kp，直到电流环出现轻微的超调或振荡，此时系统响应快但稳定性在临界点。
  2. 固定这个Kp，然后逐渐增加Ki。Ki能消除静差，但过大的Ki会引起低频振荡或使系统变慢。调节Ki直到负载突变时，电流能快速跟踪且静差被消除。
  3. 微调：根据实际响应（上升时间、超调量、恢复时间）对Kp和Ki进行微调。

注意：dq轴电流环通常是解耦的，可以分别设计PI参数。但由于电机存在反电势和耦合项，理论上需要加入前馈解耦。不过在很多中低速场合，PI控制器足够的带宽足以抑制这些耦合影响，可以省略复杂的解耦项，简化实现。

5.2 抗饱和与输出限幅

这是电流环稳定工作的保护伞，绝对不能少。

  • 积分抗饱和：当控制器输出Vd或Vq达到限幅值时，应停止积分项的累加，或者只累加与输出方向相反的误差（Conditional Integration）。这可以防止系统“深陷”饱和状态无法退出。
  • 输出限幅：PI控制器的输出电压指令不能无限大，它受到直流母线电压Udc的物理限制。根据SVPWM的调制理论，α-β平面内可合成的最大不失真电压矢量幅值为Udc / sqrt(3)。因此，需要对Vd和Vq进行限幅，通常做法是限制其合成矢量的幅值：sqrt(Vd^2 + Vq^2) <= Udc / sqrt(3)。更简单的做法是分别对Vd和Vq进行限幅，但这样会牺牲一部分电压利用率。

实操心得：

  • 控制频率的选择：电流环的频率通常最高，一般为10kHz ~ 20kHz。更高的频率意味着更快的动态响应和更低的电流纹波，但对ADC采样速度、计算能力和PWM分辨率的要求也更高。
  • 不同运行模式：在启动或低速时，可能采用开环V/F控制或I/F控制，此时电流环是打开的，直接给定Vα和Vβ。在切换到闭环FOC时，需要有一个平滑的过渡过程，避免电流冲击。
  • 调试工具：利用MCU的DAC或串口实时输出Id,Iq,Vd,Vq等关键变量到示波器，是调试PI参数最直观有效的方法。观察给定阶跃响应下的电流跟踪波形。

6. SVPWM：将电压指令变为PWM波形的艺术

6.1 基本原理与扇区判断

反Park变换后，我们得到了静止坐标系下的电压矢量指令Vα和Vβ。SVPWM的任务是，在一个PWM周期（Ts）内，通过控制逆变桥六个开关管的通断，合成出一个平均效果等于Vref (Vα, Vβ)的电压矢量。

三相逆变桥有8种基本的开关状态（000, 001, 010, ..., 111），对应6个非零矢量（U1(001), U2(010), U3(011), U4(100), U5(101), U6(110)）和2个零矢量（O000, O111）。这6个非零矢量将α-β平面均匀地划分为6个扇区。

第一步：扇区判断判断Vref落在哪个扇区，是SVPWM计算的第一步。一个经典高效的判断方法基于Vα和Vβ以及三个中间变量：

U1 = Vβ U2 = sqrt(3)/2 * Vα - 1/2 * Vβ U3 = -sqrt(3)/2 * Vα - 1/2 * Vβ

然后根据U1, U2, U3的正负号，通过查表或条件判断即可确定扇区号（Sector=1~6）。同样，这里的sqrt(3)/2可以用定点数近似。

6.2 作用时间计算与七段式调制

确定扇区后，就需要计算两个相邻非零矢量（例如扇区I的U4(100)和U6(110)）以及零矢量的作用时间T1,T2,T0。

以扇区I为例，计算公式为：

T1 = Ts * (sqrt(3) * Vα - Vβ) / Udc T2 = Ts * (2 * Vβ) / Udc T0 = Ts - T1 - T2

其中Udc是实测的直流母线电压。T1和T2必须满足T1 + T2 <= Ts，否则需要进行过调制处理（等比例缩小T1,T2）。

七段式SVPWM是最常用的调制方式，它在一个周期内对称地安排矢量的作用顺序，例如扇区I的顺序为：O000 -> U4 -> U6 -> O111 -> O111 -> U6 -> U4 -> O000。这种对称排列能有效降低开关损耗和电流谐波。我们需要根据扇区和计算出的T1,T2，计算出每个PWM通道的比较值（CCR）。

通用比较值计算（以中心对齐PWM为例）：假设PWM定时器的计数周期为PWM_Period，采用中央对齐模式（计数先增后减）。

  1. 首先计算基本时间对应的计数值：T1_cnt = T1 / Ts * PWM_Period,T2_cnt = T2 / Ts * PWM_Period。
  2. 定义t_a, t_b, t_c为三相占空比对应的计数值（高电平时间）。
  3. 根据不同的扇区，有固定的映射关系。例如对于扇区I：
    t_a = (PWM_Period - T1_cnt - T2_cnt) / 2 t_b = t_a + T1_cnt t_c = t_b + T2_cnt
  4. 将t_a, t_b, t_c赋值给三相PWM通道的比较寄存器。

6.3 工程优化与常见问题

  • 定点化与标幺值：将Vα,Vβ,Udc都标幺化处理，可以简化计算。例如，定义电压基值U_base = Udc / sqrt(3)，则Vα,Vβ的标幺值范围应在[-1, 1]之间。时间计算可以全部用标幺值进行，最后再乘以PWM_Period。
  • 过调制处理：当T1 + T2 > Ts时，说明指令电压幅值超过了线性调制区（六边形内切圆）。此时需要将T1和T2等比例缩小：T1' = T1 * Ts / (T1+T2),T2' = T2 * Ts / (T1+T2)。这会导致输出电压波形畸变，但能提高直流母线电压利用率。
  • 死区时间插入：为了防止逆变桥上下管直通，必须在互补的PWM信号中插入死区时间。这个时间由硬件定时器的死区发生器模块自动添加，在软件计算比较值时不需要考虑。但死区时间会影响实际输出电压，尤其在低速时会产生畸变，可能需要软件补偿。
  • SVPWM vs SPWM：SVPWM相比传统的正弦PWM（SPWM），在相同的直流母线电压下，能输出更高幅值的基波电压（约高出15.47%），即电压利用率更高。这是SVPWM的一大优势。

常见问题排查：

  • 电机不转或振动：首先用示波器测量三相PWM输出波形，检查是否对称，死区时间是否合适。然后检查SVPWM扇区判断和比较值计算是否正确，可以静态给定一个小的Vα和Vβ，观察生成的PWM占空比变化是否符合预期。
  • 电流波形畸变严重：检查ADC采样时刻是否在PWM中心点（对于中央对齐PWM）或谷底/峰顶（对于边沿对齐PWM），以确保采样到的是电流的平均值，避免开关噪声干扰。检查电流采样电路的带宽和滤波参数。
  • 高速时控制性能下降：可能是控制频率不够高，或者SVPWM的过调制处理不当。检查在高速大电压指令下，T1+T2是否频繁大于Ts，以及过调制算法是否正确。

7. 无感FOC的基石：位置与速度观测

对于无传感器FOC，Park变换所需的电角度θ不是测量得到的，而是通过算法观测估算出来的。这是无感FOC中最具挑战性的部分。

7.1 滑模观测器（SMO）

滑模观测器因其鲁棒性强、实现相对简单而广泛应用。其核心思想是构建一个电流观测器，利用反电动势（EMF）中包含的位置信息。

  1. 建立基于反电动势的电机模型：在α-β静止坐标系下，PMSM的电压方程可以写为：

    Uα = R*iα + L*d(iα)/dt + eα Uβ = R*iβ + L*d(iβ)/dt + eβ

    其中eα = -ω * ψ_f * sinθ,eβ = ω * ψ_f * cosθ就是包含角度信息的反电动势。

  2. 设计滑模观测器：构造一个电流观测器，其输入是实际电压Uα, Uβ，输出是观测电流iα_hat, iβ_hat。观测器模型中包含一个开关控制项Zα, Zβ（滑模控制量）来迫使观测电流跟踪实际电流。

    L*d(iα_hat)/dt = Uα - R*iα_hat - Zα L*d(iβ_hat)/dt = Uβ - R*iβ_hat - Zβ

    滑模控制律通常取Z = K * sign(s)，其中s = i_hat - i是电流误差。

  3. 提取反电动势与角度：在系统进入滑模面后（即观测电流跟踪上实际电流），Zα, Zβ的平均值就等于反电动势eα, eβ。通过一个低通滤波器（LPF）滤除开关高频分量后，即可得到平滑的反电动势估计值eα_hat, eβ_hat。

  4. 计算角度：电角度可以通过反电动势的反正切计算得到：

    θ_hat = atan2(-eα_hat, eβ_hat)

    电速度ω_hat可以通过对角度差分得到，或者通过反电动势的幅值除以磁链常数估算：ω_hat = sqrt(eα_hat^2 + eβ_hat^2) / ψ_f。

SMO的优缺点与调参：

  • 优点：对参数变化（R， L）不敏感，鲁棒性好。
  • 缺点：低通滤波器会引入相位延迟，影响高速性能；开关函数sign()会产生高频抖振。
  • 调参关键：滑模增益K和低通滤波器的截止频率。K需要足够大以保证收敛速度，但太大会加剧抖振。截止频率需要在滤除噪声和减少相位延迟之间折衷。

7.2 锁相环（PLL）

从滑模观测器得到的角度θ_hat通常含有噪声，并且低速时反电动势信噪比低，角度不准。锁相环用于从含有噪声的反电动势信号中提取出平滑的角度和速度。

PLL的结构通常包括：

  1. 鉴相器：计算反电动势估计值eα_hat, eβ_hat与根据PLL内部角度θ_pll生成的正交信号之间的相位误差。 一个常用的方法是：phase_error = -eα_hat * cos(θ_pll) + eβ_hat * sin(θ_pll)。当相位锁定时，这个误差为0。
  2. 环路滤波器：通常是一个PI控制器。它根据相位误差phase_error计算出速度估计值ω_pll。PI参数决定了PLL的带宽和动态响应。
  3. 积分器：将速度ω_pll积分，得到平滑的角度输出θ_pll。

PLL的输出θ_pll和ω_pll就是最终提供给Park变换和速度环的平滑位置和速度信号。

扩展卡尔曼滤波（EKF）是另一种更高级的观测器，它将电机模型线性化，并同时估计电流、角度、速度等多个状态量，理论上能提供最优的估计性能，但对模型精度和计算能力要求很高，调参也更复杂。

7.3 无感启动策略

无感FOC在零速和低速时，反电动势几乎为零，观测器无法工作。因此，必须有一个开环启动过程。

  1. 预定位：给定一个固定的电压矢量，将转子强制拉到一个已知的初始位置（如0度），持续一小段时间。
  2. 开环加速（I/F控制）：以开环方式，给定一个逐渐增加频率的旋转电压矢量，同时电流闭环（或开环给定电流）控制电流幅值，将电机拖到一定速度（例如5%-10%额定转速）。这个过程中，给定的电角度是虚拟的、开环递增的。
  3. 观测器切入与闭环切换：当电机速度达到足够高，反电动势观测器（如SMO）能够稳定工作时，将观测器输出的角度θ_hat逐步替换掉开环给定的角度，实现平滑过渡，最终进入完全闭环的无感FOC运行状态。

这个切换过程是软肋，切换时机和过渡策略的好坏直接决定了启动的可靠性和平稳性。