GuoXin Li's Blog

SLAM Learning

字数统计: 4.2k阅读时长: 15 min
2021/07/13 Share

本文内容参考自 「高翔-SLAM14讲」

Slam: Simultaneous localization and mapping 同时定位🌧️地图构建,概念为希望机器人从未知环境的未知地点出发,在运动过程中通过重复观测到的地图特征(如墙角,柱子)定位自身的位置和姿态,再根据自身位置增量式的构建地图,从而同时定位和地图构建的目的。

SLAM发展

上世纪80年代SLAM概念的提出,从早期的声呐,到后来的2D/3D激光雷达,再到单目、双目、RGBD、ToF等各种相机,以及与惯性测量单元IMU等传感器的融合;SLAM的算法也从开始的基于滤波器的方法(EKF、PF等)向基于优化的方法转变,技术框架也从开始的单一线程向多线程演进。

  • 激光雷达 SLAM 发展

基于激光雷达的 SLAM (LIDAR SLAM)采用2D或3D激光雷达(单线或多线激光雷达),室内一般使用 2D 激光雷达,在无人驾驶领域,一般使用 3D 激光雷达。

激光雷达有点测量精准,提供精准角度和距离信息,可达 <1角度的角度精度以及 cm 级别的测距精度,能够覆盖平面内 270度 以上的范围,基于扫描振镜式的固态激光雷达(sick、hokuyo)可以达到较高的数据刷新率(20Hz以上)缺点价格昂贵。

  • 视觉 SLAM 发展

    • 视觉 SLAM 传感器更加便宜、轻便,而且随处可得(如手机上的摄像头)

    • 且图像能够提供更加丰富的信息,特征区分度更高,缺点是图像的实时处理需要很高的计算能力

    • 小型 PC 和 嵌入式设备,移动设备上实现视觉 SLAM 已经成为可能

    • 目前视觉 SLAM 的传感器有单目、双目、RGBD 三种相机,RGBD深度信息通过结构光原理计算,也有通过投射红外 pattern 并利用双目红外相机来计算,也有通过 TOF 相机实现。

    • 目前流行的视觉 SLAM 系统可以分为前后端

      • 前端完成数据关联,研究帧之间的变换关系,完成实时位姿跟踪,对输入图像进行计算,同时检测并处理闭环。
      • 后端对前端的输出结果进行优化,利用滤波理论(EKF、PF)或者优化理论进行树或图的优化,得到最优的位姿估计和地图。
* 采用滤波的 SLAM (下图a)
  估计 n 时刻的相机位姿 Tn 需要使用地图中所有路标的信息,且每帧都需要更新这些路标的状态,随着新的路标的不断加入,状态矩阵的规模增长迅速,导致计算和求解时耗严重,因此不适宜长时间大场景的操作;

*   采用优化算法的 SLAM (下图b)

    通常结合关键帧使用,估计 n 时刻的相机位姿 Tn 可以使用整个地图的一个子集,不需要在每幅图像都更新地图数据,因此现代成功的实时 SLAM 系统大部分都采用优化的方法。

  • SLAM 发展历程

    • MonoSLAM[5]是2007年由Davison 等开发的第一个成功基于单目摄像头的纯视觉SLAM 系统:

      • 使用了扩展卡尔曼滤波
    • 同年,Davison在Oxford的师父Murray和Klein发表了实时SLAM系统PTAM(Parallel Tracking and Mapping):
      首个基于关键帧BA的单目视觉SLAM 系统,随后移植至手机端,PTAM在架构上做出了创新,将姿态跟踪(Tracking)和建图(Mapping)两个线程分开并行运行,这种架构设计为后来的实时SLAM(如ORB-SLAM)所效仿,成为了现代SLAM系统的标配

    • 2011年,Newcombe 等人提出了单目DTAM 系统, 其最显著的特点是能实时恢复场景三维模型

      基于三维模型,DTAM 既能允许AR应用中的虚拟物体与场景发生物理碰撞,又能保证在特征缺失、图像模糊等情况下稳定地直接跟踪。

    • 2013年,TUM机器视觉组的Engel 等人提出了一套同样也是基于直接法的视觉里程计(visual odometry, VO)系统,该系统2014年扩展为视觉SLAM 系统LSD-SLAM。

      • 2015年,Engel等人对LSD-SLAM进行了功能拓展,使其能够支持双目相机和全景相机
    • 2014年,苏黎世大学机器人感知组的Forster等人提出开源的SVO系统

      • 该系统对稀疏的特征块使用直接法配准(Sparse Model-based Image Alignment),获取相机位姿
      • 根据光度不变假设构造优化方程对预测的特征位置进行优化(Feature Alignment)
      • 对位姿和结构进行优化(Motion-only BA和Structure-only BA)
    • 2016年,Forster对SVO进行改进,形成SVO2.0

      • 增加了边缘的跟踪
      • 考虑了IMU的运动先验信息
      • 支持大视场角相机(如鱼眼相机和反射式全景相机)和多相机系统
    • 2015年,Mur-Artal 等提出了开源的单目ORB-SLAM,并于2016年拓展为支持双目和RGBD传感器的ORB-SLAM2,它是目前支持传感器最全且性能最好的视觉SLAM系统之一。

    • 2016年,LSD-SLAM的作者,TUM机器视觉组的Engel等人又提出了DSO系统

      • 基于直接法和稀疏法的视觉里程计
      • 不进行关键点检测和特征描述子计算,而是在整个图像内采样具有强度梯度的像素点
      • 提出了完整的光度标定方法,考虑了曝光时间,透镜晕影和非线性响应函数的影响
    • 2017年,香港科技大学的沈绍劼老师课题组提出了融合IMU和视觉信息的VINS系统

      • 首个直接开源手机平台代码的视觉IMU融合SLAM系统
      • 可以运行在iOS设备上,为手机端的增强现实应用提供精确的定位功能,同时该系统也在应用在了无人机控制上,并取得了较好的效果
      • VINS-Mobile使用滑动窗口优化方法,采用四元数姿态的方式完成视觉和IMU融合,并带有基于BoW的闭环检测模块,累计误差通过全局位姿图得到实时校正

