Efficient Multi-sensor Aided Inertial Navigation with Online Calibration

作者：Woosik Lee, Yulin Yang, and Guoquan Huang

論文、視頻地址：在公眾號(hào)「計(jì)算機(jī)視覺工坊」，后臺(tái)回復(fù)「導(dǎo)航系統(tǒng)」，即可直接下載。

摘要：在本文中，我們?cè)O(shè)計(jì)了一種多功能多傳感器輔助慣性導(dǎo)航系統(tǒng) (MINS)，可以有效地融合 IMU、相機(jī)、車輪編碼器、GPS 和 3D LiDAR 的多模態(tài)測量以及在線時(shí)空傳感器校準(zhǔn)。在工作 [1]-[3] 的基礎(chǔ)上，本次工作主要關(guān)注基于滑動(dòng)窗口的高效LiDAR集成。由于每個(gè) 3D LiDAR 掃描都包含大量 3D 點(diǎn)，這對(duì)實(shí)時(shí)性能構(gòu)成了巨大的挑戰(zhàn)，我們提倡使用包含環(huán)境結(jié)構(gòu)信息的平面圖，從稀疏的 LiDAR 點(diǎn)云中提取來有效地更新/校準(zhǔn)系統(tǒng)。所提出的 LiDAR 平面patch算法（包括提取、數(shù)據(jù)關(guān)聯(lián)和更新）被證明是高效且一致的。廣泛的 MonteCarlo 模擬和具有大規(guī)模城市駕駛場景的真實(shí)世界數(shù)據(jù)集已被用于驗(yàn)證所提出的 MINS 算法的準(zhǔn)確性和一致性。

I 引言

在線定位是自動(dòng)駕駛汽車的基本先決條件。迄今為止，已經(jīng)開發(fā)了許多算法來使用不同的傳感器實(shí)現(xiàn)高精度一致的 3D 定位。多傳感器融合通常用于實(shí)現(xiàn)這一目標(biāo)，原因有很多，包括更可靠的數(shù)據(jù)結(jié)果、更大的覆蓋范圍、適用性和更低的設(shè)備成本，但計(jì)算成本更高。在所有可能的導(dǎo)航傳感器中，IMU、相機(jī)、車輪編碼器、GPS 和 3D LiDAR 很有吸引力，因?yàn)樗鼈優(yōu)?3D 運(yùn)動(dòng)估計(jì)提供了足夠的信息，并且對(duì)商業(yè)產(chǎn)品具有良好的可訪問性。雖然原則上融合所有這些傳感器以實(shí)現(xiàn)良好的定位性能似乎是直接的，但很少有工作表明融合超過三種類型的傳感器，因?yàn)樗鼈兙哂胁煌奶匦浴⒂?jì)算量增加、異步性和校準(zhǔn)問題。此外，準(zhǔn)確的在線多傳感器校準(zhǔn)對(duì)于最佳傳感器融合至關(guān)重要，因?yàn)樗赡軙?huì)在導(dǎo)航過程中隨時(shí)間變化。因此，在這項(xiàng)工作中，我們開發(fā)了一種高效的多傳感器輔助慣性導(dǎo)航系統(tǒng) MINS，一種由多模態(tài)傳感器輔助的 INS，包括相機(jī)、車輪編碼器、GPS 和 3D LiDAR，同時(shí)在線校準(zhǔn)所有涉及的傳感器考慮到它們的異步性質(zhì)，并實(shí)現(xiàn)強(qiáng)大的準(zhǔn)確實(shí)時(shí)定位性能。在融合這些多模態(tài)傳感器時(shí)，LiDAR集成不能過分強(qiáng)調(diào)，因?yàn)樗悬c(diǎn)云攜帶的大量信息。由于 3D LiDAR 傳感器每秒可以提供超過 200 萬個(gè)點(diǎn)（例如，HDL-64E [4]），所有這些數(shù)據(jù)點(diǎn)的簡單融合可能不是實(shí)時(shí)的。與視覺測量不同，在不同的 LiDAR 掃描之間找到關(guān)聯(lián)點(diǎn)很困難，因?yàn)榭赡軟]有激光點(diǎn)擊中完全相同的物理位置。因此我們提倡使用平面塊，它包含點(diǎn)云最主要的結(jié)構(gòu)信息，便于獲得穩(wěn)健數(shù)據(jù)關(guān)聯(lián)，從 LiDAR 點(diǎn)云中提取以更新狀態(tài)并校準(zhǔn) LiDAR 和 IMU 之間的時(shí)空參數(shù)。特別是，基于我們之前的工作 [1]-[3]，我們開發(fā)了 MINS，這是一種實(shí)時(shí)、一致、緊耦合、多傳感器輔助的 INS 估計(jì)器，具有高效的 LiDAR 平面patch跟蹤，同時(shí)在所有傳感器之間進(jìn)行在線時(shí)空校準(zhǔn)。具體而言，這項(xiàng)工作的主要貢獻(xiàn)包括：? 我們?cè)O(shè)計(jì)了MINS，這是一種基于通用實(shí)時(shí)MSCKF [5] 的多傳感器輔助INS 估計(jì)器，可優(yōu)化有效地融合來自IMU、相機(jī)、車輪編碼器、GPS 和3D LiDAR 的測量值。? 我們開發(fā)了一種高效的 LiDAR 特征跟蹤算法，以從 3D LiDAR 點(diǎn)云中提取、合并和跟蹤具有適當(dāng)不確定性建模的平面塊。這些平面patch用于通過在線校準(zhǔn)進(jìn)行實(shí)時(shí) MSCKF 更新。? 我們?cè)诂F(xiàn)實(shí)模擬中廣泛評(píng)估了MINS，分析了詳細(xì)的 LiDAR 處理時(shí)間、校準(zhǔn)收斂性和估計(jì)器一致性。所提出的方法也在大規(guī)模城市駕駛數(shù)據(jù)集上得到驗(yàn)證。

