一种基于粒子滤波的SLAM优化方法与流程

本发明涉及机器人即时定位与地图构建，具体涉及一种基于粒子滤波的slam优化方法。

背景技术：

现有技术中机器人执行的各种任务，无论是定位、建图，或是识别等，其需要的环境与自身状态信息均来自于传感器。理想情况下，传感器的各种信息集合就可以代表机器人与所处环境的具体状态。但在实际使用中，由于传感器自身特性或环境限制，噪声时刻存在，机器人在执行任务时受到噪声干扰，则机器人所处环境的具体状态与实际所处环境的具体状态存在差异，造成机器人执行任务失败。

技术实现要素：

本发明目的在于提供一种基于粒子滤波的slam优化方法，解决机器人执行任务时收到噪声干扰导致机器人执行任务失败的问题。

名称解析；

slam(simultaneouslocalizationandmapping)，也称为cml(concurrentmappingandlocalization)，机器人即时定位与地图构建，或并发建图与定位。问题可以描述为：将一个机器人放入未知环境中的未知位置，是否有办法让机器人一边移动一边逐步描绘出此环境完全的地图，所谓完全的地图是指不受障碍行进到房间可进入的每个角落。

一种基于粒子滤波的slam优化方法，至少包括以下步骤；

步骤1，对机器人建立概率模型，所述建立概率模型包括运动概率学建模、传感器观测模型建立和栅格地图模型建立；

步骤2，概率模型结合粒子滤波对机器人即时定位与地图构建进行优化，优化过程包括初始化、采样、扫描匹配、权值计算和重采样；其中，优化过程引入多个变量；所述多个变量包括：

xt：描述机器人在全局坐标下的位置以及朝向的状态变量，具体为

ut：机器人控制向量，t时刻的控制向量表示经此控制驱动机器人在t-1时刻的位置状态由xt-1转换到了状态xt，且将两时刻内的位置变换看作控制输入；

m：地图状态变量，对环境信息的直观描述；

表示传感器在t时刻观测得到的第k个环境信息，由于本课题采用的是激光数据，k表示第k个激光光束。

经过长期的实践与研究，本申请的发明人发现，现有技术中机器人执行的各种任务中，由于传感器自身特性或环境限制，噪声时刻存在，机器人在执行任务时受到噪声干扰，则机器人所处环境的具体状态与实际所处环境的具体状态存在差异，造成机器人执行任务失败。本申请提供一种基于粒子滤波的slam优化方法，具体的，定义变量是为了更好的使用数学语言描述slam过程，进行概率模型建立是为了将机器人的实际运动与观测转换为概率的形式根据变量，基于粒子滤波和概率模型对slam进行优化则为了将机器人位置与地图分开进行估计，且机器人自动对位置进行确认至少包括初始化、采样、扫描匹配、权值计算和重采样，得到一个较好的估计结果即目标的真实状态。

优选的，所述运动概率学建模采用里程计运动模型，二维环境下，机器人在全局坐标下的位姿向量表示为(px,py,pθ)^t，则p(xt|xt-1,ut)表示在控制输入ut的作用下，位姿状态向量xt可能出现的概率；当机器人从a状态移动到b状态，颜色越深表示位置精度越高。

优选的，所述传感器观测模型建立基于激光雷达的可能性区域模型对观测信息进行建模。

优选的，所述传感器观测模型建立引入至少三种类型的噪声，至少三种类型的噪声包括：测量噪声、错误测量值噪声和随机测量噪声。

优选的，所述随机测量噪声包括当激光传感器击中半透明玻璃，或者声纳传感器接收到同波段声波时都会产生不固定的随机噪声，此随机噪声用均匀分布表示，如下式3-9所示：

根据传感器观测模型的误差即可计算出观测概率p(zt|xt,m)，计算公式3-10如下：

其中q表示每个激光数据的观测概率，zhit表示测量噪声出现的概率阈值，zrand表示随机噪声出现的概率阈值，zhit远大于zrand且两者和为1。

优选的，所述栅格地图模型建立采用占据栅格地图，具体的，将整个待估计二维空间表示为具有大小值的网格集合，网格的尺寸大小表示地图的分辨率，每个网格维护一个二进制值，0代表该网格处无障碍，1代表该网格坐标处有障碍。

