[人工智能]＜论文阅读＞ 基于对称距离流和多扫描对齐的稳健平面里程计（IEEE 2018）

摘要

??本文提出了一种使用激光扫描仪估计平面运动的密集方法。从扫描之间几何一致性的对称表示开始，我们推导出精确的距离流约束，并将扫描观测的运动表示为扫描仪刚性运动的函数。与将最新扫描与前一个或最后一个选择的 keyscan 对齐的现有技术相比，我们提出了一种组合且有效的公式，以在每次迭代时联合对齐所有这三个扫描。这种新的公式保留了基于 keyscan 的策略的优点，但对于 keyscan 的次优选择和动态物体的存在更加稳健。
??使用静态和动态环境中的模拟和真实数据对我们的方法进行了广泛的评估。结果表明，在所有进行的实验中，我们的方法比现有方法快一个数量级，并且明显更准确。它的运行时间约为一毫秒，适用于那些需要平面里程计且计算成本低的机器人应用。该代码以 ROS 包的形式在线提供。

一、引言

??运动估计是机器人和计算机视觉的主要挑战之一。几乎每个机器人，无论是无人机、人形机器人还是机械手，都需要准确跟踪其位置以执行自主任务。尽管存在用于估计机器人运动的不同技术（例如 GPS 系统、惯性传感器或编码器），但视觉里程计可以说是最灵活和最强大的解决方案，因为它可以处理不同的输入数据（光度/几何）并且可以适用于几乎任何类型的机器人。
??我们的方法，我们将其称为基于对称距离流的里程计（SRF-Odometry），扩展和改进了 [9] 中提出的算法。该算法基于距离流约束方程，并将每个观察点的运动公式化为传感器速度的函数，假设环境是静态的。在本文中，我们基于相同的想法，并介绍以下贡献

• 扫描之间几何一致性的新对称公式。据我们所知，该技术已应用于光流的估计，但从未应用于视觉里程计。
• 一种新的多扫描公式，它结合了视觉里程计中的两种标准技术：连续扫描/图像的对齐和 keyscan/关键帧的对齐。
• 将我们的算法的准确性建模为匹配扫描之间的平移和旋转的函数的过程。基于这个模型，我们提出了一个新的标准，通过对最大可接受/期望的平移和旋转误差施加阈值来选择 keyscan。
• 比最先进的技术更快、更准确的估计，无论是在静态环境中还是在存在移动物体的情况下

三、视觉里程计的距离流约束

??在本节中，我们通过在两次连续扫描之间施加几何一致性来推导出传感器运动的简单线性约束。该约束建立在两个主要假设之上：环境是静态的，并且传感器的平移和旋转足够小。
??设 $r,\theta$ 是一个点相对于激光扫描仪的极坐标，$R_1,R_2:\Omega \to {\mathbb{R}}^{+}$ 是分别在时间 $t_1$ 和 $t_2$ 的连续瞬间进行的两次径向扫描。为简单起见，我们假设 $\Omega$ 是激光器视场 (FV ) 内的连续域，并且直接以角坐标给出，即 $\Omega :=[?F_V/2,F_V/2]$。在时间间隔 $[t_1,t_2]$ 内，环境中任一点相对于激光扫描仪的视运动 $\Delta r,\Delta \theta$ 必须与扫描的观察结果一致：
$$?r=R_2(\theta +?\theta )?R_1(\theta )\text{\hspace{1em}(1)}$$

??该约束如上图所示，并且通常有效，除了在遮挡的情况下。通常，这个表达式被线性化以获得所谓的“距离流约束”
$$?r=R_2(\theta )+\frac{d{R}_2}{d\theta }{\mid }_{\theta }?\theta ?R_1(\theta )+O(?{\theta }^2)\text{\hspace{1em}(2)}$$$$O(?{\theta }^2)=\frac{1}{2}\frac{d^2{R}_2}{d{\theta }^2}?{\theta }^2+O(?{\theta }^3)\text{\hspace{1em}(3)}$$

