SlAM 14讲笔记

---

前言

slam学习 B站上一位学姐的课程笔记 记录了前三章的内容.
传送门:SLAM十四讲 如有侵权,请通知我删除.

一、了解slam

1、机器人自主运动的两大基本问题

1、我现在在什么地方----定位
2、周围环境什么样----建图。

2、各种相机的优缺点

单目相机：

优点：便于标定和识别；
缺点：无法确定物体的真实大小，丢失Z轴信息

双目：

优点：极限距离越大，能够测量的距离就越大越远。可用于室内和室外
缺点：配置与标定较为复杂。需要GPU和FPGA加速来处理

深度相机：红外结构光或TOF（time of fly）

优点：相比双目可以节省大量计算资源。
缺点：测量范围狭窄，噪声较大，视野小，易受日光干扰。
主要用于：三维成像和距离的测量。

3、数学表达：

1、什么是运动

从k-1时刻到k时刻，机器人的位置是如何变化的？
关于运动模型：

机器人会携带一个测量自身运动的传感器，这个传感器可以测量有关运动的读数，但不一定直接就是位置差，还可能是加速度、角速度这些信息。
使用抽象数学模型来描述：

$$\overrightarrow{x_k}=f(\overrightarrow{x_{k-1}},\overleftarrow{u_k},\overleftarrow{w_k})$$
其中 $$\overrightarrow{u_k}$$ 运动传感器的读数；
$$\overrightarrow{w_k}$$ 是运动过程中加入的噪声；即运动方程

2、什么是观测：

$$假设机器人在k时刻在\overrightarrow{x_k}处观测到了某个路标\overrightarrow{y_i}，如何用数学语言描述。$$

关于观测模型
$$机器人在\overrightarrow{x_k}位置上看到某个路标点\overrightarrow{y_j},产生了一个观测数据\overrightarrow{z_{k,j}}$$

$$\overrightarrow{z_{k,j}}=f(\overrightarrow{y_j},\overrightarrow{x_k},\overrightarrow{v_{k,j}})$$
其中 $$\overrightarrow{v_{k,j}}$$ 是观测的噪声 即观测方程；

· 事实上，根据机器人的真实运动和传感器的种类，存在着若干种参数化形式。而考虑视觉SLAM时，传感器是相机，则观测方程就是“对路标点拍摄后，得到图像中的像素”的过程。

举个例子解释：
假设机器人在平面运动，那么位姿（位置+姿态）由两个位置的坐标和一个转交来描述 :$$\overrightarrow{x_k}=(x,y,\Theta {)}^T$$,同时，运动传感器能够测量到机器人在任意两个时间间隔位置和转交的变化量:$$\overrightarrow{u_k}=(\Delta x,\Delta y,\Delta \Theta {)}^T$$于是，此时运动方程就可以写成：
$${\left(\begin{array}{c}x\\ y\\ \Theta \end{array}\right)}_k={\left(\begin{array}{c}x\\ y\\ \Theta \end{array}\right)}_{k-1}+{\left(\begin{array}{c}\Delta x\\ \Delta y\\ \Delta \Theta \end{array}\right)}_k+\overrightarrow{w_k}$$
关于观测方程，比如机器人携带着一个二维激光传感器$$（当激光传感器观测一个\underline{2D}路标点时，可以测到路标点与机器人之间的距离r和夹角\varphi )$$
$$记路标点：\overrightarrow{y_k}=[y_1,y_2{]}_j^T,位姿为：\overrightarrow{x_k}=[x_1,x_2{]}_k^T，观测数据为：\overrightarrow{z_{k,j}}=[r_{k,j},{\varphi }_{k,j}{]}^T$$
于是观测方程就可以成:
$${\left[\begin{array}{c}{r}_{k,j}\\ {\varphi }_{k,j}\end{array}\right]}_{k-1}=\left[\begin{array}{c}\sqrt{(\overrightarrow{y_{1,j}}-\overrightarrow{x_{1,k}}{)}^2+\overrightarrow{y_{2,j}}-\overrightarrow{x_{2,k}}{)}^2}\\ \arctan \left(\begin{array}{c}\frac{\overrightarrow{y_{2,j}}-\overrightarrow{x_{2,k}}}{\overrightarrow{y_{1,j}}-\overrightarrow{x_{1,k}}}\end{array}\right)\end{array}\right]+\overrightarrow{v}$$

