文/郑建杰 张海涛 陈寅

Simultaneous Localization and Mapping(SLAM)同步定位与地图构建，它被定义为解决“机器人从未知环境的未知地点出发，在运动过程中通过重复观测到的地图特征（比如，墙角，柱子等）定位自身位置和姿态，再根据自身位置增量式的构建地图，从而达到同时定位和地图构建的目”的问题方法的统称[1]。

SLAM的概念最早是由Smith和Cheesman等[2-3]提出的，同时将概率估计方法应用到机器人定位与建图。1987年，Smith Self和Cheesman提出了基于卡尔曼滤波的SLAM算法。基于卡尔曼滤波器的 SLAM 算法主要是通过卡尔曼滤波器对机器人的位姿信息不断的估计和更新，利用里程计估计机器人所走轨迹以及机器人此时的位姿，激光测距采集的观测数据作为机器人的真实位姿加入卡尔曼滤波器的计算中，不断校正机器人对自身位置的估计，使估计量不断接近真实状态[4]。但在实际运用过程中，不管怎样改进和调整滤波方程，基于卡尔曼滤波器的 SLAM 算法仍然存在计算复杂度高、可靠性不强以及数据关联等问题[5]。为解决非线性滤波问题，Metropolis等人提出了粒子滤波算法(Particle Filter,PF)[6]。在强非线性非高斯环境下，粒子滤波算法的跟踪性能要远远优于卡尔曼滤波[7]。在此基础上，Doucet 等[8]提出基于 Rao-Blackwellized 的粒子滤波器(RBPF)的方法，目前在解决SLAM问题上应用最广。

1 SLAM导航车软硬件系统构建

图1：硬件系统框图

导航车硬件平台的系统框图如图1所示。图中激光雷达和里程计分别将获取的距离信息和位姿信息送往控制单元，经里面的SLAM算法计算、融合，得到导航车周围的地图信息及自身的位姿，并通过热点接入网络，将地图和位姿信息传递到人机交互单元，实现周围地图及导航车的可视化。同时结合接收到的任务，将其转化为控制指令，通过RΟS各个串口节点传递给执行单元，改变电机转速，从而控制导航车的行驶。

1.1 核心处理器

导航车车载核心处理器采用Ιntel-NUC-8i5BEK，拥有第八代智能英特尔酷睿i5-8259U处理器，8G内存，并搭载英特尔锐炬Plus显卡655， HDMΙ接口x2，前端 USB3.1接口2个，后端USB3.1接口2个，2个内部USB2.0端口，内部内存64G，搭载英特尔双频Wireless-AC 9560无线网卡。

1.2 其它模块

设计的导航车采用铝合金骨架+铝合金外壳，12V59Rpm电机，最高速可达1m/s,最大爬坡能力为15°,转向方式采用四驱差速转向。同时装有增量式编码器以及gy-85九轴AHRS惯导模块。

2 SLAM功能实现

机器人研究的问题包含许许多多的领域，我们常见的几个研究的问题包括：建图(Mapping)、定位(Localization)和路径规划（Path Planning）。而同步定位与建图（SLAM）问题位于定位和建图的交集部分，如图2所示。SLAM需要机器人在未知的环境中逐步建立起地图，然后根据地图确定自身位置，从而进一步定位。

图2：SLAM的定义

2.1 建图

贝叶斯(Bayes)滤波大多用于解决一般的线性、高斯系统的后验概率的问题,对于一些非线性,非高斯系统的后验概率的问题,引入蒙特卡洛(MonteCarlo)算法,蒙特卡洛采用样本形式而不是以函数形式对先验和后验概率进行求解,不需要积分等复杂计算,RBPFSLAM（Rao-Blackwellized Particle Filter） 算法的实现正是通过非参数化的蒙特卡洛代替贝叶斯方法来解决在大型复杂环境下概率估计问题 [9]。SALM要解决的问题就是由控制数据u1:t和观测数据z1:t来求位姿和地图的联合分布P(x1:t,m|z1:t,u1:t-1)。RBPF-SLAM融合了EKF（扩展卡尔曼滤波）和PF（粒子滤波）的优点，在降低计算复杂度的同时又有较好的鲁棒性，基于 SLAM 问题的 Rao-Blackwellized 变换可以描述为：

