大規(guī)模環(huán)境下基于圖優(yōu)化SLAM的后端優(yōu)化方法

王忠立，趙 杰，蔡鶴皋

(1.北京交通大學(xué)電子信息工程學(xué)院，100044北京;2.機(jī)器人技術(shù)與系統(tǒng)國(guó)家重點(diǎn)實(shí)驗(yàn)室(哈爾濱工業(yè)大學(xué))，150080哈爾濱)

隨著移動(dòng)機(jī)器人的應(yīng)用逐步從小規(guī)模、靜態(tài)環(huán)境向大規(guī)模環(huán)境發(fā)展，基于圖優(yōu)化的SLAM方法成為研究熱點(diǎn).從實(shí)現(xiàn)框架上，可以將SLAM分解為前端的圖構(gòu)建和后端的圖優(yōu)化兩個(gè)過(guò)程.相對(duì)于早期的基于遞歸貝葉斯?fàn)顟B(tài)估計(jì)理論方法，如基于Kalman濾波和基于粒子濾波(PF)的方法，圖優(yōu)化SLAM方法是先通過(guò)前端的圖構(gòu)建過(guò)程得到初始狀態(tài)估計(jì)，然后在后端對(duì)初始狀態(tài)估計(jì)進(jìn)一步優(yōu)化求解，因此地圖的一致性和精度更好.基于圖優(yōu)化的SLAM方法是一種batch方法.在文獻(xiàn)［1］中，詳細(xì)介紹了圖優(yōu)化建模方法及前端的數(shù)據(jù)關(guān)聯(lián)和環(huán)形閉合檢測(cè)方法.本文主要對(duì)基于圖優(yōu)化SLAM的后端優(yōu)化方法進(jìn)行總結(jié).

在前端圖構(gòu)建得到的“初始圖”中，由于傳感器的噪聲、系統(tǒng)參數(shù)的不確定性、環(huán)形閉合檢測(cè)的誤差乃至錯(cuò)誤等因素，使得圖優(yōu)化過(guò)程具有很大的挑戰(zhàn)性.不僅要求優(yōu)化方法具有較好的魯棒性、穩(wěn)定性，同時(shí)，優(yōu)化過(guò)程通常是對(duì)整個(gè)地圖進(jìn)行的，在大規(guī)模環(huán)境下，也要考慮優(yōu)化方法的計(jì)算復(fù)雜度問(wèn)題.國(guó)內(nèi)外學(xué)者提出了各種優(yōu)化計(jì)算方法，主要包括:基于最小二乘法的優(yōu)化方法，基于松弛迭代的優(yōu)化方法，基于隨機(jī)梯度下降的優(yōu)化方法，以及基于流形的優(yōu)化方法等.并對(duì)基于圖優(yōu)化SLAM的計(jì)算效率、魯棒性和可擴(kuò)展性等方面展開了研究.但對(duì)算法的性能及地圖重建結(jié)果評(píng)估的研究相對(duì)較少.針對(duì)大規(guī)模環(huán)境下的地圖創(chuàng)建，地圖創(chuàng)建質(zhì)量的評(píng)估也是非常重要的一環(huán).為此，本文對(duì)兩種地圖質(zhì)量評(píng)價(jià)方法進(jìn)行了總結(jié)，對(duì)優(yōu)化方法存在的挑戰(zhàn)性進(jìn)行了闡述，在文獻(xiàn)［2］基礎(chǔ)上，介紹了最新的研究進(jìn)展，并對(duì)發(fā)展趨勢(shì)進(jìn)行了展望.

1 基于圖優(yōu)化SLAM的后端優(yōu)化方法

1.1 優(yōu)化方法概述

基于圖優(yōu)化的SLAM方法，是利用圖模型對(duì)SLAM問(wèn)題進(jìn)行建模.模型中的節(jié)點(diǎn)對(duì)應(yīng)不同時(shí)刻機(jī)器人及環(huán)境組成系統(tǒng)的狀態(tài)，邊則描述了系統(tǒng)狀態(tài)(節(jié)點(diǎn))之間的約束關(guān)系［3］.這類方法將SLAM問(wèn)題劃分為前端(front-end)和后端(backend)兩個(gè)部分，如圖1所示.前端完成圖的構(gòu)建，即根據(jù)觀測(cè)和系統(tǒng)約束構(gòu)建圖的節(jié)點(diǎn)和邊;后端主要完成圖的優(yōu)化.基于圖優(yōu)化的方法利用所有的觀測(cè)信息來(lái)優(yōu)化估計(jì)機(jī)器人完整的運(yùn)動(dòng)軌跡，因此也稱為全SLAM方法.