II 相關(guān)工作

MSF-EKF [6] 是第一個(gè)將來自 IMU、相機(jī)、GPS 和壓力傳感器的通用相對(duì)和絕對(duì)測量相融合的在線空間外參校準(zhǔn)。整個(gè)處理時(shí)間以幾百毫秒為界。Hausman 等 [7] 還將 IMU、相機(jī)、GPS 和超寬帶 (UWB) 測量在 EKF 框架內(nèi)進(jìn)行外參校準(zhǔn)。然而，這兩項(xiàng)工作都只能處理相對(duì)較小規(guī)模的測量，無法解決傳感器之間的時(shí)間校準(zhǔn)問題。Shen 等 [8] 使用 UKF 來集成 IMU、相機(jī)、GPS、2D LiDAR、壓力高度計(jì)和磁力計(jì)，而無需校準(zhǔn)傳感器參數(shù)和分析處理時(shí)間，而他們最近的 VINS Fusion [9] 使用松耦合graph formulation融合 IMU、相機(jī)、GPS、磁力計(jì)和氣壓計(jì)。Suhr 等 [10] 基于粒子濾波融合了 IMU、相機(jī)、GPS、車輪以及符號(hào)道路標(biāo)記圖，而 Meng 等 [11] 在UKF 框架下融合了 IMU、GPS、距離測量儀器 (DMI) 和 LiDAR。所有這些方法都假定了已知的完美校準(zhǔn)并且沒有研究計(jì)算復(fù)雜性分析。最近，有一些研究工作主要是將 IMU、LiDAR 和相機(jī)與在線校準(zhǔn)相結(jié)合。例如，我們之前的工作 LIC-Fusion 1.0 [12] 和 2.0 [13] 使用從 LiDAR 點(diǎn)云中提取的平面進(jìn)行在線校準(zhǔn)。然而，該工作中考慮的傳感器不包括車輪編碼器和 GPS，并且僅限于通過 10 Hz 的 16 通道 LiDAR 顯示其實(shí)時(shí)性能，用于更新的平面數(shù)量較少。我們最近的工作 [1] 將 IMU、相機(jī)和 GPS 與時(shí)空校準(zhǔn)相結(jié)合，[2]、[14]結(jié)合IMU、照相機(jī)和車輪編碼器進(jìn)行時(shí)空校準(zhǔn)和車輪內(nèi)參校準(zhǔn)，而[15]融合了在MSCKFf框架內(nèi)，車輪，2D激光雷達(dá)和預(yù)先構(gòu)建的2D激光雷達(dá)地圖進(jìn)行在線校準(zhǔn)。多傳感器校準(zhǔn)算法的總結(jié)如表 I 所示。

