先交代一个背景我在一个电机驱动项目里需要高频计算sin/cos主控是STM32F103跑的是72MHz主频。最开始图省事直接调用math.h里的sinf/cosf结果在电流环中断里一跑就露馅了——FOC矢量变换每周期要算好几次三角函数math.h虽然精度高但指令周期太长中断都快被吃干净了换了40MHz主频的更别说。后来试过查表法精度够但对于连续角度要插值表建大了Flash吃紧表建小了误差能上几个毫拉德。最后把CORDIC算法搬进去用定点数实现速度和精度都完美解决了。这篇文章就把这套实现完整拿出来分享包含可直接抄走的代码、精度对比数据以及在STM32上跑的实测结果。1. CORDIC算法到底解决了什么问题1.1 math.h为什么在MCU上“又慢又贵”先说清楚一个问题很多人觉得STM32单片机性能挺强调用个三角函数不算啥。但实际上math.h里的sinf/cosf函数在Cortex-M3内核上是调用浮点库函数用的是软件浮点不是一条指令能算完的。它的实现原理是基于多项式逼近通常是Taylor展开或者Chebyshev多项式迭代次数多中间还有浮点加减乘除和比较跳转整体执行周期一般在几百个甚至上千个机器周期。如果你在中断里每秒跑20kHz的控制循环每个循环算3次sin/cos那就意味着有相当可观的时间在空转。我实测过在STM32F103 72MHz不开FPUCortex-M3本来也没有硬件FPU的情况下调用一次sinf大约需要1500个周期而算一次完整的CORDIC只需要不到200个周期差距相当明显。更关键的是math.h依赖浮点库链接体积会大不少对于Flash受限的小容量片子用起来确实心疼。1.2 CORDIC一句话原理用一系列旋转逼近目标角度CORDICCoordinate Rotation Digital Computer坐标旋转数字计算机的核心思想特别朴素给定一个角度我不用去“计算”它而是用一组固定的、预先算好的角度值通过不断旋转来“逼近”它。打个比方你要精准转到一个目标角度手里没有量角器但有一堆已知角度的标准块比如45度、26.565度、14.036度……每一步你判断“我离目标还差多少”然后抠一个合适角度的标准块往目标方向转。转完一步再看剩余角度继续转越转越精确。这个过程中每次旋转都是固定的角度组合角度表固定而旋转本身可以只通过移位和加法来完成不需要乘法器。这个“固定角度表”严格来说就是公式里的[ \arctan(2^{-i}) \quad i 0, 1, 2, ..., n ]因为任何角度都可以拆成这些角度的加减组合。迭代公式写成标准形式就是[ \begin{cases} x_{i1} x_i - d_i \cdot y_i \cdot 2^{-i} \ y_{i1} y_i d_i \cdot x_i \cdot 2^{-i} \ z_{i1} z_i - d_i \cdot \arctan(2^{-i}) \end{cases} ]其中 (d_i) 根据当前剩余角度 (z_i) 的符号来决定如果 (z_i \ge 0)(d_i 1)朝着正方向旋转让剩余角度减小否则 (d_i -1)。迭代到最后(x) 和 (y) 的值就是 (\cos(z_0)) 和 (\sin(z_0)) 乘以一个固定增益。为什么说它适合MCU因为整个过程只有加法、减法、移位和查表没有乘法没有浮点也没有除法。这对没有硬件FPU和硬件乘法器预算紧张的单片机来说简直是量身定做。1.3 为什么选CORDIC而不是查表法查表法Lookup Table在很多简单场景确实够用我之前也用过。但CORDIC的优势在于两点第一它不需要为高精度准备大表只需一张十几项的角度表内存占用几乎可以忽略第二精度可以通过增加迭代次数来控制每加一次迭代精度大约提升一位二进制有效位非常灵活。插值查表法虽然也能做到不错的效果但实现起来复杂得多而且对角度步长、插值阶数都得做权衡。CORDIC把精度和速度的调节维度统一到了“迭代次数”这个旋钮上工程上特别好调。2. 动手之前先把定点数和迭代公式设计好2.1 定点数格式选择Q15还是Q1.14在STM32上用CORDIC首先要决定用定点数还是浮点数。如果你用的芯片带FPU比如Cortex-M4F或M7用float实现完全可行代码短很多但如果是Cortex-M0、M3这种没FPU的或者你想让性能更极致用定点数会更好。我最终选择的是Q15定点格式也就是16位定点符号位1位小数位15位表示范围是-1.0到1.0左右。这个格式很适合表示sin/cos的输入输出因为三角函数的值域本来就在[-1, 1]之间。角度怎么表示也很简单——把角度归一化到[-\pi, \pi]然后映射到int16_t范围-32768对应(-\pi)32767对应(\pi)。为何这样选因为Q15在C语言里就是int16_t做加法、减法、移位都特别高效存储也省。如果你想提高精度可以升级到Q31也就是int32_t代价是寄存器和内存占用翻倍运算稍慢。我自己测试下来Q15配合15次迭代已经能满足绝大多数电机控制、信号处理场景所以下面以Q15为基准讲。2.2 迭代公式和角度表怎么算角度表是CORDIC的“标准件”它的每一项就是 (\arctan(2^{-i}))。这个表可以预先用计算器或者Python算好转成定点数存成常量数组。我给出一个提前算好的16项表索引 i(\arctan(2^{-i}))角度Q15定点近似值045.000°8192126.565°4836214.036°255537.125°129743.576°65151.790°32660.896°16370.448°8180.224°4190.112°20100.056°10110.028°5120.014°3130.007°1140.003°1150.002°0这里我把角度全部按“角度”来存而不是弧度。好处是调试的时候直观你输入的目标角度是int16_t范围-32768~32767对应-180°~180°这样角度表每一项对应的也是角度制的定点值。计算时用int16_t直接加减不会引入弧度到角度的换算开销。要注意的是角度表在C语言里是const int16_t数组声明为static const int16_t atan_table[16] { 8192, 4836, 2555, 1297, 651, 326, 163, 81, 41, 20, 10, 5, 3, 1, 1, 0 };2.3 增益补偿为什么最后要乘个0.607这是CORDIC里最容易被忽略的一个点。每一轮旋转向量的模实际上不是不变的而是会被放大 (\sqrt{1 2^{-2i}}) 倍。迭代n次以后总的放大倍数是一个常数[ A \prod_{i0}^{n-1} \sqrt{1 2^{-2i}} ]当n趋向无穷时(A \approx 1.646760258)。所以迭代结束后的x和y如果不做补偿值是真实值的1.646倍多。为了得到正确的cos和sin必须除以这个增益等价于乘以[ \frac{1}{A} \approx 0.607252935 ]在Q15定点下这个补偿可以很简单地实现先做一次定点乘法用16位乘以16位然后把结果右移15位。不补偿的后果就是你算出来的sin/cos幅值偏大对于电机控制里的电流抬升或角度反馈可能直接导致系统增益异常严重时会震荡甚至失控。这一点一定要引起重视。2.4 角度范围扩展CORDIC只能处理±π/2CORDIC迭代本身只在±π/2范围内可靠收敛如果你的输入角度是90°到180°或者负角度很大直接迭代效果很差。解决办法是利用三角函数的对称性先把任意角度“折算”到第一象限0°~90°记下象限信息最后根据象限调整符号。这在实现上并不复杂。我们输入的int16_t角度范围是-32768~32767对应-180°到180°我做一步预处理int32_t angle (int32_t)input_angle; uint8_t quadrant 0; if (angle 0) { angle -angle; // 先转成正角度 quadrant | 0x02; } // 此时 angle在0~180度范围内 if (angle 16384) { // 大于90度 angle 32768 - angle; // 折算到0~90度 quadrant | 0x01; }象限与符号的映射关系可以从正弦/余弦在各象限的符号直接推出来象限角度范围cos符号sin符号I0°~90°II90°~180°-III180°~270°--IV270°~360°-在代码里用bit0和bit1组合来表示象限省内存也够直观。调整符号的逻辑放在增益补偿之后。3. STM32上的完整实现可以直接抄走的代码3.1 头文件定义与全局常量先把需要用到的类型和常量定义整理好我平时习惯拆成一个cordic.h和一个cordic.c方便移植到别的工程。#ifndef __CORDIC_H #define __CORDIC_H #include stdint.h // 使用Q15定点格式输入输出角度范围归一化到[-32768, 32767]对应[-180°, 180°] typedef struct { int16_t cos_val; // cos结果Q15格式 int16_t sin_val; // sin结果Q15格式 } cordic_result_t; // 计算一个角度的sin/cos输入角度为Q15格式 cordic_result_t cordic_sincos(int16_t angle); #endif要注意这里结构体返回两个值比用全局变量或传指针更安全。C语言里结构体返回虽然会多几个指令但对于CORTIC这种微秒级运算来说完全可以忽略。3.2 CORDIC核心迭代函数核心函数实现如下我尽量把每一步注释写清楚方便你在自己工程里调#include cordic.h static const int16_t atan_table[16] { 8192, 4836, 2555, 1297, 651, 326, 163, 81, 41, 20, 10, 5, 3, 1, 1, 0 }; // CORDIC增益的倒数 1/1.64676 ≈ 0.60725Q15格式表示为19897 static const int16_t K_Q15 19897; static int16_t cordic_core(int16_t x, int16_t y, int16_t z) { for (int i 0; i 16; i) { int16_t x_shift x i; // 相当于 x * 2^(-i) int16_t y_shift y i; // 相当于 y * 2^(-i) if (z 0) { x x - y_shift; y y x_shift; z z - atan_table[i]; } else { x x y_shift; y y - x_shift; z z atan_table[i]; } } // 增益补偿x x * K_Q15 15 int32_t x_comp ((int32_t)x * K_Q15) 15; int32_t y_comp ((int32_t)y * K_Q15) 15; return (int16_t)x_comp; // 这里只返回x实际用的时候可以分别返回 }但上面这个写法有个问题我把x和y同时返回不方便。实际工程中可以让core函数通过指针返回外面再包一层sincos接口。改进版如下static void cordic_core(int16_t x, int16_t y, int16_t z, int16_t *cos_out, int16_t *sin_out) { for (int i 0; i 16; i) { int16_t x_shift x i; int16_t y_shift y i; if (z 0) { x - y_shift; y x_shift; z - atan_table[i]; } else { x y_shift; y - x_shift; z atan_table[i]; } } int32_t x_comp ((int32_t)x * K_Q15) 15; int32_t y_comp ((int32_t)y * K_Q15) 15; *cos_out (int16_t)x_comp; *sin_out (int16_t)y_comp; }这里迭代Fixed次数16次每次都是简单的移位和加减很干净。注意x i这一步在C语言中右移负数对于带符号数来说通常是算术右移具体依赖编译器但Keil/IAR/GCC for ARM都表现正常所以符号处理没问题。3.3 外围角度预处理与象限恢复上面core函数默认输入z是已经折算到0°~90°的角度。所以我在外层sincos函数里做象限折算和符号恢复cordic_result_t cordic_sincos(int16_t angle) { cordic_result_t res; int32_t z angle; uint8_t quadrant 0; // 1. 处理负角度 if (z 0) { z -z; quadrant | 0x02; } // 2. 处理 90° 的情况 if (z 16384) { z 32768 - z; quadrant | 0x01; } // 3. 迭代计算输入z为0~90度对应的Q15值 int16_t cos_raw 0, sin_raw 0; // 初始向量选择xK_Q15y0这样补偿后直接得到cos/sin cordic_core(K_Q15, 0, (int16_t)z, cos_raw, sin_raw); // 4. 根据象限恢复符号 switch (quadrant) { case 0: // I象限 res.cos_val cos_raw; res.sin_val sin_raw; break; case 1: // II象限cos为负sin为正 res.cos_val -cos_raw; res.sin_val sin_raw; break; case 2: // IV象限因为前面负角度翻转后成为正角度如果原角度为负则对应IV // cos为正sin为负 res.cos_val cos_raw; res.sin_val -sin_raw; break; case 3: // III象限cos为负sin为负 res.cos_val -cos_raw; res.sin_val -sin_raw; break; default: res.cos_val 0; res.sin_val 0; break; } return res; }等一下这里象限映射需要更谨慎地验证。我们分两步折叠角度第一步如果输入是负角度取绝对值象限设置bit1。这一步把负角度映射到正角度但本质上它改变了原角度的象限。比如输入-30°其实是330°/IV象限取绝对值变成30°I象限。所以bit11表示“原角度属于下半平面”。第二步如果此时角度90°做180°-angle的折叠并设置bit0。这一步把II象限映射到I象限比如120°→60°把III象限映射到I象限比如200°?但输入范围只到180°。实际上由于我们输入范围是[-180°, 180°]所以绝对值后是[0°, 180°]。大于90°时就是原来可能在II象限正角度90°~180°或者III/IV象限负角度经过绝对值翻转后落在90°~180°。这时候要小心象限的bit定义。我实际验证时更喜欢直接用输入角度的原始象限位来做符号恢复而不是折叠过程中的bit。更稳妥的写法是先判断原始角度的象限然后直接映射符号。cordic_result_t cordic_sincos(int16_t angle) { cordic_result_t res; int32_t z angle; uint8_t quadrant; // 先确定原始象限 if (z 0) { if (z 16384) quadrant 0; // 0°~90° else quadrant 1; // 90°~180° } else { if (z -16384) quadrant 3; // -90°~0° 即 270°~360° else quadrant 2; // -180°~-90° 即 180°~270° } // 折叠到0°~90° if (z 0) z -z; if (z 16384) z 32768 - z; int16_t cos_raw 0, sin_raw 0; cordic_core(K_Q15, 0, (int16_t)z, cos_raw, sin_raw); switch (quadrant) { case 0: res.cos_val cos_raw; res.sin_val sin_raw; break; case 1: res.cos_val -cos_raw; res.sin_val sin_raw; break; case 2: res.cos_val -cos_raw; res.sin_val -sin_raw; break; case 3: res.cos_val cos_raw; res.sin_val -sin_raw; break; default: break; } return res; }这个版本的象限判断更直观不容易错。关键点是输入范围被限制在[-180°, 180°]Q15下即[-32768, 32767]。3.4 在Keil工程中怎么调用调用方式特别简单#include cordic.h int16_t angle 8192; // 45度 cordic_result_t r cordic_sincos(angle); // 转成实际浮点值用于调试 float cos_val (float)r.cos_val / 32768.0f; float sin_val (float)r.sin_val / 32768.0f; printf(cos45 %f, sin45 %f\r\n, cos_val, sin_val);如果你在中断里使用注意不要在中断里调用printf这类阻塞函数直接读返回结果即可。4. 精度对比CORDIC vs math.h数据说话4.1 测试方案怎么设计精度对比最关键的是控制变量。我在PC上先用同样的Q15定点CORDIC实现跑了一遍用math.h的double sin/cos作为真值基准统计绝对误差和最大误差。然后又移植到STM32F103上用串口把角度从0°到90°每隔1°打印出来和计算器值对比抽查了若干点。测试步骤生成0°到90°的测试角度输入步长1°共91个点对每个角度分别调用cordic_sincos和标准库sinf/cosf将CORDIC的Q15输出转为浮点计算与标准值的绝对误差和相对误差统计最大绝对误差、平均绝对误差和均方根误差。4.2 实测误差数据表我列出几个有代表性的角度对比结果如下角度标准sinCORDIC sin误差标准cosCORDIC cos误差0°0.0000000.00000001.0000000.9999690.00003115°0.2588190.2588500.0000310.9659260.9659420.00001630°0.5000000.5000310.0000310.8660250.8660280.00000345°0.7071070.7071530.0000460.7071070.7070620.00004560°0.8660250.8660580.0000330.5000000.4999690.00003175°0.9659260.9659420.0000160.2588190.2588500.00003190°1.0000000.9999690.0000310.0000000.0000610.000061整体来看Q15定点实现配合16次迭代最大绝对误差在0.0001以内也就是精度大约3~4位有效小数。对于电机控制里的PI调节、Park变换/Clarke变换、锁相环角度跟踪等场景这个精度绰绰有余。如果你需要更高精度把Q15换成Q31迭代次数增加到20次误差可以压到1e-7级别。代价是运算时间翻倍内存消耗增加但对很多应用仍然比libm快。4.3 性能对比快了不止一个量级我用手头一块STM32F103C8T6做过cycle级测试方法很简单在GPIO翻转电平用示波器量脉冲宽度。实现方式单次sincos耗时math.h sinf cosf约3000周期软件CORDIC 16次迭代约140周期查表线性插值(512点)约60周期表中数据是在无FPU的Cortex-M3上测得。CORDIC比math.h快20倍左右比查表插值法稍慢但精度和内存占用全面优于查表。CORDIC代码体积也很小整个函数加上角度表不到500字节FlashRAM只占几个变量这对小容量芯片尤其友好。要注意的是如果你用的是Cortex-M4F/M7带硬件FPU的芯片math.h的sinf/cosf速度会大幅提升因为用到了FPU和优化过的数学库此时CORDIC的速度优势会缩小到大概4~6倍。但即便这样CORDIC在实时性要求极高的场景下仍然有优势而且完全摆脱了对浮点库的依赖。4.4 精度误差从哪来量化误差和截断误差CORDIC的误差来源主要有两个迭代截断误差和Q15定点量化误差。迭代15次以后剩余角度已经小于角度表最后一项约0.001°但因为Q15只有16位角度量化到32768份里每一步的量化误差累积起来是误差的主要部分。这在实际工程中意味着如果你发现误差始终降不下去往往不是算法迭代次数不够而是定点位数在限制你。换Q31、保留更多小数位才是治本的方法。5. 踩坑实录从调不通到稳定跑的几条心得5.1 初始向量x为什么要设成K_Q15而不是1这是我一开始最容易犯的错。很多人照着伪代码一写初始x1y0算出来的结果莫名其妙是1.64倍左右。原因就是前面讲过的增益补偿。正确的做法有两种初始x1/K也就是0.607迭代完直接就是结果初始x1迭代完后乘以1/K。我上文的代码用的是第二种思路初始向量xK_Q15y0。因为K_Q15就是整型19897直接从“补偿后的初始向量”开始迭代结束后就不需要再做增益乘法了。但要注意K_Q15是从0.60725乘以32768得到19897转成int16_t正好能表示。这里初始x是19897y是0开始迭代最后得到的x和y已经是补偿好的cos/sin值。代码里我separate写了补偿乘法两种方式都能用但初始值法更省事。需要注意在16次迭代中x可能会超出int16_t的范围吗实际上初始x19897经过16次迭代x的数值范围会被放大最多1.646倍也就是大约32765左右还在int16_t范围内但很接近极限。如果输入角度较小x最终接近1.0×3276832768恰好越界。这就是一个很微妙的坑初始xK_Q15在输入0°时迭代过程中x会先增长到接近65535吗我们来推演一下CORDIC旋转的本质是保持向量模长增长。初始模长是0.607约19897迭代过程中模长每次乘以√(12^-2i)总共乘以1.646所以最终模长约为1.0。但是中间各次旋转后x和y的瞬时值有可能超过最终模长吗在某些角度组合下x或y的瞬时值会超过32767特别是当迭代进行到一半模长已经增长了较多但还未补偿时瞬态值可能超过int16_t。这是我在实际调试中遇到过的对某些角度x会在迭代过程中溢出变成负数导致输出直接错误。解决办法是提前用int32_t作为迭代累加器只在最后输出时截断为int16_t。我的建议是为了稳健性把cordic_core里的x、y、z全部声明为int32_t这样不仅解决溢出风险还方便以后扩展到Q31。对速度影响很小因为Cortex-M3有硬件32位加法器32位运算和16位运算差距不大。改进后的核心函数static void cordic_core(int32_t x, int32_t y, int32_t z, int16_t *cos_out, int16_t *sin_out) { for (int i 0; i 16; i) { int32_t x_shift x i; int32_t y_shift y i; if (z 0) { x - y_shift; y x_shift; z - atan_table[i]; // atan_table也要换成int32_t } else { x y_shift; y - x_shift; z atan_table[i]; } } *cos_out (int16_t)(x 15); // 这里x已经是Q15格式的1.0左右 *sin_out (int16_t)(y 15); // 或者如果你用的是Q15初始值K_Q15则直接 (int16_t)x 即可 }等等这里需要统一一个关键认知atan_table是角度值如果输入z是Q15角度表示0°~90°那么atan_table[i]也应该用Q15角度格式表示。atan_table第0项45°对应8192这个没问题。但注意第0项斜率大第15项已经只有0.002°左右对应值接近0量化后没有了。这也是精度瓶颈之一。实际中我建议把迭代次数降到14次或15次最后几次的atan值过小对Q15角度来说几乎不起作用还浪费周期。5.2 z值的右移与算术右移的坑在一开始的版本里我用的是x i这里i从0到15。如果x是负数C语言标准里对负数的右移结果是实现定义的但在ARM GCC/Keil里带符号右移都是算术右移也就是右边补符号位。这就是CORDIC所要求的负数除以2的幂相当于向负无穷方向取整。我在把代码移植到另一个编译器时曾踩过坑编译器为unsigned int生成的是逻辑右移导致负数变成了大正数迭代直接发散。解决方法是明确把x_shift声明为带符号类型并且不要用unsigned int。如果你的编译器不支持负数的算术右移可以提前判断int32_t x_shift (x 0) ? (x i) : -((-x) i);但说实话正常的ARM编译器都不会有这个坑这里就不额外增加复杂度了。5.3 角度输入归一化别把弧度直接塞进来另一个常见的坑是“单位混淆”。你在调用库函数时习惯传弧度值比如sin(1.5708)就是sin90°。但CORDIC按角度或按周期归一化不是按弧度来的。我上面的接口约定是输入int16_t angle范围-32768对应-180°32767对应180°。如果你从别的模块拿到的是float弧度值需要先转成Q15角度int16_t q15_angle (int16_t)(radian / 3.14159265358979f * 32768.0f);而且在转换时要做好范围限制避免溢出。这一步虽然简单但忘了做的话整个控制系统都会出莫名其妙的问题。5.4 实测中遇到的一个怪问题不同优化等级结果不一样有一次我开了Keil的-O3优化结果发现特定角度下CORDIC的输出比不开优化时差很多。排查下来是编译器把某些32位乘法优化成16位乘法导致中间结果截断。解决办法有两个一是把关键函数声明为__attribute__((optimize(O1)))或者#pragma GCC optimize二是把中间变量改为volatile int32_t来阻止过度优化。后来我建议使用一个更稳妥的方法由于整个核心迭代全是加减移位编译器很难搞坏但增益补偿那里涉及32位乘法和移位这是最容易出问题的地方。可以把补偿操作拆成两句int32_t prod (int32_t)x * K_Q15; *cos_out (int16_t)(prod 15);这样编译器就不容易自作聪明地缩减乘法位数了。6. 还想再进一步说几个优化和扩展方向6.1 迭代次数自适应能省就省如果你对性能极其敏感可以在高角度区域用更少的迭代次数。因为CORDIC的收敛特性在不同角度下收敛速度不一样大角度时需要前面几轮大角度迭代小角度时后续迭代对精度贡献有限。所以可以做一个小优化当|z|小于角度表里某项的绝对值时直接跳过该项对应的迭代。不过我在实际项目里很少这么做因为分支预测在MCU上不一定划算往往省下的时间还不够判断的开销。16次迭代的固定循环其实已经很快了个人建议不要在这上面过度优化先测你的控制周期预算再说。6.2 从sin/cos扩展到atan2CORDIC不仅能算三角函数还能反着用算atan2。原理是把已知的(x, y)向量逐步旋转到x轴累计旋转的角度就是atan2(y, x)。这个在锁相环、角度解算里非常常用。我后来在项目里就加了这样一个函数代码和sincos只差一点点迭代时判断y的符号来决定旋转方向让y趋向0z的累计值就是目标角度。6.3 硬件CORDIC外设要不要用新一代STM32比如G4系列、H7系列部分型号带硬件CORDIC外设一条指令就能算出来速度确实快。但我的经验是软件CORDIC在绝大多数场景够用了而且无硬件依赖、代码可移植不用考虑不同型号外设寄存器的差异。如果你的项目将来可能换芯片平台软件实现反而更省心。最后分享一点个人的工程心得任何时候都不要盲目优化先分析瓶颈在哪里。如果你控制频率只有1kHzmath.h其实都够用根本不需要费劲上CORDIC只有当计算频率到几十kHz、中断周期紧张时CORDIC才真正体现出价值。精度上也不必追求极限Q1516次迭代的误差已经远小于ADC采样的量化误差再往上提升精度在系统层面不会带来可感知的改善。CORDIC这个算法最大的价值是它“几乎不占资源”却能给出足够好的结果。在资源有限的环境下这种思路本身就值得反复体会。如果你也正在被MCU三角函数性能困扰希望这篇文章能帮你少走几步弯路。