DopRIO：多普勒技术加持的紧密耦合型4D毫米波雷达-惯性测距系统

《Robotics and Autonomous Systems》：DopRIO: Doppler strengthened tightly-coupled 4D millimeter-wave radar–inertial odometry

　　

Ruiqi Cheng|Huijun Di|Jian Li|Feng Liu|Wei Liang

• •
  提出基于多普勒的雷达点精确化方法，以降低配准误差。
• •
  为每个雷达点引入动态后验不确定性估计方法。
• •
  开发了DopRIO，这是一种基于IESKF的紧密耦合型4D雷达-惯性测距系统。
• •
  通过自主设计的实验和公开数据集验证了DopRIO的性能，并公开了相关代码。

引言

机器人导航与自动驾驶需要可靠的状态估计和精确的测距能力，以便完成后续任务。在室内外环境中，将摄像头、激光雷达及其他传感器的数据融合起来以实现精确姿态估计的技术已经相当成熟[1]。然而，摄像头和激光雷达在雨、雪、雾等恶劣天气条件下性能会显著下降，难以实现全天候运行[2]、[3]、[4]。由于毫米波雷达的波长较长，能够穿透与天气相关的障碍物，因此长期以来一直作为汽车雷达系统中的主要感知模块[5]。传统的毫米波雷达通常利用二维快速傅里叶变换以及到达方向估计，来获取距离、多普勒频移和方位角信息，从而实现目标检测和速度测量[6]。而先进的4D毫米波雷达则增加了天线阵列，用于获取高度信息[7]，从而提供更高的空间分辨率和更多的维度数据。通过将高度信息与距离、方位角和多普勒速度相结合，机器人和车辆可以利用4D毫米波雷达进行测距任务。

与激光雷达相比，4D毫米波雷达具有更远的探测范围、更高的多普勒测量能力，以及更强的全天候工作性能[8]。不过，由于其水平视场角较小，但发射波束较宽，角度分辨率有限，还存在多路径效应和反射问题[9]，因此其生成的点云比激光雷达的更为稀疏且噪声更多。如图1所示，毫米波雷达生成的点云不仅比激光雷达的点云稀疏1到2个数量级[10]（例如，4D雷达每帧仅有100到1000个点，而激光雷达每帧有5万到20万个点），而且不同帧之间的点对应关系也不明确[11]。4D毫米波雷达的帧间数据关联难度远大于激光雷达[12]，这使得直接应用激光雷达点云配准方法并不可靠。

为了解决雷达点云配准带来的诸多难题，研究人员开始利用多普勒信息来提供额外的运动信息。一些方法[13]、[14]、[15]基于多普勒信息假设物体运动速度恒定，以此约束点云的配准结果。还有其他方法[16]、[17]、[18]、[19]、[20]、[21]、[22]则将多普勒速度信息与惯性测量结果相结合，以提高姿态估计的精度。Chen等人[23]假设水平天线阵列之间的相位差是准确的，且雷达的垂直速度为零（即平面运动），他们基于这两个假设以及一维多普勒测量结果来重新计算雷达点的三维坐标。这些方法虽然证明了多普勒信息的有效性，但尚未充分发挥其潜力。

在本文中，我们提出了DopRIO，这是一种强化了多普勒功能的雷达-惯性测距方法，能够充分利用高精度的多普勒信息优势。多普勒速度测量可以为机器人自身的速度状态与雷达点的角度测量之间提供有效的约束（简称多普勒约束）。通过充分运用这些约束，我们的贡献包括：

• 1.
  我们探索了多普勒信息的一种新用途，即用来修正雷达点的不精确角度测量值。除[23]外，以往的研究并未考虑通过修正不精确的雷达点坐标来提升测距性能。与[23]中在特定假设下直接重新计算坐标不同，我们将雷达点修正视为一个基于通用理论原理的推理问题，无需依赖那些假设。实验结果表明，我们的修正方法性能更好。在多普勒约束的引导下，我们利用机器人自身的速度状态在测距过程中不断对雷达点进行修正。经过修正的雷达点有助于降低配准误差，进而提升测距性能。
