FOC实战：从零到闭环的代码思路与调试全流程

学习FOC大半年，资料堆了一摞，代码一行写不出来——这个状态我太熟了。早几年我刚开始接触矢量控制的时候，也干过同样的蠢事：书买了好几本，网盘里存了几十个PDF，每天对着坐标变换公式、磁链方程、电压方程反复琢磨，每个符号都认识，合上书照样一片空白。后来被一个项目逼着在一个月内把带编码器的同步电机跑起来，我才真正想明白问题出在哪：资料教的是一套完整理论体系，但没人告诉你从零写一份能跑的FOC，顺序其实是反过来的。这篇文章就是给卡在这个阶段的朋友看的，我会把自己从零到闭环的完整思路、写代码的先后顺序、调试时踩过的坑，全部摊开讲。

1. 先说点扎心的：资料没白买，是你的学习顺序反了

很多人学不会FOC，第一个念头是自己数学太差、看不懂公式。我跟你说，大概率不是这个原因。真正的问题是你打开资料的方式不对。

1.1 教科书从电机模型讲起，但你需要从输出讲起

随便翻开一本电机控制的书，前两章肯定是坐标变换理论，第三章开始推导永磁同步电机的d-q轴方程，什么磁链方程、电压方程、转矩方程，接着上状态空间、传递函数。写教材的人默认你要先把电机本体理解透，再学控制。但对于一个只想把电机转起来、看看控制效果的人来说，这个顺序极其劝退。

我自己后来走了条正路，把顺序完全倒过来：先看最终输出，也就是SVPWM怎么把电压变成三相PWM波形；再往前推，看坐标变换怎么把电流从三相坐标系搬到旋转坐标系；然后看PI调节器怎么把误差变成电压指令；最后发现缺哪块理论，再回头翻书查哪块。大概用了三天，开环就能转了，一周接上电流环，之前半年没看明白的东西，突然全串起来了。

不是理论没用，而是理论应该在你有代码和波形之后再来印证。脑子里没有任何工程现象做锚点，公式就只是符号。

1.2 FOC到底在干嘛：一句人话版本

FOC，磁场定向控制，说穿了一点都不神秘。交流电机的定子电流是三相正弦波，合成出来是一个旋转的磁场，转子跟着转。问题是三相正弦电流彼此耦合，直接控制起来非常别扭。FOC的思路是：我换一个坐标系去看这些电流。

有个特别好懂的类比，旋转木马。你站在地面上看坐在木马上的人，他在画圈；如果你自己也跳到木马上，站在他对面看，他就是静止的。坐标系跟着转子转，原本旋转的三相交流量，在新的坐标系里就变成了两个直流量，一个跟磁场方向重合，叫d轴电流，管励磁；一个跟磁场垂直，叫q轴电流，管出力。两个直流量互相独立，各用一个PI调节器伺候就行，控制难度直接降到直流电机的水平。

这个理解一旦建立，后面所有代码就都有了指路明灯：一切操作都是在做坐标系变换，然后在旋转坐标系里做两个独立的PID。

1.3 先把目标拆成三个能落地的小目标

不要一上来就想写出"完整FOC"。那个目标太模糊，拆成三个关卡更实际。

第一关：SVPWM能输出，电机开环转起来，不管转得好不好，先转起来再说。第二关：能读到相电流，做坐标变换后，在示波器或者上位机上看到Id和Iq是缓慢变化的直流量（至少不应该是乱跳的杂波）。第三关：电流环闭环，给一个Iq指令，电机输出一个恒定力矩，用手捏住轴能感觉到稳定阻力，但电机不丢步。

三关过了，你再去研究速度环、位置环、无感观测器。这条路比闷头看书快得多。很多人卡死是因为第一关都没过就去研究无感启动，那纯属给自己上难度。

2. 动手前要准备的家伙：硬件比你想的更简单

"我连硬件都凑不齐"——别急着下结论。入门FOC的硬件门槛没有想象中高，甚至比很多桌面电子项目还简单。

2.1 入门硬件清单：不用买很贵的

