# 基于不确定激光雷达数据的占用栅格地图构建 Yuguang Liu $^{ID}$, Marko Radanovic $^{ID}$, Krista A. Ehinger $^{ID}$, 库鲁什·科谢勒姆 (Kourosh Khoshelham) $^{ID}$, 资深会员，IEEE 摘要：高精度地图对于自动驾驶汽车至关重要。一种有前景的自动生成高精度地图的方法是使用车载激光雷达 (LiDAR) 数据进行占用栅格地图构建。现有的占用栅格地图构建方法没有将激光雷达姿态不确定性（由信号遮挡和全球导航卫星系统 (GNSS) 的多径效应引起）集成到几何反传感器模型 (ISM) 更新中。这会导致生成的地图出现系统性和随机误差。为了解决这个问题，本文提出了一种非对称高斯钟形 ISM，该模型可以最大限度地减少由于姿态不确定性引起的占用栅格地图中的系统性和随机误差。我们的非对称高斯钟形 ISM 在真实室内激光雷达数据集和小型室外激光雷达数据集上进行了评估，并使用了两种设计的评估指标。结果表明，所提出的 ISM 有效地降低了两种环境中存在的系统误差。具体来说，我们的 ISM 在室内和室外数据集上分别实现了 7.5 厘米和 5.1 厘米的最低系统误差，激光雷达姿态不确定性分别为 20 厘米和 40 厘米，与五种现有的 ISM 相比，生成了更准确的占用栅格地图，这表明它具有高精度地图相关应用的前景。 关键词：反传感器模型 (ISM)，激光雷达 (LiDAR)，地图评估，占用栅格地图构建，姿态不确定性。 ## I. 引言 占用栅格地图算法广泛应用于移动机器人中，用于将环境表示为具有相关概率值的网格结构的地图。在同步定位与地图构建 (SLAM) 中，它具有计算效率高、与各种传感器兼容以及能够实时适应环境变化等优点 [^1]，[^2]，[^3]。在自动驾驶汽车中，占用栅格地图构建是一种生成高精度地图的实用方法，可用于导航 [^4]，车道级定位 [^5]，以及详细的环境建模 [^6]。然而，占用栅格地图的精度受激光雷达姿态不确定性的影响，这可能由 GNSS 误差、运动估计误差和校准误差等因素引起，可能导致错位和安全隐患，从而影响自动驾驶等应用 [^7]。在本文中，我们重点关注激光雷达姿态不确定性的位置（平移）分量，该分量可以投影到光束方向并建模为 ISM 中的范围误差，而姿态不确定性则未被考虑。 为了减轻姿态不确定性对地图的影响，研究人员通常采用传感器融合等技术 [^8]，以及使用粒子滤波算法 [^9]，[^10] 来提高姿态估计的准确性。同时，一些研究侧重于直接将姿态不确定性集成到占用栅格地图构建过程中，例如高斯过程 (GP) [^11]，[^12]，[^13] 和模糊逻辑框架 [^14]。 逆传感器模型（ISM）是占用网格映射算法的一个重要组成部分，它将每个测距转换为每个单元格的占用概率，并以对数优势（通过概率钳制以实现可更新性）来更新网格。由于精确的ISM，即使用贝叶斯定理计算每个网格单元的占用概率，涉及很高的计算复杂度[^15]，因此，几何ISM被广泛用作近似方法。它们使用几何规则来确定基于传感器读数的网格单元被占据的可能性，例如，对于测量不确定性，使用高斯钟形曲线，或者对于激光雷达传感器中的理想情况，使用阶梯形曲线[^1]。然而，在之前的研究中，几何ISM主要考虑了小的随机误差，例如测距误差[^7]，而不是姿态不确定性，而姿态不确定性可能非常大，并在占用网格图中引入系统误差。 在本文中，我们提出了一种新的几何ISM，通过调整传统的几何ISM以适应测量不确定性，将激光雷达姿态的不确定性纳入到占用网格图中。我们的方法在映射过程中直接适应来自激光雷达传感器的姿态不确定性，并且它通过保持占用网格框架的简单性，与其他方法区分开来。修改在于更新单元格占用概率的过程，而不是姿态估计。 本文的主要贡献有以下几点。 1) 我们证明并量化了在激光雷达姿态不确定性下，传统几何ISM引起的系统误差和随机误差。 2) 我们提出了两种新的评估指标，用于测量占用网格图的随机误差和系统误差。 --- 3) 我们提出了一种新的ISM，它可以最大限度地减少系统误差，并通过真实的室内和室外激光雷达数据集，定量地展示了其优越性。 虽然我们的实验是在小规模的室内和室外地图上进行的，而不是在全尺寸的高精地图上，但提出的ISM可以直接集成到大型激光雷达地图和高清地图生成框架中，这留待未来的研究。此外，姿态不确定性的方向及其对地图的影响在本工作中未进行建模，这也留待未来的研究。 本文的其余部分组织如下。第二节回顾相关工作。然后，我们在第三节中展示了现有几何ISM在激光雷达姿态不确定性下的系统误差和随机误差，并在第四节中提出了新的ISM并确定了所提出的ISM的参数。第五节展示了两个新的评估指标以及2-D地图结果的摘要。第六节讨论实验结果，第七节总结本文。 ### II. 相关工作 ### A. 传统的占用网格映射 Elfes 等人在 [^1] 首次提出了用于占用栅格地图的方法的概率框架，该框架将环境分解为均匀间隔的网格单元，并且网格中的每个单元都与一个概率值相关联，以表示使用二元贝叶斯滤波器和马尔可夫假设的占用概率。 但是，早期的占用栅格地图算法受到局限性，这些局限性是假设单元彼此独立，并且精确姿态是已知的 [^15]。 在这种情况下，地图更新基于几何 ISM，但正如 Thrun [^16] 所述，当使用已知但具有不确定性的机器人姿态进行映射时，这些方法无法适应姿态不确定性。 ### B. 占用栅格地图中的不确定性建模 Thrun 等人 [^15] 提出了 EKF-SLAM，该方法假设在里程标感知和机器人运动中存在高斯噪声，以将扩展卡尔曼滤波器应用于估计机器人的姿态和里程标位置，但该方法受到非线性运动和观测模型的限制。 里程标是在环境中使用的、可识别的特征，用作参考点以构建一致的地图。 Montemerlo 等人 [^17] 进一步提出了 FastSLAM 方法，该方法采用 Rao–Blackwellized 粒子滤波器 [^18]，其中每个粒子代表关于机器人轨迹和地图的假设，但 FastSLAM 存在粒子退化和耗尽的问题 [^19]。 在 FastSLAM 的基础上，Reineking 和 Clemens [^20] 开发了使用 Dempster–Shafer 理论的证据 FastSLAM，该理论使用置信函数来量化由于相互矛盾的测量和缺失的测量而引起的各种不确定性维度，但它没有量化姿态不确定性的影响。 与证据栅格地图并行，Herrero-Pérez 等人 [^14] 使用模糊逻辑来解释测量不确定性和定位不确定性，但其目的是估计机器人的位置，而不是使用定位不确定性来映射环境。 Joubert 等人 [^7] 提出了一种方法，将姿态不确定性直接从 SLAM 系统集成到占用栅格地图中。 该方法使用来自 FastSLAM 或 EKF-SLAM 返回的姿态分布中的样本，然后调整占用栅格更新方程，以使用蒙特卡洛近似来适应这些样本。 该方法通过将不确定性从机器人的姿态估计过程集成到占用栅格地图中，从而使占用栅格地图受益。 它将理想 ISM [^1]（该方法为每个光束中的每个单元分配概率，以表示自由、命中和未知单元，采用阶梯模型）与以读取位置为中心的 Gaussian 卷积，以表示对范围测量值的置信度，而不是对传感器姿态的置信度。 局限性在于，它没有量化这种额外信息对地图的影响，这限制了其实际应用。 另一种建模传感器不确定性是在已知限制内的假设传感器误差的方法。 Langerwisch 和 Wagner [^21] 开发了一种 ISM，该方法利用区间分析来包含测量和姿态不确定性。 但是，与典型的概率模型相比，映射结果的精度较低，并且该模型没有考虑传感器在模型中的位置不确定性。 ### C. 非深度学习驱动的占用栅格地图 机器学习技术已被广泛应用于占用网格映射，以提高在复杂环境中的适应性和鲁棒性。独立性假设简化了空间中的映射问题，将其视为单元上的二元估计问题，但它忽略了源于障碍物物理结构的单元之间的空间相关性 [^22]。为了解决此限制，O'Callaghan 和 Rosman [^11] 采用 GP 方法生成一个连续的、非线性的、非参数函数，以模拟环境中占用情况，因为它可以通过数据集学习空间依赖性。在 [^23] 中，提出了一种将训练输入中的不确定性纳入通用 GP 内核函数的方法，但该方法受到训练过程中的高计算成本的限制，并且对所选的学习样本有很高的要求。 ### D. 基于深度学习的占用网格映射 最近，基于深度学习的 ISM 已成为占用网格映射领域的新前沿。Van Kempen 等人 [^24] 开发了一种基于模拟的端到端 ISM，该 ISM 直接从 LiDAR 点云预测明显的占用网格，从而学习推断每个单元的置信度，并提高对未观察但可推断区域的占用估计。Wirges 等人 [^25] 提出了一种基于深度学习的 ISM，该 ISM 增强了从 LiDAR 等测距传感器获得的单视图占用网格，使网络能够生成类似于多视图地图的附加置信度结构，从而弥补遮挡和视野受限的问题。Jang 等人 [^26] 采用一种深度融合网络，该网络结合 LiDAR 点云和相机图像，以生成具有语义信息的动态占用网格，有效地学习了一种多模态 ISM --- 该网络将融合的传感器数据映射到每个单元的占用、运动和类别标签。Schreiber 等人 [^27] 提出了一种多任务循环神经网络，该网络以鸟瞰图表示形式将 LiDAR 数据的序列作为输入，并直接预测具有速度和语义信息的动态占用网格，从而端到端地学习一种 ISM，并克服了几何 ISM 在建模物体形状和自由空间方面的局限性。 然而，这些深度 ISM 也存在一些显著的局限性。在上述大多数工作中，车辆和传感器姿态被视为由数据集或模拟器提供的，因此网络主要对测量噪声、遮挡和语义歧义进行建模，而没有解决由姿态不确定性引起的地图误差 [^24]、[^25]、[^26]、[^27]。此外，深度 ISM 通常需要大型、精心策划的训练数据集、非平凡的网络架构以及运行时硬件加速器，并且很难对其行为进行分析或将其与经典的概率占用网格框架联系起来。 总体而言，使用几何信息场 (ISM) 的传统占用栅格映射方法以其鲁棒性和易于实现的特点而闻名，但它受到其无法适应姿态不确定性的限制。诸如 Gmapping (基于网格的快速SLAM) [^28]、证据网格图 [^29]、模糊逻辑映射 [^14] 和 GP 占用栅格图 [^30] 等技术考虑了姿态不确定性，但它们要么努力在映射之前改进姿态估计，要么牺牲了传统占用栅格框架的简单性。基于深度神经网络的信息场 (ISM) 具有比几何 ISM 更好的性能，但如上所述，它们通常假设传感器姿态已知，并且没有明确分析姿态不确定性如何扭曲生成的占用栅格。 总而言之，相对较少的研究直接将姿态不确定性纳入到用于逐束几何 ISM 的细胞级栅格更新中。考虑到这一知识差距，本文解决了具有不确定激光雷达姿态的几何 ISM 占用栅格映射问题，并提出了一种分析上合理的 ISM，该 ISM 明确地补偿经典对数先验框架中的姿态不确定性。 ### III. 激光雷达位置不确定性引起的地图误差 本节演示了在不确定激光雷达姿态下的现有几何 ISM 在 1 维和 2 维映射中的性能。我们使用五种在占用栅格映射中广泛采用的 ISM，即八叉树 ISM [^31]、右截断高斯钟形 ISM [^32] 和 Cartographer ISM [^33]。八叉树 ISM 和 Cartographer ISM 都是理想 ISM，但参数不同。这五种 ISM 可以提供基线，以显示姿态不确定性如何在 1 维情况下引起地图误差。假设电池尺寸为 1 厘米。 ### A. 八叉树 ISM 八叉树 ISM [^31] 用于 3 维占用栅格映射中的激光雷达传感器。该模型在无噪声条件下定义了栅格单元的占用概率，假设传感器姿态精确且没有测量不确定性。八叉树 ISM 的数学定义见以下公式 [^31]： $$ P_{occ}\left(z|Y_{lidar}\right)=\begin{cases}0.4,&0\leq z(a) ![Image](images/88851ebe4fa0404d54a4486b6eaf854d780852ebfdd77b31ee3899d0272af50b.jpg) ![Image](images/dd2b178a741f6b3e14c9daf982f805b7dc088d2a25056b7d495bd145ec897d21.jpg)
(b)
![Image](images/063306838cdf9876bd84ea06ad3901281ff3095c39be6fe2ce55f1ca71fe7cf5.jpg)
(c)
![Image](images/4af254864254c87f95bb3db589f4133403971ffec90fb6ef5ed62b91b22e6655.jpg)
(d)
(e)
![Image](images/9ea2e79c202db17a4d872e61397ebfc55b0da9725e106a3147f7f0db4047d8f9.jpg)
(f)
![Image](images/c0096b7805e2f4cc6580955985725053e50af8108efc1167b75d7f3f5527031a.jpg)
(g)
![Image](images/3f381c5f56da673305c997e30926bbabf6c3d4bb0ae0922cf781e3800fc423d1.jpg)
(h)
![Image](images/f5adc4c2b324bc748b006477acc9572fc4403a0d7bb4332775a648cd8a5e7a29.jpg)
(i)
![Image](images/3522944f1c37ce2594960468113ec891503c3c5fd0c6397bdc4063bff202830c.jpg)
(i)
Fig. 1. Peak probability shifts and obstacle width increases of the five existing ISMs with LiDAR position uncertainty ( $\sigma_{sensor} = 5$ cm, cell size = 1 cm, and $z^2 = 100$ cm). (a) Octomap ISM (probability clamp [0.12, 0.97]). (b) Gaussian Bell ISM ( $\lambda = 0.9$, $\eta = 0.1$, and probability clamp [0.1, 0.9]). (c) Cartographer ISM (probability clamp [0.1, 0.9]). (d) K-NN ISM ( $p_{min} = 0.14$, $p_{m,out} = 0.38$, $p(s) = 0.8$, $s_c = 40$ cm, and probability clamp [0.02, 0.98]). (e) Supereigh2 ISM ( $p_{min} = 0.2$, $p_{max} = 0.8$, $\sigma(z') = 2$ cm, $\tau(z') = 10$ cm, and probability clamp [0.12, 0.97]. (f) (j) Noisy observations in red and the combined probability in blue. Ideally, the highest combined probability should be at the actual position of the obstacle (100 cm), but these five ISMs produce a peak probability shift away from 100 cm and an increase in obstacle width.
### C. Cartographer ISM Cartographer SLAM [^34] 通过图优化 [^35] 处理姿态误差估计的累积，而不是使用粒子滤波器 [^36]。在映射部分，Cartographer SLAM 使用 Cartographer ISM [^33] 来更新栅格地图，并使用概率钳制来限制地图上占用概率在 0.1 和 0.9 之间。以下方程显示了 Cartographer ISM： $$ P_{occ}\left(z|Y_{lidar}\right)=\begin{cases}0.49,&0\leq z s_c$ 时）的漏检概率。 在命中位置 $z = z^*$，占用概率由 $p(s)$ 给出（式 5），其中 s 是对应点的 K-NN 平均距离。 位于命中位置后面的单元格以 0.5 的概率保持未知。 图1(d)显示了K-NN ISM，它为自由空间更新定义了两个不同的值。内区域的概率 --- 是 0.14，由 $p_{m,\text{in}}$ 决定，外部区域为 0.38，由 $p_{m,\text{out}}$ 定义。 距离阈值为 40 厘米，由 $s_c$ 确定，命中概率假定为 0.8，由 $p(s)$ 给出。 如 Fig. 1(i) 所示，经过 1000 次测量，在与先前 ISM 相同的姿态不确定度下，由组合概率峰值指示的障碍物变得更宽，并从真实位置偏移。 ### E. Supereight2 ISM Supereight2 ISM [^38] 模拟了测量不确定性和每个光束沿线的有限壁厚。 与固定命中和漏检概率不同，Supereight2 ISM 使用一个分段线性函数来描述与测量表面之间的距离。 这在表面前面形成一个自由空间带，在其后面形成一个有限厚度的占用带，以及其他地方是未知空间。 对于每个 LiDAR 光束，令 $z^{*}$ 为返回值的测量距离，r 为沿该光束的查询点距离。 我们定义有符号距离 $$ d_{r}=r-z^{*} $$ 当在测量的表面 $(r < z^*)$ 前面时为负，在后面 $(r > z^*)$ 时为正。 测量的范围不确定性由 $\sigma(z^*)$ 描述，壁厚由 $\tau(z^*)$ 描述。 在概率水平上，该模型指定了三个特征值：一个强的自由空间概率 $p_{\min} < 0.5$、一个中性值 0.5 和一个强的占用概率 $p_{\max} > 0.5$。 沿单个光束，占用概率由 $$ P_{o c c}\left(d_{r}\mid Y_{l i d a r}\right)=\left\{\begin{aligned}&p_{\min},&&d_{r}\leq-3\sigma(z^{*})\\ &f_{f r o n t}\left(d_{r}\right),&&-3\sigma(z^{*})\tau(z^{*})\end{aligned}\right. $$ $$ f_{front}\left(d_{r}\right)=p_{min}+\frac{d_{r}+3\sigma\left(z^{*}\right)}{3\sigma\left(z^{*}\right)}\ (0.5-p_{min}) $$ $$ f_{back}\left(d_{r}\right)=0.5+\frac{2d_{r}}{\tau\left(z^{*}\right)}\left(p_{max}-0.5\right). $$ 测量表面前方较远位置的体素会被更新为$p_{\min}$对应的自由空间，概率在到达表面处线性增加至0.5，随后在占据带$p_{\max}$的前半段从0.5上升至$\frac{1}{2}\tau(z^{*})$，并在到达$p_{\max}$之前保持为$\tau(z^{*})$，最终对于更靠后的点，概率会回到未知值0.5。 为了随时间融合测量值，我们遵循 Supereight2 中使用的加权贝叶斯更新 [^38]。 对于每个单元 $M_p$，来自 ISM 的单帧对数先验更新 $l^k(M_p)$ 与运行平均值 $\bar{L}^{k-1}(M_p)$ 结合，使用一个上限权重 $w_k$ $$ \begin{aligned}\bar{L}^{k}\left(M_{p}\right)&=\frac{\bar{L}^{k-1}\left(M_{p}\right)w_{k-1}+l^{k}\left(M_{p}\right)}{w_{k-1}+1}\\w_{k}&=\min\left\{w_{k-1}+1,w_{\max}\right\}.\end{aligned} $$ 累积对数先验为 $$ L^{k}\left(M_{p}\right)=\bar{L}^{k}\left(M_{p}\right)w_{k} $$ ![Image](images/7a48f6d90b5afb173cdeba4895fd0deec9bd894b37087d1f0356e68e023c26db.jpg)
Fig. 2. Proposed asymmetric Gaussian Bell ISM ($\sigma_{\ell},\sigma_{r},\lambda$, and $\eta$ are assigned arbitrary values; $z^{*}=100$ cm).
该值保持平均对数先验，同时将置信度限制在最多 $w_{max}$ 个有效观测值。 图 1(e) 描绘了具有分段线性形状的 Supereight2 ISM。在图 1(j) 中，1000 次观察后的组合概率显示，占据区域变得明显更宽，并且位于实际位置的后面。 ### IV. 提出的ISM 峰值偏移问题是由以下事实引起的：现有的三种ISM为返回距离$z^{*}$两侧的单元格分配了不相等的概率增量。当带有噪声的观测值在对数优势下融合时，累积贡献在$z^{*}$周围产生不平衡的偏差，表现为峰值概率偏移。因此，我们重新设计了ISM，以便累积贡献在$z^{*}$周围达到平衡。提出的ISM由不对称的左侧和右侧叶组成，定义为 $$ w_{\ell}\left(z,z^{*}\right)=\exp\left(-\frac{\left(z-z^{*}\right)^{2}}{2\sigma_{\ell}^{2}}\right)\times(\lambda-\eta)+\eta $$ $$ w_{r}\left(z,z^{*}\right)=\exp\left(-\frac{\left(z-z^{*}\right)^{2}}{2\sigma_{r}^{2}}\right)\times(\lambda-0.5)+0.5 $$ $$ P_{o c c}\left(z|Y_{l i d a r}\right)=\begin{cases}w_{\ell}\left(z,z^{*}\right),&0\leq z\leq z^{*}\\ w_{r}\left(z,z^{*}\right),&z^{*} 0.65\}$ 为位移和加厚地图上占据的单元集合，设 $N = |\Omega|$. 对于每个单元 $i \in \Omega$，定义其带符号距离 $$ d_{i}=s_{i}\min_{j\in\Omega_{ref}}\left\|\mathbf{x}_{i}-\mathbf{x}_{j}\right\|_{2} $$ $$ s_{i}=\left\{\begin{aligned}&-1,&&obstacle~moves~toward~the~trajectory\\ &+1,&&obstacle~moves~away~from~the~trajectory\end{aligned}\right. $$ 其中 $\Omega_{ref}$ 是参考地图中的占据集合。 然后，MSD 为 $$ \mathrm{MSD}=\frac{1}{N}\sum_{i\in\Omega}d_{i}. $$ 较大的 MSD 反映了障碍物系统位移的强度（如果为负数，则向内；如果为正数，则向外）。 均方绝对偏差：为了隔离 MSD 捕获的位移，从每个带符号距离中减去位移，残差的离散度量化了障碍物的厚度。定义残差 $r_{i}=d_{i}$-MSD。然后，均方绝对偏差 (MAD) 为 $$ MAD=\frac{1}{N}\sum_{i=1}^{N}|r_{i}|=\frac{1}{N}\sum_{i=1}^{N}|d_{i}-MSD|. $$ 较大的 MAD 表明随机离散性更强，障碍物更厚；而接近于零的值表明非常少的散射和狭窄的障碍物。 本文侧重于 MSD，因为它是两种指标中更重要的指标，因为它会导致地图中障碍物位置不正确。 一旦强制执行 $\sigma_{\ell}$ 和 $\sigma_{r}$ 之间的关系，就可以通过检查其产生的 MSD 和 MAD 值来选择 $\lambda$ 和 $\eta$，从而在 1-D 模拟实验中评估所提出的 ISM 的性能。 图 3 呈现了 MSD 和 MAD 作为 $(\lambda,\eta)$ 函数的等高线图。图 3(a) 中每个 $(\lambda,\eta)$ 对都位于蓝色曲线，对应于零 MSD。该曲线是通过数值方法获得的，通过采样 $\lambda$，并对每个值搜索 $\eta$，使得 MSD 为 0。在这些候选值中，选择 (0.941, 0.136)，因为同时在图 3(b) 中显示相对较低的 MAD，并且避免在地图更新过程中出现极端的对数几率值。 --- ![Image](images/f2cd36071361a171fbb6338633c0dbdec9d378e8831f79bec1309578630f2a06.jpg) ![Image](images/736b7c24be392f30a5f0cac99a4e23a2abc218fc38d08a6ae1bab20ccda286b2.jpg)
(a)
(b)
Fig. 3. MSD and MAD contour maps for different $\lambda$'s and $\eta$'s ( $\sigma_{sensor} = 20$ cm and $z^{*} = 100$ cm). (a) MSD contour map. (b) MAD contour map.
![Image](images/e5a20e0b0a6e6cd4d9f217c1fe6aba1fc05207a15bc5d4fa461df1fc5f6181a0.jpg)
Fig. 4. Peak probability shifts and obstacle width increases of the proposed ISM with LiDAR position uncertainty ( $\sigma_{\ell} = \sigma_{sensor} = 5$ cm, $\sigma_r = 1.743$ cm, $\lambda = 0.941$, $\eta = 0.136$, cell size = 1 cm, and $z^* = 100$ cm).
在所有后续的室内和室外实验中，使用此参数对，无需进一步调整。 (31) 中的立方函数是零 MSD 等高线的一个紧凑的解析近似。它允许用户沿此曲线插值可接受的 $(\lambda, \eta)$ 对，而无需重新运行完整的 MSD/MAD 网格搜索 $$ \eta\left(\lambda\right)\;=\;1\;-\;\left(4\lambda^{3}\;-\;6\lambda^{2}\;+\;3\lambda\right). $$ ### C. 提出的 ISM 的峰值概率位移 类似于图 1(f)–(j)，图 4 显示了在 1-D 中，所提出的 ISM 的系统误差和随机误差。给定 $\sigma_{sensor}$ 为 5 厘米时，根据 (26) 和 (31)，可以确认 $\sigma_{\ell}$、$\sigma_{r}$、$\lambda$ 和 $\eta$ 的值。可以观察到，最高概率恰好位于障碍物的实际位置（100 厘米），没有系统误差，这验证了所提出的 ISM 的有效性。 ### V. 实验 ### A. 2-D 地图评估 图 5 中的 2-D 地图是室内环境的，基于宾夕法尼亚大学提供的实际里程计和 LiDAR 测量结果 [^39]。图 5(a) 显示了使用 Octomap ISM 的参考地图，其中 LiDAR 姿态精确，参考地图上白色障碍物的形状和位置清晰且精确。图 5(b)–(f) 显示了使用 Octomap ISM、高斯钟 ISM、Cartographer ISM、K-NN ISM 和 Supereight2 ISM 的地图，这些地图中 LiDAR 位置不确定性会影响结果。如图所示
TABLE I
QUANTITATIVE EVALUATION OF MAP ERROR ON THE INDOOR DATASET
ISMsMSD (cm)MAD (cm)
Octomap (Fig. 5b)19.219.7
Gaussian Bell (Fig. 5c)31.131.3
Cartographer (Fig. 5d)16.618.2
K-NN (Fig. 5e)12.819.0
Supereight2 (Fig. 5f)26.419.8
Proposed (Fig. 5g)7.510.4
TABLE II
QUANTITATIVE EVALUATION OF MAP ERROR ON THE OUTDOOR DATASET
ISMsMSD (cm)MAD (cm)
Octomap (Fig. 6b)11.924.8
Gaussian Bell (Fig. 6c)13.434.3
Cartographer (Fig. 6d)8.421.4
K-NN (Fig. 6e)14.226.5
Supereight2 (Fig. 6f)20.929.0
Proposed (Fig. 6g)5.125.3
，地图上红色轨迹表示姿态不确定性，该不确定性由遵循零均值高斯分布的噪声模拟，标准偏差为 20 厘米。噪声水平（$\sigma_{sensor} = 20$ 厘米）对应于 2-D 地图中两个像素（像素尺寸 10 厘米），从而为分析提供了适中且可见的姿态不确定性。与参考地图相比，图 5(b)–(f) 中，障碍物（尤其是墙壁）会远离 LiDAR 传感器轨迹，导致图 5(b)–(f) 中出现更宽的空隙，并且它们的厚度增加，这与图 1(f)–(j) 中概率峰值偏移和宽度增加的情况一致。图 5(g) 显示了所提出的 ISM 生成的地图。它显示了较小的概率峰值偏移，并且与图 5(b)–(f) 相比，障碍物更薄。 如表 I 所示，图 5 中每个地图的系统误差（MSD）和随机误差（MAD）均列出，省略了参考地图图 5(a)。由于图 5(a) 被视为评估中的参考地图，因此与自身相比，它的系统误差和随机误差为零。图 5(c) 显示了最高的 MSD 和 MAD，这对应于其最明显的偏移和最厚的障碍物，通过视觉检查可以观察到。相比之下，图 5(g) 在 MSD 和 MAD 方面表现最佳，其次是图 5(d) 和 (e)。总的来说，表 I 表明，当 $\sigma_{sensor}$ 达到 20 厘米时，与现有 ISM 相比，所提出的 ISM 在 MAD 和 MSD 方面均表现最佳。 图 6 显示的 2-D 户外映射结果基于 KITTI 里程计数据集 [^40]。由于 KITTI 为前 11 个序列提供了地面真实姿态，因此我们选择序列 05 的一部分，该序列是在室内环境中收集的，用作另一个数据源，以验证我们方法的泛化能力和实际部署能力。从 3-D 点云中提取 2-D 点云作为平面 LiDAR 扫描。 图 6(a) 显示了使用 Octomap ISM 的参考地图，该地图显示道路上清晰且薄的障碍物表面。噪声水平（$\sigma_{sensor} = 40$ 厘米）对应于地图中的四个像素（像素尺寸 10 厘米）。与参考地图相比，如图 6(f) 所示，障碍物在平均上会偏移 2 个像素，而与实际位置不同。 --- ![Image](images/94efe0db629510e9b6090d066536a7a56e30dac970b57c06b9c8a7e33559a5ee.jpg)
(a)
![Image](images/0b036fd35a8b3ce8be4dd24583e2d92dc45ee31df8205f22649c23c0e499e690.jpg) ![Image](images/c7f61959377f6b078302c49961f664bdc83ea056b46b83cbe76a1d18d7864dc9.jpg)
(b)
(c)
![Image](images/641f2fbb2dd75faef5075f2fa2e70d12be7ea7fb1196e3440076d80bf06ff19f.jpg)
(d)
![Image](images/2abb6f95c8cabc4635baffd184a0081820aac611730961982969152e86703af2.jpg)
(e)
![Image](images/91a4540cf56cb0abc0fa034988a3c84cccc9743b6b2b51dbbe5de5e91ab352b5.jpg)
(f)
![Image](images/782b1ef6ccc56a0d0b3e316636e39cea7f7ba1a013f589b67c247b2058f68fc8.jpg)
Fig. 5. 2-D maps based on the dataset from the University of Pennsylvania using different ISMs under LiDAR position uncertainty ( $\sigma_{sensor} = 20$ cm and pixel size = 10 cm). (a) Reference map generated by the Octomap ISM with no trajectory noise exhibits no probability peak shift and thin obstacles. (b) Map generated by the Octomap ISM with the noisy trajectory shows a significant peak shift (note the wider empty spaces) and obstacle thickening. (c) Map generated by the Gaussian Bell ISM with the noisy trajectory exhibits a strong peak shift and obstacle thickening. (d) Map generated by the Cartographer ISM with the noisy trajectory exhibits a strong peak shift and obstacle thickening. (e) Map generated by the K-NN ISM with the noisy trajectory exhibits a weak peak shift and obstacle thickening. (f) Map generated by the Supereight2 ISM with the noisy trajectory exhibits a strong peak shift and obstacle thickening. (g) Map generated by the proposed ISM with the noisy trajectory exhibits minimal peak shift and obstacle thickening.
(g)
如图 II 表格所示。图 6(c) 仍然显示最大的障碍物厚度，这与其 MAD 值一致（如图 II 表格所示）。图 6(g) 使用所提出的 ISM 方法创建，显示了一个小于一个像素的不可见峰值概率偏移，并且障碍物的厚度与其他 ISM 方法相比处于中等水平。所提出的 ISM 方法的 MAD 值仅略高于 II 表格中的最佳结果。 ### B. 2D 地图误差比较 (1)、(2)、(4)、(6)、(7) 和 (13) 中的六种 ISM 方法，在不同的 LiDAR 位置不确定性水平下应用于地图绘制过程。 对于室内环境，图 7 显示了当单元格尺寸为 10 厘米时，各种位置不确定性水平下的结果，图 8 显示了当单元格尺寸为 5 厘米时的情况。可以观察到，当 LiDAR 位置不确定性大于 5 厘米时，所提出的 ISM 方法在系统误差（MSD）和随机误差（MAD）方面表现最佳，与其他的 ISM 方法相比，在两种单元格尺寸下都是如此。 图 9 显示了当地图单元格尺寸为 10 厘米时，室外环境的结果。它表明，当 LiDAR 位置不确定性较大（大于 10 厘米）时，所提出的 ISM 方法具有最低的系统误差，并且在这些 ISM 方法中，其随机误差是适中的。 ### C. 运行时比较 实验是在一台配备 Intel Core i5-13500H 处理器和 16 GB RAM 的笔记本电脑上进行的。
TABLE III
RUNTIME COMPARISON FOR ISMS
ISMsIndoor (ms per scan)Outdoor (ms per scan)
Octomap7.6327.049
Gaussian Bell14.36214.298
Cartographer7.4757.189
K-NN11.33010.577
Supereight217.67715.146
Proposed ISM12.80313.840
该系统运行 Windows 11 (24H2) 和 MATLAB R2024a。表 III 报告了在图 5 和 6 中的室内和室外数据集上，地图绘制流程的每个扫描的运行时。 Octomap ISM 和 Cartographer ISM 的速度比其他 ISM 方法更快，因为它们都使用具有不同概率值的阶梯式理想 ISM 方法。相比之下，高斯钟形 ISM 方法的运行时明显较长。这是因为高斯钟形 ISM 和所提出的 ISM 方法都使用高斯钟形内核，与阶梯式模型相比，计算量更大。Supereight2 ISM 的速度最慢，因为它使用距离相关的分段线性模型，计算量最高。 在运行时方面，所提出的非对称高斯钟形 ISM 介于阶梯式模型和 Supereight2 模型之间，并且在实践中仍然非常轻量。在我们的实现中，每个扫描的运行时都低于 20 毫秒，这在两个数据集上都适用（见表 III）。因此，对于扫描周期为 20 毫秒（50 赫兹）的 LiDAR 传感器，所提出的 ISM 方法足够高效，可以实现实时占用网格映射。 --- ![Image](images/a8fd6dd7d5e4b2749c32cab033712475a79be90a3ee315ed2192dc3a9322bb0e.jpg)
(a)
![Image](images/9d807baec2aa73fd7bca416d093d316dc0c216fdb073452cd23b52cf2ab33949.jpg) ![Image](images/ddc2abe9d9981e369fe0c1c705af00572a81e837ba11847d817c95b506781dea.jpg)
(b)
(c)
![Image](images/943341cb134c4f05ca62f910cf64948e15bab5f1ea6f7ab16878d83f625484a1.jpg) ![Image](images/34a6857f8d6b6ef59dfa2dc26bd0f633abe250f0770fd15d32b5ac20494353e8.jpg)
(d)
(e)
![Image](images/977db30d06602b43fd4d550c3341103f95987658d64c3ad68468748a70831b57.jpg)
(f)
![Image](images/1621a33583380241cf9512905f963578b6de4782038bb27061ed72892d02df89.jpg)
(g)
Fig. 6. 2-D maps based on the KITTI Odometry dataset using different ISMs under LiDAR position uncertainty ( $\sigma_{sensor} = 40$ cm and pixel size = 10 cm). (a) Reference map created by the Octomap ISM with ground-truth poses. (b) Map created by the Octomap ISM with the noisy trajectory. (c) Map created by the Gaussian Bell ISM with the noisy trajectory. (d) Map created by the Cartographer ISM with the noisy trajectory. (e) Map created by the K-NN ISM with the noisy trajectory. (f) Map created by the Supereight2 ISM with the noisy trajectory. (g) Map created by the proposed ISM with the noisy trajectory.
### VI. 讨论 ### A. 1 维和 2 维实验结果的差异 根据图 3 中的 1 维实验结果，所提出的 ISM 方法应该生成具有零系统误差（MSD）的地图，但在图 7-9 中，所提出的 ISM 方法显示出非零的 MSD，并且随着 $\sigma_{sensor}$ 的增加而增加。有两个原因。一是由于数据集中的测量数量有限，无法满足 (17) 中的大 N 限制。另一个原因是，在 (21) 的泰勒展开中，高阶项被移除，当 $\sigma_{sensor}$ 达到相对较高值时，由于高阶项的截断误差而产生的误差不可忽略。如图 7-9 所示，当 $\sigma_{sensor}$ 小于 10 厘米时，截断误差可以忽略不计，因为由此产生的 MSD 通常小于半个单元格尺寸，并且可以忽略不计在地图上。 ### B. 推广到大型地图 在大型应用中，例如用于自动驾驶的高清地图，地图的范围会更大，并且为了能够管理地图的巨大尺寸，单元格的大小也应成比例地增加。由于在大范围的户外环境中姿态估计误差更大，LiDAR姿态误差$\sigma_{sensor}$也会增加。可以根据图7-9来探索单元格大小与$\sigma_{sensor}$的比率对地图质量的影响。结果表明，当$\sigma_{sensor}$小于单元格大小时，系统的误差（均方差）通常会减小。 --- ![Image](images/62f575483dc136f2d3c4b8676ad8071b126cb6a85d33f0ecac4206a9d2c5118a.jpg)
Fig. 7. Comparison of map errors for different ISMs evaluated on the indoor dataset (pixel size = 10 cm).
![Image](images/bcd83b038d0aa2b82285d4e12af53f6b1648ebfd445a1fac1408929062d814ee.jpg)
Fig. 8. Comparison of map errors for different ISMs evaluated on the indoor dataset (pixel size = 5 cm).
![Image](images/93f696edd71e85934a3e33f038c9214c09d56663df347650a7dd2bfd7470321d.jpg)
Fig. 9. Comparison of map errors for different ISMs evaluated on the outdoor dataset (pixel size = 10 cm).
### VII. 结论 在这项工作中，我们介绍了一种非对称高斯钟形几何ISM，该方法显式地考虑了占用栅格映射中的LiDAR位置不确定性。现有的几何ISM会受到两种类型的地图误差的影响：障碍物峰值偏移（系统误差）和障碍物增厚（随机误差），这些误差都源于LiDAR位置不确定性。在真实的室内和室外LiDAR数据集上，针对多个不确定性级别，我们的ISM始终能够减少均方差和绝对平均偏差，优于Octomap、高斯钟形、Cartographer、K-NN和Supereight2 ISM。 小于单元格大小。实验是在室内环境和小型室外环境的数据集上进行的，但这种比例可能在考虑大型环境中的室外映射的单元格大小时有所帮助。 我们的方法创建了更准确的地图，这些地图的障碍物偏移更小，障碍物增厚也减少，这对于定位和规划都有益处。我们引入了两个新的评估指标，均方差和绝对平均偏差，它们为在不确定LiDAR姿态下评估ISM提供了一个实际的基础。对于应用，所提出的ISM可以用于2D占用栅格映射。ISM参数基于$\sigma_{sensor}$和地图的分辨率，并且需要将障碍物稀疏到其中心线上。 LiDAR传感器的姿态由位置和方向组成，但本文仅对位置不确定性进行建模，而忽略了方向不确定性。未来的工作将把方向不确定性纳入相同的几何框架中，并将该方法扩展到3D占用栅格映射。 ## 参考文献 [^1]: A. Elfes, “Using occupancy grids for mobile robot perception and navigation,” Computer, vol. 22, no. 6, pp. 46–57, Jun. 1989. [^2]: B. Ristic and J. L. Palmer, “Autonomous exploration and mapping with RFS occupancy-grid SLAM,” Entropy, vol. 20, no. 6, p. 456, Jun. 2018. [^3]: X. Kang, S. Yin, and Y. Fen, “3D reconstruction & assessment framework based on affordable 2D LiDAR,” 2018, arXiv:1803.09167. [^4]: D. De Gregorio and L. Di Stefano, “SkiMap: An efficient mapping framework for robot navigation,” in Proc. IEEE Int. Conf. Robot. Autom. (ICRA), May 2017, pp. 2569–2576. [^5]: E. Bernardi, S. Masi, P. Xu, and P. Bonnifait, “High integrity lane-level occupancy estimation of road obstacles through LiDAR and HD map data fusion,” in Proc. IEEE Intell. Vehicles Symp. (IV), Oct. 2020, pp. 1873–1878. [^6]: C. Yu, V. Cherfaoui, P. Bonnifait, and D.-G. Yang, “Managing localization uncertainty to handle semantic lane information from geo-referenced maps in evidential occupancy grids,” Sensors, vol. 20, no. 2, p. 352, Jan. 2020. [Online]. Available: [^7]: D. Joubert, W. Brink, and B. Herbst, “Pose uncertainty in occupancy grids through Monte Carlo integration,” J. Intell. Robotic Syst., vol. 77, no. 1, pp. 5–16, Jan. 2015. [Online]. Available: [^8]: X. Zheng, Y. Li, D. Duan, L. Yang, C. Chen, and X. Cheng, “Multivehicle multisensor occupancy grid maps (MVMS-OGM) for autonomous driving,” IEEE Internet Things J., vol. 9, no. 22, pp. 22944–22957, Nov. 2022. [Online]. Available: [^9]: E. Ivanjko and I. Petrovic, “Extended Kalman filter based mobile robot pose tracking using occupancy grid maps,” in Proc. 12th IEEE Medit. Electrotechnical Conf., Mar. 2004, pp. 311–314. [Online]. Available: [^10]: R. Danescu, F. Oniga, and S. Nedevschi, "Modeling and tracking the driving environment with a particle-based occupancy grid," IEEE Trans. Intell. Transp. Syst., vol. 12, no. 4, pp. 1331–1342, Dec. 2011. [Online]. Available: ## [^11]: S. T. O'Callaghan and F. T. Ramos, “Gaussian process occupancy maps,” Int. J. Robot. Res., vol. 31, no. 1, pp. 42–62, Jan. 2012, doi: 10.1177/0278364911421039 --- [^12]: S. Kim and J. Kim, "Building occupancy maps with a mixture of Gaussian processes," in Proc. IEEE Int. Conf. Robot. Autom., May 2012, pp. 4756–4761. [Online]. Available: [^13]: M. G. Jadidi, J. V. Miro, and G. Dissanayake, “Warped Gaussian processes occupancy mapping with uncertain inputs,” IEEE Robot. Autom. Lett., vol. 2, no. 2, pp. 680–687, Apr. 2017. [Online]. Available: [^14]: D. Herrero-Pérez, H. Martínez-Barberá, K. LeBlanc, and A. Saffiotti, “Fuzzy uncertainty modeling for grid based localization of mobile robots,” Int. J. Approx. Reasoning, vol. 51, no. 8, pp. 912–932, Oct. 2010. [Online]. Available: [^15]: S. Thrun, W. Burgard, and D. Fox, "Probabilistic robotics," Commun. ACM, vol. 45, no. 3, pp. 52–57, Mar. 2002. [Online]. Available: [^16]: S. Thrun, Robotic Mapping: A Survey. San Mateo, CA, USA: Morgan Kaufmann, 2003, pp. 1–35. [^17]: M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit, "FastSLAM: A factored solution to the simultaneous localization and mapping problem," in Proc. 18th National Conf. Artif. Intell. (AAAI). Edmonton, AB, Canada: American Association for Artificial Intelligence (AAAI), 2002, pp. 593–598. [^18]: K. Murphy and S. Russell, “Rao-blackwellised particle filtering for dynamic Bayesian networks,” in Sequential Monte Carlo Methods in Practice. New York, NY, USA: Springer, 2001, pp. 499–515. [Online]. Available: [^19]: T. Bailey, J. Nieto, and E. Nebot, “Consistency of the FastSLAM algorithm,” in Proc. IEEE Int. Conf. Robot. Autom., Feb. 2006, pp. 424–429. [Online]. Available: [^20]: T. Reineking and J. Clemens, “Dimensions of uncertainty in evidential grid maps,” in Spatial Cognition IX, vol. 8684, C. Freksa, B. Nebel, M. Hegarty, and T. Barkowsky, Eds., Cham, Switzerland: Springer, 2014, pp. 283–298. [Online]. Available: 20 [^21]: M. Langerwisch and B. Wagner, “Building variable resolution occupancy maps assuming unknown but bounded sensor errors,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., Nov. 2013, pp. 4687–4693. [^22]: S. O'Callaghan, F. T. Ramos, and H. Durrant-Whyte, “Contextual occupancy maps using Gaussian processes,” in Proc. IEEE Int. Conf. Robot. Autom., May 2009, pp. 1054–1060. [Online]. Available: [^23]: S. T. O'Callaghan, F. T. Ramos, and H. Durrant-Whyte, “Contextual occupancy maps incorporating sensor and location uncertainty,” in Proc. IEEE Int. Conf. Robot. Autom., May 2010, pp. 3478–3485. [Online]. Available: [^24]: R. Van Kempen, B. Lampe, T. Woopen, and L. Eckstein, “A simulation-based end-to-end learning framework for evidential occupancy grid mapping,” in Proc. IEEE Intell. Vehicles Symp. (IV), Jul. 2021, pp. 934–939. [^25]: S. Wirges, C. Stiller, and F. Hartenbach, "Evidential occupancy grid map augmentation using deep learning," in Proc. IEEE Intell. Vehicles Symp. (IV), Jun. 2018, pp. 668–673. [^26]: H. Jang, T. Kim, K. Ahn, S. Jeon, and Y. Kang, “Dynamic occupancy grid map with semantic information using deep learning-based BEVFusion method with camera and LiDAR fusion,” Sensors, vol. 24, no. 9, p. 2828, Apr. 2024. [Online]. Available: [^27]: M. Schreiber, V. Belagiannis, C. Gläser, and K. Dietmayer, “A multi-task recurrent neural network for end-to-end dynamic occupancy grid mapping,” in Proc. IEEE Intell. Vehicles Symp. (IV), Jun. 2022, pp. 315–322. [^28]: G. Grisetti, C. Stachniss, and W. Burgard, “Improved techniques for grid mapping with rao-blackwellized particle filters,” IEEE Trans. Robot., vol. 23, no. 1, pp. 34–46, Feb. 2007. [Online]. Available: [^29]: T. Yang and V. Aitken, “Evidential mapping for mobile robots with range sensors,” IEEE Trans. Instrum. Meas., vol. 55, no. 4, pp. 1422–1429, Aug. 2006. [Online]. Available: [^30]: A. Y. Hata, F. T. Ramos, and D. F. Wolf, “Monte Carlo localization on Gaussian process occupancy maps for urban environments,” IEEE Trans. Intell. Transp. Syst., vol. 19, no. 9, pp. 2893–2902, Sep. 2018. [Online]. Available: [^31]: A. Hornung, K. M. Wurm, M. Bennewitz, C. Stachniss, and W. Burgard, “OctoMap: An efficient probabilistic 3D mapping framework based on octrees,” Auto. Robots, vol. 34, no. 3, pp. 189–206, Apr. 2013. [Online]. Available: [^32]: J. D. Adarve, M. Perrollaz, A. Makris, and C. Laugier, “Computing occupancy grids from multiple sensors using linear opinion pools,” in Proc. IEEE Int. Conf. Robot. Autom., May 2012, pp. 4074–4079. [Online]. Available: [^33]: W. Hess. (Aug. 2025). Cartographer-Project/Cartographer. [Online]. Available: [^34]: W. Hess, D. Kohler, H. Rapp, and D. Andor, “Real-time loop closure in 2D LiDAR SLAM,” in Proc. IEEE Int. Conf. Robot. Autom. (ICRA), May 2016, pp. 1271–1278. [Online]. Available: [^35]: H. Carrillo, P. Dames, V. Kumar, and J. A. Castellanos, “Autonomous robotic exploration using occupancy grid maps and graph SLAM based on Shannon and Rényi entropy,” in Proc. IEEE Int. Conf. Robot. Autom. (ICRA), Seattle, WA, USA, May 2015, pp. 487–494. [Online]. Available: [^36]: C. Stachniss, G. Grisetti, and W. Burgard, “Information gain-based exploration using Rao–Blackwellized particle filters,” in Robotics: Science and Systems I. Cambridge, MA, USA: MIT Press, Jun. 2005. [Online]. Available: [^37]: Y. Miao, A. Hunter, and I. Georgilas, “An occupancy mapping method based on K-nearest neighbours,” Sensors, vol. 22, no. 1, p. 139, Dec. 2021. [Online]. Available: [^38]: S. Boche, S. B. Laina, and S. Leutenegger, “Tightly-coupled LiDAR-visual-inertial SLAM and large-scale volumetric occupancy mapping,” in Proc. IEEE Int. Conf. Robot. Autom. (ICRA), May 2024, pp. 18027–18033. [^39]: P. O. Geedh. (Aug. 2025). Pgeedh/RoboticsSpecialization-UPenn-Estimation-and-Learning. [Online]. Available: [^40]: A. Geiger, P. Lenz, and R. Urtasun, “Are we ready for autonomous driving? The KITTI vision benchmark suite,” in Proc. IEEE Conf. Comput. Vis. Pattern Recognit., Jun. 2012, pp. 3354–3361.