---

相关文章：退化环境定位（一）

---

1.Quatro：单个对应关系就足够了：稳健的全局配准以避免城市环境中的退化

原文：github
原文：原文
摘要——利用三维点云进行全局配准是移动平台实现定位或管理闭环情况的关键技术。近年来，研究人员提出了多种全局配准方法，以解决大量异常对应关系的问题。然而，退化问题，即估计的内点数量少于三个的现象，仍然难以避免。为了解决这一问题，提出了一种基于解耦的抗退化全局配准方法，称为Quatro。具体而言，我们的方法通过利用城市环境中的亚特兰大世界假设，采用准SO(3)估计来避免旋转估计中的退化。因此，我们方法的最小自由度从三个减少到一个。通过室内和室外三维激光雷达数据集验证，与其它全局配准方法相比，我们提出的方法在远距离点云对的情况下也能实现稳健的全局配准性能。此外，实验结果进一步证实了我们的方法作为粗略对齐的适用性。我们的代码可在以下地址获取：https://github.com/url-kaist/quatro

图1.（T-B，L-R）：在KITTI数据集中，应用Quatro方法前后，两个相距较远且部分重叠的点云（源点云为青色，目标点云为黄色）的对比。随着源点云和目标点云之间距离的增加，假设对应关系中的异常值比例上升，内点数量减少，这通常会导致基于对应关系的全局配准方法性能下降。在这些情况下，Quatro方法表现出稳健的性能，克服了异常值的影响和退化问题。红线和绿线分别表示异常值和内点对应关系（彩色查看效果最佳）。

为了解决退化问题，本文提出了一种新颖的全局配准方法，称为 Quatro，该名称结合了 Quasi- 和 cuatro（意为 4 自由度，4-DoF）。一些针对退化情况的 SLAM 框架已经在 [49]–[51] 中提出。然而，据我们所知，这是首次在全局配准中通过减少 $SE(3)$ 所需的最少对应点数（从三个降至一个）来解决退化问题，该方法被命名为 quasi-$SE(3)$，其通过解耦 $SE(3)$ 估计为 quasi-$SO(3)$ 估计，然后结合基于分量的平移估计（COTE）[27]。

总的来说，本文的贡献包括以下三点：

• 为了避免退化，提出了一种新颖的旋转估计方法 quasi-$SO(3)$ 估计，该方法利用城市环境的特性。
• 结果表明，所提出的方法在真实世界的室内/室外 3D LiDAR 数据集上相较于最先进的方法表现更优。
• 特别值得注意的是，Quatro-c2f，一种简单的粗到细（coarse-to-fine）方案，优于现有最先进的方法，包括基于深度学习的方法。因此，最终确认了所提出方法在粗配准（coarse alignment）方面的适用性。

A. 解耦旋转和平移估计

根据文献[59]和[60]，通过解耦旋转和平移估计，可以轻松地以闭式形式获得$\hat{\mathbf{R}}$和$\hat{\mathbf{t}}$。为此，首先减去两对点$(i,j)$和$(i^{\prime },j^{\prime })$，以消除$\mathbf{t}$的影响，使用公式(1)。为了简化，令${\alpha }_k={\mathbf{p}}_{i^{\prime }}-{\mathbf{p}}_i$；${\beta }_k={\mathbf{q}}_{j^{\prime }}-{\mathbf{q}}_j$；${ϵ}_k={ϵ}_{i^{\prime }{j}^{\prime }}-{ϵ}_{ij}$，其中$K$是这些平移不变测量（TIMs）[27]的总数。注意，通过减去连续的对来构建TIMs，可以最小化计算成本，第一个TIM是通过减去第一个和第$|\mathcal{A}|$个对来构建的。

然后，${\alpha }_k$和${\beta }_k$之间的关系可以表示为${\beta }_k=\mathbf{R}{\alpha }_k+{ϵ}_k$。因此，$\hat{\mathbf{R}}$的估计如下：

$$\hat{\mathbf{R}}=\underset{\mathbf{R}\in SO(3)}{\mathrm{argmin}}\sum _{k=1}^K\min \left(w_{k}r({\beta }_k-\mathbf{R}{\alpha }_k,{ϵ}^2),c^2\right)(3)$$

其中，$w_k$表示每对的权重，$c$表示截断参数，以抑制潜在的异常值。接下来，使用COTE[27]以逐元素的方式计算$\hat{\mathbf{t}}$，如下所示：

$${\hat{t}}_i=\underset{t\in \mathbb{R}}{\mathrm{argmin}}\sum _{(i,j)\in \mathcal{A}}\min \left(r\left(\frac{t-{\mathbf{q}}_{ij}}{{\sigma }_{ij}^2},{ϵ}^2\right),c^2\right)(4)$$

其中，${\mathbf{q}}_{ij}={\mathbf{q}}_j-\mathbf{R}{\mathbf{p}}_i$，${\sigma }_{ij}$是噪声的界限，$t_{ij}^l$表示3D向量的第$l$个元素，其中$l=1,2,3$，即 t ^ = { t ^ i 1 , t ^ i 2 , t ^ i 3 } \hat{\mathbf{t}} = \{ \hat{t}_i^1, \hat{t}_i^2, \hat{t}_i^3 \} t^={ t^i1​,t^i2​,t^i3​}。因此，$\hat{\mathbf{R}}$在公式(3)中解耦，然后在公式(4)中使用$\hat{\mathbf{t}}$。

