滚动，俯仰，偏航计算

Question

如何计算与均匀变换矩阵相关的滚转角，俯仰角和偏航角？

我目前正在使用以下公式，但我不确定它们是否正确。

pitch = atan2( -r20, sqrt(r21*r21+r22*r22) );
yaw   = atan2(  r10, r00 );
roll  = atan2(  r21, r22 );

r10表示第二行和第一列。

Answer 1

只有旋转顺序为：滚动，然后是俯仰，然后是偏航，你的方程才是正确的。对于记录，与欧拉角的对应关系（相对于用变换矩阵隐式给出的参照系）如下：

• Roll 是围绕 x轴的旋转（介于-180和180度之间）;
• Pitch 是关于 y轴的旋转（介于-90和90度之间）;
• Yaw 是围绕 z轴的旋转（介于-180和180之间）。

鉴于这些，第一句中提到的顺序 roll，pitch，yaw 对应于矩阵乘积Rz Ry Rx获得的旋转矩阵（按此顺序）。请注意，您的公式给出了这些角度的值以弧度（乘以180并除以pi以获得以度为单位的值）。所有旋转都相对于轴逆时针旋转。

取自Wikipedia

的图

---

根据您对this链接的评论，我认为this paper可能有助于了解您所指的计划。 Matlab函数的输入应该是你的变换矩阵，如果你想要以度为单位返回角度，则返回'deg';如果旋转的顺序是z左右，则返回过时选项'zyx'，然后是y ，然后围绕x。

Answer 2

[这可能更适合作为评论，但它可以很长时间;）

当我将您的公式与德语维基百科页面上的公式进行比较时，关于滚动，俯仰偏航（see here），音高的计算会有所不同。根据维基百科，您的公式应如下所示：

pitch = atan2(-r20,(sqrt(pow(r21,2)+pow(r00,2))); // replaced r22 by r00

请注意，在维基百科页面上，他们对矩阵元素使用不同的索引（thex以1开头，而第一行/列不是0）。此外，他们称音调beta，偏航alpha和滚动伽玛。此外，他们将偏航和滚动计算中atan2的系数除以cos(pitch)，但这应取消。

否则你的公式对我来说很好。

Answer 3

＆＃34; Roll是围绕x轴的旋转（介于-180和180度之间）; 间距是围绕y轴的旋转（介于-90和90度之间）; 偏航是围绕z轴的旋转（介于-180和180之间）。＆＃34;

上面发布的这个引用的答案中有一个错误。虽然这个数字是准确的，但＆＃34; roll＆＃34;和＃34;音调＆＃34;在引用的答案中已经互换了。它应该是：

＆＃34; Roll是围绕y轴的旋转......; 间距是围绕x轴的旋转......; 偏航是围绕z轴的旋转......＆＃34;

我希望这对你有所帮助。

Answer 4

您可以从下一张图片中获得所有方程式。那里有免费的/卡尔曼滤波器，以减少噪音。