SLAM第三讲笔记（1）之矩阵

---

一、名词解释

1. 点：空间基本元素，没有长度、体积。
2. 向量：把两个点连接起来，就构成向量。
3. 刚体：既有位置（三维空间），还有自身的姿态。
4. 基底：张成这个空间的一组线性无关的向量，也就是坐标系。
5. 三维坐标系：通常由3个正交的坐标轴组成（也有非正交，很少）根据坐标的定义不同，分为左手系和右手系。
6. 反对称矩阵：英文Skew-symmetric Matrix，满足$\text{A}=-{\text{A}}^T$，反对称符号^，此符号是一一映射，意味着任意向量都对应着唯一的一个反对称矩阵。
7. 惯性坐标系：也称世界坐标系，特点固定不动。
8. 刚体运动：两个坐标系之间的运动由一个旋转加上一个平移组成。
9. 欧式变换：英文EuclideanTransform，从一个坐标系到另一个坐标系之间，相差了一个欧式变换。欧式变换由旋转和平移组成的。
10. 坐标变换：对于同一个向量p,在不同的坐标系下的向量是不同的，这个变换关系由变换矩阵 T 来描述。
11. 内积：描述向量间的投影关系，结果为$|\text{a}||\text{b}|cos$<a,b>
12. 外积：外积的结果是个向量，它的方向是垂直于这两个向量，大小是$|\text{a}||\text{b}|sin$<a,b>
13. 旋转矩阵：由两组基之间的内积组成，刻画了旋转前后同一个向量坐标的变换关系。是用来描述旋转本身。
14. 齐次坐标：n维的向量用n+1维向量表示，是指一个用投影几何的坐标系统。齐次坐标可以表示无穷远处的点。

---

二、旋转矩阵

1.点、向量和坐标系

1. 物体的位置是指在空间中的哪个地方，姿态则是指方向。用线性代数的知识来描述三维空间中的某个点的坐标，先找到该空间的基底（$e_1$,$e_2$$e_3$）,那么任意向量a在这组基下就有一个坐标：
   $$a=[e_1,e_2,e_3]\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=a_1{e}_1+a_2{e}_2+a_3{e}_3$$
   这里$(a_1,a_2,a_3{)}^T$称为a在此基下的坐标。坐标的具体取值，一是和向量本身有关，二是和坐标系（基）的选取有关。
2. 根据线性代数的基本知识，我们可以谈论向量与向量，以及向量与数之间的运算。例如：数乘、加法、减法、内积、外积等。向量加法就是把他们的各自的坐标相加，减法则是相减。内积是"·"乘，外积是"x"乘：
   $$\text{a}\cdot \text{b}={\text{a}}^T\text{b}=\sum _{i=1}^3{a}_i{b}_i=|a||b|cos<\text{a},\text{b}>$$
   其中<a,b>指向量a,b的夹角，内积也可以描述向量间的投影关系。外积则是：
   $$\text{a}\times \text{b}=\begin{array}{c}\Vert \begin{array}{ccc}{e}_1& e_2& e_3\\ a_1& a_2,& a_3\\ b_1& b_2& b_3\end{array}\Vert \end{array}=\begin{array}{c}\left[\begin{array}{c}{a}_2{b}_3-a_3{b}_2\\ a_3{b}_1-a_1{b}_3\\ a_1{b}_2-a_2{b}_1\end{array}\right]\end{array}=\begin{array}{c}\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]\end{array}\text{b}{=}^{def}{\text{a}}^{\wedge }\text{b}$$
   外积的结果是一个向量，它的方向垂直于这两个向量，大小为|a||b|sin<a,b>,是两个向量张成的四边形的有向面积。对于外积运算，我们引入^符号，把a写成一个矩阵。事实上是一个反对称矩阵。外积把$\text{a}\times \text{b}$写成了矩阵与向量的乘法${\text{a}}^{\wedge }\text{b}$，把它变成了线性运算。a向量的反对称矩阵:
   $${\text{a}}^{\wedge }=\begin{array}{c}\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]\end{array}$$
   向量和加减法、内外积，即使在不谈论它们的坐标时也可以计算。例如：虽然内积在有坐标时，可以用两个向量的分量乘积之和表达，但是即使不知道它们的坐标，也可以通过长度和夹角来计算二者的内积，所以两个向量的内积结果和坐标系的选取是无关的。

