车辆姿态表达：旋转矩阵、欧拉角、四元数的转换以及eigen、matlab、pathon方法实现

1 概述

旋转矩阵、欧拉角、四元数主要用于表示坐标系中的旋转关系，通过三者之间的转换可以减小一些算法的复杂度。
本文主要概述旋转矩阵、欧拉角、四元数的基本理论、三者之间的转换关系以及三者转换在eigen、matlab和pathon上的方法实现。

2 原理

2.1 旋转矩阵

对于两个三维点p1、p2：
$$p_1\left(x_1,y_1,z_1\right),p_2\left(x_2,y_2,z_2\right)$$
由点p1经过旋转矩阵R旋转到p2，则有：
$$R=\left[\begin{array}{lll}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right],\left[\begin{array}{l}{x}_2\\ y_2\\ z_2\end{array}\right]=R\left[\begin{array}{l}{x}_1\\ y_1\\ z_1\end{array}\right]$$

其中，旋转矩阵为正交矩阵RR^T=E。

2.1.1 绕x轴旋转

$$R_{\chi }(\theta )=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \theta & -\sin \theta \\ 0& \sin \theta & \cos \theta \end{array}\right]$$

2.1.2 绕y轴旋转

$$R_y(\theta )=\left[\begin{array}{ccc}\cos \theta & 0& \sin \theta \\ 0& 1& 0\\ -\sin \theta & 0& \cos \theta \end{array}\right]$$

2.1.3 绕z轴旋转

$$R_z(\theta )=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & 0\\ \sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]$$
三维空间内任意一个旋转可表示为依次绕着以上三个旋转轴旋3个角度的组合。这3个角度称为欧拉角。
这三个轴可以指固定的世界坐标系轴，也可以指被旋转的物体坐标系的轴。绕轴旋转次序不同，会导致结果也不同。

2.2 欧拉角

对于在三维空间里的一个参考系，任何坐标系的取向，都可以用三个欧拉角来表现。参考系又称为实验室参考系，是静止不动的，而坐标系则固定于刚体，随着刚体的旋转而旋转。

设定xyz-轴为参考系的参考轴。称xy-平面与XY-平面的相交为交线，用英文字母（N）代表。

图中，三个欧拉角分别为：(α,β,γ)，其中：

$$\alpha \in [0,2\pi ],\beta \in [0,\Pi ],\gamma \in [0,2\pi ]$$蓝色的轴为：xyz轴。 红色的轴为：XYZ轴。 绿色的线为交线：N。

2.2.1 基本思想

欧拉角的基本思想是将角位移分解为绕三个互相垂直轴的三个旋转组成的序列。所以，欧拉旋转的三个角，可以对应于Yaw、Pitch和Roll三个旋转矩阵。

• Yaw（偏航）：欧拉角向量的y轴；
• Pitch（俯仰）：欧拉角向量的x轴；
• Roll（翻滚）：欧拉角向量的z轴。

2.2.2 欧拉角的缺点

1. 欧拉角的表示方式不唯一。给定某个起始朝向和目标朝向，即使给定yaw、pitch、roll的顺序，也可以通过不同的yaw/pitch/roll的角度组合来表示所需的旋转。这主要是由于万向锁(Gimbal Lock)引起的；
2. 欧拉角的插值比较难；
3. 计算旋转变换时，一般需要转换成旋转矩阵，这时候需要计算很多sin, cos，略复杂。

2.3 四元数

三维空间的任意旋转，都可以用绕三维空间的某个轴旋转过某个角度来表示，即所谓的Axis-Angle表示方法。这种表示方法里，Axis可用一个三维向量(x,y,z)来表示，θ可以用一个角度值来表示，直观来讲，一个四维向量(θ,x,y,z)就可以表示出三维空间任意的旋转。注意，这里的三维向量(x,y,z)只是用来表示axis的方向朝向，因此更紧凑的表示方式是用一个单位向量来表示方向axis，而用该三维向量的长度来表示角度值θ。这样以来，可以用一个三维向量(θx,θy,θz)就可以表示出三维空间任意的旋转，前提是其中(x,y,z)是单位向量。这就是旋转向量(Rotation Vector)的表示方式，OpenCV里相机标定部分的rvec就是用这种方法来表示旋转。