• 2.
  我们在4D雷达-惯性测距系统中为每个雷达点引入了一种新的动态不确定性估计方法。与现有方法[22]、[24]通过对雷达点不确定性进行统计先验建模不同，我们的不确定性估计是在雷达点修正过程中进行的动态后验估计，能够反映每个雷达点在满足多普勒约束方面的程度。通过量化这种不确定性，我们可以控制每个雷达点在点云配准和状态更新过程中的影响程度，从而有效应对非静态物体以及雷达数据噪声带来的不良影响。
• 3.
  我们提出了一种高效的测距方法DopRIO，该方法在迭代误差状态卡尔曼滤波器框架下将4D毫米波雷达与惯性测量单元紧密结合。我们在DopRIO的IESKF框架内整合了点修正方法和动态不确定性估计方法。通过将雷达点配准和逐点多普勒约束纳入统一的观测函数，我们能够充分利用4D雷达的测量数据，实现低漂移的测距效果。

为了严格验证DopRIO的性能，我们搭建了一个真实的物理实验平台。在该定制平台上进行的实验，再加上在两个公开数据集上的测试，都证明了所提出的测距方法的有效性及其卓越的性能。DopRIO的性能优于现有的4D雷达-惯性测距方法，甚至可与最先进的摄像头/激光雷达-惯性测距方法相媲美。我们已将该方法开源，希望激励更多人开展关于4D雷达测距或同步定位与建图任务的研究。

本文的结构如下：第2节回顾相关研究；第3节介绍所提出的系统；第4节展示实验结果与分析；最后，第5节总结全文并探讨未来的研究方向。

章节概要

基于4D雷达的测距与SLAM

由于具备出色的全天候稳定性，4D毫米波雷达受到了广泛关注。它既能提供三维空间信息，又能给出多普勒信息，因此能够在恶劣环境下为测距系统和SLAM系统实现姿态估计。不过，仅依靠雷达的测距和SLAM系统通常需要将额外的约束条件引入标准的点云配准流程，以弥补雷达点本身分布稀疏的缺陷。有些方法会构建专门的配准约束

系统架构概述

DopRIO的系统架构如图2所示。当惯性测量单元的数据到达后，系统会利用6自由度的IMU数据来更新系统状态。当雷达数据传来时，系统会通过IESKF算法，逐步进行包含多普勒信息的状态更新以及由多普勒信息引导的点云修正。在每次迭代中，都会利用测距系统中的雷达自身速度估计值，在多普勒约束的前提下对雷达点进行修正。同时，每个雷达点的不确定性也会被

数据集与实验设置

物理实验：我们在AGILEX公司的TRACER平台上安装了多个传感器，构建了一台地面机器人用于物理实验，如图5(a)所示。该机器人所使用的毫米波雷达是大陆集团的ARS548型号，其探测范围可达300米，视场角为100°×30°。惯性测量则由HiPNUC公司的CH110 IMU承担。此外，还使用了Robosense Helios 16激光雷达作为真实姿态参考。我们使用笔记本电脑实时运行我们的方法，以实现

结论

在本文中，我们提出了DopRIO，这是一种基于多普勒技术的4D毫米波雷达-惯性测距方法，旨在利用稀疏的雷达数据实现高精度的状态估计。为了解决毫米波雷达本身的噪声问题，我们提出利用精确的多普勒信息来指导雷达点的修正过程。在修正过程中，多普勒信息会持续更新雷达点的不确定性，这些不确定性信息会被用于点云配准和状态更新，从而确保测距的准确性

CRediT作者贡献说明

Ruiqi Cheng：论文撰写——审稿与编辑、论文撰写——初稿、可视化处理、实验验证、软件开发、方法设计、正式分析。 Huijun Di：论文撰写——审稿与编辑、项目指导。 Jian Li：项目指导。 Feng Liu：项目指导、资金筹集。 Wei Liang：项目指导、资金筹集。

利益冲突声明

作者声明不存在任何可能影响本文研究结果的已知财务利益关系或个人关系。

致谢

本研究得到了中国国家自然科学基金 [项目编号62172043]以及中国煤炭科学研究总院科技发展基金重点项目 [项目编号2024ZDI-12]的支持。