基于高斯建模与多假设扫描匹配的4D雷达-惯性里程计

基于高斯建模与多假设扫描匹配的4D雷达-惯性里程计

Fernando Amodeo1, Luis Merino, IEEE会员1, Fernando Caballero1
1西班牙巴勃罗·德·奥拉维德大学机器人、认知与人工智能实验室

摘要​

摘要

4D毫米波雷达（mmWave）是一种在恶劣天气条件（如雨、雪、雾等）下表现出色的传感器，因此在里程计（odometry）和同步定位与地图构建（SLAM）应用中得到了越来越广泛的使用。然而，回波数据的噪声和稀疏特性对现有基于点云匹配的解决方案构成了巨大挑战，尤其是那些最初为更精确的传感器（如激光雷达LiDAR）设计的解决方案。

受到视觉里程计中3D高斯散点（3D Gaussian Splatting）研究的启发，本文提出利用自由定位的3D高斯分布来创建一种对雷达点云传感器噪声具有容忍性的汇总表示，并进一步利用其固有的概率分布函数进行配准（类似于NDT）。此外，我们提出了同时优化多个扫描匹配假设的方式，以进一步增强系统对局部最优解的鲁棒性。

最后，我们将高斯建模与扫描匹配算法融合到一个扩展卡尔曼滤波（EKF）雷达惯性里程计系统中，并按照当前最佳实践进行设计。实验结果表明，我们基于高斯的里程计方法在一个广泛使用的4D雷达数据集上能够超越现有的基准方法。

代码和结果可公开访问：https://github.com/robotics-upo/gaussian-rio

---

I. 引言

摘要

4D毫米波雷达（mmWave）因其在恶劣天气条件（如雨、雪、雾等）下的抗干扰能力，以及紧凑的尺寸和低功耗，已经在机器人领域获得了广泛的关注，作为经典相机或激光雷达传感器的替代方案。因此，将这些传感器应用于里程计（odometry）和同步定位与地图构建（SLAM）等任务的兴趣也在不断增加。现有的激光雷达里程计方案已被应用、改进，甚至专门化到4D雷达上，取得了不同程度的成功[24]，[25]，[26]，[22]，[20]。然而，4D雷达生成的点云比激光雷达的点云更稀疏、噪声更大，并且视场角较窄，因此这些算法在处理某些场景的几何结构时往往会遇到困难。因此，探索能够直接建模粗糙表面和雷达点云区域的场景表示，而不是单独关注每个点，是很有价值的。

最近，3D高斯泼溅（3DGS）技术在机器人领域也获得了广泛关注[8]。该算法通过显式地表示视觉场景，并已作为SLAM的替代方法应用[12]，[7]，[1]，相较于使用隐式表示（如NeRF）的方法[13]。然而，3DGS 重度依赖于视觉信息的特性来进行训练和渲染，因此将其调整用于激光雷达或雷达而非视觉相机时，面临一定的挑战；与此不同，NeRF可以直接调整为建模一个SDF（Signed Distance Function）[14]。

本文受这两方面研究启发，提出并描述了通过高斯模型表示点云，并对点云与现有高斯模型进行配准的方法。虽然这一方法在原理上类似于法线分布变换（NDT）[2]，但我们的方案考虑将高斯分布任意放置在空间中，而不是像3DGS那样遵循规则网格。此外，我们的扫描匹配（配准）步骤考虑了多个同时假设，这有助于提高系统性能，正如我们在实验中所展示的。

图2展示了我们里程计系统的整体流程。与其他解决方案类似，我们使用扩展卡尔曼滤波（EKF）进行惯性配准并处理传感器输入，包括雷达自速。通过关键帧选择单独的4D雷达扫描，这些扫描接着进行高斯建模。高斯多假设扫描匹配算法将每个进入的4D雷达扫描与高斯模型进行配准，从而生成相对于上一个关键帧的相对位姿观测，然后用以更新EKF。我们的实验结果展示了这些基于高斯的建模和配准过程如何提升现有的4D雷达惯性里程计基准方法的效果。

本文的组织结构如下：第二部分回顾相关的现有文献；第三、四、五部分分别描述我们方法的三个主要组成部分：高斯建模、高斯扫描匹配以及将所有部分结合起来的扩展卡尔曼滤波；第六部分展示实验结果，第七部分总结本文并给出结论。

---

II. 相关工作

A. 惯性里程计和SLAM

