大規模環境下基于圖優化SLAM的后端優化方法

王忠立，趙 杰，蔡鶴皋

(1.北京交通大學電子信息工程學院，100044北京;2.機器人技術與系統國家重點實驗室(哈爾濱工業大學)，150080哈爾濱)

隨著移動機器人的應用逐步從小規模、靜態環境向大規模環境發展，基于圖優化的SLAM方法成為研究熱點.從實現框架上，可以將SLAM分解為前端的圖構建和后端的圖優化兩個過程.相對于早期的基于遞歸貝葉斯狀態估計理論方法，如基于Kalman濾波和基于粒子濾波(PF)的方法，圖優化SLAM方法是先通過前端的圖構建過程得到初始狀態估計，然后在后端對初始狀態估計進一步優化求解，因此地圖的一致性和精度更好.基于圖優化的SLAM方法是一種batch方法.在文獻［1］中，詳細介紹了圖優化建模方法及前端的數據關聯和環形閉合檢測方法.本文主要對基于圖優化SLAM的后端優化方法進行總結.

在前端圖構建得到的“初始圖”中，由于傳感器的噪聲、系統參數的不確定性、環形閉合檢測的誤差乃至錯誤等因素，使得圖優化過程具有很大的挑戰性.不僅要求優化方法具有較好的魯棒性、穩定性，同時，優化過程通常是對整個地圖進行的，在大規模環境下，也要考慮優化方法的計算復雜度問題.國內外學者提出了各種優化計算方法，主要包括:基于最小二乘法的優化方法，基于松弛迭代的優化方法，基于隨機梯度下降的優化方法，以及基于流形的優化方法等.并對基于圖優化SLAM的計算效率、魯棒性和可擴展性等方面展開了研究.但對算法的性能及地圖重建結果評估的研究相對較少.針對大規模環境下的地圖創建，地圖創建質量的評估也是非常重要的一環.為此，本文對兩種地圖質量評價方法進行了總結，對優化方法存在的挑戰性進行了闡述，在文獻［2］基礎上，介紹了最新的研究進展，并對發展趨勢進行了展望.

1 基于圖優化SLAM的后端優化方法

1.1 優化方法概述

基于圖優化的SLAM方法，是利用圖模型對SLAM問題進行建模.模型中的節點對應不同時刻機器人及環境組成系統的狀態，邊則描述了系統狀態(節點)之間的約束關系［3］.這類方法將SLAM問題劃分為前端(front-end)和后端(backend)兩個部分，如圖1所示.前端完成圖的構建，即根據觀測和系統約束構建圖的節點和邊;后端主要完成圖的優化.基于圖優化的方法利用所有的觀測信息來優化估計機器人完整的運動軌跡，因此也稱為全SLAM方法.

圖1 基于圖優化的SLAM框架

在前端部分，順序數據關聯是指相鄰觀測數據幀間的匹配及相對姿態估計問題，而環形閉合檢測則根據觀測數據判斷機器人是否處在之前已訪問過的環境中.二者的核心是要解決數據關聯問題，前者考慮局部數據關聯，而后者則涉及全局數據關聯.順序數據關聯與環形閉合檢測都是根據觀測信息建立圖節點間的約束，即完成圖的構建，是基于圖優化方法前端的兩個核心部分.

由于觀測噪聲以及數據關聯誤差的存在，前端得到的圖往往存在不一致性.若用Ti來表示數據幀間的相對變換矩陣，而且T0，T1，…，Tn構成一個閉環，則理想情況下，應滿足:

式中Ⅰ為單位矩陣.但通過觀測信息關聯得到的相對變換矩陣通常不滿足該約束.在基于圖的模型描述中，機器人的位姿是待估計的隨機變量，位姿間的約束則是與隨機變量相關的觀測，圖優化結果對應于位姿的最大似然估計.與順序數據關聯及環形閉合檢測不同，圖優化部分一般不直接處理觀測數據，而是在前端構造的圖基礎上進行優化運算.

