描述三维刚体旋转

摘要
本文介绍了机器人运动学中的关键概念，包括点与向量的基本定义及其运算特性。重点阐述了坐标系的刚体变换——通过旋转和平移实现物体的姿态变化，并引入齐次坐标简化描述方式。此外，文章探讨了旋转向量（轴角）作为紧凑表示工具的优势，并通过罗德里格斯公式展示了其与旋转矩阵之间的转换关系。最后讨论了欧拉角的应用及其局限性——万向锁问题，在姿态估计中常用于人机交互但受限于奇异性问题而未被广泛应用于滤波或优化过程。

文章目录

第一部分：点与向量

• 第二章 欧氏空间中的坐标变换

  • • 1. 简述欧氏变换的核心内容
    • （二）欧氏变换中的旋转变换
      • 2.1 引出旋转变换对应的矩阵表示形式
      • 2.2 探讨旋转变换矩阵的主要特性

• （三）我们称欧氏空间中的一种刚体运动为平移运动（Translation），其数学表达式如下：

T = \begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix}

其中R表示旋转矩阵t表示平移向量。

• 在本节中我们将全面阐述欧氏变换的概念及其数学表示方法，并探讨其实现过程中的关键要素。

• 在实际应用中，我们经常需要建立多个相互关联的空间坐标系，并研究它们之间的转换关系。

  • 三、变换矩阵与齐次坐标
  • • （一）变换矩阵与齐次坐标
    • （二）特殊欧氏群

• 四、旋转向量

  • （一）阐述旋转变换在空间中的表示方法

    • （二）探讨旋转变换对应的矩阵表示方式及其转换规则
    • （三）分析基于矩阵表示的旋转变换如何还原为向量形式

  • 五、欧拉角

  • • （一）RPY欧拉角
    • （二）万向锁问题

一、点与向量

• 日常生活的空间是三维的，因此我们生来就习惯于三维空间的运动。三维空间由3个轴组成，所以一个空间点的位置可以由3个坐标指定。不过，我们现在要考虑的刚体，它不光有位置，还有自身的姿态。相机也可以看成三维空间的刚体，于是位置是指相机在空间中的哪个地方，而姿态则是指相机的朝向。结合起来可以说，“相机正处于空间(0, 0, 0)点处，朝向正前方”，但这种自然语言很烦琐，我们喜欢用数学语言来简洁地描述。

（一）点

• 点就是空间中的基本元素，没有长度，没有体积。

（二）向量

连接两个指定的点所形成的线段即为向量，在几何学中我们通常将其表示为从一点指向另一点的有向线段；它既具有大小又带有明确的方向；请注意：不要将此与该矢性（coordinate）的概念相混淆；每个特定的几何体在空间中独立存在；例如符号\bold a代表一个特定的矢性；值得注意的是，在这里符号\bold a并不直接关联任何一组实数；为了确定该矢性在某一标架（coordinate system）中的表示方法（即其对应的数值坐标），我们需要先选定一个三维空间中的特定标架；这样做的结果就是能够用一组实数精确描述该矢性的位置与方向信息。

（三）基与坐标

在三维空间中任一点的位置可通过\bold R^3进行描述。考虑在一个线性空间内选取一组基(e_1, e_2, e_3)的情况下，则任意向量\bold a在这一组基下都会对应一个唯一坐标：\bold a = [e_1, e_2, e_3]\left[ \begin{matrix} a_1 \\ a_2 \\ a_3 \end{matrix} \right ] = a_1e_1 + a_2e_2 + a_3e_3。其中，

\left[ \begin{matrix} a_1 \\ a_2 \\ a_3 \end{matrix} \right ]

被称为向量\bold a在该基下的坐标。其具体数值则与向量本身及所选坐标系（即基的选择）有关。\n通常由三个相互正交的轴构成的空间直角坐标系是最常用的类型

（四）向量运算

• \bold a, \bold b \in \mathrm R^3

1、数乘运算

标量k与向量\mathbf{a}相乘等于k倍的各分量分别乘以k

2、加法运算

