【机器人-基础知识】欧拉角、旋转矩阵和四元数

1. 欧拉角

1.1. 欧拉角的定义

欧拉角是一组三个角度，用于描述一个刚体在三维空间中的定向关系。具体来说，它们表示从一个固定参考坐标系到刚体坐标系的一系列旋转。常见的定义方式是将总体旋转分解为三个连续的简单旋转，每次旋转都绕着当前坐标系的某一固定轴进行。

例如，一种常用的欧拉角序列是 Z–Y–X（或称为航向、俯仰、滚转顺序），其含义如下：

• Yaw 第一个旋转（航向/偏航）：绕固定坐标系的 Z 轴旋转角度 $\psi$；
• Pitch 第二个旋转（俯仰）：绕新坐标系的 Y 轴旋转角度 $\theta$；
• Roll 第三个旋转（滚转）：绕再次更新后的 X 轴旋转角度 $\varphi$。

这种分解使得三维旋转问题转化为三个独立旋转角度的叠加。

1.2. 欧拉角的公式

以 Z–Y–X 顺序为例，刚体总旋转矩阵 $R$ 可以写成三个旋转矩阵的乘积：

$$R=R_x(\varphi )R_y(\theta )R_z(\psi )$$

其中：

• 绕 X 轴旋转的矩阵：

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

• 绕 Y 轴旋转的矩阵：

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

• 绕 Z 轴旋转的矩阵：

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

将上述三个矩阵依次相乘，就可以得到描述刚体最终定向的旋转矩阵 $R$

1.3. 欧拉角的常见问题

在使用欧拉角描述旋转时，需要注意以下问题：

1. 万向锁（Gimbal Lock）
   以常见的ZYX欧拉角为例，即首先绕全局z轴旋转，然后绕新的y轴旋转，最后绕新的x轴旋转。当第二次旋转（即绕y轴的旋转）旋转角（例如上例中的 $\theta$）达到 $\pm 90^{\circ }$ 时。
   第一次和第三次旋转将会围绕同一个轴。这意味着，在这些特殊的角度下，系统失去了一个自由度，因为两次旋转现在作用于相同的轴，不能独立地控制物体的方向。

2. 多值性与奇异性
   欧拉角存在多值性，即不同的角度组合可能表示相同的旋转。此外，某些角度组合可能使反求公式退化，导致奇异性问题，增加了数值计算的复杂性。

2. 旋转矩阵

2.1. 旋转矩阵的定义

旋转矩阵是一种用于描述刚体或坐标系在欧几里得空间中旋转变换的矩阵。其主要特点包括：

• 正交性：旋转矩阵 $R$ 满足 $R^{T}R=I$，其中 $R^T$ 为 $R$ 的转置，$I$ 为单位矩阵。这一性质保证了向量的长度和旋转前后向量间的夹角不变。
• 行列式为1：即 $\det (R)=1$，这一性质表明旋转矩阵不含有镜像或反射分量。
• 参数冗余性：在三维空间中，尽管旋转矩阵有 9 个元素，但由于正交性和行列式约束，其实际自由度仅为 3 个。

2.2. 常见旋转矩阵的公式

1. 二维旋转矩阵

在二维平面上，绕原点逆时针旋转角度 $\theta$ 的旋转矩阵为：
$$R(\theta )=\left[\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right]$$

2. 三维旋转矩阵

在三维空间中，常用的旋转矩阵包括绕各个坐标轴旋转的矩阵：

• 绕 X 轴旋转角度 $\varphi$ 的矩阵：

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

• 绕 Y 轴旋转角度 $\theta$ 的矩阵：

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

• 绕 Z 轴旋转角度 $\psi$ 的矩阵：

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

2.3. 旋转矩阵的示例

1. 二维示例

设有一个二维向量 $\mathbf{v}=\left[\begin{array}{c}1\\ 0\end{array}\right]$，我们希望将它逆时针旋转 $90^{\circ }$（即 $\theta =\frac{\pi }{2}$）。

对应的旋转矩阵为：

$$R\left(\frac{\pi }{2}\right)=\left[\begin{array}{cc}\cos \frac{\pi }{2}& -\sin \frac{\pi }{2}\\ \sin \frac{\pi }{2}& \cos \frac{\pi }{2}\end{array}\right]=\left[\begin{array}{cc}0& -1\\ 1& 0\end{array}\right]$$

计算变换后的向量：

