移動(dòng)機(jī)器人的同時(shí)定位與地圖創(chuàng)建的算法研究

王芳 劉晉

摘 要：移動(dòng)機(jī)器人的同時(shí)定位與地圖創(chuàng)建是當(dāng)前機(jī)器人領(lǐng)域研究的基本問題與熱點(diǎn)，也是實(shí)現(xiàn)智能機(jī)器人實(shí)現(xiàn)自主導(dǎo)航和控制決策的關(guān)鍵。本文從基于貝葉斯濾波器模型和基于圖優(yōu)化平滑模型兩個(gè)方面對(duì)移動(dòng)機(jī)器人同時(shí)定位與地圖創(chuàng)建（SLAM）的研究進(jìn)行了綜述，介紹了這兩種模型的基本框架以及關(guān)鍵技術(shù)，闡述了模型的各種不同實(shí)現(xiàn)形式，分析了這些算法的性能，并探討了這些算法的優(yōu)缺點(diǎn)以及相關(guān)難點(diǎn)的解決思路。最后對(duì)基于濾波器模型和基于平滑模型的同時(shí)定位與地圖創(chuàng)建的未來發(fā)展進(jìn)行展望。

關(guān)鍵詞：同時(shí)定位與地圖創(chuàng)建；貝葉斯；濾波器模型；圖優(yōu)化；平滑模型

中圖分類號(hào)：TP 文獻(xiàn)標(biāo)識(shí)號(hào)：A 文章編號(hào)：2095-2163（2015-）02-

Research of Simultaneous Location and Mapping of the Mobile Robot

Liu Jin1， Wang Fang2

（1 Unit 261251， Qinhuangdao Hebei， 066102， China；2 Weather Bureaus of Changdu in Tibet， Changdu Xizang， 854000， China）

Abstract： Simultaneous Location and Mapping of the mobile robot are the basic problems and hot pots in the field of robotics，and they are also the key to realize autonomous navigation and control decision. This paper introduces two algorithms：Filter based on Bayes and Smoothing based on graph-based， reviews the basic framework and the key technologies， and elaborates the various forms of achieving. After that， the paper analyzes the performance of these algorithms， and lists the advantages and disadvantages， as well as the way of dealing with difficulties.Finally， the potential future issues and research trends are also explored.

Keywords： SLAM； Bayes； Filter； Graph-Based； Smoothing

0 引言

為了有效地解決移動(dòng)機(jī)器人能夠自主完成許多預(yù)想的任務(wù)，例如運(yùn)輸、搜索、救援、自動(dòng)吸塵等現(xiàn)實(shí)需求，機(jī)器人需要一個(gè)準(zhǔn)確的工作環(huán)境地圖。地圖的準(zhǔn)確性可以使所設(shè)計(jì)的系統(tǒng)在復(fù)雜的環(huán)境下，僅依賴自身攜帶的傳感器來完成相應(yīng)的指令，而不需要依賴像GPS等外部參考系統(tǒng)，尤其是在室內(nèi)環(huán)境下GPS還不能使用。地圖創(chuàng)建是指建立機(jī)器人所處環(huán)境的不同物體如障礙物、路標(biāo)等的準(zhǔn)確的空間位置以及特征描述，定位是指移動(dòng)機(jī)器人在其所處的工作環(huán)境中根據(jù)一些已知特征來確定自身位置的過程。機(jī)器人依據(jù)已建立的環(huán)境地圖信息進(jìn)行自身的定位，同時(shí)又根據(jù)機(jī)器人定位的效果來更新環(huán)境地圖，這兩者相互依賴，將兩者結(jié)合起來進(jìn)行研究，稱為移動(dòng)機(jī)器人的同時(shí)定位與地圖創(chuàng)建（Simultaneous Location and Mapping，SLAM），有時(shí)也稱為并發(fā)定位與建圖（Concurrent Localization and Mapping，CLM）。SLAM可以使用很多種方法來實(shí)現(xiàn)，這些方法整體上可以概括為以下兩類：濾波器方法和平滑方法。

