读SimpleFOC源码这件事越往后越有意思。前面几篇我把电机控制的主循环、电流采样、PID调节都过了一遍发现一个规律很多看起来神奇的上层行为本质上都是底层几个数学函数堆出来的。这次轮到的就是整个库的数学基础层——foc_utils.cpp和foc_utils.h。对照的源码版本是v2.3.2也就是我一直在啃的这个tag。别看这两个文件加起来没多少行角度归一化、数值限幅、矢量旋转、电角度换算全在这里几乎每个上层模块都会include它。如果你也想弄明白为什么电机在某个位置会抖、为什么角度偶尔会跳变、为什么控制器输出有时候会冲出限幅建议先把这一层啃透。这篇文章我会把每个函数的实现思路、边界条件、调用场景都拆开讲还附上我在移植和调试过程中踩过的坑适合正在啃SimpleFOC源码、或者准备自己写一套FOC数学工具的人。1. 先说清楚它在整个源码里的位置1.1 一个被所有算法环节依赖的公共层打开v2.3.2的源码目录在src/common下会看到foc_utils.cpp和foc_utils.h和pid_controller、lowpass_filter、time_utils这些文件平级。按照SimpleFOC自己的模块划分它属于“公共工具”而不是“电机驱动”或者“传感器驱动”。通俗点讲这个文件就是整个库的数学工具箱不管你是电流环、速度环、位置环还是编码器读数、无感观测器估算只要涉及到角度处理和坐标变换最终都会落到这一层。我实际翻代码的时候发现凡是看到#include foc_utils.h的地方背后几乎都是同一类需求要把某个角度转变到适合三角函数计算的常规范围或者要把某个矢量从旋转坐标系搬到静止坐标系。它不直接操作PWM引脚也不读电流采样寄存器但它计算出来的每个数字都会直接影响你电机的每一相电压。这一层最大的特点是没有硬件依赖这意味着你可以把这两个文件原封不动地拷到PC上单独编译、单独测试这也是我后面选择先把它拎出来做单元测试的原因。1.2 为什么SimpleFOC要把“数学基础”单独拆出来这个问题我在读第一遍的时候没在意后来看了几个别的电机控制库才体会到拆出来的好处。最直接的一点是复用_constrain这个限幅函数在电流限制、电压限制、位置限制里都用到如果每个模块自己写一遍很容易出现边界行为不一致。比如某个模块用的是小于判断另一个模块用的等于判断结果在限幅边界上就差出一个极小量的误差长期运行下来会变成可见的电流波动。第二个好处是语义统一。角度归一化这件事如果十处调用各自实现有的返回[0, 2π)有的返回[-π, π)有的函数内部顺手归一化、有的不归一化那你调试的时候就等着被坑。SimpleFOC把_normalizeAngle固定成“返回[0, 2π)区间”所有上层模块都遵循这个约定出了角度相关的问题你只需要怀疑传入的角度是不是合法不需要怀疑归一化函数本身的逻辑。第三个好处也是我后来自己做移植时最认可的性能优化有集中点。如果以后想换查表三角函数、或者把浮点运算改成定点运算只需要动foc_utils这一个文件不需要去翻十几个调用方。当然这种集中也有代价就是所有模块都依赖它一旦改了某个函数的返回语义连锁反应会很大。所以我在阅读过程中给自己定了一条规矩先读懂先别急着改。2. 头文件里的常量、类型与函数面面观2.1 自带的数学常量与M_PI的取舍foc_utils.h里一上来就定义了几个圆周率相关的常量比如_PI、_2PI、_3PI_2、_PI_2。为什么SimpleFOC不直接用math.h里的M_PI我个人的理解是C标准其实并不保证M_PI一定存在虽然gcc和很多桌面编译器都提供了但某些嵌入式工具链默认不开GNU扩展M_PI就可能编译不过。SimpleFOC要兼容Arduino、各种STM32工程、ESP32与其依赖编译器心情不如自己把常量写清楚。另外_2PI这个值在角度归一化里太常用了如果每次都是angle fmod(angle, 6.2831853f)一是容易抄错二是阅读代码的人还要反应一下这个数是什么。独立定义成常量之后代码意图一目了然。这里我整理了一张表对应关系如下常量数值典型用途_PI3.141592653589793半圈角度PI控制里也常用_2PI6.283185307179586一圈角度_normalizeAngle的模_3PI_24.71238898038469三相坐标变换、扇区判断_PI_21.5707963267948966直角旋转测试时经常用有一点值得提醒这些常量的类型默认是double还是float取决于具体代码怎么写的。如果你在float环境下用最好在常量后面加f后缀避免每次计算都发生double/float隐式转换。SimpleFOC在部分版本里已经注意了这一点但你读自己工程里的本地代码时还是扫一眼比较放心。2.2 Vector结构体与旋转方法的继承关系foc_utils.h里定义了一个非常简单的结构体typedef struct { float alpha; float beta; } Vector;名字不叫x和y而叫alpha和beta是因为在FOC语境下它对应的就是静止两相坐标系里的αβ分量。同一个数学结构在电流采样后是Iα和Iβ在逆Park变换前是Vα和Vβ在SVPWM里又是电压矢量在αβ轴上的投影。用alpha/beta命名可以从命名上强迫你记住这点这个矢量是两相静止坐标系里的东西不是随便一个二维坐标。这个结构体里还带了一个旋转方法大致实现是这样的void Vector::rotate(float angle) { float rot_alpha alpha * _cos(angle) - beta * _sin(angle); float rot_beta alpha * _sin(angle) beta * _cos(angle); alpha rot_alpha; beta rot_beta; }C里结构体带方法本质上和类没有区别只是默认访问权限是public。SimpleFOC把旋转方法直接放进Vector里意味着你随时可以拿一个矢量出来旋转一下不需要额外调用全局函数。这种设计在源码阅读时非常友好你看到v.rotate(electrical_angle)就知道这是在把一个dq轴矢量转到αβ轴。2.3 这个文件对外暴露的函数一览我在读代码的时候习惯先把头文件里的函数声明抄一遍列成一张清单后面实现部分对着清单逐个过这样不容易漏。foc_utils这层暴露的核心函数大致有这几类函数签名功能注意点float _normalizeAngle(float angle)把任意角度归一化到[0, 2π)输入弧度制返回也是弧度float _electricalAngle(float shaft_angle, int pole_pairs)机械角换算电角度结果通常不在此文件内归一化float _constrain(float val, float min, float max)数值限幅NaN和Inf会穿透float _sign(float val)符号函数0返回正号void Vector::rotate(float angle)矢量原地旋转角度为弧度制另外函数名前面加下划线是库内部符号的一种表达习惯。它暗示你这些函数并不是面向最终用户的“官方API”而是内部基础设施。你自己写上层代码的时候尽量通过SimpleFOC公开的接口走不要到处直接调用这些底层函数不然以后版本一升级函数签名变了你的工程会非常痛苦。3. 核心函数实现逐行拆解3.1 _normalizeAngle角度归一化的精度与边界_normalizeAngle是这层文件里最有代表性的函数代码很短但细节非常多。典型的实现长这样float _normalizeAngle(float angle) { float a fmod(angle, _2PI); return a 0 ? a : (a _2PI); }数学逻辑很简单任意角度用fmod取余把结果压到(-2π, 2π)区间如果余数为负就加一个2π最后返回落在[0, 2π)范围内的值。为什么不用while循环因为输入角度可能很大用while一次一次减不现实循环次数不确定实时控制系统最忌讳这种性能不确定的代码。fmod是库函数计算时间是相对稳定的。这里有几个边界值得单独讲。第一如果输入角度本身是2π的整数倍比如正好6.283185307由于float精度问题fmod结果可能是一个非常接近0的数也可能返回一个非常接近2π的数具体取决于浮点舍入。第二如果返回结果恰好略小于2π那没问题如果某些编译器下刚好等于2π那就违反了[0, 2π)的严格定义个别版本会再补一句if (a _2PI) a - _2PI;。第三也是最容易被忽视的当输入角度是一个万级别的数时float只有大概7位有效数字fmod本身的精度也会跟着下降余数的误差可能达到肉眼可见的程度。所以不要以为这个函数万能它救不了“长期不归一化导致角度累积到百万级”的烂账。3.2 _constrain一个断言不出来的隐形坑再来看_constrain实现比normalize还要短float _constrain(float val, float min, float max) { return val min ? min : (val max ? max : val); }它的作用就是限幅把val限制在[min, max]区间内。这个函数在SimpleFOC里出现频率极高电流环输出限幅、电压限幅、位置参考限幅都会用到。有人会问为什么不直接用std::clamp因为SimpleFOC要兼容老旧嵌入式工具链很多环境不支持C17自己写一个更保险。但我要重点说一个隐藏坑这个函数拦不住NaN。如果val是NaN那么NaN min为falseNaN max也为false整个表达式会返回NaN本身。也就是说当你的PID控制器因为某些原因发散成NaN时限幅函数并不会把它“卡”在边界上而是让NaN继续往后传。这个NaN最终会变成PWM的比较值轻则电机抖一下重则母线过流。Inf同理正无穷会返回max负无穷会返回min看起来好像安全但Inf本身说明算法已经出问题了限幅只是掩盖了症状。所以我在自己的代码里加了一层安全网在PID输出之后、进入电流限幅之前做一个判断如果值是NaN或Inf直接置0。这比指望_constrain兜底可靠得多。另外还要注意如果调用时min比max还大这个函数不会帮你纠正返回的结果可能完全不在你预期的范围内所以调用方的参数顺序一定要自己保证。3.3 Vector::rotate旋转公式为什么不能省中间变量矢量旋转是FOC坐标变换的核心之一公式本身大家应该都熟把一个矢量旋转θ角新alpha等于原alpha乘cosθ减去原beta乘sinθ新beta等于原alpha乘sinθ加上原beta乘cosθ。写成代码就是void Vector::rotate(float angle) { float rot_alpha alpha * _cos(angle) - beta * _sin(angle); float rot_beta alpha * _sin(angle) beta * _cos(angle); alpha rot_alpha; beta rot_beta; }这里有一个很值得说的工程细节为什么需要rot_alpha和rot_beta两个临时变量而不是直接写alpha alpha * cos - beta * sin; beta alpha * sin beta * cos;因为如果你直接更新alpha第二个式子里的alpha已经是旋转后的值了用旋转后的alpha接着算beta结果就完全错了。这种错误我在自己写坐标变换时犯过现象特别隐蔽低速时看不出来转速一高电流相位就乱电机发热严重。在FOC算法里这个rotate方法最常见的用途是逆Park变换。你有一个dq轴上的电压矢量比如Vector(vq, vd)或者Vector(vd, vq)然后调用rotate传入电角度就能得到αβ轴上的电压矢量。源码里很多地方就是这么写的一行代码完成坐标变换非常简洁但读代码时一定要心里清楚这个rotate背后是一次完整的三角函数运算成本并不低。3.4 _electricalAngle与电角度换算的细节_electricalAngle的函数实现非常直白float _electricalAngle(float shaft_angle, int pole_pairs) { return shaft_angle * pole_pairs; }机械角度乘以极对数得到电角度。举个直观的例子一个4极对电机转子每转一圈电气角度变化4个2π周期所以同一个电压矢量模式在一圈里会出现4次。极对数越大同样的机械转速下电频率越高对控制器的计算频率要求也越高。这个函数的关键在于它只做乘法不做归一化。也就是说如果机械角已经超过一圈返回的电角度可能大于2π甚至更大。SimpleFOC把归一化留给上层调用者是因为三角函数本身是周期函数你直接把一个未归一化的大角度传给sin/cos结果依然是正确的。但是如果你想做角度比较、调试打印、或者把电角度用在下游的观测器里最好还是先_normalizeAngle一下。我实际调试时发现一个问题有些代码路径会传入累计的机械角有些代码路径传入的是单圈内的机械角两者乘完极对数之后的值域差很多。如果你用同一个电角度去驱动SVPWM因为三角函数周期性的关系可能没问题但如果你拿着这个电角度去查校正表或者去做扇区判断那么未归一化的角度就会导致错误。所以看代码的时候要特别留意调用_electricalAngle的上层函数有没有顺手做归一化不要默认它已经做了。3.5 _sign简单到不值得写但值得注意最后一个函数是符号函数float _sign(float val) { return val 0 ? -1.0f : 1.0f; }这个比前面几个还简单就是判断正负。在SimpleFOC里它主要用于方向判断和滞回逻辑比如某些情况下需要知道当前位置误差的符号来决定输出反向。注意它把0归到了正号那一侧也就是说val等于0时返回1.0f。如果你需要“零返回零”的三态符号函数得自己再加一层判断。这类小函数单看不值一提但它体现了嵌入式库的一个风格能内联就内联能简单就不啰嗦每个函数都尽量保证在实时循环里可以被频繁调用而不拖慢速度。4. 实操在PC上把这几个函数单独拉出来测一遍4.1 最小测试工程搭建源码学习最忌讳只看不练尤其数学层这种无硬件依赖的文件完全可以在PC上直接跑。我的做法是建一个临时目录把源文件里的foc_utils.h和foc_utils.cpp复制过去再写一个test_main.cpp用g编译把所有边界条件打印出来。测试代码如下#include stdio.h #include assert.h #include math.h #include foc_utils.h int main() { float a1 _normalizeAngle(_2PI); printf(normalize(2PI) %.9f\n, a1); assert(a1 0.0f a1 _2PI); float a2 _normalizeAngle(-0.001f); printf(normalize(-0.001) %.9f\n, a2); Vector v{1.0f, 0.0f}; v.rotate(_PI_2); printf(rotate(PI/2): alpha%.6f beta%.6f\n, v.alpha, v.beta); assert(fabs(v.alpha - 0.0f) 1e-6); assert(fabs(v.beta - 1.0f) 1e-6); float c _constrain(5.0f, -1.0f, 1.0f); printf(constrain(5, -1, 1) %.1f\n, c); float nan_result _constrain(NAN, -1.0f, 1.0f); printf(constrain(NAN) %f\n, nan_result); return 0; }编译命令很简单g -stdc11 -O2 -o test_foc_utils test_main.cpp foc_utils.cpp -lm注意加上-lm因为foc_utils实现里用了libm的数学函数。如果你是在Windows上用MinGW也一样能编译不需要改代码。4.2 边界值测试结果与解读跑完上面的测试你会看到几个很有说服力的输出。比如normalize(2PI)在float精度下很可能不会返回0而是返回一个非常接近2π的数这正好印证了我前面说的浮点舍入问题。normalize(-0.001)会返回6.281左右也就是从2π那边继续往前绕了小半圈。而rotate(PI/2)如果输入是(1, 0)输出应该非常接近(0, 1)误差在1e-6量级这说明旋转公式本身实现正确。我建议你把测试用例扩一下把输入角度从-1000到1000都打印一遍特别关注几个关键点0附近、π附近、2π附近、负值边界。然后你会发现一个规律绝大多数输入都能正确落到[0, 2π)但经过float表示后某些输入会出现微小的偏大或偏小。这个现象本身不是bug但如果你在下游拿电角度做严格相等比较就会踩到。还有一个值得做的测试是把_constrain的输入改成NaN观察输出。我第一次跑这个测试的时候看到输出是nan心里咯噔一下因为这意味着SimpleFOC的默认限幅逻辑并不会帮你挡住非法数值。后来我在PID输出侧主动加了NaN检查才算彻底放心。4.3 把测试结果对应到真实电机现象测试不是跑完就完要把结果映射回电机现象这样读源码才有意义。比如rotate测出来是正确的说明逆Park变换这个环节没问题如果实际电机在运行时电流波形畸变那就不应该怀疑Vector::rotate而应该去查输入的角度和dq电压是不是对的。再比如normalize的浮点舍入问题实际运行中的表现可能是这样的电机长时间高速旋转后电角度在一个周期边界处偶尔跳变几十毫秒电流波形出现毛刺。这种问题在低速短时间测试时根本不会出现只有长时间运转或者高速运转时才会暴露。你在PC上测过normalize之后就会知道根源在于角度累加别让值变得太大而不是normalize本身写错。_constrain的NaN穿透问题对应到真实场景就是PID参数整定不当导致输出发散然后电机突然剧烈抖动。如果你只盯着PID的参数去调可能要调很久如果你在数据链路里加一个NaN安全网能立刻把故障隔离到“PID发散”这一步。这就是底层函数边界行为带来的排查效率差异。5. 源码阅读中的常见问题与避坑清单5.1 弧度单位混用几乎每个人都翻过车SimpleFOC全库的角度单位都是弧度制这是毋庸置疑的。但实际使用中你从编码器读到的往往是脉冲数从某些控制器例程里拿到的可能是角度值。如果你在换算过程中少乘了π/180或者多乘了180/π就会出现一个很隐蔽的问题低速时电机还能转一提高速度电流相位就开始错乱电机发热效率明显下降甚至啸叫。排查方法比较简单在正常运行时打印shaft_angle和electrical_angle手动转电机一圈看打印值是不是从0变化到大约6.283而不是从0变化到360。如果是360那就是单位换算错了问题在foc_utils外面而不是这个文件本身。5.2 大角度累加fmod不是万能的前面已经提过float精度有限当角度值达到几百万时fmod的余数误差会变得很大。这里我给出一个更实用的建议不要在程序里把角度累加器设计成无限增长。每转一圈也就是每检测到一次角度超过2π就把累加值减去2π让角度始终保持在一个合理的范围内。这样_normalizeAngle只是为了应对偶尔的越界而不是天天去处理一个天文数字。我在调试某次高速运转时遇到过电机正常运行了几分钟后电流突然开始周期性波动后来发现是角度累加到了十几万弧度fmod之后误差到了0.01弧度量级对于高速电机来说0.01弧度的电角度误差已经足够让力矩产生明显波动了。问题解决之后我把所有角度累加点都加了周期自减逻辑这个现象再没复现过。5.3 NaN与Inf限幅函数拦不住的数据污染前面在_constrain那一节已经详细说过NaN会穿透限幅的问题这里把它放进避坑清单是因为我见过好几次类似案例电机在高负载下突然失控查了一圈发现是某个观测器在特定速度下算出了NaN然后一路传到了PWM输出。限幅看起来是在“保护”但它在NaN面前就是透明的。我建议在电流环和速度环的输出位置各加一个安全函数大致逻辑如下float _safe_float(float val) { if (isnan(val) || isinf(val)) return 0.0f; return val; }不要小看这一层它在故障时能把“电机猛烈抖动”变成“电机停一下”给你争取到断电排查的时间。同时配合串口或调试器打NaN日志能快速定位是哪个环节产出了非法数据。5.4 三角函数开销与替代方案的边界Vector::rotate内部用到了两次sin和两次cos在带FPU的M4或更高性能处理器上还好但在M0这样的无FPU内核上标准库的sinf/cosf调用开销非常可观。如果在10kHz电流环里做坐标变换一个rotate就有4次三角函数运算整个控制周期可能要吃掉很大比例的CPU时间。有些人会为了性能去换成查表法或者CORDIC近似但我建议先别急着改。查表法的精度和角度归一化时序会引入新的误差CORDIC在角度范围上和标准库也不太一致。如果你一定要优化先把_normalizeAngle调用做好确保传入rotate的角度已经落在[0, 2π)再去看近似算法否则查表函数对大角度外推的处理可能比标准库还差。性能优化有一个前提先保证行为和原实现对得上。5.5 源码版本差异v2.3.2与后续版本的变化最后提醒一点源码学习一定要盯准版本。SimpleFOC在v2.3.2和后续的v2.3.x、甚至3.x之间有区别某些函数的实现细节可能变了比如Vector结构体的位置可能被移动到独立头文件或者部分函数从cpp移到了头文件变成inline。你如果拿网上旧文章的思路来找函数可能找不到或者找到的代码长得不一样。最好是用git checkout到你正在学的那个tag再配合本地的代码搜索不要只看网上的截图和旧版走读。我自己在学的过程中就踩过这个坑有一次照着较早版本的笔记去找一个函数结果在当前tag里根本没有同名文件翻了好几层目录才确认名字变了。从那以后每次开始读源码前我都会先确认当前分支和版本号并且把所有include关系拉一遍这样后续阅读会顺畅很多。我个人从这轮源码阅读中最深的体会是foc_utils这层函数没有高深算法难的全在边界。角度要不要归一化、NaN会不会穿透限幅、矢量的临时变量为什么不能省这些东西单独看都很小但组合起来就是电机能不能稳定跑高速的关键。建议你也像我一样先把foc_utils在PC上跑一遍把边界条件打印出来再回过去看FOC主流程很多之前疑惑的现象会一下子通掉。下一步我准备把SVPWM里的扇区判断和空间矢量合成与这一层的Vector旋转串起来再写一篇把电压从dq到三相PWM的整条链路彻底打通到时候我们接着聊。
