退化环境下定位（一）

---

相关文章:退化环境（二）

---

1.退化环境定义

在SLAM（同时定位与地图构建中，退化环境是指那些由于以下原因，导致定位和建图效果不佳的环境：

• 环境特征不足：环境中缺乏足够的特征点或纹理信息，使得传感器难以获取足够的约束来进行准确的定位和建图。
• 传感器观测数据与地图元素匹配关系错误：由于各种原因（如光线变化、遮挡、动态变化等），传感器观测到的数据与地图中的元素无法正确匹配。

退化环境包含以下具体场景：

1. 空旷环境：如广场、仓库等，缺乏足够的特征点或纹理信息，导致传感器难以捕捉到足够的约束。
2. 隧道或狭长走廊：具有单一的特征和纹理，可能不足以支持SLAM系统的有效运行。
3. 动态变化的环境：环境中的动态变化（如移动的人或物体）可能会干扰传感器的观测，导致匹配关系错误。

在退化环境中，SLAM系统可能会面临以下问题：

• 定位不准确：由于环境特征不足或匹配关系错误，导致定位结果偏离实际位置。
• 建图质量差：由于传感器观测数据不准确或缺失，导致构建的地图质量低下。
• 系统稳定性差：在退化环境中，SLAM系统的性能和稳定性可能会受到影响，导致系统崩溃或无法正常工作。

2.相关数据集

2.1 退化环境最新数据集GEODE：多样环境助力突破LiDAR-SLAM技术的局限性

网址：github

介绍了GEODE数据集，这是一个专门针对LiDAR-SLAM技术在退化环境中表现的多场景数据集。LiDAR SLAM技术在面对隧道和地铁等具有几何约束不足的场景时，容易出现退化解，即状态优化收敛到由噪声引发的最优解。文章指出，LiDAR硬件的选择对退化现象有显著影响，例如，具有较少扫描线的旋转式LiDAR在移动过程中可能会失去对结构特征的捕捉，从而更容易出现退化。此外，配备了反射率等通道的LiDAR可以通过利用额外的环境模态来增强定位，而不仅仅依赖空间特征。GEODE数据集包含了七个真实世界的环境，涵盖了广泛的几何退化片段，是第一个公开的多LiDAR、多场景的数据集，旨在帮助研究人员和开发者更好地理解和改进LiDAR-SLAM技术在复杂环境中的表现。

2.2 雷达退化数据集

介绍： 提供了一个与 LiDAR 退化环境处理相关的数据集，该数据集通过 LiDAR-雷达-惯性融合方法与因子图结合使用。这个数据集包含了一个空中机器人在对 LiDAR 传感器具有挑战性的环境中手动飞行时的传感器测量数据，包括 IMU、LiDAR 和 FMCW 雷达。这些环境包括具有几何自相似性的自行车隧道和充满雾气的大学走廊。此外，还提供了传感器之间的 CAD 外参。数据集旨在支持研究如何提高 LiDAR 在退化环境中的性能，例如在雾气或隧道等情况下的鲁棒性。

rosbag下载链接

浓雾大学走廊

2.3 ENWIDE LiDAR 惯性数据集

介绍： enwide 数据集由 ETH Zurich 的 ASL 提供，专注于在具有退化几何特征的环境中收集激光雷达惯性数据，以支持机器人定位和导航研究。该数据集使用 Ouster OS0 128 激光雷达和 IMU 记录，提供包括平滑和动态动作的多种环境序列，并以 rosbags 格式提供。此外，还提供 IMU 到棱镜的外参和真实位置的 CSV 文件，方便研究人员下载和使用。
链接：网址

3.相关算法

Switch-SLAM
检测方法：Switch-SLAM通过分析LiDAR里程计的Hessian矩阵特征值来检测退化。选择特征值中的三个最小值，按升序排列并归一化处理，得到归一化特征值。利用卡方检验定义归一化特征值的非启发式阈值，通过比较归一化特征值与阈值来判断是否发生退化。
处理方法：当检测到LiDAR里程计退化时，Switch-SLAM从LiDAR过渡到视觉里程计，以稳定整个扫描到地图优化过程。采用状态缓冲方法，防止状态发生剧烈变化，并通过线性插值弥合LiDAR和视觉里程计在退化开始或结束时估计状态之间的差距。

drpm
检测方法：drpm通过量化Hessian在任意方向上的退化概率来进行检测。估计进入Hessian的噪声并评估信噪比，计算方向上的Hessian方差，利用这些信息量化方向退化的可能性。
处理方法：在点到平面ICP算法中，通过减少退化方向上虚假信息的影响来改进配准过程。定义修改后的伪逆，根据退化检测结果调整特征值的倒数，以平滑的方式考虑退化，缓和非退化和退化几何体之间的过渡。

P2D-DO
检测方法：P2D-DO通过观察局部区域内分布概率的变化来检测退化。利用自适应体素分割算法确保局部几何特征满足高斯分布假设，通过分析相邻帧点云在局部区域内的概率分布变化，输出表征退化状态的因子。
处理方法：在匹配过程中通过分配更大的权重来优先考虑可靠的点云，减少低置信度点云的影响，从而提升LiDAR SLAM位姿估计的准确性和鲁棒性。

退化感知点云配准
检测方法：通过优化Hessian的特征值分解来检测退化方向。将优化Hessian分解为对应于旋转和平移部分的子矩阵，通过特征值分解识别退化方向。
处理方法：在优化过程中引入退化感知的硬约束或软约束，以提高点云配准的鲁棒性。主动优化退化抑制在缺乏可靠外部估计辅助的情况下是必要和有利的。