濾波器方法的主要思想是：運(yùn)用動(dòng)態(tài)貝葉斯理論，根據(jù)前面時(shí)刻機(jī)器人的位姿、控制輸入信息以及觀測(cè)信息這些已知的先驗(yàn)信息，來估計(jì)當(dāng)前系統(tǒng)狀態(tài)（機(jī)器人當(dāng)前位姿以及地圖特征位置）的后驗(yàn)概率。根據(jù)先驗(yàn)概率的不同假設(shè)，運(yùn)用不同的求解方式來計(jì)算系統(tǒng)的后驗(yàn)概率，就可以得到不同的基于濾波器的算法。常見的濾波器算法有：卡爾曼濾波器（KF）和擴(kuò)展卡爾曼濾波器（EKF）[1-2]、粒子濾波器（PF）[3-5]和信息濾波器（IF）[6-7]，濾波器主要強(qiáng)調(diào)的是時(shí)間以及增量特性，此方法也被稱為在線SLAM。

相反，平滑方法的主要思想是：根據(jù)機(jī)器人在運(yùn)動(dòng)過程中所有的控制信息和觀測(cè)信息來優(yōu)化機(jī)器人完整的運(yùn)功軌跡，并在此基礎(chǔ)上創(chuàng)建環(huán)境地圖，因此也被稱為完全SLAM方法[8-10]。這類方法可以使用位姿圖來構(gòu)造SLAM，位姿圖中的圖節(jié)點(diǎn)代表了機(jī)器人在不同時(shí)刻的不同位置信息，圖中的邊對(duì)應(yīng)兩個(gè)位姿之間的約束信息，其中約束通常是指機(jī)器人在某時(shí)刻的觀測(cè)信息或者是運(yùn)動(dòng)過程中的控制輸入信息。將位姿圖構(gòu)建好之后，對(duì)圖中的節(jié)點(diǎn)進(jìn)行優(yōu)化，即對(duì)機(jī)器人的位姿進(jìn)行調(diào)整，使得位姿之間的關(guān)系更好地滿足節(jié)點(diǎn)之間的約束，在優(yōu)化完成之后得到的圖對(duì)應(yīng)機(jī)器人的運(yùn)動(dòng)軌跡。此方法又稱為圖優(yōu)化的SLAM。

1 基于濾波器的SLAM算法

1.1 Bayes濾波模型

Bayes濾波原理：對(duì)于SLAM問題，根據(jù)之前的移動(dòng)機(jī)器人位姿、觀測(cè)信息以及控制輸入信息來求得t時(shí)刻機(jī)器人位姿x和環(huán)境中特征m的聯(lián)合后驗(yàn)概率：

（1）

SLAM問題可以分為預(yù)測(cè)、更新兩步遞歸進(jìn)行：

（1）預(yù)測(cè)：根據(jù)前一時(shí)刻狀態(tài)的后驗(yàn)概率密度，結(jié)合狀態(tài)轉(zhuǎn)移概率來求。數(shù)學(xué)公式為：

（2）

（2）更新：利用實(shí)際的測(cè)量值以及由觀測(cè)模型得到的觀測(cè)值來更新當(dāng)前t時(shí)刻狀態(tài)的后驗(yàn)概率分布，計(jì)算公式為：

（3）

其中，為標(biāo)準(zhǔn)化因子。

1.2 KF-SLAM

卡爾曼濾波器是一種最優(yōu)線性遞歸估計(jì)算法，利用線性的系統(tǒng)狀態(tài)轉(zhuǎn)移方程和觀測(cè)方程得到一個(gè)全局最優(yōu)的狀態(tài)估計(jì)。在SLAM的應(yīng)用中，卡爾曼濾波器分為3個(gè)步驟：預(yù)測(cè)、觀測(cè)、更新。各步的功能實(shí)現(xiàn)可做如下描述：

（1） 預(yù)測(cè)：根據(jù)機(jī)器人的運(yùn)動(dòng)模型和觀測(cè)模型，預(yù)測(cè)系統(tǒng)狀態(tài)的估計(jì)值和觀測(cè)估計(jì)值，并計(jì)算預(yù)測(cè)系統(tǒng)估計(jì)的誤差協(xié)方差；

（2） 觀測(cè)：當(dāng)機(jī)器人在某時(shí)刻獲取觀測(cè)量后，將傳感器的觀測(cè)量與之前預(yù)測(cè)的觀測(cè)量的值進(jìn)行比較獲得全新信息，同時(shí)計(jì)算該全新信息產(chǎn)生的協(xié)方差；

