突破非视距条件依赖！大规模区域、低成本的UWB-LiDAR标定与单次定位框架

UWB大规模实时工业定位

超宽带（UWB）是一种短距离、节能的无线电通信技术， 主要用于精确的定位检测和相对距离测量 。例如，Apple 的 AirTags 和 Android 的 SmartTags 等设备因其能够精确追踪家用物品而越来越受欢迎。

虽然 UWB 在运动跟踪（如足球比赛）等应用中表现出色，但在大规模实时工业定位应用中仍然面临重大挑战。目前，大多数 UWB 研究仅限于较小或室内环境，并以绝对定位（AP）模式运行，如图 1 所示。在这种条件下，所有基站必须经过标定，并且标签需要持续从多个基站进行距离测量。然而，在遮挡环境中，这种方式变得不切实际。 这样的限制降低了 UWB 在如港口或仓库等大规模区域中的实用性 ，因为这些场景中障碍物显著增加了技术难度。

在大规模室外环境中使用 UWB 的 挑战 主要包括：非视距（NLoS）问题的频繁出现和其他设备的干扰，这些问题会限制有效的测量范围。在最糟糕的情况下，由于新加入的集装箱，基站之间可能完全失去连接（如图 1 所示）。可靠 GPS 信号的缺乏进一步加剧了标定的复杂性。即便完成标定，在非视距条件下，系统在定位方面仍然困难，成为现场机器人和物流自动化的一个瓶颈。

针对这些问题， 本文介绍一种综合方法【1】，以解决大规模 UWB 标定和单次定位的挑战 。利用连续时间 LiDAR 惯性里程计（CT-LIO）生成可在任意点采样的轨迹。通过对该轨迹进行插值，我们将每个 UWB 测量值与其对应的位置对齐。然后，使用高斯过程迭代优化基站位置估计。尽管完成标定后，标准 UWB 技术在非视距模式下依然无法定位，为解决此问题，结合了一种基于快速描述符的方法，增强了在大规模重复环境中的单次全局定位能力。我们的方法显著改进了现有的定位方法，并引入了此前未曾探索的新可能性。

主要贡献 ：

1. 提出了一种结合 UWB 数据、CT-LIO 和高斯过程的方法，仅需一次采样便能在大规模环境中精确标定 UWB 基站。这种方法在遮挡和重复环境中显著提高了定位性能，在 UWB 或 LiDAR 单独使用均失效的场景中尤为有效。

2. 提出了单次定位流程，通过利用已有描述符对 UWB 范围进行过滤，减少了搜索时间，从而实现了复杂区域中的高效准确定位，不再需要多个 UWB 基站，使得该方案经济实惠且实用。

3. 方法通过真实世界实验验证，在600x450 平方米的环境中实现了更高的精度和更短的处理时间，证明了其在具有挑战性条件下的大规模应用的实用性。

4. 将公开数据集和标定代码，以供社区使用。

问题描述

最终目标是 在大规模重复环境中准确估计机器人的位置 。为此，我们首先从 LiDAR SLAM 框架中获取 UWB 测量值 和 3D 姿态 ，并通过 B 样条插值进行处理。随后，这些插值后的姿态和样本被集成到高斯过程模型中，用于标定 UWB 基站的位置，从而提升 LiDAR 在重复物流环境中的定位能力。

连续时间 LiDAR-惯性里程计

对于连续时间 LiDAR-惯性里程计（CT-LIO），我们利用了之前的研究成果 SLICT，该方法能够通过离散的 LiDAR 和 IMU 输入生成连续的轨迹表示。为了确保问题规模受控，SLICT 系统仅在滑动窗口内优化一组控制点，优化区间为 。SLICT 的状态可以表示为：

其中， 是局部轨迹的控制点， 是 B 样条的阶数，区间 。 表示 IMU 的偏置。

为了优化状态 ，目标函数 被设计为最小化来自 LiDAR 和 IMU 测量的残差：

其中：

• 和 分别表示 LiDAR 和 IMU 的残差；
• 和 是权重矩阵，用于调整残差的影响。

目标函数通过一个基于流形的优化求解器进行最小化，该求解器仅依赖于 Eigen 库，从而实现最高的计算效率。这种优化方法保证了在指定时间窗口内生成连续时间轨迹，同时处理传感器输入中的噪声和偏差问题。

插值的 UWB 姿态

LiDAR 的运行频率为 10Hz，而 UWB 的运行频率为 20Hz，这导致两者在数据采集时间上存在时间偏差且频率不一致。为了在任意时间点 获取 UWB 样本姿态 ，我们通过 B 样条插值法来处理。这一过程利用控制点 进行插值。值得注意的是，在标定过程中旋转矩阵 被省略。

插值过程：

1. 确定时间节： 首先定位时间 所处的样条节点区间 。

2. 归一化时间计算： 计算归一化时间 ，其公式为：

3. 混合系数计算： 使用 B 样条基矩阵 和 的幂计算混合系数 ：

   这些混合系数决定了每个控制点对插值结果的贡献。

