经典文献阅读之--LP-ICP(退化环境下通用、精确、鲁棒的点云配准)

0. 简介

迭代最近点算法是激光雷达SLAM算法的重要组成部分。然而，在缺乏特征和几何结构的非结构化环境中，它的性能会受到负面影响，导致定位和映射的准确性低和鲁棒性差。众所周知，缺乏几何约束导致的退化会导致沿病态方向的6自由度姿态估计的误差。因此，需要更广泛和更细粒度的退化检测和处理方法。《LP-ICP: General Localizability-Aware Point Cloud Registration for Robust Localization in Extreme Unstructured Environments》提出了一种新的点云配准框架LP-ICP，它结合了ICP算法中点到线和点到平面的距离度量，以及可定位性检测和处理。LP-ICP由可定位性检测模块和优化模块组成。可定位性检测模块通过利用边缘点(具有低局部平滑度)与扫描和地图之间的线以及平面点(具有高局部平滑度)与平面之间的对应性来执行可定位性分析。个体对应约束的可定位性贡献可以应用于更广的范围。优化模块基于可本地化类别向优化方程添加额外的软和硬约束。这允许姿态沿着病态方向被约束，更新趋向于约束值或者保持初始估计不变。这提高了精确度并减少了波动。通过在模拟和真实数据集上的实验，对所提出的方法进行了广泛的评估，证明了比最先进的方法更高或相当的准确性。相关代码将会在Github上开源。

1. 主要贡献

所提出的框架已在多个实验中针对具有挑战性的仿真和真实世界数据集进行了测试。实验结果表明，LP-ICP在极端非结构化环境中表现良好，并能够提高机器人在退化环境中的定位能力和地图精度。与最先进的方法相比[20]，[21]，所提出的框架在所有实验中均实现了更高或可比的精度。总结而言，本工作的贡献如下：

• 我们提出了一种新的ICP算法框架，具有可定位性检测和处理功能，利用点到线和点到面的可定位性信息。该框架用于检测和处理极端非结构化环境中的退化现象。

• 我们提出了一种通用方法，用于计算单个对应点的可定位性贡献。该方法可用于使用高斯-牛顿法优化的姿态估计问题。

• 开发了一种结合软约束和硬约束的ICP优化模块，利用可定位性信息来处理退化现象。

• 我们从一个视觉上逼真的类月球环境中收集了一个多传感器仿真SLAM数据集，用于在典型的极端退化环境中进行SLAM测试。

• 与最先进的方法相比，各种实验验证了所提出框架的有效性。

2. 问题表述、预备知识与系统概述

本节介绍点云配准和退化检测的预备知识，以及我们旨在解决的问题。最后，提供LP-ICP系统框架的概述。

2.1 基于优化的点云配准

可以通过求解以下函数来表述一个基于优化的状态估计问题：

$${\text{argmin}}_x{f}^2(x).\text{\hspace{1em}(1)}$$

这里，$x$是一个$n\times 1$的状态向量，其中$n$是状态空间的维度。

在点云配准中，最优解可以通过迭代求解源点云和目标点云之间的姿态变换，最小化一个代价函数来获得。令$f_i(x)$表示源点云和目标点云之间每个对应点的残差。该问题等价于求解以下问题：

$${\text{arg min}}_{x\in {\mathbb{R}}^6}\sum _{i=1}^N\Vert f_i(x){\Vert }^2.\text{\hspace{1em}(2)}$$

这里，$x=(r^T,t^T{)}^T\in {\mathbb{R}}^6$。$x$是姿态状态向量，其中$r$表示3自由度的旋转表示，$t$表示3自由度的平移向量，$N$表示匹配对应点的数量。

目前，结合点到线和点到面距离度量的ICP算法仍然是首选之一。它已被应用于许多先进的SLAM系统，如[2]、[10]、[11]、[27]。为了平衡效率、精度和鲁棒性，通常会提取代表性的几何特征点进行优化。例如，边缘点与点到线代价函数一起使用，而平面点则与点到面代价函数一起使用，这两种类型的点结合参与ICP优化[2]。

结合点到线和点到面的ICP最小化问题定义如下：

$$\underset{R,t}{\min }\sum _{i=1}^{N_1}\Vert f_{ei}(x){\Vert }^2+\sum _{j=1}^{N_2}\Vert f_{pj}(x){\Vert }^2\text{\hspace{1em}(3)}$$

其中，

$$f_{ei}(x)=\Vert (R{p}_i^L+t-q_i^M)\times l_i^M\Vert \text{\hspace{1em}(4)}$$

$$f_{pj}(x)=\Vert (R{p}_j^L+t-q_j^M)\cdot n_j^M\Vert \text{\hspace{1em}(5)}$$

在这里，$f_{ei}(x)$是边缘点到线的残差，而$f_{pj}(x)$是平面点到面的残差。$N_1$是边缘点的数量，$N_2$是平面点的数量，点的总数为$N=N_1+N_2$。

