移动机器人多传感器融合的SLAM新方法

马国力，朱晓春，陈子涛

（1.江苏省连云港工贸高等职业技术学校，连云港 222000；2.南京工程学院 江苏省先进数控技术重点实验室，南京 211100）

0 引言

SLAM（simultaneous localization and mapping，SLAM）是移动机器人实现精确自主导航的基础，定位与建图是机器人自主导航的关键技术。粒子滤波原理是以贝叶斯推理和重要性采样为基本框架的非参滤波器，它可以处理非线性、多峰分布等问题[1，2]。Thrun等人首先提出了基于粒子滤波的SLAM方法，将空间状态中的粒子赋予权值，得到机器人系统状态的后验概率分布，使定位更加准确，但随着粒子数的增加，地图构建复杂度也随之增加[3]。Murphy等人提出用Rao-Blackwillised particle filter（RBPF）方法来解决SLAM问题[4，5]。RBPF-SLAM将SLAM问题分解为机器人姿态估计和地图估计，使得粒子滤波的SLAM方法计算量大幅度减少。国内罗荣华等人提出基于自适应重采样的RBPF-SLAM方法，提高了SLAM的运算效率[6]。基于单一传感器的SLAM方法在室内环境条件差、机器人运动速度或转向过快时表现不稳定，例如：激光雷达传感器扫描观测距离有限、易受到环境中复杂几何结构影响。相机对机器人周围环境的照明条件有一定的要求。编码器电机经过长时间工作会产生累计误差。罗元等人融合里程计与雷达数据，优化建议分布函数，有效降低了预测阶段机器人位姿的不确定性，减少了SLAM所需粒子数量[7]。

本文在以上方法的基础上，将2D激光雷达、IMU、轮式里程计等多传感器进行融合，提出一种基于多传感器融合的SLAM新方法。首先利用IMU对里程计数据进行校正，建立机器人运动模型，再引入激光雷达观测数据修正机器人运动模型，联合优化建议分布，提高机器人位姿后验概率模型的精度。同时针对RBPF-SLAM算法存在粒子耗散问题，改进粒子重采样策略。为了验证本文算法的有效性，分别在仿真环境与实体环境进行试验。

1 基于多传感器融合的SLAM方法

1.1 RBPF-SLAM算法原理

基本的SLAM算法描述为：通过在给定传感器数据的情况下，利用机器人的运动模型p(xt|ut,xt-1)与观测模型p(zt|xt,m)，来预测机器人下一时刻的位姿状态[8]。基于RBPF（Rao-Blackwellized partical filter）的SLAM算法是通过蒙特卡洛方法解决机器人在大型复杂环境下的状态估计问题[9]。其基本原理是：计算机器人运动轨迹x1:t和地图m的联合后验概率p(x1:t|z1:t,u1:t)，通过贝叶斯滤波器将其分解为如式1所示的轨迹估计和地图估计两个后验概率的乘积，降低了估计值的维度。

RBPF-SLAM算法步骤如下：

1）粒子重要性采样。利用给定的重要性建议分布q(x1:t|z1:t,u1:t)采样得到N个初始粒子，每个粒子对应一个权值，其中表示t时刻第i个粒子的权值；粒子权值计算。根据重要性采样原则，粒子的权重定义为建议分布与目标分布的比值。粒子权值计算公式为：

归一化处理得：

重采样。根据粒子权值的大小，重新筛选粒子，淘汰小权值粒子，复制大权值粒子。为了减少重采样次数，用有效粒子数Neff来衡量粒子权值的退化程度：

当Neff>Nth，不做重采样；当Neff＜Nth，需要进行重采样。

4）状态估计与地图更新。根据所有的粒子信息，得到机器人的位姿状态x(i)

1:t，根据传感器的观测信息z(i)1:t，求出环境地图p(m(i)|x(i)

1:t,z1:t)。

1.2 多传感器融合SLAM算法

传统的RBPF-SLAM算法通过里程计与激光雷达传感器信息分别提供机器人运动模型与观测模型，单独将运动模型作为粒子滤波的建议分布，在估计机器人位姿时存在较大的累计误差[10]，使粒子计算的地图后验分布与真实环境有较大的偏差，存在粒子内存爆炸问题。本文针对上述问题，提出多传感器融合SLAM方法，第一步融合IMU-里程计建立机器人运动模型，第二步联合激光雷达观测信息进行二次融合，优化建议分布函数；第三步改进粒子重采样策略，减缓粒子耗散问题。

1.2.1 基于里程计-IMU-激光雷达的多传感器数据融合