\boldsymbol{a} + \boldsymbol{b} = \left[\begin{matrix}a_{1}\\a_{2}\\a_{3}\end{matrix}\right] + \left[\begin{matrix}b_{1}\\b_{2}\\b_{3}\end{matrix}\right] = \left[\begin{matrix}a_{1}+b_{1}\\a_{2}+b_{2}\\a_{3}+b_{3}\end{matrix}\right]

3、减法运算

该向量差的结果等于对应分量相减后的向量。

4、内积运算

向量点积的结果由两部分组成：首先是两个列向量之间的乘法运算；其次通过矩阵乘法的形式展开计算过程；最终得到各分量相乘之和的结果；这也可以用求和符号表示为…此外，在三维空间中该运算还可以表示为…其中夹角θ即为这两个向量之间的角度；而点积实际上反映了这两个向量在方向上的投影关系。

5、外积运算

向量a与向量b的叉乘运算结果等于由三个标准基矢量构成的三阶行列式展开后的值。具体而言，该运算等同于计算各子行列式的代数余子式所形成的向量。进一步展开后可得其具体分量数值分别为a_{y}b_{z}-a_{z}b_{y}、- (a_{x}b_{z}-a_{z}b_{x})以及a_{x}b_{y}-a_{y}b_{x}。将这些分量按照对应的顺序排列后所得的结果与右侧矩阵所表示的数量完全一致，并定义该结果为向量a与向量b的叉乘积

外积运算的结果是一个向量，
其方向垂直于原始两个向量，
其大小等于|\bold a||\bold b|\mathrm{sin}\theta，
等于由这两个向量张成的平行四边形的有向面积。
在处理外积运算时，
我们引入了一种特殊的记号系统，
将\bold a表示为一个矩阵。
实际上属于一种特殊的矩阵形式——反称矩阵（Skew-symmetric Matrix）。
我们可以将其简记为一种反称符号。
从而实现了外积运算与线性代数运算之间的转换。
记住这一表示方法，
它是一种精确的一一对应关系：
任意向量都唯一地对应着一个反称矩阵，
反之亦然：
\bold a^{\bigwedge} = \left [ \begin{matrix} 0 & -a_3 & a_2\\ a_3 & 0 & -a_1\\ -a_2 & a_1 & 0\\ \end{matrix} \right ]

6、注意事项

向量及其加减法运算、点积（内积）等操作，在无需讨论其坐标的前提下仍可进行计算。例如，在存在坐标系的情况下，则点积可表示为两向量对应分量乘积之和。然而即便未明确给出它们的具体坐标信息，则可通过两向量长度及其夹角余弦来计算点积。由此可见，两向量之间的点积结果与其所处的空间中所选取的具体坐标系无关。

二、坐标系的欧氏变换

（一）简述欧氏变换

• 通常的做法是为运动的机器人设定一个惯性坐标系（或称为世界坐标系），这种情况下该系统可以被视为静止的（例如下图所示的x_w, y_w, z_w定义的惯性坐标系）。同时，在机器人或相机所在的移动坐标系中（如x_c, y_c, z_c定义的相机或机器人自身坐标系），同一个空间中的点在不同参考框架中的位置关系会有所差异。
  读者可能会好奇：在一个相机视野中的向量p在相机坐标系中的位置为p_c而在惯性坐标系中则为p_w之间如何相互转换？解决这一问题的方法通常是先确定该点相对于机器人自身的空间位置即机器人坐标系下的位置值随后将其通过刚性变换转换至惯性坐标系统中。
• 在刚体运动过程中两个参考框架之间的位姿变化由旋转矩阵和平移向量共同决定这种变换过程被称为欧氏变换（Euclidean Transform）。具体而言相机运动属于一种典型的rigid motion即物体在空间中仅发生平移和旋转而不会发生变形或缩放等形变现象（就像你将手机抛入空中仅会发生位置和姿态的变化而不影响其长度形状等内在特性）。
  这种情况下我们可以说相机所处的局部坐标系统与全局惯性系统之间存在一个欧氏变换关系即从局部到全局只需进行一次旋转变换和平移操作即可实现点坐标的相互转化。

