简介这是一份面向电机控制与嵌入式开发者的FOC控制算法程序包提供单电压闭环与电压电流双闭环两套源码实现适用于STM32等单片机平台。压缩包共687个文件约27.76MB以C/C源码h、c、cpp为主同时包含Keil/IAR工程文件uvprojx、uvoptx、CubeMX配置ioc、编译产物hex、bin、axf及CMake构建脚本cmake、make便于直接导入工程学习或二次开发。内容覆盖FOC控制链路的关键模块如Clark/Park变换、SVPWM调制、PID调节及电流采样方案并区分有感与无感方式可对照源码理解电压环与电流环的代码实现和参数整定思路。已有677人学习下载适合具备一定单片机基础、希望深入掌握FOC工程化实现的开发者。1. FOC控制算法程序先分清单电压闭环与电压电流闭环同一个电机、同一条控制板、同一套功率管有的人一上电电流就是光滑的正弦跟着负载走有的人一上电就是锯齿波带一点负载就失步。差别往往不在功率级而在FOC控制算法程序里到底是只做了单电压闭环还是做了电压电流闭环。单电压闭环省掉电流传感器代价是电流冲击只能靠限幅硬接电压电流闭环把Id/Iq变成直接可控的量代价是电流采样、时序和更多PI带宽。下面按从坐标变换到SVPWM的顺序把两套闭环拆开讲适合正在从方波驱动往无感FOC迁移或者正在处理FOC堵转检测和转子初始位置检测的嵌入式工程师。2. FOC坐标变换与两种闭环的架构差异从Clarke到内环外环2.1 Clarke变换为什么只需要两相电流坐标基底与增益约定FOC控制算法程序的第一步是把三相静止坐标系下的物理量搬到两相静止坐标系αβ再搬到两相旋转坐标系dq。这个过程中最常见的疑问不是怎么算而是为什么三相电流只采了两个就够了——因为星形绕组中性点不引出时三相电流满足基尔霍夫电流定律给出任意两相就能推出第三相import numpy as np def clarke(ia, ib, icNone): if ic is None: ic -ia - ib # 三相平衡第三相由前两相推得 i_alpha ia # 等幅值约定α轴直接取A相电流 i_beta (ib - ic) / np.sqrt(3.0) return i_alpha, i_beta def park(i_alpha, i_beta, theta): # theta 是转子电角度由编码器或观测器给出 id i_alpha * np.cos(theta) i_beta * np.sin(theta) iq -i_alpha * np.sin(theta) i_beta * np.cos(theta) return id, iq这里的核心约定是“等幅值变换”。等幅值约定下输出电压/电流的幅值与真实相量一致调试时用示波器直接量相电流就能和Id/Iq对上而等功率约定会在矩阵前面多一个 sqrt(2/3) 系数。很多源码交流区里“同样的PI参数抄过来就跑飞”的案例最后查出是两套约定混用了。如果你手上代码的电流环输出和实际相电压之间差了一个固定比例系数优先查这一项而不是怀疑PI方向。2.2 单电压闭环电压矢量幅值闭环角度靠观测器补单电压闭环在无感FOC里最常见的形态是速度环PI直接输出Vq_ref交轴电压参考Vd_ref给零或按弱磁方向修正转子角度由反电动势观测器估算最后合成电压矢量输出PWM。它的“闭环”体现在速度环和角度环上电流环不存在所以电流冲击只能靠电压限幅间接兜底。这和开环VF驱动有本质区别。VF只控频率和幅值角度是积分出来的负载一变角度就漂单电压闭环的角度来自反电动势属于真正的转子位置反馈因此电压矢量方向一直是跟着转子走的。下表给出了两种结构的关键差异对比项单电压闭环电压电流双闭环电流采样不需要至少两相常见双分流或单分流被控量电压矢量幅值Id/Iq 电流分量堵转行为电流不可控迅速发热Iq_ref 饱和可检测可保护起动策略强制换向拉入同步需要初始位置检测再切入电流环典型场景风扇、低成本PMSM伺服、机器人关节、泵类需要说明的是单电压闭环同样能跑出正弦电流波形但它对负载突变的响应本质上还是电压源行为电感让电流滞后于电压堵转瞬间电压环会拼命往上顶电压直到限幅才停。这就是为什么很多FOC调试难点集中在“单电压闭环带载往下沉”这个问题上因为电流信息缺失你分不清是角度漂了还是电压给少了。2.3 电压电流闭环电流内环把电感效应隔离在外环之外当控制板加上分流电阻或霍尔电流传感器之后FOC控制算法程序就从“电压源驱动”升级成“电流源驱动”。电流内环的作用不是简单加一个PI而是把电机定子电感的积分环节掰直对外环而言被控对象从“R sL”变成接近1的惯性环节外环可以大胆地提高增益而不担心相位滞后。这个标题里的“电压电流闭环”工程上通常指外环是电压/速度环内环是电流环外环输入转速误差输出Iq_ref电流环输入Iq_ref与Id_ref输出Vd/Vq。和电源稳压里那个电压环不是一个东西读别人源码时先区分清楚。电流环的存在让堵转检测变成了纯软件判断——连续一段时间Iq_ref超过阈值、转速始终接近于零就可以直接判定堵转这比单电压闭环下看电压幅值要可靠得多。3. 单电压闭环源码实现SMO角度观测与电压矢量限幅3.1 反电动势滑模观测器角度从哪来无感FOC控制算法程序里角度是观测出来的不是测出来的。这里采用滑模观测器SMO做中高速段的角度估计核心思路是用电压方程估算αβ电流估算电流和实测电流的误差驱动滑模面收敛滑模开关函数的等效平均输出正好等于反电动势。// 滑模观测器输入αβ轴电压与电流输出反电动势与转子角度 // R_PHASE: 相电阻, L_PHASE: 相电感, K_SMO: 滑模增益 typedef struct { float i_alpha_hat, i_beta_hat; // 估算电流 float e_alpha, e_beta; // 反电动势估算值 float theta; // 电角度 } smo_t; void smo_run(smo_t *s, float v_alpha, float v_beta, float i_alpha, float i_beta, float Ts) { float sig_alpha (s-i_alpha_hat i_alpha) ? 1.0f : -1.0f; float sig_beta (s-i_beta_hat i_beta) ? 1.0f : -1.0f; // 电流观测器v - R*i - K*sign(i_hat - i) 除以 L 积分 s-i_alpha_hat (v_alpha - R_PHASE * s-i_alpha_hat - K_SMO * sig_alpha) * Ts / L_PHASE; s-i_beta_hat (v_beta - R_PHASE * s-i_beta_hat - K_SMO * sig_beta) * Ts / L_PHASE; // 开关信号经低通后得到反电动势截止频率 CUTOFF s-e_alpha (K_SMO * sig_alpha - s-e_alpha) * 2.0f * 3.14159f * CUTOFF * Ts; s-e_beta (K_SMO * sig_beta - s-e_beta) * 2.0f * 3.14159f * CUTOFF * Ts; // 反电动势矢量方向即转子磁极方向符号随坐标定义可能有偏移 s-theta atan2f(-s-e_alpha, s-e_beta); }这里的参数要按电机模型对号入座K_SMO取电机峰值反电动势的2到3倍太小会抖振太大则把PWM开关纹波直接带进角度CUTOFF低通截止频率取电流环频率的1/10左右留出速度带宽空间。这一段代码跑通后如果电机反转或者电流相位不对优先检查反电动势与角度之间的符号约定把atan2里的负号换一边再试。直接反正切给角度会有噪声纹波更稳妥的做法是再接一个PLL做角度跟踪// PLL角度跟踪把SMO给出的角度平滑成速度位置 float err atan2f(sinf(theta_smo - pll.theta), cosf(theta_smo - pll.theta)); pll.omega pll.Ki * err * Ts; // 积分器产生速度 pll.theta (pll.omega pll.Kp * err) * Ts; // 比例积分的位置输出PLL的Kp和Ki决定观测带宽一般把观测带宽设在200Hz上下阻尼取0.707。它输出的omega就是后面速度环反馈堵转检测里的“转速为零”也用它判断。3.2 速度PI输出Vq_ref与三相PWM合成单电压闭环的主循环固定节拍执行先跑SMO得到角度再算速度速度PI输出Vq_refVd_ref置零或按弱磁修正然后走逆Park和Clarke逆变换得到三相调制波。下面是三相PWM合成的标准写法// 电压矢量合成逆Park 零序分量注入等效SVPWM void pwm_set_voltage(float v_alpha, float v_beta, float vdc) { // Clarke逆变换αβ - 三相静止坐标 float va v_alpha; float vb -0.5f * v_alpha 0.8660254f * v_beta; float vc -0.5f * v_alpha - 0.8660254f * v_beta; // 注入零序分量让三相调制波对齐母线中点等效电压空间矢量 float v0 -(fmaxf(fmaxf(va, vb), vc) fminf(fminf(va, vb), vc)) * 0.5f; va v0; vb v0; vc v0; // 归一化到占空比0.5对应50%占空比 float duty_a 0.5f va / vdc; float duty_b 0.5f vb / vdc; float duty_c 0.5f vc / vdc; set_pwm_duty(duty_a, duty_b, duty_c); // 需要自行实现死区互补 }关键参数是母线电压归一化方式。对于星形绕组线性调制区最大相电压峰值是 Vdc/√3也就是占空比最大到0.5 1/√3/2 ≈ 0.789。如果速度PI饱和上限给大了电压矢量进入过调制区波形会削顶电流谐波增大角度观测器首当其冲受影响。单电压闭环下Vq_ref的限幅必须在速度PI里做建议直接限到Vdc/√3对应的标幺值不要留余量让PI自己去撞。3.3 单电压闭环的最小参数表参数起步值调试方向速度环Kp标幺值0.01~0.1负载突变掉速大就加大噪声大就减小速度环Ki标幺值0.1~1.0稳态误差消不掉就加大振荡就减小Vq输出限幅0.9 × Vdc/√3DCDC供电不足时还要降SMO的K_SMO2~3倍峰值反电动势低速抖振大就加大噪声大就减小observer低通电流环频率/10相位滞后太大就提高截止频率这套结构里最常出问题的是速度环方向。Iq为正时电机应该往正转但αβ电流方向、SMO角度符号、逆Park符号三个地方各错一次结果就是速度环正反馈——转速越高Vq越大。排查方法很简单把速度环断开给定一个固定的Vq正电压矢量看电机是否正向转动且电流是否受限逐级确定符号。4. 电压电流闭环源码实现电流采样、双PI与抗积分饱和4.1 电流采样重构双分流电阻与时序从单电压闭环升级到电压电流闭环第一件事是电流采样。三电阻采样最直观每个桥臂下管串一个采样电阻双分流电阻省一路成本利用三相电流之和为零重构第三相。采样窗口要避开死区和开关切换瞬间否则采到的是续流噪声。// PWM中心对齐模式计数器顶点触发ADC此时下管导通且电流稳定 // 双分流电阻采样A、B两相C相由 KCL 重构 void adc_isr(void) { float ia read_adc(CH_A); // 读A相电流 float ib read_adc(CH_B); // 读B相电流 float ic -ia - ib; // 重构C相 float i_alpha ia; float i_beta (ib - ic) / 1.7320508f; // 再经过Park变换得到id/iq交给电流环 }采样点位置对噪声影响很大占空比接近0或1时某相下管导通时间太短采样窗口不够这时候要根据占空比把重构相切换成另外两相。常见做法是在每相占空比小于0.05或大于0.95时放弃直接采样该相用剩余两相重构。GitHub上能搜到的FOC控制算法程序里单电阻重构的代码最多但稳定性最差双分流是性价比最高的方案电流环跑到8kHz没问题。4.2 电流内环带解耦与抗饱和实现电流内环的PI输出是Vd_ref和Vq_ref。如果只做两个独立PI高速时d轴和q轴会互相耦合——Iq变化会引起Id扰动。所以要在电流环输出处加解耦补偿并把限幅放在合成矢量上而不是分别对Vd、Vq限幅// 电流环d/q轴PI 交叉解耦 合成矢量限幅 // LD、LQd/q轴电感PSI_FLUX永磁体磁链OMEGA_E电角速度 void current_loop(float id_ref, float iq_ref, float id_fb, float iq_fb, float theta_elec, float vdc, float Ts) { float ud_ff -OMEGA_E * LQ * iq_fb; // d轴解耦项 float uq_ff OMEGA_E * (LD * id_fb PSI_FLUX); // q轴解耦项 反电动势前馈 float id_err id_ref - id_fb; float iq_err iq_ref - iq_fb; float vd KP_I * id_err id_integ ud_ff; float vq KP_I * iq_err iq_integ uq_ff; // 合成矢量限幅先等比缩放再积分 float vmax vdc / 1.7320508f; // 线性调制区上限 float mag sqrtf(vd * vd vq * vq); if (mag vmax) { vd * vmax / mag; vq * vmax / mag; } id_integ KI_I * id_err * Ts; iq_integ KI_I * iq_err * Ts; // 逆Park后交给pwm_set_voltage float v_alpha vd * cosf(theta_elec) - vq * sinf(theta_elec); float v_beta vd * sinf(theta_elec) vq * cosf(theta_elec); pwm_set_voltage(v_alpha, v_beta, vdc); }这里最容易让人栽跟头的就是抗饱和的处理位置。把Vd、Vq分别限幅的做法会让电压矢量方向弯曲本来只需要饱和的弱磁方向可能被限到错误的电磁转矩方向。合成矢量一起等比缩放能保证角度不偏之后再计算PI积分项就不会出现“积分已经飞出天际退饱和却迟迟回不来”的问题。4.3 电流环、速度环、位置环的带宽如何配双闭环不是两个PI随便级联就完事。电流环采样频率一般是8kHz到20kHz对应62.5us到125us的周期——很多支持CIA402协议的伺服运动控制库EtherCAT同步周期就压在125us底下的电流环正是按这个节奏跑的。电流环带宽做到采样频率的1/10到1/20速度环带宽再取电流环的1/5到1/10位置环在上层只给几十赫兹带宽就够。环典型周期典型带宽电流环内环62.5us ~ 125us500Hz ~ 1kHz速度环外环250us ~ 500us20Hz ~ 100Hz位置环最外层1ms ~ 4ms5Hz ~ 20Hz整定顺序也有讲究先把电流环PI单独整定到阶跃响应无超调再挂速度环。判断电流环是否合格的土办法是给一个Iq_ref阶跃看电流响应有没有紧随上升、有没有大的振荡尾巴。速度环带宽设得比电流环低一个数量级以上时双闭环系统的相位裕度通常不会有大问题。5. FOC调试难点与进阶初始位置检测、堵转检测与弱磁5.1 无感FOC转子初始位置检测高频注入从静止起动是无感FOC的一个关键门槛因为反电动势为零SMO没有任何输入。最常用的办法是脉振高频注入在估计的d轴上注入一个高频正弦电压然后解调q轴电流响应因为d/q轴电感不相等凸极效应q轴高频电流幅值正比于sin(2Δθ)把误差信号低通后交给一个PI就能把估计角度拉向真实角度。需要注意表贴式电机Ld与Lq接近这个方法响应很弱判断磁极极性还需要对比非线性饱和效应否则可能差180度电角度反转这是FOC控制算法程序从静止第一次上电时不转、反而“嗡”一声卡住的典型原因。5.2 堵转检测速度与电流指令的双重条件有了电流环之后堵转检测变成了速度环和电流环联合判断。下面这个函数放在固定中断里调用// 堵转检测转速接近0 电流指令持续超限 // speed_est 来自SMO输出的PLL速度观测单位rad/s // iq_ref 为电流环参考值标幺化到额定电流 void stall_monitor(float speed_est, float iq_ref, float ts) { float speed_threshold 1.0f; // 约0.16转/秒 float current_threshold 1.5f; // 1.5倍额定电流 float stall_time 0.2f; // 持续200ms才确认 if (fabsf(speed_est) speed_threshold fabsf(iq_ref) current_threshold) { stall_count ts; if (stall_count stall_time) { pwm_enable(false); // 先切断输出 stall_count 0; // 复位等待上层策略 // 常见策略反向给一个短脉冲越过卡死位置再重新起动 } } else { stall_count 0; } }判断堵转的关键是“双重条件”而不是仅看电流。正常起动瞬间电流也会冲到1.5倍以上但转速在上升不能用单一条件误触发。时间阈值要覆盖最大起动时间我一般取起动时间的2倍以上。单电压闭环没有电流反馈时可以用速度环输出的Vq_ref替代iq_ref做同样判断只是灵敏度会低——“电压高转速零”这种情况说明反电动势为零要么堵转要么角度观测失效两种原因都需要停机处理。5.3 弱磁电压环饱和之后的Id_ref调整无感FOC高速运行到电压极限时电流环输出的电压矢量会持续顶在Udc/√3限幅上q轴电流给不进去。这时候要主动把Id_ref往负方向拉用直轴去磁分量释放电压空间给交轴。常见的做法是在电流环之外加一个弱磁调节器实时检测电压矢量幅值与限幅值的差误差超过一定时间就给Id_ref一个负向增量退出弱磁时用斜坡把Id_ref拉回零或MTPA曲线不要直接跳变。弱磁调节得当的标志是母线电压不泵升、电流环不振荡同时转速还能继续往上顶一点——如果母线电压开始往上爬说明能量在回灌弱磁给得太激进需要回退。本文还有配套的精品资源点击获取