第 2 讲初识 SLAM

最新推荐文章于 2024-10-05 19:43:08 发布

howtoloveyou

最新推荐文章于 2024-10-05 19:43:08 发布

阅读量459

点赞数

分类专栏：《视觉SLAM十四讲》文章标签：自动驾驶

本文链接：https://blog.csdn.net/qq_38758049/article/details/121169455

版权

《视觉SLAM十四讲》专栏收录该内容

3 篇文章 0 订阅

订阅专栏

光荣在于平淡，艰巨在于漫长

文章目录

第 1 讲预备知识
- 1.1 本书讲什么
- 1.2 代码
第 2 讲初识 SLAM

第 1 讲预备知识

1.1 本书讲什么

SLAM 是 Simultaneous Localization and Mapping 的缩写，中文译作“同时定位与地图构建”。它是指搭载特定传感器的主体，在没有环境先验信息的情况下，于运动过程中建立环境的模型，同时估计自己的运动。如果这里的传感器主要为相机，那就称为“视觉 SLAM”。

1.2 代码

本书所有源代码均托管在 GitHub 上

第 2 讲初识 SLAM

2.1 引子：小萝卜的例子

传感器：利用二维码进行定位的增强现实软件；GPS 定位装置；铺设导轨的小车；激光雷达；IMU 单元；双目相机

2.2 经典视觉 SLAM 框架

在这里插入图片描述
整个视觉 SLAM 流程包括以下步骤：

传感器信息读取。在视觉 SLAM 中主要为相机图像信息的读取和预处理。如果是在机器人中，还可能有码盘、惯性传感器等信息的读取和同步。
视觉里程计（Visual Odometry，VO）。视觉里程计的任务是估算相邻图像间相机的运动，以及局部地图的样子。VO 又称为前端（Front End）。
后端优化（Optimization）。后端接受不同时刻视觉里程计测量的相机位姿，以及回环检测的信息，对它们进行优化，得到全局一致的轨迹和地图。由于接在 VO 之后，又称为后端（Back End）。
回环检测（Loop Closing）。回环检测判断机器人是否到达过先前的位置。如果检测到回环，它会把信息提供给后端进行处理。
建图（Mapping）。它根据估计的轨迹，建立与任务要求对应的地图。

2.2.1 视觉里程计

视觉里程计关心相邻图像之间的相机运动。

仅通过视觉里程计来估计轨迹，将不可避免地出现累积漂移（Accumulating Drift）。这是由于视觉里程计（在最简单的情况下）只估计两个图像间的运动造成的。为了解决漂移问题，我们还需要两种技术：后端优化和回环检测。回环检测负责把“机器人回到原始位置”的事情检测出来，而后端优化则根据该信息，校正整个轨迹的形状。

2.2.2 后端优化

后端优化主要指处理 SLAM 过程中噪声的问题，这也可以称之为状态估计问题。

后端优化要考虑的问题，就是如何从这些带有噪声的数据中估计整个系统的状态，以及这个状态估计的不确定性有多大——这称为最大后验概率估计（Maximum-a-Posteriori，MAP）。这里的状态既包括机器人自身的轨迹，也包含地图。

在视觉 SLAM 中，前端和计算机视觉研究领域更为相关，比如图像的特征提取与匹配等，后端则主要是滤波与非线性优化算法。

2.2.3 回环检测

回环检测，又称闭环检测（Loop Closure Detection/Loop Closingn），主要解决位置估计随时间漂移的问题。让机器人知道“回到了原点”这件事，或者把“原点”识别出来，再把位置估计值“拉”过去，这样就可以消除漂移了。

地图大体上可以分为如下两类：

度量地图（Metric Map）：度量地图强调精确地表示地图中物体的位置关系，通常用稀疏（Sparse）与稠密（Dense）对其分类。稀疏地图进行了一定程度的抽象，并不需要表达所有的物体。稠密地图着重于建模所有看到的东西。对于定位来说，稀疏路标地图就足够了。而用于导航时，则往往需要稠密的地图。
拓扑地图（Topological Map）：相比于度量地图的精确性，拓扑地图则更强调地图元素之间的关系。

