融合激光雷达与双层地图模型的智能车定位*

邓泽武，胡钊政，周 哲，刘裕林，彭 超

（1. 武汉理工大学信息工程学院，武汉 430070；2. 武汉理工大学智能交通系统研究中心，武汉 430063；3. 武汉理工大学重庆研究院，重庆 401120）

前言

在智能车推广过程中，高精度定位技术是车辆进行路径规划、精确导航和障碍物避撞的关键基础技术。目前，车辆定位中最常用的方式是GPS（global positioning system）定位，但GPS 定位精度较低，误差通常在10 m左右，且存在多路径和非视距问题，难以满足人们对智能车定位的要求。为将GPS 的定位精度提升到厘米级，通常须建立地面差分基站，然而该方案的使用和维护成本较高，不利于大规模推广。此外，研究人员将GPS 与惯性导航技术相融合虽可获取连续的位置信息，但在“城市峡谷”等条件下累积误差无法及时消除，且惯性导航传感器存在价格昂贵等缺点，无法在智能车上大规模使用。

近年来，随着人们对智能汽车定位研究的不断加深，基于视觉和激光的定位方法为智能车定位提供了新的思路。文献［4］中提出了以相机为传感器的ORB-SLAM 视觉定位方法，通过提取与匹配图像特征点来估计车辆运动，但视觉传感器对光照较敏感，在光照条件恶劣的环境中该定位方法的鲁棒性有待提高。激光传感器具有测量精度高、对光照不敏感的特点，受到研究人员的关注。最初研究人员使用经典的迭代最近点（iterative closest point，ICP）算法来求解车辆的相对运动关系从而实现定位，但该方法存在鲁棒性差和定位精度低等问题。文献［6］中提出了以激光雷达为核心的LOAM（LiDAR odometry and mapping）定位方案，该方法利用提取的平面、边缘等特征点进行匹配，优化点到直线与平面的距离从而估计车辆的运动状态。在LOAM 框架基础上，Shan 等又提出了改进算法，它更改了特征点的提取形式，添加了后端回环优化，因此，定位精度得到提升。文献［8］中引入了车辆运动学预测模块，在充分利用LiDAR 和IMU（inertial measurement unit）测量特性的基础上以提高定位精度。然而，这些基于激光雷达定位方法具有马尔科夫性质，下一时刻的位姿由历史时刻的位姿推算而来，随着时间的推移，多次位置推算后，由于缺少回环条件和先验信息的辅助，其定位效果将变差，不能满足车辆定位需求。为此，研究人员开始探索基于先验高精地图的方法。

高精地图是智能车定位的关键基础设施，在智能车定位中具有重要作用，它可以协助其他设备提升车辆定位精度。文献［10］中在GPS 信号缺失情况下，利用激光雷达与高精地图结合快速实现了车辆的重定位，保障了定位的稳定性。文献［11］中利用路侧单元实现车辆定位，文献［12］和文献［13］中提出了基于地图的定位方法，采用配准方法将道路标记的边缘与投影地图对齐，并将其表述为非线性优化问题实现定位。Caselitz 等将图像特征重建为一组稀疏的三维点，然后与地图三维点云匹配对齐来间接定位车辆。但这些方法受环境影响较大，算法鲁棒性有待提高。文献［15］和文献［16］中利用在线数据与离线地图进行匹配来实现定位，利用地图来校正车辆轨迹，并减小定位误差。但此类方法须保存大量的地图三维点云信息且匹配耗时长，实时性较差。为降低地图存储空间，保证定位的实时性，Li 等提出了基于杆状目标地图的车辆定位方法，利用提取的目标特征进行匹配，该方法高度依赖于一种特定类型特征，不利于很好表征道路环境信息，限制了方法的推广。文献［18］中提出了城市环境下基于多层激光雷达的车辆定位方法，将特征检测方法与蒙特卡罗定位方法相结合来估计车辆位置，但该方法在长直道路上表现较差。

综上所述，针对目前定位方法中存在的问题，本文中提出了融合激光雷达与双层地图模型的智能车定位方法，其核心思想是利用车道地图与稀疏特征地图双层结构信息对车辆轨迹进行约束，并通过构建线性卡尔曼滤波融合算法提高定位的稳定性和鲁棒性。其主要工作为：（1）面向智能车定位，提出了一种双层结构高精地图模型，该模型在车道图层基础上构建了稀疏特征图层，为智能车定位提供高精度的位置参考，能有效降低里程计的累积误差；（2）在双层地图模型基础上，提出融合基于车道线的横向距离约束和基于稀疏特征的绝对位置约束，并通过构建短时间的匀速运动模型，将车辆的运动信息与观测信息相融合，以提高定位的稳定性。

