陈平　张少将　朱强　张庆余　余楚礼

摘 要：结合自动驾驶汽车在无GPS信号的环境中如何解决定位问题，本文通过视觉激光传感器融合的同时定位与地图构建技术（VL-SLAM），基于机器人操作系统（ROS）构建了自动驾驶汽车的物理仿真平台和点云地图可视化界面。应用最近邻迭代（ICP）相对位姿估计算法采集到地库环境的高精度点云地图，又应用粒子滤波算法解决了在地图中的准确定位问题。最终，在实际地库环境中进行实车测试，验证该系统的算法及解决方案。

关键词：视觉激光传感器融合；SLAM；自动驾驶汽车定位；ROS

1 引言

随着自动驾驶技术的发展，在未知环境中智能汽车的定位技术成为该领域研究的核心。目前定位技术主要的解决方案是基于全球定位系统（GPS），但是在某些特殊的环境中如地下车库，没有GPS信号如何解决定位问题就是本文研究的关键所在。近年来，同步定位与地图构建（Simultaneous Localization and Mapping，SLAM）技术的日益成熟，配合多传感器融合解决方案，自动驾驶车辆在未知环境无GPS信号的情况下，完成路径规划的自动驾驶任务得以实现。本文先介绍了自动驾驶系统概述、视觉激光融合的SLAM理论与算法，又基于ROS框架搭建了自动驾驶汽车的建图与路径规划仿真实验，最后完成了在地库中的实车算法验证实验，并做了论文结论总结与自动驾驶技术的未来展望。

2 自动驾驶系统概述

自动驾驶汽车即无人驾驶智能汽车，在没有人为参与的情况下，依靠車内的控制系统与智能算法，通过多重传感器数据融合控制汽车底层协议完成正常的车辆行驶功能。智能驾驶汽车是一个综合的集成系统，包括了自动泊车系统、自动驾驶系统、障碍物停障系统等，又分为了感知层、决策层和控制层三个部分如图1所示。其中感知层包括各路传感器的数据采集、处理与融合等，更加精确和全面的感知周围环境信息。决策层的输入包括感知层的信息、路径的规划以及控制层反馈回来的数据，通过增强学习算法下发决策指令。决策指令包括了循迹、跟车、超车、刹车、转向、调头等等；最终通过控制层下发CAN总线下发指令完成智能驾驶汽车的自动驾驶任务，包括油门与刹车的控制、方向盘与挡位的控制等等。

自动驾驶汽车发展与研发中的核心技术是车辆线控技术和车辆精确定位技术。本论文主要分析车辆的精确定位，目前最常用的解决方法就是使用GPS，可以让汽车实时地得到自身的位置坐标。本文则基于SLAM技术研究了一种新的定位方法。

结合SLAM技术，自动驾驶汽车的传感器分别由摄像头、激光雷达、车载毫米波雷达、车载超声波雷达、惯导组合、工业控制器等组成，具体的安装位置如图2所示。使用的自动驾驶控制流程图如图3所示。

3 SLAM融合算法

3.1 SLAM技术介绍

SLAM（Simultaneous Localization and Mapping），同时定位与地图构建，主要解决的是自动驾驶汽车在未知环境中从未知位置开始移动，通过传感器的输入信号估计自身位姿并构建空间环境地图进行自身定位，实现自主导航。

这里记自动驾驶汽车在各个时刻的状态为x1，x2，…，xk，其中k表示离散时间下标。下面分别用观测模型和运动模型描述自动驾驶汽车的SLAM问题：

zk，j表示观测量，yi表示当前环境量，xk表示当前的位姿状态，uk表示当前运动传感器的读数，vkwk都是噪声值。

SLAM技术是一个比较庞大的系统，其中包括前端里程计、后端优化、回环检测、建图四大部分。本文主要介绍基于视觉激光融合的SLAM前端里程计的位姿估计与定位技术。

3.2 位姿估计算法

视觉激光的数据融合可以让自动驾驶汽车知道周围环境的情况，检测到障碍物的种类和对应的距离深度，假设两个关键帧图像数据之间已经匹配好的三维对应点：

p=﹛p1…pn﹜，p′=﹛p1′，…pn′﹜