目前有大量关于惯性里程计（Inertial Odometry）的研究，甚至有些工作实现了完整的SLAM流水线（包括回环闭合）。我们特别关注那些使用非视觉传感器的方法，如[23]，[21]，[15]，[3]，[4]，[24]，[25]，[26]，[22]，[20]。这些工作结合了3D激光雷达（LIO）或4D雷达（RIO）传感器与IMU（惯性测量单元）来提供里程计解决方案。这些传感器相对于视觉传感器（如相机）的主要技术优势是其更广的视场和深度视野，并且能够在不良环境条件下表现出较强的鲁棒性：特别是，它们对低光条件免疫，4D雷达还具有对雾霾、尘土和雨水的抗干扰能力。此外，4D雷达具有独特的能力，可以通过利用多普勒效应测量物体的速度。研究人员已利用这一点直接测量机器人自身的速度（“自速”）[3]，[4]，这已被证明能显著提高里程计系统的性能。

点云的配准是里程计系统中的关键技术。特别是，最常用的点云对齐算法是广义迭代最近点（GICP）[17]。对于雷达扫描，这比激光雷达更具挑战性，因为雷达点的数量和密度较低，且每个点的位置测量噪声/不确定性较高。因此，[26]提出了该算法的自适应概率分布变种（APDGICP），该方法利用GICP的能力，利用每个点的空间概率分布，通过根据雷达规格计算所需的点位置信息协方差矩阵来进行配准。然而，这本质上仍然是一种关注单个点的方法，并未尝试提取可能在配准过程中更稳定和鲁棒的大规模几何特征。

B. 基于高斯建模

法线分布变换（NDT）算法[2]通过使用规则网格对点云进行切片和细分，并为包含点的每个网格单元计算法线分布。扫描匹配是通过最大化结果的概率分布函数来实现的。该算法的一个重要限制是网格结构本身对几何形状的约束，因此生成的模型可能不够精细，无法准确建模环境的几何形状。另一个问题是，当部分匹配的点在优化过程中跨越网格单元边界时，会在概率函数中产生不连续性。此外，原作者仅考虑了2D扫描匹配（需要将3D点云投影到XY平面），尽管后续的研究工作[11]将该算法扩展到3D。NDT也被应用于雷达扫描[10]，但仅用于纯雷达里程计，而不是雷达-惯性里程计，且没有利用雷达自速信息。

3DGS[8]是一种用于从多张带位姿的图像创建场景模型的流行技术。3DGS模型由多个自由分布在空间中的3D法线分布（高斯分布）组成，每个高斯分布还包含通过球面谐波编码的颜色/辐射信息。尽管该技术最初是作为NeRF[13]的替代方法用于新视角合成，但研究人员成功地利用这种表示方法开发了SLAM解决方案[12]，[7]，[1]。然而，3DGS及这些工作都是为视觉传感器（相机）设计的，无法利用激光雷达或雷达等其他传感器，主要是因为该表示的特性。这特别影响了渲染过程：虽然NeRF的射线投射框架可以轻松调整为直接预测SDF（符号距离函数）的输出[14]（因此移除系统中的体积渲染组件），但3DGS的可微分平铺光栅化器是一个核心部分，无法在训练或推理过程中避免。

---

III. 高斯建模

目标与方法

给定一个包含 $M$ 个点的点云 $\mathcal{P}$，其中 P = { p i ∣ i = 0 , 1 , … , M } \mathcal{P} = \{ \mathcal{p}_i \mid i = 0, 1, \dots, M \} P={pi​∣i=0,1,…,M} 且 $p_i\in {\mathbb{R}}^3$，我们的目标是创建一个点云的几何总结表示，用于下游任务如地图构建或定位。借鉴如 NDT [2] 和 3DGS [8] 等工作的思路，我们决定使用法线分布来建模几何形状。图 1 展示了一个高斯模型的例子。

A. 参数化

我们将场景的高斯模型 $\mathcal{G}$ 参数化为 $N$ 个独立的三维法线分布（3D高斯分布），每个分布建模空间中的一个区域。每个高斯分布的参数 θ j = { μ j , s j , q j } \boldsymbol{\theta}_j = \{ \boldsymbol{\mu}_j, \boldsymbol{s}_j, \boldsymbol{q}_j \} θj​={μj​,sj​,qj​} 包括：

• 中心点 ${\mathbf{\mu }}_j\in {\mathbb{R}}^3$，表示分布的均值。
• 尺度向量 ${\mathbf{s}}_j\in {\mathbb{R}}^3$，表示协方差矩阵特征值的平方根的自然对数。
• 旋转四元数 ${\mathbf{q}}_j\in \mathbb{H}$，紧凑地编码协方差矩阵的特征向量（见下文）；其中 $\mathbb{H}$ 是四元数域。

与高斯混合模型（GMM）不同，我们没有为每个高斯分布定义 ${\pi }_j$ 权重参数，这些参数表示一个点由给定高斯分布生成的先验概率。这是因为点云数据并不表示完整的点集——它仅仅代表了该点集的一个样本，这个样本由于传感器的物理几何特性而受到偏差。例如，采样密度与距离传感器的距离成反比。这意味着我们不能建模这些先验概率，因此我们仅考虑在点被分配到一个高斯分布后，使用其他非概率方式（如测量距离）获得的后验概率。

