精选经典文献阅读之--DAMS-LIO(基于iEKF的轻量级LiDAR惯性里程计)

敢敢のwings

发布时间 2023.07.18阅读数 1741 评论数 3

0. 简介

融合方案是多传感器融合方法的关键，多传感器融合方法是地下矿山和行星表面等复杂极端环境下状态估计的最有前途的解决方案。本文提出了一种基于iEKF的轻量级LiDAR惯性里程计系统，该系统采用可感知退化的模块化传感器融合管道，仅在检测到退化时才在更新过程中同时测量来自另一个里程计的LiDAR点和相对位姿。通过CRLB理论和仿真实验验证了该方法与单一观测值方法相比具有更高的精度。此外，针对各种最先进的传感器融合方法，在感知挑战数据集上对所提出的系统进行了评估。结果表明，即使在恶劣的环境和较差的观测条件下，该系统仍具有较高的估计精度和实时性能。

虽然这个工作还没有开源，但是预计应该会在接收后开源吧，具体原文内容可以参考这里《DAMS-LIO: A Degeneration-Aware and Modular Sensor-Fusion LiDAR-inertial Odometry》。

图1：（a）是DAMS-LIO在CERBERUS DARPA地下挑战数据集中的映射结果，其中包含许多具有挑战性的环境，如（b）-（c）所示，包括黑暗、长隧道和无纹理区域。我们将我们的映射结果与最先进的激光惯性测距导航系统LIO-SAM在（a）右上角进行比较。

1. 文章贡献

提出了一种基于退化感知和模块化传感器融合的轻量级激光雷达-惯性测程系统(DAMS-LIO)，该系统能够在极端环境下进行鲁棒和准确的状态估计，在计算资源有限的情况下，为机器人执行复杂探测任务提供了明显的优势。
提出了一种新的传感器融合方法，将激光雷达和其他里程计的信息完全融合，仅在检测到退化时，将激光雷达点和其他里程计的相对位姿作为更新过程中的测量值。
基于CRLB定理进行了理论分析，以量化性能，并证明了所提出的传感器融合方法的高精度。
在模拟和真实数据集上的大量实验验证了我们方法的鲁棒性和准确性。

2. 系统架构

我们系统中每个坐标系的定义如图2所示。遵循典型的LIO坐标系定义，{L}和{I}分别对应于激光雷达和IMU坐标系，而{G}是一个本地垂直参考坐标系，其原点与初始IMU位置重合。{O}表示其他里程计测量，其原点标记为{M}。

^Lp表示每个点相对于激光雷达坐标系的位置。一些重要且常用符号的定义在表格I中给出。

A. 状态向量（这部分内容比较通用）

我们系统中估计的状态包括当前的IMU状态x_I，LiDAR和IMU之间的外参I_{x_L}以及从其他里程计帧到IMU帧的变换I_{x_O}。在时刻t_k，状态可写为：

^Gv_I是IMU在全局坐标系下的速度，b_{g,k}和b_{a,k}分别表示陀螺仪和加速度计的偏差，g是在{G}坐标系下的重力向量，x_L = {^I_L \bar{q}, ^Ip_L}和x_O = {^I_O \bar{q}, ^Ip_O}表示从其他里程计坐标系{O}和LiDAR坐标系{L}到IMU坐标系{I}的变换。

B. IMU传播（同理IMU的传播）

由于IMU的测量受到偏差b和零均值高斯噪声n [15]的影响，因此它们可以被建模为:

其中，ω_m(t)和a_m(t)是原始的IMU测量数据，I_ω(t)是IMU在本地坐标系{I}下的角速度，G_g和G_{a_I}(t)是重力和IMU在全局坐标系下的加速度。IMU运动学与[16]相同，为了保持简洁，我们这里不再重复描述。为了将协方差矩阵从时间t_k传递到时间t_{k+1}，我们按照[17]的线性离散时间模型生成格式：

