三角函數(shù)計(jì)算,Cordic 算法入門(mén)

1、三角函數(shù)計(jì)算，Cordic 算法入門(mén)三角函數(shù)的計(jì)算是個(gè)復(fù)雜的主題，有計(jì)算機(jī)之前，人們通常通過(guò)查找三角函數(shù)表來(lái)計(jì)算任意角度的三角函數(shù)的值。這種表格在人們剛剛產(chǎn)生三角函數(shù)的概念的時(shí)候就已經(jīng)有了，它們通常是通過(guò)從已知值（比如sin(/2)=1）開(kāi)始并重復(fù)應(yīng)用半角和和差公式而生成?，F(xiàn)在有了計(jì)算機(jī)，三角函數(shù)表便推出了歷史的舞臺(tái)。但是像我這樣的喜歡刨根問(wèn)底的人，不禁要問(wèn)計(jì)算機(jī)又是如何計(jì)算三角函數(shù)值的呢。最容易想到的辦法就是利用級(jí)數(shù)展開(kāi)，比如泰勒級(jí)數(shù)來(lái)逼近三角函數(shù)，只要項(xiàng)數(shù)取得足夠多就能以任意的精度來(lái)逼近函數(shù)值。除了泰勒級(jí)數(shù)逼近之外，還有其他許多的逼近方法，比如切比雪夫逼近、最佳一致逼近和Pad&

2、#233;逼近等。所有這些逼近方法本質(zhì)上都是用多項(xiàng)式函數(shù)來(lái)近似我們要計(jì)算的三角函數(shù)，計(jì)算過(guò)程中必然要涉及到大量的浮點(diǎn)運(yùn)算。在缺乏硬件乘法器的簡(jiǎn)單設(shè)備上（比如沒(méi)有浮點(diǎn)運(yùn)算單元的單片機(jī)），用這些方法來(lái)計(jì)算三角函數(shù)會(huì)非常的費(fèi)時(shí)。為了解決這個(gè)問(wèn)題，J. Volder于1959年提出了一種快速算法，稱之為CORDIC(COordinate Rotation DIgital Computer) 算法，這個(gè)算法只利用移位和加減運(yùn)算，就能計(jì)算常用三角函數(shù)值，如Sin，Cos，Sinh，Cosh等函數(shù)。 J. Walther在1974年在這種

3、算法的基礎(chǔ)上進(jìn)一步改進(jìn)，使其可以計(jì)算出多種超越函數(shù)，更大的擴(kuò)展了Cordic 算法的應(yīng)用。因?yàn)镃ordic 算法只用了移位和加法，很容易用純硬件來(lái)實(shí)現(xiàn)，因此我們常能在FPGA運(yùn)算平臺(tái)上見(jiàn)到它的身影。不過(guò)，大多數(shù)的軟件程序員們都沒(méi)有聽(tīng)說(shuō)過(guò)這種算法，也更不會(huì)主動(dòng)的去用這種算法。其實(shí)，在嵌入式軟件開(kāi)發(fā)，尤其是在沒(méi)有浮點(diǎn)運(yùn)算指令的嵌入式平臺(tái)（比如定點(diǎn)型DSP）上做開(kāi)發(fā)時(shí)，還是會(huì)遇上可以用到Cordic 算法的情況的，所以掌握基本的Cordic算法還是有用的。從二分查找法說(shuō)起先從一個(gè)例子說(shuō)起，知道平面上一點(diǎn)在直角坐標(biāo)系下的坐標(biāo)（X,Y）=（100，200），如何求的在極坐標(biāo)

4、系下的坐標(biāo)（,）。用計(jì)算器計(jì)算一下可知答案是（223.61，63.435）。圖 1 直角坐標(biāo)系到極坐標(biāo)系的轉(zhuǎn)換為了突出重點(diǎn)，這里我們只討論X和Y都為正數(shù)的情況。這時(shí)=atan(y/x)。求的過(guò)程也就是求atan 函數(shù)的過(guò)程。Cordic算法采用的想法很直接，將（X，Y）旋轉(zhuǎn)一定的度數(shù)，如果旋轉(zhuǎn)完縱坐標(biāo)變?yōu)榱?，那么旋轉(zhuǎn)的度數(shù)就是。坐標(biāo)旋轉(zhuǎn)的公式可能大家都忘了，這里把公式列出了。設(shè)（x,y）是原始坐標(biāo)點(diǎn)，將其以原點(diǎn)為中心，順時(shí)針旋轉(zhuǎn)之后的坐標(biāo)記為（x,y）,則有如下公式：也可以寫(xiě)為矩陣形式：如何旋轉(zhuǎn)呢，可以借鑒二分查找法的思想。我們知道的范圍是0到90度。那么就先旋