位于共面区域，随着两个视点之间的距离增加 [63]-[65]。然而，大多数当前的制图/导航系统采用惯性导航系统（INS）。因此，通过从重力向量估计水平面，滚转角和俯仰角可以完全观测到 [65], [66]。因此，滚转和俯仰可以表示为世界坐标系中的绝对状态，这意味着这些状态是无漂移的 [67]。因此，这仍然允许我们的目标有效，同时将相对旋转限制为相对偏航旋转（参见第IV-D节）。

C. 城市环境中的准SO(3)

接下来，我们引入准SO(3)的概念，以指出相对旋转可以近似为城市环境中的纯偏航旋转，这由Kim等人[6]和Scaramuzza[61]提出。这是因为，即使城市环境不是平的，源点云和目标点云的两个位姿也可以基于亚特兰大世界假设[56]局部近似为共面。因此，偏航旋转被认为是比俯仰和滚转旋转更占主导地位。

更具体地说，我们定义相对旋转矩阵为$\mathbf{R}={\mathbf{R}}_z{\mathbf{R}}_y{\mathbf{R}}_x$，其中${\mathbf{R}}_z,{\mathbf{R}}_y,{\mathbf{R}}_x$分别表示绕z轴、y轴和x轴的旋转。这些旋转元素表示偏航、俯仰和滚转。正如前面提到的，观察到俯仰和滚转的变化远小于偏航，即$\angle {\mathbf{R}}_y\gg \angle ({\mathbf{R}}_y\cdot {\mathbf{R}}_z)$。此外，如图2(a)所示，$\angle ({\mathbf{R}}_y\cdot {\mathbf{R}}_z)$通常小于10°。因此，可以应用小角度假设[62]，使得${\mathbf{R}}_y\cdot {\mathbf{R}}_z\approx {\mathbf{I}}_3$，其中${\mathbf{I}}_3$表示$n\times n$的单位矩阵。这个假设使得$\mathbf{R}\approx {\mathbf{R}}_z$，从而简化了我们的目标，直接估计${\mathbf{R}}_z$。最后，这个假设将旋转的自由度从三个减少到一个，使我们的方法对退化具有鲁棒性。为了简化，近似的$\mathbf{R}$表示为${\mathbf{R}}_z$。有人可能会认为，这个假设在非平坦区域或两个视点不再共面时不再成立。然而，我们的方法仍然有效，因为即使在这些情况下，偏航旋转仍然是主要的旋转分量。此外，我们的方法可以轻松地与其他方法结合，以处理这些非平坦区域，例如通过使用局部平面假设[63]。
简而言之，${\mathbf{R}}_{+}$ 相比 $\mathbf{R}$ 具有以下三个优势。首先，${\mathbf{R}}_{+}$ 的估计解决了其自身的退化问题。如前所述，在 GNC 中，估计的内点数量并不总是保证超过旋转的最小自由度。然而，需要注意的是，$\mathbf{R}$ 的最小自由度为 3，而 ${\mathbf{R}}_{+}$ 的最小自由度为 1。因此，${\mathbf{R}}_{+}$ 对退化更具鲁棒性。经验表明，当源视点和目标视点之间的距离超过 9 米时，例如在户外环境中，退化问题偶尔会发生。然而，即使两个视点相距更远（见第 IV-A 节），我们的方法仍能进行鲁棒的配准。

其次，${\mathbf{R}}_{+}$ 有助于抑制异常值的影响。由于地面可以被认为是足够平坦的，且大多数结构倾向于与地面正交，因此局部几何特征（例如，表面法线、密度等）沿地面法线方向通常是相似的。因此，${\mathbf{ϵ}}_k$ 可以分解为两个部分：a) 平行于地面的分量，即 xy 平面，记为 ${\mathbf{ϵ}}_k^{\parallel }$；b) 垂直于地面的分量，记为 ${\mathbf{ϵ}}_k^{\perp }$。这样，${\mathbf{ϵ}}_k={\mathbf{ϵ}}_k^{\parallel }+{\mathbf{ϵ}}_k^{\perp }$。因此，异常值被分为两类：一类是准内点（quasi-inliers），满足 $||{\mathbf{ϵ}}_k^{\parallel }||\approx 0$ 且 $||{\mathbf{ϵ}}_k^{\perp }||\gg 0$；另一类是确定的异常值（definite outliers），满足 $||{\mathbf{ϵ}}_k^{\parallel }||\gg 0$。注意，$\hat{Q}\cup \hat{D}=\hat{O}$，其中 $\hat{O}$ 表示 GNC 的估计输出。在这种情况下，准内点很少影响 ${\mathbf{R}}_{+}$ 的估计，因为 ${\mathbf{R}}_{+}$ 的估计对 z 值是不变的。因此，$\hat{Q}$ 可以被视为额外的内点，当优化 (5) 时。因此，$\hat{Q}$ 通过增加内点与异常值的比例（即，$\frac{|\hat{Q}|}{|\hat{D}|}$ 到 $\frac{|\hat{Q}|+|Q|}{|\hat{D}|}$），对防止发散有积极影响。这是因为当异常值对应的比例过大时，基于 GNC 的优化有时会发散 [42]，[47]。

