SLAM后端優化算法的研究

劉芳

摘要：隨著虛擬現實和自動駕駛的飛速發展，同時定位與地圖創建（sLAM）是近幾年研究的熱點，也是各領域實現智能化的關鍵。本文針對SLAM后端優化的兩種主要方法一基于濾波理論優化和基于非線性優化（圖優化）展開綜述。首先，介紹了基于線性優化的卡爾曼濾波器（KF）、粒子濾波器（PF），并詳細分析了不同濾波器模型的具體實現、性能、優缺點;其次，闡述了基于圖優化算法的整體框架、關鍵技術，著重介紹了在歐式空間以及流形空間優化算法;最后對SLAM后端優化算法的未來發展進行了展望。

關鍵詞：同時定位與地圖創建;濾波器模型;圖優化;流形空間

0引言

智能主體能夠在未知環境中自主地完成一些功能，例如自動駕駛、掃地機器人、災后救援等，需要不斷對周圍環境進行探測并創建環境地圖，對自身定位進行路徑規劃，避開障礙物準確到達目的。這些任務要求智能主體既要明白自身狀態（即位置），也需要了解外在環境（即地圖）。研究者把這類問題稱為同時定位與地圖創建（Simultaneous Locationand Mapping.SLAM），定位是指在室內環境下，GPS或BD模塊接收不到衛星信號，無法獲取經緯度等位置相關信息時，智能主體則通過環境中特征（如路標或障礙物）來對自身的位姿進行估計：地圖創建是指將未知環境中所有物體的相關位置等信息提取出來，即對環境進行描述，這兩者密切相關，相互依賴。智能主體需要利用已經創建出來的地圖對自身進行定位，又需要智能主體依據當前的位姿信息對環境地圖進行更新。近幾年來，SLAM成為智能主體研究的核心內容，SLAM主要有傳感器數據讀取、前端視覺里程計、后端優化、回環檢測、建圖五部分構成。本文主要研究后端優化部分，后端在接受不同時刻視覺里程計測量的相機位姿信息、回環檢測的信息，對這些信息進行優化，得到智能主體全局一致的軌跡和地圖。主要有兩種實現方式：基于濾波理論優化和基于非線性優化（圖優化）。

基于濾波理論優化的方法主要是利用貝葉斯原理，從開始時刻到結束遞歸地進行。依據上一時刻置信度和運動變換概率的積分（或求和）估計當前狀態的置信度，然后利用當前時刻傳感器的觀測數據概率乘以當前時刻狀態的置信度，得到后驗概率。對于全局估計問題，由于存在很多不同的假設，每一種假設都會形成不同的后驗模式，從而存在不同的濾波器算法。常見的有：卡爾曼濾波（KF）和擴展卡爾曼濾波（EKF）、信息濾波器（IF）和粒子濾波器（PF）。濾波算法具有時間約束和增量特性，通常也被稱為在線SLAM（On-Line SLAM）。

與濾波理論的求解算法相反，基于非線性（圖優化）不再依賴某一時刻的信息，而是通過智能主體所有的運動信息和觀測數據來優化智能主體完整的運動軌跡和環境特征地圖，也被稱為完全SLAM（Full SLAM）。其核心思想是：把后端優化算法轉換成圖的一種形式，圖中的頂點代表了不同時刻智能主體的位姿和環境特征，有約束關系的各個頂點用邊來表示。建好圖之后，利用圖優化算法對智能主體的位姿進行求解，使得頂點更好地滿足對應邊上的約束條件，優化算法結束之后，對應的圖即是智能主體的運動軌跡和環境地圖。

本文將從以上兩方面對SLAM后端優化算法展開研究。首先介紹線性優化SLAm.包括高斯濾波算法卡爾曼濾波和非參數濾波算法粒子濾波：其次闡述圖優化SLAm.包括歐式空間隨機梯度下降法和松弛算法，以及流形空間圖優化算法。