雖然存在大量關(guān)于 LiDAR 處理的文獻(xiàn)，但我們只關(guān)注將 3D LiDAR 與其他傳感器實(shí)時(shí)集成的 SLAM 方法。特別是，Xu 等 [16] 使用information filter來利用不反轉(zhuǎn)大的測量協(xié)方差來融合 IMU 和 LiDAR，[17]-[19] 的作者從 LiDAR 測量中提取 LOAM [20] 特征并與其他傳感器，而Zhang等 [21] 引入了一種快速平面分割和地圖細(xì)化算法，可以節(jié)省計(jì)算時(shí)間并提高地圖質(zhì)量。Pathak 等[22] 從傳入的點(diǎn)云中提取平面并有效地找到對(duì)應(yīng)關(guān)系，從而提高效率。Shan 等 [23] 利用圖優(yōu)化將 IMU 與 LiDAR 融合在一起，僅在滑動(dòng)窗口內(nèi)保持局部 LiDAR 掃描以確保系統(tǒng)的實(shí)時(shí)性能，而 Maddern 等 [24] 將雙目視覺信息和 LiDAR 點(diǎn)云融合在一個(gè)滑動(dòng)窗口內(nèi)相機(jī)視場以改善圖像視差估計(jì)。雖然最近的重要研究工作也集中在深度學(xué)習(xí) [25]-[28] 上，但尚不清楚這些數(shù)據(jù)驅(qū)動(dòng)方法的效率和通用性如何。

III 基于MSCKF框架的多傳感器輔助INS

在介紹我們的激光雷達(dá)集成方法之前，在我們前期工作[1]-[3]的基礎(chǔ)上提出了基于MSCKF的多傳感器輔助INS融合IMU、相機(jī)、GPS和車輪編碼器的測量。具體地說，在時(shí)間tk時(shí)，狀態(tài)向量xk由當(dāng)前慣性狀態(tài)xIk和n個(gè)在相機(jī)測量時(shí)間捕獲的歷史IMU位姿xck組成：

我們定義了x=x^田x~，其中x是真實(shí)狀態(tài)，x^是它的估計(jì)，x~是誤差狀態(tài)。A. IMU Kinematic Model使用IMU線性加速度am和角速度wm測量值，狀態(tài)及時(shí)向前傳播：

我們?cè)诹阍肼暭僭O(shè)下基于標(biāo)準(zhǔn)慣性運(yùn)動(dòng)模型 f(·) [29] 將狀態(tài)估計(jì)和協(xié)方差從時(shí)間 tk 傳播到 tk+1：

B. Camera Measurement Model在與幀 xCk 相關(guān)聯(lián)的圖像窗口上檢測和跟蹤稀疏角點(diǎn)特征。由此產(chǎn)生的軸承測量值 zk 由下式給出：

堆疊與單個(gè)特征對(duì)應(yīng)的所有測量值并對(duì)特征位置執(zhí)行線性邊緣化（通過零空間投影）會(huì)產(chǎn)生殘差 [5]：

然后，這可以直接用于 EKF 更新，而無需在狀態(tài)中存儲(chǔ)特征，從而節(jié)省大量計(jì)算量并限制狀態(tài)大小。

C. Wheel Measurement Model與 [1] 中每次輪式編碼器讀數(shù)進(jìn)入時(shí)都向狀態(tài)添加clones不同，我們整合測量以獲得 2D 中兩次clones時(shí)間之間的相對(duì)姿態(tài)測量（旋轉(zhuǎn) Ok+1 Okθ 和平移 OkdOk+ 1)，這不會(huì)增加狀態(tài)向量。

D. GPS Measurement Model除了視覺/車輪測量更新，每當(dāng) ENU 框架 {E} 中的新 GPS 測量可用時(shí)，我們使用它來更新狀態(tài)，如 [2]。特別地，在時(shí)間步 k 處的 GPS 測量 Epgpsk 是：

由于傳感器的異步性，GPS 測量時(shí)間與clones時(shí)間不完全匹配，因此我們使用兩個(gè)邊界 IMU 位姿的線性插值 [32] 來計(jì)算 GPS 測量時(shí)間的 IMU 位姿。

