用于精確導航和場景重建的 3D 配準方法（ICRA 2021）

Differential Information Aided 3-D Registration for Accurate Navigation and Scene Reconstruction

作者：Jin Wu, Shuyang Zhang, Yilong Zhu, Ruoyu Geng, Zhongtao Fu, Fulong Ma and Ming Liu（香港科技大學 機器人與多感知實驗室(RAM-LAB)

論文地址：https://zhuanlan.zhihu.com/p/378591025?utm_medium=social&utm_oi=985951691249852416

本文核心是研究基于時間差分信息的點云配準問題，提出了在時間差分基礎上的點云匹配數學模型，時間差分信息通常來自于點云變化量測量和外部差分信息，如IMU、光流（Optical Flow）、場景流（Scene Flow）等。利用了kD樹對提出的新點云配準模型進行迭代最近點（ICP）估計，可以有效的利用時間差分信息對點云對應性帶來精確度上的提升。

摘要： 提出了一種新的用于點云配準的3-D 對齊方法，其中采用了測量點的時間差分信息。新問題被證明是一種新穎的多維優化。然后獲得此優化的解析解，這為使用 k-D 樹的進一步對應匹配奠定了基礎。最后，通過許多例子，我們表明新方法在實際實驗中具有更好的配準精度。

I 引言

A. Background

3-D配準是當前生物醫學成像、多維重建、機器人感知等前沿領域的熱門技術[1]、[2]。它找出兩個測量點云之間的仿射、剛性或非剛性變換，從而可以有效地合并多個 3-D 視圖。這些點云測量通常來自激光掃描儀、RGB-D 相機、結構光等。它也成為使用視覺測量的姿態確定、定位和建圖的關鍵技術[3]-[5]。潛在地，配準也有利于手眼校準問題[6]、[7]。兩個點間坐標系{B}和{R}之間的經典 3-D 配準問題可以指定為：

此問題使用最小二乘公式對齊{bi}和{ri} 的兩個點集。目標是找到最佳旋轉和 R 以及平移 T 以獲得最佳點云對齊。

B. Related Work

(1) 中的問題已經通過奇異值分解 (SVD, [8]) 和特征分解 (EIG, [9]) 得到廣泛解決。(1) 在點云匹配算法中也起著重要作用，包括迭代最近點（ICP，[10]）和幾何特征匹配[11]。問題（1）對于具有大量點的場景非常有效。然而，在工程中，匹配兩個點云是具有挑戰性的，因為迭代搜索是非凸的。目前，已經付出了大量努力來尋求全局最優 ICP 解決方案，例如 Go-ICP [12] 和 BnB [13]。然而，這些全局最優變體在計算上效率低。隨著點數的大幅減少，搜索的復雜性急劇增加。因此，配準性能相應下降，甚至可能導致工程使用失敗。這些應用程序中存在的一個關鍵特性是它們是實時實現的，因此激光掃描儀是連續移動的?？梢宰⒁獾剑哂刑魬鹦缘陌咐ǔＪ莿討B的。因此，應添加除點云測量之外的更多實質性信息以進行改進。連續測量點云的時間微分測量包含非常有用的 3-D 配準信息。此外， IMU等許多其他傳感器可以通過慣性積分 [14]、[15] 提供位姿的差分信息。本文正是基于這樣的想法，并提出了一種新的 3-D 配準方法，借助測量的 3-D 點的時間差分。所研究的問題之所以有效，有以下幾個原因：

以前的方法通常通過 IMU 測量 [16]、[17] 來近似適當的旋轉 R 來解決（1）。但是，如果 bi 和 ri 有偏差，則對應匹配可能會失敗。

考慮差分信息時，可以消除這些偏差，從而提高配準精度。也就是說，先前方法的測量是松耦合的，而所提出的解決方案的測量是緊耦合的。

在新問題中考慮了更多的時間差分信息。因此，將給所研究的問題帶來更多的實質性信息，從而產生更準確的結果。

C. Contributions

主要貢獻是

1) 差分 3-D 配準首次被提出并在數學上進行了表述。這允許一組新的改進配準方程。這些方程在存在多種條件時非常有用。

2) 根據提出的新方程導出解析解的結果，這允許實時計算高效計算。

3）我們建立多個k-D樹對不同時刻的點進行同時對應匹配。通過使用匹配良好的點對改進配準結果。

II 差分3D配準方法

A. Rigid Registration

其中 ? 表示增量項。這里增量測量 ?b 和 ?r 可以直接由連續輸出的差分給出。在本文中，旋轉和平移增量是使用加窗遞歸方法（WRA，[19]）估計的。WRA 可以用很小的窗口大小以及很少的歷史數據來估計和預測變量，并且被證明比傳統的內插或外推方法更有效。建立 (3) 的另一個核心任務是獲得 ?R 和 ?T 的準確估計，以便可以適當地改進配準。將超低成本 IMU 集成到 LIDAR 中已成為一種普遍做法。與 IMU 結合時，可以直接使用慣性導航機制計算 ?R 和 ?T：

平移增量ΔT可以通過IMU預積分獲得[14]：

然而，由于 IMU 中的偏差，(4) 和 (5) 中的積分將受到長期漂移的影響。有幾種方法可以補償這種漂移：

1）加速度計和磁力計的輔助：可以通過融合重力加速度和當地地磁場來消除旋轉漂移[21]。

