移动机器人原理与技术 课件 第六章 移动机器人同时定位与建图

移动机器人技术原理与应用第六章

移动机器人同时定位与建图基于滤波的移动机器人SLAM原理基于滤波的移动机器人SLAM滤波算法框架6.1基于滤波的移动机器人SLAM简介6.26.3基于滤波的移动机器人及环境的模型6.4移动机器人卡尔曼滤波SLAM算法移动机器人粒子滤波SLAM算法6.56.6基于图优化的移动机器人SLAM算法6.76.1基于滤波的移动机器人SLAM简介SLAM主要研究在对机器人位姿和其环境信息都不具备先验知识的情况下，如何应用合理的表征方法对环境建模（也就是构建地图）并同时确定移动机器人自身位姿。在应用随机概率方法解决同时定位与地图创建问题时，首先要建立概率表示的移动机器人运动模型和观测模型，用状态向量表示移动机器人自身的位姿和环境特征的位置，运用滤波技术对移动机器人的位姿和环境特征进行概率估计。6.1基于滤波的移动机器人SLAM简介机器人所有的可能位姿和环境特征保持概率分布，随着机器人的运动，观测到新的环境数据，概率分布被更新，从而减少机器人位姿估计和环境特征估计的不确定性。高斯分布是最常用的概率分布，在假设机器人位姿和环境特征服从高斯分布的情况下，SLAM问题转化为状态向量的高斯均值和方差估计问题。实际的移动机器人系统是非线性非高斯的，对于非线性非高斯的状态估计问题，粒子滤波器（ParticlFilter）更具有普遍适用性。6.2基于滤波的移动机器人SLAM原理6.2.1移动机器人SLAM系统状态在移动机器人实际运动过程中，控制量uk在k-1时刻作用于机器人，使机器人在k时刻到达状态Xk，Xk表示k时刻移动机器人位姿(位置和姿态)。在移动机器人运动起始的0时刻和k-1时刻间的移动机器人状态序列记为X0:k-1=(X0,X1,....,Xk-1)，在0时刻和k-1时刻间的控制量序列可表示为u0:k-1=(u0,u1,....,uk-1)。环境中景物的外观特征可作为环境状态信息，考虑到存储空间及运算能力，实际问题中只提取景物的关键特征作为环境状态信息，某时刻对应的6.2.1移动机器人SLAM系统状态环境状态记为M={m1,m2,...,mn}，其在与地图中已有的特征匹配后加入地图，系统会逐渐建立起增量地图。移动机器人与环境构成了移动机器人系统，系统状态用[XkM]T表示，即系统状态包含移动机器人自身的状态和移动机器人环境中景物的特征。SLAM问题描述为：在环境信息M未知，移动机器人初始位姿X0已知，输入控制量uk给定的前提下，创建地图(由环境信息构成)的同时确定机器人的状态(即位姿)Xk。6.2.1移动机器人SLAM系统状态环境状态记为M={m1,m2,...,mn}，其在与地图中已有的特征匹配后加入地图，系统会逐渐建立起增量地图。移动机器人与环境构成了移动机器人系统，系统状态用[XkM]T表示，即系统状态包含移动机器人自身的状态和移动机器人环境中景物的特征。SLAM问题描述为：在环境信息M未知，移动机器人初始位姿X0已知，输入控制量uk给定的前提下，创建地图(由环境信息构成)的同时确定机器人的状态(即位姿)Xk。6.2.1移动机器人SLAM系统状态对于运动在平面上的移动机器人，其在根据uk而进行的运动中，应用内部传感器即本体感受器，根据里程计进行位置预测，也称为估计；同时根据系统观测模型进行该位置上的观测特征预测；应用外部传感器对环境进行测量，获得观测特征，将该观测特征与预测观测特征进行匹配，匹配成功的观测特征对移动机器人的位置估计进行修正，得到Xk；同时，匹配成功的特征信息构建地图，即得到M。6.2.1移动机器人SLAM系统状态SLAM通用架构6.2.2移动机器人状态的概率描述移动机器人本身状态Xk与移动机器人系统在k时刻之前发生的所有事件是概率相关的。k时刻的移动机器人状态Xk的出现是基于所有先前时刻的移动机器人状态X0:k-1、控制量u0:k-1和观测量Z1:k-1。因此，移动机器人Xk可表示为如下概率形式：由马尔可夫法则，Xk-1可完整地表征Z1:k-1和u0:k-1，若Xk-1已经计算得到，则Xk只与uk和Xk-1有关。根据概率学的条件独立原理，下式成立：6.2.2移动机器人状态的概率描述

