简介一份面向电机控制与嵌入式开发者的FOC矢量控制驱动器工程源码包完整给出位置环、速度环、电流环三环控制实现。电流环采用基于带宽的PI参数整定速度环支持刚性等级调节位置环仅需P参数即可完成跟随整体控制策略简洁调制部分使用基于零序注入的SPWM等效SVPWM并包含零点电角度识别算法便于实际电机应用中准确获取转子位置。工程基于STM32G4系列构建配齐HAL驱动、数学库与链接脚本可帮助理解从三相电流采样到占空比输出的完整链路。压缩包共1338个文件以C源码689个、头文件287个为主另含汇编、工程配置、库文件及说明文档等整体约39.46MB目录结构适合对照学习。已有1881人学习下载适合需要快速搭建FOC驱动器原型、调试三环参数或深入理解调制与电角度识别细节的开发者参考。1. 手搓FOC驱动器三环控制怎么从「能转」练到「转得准」FOC磁场定向控制堪称电机控制绕不开的方案现成驱动器很多但自己手搓一套三环FOC时最卡人的往往不是原理而是参数怎么给、调制器怎么简化、零点角度哪来。这套方案以位置环、速度环、电流环为骨架电流环PI按带宽直接算速度环按刚性等级选带宽位置环只剩一个P参数SVPWM改用基于零序注入的SPWM实现母线利用率与SVPWM等价再配合零点电角度识别把电机从“通电抖一下”调到“指哪打哪”。适合想从零搭驱动器的开发者、电机控制入门者也适合被现成方案参数黑匣子折磨过的人。不用先啃完推导把这几组公式落到代码里就能跑通。2. 三环控制架构与参数整定电流环算带宽速度环分刚性位置环保留P2.1 三环嵌套结构为什么电流环必须放最内层先把三环对应的物理量说清楚。电流环控制的是定子绕组里的d/q轴电流速度环控制转子转速位置环控制转子角位置。三条回路时间尺度差得很开电流环响应几个毫秒速度环几十到几百毫秒位置环往往到秒级。这种时间尺度分离决定了嵌套顺序——最快的那一环放最里层再一层层向外包。反过来想如果位置环在最里层、电流环在最外层位置环每次调整直接改变电压指令而绕组电感导致的电流滞后又叠加在位置上整个回路相位余量根本不够。所以标准做法永远是电流环最内速度环中间位置环最外。这个顺序不是偏好是稳定性要求。嵌套之后还要给每层分配带宽。工程上我习惯按「电流环带宽 ≈ 10 ~ 20 × 速度环带宽 ≈ 100 倍 × 位置环带宽」来规划。比如电流环带宽取2000 rad/s速度环就取80~200 rad/s位置环取10~20 rad/s。带宽留出级差后内环的动态对外环来说基本可以当成理想环节外环设计就不必照顾高频细节。控制频率上电流环一般跑PWM周期的控制中断比如10~20kHz速度环放在同一个中断里也不算错但如果想干净一点可以把速度环和位置环降到2~5kHz。很多简单实现三环共用一个中断只要保证速度环计算量小滤波PI限幅几十行代码就够一个中断里全跑完完全可行。注意速度反馈的来源。增量式编码器直接微分会有很大的量化噪声我会在进入速度环之前先做一阶低通截止频率放在速度环带宽的2~4倍。这样既削掉了微分毛刺又不至于把速度环的相位拖垮。2.2 电流环PI参数把带宽换成Kp和Ki就这么简单电流环控制对象是电机的电阻和电感在旋转坐标系下d轴和q轴电流满足一阶微分方程近似反电动势项和交叉耦合项先用前馈或直接忽略剩下的就是RL串联负载。PI控制器足以压住剩余扰动。给PI设定目标电流环带宽ω_c物理含义是电流闭环后频率低于ω_c的信号能基本无衰减通过。按极点对消法计算Kp和KiKp ω_c × L单位 V/A Ki ω_c × R单位 V/(A·s)公式里L是相电感R是相电阻ω_c单位rad/s。Kp、Ki这两个参数把PI的零点放在s -R/L位置正好抵消电机的电气极点闭环传递函数近似为一阶惯性环节带宽就是ω_c。这个整定方式最友好的地方在于不需要在线整定算法只需要三个数——目标带宽、电阻、电感。电阻可以用万用表量相线间电阻再折算电感用电桥测没有电桥的话堵转电机状态下注入一个方波电压观察电流变化率L ≈ V / (di/dt)。一个常见误区是Kp、Ki脱开带宽单独调。看到电流响应慢就加Kp结果电流纹波变大、电机噪音刺耳看到静态误差大就加Ki结果积分项把系统推向振荡。用公式算出来后所有改动都有明确方向想要更快升ω_c想要更稳降ω_c。电流环还要做输出限幅。逆变器能输出的电压上限是母线电压的一半线性区PI输出超过这个范围就进入饱和。饱和之后积分器会继续积分造成积分饱和——给定降下来时电流还要冲一段。我一般把积分累积单独限在额定电压的80%输出限幅的90%并且输出接近限幅时暂停积分更新。代码示意typedef struct { float kp; float ki; float integral; float out_lim; /* 输出电压限幅 */ float int_lim; /* 积分限幅 */ float dt; } pi_controller; void current_pi_update(pi_controller *pi, float err, float *out) { pi-integral pi-ki * err * pi-dt; CLAMP(pi-integral, -pi-int_lim, pi-int_lim); *out pi-kp * err pi-integral; CLAMP(*out, -pi-out_lim, pi-out_lim); if ((*out pi-out_lim err 0) || (*out -pi-out_lim err 0)) { /* 输出饱和时不累积积分防止积分饱和 */ pi-integral - pi-ki * err * pi-dt; } }这里有两个限幅out_lim是最终电压限制int_lim是积分稳态值限制。饱和后回退积分这一步很关键否则位置环或速度环里的给定突变会把电流环拖进漫长的饱和恢复期整体动作变得迟钝。参数示例一台50W小型伺服相电阻约2Ω相电感约2mH母线24VPWM 20kHz取ω_c 2000 rad/s则Kp 4 V/AKi 4000 V/(A·s)带进代码就能工作台架上微调范围一般在±20%以内。2.3 速度环刚性等级到PI带宽的映射速度环的整定我不用复杂公式而是先定义“刚性等级”也就是目标速度环带宽ω_v。刚性等级直观描述电机抵抗负载扰动的能力等级越高负载突变时转速跌落越小、恢复越快但机械共振被激励的风险也升高。几个常用等级的带宽参考等级1约10 rad/s适合大惯量、结构刚性差的负载等级2约20 rad/s适合一般皮带或齿轮传动等级3约40 rad/s适合直连小惯量等级4约80 rad/s适合高动态关节等级5约120 rad/s通常需要机械结构很结实否则会听到底座嗡嗡响。定了ω_v之后速度环PI用对称最优法折算Kp_v J_total × ω_v Ki_v Kp_v × ω_v / 4J_total是折算到电机轴的转动惯量kg·m²。Ki除以4而不是等于ω_v是为了让速度环在ω_v处保留至少45°相位裕度防止速度信号噪声和电流环残余相位滞后一起把系统拖入振荡。刚性等级速度环带宽 ω_v (rad/s)适用场景110大惯量转台机械共振明显220一般传动轴皮带或齿轮340直连小惯量要求响应快480高动态机械臂关节5120尽量少用抖动风险高转动惯量怎么估最省事是查电机手册的转子惯量但这通常是空载值。带负载时我会做一个加减速辨识让速度环开环直接给一个固定力矩指令T让电机从静止开始加速记录角加速度aJ T / a。测量几十毫秒数据就能得到一个能用的J注意别在大惯量负载上加速太久否则速度会飞起来。速度环输出是电流给定所以输出限幅直接就是最大允许电流。如果电流环额定是5A速度环输出限幅就设±5A。但这里有个细节位置环输出是速度给定速度环输出是电流给定两层限幅都别放得太宽否则一旦外环出错内环会顶着极限电流跑容易烧驱动。速度环PI的伪代码如下注意速度环不一定需要积分单独限幅但必须有输出限幅void speed_pi_update(pi_controller *pi, float speed_ref, float speed_fb, float *iq_ref, float max_iq) { float err speed_ref - speed_fb; /* 斜坡限速在调用侧完成假设传入的ref已限过斜坡 */ pi-integral pi-ki * err * pi-dt; CLAMP(pi-integral, -max_iq * 0.8f, max_iq * 0.8f); *iq_ref pi-kp * err pi-integral; CLAMP(*iq_ref, -max_iq, max_iq); }速度给定必须做斜坡限幅。给定位从0跳到额定转速时如果不限斜坡速度环PI的输出直接顶到电流限幅电机急加速机械冲击大。我习惯让0到额定转速的斜坡时间为100~300ms具体跟负载惯量匹配。2.4 位置环只留P为什么积分在这里是负资产位置环反应很慢它的输出是速度给定。位置误差本身是角度差积分位置误差会产生一个持续增大的速度指令修正看似能消除静态误差实际在伺服系统里代价很大到位后过冲回摆形成低频爬行负载特性不稳定时还会反复震荡。纯P控制就是一个比例弹簧speed_ref Kp_p × (θ_ref − θ_fb)Kp_p的单位是1/s速度/位置数值上直接取位置环的目标带宽ω_p即Kp_p ω_p。这个P参数决定了电机抵抗位置扰动的刚度越大越硬越大也越容易让速度环饱和。我通常取ω_p ω_v / 5 ~ ω_v / 10确保位置环动态比速度环慢一个数量级以上。纯P的代价是静态误差。带恒定负载时位置环必须持续输出速度给定而速度给定由误差产生所以最终会停在一个偏离目标的位置。想要消除这个误差正确做法是加前馈而不是加积分speed_ref Kp_p × err Kff × dθ_ref/dt前馈增益Kff取0.8~1.0它不改变闭环稳定性只负责让给定在跟随阶段预先产生速度指令把位置环的“追赶”职责减掉。跟随误差能从纯P的几十毫弧度降到毫弧度级别。位置环还有两个实际细节一是位置反馈来自编码器累计值要注意多圈溢出二是位置给定如果突变同样要做斜坡或S曲线整形否则速度环在启动瞬间会被打到限幅造成机械冲击。位置环输出限幅在初调时先设成额定转速的30%~50%确认无误再放开。到这里三环参数的整定思路已经完整电流环用RL模型和带宽公式速度环用刚性等级到带宽再到PI位置环用纯P加前馈。剩下的难点就转移到调制器和零点识别上。3. SVPWM的工程简化基于零序注入的SPWM实现3.1 传统SVPWM哪一步最费劲传统电压空间矢量PWM要做的事根据αβ轴电压判断当前落在六个扇区中的哪一个查表计算两个相邻非零矢量以及零矢量的作用时间再把作用时间换算成三相占空比。代码不算多但每一步都有边界条件——扇区边界、过调制、矢量切换顺序任何一个细节错了输出的相电压就会在某个扇区交界处畸变。而基于零序注入的SPWM直接从三相调制波出发把反Park变换得到的αβ电压转换回三相正弦相电压然后叠加一个零序分量u0让三相调制波同时抬高或降低相同量。因为零序分量在三相之间不产生线电压差电机看到的相电压波形完全不受影响但调制波整体幅值被压低母线电压利用率被提升到与SVPWM相同的1.1547倍。为什么能代替SVPWM因为SVPWM的实际输出电压特性与「正弦调制波三倍频零序分量注入」是等效的。零序注入不需要判断扇区不需要计算矢量作用时间只要在每个PWM周期取当前三相电压的最大值、最小值算出一个零序量加回去剩下交给比较器输出占空比实现复杂度大幅下降。3.2 零序注入公式与C代码十几行算出接近SVPWM的效果先走一遍信号流。反Park变换输出的v_alpha、v_beta范围是±Udc/2线性区。要让PWM比较器用它们需要三步反Clarke变换把v_alpha、v_beta变成三相相电压va、vb、vc计算零序分量u0 -0.5 × (v_max v_min)三相调制波各自加上u0除以半母线电压得到0~1范围的占空比。第二步为什么要用maxmin的均值因为三相电压在当前时刻的中心点偏离载波中点取(maxmin)/2就是中心偏移量注入负的偏移量可把中心拉回0位置调制波被对称压到最小峰值同时线电压差保持不变。C实现void spwm_with_zero_injection(float v_alpha, float v_beta, float udc_half, float *d_a, float *d_b, float *d_c) { /* 反Clarke等幅值变换系数已包含在前级 */ float va v_alpha; float vb -0.5f * v_alpha 0.866025f * v_beta; float vc -0.5f * v_alpha - 0.866025f * v_beta; float vmax fmaxf(fmaxf(va, vb), vc); float vmin fminf(fminf(va, vb), vc); float zero -0.5f * (vmax vmin); /* 加零序量得到调制波除以半母线标定到0~1 */ *d_a (va zero) / udc_half 0.5f; *d_b (vb zero) / udc_half 0.5f; *d_c (vc zero) / udc_half 0.5f; }代码里的v_alpha、v_beta范围假设是±udc_half。如果前级已经是0~Udc标定最后那个0.5f要去掉否则把占空比凭空抬了半格输出会恒定偏置。zero就是零序注入量注入后调制波范围会被限制在udc_half以内线性调制区不削顶。反Clarke变换里的0.866025f是√3/2这是等幅值形式如果前级用的是功率不变变换系数会不一样两种体系不能混用这是常见的翻车点。调制结果对比低调制比电压峰值小于0.5Udc时零序分量很小调制波接近正弦高调制比时调制波明显变成马鞍形峰值被压低、谷底被抬高。用示波器看滤波后的相电压能看到马鞍形就说明零序注入生效。如果看到平顶削波说明调制波超出载波范围进入过调制区低速时一般避免进入。3.3 载波频率、死区时间与采样时刻的配合零序注入只是调制算法部分PWM外设的参数设置同样影响最终波形。载波频率也就是PWM开关频率决定电流环带宽上限和电流纹波。我的参考值小功率MOS驱动的伺服电机20kHz是甜点噪音低、动态够低电感无框电机或空心杯电机建议50kHz以上否则电流纹波偏大大功率IGBT驱动10kHz以下死区要放大电流环带宽相应收紧。载波频率与电流环带宽要留至少10倍间隔比如20kHz载波电流环带宽取到2000rad/s已经接近极限再高就会看到采样延迟带来的相位裕度不足。死区时间不管用哪种调制桥臂互补PWM都要插死区。死区太小会桥臂直通烧管子太大则电流过零畸变明显、低速转矩脉动大。我一般按开关器件关断时间的2~3倍设高速MOS大概0.5~1μsIGBT要2μs以上。低速抖动时先别急着做死区补偿把死区时间确认到最小安全值再观察最常见问题是死区设得过大波形畸变全来自这里。采样时刻电流采样要避开死区边沿和开关噪声。中心对齐PWM里载波顶点是上下桥臂都稳定的窗口把ADC触发放在这一点采样到的电流最干净。同时ADC采样值用两次采样取平均可以减少一个PWM周期内的开关纹波影响。以下关键绑定用通用定时器描述/* 中心对齐PWM 载波顶点采样 */ /* ARR PWM_PERIOD - 1 决定开关频率 */ /* CCRx 比较值由占空比更新 */ /* 使能中央对齐模式1ADC触发源选定时器更新事件 */占空比在更新中断里写入ADC在三角载波顶点自动触发。两者不同步会造成电流反馈滞后半个甚至一个PWM周期让电流环整定结果与理论带宽对不上。碰到“Kp按公式算但实际电流发散”的怪问题先检查采样是不是跟着载波走。4. 零点电角度识别让每份力矩都作用在正确方向4.1 电角度的构成为什么必须实测磁场定向控制最核心的是要知道转子磁极方向。电角度θe与机械角度θm的关系θe θm × 极对数 θoffset。θm由编码器读取极对数在铭牌上能查唯独θoffset不能凭铭牌确定——它取决于编码器安装相位、电机绕组轴线、磁极初始位置之间的关系。同一型号两台电机θoffset可以差出几十度电角度。如果θoffset不对Iq给定的一部分会投影到d轴导致同样电流产生的力矩下降严重时电机抖动、过热。更头疼的是这个误差在低速时表现不明显一旦带载或高速运行就被放大容易误判为电流环参数问题。4.2 定子磁场对齐法最可靠的电零点标定常用的标定流程叫转子对齐不给编码器反馈直接用程序给一个固定电角度0把Iq设为一个小电流Id设为0让定子磁场把转子拉到一个确定位置。此时转子磁极方向必然与定子磁场方向对齐记录下编码器值这个值就是电角度0对应的位置。实现步骤电流环开环给定θe0Iq10%~20%额定电流等待0.5~1秒让转子稳定停住读取编码器原始位置作为θoffset后续所有电机控制都减掉这个offset。uint32_t encoder_zero_offset; void align_rotor_zero(void) { set_iq_ref(0.2f * rated_current); /* 摩擦负载偏大时可加到0.3f */ set_id_ref(0.0f); set_theta_elec(0.0f); /* 固定电角度0 */ delay_ms(800); /* 等机械振荡衰减 */ encoder_zero_offset encoder_read_raw(); encoder_clear_count(); }参数说明对齐电流超过额定电流30%要谨慎长时间通电绕组会发热。等待时间给足转子机械振荡周期可能到几百毫秒只等200ms可能转子还没停稳记录的offset自带偏差。对齐后把编码器计数清零后续直接用累加值算机械角就不必每次减offset。清零这个动作不要在运动过程中做会破坏位置环累计值。4.3 带载启动开环强拖与闭环切换对齐法在负载为摩擦类负载时能用但如果负载是一个方向不确定的重力负载转子不一定被定子磁场拉到理想位置。这时就要上开环强拖程序生成旋转电角度以斜坡方式把频率从0拉起来同时给恒定Iq让电机像开环步进电机一样被拖着转。流程设定目标电频率和加速度每个控制周期累加电角度θe 电频率 × dt写入电流环Iq给额定电流30%~50%Id给0拖住电机转速到达预设值后读取编码器反馈角度与程序角度求相位差修正θoffset切换为闭环。void open_loop_start(float target_freq, float accel) { float freq 0.0f; float theta 0.0f; while (freq target_freq) { freq fminf(freq accel * dt, target_freq); theta add_angle(theta, freq * dt); set_theta_elec(theta); set_iq_ref(0.4f * rated_current); delay_ms(dt_ms); } }add_angle要处理角度回绕到2π的问题否则长时间强拖后浮点误差会累积出可见的相位偏斜。强拖过程中最容易踩的坑是失步加速度设得太大转子跟不上旋转磁场实际转速与程序频率脱开表现为电流突然冲高、噪声变调。判断失步的标准很简单记录强拖结束时的编码器角度连续变化说明跟上了长时间在某个区间来回就是失步。切换闭环的时机也很重要。转速太低时反电动势信号弱闭环初始误差大转速太高时负载离心力大切换瞬间冲击明显。我一般控制在额定转速10%~20%之间切换切换前先让Iq回落到空载水平让转子自由转一会儿再接入闭环PI。提示其他零点识别方法比如检测反电动势过零点原则上免去对齐但低速时反电动势幅值太小实现复杂度和可靠性都不如对齐法。磁编码器带零点标记时可以在装配时对齐但机械公差依然存在我宁愿让程序先跑一遍对齐。5. 手搓FOC的常见问题排查波形畸变、参数发散与启动失败5.1 电流波形毛刺大PI输出抖成锯齿现象Iq反馈值高频抖动电机啸叫力矩忽大忽小。原因最常见是ADC采样时刻落在功率管开关边沿或者采样触发的DMA没有在PWM周期内对齐。解决把ADC触发源从软件定时改为定时器比较事件与三角载波顶点对齐采样点避开死区插入时刻。若仍有毛刺给电流反馈加一阶低通截止频率放在电流环带宽的5~10倍不要用滑动平均拖慢相位。5.2 电机能转但出力明显不足现象同样的Iq指令电机输出力矩只有预期的一半低速时尤其明显。原因θoffset不正确导致Iq有一部分跑到了d轴。另一个常见原因是电流采样标定错误——相电流采样增益不对反馈值整体偏小PI一直往上推电压。解决先做电流校准给定已知Iq把实际母线电流读数与代码里的换算对照再重做零点对齐确认电机在给定正向q轴电流时能正转。5.3 零序注入后调制波削顶现象相电压波形顶端变成平顶转速在高电压区上不去。原因零序注入公式选错了坐标系或调制波范围已经是0~Udc又加了一次0.5f偏置把调制波整体顶到载波上限之上。解决打印三相占空比看空载、固定电压指令下输出是否在0~1之间。发现恒定偏置就把坐标转换里的偏置项去掉。这类问题用串口或DAC打印调制波最直观别靠猜测。5.4 速度环震荡转速来回摆现象给定2000rpm实际速度在1800到2200之间周期波动声音跟着摆动。原因速度环带宽ω_v与电流环带宽没拉开级差或速度反馈滤波太弱微分噪声被放大。解决先降低ω_v比如从80降到40rad/s看震荡是否消失。保持速度滤波截止频率在2~4倍ω_v。如果震荡还没消检查机械共振用手摸电机底座60~200Hz之间有嗡嗡共振时把速度环带宽降到共振频率的一半以下。5.5 上电直接抖动甚至转不动现象每次上电电机先抖一下或者给定速度完全不转。原因增量式编码器没有绝对位置上电时θoffset丢失或者对齐流程被跳过PI在错误角度上强行出力。解决把零点对齐做成强制上电流程用一个标志位存进Flash没有零点标志禁止闭环输出。对齐之后不要马上跑满速度先低速转一圈确认方向正确。排查这类问题时有一条经验把三环的给定、反馈、输出全部用串口或DAC打印出来做一个“控制环示波器”。参数发散和结构错误在数据里藏不住多数玄学问题最后都归结到某个环节标定单位不一致。我每换一套硬件先花半小时确认零点、电流增益、母线电压三个常量再谈调参这半小时能让后续调试省出几小时。6. 三环参数的自检与验证用阶跃响应确认带宽6.1 电流环测阶跃上升时间反推真实带宽给Iq一个额定电流20%的阶跃记录反馈电流从10%升到90%的时间t_r。一阶系统近似带宽ω ≈ 0.35 / t_r。比如t_r0.5msω≈700rad/s与目标带宽800rad/s接近就说明参数合理。如果差很多优先检查电感和电阻参数再检查电流采样是否滞后。6.2 速度环看阶跃超调与恢复时间给定速度阶跃记录超调量。刚性等级低时响应偏软超调小等级过高时超调大且有回摆。把ω_v降下来重新算Kp_v和Ki_v二次阶跃超调能控制在5%~10%就算整定完成。6.3 位置环斜坡跟随误差定位纯P位置环一定存在稳态误差用斜坡给定测跟随误差。误差与速度成正比可以算出等效刚度Kp_p够不够。加上前馈后误差应显著下降如果前馈增益加到1.0时误差仍大检查单位换算——位置环输出是速度给定速度量纲混用rpm和rad/s时前馈系数怎么调都不对。6.4 调制器看马鞍形波形确认等效SVPWM零序注入是否生效示波器看相电压最直接。逐步加大Iq调制波从正弦变成马鞍形说明注入起作用如果是平顶或梯形失真回到第3章查坐标转换和偏置。整套自检从电流环开始逐层向外。每次换电机只改电感、电阻、惯量和极对数四个参数带宽按需求微调就能覆盖大多数场景。从那以后我每次换电机都强制走一遍「对齐→电流阶跃→速度阶跃→位置斜坡」这套流程不再凭感觉调参也希望这套流程帮到你。本文还有配套的精品资源点击获取