四元数、欧拉角、旋转矩阵、旋转向量之间的转换

泠山

已于 2024-04-11 16:33:55 修改

阅读量8.6k

点赞数 17

分类专栏： Math 文章标签：矩阵线性代数几何学

于 2022-02-09 21:11:00 首次发布

本文链接：https://blog.csdn.net/qq_28087491/article/details/122830550

版权

Math 专栏收录该内容

7 篇文章 1 订阅

订阅专栏

四元数、欧拉角、旋转矩阵、旋转向量之间的转换

1. 欧拉旋转定理
2. 轴角表示
- 2.1 旋转性质
3. 罗德里格斯公式
4. pitch yaw roll方向
5. 内旋和外旋
6. 使用哪种表示方法？
7. 四元数、欧拉角、旋转矩阵、旋转向量之间的转换

1. 欧拉旋转定理

在运动学里，欧拉旋转定理(Euler's rotation theorem)表明，在三维空间里，假设一个刚体在做一个旋转的时候，刚体内部至少有一点固定不动，则此位移等价于一个绕着包含那固定点的固定轴的旋转。
在这里插入图片描述

2. 轴角表示

旋转一个向量从 $a$ 到 $b$ ，轴角为 $(u,\theta)$ ，如下图所示：
在这里插入图片描述
罗德里格斯公式：
$b=acos\theta+(u\times a)sin\theta+u(u\cdot a)(1-cos\theta)$

2.1 旋转性质

$(u,\theta)$ 和 $(-u,-\theta)$ 为相同旋转。
$(u,-\theta)$ 为 $(u,\theta)$ 的逆旋转。
旋转运算是不能交换的，如：
$a=\left[\begin{array}{lll}3 & 0 & 0\end{array}\right]^{\top} \quad\left(u_{1}, \theta_{1}\right)=\left(\left[\begin{array}{lll}1 & 0 & 0\end{array}\right]^{\top}, 90^{\circ}\right) \quad\left(u_{2}, \theta_{2}\right)=\left(\left[\begin{array}{lll}0 & 1 & 0\end{array}\right]^{\top}, 90^{\circ}\right)$
情况一：
旋转 $a$ $(u_1,\theta_1)$ ，得到 $b=a=\left[\begin{array}{lll}3 & 0 & 0\end{array}\right]^{\top}$
旋转 $b$ $(u_2,\theta_2)$ ，得到 $c=\left[\begin{array}{lll}0 & 0 & 3\end{array}\right]^{\top}$
情况二：
旋转 $a$ $(u_2,\theta_2)$ ，得到 $b=a=\left[\begin{array}{lll}0 & 0 & 3\end{array}\right]^{\top}$
旋转 $b$ $(u_1,\theta_1)$ ，得到 $c=\left[\begin{array}{lll}0 & 3 & 0\end{array}\right]^{\top}$

3. 罗德里格斯公式

向量 $a, b$ 可以被分解成相对于 $u$ 的正交 $(a_{\perp}, b_{\perp})$ 和平行 $a_{\|}, b_{\|})$ 分量。
在这里插入图片描述此时以下关系是成立的：
$a_{\|}=b_{\|}\\ b_{\perp}=a_{\perp} \cos \theta+k \sin \theta$ 因此：
$\begin{aligned} b &=b_{\perp}+b_{\|}=b_{\perp} \cos \theta+k \sin \theta+u(u \cdot a) \\ &=a \cos \theta-u(u \cdot a) \cos \theta+(u \times a) \sin \theta+u(u \cdot a) \\ &=a \cos \theta+(u \times a) \sin \theta+u(u \cdot a)(1-\cos \theta) \end{aligned}$

在这里插入图片描述

4. pitch yaw roll方向

roll: 翻滚角，顺时针翻滚即正值，逆时针翻滚为负值(图左)。
yaw(heading): 航向角(图中)。
pitch: 俯视角，俯仰是从局部 $x y$ 平面的旋转(图右)。

5. 内旋和外旋

