深度解析CORDIC算法原理

CORDIC算法詳解

1 平面坐標(biāo)系旋轉(zhuǎn)

CORDIC算法的思想是通過迭代的方法，使得累計(jì)旋轉(zhuǎn)過的角度的和無限接近目標(biāo)角度。它是一種數(shù)值計(jì)算逼近的方法，運(yùn)算只有移位和加減。

通過圓坐標(biāo)系可以了解CORDIC算法的基本思想，如圖1所示，初始向量( x 1 , y 1 ) left( x_{1},y_{1} ight)(x1,y1)旋轉(zhuǎn)θ hetaθ角度之后得到向量( x 2 , y 2 ) left( x_{2},y_{2} ight)(x2,y2)，兩者之間滿足（公式1）關(guān)系。

圖1 CORDIC算法原理示意圖

{ x 2 = x 1 cos ? θ n ? y 1 sin ? θ n y 2 = y 1 cos ? θ n ? x 1 sin ? θ n left{ egin{matrix} x_{2} = x_{1} cos heta_{n} - y_{1} sin heta_{n} \ y_{2} = y_{1} cos heta_{n} - x_{1}sin heta_{n} \ end{matrix} ight.{x2=x1cosθn?y1sinθny2=y1cosθn?x1sinθn

通過提取因數(shù)cos ? θ cos hetacosθ，方程式可以改寫成下面的形式：

x 2 = x 1 cos ? θ ? y 1 sin ? θ = cos ? θ ( x 1 ? y 1 tan ? θ ) y 2 = y 1 cos ? θ ? x 1 sin ? θ = cos ? θ ( y 1 + x 1 tan ? θ ) egin{matrix} x_{2} = x_{1} cos heta - y_{1} sin heta = cos hetaleft( x_{1} - y_{1} an heta ight) \ y_{2} = y_{1} cos heta - x_{1}sin heta = cos hetaleft( y_{1} + x_{1} an heta ight) \ end{matrix}x2=x1cosθ?y1sinθ=cosθ(x1?y1tanθ)y2=y1cosθ?x1sinθ=cosθ(y1+x1tanθ)

2 偽旋轉(zhuǎn)

如果去掉cos ? θ cos hetacosθ，我們可以的到偽旋轉(zhuǎn)方程式（公式3），即旋轉(zhuǎn)的角度是正確的，但是x xx，y yy的值增加了cos ? ? 1 θ cos^{- 1} hetacos?1θ倍，由于cos ? ? 1 θ > 1 cos^{- 1} heta > 1cos?1θ>1，所以模值變大。這里并不能通過數(shù)學(xué)方法去除cos ? θ cos hetacosθ項(xiàng)，但是可以化簡坐標(biāo)平面旋轉(zhuǎn)的計(jì)算。

x ^ 2 = x 1 ? y 1 tan ? θ y ^ 2 = y 1 + x 1 tan ? θ egin{matrix} {widehat{x}}_{2} = x_{1} - y_{1} an heta \ {widehat{y}}_{2} = y_{1} + x_{1} an heta \ end{matrix}x2=x1?y1tanθy2=y1+x1tanθ

圖2 去除cos ? θ cos hetacosθ后偽坐標(biāo)系

3 CORDIC方法

這里為了便于硬件的計(jì)算，采用迭代的思想，旋轉(zhuǎn)角度θ hetaθ可以通過若干步實(shí)現(xiàn)，每一步旋轉(zhuǎn)一個(gè)角度θ i heta^{i}θi。并且約定每一次旋轉(zhuǎn)的角度的正切值為2的倍數(shù)，即tan ? θ i = 2 ? i { an heta^{i} = 2}^{- i}tanθi=2?i。下面表格是用于CORDIC算法中每個(gè)迭代i的旋轉(zhuǎn)角度。這樣，乘以正切值就可以變成移位操作。

表1 迭代角度θ i heta^{i}θi正切值

4 角度累加器

引入控制算子d i d_{i}di，用于確定旋轉(zhuǎn)的方向。其中第i步順時(shí)針旋轉(zhuǎn)時(shí)d i = ? 1 d_{i} = - 1di=?1，逆時(shí)針旋轉(zhuǎn)d i = 1 d_{i} = 1di=1。前面的偽旋轉(zhuǎn)坐標(biāo)方程現(xiàn)在可以表示為：

x i + 1 = x i ? d i 2 ? i y i y i + 1 = y i + d i 2 ? i x i egin{matrix} x_{i + 1} = x_{i} - {d_{i}2^{- i} y}_{i} \ y_{i + 1} = y_{i} + {d_{i} 2^{- i} x}_{i} \ end{matrix}xi+1=xi?di2?iyiyi+1=yi+di2?ixi

這里，我們引入第三個(gè)方程，被稱為角度累加器，用來在每次迭代過程中追蹤累加的旋轉(zhuǎn)角度，表示第n次旋轉(zhuǎn)后剩下未旋轉(zhuǎn)的角度，定義為

z i + 1 = z i ? d i θ i z_{i + 1} = z_{i} - d_{i} heta^{i}zi+1=zi?diθi，其中d i = ± 1 d_{i} = pm 1di=±1

5移位-加法算法

因此，原始的算法現(xiàn)在已經(jīng)被簡化為使用向量的偽旋轉(zhuǎn)方程式（公式6）來表示迭代（移位-加法）算法。

2次移位；
（2 ? i 2^{- i}2?i用移位實(shí)現(xiàn)，每右移n位就把原來數(shù)值乘以2的-i次方了）

1次查找表；（每一次迭代都會(huì)有一個(gè)固定角度θ i heta^{i}θi的累加，這個(gè)角度是2 ? i 2^{- i}2?i對(duì)應(yīng)的角度值，使用查表實(shí)現(xiàn)。θ i = arc tan ? 2 ? i heta^{i} = { ext{arc} an}2^{- i}θi=arctan2?i）

3次加法；（x，y，z的累加，共三次）

x i + 1 = x i ? d i 2 ? i y i y i + 1 = y i + d i 2 ? i x i z i + 1 = z i ? d i θ i egin{matrix} x_{i + 1} = x_{i} - {d_{i} 2^{- i} y}_{i} \ y_{i + 1} = y_{i} + {d_{i} 2^{- i} x}_{i} \ z_{i + 1} = z_{i} - d_{i} heta^{i} \ end{matrix}xi+1=xi?di2?iyiyi+1=yi+di2?ixizi+1=zi?diθi

6 伸縮因子

當(dāng)簡化算法以偽旋轉(zhuǎn)時(shí)，cos ? θ cos hetacosθ項(xiàng)被忽略，這樣( x n , y n ) left( x_{n},y_{n} ight)(xn,yn)就被伸縮了K n K_{n}Kn倍。如果迭代次數(shù)已知，可以預(yù)先計(jì)算伸縮因子K n K_{n}Kn。同樣1 / K n 1/K_{n}1/Kn也可被預(yù)先計(jì)算以得到( x n , y n ) left( x_{n},y_{n} ight)(xn,yn)的真值。

K n = ∏ n 1 cos ? θ i = ∏ n ( 1 + 2 ? 2 i ) K_{n} = prod_{n}^{}frac{1}{cos heta^{i}} = prod_{n}^{}left( sqrt{1 + 2^{- 2i}} ight)Kn=n∏cosθi1=n∏(1+2?2i)

這里K n ? > 1.64676 K_{n} - > 1.64676Kn?>1.64676，當(dāng)n ? > ∞ n - > inftyn?>∞

1 / K n ? > 0.607253 1/K_{n} - > 0.6072531/Kn?>0.607253，當(dāng)n ? > ∞ n - > inftyn?>∞

7 旋轉(zhuǎn)模式

CORDIC方法有兩種模式，旋轉(zhuǎn)模式和向量模式。工作模式?jīng)Q定了控制算子d i d_{i}di的條件。在旋轉(zhuǎn)模式中，旋轉(zhuǎn)剩余角度初始變量z 0 = θ z_{0} = hetaz0=θ，經(jīng)過n次旋轉(zhuǎn)后，使z 0 = 0 z_{0} = 0z0=0。整個(gè)旋轉(zhuǎn)過程可以表示為一系列旋轉(zhuǎn)角度不斷偏擺，從而逼近所需旋轉(zhuǎn)角度的迭代過程。n次迭代后可以得到：

x n = K n ( x 0 cos ? z 0 ? y 0 sin ? z 0 ) y n = K n ( y 0 cos ? z 0 + x 0 sin ? z 0 ) z n = 0 egin{matrix} x_{n} = K_{n}left( x_{0} cos z_{0} - y_{0} sin z_{0} ight) \ y_{n} = K_{n}left( y_{0} cos z_{0} + x_{0} sin z_{0} ight) \ z_{n} = 0 \ end{matrix}xn=Kn(x0cosz0?y0sinz0)yn=Kn(y0cosz0+x0sinz0)zn=0

通過設(shè)置x 0 = 1 / K n = 0.6073 x_{0} = 1/K_{n} = 0.6073x0=1/Kn=0.6073和y 0 = 0 y_{0} = 0y0=0可以計(jì)算cos ? z 0 cos z_{0}cosz0和sin ? z 0 sin z_{0}sinz0。分析可知CORDIC算法在圓周系統(tǒng)旋轉(zhuǎn)模式下可以用來計(jì)算一個(gè)輸入角的正弦值、余弦值。

x n = K n ( x 0 cos ? z 0 ? y 0 sin ? z 0 ) = cos ? θ y n = K n ( y 0 cos ? z 0 + x 0 sin ? z 0 ) = sin ? θ egin{matrix} x_{n} = K_{n}left( x_{0} cos z_{0} - y_{0} sin z_{0} ight) = cos heta \ y_{n} = K_{n}left( y_{0} cos z_{0} + x_{0} sin z_{0} ight) = sin heta \ end{matrix}xn=Kn(x0cosz0?y0sinz0)=cosθyn=Kn(y0cosz0+x0sinz0)=sinθ

圖 3 旋轉(zhuǎn)模式角度逼近

8 象限判斷

對(duì)于任意角度θ hetaθ，都可以通過旋轉(zhuǎn)一系列小角度來i完成。第一次迭代旋轉(zhuǎn)45°，第二次迭代旋轉(zhuǎn)26.6°，第三次迭代旋轉(zhuǎn)14°。很顯然，每次旋轉(zhuǎn)的方向都影響到最終的累計(jì)角度。在-99.7°

9溢出處理

通過表2可以看出，把初始θ hetaθ角度設(shè)置為90°，由于x 0 x_{0}x0賦的初值存在誤差（一般是偏大），最終獲得的x n x_{n}xn和y n y_{n}yn是有可能大于1的。所以設(shè)計(jì)時(shí)需要做溢出保護(hù)。或者把x 0 x_{0}x0初值減小，這可能會(huì)犧牲精度。

表2θ hetaθ接近90°數(shù)據(jù)溢出

10 Verilog代碼實(shí)現(xiàn)

CORDIC算法代碼

module MyCORDIC(
input clk,
input rst_n,
input  [15:0]theta,
output reg [15:0]sin_theta,
output reg [15:0]cos_theta
);

parameter Kn  = 'd19898;    // 0.607253*2^15
parameter iKn = 'd53961;    // 1.64676*2^15 

parameter arctan_0 = 8192    ;  // arctan(1/2)
parameter arctan_1 = 4836    ;  // arctan(1/2^1)
parameter arctan_2 = 2555    ;  // arctan(1/2^2)
parameter arctan_3 = 1297    ;  // arctan(1/2^3)
parameter arctan_4 = 651     ;  // arctan(1/2^4)
parameter arctan_5 = 326     ;  // arctan(1/2^5)
parameter arctan_6 = 163     ;  // arctan(1/2^6)
parameter arctan_7 = 81      ;  // arctan(1/2^7)
parameter arctan_8 = 41      ;  // arctan(1/2^8)
parameter arctan_9 = 20      ;  // arctan(1/2^9)
parameter arctan_10 = 10     ;  // arctan(1/2^10)
parameter arctan_11 = 5      ;  // arctan(1/2^11)

reg signed [15:0] x [11:0];
reg signed [15:0] y [11:0];
reg signed [15:0] z [11:0];

wire [15:0]x_tmp;
wire [15:0]y_tmp;

reg signed [15:0]theta_1;
wire [2:0] Quadrant;//theta角所在的象限

// 象限判斷
assign Quadrant = theta[15:14] + 1;

always@(*)
begin
begin
theta_1 <= {2'b00,theta[13:0]};
if(Quadrant==1)
begin
theta_1 <= theta;
end
else if(Quadrant==2)
begin
theta_1 <= 32768 - theta;
end
else if(Quadrant==3)
begin
theta_1 <= theta - 32768;
end
else if(Quadrant==4)
begin
theta_1 <= 65536 - theta;
end
end
end

always@(posedge clk or negedge rst_n)
begin
if(!rst_n)
begin
x[0] <= 16'd0;
y[0] <= 16'd0;
z[0] <= 16'd0;
end
else
begin
x[0] <= Kn;
y[0] <= 16'd0;
z[0] <= theta_1;
end
end

always@(posedge clk or negedge rst_n) // i=0
begin
if(!rst_n)
begin
x[1] <= 16'd0;
y[1] <= 16'd0;
z[1] <= 16'd0;
end
else
begin
if(z[0][15]) // 剩余角度為負(fù)數(shù)，順時(shí)針旋轉(zhuǎn)，d=-1
begin
x[1] <= x[0] + y[0];
y[1] <= y[0] - x[0];
z[1] <= z[0] + arctan_0;
end          // 剩余角度為正數(shù)，順時(shí)針旋轉(zhuǎn)，d=+1
else
begin
x[1] <= x[0] - y[0];
y[1] <= y[0] + x[0];
z[1] <= z[0] - arctan_0;
end
end
end

// >>>符號(hào)表示算術(shù)右移，不改變符號(hào)位
always@(posedge clk or negedge rst_n) // i=1
begin
if(!rst_n)
begin
x[2] <= 16'd0;
y[2] <= 16'd0;
z[2] <= 16'd0;
end
else
begin
if(z[1][15]) // 剩余角度為負(fù)數(shù)，順時(shí)針旋轉(zhuǎn)，d=-1
begin
x[2] <= x[1] + (y[1] >>> 1);
y[2] <= y[1] - (x[1] >>> 1);
z[2] <= z[1] + arctan_1;
end          // 剩余角度為正數(shù)，逆時(shí)針旋轉(zhuǎn)，d=+1
else
begin
x[2] <= x[1] - (y[1] >>> 1);
y[2] <= y[1] + (x[1] >>> 1);
z[2] <= z[1] - arctan_1;
end
end
end

always@(posedge clk or negedge rst_n) // i=2
begin
if(!rst_n)
begin
x[3] <= 16'd0;
y[3] <= 16'd0;
z[3] <= 16'd0;
end
else
begin
if(z[2][15]) // 剩余角度為負(fù)數(shù)，順時(shí)針旋轉(zhuǎn)，d=-1
begin
x[3] <= x[2] + (y[2] >>> 2);
y[3] <= y[2] - (x[2] >>> 2);
z[3] <= z[2] + arctan_2;
end          // 剩余角度為正數(shù)，逆時(shí)針旋轉(zhuǎn)，d=+1
else
begin
x[3] <= x[2] - (y[2] >>> 2);
y[3] <= y[2] + (x[2] >>> 2);
z[3] <= z[2] - arctan_2;
end
end
end

always@(posedge clk or negedge rst_n) // i=3
begin
if(!rst_n)
begin
x[4] <= 16'd0;
y[4] <= 16'd0;
z[4] <= 16'd0;
end
else
begin
if(z[3][15]) // 剩余角度為負(fù)數(shù)，順時(shí)針旋轉(zhuǎn)，d=-1
begin
x[4] <= x[3] + (y[3] >>> 3);
y[4] <= y[3] - (x[3] >>> 3);
z[4] <= z[3] + arctan_3;
end          // 剩余角度為正數(shù)，逆時(shí)針旋轉(zhuǎn)，d=+1
else
begin
x[4] <= x[3] - (y[3] >>> 3);
y[4] <= y[3] + (x[3] >>> 3);
z[4] <= z[3] - arctan_3;
end
end
end


always@(posedge clk or negedge rst_n) // i=4
begin
if(!rst_n)
begin
x[5] <= 16'd0;
y[5] <= 16'd0;
z[5] <= 16'd0;
end
else
begin
if(z[4][15]) // 剩余角度為負(fù)數(shù)，順時(shí)針旋轉(zhuǎn)，d=-1
begin
x[5] <= x[4] + (y[4] >>> 4);
y[5] <= y[4] - (x[4] >>> 4);
z[5] <= z[4] + arctan_4;
end          // 剩余角度為正數(shù)，逆時(shí)針旋轉(zhuǎn)，d=+1
else
begin
x[5] <= x[4] - (y[4] >>> 4);
y[5] <= y[4] + (x[4] >>> 4);
z[5] <= z[4] - arctan_4;
end
end
end

always@(posedge clk or negedge rst_n) // i=5
begin
if(!rst_n)
begin
x[6] <= 16'd0;
y[6] <= 16'd0;
z[6] <= 16'd0;
end
else
begin
if(z[5][15]) // 剩余角度為負(fù)數(shù)，順時(shí)針旋轉(zhuǎn)，d=-1
begin
x[6] <= x[5] + (y[5] >>> 5);
y[6] <= y[5] - (x[5] >>> 5);
z[6] <= z[5] + arctan_5;
end          // 剩余角度為正數(shù)，逆時(shí)針旋轉(zhuǎn)，d=+1
else
begin
x[6] <= x[5] - (y[5] >>> 5);
y[6] <= y[5] + (x[5] >>> 5);
z[6] <= z[5] - arctan_5;
end
end
end

always@(posedge clk or negedge rst_n) // i=6
begin
if(!rst_n)
begin
x[7] <= 16'd0;
y[7] <= 16'd0;
z[7] <= 16'd0;
end
else
begin
if(z[6][15]) // 剩余角度為負(fù)數(shù)，順時(shí)針旋轉(zhuǎn)，d=-1
begin
x[7] <= x[6] + (y[6] >>> 6);
y[7] <= y[6] - (x[6] >>> 6);
z[7] <= z[6] + arctan_6;
end          // 剩余角度為正數(shù)，逆時(shí)針旋轉(zhuǎn)，d=+1
else
begin
x[7] <= x[6] - (y[6] >>> 6);
y[7] <= y[6] + (x[6] >>> 6);
z[7] <= z[6] - arctan_6;
end
end
end


always@(posedge clk or negedge rst_n) // i=7
begin
if(!rst_n)
begin
x[8] <= 16'd0;
y[8] <= 16'd0;
z[8] <= 16'd0;
end
else
begin
if(z[7][15]) // 剩余角度為負(fù)數(shù)，順時(shí)針旋轉(zhuǎn)，d=-1
begin
x[8] <= x[7] + (y[7] >>> 7);
y[8] <= y[7] - (x[7] >>> 7);
z[8] <= z[7] + arctan_7;
end          // 剩余角度為正數(shù)，逆時(shí)針旋轉(zhuǎn)，d=+1
else
begin
x[8] <= x[7] - (y[7] >>> 7);
y[8] <= y[7] + (x[7] >>> 7);
z[8] <= z[7] - arctan_7;
end
end
end

always@(posedge clk or negedge rst_n) // i=8
begin
if(!rst_n)
begin
x[9] <= 16'd0;
y[9] <= 16'd0;
z[9] <= 16'd0;
end
else
begin
if(z[8][15]) // 剩余角度為負(fù)數(shù)，順時(shí)針旋轉(zhuǎn)，d=-1
begin
x[9] <= x[8] + (y[8] >>> 8);
y[9] <= y[8] - (x[8] >>> 8);
z[9] <= z[8] + arctan_8;
end          // 剩余角度為正數(shù)，逆時(shí)針旋轉(zhuǎn)，d=+1
else
begin
x[9] <= x[8] - (y[8] >>> 8);
y[9] <= y[8] + (x[8] >>> 8);
z[9] <= z[8] - arctan_8;
end
end
end

always@(posedge clk or negedge rst_n) // i=9
begin
if(!rst_n)
begin
x[10] <= 16'd0;
y[10] <= 16'd0;
z[10] <= 16'd0;
end
else
begin
if(z[9][15]) // 剩余角度為負(fù)數(shù)，順時(shí)針旋轉(zhuǎn)，d=-1
begin
x[10] <= x[9] + (y[9] >>> 9);
y[10] <= y[9] - (x[9] >>> 9);
z[10] <= z[9] + arctan_9;
end          // 剩余角度為正數(shù)，逆時(shí)針旋轉(zhuǎn)，d=+1
else
begin
x[10] <= x[9] - (y[9] >>> 9);
y[10] <= y[9] + (x[9] >>> 9);
z[10] <= z[9] - arctan_9;
end
end
end

always@(posedge clk or negedge rst_n) // i=10
begin
if(!rst_n)
begin
x[11] <= 16'd0;
y[11] <= 16'd0;
z[11] <= 16'd0;
end
else
begin
if(z[10][15]) // 剩余角度為負(fù)數(shù)，順時(shí)針旋轉(zhuǎn)，d=-1
begin
x[11] <= x[10] + (y[10] >>> 10);
y[11] <= y[10] - (x[10] >>> 10);
z[11] <= z[10] + arctan_10;
end          // 剩余角度為正數(shù)，逆時(shí)針旋轉(zhuǎn)，d=+1
else
begin
x[11] <= x[10] - (y[10] >>> 10);
y[11] <= y[10] + (x[10] >>> 10);
z[11] <= z[10] - arctan_10;
end
end
end

// 溢出判斷
assign x_tmp = x[11][15]==1 ? 16'h7FFF : x[11];
assign y_tmp = y[11][15]==1 ? 16'h7FFF : y[11];
//assign x_tmp = x[11];
//assign y_tmp = y[11];

always@(posedge clk or negedge rst_n) // i=11
begin
if(!rst_n)
begin
sin_theta <= 16'd0;
cos_theta <= 16'd0;
end
else
begin
if(Quadrant == 3'd1)
begin
sin_theta <= y_tmp;
cos_theta <= x_tmp;
end
else if(Quadrant == 3'd2)
begin
sin_theta <= y_tmp;
cos_theta <= -x_tmp;
end
else if(Quadrant == 3'd3)
begin
sin_theta <= -y_tmp;
cos_theta <= -x_tmp;
end
else if(Quadrant == 3'd4)
begin
sin_theta <= -y_tmp;
cos_theta <= x_tmp;
end
else
begin
sin_theta <= sin_theta;
cos_theta <= cos_theta;
end
end
end
endmodule

testbench文件

`timescale 1 ns/ 1 ns

module MyCORDIC_tb(
);

integer i;

reg clk,rst_n;
reg [15:0] theta,theta_n;
wire [15:0]sin_theta,cos_theta;

reg [15:0] cnt,cnt_n;

MyCORDIC u0(
.clk       (clk      ),
.rst_n     (rst_n    ),
.theta     (theta    ),
.sin_theta (sin_theta),
.cos_theta (cos_theta)
);

initial
begin
    #0 clk = 1'b0;
    #10 rst_n = 1'b0;
    #10 rst_n = 1'b1;
    #10000000 $stop;
end 

initial
begin
    #0 theta = 16'd20;
   for(i=0;i<10000;i=i+1)
begin
#400;
theta <= theta + 16'd200;
end
end 

always #10
begin
    clk = ~clk;
end

endmodule

ModelSim仿真

審核編輯：黃飛