在工作环境中，机器人SLAM算法通过里程计计算机器人线速度与角速度，获取机器人实时位姿。但里程计自身会出现一定的累计误差[11]。IMU传感器由加速度计、地磁计、陀螺仪组成，能提供稳定的机器人姿态信息，本文利用IMU传感器短时间内精度高、响应快的特点对里程计误差进行修正。

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

式(5)中Xt、Yt分别为移动机器人X、Y方向位移量；θ为机器人的姿态角。

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

系统的运动方程为：

设置阈值θ0，求出当前IMU传感器与里程计测得的姿态角度之差，进行差值判断，如果差值大于给定阈值，则采用IMU测出姿态角进行机器人的姿态估计，否则通过加权平均求得姿态角并对机器人进行姿态估计。IMU与里程计的融合过程如图1所示。

图1 IMU-里程计融合示意图

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

代入粒子权重计算公式可得：

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

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

1.2.2 改进粒子重采样策略

在得到改进的建议分布函数后，利用估计结果从所提出的分布函数中提取一组新的粒子。为了缓解粒子耗散的问题，本文改进粒子重采样策略。该策略通过构造新的分布函数，求出各粒子对应分布函数值，保留分布函数值在之间的粒子作为新粒子集，选取权值前三的粒子，最后在指定区间进行重采样，具体步骤如下；

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

2）构造分布函数H(wk)，求出粒子分布函数值。粒子权重越小，对应分布函数值越大。分布函数值在之间的粒子权重小，与机器人实际位姿偏差较大。故保留分布函数值小于的粒子，舍弃分布函数值在的粒子：

3）根据分布函数值进行筛选，取出权值排名前三的粒子，然后任意选取一个粒子，设其在新粒子集中对应的位置为A，分布函数值为的粒子在新粒子集中对应的位置为B，最后在[A，B]之间执行重采样。

1.3 基于多传感器融合的SLAM算法流程

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

步骤2：根据地图m(i)t-1，激光雷达传感器使用迭代最近邻（ICP）算法进行扫描匹配得到观测模型，融合机器人运动模型与观测模型得到新的建议分布函数。

步骤3：从步骤2中的建议分布中，进行粒子的采样。

步骤4：计算并更新粒子的重要性权值。

步骤5：根据改进的粒子重采样策略执行重采样操作。

步骤6：根据机器人位姿x(i)t和观测信息zt计算m(i)t，对地图进行更新。

2 实验与分析

2.1 算法建图仿真环境实验与分析

采用开源的计算机仿真数据集检验算法的性能，对两种SLAM方法进行仿真实验。在仿真实验中，建图仿真环境尺寸为300m×300m，机器人最大行驶速度为4m/s，激光雷达传感器最大观测范围为20m，速度误差σv=0.3m/s，距离误差为0.1m，角度误差为1°，为了保证客观性，所有仿真在同台主机完成，系统类型为64位操作系统，处理器为Intel i5 8300，运行内存为8.00GB，仿真平台为MATLAB 2016a。建图实验结果图如图3所示。

图3 仿真构建地图

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

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

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

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

2.2 算法建图实体环境实验与分析

2.2.1 移动机器人硬件平台

本文实验平台长度为45cm，宽度为38cm，高度为30cm，如图5所示。移动平台采用两轮差速底盘，可以实现全方位移动。传感器部分由激光雷达、惯性测量传感器（IMU）、轮式里程计组成，激光雷达扫描半径12米，可以按照设定的频率进行扫描匹配。IMU提供稳定的实时加速度与角速度，里程计记录机器人走过的路程并推算机器人的运动轨迹。控制系统部分包括微型工控机与电机驱动控制器。微型工控机装有Ubuntu 16.04系统，配备Kinetic版ROS并利用里程计、IMU信息和激光雷达数据信息在线实时构建地图，通过Rviz可视化软件显示地图。电机驱动控制器为STM32F1，主要负责电机的驱动以及部分传感器信息的收集。

图5 机器人平台

2.2.2 机器人软件系统

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

2.2.3 实体环境实验与分析

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

表1 构建一致性地图参数列表

图6 实验环境

图7 两种算法分别构建栅格地图

3 结语

本文提出了移动机器人多传感器融合SLAM新方法，研究了里程计数据与IMU数据融合的具体实现，得到更精准的机器人运动模型；在建议分布中融合激光雷达的观测信息，有效减少系统所需的粒子个数；通过改进粒子重采样策略，缓解重采样导致的粒子耗散问题。最后分别在仿真环境和实体环境进行建图实验，实验结果表明：本文SLAM方法使用更少运行时间同时地图构建的精度更优。