慣性傳感器的傾角計算要用到三角函數.

在 MCS-51, Cortex M0, M3 之類的芯片上編程時, 能使用的資源是非常有限, 通常只有兩位數KB的Flash, 個位數KB的RAM. 如果要使用三角函數和開方就要引入 math.h, 會消耗掉10KB以上的Flash空間. 在很多情況下受硬件資源限制無法使用 math.h, 這時候使用簡化的方法進行三角函數和開方運算就非常有意義, OlliW’s Bastelseiten在2014年的一篇文章里, 提供了幾個實用的計算方法. 下面介紹其計算方法和代碼實現.

快速正弦余弦(Sin, Cos)計算

將角度 $x\in [0,\frac{\pi }{2}]$通過下面的式子轉換到 $ \alpha \in [-\frac{1}{2}, \frac{1}{2}]$ 區間

$$\alpha =\frac{2}{\pi }x?\frac{1}{2}$$

于是, 對應 $\alpha$ 的多項式近似計算為
$$\begin{gathered} \sin \alpha =a_0?b_1\alpha +a_2{\alpha }^2?b_3{\alpha }^3+a_4{\alpha }^4?b_5{\alpha }^5+a_6{\alpha }^6 \\ \cos \alpha =a_0+b_1\alpha +a_2{\alpha }^2+b_3{\alpha }^3+a_4{\alpha }^4+b_5{\alpha }^5+a_6{\alpha }^6 \end{gathered}$$
如果將上面的符號固定項和變化項分成$A$和$B$兩部分
$$\begin{gathered} A=a_0+a_2{\alpha }^2+a_4{\alpha }^4+a_6{\alpha }^6 \\ B=b_1\alpha +b_3{\alpha }^3+b_5{\alpha }^5 \end{gathered}$$
則 $\sin \alpha$ 和 $\cos \alpha$ 可以通過 A 和 B 的值表達
$$\begin{gathered} \sin \alpha =A?B \\ \cos \alpha =A+B \end{gathered}$$

對應的各項系數值

$$\begin{gathered} a_0=0.707106781187 \\ {a}_2=?0.872348075361 \\ {a}_4=0.179251759526 \\ {a}_6=?0.0142718282624 \\ {b}_1=?1.110670322264 \\ {b}_3=0.4561589075945 \\ {b}_5=?0.0539104694791 \end{gathered}$$

使用上面的計算方式, 結果絕對誤差小于$6.5\times 10^{?6}$, 并且 ${\cos }^{2}x+{\sin }^{2}x$ 不會超過 1. 計算過程只需要7次乘法和7次加法.

C語言實現

const   float coeff[7] = {// a0 ~ a6           b1 ~ b50.707106781187,  -1.110670322264,-0.872348075361,   0.4561589075945,0.179251759526,  -0.0539104694791,-0.0142718282624
};/*** @param alpha: value between 0 and 0.5
*/
void sincos_normalized(float alpha, float *sin, float *cos)
{int i;float alpha_exp = 1.0, part_a = 0, part_b = 0;for (i = 0; i < 7; i++){if (i % 2 == 0){part_a = part_a + (coeff[i] * alpha_exp);}else{part_b = part_b + (coeff[i] * alpha_exp);}alpha_exp = alpha_exp * alpha;}*sin = part_a - part_b;*cos = part_a + part_b;
}float calculate(float degree_in)
{int quadrant, multi;float degree = degree_in, alpha, cos, sin, c, s;multi = (int)(degree / 90.0);degree = degree - (multi * 90.0);alpha = (degree / 90) - 0.5;sincos_normalized(alpha, &s, &c);multi = multi % 4;if (multi == 0){sin = s;cos = c;}else if (multi == 1){sin = c;cos = -s;}else if (multi == 2){sin = -s;cos = -c;}else if (multi == 3){sin = -c;cos = s;}printf("d_in:%5.0f d:%5.0f a:%10.5f  sin:%10.5f  cos:%10.5f\r\n", degree_in, degree, alpha, sin, cos);
}

計算的結果和 math.h 的 sin cos 函數對比, 數值幾乎一樣, 僅在個別數值的小數點后第五位會有$\pm 1$的差異.

平方根倒數計算

對于1附近的數值, 平方根倒數可以使用牛頓迭代法計算, 實際上非常簡單，因為它只涉及加法和乘法，而不涉及除法, 對于 $x\in [0.6,1.4]$, 計算式為
$$\begin{gathered} y_0=1 \\ {y}_{n+1}=y_n(1.5?0.5x{y_n}^2) \end{gathered}$$

計算兩次牛頓迭代需要3次乘法, 而二階泰勒級數只需要2次, 但是牛頓迭代法精度更高, 甚至比三階泰勒級數的精度更高. 如果執行三次牛頓迭代則需要6次乘法, 在$0.6<x<1.4$的范圍內結果精度優于$1\times 10^{?4}$, 注意$x$的取值范圍, 因為近似是以1為中心展開的, 所以離1越遠差異越大, 在這個范圍之外例如$x=0.5$的時候, 三次迭代就達不到這個精度. 在實際應用中, 可以將要計算的數值提一個系數轉換到 $x\in [0.6,1.4]$區間進行計算.