（3） 更新：利用觀測(cè)的信息對(duì)預(yù)測(cè)的系統(tǒng)狀態(tài)估計(jì)進(jìn)行校正，并對(duì)系統(tǒng)狀態(tài)估計(jì)誤差的協(xié)方差進(jìn)行更新。

首先，用下面的線性方程來表示系統(tǒng)狀態(tài)轉(zhuǎn)移方程和觀測(cè)方程：

（4）

（5）

其中，為系統(tǒng)的狀態(tài)向量，為系統(tǒng)的觀測(cè)序列，為系統(tǒng)過程噪聲序列，為系統(tǒng)觀測(cè)噪聲序列，為狀態(tài)轉(zhuǎn)移矩陣，為系統(tǒng)輸入控制矩陣，為系統(tǒng)輸入控制向量，為系統(tǒng)的觀測(cè)矩陣。假設(shè)和vk均服從均值為0方差分別為和的高斯白噪聲序列，在k-1時(shí)刻已知系統(tǒng)的狀態(tài)估計(jì)值為和協(xié)方差矩陣，則卡爾曼濾波算法的具體步驟為：

1、運(yùn)用前一時(shí)刻的狀態(tài)估計(jì)值和協(xié)方差矩陣來預(yù)測(cè)當(dāng)前時(shí)刻的狀態(tài)估計(jì)和，即：

（6）

（7）

2、根據(jù)預(yù)測(cè)的協(xié)方差矩陣和觀測(cè)噪聲協(xié)方差矩陣來計(jì)算卡爾曼濾波器的增益：

（8）

3、根據(jù)預(yù)測(cè)的狀態(tài)估計(jì)和實(shí)際觀測(cè)值修正系統(tǒng)的狀態(tài)估計(jì)，并計(jì)算相應(yīng)的協(xié)方差矩陣，即測(cè)量更新：

（9）

（10）

卡爾曼濾波器雖然不需要存儲(chǔ)系統(tǒng)以前的數(shù)據(jù)就可以不斷地進(jìn)行系統(tǒng)更新，但是卡爾曼濾波器中，系統(tǒng)的狀態(tài)模型和傳感器模型必須是線性的，然而在實(shí)際的應(yīng)用中，這些模型往往是非線性的，為了適應(yīng)非線性系統(tǒng)擴(kuò)展卡爾曼（EKF）以及EKF的改進(jìn)算法，這些算法的主要思想是：將非線性的運(yùn)動(dòng)方程和非線性的觀測(cè)方程進(jìn)一步轉(zhuǎn)換為可以用卡爾曼濾波器來解決的線性系統(tǒng)函數(shù)，從而將非線性的狀態(tài)估計(jì)問題運(yùn)用最優(yōu)的線性卡爾曼濾波器來求解。同時(shí)這些算法得到的后驗(yàn)概率分布是類似的高斯分布，對(duì)于一些非高斯分布的模型系統(tǒng)仍然存在很大的誤差。

1.3 PF-SLAM

粒子濾波是一種基于蒙特卡羅方法和遞推貝葉斯估計(jì)的統(tǒng)計(jì)濾波方法，即依據(jù)大數(shù)定理采用蒙特卡羅方法來求解貝葉斯估計(jì)中的積分運(yùn)算。其基本思想是：首先依據(jù)系統(tǒng)狀態(tài)向量的經(jīng)驗(yàn)條件分布在狀態(tài)空間產(chǎn)生一組隨機(jī)樣本的集合，稱這些樣本為粒子，然后根據(jù)量測(cè)不斷調(diào)整粒子的權(quán)重和位置，通過調(diào)整后的粒子信息修正最初的經(jīng)驗(yàn)條件分布。其實(shí)質(zhì)是用由粒子及其權(quán)重組成的離散隨機(jī)測(cè)度近似相關(guān)的概率分布，并且根據(jù)算法遞推更新離散隨機(jī)測(cè)度。當(dāng)樣本容量很大時(shí)，這種蒙特卡羅描述就近似于狀態(tài)變量真實(shí)的后驗(yàn)概率密度函數(shù)。

