＜论文阅读＞ RF-LIO：高动态环境中首先移除动态点的紧耦合激光雷达惯性里程计
继续在家读文献～ 希望下周能继续打工，最近还是更多的关注激光里程计相关的方向。
论文地址：Removal-First Tightly-coupled Lidar Inertial Odometry in High Dynamic Environments
摘要：同步定位和建图（SLAM）被认为是智能车辆和移动机器人的基本能力。然而，当前大多数激光雷达 SLAM 方法都是基于静态环境的假设。因此，在具有多个移动对象的动态环境中定位实际上是不可靠的。论文提出了一种动态 SLAM 框架 RF-LIO，它建立在 LIO-SAM 的基础上，增加了自适应多分辨率距离图像，并首先使用紧耦合的激光雷达惯性里程计去除动态物体，然后将激光雷达扫描与子图匹配。因此，即使在高动态环境中也可以获得准确的姿势。提出的 RF-LIO 在自己收集的数据集和公开 Urbanloco 数据集上进行评估。实验结果表明，在高动态环境下与 LOAM 和 LIO-SAM 相比，所提出的 RF-LIO 的绝对轨迹精度可以分别提高 90% 和 70%。 RF-LIO 是高动态环境中最先进的 SLAM 系统之一。
一、引言
稳定而精准的定位是智能车辆或移动机器人的前提。基于激光雷达的 SLAM 技术可以在没有 GPS 的情况下提供稳健的厘米级状态估计和高精度点云地图，因此最近备受关注。例如，LOAM 提出了一种基于边缘和平面的特征提取策略，已成为低漂移和实时状态估计和建图最广泛的方法。 LIO-SAM 在LOAM的基础上，采用紧耦合激光雷达惯性里程计（LIO）和关键帧策略，进一步提高处理速度和轨迹精度。然而，这些主流的 SLAM 系统和点云配准方法是基于静态环境的假设，即背景中没有移动物体。事实上，自主系统通常工作在具有大量移动物体的现实环境中，例如车辆、行人等。由于传感器的视野（FOV）被遮挡，大部分特征点落在移动物体而不是静态地图上，如图 1 所示。
之前基于静态环境假设的方法将失效。此外，在点云地图中提取道路标记、交通标志和其他关键静态特征也具有挑战性，因为移动物体的轨迹可能会遮挡它们。因此，提高SLAM在动态环境中的性能至关重要。
动态 SLAM 的一个简单的解决方案是构建一个仅包含静态对象的地图，即从点云地图中删除移动的对象点。或者例如，Suma++ 首先通过 scan-matching 获得多次测量的准确位姿，然后迭代更新通过融合语义信息来生成的地图体素的状态（动态或静态）。然而，这个前提在高动态环境中往往是站不住脚的，因为大量的运动物体会使现有的 scan-matching 方法失效。因此，这些方法陷入了一个先有鸡还是先有蛋的问题：运动物体的点去除依赖于准确的位姿，而当点云图包含运动物体时，无法获得准确的位姿。上述问题使得这些 SLAM 方法在高动态环境下无法取得良好的效果。此外，深度学习方法对训练数据的依赖很大，这也限制了 Suma++ 从一个场景到其他场景的应用。
本文提出了一种新的先剔除、紧耦合激光雷达惯性里程计框架，即RF-LIO，以解决高动态环境中的SLAM问题。先剔除是指所提出的RF-LIO首先去除没有准确姿势的运动物体，然后采用 scan-matching 。当新的 scan 到达时，RF-LIO 不会立即执行扫描匹配以获得准确的位姿，因为它很容易受到动态环境的影响，相反，我们使用紧耦合惯性测量单元 (IMU) 里程计来获得粗略的初始状态估计，然后 RF-LIO 可以利用自适应分辨率距离图像初步去除环境中的运动点。在初步去除运动点后，RF-LIO 使用 scan-matching 来获得相对更准确的位姿。通过这些迭代去除和 scan-matching 步骤，RF-LIO 最终可以在高动态环境中获得准确的位姿。我们使用运动物体的去除率和绝对轨迹精度来评估 RF-LIO 在自己收集的数据集和开放的 Urbanloco 数据集的效果。实验表明，RF-LIO的平均运动物体去除率为96.1%，与LOAM和LIO-SAM相比，RF-LIO的绝对轨迹精度分别提高了90%和70%。
三、REMOVAL-FIRST 的激光雷达惯性里程计
A、系统概述
图 2 显示了 RF-LIO 的总体框架，它由三个主要模块组成：IMU 预积分、特征提取和建图。首先，IMU 预积分模块用于推断系统运动并生成 IMU 里程计。然后，特征提取模块补偿点云的运动畸变。通过评估点的粗糙度来提取边缘和平面特征。
建图模块是我们提出方法的关键模块，要在没有准确位姿的情况下先去除动态物体，有几个关键步骤：
- 初始位姿是通过 IMU 里程计获得的。然后使用 IMU 预积分和 scan-matching 之间的误差来确定初始分辨率（即每个像素对应多少个 FOV 角度）。
- RF-LIO 使用此初始分辨率从当前激光雷达扫描和相应的子图分别构建距离图像。
- 通过比较它们的能见度，去除子图的大部分动态点。
- RF-LIO 将激光雷达扫描与子图进行匹配，并判断 scan-matching 是否收敛。如果是收敛的，经过图优化后，用最终的高分辨率去除当前关键帧中剩余的动态点，否则，将生成新的分辨率，并重复步骤2、3、4。
B. IMU 预积分和初始位姿
我们首先将世界坐标系定义为 W W W，将与机器人坐标系重合的 I M U IMU IMU 坐标系记为 B。系统的状态可以表示如下：
x = [ R T , p T , v T , b T ] T x = \begin{bmatrix} R^T,p^T,v^T,b^T \end{bmatrix}^T x=[RT,pT,vT,bT]T
其中 R ∈ S O ( 3 ) R ∈ SO(3) R∈SO(3) 是旋转矩阵， p ∈ R 3 p ∈ \mathbb R^3 p∈R3 是位置向量， v v v 是速度向量， b b b 是 IMU 偏置向量。 b b b 由缓慢变化的加速度计偏置 b a b_a ba 和陀螺仪偏置 b g b_g bg 组成。
C. IMU 预积分误差和初始分辨率
在使用 IMU 测量值推断系统运动的过程中，不可避免地会出现与 ground truth 的偏差，这使得激光点与对应的地图点之间的查询结果是矛盾的。为了解决这个问题，Palazzolo 和 Stachniss 提出了一种基于窗口的方法（即不是像素到像素，而是像素到窗口的比较）。Removert 提出了一种更方便的方法，该方法使用具有不同分辨率的距离图像。但是，Removert 使用固定分辨率，因为它基于精准的定位信息。但是 RF-LIO 需要在扫描匹配之前去除动态点（即没有准确的位姿）。因此，我们使用 IMU 预积分和 scan-matching 之间的位姿误差来动态生成初始分辨率。
当执行 scan-matching 时，IMU预积分的平移和旋转误差可以计算如下：
E R k = L o g ( ( Δ R k − 1 , k E x p ( J Δ R g b k − 1 g ) ) T R B W k − 1 R W B k ) (7) E^k_R = Log((\Delta R_{k-1,k}Exp(J^g_{\Delta R}b^g_{k-1}))^TR^{k-1}_{BW}R^k_{WB}) \tag{7} ERk=Log((ΔRk−1,kExp(JΔRgbk−1g))TRBWk−1RWBk)(7) E v k = R B W k − 1 ( v B k − v B k − 1 − g W Δ t k − 1 , k ) − ( Δ v k − 1 , k + J Δ v g b k − 1 g + J Δ v a b k − 1 a ) (8) E^k_v = R^{k-1}_{BW}(v^k_B-v^{k-1}_B-g_W\Delta t_{k-1,k}) - (\Delta v_{k-1,k}+J^g_{\Delta v}b^g_{k-1}+J^a_{\Delta v}b^a_{k-1}) \tag{8} Evk=RBWk−1(vBk−vBk−1−gWΔtk−1,k)−(Δvk−1,k+JΔvgbk−1g+JΔvabk−1a)(8) E p k = R B W k − 1 ( p B k − p B k − 1 − v B k − 1 Δ t k − 1 , k − 1 2 g W Δ t k − 1 , k 2 ) − ( Δ p k − 1 , k + J Δ p g b k − 1 g + J Δ p a b k − 1 a ) (9) E^k_p = R^{k-1}_{BW}(p^k_B - p^{k-1}_B-v^{k-1}_B\Delta t_{k-1,k}-{1\over 2}g_W \Delta t^2_{k-1,k})-(\Delta p_{k-1,k}+J^g_{\Delta p}b^g_{k-1}+J^a_{\Delta p}b^a_{k-1}) \tag{9} Epk=RBWk−1(pBk−pBk−1−vBk−1Δtk−1,k−21gWΔtk−1,k2)−(Δpk−1,k+JΔpgbk−1g+JΔpabk−1a)(9) E b k = b k − b k − 1 (10) E^k_b = b_k -b_{k-1}\tag{10} Ebk=bk−bk−1(10)
通过(7)、(8)、(9)、(10)，我们可以得到前一个关键帧 k k k 的位姿误差，但是，在scan-matching之前，使用上述方法无法得到当前关键帧 k + 1 k+1 k+1 的 IMU 预积分误差。为了得到当前关键帧 k + 1 k+1 k+1 的误差，我们使用非线性系统的误差传递关系：
δ X k = δ X k − 1 + δ X ˙ k − 1 Δ t \delta X_k = \delta X_{k-1}+\delta \dot{X}_{k-1}\Delta t δXk=δXk−1+δX˙k−1Δt
我们只对 δ X δ X δX 的 δ θ δ θ δθ 和 δ p δ p δp 感兴趣，因此我们的方法可以写成如下：
[ δ θ k + 1 δ p k + 1 ] = A [ δ θ k δ p k δ v k δ b k a δ b k g ] + B [ η g η a η b g η b a ] (12) \begin{bmatrix} \delta \theta^{k+1} \\ \delta p^{k+1} \\ \end{bmatrix} =A \begin{bmatrix} \delta \theta^k \\ \delta p^k \\ \delta v^k \\\delta b^a_k \\ \delta b^g_k \\ \end{bmatrix} + B \begin{bmatrix} \eta^g \\ \eta^a \\ \eta_{b^g} \\\eta_{b^a} \\ \end{bmatrix} \tag{12} [δθk+1δpk+1]=A⎣⎢⎢⎢⎢⎡δθkδpkδvkδbkaδbkg⎦⎥⎥⎥⎥⎤+B⎣⎢⎢⎡ηgηaηbgηba⎦⎥⎥⎤(12) A = [ I − [ ω ] × Δ t 0 0 0 − I Δ t 0 I I Δ t 0 0 ] (13) A = \begin{bmatrix} I-[\omega]_{\times}\Delta t & 0&0&0& -I\Delta t \\ 0&I&I\Delta t &0&0 \\ \end{bmatrix} \tag{13} A=[I−[ω]×Δt00I0IΔt00−IΔt0](13) B = [ I Δ t 0 0 0 0 0 0 0 ] (14) B = \begin{bmatrix} I\Delta t & 0&0&0 \\ 0&0&0&0 \\ \end{bmatrix} \tag{14} B=[IΔt0000000](14)
通过预测 IMU 里程计定位误差，我们可以获得距离图像的初始分辨率。我们使用以下经验公式将平移和旋转误差转换为分辨率：
r = a δ p + δ θ (15) r = a\delta p + \delta \theta \tag{15} r=aδp+δθ(15)
其中 α α α 为 0 到 1 之间的值，用于平衡平移误差和旋转误差对分辨率的权重。
为了平衡实时性和去除率，避免错误预测分辨率造成的影响，我们设置了最小分辨率 r 0 r_0 r0，并适当放大了预测分辨率 r r r 。最终的初始分辨率 r f r_f rf 定义如下：
r f = m a x ( β r , r 0 ) (16) r_f = max(\beta r,r_0) \tag{16} rf=max(βr,r0)(16)
其中 β β β 为大于 1 的系数。 r 0 = v e r t i c a l F O V v e r t i c a l r a y s r_0 ={vertical \; FOV \over vertical \; rays} r0=verticalraysverticalFOV，可有效避免距离图出现空像素值。
D. 深度图像构建和动态点移除
我们首先定义 F k + 1 F_{k+1} Fk+1 为当前关键帧 scan， M k M_k Mk 为对应的子图，即围绕 F k + 1 F_{k+1} Fk+1 滑动窗口方法创建的点云图。此外，为了平衡动态点的去除率和实时性能，我们使用 完整 scan 与特征子图进行比较。这是因为具有多个关键帧的特征子图具有与 完整 scan 相似的密度，并且比完整子图具有更少的点。然后，我们将点云划分为两个互斥的子集：动态点集 ( ⋅ ) D (·)^D (⋅)D 和静态点集 ( ⋅ ) S (·)^S (⋅)S。形式上，上述问题表示为：
M = M D ∪ M S F = F D ∪ F S M = M^D \cup M^S \quad F = F^D \cup F^S M=MD∪MSF=FD∪FS其中 ( ⋅ ) D ∩ ( ⋅ ) S = ∅ (·)^D \cap (·)^S =\emptyset (⋅)D∩(⋅)S=∅
使用与现有方法类似的理念，我们使用预计距离图像的能见度来执行动态点识别。如图 3 所示，距离图像 ( i , j ) (i,j) (i,j) 的像素值 I k + 1 , i j I_{k+1,ij} Ik+1,ij 定义为点 p ∈ R 3 p ∈\mathbb R^3 p∈R3 到第 k + 1 k + 1 k+1 个关键帧的局部坐标系 B k + 1 B_{k+1} Bk+1 的距离：
I k , i j M = m i n p ∈ P i j M d i s t ( p ) I k + 1 , i j F = m i n p ∈ P i j F d i s t ( p ) (18) I^M_{k,ij} = \underset{p \in P^M_{ij}}{min} \; dist(p) \quad I^F_{k+1,ij} = \underset{p \in P^F_{ij}}{min} \; dist(p) \tag{18} Ik,ijM=p∈PijMmindist(p)Ik+1,ijF=p∈PijFmindist(p)(18)
其中距离图像大小由给定的分辨率和激光雷达的水平和垂直 F O V FOV FOV 范围决定。 P i j M o r F P^{M or F}_{ij} PijMorF 是点 P i j M o r F P^{M or F}_{ij} PijMorF 的球坐标（即方位角 i i i 和仰角 j j j ）。
然后，子图点和 scan 点的能见度通过它们的矩阵减法计算为：
I k + 1 D i f f = I k + 1 F − I k M (19) I_{k+1}^{Diff} = I^F_{k+1} -I^M_k \tag{19} Ik+1Diff=Ik+1F−IkM(19)
如果其对应的 I k + 1 , i j D i f f I^{Diff}_{k+1,ij} Ik+1,ijDiff 像素值大于自适应阈值 τ τ τ，我们确定点 p ∈ P i j M o r F p∈P^{M or F}_{ij} p∈PijMorF 是动态的。最后，动态点定义为： M k D = { M k ∣ I k + 1 D i f f > τ } (20) M^D_k = \lbrace M_k|I^{Diff}_{k+1} > τ \rbrace \tag{20} MkD={Mk∣Ik+1Diff>τ}(20) F k + 1 D = { F k + 1 ∣ I k + 1 D i f f < − τ } (21) F^D_{k+1} = \lbrace F_{k+1}|I^{Diff}_{k+1} < -τ \rbrace \tag{21} Fk+1D={Fk+1∣Ik+1Diff<−τ}(21) τ = γ d i s t ( p ) (22) τ = \gamma dist(p)\tag{22} τ=γdist(p)(22)
其中 γ γ γ 是相对于点距离的灵敏度。我们建议将 γ γ γz 设置为图4（c）所示的最终分辨率的最大值，这可以有效避免由于错误的预测分辨率而将静态点误认为是动态点的情况。
五、结论
我们提出的 RF-LIO 在高动态环境中执行实时和稳健的状态估计和建图。 RF-LIO 采用移动物体去除优先算法，采用紧耦合 LIO 解决高动态环境中先去除动态点或先扫描匹配的先有鸡还是先有蛋的问题。所提出的自适应距离图像运动点去除算法不依赖任何先前的训练数据，也不受运动物体的类别和数量的限制。因此，RF-LIO 可以稳健地应用于各种场景。然而，RF-LIO 仍有一些正在进行的工作。在非常开阔的环境中，如果周围环境中没有对应的远点，则基于可见性的距离图像方法无法去除运动点。另一个问题是，当移动物体完全挡住了我们传感器的 FOV 时，这种方法就不合适了。