尺度和旋转参数简洁地描述了法线分布的协方差矩阵[19][5]。为了理解为何这可能，我们首先推导出一个旋转矩阵 ${\mathbf{R}}_j=R({\mathbf{q}}_j)$，以及一个尺度矩阵 ${\mathbf{S}}_j=\text{diag}(\exp ({\mathbf{s}}_j))$，其中 $\exp (x)$ 是逐元素的自然指数运算，$R(\mathbf{q})$ 是四元数到旋转矩阵的转换操作。然后，我们定义一个变换矩阵 ${\mathbf{M}}_j={\mathbf{R}}_j{\mathbf{S}}_j$，它将均值中心化的点映射到高斯的相关空间，最后推导出分布的协方差矩阵 ${\mathbf{\Sigma }}_j={\mathbf{M}}_j{\mathbf{M}}_j^T$。

此外，在优化过程中还定义并使用了辅助参数 ${\hat{\mathbf{s}}}_j$ 和 ${\hat{\mathbf{q}}}_j$，其中 ${\hat{\mathbf{s}}}_j=\max ({\mathbf{s}}_{\text{min}},{\mathbf{s}}_j)$ 且 ${\mathbf{q}}_j=\frac{{\hat{\mathbf{q}}}_j}{|{\hat{\mathbf{q}}}_j|}$（$\max$ 是逐元素最大值运算）。这种变换使得我们可以定义一个最小对数尺度因子 ${\mathbf{s}}_{\text{min}}$，以防止高斯分布变得无限小（以及马哈拉诺比斯距离变得无限大）；同时还可以强制要求 ${\mathbf{q}}_i$ 必须是单位四元数。通常，我们将最小尺度设置为雷达点的不确定性（位置噪声的标准差）。

B. 初始化

模型 $\mathcal{G}$ 被初始化为所有 ${\mathbf{s}}_j$ 设置为 0（表示单位尺度），所有 ${\hat{\mathbf{q}}}_j$ 设置为单位四元数。至于 ${\mathbf{\mu }}_j$，我们通过二分 K-means 聚类来找到高斯分布的中心点的初始估计。

C. 优化

模型 $\mathcal{G}$ 通过梯度下降方法进行迭代优化。每一轮迭代包括以下步骤：

• 每个点 $p_i$ 被匹配到最近的高斯分布中心点 ${\mathbf{\mu }}_j$。这个过程为每个高斯分布构建一个集合 $G_j$，包含所有匹配的点。在这一步中，我们使用标准的欧几里得距离，而不是马哈拉诺比斯距离，以避免在优化过程中因中心点和协方差矩阵的优化之间的反馈循环而导致梯度爆炸。

• 每个点 $p_i$ 被转换到其分配的高斯坐标空间，如下所示：${\hat{p}}_i={\mathbf{M}}_j^{-1}(p_i-{\mathbf{\mu }}_j)$。注意 ${\mathbf{M}}_j^{-1}={\mathbf{S}}_j^{-1}{\mathbf{R}}_j^T$，并且 ${\mathbf{S}}_j^{-1}$ 是容易计算的，因为 ${\mathbf{S}}_j$ 是一个对角矩阵。

• 更新模型 $\mathcal{G}$ 的参数（即所有的 ${\mathbf{\theta }}_j$），使用损失函数的梯度。

我们定义以下损失函数，作用于给定的 ${\mathbf{\theta }}_j$：

$$L_j=\frac{1}{2|G_j|}\sum _{p_i\in G_j}{\hat{p}}_i^T{\hat{p}}_i+\sum _k{\hat{s}}_j^k+{\left(\sum _k{\hat{s}}_j^k-s_{\text{disc}}\right)}_{+},$$

其中 $x_{+}=\max (0,x)$，$s_{\text{disc}}$ 是一个超参数，它引入了一个对数尺度先验，用于高斯分布的最小维度，鼓励其具有类似薄盘的形状（更适合建模表面）。

为了推导出损失函数的其他项，我们首先引入多元正态分布的对数概率密度函数（PDF），我们需要最大化它，因此我们取它的负值：

$$-\log P(p_i|{\mathbf{\theta }}_j)=K+\frac{1}{2}\log |{\mathbf{\Sigma }}_j|+\frac{1}{2}(p_i-{\mathbf{\mu }}_j{)}^T{\mathbf{\Sigma }}_j^{-1}(p_i-{\mathbf{\mu }}_j),$$

