FPGA算法学习(1) -- Cordic（圆周系统之旋转模式）

三角函数的计算是个复杂的主题，有计算机之前，人们通常通过查找三角函数表来计算任意角度的三角函数的值。这种表格在人们刚刚产生三角函数的概念的时候就已经有了，它们通常是通过从已知值（比如sin(π/2)=1）开始并重复应用半角和和差公式而生成。

现在有了计算机，三角函数表便推出了历史的舞台。但是像我这样的喜欢刨根问底的人，不禁要问计算机又是如何计算三角函数值的呢。最容易想到的办法就是利用级数展开，比如泰勒级数来逼近三角函数，只要项数取得足够多就能以任意的精度来逼近函数值。除了泰勒级数逼近之外，还有其他许多的逼近方法，比如切比雪夫逼近、最佳一致逼近和Padé逼近等。所有这些逼近方法本质上都是用多项式函数来近似我们要计算的三角函数，计算过程中必然要涉及到大量的浮点运算。

在缺乏硬件乘法器的简单设备上（比如没有浮点运算单元的单片机或FPGA），用这些方法来计算三角函数会非常的费时。为了解决这个问题，J.Volder于1959年提出了一种快速算法，称之为CORDIC(Coordinate Rotation Digital Computer) 算法，这个算法只利用移位和加减运算，就能计算常用三角函数值，如Sin，Cos，Sinh，Cosh等函数。

CORDIC可以应用于圆周系统、线性系统和双曲系统等，在不同的系统内解决不同的复杂计算问题。圆周系统中解决三角函数计算问题，线性系统中解决乘法和除法问题。

1. 基本思路

通常，旋转模式常用来解决三角函数的问题，体现的应用主要是极坐标向直角坐标转换，即已知一点的极坐标(α,r)，求其直角坐标(x,y)，实际上是求sinα、cosα或者tanα。
为了便于理解，开头先抛出cordic算法的实质，即类似于二分法加旋转的概念，下面来解释。

如图1.1所示，在单位圆上，向量OP与X轴的正半轴夹角为α，故P点的坐标可表示为：

将向量OP顺时针旋转θ角至向量OQ，此时OQ与X轴正半轴的夹角为α-θ，举个例子来简单说明一下思想，若P点对应的极坐标为(51°,1)，要转换为直角坐标系，我们需要知道cos51°和sin51°，假设我们不能借助其他的手段来直接计算该值，那么我们逐次的旋转一定的角度来逼近51°，而旋转的这些角度值的选择有一定的特殊性，一会再解释。假设我们从x轴开始逆时针旋转（或者从51°开始顺时针旋转），旋转45°，发现没到51°，再逆时针旋转45°/2=22.5°，则一共旋转了67.5°，超了，再顺时针旋转22.5°/2=11.25°……逐次逼近51°，这个过程用公式描述为：

C语言描述过程如下，以及模拟旋转结果：

#include <stdio.h>
#include <stdlib.h>

double my_tan2(double z);	

int main(viod)
{
	double x = my_tan2(51.0);
	return 0;
}

