【论文笔记】MSTSL: Multi-Sensor Based Two-Step Localization in Geometrically Symmetric Environments

【论文笔记】MSTSL: Multi-Sensor Based Two-Step Localization in Geometrically Symmetric Environments

    ~~~          ~~~~     由于高度相似的几何结构和不足的鲜明特征，对称环境是移动机器人完成全局定位任务最棘手和最具挑战性的场景之一。在这种情况下，现有的定位解决方案要么依赖于昂贵、不灵活且难以维护的预部署基础设施；或者依赖基于单个传感器的方法，其初始化模块无法提供足够的独特信息。因此，本文提出了一种名为 MTSSL 的新型基于多传感器的两步定位框架，该框架通过利用测量的磁场、2D LiDAR 和车轮里程计信息解决了几何对称环境中移动机器人全局定位的问题。所提出的系统主要包括两个步骤：1）基于磁场的初始化，以及 2）基于 LiDAR 的定位。基于预先建立的磁场数据库，可以首先通过所提出的两阶段初始化算法确定多个初始假设位姿。然后，利用获得的多个初始假设，可以通过基于激光雷达的定位更准确地定位机器人。大量实验证明了所提出的系统在现实世界场景中相对于替代方法的实用性和准确性。

系统概述

    ~~~          ~~~~     所提出的基于多传感器的两步定位（MSTSL）系统的工作流程如图 2 所示。本文重点研究二维全局定位问题，其中机器人在时间步长 t 的姿态 xt 可以表示为随机变量 $x_t=[x;y;\psi ]T\in R^2\times S$。

在所提出的系统中，初始机器人姿态 xi 作为隐藏变量被引入以提高机器人定位性能，这可以通过利用 MF 信息 B 和 LiDAR 信息 L 的初始化步骤推导出。给定独立的传感器观测值 $z=[z_i;z_l]T=[z_i^B;z_i^L;z_l^L]T$ 、控制输入 ul 和预先构建的映射 M = [mB; mL]T ，几何对称环境中的移动机器人全局定位问题可以表述为联合概率分布的估计，表示为：
　　
其中 k ∈ [1; K] 表示第 k 个假设姿势，K 表示以高 MF 匹配概率选择的前 K 个机器人假设姿势。 $z_i=[z_i^B;z_i^L]T$ 和 $zl=z_l^L$ 分别表示初始化步骤和基于 LiDAR 的定位步骤的传感器观测值。 mB 和 mL 分别表示离线阶段构建的 MF 数据库和占用网格图 (OGM)。

基于磁场的初始化

    ~~~          ~~~~     首先，我们看初始姿态概率分布 p(xk i jzk i ; mB)，它是方程（1）右侧的第一项。基于 MF 的初始化的目标是推断初始假设姿势 xk i 。通过遵循贝叶斯定理，它可以推导出为：

　　其中 ηi 是归一化常数，项 p(xk i ) 和 p(zk i jxk i ; mB) 分别是先验估计和观察模型。为了指定初始化，可以合并先验信息来评估后验。如果没有可用的先验信息，我们假设 p(xk i ) 是均匀分布 [1]。对于观测模型，详细信息在第 III-A.3 节中讨论。

基于 LiDAR 的定位

    ~~~          ~~~~     对于基于 LiDAR 的机器人姿态推断，我们详细阐述了方程（1）右侧的第二项。基于 LiDAR 的定位的目标是估计准确的机器人位姿 xk l ，可以通过应用贝叶斯定理和马尔可夫性质表示为：

其中 ηl 是归一化常数。 p(xk l jxk i ; uk l ) 和 p(zk l jxk l ; mL) 分别是基于 LiDAR 的定位步骤的运动模型和观测模型。相对于初始假设姿态的姿态是通过建议的局部地图到全局地图匹配来估计的。最终在去除假假设后得到准确的机器人位姿。基于 LiDAR 的定位的详细过程在第 III-B 节中介绍。

基于多传感器的两步定位

    ~~~          ~~~~     建议的 MTSSL 系统由两个模块组成：1）基于 MF 的初始化； 2）基于激光雷达的定位。详情如下