优选的，所述初始化包括在slam算法起始即t＝0时刻开始执行，默认初始机器人位姿向量为(0,0,0)^t，初始化得到的地图表示为m0，初始化完成后的粒子内容为公式3-17，其中每个粒子的权值相同，均为1/m；

由式3-16可知，已知机器人位置时，仅根据传感器数据即可对地图进行更新，初始状态时对于一帧中的所有k组激光数据根据式3-10可计算得到激光光束端点在全局坐标系下的位置并初始化地图。

优选的，所述采样包括使用里程计运动模型进行采样，粒子中位置的更新如下式3-18所示，其中δ′trans，δ′rot1，δ′rot2分别代表了带误差的位置变换值：

优选的，所述扫描匹配采用基于可能性区域模型的扫描匹配算法。

优选的，所述权值计算与重采样包括第m个粒子的权值计算方式如下式3-22所示：

所述重采样采用自适应重采样技术决定重采样步骤何时发生，如下式3-23所示：

其中neff又被称为有效粒子数，当neff值小于某一给定阈值时进行重采样步骤，否则在权值计算完成后直接进行下一次算法迭代。

本发明具有如下的优点和有益效果：

本发明一种基于粒子滤波的slam优化方法，通过定义变量、概率模型建立以及基于粒子滤波和概率模型对slam进行优化得到目标的真实状态，避免机器人执行任务时收到噪声干扰导致机器人执行任务失败。

附图说明

此处所说明的附图用来提供对本发明实施例的进一步理解，构成本申请的一部分，并不构成对本发明实施例的限定。在附图中：

图1为本发明中slam马尔可夫过程示意图。

图2(a)为本发明中机器人全局位姿图。

图2(b)为本发明中机器人采样位姿图。

图3为本发明中里程计模型分解示意图。

图4为本发明中机器人所在地图更新示意图。

图5为本发明中提议分布采样示意图。

图6为本发明中权值计算示意图。

图7为本发明中重采样示意图。

图8(a)为本发明中仿真坐标示意图。

图8(b)为本发明中雷达观测示意图。

图9为本发明中机器人运动轨迹示意图。

图10为本发明中初始化地图示意图。

图11为本发明中粒子数为30的采样示意图。

图12为本发明中待匹配激光数据示意图。

图13(a)为本发明中粒子采样分布图。

图13(b)为本发明中扫描匹配粒子分布图。

图14(a)为本发明中扫描匹配距离误差对比示意图。

图14(b)为本发明中扫描匹配角度误差对比示意图。

图15为本发明中粒子滤波slam系统流程图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，下面结合实施例和附图，对本发明作进一步的详细说明，本发明的示意性实施方式及其说明仅用于解释本发明，并不作为对本发明的限定。

在以下描述中，为了提供对本发明的透彻理解阐述了大量特定细节。然而，对于本领域普通技术人员显而易见的是：不必采用这些特定细节来实行本发明。在其他实例中，为了避免混淆本发明，未具体描述公知的结构、电路、材料或方法。

在整个说明书中，对“一个实施例”、“实施例”、“一个示例”或“示例”的提及意味着：结合该实施例或示例描述的特定特征、结构或特性被包含在本发明至少一个实施例中。因此，在整个说明书的各个地方出现的短语“一个实施例”、“实施例”、“一个示例”或“示例”不一定都指同一实施例或示例。此外，可以以任何适当的组合和、或子组合将特定的特征、结构或特性组合在一个或多个实施例或示例中。此外，本领域普通技术人员应当理解，在此提供的示图都是为了说明的目的，并且示图不一定是按比例绘制的。这里使用的术语“和/或”包括一个或多个相关列出的项目的任何和所有组合。

在本发明的描述中，需要理解的是，术语“前”、“后”、“左”、“右”、“上”、“下”、“竖直”、“水平”、“高”、“低”“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明保护范围的限制。

实施例

本发明目的在于提供一种基于粒子滤波的slam优化方法，解决机器人执行任务时收到噪声干扰导致机器人执行任务失败的问题。

名称解析；

