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

Question

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

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

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

Θ=arccos(v.vf)

然后 python 计算一组给定方向的 Θ 并拉出最大的方向应该相当简单。

我的问题是，是否有更简单的方法使用 python 执行此操作，如果没有，将这些固有旋转转换为单位向量的最有效方法是什么。

Answer 1

假设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最大化。

Answer 2

我认为球面三角学会对你有所帮助。 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;
}

两个pitch是球面三角形的边，delta roll是它们之间的夹角。为了计算剩余边，我们使用半正弦法则。我还提供了一个角度的经纬度参数化解决方案，以防您指定方向而不是使用偏航俯仰。

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

I need to find the angle between two sets of Roll and Yaw angles

python

math

trigonometry

angle

euler-angles