滤波算法与SLAM：预备知识及SLAM基础八股文

滤波算法与SLAM

第一章：预备知识（均来自视觉SLAM十四讲的三、四讲）
本系列并不面向0基础SLAM小白，默认读者拥有一定的slam和机器人开发经验
至少跑过了基础开源代码如FAST_LIO或ORB-SLAM，理解坐标系变换等基本知识

---

---

前言

本章内容包含SLAM基础数学知识，包括旋转矩阵、李群、李代数等
如需要进一步公式推导或更细致的内容，推荐阅读《视觉SLAM十四讲》第三讲和第四讲

---

一、状态方程与观测方程

定义机器人在$k$时刻的状态为$x_k$，输入为$u_k$，状态噪声为${\omega }_k$，则状态传递满足：
$$x_{k+1}=f(x_k,u_k,{\omega }_k)$$设机器人在$k$时刻观测到环境中第$j$个路标$y$，观测噪声为${\nu }_{k,j}$，则观测方程定义为：$$z_{k,j}=h(y_j,x_k,{\nu }_{k,j})$$这里的路标在代码中的表现形式取决于传感器的信息维度。如果是相机，则$y_j$实际上代表一个特征点的坐标，如果是雷达，则是三维点。不管如何，其代表的意义都是：真实世界中某一点在传感器坐标系中的投影。

二、齐次坐标

定义三维点$p=[x,y,z{]}^T\in {\mathbb{R}}^3$，则其具有唯一的齐次坐标$\tilde{p}=[x,y,z,1{]}^T\in {\mathbb{R}}^4$

三、刚体变换、相似变换

变换矩阵一般记为$T$，按照《视觉SLAM十四讲》的习惯，记$T_{21}$为坐标系1到坐标系2的变换矩阵。
例如：在通用的场景下，设$\widetilde{p_1},\widetilde{p_2}$两个点，则有$\widetilde{p_2}=T_{21}\widetilde{p_2}$
在多传感器融合的场景下，$T_{LC}$也可代表相机到雷达的外参矩阵
在先验地图定位的场景下，$T_{k0}$也可代表k时刻机器人在地图中的位姿，虽然一般这种情况论文中会记为$T_0^k$
具体地，$T$一般可以写为
$$T=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]\in SE(3)$$
其中$R,t$分别代表旋转矩阵和平移向量，满足$R\in SO(3),t\in {\mathbb{R}}^3$
则对于$\widetilde{p_2}=T_{21}\widetilde{p_2}$也可以写为$p_2=R{p}_1+t$
相似变换一般记为$S$，写为
$$S=\left[\begin{array}{cc}sR& t\\ 0& 1\end{array}\right]\in Sim(3)$$
在单目视觉SLAM中会常用到，这里不展开

四、升维&降维符号

这里遵循《视觉SLAM十四讲》中对"$\wedge$"符号的定义，对向量$a=[a_1,a_2,a_3{]}^T$有
$$a^{\wedge }=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]=A$$
同理的，对“$\vee$”满足：$A^{\vee }=a$
这一变换为了方便记住，本文称"$\wedge$"为升维，将$3\ast 1$的向量升维到$3\ast 3$矩阵，“$\vee$”则为降维符号

五、其他的旋转表示方法

1）欧拉角：用XYZ三轴上的旋转角度表示一次旋转，roll-pitch-yaw分别对应X-Y-Z轴上的旋转，存在万向锁的问题，一般不用；
2）旋转向量：用一个单位向量$\vec{n}$和一个旋转角$\theta$表示一次旋转，使用罗德里格旋转公式和旋转矩阵转换：
$$\begin{array}{c}R=cos\theta \cdot I+(1-cos\theta )\vec{n}\cdot {\vec{n}}^T+sin\theta \cdot {\vec{n}}^{\wedge }\\ \\ \theta =arccos\frac{tr(R)-1}{2}\end{array}$$
3）四元数：一般记为$q=[w,x,y,z{]}^T$，代表一次旋转，亦记为$q=[s,\vec{v}{]}^T$。设$q_{21}$代表$R_{21}$对应的四元数，那么$p_2,p_1$满足：
$$p_2=R_{21}{p}_1=q_{21}{p}_1{q}_{21}^{-1}$$
由四元数计算旋转矩阵通过以下公式：
$$R=\vec{v}{\vec{v}}^T+s^2\cdot I+2s\cdot {\vec{v}}^{\wedge }+({\vec{v}}^{\wedge }{)}^2$$

