劉芳

摘要：隨著虛擬現(xiàn)實(shí)和自動(dòng)駕駛的飛速發(fā)展，同時(shí)定位與地圖創(chuàng)建（sLAM）是近幾年研究的熱點(diǎn)，也是各領(lǐng)域?qū)崿F(xiàn)智能化的關(guān)鍵。本文針對(duì)SLAM后端優(yōu)化的兩種主要方法一基于濾波理論優(yōu)化和基于非線性優(yōu)化（圖優(yōu)化）展開綜述。首先，介紹了基于線性優(yōu)化的卡爾曼濾波器（KF）、粒子濾波器（PF），并詳細(xì)分析了不同濾波器模型的具體實(shí)現(xiàn)、性能、優(yōu)缺點(diǎn);其次，闡述了基于圖優(yōu)化算法的整體框架、關(guān)鍵技術(shù)，著重介紹了在歐式空間以及流形空間優(yōu)化算法;最后對(duì)SLAM后端優(yōu)化算法的未來(lái)發(fā)展進(jìn)行了展望。

關(guān)鍵詞：同時(shí)定位與地圖創(chuàng)建;濾波器模型;圖優(yōu)化;流形空間

0引言

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

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

與濾波理論的求解算法相反，基于非線性（圖優(yōu)化）不再依賴某一時(shí)刻的信息，而是通過(guò)智能主體所有的運(yùn)動(dòng)信息和觀測(cè)數(shù)據(jù)來(lái)優(yōu)化智能主體完整的運(yùn)動(dòng)軌跡和環(huán)境特征地圖，也被稱為完全SLAM（Full SLAM）。其核心思想是：把后端優(yōu)化算法轉(zhuǎn)換成圖的一種形式，圖中的頂點(diǎn)代表了不同時(shí)刻智能主體的位姿和環(huán)境特征，有約束關(guān)系的各個(gè)頂點(diǎn)用邊來(lái)表示。建好圖之后，利用圖優(yōu)化算法對(duì)智能主體的位姿進(jìn)行求解，使得頂點(diǎn)更好地滿足對(duì)應(yīng)邊上的約束條件，優(yōu)化算法結(jié)束之后，對(duì)應(yīng)的圖即是智能主體的運(yùn)動(dòng)軌跡和環(huán)境地圖。

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

1線性優(yōu)化SLAM

1.1線性優(yōu)化基本思想

SLAM過(guò)程可以由智能主體的運(yùn)動(dòng)方程和觀測(cè)方程來(lái)進(jìn)行描述。其中運(yùn)動(dòng)方程是依據(jù)某一時(shí)刻之前智能主體的位姿、周圍環(huán)境空間特征、控制輸入變量來(lái)預(yù)測(cè)該時(shí)刻的狀態(tài)：觀測(cè)方程是智能主體自身攜帶傳感器的觀測(cè)值。在SLAM的求解過(guò)程中，線性優(yōu)化采用濾波器模型，該模型是基于貝葉斯概率的求解方式，進(jìn)而將SLAM問(wèn)題轉(zhuǎn)成求解智能主體位姿和周圍環(huán)境空間特征聯(lián)合概率。假設(shè)在某一時(shí)刻k.智能主體的位姿為x_k，周圍環(huán)境空間特征為y_k，控制輸入變量為u_k。

首先利用以前狀態(tài)的后驗(yàn)概率密度p（x_k-1，y_k-1|x_k-2，u_k-1，z_k-1）和運(yùn)動(dòng)變換轉(zhuǎn)移概率P（x_k|x_k-1，u_k）來(lái)預(yù)測(cè)k時(shí)刻的狀態(tài)分布：

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

依據(jù)上述的分析，可知線性優(yōu)化模型使用概率思想求解后端優(yōu)化問(wèn)題，為了遞歸求解后驗(yàn)概率，需要初始化置信度，并且在求解的過(guò)程中依據(jù)有關(guān)測(cè)量、運(yùn)動(dòng)變換轉(zhuǎn)移概率的不同假設(shè)，每一種假設(shè)都形成自己的后驗(yàn)?zāi)Ｊ?，因此得出不同的濾波算法。常見(jiàn)的兩種濾波算法有：卡爾曼濾波器和粒子濾波器。

1.2 卡爾曼濾波算法

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

（1）假定運(yùn)動(dòng)變換概率p（x_k|x_k-1，u_k）和觀測(cè)概率p（z_k|x_k）用下式表示：式中，x_k和x_k-1為狀態(tài)向量;u_k為k時(shí)刻的控制變量;A_k運(yùn)動(dòng)變換矩陣;B_k為輸入控制矩陣;ε_k是一個(gè)高斯隨機(jī)變量，表示由狀態(tài)轉(zhuǎn)移引入的不確定性，均值為O;方差用只R_k表示;C_k為觀測(cè)矩陣;向量δ_k為觀測(cè)噪聲，均值為0.方差為Q_k。