E. Online Spatiotemporal Calibration雖然我們已經(jīng)簡要介紹了不同的傳感器集成模型來估計(jì)狀態(tài) (1)，但我們進(jìn)一步擴(kuò)展了我們的狀態(tài)以包括傳感器之間的傳感器內(nèi)參、外參和時(shí)間偏移，因此，可以執(zhí)行完整的校準(zhǔn)。我們省略了這些細(xì)節(jié)，感興趣的讀者可以參考[33]進(jìn)行IMU-camera校準(zhǔn)，[2]進(jìn)行IMU-wheel校準(zhǔn)，以及[1]進(jìn)行IMU-GPS校準(zhǔn)。

IV 高效的激光雷達(dá)測量系統(tǒng)更新

使用 LiDAR 測量有兩個(gè)主要困難：實(shí)時(shí)處理和數(shù)據(jù)關(guān)聯(lián)。由于 3D LiDAR 傳感器提供大量數(shù)據(jù)點(diǎn)，因此幾乎不可能實(shí)時(shí)跟蹤所有點(diǎn)。與相機(jī)測量不同，找到不同掃描之間的點(diǎn)對(duì)應(yīng)非常具有挑戰(zhàn)性，因?yàn)檫@些點(diǎn)通常不代表相同的物理位置。為了解決這些問題，我們有效地從點(diǎn)云中提取平面塊，并且由于它們包含主要結(jié)構(gòu)信息，因此我們可以在掃描過程中有效地跟蹤它們。為了描述平面patch (pp)，我們使用該平面的中心點(diǎn) p 和 Hesse 法線 n：

利用MSCKF和pp表示，我們能夠使用在局部窗口中收集的所有激光雷達(dá)測量值，有效地更新狀態(tài)（1）。接下來，我們?cè)敿?xì)解釋了從激光雷達(dá)點(diǎn)云中提取pp、合并pp、查找數(shù)據(jù)關(guān)聯(lián)和執(zhí)行更新/校準(zhǔn)的方法。

A. Plane Patch Extraction

當(dāng)一個(gè)新的激光雷達(dá)掃描進(jìn)入時(shí)，我們稀疏地選擇點(diǎn)并為每個(gè)選定點(diǎn)的鄰近點(diǎn)來構(gòu)建一個(gè)局部點(diǎn)云pi（參見圖2左中的綠色和藍(lán)色點(diǎn)）。

我們利用kd-tree快速找到鄰近的點(diǎn)。一旦有了pi，我們利用[34]方法快速提取pp，并進(jìn)一步擴(kuò)展該方法來計(jì)算提取平面塊的噪聲協(xié)方差。我們首先通過減去云的中心點(diǎn)pc：

現(xiàn)在，可以從該點(diǎn)云中提取平面，參數(shù)化為（通過沿z分量歸一化）：

注意，當(dāng)平面法線的z分量接近零時(shí)，這個(gè)表達(dá)式可以是奇異的，而通過選擇不同的歸一化軸可以很容易地解決這個(gè)問題。然后我們可以解決以下線性最小二乘：

然后可以計(jì)算平面法線nz：=[a b 1]T。請(qǐng)注意，計(jì)算出的平面通過了原點(diǎn)，因?yàn)閐=0，因此pc的中心點(diǎn)也成為了提取的pp的中心。為了避免從非平面或不良條件下提取pp，我們檢查了點(diǎn)到平面的平均距離和nc的大小，然后在它們沒有通過閾值時(shí)丟棄?；谠键c(diǎn)測量噪聲跟蹤平面不確定性是很直接的：

請(qǐng)注意，有更穩(wěn)健的方法來用更高的計(jì)算成本來解決這個(gè)問題，比如特征值分解。然而，計(jì)算速度是至關(guān)重要的，因此我們選擇使用所提出的方法，因?yàn)榻鉀Q方案可以立即從（15）中計(jì)算出來。我們?cè)谝粋€(gè)新的點(diǎn)云中配置這個(gè)新的pp={pc,nc}，通過指定中心點(diǎn)pc作為點(diǎn)的位置，nc作為點(diǎn)的附加信息。請(qǐng)注意，由每個(gè)pp組成的局部點(diǎn)云pi也被跟蹤到以后的合并步驟（IV-B）。平面斑點(diǎn)云(pppc)是新點(diǎn)云的名稱，以區(qū)別于激光雷達(dá)點(diǎn)云。通過將pp視為點(diǎn)并構(gòu)建pppc，我們可以方便地再次利用樹樹從選定的pp中搜索相鄰的pp。

