移動機(jī)器人SLaM方法及其發(fā)展前景*

姜鵬 陶肖 戴立根

1.復(fù)雜環(huán)境特種機(jī)器人控制技術(shù)與裝備湖南省工程研究中心；2.湖南理工職業(yè)技術(shù)學(xué)院；3.OPPO廣東移動通信有限公司

移動機(jī)器人的同步定位與制圖（SLAM）問題經(jīng)過幾十年的研究已經(jīng)得到了很大的發(fā)展。近年來隨著計(jì)算機(jī)技術(shù)、人工智能技術(shù)的不斷發(fā)展，移動機(jī)器人SLAM問題出現(xiàn)了新的發(fā)展方向。精度更高、價(jià)格更加低廉的傳感器用于SLAM問題，多傳感融合SLAM方案在蓬勃發(fā)展。本文對移動機(jī)器人SLAM問題的基本方法原理及關(guān)鍵技術(shù)進(jìn)行梳理，并對SLAM未來發(fā)展的趨勢與關(guān)鍵性的問題進(jìn)行了討論。

0 引言

移動機(jī)器人的同時定位與制圖（SLAM，Simultaneous Localization and Mapping）被認(rèn)為是機(jī)器人能否實(shí)現(xiàn)自主功能的關(guān)鍵性問題。大多數(shù)情況下，GPS信號的精度不能達(dá)到控制的要求，甚至在某些場景如礦井、地下室等GPS信號及其微弱。因此，依靠移動機(jī)器人自身實(shí)現(xiàn)定位及制圖功能是其能夠?qū)崿F(xiàn)自主功能的必備前提功能。本文在介紹SLAM方法的分類、基本框架、原理的同時，分析探討SLAM研究的最新技術(shù)、發(fā)展前景以及亟待解決的一些關(guān)鍵性問題。

1 SLAM技術(shù)概述

按照SLAM[1]問題求解方式的不同，可將SLAM方法分為兩大類，一類為濾波（Filtering）[2]的方式，另一類為圖優(yōu)化（Graph Optimization）[2]的方式。SLAM問題的提出SLAM研究的首次提出源于Smith[3]年的一系列論文。Smith將機(jī)器人的運(yùn)動軌跡描述定位過程，將傳感器觀測的路標(biāo)集合點(diǎn)描述環(huán)境地圖，從而將同時定位與建圖引申為擴(kuò)展卡爾曼濾波器（Extended Kalman Filter，EKF）[4]估計(jì)問題。如圖1所示，用白色的三角形表示機(jī)器人在運(yùn)動過程中的實(shí)際位姿，灰色的三角形表示機(jī)器人估計(jì)的軌跡位姿，白色的圓圈表示在環(huán)境當(dāng)中觀測路標(biāo)的實(shí)際位置，灰色的圓圈表示在環(huán)境中觀測路標(biāo)的估計(jì)位置，黑色的箭頭表示機(jī)器人傳感器觀測的方向，實(shí)線表示機(jī)器人的實(shí)際運(yùn)動軌跡，虛線則表示相應(yīng)的估計(jì)運(yùn)動軌跡。

圖1 SLAM問題示意圖Fig.1 Schematic diagram of SLAM problem

SLAM問題實(shí)則是在0∶t時刻，移動機(jī)器人在未知的環(huán)境中進(jìn)行運(yùn)動，已知各時刻機(jī)器人的輸入量ut以及傳感器觀測環(huán)境數(shù)據(jù)的觀測量Zt求解各個時刻的機(jī)器人位姿Xt以及各個時刻的環(huán)境路標(biāo)mt的位置向量，所有時刻的mt集合即構(gòu)成環(huán)境地圖。圖中，黑灰不同的三角形以及虛線代表著因?yàn)閭鞲衅饔^測的數(shù)據(jù)與是數(shù)據(jù)之間存在誤差，導(dǎo)致估計(jì)的Xt與mt與實(shí)際的不可避免的會存在誤差，因此從另一個角度而言，SLAM問題又是一個求解最優(yōu)解的過程。濾波的SLAM方法[4]對于t時刻的Xt、mt的求解依靠于t-1時刻的機(jī)器人位姿與地圖的先驗(yàn)概率估計(jì)而與之前很多時刻機(jī)器人的位姿與地圖無關(guān)，即之前的數(shù)據(jù)一經(jīng)使用即被丟棄。與濾波方法的不同，圖優(yōu)化方法涉及對整個時間段的機(jī)器人位姿X1∶t以及地圖m的后驗(yàn)概率估計(jì)。以下將對兩類方法的基本原理及主流方法進(jìn)行詳細(xì)的介紹。

2 濾波SLAM方法

基于濾波的SLAM方法又稱之為在線SLAM方法。這類方法不需要對傳感器之前的觀測數(shù)據(jù)Zt及控制數(shù)據(jù)Ut進(jìn)行保存，機(jī)器人下一時刻的位姿狀態(tài)只與當(dāng)前時刻的控制量Ut及前一時刻的機(jī)器人位姿狀態(tài)有關(guān)。利用貝葉斯估計(jì)原理，可表示為式（1）。

