在嵌入式设备中用多项式快速计算三角函数和方根

来源：cnblogs　　作者：Milton　　时间：2024/3/4 9:17:18　　对本文有异议

惯性传感器的倾角计算要用到三角函数.

在 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$ 的多项式近似计算为

\[\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 \]

如果将上面的符号固定项和变化项分成$A$和$B$两部分

\[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 \]

则 $\sin\alpha$ 和 $\cos\alpha$ 可以通过 A 和 B 的值表达

\[\sin\alpha = A - B \\cos\alpha = A + B \]

对应的各项系数值

$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$

使用上面的计算方式, 结果绝对误差小于$6.5 \times 10^{-6}$, 并且 $\cos^2 x + \sin^2 x$ 不会超过 1. 计算过程只需要7次乘法和7次加法.

C语言实现

const   float coeff[7] = {
  // a0 ~ a6           b1 ~ b5
   0.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 函数对比, 数值几乎一样, 仅在个别数值的小数点后第五位会有$\pm1$的差异.

平方根倒数计算

对于1附近的数值, 平方根倒数可以使用牛顿迭代法计算, 实际上非常简单，因为它只涉及加法和乘法，而不涉及除法, 对于 $x \in [0.6, 1.4]$, 计算式为

\[y_0 = 1 \y_{n+1} = y_n (1.5 - 0.5 x {y_n}^2) \\]

计算两次牛顿迭代需要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_2x^2 + a_4x^4 + a_6x^6}{b_0 + b_2x^2 + b_4x^4 + b_6x^6} \]

对应的系数数值为
$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$

当 $|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 ~ a6
   0.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