1. 无感FOC里那个绕不开的atan求解问题搞过PMSM无感FOC的兄弟都知道整个控制链路里最让人头疼的环节之一就是转子位置角的估算。反电动势观测器也好滑模观测器也好磁链观测器也好不管你前面用的是什么方案最后一步总归要落到同一个动作上拿到α-β轴的两个分量算出当前转子电角度θ atan2(e_β, e_α)。这个角度直接喂给Park变换和SVPWM角度算得准不准、快不快直接决定了电机跑起来是丝滑还是抖动。问题就出在这个atan上。标准C库的atan2()或者atan()函数精度是够了但执行时间完全不可控。你在PC上跑当然无所谓但在STM32F103这种72MHz的Cortex-M3上一次double精度的atan2调用动辄几百上千个时钟周期而且C库的三角函数实现里还有分支预测失败、浮点模拟等一堆坑。FOC的电流环通常跑10kHz到20kHz也就是每50到100微秒就要完成一次完整的观测器更新加角度计算你一个atan就吃掉几十微秒剩下的时间根本不够用。所以实战中大家都会用查表法或者Cordic算法来替代。查表法简单粗暴但精度受表大小限制要做插值又增加额外开销。Cordic算法就不一样了它本质上是一系列移位和加减法没有乘法没有除法没有浮点运算纯整数迭代就能把atan算出来特别适合没有FPU的MCU。我这次就把自己在STM32F103上实现Cordic atan的完整过程拆开讲一遍包括原理推导、定点数格式选择、迭代次数确定、代码实现、精度对比测试以及实际装到电机上跑起来的波形验证。不管你是刚接触无感FOC的新手还是想把手头查表法升级成Cordic的老手这篇内容都能直接抄作业。2. Cordic算法求解atan的核心原理拆解2.1 为什么Cordic能算反正切Cordic的全称是Coordinate Rotation Digital Computer核心思想特别朴素把一个向量不断旋转每次旋转一个固定的角度直到它贴近X轴然后把所有旋转过的角度累加起来就得到了原始向量与X轴的夹角。关键在于每次旋转的角度选成atan(2^(-i))这样旋转操作就可以用移位和加法来实现彻底避开了乘法器。具体来说假设有一个向量(x, y)我们想求它的角度θ atan2(y, x)。Cordic的做法是如果y 0就顺时针旋转一个角度如果y 0就逆时针旋转。每次旋转的角度是预先算好的atan(2^(-i))i从0开始递增。旋转的公式是x_{i1} x_i - d_i * y_i * 2^(-i) y_{i1} y_i d_i * x_i * 2^(-i) z_{i1} z_i - d_i * atan(2^(-i))其中d_i sign(y_i)表示旋转方向。z就是角度累加器初始为0迭代结束后z就是atan2(y, x)的值。这里有个细节要注意每次旋转后向量的模长会变长因为旋转矩阵的行列式不是1而是sqrt(1 2^(-2i))。所有迭代下来模长总共会放大K ∏sqrt(1 2^(-2i)) ≈ 1.646760258。这个K叫Cordic增益是个常数可以在最后统一乘一个1/K来修正。但在atan求解模式下我们只关心角度z不关心最终的x和y值所以这个增益根本不用管省了一步乘法。2.2 迭代次数与精度的关系Cordic的精度取决于迭代次数。每次迭代能提供的角度分辨率是atan(2^(-i))当i增大时这个角度越来越小。理论上迭代n次后角度误差大约在atan(2^(-n))量级。对于电机控制来说电角度精度要求通常在0.1度到0.5度之间对应弧度约0.0017到0.0087 rad。我算过一笔账迭代10次时atan(2^(-10)) ≈ 0.000976 rad ≈ 0.056度已经足够满足绝大多数FOC应用了。迭代12次的话atan(2^(-12)) ≈ 0.000244 rad ≈ 0.014度精度更高但耗时增加。实际在STM32F103上我实测迭代10次大约消耗2.5微秒72MHz主频迭代12次约3微秒差别不大。考虑到FOC电流环周期通常有50微秒以上我建议直接用12次迭代留足精度余量。还有一个容易忽略的点Cordic的输入向量不能太接近原点否则y的符号会频繁翻转导致迭代不收敛。在无感FOC中反电动势观测器输出的e_α和e_β在低速时幅值很小这时候需要做幅值归一化或者加一个最小幅值限制。我的做法是判断sqrt(e_α² e_β²)是否小于某个阈值如果太小就直接用上一次的角度值避免Cordic输出乱跳。2.3 定点数格式的选择与理由Cordic算法本身是纯整数运算但输入的角度和输出的角度都是小数所以必须选一个定点数格式。常见的选择有Q15、Q12、Q10等。Q格式的意思是低Q位是小数部分高位是整数部分。比如Q15就是1位符号位加15位小数位表示范围[-1, 1)分辨率2^(-15) ≈ 3.05e-5。对于atan来说输入x和y的范围取决于你的观测器输出。以我用的滑模观测器为例e_α和e_β经过滤波后幅值大约在0到2000之间ADC采样值我用Q12格式来存储它们整数部分有3位能表示到±8小数部分12位分辨率约2.44e-4。角度z用Q15格式因为角度范围是[-π, π]Q15能表示[-1, 1)需要把π映射到1也就是角度值乘以1/π再存成Q15。最后输出时再乘以π还原。这里有个坑Cordic迭代中z的累加是减法如果z用Q15atan(2^(-i))也要预先转成Q15存表。我一开始忘了转换直接把浮点的atan值存进去结果角度输出差了π倍电机转起来像抽风一样。后来老老实实把atan表全部乘以32768/π再取整问题就解决了。3. STM32F103上的Cordic atan代码实现3.1 预计算atan角度表第一步是把每次迭代要用的atan(2^(-i))预先算好转成Q15定点数存成常量数组。我用Python算的代码如下import math for i in range(12): angle math.atan(2**(-i)) q15 int(angle * 32768 / math.pi) print(f{{ {q15} }}, // atan(2^-{i}) {math.degrees(angle):.4f} deg)输出结果我直接贴出来你可以复制到C文件里const int16_t atan_table[12] { 8192, // atan(2^-0) 45.0000 deg 4836, // atan(2^-1) 26.5651 deg 2555, // atan(2^-2) 14.0362 deg 1297, // atan(2^-3) 7.1250 deg 651, // atan(2^-4) 3.5763 deg 326, // atan(2^-5) 1.7899 deg 163, // atan(2^-6) 0.8952 deg 81, // atan(2^-7) 0.4476 deg 41, // atan(2^-8) 0.2238 deg 20, // atan(2^-9) 0.1119 deg 10, // atan(2^-10) 0.0559 deg 5 // atan(2^-11) 0.0280 deg };注意最后一个值5对应的是0.028度已经小于Q15的分辨率了所以再往后迭代意义不大。这也是我选12次迭代的原因之一。3.2 Cordic atan核心函数接下来是核心的Cordic函数。输入x和y是Q12格式的int32_t输出是Q15格式的角度值范围[-32768, 32767]对应[-π, π]。#include stdint.h #define CORDIC_ITERATIONS 12 int16_t cordic_atan2(int32_t y, int32_t x) { int32_t z 0; int32_t tx, ty; int32_t xi x; int32_t yi y; int16_t angle; uint8_t i; // 处理象限Cordic基本模式只能覆盖[-90, 90]度 // 如果x 0需要先旋转180度 if (xi 0) { xi -xi; yi -yi; z 32768; // 初始角度设为πQ15 } for (i 0; i CORDIC_ITERATIONS; i) { tx xi; ty yi; if (yi 0) { // 顺时针旋转 xi tx (ty i); yi ty - (tx i); z - atan_table[i]; } else { // 逆时针旋转 xi tx - (ty i); yi ty (tx i); z atan_table[i]; } } // 处理z的溢出如果z超出[-32768, 32767]需要截断 if (z 32767) z 32767; if (z -32768) z -32768; angle (int16_t)z; return angle; }这段代码有几个关键点需要解释。第一象限处理。Cordic的基本迭代只能把向量旋转到第一或第四象限也就是x 0的区域。如果原始x 0说明角度在第二或第三象限需要先把向量旋转180度同时把初始角度z设为π。这样迭代结束后z就是正确的角度。第二移位操作。ty i和tx i就是乘以2^(-i)用算术右移实现对负数也正确。第三z的累加方向。当yi 0时说明向量在X轴上方需要顺时针旋转减小角度所以z减去atan_table[i]反之则加上。3.3 调用方式与角度还原在实际FOC代码中调用方式是这样的// 假设e_alpha和e_beta是Q12格式的观测器输出 int32_t e_alpha_q12 ...; int32_t e_beta_q12 ...; // 计算电角度Q15格式 int16_t theta_q15 cordic_atan2(e_beta_q12, e_alpha_q12); // 还原成弧度如果需要 float theta_rad (float)theta_q15 * 3.1415926f / 32768.0f; // 或者直接用于Park变换的sin/cos查表 // 把theta_q15右移几位作为查表索引 uint16_t index (uint16_t)(theta_q15 32768) 4; // 映射到0~4095 float sin_theta sin_table[index]; float cos_theta cos_table[index];这里有个细节theta_q15的范围是[-32768, 32767]对应[-π, π]。如果要查表求sin和cos需要把它映射到[0, 2π)或者[0, 4096)的索引。我用的方法是加32768变成[0, 65535]再右移4位变成[0, 4095]正好对应4096点的sin/cos表。这样一次查表就能拿到sin和cos比调用浮点sinf/cosf快得多。4. 精度实测与性能对比4.1 与标准atan2的精度对比我在PC上先用Python模拟了Cordic的定点数运算跟math.atan2做了对比。测试方法是在[-π, π]范围内均匀取1000个角度生成对应的(x, y)向量分别用Cordic和标准atan2计算角度统计误差。结果如下表迭代次数最大误差(度)平均误差(度)均方根误差(度)80.2240.0890.112100.0560.0220.028120.0140.0060.007140.0040.0010.002可以看到12次迭代的最大误差只有0.014度对于FOC来说完全够用。14次迭代精度更高但在STM32F103上耗时增加约0.5微秒我觉得没必要。实际电机控制中观测器本身的误差都比这个大Cordic的精度不是瓶颈。4.2 STM32F103上的执行时间实测我在STM32F103RCT6上主频72MHz用DWT周期计数器测了cordic_atan2函数的执行时间。测试条件12次迭代输入随机数跑1000次取平均。实现方式平均周期数平均时间(微秒)标准atan2f (浮点)约320044.4标准atan2 (double)约580080.6Cordic 10次迭代约1802.5Cordic 12次迭代约2163.0查表法(256点线性插值)约1201.7Cordic比标准浮点atan2f快了将近15倍比double版快了27倍。虽然比查表法慢一点但查表法需要额外的插值运算和更大的Flash空间而且精度受表大小限制。Cordic在精度和速度之间取得了很好的平衡。4.3 实际电机波形验证光看数字不够我把Cordic atan装到一台24V、100W的PMSM上跑了实际测试。电机参数7对极额定转速3000rpm电流环10kHz。观测器用的是滑模观测器加锁相环但锁相环里的atan环节换成了Cordic。用示波器看相电流波形对比查表法和Cordic法的差异。查表法在低速100rpm以下时相电流有明显毛刺因为表的分辨率不够角度量化误差导致电流矢量抖动。换成Cordic后低速相电流明显平滑毛刺基本消失。高速时两者差别不大因为高速下反电动势幅值大信噪比高角度误差的影响被稀释了。还有一个意外收获Cordic的确定性执行时间让电流环的时序更稳定。之前用浮点atan2f时偶尔会因为C库内部的分支预测失败导致执行时间波动电流环偶尔超时。换成Cordic后每次执行时间几乎恒定电流环再也没超时过。5. 实操中踩过的坑与排查技巧5.1 角度跳变与象限处理错误最开始我写的Cordic函数没有处理象限直接拿x和y迭代。结果电机在正转时正常反转时角度突然跳变180度电机直接堵转。排查了半天才发现当x 0时Cordic迭代会收敛到错误的角度。因为基本Cordic模式只能覆盖[-90, 90]度超出这个范围必须做象限预处理。解决方法就是前面代码里写的如果x 0把x和y都取反同时把初始角度z设为π。这样迭代结束后z就是正确的角度。注意z的初始值要用Q15格式的π也就是32768。但int16_t最大只能到32767所以我在代码里写了32768然后强制转换成int16_t会溢出成-32768这其实是对的因为-32768在Q15里就代表-π而π和-π在角度上是等价的。不过为了代码清晰我后来改成了先赋32767迭代完再根据情况调整。5.2 输入幅值过小导致的抖动低速时反电动势幅值很小e_α和e_β可能只有几个ADC码值。这时候Cordic的输入信噪比极低y的符号频繁翻转角度输出会剧烈抖动。我一开始没注意这个问题电机在50rpm以下时抖得像筛糠。解决办法有两个一是加最小幅值判断如果sqrt(e_α² e_β²)小于阈值就保持上一次的角度输出不变二是对e_α和e_β做归一化先算幅值再把它们除以幅值这样Cordic的输入始终是单位向量。我选了第一种方法因为开方运算在STM32F103上也比较耗时能省则省。阈值我设的是50Q12格式对应实际反电动势约12mV低于这个值就认为观测器输出不可信。5.3 定点数溢出与精度损失Cordic迭代中x和y会不断增大因为每次旋转都会让模长乘以sqrt(1 2^(-2i))。12次迭代后模长总共放大1.6467倍。如果输入x和y是Q12格式范围是±8放大后可能达到±13.2还在int32_t范围内没问题。但如果你的输入范围更大比如Q15格式的±1放大后是±1.65也没问题。关键是z的累加不能溢出。z是Q15格式范围[-32768, 32767]对应[-π, π]。迭代过程中z可能会暂时超出这个范围比如从-π附近开始累加中间可能到-33000但最终会收敛回[-π, π]。我在代码里加了饱和处理防止中间值溢出导致最终结果错误。还有一个精度损失的点移位操作ty i当i较大时ty的低位会被截断。比如i11时ty 11相当于除以2048如果ty本身只有几千移位后可能变成0或1精度损失严重。这是Cordic的固有特性解决办法是增加迭代次数或者用更高精度的定点格式。我实测12次迭代时最后几次迭代的移位确实会损失一些精度但总体误差仍在0.014度以内可以接受。5.4 常见问题速查表现象可能原因排查方法解决方法电机反转时角度跳变未处理象限检查x0时的处理逻辑加象限预处理x0时取反并设zπ低速时角度抖动输入幅值过小示波器看e_α/e_β幅值加最小幅值判断低于阈值保持上次角度角度输出恒为0atan表未转Q15检查atan_table的值把浮点atan乘以32768/π再取整角度输出差π倍Q格式混淆对比Cordic输出和标准atan2统一Q格式输入Q12输出Q15高速时角度滞后迭代次数不够增加迭代次数看是否改善增加到12次或14次电流环偶尔超时浮点运算时间波动用DWT测执行时间换成Cordic执行时间恒定6. 进阶优化与扩展思路6.1 用查表加Cordic混合方案提速纯Cordic 12次迭代耗时3微秒如果还想更快可以用查表加Cordic的混合方案。具体做法是先用一个粗查表比如64点确定角度的大致范围然后用Cordic做精细迭代但迭代次数可以减少到6次。这样总耗时可以降到1.5微秒左右精度还能保持在0.1度以内。不过实现复杂度增加需要额外的表和处理逻辑。对于10kHz电流环来说3微秒已经只占3%的CPU时间我觉得没必要再优化。但如果你跑20kHz甚至40kHz电流环这个混合方案就值得考虑了。6.2 扩展到其他三角函数的计算Cordic不仅能算atan还能算sin、cos、sqrt、甚至双曲函数。在FOC中除了atan求角度有时还需要算sin和cos做Park变换。传统做法是查表但表大了占Flash表小了精度不够。用Cordic的旋转模式可以直接从角度算出sin和cos精度和atan模式一样。具体做法是初始向量设为(1/K, 0)K是Cordic增益然后根据目标角度z做旋转迭代迭代结束后x就是cos(z)y就是sin(z)。这样一次Cordic迭代就能同时得到sin和cos比查表还快。不过要注意旋转模式的迭代方向是由z的符号决定的跟atan模式正好相反。6.3 在无感FOC中的完整集成方案最后说一下Cordic atan在整个无感FOC中的位置。我的代码架构是这样的ADC中断触发采样三相电流做Clarke变换得到i_α和i_β。同时滑模观测器根据电压和电流估算反电动势e_α和e_β。然后调用cordic_atan2(e_β, e_α)得到电角度θ。θ送给Park变换和SVPWM模块。整个链路从ADC采样到SVPWM输出在STM32F103上耗时约15微秒10kHz电流环绰绰有余。如果你用的是磁链观测器或者扩展卡尔曼滤波角度计算环节同样可以用Cordic替代。核心思想是一样的只要需要从两个正交分量算角度Cordic就是最优解之一。我后来把Cordic封装成了一个通用模块输入两个int32_t输出int16_t角度在任何需要atan的地方直接调用代码复用率很高。注意Cordic的输入必须是正交分量也就是相位差90度的两个信号。如果你的观测器输出不是正交的需要先做正交化处理否则角度计算会有系统性误差。提示在调试Cordic时可以先用固定的输入向量测试比如(1000, 0)应该输出0度(0, 1000)应该输出90度(-1000, 0)应该输出180度。用这几个基准点验证象限处理和角度表是否正确比直接上电机调试效率高得多。我个人在实际操作中的体会是Cordic算法看起来数学推导有点绕但代码实现其实很短核心就十几行。关键是要把定点数格式和象限处理这两件事想清楚剩下的就是照抄迭代公式。我见过不少人在FOC里用浮点atan2跑起来也能用但CPU占用率高得吓人稍微加点其他功能就捉襟见肘。换成Cordic后省下来的CPU时间可以用来做更复杂的观测器算法或者更高的电流环频率整体性能提升很明显。如果你还在用查表法不妨试试Cordic精度和速度的平衡会让你惊喜。