单位向量(x,y,z)旋转角度θ后的四元数：
$$\left(\cos \frac{\theta }{2},x\ast \sin \frac{\theta }{2},y\ast \sin \frac{\theta }{2},z\ast \sin \frac{\theta }{2}\right)$$
对于三维坐标的旋转，可以通过四元数乘法直接操作，与旋转矩阵操作可以等价，但是表示方式更加紧凑，计算量也可以小一些。

2.3.1 四元数的复数定义

$$\mathbf{q}=q_0+q_{1}i+q_{2}j+q_{3}k=[s,\mathbf{v}]$$

其中，q₀, q₁, q₂, q₃均为实数，s=q₀， v=[q₁, q₂, q₃]，i²=j²=k²=-1 。

对于i,j,k本身的几何含义可以理解为一种旋转，其中i代表x轴与y轴相交平面中x轴正向向y轴正向的旋转，j旋转代表z轴与x轴相交平面中z轴正向向x轴正向的旋转，k旋转代表y轴与z轴相交平面中y轴正向向z轴正向的旋转，-i,-j,-k分别代表i,j,k的反向旋转。

2.3.2 四元数的缺点

1. 非奇异表达（和欧拉角之类的表示相比）；
2. 比矩阵更紧凑（更快速）；
3. 单位四元数的对可以表示四维空间中的一个旋转。

3 三者转换计算公式

由欧拉角求旋转矩阵

设三个轴x,y,z的欧拉角分别为θ_x，θ_y，θ_z，正弦值、余弦值分别为s_x,c_x,s_y,c_y,s_z,c_z。那么旋转矩阵为：
$$\begin{array}{l}R\left({\theta }_x,{\theta }_y,{\theta }_z\right)=R_z\left({\theta }_z\right)R_y\left({\theta }_y\right)R_x\left({\theta }_x\right)\\ =\left[\begin{array}{ccc}{C}_y{C}_z& C_z{S}_x{S}_y-C_x{S}_z& S_x{S}_z+C_x{C}_z{S}_y\\ C_y{S}_z& C_x{C}_z+S_x{S}_y{S}_z& C_x{S}_y{S}_z-C_z{S}_z\\ -S_y& C_y{S}_x& C_x{C}_y\end{array}\right]\end{array}$$

3.2 由旋转矩阵求欧拉角