1 本文算法

本文中提出的融合激光雷达与双层地图模型的智能车定位方法，其整体流程如图1 所示。双层地图采用离线方式构建，定位阶段主要分为点云处理与基于卡尔曼滤波的融合定位两个部分。点云处理须提取车道线和特征点云，其中，特征点云用于稀疏特征地图匹配提供绝对位姿参考，车道线与车道地图相结合提供横向距离约束。基于卡尔曼滤波的融合定位主要计算状态转移矩阵和观测矩阵，输入是车辆的位置信息和检测得到的横向距离，输出为融合后的车辆位姿。

图1 系统流程图

1.1 双层高精地图模型构建

本文中所建立的地图元素信息较为简单，相对于目前高精度地图中须构建各元素之间的连接关系而言，本文只对车道线和稀疏特征点进行组织，形成车道地图与稀疏特征地图双层结构，如图2 所示。其中车道图层为智能车定位提供线性的横向位置约束，稀疏特征图层为智能车定位提供高精度的位置参考，有效降低累计定位误差。车道级地图采用广泛应用的OpenDrive的构图方式制作，针对弯道部分采用折线化方式处理。稀疏特征地图选取强度特征明显的路段作为稀疏地图采集点，采用离线的方式制作。稀疏特征地图由全局位姿、二维特征和三维点云特征3 部分组成。其中，全局位姿通过搭载的高精度组合惯性导航系统和LiDAR 里程计融合获取，并采用图优化的方式对车辆位姿进行优化，其定位精度可达到厘米级，全局位姿为后续的粗定位与度量级定位提供精确的参考位姿。二维特征则是对激光雷达点云强度投影到可以展开的圆柱形表面，形成强度二维特征图，投影方法如下所述。

图2 双层地图模型示意

首先将激光传感器扫描范围划分为水平和竖直两个方向，形成一个二维投影空间，再设定两个方向的投影角度分辨率，最终得到一个N× N的矩阵，矩阵的数值为该段区域点的最大强度值：

式中：()为激光点云中点的强度值；(P)为二维矩阵第行与第列的值，该值大小为归属于该空间所有三维点的最大强度值。获取每个矩阵元素的投影值，将其组合形成一个二维矩阵，用于二维特征相似度计算，完成节点级定位：

稀疏特征地图中的三维特征由一些标志物组成，包括路标、路牌、杆状物和弯道的车道线等，这些特征能很好地表征环境，具有唯一性。三维特征提取方法可参见文献［17］和文献［19］，保留了特征点的三维坐标信息，将所有特征点进行组合构成最终的三维特征。在定位过程中，利用当前获取的三维特征与地图中的三维特征进行配准得到相对于地图中的三维特征的相对关系，最后结合特征地图的全局位姿实现定位。由于获取的车道线信息为平面坐标系下的距离信息，为计算和描述方便，本文中将GPS 全局坐标转化为UTM（universal transverse mercator）坐标系下，实现坐标的统一，其转换方式见文献［20］。

1.2 基于双层地图模型的定位方法

基于双层地图模型的定位包含基于稀疏特征图层的多尺度定位与基于车道图层的横向距离约束两部分，不同图层实现不同定位效果。当车辆行驶至特征地图附近时，可利用特征地图包含的二维特征与三维特征来提供一个绝对位姿参考，降低累计定位误差。车道图层可利用检测的车道线提供高精度的、线性的横向位置约束，结合里程计信息提升定位精度。

1.2.1 基于稀疏特征图层的多尺度定位

为保证定位的准确性，基于稀疏特征图层的多尺度定位主要分为3 个部分：粗定位、基于二维特征的节点定位和基于点云三维特征的度量级定位。粗定位是指：当车辆行驶至路段的某个位置时，根据当前定位结果预测下一时刻位置，将预测位置与地图中的位置信息进行匹配，选取距离最近的地图数据节点作为定位节点，但考虑到当前定位存在一定误差，因此本文选取距离当前定位结果一定范围内的地图数据节点作为候选节点，以降低定位误差所产生的影响，完成粗定位。