Golfarelli等［4］將圖優化視為質量-彈簧模型，從另一個視角來解釋圖的優化問題，如圖2所示.在該模型中，將機器人的位姿看作是帶質量的節點(黑色圓點)，而約束則看作是連接這些節點的彈簧.由于每個約束都是根據與之相關的觀測獨立求解的，它們之間存在不一致性，此時彈簧處在受力形變狀態，這樣的物理系統通常并不穩定.當彈簧對質點的作用力使系統重新達到平衡時，系統處在能量最小狀態，此時質點的分布即代表最優的位姿序列.彈簧的系數通過觀測的不確定性來(協方差)表示.觀測的不確定性越小，彈簧的強度越大，使其形變需要的外部作用力也越大;反之，觀測的不確定性越大，彈簧的強度越小，使其形變需要的外部作用力也就越小.系統的能量最小狀態對應于非線性最小二乘問題的最優解.

圖2 基于圖優化的mass-spring模型

基于圖優化SLAM的后端優化方法，概括起來可以分為基于最小二乘法的優化方法，基于松弛迭代的優化方法，基于隨機梯度下降的優化方法，以及基于流形的優化方法等.

1.2 基于最小二乘法的優化方法

SLAM可以看作是一個非線性最小二乘問題［5］.基于最小二乘的優化方法是通過對目標函數的一階泰勒展開對系統進行線性化，采用迭代法求解線性系統的解，如 Gauss-Newton方法、Levenberg-Marquardt方法等.如果不考慮SLAM問題的稀疏結構特性并假定圖中的節點數為n，則Levenberg-Marquardt算法的時間復雜度為O(n3)［6］，在實際問題求解中遠不能滿足實時性要求［7］.Dellaert和 Kaess［5］提出利用 SLAM 問題中內在的稀疏結構特性，通過稀疏矩陣分解(如稀疏Cholesky分解等)來求解的非線性最小二乘的方法.Kaess等［8］將圖建模和稀疏線性代數方法相結合，提出了iSAM方法，iSAM是增量式圖優化方法的代表.該方法通過對平滑信息矩陣作QR分解，并選擇性對其進行增量式更新，從而避免了每次重新計算平滑信息矩陣，提高了更新的效率，處理問題的規模也大大增加，其最佳復雜度為O(n).在iSAM的基礎上，通過節點的重新排序和重新線性化，Kaess等［9］又提出了改進后的iSAM2方法.為了進一步提高增量式更新過程的計算效率，Kaess等［10］在 iSAM2的框架下，提出采用貝葉斯樹結構來描述SLAM的方法.在SLAM稀疏特性的研究方法上，Konolige等［11］提出一種根據給定圖約束快速構造稀疏矩陣的SPA(sparse pose adjustment)方法.

1.3 基于隨機梯度下降法的優化方法

Olson等［7］將隨機梯度下降方法應用到位姿圖SLAM的優化中.每次迭代時隨機選取圖中的一條邊作為當前約束并計算相應的梯度下降方向，然后在該方向上對目標函數尋優.隨機梯度下降方法具有不易陷入局部極值的優點，對初始值具有較高的魯棒性.實驗證明，即使初始值與最優值相差較遠，甚至在全零初始值或隨機初始值時，隨機梯度下降方法也能取得較好的收斂結果.Grisetti等［12］對Olson等所提的方法進行了改進和拓展，采用樹型結構來描述位姿間的關系并通過增量方式表示待求解的狀態，從而能更有效地對位姿進行更新.Grisetti等［13］還將樹型結構表示以及隨機梯度方法應用到6自由度位姿的優化中，提出了基于樹的網絡優化(TORO)方法.

1.4 基于松弛的優化方法

Duckett等［14］提出采用 Gauss-Seidel松弛方法實現后端優化.其基本思想是依次選取每個節點，根據其相鄰節點的位置及它們之間的約束關系重新計算并更新該節點的位置，且每次迭代都遍歷所有節點.在假定方位角已知(如通過電子羅盤測量)的情況下，Duckett等證明了其必收斂于最優解.該方法可用于增量式的SLAM中，在每次有新的觀測到來時直接在原來結果基礎上進行更新.該方法的缺陷是，當某條邊的誤差較大時，需要多次迭代才能將誤差分配到其他邊中，而這正是出現環形閉合時所需要應對的情況.Frese等［15］提出多層次松弛的優化策略，并利用多重網格方法求解偏微分方程，從而大大地提高了出現環形閉合時節點的優化更新效率.

