从旋转矩阵分解音高（y轴旋转）时的歧义答案

【问题标题】：ambiguity when decomposing pitch (yaxis rotation) from rotation matrix从旋转矩阵分解音高（y轴旋转）时的歧义
【发布时间】：2019-01-25 17:49:52
【问题描述】：

我得到一个 4x4 的机器人姿势，其中 z 轴朝前，x 轴朝东（右），y 轴朝下。

现在，为了提取机器人的航向，我使用了以下函数

void inline mat2xyh(Matrix4f& pose, float &x, float &y, float &heading){
    heading = atan2(-pose(2, 0),  sqrt(pose(2, 1) * pose(2, 1) +  pose(2,2) * pose(2,2)) );
    x = pose(0, 3);
    y = pose(2, 3); 
};

为了验证，我设置了下面的代码。当我检查分解的矩阵角度和我用来构造旋转矩阵的角度时，它们不匹配！

这是当我发现音高有两种独特的解决方案，或者音高的范围是-PI/2 < pitch < PI/2。

现在我不知道机器人是面向 45 度还是 135 度，有什么办法可以解决这个问题吗？

    Vector4f v(0, 0, 1, 0);

    MatrixXf rotation = AngleAxisf(135 * M_PI/180., Vector3f::UnitY()).toRotationMatrix();
    float x,y,h;
    Matrix4f pose1 = Matrix4f::Identity();
    pose1.topLeftCorner<3,3>() = rotation;

    mat2xyh(pose1, x,y,h);

    cout << pose1 << endl;
    cout << rad2deg(h) << endl;
    cout << "Pose: \n" << pose1 * v << endl;

    cout << "====================" << endl;

    MatrixXf rotation2 = AngleAxisf(45 * M_PI/180., Vector3f::UnitY()).toRotationMatrix();
    Matrix4f pose2 = Matrix4f::Identity();
    pose2.topLeftCorner<3,3>() = rotation2;

    mat2xyh(pose2, x,y,h);

    cout << pose2 << endl;
    cout << rad2deg(h) << endl;
    cout << pose2 * v << endl;

结果

-0.707107         0  0.707107         0
        0         1         0         0
-0.707107         0 -0.707107         0
        0         0         0         1
heading: 45
Pose: 
 0.707107
        0
-0.707107
        0
====================
 0.707107         0  0.707107         0
        0         1         0         0
-0.707107         0  0.707107         0
        0         0         0         1
heading:45
Pose: 
0.707107
       0
0.707107
       0

【问题讨论】：

您能否提供有关您的坐标系的更多详细信息？具体是机器人身体固定还是世界固定？您使用的术语不一致，因此很难说 XYZ 对应于 EDN（东、下、北）还是 RDF（右、下、前）。当机器人旋转时，差异很重要。
围绕下 (Y) 轴的旋转也是偏航，而不是俯仰。这可能会导致您的困惑。
嗨尼尔，谢谢你的回复。是的，我的意思是偏航。我不熟悉这些术语，但让我详细说明一下。我使用的数据集来自 Kitti，地图坐标系定义为机器人在时间 t=0 的第一个姿势。这有帮助吗？

标签： linear-algebra eigen rotational-matrices

【解决方案1】：

如果atan2 的第二个参数始终为非负数，则只能得到-pi/2 和pi/2 之间的角度。如果你想要-pi和pi之间的角度，你可以计算，例如，

heading = atan2(pose(0, 2), pose(2,2));

顺便说一句：正如@Neil 在 cmets 中所说，“航向”也称为“偏航”。 'Pitch' 和 'roll' 也被称为 'attitude' 和 'bank'。

另外：z 轴指向上方（或下方）而 x 轴指向前方的情况并不少见，尤其是在通常仅在表面上移动的区域中。在计算机视觉中，z 轴指向前方，而 x 轴指向右侧。即，确实要确保您知道您的数据集使用什么约定！

要了解有关欧拉角的更多信息，我建议阅读以下内容：

【讨论】：

chtz... 试图围绕你提到的内容。对于航向 = atan2(pose(0, 2), pose(2,2));它是否适用于所有场景，例如滚动、俯仰和偏航都是非零的？
它总会给你一些或多或少有意义的结果。如果您的滚动或俯仰角接近 90°，您可能会遇到奇点。在这些情况下，欧拉角是非常糟糕的旋转表示。尽管。本质上，您需要问自己的问题是，您期望“偏航”是什么？
“有意义的结果”是什么意思？机器人是一辆在平面上运动的汽车，偶尔会有小幅俯仰和滚动，所以它不太可能会遇到云台锁定？不知道你所说的我期望的偏航是什么意思。本质上，我得到了机器人相对于地图的 4x4 姿势，所以我只需要提取一个相当准确的机器人前进方向的估计。这会给你一个更好的主意吗？
如果您也不知道您所指的偏航角到底是什么，我无法为您提供更多帮助。如果您的俯仰角和侧倾角大约为 10°，则根据您使用的约定，您可能会得到大约 1.5% 的误差，因此您可能不必太担心。
谢谢！但是当你说我不知道我指的是什么偏航角时，我能理解你需要什么信息吗？我知道偏航（当我写 pitch 时这是一个错字）我需要的是围绕 y 轴的旋转，我知道如果我使用不同的约定，我可能会得到不同的结果）