标题：LO误差建模及横向运动约束辅助的GNSS/IMU/LO融合导航算法
作者：陈含智，孙蕊，程琦，尹彤*，周益，Washington Yotto Ochieng
关键词：组合导航；质量控制；激光雷达里程计；城市环境定位
图片来自作者
GNSS/IMU/LO Integration with a New LO Error Model and Lateral Constraint for Navigation in Urban Areas
Hanzhi Chen, Rui Sun, Qi Cheng, Tong Yin, Yi Zhou and Washington Yotto Ochieng
Satellite Navigation (2024) 5: 30
引用文章:
Chen, H. Z., Sun, R., Cheng, Q. et al. GNSS/IMU/LO Integration with a New LO Error Model and Lateral Constraint for Navigation in Urban Areas. Satell Navig 5, 30 (2024). https://doi.org/10.1186/s43020-024-00151-8
PDF文件下载链接：
https://satellite-navigation.springeropen.com/articles/10.1186/s43020-024-00151-8
1. 文章利用车辆三维速度和激光雷达（Light Detection and Ranging，LiDAR）点云的边缘、平面特征数量组成特征向量，标定帧间激光雷达里程计（LiDAR Odometry，LO）误差增量（以避免历史累积LO误差的影响）并将标定结果作为标签，构建了城市环境下基于平方指数高斯过程回归（Squared Exponential Gaussian Progress Regression，SE-GPR）的LO误差模型。
2. 本文基于LO误差模型预测，并结合用于包络GNSS定位误差的保护级（Protection Level，PL），实时计算LO与GNSS定位结果的权重系数，提出了一种基于扩展卡尔曼滤波（Extended Kalman Filter，EKF）的GNSS/IMU/LO加权融合导航策略。
3. 本文提出了LiDAR辅助的横向运动约束（LiDAR-Aided Lateral Constraint，LALC）模型，利用LiDAR扫描获得的直线路段反射点曲线的几何特性（一束激光的反射点在路面和路缘交界处对应曲率发生突变），提取车辆到道路两边路缘的横向距离信息，构建了车辆的横向运动约束方程，对多源融合导航EKF状态估计向量进行误差累积抑制。
内容简介
快速发展的智能交通对导航定位性能提出了更严苛的需求。但在城市环境下，GNSS受信号遮挡或多路径干扰影响，提供的导航定位精度和可靠性显著下降。LiDAR作为一种环境感知传感器，可通过点云匹配等方式实现自主定位即LO，与GNSS形成很好的优势互补，因而GNSS/IMU/LO等组成的多源融合导航定位成为研究热点。然而，当前LO误差建模研究尚不充分，造成不同传感器定位信息权重的精细度不足，进一步降低多源融合导航系统的性能。因此，本文提出了一种基于数据驱动LO误差建模的GNSS/IMU/LO多源信息加权融合导航定位算法。考虑车辆速度和点云特征点数量，建立了基于SE-GPR的LO误差在线预测模型；在LO误差建模和GNSS定位保护级计算的基础上，确定LO和GNSS定位信息权重系数，提出了一种基于EKF的GNSS/IMU/LO多源信息加权融合策略；从点云数据中提取路面与路缘交界处的特殊点，在直线路段进一步确定车辆在相邻点云帧间的横向位移，以控制GNSS挑战场景下导航状态估计的误差累积。
为验证提出算法，本文在不同城市区域分别采集并构建了训练数据集与测试数据集。对训练数据集，SE-GPR模型预测的均方根误差（Root Mean Square Error，RMSE）和R方指标均优于其他机器学习模型。对测试数据集，本文建立的LO误差模型成功预测了大部分历元的参考LO误差。为评估提出的基于LO误差建模的加权融合策略，本文计算了融合过程中GNSS和LO定位信息的权重系数。所提出的权重策略能够正确地适应GNSS和LO各自定位精度的变化。本文算法与多种对照算法的定位结果对比分析也证明了LALC对水平定位精度提升的贡献。相比基于EKF的GNSS/IMU/LO松组合算法，提出算法分别实现了35.9%和50.0%的水平和3维定位精度提升。
图文展示
Ⅰ 算法介绍
该多源融合导航算法采用松组合框架，分为离线阶段和在线阶段。在离线阶段，本文选择车辆速度和点云特征数量组成特征向量，利用参考轨迹标定LO相邻帧内的误差增量，构建数据集对SE-GPR模型进行训练，挖掘LO误差预测规则。在线阶段中，本文通过伪距单点定位和最小二乘残差法获得GNSS定位解和对应保护级（Protection Level，PL），并将PL作为GNSS定位解的误差估计；利用轻量和地面优化的LiDAR里程计和建图（Lightweight and Ground-Optimized LiDAR Odometry and Mapping，LeGO-LOAM）算法的点云特征提取与匹配模块获取LO定位估计，利用离线阶段的LO误差预测规则对实时LO定位误差进行在线估计；根据对GNSS和LO的定位误差估计计算GNSS和LO定位权重矩阵，在EKF中使用GNSS/LO加权融合结果对IMU力学编排状态估计进行测量更新；为了进一步约束多源融合导航状态误差的累积，提出LALC运动约束，利用激光束反射点在路面和路缘交界处发生的曲率突变，提取车辆到路缘的横向距离，进一步获得车辆在相邻帧间的横向位移并结合非完整性约束（Non-Holonomic Constraint，NHC）对基于EKF的导航状态估计进行误差约束。
图1 本文提出的GNSS/IMU/LO多源信息加权融合框架
图2 LiDAR的一束负仰角的激光扫描一圈获得的反射点在LiDAR坐标系（原点为LiDAR几何中心，X、Y和Z轴分别指向右、前和上方向）XY平面下的投影。红色和绿色的小方块分别代表路面和路缘上的反射点。蓝色的小方块为路面和路缘交界处的反射点。常规城市道路直线路段路面和路缘石的几何结构决定了由上述反射点组成曲线的曲率在二者交界处发生突变
Ⅱ 实地实验
为验证提出算法的性能提升，本文基于图3的车载实验平台在南京两处深度城市环境区域（图4和图5）分别采集并构建训练数据集和测试数据集。
图3 实地实验采用的实验车和所用的导航传感器（包括采集原始GNSS/IMU/LO多源传感器数据的北斗星通接收机、STIM300的IMU、Velodeny的VLP-16激光雷达，以及用于获取参考轨迹的GNSS/INS组合导航设备Honeywell的N580）
图4 测试数据集对应的实验车行驶轨迹。大部分轨迹经过城市峡谷或者高架下方窄路场景
图5 训练数据集对应的实验车行驶轨迹。大部分轨迹经过城市峡谷，较多路段经过两侧茂密行道树的窄路场景
Ⅲ LO误差模型测试结果
本文利用测试数据集评估了通过离线训练建立的LO误差模型。如图6所示，预测的LO定位误差增量基本符合其标定参考值。少量历元内LO误差增量发生跳跃未被成功预测。LO误差增量跳跃主要是车辆周围动态的其他车辆或行人造成的，而所建立的LO误差模型未考虑环境中动态干扰的影响，因而此时预测效果不佳。未来研究将通过识别和考虑动态特征以优化模型。
图6 离线阶段建立的LO误差模型对测试数据集的预测结果（绿色线：基于高精度参考轨迹辅助标定的LO参考误差增量；红色线：提出误差模型预测的LO误差增量）
Ⅳ 加权融合策略测试结果
本文对比了测试数据集的单GNSS和单LO定位结果（图7）。需要指出的是单LO定位使用了本文算法的LO误差校正策略（LO累积定位误差预测值超过阈值时用当前多源融合导航状态更新LO位姿推算的初始值）。单GNSS定位在较多历元存在粗差，甚至在少量历元突变至近百米，而单LO定位误差没有突变，但存在逐渐累积至超过15米。图8对比了基于本文权重策略的LO和GNSS定位权重系数。当GNSS定位精度更高时，GNSS定位的权重系数大于LO定位的权重系数，如620s至660s；而当LO定位精度更高时，其权重系数也较大，如410s至440s。
图7 单GNSS和单LO定位结果对比（绿色线：单GNSS定位误差，红色线：单LO定位误差）
图8 GNSS和LO定位权重系数对比（绿色线：GNSS定位权重系数；红色线：LO定位权重系数）
Ⅴ 定位结果对比
为验证所提算法对城市环境多源融合导航定位性能的提升，以及评估提出的基于LO误差建模的加权融合策略和LALC对定位性能提升的贡献，本文对比了本文算法和多种对照算法对测试数据集的定位结果。相比基础的基于EKF的GNSS/IMU/LO松组合算法，使用加权融合策略后，水平定位精度提升18.5%，高程定位精度提升9.6%；在此基础上应用LALC后，水平定位精度提升29.9%，高程定位精度提升较少（13.9%），这是由于LALC主要是对水平方向的定位误差进行约束。因此本文算法增加了NHC约束的使用，使得提出算法最终分别实现了35.9%和57.5%的水平和高程定位精度提升。
图9 导航坐标系下提出算法与对照算法定位结果对比（绿色线：基础的基于EKF的GNSS/IMU/LO松组合算法；紫红色线：基于所提加权融合策略的GNSS/IMU/LO松组合算法；淡蓝色线：LALC辅助的基于所提加权融合策略的GNSS/IMU/LO松组合算法；深蓝色线：NHC辅助的基于所提加权融合策略的GNSS/IMU/LO松组合算法；红色线：LALC结合NHC辅助的基于所提加权融合策略的GNSS/IMU/LO松组合算法（提出算法））
图10 实际地图上提出算法与对照算法定位结果对比（黄色线：参考定位轨迹，其他颜色的线对应的算法同图9）
▍通讯作者
尹彤，自然资源部测绘标准化研究所测绘工程师，主要从事城市复杂环境下的导航定位技术和应用以及测绘地理信息标准化研究。
▍第一作者
陈含智，南京航空航天大学博士生。研究方向为弹性多源信息融合导航定位及其完好性。
