一种基于多传感器融合的移动机器人室内定位建图方法

摘要

本发明公开了一种基于多传感器融合的移动机器人室内定位建图方法，将激光雷达、惯性测量单元(IMU)、轮式里程计进行数据融合，建立了基于里程计与IMU融合的机器人运动模型，利用激光雷达观测信息融合运动模型优化建议分布函数，解决系统预测分布误差大与粒子内存爆炸问题；给出粒子重采样策略新方法，保持粒子多样性，减缓粒子耗散问题，在建图效率与建图精度上有明显的提升。

说明书

技术领域

本发明涉及智能化技术领域，特别是涉及一种基于多传感器融合的移动机器人室内定位建图方法。

背景技术

SLAM(simultaneous localization and mapping,SLAM)是移动机器人实现精确自主导航的基础，定位与建图是机器人自主导航的关键技术。粒子滤波原理是以贝叶斯推理和重要性采样为基本框架的非参滤波器，它可以处理非线性、多峰分布等问题。Thrun等人首先提出了基于粒子滤波的SLAM方法，将空间状态中的粒子赋予权值，得到机器人系统状态的后验概率分布，使定位更加准确，但随着粒子数的增加，地图构建复杂度也随之增加。Murphy等人提出用Rao-Blackwillised particle filter(RBPF)方法来解决SLAM问题。RBPF-SLAM将SLAM问题分解为机器人姿态估计和地图估计，使得粒子滤波的SLAM方法计算量大幅度减少，提高了SLAM系统的效率。国内罗荣华等人提出基于自适应重采样的RBPF-SLAM方法，提高了SLAM系统的运算效率，罗元融合里程计与雷达数据，优化建议分布函数，有效降低了预测阶段机器人位姿的不确定性，有效缓解了SLAM所需粒子数量。

但现有技术中基于单一传感器的SLAM方法在室内环境条件差、机器人运动速度或转向过快时表现不稳定，例如：激光雷达传感器扫描观测距离有限、易受到环境中复杂几何结构影响。相机对机器人周围环境的照明条件有一定的要求，编码器电机经过长时间工作会产生累计误差，传统的RBPF-SLAM算法中存在粒子建议分布误差大、粒子消耗、算法运行时间长等问题，从而造成现有的移动机器人室内定位建图方法存在预测分布误差大与粒子内存爆炸问题。

发明内容

为了克服上述现有技术的不足，本发明提供了一种基于多传感器融合的移动机器人室内定位建图方法，主要用于室内地图的构建，本方法在RBPF-SLAM方法的基础上，将2D激光雷达、IMU、轮式里程计等多传感器相结合，首先利用惯性测量传感器(IMU)对里程计数据进行校正建立机器人运动模型，再引入激光雷达观测数据修正机器人运动模型，联合优化建议分布，提高机器人位姿后验概率模型的精度；同时针对RBPF-SLAM算法粒子耗散问题，提出粒子重采样新策略。

本发明所采用的技术方案是：一种基于多传感器融合的移动机器人室内定位建图方法，包括：

采用惯性测量传感器对里程计数据进行校正，并建立移动机器人运动模型；

采用激光雷达观测数据修正所述移动机器人运动模型，形成优化后的混合建议分布；

基于所述混合建议分布，进行粒子采样，计算并更新粒子的重要性权值，通过本文给出的重采样新策略执行重采样操作；根据移动机器人位姿和观测信息进行状态估计和地图更新。

其中，所述惯性测量传感器由加速度计、地磁计、陀螺仪组成，能提供稳定的机器人姿态信息；

其中，所述激光雷达传感器使用迭代最近邻ICP算法进行扫描匹配得到观测模型。

其中，所述粒子重采样新策略，具体步骤如下：

步骤1：对粒子权值进行降序排序；

