视觉SLAM十四讲学习笔记-第三讲-旋转向量、欧拉角、四元数

专栏系列文章如下：

视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第二讲-开发环境搭建_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第三讲-旋转矩阵和Eigen库_goldqiu的博客-CSDN博客

旋转向量

矩阵表示方式至少有以下几个缺点：

1.SO(3)的旋转矩阵有9个量，但一次旋转只有3个自由度。因此这种表达方式是冗余的。同理，变换矩阵用16个量表达了6自由度的变换。

2.旋转矩阵自身带有约束：它必须是个正交矩阵，且行列式为 1。变换矩阵也是如此。当想要估计或优化一个旋转矩阵/变换矩阵时，这些约束会使得求解变得更困难。

因此，我们希望有一种方式能够紧凑地描述旋转和平移。例如，用一个三维向量表达旋转，用六维向量表达变换。事实上，任意旋转都可以用一个旋转轴和一个旋转角来刻画。于是，我们可以使用一个向量，其方向与旋转轴一致，而长度等于旋转角。这种向量称为旋转向量（或轴角/角轴，Axis-Angle），只需一个三维向量即可描述旋转。同样，对于变换矩阵，我们使用一个旋转向量和一个平移向量即可表达一次变换。这时的变量维数正好是六维。

考虑某个用R表示的旋转。如果用旋转向量来描述，假设旋转轴为一个单位长度的向量n，角度为 θ，那么向量 θn也可以描述这个旋转。

从旋转向量到旋转矩阵的转换过程由罗德里格斯公式（Rodrigues’s Formula ）表明，转换的结果 ：

符号∧是向量到反对称矩阵的转换符。反之，我们也可以计算从一个旋转矩阵到旋转向量的转换。对于转角 θ，取两边的迹，有

得：

关于转轴n，由于旋转轴上的向量在旋转后不发生改变，说明：

Rn = n

因此，转轴n是矩阵R特征值1对应的特征向量。求解此方程，再归一化，就得到了旋转轴。

也可以从“旋转轴经过旋转之后不变”的几何角度看待这个方程。

注：求迹（trace）即是求矩阵的对角线元素之和。

欧拉角

无论是旋转矩阵、旋转向量，它们虽然能描述旋转，但对我们人类是非常不直观的。当我们看到一个旋转矩阵或旋转向量时，很难想象出这个旋转究竟是什么样的。当它们变换时，我们也不知道物体是向哪个方向在转动。

而欧拉角则提供了一种非常直观的方式来描述旋转——它使用了3个分离的转角，把一个旋转分解成 3 次绕不同轴的旋转。而人类很容易理解绕单个轴旋转的过程。

但是，由于分解方式有许多种，所以欧拉角也存在着众多不同的、易于混淆的定义方法。比如说，先绕X轴旋转，再绕Y轴，最后绕Z轴，就得到了一个XYZ轴的旋转。同理，可以定义ZYZ、ZYX等旋转方式。

如果讨论得更细一些，还需要区分每次是绕固定轴旋转的，还是绕旋转之后的轴旋转的。这种定义方式上的不确定性带来了很多实用当中的困难。在特定领域内，欧拉角通常有统一的定义方式。

欧拉角当中比较常用的一种，便是用“偏航−俯仰−横滚”（yaw-pitch-roll）3个角度来描述一个旋转。它等价于ZYX轴的旋转。假设一个刚体的前方（朝向我们的方向）为X轴，右侧为Y轴，上方为Z轴。那么，ZYX转角相当于把任意旋转分解成以下3个轴上的转角：

1. 绕物体的Z轴旋转，得到偏航角 yaw；

2. 绕旋转之后的Y轴旋转，得到俯仰角 pitch；

3. 绕旋转之后的 X 轴旋转，得到横滚角 roll。

此时，可以使用 [r,p,y]T 这样一个三维的向量描述任意旋转。

大部分领域在使用欧拉角时都有各自的坐标方向和顺序上的习惯，比如东北天和右前上。

欧拉角的一个重大缺点是会碰到万向锁问题（Gimbal Lock ）：在俯仰角为 ±90◦时，第一次旋转与第三次旋转将使用同一个轴，使得系统丢失了一个自由度（由 3 次旋转变成了 2 次旋转）。这被称为奇异性问题，只要想用3个实数来表达三维旋转时，都会不可避免地碰到。

由于这种问题，欧拉角不适于插值和迭代，往往只用于人机交互中。我们很少在SLAM程序中直接使用欧拉角表达姿态，同样不会在滤波或优化中使用欧拉角表达旋转，因为它具有奇异性。如果想验证自己的算法是否有错，转换成欧拉角能够帮你快速分辨结果是否正确。

注：1.可以看看相关视频来理解万向锁。

2.旋转向量也有奇异性，发生在转角θ超过2π而产生周期性时。

四元数

四元数的定义

前面讲到，旋转矩阵用9个量描述3自由度的旋转，具有冗余性；欧拉角和旋转向量是紧凑的，但具有奇异性。事实上，我们找不到不带奇异性的三维向量描述方式。类似于用两个坐标表示地球表面（如经度和纬度），必定存在奇异性（纬度为 ±90◦ 时经度无意义）。

