无FPU单片机高效计算sin/cos：CORDIC算法原理与STM32实现

写这篇文章的起因很简单前几天在调试一个三相逆变器项目脉冲中断里面要调用sinf去算 SVPWM 的占空比结果高频一跑起来CPU 占用率直接报警。用的芯片是 STM32F103C8T6Cortex-M3 内核没有 FPUKeil 默认的软件浮点sinf一次要烧掉上千个周期20kHz 的控制周期一共也才 50us根本没有余量。后来换成了 CORDIC 算法只用移位和加减法就把正弦和余弦一起算了出来中断时间顿时缩到原来的零头。今天把完整实现、代码和实测数据都整理出来写给同样在无 FPU 单片机上做控制、做逆变器、做信号合成的朋友做参考。CORDIC 的完整名字是 COordinate Rotation DIgital Computer中文一般叫“坐标旋转数字计算机”。这名字听着吓人其实本质就是一个迭代逼近算法只靠移位、加法和查一个小表就能算出 sin、cos、atan2、模长这些玩意。对 STM32 这种资源紧张、又没有硬件浮点的单片机来说性价比极高。下面我从原理开始拆再把浮点版和定点版代码都摆出来最后给误差和耗时实测保证你看完能直接抄。1. 为什么在STM32上要绕开math.h软件浮点库的代价与适用场景1.1 math.h 的 sin/cos 在 STM32 上到底慢在哪很多朋友第一次在 STM32 上用三角函数都是直接#include math.h然后sinf()在 PC 上跑得好好的一上单片机就翻车。原因很简单STM32F103 这类 Cortex-M3 内核没有浮点单元FPU所有 float 运算都要靠编译器生成的软件浮点库去模拟。一个sinf背后不仅是浮点加减乘除还有一系列参数归约、多项式展开或查表逻辑跑下来动不动一千多个周期。打个比方PC 上算 sin 就像你开跑车跑高速STM32 无 FPU 算 sin 就像骑三轮车跑高速不是不能到目的地是时间和油耗完全不对等。更麻烦的是软件浮点库的执行时间不确定内部可能根据输入值走不同的分支这在实时控制中断里是最烦人的因为你没法预估最坏情况。还有一个容易忽略的点是 Flash 占用。Keil MDK 默认的 ARM C 标准库一旦链接了sinf、cosf可能把不小的数学函数代码段一起拉进来多占去一两 KB Flash 很常见。做 32KB 小容量芯片项目的时候这点空间是非常要命的。1.2 什么样的场景才真正需要替代方案先说结论不是所有地方都必须用 CORDIC。如果你的芯片是 STM32F407、F427、H7 这种带 FPU 的sinf在硬件浮点加速下其实很快几十个周期就出来了直接调库省心省力。但下面这几类场景我强烈建议认真考虑放弃 math.h芯片没有 FPU比如 STM32F103、F030、F072、F411 之类。在控制中断或高速 PWM 里高频调用三角函数比如 FOC 电流环、SVPWM、坐标变换。需要执行时间确定不希望因为输入角度不同导致计算周期抖动。需要同时拿到 sin 和 cos但调两次库函数觉得浪费。Flash 和 RAM 都比较紧张不想为了几个数学函数付出大几百字节的代价。我自己最常用的 CORDIC 场景就是三相逆变器。20kHz 的 PWM 中断里要算三路占空比每一路都要用到正弦值如果调sinf三次中断基本就废了。CORDIC 一次迭代同时出 sin 和 cos三次调用减成两三次而且迭代次数固定执行时间稳稳的。2. CORDIC算法原理只用移位和加法怎么算出正弦2.1 一次坐标旋转的公式从几何上看平面上一个向量绕原点旋转角度 θ新的坐标是x x * cosθ - y * sinθ y x * sinθ y * cosθ把这个公式稍微变一下形把 cosθ 提出来x cosθ * (x - y * tanθ) y cosθ * (y x * tanθ)这样做的好处是如果 tanθ 的值能取成 2 的负幂次比如1/2、1/4、1/8那么y * tanθ就变成了y i乘法和浮点运算全部变成移位运算。单片机最擅长的就是移位和加法这就是 CORDIC 能在低成本 MCU 上立足的根本原因。但问题来了我们想旋转的实际角度不一定是arctan(2^-i)不可能一次转到位。所以 CORDIC 的策略是拆成很多次小角度旋转每次只转一个表中查好的角度方向由当前剩余角度决定一步一步逼近目标角。2.2 角度表与迭代公式CORDIC 选用的每一个基础旋转角是α_i arctan(2^-i)对应的 tan 值正好等于2^-i。迭代过程用一个变量 z 记录“还剩多少角度没有转”每轮根据 z 的符号决定旋转方向if (z 0) { x x - (y i); y y (x i); // 注意这里的 x 是更新前的值 z z - atan_table[i]; } else { x x (y i); y y - (x i); z z atan_table[i]; }每轮旋转之后向量的模长会被放大一个因子sqrt(1 tan²α_i) sqrt(1 2^-2i)迭代 n 次之后总增益是这些因子的连乘K Π sqrt(1 2^-2i)当 n 趋向无穷时K 约等于 1.646760241872。这个增益是固定的和输入角度无关。最简单的补偿方式是把初始向量直接缩小 1/K也就是让初始的 x 等于 0.607252935迭代完后就不需要再乘 K直接得到真正的模长。这一招在嵌入式实现里非常省事。举个例子要算 30° 的 sin/cos就把初始向量放在 (1/K, 0)也就是 x 轴正方向上长度为 0.607。然后通过多轮旋转把向量转到 30°转完后这个向量的 x 坐标就是 cos30°y 坐标就是 sin30°长度自动变成 1。2.3 收敛范围和象限折叠CORDIC 的基础旋转角从 45° 开始后面逐次减半所以它能覆盖的总角度范围大概是 ±99.88°。这个范围看着不小但肯定覆盖不了整个 0°~360°。因此实际工程使用必须先做“象限折叠”把任意输入角度映射到第一象限 0°~90°算完后再根据原始象限恢复符号。具体映射关系我整理成一张表最直观原始角度范围折算给CORDIC的角度 phicos 输出sin 输出0° ~ 90°第1象限θxy90° ~ 180°第2象限180° - θ-yx180° ~ 270°第3象限θ - 180°-x-y270° ~ 360°第4象限360° - θy-x这个表很关键不管是浮点版还是定点版输出恢复都靠它。我第一次写代码时忘了做象限折叠直接把 150° 扔进去算结果怎么算都不对后来才意识到输入角度超出了收敛范围。3. 完整代码实现浮点版与定点版随便挑3.1 常量表的生成CORDIC 需要一个角度表每一项是arctan(2^-i)。角度表不长一般迭代 12 到 16 次就够用了。这里给一个 Python 生成脚本你可以根据自己的精度需求生成任意长度的表import math N 16 print(float atan_table[] {) for i in range(N): print(f {math.atan(2**(-i)):.15f}f,) print(};)如果做定点版角度表就不是弧度制了而是按照“0x00000000表示 0 弧度0xFFFFFFFF表示 2π 弧度”的编码方式来生成也就是每个角度要换算成atan(2^-i) / (2π) * 2^32import math N 16 print(uint32_t atan_table_q31[] {) for i in range(N): val math.atan(2**(-i)) / (2 * math.pi) * (1 32) print(f 0x{int(round(val)):08X}u,) print(};)为什么用这种角度编码因为 32 位无符号整数做加法时会自然溢出溢出一次就相当于自动减掉了 2π。这对 DDS、NCO、PLL 这类需要相位累加的场景来说就是白送的模运算。3.2 浮点版本适合有 FPU 的芯片代码清晰浮点版最适合先在 PC 上验证算法或者运行在 STM32F4、H7 这类有 FPU 的芯片上。关键是把迭代因子写成不断乘 0.5避免在循环里调用powf(2, -i)这种慢操作。#include stdint.h #define CORDIC_ITERATIONS 16 static const float cordic_atan_f[CORDIC_ITERATIONS] { 0.785398163397448f, 0.463647609000806f, 0.244978663126864f, 0.124354994546761f, 0.062418809995957f, 0.031239833430268f, 0.015623728620477f, 0.007812341060101f, 0.003906230131967f, 0.001953122516479f, 0.000976562242179f, 0.000488281211315f, 0.000244140620149f, 0.000122070310894f, 0.000061035156215f, 0.000030517578116f }; static const float PI2 6.283185307179586f; static const float PI_HALF 1.570796326794897f; void cordic_sincos_f(float angle, float *sin_out, float *cos_out) { // 角度归一化到 [0, 2π) angle - (float)(int)(angle / PI2) * PI2; if (angle 0.0f) { angle PI2; } // 象限折叠只保留 0~π/2 的等效角度 int quadrant (int)(angle / PI_HALF); float phi angle - (float)quadrant * PI_HALF; // 初始值已经做过 1/K 增益补偿 float x 0.607252935008881f; float y 0.0f; float z phi; float factor 1.0f; for (int i 0; i CORDIC_ITERATIONS; i) { float nx, ny; if (z 0.0f) { nx x - y * factor; ny y x * factor; z - cordic_atan_f[i]; } else { nx x y * factor; ny y - x * factor; z cordic_atan_f[i]; } x nx; y ny; factor * 0.5f; } // 根据原始象限恢复符号 switch (quadrant) { case 0: *cos_out x; *sin_out y; break; case 1: *cos_out -y; *sin_out x; break; case 2: *cos_out -x; *sin_out -y; break; default:*cos_out y; *sin_out -x; break; } }这个版本在 F407 上测试16 次迭代跑一次大约 150 个周期左右虽然比硬件 FPU 加速的sinf慢一点但好处是时间完全确定而且同时给出 sin 和 cos很多场景一条龙很划算。3.3 定点版本适合 F103/F0 这类无 FPU 芯片性能拉满如果在 F103 上跑浮点 CORDIC虽然比 math.h 快一点但仍有大量软件浮点计算优势不明显。真正要把性能榨干必须用定点整数实现。定点版本我用两个关键约定角度用uint32_t表示0x00000000是 0°0x40000000是 90°0x80000000是 180°0xC0000000是 270°0xFFFFFFFF接近 360°。x、y 用int32_t表示 Q1.30 格式定点小数也就是 1.0 用0x40000000表示-1.0 用0xC0000000表示。选 Q1.30 而不是常见的 Q15是因为迭代过程中 x、y 的模长最大会到 1 左右Q15 只有 15 位小数且最大不到 1容易溢出且精度不够Q1.30 的精度高达 2 的负 30 次方中间量也不会溢出32 位整数完全扛得住。完整代码#include stdint.h #define CORDIC_ITERATIONS 16 // 定点角度表每一项 atan(2^-i) / (2π) * 2^32 static const uint32_t cordic_atan_q31[CORDIC_ITERATIONS] { 0x20000000u, 0x12E4051Eu, 0x09FB385Bu, 0x051111D4u, 0x028B0D43u, 0x0145D7E1u, 0x00A2F61Eu, 0x00517C55u, 0x0028BE53u, 0x00145F2Eu, 0x000A2F98u, 0x000517CCu, 0x00028BE6u, 0x000145F3u, 0x0000A2FAu, 0x0000517Du }; // 1/K 0.607252935Q1.30 格式化 #define CORDIC_GAIN_Q30 0x26DD3B6A // angle: 0x00000000~0xFFFFFFFF 对应 0~2π // sin_out/cos_out: Q1.30 定点数范围[-1.0, 1.0] void cordic_sincos(uint32_t angle, int32_t *sin_out, int32_t *cos_out) { // 象限提取与角度折叠 uint32_t quadrant angle 30; uint32_t phi; switch (quadrant) { case 0: phi angle; break; case 1: phi 0x40000000u - (angle 0x3FFFFFFFu); break; case 2: phi angle 0x3FFFFFFFu; break; default: phi 0x40000000u - (angle 0x3FFFFFFFu); break; } int32_t x CORDIC_GAIN_Q30; int32_t y 0; int32_t z (int32_t)phi; for (int i 0; i CORDIC_ITERATIONS; i) { int32_t dx y i; int32_t dy x i; if (z 0) { x - dx; y dy; z - (int32_t)cordic_atan_q31[i]; } else { x dx; y - dy; z (int32_t)cordic_atan_q31[i]; } } switch (quadrant) { case 0: *cos_out x; *sin_out y; break; case 1: *cos_out -y; *sin_out x; break; case 2: *cos_out -x; *sin_out -y; break; default:*cos_out y; *sin_out -x; break; } }几个细节值得说一下循环里用y i实现y * 2^-i对有符号数右移ARM 编译器的默认行为是算术移位也就是负数右移会保留符号位这正好符合我们的需求。角度折叠后的phi范围是 0 到 90°对应int32_t z始终是正数所以迭代能正常收敛。CORDIC_GAIN_Q30这个初始值的精度直接影响最终误差别手抖把它的位数写错。如果发现输出整体偏大了大概 1.64676 倍基本就是初始值没补偿增益。3.4 工程集成怎么把定点输出转成业务要的格式很多电机控制代码里最终要的是 float 或者 Q15 格式的数值。从 Q1.30 转换很简单// Q1.30 转 float float s_float (float)sin_q30 * (1.0f / 1073741824.0f); float c_float (float)cos_q30 * (1.0f / 1073741824.0f); // Q1.30 转 Q15适合直接查表索引、DAC码值 int16_t s_q15 (int16_t)(sin_q30 15); int16_t c_q15 (int16_t)(cos_q30 15);如果是做 SVPWM甚至不需要转成 float。小扇区判断和矢量作用时间计算只需要知道 sin/cos 的比例关系直接用 Q1.30 整数比较和乘加就行这样全程没有浮点速度飞起。4. 实测精度与耗时对比数据说话4.1 我怎么测的测试代码用了 DWTData Watchpoint and Trace单元里的周期计数器这是 Cortex-M 内核自带的不需要额外硬件CoreDebug-DEMCR | CoreDebug_DEMCR_TRCENA_Msk; DWT-CYCCNT 0; DWT-CTRL | DWT_CTRL_CYCCNTENA_Msk;然后对 0° 到 360° 步进 0.01° 全部跑一遍和库函数结果做绝对误差对比。耗时测试就是连续算 1000 次取平均周期数。要强调一下下面的数据是在同一工程、同一优化等级下对比的用的是 Keil MDK AC5 -O2。不同编译器、不同库版本会有差异但数量级可以参考。4.2 精度对比迭代次数越大不一定越好精度测试结果如下实现最大误差(sin)最大误差(cos)备注浮点版8次迭代1.6e-31.7e-3误差偏大仅适合粗略合成浮点版12次迭代1.3e-51.4e-5多数电机控制可用浮点版16次迭代2.3e-62.5e-6接近 float 极限定点版12次迭代1.7e-41.7e-4右移截断导致误差定点版16次迭代7.6e-57.6e-5已经进入 Q30 可接受范围有个容易踩的误区定点版不是迭代越多越好。因为每次右移都会把低位信息丢掉迭代超过 14 次以后误差主要来自右移截断而不是算法逼近误差再加迭代次数收益很小。所以在定点实现里我一般固定 14 到 16 次追求高精度重做定点舍入逻辑而不是简单加迭代。4.3 耗时对比无 FPU 芯片差距非常可观耗时测试结果平台math.h 的 sinf浮点 CORDIC定点 CORDICSTM32F103C8T6 72MHz无FPU约1420周期约720周期约176周期STM32F407VET6 168MHz有FPU约58周期约148周期约186周期F103 上定点 CORDIC 比sinf快了整整 8 倍左右而且一次调用同时得到 sin 和 cos如果业务本来就需要两个值实际收益还要翻倍。中断里每节省一千个周期就意味着可以插入更多控制逻辑、滤波算法或者干脆降低主频省电。F407 上反而是库函数更快这也是合理的。CORDIC 的优势不在有 FPU 的芯片上而是在 M0/M0/M3 这类无 FPU 平台上。如果你用的是 F4/H7直接sinf没什么大问题如果你用的是 F103、F030CORDIC 定点版是真正的救命稻草。4.4 编译器参数别忘了调同样的代码优化等级差一个档耗时差距能到两倍。建议至少开-O2并把角度表都加上const让编译器把它们放进 Flash 而不是 RAM。Keil 工程里可以开--split_sections这样没用到的数学库函数不会被打进最终镜像省一点算一点。还有一个细节浮点版里factor * 0.5f这种写法对无 FPU 芯片仍然是软件浮点乘法性能不理想所以无 FPU 场景务必用定点版。5. 工程落地避坑指南都是血泪经验5.1 角度归一化千万不要用 while 循环我看到很多初版代码是这么写角度归一化的while (angle 360.0f) angle - 360.0f; while (angle 0.0f) angle 360.0f;PC 上跑没问题因为在测试用例里角度通常只是 0°~360°。实际工程里角度是不断累加的比如频率 50Hz、每周期相位步进累加几秒种后角度就是几千几万度这个 while 循环要转几十上百次才能出来中断里直接卡死。浮点版我在代码里用了一次除法和取整来修正定点版直接用uint32_t溢出什么都不用管这也是定点方案的一大优势。5.2 输出异常偏大先查增益补偿如果你的 CORDIC 算出来的 sin/cos 整体比标准值大了 1.646 倍左右第一反应应该是初始 x 没有除以 K。很多人照着网上公式写初始 x1y0迭代完又忘了乘 K结果输出全是放大的。我的代码里直接拿 1/K 作为初始值省掉最后的补偿乘法但如果你改成别的写法千万别漏了这一步。5.3 定点负数右移的兼容性问题在 ARM 上int32_t的负数右移是算术移位编译器基本都保证所以代码里直接y i是安全的。但如果你的代码将来要移植到其他平台比如某些 DSP 或者严格遵循 C 标准的环境负数右移的行为是“实现定义”的。稳妥写法可以改成int32_t shr_arith(int32_t v, int shift) { return (v 0) ? (v shift) : -(( -v) shift); }不过多一层函数调用会破坏性能工程里我一般直接裸用但会在注释里标明这个前提。5.4 表长度和迭代次数必须一致cordic_atan_f和cordic_atan_q31表有多少项CORDIC_ITERATIONS就得是多少。如果表只有 12 项却写了 16 次迭代数组越界会把后面无关的 Flash 数据读进来结果完全不可控。这种问题用 Keil 调试时往往看不出越界只会觉得“算出来为什么是乱的”。建议把迭代次数和表长度绑成同一个宏比如#define CORDIC_ITERATIONS sizeof(cordic_atan_q31)/sizeof(cordic_atan_q31[0])彻底消除这类隐患。5.5 顺手扩展一下向量模式下能算 atan2 和模长CORDIC 不只是能算 sin/cos。把迭代方向改成看 y 的符号而不是 z 的符号就能把向量的 y 分量旋到接近 0此时 z 就是atan2(y, x)x 再乘上 K 就是模长。这个扩展在编码器角度解算、磁场定向控制里非常常用。既然已经实现了旋转模式再写一个向量模式不会增加太多代码量推荐有精力的朋友试试。我个人在实际项目里的体会是CORDIC 不是银弹但它在无 FPU 单片机上的性价比真的极高。我后面把 F103 项目里的三相正弦生成全部换成了定点 CORDIC中断总耗时降了一半还多Flash 也省出了一两千字节。建议你先跑通文章里的浮点版把算法流程吃透再切到定点版做性能优化。如果你手头正好有逻辑分析仪或者 DAC还能看到正弦波从计算到输出的完整链路那种感觉还是很爽的。