1線性優化SLAM

1.1線性優化基本思想

SLAM過程可以由智能主體的運動方程和觀測方程來進行描述。其中運動方程是依據某一時刻之前智能主體的位姿、周圍環境空間特征、控制輸入變量來預測該時刻的狀態：觀測方程是智能主體自身攜帶傳感器的觀測值。在SLAM的求解過程中，線性優化采用濾波器模型，該模型是基于貝葉斯概率的求解方式，進而將SLAM問題轉成求解智能主體位姿和周圍環境空間特征聯合概率。假設在某一時刻k.智能主體的位姿為x_k，周圍環境空間特征為y_k，控制輸入變量為u_k。

首先利用以前狀態的后驗概率密度p（x_k-1，y_k-1|x_k-2，u_k-1，z_k-1）和運動變換轉移概率P（x_k|x_k-1，u_k）來預測k時刻的狀態分布：

其中，η為歸一化變量（乘積結果不再是一個概率，其總和可能不為1），Z_k為傳感器觀察值。

依據上述的分析，可知線性優化模型使用概率思想求解后端優化問題，為了遞歸求解后驗概率，需要初始化置信度，并且在求解的過程中依據有關測量、運動變換轉移概率的不同假設，每一種假設都形成自己的后驗模式，因此得出不同的濾波算法。常見的兩種濾波算法有：卡爾曼濾波器和粒子濾波器。

1.2 卡爾曼濾波算法

卡爾曼濾波器是一種線性系統的最優無偏估計算法，使用高斯函數表示后驗概率，在可能的狀態空間中使用均值和方差（即高斯函數分布的均值和方差）來表達置信度。其中運動變換方程必須是帶有隨機高斯噪聲參數的線性函數，觀測方程也與帶高斯噪聲的自變量呈線性關系，初始置信度必須服從正態分布，依據以上的假定得到一個全局最優的智能主體和周圍環境空間狀態估計。在具體的SLAM應用中，KF算法主要由兩部分組成：狀態預測和更新。預測：依據智能主體上一時刻的位姿和環境特征、傳感器數據預測當前時刻的位姿和環境特征的聯合狀態估計（置信度），其中位姿和環境特征的聯合狀態表示均值，方差由運動變換矩陣求解：更新：首先計算卡爾曼增益，利用預測置信度、卡爾曼增益更新置信度，即更新智能主體當前時刻的后驗概率分布;以上兩個步驟不斷地遞歸執行。算法的具體執行步驟如下：

（1）假定運動變換概率p（x_k|x_k-1，u_k）和觀測概率p（z_k|x_k）用下式表示：式中，x_k和x_k-1為狀態向量;u_k為k時刻的控制變量;A_k運動變換矩陣;B_k為輸入控制矩陣;ε_k是一個高斯隨機變量，表示由狀態轉移引入的不確定性，均值為O;方差用只R_k表示;C_k為觀測矩陣;向量δ_k為觀測噪聲，均值為0.方差為Q_k。

（2）預測

卡爾曼濾波作為線性高斯系統，觀測是狀態的線性函數，并且下一時刻狀態是歷史狀態的線性函數，使用最小均方誤差的方式估計線性系統，實際上運動變化和測量很少是線性的。因此，研究者提出了改進算法擴展卡爾曼濾波（EKF），放寬了其中的一個假設條件，將KF方法中線性的狀態轉移和觀測轉換為非線性的，使用雅可比矩陣替換KF中線性系統矩陣，從而置信度不再是一個高斯分布，而是高斯的近似值，得到狀態系統的次優估計，EKF對一些強非線性和非高斯分布的應用存在較大的近似誤差。

1.3粒子濾波算法

