RF2O

RF2O是一种快速而精确的方法，用于从连续范围扫描中估计激光雷达的平面运动。对于每个扫描点，可以根据传感器速度制定范围流约束方程，并最小化所得几何约束的鲁棒函数以获得运动估计。与传统方法相反，该方法不搜索对应关系，而是以密集的3D视觉测距法的方式基于扫描梯度执行密集扫描对准。最小化问题以粗到精方案解决以应对大位移，并且基于估计的协方差的平滑滤波器用于处理无约束情景（例如走廊）中的不确定性。

里程计是机器人定位的重要组成部分，大家普遍采用的是编码器里程计或者是视觉里程计。依靠电机编码的里程计在低速情况下比较可靠，但不可避免地因为轮子打滑等因素造成的漂移问题，而视觉里程计对于运动估计问题来说是一个比较灵活的解决方案，能适用不同的机器人，但是计算量大，占用较大资源空间，对传感器要求较高。激光里程计综合上述两种方法的优点，能快速而精确地从连续范围扫描中估计激光雷达的平面运动，且运算成本低，表现取决于激光的分辨率和环境因素，转换评估准确，但是由于激光雷达自身的旋转，会导致odom旋转不准确，这可以通过和IMU的传感器数据融合来解决。

2D激光扫描速度的描述