回忆以前学习过的复数。我们用复数集C表示复平面上的向量，而复数的乘法则表示复平面上的旋转：乘上复数i相当于逆时针把一个复向量旋转 90◦。

类似地，在表达三维空间旋转时，也有一种类似于复数的代数：四元数（Quaternion）。四元数是Hamilton找到的一种扩展的复数。它既是紧凑的，也没有奇异性。缺点是四元数不够直观，其运算稍复杂些。

把四元数与复数类比可以更快地理解四元数。例如，当我们想要将复平面的向量旋转 θ 角时，可以给这个复向量乘以 eiθ。这是极坐标表示的复数，它也可以写成普通的形式，只要使用欧拉公式即可： eiθ = cosθ + isinθ.

这是一个单位长度的复数。所以，在二维情况下，旋转可以由单位复数来描述。类似地，三维旋转则可以由单位四元数来描述。

一个四元数q拥有一个实部和三个虚部。 q = q0 + q1i + q2j + q3k,

其中i,j,k为四元数的三个虚部。这三个虚部满足以下关系式：

如果把i,j,k看成三个坐标轴，那么它们与自己的乘法和复数一样，相互之间的乘法和外积一样。有时人们也用一个标量和一个向量来表达四元数：

q = [s,v]T , s = q0 ∈ R, v = [q1,q2,q3]T ∈ R3,

s 称为四元数的实部，v称为它的虚部。如果一个四元数的虚部为0，称之为实四元数。反之，若它的实部为 0，则称之为虚四元数。

可以用单位四元数表示三维空间中任意一个旋转，不过这种表达方式和复数有些许不同。

在复数中，乘以i意味着旋转 90◦。这是否意味着四元数中，乘i就是绕i轴旋转90◦？那么，ij=k是否意味着，先绕i转90◦，再绕j转90◦，就等于绕k转90◦？

情况并不是这样。应该是乘以i对应着旋转180◦，这样才能保证ij=k的性质。而i2（平方） = −1，意味着绕i轴旋转360°后得到一个相反的东西。这个东西要旋转两周（720°）才会和它原先的样子相等。

四元数的运算

四元数常见的运算有四则运算、数乘、求逆、共轭等。

现有两个四元数qa,qb，它们的向量表示为 [sa,va]T,[sb,vb]T，或者原始四元数表示为：

qa = sa + xai + yaj + zak, qb = sb + xbi + ybj + zbk. 那么，

1.四元数qa,qb 的加减运算为：

qa ± qb = [sa ± sb,va ± vb]T

2. 乘法是把qa的每一项与qb的每项相乘，最后相加。整理可得：

写成向量形式并利用内外积运算：

在该乘法定义下，两个实的四元数乘积仍是实的，这与复数也是一致的。然而，由于最后一项外积的存在，四元数乘法通常是不可交换的，除非va和vb在R3中共线，此时外积项为零。

3. 四元数的模长定义为

两个四元数乘积的模即为模的乘积。这使得单位四元数相乘后仍是单位四元数。

4. 四元数的共轭是把虚部取成相反数：

四元数共轭与其本身相乘，会得到一个实四元数，其实部为模长的平方：

5. 一个四元数的逆为

按此定义，四元数和自己的逆的乘积为实四元数1：

如果q为单位四元数，其逆和共轭就是同一个量。同时，乘积的逆和矩阵有相似的性质：

6. 四元数与数相乘：

kq = [ks,kv]T

用四元数表示旋转

可以用四元数表达对一个点的旋转。假设一个空间三维点p=[x,y,z]∈R3，以及一个由单位四元数q指定的旋转。三维点p经过旋转之后变为p′。如果使用矩阵描述，那么有p′ = Rp。而如果用四元数描述旋转，它们的关系又如何来表达呢？

首先，把三维空间点用一个虚四元数来描述：p = [0,x,y,z]T = [0,v]T.

相当于把四元数的3个虚部与空间中的3个轴相对应。那么，旋转后的点p′即可表示为这样的乘积：p′ = qpq−1.

乘法均为四元数乘法，结果也是四元数。最后把p′的虚部取出，即得旋转之后点的坐标。计算结果的实部为0，故为纯虚四元数。

四元数到其他旋转表示的转换

任意单位四元数描述了一个旋转，该旋转也可用旋转矩阵或旋转向量描述。四元数乘法也可以写成一种矩阵的乘法。设q = [s,v]T，那么，定义如下的符号 + 和 ⊕ 为：

这两个符号将四元数映射成为一个 4×4 的矩阵。于是四元数乘法可以写成矩阵的形式：

同理：

考虑使用四元数对空间点进行旋转的问题。有：

代入两个符号对应的矩阵，得：

因为p′和p都是虚四元数，那么事实上该矩阵的右下角即给出了从四元数到旋转矩阵的变换关系：

对上式两侧求迹，得：

由

得

所以：

总之，四元数到旋转向量的转换公式可列写如下：

在实际编程中，程序库通常会为我们准备好各种形式之间的转换。无论是四元数、旋转矩阵还是轴角，它们都可以用来描述同一个旋转。我们应该在实际中选择最为方便的形式。