粒子濾波（PF）是貝葉斯濾波的一種非參數實現。與線性高斯KF不同，其不依賴確定的后驗函數，而通過有限數量的值來近似后驗，每一個值大致與狀態空間的某個區域有關，將Bayes中求解積分轉換成有限個樣本點求和的過程。PF的核心思想是利用一系列隨機狀態采樣的加權和來近似積分，求解后驗概率密度函數（置信度），當采樣粒子數量不斷增多達到一定閾值時，加權和就接近于狀態變量的后驗概率密度函數。在SLAM應用中，PF主要包括兩個過程：采樣、權值更新，算法的詳細執行如下：

（1）采樣：依據系統已知的、容易采樣的重要性概率密度函數g（x_0：k|z_1：k，從中抽取N個獨立的粒子樣本集合。

（2）權值更新：對樣本集合中每個采樣粒子進行權值求解。

系統的重要性概率密度函數為：

由于進行了乘法運算，乘法的總和可能不再是1.因此需要對粒子的權值進行歸一化，則有：

使用所有粒子以及對應的權值求和來近似位姿和環境特征點的聯合后驗函數，則有：

當采樣粒子的數量足夠大（N_m→∞）時，公式（17）可以很接近地描述在智能主體的位姿和環境特征在該狀態下，最有可能產生當前的觀測數據。

粒子濾波算法為了得到正確的后驗概率估計值，樣本空間中粒子權值方差越接近零越好，但是隨著算法迭代次數的增多，粒子樣本權值存在退化現象。在樣本空間中只有少數粒子參與計算，甚至迭代次數達到某一閾值后，可能只有一個粒子樣本的權值為非零，其它粒子樣本的權值都無效，導致計算量浪費在對聯合后驗概率幾乎不起作用的無效粒子的更新操作上，導致SLAM系統的性能降低。為了緩解上述涉及的粒子退化現象，一般采用以下兩種方法：在進行粒子采樣時選擇恰當的重要性概率密度函數，在SLAM中通常選用運動變換概率作為采樣的概率密度函數。使用該方法的缺點是：在進行采樣時，沒有考慮當前智能主體的觀測值：在權值更新完成之后引入重采樣技術，重采樣有多種實現方式，其中最簡單的是移除權值較小的粒子樣本，對剩余的粒子樣本進行歸一化處理，所以粒子的權值都相等，為粒子樣本空間數的倒數，重采樣技術減少了粒子的多樣性，減輕了樣本權值退化的現象，但并不能從本質上得到解決，

2 基于圖優化SLAM

2.1 基本思想

圖優化利用圖論意義上的圖，將SLAM后端最優化問題轉換圖的一種表現形式，圖是由若干個頂點（vertex）和連接這些頂點的邊（edge）構成的。在SLAM應用中，智能主體在任意時刻的位姿構成了圖優化的頂點，相鄰時刻位姿之間的運動狀態轉換構成了圖優化的邊，頂點即需要優化的變量，在構建好圖之后，需要調整智能主體的位姿盡可能地滿足圖中每條邊對應的約束。因此，后端圖優化過程可以分成以下兩個步驟來實現一構造圖和優化圖。構造圖：智能主體位姿表示頂點，位姿之間的關系表示邊，智能主體自身攜帶的傳感器信息構造出圖，該步驟是傳感器數據的堆積，也被稱為前端（front-end）;優化圖：即優化智能主體位姿，保證圖中頂點最符合其對應邊的約束，也被稱為后端（back-end）。

2.2 基于歐式空間的圖優化算法

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