$$\begin{array}{l}R=\left[\begin{array}{lcc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]\\ =\left[\begin{array}{ccc}{C}_y{C}_z& C_z{S}_x{S}_y-C_x{S}_z& S_x{S}_z+C_x{C}_z{S}_y\\ C_y{S}_z& C_x{C}_z+S_x{S}_y{S}_z& C_x{S}_y{S}_z-C_z{S}_z\\ -S_y& C_y{S}_x& C_x{C}_y\end{array}\right]\end{array}$$
解方程得：
$$\begin{array}{c}{\theta }_x=\mathrm{atan}2\left(r_{32},r_{33}\right)\\ {\theta }_y=\mathrm{atan}2\left(-r_{31},\sqrt{r_{32}^2+r_{33}^2}\right)\\ {\theta }_z=\mathrm{atan}2\left(r_{21},r_{11}\right)\end{array}$$

其中：atan2(y,x)求的是y/x的反正切，其返回值为[-pai, +pai]之间的一个数。

3.3 由四元数求旋转矩阵

• 情况1：

  将四元数转化为轴角θ与向量(x,y,z)：
  $$\mathbf{q}={\left(\begin{array}{llll}\theta & x& y& z\end{array}\right)}^T$$
  利用Rodrigues公式可以由单位向量旋转角度θ后的四元数求得旋转矩阵R：
  $$\begin{array}{c}R=e^{\omega \theta }=I+{\omega }_{\times }\sin \theta +{\omega }_{\times }^2(1-\cos \theta )=\\ \left[\begin{array}{ccc}\cos \theta +x^2(1-\cos \theta )& -z\sin \theta +xy(1-\cos \theta )& y\sin \theta +xz(1-\cos \theta )\\ z\sin \theta +xy(1-\cos \theta )& \cos \theta +y^2(1-\cos \theta )& -x\sin \theta +yz(1-\cos \theta )\\ -y\sin \theta +xz(1-\cos \theta )& x\sin \theta +yz(1-\cos \theta )& \cos \theta +z^2(1-\cos \theta )\end{array}\right]\end{array}$$

• 情况2：

  已知四元数为，
  $$\mathbf{q}=q_0+q_{1}i+q_{2}j+q_{3}k=[s,\mathbf{v}]$$
  利用Rodrigues公式可以由四元数求得旋转矩阵R，
  $$\mathbf{q}=q_0+q_{1}i+q_{2}j+q_{3}k=[s,\mathbf{v}]$$
  $R$
  $$=[\begin{array}{ccc}1-2q_2^2-2q_3^2& 2q_1{q}_2-2q_0{q}_3& 2q_1{q}_3+2q_0{q}_2\\ 2q_1{q}_2+2q_0{q}_3& 1-2q_1^2-2q_3^2& 2q_2{q}_3-2q_0{q}_1\\ 2q_1{q}_3-2q_0{q}_2& 2q_2{q}_3+2q_0{q}_1& 1-2q_1^2-2q_2^2\end{array}$$

3.4 由旋转矩阵求四元数

$$\begin{array}{c}{q}_0=\frac{\sqrt{1+r_{11}+r_{22}+r_{33}}}{2}.\\ q_1=\frac{r_{32}-r_{23}}{4q_0}.\\ q_2=\frac{r_{13}-r_{31}}{4q_0}\\ q_3=\frac{r_{21}-r_{12}}{4q_0}.\end{array}$$
其中, 要满足 $q_0\ne 0,1+r_{11}+r_{22}+r_{33}>0$, 即 $1+\mathrm{tr}(R)>$ 0。若 $q_0$ 趋近于 0, 则求解结果略有不同。

3.5 由四元数求欧拉角

如依次绕 $z,y,x$ 分别旋转一个固定角度, 使用 yaw, pitch, roll 分别表示物体绕, $x,y,z$ 的旋转角度, 记为 $\psi ,\theta ,{\varphi }_{\text{。 }}$
$$\left[\begin{array}{l}\varphi \\ \theta \\ \psi \end{array}\right]=\left[\begin{array}{c}\arctan \frac{2\left(q_0{q}_1+q_2{q}_3\right)}{1-2\left(q_1^2+q_2^2\right)}\\ \arcsin \left(2\left(q_0{q}_2-q_3{q}_1\right)\right)\\ \arctan \frac{2\left(q_0{q}_3+q_1{q}_2\right)}{1-2\left(q_2^2+q_3^2\right)}\end{array}\right]$$
注意, $\arctan$ 和 $\arcsin$ 的结果 $\left[-\frac{\Pi }{2},\frac{\Pi }{2}\right]$, 这并不能覆盖所有朝向(对 于 $\theta$ 角 $\left[-\frac{\Pi }{2},\frac{\Pi }{2}\right]$ 的取值范围已经满足), 因此需要用 atan2 来代替 ${\arctan }_{\circ }$
$$\left[\begin{array}{c}\varphi \\ \theta \\ \psi \end{array}\right]=\left[\begin{array}{c}\mathrm{atan}2\left(2\left(q_0{q}_1+q_2{q}_3\right),1-2\left(q_1^2+q_2^2\right)\right)\\ \mathrm{asin}\left(2\left(q_0{q}_2-q_3{q}_1\right)\right)\\ \mathrm{atan}2\left(2\left(q_0{q}_3+q_1{q}_2\right),1-2\left(q_2^2+q_3^2\right)\right)\end{array}\right]$$
其中, $\mathrm{atan}2(y,x)$ 为 $\mathrm{C}++$ 中的函数, 求的是 $\mathrm{y}/\mathrm{x}$ 的反正切, 其返 回值为 $[-\Pi ,+\Pi ]$ 之间的一个数。

3.6 由欧拉角求四元数

$$\begin{array}{c}{\mathbf{q}}_{\mathrm{IB}}=\left[\begin{array}{c}\cos (\psi /2)\\ 0\\ 0\\ \sin (\psi /2)\end{array}\right]\left[\begin{array}{c}\cos (\theta /2)\\ 0\\ \sin (\theta /2)\\ 0\end{array}\right]\left[\begin{array}{c}\cos (\varphi /2)\\ \sin (\varphi /2)\\ 0\\ 0\end{array}\right]\\ =\left[\begin{array}{l}\cos (\varphi /2)\cos (\theta /2)\cos (\psi /2)+\sin (\varphi /2)\sin (\theta /2)\sin (\psi /2)\\ \sin (\varphi /2)\cos (\theta /2)\cos (\psi /2)-\cos (\varphi /2)\sin (\theta /2)\sin (\psi /2)\\ \cos (\varphi /2)\sin (\theta /2)\cos (\psi /2)+\sin (\varphi /2)\cos (\theta /2)\sin (\psi /2)\\ \cos (\varphi /2)\cos (\theta /2)\sin (\psi /2)-\sin (\varphi /2)\sin (\theta /2)\cos (\psi /2)\end{array}\right]\end{array}$$

4 三者转换Eigen实现

Eigen库中三种形式可表示为：

• 旋转矩阵（3X3）：Eigen::Matrix3d
• 欧拉角（3X1）：Eigen::Vector3d
• 四元数（4X1）：Eigen::Quaterniond

// 初始化旋转矩阵
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::Vector3d eulerAngle(yaw,pitch,roll);
// 初始化四元数
Eigen::Quaterniond quaternion(w,x,y,z);

4.1 由欧拉角求旋转矩阵

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;

4.2 由旋转矩阵求欧拉角

// Z-Y-X，即RPY
Eigen::Vector3d eulerAngle=rotation_matrix.eulerAngles(2,1,0);

4.3 由四元数求旋转矩阵

Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.matrix();
// 两种方法均可
Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.toRotationMatrix(); 

4.4 由旋转矩阵求四元数

Eigen::Quaterniond quaternion(rotation_matrix);
// 两种方法均可
Eigen::Quaterniond quaternion;quaternion=rotation_matrix;

4.5 由四元数求欧拉角

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

4.6 由欧拉角求四元数

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;

5 三者转换matlab实现

5.1 由欧拉角求旋转矩阵

eul2rotm()

5.2 由旋转矩阵求欧拉角

rotm2eul()

5.3 由四元数求旋转矩阵

quat2rotm()

5.4 由旋转矩阵求四元数

rotm2quat()

5.5 由四元数求欧拉角

eul2quat()

5.6 由欧拉角求四元数

eul2quat()

6 三者转换python实现

可使用scipy包中的scipy.spatial.transform.Rotation实现。

6.1 由欧拉角求旋转矩阵

from_euler()
as_matrix()

6.2 由旋转矩阵求欧拉角

from_matrix()
as_euler()

6.3 由四元数求旋转矩阵

from_quat()
as_matrix()

6.4 由旋转矩阵求四元数

from_matrix()
as_quat()

6.5 由四元数求欧拉角

from_quat()
as_euler()

6.6 由欧拉角求四元数

from_euler()
as_quat()

如果觉得我的文章对你起到了些许的帮助，做下手指操点击下面的大拇指感谢~
我是专注自动驾驶、LiDAR感知、三维点云处理、激光SLAM领域的阿川，欢迎各位志同道合的朋友在下面积极留言。欲将心事付瑶琴，希望能在交流中收获知识、经验和挚友