使用雷达速度因子进行越野导航的鲁棒高速状态估计

Morten Nissov${}^{1,2}$, Jeffrey A. Edlund${}^1$, Patrick Spieler${}^1$, Curtis Padgett${}^1$, Kostas Alexis${}^2$ 和 Shehryar Khattak${}^1$

摘要

在复杂环境中实现机器人自主性以用于关键任务应用需要鲁棒的状态估计。特别是在导航所依赖的外感传感器可能因环境挑战而退化，从而导致任务失败的情况下。这种情况下，调频连续波（FMCW）雷达传感器的潜力得以凸显：作为一种具有直接速度测量能力的补充外感传感方式。在这项工作中，我们整合了FMCW雷达传感器的径向速度测量，使用径向速度因子提供线速度更新，并将其融合到滑动窗口状态估计器中，以与LiDAR姿态和IMU测量相结合。我们展示了这种增强增加了状态估计器对环境中存在的挑战的鲁棒性，以及这些挑战对脆弱的外感方式的负面影响。通过在复杂沙漠环境中以高速（约12 m/s）运行的全尺寸自主越野车辆进行的机器人实地实验，对所提出的方法进行了广泛的评估。此外，该方法的鲁棒性在LiDAR里程计性能的模拟和实际退化情况下得到验证，并与公共数据集上的雷达-惯性里程计的最新方法进行了比较。

I. 引言

现代自主机器人操作方法，尤其是在具有挑战性的条件下，需要可靠的状态估计来促进低级和高级任务的执行，如控制、规划和决策。正如文献[1]，[2]所展示的，当前方法通常遵循一个典型的架构：惯性导航结合外感传感，作为标准辅助惯性导航系统中GPS的替代方案。这类方法源于在有限或无法使用GPS的情况下（例如地下、恶劣天气等）实现自主操作的必要性。

通常，预积分IMU测量和外感姿态更新就足够了。然而，在这种架构中，机器人控制所需的一致且高频率的速度估计通常通过加速度计积分获得（角速度由陀螺仪提供）。对于高速、复杂环境和具有挑战性条件下的应用，保持准确且高频率的线速度估计（除了姿态估计之外）对于确保鲁棒操作变得愈发重要。对于姿态估计，视觉和LiDAR里程计可以提供可靠的估计，但对于速度估计，结合线速度输入以辅助加速度计积分会有帮助。尤其当速度更新频率高于视觉或LiDAR里程计的更新频率时，这种优势尤为明显。为此，雷达传感器变得值得关注，因为它们可以以高频率（高达60 Hz）采样并提供直接的线速度测量。此外，将雷达与典型的外感姿态更新结合意味着状态估计器能够同时接收姿态和线速度的测量，从而提高估计器的整体鲁棒性，只需估计IMU偏差而无需直接测量。这种配置更接近于经典的地面车辆GPS辅助惯性导航解决方案，在这种方案中，速度测量通常由轮式里程计提供，用于推算[3]。然而，要充分利用这些高频率的直接线速度测量，需要将它们以低延迟的方式整合到状态估计器中。

基于上述讨论，本文提出了一种方法，通过整合来自FMCW雷达的径向速度测量来提高状态估计的鲁棒性。所提出的方法强调低延迟更新，因此在径向速度测量可用时立即将其纳入。通过使用专为越野导航定制的全地形车，在两个不同的非结构化越野环境中以高达12 m/s的高速行驶，展示了该方法在机器人实地实验数据集上的鲁棒性增强。

图1: 图中显示了机器人车辆在沙漠环境中进行实地测试的场景。4公里的轨迹上标注了实验中实现的线速度和旋转速度。贫瘠且无结构的环境给LiDAR里程计带来了挑战，并导致在急转弯处发生失败（用A标记）。下排显示了通过车载摄像头捕捉的环境图像。

图1显示了沙漠环境中高速行驶的实例。此外，本文还评估了径向速度测量因子作为独立的里程计来源，并与公共数据集上的最新雷达-惯性里程计方法进行了对比。本文的贡献包括：

