基于扩展卡尔曼滤波的2D激光SLAM研究

王 博，张国伟，卢秋红，徐晓康，疏雅丽

（1.上海电力大学自动化工程学院，上海 200082；2.上海合时智能科技有限公司，上海 201100）

0 引言

随着国家科技的发展以及战略方案的引导，无人车、无人机等智能移动机器人设备的应用场景日益广泛，涵盖了工业、农业、军事、日常使用等方面。无人清洁洗地车作为移动机器人的一个分支，在国家提出的“中国制造2025”大战略背景的情况下诞生。放眼环境卫生领域，人们对环境卫生的要求在不断提高，加之近年来人力成本在逐年上升，无人智能清洁设备取代传统方式进行清洁作业将成为发展的主流趋势，人工智能和清洁设备的结合，也是技术和思维融合的一个过程。针对一些大型公共场所如商场、停车场、展览馆等，如果配置较多的作业人员进行场地清洁，人工成本势必会大大增加。而使用智能清洁车进行作业，预先设定工作任务，不受时间和人员等不稳定因素的限制，有效地提高了工作效率。

无人智能清洁车中核心技术就是无人驾驶概念，这也使得同时定位与地图构建（SLAM）技术成为热点问题。SLAM 问题最初是在1988 年由Smith 等人提出。SLAM 作为移动机器人的核心技术，在过去的几十年里发展迅速，并且已经完成了很多开源算法以及系统构建。激光SLAM 方法主要基于滤波器和图优化两种求解方法，其中基于滤波器的方法主要包含了2002 年的Fast-SLAM，2007 年的Gmapping 等开源算法。此后主要的研究方向转向了基于图优化的方法，主要包含了2010 年的KartoSLAM，2016年的Gartograph等开源算法。

卡尔曼滤波状态估计在SLAM问题中主要作为传感器数据融合手段。激光传感器可靠性高精度高且具有深度信息，但其垂直分辨率低只能获得稀疏点云，并且价格昂贵。视觉传感器可以获得环境语义动态等丰富的信息，并且可以实现人脸识别、物体追踪等扩展功能，但其受光照、运动等噪声影响较大，鲁棒性较差。超声波传感器具有短距离测量高精度的特点，由于其在空气中传播损失较大，无法在大范围环境稳定工作。IMU 传感器模块集成陀螺仪、加速度计、磁场仪等可以提供高精度的姿态信息。编码器里程计可以获得机器的准确位置信息，但增量式缺少位置记忆功能。因此多传感器的融合方法综合各种传感器优势，有效提高了SLAM问题中移动机器人定位精度，以及在复杂环境的噪声干扰下的鲁棒性。目前，SLAM 问题中的多传感器融合技术也是国内外学者的研究热点，2019 年张艳国等提出了IMU融合激光构建稀疏点云地图方法，依据历史点云记录，有效提高了传感器对环境信息的获取。2018 年潘爽等提出了扩展卡尔曼滤波进行联接式的IMU 以及声呐传感器融合方案解决水下SLAM 技术，有效提高了水下机器人自主导航的定位精度。本文基于上海合时智能科技公司的无人清洁车作为研究硬件平台，对无人车进行SLAM 数学模型建立，在此基础上完成基于扩展卡尔曼滤波的多传感器融合理论以及程序实现，同时在地下车库完成实际运行试验并对结果进行数据分析说明。

1 SLAM系统

1.1 数据模型及问题描述

SLAM 问题可以描述为：无人车在未知环境下开始运行，在运行过程中通过自身位置的估算以及路标观测信息，构建出一个增量式的地图，在此基础上可以实现机器人自身定位以及导航。根据该问题可以定义两个变量分别为运动状态量以及观测量：

其中，状态变量指每一个时刻下无人车运动传感器的数据，包括位置速度偏航等信息，为噪声干扰。观测量指当前状态下相对于路标点的观测值，对于不同传感器有所不同，为噪声干扰。

此时需要完成对无人车的状态估计，将SLAM问题转化为求后验概率的问题：

对于直接求解后验概率十分困难，根据贝叶斯推断可知，求一个状态最优估计使得在该状态下后验概率最大（maximize a posterior，MAP）是可行的：

由于贝叶斯公式右侧的分母和状态无关，后验概率最大化无需考虑分母，此时只需要当前时刻的状态，又可写为：

综上所述，在对SLAM问题的运动学以及观测模型建模后，SLAM 问题的本质即为求解贝叶斯公式最大后验概率。

1.2 SLAM系统框架

激光SLAM系统框架主要如图1所示。

图1 SLAM框架

根据框图可以将整个SLAM 流程分为以下部分：

（1）传感器信息读取。在激光SLAM 中主要为激光雷达的扫描信息和预处理，在无人车中还包含了编码器、IMU 等传感器信息的读取和同步。

