UWB/LiDAR-SLAM 紧耦合定位系统

超宽带(UWB)定位技术是近年来得到广泛应用的无线定位技术，由于其系统复杂度低、时间分辨率高、抗多径效应强、信号穿透性强等优点，被应用于室内定位技术研究。UWB定位精度由UWB的脉冲宽度和响应频率决定。UWB脉冲宽度仅为纳秒或亚纳秒级，具有超高带宽特性，从而保证发射出超低的信号功率谱密度，理论上可以达到厘米级。但在复杂的室内环境中，障碍物(行人、车辆、墙柱等)会引起UWB脉冲信号在传播过程中发生反射、折射和穿透，导致信号有一定的衰减、时间延迟和较大的传播距离，这些因素又会造成基于定位锚点与移动标签之间信号强度测量或时间测量的距离估计有偏差，这种偏差称为非视距(NLOS)误差。

NLOS误差的干扰会显著降低UWB系统的定位精度和稳定性，严重时甚至导致无法测距定位，这与UWB应用于室内高精度定位的期望严重不符。因此，如何在NLOS环境下有效识别和抑制NLOS误差，以提高定位算法的精度和稳定性，成为室内高精度定位的关键。

对于存在NLOS误差的室内定位场景，传统的NLOS识别技术大致分为三类，即基于距离估计的方法，基于信道统计的方法和基于位置估计的方法。

基于距离估计的方法通过将距离估计的方差与预定义阈值进行比较或使用概率密度函数（ PDF ）来区分视距（LOS）和NLOS误差。然而，这种方法不可避免地增加了由于距离采集而导致的时间延迟，也受到先验分布函数的要求和选择合适阈值的困难的限制。

基于信道统计的方法通常使用假设检验或分类理论，利用从信道脉冲响应（CIR）获得的幅度（如接收信号强度（RSS）、接收信号的最 大幅度、功率差和功率比）和时间（如到达时间（TOA）、均方根延迟扩展、峰值至前置延迟、上升时间、平均额外延迟和最 大额外延迟）统计数据来区分 LOS 和 NLOS 误差，并提取信道特征参数，如偏度、峰度和核函数以进行识别。然而，这些方法需要对多个统计数据的分布进行建模，因此需要创建和频繁更新大型训练数据库，而 NLOS 数据的收集通常非常繁琐和密集。

基于位置估计的方法可在定位过程中识别 NLOS 误差。在有效测量冗余的情况下，可以通过比较不同距离值子集的位置估计结果来识别 NLOS 误差。然而，当有多个测量值存在NLOS误差或冗余测量值不足（二维定位至少需要3个测量值）时，该方法无法可靠地识别NLOS测量值，因此不适用于室内锚点放置稀疏的复杂定位场景。

如果事先知道用户的位置信息，也可以结合环境数据（衰减因子和几何形状）来识别NLOS误差。Jo等人利用二维地图进行信号射线追踪，从而准确掌握无线信号传播过程，识别NLOS信号并估计NLOS引起的误差。然而，由于计算复杂度高，且需要提前了解环境中物体的介电常数、边界条件等物理属性，通常无法实现实时射线追踪。

Suski等和Zhu等通过建立室内静态环境的误差指纹数据库，提高了UWB定位精度。王等采用手工绘制的二维地图匹配线段信息进行NLOS识别，并根据遮挡情况调整UWB测距值的权重，有效消除了NLOS误差的影响。但生成环境地图所需的初步测量对于大型建筑物来说可能是一项艰巨的任务，并且如果空间发生任何变化，包括人或物体的移动，相应的定位误差地图或数字地图就不再有效。因此，这些方法由于缺乏适用于所有环境的附加信息而不实用，并且动态环境阻碍了它们的实时实施。

n  UWB和 LiDAR-SLAM融合

利用LiDAR点云的NI方法可以高效准确地识别UWB NLOS误差，从而提高实际场景中UWB测距定位的性能。结合NI和REKF的TC集成方法比其他对比方法获得了更好的定位效果和鲁棒性

通过 LiDAR-SLAM 生成的 LiDAR 点云地图与来自 UWB 锚点的位置信息相结合，通过实时障碍物检测和 NLOS 识别 (NI) 区分视距 (LOS) 和 NLOS 测量。此外，为缓解长期SLAM中定位误差的积累，利用UWB LOS测量和LiDAR-SLAM定位信息，构建了改进的UWB/LiDAR-SLAM TC定位模型。采用鲁棒扩展卡尔曼滤波器（REKF）进行参数求解，抑制UWB粗误差的影响，提高了集成系统的鲁棒性和定位性能。