根據(jù)Bayes理論，在已知t時(shí)刻之前的觀測(cè)信息的條件下，所有與系統(tǒng)狀態(tài)相關(guān)的信息都可以從后驗(yàn)概率分布中獲得。設(shè)有一個(gè)帶權(quán)重的離散粒子的集合，，其中是一個(gè)采樣或稱之為粒子，表示在t時(shí)刻系統(tǒng)的一個(gè)可能狀態(tài)；被稱為采樣或者粒子的權(quán)重，且和，N表示粒子的個(gè)數(shù)。用該采樣集合表示系統(tǒng)的后驗(yàn)概率。

粒子濾波算法包括三個(gè)主要步驟：

1.3.1 采樣

根據(jù)前一時(shí)刻的樣本集構(gòu)建下一時(shí)刻的樣本集，即采樣或說是從提議分布中抽取樣本。

1.3.2 重要性賦權(quán)

計(jì)算新樣本集每一個(gè)樣本的重要性權(quán)值。對(duì)應(yīng)公式為：

（11）

若提議分布可以分解為下式：

（12）

（13）

則得到權(quán)值為：

（14）

如果，則重要密度函數(shù)僅依賴于和，在計(jì)算時(shí)，僅需存儲(chǔ)粒子，而不必關(guān)心粒子集和過去量測(cè)值。修正后的權(quán)值為：

（15）

在標(biāo)準(zhǔn)的粒子濾波算法中選擇相對(duì)來說容易實(shí)現(xiàn)的先驗(yàn)概率密度函數(shù)作為提議分布，即 （16）

那么將權(quán)重簡(jiǎn)化為：

（17）

再將權(quán)值歸一化，即：

（18）

后驗(yàn)概率密度可表示為：

（19）

當(dāng)時(shí)，由大數(shù)定理即可保證上式可逼近真實(shí)后驗(yàn)概率。

1.3.3 重新采樣

根據(jù)1.3.2中計(jì)算出來的每一個(gè)樣本的重要性權(quán)值，從樣本集中重新采樣，保證每個(gè)樣本被抽中的概率與其重要性權(quán)值成正比。經(jīng)過重新采樣得到的樣本組成新的樣本集合。

但是，由于標(biāo)準(zhǔn)粒子濾波算法選擇先驗(yàn)概率密度作為重要密度函數(shù)，若在對(duì)量測(cè)精度要求低的場(chǎng)合，這種選取方法能夠獲得較好的結(jié)果。不過，由于沒有考慮當(dāng)前的量測(cè)值，從重要性概率密度中取樣得到的樣本與從真實(shí)后驗(yàn)概率密度采樣得到的樣本有很大偏差。因此，重要性權(quán)重的方差隨著時(shí)間而隨機(jī)遞增，使得粒子的權(quán)重集中到少數(shù)粒子上，甚至在經(jīng)過幾步的遞歸之后，可能只有一個(gè)粒子有非零權(quán)值，其他粒子的權(quán)值卻都很小，為此忽略不計(jì)，從而使得大量的計(jì)算工作都浪費(fèi)在用來更新那些對(duì)的估計(jì)幾乎不起作用的粒子上，結(jié)果粒子集無法表達(dá)實(shí)際的后驗(yàn)概率分布，造成粒子退化。為避免退化問題發(fā)生，引入重采樣技術(shù)，最簡(jiǎn)單的方法就是減少或剔除小權(quán)值粒子，而對(duì)大權(quán)值粒子則按照其權(quán)值大小進(jìn)行復(fù)制，最后將所有的粒子權(quán)值設(shè)置為，來避免粒子的退化現(xiàn)象。

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

2.1基本思想

基于圖優(yōu)化的SLAM算法中構(gòu)建的位姿圖，圖中的每個(gè)節(jié)點(diǎn)表示機(jī)器人的位姿，即機(jī)器人在工作環(huán)境下的位置，節(jié)點(diǎn)之間的空間約束是通過觀測(cè)變量或者是控制輸入來實(shí)現(xiàn)的，同時(shí)空間約束服從一定的概率分布。圖優(yōu)化的SLAM算法主要分為兩個(gè)部分：圖構(gòu)造和圖優(yōu)化。具體來說，圖構(gòu)造是根據(jù)機(jī)器人傳感器的原始數(shù)據(jù)構(gòu)造出相應(yīng)的圖，這部分嚴(yán)重依賴于傳感器，也稱為前端；而圖優(yōu)化則是確定最符合圖中邊約束的機(jī)器人位姿的布局，此部分依賴于抽象的數(shù)據(jù)，與傳感器的數(shù)據(jù)無關(guān)，也稱為后端。下面主要介紹了圖優(yōu)化即后端的算法。