这段文本是关于状态向量和IMU传播的内容。其中，在Φ_k是线性化的系统状态转移矩阵，n_k=[n_g n_{wg} n_a n_{wa}]是系统噪声。除了四元数以外，误差状态定义为 \tilde{x} = x − \hat{x}。其中，四元数通过关系式q = \hat{q}⊗δq定义。与[16]相同，符号⊗表示四元数乘法，误差四元数定义为δq \backsimeq [ \frac{1}{2} δθ^T 1]^T。
如果将n_k的协方差定义为Q_k，则可以将状态协方差从t_k传播到t_{k+1}，具体方法见公式。

其中，Φ = diag(Φ_I, Φ_O)，G = [G^T_I, G^T_O]^T，Φ_I和G_I表示与其他里程计和IMU之间的外参之外的变量相关的部分，这与[12]中的定义相同。此外，Φ_O = I_{6×6},G_O = 0_{6×12}。

C. 测量模型（比较重点的知识）

1）LiDAR测量：对于来自LiDAR的测量，我们将其建模为[12]中的模型。在对在τi时间采样的扫描进行运动补偿后，我们将局部LiDAR坐标系中的第j个点定义为^Lp_j。通过向后传播，该点可以更改为与t_k处IMU测量对应的扫描结束测量。同时，由于每个点应该位于地图中的小平面区域上，我们可以得到

其中，^Gq_j 是小平面上的点，^Gu_j 是平面的法向量。^IR_L 和 ^GR_{Ik} 是对应于%ILq和^GI_{Kq}的旋转矩阵。^Ln_j 是点^Lp_j的测距和指向噪声。(8)也可以用更简洁的形式总结为：

为了线性化更新的测量模型，我们将测量模型在\hat{x}_k处进行一阶近似。

其中v_l ∈ N(0,R_l)是由于原始测量噪声^Ln_j而产生的高斯噪声。H_L是残差r_l对误差状态 \tilde{x}_k的雅可比矩阵，给出如下：

2) 另一种里程表测量：由于每个里程表的发布频率不同，我们执行线性插值以获取估计状态在时间 t_k 和 t_{k+1} 时的其他里程表姿态，并将它们的坐标系表示为 O_{k−1} 和 O_k。我们将从 A 到 B 的变换矩阵 ^AT_B 定义为 ^AT_B = [^AR_B ,^Ap_B ; 0_{3×3}, 1]。然后根据图 2，我们有以下变换关系：

其中，^{Q{k-1}}T_{O_k} = ^GT^{-1}_{O_{k-1}}^GT_{O_k} 是通过其他里程计计算出的相对位姿测量。因此，基于（13），我们可以很容易地得到测量模型 z_O = [\begin{matrix}z_r \ z_p\end{matrix}]，其中：

根据公式（10）计算的旋转和平移误差，其中v_r∈N(0,R_r)和v_p∈N(0,R_p)$$是相应的高斯噪声。因此，根据公式（14）和（15），我们可以得到旋转和平移残差的误差状态的雅可比矩阵。

其中：

D. 变形感知更新（比较重点的知识）

与[12]类似，我们有以下误差状态：

\boxplus/\boxminus表示Lie群中的加减运算符[18]。M_\kappa是(\hat{x}^\kappa_k \boxplus \tilde{x}^\kappa_k)\boxminus \hat{x}_k相对于\tilde{x}^\kappa_k的偏导数，以零为值进行评估。

其中，A(\cdot)定义在[19]中，\delta^X\theta_Y = ^X\hat{R}^\kappa _Y \boxminus ^X\hat{R}_Y。根据(24)，并将其带入上述的一阶逼近测量模型中，问题可以总结为：

