机器人视觉 课件 07第七章 机器人视觉同时定位与建图

机器人视觉第七章机器人视觉同时定位与建图本章内容安排1SLAM概述前端设计方案后端优化方法闭环检测全局地图构建与表示SLAM的概念与分类同时定位与建图（SimultaneousLocalizationandMapping，SLAM）机器人利用自身搭载的传感器，在没有环境先验信息的情况下，于运动过程中建立环境模型，同时利用环境信息进行自主定位，是机器人感知自身状态和外部环境的关键技术。2常用传感器激光雷达；单目摄像头；惯性测量单元（IMU）；RGB-D摄像头；双目摄像头。SLAM的概念与分类3视觉SLAM激光雷达SLAM多传感器融合SLAM主要传感器单目摄像头双目摄像头RGB-D摄像头优点主要体现在对环境的精细描述，比如在建筑物测绘和虚拟现实中有广泛应用缺点视觉信息对光照和纹理条件十分依赖，对于光照不足、变化或者纹理单一的环境，其性能会受到较大影响视觉信息处理计算量较大，保证实时性是一大挑战主要传感器激光雷达（LiDAR）优点准确性高，能提供精确的距离信息，在无人驾驶和物流机器人等需要精确导航和避障的领域有着广泛应用激光雷达不依赖环境光照和纹理，性能稳定缺点成本高且激光雷达观测数据较为稀疏。在几何结构单一的环境中，激光雷达SLAM容易因为几何约束缺失而失效主要传感器视觉传感器激光雷达（LiDAR）加速度计、陀螺仪等特点既可以获得精细的环境描述，又能得到精确的距离信息这种SLAM适用于更复杂的环境，尤其是对于动态干扰、光照变化等环境有着良好的适应性经典视觉SLAM算法框架41.传感器信息读取2.前端视觉里程计（VO）视觉SLAM的第一步是读取传感器信息，其中最重要的是来自相机的图像数据。这些图像数据经过预处理，可以提供对环境的光学成像与测量。此外，如果SLAM系统部署在机器人中，还可以包括从编码器和惯性测量单元（IMU）等其他传感器获取的数据，这些数据可以帮助在移动过程中更准确地估计机器人的运动。视觉里程计的步骤是利用相邻图像之间的视觉信息来估计相机的运动或机器人的位姿。通过在连续的视觉信息中提取和匹配关键点，视觉里程计能够通过相邻帧间的图像估计相机运动，并恢复场景的空间结构，以得到一种有效地描述相机运动的方式。视觉里程计还负责构建一个局部地图，用于帮助前端模块在位姿估计过程中进行局部优化。经典视觉SLAM算法框架53.后端优化（Optimization）4.闭环检测（LoopClosureDetection）如果仅仅通过前端视觉里程计来估计机器人的运动轨迹，将不可避免地出现比较大的累积漂移（AccumulatingDrift）。考虑到该问题，可引入后端优化环节进行整体轨迹校正来缓解这一问题。后端优化负责接收视觉里程计和闭环检测的信息，通过对这些信息进行一致性检查和全局优化，得到一个全局一致的相机轨迹和地图。后端主要采用滤波框架或者非线性优化框架进行实现。闭环检测与机器人的定位和建图功能密切相关。它的总体目标是要通过地图让机器人重新识别已到过的地方，从而触发闭环轨迹优化的后端优化程序。为了实现这一点，机器人需要具有识别相同场景的能力。我们希望机器人能够使用其搭载的视觉传感器，来实现闭环检测。这可以通过计算图像间的相似性来实现。在检测到闭环后，算法会将此信息反馈给后端优化算法，进而调整轨迹和地图以符合闭环检测结果。经典视觉SLAM算法框架65.建图（Mapping）建图（Mapping）指的是创建代表环境的地图，这个过程会根据SLAM的不同应用场景产生不同的需求。地图可呈现出多种形式，如空间点集合的地图，准确的三维模型地图，或者标记城市、村镇、铁路和河流的二维地图等等，它们的形式取决于机器人应用的实际需求。地图主要可以分为度量地图与拓扑地图两类。三种环境地图：(a)二维地图；(b)三维点云地图；(c)带纹理的三维模型地图本章内容安排7SLAM概述前端设计方案后端优化方法闭环检测全局地图构建与表示传感器选型8视觉SLAM常用传感器单目（Monocular）相机双目（Stereo）相机RGB-D相机单目SLAM是最简单的视觉SLAM系统，单目相机结构简单，成本较低，因而非常受研究者关注。单目相机的图像本质上是三维环境在相机的二维成像平面上留下的投影，以二维的形式记录三维的世界，丢掉了环境的深度信息。因此，仅靠单张图像无法计算场景中的物体与相机之间的距离，而这个距离在SLAM系统中起着至关重要的作用。若要恢复环境的三维结构，必须改变相机的视角，即通过移动相机的方式来获得视差（Disparity），从而估计相机的运动和环境的结构信息。