$${\mathbf{v}}^{\prime }=R\left(\frac{\pi }{2}\right)\mathbf{v}=\left[\begin{array}{cc}0& -1\\ 1& 0\end{array}\right]\left[\begin{array}{c}1\\ 0\end{array}\right]=\left[\begin{array}{c}0\\ 1\end{array}\right]$$

这表明向量 $\mathbf{v}$ 被成功旋转到 $(0,1)$ 的位置。

2. 三维示例

考虑在三维空间中，绕 Z 轴旋转 $90^{\circ }$ 的情况。旋转矩阵为：

$$R_z\left(\frac{\pi }{2}\right)=\left[\begin{array}{ccc}\cos \frac{\pi }{2}& -\sin \frac{\pi }{2}& 0\\ \sin \frac{\pi }{2}& \cos \frac{\pi }{2}& 0\\ 0& 0& 1\end{array}\right]=\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right]$$

对向量 $\mathbf{v}=\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right]$ 进行变换，得到：

$${\mathbf{v}}^{\prime }=R_z\left(\frac{\pi }{2}\right)\mathbf{v}=\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right]=\left[\begin{array}{c}0\\ 1\\ 0\end{array}\right]$$

这样，原本沿 X 轴的向量被旋转到 Y 轴方向。

2.4. 旋转矩阵的问题

1. 数值稳定性
   在计算机中进行数值运算时，由于舍入误差，得到的旋转矩阵可能不再严格满足正交性（即 $R^{T}R$ 不完全等于 $I$），这会引起累积误差。为解决这一问题，通常需要使用正交化算法（如 Gram-Schmidt 或 SVD 方法）对矩阵进行修正。

2. 冗余参数
   在三维空间中，旋转矩阵有 9 个元素，但实际只有 3 个自由度。这种冗余性在一些优化问题和数据融合算法中可能增加额外的计算复杂度。

3. 直观性不足
   虽然旋转矩阵在数学上精确且易于组合（例如多个旋转矩阵相乘可以得到总体旋转），但其 9 个元素之间的关系较难直观理解和解释。在需要直观展示旋转状态（例如动画、用户界面设计）时，欧拉角或四元数可能更为直观。

4. 从旋转矩阵到其他表示的转换问题
   将旋转矩阵转换为欧拉角或轴—角表示时，可能会遇到奇异性问题（例如万向锁现象），导致数值计算不稳定或多值性问题。特别是在旋转角接近特定值（如 $\pm 90^{\circ }$）时，反求过程可能会出现退化情况。

3. 四元数

3.1. 四元数的定义

一个四元数通常表示为

$$q=w+xi+yj+zk$$

其中，$w,x,y,z$ 均为实数，而 $i,j,k$ 是满足以下乘法规则的虚数单位：

$$i^2=j^2=k^2=ijk=-1$$

并且具有以下非交换性（例如：$ij=k$，但 $ji=-k$）。四元数构成一个四维实向量空间，是一个非交换的除环，常用于描述三维空间的旋转。

3.2. 四元数的运算

1. 加法与标量乘法

• 加法：两个四元数按各个分量相加
  $$q_1+q_2=(w_1+w_2)+(x_1+x_2)i+(y_1+y_2)j+(z_1+z_2)k$$
• 标量乘法：对四元数的每个分量乘以一个实数 $\lambda$
  $$\lambda q=\lambda w+\lambda xi+\lambda yj+\lambda zk$$

2. 乘法

给定两个四元数

$$q_1=w_1+x_{1}i+y_{1}j+z_{1}k,q_2=w_2+x_{2}i+y_{2}j+z_{2}k,$$

它们的乘积 $q=q_1{q}_2$ 可以写成

$$\begin{array}{rl}q=& (w_1{w}_2-x_1{x}_2-y_1{y}_2-z_1{z}_2)\\ & +(w_1{x}_2+x_1{w}_2+y_1{z}_2-z_1{y}_2)i\\ & +(w_1{y}_2-x_1{z}_2+y_1{w}_2+z_1{x}_2)j\\ & +(w_1{z}_2+x_1{y}_2-y_1{x}_2+z_1{w}_2)k.\end{array}$$

注意，四元数乘法满足结合律和分配律，但不满足交换律。

3.3. 四元数在旋转变换中的应用

在三维旋转表示中避免欧拉角的万向锁问题，同时在旋转插值（如 Slerp）中表现良好。

1. 旋转表示

一个单位四元数可用于表示三维空间中的旋转。设旋转轴为单位向量 $\mathbf{n}=(n_x,n_y,n_z)$，旋转角度为 $\theta$，对应的单位四元数为

