python - 我需要找到两组横滚角和偏航角之间的角度

Question

我在钻头上安装了一个传感器。传感器输出航向横滚和俯仰方向。据我所知，这些是按顺序进行的内在旋转。传感器的Y轴平行于钻头的纵轴。我想从传感器获取一组输出，并找到相对于最终方向的最大方向变化。

由于钻头会绕俯仰轴旋转，我相信它可以忽略不计。

我的第一个想法是尝试将航向和滚转转换为单位向量，假设俯仰为 0。一旦我有了它们的向量，称它们为 v 和 vf，它们之间的角度将为

θ=arccos(v . vf)

然后让 python 计算给定方向集的 θ 并提取最大的值应该是相当简单的。

我的问题是，是否有一种更简单的方法可以使用 python 来完成此操作，如果没有，将这些内在旋转转换为单位向量的最有效方法是什么。

Answer 1

我认为球面三角学会对你有所帮助。 https://en.wikipedia.org/wiki/Spherical_trigonometry https://en.wikipedia.org/wiki/Haversine_formula#The_law_of_haversines

如果我理解正确，下面的函数应该可以解决问题:

double AngleBetweenRollPitch(double roll1, double pitch1, double roll2, double pitch2) {
    return SphericalSideFromSidesAndIncludedAngle(pitch1, pitch2, roll2 - roll1);
}
double SphericalSideFromSidesAndIncludedAngle(double b, double c, double alpha) { /* the Law of haversines */
    return archaversin(haversin(b - c) + sin(b) * sin(c) * haversin(alpha));
}
double SphericalAngleBetweenTwoLongLats(double long0, double lat0, double long1, double lat1) {
    return archaversin(haversin(lat1 - lat0) + cos(lat0) * cos(lat1) * haversin(long1 - long0));
}
double haversin(double x) {
    x = sin(x * .5);
    x *= x;
    return x;
}
double archaversin(double x) {
    if      (x >= 1.)   x = M_PI * .5;
    else if (x <= 0.)   x = 0.;
    else                x = asin(sqrt(x)) * 2.;
    return x;
}

两个节距是球面三角形的边，而 Delta Roll 是它们之间的角度。为了计算剩余边，我们使用半正矢定律。我还提供了角度经纬度参数化的解决方案，以防您指定方向而不是使用偏航俯仰。