B. Plane Patch Merging

我們需要合并提取的pp。如果激光雷達(dá)是在一個(gè)結(jié)構(gòu)化的環(huán)境中，特別是在一堵高墻附近，許多pp代表相同的平面，這是多余的。這種冗余不能被忽略，因?yàn)樗梢燥@著增加計(jì)算量，特別是當(dāng)從幾個(gè)pppc具有相同的平面時(shí)。此外，如果點(diǎn)在同一平面上，提取p的點(diǎn)越多，就越準(zhǔn)確、越一致。因此，我們?cè)噲D在從激光雷達(dá)點(diǎn)云中提取它后，合并在同一平面上的pp。為此，我們首先從pppc中稀疏地選擇pp，并使用kd-tree找到每個(gè)相鄰的pp。一旦找到相鄰的pp，我們通過檢查選定的pps={ns，ps}和每個(gè)相鄰的ppn={nn,pn}之間的以下剩余距離，迭代地檢驗(yàn)“同一平面假設(shè)”：

殘差（23）的第一個(gè)原始值的物理意義是平面法線的平行性，第二行是點(diǎn)到平面的距離。在測試了所有的ppn后，ppn的原始激光雷達(dá)點(diǎn)通過了測試，pps收集不重復(fù)，從收集的激光雷達(dá)點(diǎn)云創(chuàng)建一個(gè)新的平面補(bǔ)丁，使用的pps和ppn從云中刪除。圖中示。2（右）顯示了如何針對(duì)單一pps案例處理補(bǔ)丁之間的合并。重復(fù)此操作，直到處理所有pps，根據(jù)環(huán)境的結(jié)構(gòu)，整個(gè)合并步驟可以重復(fù)幾次。

C. Data Association

與前面的步驟不同，所有的操作都是在相同的參考幀內(nèi)完成的，在需要狀態(tài)信息的數(shù)據(jù)關(guān)聯(lián)/更新過程中，pp必須將一幀轉(zhuǎn)換為另一幀。然而，由于傳感器之間的異步性，我們的狀態(tài)在精確的激光雷達(dá)測量時(shí)間內(nèi)沒有一個(gè)IMU姿態(tài)來表示測量。我們使用邊界clone的線性插值[32]來表示測量時(shí)的IMU姿態(tài)，與每次激光雷達(dá)測量時(shí)添加clone相比，我們節(jié)省了大量的計(jì)算量。因此，我們首先收集所有可以找到邊界位姿的pppc進(jìn)行進(jìn)一步步驟，如圖3所示。

圖3 ?為更新選擇 LiDAR 測量。{I}和 {L}分別表示測量時(shí)的歷史 IMU 位姿和 LiDAR 幀。在此示例中，收集了 tk 和 tk+1 處的 LiDAR 測量以進(jìn)行更新，而在 tk?1 處則不是因?yàn)?IMU 位姿無法限制測量.假設(shè)在 tk 的 LiDAR 幀{L} 中測量的 Lkpppc 是收集到的最舊的 pppc，我們想要找到 Lkppi ∈Lkpppc 的關(guān)聯(lián)。對(duì)于每個(gè)收集到的 pppc，我們使用 kd-tree 找到最接近 Lk ppi 的 pp，并檢查它們之間的殘差。例如，用于測試 Lkppi 和 Lk+1ppj ∈ Lk+1pppc 的殘差可以描述為：

如果殘差小于閾值，我們認(rèn)為兩個(gè) pp代表同一平面。請(qǐng)注意，我們?cè)谶@里只檢查殘差；由于其與矩陣求逆相關(guān)的過程，Mahalanobisdistance檢驗(yàn)可以在統(tǒng)計(jì)上更加一致但繁重?？赡艿腻e(cuò)誤關(guān)聯(lián)將在下一個(gè)更新步驟中被過濾掉。我們?cè)谑占? pppc 中迭代地找到 Lkpp 的所有關(guān)聯(lián)。

D. Plane Measurement Update and Calibration