D. 使用渐进非凸优化的准SO(3)估计

为了估计${\hat{\mathbf{R}}}_{+}$，引入了使用截断最小二乘法的渐进非凸优化（GNC） [28]。为了简化，将第k个测量对，即${\mathbf{\alpha }}_k$和${\mathbf{\beta }}_k$，表示为${\mathbf{\zeta }}_k$，(3)可以重新定义为：

$${\hat{\mathbf{R}}}_{+}=\arg \underset{{\mathbf{R}}_{+}}{\min }\sum _{k=1}^K{\rho }_{\mu }(r({\mathbf{\zeta }}_k,{\mathbf{R}}_{+}))$$

其中，${\rho }_{\mu }(\cdot )$表示由参数$\mu$控制的替代函数，如图2(b)所示。为此，首先利用Black-Rangarajan对偶性重写该方程 [26], [27]：

$${\hat{\mathbf{R}}}_{+}=\arg \underset{{\mathbf{R}}_{+}\in SO(3),w_k\in [0,1]}{\min }\sum _{k=1}^K\left[w_{k}r({\mathbf{\zeta }}_k,{\mathbf{R}}_{+})+{\Phi }_{{\rho }_{\mu }}(w_k)\right]$$

其中，${\Phi }_{{\rho }_{\mu }}(w_k)=\frac{\mu (1-w_k)}{w_k}{r}^2$是一个惩罚项 [42]。不幸的是，目标函数不能直接求解 [26]。因此，(5)通过交替优化求解：

$${\hat{\mathbf{R}}}_{+}^{(t)}=\arg \underset{{\mathbf{R}}_{+}}{\min }\sum _{k=1}^K{w}_k^{(t-1)}r({\mathbf{\zeta }}_k,{\mathbf{R}}_{+})$$

$${\hat{\mathbf{W}}}^{(t)}=\arg \underset{w_k\in [0,1]}{\min }\sum _{k=1}^K\left[w_{k}r({\mathbf{\zeta }}_k,{\hat{\mathbf{R}}}_{+}^{(t)})+{\Phi }_{{\rho }_{\mu }}(w_k)\right]$$

其中，上标$(t)$表示第t次迭代，$\mathbf{W}=\text{diag}(w_1,w_2,\dots ,w_K)$。每个$w_k$可以通过截断闭式求解：