式（1）中描述了Rao-Blackwellized分解的核心思想，用条件概率把P(x1:t,m|z1:t,u1:t-1)拆开先求解位姿，而后建图。RBPF使用粒子滤波来估计位姿，算法总共包含四个步骤：采样-计算权重-重采样-地图估计。

图3：gmapping算法运行结构图

图4：AMCL架构图

图5：导航车实物图

图6：实际测试环境

图7：建图结果

图8：导航过程

步骤1：采样。

步骤2：计算权重。

步骤3：重采样：舍弃低权值粒子，只保留高权值的粒子，由粒子的后验分布和后验概率密度，可求得地图的后验概率为：

步骤4：地图估计，使用卡尔曼滤波对上式进行计算。

本系统基于RBPF的算法，根据激光雷达和里程计的信息，对环境地图进行构建，并且对自身状态进行估计，算法的实际运行的结构如图3。

它主要输入机器人底盘和激光雷达之间、底盘和里程计原点之间的坐标变换以及激光雷达数据，经过slam_gmapping运算后，输出地图和里程计原点之间的变换，建立的地图及其相关信息。

2.2 定位

Adaptive Mentcarto Localization(AMCL),蒙特卡洛自适应定位是一种很常用的定位算法，它通过比较检测到的障碍物和已知地图来进行定位，同时可对里程计误差进行修正。它是通过改变采样方式和滤波条件,以改进蒙特卡洛算法。在采样阶段,自适应蒙特卡洛算法是把前一时刻的采样点作为圆心进行采样,这使得实际密度与估计密度接近,从而有效提高定位精度；在滤波阶段，自适应蒙特卡洛算法是给粒子赋予权值,这样离节点真实位置越近的粒子,权值越大。[10]通过结合粒子位置和粒子权值可以估算节点的真实位置,并减小测量误差。其定位过程主要分为预测阶段和滤波阶段，具体如下：

图9：实际测试环境

图10：建图结果

图11：导航过程

（1）预测阶段。基本思想是选择一个估计密度q(x)来代替采样的真实概率密度p(x),即：

而后利用多个q（x）独立样本的加权来求近似：

其中，Np为采样粒子个数，为权重。

（2）滤波阶段。主要是通过粒子的位置以及权值的大小来提高各节点的定位精度。假设k时刻获取的采样粒子中，只有P1P2P3P4P7的待测点区域可构成封闭图形，则可得到该5个粒子的密集程度权值为：

根据这5个粒子的权值，可得到采样集合与真实位置的相关程度：

AMCL架构图如图4所示。

蒙特卡洛自适应定位主要是输入坐标变换和激光扫描数据，经过AMCL运算后，输出地图与里程计原点之间的坐标变换（其实质就是对导航车的定位），导航车在地图上的估计姿态以及由过滤器维护的姿态估计集合。

3 算法试验

3.1 室内

基于激光slam搭建的导航车如图5所示，在室内真实环境下进行定位与导航的测试，实际测试环境如图6所示。启动导航车,绕室内运动一周，完成slam建图，建图结果如图7所示。而后给定其一个目标点，导航车自主规划一条路线，并运动到目标点，导航过程如图8所示。经实际测试室内构建的地图精度较高，且自主导航过程路线规划合理，系统鲁棒性较好。

3.2 室外

在校园随机选一茂密的小树林，如图9所示。启动导航车,在小树林里运动一圈，完成slam建图，建图结果如图10所示。而后给定其一个目标点，导航车自主规划一条路线，并运动到目标点，导航过程如图11所示。通过实验检测，室外复杂环境下导航车的建图、定位精度较高，且自主导航过程路线规划合理，系统鲁棒性较好。

4 总结

本文基于激光雷达slam技术，采用RBPF粒子滤波和蒙特卡洛自适应定位算法，搭建一款可自主建图、定位和导航的无人导航车。通过室内及室外复杂环境下的实验测试，导航车的精度和鲁棒性较好，具有较好的应用前景。但是实验过程中也发现障碍物高度低于激光雷达所在平面时，无法被检测出来，下步我们将基于此，进行更进一步的研究和改进。