• 一种通用的方法，能够在不依赖最小二乘法的情况下整合雷达速度测量，从而在低延迟条件下使用视野有限的单束径向速度数据，进行可靠的高速状态估计。
• 通过高速机器人实地测试，展示了在引入所提出的雷达因子时，多模态状态估计器的鲁棒性增强。
• 在公共数据集上对提出的径向-惯性里程计估计因子进行了评估，并与最新方法进行了对比。

本文的其余部分结构如下：第二部分介绍了与多模态状态估计相关的工作；第三部分描述了径向速度因子和基于因子图的状态估计器；第四部分展示了实验结果、分析和对比；第五部分总结了经验教训和结论。

II. 相关工作

在复杂环境下执行关键任务的自主体的状态估计，传感器融合仍然是一个核心研究课题。许多方法借鉴了经典的GPS辅助惯性导航系统，解决了如何在高精度和高频率的状态估计之间取得平衡的问题，利用不同传感器的互补优势来减轻其各自的弱点。尽管GPS长期以来一直是首选的辅助传感器，但在许多环境中GPS的使用变得不可行。因此，一些提出的方法探索了其他可用的辅助传感器。随着多传感器和基于LiDAR的同步定位与地图构建（SLAM）方法的成熟，研究越来越关注这些方法在挑战性环境中的表现，尤其是如何利用多个新传感器来提升在各种环境条件下的性能【1】【2】。此外，全地形条件下的高速自主性不仅对地面汽车应用有吸引力，还对于推进未来地外科学任务的可能性至关重要【4】【5】。

在汽车研究领域，LiDAR已成为一种关键传感器，通常与IMU配对使用，其广泛应用从状态估计到地图构建，具有高精度和宽广的视野（FoV）【6】。尽管LiDAR里程计方法众多，维持地图并执行点云到地图的配准（通常使用迭代最近点（ICP）算法的变体【7】【8】）仍然是核心步骤【9】。然而，尽管LiDAR有诸多优势，但在含有遮挡物或不适合依赖几何点云对齐的环境中，LiDAR感知仍会受到挑战。在【10】中，作者提出了一种检测由缺乏多样环境几何导致的退化轴的方法，并将其集成到点云配准中。FMCW雷达是另一种典型的汽车感知传感器，因其独特的特性（如直接测量径向速度）而受到机器人社区的关注，尤其在传统感知方式（如视觉或LiDAR）难以发挥作用的环境中具有广泛应用潜力【11】。不仅在极端环境中，甚至在典型的汽车操作场景中也可能面临具有挑战性的环境条件。为此，作者在【12】中研究了天气对LiDAR感知的影响，并展示了通过雷达到雷达和雷达到LiDAR的扫描匹配方法提高性能的潜力。

在雷达惯性里程计方面，已经提出了许多不同的方法。作者在【13】中首次展示了使用FMCW雷达传感器进行自运动估计的潜力，通过稳健的最小二乘估计实现。在【14】中，作者从FMCW雷达点云中估计线速度和偏航角，然后将这些估计值与IMU测量值融合到扩展卡尔曼滤波器（EKF）中，用于辅助惯性导航。为了采用更紧耦合的方法，作者在【15】中将单个径向速度测量与点关联融合在一起，用于辅助惯性导航。与经典的EKF方法相反，作者在【16】中构建了一个滑动窗口平滑器来解决估计优化问题，并在浓雾环境下展示了其性能。重要的是，并非所有雷达传感器都相同，旋转雷达（通常不测量多普勒）在汽车应用中更为普遍；作者在【17】中提出了一种用于过滤和配准的高精度扫描匹配雷达里程计方法。

鉴于LiDAR在汽车应用中的普遍性以及其互补优势所带来的鲁棒性和精度的提升，LiDAR-雷达融合方法也引起了研究兴趣。在【18】中，作者通过在先前的LiDAR地图上对雷达点云进行配准来融合LiDAR和旋转雷达的测量。在【19】中，作者开发了一种端到端的机器学习方法，通过对旋转雷达点云与LiDAR地图进行配准来实现定位。我们的前期工作【20】研究了雷达-LiDAR融合，旨在通过滑动窗口平滑器在越野环境中提高鲁棒性，缺点是仅在车体前进方向上增加了速度信息。在【21】中，作者提出了一种将LiDAR特征与雷达最小二乘线速度估计融合的方法（两者具有相同的测量频率），以改善在几何自相似或浓雾环境中的性能。该方法显著改变了LiDAR因子的结构，将位姿单元因子分解为各个特征因子。此更改增加了整体计算成本，同时要求单个雷达测量包含足够的信息来求解3D线速度，而这并不一定适用于所有雷达传感器（例如本文中使用的传感器）。