slam(simultaneouslocalizationandmapping)，也称为cml(concurrentmappingandlocalization)，机器人即时定位与地图构建，或并发建图与定位。问题可以描述为：将一个机器人放入未知环境中的未知位置，是否有办法让机器人一边移动一边逐步描绘出此环境完全的地图，所谓完全的地图是指不受障碍行进到房间可进入的每个角落。

一种基于粒子滤波的slam优化方法，至少包括以下步骤；

步骤1，对机器人建立概率模型，所述建立概率模型包括运动概率学建模、传感器观测模型建立和栅格地图模型建立；

步骤2，概率模型结合粒子滤波对机器人即时定位与地图构建进行优化，优化过程包括初始化、采样、扫描匹配、权值计算和重采样；其中，优化过程引入多个变量；所述多个变量包括：

xt：描述机器人在全局坐标下的位置以及朝向的状态变量，具体为

ut：机器人控制向量，t时刻的控制向量表示经此控制驱动机器人在t-1时刻的位置状态由xt-1转换到了状态xt，且将两时刻内的位置变换看作控制输入；

m：地图状态变量，对环境信息的直观描述；

表示传感器在t时刻观测得到的第k个环境信息，由于本课题采用的是激光数据，k表示第k个激光光束。

经过长期的实践与研究，本申请的发明人发现，现有技术中机器人执行的各种任务中，由于传感器自身特性或环境限制，噪声时刻存在，机器人在执行任务时受到噪声干扰，则机器人所处环境的具体状态与实际所处环境的具体状态存在差异，造成机器人执行任务失败。本申请提供一种基于粒子滤波的slam优化方法，具体的，定义变量是为了更好的使用数学语言描述slam过程，进行概率模型建立是为了将机器人的实际运动与观测转换为概率的形式根据变量，基于粒子滤波和概率模型对slam进行优化则为了将机器人位置与地图分开进行估计，且机器人自动对位置进行确认至少包括初始化、采样、扫描匹配、权值计算和重采样，得到一个较好的估计结果即目标的真实状态。

优选的，所述运动概率学建模采用里程计运动模型，二维环境下，机器人在全局坐标下的位姿向量表示为(px,py,pθ)^t，则p(xt|xt-1,ut)表示在控制输入ut的作用下，位姿状态向量xt可能出现的概率；当机器人从a状态移动到b状态，颜色越深表示位置精度越高。

优选的，所述传感器观测模型建立基于激光雷达的可能性区域模型对观测信息进行建模。

优选的，所述传感器观测模型建立引入至少三种类型的噪声，至少三种类型的噪声包括：测量噪声、错误测量值噪声和随机测量噪声。

优选的，所述随机测量噪声包括当激光传感器击中半透明玻璃，或者声纳传感器接收到同波段声波时都会产生不固定的随机噪声，此随机噪声用均匀分布表示，如下式3-9所示：

根据传感器观测模型的误差即可计算出观测概率p(zt|xt,m)，计算公式3-10如下：

其中q表示每个激光数据的观测概率，zhit表示测量噪声出现的概率阈值，zrand表示随机噪声出现的概率阈值，zhit远大于zrand且两者和为1。

优选的，所述栅格地图模型建立采用占据栅格地图，具体的，将整个待估计二维空间表示为具有大小值的网格集合，网格的尺寸大小表示地图的分辨率，每个网格维护一个二进制值，0代表该网格处无障碍，1代表该网格坐标处有障碍。

优选的，所述初始化包括在slam算法起始即t＝0时刻开始执行，默认初始机器人位姿向量为(0,0,0)^t，初始化得到的地图表示为m0，初始化完成后的粒子内容为公式1-17，其中每个粒子的权值相同，均为1/m；

由式3-16可知，已知机器人位置时，仅根据传感器数据即可对地图进行更新，初始状态时对于一帧中的所有k组激光数据根据式3-10可计算得到激光光束端点在全局坐标系下的位置并初始化地图。

优选的，所述采样包括使用里程计运动模型进行采样，粒子中位置的更新如下式3-18所示，其中δ′trans，δ′rot1，δ′rot2分别代表了带误差的位置变换值：