$$\{\begin{array}{ll}{x}_k=f(x_{k-1},u_k,w_k),& k=1,\dots ,K\\ z_{k,j}=f(y_j,x_k,v_{k,j}),& (k,j)\in O\end{array}$$
这两个方程描述了最基本的SLAM问题：
当知道运动测量的读数u以及传感器的读书z时，如何求解定位问题（估计x）和见图问题（估计y）

二、三位空间刚体运动

1. 旋转矩阵与变换矩阵

1.1 基础知识

三维空间由3个轴组成，则一个空间点的位置可以由3个坐标指定
对于一个刚体（在运动和受力作用后，形状和大小不变，而且内部各点的相对位置不变的物体），不光有位置，还有自身的姿态–相机可以堪称三维空间中的刚体，则位置就是说相机在空间中的那个地方，姿态是指相机的朝向（相机处于空间（0，0，0）处，朝向正前方）；

那么数学语言怎么描述?
$$点=\{\begin{array}{l}点没有长度，没有体积\\ 点和点可以组成向量\end{array}$$

$$向量=\{\begin{array}{l}带指向性的箭头（方向性）\\ 可以进行加法、减法等运算\end{array}$$

$$坐标=\{\begin{array}{l}当我们指定坐标后，才可以谈论该向量在此坐标系下的坐标\\ 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\end{array}$$

$$坐标系=\{\begin{array}{l}构成线性空间的一组基\\ 左手系和右手系\end{array}$$
左手系：右手系：

向量的运算可以由坐标运算来表示
$$\begin{gathered} 加减法内积：a\cdot b=a^{T}b=\sum _{i=1}^3{a}_i{b}_i=|a||b|cos<a,b> \\ 外积：a\times b=\left[\begin{array}{ccc}i& j& k\\ a_1& a_2& a_3\\ b_1& b_2& b_3\end{array}\right]=\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]=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]b\widehat{=}\hat{a}b \end{gathered}$$

对于外积，备注一下
$$若\vec{i},\vec{j},\vec{k}分别表示x轴，y轴，z轴正向的单位向量，则$$
$$\begin{gathered} \{\begin{array}{l}\vec{i}\times \vec{j}=\vec{k}\\ \vec{j}\times \vec{k}=\vec{j}\\ \vec{k}\times \vec{i}=\vec{j}\end{array}\{\begin{array}{l}\vec{a}=a_1\vec{i}+a_2\vec{j}+a_3\vec{k}\\ \vec{b}=b_1\vec{i}+b_2\vec{j}+b_3\vec{k}\end{array} \\ a\times b=(a_2{b}_3-a_3{b}_2)\vec{i}+(a_3{b}_1-a_1{b}_3)\vec{j}+(a_1{b}_2-a_2{b}_1)\vec{k} \end{gathered}$$

1.2 坐标系之间的变换

基本问题： 坐标系之间是如何变化的->如何计算同一个向量在不同坐标系里的坐标

• 两个不同的坐标系，如何描述左侧到右侧的变化？
• 直观来看由两个部分组成1、三个轴的旋转 2、原点之间的平移 平移是一个向量
• 那么旋转是什么？

• 旋转
  $$-设某坐标系（e_1.e_2.e_3)经过一次旋转变成了(e_1^{‘},e_2^{‘},e_3^{‘})$$
  对于某个固定的向量a， 它的坐标是如何变化的？
  坐标关系：
  $$[e_1.e_2.e_3]\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=[e_1^{‘},e_2^{‘},e_3^{‘}]\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]$$
  $$左乘\left[\begin{array}{c}{e}_1^T\\ e_2^T\\ e_3^T\end{array}\right]->\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[\begin{array}{ccc}{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{array}\right]\widehat{=}R{a}^{‘}.$$

• 中间的矩阵R称为旋转矩阵
• 可以证明 $$\begin{gathered} R是一个正交矩阵 \\ R的行列式为\pm 1 \end{gathered}$$
• 取行列式为1的正交矩阵称为旋转矩阵
  S O ( n ) = { R ∈ R n × n ∣ R R T = I , d e t ( R ) = 1 } . S O ( n ) 是特殊正交群 SO(n)=\{ R∈R^{n×n}|RR^T=I,det(R)=1\}. \\SO(n)是特殊正交群 SO(n)={R∈Rn×n∣RRT=I,det(R)=1}.SO(n)是特殊正交群
• 旋转矩阵描述了两个坐标的变换关系：
  $$\begin{gathered} 比如：a_1=R_{12}{a}_2 \\ 反之：a_2=R_{21}{a}_1 \\ 于是：R_{21}=R_{12}^{-1}=R_{12}^T \end{gathered}$$
• 加上平移 $$a^{‘}=R_a+t$$
• $$两个坐标系的刚体运动可以由R，t完全描述$$

上述式子证明：

1.3 齐次坐标和变换矩阵

上述已经完整的表达了欧式空间的旋转和平移，但不是一个线性关系
$$b=R_{1}a+t_1，c=R_{2}b+t_2$$
但是从a到c的变换为：
$$c=R_2(R_{1}a+t_1)+t_2$$
很显然，这样叠加起来过于复杂
则改写成

$$\left[\begin{array}{c}{a}^{‘}\\ 1\end{array}\right]=\left[\begin{array}{cc}R& t\\ 0^T,& 1\end{array}\right]\left[\begin{array}{c}a\\ 1\end{array}\right]\widehat{=}T\left[\begin{array}{c}a\\ 1\end{array}\right].$$
这样多次变换就可以写成
$$\tilde{b}=T_1\tilde{a}，\tilde{c}=T_2\tilde{b}=>\tilde{c}=T_2{T}_1\tilde{a}$$

• 这种用四个数表达三维向量的做法称为其次坐标
• 引入齐次坐标后，旋转和平移可以放在同一个矩阵中，称为变换矩阵
  S E ( 3 ) = { T = [ R t 0 T , 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } SE(3)=\{ T= \begin{bmatrix} R & t \\ 0^T, & 1 \\ \end{bmatrix} ∈R^{4×4}|R∈SO(3),t∈R^3 \} SE(3)={T=[R0T,​t1​]∈R4×4∣R∈SO(3),t∈R3}
• 称为特殊欧式群
• 类似的，可以反向的变换
  $$T^{-1}=\left[\begin{array}{cc}{R}^T& -R^{t}t\\ 0^T,& 1\end{array}\right]$$
  证明：

1.3 总结

1、SO(3)的旋转矩阵有九个量。但一次旋转只有三个自由度。因此这种表达方式是冗余的。同理，变换矩阵用十六个两表示六个自由度的变换，那么，是否有更紧凑的表示呢？
2、旋转矩阵自身带有约束：它必须是个正交矩阵，且行列式为1。 变换矩阵也是如此。当我们想要估计或优化一个旋转矩阵/变换矩阵时，这些约束会使得求解变得更困难。

2. 旋转向量

• 除了旋转矩阵之外的旋转表示
• 旋转矩阵表示旋转时冗杂的（旋转矩阵有9个量，但一次旋转只有3个自由度并且旋转矩阵自身带有约束）
• $$\begin{gathered} 旋转向量：三个自由度，用R^3向量表示 \\ 方向为旋转轴，长度为转过的角度 \\ \vec{w}=\theta \vec{n} \end{gathered}$$
  
• 旋转向量和旋转矩阵的不同
• –旋转矩阵：9个量，有正交性约束和行列式值约束
• –旋转向量：三个量，没有约束
• 这两个只是表达方式不同，但是表达的东西是同一个。

转换关系
— 旋转向量旋转矩阵：罗德里格斯公式
$$R=cos\theta -(1-cos\theta )n{n}^T+sin\theta n\widehat{.}$$

— 旋转矩阵旋转向量：
$$\begin{gathered} 角度：\theta =arccos(\frac{tr(R)-1}{2}) \\ 轴：Rn=n. \end{gathered}$$
转轴是矩阵R特征值1对应的特征向量

罗德里格斯公式证明

$$R=cos\theta -(1-cos\theta )n{n}^T+sin\theta n\widehat{.}其中n\widehat{.}为反对称矩阵$$
$$\begin{gathered} 对与转角\theta ,取两边的{迹}^{II}，有： \\ tr(R)=cos\theta tr(I)-(1-cos\theta )tr(n{n}^T)+sin\theta tr(n\widehat{)}，其中tr(n\widehat{)}=0. \\ =3cos\theta +(1-cos\theta )=1+2cos\theta \\ \theta =arccos\frac{tr(R)-1}{2} \end{gathered}$$
关于转轴n，旋转轴上的向量在旋转后不发生改变，说明：$$Rn=n.$$
转轴是矩阵R特征值1对应的特征向量

总结：

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

3. 欧拉角

3.1、 欧拉角介绍

• 旋转矩阵和旋转向量是不直观的表示，欧拉角提供了一种非常直观的方式
• 欧拉角：
• –将旋转分解为三次不同轴上的移动，一边理解
• –例如按照Z-Y-X转动
• –轴顺序亦可不同，因此存在许多种定义方式不同的欧拉角。
  

偏航角(yaw)	俯仰角(pitch)	滚转角(roll)
绕z轴转动	绕y轴转动	绕x轴转动

3.2、万向锁

–在ZYX顺序种，若第二次旋转得到的为正负90°，则第三次和第一次绕同一个轴，是的系统丢失了一个自由度，存在奇异性问题

• 由于万向锁，欧拉角不适合插值和迭代，往往用于人机交互种
• 可以证明，用三个实属来表达三维旋转时，会不可避免地碰到奇异性问题。
• SLAM程序种很少直接使用欧拉角表示姿态

3.3、总结

旋转矩阵用九个量描述三个自由度的旋转，具有冗余性；欧拉角和旋转向量时紧凑的，但具有奇异性。事实上，我们找不到不带奇异性的三维向量表述方式。

4、四元数

4.1、四元数介绍

• 一种拓展的复数，即是紧凑的也没有奇异性

• 四元数有三个虚部，可以表达三维空间种的旋转
  $$q=q_0+q_{1}i+q_{2}j+q_{3}k,q=[s,v],s=q_0\in R，v=[q_1,q_2,q_3{]}^T\in R^3$$

• 虚部之间的关系：(右手法则)
  $$\{\begin{array}{l}{i}^2=j^2=k^2=-1\\ ij=k,ji=-k\\ jk=i,kj=-i\\ ki=j,ik=-j\end{array}$$

• 单位四元数满足四元数的模长为1 $$q_1^2+q_2^2+q_3^2=1$$

• （单位四元数可以表达三维空间的一次旋转）

4.2、四元数的一些运算和性质

$$q_a=s_a+x_{a}i+y_{a}j+z_{a}k,q_b=s_b+x_{b}i+y_{b}j+z_{b}k.$$

加减法：
$$q_a\pm q_b=[s_a\pm s_b,v_a\pm v_b].$$
乘法：
$$\begin{gathered} q_a{q}_b=s_a{s}_b-x_a{x}_b-y_a{y}_b-z_a{z}_b \\ +(s_a{x}_b+x_a{s}_b+y_a{z}_b-z_a{y}_b)i \\ +(s_a{y}_b-x_a{z}_b+y_a{s}_b+z_a{x}_b)j \\ +(s_a{z}_b+x_a{y}_b-y_a{x}_b-z_a{s}_b)k. \\ {q}_a{q}_b=[s_a{s}_b-v_a^T{v}_b,s_a{v}_b+s_b{v}_a+v_a\times v_b] \end{gathered}$$

共轭：$$q_a^{\ast }=s_a-x_{a}i-y_{a}j-z_{a}k=[s_a,-v_a].$$
模长：$$||q_a||=\sqrt{s_a^2+x_a^2+y_a^2+z_a^2}.$$
逆：$$q^{-1}=\frac{q^{\ast }}{||q|{|}^2}$$

数乘：$$kq=[ks,kv].$$
点乘：$$q_a\cdot q_b=s_a{s}_b+x_a{x}_{b}i+y_a{y}_{b}j+z_a{z}_{b}k$$

4.3、用四元数表示旋转

任意单位向量v，沿着一单位向量定义的旋转轴u旋转θ度之后v`可以使用四元数乘法来获得。
$$\begin{gathered} 令v=[0,v],p=[cos(\frac{\theta }{2}),sin(\frac{\theta }{2})u],那么： \\ {v}^{‘}=pv{p}^{\ast }=pv{p}^{-1} \end{gathered}$$
并证明变换之后的v实部为0，虚部是罗德利斯公式结果。
证明：

四元数和旋转向量的关系

– 旋转向量到四元数
假设某个旋转向量是绕单位向量$$\begin{gathered} \vec{n}=[n_x,n_y,n_z{]}^T进行了角度为\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 \end{gathered}$$
– ·四元数到旋转向量·
$$\{\begin{array}{l}\theta =2arccos{q}_0\\ [n_x,n_y,n_z{]}^T=\frac{[q_1,q_2,q_3{]}^T}{sin\frac{\theta }{2}}\end{array}$$
四元数和旋转矩阵的关系

4.4、总结

四元数相比旋转向量的优势：紧凑、无奇异性。

三、李群与李代数

3.1 李代数的引入

• 在SLAM种，除了表示之外，还要对他们进行估计和优化，因为SLAM整个过程就是在不断地估计机器人的位姿与地图，该位姿是由SO(3)上的旋转矩阵或SE(3)上的变换矩阵描述的。
• 为了做这件事情，需要对变换矩阵进行插值、求导、迭代等操作。

$$\begin{gathered} 设某个时刻机器人的位姿与T_{cw},它观察到了一个世界坐标位于P_w的点，产生了观测数据Z_c，根据坐标变换 \\ 任取i{Z}_c=T_{cw}{P}_{wi}+w \\ 那我们实际要做的事情是求一个欧式变换T_{cw},是的T_{cw}满足上式。 \\ ps:其中T_{cw}是指从世界坐标系\to 相机坐标系的一个转换。 \end{gathered}$$

然而，由于观测噪声w的存在，z往往不可能精确地满足Z=Tp的关系。所以我们通常会计算理想的观测与实际数据的误差：
$$e=z-Tp.$$
假设一共由N个这样的路标点和观测，则就有N个上式，则对机器人的位姿估计，相当于寻找一个最优的T，是的整体误差最小化：
$$\underset{T}{\lim }J(T)=\sum _{i=1}^N||z_i-T{p}_i|{|}_2^2$$
求解此问题，需要解决一下两个问题：
1、如何求解目标函数相对于T的导数。
2、T对于乘法封闭，对于加法不封闭。

导数定义:
$$f^{‘}(x)=\underset{△x\to 0}{\lim }\frac{f(x+△x)-f(△x)}{△x}$$
对于加减法不封闭,对于乘法封闭的例子:
$$\begin{gathered} 例如集合V=(a,0)\cup (0，b)，a，b属于全体实数， \\ （a,0)乘以任意实数都跟原来一样是一个横轴上的向量； \\ （0，b）乘以任意实数也跟原来一样是一个纵轴上的向量，故对乘法封闭， \\ 但是（a，0）+（0，b）就成了一个在坐标轴外的向量，故对加减法不封闭。 \end{gathered}$$

解决上述问题的方法: 李代数

3.2 李群 李代数

3.2.1 群

S O ( 3 ) = R ∈ R 3 × 3 ∣ R R T = I , d e t ( R ) = 1 .  特殊正交群 S O ( 1 ) S E ( 3 ) = { T = [ R t 0 T , 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 }      特殊欧式群 S E ( 3 ) SO(3)={R∈R^{3×3}|RR^T=I,det(R)=1}. \ 特殊正交群SO(1) \\ SE(3)= \{T= \begin{bmatrix} R & t \\ 0^T, & 1 \\ \end{bmatrix}∈R^{4×4}|R∈SO(3),t∈R^3\} \ \ \ \ \ 特殊欧式群SE(3) SO(3)=R∈R3×3∣RRT=I,det(R)=1. 特殊正交群SO(1)SE(3)={T=[R0T,​t1​]∈R4×4∣R∈SO(3),t∈R3}     特殊欧式群SE(3)

• 群是一种集合加上一种运算的代数结构
• 集合即为A，运算几位 · ，那么 当运算满足一下性质时，称（A，·）成群

• 1、封闭性：$$\forall a_1,a_2\in A，a_1\cdot a_2\in A.$$

• 2、结合律：$$\forall a_1,a_2,a_3\in A，(a_1\cdot a_2)\cdot a_3=a_1\cdot (a_2\cdot a_3).$$

• 3、幺元：$$\exists a_0\in A，s\cdot t.\forall a\in A，a_0\cdot a=a\cdot a_0=a.$$

• 4、逆：$$\forall a\in A，\exists a^{-1}\in A，s\cdot t.a\cdot a^{-1}=a_0.$$

• 可以验证：
• 旋转矩阵集合和矩阵乘法构成群-特殊正交群/旋转矩阵群SO(3)
• 变换矩阵集合和矩阵乘法构成群->特殊欧式群/变换矩阵群SE(3)
• 其他常见群
• $$\begin{gathered} 一般线性群GL(n)指n\times n的可逆矩阵，他们对矩阵乘法成群。 \\ 特殊正交群SO(n)也就是所谓的旋转矩阵群，其中SO(2)和SO(3)最为常见。 \\ 特殊欧式群SE(n)也就是最前面提到n维欧式变换，如SE(2)和SE(3)。 \end{gathered}$$

证明：旋转矩阵集合和旋转乘法构成群

$$\begin{gathered} 设R_1,R_2,R_3为旋转矩阵： \\ 1、R_1,R_2为旋转矩阵。(R_1{R}_2)(R_1{R}_2{)}^T=Idet(R_1{R}_2)=det(R_1)det(R_2)=1故R_1{R}_2为旋转矩阵 \\ 2、(R_1{R}_2)R_3=R_1(R_2{R}_3)由矩阵乘法，可得结论存在。 \\ 3、\exists 单位矩阵E。（单位矩阵时旋转矩阵）R_{1}E=R_1 \\ 4、\forall R为旋转矩阵。R_1^{-1}(R_1^{-1}{)}^T=R_1^{-1}(R_1^T{)}^T=R_1^{-1}{R}_1=I.det(R_1^{-1})=det(R_1^T)=det(R_1)=1 \\ 故\exists R_1^{-1}为旋转矩阵，s\cdot t.R_1{R}_1^{-1}=E \end{gathered}$$

变换矩阵和矩阵乘法构成群

$$\begin{gathered} 设T_1,T_2,T_3为变换矩阵 \\ 1、T_1,T_2为变换矩阵：T_1{T}_2=\left[\begin{array}{cc}{R}_1& t_1\\ 0^T,& 1\end{array}\right]\left[\begin{array}{cc}{R}_2& t_2\\ 0^T,& 1\end{array}\right]=\left[\begin{array}{cc}{R}_1{R}_2& R_1{t}_2+t_1\\ 0^T,& 1\end{array}\right] \\ 其中R_1{R}_2\in SO(3),R_1{t}_2+t_1\in {\mathbb{R}}^3;故T_1{T}_2\in 变换矩阵. \\ 2、(T_1{T}_2)T_3=T_1(T_2{T}_3)由矩阵表示法 \end{gathered}$$

李群

• 具有连续（光滑）性质的群
• 直观上看，一个刚体能够连续第在空间上运动，故SO(3)和SE(3)都是李群。
• 但是，SO(3)和SE(3)只有定义良好的乘法，没有加法，所以难以进行取极限，和求导等操作。

李代数

• 李代数对应李群的正切空间。它描述了李群局部的导数。
• 记作so(3)和se(3).
  

• 考虑简单的情况
  $$\begin{gathered} t_0=0,R(0)=I \\ R(t)\approx R(t_0)+\dot{R}(t_0)(t-t_0)=I+\varphi (t_0{)}^{\wedge }(t). \\ \to 1、这里{\varphi }^{\wedge }为R(t)的李代数，是李群在‘单位元‘(t_0)处的切空间 \end{gathered}$$
  -$$\begin{gathered} 在t_0附近，设\varphi 保持为常数向量\varphi (t_0)={\varphi }_0,则有微分方程 \\ \dot{R}(t)=\varphi (t_0{)}^{\wedge }R(t)={\varphi }^{\wedge }R(t) \end{gathered}$$
• 已知初始情况，解的：$$\to \{\begin{array}{l}2、R(t)与\varphi 之间的关系称为指数映射。\\ 3、这里的\varphi 成为SO(3)对应的李代数:so(3)\end{array}$$
  问题：so(3)的指定和性质这个指数映射如何求？

3.2.2 李代数的定义

每个李群都有与之对应的李代数，李代数描述了利群单位元数的正切空间性质。

李代数油由一个集合V，一个数域F和一个二元运算[,]组成。如果他们满足一下几条性质，称(V,F,[,])为一个李代数，基座g。
$$\begin{gathered} 1、密封性\forall X,Y\in V,[X,Y]\in V. \\ 2、双线性\forall X，Y，Z\in V，a,b\in F有： \\ [aX+bY,Z]=a[X,Z]+b[Y,Z],[Z,aX+bY]=a[Z,X]+b[Z,Y]. \\ 3、自反{性}^{①}\forall X\in V，[X,X]=0 \\ .4、雅可比等价\forall X,Y,Z\in V，[X,[Y,Z]]+[Z,[Y,X]]+[Y,[Z,X]]=0 \end{gathered}$$

• 二元运算被称为李括号
• 例子：三位空间向量+R^3+叉积运算构成李代数。

证明：

李群SO(3)

S O ( 3 ) = { R ∈ R 3 × 3 ∣ R R T = I , d e t ( R ) = 1 } . SO(3)=\{R∈\mathbb{R}^{3×3}|RR^T=I,det(R)=1\}. SO(3)={R∈R3×3∣RRT=I,det(R)=1}.

李代数so(3)

s o ( 3 ) = { ϕ ∈ R 3 ， Φ = ϕ ∧ ∈ R 3 × 3 } . Φ = ϕ ∧ = [ 0 − ϕ 3 ϕ 2 ϕ 3 0 − ϕ 1 − ϕ 2 ϕ 1 ϕ 0 ] ∈ R 3 × 3 so(3)= \{\phi∈\mathbb{R}^3，\Phi=\phi^\wedge ∈\mathbb{R}^{3×3} \}. \\ \Phi=\phi^{\wedge}=\begin{bmatrix} 0 & -\phi_3&\phi_2 \\ \phi_3 & 0&-\phi_1 \\ -\phi_2&\phi_1&\phi_0\\ \end{bmatrix}∈\mathbb{R}^{3×3}\\ so(3)={ϕ∈R3，Φ=ϕ∧∈R3×3}.Φ=ϕ∧= ​0ϕ3​−ϕ2​​−ϕ3​0ϕ1​​ϕ2​−ϕ1​ϕ0​​ ​∈R3×3
定义的李括号：
$$[{\varphi }_1,{\varphi }_2]=({\Phi }_1{\Phi }_2-{\Phi }_2{\Phi }_1{)}^{▽}.ps:▽与\wedge 相反$$
可以证明包括李括号满足上述的性质
也可以说so(3)的元素时三维向量或者三维反对称矩阵

综上所述，把李代数理解成向量形式或者矩阵形式都可以，向量形式更加自然一些。

证明李括号：

李群SE(3)

S E ( 3 ) = { T = [ R t 0 T , 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } .      特殊欧式群 S E ( 3 ) SE(3)= \{T= \begin{bmatrix} R & t \\ 0^T, & 1 \\ \end{bmatrix}∈\mathbb{R}^{4×4}|R∈SO(3),t∈\mathbb{R}^{3}\}. \ \ \ \ \ 特殊欧式群SE(3) SE(3)={T=[R0T,​t1​]∈R4×4∣R∈SO(3),t∈R3}.     特殊欧式群SE(3)

李代数se(3)

s e ( 3 ) = { ξ = [ ρ ϕ ] ∈ R 6 , ρ ∈ R 3 , ϕ ∈ s o ( 3 ) , ξ ∧ = [ ϕ ∧ ρ 0 T 0 ] ∈ R 4 × 4 } . se(3)=\lbrace \xi=\begin{bmatrix} \rho \\ \phi \\ \end{bmatrix}∈\mathbb{R}^6,\rho∈\mathbb{R}^3,\phi∈so(3),\xi^\wedge=\begin{bmatrix} \phi^\wedge & \rho \\ 0^T & 0\\ \end{bmatrix}∈\mathbb{R}^{4×4} \rbrace. se(3)={ξ=[ρϕ​]∈R6,ρ∈R3,ϕ∈so(3),ξ∧=[ϕ∧0T​ρ0​]∈R4×4}.

se(3)由三个平移分量和三个旋转分量组成（六维向量）
— — 旋转与so(3)相同
— — 平移时普通的向量，不是SE(3)上的平移分量

定义的李括号
$$\begin{gathered} [{\xi }_1,{\xi }_2]=({\xi }_1^{\wedge }{\xi }_2^{\wedge }-{\xi }_2^{\wedge }{\xi }_1^{\wedge }{)}^{▽}. \\ 可以证明该李括号满足上述的性质。 \end{gathered}$$
在se(3)的表示中，^不再时反对称矩阵，但仍保留记法。

3.3 指数与对数映射

3.3.1 SO(3)上的指数映射

指数映射反映了从李代数到李群的对应关系
$$R=exp({\varphi }^{\wedge })$$
– φ^是一个矩阵，任意矩阵的指数映射可以写成一个泰勒展开
$$exp({\varphi }^{\wedge })=\sum _{n=0}^{\infty }\frac{1}{n!}({\varphi }^{\wedge }{)}^n.$$
– 由于φ是一个向量，可以定义模长（θ）和单位方向向量a —旋转向量
$$\varphi =\theta a$$
– 关于a，可以验证一下性质:
$$\{\begin{array}{l}{a}^{\wedge }{a}^{\wedge }=a{a}^T-I,\\ a^{\wedge }{a}^{\wedge }{a}^{\wedge }=-a^{\wedge }\end{array}\Leftarrow 这为化解Taylor展开中的高阶项提供了有效方法。$$
证明：

SO(3)上的指数映射

so(3)->SO(3)

Taylor展开:

$$exp(\varphi )=exp(\varphi )=\sum _{n=0}^{\infty }\frac{1}{n!}{\theta a^{\wedge }}^n$$

最后得到一个似曾相识的结果:

$$exp(\theta a^{\wedge })=\cos \theta I+(1-cos\theta )a{a}^T+sin\theta a^{\wedge }.$$

罗德里格斯公式:

$$R=\cos \theta I+(1-cos\theta )n{n}^T+sin\theta n^{\wedge }.$$

sinx和cosx的泰勒展开式:

1. so(3)的物理意义就是旋转向量,即so(3)的李代数空间就是由旋转向量组成的空间.
2. 如果李群(旋转矩阵,R(t),类似一个函数)代表一个球面,那么球上所有点的切线(单位元处李群的切空间李代数,旋转向量),也会组成一个球面,这样这个球面和原来的球面一样
   $$\begin{gathered} a^{\wedge }{a}^{\wedge }=a{a}^T-I, \\ {a}^{\wedge }{a}^{\wedge }{a}^{\wedge }=-a^{\wedge } \end{gathered}$$

---

SO(3)->so(3)
反之,给定旋转矩阵时,亦能求李代数(把SO(3)中的元素对应到so(3))
$$\varphi =ln(R{)}^{\surd }=(\sum _{n=0}^{\infty }\frac{(-1{)}^n}{n+1}(R-I{)}^{n+1}{)}^{\surd }$$
但在实际当中没有必要这样求,之前已经介绍了旋转矩阵到旋转向量的转换关系了:
$$\begin{gathered} \theta =arccos(\frac{tr(R)-1}{2}). \\ Rn=n. \end{gathered}$$

SE(3)上的指数映射

se(3)->SE(3)

$$\begin{gathered} exp({\xi }^{\wedge })=\left[\begin{array}{cc}{\sum }_{n=0}^{\infty }\frac{1}{n!}({\varphi }^{\wedge }{)}^n& {\sum }_{n=0}^{\infty }\frac{1}{(n+1)!}({\varphi }^{\wedge }{)}^n\rho \\ 0^T& 1\end{array}\right] \\ {\xi }^{\wedge }:这里不是反对称矩阵 \\ \triangleq \left[\begin{array}{cc}R& J_{\rho }\\ 0^T& 1\end{array}\right]=T \end{gathered}$$
R就是我们熟知的SO(3)上的元素
J可以理解成李代数的平移部分到李群的平移部分差一个线性变换(雅可比矩阵)
$$J=\frac{sin\theta }{\theta }I+(1-\frac{sin\theta }{\theta })a{a}^T+\frac{1-cos\theta }{\theta }{a}^{\wedge }.$$
SE(3)->se(3)
可以类比推得其对数映射
但是根据变换矩阵T求se(3)上的对应向量也有更简单的方法:
$$\begin{gathered} 从R计算从旋转向量\varphi \\ 由t=Jp(J可由旋转向量\varphi 得到,\rho 可通过求解线性方程求出.) \end{gathered}$$

3.3 李代数求导与扰动模型

3.3.1 BCH公式

• 在估计和优化过程中,求导是非常必要的.
• 但是李群元素只有乘法,无法定义导数$$(R_1+R_2\notin SO(3))$$
• 已知SO(3)和SE(3)上的李群和李代数关系,但是当SO(3)中完成两个矩阵乘法时,李代数so(3)上发生什么变化?换言之,当so(3)上做两个李代数相加时,SO(3)上是否对应着两个矩阵的乘积?
• $$exp({\varphi }_1^{\wedge })exp({\varphi }_2^{\wedge })=exp(({\varphi }_1+\varphi 2{)}^{\wedge })?$$
• 矩阵的指数函数
• $$ln(exp(A)exp(B))=A+B?$$
• 两个李代数指数映射乘积的完整形式,由BCH公式给出,由于完整形式复杂,只给出展开的前几项:
• $$ln(exp(A)exp(b))=A+B+\frac{1}{2}[A,B]+\frac{1}{12}[A,[A,B]]-\frac{1}{12}[B,[A,B]]+......$$
  当处理两个矩阵指数乘积之时,会产生一些由李括号组成的余项.

当其中一个量很小时,忽略其高阶项,BCH具有线性浸塑形式
$$ln(exp({\varphi }_1^{\wedge })exp({\varphi }_2^{\wedge }){)}^{\surd }\approx \{\begin{array}{ll}{J}_l({\varphi }_2{)}^{-1}{\varphi }_1+{\varphi }_2& if{\varphi }_{1}issmall,\\ J_r({\varphi }_1{)}^{-1}{\varphi }_2+{\varphi }_1& if{\varphi }_{2}issmall.\end{array}\varphi =\theta a$$

$$理解一下这个式子：当对一个旋转矩阵R_2(李代数为{\varphi }_2)左乘一个微小旋转矩阵R_1（李代数为{\varphi }_1）时，可以近似看作，在原有的李代数{\varphi }_2上，加上了一项J_l({\varphi }_2{)}^{-1}{\varphi }_1$$

其中：$$\begin{gathered} J_l=J\frac{sin\theta }{\theta }I+(1-\frac{sin\theta }{\theta })a{a}^T+\frac{1-cos\theta }{\theta }{a}^{\wedge } \\ {J}_l^{-1}=\frac{\theta }{2}cot\frac{\theta }{2}I+(1-\frac{\theta }{2}cot\frac{\theta }{2})a{a}^T-\frac{\theta }{2}{a}^{\wedge } \\ {J}_r(\varphi )=J_l(-\varphi ) \end{gathered}$$

-so(3)上的BCH近似

$$\begin{gathered} (1)某个旋转R，对应的李代数为\varphi .给它左乘一个微小旋转，记作\Delta R,则对应李代数为\Delta \varphi .\to \Delta R=\Delta \varphi \\ 在李群上，得到的为\Delta R\cdot R，而在李代数上，根据BCH近似，作为J_l^{-1}(\varphi )\Delta \varphi +\varphi . \\ exp(\Delta {\varphi }^{\wedge })exp({\varphi }^{\wedge })=exp((\varphi +J_l^{-1}(\varphi )\Delta \varphi {)}^{\wedge }) \\ 在李群上左乘小量时，李代数上的加法相差左雅可比的逆 \end{gathered}$$
(2)反之
$$\begin{gathered} exp((\varphi +\Delta \varphi {)}^{\wedge })exp((J_l\Delta \varphi {)}^{\wedge })exp({\varphi }^{\wedge })=exp((J_r\Delta \varphi {)}^{\wedge }). \\ 李代数上进行小量加法时，李代数上乘法相差左（右）雅可比 \end{gathered}$$

$$\begin{gathered} exp((\varphi +\Delta \varphi )\wedge )=exp((\varphi +J_l^{-1}(\varphi )J_l(\varphi )\Delta \varphi {)}^{\wedge }) \\ =exp((J_l(\varphi )\Delta \varphi {)}^{\wedge })exp({\varphi }^{\wedge }). \end{gathered}$$

se(3)上BCH近似

$$\begin{gathered} exp(\Delta {\xi }^{\wedge })exp({\xi }^{\wedge })\approx exp((J_l^{-1}\Delta \xi +\xi {)}^{\wedge }), \\ exp({\xi }^{\wedge })exp(\Delta {\xi }^{\wedge })\approx exp((J_r^{-1}\Delta \xi +\xi {)}^{\wedge }). \\ ps：其中：(J_l^{-1}\Delta \xi +\xi {)}^{\wedge }形势复杂，一个6\times 6的矩阵。 \end{gathered}$$

背景知识：
在SLAM中，我们要估计一个相机的位置和姿态，该位置是由SO(3)上的旋转矩阵或SE(3)上的变换矩阵描述的。
设某个时刻机器人的位姿为T，它观察到了一个世界坐标位于P的点，产生了一个观测数据Z，根据坐标变换关系Z=TP+W可知，由于噪声的存在，Z往往不可能精确地满足Z=TP这个关系，所以我们通常会计算理想的观测与实际数据的误差。
假设一共由N个这样的路标点和观测，则就有N个上式，则对于机器人的位姿估计，相当于寻找一个最优的T，使得整体误差最小化：
$$\underset{T}{\min }J(T)=\sum _{i=1}^N||z_i-T{p}_i|{|}_2^2.$$
求解此问题，需要计算目标函数J对于变换矩阵T的导数。需要注意的是，我们经常会构建与位姿有关的函数，然后讨论该函数对于位姿的导数，从而调整当前的估计值。

使用李代数解决求导问题的思路分为两种：
（1）、用李代数表示位姿，然后根据李代数加法对李代数求导。
（2）、对李群左乘或右乘微小扰动，然后对该扰动求导。

3.3.1 导数模型

• 假设对一个空间点P进行了旋转，得到了RP. 现在要计算旋转之后点的坐标相对于旋转的导数。
• $$\frac{\delta (Rp)}{\delta R}.不严谨的记号$$
• 由于矩阵没有加法，导数无法定义，则我们设R对应的李代数为φ，则转而计算：
• $$\frac{\delta (exp{\varphi }^{\wedge })p}{\delta \varphi }$$
• 
  $$上面内容中的\phi =\varphi$$

3.3.2 扰动模型

左扰动模型

设左扰动ΔR对应的李代数为φ，然后对φ求导
$$\begin{gathered} \frac{\delta (Rp)}{\delta \phi }=\underset{\phi \to 0}{\lim }\frac{exp({\phi }^{\wedge })exp({\varphi }^{\wedge })p-exp({\varphi }^{\wedge })p}{\phi }泰勒展开舍去高阶项近似. \\ =\underset{\phi \to 0}{\lim }\frac{(1+\phi )exp({\varphi }^{\wedge })p-exp({\varphi }^{\wedge })p}{\phi } \\ =\underset{\phi \to 0}{\lim }\frac{{\phi }^{\wedge }Rp}{\phi }=\underset{\phi \to 0}{\lim }\frac{-(Rp{)}^{\wedge }\phi }{\phi }=-(Rp{)}^{\wedge }. \\ \wedge 反对称符号看作叉积，交换之后变号。 \end{gathered}$$

1、对李群左乘或者右乘微小扰动，然后对这个扰动求导，即把增量的扰动添加在李群上，然后利用李代数表示此扰动。
2、把增量直接定义在李群上需要注意：传统上我们通常用加法表示增量，而李群对加法不封闭。所以这里的增量不再用加法表示，而是乘法。
3、乘法：增量指的是，在原来的基础上改变一点点。当对旋转矩阵做乘法，乘以的是一个趋近于单位矩阵，也就是差不多没旋转，这样就是对其”加了“一个小量。
4、单位矩阵的李代数为0
5、与代数模型相比，省去了一个雅可比的计算，更为实用。

右扰动模型

设左扰动ΔR对应的李代数为φ，然后对R求导
$$\begin{gathered} \frac{\delta (Rp)}{\delta R}=\frac{\delta (exp({\varphi }^{\wedge })p)}{\delta (exp({\varphi }^{\wedge }))} \\ =\underset{\phi \to 0}{\lim }\frac{exp({\varphi }^{\wedge })exp({\phi }^{\wedge })p-exp({\varphi }^{\wedge })p}{\phi }泰勒展开舍去高阶项近似. \\ =\underset{\phi \to 0}{\lim }\frac{exp({\varphi }^{\wedge })(I+\phi \wedge )p-exp({\varphi }^{\wedge })p}{\phi } \\ =\underset{\phi \to 0}{\lim }\frac{exp({\varphi }^{\wedge }){\phi }^{\wedge }p}{\phi }=\underset{\phi \to 0}{\lim }\frac{-exp({\varphi }^{\wedge })p^{\wedge }\phi }{\phi }=-exp({\varphi }^{\wedge })p^{\wedge }=-R{p}^{\wedge }. \end{gathered}$$

SE(3)李代数上的扰动模型

某空间点p经过一次变换T（对应李代数为ξ）,得到Tp^①。现在，给T左乘一个扰动：ΔT=exp(δξ^∧),我们设扰动项的李代数为δξ=[δρ,δϕ]^T,那么：

3.3.3 导数模型与扰动模型的对

1、共同点：用李代数表示姿态，然后根据李代数的加法来对李代数求导
2、传统导数方法：把增量直接定义在李代数上；扰动模型把增量直接定义到李群上。
3、扰动模型：对李群左乘或者右乘微小扰动给，然后对这个扰动模型求导，即把增量的扰动添加在李群上，然后利用李代数表示此扰动。
4、与导数模型相比，省去了一个雅可比计算，更为实用。

3.4 总结

1、在SLAM中，除了刚体运动之外，还要对他们进行估计和优化，因为SLAM整个过程就是在不断地估计机器人的位姿与地图。为了做这件事，需要对变换矩阵进行差值、求导、迭代等操作。
2、问题：旋转矩阵、变换矩阵（李群）对加法不封闭。
3、引入李代数。
4、对于某个时刻的R(t)(李群空间)，存在一个三维向量φ（李代数），用来描述R在t时刻的局部导数。他们之间通过指数和对数映射进行交换。
5、直观的想法：能否利用李代数上的加法，定义李群元素的导数？
$$\begin{gathered} 6、ln(exp(A)exp(B))=A+B? \\ ln(exp(A)exp(B))=A+B+\frac{1}{2}[A,B]+\frac{1}{12}[A,[A,B]]-12[B,[A,B]]+... \end{gathered}$$
7、使用李代数解决导数问题的思路分为两种:
用李代数表示姿态，然后根据李代数加法来对李代数求导。
对李群左乘或右乘微小扰动，然后对该扰动求导。