定位和导航的需求不断增长。目前，全球导航卫星系统（GNSS）被认为是全球定位服务的重要手段。然而，由于严重的信号衰减、反射和阻塞，其在城市地区的应用受到了限制。惯性导航系统（INS）可以在短时间内提供高精度的导航输出，但其精度存在误差累积的问题，特别是在配备低成本的2微纳制造的（MEMS）惯性测量单元（IMUs）时。此外，光检测和测距（LiDAR）在车辆中越来越普遍，它可以检测环境中丰富的几何信息，从而进行自我运动估计。针对利用这些车载技术的互补特点在城市环境中导航，提出了一种紧密耦合的多GNSS精确点定位（PPP）/INS/激光雷达融合系统。我们还开发了一种激光雷达滑动窗口平面特征跟踪方法来进一步改进导航精度和计算效率。在不同的GNSS观测条件下的车辆实验中，评价了所提出的集成系统的性能。结果表明，我们提出的GNSS/INS/LiDAR集成可以在具有GNSS挑战性的环境中保持亚米水平的水平定位精度，与传统的GNSS/INS集成相比，其提高了（73.3%，59.7%，64.2%）。此外，该平面特征跟踪方法在精度和效率方面都优于传统的点对线和点对平面扫描匹配方法。
在一些位置驱动的物联网（IoT）应用程序中，高精度导航和连续导航解决方案已经变得至关重要，比如自动驾驶的[1]-[3]。然而，在复杂的场景[4]中，一个独立的传感器通常很难满足高精度导航的需求。因此，多传感器融合可以利用异构传感器的互补特性，可能是保证城市环境[5]导航性能的可行解决方案。
目前，精确点定位（PPP）技术已经成为一种很有前途的提供全天候定位服务的方法，特别是在卫星轨道和时钟[6]的精度显著提高之后。不同于差分全球导航卫星系统（GNSS）技术，PPP可以有效、灵活地实现相当的定位精度。
随着多GNSS星座的快速发展，采用GPS、BDS、格拉斯和伽利略观测的多GNSS 的PPP比GPS的PPP [7]具有更高的精度和更快的再收敛速度。当使用来自不同系统的GNSS观测时，存在系统间偏差（ISBs），应该被考虑在内[8]。然而，PPP在卫星可用性差或弱星座几何[9]中容易退化。同时，惯性导航系统（INS）可以在不需要外部支持[10]的情况下提供高频、连续的导航输出。因此，一些研究将INS与GNSS相结合，以提高在露天天空和城市环境[11]-[14]中的定位性能。其中，GNSS和INS的松散耦合集成由于其简单性而被广泛应用，它融合了INS测量的位置[11]和GNSS测量的位置[11]。但是，当子系统无法工作[12]时，导航解决方案将是不连续的和脆弱的。在[13]中，紧密耦合的PPP/INS融合已被证明在准确性和鲁棒性方面优于松散耦合的融合。
除了提高导航精度外，INS还可以帮助恢复GNSS数据间隙，并以紧密耦合的方式通过[14]缩短模糊性重新初始化的时间。然而，在GNSS中断时，特别是在配备低成本微机电系统（MEMS）惯性测量单元（IMUs）时。因此，在具有稀疏GNSS观测的城市环境中，从其他传感器中进行的改进是非常可取的。
光检测和测距（激光雷达）作为一种外感受传感器，可以检测到详细的结构信息，用于自我运动估计。大量的研究工作已经致力于解决激光雷达扫描之间的相对姿态，这是基于激光雷达的导航解决方案的基本概念。迭代最近点（ICP）算法[15]及其变体[16]是最常用的估计相对姿态变化的方法。然而，这些方法依赖于初始值，在具有垂直稀疏和圆形结构的环境中可能会失效。为了解决这一问题，一些研究将原始激光雷达点云转换为更有效的表示方法，如线段[17]、车道标记[18]和曲率函数[19]。以至于在建设方面残差、点对点距离和点对点平面距离由于其有效性和简单性，在以往的实现中得到了广泛的应用。在众所周知的激光雷达里程计和建图（LOAM）框架[20]中，提取低曲率或高曲率的边缘和平面特征来构造两次扫描的约束（点到平面和点到线距离）。这些几何约束是有效的，但当它们跨多个连续的激光雷达帧执行时，它们将非常耗时。受视觉特征跟踪的启发，[21 Bundle Adjustment for Lidar Mapping]和[22]的作者在多次扫描中提取和跟踪平面特征，这可以克服典型的帧对帧跟踪方法的次优性。此外，在[23]中提出了一种新的自适应体素化方法来有效地搜索平面特征对应关系。近年来，深度学习方法[24]在点云处理中也引起了广泛的关注。但这些数据驱动的方法是否足够有效和通用，仍然值得探索。
基于三维激光雷达测量的成功处理，GNSS、INS和激光雷达的融合成为可能，并迅速增殖[25]-[31]。正如[25]和[26]所研究的那样，高频GNSS/INS姿态估计分别被反馈到激光雷达模块中，作为精确扫描匹配和森林项建图的先验值。基于扩展卡尔曼滤波器（EKF），[27 INS/GPS/LiDAR Integrated Navigation System for Urban and Indoor Environments Using Hybrid Scan Matching Algorithm]采用了一种创新的混合扫描匹配算法，进一步提高了松散耦合GPS/INS/LiDAR集成的精度和稳定性。在两个相邻时期之间的GPS位置和激光雷达姿态估计提供了周期性的修正来抑制INS的发散。此外，[28]提出了一种基于优化的平滑映射激光雷达惯性里程计（LIO-SAM），该方法通过融合IMU预积分因子、激光雷达测程因子和可选GPS位置因子得到最优估计。
为了避免松散耦合融合[29]中的信息丢失，提出了一种紧密耦合RTK/INS/LiDAR集成系统，用于GNSS观测条件[30]下的导航。利用差分GNSS技术对激光雷达特征的全局坐标进行了精确的预先登记，并在GNSS退化环境中提供了有效的位置约束。此外，[31“GIL: a tightly coupled GNSS PPP/INS/LiDAR method for precise vehicle navigation]最近的一项工作将[20 LOAM]中的扫描匹配模块扩展到多帧，并开发了一个基于EKF的GNSS/INS/LiDAR估计器，可以在城市环境中保持米级定位精度。在滑动窗口中每两个激光雷达帧之间的扫描匹配将导致沉重的计算负担。此外，帧间间隔越大，导致点云之间的相似性较低，难以建立准确的数据关联。
以往关于GNSS、IMU和LiDAR融合的工作主要使用GNSS位置或集中于松散耦合积分。在这篇文章中，我们提出了一种紧密耦合的多GNSS PPP/INS/激光雷达融合方法，以实现在城市环境中精确和连续的导航。原始测量从GNSS载波相位和伪距观测，MEMS-IMU和提取的激光雷达平面特征通过一个集中的EKF在观测水平上直接融合。
此外，受[23]和[32]的启发，开发了一种改进的平面特征提取、平面特征合并和数据关联等改进的平面特征跟踪方法，以进一步提高计算效率和导航精度。在GNSS部分阻塞和GNSS困难环境下进行了车辆实验，以评估该系统的有效性。
本文的主要贡献可以总结如下：
●据我们所知，这是第一次原始GNSS测量（即伪距和载波相位测量），MEMS-IMU数据和激光雷达平面特征紧密融合在一个一致的基于滤波器的框架，这可以有效地克服独立传感器的缺点，避免信息损失松散耦合方案。
●提出了一种具有多种检查机制的激光雷达滑动窗平面特征跟踪方法，并将其引入到该集成系统中。与传统的使用点对点和点对点平面测量的扫描匹配方法相比，该方法在效率和精度方面都表现得更好，这对实时传感器融合至关重要。
●在不同GNSS观测条件的城市环境下评估了该系统的导航性能，并利用GNSS和点对点测量[31]与GNSS/INS集成和GNSS/INS/LiDAR集成进行了仔细比较。此外，我们还对平面特征跟踪方法进行了详细的实时效率分析。
高鲁棒性和高效性对于处理原始激光雷达点云至关重要。然而，原始激光雷达点云的数量通常是巨大的，在实际应用中单独处理每个点是不必要的和计算上不可行的。同时，单个点所包含的信息非常有限，很难在多个激光雷达帧之间建立稳定的数据关联。为了应对这些挑战，我们开发了一种改进的平面特征跟踪算法，具有多种检查机制，以在滑动窗口中的多个激光雷达帧上有效地跟踪三维环境平面特征。根据[32]中的基本思想，该平面特征跟踪方法可分为三个步骤：特征提取、特征合并和数据关联，这将在下一节中详细介绍。根据[33]，每个平面特征都可以由法向量和中心点组成的6自由度的（6-DOF）向量表示。为方便起见，以下公式中的平面特征简称为PF。
正如[20]中众所周知的LOAM在[20]中所做的那样，我们首先从原始点云中提取具有低局部平滑度的点作为候选点。然后，对于每个候选点，搜索并选择有限距离内的周围点，在图1左侧用蓝色标记。
一旦所选点可用，就可以通过[34]来计算中心点(c)：
根据[34]，在平面上采样的单个点满足以下约束：
n=[A B C]即为平面的法向量，
为平面上的矢量
通过将所有点叠加在同一平面上，可以构造一个最小二乘问题来求解法向量为(3)：
为了在提取中剔除可能的病态平面，我们计算了从所有点到该平面的距离之和，并排除了那些大于阈值的距离。
该阈值的经验值设置为0.4 m [35]。虽然也有一些其他的方法通过计算平面[22]的不确定性来过滤非条件平面，但我们的方法同时考虑了效率和稳定性。在(1)和(3)的基础上，我们可以得到经过初步筛选的所有平面特征的法向量和中心点。然后，提取出的平面特征将形成一个新的平面点云，这将成为后续平面特征合并的基本操作对象。请注意，我们建议kd-tree在所有搜索过程中进行快速索引。
在以往的平面特征提取中，平面由候选点和一定数量的周围点决定。这种方法可能会导致以下一些问题： 1)重复性和冗余性的几何信息。特别是当激光雷达扫描结构环境时，不同的平面特征通常代表相同的表面，如墙壁和广告牌。2）可靠性和稳定性较低。为了节省计算成本，选择的周围点的数量通常小于20个，这可能导致脆弱和不良的平面。3)计算复杂度和内存需求较高。提取操作相对耗时，提取后的平面特征数量较大。如果它们直接在测量更新中使用，计算负担仍然很大。
因此，迫切需要一种能够减少冗余度和提高提取平面稳定性的合并操作。对于中选择的每一个，kd树被用来搜索它的邻近树。然后，我们可以构造以下残差来确定这两个平面是否可以被视为同一平面，我们选择平面特征, 其相邻特征为
其中(5)中的第一项表示两个平面之间的并行性，第二项和第三项为双向距离检查，分别表示从第一个中心点到第二个平面特征的距离和到第二个特征点到第一个平面的距离。如果残差小于经验阈值（法向差为0.2rad，距离差为0.4m），则将两个平面特征合并，如图1右侧所示。此外，相应的中心点和法向量也将被重新计算在(1)和(3)合并之后。此过程将重复多次，以确保合并所有相同的平面特征。最后，只保留了具有较高局部稳定性和帧间重合的平面特征。
算法1：
输入：给定一个传入的激光雷达扫描
输出：在滑动窗口的平面特征对应激光雷达帧
●使用系统预测姿态后进行去畸变[33]
●提取平面特征从原始激光雷达点云
1)计算中心点公式(1)
2)计算法向量公式(3)
3)残差检查如公式(4)
●合并平面特性在局部的特征平面
1)为每个平面特性，找到最近的对应平面特征
2)构造残差向量(5)
3)确定两个平面特征是否代表相同的地标通过检查并行性和双向点对点距离
4)如果应该是相同的地标，合并两个平面特征和重新计算范数向量和中心点第二-A
5)重复多次以确保所有同一平面合并
●构建平面数据关联跨多个帧滑动窗口
1)在最古老的每个平面特征帧，找到最相邻的平面特征
2）将残差向量构造为(6)
3)如果残差大于阈值，则剔除相关特征
4)执行RANSAC以进一步去除离群值
在滑动窗口中寻找多个激光雷达帧之间的平面特征对应关系。假设一个平面特征在最古老的激光雷达帧m中被i索引。它将被投影到另一个激光雷达帧，以找到其最近的平面特征的索引。根据[32]，我们可以通过检查残差来确定两个平面是否代表相同的位置，可以用以下描述：
如果残差大于经验阈值（法向差为0.35rad，距离差为0.5m），我们将拒绝将平面特征与平面特征联系起来。在获得激光雷达帧之间的所有特征对应后，我们建议RANSAC[36]进一步去除时间跟踪中的离群值。在算法1中简要介绍了平面特征跟踪方法。
为了实现这个融合系统，我们选择了一个EKF来融合来自GNSS、INS和LiDAR的原始测量数据。在本节中，我们首先描述紧密耦合的GNSS/INS/LiDAR集成的系统结构。然后，按照标准的EKF例程，进行滤波器描述、状态预测、多GNSS PPP测量更新和激光雷达测量更新均按顺序表示。
图2所示为所提出的GNSS/INS/LiDAR融合系统的系统结构。自给式惯性传感器因其采样率高、故障率低而被认为是核心传感器。经过参数初始化和初始动态对齐[37]后，INS机械化可用于状态预测。利用欧洲轨道确定中心（CODE）提供的精确轨道和时钟产品，原始的GNSS载流子相位和伪距观测将进行周期滑移检测[38]和异常值检测[39]。对于激光雷达部分，将处理原始的激光雷达扫描点，包括平面特征提取、平面特征合并和数据关联，详见第二节。如果有预处理过的GNSS或激光雷达观测数据，则将依次执行相应的测量更新。最后，将反馈估计的IMU偏差，以纠正下一个IMU数据。
完整状态向量由INS状态、PPP状态和激光雷达状态组成，可表示为：
其中上标和e分别为body坐标系（b帧）和ECEF坐标系（e系），符号分别表示e帧的三维姿态、速度和位置误差；在(8)中，在线估计加速度计偏置ba和陀螺仪偏置bg，并分别建模为白噪声na和ng驱动的随机行走过程[40]。
在PPP状态参数化方面，我们遵循标准的多GNSS PPP处理[7]，并应用无电离层（IF）组合，以消除一阶电离层延迟。对应的状态向量可以写成：
上式(9)第一项为GPS接收机时钟，
则分别代表北斗和Galileo相对于GPS系统的ISB。
ZWD表示天顶对流层延迟的湿分量的误差，
是三个卫星系统的中频模糊度校正向量。关于PPP误差状态构建的更多细节请参考[40]。
利用多状态约束卡尔曼滤波（MSCKF）[42]的基本思想，在过去的状态向量中保持一个克隆的滑动窗口，用于构造激光雷达约束。请注意，提取的激光雷达平面特征将被边缘化，并在[43]后从状态向量中移除。因此，激光雷达的状态可以写成：
其中，l符号表示激光雷达坐标系（l-系），遵循右上移顺序；分别表示激光雷达姿态和位置误差；k为激光雷达姿态的预定义滑动窗口大小。（多对位置和姿态组成的）
在此步骤中，IMU数据被用于状态预测。INS连续动力学模型可以表示为：
上式坐标的项分别代表了姿态的导数、速度的导数、位置的导数。R为余弦矩阵，从base到地球e系得余弦矩阵。为地球的自转角速度，g代表在地球e系得重力误差，
为陀螺仪的误差，f为角速度的误差值。
基于式（11），融合系统的完整过程模型可写为：
对于激光雷达部分，激光雷达在滑动窗口中的历史姿态被视为没有过程噪声的常数。然后，可以根据（12）进行协方差传播。每当接收到一个新的激光雷达扫描时，当前的激光雷达姿态将被增强到状态矢量中。其初始姿态估计可以通过当前INS预测的姿态来计算：
其中，符号（^）表示估计值。是IMU和激光雷达之间预先校准的外部参数，在处理[44]过程中被认为是恒定的。相应的增广协方差可以表示为：
在PPP处理中，IF组合被广泛用于消除一阶电离层延迟，它可以表示为：
上式的P和L分别代表了伪距和载波相位观测，为接收机(r)到卫星(s)的几何距离；分别表示中频组合中的波长和模糊度；分别为伪距和载波相位的测量噪声；与频率无关的对流层延迟(T)由干、湿分量组成，两者都可以用各自的天顶延迟(ZHD，ZWD)和对应的映射函数(
)干湿[8]来表示。对于GPS、BDS和Galileo的多星座情况，伪距观测的IF组合可以表示为：
载波相位的值可以表示为：
在构造IF组合之前，我们利用[37]中提出的动态DIA方法来拒绝总误差。根据（18）和（19），多GNSS的PPP测量值的残余向量可以写成：
其中，下标INS和GNSS分别表示INS预测的GNSS测量值和原始GNSS测量值；为相应的测量噪声；，为INS误差状态和PPP误差状态的雅可比矩阵，可参考[40]。
在第二节中，我们获得了滑动窗口中多个激光雷达帧的相关平面特征。虽然该表示可以显著加速平面特征跟踪，但不能直接用于测量更新。其主要原因是平面特征的6-DOF表示比具有3-DOF的真实平面更冗余，这可能会导致额外的计算资源和数值不稳定性。因此，与[32]类似，平面特征的表示通过最近点来代替来重新参数化平面。最近点和平面特征和之间的相对关系可以表示为：
此外，最近点CP可以用其法向量和距离标量d表示为：
以激光雷达帧m中的第i个平面特征和激光雷达帧n中对应的第j个平面特征
为例，测量值
可以表示为：
上式中R为平面特征的法向量由n帧特征的法向量和m帧特征之间的相对旋转；
为平面特征的法向量和距离标量得到的距离标量以及帧与帧之间的相对平移。将当前估计的测量模型线性化，测量函数的残差可以近似为：
其中iH和HLiDAR分别为对应的第个平面特征的雅可比矩阵和涉及的激光雷达状态。它们的具体形式可以参考附录b。通过堆叠同一平面特征的所有关联，我们有：
受[43]的启发，将平面特征误差ψ投影到其左Null空间，使其与激光雷达状态去相关。然后，（25）可以重写为：
为了去除在第II-C节中获得的相关平面特征中的异常值，在计算出最终的rLiDAR和HLiDAR后，执行卡方检验[45 LIC-Fusion: LiDARInertial-Camera Odometry]来检查先验残差：
其中Q和Qw为ronghe 系统和测量噪声的协方差矩阵。如果马氏氏距离小于卡方分布的95%，则可以进行相应的激光雷达测量更新。残差向量本质上是点-面距离，其噪声经验值设置为0.05m。