基于磁场的初始化

    ~~~          ~~~~      1) 磁场特性：假设环境 MF 是时间平稳的 [10], [16], [18]，环境 MF 可以建模为 3-D 矢量 B = [Bx;经过; Bz]T 2 R3 在笛卡尔坐标系中，其中 Bx、By 和 Bz 是三个正交分量，表示以微特斯拉 (µT) 报告的强度。测得的 Bx 始终与 IMU/磁力计的航向方向相同 [10]、[16]。地球 MF 为环境 MF 设置了背景，但是由铁磁物体（例如钢、电气和电子设备）引起的异常会使地球 MF 偏转，从而使环境 MF 在当地环境中是独一无二的。
　　 2）磁场建模：由于磁力计的测量范围有限，插入和建立MF数据库mB是必不可少的。实现了不同的 MF 建模方法，例如基于指纹 (FP) 的 [7]、[12]、基于双线性插值 (BI) 的 [16]、[23] 和基于高斯过程回归 (GPR) 的 [19] ], [22]。在我们的框架中，基于 GPR 的 [19] 方法因其突出的准确性和概率推导而被使用。首先，沿机器人勘测轨迹每隔5cm采集MF矢量作为指纹。机器人在每个位置收集各个方向的 MF 指纹是不切实际的。因此，移动机器人在不同的所需路线上进行遥操作，这些路线沿着平行于环境直线边界的轨迹，以记录两个方向（0°和 180°）的 MF 指纹。
　　 让 F = (p1; q1); :::; (pn; qn) 是从测量的 MF 指纹中选择的一组训练样本，其中 pi 和 qi 表示二维位置向量和相应的一维磁场强度 Bx/By/Bz。 GPR 从训练数据 F 估计模型函数 f§ 的后验分布。模型函数可以表示为：f§ ∼ GP(m§; k(p; p0))，其中 m§ ) , E[f§] 是均值函数, k(p; p0) , E[(f§−m§)(f(p0)−m(p0))] 是协方差函数（其中通常选择高斯核），并且 E[·] 是期望算子。我们的目标是在任意查询位置 p* 上预测 f§ 上的后验分布。因此，应用标准 GPR 方程 [30] 来推断均值 μ 和协方差矩阵 K。 GPR 算法的详细讨论超出了本文的范围，因此我们建议读者参考 [30]。最终，我们定义了网格大小为 5cm × 5cm 的 MF 数据库，每个网格的后验分布由三个独立的 GPR 计算。 MF 数据库的每个网格单元包含 3-D MF 矢量、2-D 位置和机器人身体框架。
　　 3）两阶段初始化算法：为了解决移动机器人定位的初始化问题，特别是在几何对称环境下，我们提出了两阶段初始化算法，其中所提算法的坐标变换关系如图3所示. 所提出的算法可以分解为两个阶段：1）位置确定； 2) 方向确定。这两个阶段的细节将在以下部分详细说明。如第 III-A.2 节所述，仅收集两个方向（0°和 180°）的 MF 指纹。因此，机器人需要旋转角度 β 以与环境平行对齐，然后才能实现基于 MF 的匹配。我们利用激光扫描的第一帧来估计航向 Qx 和 Dx 之间的角度 β。首先，由于大多数对称环境包含足够的线配置，我们将激光扫描的第一帧转换为二维点云并实施 RANSAC 算法 [31] 来拟合线。然后我们选择具有最多内点的线作为已知β的目标线。最终，我们命令机器人旋转 β 使其与拟合线对齐。
　　 a) 第 1 阶段：位置确定：调整机器人初始方向后，我们可以将当前测量的 MF 数据 Dt 与预先构建的 MF 数据库进行匹配。对于图 3 所示的 Q 到 D 旋转部分，我们将 α = tan−1jQyj=jQxj 定义为 Qx 和 BNorth 之间的角度，并将 γ = tan−1jDyj=jDxj 定义为 Dx 和 BNorth 之间的角度。鉴于附近位置的 MF 数据可能具有相似的值并考虑到不确定性，根据概率选择前 K 个假设位置。在我们的框架中，我们利用多元高斯概率密度函数 [19], [22] 作为观察模型 p(zk i jxk i ; mB) 在等式。 (2)为基于探地雷达的方法。关于基于 FP 和基于 BI 的方法，我们利用嵌入欧几里德距离度量的 k-NN 算法来选择 K 个最近的位置。
　　 b) Stage 2: Orientation Determination: 由于我们在前一阶段已经推导出了第 i 个初始假设位置和对应的 MF 数据库体坐标系 D，显然 MF 数据库体坐标系 D 和全局坐标系 G 的原点与如图 3 所示。那么对于方位确定部分，第 i 个初始假设体坐标系 Di 相对于 G 的航向可以表示为满足 G = DGRi·Di 的旋转矩阵 DGRi。旋转的角度φ和统一轴a导出为：φi=arccos((Di·G)=(DikGk))； ai = (Di × G)=( Di × G )。由于假设机器人与直线环境配置平行移动，我们可以将机器人方向 θ 确定问题分为以下四种情况：