具体而言，当前扫描点云$p^L$在激光雷达坐标系（记作$L$）中与地图点云$q^M$在地图坐标系（记作$M$）中的对齐是通过变换$T=[R|t]$（或$x=(r^T,t^T{)}^T\in {\mathbb{R}}^6$）实现的。这里，$R\in SO(3)$表示旋转矩阵，$t\in {\mathbb{R}}^3$表示平移向量。$p_i^L$和$p_j^L$分别是从激光雷达坐标系中提取的边缘点和平面点。$q_i^M$和$q_j^M$是地图坐标系中对应的匹配点，通常通过搜索方法（如k-d树搜索）找到。$l_j^M$是通过$q_i^M$的边缘线的单位方向向量，而$n_j^M$是通过$q_j^M$的平面的单位法向量。该问题可以使用高斯-牛顿法迭代求解。

2.2 退化检测

在退化环境中，点云配准的性能会下降。因此，有必要检测和处理退化现象。Zhang等人[20]首次提出使用退化因子$D$来评估6自由度姿态解$x$在某一方向上的退化程度。$D$的含义如下：通过以距离$\delta d$扰动约束，得到的真实解$x_0$在扰动方向$c$上的位移$\delta x_c$为：$D=\frac{\delta d}{\delta x_c}.$ 换句话说，$D$表示在约束扰动下解的刚度。如果$D$较小，意味着在相同的扰动$\delta d$下，解在该方向上的位移$\delta x_c$较大。这表明在该方向上对扰动的刚度较低。通过[20]中的数学推导，得出结论：

$$D=\frac{\delta d}{\delta x_c}=\lambda +1.\text{\hspace{1em}(6)}$$

这里，$\lambda$是矩阵$A^{T}A$的特征值，其中$A$来自以下方程（即（1）的线性化形式）：

$${\text{argmin}}_x\Vert Ax-b\Vert .\text{\hspace{1em}(7)}$$

高斯-牛顿法是解决优化问题的常用方法。在使用高斯-牛顿法解决的优化问题中，$\lambda$是原始优化问题的海森矩阵对应的特征值。$\lambda$可以用来评估特定方向上的优化是否存在退化。然而，它无法评估该方向上单个对应约束的可定位性贡献。因此，几何信息无法在优化过程中更有效地利用，以提高定位精度。因此，获得更细粒度的可定位性检测结果是本研究的目标之一。

2.3 系统综述

LP-ICP分为两个模块：可定位性检测模块（LocDetect-Module）和带有软约束和硬约束的优化模块（Optimization-Module）。所提出的框架如图2所示。LP-ICP集成到LVISAM[11]的点云配准框架中进行评估和验证。LVI-SAM集成了LIO（激光雷达-惯性测量）子模块和VIO（视觉-惯性测量）子模块。LIO子模块是主要组件，而VIO子模块为LIO子模块提供初始估计。需要注意的是，在LVISAM中，姿态中的3自由度旋转使用欧拉角表示。

图2. 所提出的点云配准框架概述。当前来自激光雷达坐标系的扫描通过初始估计$x_0$转换为地图坐标系，并与现有地图一起进入ICP循环。接下来，LP-ICP将点到线和点到面的距离度量结合到ICP框架中，应用所提出的可定位性检测和优化模块，以估计最优姿态。

LocDetect-Module首先推导出计算单个对应约束的可定位性贡献的公式。需要注意的是，在ICP算法中，我们将用于约束姿态估计的对应关系（如点到线或点到面的对应关系）称为单个对应约束。这种方法不仅适用于点到线和点到面的ICP算法，还可以扩展到使用高斯-牛顿法优化的姿态估计问题，例如ICP的变体或通过优化重投影误差进行的姿态估计。这对应于第3节。然后，计算点到线度量中每个边缘点$p_i$的可定位性贡献向量：$F_{e_r}^i\in {\mathbb{R}}^{3\times 1}$（旋转方向）和$F_{e_t}^i\in {\mathbb{R}}^{3\times 1}$（平移方向）。类似地，计算点到面度量中每个平面点$p_j$的可定位性贡献向量：$F_{p_r}^j\in {\mathbb{R}}^{3\times 1}$（旋转方向）和$F_{p_t}^j\in {\mathbb{R}}^{3\times 1}$（平移方向）。在6自由度姿态的每个方向上的可定位性向量被聚合，以评估相应方向上的可定位性。然后，它们与阈值进行比较，将可定位性分为三种类型：不可定位（None）、部分可定位（Partial）和完全可定位（Full）。这对应于第4节和第5节。

优化模块（第6节）利用LocDetect-Module对6自由度姿态每个方向的分类结果，以及更高的可定位性贡献。它构建优化方程以求解最优姿态估计$x^{\ast }$。我们使用结合软约束和硬约束的方法。在不可定位方向上施加硬约束，以防止在这些方向上的姿态更新。在部分可定位方向上添加软约束，允许在约束下更新姿态，姿态趋向于约束值而不是严格等于它。这减少了大幅度姿态波动或漂移的风险，否则可能导致定位失败。在完全可定位方向上的姿态更新不受影响。