（2）扫描匹配。扫描匹配主要根据2D 激光点云对相邻时刻的无人车运动的状态估计以及局部地图的构建。

（3）精确定位。精确定位主要包含了后端优化，接收不同时刻的点云信息，以及回环检测的信息，可以获得全局一致的轨迹和地图。对于优化方法，主要包含了滤波以及图优化等方案。

（4）回环检测。回环检测用于判断无人车是否曾经到达过先前位置，根据信息提供可以进一步进行处理。

（5）地图构建。根据估计的轨迹以及自身的位姿信息建立未知环境下的地图。对于不同的传感器建立的地图可能包含2D 栅格地图、拓扑地图、网格地图等，对于本文实验平台只在低矮平面中运动的无人清洁车，根据2D 激光传感器可以构建2D 栅格地图，可以满足多种任务的实现。

（6）路径规划。在地图和定位精确的基础上，无人车可以根据规划路径完成自主循迹任务，同时可以实现实时避障等功能。

2 卡尔曼滤波

卡尔曼滤波是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。在SLAM问题的观测系统中容易受到环境噪声的干扰影响，所以系统状态估计也可以看作是滤波的过程。本文利用卡尔曼滤波对无人车的目标位置数据进行追踪，由于激光传感器、IMU 以及编码器里程计的相关观测值均会受到环境或者本身特性的噪声影响，利用滤波器将无人车的动态信息和观测结果相结合，从而有效抑制噪声。

2.1 卡尔曼滤波器原理

使用卡尔曼滤波算法主要由两个阶段构成一个迭代循环，一个是预测阶段，一个是更新阶段。其流程如图2所示。

图2 卡尔曼滤波流程

2.1.1 预测部分

卡尔曼滤波器更新初始值预测新值，根据系统中存在的各种过程噪声引入预测中的不确定性。

预测状态：

其中即位置和速度，利用状态转移矩阵，再加上外部影响，预测下一时刻的状态。

预测误差的协方差矩阵：

其中，状态向量中的两个变量（位置）和（速度）存在相关性，用误差协方差矩阵表示。而外部一些无法检测的干扰当作协方差为的噪声进行处理。为过程噪音协方差矩阵，增加不确定性。

2.1.2 更新部分

更新步骤中，根据无人清洁车上传感器获取的实际测量值，通过计算预测值和实际测量值之间的差值，利用卡尔曼增益来决定预测值或者测量值的权重。根据卡尔曼增益的决策，迭代计算新值和新的不确定性。

测量和预测值之差：

其中是测量向量，表示传感器测量的数据（本文利用激光雷达和编码器里程计测量的无人车位置速度信息，IMU 测量角度信息）。为测量值和更新后的预测值之间存在的误差。

计算卡尔曼增益的中间变量：

其中传感器测量由于存在噪声导致测量结果存在噪声协方差，此处R为位置信息的方差。

卡尔曼增益：

更新状态矩阵：

更新预测误差协方差矩阵：

卡尔曼滤波算法如下：

卡尔曼滤波算法

2.2 传感器融合

使由于单一传感器在移动无人车SLAM的问题解决上有所局限，本文结合激光、IMU、轮式里程计多种传感器的优势对无人洗地车的定位问题进行优化。利用激光传感器获取无人车当前位姿信息，IMU 获得无人车航向信息以及航向角速度，轮式里程计获得无人车线速度信息。多传感器融合框图如图3所示。

图3 多传感器融合流程

3 扩展卡尔曼滤波

该无人清洁车在实际环境运行下为非线性系统，此时数据不符合高斯分布，因此无法直接利用卡尔曼滤波进行数据融合，针对此提出扩展卡尔曼滤波方法。扩展卡尔曼滤波是标准卡尔曼滤波在非线性情况下的扩展形式，利用泰勒级数展开将非线性系统线性化，将系统方程和状态方程改写为雅可比矩阵形式，从而进行后验估计。

在卡尔曼滤波基础上引入非线性模型：

此时扩展卡尔曼滤波算法流程为：

扩展卡尔曼滤波算法

本文设计针对无人清洁车进行运动学建模分析，通过激光雷达，IMU 以及轮式里程计数据进行状态更新。假定以地面-无人车坐标系，无人清洁车的运动学模型观测矩阵为：

实验平台为两轮驱动差速无人车模型，根据轮式编码器可以获得其左右轮线速度分别为v，v，轮间距为。由此可得无人车线速度和角速度：

根据线速度和角速度可得无人车圆弧运动半径：

可得圆心坐标为

则+Δ时刻：

由此推出运动模型的雅可比矩阵：

将其代入公式（13）中可以得到非线性模型，至此完成了扩展卡尔曼滤波中核心的非线性模型推导。而在实际工程应用中，由于算法时间复杂度较大，遇到复杂工作环境时运行负载过重，针对时间间隔引入趋于无穷小，针对误差的多方面来源引入加速度方差作为过程误差协方差矩阵。在保证定位精度的情况下，提高运行效率。

