摘要
介绍
II相关工作
III生成激光雷达地图
四、激光雷达的定位
A.航向角估计
B.水平定位
五、基于GNSS的定位
A. ins辅助模糊解决方案
B.相位转差检测0
传感器融合-toc" style="margin-left:0px;">VI.传感器融合
A. SINS运动学方程和误差方程
滤波器状态方程-toc" style="margin-left:0px;">B.滤波器状态方程
C.滤波测量更新方程
D.延迟处理
VII实验结果
A:定量分析
B.定性分析
C.详细分析激光雷达
D.运行时分析
VIII结论与未来工作
感谢
摘要
本文提出了一种鲁棒精确的定位系统，可以在不同的城市场景中实现厘米级的定位精度。我们的系统自适应地使用互补传感器的信息，如GNSS、激光雷达和IMU，在城市中心、高速公路、隧道等具有挑战性的场景中，实现高定位精度和弹性。与仅依靠激光雷达的强度或三维几何形状不同，我们创新地利用激光雷达的强度和高度线索来显著提高定位系统的精度和鲁棒。我们的GNSS RTK该模块采用多传感器集成框架，获得了较好的模糊分辨成功率。卡尔曼滤波法采用误差状态，采用新的不确定性估计方法集成不同源的定位测量值。我们详细验证了我们方法的有效性，实现了5-10cm的RMS精度超过了以前最先进的系统。
重要的是，我们的系统部署在一个庞大的自动驾驶团队中，使我们的车辆能够在拥挤的城市街道上完全自动驾驶，尽管道路建设不时发生。系统采用60多公里的城市道路真实交通驾驶数据集进行了全面测试。
介绍
车辆定位是自动驾驶的基本任务之一。全球导航卫星系统由于卫星轨道和时钟误差，对流层和电离层延迟(GNSS)单点定位精度约10米。这些误差可以通过测量参考站的观测值来校准。基于载波相位的差分GNSS技术被称为实时运动学(RTK)，cm定位精度[1]可提供。RTK最显著的优点是几乎提供全天候可用性。然而，它的缺点也很明显，由于依赖于精确的载波相位定位技术，很容易被信号堵塞和多路径。激光雷达是一种非常有前途的精确定位传感器。虽然相关工作在小雨[2]、雪[3]等解决这些问题方面取得了良好的进展，但在恶劣天气条件下和道路施工中的故障仍然是基于激光雷达方法的重要问题。此外，激光雷达和RTK是两种适用场景互补的传感器。
充满3的激光雷达D或而RTK出色的开放空间。惯性测量单元(IMU)，通过通常称为航迹计算技术的陀螺仪和加速度计，连续计算位置、方向和速度。这是一种不受干扰和欺骗的独立导航方法。但它深受一体化漂移的影响。
因此，每个传感器都有其独特的特性和工作条件。在这里，我们提出了在复杂的城市和道路场景中使用多传感器集成设计的自动驾驶车辆的鲁棒和精确定位系统。更准确地说，我们基于激光雷达，RTK和IMU不同的定位方法与传感器自适应相结合。如图1所示，我们系统的传感器配置。我们的系统为自动驾驶汽车中的其他模块提供稳定、灵活、准确的定位服务。自动驾驶汽车可以在市中心、隧道、林荫道、停车场和高速公路等多种复杂场景下驾驶。在动态城市和高速公路场景中，我们演示了大规模定位60多公里的数据。
在图2中，我们展示了多传感器集成框架的架构。
?自适应性集成了不同的传感器，包括激光雷达、RTK和IMU。它通过有效的不确定性估计，有效地利用它们的优势，使我们的系统在各种场景中避免它们的失败。
?一种激光雷达定位方法，自适应结合强度和高度线索，实现稳健和准确的结果，特别是在道路建设等具有挑战性的情况下，超过以往的工作。
?车辆定位系统每天对拥挤的城市街道进行严格的测试，使我们的车辆能够在市中心、高速公路和隧道等各种具有挑战性的场景中自动驾驶。
图2:结合传感器输入(紫色)和预先构建的系统架构概述LiDAR地图(黄色)估计自动驾驶车辆的最佳位置、速度和姿势(PVA)。GNSS和LiDAR卡尔曼滤波法采用误差状态估计PVA卡尔曼滤波法作为测量值提供预测的先验PVA。使用捷联惯性导航系统(SINS)加速度计测得的比力fb以及陀螺仪测得的转速ωbib作为卡尔曼滤波传播相的预测模型。将卡尔曼滤波器估计的加速度计与陀螺仪的偏差，PV-A将误差等修正量反馈给捷联惯导系统。
II相关工作
多传感器集成不是一个全新的想法。然而，整合多个传感器使整个系统准确、强大、适用于各种场景是一项非常具有挑战性的任务。答:Soloviev。[4]和Y。Gao等人[5]使用2D加上激光扫描仪GNSS和IMU实现了集成GNSS/LiDAR/IMU导航系统。由于激光雷达定位模块依赖于建筑墙体等特定功能，适用场景有限。激光雷达还可以在惯性传感器的帮助下定位自动驾驶车辆。
它提供定位测量，惯性传感器通常用于预测扫描之间的增量运动，以改善点云匹配。其中包括[6]、[7]和[8]。它们的定位方法仅依赖于激光雷达RTK是激光雷达的完美补充。RTK在道路建设的空地或街道上发挥着重要作用。
基于激光雷达的车辆本地化近年来非常流行。R. Kümmerle和W. Burgard[9]利用相应环境的多层次表面映射，开发了一种基于粒子滤波定位自己的自治系统。K. Yoneda和S. Mita[10]通过使用迭代最近点(ICP)方法[11]将实时扫描点云与地理参考地图对齐，本地化自动驾驶车辆。然而，众所周知，CP等方法对最初的猜测非常敏感。在没有丰富的3D特征(如高速公路或其他开放空间)的场景中，它们会失效。与我们最接近的作品是J. Levinson和S. Thrun[2]，[12]。他们提出了一种基于激光雷达强度的定位方法。与仅仅基于点云的3D几何形状的定位系统相比，激光雷达强度提供了更多的环境纹理信息，作为有价值的额外线索。
我们对该方法进行了多方面的改进，包括一个新的图像对齐步骤来改进航向角估计，一个包含强度和高度的联合代价函数来获得稳健的结果，以及一个新的步骤来估计结果的协方差矩阵。目前所提到的方法都是针对多层激光雷达设计的，例如Velodyne HDL-64E、Velodyne HDL-32E甚至Velodyne VLP-16。预计这些激光雷达扫描仪的零售价格将迅速下降，因为在我们写这篇文章的时候，有40多家制造商在这个上升的领域竞争。与使用多层激光雷达不同，works[13]，[14]，[15]，[16]，[17]，[18]，[19]尝试使用2D或低端激光雷达完成类似的任务。
III激光雷达地图生成
我们基于激光雷达的定位模块依赖于预生成的地图。我们的目标是获得环境的网格单元表示。每个单元存储激光反射强度和高度的统计信息。在[2]和[12]中，每个单元都用单个高斯分布模型表示，该模型保持强度的平均值和方差。
在[20]中，这被扩展到高斯混合模型(GMM)，同时保持强度和高度。在我们的工作中，我们仍然使用单高斯分布来模拟环境，但同时涉及强度和高度，如图3所示。我们发现，通过自适应加权方法结合代价函数中的强度和高度测量值，可以显著提高定位精度和鲁棒性。这将在第四- b节和第七- c节详细讨论。
四、基于激光雷达的定位
我们的定位系统联合估计了位置、速度和姿态(PVA)。但是，我们在LiDAR定位模块中寻找一个只包含三维位置和航向(x, y, a, h)的最优解。这里(x, y)是一个二维笛卡尔坐标，来自于一个正形投影，如通用横墨卡托(UTM)。
我们预先构建的地图包含了路面高度的统计数据。我们可以通过假设车辆在路面上行驶得到高度估计值a。[20]和[21]对(x, y, h)进行穷举搜索，导致计算复杂度为O(n3)。为了更好的效率，我们有一个独立的h估计步骤，基于Lucas-Kanade算法[22]和一个直方图过滤器用于水平(x, y)估计。请参考算法1来获得完整的一步一步的概述。
图3:包括激光强度和高度统计数据的预建激光雷达地图示例。(a)用激光强度值绘制的覆盖3.3km × 3.1km区域的预建地图。(b) (A)的放大图。(c) (A)同样的放大图，但呈现在高度A上。
A.航向角估计
与地图生成部分的步骤类似，我们再次将在线点云投影到地平面上，生成了与图3类似的强度图像，但由于在线点云的稀疏性，图像中填充强度值的像素稀疏。
将该稀疏图像与预构建的LiDAR地图进行匹配，得到定位结果(x, y, h)。在航向角h估计步骤中，应用了Lucas-Kanade算法[22]，这是一种利用图像梯度搜索两幅图像之间的最佳匹配的技术。更准确地说，应用了一种Lucas-Kanade算法，即正向加性算法[23]。它本质上是一种高斯牛顿梯度下降非线性优化算法。
我们验证了Lucas-Kanade算法可以利用在线强度收敛到分度精度的解作为模板映像。战术级MEMS IMU本身无法提供足够精确的航向角估计。这一步骤的效果见第七- c节表二。虽然Lucas-Kanade算法可以产生水平翻译估计，但在实践中我们发现它不够准确和稳健。因此，我们只使用旋转分量作为航向角估计，用于下一步的水平定位。
B.水平定位
直方图滤波是一种非参数化方法。它通过将状态空间分解为有限多个区域来逼近后验，并用单个概率值表示每个区域的累积后验。为了完备性，我们引用[24]中直方图过滤器的预测和更新阶段的方程:
式中，Pk,t表示t时刻各状态的信念xk, ut为控制输入，zt为测量向量。
我们将直方图过滤器应用于水平定位。
状态包含(x, y)。我们使用直方图滤波的原因是后验密度的累积概率是以离散的方式计算的。这种穷举搜索确保了最优解。方程1和2分别对应预测和更新步骤，我们的方法对应的方程分别为方程3和方程4。
1)预测步骤:预测步骤是根据滤波器和控制变量的历史分布或运动模型对状态的新信念进行预测。在此，我们利用SINS的运动预测直接更新直方图滤波中心，并利用高斯噪声随机游走更新置信分布。
之前的工作[2]，[12]依赖于预融合的GNSS/IMU解决方案。一个完整的多传感器融合框架允许我们的系统在各种具有挑战性的环境下工作。
因此，预测步骤更新每个单元格的概率如下:
其中，¯P (x, y)是预测概率，P (i, j)是SINS运动更新后的概率。其中σ是描述两帧间漂移速率的参数。
2)更新步骤:直方图滤波的第二步是测量值更新步骤，每个状态的后验信念估计公式如下:
其中z为在线激光扫描构建的在线地图，方法与第三节映射过程相同，Nz为在线地图有效数据单元格数。m是预构建的LiDAR地图，(x, y)是运动姿态，η是归一化常数，κ是PxyP(z|x,y,m)和Pxy¯P(x, y)分布之间的Kullback-Leibler散度[25]。KL散度描述了鉴别信息，用于平衡预测的影响。与[12]不同，我们摒弃了GNSS/IMU姿态的不确定性。我们有一个更全面的融合框架，融合了包括GNSS在内的各种来源的输入。
将在线点云与预构建的地图进行匹配，计算出概率P (z|x, y, m)。我们通过建立一个带有动态加权参数γ的代价函数来自适应地融合强度和高度测量:
式中zr、za为在线传感器输入的强度和高度测量值。强度部分形式化如下:
式中，rm、rz分别为预建图和在线图的平均强度值。σm和σz表示强度值的标准差。
SSD (Sum of Squared Differences)用于评估传感器输入和地图之间的相似性。方差项隐含地减少了环境变化的影响。两个显著不同且方差均较低的强度值暗示着环境的变化。同样，高度的SSD考虑如下:
其中am和az分别表示预建地图和在线地图的平均海拔高度。我们在SSDa中去掉方差项，因为高度在垂直方向上是自然变化的。
P(zr|x, y, m)和P(za|x, y, m)的似然定义为:
当参数α = e时，它们是类高斯概率分布。我们改变α来调整它的平滑度。
自适应权重参数γ在强度融合和高度融合过程中起着重要作用。我们让它由[P (zr|x, y, m)]xy和[P (za|x, y, m)]xy的方差决定。
方差由下列方程定义:
其中¯x和¯y是分布的质心。
因此，强度和海拔的x、y方差表示为σ2x(r)、σ2y(r)、σ2x(a)、σ2y(a)。重量γ的计算公式为:
3)最优偏移量:根据直方图滤波器的后验分布估计最优偏移量。而不是使用直方图滤波器的所有状态来计算最佳偏移量，我们只使用最大或第二大后验信念状态周围的一个小的平方区域。如果第二大后验信念的值与第二大后验信念的值达到给定的比例，并且更接近直方图滤波的中心，我们将第二大后验信念的状态作为小平方区域的中心。
否则，我们取最大后验信念的状态。假设小面积为Z，最优偏移量(ˆx，ˆy)计算如下:
4)不确定性估计:在第6节中，定位结果用于更新卡尔曼滤波器，关键问题是与状态估计相关的不确定性的评估。我们让向量~td =(ˆx，ˆy)T和~tx,y = (x, y)T分别是优化的偏移量和(x, y)单元格的偏移量。得到的协方差矩阵Cxy可以计算为:
在实验部分的图6中，我们展示了滤波器分布、估计协方差矩阵以及状态估计的样本。我们观察到，与地面真实值相比，协方差矩阵估计与观测误差是一致的。
五、基于GNSS的定位
RTK算法的实现充分利用了其他传感器的特性。在这里，我们介绍了传感器融合框架是如何帮助RTKmodule的，但没有介绍RTK本身实现的细节。在不损失一般性的前提下，系统s的i号卫星上，探测器(r)与基座(b)之间的GNSS单差(SD)伪距和相位观测可以构建为:
其中∆表示单次差分计算，ρ为伪范围，其中N为φ (m)， N表示待解决的歧义，λ为卫星i的某个波段的波长。R为几何距离，l为观测矩阵，均由卫星位置计算，月球车先前位置为x0r，预估修正量为dxr。
t为待估计的相对接收器时钟偏移量，C为光速。其中n和ρ分别是相位和范围上的噪声。
为了提高模糊度分辨成功率，我们利用了现有的GPS、北斗和GLONASS观测数据，估计GLONASS的频间偏差为[26]。利用最小二乘法得到SD浮点二义性及其协方差，利用变换矩阵将其转换为整数性质的双差分(DD)二义性，然后利用MLAMBDA[27]对二义性进行求解。
虽然模糊解决(AR)RTK解决方案是首选，但确实有很多情况下，当存在多个多路径和信号阻塞时，很难解决歧义，例如，在城市建筑下或在森林中，只有浮动的模糊RTK解决方案或基于代码的差分GNSS可获得亚米精度。总体而言，在我们的框架中，使用GNSS定位结果ar -RTK或float RTK，用对应的不确定度δg更新卡尔曼滤波器，由:
其中V和P分别表示SD观测的后验残差和权矩阵，其中数为n, r为式13中估计的状态数，由式13导出，B为观测矩阵，其第i行Bi =[∆li C]。
A. ins辅助模糊性解决
在没有其他传感器辅助的情况下，模糊消解的成功率很大程度上依赖于伪距离精度，而在多路径服务的城市建筑下，模糊消解的成功率会大大降低。然而，在我们的工作中，受激光雷达(第4节)和/或GNSS约束的INS可以为缩小模糊度搜索空间提供很好的预测。例如，当通过没有GNSS信号的隧道时，该框架可以继续使用激光雷达约束INS误差，并提供精确的位置预测，以帮助解决歧义，直到重新获得GNSS信号。
目前，我们的工作仅松散耦合传感器观测(紧密耦合是未来的计划)，这里我们简单地将INS集成结果xins作为一个虚拟观测，其方差Rins:ˆxr = xins。
B.相位转差检测
当GNSS接收机失去对信号跟踪的锁定时，载波相位测量可能会突然发生跳变，称为周期滑移，这将迫使整数模糊度的中断，并使定位恶化。此外，在GNSS信号普遍受阻和反射的城市环境下，周期滑移比静态开放条件下发生的频率要高得多，需要对其进行检测和修复，以提高移动车辆的RTK性能。
在这里，我们根据卫星几何形状，在对流层和电离层延迟保持不变的情况下，估计月球车位置增量偏移和接收器时钟漂移在两个连续时间点(比如k和k−1)之间为5Hz。时差观测可记为:
利用SD伪距估计SD相位模糊度。再次，利用变换后的DD浮点二义性及其协方差，利用MLAMBDA引擎获得固定二义性。显然，固定DD歧义向量中的非零元素表示相关载波相位上的周期滑移，这值得在第V-A节中采用新的歧义解决过程。
VI.传感器融合
在我们的融合框架中，一个误差状态卡尔曼滤波器被用于融合定位测量，在上节讨论，与IMU。该融合框架可以优化结合来自IMU的定向速率和加速度计信息，提高精度。IMU足够精确，可以在LiDAR和RTK测量之间提供稳健的状态估计。
A. SINS运动学方程和误差方程
捷联惯性导航系统(SINS)通过集成IMU数据来估计位置、速度和姿态。本文选取东南北上坐标系(ENU)作为导航坐标系(n)，右前上坐标系(RFU)作为体坐标系(b)[5]，地球坐标系(e)和惯性坐标系(i)[29]。首先，捷联惯导n坐标系中的微分方程[5][29][1]被称为:
式中r = (λ，L, a)T为车辆位置;V n是车辆速度;Q nb是b坐标系到n坐标系的姿态四元数;C nb是b坐标系到n坐标系的方向余弦矩阵;G n是重力;Bg为陀螺仪偏差;Ba是加速度计的偏差;ωbib, f b分别为IMU陀螺仪和加速度计输出;ωγ αβ是β坐标系相对于α坐标系的角速率，在γ坐标系中分解;⊗是四元数乘法运算符;Rc将速度的积分变换为经度λ、纬度L和海拔a，且Rc = diag(1 (RN +a)cos(L)， 1 RM +a, 1)，其中RN、RM分别为横半径和子午半径。
对于战术级MEMS IMU, IMU偏差可以建模为一个定值模型。当SINS与RTK或激光雷达等辅助传感器结合使用误差状态卡尔曼滤波器时，需要建立SINS误差模型。速度、位置、姿态误差的ψ角模型误差方程在导航框[30]中表示为:
式中δv n， δr， δf b， δωbib分别为v n, r, f b， ωbib的误差;δψ是姿态角的误差。
B.滤波器状态方程
由于捷联惯导系统的误差随着时间的增长而增长，为了得到精确的PVa，我们使用误差状态卡尔曼滤波器来估计捷联惯导系统的误差，并使用估计的误差状态来校正捷联惯导系统。
特别是对于式16中的qnb，当假定其在0附近是一个非常小的角时，摄动四元数δq nb可以表示为一个小的角近似:
因此，我们选择状态变量X =?r v n q nb ba bg ?T，状态变量误差为δX = ?δr δv n δψ δba δbg ?T。
由捷联惯导误差方程(17)和IMU模型，可以得到卡尔曼滤波器的状态方程如下:
其中W = ? W a W g W ba W bg ?T为系统噪声，包括IMU输出噪声和IMU偏置噪声。
(·×)表示向量的斜对称矩阵。
F (X)和G(X)可以表示为:
C.滤波测量更新方程
测量更新包括激光雷达和GNSS部分。误差状态卡尔曼滤波器的测量更新步骤是在给定全局校正Z的情况下更新状态的不确定性。其余的时间或测量更新只是遵循标准卡尔曼滤波。
1) LiDAR测量更新方程:基于LiDAR的定位输出车辆的位置和航向角作为滤波测量。测量更新方程可表示为:
其中下标为S的变量是msins的预测，下标为L的变量是LiDARmeasurement。V L是均值为零的激光雷达的估计噪声，其协方差矩阵为RL。由式(12)计算RL中λ和L对应的部分。
我们设a和h的部分为常数。H L可由下式得到:
其中cij是cnb的第i行第j列元素。
2) GNSS测量更新方程:GNSS可以估计车辆的位置。测量更新方程可表示为:
式中，下标为G的变量为GNSS测量值。V G为均值为零的GNSS的估计噪声，其协方差矩阵为式(14)得到的RG。H G定义为:H G = ?I3×3 03×12 ?
D.延迟处理
由于传输和计算的延迟，必须考虑测量的延迟和无序。解决方案是，我们维护两个滤波器和一个固定长度的滤波器状态缓冲区，滤波器测量和IMU数据，按时间顺序。filter-1在接收到新的IMU数据时，通过执行时间更新和即时整合IMU数据，计算实时PVA及其协方差。filter-2处理延迟测量。当接收到t1处的新测量值时，我们执行以下操作:
1)从缓冲区中获取t1时刻的滤波器状态。更新filter-2中的过滤器状态。
2)在filter-2中执行t1测量更新。
3)使用缓冲区中的IMU数据在filter-2中执行时间更新，直到到达当前时间。或者我们在t2处停止，如果在t2处有另一个测量值缓冲，t2晚于t1。接收t1和t2的测量值的顺序是错误的。
4)如果在t2有另一个测量，则在t2执行测量更新。然后重复步骤3，以错误的顺序找到更多的测量结果。
5)当我们完成时间更新到达当前时间时，过滤器状态在缓冲区中，根据t1到当前时间的新结果更新filter-1的状态。
图4用于说明这个过程。
图4:延迟和无序测量处理。ta、tb和t3分别表示IMU时间序列、测量发生时间序列和测量接收时间序列。矩形和星形代表两个接收顺序错误的测量值。
VII实验结果
我们的测试平台如图1所示。采用离线方法生成真实地面的车辆运动轨迹，用于定量分析。在GNSS信号接收良好的开放空间，基于后处理算法的GNSS/INS解决方案，如NovAtel惯性探测器，能够产生足够准确的车辆运动轨迹。在GNSS信号较弱的场景下，如复杂的城市道路，我们将其作为一个经典的地图重建问题，结合了NovAtel IE后处理、LiDAR SLAM、环路闭合和全局位图优化等技术。我们只在演示视频片段中对拒绝gnss的场景(如隧道或地下车库)显示定性结果。因此，我们将我们的测试数据集(总共60公里)分为三个大类:1)48.1公里的常规道路:YF-1、YF-2、YF-3、YF-4、YF-5，涵盖了常见的道路条件，如城市、农村和交通拥堵。2) 10.4公里弱GNSS信号道路:HBY -1、DS-1覆盖高楼或树木林立的狭窄道路。
3) 2.1km gnss阻断道路:DT-1覆盖隧道。
A:定量分析
我们的系统已经在真实的驾驶场景中进行了广泛的测试。我们将我们的定位性能与Levinson等人提出的最先进的基于强度的定位方法进行了比较。为了明确展示不同传感器的贡献，测试结果分为两种模式:1)2- systems: LiDAR + IMU 2) 3-Systems: LiDAR + GNSS + IMU。在表1中，我们分别给出了常规和弱GNSS道路的量化结果。请注意，与[12]相比，我们的性能有了巨大的提高，并且在常规和弱GNSS场景下都能实现稳健和准确的定位，精度达到厘米级。“2-系统”和“3-系统”都运作良好，这表明我们的系统不依赖于单一系统传感器，但融合传感器输入使用弹性和自适应的方法。我们在图5中显示了横向和纵向误差随时间的变化。如前所述，我们提出的解决方案能够在一段时间内始终如一地实现比[12]更好的性能。
B.定性分析
对于拒绝gnss的道路，由于缺乏地面真实情况，我们不进行定量比较。在我们的其他视频片段中，我们展示了我们的结果与NovAtel的GNSS RTK/IMU姿势之间的定性比较。我们之所以没有展示[12]的结果，是因为NovAtel的RTK/IMU姿势不够稳定和平滑，导致[12]失败。
在图6中，我们对系统中各个模块和功能进行了详细的性能分析。(a)展示了一个典型的案例，RTK和激光雷达都给出了良好的结果，融合系统也一样。在(b)中，由于地图过时，激光雷达失败。然而，在RTK的辅助下，熔融系统获得了良好的结果。我们在(c)中展示了一个相反的例子，由于信号堵塞，RTK较差，而激光雷达处于良好的工作状态。(d)及(e)证明我们的系统在人或车辆拥挤的场景中表现良好。(f)展示了一个非常有趣的例子，有一条新铺的路和一堵最近修建的墙。激光雷达不能处理如此重大的环境变化仅基于强度线索。当我们适应地融合额外的海拔线索时，它会给出很好的结果。
表一:与[12]的定量比较。结果表明:1)2- system: LiDAR + IMU;2) 3-系统:激光雷达+ GNSS + IMU。在普通道路上使用导航卫星系统的好处显而易见。我们的定位误差远低于[12]。
C.激光雷达的详细分析
为了证明上述每一个贡献在第IV节的有效性，我们在表II中使用2-系统展示了不同方法的定位精度。我们还引入了一种特殊的数据日志(YF-6)，其中包括地图数据收集后新铺设道路的区域。强度表示基线方法，只使用强度线索。在Heading中，我们加入了Heading估计步骤，这对低等级IMU特别有帮助。在FixedAlt中，我们在激光雷达匹配期间合并海拔线索。请注意这个巨大的改进，它清楚地证明了海拔线索的有效性。此外，在AdaptAlt中，我们将高度线索与适应性权重相结合。从表2中YF-4的结果可以看出，我们的航向角优化步骤对激光雷达定位至关重要。表II中YF-6的结果表明，我们的自适应加权策略使系统对环境变化更具鲁棒性，例如道路建设，以及季节变化。显然，这为我们提供了所有指标中最低的本地化错误。
表二:本系统使用的各种定位方法的定位误差比较。每个改进的航向角、高度线索和自适应权重的好处都是显而易见的。
D.运行时分析
我们的系统主要包括三个模块:激光雷达、GNSS和SINS。激光雷达、GNSS和捷联惯导系统的工作频率分别为10hz、5hz和200hz。有两种针对不同应用程序的实现。其中一个只占用一个CPU核。另一种是使用单CPU核加上FPGA。在单核版本中，我们通过使用更小的直方图滤波尺寸和在评估航向角时降采样数据来减少计算量。这两个版本在准确性方面提供了类似的定位结果，但较大的直方图滤波器大小可能会增加在异常事件中滤波器漂移时收敛的可能性。GNSS和SINS模块只需要0.2个左右的CPU核心。
VIII结论与未来工作
我们提出了一个完整的定位系统，专为全自动驾驶应用。它自适应融合来自互补传感器的输入，如GNSS, LiDAR和IMU，在各种具有挑战性的场景，包括城市市中心，高速公路或高速公路，隧道，实现良好的定位精度。我们的系统在纵向和横向上都达到了5-10cm的RMS精度，并通过具有不同计算硬件要求的两个版本为工业使用做好了准备。
我们的系统部署在一个庞大的自动驾驶车队中，使我们的车辆每天在拥挤的城市街道上实现完全自动驾驶。我们的融合框架的通用性意味着它可以用于融合更多的传感器，在不同的成本水平，面对不同的应用。事实上，我们已经开始用低成本、低端的MEMS IMU测试我们的系统。我们未来的工作还包括为ADAS或3级自动驾驶汽车构建低成本的本地化解决方案。
感谢
我们要感谢我们的同事在整个项目中对我们的热心帮助和支持。陆伟新协助车辆准备工作。姚周和王成帮助制作了演示视频。易世春、李玉和王成生成了激光雷达地图。Nadya Bosch帮助进行文本编辑。
图6:各单项功能的性能分析。第一行是场景的前置摄像头图像。第二行的数字表示我们系统的性能。绿色、蓝色和红色椭圆分别代表Fusion、LiDAR和RTK方法定位估计的不确定性。角落里的蓝色小图代表了LiDAR模块的直方图滤波中的后验密度。图(a)至(f)显示我们的系统在不同情况下的表现。(a)激光雷达和RTK都有很好的结果。(b)由于地图过时，激光雷达的结果很差。(c)由于信号阻塞，RTK结果不稳定。(d) - (e)系统在人群或汽车周围表现良好。(f)表明，当只使用强度线索时，激光雷达失败，但当我们在新铺设的道路上使用强度和高度线索时，激光雷达成功。