3.1 Switch-SLAM： 用于简并环境的基于开关的 LiDAR 惯性视觉 SLAM

介绍了 Switch-SLAM，一种基于开关的 LiDAR 惯性视觉 SLAM，用于简并环境，旨在解决 LiDAR 和视觉 SLAM 在简并环境中的挑战。Switch-SLAM 通过利用开关结构实现高鲁棒性和准确性，当检测到 LiDAR 里程计退化时，该开关结构从 LiDAR 过渡到视觉里程计。为了有效地检测退化，Switch-SLAM 采用了一种非启发式退化检测方法，该方法不需要启发式调整，并展示了在各种环境中的泛化性。Switch-SLAM 在包含 LiDAR 和视觉里程计退化场景的不同数据集上进行评估。实验结果突出了所提出的方法在具有 LiDAR 或视觉 SLAM 简并的多种具有挑战性的环境中准确和稳健的定位。

图 1. 来自模拟 Farm 数据集的快照和地图，它们表现出激进的运动和结构较少的环境。

3.1.1. LiDAR里程计的退化检测

大多数LiDAR退化情况源于无结构环境，例如广阔的开放区域、长走廊和隧道状结构，如图3所示。然而，即使在这些场景中，LiDAR的感知范围内仍然存在平面或边缘特征，使得LiDAR里程计很少退化超过3-DOFs。同样，文献[24]展示了多种无结构形状，在这些形状中LiDAR里程计会退化；然而，它们中没有一个也超过3-DOFs。因此，我们可以做出物理假设，即当存在平面或边缘特征时，退化很少发生在六个自由度中的三个。因此，我们的研究主要集中在其他三个自由度方向的退化上。

Hessian矩阵的$H_1$的特征值用于检测退化，其中$d_1,d_2,d_3$表示最退化的DOFs。对应的三个特征值，表示为$\lambda =[{\lambda }_1,{\lambda }_2,{\lambda }_3]$，是从$H_1$的特征值中按升序提取的三个最小值。然后，$\lambda$被归一化为$\bar{\lambda }=[{\bar{\lambda }}_1,{\bar{\lambda }}_2,{\bar{\lambda }}_3]$。我们使用卡方检验[25]定义归一化特征值的非启发式阈值。卡方检验是一种统计测试，用于评估两个分类变量是否密切相关。因此，通过使用卡方检验，可以定义统计显著性在预期和观察值之间丧失的边界线作为非启发式阈值。在我们的情况下，卡方检验的DOF可以设置为2，因为$\bar{\lambda }$被归一化为1，当观察到其他两个值时可以确定一个值。零假设认为每个归一化特征值在95%区间内遵循其各自的期望值，允许在预期值周围5%显著性水平的观察值异常值。因此，拒绝零假设的卡方检验公式为：

$${\left(\bar{\lambda }-e_m\right)}^2/e_m>0.103,\text{\hspace{1em}(11)}$$
其中0.103表示在95%置信水平下2-DOFs的卡方值，$e_m$表示每个特征值的期望值。请注意，尽管卡方值是唯一的用户定义参数，但它在所有实验中保持不变。根据[26]，具有有限维度的随机构造的真实对称矩阵的特征值分布可以用半圆分布[27]来近似。因此，为了确定$e_m$，我们假设$\bar{\lambda }$的每个分布都遵循对称概率分布。此外，请注意$\bar{\lambda }$的范数为1，且${\bar{\lambda }}_1\le {\bar{\lambda }}_2\le {\bar{\lambda }}_3$。因此，由于${\bar{\lambda }}_1$可以取最大值$1/\sqrt{3}$（当${\bar{\lambda }}_1={\bar{\lambda }}_2={\bar{\lambda }}_3$）和最小值0，${\bar{\lambda }}_1$的期望值，表示为$e_1$，是$1/2\sqrt{3}\approx 0.289$。对于${\lambda }_2$，最大值是$1/\sqrt{2}$（当${\bar{\lambda }}_1=0$且${\bar{\lambda }}_2={\bar{\lambda }}_3$）且平均最小值是$e_1$（当${\bar{\lambda }}_1={\bar{\lambda }}_2$）。因此，$e_2$是0.498。同样，对于${\bar{\lambda }}_3$，最大值是1，平均最小值是$e_2$。因此，$e_3$是0.749。根据(11)，$\bar{\lambda },{\bar{\lambda }}_t$的退化阈值是：

$${\bar{\lambda }}_t=e_m-\sqrt{0.103}\ast e_m.\text{\hspace{1em}(12)}$$

因此，非启发式阈值${\bar{\lambda }}_t$被确定为[0.12, 0.27, 0.48]从(12)。如果任何$\bar{\lambda }$值低于${\bar{\lambda }}_t$的相应值，初始猜测将从LiDAR里程计的值“切换”到视觉里程计以稳定整个扫描到地图优化过程并帮助稳定估计每个特征值。相反，如果所有$\bar{\lambda }$值都大于${\bar{\lambda }}_t$的相应值，系统将基于纯LiDAR里程计设置初始猜测。

然而，在处理接近定义阈值的状态时仍然存在问题。这种情况可以迅速改变初始猜测的值，并导致扫描到地图优化过程中的非平滑结果。为了解决这个问题，我们采用了状态缓冲方法，该方法防止状态发生剧烈变化。在这种方法中，当前状态被分类为“正常”、“开始/结束退化”和“完全退化”，从一个具有预定义大小的队列$Q_s$中，该队列连续存储过去的状态信息，如图4所示。