1.5 流形優化

以上三類優化方法均假定優化過程是在歐氏空間中進行的.在歐式空間中，機器人位姿中的旋轉分量的估計可能會出現奇異.為了避免奇異值問題，旋轉分量部分可以采用四元數法表示，但又產生了額外的自由度，引入不必要的誤差.為此，Grisetti等［16］提出在流形空間中進行優化的思想，避免狀態空間參數化時可能出現的奇異值問題，提出了一種分層優化的圖優化技術(即HOGMAN方法).該方法在在線建圖過程中，根據當前的觀測約束，對地圖的修正只在上層(粗略描述層)進行，從而提高了效率.最近，研究者們提供了能用于流形優化的開源工具(g2o).Kummerle等［17］將HOG-MAN方法和Konolige等人提出的SPA思想結合起來，提出了基于流形的圖優化通用框架(g2o框架)，大大提高了開發效率.在g2o框架的基礎上，Kummerle等［18］進一步擴展了狀態空間，如增加描述可能隨時間變化的系統參數，從而可實現同步傳感器標定、建圖和機器人定位任務的方法.Hertzberg［19］和 Wagner［20］等也將流形方法擴展到傳感器的融合和標定問題中，取得了初步成果.

1.6 圖優化的計算效率、魯棒性和擴展性

非線性最小二乘法的一個不足是對初始值的依賴，如果給定的初始值離最優解距離遠，則很容易陷入局部極值點.為此，Carlone等［21］提出對基于圖優化的SLAM作線性近似并給出解析求解的方法，可以利用其結果作為非線性最小二乘方法的初始值.但目前該方法只適用于2D SLAM.針對非線性最小二乘法對異常點的魯棒性不好的特點，目前已有多種解決方法.Sunderhauf等［22］提出了允許在圖優化的過程中改變圖的拓撲結構以剔除錯誤的環形閉合，從而提高了方法的魯棒性.在隨機梯度下降法(SGD)和Levenberg-Marquardt方法的基礎上，提出了前置濾波SGD和前置濾波Levenberg-Marquardt方法，二者均是在優化操作之前，利用一個前置濾波器來完成一些預處理過程，以確保全局一致性.這些方法的應用使得非線性最小二乘法的魯棒性有所提高.

當機器人在大小固定的環境中行走時，圖的節點數目應該跟環境的規模大小相關，而不是與機器人運動軌跡的長度相關.因此，要使SLAM方法具備良好的擴展性，關鍵是對圖節點進行有效的控制.減少節點數最為直觀的方法是對節點間的距離進行限制，即只有節點間的距離超過一定的閾值時才添加到圖中［23］.Kretzschmar 等［24］從觀測所含的信息出發，評估觀測幀的信息增益，并依此對圖進行剪枝，以控制節點數目.該方法在保持節點規模的同時具有最小的信息損失，因而也保證了地圖信息的完整性.對節點進行剪枝實際對應節點的邊緣化過程，這可能導致圖的結構變得密集.Kretzschmar等［25］提出采用 Chow-Liu 樹對節點間的關系作近似描述，以保證節點連接的稀疏性.Yasir和 Jose等［26］提出利用線段擬合機器人的運動軌跡，從而減小位姿圖規模的方法.Xiang等［27］提出一種變分辨率的地圖表示方法.

上述研究方法進一步提高了基于圖優化方法的計算效率、魯棒性和可擴展性.基于圖優化的增量式SLAM方法目前研究最廣泛.

2 地圖質量的評價

由于SLAM問題的復雜性，其結果是多方面綜合，因此要給出一個大而全的評價方法是很困難的.比較合理的方法就是對SLAM的各個子問題分別給出評價的方法.盡管如此，要對一些子問題進行評價也是非常困難的，像視覺領域中對立體視覺算法的評估一樣［28-29］，因為這些子問題本身也很復雜.另外，對于大規模環境下的地圖重建結果進行評估時，目前可用于對比分析的數據集也很有限.有學者提出了一些通用的數據集［30-31］，但用于性能對比時，由于這些數據集最初不是用于對比的目的，因此很多沒有真實數據.這也是目前進行評估時存在的困難之一.

