用于精確導(dǎo)航和場景重建的 3D 配準(zhǔn)方法（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（香港科技大學(xué) 機器人與多感知實驗室(RAM-LAB)

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

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

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

I 引言

A. Background

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

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

B. Related Work

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

以前的方法通常通過 IMU 測量 [16]、[17] 來近似適當(dāng)?shù)男D(zhuǎn) R 來解決（1）。但是，如果 bi 和 ri 有偏差，則對應(yīng)匹配可能會失敗。

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

在新問題中考慮了更多的時間差分信息。因此，將給所研究的問題帶來更多的實質(zhì)性信息，從而產(chǎn)生更準(zhǔn)確的結(jié)果。

C. Contributions

主要貢獻是

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

2) 根據(jù)提出的新方程導(dǎo)出解析解的結(jié)果，這允許實時計算高效計算。

3）我們建立多個k-D樹對不同時刻的點進行同時對應(yīng)匹配。通過使用匹配良好的點對改進配準(zhǔn)結(jié)果。

II 差分3D配準(zhǔn)方法

A. Rigid Registration

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

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

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

1）加速度計和磁力計的輔助：可以通過融合重力加速度和當(dāng)?shù)氐卮艌鰜硐D(zhuǎn)漂移[21]。

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

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

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

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

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

最終，可以通過計算以下矩陣的最大特征值 λmax 及其關(guān)聯(lián)的特征向量 q 來解決新的優(yōu)化（8）

B. Correspondence Matching

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

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

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

III 實驗結(jié)果

A. Overview

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

B. Results

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

C. Discussions

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

IV結(jié)論

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