$${\hat{w}}_k^{(t)}=\{\begin{array}{llcc}0& \text{如果 }{\tilde{r}}_k\in \left[\frac{\mu +1}{\mu }{c}^2,+\infty \right]\frac{c}{{\tilde{r}}_k}\sqrt{\frac{\mu (\mu +1)}{{\tilde{r}}_k^2}-\mu }& \text{如果 }{\tilde{r}}_k\in \left[\frac{\mu }{\mu +1}{c}^2,\frac{\mu +1}{\mu }{c}^2\right]1& \text{如果 }{\tilde{r}}_k\in \left[0,\frac{\mu }{\mu +1}{c}^2\right]\end{array}$$

其中，${\tilde{r}}_k=r({\mathbf{\zeta }}_k,{\hat{\mathbf{R}}}_{+}^{(t)})$。整个过程如图3所示。每次迭代，$\mu$更新为${\mu }^{(t)}=\kappa \cdot {\mu }^{(t-1)}$，其中${\mu }^{(0)}=\frac{c^2}{\max (r({\mathbf{\zeta }}_k,{\mathbf{I}}_3))-c^2}$，$\kappa >1$是一个逐渐增加非凸性程度的因子，如图2(b)所示。当${\sum }_{k=1}^K{\hat{w}}_k^{(t)}r({\mathbf{\zeta }}_k,{\hat{\mathbf{R}}}_{+}^{(t)})$的微分变得足够小时，迭代结束。

不幸的是，异常值对第一次迭代的优化有直接的负面影响，因为所有权重都初始化为1，即${\mathbf{W}}^{(0)}={\mathbf{I}}_K$（图3©）。此外，在权重更新中，如(8)所示，大多数权重偶尔被赋值为零（图3(d)），导致3D空间中的退化（图3(d)和3(g)）。然而，${\hat{\mathbf{R}}}_{+}$旨在抑制异常值的影响，并对退化具有鲁棒性；因此，可以防止灾难性失败，如图3(e)所示。

E. 分量式平移估计

最后，相对平移以分量式的方式进行估计，如图 3(f) 所示。设边界区间集 $\mathcal{E}$ 是包含下界 ${\mathbf{v}}_{ij}-{\sigma }_{ij}{\hat{\mathbf{v}}}_i$ 和上界 ${\mathbf{v}}_{ij}+{\sigma }_{ij}{\hat{\mathbf{v}}}_i$ 的 2|A|-元组，并假设 $\mathcal{E}$ 中的所有元素按降序排列。然后，设第 g 个共识集为 ${\mathcal{I}}_g=\{(i,j)|{\mathbf{v}}_{ij}^g<{\varphi }_g,{\mathbf{v}}_{ij}^g<{\mathbf{v}}_{ij}^{g+1}\}$，其中 ${\varphi }_g\in \mathbb{R}$ 是任何满足 ${\mathbf{v}}_{ij}^g<{\varphi }_g<{\mathbf{v}}_{ij}^{g+1}$ 的值，对于 $g=1,2,\dots ,2|A|-1$。然后，${\hat{\mathbf{t}}}_i^{+}$ 通过非空 ${\mathcal{I}}_g$ 的加权平均估计如下：

$${\hat{\mathbf{t}}}_i^{+}={\left(\sum _{(i,j)\in {\mathcal{I}}_g}\frac{1}{{\sigma }_{ij}^2}\right)}^{-1}\sum _{(i,j)\in {\mathcal{I}}_g}\frac{{\mathbf{v}}_{ij}}{{\sigma }_{ij}^2}$$

最后，在最多 2|A| - 1 个候选值 ${\hat{\mathbf{t}}}_i^{+}$ 中，选择使截断目标函数 (4) 最小化的 ${\hat{\mathbf{t}}}_i^{+}$。由于 COTE 基于估计的旋转是精确的前提，我们的方法最终实现了退化鲁棒的全局配准，即使由于特征匹配不佳和大幅对应剪枝，只剩下一对点。

F. 对应预处理

在本文中，我们使用了快速点特征直方图 (FPFH) [57] 作为常规描述符 [26]。然而，原始的 FPFH 对于 64 线激光雷达传感器捕获的 3D 点云来说太慢了，需要几十秒。因此，我们采用了体素采样 FPFH，体素大小为 $v$，这在对应测试 [6] 之前进行，输出为 ${\mathcal{A}}_{\text{ransac}}$，如图 3(a) 所示。

此后，应用最大团内点选择 (MCIS) [68]，它以 ${\mathcal{A}}_{\text{ransac}}$ 为输入，输出 $\mathcal{A}$，如图 3(b) 所示。TEASER++ [27] 是采用 MCIS 的代表性方法，研究人员表明 MCIS 在速度方面成功地剔除了大量异常值。我们发现寻找精确的最大团是算法的瓶颈。因此，我们对其进行修订，找到一个足够大的启发式最大团，命名为 MCIS-启发式。

2.DARE-SLAM: 感知退化环境中的退化感知与鲁棒闭环

原文：原文
摘要 ： 使能完全自主的机器人能够在大规模、未知且复杂的环境中导航和探索，一直是机器人学研究的核心课题，持续了几十年。自主探索的一个关键要求是构建未知环境的准确且一致的地图，这些地图可用于可靠的导航。闭环检测，即机器人能够断言其已返回到先前访问过的位置的能力，对于一致的地图构建至关重要，因为它减少了由于估计的机器人轨迹中误差累积导致的漂移。此外，在多机器人系统中，闭环检测使得一组机器人获取的局部地图能够合并成环境的一致全局地图。在本文中，我们提出了一种退化感知且抗漂移的闭环方法，以改善在GPS拒止、大规模且感知退化环境中的定位与地图构建（SLAM）中的位置识别和解决3D位置歧义。更具体地说，我们关注的是在地下环境（例如，熔岩管道、洞穴和矿山）中的SLAM，这些环境是复杂且模糊的，当前方法在这些环境中表现不足。本文的第一个贡献是一种退化感知的基于激光雷达的SLAM前端，用于确定未知环境中的可观测性和几何退化程度。利用这一关键能力，可以确定并排除未知环境中的模糊和不可观测区域，从而避免由于虚假或不准确的闭环导致的整个地图的变形。本文的第二个贡献是一种抗漂移的闭环管道，该管道利用从激光雷达点云数据中提取的显著2D和3D特征，以实现鲁棒的多阶段闭环能力。与在大规模环境中仅在局部进行闭环搜索的方法不同，这些方法旨在补偿激光雷达闭环的模糊性和高计算成本，我们提出的方法依赖于快速的预匹配步骤，使得能够在整个机器人轨迹上进行搜索，因此不受机器人轨迹中漂移和误差累积的影响。我们对性能和鲁棒性进行了广泛的评估和分析，并提供了在美国各种极端且感知退化的地下矿山中与最新方法的定位和地图构建结果的比较。

本文核心贡献如下：

• 1. 一个实时的、能够感知退化的SLAM前端，用于确定未知环境中的几何退化程度。
• 2. 一种基于显著的二维和三维几何特征的抗漂移和姿态不变的闭环检测方法，这些特征是从激光雷达点云数据中提取的，与传统的几何方法相比，能够以更高的鲁棒性和准确性检测闭环。所提出的方法不需要额外的传感器来实现更高的性能.
• 3. 在多种极端地下环境中，与现有技术方法的性能进行了广泛的评估和比较。这包括在DARPA地下挑战赛的隧道赛道[49]中收集的数据，以及在宾夕法尼亚州匹兹堡的布鲁斯敦安全研究煤矿和实验矿。

A.基于激光雷达的SLAM前端

基于激光雷达的SLAM前端通过数据关联生成里程计估计和闭环约束，主要通过计算连续激光雷达扫描的相对3D运动来实现。前端包括三个处理层：点云滤波、扫描到扫描配准和扫描到子地图配准。点云滤波采用随机下采样和体素网格滤波，但这些方法会删除重要点。我们使用Zhang等人的方法，提取显著特征来简化点云，然后通过GICP算法进行两阶段配准，以提高里程计估计的准确性。实验表明，扫描到子地图配准能显著减少误差累积，但在复杂环境中仍需抗漂移的闭环检测方法来进一步减少累积误差。

B.几何退化的确定

为了评估未知环境中几何退化的程度，我们开发了一种基于ICP算法获得的刚性变换的距离成本函数的方法。从ICP算法的最后一次迭代中，我们获得了所有对应点集 $P^t$ 和 $P^{t+1}$，以及连续激光雷达扫描之间的相对位姿测量 ${\mathbf{u}}_{t+1}^t=[\mathbf{R},\mathbf{t}]$，其中 $\mathbf{R}({\theta }_x,{\theta }_y,{\theta }_z)$ 是用欧拉角定义的3D旋转，即滚转 ${\theta }_x$、俯仰 ${\theta }_y$ 和偏航 ${\theta }_z$，而 $\mathbf{t}=[t_x,t_y,t_z]$ 是3D平移向量。

给定 ${\mathbf{u}}_{t+1}^t$，它最佳地对齐了两个连续的激光雷达扫描，第 $k$ 对对应点 $p_k^t$ 和 $p_k^{t+1}$ 之间的对齐误差由下式给出

$${\mathbf{d}}_k={\mathbf{u}}_{t+1}^t{p}_k^t-p_k^{t+1},$$

其中 ${\mathbf{d}}_k=[d_{k_x},d_{k_y},d_{k_z}]$ 是失准向量。所有对应点集 $P^t$ 和 $P^{t+1}$ 之间的均方距离由下式给出

$$\mathcal{E}({\mathbf{u}}_{t+1}^t{P}^t,P^{t+1})=\frac{1}{N_k}\sum _{k=1}^{N_k}\Vert {\mathbf{d}}_k{\Vert }^2,$$

其中 $\mathcal{E}(\cdot )$ 是一个标量值成本函数，$N_k$ 是ICP算法中的对应点总数，$\Vert \cdot \Vert$ 表示 ${\mathcal{L}}_2$ 范数。基于ICP的扫描匹配可以表述为一个优化问题，旨在找到使成本函数最小化的 ${\mathbf{u}}_{t+1}^t$，如下所示

$${\mathbf{u}}_{t+1}^t=\underset{{\hat{\mathbf{u}}}_{t+1}^t}{\mathrm{argmin}}\mathcal{E}({\hat{\mathbf{u}}}_{t+1}^t{P}^t,P^{t+1}).$$

使用在中计算的第 $k$ 对对应点的位移向量，可以定义一个以 ${\mathbf{d}}_k$ 为变量的成本函数，如下所示

$${\xi }_k({\mathbf{u}}_{t+1}^t)=d_{k_x}^2+d_{k_y}^2+d_{k_z}^2.$$

假设由于连续激光雷达扫描之间的微小运动导致局部线性，我们计算成本函数的梯度，以便将传感器噪声的误差传播到状态估计的不确定性，如下所示

$${\mathbf{J}}_k^{t+1}=\frac{\partial {\xi }_k({\mathbf{u}}_{t+1}^t)}{\partial (t_x,t_y,t_z,{\theta }_x,{\theta }_y,{\theta }_z)}=\left[\frac{\partial {\xi }_k({\mathbf{u}}_{t+1}^t)}{\partial t_x},\frac{\partial {\xi }_k({\mathbf{u}}_{t+1}^t)}{\partial t_y},\frac{\partial {\xi }_k({\mathbf{u}}_{t+1}^t)}{\partial t_z},\frac{\partial {\xi }_k({\mathbf{u}}_{t+1}^t)}{\partial {\theta }_x},\frac{\partial {\xi }_k({\mathbf{u}}_{t+1}^t)}{\partial {\theta }_y},\frac{\partial {\xi }_k({\mathbf{u}}_{t+1}^t)}{\partial {\theta }_z}\right],$$

其中 ${\mathbf{J}}_k^{t+1}$ 是在时间步 $t+1$ 为第 $k$ 对对应点计算的梯度向量。成本函数的海森矩阵可以从所有 $N_k$ 个对应点集的梯度外积之和近似得到，如下所示

$${\mathbf{H}}^{t+1}=\sum _{k=1}^{N_k}({\mathbf{J}}_k^{t+1}{)}^T{\mathbf{J}}_k^{t+1}.$$

在 ${\mathbf{H}}^{t+1}$ 的特征值分解中，与最小特征值相关的特征向量表示在估计的刚性3D变换中可观测性最低的方向。我们通过计算最大和最小特征值的比值来确定最可观测方向和最不可观测方向之间的相对尺度，如下所示

$${\kappa }^{t+1}=\frac{{\lambda }_{\max }}{{\lambda }_{\min }}.$$
其中 ${\kappa }^{t+1}$ 是近似海森矩阵 $H^{t+1}$ 的条件数，该矩阵是对称的。尽管特征值将依赖于一般的环境变量，例如测量噪声或在获取的激光雷达扫描中的对应点数量，但计算比值将消除对缩放特征值的环境变量的依赖。因此，较大的条件数对应于更高水平的几何退化 [57]。这意味着计算出的变换 ${\mathbf{u}}_{t+1}^t$ 的一个或多个参数的线性组合存在较大的误差。接近 1 的小条件数对应于计算出的变换中所有参数的等效可观测性。在我们的实验中，如果 $\log ({\kappa }^{t+1})\ge {\kappa }_{th}$，则将姿态分类为退化的，其中 ${\kappa }_{th}$ 是基于正常变化的期望的退化阈值，并通过实验结果验证。为了通过将闭环搜索限制在可观测性最高的区域来实现预期的性能目标，从而减少错误或不准确的闭环的可能性，将被分类为退化的机器人姿态和相应的关键扫描从闭环考虑中移除。

图6 (a) 室内办公室走廊的地图。几个位置标记为A、B、C和D，以展示特征值和条件数对场景中不同几何退化程度的响应。log λ显示了机器人在走廊中导航时近似海森矩阵的六个特征值的响应。log(κ)图显示了由最大和最小特征值比值计算得到的条件数的对数。激光雷达滑移图显示了在1秒间隔内，由轮式惯性里程计和激光雷达里程计估计的机器人前进位移的差异。在这项实验中，车轮滑移可以忽略不计。

图 6 展示了在室内办公环境中确定几何退化的过程，当机器人沿着一条具有平坦和对称墙壁的长走廊导航时。对数特征值 $log(\lambda )$ 的图显示了近似海森矩阵的所有六个特征值的响应。当机器人在无特征的走廊中导航时，在轨迹的某一段中，可以看到海森矩阵的最小特征值的值显著下降，而其他特征值的变化最小。这导致条件数的值增加，如 $log(\kappa )$ 值的图所示。在这项实验中，可以验证 $log(\kappa )$ 值的增加对应于基于车轮惯性里程计的嘈杂激光雷达里程计估计。当机器人在室内办公环境中导航时，我们比较了激光雷达里程计的输出与车轮惯性里程计的输出。通常，车轮里程计不可靠，但由于机器人以低速移动且走廊铺有地毯，车轮打滑可以忽略不计。激光雷达打滑图显示了基于车轮惯性和激光雷达里程计计算的前向运动在 1 秒间隔内的差异。结果表明，$log(\kappa )>2.0$ 的值对应于激光雷达和车轮惯性里程计之间最大的差异。退化度量 $\kappa$ 对无特征走廊的几何退化有响应，因为机器人沿墙壁的前向运动对激光雷达里程计变得不可观测：我们称之为激光雷达打滑。

3. 抗退化的激光雷达-雷达-惯性里程计

链接：论文
代码：github
摘要——为了使自主机器人在未来能够更加稳健地在具有挑战性的环境中运行，有必要实现更高水平的自主性。对于许多自主系统而言，估计和里程计仍然是一个单一的故障点，从这一点上恢复通常非常困难，甚至几乎不可能。因此，鲁棒的里程计解决方案至关重要。在这项工作中，我们提出了一种紧耦合的激光雷达-雷达-惯性融合方法，通过利用互补的感知模态来减轻激光雷达退化的影响，同时在环境条件良好的情况下保持激光雷达的精度。所提出的方法在基于因子图的窗口平滑器中结合了多种模态，并具有特定于传感器信息的因子公式，这使得在退化情况下，部分信息可以通过非退化轴传递给图。所提出的方法在真实世界的测试中得到了验证，测试对象是一个在飞行中经历退化条件（包括几何自相似性和遮挡）的机器人。

图1. 本文探讨的具有挑战性的环境示例，包括几何自相似的隧道和充满雾的走廊的建筑。LiDAR点云显示了感知退化的影响。雷达数据被融合以实现鲁棒的里程计。

B. 状态估计

所提出方法的状态空间包括惯性测量单元（IMU）在世界坐标系中的位姿 ${}^w{\mathbf{T}}_I\in SE(3)$、IMU 的线速度 ${\mathbf{v}}_I\in {\mathbb{R}}^3$ 以及 IMU 的偏置 $\mathbf{b}\in {\mathbb{R}}^6$，因此图中给定节点的状态表示为

$$\mathbf{x}=\left[\begin{array}{ccc}{}^w{\mathbf{T}}_I& {\mathbf{v}}_I& \mathbf{b}\end{array}\right]\text{\hspace{1em}(1)}$$

其中，IMU 相对于世界坐标系的位置和方向（${}^w{\mathbf{p}}_I,{}^w{\mathbf{R}}_I$）构成位姿，$\mathbf{b}$ 包含加速度计偏置 $b_a$ 和陀螺仪偏置 $b_g$ 的项。

在所提出的方法中，估计问题通过因子图求解，采用滑动窗口上的增量优化来降低计算成本。以一致的速率获得状态估计是理想的，而雷达测量的延迟比激光雷达低，因此使用雷达测量的时间戳来创建图节点。此外，由于激光雷达将进行去偏斜处理，假设每个激光雷达测量至少有一个雷达测量，激光雷达可以简单地去偏斜到雷达测量的时间戳。

设时间 $i$ 的雷达测量为 ${\mathcal{R}}_i$，时间 $i$ 和 $j$ 之间的时间段内的 IMU 测量为 ${\mathcal{I}}_{ij}$，时间 $i$ 的激光雷达特征测量集（包括平面和线特征，见 III-B.2 节）为 ${\mathcal{L}}_i$。所有雷达测量的集合到时间 $k$ 为 M k = { I i j , L i , R i } \mathcal{M}_k = \{\mathcal{I}_{ij}, \mathcal{L}_i, \mathcal{R}_i\} Mk​={ Iij​,Li​,Ri​}，其中 $(i,j)\in {\mathcal{M}}_k$。因此，最大后验估计（MAP）为

$${\mathbf{x}}_{k-l:k}^{\ast }=\arg \underset{{\mathbf{x}}_{k-l:k}}{\max }p({\mathbf{x}}_{k-l:k}|{\mathcal{Z}}_{k-l:k})\text{\hspace{1em}(2)}$$

$$\propto p({\mathbf{x}}_{k-l-1})p({\mathcal{Z}}_{k-l:k}|{\mathbf{x}}_{k-l:k})$$

其中，${\mathbf{x}}_{k-l:k}$ 是从时间 $k-l$ 到 $k$ 的窗口状态集。假设高斯噪声模型且均值为零，这可以重写为因子图优化的目标函数

$${\mathbf{x}}_{k-l:k}^{\ast }=\arg \underset{{\mathbf{x}}_{k-l:k}}{\min }\left(\Vert {\mathbf{e}}_{k-l-1}{\Vert }_{{\Sigma }_0}^2+\sum _{(i,j)\in {\mathcal{M}}_{k-l:k}}\Vert {\mathbf{e}}_{{\mathcal{I}}_{ij}}{\Vert }_{{\Sigma }_{\mathcal{I}}}^2+\sum _{i\in {\mathcal{M}}_{k-l:k}}\Vert {\mathbf{e}}_{{\mathcal{R}}_i}{\Vert }_{{\Sigma }_{\mathcal{R}}}^2+\sum _{i\in {\mathcal{M}}_{k-l:k}}\Vert {\mathbf{e}}_{{\mathcal{L}}_i}{\Vert }_{{\Sigma }_{\mathcal{L}}}^2\right)\text{\hspace{1em}(3)}$$

其中，${\mathbf{e}}_{k-l-1}$、${\mathbf{e}}_{{\mathcal{I}}_{ij}}$、${\mathbf{e}}_{{\mathcal{R}}_i}$ 和 ${\mathbf{e}}_{{\mathcal{L}}_i}$ 分别是先验和各传感器测量因子的残差，${\Sigma }_0$、${\Sigma }_{\mathcal{I}}$、${\Sigma }_{\mathcal{R}}$ 和 ${\Sigma }_{\mathcal{L}}$ 分别是它们的协方差矩阵。这些残差项以及协方差和雅可比矩阵定义了添加到因子图中的因子，最终架构如图 2 所示。此外，这些因子的解析雅可比矩阵是可用的，与在线数值微分相比，计算成本更低。这些推导在此不提供，详见 [48] 以了解李群的介绍。

1) 惯性测量单元测量