（2）預(yù)測(cè)

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

1.3粒子濾波算法

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

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

（2）權(quán)值更新：對(duì)樣本集合中每個(gè)采樣粒子進(jìn)行權(quán)值求解。

系統(tǒng)的重要性概率密度函數(shù)為：

由于進(jìn)行了乘法運(yùn)算，乘法的總和可能不再是1.因此需要對(duì)粒子的權(quán)值進(jìn)行歸一化，則有：

使用所有粒子以及對(duì)應(yīng)的權(quán)值求和來(lái)近似位姿和環(huán)境特征點(diǎn)的聯(lián)合后驗(yàn)函數(shù)，則有：

當(dāng)采樣粒子的數(shù)量足夠大（N_m→∞）時(shí)，公式（17）可以很接近地描述在智能主體的位姿和環(huán)境特征在該狀態(tài)下，最有可能產(chǎn)生當(dāng)前的觀測(cè)數(shù)據(jù)。

粒子濾波算法為了得到正確的后驗(yàn)概率估計(jì)值，樣本空間中粒子權(quán)值方差越接近零越好，但是隨著算法迭代次數(shù)的增多，粒子樣本權(quán)值存在退化現(xiàn)象。在樣本空間中只有少數(shù)粒子參與計(jì)算，甚至迭代次數(shù)達(dá)到某一閾值后，可能只有一個(gè)粒子樣本的權(quán)值為非零，其它粒子樣本的權(quán)值都無(wú)效，導(dǎo)致計(jì)算量浪費(fèi)在對(duì)聯(lián)合后驗(yàn)概率幾乎不起作用的無(wú)效粒子的更新操作上，導(dǎo)致SLAM系統(tǒng)的性能降低。為了緩解上述涉及的粒子退化現(xiàn)象，一般采用以下兩種方法：在進(jìn)行粒子采樣時(shí)選擇恰當(dāng)?shù)闹匾愿怕拭芏群瘮?shù)，在SLAM中通常選用運(yùn)動(dòng)變換概率作為采樣的概率密度函數(shù)。使用該方法的缺點(diǎn)是：在進(jìn)行采樣時(shí)，沒(méi)有考慮當(dāng)前智能主體的觀測(cè)值：在權(quán)值更新完成之后引入重采樣技術(shù)，重采樣有多種實(shí)現(xiàn)方式，其中最簡(jiǎn)單的是移除權(quán)值較小的粒子樣本，對(duì)剩余的粒子樣本進(jìn)行歸一化處理，所以粒子的權(quán)值都相等，為粒子樣本空間數(shù)的倒數(shù)，重采樣技術(shù)減少了粒子的多樣性，減輕了樣本權(quán)值退化的現(xiàn)象，但并不能從本質(zhì)上得到解決，

2 基于圖優(yōu)化SLAM

2.1 基本思想

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

2.2 基于歐式空間的圖優(yōu)化算法

2.2.1 基于隨機(jī)梯度下降的圖優(yōu)化方法