Olson等［32］對地圖優化算法的評估方法進行了研究，提出要對全局優化(Batch模式，通常是離線的)和在線優化分別進行比較.

在假定前端的特征提取、匹配(包括環形閉合檢測)以及異常點已經排除的前提下，地圖優化實質是后驗概率估計問題.目前主要有兩種方法:基于χ2誤差和基于均方差(MSE)的評價方法.

2.1 基于χ2誤差和均方差的評價方法比較

研究表明，基于χ2誤差方法的局限性在于:χ2誤差小的地圖優化結果不一定比χ2誤差大的更好［33］.如圖3所示，是兩種不同誤差定義方法的優化仿真結果比較.圖3(a)是真實值，通過對比圖3(b)和圖3(c)可見，在圖3(b)中的χ2誤差很小，但卻遠遠偏離真實值，而圖3(c)的χ2誤差雖然很大，但卻更接近真實值.

在地圖優化問題上，產生如圖3所示結果的主要原因是由于觀測過程具有高度非線性本質，造成優化曲面非常復雜.某些情況下，映射問題產生的優化曲面有“淺谷”出現，在此處，優化表面變化大，但對χ2誤差的影響卻很小.即基于χ2誤差的優化目標函數定義中，機器人位姿估計和地圖特征點位置估計相互耦合的關系在某種程度上抵消了相互誤差對地圖結果的影響.圖3(b)中特征點的位置和機器人的位姿在優化過程中同時發生了偏離，而χ2誤差很小.

由此可見，在大規模環境地圖創建中，誤差函數的定義非常關鍵.常用的基于觀測的誤差定義方法(χ2誤差)有時不一定很有效.基于MSE的誤差定義能得到較好的結果.

2.2 過擬合(over-fitting)問題

在機器學習中，為了得到一致假設而使假設變得過于復雜，在訓練數據上能夠獲得比其他假設更好地擬合，但是在訓練數據外的數據集上，卻不能很好地擬合數據，出現了過擬合現象.出現這種現象的主要原因是訓練數據中存在噪聲或者訓練數據太少.在地圖優化估計時，如果平均節點度(average node degree)小會出現該問題.如圖4所示，機器人沿著一個方格運動，在右下角的區域，由于節點度小，存在過擬合問題，導致地圖優化結果和實際情況的不一致.解決過擬合問題的方法主要有兩種:提前停止樹的增長或者對已經生成的樹按照一定的規則進行后剪枝.

圖4 過擬合問題

3 發展趨勢

近年來，SLAM的研究也出現一些新的趨勢:

1)隨著移動機器人的工作環境從室內到室外的擴展，工作空間越來越大，面臨的環境也越來越復雜.相對于室內較為簡單的環境對象，室外環境由各種對象組成，有靜止的、移動的，有行人、各種車輛等.如何實現復雜、大規模、動態環境下地圖表示，高精度、高效率的地圖創建，以及移動機器人的自定位是一個很有挑戰性的研究課題，是現階段SLAM問題研究的重點.

2)對終生地圖創建(lifelong mapping)和維護的研究.在傳統意義上，機器人一旦通過SLAM實現了未知環境的建圖，則建圖任務結束，機器人即可利用已經建好的地圖進行定位或運動規劃.終生地圖研究將機器人長期置于未知環境中，因此需要應對環境的變化，并持續對地圖進行更新、維護.這種對地圖的不斷更新、維護就構成了終生地圖研究的主要內容［33］.

3)語義地圖(Semantic map)創建的研究.為了使機器人具備理解場景以及能夠識別場景物體的能力，構造具有語義信息的地圖是一種重要途徑.構建語義地圖，可使機器人能夠更好地為人類提供服務，是SLAM發展的趨勢之一.

4)基于多傳感器融合的SLAM研究.通過多傳感器信息融合，可以彌補單一傳感器在數據獲取時的不足，降低狀態估計的不確定性，改善數據關聯、環閉檢測等關鍵環節的精度和可靠性，進而提高機器人定位和環境建圖的精度.多傳感器融合在移動機器人SLAM中的研究和應用將會越來越多.