2) 零更新輔助：如果激光掃描儀并不總是處于高動態的運動中，它有時會停止。在這種時候，可以調用零角速率更新 (ZARU) 和零速度更新 (ZUPT) 來補償旋轉和平移偏差 [22]。

3) 借助 3-D 激光掃描的內部信息：就像從圖像序列中估計 2-D 速度的光流一樣，場景流可用于從 3-D 測量中提取 3-D 運動。此外，在城市環境中，有許多具有豐富線和平面特征的建筑可以進一步處理以估計旋轉和平移 [23]。

4) 視覺-激光里程計輔助：使用相機和激光掃描儀的組合，可以同時測量2-D和3-D場景。然后它能夠進行視覺-激光里程計以進行連續的自我運動估計。

將這種時間差分信息添加到 3-D 配準中可以使測量點數增加一倍，從而在一定程度上提高成功率。配準問題（1）轉化為

由于（8）中點云的兩個子類別具有不同的尺度，我們需要通過引入點分散和歸一化來平衡這種不一致。最終的優化將是

最終，可以通過計算以下矩陣的最大特征值 λmax 及其關聯的特征向量 q 來解決新的優化（8）

B. Correspondence Matching

從連續的點云測量中，很難直接獲得 ?bi 和 ?ri。原因是bi,k和ri,k之間沒有對應關系，找到bi,k-1和bi,k也需要對應匹配，其中k表示時刻。為了解決這個問題，我們建立了以下匹配機制：

(13) 形成一個新的ICP形式，與來自連續時間點集的對應匹配。為了有效地獲得對應關系，我們使用 k-D 樹來加速匹配。用于比較的經典 ICP 公式是

從（14）中，我們可以看到ICP的匹配過程比提出的要簡單得多，這表明匹配可能在某些動力學條件下是不適定的。

III 實驗結果

A. Overview

為了驗證所開發算法的優越性，我們使用圖 2 中的無人機 (UAV) 平臺，配備機載剛性安裝 IMU (Honywell) 和 3-D 激光掃描儀 (Velodyne VLP-16)。IMU 和激光掃描儀之間的時間同步是通過實時運動學 (RTK) 全球導航衛星系統 (GNSS) 接收器的每秒脈沖 (PPS) 進行的。IMU 的采樣率為 100Hz，激光掃描儀的頻率為 20Hz。該系統還包含一個高精度的姿態和航向參考系統（AHRS）和一個精確的集成導航系統（INS），提供精確的姿態、速度和位置估計。GNSS提供真值。用于對應匹配的 k-D 樹的典型實現可以在 [26] 中找到。需要注意的是，由于使用了更多的 k-D 樹，因此順序 k-D 樹的計算負載高于經典 ICP。但是，請注意，所有 k-D 樹都可以通過并行化實現。因此，所提出的現代多核處理計算機方法的計算成本不會比經典 ICP 高多少。相反，由于考慮了更多的動態測量，因此提出的方法比傳統的 ICP 收斂得更快。出于比較的目的，我們使用 ICP 及其 IMUaided 變體進行比較。IMU輔助ICP使用IMU測量的旋轉作為預測。慣性姿態/速度/位置傳播的機制如（4）和（5）所示。

B. Results

使用開發的無人機平臺進行了 20 分鐘的試飛。無人機已在城市場景中遠程控制以進行 3D 重建。使用我們提出的方法的結果拼接原始點云測量。重建場景如圖 1 所示，某一地點的放大視圖如圖 3 所示。使用不同算法的軌跡如圖 4 所示。我們還獲得了無人機姿態確定和定位的統計結果，其中 顯示在表 I 中。我們還測試了所提出的方法和經典 ICP 的計算負載，其結果在表 II 中提供。在表II中，顯示了最大絕對誤差(MAE)、均方根誤差(RMSE)。

C. Discussions

從上面給出的結果，我們可以觀察到所提出方法的優越性。從圖 3 可以看出，所提出的方法在重建場景中合并了更豐富的點云掃描。原因是所提出的方法使用多個k-D樹進行點云點和差分點的對應匹配。因此，新設計的匹配與松耦合的 IMU 輔助 ICP 相比更準確，并且將匹配更多點進行點云合并。從圖 4 所示使用不同算法的軌跡，我們可以看出，與 RTK GNSS 接收機的參考軌跡相比，純慣性算法的精度最差，而所提出的方法具有最佳精度。IMU 輔助的 ICP 僅因其松耦合的性質而排名第二。所提出的緊耦合差分方法能夠消除原始數據中的點偏差，因此具有更好的魯棒性和準確性。因此，同時提高了姿態確定精度和定位精度，如表 I 所示。如第 III-A 節所示，有更多的 k-D 樹用于對應匹配。但是，由于所有 k-D 樹都是并行實現的，因此整體計算效率并不比經典 ICP 高多少（見表二）。然而，由于使用了更多的 k-D 樹，因此不可避免地必須占用更多的隨機存取存儲器 (RAM) 空間。

IV結論

在本文中，引入了一種包含時間差分信息的新點云配準公式，以實現混合配準性能。我們表明可以輕松獲取時間差分信息。因此，所設計的方案將是實用且易于實施的。通過實驗研究，已經發現新的公式可以導致更有效的對應匹配。然而，隨著測量維度的增加，我們不可避免地需要更多的空間進行在線處理。