通过求解欧式变换R，t，使得i，pi =Rpi′+t，求解上述表达式采用迭代最近邻点法（Iterative Closest Point，ICP），具体ICP算法的求解方法有两种：线性代数法（SVD）和非线性优化法。

3.2.1 线性代数法（SVD）

定义第i对点的误差项为；ei=pi-（Rpi′+t），构建最小二乘函数，使得误差的平方和达到极小值的R，t：J=

定义两组点的质心为：

对应的平方项展开并去掉交叉项为零的部分，优化目标函数可以简化为：

令：

qi=pi-p，qi′=pi′-p′

同样的方法展开只含有R的左侧式子的平方和，并消去与R无关的项，最终化简为需要优化的式子：

先定义矩阵：

W是一个三维矩阵，对其进行SVD分解：

W=UXVT

其中U和V为对角阵，当W满秩时，

R=UVT

求解R后，代回优化目标函数可求出t。

3.2.2 非线性优化法

求解ICP用非线性优化的方法，是根据迭代思想寻找最优解，与PnP算法类似。优化过程中为了消除旋转矩阵的自身约束，使用了李代数表示位姿，构建的目标函数为：

式子中只有一个未知量，所以可以采用李代数的扰动模型求解：

因为ICP算法中特征点已经匹配好，所以构建的最小二乘方程一定有解析解，就可以不进行迭代优化。为了方便计算，可以采用两种算法放在一起考虑，例如：如果深度信息未知，可以采用3D-2D的PNP重投影误差；若深度信息已经被测出，就可以用3D-3D的ICP算法完成建模与求解。

3.2.3 基于滤波的定位算法

通过观测模型解决了自动驾驶汽车的位姿估计和空间运动求解问题，还要解决自动驾驶汽车在构建地图中的定位问题。本论文主要采用的贝叶斯滤波中一种特殊滤波方式——粒子滤波。

（1）贝叶斯滤波

自动驾驶汽车的当前定位存在观测值z和位姿真实值x，两者之间是存在误差的，从观测值到位姿真实值是一个概率分布问题，是所有空间环境定位中的可能性事件分布。贝叶斯滤波的流程是：已知状态量t-1时刻的xt-1概率分布和运动传感器值ut-1，在给定 t时刻的观测数据zt的情况下，估计出状态量在t时刻的概率分布。对应的贝叶斯滤波公式为：

化简可得：

p（xt|z1：t，u1：t）=ηp（z1|x1）∫p（xt|xt-1，ut）p（xt-1|z1：t-1，u1：t-1） dxt-1

贝叶斯滤波的核心就是通过t-1时刻的后验概率推导在t时刻的先验概率，在通过t 时刻的先验概率更新t时刻的后验概率。

（2）粒子滤波算法

粒子滤波是基于蒙特卡洛的方法，用粒子集表示概率，寻找随机样本用来近似的描述概率密度函数，用样本均值代替积分运算，进而获得系统状态的最小方差估计的过程。由于粒子滤波在非高斯、非线性系统和多峰分布表现出来的优势，可以用来估计带噪声的观测数据中自动驾驶汽车的位姿和运动状态。

粒子滤波可表示为：X={（xti，wti）[i=1，…，n]}，其中xti表示自动驾驶汽车的位姿，wti表示当前位姿所处环境跟地图的匹配度的权重。如图4所示为粒子近似分布示意图。

本论文中实现自动驾驶系统中粒子滤波算法的流程分为以下三步：

1.用粒子进行状态传播：

xti～p（xt|ut，xt-1i ）

2.评价每一个粒子的权重：

wti=ηp（zt|xt ）

3.根据权重进行重采样，以wti的概率接受xti，目的是去除低权重的粒子。

其中粒子滤波的状态传播模型为：

bel'（xt）=∫p（xt|xt-1，ut）p（xt-1|z1：t-1，u1：t-1）dxt-1

因为在没有GPS信号的情况下，无法知道自动驾驶汽车位姿的实际发布，从它的预测分布进行定位采样如图5所示，并用观测模型进行计算权重，联合权重再重新采样来近似自动驾驶汽车的后验概率分布。概率越大，权重越大，定位越准确，重定位需要复制的粒子越多，粒子权重就更符合后验概率分布如图6，图7所示。

4 基于ROS的定位与建图仿真实验

