从传感器到姿态：旋转矩阵、四元数与欧拉角的定义、转换及坐标系应用全解

---

摘要

本文系统讲解了三维空间中最常用的旋转表示形式——旋转矩阵、四元数 与 欧拉角。
从数学定义出发，逐步推导它们之间的转换关系，
再结合实际传感器采集过程（IMU、VIO、LiDAR等）说明如何获得旋转信息，
最后给出坐标系应用与工程中最常用的转换流程，
从而真正理解“旋转”从几何到工程实现的完整闭环。

---

一、三种旋转表示的基本定义

在三维空间中，旋转可以有多种数学表示方式。
最常用的三种是 旋转矩阵 (Rotation Matrix)、四元数 (Quaternion) 和 欧拉角 (Euler Angles)。
它们都描述同一个物理过程——绕某个轴旋转一定角度，但适用场景不同。

表示方式	定义	特点	是否有奇异点
旋转矩阵 $R$	3×3 正交矩阵，满足 $R^{\top }R=I,\det R=1$	直观、线性代数友好	无奇异，但参数冗余
四元数 $q=(w,x,y,z)$	单位四元数 $q=\cos (\frac{\theta }{2})+\mathbf{n}\sin (\frac{\theta }{2})$	紧凑、数值稳定	无奇异、±q 等价
欧拉角 (roll, pitch, yaw)	按固定顺序依次旋转（如 Z–Y–X）	可读性强	有奇异点（万向节锁）

---

二、三者之间的相互转换公式

1️⃣ 四元数 → 旋转矩阵

$$R(q)=\left[\begin{array}{ccc}1-2(y^2+z^2)& 2(xy-wz)& 2(xz+wy)\\ 2(xy+wz)& 1-2(x^2+z^2)& 2(yz-wx)\\ 2(xz-wy)& 2(yz+wx)& 1-2(x^2+y^2)\end{array}\right]$$

这来自四元数夹乘 $q\mathbf{v}{q}^{-1}$，
表示用四元数旋转向量的矩阵化形式。

---

2️⃣ 旋转矩阵 → 四元数

$$w=\frac{1}{2}\sqrt{1+\mathrm{tr}R},x=\frac{r_{32}-r_{23}}{4w},y=\frac{r_{13}-r_{31}}{4w},z=\frac{r_{21}-r_{12}}{4w}.$$

---

3️⃣ 欧拉角 → 四元数（Z–Y–X 顺序）

$$\begin{array}{rl}w& =c_{\varphi }{c}_{\theta }{c}_{\psi }+s_{\varphi }{s}_{\theta }{s}_{\psi },\\ x& =s_{\varphi }{c}_{\theta }{c}_{\psi }-c_{\varphi }{s}_{\theta }{s}_{\psi },\\ y& =c_{\varphi }{s}_{\theta }{c}_{\psi }+s_{\varphi }{c}_{\theta }{s}_{\psi },\\ z& =c_{\varphi }{c}_{\theta }{s}_{\psi }-s_{\varphi }{s}_{\theta }{c}_{\psi },\end{array}$$

其中 $c_{\varphi }=\cos (\varphi /2),s_{\varphi }=\sin (\varphi /2)$。

---

4️⃣ 四元数 → 欧拉角

$$\begin{array}{rl}\varphi & =\arctan 2(2(wx+yz),1-2(x^2+y^2)),\\ \theta & =\arcsin (2(wy-zx)),\\ \psi & =\arctan 2(2(wz+xy),1-2(y^2+z^2)).\end{array}$$

当 pitch=±90° 时会出现奇异点（万向节锁）。

---

5️⃣ 欧拉角 ↔ 旋转矩阵

$$R=\left[\begin{array}{ccc}{c}_{\psi }{c}_{\theta }& c_{\psi }{s}_{\theta }{s}_{\varphi }-s_{\psi }{c}_{\varphi }& c_{\psi }{s}_{\theta }{c}_{\varphi }+s_{\psi }{s}_{\varphi }\\ s_{\psi }{c}_{\theta }& s_{\psi }{s}_{\theta }{s}_{\varphi }+c_{\psi }{c}_{\varphi }& s_{\psi }{s}_{\theta }{c}_{\varphi }-c_{\psi }{s}_{\varphi }\\ -s_{\theta }& c_{\theta }{s}_{\varphi }& c_{\theta }{c}_{\varphi }\end{array}\right]$$

---

三、在坐标系中的实际作用

设有两个坐标系：

• 世界系 { W } \{W\} {W}
• 机体系 { B } \{B\} {B}

若同一点在两坐标系下的坐标为：
$${\mathbf{p}}_W=R_{WB}{\mathbf{p}}_B+{\mathbf{t}}_{WB},$$
其中：

• $R_{WB}$：从机体系到世界系的旋转；
• ${\mathbf{t}}_{WB}$：机体系原点在世界系中的位置。

使用四元数时：
$${\mathbf{p}}_W=q_{WB}{\mathbf{p}}_B{q}_{WB}^{-1}+{\mathbf{t}}_{WB}.$$
逆变换为：
$${\mathbf{p}}_B=R_{WB}^{\top }({\mathbf{p}}_W-{\mathbf{t}}_{WB}).$$

---

四、从传感器到姿态矩阵的形成

在实际采集过程中，不同类型传感器输出的姿态信息形式不同：

传感器类型	输出形式	旋转表示	常见来源
IMU / 飞控	四元数 + 欧拉角	✅ 四元数	Madgwick / EKF 姿态融合
视觉 / SLAM	位姿矩阵 (T=[R, t])	✅ 旋转矩阵	PnP、SVD、Bundle Adjustment
LiDAR / 点云	位姿矩阵 $T$	✅ 旋转矩阵	ICP / NDT 配准
混合导航系统 (VIO / INS)	同时输出四元数与矩阵	两者兼具	内部自动转换

实际例子

1️⃣ IMU 驱动（ROS sensor_msgs/Imu）

orientation:
  x: 0.12
  y: -0.01
  z: 0.36
  w: 0.92

→ 可直接转为旋转矩阵 $R(q)$。

2️⃣ VIO 或 SLAM 系统

T = [ R | t ]

→ 从矩阵中提取 R，即为姿态旋转矩阵。

---

五、常用旋转表示的工程应用

表示方式	常用场景	优点	缺点
旋转矩阵	点云配准、坐标变换链	无奇异、线性运算方便	存储冗余
四元数	姿态融合、VIO / SLAM	紧凑稳定、可插值	抽象、不直观
欧拉角	调试与人机界面	易读	奇异性强、顺序依赖

✅ 在大多数工程系统（ROS、SLAM、IMU融合）中：
姿态存储用四元数，计算与坐标变换用旋转矩阵。

---

六、总结：从传感器到坐标变换的闭环

层次	内容	说明
定义层	三种旋转表示（R, q, 欧拉角）	数学表达方式
转换层	它们的相互转换公式	理论统一
传感器层	IMU→q, SLAM→R	实际采集形式
坐标层	${\mathbf{p}}_W=R_{WB}{\mathbf{p}}_B+t_{WB}$	坐标变换本质
工程层	四元数 ↔ 矩阵最常用	稳定高效的标准方案

🧭 一句话总结：
在现实中，我们从传感器获得的姿态往往以四元数或旋转矩阵的形式存在；
它们通过互相转换参与坐标系变换与数据融合；
四元数 ↔ 旋转矩阵 是工程实现中最稳定、最通用的姿态转换形式。

最后，有兴趣的话可以看看我前面的这篇关于四元数的推导文章加深印象：
从几何到四元数：三维旋转的完整推导与工程意义