LION: Lidar-Inertial Observability-Aware Navigator for Vision-Denied Environments
摘要
在GPS拒绝和感知的环境中导航的机器人的状态估计，例如地下隧道，矿区和行星子表面空隙[1]，在机器人中仍然具有挑战性。
LION通过融合来自IMU的高频惯性数据和通过固定滞后滑动窗口的LIDAR低频相对位姿观测来提供高频的里程估计。同时也不需要事先获取LiDAR和IMU外参，进行了实时在线外参标定，此外，还使用了可观测性的度量来评估位姿估计是否是几何病态的。
介绍
提出了LiDAR-Inertial的可观测性算法，用于在感知性降低的环境中，这是CoSTAR团队在第一届DARPA Subterranean挑战中的方法。
我们的解决方案依赖于IMU预积分和scan-to-scan的ICP，以及固定滞后的滑动窗口。并且该方法实时在线标定LiDAR和IMU的外参，为了解决潜在的可观测性问题，我们使用了几何可观测性分数[23]，使得LION预测其输出中的观测场景的几何结构。通过该输出的得分，基于监督算法(如HeRO)，我们可以切换到不同的状态估计算法（如轮速编码、视觉惯性等）。该方法保证了状态估计的连续性、可靠性以及重力对齐的特点，为后续级联规划和控制算法提供保障。
具体方法
激光惯性里程计
LION是基于滑动窗口的LIO，分为两个部分，前端由LO和IMU预积分、可观测性检测模块组成，后端是因子图优化，如图2所示。
在下面的叙述中，遵循如下约定：
- 里程计世界坐标系\(W\)
- 载体坐标系\(B\)，即IMU坐标系
- 激光雷达坐标系\(L\)
- 一个点聪坐标系A到坐标系B的表示为4x4的矩阵：\(_B\mathbf{T}_A\)，由旋转矩阵\(_B\mathbf{R}_A\)和平移向量\(_Bt_A\)组成
LO模块使用GICP[31]来获取两帧激光扫描之间的相对位姿变换\(_{L_{k-1}} \mathbf{T}_{L_{k}}\)。为了简化ICP算法的收敛性，对于每一帧激光扫描到达，首先进行重力对齐（使用IMU对其进行旋转部分的坐标表变换作为ICP的初始值）。
IMU预积分模块利用state-of-art的流形积分理论来将关键帧之间的IMU整合成单个运动约束[22,32]。
或者，基于scan-to-scan的前端可以使用LOCUS[33]框架替换，它额外地将新的激光扫描与局部地图进行对齐，以执行一个refinement步骤。
在后端，由前端产生的相对位姿观测与IMU测量结合使用，图3展示了因子图中的状态和因子，其中，在第j个时间步的状态\(x_j\)表示如下：
\[ \mathbf{x}_{j}:=\left\{_{W} \mathbf{T}_{B}, _W \mathbf{v},{ }_{B} \mathbf{b}^{a},{ }_{B} \mathbf{b}^{g},{ }_{B} \mathbf{T}_{L}\right\}_{j} \]
其中，
- \(_W{\mathbf{T}}_{B}\)是IMU$到世界坐标系的变换
- \(_Wv\)是线速度
- \(_Bb^a,_Bb^g\)是IMU的加速度计bias和陀螺仪bias
- \(_BT_L\)是激光雷达到IMU的变换
遵循[32]，记\(\mathcal{K}_{k}:=\{k-m+1,\dots,k\}\)为滑动窗口中的m个时间步，并记\(\mathcal{X}_{k}:={\mathbf{x}_j}_{j\in\mathcal{K}_k}\)和\(\mathcal{Z}_k\)分别记为滑动窗口中的状态和观测。
因子图优化旨在求解如下[32]函数：
\[ \mathcal{X}_k^{*}:= \arg \min_{\mathcal{X}_k}(-\log_c p(\mathcal{X}_k | \mathcal{Z}_k)) \]
其中，\(p(\mathcal{X}_k | \mathcal{Z}_k)\)是一个后验概率分布。
我们使用GTSAM[29]作为后端，并使用iSAM2[28]求解。
可观测性度量
在地下场景，确定场景的几何特性是否有利于求解平移方向的位移是至关重要的。遵循[23,34]，假设旋转是小量的，那么Point-to-PlaneICP的cost的Hessian使用\(2A\)来近似，其中，
\[ \begin{aligned} A:=\sum_{i=1}^{M} H_i^TH_i:= \begin{bmatrix} A_{rr} & A_{rt} \\ A_{rt}^{T} & A_{tt} \end{bmatrix} \end{aligned} \]
且有
\[ H_i:=[-(p_i \times n_i)^{\mathbf{T}},-n_i^{\mathbf{T}}] \]
其中，\(n_i\)是与点\(p_i\)对应的平面单位法向量。
因此，矩阵\(A\)的最小特征值即为最小可观测性的方向，平移部分通常是位姿估计具有挑战性的部分，主要是环境中可能存在长廊等场景。
因此，我们使用条件\(\kappa\left(\boldsymbol{A}_{t t}\right):=\frac{\left|\lambda_{\max }\left(\boldsymbol{A}_{t t}\right)\right|}{\left|\lambda_{\min }\left(\boldsymbol{A}_{t t}\right)\right|}\)作为可观测性的度量。
当\(\kappa\left(\boldsymbol{A}_{t t}\right)\)越大，优化问题中在平移部分的约束越差，当高于阈值时，LION向切换逻辑的HeRO[4]模块发出警告，以便可以使用其他更加准确的里程计来源。
实验
2019隧道竞赛
我们首先在美国匹兹堡的Niosh实验矿山举行的两种不同赛道中的两次不同赛道中的两次不同赛道的两次不同。
激光里程计计算频率为10Hz，其中IMU和LION输出可以高达20Hz。LION使用的滑动窗口保持在3s内，并且后端被调整为使用i7 Intel NUC核心的30%。
对于参考，我们将其性能与：轮速惯性里程计（通过扩展卡尔曼滤波器融合的轮速惯性里程计）、scan-to-scan的里程计（LION的相对姿势输入）进行比较，并使用LAMP[35]作为ground-truth。
结果总结在表1中。
我们可以看到，融合惯性数据与前端的里程计中显着降低了纯激光雷达里程计方法的漂移（scan-to-scan）。这从沿图4所示的z轴的z轴尤其明显，另外，LION可靠的估计了机器人的姿态（如图5所示），实现了小的roll和pitch误差
没看出来.
为了展示LION的自动校准能力，我们在模拟中生成了一个数据集，其中LIDAR沿IMU的Y轴相对于IMU转换为0.1米。 在图7中，我们观察到大约20秒后，LION（连续线）估计正确的外参（虚线）
> 精度不咋地。
可观测性模块
我们首先展示\(\kappa\left(\boldsymbol{A}_{t t}\right)\)如何检测几何上的无约束场景。 要测试这一点并建立直觉，我们首先使用JPL-Corlidor数据集，在JPL的办公室录制，其中主要的挑战是该环境沿长廊方向缺少几何特征。
此外，我们也使用了Arch-Coal-Mine数据集进行测试，它由一条直隧道和一个交叉路口组成。
将JPL-Corlidor数据集中的\(\kappa\left(\boldsymbol{A}_{t t}\right)\)以及矩阵特征值进行可视化，如图8所示：
某个方向特征值越小，轴越短，不确定性越小，\(\kappa\left(\boldsymbol{A}_{t t}\right)\)越大，约束越少。
在走廊的开始和末尾，所有方向都有足够的特征，条件号是\(\kappa\left(\boldsymbol{A}_{t t}\right)\)≈2。但是，在走廊的中间，条件号达到值\(\kappa\left(\boldsymbol{A}_{t t}\right)\)> 13，同时特征向量沿着隧道的方向相关的特征值很小
将运行Arch-Coal-Mine数据集时的\(\kappa\left(\boldsymbol{A}_{t t}\right)\)以及矩阵特征值进行可视化，如图9所示：
使用此\(\kappa\left(\boldsymbol{A}_{t t}\right)\)作为可观察性度量标准，HeRO可以决定在没有足够的LIDAR功能时切换到不同于LION（如WIO）的其他里程计源。这种行为如图10所示。对于在办公室的环境中进行的真实实验，其中第一走廊的一部分没有足够的激光雷达特征。
如果未使用可观察性模块，则走廊中的这种特征缺乏会造成lidar slip，即无法观测到机器人的运动，这产生了9m的误差（机器人回到原点）。
当使用可观测性模块后，这种特征被检测到，将会切换到WIO模块来暂时代替LION，减少误差，此时误差为1m。
总结
Local state estimator: LION的主要目标是提供高速、连续、平滑的输出给下游算法。因此，LION并不构建任何地图，也不执行回环检测，因此其参考坐标系会逐渐偏移。这个偏移会由LAMP[35]中进行补偿。
Loosely-coupled architecture： 与其他先进工作相比[16-18]，LION的前后端是松耦合的（即估计的状态x不包含特征点或激光扫描），目的是与建图模块[35]共享计算资源。此外，这种架构通过在选择前端/后端算法中的模块化时决定，并在单个估计引擎的情况下分配几个估计引擎之间的风险以消除单点故障。
Not feature-based： 不同于[18],LION并没有使用特征点进行匹配，原因有两个，以是特征提取耗费计算资源，这将会降低板载资源的运行性能，另外，LION旨在探索完全未知的环境，在那里可以有人类制造的结构（充满了角点，平幔和线条）或完全非结构化的地形。这种不确定环境的特征提取的使用造成了在环境中的先验，因此引起风险或故障。
Automatic extrinsic calibration：激光雷达与IMU的标定非常重要，特别是旋转方面，由于小误差可以快速地积累并导致大漂移。离线标定的方法通常需要校准目标[36]或特定运动序列[37]。
Supervisory Algorithm：LION被设计成由HeRO[4]调用的多种里程计源之一，并且根据可观测性度量进行自主切换。