六、李群&李代数

终于到这里了，最麻烦的内容，简单过一下面试能答点就行
李群的定义可以简单理解为：只对一种运算封闭的集合，叫做李群（但是作为普通SLAM玩家不需要管这些）
对于旋转矩阵$R$，属于李群$SO(3)$，满足：$R_1{R}_2\in SO(3)$，但是$R_1+R_2\notin SO(3)$，同理变换矩阵$T$属于李群$SE(3)$
对于旋转矩阵，定义李代数$\varphi \in {\mathbb{R}}^3$，那么有$$R=\exp ({\varphi }^{\wedge })$$这一公式也叫指数变换：已知李代数，求李群
同理有对数变换：已知李群，求李代数
理解起来可能有点乱，代入正常的指数函数就好理解了，知道x，求exp(x)就是对x进行指数变换
就像已知李代数，要对李代数进行指数变换才能得到李群，即：李群=exp(李代数)
李代数的表示类似李群，即：$R\in SO(3),\varphi \in so(3)$，$T\in SE(3),\xi \in se(3)$
李括号用于表示两个李代数的差异性，如果是自身与自身的李括号，则为0（因为没有差别嘛）

七、BCH近似

引入李代数和李群，一个原因是旋转矩阵不满足加减法，只能乘除，李代数则有加减法
如果是简单的指数函数$e^x$，$xy=e^{x+y}$是显而易见的
但是对旋转矩阵是否也满足这个，答案是否定的，所以引入BCH近似，用于表示李群乘法在李代数上的表现
设旋转矩阵$R$对应李代数$\varphi$，增加一旋转$\Delta R$，对应李代数$\Delta \varphi$
那么对于旋转$\Delta R\cdot R$，设其对应的李代数为${\varphi }_{gt}$
则用BCH近似可得$${\varphi }_{gt}=J_l^{-1}(\varphi )\Delta \varphi +\varphi$$这里的雅可比矩阵与指数映射过程有关

八、李代数求导

设$R$是对点$p$的一次旋转，得到结果$Rp$，求其对旋转的偏导数，而旋转矩阵并无加减法计算，无法求导，所以转为对李代数求导
回想导数的定义：$$\frac{df(x+\Delta x)}{d(\Delta x)}=\underset{\Delta x\to 0}{\lim }\frac{f(x+\Delta x)-f(x)}{\Delta x}$$
而《视觉SLAM十四讲》中提到的左扰动模型，是对旋转左乘一个微小扰动$\Delta R$，对应的李代数为$\Delta \varphi$，那么有$$\frac{\partial Rp}{\partial \Delta \varphi }=\underset{\Delta \varphi \to 0}{\lim }\frac{\Delta R\cdot Rp-\Delta Rp}{\Delta \varphi }=\underset{\Delta \varphi \to 0}{\lim }\frac{exp(\Delta {\varphi }^{\wedge })exp({\varphi }^{\wedge })p-exp(\Delta {\varphi }^{\wedge })p}{\Delta \varphi }$$计算过程不复杂，最后结果为$$\frac{\partial Rp}{\partial \Delta \varphi }=-(Rp{)}^{\wedge }$$
相比直接求导省去一个雅可比矩阵的计算，同样的，对变换矩阵也有：
$$\frac{\partial Tp}{\partial \Delta \xi }=\left[\begin{array}{cc}I& -(Rp+t{)}^{\wedge }\\ 0^T& 0^T\end{array}\right]$$

---