圖1 基于圖優(yōu)化的SLAM框架

在前端部分，順序數(shù)據(jù)關(guān)聯(lián)是指相鄰觀測(cè)數(shù)據(jù)幀間的匹配及相對(duì)姿態(tài)估計(jì)問(wèn)題，而環(huán)形閉合檢測(cè)則根據(jù)觀測(cè)數(shù)據(jù)判斷機(jī)器人是否處在之前已訪問(wèn)過(guò)的環(huán)境中.二者的核心是要解決數(shù)據(jù)關(guān)聯(lián)問(wèn)題，前者考慮局部數(shù)據(jù)關(guān)聯(lián)，而后者則涉及全局?jǐn)?shù)據(jù)關(guān)聯(lián).順序數(shù)據(jù)關(guān)聯(lián)與環(huán)形閉合檢測(cè)都是根據(jù)觀測(cè)信息建立圖節(jié)點(diǎn)間的約束，即完成圖的構(gòu)建，是基于圖優(yōu)化方法前端的兩個(gè)核心部分.

由于觀測(cè)噪聲以及數(shù)據(jù)關(guān)聯(lián)誤差的存在，前端得到的圖往往存在不一致性.若用Ti來(lái)表示數(shù)據(jù)幀間的相對(duì)變換矩陣，而且T0，T1，…，Tn構(gòu)成一個(gè)閉環(huán)，則理想情況下，應(yīng)滿足:

式中Ⅰ為單位矩陣.但通過(guò)觀測(cè)信息關(guān)聯(lián)得到的相對(duì)變換矩陣通常不滿足該約束.在基于圖的模型描述中，機(jī)器人的位姿是待估計(jì)的隨機(jī)變量，位姿間的約束則是與隨機(jī)變量相關(guān)的觀測(cè)，圖優(yōu)化結(jié)果對(duì)應(yīng)于位姿的最大似然估計(jì).與順序數(shù)據(jù)關(guān)聯(lián)及環(huán)形閉合檢測(cè)不同，圖優(yōu)化部分一般不直接處理觀測(cè)數(shù)據(jù)，而是在前端構(gòu)造的圖基礎(chǔ)上進(jìn)行優(yōu)化運(yùn)算.

Golfarelli等［4］將圖優(yōu)化視為質(zhì)量-彈簧模型，從另一個(gè)視角來(lái)解釋圖的優(yōu)化問(wèn)題，如圖2所示.在該模型中，將機(jī)器人的位姿看作是帶質(zhì)量的節(jié)點(diǎn)(黑色圓點(diǎn))，而約束則看作是連接這些節(jié)點(diǎn)的彈簧.由于每個(gè)約束都是根據(jù)與之相關(guān)的觀測(cè)獨(dú)立求解的，它們之間存在不一致性，此時(shí)彈簧處在受力形變狀態(tài)，這樣的物理系統(tǒng)通常并不穩(wěn)定.當(dāng)彈簧對(duì)質(zhì)點(diǎn)的作用力使系統(tǒng)重新達(dá)到平衡時(shí)，系統(tǒng)處在能量最小狀態(tài)，此時(shí)質(zhì)點(diǎn)的分布即代表最優(yōu)的位姿序列.彈簧的系數(shù)通過(guò)觀測(cè)的不確定性來(lái)(協(xié)方差)表示.觀測(cè)的不確定性越小，彈簧的強(qiáng)度越大，使其形變需要的外部作用力也越大;反之，觀測(cè)的不確定性越大，彈簧的強(qiáng)度越小，使其形變需要的外部作用力也就越小.系統(tǒng)的能量最小狀態(tài)對(duì)應(yīng)于非線性最小二乘問(wèn)題的最優(yōu)解.

圖2 基于圖優(yōu)化的mass-spring模型

基于圖優(yōu)化SLAM的后端優(yōu)化方法，概括起來(lái)可以分為基于最小二乘法的優(yōu)化方法，基于松弛迭代的優(yōu)化方法，基于隨機(jī)梯度下降的優(yōu)化方法，以及基于流形的優(yōu)化方法等.

1.2 基于最小二乘法的優(yōu)化方法