值得注意的是，如果把相机的运动和场景大小同时放大相同倍数，单目相机的图像是相同的。这意味着，单目SLAM估计的轨迹和地图与真实的轨迹和地图之间相差一个因子，这个因子称为尺度（Scale）因子。单目视觉SLAM无法仅凭图像确定这一真实尺度的性质被称为尺度不确定性。1.单目SLAM传感器选型9视觉SLAM常用传感器单目（Monocular）相机双目（Stereo）相机RGB-D相机双目相机由两个单目相机组成，且二者之间的距离，即基线（Baseline），是已知的。双目相机能测量的深度范围与基线相关，基线越长，则能测量的深度越大，如下图所示。双目相机的配置与标定较为复杂，其深度量程和精度受基线和分辨率所限制，而且视差的计算非常消耗计算资源，需要使用GPU或FPGA进行加速，才能实时输出整张图像的深度信息。2.双目SLAM传感器选型10视觉SLAM常用传感器单目（Monocular）相机双目（Stereo）相机RGB-D相机RGB-D相机是光学与深度相机的结合，不但能采集彩色图像，还能输出每个像素的深度值，如下图所示。其测距原理是通过红外结构光或者ToF原理，像激光雷达一样主动向物体发射光并接收物体返回的光，测出物体与相机之间的距离。它并不像双目相机那样通过软件计算距离，而是用物理方法测量距离，因而比双目相机更节省计算资源。但是，由于RGB-D相机存在测距范围窄、噪声大、视野小、易受日光干扰和无法测量投射材质等问题，其主要应用于室内场景的SLAM，室外环境较难应用。3.RGB-DSLAM里程计估计方法11视觉里程计特征点法直接法特征点法具有运行稳定，对光照、动态物体不敏感等特点是目前较为成熟的视觉里程计解决方案，被认为是视觉里程计的主流方法直接法不依赖于特征点，避免了关键点和描述子的计算，直接利用图像的灰度信息估计相机的运动既避免了特征计算耗时的缺陷，又规避了场景特征缺失的情况仅靠视觉里程计来估计轨迹，将会带来无法避免的累计漂移，这是由于视觉里程计在最简单的情况下只估计两张图像间的运动造成的。该累计漂移将会导致我们无法建立前后一致的地图，如上图所示。为了解决这一问题，就需要SLAM中的另外两种技术：后端优化和闭环检测。本章内容安排12SLAM概述前端设计方案后端优化方法滤波器方法BA与图优化方法闭环检测全局地图构建与表示后端优化方法13离散时间状态估计问题运动方程观测方程

滤波器方法14线性系统卡尔曼滤波（KalmanFilter,KF）非线性系统扩展卡尔曼滤波（ExtendedKalmanFilter,EKF）无迹卡尔曼滤波（UnscentedKalmanFilter,UKF）粒子滤波（ParticleFilter,PF）滤波器方法：卡尔曼滤波（KF）15卡尔曼滤波（KalmanFilter,KF）预测步骤：根据系统的状态转移模型，预测下一个时刻的状态和协方差矩阵更新步骤：根据观测值修正状态和协方差矩阵

滤波器方法：扩展卡尔曼滤波（EKF）16扩展卡尔曼滤波（ExtendedKalmanFilter,EKF）卡尔曼滤波的非线性扩展，通过对非线性函数进行一次泰勒展开，将非线性问题线性化EKF的核心在于将非线性状态转移和观测方程线性化

滤波器方法：无迹卡尔曼滤波（UKF）17无迹卡尔曼滤波（UnscentedKalmanFilter,UKF）通过一组精心选择的采样点（称为Sigma点）来近似非线性函数的传递过程这些Sigma点用来捕捉状态分布的均值和协方差的演化

滤波器方法：粒子滤波（PF）18粒子滤波（ParticleFilter,PF）通过一组随机采样的粒子表示状态的概率分布，是处理非线性、非高斯系统的强大工具该方法非常适用于复杂的SLAM问题，

BA与图优化方法19图优化方法近年来已成为SLAM后端的主流方向通过构建和优化图模型，将SLAM问题转化为图（Graph）上的优化问题核心思路：将位姿和观测数据表示为图中的节点（Node）和边（Edge），通过优化整个图的结构来得到最优解BA求解20

BA求解21

位姿图的概念与优化22

位姿图示例位姿图的概念与优化23