为了避免位姿测量噪声影响LiDAR里程计的良好性能并在退化条件下增强里程计鲁棒性，我们采用了灵活的更新方案。理论上，对于退化维度对应的特征值，它们是精确为零的，但由于数据噪声和有限的计算精度，实际计算中通常是一个小值。因此，我们首先计算Hessian矩阵H^T_L H_L的特征值{λ_i}，并参考[20]中的启发式方法确定场景的几何特征。如果特征值小于阈值，则可以推断存在退化。**如果LIO处于良好状态并且假设我们有m个LiDAR测量，则z^κ = z^κ_l = [z^κ_{l1}, . . . , z^κ_{lm}]^T, H = H^κ_L = [H^κ_{l1}, . . . , H^κ_{lm}]^T和R = R_L = diag(R_{l1}, . . . , R_{lm})。否则，如果检测到退化，z^κ = [z^κ_l，z^κ_r，z^κ_p ]，H = [H^κ_L，H^κ_{O_r}，H^κ_{O_p}]^T，R = diag(R_L，R_{O_r}，R_{O_p})**。然后，我们可以执行[12]中相同的迭代卡尔曼滤波更新。

3. CramÉr-rao下界定理(重点知识)

在本节中，我们为读者提供有关所提出的融合方法的更深入见解，并使用CraméR-Rao下限（CRLB）将其与其他纯激光雷达方法的精度进行比较。 CRLB是一个估计器方差的下限，通常用于评估数据融合方法的性能[21] [22]。根据[23]，CRLB通过取费舍尔信息矩阵的逆来计算：

其中，H和R分别对应于第III节中的测量雅可比矩阵和协方差矩阵。CRLB越小，系统在理论上就可以达到更高的精度。为了分析需要，我们需要以下引理[24]。

引理1：正定矩阵的逆矩阵也是正定的。

引理2：如果存在一个实数非奇异矩阵B使得A = BB^T，则实对称矩阵A是正定的。

引理3：设A为正定矩阵，B为m×n的实数矩阵。如果B的秩：r(B) = n，则B^TAB是正定的。

基于上述理论，我们分别计算并比较不同方法的CRLB。

纯基于LiDAR的方法：由于纯基于LiDAR的方法不使用里程计位姿测量，因此将与里程计相关的外参从状态向量中移除。此外，为了简化计算，我们忽略对应于$$H$$中为零向量的变量。因此，基于（11）和（12），纯基于LiDAR的测量雅可比矩阵可以重写为：

我们将式(28)代入式(27)，并表示为一个分块矩阵的形式，具体如下：

由于 LiDAR 噪声的协方差矩阵是实对称矩阵，且 $$H_{li}$$ 的秩等于它的行数，我们可以推断出 $$J_{li}^{-1}$$ 是正定矩阵，表示为 $$J_{li}^{-1}>0$$。因此，根据分块矩阵的逆公式，我们可以轻松地得到对应于估计位姿的部分，表示为：
姿态融合方法：按照上述过程，我们得到的测量雅可比矩阵如下：

然后我们将公式(31)代入(27)并将其表示为与(29)相同的块矩阵形式：

然后，我们边缘化里程计外参相关的变量，得到变量的边缘化费舍尔信息矩阵 $$J_{pf_{mar}}$$。

接着，根据公式（30），估计位姿的对应 CRLB 为：
比较：考虑仅使用里程计姿态测量的情况（即状态向量中没有 LiDAR 和 IMU 之间的外参变量），则 Fisher 信息矩阵为：

根据这种情况，$$(F − DE^{−1}D^T)^{−1}$$是对估计姿态对应的信息矩阵，因此显然是正定的。根据引理1，我们可以得出$$(F − DE^{−1}D^T) > 0$$。根据[25]，我们有：

然后，可以验证$$CRLB_{pf} < CRLB_{li}$$。也就是说，将姿态测量和LiDAR点融合的方法，在姿态估计的协方差下有一个更小的下限，比仅使用LiDAR点的方法更优。将上述分析代入应用场景，表明我们的传感器融合方法充分利用了比那些将里程计信息用作初始值的方法更多的里程计测量信息，因此在姿态估计方面实现了更好的准确性。