當前位置：首頁 > 人文社科 > 生活经验 >内容正文

生活经验

一种视觉惯性+激光传感器的SLAM系统

發布時間：2023/11/27 生活经验 39 豆豆

生活随笔收集整理的這篇文章主要介紹了一种视觉惯性+激光传感器的SLAM系统小編覺得挺不錯的,現在分享給大家,幫大家做個參考.

一種視覺慣性+激光傳感器的SLAM系統

這篇博客
論文摘要
一些假設和標注
系統總覽
- VI 里程計
- 掃描匹配（scan matching）優化
提高系統魯棒性的措施
閉環檢測和臨近檢測
- 全局位姿圖優化
總結

這篇博客

?這篇論文“Robust High Accuracy Visual-Inertial-Laser SLAM System”發表于2019年的 IROS 會議上。它提出了一個融合了相機、慣性元件、激光這三種傳感器的系統。通過不同傳感器之間的互補作用，該系統，較之于視覺慣性系統和激光系統，具有更好的魯棒性。

論文摘要

?純視覺SLAM在光照變化明顯、紋理較少的環境中容易跟蹤丟失，因此研究者將慣性測量元件(IMU)加入到視覺SLAM中，組成 VI-SLAM (視覺慣性SLAM)。但作者發現， IMU 只能在短期內解決純視覺SLAM 所面對的問題，如果機器長期工作在光照變化明顯、紋理缺失的環境中，系統仍會跟蹤丟失(因為IMU的偏差是隨機變化的，長時間不修正會直接影響位姿估計)。此外，作者還指出了激光SLAM的不足：在結構性特征缺失的環境中（(比如在走廊)）會跟蹤丟失。所以作者將視覺慣性、激光兩類SLAM相結合，搭建了基于三種傳感器工作的更加魯棒的系統。該系統可大致分為視覺慣性和激光兩大位姿估計模塊。它先通過視覺慣性模塊估計出位姿的初值，再根據激光掃描的結果完成位姿的優化。兩個模塊可以來聯合工作，也可以各自獨立工作。這也是該系統魯棒的原因：當一個模塊跟丟了，系統也可以只通過剩余模塊完成定位和建圖。

一些假設和標注

1、假設相機內參 $K$ 已知，同時三個傳感器(相機、IMU、激光)之間的外參矩陣（相對位姿變換關系）也已知；
2、默認這三個傳感器在時間上是同步的！！(這點很重要)；
3、三個傳感器都有各自的坐標系，依次記為相機： $C$ ，IMU： $I$ ，激光： $L$ 。此外將世界坐標系記為 $W$ 。在初始化后，將 $L$ 坐標系作為主要觀測坐標系（系統主要記錄3D點在 $L$ 上的坐標）；
4、使用 $S_{i}$ 表示 $i$ 時刻的某個坐標系。并用 $TbaT^{a}_{b}$ 表示從 b 到 a 的位姿變換矩陣；
5、用 $XjSX^{S}_{j}$ 表示地圖點 $j$ 在 $S$ 坐標系上的三維齊次坐標；
6、為了方便，下文用 VI 代表 “視覺慣性” 。

系統總覽

?系統先通過一個緊耦合的 VI 方法估計位姿，再通過激光掃描的結果來優化該估計值，最后再完成建圖。整個過程的框圖如下所示：

?下面介紹每個模塊。

VI 里程計

?該系統的 VI 前端是基于 VINS-Mono 系統實現的，主要細節可查看 VINS-Mono 的論文。

掃描匹配（scan matching）優化

?（這部分主要涉及激光定位，主要是 LOAM系統中的知識）掃描匹配（scan matching）模塊的工作流程框圖如下所示：

?激光雷達會連續檢測到地圖點，并獲得其在對應時刻 $L_{t_{i}}$ 坐標系下的坐標。以第 $k$ 次激光掃描為例，記 $t_{k}$ 為這次掃描的開始時間， $t_{k+1}$ 為結束時間。令 $Pk~\widetilde{P_{k}}$ 為第 $k$ 次掃描過程中檢測到的所有 3D 點的集合。因為所有傳感器是時間同步的，同時 $t_{k}$ 、 $t_{k+1}$ 時刻機器的位姿已由 VI 里程計估計出，所以可通過 IMU 與激光傳感器間的外參矩陣估計出 $L_{k}$ 、 $L_{k+1}$ 間的位姿關系，如下式：

式中 $TILT^{L}_{I}$ 是 IMU 與激光傳感器間的外參矩陣， $TIk+1Ik~\widetilde{T^{I_{k}}_{I_{k+1}}}$ 是 VI 里程計估計的結果。