因此，我们采用线性插值来弥合LiDAR和视觉里程计在LiDAR里程计退化开始或结束时估计状态之间的差距，如图4(b)和4(d)所示。6-DOF状态的初始猜测，$T_k\in \mathcal{M}$，在退化状态的开始或结束时使用先前状态$T_{k-1}$，LiDAR里程计的微分状态$\delta T_{k-1,k}^l=T_k^l\boxminus T_{k-1}^l\in {\mathbb{R}}^n$，和视觉里程计的微分状态$\delta T_{k-1,k}^v=T_k^v\boxminus T_{k-1}^v$如下所示：

$$T_k=T_{k-1}⊞\sqrt{3}{\overline{\lambda }}_1\delta T_{k-1,k}^l⊞\left(1-\sqrt{3}{\overline{\lambda }}_1\right)\delta T_{k-1,k}^v,\text{\hspace{1em}(13)}$$

这里，$\sqrt{3}{\overline{\lambda }}_1$的最大值是1。“$⊞$”和“$⊞$”表示将元素映射到和从给定流形$\mathcal{M}$及其切空间${\mathbb{R}}^n$[28]的操作。

方法

• 特征值分析：使用H₁也就是Hessian矩阵的特征值来识别退化方向。选择特征值中的三个最小值，按升序排列，记为λ = [λ₁, λ₂, λ₃]，并进行归一化处理，得到λ̅ = [λ̅₁, λ̅₂, λ̅₃]。
• 卡方检验：采用卡方检验来定义归一化特征值的非启发式阈值。卡方检验用于评估两个分类变量的关联性，并确定在预期值和观察值之间失去统计显著性的边界线。
• 期望值计算：根据有限维随机构造的真实对称矩阵的特征值分布可以用半圆分布来近似，计算每个归一化特征值的期望值em。对于λ̅₁, λ̅₂, λ̅₃，分别计算其期望值e₁, e₂, e₃。
• 退化阈值设定：根据公式(11)，设定退化阈值λ̅_t = em - √0.103 * em。非启发式阈值λ̅_t由公式(12)确定为[0.12, 0.27, 0.48]
  

3.2 drpm:用于点到平面误差最小化的概率退化检测

代码：github
摘要：由于非信息性几何结构引起的退化现象已知会恶化基于激光雷达（LiDAR）的定位和映射。这项工作引入了一种新的概率方法来检测和减轻点到平面误差最小化中退化的影响。点到平面优化问题的Hessian矩阵上的噪声由用于构建它的点和表面法线的噪声所表征。我们利用这种表征来量化一个方向退化的概率。退化检测过程被用于一个新的实时退化感知迭代最近点算法中，用于激光雷达配准，其中我们平滑地减弱在退化方向上的更新。该方法的参数是基于激光雷达数据表中提供的噪声特性选择的。我们在四个真实世界的实验中验证了这种方法，证明它在检测和减轻退化的不利影响方面优于最先进的方法。为了社区的利益，我们在github.com/ntnu-arl/drpm上发布了该方法的代码

图 1：我们提出的方法（中间列）与最先进的方法的比较[14]（左列）。我们的方法通过成功检测和解释简并来避免漂移。顶行：从 Seemühle 地下矿井出发，使用带腿的 ANYmal 平台（右上）。底行：来自使用飞行平台的圆柱形水箱（右下角）

该方法在四个真实世界的数据集上进行了验证，这些数据集是由在具有挑战性环境中的腿式和空中机器人收集的。我们将我们的方法与最先进的方法[14]、[15]和最近的方法[19]进行了比较。图1显示了指示性的结果。补充视频总结了该方法和实验。
总体而言，我们的主要贡献是：
一种新的适用于点到平面代价函数的概率和可推广的退化检测方法。
一种概率退化感知的点到平面ICP算法，用于实时激光雷达配准，该算法减少了在有效退化方向上的噪声影响。
在四个真实世界数据集上验证该方法的实验，性能优于[14]、[15]、[19]的方法。
由于我们的退化检测方法需要估计法线的协方差，我们还额外包括了一种实用的方法来做到这一点。

3.2.1 概率退化检测

算法 1 退化感知ICP的更新步骤

输入: { p ^ i , n ^ i , d ^ i , w r i , Σ p i , Σ n i } i = 1 , … , N \{\hat{p}_i, \hat{n}_i, \hat{d}_i, w_{r_i}, \Sigma_{p_i}, \Sigma_{n_i}\}_{i=1,\ldots,N} { p^​i​,n^i​,d^i​,wri​​,Σpi​​,Σni​​}i=1,…,N​
输出: ${\hat{x}}^{\ast }$

1. 计算Hessian矩阵 $\hat{\mathbf{H}}$ 和右侧 ${\sum }_i{\hat{\mathbf{J}}}_i^T{\hat{b}}_i$
2. 对Hessian矩阵进行对角化 $\hat{\mathbf{H}}=\hat{\mathbf{U}}\hat{\Lambda }{\hat{\mathbf{U}}}^T$
3. 计算 ${\hat{\mu }}_{{\hat{u}}_k}$, ${\hat{\sigma }}_{{\hat{u}}_k}$ 和 $p_{{\hat{u}}_k}$，$k=1,\dots ,6$
4. ${\hat{x}}^{\ast }\leftarrow \hat{\mathbf{U}}\mathbf{P}{\hat{\Lambda }}^{-1}{\hat{\mathbf{U}}}^T{\sum }_i{\hat{\mathbf{J}}}_i^T{\hat{b}}_i$.
5. 返回 ${\hat{x}}^{\ast }$