double my_tan2(double z)
{
	const double theta[] = { 45.0, 26.56505118, 14.03624347, 7.125016349, 
		3.576334375, 1.789910608, 0.8951737102, 0.4476141709, 
		0.2238105004, 0.1119056771, 0.05595289189, 0.02797645262, 
		0.01398822714, 0.006994113675, 0.003497056851, 0.001748528427
	};

	int i = 0;
	double angle_new = 0;
	double angle_remain = z;
	char detection;

	for( i=0; i<16;i++)
	{
		if(angle_remain > 0)
		{
			angle_remain = angle_remain - theta[i];
			detection = '+';
		}
		
		else
		{
			angle_remain = angle_remain + theta[i];
			detection = '-';
			 
		}
		printf(" 旋转次数 = %-8d 旋转角度 = %-12f  旋转方向：%-8c  剩余角度 = %-8f
", i, theta[i],detection, angle_remain);
		
	}
	return angle_remain;
}

再精度满足要求的前提下，旋转一定从次数，则可以求得相应点的直角坐标。有了举例铺垫，再从理论上分析过程，以及选择旋转角度的依据。

2. 减少乘法运算

图中，Q点的坐标可表示为：

这里定义θ为目标旋转角度。根据三角函数公式可将上式展开为：

现在已经有点 Cordic 算法的样子了，但是我们看到每次旋转都要计算 4 次浮点数的乘法运算，运算量还是太大了。还需要进一步的改进，改进的切入点当然还是坐标变换的过程。
将式(1.1)代入到式(1.3)中可得

提取cosθ，上式可重新表示为：

从上式中可看出，cosθ只是改变了目标向量OQ的模长，每次旋转后的新坐标点到原点的距离都变长了，如果去掉cosθ，缩放系数就为1/cosθ，图1.2所示，此时OP旋转θ角之后到达OR，这种旋转称为伪旋转。

不难看出，OQ与OR幅度上的差异，且可证明向量OP和PR是正交的，此时R点的坐标可表示为:

我们求的是θ，不关心模长r的改变，这样的变形非常的简单，最终可以通过对伪旋转的输出加以补偿来获取真实的旋转结果。
对于伪旋转，可以将θ分解为一系列满足要求的小角度和，即

这样，将一次旋转分解成了一系列的微旋转。那么第i+1次旋转后的结果为

但是每次循环的运算量一下从4次乘法降到了2次乘法了。同理，在编程过程中，可以利用查表法，事先将如45°、22.5°等角度的正切值存入表中，在计算过程中，对照旋转值，最终解得极角的三角函数，从而得到某极坐标的直角坐标。

3. 消除乘法运算

这一步至关重要，其实就是cordic算法能在FPGA上应用的关键所在。上述分析中，我们已经成功的将乘法的次数减少了一半，但是毕竟还是有乘法运算，以为这在硬件上实现还是会消耗很多资源，那么还有没有可能进一步降低运算量呢？还要从计算式入手。

第一次循环时，tan45°=1，所以第一次循环实际上是不需要乘法运算的。第二次运算呢？
tan22.5°=0.4142135623731，第二次循环乘数是小数，选择22.5°是因为二分查找法的查找效率最高。如果选用个在22.5到45度之间的值，通过牺牲查找效率而减少乘法，这是可以考虑的。
如果令

d_i代表方向，取+1时代表顺时针，取-1代表逆时针。(1.8)式，可重新改为

从上式不难看出，每次微旋转只需加法、减法和移位操作（乘以2的-i次幂，即右移i位）即可完成。
我们发现tan26.565051177078°=0.5，如果我们第二次旋转采用26.565051177078°，那么乘数变为0.5，如果我们采用定点数运算的话（没有浮点协处理器时为了加速计算我们会大量的采用定点数算法）乘以0.5就相当于将乘数右移一位。右移运算是很快的，这样第二次循环中的乘法运算也被消除了。类似的方法，第三次循环中不用11.25°，而采用 14.0362434679265 °。tan14.0362434679265°=1/4，乘数右移两位就可以了。剩下的都以此类推。

为了确定d_i的值，引入一个新的变量z，定义

因为旋转可以从两个方向进行，可以从0°旋转来逼近目标极角α，也可以从目标极角α旋转来逼近0°角。若设z初始化为0°，即z_0 = 0。根据条件对z_i执行加或者减tan^(-1) 2^(-i)，使得z的最终值逼近为α。z的迭代过程，是将z与α的差值收敛于0的过程，也正是将旋转角度θ分解为一系列θ_i的过程，过z_i可认为是第i次旋转之后的终点角度。至此，cordic算法之圆周系统的旋转模式迭代过程可表示为：

旋转角度θ的最大值和最小值，可计算得

即表明，算法只能支持{-99.8829°，99.8829°}范围内的角度处理。在很多场合中需要目标旋转角度可以覆盖[-180°,180°]，这就需要预处理。只有当目标角度|α|>π/2时需要预旋转π/2。
上述讨论都是基于伪旋转，即每次旋转模长都有变化，省掉了cosθ_i。以k_i表示第i次微旋转模长补偿因子，故第i次微旋转真实的旋转结果应为

模长补偿因子可以建立一个查找表，根据旋转次数定，此时不得不增加乘法，但是若旋转次数为n，则总的模长补偿因子k可表示为：

当n趋于无穷大时，K逼近0.607252935。所以当旋转次数定了之后，可以在最后的结果只增加一次乘法，来补偿模长，从而得到真实的直角坐标，至此就完成了极坐标到直角坐标的转换。满足要求的角度和模长补偿因子可以用matlab生成，附程序：

matlab生成旋转角度和模长补偿因子
clear all
clc
i = 15;         %旋转阶数-1
for n = 0:i;
    theta(n+1) = atand(1/(2^n));        %生成符合要求的角度
    k(n+1) = 1/sqrt(1+(2^(-2*n)));      %生成模长补偿因子
end
vpa(theta,10);          %控制精度
vpa(k,10);
round(theta);           %取整

附C程序来理解过程，注意我采用的方法是从0°（x轴）逐次旋转逼近目标角度，注意代码还是以浮点数运算，稍后会根据FPGA等暂时适合做定点处理的平台进行微改。

#include <stdio.h>
#include <stdlib.h>

double cordic_c(double a,double r);	
double x = 1, y = 0;		//以X轴为旋转起始点

int main(viod)
{
	double remain = cordic_c(31.3,15.0);		//极坐标值（极角，极径）
	printf("旋转角度误差：%f, 直角坐标：x = %f, y = %f",remain,x,y);
	return 0;
}

double cordic_c(double a,double r)
{
	const double theta[] = { 45.0, 26.56505118, 14.03624347, 7.125016349, 
						3.576334375, 1.789910608, 0.8951737102, 0.4476141709, 
						0.2238105004, 0.1119056771, 0.05595289189, 0.02797645262, 
						0.01398822714, 0.006994113675, 0.003497056851, 0.001748528427
						}; //旋转角度

	const double k[] = { 0.7071067812, 0.894427191, 0.9701425001, 0.9922778767,
						0.9980525785, 0.9995120761, 0.999877952, 0.9999694838, 0.9999923707, 
						0.9999980927, 0.9999995232, 0.9999998808, 0.9999999702, 0.9999999925, 
						0.9999999981, 0.9999999995
						};//模长补偿因子

	int i = 0;
	double x_temp = 1.0, y_temp = 0.0;
	double angle_new = 0.0;		//旋转后终止角度
	double angle_remain = a;	//旋转后，剩余角度
	char detection;				//旋转方向

	for( i=0; i<16;i++)
	{
		if( angle_remain == 0)
		{
			break;
		}
		if(angle_remain > 0)
		{
			angle_new = angle_new + theta[i];
			angle_remain = a - angle_new;
			x_temp = (x - y / (1<<i))*k[i];
			y_temp = (y + x /(1<<i))*k[i];
			x = x_temp;
			y = y_temp;
			detection = '+';
		}
		else
		{
			angle_new = angle_new - theta[i];
			angle_remain = a - angle_new;
			x_temp = (x + y /(1<<i))*k[i];
			y_temp = (y - x /(1<<i))*k[i];
			x = x_temp;
			y = y_temp;
			detection = '-'; 
		}
		printf(" 旋转次数 = %-8d 旋转角度 = %-12f  旋转方向：%-8c  终点角度 = %-8f
", i+1, theta[i],detection,angle_new);
	}
	x = r*x;
	y = r*y;
	return angle_remain;
}

从结果中，看到旋转精度能接受。如用查表法的话，其实是空间换时间，速度最快，但是想要精度高的话会占用大量的存储空间（你不可能每个角度都占用一个空间）。 cordic 属于比较均衡的算法，空间占用很少，计算时间也挺快。
值得注意的是，理论分析过程是针对第一象限的旋转，如果极坐标在其他象限，则首先有个预处理过程，将极坐标先转到第一象限后，再进行cordic算法。处理完之后再做相应的后处理。

参考

• 三角函数计算，Cordic 算法入门--Ivan

• 《基于FPGA的数字信号处理（第2版）》——高亚军著