SLAM后端優(yōu)化算法的研究
2019-03-11 07:29:31劉芳
智能計算機與應(yīng)用 2019年6期
劉芳
摘要：隨著虛擬現(xiàn)實和自動駕駛的飛速發(fā)展，同時定位與地圖創(chuàng)建（sLAM）是近幾年研究的熱點，也是各領(lǐng)域?qū)崿F(xiàn)智能化的關(guān)鍵。本文針對SLAM后端優(yōu)化的兩種主要方法一基于濾波理論優(yōu)化和基于非線性優(yōu)化（圖優(yōu)化）展開綜述。首先，介紹了基于線性優(yōu)化的卡爾曼濾波器（KF）、粒子濾波器（PF），并詳細分析了不同濾波器模型的具體實現(xiàn)、性能、優(yōu)缺點;其次，闡述了基于圖優(yōu)化算法的整體框架、關(guān)鍵技術(shù)，著重介紹了在歐式空間以及流形空間優(yōu)化算法;最后對SLAM后端優(yōu)化算法的未來發(fā)展進行了展望。
關(guān)鍵詞：同時定位與地圖創(chuàng)建;濾波器模型;圖優(yōu)化;流形空間
0引言
智能主體能夠在未知環(huán)境中自主地完成一些功能，例如自動駕駛、掃地機器人、災(zāi)后救援等，需要不斷對周圍環(huán)境進行探測并創(chuàng)建環(huán)境地圖，對自身定位進行路徑規(guī)劃，避開障礙物準確到達目的。這些任務(wù)要求智能主體既要明白自身狀態(tài)（即位置），也需要了解外在環(huán)境（即地圖）。研究者把這類問題稱為同時定位與地圖創(chuàng)建（Simultaneous Locationand Mapping.SLAM），定位是指在室內(nèi)環(huán)境下，GPS或BD模塊接收不到衛(wèi)星信號，無法獲取經(jīng)緯度等位置相關(guān)信息時，智能主體則通過環(huán)境中特征（如路標或障礙物）來對自身的位姿進行估計：地圖創(chuàng)建是指將未知環(huán)境中所有物體的相關(guān)位置等信息提取出來，即對環(huán)境進行描述，這兩者密切相關(guān)，相互依賴。智能主體需要利用已經(jīng)創(chuàng)建出來的地圖對自身進行定位，又需要智能主體依據(jù)當前的位姿信息對環(huán)境地圖進行更新。近幾年來，SLAM成為智能主體研究的核心內(nèi)容，SLAM主要有傳感器數(shù)據(jù)讀取、前端視覺里程計、后端優(yōu)化、回環(huán)檢測、建圖五部分構(gòu)成。本文主要研究后端優(yōu)化部分，后端在接受不同時刻視覺里程計測量的相機位姿信息、回環(huán)檢測的信息，對這些信息進行優(yōu)化，得到智能主體全局一致的軌跡和地圖。主要有兩種實現(xiàn)方式：基于濾波理論優(yōu)化和基于非線性優(yōu)化（圖優(yōu)化）。
基于濾波理論優(yōu)化的方法主要是利用貝葉斯原理，從開始時刻到結(jié)束遞歸地進行。依據(jù)上一時刻置信度和運動變換概率的積分（或求和）估計當前狀態(tài)的置信度，然后利用當前時刻傳感器的觀測數(shù)據(jù)概率乘以當前時刻狀態(tài)的置信度，得到后驗概率。對于全局估計問題，由于存在很多不同的假設(shè)，每一種假設(shè)都會形成不同的后驗?zāi)Ｊ?，從而存在不同的濾波器算法。常見的有：卡爾曼濾波（KF）和擴展卡爾曼濾波（EKF）、信息濾波器（IF）和粒子濾波器（PF）。濾波算法具有時間約束和增量特性，通常也被稱為在線SLAM（On-Line SLAM）。
與濾波理論的求解算法相反，基于非線性（圖優(yōu)化）不再依賴某一時刻的信息，而是通過智能主體所有的運動信息和觀測數(shù)據(jù)來優(yōu)化智能主體完整的運動軌跡和環(huán)境特征地圖，也被稱為完全SLAM（Full SLAM）。其核心思想是：把后端優(yōu)化算法轉(zhuǎn)換成圖的一種形式，圖中的頂點代表了不同時刻智能主體的位姿和環(huán)境特征，有約束關(guān)系的各個頂點用邊來表示。建好圖之后，利用圖優(yōu)化算法對智能主體的位姿進行求解，使得頂點更好地滿足對應(yīng)邊上的約束條件，優(yōu)化算法結(jié)束之后，對應(yīng)的圖即是智能主體的運動軌跡和環(huán)境地圖。
本文將從以上兩方面對SLAM后端優(yōu)化算法展開研究。首先介紹線性優(yōu)化SLAm.包括高斯濾波算法卡爾曼濾波和非參數(shù)濾波算法粒子濾波：其次闡述圖優(yōu)化SLAm.包括歐式空間隨機梯度下降法和松弛算法，以及流形空間圖優(yōu)化算法。
1線性優(yōu)化SLAM
1.1線性優(yōu)化基本思想
SLAM過程可以由智能主體的運動方程和觀測方程來進行描述。其中運動方程是依據(jù)某一時刻之前智能主體的位姿、周圍環(huán)境空間特征、控制輸入變量來預(yù)測該時刻的狀態(tài)：觀測方程是智能主體自身攜帶傳感器的觀測值。在SLAM的求解過程中，線性優(yōu)化采用濾波器模型，該模型是基于貝葉斯概率的求解方式，進而將SLAM問題轉(zhuǎn)成求解智能主體位姿和周圍環(huán)境空間特征聯(lián)合概率。假設(shè)在某一時刻k.智能主體的位姿為xk，周圍環(huán)境空間特征為yk，控制輸入變量為uk。
首先利用以前狀態(tài)的后驗概率密度p（xk-1，yk-1|xk-2，uk-1，zk-1）和運動變換轉(zhuǎn)移概率P（xk|xk-1，uk）來預(yù)測k時刻的狀態(tài)分布：
其中，η為歸一化變量（乘積結(jié)果不再是一個概率，其總和可能不為1），Zk為傳感器觀察值。
依據(jù)上述的分析，可知線性優(yōu)化模型使用概率思想求解后端優(yōu)化問題，為了遞歸求解后驗概率，需要初始化置信度，并且在求解的過程中依據(jù)有關(guān)測量、運動變換轉(zhuǎn)移概率的不同假設(shè)，每一種假設(shè)都形成自己的后驗?zāi)Ｊ?，因此得出不同的濾波算法。常見的兩種濾波算法有：卡爾曼濾波器和粒子濾波器。
1.2 卡爾曼濾波算法
卡爾曼濾波器是一種線性系統(tǒng)的最優(yōu)無偏估計算法，使用高斯函數(shù)表示后驗概率，在可能的狀態(tài)空間中使用均值和方差（即高斯函數(shù)分布的均值和方差）來表達置信度。其中運動變換方程必須是帶有隨機高斯噪聲參數(shù)的線性函數(shù)，觀測方程也與帶高斯噪聲的自變量呈線性關(guān)系，初始置信度必須服從正態(tài)分布，依據(jù)以上的假定得到一個全局最優(yōu)的智能主體和周圍環(huán)境空間狀態(tài)估計。在具體的SLAM應(yīng)用中，KF算法主要由兩部分組成：狀態(tài)預(yù)測和更新。預(yù)測：依據(jù)智能主體上一時刻的位姿和環(huán)境特征、傳感器數(shù)據(jù)預(yù)測當前時刻的位姿和環(huán)境特征的聯(lián)合狀態(tài)估計（置信度），其中位姿和環(huán)境特征的聯(lián)合狀態(tài)表示均值，方差由運動變換矩陣求解：更新：首先計算卡爾曼增益，利用預(yù)測置信度、卡爾曼增益更新置信度，即更新智能主體當前時刻的后驗概率分布;以上兩個步驟不斷地遞歸執(zhí)行。算法的具體執(zhí)行步驟如下：
（1）假定運動變換概率p（xk|xk-1，uk）和觀測概率p（zk|xk）用下式表示：式中，xk和xk-1為狀態(tài)向量;uk為k時刻的控制變量;Ak運動變換矩陣;Bk為輸入控制矩陣;εk是一個高斯隨機變量，表示由狀態(tài)轉(zhuǎn)移引入的不確定性，均值為O;方差用只Rk表示;Ck為觀測矩陣;向量δk為觀測噪聲，均值為0.方差為Qk。
（2）預(yù)測
卡爾曼濾波作為線性高斯系統(tǒng)，觀測是狀態(tài)的線性函數(shù)，并且下一時刻狀態(tài)是歷史狀態(tài)的線性函數(shù)，使用最小均方誤差的方式估計線性系統(tǒng)，實際上運動變化和測量很少是線性的。因此，研究者提出了改進算法擴展卡爾曼濾波（EKF），放寬了其中的一個假設(shè)條件，將KF方法中線性的狀態(tài)轉(zhuǎn)移和觀測轉(zhuǎn)換為非線性的，使用雅可比矩陣替換KF中線性系統(tǒng)矩陣，從而置信度不再是一個高斯分布，而是高斯的近似值，得到狀態(tài)系統(tǒng)的次優(yōu)估計，EKF對一些強非線性和非高斯分布的應(yīng)用存在較大的近似誤差。
1.3粒子濾波算法
粒子濾波（PF）是貝葉斯濾波的一種非參數(shù)實現(xiàn)。與線性高斯KF不同，其不依賴確定的后驗函數(shù)，而通過有限數(shù)量的值來近似后驗，每一個值大致與狀態(tài)空間的某個區(qū)域有關(guān)，將Bayes中求解積分轉(zhuǎn)換成有限個樣本點求和的過程。PF的核心思想是利用一系列隨機狀態(tài)采樣的加權(quán)和來近似積分，求解后驗概率密度函數(shù)（置信度），當采樣粒子數(shù)量不斷增多達到一定閾值時，加權(quán)和就接近于狀態(tài)變量的后驗概率密度函數(shù)。在SLAM應(yīng)用中，PF主要包括兩個過程：采樣、權(quán)值更新，算法的詳細執(zhí)行如下：
（1）采樣：依據(jù)系統(tǒng)已知的、容易采樣的重要性概率密度函數(shù)g（x0：k|z1：k，從中抽取N個獨立的粒子樣本集合。
（2）權(quán)值更新：對樣本集合中每個采樣粒子進行權(quán)值求解。
系統(tǒng)的重要性概率密度函數(shù)為：
由于進行了乘法運算，乘法的總和可能不再是1.因此需要對粒子的權(quán)值進行歸一化，則有：
使用所有粒子以及對應(yīng)的權(quán)值求和來近似位姿和環(huán)境特征點的聯(lián)合后驗函數(shù)，則有：
當采樣粒子的數(shù)量足夠大（Nm→∞）時，公式（17）可以很接近地描述在智能主體的位姿和環(huán)境特征在該狀態(tài)下，最有可能產(chǎn)生當前的觀測數(shù)據(jù)。
粒子濾波算法為了得到正確的后驗概率估計值，樣本空間中粒子權(quán)值方差越接近零越好，但是隨著算法迭代次數(shù)的增多，粒子樣本權(quán)值存在退化現(xiàn)象。在樣本空間中只有少數(shù)粒子參與計算，甚至迭代次數(shù)達到某一閾值后，可能只有一個粒子樣本的權(quán)值為非零，其它粒子樣本的權(quán)值都無效，導致計算量浪費在對聯(lián)合后驗概率幾乎不起作用的無效粒子的更新操作上，導致SLAM系統(tǒng)的性能降低。為了緩解上述涉及的粒子退化現(xiàn)象，一般采用以下兩種方法：在進行粒子采樣時選擇恰當?shù)闹匾愿怕拭芏群瘮?shù)，在SLAM中通常選用運動變換概率作為采樣的概率密度函數(shù)。使用該方法的缺點是：在進行采樣時，沒有考慮當前智能主體的觀測值：在權(quán)值更新完成之后引入重采樣技術(shù)，重采樣有多種實現(xiàn)方式，其中最簡單的是移除權(quán)值較小的粒子樣本，對剩余的粒子樣本進行歸一化處理，所以粒子的權(quán)值都相等，為粒子樣本空間數(shù)的倒數(shù)，重采樣技術(shù)減少了粒子的多樣性，減輕了樣本權(quán)值退化的現(xiàn)象，但并不能從本質(zhì)上得到解決，
2 基于圖優(yōu)化SLAM
2.1 基本思想
圖優(yōu)化利用圖論意義上的圖，將SLAM后端最優(yōu)化問題轉(zhuǎn)換圖的一種表現(xiàn)形式，圖是由若干個頂點（vertex）和連接這些頂點的邊（edge）構(gòu)成的。在SLAM應(yīng)用中，智能主體在任意時刻的位姿構(gòu)成了圖優(yōu)化的頂點，相鄰時刻位姿之間的運動狀態(tài)轉(zhuǎn)換構(gòu)成了圖優(yōu)化的邊，頂點即需要優(yōu)化的變量，在構(gòu)建好圖之后，需要調(diào)整智能主體的位姿盡可能地滿足圖中每條邊對應(yīng)的約束。因此，后端圖優(yōu)化過程可以分成以下兩個步驟來實現(xiàn)一構(gòu)造圖和優(yōu)化圖。構(gòu)造圖：智能主體位姿表示頂點，位姿之間的關(guān)系表示邊，智能主體自身攜帶的傳感器信息構(gòu)造出圖，該步驟是傳感器數(shù)據(jù)的堆積，也被稱為前端（front-end）;優(yōu)化圖：即優(yōu)化智能主體位姿，保證圖中頂點最符合其對應(yīng)邊的約束，也被稱為后端（back-end）。
2.2 基于歐式空間的圖優(yōu)化算法
2.2.1 基于隨機梯度下降的圖優(yōu)化方法
Olson等將機器學習中最常用的隨機梯度下降法（stochastic gradient descent.SGD）應(yīng)用到SLAM的后端優(yōu)化算法中，提出了基于SGD的圖優(yōu)化算法，在求解SLAM迭代的過程中，遍歷位姿圖中的每條邊，依據(jù)邊上限制的位姿變換關(guān)系，不斷地尋找梯度并下降的過程，從而在該方向上尋找最優(yōu)的參數(shù)。直到某一時刻增量非常小，函數(shù)無法再下降，此時SGD收斂，目標函數(shù)達到了極小，完成極小值的搜索。通過實驗證明，基于SGD的圖優(yōu)化算法計算速度快，對SLAM系統(tǒng)的初始值具有較高的健壯性，并且不易陷入局部最優(yōu)值，同時對SLAM系統(tǒng)給不同的初始值，零值或隨機產(chǎn)生值，甚至與最優(yōu)值相差很多，該算法都會產(chǎn)生很好的收斂效果。在Olson的基礎(chǔ)上，Grisetti對SGD進行了進一步的改進和擴展，使用數(shù)據(jù)結(jié)構(gòu)中的樹結(jié)構(gòu)來描述智能主體（2維空間或3維空間）的位姿變換，采用增量方式來求解智能主體最可能的狀態(tài)，從而將整個優(yōu)化問題的核心轉(zhuǎn)換成求解增量方程。通過實驗證明，該算法既能夠有效地更新智能主體位姿和環(huán)境特征，也加快了收斂速度。除此之外，Grisetti將樹結(jié)構(gòu)和SGD結(jié)合提出了基于樹型網(wǎng)格的優(yōu)化法（tree-basednetwork optimizer.TORO），該算法用于6自由度的智能主體優(yōu)化。
2.2.2 基于松弛的圖優(yōu)化方法
Howard等將松弛算法（relaxation）應(yīng)用到SLAM后端優(yōu)化算法中，提出了基于松弛的圖優(yōu)化算法。在后端算法的迭代過程中，對位姿圖中的每個頂點都做如下操作：其中頂點中包含智能主體位姿和環(huán)境特征信息，利用選取頂點的鄰接節(jié)點信息和運動變換方程重新計算頂點的最優(yōu)解并更新。同時Duckett等假設(shè)在智能主體旋轉(zhuǎn)角度已知的情況下，證明了relaxation的圖優(yōu)化算法必定收斂于系統(tǒng)最優(yōu)解。通過實驗證明，基于relaxation的圖優(yōu)化算法既可以解決靜態(tài)環(huán)境特征點下不變的SLAM問題，還可以應(yīng)用于增量式SLAM系統(tǒng)，當環(huán)境中有特征發(fā)生改變，例如增加一個路標點，該算法可以直接將新環(huán)境特征更新到原有地圖中。但該算法存在一個缺陷，兩個頂點之間的約束條件存在較大的誤差時，relaxation可能需要多次迭代才能將兩頂點對應(yīng)邊上的誤差減少并分配到其它頂點對應(yīng)的約束條件上，從而增加了算法復(fù)雜度，同時環(huán)形閉合也需要解決此問題。在relaxation算法基礎(chǔ)上不斷改進，F(xiàn)rese等結(jié)合多網(wǎng)格方法來替代SLAM系統(tǒng)中的偏微分方程，提出一種多層次松弛（MLR）優(yōu)化算法，提高了在環(huán)形閉合情況下頂點的優(yōu)化效率。
2.3 基于流形空間的圖優(yōu)化算法
基于SGD和relaxation的圖優(yōu)化算法是在歐式空間進行運算的，在歐式三維空間中，對于智能主體來說，旋轉(zhuǎn)矩陣需要9個變量來描述3自由度的旋轉(zhuǎn)，具有一定的冗余性：其次歐拉角和旋轉(zhuǎn)向量是緊湊的但具有奇異性。因此需要尋找不帶奇異性的三維向量描述方式，引入四元數(shù)，會產(chǎn)生額外的自由度，Grisetti等進一步提出了在流形空間進行SLAM后端圖優(yōu)化的思想，提出一種分層的圖優(yōu)化算法（hierarchical graph optimization.HGO），該算法在圖優(yōu)化算法創(chuàng)建位姿圖的過程，依據(jù)智能主體觀測值只在粗略描述這一層對地圖進行更新，降低了算法復(fù)雜度。在圖優(yōu)化中，常見的有g(shù)20庫，Kummerle擴展了g2o庫，提出了基于流形空間的g2o通用庫，提高了開發(fā)效率：在通用庫基礎(chǔ)上又擴展了狀態(tài)空間，從而更好地完成智能主體自身定位和同步創(chuàng)建地圖的任務(wù)。
3 結(jié)束語
SLAM是實現(xiàn)移動機器人、自動駕駛以及混合現(xiàn)實等研究領(lǐng)域的關(guān)鍵技術(shù)之一，同時也是智能移動主體感知周圍環(huán)境的重要部分，近年來SLAM成為很多領(lǐng)域研究的重點與熱點。SLAM后端優(yōu)化算法出現(xiàn)了濾波算法和圖優(yōu)化算法，濾波算法依賴上一時刻智能主體的狀態(tài)，對于高斯濾波算法不能解決非強高斯或者非強線性的情況，而非參數(shù)濾波中PF存在粒子退化無法保證粒子多樣性問題：圖優(yōu)化算法則依據(jù)系統(tǒng)所有的狀態(tài)信息進行優(yōu)化，隨著對圖優(yōu)化算法的深入研究，算法處理環(huán)境的規(guī)模能力逐漸提高，魯棒性較好，在非線性、稀疏結(jié)構(gòu)以及擴展性方面仍需深入研究，語義地圖的創(chuàng)建具有較大的發(fā)展趨勢。