2.3 SLAM 问题的数学表述

“小萝卜携带着传感器在环境中运动”，由如下两件事情描述：

什么是运动？我们要考察从 $k - 1$ 时刻到 $k$ 时刻，小萝卜的位置 $x$ 是如何变化的。
什么是观测？假设小萝卜在 $k$ 时刻于 $x_k$ 处探测到了某一个路标 $y_j$ ，我们要考察如何用数学语言来描述这件事情。

通常使用如下的数学模型表述：从 $k - 1$ 时刻位置 $x_{k-1}$ 移动到 $k$ 时刻位置 $x_k$ : $x_k=f\left(x_{k-1},u_k,w_k\right)$ 其中 $w_k$ 为该过程中加入的噪声。可以把它称为运动方程。

与运动方程相对应，还有一个观测方程。观测方程描述的是，当小萝卜在 $x_k$ 位置上看到某个路标点 $y_j$ ，产生了一个观测数据 $z_{k,j}$ 。同样，用一个抽象的函数 $h$ 来描述这个关系： $z_{k,j}=h\left(y_j,x_k,v_{k,j}\right)$ 这里， $v_{k,j}$ 是这次观测里的噪声。

机器人的位姿由两个位置和一个转角来描述，即 $x_k=\left[x_1, x_2, \theta\right]^T_k$ ，其中 $x_1$ , $x_2$ 是两个轴上的位置而 $\theta$ 为转角。同时，输入的指令是两个时间间隔位置和转角的变化量 $u_k =\left[∆x_1, ∆x_2, ∆\theta\right]^T_k$ ，于是，此时运动方程就可以具体化为： $\begin{bmatrix}x_1 \\ x_2 \\ \theta \end{bmatrix}_k=\begin{bmatrix}x_1 \\ x_2 \\ \theta \end{bmatrix}_{k-1}+\begin{bmatrix}∆x_1 \\ ∆x_2 \\ ∆\theta \end{bmatrix}_k+w_k.$

至 21 世纪早期，以 EKF 为主的滤波器方法在 SLAM 中占据了主导地位。我们会在工作点处把系统线性化，并以预测—更新两大步骤进行求解（见第 10 讲）。最早的实时视觉 SLAM 系统即是基于 EKF[2] 开发的。随后，为了克服 EKF 的缺点（例如线性化误差和噪声高斯分布假设），人们开始使用粒子滤波器（Particle Filter）等其他滤波器，乃至使用非线性优化的方法。时至今日，主流视觉SLAM 使用以图优化（Graph Optimization）为代表的优化技术进行状态估计[13]。我们认为优化技术已经明显优于滤波器技术，只要计算资源允许，通常都偏向于使用优化方法。

2.4 实践：编程基础

2.4.1 安装 Linux 操作系统

安装 Ubuntu20.04

2.4.2 Hello SLAM

slambook2/ch2/helloSLAM.cpp

#include <iostream>
using namespace std;

int main(int argc, char **argv) {
  cout << "Hello SLAM!" << endl;
  return 0;
}

编译并运行文件：

//这条编译命令把 helloSLAM.cpp 这个文本文件编译成了一个可执行程序 a.out
$ g++ helloSLAM.cpp
$ ./a.out

2.4.3 使用 cmake

在一个 cmake 工程中，我们会用 cmake 命令生成一个 makefile 文件，然后，用 make 命令根据这个 makefile 文件的内容编译整个工程。
slambook2/ch2/CMakeLists.txt

# 声明要求的 cmake 最低版本
cmake_minimum_required(VERSION 2.8)
# 声明一个 cmake 工程
project(HelloSLAM)
# 添加一个可执行程序
# add_executable( 程序名 源代码文件 ）
add_executable(helloSLAM helloSLAM.cpp)

在当前目录下（slambook2/ch2/），调用 cmake 对该工程进行 cmake 编译：cmake .
cmake 会输出一些编译信息，然后在当前目录下生成一些中间文件，其中最重要的就是 MakeFile。现在，用 make 命令对工程进行编译：make
编译完成后，可以得到在 CMakeLists.txt 中声明的可执行程序 helloSLAM。执行这个程序：./helloSLAM