基于二维特征的节点定位。首先须将点云数据进行投影转化为二维强度图特征，为加强其表征的唯一性，消除环境中车牌等影响，本文中只将一定高度范围内的点云进行投影。具体做法是将水平方向的点按1°的间隔进行投影；将竖直方向的点按0.5°间隔进行投影，最终形成一个50×80 大小的二维矩阵，利用这个矩阵来表征环境信息，图3（b）为投影形成的二维特征。在粗定位基础上，选取候选节点的二维矩阵信息进行匹配来定位最相似的地图节点。其相似度的计算公式为

式中：M和c表示当前投影矩阵和该矩阵的第列；M和c表示待匹配地图节点存储的二维矩阵和该矩阵的第列；为矩阵列数，本文中= 80；(M，M)为相似度大小，其值越大说明两者越相似。将候选地图节点与当前节点进行相似度计算，选择其中相似度最大的地图节点作为定位节点，完成节点定位。

基于点云三维特征的度量级定位。点云三维特征提取方法与制图过程一样，提取目标主要包含路标、路牌和路杆等，三维特征提取结果如图3（c）所示。定位过程中，在获取当前三维点云特征后，利用优化算法来优化求解待定位节点与地图数据库中最接近节点之间的旋转向量与平移矩阵，其误差模型为

图3 特征信息提取

式中：(，)为误差；=，，...，x为当前数据；=，，...，y为地图数据，再结合地图中的全局位姿信息完成车辆定位。在Kalman 滤波融合框架中，观测信息的误差水平对于最后的融合结果有较大影响。为确定基于稀疏特征地图的车辆定位误差，事先选取一段测试路段，获取基于稀疏特征的车辆定位误差。

1.2.2 基于车道图层的横向距离约束

对于横向距离约束，利用激光雷达获取路缘石或建筑等信息来确定当前车辆所处的车道，进而精确求解与车道线之间的横向距离。对于车道线提取，为消除其他物体的影响，在提取地面的基础上进行。对每帧点云，根据激光雷达放置的高度选择地面点存在的范围，滤除非地面点并降低车道点提取的复杂度。将经上述处理的点云再用RANSAC算法进行平面拟合得到地面点，利用强度特征把地面点云中的车道线点云分离出来。为降低噪声点对测试的影响，对分离出来的点还须进行离群点剔除和欧式聚类，再利用主成分分析算法选择具有直线特征的类别。经过数据预处理后，对每个类别进行直线拟合，求解每个类别的直线方程。由于地图构建得到的方程是将点云投影到平面中，因此定位中同样须将点云进行投影，以求解直线方程。直线方程的形式为

在单帧点云中，由于激光雷达测距具有很高的精度，所以可提供一个很强的约束条件。本文在长度为200 m的道路上选取100个测点进行横向误差分析，测试表明横向距离计算的平均误差为0.05 m。

对全局坐标系下的距离，可根据当前定位提供的在全局坐标系下的坐标来查询离线构建地图中的直线方程参数进行计算。在构建地图时，对其进行了分段处理，每段中有一个坐标信息，定位中只须查询距离当前位置最近的分段点并获取其直线方程参数。假设查询到最近点的直线方程为l：

式中l表示地图数库中第个查询点的直线方程信息，坐标系为全局坐标。

结合上述获得的横向距离，便可对车辆所在位置坐标建立约束，其推导过程为

式中(x，y)表示当前车辆在全局坐标系下的位置。

1.3 融合双层地图模型的智能车定位

在上述利用激光点云数据得到全局位置坐标和横向距离的基础上，采用卡尔曼滤波器将其进行融合，利用检测获取的可靠的横向约束，结合地图信息，实现车辆更高精度的定位。

利用2 阶马尔可夫模型对短时间车辆运动进行匀速运动建模，用车辆的历史状态对当前状态进行预测。卡尔曼滤波器的状态变量X由当前位置及其个历史时刻位置信息来表示，可以写成式（10）的形式：

其中位置x为一个二维向量：

为降低计算复杂度，本文中状态变量只选取当前位置和上一时刻位置，即取值为1。在较短时间内，车辆可以近似认为在匀速运动，根据匀速运动模型最终可以得到

由式（10）～式（13）进行推导，最终可以得到卡尔曼滤波器中的状态转移矩阵，并假设系统过程噪声服从0 均值的高斯分布，通过选取的测试点确定预测过程的定位结果与真实位置之间的误差，确定过程协方差矩阵：

观测信息来源于多个方面，一部分来自激光里程计与基于稀疏地图多尺度定位产生的观测值，一部分来自于车道线检测提供的横向距离约束。对于里程计部分，其观测矩阵可以表示为

基于稀疏地图多尺度定位产生的观测值为

