使用Cordic算法在定点MCU中实现快速计算反正切函数atan()和atan2().docxVIP

使用Cordic算法在定点MCU中实现快速计算反正切函数atan()和atan2() ——写代码的Tobem 在一篇Microchip技术文档中，简单提到了一种计算反正切函数atan()的方法，说该方法不仅快速，耗的内存还小。这立马吸引了我的注意力。本人从事电机控制多年，随着降成本压力的增大，之前的带FPU的浮点MCU慢慢被替换为定点MCU。而对于线性霍尔电机控制方案，难免要使用atan2()进行电机电角度的计算，如果使用数学库math.h中的atan2f()函数进行计算，算力开销会很大，特别是高速电机的场合下，因为这时需要更高的控制频率，电角度更新频率也要更快。我们知道atan2()是可以通过atan()来计算的，如果解决了atan()快速计算的问题，也就解决了atan2()快速计算的问题。 于是便开始查找文档中提到的CORDIC算法。CORDIC(Coordinate Rotation DIgital Computer) 算法，只利用移位和加减运算，就能计算常用三角函数值，如Sin，Cos，Sinh，Cosh等函数，当然，也包括前面提到的反正切Atan。Cordic算法的思路很简单，在坐标系中，将（X，Y）旋转一定的度数，如果旋转之后纵坐标Y变为了0，那么旋转的角度就是我们要求解的反正切角θ。但是我们事先并不知道要旋转多少角度，才能让旋转后的纵坐标Y=0。因此，我们每次旋转某一固定角度，每次旋转角度都为前一次旋转角度的一半，并且需要根据纵坐标Y的数值正负进行旋转方向的选取，如此，经过多次旋转之后，就能让y趋近于0，而每次旋转的角度累加出来的结果就是我们要求取的反正切角θ了。举例说明如下： 平面上一点在直角坐标系下的坐标（X,Y）=（100，200），则θ=atan(Y/X)。 设（x,y）是原始坐标点，将其以原点为中心，顺时针旋转θ之后的坐标记为（x’,y’）,则有如下公式： 写成矩阵形式： 借鉴二分法的思想，我们每次旋转角度为前一次的一半，如果Y为正，θ为正；Y为负，θ为负；如果Y=0，则说明总的旋转角度刚好为θ。令第一次旋转角度为45°： 旋转之后纵坐标Y为70.711，大于0，说明旋转的度数不够，接着再旋转22.5度（45度的一半）: 这时总共旋转了45+22.5=67.5度。结果纵坐标变为了负数，说明θ67.5度，这时就要往回转，还是二分查找法的思想，这次旋转11.25度: 这时总共旋转了45+22.5-11.25=56.25度。又转过头了，接着旋转，这次顺时针转5.625度： 这时总共旋转了45+22.5-11.25+5.625=61.875度，可以看出，这时纵坐标已经接近0了。我们可以继续旋转下去，旋转次数越多，旋转之后的纵坐标Y越接近0，也就是θ精度越高，但是，当然耗时也越长。通常，我们只需要达到一定精度即可，以便减少旋转次数，加快算法的运行。旋转的过程图示如下： 前述演示过程中，用到了 sin 函数和 cos 函数，在定点MCU中，仍过于复杂。为了进一步简化过程，我们可以提取因数cos，则： 由于我们只需要求取角度θ，因此可以进一步去掉cos，这样，旋转的角度保持不变，只是模长发生了变化。 由于cos在0~1之间，因此模长是变长的。 具体软件中如何实现呢？由于我们每次旋转角度都为前一次的一半，令第一次旋转角度为45°，因此，我们只需要提前计算好tan(45)、tan(45/2)、tan(45/4)……，并把结果存到一个数组中，然后根据上式进行迭代计算至需要的精度即可。但是这种方式中，还存在乘法运算，而我们知道，定点MCU中的移位运算要快速得多，因此实际代码实践中，会稍微进行修改。第一旋转仍为45°(atan(1))，第二次，我们不是旋转(45/2)°,而是旋转atan(1/2)，第三次旋转atan(1/4)，第四次旋转atan(1/8)……这样，我们就可以把乘法运算变为移位运算，大大提高了运算速度。 我们把旋转方向考虑进来： Cordic算法可以快速计算出atan()函数，其返回值在-Pi /2~ +Pi/2之间，而实际计算角度时， 我们往往使用的是atan2()函数，其返回值在-Pi ~ +Pi之间。根据atan2()和atan()之间的联系，可以通过使用atan()计算出atan2()，即如果是在二三象限的矢量，则需要先旋转90度或者180度到一四象限，然后再迭代。具体C语言代码实现见附件如下： 使用定点MCU进行测试，例如，使用STM32F103进行烧录测试，通过Systick计算 Cordic算法和数学库atan2f()各自计算289次的总耗时，得到： 可以看到，使用Cordic算法计算atan2()函数，289次总耗时为27760个Systick，而数学库 的atan2f()则需要57126个Sy