移動機(jī)器人同時(shí)定位與建圖研究綜述

張國榮 王志亮 趙振棟 王明濤 張心印

摘要：移動機(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)展，并對機(jī)器人SLAM未來研究進(jìn)行了展望。

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

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

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

0引言

機(jī)器人領(lǐng)域一個(gè)重要問題是實(shí)現(xiàn)機(jī)器人在陌生環(huán)境中自主定位、導(dǎo)航和理解環(huán)境信息，進(jìn)而實(shí)現(xiàn)機(jī)器人（機(jī)器人群）自主移動和信息交流。為實(shí)現(xiàn)機(jī)器人的自主定位和導(dǎo)航，目前大多數(shù)研究都是在機(jī)器人上安裝多種傳感器，如陀螺儀、GPS、激光測距和氣壓傳感器等進(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)化理論，由于其能通過閉環(huán)檢測減小系統(tǒng)累計(jì)誤差，提高定位和建圖精度，目前成為研究主流。

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

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

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

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

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

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

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

2圖優(yōu)化SLAn

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

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

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

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

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

3SLAn研究展望

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

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

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

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

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

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