概率退化检测的目标是量化Hessian在任意方向$u\in {\mathbb{R}}^6$上退化的概率。为此，我们估计进入Hessian的噪声并评估信噪比。噪声Hessian $\hat{H}$ 是向量的外积之和

$$\hat{H}=\sum _i{\hat{v}}_i{\hat{v}}_i^T,{\hat{v}}_i=w_i\left[\begin{array}{c}[{\hat{p}}_i{]}_{\times }\\ I\end{array}\right]{\hat{n}}_i.\text{\hspace{1em}(12)}$$

外积的噪声特性由它们所包含的向量的噪声特性决定。使用噪声模型(7)-(8)与(12)中的${\hat{v}}_i$得到

$${\hat{v}}_i=v_i+w_i\underset{:=B_i}{\underbrace{\left[\begin{array}{cc}-[n_i{]}_{\times }& [p_i{]}_{\times }[n_i{]}_{\times }\\ 0& [n_i{]}_{\times }\end{array}\right]}}\left[\begin{array}{c}{ϵ}_i\\ {\eta }_i\end{array}\right]+w_i[{ϵ}_i{]}_{\times }[n_i{]}_{\times }{\eta }_i,v_i=w_i\left[\begin{array}{c}[p_i{]}_{\times }\\ I\end{array}\right]n_i.\text{\hspace{1em}(13)}$$

期望$E[{\hat{v}}_i]$和协方差${\Sigma }_i=\text{cov}({\hat{v}}_i)$为

$$E[{\hat{v}}_i]=v_i,{\Sigma }_i=B_i\left[\begin{array}{cc}E[{ϵ}_i{ϵ}_i^T]& 0\\ 0& E[{\eta }_i{\eta }_i^T]\end{array}\right]B_i^T.\text{\hspace{1em}(14)}$$

反过来，(12)中噪声Hessian的期望为

$$E[\hat{H}]=\sum _{i}E[{\hat{v}}_i{\hat{v}}_i^T]=\sum _i{v}_i{v}_i^T+\sum _i\text{cov}({\hat{v}}_i)=H+\Sigma ,\Sigma :=\sum _i\text{cov}({\hat{v}}_i).\text{\hspace{1em}(15)}$$

也就是说，在期望中，$H$被$\Sigma$扰动。我们计算方向$u$（$\Vert u{\Vert }_2=1$）上的$\hat{H}$的方差，如下所示

$${\sigma }_u^2=\text{cov}(u^T\hat{H}u)=\sum _i\text{cov}(u^T{\hat{v}}_i{\hat{v}}_i^{T}u).\text{\hspace{1em}(16)}$$

由于$\text{cov}(u^T{\hat{v}}_i{\hat{v}}_i^{T}u)=\text{cov}({\hat{v}}_i^{T}u{u}^T{\hat{v}}_i)$，我们使用[24]中二次型方差的表达式来找到

$$\text{cov}(u^T{\hat{v}}_i{\hat{v}}_i^{T}u)=2(u^T{\Sigma }_{i}u{)}^2+4(u^T{\Sigma }_{i}u)(u^T{v}_i{)}^2.\text{\hspace{1em}(17)}$$

请注意，如果${\hat{v}}_i$是具有单位方差的标量，则(15)和(16)将分别对应于非中心卡方分布的均值和方差[25]。期望和方差在(15)-(16)中用于量化方向退化的可能性。由于在实践中我们无法获得真实的点和法线，我们使用它们的噪声对应物来估计${\Sigma }_i,\Sigma$和${\sigma }_u^2$。这些估计分别表示为${\hat{\Sigma }}_i,\hat{\Sigma }$和${\hat{\sigma }}_u^2$。设${\xi }_u$表示方向$u$上的噪声，并假设${\xi }_u\sim \mathcal{N}({\hat{\mu }}_u,{\hat{\sigma }}_u^2)$，其中

$${\hat{\mu }}_u=u^T\hat{\Sigma }u,\text{\hspace{1em}(18)}$$

$${\hat{\sigma }}_u^2=\sum _i\left[2(u^T{\hat{\Sigma }}_{i}u{)}^2+4(u^T{\hat{\Sigma }}_{i}u)(u^T{\hat{v}}_i{)}^2\right].\text{\hspace{1em}(19)}$$

此外，设$a_u=u^{T}Hu$和${\hat{a}}_u=u^T\hat{H}u$。根据定义，我们有${\hat{a}}_u=a_u+{\xi }_u$。$a_u/{\xi }_u$是一个信噪比。如第二节所讨论的，为了可靠地估计方向$u$，信号$a_u$应该主导噪声${\xi }_u$。因此，我们估计信号在方向$u$上比噪声大$s$倍的概率为

$$p_u=P(a_u\ge s{\xi }_u)=P({\hat{a}}_u\ge (s+1){\xi }_u).\text{\hspace{1em}(20)}$$

为了设定$s$，请注意相对误差$e_u^{\text{rel}}=({\hat{a}}_u-a_u)/a_u={\xi }_u/a_u$在$a_u\ge s{\xi }_u$时被$s$限制，即$e_u^{\text{rel}}\le s^{-1}$。我们设定$s=10$以目标相对误差最多为$10\%$。通常，增加$s$会使退化检测更加保守。如果$p_u\approx 0$，则认为$u$是退化的；如果$p_u\approx 1$，则认为是非退化的。