III. 提出的方法

A. 符号与坐标系

本文使用了多个坐标系及其之间的转换，其中假设与车体固定的坐标系是已知的。坐标系包括世界坐标系（记为 $W$）以及IMU、LiDAR、FMCW 雷达和GPS传感器的车体固定坐标系，分别记为 $I$、$L$、$R$ 和 $G$。

此外，设 $R$ 相对于 $I$ 的位置在 $W$ 中表示为 $ w p{IR} ∈ \mathbb{R}^3$，$R$ 到 $I$ 的旋转为 $ _I R_R∈SO(3)$。再进一步，平移和旋转可以组合成从 $I$ 到 $W$ 的齐次变换：$ _w T_I∈SE(3)$。每个前述的李群（$SO(3)$ 和 $SE(3)$）也都有一个相应的李代数，可以通过指数映射与群上的元素相关联：

$$R=\exp (\tau ),R\in SO(3),\tau \in so(3)\text{\hspace{1em}(1)}$$

以 $SO(3)$ 为例。注意，李代数中的元素 $\tau$ 可以通过楔积（wedge）和对偶（vee）映射与笛卡尔空间中的元素相关联：

$$(\cdot )\wedge :{\mathbb{R}}^a\to a$$

$$(\cdot )\vee :a\to {\mathbb{R}}^a\text{\hspace{1em}(2)}$$

对于具有 $a$ 自由度的任意李代数 $a$，楔积算符会从 ${\mathbb{R}}^3$ 的向量创建一个反对称矩阵。感兴趣的读者可以参考 [22] 和 [23] 来更深入地了解李代数理论。

B. 调频连续波雷达

作为对FMCW雷达的介绍，我们提供了与提出的方法相关的感知原理的简要概述，并参考文献 [24] 以获取全面的概述。

FMCW雷达通过发射一个脉冲，即一个频率调制信号（通常线性随时间调制），并处理返回信号以揭示反射物体的相对位置和径向速度。脉冲开始发射与接收到返回信号之间的时间差与反射物体的距离相关。这个时间差表现为混合信号中的一个恒定频率（由发射和返回配对信号组成）。因此，可以应用快速傅里叶变换（FFT）来分离来自不同距离物体的频率，从而分离范围测量，这个操作被称为范围FFT。通过连续发射和接收多个脉冲，可以通过另一个FFT（称为多普勒FFT）来测量多普勒效应，并检查同一范围检测峰值在多个脉冲中的相位差。

这个相位差与给定物体的多普勒测量 $v_r$（也称为径向速度）相关，作为单个脉冲的发射时间和波长的函数。然后，通过在已知物理位移的天线阵列中发射脉冲，可以解决给定返回信号的方位角和俯仰角。沿给定的方位向量（通过归一化笛卡尔坐标或从方位角和俯仰角计算），范围和多普勒FFT的结果可以重构成一个称为范围-多普勒图（RD-Map）的二维数组。RD-Map 是一个类似图像的数据结构，填充了信号强度，其中行对应于范围值，列对应于多普勒值。

在估计过程中，虽然FMCW雷达生成的点云可能稀疏且噪声较大 [11]，因此难以用于配准，但多普勒测量与雷达传感器与环境之间的相对运动直接相关。这进一步加剧了传感器返回测量覆盖小视场或返回稀疏的问题（如 [20] 中所述），意味着径向速度是一种更为稳健的测量，因为它可以即时融合，而不需要考虑以前的测量。因此，假设环境静态，径向速度测量可以表示为车辆自我运动的函数，并用于辅助惯性导航应用，如下所示：