SLAM可以看作是一個(gè)非線性最小二乘問(wèn)題［5］.基于最小二乘的優(yōu)化方法是通過(guò)對(duì)目標(biāo)函數(shù)的一階泰勒展開對(duì)系統(tǒng)進(jìn)行線性化，采用迭代法求解線性系統(tǒng)的解，如 Gauss-Newton方法、Levenberg-Marquardt方法等.如果不考慮SLAM問(wèn)題的稀疏結(jié)構(gòu)特性并假定圖中的節(jié)點(diǎn)數(shù)為n，則Levenberg-Marquardt算法的時(shí)間復(fù)雜度為O(n3)［6］，在實(shí)際問(wèn)題求解中遠(yuǎn)不能滿足實(shí)時(shí)性要求［7］.Dellaert和 Kaess［5］提出利用 SLAM 問(wèn)題中內(nèi)在的稀疏結(jié)構(gòu)特性，通過(guò)稀疏矩陣分解(如稀疏Cholesky分解等)來(lái)求解的非線性最小二乘的方法.Kaess等［8］將圖建模和稀疏線性代數(shù)方法相結(jié)合，提出了iSAM方法，iSAM是增量式圖優(yōu)化方法的代表.該方法通過(guò)對(duì)平滑信息矩陣作QR分解，并選擇性對(duì)其進(jìn)行增量式更新，從而避免了每次重新計(jì)算平滑信息矩陣，提高了更新的效率，處理問(wèn)題的規(guī)模也大大增加，其最佳復(fù)雜度為O(n).在iSAM的基礎(chǔ)上，通過(guò)節(jié)點(diǎn)的重新排序和重新線性化，Kaess等［9］又提出了改進(jìn)后的iSAM2方法.為了進(jìn)一步提高增量式更新過(guò)程的計(jì)算效率，Kaess等［10］在 iSAM2的框架下，提出采用貝葉斯樹結(jié)構(gòu)來(lái)描述SLAM的方法.在SLAM稀疏特性的研究方法上，Konolige等［11］提出一種根據(jù)給定圖約束快速構(gòu)造稀疏矩陣的SPA(sparse pose adjustment)方法.

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

Olson等［7］將隨機(jī)梯度下降方法應(yīng)用到位姿圖SLAM的優(yōu)化中.每次迭代時(shí)隨機(jī)選取圖中的一條邊作為當(dāng)前約束并計(jì)算相應(yīng)的梯度下降方向，然后在該方向上對(duì)目標(biāo)函數(shù)尋優(yōu).隨機(jī)梯度下降方法具有不易陷入局部極值的優(yōu)點(diǎn)，對(duì)初始值具有較高的魯棒性.實(shí)驗(yàn)證明，即使初始值與最優(yōu)值相差較遠(yuǎn)，甚至在全零初始值或隨機(jī)初始值時(shí)，隨機(jī)梯度下降方法也能取得較好的收斂結(jié)果.Grisetti等［12］對(duì)Olson等所提的方法進(jìn)行了改進(jìn)和拓展，采用樹型結(jié)構(gòu)來(lái)描述位姿間的關(guān)系并通過(guò)增量方式表示待求解的狀態(tài)，從而能更有效地對(duì)位姿進(jìn)行更新.Grisetti等［13］還將樹型結(jié)構(gòu)表示以及隨機(jī)梯度方法應(yīng)用到6自由度位姿的優(yōu)化中，提出了基于樹的網(wǎng)絡(luò)優(yōu)化(TORO)方法.

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

Duckett等［14］提出采用 Gauss-Seidel松弛方法實(shí)現(xiàn)后端優(yōu)化.其基本思想是依次選取每個(gè)節(jié)點(diǎn)，根據(jù)其相鄰節(jié)點(diǎn)的位置及它們之間的約束關(guān)系重新計(jì)算并更新該節(jié)點(diǎn)的位置，且每次迭代都遍歷所有節(jié)點(diǎn).在假定方位角已知(如通過(guò)電子羅盤測(cè)量)的情況下，Duckett等證明了其必收斂于最優(yōu)解.該方法可用于增量式的SLAM中，在每次有新的觀測(cè)到來(lái)時(shí)直接在原來(lái)結(jié)果基礎(chǔ)上進(jìn)行更新.該方法的缺陷是，當(dāng)某條邊的誤差較大時(shí)，需要多次迭代才能將誤差分配到其他邊中，而這正是出現(xiàn)環(huán)形閉合時(shí)所需要應(yīng)對(duì)的情況.Frese等［15］提出多層次松弛的優(yōu)化策略，并利用多重網(wǎng)格方法求解偏微分方程，從而大大地提高了出現(xiàn)環(huán)形閉合時(shí)節(jié)點(diǎn)的優(yōu)化更新效率.

1.5 流形優(yōu)化