5、轉(zhuǎn)45度試試。旋轉(zhuǎn)之后縱坐標(biāo)為70.71，還是大于0，說(shuō)明旋轉(zhuǎn)的度數(shù)不夠，接著再旋轉(zhuǎn)22.5度（45度的一半）。這時(shí)總共旋轉(zhuǎn)了45+22.5=67.5度。結(jié)果縱坐標(biāo)變?yōu)榱素?fù)數(shù)，說(shuō)明<67.5度，這時(shí)就要往回轉(zhuǎn)，還是二分查找法的思想，這次轉(zhuǎn)11.25度。這時(shí)總共旋轉(zhuǎn)了45+22.5-11.25=56.25度。又轉(zhuǎn)過(guò)頭了，接著旋轉(zhuǎn)，這次順時(shí)針轉(zhuǎn)5.625度。這時(shí)總共旋轉(zhuǎn)了45+22.5-11.25+5.625=61.875度。這時(shí)縱坐標(biāo)已經(jīng)很接近0了。我們只是說(shuō)明算法的思想，因此就不接著往下計(jì)算了。計(jì)算到這里我們給的答案是 61.875±5.625。二分查找法本質(zhì)上查找的

6、是一個(gè)區(qū)間，因此我們給出的是值的一個(gè)范圍。同時(shí)，坐標(biāo)到原點(diǎn)的距離也求出來(lái)了，=223.52。與標(biāo)準(zhǔn)答案比較一下計(jì)算的結(jié)果還是可以的。旋轉(zhuǎn)的過(guò)程圖示如下?？赡苡凶x者會(huì)問(wèn)，計(jì)算中用到了 sin 函數(shù)和 cos 函數(shù)，這些值又是怎么計(jì)算呢。很簡(jiǎn)單，我們只用到很少的幾個(gè)特殊點(diǎn)的sin 函數(shù)和 cos 函數(shù)的值，提前計(jì)算好存起來(lái)，用時(shí)查表。下面給出上面方法的C 語(yǔ)言實(shí)現(xiàn)。cpp view plaincopy1. #include <stdio.h>  2. #include

7、 <stdlib.h>  3.   4. double my_atan2(double x, double y);  5. int main(void)  6.   7.     double z = my_atan2(100.0, 200.0);  8.     printf(&quo

8、t;n z = %f n", z);  9.   10.     return 0;  11.   12.   13.   14. double my_atan2(double x, double y)  15.   16.     const

9、60;double sine = 0.7071067811865,0.3826834323651,0.1950903220161,0.09801714032956,  17. 0.04906767432742,0.02454122852291,0.01227153828572,0.006135884649154,0.003067956762966  18. ,0.001533980186285,7.669903187427045e-4,3.834951875713956e-4,1.917475973107033e-4,

10、60; 19. 9.587379909597735e-5,4.793689960306688e-5,2.396844980841822e-5  20.                            21.   22.     con

11、st double cosine = 0.7071067811865,0.9238795325113,0.9807852804032,0.9951847266722,  23. 0.9987954562052,0.9996988186962,0.9999247018391,0.9999811752826,0.9999952938096,  24. 0.9999988234517,0.9999997058629,0.9999999264657,0.9999999816164,0.9999999954041, 

12、; 25. 0.999999998851,0.9999999997128  26.                              27.   28.   29.     int 

13、i = 0;  30.     double x_new, y_new;  31.     double angleSum = 0.0;  32.     double angle = 45.0;  33.   34.     for(i&

14、#160;= 0; i < 15; i+)  35.       36.         if(y > 0)  37.           38.        

15、60;    x_new = x * cosinei + y * sinei;  39.             y_new = y * cosinei - x * sinei;  40.     &

16、#160;       x = x_new;  41.             y = y_new;  42.             angleSum += angle; 

17、60;43.           44.         else  45.           46.             x_new = x 

18、* cosinei - y * sinei;  47.             y_new = y * cosinei + x * sinei;  48.             x

19、0;= x_new;  49.             y = y_new;  50.             angleSum -= angle;  51.        

20、60;  52.         printf("Debug: i = %d angleSum = %f, angle = %fn", i, angleSum, angle);  53.         angle /= 2; &

21、#160;54.       55.     return angleSum;  56.   程序運(yùn)行的輸出結(jié)果如下：plain view plaincopy1. Debug: i = 0 angleSum = 45.000000, angle = 45.000000  2. Debug: i = 

22、1 angleSum = 67.500000, angle = 22.500000  3. Debug: i = 2 angleSum = 56.250000, angle = 11.250000  4. Debug: i = 3 angleSum = 61.875000, angle = 5.625000 

23、 5. Debug: i = 4 angleSum = 64.687500, angle = 2.812500  6. Debug: i = 5 angleSum = 63.281250, angle = 1.406250  7. Debug: i = 6 angleSum = 63.984375, 

24、;angle = 0.703125  8. Debug: i = 7 angleSum = 63.632813, angle = 0.351563  9. Debug: i = 8 angleSum = 63.457031, angle = 0.175781  10. Debug: i = 9 an

25、gleSum = 63.369141, angle = 0.087891  11. Debug: i = 10 angleSum = 63.413086, angle = 0.043945  12. Debug: i = 11 angleSum = 63.435059, angle = 0.021973  1

26、3. Debug: i = 12 angleSum = 63.424072, angle = 0.010986  14. Debug: i = 13 angleSum = 63.429565, angle = 0.005493  15. Debug: i = 14 angleSum = 63.432312, 

27、angle = 0.002747  16.   17.  z = 63.432312  減少乘法運(yùn)算現(xiàn)在已經(jīng)有點(diǎn) Cordic 算法的樣子了，但是我們看到?jīng)]次循環(huán)都要計(jì)算 4 次浮點(diǎn)數(shù)的乘法運(yùn)算，運(yùn)算量還是太大了。還需要進(jìn)一步的改進(jìn)。改進(jìn)的切入點(diǎn)當(dāng)然還是坐標(biāo)變換的過(guò)程。我們將坐標(biāo)變換公式變一下形?？梢钥闯?#160;cos()可以從矩陣運(yùn)算中提出來(lái)。我們可以做的再?gòu)氐仔?，直接?#160;cos() 給省略掉。省略cos()后發(fā)生了什么