2.坐标系间的欧式变换

在实际场景中，定义各种各样的坐标系。在机器人学中，我们会给每一个连杆和关节定义它们的坐标系。在运动中，设定一个惯性坐标系（世界坐标系）。刚体运动是两个坐标系之间的运动由一个旋转加上一个平移组成，两个坐标系之间相差了一个欧式变换。分别考虑旋转和平移。

2.1 旋转矩阵

设某个单位正交基$(e_1,e_2,e_3)$经过一次旋转转变成了$(e_1^{,},e_2^{,},e_3^{,})$。那么，对于同一个向量$a$(该向量并没有随着两个坐标系的旋转而发生运动)，它在两个坐标系下的坐标为$[a_1,a_2,a_3{]}^T$和$[a_1^{,},a_2^{,},a_3^{,}{]}^T$。因为向量本身没有变化。所以根据坐标定义有：
$$[e_1,e_2,e_3]\begin{array}{c}\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]\end{array}=[e_1^{\prime },e_2^{\prime },e_3^{\prime }]\begin{array}{c}\left[\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]\end{array}$$
为了描述两个坐标系的关系，我们对上述等式的左右两边同时左乘$\begin{array}{c}\left[\begin{array}{c}{e}_1^T\\ e_2^T\\ e_3^T\end{array}\right]\end{array}$，那么左边就变成单位矩阵了。所以：
$$\begin{array}{c}\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]\end{array}=\begin{array}{c}\left[\begin{array}{ccc}{e}_1^T{e}_1^{\prime }& e_1^T{e}_2^{\prime }& e_1^T{e}_3^{\prime }\\ e_2^T{e}_1^{\prime }& e_2^T{e}_2^{\prime }& e_2^T{e}_3^{\prime }\\ e_3^T{e}_1^{\prime }& e_3^T{e}_2^{\prime }& e_3^T{e}_3^{\prime }\end{array}\right]\end{array}\begin{array}{c}\left[\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]\end{array}{=}^{def}{\text{Ra}}^{\prime }$$
我们把中间的矩阵拿出来，定义一个矩阵R。这个矩阵由两组基之间的内积组成，刻画了旋转本身。因此，称为旋转矩阵同时，该矩阵各个分量是两个坐标系基的内积，由于基向量的长度为1，所以实际上是各基向量夹角的余弦值。所以这个矩阵也叫方向余弦矩阵。
旋转矩阵有些特别的性质，它是一个行列式为1的正交矩阵，行列式为1的正交矩阵也是一个旋转矩阵。所以，可以将n维的旋转矩阵的集合定义如下：
S O ( n ) = { R ∈ R n × n ∣ RR T = I , d e t ( R = 1 ) } SO(n)= \{ \textbf{R} \in R^{n \times n} | \textbf{RR}^T = I , det(\textbf{R}=1) \} SO(n)={R∈Rn×n∣RRT=I,det(R=1)}
SO(n)是特殊正交群（Special Orthogonal Group)，这个集合由n维空间的旋转矩阵组成。SO(3)就是指三维空间的旋转。通过旋转矩阵，我们可以直接谈论两个坐标系之间的旋转变换。由于旋转矩阵为正交矩阵，它的逆（转置）描述了一个相反的旋转，即：
$${\text{a}}^{\prime }={\text{R}}^{-1}\text{a}={\text{R}}^T\text{a}$$
$R^T$刻画了一个相反的旋转。

2.2 平移

在欧式变换中，除了旋转还有平移，考虑世界坐标系中的向量a,经过一次旋转（用旋转矩阵R）和一次平移t后，得到了${\text{a}}^{\prime }$那么旋转加平移：
$${\text{a}}^{\prime }=\text{Ra}+\text{t}$$
其中，t 称为平移向量。

2.3 欧式变换