本论文中应用以上的SLAM位姿估计算法和粒子滤波定位算法，结合机器人操作系统ROS（Robot Operating System）进行算法验证与仿真实验。ROS是一个适用于自动驾驶汽车开源的元级操作系统。主要的优点是可以为代码复用提供支持、良好的通讯架构、具备插件仿真工具、有强大的库且兼容多种编程语言。它提供了操作系统应有的功能，包括硬件抽象、底层设备控制、共用功能执行、进程间消息传递，以及功能包管理。

在本仿真实验中，用URDF机器人统一描述平台构建了自动驾驶汽车的模型，然后把自动驾驶汽车模型加入Gazebo物理仿真平真平台进行仿真地下车库的环境如图8所示，通过粒子滤波算法进行模型定位仿真，用gmapping导航功能包在Rviz界面中显示地图的构建情况，并在地图中进行自主定位导航与路径规划如图9所示。

5 实车测试与结果分析

进行了理论分析与实验算法仿真以后，用自动驾驶车辆进行测试与实验，选择的环境为没有GPS信号的地下车库如图10图11所示。首先通过自动驾驶车辆的相对位姿估计算法进行求解当前车的空间刚体运动矩阵，得到的位姿关系进行了整个地库地图拼接与构建并在Rviz中进行显示如图12所示，转换为PCD格式的点云地图如图13所示。

将构建好的整个车库地图选取部分路径进行定位与导航试验，通过VL-SLAM算法可以得到自动驾驶的位姿估计以及精度地图。为了精确定位自动驾驶汽车在精度地图中的位置关系，将velodyne32线激光雷达和单目视觉相机采集的数据融合后与精度地图进行匹配，根据匹配的权重大小确定当前的采集数据是在精度地图中的哪个位置。已知自身定位，然后再设定目标位置，结合ROS中的gmapping导航功能包就可以实现地图环境中的路径规划任务。在RVIZ中的实验结果如图14所示，用红色正方形标出自动驾驶车辆正在初始化，加载汽车模型。如图15所示，汽车模型加载成功，并且定位准確作为初始位置出发点。随后自动驾驶汽车开启自动驾驶功能，如图16所示为运动过程中自动驾驶汽车的精确定位。

5.1 论文总结与未来展望

本论文主要研究的是在无GPS信号的特殊环境下，基于单目视觉与velodyne32线激光传感器相互融合的自动驾驶系统定位的解决方案。本论文概述了自动驾驶的系统架构，又分析了VL-SLAM的相对位姿估计求解算法和粒子滤波定位求解算法。在Linux操作系统安装ROS环境搭建了自动驾驶的物理仿真平台和地图可视化界面，最终在无GPS的地库环境中进行实车测试，完成了相对位姿估计、精度地图构建与定位导航的功能。

自动驾驶系统是未来汽车行业的发展趋势，比如传感器数据融合、结合深度学习的自动驾驶决策系统、以及高精度地图的开发、基于大数据的智能网联汽车等等都是未来的研究方向。

参考文献：

[1]高翔.视觉SLAM十四讲.北京：电子工业出版社，2017，130～178.

[2]沈彤.立体视觉的目标测距系统[J].电子测量技术，2015，第38卷第4期.

[3]权美香.视觉SLAM综述[J].智能系统学报.2016，第11卷第6期.

[4]管叙军.视觉导航技术发展综述[J].航空兵器，2014，第5期.

[5]路研研.在轨航天器双目视觉图像采集与处理系统研制[M].黑龙江：哈尔滨工业大学，2015.

[6]胡春旭.ROS机器人开发实践.北京：机械工程出版社，2017，214～284.

[7]段华旭.无人车基于双目视觉的同时定位与地图构建[J].华中科技大学学报.2015，第43卷增刊.

[8]Strasdat et.Visual SLAM：Why filter？，Image and Vision Computing，2012.

[9]Paz Lina M et al.Divide and Conquer：EKF SLAM，IEEE Transaction on Robotics，2008.

[10]Triggs B et.El.Bundle adjustment - a Modern Synthesis，Springer，2000.

[11]Kummerle Rainer，et.al.g2o：A General Framework for Graph Optimization，ICRA，2011.

[12]Endres et al.3-D Mapping with an RGB-D Camera，IEEE Transaction on Robotics，2014.