2.1 卡爾曼濾波及擴(kuò)展卡爾曼濾波SLAM方法

早期的SLAM方法均為基于濾波的SLAM方法，最早的為卡爾曼濾波的方法?？柭鼮V波由Kalman[5]提出的一種線性的最優(yōu)的狀態(tài)估計(jì)方法。該算法認(rèn)為機(jī)器人當(dāng)前時刻的狀態(tài)與前一時刻的狀態(tài)是線性的關(guān)系，滿足式（2）。

式中At為無控制量時系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣，Bt為存在控制量時的狀態(tài)轉(zhuǎn)移矩陣，wt為噪聲。該算法還認(rèn)為傳感器不同時刻的觀測量也是線性的，滿足式（3）。

式中Ct為觀測矩陣，vt為噪聲。同時該算法認(rèn)為vt、wt均為高斯噪聲。該算法實(shí)質(zhì)上是一個加權(quán)的自適應(yīng)算法，通過觀測量噪聲的方差及控制量噪聲的方差求解出卡爾曼增益矩陣，從而對機(jī)器人的狀態(tài)進(jìn)行更新。然而，實(shí)際的機(jī)器人狀態(tài)關(guān)系與觀測量的關(guān)系并不總是滿足線性的關(guān)系，因此在非線性的狀態(tài)卡爾曼濾波算法將失效。為解決非線性情況下卡爾曼濾波算法失效的問題擴(kuò)展卡爾曼濾波算法被提出，即EKF-SLAM，并取得了成功。擴(kuò)展卡爾曼濾波算法其關(guān)鍵就是將式（2）、式（3）中非線性關(guān)系利用泰勒公式進(jìn)行展開，同時舍棄掉二階及以上的高階項(xiàng)，從而對非線性關(guān)系進(jìn)行線性化。擴(kuò)展卡爾曼濾波雖在一定程度上取得了成功但在計(jì)算效率、自身線性化的誤差等方面的問題是比較突出的，為了獲得比較準(zhǔn)確的機(jī)器人狀態(tài)轉(zhuǎn)移矩陣及傳感器觀測矩陣，環(huán)境信息特征的提取與匹配是該算法中的另一個關(guān)鍵性問題。

2.2 粒子濾波SLAM方法

為解決擴(kuò)展卡爾曼濾波的非線性不足問題，粒子濾波的SLAM[6]算法被提出，最早的為FastSlam[7]算法。粒子濾波算法將機(jī)器人位姿與地圖后驗(yàn)聯(lián)合概率密度函數(shù)分解成機(jī)器人位姿密度函數(shù)與地圖后驗(yàn)概率密度函數(shù)的乘積。如式（4）。

由上式，與卡爾曼濾波SLAM算法及EKF-SLAM算法對機(jī)器人位姿軌跡與環(huán)境地圖同時進(jìn)行估算不同，粒子濾波算法先對機(jī)器人的運(yùn)動軌跡進(jìn)行估計(jì)，然后利用已知軌跡對地圖進(jìn)行計(jì)算。其基本原理是對任意未知的概率分布函數(shù)，可以用一系列的帶權(quán)重的粒子來表示，理論上來講當(dāng)采樣粒子無限多時其誤差可以接近無限小。最早的粒子濾波算法為FastSLAM，其先將式（4）中的機(jī)器人軌跡概率密度函數(shù)用一群粒子來表示，每一個粒子都是對機(jī)器人移動軌跡的一個采樣。而后根據(jù)傳感器數(shù)據(jù)對粒子群里粒子權(quán)重進(jìn)行計(jì)算更新，隨后對機(jī)器人軌跡進(jìn)行重新采樣以更新權(quán)重，最后對每個粒子的地圖信息進(jìn)行更新計(jì)算，完成整個過程。目前比較流行的有SIR[8]粒子濾波算法等。粒子濾波算法比較突出的問題就是其對內(nèi)存的要求，由于粒子濾波算法的效果直接由粒子的數(shù)量影響，因此大多數(shù)情況下會要求采集數(shù)量較多的粒子，而每一個粒子是機(jī)器人軌跡及地圖的采樣，因此對計(jì)算機(jī)的內(nèi)存要求較高。

3 圖優(yōu)化SLAM方法