用一个旋转矩阵R和一个平移向量t完整地描述了一个欧式空间的坐标变换关系。实际中，我们会定义坐标系1和坐标系2，那么向量a在两个坐标系下的坐标为${\text{a}}_1$、${\text{a}}_2$，它们之间的关系：
$${\text{a}}_1={\text{R}}_{12}{\text{a}}_2+{\text{t}}_{12}$$
其中，$R_{12}$是指“把坐标系2的向量变换到坐标系1”中。$t_{12}$是坐标系1原点指向坐标系2原点的向量，在坐标系1下取的坐标，即从1到2的向量。$t_{21}$，是从2指向1的向量，在坐标系2下的坐标，却并不等于$-t_{21}$，而是和两个系的旋转还有关系。

3 变换矩阵与其次矩阵

$\text{a}={\text{Ra}}^{\prime }+\text{t}$ 完美的描述了欧式空间的旋转和平移。不过还有小个问题：这里的变换不是一个线性关系。假设进行两次变换：${\text{R}}_1{\text{t}}_1$和${\text{R}}_2{\text{t}}_2$：
$$\text{b}={\text{R}}_1\text{a}+{\text{t}}_1,\text{c}={\text{R}}_2\text{b}+{\text{t}}_2$$
那么从a到c的变换：
$$\text{c}={\text{R}}_2({\text{R}}_1\text{a}+{\text{t}}_1)+{\text{t}}_2$$
在三维向量的末尾添加1（单位向量），将其变成四维向量，称为齐次坐标。对于四维向量我们可以把旋转和平移写在一个矩阵里：
$$\begin{array}{c}\left[\begin{array}{c}{a}^{\prime }\\ 1\end{array}\right]\end{array}=\begin{array}{c}\left[\begin{array}{cc}\text{R}& \text{t}\\ 0^T& 1\end{array}\right]\end{array}\begin{array}{c}\left[\begin{array}{c}a\\ 1\end{array}\right]\end{array}{=}^{def}\text{T}\begin{array}{c}\left[\begin{array}{c}a\\ 1\end{array}\right]\end{array}$$
整个关系变成了线性关系，该式中，矩阵T称为变换矩阵。它具有比较特别的结构：左上角为旋转矩阵，右侧为平移矩阵，左下角为0向量，右下角为。这种矩阵又称为特殊欧式群（Special Euclidean Group）：
S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 ) } SE(3)= \{ \textbf{T}= {\begin{gathered} \begin{bmatrix} \textbf{R} & \textbf{t}\\ \textbf{0}^T& 1 \end{bmatrix} \end{gathered}} \in R^{4 \times 4} | \textbf{R} \in SO(3) , \text{t} \in R^3) \} SE(3)={T=[R0T​t1​]​∈R4×4∣R∈SO(3),t∈R3)}
与SO(3)一样，求解该矩阵的逆表示一个反向的变换：
$${\text{T}}^{-1}=\left[\begin{array}{cc}{\text{R}}^T& -{\text{R}}^T\text{t}\\ {\text{0}}^T& 1\end{array}\right]$$
我们同样用${\text{T}}_{12}$表示从2到1的变换。为了保持符号的简洁，在不引起歧义的情况下，不可以区分齐次坐标系和普通坐标系，当我们写Ta时，使用的时齐次坐标，而Ra时，使用的是非齐次坐标。
用$\tilde{a}$表示a的齐次坐标，用齐次坐标和变换矩阵，两次变换的叠加就可以线性的表示：
$$\tilde{b}={\text{T}}_1\tilde{a},\tilde{c}={\text{T}}_2\tilde{b}\Rightarrow \tilde{c}={\text{T}}_2{\text{T}}_1\tilde{a}$$

---

三 、总结

1. 向量及其坐标的表示，向量间的运算：数乘、加减法、内外积。
2. 坐标系之间的运动由欧式变换描述，由旋转和平移组成：旋转矩阵SO(3)描述，平移直接由一个$R^3$向量描述。
3. 变换矩阵SE(3)：使用齐次坐标的数学技巧，把平移和旋转放在一个矩阵中。

---