28、呢，每次旋轉(zhuǎn)后的新坐標(biāo)點(diǎn)到原點(diǎn)的距離都變長(zhǎng)了，放縮的系數(shù)是1/cos()。不過(guò)沒(méi)有關(guān)系，我們求的是，不關(guān)心的改變。這樣的變形非常的簡(jiǎn)單，但是每次循環(huán)的運(yùn)算量一下就從4次乘法降到了2次乘法了。還是給出 C 語(yǔ)言的實(shí)現(xiàn)：cpp view plaincopy1. double my_atan3(double x, double y)  2.   3.     const double tangent = 1.0,

29、0.4142135623731,0.1989123673797,0.09849140335716,0.04912684976947,  4. 0.02454862210893,0.01227246237957,0.006136000157623,0.003067971201423,  5. 0.001533981991089,7.669905443430926e-4,3.83495215771441e-4,1.917476008357089e-4,  6. 9.587379953660303e-5,4.79368996581451e-

30、5,2.3968449815303e-5  7.                            8.   9.   10.     int i = 0;  11. 

31、0;   double x_new, y_new;  12.     double angleSum = 0.0;  13.     double angle = 45.0;  14.   15.     for(i = 0; i < 

32、;15; i+)  16.       17.         if(y > 0)  18.           19.             x_new

33、0;= x + y * tangenti;  20.             y_new = y - x * tangenti;  21.             x = x_new;

34、60; 22.             y = y_new;  23.             angleSum += angle;  24.           25.

35、         else  26.           27.             x_new = x - y * tangenti;  28.     

36、        y_new = y + x * tangenti;  29.             x = x_new;  30.             y