圖優(yōu)化方法把SLAM問題看作一個優(yōu)化問題。 濾波的SLAM方法一定程度上對于數(shù)據(jù)的處理都是線性化的處理，而實(shí)際的SLAM問題很大程度上都是非線性的，且以濾波的SLAM方法只利用機(jī)器人前一狀態(tài)的值來估計(jì)當(dāng)前狀態(tài)的值很難做到全局的優(yōu)化。與濾波的SLAM方法不同圖優(yōu)化的SLAM方法是非線性的優(yōu)化方法。圖優(yōu)化方法保存之前每一時刻的機(jī)器人狀態(tài)值，當(dāng)前的值先與之前保存的值一起優(yōu)化后，所有數(shù)據(jù)再放在一起進(jìn)行優(yōu)化。故此，圖優(yōu)化的方法一般分為優(yōu)化前端和后端。圖優(yōu)化方法總體思路是根據(jù)移動機(jī)器根據(jù)傳感器觀測數(shù)據(jù)對機(jī)器人的軌跡位姿進(jìn)行最大后驗(yàn)估計(jì)。一般來講，其過程可如下描述:首先，系統(tǒng)對機(jī)器人的位姿、地圖信息進(jìn)行預(yù)估取一個值，然后根據(jù)傳感器的觀測值、運(yùn)動模型以及機(jī)器人控制量得到測量值。機(jī)器人位姿數(shù)據(jù)預(yù)估值與測量值以及地圖信息估計(jì)值與測量值之間不可避免將存在誤差。我們將各時刻機(jī)器人位姿信息誤差記為，將地圖信息誤差記為。圖優(yōu)化方法初始目標(biāo)可記為求解式（5）的最小值?？紤]到誤差有正負(fù)之分因此將每個誤差值作平方處理。如式（5）。

利用優(yōu)化學(xué)求上式的梯度及二階梯度矩陣?yán)锰荻认陆捣ㄇ蠼庾顑?yōu)值。如式（6）。

以視覺SLAM圖優(yōu)化方法為例一次SLAM過程中數(shù)據(jù)幀可能有上萬個，作上式（6）處理其優(yōu)化變量將以成百萬增長，計(jì)算量是非常巨大的，因此圖優(yōu)化方法初期認(rèn)為在移動機(jī)器人本體上是不可實(shí)現(xiàn)的。隨著研究的深入，發(fā)現(xiàn)J和H兩個矩陣具有“稀疏性”，因此用稀疏代數(shù)方法其求解在移動機(jī)器人身上成為可能，圖優(yōu)化方法得以發(fā)展。當(dāng)前圖優(yōu)化方法是移動機(jī)器人SLAM問題比較流行的方法。其中視覺SLAM圖優(yōu)化方法較多學(xué)者對其進(jìn)行研究。

4 SLAM方法的發(fā)展前景及問題

當(dāng)前SLAM相關(guān)理論已經(jīng)發(fā)展的比較的成熟，由于實(shí)際環(huán)境的復(fù)雜性在實(shí)際應(yīng)用方面仍舊存在很多問題?；谝曈X的SLAM方法在實(shí)際應(yīng)用中較為普遍，但是當(dāng)前基于視覺的SLAM與三維地圖重建難以落地與產(chǎn)品化。同時，視覺傳感器相較于IMU[9]、高分辨率的激光雷達(dá)以及差分GPS[7]在精度上有所缺陷。與此同時，隨著科技的不斷發(fā)展高精度的激光雷達(dá)、IMU及差分GPS等以前較為昂貴的傳感器變得越來越便宜。因此當(dāng)前多傳感器融合的SLAM方案成為趨勢，比如將高分辨率的激光雷達(dá)與差分GPS相機(jī)和的SLAM系統(tǒng)再輔以視覺作為補(bǔ)充在無人駕駛問題上得到應(yīng)用。因此隨著科技水平與計(jì)算機(jī)技術(shù)的不斷進(jìn)步，新的更好的傳感器用于SLAM問題將是SLAM發(fā)展的一個重要方面。

隨著大數(shù)據(jù)云計(jì)算的及計(jì)算機(jī)技術(shù)的不斷發(fā)展，深度學(xué)習(xí)在各個科研領(lǐng)域得到廣泛的應(yīng)用。同樣的，SLAM領(lǐng)域也有著深度學(xué)習(xí)的影子。研究這們嘗試使用深度學(xué)習(xí)代替SLAM過程中的某一模塊，如機(jī)器人位姿估計(jì)、圖像特征提取與匹配等。如在文獻(xiàn)[3]中使用神經(jīng)網(wǎng)絡(luò)解決視覺SLAM過程中地圖點(diǎn)深度的不確定性問題，使得視覺SLAM幀間匹配準(zhǔn)確率提高，提高系統(tǒng)魯棒性。因此SLAM發(fā)展的另一個趨勢就是與深度學(xué)習(xí)相結(jié)合。

5 結(jié)語

移動機(jī)器人SLAM問題經(jīng)過幾十年的發(fā)展研究，已開始逐步應(yīng)用于生產(chǎn)生活。如掃地機(jī)器人、服務(wù)機(jī)器人、無人駕駛等領(lǐng)域。而然由于實(shí)際環(huán)境的復(fù)雜性與動態(tài)性，其在生產(chǎn)生活中的應(yīng)用還存在諸多的問題。在實(shí)際生產(chǎn)生活環(huán)境下其魯棒性、準(zhǔn)確性都面臨著很大的挑戰(zhàn)。隨著自動化程度的進(jìn)一步提高，機(jī)器人實(shí)現(xiàn)未知環(huán)境下的自主移動必將會愈發(fā)重要，因此SLAM問題也將愈發(fā)受到更多科研人員的關(guān)注與研究。