?此時要引入一個假設：在 $t_{k}$ 、 $t_{k+1}$ 間，機器以恒定的速度運動。此假設使得我們能通過線性插值的方法獲得 $ti∈[tk,tk+1]t_{i}\in [t_{k},t_{k+1}]$ 時刻， $L_{i}$ 與 $L_{k}$ 間的位姿關系，如下所示：

?通過上式計算的位姿，可以將任意 $t_{i}$ 時刻檢測到的點都轉換到 $L_{k+1}$ 坐標系上表示：

此時，將轉換后所有在 $L_{k+1}$ 坐標系中的點的集合記為 ${P_{k}}$ 。

?根據 $P_{k}$ 中各點局部表面的平滑程度，決定某點屬于邊緣特征點或平面特征點。將提取出來的特征點的集合記為 $F_{k}$ 。此時，再根據已經通過優化得出的 $L_{k}$ 與世界坐標系的相對位姿結果 $TLkWT^{W}_{L_{k}}$ 和公式 (1) ，推出 $L_{k+1}$ 與世界坐標系之間的相對位姿估計值 $TLk+1W~\widetilde{T^{W}_{L_{k+1}}}$ ：

?通過公式(4) 求得的 $TLk+1W~\widetilde{T^{W}_{L_{k+1}}}$ 將 $F_{k}$ 包含的所有特征點都投影到世界坐標系中，并為它們在已構建好的地圖中尋找相匹配的線邊緣、平面塊等結構特征（由此就建立了 $L_{k+1}$ 與地圖的聯系。根據這個聯系就能構建出圖優化的邊）。然后計算 $F_{k}$ 中特征點到相匹配的線邊緣或平面塊的距離 $d$ 。這個過程可以用下面的函數 $f$ 表示：

（PS:特征點尋找匹配的邊緣線和平面塊的方法，以及距離的計算方法可在 “LOAM: Lidar odometry and mapping in real time” 論文中查看）
?將所有 $d_{i}$ 相加，即為需要優化的變量：

?由此，激光的掃描匹配（scan matching）的結果建模出了一個非線性優化問題。通過牛頓梯度下降的方法使 $d$ 的值趨近于 0 （因為理論上特征點到與其匹配的線或面的距離應該為 0）。如果結果能夠收斂，則能獲得一個關于激光傳感器的優化后位姿： $TLk+1WT^{W}_{L_{k+1}}$ 。最后通過 $TLk+1WT^{W}_{L_{k+1}}$ 將第 $k$ 次激光掃描獲得的點云圖轉換到世界坐標系中，更新地圖。
（個人想法：從系統的流程可以看出，系統對于 VI 模塊和激光掃描匹配模塊采用的是松耦合的方式（先 VI 估計再掃描匹配優化）。因此激光模塊優化的效果比較依賴于 VI 的估計初值。這或許能通過緊耦合的方式來改善，但會提高系統的復雜度且降低各模塊的靈活性）

提高系統魯棒性的措施

?如最初在摘要中所說的，視覺慣性SLAM 和激光SLAM 在某些環境下都會存在跟蹤丟失的風險。所以作者構建的這個系統主要是由兩個獨立性較強的模塊（VI 位姿估計模塊和激光掃描匹配模塊）組成的。在某個模塊失效后，另一個能夠獨立工作，以保證系統的正常運行。這就是該系統具有較高魯棒性的原因。
?根據系統工作時所使用的模塊的不同，可分成以下三種工作模式：
（1）正常工作模式：
??此時 VI 和激光模塊都正常工作，系統按照前幾節描述的流程完成定位和建圖；
（2）scan to scan matching 工作模式：
??此時 VI 模塊跟蹤丟失，啟動 “兩次激光掃描間的匹配（scan to scan matching）” 定位模塊（自己取的名字，見笑見笑）。這個模塊是基于 LOAM 系統構造的，它的大致工作過程和上一小節中的 “掃描匹配（scan matching）優化” 相似，只是這里估計的是 $L_{k}$ 和 $L_{k-1}$ 之間的位姿 $TLk?1LkT^{L_{k}}_{L_{k-1}}$ ，而不是 $TLkWT^{W}_{L_{k}}$ 。此模式下系統的工作流程圖如下所示：

（PS：當發生以下幾種情況時，系統會認定 VI 模塊失效：當前跟蹤到的特征點很少、IMU 偏差變化較大、VI 滑動窗口中的位姿估計值與先前估計的結果偏差較大）
（3）僅使用 VI 工作模式：
??此時系統僅使用 VI 模塊完成定位和建圖，流程圖如下所示：

（PS：此模式主要是在結構信息缺失的環境中使用）
?這三種工作模式使得系統能在多種不同的、難度較高的環境下繼續工作。同時，當環境條件變好后，系統仍會重新啟用之前失效的模塊，變回正常的工作模式。
（疑惑：以第三個模式為例，當激光模塊重新工作時，由 VI 模塊構建的地圖作為激光模塊的初始地圖，還是激光模塊會重新構建新的地圖？即 VI 和激光模塊兩者所構建的地圖是否能夠互相使用。）

