一種帶有3D運動狀態估計的slam系統
A Flexible and Scalable SLAM System with Full 3D Motion Estimation
摘要：
在許多應用場景中，比如城市搜救和搜索（USAR）機器人，需要去獲取未知環境的地圖。我們提出了一個快速在線學習占用柵格地圖、占用較少計算資源的系統。它利用激光雷達系統與基於慣性傳感器的3D位姿估計系統進行融合，實現了一種魯棒的掃描匹配方法。通過地圖變化的快速近似和多分辨率柵格地圖，在各種有挑戰性的環境中實現了可靠的定位與建圖。提供了多種數據集以適應嵌入式手持建圖系統。我們表明，該系統是足夠准確的，在我們考慮的應用場景中，不需要顯式閉環檢測技術。該軟件可作為ROS的開源代碼包。
I.介紹
學習環境模型並定位自身是一個真正的機器人在真實世界運行的最重要的能力。在本文中，我們提出了一種靈活的、可升級的系統來解決SLAM問題，已成功的運用在了UGV、USV和一個小型的室內導航系統上。該方法消耗的計算資源較少，故可以應用於低成本、低功耗的嵌入式系統。該方法是在ROS上實現的開源軟件。它適應ROS上的API和導航stack，並可以在ROS的生態中替代其他SLAM方法。
本文介紹的系統旨在保證計算力要求低的前提下，實現足夠精確的環境感知和自我定位。它可以應用在小尺度的、不必做大的閉環的系統中，並需要使用高更新速率的激光雷達系統。類似的場景包括RoboCup搜救比賽，可能需要在模擬的地震場景中找到受害者，因此需要對車輛在6Dof上進行姿態估計。或者，比地面機器人更靈活的室內飛行器的導航。有關USAR的結果和模型在參考【2】中可以找到。
我們的方法結合了2D SLAM（基於激光雷達的平面地圖）和3D導航（基於IMU）融合了2D的SLAM信息作為輔助（FIG.I）。SLAM過程通常是由激光雷達的數據更新來觸發的，而整個3D導航解決方案是需要實時計算的，構成車輛控制系統的一部分。
II.相關事宜
近些年已經有大量的研究關於SLAM的問題，例如作為開源軟件的gmapping使用的是Rao-Blackwellized粒子過濾器，可以可靠在的典型辦公室室內場景使用。然而，這些解決方案的工作最好在平面環境，依賴於現有的，足夠精確的航跡以及不利用現代雷達系統提供的高更新率。對於非結構化環境，會導致載體的顯著滾轉和俯仰運動，或在空中平台上實現這種系統不適用或必須進行顯著修改。
一個SLAM的前端和后端系統之間的區別。在大滿貫的前端，用於實時在線估計機器人運動，后端用於優化位姿圖，和在使用的前端產生的位姿之間的約束。本文提出的方法可以作為一個SLAM的前端和不提供姿勢圖優化像[ 4 ]和[ 5 ]提出的解決方案。然而，我們表明，在許多情況下，這種優化並不需要在現實中的一些條件，因為這種方法是足夠准確的，可用於機器人來執行他們的任務。
基於激光掃描匹配的室內導航系統提出了對旋翼無人機使用[ 6 ]、[ 7 ]、[ 8 ]。在這里，采用兩階段的方法，前端快速掃描的位姿估計，和用於在后台或遠程計算機上進行的較慢的后端建圖步驟。從雷達掃描對准的位姿估計不直接納入車輛的控制回路，因此他們只在低速行駛。
在[ 9 ]和[ 10 ]中描述了移動機器人上使用的其他前端系統。在本文的對比，他們沒有提供完整的六自由度位姿估計和開源軟件。
使用掃描匹配進行定位的工作始於ICP[ 11 ]，它起源於注冊三維點雲的一般方法。許多基於ICP的方法的主要缺點是對點對應的高復雜度搜索，這必須在每次迭代中進行。極坐標掃描匹配（PSM）[ 12 ]避免了利用激光掃描的自然極坐標系統來估計它們之間的匹配的對應搜索。掃描進行預處理，可用於極性掃描匹配。實時相關掃描匹配方法[ 13 ]采用窮舉抽樣方法進行掃描匹配。通過多種優化，這種方法能夠實時應用。基於正態分布變換（NDT）[ 14 ]的掃描匹配將掃描對齊到代表前掃描的正態分布混合。
對於沿海的情況，有研究使用昂貴的多傳感器掃描儀[ 15 ]，但據筆者的知識，沒有單發射器激光雷達為基礎的SLAM方法，可在現實世界中的條件下進行測試。
III.系統概述
相對許多其他基於網格的2D-SLAM來說，本文提供了一種可用的，具有完整的6自由度運動的平台。我們的系統可以預測6自由度的平移和旋轉狀態。為了實現這一點，我們的系統由兩個主要部分組成，導航濾波器融合了來自於慣性傳感器和其他可用的傳感器到一個可用的3D數據，而2D SLAM則提供平面的位姿信息。這兩部分的更新都是單獨的，是松耦合系統，他們會定時保持同步。
我們定義導航坐標系統是一個右手系統，它的原點在平台的起點上，Z軸指向上方，X軸在啟動時指向平台的偏航方向。完整的3D狀態表示為：
，其中
是歐拉角的roll、pitch和yaw，
和
是位置和速度在導航坐標系中的表示。
慣性的測量構成了輸入矢量
，其中角速度矢量以及加速度矢量
。由非線性微分方程描述剛體運動：
(1)
(2)
(3)
是映射自身坐標系到導航坐標系的方向余弦矩陣，
是歐拉角到角速度的導數，g是固定重力向量[16]。當使用低成本傳感器時，通常會忽略由於地球自轉而產生的偽力的影響。
由於傳感器噪聲，速度和位置表現出明顯的漂移。因此，必須進一步融合傳感器信息。在本文中，這個信息是通過掃描匹配，非常適合用在室內場景。其他可能的來源是用於航向信息的磁場傳感器或用於海拔高度估計的氣壓傳感器。如果可用，odom航跡信息可以用來提供速度的測量。在室外場景中，衛星導航系統通常被用作輔助系統以防止慣性導航解決方案漂移[ 17 ]。
根據不同的平台，系統方程可以引入更多的約束，從而化簡系統狀態空間。
IV.2D SLAM
能夠代表任意環境的柵格地圖的使用，這是在現實世界環境中，利用激光雷達的移動機器人定位的一種行之有效的方法[18]。由於雷達平台可能具有6自由度運動，掃描已被轉化成一個估計位姿在局部穩定的坐標系。使用估計的方位和聯合值，掃描被轉換成掃描端點的點雲。
根據具體情況，這個點雲可以進行預處理，例如對點數進行降采樣或移除無效點。對於所提出的方法，只使用基於端點z坐標的濾波，以便在掃描匹配過程中僅使用預期掃描平面的閾值內的端點。
A.Map Access
FIG.2. (a) 占用柵格地圖的雙線性濾波。點是其值應插值的點。(b) 占用柵格地圖和空間導數。
占用柵格地圖的離散性限制了可以實現的精度，也不允許直接計算插值值或導數。出於這個原因，一個插值方案允許通過雙線性濾波子網格單元的精度估計占用概率和衍生工具。直觀地說，網格映射單元格值可以看作是底層連續概率分布的樣本。
給定一個連續的地圖坐標
，占用值
以及梯度
可以像FIG.2(a)一樣用四個臨近點近似,沿着x軸和y軸進行線性插值，然后得到:
(4)
導數可以近似為：
(5)
(6)
需要注意的是，地圖的采樣點、柵格單元位於單位距離為1的規則網格上，從而簡化了梯度近似方程。
B.Scan Matching
掃描匹配是將激光雷達掃描與現有地圖對齊的過程。現代激光掃描儀具有低測量噪聲和高掃描速度。由於這種原因，注冊掃描的方法可能會產生非常精確的結果。多機器人系統的激光掃描儀的精度和精度比里程計數據高很多，如果在所有。第VI部分顯示的例子，對為什么不可用odom里程計數據提出了評價。
迄今為止，我們的方法是基於帶有地圖記憶的端點對齊的優化方法。根據計算機視覺工作帶來的啟發，我們的基本思路是使用高斯-牛頓法。使用這種方法，不需要在激光端點之間進行數據關聯搜索或窮舉搜索。當掃描與現有地圖對齊時，匹配將通過所有前面的掃描隱式進行。
我們尋求一個剛性變換
的函數最小值：
(7)
也就是說，我們需要找到一個轉換，使得激光掃描與地圖有最佳的對齊。在這里，
是掃描端點
的世界坐標。有一個函數
，可以表示機器人在世界坐標系中的坐標：
(8)
函數
返回
給出的地圖坐標值。鑒於一些
的的初始估計，我們要估計
，它根據以下方程去優化測量誤差：
(9)
我們對
做一階泰勒展開得：
（10）
通過將
的偏導數設置為0而使以下方程最小化：
(11)
解
得到高斯-牛頓方程的最小化問題：
(12)
其中：
(13)
對地圖梯度
的近似是在第IV-A提到。用方程（8）我們得到：
(14)
用
和
可以解出高斯-牛頓方程（12），得到一個
接近最小值。重要的是要注意，該算法適用於非光滑線性逼近的地圖梯度
，這意味着局部二次收斂到最小不能保證。然而，該算法在實踐中具有足夠的精度。
在許多應用中，高斯近似的匹配不確定性是可取的。實例參數過濾器的更新以及使用圖優化SLAM后端構成約束。一種方法是使用基於抽樣的協方差估計，取樣不同的位姿估計接近掃描匹配的位姿，並從這些構造協方差。這類似於無跡卡爾曼濾波的想法[ 20 ]。第二種方法是使用近似Hessian矩陣得到協方差估計。這里，協方差矩陣近似於：
(15)
其中
是一個比例因子，取決於激光雷達的性能。全面的推導可在[ 21 ]中獲得。
C.地圖的多重分辨率表示
任何爬山法或基於梯度的方法有其陷入局部極小的風險。由於所提出的方法是基於梯度上升，它也很容易陷入局部極小。使用計算機視覺中常用的類似於圖像金字塔方法在多分辨率地圖中表示會得到緩解。在我們的方法中，我們可以選擇使用多個占用柵格地圖，每個粗糙的地圖具有前一個分辨率的一半。然而，多個地圖的等級並不是從一個單一的高分辨率地圖生成（采用高斯濾波和降采樣等在圖像處理中常見的做法）。相反，不同的映射被保存在內存中，同時使用對齊過程產生的位姿估計進行同步更新。這種生成方法確保地圖是一致的尺度同時避免昂貴的下采樣操作。掃描定位過程是在粗糙的地圖的等級開始，由此估計的姿態獲得作為下一級開始估計，類似於在[22]。帶來的一個好處是粗粒度地圖的即時可用性，例如可以用於路徑規划。
Fig.3. 多分辨率地圖：（a）20cm網格（b）10cm網格（c）5cm網格
V.3D狀態估計
本節包括第III節的全三維狀態向量估計和2D SLAM。導航濾波器以100赫茲的恆定速率實時運行，並在掃描匹配位姿和其他傳感器信息一到達時就同步更新。其他傳感器的融合除了掃描匹配位姿以外都不在本文的范圍內。具體實現細節可在[23]中找到。
A.導航濾波器
為了估計平台的6D位姿，我們采用了帶有通用模型（方程（1）-（3））的擴展卡爾曼濾波器（EKF）。此外，隨着加速度隨時間的變化，陀螺儀和加速度計的偏差增大了狀態向量，並對結果產生顯著影響。請注意，由於矩陣
和
中的歐拉角項，系統方程是非線性的，因此必須使用非線性濾波器。慣性測量被認為是已知的系統輸入。
速度和位置更新是測得的加速度的一個純積分，如果沒有通過測量更新的額外反饋，系統將是不穩定的。防止狀態估計無限增長的通常對策是在方差達到一定閾值時，偽零速度更新，否則不能保證穩定性。在本文中，在平面的二維位姿定位是通過掃描匹配更新，而全3D的位姿估計需要額外的高度氣壓計和距離傳感器。
B.SLAM整合
為了獲得最佳性能，2D SLAM解決方案和3D EKF估計之間的信息必須在兩個方向上進行交換。系統不同步，EKF通常以較高的更新速率運行。為了提高掃描匹配過程的性能，EKF的位姿估計投影在XY平面，用於掃描匹配優化過程的估計初始化。另外，估計的速度和角速率也可以被整合以提供掃描匹配的估計初始化。
在相反的方向，協方差交叉（CI）是用來融合SLAM位姿與信念狀態[24]。一個簡單的卡爾曼測量更新會導致過於自信的估計，因為它假設統計獨立的測量誤差。
我們將掃描時的卡爾曼估計表示為
，協方差為P。還有，方程（7）和（15）中的SLAM位姿為
。融合結果有：
(16)
以及：
(17)
利用觀測矩陣將整個狀態空間投影到SLAM的3維子空間中。參數
用來調整SLAM的更新效果。
類似於卡爾曼濾波和信息濾波之間的二元性，協方差交叉也可以用它的協方差形式來寫：
(18)
以及：
(19)
其中：
(20)
根據（16）和（17）的全狀態協方差的推理在計算上的開銷是很大的，這是最合適的方法。
VI.結果
我們展示了三種不同用例的通用性和健壯性的例子。應該指出的是，用於基准測試的SLAM方法缺乏6自由度運動，考慮到我們的方法以及激光雷達的高更新率，它利用標准數據集。為此，我們提供ROSBAG格式的數據集進行測試。
Fig.4.平台：（a）RoboCup搜救大賽中的Hector UGV（b）整合的建圖系統，可以手持建圖（c）UGV建圖系統裝在一個USV平台上
A.USAR場景
所提出的方法是使用無人地面車輛進行自主探索和城市搜索和救援（搜救）受害者檢測場景RoboCup救援機器人聯賽。Hector UGV系統如Fig.4(a)所示。激光雷達穩定在滾轉和俯仰軸周圍，以保持激光與地面一致，最大限度地提高平面激光雷達掃描的信息增益。在Fig.5(a)和Fig.5(b)，我們的方法能夠學習高度准確和一致的地圖，盡管在UGV系統自由度狀態的顯著變化。視頻顯示在機器人世界杯2010競技場救援現場的在線地圖以及本節中的其他實驗可在線查閱。
B.沿岸場景
測試在沿海水域由於植被茂密,GPS中斷頻繁，Hector UGV的定位系統安裝在一個控制系統（Fig.4(c)）。沒有對USV系統改造的必要性，完整的SLAM系統安裝和運行了不到30分鍾。在維吉尼亞的克萊特湖，部分日志文件記錄和用於學習的環境地圖沒有GPS數據，還是沒有具體的運動模型的車輛，這都將需要更緊密的集成和較長的准備時間。如Fig.5所示，我們的方法可以在GPS中斷的情況下為導航提供足夠精確的地圖。因為它不使用提取的特征線，它在沿海植被提供了高度的非結構化環境中工作。應該指出的是，用銀行utm-30lx雷達不返回有效的距離測量，當電子束擊中水，這是優勢，利於預防錯誤的測量影響的SLAM系統。一旦GPS購回，一個基於圖形的SLAM方法可用於優化提供的姿勢圖掃描匹配通過添加GPS構成GPS停電前和額外的絕對位置約束后取得。這是今后工作的主題。需要指出的是，使用hokuyo UTM-30LX激光雷達，有時激光束照射在水面上不返回有效測量時，是有利於防止錯誤測量對SLAM的影響的。一旦GPS信號恢復，一個基於圖形的SLAM方法可以用於優化掃描匹配（通過GPS中斷之前和之后的GPS位姿作為額外的位置約束）提供的位姿圖。這將是今后的工作方向。
C.嵌入式建圖系統
FIg.4(b)中展示了嵌入式建圖系統。它由一個hokuyo UTM-30LX激光雷達系統，基於英特爾Atom Z530 CPU板以及一個小型低成本MEMS IMU組成。如此，該系統為所提出的SLAM方法的應用提供了所有必要的傳感器。它可以很容易地安裝在無人駕駛的車輛上，也可以用手攜帶來學習環境地圖。Fig.6(a)顯示了通過在地面實況地圖上通過機器人在RoboCup2011救援競技場行走的地圖。Fig.6(b)顯示了在德國Dagstuhl castle的一個新建築中的建圖。從路徑和映射中可以看出，該系統足夠精確地關閉在小規模場景中通常遇到的閉環，而不使用顯式循環閉包的方法，保持計算要求低，並防止運行期間對估計地圖的更改。視頻和ROS記錄文件已經上線。記錄文件可以在atom z530上以3倍速度運行而沒有質量損失。因此，以SLAM使用的設置，消耗不到嵌入式建圖系統的全部計算資源的一半。
Fig.5車輛實驗：（a）RoboCup2010上的地圖（b）0.25m精度的柵格地圖覆蓋在衛星照片上
Fig.6真實環境的手持建圖（a）RoboCup2011搜救環境（多個小閉環）（b）Schloss Dagstuhl的一個新建築（大閉環）
VII.結論
本文提出了一種靈活的、可伸縮的SLAM方法，可用於多種場景。我們展示的適用性不同的情況下如模擬城市搜索和救援，在USV以及手持嵌入式系統沿海建圖。我們表明，該系統正確估計位姿，並整合所使用的激光雷達系統的三維運動，同時消耗低計算資源。該系統現在作為開源軟件提供。
據作者所知，高更新率，同時在二維繪圖和低功耗平台運行的六自由度位姿估計的組合是不可用的開放源碼軟件。
VIII.感謝
德國研究基金會（DFG）RTG1362“混合模式環境中的協作、自適應和響應監視”。我們感謝弗吉尼亞理工大學電氣與計算機工程系的Dan Stilwell和Aditya Gadre，布拉德利，給我們機會來執行克萊特湖USV實驗。感謝Christian Lindig博士提供Dagstuhl floor圖。