37、 = y_new;  31.             angleSum -= angle;  32.           33.         printf("Debug: i =

38、0;%d angleSum = %f, angle = %f,  = %fn", i, angleSum, angle, hypot(x,y);  34.         angle /= 2;  35.       36.    

39、; return angleSum;  37.   計(jì)算的結(jié)果是：plain view plaincopy1. Debug: i = 0 angleSum = 45.000000, angle = 45.000000,  = 316.227766  2. Debug: i = 1 angleSum = 67.500000,

40、60;angle = 22.500000,  = 342.282467  3. Debug: i = 2 angleSum = 56.250000, angle = 11.250000,  = 348.988177  4. Debug: i = 3 angleSum = 61.875000, angle =&

41、#160;5.625000,  = 350.676782  5. Debug: i = 4 angleSum = 64.687500, angle = 2.812500,  = 351.099697  6. Debug: i = 5 angleSum = 63.281250, angle = 1.406250,

42、0; = 351.205473  7. Debug: i = 6 angleSum = 63.984375, angle = 0.703125,  = 351.231921  8. Debug: i = 7 angleSum = 63.632813, angle = 0.351563,  = 351

43、.238533  9. Debug: i = 8 angleSum = 63.457031, angle = 0.175781,  = 351.240186  10. Debug: i = 9 angleSum = 63.369141, angle = 0.087891,  = 351.240599 

44、0;11. Debug: i = 10 angleSum = 63.413086, angle = 0.043945,  = 351.240702  12. Debug: i = 11 angleSum = 63.435059, angle = 0.021973,  = 351.240728  13. Debug:&#

45、160;i = 12 angleSum = 63.424072, angle = 0.010986,  = 351.240734  14. Debug: i = 13 angleSum = 63.429565, angle = 0.005493,  = 351.240736  15. Debug: i =&#

46、160;14 angleSum = 63.432312, angle = 0.002747,  = 351.240736  16.   17.  z = 63.432312  消除乘法運(yùn)算我們已經(jīng)成功的將乘法的次數(shù)減少了一半，還有沒(méi)有可能進(jìn)一步降低運(yùn)算量呢？還要從計(jì)算式入手。第一次循環(huán)時(shí)，tan(45)=1，所以第一次循環(huán)實(shí)際上是不需要乘法運(yùn)算的。第二次運(yùn)算呢？Tan(22.5)=0.4142135623731,很不

47、幸，第二次循環(huán)乘數(shù)是個(gè)很不整的小數(shù)。是否能對(duì)其改造一下呢？答案是肯定的。第二次選擇22.5度是因?yàn)槎植檎曳ǖ牟檎倚首罡?。如果選用個(gè)在22.5到45度之間的值，查找的效率會(huì)降低一些。如果稍微降低一點(diǎn)查找的效率能讓我們有效的減少乘法的次數(shù)，使最終的計(jì)算速度提高了，那么這種改進(jìn)就是值得的。我們發(fā)現(xiàn)tan(26.565051177078)=0.5，如果我們第二次旋轉(zhuǎn)采用26.565051177078度，那么乘數(shù)變?yōu)?.5，如果我們采用定點(diǎn)數(shù)運(yùn)算的話（沒(méi)有浮點(diǎn)協(xié)處理器時(shí)為了加速計(jì)算我們會(huì)大量的采用定點(diǎn)數(shù)算法）乘以0.5就相當(dāng)于將乘數(shù)右移一位。右移運(yùn)算是很快的，這樣第二次循環(huán)中的乘法運(yùn)算也被消除了。類

48、似的方法，第三次循環(huán)中不用11.25度，而采用 14.0362434679265 度。Tan(14.0362434679265)= 1/4乘數(shù)右移兩位就可以了。剩下的都以此類推。plain view plaincopy1. tan(45)= 1  2. tan(26.565051177078)= 1/2  3. tan(14.0362434679265)= 1/4  4. tan(7.1250163489018)= 1/8  5. ta

49、n(3.57633437499735)= 1/16  6. tan(1.78991060824607)= 1/32  7. tan(0.8951737102111)= 1/64  8. tan(0.4476141708606)= 1/128  9. tan(0.2238105003685)= 1/256  還是給出C語(yǔ)言的實(shí)現(xiàn)代碼，我們采用循序漸進(jìn)的方法，先給出浮點(diǎn)數(shù)的實(shí)現(xiàn)（因?yàn)橛玫搅烁↑c(diǎn)數(shù)，所以并沒(méi)有減少乘法運(yùn)算量，查找的效率也比二分查找法要低

50、，理論上說(shuō)這個(gè)算法實(shí)現(xiàn)很低效。不過(guò)這個(gè)代碼的目的在于給出算法實(shí)現(xiàn)的示意性說(shuō)明，還是有意義的）。cpp view plaincopy1. double my_atan4(double x, double y)  2.   3.     const double tangent = 1.0, 1 / 2.0, 1 / 4.0, 1 / 8.0

51、, 1 / 16.0,  4.                               1 / 32.0, 1 / 64.0, 1 / 128.0, 1&#

52、160;/ 256.0, 1 / 512.0,  5.                               1 / 1024.0, 1 / 2048.0, 1 /

53、 4096.0, 1 / 8192.0, 1 / 16384.0  6.                                7.     const

54、 double angle = 45.0, 26.565051177078, 14.0362434679265, 7.1250163489018, 3.57633437499735,  8.                          

55、0;  1.78991060824607, 0.8951737102111, 0.4476141708606, 0.2238105003685, 0.1119056770662,  9.                            