坐姿变化：同一个向量在不同参考框架下具有不同的表达方式。具体而言，在世界参考框架中该向量的表示为p_w，而在相机参考框架中的表示为p_c。这种转换关系可由变换矩阵\mathrm T来进行描述。

（二）欧氏变换的旋转（Rotation）

1、引出旋转矩阵

• 设某个单位正交基(e_1,e_2,e_3)经过一次旋转变成了(e_1',e_2',e_3')，那么对于同一个向量\bold a，它在两个坐标系下的坐标为[a_1,a_2, a_3]^{\mathrm T}和[a_1',a_2', a_3']^{\mathrm T}。因为向量本身没有变化，所以根据坐标的定义得到\left[ \begin{matrix}\\ e_1,e_2,e_3 \end{matrix}\right ]\left[ \begin{matrix}\\ a_1\\ a_2\\ a_3 \end{matrix}\right ]= \left[ \begin{matrix}\\ e_1',e_2',e_3' \end{matrix}\right ]\left[ \begin{matrix}\\ a_1'\\ a_2'\\ a_3' \end{matrix}\right ]
• 为了描述两个坐标之间的关系，我们对上述等式的左右两边同时左乘\left[ \begin{matrix}\\ e_1^\mathrm T\\ e_2^\mathrm T\\ e_3^\mathrm T \end{matrix}\right ]，那么左边的系数就变成了单位矩阵，因此，我们得到\left[ \begin{matrix} a_1\\ a_2\\ a_3 \end{matrix}\right ]= \left [ \begin{matrix} e_1^\mathrm Te_1' & e_1^\mathrm Te_2' & e_1^\mathrm Te_3'\\ e_2^\mathrm Te_1' & e_2^\mathrm Te_2' & e_2^\mathrm Te_3'\\ e_3^\mathrm Te_1' & e_3^\mathrm Te_2' & e_3^\mathrm Te_3'\\ \end{matrix} \right ] \left [ \begin{matrix} a_1'\\ a_2'\\ a_3'\\ \end{matrix} \right ]\xlongequal{\mathrm{def}}\bold Ra'
• 这样我们得到一个矩阵\bold R，由两组基之间的内积组成，刻画了旋转前后同一个向量的坐标变换关系。只要旋转是一样的，这个矩阵就是一样的。可以说，矩阵\bold R描述了旋转本身。因此，称为旋转矩阵 （Rotation Matrix）。同时，该矩阵各分量是两个坐标系基的内积，由于基向量的长度为1，所以实际上是各基向量夹角的余弦值，所以这个矩阵也叫方向余弦矩阵 （Direction Cosine Matrix）。

2、旋转矩阵的性质

• 旋转矩阵是一个行列式为1的正交矩阵（转置矩阵与逆矩阵相等的矩阵）。反之，行列式为1的正交矩阵也是一个旋转矩阵。定义n维旋转矩阵的集合：SO(n)=\{\bold R \in \Bbb R^{n\times n}| \bold R \bold R ^\mathrm T=\bold I, \mathrm {det}(\bold R) =1\}，SO(n)是特殊正交群（Special Orthogonal Group）的意思。这个集合由n维空间的旋转矩阵组成，特别地，SO(3)就是指三维空间的旋转。通过旋转矩阵，可以直接谈论两个坐标系之间的旋转变换，而不用再从基开始谈起。
• 由于旋转矩阵为正交矩阵，它的逆（即转置）描述了一个相反的旋转。按照上面的定义方式\bold a=\bold Ra'，有\bold a'=\bold R^{-1}a=\bold R^\mathrm T\bold a，显然，\bold R^\mathrm T刻画了一个相反的旋转。

（三）欧氏变换的平移（Translation）

在欧氏变换中存在两种基本的操作：一个是旋转（Rotation），另一个是位移（Translation）。在世界坐标系中取一个位置向量a（即位置点），通过执行一次位移操作t后得到的结果是新的位置点a'（即新的位置点）。根据定义可知：**

