无感FOC核心代码：SMO+PLL三段式启动，PMSM/BLDC通用

做无感FOC最怕的不是写不出代码，而是写出来以后没法用。我手头这个设备改造项目就是这样：主轴没有编码器安装空间，客户只给了电机三根线，让我把原来的方波驱动改成无感FOC。网上代码找了一大堆，有的空载能转、一带负载就失步，有的一上电就啸叫，真正能在PMSM和BLDC上都能稳定跑起来、还能处理启动切换全过程的核心代码，其实并不多。

这套代码是我自己从头整理并实测通过的无感FOC核心架构，核心是三段式启动 + 滑模观测器（SMO）+ 锁相环（PLL）的角度链路。它同时适用于永磁同步电机（PMSM）和直流无刷电机（BLDC），只要电机参数能测出来就能跑。这篇文章不仅聊能用的代码，还会把观测器为什么这么搭、启动切换为什么这样设计、踩过的坑怎么排查，一条线讲透。

1. 这套无感FOC代码的核心架构：SMO + PLL怎么配合才能稳定换相

1.1 为什么是滑模观测器而不是高频注入

无感FOC的本质问题很简单：我要驱动一个三相电机，但手上没有霍尔、没有编码器，根本不知道转子转在哪里。没有角度，Park变换就是空谈，电流环也闭环不起来。所以所有无感方案的核心，都是从定子电压和电流里把反电动势（或者磁链）估计出来，再从反电动势里把转子位置抠出来。

常见的路线有两条：高频注入和反电动势观测。高频注入适合低速甚至零速，但它的原理依赖转子的凸极性，表贴式永磁电机（SPMSM）往往不太给力，还会引入明显的高频噪音，调试成本不低。而滑模观测器依赖的是反电动势，转速越高信号越强，中高速段非常可靠，结构又简单，算力要求也不高，一台几十块钱的MCU就能跑。

这套代码我选择SMO，是因为应用场景以中高速为主，启动阶段用开环强拖补位，正好绕开SMO低速信号弱的短板。整套方案在PMSM和BLDC上都能用，因为两者的定子电压方程在αβ坐标系下长得一模一样，区别只在反电动势的谐波含量，这部分后面专门讲。

1.2 从电流误差到反电动势：SMO的完整信号链路

先看电机在αβ坐标系下的电压方程：

vα = R * iα + L * diα/dt + eα vβ = R * iβ + L * diβ/dt + eβ

其中 eα、eβ 就是反电动势，它和转子电角度直接相关：

eα = -Ψf * ωe * sin(θe) eβ = Ψf * ωe * cos(θe)

只要能拿到 eα、eβ，θe 就能算出来。但问题是我采样到的只有电压和电流，反电动势藏在这两个量中间，没法直接读。SMO的思路是：把电流估计值和实际采样值做差，用这个误差驱动一个滑模切换项，让电流估计值紧紧咬住实际值。当电流误差收敛到零时，那个切换项的等效平均值恰好就是反电动势。

信号链是这样的：

电压/电流采样 → Clarke变换 → αβ轴电流误差 → 符号函数×滑模增益 → 低通滤波 → eα/eβ → PLL → θe/ωe

滑模切换项本质是个高频开关信号，直接拿它当反电动势用，噪声会拉爆PLL，所以后面必须加低通滤波器，滤波后再进PLL。PLL在这里的作用不只是拿角度，它本身是个闭环跟踪器，能压制谐波和噪点，比直接atan2算角度要平滑得多。

这里我一直用PLL而不是atan2，原因很实际：atan2对每个周期的估算误差都直接反映在角度上，带载时电流谐波一大，角度就跟着抖，速度环也连带抖。PLL相当于给角度加了一个有带宽限制的跟踪环，谐波抖动被天然滤掉一大截，切换和带载的稳定性都明显更好。

2. 三段式无感启动：预定位、开环强拖、切闭环，每个环节的参数怎么定

2.1 预定位的两个关键参数：电压矢量和驻留时间

