一种融合最小二乘法的粒子滤波SLAM方法与流程

本发明涉及机器人即时定位与地图构建，具体涉及一种融合最小二乘法的粒子滤波slam方法。

背景技术：

我国为推进实施“科技兴安”战略，调动社会力量参与安全生产科技攻关，进一步强化防范遏制重特大事故科技保障能力,在重点行业领域开展“机械化换人、自动化减人”科技强安专项行动,重点是以机械化生产替换人工作业、以自动化控制减少人为操作，大力提高企业安全生产科技保障能力。

在机器人的定位与地图构建中，通常情况下使用粒子滤波的slam算法可以得到一个较好的估计结果，但由于采样过程对里程计数据的严重依赖，导致当机器人里程计数据误差较大时，基于里程计的提议分布与目标分布严重不符，后续进行的扫描匹配，权值计算等步骤毫无意义。且由于粒子滤波系统本身的复杂度较高，当机器人移动速度过快，t-1至t时刻的时间段内机器人位置已经有了较大变化，而此时算法正在执行t-1时刻的估计过程，这种现象会使未知环境下的扫描匹配过程精度下降。目前广泛应用研究的slam系统大多专注于轮式移动机器人，且要求机器人自身携带定位传感器，如编码器，陀螺仪等，并由此计算得到机器人运动里程。但此类算法严重依赖里程计的精度，当地面有坑洼或轮子发生漂移时，里程计等传感器误差较大，极限情况下机器人实际位置并没有发生改变，但里程计数据已经显示机器人移动一段距离，这种情况不仅会使slam算法无法得到机器人的准确位置，创建的栅格地图也会发生撕裂、扭曲等现象。

技术实现要素：

本发明目的在于提供一种融合最小二乘法的粒子滤波slam方法，该方法不需要过度依赖里程计数据作为估计初值，本申请解决的技术问题为：机器人在地图中的位置数据采样严重依赖里程计数据。

本发明通过下述技术方案实现：

一种融合最小二乘法的粒子滤波slam方法，包括线程ⅰ和线程ⅱ，所述线程ⅰ为采用粒子滤波获取机器人在地图中的位置，线程ⅱ为采用最小二乘匹配获取机器人在地图中的位置，具体步骤包括：

步骤1，t＝0时，线程ⅰ和线程ⅱ同时获取雷达数据得到机器人在地图中的初始位置，此后，机器人开始移动；

步骤2，当机器人移动距离超过设定阈值时，进入t＝1时刻，线程ⅰ中粒子滤波算法使用里程计运动模型对粒子进行采样得到的样本代替地图内机器人位置，并根据权值对粒子进行排序；

步骤3，在t＝0～t-1时间段内，线程ⅱ同步获取雷达数据得到机器人位置x1′，则可得到粒子集内权值最大的n个粒子，使用最小二乘法对x1′采样得到的样本代替t＝0～t-1时间段内机器人位置即对局部地图重置，得到扩充后的粒子集，粒子集内粒子的总数变为m+n；

步骤4，线程ⅱ对粒子集进行扫描匹配、权值计算、重采样完成更新，其中权值最大的粒子代表的机器人在地图中的位置，即代表机器人此时的位置与环境地图数据；

步骤5，线程ⅱ对粒子集进行排序，舍弃权值较小的n个粒子，粒子总数变为m，由于线程ⅰ在t＝1时刻地图内机器人位置更新已经完成，线程ⅱ内对局部地图重置，待下一个t＝2时刻到来时继续执行步骤2～5，则线程ⅰ的一个更新时刻内线程ⅱ会进行多次更新。

通过长期的研究和实践，本申请的发明人发现，在机器人的定位与地图构建中，由于数据机器人移动位置采样过程对里程计数据的严重依赖，当地面有坑洼或轮子发生漂移时，里程计等传感器误差较大，极限情况下机器人实际位置并没有发生改变，但里程计数据已经显示机器人移动一段距离，这种情况不仅会使slam算法无法得到机器人的准确位置，创建的栅格地图也会发生撕裂、扭曲等现象。本申请提供一种融合最小二乘法的粒子滤波slam方法，包括线程ⅰ和线程ⅱ双线程执行以获取机器人在地图中的位置，由于线程ⅱ所使用的粒子滤波算法耗时长，故在t-1～t时间间隔内线程ⅱ已经进行多次更新。线程ⅱ内的最小二乘匹配算法的目的就是计算t-1～t时间内的机器人相对移动位置，同时本申请使用了局部地图的方式，当线程ⅰ内粒子滤波系统更新完成后局部地图重置。其核心在于每次迭代过程中，使用最小二乘匹配计算得到的机器人位置对粒子集进行扩充，当一次更新间隔内里程计误差较大时可以有效将部分粒子限制在真实分布附近。