3. 单个对应约束的可定位性

已知退化因子$D$可用于评估6自由度姿态在各个方向上的退化程度，并适用于一般的姿态估计问题。如果使用高斯-牛顿法解决公式(2)中的姿态估计问题，则公式(6)中的$\lambda$也是原始优化问题(2)的海森矩阵对应的特征值。海森矩阵如下所示：

$$H=J^{T}J=\sum _{i=1}^N{J}_i^T{J}_i\in {\mathbb{R}}^{6\times 6}\text{\hspace{1em}(8)}$$

其中$J_i$是关于姿态$x=(r^T,t^T{)}^T\in {\mathbb{R}}^6$的函数$f_i(x)$的雅可比矩阵，即：$J_i=\frac{\partial f_i(x)}{\partial x}=\left(\frac{\partial f_i(x)}{\partial x_1},\dots ,\frac{\partial f_i(x)}{\partial x_6}\right)\in {\mathbb{R}}^{1\times 6}$因此，对于$N$个对应约束，我们有：$J=\left(\begin{array}{ccc}{J}_1^T& \dots & J_N^T\end{array}\right)\in {\mathbb{R}}^{N\times 6}$

由于$H$也是一个对称矩阵，它可以通过特征值分解对角化，如下所示：

$$H=V\Lambda V^T$$

这里，$V=(v_1,\dots ,v_6)$是特征向量的矩阵形式，$v_i\in {\mathbb{R}}^{6\times 1}$是对应于对角矩阵$\Lambda$中特征值${\lambda }_i$的特征向量。因此，我们得到：

$$\Lambda =V^{T}HV=V^T{J}^{T}J=(JV{)}^{T}JV$$

这里，$JV$的计算公式如下：

$$JV=\left(\begin{array}{ccc}{J}_1{v}_1& \cdots & J_1{v}_6\\ ⋮& \ddots & ⋮\\ J_N{v}_1& \cdots & J_N{v}_6\end{array}\right)\in {\mathbb{R}}^{N\times 6}$$

其中$J_i{v}_j$表示$J_i$在特征向量$v_j$方向上的投影。由于$\Lambda$是一个对角矩阵，我们有：

$$\Lambda =(JV{)}^{T}JV=\left(\begin{array}{ccc}{\sum }_{i=1}^N(J_i{v}_1{)}^2& 0& 0\\ 0& \ddots & 0\\ 0& 0& {\sum }_{i=1}^N(J_i{v}_6{)}^2\end{array}\right)\in {\mathbb{R}}^{6\times 6}=\left(\begin{array}{ccc}{\lambda }_1& 0& 0\\ 0& \ddots & 0\\ 0& 0& {\lambda }_6\end{array}\right)\text{\hspace{1em}(9)}$$

因此，我们得到：

$${\lambda }_j=\sum _{i=1}^N(J_i{v}_j{)}^2\text{\hspace{1em}(10)}$$

也就是说，${\lambda }_j$等于$J_i$在特征向量$v_j$方向上的平方投影之和，其中$J_i$是关于姿态$x$的第$i$个残差$f_i(x)$的雅可比。

根据公式(6)，${\lambda }_j$可以与一个阈值进行比较，以确定公式(2)中的状态估计问题在$v_j$方向上是否退化。${\lambda }_j$可用于评估$v_j$方向上的退化程度。这相当于使用公式(10)与阈值进行比较，以评估$v_j$方向上的退化程度。换句话说，它评估了在$v_j$方向上$N$个对应约束的可定位性。

我们取求和公式(10)中的一个项，并定义$F_{ji}$为单个对应约束的可定位性贡献，如下所示：

$$F_{ji}=(J_i{v}_j{)}^2\text{\hspace{1em}(11)}$$

它用于评估第$i$个对应约束在$v_j$方向上的可定位性贡献。因此，我们有：

$${\lambda }_j=\sum _{i=1}^N{F}_{ji}\text{\hspace{1em}(12)}$$

请注意，我们提供了两个与公式(11)和(12)类似的方程，如下所示：

$$L_j=\sum _{i=1}^N|J_i{v}_j|\text{\hspace{1em}(13)}$$

$$F_{ji}=|J_i{v}_j|\text{\hspace{1em}(14)}$$

