本文内容参考自 「高翔-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 框架
传感器信息读取
相机图像的读取和预处理,传感器信息读取同步等。
前端视觉里程设计 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 | # declare lowest version of cmake |
cmake.txt 用于告诉 cmake 对此目录文件做什么事情。
1 | #cmake . 会用 cmake 对该工程进行 cmake 编译 |
三维空间刚体运动
内外积与反对称矩阵
左右手坐标系:OpenGL、3D Max —> 右手坐标系, Unity、Direct3D —> 左手坐标系
叉积又称为外积:
坐标系间的欧式变换
惯性坐标系(世界坐标系),其为固定不动的,相机或者机器人是一个移动的坐标系。
对于一个相机视野中的 P ,其在相机坐标系中的坐标为 Pc,而在世界坐标系中的坐标为 Pw,这两个坐标之间的转换需要通过矩阵变换来实现。两个坐标系之间差了一个 欧式变换(旋转+平移)。
两个坐标系之间的运动由一个旋转加上一个平移组成,这种运动成为刚体运动。
矩阵 R 为旋转矩阵,其为两个坐标系基的内积。其基向量长度为1,实际则为各基向量夹角的余弦值,所以旋转矩阵也称为方向余弦矩阵。
有关旋转矩阵:
- 其为行列式为1的正交矩阵
- 行列式为1的正交矩阵也是一个旋转矩阵
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 | { |
task.json
1 | { |
旋转向量和欧拉角
旋转向量:
即利用一个三维向量表达出旋转矩阵SO(3) 所能表达的事情:任意旋转都可以用一个旋转轴和一个旋转角来刻画。
一个三维向量来描述一个旋转:
- 其方向与旋转轴一致
- 其长度大小与旋转角相等
旋转矩阵和旋转向量可以进行相互转化。
旋转轴n,旋转轴上的向量在旋转后不发生改变:Rn = n.
欧拉角
航空、航模中会有:
“俯仰角”,pitch ——绕 Y 轴,
“偏航角”,yam ——绕 Z 轴,
“滚转角”,roll —— 绕 X 轴。
欧拉角会产生 万向节死锁的问题: 在俯仰角为 $\pm 90^\circ$时,第一次旋转和第三次旋转将使用同一个轴,这样就会使得系统丢掉一个自由度,这种称为奇异性问题。
四元数
为了解决 万向节死锁的问题,因为找不到不带奇异的三维向量描述方式,用一种类似复数的代数:四元数,来解决旋转的问题。
一个四元数q有一个实部三个虚部:
其中,i,j,k 为四元数的三个虚部:
四元数的运算
加减法为常规运算。
乘法:
膜长:
共轭:
逆:
用四元数表示旋转
空间三维点:$p = [x,y,z]\in R^3$ ;
单位四元数 q 指定的旋转;
三维点 p 到 p’ :用矩阵的表示方式为: p’ = Rp.
四元数的表示方式:
虚四元数表示一个三维空间点
$p=[0,x,y,z]^T=[0,v]^T$
相当于四元数的 3 个虚部与空间中的3个轴相对应:
最后取出 p’ 的虚部部分,即得到旋转后的坐标。
李群和李代数
群是一种 集合 加上一种 运算 的代数结构。群满足如下几个条件:
- 封闭性: $\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)$