$$v_r={-}_R{\mu }_R^{\top }{v}_{WR}\text{\hspace{1em}(3)}$$

其中，${}_R\mu$ 是沿单位长度方位向量的径向速度，指向目标在 $R$ 中的三维位置，由雷达传感器的线速度 ${}_R{v}_{WR}$ 引起。图2 显示了 RD-Map 的视觉表现以及相关的坐标系。

图2: 车辆上不同传感器坐标系的可视化以及来自静态物体的雷达返回信号和生成的RD-Map示例。

尽管不同的雷达可能以不同的形式表示测量结果（例如点云、RD-Map等），但上述物理关系依然适用。因此，包含测量值在因子图中的公式（在下一节中描述）实际上适用于几乎所有的FMCW雷达。唯一的要求是雷达提供多普勒和方位信息。方位信息可以是单位方位向量本身或方位角和俯仰角。请注意，由于该因子是多普勒和方位的函数，因此范围测量本身并不是必需的。

C. 状态估计

对于状态估计问题，设 ${\mathbf{T}}_I^W$ $\in$ SE(3) 为从 $I$ 到 $W$ 的齐次变换，${\mathbf{v}}_{WI}^W$ 为 $I$ 相对于 $W$ 的线速度，${\mathbf{b}}_I\in {\mathbb{R}}^6$ 为叠加的加速度计偏差 ($b_a$) 和陀螺仪偏差 ($b_g$)，因此状态空间为：

$$\mathbf{x}=\left[\begin{array}{c}{\mathbf{T}}_I^W\\ {\mathbf{v}}_{WI}^W\\ {\mathbf{b}}_I\end{array}\right]$$

局部切线导航框架的动态方程为：

$$\begin{array}{rl}{\dot{\mathbf{R}}}_I^W& ={\mathbf{R}}_I^W({\tilde{\mathbf{\omega }}}_{WI}-{\mathbf{b}}_g{)}^{\wedge }\\ {\dot{\mathbf{v}}}_{WI}^W& =\mathbf{W}{\mathbf{v}}_{WI}\\ {\dot{\mathbf{b}}}_g& ={\nu }_g\\ {\dot{\mathbf{b}}}_a& ={\nu }_a\end{array}$$

其中 ${\tilde{\mathbf{\omega }}}_{WI}$ 和 ${\tilde{\mathbf{a}}}_{WI}$ 分别为IMU测得的角速度和线加速度，$ \mathbf{g}_w $ 为局部重力向量，随机游走偏差动态由均值为零的高斯噪声源 ${\nu }_g$ 和 ${\nu }_a$ 驱动。注意，状态估计以 $I$ 帧为基础，因此，相对于其他传感器帧的测量必须与相对于 $I$ 表示的物理量相关联。

给定上述状态空间，通过将动态与传感器测量相关联，可以构建估计。在所提出的方法中，这通过基于因子图的滑动窗口平滑器来处理，解决辅助惯性导航问题，其中辅助传感器为LiDAR（提供姿态信息）和FMCW雷达（提供线速度信息）。在组装优化问题时（参见 [25]），设 $I_i$ 为时间 $i$ 处的IMU测量，$L_j$ 为时间 $j$ 的LiDAR里程计（LO）测量，$R_k$ 为时间 $k$ 的雷达测量。此外，设从 0 到时间 $m$ 收集的IMU、LiDAR和雷达的测量时间集合分别为 ${\mathbf{M}}_I^{(0:m)}$、${\mathbf{M}}_L^{(0:m)}$ 和 ${\mathbf{M}}_R^{(0:m)}$。平滑器通过求解以下最小化问题来计算状态集 ${\mathbf{X}}_{(m-l:m)}$ （从 $m$ 到 $m-l$ 由平滑器滞后 $l$ 给出）的最优估计，假设噪声模型为零均值高斯噪声：

$${\mathbf{X}}_{(m-l:m)}^{\ast }=\arg \underset{{\mathbf{X}}_{(m-l:m)}}{\min }\left(\Vert {\mathbf{e}}_0{\Vert }_{{\Sigma }_0}^2+\sum _{i\in {\mathbf{M}}_I^{(m-l:m)}}\Vert {\mathbf{e}}_{I_i}{\Vert }_{{\Sigma }_I}^2\right)$$

