视觉SLAM十四讲(三)——三维空间刚体运动(上)

三维空间刚体运动的描述方法有：旋转矩阵、变换矩阵、旋转向量、欧拉角和四元数，接下来将逐一介绍它们

一、旋转矩阵

点、向量、坐标系
* 点——存在于三维空间之中，点和点组成向量，点本身由原点指向它的向量所描述
* 向量——带指向性的箭头，可以进行加法减法等运算，定义坐标系后，向量可以由$ R^3 $当中的三个数表示，如何理解这句话呢。如下图所示：

在代数中，我们用一组基底和向量 $a$ 在每个坐标轴上的投影来表示一个向量，对于 $a$，通过某种线性组合，可以表示为$a = a_xe_1+a_ye_2+a_ze_3$
而上面那句话的意思是在矩阵运算中，$a$ 可以表示为 $\left[ \begin{matrix} a_x \\ a_y \\ a_z \end{matrix} \right]$，因为$(e_1,e_2,e_3)\left[ \begin{matrix} a_x \\ a_y \\ a_z \end{matrix} \right] = a_xe_1+a_ye_2+a_ze_3$
* 坐标系——三个正交的轴，构成线性空间的一组基，分为左手系和右手系
* 向量的运算可以由坐标运算来表达：加减法，内积，外积
问题的出现——一个最简单的情况，机器人从某一点直线运动到另一点，假设机器人是质点，并且和目标点处于同一平面上，分别以机器人和目标点建立坐标系，在移动过程中机器人的坐标系位置一直在变，要计算与目标点的距离，就需要描述坐标系之间如何变化
* 进而——如何计算同一个向量在不同坐标系里的坐标
* 如果刚才的机器人不是直线运动，而会有拐弯，这时坐标系就会旋转，因此描述整个运动过程就是三个轴的旋转和原点间的平移，这就是所谓的欧式变换，保证同一个向量在各个坐标系下的长度和夹角都不会发生变化，通过旋转和平移两部分组成
问题解决
* 平移是一个向量
* 旋转
- 设某坐标系(用三个方向上的单位向量表示) $(e_1,e_2,e_3)$ 发生了一次旋转，变成了$(e_1^{'},e_2^{'},e_3^{'})$
- 对于某个固定的向量 $a$(向量不随坐标系旋转)，它的坐标怎么变化，其中 $\left[ \begin{matrix} a_1 \\ a_2 \\ a_3 \end{matrix} \right]$ 是 $a$ 在第一个坐标系中的坐标，$\left[ \begin{matrix} a_1^{'} \\ a_2^{'} \\ a_3^{'} \end{matrix} \right]$ 是 $a$ 在另一个坐标系中的坐标，如图，P为向量 $a$
- 坐标关系$[e_1,e_2,e_3]\left[ \begin{matrix} a_1 \\ a_2 \\ a_3 \end{matrix} \right] = [e_1^{'},e_2^{'},e_3^{'}]\left[ \begin{matrix} a_1^{'} \\a_2^{'} \\ a_3^{'} \end{matrix} \right] \$，乘出来的就是向量本身
- 左乘$\left[ \begin{matrix} e_1^{T} \\ e_2^{T} \\ e_3^{T} \end{matrix} \right]$,得：$\left[ \begin{matrix} a_1 \\ a_2 \\ a_3 \end{matrix} \right] = \left[ \begin{matrix} e_1^{T}e_1^{'} & e_1^{T}e_2^{'} & e_1^{T}e_3^{'}\\ e_2^{T}e_1^{'} & e_2^{T}e_2^{'} & e_2^{T}e_3^{'} \\ e_3^{T}e_1^{'} & e_3^{T}e_2^{'} & e_3^{T}e_3^{'} \end{matrix} \right]\left[ \begin{matrix} a_1^{'} \\ a_2^{'} \\ a_3^{'} \end{matrix} \right] \begin{matrix}\Delta\\=\end{matrix}Ra^{'}$
- 中间的矩阵 $R$ 称为旋转矩阵
- 根据定义可以验证
  * $R$ 是一个正交矩阵(矩阵的逆是其转置)
  * $R$ 的行列式为$+1$
- 满足这两个性质的矩阵称为旋转矩阵
旋转矩阵描述了两个坐标的变换关系，ex：$a_1 = R_{12}a_2$，反之 $a_2 = R_{21}a_1$，于是$R_{21} = R_{12}^{-1} = R_{12}^T$，进一步，三个坐标系亦有 $a_3 = R_{32}a_2 = R_{32}R_{21}a_1 = R_{31}a_1$
加上平移 $a^{'} = Ra + t$，因此两个坐标系的刚体运动可以由 $R， t$ 完全描述

二、变换矩阵

变换矩阵的引入
* 用旋转和平移方式有一点不便之处，比如发生了两次变换 $b = R_1a + t_1，c = R_2b + t_2$
* 这时 $c = R_2(R_1a + t_1) + t_2$，叠加起来过于复杂
* 改变形式，写成 $\left[\begin{matrix} a^{'} \ 1 \end{matrix} \right] = \left[ \begin{matrix} R & t\ 0^T & 1 \end{matrix} \right]\left[ \begin{matrix} a\ 1\end{matrix} \right] \begin{matrix} \Delta \ = \end{matrix} T\left[ \begin{matrix}a \ 1 \end{matrix} \right] $
* 例如 $b = T_1a，c = T_2b ，$得出 $ c = T_2T_1a$
这种用四个数表达三维向量的做法称为齐次坐标，这样旋转和平移可以放入同一个矩阵，称为变换矩阵，即 $\left[ \begin{matrix} R & t \\ 0^T & 1\end{matrix} \right]$,其反向变换为 $\left[ \begin{matrix} R^T & -R^Tt \\ 0^T & 1 \end{matrix} \right]$，即矩阵的逆
在 SLAM 中，通常定义世界坐标系 $T_W$ 与机器人坐标系 $T_R$，一个点的世界坐标为 $p_W$，机器人坐标系下为 $p_R$，那么满足关系：$p_R = T_{RW}p_W$，反之亦然，实际编程中可以使用 $T_{RW}$ 或 $T_{WR}$ 来描述机器人位姿

三、旋转向量和欧拉角

旋转向量
* 旋转矩阵和变换矩阵固然可以表示旋转，但是要求太多：每次旋转只需要三个自由度，也就是x，y，z，但是旋转矩阵用9个量表达了3个自由度，变换矩阵用16个量表达了6个自由度，这种表达方式是冗余的，而且旋转矩阵和变换矩阵都必须是正交矩阵，自身约束会在实际求解中增加困难
* 任意旋转都可以用一个旋转轴和一个旋转角刻画，如何理解这句话呢，我们引入向量的外积的概念，别的你先不用知道，记住这句话：向量的外积的方向垂直于这两个向量。是不是一下子明白了，对于某个坐标系的一次旋转，可以通过移动后与该向量的外积来确定它是怎么旋转的
* 方向为旋转轴，长度为转过的角度，这就称为轴角或旋转向量，$w = \theta n$
* 轴角与旋转矩阵的不同：旋转矩阵需要九个量，有正交性约束和行列式约束，轴角：三个量，没有约束
* 只是表达方式不同，表达内容一样
* 轴角即为李代数，这里只是简单了解一下，下一篇会介绍它的原理
* 轴角转旋转矩阵——$R = \cos \theta I+ (1-\cos\theta)nn^T + \sin\theta n^{\land} $
* 旋转矩阵转轴角——角度： $\theta = \arccos (\frac{tr (R)-1}{2})$，轴： $Rn = n$
欧拉角
* 将旋转分解为三次不同轴上的转动，以便理解
* 例如：按照 Z-Y-X 顺序转动
* 轴可以使定轴或动轴，顺序亦可不同，因此存在许多种定义方式不同的欧拉角
* 常见的有 yaw-pitch-roll (偏航-俯仰-滚转) 角等等
欧拉角的万向锁问题
* ZYX顺序中，若 pitch 为正负90度，则第三次旋转和第一次绕同一个轴，使得系统丢失了一个自由度——存在奇异性问题
* 有点难理解，可以看看这个视频：视频传送门

四、四元数
一种扩展的复数
* 回忆：复数可以表达二维平面的旋转
* 怎么理解这句话，首先看一下复数的另一种表达方式——矩阵表达式
* $a+ib \leftrightarrow \begin{pmatrix}a & -b \\ b & \;\; a \end{pmatrix} = r\begin{bmatrix} \cos\varphi & -\sin\varphi \\ \sin\varphi & \cos\varphi \end{bmatrix} = r\exp\left(\varphi\begin{bmatrix} 0 & -1 \\ 1 & 0 \end{bmatrix}\right)$
* $a,b$ 为实数，此类矩阵的和、积及乘法逆都是此类矩阵，$\begin{pmatrix}a & -b \\ b & \;\; a \end{pmatrix} = a\begin{pmatrix}1 & 0 \\ 0 & 1 \end{pmatrix} + b\begin{pmatrix}0 & -1 \\ 1 & 0 \end{pmatrix}$,即实数1对应着单位矩阵$\begin{pmatrix}1 & 0 \\ 0 & 1 \end{pmatrix}$，虚数单位 $i$ 对应着 $\begin{pmatrix}0 & -1 \\ 1 & 0 \end{pmatrix}$
* 现假设 $a$ = 1, $b$ = 1，在复平面上代表点(1,1)，给 $1+i$ 乘以 $i$，即原实数单位对应的矩阵成为 $\begin{pmatrix}0 & -1 \\ -1 & 0 \end{pmatrix}$，行列式值为 -1，代表点(-1,1)，相当于逆时针旋转了90度
* 对于 $a+ib$ ，如果 $a$ 本身是 $m+jn$ ，虚部为 $j$，$b$ 也是一个复数 $x+ky$，虚部为 $k$，则构成的代数就是四元数
四元数有三个虚部，可以表达三维空间的旋转
* $q = q_0 + q_1i + q_2j + q_3k$
* $q = [s,v]，s = q_0 ∈ R，v= [q_1,q_2,q_3]^T ∈ R^3$
* 虚部之间的关系$\begin{cases} i^2 = j^2 = k^2 = -1 \\ ij = k,ji = -k \\ jk = i,kj = -i \\ ki = j,ik = -j\end{cases}$
* 四元数表示空间中的点——若三维空间的一个点的坐标为(x,y,z)，则它用纯四元数(类似于纯虚数，实部为0) $xi+yj+zk$ 表示
* 单位四元数——单位四元数的欧拉公式: $\cos\frac{\theta}{2} + (xi+yj+zk)\sin\frac{\theta}{2}$，则$q = [\cos\frac{\theta}{2},n_x\sin\frac{\theta}{2},n_y\sin\frac{\theta}{2},n_z\sin\frac{\theta}{2}]^T$ 表示单位四元数，其中 $[n_x,n_y,n_z]^T$ 是一个单位向量
* 四元数的一些运算和性质

加减法：$q_a \pm q_b = [s_a \pm s_b,v_a \pm v_b]$
乘法：$q_aq_b = s_as_b - x_ax_b -y_ay_b -z_az_b + (s_ax_b - x_as_b -y_az_b -z_ay_b)i + (s_ay_b - x_az_b -y_as_b -z_ax_b)j + (s_az_b - x_ay_b -y_ax_b -z_as_b)k$
乘法(向量表示)：$q_aq_b = [s_as_b - v_a^Tv_b , s_av_b+s_bv_a+v_a×v_b]$
共轭：$q_a^* = s_a - x_ai - y_aj - z_ak = [s_{a,} - v_a]$
模长：$||q_a|| = \sqrt{s_a^2 + s_a^2 + y_a^2 + z_a^2}$
逆：$q^{-1} = q^*/||q||^2$
数乘：$kq = [k_s,k_v]$
点乘：$q_a · q_b = s_as_b + x_ax_bi + y_ay_bj + z_az_bk$

四元数和轴角的关系
* 来看看旋转向量，某个旋转是绕着单位向量 $n = [n_x,n_y,n_z]$ 进行了角度为 $\theta$ 的旋转，那么其四元数形式为： $q = [\cos\frac{\theta}{2},n_x\sin\frac{\theta}{2},n_y\sin\frac{\theta}{2},n_z\sin\frac{\theta}{2}]^T$，你可能会产生疑问，为什么是 $\frac{\theta}{2}$ , 这个问题下面再说
* 四元数到轴角：$\begin{cases}\theta = 2\arccos q_0 \\ [n_x,n_yn_z]^T = [q_1,q_2,q_3]^T/\sin \frac{\theta}{2}\end{cases}$
* 类似可知四元数亦可转换为旋转矩阵，欧拉角
如何用单位四元数表示一个三维空间旋转

设点 $p$ 经过一次以 $q$ 表示的旋转后，得到了 $p^{'}$，它们的关系如何表示?
将 $p$ 的坐标用四元数表示(纯四元数)：$p = [0,x,y,z] = [0,v]$
旋转之后的关系为：$p^{'} = qpq^{-1}$
四元数相比于轴角，欧拉角的优势：紧凑，无奇异性

问题

为什么旋转了角度$\theta$ 要用 $\frac{\theta}{2}$ ？
为什么用单位四元数表示一个三维空间旋转时，旋转之后的关系为$p^{'} = qpq^{'}$

解决
感谢知乎用户 Yang Eninala 的回答
链接：https://www.zhihu.com/question/23005815/answer/33971127

单位四元数有四个变量，完全可以被看作一个四维向量。单位四元数（norm=1）则存在于四维空间的一个球面上。$q_aq_b$，四元数$q_a$乘以四元数$q_b$其实看作（1）对进行左旋转，或者（2）对进行右旋转。所以从始至终，四元数定义的都是四维旋转，而不是三维旋转！任意的四维旋转都可以唯一的拆分为一个左旋转和一个右旋转，表达出来就是。这里，我们对单位四元数（四维向量）进行了一个左旋转和一个右旋转。结果当然是一个单位四元数
三维旋转就是四维旋转的一个特例，就像二维旋转是三维旋转的一个特例一样。说是特例其实不准确，准确的说是一个子集或者subgroup。为了进行三维旋转运算，汉密尔顿首先在四维空间里划出了一块三维空间。汉密尔顿定义了一种纯四元数（pure quaternion），其表达式为。纯四元数第一项为零，它存在于四维空间的三维超平面上，与三维空间中的三维向量一一对应。然后，就有了我们常见的这种左乘单位四元数，右乘其共轭的表达式。简单的说，当对一个三维向量进行三维旋转后，我们希望得到的是一个三维向量。那么这个左乘单位四元数，右乘其共轭的运算保证了结果是一个在三维超平面上中的纯四元数。
至于为什么是 $\frac{\theta}{2}$ 呢，原因如下：

$q$ 做的就是一个 $\frac{\theta}{2}$ 的旋转，而 $q^{-1}$ 做的也是一个 $\frac{\theta}{2}$ 的旋转，两次旋转的结果是一个旋转角为 $\theta$ 的旋转

旋转矩阵、变换矩阵、旋转向量、欧拉角、四元数

视觉SLAM十四讲(三)——三维空间刚体运动(上)

一、旋转矩阵

二、变换矩阵

三、旋转向量和欧拉角

四、四元数

猜你喜欢