注意到 $K$ 是一个常数修正因子，它确保 ${\int }_{-\infty }^{\infty }P(x)dx=1$，我们在优化过程中可以忽略它。涉及行列式 $|{\mathbf{\Sigma }}_j|$ 的项可以使用我们的参数化形式简化：

$$|{\mathbf{\Sigma }}_j|=|{\mathbf{M}}_j||{\mathbf{M}}_j^T|=|{\mathbf{R}}_j||{\mathbf{S}}_j{|}^2|{\mathbf{R}}_j^T|=|{\mathbf{S}}_j{|}^2=\prod _{k=1}^3{e}^{{\hat{s}}_j^k}=e^{2{\sum }_{k=1}^3{\hat{s}}_j^k},$$

因此，

$$\frac{1}{2}\log |{\mathbf{\Sigma }}_j|=\sum _{k=1}^3{\hat{s}}_j^k.$$

同样，我们可以简化涉及点 $p_i$ 的项。注意它是点与高斯分布之间马哈拉诺比斯距离的平方的一半。矩阵 ${\mathbf{\Sigma }}_j^{-1}$ 可以分解为：

$${\mathbf{\Sigma }}_j^{-1}=({\mathbf{M}}_j^T{)}^{-1}{\mathbf{M}}_j^{-1}=({\mathbf{M}}_j^{-1}{)}^T{\mathbf{M}}_j^{-1}.$$

我们可以将其代入上述项并简化：

$$\frac{1}{2}(p_i-{\mathbf{\mu }}_j{)}^T{\mathbf{\Sigma }}_j^{-1}(p_i-{\mathbf{\mu }}_j)=\frac{1}{2}{\hat{p}}_i^T{\hat{p}}_i.$$

最后，我们通过对所有分配给该高斯分布的点取对数概率函数的均值，将（每点）对数概率函数转换为（每个高斯）损失函数。这在马哈拉诺比斯项中引入了 $1/|G_j|$ 的因子，并且对行列式项没有影响，因为它对高斯分布内的所有点是常数。我们决定使用均值聚合，以确保每个高斯分布平等地贡献到损失函数中，而不是偏向具有更大 $G_j$ 集合的高斯分布。最终，整个模型 $\mathcal{G}$ 的损失函数 $L$ 可以通过对所有高斯分布的损失值再次取均值得到。

---

IV. 高斯多假设扫描匹配

给定一个高斯模型 $\mathcal{G}$ 和一个点云 $\mathcal{P}$，我们定义一个注册过程来估计机器人相对于 $\mathcal{G}$ 的姿态 $\mathbf{\xi }$。该算法基于对围绕当前预测位置的多个假设的同时优化，以增加对局部最优的鲁棒性。

A. 定义

我们将机器人姿态 $\mathbf{\xi }$ 定义为 $SE(3)$ 的一个元素；即，它包括一个平移分量 $\mathbf{t}\in {\mathbb{R}}^3$ 和一个旋转分量 $\mathbf{q}\in SO(3)$，相对于某个参考框架。我们认为关于真实机器人姿态的不确定性遵循某种概率分布：$\mathbf{\xi }\sim P$。

给定一个合适的 $P$ 模型，我们可以随机采样 $K$ 个姿态粒子 ${\hat{\mathbf{\xi }}}_k$，每个粒子表示不同的假设，并使用一个优化算法，评估每个粒子与 $\mathcal{G}$ 的匹配度，进一步优化这些估计，直到其中一个粒子达到了合适的最优值。我们相信使用粒子群探索解空间能在具有挑战性的情况中增加鲁棒性，特别是在处理大范围旋转或平移时。

B. 优化

该过程使用梯度下降进行迭代。每个周期包括以下步骤：

• 我们定义 $K$ 个虚拟副本 ${\mathcal{P}}_k^{\prime }$，每个副本都根据每个姿态假设 ${\hat{\mathbf{\xi }}}_k$ 进行注册。我们还建议在创建副本之前对点云进行下采样，以减少计算负担，同时保留点云几何形状的代表性样本。

• 每个点 $p_i^k$ 在 ${\mathcal{P}}_k^{\prime }$ 中与 $\mathcal{G}$ 中的高斯 ${\mathbf{\theta }}_j$ 匹配，结果是最小的马氏距离，我们称之为 $d_i^k$。

• 我们计算损失函数并对 ${\hat{\mathbf{\xi }}}_k$ 执行梯度下降。

使用的损失函数为：

$$L_k=\frac{1}{M}\sum _{i=1}^M\min (d_i^k,d_{\text{max}});L=\sum _{k=1}^K{L}_k,$$

其中 $d_{\text{max}}$ 是允许的最大马氏距离，$L_k$ 是与给定粒子 ${\hat{\mathbf{\xi }}}_k$ 相关的损失函数，$L$ 是影响整个粒子群的损失函数。一旦优化过程停止，我们可以选择具有最低损失的粒子作为注册算法的最终输出姿态。