SLAM 感知入门

一个机器人需要至少做的两件事情:

  • 定位:知道自己在什么位置
  • 建图:知道周围环境是什么样子的

传感器:

  • 一类是携带在机器本体上的
  • 另一类是安装在环境里的

通过在机器本体上的传感器可以来感知未知的世界。

传感器相机

  • 单目,在单张图像里,无法确定一个物体的真实大小。
  • 双目(Stereo),双目摄像机之间的距离称为 基线。基线距离越大,能够测量到的物体就越远,所以无人车上搭载的双目相机通常会是一个大家伙。
  • 深度(RGB-D):除了能够采集到彩色图片,还能够读出每个像素与相机之间的距离。深度相机通常会携带多个摄像头。

(除此之外,SLAM 中还有全景相机,Event相机等)

经典视觉 SLAM 框架

Screen Shot 2021-07-20 at 23.15.43

  • 传感器信息读取

    相机图像的读取和预处理,传感器信息读取同步等。

  • 前端视觉里程设计 Visual Odometry

    估计相邻图像间相机的运动

  • 后端(非线性)优化 optimization

    后端接受前端测量的相机位姿,以及回环检测的信息,对其进行优化,得到轨迹和地图。

  • 回环检测

    回环检测是判断机器人是否到达过先前的位置。

  • 建图

    根据估计的轨迹,建立与任务要求对应的地图。

视觉里程计

  • 通过图像确定相机的运动,通过相邻帧之间的图像估计运动相机,恢复场景的空间结构。

  • 值计算相邻时刻的运动,与过去的信息没有关联。

  • 累计漂移

    仅通过视觉里程计来估计轨迹,会出现累计漂移问题。—> 误差传递的问题

    需要 回环检测全局校正

    回环检测:需要机器人回到原始位置的事情进行检测,后端优化根据该回环检测信息,纠正整个轨迹。

后端优化

  • SLAM 视觉中,前端和计算机视觉研究领域更相关,如图像特征提取和匹配,后端则主要是滤波和非线形优化算法。
  • 状态估计,后端优化要解决的——空间状态的不确定性估计
  • SLAM 问题的本质,对运动主体自身和周围环境空间不确定性的估计

回环检测

  • 闭环检测,主要解决位置估计 随时间漂移 的问题

  • 让机器人具有识别到过的场景的能力。

  • 利用图像间的相似性来完成回环检测。

建图

即,构建地图。

  • 对于家用扫地机器人,矮平面运动的机器人,只需要一个二维的地图。

  • 度量地图

    强调精确的表示地图中的物体的位置关系,通常用稀疏 (Sparse)与稠密(Dense)进行分类。

    稀疏:路标进行标记,其它的空白地方无用可以忽略。

    稠密:按照某种分辨率,由许多个小格子进行组成,通过导航算法结合小格子状态进行导航,会有存储量大的问题和地图一致性的问题。

  • 拓扑地图

    强调地图元素之间的关系,考虑连通性。