从(12)-(15)可以观察到，进入Hessian的噪声取决于测量计数$N$，权重 { w i } i = 1 , … , N \{w_i\}_{i=1,\ldots,N} { wi​}i=1,…,N​，来自点和法线的噪声(7)-(8)，以及通过叉乘$[{\hat{p}}_i{]}_{\times }{\hat{n}}_i$的环境尺度。与之前的方法[14]-[17],[20]不同，(20)中的$p_u$同时考虑了所有这些因素。

3.2.2. 退化感知ICP

概率退化感知ICP的目标是通过减少退化方向中虚假信息的影响来改进点到平面ICP[26]。我们将讨论限制在对标准点到平面ICP所做的具体更改上，并建议读者参考[23]以深入了解完整的ICP点云配准流程。类似于[14]，[15]，我们调整了ICP算法的更新步骤。

作为动机，我们重新审视了第二节中讨论的无噪声情况，并特别考虑了$H$的Moore-Penrose逆。使用对角化 $H=U\Lambda U^T$，其中 $U=[u_1,\cdots ,u_6]$ 是一个正交矩阵，使得 $U^{T}U=I$ 且 $\Lambda =\text{diag}({\lambda }_1,\cdots ,{\lambda }_6)$。$H$的Moore-Penrose逆是 $H^{†}=U{\Lambda }^{†}{U}^T$，其中 ${\Lambda }^{†}=\text{diag}({\lambda }_1^{†},\cdots ,{\lambda }_6^{†})$ 且