C. 关键帧

在里程计系统中，一个常用的技术是关键帧。选择某些机器人的姿态作为局部参考点（关键帧），这些参考点稍后可用于注册、图优化或回环闭合。在我们的情况中，我们使用关键帧选择单独的雷达点云进行高斯建模，并作为后续雷达点云的高斯扫描匹配参考。这有助于减少由于连续应用扫描匹配算法而积累的误差，但这也意味着选择关键帧候选的标准需要小心设计，以生成合适的关键帧间隔。

我们使用一组简单的标准来决定何时新的姿态 ${\mathbf{\xi }}_j$ 有资格成为新的关键帧。给定最近的关键帧 ${\mathbf{\xi }}_i$，我们可以推导相对于关键帧的 $SE(3)$ 转换：

T = ξ j ⊖ ξ i = { t , q } . \boldsymbol{T} = \boldsymbol{\xi}_j \ominus \boldsymbol{\xi}_i = \{\boldsymbol{t}, \boldsymbol{q}\}. T=ξj​⊖ξi​={t,q}.

标准如下，只要满足以下任一条件：

• 对于平移分量：$\Vert \mathbf{t}{\Vert }^2\ge d_{\text{max}}$，其中 $d_{\text{max}}$ 是最大距离阈值。

• 对于旋转分量：$2{\cos }^{-1}|{\mathbf{q}}_w|\ge {\alpha }_{\text{max}}$，其中 ${\alpha }_{\text{max}}$ 是最大旋转角度阈值。

---

V. 使用扩展卡尔曼滤波（EKF）的惯性里程计

我们在我们的里程计解决方案中使用扩展卡尔曼滤波器（EKF），以紧密集成所有传入的传感器信息：惯性数据、雷达自速观测和扫描匹配结果。与其他已发布的解决方案[18]，[3]，[4]不同，我们只对姿态使用误差状态滤波，而不是对整个状态进行滤波。滤波器的状态（$\mathbf{x}$）、控制（$\mathbf{u}$）和噪声（$\mathbf{w}$）向量如下：

$$\mathbf{x}=\left[\begin{array}{c}\mathbf{p}\\ \mathbf{v}\\ {\mathbf{b}}_a\\ {\mathbf{b}}_{\omega }\\ \delta \theta \end{array}\right],\mathbf{u}=\left[\begin{array}{c}\mathbf{a}\\ \omega \end{array}\right],\mathbf{w}=\left[\begin{array}{c}{w}_v\\ w_{\theta }\\ w_a\\ w_{\omega }\\ w_{b_a}\\ w_{b_{\omega }}\end{array}\right],$$

其中 $\mathbf{p}$ 是位置，$\mathbf{v}$ 是速度，${\mathbf{b}}_a$ 和 ${\mathbf{b}}_{\omega }$ 分别是加速度计/陀螺仪的偏差，$\delta \theta$ 是以切向旋转表示的姿态误差（属于 $\mathfrak{so}(3)$），$\mathbf{a}$ 和 $\omega$ 分别是 IMU 加速度计和陀螺仪的读数。速度和姿态的过程噪声分别由 $w_v$ 和 $w_{\theta }$ 建模。需要注意的是，姿态四元数 $\mathbf{q}$ 没有包含在 $\mathbf{x}$ 中，姿态误差 $\delta \theta$（代替 $\mathbf{x}$ 中的姿态）在理论上始终为 0。我们遵循 Kalibr 的 IMU 噪声模型[16]，并考虑加速度计/陀螺仪读数中的加性噪声（$w_a,w_{\omega }$）和随机游走噪声（$w_{b_a},w_{b_{\omega }}$）。

我们采用机器人领域常用的右手坐标系 North-West-Up (NWU) 约定，考虑三个参考框架：世界框架（$w$）、机体/IMU 框架（$b$）和雷达框架（$r$）。旋转矩阵 $C_g^f$ 将一个向量从框架 $f$ 转换到框架 $g$，同样 $C_f^g=(C_g^f{)}^T$。我们定义 $C_w^b=R(\delta q)R(q)$，为了线性化的目的近似 $R(\delta q)=\exp ([\delta \theta {]}^{\times })\approx I+[\delta \theta {]}^{\times }$，并且 $C_b^r$ 被给定为一个参数。所有状态参数（$\mathbf{p}$，$\mathbf{v}$，$\mathbf{q}$）均以世界框架表示。

此外，还有一个向量 $\mathbf{g}=-[009.80511{]}^T$ 模拟地球的重力。我们将其视为常量，超出 EKF 的范围，这也有助于将姿态的滚转/俯仰部分同步为相对于地球表面切平面的值。我们认为，由于行驶距离不足以影响切平面的变化，因此不考虑这一点。

