張國(guó)榮 王志亮 趙振棟 王明濤 張心印

摘要：移動(dòng)機(jī)器人同時(shí)定位與地圖構(gòu)建（Simuhaneous Localization and Mapping，SLAM）是近年研究熱點(diǎn)。介紹了常用的卡爾曼濾波SLAM、圖優(yōu)化SLAM原理和基礎(chǔ)理論，闡述卡爾曼濾波SLAM、圖優(yōu)化SLAM關(guān)鍵技術(shù)及研究進(jìn)展，并對(duì)機(jī)器人SLAM未來(lái)研究進(jìn)行了展望。

關(guān)鍵詞：SLAM;卡爾曼濾波;圖優(yōu)化

DOI：10.11907/rjdk.191818開(kāi)放科學(xué)（資源服務(wù)）標(biāo)識(shí)碼（OSID）：

中圖分類(lèi)號(hào)：TP3-0文獻(xiàn)標(biāo)識(shí)碼：A 文章編號(hào)：1672-7800（2019）010-0005-03

0引言

機(jī)器人領(lǐng)域一個(gè)重要問(wèn)題是實(shí)現(xiàn)機(jī)器人在陌生環(huán)境中自主定位、導(dǎo)航和理解環(huán)境信息，進(jìn)而實(shí)現(xiàn)機(jī)器人（機(jī)器人群）自主移動(dòng)和信息交流。為實(shí)現(xiàn)機(jī)器人的自主定位和導(dǎo)航，目前大多數(shù)研究都是在機(jī)器人上安裝多種傳感器，如陀螺儀、GPS、激光測(cè)距和氣壓傳感器等進(jìn)行機(jī)器人自身定位，安裝單目相機(jī)、雙目相機(jī)、深度相機(jī)、激光雷達(dá)和聲吶等實(shí)現(xiàn)機(jī)器人的環(huán)境感知、地圖構(gòu)建與定位。SLAM技術(shù)不斷發(fā)展，其中以卡爾曼濾波SLAM和圖優(yōu)化理論最為經(jīng)典，早期都是采用卡爾曼濾波（Kalman filter，KF）為原理的機(jī)器人進(jìn)行研究;2007年后提出圖優(yōu)化理論，由于其能通過(guò)閉環(huán)檢測(cè)減小系統(tǒng)累計(jì)誤差，提高定位和建圖精度，目前成為研究主流。

1基于卡爾曼濾波的概率SLAn

卡爾曼濾波理論在機(jī)器人導(dǎo)航、傳感器數(shù)據(jù)融合、雷達(dá)目標(biāo)跟蹤和控制等領(lǐng)域應(yīng)用已有幾十年?？柭鼮V波SLAM核心思想是遞推求解，分為觀測(cè)、預(yù)測(cè)和更新3個(gè)步驟。下面介紹卡爾曼濾波SLAM的濾波和定位導(dǎo)航過(guò)程。

如圖1所示，設(shè)機(jī)器人初始位置為B點(diǎn)，定位不確定區(qū)域概率為橢圓B;當(dāng)機(jī)器人從B移動(dòng)x_i位置時(shí)，橢圓x_i區(qū)域?yàn)槎ㄎ徊淮_定的概率區(qū)域。隨著機(jī)器人的移動(dòng)，傳感器誤差累積造成的位置不確定區(qū)域增加，這時(shí)就需要另外的傳感器提供觀測(cè)z_i以減小誤差，此時(shí)的概率為條件概率P（Z_i|X_i），由此得到機(jī)器人預(yù)測(cè)概率P（X_i）和觀測(cè)概率P（Z_i|X_i，B）。

通過(guò)全概率公式和貝葉斯公式得到式（1）。

（5）為濾波后的數(shù)據(jù)融合。