按旋转的坐标系分为内旋(intrinsic rotation)和外旋(extrinsic rotation)。

内旋: 绕物体自身的坐标系(object-space)旋转，举个例子，一个 $(\phi, \theta, \psi)$ $(x yz, in t r in s i c)$ 的欧拉角，指绕物体的 $x$ 轴转 $\phi$ 后，再绕物体的 $y^{'}$ 轴(这里用 $y^{'}$ 表示这个新的 $y$ 轴已经和一开始世界坐标系下的那个物体的 $y$ 轴不一样了)旋转 $\theta$ ，最后绕 $z^{'}$ 轴旋转 $\psi$ ，每一次旋转都会改变下一次旋转的轴。这种情况下旋转的轴是动态(moving axis)的。

外旋: 绕惯性系(upright-space)旋转(upright space 指基向量平行于 world-space 或 parent-space，原点与 object-space 的原点重合的空间)。也就是说，无论是三步旋转中的哪一步，轴都是固定不会动的。

在这里插入图片描述内旋与外旋的转换关系：互换第一次和第三次旋转的位置则两者结果相同。例如 $Z - Y - X$ 旋转的内部旋转和 $X - Y - Z$ 旋转的外部旋转的旋转矩阵相同。

证明：
假设绕 $X Y Z$ 三个轴旋转的角度分别为 $\alpha$ $\beta$ $\gamma$ ，则三次旋转的旋转矩阵计算方法如下：
$\begin{array}{l} R_{x}(\alpha)=\left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & \cos \alpha & -\sin \alpha \\ 0 & \sin \alpha & \cos \alpha \end{array}\right] \\ R_{y}(\beta)=\left[\begin{array}{ccc} \cos \beta & 0 & \sin \beta \\ 0 & 1 & 0 \\ -\sin \beta & 0 & \cos \beta \end{array}\right] \\ R_{z}(\gamma)=\left[\begin{array}{ccc} \cos \gamma & -\sin \gamma & 0 \\ \sin \gamma & \cos \gamma & \\ 0 & 0 & 1 \end{array}\right] \end{array}$

按照内旋方式， $Z - Y - X$ 旋转顺序（指先绕自身轴 $Z$ ，再绕自身轴 $Y$ ，最后绕自身轴 $X$ ），可得旋转矩阵(内旋是右乘)：
$1=R_{z}(\gamma) * R_{y}(\beta) * R_{x}(\alpha)$

按照外旋方式， $X - Y - Z$ 旋转顺序（指先绕固定轴 $X$ ，再绕固定轴 $Y$ ，最后绕固定轴 $Z$ ），可得旋转矩阵(外旋是左乘)：
$2=R_{z}(\gamma) * R_{y}(\beta) * R_{x}(\alpha)$

因此 $R 1 = R 2$ ， $Z Y X$ 顺序的内旋等价于 $X Y Z$ 顺序的外旋。

SLAM十四讲中提到的常用旋转顺序是 $Z - Y - X$ ，对应 RPY(Roll-Pitch-Yaw) 角，指的就是内旋（绕自身轴） $Z - Y - X$ 顺序。而欧拉角转换成旋转矩阵（相对于世界坐标系的旋转矩阵）通常是按外旋方式（绕固定轴），即 $X - Y - Z$ 顺序，所以旋转矩阵为：
$2=R_{z}(\gamma) * R_{y}(\beta) * R_{x}(\alpha)$

6. 使用哪种表示方法？

奇异性和不连续性: 用很少的参数表示可能导致数值问题：
欧拉角(3)：
– 奇异性：万向锁配置
– 不连续性：从 0° 跳变到 360°

旋转向量(3)：
– 不连续性：从 0° 跳变到 360°

旋转矩阵(9) / 四元数(4)：
– 没有奇异性或不连续性
几何解释: 有些表示法更容易用几何解释。它们更接近原始的传感器测量，也更容易与人类用户交互(显示或输入)
欧拉角：
– 参数是接近几何解释的角度和轴