5)多機器人協作 SLAM研究.與單機器人SLAM相比，多機器人SLAM問題涉及多幾個機器人得到的子圖如何拼接得到全局地圖，以及在全局地圖中各個機器人的定位問題.多機器人協作完成SLAM具有更準確、更高效率的優勢，因此受到越來越多的關注，正成為一個熱點研究問題.

6)非歐式空間下的建模與狀態估計方法.針對SLAM問題，在歐式空間中對機器人的位姿進行求解存在奇異值問題.另外，目前大多數SLAM方法是基于傳感器空間的，但機器人控制，需要將機器人坐標系和傳感器坐標系之間關聯考慮，如在基于激光掃描的傳感器中，利用里程計獲取的運動信息，是在機器人坐標系下描述，和觀測傳感器(激光掃描)是不同的坐標系，二者的變換關系是假定準確已知的.雖然可以通過標定得到二者變換關系，但結果也有不確定性，不確定性對SLAM結果的影響目前還未知.Grisetti等［16］提出在流形空間中進行優化的思想，其實質是將SLAM問題置于一個高維空間中，不僅可以避免狀態空間參數化時可能出現的奇異值問題，還可以將傳感器自身參數的在線估計等問題統一到一個完整的系統框架下.對非歐式空間中的求解方法的討論，將對SLAM的發展產生深刻的影響.

［1］王忠立，趙杰，蔡鶴皋.大規模環境下基于圖優化SLAM的圖構建方法［J］.哈爾濱工業大學學報，2015，47(1):75-85.

［2］梁明杰，閔華清，羅榮華.基于圖優化的同時定位與地圖創建綜述［J］.機器人，2013，35(4):500-512.

［3］GRISETTI G，KUMMERLE R，STACHNISS C，et al.A tutorial on graph-based SLAM［J］.IEEE Transaction on Intelligent Transportation Systems Magazine，2010，2(4):31-43.

［4］GOLFARELLI M，MAIO D，RIZZI S.Elastic correction of dead-reckoning errors in map building［C］//IEEE/RSJ International Conference on Intelligent Robots and Systems.Piscataway:IEEE，1998:905-911.

［5］DELLAERT F，KAESS M.Square root SAM:simultaneous localization and mapping via square rootinformation smoothing［J］.International Journal of Robotics Research，2006，25(12):1181-1203.

［6］ FRESE U，LARSSON P，DUCKETT T.A multilevel relaxation algorithm for simultaneous localization and mapping［J］.IEEE Trans on Robotics，2005，21(2):196-207.

［7］ OLSON E，LEONARD J，TELLER S.Fast iterative alignment of pose graphs with poor initial estimates［C］//IEEE International Conference on Robotics and Automation.Piscataway:IEEE，2006:2262-2269.

［8］KAESS M，RANGANATHAN A，DELLAERT F.iSAM:Incremental smoothing and mapping［J］.IEEE Trans on Robotics，2008，24(6):1365-1378.

［9］ KAESS M，JOHANNSSON H，ROBERTS R，et al.iSAM2:incremental smoothing and mapping with fluid relinearization and incremental variable reordering［C］//Intl Conf on Robotics and Automation(ICRA).Shanghai:IEEE，2011:3281-3288.

［10］KAESS M，JOHANNSSON H，ROBERTS R，et al.iSAM2:incremental smoothing and mapping using the bayes tree［J］.InternationalJournalofRobotics Research，2012，31(2):216-235.

［11］KONOLIGEK， GRISETTIG， KUMMERLER.Efficient sparse pose adjustment for 2D mapping［C］//IEEE/RSJ International Conference on Intelligent Robots and Systems.Piscataway:IEEE，2010:22-29.

［12］GRISETTI G，STACHNISS C，GRZONKA S，et al.A tree parameterization for efficiently computing maximum likelihood maps using gradient descent［C］//Robotics:Science and Systems III.Cambridge:MIT Press，2008:65-72.

［13］GRISETTI G，STACHNISS C，GRZONKA S.TORO-tree-based network optimizer［CP/OL］.［2014-03-15］.http://www.openslam.org/toro.html.

［14］DUCKETT T，MARSLAND S，SHAPIRO J.Learning globally consistentmaps by relaxation［C］//IEEE International Conference on Robotics and Automation.Piscataway:IEEE，2000:3841-3846.