卡爾曼增益嚴(yán)格依賴于方差大小比較，方差越大信任概率越小，方差越小信任概率越大。許多學(xué)者對(duì)卡爾曼濾波進(jìn)行改進(jìn)，其中擴(kuò)展卡爾曼濾波（Extended Kalman Filter，EKF）是將卡爾曼濾波放進(jìn)非線性領(lǐng)域，通過(guò)泰勒級(jí)數(shù)展開(kāi)式對(duì)非線性函數(shù)展開(kāi)，濾除二階以上的高階項(xiàng)以達(dá)到線性化目的。粒子濾波是卡爾曼在機(jī)器人SLAM中的具體應(yīng)用。由于粒子濾波的非線性和處理多峰分布的特點(diǎn)，決定它比擴(kuò)展卡爾曼應(yīng)用更廣，但粒子濾波需要足夠的粒子樣本才能更確切地近似后驗(yàn)概率密度。每個(gè)粒子都會(huì)占用較大內(nèi)存，因此會(huì)造成算法內(nèi)存占用過(guò)大;粒子濾波的另一個(gè)問(wèn)題是粒子耗散，即隨著重采樣增加，粒子多樣性會(huì)耗散掉。針對(duì)粒子濾波的粒子數(shù)量和粒子耗散問(wèn)題，學(xué)者采用proposal分布保持粒子數(shù)量在一個(gè)較小的值，并選擇采樣Fast-SLAM。

基于濾波的概率SLAM中，傳感器非線性分布噪聲隨時(shí)間增加非線性誤差累計(jì)較大，對(duì)此學(xué)者提出一些改進(jìn)方法，如羅元等提出改進(jìn)的Rao-Blackwellized算法等，這些算法在一定程度上提高了機(jī)器人應(yīng)用場(chǎng)景大小和算法速度，但是沒(méi)有從根本上解決大范圍場(chǎng)景定位精度問(wèn)題，更重要的是基于卡爾曼濾波的概率SLAM無(wú)法進(jìn)行全局優(yōu)化，因此出現(xiàn)了基于圖優(yōu)化理論的SLAM。

2圖優(yōu)化SLAn

基于概率的SLAM都是局部遞推式優(yōu)化，無(wú)法進(jìn)行閉環(huán)回路檢測(cè)?；丨h(huán)檢測(cè)保證SLAM能夠有效抑制累計(jì)誤差，機(jī)器人通過(guò)回溯關(guān)鍵幀進(jìn)行回環(huán)檢測(cè)以減小全局累計(jì)誤差。圖2是機(jī)器人移動(dòng)中的建圖過(guò)程：機(jī)器人在移動(dòng)中通過(guò)當(dāng)前t_i時(shí)刻的x_i位置運(yùn)動(dòng)，預(yù)計(jì)機(jī)器人t_i時(shí)刻的x_j位置和相同特征點(diǎn)在x₂位置時(shí)，以及當(dāng)相機(jī)運(yùn)動(dòng)到t_j時(shí)刻x_j位置時(shí)，觀測(cè)到同一特征點(diǎn)位置與預(yù)測(cè)的差值應(yīng)該為零。其中，Z_ij為x_i與x_j位置觀測(cè)到的同一特征點(diǎn)，Ω為觀測(cè)噪聲。

上述過(guò)程為圖優(yōu)化SLAM原理。圖優(yōu)化SLAM相繼發(fā)展出SLAM++和iSAME等。圖優(yōu)化解決SLAM問(wèn)題時(shí)需要對(duì)SLAM進(jìn)行建模，常用的建模方法有動(dòng)態(tài)貝葉斯網(wǎng)絡(luò)、因子圖、馬爾科夫等。不同的建模方法對(duì)后期矩陣的稀疏性和非線性優(yōu)化產(chǎn)生不同影響。動(dòng)態(tài)貝葉斯建模時(shí)變量較多，因子圖建模和馬爾科夫建模隱藏變量較多。