由于 IMU 测量的速率远高于激光雷达或雷达，实现中将 IMU 测量累积到一个缓冲区中，等待雷达测量。接收到雷达测量后，在图中创建一个新节点，并将所有 IMU 测量作为因子添加到图中。

从图的前一个节点的时间戳到雷达测量的时间戳，IMU测量被积分并用于通过预积分的IMU因子[49]连接两个节点，包括以下残差及其协方差：

$${\mathbf{e}}_{\mathcal{I}}={\left[\begin{array}{ccc}{\mathbf{e}}_{\Delta {\mathbf{R}}_{\mathcal{I}}}& {\mathbf{e}}_{\Delta {\mathbf{v}}_{\mathcal{I}}}& {\mathbf{e}}_{\Delta {\mathbf{p}}_{\mathcal{I}}}\end{array}\right]}^{\top },{\Sigma }_{\mathcal{I}}\text{\hspace{1em}(4)}$$

其中，${\mathbf{e}}_{\Delta {\mathbf{R}}_{\mathcal{I}}}$、${\mathbf{e}}_{\Delta {\mathbf{v}}_{\mathcal{I}}}$ 和 ${\mathbf{e}}_{\Delta {\mathbf{p}}_{\mathcal{I}}}$ 分别是IMU因子在方向、速度和位置方面的残差。