无感启动第一件事，是预定位。零速下反电动势几乎没有，观测器完全瞎了，你不能直接给随机角度，否则电机会卡顿甚至反方向转。预定位的原理很简单：给一个固定的电流矢量，让定子磁场把转子强行拉到一个已知位置。

我习惯用电流闭环给定一个固定方向的电流，而不是直接给电压。这样不管电机扭矩常数多大，预定位电流都是可控的。参数上有两个重点：

• 电流幅值：一般取额定电流的20%到40%。太小，摩擦力矩和负载力矩拽着转子，拉不到位；太大，转子到位后继续发热，预定位又没实际意义。
• 驻留时间：不是给几个毫秒就行，要让转子的机械振荡衰减掉。我实测下来，小惯量电机100ms左右就够，大惯量场合至少300ms起步。判断标准是电机的电流和转速都稳定下来，没有振荡感。

有些代码预定位结束直接怼开环强拖，结果开环阶段明显顿挫。这多半是驻留时间不够，转子还在振荡就被拽走了。

2.2 开环强拖的频率斜坡规划与电流闭环保持

预定位完成后，转子已经在已知角度。这时候我开始让给定角度匀速增大，速度从零或者一个很低的起始转速往上爬，同时用电流环把电流幅值钳住，这就是开环强拖。

关键点是：强拖阶段的电流环是闭环的，角度环是开环的。我给电流环一个固定电流幅值指令，方向由不断递增的给定角度决定。角度递增的速度由电频率决定，公式是：

θe(k) = θe(k-1) + 2π * f_step * Ts

f_step 的规划是启动成败的核心。斜坡太快，转矩跟不上，转子被拖着跑就失步；斜坡太慢，启动时间太长，重载场合还可能一直顶死在堵转区。我现在的做法是：起始频率设在2Hz左右，斜坡加速度按负载惯量估，轻载200Hz每秒左右，重载要降到50Hz甚至更低。

强拖阶段的电流幅值也不是固定不变的。速度上来以后，反电动势变大，母线电压余量变小，此时如果还按大电流强拖，很容易过压失步。我一般做两段：低速段用启动电流，转速超过切换点的一半以后，电流按线性斜坡降到额定电流的80%左右，给后面切闭环留缓冲。

2.3 切换闭环的判定条件与角度补偿

切闭环是整个无感FOC里最容易翻车的一步。切太早，SMO的估算角度还不准，直接跳变过流；切太晚，开环角度和实际角度可能已经偏差很大，切换瞬间同样会过流。

我的判定条件有两个，缺一不可：

1. 转速条件：电频率已经超过SMO可靠运行的最低速度。对于这套SMO，我一般取5%到10%额定转速以上，低于这个值反电动势的信噪比太差。
2. 角度差条件：开环给定角度和SMO估算角度之间的差值持续稳定在一个小范围内。注意是"持续"，不是瞬时，我一般要求连续50个周期内误差小于5°电角度。

切换瞬间必须做角度补偿。开环阶段我用的给定角度，切换时刻要平滑过渡到观测角度，不能直接硬切。方法是：在状态机切到闭环的那一刻，让PLL的θ输出先等于当前开环给定角度，然后让观测器角度自然接管。这样电流环在切换前后不会感知到角度突变，过流和顿挫基本消失。

3. 直接可用的核心代码：SMO迭代、PLL提取角度、启动状态机

3.1 SMO观测器核心代码

这里给的是我实际在用的SMO简化版，刻意去掉了厂商库依赖，全部用标准C写，方便移植到任何平台。核心逻辑就在电流误差和切换项上：