其中 $\mathbf{e}$ 和 $\Sigma$ 分别表示残差和协方差矩阵，0 为边际化先验的下标，$I$、$L$ 或 $R$ 为从传感器测量中导出的值的下标。注意，平滑器窗口优化和边际化先验的计算参见 [26] 和 [27]。在因子图中（见图 3），状态估计由通过因子相互连接的节点表示。这些因子通过残差/协方差矩阵对将测量提供的信息编码为状态空间的函数。所提出的方法使用的因子将在以下章节中描述。

图3: 说明因子图中的连接，包括来自IMU、LiDAR里程计和雷达的因子，这些因子与姿态、线速度和IMU偏差状态相连，同时包括平滑器滑动窗口的边际化先验。

雷达因子

雷达测量根据第III-B节处理，并且从给定雷达束的径向速度测量与相应的束中心方向向量配对，以计算上述测量与状态空间之间的关系。传感器线速度与沿方向向量的径向速度之间的关系由公式 (3) 给出，虽然这还不是以状态空间形式表示的。雷达线速度通过考虑旋转参考框架的影响（如[28]所述）与IMU线速度相关联，如下所示：
$${\mathbf{v}}_{WR}={\mathbf{R}}_{RI}\left({\mathbf{v}}_{WI}+{\mathbf{\omega }}_{WI}\times {\mathbf{p}}_{IR}\right)$$
使用IMU-雷达外部变换 { R R I , p I R } ∈ SE ( 3 ) \{ \mathbf{R}_{R I}, \mathbf{p}_{IR} \} \in \text{SE}(3) {RRI​,pIR​}∈SE(3)。这可以用状态空间中的变量和IMU测量展开为：
$${\mathbf{v}}_{WR}={\mathbf{R}}_{RI}\left({\mathbf{R}}_{WI}^{\top }{\mathbf{v}}_{WI}+\left({\mathbf{\omega }}_{WI}-{\mathbf{b}}_g\right)\times {\mathbf{p}}_{IR}\right)$$
因此，给定雷达测量（由多普勒和方向向量组成）的残差误差可以写为：
$$e_R=-{\mathbf{R}}_{\mu }^{\top }{\mathbf{R}}_{RI}\left({\mathbf{R}}_{WI}^{\top }{\mathbf{v}}_{WI}-\left({\mathbf{\omega }}_{WI}-{\mathbf{b}}_g\right)\times {\mathbf{p}}_{IR}\right)-{\tilde{v}}_r$$
这定义了因子的实现及其相对于状态空间成员的雅可比矩阵。雷达径向速度因子的非零雅可比矩阵为：
$$\frac{\partial e_R}{\partial {\mathbf{R}}_{WI}}=-{\mathbf{R}}_{\mu }^{\top }{\mathbf{R}}_{RI}{\left({\mathbf{R}}_{IW}{\mathbf{v}}_{WI}\right)}^{\wedge }$$
$$\frac{\partial e_R}{\partial {\mathbf{b}}_g}=-{\mathbf{R}}_{\mu }^{\top }{\mathbf{R}}_I{\left({\mathbf{p}}_{IR}\right)}^{\wedge }$$
离群点测量通过在因子图中对雷达因子应用Huber损失进行处理。

IMU因子

所提出的方法使用的是[25]中提出的预积分因子，其中包括对姿态 $e_{({\Delta }_w{R}_I)}$、位置 $e_{({\Delta }_w{p}_{WI})}$ 和速度 $e_{({\Delta }_w{v}_{WI})}$ 的残差函数，因此总的IMU残差为：
$$e_I=\left[\begin{array}{c}{e}_{({\Delta }_w{R}_I)}^{\top }\\ e_{({\Delta }_w{p}_{WI})}^{\top }\\ e_{({\Delta }_w{v}_{WI})}^{\top }\end{array}\right]$$
与[25]不同，这个因子图构造方法借鉴了[29]，其中每个IMU消息都会在图中添加一个节点。因此，该因子连接了具有单个IMU测量的节点，而不是将多个测量合并为一个因子。这允许通过时间戳将延迟测量与图中最接近的节点关联，从而最小化此关联所产生的误差。由于节点根据IMU采样率分布，关联所犯的误差得以最小化。