利用稀疏性进行边缘化24在视觉SLAM问题中，信息矩阵通常具有稀疏结构（矩阵的大部分元素为零）这种稀疏性使得我们可以利用稀疏矩阵技术来显著提高计算效率边缘化是一种常用的技术，通过消除不重要变量来简化计算Schur补：通过将边缘化变量对主变量的影响压缩到一个较小的矩阵中，降低计算复杂度利用稀疏性进行边缘化25

利用稀疏性进行边缘化26

本章内容安排27SLAM概述前端设计方案后端优化方法闭环检测词袋模型相似度计算全局地图构建与表示闭环检测28核心问题：累积漂移仅依赖前端（视觉里程计）和局部优化处理相邻时刻关联，误差会随时间不可避免地累积需引入外部约束来解决系统漂移问题基本概念定义：检测相机是否重新拍摄到了曾经出现过的场景作用：提供两个时间点之间的长期运动约束目标：利用约束校正轨迹，获得全局一致的地图与轨迹闭环检测29直观理解：质点弹簧模型位姿图：可视作质点弹簧系统，位姿节点为“质点”，观测数据为“弹簧”引入回环：相当于为系统引入额外的“弹簧”效果：提高系统的稳定性和收敛性，将带有误差的边校正回正确位置核心意义消除误差：限制长时间运行下的累积误差，保证长期正确性增强鲁棒性：提供重定位能力，在视觉跟踪失效时通过历史数据恢复定位区分VO与SLAM：前端+局部优化=视觉里程计(VO)VO+闭环检测+全局优化=完整的SLAM系统词袋模型30

词袋模型31

K叉树字典示意图词袋模型32

K叉树字典示意图相似度计算33TF-IDF（TermFrequency-InverseDocumentFrequency）被广泛用于评价某个词对文档的重要性程度，通过对单词的区分性或重要性加以评估，赋予不同的权值以起到更好的效果TF：某单词在当前图像中出现的频率越高，区分度越高IDF：某单词在字典中出现的频率越低，则它对图像的区分能力越强相似度计算34

相似度计算35

相似度计算36

相似度计算37

相似度计算38评估闭环检测算法：准确率（Precision）召回率（Recall）P-R曲线（Precision-RecallCurve）横轴：召回率纵轴：准确率曲线偏向右上方的程度：理想的算法，其P-R曲线应该尽可能偏向于右上方，这意味着在大多数情况下，算法都能同时获得较高的准确率和较高的召回率100%准确率下的召回率：反映了算法在最严格的条件下，能够覆盖真实闭环情况的程度50%召回率时的准确率：反映了算法在中等覆盖面的情况下，能够保持的“纯度”水平本章内容安排39SLAM概述前端设计方案后端优化方法闭环检测全局地图构建与表示面向机器人应用的地图表达与存储典型地图表示方法面向机器人应用的地图表达与存储401.定位定位是SLAM中地图最基本的功能，视觉里程计可以通过局部地图实现定位，回环检测则可以通过全局描述子信息实现定位。这类地图一般是稀疏的路标点地图，通常具有较小的存储需求，并且需要对定位任务具有较好的鲁棒性。易于存储的特性使得地图便于保存，并且使得机器人在重启后仍能定位，而无需重建做SLAM全过程。面向机器人应用的地图表达与存储412.导航规划导航规划是使得机器人能在地图中进行路径规划，从地图中任意两点之间寻找可行路径从而能够运动到目标位置的过程。因此，地图需要有可通行、不可通行位置的信息，稀疏路标地图无法做到。因此，需要一种稠密的地图形式来实现这一任务。面向机器人应用的地图表达与存储423.避障避障与规划任务比较类似，同样是在机器人运动过程中实现的目标。但是避障往往是局部的，更注重局部信息以及动态障碍物处理。稀疏路标点地图只具备一些角点、直线特征或其他可区分的视觉特征点，无法用于判断某个特征点是否是障碍物。因此也需要一种稠密的地图形式来实现避障。面向机器人应用的地图表达与存储434.三维重建除了机器人本身的应用外，我们往往也希望获得的地图能够有展示效果，能真实美观的展示出场景信息，使人能够依据地图进行决策。这就需要对地图进行更全面的重建，这种重建地图也必须是稠密的，准确的，而且有美观等要求，因此除了一般的稠密点云重建还往往会构建纹理等信息，从而实现更逼真的效果。面向机器人应用的地图表达与存储445.人机交互人机交互主要是指人与地图之间的互动。例如在场景中放置虚拟的物体、场景中进行仿真实验与场景设置等。这种交互可以作用于很多领域，并且使得人、地图、机器人实现良好的互动。这一需求需要机器人对于地图有更高级的认知，符合人类对地图的认识层级，因此需要引入语义层面的信息来进一步构建语义地图。典型地图表示方法45点云地图

典型地图表示方法46栅格地图栅格地图是另