控制板：随便一块能输出中心对齐PWM、能触发ADC采样的MCU开发板就行。这类芯片很普及，资料也全。关键看三点：PWM能不能做互补输出、中心对齐，ADC能不能由定时器硬件触发，以及有没有足够的中断优先级配置。那些花里胡哨的AI功能、网络功能，跟FOC一点关系都没有。

驱动板：三相全桥加栅极驱动，入门阶段强烈建议直接买集成好的驱动模块，别自己拿分立MOS搭。自己搭不是不行，但驱动电路的Layout、死区、自举电容这些坑还没等你写代码就先把你干趴下了。集成模块一般还带电流采样运放，省掉好大一堆事。

电机：带增量编码器的低压同步电机，24V或者48V，功率几十瓦，手头能握住那种。为什么非要带编码器？因为无传感器控制是另一个维度的难题，入门阶段主要矛盾是先把坐标变换和PI跑通，先把位置信息这个问题外包给编码器。

电源：直流稳压电源，限流可调。这是个隐形刚需，有几个板子就是上电的时候炸的，限流调到1A左右，哪怕程序有bug也不至于烧一片。

2.2 先补三个最基础的概念

第一个，电角度和机械角度。极对数乘以机械角度等于电角度。编码器测到的是机械角度，到了代码里要做换算。第二个，三相绕组空间上差120度，目的是合成旋转磁场，这个不用深究，知道就行。第三个，相序U-V-W，接反了一个电机方向会反，两个桥臂可能同时导通过流。

这三个概念每个都对应一处常见bug，先记住，后面排查起来能省很多时间。

2.3 软件工具链：示波器比IDE重要

IDE选自己熟悉的就行，调试器会看变量就够。真正重要的是示波器，至少两个通道，便宜的就行，但必须有。调试FOC没有示波器等于闭眼开车。如果条件允许，搞一个能看波形的上位机或者串口绘图工具也行，但实时性差一些，看PI响应还行，看开关噪声就不太够。

另外建议准备一个简单的串口协议，把角度、Id、Iq、转速这些内部变量传上来。这个成本很低，但调试效率能翻倍。我在所有项目里都会默认留一套这种调试通道。

3. FOC代码逐个模块拆开：每一步都在算什么

这是重头戏。我会按代码执行的先后顺序拆，每个模块说清楚输入、输出、以及最常见的坑。

3.1 电流采样：一切闭环的基础，但也是坑最多的地方

FOC里最容易被忽视的就是电流采样。代码看着简单，读ADC然后乘个系数，但采样时刻不对，后面全都会乱套。

工程上三相电流采两相就够了，因为三相电流之和为零，第三相算出来：Ic = -Ia - Ib。采样电阻一般放在下桥臂，采样点放在PWM周期的正中心。为什么？逆变桥在PWM周期内不断开关，产生大量噪声，只有在中心对齐模式下、三个下管都导通的那个时刻，母线回流路径确定，采样电阻上的电压才真实反映相电流。用定时器触发ADC，而不是软件轮询，能保证每次采样都在这个窗口内。

采样电路会有增益和偏置，必须做标定。常见做法是上电时电机不动，采一次零点，之后所有采样值减去这个零点再乘以系数。不做这一步，电流里有直流偏置，坐标变换后Id、Iq全偏移，电机抖动、发热、力不均就全来了。

3.2 Clark变换和Park变换：换个参考系看电流

拿到Ia和Ib之后，第一步是Clark变换，把三相静止坐标系变成两相静止坐标系。工程实现常用简化式：

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

这个式子不同资料的系数可能有差异，但不影响控制效果，因为PI会吸收系数误差。重点是Iα、Iβ仍然是在静止坐标系下的交流量，还要继续转。

第二步是Park变换，把静止坐标系变成旋转坐标系。这里的旋转角就是电角度θ：

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

到这里，原本三相正弦电流就变成了两个直流分量。Id对应励磁，Iq对应力矩。对于表贴式同步电机，目标就是控制Id为零，控制Iq为需要的力矩。

注意正负号各家资料可能不一样，这不是你算错了，而是坐标系轴方向的约定不同。你只要保证反Park变换配套就行，也就是后面SVPWM之前那个逆变换，正负号要和这里严格反着来。