2.2基于松弛的優(yōu)化方法

Howard[11]等提出采用松弛方法來求解移動(dòng)機(jī)器人的同時(shí)定位與地圖的創(chuàng)建問題。基本思想是：在每一次的迭代過程中，遍歷位姿圖中的所有節(jié)點(diǎn)，遍歷的過程中對(duì)每一個(gè)節(jié)點(diǎn)都執(zhí)行以下的操作，根據(jù)其相鄰節(jié)點(diǎn)的位置及節(jié)點(diǎn)之間的約束關(guān)系重新計(jì)算并更新該節(jié)點(diǎn)的位置信息。而在機(jī)器人與坐標(biāo)系的夾角確定的情況下，Duckett 等證明了使用圖優(yōu)化的松弛算法必收斂于最優(yōu)解。該方法不僅可以用于根據(jù)全局的數(shù)據(jù)來解決SLAM問題的算法中，還可用于增量式的SLAM 中，每當(dāng)有新的特征出現(xiàn)時(shí)，可以直接更新上一輪結(jié)果中得到的地圖。但算法存在的缺陷是，當(dāng)機(jī)器人兩個(gè)位姿之間的約束即節(jié)點(diǎn)對(duì)應(yīng)的邊存在較大的誤差時(shí)，要想將此誤差分配到圖中其它的邊上，就需要進(jìn)行多次迭代才能實(shí)現(xiàn)，從而加大了計(jì)算量，同時(shí)而這也是出現(xiàn)環(huán)形閉合時(shí)所需要解決的問題。隨著算法的不斷改進(jìn)，F(xiàn)rese[12]提出一種改進(jìn)的多層次松弛（MLR）的優(yōu)化算法，同時(shí)結(jié)合多重網(wǎng)格方法來求解系統(tǒng)中的偏微分方程，從而在很大程度上提高了出現(xiàn)環(huán)形閉合時(shí)位姿圖中節(jié)點(diǎn)的優(yōu)化效率。

2.3基于隨機(jī)梯度下降的方法

Olson[10]將神經(jīng)網(wǎng)絡(luò)中的隨機(jī)梯度下降方法（stochastic gradient descent，SGD）應(yīng)用到基于圖優(yōu)化的SLAM中，在每次迭代的過程中，依次選取位姿圖中的任意一條邊，并將此條邊上對(duì)應(yīng)的約束作為當(dāng)前約束，計(jì)算下降的最快的方向即梯度，然后在該方向上求出最優(yōu)的目標(biāo)函數(shù)。通過相關(guān)的實(shí)驗(yàn)可以證明隨機(jī)梯度下降在執(zhí)行的過程中，初始值與最優(yōu)值的誤差超出了很大的閾值范圍，在不修改此時(shí)初始值的情況下，隨機(jī)梯度下降仍然能夠得到較好的收斂效果。因此，隨機(jī)梯度下降算法可以避免落入局部最優(yōu)值，對(duì)系統(tǒng)初始化的值具有較高的魯棒性。Grisetti對(duì)Olson提出的隨機(jī)梯度下降方法進(jìn)行了改進(jìn)和拓展，采用樹型結(jié)構(gòu)來描述二維和三維空間中機(jī)器人位姿之間的關(guān)系，系統(tǒng)待求解的狀態(tài)用增量方式來代替，不僅有效地更新了移動(dòng)機(jī)器人的位姿，而且加快了收斂的速率[13]。

3 結(jié)論

移動(dòng)機(jī)器人在未知環(huán)境下的同時(shí)定位與地圖創(chuàng)建，是機(jī)器人學(xué)和智能控制的重要研究領(lǐng)域，同時(shí)SLAM也是自主導(dǎo)航以及自動(dòng)完成其他指令的關(guān)鍵部分，因此近幾年來在機(jī)器人的研究領(lǐng)域，SLAM已成為研究的重點(diǎn)與熱點(diǎn)。與此同時(shí)，出現(xiàn)了分別受時(shí)間約束的貝葉斯濾波器算法以及受空間約束的平滑算法，前者主要是根據(jù)上一時(shí)刻機(jī)器人的狀態(tài)、觀測(cè)信息以及控制輸入信息來確定此刻機(jī)器人的位姿和地圖特征；后者則是根據(jù)全局的所有信息來構(gòu)建以及優(yōu)化地圖。