關(guān)鍵幀是通過(guò)計(jì)算機(jī)器人移動(dòng)過(guò)程中拍攝照片之間的歐式空間距離決定生成數(shù)據(jù)庫(kù)，它包含了機(jī)器人的位姿和特征點(diǎn)。因?yàn)樘卣鞯拇嬖冢箼C(jī)器人可以在移動(dòng)過(guò)程中通過(guò)新、舊關(guān)鍵幀對(duì)比特征點(diǎn)進(jìn)行回路檢測(cè)。若機(jī)器人檢測(cè)到之前到過(guò)該位置，則向約束中添加位姿約束。但這樣帶來(lái)的問(wèn)題是隨著時(shí)間的推移，機(jī)器人建圖規(guī)模會(huì)越來(lái)越大，如城市運(yùn)用的清潔機(jī)器人和大場(chǎng)景運(yùn)用的服務(wù)機(jī)器人等，不僅需要比較的關(guān)鍵幀數(shù)量會(huì)呈線性無(wú)限增長(zhǎng)，而且系統(tǒng)計(jì)算量會(huì)變得異常龐大。針對(duì)降低因子圖復(fù)雜性的優(yōu)化主要有稀疏化方法和多機(jī)器人（或多處理器）并行計(jì)算。

稀疏化通過(guò)向因子圖中添加或減少節(jié)點(diǎn)解決問(wèn)題。Johannsson通過(guò)在現(xiàn)有節(jié)點(diǎn)之間引入新約束避免向圖中添加新節(jié)點(diǎn)，使變量的數(shù)量?jī)H隨探索空間大小增長(zhǎng)，而不是隨著映射持續(xù)時(shí)間而增長(zhǎng);郭潤(rùn)提出一種基于熵稀疏規(guī)則的改進(jìn)SLAM算法，該算法利用熵性質(zhì)、綜合當(dāng)前以及下一觀測(cè)時(shí)刻選擇與位姿關(guān)聯(lián)性最弱的環(huán)境特征作為稀疏特征點(diǎn);Mazuran分別介紹了通用線性約束（GLC）因子和非線性圖形稀疏化（NGS）方法;Tong將軌跡的參數(shù)化從基礎(chǔ)曲線改變?yōu)楦咚惯^(guò)程，其中因子圖中的節(jié)點(diǎn)是實(shí)際的機(jī)器人姿勢(shì)，并可通過(guò)計(jì)算給定時(shí)間的后驗(yàn)平均值來(lái)插值任何其它姿勢(shì);Barfoot提出了一個(gè)具有精確稀疏逆核的高斯過(guò)程，大大減少了批處理解的計(jì)算時(shí)間。

多機(jī)器人或多處理器并行分配計(jì)算思想是：把因子圖劃分為不同的子圖，通過(guò)局部和全局優(yōu)化整個(gè)過(guò)程。朱福利等嘗試使用并行計(jì)算處理大規(guī)模建圖時(shí)的計(jì)算問(wèn)題;Strasdat采用兩階段方法，首先優(yōu)化局部姿勢(shì)特征圖，然后優(yōu)化姿勢(shì)圖;Williams等提出高頻濾波器中的分離因子圖優(yōu)化和低頻平滑器。

3SLAn研究展望

（1）多機(jī)器人協(xié)同建圖。多機(jī)器人同時(shí)建圖，并保證尺度的一致性和地圖有效拼接，形成機(jī)器人群的定位建圖信息共享。

（2）地圖語(yǔ)義表達(dá)。人在導(dǎo)航時(shí)除預(yù)測(cè)與觀測(cè)外，還有對(duì)地圖的顯著特征（ID）表達(dá)，如人在去某地導(dǎo)航時(shí)，能很好地理解和表達(dá)地圖;機(jī)器人在運(yùn)動(dòng)過(guò)程中無(wú)法形成自己理解的語(yǔ)義地圖，并對(duì)語(yǔ)義進(jìn)行表達(dá)。有學(xué)者提出Bag-words方法。Bag-words能很好地壓縮圖片信息，但是丟棄了特征的空間位置，因此如何更好地壓縮信息，形成更好的語(yǔ)義是研究熱點(diǎn)。

