旋转矩阵与欧拉角的相互转换及代码

最新推荐文章于 2024-11-15 15:30:01 发布

原创

最新推荐文章于 2024-11-15 15:30:01 发布 · 1.3w 阅读

67 ·

CC 4.0 BY-SA版权

文章标签：

#旋转矩阵 #欧拉角

本文详细介绍了旋转矩阵和欧拉角之间的转换方法，包括从欧拉角到旋转矩阵的转换公式，以及如何在OpenCV中实现从旋转矩阵到欧拉角的转换。文章还提供了C++和Python代码示例。

这篇博客将会分享旋转矩阵和欧拉角的相互转换。

三维旋转矩阵有三个自由度，旋转能够使用多种方法表示（旋转矩阵，欧拉角，四元数，轴角，李群与李代数），比如一个3x3的矩阵，比如四元数，甚至可以将旋转表示为三个数字，即绕三个轴x,y,z的旋转角度。在原始的欧拉角公式中，旋转是通过围绕Z轴，Y轴，X轴分别旋转得到的。它分别对应于偏航，俯仰和横滚。

当定义一个旋转时，可能还会引起歧义，比如对于一个给定的点(x,y,z),可以把这个点视为行向量（x,y,z）或者列向量量 $x, y, z]^{T}$ ，如果使用行向量，那么就必须后乘旋转矩阵（旋转矩阵在后），如果使用列向量，就要左乘旋转矩阵（旋转矩阵在前）。

在MATLAB中，rotm2euler.m能够实现旋转矩阵到欧拉角的转换。下文中将要叙述的转换代码参考于MATLAB中的rotm2euler.m实现。不同于MATLAB实现的是，它的旋转顺序是Z-Y-X，而下面的实现是X-Y-Z。

在计算将旋转矩阵转换成欧拉角之前，先介绍一下欧拉角转换为旋转矩阵的方法。

欧拉角转换为旋转矩阵

假如已知旋转角，绕着X-Y-Z三个轴的角度分别为 $\theta_{x} \quad \theta_{y} \quad \theta_{z}$ 那么三个旋转矩阵可以表示如下：
在这里插入图片描述
如果旋转顺序为Z-Y-X的顺序，那么旋转矩阵可表示如下：

关于上面的旋转矩阵，以z轴旋转为例：
第一种物理意义，坐标系的旋转。 其应用场景有SLAM，机械臂运动学等。

如上图所示， P点不变，坐标系 O-XYZ旋转 α ,得到新的坐标系O-X’Y’Z’，在新坐标系下，P点的坐标变为P’ ，则有：
在这里插入图片描述
第二种物理意义，向量的旋转，其应用场景有机器人的姿态估计等。
向量旋转
如上图所示，坐标系 O-XYZ不变， P’点旋转α ,得到新的点P （我也想反过来，但用的教材上的图，没办法），则有：

对应于上述旋转顺序的C++代码如下：

C++

// Calculates rotation matrix given euler angles.
Mat eulerAnglesToRotationMatrix(Vec3f &theta)
{
   
   
    // Calculate rotation about x axis
    Mat R_x = (Mat_<double>(3,3) <<
               1,       0,              0,
               0,       cos(theta[0]),   -sin(theta[0]),
               0,       sin(theta[0]