优选的，所述扫描匹配采用基于可能性区域模型的扫描匹配算法。

优选的，所述权值计算与重采样包括第m个粒子的权值计算方式如下式3-22所示：

所述重采样采用自适应重采样技术决定重采样步骤何时发生，如下式3-23所示：

其中neff又被称为有效粒子数，当neff值小于某一给定阈值时进行重采样步骤，否则在权值计算完成后直接进行下一次算法迭代。

具体的，为了更好的使用数学语言描述slam过程，我们定义如下变量(t时刻)：

xt：描述机器人在全局坐标下的位置以及朝向的状态变量，具体为

ut：机器人控制向量，t时刻的控制向量表示经此控制驱动机器人在t-1时刻的位置状态由xt-1转换到了状态xt。特别的，可以将两时刻内的位置变换看作控制输入；

m：地图状态变量，对环境信息的直观描述；

表示传感器在t时刻观测得到的第k个环境信息，由于本课题采用的是激光数据，k表示第k个激光光束；

从概率学的角度来看，slam问题主要估计的是在已知控制输入与环境观测信息下机器人位置以及地图环境的条件概率，如下式3-1所示：

p(xt,m|z1:t,u1:t)(3-1)；

其中z1:t与u1:t表示从机器人开始运动时刻起所有的观测数据与控制向量。实际上，slam算法是一个迭代的估计问题，3-1式的结果是t-1时刻的估计值p(xt-1,m|z1:t-1,u1:t-1)根据t时刻的控制输入ut和观测值zt计算得到，同时slam也是一个马尔可夫过程，如图1所示，在已知机器人位置的情况下，地图由某一位置状态下的观测数据构成。

概率模型建立，为了将机器人的实际运动与观测转换为概率的形式，需要对机器人运动、传感器观测、地图表示进行概率学建模。

运动概率学建模，运动概率学建模的过程就是将实际的控制输入转化为算法计算需要的运动学概率值的过程。目前使用最为广泛的是速度控制模型与里程计控制模型，本课题使用里程计运动模型。二维环境下，机器人在全局坐标下的位姿向量如图2(a)所示，表示为(px,py,pθ)^t，因此p(xt|xt-1,ut)表示在控制输入ut的作用下，位姿状态向量xt可能出现的概率。如图2(b)所示，机器人从a状态移动到b状态，由于误差的原因无法知道b状态的确切位置，颜色越深表示b状态位置的可能性越高。

对于里程计运动模型，在t-1到t的时间间隔内里程计反馈的为机器人位姿状态到的相对变换估计值，如下式3-2所示：

其中为了能够准确的计算状态转移概率，将ut分解为旋转-平移-旋转的步骤，如图3所示：

初始旋转为δrot1，平移为δtrans，第二个旋转为δrot2。每一对位置都有一个独特的参数向量(δrot1,δtrans,δrot2)^t。概率学运动模型假设这些参数由独立的噪声所扰动。针对这三个参数我们可以得到状态转移概率的计算过程:

计算控制输入变换参数：

计算由控制输入导致的状态估计变换参数：

计算状态转移概率：

其中参数αrr,αtr,αrt,αtt分别表示由角度变换引起的角度误差参数、平移变换引起的角度误差参数、角度变换引起的平移误差参数、平移变换引起的平移误差参数，getp(a,b)函数表示a在均值为0，方差为b的高斯分布下的概率值。最终的状态转移概率值即为3-5式中各个结果的乘积：

p(xt|xt-1,ut)＝p1·p2·p3(3-6)；

传感器观测模型建立。传感器观测概率模型种类较多，例如基于激光雷达的光束模型、可能性区域模型、相关性模型，基于特征识别传感器的特征测量模型等等。本申请将使用基于激光雷达的可能性区域模型对观测信息进行建模。

未经处理的原始激光雷达数据为k组距离值与角度值，距离值表示环境障碍物点与激光雷达之间的距离，角度值表示该障碍点与激光雷达正方向的夹角(通常激光雷达正方向与机器人正方向一致)。对于t时刻任意一组激光数据其中r代表距离，表示角度，为了计算激光终点在全局坐标系下的位置，我们需要知道雷达本体相对于机器人中心的安装位置(xsens,ysens)^t与机器人本身在世界坐标系下的估计位置通过坐标系间的变换关系我们可以得到式3-7：