閉環檢測和臨近檢測

?老規矩，為了消除累積誤差，系統需要有閉環檢測的能力。因為當前系統搭載了視覺、激光傳感器，所以它實現閉環檢測的方式有兩種：
?1、基于視覺和詞袋向量的閉環檢測，也就是最經典的視覺閉環檢測方法；
?2、基于激光雷達的臨近檢測。
作者指出，第一種方法存在一定缺點：必須觀測到相同物體，才能檢測到閉環(對機器的觀測視角有要求)。而第二種方法則沒有這個限制，因為激光雷達是 360° 掃描的。為了避免過多的約束和計算，某個關鍵幀( KF )在實現視覺閉環檢測后，將不再進行臨近檢測。
?閉環檢測想必大家都較為了解，所以這里主要介紹一下臨近檢測（這部分內容也是第一次看，理解不對之處望諒解和指出），它的過程圖如下：

?首先在 KF 數據庫中計算各 KF 與當前關鍵幀( $KF_{c}$ )在世界坐標系下的相對距離（即二者三維坐標的距離）。記所有與 $KF_{c}$ 距離小于 $R_{1}$ 關鍵幀的集合為 $KFs_{loop}$ 。以 $KFs_{loop}$ 中的一個關鍵幀（ $KF_{a}$ ）為例：
?在 $KF_{c}$ 和 $KF_{a}$ 已有的估計位姿基礎上，求出兩者間的相對位姿 $T^{KF_{c}}_{KF_{c}}$ ，再根據先前提到的 “兩次激光掃描間的匹配方法” ，優化 $T^{KF_{c}}_{KF_{c}}$ 。此時如果 $KF_{c}$ 和 $KF_{a}$ 之間的相對距離小于 $R_{2}$ ，則兩幀滿足臨近檢測要求。
（PS：每個關鍵幀包含機器的位姿、對應圖片的特征點和從激光掃描中獲得的幾何特征點信息）
?對 $KF_{c}$ 和 $KFs_{loop}$ 中每個 KFs 都進行如上操作。如果有多個 KFs 滿足臨近檢測要求，則只選擇產生時間最早的那個 KF 與 $KF_{c}$ 構成閉環（因為這樣可以獲得較大的閉環，有利于消除更多的累積誤差）。此外，假設 $KF_{c}$ 的編號為 $k$ ，那么編號為 $k ? e, . . ., k ? 1$ 的 KF 不會參與臨近檢測的判定（ e 的值根據經驗設定）。這樣可以降低計算量。
（個人理解：從論文的實驗結果來看，同時使用視覺閉環和臨近檢測與只使用其中之一相比，最終精度的差距不是很大。但是為了保證系統的兩個模塊的獨立性，這兩個檢測方法需要要同時存在。）

全局位姿圖優化

?當完成閉環產生后，系統將執行全局位姿圖優化。圖中 KF 作為頂點，它包含三種邊：
1、相鄰 KF 之間的連續邊，這個邊計算的是相鄰 KF 間的相對平移和旋轉變換：

2、閉環檢測邊。這個邊計算的是閉環 KF 之間的相對平移和旋轉變換，使用的是 PnP方法。
3、臨近檢測邊。和閉環邊類似，只不過采用的是 “兩次激光掃描間的匹配方法”。
?所以，圖中頂點 $i$ 和 $j$ 之間的誤差可表示為：

（個人理解：式中減號左邊的項可能由 IMU 預積分獲得，因為 IMU 測量值的預積分結果可能更接近于真值）
整個優化圖中的總誤差為：

式中 $A 、 B 、 C$ 分別對應上述 1、2、3 類邊。這個非線性優化問題通過 Ceres Solver解決。為了降低計算量，系統會限制 KF 數據庫中關鍵幀的數量。構成閉環的 KFs 會被保留，而那些與自己鄰近 KFs 距離較近的關鍵幀將被剔除。

總結

?這個系統應該是多傳感器融合 SLAM 中較簡單的一個實現方法。它將視覺慣性和激光匹配兩種位姿估計方法進行松耦合（先 VI 估計，再激光匹配優化），讓兩個不同、相對獨立的模塊共同完成機器的位姿估計。這使得系統的工作流程更為清晰，降低了復雜度，同時也保證系統能夠靈活地對切換工作模式，以應對不同的環境條件。此外，系統在閉環檢測時采用的兩種不同方法，也能提高整體精度。

總結

以上是生活随笔為你收集整理的一种视觉惯性+激光传感器的SLAM系统的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。