假设环境是静态和刚性的。 设R（t，α）为范围扫描，其中t是时间，α∈[0，N）⊂R是扫描坐标，N是扫描的大小。 任何点P相对于连接到传感器的局部参考系的位置由其极坐标（r，θ）给出（见图1）。

与某物相交的激光扫描图表示。 经历时间间隔Δt之后，观察点P相对于扫描仪移动到P0。

如果从激光雷达扫描点P，则将在与P的角坐标直接相关的扫描坐标α处观察到：

其中FOV是扫描仪的视野。 类似于光学流动约束方程，可以从两个扫描对的几何一致性的一般表达式导出线性约束。 假设R的可微性，第二次扫描的任何点的范围可以表示为泰勒展开

其中Δt是连续扫描之间的时间间隔，Δα表示所考虑的点的扫描坐标的变化。 忽略二阶/更高阶项，除以Δt，我们可以得到一个简单的表达式，它将扫描梯度与区间[t，t +Δt]期间点的范围和扫描坐标的变化联系起来：

如果我们认为˙r=ΔR/Δt和˙α=Δα/Δt是区间[t，t +Δt]期间范围内点和扫描坐标的平均速度，则我们可以得到：

方程（4）首先由Gonzalez＆Gutierrez [2]引入，随后在[16]中推广并命名为“范围流约束方程”。 为了描述所有点相对于相同矢量基础的速度，我们将径向和方位角速度（˙r，˙θ）转换为笛卡尔表示（˙x，˙y），如图1所示：

作为最后一步，我们需要强制每个明显的运动实际上是由激光雷达平移和/或旋转引起的。换句话说，我们假设每个点都相对于传感器移动，好像它是刚体的一部分，其速度相同但与传感器的符号相反：

得到ξs=（vx，s，vy，s，ωs）2D Twist（传感器速度）和（x，y）笛卡尔坐标P。如果笛卡尔速度（5）（6）代入（4）和 强加了刚度假设（7），我们可以将范围流约束方程转换为激光雷达速度的约束：

总得来说，每个扫描点对传感器运动都施加了限制，因此，3个线性独立的限制在理论上足以估计它。

速度估计

在实践中，激光雷达运动不能仅用三个独立的限制来估计，因为一般来说，（8）由于距离测量的噪声，线性近似（3）或运动物体的存在所造成的误差而不精确（ 非静态环境）。 因此，我们提出了一种密集的公式，其中扫描的所有点都有助于运动估计。 对于每个点，我们将几何残差ρ（ξ）定义为给定Twist ξ的范围流约束（8）的评估：

为了获得准确的估计，通过最小化鲁棒函数F内的所有几何残差来计算传感器运动：

函数F是Cauchy M-estimator，k是可调参数。 与L2或L1规范的更常见选择相反，该函数降低了具有非常高残差的那些点的相关性，并且代表了处理异常值的有效且自动的方式。 迭代重加权最小二乘法（IRLS）解决了优化问题，其中与Cauchy M-estimator相关的权重为：

利用IRLS，通过重新计算残差和权重来迭代求解系统直到收敛。如前所述，有一些因素可能导致（8）不准确，主要是刚性假设（7）的不完整填充和（3）中的线性近似的偏差。尽管Cauchy M-estimator估计可以减轻它们对整体运动估计的影响，但它并没有完全消除它。在解决运动之前很难检测到运动物体的存在，因此，我们依靠Cauchy M-estimator估计器在最小化过程中减小它们的权重。另一方面，可以预先检测与（3）中采用的线性近似的偏差，这有助于加速（10）中的收敛并且还导致更准确的结果。为此，我们提出了一种预加权策略，以减轻那些范围函数是非线性或甚至不可微分的点的残差。我们称之为“预加权”，因为它是在最小化问题（10）解决之前应用的。为了量化与（2）的线性化相关的误差，我们将泰勒级数展开为二阶：

可以注意到，忽略高阶项，R2o中的二阶导数（Δt，˙α）可用于检测线性偏差。 一个特殊情况是关于时间（Rtt）的二阶导数，其不能在粗到细方案中计算，因为扭曲图像是永恒的，因此，第二时间导数的概念没有意义（粗 -  第V节中描述了to-fi ne。 此外，重要的是检测扫描区域，其中范围函数不仅是非线性的而且是不可微分的。 这些区域主要是观察到的不同物体的边缘，并且通常以非常高的一阶导数值（Rt和/或Rα）为特征。 为了惩罚这两种效应，非线性和不连续性，我们为每个扫描点定义了以下预加权函数：

参数Kd标志着一阶和二阶导数的相对重要性，并且是避免奇异情况的小常数。 因此，我们最初计算一组预加权残差

因此，我们最初计算一组预加权残差

随后根据（10）（11）将其最小化。 虽然我们没有在论文中显示比较，但这种策略提供了比标准IRLS最小化更好的结果，没有预加权和收敛更快（大约2倍）。

粗到细方案和扫描弯曲

（3）中给出的线性化要么适用于连续扫描之间的小位移，要么适用于具有恒定范围梯度的区域（在激光雷达的情况下，对于非常不寻常的几何形状会发生：阿基米德螺旋）。为了克服这个限制，我们估计了粗糙度方案中的运动，其中较粗糙的水平提供粗略估计，随后在较高水平上得到改善。 Battiti等人介绍了粗到细方案。 [17]解决大位移的光学问题，并且自[18] [19]以来一直被采用。设R0，R1为两次连续激光扫描。最初，通过连续下采样（通常为2）原始扫描R0，R1来创建两个高斯金字塔。通常，高斯掩模应用于下采样RGB或灰度图像，但是在范围数据的情况下，标准高斯滤波器不是最佳选择，因为它在滤波扫描上产生伪像。作为替代方案，我们采用双边滤波器[20]，它不会混合可能属于场景的不同对象的远点。一旦构建了金字塔，速度估计问题就会从最粗糙到嵌套级别迭代求解。在每次过渡到一个水平时，两个输入扫描中的一个必须根据先前水平（ξp）中估计的总体速度相互扭曲。这种变形过程总是分为两个步骤，并且在我们的公式中，应用于第二次扫描R1。首先，使用与Twist ξp相关的刚体运动对在R1中观察到的每个点P进行空间变换：

其次，必须将转换后的点重新投影到R1上以构建弯曲扫描Rw 1，以便：

可以将几个点翘曲到相同的坐标αw，在这种情况下保留最接近的点（其他点将被遮挡）。 如果ξp收敛到实际速度，那么弯曲扫描Rw 1将比原始R1更接近第一扫描R0，这允许我们在（2）中以更精细的分辨率应用线性近似。

实施

573/5000

我们的算法特别注意范围梯度的计算。 通常，使用固定的离散公式来近似扫描或图像梯度。 在范围数据的情况下，该策略导致对象边界处的梯度的非常高的值，其不表示这些对象上的真实梯度。 作为替代方案，我们使用一种考虑环境几何形状的自适应公式。 此公式使用连续观察点（点）之间的2D距离对扫描中的前向和后向导数进行加权：

因此，最近邻居总是对梯度计算贡献更多，而远点几乎不影响它。在两个邻居近似等距的情况下，所呈现的公式等效于居中的有限差分近似。关于梯度计算的更多细节可以在[19]中找到。最后，重要的是要注意到存在一些环境的几何配置，传感器运动无法从中恢复。当激光雷达仅观察到墙壁时，最常见的情况就出现了。在这种情况下，平行于墙的运动是不确定的，因此求解器将为它提供任意解（不仅是我们的方法，而且是纯粹基于几何的任何方法）。为了缓解这个问题，我们在速度ξ的本征空间中应用低通滤波器，其工作原理如下所述。首先，分析IRLS解的协方差矩阵Σ∈R3×3的特征值，以检测哪些运动（或运动的组合）是不确定的，哪些是完全约束的。在特征向量的空间中，由（10）提供的速度ξtM与前一时间间隔ξt-1的速度ξtM加权，以获得新的滤波速度ξt：

其中E是包含特征值和k1的对角矩阵，ke是滤波器的参数。 具体地说，kl在来自求解器的解与先前估计之间施加恒定加权，同时确定特征值如何影响最终估计。 这些参数设置为以下值：

其中l是金字塔等级，范围从1（最粗）到所考虑的等级数。 有关此过滤器及其应用方式的更详细说明，请参阅[19]。