2) 激光雷达测量： 对于每个激光雷达点云测量，首先将当前状态传播到收集期间所有IMU测量的时间戳，然后在恒定线性和角速度模型下插值，以获得每个点的时间戳下的激光雷达位姿。利用激光雷达和雷达之间的同步，以及两者均以10 Hz运行的事实，可以将点云去偏移到位于激光雷达收集期间的雷达时间戳，从而对齐测量。使用雷达时间戳和点时间戳之间的相对位姿对点进行去偏移。去偏移后，提取特征并用于制定添加到图中的因子。需要注意的是，在退化场景下（例如在几何自相似环境中），基于特征的单个因子被添加，而不是单个6自由度位姿先验（如[14, 47]中所述），这将不可避免地沿退化轴引入错误信息。通过添加基于特征的单个因子，我们确保不会沿退化轴引入信息。在所提出的方法中，考虑了两种类型的特征，即“平面”和“线”特征，这意味着激光雷达测量由平面特征测量 ${\mathcal{L}}^p$ 和线特征测量 ${\mathcal{L}}^l$ 组成，即 $\mathcal{L}={\mathcal{L}}^p\cup {\mathcal{L}}^l$。这些因子在本节后面描述。

对于每个去偏移的点云，选择具有高曲率的点作为线特征，选择具有低曲率的点作为平面特征，如[10]中所述。然后，这两组点分别在它们各自的局部子图中下采样，以找到对应关系，构建用于优化的约束因子。特征的对应关系通过首先找到特征点的5个最近邻，然后使用这些邻域的分布的特征值来验证它们是否形成3D线或平面。从子图中构建k-d树，用于加速搜索。最近邻的均值被设置为位于相应线或平面上的点 ${}^w\mathbf{j}$，而分布的特征向量用于找到表示平面的法向量 ${}^w\mathbf{n}$ 或表示线的方向向量 ${}^w\mathbf{d}$。