这次使用了先执行 cmake 在执行 make 的做法，执行 cmake 的过程处理了工程文件之间的关系，而执行 make 过程实际调用了 g++ 来编译程序。对项目的编译管理工作，从输入一串 g++ 命令，变成了维护若干个比较直观的 CMakeLists.txt 文件，这将明显降低维护整个工程的难度。比如，如果想新增一个可执行文件，只需在 CMakeLists.txt 中添加一行“add_executable” 命令即可，而后续的步骤是不变的。

唯一不足的是，cmake 生成的中间文件还留在代码文件当中。一种更好的做法是让这些中间文件都放在一个中间目录中，在编译成功后，把这个中间目录删除即可。所以，更常见的编译 cmake 工程的做法如下：

$ mkdir build
$ cd build
$ cmake ..
$ make

2.4.4 使用库

在一个 C++ 工程中，并不是所有代码都会编译成可执行文件。只有带有 main 函数的文件才会生成可执行程序。而另一些代码，我们只想把它们打包成一个东西，供其他程序调用。这个东西叫作库（Library）。现在尝试自己编写一个库：
slambook2/ch2/libHelloSLAM.cpp

#include <iostream>
using namespace std;

void printHello() {
  cout << "Hello SLAM" << endl;
}

然后在 CMakeLists.txt 文件中加上如下内容：add_library(hello libHelloSLAM.cpp)。然后使用 cmake 编译整个工程：

$ cd build
$ cmake ..
$ make

这时，在 build 文件夹中就会生成一个 libhello.a 文件，这就是我们得到的库。

在 Linux 中，库文件分成静态库和共享库两种。静态库以.a作为后缀名，共享库以.so结尾。所有库都是一些函数打包后的集合，差别在于静态库每次被调用都会生成一个副本，而共享库则只有一个副本，更省空间。如果想生成共享库而不是静态库，只需添加以下语句即可：add_library( hello_shared SHARED libHelloSLAM.cpp )。此时得到的文件就是 libhello_shared.so 了。

为了让别人（或者自己）使用这个库，我们需要提供一个头文件，说明这些库里都有些什么。因此，对于库的使用者，只要拿到了头文件和库文件，就可以调用这个库了。下面编写 libhello 的头文件。
slambook2/ch2/libHelloSLAM.h

#ifndef LIBHELLOSLAM_H_
#define LIBHELLOSLAM_H_
// 上面的宏定义是为了防止重复引用这个头文件而引起的重定义错误

// 打印一句hello的函数
void printHello();

#endif

这样，根据这个文件和我们刚才编译得到的库文件，就可以使用 printHello 函数了。最后，我们写一个可执行程序来调用这个简单的函数：
slambook2/ch2/useHello.cpp

#include "libHelloSLAM.h"

// 使用 libHelloSLAM.h 中的 printHello() 函数
int main(int argc, char **argv) {
  printHello();
  return 0;
}

然后，在 CMakeLists.txt 中添加两条命令，一个是可执行程序的生成命令，一个是可执行程序链接到刚才使用的库的命令：

add_executable( useHello useHello.cpp )
target_link_libraries( useHello hello_shared )

通过这两行语句，useHello 程序就能顺利使用 hello_shared 库中的代码了。

2.4.5 使用 IDE

KDevelop 的使用
Kdevelop 原生支持 cmake 工程。具体做法是，在终端建立 CMakeLists.txt 后，用 Kdevelop 中的“工程 → 打开/导入工程”打开 CMakeLists.txt。软件会询问你几个问题，并且默认建立一个 build 文件夹，帮你调用刚才的 cmake 和 make 命令。只要按下快捷键 F8，这些都可以自动完成。

打开“运行 → 配置启动器”，然后单击左侧的“Add New→ 应用程序”。在这一步中，我们的任务是告诉 Kdevelop 想要启动哪一个程序。