简介面向电机控制与嵌入式开发学习者的有感FOC位置、速度与电流三环算法实现资料围绕坐标变换、PID调节、SVPWM调制、编码器反馈与电流采样等关键环节展开代码在STM32F4平台上运行适合希望理解有感控制链路、搭建实验样机或对照调试的读者参考。压缩包共194个文件约5.05MB以57个h头文件、46个c源文件为核心另有d、o、crf、axf、hex、map等编译中间文件与工程产物并包含工程文件及少量配置、脚本和说明文件便于还原完整工程结构、核对模块划分与编译依赖。已有979人学习可作为三环控制代码组织、外设驱动与工程配置的实践样本帮助读者梳理从电流采样、坐标变换、控制运算到调制输出的实现脉络并积累编码器接入、参数整定和排错思路对理解三环闭环结构与代码落地有一定帮助也可供课程设计、毕业设计及电机驱动入门项目参考。1. 有感 FOC 三环实现开环能转、闭环一上就抖的那道坎很多人做 FOC 都卡在同一个位置六步换相能转开环 SVPWM 也能转电流环单独跑波形还挺干净可一旦把速度环、位置环串上去电机立刻开始嗡嗡、低频摆动给个位置指令还会冲过头再荡回来。这套有感 FOC 三环实现要解决的就是这一段用编码器拿真实转子位置把电角度对齐、电流环、速度环、位置环按明确的时间尺度分层配上轨迹规划和波形记录让电机从「能转」走到「指哪停哪」。整份是 C 语言实现算法层不依赖第三方库适合从方波换相往 FOC 闭环迁的人、单电流环跑通要补速度和位置的人以及做云台关节、转台这类低速高精度机构的人。2. 三环的串接时序从编码器计数到 SVPWM 占空比的一条链三环代码写得对不对先别看 PI 参数先看时序。同样是三个 PI执行频率排错了参数怎么调都是玄学。这一章把一条完整链路拆开编码器计数怎么变成电角度、三个环分别在哪一拍执行、ADC 在 PWM 周期的哪个点触发。2.1 三环不该挤在一个节拍里先把时间尺度分层最容易犯的错是把三个 PI 全塞进同一个 1 ms 定时器觉得「反正都是算一次」。电流环的被控对象是电感电气时间常数 L/R 通常只有几百微秒到几毫秒速度环的对象是转动惯量机械时间常数在几十毫秒量级位置环还要更慢。三者差两个数量级全按最慢的节拍跑电流环的相位裕度会被采样延迟吃掉表现就是 iq 波形毛刺大、带载啸叫、一加位置环就振。常见的分层方式按下表来注意最后一列的带宽不是拍脑袋定的而是逐级往下除十。环典型执行频率周期输入输出带宽经验值电流环与 PWM 同频10~20 kHz50~100 μsid、iqvd、vqPWM 频率的 1/10~1/20速度环1~2 kHz0.5~1 ms转速反馈iq_ref电流环带宽的 1/10位置环200~500 Hz2~5 ms位置反馈速度指令速度环带宽的 1/5~1/10分频不用另开定时器直接在 PWM 中断里数拍子最省事也避免了两个中断互相打断导致的时序抖动。下面这段是工程里实际用的骨架/* PWM 中断 16 kHz电流环每拍都跑速度环 16 分频位置环再 4 分频 */ void PWM_IRQHandler(void) { /* 1. 先读本拍采样好的三相电流做 Clarke/Park */ foc_current_loop(); /* 1 kHz 带宽16 kHz 执行 */ /* 2. 速度环 1 kHz位置环 250 Hz靠计数器分频 */ if (div_speed 16) { div_speed 0; foc_speed_loop(); if (div_pos 4) { div_pos 0; foc_position_loop(); } } /* 3. 三个环都算完统一更新一次占空比避免半拍更新 */ svpwm_update(); }逻辑上有两个点值得说清楚。第一foc_current_loop()必须排在最前它吃的是本拍刚采到的电流晚一步就变成用上一拍的电流算这一拍的电压等效增加一拍延迟。第二svpwm_update()只在最后调一次中间任何一环更新了占空比都算「半拍更新」占空比会在一个 PWM 周期内被改写两次桥臂上出现窄脉冲电流采样直接失真。参数上唯一要盯的是分频系数和 PWM 频率的整除关系。16 kHz 分 16 得 1 kHz再分 4 得 250 Hz都是整数如果 PWM 设成 15 kHz速度环就只能取 1 kHz分 15或 937.5 Hz分 16后者会让速度环周期和电流环周期失去整倍数关系速度环每次采到的电流纹波相位都不一样反馈里混进一个和转速无关的拍频。2.2 电角度换算极对数、方向与零位偏移有感 FOC 和开环最大的差别就在这一步电角度不是积分积出来的是从编码器读数换算出来的。换算链是「计数 → 机械角 → 乘极对数 → 加零位偏移 → 绕回 0~2π」。链上任何一环的符号或单位错掉现象都很像电流给得挺大出力却很小电机发热明显iq 用不满。#define ENC_CPR 10000.0f /* 2500 线增量编码器四倍频后 10000 计数/圈 */ #define POLE_PAIRS 7 /* 极对数必须和转子磁钢实际数量一致 */ typedef struct { int32_t raw; /* 本拍读到的硬件计数 */ int32_t last; /* 上一拍计数用于算增量 */ int32_t total; /* 多圈累计计数掉电可存 Flash */ float theta_m; /* 机械角rad范围 0~2π */ float theta_e; /* 电角度rad范围 0~2π */ float offset_e; /* 对齐阶段测出的零位偏移 */ float dir; /* 1.0f 或 -1.0f编码器安装方向 */ } enc_t; void enc_update(enc_t *e) { int32_t cnt read_encoder_raw(); int32_t diff cnt - e-last; /* 计数回绕超过半圈就认为是反向跨零而不是真跳了半圈 */ if (diff (int32_t)(ENC_CPR * 0.5f)) diff - (int32_t)ENC_CPR; if (diff -(int32_t)(ENC_CPR * 0.5f)) diff (int32_t)ENC_CPR; e-last cnt; e-total diff; /* 单圈机械角负数取模要先补一圈否则会得到负角度 */ int32_t single e-total % (int32_t)ENC_CPR; if (single 0) single (int32_t)ENC_CPR; e-theta_m e-dir * ((float)single / ENC_CPR) * 2.0f * PI_F; /* 电角度 机械角 × 极对数 零位偏移再绕回一圈 */ float ang e-theta_m * POLE_PAIRS e-offset_e; while (ang 2.0f * PI_F) ang - 2.0f * PI_F; while (ang 0.0f) ang 2.0f * PI_F; e-theta_e ang; }dir这一项不是可有可无的。编码器正转计数增加、电机相序却是 A-C-B 的情况非常常见方向错一位Park 变换用的 sin/cos 就和实际磁场反了闭环会变成正反馈给定 iq 越大转矩反而越小最后过流保护。判断方向有个笨办法但很可靠手动正转电机轴一圈看total是增是减再看三相电流按 A→B→C 通电时转子是不是往同一个方向走两者不一致就把dir取反。零位偏移offset_e的测法在工程里基本固定给 d 轴一个不大的电流额定电流的 20%~30%iq 给定 0转子会被拉到 d 轴并与 A 相轴线对齐等转子稳定不再动读一次theta_m则offset_e -theta_m * POLE_PAIRS。这一步的坑在于「稳定」的判断——只看时间不够要看连续若干拍的编码器增量都小于阈值否则转子还在微振时读到的角度会带几度误差而这个误差会以同样的比例全部跑到电流环里去。2.3 采样时刻ADC 触发点选错后面全白搭电流采样和 PWM 的相位关系是这套代码里最容易被忽略、又最能解释「参数明明算对了但波形就是脏」的一环。三相桥在开关瞬间会有很大的 dv/dt采样点落在开关沿上采到的既不是母线续流值也不是绕组真实电流而是一个尖峰。这类干扰在示波器上看是每拍一个窄毛刺在 dq 轴上表现为 id 有直流偏置、iq 有周期性纹波速度环再把这个纹波放大成低频啸叫。正确做法是用中心对齐 PWM把计数器设成「增-减」模式在计数器等于周期值也就是三角波顶点时触发 ADC。这个时刻三只下桥臂的状态在一个 PWM 周期内最稳定续流路径确定采样窗口足够宽。采样点位置采到的值现象计数器 0下桥臂全开近似相电流可用但窗口窄死区影响大计数器 ARR上桥臂全开母线电流而非相电流双电阻采样时不可用计数器 ARR/2中点相电流纹波峰峰值最小推荐配合中心对齐开关沿后立即触发开关尖峰毛刺大、id 有偏置、易误保护配置上就是两句话的事但顺序不能反先配 PWM 中心和死区再配 ADC 的外部触发源指向 PWM 的 TRGO最后把电流环的读取函数放在 ADC 转换完成中断里或者在 PWM 中断开头直接读已经转换好的结果。后一种更稳因为它保证了「读到的值一定属于本拍」。提示死区时间不要超过 PWM 周期的 3%。死区越大低速时电压误差占比越高位置环低速爬行时会出现一顿一顿的爬行感这种问题加滤波是压不下去的只能从死区补偿或缩短死区入手。3. 电流环与速度环的整定把 PI 参数从「试出来」变成「算出来」调参调三天不如先把被控对象写出来。电流环的对象是一个一阶惯性环节速度环的对象是一个积分环节两者的 PI 参数都有解析解算出来之后再微调比盲试快一个数量级。这一章给出计算脚本、离散化实现以及浮点误差和抗饱和的处理。3.1 电流环被控对象就是 1/(LsR)忽略反电动势的交叉耦合和死区d 轴和 q 轴可以各自看成独立的一阶系统电压到电流的传递函数是 1/(LsR)。用 PI 控制器 C(s)KpKi/s 去对它让零点抵消掉对象的极点环路增益就变成 Kp/(Ls)穿越频率等于 Kp/L。于是有Kp ωc · LKi ωc · R其中 ωc 是期望带宽rad/s。带宽不能随便给上限是开关频率的 1/10 左右再高离散化后的相位延迟会把裕度吃光。import math def current_loop_gains(L, R, fc, fs, vdc): 按带宽整定电流环 PI L: 相电感(H) R: 相电阻(ohm) fc: 期望带宽(Hz) fs: 电流环执行频率(Hz) vdc: 母线电压(V) wc 2.0 * math.pi * fc if fc fs / 10.0: raise ValueError(带宽超过执行频率的 1/10离散后相位裕度不足) kp wc * L # V/A ki wc * R # V/(A·s) ki_ts ki / fs # 离散化后每拍累加的系数 v_limit vdc / math.sqrt(3.0) # SVPWM 线性调制区最大相电压 return kp, ki_ts, v_limit kp, ki_ts, vlim current_loop_gains(L0.8e-3, R0.35, fc800, fs16000, vdc24) print(fkp{kp:.3f} V/A ki_ts{ki_ts:.4f} V/A v_lim{vlim:.2f} V) # 输出kp4.021 V/A ki_ts0.1100 V/A v_lim13.86 V三个返回值分别对应比例增益、积分项每拍增量、输出电压的圆限幅半径。以一套小惯量云台电机为例参数和算出来的结果可以对照下表改自己的电机时把前四行换掉重跑脚本即可。参数符号示例值说明相电感L0.8 mH用电感表测线电感后除以 2相电阻R0.35 Ω线电阻除以 2冷态期望带宽fc800 Hz不超过 PWM 频率的 1/10执行频率fs16 kHz与 PWM 同频母线电压vdc24 V圆限幅取 vdc/√3比例增益Kp4.02 V/A积分系数Ki·Ts0.110 V/A位置式每拍累加顺带说一句Kp 算出来是 4 V/A 这种量级看着很吓人其实正常电机只有 3 A 额定电流误差只要 3 A 输出就打到限幅了电流环本来就是「要么满压、要么不动作」的快环。真正需要盯的是 Ki·Ts 这个数——它决定积分多久能把稳态误差磨掉越小响应越慢越大越容易在限幅附近来回跳。3.2 从连续域到离散域差分形式与抗饱和连续域的 Ki 是「每秒」写进代码要乘上控制周期 Ts这一点大部分人都知道。真正翻车的是抗饱和电压输出被 SVPWM 的圆限幅卡住之后误差还在积分项继续累加等误差反向时积分项要花很久才能退回来表现出来就是超调大、恢复慢、大指令下电机「冲一下再回头」。typedef struct { float kp; /* 比例增益V/A */ float ki_ts; /* Ki × Ts每拍积分增量 */ float i_out; /* 积分项累计值单位与输出相同 */ float out_max; float out_min; } pi_t; void pi_init(pi_t *p, float kp, float ki_ts, float out_max) { p-kp kp; p-ki_ts ki_ts; p-i_out 0.0f; p-out_max out_max; p-out_min -out_max; } float pi_step(pi_t *p, float err) { float out p-kp * err p-i_out; /* 输出撞上限时只允许误差为负往回拉时继续积分 */ if (out p-out_max) { if (err 0.0f) p-i_out p-ki_ts * err; return p-out_max; } /* 撞下限同理只允许误差为正时积分 */ if (out p-out_min) { if (err 0.0f) p-i_out p-ki_ts * err; return p-out_min; } p-i_out p-ki_ts * err; /* 浮点累计兜底防止长时间偏置把积分项推出物理范围 */ if (p-i_out p-out_max) p-i_out p-out_max; if (p-i_out p-out_min) p-i_out p-out_min; return out; }pi_step里做了两层保护撞限幅时的条件积分以及积分项本身的硬钳位。第一层保证退饱和快第二层保证即使遇到传感器异常导致误差长期单调积分项也不会跑成一个大到离谱的数——真跑出去了恢复时电机会有明显一下冲击。还有个小细节值得写进注释ki_ts的精度。如果 Ki 是按每秒算的Ts 是 62.5 μs乘积只有 0.1 量级用 float 存没问题但如果有人图省事把 Ki 直接乘 1e-6 或者把单位换成 A 的千分之一这个数会掉到 1e-5 以下和 float 的有效位打架积分就变成「加了很多次等于没加」。统一用 SI 单位、在代码里只做一次单位换算能省掉后面排查的大半时间。3.3 速度环惯量、转矩常数与反馈滤波速度环的被控对象是机械环节iq 到转速的传递函数是 Kt/(Js)其中 Kt 1.5 · p · ψf 是转矩常数J 是折算到电机轴的转动惯量。对它用 PI同样让零点抵消对象极点得到Kp J · ωs / KtKi Kp · ωs / 5ωs 是速度环带宽。这里比电流环多了一个「/5」的经验系数原因是速度反馈里必然带滤波和量化噪声环路增益给太高会把噪声放大成电流抖动零点放在带宽的 1/5 处能让相位裕度落在 60° 附近。J 和 Kt 都可以实测Kt 用堵转加给定 iq 测静态转矩或者从反电动势常数换算J 用「给定 iq 阶跃看转速上升斜率」反推。两个数都不需要很准20% 的误差靠后面的微调就能盖住。#define TS_SPEED 0.001f /* 速度环周期 1 ms */ #define W_LPF 1256.6f /* 反馈低通截止 200 Hz 2π×200 */ #define IQ_MAX 3.0f /* 转矩电流限幅A */ typedef struct { pi_t pi; float w_fbk; /* 滤波后的转速rad/s */ float iq_ref; } spd_t; void speed_loop(spd_t *s, float w_cmd) { float w_raw speed_from_encoder(); /* M/T 法测速rad/s */ float a TS_SPEED / (1.0f / W_LPF TS_SPEED); /* 一阶低通系数≈0.557 */ s-w_fbk a * (w_raw - s-w_fbk); float err w_cmd - s-w_fbk; s-iq_ref pi_step(s-pi, err); /* 限幅放在 PI 之后同时作为下一拍 PI 的 out_max 参考 */ if (s-iq_ref IQ_MAX) s-iq_ref IQ_MAX; if (s-iq_ref -IQ_MAX) s-iq_ref -IQ_MAX; }测速方法这里单独拎出来说。低速时用 M 法固定时间数脉冲分辨率极差10000 计数/圈、1 ms 采样一个脉冲对应 0.1 圈/秒也就是 6 rpm低于这个转速反馈全是台阶。台阶进到 PI 里积分项会跟着台阶一跳一跳电机低速爬行就是这么来的。换成 M/T 法同时数高频时钟和脉冲低速时自动变成测周期分辨率能往下探两个数量级。代码里speed_from_encoder()就是这一段实现上要注意高频时钟的计数溢出——1 MHz 时钟、1 ms 周期计数值 1000 左右用 16 位定时器没问题但别用 8 位。4. 位置环P 增益、前馈与轨迹规划位置环是三个环里最容易被过度设计的一个。很多人上来就给它加积分结果到位之后来回蹭越调越乱。这一章讲清楚为什么位置环通常只用 P、带宽怎么取以及把阶跃指令变成电机跟得上的轨迹。4.1 位置环只用 P 的理由与带宽取值内层速度环如果已经闭环从速度指令到实际转速近似为 1在速度环带宽以内那么位置环的被控对象就是一个纯积分器 1/s。对它用纯比例控制闭环传递函数是 Kp/(sKp)一阶、无超调、带宽就等于 Kp单位 rad/s。如果用 PI开环多一个积分闭环变成二阶阻尼比由两个参数共同决定很容易调出 20% 以上的超调而这个超调在机构上就是撞限位。Kp 的取法有明确约束位置环带宽取速度环带宽的 1/5 到 1/10。速度环带宽 80 Hz 的话位置环给 10~16 Hz也就是 Kp 大约 63~100 rad/s。低于这个范围响应太肉高于这个范围外环会开始「指挥不动」内环出现等幅振荡。#define KP_POS 62.8f /* rad/s约 10 Hz 位置环带宽 */ #define W_MAX 30.0f /* 速度指令限幅rad/s */ float pos_loop(float th_ref, float th_fbk, float v_ff) { /* 角度误差必须绕回 ±π否则跨零时会给出接近 2π 的错误指令 */ float err wrap_pi(th_ref - th_fbk); /* 纯 P 速度前馈前馈直接叠在速度指令上 */ float w_cmd KP_POS * err v_ff; if (w_cmd W_MAX) w_cmd W_MAX; if (w_cmd -W_MAX) w_cmd -W_MAX; return w_cmd; }wrap_pi这一步不能省。位置反馈是多圈累计值指令也是多圈值直接相减在跨零时没有任何问题但如果指令来自「最短路径」规划比如转台从 350° 走到 10° 想走近路误差就会变成 -340°纯 P 会给一个巨大且方向错误的速度指令机构直接撞过去。统一在误差上绕回 ±π是最省心的做法。4.2 梯形与 S 曲线规划把阶跃变成电机跟得上的指令给位置环一个阶跃指令等于要求电机在零时间内产生无限大加速度物理上做不到结果就是电流瞬间打到限幅、速度环饱和、位置超调。所以位置指令一定要经过规划器最常用的是梯形速度规划加速段、匀速段、减速段三段都有明确的加速度上限。import math def trapezoid_profile(dist, vmax, amax, dt): 梯形速度规划返回 (t, pos, vel, acc) 列表 dist: 总位移(rad) vmax: 最大速度(rad/s) amax: 最大加速度(rad/s^2) dt: 插补周期(s) sign 1.0 if dist 0 else -1.0 d abs(dist) t_acc vmax / amax if amax * t_acc * t_acc d: # 2×(0.5·a·t²) d说明到不了 vmax t_acc math.sqrt(d / amax) # 退化成三角形重算加速时间 vpk amax * t_acc else: vpk vmax d_acc 0.5 * amax * t_acc ** 2 t_const (d - 2.0 * d_acc) / vpk if vpk 0 else 0.0 t_total 2.0 * t_acc t_const out, t, pos, vel [], 0.0, 0.0, 0.0 while t t_total: if t t_acc: # 加速段 acc amax elif t t_acc t_const: # 匀速段 acc 0.0 else: # 减速段 acc -amax vel acc * dt pos min(pos vel * dt, d) # 末段积分 p a hrefhttps://download.csdn.net/download/xiaoyouxi12306/82890612 stylecolor:#ec7500;font-size:14px; 本文还有配套的精品资源点击获取 /a img altmenu-r.4af5f7ec.gif srchttps://csdnimg.cn/release/wenkucmsfe/public/img/menu-r.4af5f7ec.gif stylewidth:16px;margin-left:4px;vertical-align:text-bottom;cursor:text; /p