以上三類優(yōu)化方法均假定優(yōu)化過(guò)程是在歐氏空間中進(jìn)行的.在歐式空間中，機(jī)器人位姿中的旋轉(zhuǎn)分量的估計(jì)可能會(huì)出現(xiàn)奇異.為了避免奇異值問(wèn)題，旋轉(zhuǎn)分量部分可以采用四元數(shù)法表示，但又產(chǎn)生了額外的自由度，引入不必要的誤差.為此，Grisetti等［16］提出在流形空間中進(jìn)行優(yōu)化的思想，避免狀態(tài)空間參數(shù)化時(shí)可能出現(xiàn)的奇異值問(wèn)題，提出了一種分層優(yōu)化的圖優(yōu)化技術(shù)(即HOGMAN方法).該方法在在線建圖過(guò)程中，根據(jù)當(dāng)前的觀測(cè)約束，對(duì)地圖的修正只在上層(粗略描述層)進(jìn)行，從而提高了效率.最近，研究者們提供了能用于流形優(yōu)化的開源工具(g2o).Kummerle等［17］將HOG-MAN方法和Konolige等人提出的SPA思想結(jié)合起來(lái)，提出了基于流形的圖優(yōu)化通用框架(g2o框架)，大大提高了開發(fā)效率.在g2o框架的基礎(chǔ)上，Kummerle等［18］進(jìn)一步擴(kuò)展了狀態(tài)空間，如增加描述可能隨時(shí)間變化的系統(tǒng)參數(shù)，從而可實(shí)現(xiàn)同步傳感器標(biāo)定、建圖和機(jī)器人定位任務(wù)的方法.Hertzberg［19］和 Wagner［20］等也將流形方法擴(kuò)展到傳感器的融合和標(biāo)定問(wèn)題中，取得了初步成果.

1.6 圖優(yōu)化的計(jì)算效率、魯棒性和擴(kuò)展性

非線性最小二乘法的一個(gè)不足是對(duì)初始值的依賴，如果給定的初始值離最優(yōu)解距離遠(yuǎn)，則很容易陷入局部極值點(diǎn).為此，Carlone等［21］提出對(duì)基于圖優(yōu)化的SLAM作線性近似并給出解析求解的方法，可以利用其結(jié)果作為非線性最小二乘方法的初始值.但目前該方法只適用于2D SLAM.針對(duì)非線性最小二乘法對(duì)異常點(diǎn)的魯棒性不好的特點(diǎn)，目前已有多種解決方法.Sunderhauf等［22］提出了允許在圖優(yōu)化的過(guò)程中改變圖的拓?fù)浣Y(jié)構(gòu)以剔除錯(cuò)誤的環(huán)形閉合，從而提高了方法的魯棒性.在隨機(jī)梯度下降法(SGD)和Levenberg-Marquardt方法的基礎(chǔ)上，提出了前置濾波SGD和前置濾波Levenberg-Marquardt方法，二者均是在優(yōu)化操作之前，利用一個(gè)前置濾波器來(lái)完成一些預(yù)處理過(guò)程，以確保全局一致性.這些方法的應(yīng)用使得非線性最小二乘法的魯棒性有所提高.

當(dāng)機(jī)器人在大小固定的環(huán)境中行走時(shí)，圖的節(jié)點(diǎn)數(shù)目應(yīng)該跟環(huán)境的規(guī)模大小相關(guān)，而不是與機(jī)器人運(yùn)動(dòng)軌跡的長(zhǎng)度相關(guān).因此，要使SLAM方法具備良好的擴(kuò)展性，關(guān)鍵是對(duì)圖節(jié)點(diǎn)進(jìn)行有效的控制.減少節(jié)點(diǎn)數(shù)最為直觀的方法是對(duì)節(jié)點(diǎn)間的距離進(jìn)行限制，即只有節(jié)點(diǎn)間的距離超過(guò)一定的閾值時(shí)才添加到圖中［23］.Kretzschmar 等［24］從觀測(cè)所含的信息出發(fā)，評(píng)估觀測(cè)幀的信息增益，并依此對(duì)圖進(jìn)行剪枝，以控制節(jié)點(diǎn)數(shù)目.該方法在保持節(jié)點(diǎn)規(guī)模的同時(shí)具有最小的信息損失，因而也保證了地圖信息的完整性.對(duì)節(jié)點(diǎn)進(jìn)行剪枝實(shí)際對(duì)應(yīng)節(jié)點(diǎn)的邊緣化過(guò)程，這可能導(dǎo)致圖的結(jié)構(gòu)變得密集.Kretzschmar等［25］提出采用 Chow-Liu 樹對(duì)節(jié)點(diǎn)間的關(guān)系作近似描述，以保證節(jié)點(diǎn)連接的稀疏性.Yasir和 Jose等［26］提出利用線段擬合機(jī)器人的運(yùn)動(dòng)軌跡，從而減小位姿圖規(guī)模的方法.Xiang等［27］提出一種變分辨率的地圖表示方法.