其中与就代表了第个k激光光束终点在全局坐标系下的位置。值得注意的是仅当传感器观测到障碍物时式上式的结果才有意义，因此实际使用中一般都会设置r的范围，将大于某一阈值范围外的传感器数据舍弃掉。

为了计算观测概率，同运动模型类似，观测模型也引入了三种类型的噪声：

测量噪声。测量噪声使用高斯分布表示，对任意一组激光数据的端点坐标计算其与现有地图中最近障碍物坐标的欧式距离dist，则由测量噪声导致的传感器测量概率值如下式：

错误测量值噪声。对于激光雷达此类的距离传感器，较为典型的错误测量值就是返回激光数据的最大范围，但由于实际应用中对激光数据的预处理会将最大观测距离的数据过滤掉，因此计算时不做考虑。

随机测量噪声。当激光传感器击中半透明玻璃，或者声纳传感器接收到同波段声波时都会产生不固定的随机噪声，此随机噪声一般用均匀分布表示，如下：

根据上述观测模型的误差即可计算出观测概率p(zt|xt,m)，计算公式如下：

其中q表示每个激光数据的观测概率，zhit表示测量噪声出现的概率阈值，zrand表示随机噪声出现的概率阈值，一般情况下zhit远大于zrand且两者和为1。

相对于其他几种观测模型，可能性区域模型的优点在于其平滑性，当机器人位姿移动较小时，观测概率p(zt|xt,m)并不会发生大幅度的跳变，因此该模型也被应用在扫描匹配算法当中。

栅格地图模型建立。目前2d环境下的地图表示方式主要有拓扑地图，特征地图，栅格地图等。本课题使用的地图描述方式为占据栅格地图，将整个待估计二维空间表示为一定大小的网格(cell)集合，网格的尺寸大小表示地图的分辨率，每个网格维护一个二进制值，0代表该网格处无障碍(free),1代表该网格坐标处有障碍(occupied)。使用概率学估计占据栅格地图可抽象为对概率p(m|z1:t,x1:t)的计算。

值得注意的是，假定一副地图由1000个网格构成，那么由这些网格构成的地图数量有21000个，单独估计每幅地图的概率显然是不现实的，因此需单独的对每一个网格的占据概率进行估计：

由于占据网格概率的独特性质，可以使用下式3-12的形式简化概率计算，同时避免占据概率过于接近0或者1而导致错误估计情况的发生：

实际上，每进行一次估计时并不会更新地图内的所有网格，而是仅针对k组激光数据扫描过的网格展开计算，因此我们可以得到地图模型的更新过程：

获取每组激光数据的端点以及机器人与端点之间的地图网格坐标。

t时刻对于激光光束端点的网格占据值lt,i＝lt-1,i+locc，locc表示占据更新值：

t时刻对于机器人与激光雷达端点之间的网格占据值lt,i＝lt-1,i+lfree，lfree表示空闲更新值。

图4就表示了一部分激光数据的更新方式，三角形代表机器人，假定两台边线之间激光光束稠密，黑色区域网格为占据网格，白色区域网格为空闲网格，灰色区域网格为未知环境网格，特别的，未知区域网格p(mi)＝0.5，lt,i＝0。

粒子滤波基本概念

粒子滤波采用状态空间内的有限个样本来对结果后验概率进行估计，减少了高斯滤波器中对非线性系统的线性化步骤带来的误差。标准的粒子滤波是一个迭代的估计过程，t时刻系统的更新需要上一t-1时刻的粒子集xt-1，控制输入ut，观测值zt作为算法输入。具体的，更新过程主要分为采样，加权，重采样三个步骤，粒子集由待估计状态样本与粒子权值构成。

采样，采样过程表示t-1时刻的待估计状态在控制输入ut的作用下发生的改变，假设粒子总数为m，则对于单个粒子有：

其中m表示粒子集内粒子序号，p(xt|ut,xt-1)称为提议分布。对于t-1时刻的粒子集，经过t时刻的采样过程后，粒子集xt-1变为如下式3-14：