Olson等將機(jī)器學(xué)習(xí)中最常用的隨機(jī)梯度下降法（stochastic gradient descent.SGD）應(yīng)用到SLAM的后端優(yōu)化算法中，提出了基于SGD的圖優(yōu)化算法，在求解SLAM迭代的過(guò)程中，遍歷位姿圖中的每條邊，依據(jù)邊上限制的位姿變換關(guān)系，不斷地尋找梯度并下降的過(guò)程，從而在該方向上尋找最優(yōu)的參數(shù)。直到某一時(shí)刻增量非常小，函數(shù)無(wú)法再下降，此時(shí)SGD收斂，目標(biāo)函數(shù)達(dá)到了極小，完成極小值的搜索。通過(guò)實(shí)驗(yàn)證明，基于SGD的圖優(yōu)化算法計(jì)算速度快，對(duì)SLAM系統(tǒng)的初始值具有較高的健壯性，并且不易陷入局部最優(yōu)值，同時(shí)對(duì)SLAM系統(tǒng)給不同的初始值，零值或隨機(jī)產(chǎn)生值，甚至與最優(yōu)值相差很多，該算法都會(huì)產(chǎn)生很好的收斂效果。在Olson的基礎(chǔ)上，Grisetti對(duì)SGD進(jìn)行了進(jìn)一步的改進(jìn)和擴(kuò)展，使用數(shù)據(jù)結(jié)構(gòu)中的樹結(jié)構(gòu)來(lái)描述智能主體（2維空間或3維空間）的位姿變換，采用增量方式來(lái)求解智能主體最可能的狀態(tài)，從而將整個(gè)優(yōu)化問(wèn)題的核心轉(zhuǎn)換成求解增量方程。通過(guò)實(shí)驗(yà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)化算法。在后端算法的迭代過(guò)程中，對(duì)位姿圖中的每個(gè)頂點(diǎn)都做如下操作：其中頂點(diǎn)中包含智能主體位姿和環(huán)境特征信息，利用選取頂點(diǎn)的鄰接節(jié)點(diǎn)信息和運(yùn)動(dòng)變換方程重新計(jì)算頂點(diǎn)的最優(yōu)解并更新。同時(shí)Duckett等假設(shè)在智能主體旋轉(zhuǎn)角度已知的情況下，證明了relaxation的圖優(yōu)化算法必定收斂于系統(tǒng)最優(yōu)解。通過(guò)實(shí)驗(yàn)證明，基于relaxation的圖優(yōu)化算法既可以解決靜態(tài)環(huán)境特征點(diǎn)下不變的SLAM問(wèn)題，還可以應(yīng)用于增量式SLAM系統(tǒng)，當(dāng)環(huán)境中有特征發(fā)生改變，例如增加一個(gè)路標(biāo)點(diǎn)，該算法可以直接將新環(huán)境特征更新到原有地圖中。但該算法存在一個(gè)缺陷，兩個(gè)頂點(diǎn)之間的約束條件存在較大的誤差時(shí)，relaxation可能需要多次迭代才能將兩頂點(diǎn)對(duì)應(yīng)邊上的誤差減少并分配到其它頂點(diǎn)對(duì)應(yīng)的約束條件上，從而增加了算法復(fù)雜度，同時(shí)環(huán)形閉合也需要解決此問(wèn)題。在relaxation算法基礎(chǔ)上不斷改進(jìn)，F(xiàn)rese等結(jié)合多網(wǎng)格方法來(lái)替代SLAM系統(tǒng)中的偏微分方程，提出一種多層次松弛（MLR）優(yōu)化算法，提高了在環(huán)形閉合情況下頂點(diǎn)的優(yōu)化效率。

2.3 基于流形空間的圖優(yōu)化算法

基于SGD和relaxation的圖優(yōu)化算法是在歐式空間進(jìn)行運(yùn)算的，在歐式三維空間中，對(duì)于智能主體來(lái)說(shuō)，旋轉(zhuǎn)矩陣需要9個(gè)變量來(lái)描述3自由度的旋轉(zhuǎn)，具有一定的冗余性：其次歐拉角和旋轉(zhuǎn)向量是緊湊的但具有奇異性。因此需要尋找不帶奇異性的三維向量描述方式，引入四元數(shù)，會(huì)產(chǎn)生額外的自由度，Grisetti等進(jìn)一步提出了在流形空間進(jìn)行SLAM后端圖優(yōu)化的思想，提出一種分層的圖優(yōu)化算法（hierarchical graph optimization.HGO），該算法在圖優(yōu)化算法創(chuàng)建位姿圖的過(guò)程，依據(jù)智能主體觀測(cè)值只在粗略描述這一層對(duì)地圖進(jìn)行更新，降低了算法復(fù)雜度。在圖優(yōu)化中，常見(jiàn)的有g(shù)20庫(kù)，Kummerle擴(kuò)展了g2o庫(kù)，提出了基于流形空間的g2o通用庫(kù)，提高了開發(fā)效率：在通用庫(kù)基礎(chǔ)上又?jǐn)U展了狀態(tài)空間，從而更好地完成智能主體自身定位和同步創(chuàng)建地圖的任務(wù)。

3 結(jié)束語(yǔ)

SLAM是實(shí)現(xiàn)移動(dòng)機(jī)器人、自動(dòng)駕駛以及混合現(xiàn)實(shí)等研究領(lǐng)域的關(guān)鍵技術(shù)之一，同時(shí)也是智能移動(dòng)主體感知周圍環(huán)境的重要部分，近年來(lái)SLAM成為很多領(lǐng)域研究的重點(diǎn)與熱點(diǎn)。SLAM后端優(yōu)化算法出現(xiàn)了濾波算法和圖優(yōu)化算法，濾波算法依賴上一時(shí)刻智能主體的狀態(tài)，對(duì)于高斯濾波算法不能解決非強(qiáng)高斯或者非強(qiáng)線性的情況，而非參數(shù)濾波中PF存在粒子退化無(wú)法保證粒子多樣性問(wèn)題：圖優(yōu)化算法則依據(jù)系統(tǒng)所有的狀態(tài)信息進(jìn)行優(yōu)化，隨著對(duì)圖優(yōu)化算法的深入研究，算法處理環(huán)境的規(guī)模能力逐漸提高，魯棒性較好，在非線性、稀疏結(jié)構(gòu)以及擴(kuò)展性方面仍需深入研究，語(yǔ)義地圖的創(chuàng)建具有較大的發(fā)展趨勢(shì)。