\mathbf{a'} = \mathbf{a} + \mathbf{t}

其中称该位移操作对应的向量为\textit{位移（Translation）}或\textit{移动（Movement）}。与旋转不同的是，在进行一次位移操作时，则只需要将该位移操作对应的\textit{位移（Translation）}施加于原始位置即可完成转换过程。

（四）欧氏变换的完整描述

在世界坐标系中取定任意向量\bold a进行分析研究，在通过一次旋转变换矩阵\bold R和一次平移操作后得到了新的位置向量\bold a'。根据刚体变换原理可知：

\begin{cases} \hat{\omega} = \omega \\ \hat{v} = v - \omega \times r_{g} \end{cases}

其中称为平移向量的是 \{\bf t\} 。需要注意的是，在刚体运动学中，

\hat{\omega} = \omega

这一特性表明刚体运动不会引起各点速度方向的变化。
因此，在仿射变换模型中我们通常采用一个包含旋转变换矩阵和位姿参数的整体变换表达方式。

（五）两个坐标系的变换

在实际应用中，在空间中我们通常会设置两个不同的坐标系来描述物体的位置和运动情况。为了方便计算和转换信息，在这种情况下我们需要定义两个独立的笛卡尔坐标系：第一坐标系（Coordinate System 1）和第二坐标系（Coordinate System 2）。在这种设定下，同一个几何实体在两个不同参考框架下的位置参数会发生变化。具体来说，在第一种情况下该实体的位置参数可以用\bold a_1来表示，在第二种情况下则用\bold a_2来表示；这两者之间存在一种确定的关系式：\bold a_1= \bold R_{12}\cdot \bold a_2+\bold t_{12}

在这里面，
\bold R_{12}被定义为"从一个坐标系转换到另一个坐标的旋转矩阵"。
需要注意的是，
由于矩阵乘法的方向性，
这个旋转矩阵是在右乘的情况下起作用，
即所谓的"右乘法则"。
类似地，
如果我们想要表达"从第一个到第二个坐标的旋转关系",
那么相应的旋转矩阵就是\bold R_{21}。
这个细节对于后续坐标的变换具有非常重要的意义

关于平移项\bold t_{12}，
它实际上代表的是第一个参考框架相对于第二个参考框架的位置向量，
并且这个向量是在第一个参考框架下进行测量得到的结果。
因此，
为了明确表达这种关系，
建议将其称为"从第一个框架指向第二个框架的位移矢量"。
然而，
当反过来考虑从第二个框架指向第一个框架时，
对应的位移矢量记作\bold t_{21}，
它并不等于-\bold t_{12}；
而是还受到两个参考框架之间相对旋转的影响。
这一特性表明两者之间并不是简单的符号相反关系

三、变换矩阵与齐次坐标

（一）变换矩阵与齐次坐标

• \bold a'= \bold R \bold a + \bold t完整地表达了欧氏空间的旋转与平移，不过还存在一个小问题：这里的变换关系不是一个线性关系。
• 假设我们对向量\bold a进行了两次变换：\bold R_1, t_1和\bold R_2, t_2，即\bold b=\bold R_1\bold a+\bold t_1，\bold c=\bold R_2\bold b+\bold t_2，那么从\bold a到\bold c的变换为\bold c=\bold R_2(\bold R_1\bold a + \bold t_1) + \bold t_2
• 这样的形式在变换多次之后会显得很啰嗦。因此，我们引入齐次坐标和变换矩阵，重写为：\left [ \begin{matrix} \bold a'\\ 1\\ \end{matrix}\right ]= \left [ \begin{matrix} \bold R & \bold t \\ \bold 0^\mathrm T &1\\ \end{matrix}\right ] \left [ \begin{matrix} \bold a\\ 1\\ \end{matrix}\right ]\xlongequal{\mathrm{def}}\bold T \left [ \begin{matrix} \bold a\\ 1\\ \end{matrix}\right ]
• 说明：在一个三维向量的末尾添加1，将其变成了四维向量，称为齐次坐标 。对于这个四维向量，可以把旋转和平移写在一个矩阵里，使得整个关系变成线性关系。该式中，矩阵\bold T称为变换矩阵 （Transform Matrix）。
• 暂时用\tilde \bold a表示\bold a的齐次坐标。那么依靠齐次坐标和变换矩阵，两次变换的叠加就可以有很好的形式：\tilde \bold b=\bold T_1 \tilde \bold a,\tilde \bold c=\bold T_2 \tilde \bold b \Longrightarrow\tilde \bold c=\bold T_2\bold T_1 \tilde \bold a
• 但是区分齐次和非齐次坐标的符号令人厌烦，因为此处只需要在向量末尾添加1或者去掉1。所以，在不引起歧义的情况下，以后我们就直接把它写成\bold b =\bold T \bold a的样子，默认其中进行了齐次坐标的转换。

（二）特殊欧氏群

• 关于变换矩阵\bold T，它具有比较特别的结构：左上角为旋转矩阵，右侧为平移向量，左下角为0向量，右下角为1。这种矩阵又称为特殊欧氏群 （Special Euclidean Group）：\mathrm{SE(3)}=\{ \bold T=\left [ \begin{matrix} \bold R & \bold t \\ \bold 0^\mathrm T &1\\ \end{matrix}\right ] \in \Bbb R^{4\times 4}| \bold R \in \mathrm{SO(3)},\bold t \in \Bbb R^3 \}
• 与\mathrm{SO(3)}一样，求解该矩阵的逆表示一个反向的变换：\bold T^{-1}=\left [ \begin{matrix} \bold R^\bold T & -\bold R^\bold T\bold t \\ \bold 0^\mathrm T &1\\ \end{matrix}\right ]
• 同样，我们用\bold T_{12}这样的写法来表示从2到1的变换。并且，为了保持符号的简洁，在不引起歧义的情况下，以后不刻意区别齐次坐标与普通坐标的符号，默认使用的是符合运算法则的那一种。例如，当我们写\bold T \bold a时，使用的是齐次坐标（不然没法计算）。而写\bold R \bold a时，使用的是非齐次坐标。如果写在一个等式中，就假设齐次坐标到普通坐标的转换是已经做好了的——因为齐次坐标和非齐次坐标之间的转换事实上非常容易，而在C++程序中你可以使用运算符重载来完成这个功能，保证在程序中看到的运算是统一的。

---

在介绍完向量及其坐标表示和相关运算之后，在阐述了欧氏空间中的坐标系运动时，则由平移操作和平移-旋转操作所构成。其中，旋转过程可以通过特殊正交群\mathrm{SO}(3)来表示，在此过程中仅涉及旋转向量；而平移运动则直接通过\mathbb{R}^3空间中的向量来进行描述。最后部分，则探讨了如何将这些操作整合到一个矩阵形式中以实现完整的坐标系变换效果。

---

四、旋转向量

（一）引入旋转向量

• 用变换矩阵描述一个6自由度的三维刚体运动，至少有以下两个缺点：

1. SO(3)群中的旋转变换由九个参数定义,然而每个旋转变换仅涉及三个自由度参数.由此可知,该参数化方法存在显著的冗余性.相应地,齐次变换矩阵则通过十六个参数表征了仅有的六个自由度.
2. 每一个旋转变换都必须满足正交性条件,并且其行列式的绝对值恒定为1.相应地,齐次变换矩阵也同样需要遵循类似的约束条件,这些限制使得优化问题更加复杂.

是否有方法能够简洁地用旋转向量来表示旋转和平移？例如，在旋转变换中使用三维向量，在平移变换中使用六维向量是否可行？事实上，在任意一次旋转变换中都可以被简洁地表示为一个旋转向量（或轴角/角轴）。这种旋转向量由一个方向与旋转轴一致、长度等于旋转角度的向量构成。因此，在这种情况下仅需三个参数即可描述一次完整的旋转变换操作。同样地，在平移操作中则需要额外增加三个参数（即平移向量），从而使得整个仿射变换所需的参数数目恰好为六维度。

（二）旋转向量到旋转矩阵的转换

考虑一个用\mathbf{R}表示的旋转矩阵。如果使用旋转向量来进行描述，则存在一个旋转向量θn可以等效地描述该旋转。其将旋转向量映射至对应的旋转矩阵的过程则被称为罗德里格斯映射（Rodrigues’s Mapping）。具体而言，在这种情况下有：

\mathbf{R} = \cos\theta \cdot \mathbf{I} + (1 - \cos\theta) \cdot (\mathbf{n}\otimes\mathbf{n}) + \sin\theta \cdot (\mathbf{n})^{\wedge}

其中I代表三维单位矩阵；符号\wedge用于表示将向量映射为其对应的反对称矩阵形式。

（三）旋转矩阵到旋转向量的转换

对于转角\theta以参数形式引入，在罗德里格斯矩阵方程两边取迹运算：
对罗德里格斯旋转矩阵\mathbf R求迹的结果进行展开分析如下：
其中\mathbf I为单位矩阵，
而\mathbf n为单位向量，
其外积运算符记作\bigwedge。
因此，

\begin{aligned} \operatorname {tr} (\mathbf R ) &= \cos\theta \cdot \operatorname {tr} (\mathbf I ) + (1 - \cos\theta) \cdot \operatorname {tr}( \mathbf n \mathbf n^\top ) + \\ &\quad \sin\theta \cdot \operatorname {tr}( \underbrace{ (\mathbf v_3)_{×} }_{% 其外积运算符记作 $\bigwedge$ } ) \\ &= 3\cosθ + (1 - cosθ) \\ &= 1 + 2 cosθ . \end{aligned}

由此可知θ等于arccos((tr(R)−1)/2)。
关于旋转轴单位向量n̂来说，在变换过程中其方向保持不变，则有矩阵R作用于该向量时结果为其自身。
这表明该单位向量n̂恰好是矩阵R所对应的特征值1所对应的特征向量。
通过求解上述线性方程组并将其单位化处理即可确定该旋转轴的具体方向。

五、欧拉角

（一）RPY欧拉角

• 欧拉角则提供了一种非常直观的方式去描述物体的空间取向——它采用三个分立的转动角度将整个旋转变换分解为三次分别绕不同轴线进行的操作。这种分步处理使得每次单独操作都较为简单明了。
  • 在欧拉角体系中有一种比较常用的表示方法即是采用"偏航-俯仰-滚转"（即Y-P-R）三个参数来进行空间坐标系变换描述。这种表示法实际上等效于围绕Z-Y-X顺序依次进行一次绕Z轴、一次绕Y轴和一次绕X轴的旋转变换。

以物体的质量中心为基准进行Z轴方向上的旋转变换操作后能够获得偏航角参数。
围绕经前Y轴进行旋转变换能够计算出俯仰角参数。
基于前一次旋转变换后的X轴基准线能够计算出滚转角参数。

（二）万向锁问题

一个显著的缺点是当采用欧拉角时会遇到著名的万向锁问题（Gimbal Lock）。具体而言，在俯仰角达到±90度时会发生这一现象：第一次和第三次旋转将绕同一个轴进行操作。这种情况下系统将失去一个自由度（原本需要三次旋转变换现在只需要两次）。这种现象被称为奇异性或奇异点问题，在其他变体形式中也会出现。

• 从数学上说,仅用三个实数表达三维空间中的任意旋转都会不可避免地存在奇异性.这一特性导致欧拉角在插值运算和迭代计算过程中面临本质限制,因此不适合作为机器人学中的通用表示方法.在SLAM算法中,我们通常不会直接采用欧拉角来描述姿态,也不会将其用于滤波或优化过程（因为这会引入奇异性问题）.不过,为了验证算法的正确性,将旋转参数转换为欧拉角进行验证是一个有效的方法.
• 在一些专注于二维运动的应用场景下（例如扫地机、自动驾驶车辆等），我们可以将旋转变解为三个欧拉角的组合,并从中提取一个角度（如俯仰角）作为定位数据输出.