现在描述优化中使用的因子的构建。注意，对于激光雷达因子，假设激光雷达-IMU外参 ${}_I^{}{\mathbf{T}}_L$ 是已知的，并用于将点云转换到IMU坐标系。

a) 激光雷达点到平面因子： 对于在激光雷达测量坐标系中表示的点 ${}^L\mathbf{i}$，对应关系由位于3D平面 ${}^w\mathbf{j}$ 上的点和平面的法向量 ${}^w\mathbf{n}$ 参数化。误差计算如下：

$${\mathbf{e}}_{{\mathcal{L}}^p}=\left(({}^w\mathbf{i}-{}^w\mathbf{j})\cdot {}^w\mathbf{n}\right){}^w\mathbf{n}\text{\hspace{1em}(5)}$$

唯一的非零雅可比矩阵为：

$$\frac{\partial {\mathbf{e}}_{{\mathcal{L}}^p}}{\partial {}^w{\mathbf{T}}_I}={}^w\mathbf{n}{}^w{\mathbf{n}}^{\top }\left[\begin{array}{cc}-{}^w{\mathbf{R}}_I[{}^I\mathbf{i}{]}^{\times }& {}^w{\mathbf{R}}_I\end{array}\right]\text{\hspace{1em}(6)}$$

b) 激光雷达点到线因子： 对于点 ${}^L\mathbf{i}$，对应关系由3D线上的点 ${}^w\mathbf{j}$ 和其方向向量 ${}^w\mathbf{d}$ 参数化。误差形式如下：

