0. 简介
激光雷达的畸变矫正是一个非常重要的工作。由于扫描式激光雷达传感器需要有限的时间来创建点云，所以一次扫描过程中传感器的运动会导致点云发生畸变，这种现象被称为运动畸变或者卷帘快门。运动畸变校正方法已经存在，但是它们依赖于外部测量或者多次激光雷达扫描上的贝叶斯滤波。《Correcting Motion Distortion for LIDAR HD-Map Localization》一文提出了一种新型算法，其执行快照处理以实现运动畸变校正。快照处理是将当前激光雷达扫描配准到参考图像，而不使用外部传感器或者贝叶斯滤波，其对于高精（HD）地图定位尤其重要。本文方法称为速度校正迭代紧凑椭球变换（VICET），其扩展了众所周知的正态分布变化（NDT）算法，以联合求解两个激光雷达扫描帧之间的6自由度（DOF）刚体变换以及描述当前激光雷达扫描帧内畸变的一组6DOF运动状态。通过实验，我们证明，当在无畸变的高精地图中定位带有畸变的原始激光雷达扫描帧时，VICET相比NDT或者迭代最近点（ICP）算法，实现了明显更高的精度。相应的代码目前在Github上已经开源了。
图1. 用于研究运动畸变校正的简单测试场景。等距房间视图显示了三个可能的激光雷达位置，标记为a、b和c。
在每个位置上，激光雷达单元的方向由一组正交基向量描述。对于每种配置，假设激光雷达束从红色箭头开始与垂直（蓝色）轴逆时针旋转。在扫描过程中，激光雷达单元可以保持静止（a → a），前向线性运动（a → b），或进行复合平移和旋转（a → c）。在每种情况下，激光雷达束在激光雷达定子的参考系中旋转360度，而定子本身也在移动，导致畸变的原始点云（从上方视角看，用红色表示）。通过补偿定子运动，可以将原始图像转换为固定于房间的坐标系（用蓝色表示），从而恢复出房间的正方形形状。
1. 主要贡献
本文的主要贡献如下：
1）本文提出了一种新的基于激光雷达的畸变校正方法，该方法不依赖于外部测量或者估计器，因此可以进行快照处理，仅使用参考图像和当前激光雷达扫描帧进行匹配。这对于基于地图的导航尤其重要，其中通过将单帧扫描与先前存在的地图进行匹配来估计瞬时位置；
2）本文还提出了一种能够对测量不确定性进行有意义预测的新方法，鲁棒误差边界的计算是自动驾驶乘用车等安全关键导航系统的一个重要方面；
3）本文通过加入运动畸变补偿来改进NDT算法，以提高配准精度。
2. 算法表述
本节开发了一个快照最小二乘误差（MSE）估计器，通过与无畸变参考地图进行比较，推断出当前激光雷达扫描的相对姿态（平移和旋转）和运动畸变参数。我们将该算法称为速度校正迭代紧致椭球变换（VICET）。
首先，我们定义了两个相关的参考坐标系：一个与地图相关联，另一个与激光雷达定子相关联。我们用原点Q_M和由基向量M定义的坐标系来描述地图坐标系。我们定义了激光雷达定子坐标系，也称为机体坐标系，其中包括参考点Q_B和机体固定坐标系B。机体坐标系随时间相对于地图坐标系进行平移和旋转，如图2所示。虽然激光雷达转子围绕B的固定轴旋转，但我们不为转子定义一个独立的参考坐标系，因为激光雷达系统使用编码器数据将测量转换到定子坐标系B中。
图2. 激光雷达定子（或机身）B相对于地图坐标系M移动。为了说明目的，基座以2D形式显示，从上方视角观察。时间的推移由阴影灰色箭头表示，激光雷达束在时间t = 0到t = T之间扫描一圈，记录测量值（红点）。
地图匹配算法旨在寻找刚性变换，将原始的激光雷达扫描与地图在扫描开始时（t = 0）对齐。通过推断运动状态，我们可以根据扫描开始和扫描结束（t = T）之间的运动来解开激光雷达扫描。使用零阶保持模型，我们近似认为B相对于M的速度和角速度在时间t ∈ [0, T]内保持恒定。现在考虑一个激光雷达测量向量p^B_t。向量p^B_t描述了一个特征在世界上（或地图上）相对于机体坐标系参考点Q_B的位置，使用机体固定坐标（由尾标B指定）在时间t（由尾标指定）。下面的变换将这个测量转换为p^M_t，它以地图坐标相对于地图原点Q_M表示相同的测量值。
这里的旋转矩阵^MR^B_t将B基底转换为时间t时的M基底。平移矩阵m^M_t表示在时间t时，以地图固定坐标系中Q_M到Q_B的向量。作为下一步，我们将(1)式中的项进行分解，以将它们与初始状态和变化状态相关联。这种分解对于线性的平移项m^M_t 最容易进行。在零阶保持的假设下，我们可以使用杠杆原理重新表达平移，以在初始平移m^M_0和最终平移m^M_T之间进行插值。杠杆原理可以写成
这个表达式引入了以下简写符号：x_0 = m^M_0，∆x =m^M_T − m^M_0，以及s = t/T。通过LIDAR时间标签推导出的缩放时间s，被归一化为s ∈ [0, 1]。变量x_0和∆x被特别标识出来，因为这些是估计器将要推断的状态。
就像平移定义了初始状态和变化状态一样，旋转也需要定义类似的状态。在我们的实现中，我们使用一个由三个欧拉角Θ0组成的数组来描述初始的身体坐标系的方向。旋转矩阵是由欧拉角Θ0的函数f构建的。
方向的变化可以用一组三个角度∆Θ来描述。在时间T时的最终旋转矩阵可以通过以下方式与初始旋转矩阵和∆Θ相关联。
将初始状态和变化状态编译在一起，我们可以定义一个组合状态向量X ∈ R^{12×1}，通过将激光雷达扫描与地图进行比较来估计该向量。
为了将（1）与（5）中的状态相关联，我们使用泰勒级数展开，其中我们围绕一个合理的猜测展开（1）。在实现中，我们使用一个粗略的猜测来初始化位置和欧拉角，并使用零值来初始化变化参数。泰勒级数的仿射项为：
在这里，我们使用帽子符号来标识使用初始猜测计算得到的项。此外，我们引入了δ符号来标识线性扰动。
由于平移项是线性的，可以通过对（7）的导数得到平移扰动。
旋转扰动δR是通过旋转矩阵的导数得到的，在小角度限制下可以表示为：
这里的δΘ是通过[ ×]运算符转换为叉积的角度修正。对于一个通用的向量v，其元素为{v1, v2, v3}，该矩阵表示为：
尽管欧拉角通常不能求和，但在线性化之后，求和是一个很好的近似方法，因此δΘ可以用初始状态和变化状态来表示。将（7）-（10）代入（6），我们得到一个线性方程组，将校正状态与激光雷达数据相关联。
这里的状态扰动向量 δX ∈ \mathbb{R}^{12×1} ,并且雅可比矩阵 H_X ∈ \mathbb{R}^{3×12}
在P = [^M\hat{R}^B_t p^B_t ×]中，叉乘被嵌入其中。为了反转方程(11)并获得状态修正(12)，还有两个重要的细节需要解决。一个问题是数据关联，也就是说，由p^B_t描述的机体坐标系点必须与地图数据中对应的点p^M_t匹配。第二个问题是(11)只包含3个方程，因此必须同时解决多个这样的系统，以便在(12)中独立求解十二个状态。此外，为了避免条件数的不足，必须从广泛的s值范围内比较点（意味着通过激光雷达旋转扫描的广泛角度范围）；否则，如果s的范围很窄，(7)的结构会破坏δx_0和δ∆x的独立性，类似地，(10)的结构会破坏δΘ_0和δ∆Θ的独立性。在本节的其余部分，我们假设在整个s的广泛范围内成像了大量特征，以便获得一组良好条件的方程。因此，我们的主要关注点是数据关联。
为了解决数据关联问题，我们使用基于体素的策略，这是流行的NDT算法和我们的变种ICET所采用的方法。在这些算法中，点云位于一个三维网格中，由体素组成。如果体素网格在M帧中定义，那么将每个激光雷达点从HD地图分配到包含它的体素中就很简单。在将扫描转换到M帧使用初始猜测后，当前扫描中的点也可以与体素关联起来。然后，通过将地图点的分布与每个体素内当前扫描点的分布对齐，可以推断出改进的配准。最终，网格强制执行空间关联，从而避免了ICP、LOAM和其他方法中由于地图和当前扫描之间的点（或提取的特征）不正确匹配而引起的关联问题的模糊性。
为了比较给定体素中的点分布，我们对体素中的所有点应用公式（11）的期望值，得到如下结果。
在等式的右侧，旋转矩阵^M\hat{R}^B_t、平移向量\hat{m}^M_t和状态校正向量X对于点云中的所有点（因此也对于体素中的所有点）都是相同的。因此，这三个项可以移到期望值操作符的外面。
在这个方程中，矩阵H_X取决于s和P，对于体素中的每个点，它们的值略有不同。这些变化在实践中很小，通过定义^{(j)}\tilde{H}_X可以得到一个非常好的近似，它与H_X等价，只是在(13)中，s的值是在体素j的中点确定的，而P被设置为P = [ ^M\hat{R}^B_{mid} ^{(j)}µ^B ×]，其中旋转矩阵也在体素中点处计算。使用这个简化，我们可以通过设置^(j)\tilde{H}_X ≈ E(H_X)来近似(15)。剩下的两个期望值操作是在给定体素j中的激光雷达点之间的平均值。因此，我们引入j索引，并引入符号^{(j)}µ^M = E(p^M_t)和^{(j)}µ^B = E(p^M_t)。向量^{(j)}µ^M是落在体素j中的地图中所有点的平均位置。向量^(j)µ^B是落在体素j中的当前扫描点的平均位置，以车体坐标系表示。将^{(j)}\tilde{H}_X ，^{(j)}µ^M和^{(j)}µ^B代入(15)得到以下结果。
这里是通过同时解决所有体素上的方程获得的状态修正向量δX。通过具有足够数量的几何多样性的体素，系统是超定的，可以使用加权最小二乘解来减轻测量误差。
W是一个权重矩阵，H是将所有体素j的矩阵^{(j)}\tilde{H}_X连接起来的结果，而y是将相同体素上的可观测量^{(j)}y连接起来的结果。
3. 附加细节
在本节中，我们将介绍与在代码中实例化上述VICET算法相关的实际细节。
时间戳：可以通过使用光束角度ψ来近似激光雷达点的时间戳。重要的是不要将记录在不同时间（例如在扫描开始和结束时）的点分组在一起。我们通过在0°（扫描开始）处定义一个体素边界，然后移除ψ < 0°或ψ ≥ 360°的偶发异常点来解决混叠问题。
初始化：如（6）所述，VICET假设对初始姿态有一个合理的估计。我们通过首先运行标准的扫描匹配算法（具体为NDT）来获得初始姿态，然后VICET通过补偿运动畸变来改进这个估计。
扩展表面：NDT无法识别墙壁（以及跨越体素边界的其他平面表面）仅在表面法线方向上提供有用信息；为了增强收敛可靠性和准确性，我们采用ICET [4]的扩展表面抑制方法。
评论（0）
您还未登录，请登录后发表或查看评论