其中w表示每个粒子的权值，采样过程不会使权值发生任何变化。特别的，图5为根据提议分布进行的采样计算示意图：

其中黑色矩形表示粒子，粒子的个数为10个，从图5中可以看到粒子的分布遵循提议分布在状态空间中的分布形式，粒子越密集表示待估计状态落在该区域的可能性越高。

加权。经过采样步骤，已经得到了t时刻状态的m个初始估计粒子，加权步骤会引入传感器观测信息，权值的计算如下式3-15：

对应的，粒子权值计算的示意图如图6所示，粒子的权值越高，其对应的黑色矩形高度越高。

重采样。重采样也叫做重要性采样过程。t-1时刻的粒子集xt-1经过对提议分布采样，计算权值两步骤已经得到了加权粒子集重采样步骤将中权值较小的粒子舍弃，用较大权值粒子代替其位置，意味着重采样过程后，粒子集中粒子总数目并不会发生改变，某些权值较大的粒子会在粒子集中多次出现。重采样的示意图如图7所示，重采样完成后，对每个粒子的权值进行归一化处理，粒子的分布逐渐向目标分布接近。此时的粒子集已经可以代表目标分布。

对于标准粒子滤波算法来说，粒子数目越大，系统精度越高，但相应的计算时间会大大增加，同时频繁的重采样会使粒子的多样性减少，有限的粒子数目，极小的粒子多样性会使粒子滤波的估计结果出现较大的偏差，因为当t时刻的控制输入到来时，采样步骤得到的粒子无法代表提议分布的空间状态分布。但随着滤波算法不断迭代，不进行重采样或极少数的运行重采样步骤会使少数粒子维护较大的权值，这意味着大多数粒子的权值较小，滤波系统需要耗费大量的计算资源来处理更新对整个状态估计过程无用的粒子。综上所示，粒子数目的选择与重采样步骤的运行时机对整个估计系统的精度来说十分重要。

基于粒子滤波的slam算法优化

基于粒子滤波的激光slam算法核心在于将机器人位置与地图分开进行估计，每个粒子内存在一组机器人位置数据与地图数据，这意味着每次地图的更新都假设机器人位置已知且准确，如下3-16所示。

具体的，基于粒子滤波的slam算法主要步骤主要有初始化，粒子采样，扫描匹配，权值计算，重采样五个步骤，下文将依次对其进行详细描述。

为了直观描述算法效果使用了ros+gazebo+rviz的三维仿真环境，使用gazebo中的激光雷达模拟插件获取虚拟激光数据，使用双轮差分驱动插件获取里程计数据，并假定此数据为机器人真实位置数据，并通过对此数据添加噪声来验证估计结果。生成的栅格地图在rviz中显示，同时仿真使用键盘控制环境中的机器人进行移动，验证本申请所描述的算法效果与可行性。特别的，虚拟激光雷达的使用参数如表1所示，其中采样精度1弧度表示在-π～π的区间范围内共有360组激光数据，角度增益为1弧度。

仿真环境与激光雷达数据如图8(a)和图8(b)所示，机器人起始位置即为全局坐标系下的原点，且此时机器人朝向为x轴正方向，可以看到-π～π范围内的激光数据反应了机器人周围360度区域内的所有障碍物信息，如图8(a)和图8(b)所示。

表1仿真激光参数

为了方便说明基于粒子滤波的slam算法实现过程，机器人在图8(a)中以如图9所示的轨迹进行移动，算法步骤将结合此仿真过程进行说明。

初始化，slam算法起始即t＝0时刻开始执行初始化步骤，由于此时未接收到任何传感器数据，因此本申请默认初始机器人位姿向量为(0,0,0)^t，初始化步骤的目的有两个，一是根据粒子总数初始化粒子集，二是根据第一帧激光数据初始化环境地图，此地图将作为后续迭代估计算法的输入，初始化步骤得到的地图表示为m0。初始化完成后的粒子内容为式1-17，其中每个粒子的权值相同，均为1/m。

