SLAM笔记

写在前面

• 卡尔曼滤波前面的内容是2020.7.21日完成的，此后我将再次基础上继续更新相关知识
  Update: 2021.07.16

---

SLAM背景

随着机器人技术的发展，服务型室内机器人与移动机器人获得广泛关注
室内机器人普及存在许多亟待解决的问题，定于与导航就是去中关键问题之一
解决这个问题需要把握三个重点

• 地图精确建模
• 机器人精准定位
• 路径实时规划

室外定位与导航可以使用GPS,但在室内这个问题就变得比较复杂

• GPS定位效果差
• IMU惯性传感器存在误差累积
  一批技术不断涌现，SLAM就在其中脱颖而出

SLAM简介

SLAM = Simultaneous Localization and Mapping，即时定位与地图构建
SLAM问题提出在1990年由Randall Smitht, Matthew SelF和Peter Cheeseman提出

• 定位？机器人感知环境
• 无人车等级划分？L1 - L5，L4等级需要对地图进行构建
• 地图绘制？稀疏制图？稠密制图？
  稀疏制图表示你对周围的环境只有部分的了解，而稠密建图则表示你对周围的环境的每一个点都清楚

SLAM可以描述为：
“机器人在未知环境中，从一个未知开始移动，移动过程中根据位置估计和地图进行自身定位，同时建造增量式地图，实现机器人的自主定位和导航”

• 感知 - 传感器
• 无人车等级划分？L1 - L5
• 地图绘制？稀疏制图？稠密制图？

传感器

• 激光雷达
• 摄像头
• RGB-D摄像头

激光雷达

激光雷达是研究最多、使用最成熟的深度传感器，可以提供机器人本体与环境障碍物之间的距离信息
优点：

• 精度高
• 响应快
• 数据量小
• 可以完成实时SLAM任务
  缺点：
• 成本高
  

摄像头

SLAM摄像头分为单目摄像头和双目摄像头
优点：

• 适用性强
• 传感器简单
  缺点：
• 复杂度高
• 单目摄像头在静止时无法测量距离，双目摄像头可以计算距离，但是运算量较大
  

RGB-D摄像头

RGB-D摄像头是近年来兴起的一种新型传感器，不仅可以获得环境的RGB图像信息，也可以通过红外结构光、Time-of-Flight等原理获取每个元素的深度信息。
丰富的数据可以让RBG-D摄像头不仅用于SLAM，还可以用于图像处理，物品识别等
同时RGB-D摄像头成本低，也是室内服务机器人主流传感器方案

缺点：

• 视野窄
• 盲区大
• 噪声大
  

SLAM理论

SLAM一般过程

当机器人运动时，其位置将会发生变化。此时，根据机器人位置传感器的观测，提取得到观测信息中的特征点，然后机器人通过EKF将目前观测到特征点的位置、机器人运动距离、机器人运动前观测到特征点的位置相互结合，对机器人当前位置和当前环境信息进行估计。
初始值 -> 预测值 -> 测量值 -> 估计值

地标是环境中易于观测和区分的特征。一般使用这些特征确定机器人位置。我们可以通过下面的方法想象上述工作过程，假设在一个陌生的房间内，闭上眼睛，那么此时我们如何确定自身的位置呢？通常而言，我们将在环境中不断走动，通过触摸物体或者墙壁确定自身位置。上述如被触摸的物体以及墙壁等都可以被看做用于估计自身位置的地标。下面是一些典型的地标。
可以看出，通常而言，对于不同的环境，一般我们选择不同的地标。
一般而言，地标需要满足下面的条件：

1. 地标应该可以从不同的位置和角度观察得到；
2. 地标应该是独一无二的，从而可以很容易的将底边从其他物体中分辨出来
3. 地标不应该过少，从而导致机器人需要花费额外的代价寻找地标；
4. 地标应该是静止的，因而，我们最好不要使用一个人作为地标

地标特征提取 Spike方法和RANSAC方法

数据关联
数据关联是将不同时刻位置传感器提取到的地标信息进行关联的过程，也成为重观察过程。

在实际应用中进行数据关联时，我们可能遇到下面的问题：