SLAM 问题数学表述

机器人各个时刻的位置 $X_1, X_2…X_k$

路标N个,$y_1,…y_N$

运动方程:$X{k} = f(X{k-1}, u{k},w{k})$

运动方程为机器人读入 运动传感器读数或者输入噪声等,使得这个那个函数可以指代人意的运动传感器/输入,成为一个通用的方程。

观测方程:$Z{k} = h(y{i}, xk,v{k,j})$

观测方程为,机器人在某个 $X{k}$ 位置看到某个 路标 $y_j$ 时产生的一个观测数据 $Z{k,j}$

SLAM 过程可以总结为 运动方程观测方程,其描述的基本 SLAM 问题为:当知道运功测量的读数 u,以及传感器的读数 z,如何求解定位问题(估计x)和建图问题(估计y),此时 SLAM 问题为一个状态估计问题。

状态估计的问题的求解,与两个方程的具体形式,以及噪声服从哪种分布(线性/非线形/高斯/非高斯)有关系。

线性高斯为最简单,它的无偏可以用卡尔曼滤波器(Kalman Filter)给出。

非线形高斯系统(Non-Linear No-Gaussian) 会使用拓展卡尔曼滤波器(Extended Kalman Filter)和非线形优化两类方法去求解。

SLAM 编程基础

安装 Linux 操作系统

推荐使用 ubuntu 操作系统。

使用编译器g++

安装g++

1
sudu apt-get install g++

使用 cmake

cmake 用来对 C++ 项目进行编译管理,在 cmake 工程中:

  • 使用 cmake 命令生成一个 makefile 文件
  • 然后用 make 命令根据这个 makefile 文件的内容编译整个工程。
1
2
3
4
5
6
7
8
9
# declare lowest version of cmake
cmake_minimum_required( VERSION 2.8 )

# declare a project of cmake
project( HELLOSLAM)

# add an exe program
# grammar: add_executable ( programName sourceFile )
add_executable ( helloSLAM helloSLAM.cpp )

cmake.txt 用于告诉 cmake 对此目录文件做什么事情。

1
2
3
4
#cmake . 会用 cmake 对该工程进行 cmake 编译
#cmake 会输出编译信息,在当前文件目录下生成一些中间文件,最重要的是 MakeFile,MakeFile 是自动生成的。
cmake .
# cmake . 之后就是使用 make 进行对工程的编译。

三维空间刚体运动

内外积与反对称矩阵

左右手坐标系:OpenGL、3D Max —> 右手坐标系, Unity、Direct3D —> 左手坐标系

叉积又称为外积:

image-20210816231511145

image-20210816233544284

坐标系间的欧式变换

惯性坐标系(世界坐标系),其为固定不动的,相机或者机器人是一个移动的坐标系。

对于一个相机视野中的 P ,其在相机坐标系中的坐标为 Pc,而在世界坐标系中的坐标为 Pw,这两个坐标之间的转换需要通过矩阵变换来实现。两个坐标系之间差了一个 欧式变换(旋转+平移)

两个坐标系之间的运动由一个旋转加上一个平移组成,这种运动成为刚体运动。

image-20210910000308957

image-20211024233821589

矩阵 R 为旋转矩阵,其为两个坐标系基的内积。其基向量长度为1,实际则为各基向量夹角的余弦值,所以旋转矩阵也称为方向余弦矩阵。

有关旋转矩阵:

  • 其为行列式为1的正交矩阵
  • 行列式为1的正交矩阵也是一个旋转矩阵

image-20211024234416227

Eigen实践

第三方库安装

首先使用 git submodule init git submodule update 来同步相关的第三方库。

  • Pangolin库的安装:

安装依赖:sudo apt-get install libglew-dev

然后进入到 3rdparty 进行 cmake 与 make 编译

Eigen为一个模板类。

在 vscode 中运行 slambook2 中的程序需要设置 📁 .vscode 中添加 task.json 与 launch.json 文件


launch.json

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
{
"configurations": [
{
"name": "C/C++",
"type": "cppdbg",
"request": "launch",
"program": "${fileDirname}/${fileBasenameNoExtension}",
"args": [],
"stopAtEntry": false,
"cwd": "${workspaceFolder}",
"environment": [],
"externalConsole": false,
"MIMode": "gdb",
"preLaunchTask": "compile",
"setupCommands": [
{
"description": "Enable pretty-printing for gdb",
"text": "-enable-pretty-printing",
"ignoreFailures": true
}
]
}
]
}