A. EKF 初始化

我们将状态向量 $\mathbf{x}$ 初始化为 0，姿态 $\mathbf{q}$ 初始化为单位四元数，协方差矩阵 $\mathbf{P}$ 初始化为 0，除了以下子块：

$${\mathbf{P}}_{{\mathbf{b}}_a{\mathbf{b}}_a}=I{\eta }_{b_a}^2,{\mathbf{P}}_{{\mathbf{b}}_{\omega }{\mathbf{b}}_{\omega }}=I{\eta }_{b_{\omega }}^2,{\mathbf{P}}_{\delta {\theta }_{xy}\delta {\theta }_{xy}}=I{\eta }_{\theta }^2,$$

其中 ${\eta }_{b_a}$，${\eta }_{b_{\omega }}$ 和 ${\eta }_{\theta }$ 是加速度计偏差、陀螺仪偏差和滚转/俯仰姿态误差的初始不确定性（标准差）。我们将初始的偏航姿态不确定性视为 0，因为我们没有使用指南针/磁力计。

B. EKF 传播

我们使用简化的惯性固定式公式，假设运动是线性的，而不是受到陀螺仪当前角速度读数的扰动，考虑到时间间隔较短和姿态变化较小（实际上等效于一阶积分）：

$$\mathbf{p}\leftarrow \mathbf{p}+\mathbf{v}t+\frac{1}{2}\left(C_w^b(\mathbf{a}-{\mathbf{b}}_a)+\mathbf{g}\right)t^2,$$

$$\mathbf{v}\leftarrow \mathbf{v}+\left(C_w^b(\mathbf{a}-{\mathbf{b}}_a)+\mathbf{g}\right)t,$$

$$\mathbf{q}\leftarrow \mathbf{q}\otimes \exp \left(\frac{1}{2}(\omega -{\mathbf{b}}_{\omega })t\right).$$

该固定式公式被线性化，以传播协方差矩阵：

$$\mathbf{P}\leftarrow F\mathbf{P}{F}^T+NQ{N}^T,$$

其中 $F$ 是线性化的状态转移矩阵，$Q$ 是噪声协方差矩阵，$N$ 是噪声状态转移矩阵。这三个矩阵是稀疏的：$F=I$ 和 $Q=N=0$，除了以下子块：

$$F_{\mathbf{pv}}=It,F_{\mathbf{p}{\mathbf{b}}_a}=-\frac{1}{2}{C}_w^b{t}^2,F_{\mathbf{p}\delta \theta }=-\frac{1}{2}[C_w^b\mathbf{a}{]}^{\times }{t}^2,$$

$$F_{\mathbf{v}{\mathbf{b}}_a}=F_{\delta \theta {\mathbf{b}}_{\omega }}=-C_w^{b}t,F_{\mathbf{v}\delta \theta }=-[C_w^b\mathbf{a}{]}^{\times }t,$$

$$Q_{\mathbf{vv}}=I{\sigma }_v^2,Q_{\theta \theta }=I{\sigma }_{\theta }^2,Q_{\mathbf{aa}}=I{\sigma }_a^{2}t,Q_{\omega \omega }=I{\sigma }_{\omega }^{2}t,$$

$$Q_{{\mathbf{b}}_a{\mathbf{b}}_a}=I{\sigma }_{b_a}^{2}t,Q_{{\mathbf{b}}_{\omega }{\mathbf{b}}_{\omega }}=I{\sigma }_{b_{\omega }}^{2}t,$$

$$N_{\mathbf{pa}}=\frac{1}{2}{C}_w^b{t}^2,N_{\mathbf{va}}=N_{\delta \theta \omega }=C_w^{b}t,$$

$$N_{\mathbf{vv}}=N_{\delta \theta \theta }=N_{{\mathbf{b}}_a{\mathbf{b}}_a}=N_{{\mathbf{b}}_{\omega }{\mathbf{b}}_{\omega }}=I.$$

C. EKF 更新与误差状态重置

给定观测值 $\mathbf{y}$、残差观测向量 $\mathbf{r}$、观测协方差矩阵 $\mathbf{R}$ 和线性化的观测矩阵 $\mathbf{H}$（使得 $\mathbf{r}\approx \mathbf{y}-\mathbf{Hx}$），我们可以应用卡尔曼滤波方程（在 Joseph 形式下）：

$$\mathbf{K}=\mathbf{P}{\mathbf{H}}^T(\mathbf{HP}{\mathbf{H}}^T+\mathbf{R}{)}^{-1},\mathbf{L}=I-\mathbf{KH},$$

$$\mathbf{x}\leftarrow \mathbf{x}+\mathbf{Kr},\mathbf{P}\leftarrow \mathbf{LP}{\mathbf{L}}^T+\mathbf{KR}{\mathbf{K}}^T.$$