旋转矩阵 / 四元数：
– 参数是正弦/余弦函数，其几何解释有些无关紧要

7. 四元数、欧拉角、旋转矩阵、旋转向量之间的转换

7.1 旋转向量

初始化旋转向量：旋转角为 alpha，旋转轴为 $(x, y, z)$

Eigen::AngleAxisd rotation_vector(alpha,Vector3d(x,y,z))；

旋转向量转旋转矩阵

Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.matrix();

Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.toRotationMatrix();

旋转向量转欧拉角(Z-Y-X内旋，即 RPY)

//(2,1,0)表示旋转顺序ZYX，数字越小表示优先级越高
Eigen::Vector3d eulerAngle=rotation_vector.matrix().eulerAngles(2,1,0);

旋转向量转四元数

Eigen::Quaterniond quaternion(rotation_vector);

7.2 旋转矩阵

初始化旋转矩阵

Eigen::Matrix3d rotation_matrix;
rotation_matrix<<x_00,x_01,x_02,x_10,x_11,x_12,x_20,x_21,x_22;

旋转矩阵转旋转向量

Eigen::AngleAxisd rotation_vector(rotation_matrix);

Eigen::AngleAxisd rotation_vector;
rotation_vector=rotation_matrix;

Eigen::AngleAxisd rotation_vector;
rotation_vector.fromRotationMatrix(rotation_matrix);

旋转矩阵转欧拉角(Z-Y-X内旋，即 RPY)

Eigen::Vector3d eulerAngle=rotation_matrix.eulerAngles(2,1,0);

旋转向量转四元数

Eigen::Quaterniond quaternion(rotation_matrix);
Eigen::Quaterniond quaternion;
quaternion=rotation_matrix;

7.3 欧拉角

初始化欧拉角(Z-Y-X，即RPY)

Eigen::Vector3d eulerAngle(yaw,pitch,roll);

欧拉角转旋转向量

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitZ()));
Eigen::AngleAxisd rotation_vector;
rotation_vector=yawAngle*pitchAngle*rollAngle;

欧拉角转旋转矩阵

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitZ()));
Eigen::Matrix3d rotation_matrix;
rotation_matrix=yawAngle*pitchAngle*rollAngle;

欧拉角转四元数

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitZ()));
Eigen::Quaterniond quaternion;
quaternion=yawAngle*pitchAngle*rollAngle;

7.4 四元数

初始化四元数

Eigen::Quaterniond quaternion(w,x,y,z);

四元数转旋转向量

Eigen::AngleAxisd rotation_vector(quaternion);

Eigen::AngleAxisd rotation_vector;
rotation_vector=quaternion;

四元数转旋转矩阵

Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.matrix();

Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.toRotationMatrix();

四元数转欧拉角(Z-Y-X，即RPY)

// yaw pitch roll
Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(2,1,0);

下面代码与上面接口可得到相同结果：

static Eigen::Vector3d getEulerFromQuternion(const Eigen::Quaterniond& q){
    double roll, pitch ,yaw;

    // roll (x-axis rotation)
    double sinr_cosp = +2.0 * (q.w() * q.x() + q.y() * q.z());
    double cosr_cosp = +1.0 - 2.0 * (q.x() * q.x() + q.y() * q.y());
    roll = atan2(sinr_cosp, cosr_cosp);

    // pitch (y-axis rotation)
    double sinp = +2.0 * (q.w() * q.y() - q.z() * q.x());
    if (fabs(sinp) >= 1)
        pitch = copysign(M_PI / 2, sinp); // use 90 degrees if out of range
    else
        pitch = asin(sinp);

    // yaw (z-axis rotation)
    double siny_cosp = +2.0 * (q.w() * q.z() + q.x() * q.y());
    double cosy_cosp = +1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z());
    yaw = atan2(siny_cosp, cosy_cosp); 

    Eigen::Vector3d eulers; 
    eulers << yaw, pitch, roll;
    return eulers;
}