3.3 电角度：坐标变换的灵魂，很多人栽在这里

Park变换和反Park变换都要用到电角度θ。如果θ不对，坐标变换就是错的，PI看谁都是歪的。

用增量编码器时，机械角度要乘以极对数得到电角度。但这还不够，增量编码器只有相对位置，上电那一刻不知道转子在哪个绝对角度，所以必须先找零。

最土的找零办法是给d轴一个恒定的电压矢量，让转子自己转到d轴对齐的位置，这个位置转子是被强行拉过去的，会锁在某个固定电角度上。等转子稳定后，把编码器当前位置记为零点，以后从这个零点累加就能算出真实电角度。这个方法简单可靠，缺点是转子会动一下，对不允许上电转动的场合不适用，入门阶段完全够用。

还有一个经典坑：编码器方向没搞对。电机正转方向角度增加，代码里却是减小，PI会越控越差，最后电机剧烈抖动。排查方法很简单，上位机观察电角度，手动缓慢转动电机轴，看角度是否单调增加，如果减小，就把编码器方向取反或换相序。

3.4 PI调节器：用最土的办法让它稳下来

旋转坐标系里的d和q轴各自需要一个PI调节器，把目标电流和采样电流的误差变成电压指令。这是整个闭环的核心部分，代码本身很短，但坑很深。

我推荐用位置式PI，离散化之后的表达式：

u[k] = Kp * err[k] + Ki * integral[k]

integral += err[k] * Ts

这里的Ts是控制周期。看起来简单，但有两个东西必须加：积分限幅和输出限幅。

积分项会累积误差，如果输出一直饱和，积分还在涨，等误差反向时就会出现很长的恢复死区，这叫积分饱和。我见过很多新手的PI代码跑飞，都是这个原因。正确做法是输出限幅之后，把实际输出值和未限幅值做差，反馈回去消掉积分多出来的部分，或者最简单粗暴一点，直接对积分值做限幅。

Kp和Ki先给个能转的初值，比如Kp很小，Ki更小，然后慢慢加，后面第四章会说整定方法。

3.5 SVPWM：把Vd、Vq变成三相占空比

PI输出的是d-q轴电压指令Vd、Vq，得先做反Park变换变成静止坐标系的Vα、Vβ：

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

然后交给SVPWM。SVPWM的思想不是三个独立的半桥各自随便给占空比，而是让合成的电压矢量落在空间里的某个扇区，用这个扇区相邻的两个非零矢量和零矢量，按时间比例拼出一个等效的期望矢量。

判扇区可以不用查表，直接算三个判别量：

V1 = Vβ V2 = √3/2 * Vα - 1/2 * Vβ V3 = -√3/2 * Vα - 1/2 * Vβ

根据这三个量的正负组合就可以确定扇区。得到扇区后，再代入标准公式算出两个相邻矢量的作用时间T1、T2，T0 = Tpwm - T1 - T2分给零矢量。如果T1+T2超过了Tpwm，要做过调制处理，按比例收缩。最后把这些时间换算成三个比较寄存器值，配置成中心对齐的互补PWM输出。

SVPWM这部分代码本身手写也没有多少行，网上到处都是参考。我的建议是找一个你能看懂的版本抄下来，先把开环跑通，验证波形是马鞍形的相电压调制波，再谈优化。

3.6 整个控制环路的顺序与时序

把上面几块串起来，一个完整的中断服务流程是这样的：

进入PWM周期的中心时刻，定时器触发ADC采样，读出两相电流；读取编码器位置，换算电角度；做Clark变换、Park变换，得到实际Id、Iq；分别与目标Id（一般给0）、目标Iq（来自速度环输出或直接给定）比较，经过两个PI调节器得到Vd、Vq；反Park变换得到Vα、Vβ；SVPWM计算三个占空比并更新PWM寄存器。全部在一个控制周期内完成，控制周期通常就是PWM周期本身，比如10kHz的开关频率，那控制周期是100微秒。