称为移动机器人的状态传递概率，表示k时刻移动机器人的起始状态为Xk-1，在输入控制量uk后到达状态Xk的概率。实际应用系统中，一般用下式函数来表征该状态传递概率：上式称为机器人运动模型或状态传递模型，其中，f(·)称为状态传递函数，一般为非线性函数；ωk为用来表示建模误差或控制量噪声的加性噪声。6.2.3移动机器人传感器观测的概率表示移动机器人通过自身传感器得到的位置状态是不准确的，需要借助如摄像头、激光、雷达、超声波等外部传感器的观测值Zk来修正移动机器人系统的状态估计值，从而使移动机器人系统的状态尽量接近实际值。对于从1时刻到k时刻时间段内的传感器观测表示为Z1:k{Z1,Z2,...,Zk},观测Zk表述为如下概率形式：k时刻观测的Zk完全可由Xk和M推测。

称为观测概率，表示k时刻在系统状态Xk时所能观测到的Zk的似然概率。6.2.3移动机器人传感器观测的概率表示实际应用系统中，一般下式函数来表示：h(·)称为观测模型，vk为传感器本身存在或测量方式引起的噪声。移动机器人系统是一个动态随机系统，由移动机器人状态传递概率和传感器观测概率共同描述，对应的时间生成模型又称为隐马尔可夫模型或者动态贝叶斯网络。6.3基于滤波的移动机器人SLAM滤波算法框架6.3.1贝叶斯滤波的时间更新贝叶斯法则是贝叶斯滤波推导的基础，根据概率问题中的边缘概率法则，时刻系统的后验概率可分解为：由条件概率的链式法则，有下式成立：由于移动机器人的运动是马尔可夫的，Xk仅与Xk-1和uk有关，所以有：6.3.1贝叶斯滤波的时间更新又因为Xk-1与k时刻的控制量uk无关，则由此当已知k-1时刻的联合后验概率

和k时刻的控制量uk，得到：上式即为贝叶斯滤波的时间更新过程。6.3.2贝叶斯滤波的测量更新若传感器在k时刻的观测量为Zk，由贝叶斯法则可知下式成立：按马尔可夫假设，观测量条件Zk独立于过去的测量Z1:k-1和控制量uk，有下式成立：6.3.2贝叶斯滤波的测量更新