我们认为(11)和(14)，以及(12)和(13)在本质上是相似的，但在数值上是不同的。当(13)和(14)用于点到面ICP算法时，并且旋转使用李代数表示时，它们变得与X-ICP的可定位性评估公式相同[21]。换句话说，[21]的可定位性检测公式是本研究中推导结论的特例。为了方便后续的计算和使用，我们选择在上述推导中解耦平移和旋转。也就是说，对于$N$个对应约束，我们有：$J_r = \begin{pmatrix} J_{r1}^T & \dots & J_{rN}^T \end{pmatrix} \in \mathbb{R}^{N \times 3} \quad $(在旋转方向上）$J_t = \begin{pmatrix} J_{t1}^T & \dots & J_{tN}^T \end{pmatrix} \in \mathbb{R}^{N \times 3} \quad$（在平移方向上）。其中$J_{ri} \in \mathbb{R}^{1 \times 3}$是关于3自由度旋转的第$i$个残差$f_i(x)$的雅可比，$J_{ti} \in \mathbb{R}^{1 \times 3}$是关于3自由度平移的第$i$个残差$f_i(x)$的雅可比。也就是说，相当于对海森矩阵$H$的左上角$3\times 3$矩阵$H_r$和右下角$3\times 3$矩阵$H_t$进行特征值分解，如下所示：

$$H_r=J_r^T{J}_r=V_r{\Lambda }_r{V}_r^T\text{\hspace{1em}(15)}$$

$$H_t=J_t^T{J}_t=V_t{\Lambda }_t{V}_t^T\text{\hspace{1em}(16)}$$

在旋转方向上，我们有$V_r=(v_{r1},v_{r2},v_{r3})$，其中$v_{rj}\in {\mathbb{R}}^{3\times 1}$是对应于对角矩阵${\Lambda }_r$中特征值${\lambda }_j$的特征向量。在平移方向上，我们有$V_t=(v_{t1},v_{t2},v_{t3})$，其中$v_{tj}\in {\mathbb{R}}^{3\times 1}$是对应于对角矩阵${\Lambda }_t$中特征值${\lambda }_j$的特征向量。类似地，我们可以推导出与公式(11)和(12)类似的结论。在旋转方向上，我们得到：

$${\lambda }_j=\sum _{i=1}^N(J_{r_i}{v}_{rj}{)}^2\text{\hspace{1em}(17)}$$

其中 $j=1,2,3$。也就是说，${\lambda }_j$等于$J_{r_i}$在与旋转相关的特征向量$v_{r_j}$方向上的平方投影之和。第$i$个对应约束在旋转特征空间的三个方向$v_{r1}$、$v_{r2}$和$v_{r3}$的可定位性贡献向量如下：

$$F_{ri}=\left(\begin{array}{c}(J_{r_i}{v}_{r1}{)}^2\\ (J_{r_i}{v}_{r2}{)}^2\\ (J_{r_i}{v}_{r3}{)}^2\end{array}\right)\in {\mathbb{R}}^{3\times 1}$$

类似地，在平移方向上，我们有：

$${\lambda }_j=\sum _{i=1}^N(J_{t_i}{v}_{t_j}{)}^2\text{\hspace{1em}(18)}$$

其中$j=1,2,3$

第$i$个对应约束在平移特征空间的三个方向$v_{t1}$、$v_{t2}$和$v_{t3}$的可定位性贡献向量如下：

$$F_{ti}=\left(\begin{array}{c}(J_{t_i}{v}_{t1}{)}^2\\ (J_{t_i}{v}_{t2}{)}^2\\ (J_{t_i}{v}_{t3}{)}^2\end{array}\right)\in {\mathbb{R}}^{3\times 1}\text{\hspace{1em}(20)}$$

公式(18)和(20)用于评估第$i$个对应约束在6自由度姿态的每个方向上的可定位性贡献。后续计算将使用(18)和(20)。

4. 边缘点到直线和面点到平面对应关系的可定位性贡献

在这里，我们分别计算每个边缘点到直线对应约束和每个平面点到平面对应约束的可定位性贡献。单个对应约束的可定位性贡献向量可以使用(18)和(20)进行计算。首先，我们计算雅可比矩阵，然后将其投影到特征空间，以获得每个对应约束的可定位性贡献向量。请注意，对于3自由度旋转，我们将提供两个版本的雅可比矩阵以供参考：一个是使用李代数计算的，另一个是使用欧拉角计算的。

我们将LPICP集成到LVISAM [11] 的点云配准框架中。由于LVI-SAM算法中的旋转使用欧拉角表示，因此我们将在后续计算中使用基于欧拉角的雅可比矩阵。

4.1 点到直线代价函数的雅可比矩阵：

在点到直线代价函数(4)中，边缘点$p_i^L=(p_{ix},p_{iy},p_{iz}{)}^T\in {\mathbb{R}}^{3\times 1}$的距离残差$f_{ei}$关于平移$t$的雅可比矩阵计算如下：

$$J_{ti}^e=(d_i^M{)}^T\text{\hspace{1em}(21)}$$

这里，$d_i^M=(d_{ix},d_{iy},d_{iz}{)}^T\in {\mathbb{R}}^{3\times 1}$是从边缘点$p_i^M$到相关直线的单位距离向量。

边缘点$p_i^L$的距离残差$f_{ei}(x)$关于旋转$r$的雅可比矩阵在旋转使用李代数表示时计算如下：

$$J_{ri}^e=(p_i^L\times d_i^L{)}^T\text{\hspace{1em}(22)}$$

当旋转使用欧拉角$(\alpha ,\beta ,\gamma )$表示时，计算如下：

$$J_{ri}^e=\left(\begin{array}{c}{J}_{rix}^e,J_{riy}^e,J_{riz}^e\end{array}\right)\text{\hspace{1em}(23)}$$

其中，(23) 中的 $J_{r_{ix}}^e$ 表示为：

$$J_{r_{ix}}^e=\left[(s_{\alpha }{s}_{\gamma }+c_{\alpha }{s}_{\beta }{c}_{\gamma })p_{iy}+(s_{\alpha }{c}_{\gamma }-c_{\alpha }{s}_{\beta }{s}_{\gamma })p_{iz}\right]\times d_{ix}^M+\left[(-c_{\alpha }{s}_{\gamma }+s_{\alpha }{s}_{\beta }{c}_{\gamma })p_{iy}+(-c_{\alpha }{c}_{\gamma }-s_{\alpha }{s}_{\beta }{s}_{\gamma })p_{iz}\right]\times d_{iy}^M+\left[c_{\beta }{c}_{\gamma }{p}_{iy}-c_{\beta }{s}_{\gamma }{p}_{iz}\right]\times d_{iz}^M$$

$J_{r_{iy}}^e$ 表示为：

$$J_{r_{iy}}^e=\left[-c_{\alpha }{s}_{\beta }{p}_{ix}+(c_{\alpha }{c}_{\beta }{s}_{\gamma })p_{iy}+(c_{\alpha }{c}_{\beta }{c}_{\gamma })p_{iz}\right]\times d_{ix}^M+\left[-s_{\alpha }{s}_{\beta }{p}_{ix}+(s_{\alpha }{c}_{\beta }{s}_{\gamma })p_{iy}+(s_{\alpha }{c}_{\beta }{c}_{\gamma })p_{iz}\right]\times d_{iy}^M+\left[-c_{\beta }{p}_{ix}-s_{\beta }{s}_{\gamma }{p}_{iy}-s_{\beta }{c}_{\gamma }{p}_{iz}\right]\times d_{iz}^M$$

$J_{r_{iz}}^e$ 表示为：

$$J_{r_{iz}}^e=\left[-s_{\alpha }{c}_{\beta }{p}_{ix}+(-c_{\alpha }{c}_{\gamma }-s_{\alpha }{s}_{\beta }{s}_{\gamma })p_{iy}+(c_{\alpha }{s}_{\gamma }-s_{\alpha }{s}_{\beta }{c}_{\gamma })p_{iz}\right]\times d_{ix}^M+\left[c_{\alpha }{c}_{\beta }{p}_{ix}+(-s_{\alpha }{c}_{\gamma }+c_{\alpha }{s}_{\beta }{s}_{\gamma })p_{iy}+(s_{\alpha }{s}_{\gamma }+c_{\alpha }{s}_{\beta }{c}_{\gamma })p_{iz}\right]\times d_{iy}^M$$

这里，$s_{\alpha }=\sin (\alpha )$，$c_{\alpha }=\cos (\alpha )$，其他变量同理。本文后续对 $J_{t_i}^e$ 和 $J_{r_i}^e$ 的计算将分别使用 (21) 和 (23)。

4.2 点到平面代价函数的雅可比

同样，在点到平面代价函数 (5) 中，距离残差 $f_{pi}(x)$ 关于平面点 $p_i^L=(p_{ix},p_{iy},p_{iz}{)}^T\in {\mathbb{R}}^{3\times 1}$ 的平移 $t$ 的雅可比计算如下：

$$J{p}_{t_i}=(n_i^M{)}^T\text{\hspace{1em}(24)}$$

这里，$n_i^M=(n_{ix},n_{iy},n_{iz}{)}^T\in {\mathbb{R}}^{3\times 1}$ 是与平面点 $p_i^M$ 相关的平面的单位法向量。关于平面点 $p_{L_i}$ 的距离残差 $f_{pi}(x)$ 关于旋转 $r$ 的雅可比在旋转用李代数表示时计算如下：

$$J_{r_i}^p=(p_i^L\times n_i^L{)}^T\text{\hspace{1em}(25)}$$

请翻译如下内容：

在这里，$n_i^M=(n_{ix},n_{iy},n_{iz}{)}^T\in {\mathbb{R}}^{3\times 1}$ 是与平面点 $p_i^M$ 相关的平面的单位法向量。关于平面点 $p_{L_i}$ 的距离残差 $f_{pi}(x)$ 关于旋转 $r$ 的雅可比在旋转用李代数表示时计算如下：

$$J_{r_i}^p=(p_i^L\times n_i^L{)}^T\text{\hspace{1em}(25)}$$

当旋转用欧拉角（$\alpha ,\beta ,\gamma$）表示时，计算如下：

$$J_{r_i}^p=\left[\begin{array}{c}{J}_{r_{ix}}^p,J_{r_{iy}}^p,J_{r_{iz}}^p\end{array}\right]\text{\hspace{1em}(26)}$$

其中，

$$J_{r_{ix}}^p=\left[(s_{\alpha }{s}_{\gamma }+c_{\alpha }{s}_{\beta }{c}_{\gamma })p_{iy}+(s_{\alpha }{c}_{\gamma }-c_{\alpha }{s}_{\beta }{s}_{\gamma })p_{iz}\right]\times n_{ix}^M+\left[(-c_{\alpha }{s}_{\gamma }+s_{\alpha }{s}_{\beta }{c}_{\gamma })p_{iy}+(-c_{\alpha }{c}_{\gamma }-s_{\alpha }{s}_{\beta }{s}_{\gamma })p_{iz}\right]\times n_{iy}^M+\left[c_{\beta }{c}_{\gamma }{p}_{iy}-c_{\beta }{s}_{\gamma }{p}_{iz}\right]\times n_{iz}^M,$$

$$J_{r_{iy}}^p=\left[-c_{\alpha }{s}_{\beta }{p}_{ix}+(c_{\alpha }{c}_{\beta }{s}_{\gamma })p_{iy}+(c_{\alpha }{c}_{\beta }{c}_{\gamma })p_{iz}\right]\times n_{ix}^M+\left[-s_{\alpha }{s}_{\beta }{p}_{ix}+(s_{\alpha }{c}_{\beta }{s}_{\gamma })p_{iy}+(s_{\alpha }{c}_{\beta }{c}_{\gamma })p_{iz}\right]\times n_{iy}^M+\left[-c_{\beta }{p}_{ix}-s_{\beta }{s}_{\gamma }{p}_{iy}-s_{\beta }{c}_{\gamma }{p}_{iz}\right]\times n_{iz}^M,$$

$$J_{r_{iz}}^p=\left[-s_{\alpha }{c}_{\beta }{p}_{ix}+(-c_{\alpha }{c}_{\gamma }-s_{\alpha }{s}_{\beta }{s}_{\gamma })p_{iy}+(c_{\alpha }{s}_{\gamma }-s_{\alpha }{s}_{\beta }{c}_{\gamma })p_{iz}\right]\times n_{ix}^M+\left[c_{\alpha }{c}_{\beta }{p}_{ix}+(-s_{\alpha }{c}_{\gamma }+c_{\alpha }{s}_{\beta }{s}_{\gamma })p_{iy}+(s_{\alpha }{s}_{\gamma }+c_{\alpha }{s}_{\beta }{c}_{\gamma })p_{iz}\right]\times n_{iy}^M.$$

在本文中，后续的 $J_{t_i}^p$ 和 $J_{r_i}^p$ 计算将分别使用（24）和（26）。

4.3 可定位性贡献向量

旋转方向的雅可比受点 $p$ 坐标大小的影响，通常会导致较大的值。为了便于后续设置平移和旋转方向的统一阈值，这两个方向的可定位性尺度被统一。在这里，我们采用文献[21]中的矩阵归一化方法来归一化旋转方向的结果。如果 $\Vert J_{r_i}^e\Vert$ 在（23）中或 $\Vert J_{r_i}^p\Vert$ 在（26）中大于1，则操作如下：

$${\mathbf{J}}_{r_i}^e=\{\begin{array}{ll}\frac{{\mathbf{J}}_{r_i}^e}{\Vert {\mathbf{J}}_{r_i}^e\Vert },& \text{if }\Vert {\mathbf{J}}_{r_i}^e\Vert >1\\ {\mathbf{J}}_{r_i}^e,& \text{otherwise}\end{array}$$

$${\mathbf{J}}_{p_i}^e=\{\begin{array}{ll}\frac{{\mathbf{J}}_{p_i}^e}{\Vert {\mathbf{J}}_{p_i}^e\Vert },& \text{if }\Vert {\mathbf{J}}_{p_i}^e\Vert >1\\ {\mathbf{J}}_{p_i}^e,& \text{otherwise}\end{array}$$

最后，基于（18）和（20），结合（15）、（16）、（21）和（23），可以得到边缘点 $p_i$ 对线对应约束的可定位性贡献向量如下：

$$F_{r_i}^e=\left(\begin{array}{c}(J_{r_i}^e{v}_{r1}{)}^2\\ (J_{r_i}^e{v}_{r2}{)}^2\\ (J_{r_i}^e{v}_{r3}{)}^2\end{array}\right)\in {\mathbb{R}}^{3\times 1}\text{\hspace{1em}(27)}$$

$$F_{t_i}^e=\left(\begin{array}{c}(J_{t_i}^e{v}_{t1}{)}^2\\ (J_{t_i}^e{v}_{t2}{)}^2\\ (J_{t_i}^e{v}_{t3}{)}^2\end{array}\right)\in {\mathbb{R}}^{3\times 1}\text{\hspace{1em}(28)}$$

同样，基于（18）和（20），结合（15）、（16）、（24）和（26），可以得到平面点 $p_j$ 对平面对应约束的可定位性贡献向量如下：

$$F_{r_i}^p=\left(\begin{array}{c}(J_{r_i}^p{v}_{r1}{)}^2\\ (J_{r_i}^p{v}_{r2}{)}^2\\ (J_{r_i}^p{v}_{r3}{)}^2\end{array}\right)\in {\mathbb{R}}^{3\times 1}\text{\hspace{1em}(29)}$$

$$F_{t_i}^p=\left(\begin{array}{c}(J_{t_i}^p{v}_{t1}{)}^2\\ (J_{t_i}^p{v}_{t2}{)}^2\\ (J_{t_i}^p{v}_{t3}{)}^2\end{array}\right)\in {\mathbb{R}}^{3\times 1}\text{\hspace{1em}(30)}$$

所有与边缘点和平面点相关的可定位性贡献向量被堆叠在一起，形成以下信息矩阵：

$$F^r={\left(\begin{array}{cc}{F}_{r1}^e\cdots F_{r{N}_1}^e& F_{r1}^p\cdots F_{r{N}_2}^p\end{array}\right)}^T\text{\hspace{1em}(31)}$$

$$F^t={\left(\begin{array}{cc}{F}_{t1}^e\cdots F_{t{N}_1}^e& F_{t1}^p\cdots F_{t{N}_2}^p\end{array}\right)}^T\text{\hspace{1em}(32)}$$

5. 确定可定位性类别

所有与边缘点和平面点相关的可定位性贡献向量已经获得。接下来，这些向量将用于确定每个方向的6自由度姿态的可定位性类别。首先，为了减少噪声的影响，将过滤掉低可定位性贡献。同时，将选择较高的可定位性贡献用于后续的约束计算。此外，6自由度姿态在每个方向的可定位性将被求和，并与阈值进行比较，以确定每个方向的相应可定位性类别。

5.1 过滤噪声并挑选高可定位性贡献

定义 $F=[F^r,F^t]\in {\mathbb{R}}^{N\times 6}$ 为可定位性矩阵。为了过滤掉低可定位性贡献并减少噪声的干扰，我们设定低贡献阈值 $h_f$。对于 $F$ 中的每个可定位性分量 $F(i,j)$（即第 $i$ 个对应约束在方向 $v_j$ 上的可定位性贡献），如果其低于 $h_f$，则将其设为0，并视为噪声，如（33）所示。然后，我们进一步区分中等可定位性贡献 $F_f$ 和高可定位性贡献 $F_u$。

$$F_f(i,j)=\{\begin{array}{ll}F(i,j),& \text{if }F(i,j)\ge h_f\\ 0,& \text{otherwise}\end{array}\text{\hspace{1em}(33)}$$

本文经验性地建议 $h_f=0.03$。$h_f$ 的值可以根据激光雷达的噪声特性进行调整。噪声越大，值越大。此外，$F_f$ 中的非零可定位性贡献可以视为中等可定位性贡献。在方向 $v_j$ 上所有中等可定位性贡献的总和计算如下：

$$L_f(j)=\sum _{i=1}^{N_1+N_2}{F}_f(i,j)\text{\hspace{1em}(34)}$$

$L_f(j)$ 是在方向 $v_j$ 上所有有用对应约束的可定位性贡献之和。其值越大，表示该方向上的整体可定位性贡献越强，表明退化程度较低。接下来，我们设定高贡献阈值 $h_u$。$F_f(i,j)$ 中大于 $h_u$ 的可定位性贡献被视为高可定位性贡献，表明相应的对应约束在方向 $v_j$ 上具有强影响力，如下所示：

$$F_u(i,j)=\{\begin{array}{ll}F(i,j),& \text{if }F(i,j)\ge h_u\\ 0,& \text{otherwise}\end{array}\text{\hspace{1em}(35)}$$

本文经验性地建议 $h_u=0.4998$。在方向 $v_j$ 上所有高可定位性贡献的总和计算如下：

$$L_u(j)=\sum _{i=1}^{N_1+N_2}{F}_u(i,j)\text{\hspace{1em}(36)}$$

$L_u(j)$ 是在方向 $v_j$ 上所有高可定位性点的可定位性贡献之和。其值越大，表明在该方向上可以提供更多高质量的约束。图3提供了 $L_f(j)$ 和 $L_u(j)$ 的计算结果示例。这些值是从在稍微起伏的开阔地带进行的激光雷达扫描的可定位性贡献中计算得出的。在沿 $v_{r1}$、$v_{r2}$ 和 $v_{t1}$ 的方向上，$L_f(j)$ 和 $L_u(j)$ 的值较大，表明约束条件良好。在沿 $v_{r3}$、$v_{t2}$ 和 $v_{t3}$ 的方向上，$L_f(j)$ 和 $L_u(j)$ 的值较小，表明约束条件不佳。在不良条件的方向上，边缘点到线的对应关系（红色区域）所带来的可定位性贡献占主导地位，尤其是在 $L_u(j)$ 的计算中，后者代表高可定位性贡献的总和。因此，利用边缘点到线的对应关系所提供的可定位性信息至关重要。

5.2 分类

沿着6自由度姿态特征向量方向的所有与边缘点和平面点相关的对应约束的可定位性贡献被求和，以确定每个对应方向的可定位性类别。$L_f(j)$ 表示在方向 $v_j$ 上的整体可定位性贡献。$L_u(j)$ 表示在方向 $v_j$ 上高质量约束的可定位性贡献。根据它们的特征，设定阈值 $T_1$、$T_2$、$T_3$ 和 $T_4$ 来确定沿特征向量方向的6自由度姿态的可定位性类别，如下所示：

在沿 $v_j$ 的方向上，
$$\text{Category}=\{\begin{array}{ll}\text{Full},& \text{if }{L}_f(j)\ge T_1\text{ or }{L}_u(j)\ge T_2\\ \text{Partial},& \text{if }{L}_f(j)\ge T_3\text{ and }{L}_u(j)\ge T_4\\ \text{None},& \text{otherwise}\end{array}\text{\hspace{1em}(37)}$$

也就是说，当 $L_f(j)\ge T_1$ 或 $L_u(j)\ge T_2$ 时，沿 $v_j$ 的方向的可定位性类别被视为完全可定位，即类别为“Full”。当 $L_f(j)\ge T_3$ 且 $L_u(j)\ge T_4$ 时，沿 $v_j$ 的方向的可定位性类别被视为部分可定位，即类别为“Partial”。在这个方向上，$L_f(j)$ 和 $L_u(j)$ 必须满足各自的阈值。这意味着在沿 $v_j$ 的方向上必须有足够数量的高可定位性贡献点，这有助于在后续步骤中添加额外约束后提高优化质量。否则，沿 $v_j$ 的方向的可定位性类别被视为不可定位，即类别为“None”。在本研究中，基于实验验证，对于所有后续实验，$T_1$ 设置为50，$T_2$ 设置为30，$T_3$ 设置为15，$T_4$ 设置为9。LocDetect模块使用来自激光雷达的扫描来分配沿每个特征向量方向的6自由度姿态的可定位性类别。图4展示了以下过程：在一个起伏的开阔区域中，当前的激光雷达扫描被用作输入。通过计算所有边缘点到线和平面点到平面的对应关系的可定位性贡献，获得沿 $v_j$ 的方向上 $L_f(j)$ 和 $L_u(j)$ 的值。这些值以与图3相同的双y轴直方图形式呈现在图4(b)中。随后，$L_f(j)$ 和 $L_u(j)$ 在每个方向上的值被有条件地评估并与预定义的阈值进行比较，以确定该方向的可定位性类别。决策过程在图4©中进行了说明。所有方向的分类结果在图4(d)中显示如下：沿 $v_{r1}$、$v_{r2}$ 和 $v_{t1}$ 的方向的类别为完全可定位，表明几何约束良好。沿 $v_{r3}$ 和 $v_{t2}$ 的方向的类别为部分可定位，这表明尽管这些方向表现出退化现象，但仍然可以利用一些几何信息。而沿 $v_{t3}$ 的方向的类别为不可定位，表明该方向存在严重的退化，且可用的几何信息极为有限。

图 3. 两个示例性的双 Y 轴直方图，展示了沿 6 自由度姿态的每个特征向量方向的 $L_f$ 和 $L_u$。计算得到的可定位性贡献来自于激光雷达扫描。结果显示，在 $v_{r3}$、$v_{t2}$ 和 $v_{t3}$ 方向上，$L_f(j)$ 和 $L_u(j)$ 的值较小。红色区域表示来自边缘点到线段对应关系的可定位性贡献，而粉色区域则表示来自平面点到平面对应关系的贡献。

图 4. 可定位性检测模块的示例。(a) 扫描：在稍微起伏的开阔地带获得的激光雷达扫描。(b) 可定位性贡献：两个与图 3 中相同的双 Y 轴直方图。这些直方图是基于 (a) 中扫描的可定位性贡献计算得出的。© 分类：通过条件判断为 6 自由度姿态的每个特征向量方向分配可定位性类别。阈值设置参照第 IV-C 节。(d) 结果：沿特征向量方向的可定位性类别结果展示。

6. 结论

为了提高基于激光雷达的 SLAM 算法在复杂非结构化环境中的准确性，检测和处理退化现象至关重要。本文提出了一种新的 ICP 算法框架，称为 LP-ICP，该框架结合了点到线和点到面的代价函数，以及可定位性检测和处理。LP-ICP 通过计算几何对应关系的可定位性贡献来检测退化，并增加额外约束以提高姿态估计的准确性。通过在我们的类行星模拟数据集和实际地下隧道数据集上的实验评估，验证了所提方法的有效性，结果显示其准确性高于或可与最先进的方法相媲美。

为了充分利用公众号的碎片化时间，我们精心整理了经典博客文章、面试经验和一线开源项目等优质内容，并进行系统化的分享。这些资源不仅涵盖了前沿技术和行业动态，还提供了实用的学习和实战经验，使读者能够在短时间内快速获取有价值的信息。通过这种方式，我们希望帮助大家在忙碌的生活中有效利用零散时间，持续补充和深化知识，提升自身的专业素养和竞争力。无论是在通勤途中还是闲暇时光，都能轻松获取知识，助力个人成长。可以点击下面的图标关注公众号↓