1. 我们可能上一次看到了某个地标，但下一次却没有看到；
2. 我们可能这次看到了地标，但之后却再也看不到这个地标；
3. 我们可能错误的将现在看到的某个地标与之前看到的某个地标进行关联；

根据我们选择路标时的标准，我们可以很容易的排除上面第1和第2个问题。但对于第三个问题，如果发生了，将会对我们的导航以及地图绘制造成严重的问题。
现在我们将讨论解决上面第三个问题的方法。假设我们现在已经到了每时每刻采集处理得到的路标的方位信息，并将其其中的特征存储在一数据库中。数据库初始阶段是空的，首先我们建立的第一条规则是，除非该特征已经出现了N次，否则我们并不将其加入数据库。当得到一副新的传感器信息后，我们进行下面的计算：

1. 得到一副新的传感器信息后，首先利用上面的特征提取方法提取特征；
2. 将提取到的特征与数据库中已经出现N次的并且距离最近的特征关联起来；
3. 通过验证环节验证上面的关联过程是正确的，如果验证通过，则表明我们再次看到了某个物体，因而其出现次数+1，否则表明我们看到了一个新的特征，在数据库中新建一个特征，并将其记作1.

卡尔曼滤波器

《A New Approach to Linear Filtering and Prediction Problems》 卡尔曼，1960
卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如人脸识别，图像分割，图像边缘检测等等。

基础知识

高斯分布
$\mathcal{N}(x,\mu ,\sigma )=\frac{1}{\sigma \sqrt{2\pi }}{e}^{\frac{-(x-\mu {)}^2}{2{\sigma }^2}}$

设${\mathcal{N}}_1(x,{\mu }_1,{\sigma }_1),{\mathcal{N}}_2(x,{\mu }_2,{\sigma }_2)$

$$\begin{array}{rl}{\mathcal{N}}_1{\mathcal{N}}_2& =e^{\frac{-(x-{\mu }_1{)}^2}{2{\sigma }_1^2}}{e}^{\frac{-(x-{\mu }_2{)}^2}{2{\sigma }_2^2}}{e}^C\\ & =e^{\frac{-{\sigma }_2^2(x-{\mu }_1{)}^2-{\sigma }_1^2(x-{\mu }_2{)}^2}{2{\sigma }_1^2{\sigma }_2^2}+C}\\ & =e^{\frac{-[({\sigma }_1^2+{\sigma }_2^2)x^2-2({\sigma }_2^2{\mu }_1+{\sigma }_1^2{\mu }_2)x]}{2{\sigma }_1^2{\sigma }_2^2}+C^{\prime }}\end{array}$$