#include <math.h> typedef struct { float Ts; // 控制周期，单位秒 float Rs; // 定子电阻，欧姆 float Ls; // 定子电感，亨 float lpf_w; // 低通滤波截止频率，rad/s float K_slide; // 滑模增益 float ialpha, ibeta; // 采样电流 float valpha, vbeta; // 给定电压，或经过重构的相电压 float ihat_alpha, ihat_beta; // 估计电流 float ealpha, ebeta; // 估计反电动势 } SMOParams; typedef struct { SMOParams p; float err_alpha, err_beta; float z_alpha, z_beta; } SMOHandle; void SMO_Init(SMOHandle *smo, float Ts, float Rs, float Ls) { smo->p.Ts = Ts; smo->p.Rs = Rs; smo->p.Ls = Ls; smo->p.lpf_w = 2000.0f; // 初始2000rad/s，具体看噪声 smo->p.K_slide = 5.0f; // 初始值，必须整定 smo->p.ihat_alpha = 0.0f; smo->p.ihat_beta = 0.0f; smo->p.ealpha = 0.0f; smo->p.ebeta = 0.0f; smo->err_alpha = 0.0f; smo->err_beta = 0.0f; smo->z_alpha = 0.0f; smo->z_beta = 0.0f; } void SMO_Update(SMOHandle *smo) { float Ts = smo->p.Ts; float Rs = smo->p.Rs; float Ls = smo->p.Ls; float dt_L = Ts / Ls; // 电流误差 smo->err_alpha = smo->p.ialpha - smo->p.ihat_alpha; smo->err_beta = smo->p.ibeta - smo->p.ihat_beta; // 滑模切换项 smo->z_alpha = (smo->err_alpha > 0.0f) ? smo->p.K_slide : -smo->p.K_slide; smo->z_beta = (smo->err_beta > 0.0f) ? smo->p.K_slide : -smo->p.K_slide; // 电流估计更新 smo->p.ihat_alpha += dt_L * (smo->p.valpha - Rs * smo->p.ihat_alpha - smo->z_alpha); smo->p.ihat_beta += dt_L * (smo->p.vbeta - Rs * smo->p.ihat_beta - smo->z_beta); // 低通滤波，提取反电动势等效值 smo->p.ealpha += Ts * smo->p.lpf_w * (smo->z_alpha - smo->p.ealpha); smo->p.ebeta += Ts * smo->p.lpf_w * (smo->z_beta - smo->p.ebeta); }

注意几个细节。第一，输入的 valpha/vbeta 建议用SVPWM重构后的相电压（用母线电压和占空比反推），不要用给定电压，因为占空比受限后给定和实际偏差很大。第二，滑模增益 K_slide 必须大过该转速下的反电动势幅值，否则滑模面到不了，电流误差永远收敛不了。第三，低通滤波的截止频率不能太低，否则相位滞后太严重，角度会明显偏后。这些参数后面专门讲。

3.2 PLL角度跟踪代码

拿到了 eα、eβ 之后，角度估算有两种选择。简单粗暴的是atan2：

theta_est = atan2f(ebeta, ealpha);

但实际跑下来，这个角度在谐波大的电机上抖得厉害，速度环很容易跟着抖。我更推荐PLL，代码也不复杂：