??如果忽略二阶和高阶项，这是众所周知的光流约束的几何版本。
??我们建议使用 (2) 的略微修改版本，其中运动在两次扫描之间均匀分布：
$$?r=R_2(\theta +?\theta /2)?R_1(\theta ??\theta /2)\text{\hspace{1em}(4)}$$
??这种替代表示（图 1）已经在计算机视觉中用于估计固有对称的光流 [34]。对我们来说，这个公式的主要优点是它的线性化更精准，相对于：
$$?r=R_2(\theta )?R_1(\theta )+(\frac{d{R}_2}{d\theta }{\mid }_{\theta }+\frac{d{R}_1}{d\theta }{\mid }_{\theta })\frac{?\theta }{2}+O(\frac{?{\theta }^2}{4})\text{\hspace{1em}(5)}$$$$O(\frac{?{\theta }^2}{4})=\frac{?{\theta }^2}{8}(\frac{d^2{R}_2}{d{\theta }^2}{\mid }_{\theta }?\frac{d^2{R}_1}{d{\theta }^2}{\mid }_{\theta })+O(\frac{?{\theta }^3}{8})\text{\hspace{1em}(6)}$$

??可以看出，这种对称公式需要比标准范围流量约束更多的信息（它需要两次扫描的梯度），但对于任何给定的 $\Delta \theta$，它具有较小的线性化误差。

??接下来我们需要用笛卡尔坐标表达运动。如果应用于瞬时速度，则从极坐标到笛卡尔坐标 $(x,y)$ 的变换是精确且线性的：
$$r=\dot{x}cos\theta +\dot{y}sin\theta \text{\hspace{1em}(7)}$$$$r\dot{\theta }=\dot{y}cos\theta ?\dot{x}sin\theta \text{\hspace{1em}(8)}$$
??为了用增量来表示 (7) 和 (8)，它们必须在 $t_1$ 和 $t_2$ 之间积分。假设位移很小，我们可以通过以下线性项来近似这些积分：
$$?r={\int }_{t_1}^{t_2}\dot{x}cos\theta +\dot{y}sin\theta dt\approx ?xcos\theta +?ysin\theta \text{\hspace{1em}(9)}$$$$?\theta ={\int }_{t_1}^{t_2}\frac{\dot{y}cos\theta ?\dot{x}sin\theta }{r}dt\approx \frac{?ycos\theta ??xsin\theta }{\overline{r}}\text{\hspace{1em}(10)}$$
其中 $\overline{r}=(R_1(\theta )+R_2(\theta ))/2$ 是 $t_1$ 和 $t_2$ 之间 $r$ 的最佳常数近似值
??另一方面，我们需要施加约束，即环境与传感器之间的相对运动仅由传感器本身的运动引起（环境是静止的）。该运动由速度矢量 ${\xi }^s=({\xi }_x^s,{\xi }_y^s,{\xi }_{\omega }^s)$ 编码，这是与 2D 刚性变换相关的李代数元素（即 ${\xi }^s\in se(2))$。因此，根据刚体的运动学，环境中每个点的运动都可以表示为 ${\xi }^s$ 的函数：
$$\left(\begin{array}{c}{\Delta }_x\\ {\Delta }_y\end{array}\right)={\int }_{t_1}^{t_2}\left(\begin{array}{c}\dot{x}\\ \dot{y}\end{array}\right)dt\approx \left(\begin{array}{c}?{\xi }_x^s+\overline{y}{\xi }_w^s\\ ?{\xi }_y^s?\overline{x}{\xi }_w^s\end{array}\right)\text{\hspace{1em}(11)}$$

其中平均坐标计算为:
$$\left(\begin{array}{c}\overline{x}\\ \overline{y}\end{array}\right)\approx \frac{R_1(\theta )+R_2(\theta )}{2}\left(\begin{array}{c}cos\theta \\ sin\theta \end{array}\right)\text{\hspace{1em}(11)}$$
??最后，将 (9)、(10) 和 (11) 插入距离流 (5) 并丢弃高阶项，我们最终得到传感器运动的线性约束：
$$(cos\theta +\frac{{\overline{R}}_{\theta }sin\theta }{r}){\xi }_x^s+(sin\theta ?\frac{{\overline{R}}_{\theta }cos\theta }{\overline{r}}){\xi }_y^s+(\overline{x}sin\theta ?\overline{y}cos\theta ?{\overline{R}}_{\theta }){\xi }_w^s+R_2(\theta )?R_1(\theta )=0\text{\hspace{1em}(13)}$$
其中：
$${\overline{R}}_{\theta }=\frac{1}{2}(\frac{d{R}_2}{d\theta }{|}_{\theta }+\frac{d{R}_1}{d\theta }{|}_{\theta })\text{\hspace{1em}(14)}$$
是两次连续扫描的平均导数。因此，传感器 ${\xi }^s$ 的运动可以通过匹配连续扫描（应该是可微的或分段可微的）来获得，而无需搜索和精确对应关系。
??如前所述，这种推导在小运动的假设下是有效的，即线性化 (5) 成立的那些。尽管“小”和“大”运动之间没有急剧的过渡，但我们通常认为，如果 $\Delta \theta$ 始终小于或等于用于近似距离梯度 ${\overline{R}}_{\theta }$ 的局部邻域，则该运动很小。通常，这些梯度是使用以下和先前观察值的中心公式来近似的，因此，角增量 $\Delta \theta$ 应该小于或等于扫描中连续观察之间的角度。