配方可得 $\mu =\frac{{\mu }_1{\sigma }_2^2+{\mu }_2{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2},\sigma =\frac{{\sigma }_1^2{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}$
令 $k=\frac{{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2}$,
则 $\mu ={\mu }_1+k({\mu }_2-{\mu }_1),{\sigma }^2=k{\sigma }_2^2=(1-k){\sigma }_1^2$

若有多个变量，则用协方差矩阵
$K={\mathrm{\Sigma }}_1({\mathrm{\Sigma }}_1+{\mathrm{\Sigma }}_2),\overrightarrow{\mu }=\overrightarrow{{\mu }_1}+K(\overrightarrow{{\mu }_2}-\overrightarrow{{\mu }_1}),\mathrm{\Sigma }=K{\mathrm{\Sigma }}_2=(I-K){\mathrm{\Sigma }}_1$

矩阵的迹

$$tr(A)=\sum _{i=1}^n{A}_{ii}$$

矩阵微分方程

$$\frac{\partial tr(AB)}{\partial A}=B^T$$

若$B$是对称矩阵，则

$$\frac{\partial tr(AB{A}^T)}{\partial A}=2AB$$

核心公式

预测过程

$$\begin{gathered} x^{\prime }=Fx+u \\ {P}^{\prime }=FP{F}^T+Q \end{gathered}$$

参数调整

$$\begin{gathered} y=z-H{x}^{\prime } \\ S=H{P}^{\prime }{H}^T+R \\ K=P^{\prime }{H}^T{S}^{-1} \\ x=x^{\prime }+Ky \\ P=(I-KH)P^{\prime } \end{gathered}$$

$x$ - 机器人状态
$F$ - 状态转移矩阵
$P$ - 协方差矩阵，表示系统不确定度
$H$ - 测量矩阵/观测矩阵，用于运算统一维度，Jacobian矩阵
$K$ - 卡尔曼增量/加权矩阵，在原状态和预测状态分布之间的比值
$Q$ - 过程噪声协方差矩阵(上坡)(w)
$R$ - 测量噪声协方差矩阵(v)
$z$ - 实际观测值
$y$ - 测量值与预测值之间的差值
$S$ - 信息协方差

数学推导

设$x_r$ 为真实值，但并不知道，$x$为估计值，$x^{\prime }$为预测值

真实值与预测值之间的误差

$$e^{\prime }=x_r-x^{\prime }$$

真实值与估计值之间的误差

$$\begin{array}{rl}e& =x_r-x\\ & =x_r-(x^{\prime }+K(z-H{x}^{\prime }))\\ & =x_r-(x^{\prime }+K(H{x}_r+v-H{x}^{\prime }))\\ & =(I-KH)(x_r-x^{\prime })-Kv,z=H{x}_r+v\end{array}$$

预测值误差的协方差

$$\begin{array}{rl}{P}^{\prime }& =E[e^{\prime }{e}^{\prime T}]\\ & =E[(x_r-x^{\prime })(x_r-x^{\prime }{)}^T]\end{array}$$

估计值误差的协方差

$$\begin{array}{rl}P& =E[e{e}^T]\\ & =E[[(I-KH)(x_r-x^{\prime })-Kv][(I-KH)(x_r-x^{\prime })-Kv{]}^T]\\ & =(I-KH)E[(x_r-x^{\prime })(x_r-x^{\prime }{)}^T](I-KH{)}^T+KE[v{v}^T]K^T\end{array}$$

其中，$E[v]=0,E[v{v}^T]=R$
最后得到

$$P=(I-KH)P^{\prime }(I-KH{)}^T+KR{K}^T$$

让方差尽量小，位置约确定，方差为$tr(P)$，自变量是权值矩阵$K$，对$K$求偏导，令其为0
求导之前，先让我们化简一下$tr(P)$
先用$P^{\prime }$来表示$P$，因为这里面有$K$，可以将不相关的变量消掉

$$\begin{array}{rl}P& =(I-KH)P^{\prime }(I-KH{)}^T+KR{K}^T\\ & =(P^{\prime }-KH{P}^{\prime })-(P^{\prime }-KH{P}^{\prime }{H}^T{K}^T)+KR{K}^T\\ & =P^{\prime }-KH{P}^{\prime }-P^{\prime }{H}^T{K}^T+K(H{P}^{\prime }{H}^T+R)K^T\end{array}$$

所以

$$tr(P)=tr(P^{\prime })-2tr(KH{P}^{\prime })+tr(K(H{P}^{\prime }{H}^T+R)K^T)$$

利用基础知识中矩阵迹的微分方程，求偏导

$$\frac{\partial tr(P)}{\partial x}=0-2(H{P}^{\prime }{)}^T+2K(H{P}^{\prime }{H}^T+R)$$

令其为0，可以得到

$$K=\frac{P^{\prime }{H}^T}{H{P}^{\prime }{H}^T+R}$$

更新$P^{\prime }$，怎么做呢？我们想让我们的预测接近我们的估计值，那么这一时刻的估计误差=上一时刻的预测误差，即$x_{r,t}-x_t^{\prime }=F{x}_{r,t-1}+w-F{x}_{t-1}$

$$\begin{array}{rl}{P}^{\prime }& =E[(x_r-x^{\prime })(x_r-x^{\prime }{)}^T]\\ & =E[(F{x}_r+w-Fx)(F{x}_r+w-Fx{)}^T]\\ & =E[(Fe+w)(Fe+w{)}^T]\\ & =FE[e{e}^T]F+E[w{w}^T]\\ & =FP{F}^T+Q\end{array}$$

至此，所有公式都推导完成了-，-

扩展卡尔曼滤波器

卡尔曼滤波器适用于高斯分布的误差，但实际误差可能并不是高斯分布，甚至不是线性的
扩展卡尔曼滤波器利用First Order Taylor Expansion，也就是一阶泰勒展开，将概率函数线性化，完成和卡尔曼滤波器一样的效果

Reference