使用Cordic算法在定點(diǎn)MCU中實(shí)現(xiàn)快速計(jì)算反正切函數(shù)atan()和atan2()——寫(xiě)代碼的Tobem在一篇Microchip技術(shù)文檔中，簡(jiǎn)單提到了一種計(jì)算反正切函數(shù)atan()的方法，說(shuō)該方法不僅快速，耗的內(nèi)存還小。這立馬吸引了我的注意力。本人從事電機(jī)控制多年，隨著降成本壓力的增大，之前的帶FPU的浮點(diǎn)MCU慢慢被替換為定點(diǎn)MCU。而對(duì)于線性霍爾電機(jī)控制方案，難免要使用atan2()進(jìn)行電機(jī)電角度的計(jì)算，如果使用數(shù)學(xué)庫(kù)math.h中的atan2f()函數(shù)進(jìn)行計(jì)算，算力開(kāi)銷會(huì)很大，特別是高速電機(jī)的場(chǎng)合下，因?yàn)檫@時(shí)需要更高的控制頻率，電角度更新頻率也要更快。我們知道atan2()是可以通過(guò)atan()來(lái)計(jì)算的，如果解決了atan()快速計(jì)算的問(wèn)題，也就解決了atan2()快速計(jì)算的問(wèn)題。于是便開(kāi)始查找文檔中提到的CORDIC算法。CORDIC(CoordinateRotationDIgitalComputer)算法，只利用移位和加減運(yùn)算，就能計(jì)算常用三角函數(shù)值，如Sin，Cos，Sinh，Cosh等函數(shù)，當(dāng)然，也包括前面提到的反正切Atan。Cordic算法的思路很簡(jiǎn)單，在坐標(biāo)系中，將（X，Y）旋轉(zhuǎn)一定的度數(shù)，如果旋轉(zhuǎn)之后縱坐標(biāo)Y變?yōu)榱?，那么旋轉(zhuǎn)的角度就是我們要求解的反正切角θ。但是我們事先并不知道要旋轉(zhuǎn)多少角度，才能讓旋轉(zhuǎn)后的縱坐標(biāo)Y=0。因此，我們每次旋轉(zhuǎn)某一固定角度，每次旋轉(zhuǎn)角度都為前一次旋轉(zhuǎn)角度的一半，并且需要根據(jù)縱坐標(biāo)Y的數(shù)值正負(fù)進(jìn)行旋轉(zhuǎn)方向的選取，如此，經(jīng)過(guò)多次旋轉(zhuǎn)之后，就能讓y趨近于0，而每次旋轉(zhuǎn)的角度累加出來(lái)的結(jié)果就是我們要求取的反正切角θ了。舉例說(shuō)明如下：平面上一點(diǎn)在直角坐標(biāo)系下的坐標(biāo)（X,Y）=（100，200），則θ=atan(Y/X)。設(shè)（x,y）是原始坐標(biāo)點(diǎn)，將其以原點(diǎn)為中心，順時(shí)針旋轉(zhuǎn)θ之后的坐標(biāo)記為（x’,y’）,則有如下公式：寫(xiě)成矩陣形式：借鑒二分法的思想，我們每次旋轉(zhuǎn)角度為前一次的一半，如果Y為正，θ為正；Y為負(fù)，θ為負(fù)；如果Y=0，則說(shuō)明總的旋轉(zhuǎn)角度剛好為θ。令第一次旋轉(zhuǎn)角度為45°：旋轉(zhuǎn)之后縱坐標(biāo)Y為70.711，大于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了。我們可以繼續(xù)旋轉(zhuǎn)下去，旋轉(zhuǎn)次數(shù)越多，旋轉(zhuǎn)之后的縱坐標(biāo)Y越接近0，也就是θ精度越高，但是，當(dāng)然耗時(shí)也越長(zhǎng)。通常，我們只需要達(dá)到一定精度即可，以便減少旋轉(zhuǎn)次數(shù)，加快算法的運(yùn)行。旋轉(zhuǎn)的過(guò)程圖示如下：前述演示過(guò)程中，用到了sin函數(shù)和cos函數(shù)，在定點(diǎn)MCU中，仍過(guò)于復(fù)雜。為了進(jìn)一步簡(jiǎn)化過(guò)程，我們可以提取因數(shù)cos，則：由于我們只需要求取角度θ，因此可以進(jìn)一步去掉cos，這樣，旋轉(zhuǎn)的角度保持不變，只是模長(zhǎng)發(fā)生了變化。由于cos在0~1之間，因此模長(zhǎng)是變長(zhǎng)的。具體軟件中如何實(shí)現(xiàn)呢？由于我們每次旋轉(zhuǎn)角度都為前一次的一半，令第一次旋轉(zhuǎn)角度為45°，因此，我們只需要提前計(jì)算好tan(45)、tan(45/2)、tan(45/4)……，并把結(jié)果存到一個(gè)數(shù)組中，然后根據(jù)上式進(jìn)行迭代計(jì)算至需要的精度即可。但是這種方式中，還存在乘法運(yùn)算，而我們知道，定點(diǎn)MCU中的移位運(yùn)算要快速得多，因此實(shí)際代碼實(shí)踐中，會(huì)稍微進(jìn)行修改。第一旋轉(zhuǎn)仍為45°(atan(1))，第二次，我們不是旋轉(zhuǎn)(45/2)°,而是旋轉(zhuǎn)atan(1/2)，第三次旋轉(zhuǎn)atan(1/4)，第四次旋轉(zhuǎn)atan(1/8)……這樣，我們就可以把乘法運(yùn)算變?yōu)橐莆贿\(yùn)算，大大提高了運(yùn)算速度。我們把旋轉(zhuǎn)方向考慮進(jìn)來(lái)：Cordic算法可以快速計(jì)算出atan()函數(shù)，其返回值在-Pi/2~+Pi/2之間，而實(shí)際計(jì)算角度時(shí)，我們往往使用的是atan2()函數(shù)，其返回值在-Pi~+Pi之間。根據(jù)atan2()和atan()之間的聯(lián)系，可以通過(guò)使用atan()計(jì)算出atan2()，即如果是在二三象限的矢量，則需要先旋轉(zhuǎn)90度或者180度到一四象限，然后再迭代。具體C語(yǔ)言代碼實(shí)現(xiàn)見(jiàn)附件如下：使用定點(diǎn)MCU進(jìn)行測(cè)試，例如，使用STM32F103進(jìn)行燒錄測(cè)試，通過(guò)Systick計(jì)算Cordic算法和數(shù)學(xué)庫(kù)atan2f()各自計(jì)算289次的總耗時(shí)，得到：可以看到，使用Cordic算法計(jì)算atan2()函數(shù)，289次總耗時(shí)為27760個(gè)Systick，而數(shù)學(xué)庫(kù)的atan2f()則需要57126個(gè)Systick。因此，Cordic