黑匣子搜寻方向角和俯仰角求解方法

 

1 前言

项目做黑匣子搜寻，需要计算黑匣子在AUV的方向角和俯仰角。以AUV为参考，右手坐标系，Z向下，如下图所示。已知黑匣子坐标(x, y, z)。

2 方向角

方向角指的是采用某坐标轴方向作为标准方向所确定的方位角。有时，方向角是从正北或正南方向到目标方向所形成的小于九十度的角。

 这里，以AUV右侧方向角为+，左侧方向角为-。以反正切函数为例，公式如下：

atan() 函数的功能是求反正切值。反正切函数 atan() 和正切函数 tan() 的功能正好相反。tan() 是已知一个角的弧度值 x，求该角的正切值 y；而 atan() 是已知一个角的正切值 y，求该角的弧度值 x。 

*atan求解结果为弧度，公式已经转换为角度。

C语言用法如下：

#include <stdio.h>
#include <math.h>
#define PI 3.14159265
int main ()
{
    double x, y, alpha;
    x = -100;
    y = -100;
    alpha = atan ( y / x ) * 180 / PI;
    return 0;
}

alpha = 45°。

但是，atan() 并不能确定角度所在的象限，例如求得的度数是 45°，并不能说明是第一象限的角度，还有可能是第三象限的角度。因此需要用到atan2()函数。

atan2() 函数的功能是求 y/x 的反正切值。atan2() 是 atan() 的增强版，能够确定角度所在的象限。

反正切函数 atan2() 和正切函数 tan() 的功能恰好相反：tan() 是已知一个角的弧度值，求该角的正切值；而 atan2() 是已知一个角的正切值（也就是 y/x），求该角的弧度值。

C语言用法如下：

#include <stdio.h>
#include <math.h>
#define PI 3.14159265
int main ()
{
    double x, y, alpha;
    x = -100;
    y = -100;
    alpha = atan2 ( y, x ) * 180 / PI;
    return 0;
}

alpha = 135°。

当（x,y）在象限中时：

• 当 (x, y) 在第一象限，0 < θ < π/2；
• 当 (x, y) 在第二象限，-π/2 < θ < 0；
• 当 (x, y) 在第三象限，-π < θ < -π/2；
• 当 (x, y) 在第四象限，π/2 < θ < π。

当 (x, y) 在象限的边界上时：

• 当 y 是 0，且 x 为非负值，θ = 0；
• 当 y 是 0，且 x 是负值，θ = π；
• 当 y 是正值，且 x 是 0，θ = π/2；
• 当 y 是负值，且 x 是 0，θ = -π/2。

*由此可知，一般情况下用 atan() 即可，当对所求出角度的象限有特殊要求时，应使用 atan2()。

*注意，x 和 y 同时为 0 时将导致域错误，因为此时的角度是不存在的，或者说是没有意义的。

*除了atan()反正切函数，使用asin()反正弦函数和acos()反余弦函数也可以求解方向角。

3 俯仰角

机体坐标系x轴与水平面的夹角。当机体坐标系的x轴在惯性坐标系XOY平面上方时，俯仰角为正，否则为负。即平行于机身轴线并指向飞行器前方的向量与地面的夹角。

这里，俯仰角是黑匣子与AUV平面的夹角。

求解公式为：

上式中，fabs()为浮点数绝对值函数，sqrt()为开方函数，并且将弧度转换为角度。通常认为，俯仰角范围为[0, π]，因此不存在负数。

C语言用法如下：

#include <stdio.h>
#include <math.h>
#define PI 3.14159265
int main ()
{
    double x, y, z, beta;
    x = -100;
    y = -100;
    z = 100;
    beta = fabs ( atan ( Z / sqrt ( X * X + Y * Y ) ) ) * ( 180 / PI );
    return 0;
}

beta = 35.26°（四舍五入）。

---

参考资料：博客园_C语言中的atan和atan2：https://www.cnblogs.com/dutlei/archive/2013/01/14/2860332.html

                  C语言中文网_<math.h>头文件详解：http://c.biancheng.net/ref/math_h/

          百度百科_方向角：https://baike.baidu.com/item/%E6%96%B9%E5%90%91%E8%A7%92/3163563?fr=aladdin

          百度百科_俯仰角：https://baike.baidu.com/item/%E4%BF%AF%E4%BB%B0%E8%A7%92