4. 插值计算： 利用控制点的位置信息 ，插值计算样本姿态的位置信息 ：

结果： 这一插值过程生成了任意时间点 的姿态 ，从而对机器人估计的轨迹进行了时间对齐，使得 UWB 测量值能够与具体的时间戳和位置关联。这种方法有效缓解了不同传感器数据采集频率和时间不一致的问题，为后续的高斯过程建模和定位优化奠定了基础。

基于高斯过程的 UWB 基站标定

高斯过程 (Gaussian Process, GP) 提供了一种非参数框架，用于在环境 中确定 UWB 基站的分布。该方法将未知分布建模为一个连续函数 ，表示基站在位置 的测距。通过使用从 SLICT 生成的样本数据 ，我们定义了高斯过程：

该模型描述了 SLAM 推导的姿态与 UWB 测距值之间的关系，其核心包含：

• 均值函数 ： 在缺乏基站位置先验信息时，通常设为零；
• 协方差函数 ： 描述不同位置之间的空间关系。

协方差函数

为了准确捕捉空间关系，我们采用了 Matérn 3/2 核函数，其能够适应空间数据中不同的平滑程度：

其中：

• 是平滑度参数；
• 是长度尺度参数；
• 是修正贝塞尔函数；
• 是伽马函数。

这些参数根据环境复杂性和数据的变化性进行调整，以提高模型的适应性。

预测方程

高斯过程通过以下公式计算预测均值 和协方差 ：

其中：

• ：待推断的位置；
• ：已知采样点集；
• ：对应的 UWB 测量值；
• ：测量噪声。

标定过程

标定基站位置的具体过程如下：

1. 初始拟合： 首先在较粗的尺度上拟合初始点。
2. 区域采样： 在矩形区域内按规则间隔采样未定义的位置。
3. 模型更新： 选取采样值中前 10 个最佳值的均值，更新高斯过程模型。
4. 分层随机采样： 确保数据覆盖整个区域，从而提升泛化能力，并加速收敛。

优势

通过高斯过程标定 UWB 基站位置，可以实现更高的精度和鲁棒性。相比传统方法，该方法能够更好地应对复杂和非视距 (NLoS) 环境中的基站标定问题，从而显著提升整体定位性能。

单次定位

我们对 Stable Triangle Descriptor (STD) 稳定三角形描述符进行了改进，以优先匹配靠近已知 UWB 基站的场景。这种方法通过整合标定后的 UWB 基站位置与 STD 描述符，旨在降低大规模重复环境中的误匹配率。

改进的 STD 方法

STD 仅依赖三角形描述符来识别场景，但在重复场景中容易出错。通过将 STD 与标定后的 UWB 数据结合，我们引入了基于范围的分区搜索来增强定位性能。

1. 分区 UWB 测量：

• UWB 测量值被划分为离散范围区间 ，每个区间 对应一个以基站 为中心、半径为 的圆环。
• 区间的边界为：

  其中 是调整搜索范围的偏差。

• 根据标定的基站位置 ，提取各基站周围的先验描述符 。
• 在运行过程中，当前 STD 描述符与基于测量 UWB 距离 的先验描述符进行匹配。

• 参数 用于扩展搜索区域，从而更好地表示 STD 描述符。这种方法提高了在重复场景中的鲁棒性，减少了误匹配的可能性。

单次定位流程

通过改进的 STD 方法，我们实现了以下单次定位流程：

1. 预处理： 根据标定的 UWB 基站位置，划分搜索区域并提取先验描述符。
2. 实时匹配： 使用当前 STD 描述符与预定义区域内的描述符进行匹配。
3. 位置推断： 基于匹配结果，快速定位机器人或目标物体的当前位置。

参数选择

• 偏差 和分区半径 的数量根据实验经验设置，以在准确性和计算效率之间取得平衡。

优势

与传统 STD 方法相比，结合 UWB 基站位置的改进方案具有以下优势：

• 提高准确性： 标定后的基站位置能够显著减少重复场景中的误匹配。
• 增强鲁棒性： 即便在非视距 (NLoS) 和复杂环境下，也能保持较高的定位成功率。
• 优化效率： 基于范围的分区搜索缩小了搜索空间，从而减少了计算时间。

通过这一方法，我们能够在大规模重复环境中实现高效且准确的单次定位，为工业物流和机器人应用提供了经济实用的解决方案。

实验效果

总结一下

该文章提出了一种应对非视距 (NLoS) 问题的 UWB 标定方法，利用 SLICT 生成连续时间轨迹。通过采样 UWB 采集的姿态并应用迭代高斯过程，我们成功地在600x450平方米的大规模环境中实现了大约2米的标定精度。即便在传统 UWB 定位失效的非视距条件下，标定后的基站位置也能作为独立插件，提高重复性和大规模环境中的单次定位成功率。实验表明， 配备稀疏 UWB 和车载 LiDAR 的集装箱运输自动移动机器人可以在具有挑战性的重复环境中实现精确的单次定位，同时成本最低 。

参考

【1】Large-Scale UWB Anchor Calibration and One-Shot Localization Using Gaussian Process