上述研究方法進(jìn)一步提高了基于圖優(yōu)化方法的計(jì)算效率、魯棒性和可擴(kuò)展性.基于圖優(yōu)化的增量式SLAM方法目前研究最廣泛.

2 地圖質(zhì)量的評(píng)價(jià)

由于SLAM問(wèn)題的復(fù)雜性，其結(jié)果是多方面綜合，因此要給出一個(gè)大而全的評(píng)價(jià)方法是很困難的.比較合理的方法就是對(duì)SLAM的各個(gè)子問(wèn)題分別給出評(píng)價(jià)的方法.盡管如此，要對(duì)一些子問(wèn)題進(jìn)行評(píng)價(jià)也是非常困難的，像視覺(jué)領(lǐng)域中對(duì)立體視覺(jué)算法的評(píng)估一樣［28-29］，因?yàn)檫@些子問(wèn)題本身也很復(fù)雜.另外，對(duì)于大規(guī)模環(huán)境下的地圖重建結(jié)果進(jìn)行評(píng)估時(shí)，目前可用于對(duì)比分析的數(shù)據(jù)集也很有限.有學(xué)者提出了一些通用的數(shù)據(jù)集［30-31］，但用于性能對(duì)比時(shí)，由于這些數(shù)據(jù)集最初不是用于對(duì)比的目的，因此很多沒(méi)有真實(shí)數(shù)據(jù).這也是目前進(jìn)行評(píng)估時(shí)存在的困難之一.

Olson等［32］對(duì)地圖優(yōu)化算法的評(píng)估方法進(jìn)行了研究，提出要對(duì)全局優(yōu)化(Batch模式，通常是離線的)和在線優(yōu)化分別進(jìn)行比較.

在假定前端的特征提取、匹配(包括環(huán)形閉合檢測(cè))以及異常點(diǎn)已經(jīng)排除的前提下，地圖優(yōu)化實(shí)質(zhì)是后驗(yàn)概率估計(jì)問(wèn)題.目前主要有兩種方法:基于χ2誤差和基于均方差(MSE)的評(píng)價(jià)方法.

2.1 基于χ2誤差和均方差的評(píng)價(jià)方法比較

研究表明，基于χ2誤差方法的局限性在于:χ2誤差小的地圖優(yōu)化結(jié)果不一定比χ2誤差大的更好［33］.如圖3所示，是兩種不同誤差定義方法的優(yōu)化仿真結(jié)果比較.圖3(a)是真實(shí)值，通過(guò)對(duì)比圖3(b)和圖3(c)可見，在圖3(b)中的χ2誤差很小，但卻遠(yuǎn)遠(yuǎn)偏離真實(shí)值，而圖3(c)的χ2誤差雖然很大，但卻更接近真實(shí)值.

在地圖優(yōu)化問(wèn)題上，產(chǎn)生如圖3所示結(jié)果的主要原因是由于觀測(cè)過(guò)程具有高度非線性本質(zhì)，造成優(yōu)化曲面非常復(fù)雜.某些情況下，映射問(wèn)題產(chǎn)生的優(yōu)化曲面有“淺谷”出現(xiàn)，在此處，優(yōu)化表面變化大，但對(duì)χ2誤差的影響卻很小.即基于χ2誤差的優(yōu)化目標(biāo)函數(shù)定義中，機(jī)器人位姿估計(jì)和地圖特征點(diǎn)位置估計(jì)相互耦合的關(guān)系在某種程度上抵消了相互誤差對(duì)地圖結(jié)果的影響.圖3(b)中特征點(diǎn)的位置和機(jī)器人的位姿在優(yōu)化過(guò)程中同時(shí)發(fā)生了偏離，而χ2誤差很小.

由此可見，在大規(guī)模環(huán)境地圖創(chuàng)建中，誤差函數(shù)的定義非常關(guān)鍵.常用的基于觀測(cè)的誤差定義方法(χ2誤差)有時(shí)不一定很有效.基于MSE的誤差定義能得到較好的結(jié)果.

2.2 過(guò)擬合(over-fitting)問(wèn)題