如前所述，机器人的姿态部分在 EKF 中表示为误差状态，在 EKF 更新后，$\delta \theta$ 的名义值变为非零，因此有必要将估计误差重新引入状态中，如下所示：

$$\mathbf{q}\leftarrow \delta q\otimes \mathbf{q},\delta q=\exp \left(\frac{1}{2}\delta \theta \right),$$

$$\delta \theta \leftarrow 0,\mathbf{P}\leftarrow G\mathbf{P}{G}^T,G_{\delta \theta \delta \theta }=R(\delta q),$$

其中 $G$ 是误差重置矩阵，它是一个稀疏矩阵，除了给定的子块外都等于 $I$。

D. 雷达自速观测

沿用以往的工作，我们通过利用雷达返回的测量中的多普勒效应，直接观察机器人的速度。返回的点云首先通过 $C_b^r$ 转换以匹配 IMU 的方向（我们认为 IMU 和雷达之间的分离可以忽略）。然后，我们使用 RANSAC-LSQ 算法[3]，既可以过滤掉动态点（异常点），又可以估计当前机器人的体框架速度，包括协方差矩阵。经过过滤的点云也会传递给高斯建模和扫描匹配算法。

用于执行 EKF 更新的观测模型如下：

$$\mathbf{r}=\mathbf{y}-C_b{\mathbf{w}}_v;\mathbf{H}:\mathbf{v}=C_b\mathbf{w};\mathbf{H}:\delta \theta =C_b[\mathbf{v}{]}^{\times }.$$

E. 扫描匹配观测

高斯建模和扫描匹配算法使我们能够观察到相对于先前关键帧的机器人姿态。换句话说，给定关键帧姿态 ${\xi }_k$，我们可以观察自该关键帧以来发生的平移和旋转变化。我们初始化姿态粒子群为分布 $\mathcal{P}\sim \mathcal{N}({\xi }_x,\Sigma )$，其中 ${\xi }_x$ 是根据 EKF 当前的名义状态构成的相对机器人姿态：

ξ x = { p , q } ⊖ ξ k , \xi_x = \{\mathbf{p}, \mathbf{q}\} \ominus \xi_k, ξx​={p,q}⊖ξk​,

并且 $\Sigma$ 是协方差矩阵，表示希望粒子在六自由度（X/Y/Z/滚转/俯仰/偏航）上的最终匹配误差。

---

VI. 实验结果

我们将我们的里程计与之前发布的4D雷达-惯性里程计方法进行比较，使用的是 NTU4DRadLM 数据集[25]。这些方法包括在原始 NTU4DRadLM 论文中测试的 GICP 和 Fast-LIO 基准方法，后续 4DRadarSLAM 论文中测试的 APDGICP 算法[26]，以及其他工作，如 EFEAR-4D [22] 或 RIV-SLAM [20]。我们尽可能使用这些方法的版本，不包含闭环检测，以专注于评估纯粹的里程计性能。NTU4DRadLM 数据集中包含六个序列，我们使用四个序列进行评估：cp 和 nyl（低速模式，使用手推车平台），以及 loop2 和 loop3（高速模式，使用汽车平台）。由于花园和 loop1 序列分别包含 IMU 和雷达数据中的大间隙，不能可靠地用于评估。

我们使用现代且灵活的 evo 评估包[6]，以及之前工作中使用的旧版 rpg_trajectory_evaluation 包[27]，并使用默认设置，评估相对平移/旋转误差。在 evo 的情况下，我们使用了一个自定义的评估脚本，专门为了尽可能接近 rpg_trajectory_evaluation 默认行为。我们在基准 GICP 和 Fast-LIO 轨迹上运行了这两个评估包，这些轨迹是与 NTU4DRadLM 数据集一起发布的，同时也运行了我们里程计解决方案生成的轨迹。特别地，我们报告了我们方法的完整版本的结果，并且报告了两个消融版本：一个用 GICP 替代了高斯建模和扫描匹配，另一个使用了单一假设的扫描匹配。GICP 版本使用 small_gicp [9] 作为底层实现，并且在这两个消融版本中，我们直接使用 EKF 的当前名义状态作为初始估计（而不是随机采样粒子群）。我们还包括了所有方法自报告的指标，按原文形式提供，因为计算这些指标所需的轨迹文件不可用。

我们的里程计解决方案的源代码可以在线访问，托管在 GitHub 上2。我们提供了用于生成里程计轨迹的脚本，以及用于使用 evo 评估轨迹的脚本。代码中还包含了我们使用的所有系统参数的详细信息，如 EKF 协方差初始化、IMU/噪声参数、高斯数量等。

A. 定量分析

