CORDIC算法 arctan反正切计算原理及C语言定点实现

tilblackout

已于 2022-06-25 12:35:20 修改

阅读量1.1w

点赞数 27

分类专栏： FOC 嵌入式文章标签：嵌入式 c语言

于 2022-04-06 21:12:22 首次发布

本文链接：https://blog.csdn.net/tilblackout/article/details/123975335

版权

嵌入式同时被 2 个专栏收录

27 篇文章

订阅专栏

FOC

7 篇文章

订阅专栏

之前做FOC算法的时候，用到了TLV493D磁传感芯片通过两个相差90°的正弦曲线计算角度，所以需要计算arctan的值。这里就基于CORDIC(坐标旋转数字计算方法，Coordinate Rotation Digital Computer)理论，实现一种arctan的定点计算方法。

一、原理

坐标旋转

与FOC中的Park变换原理相同，CORDIC也是通过旋转坐标轴来求解反正切值的。

$x_{2} = x_{1}cos\theta - y_{1}sin\theta = cos\theta(x_{1}-y_{1}tan\theta)$

$y_{2} = x_{1}sin\theta + y_{1}cos\theta = cos\theta(y_{1}+x_{1}tan\theta)$

如果去掉cosθ，这样不会影响角度旋转的结果，只是x和y都被缩小了cosθ，这样能简化计算，这称为伪旋转。得到下式：

$x_{2} = x_{1}-y_{1}tan\theta$

$y_{2} = y_{1}+x_{1}tan\theta$

而CORDIC的核心则是令 $tan\theta ^{i} = 2^{-i}$ ，此时得到下式：

$x_{2} = x_{1}-y_{1}2^{-i}$

$y_{2} = y_{1}+x_{1}2^{-i}$

（1）为什么用 $2^{-i}$ 呢？

因为在实际代码中，可以使用右移操作来完成，能加快计算速度。

（2）这个方法能计算的角度范围？

上表中列出了5个θ值，若为无穷个呢？由 $tan\theta = 2^{-i}$ 可得 $\theta = arctan 2^{-i}$

$\pm \theta =\pm \sum_{0}^{\infty} arctan 2^{-i} \approx \pm 99.8829^{\circ}$

所以可以看出用这个方法能求的角度范围在[-99.8829°,99.8829°]。而在大多数场合，我们的计算范围是在[-180°,180°]。四个象限内的arctan都可以相互转换，且arctan是个奇函数，所以我们只需要将其它角度都转到[0°,45°]范围内计算，得到结果后变换一下即可。

迭代

迭代求arctan的方法的原理就是：每次旋转上面表中计算出的角度，累加这个角度，使y趋近于0。

y趋近于0的意思，可以把旋转坐标系想象为Park变换。如图所示，坐标轴在以表中的角度旋转，假设此时要求点(1,1)相对x轴的角度，即θ = arctan(1/1) = 45°，坐标轴第一次旋转，则为表中最大的45°，此时在旋转坐标系(下图黑色)中，(1,1)点变为( $\sqrt{2}$ ,0)。当然在计算的过程中，x最后不会是 $\sqrt{2}$ ，因为最开始我们为了方便计算将x和y都缩小了cosθ，但是一旦旋转到了我们期望的角度，y就是0。

因此，我们只需要不断让坐标轴旋转表中的角度，得到下式：

$x_{i+1} = x_{i}-d_{i} (y_{i}2^{-i})$

$y_{i+1} = y_{i}+d_{i} (x_{i}2^{-i})$

坐标轴在逆时针旋转时 $d_{i}=1$ ，顺时针旋转时 $d_{i}=-1$ 。其实就是因为 $tan(-\theta) = -tan\theta$ ，规定以逆时针旋转为正，顺时针旋转为负。

假设θ=30°，初始角度为0°。第一次逆时针旋转45°， $d_{i}=1$ 。此时45°＞30°，坐标轴要往回旋转26°， $d_{i}=-1$ ，此时角度为19°<30°，所以 $d_{i}=1$ ，就这样按表中的角度继续旋转，直到y趋近于0。

二、C语言实现

1、定义tan表格，即表示1、0.5、0.25.....的表格，采用Q15格式保存