$$q=\cos \frac{\theta }{2}+\sin \frac{\theta }{2}(n_{x}i+n_{y}j+n_{z}k).$$

2. 旋转操作

给定一个需要旋转的三维向量 $\mathbf{v}$（将其表示为纯四元数，即实部为 0：$v=0+v_{x}i+v_{y}j+v_{z}k$），通过四元数进行旋转的公式为

$$v^{\prime }=qv{q}^{-1},$$

其中 $v^{\prime }$ 为旋转后的四元数，对应的向量部分即为旋转后的三维向量。通过这种方式，可以将旋转变换高效地应用到向量上。

3. 旋转的组合与插值

• 组合旋转：多个旋转可以通过四元数乘法进行组合。若有两个旋转分别对应的单位四元数 $q_1$ 和 $q_2$，则先后执行这两个旋转的总体旋转由 $q=q_2{q}_1$ 表示（注意乘法的顺序）。
• 旋转插值（Slerp）：四元数还可以用于球面线性插值（Spherical Linear Interpolation），这在计算机图形学和动画中用于平滑过渡旋转。

4. 相比旋转矩阵的计算效率高

当使用旋转矩阵进行连续旋转时，通常需要进行多次矩阵乘法。每次矩阵乘法涉及大量的浮点运算，对于3x3矩阵而言，这通常意味着27次乘法和18次加法。相比之下，四元数的乘法仅需要16次乘法和12次加法，减少了计算量。

5. 单位四元数的意义

• 单位四元数用于表示纯旋转，但非单位四元数可以用来表示带有缩放因子的旋转。
• 在实际应用中，如果需要将非单位四元数转换为单位四元数（比如用于表示旋转），通常需要对其进行归一化处理，即将其除以自身的模长，使其成为一个单位四元数。

4. 四元数、旋转矩阵和变换矩阵

1. 四元数 -> 旋转矩阵

给定一个单位四元数
$$q=w+xi+yj+zk,$$
其对应的旋转矩阵 $R$（通常用于右手坐标系）可写为

$$R=\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].$$

2. 旋转矩阵 -> 四元数

设旋转矩阵为

$$R=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right].$$

已知旋转矩阵 $R$ 后，可以通过矩阵的迹（trace）[矩阵的对角线元素之和] 来恢复对应的四元数。一种常用方法如下：

1. 计算迹：$$\mathrm{tr}(R)=r_{11}+r_{22}+r_{33}.$$
2. 若 $$\mathrm{tr}(R)>0,$$ 则令
   $$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. 否则，当旋转矩阵的迹 $\mathrm{tr}(R)\le 0$ 时，根据矩阵对角线元素的大小，选择最大的那个来计算四元数的分量。

   • 如果 $r_{11}$ 是最大的对角元素：
     $$x=\frac{1}{2}\sqrt{1+r_{11}-r_{22}-r_{33}},$$
     然后计算：
     $$w=\frac{r_{32}-r_{23}}{4x},y=\frac{r_{12}+r_{21}}{4x},z=\frac{r_{13}+r_{31}}{4x}.$$

   • 如果 $r_{22}$ 是最大的对角元素：
     $$y=\frac{1}{2}\sqrt{1+r_{22}-r_{11}-r_{33}},$$
     然后计算：
     $$w=\frac{r_{13}-r_{31}}{4y},x=\frac{r_{12}+r_{21}}{4y},z=\frac{r_{23}+r_{32}}{4y}.$$

   • 如果 $r_{33}$ 是最大的对角元素：
     $$z=\frac{1}{2}\sqrt{1+r_{33}-r_{11}-r_{22}},$$
     然后计算：
     $$w=\frac{r_{21}-r_{12}}{4z},x=\frac{r_{13}+r_{31}}{4z},y=\frac{r_{23}+r_{32}}{4z}.$$

这些分支确保了当迹小于等于零时，能够避免数值不稳定问题，并正确提取出最大的旋转分量，从而稳定地计算出四元数。

3. 旋转矩阵 -> 变换矩阵

在齐次坐标下，三维变换矩阵通常是 $4\times 4$ 的，其形式为

$$T=\left[\begin{array}{cc}R& \mathbf{t}\\ 0^T& 1\end{array}\right],$$

其中：

• $R$ 为 $3\times 3$ 的旋转矩阵；
• $\mathbf{t}=\left[\begin{array}{c}{t}_x\\ t_y\\ t_z\end{array}\right]$ 为平移向量；
• 下排 $0^T=[000]$。

