基于图优化的室外无人车多图层地图构建*

牛国臣，冯 宁，王 瑜

（中国民航大学机器人研究所，天津 300300）

前言

在过去的几十年里，同时定位与建图（SLAM）的应用领域受到了越来越多的关注，尤其是在无人驾驶领域。而地图构建是实现无人驾驶的基本前提，目前典型的地图主要有4 种：拓扑地图、栅格地图、点云地图、语义地图。

不同类型的地图在无人驾驶自主导航方面各有优缺点。拓扑地图和栅格地图易于路径规划。Wang 等提出一种双层路径规划算法，分别在拓扑层和路由层进行路径规划。Jubair 等在栅格地图中寻找源节点到单个目标节点的最短路径。Sepehrifar 和Asaduzzaman 等解决了栅格地图中多目标点的最短路径搜索问题。但由于拓扑和栅格地图缺乏足够的3D特征信息，往往难以用于定位。

点云地图容易获得足够的特征信息，但在室外场景下，地图的数据量很大且存在动态物体，会给定位的准确性和实时性带来很大挑战。近年来，由于激光雷达传感器具有测量精度高、可靠性好、不易受光照变化影响等优点，激光SLAM 在无人驾驶领域的应用越来越广泛。Zhang 等提出一种低漂移实时的雷达定位和建图方法（LOAM），通过计算每个点的曲率来提取特征点，考虑到系统的实时性，采取高频里程计和低频建图相结合的方法，但由于缺少后端优化，导致所建地图准确性较差。Ji 等在LOAM 的基础上，加入GPS 约束来消除部分累积误差。Liu等利用地平面约束减小匹配误差。Holder等提出一种基于实时位姿图的SLAM 系统。最近，Le等在后端提出一个优化问题对LIDAR运动进行补偿，相比以前的工作，后端处理方法取得了更好的性能，但是不能实时运行。

语义地图可以为人机交互提供环境语义信息，有效提升对环境的理解能力，但语义信息提取复杂。Chen 等采用RangeNet++的方法提取语义信息，滤除建图过程中的动态物体。Bavle 等提出一种视觉语义SLAM 方法。Erik 等利用语义信息进行定位，节省了存储空间，但是不适用于单一环境。Qin等提出了一个基于视觉的定位解决方案，利用鲁棒的语义特征来辅助车辆在停车场中导航，但是只适用于停车场。而用于无人驾驶的高精度地图需要昂贵的测绘系统和人工标注等复杂的后处理步骤，成本昂贵。

为解决上述问题，有必要将不同类型的地图组合到一起。Bücken 等提出一种结合栅格和拓扑的组合地图用来进行室内导航，但在室外环境下，无法进行精确定位。Liu 等提出一种融合拓扑地图和栅格地图的方法，解决了机器人在未知环境中的自动探索问题，但其选定的候选目标点是二维，缺乏三维空间信息。Galindo 等利用语义信息标记拓扑地图，进行导航。Richter 和Yu 等提出语义栅格地图。Cailhol 等结合语义和拓扑地图，提高了人机交互能力。Erik 等构建点云语义地图，打破传统点云匹配定位方法，利用环境语义信息定位，但实现路径规划比较困难。

基于上述讨论，本文中提出一种适用于无人驾驶的室外多图层地图。区别于传统单一地图，该地图包含有拓扑、点云、栅格和动态数据的多层环境信息。动态层实时点云信息与点云地图匹配，并融合拓扑地图的绝对位置信息，实现精确、实时且鲁棒的无人车定位；拓扑和栅格地图以及对动态层检测的实时障碍物数据，为无人车的路径规划和避障等功能提供丰富的环境信息。为验证提出的地图，在实际环境下进行实验。实验表明，该地图在大规模的园区环境中也能很好地工作。

1 系统概述

1.1 多图层地图构建