在機(jī)器學(xué)習(xí)中，為了得到一致假設(shè)而使假設(shè)變得過(guò)于復(fù)雜，在訓(xùn)練數(shù)據(jù)上能夠獲得比其他假設(shè)更好地?cái)M合，但是在訓(xùn)練數(shù)據(jù)外的數(shù)據(jù)集上，卻不能很好地?cái)M合數(shù)據(jù)，出現(xiàn)了過(guò)擬合現(xiàn)象.出現(xiàn)這種現(xiàn)象的主要原因是訓(xùn)練數(shù)據(jù)中存在噪聲或者訓(xùn)練數(shù)據(jù)太少.在地圖優(yōu)化估計(jì)時(shí)，如果平均節(jié)點(diǎn)度(average node degree)小會(huì)出現(xiàn)該問(wèn)題.如圖4所示，機(jī)器人沿著一個(gè)方格運(yùn)動(dòng)，在右下角的區(qū)域，由于節(jié)點(diǎn)度小，存在過(guò)擬合問(wèn)題，導(dǎo)致地圖優(yōu)化結(jié)果和實(shí)際情況的不一致.解決過(guò)擬合問(wèn)題的方法主要有兩種:提前停止樹的增長(zhǎng)或者對(duì)已經(jīng)生成的樹按照一定的規(guī)則進(jìn)行后剪枝.

圖4 過(guò)擬合問(wèn)題

3 發(fā)展趨勢(shì)

近年來(lái)，SLAM的研究也出現(xiàn)一些新的趨勢(shì):

1)隨著移動(dòng)機(jī)器人的工作環(huán)境從室內(nèi)到室外的擴(kuò)展，工作空間越來(lái)越大，面臨的環(huán)境也越來(lái)越復(fù)雜.相對(duì)于室內(nèi)較為簡(jiǎn)單的環(huán)境對(duì)象，室外環(huán)境由各種對(duì)象組成，有靜止的、移動(dòng)的，有行人、各種車輛等.如何實(shí)現(xiàn)復(fù)雜、大規(guī)模、動(dòng)態(tài)環(huán)境下地圖表示，高精度、高效率的地圖創(chuàng)建，以及移動(dòng)機(jī)器人的自定位是一個(gè)很有挑戰(zhàn)性的研究課題，是現(xiàn)階段SLAM問(wèn)題研究的重點(diǎn).

2)對(duì)終生地圖創(chuàng)建(lifelong mapping)和維護(hù)的研究.在傳統(tǒng)意義上，機(jī)器人一旦通過(guò)SLAM實(shí)現(xiàn)了未知環(huán)境的建圖，則建圖任務(wù)結(jié)束，機(jī)器人即可利用已經(jīng)建好的地圖進(jìn)行定位或運(yùn)動(dòng)規(guī)劃.終生地圖研究將機(jī)器人長(zhǎng)期置于未知環(huán)境中，因此需要應(yīng)對(duì)環(huán)境的變化，并持續(xù)對(duì)地圖進(jìn)行更新、維護(hù).這種對(duì)地圖的不斷更新、維護(hù)就構(gòu)成了終生地圖研究的主要內(nèi)容［33］.

3)語(yǔ)義地圖(Semantic map)創(chuàng)建的研究.為了使機(jī)器人具備理解場(chǎng)景以及能夠識(shí)別場(chǎng)景物體的能力，構(gòu)造具有語(yǔ)義信息的地圖是一種重要途徑.構(gòu)建語(yǔ)義地圖，可使機(jī)器人能夠更好地為人類提供服務(wù)，是SLAM發(fā)展的趨勢(shì)之一.

4)基于多傳感器融合的SLAM研究.通過(guò)多傳感器信息融合，可以彌補(bǔ)單一傳感器在數(shù)據(jù)獲取時(shí)的不足，降低狀態(tài)估計(jì)的不確定性，改善數(shù)據(jù)關(guān)聯(lián)、環(huán)閉檢測(cè)等關(guān)鍵環(huán)節(jié)的精度和可靠性，進(jìn)而提高機(jī)器人定位和環(huán)境建圖的精度.多傳感器融合在移動(dòng)機(jī)器人SLAM中的研究和應(yīng)用將會(huì)越來(lái)越多.

5)多機(jī)器人協(xié)作 SLAM研究.與單機(jī)器人SLAM相比，多機(jī)器人SLAM問(wèn)題涉及多幾個(gè)機(jī)器人得到的子圖如何拼接得到全局地圖，以及在全局地圖中各個(gè)機(jī)器人的定位問(wèn)題.多機(jī)器人協(xié)作完成SLAM具有更準(zhǔn)確、更高效率的優(yōu)勢(shì)，因此受到越來(lái)越多的關(guān)注，正成為一個(gè)熱點(diǎn)研究問(wèn)題.