由于三维（3-D）传感器技术的快速发展，大型建筑物的环境地图可以通过传感器快速准确地获取，例如光检测和测距（LiDAR）、红绿蓝深度（RGB-D）相机和立体相机[。与相机相比，基于LiDAR的同步定位和地图构建（SLAM）可以直接、准确地提供环境中结构信息的密集三维点，并且受光线和天气的影响较小，并且由于其检测范围远、精度高和鲁棒性强而被广泛应用于三维建筑物的室内地图构建 。

通常，LiDAR-SLAM通过配准连续帧的点云来估计帧间姿态变换矩阵，然后获得轨迹并构建环境地图。因此，环境地图构建质量的关键在于LiDAR六自由度（6-DOF）姿态估计的准确性。

传统的LiDAR-SLAM多采用基于迭代最近点（ICP）和正态分布变换（NDT）的算法或其变体进行帧间姿态估计。ICP通过迭代匹配相邻帧的最近点并最小化对应特征点之间的距离来估计两帧点云之间的姿态变换矩阵。但该类算法对初值敏感，初始化不正确会导致收敛到真实解的吸引盆之外的局部极小值，并且收敛速度慢。作为ICP的替代，NDT算法不根据对应点的特征进行匹配。该算法将底层扫描表示为一组高斯分布，将表面扫描局部建模为PDF，并通过优化方法获得最 大化概率密度和的最优变换参数。与ICP算法相比，该算法的运算速度和鲁棒性有较大提高。然而，NDT 将原始点云离散化为单元，而单元大小的定义会显著影响最终的地图质量 。

LiDAR 里程计和地图绘制 (LOAM) 是当前最 先进的 LiDAR-SLAM 解决方案，在 KITTI 视觉基准排名中，其旋转和平移误差得分较高 ,LOAM是一种基于线面特征的全局地图跟踪方法，通过提取线和面特征将连续帧与全局地图的一致性关联起来，通过执行高频里程计线程和低频建图线程，可以解决复杂的SLAM问题，这些并行线程大大提高了SLAM技术的实时性。LOAM能够在室内外大范围环境中以可接受的精度进行高效建图，但由于缺乏闭环检测，误差积累不可避免。轻量级地面优化的LOAM（LeGO-LOAM)采用地面分割和点云聚类，使提取的特征点更加有效，同时在LOAM中添加优化的后端，并使用贝叶斯树（iSAM2）闭环模块进行增量平滑和建图，抑制长期漂移，提高了LOAM的计算效率和精度。然而，上述两种方法在判别性结构较差的场景中容易受到运动退化的影响，性能较差。

考虑到实际场景中单个传感器的脆弱性，以及UWB与LiDAR传感器的互补性（UWB在开阔场景中表现更佳，而LiDAR-SLAM更适合于UWB表现不佳的复杂场景），将UWB与LiDAR -SLAM结合起来，应用信息融合的方法，充分利用和结合两种定位系统的优势，实现高精度、高普适性、强鲁棒性、连续工作的室内定位系统。一方面，LiDAR-SLAM可以提高UWB定位系统在复杂场景下的可靠性和可用性。另一方面，作为一种递归导航方案，LiDAR-SLAM导航误差随着移动距离的增加逐渐发散。LiDAR-SLAM与UWB的融合有助于抑制SLAM的误差积累。

在大型建筑或时变环境等复杂环境下，UWB信号受NLOS误差影响较大，丰富的几何特征使LiDAR-SLAM能够提供准确、鲁棒的位姿估计与建图结果。在先进的LiDAR-SLAM算法LeGO-LOAM的基础上，提出并实现了基于LiDAR点云算法的UWB NLOS识别。该方法将UWB锚点的位置信息与LeGO-LOAM生成的环境地图相结合，通过高效、精准地对锚点视线方向进行障碍物检测与NLOS识别，实时区分LOS与NLOS测量，提高UWB数据质量。该方法不需要前期繁琐的数据收集工作与训练阶段，具有良好的普适性，能很好地应对环境中动态障碍物的干扰。实验结果表明，该NLOS识别算法在不增加大量计算量的情况下，是合理有效的。

为了抑制LiDAR-SLAM的误差积累，同时获得世界坐标系下的定位结果，提出一种改进的UWB/LiDAR-SLAM紧耦合定位系统，以LiDAR点云识别的UWB LOS测量值和LeGO-LOAM的定位结果作为集成系统的输入。考虑到UWB测量值除受NLOS传播影响外，还受信号多径效应、强度衰减等因素影响，容易产生较大的粗略误差，采用鲁棒扩展卡尔曼滤波器（REKF）进行参数解算，通过降低异常值权重，有效抑制异常测量值对滤波结果的影响。动态定位实验证明了所提NLOS识别与REKF相结合的紧耦合集成方法的准确性和鲁棒性。