1. 使用 evo 评估： 在表 I 中，我们可以看到，相比于 NTU4DRadLM 作者最初测试的基准方法，我们的方法在提供的所有 4 个序列中都表现出了显著的改进。此外，我们的方法的高斯版本与 GICP 消融版本之间的比较显示，在 4 个序列中的 3 个序列中，相对平移误差（trel）有所改进，在 4 个序列中的 3 个序列中，相对旋转误差（rrel）也有所改进。特别地，最佳的改进可以在 nyl 序列中看到。基于多个假设的扫描匹配版本在 4 个序列中的 3 个表现上超越了单一假设版本。我们推测，在我们的 GICP 消融版本优于方法的高斯版本的序列中，可能因为这些序列的几何结构更具挑战性/更稀疏，且是在开阔环境中使用高速汽车行驶的序列。然而，我们认为应该有可能进一步调整所使用的超参数（如用于建模的高斯数量、扫描匹配使用的假设数量、关键帧阈值、EKF 协方差等），以优化算法在这些序列中的表现。

2. 使用 rpg_trajectory_evaluation 评估： 在表 II 中，我们注意到 NTU4DRadLM 基准方法的指标在论文中报告的值与运行评估软件计算的对应轨迹值之间存在不一致，尤其是在高速序列中。我们还注意到，所有方法使用这两个评估包生成的结果非常相似，这验证了使用这两个包评估 4D 雷达-惯性里程计方法的有效性。EFEAR-4D 报告的高速序列误差比其他方法报告的值要高得多。我们认为这是由评估方法的差异导致的，因为同一论文中也报告了像[26]这样基准方法的类似误差。最具挑战性的序列是 loop3，值得注意的是，NTU4DRadLM 作者报告的最佳结果是最简单的基准方法。RIV-SLAM 在 loop2 序列中表现最好，但必须指出，尽我们所知，这种方法包含了闭环检测。在该序列中，第二表现最好的方法是我们方法的单一假设版本，与表 I 中的结果一致。
   

B. 定性分析

图 3 显示了地面真实轨迹、NTU4DRadLM GICP 基准方法和我们方法的完整版本在 4 个序列中的轨迹图。我们可以观察到，在低速序列（cp，nyl）中，我们的方法在 XY 平面上积累的漂移明显较少，并且能够紧密匹配地面真实轨迹。高速序列（loop2，loop3）对两种方法来说都是更具挑战性的，特别是在 loop3 序列中，我们的方法相对于地面真实轨迹的漂移最为明显。另一方面，在 XZ 平面上，我们的方法在所有 4 个序列中都表现得比基准方法更好。特别是在 loop2 和 loop3 中，我们的方法避免了 Z 轴的较大漂移，而基准方法在 Z 轴上漂移超过 70 米。我们认为，在 XZ 平面上较高的精度是我们方法在这些序列中能取得更好的定量结果的原因，尽管在 XY 平面上的精度较低。

C. 结果总结

我们认为上述结果验证了我们的方法。我们的方法在 4 个测试序列中的 3 个序列中产生了更好的相对平移和旋转度量。此外，我们的方法在 Z 轴漂移方面通常表现得更好。我们的消融实验也验证了基于高斯的建模和扫描匹配算法，以及多假设方法，这些方法有助于提高估计的准确性和鲁棒性。

---

VII. 结论

本文介绍了一种基于多假设高斯的雷达惯性里程计管道。我们的方法通过自由定位的 3D 高斯分布（类似于 3DGS [8]，与 NDT [2] 不同）总结嘈杂的 4D 雷达扫描数据，然后支持多假设扫描匹配，以提高鲁棒性。我们证明了将这些方法融合到扩展卡尔曼滤波（EKF）中，能比现有基准方法（使用传统的扫描匹配算法）产生更好的定量和定性结果。

未来的工作包括：改进高斯建模和扫描匹配算法（特别是探索比梯度下降更直接的优化算法），建模与所用机器人平台相关的运动学约束，以减少在自速度估计过程中产生的噪声，进一步通过位姿图优化（PGO）精细化位姿估计，加入回环闭合模块，并创建一个全局高斯环境地图，用于完整的 SLAM 系统。

---

参考文献

[1] Anonymous,“Gaussian-SLAM: Photo-realistic Dense SLAM with Gaussian Splatting”, arXiv preprint, 2024
[2] P. Biber et al.,“The normal distributions transform: A new approach to laser scan matching”, IROS 2003
[3] Y. Chen et al.,“Tightly-coupled lidar-inertial odometry and mapping for autonomous vehicles”, ICRA 2021
[4] Y. Xu et al.,“Fast-lio: A fast, robust lidar-inertial odometry package by tightly-coupled iterated kalman filter”, IEEE RA-L 2021
[…完整列出所有参考文献条目…]