雖然平面的 pp 表示可以提供快速的數(shù)據(jù)關(guān)聯(lián)，但由于其非最小表示如果天真地用于更新，則存在一些與計(jì)算相關(guān)的問題。由于平面具有 3DOF，而 pp 使用 6DOF 向量 (p，n) 來表示這樣的平面，這種過度參數(shù)化可能會(huì)顯著增加計(jì)算負(fù)擔(dān)和數(shù)值不穩(wěn)定性。因此，我們將平面表示從平面patch (pp) 更改為最近點(diǎn) (cp) [35]，這是平面的最小表示，并且可以制定緊湊的殘差函數(shù)以執(zhí)行有效的 MSCKF 更新。具體來說，cp 可以被認(rèn)為是位于平面上并且最接近被測幀原點(diǎn)的 3D 點(diǎn)?？梢允褂镁哂幸韵玛P(guān)系的 Hesse 法向量 n 和距離標(biāo)量 d 來描述這種 cp 表示：

線性化：

如第 IV-C 節(jié)所述，可能存在錯(cuò)誤關(guān)聯(lián)，因此我們使用殘差 (36) 執(zhí)行以下 Mahalanobisdistance 測試：

如果Mahalanobisdistance小于閾值，我們認(rèn)為關(guān)聯(lián)是有效的，等式 (36) 可以直接用于 MSCKF 更新，而無需在狀態(tài)中存儲(chǔ)特征，從而節(jié)省大量計(jì)算量。從云中刪除使用過的平面patch以避免信息的重用。

VI 實(shí)驗(yàn)分析

我們?cè)谡鎸?shí)世界數(shù)據(jù)集 KAIST Urban39 [36] 中進(jìn)一步評(píng)估了 MINS，該數(shù)據(jù)集在具有 11.06 公里長軌跡的市區(qū)收集，并使用雙目相機(jī)、IMU、車輪編碼器、GPS 和 16 線 LiDAR 進(jìn)行估計(jì)。基于 VIO，不同的傳感器組合與校準(zhǔn)一起進(jìn)行測試，每種算法的結(jié)果軌跡如圖 6 所示。與數(shù)據(jù)集提供的地面實(shí)況相比，每種算法的方向和位置的均方根誤差 (RMSE) 為 總結(jié)在Table III中。

總的來說，VIO 出現(xiàn)了規(guī)模問題，結(jié)合額外的傳感器能夠解決這個(gè)問題。GPS-VIO 利用 GPS 的全球測量結(jié)果，在三個(gè)傳感器組合中顯示出最佳結(jié)果。LiDAR-VIO 的運(yùn)行速度比實(shí)時(shí)速度快約 2 倍，尤其顯示出良好的 z 軸估計(jì)結(jié)果。這是因?yàn)?LiDAR 以向下 45 度的角度安裝，從道路表面提供大部分掃描，因此道路上提取的平面可以防止 z 方向漂移。所提出的MINS融合了所有傳感器，并在實(shí)時(shí)運(yùn)行時(shí)記錄了所有校準(zhǔn)參數(shù)收斂的最準(zhǔn)確結(jié)果，顯示了全局準(zhǔn)確和局部精確定位性能。

VII 總結(jié)

在本文中，我們開發(fā)了一種高效且一致的基于 MSCKF 的多傳感器輔助 INS（MINS），它融合了 IMU、相機(jī)、車輪、GPS 和 LiDAR 測量，并對(duì)所有傳感器進(jìn)行在線時(shí)空校準(zhǔn)。特別是，我們一直主要專注于有效集成 LiDAR 測量，這是實(shí)時(shí)多傳感器定位的瓶頸問題，因?yàn)樗臄?shù)據(jù)量很大，并建議從點(diǎn)云中提取平面塊并對(duì)其進(jìn)行跟蹤 掃描以形成 MSCKF 更新的運(yùn)動(dòng)約束。仿真結(jié)果表明我們的方法能夠?qū)崟r(shí)集成 64線 20 Hz LiDAR 以及校準(zhǔn)收斂。所提出的 MINS 也在真實(shí)數(shù)據(jù)集中得到了驗(yàn)證，顯示了其全局準(zhǔn)確和局部精確的實(shí)時(shí)定位性能。將來，我們將研究如何有效地將閉環(huán)檢測包含到系統(tǒng)中。