Olson等將機器學習中最常用的隨機梯度下降法（stochastic gradient descent.SGD）應用到SLAM的后端優化算法中，提出了基于SGD的圖優化算法，在求解SLAM迭代的過程中，遍歷位姿圖中的每條邊，依據邊上限制的位姿變換關系，不斷地尋找梯度并下降的過程，從而在該方向上尋找最優的參數。直到某一時刻增量非常小，函數無法再下降，此時SGD收斂，目標函數達到了極小，完成極小值的搜索。通過實驗證明，基于SGD的圖優化算法計算速度快，對SLAM系統的初始值具有較高的健壯性，并且不易陷入局部最優值，同時對SLAM系統給不同的初始值，零值或隨機產生值，甚至與最優值相差很多，該算法都會產生很好的收斂效果。在Olson的基礎上，Grisetti對SGD進行了進一步的改進和擴展，使用數據結構中的樹結構來描述智能主體（2維空間或3維空間）的位姿變換，采用增量方式來求解智能主體最可能的狀態，從而將整個優化問題的核心轉換成求解增量方程。通過實驗證明，該算法既能夠有效地更新智能主體位姿和環境特征，也加快了收斂速度。除此之外，Grisetti將樹結構和SGD結合提出了基于樹型網格的優化法（tree-basednetwork optimizer.TORO），該算法用于6自由度的智能主體優化。

2.2.2 基于松弛的圖優化方法

Howard等將松弛算法（relaxation）應用到SLAM后端優化算法中，提出了基于松弛的圖優化算法。在后端算法的迭代過程中，對位姿圖中的每個頂點都做如下操作：其中頂點中包含智能主體位姿和環境特征信息，利用選取頂點的鄰接節點信息和運動變換方程重新計算頂點的最優解并更新。同時Duckett等假設在智能主體旋轉角度已知的情況下，證明了relaxation的圖優化算法必定收斂于系統最優解。通過實驗證明，基于relaxation的圖優化算法既可以解決靜態環境特征點下不變的SLAM問題，還可以應用于增量式SLAM系統，當環境中有特征發生改變，例如增加一個路標點，該算法可以直接將新環境特征更新到原有地圖中。但該算法存在一個缺陷，兩個頂點之間的約束條件存在較大的誤差時，relaxation可能需要多次迭代才能將兩頂點對應邊上的誤差減少并分配到其它頂點對應的約束條件上，從而增加了算法復雜度，同時環形閉合也需要解決此問題。在relaxation算法基礎上不斷改進，Frese等結合多網格方法來替代SLAM系統中的偏微分方程，提出一種多層次松弛（MLR）優化算法，提高了在環形閉合情況下頂點的優化效率。

2.3 基于流形空間的圖優化算法

基于SGD和relaxation的圖優化算法是在歐式空間進行運算的，在歐式三維空間中，對于智能主體來說，旋轉矩陣需要9個變量來描述3自由度的旋轉，具有一定的冗余性：其次歐拉角和旋轉向量是緊湊的但具有奇異性。因此需要尋找不帶奇異性的三維向量描述方式，引入四元數，會產生額外的自由度，Grisetti等進一步提出了在流形空間進行SLAM后端圖優化的思想，提出一種分層的圖優化算法（hierarchical graph optimization.HGO），該算法在圖優化算法創建位姿圖的過程，依據智能主體觀測值只在粗略描述這一層對地圖進行更新，降低了算法復雜度。在圖優化中，常見的有g20庫，Kummerle擴展了g2o庫，提出了基于流形空間的g2o通用庫，提高了開發效率：在通用庫基礎上又擴展了狀態空間，從而更好地完成智能主體自身定位和同步創建地圖的任務。

3 結束語

SLAM是實現移動機器人、自動駕駛以及混合現實等研究領域的關鍵技術之一，同時也是智能移動主體感知周圍環境的重要部分，近年來SLAM成為很多領域研究的重點與熱點。SLAM后端優化算法出現了濾波算法和圖優化算法，濾波算法依賴上一時刻智能主體的狀態，對于高斯濾波算法不能解決非強高斯或者非強線性的情況，而非參數濾波中PF存在粒子退化無法保證粒子多樣性問題：圖優化算法則依據系統所有的狀態信息進行優化，隨著對圖優化算法的深入研究，算法處理環境的規模能力逐漸提高，魯棒性較好，在非線性、稀疏結構以及擴展性方面仍需深入研究，語義地圖的創建具有較大的發展趨勢。