4. 变换矩阵 -> 旋转矩阵

若给定变换矩阵 $T$，只需取出左上角 $3\times 3$ 部分，即为旋转矩阵 $R$：

$$R=T(1:3,1:3).$$

5. 变换矩阵的解析

5.1. 变换矩阵的示例

当坐标系A沿 x轴正方向平移1、y轴正方向平移2、z轴正方向平移3 时，B到A的变换矩阵为：
$$T_{B\to A}=\left[\begin{array}{cccc}1& 0& 0& 1\\ 0& 1& 0& 2\\ 0& 0& 1& 3\\ 0& 0& 0& 1\end{array}\right]$$
B坐标系的点可以通过 B到A的变换矩阵 $T_{B\to A}$得到在A坐标系的表达
$$P_A=T_{B\to A}{P}_B$$
$T_{B\to A}$求逆得到A到B的变换矩阵为：
$$T_{A\to B}=\left[\begin{array}{cccc}1& 0& 0& -1\\ 0& 1& 0& -2\\ 0& 0& 1& -3\\ 0& 0& 0& 1\end{array}\right]$$
$$P_B=T_{A\to B}{P}_A$$
例如，将A的原点(0,0,0)带入得到 其在B坐标系的坐标是(-1,-2,-3)

5.2. 变换矩阵的意义

以相机坐标系到世界坐标系的变换矩阵 $T_{c2w}$ 为例，其核心作用是描述相机坐标系相对于世界坐标系的位姿（位置和姿态）。

---

一、几何描述
变换矩阵 $T_{c2w}$ 是一个 4×4 齐次矩阵，由 旋转矩阵 $R$ 和 平移向量 $t$ 组成：
$$T_{c2w}=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]$$

• 几何意义：

  1. 旋转矩阵 $R$：描述相机坐标系的坐标轴在世界坐标系中的方向。例如，若相机绕 Z 轴旋转 θ 角，则 $R$ 为绕 Z 轴的旋转矩阵。
  2. 平移向量 $t$：表示相机坐标系原点在世界坐标系中的位置。

• 符号惯例： $T_{c2w}$ 表示从相机坐标系到世界坐标系的变换，与$T_{wc}$（世界到相机的变换）互为逆矩阵。

二、坐标转换
变换矩阵 $T_{c2w}$ 可将相机坐标系中的点 $P_c$ 转换为世界坐标系中的点 $P_w$： $$P_w=T_{c2w}\cdot P_c$$
公式推导：

1. 齐次坐标表示：

   • 相机坐标系中的点 $P_c=[X_c,Y_c,Z_c,1{]}^T$。
   • 世界坐标系中的点 $P_w=[X_w,Y_w,Z_w,1{]}^T$。

2. 转换过程：
   $$\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]\cdot \left[\begin{array}{c}{X}_c\\ Y_c\\ Z_c\\ 1\end{array}\right]=\left[\begin{array}{c}R\cdot \left[\begin{array}{c}{X}_c\\ Y_c\\ Z_c\end{array}\right]+t\\ 1\end{array}\right]$$

• 示例：
  若相机坐标系中点 $P_c=[1,0,0{]}^T$，则世界坐标系中的坐标为：
  $$P_w=R\cdot [1,0,0{]}^T+t=\left[\begin{array}{c}\cos 45°\cdot 1+\sin 45°\cdot 0\\ 0\cdot 1+1\cdot 0\\ -\sin 45°\cdot 1+\cos 45°\cdot 0\end{array}\right]+\left[\begin{array}{c}2\\ 0\\ 0\end{array}\right]=\left[\begin{array}{c}\cos 45°+2\\ 0\\ -\sin 45°\end{array}\right]$$

三、运动表示
变换矩阵可描述相机的运动特性，其核心性质包括：

1. 组合运动：多个变换可通过矩阵乘法组合。
   $$T_{c2w}^{(总)}=T_{c2w}^{(1)}\cdot T_{c2w}^{(2)}$$

2. 逆运动：从世界坐标系到相机坐标系的变换为 $T_{w2c}=T_{c2w}^{-1}$，满足：
   $$T_{w2c}=\left[\begin{array}{cc}{R}^T& -R^{T}t\\ 0^T& 1\end{array}\right]$$

3. 动态解释：
   • 旋转：相机坐标系的姿态变化（如旋转角度）。

   • 平移：相机坐标系原点的位置偏移。