我需要找到两组Roll和Yaw角度之间的角度

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;
}

两个节距是球形三角形的边，三角形卷是它们之间的角度。为了计算剩下的一面，我们使用了半身定律。我还提供了一个解决方案，用于角度的纬度参数化，以防您使用偏航间距指定方向。

Answer 2

假设u(1), u(2), ..., u(m), v是所有单位向量。您想确定i，以使u(i)和v之间的角度最大化。这相当于找到i以使np.dot(u(i), v)最小化。因此，如果您有一个矩阵U，其中行是u(i)，您只需执行i = np.argmin(np.dot(U, v))即可查找i与u(i)之间的角度v最大化。