$${\lambda }_k^{†}=\{\begin{array}{ll}\frac{1}{{\lambda }_k},& {\lambda }_k>0\\ 0,& {\lambda }_k=0\end{array}.\text{\hspace{1em}(21)}$$

如果方向 $u_k$ 是退化的，那么 ${\lambda }_k$ 和 ${\lambda }_k^{†}$ 都是零。因此，使用(6)中的最小范数扰动 $x^{\star }$ 确保退化方向保持不变，即如果 ${\lambda }_k=0$，则 $u_k^T{x}^{\star }=0$。

为了考虑噪声，我们定义了一个修改后的伪逆。噪声Hessian的对角化为 $\hat{H}=\hat{U}\hat{\Lambda }{\hat{U}}^T$。当 $N\ge 6$ 时，噪声使 $H$ 的所有特征值都非零，并且(21)不再有用。相反，定义指示变量 I k ∈ { 1 , 0 } I_k \in \{1, 0\} Ik​∈{ 1,0}，如果方向 ${\hat{u}}_k$ 被分类为退化，则 $I_k=0$，否则为1。我们用以下公式替换(21)

$${\hat{\lambda }}_k^{‡}=I_k\frac{1}{{\hat{\lambda }}_k}+(1-I_k)\cdot 0.\text{\hspace{1em}(22)}$$

[14]，[15] 基于所有特征值的单一阈值设置退化分类指示变量 $I_k$。特别地，如果 ${\hat{\lambda }}_k>{\lambda }_{\min }$ 对于某个阈值 ${\lambda }_{\min }$，则 $I_k=1$。这相当于对 $H$ 进行截断奇异值分解[27]。在我们的情况下，我们使用第三节A中的退化检测来确定每个特征向量 ${\hat{u}}_k$ 的 $I_k$ 为零或一的概率。我们使用(20)中的概率来计算(22)关于 $I_k$ 的期望，以检索

$${\hat{\lambda }}_k^{+}=E_{I_k}[{\hat{\lambda }}_k^{‡}]=E_{I_k}[I_k\frac{1}{{\hat{\lambda }}_k}]=p_{{\hat{u}}_k}\frac{1}{{\hat{\lambda }}_k}.\text{\hspace{1em}(23)}$$

(23)以平滑的方式考虑退化，缓和了非退化和退化几何体之间的过渡。设 $J^T=[J_1^T\cdots J_N^T]$ 和 $b^T=[b_1\cdots b_N{]}^T$，让 $\hat{J}$ 和 $\hat{b}$ 分别是它们的噪声对应物。使用(23)，我们估计扰动为

$${\hat{x}}^{\star }=\hat{U}P{\hat{\Lambda }}^{-1}{\hat{U}}^T{\hat{J}}^T\hat{b},\text{\hspace{1em}(24)}$$

3.3 P2D-DO： 针对LiDAR SLAM的退化优化方法：基于点到分布检测因子

github
摘要：尽管激光雷达SLAM技术已经在各种机器人上得到了广泛应用，但在几何特征稀疏的场景中，由于约束不足，它仍然可能遭受退化问题。如果退化没有被检测到并得到适当处理，定位和地图绘制的准确性将显著降低。在这篇文章中，我们提出了P2d-DO方法，它由一个点到分布的退化检测算法和一个点云加权退化优化算法组成，以减轻退化的负面影响。退化检测算法通过观察局部区域内分布概率的变化来输出表征退化状态的因素。然后，反映点云置信度的因素被输入到退化优化算法中，使系统能够在匹配过程中通过分配更大的权重来优先考虑可靠的点云。全面的实验验证了我们方法的有效性，展示了在退化检测和姿态估计方面准确性和鲁棒性的显著提高。

在本文中，提出了一种 P2d-DO 方法，该方法由两个关键部分组成：基于点到分布（P2d）的退化检测算法，该算法利用局部几何特征增强退化检测能力；以及基于点云加权的退化优化（DO）算法，该算法在存在退化情况时提升位姿估计的精度。

为了确保局部几何特征满足高斯分布假设（这一假设对于精确的退化检测至关重要），P2d 算法中集成了一种自适应体素分割（AVS）算法。P2d 算法输出的退化因子（如图 1 所示）随后被用于 DO 算法，以减少低置信度点云的影响，从而提升 LiDAR SLAM 位姿估计的准确性和鲁棒性。

主要贡献如下：

• 提出了一种基于分布密度的自适应体素分割算法，以确保局部几何特征的质量。
• 提出了一种点到分布的退化检测算法，通过融合局部几何特征，有效降低噪声干扰并减少误检测。
• 提出了一种基于点云加权的退化优化算法，以缓解退化点云对 LiDAR SLAM 位姿估计的影响。

图 2 展示了所提出的 P2d-DO 方法在解决 LiDAR SLAM 退化问题中的整体框架。该方法由三个模块组成：AVS、P2d 和 DO，分别以蓝色、黄色和绿色表示。AVS 模块对点云进行离散化，并根据点云分布密度自适应调整体素大小。该过程通过优化每个体素内的空间分布，提高局部几何特征的准确性。P2d 模块利用提取的局部几何特征，分析相邻帧点云在局部区域内的概率分布变化。因此，P2d 模块输出退化因子，以量化点云的退化程度，为后续优化提供重要依据。DO 模块利用退化因子评估所有点云的置信水平，并对高退化度的点云赋予较低权重，以削弱其对整体位姿估计的影响，从而提高 LiDAR SLAM 位姿估计的准确性和鲁棒性。

3.3.1 P2d模块

退化场景通常具有结构对称性，导致从激光雷达点云中提取的几何特征几乎不变。基于这一观察，我们假设通过利用退化场景的固有特性可以提高退化检测的准确性。因此，我们提出的P2d模块将局部几何模型的变化与配准残差相结合。假设均值 $\mu$ 和协方差矩阵 $\Sigma$ 表示局部几何模型内点云的分布属性，那么在其对应位置的体素内每个点的分布概率可以计算为：

$$S_i(T_k)=\frac{1}{\sqrt{2\pi }\sqrt{|\Sigma |}}\exp \left(-\frac{(p_i^{\prime }-\mu {)}^T{\Sigma }^{-1}(p_i^{\prime }-\mu )}{2}\right),\text{\hspace{1em}(1)}$$

其中 $T_k$ 表示源点云和目标点在时间 $k$ 之间的变换增量，$p_i^{\prime }$ 表示第 $i$ 个变换后的点云。通过应用对数运算并省略常数项，(1) 可以转换为：

$$F_i(T_k)=e_i^T(T_k){\Sigma }^{-1}{e}_i(T_k),\text{\hspace{1em}(2)}$$

其中 $e_i(T_k)=p_i^{\prime }-\mu$ 且 $F_i(T_k)=-2\ln S_i(T_k)$。

为了模拟退化状态，向模型中引入了一个扰动 $\delta d=(d_x,d_y,d_z,d_R,d_P,d_Y)$。因此，考虑扰动 $\delta d$ 的分布概率变为：

$$F_i(T_k+\delta d)=-2\ln S_i(T_k+\delta d)$$
$$=e_i^T(T_k+\delta d){\Sigma }^{-1}{e}_i(T_k+\delta d).\text{\hspace{1em}(3)}$$

使用泰勒展开，(3) 可以转换为：

$$F_i(T_k+\delta d)\approx (e_i(T_k+\delta d)+J_i\delta d{)}^T{\Sigma }^{-1}(e_i(T_k+\delta d)+J_i\delta d)$$
$$=e_i^T{\Sigma }^{-1}{e}_i+2e_i^T{\Sigma }^{-1}{J}_i\delta d+\delta d^T{J}_i^T{\Sigma }^{-1}{J}_i\delta d,\text{\hspace{1em}(4)}$$

其中 $J_i$ 表示反映观测模型和位置之间相关性的雅可比矩阵。(4) 可以进一步简化为：

$$F_i(T_k+\delta d)=e_i^T{\Sigma }^{-1}{e}_i+2e_i^T{\Sigma }^{-1}{J}_i\delta d+\delta d^T{J}_i^T{\Sigma }^{-1}{J}_i\delta d.\text{\hspace{1em}(5)}$$
其中 $b_i^T=e_i^T{\Sigma }^{-1}{J}_i$ 且 $H_i=J_i^T{\Sigma }^{-1}{J}_i$。由雅可比矩阵构建的近似Hessian矩阵 $H_i$ 是对称的并且至少是半正定的。添加扰动后分布概率的变化可以计算为：

$$\begin{gathered} D_i=F_i(T_k+\delta d)-F_i(T_k) \\ =2\ln \frac{S_i(T_k)}{S_i(T_k+\delta d)}=2b_i^T\delta d+\delta d^T{H}_i\delta d.\text{\hspace{1em}(6)} \end{gathered}$$

根据公式(6)，较大的扰动会导致更大的不确定性，这会降低分布概率 $S_i(T+\delta d)$并相应地增加$D_i$。在本研究中，我们将退化因子定义为 $D_i$的最大值：

$$D_i^{\ast }=\underset{\delta d}{\max }(2b_i^T\delta d+\delta d^T{H}_i\delta d)\text{\hspace{1em}(7)}$$

其中 $2b_i^T\delta d$ 是关于$\delta d$ 的线性项，而 $\delta d^T{H}_i\delta d$是一个凸二次项，这在求解$D_i^{\ast }$ 时占主导地位。忽略(7)中的线性项得到：

$$\begin{array}{rl}{D}_i^{\ast }& =\underset{\delta d}{\max }(\delta d^T{H}_i\delta d)\\ & =\underset{\delta d}{\max }\frac{\delta d^T{H}_i\delta d}{\delta d^T\delta d}\delta d^T\delta d\end{array}\text{\hspace{1em}(8)}$$

根据Rayleigh商[30]，由于 $H_i$是一个对称矩阵，(8)中分数的最大值等于$H_i$的最大特征值。因此，退化因子可以计算为：

$$D_i^{\ast }=a_i{\lambda }_i^{\max }\text{\hspace{1em}(9)}$$

其中 $a_i=\delta d^T\delta d$表示常数项，${\lambda }_i^{\max }$表示 $H_i$的最大特征值。$a_i$可以从帧间点云的配准误差中计算出来。因此，第 $j$个特征值 ${\lambda }_i^j$ 决定了由$H_i$的相应特征向量$v_i^j$表示的方向上的退化程度，其中$j=1,2,\dots ,6$。

相应地，包含 $n$ 个点云的系统在时间$k$ 的退化状态可以定义为：

$$D(k)=\sum _{i=1}^n{D}_i^{\ast }\text{\hspace{1em}(10)}$$

我们可以通过检查$D(k)$来确定系统是否遇到退化问题。在本文中，我们预设了一个固定的阈值$D_{\text{thre}}$来检测系统退化。

3.3.2.DO模块

抑制退化状态下点云的功能应有利于减少激光雷达SLAM系统的姿态估计误差。调整与点云相关的权重提供了一种直接的方法。设$D_i^{\prime }$表示第$i$个点的退化置信度。可以通过对时间$k$的退化因子进行最大最小归一化来获得，例如：

$$D_i^{\prime }=\frac{D_i^{\ast }-D_{\min }}{D_{\max }-D_{\min }},\text{\hspace{1em}(11)}$$

其中$D_{\min }$和$D_{\max }$分别表示最小和最大的退化因子。归一化后，由于高退化置信度对应较小的权重，因此可以在时间$k$为每个点分配权重因子：

ω k = { 1 − D i ′ ∣ i = 1 , 2 , … , n } . (12) \omega_k=\{1-D_i^{\prime}|i=1,2,\ldots,n\}. \tag{12} ωk​={ 1−Di′​∣i=1,2,…,n}.(12)

集成到扩展卡尔曼滤波器[31]中，权重因子${\omega }_k$实时应用于观测模型。此操作通过两个步骤实现：预测和更新。

1. 预测：预测步骤涉及基于先前的状态估计$x_{k-1}$，在时间$k$估计系统状态$x_k$和相关的不确定性$P_k$。由于状态估计是非线性的，有必要使用一阶泰勒展开来线性化非线性方程，以预测位置信息，形式如下，

$$x_k=f(x_{k-1},u_k)\approx {\dot{x}}_k+J_{k-1}(x_{k-1}-{\hat{x}}_{k-1}),\text{\hspace{1em}(13)}$$

其中${\hat{x}}_{k-1}$和$x_k^{\ast }$分别表示先前帧的最优姿态和当前帧的预测姿态。$J_{k-1}$表示先前帧的状态方程的雅可比矩阵，$u_k$是机器人的加速度或转向角。

此外，预测下一姿态的不确定性对于更好地指导滤波器的更新方向至关重要。表示状态不确定性的误差协方差$P_k$，可以通过利用$J_{k-1}$推导如下：

$${\check{P}}_k=F_{k-1}{\hat{P}}_{k-1}{J}_{k-1}^T+Q_k,\text{\hspace{1em}(14)}$$

其中$F_{k-1}$描述了从时间步$k-1$到$k$的系统状态演变，$Q_k$是测量噪声的协方差矩阵。

2. 更新：使用时间$k$的激光雷达观测来细化状态估计和相关的不确定性。同样，点云的非线性观测方程使用泰勒展开进行线性化：

$$y_k=g(x_k,n_k)\approx {\omega }_k{\ddot{y}}_k+H_k(x_k-{\ddot{x}}_k),\text{\hspace{1em}(15)}$$

其中$n_k$表示观测噪声，${\check{y}}_k$表示从运动模型和先前地图匹配获得的预测观测，$H_k$表示观测的雅可比矩阵。

3.4 关于退化感知点云配准的分析

Informed, Constrained, Aligned: A Field Analysis on Degeneracy-aware Point Cloud Registration in the Wild

3.4.1 介绍

近十年来，ICP配准算法一直是基于激光雷达的机器人定位的优选方法。然而，即使在现代SLAM解决方案中，ICP也会在几何条件恶劣的环境中降级并变得不可靠。当前的解决方案主要集中于利用额外的信息源，例如外部里程计，来替换优化解决方案的退化方向，或者在之后的传感器融合设置中添加额外的约束。作为回应，这项工作调查和比较了新的和现有的用于鲁棒的基于激光雷达的定位的退化缓解方法，并在该尺度的文献中首次分析了这些方法在退化环境中的功效。具体而言，这项工作提出并研究了：

• I)将不同类型的约束合并到ICP算法中，
• ii)使用主动或被动退化缓解技术的效果，
• iii)在激光雷达退化环境中对病态ICP问题使用全局点云配准方法的选择。