56、0;0.0559528918938, 0.027976452617, 0.01398822714227, 0.006994113675353, 0.003497056850704  10.                              1

57、1.   12.     int i = 0;  13.     double x_new, y_new;  14.     double angleSum = 0.0;  15.   16.     for(i = 0; i&#

58、160;< 15; i+)  17.       18.         if(y > 0)  19.           20.            &#

59、160;x_new = x + y * tangenti;  21.             y_new = y - x * tangenti;  22.             x =&#

60、160;x_new;  23.             y = y_new;  24.             angleSum += anglei;  25.         &

61、#160; 26.         else  27.           28.             x_new = x - y * tangenti;  29.  

62、0;          y_new = y + x * tangenti;  30.             x = x_new;  31.           

63、;  y = y_new;  32.             angleSum -= anglei;  33.           34.         printf("Debug:

64、60;i = %d angleSum = %f, angle = %f,  = %fn", i, angleSum, anglei, hypot(x, y);  35.       36.     return angleSum;  37.   程序運(yùn)行的輸出

65、結(jié)果如下：plain view plaincopy1. Debug: i = 0 angleSum = 45.000000, angle = 45.000000,  = 316.227766  2. Debug: i = 1 angleSum = 71.565051, angle = 26.565051,  = 353.5533

66、91  3. Debug: i = 2 angleSum = 57.528808, angle = 14.036243,  = 364.434493  4. Debug: i = 3 angleSum = 64.653824, angle = 7.125016,  = 367.270602  5.

67、Debug: i = 4 angleSum = 61.077490, angle = 3.576334,  = 367.987229  6. Debug: i = 5 angleSum = 62.867400, angle = 1.789911,  = 368.166866  7. Debug: i 

68、;= 6 angleSum = 63.762574, angle = 0.895174,  = 368.211805  8. Debug: i = 7 angleSum = 63.314960, angle = 0.447614,  = 368.223042  9. Debug: i = 8 ang

69、leSum = 63.538770, angle = 0.223811,  = 368.225852  10. Debug: i = 9 angleSum = 63.426865, angle = 0.111906,  = 368.226554  11. Debug: i = 10 angleSum =

70、60;63.482818, angle = 0.055953,  = 368.226729  12. Debug: i = 11 angleSum = 63.454841, angle = 0.027976,  = 368.226773  13. Debug: i = 12 angleSum = 63.440853,&

71、#160;angle = 0.013988,  = 368.226784  14. Debug: i = 13 angleSum = 63.433859, angle = 0.006994,  = 368.226787  15. Debug: i = 14 angleSum = 63.437356, angle

72、0;= 0.003497,  = 368.226788  16.   17.  z = 63.437356  有了上面的準(zhǔn)備，我們可以來(lái)討論定點(diǎn)數(shù)算法了。所謂定點(diǎn)數(shù)運(yùn)算，其實(shí)就是整數(shù)運(yùn)算。我們用256 表示1度。這樣的話我們就可以精確到1/256=0.00390625 度了，這對(duì)于大多數(shù)的情況都是足夠精確的了。256 表示1度，那么45度就是 45*256 = 115200。其他的度數(shù)以此類推。序號(hào)度數(shù)&#

73、215;256取整145.01152011520226.5650511770786800.653101331966801314.03624346792653593.27832778918359347.12501634890181824.00418531886182453.57633437499735915.54159999932291661.78991060824607458.21711571099445870.8951737102111229.16446981403522980.4476141708606114.58922774030211590.223810500368557.295488

74、094345857100.111905677066228.64785332894929110.055952891893814.323940324813714120.0279764526177.161971869952947130.013988227142273.580986148419844140.0069941136753531.790493100890352150.0034970568507040.89524655378021C 代碼如下：cpp view plaincopy1. int my_atan5(int x, int y

75、)  2.   3.     const int angle = 11520, 6801, 3593, 1824, 916, 458, 229, 115, 57, 29, 14, 7, 4, 2, 1;  4.   5.     int i 

76、;= 0;  6.     int x_new, y_new;  7.     int angleSum = 0;  8.   9.     x *= 1024;/ 將 X Y 放大一些，結(jié)果會(huì)更準(zhǔn)確  10.     

77、y *= 1024;  11.   12.     for(i = 0; i < 15; i+)  13.       14.         if(y > 0)  15.     

78、60;     16.             x_new = x + (y >> i);  17.             y_new = y - (x >&g

79、t; i);  18.             x = x_new;  19.             y = y_new;  20.          

80、60;  angleSum += anglei;  21.           22.         else  23.           24.             x_new = x - (y >> i);  25.             y_new = y +