typedef struct { float Ts; float theta; // 输出电角度 float omega; // 输出电角速度 float kp; // PLL比例增益 float ki; // PLL积分增益 } PLLParams; typedef struct { PLLParams p; float integral; float err; } PLLHandle; void PLL_Init(PLLHandle *pll, float Ts, float bandwidth) { pll->p.Ts = Ts; pll->p.theta = 0.0f; pll->p.omega = 0.0f; // 带宽选10~50Hz，按阻尼比1整定 pll->p.kp = 2.0f * bandwidth; pll->p.ki = bandwidth * bandwidth; pll->integral = 0.0f; } void PLL_Update(PLLHandle *pll, float ealpha, float ebeta) { // 用atan2得到粗略角度作为PLL输入 float theta_raw = atan2f(ebeta, ealpha); // 角度差，注意wrap到[-pi, pi) float err = theta_raw - pll->p.theta; err = fmodf(err + 3.14159265f, 6.28318530f); if (err > 3.14159265f) { err -= 6.28318530f; } else if (err < -3.14159265f) { err += 6.28318530f; } pll->p.err = err; pll->integral += pll->p.ki * err * pll->p.Ts; pll->p.omega = pll->p.kp * err + pll->integral; pll->p.theta += pll->p.omega * pll->p.Ts; // 角度归一化 pll->p.theta = fmodf(pll->p.theta, 6.28318530f); if (pll->p.theta < 0.0f) { pll->p.theta += 6.28318530f; } }

PLL的带宽选择很关键。带宽高了，响应快但谐波也进得来；带宽低了，角度平滑但动态响应差，负载突变时会滞后。我一般先把带宽设在中间值，再根据实际带载表现调整，原则是“能稳就不追快”。

3.3 启动状态机代码

三段式启动的工程实现最好用状态机，别用一堆if散写。核心状态定义如下：

typedef enum { MOTOR_IDLE = 0, MOTOR_ALIGN, MOTOR_OPENLOOP_RAMP, MOTOR_CLOSEDLOOP_RUN, MOTOR_FAULT } MotorState; typedef struct { MotorState state; float theta_openloop; // 开环给定角度 float speed_elec; // 当前电频率，rad/s float ramp_accel; // 开环斜坡加速度 float align_current; // 预定位电流 float run_current; // 强拖电流 float switch_speed; // 切换闭环对应的电频率 uint32_t angle_ok_cnt; // 角度差持续时间计数器 } StartupSM;

状态机的转移条件我写成了这样：

void Startup_Tick(StartupSM *sm, float theta_smo, float speed_smo, float current_ref) { switch (sm->state) { case MOTOR_ALIGN: // 保持固定角度，电流闭环 sm->theta_openloop = 0.0f; current_ref = sm->align_current; if (align_time_done()) { sm->state = MOTOR_OPENLOOP_RAMP; sm->speed_elec = 2.0f * 6.28318530f; // 起始2Hz } break; case MOTOR_OPENLOOP_RAMP: // 角度递增，等效于频率斜坡 sm->speed_elec += sm->ramp_accel * CONTROL_PERIOD; sm->theta_openloop += sm->speed_elec * CONTROL_PERIOD; current_ref = sm->run_current; // 切换条件：转速够 + 角度差小 if (sm->speed_elec >= sm->switch_speed) { float angle_diff = angle_wrap(theta_smo - sm->theta_openloop); if (fabsf(angle_diff) < 0.087f) { // 5度 sm->angle_ok_cnt++; if (sm->angle_ok_cnt > 50) { sm->state = MOTOR_CLOSEDLOOP_RUN; // 让PLL角度从开环角度接管，避免跳变 pll_set_theta(sm->theta_openloop); } } else { sm->angle_ok_cnt = 0; } } break; case MOTOR_CLOSEDLOOP_RUN: // 使用SMO+PLL的角度闭环 current_ref = speed_pi_output(speed_smo); break; default: break; } }

代码里的角度wrap函数，我习惯手写而不是依赖现成库，因为很多平台的标准库在角度归一化上性能参差不齐，而且角度wrap的边界条件容易踩坑，写清楚比调库放心。

4. PMSM和BLDC的适配差异：反电动势波形、电机参数、低速表现

4.1 反电动势波形对SMO输出的影响

PMSM的反电动势是正弦波，SMO提取出来的eα/eβ非常干净，PLL跑得很舒服。BLDC就不一样了，它的反电动势是梯形波，含有大量3次、5次、7次谐波。这些谐波会直接混进eα/eβ，导致估算角度带纹波，电流波形跟着失真。

我用一套BLDC电机实测，带载时如果不处理谐波，电流波形能看到明显的6倍频脉动（相对电周期），转速越高越明显。解决思路不是换算法，而是让PLL带宽远离谐波频率。梯形波反电动势的谐波主要以3次为主，PLL带宽取10到20Hz，压制效果就很明显了。代价是动态响应变慢，但对大部分BLDC应用足够。

如果谐波实在太离谱，我会在SMO低通滤波后面再叠一个针对6倍频的陷波器。这个陷波器只在BLDC上启用，PMSM不需要，因为加了反而增相位滞后。

4.2 从电机铭牌到PI参数：那几个换算公式

SMO和电流环都需要电机的R和L。这个参数测不准，后面的整定全部白搭。用万用表量相电阻，再用LCR表在1kHz下量相电感，这是最普通的做法。但我提醒一句，电机在不同转速和负载下，L的饱和程度不一样，最好在额定条件下复测一次。

电流环PI参数不是拍脑袋写的。电流环被控对象是电阻和电感，带宽设1000到2000rad/s比较合理，然后按下面算：

Kp_i = L * bw Ki_i = R * bw

其中bw是电流环带宽。这个公式是连续域理想的，离散化时还要乘周期和做积分限幅，但只要从这套值起步，基本都能稳定。速度环带宽通常只有电流环的1/10到1/5，我再按幅值裕度调一下，效果就很扎实了。

表贴式PMSM的Ld和Lq差别不大，一套标量电感就能跑。内置式PMSM的Ld远小于Lq，反电动势方程里还藏着磁阻转矩项，这套SMO直接用会偏，需要加磁阻补偿或换高阶观测器。这也是我说的“适用电机的边界”所在。

4.3 为什么低速阶段只能靠开环强拖

SMO的低速表现不好，原因很物理。反电动势幅值正比于转速，转速低于某一阈值时，eα、eβ的幅值可能只有几百毫伏，而电流采样噪声、PWM开关噪声、死区产生的电压误差，量级和它差不多。信号被噪声淹没，观测器自然没法用。

这就是为什么我再强调三段式启动：低速段不用观测器，直接用开环强拖，把速度拉到信号信噪比能够接受的范围，再由观测器接管。这样做虽然启动阶段没有角度反馈，扛负载能力弱一些，但胜在稳定、可预测。真要低速带重载启动，那得换高频注入方案，成本和复杂度和这套代码不在一个数量级。

5. 实测踩过的坑：振荡、失步、过流的完整排查链路

5.1 滑模增益K的工程整定方法

SMO的K_slide是最容易让人困惑的参数。调小了，电流误差收敛不了，进入闭环后电流振荡；调大了，低通滤波前面的切换信号过强，反电动势估算值中叠加了明显的开关纹波。

我现在的整定路线是这样的：先在开环强拖阶段让SMO跑起来，但不用它的角度。把K_slide从0开始往上加，同时看第五个变量——估算反电动势的幅值。当eα/eβ的波形开始变成干净的正弦波且幅值随转速线性增大时，K就基本到头了。再加一点余量，然后固定。

这个方法比盯着电流波形试K要快得多，尤其是BLDC，因为反电动势不干净很正常，关键是幅值随转速的线性关系要在。如果发现反电动势幅值和转速不成比例，先查电压重构准不准，别急着调K。

5.2 启动切换时角度突变导致的过流

我第一版代码切闭环时直接用了SMO的估算角度，结果每次切换瞬间电流都冲一下，严重的时候直接触发过流保护。测量下来，切换前开环角度和SMO角度差了大约8度，切换瞬间角度跳变8度，电流环的输出电压跟着突跳，电流自然冲。

修复方法就像上面状态机代码里写的，切闭环瞬间让PLL的角度先等于开环角度，再让PLL自然跟踪。实际跑下来，电流峰值下降了接近一半，切换过程基本看不到顿挫。这个经验后来被我复制到所有无感FOC项目里，属于“切换必做”的一步。

5.3 电流采样噪声和死区补偿对观测器的影响

还有一次调试，电机高速运转时观测器角度抖动明显，电流波形畸变，但低速却正常。排查下来问题出在电流采样：高速时PWM占空比变化快，采样点没对准PWM中心，采到的电流带了很大的开关纹波。把采样点对齐到PWM中心后，问题消失。

这个细节容易被忽略，因为代码层面看不出来，必须用示波器对照PWM波形和ADC采样时刻。另外一个直接影响观测器的变量是死区时间。死区会让实际输出电压偏离给定电压，偏差幅值不大但恰好影响反电动势的估算精度。中高速时影响小，低速启动段影响大，我建议上死区补偿，或者至少把死区长度限制在1微秒以内。

以下是我整理的排查顺序，对照这张表能少走很多弯路：

6. 从这套代码移植到新项目时，建议你重写的三个函数

6.1 Clarke变换和Park变换的定标方式

网上很多代码用的是Q15定点或厂商库的IQ格式。这套代码我全程用float，移植时最省心。但如果你的平台是定点MCU或者对性能压得很紧，需要把float换成定点，那Clarke变换里的缩放系数就必须重新设计。

我说一个实际经验：Clarke变换本身很简单，但不同代码的电流定标不一样，有的用安培满量程，有的用电流传感器满量程。移植时不改定标，电流环PI参数完全对不上，现象就是无论怎么调PI电机都啸叫。我的做法是定义一套统一的per-unit体系，即所有内部量都用标幺值，只在ADC采样和PWM输出处做物理量转换。这样电机参数变了，只需要改标幺化的基值，PI参数可以保持不变。

PWM输出端也有个坑。SVPWM的占空比输出要适配你具体MCU的定时器，每家的定时器分辨率、对齐方式、死区极性都不一样。我建议把SVPWM计算和底层PWM驱动拆成两个函数，上层算出来的三相占空比只用来生成中断比较值，不要在算法代码里混入芯片寄存器操作。

6.2 电流环PI的离散化差异

一次把一套代码从20kHz PWM的MCU移植到10kHz PWM的平台，结果电流环PI参数拿原来那套直接用，电流啸叫。问题出在离散化上。连续域PI参数一样，离散化后积分项和比例项展开出来的数字完全不是一个量级。

离散化方式不同，效果也不同。这里我用最稳妥的增量式PI，同时带积分抗饱和。核心思路是——积分项有限幅，输出也有限幅，防止启动大电流时积分windup。我见过很多初次调FOC的人，把PI输出限幅设在母线电压满量程，结果电流环过冲一路飞，实际上限幅要按SVPWM最大线性输出电压（约0.577倍母线电压）来设。

还要注意比例项和积分项的反馈方向，用电流反馈的控制回路如果符号搞反了，电机直接飞车。我每次换平台做的第一件事，就是人为给定一个很小的d轴电流，确认电流环的响应方向是对的，再去做后面的启动和闭环。

6.3 保护逻辑：堵转、限流、角度跳跃检测

无感FOC必须写保护，否则调试过程中炸机只是时间问题。至少要有三样：

一是堵转保护。开环强拖时，如果给定角度一直在走，但电流始终顶着限幅且速度没有起来，基本可以断定堵转或者负载卡死。这时要在几百毫秒内停机，别硬拉。实现方式就是在开环阶段检测速度是否跟随斜坡指令，偏差超过20%就报堵转。

二是角度跳跃检测。SMO跑飞时，eα/eβ的估算角度会出现非物理的跳变。连续N个周期内角度变化超过一个阈值，比如一个控制周期内电角度变化超过10度，直接判定观测器失效。这个判断在高速时特别重要，因为高速下角度变化本来就快，阈值要根据转速自适应。

三是硬件过流和母线过压保护，这个不用多说，但我想补充一点：FOC的过流保护不能只靠硬件比较器，还要在软件里做三相电流有效值检测，并带着方向关管。因为无感算法在过流瞬间可能正好在换相，直接封锁所有管子反而容易在感性负载上打高压，先顺着电流方向续流再停机，波形和寿命都会好很多。

调试无感FOC，我到现在还保留一个习惯：任何参数改动，先只小步调一个量，每调一次都记录电流波形和观测器输出。无感FOC的怪故障太多，很多时候不是算法问题，而是某个参数在不同工况下悄悄越过了边界。把这套代码移植到自己的板子上以后，建议你也从电流闭环开始测，然后是开环强拖，最后才是切闭环，一步步来，比一次怼到高速要靠谱得多。