6)非歐式空間下的建模與狀態(tài)估計(jì)方法.針對(duì)SLAM問(wèn)題，在歐式空間中對(duì)機(jī)器人的位姿進(jìn)行求解存在奇異值問(wèn)題.另外，目前大多數(shù)SLAM方法是基于傳感器空間的，但機(jī)器人控制，需要將機(jī)器人坐標(biāo)系和傳感器坐標(biāo)系之間關(guān)聯(lián)考慮，如在基于激光掃描的傳感器中，利用里程計(jì)獲取的運(yùn)動(dòng)信息，是在機(jī)器人坐標(biāo)系下描述，和觀測(cè)傳感器(激光掃描)是不同的坐標(biāo)系，二者的變換關(guān)系是假定準(zhǔn)確已知的.雖然可以通過(guò)標(biāo)定得到二者變換關(guān)系，但結(jié)果也有不確定性，不確定性對(duì)SLAM結(jié)果的影響目前還未知.Grisetti等［16］提出在流形空間中進(jìn)行優(yōu)化的思想，其實(shí)質(zhì)是將SLAM問(wèn)題置于一個(gè)高維空間中，不僅可以避免狀態(tài)空間參數(shù)化時(shí)可能出現(xiàn)的奇異值問(wèn)題，還可以將傳感器自身參數(shù)的在線估計(jì)等問(wèn)題統(tǒng)一到一個(gè)完整的系統(tǒng)框架下.對(duì)非歐式空間中的求解方法的討論，將對(duì)SLAM的發(fā)展產(chǎn)生深刻的影響.

［1］王忠立，趙杰，蔡鶴皋.大規(guī)模環(huán)境下基于圖優(yōu)化SLAM的圖構(gòu)建方法［J］.哈爾濱工業(yè)大學(xué)學(xué)報(bào)，2015，47(1):75-85.

［2］梁明杰，閔華清，羅榮華.基于圖優(yōu)化的同時(shí)定位與地圖創(chuàng)建綜述［J］.機(jī)器人，2013，35(4):500-512.

［3］GRISETTI G，KUMMERLE R，STACHNISS C，et al.A tutorial on graph-based SLAM［J］.IEEE Transaction on Intelligent Transportation Systems Magazine，2010，2(4):31-43.

［4］GOLFARELLI M，MAIO D，RIZZI S.Elastic correction of dead-reckoning errors in map building［C］//IEEE/RSJ International Conference on Intelligent Robots and Systems.Piscataway:IEEE，1998:905-911.

［5］DELLAERT F，KAESS M.Square root SAM:simultaneous localization and mapping via square rootinformation smoothing［J］.International Journal of Robotics Research，2006，25(12):1181-1203.

［6］ FRESE U，LARSSON P，DUCKETT T.A multilevel relaxation algorithm for simultaneous localization and mapping［J］.IEEE Trans on Robotics，2005，21(2):196-207.

［7］ OLSON E，LEONARD J，TELLER S.Fast iterative alignment of pose graphs with poor initial estimates［C］//IEEE International Conference on Robotics and Automation.Piscataway:IEEE，2006:2262-2269.

［8］KAESS M，RANGANATHAN A，DELLAERT F.iSAM:Incremental smoothing and mapping［J］.IEEE Trans on Robotics，2008，24(6):1365-1378.

［9］ KAESS M，JOHANNSSON H，ROBERTS R，et al.iSAM2:incremental smoothing and mapping with fluid relinearization and incremental variable reordering［C］//Intl Conf on Robotics and Automation(ICRA).Shanghai:IEEE，2011:3281-3288.

［10］KAESS M，JOHANNSSON H，ROBERTS R，et al.iSAM2:incremental smoothing and mapping using the bayes tree［J］.InternationalJournalofRobotics Research，2012，31(2):216-235.

［11］KONOLIGEK， GRISETTIG， KUMMERLER.Efficient sparse pose adjustment for 2D mapping［C］//IEEE/RSJ International Conference on Intelligent Robots and Systems.Piscataway:IEEE，2010:22-29.

［12］GRISETTI G，STACHNISS C，GRZONKA S，et al.A tree parameterization for efficiently computing maximum likelihood maps using gradient descent［C］//Robotics:Science and Systems III.Cambridge:MIT Press，2008:65-72.