与X无关，用η表示，称为正则化参数上式(描述了贝叶斯滤波的测量更新过程。6.4基于滤波的移动机器人及环境的模型6.4.1移动机器人运动模型1．基于里程计的运动模型移动机器人位姿变化的大小通过安装在其驱动车轮上的光电编码器来推算，根据编码器码盘输出的脉冲个数、码盘转数和车轮半径，计算出驱动车轮在给定时间内转过的弧度，进而计算出驱动车轮运动的距离。设Δt时间内光码盘输出的脉冲数为N，光码盘为p线/转，驱动车轮半径为r，则该驱动车轮运动的距离Δd为：6.4.1移动机器人运动模型1．基于里程计的运动模型对于具有两个驱动轮的轮式移动机器人，假设按照上式计算出的左右两个驱动车轮的移动距离分别为ΔdL和ΔdR，移动机器人从状态移动到，则移动机器人的移动距离表示为，移动机器人转动的角度为，L为移动机器人车轮间距。6.4.1移动机器人运动模型1．基于里程计的运动模型移动机器人运动模型若由移动机器人移动距离和转动的角度表示移动机器人的输入控制变量移动机器人的运动半径可表述为：6.4.1移动机器人运动模型1．基于里程计的运动模型在移动机器人运动中，基于里程计的运动模型同时考虑了移动机器人移动的距离变化和转动的方向角变化，更有效地逼近移动机器人运动的实际轨迹。基于里程计的移动机器人运动模型可描述为：在时间Δt内，机器人路径为弧长ΔDk，偏转角度为Δφk。6.4.1移动机器人运动模型2．基于控制命令的运动模型当可以得到移动机器人运动的线速度和偏转角时，由线速度和偏转角组成输入控制变量的移动机器人运动模型表示为:6.4.2传感器观测模型传感器观测模型6.4.2传感器观测模型1．基于移动机器人坐标系的极坐标表示法观测模型通过移动机器人携带的传感器观测得到坐标为的特征点，在移动机器人坐标系下的模型可写为：vk为传感器本身存在或测量方式引起的噪声。6.4.2传感器观测模型2．基于全局坐标系的观测模型把在移动机器人坐标系中某个环境特征点的坐标转换到世界坐标系下，就得到观测量在世界坐标系中的坐标的观测方程，即移动机器人系统的观测模型：vk为传感器本身存在或测量方式引起的噪声。移动机器人系统含有n维的状态向量Xk、m维的观测序列Zk、m维的观测噪声vk和n维的过程噪声序列ωk，在系统6.5

移动机器人卡尔曼滤波SLAM算法是线性离散的假设条件下,Fk是系统n×n维的状态转移矩阵，Bk是系统的输入控制矩阵，Hk表示m×n维的观测矩阵，uk用来表示系统的输入控制向量，此系统的状态方程和观测方程可以表示如下：过程噪声ωk和观测噪声vk满足高斯白噪声假设，即：Qk为ωk的协方差矩阵，Rk为vk的协方差矩阵。6.5

移动机器人卡尔曼滤波SLAM算法系统通过时刻的状态值获得时刻的状态和协方差的先验估计的过程称为系统的时间更新。利用时刻的观测对上述先验估计进行校正的过程则称为测量更新，校正后的系统状态和协方差矩阵就成为时刻的先验估计。将非线性状态转移函数和观测函数围绕移动机器人当前状态估计值扩展成泰勒级数形式，并略去其二次幂及其高阶小项，得到近似线性化模型，实现非线性系统的线性化，并运用卡尔曼滤波算法框架递推计算移动机器人系统状态，即扩展卡尔曼滤波（extendedkalmanfilter，EKF）SLAM算法。6.5

移动机器人卡尔曼滤波SLAM算法基于扩展卡尔曼滤波的SLAM算法的5个步骤如下：步骤1：由k-1时刻的移动机器人状态和控制命令uk计算k时刻的状态及误差协方差；式中r代表机器人，l表示环境。6.5

移动机器人卡尔曼滤波SLAM算法步骤2：应用传感器获得k时刻的对环境特征的实际观测值Yk；步骤3：根据移动机器人位姿预测与实际观测计算得到观测预测

；步骤4：计算新息

和增益Kk；步骤5：状态更新；6.5

移动机器人卡尔曼滤波SLAM算法澳大利亚机器人中心提供了“CarParkDataset”标准数据集，用以进行EKF-SLAM算法实验。该数据集可以在其网站.au下载得到。实验用的智能车上安装了GPS、激光雷达和惯导三种传感器，惯导测量车辆左后轮线速度及车辆舵角，激光雷达则测量路标的距离和方向，智能车在运动过程中的经纬度通过GPS记录。“CarParkDataset”数据集纪录了智能车在实验场地东北部45m*30m的区域中行驶约2分钟的过程中各传感器的测量值。数据由三部分组成，一部分是智能车在运动过程中6.5

移动机器人卡尔曼滤波SLAM算法的GPS信息，记录智能车的运动路径，虽然有一定的误差，一般仍被作为从总体上对SLAM算法的性能进行评估的参考标准。第二部分是惯导传感器在各个时刻的测量值，代入智能车的运动方程则可预测智能车在下一个时刻的位姿，第三部分是激光雷达在各个时刻对人工路标的观测数据，通过提取观测特征并进行数据关联，最终根据观测数据值和数据关联结果修正地图估计和智能车的位姿估计。6.5

移动机器人卡尔曼滤波SLAM算法“CarParkDataset”数据集的EKF-SLAM结果6.6

移动机器人粒子滤波SLAM算法6.6.1蒙特卡罗采样对于要解决的问题，蒙特卡罗方法首先建立一个概率模型，使概率模型的参数等于问题的解，所求参数的统计特征通过对模型的观察或抽样试验来计算，最后给出所求解的近似值。蒙特卡罗方法通过随机试验求解积分问题，将服从分布密度函数p(r)的随机变量g(r)的数学期望E(g(r))作为所要求解的积分：6.6.1

蒙特卡罗采样从分布密度函数p(r)中通过某种试验获得采样N个观测值和N个相应的随机变量值g(ri)i=1,...,N的算术平均值作为积分的估计值:当采样点N足够大时，根据大数定律，趋于数学期望E(g)。6.6.2重要性采样蒙特卡罗采样需要从后验概率分布p(r)中采样N个点，对于移动机器人系统，移动机器人位置估计E(g(X0:k))可以表示为：在移动机器人系统中，系统的后验分布密度函数

无法直接获得，对系统状态的估计无法通过蒙特卡罗方法采样后验分布密度函数而进行。但可以对系统中一个已知的并且容易采样的概率分布函数

进行采样，该概率分布函数被称为重要性采样分布或者重要性函数。通过对由重要性函数的采样点进行加权来逼近

：6.6.2重要性采样具体推导如下：6.6.2重要性采样

称为未归一化的重要性权值。另外，

可以表示为：6.6.2重要性采样如果能够从重要性函数式

采样到样本点，则通过蒙特卡罗采样后的表示为：称为归一化权值。6.6.3序列重要性采样重要性函数

可以分解为：实际动态系统服从一阶马尔可夫过程并且满足系统观测独立，其数学表达式可以分别描述为：p(X0)表示该动态系统的初始先验密度。6.6.3序列重要性采样如果重要性分布函数只依赖于前一状态Xk-1和当前的观测值Zk，即上式的重要性采样函数采样得到的样本

，样本的最优的选择方法是：当前时刻的粒子由前一时刻粒子Xk-1和当前的观测值Zk采样得到，这种采样方法称为序列重要性采样。将一定的权值wik赋予给每一个样本Xik，则带权重的粒子集表示Xk的概率分布，形式如下：6.6.3序列重要性采样可以近似地表达为：式中，

是狄拉克函数。6.6.4退化问题与重采样序列重要性采样算法中，当前时刻的粒子权值是由上一时刻的粒子权值递推得到，存在误差的权值的误差会随着时间的传播而进一步的积累，导致只有少数粒子的权值比较大，而大多数的粒子因权值太小以至于最终被忽略不计，即序列重要性采样存在“粒子退化”现象。6.6.4

退化问题与重采样“粒子退化”用指标Neff来衡量，Neff值小于阈值则表明“粒子退化”现象严重，需要采取措施改善退化现象。Neff由下式近似计算：通过两种方案来改善退化现象：选择好的建议分布或进行重采样。在通过序列重要性函数采样得到个样本的基础上，再对序列重要性函数进行次采样，剔除权值较小的样本，保留权值较大的样本。也就是权值较小的样本被复制的权值较大的样本所代替，得到一组新的样本集合。6.6.5基于粒子滤波的SLAM算法标准PF是在SIS方法上加入重采样策略，也称为SIR粒子滤波，从序列重要性函数采样得到的样本就是粒子滤波中的粒子。Montemerlo首先将粒子滤波器用于SLAM领域，提出了FastSLAM算法。其核心思想是用Rao-Blackwellise分解将SLAM问题分离为线性状态的地图特征估计与非线性状态的路径估计，应用SIR粒子滤波器估移动计机器人的路径，地图则采用扩展卡尔曼滤波器更新，FastSLAM算法也称为RBPF-SLAM算法。6.6.5基于粒子滤波的SLAM算法根据贝叶斯公式以及环境特征估计间的独立性假设，FastSLAM对移动机器人位姿和环境地图的联合后验概率分布

分为移动机器人路径估计和地图估计

两部分，然后再将地图估计分解为N个相互独立的特征估计

，具体形式如下式所示：6.6.5基于粒子滤波的SLAM算法移动机器人路径估计

的近似表达式为:对于假设的近似后验概率的提议分布

，满足：从这个提议分布产生粒子，并赋权值:已知k-1时刻的粒子集

，从提议分布的状态空间中采样得到第i个粒子k时刻的位姿估计Xik，根据Xi1:k-16.6.5基于粒子滤波的SLAM算法和Xik得到第i个粒子k时刻的路径移动机估计Xi1:k，同时权值递推表达式为:FastsLAM2.0算法步骤具体如下所示：1.预测预测机器人k时刻的位姿分布，根据给定的系统控制向量uk和机器人的运动模型实现，同时计算在k时刻的位姿向量的预测均值和方差。6.6.5基于粒子滤波的SLAM算法2.数据关联将k时刻观测信息Zk和各个粒子在k-1时刻的地图估计应用极大化观测概率函数方法依次进行数据关联。3.获取提议分布采用EKF根据粒子的关联观测特征对粒子的先验分布

进行观测更新，计算各个粒子位姿向量在k时刻的滤波均值和方差，得各个粒子的提议分布4.移动机器人路径估计采用SIR粒子滤波器估计移动机器人的路径，获取k时刻6.6.5基于粒子滤波的SLAM算法表示移动机器人路径后验概率分布

的粒子集

。5.更新地图应用EKF根据粒子的关联观测特征更新各个粒子时刻的特征估计。对于没有关联上的观测特征，则将对应的观测特征作为新特征加入地图，并删除伪特征。6.7基于图优化的移动机器人SLAM算法图优化是把非线性系统用图(Graph)表达，根据图论知识对非线性系统状态进行估计的方式。图论中，图由顶点（Vertex）和连接顶点之间边（Edge）组成，图通常表示为G={V,E}，其中，V表示顶点集合，E表示边集合。图中的顶点表示非线性系统中状态值，顶点之间的边表示非线性系统中状态之间的约束关系。6.7基于图优化的移动机器人SLAM算法位姿Xk之间的约束关系满足移动机器人运动模型，位姿与观测的路标之间的约束关系满足移动机器人观测模型。位姿x1到x4以及路标p1、p2构成图G的顶点，各位姿之间以及位姿与路标之间都存在的约束关系构成图G的边。图优化SLAM是指图论知识求解SLAM中移动机器人位姿问题。6.7基于图优化的移动机器人SLAM算法图优化SLAM过程分为以下几个步骤：1.传感器数据获得。移动机器人通过携带的传感器采集外界环境信息，移动机器人通过码盘、惯性传感器等进行本体移动数据的读取。图优化SLAM框架图6.7基于图优化的移动机器人SLAM算法2.前端视觉里程计构成。视觉里程计指对传感器采集的相邻图像特征点进行提取，同时，根据机器视觉以及相机成像原理计算出相机位姿，该相机安装于移动机器人上，因为，可以确定出移动机器人位姿，同时，部分图像特征点形成局部地图。VO又称为前端。3.后端非线性优化。由于前端对移动机器人的位姿估计不可避免地存在误差，移动机器人需要将位姿估计通过回环检测等措施进行优化，得到全局一致的移动机器人位姿轨迹和地图。由于该过程在VO之后，又称为后端。6.7基于图优化的移动机器人SLAM算法4.回环检测，又称闭环检测。闭环检测主要解决移动机器人的位姿估计误差会随时间累积的问题，目的是通过前后帧图像间的关系对比来判断移动机器人走过的位置是否重合即回环。通过充分而且正确的回环检测，就可以消除累积误差，得到正确的移动机器人轨迹，构建出高精度地图。5.建图。根据移动机器人位姿与观测的关系，采用一定的表达方式，建立出环境的地图。6.7.1ORB-SLAM算法ORB-SLAM是一个完整的视觉SLAM系统，包括视觉里程计、跟踪、回环检测等步骤，是一种完全基于稀疏特征点的单目SLAM系统，同时还有单目、双目、RGBD相机的接口。其核心是使用ORB(OrintedFASTandBRIEF)作为图像的核心特征。ORB-SLAM是三个流程同时运行的，结构图如图6-7所示。第一个是跟踪，第二个是建图，第三个是闭环检测。跟踪是从图像中提取ORB