研究结果通过多个真实现场和模拟实验得到了验证。分析表明，在缺乏可靠的外部估计辅助的情况下，主动优化退化抑制是必要和有利的。此外，在优化之前或期间在优化中引入退化感知的硬约束被示出在野生环境中比在之后包括约束执行得更好。此外，通过启发式微调参数，软约束可以在复杂的病态场景中提供相同或更好的结果。这项工作分析中使用的实现对社区公开。

图1：展示了ANYmal森林开阔场地退化实验（参见第5.3节）的映射结果。顶部：展示了由等式约束方法生成的地图和机器人轨迹，旁边是感兴趣区域的真实基准和图像。其余部分：展示了每种方法的点云误差图。颜色编码反映了与环境真实地图的点对点距离。

3.4.2 贡献

本工作通过各种模拟和现实世界实验（如图1所示）研究并比较了新的和现有的退化缓解方法，以调查在复杂退化环境中点云配准的退化缓解效果。除了研究结果外，本工作还介绍了三种不同的方法来约束优化问题，以实现稳健的点云配准。最后，将研究结果与最先进方法的局限性进行了对比，并通过多个机器人现场实验和模拟示例进行了验证。本工作的贡献如下：

• 对不同类型约束在退化点云配准优化问题中的有效性进行了深入研究，旨在使机器人在现实世界中的几何病态环境（如自然开阔地、城市隧道和复杂建筑工地）中操作。