［15］FRESE U，LARSSON P，DUCKETT T.A multilevel relaxation algorithm for simultaneous localization and mapping［J］.IEEE Trans on Robotics，2005，21(2):196-207.

［16］GRISETTI G，KUMMERLE R，STACHNISS C，et al.Hierarchical optimization on manifolds for online 2D and 3D mapping［C］//IEEE International Conference on Robotics and Automation.Piscataway:IEEE，2010:273-278.

［17］KUMMERLE R，GRISETTI G，STRASDAT H，et al.g2o:A general framework for graph optimization［C］//IEEE International Conference on Robotics and Automation.Shanghai:IEEE，2011:3607-3613.

［18］KUMMERLE R，GRISETTI G，BURGARD W.Simultaneous calibration， localization， and mapping ［C］//InternationalConference on IntelligentRobots and Systems.Piscataway:IEEE，2011:3716-3721.

［19］HERTZBERG C，WAGNER R，FRESE U，et al.Integrating generic sensorfusion algorithms with sound state representations through encapsulation of manifolds［J］.Information Fusion，2013，14(1):57-77.

［20］WAGNER R，BIRBACH O，FRESE U.Rapid development of manifold-based graph optimization systems for multisensor calibration and SLAM ［C］//IEEE/RSJ InternationalConference on IntelligentRobots and Systems.Piscataway:IEEE，2011:3305-3312.

［21］CARLONE L，ARAGUES R，CASTELLANOS J A，et al.A first-order solution to simultaneous localization and mapping with graphical models［C］//IEEE International Conference on Robotics and Automation.Shanghai:IEEE，2011:1764-1771.

［22］SUNDERHAUF N，PROTZEL P.Towards a robust back-end for pose graph SLAM［C］//IEEE International Conference on Robotics and Automation(ICRA)，Minnesota:IEEE，2012:1254-1261.

［23］KONOLIGE K，AGRAWAL M.Frame SLAM:from bundle adjustment to real-time visual mapping［J］.IEEE Trans on Robotics，2008，24(5):1066-1077.

［24］KRETZSCHMAR H，GRISETTI G，STACHNISS C.Lifelong map learning for graph-based SLAM in static environments［J］.Kunstliche Intelligenz，2010，24(3):199-206.

［25］KRETZSCHMAR H，STACHNISS C，GRISETTI G.Efficient information theoretic graph pruning for graphbased SLAM with laser range finders［C］//IEEE/RSJ International Conference on Intelligent Robots and Systems.Piscataway:IEEE，2011:865-871.

［26］YASIR L，JOSE N.Go straight，turn right:pose graph reduction through trajectory segmentation using line segments［C］//European Conference on Mobile Robots(ECMR).Barcelona:IEEE，2013:144-149.

［27］XIANG J，TAZAKI Y，INAGAKI S.Autonomous variable-resolution map building for mobile robots in unknown environments［J］.Electeical Engineering in Japan，2014，186(4):59-69.

［28］SCHARSTEIN D，SZELISKI R，HIRSCHMOULLER H.Middlebury stereo vision page［DB/OL］.［2014-03-10］.http://vision.middlebury.edu/stereo/.

［29］SCHARSTEIN D，SZELISKI R.A taxonomy and evaluation of dense two-frame stereo correspondence algorithms［C］//IEEE Workshop on Stereo and Multi-Baseline Vision.Hawaii:IEEE，2002:131-140.

［30］HOWARD A，ROY N.The robotics data set repository(Radish)2003［DB/OL］.［2014-03-10］.http://www.lib.uts.edu.au/data-archive/4843/radish-roboticsdata-set-repository.

［31］NEWMAN P，CORKE P.Editorial:data papers-peer reviewed publication of high quality data sets［J］.International Journal of Robotics Research，2009，28(5):587.

［32］OLSON E，KAESS M.Evaluating the performance of map optimization algorithm［EB/OL］.［2014-03-20］.http://april.eecs.umich.edu/pdfs/olson2009rss.pdf.

［33］KRETZSCHMAR H，GRISETTI G，STACHNISS C.Lifelong map learning for graph-based SLAM in static environments［J］.Kunstliche Intelligenz，2010，24(3):199-206.