LiDAR里程计测量

在没有绝对位置信息（例如GPS）的情况下，LiDAR里程计（LO）估计被添加到因子图中，作为一元因子的伪全球姿态更新。LO方法以IMU测量和多个LiDAR点云作为输入，在我们的实验中使用了三组点云（第IV-A节）。输入点云在一个共同的时间戳下使用IMU测量进行了去畸变，并使用外部标定在共同的LiDAR框架（L）中表示。合并后的点云用于执行扫描到子图的配准，以估计机器人姿态 ${\mathbf{T}}_{W\hat{I}}$，如[30]所述。姿态一元因子的残差误差为：
$$e_L=\text{log}\left({\left({\mathbf{T}}_{W\hat{I}}\right)}^{-1}{\mathbf{T}}_{WI}\right)$$
其中 $\text{log}$ 是从 $\text{SE}(3)$ 到 $\text{se}(3)$ 的对数映射。

IV. 实验评估

提出的方法在高速度越野机器人导航中的性能通过实际场地实验进行了评估。此外，还使用了公开的森林和矿山雷达里程计数据集 [31] 来与当前最先进的雷达惯性状态估计方法进行比较。

A. 高速度越野机器人实验

提出的方法在NASA喷气推进实验室（JPL）使用修改过的Polaris RZR全地形车进行实验，如图1所示。在实验过程中，收集了来自Xsens MTi-630 IMU（400 Hz）、三台Velodyne VLP-32 LiDAR（10 Hz）、Echodyne EchoDrive FMCW雷达和Swift Navigation Duro GPS的传感器数据。所有传感器通过PTP同步到GPS时间。

EchoDrive雷达测量沿着一个狭窄的方向窗口的RD-Map网格。这个窗口称为束，由3×4个RD-Map组成，每个RD-Map在方位角上跨越1${}^{\circ }$，在仰角上跨越2.5${}^{\circ }$。从每个束测量中提取一个多普勒/方向对，按照[20]中的平方滤波法，将其与一个径向因子一起添加到图中。实验中使用的雷达参数见表◻。

表 I: S21a EchoDrive雷达脉冲的参数。

参数	值
最大范围	100 m
距离分辨率	0.49 m
最大多普勒	43.178 m/s
多普勒分辨率	0.169 m/s
束采样时间	15.8 ms

对于使用JPL车辆收集的数据集（即，Sections IV-A1 和 IV-A2），将车载GPS与IMU融合以创建一个地面真实值进行比较。使用因子图计算车辆姿态、线速度和IMU偏差的估计，该因子图的架构类似于提出的方法，但仅使用IMU和GPS，并使用Levenberg-Marquardt优化器来求解整个轨迹的状态估计，而不是滑动窗口优化。

手动越野驾驶验证实验

为了验证提出方法的性能，使用JPL车辆在JPL校园东区（与[20]中的相同赛道）附近的越野赛道上以手动模式驾驶了5圈，收集了数据集。总行驶距离约为3 km，实验持续约6分钟。在完成5圈赛道后的起始和结束位置大致相同。车辆以平均速度8$m/s$行驶，通过赛道，并达到最大速度12$m/s$。从这次实验中进行了两个调查，以分析提出方法在名义条件和故意降级的LiDAR里程计下的性能。

a) 名义条件：在这个实验中，名义条件指环境提供了足够的几何特征，使得LiDAR里程计方法能够准确执行，而不会出现性能降级。因此，LO方法的姿态估计提供了一个基线，以调查添加雷达信息是否会对整体状态估计质量产生不利影响。该实验的定性和定量结果分别呈现在图4a和表II的顶部行，显示了增加雷达（标记为LRI）的系统表现与没有雷达（标记为LI）的提出方法相当，确认提出的方法在名义条件下不会对状态估计质量产生不利影响。

