《自主移动机器人 》课件 第7章 定位与建图

第七讲移动机器人的定位与地图构建熊蓉浙江大学控制科学与工程学院为什么需要SLAM？随着机器人的工作时间越来越长，里程估计的误差会不断累积，导致对自身位姿的估计精度越来越差SLAM能够借助机器人在同一场景的回访，减少累积误差回访前回访后图片来自AMarkov-ChainMonteCarloApproachtoSimultaneousLocalizationandMappingSLAM系统框架特征跟踪：里程估计中的前后帧特征匹配闭环检测：判断是否回访同一场景两者和传感器的信息模态有关，所以称为SLAM系统前端SLAM系统框架前端用于生成约束后端负责求解约束构成的方程，得到优化的机器人轨迹估计后端和传感器信息模态无关，所以称为SLAM系统后端闭环检测基于传感器信息，判断当前所处的地点是否是过去已经访问过的地点。为什么基于传感器信息，而非里程估计的定位结果？由于误差累积，即使现在和过去某个时刻处在同一个地点，现在的里程估计结果，和当时的结果也不相同。如何定义回访到同一个地点?现在和过去某个时刻的传感器信息相似闭环检测问题定义给定地图数据库，存储过去所有地点的传感器信息，给定当前的传感器信息，从数据库中返回与当前帧传感器信息最相似的一帧数据，判断是否为同一地点最相似的判断：关于图像相似度的度量，并且可快速计算，因为数据库可能很大。相似度的依据：在同一地点应当得到类似的传感器信息，注意不是完全相等基于图像的闭环检测构建图像特征，避免像素层面可能存在的区别利用特征的欧式距离反应相似度词包模型相似的图像应当可以匹配大量特征，但逐张匹配效率太低采用图像中特征的频率分布描述图像，实现欧式距离匹配词包模型

其他图像特征方法VLAD，避免离散化损失信息，采用描述子和中心的差向量描述图像DenseVLAD，避免关键点提取损失信息，采用每个像素的描述子，构建VLAD……序列判断基于特征的欧氏距离，简单阈值判定？阈值可能在不同的地点不同SLAM是一个序列过程，如果t0地点的闭环是t+1时刻的地点，那么t1地点的闭环，不会离t+1时刻的地点很远闭环检测剩余问题得到同一地点的传感器数据以后？问题退化为临近图片/激光数据的位姿估计问题和里程估计的t-1和t时刻的相对位姿估计问题解法类似如何解决激光的闭环检测？提取激光的特征点，采用BoW或者VLAD的思路采用所有的激光点，通过DenseVLAD的思路SLAM后端把位姿看做节点，把每个约束看做边，机器人在运行过程中能够生成一个位姿图模型其中相邻节点间的边代表里程估计生成的约束跨多个节点的边代表闭环检测生成的约束SLAM后端的任务就是求解整条轨迹，逼近所有约束方程里程估计约束闭环检测约束SLAM后端为什么SLAM能够比里程估计更准确？里程估计的结果可以看做是没有闭环检测的SLAM直观理解，同样多的变量，SLAM有更多的约束方程概率统计的角度，里程估计是所有里程估计约束方程的极大后验估计（MAP），并不是所有约束的MAP，而SLAM是。里程估计约束闭环检测约束SLAM的MAP构造和求解

SLAM的MAP构造和求解

因子图里程估计和闭环检测约束本质上都是一种建立在从未知变量到某个测度的函数，将其统称为因子SLAM问题的一般化表达可以看作因子图，而因子的值域未必需要归一化比如此时的里程计约束可以忽略归一化因子，写为

更多约束

多传感融合-最小二乘SLAM约束方程构造案例

SLAM的MAP小结优势构造精简，可以采用最小二乘优化器进行求解精度最好，逼近所有的约束方程劣势整条轨迹都是变量，运行越久，变量越多，速度越慢哪怕是在固定大小的环境中运行，复杂度仍然没有上界SLAM的滤波构造为了实现在线的SLAM，需要控制变量的规模只保留当前的位姿，SLAM会退化为里程，因为过去的位姿不属于可优化变量，无法考虑闭环保留过去的位姿，则无法控制变量的规模将环境表示为特征的集合，放在变量中参与优化，此时闭环可以转化为当前位姿和环境特征之间的约束。此时复杂度为环境的大小，如果环境固定大小，则复杂度有上界，使在线计算成为可能。卡尔曼滤波构造

卡尔曼滤波构造原因在于特征静态，和机器人的运动无关，可构造EKF运动模型如下