［13］GRISETTI G，STACHNISS C，GRZONKA S.TORO-tree-based network optimizer［CP/OL］.［2014-03-15］.http://www.openslam.org/toro.html.

［14］DUCKETT T，MARSLAND S，SHAPIRO J.Learning globally consistentmaps by relaxation［C］//IEEE International Conference on Robotics and Automation.Piscataway:IEEE，2000:3841-3846.

［15］FRESE U，LARSSON P，DUCKETT T.A multilevel relaxation algorithm for simultaneous localization and mapping［J］.IEEE Trans on Robotics，2005，21(2):196-207.

［16］GRISETTI G，KUMMERLE R，STACHNISS C，et al.Hierarchical optimization on manifolds for online 2D and 3D mapping［C］//IEEE International Conference on Robotics and Automation.Piscataway:IEEE，2010:273-278.

［17］KUMMERLE R，GRISETTI G，STRASDAT H，et al.g2o:A general framework for graph optimization［C］//IEEE International Conference on Robotics and Automation.Shanghai:IEEE，2011:3607-3613.

［18］KUMMERLE R，GRISETTI G，BURGARD W.Simultaneous calibration， localization， and mapping ［C］//InternationalConference on IntelligentRobots and Systems.Piscataway:IEEE，2011:3716-3721.

［19］HERTZBERG C，WAGNER R，F(xiàn)RESE U，et al.Integrating generic sensorfusion algorithms with sound state representations through encapsulation of manifolds［J］.Information Fusion，2013，14(1):57-77.

［20］WAGNER R，BIRBACH O，F(xiàn)RESE U.Rapid development of manifold-based graph optimization systems for multisensor calibration and SLAM ［C］//IEEE/RSJ InternationalConference on IntelligentRobots and Systems.Piscataway:IEEE，2011:3305-3312.

［21］CARLONE L，ARAGUES R，CASTELLANOS J A，et al.A first-order solution to simultaneous localization and mapping with graphical models［C］//IEEE International Conference on Robotics and Automation.Shanghai:IEEE，2011:1764-1771.

［22］SUNDERHAUF N，PROTZEL P.Towards a robust back-end for pose graph SLAM［C］//IEEE International Conference on Robotics and Automation(ICRA)，Minnesota:IEEE，2012:1254-1261.

［23］KONOLIGE K，AGRAWAL M.Frame SLAM:from bundle adjustment to real-time visual mapping［J］.IEEE Trans on Robotics，2008，24(5):1066-1077.

［24］KRETZSCHMAR H，GRISETTI G，STACHNISS C.Lifelong map learning for graph-based SLAM in static environments［J］.Kunstliche Intelligenz，2010，24(3):199-206.

［25］KRETZSCHMAR H，STACHNISS C，GRISETTI G.Efficient information theoretic graph pruning for graphbased SLAM with laser range finders［C］//IEEE/RSJ International Conference on Intelligent Robots and Systems.Piscataway:IEEE，2011:865-871.

［26］YASIR L，JOSE N.Go straight，turn right:pose graph reduction through trajectory segmentation using line segments［C］//European Conference on Mobile Robots(ECMR).Barcelona:IEEE，2013:144-149.

［27］XIANG J，TAZAKI Y，INAGAKI S.Autonomous variable-resolution map building for mobile robots in unknown environments［J］.Electeical Engineering in Japan，2014，186(4):59-69.

［28］SCHARSTEIN D，SZELISKI R，HIRSCHMOULLER H.Middlebury stereo vision page［DB/OL］.［2014-03-10］.http://vision.middlebury.edu/stereo/.

［29］SCHARSTEIN D，SZELISKI R.A taxonomy and evaluation of dense two-frame stereo correspondence algorithms［C］//IEEE Workshop on Stereo and Multi-Baseline Vision.Hawaii:IEEE，2002:131-140.

［30］HOWARD A，ROY N.The robotics data set repository(Radish)2003［DB/OL］.［2014-03-10］.http://www.lib.uts.edu.au/data-archive/4843/radish-roboticsdata-set-repository.

［31］NEWMAN P，CORKE P.Editorial:data papers-peer reviewed publication of high quality data sets［J］.International Journal of Robotics Research，2009，28(5):587.

［32］OLSON E，KAESS M.Evaluating the performance of map optimization algorithm［EB/OL］.［2014-03-20］.http://april.eecs.umich.edu/pdfs/olson2009rss.pdf.

［33］KRETZSCHMAR H，GRISETTI G，STACHNISS C.Lifelong map learning for graph-based SLAM in static environments［J］.Kunstliche Intelligenz，2010，24(3):199-206.