注意，在这个实验中，优化时间平均为LRI的14.612±0.097 ms和LI的16.211±0.0254 ms，运行在配备Intel 11代i7-11850H的笔记本电脑上。这表明在处理能力显著低于越野车辆的CPU上，来自雷达的额外信息有助于减少单次优化所需的时间。

b) Degraded LiDAR Odometry: 为了展示雷达速度信息对状态估计过程的鲁棒性提升，LiDAR里程计因子在加入状态估计因子图之前被故意加噪。没有雷达因子的情况下，噪声的添加使得状态估计器的输出变得嘈杂，这进一步反馈到点云去畸变中，影响LiDAR里程计模块。所有实验中，噪声是在初始延迟后添加的，以确保IMU偏差的适当收敛。此外，由于雷达速度因子仅提供$x$轴和$y$轴上的线性速度信息，高斯噪声仅添加到这些轴上的LO位置估计中，以 ${\sigma }_{xy}$ 来理解改进速度估计的直接好处。因此，对于每个LO位姿估计，嘈杂的估计 $\widetilde{W{T}_I}$ 被给出为：

$\widetilde{W{T}_I}=W{T}_I\exp ({ϵ}^{\wedge }),ϵ\in \mathcal{N}(0,\Sigma )$

其中 $\Sigma$ 是噪声协方差。

请注意，因子图的参数没有改变以考虑这些添加的噪声，目的是模拟LO质量意外恶化对状态估计过程的影响。不同的噪声值被评估，结果如表[I]所示，对于 ${\sigma }_{xy}=1$ 的结果可视化在图4b和4c中，其中雷达（作为速度传感器整合）对提高速度估计误差有显著影响，使得RMSE几乎保持不变。此外，可以在表II中注意到，这些信息也能够提高位姿估计质量，除了在一个实例中，绝对位姿误差（APE）表现突出。这是由于LRI在z估计误差中的偏差，与其他噪声组合中的LI差异不大。

2. 在具有挑战性的沙漠环境中的越野驾驶: 为了测试所提出方法在实际越野驾驶场景中的适用性，在美国加利福尼亚州Helendale附近的沙漠环境中进行了测试。在这个测试中，JPL车辆行驶了大约4公里的路径，持续时间约为10分钟。在实验期间，车辆的平均速度为6.8 m/s，最大速度为10 m/s。行驶路径穿过相对平坦的沙漠环境，植被稀疏且密度低。因此，这种环境对LiDAR扫描匹配的几何特征有限，除了地面上的特征。此外，轨迹包含了接近40°/s的偏航率的激烈驾驶动作，如IMU测量所示。

这种环境对纯LiDAR-IMU方法提出了挑战，因为平坦区域与稀疏的植被不提供足够独特的几何特征。此外，这一挑战因高速穿越和在这些区域进行的激烈转弯而加剧。因此，LO性能显著下降，并在最困难的路径段出现了发散，如图5a所示。在相同的轨迹上，加入所提出的雷达速度因子显示了增强的鲁棒性，因为额外的信息能够稳定优化并在这个特别困难的区域支持LiDAR里程计，结果是所提出的方法在整个轨迹中保持了可靠的状态估计，并跟踪了车辆的GPS位置。请注意，在挑战性部分，由于雷达未提供沿$z$轴的重要信息，估计质量在$z$轴上的表现受到了影响。

表 II: JPL东区越野赛道和赫伦代尔沙漠实验的姿态和速度估计的均方根误差（RMSE）及标准差

图4: 展示了不同方法相对于GPS在JPL东区实验中的性能比较，包括名义性能（a）以及添加噪声实验（b,c），其中噪声标准差为1米。

图5: 比较不同方法与GPS在JPL Helendale实验中的性能，包括位置（a），车身前向速度（b），以及车身横向速度（c）估计。

B. 雷达惯性里程计与公共数据集的比较

为了展示所提雷达速度因子在挑战性环境中作为单独解决方案的适用性，使用公开的Forest和Mine数据集[31]进行了与最先进雷达惯性方法的比较，这些数据集使用了另一种汽车雷达传感器（Sensrad Hugin A3）。Forest数据集包含一辆车辆在森林环境中行驶，路径长度为1.8公里，持续时间为8.3分钟。车辆在路径上行驶时的平均速度约为3 m/s，最高速度约为7 m/s。Mine数据集则记录了一辆不同的车辆在矿井中行驶13分钟，路径长度为4.5公里。在这个数据集中，车辆的平均速度为5 m/s，最高速度为10 m/s。