基于 LiDAR 的定位

    ~~~          ~~~~     鉴于从先前的初始化模块推导出的多个初始假设姿态，所提出的基于 LiDAR 的定位模块的目标是将当前局部地图与全局参考地图进行最佳匹配，并找到准确的坐标变换。首先，激光测距点被投影到二维网格单元中。 2-D SLAM 方法 Hector-SLAM [27] 和 Cartographer [28] 被用来构建 OGM mL，并获得相应的子图 MG；a。为确保成功定位，移动机器人将从 xk i 开始并移动到 xk l 以观察环境，直到成功定位自己。因此，方程中的运动模型 p(xk l jxk i ; uk l )。 (3) 可表示为：

　　其中 uk l = [∆dk l ; Δθlk]T 描述了由车轮编码器测量的从第 k 个初始假设状态 xk i 到最终状态 xk l 的平移和旋转运动。
　　在我们的方法中，移动机器人的传入激光扫描通过迭代过程与局部地图坐标系重复对齐，以构建第 k 个假设姿势 ML;k 的局部地图。我们假设每个局部地图包含 n 个小网格单元，如 ML;k = fhm(1) L;k; :::; m(L;k n) i j n = 1; :::; Ng，其中 m( L;k n) 是 ML;k 中的网格单元。类似地，基于全局坐标系以机器人位姿 xs（构建 OGM 时）为中心的子图 MG;s 被描述为 MG;s = fhm(1) G;s; :::; m( G;s n) i j s = 1; :::; SG。值得注意且明显的是，ML;k 和 MG;s 的大小相同。在由基于 MF 的初始化推导出的每个初始假设位置 (xk i ; yik) 的邻域中，每个局部地图 ML;k 寻求仅在 xk i 的 3σ xk i 置信区间内进行搜索以减少计算负载。此后，我们利用基于 Ceres 的扫描匹配器 [32] 作为等式中的观察模型 p(zk l jxk l ; mL)。 (3)，当ML;k 和MG;s 达到最佳匹配时，可以获得准确的机器人位姿xk l [33]。
　　为了获得更好的定位精度，我们评估每个获得的机器人姿态 xk l 的可信度。假设当前第k个精化位姿的激光扫描中有Jk个激光点，并且每个被激光点击中的被占据的网格单元都被赋值为1，我们定义假设置信度Sk j 为： Sjk = (PJ j =1 k Pjk)=Jk，其中 Pjk 表示命中占据的网格单元的数量。设置经验阈值 Sj = 0:85 以消除错误假设。此外，通过实体结构或超出地图边界的错误假设也被删除，这表明机器人定位在不切实际的位置。为了更清楚地说明这个问题，图 4 显示了所提议的 MTSSL 系统在几何对称办公走廊环境中的一个定位案例。图 4b 和图 4c 显示了错误假设，其中它们不符合要求物理约束或置信度分数。可信的真实假设如图 4d 所示。