PI运算要在中断里做完整，不能让其他任务打断它，中断优先级要比主循环和通信任务更高。Rotor的位置读取也要注意，编码器一般是正交解码定时器自己累加计数的，中断里直接读寄存器的当前值就行，不用软件查询。

4. 调试实录：从开环到闭环的真实过程

现在假设你已经按上面的模块把代码拼出来了。接下来不是看一遍代码直接上电就能转的，调试是有顺序的。

4.1 第一关实测：让电机开环先转起来

先不写电流采样，也不用PI，直接让SVPWM模块输出一个恒定的电压矢量，然后每隔一段时间让它跳变到下一个角度。这样电机轴上等于有一个不连续旋转的磁场，会顿挫地一步步转动。能转，说明SVPWM的扇区判断、占空比计算、PWM映射都是对的。

再把步进改成连续扫描角度，也就是让电压矢量角度连续增加，电机就变成缓慢匀速旋转。这时候观察电机声音是否均匀、有没有高频啸叫，如果有啸叫，多半是PWM频率进入听觉范围或者死区设置不对。

这一步检验的是最底层的代码正确性。很多人在这一步卡住，经常是三相PWM的映射关系错了，导致每60度扇区里有一两个扇区方向不对，现象是电机转到某个特定角度位置明显卡顿、发烫。解决办法就是逐扇区检查计数值的映射，用示波器看三相PWM波形是不是互差120度的正弦调制序列。

4.2 闭环之前必查的两件事

电机能开环转之后，接入电流采样和坐标变换，但暂时不闭环，先把Iq和Id波形当作观测量。手动把转子拨到一个固定位置，给定一个固定的电压矢量，让电机锁住，观察上位机里的Id、Iq数值。如果电流采样方向反了，你会发现给正方向电压时Iq算出来是负的，而且绝对值还很大。这个必须修正，否则闭环之后越控越乱。

第二个必查项是电角度零点。用前面说的给d轴电压找零的办法，记录零点偏移。然后手动缓慢转一圈转子，观察电角度是不是跟着电机的极对数规律变化，从0到2π连续走极对数圈。如果角度跳变不连续，多半是计算错了，最典型的是机械角度乘极对数之后超过了编码器单圈范围，没有做取模归一化。

这两项确认无误，闭环才算有资格接通。我见过太多人跳步直接闭环，结果电机一顿乱抖，根本分不清是电流问题还是角度问题，浪费时间。

4.3 常见问题速查表

4.4 我的调试习惯总结

调试这些年下来，我总结出三个习惯，对入门FOC有奇效。第一个，一次只改一个变量。很多人喜欢同时调Kp、Ki又换PWM频率，出了问题根本不知道谁引起的。第二个，时刻盯着Id和Iq这两个量。它们是最直接的信息来源，比看转速和位置可靠得多。第三个，在所有输出路径上面加保护。速度环输出限幅、电流环输出限幅都要设，宁可电机动不了，也不要因为一个饱和让过流烧板子。

另外一个特别增效的小办法是给中断里的代码跑一个粗略的执行时间统计，比如用个IO口在中断开头拉高、结尾拉低，接示波器看占空比。如果中断任务太重，会在高PWM频率时出问题，这个习惯能提前帮你发现隐患。

能坚持走到闭环这一关，FOC的骨架基本就算搭起来了。到这一步再去翻你手头那堆资料，会发现以前看不懂的磁链方程、电压方程，突然变得有落点了——它们描述的正是你代码里正在算的东西。之后要扩展速度环、位置环甚至无感控制，本质上都是在这个骨架上叠加新的模块。最后再分享一个让我受益最多的习惯：刚开始做调试时，每写一个模块都先用最简单的方式独立验证，不要一次性把所有代码全写完再上电。我当时是写好SVPWM先转开环，看到电机转起来才继续写采样，然后写坐标变换，一个个接上去。每接一个模块就验证一次，出问题时定位范围很小，几乎不会再出现"哪哪都不对"的局面。这份路数按部就班走下来，哪怕基础弱一点，大概率也能在两周内看到自己的电流环跑起来。剩下的，就交给耐心和时间。