由式3-16可知，已知机器人位置时，仅根据传感器数据即可对地图进行更新，初始状态时对于一帧中的所有k组激光数据根据式3-10可计算得到激光光束端点在全局坐标系下的位置并初始化地图。具体的，设置地图分辨率为5cm，使用rviz观察初始化步骤得到的地图如图10所示，其中黑色区域表示机器人周围障碍物，对应图4中机器人四周墙壁，白色区域表示空闲无障碍，黑色区域表示环境未知，传感器无法获得此处数据。

采样。采样过程与标准粒子滤波算法类似，t时刻粒子代表的机器人位置根据控制输入ut与t-1时刻的机器人位置得到，对于本课题使用的里程计运动模型，采样过程相当于状态转移过程的逆，对位姿状态的采样实际为对误差分布进行采样，粒子中位置的更新如下式3-18所示，其中δ′trans，δ′rot1，δ′rot2分别代表了带误差的位置变换值：

如图11所示对于图9所示的仿真过程，取t＝10时刻进行分析。不考虑机器人角度数据，t＝9时的机器人真实位置坐标为(2.12,-1.10),估计位置结果为(2.16,-1.16)。t＝10时刻的机器人真实位置变为(2.17,-1.58)，相比于上一时刻其真实位置变换为(0.05，-0.48)，但由于噪声的影响，t时刻里程计获取到的机器人位置变换为(-0.03,-0.51)。t时刻的粒子集将由此变换值与t-1时刻的粒子集采样得到，其中，粒子数总数为30,黑色点表示粒子位置，每个粒子代表了一个真实状态可能出现的结果，t时刻的样本集转换为下式3-19的形式。

扫描匹配。粒子滤波系统的整体估计精度与粒子数目的选取密切相关，但对于slam算法，每个粒子内不仅包括机器人位置数据，还维护一份包含有多个个网格的栅格地图。粒子数目过大会使整个系统运算时间增加，算法可用性降低。为了减少粒子数目，优化粒子采样结果，本申请使用了基于可能性区域模型的扫描匹配算法。其核心思想在于将激光数据与已有地图进行匹配，进一步改善机器人位置，特别的，图12即表示了一组待匹配地图与激光数据。

粒子滤波算法中，在权值计算步骤前根据激光雷达数据与当前栅格地图对每个粒子内的机器人位置数据进行优化。如下式3-20所示：

对于图11中所示的粒子分布，扫描匹配后的结果如图13(a)和图13(b)所示，可以看到，扫描匹配步骤后粒子总体开始朝机器人真实位置处变化。

更进一步，由于t时刻的粒子是由t-1时刻的粒子通过采样计算得到，粒子误差会随时间累积。为了更好的描述扫描匹配的作用，此处假设不进行重采样，即粒子集内的样本不会发生删除，取代等操作。定义误差和e，表示t时刻每个粒子内机器人位置与真实位置的偏差值。如下式：

其中分别代表机器人的真实位置值。

采用图9中的仿真方式，记录有无扫描匹配过程的总误差和可以得到图14(a)和图14(b)，其中b线条表示无扫描匹配过程发生，a线条表示扫描匹配过程后的误差总和，可以看到扫描匹配算法极大的改善了粒子集中机器人位置分布情况，使其更接近机器人真实位置。但随着时间戳的增加，机器人不断移动，无论是否进行扫描匹配，粒子集代表的总体机器人位置误差都在不断增加，为了解决此问题，需要重采样步骤改善粒子内容，将误差较大的粒子舍弃，保留精准粒子。

权值计算与重采样。与标准粒子滤波算法中权值的计算略有不同，第m个粒子的权值计算方式如下式3-22所示：

上式中的概率值正是计算得到的观测概率值。同时本申请使用了自适应重采样技术决定重采样步骤何时发生，如下式：

其中neff又被称为有效粒子数，当neff值小于某一给定阈值时进行重采样步骤，否则在权值计算完成后直接进行下一次算法迭代。与标准粒子滤波中的重采样步骤类似，重采样发生时，每个粒子根据其权值的大小确定此粒子被保留的概率，小权值粒子会在重采样过程中由权值较大的粒子代替，保证粒子总数不发生变化。

综上所述，基于粒子滤波的slam算法流程图如图15所示。此slam算法可以得到一个较好的估计结果。

以上所述的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施方式而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。