本文主要面向园区内L4 级别的无人驾驶，园区环境具有车速较低、车流量较少、障碍物较少且多为静态障碍物的特点。实验平台为改装长城欧拉电动汽车，如图1 所示，配备有北科天绘的16 线激光雷达，华测的组合导航和毫米波雷达等传感器。地图体系结构如图2 所示，由轨迹层、静态层和动态层组成。组合导航路径点构建轨迹层，激光SLAM 生成点云和栅格地图，共同构建静态层，动态层包含激光雷达的实时点云。

图1 实验平台

图2 地图体系结构

1.2 基于多图层地图的定位和障碍物检测

基于多图层地图的定位方法如图3（a）所示，根据轨迹层的GNSS 位置信息给定无人车初始位置，并实现静态层点云地图的分块动态加载，然后将动态层的实时点云信息与局部点云地图做NDT匹配定位。当点云定位偏差超过阈值时，将使用GNSS 位置信息进行重定位，输出无人车精确位姿。

图3（b）显示障碍物的检测方法，根据动态层的实时信息，结合定位输出的无人车位姿实现障碍物的实时检测。将检测的障碍物同静态层栅格地图结合，为无人车的局部路径规划和避障提供环境障碍物信息。

图3 定位和障碍物检测

2 多图层地图构建

2.1 轨迹层

轨迹层包含了由GNSS 路径点组成的拓扑地图，为无人车的全局路径规划提供信息。根据GNSS位置信息给定无人车的初始位置，并实现点云地图的分块动态加载。

通过将组合导航原始数据的经纬度信息转换到WGS-84 坐标系下，计算每个坐标相对于初始坐标的位置，得到运动轨迹，并采用g2o 库生成拓扑地图。

2.2 静态层

2.2.1 点云地图

根据激光SLAM 输出得到最优轨迹，将其对应的点云帧拼接生成三维点云地图，因此位姿的精确度影响地图的质量。实验平台所使用的传感器是16 线激光雷达，数据较为稀疏，所以后端采用图优化方法，通过将地平面约束、GNSS约束作为一元边，聚类特征作为闭环约束加入到图中，在后端优化得到精确位姿。

（1）图优化

图由“顶点”和“边”组成，“顶点”为车辆位姿，“边”为位姿间的约束。图的结构如图4所示。

图4 图优化结构

（2）地平面约束

地面点云是对地面车辆位姿估计的良好约束。设定无人车前进方向为轴，轴指向左侧，轴指向上方，通过将三维点云降到二维平面，可以根据激光雷达投影到地面的射线中前后两点的坡度是否大于事先设定的坡度阈值来判断点是否为地面点。

为输入点云，对任一点，∈，有

式中：x、y为点在LIDAR 坐标系下的坐标位置；r为点到LIDAR的水平距离。

按照水平距离排序，计算同一射线上相邻两点的坡度：

式中：z为点的垂直高度；d为点和+ 1点的坡度；为LIDAR的安装高度。

将式（2）的坡度与所设定的阈值作比较，当d＜时，将点归为地面点。

完成地平面的检测后，根据时刻的车辆位姿，估计地平面：

式中：[n，n，n，]=[0，0，1，0]为初始时刻平面；T为时刻的车辆位姿；′、′、′、′为时刻车辆位姿估计的地平面′。

采用最小参数法，定义平面参数表达式：

式中：arctan(n/n)为平面方位角；arctan(n/||)为平面仰角；为截距的长度。

则位姿节点和地平面节点之间的误差为

（3）GNSS约束

将位姿节点和GNSS 数据的时间戳对齐，GNSS位置可作为先验位置信息，成为车辆位姿节点的一元边。计算两者之间的误差：

式中：T为时刻的车辆位姿；G为GNSS数据生成的节点。

（4）闭环约束

完成聚类特征提取后，基于点云聚类特征闭环得到当前帧点云与局部地图的粗匹配，利用NDT 得到当前点云和局部地图间的精确变换。最后将闭环得到的变换矩阵，作为约束加入到后端进行优化位姿。

2.2.2 二维栅格地图