四、优化问题

??理论上，三个独立的约束足以获得激光雷达运动，但在实践中这是不可行的，因为由于测量的噪声、线性近似 (5) 产生的误差或移动物体的存在，(13) 往往不精确（非静态环境）。因此，我们使用了一个稠密的公式，其中所有的扫描观察都有助于运动估计。
??几何残差 $\rho (\xi ,\theta )$ 被定义为对给定运动 $\xi$ 在给定角度 $\theta$ 下的距离流约束 (13) 的评估：
$$\rho (\xi ,\theta )=R_2(\theta )?R_1(\theta )+(\overline{x}sin\theta ?\overline{y}cos\theta ?{\overline{R}}_{\theta }){\xi }_{\omega }+(cos\theta +\frac{{\overline{R}}_{\theta }sin\theta }{\overline{r}}){\xi }_x+(sin\theta ?\frac{{\overline{R}}_{\theta }cos\theta }{\overline{r}}){\xi }_y\text{\hspace{1em}(15)}$$
??由于不是每个任意角度 $\theta$ 都可以评估，而只能评估由激光扫描仪采样的角度，我们简化了符号并使用 ${\rho }_n(\xi )$ 来指代与扫描的第 $n$ 次观察相关的残差。为了获得准确的运动估计，所有几何残差都在鲁棒的成本函数内最小化：
$${\xi }^M=\underset{\xi }{\mathrm{argmin}}\sum _{n=1}^{N}F({\rho }_n(\xi ))\text{\hspace{1em}(16)}$$$$F(\rho )=\{\begin{array}{ll}\frac{{\rho }^2}{2}(1?\frac{{\rho }^2}{2c^2}),& \text{∣ρ∣≤c}\\ \frac{c^2}{4},& \text{?∣ρ∣>c?}\end{array}\text{\hspace{1em}(17)}$$

??N 是扫描中的点数。函数 $F(\rho )$ 是截断抛物线的平滑版本（图 2），$c$ 是可调整的参数。 $F(\rho )$ 在任何地方都是连续且可微的，并且对于高于 $c$ 的残差变得平坦，这代表了一种降低（甚至丢弃）异常值的有效且自动的方法。参数 $c$ 计算为残差的中值绝对偏差 (MAD) 的比率（参见第 VIII-C 节）。

A. 预加权策略

??有一些因素会导致 (13) 不准确，主要是刚性假设 (11) 的不满足和与 (5) 中的线性近似的偏差。尽管鲁棒函数 F(ρ) 可以减轻它们对整体运动估计的影响，但它并不能完全消除它。在求解系统之前很难检测到移动物体的存在，因此我们依靠鲁棒函数 $F(\rho )$ 在最小化过程中降低它们的权重。另一方面，可以预先检测与（5）中采用的线性近似的偏差，这有助于加速（16）中的收敛并导致更准确的结果。为此，我们提出了一种预加权策略来降低距离函数（4）高度非线性甚至不可微分的那些观察值的残差。我们称其为“预加权”，因为它是在解决最小化问题 (16) 之前应用的。

??为了量化与 (5) 的线性化相关的误差，我们评估泰勒级数的二阶项 (6)。此外，识别距离函数不仅非线性而且不可微的扫描区域也很重要。这些区域主要是各种观察对象的边缘，通常以非常高的一阶导数为特征，包括角度 ${\overline{R}}_{\theta }$ 和时间 $R_t=R_2?R_1$

为了惩罚这两种效应，非线性和不连续性，我们定义了以下预加权函数：
$$w=\frac{1}{{\sigma }_s^2+K_D({\overline{R}}_{\theta }^2+R_t^2)+K_{2D}{\overline{R}}_{\theta \theta }^2}\text{\hspace{1em}(18)}$$
其中 ${\overline{R}}_{\theta \theta }$ 是 $R_1$ 和 $R_2$ 的平均二阶导数。参数 $K_D$,$K_{2D}$ 量化了一阶和二阶导数的相对重要性。此外，我们添加了一个附加项 ${\sigma }_s^2$ 来模拟测量的噪声。在本文中，我们为 ${\sigma }_s$ 使用了一个简单的常数值，但可以使用更精细和更精确的噪声模型来代替。
$${\rho }_n^w(\xi )=w_n{\rho }_n(\xi )n\in 1,2...N\text{\hspace{1em}(19)}$$
随后根据(16),(17)最小化。

五、多扫描公式

??纯里程计总是估计连续输入数据集之间的运动，无论这些数据是车轮旋转、RGB 图像还距离扫描。然而，这种纯粹的增量策略在视觉里程计中有一个主要缺点：每个新增量都会在姿态估计中引入一些误差，即使实际运动非常小或为零。这种缺陷通常通过定期选择序列的特定扫描来解决，称为“参考扫描”或“关键扫描”，并将每个新扫描与它对齐。此关键扫描充当本地锚点，有助于减少估计轨迹的漂移。当传入的扫描与选定的 keyscan 太远时，从某种意义上说两者之间没有太多重叠，必须设置新的 keyscan 并继续该过程。基于关键扫描的公式通常比纯粹的增量估计更准确，但很大程度上取决于用于引入新键扫描的标准。该标准必须防止插入冗余关键扫描帧，并且，最重要的是，它必须保证最新的扫描总是与keyscan足够接近，以便它们可以对齐。
??在本文中，我们提出了一种混合公式，其中最新的扫描与先前的扫描和关键扫描帧同时对齐（图 3）。该策略保留了基于关键扫描帧的方法的优点，同时降低了因关键键扫描选择不当而产生的风险。由于现在将三种不同的扫描输入算法，传感器噪声的不利影响和动态物体也有所缓解。

??令 $T_{ki},T_{kj}\in SE(2)$ 分别为扫描 $R_i$ 和 $R_j(j=i?1)$ 和最后一个 keyscan $R_k$ 之间的齐次变换，令 $T_{ji}\in SE(2)$ 为 $R_i$ 和 $R_j$ 之间的增量变换 (见图 3)。由于这些转换形成一个循环，因此必须满足以下约束：
$$T_{kj}{T}_{ji}=T_{ki}\text{\hspace{1em}(20)}$$
其中，假设 $T_{kj}$ 从先前的估计中已知。因此，我们可以建立一个优化问题，将扫描 $R_k$、$R_j$ 与 $R_i$ 联合对齐，服从 (20)。有两种可能的方式来表述这个问题：

• 显式优化两组未知数，即与 $T_{ji}$ 和 $T_{ki}$ 相关的向量 ${\xi }_{ji}$ 和 ${\xi }_{ki}$
• 根据已知的变换 $T_{kj}$ 将 $R_k$ 向 $R_j$ 弯曲并在两种情况下求解 $T_{j}i$

??第二个选项更有效，因为它涉及更少的未知数（我们只需要估计 ${\xi }_{ij}$）并通过 $R_k$ 向 $R_j$ 的扭曲隐式地施加约束（20）。因此，与所提出的多扫描公式相关的优化问题是：
$${\xi }^M=\underset{\xi }{\mathrm{argmin}}\sum _{n=1}^{N}F(w_n^{ji}{\rho }_n^{ji}(\xi ))+F(w_n^{ki}{\rho }_n^{ki}(\xi ))\text{\hspace{1em}(21)}$$

六、求解器

??我们的运动估计问题是非线性和非凸的，因为几何一致性的原始约束 (4) 和鲁棒函数 $F(\rho )$ (17) 都是非线性和非凸的。在涉及几何一致性约束的情况下，通过推导距离流方程（5）并将几何残差定义为激光雷达运动的线性函数来解决此限制。然而，由此产生的线性约束仅对非常小的位移和/或旋转有效，并且在实践中无法估计实际运动。然而，这个问题已经在文献中得到解决，并且可以通过在从粗到细的方案中制定运动估计问题来解决。在从粗到细的方案中，从最粗的级别开始构建和对齐两个扫描金字塔（或通常是图像），其中线性化（5）适用于更大的位移，然后通过随后的级别细化。因此，每个级别都会逐步改进对齐方式，并在剩余（和更精细）的级别中留下“更少的运动”来估计。关于从粗到细的策略和扭曲的更多细节可以在 [9]、[35] 中找到。
??在从粗到细方案的每个级别，优化问题 (21) 使用迭代重加权最小二乘法 (IRLS) 解决，其中与平滑截断抛物线 $F(\rho )$ 相关的权重为：
$$W(\rho )=\{\begin{array}{ll}1?\frac{{\rho }^2}{c^2},& \text{∣ρ∣≤c}\\ 0,& \text{?∣ρ∣>c?}\end{array}\text{\hspace{1em}(22)}$$

??由于鲁棒函数 $F(\rho )$ 是非凸的，因此我们还考虑并测试了其他优化方法 (21)，例如 [36] 中提出的提升策略，但与 IRLS 相比，它们并没有改善结果并且涉及更多复杂且较慢的实现。

??最后，对于粗到细方案的较粗级别，可能会出现要估计的运动实际上超出线性化 (5) 所适用的运动范围。在这种情况下，可能出现两种不同的结果：

• 实际运动远大于线性化的有效范围，因此求解器将提供完全错误的解决方案。
• 实际运动超出但接近线性化的有效范围，因此求解器将提供不精确但接近实际运动的解。

??第一种情况的大失败是不可避免的：扫描距离太远，算法就会失败。然而，在第二种情况下，该解决方案可用于将两个扫描中的一个向另一个扭曲，创建一个新配置，其中剩余运动（待估计）更小，因此可以通过重新获得更精确的运行算法。只要最后估计的运动大于给定的阈值，就可以迭代地执行估计运动和翘曲扫描的过程，从而提高我们方法的收敛范围。

七、keyscan 选择

??如前所述，基于关键扫描的公式提供比连续扫描对齐更准确的轨迹估计，但需要合适的关键扫描选择标准。当某个幅度超过手动设置的阈值时，已经提出了不同的策略来引入新的关键扫描（或关键帧）。通常，该阈值应用于估计的角度和线性位移 [37]、图像对齐后的残差 [38] 或估计的熵 [39]。这些方法的主要问题是阈值的选择与算法的最终性能没有直接关系，因此，位姿估计误差不受限制在明确定义的范围内。因此，这些方法涉及繁琐的试错阶段来调整其阈值，直到达到所需的性能。
??在这项工作中，我们建议将我们算法的姿态估计误差建模为匹配扫描之间的平移和旋转的函数。使用此模型，可以直接在错误域上设置关键扫描选择的阈值，从而不会超过最大旋转和/或平移错误。这些阈值定义了平移和旋转域上的 2D 工作区域，将在操作期间用于触发新关键扫描的选择。
??在对误差进行建模时，激光扫描仪的规格（视野、最大范围和扫描中的点数）是性能变化的主要来源。这意味着不可能获得单一且通用的误差模型，而是必须根据所用扫描仪的特性调整模型。在本节中，我们获得了两种不同激光测距仪的误差模型：Hokuyo UTM-30LX 和 SICK LMS-500（见表 I），用于第 IX 节中的模拟和真实实验。
??我们依靠具有精确基本事实的模拟实验来生成足够多的样本来模拟估计误差。通过在特定环境中模拟激光扫描仪并在 $[0,1]$ 米和 $[0,1]$ 弧度范围内对其应用随机位移和旋转来生成数据。对于这些模拟，我们使用了三个公开可用的场景/地图：来自机器人数据集存储库 (Radish) [40] 的 Belgioioso Castle 和英特尔研究实验室，以及来自 Robot@home 数据集 [41] 的 Sarmis 家庭环境。此外，在激光测量中添加了 $\sigma$ 范围为 5 mm 至 25 mm 的高斯噪声。总体而言，使用了 60,000 个里程计估计（即每个场景 20,000 个错误样本）来对错误进行建模。
??收集样本后，我们通过使用 ${\sigma }_R=0.1$ rad 的各向异性高斯窗函数计算在模拟中获得的误差的加权平均值来估计平移-旋转平面的任何位置 $(x^T,x^R)$ 的平移 $e^T$ 和旋转 $e^R$ 误差和 ${\sigma }_T=0.12$ m：

$$e^T(x^T,x^R)=\frac{{\sum }_{s=1}^S{\lambda }_s{e}_s^T}{{\sum }_{s=1}^S{\lambda }_s}\text{\hspace{1em}(24)}$$$$e^R(x^T,x^R)=\frac{{\sum }_{s=1}^S{\lambda }_s{e}_s^R}{{\sum }_{s=1}^S{\lambda }_s}\text{\hspace{1em}(25)}$$$${\lambda }_s=exp(?\frac{||x^T?x_s^T|{|}^2}{2{\sigma }_T^2}?\frac{||x^R?x_s^R|{|}^2}{2{\sigma }_R^2})\text{\hspace{1em}(26)}$$
??其中 S 是样本总数。得到的表面如图 4 所示。正如预期的那样，当连续扫描之间的平移和/或旋转增加时，姿势估计中的平均误差也会增加。比较两台激光扫描仪的误差模型，我们可以看到它们的形状相似，但 SICK LMS-500 的误差值更高。这是意料之中的，因为与 Hokuyo 扫描仪相比，SICK 测距仪的视野更小，每次扫描的点数更少。
??利用这些模型，我们直接为平移和旋转误差设置阈值，以将它们限制在可接受的范围内。具体来说，在这项工作中，我们将这些阈值分别设置为 10 毫米和 0.1 度。通过将它们与误差模型的表面相交，我们获得了如图 5 所示的最终工作区域。可以看出，这些区域的形状相似，但它们的尺度不同。具有更大视野和更多点数的扫描仪提供了更大的工作区域，这表明携带更多信息的扫描可以与更稀疏的键扫描一起使用，同时将位姿误差保持在相同的范围内。一旦计算出工作区域，我们将四次多项式拟合到其边界，将在每次迭代时对其进行评估，以确定系统是在工作区域内还是在工作区域外。
??如果估计的姿势相对于当前keyscan在工作区域内，我们相信里程计估计并保持当前keyscan，否则我们触发keyscan更新。

八、实施细节

??在本节中，我们将描述我们算法的重要细节，这些细节不是其理论核心的一部分，但会影响其性能。我们还设置了整篇论文中介绍的参数值，并解释了它们如何影响运动估计。

A. 梯度近似

??通常，使用固定的离散公式来近似扫描或图像梯度。在距离数据的情况下，这种策略会导致对象边界处的梯度值非常高，这并不代表观察到的表面的真实梯度。作为替代方案，我们使用自适应公式，在扫描中使用连续观察（点）之间的二维距离对前向 $(R_{\theta }^{+})$ 和后向 $(R_{\theta }^{?})$ 导数进行加权：
$$R_{\theta }(n)=\frac{d(n+1)R_{\theta }^{?}(n)+d(n)R_{\theta }^{+}(n)}{d(n+1)+d(n)}\text{\hspace{1em}(27)}$$$$d(n)=||(x(n)?x(n?1),y(n)?y(n?1))||\text{\hspace{1em}(28)}$$
??其中 $n$ 指的是扫描中的特定索引。因此，最近的邻居总是对梯度计算贡献更多，而非常远的点几乎不影响它。如果两个邻居近似等距，则所提出的公式等效于中心有限差分近似。

B. 运动过滤器

??机器人运行的环境有时包括传感器运动无法完全恢复的房间或区域，例如：长走廊。

C. 稳健优化的参数

??有几个参数直接影响优化问题 (21)。一方面，预加权函数 $w$ 取决于两个参数（$K_D$ 和 $K_{2D}$）和传感器噪声模型 ${\sigma }_s$。我们没有提供调整 $K_D$ 和 $K_{2D}$ 的正式程序，而是使用经验上为我们提供最准确结果的值。具体来说，我们设置 $K_D=0.01$ 和 $K_{2D}=2\times 10^{?4}$。一般来说，较高的 $K_D$ 和 $K_{2D}$ 值会导致靠近传感器的点的权重较高（其坐标和导数往往更精确），但会过度降低远距离点的权重，这有时是约束估计运动所必需的。

??对于传感器噪声，我们在所有情况下设置 ${\sigma }_s=0.02m$，这是机器人技术中使用的普通激光扫描仪中发现的噪声的代表性平均值。另一方面，稳健的惩罚函数 $F(\rho )$ (17) 包括参数 $c$，它标志着异常值和异常值之间的界限（高于 $c$ 的残差位于 $F(\rho )$ 的平坦区域，在优化过程中不做贡献）。我们使用残差的中值绝对偏差 (MAD) 来调整 $c$，更具体地说，我们设置 $c=4MAD({\rho }_n)$。这是异常值拒绝的高阈值，因为它将大多数观察值保留为异常值，只有那些具有明显高残差的值才会成为异常值（例如移动物体）。