对于横向约束提供的观测矩阵，可由式（9）进行求解。在计算出横向距离d和寻找到距离当前位置最近的车道线方程后，利用式（9）将激光相对距离信息转换为对车辆位置坐标的约束，将其进行转化后得到观测矩阵。

结合里程计观测信息、多尺度定位产生的观测信息和横向约束信息，最终的观测矩阵为

考虑到在实际道路场景中，激光雷达扫描视野中可能包含多条车道线，根据其检测个数可以建立多个约束，观测矩阵可进行相应修改。利用获取的状态转移矩阵和观测矩阵来构建卡尔曼滤波器，利用激光实时检测车道线提供的厘米级横向距离来约束车辆轨迹，实现车辆更精确的定位。

2 测试与结果分析

本文的测试平台为搭载激光雷达和组合惯导的智能车，如图4（a）所示。其中，北斗星通生产的Npos220 型号组合惯导，结合载波相位差分技术，可提供高精度经度、纬度和水平姿态等信息，其定位精度可达厘米级，因此本文用高精度组合惯导数据作为真值轨迹用于定位精度比较。采用大疆公司的Horizon 固态激光雷达，与传统机械式激光雷达相比，它采用独特的非重复扫描方式，每秒多达24 万个高精度点云，探测距离达到260 m，角度精度为0.05°，可以为车辆定位提供很好的环境检测。

图4 测试平台和场景路线图

测试选取校园和城市道路两种不同场景。校园场景的路线图如图4（b）所示，该路线的总长度约为1.1 km，在4 个特征明显的路段设置了稀疏地图节点，车道线均可稳定检测。城市道路场景的路线图如图4（c）所示，总长度约为3.4 km，在16 个特征明显的路段设置稀疏地图节点。测试在相同的计算机硬件条件下进行，其配置为Intel Core i7-10875H 2.8 GHz CPU，8 GB 内存。对于地图模型中车道图层，本文中采用现有的OpenDrive 方式进行，可参见文献［21］。稀疏特征图层通过搭载的高精度组合惯性导航系统与LiDAR 里程计融合获得，并采用图优化的方式对车辆位姿进行优化，同时对激光雷达点云进行特征提取，保存提取得到的特征点，并记录此时刻车辆在全局坐标系下的位姿形成稀疏特征图层数据。

本测试在制图过程和定位过程中，传感器的安装位置无变动，对于使用该地图的其他车辆须事先对制图车辆的传感器的外部参数进行标定，消除由于传感器安装位置导致的定位误差。此外，对于目前的量产车来说，传感器的安装位置一般相对固定，为本算法的实施提供便利，故制图和定位时传感器的安装位置差别不大时，所提出的双层地图模型均具有较好的适应性。

2.1 校园场景测试结果

首先利用高精度组合惯导和激光雷达来采集数据，采用离线方式构建地图信息。激光雷达和组合惯导数据采集频率分别为10 和30 Hz，车辆最大行驶速度为15 km/h，为保证定位时能查询到最近的地图节点，每间隔5 m 设置一个查询点，查询点包含了该点的位置信息和该小段的直线方程参数。

在车辆定位测试过程中，首先利用GPS 数据进行位置初始化，确定车辆的一个起始状态，然后利用激光雷达获得一个里程计信息，并将该里程计信息投影至全局坐标系下，同时就每时刻采集的点云数据，对其进行车道线检测和特征点提取，如果行驶至稀疏特征地图节点一定范围内，则从地图中选择最接近当前位置的地图节点，然后用当前点云与地图特征点云进行匹配，得到一个绝对位姿，该绝对位姿能有效减小累计误差。对于检测到的车道线，求解出局部坐标系下的车道线方程，然后计算当前时刻距离车道线的距离，根据当前状态来查询距离当前位置最近的车道地图，利用地图信息和当前横向距离来约束车辆轨迹，最后通过卡尔曼滤波实现融合定位。校园场景共进行了3 次测试，每次测试，车辆从不同起点出发测试算法性能。图5（a）是测试部分路段的定位误差比较图，图5（b）为某次测试不同算法的定位轨迹比较图。从图5 可见，本文算法的定位误差比文献［6］中LOAM 算法的定位误差小，验证了本文算法的有效性。

图5 定位误差水平比较图

表1 为里程计定位与融合高精度地图定位情况的误差对比。展示地图信息对定位的辅助作用，由表可见，融合地图的定位算法比单一里程计的定位精度都高。在校园场景测试中，融合单一车道地图与融合单一稀疏地图的平均误差为0.82 和0.68 m，相比于单一里程计的1.65 m，定位误差降低了50.3%和58.8%，而同时融合两者的平均误差为0.31 m，比里程计定位误差降低了81.2%。