二维栅格地图用占用栅格的形式存储环境中静态障碍物信息，为无人驾驶的路径规划提供信息。栅格地图由滤除地平面后的点云地图向水平平面投影得到。

栅格状态有占用、空闲、未知（即初始化）3 种状态，栅格的初始状态= 0。用（空闲）、（占据）表示对栅格的观测。

式中：为栅格测量值；(= 0)为栅格空闲的概率；(= 1)为栅格占据的概率。

更新栅格状态：

或

式中和分别为更新前后栅格的状态。

2.3 动态层

动态层接收RTK 和LIDAR 传感器的实时数据。首先对LIDAR 进行运动补偿，去除扫描过程中由于车辆移动造成的运动畸变，再利用体素滤波去除噪点和地面点，得到处理过的当前帧点云数据。

根据处理后的实时感知信息，进行NDT 匹配定位和障碍物检测，得到的障碍物信息可用于无人车避障。为实现局部路径规划等功能，需最后将障碍物信息同静态层栅格地图结合，为无人车自主导航提供环境信息。

2.4 多图层地图

将各层地图转换到世界坐标系进行组合，完成多图层地图的构建。如图5 所示，GNSS 拓扑地图提供绝对位置信息，二维栅格地图存储环境静态障碍物信息，两者为无人车的路径规划提供信息，点云地图提供环境几何结构信息，与动态层点云匹配为无人车提供相对定位信息，动态环境信息层提供了实时环境信息，组合构成了完整的环境感知系统。

图5 多图层地图构建

3 基于多图层地图的定位和障碍物检测

3.1 点云地图处理

室外点云地图的数据量非常大，加载全局地图耗时很长，因此在实际定位时，将当前帧点云与局部地图进行配准。本文对点云地图进行分割，按照方形区域划分并分块存储，设置划分区域的边长为30 m，定位时根据轨迹层GNSS 信息实现点云地图分块动态加载，提高定位效率。

3.2 定位

在实际应用场景中定位时，由于环境变化的影响，例如车辆及行人的移动、树木的稀疏程度等，会导致较大匹配误差，选择NDT 来解决上述问题，可以在很大程度上消除这种不确定性，具体步骤如下。

（1）根据轨迹层的GNSS 位置信息，给定无人车的初始位置，并动态加载分块点云地图。

（2）动态层实时点云与点云地图进行NDT 匹配定位。

NDT 首先将参考点云（即先验地图）网格化，基于网格内的点计算其概率密度：

式中：为正态分布的均值；为协方差矩阵；=[，…，x]，对3D点云，= 3。

NDT配准：定义当前帧点云=[，，]，变换参数，空间转换函数(，)表示使用对点进行变换。求出使得概率密度之和最大的变换参数。

非线性优化：使用牛顿法进行优化，更新姿态。

（3）为进一步保证定位精度，设定NDT 匹配定位得分_＞100 时，用轨迹层提供的GNSS位置信息进行重定位。

3.3 障碍物检测

动态层接收传感器的实时点云信息，在对点云进行障碍物检测时，由于点云密度不均等原因可能会导致误检等问题，本文中采用DBSCAN 算法检测障碍物。输出的障碍物信息，通过与静态层栅格地图信息结合，为无人车自主导航提供丰富的环境信息。检测步骤如下：

（1）设定点云分割阈值，在LIDAR 坐标系下按长方形区域划分车体周围点云，正方向上取30 m，负方向取10 m，正负方向各取10 m；

（2）将LIDAR获取的点云数据去除地面点；

（3）密度聚类确定障碍物的点云簇；

（4）利用最小面积矩形包络法构建障碍物的外接框，输出环境中的障碍物信息。

4 实验结果与分析

采用实际数据对提出的地图进行评估。首先创建实际环境中的多图层地图，然后基于该地图实现实时定位和障碍物检测，最后对结果进行分析。

4.1 环境地图创建

本文中分别对多图层地图的各个图层构建进行了实验验证，最后将各个图层转换到世界坐标系，构建出完整地图并进行分析。