步骤2：构造分布函数H(w

步骤3：根据分布函数值进行筛选，取出权值排名前三的粒子，然后任意选取一个粒子，设其在新粒子集中对应的位置为A，分布函数值为

传统的RBPF-SLAM方法通过里程计与激光雷达传感器信息提供机器人运动模型与观测模型，里程计模型作为粒子滤波的建议分布，在估计机器人位姿时存在较大的累计误差，使粒子计算的地图后验分布与真实环境有较大的偏差，存在粒子内存爆炸问题；针对这些问题，本发明提出融合IMU-里程计建立机器人运动模型，同时联合激光雷达观测信息优化建议分布函数；提出粒子重采样新策略，减缓粒子耗散问题。

本发明还提供一种移动机器人系统，所述系统采用如上所述的一种基于多传感器融合的移动机器人室内定位建图方法。

本发明还提供一种移动机器人，所述机器人包括如上所述的移动机器人系统。所述移动机器人的传感器由激光雷达、惯性测量传感器(IMU)、轮式里程计组成。

与现有技术相比，本发明的有益效果是：

将激光雷达、惯性测量单元(IMU)、轮式里程计等传感器进行数据融合，得到一种多传感器融合的SLAM新方法,里程计数据与IMU数据通过扩展卡尔曼滤波融合的具体实现，得到更精准的机器人运动模型；在建议分布中融合激光雷达的观测信息，有效减少系统所需的粒子个数；提出了粒子重采样新策略，缓解重采样导致的粒子耗散问题。

附图说明

图1为：IMU-里程计融合示意图；

图2为：观测模型与运动模型的似然函数分布；

图3为：仿真试验构建地图，其中(a)传统算法构建仿真地图(b)本文多传感器融合算法构建仿真地图；

图4为：机器人误差X、Y轴误差估计曲线；

图5为：实体试验用移动机器人；

图6为：实体试验环境，其中(a)传统算法构建仿真地图,(b)本文多传感器融合算法构建仿真地图；

图7为：实体试验结果，其中，图7(a)为常规算法构建的实验室栅格图，图7(b)为本文多传感器融合算法构建的实验室栅格图，图7(c)为常规算法构建的办公区域栅格地图，图7(d)为本文多传感器融合算法构建的办公区域栅格地图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。

需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。

下面结合附图和具体实施例对本发明作进一步说明，但不作为本发明的限定。

实施例1

本发明的的一种基于多传感器融合的移动机器人室内定位建图方法，具体实施方式包括以下步骤：

步骤1：里程计与IMU数据融合，使用EKF方法融合里程计与IMU数据建立机器人运动模型；

在工作环境中，机器人SLAM算法通过编码器计算机器人线速度与角速度，获取机器人实时位姿。由于基于编码器的里程计运动模型会因为轮胎打滑漂移产生一定误差，误差随着时间的延续而增大[11]。IMU传感器由加速度计、地磁计、陀螺仪组成，能提供稳定的机器人姿态信息，本文利用IMU传感器短时间内精度高、响应快的特点对里程计误差进行修正。

针对移动机器人，对采集到的轮式里程计数据以及IMU数据进行建模，其空间状态量x

x

t+1时刻的位姿可表示为：

x

公式(2)中，

系统的运动方程为：

x

即：

x

公式(1)、公式(2)、公式(3)、公式(4)中X

设置阈值ψ

IMU传感器与里程计的融合过程如图1所示，具体过程如下：

(1)编码器输出线速度与角速度信息；

(2)IMU传感器内部加速度计与地磁计通过互补滤波进行融合，输出IMU线速度信息，陀螺仪提供IMU角速度信息。通过扩展卡尔曼滤波(EKF)将陀螺仪、地磁计、加速度计进行IMU内部融合，更新机器人观测方程与状态方程。

(3)通过扩展卡尔曼滤波融合编码器与IMU输出信息，得到精准的机器人位姿估计。

步骤2：多传感器融合混合建议分布，根据地图

传统的RBPF-SLAM算法中通过里程计的信息构建SLAM系统的运动模型。针对里程计易受外界信息扰动的特点，本方法将精确的激光雷达数据融合到建议分布函数中，通过将里程计与IMU数据融合得到的运动模型与基于激光雷达的观测模型联合，获得基于多传感器融合混合建议分布；

其中，混合建议分布函数为：

代入粒子权重计算公式：

可得基于多传感器融合混合建议分布，

公式(5)、公式(6)和公式(7)中，x代表机器人位姿信息；m代表地图；z代表观测信息；u代表控制输入信息；t代表t时刻；p代表概率分布(不需做描述)；

对似然分布函数即机器人观测模型与运动模型的概率分布进行分析，如图2所示，激光信息匹配的重要性权重方差非常小，加入精确的激光雷达扫描匹配信息来表示建议分布，则可以修正基于里程计信息的机器人运动模型，将采样范围限制在一个相对较小得到区域内，如图2所示，融入激光信息的建议分布能更接近真实目标分布，用较少的粒子就可以表示机器人位姿的后验概率分布，有效减少了所需粒子的数量。

步骤3：根据步骤2中的混合建议分布，进行粒子的采样；计算并更新粒子的重要性权值；根据重采样新策略执行重采样操作；根据机器人位姿

在得到改进的建议分布函数后，利用估计结果从所提出的分布函数中提取一组新的粒子，为了缓解粒子耗散的问题，本方法给出粒子重采样新策略，具体步骤如下；

1)对粒子权值进行降序排序。