进一步的，所述线程ⅰ对机器人位置进行初始化的方式包括：默认初始机器人位姿向量为(0,0,0)^t，采用ros、gazebo和rviz的三维仿真环境，并使用gazebo中的激光雷达模拟获取虚拟激光数据，根据第一帧激光数据初始化环境地图。

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

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

其中，为机器人观测概率值；所述重采样采用自适应重采样技术决定重采样步骤何时发生，如下式3-23所示：

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

进一步的，所述观测概率值通过建立观测模型获取。

进一步的，所述建立观测模型包括采用激光雷达的可能性区域模型对观测信息进行建模。

进一步的，为了获取所述观测概率值，观测模型引入了测量噪声和随机测量噪声。

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

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

进一步的，观测概率值p(zt|xt,m)的计算公式3-10如下：

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

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

每次迭代过程中，使用最小二乘匹配计算得到的机器人位置对粒子集进行扩充，当一次更新间隔内里程计误差较大时可以有效将部分粒子限制在真实分布附近，避免过度依赖里程计数据作为估计初值。

附图说明

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

图1为本发明的流程图。

图2为本发明中线程ⅰ的粒子滤波初始化地图示意图。

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

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

图5a为本发明中粒子采样分布图。

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

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

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

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

图8为本发明中线程ⅰ和线程ⅱ更新地图的时间轴示意图。

图9a为本发明中里程计漂移示意图。

图9b为本发明中里程计漂移结果图。

图10a为本发明中局部地图位置更新误差示意图。

图10b为本发明中局部地图角度更新误差示意图。

具体实施方式

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

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

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

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

实施例

一种融合最小二乘法的粒子滤波slam方法，包括线程ⅰ和线程ⅱ，所述线程ⅰ为采用粒子滤波获取机器人在地图中的位置，线程ⅱ为采用最小二乘匹配获取机器人在地图中的位置，具体步骤包括：

步骤1，t＝0时，线程ⅰ和线程ⅱ同时获取雷达数据得到机器人在地图中的初始位置，此后，机器人开始移动；

步骤2，当机器人移动距离超过设定阈值时，进入t＝1时刻，线程ⅰ中粒子滤波算法使用里程计运动模型对粒子进行采样得到的样本代替地图内机器人位置，并根据权值对粒子进行排序；

步骤3，在t＝0～t-1时间段内，线程ⅱ同步获取雷达数据得到机器人位置x1′，则可得到粒子集内权值最大的n个粒子，使用最小二乘法对x1′采样得到的样本代替t＝0～t-1时间段内机器人位置即对局部地图重置，得到扩充后的粒子集，粒子集内粒子的总数变为m+n；

步骤4，线程ⅱ对粒子集进行扫描匹配、权值计算、重采样完成更新，其中权值最大的粒子代表的机器人在地图中的位置，即代表机器人此时的位置与环境地图数据；

步骤5，线程ⅱ对粒子集进行排序，舍弃权值较小的n个粒子，粒子总数变为m，由于线程ⅰ在t＝1时刻地图内机器人位置更新已经完成，线程ⅱ内对局部地图重置，待下一个t＝2时刻到来时继续执行步骤2～5，则线程ⅰ的一个更新时刻内线程ⅱ会进行多次更新。

通过长期的研究和实践，本申请的发明人发现，在机器人的定位与地图构建中，由于数据机器人移动位置采样过程对里程计数据的严重依赖，当地面有坑洼或轮子发生漂移时，里程计等传感器误差较大，极限情况下机器人实际位置并没有发生改变，但里程计数据已经显示机器人移动一段距离，这种情况不仅会使slam算法无法得到机器人的准确位置，创建的栅格地图也会发生撕裂、扭曲等现象。本申请提供一种融合最小二乘法的粒子滤波slam方法，包括线程ⅰ和线程ⅱ双线程执行以获取机器人在地图中的位置，由于线程ⅱ所使用的粒子滤波算法耗时长，故在t-1～t时间间隔内线程ⅱ已经进行多次更新。线程ⅱ内的最小二乘匹配算法的目的就是计算t-1～t时间内的机器人相对移动位置，同时本申请使用了局部地图的方式，当线程ⅰ内粒子滤波系统更新完成后局部地图重置。其核心在于每次迭代过程中，使用最小二乘匹配计算得到的机器人位置对粒子集进行扩充，当一次更新间隔内里程计误差较大时可以有效将部分粒子限制在真实分布附近。