图6 显示了实验环境和行驶路线，A 点为起点，车辆沿A-B-C-D-E-F-A 路线行驶，总路线长约700 m，双向车道，道路平坦宽广，道路宽约6 m，有减速带。

图6 实验环境与行驶路线

4.1.1 轨迹层

如图7 所示，绿线表示无人车的行驶轨迹，由组合导航的路径点组成，A 为起点，B～F 代表拐点。实验平台配备的高精度的组合导航，精度约在2～3 cm，构建的GNSS 拓扑地图精度达到了厘米级，能够应用于真实环境。

图7 GNSS拓扑地图

4.1.2 静态层

（1）三维点云地图

为了不丢失特征信息，且有更好的建图效果，在实车建图实验时采用了后端图优化的方法。图8（a）表示生成的点云地图，图8（b）和图8（c）分别表示局部细节。从地图来看，没有丢失环境细节信息，误匹配较少，没有出现模糊不清的场景。

图8 点云地图效果示意

（2）二维栅格地图

通过将滤除地平面后的点云地图向地面投影，实现栅格地图的创建。为保证地图的存储效率，设置栅格地图分辨率为0.2 m ，栅格占据和空闲的概率分别设置为0.3和0.7。图9（a）为栅格地图，图中黑色部分指的是障碍物占据区域，环境边界清晰，能很好的描述环境中的静态障碍物。图9（b）和图9（c）是栅格地图的细节，为更好展示栅格地图构建效果，图中还显示了原始点云信息（彩色为点云）。

图9 栅格地图效果示意

4.1.3 动态层

传感器获取实时环境信息，图10（a）为A路口处的当前帧点云，在地图中显示如图10（b）所示，红色为当前帧点云。

图10 动态环境信息

4.1.4 多图层地图

通过将各个图层转换到同一坐标系，构建完整的环境地图。如图11 所示，彩色为点云地图，栅格地图在点云下方，蓝色轨迹为GNSS路径。

图11 多图层地图

4.2 基于多图层地图的定位和障碍物检测

4.2.1 定位

基于多图层地图进行定位，效果如图12 所示，黄线为匹配定位得到的轨迹，蓝线为真实轨迹。可以明显看出，定位轨迹和真实轨迹基本重合，没有明显的偏差，定位精确。

图12 定位效果示意图

为验证定位的准确性，将定位得到的轨迹与真实轨迹作对比，并计算误差，进行定量分析，如图13所示。

图13（a）左列显示了定位得到的轨迹与真实轨迹的对比图，右列展示了两者对比的细节图。图13（b）、图13（c）显示了两个轨迹间、方向上的误差，误差均值分别为0.095、0.180 m。图13（d）展示了轨迹间的距离误差，距离误差均值为0.222 m，定位精度较高。结果表明，本文提出的多图层地图能够用于室外实时定位。

图13 定位误差分析

4.2.2 障碍物检测

对动态层的实时信息进行障碍物检测，检测效果如图14 所示，左列为实际环境图，右列为检测结果。图14（a）是静态障碍物场景，左图红框区域为检测到的静态障碍物。图14（b）为动态障碍物场景，红框区域为动态物体。结果表明检测较准确，能用于无人车避障等，且检测到的实时障碍物信息可以同栅格地图结合，为无人车的路径规划提供信息。

图14 障碍物检测效果

5 结论

本文中提出一种基于图优化的室外多图层地图，经实验验证得到以下结论：

（1）区别于传统单一地图，该地图由轨迹层、静态层和动态层构成，包含了拓扑、点云、栅格和动态数据的多种环境信息，为无人车自主导航提供了充足的信息。

（2）基于该地图可以实现精确、实时且鲁棒的无人车定位，在实际实验中，距离误差均值可达到0.222 m 以下。

（3）在地图的动态层准确完成了实时障碍物检测，与栅格地图信息相结合，为无人车自主导航提供了精准的环境障碍物信息。

加入语义信息提供人机交互功能将是今后进一步研究的方向。