2)构造分布函数H(w

公式(8)中，w

3)根据分布函数值进行筛选，取出权值排名前三的粒子，然后任意选取一个粒子，设其在新粒子集中对应的位置为A，分布函数值为

为了验证该方法的有效性，以下分别在仿真环境与实体环境进行试验。

测试例1：仿真环境试验

采用开源的计算机仿真数据集检验算法的性能，对两种SLAM方法进行仿真试验。在仿真试验中，建图仿真环境尺寸为300*300m，机器人最大行驶速度为4m/s，激光雷达传感器最大观测范围为20m，速度误差σ

在仿真建图试验中，传统算法平均建图时间为375s，最低粒子数为40。本文算法需要平均建图时间为302s，最低粒子数为25。图3(a)和图3(b)分别为传统算法与本文基于多传感器融合SLAM算法构建的仿真环境地图。

试验结果分析：当运行时间较长、粒子数较多时，传统算法生成的地图出现偏差，精度差。本文算法则维持了粒子多样性，有效较少了粒子耗散，提高了算法的运行效率，可以获取机器人精确的实时姿态，有效提高了地图的精度。

为更好的验证本文算法的性能，使用MATLAB绘制机器人在仿真环境下的X、Y轴误差曲线。曲线如图4所示。试验结果分析如下：本文基于多传感器融合的SLAM算法在X、Y轴的误差比传统RBPF算法误差更小，更接近真实状态。随着时间的增加，总体上传统算法的误差曲线波动幅度较大，表明本文算法在位姿估计时的精度高于传统算法。

测试例2：实体环境试验——应用于移动机器人

试验用移动机器人长度为45cm，宽度为38cm，高度为30cm，如图5所示。移动机器人采用两轮差速底盘，可以实现全方位移动。

移动机器人传感器部分由激光雷达、惯性测量传感器(IMU)、轮式里程计组成，激光雷达扫描半径12米，可以按照设定的频率进行扫描匹配。IMU提供稳定的实时加速度与角速度，里程计可以记录机器人走过的路程并推算机器人的运动轨迹。

移动机器人控制系统部分包括微型工控机与电机驱动控制器，微型工控机装有Ubuntu 16.04系统，配备Kinetic版ROS并利用里程计、IMU信息和激光雷达数据信息在线实时构建地图，通过Rviz可视化软件显示地图。电机驱动控制器为STM32F1主要负责电机的驱动以及部分传感器信息的收集。

移动机器人操作系统采用ROS系统，ROS系统是一个面向机器人的软件平台；ROS分为三个级层：文件系统级、计算图级、开源社区级[12]；在ROS中，节点(NODE)为最小的进程单位，节点包括发布端与接收端，系统通过节点实现具体功能，如传感器数据获取、传输、发布；SLAM通过节点将传感器数据传输至地图模块，实现机器人定位与建图功能。

在实际情况下，机器人的真实运动环境比仿真环境更加复杂，存在许多不确定因素。本试验利用本文SLAM算法和传统RBPF-SLAM进行SLAM实体试验。试验环境为南京工程学院科创中心实验室和办公区域的外部走廊，如图6所示，每个环境重复10次试验，试验结果如图7所示，图7(a)为常规算法构建的实验室栅格图，图7(b)为本文算法构建的实验室栅格图，图7(c)为常规算法构建的办公区域栅格地图，图7(d)为本文算法构建的办公区域栅格地图。由表1可以看出，传统RBPF-SLAM算法和本文算法在实体环境建图时，所需的粒子数量少于传统算法，缩短了构建地图的时间，极大地提高了算法的效率；由图6可以看出，由于传统算法只使用里程计作为建议分布，里程计的累积误差随着时间的推移越来越大，机器人绕行一段时间后，构建地图出现丢失和假墙现象，而当使用本文基于多传感器融合的SLAM算法时，所构建的地图相对稳定准确。

表1：实体环境试验试验结果

通过以上分别在仿真环境和实体环境进行移动机器人建图试验，试验结果表明：本文算法使用更少运行时间同时地图构建的精度更优。