Forest和Mine数据集与我们在JPL车辆上进行的实验之间的一个关键区别是使用的雷达传感器类型。[31]中使用的雷达返回一个处理过的点云，每个点都有径向速度测量，点云的大小可以达到10,000个点，覆盖整个传感器的视场。因此，在将因子添加到状态估计器因子图之前，数据必须以不同的方式处理。此外，这些数据集特征包含更多繁忙的环境，因此处理的第一步是找到来自环境中静态物体的雷达点。模拟[31]中提出的方法，假设共识集是静态物体集，我们应用RANSAC解决与雷达线性速度相关的径向速度和方向测量的线性速度最小二乘问题：

表 III: 不同平移增量的平移和旋转中位数RPE，与[31]中的结果进行比较。

其中，${\tilde{v}}_r^n$ 和 $-\mathbf{R}{\mu }_{\tilde{r}}^n$ 是对应于第$n$个点的多普勒和方位角向量。之后，从内点集中选择N个最佳点（根据估计模型的误差）。此外，迭代选择点，以确保新选择的点与先前选择的点之间保持最小的方位角和俯仰角差异，从而确保所选点在传感器视场内分布均匀。

使用中位数RPE作为指标报告森林和矿井数据集的里程计性能，因此我们将比较所提出的方法在这些数据集上的表现，具体来说，是对每个数据集计算的中位数RPE，并取其平均值。表III中显示了其他方法[31]，[28]，[32]的性能指标，以及所提出的方法在数据集上的结果。需要注意的是，这一比较使用了所提出方法的雷达径向速度因子进行雷达惯性里程计，与文献[31]中的大多数比较方法相比，表现优越。特别是与EKF[28]解的比较是有意义的，因为这两种方法都将来自雷达传感器的速度信息与IMU的加速度计和陀螺仪测量进行融合。所提出的方法仅次于IMU+ Doppler[31]，这是一个使用磁力计来限制方向漂移，并根据IMU的方向估计算法整合雷达速度测量的方法。因此，这种方法考虑了所提出方法没有的附加信息，即使如此，所提出的方法仍具有可比的RPE。总之，所提出的方法能够生成平滑、局部准确的估计，与其他雷达惯性方法相竞争。然而，如文献[31]，[21]所示，长期漂移，特别是由偏航或z轴的误差引起的漂移，仍然不可避免，仅依赖于车体框架速度和IMU信息。

结论与经验教训

这些研究为作者带来了几条重要的经验教训。首先，次优或甚至退化的条件可能会突然出现，如Helendale数据集中所示。未能处理这种意外情况的估计算法可能会遭遇性能下降。此外，利用互补的多模态测量源似乎是一种创建鲁棒状态估计架构的有效方法。然而，这种融合方法有时会产生稍微令人意外的结果。例如，在East Lot数据集中，尽管雷达传感器几乎没有提供关于垂直速度的信息，但仍然对整体结果产生了影响。不幸的是，这也意味着在这些实验中，沿垂直方向的准确性改进潜力有限，因为雷达的波束分布没有设计成具有大的垂直视场，并且汽车雷达通常具有更有限的垂直视场。这与文献[20]，[21]中观察到的行为一致，似乎是速度-惯性融合与视场受限的雷达感应的一个限制。

考虑到这一点，本研究提出了将雷达速度测量与LiDAR里程计和IMU测量融合在因子图优化中，以提高机器人在复杂环境下的状态估计器的鲁棒性。该方法在真实世界和人为传感器退化的影响下进行了高速越野驾驶数据集的评估。此外，雷达径向速度因子作为独立雷达惯性里程计方法在包含复杂地下和森林环境的公开数据集上进行了评估，并使用了不同的FMCW雷达传感器。提高了越野场地实验的鲁棒性，并在公开数据集上展示了最先进的比较性能。