表1 在校园场景中融合地图测试结果

为展示本文算法的有效性，在两个测试场景中将其与文献［15］和文献［22］的算法进行对比，本文算法是同时融合车道图层和稀疏图层算法。文献［15］组合里程计和位置识别来完成基于先验地图的定位，利用先验离线点云地图估计与当前激光帧的相对位姿，最后通过全局位姿图优化修正里程计的漂移；文献［22］中采用紧密耦合的迭代扩展卡尔曼滤波器将激光雷达特征点与IMU 数据融合，使用前向和后向传播来预测激光雷达扫描中的运动状态和补偿，并使用了新的卡尔曼增益公式加速计算。表2 为校园场景下不同算法的对比结果。从表2 可以看到，3 种算法的平均误差分别为0.31、0.62 和0.85 m，最大误差为1.25、1.81 和2.75 m。按平均误差计，本文算法比文献［15］和文献［22］，其定位误差分别降低了50%和63.5%。且本文算法平均耗时为33.1 ms，比两个文献分别缩短56%和17.3%，定位的实时性更强。

表2 不同算法在校园场景的测试结果

3 种算法的定位误差概率分布如图6 所示。由图可见，本文算法误差在0.5 m 内的数据占75.4%，在1 m 内的定位数据占96.4%，与另两种算法相比，极大地提高了车辆的定位精度，保障了智能车的行驶。

图6 校园场景测试定位误差概率对比

2.2 城市道路场景测试结果

城市道路场景下车辆最大行驶速度为40 km/h，相比于校园场景，车辆的行驶速度更高，数据的采集频率也相应提高，激光雷达和组合惯导数据采集频率分别设为15 和80 Hz。城市道路场景也共进行了3 次测试，每次的起点不同。为保证定位时能够查询到最近的地图节点，每间隔10 m 设置一个查询点，查询点包含了该点的位置信息和该小段的直线方程参数。在该场景中，激光雷达扫描视野中检测到多条车道线，可以建立多个约束关系。和校园场景一样，先进行高精度地图构建，保留其中稀疏特征地图和车道地图，在定位过程中再进行地图加载。其定位效果如表3、表4和图7所示。

图7 城市道路场景误差分布对比

表4 不同算法在城市道路场景测试的对比

表3 示出在城市道路场景中融合高精度地图的定位效果。由表3 可见，以融合单一车道地图和单一稀疏地图的平均误差为0.92 和0.76 m，相比于里程计定位，融合地图方法定位精度都得到了不同程度的提高，特别是融合车道地图与稀疏地图的方法，整个场景定位平均误差可以控制在0.47 m，表明地图起到了很好的辅助作用。

表3 在城市道路场景中融合地图测试结果

从表4和图7中可以看到，本文算法的平均误差为0.47 m，最大误差为1.53 m，误差范围在0.5 m 内的数据占57.2%，在1 m 内的定位数据占90.2%，而文献［15］和文献［22］算法的平均误差为0.79 和1.03 m，误差范围在0.5 m 内的数据占44.1%和32.3%，1 m 内的定位数据占66.5%和52.6%。按平均误差计，本文算法比两个文献算法其定位误差分别降低了40.5%和54.4%。与校园场景相比，城市开放道路场景环境较为复杂，定位精度也有所降低，但总体保持在一个较高水平，平均误差都在0.5 m以内，相对误差在0.3%以内，满足车辆定位需求。

综上两种不同环境测试结果可以看出，本文提出的融合激光雷达和双层地图模型的智能车定位方法精度高，实时性好。

3 结论

针对智能车定位精度低和地图数据复杂等问题，本文中提出了融合激光雷达和双层地图模型的智能车定位方法，该方法利用不同地图图层实现不同约束，在定位过程中利用检测到的车道线并结合车道地图信息来提供一个稳定和可靠的距离约束，利用稀疏特征图层为智能车定位提供高精度的位置参考，以提升定位精度。测试结果表明，在保证车辆定位实时性的需求下，该方法可以提供一个很好的定位效果，与现有算法相比，定位误差降低了40%～60%，拥有更好的定位精度。此外，本文的研究工作尚存在一些不足之处，在某些环境中，激光强度信息可能易受干扰，未来将增加点云强度校正，利用激光雷达回波宽度等信息提高对车道线的检测精度。