$${\mathbf{e}}_{{\mathcal{L}}^l}=({}^w{\mathbf{T}}_I{}^L\mathbf{i}-{}^w\mathbf{j})-\left(({}^w{\mathbf{T}}_I{}^L\mathbf{i}-{}^w\mathbf{j})\cdot {}^w\mathbf{d}\right){}^w\mathbf{d}\text{\hspace{1em}(7)}$$

唯一的非零雅可比矩阵为：

b) 不确定性：该因子公式的不确定性是根据最小二乘解的协方差矩阵估计公式计算的

$$\mathrm{cov}({}^R\bar{\mathbf{v}})=({\mathbf{X}}^{\top }\mathbf{X}{)}^{-1}\frac{\Vert {\mathbf{v}}_r-\mathbf{X}{}^R\mathbf{v}{\Vert }^2}{N-N_{\text{dof}}}(14)$$

其中，自由度 $N_{\text{dof}}$ 为 3。请注意，这里假设与 $e_R$ 相关的不确定性（即 ${\Sigma }_R$）主要由 ${}^R\mathbf{v}$ 主导。

最小二乘公式假设最小二乘公式中的不确定性仅存在于因变量中，而这里因变量和自变量都包含不确定性。具体来说，因变量的不确定性来自多普勒速度测量的不确定性，而自变量的不确定性来自方位/仰角测量噪声通过方向向量计算的传播。其他作者已经解决了这个问题 [51, 52]，但改进似乎微乎其微，同时导致了高昂的计算成本。因此，这里没有采用这种不同的方法。

4. 初始化：增加了一个初始化程序，假设机器人静止，这提高了轨迹初始部分的收敛速度和精度。在此期间，积累了几秒钟的 IMU 数据，用于估计重力大小（假设加速度计偏置较小）、初始滚转和俯仰角以及陀螺仪偏置。

C. 实现细节

所提出的方法使用开源库 GTSAM [53] 以 C++ 实现，用于因子图优化。我们使用了库中提供的增量固定滞后平滑器，窗口大小为 0.75 秒。特征集（和相应的地图）以 0.2 米的分辨率维护，在较大环境中导致 1000 到 2000 个因子被添加。此外，所有基于激光雷达和雷达的因子均采用 Huber 损失函数，以提高对异常值的鲁棒性。所提出的方法经过调整，以 10 Hz 的一致输出速率运行，通过利用多线程，能够在 Intel i7-11800H CPU 上实时运行。

此外，在雾环境中，为了提高性能，增加了一个特征启发式方法，优先选择在地面上发现的平面特征。这里的逻辑是，在人造结构中低空飞行时，更有可能在地板上找到良好的特征。这在前视雷达对垂直漂移最不敏感的情况下特别有帮助。地面上的特征在 III-B.2 节的点到平面因子之后添加。