task.json

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
{
"version": "2.0.0",
"tasks": [{
"label": "compile",
"command": "g++",
"args": [
"-g",
"${file}",
"-o",
"${fileDirname}/${fileBasenameNoExtension}"
],
"problemMatcher": {
"owner": "cpp",
"fileLocation": [
"relative",
"${workspaceRoot}"
],
"pattern": {
"regexp": "^(.*):(\\d+):(\\d+):\\s+(warning|error):\\s+(.*)$",
"file": 1,
"line": 2,
"column": 3,
"severity": 4,
"message": 5
}
},
"group": {
"kind": "build",
"isDefault": true
}
}
]
}

旋转向量和欧拉角

旋转向量:

即利用一个三维向量表达出旋转矩阵SO(3) 所能表达的事情:任意旋转都可以用一个旋转轴和一个旋转角来刻画。

一个三维向量来描述一个旋转:

  • 其方向与旋转轴一致
  • 其长度大小与旋转角相等

旋转矩阵和旋转向量可以进行相互转化。

Screen Shot 2021-10-25 at 23.22.10

Screen Shot 2021-10-25 at 23.22.29

旋转轴n,旋转轴上的向量在旋转后不发生改变:Rn = n.

欧拉角

航空、航模中会有:

“俯仰角”,pitch ——绕 Y 轴,

“偏航角”,yam ——绕 Z 轴,

“滚转角”,roll —— 绕 X 轴。

image-20211026225238811

欧拉角会产生 万向节死锁的问题: 在俯仰角为 $\pm 90^\circ$时,第一次旋转和第三次旋转将使用同一个轴,这样就会使得系统丢掉一个自由度,这种称为奇异性问题。

四元数

为了解决 万向节死锁的问题,因为找不到不带奇异的三维向量描述方式,用一种类似复数的代数:四元数,来解决旋转的问题。

一个四元数q有一个实部三个虚部:

其中,i,j,k 为四元数的三个虚部:

四元数的运算

加减法为常规运算。

乘法:

Screen Shot 2021-10-27 at 23.12.08

膜长:

Screen Shot 2021-10-27 at 23.12.55

共轭:

Screen Shot 2021-10-27 at 23.14.38

逆:

Screen Shot 2021-10-27 at 23.15.35

用四元数表示旋转

空间三维点:$p = [x,y,z]\in R^3$ ;

单位四元数 q 指定的旋转;

三维点 p 到 p’ :用矩阵的表示方式为: p’ = Rp.

四元数的表示方式:

  • 虚四元数表示一个三维空间点

    $p=[0,x,y,z]^T=[0,v]^T$

  • 相当于四元数的 3 个虚部与空间中的3个轴相对应:

最后取出 p’ 的虚部部分,即得到旋转后的坐标。

李群和李代数

image-20211031203435595

群是一种 集合 加上一种 运算 的代数结构。群满足如下几个条件:

  • 封闭性: $\forall a_1,a_2 \in A, a_1•a_2 \in A$
  • 结合律: $\forall a_1,a_2,a_3 \in A, (a_1•a_2)a_3=a_1•(a_2•a_3)$
  • 幺元:$\exists a_0 \in A, s.t.\forall a \in A ,a_0•a=a•a_0=a$
  • 逆: $\forall a \in A, \exists a^{-1} \in A, s.t. a•a^{-1}=a_0$

李群是指具有连续(光滑)性质的群

李代数:每个李群都有与之对应的李代数,李代数描述了李群的局部性质。

对于旋转矩阵的每一次求导,只需要左乘一个 $\phi^{(t)}$ 即可。$\phi^{(t)}$ 为一个三维向量。

旋转矩阵R与反对称矩阵 $\phi^{\bigwedge} t$ 之间的关系: $R(t) = exp(\phi^{\bigwedge} t)$

CATALOG
  1. 1. SLAM发展
  2. 2. SLAM 感知入门
    1. 2.1. 传感器相机
  3. 3. 经典视觉 SLAM 框架
  4. 4. SLAM 问题数学表述
  5. 5. SLAM 编程基础
    1. 5.1. 安装 Linux 操作系统
    2. 5.2. 使用编译器g++
    3. 5.3. 使用 cmake
  6. 6. 三维空间刚体运动
    1. 6.1. 内外积与反对称矩阵
    2. 6.2. 坐标系间的欧式变换
    3. 6.3. Eigen实践
    4. 6.4. 旋转向量和欧拉角
    5. 6.5. 欧拉角
    6. 6.6. 四元数
    7. 6.7. 四元数的运算
    8. 6.8. 用四元数表示旋转
  7. 7. 李群和李代数