（3）大場(chǎng)景SLAM數(shù)據(jù)處理。當(dāng)機(jī)器人面臨的不是狹小室內(nèi)環(huán)境，而是一座城市或一個(gè)國(guó)家時(shí)，計(jì)算機(jī)要存儲(chǔ)的數(shù)據(jù)量是巨大的。怎么管理、壓縮和回溯數(shù)據(jù)是SLAM未來(lái)研究的關(guān)鍵問(wèn)題之一。

（4）動(dòng)態(tài)場(chǎng)景下的SLAM。當(dāng)前SLAM研究是假設(shè)機(jī)器人處于靜態(tài)（準(zhǔn)靜態(tài)）環(huán)境中，周?chē)奈矬w為剛體。這種假設(shè)只適合短期動(dòng)態(tài)，不適合劇烈變化的環(huán)境。機(jī)器人在長(zhǎng)時(shí)間運(yùn)行過(guò)程中環(huán)境會(huì)發(fā)生變化，因此如何自我調(diào)節(jié)異常值，考慮特征退化避免系統(tǒng)故障，不同環(huán)境選擇不同的參數(shù)是研究的關(guān)鍵問(wèn)題之一。

（5）深度學(xué)習(xí)與視覺(jué)SLAM。視覺(jué)SLAM系統(tǒng)可以很好地采集和儲(chǔ)存圖像，但缺乏對(duì)圖像的深度分析、識(shí)別和挖掘。有了深度學(xué)習(xí)，視覺(jué)SLAM系統(tǒng)就可以進(jìn)行物體識(shí)別，使機(jī)器人更好地理解世界，進(jìn)一步從路徑規(guī)劃拓展到更廣領(lǐng)域進(jìn)行任務(wù)規(guī)劃，然后根據(jù)不同的任務(wù)選擇不同的傳感器和執(zhí)行器去完成任務(wù)。但深度學(xué)習(xí)都是建立在高性能計(jì)算機(jī)和GPU上進(jìn)行大數(shù)據(jù)和長(zhǎng)時(shí)間的訓(xùn)練，視覺(jué)SLAM應(yīng)用深度學(xué)習(xí)理論面臨的首要問(wèn)題是：如何在嵌入式系統(tǒng)中提供足夠的計(jì)算能力或小樣本推進(jìn)深度學(xué)習(xí)算法的進(jìn)步。計(jì)算能力問(wèn)題只能依賴于計(jì)算機(jī)硬件的進(jìn)步或更好的網(wǎng)絡(luò)提供高性能的分布式云計(jì)算來(lái)解決，但視覺(jué)SLAM算法如何把估計(jì)、感知和人工智能（控制、導(dǎo)航和深度學(xué)習(xí)）等有機(jī)結(jié)合，實(shí)現(xiàn)良好的任務(wù)規(guī)劃值得深入研究。

（6）機(jī)器人SLAM硬件可靠性。硬件可靠性主要體現(xiàn)在長(zhǎng)時(shí)間運(yùn)行過(guò)程中故障率和噪聲模型的變化，這些硬件故障和噪聲模型的變化在無(wú)人駕駛領(lǐng)域都可能是致命的。因此，機(jī)器人要能在運(yùn)轉(zhuǎn)過(guò)中識(shí)別CPU、GPU、內(nèi)存等硬件的消耗，調(diào)整自身資源消耗，并能識(shí)別傳感器老化和噪聲模型的變化，進(jìn)行動(dòng)態(tài)參數(shù)調(diào)整和預(yù)判故障，這是SLAM研究領(lǐng)域的一個(gè)關(guān)鍵性問(wèn)題。