针对粒子滤波严重依赖里程计数据，本申请提出了一种融合最小二乘法的粒子滤波slam方法。其核心在于每次迭代过程中，使用最小二乘匹配计算得到的机器人位置对粒子集进行扩充，当一次更新间隔内里程计误差较大时可以有效将部分粒子限制在真实分布附近。算法整体流程如图1所示，包括线程ⅰ和线程ⅱ，所述线程ⅰ为采用粒子滤波获取机器人在地图中的位置，线程ⅱ为采用最小二乘匹配获取机器人在地图中的位置，由于线程ⅱ所使用的粒子滤波算法耗时长，故在t-1～t时间间隔内线程ⅱ已经进行多次更新。线程ⅱ内的最小二乘匹配算法的目的就是计算t-1～t时间内的机器人相对移动位置，同时本申请使用了局部地图的方式，当线程ⅰ内粒子滤波系统更新完成后局部地图重置。具体的执行步骤如下：

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

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

公式3-10中，q表示每个激光数据的观测概率，zhit表示测量噪声出现的概率阈值，zrand表示随机噪声出现的概率阈值，一般情况下zhit远大于zrand且两者和为1，具体的，设置地图分辨率为5cm，使用rviz观察初始化步骤得到的地图如图2所示，其中黑色区域表示机器人周围障碍物，白色区域表示空闲无障碍。最小二乘匹配线程则使用机器人初始位置更新地图。

2)当机器人移动距离超过设定阈值时，进入t＝1时刻，线程ⅰ中粒子滤波算法根据里程计信息对粒子进行采样，并根据权值对粒子进行排序；

3)由于在t＝0～t-1时间段内，线程ⅱ同步计算机器人位置，则可以得到线程中的机器人位置x1′，获取粒子集内权值最大的n个粒子，对x1′采样得到的样本代替其内机器人位置，得到扩充后的粒子集，粒子集总数变为m+n。

4)对粒子集进行扫描匹配、权值计算、重采样完成更新，其中权值最大的粒子所代表的机器人位置以及地图即表示了机器人此时的位置与环境地图数据。

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

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

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

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

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

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

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

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

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

5)对粒子集进行排序，舍弃权值较小的n个粒子，粒子总数变为m。由于线程ⅰ在t＝1时刻的更新已经完成，线程ⅱ内局部地图重置，待下一个t＝2时刻到来时继续执行2～5步骤。具体的，两线程的更新时间轴图8所示，线程ⅰ的一个更新时刻内线程ⅱ会进行多次更新。

为了描述融合后算法的效果，使用图9a所示的特殊情况来代替轮子打滑过程，使用键盘控制机器人向墙面移动，由于轮子持续转动导致里程计数据不断累积而机器人真实位置不发生变化。表1即描述了此情况下t时刻粒子的分布情况，表1表示里程计漂移时标准粒子分布，暂时不考虑角度数据，t-1时刻的机器人位置为(2.37,1.97)，t时刻的机器人位置为(2.71.1.97)取粒子数目为30。

表1

从表1可以看出，由于t-1时刻进行的扫描匹配步骤，导致粒子集内机器人位置分布较为集中，而在t-1～t时刻内由于漂移现象里程计误差较大，t时刻采样得到的粒子中很难找到与真实位置相近的粒子，这种现象会直接导致定位精度下降，地图发生撕裂，如图9b所示。针对上述情况，使用融合最小二乘匹配的slam算法，粒子总数设置为25，每次采样时扩充的粒子数为5，t时刻的粒子分布如下表2所示,表2表示里程计漂移时改进粒子滤波算法粒子分布，可以看到，由于线程ⅱ中最小二乘匹配的作用，扩充后序号为26～30的5个粒子并没有根据里程计数据进行采样，当其他粒子无法表示真实机器人位置时，仍能得到一个较好的估计结果。

表2

同样控制机器人沿着图6中描述的轨迹移动，记录每次局部地图更新结果并与机器人真实移动量进行对比，x方向、y方向、角度的误差图像如图10a和图10b所示，可以看到，随着机器人移动，局部地图更新位置误差基本可以保持在10cm内，角度误差基本保持在0.1弧度内，精度较高。

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