#define DEPTH      16            //最大赋值为16
#define DEP_SHIFT (DEPTH-1)      //tan数组中的标幺值移位
static uint16_t _tan[DEPTH];
for(i=0; i<DEPTH; i++)
{
	_tan[i] = 1<<(DEPTH-i-1);    //减1,最大支持Q15表示
}
//如果DEPTH为16,则0号元素为32768,超过int16范围,后续会溢出,故最大元素减1
if(DEPTH == 16)
    _tan[0] -= 1;

2、定义angle表格

#include <math.h>

#define DIVPI 57.295779513082    //即弧度转角度,180/pi
#define ANG_AMP        64        //角度放大的倍数,理论上来说越大精度越高,建议设为2的倍数
#define ANG_PRECICE    100       //计算出来的结果是小数点后几位的,100表示后两位
#define ANG_MUL        (ANG_AMP*ANG_PRECICE)
static uint32_t _angle[DEPTH];   //tanθ = 2^(-i)时对应的θ值

double ang_tmp = 1;
for(i=0; i<DEPTH; i++)
{
	res = (int)(1.0 * atan(ang_tmp) * DIVPI * ANG_MUL);
	_angle[i] = res;
	ang_tmp /= 2;
}

在确定好求解深度DEPTH后，上述表格都可以先求出来放在一个const数组中，而不用每次初始化中计算。

由于FOC中使用的是int16_t来表示0~360°，故将宏定义定义为如下：

#define ANG_AMP        182.5487465181058
#define ANG_PRECICE    32

最后的结果右移5位赋值给int16_t变量，即为FOC表示的电角度。

3、CORDIC算法

（1）预处理:所有角度都转为第一象限的[0°,45°]范围内处理
①保证x、y都大于0，即位于第一象限
②保证x>y，否则交换，结果根据arctan(y/x)+arctan(x/y)=π/2得出

if(x < 0)
{
	x = -x;
	flag |= 1<<0;
}
if(y < 0)
{
	y = -y;
	flag |= 1<<1;
}
if(y > x)
{
	tmp = y;
	y = x;
	x = tmp;
	flag |= 1<<2;
}

（2）防溢出处理

不能直接将x、y用于计算，否则后面的乘法过程可能会超出int32变量的范围。

/* 限制范围在uint16范围内,后续乘法不会超过uint32范围,防止溢出 */
_y = ((y << DEPTH)-1)/x;    //前面保证x>y,所以这里_y<65535
_x = (1<<DEPTH)-1;          //即为1

（3）CORDIC迭代计算

/* 迭代计算 */
int x_tan,y_tan,_y,_x;
uint16_t *ptan = (uint16_t *)&_tan;    //指针访问比数组访问快
uint32_t *pang = (uint32_t *)&_angle;
uint32_t res = 0;

for(i=0; i<DEPTH ;i++)
{
    //由于x_tan和y_tan有正有负,设置为int类型,但最大值为31位,故前面的tan用Q15来表示
	x_tan = (_x * (*ptan)) >> DEP_SHIFT;   //Q15的tan * Q16的用户输入,计算后要右移Q15
	y_tan = (_y * (*ptan++)) >> DEP_SHIFT;
	if(_y > 0)
	{
		_x += y_tan;
		_y -= x_tan;
		res += *pang++;
	}else
	{
		_x -= y_tan;
		_y += x_tan;
		res -= *pang++;
	}
}

（4）角度还原

前面把所有的角度都转化为[0°,45°]范围内,这里要转化回去,输出[0°,360°]范围的角度

if(flag & 0x04)
{
    switch(flag)
    {
        case 7:
            res = 270 * ANG_MUL - res;
            break;
        case 4:
            res = 90 * ANG_MUL - res;
            break;
        case 5:
            res = 90 * ANG_MUL + res;
            break;
        case 6:
            res = 270 * ANG_MUL + res;
            break;
    }
}else
{
    switch(flag)
    {
        case 0:
            break;
        case 1:
            res = 180 * ANG_MUL - res;
            break;
        case 2:
            res = 360 * ANG_MUL - res;
            break;
        case 3:
            res = 180 * ANG_MUL + res;
            break;
    }
}

最终的结果除以ANG_AMP，即为精度为ANG_PRECICE的角度值。