机器人从地图原点到当前时刻，单步运动后的方差情况卡尔曼滤波构造原因在于特征静态，和机器人的运动无关，可构造EKF运动模型如下在卡尔曼滤波的更新步骤以后，方程会相对运动模型的预测缩小通过标准的卡尔曼滤波预测和更新公式，可以实现SLAM的滤波估计卡尔曼滤波小结卡尔曼滤波可以实现与环境规模相关的复杂度，为什么还需要SLAM的MAP构造对于非线性系统，EKF在理论上并没有最优估计的保证，这主要体现在模型线性化时，仅线性化一次。而对于MAP，整条轨迹的求解使重线性化成为可能。当求解SLAM是为了离线构建地图，则MAP是最佳选择当求解在线SLAM时，目前的前沿方法通常选择MAP和滤波的均衡，将一个窗口内的多个变量放到优化变量中提高里程的精度。同时采用另一个线程求解整个SLAM，以非实时的频率对里程进行纠正。比如ORB-SLAM等。最小二乘求解器SLAM的MAP构造可以用任意最小二乘求解器求解以下图为例，的高斯牛顿求解过程中，需要通过求解线性化后的最小二乘得到当前迭代步骤的增量

稀疏结构一般的求解器不假设方程矩阵有特殊的结构考虑到约束通常只和1-2个变量有关，SLAM问题中线性化的雅克比只在有关的变量处才为非0如GPS因子对所有变量求导，仅在1个有关的变量处为非0

矩阵高度稀疏！稀疏结构最小二乘求解器采用稀疏矩阵的方式表达SLAM的雅克比，能够大幅度减少存储空间，避免大量0和0相乘的运算，极大提高计算效率。因此，采用基于稀疏矩阵的最小二乘求解器，能够将一般求解器的复杂度显著下降1-2个数量级，使大规模的SLAM问题也能够被快速求解。非高斯因子及作用高斯分布能够对应到具有平方项的因子，形成最小二乘问题也可以构造因子为非平方项，比如采用绝对值对应到拉普拉斯分布

非高斯因子及作用高斯分布和拉普拉斯分布相比区别是，拉普拉斯具有厚尾特性，对于远离均值的数据，具有更高的概率非高斯因子及作用高斯分布和拉普拉斯分布相比区别是，拉普拉斯具有厚尾特性，对于远离均值的数据，具有更高的概率这种分布对于可能出现较多异常故障数据的传感器而言，具有重要意义，对因子取负对数后可以体现出这种效果远离均值的数据不会造成巨大的误差定位相比于SLAM，另一种常见的应用是定位，即机器人在SLAM构建的地图上，计算自身当前的位姿，但不改变地图中的变量可以看做是求解SLAM，但是所有的环境变量均为已知的值定位相比于SLAM有什么优势？定位的求解变量恒定，因此具有实时性。同时每个时刻因为环境变量均作为已知值，所以方差有上界定位的滤波构造

定位的滤波构造小节

蒙特卡洛方法为了解决全局定位的问题，本质是要寻找支持非高斯分布的滤波方法非高斯分布情况下，滤波公式无法导出如同卡尔曼滤波这样的解析迭代形式采用非参数的统计方法，利用采样的思路，通过样本集合反应概率分布此时对分布的处理，都转化对采样的处理，具有可行性圆周率计算案例

圆周率计算案例

圆周率计算案例

蒙特卡洛积分

蒙特卡洛积分

蒙特卡洛积分计算滤波

每个样本根据运动形成一个新样本蒙特卡洛积分计算滤波

机器人从地图原点到当前时刻，单步运动后的方差和样本情况蒙特卡洛积分计算滤波

蒙特卡洛积分计算滤波

机器人得到特征观测后，样本的权重情况

重要性重采样

重要性重采样如何重采样？重要性高的样本保留，重要性低的样本舍弃，以下位轮盘赌方法进行重采样的思路粒子滤波利用蒙特卡洛积分，能够在任意分布下计算滤波的公式步骤如下：给定样本集通过运动产生新的样本集通过观测对样本集加权如果粒子匮乏，则重采样，否则忽略迭代该滤波方法称为粒子滤波（Particlefilter）粒子滤波其他问题粒子滤波相比于卡尔曼滤波优势无需高斯分布假设，可解决全局定位无需非线性模型线性化样本数决定性能（作为一种优势，即算力可以换性能）劣势样本数决定性能（作为一种劣势，即计算相比之下更慢）当未知数的维度增加，样本数量需求指数上升，因此不能用于SLAM问题的计算本章小结回访触发闭环检测，使里程估计累积的误差能够减少，形成里程估计和闭环检测约