4 实验

4.1 实验系统设计

本提本文采用如图所示无人清洁车作为实验平台，重点研究无人清洁车在车库清洁工作环境下的激光SLAM多传感器融合问题。

图4 无人清洁车平台

该无人清洁车实验平台根据其执行功能逻辑可以划分为四个部分，分别为指令层、算法层、控制层以及硬件层。其中，硬件层主要包括上海合时科技智能有限公司的无人清洁车平台，具有两个驱动轮，两个随动轮，利用差速驱动实现对无人车的运动控制，承载性以及稳定性较好。同时驱动电机带有2500 线增量式编码器，通过CAN 总线在一定频率内向主控设备反馈位置信息；传感器采用德国sick 公司的TiM571 型号2D 激光传感器，开启角度为水平270°，扫描频率15 Hz，角度分辨率为0.33°，可完成扫描0.05 ～25 m 之间的物体，通过以太网接口向主控设备返回扫描信息，激光传感器安装在无人清洁车头部，保证其扫描范围内不被遮挡；IMU 采用广州维特智能公司的HWT901B九轴姿态角度传感器，模块集成高精度陀螺仪、加速度计以及地磁场传感器，数据频率为0.1 ～200 Hz 可调，通过CAN 总线向主控设备返回姿态信息。控制层主要为从无人清洁车底层获取数据，在控制层完成简单的处理后通过mcu 下位机对电机进行控制，实现无人车的移动、手控、灯光以及清洁设备的简单控制。算法层主要包括SLAM模块，在获取底层数据后完成无人车的建图以及实时定位；导航模块完成定位信息以及地图建立后的自主运行功能实现；通讯模块主要为主控设备的通讯协议，其中包括CAN 总线通讯协议以及基于TCP/IP 的socket 通信协议。算法层的开发在Windows 系统中的Visual Studio 2013 利用C++语言完成。指令层主要为发送指令的主控终端，本次实验采用ASUS笔记本配置为Intel Core i7-8750H CPU，64 位Windows10 操作系统。指令层通过VS2013C#开发的上位机界面完成输入。整体架构如图5所示。

图5 系统整体架构

4.2 实验结果分析

使本实验将在算法层中加入多线程进行实验过程中数据的提取，离线后利用Python 进行实验数据进行整合以及结果分析。实验场地选择地下车库环境，在长为40 m，宽为2 m 的环境下利用无人车完成地图建立后沿预设轨迹运动一周，选取不同节点进行采样，根据真实值和单一传感器、多传感器融合后轨迹进行对比并且进行误差分析。完成环境建图后栅格地图如图6所示。

图6 无人车定位栅格地图

图7—图9 分别为激光传感器，编码器分别单独测量，通过扩展卡尔曼滤波进行传感器融合后与真实路径的匹配图。

图7 激光传感器测量

图8 编码器测量值

图9 传感器融合后测量值

在不同位置进行数据采样，可以获得不同位置不同传感器定位信息的误差，如表1所示。

表1 不同传感器定位信息分析

根据图7—图9 可以看出，在依靠单一编码器进行定位信息获取时，累计误差过大，在无人清洁车行驶10 m 之后编码器误差趋于不可接受范围。在依靠单一激光传感器进行定位时在部分位置振幅较大，实际控制中容易导致无人清洁车行驶轨迹有所摆动，而在经过扩展卡尔曼滤波后路径定位趋于平滑，无人车行驶也更加稳定。同时根据算法2可以进一步得出，在经过扩展卡尔曼滤波后定位信息误差相较单一传感器均有所减少。综上所述，在多传感器组合定位时，对于无人清洁车的定位精度以及实际行驶稳定性均有所提高。

5 结语

本文针对无人清洁车SLAM定位问题提出了多传感器融合的应用研究。首先完成了对SLAM问题的建模以及框架解释，同时对卡尔曼滤波的原理以及算法流程进行分析，在此基础上提出了改进扩展卡尔曼滤波算法针对非线性系统中多种传感器的优劣势进行融合处理，有效改善了在单一传感器解决SLAM 定位中的精度问题，增加了系统鲁棒性以及兼容性。同时在实验平台下完成实验结果分析，验证了系统的可行性以及实时性。但由于扩展卡尔曼滤波忽略泰勒展开的高阶项，容易对误差形成累积，在动态环境下运行有所不足。随着硬件以及算法的不断成熟，在未来的卡尔曼滤波算法应用上结合日渐成熟的深度学习将会大大增加系统的精确度以及鲁棒性。视觉SLAM技术的发展也趋于成熟，加入摄像头传感器对解决SLAM问题在性能上也能有所提高，未来在面对更加复杂的实际运行环境中更加稳定。