• 在文献中首次实现了子空间Tikhonov正则化、截断奇异值分解和带不等式约束的ICP方法，并针对退化点云配准问题进行了调查和讨论。

• 通过模拟示例和使用配备不同LiDAR的足式机器人、步行挖掘机和手持设备进行的多项全面机器人现场实验，以公平和独立的方式对所讨论的方法进行了比较。

• 在单一框架中实现了所有研究方法的开源实现和整合，以促进未来对开发退化感知LiDAR-SLAM系统的研究。

3.4.2 雷达退化环境检测

在机器人被部署于几何特征不明显且挑战性环境中时，点云配准可能会遇到病态问题。为解决这一问题，需要检测优化过程中的退化方向。本工作侧重于退化缓解，采用X-ICP方法中描述的退化检测技术来确保所有需要可定位性信息的方法之间的公平比较。这一可定位性检测过程在图2中的可定位性检测块中展示。具体来说：

• 优化Hessian被分解为子矩阵，以识别与优化变量x的旋转部分r和翻译部分t相对应的特征向量。
• 通过特征值分解，$A_{tt}$ 和 $A_{rr}$ 子矩阵分别对应于平移和旋转信息，并用于退化检测。
• 如果特征向量位于$V_t$ 或 $V_r$的零空间内，则被标记为退化（不可定位）；否则为非退化（可定位）。

当机器人被部署在几何特征不明显且具有挑战性的环境中时，点云配准可能会变得病态。 因此，需要一种方法来检测优化的退化方向。由于这项工作侧重于退化缓解，这里讨论的所有工作都使用X-ICP [Tuna et al., 2023]中描述的退化检测方法，以确保公平比较（对于所有需要可定位性信息的方法）。**这种可定位性检测过程如图2中的可定位性检测块所示。根据[Tuna et al., 2023]的推导，优化Hessian被分为子矩阵，以识别对应于优化变量x的旋转部分r和翻译部分t的特征向量

$$A={\left[\begin{array}{cc}{A}_{rr}& A_{rt}\\ A_{tr}& A_{tt}\end{array}\right]}_{6\times 6}.$$

分解后的Hessian $A_{rr}\in {\mathbb{R}}^{3\times 3}$ 和 $A_{tt}\in {\mathbb{R}}^{3\times 3}$ 分别对应于旋转和翻译信息。此外，$A_{rr}$ 和 $A_{tt}$ 被用于退化检测，因为由于尺度和物理意义的差异，联合处理t和r并不简单。对于相应的子矩阵，特征值分解可以写为

$$A_{tt}=V_t{\Sigma }_t{V}_t^{\top },A_{rr}=V_r{\Sigma }_r{V}_r^{\top }.$$

矩阵 $V_t\in {\mathbb{R}}^{3\times 3}$ 和 $V_r\in {\mathbb{R}}^{3\times 3}$ 是矩阵形式的特征向量，而 Σ t ∈ { diag ( σ t t ) : σ t t ∈ R 3 ≥ 0 } \Sigma_{t} \in \{\text{diag}(\sigma_{tt}) : \sigma_{tt} \in \mathbb{R}^{3} \geq 0\} Σt​∈{ diag(σtt​):σtt​∈R3≥0} 和 Σ r ∈ { diag ( σ r r ) : σ r r ∈ R 3 ≥ 0 } \Sigma_{r} \in \{\text{diag}(\sigma_{rr}) : \sigma_{rr} \in \mathbb{R}^{3} \geq 0\} Σr​∈{ diag(σrr​):σrr​∈R3≥0} 是 $A_{tt}$ 和 $A_{rr}$ 的缩放矩阵。这里，${\sigma }_{tt}$ 和 ${\sigma }_{rr}$ 是 $A_{tt}$ 和 $A_{rr}$ 的特征值。在工作的其余部分中，假设通过这种分析结合 ${\Sigma }_t$ 和 ${\Sigma }_r$ 中的相应特征值，已知 $V_t$ 和 $V_r$ 的零空间近似。如果一个特征向量 v j ∈ { V t , V r } v_{j} \in \{V_{t}, V_{r}\} vj​∈{ Vt​,Vr​} 在 $V_t$ 或 $V_r$ 的零空间内，则它被标记为退化（不可定位），否则为非退化（可定位）。此外，考虑到第4节中描述的一组方法期望在 ${\mathbb{R}}^{3\times 3}$ 中的退化特征向量，而其他方法期望它们在 ${\mathbb{R}}^{6\times 6}$ 中。此外，分别检测平移和旋转退化允许仔细考虑Hessian A中条目的尺度差异。由于这种考虑，省略了交叉项 $A_{tr}$ 和 $A_{rt}$。然而，由于所有需要退化检测信息的方法都可以访问相同的退化信息，这种省略的影响得以缓解。