濾波器算法中需要改進(jìn)的地方：基于卡爾曼濾波器的基本算法以及改進(jìn)算法中，不能解決非強(qiáng)高斯和非強(qiáng)線性的模型，基于標(biāo)準(zhǔn)的粒子濾波器算法以及改進(jìn)算法存在著粒子退化，從而導(dǎo)致樣本匱乏的現(xiàn)象。

平滑算法技術(shù)中以下方面仍具有較大的發(fā)展趨勢(shì)：對(duì)SLAM問題非線性結(jié)構(gòu)的深入研究，增強(qiáng)后端的魯棒性以及地圖的表示和創(chuàng)建形式。

參考文獻(xiàn)：

[1] SIMTH R， SELF M， CHEESEMAN P. Estimating uncertain spatial realtion ships in robotics[C]// Autonomous Robot Vehicles， COX I， WILFONG G， Eds. Berlin： Springer-Verlag， 1990：167–193.

[2] CASTELLANOS J A， MONTIEL J M M， NEIRA J， et al. The SPmap： A probabilistic framework for simultaneous localization and map building[J]. IEEE Trans. Robot. Automat.， 1999，15（5）：948–953.

[3] MONTEMERLO M， THRUN S， KOLLER D， et al. FastSLA M： A factored solution to simultaneous localization and mapping[C]//Proc. Nat. Conf. Artificial Intelligence （AAAI）. Edmonton， Canada：AAAI， 2002：593–598.

[4] HAHNEL D， BURGARD W， FOX D， et al. An efficient FastSLAM algorithm for generating maps of large-scale cyclic environments from raw laser range measurements[C]//Proc. IEEE/RSJ Int. Conf. Intelligent Robots and Systems （IROS）. Las Vegas， NV：IEEE， 2003： 206–211.

[5] GRISETTI G， STACHNISS C， BURGARD W. Improved techniques for grid mapping with rao-blackwellized particle filters[J]. IEEE Trans.Robot.， 2007， 23（1）： 34–46.

[6] EUSTICE R， SINGH H， LEONARD J J. Exactly sparse delayed-state filters[C]// Proc. IEEE Int. Conf. Robotics and Automation （ICRA）， Barcelona， Spain：IEEE，2005： 2428–2435.

[7] THRUN S， LIU Y， KOLLER D， et al. Simultaneous localization and mapping with sparse extended information filters[J]. Int. J. Robot. Res.， 2004，23（ 7/8）：693–716.

[8] LU F， MILIOS E. Globally consistent range scan alignmen t for environment mapping[J].Autonom. Robots，1997， 4： 333–349.

[9] LAERT D F， KAESS M. Square root SAM： Simultaneous location and mapping via square root information smoothing[J]. Int. J. Robot. Res.， 2006.

[10] OLSON E， LEONARD J， TELLER S. Fast iterative optimization of pose graphs with poor initial estimates[C]// Proc. IEEE Int. Conf. Robotics and Automation （ICRA）.Oralando， Florida：IEEE， 2006： 2262–2269.

[11] HOWARD A， MATARIC' M J， SUKHATME G. Relaxation on a mesh： A formalism for generalized localization[C]// Proc. IEEE/RSJ Int. Conf. Intelligent Robots and Systems （IROS）. Los Alamitos，Calif：IEEE， 2001.

[12] FRESE U， LARSSON P， DUCKETT T. A multilevel relaxation algorithm for simultaneous localisation and mapping[J]. IEEE Trans. Robot.， 2005， 21（ 2）：1–12.

[13] GRISETTI G， STACKNISS C， BURGARD W. Non-linear constraint network optimization for efficient map learning[J]. IEEE Trans. Intell. Transport. Syst.， 2009.

作者簡(jiǎn)介：王芳（1984-），女，江蘇沭陽人，碩士，助工，主要研究方向：人工智能、無人機(jī)等；

劉晉（1987-），男，山西晉城人，本科，主要研究方向： 數(shù)學(xué)與應(yīng)用數(shù)學(xué)。