C語言實現

float inverse_sqrt(int interates, float x)
{float y;y = 1.5 - (x / 2);while (interates--){y = y * (1.5 - 0.5 * x * y * y);}return y;
}// 使用 0.5 ~ 2.1 之間的數字測試, 分別迭代5次
int main(int argc, char *const argv[])
{int i, j;for (i = 0; i < 17; i++){printf("%4.1f ", i*0.1 + 0.5);for (j = 0; j < 5; j++){printf("%11.9f ", inverse_sqrt(j, i*0.1 + 0.5));}printf("\r\n");}return 0;
}

快速反正弦(Arcsin)計算

原文最終選擇的是多項式近似, 避免了取絕對值等復雜處理, 只是在 $x=\pm 1$ 附近的絕對精度較差, 輸出值規范化為 $\pi$，即定義 $\arcsin (x)=\pi \times asin(x)$. 計算式為
$$asin(x)=\frac{x}{2}\times \frac{a_0+a_2{x}^2+a_4{x}^4+a_6{x}^6}{b_0+b_2{x}^2+b_4{x}^4+b_6{x}^6}$$
對應的系數數值為
$$\begin{gathered} a_0=0.318309886 \\ {a}_2=?0.5182875 \\ {a}_4=0.222375 \\ {a}_6=?0.016850156 \\ {b}_0=0.5 \\ {b}_2=?0.89745875 \\ {b}_4=0.46138125 \\ {b}_6=?0.058377188 \end{gathered}$$

當 $|x|<0.75$時, 絕對誤差小于 $1\times 10^{?5}$, 當 $|x|<0.91$時, 絕對誤差小于 $4.2\times 10^{?5}$, 在 $x\approx 0.997$時, 最大誤差為 $0.011$.

C語言實現

const float coeffa[4] = {// a0 ~ a60.318309886,-0.5182875,0.222375,-0.016850156
};const float coeffb[4] = {0.5,-0.89745875,0.46138125,-0.058377188
};const float pi = 3.14159265358979;float arcsin(float x)
{int i;float x2 = 1, a = 0, b = 0;for (i = 0; i < 4; i ++){a = a + coeffa[i] * x2;b = b + coeffb[i] * x2;x2 = x2 * x * x;}return (x * pi / 2) * (a / b);
}int main(int argc, char *const argv[])
{int i;float x, alpha, expect;for (i = 0; i < 20; i++){x = 0.01 + (i * 0.05);alpha = arcsin(x);expect= asin(x);printf("x:%4.2f  a:%9.6f e:%9.6f\r\n", x, alpha, expect);}
}

計算結果, 最右側一列為 math.h 的 asin() 函數, 用于對比

x:0.01  a: 0.010000 e: 0.010000
x:0.06  a: 0.060036 e: 0.060036
x:0.11  a: 0.110223 e: 0.110223
x:0.16  a: 0.160691 e: 0.160691
x:0.21  a: 0.211575 e: 0.211575
x:0.26  a: 0.263022 e: 0.263022
x:0.31  a: 0.315193 e: 0.315193
x:0.36  a: 0.368268 e: 0.368268
x:0.41  a: 0.422454 e: 0.422454
x:0.46  a: 0.477996 e: 0.477995
x:0.51  a: 0.535185 e: 0.535185
x:0.56  a: 0.594386 e: 0.594386
x:0.61  a: 0.656060 e: 0.656061
x:0.66  a: 0.720815 e: 0.720819
x:0.71  a: 0.789485 e: 0.789498
x:0.76  a: 0.863278 e: 0.863313
x:0.81  a: 0.944073 e: 0.944152
x:0.86  a: 1.035139 e: 1.035270
x:0.91  a: 1.143404 e: 1.143284
x:0.96  a: 1.291451 e: 1.287002

將0.9附近細分一下

x:0.90  a: 1.119760 e: 1.119769
x:0.91  a: 1.143404 e: 1.143284
x:0.92  a: 1.168431 e: 1.168081
x:0.93  a: 1.195150 e: 1.194413
x:0.94  a: 1.224027 e: 1.222630
x:0.95  a: 1.255752 e: 1.253236
x:0.96  a: 1.291451 e: 1.287002
x:0.97  a: 1.333107 e: 1.325231
x:0.98  a: 1.384628 e: 1.370462
x:0.99  a: 1.455034 e: 1.429257

在 $0<x<0.6$區間的精度最高, 在$0.6<x<0.9$區間結果數值偏小, 在$0.9<x<1$區間結果數值偏大. 越接近1精度越差, 實際使用時在大于$0.97$時需要做一些補償.

參考

• 用多項式快速計算三角函數等 https://www.olliw.eu/2014/fast-functions/