来源：3D视觉工坊
添加小助理：dddvision，备注：SLAM，拉你入群。文末附行业细分群
一句话总结：建模Radar测量的不确定性，并将不确定性建模加入到Radar SLAM提高性能。
Radar能够在挑战性条件下有效运作，但是其测量通常是稀疏且带有噪声的点，给SLAM前端数据处理和后端状态估计带来了重大挑战。
SLAM 系统不仅涉及姿态估计，还涉及不确定性建模。不确定性建模涵盖了从前端测量到后端状态的范围，描述了测量和状态的"置信度"。但是大多数现有的雷达 SLAM 系统主要集中于运动估计，即提供给定输入的顺序姿态。作者认为利用雷达测量的不确定性是提高雷达 SLAM 性能的关键。
作者采用了之前设计的雷达惯性里程计（RIO）作为简化但基本的雷达 SLAM。该系统利用了带有速度辅助的雷达点和高频惯性测量。然后，提出通过考虑雷达感知的本质，对极坐标中雷达点的不确定性进行建模。该模型的不确定性在数据关联和加权最小二乘运动估计中得到利用。为了验证有效性，在公开的 Coloradar 数据集和自己的移动平台上进行了真实世界实验。
下面一起来阅读一下这项工作~
标题：Modeling Point Uncertainty in Radar SLAM
作者：Yang Xu, Qiucan Huang, Shaojie Shen, Huan Yin
机构：香港科技大学
原文链接：https://arxiv.org/abs/2402.16082
尽管视觉和基于激光的同时定位与地图构建（SLAM）技术引起了重大关注，但雷达SLAM仍然是应对挑战性条件的稳健选择。本文旨在通过对点不确定性建模来提高雷达SLAM的性能。基本的SLAM系统是一个雷达惯性测程（RIO）系统，利用速度辅助雷达点和高频惯性测量。我们首先提出通过考虑雷达感知的性质，在极坐标中建模雷达点的不确定性。然后在SLAM系统中，不确定性模型被设计成数据关联模块，并纳入以加权运动估计。对公开和自收集的数据集进行的真实世界实验验证了所提模型和方法的有效性。研究结果突显了将雷达点不确定性建模纳入以改善逆境环境中的雷达SLAM系统的潜力。
将不确定性建模加入到SLAM系统，测试轨迹精度：红色轨迹是提出的完整系统，蓝色是无不确定性模型的系统，黑色是真值轨迹。
（1）基于雷达感知的测量模型，对极坐标中雷达点的不确定性进行分析。
（2）将不确定性建模纳入 SLAM 系统中，增强了设计的数据关联和加权残差函数。
（3）对真实世界数据集进行了消融研究，证明了所提出的建模和方法的有效性。
雷达-惯性里程计的因子图表示。从输入的雷达和IMU测量值中构造三个因子，不仅提供了用于位姿估计的估计位姿，还提供了作为建图结果的雷达地标。
雷达测量及噪声模型：
雷达传感器以方位角和俯仰角观测值以及检测到的距离发出观测值，因此使用极坐标测量模型提供了更准确的误差分布制定。给定雷达框架R中的雷达点Rpk，将Rpk分解为两个不同的分量：距离，用rk ∈ R表示，以及方位角和俯仰角，表示为Ωk ∈ S2：
关于距离测量噪声，可以将其建模为均值为零、标准差为σr的正态分布，表示为：δrk ~ N(0, σr^2)。具体而言，σr是距离测量噪声的标准差，包含在定义的置信区间内距离测量的变异性。然后地面真实距离可以表示为：
对于角度测量噪声，将方位角和俯仰角噪声建模为独立的以零为中心的高斯分布，标准差分别为σθ和σφ。因此，Ω的角度测量噪声表示为：δΩk ~ N(0_{2×1}, ΣΩ)。ΣΩ是角度测量的协方差矩阵：
其中σθ和σφ分别是方位角和俯仰角的测量噪声的标准差，可以建立实际方位方向Ωgt_k和其对应测量Ωk之间的连接如下：
其中N(Ωk) = [N1 N2] ∈ R^3×2表示正交基，记作N(Ωk)，在Ωk处的切平面上。此外，符号(·)∧指的是将叉乘映射为反对称矩阵。S2操作涉及将单位向量Ωk绕在Ωk处的切平面上的轴δΩk旋转。此操作确保结果向量仍保持在S2的表面上。
通过以上分析，给定极坐标中的雷达点Rpk，其真实值Rpgt_k的制定如下：
其中Rnk表示此雷达点测量的噪声：
此雷达点的协方差RΣk可以制定如下
到目前为止，已经对雷达测量及其在距离和角度上的噪声进行了建模，即一个雷达点的不确定性建模。
有人可能会争辩说，雷达感知不仅提供距离和角度，还提供多普勒速度。多普勒速度的不确定性建模也是提高运动估计系统的潜在点。然而，多普勒速度测量通常包含非高斯噪声，因此将噪声建模为高斯白噪声将导致不准确的结果。此外，有关数据准确性的文档表明，多普勒速度测量相对精确，因此作者决定在本研究中省略多普勒速度噪声。
不确定性感知多普勒速度残差
当设备以相对较高的速度运行时，融合多普勒速度测量就显得至关重要，因为它可以减小IMU传播误差。在理想情况下，雷达报告的多普勒速度测量应该与雷达和物体之间的相对速度在检测方向向量上的投影相同。这使得我们可以通过融合多普勒速度测量得到残差项：
如果直接融合残差而不加权，会导致里程估计偏离，因为不同的残差对运动估计的"贡献"不同。尽管设定预定义的权重可能在一定程度上改善结果，但由于雷达感知的物理特性，每个点测量的重要性是不同的。较近点的不确定性通常低于较远点。因此，利用不确定性模型，根据其相关的不确定性水平为残差分配权重。这种方法可以更准确地融合残差，并减轻对总体里程估计的不太可靠测量的影响。
将噪声项 积分，得到以下公式：
其中，r_D是多普勒速度的残差，n_k是雷达点测量的噪声，J_{n_k}可以表示为：
残差项的协方差可以确定为：
请注意，这个协方差表示了一个雷达点关于残差项 r_D的"置信度"。例如，较近的雷达点具有较高的置信度，导致在优化问题中有更大的多普勒速度辅助残差，从而使得雷达SLAM系统更加准确。
概率引导的点匹配
数据关联，或点匹配，在机器人姿态估计的基于点的扫描匹配中发挥着关键作用。先前的研究已经证明了跟踪雷达点在连续雷达扫描中的可行性。在之前的研究中，作者在欧几里得空间内采用了基于最近邻的搜索。然而，由于雷达点测量中固有的不确定性，通过这种方法确定的近距离点并不总是在统计意义上对应于最有可能的匹配。雷达点在欧几里得空间内的概率分布由于测量的不确定性模型而呈椭圆形状。因此，到中心点的较短欧几里得距离并不一定表示该位置上点的出现概率更高。这种差异表明，在欧几里得空间中使用最近邻搜索可能会导致状态估计不匹配。
为了解决这个问题，作者提出了一个在概率不确定性模型下概率引导匹配方法。这种方法涉及计算每个点对的成功匹配概率，从而优化数据关联步骤，即点对点匹配步骤。这种方法旨在通过利用测量不确定性的概率分布来减轻传统最近邻搜索的局限性。
具体来说，对于一个以极坐标形式附加在当前雷达姿态中的雷达点pk，一些地图地标lm ∈ X 可以被视为候选。在匹配过程中，计算每个候选地标在雷达点pk周围的概率密度Pk(r, θ, φ)。在提出的不确定性模型中，r，θ，φ被假定为相互独立的高斯随机变量。因此，一个地标lq ∈ lm 的匹配概率是Pk，可以推导为：
然后，要找到的匹配地标是具有最大概率的地标，如下所述：
尽管采用了概率引导的点匹配，还将验证ppaired是否在预期范围内，使用3σ准则。不符合此范围的点被视为异常值并被移除。在移除动态点之后，利用上述方法计算点对，有助于建立连续帧之间的对应关系。
点匹配上的不确定性感知残差
作者将点对点残差项 rP (·) 形式化如下：
其中 lk 是匹配地标的位置；pk 是第 i 次扫描中 lk 的观测点。该残差通过考虑地标和对应测量点之间的距离来形成。
包含噪声项 nk 的公式化导致了以下形式：
其中 rP (·) 指的是点对点匹配上的残差，而 nk 表示一个雷达点测量 pk 的噪声。Jnk 可以如下导出：
然后，残差加权的协方差为：
与前述的残差 rD(·) 类似，每个雷达点都带有不同的噪声，因此提出的不确定性模型导致了对分配给残差 rP (·) 的权重的调整。
到此为止，已经可以回答的两个问题的模型和方法：如何建模雷达测量的不确定性？和如何有效地将不确定性建模纳入雷达 SLAM 系统中？
对提出的概率引导的点匹配进行了消融研究。如果将所提出的匹配集成到系统中，雷达SLAM表现更好。
定位精度对比。
归一化均方根误差在0 ~ 1范围内，x轴表示系数α在0.25 ~ 4之间，如表II所示。
这项工作对雷达点的不确定性进行建模，并将其纳入RIO系统中。具体来说，将不确定性模型用于构建雷达点匹配，并加权残差项用于运动估计。通过对两种不同雷达传感器进行消融研究，实际世界的实验证明了方法的有效性。作者还对不确定性参数设置进行了详细的测试和分析，使得所提出的模型和方法具有可解释性和适用性。
在未来，作者将专注于未来的几个有前景的方向，如多模态融合、多雷达校准和基于图论的数据关联用于鲁棒估计。
对更多实验结果和文章细节感兴趣的读者，可以阅读一下论文原文~
下载
在公众号「3D视觉工坊」后台，回复「 3dcv」,即可获取工业3D视觉、SLAM、自动驾驶、三维重建、事件相机、无人机等近千余篇最新顶会论文；巴塞罗那自治大学和慕尼黑工业大学3D视觉和视觉导航精品课件；相机标定、结构光、三维重建、SLAM，深度估计、模型部署、3D目标检测等学习资料。
3D视觉方向交流群成立啦
目前工坊已经建立了3D视觉方向多个社群，包括SLAM、工业3D视觉、自动驾驶、三维重建、无人机方向，细分群包括：
[工业3D视觉]相机标定、立体匹配、三维点云、结构光、机械臂抓取、缺陷检测、6D位姿估计、相位偏折术、Halcon、摄影测量、阵列相机、光度立体视觉等。
[SLAM]视觉SLAM、激光SLAM、语义SLAM、滤波算法、多传感器融合、多传感器标定、动态SLAM、MOT SLAM、NeRF SLAM、机器人导航等。
[自动驾驶]深度估计、Transformer、毫米波|激光雷达|视觉摄像头传感器、多传感器标定、多传感器融合、自动驾驶综合群等、3D目标检测、路径规划、轨迹预测、3D点云分割、模型部署、车道线检测、Occupancy、目标跟踪等。
[三维重建]NeRF、多视图几何、OpenMVS、MVSNet、colmap、纹理贴图等
[无人机]四旋翼建模、无人机飞控等
除了这些，还有求职、硬件选型、视觉产品落地、最新论文、3D视觉最新产品、3D视觉行业新闻等交流群
大家可以添加小助理：dddvisiona，备注：加群+方向+学校|公司, 小助理会拉你入群。