USME: 統(tǒng)一SLAM度量與評(píng)測(cè)技術(shù)研究

屈楨深，張啟航，楊志偉，董鴻宇

(哈爾濱工業(yè)大學(xué)，哈爾濱 150001)

0 引言

隨著智能無(wú)人系統(tǒng)的不斷發(fā)展，同時(shí)定位與建圖(Simultaneous Localization and Mapping，SLAM)技術(shù)作為無(wú)人系統(tǒng)感知自身狀態(tài)與周圍環(huán)境的關(guān)鍵技術(shù)，已經(jīng)成為了當(dāng)前科技研究的熱點(diǎn)領(lǐng)域。針對(duì)現(xiàn)存SLAM算法，如何建立一致、定量的評(píng)估框架，以及用什么樣的具體評(píng)估指標(biāo)集和測(cè)試方法來(lái)實(shí)現(xiàn)評(píng)估，從而既滿足不同算法統(tǒng)一評(píng)估的需求，又可針對(duì)特定應(yīng)用場(chǎng)景進(jìn)行專項(xiàng)評(píng)估，成為了當(dāng)前待研究的重要課題。

SLAM算法研究初期，大多論文僅針對(duì)具體測(cè)試數(shù)據(jù)給出測(cè)試指標(biāo)，不同方法之間缺乏一致性。2015年，Nardi等提出了SLAMBench[1]評(píng)估框架，該框架的主要工作是以不同的方式實(shí)現(xiàn)KinectFusion算法，然后通過(guò)ICL-NUIM數(shù)據(jù)集對(duì)其進(jìn)行評(píng)估，比較不同方法實(shí)現(xiàn)時(shí)SLAM系統(tǒng)的性能、精度和能耗。2018年，在其后續(xù)的工作SLAM-Bench2[2]中， Bodin等在SLAMBench的基礎(chǔ)上增加了框架的適用性，具有與數(shù)據(jù)集無(wú)關(guān)，支持即插即用的特點(diǎn)。評(píng)價(jià)指標(biāo)方面，在計(jì)算速度、算法精度和功耗之外增加了SLAM系統(tǒng)內(nèi)存使用的評(píng)估指標(biāo)，以全面評(píng)估算法在硬件平臺(tái)上的性能。該方法還支持使用多種數(shù)據(jù)集(包括ICL-NUIM、TUM RGB-D和EuRoC等)對(duì)多種SLAM算法進(jìn)行評(píng)估。2019年，Zhao等提出了新的SLAM框架GSLAM[3]，該框架不僅能夠提供性能評(píng)估的功能，還開(kāi)發(fā)了對(duì)應(yīng)的工具包,以便于定制化SLAM系統(tǒng)的開(kāi)發(fā)。GSLAM將輸入數(shù)據(jù)集、SLAM算法實(shí)現(xiàn)、可視化以及SLAM應(yīng)用全部置于一個(gè)統(tǒng)一的框架中。同年，Li等針對(duì)增強(qiáng)現(xiàn)實(shí)中的單目視覺(jué)慣性SLAM算法提出了一種評(píng)測(cè)方法[16]，使用手機(jī)在真實(shí)場(chǎng)景中采集數(shù)據(jù)制作數(shù)據(jù)集，并通過(guò)跟蹤精度及魯棒性、初始化質(zhì)量和重定位時(shí)間對(duì)SLAM算法進(jìn)行評(píng)測(cè)。這些評(píng)估框架可以實(shí)現(xiàn)不同SLAM算法在同一框架下進(jìn)行比較，但在性能評(píng)估方面還存在評(píng)價(jià)指標(biāo)較為單一的問(wèn)題。

對(duì)于測(cè)試SLAM算法性能的數(shù)據(jù)集主要包含兩種，一種是在仿真環(huán)境中獲取，而另一種是在實(shí)際場(chǎng)景中進(jìn)行采集。針對(duì)第一種，2014年，Handa等利用POVRay作為渲染軟件為RGB-D SLAM生成兩個(gè)評(píng)測(cè)序列[24]。該數(shù)據(jù)集只含有用于定位和局部深度圖的真實(shí)值，而且圖像的數(shù)量和場(chǎng)景的復(fù)雜度都十分有限。2016年，Ros等提供了多個(gè)含有不同天氣、季節(jié)和光照條件的序列用于評(píng)測(cè)SLAM的性能[25]，還提供了多視角相機(jī)的圖片數(shù)據(jù)和標(biāo)定數(shù)據(jù)。但是該數(shù)據(jù)集僅以5fps的幀率進(jìn)行錄制，無(wú)法對(duì)實(shí)時(shí)SLAM系統(tǒng)進(jìn)行評(píng)估，而且圖片的質(zhì)量也較低。針對(duì)第二種在實(shí)際場(chǎng)景中采集的數(shù)據(jù)集，現(xiàn)有的典型SLAM數(shù)據(jù)集，如KITTI數(shù)據(jù)集[4]、TUM數(shù)據(jù)集[5]和EuRoC數(shù)據(jù)集[6]等，通過(guò)使用不同的傳感器在不同的環(huán)境中采集數(shù)據(jù)，為SLAM系統(tǒng)的評(píng)估提供了較好的參照，但它們各自也存在場(chǎng)景不夠豐富、性能評(píng)估指標(biāo)比較單一等問(wèn)題。

本文針對(duì)現(xiàn)有評(píng)估框架和數(shù)據(jù)集存在的問(wèn)題，提出了一類統(tǒng)一SLAM度量與評(píng)測(cè)(Unified SLAM Measurement and Evaluation，USME)框架。首先該框架針對(duì)SLAM性能從定位精度、地圖構(gòu)建以及魯棒性等方面建立了指標(biāo)體系。之后針對(duì)現(xiàn)有數(shù)據(jù)集存在的場(chǎng)景單一、測(cè)試方法缺乏針對(duì)性等問(wèn)題，提出了數(shù)據(jù)集生成方式，并對(duì)典型SLAM算法長(zhǎng)時(shí)間運(yùn)行漂移、閉環(huán)檢測(cè)能力和算法魯棒性等方面的性能進(jìn)行評(píng)估。最后建立了多體協(xié)同SLAM子系統(tǒng)性能、全局狀態(tài)以及協(xié)同SLAM效率的評(píng)估方法。

1 指標(biāo)體系

SLAM系統(tǒng)通過(guò)搭載環(huán)境感知傳感器，利用環(huán)境觀測(cè)信息實(shí)現(xiàn)了實(shí)時(shí)定位與建圖[7]。所以為了對(duì)SLAM系統(tǒng)進(jìn)行全面準(zhǔn)確的評(píng)估，本文分別從SLAM的定位精度、地圖構(gòu)建、魯棒性等方面對(duì)SLAM系統(tǒng)進(jìn)行評(píng)估。在定位精度評(píng)估指標(biāo)中，通過(guò)絕對(duì)軌跡誤差和相對(duì)位姿誤差對(duì)SLAM算法進(jìn)行評(píng)估；在建圖方面，通過(guò)點(diǎn)云的重構(gòu)誤差以及3D重構(gòu)能力、點(diǎn)云稀疏程度和噪聲的比例來(lái)綜合評(píng)判SLAM所建的地圖；在魯棒性方面，則是通過(guò)跟蹤的魯棒性和跟蹤丟失時(shí)重定位的時(shí)間進(jìn)行評(píng)估。最后通過(guò)初始化時(shí)間、內(nèi)存的使用和能耗對(duì)SLAM系統(tǒng)的其他方面進(jìn)行評(píng)估。其主要框架如圖1所示。

圖1 SLAM算法性能指標(biāo)Fig.1 SLAM algorithm performance metrics

1.1 SLAM算法定位精度評(píng)估指標(biāo)

無(wú)人系統(tǒng)通過(guò)SLAM定位，計(jì)算未知環(huán)境中自身的位置與姿態(tài)。SLAM定位精度也是SLAM性能最直接的評(píng)價(jià)指標(biāo)之一，可通過(guò)絕對(duì)軌跡誤差和相對(duì)位姿誤差進(jìn)行測(cè)算。

(1)絕對(duì)軌跡誤差

在已知真實(shí)軌跡的情況下，絕對(duì)軌跡誤差是直接測(cè)量真實(shí)軌跡點(diǎn)和估計(jì)軌跡點(diǎn)之間的差值。首先通過(guò)時(shí)間同步機(jī)制，在統(tǒng)一的時(shí)間度量下將估計(jì)軌跡點(diǎn)與真實(shí)軌跡點(diǎn)關(guān)聯(lián)起來(lái)；之后將軌跡進(jìn)行對(duì)齊，過(guò)程如圖2所示；最后計(jì)算每個(gè)點(diǎn)對(duì)的差值，并輸出它們的均方根作為最后的絕對(duì)軌跡誤差。該指標(biāo)可直觀地反映全局的位姿誤差，同時(shí)也可以很好地反映SLAM算法的性能。

圖2 絕對(duì)軌跡誤差示意圖Fig.2 Schematic diagram of absolute trajectory error

(1)

所以整條軌跡的絕對(duì)軌跡誤差可以定義為

(2)

其中，ΔRi表示估計(jì)軌跡狀態(tài)與真實(shí)軌跡狀態(tài)之間的旋轉(zhuǎn)矩陣；N表示軌跡上所有狀態(tài)的數(shù)量。

絕對(duì)軌跡誤差通過(guò)最直觀的方式給出了整條軌跡的誤差，其優(yōu)點(diǎn)是易于比較。但它對(duì)誤差發(fā)生的時(shí)間比較敏感，誤差發(fā)生在軌跡剛開(kāi)始時(shí)會(huì)比發(fā)生在結(jié)束時(shí)產(chǎn)生更大的誤差。因此除了絕對(duì)軌跡誤差，相對(duì)位姿誤差也被廣泛使用，從而提供了更多信息。

(2)相對(duì)位姿誤差

相對(duì)位姿誤差是計(jì)算位姿變化量的差值。通過(guò)時(shí)間同步機(jī)制，在統(tǒng)一的時(shí)間度量下計(jì)算真實(shí)軌跡與估計(jì)軌跡每隔一段時(shí)間位姿的變化量，然后對(duì)該變化量做差，以獲得相對(duì)位姿誤差，該指標(biāo)主要用于估計(jì)系統(tǒng)的漂移量。

相對(duì)位姿誤差的基本思想是當(dāng)SLAM里程計(jì)在沒(méi)有全局參考時(shí)，通過(guò)測(cè)量不同時(shí)刻狀態(tài)之間的相對(duì)關(guān)系來(lái)評(píng)估軌跡的質(zhì)量，其過(guò)程如圖3所示。

圖3 相對(duì)位姿誤差示意圖Fig.3 Schematic diagram of relative pose error

沿著軌跡選擇K個(gè)狀態(tài)對(duì)dk作為一個(gè)集合I

(3)

其中，e>s，對(duì)于每一段dk的相對(duì)位姿誤差δdk為

(4)

所以整條軌跡的相對(duì)位姿誤差可以記作

(5)

由于相對(duì)位姿誤差不是一個(gè)單一的量，而是滿足某些標(biāo)準(zhǔn)的所有子軌跡的誤差集合，因此可以通過(guò)計(jì)算中值和平均值等統(tǒng)計(jì)數(shù)據(jù)，這與絕對(duì)軌跡誤差相比能夠提供更多的信息。

1.2 SLAM算法地圖構(gòu)建評(píng)估指標(biāo)

地圖構(gòu)建作為SLAM系統(tǒng)的另一個(gè)重要功能，地圖的質(zhì)量直接影響了后續(xù)路徑規(guī)劃或者導(dǎo)航等其他功能的成功率。針對(duì)3D點(diǎn)云地圖[8]，本節(jié)基于點(diǎn)云的幾何誤差、3D重構(gòu)能力和稀疏程度等對(duì)3D點(diǎn)云地圖進(jìn)行評(píng)估。

(1)點(diǎn)云的幾何精度

由于設(shè)備的精度、人為操作或者環(huán)境因素等影響，SLAM系統(tǒng)輸出的點(diǎn)云數(shù)據(jù)中不可避免地會(huì)出現(xiàn)一些離群點(diǎn)。為了能夠更加精確地計(jì)算點(diǎn)云的幾何精度，在進(jìn)行點(diǎn)云配準(zhǔn)之前，需要對(duì)這些點(diǎn)進(jìn)行剔除。剔除通常使用統(tǒng)計(jì)異常值去除(Statistical Outlier Removal，SOR)濾波器。其主要思想是計(jì)算每一個(gè)點(diǎn)鄰域內(nèi)所有臨近點(diǎn)的平均距離μ和標(biāo)準(zhǔn)差σ，然后篩選掉μ±ασ以外的點(diǎn)，此處的α取決于所分析鄰域的大小。剔除離群點(diǎn)后使用最近點(diǎn)迭代(Iterative Closest Points，ICP)[9]算法進(jìn)行點(diǎn)云的配準(zhǔn)[10]。ICP算法是一種點(diǎn)集對(duì)點(diǎn)集的配準(zhǔn)方法，基于最小二乘的最優(yōu)匹配，重復(fù)進(jìn)行確定對(duì)應(yīng)關(guān)系點(diǎn)集到計(jì)算最優(yōu)剛體變換的過(guò)程

(6)

當(dāng)點(diǎn)云配準(zhǔn)之后，通過(guò)計(jì)算點(diǎn)云之間的距離評(píng)估SLAM構(gòu)建地圖的幾何精度。最常用的方法是點(diǎn)云到點(diǎn)云的直接計(jì)算(Cloud-to-Cloud, C2C)。利用該方法比較3D點(diǎn)云地圖是最簡(jiǎn)單直接的，不需要對(duì)點(diǎn)云進(jìn)行分割或者特征提取。針對(duì)每一個(gè)需要比較的點(diǎn)，該算法都會(huì)在真實(shí)環(huán)境點(diǎn)云中尋找最近的點(diǎn)，同時(shí)計(jì)算它們的歐幾里得距離，如圖4所示。

圖4 點(diǎn)云距離計(jì)算示意圖Fig.4 Point cloud distance calcuation schematic diagram

在計(jì)算估計(jì)點(diǎn)云與真實(shí)點(diǎn)云之間的距離時(shí)，可能不是點(diǎn)對(duì)點(diǎn)之間的距離計(jì)算，而是一個(gè)點(diǎn)集對(duì)一個(gè)點(diǎn)集之間的計(jì)算。因此在C2C算法中，采用豪斯多夫距離(Hausdorff distance)計(jì)算點(diǎn)云之間的距離。點(diǎn)集A與點(diǎn)集B之間的豪斯多夫距離為

(7)

其中，a和b為點(diǎn)集A和B中的點(diǎn)，而d(a,b)表示a和b之間的歐幾里得距離。式(7)的含義是在A中選擇一個(gè)點(diǎn)a，然后計(jì)算它和B中所有點(diǎn)的歐幾里得距離，并選擇最小的一個(gè);之后遍歷A中的點(diǎn)，選擇結(jié)果最大的一個(gè)作為豪斯多夫距離。這種方法只依賴于3D點(diǎn)云數(shù)據(jù)中點(diǎn)的密度變化，在生成點(diǎn)云時(shí)，如果可以使點(diǎn)云具有相似的局部密度，則可以最小化距離測(cè)量的誤差。

(2)點(diǎn)云地圖的其他評(píng)估指標(biāo)

通過(guò)計(jì)算點(diǎn)云距離可以得到點(diǎn)云的幾何精度。但是針對(duì)點(diǎn)云地圖，只通過(guò)一個(gè)幾何精度并不能全面地評(píng)判其好壞。所以為了更客觀全面地評(píng)估SLAM算法的地圖構(gòu)建能力，在幾何精度的基礎(chǔ)上又提出了三個(gè)指標(biāo)，分別為點(diǎn)云的噪聲、點(diǎn)云稀疏程度以及3D重構(gòu)能力。

當(dāng)通過(guò)濾波器對(duì)點(diǎn)云進(jìn)行過(guò)濾后，依然會(huì)存在一些無(wú)關(guān)點(diǎn)，這些點(diǎn)可以通過(guò)視覺(jué)上的檢查直接進(jìn)行剔除，噪聲點(diǎn)的百分比就是清理掉點(diǎn)的數(shù)量與原始點(diǎn)云中點(diǎn)的數(shù)量的比。所以點(diǎn)云的噪聲可以定義為

(8)

其中，Nn和Nc分別代表清除掉的無(wú)關(guān)點(diǎn)數(shù)量和點(diǎn)云的總點(diǎn)數(shù),同時(shí)Nc也可以反映點(diǎn)云的稀疏程度。像一些點(diǎn)云準(zhǔn)確率很高的算法如ORB-SLAM[8,11]，由于太過(guò)稀疏，幾乎不能應(yīng)用于導(dǎo)航任務(wù)[12];反而像LSD-SLAM[13]這種準(zhǔn)確率中等，但可以提供半稠密點(diǎn)云地圖的算法更適用于導(dǎo)航任務(wù)。對(duì)于3D重構(gòu)這項(xiàng)指標(biāo)，通過(guò)視覺(jué)上觀察就可以對(duì)其進(jìn)行評(píng)價(jià)。

在對(duì)SLAM系統(tǒng)生成的3D點(diǎn)云地圖進(jìn)行評(píng)估時(shí)，并不能僅僅只是關(guān)注精度的問(wèn)題，還需綜合考慮該SLAM算法的應(yīng)用場(chǎng)景。例如應(yīng)用于導(dǎo)航或者路徑規(guī)劃時(shí)，一個(gè)高精度的稀疏點(diǎn)云地圖幾乎沒(méi)有任何幫助，但一個(gè)精度較低的半稠密或者稠密點(diǎn)云地圖往往會(huì)有更好的效果[14]。

1.3 SLAM算法魯棒性評(píng)估指標(biāo)

當(dāng)SLAM系統(tǒng)運(yùn)行時(shí)，可能會(huì)受到各種各樣來(lái)自于外界的干擾，如光照的變化、移動(dòng)物體等。一個(gè)魯棒性較好的SLAM系統(tǒng)可以對(duì)這些干擾做出正確的判斷,并盡可能地減少這些干擾對(duì)其產(chǎn)生的影響，從而保證系統(tǒng)的穩(wěn)定運(yùn)行[15]。本節(jié)則通過(guò)跟蹤的魯棒性以及重定位的時(shí)間對(duì)SLAM系統(tǒng)的魯棒性進(jìn)行評(píng)估。

(1)跟蹤的魯棒性

在極端情況下，視覺(jué)軌跡可能會(huì)出現(xiàn)跟丟的情況。一個(gè)魯棒性較好的系統(tǒng)應(yīng)該盡可能快地從丟失狀態(tài)中恢復(fù)，并且可以將新的位姿與之前的軌跡相結(jié)合。假設(shè)在跟蹤時(shí)，發(fā)生了一小段丟失的情況。其中s1和s2分別代表丟失前后的兩段軌跡，這兩段軌跡相對(duì)于真實(shí)軌跡之間會(huì)分別存在一個(gè)旋轉(zhuǎn)矩陣和平移向量

s1→{R1,t1},s2→{R2,t2}

(9)

對(duì)于一個(gè)魯棒性較好的SLAM系統(tǒng)，子軌跡s1和s2應(yīng)該能夠較好地與真實(shí)軌跡進(jìn)行對(duì)齊，因此{(lán)R1,t1}和{R2,t2}應(yīng)該比較接近;否則，估計(jì)軌跡將會(huì)與真實(shí)軌跡存在較大的偏差，從而造成全局的不一致性，如圖5所示。

圖5 SLAM系統(tǒng)跟蹤魯棒性示意圖Fig.5 SLAM system tracking robustness schematics diagram

通過(guò)SLAM系統(tǒng)獲得一個(gè)跟蹤結(jié)果時(shí)，其中可能發(fā)生多段丟失的情況。首先根據(jù)這些丟失的情況將軌跡進(jìn)行分段,然后使用上述方法將每一段估計(jì)軌跡和真實(shí)軌跡進(jìn)行對(duì)齊。通過(guò)比較兩個(gè)臨近子軌跡的變化來(lái)計(jì)算重定位誤差

(10)

其中，n代表總共子軌跡的數(shù)量；ξi和ξi+1則代表相鄰兩段子軌跡與其對(duì)應(yīng)真實(shí)軌跡之間的相似變化。

一個(gè)好的SLAM系統(tǒng)應(yīng)該可以從丟失狀態(tài)中快速恢復(fù)，并且回到正確的位置。同時(shí)丟失的時(shí)間越短,重定位的誤差越小，SLAM系統(tǒng)的魯棒性也就越好。

(2)重定位時(shí)間

跟蹤的魯棒性是用來(lái)反映SLAM系統(tǒng)穩(wěn)定性的性能指標(biāo)，同時(shí)在軌跡發(fā)生丟失后，SLAM系統(tǒng)完成重定位的時(shí)間也十分重要。通過(guò)強(qiáng)迫系統(tǒng)進(jìn)入重定位階段可以準(zhǔn)確地獲取重定位時(shí)間，為此設(shè)計(jì)了一種測(cè)試方式。首先制作一個(gè)測(cè)試序列,其具體方式是：圍繞一個(gè)有紋理的物體平穩(wěn)旋轉(zhuǎn)幾圈。在繞過(guò)第一圈之后，將后面的一些幀換成黑色幀，其中黑色幀的時(shí)長(zhǎng)定為τ。在第一圈時(shí)，一個(gè)SLAM系統(tǒng)可以將其所看到的視覺(jué)信息存儲(chǔ)起來(lái)，當(dāng)黑色幀到來(lái)時(shí)，它被迫進(jìn)入重定位階段;在當(dāng)黑色幀結(jié)束后，會(huì)有一段時(shí)間的原始幀數(shù)據(jù)，SLAM系統(tǒng)可以在這段時(shí)間內(nèi)進(jìn)行重定位。通過(guò)手動(dòng)添加黑色幀的方式可以準(zhǔn)確地計(jì)算出重定位的時(shí)間，即黑色幀結(jié)束到開(kāi)始輸出估計(jì)姿態(tài)的時(shí)間差。

假設(shè)將tsi作為第i次黑色幀結(jié)束的時(shí)間，tei是在第i次黑色幀結(jié)束后輸出的第一個(gè)有效位姿的時(shí)間。如果一共插入了N次黑色幀，那么平均的重定位時(shí)間可以定義為

(11)

1.4 SLAM算法的其他評(píng)估指標(biāo)

(1)初始化時(shí)間

對(duì)于SLAM系統(tǒng)，一個(gè)快速準(zhǔn)確的初始化可以帶來(lái)很好的用戶體驗(yàn)。但是不同的SLAM系統(tǒng)可能使用不同的初始化策略。原始的PTAM算法需要用戶的交互，基于濾波的MSCKF需要設(shè)備保持靜止一段時(shí)間，而ORB-SLAM2則能夠自動(dòng)地選擇兩幀用于初始化。如果使用SLAM系統(tǒng)傳回來(lái)的第一個(gè)有效位姿作為初始化結(jié)束的標(biāo)志，其后面的估計(jì)位姿可能不是一個(gè)很好的估計(jì)結(jié)果。因此，通過(guò)判斷運(yùn)動(dòng)的估計(jì)尺度是否穩(wěn)定來(lái)確定初始化是否完成[16]。

在t時(shí)刻，將SLAM系統(tǒng)估計(jì)軌跡的尺度定義為s(t)，當(dāng)SLAM系統(tǒng)完成初始化時(shí)，需要達(dá)到一個(gè)穩(wěn)定的尺度。所以在時(shí)間[t,t+Δt]內(nèi)定義最大相對(duì)尺度r(t)為

(12)

當(dāng)r(t)小于某個(gè)閾值rinit時(shí)，就可以認(rèn)為SLAM系統(tǒng)估計(jì)軌跡的尺度已經(jīng)穩(wěn)定，系統(tǒng)已經(jīng)完成初始化，此時(shí)的t記作tinit，表示SLAM系統(tǒng)的初始化時(shí)間。

除了初始化時(shí)間，在初始化時(shí)估計(jì)軌跡尺度的準(zhǔn)確性也非常重要。計(jì)算在tinit時(shí)的對(duì)稱相對(duì)尺度誤差[16]為

(13)

其中，sg代表每條軌跡的全局尺度。對(duì)于視覺(jué)SLAM系統(tǒng)，可以通過(guò)將所有結(jié)果與真實(shí)軌跡進(jìn)行對(duì)齊來(lái)獲得sg;對(duì)于視覺(jué)慣性SLAM系統(tǒng)[17, 26]，則需要將sg設(shè)為1，因?yàn)閷?duì)其來(lái)說(shuō)，使用準(zhǔn)確的尺度進(jìn)行初始化是非常重要的。

(2)內(nèi)存的使用及能耗

內(nèi)存的使用及能耗是指SLAM系統(tǒng)在工作時(shí)所消耗的內(nèi)存及能量。不同的使用者對(duì)于SLAM系統(tǒng)的需求可能不同，他們所使用的硬件平臺(tái)也不盡相同。有一些可能使用一些大型的移動(dòng)設(shè)備，它們擁有較高的功耗和性能較好的處理器；而有一些則可能使用一些小型的移動(dòng)載體[18]，如小型的無(wú)人機(jī)，像這樣的載體設(shè)備對(duì)于功耗的需求和處理器的性能就會(huì)較為敏感。然而，SLAM系統(tǒng)，往往會(huì)消耗大量的內(nèi)存，但是對(duì)于內(nèi)存的消耗通常不會(huì)作為評(píng)估的標(biāo)準(zhǔn)。所以為了更好地幫助使用者選擇適合他們的SLAM系統(tǒng)，應(yīng)該將功耗以及內(nèi)存的使用作為兩個(gè)評(píng)估SLAM系統(tǒng)的重要指標(biāo)。目前，SLAM算法的動(dòng)態(tài)內(nèi)存消耗可以在每一幀的水平上進(jìn)行精確測(cè)量，同時(shí)也可以計(jì)算整體的內(nèi)存消耗。而對(duì)于SLAM系統(tǒng)能耗指標(biāo)的測(cè)量，可以采用一種常用的傳感器應(yīng)用程序編程接口(Application Programming Interface, API)對(duì)其進(jìn)行測(cè)量。

2 數(shù)據(jù)集

對(duì)SLAM算法進(jìn)行評(píng)估時(shí)，往往需要使用數(shù)據(jù)集[19]。一個(gè)通用準(zhǔn)確的數(shù)據(jù)集對(duì)于視覺(jué)SLAM系統(tǒng)的發(fā)展是非常重要的。目前,數(shù)據(jù)集的種類主要有兩種，一種是在實(shí)際環(huán)境中采集的數(shù)據(jù)集，現(xiàn)有的典型數(shù)據(jù)集有TUM RGB-D數(shù)據(jù)集、KITTI數(shù)據(jù)集和EuRoC MAV數(shù)據(jù)集。它們各自的特點(diǎn)如表1所示。

表1 典型SLAM數(shù)據(jù)集的特點(diǎn)

同時(shí)TUM數(shù)據(jù)集和KITTI數(shù)據(jù)集也提供了軌跡評(píng)估工具，可以通過(guò)絕對(duì)軌跡誤差和和相對(duì)位姿誤差對(duì)估計(jì)軌跡進(jìn)行評(píng)估。但在真實(shí)環(huán)境中采集數(shù)據(jù)集也存在很多困難：

1)相機(jī)的位置和姿態(tài)難以獲取。準(zhǔn)確地測(cè)量相機(jī)在每一幀的6自由度位姿是一項(xiàng)艱巨的任務(wù)。

2)準(zhǔn)確的校準(zhǔn)參數(shù)獲取較為復(fù)雜。大多數(shù)視覺(jué)SLAM系統(tǒng)都需要對(duì)相機(jī)的內(nèi)參進(jìn)行標(biāo)定，如果是視覺(jué)慣性[20-21]SLAM系統(tǒng),還需要對(duì)慣性測(cè)量單元(Inertial Measurement Unit, IMU)以及相機(jī)和IMU的外參進(jìn)行標(biāo)定。

3)無(wú)法模擬失真場(chǎng)景。實(shí)際場(chǎng)景中相機(jī)的畸變參數(shù)是固定的，并且人為不可控，所獲得的畸變效果不一定可以滿足使用的需求。

4)標(biāo)簽標(biāo)注較為困難。在一些結(jié)合語(yǔ)義信息的SLAM系統(tǒng)需要對(duì)每一張圖片進(jìn)行語(yǔ)義的標(biāo)注，但通過(guò)手動(dòng)的方式對(duì)圖片進(jìn)行標(biāo)注需要大量的人力，同時(shí)也無(wú)法保證在每個(gè)物體的邊緣都有像素級(jí)的精度。

而另一種數(shù)據(jù)集是在仿真環(huán)境中進(jìn)行采集。在真實(shí)環(huán)境中采集數(shù)據(jù)集存在以上困難，而在仿真環(huán)境中這些問(wèn)題都可以輕松地被解決。首先在仿真環(huán)境中，可以準(zhǔn)確地獲取相機(jī)的運(yùn)動(dòng)軌跡、任意時(shí)刻的姿態(tài)和相機(jī)的參數(shù)；同時(shí)相機(jī)的參數(shù)還可以根據(jù)自己的需求進(jìn)行更改，從而實(shí)現(xiàn)了獲取的圖片中含有運(yùn)動(dòng)模糊等畸變效果。另外，針對(duì)環(huán)境的語(yǔ)義分割結(jié)果也可以實(shí)現(xiàn)像素級(jí)的精度，同時(shí)在仿真環(huán)境中，使用者可以使用任何載體搭載相機(jī)來(lái)獲取數(shù)據(jù)；除此之外，在仿真環(huán)境的另一個(gè)優(yōu)勢(shì)就是可以根據(jù)使用者的需求任意選擇場(chǎng)景。

在仿真環(huán)境中采集數(shù)據(jù)集具有對(duì)場(chǎng)景的選擇更加靈活、成本更低、獲取數(shù)據(jù)更加豐富準(zhǔn)確的優(yōu)點(diǎn)，因此通過(guò)建立仿真環(huán)境來(lái)生成數(shù)據(jù)集，并使用仿真數(shù)據(jù)集對(duì)SLAM算法進(jìn)行評(píng)估。本文基于Unreal Engine游戲開(kāi)發(fā)平臺(tái)，使用由微軟開(kāi)發(fā)的AirSim[22]插件來(lái)采集數(shù)據(jù)。同時(shí)為了對(duì)SLAM系統(tǒng)進(jìn)行更加客觀真實(shí)的評(píng)測(cè)，選擇城市交通、居民住宅、工業(yè)廠房和房間等場(chǎng)景對(duì)SLAM算法進(jìn)行測(cè)試。其中部分場(chǎng)景如圖6所示。

(a)住宅區(qū)

(b)城市場(chǎng)景

(c)工廠廠房

(d)房間場(chǎng)景圖6 部分SLAM測(cè)試場(chǎng)景Fig.6 Part of the SLAM test scenarios

住宅區(qū)場(chǎng)景和城市場(chǎng)景均為室外場(chǎng)景，其中住宅區(qū)場(chǎng)景的特點(diǎn)是含有較多的植被，在該環(huán)境下采集數(shù)據(jù)時(shí)，由于影子的原因會(huì)產(chǎn)生很多明暗變化的效果;同時(shí)該場(chǎng)景中的馬路較為規(guī)則，很方便形成多個(gè)閉環(huán)，可以用于測(cè)試SLAM系統(tǒng)回環(huán)檢測(cè)的能力。而在城市場(chǎng)景中，存在很多運(yùn)動(dòng)的車和行人，可以用于測(cè)試SLAM系統(tǒng)應(yīng)對(duì)移動(dòng)物體時(shí)的魯棒性;同時(shí)該場(chǎng)景中的馬路交錯(cuò)復(fù)雜，所以可以選擇合適的路線，在沒(méi)有閉環(huán)的情況下測(cè)試SLAM系統(tǒng)長(zhǎng)期工作時(shí)的漂移量。

工廠廠房和房間場(chǎng)景均為室內(nèi)場(chǎng)景，其中工廠廠房場(chǎng)景較大，而房間場(chǎng)景就相對(duì)狹窄。在工廠廠房中有很多移動(dòng)的機(jī)器人，還有很多特征較為相似的貨架，這些因素對(duì)SLAM系統(tǒng)來(lái)說(shuō)具有一定的挑戰(zhàn)性。在房間場(chǎng)景中，由于空間比較狹窄，相機(jī)移動(dòng)時(shí)很有可能遇到白墻等特征點(diǎn)較少的情況，這對(duì)基于特征點(diǎn)匹配的SLAM算法也具有一定的挑戰(zhàn)性。同時(shí)在房間中光線變化較為明顯，這也可以對(duì)SLAM系統(tǒng)應(yīng)對(duì)室內(nèi)光照變化的情況進(jìn)行評(píng)測(cè)。

在文中生成的仿真數(shù)據(jù)集中，包含在仿真環(huán)境中采集的RGB-D圖片、深度圖片、語(yǔ)義分割圖片，還包含相機(jī)的真實(shí)位姿，其中包括每個(gè)采樣時(shí)間所對(duì)應(yīng)的三維位置和用四元數(shù)表示的旋轉(zhuǎn)。

3 測(cè)試方法

通過(guò)測(cè)試方法確定測(cè)試場(chǎng)景及如何開(kāi)展測(cè)試。本文選擇8個(gè)典型數(shù)據(jù)集序列，針對(duì)不同情況開(kāi)展指標(biāo)測(cè)試。它們分別是在室外的住宅區(qū)和城市場(chǎng)景以及室內(nèi)的工廠廠房和普通的房間場(chǎng)景中進(jìn)行采集，然后針對(duì)在室內(nèi)室外場(chǎng)景、是否存在閉環(huán)、是否含有運(yùn)動(dòng)模糊等一系列不同的情況，對(duì)SLAM系統(tǒng)進(jìn)行測(cè)試。其中具體的測(cè)試方法如圖7所示。

圖7 數(shù)據(jù)集所包含的主要測(cè)試方法Fig.7 The primary test methods contained in the dataset

在實(shí)驗(yàn)中，使用上述仿真環(huán)境生成的數(shù)據(jù)集對(duì)SLAM算法進(jìn)行測(cè)試。為了對(duì)SLAM不同方面的性能進(jìn)行評(píng)估，通過(guò)使用不同的場(chǎng)景以及不同特點(diǎn)的軌跡對(duì)其進(jìn)行測(cè)試。具體的測(cè)試方式如下：

1)選擇不同的室內(nèi)場(chǎng)景與室外場(chǎng)景進(jìn)行測(cè)試。在所選的測(cè)試數(shù)據(jù)集中，室內(nèi)場(chǎng)景大部分的光線強(qiáng)弱比較均勻，同時(shí)還存在一些像墻角和貨架等形狀規(guī)則的結(jié)構(gòu)，這些結(jié)構(gòu)有利于特征的提取及匹配。在室外場(chǎng)景中，由于存在一些植被和建筑，它們的影子會(huì)產(chǎn)生明暗變化的效果，所以可以通過(guò)這些序列測(cè)試SLAM算法對(duì)光照變化的魯棒性。

2)在住宅區(qū)場(chǎng)景中，本文通過(guò)錄制兩個(gè)相似的長(zhǎng)序列，它們的主要區(qū)別是一個(gè)不存在閉環(huán)，而另一個(gè)只是在最后形成一個(gè)大的閉環(huán)，分別用于測(cè)試SLAM系統(tǒng)長(zhǎng)時(shí)間運(yùn)行的漂移量和檢測(cè)閉環(huán)的能力。

3)同樣是在住宅區(qū)場(chǎng)景中，本文則錄制了一個(gè)存在三個(gè)閉環(huán)的長(zhǎng)序列，并且在其基礎(chǔ)上加入運(yùn)動(dòng)模糊生成另一個(gè)與其相似的序列，主要目的是測(cè)試視覺(jué)SLAM系統(tǒng)在應(yīng)對(duì)相機(jī)失真情況時(shí)的性能表現(xiàn)。

4)在工廠廠房和城市交通場(chǎng)景中存在很多運(yùn)動(dòng)物體，這些運(yùn)動(dòng)物體對(duì)SLAM系統(tǒng)的定位與建圖會(huì)造成一定的干擾，所以可以通過(guò)這些序列對(duì)SLAM系統(tǒng)應(yīng)對(duì)動(dòng)態(tài)環(huán)境時(shí)的性能表現(xiàn)進(jìn)行測(cè)試。

5)最后還有一些序列，它們的特點(diǎn)是存在相機(jī)的快速移動(dòng)和旋轉(zhuǎn)，借此可以測(cè)試SLAM系統(tǒng)在快速移動(dòng)和旋轉(zhuǎn)情況下跟蹤的穩(wěn)定性。同時(shí)還在其中一個(gè)序列中插入一段黑色幀，主要目的是測(cè)試SLAM系統(tǒng)從傳感器短暫中斷中恢復(fù)過(guò)來(lái)的能力。

每個(gè)數(shù)據(jù)集序列的具體特點(diǎn)如表2所示。

表2 仿真測(cè)試數(shù)據(jù)集

目前被廣泛使用的SLAM系統(tǒng)有很多，而ORB-SLAM與其他SLAM系統(tǒng)相比，其前端采用ORB特征進(jìn)行提取與跟蹤，具有較好的魯棒性；后端則是基于圖優(yōu)化進(jìn)行全局位姿的優(yōu)化，同時(shí)還采用詞袋模型實(shí)現(xiàn)了較為準(zhǔn)確的回環(huán)檢測(cè)。因此，本文選擇使用ORB-SLAM對(duì)數(shù)據(jù)集進(jìn)行測(cè)試，其輸出結(jié)果如圖8所示，圖中分別表示在住宅區(qū)場(chǎng)景、城市交通場(chǎng)景、工廠廠房場(chǎng)景和房間場(chǎng)景中ORB-SLAM輸出的估計(jì)軌跡及點(diǎn)云地圖。通過(guò)實(shí)驗(yàn)結(jié)果驗(yàn)證了仿真數(shù)據(jù)集在實(shí)際使用過(guò)程中的可行性。

圖8 在不同場(chǎng)景下運(yùn)行ORB-SLAM2得到的輸出軌跡Fig.8 The output trajectories obtained by running ORB- SLAM2 in different scenarios

4 多體協(xié)同SLAM性能

相對(duì)于單體SLAM系統(tǒng)而言，多體協(xié)同SLAM系統(tǒng)能夠適應(yīng)更加難以預(yù)先判斷的動(dòng)態(tài)環(huán)境，同時(shí)在SLAM系統(tǒng)出現(xiàn)故障或者環(huán)境中干擾較大時(shí)具有更好的魯棒性[23]。大多數(shù)的多體協(xié)同SLAM系統(tǒng)采用一種集中式的結(jié)構(gòu)，主要包括由各個(gè)子端組成的前端、服務(wù)器后端和多體協(xié)同工作決策系統(tǒng)。為了對(duì)多體協(xié)同SLAM系統(tǒng)性能進(jìn)行準(zhǔn)確和全面的評(píng)估，可以對(duì)每個(gè)子系統(tǒng)的性能、經(jīng)過(guò)服務(wù)器后端融合優(yōu)化后的全局狀態(tài)以及多體協(xié)同SLAM系統(tǒng)的工作效率三個(gè)方面進(jìn)行評(píng)估。

針對(duì)不同的子端和服務(wù)器優(yōu)化后的全局狀態(tài)的評(píng)估可以沿用前文提到的單體SLAM性能評(píng)估指標(biāo)，但對(duì)于它們各自的側(cè)重點(diǎn)有所不同。其中各個(gè)子端是整個(gè)協(xié)同SLAM系統(tǒng)的前端，對(duì)其進(jìn)行準(zhǔn)確性、穩(wěn)定性和實(shí)時(shí)性的評(píng)估是十分必要的。針對(duì)定位的準(zhǔn)確性，可以使用絕對(duì)軌跡誤差和相對(duì)位姿誤差進(jìn)行評(píng)估，同時(shí)根據(jù)其跟蹤的穩(wěn)定性和實(shí)時(shí)性測(cè)試整個(gè)系統(tǒng)的魯棒性。

對(duì)于多體協(xié)同SLAM系統(tǒng)的后端，負(fù)責(zé)接收各個(gè)子端的關(guān)鍵幀信息，并對(duì)每個(gè)子系統(tǒng)的位姿估計(jì)進(jìn)行優(yōu)化，對(duì)局部點(diǎn)云地圖進(jìn)行融合、去噪與優(yōu)化，并作為SLAM系統(tǒng)的輸出。因此，對(duì)其整體的定位精度和地圖構(gòu)建結(jié)果進(jìn)行評(píng)估也十分重要。

(14)

所以，一個(gè)完整的系統(tǒng)軌跡的絕對(duì)軌跡誤差可以定義為

(15)

其中，ΔRi表示估計(jì)值和真實(shí)值狀態(tài)之間的旋轉(zhuǎn)矩陣；N表示軌跡上所有狀態(tài)的數(shù)量；M表示子系統(tǒng)的數(shù)量。

多體協(xié)同SLAM的絕對(duì)軌跡誤差給出了整個(gè)系統(tǒng)各個(gè)子端的平均誤差，便于對(duì)系統(tǒng)性能進(jìn)行最直觀的測(cè)量。

(16)

所以，一個(gè)完整的多體協(xié)同SLAM系統(tǒng)軌跡的相對(duì)位姿誤差RE可以定義為

(17)

其中，M表示子系統(tǒng)的數(shù)量；N表示運(yùn)動(dòng)過(guò)程中所有子系統(tǒng)之間的狀態(tài)數(shù)量。

圖9 相對(duì)位姿誤差狀態(tài)圖Fig.9 Relative pose error state map

對(duì)于多體協(xié)同SLAM算法后端融合優(yōu)化得到的點(diǎn)云地圖進(jìn)行評(píng)估時(shí)，本文延用之前對(duì)于單體SLAM系統(tǒng)3D點(diǎn)云地圖的指標(biāo)進(jìn)行評(píng)估，分析了3D點(diǎn)云的幾何誤差、3D重構(gòu)能力和稀疏程度等因素。與單體SLAM的建圖效果進(jìn)行比較，可得出協(xié)同SLAM算法的優(yōu)點(diǎn)與有待改進(jìn)之處。

最后，提出了對(duì)多體協(xié)同SLAM工作效率的評(píng)估方法。相對(duì)于單體SLAM而言，多體協(xié)同系統(tǒng)額外引入了一個(gè)組合優(yōu)化的問(wèn)題，優(yōu)化的效率決定了子系統(tǒng)的利用與協(xié)同效率。在面對(duì)大型或復(fù)雜任務(wù)時(shí)，通常會(huì)考慮引入多體協(xié)同SLAM系統(tǒng)，通過(guò)多個(gè)子端共同工作，減少了SLAM系統(tǒng)運(yùn)行的時(shí)間，從而提高了系統(tǒng)的魯棒性與任務(wù)的執(zhí)行效率。所以在子系統(tǒng)個(gè)數(shù)相同的情況下，通過(guò)引入運(yùn)行時(shí)間和各個(gè)子系統(tǒng)移動(dòng)的平均路程這兩個(gè)重要指標(biāo)來(lái)衡量協(xié)同SLAM系統(tǒng)的任務(wù)執(zhí)行效率。當(dāng)SLAM系統(tǒng)任務(wù)完成的時(shí)間越短，移動(dòng)的平均路程越少時(shí)，說(shuō)明此系統(tǒng)具有更高的工作效率。

在時(shí)間的計(jì)算上，記錄多體協(xié)同SLAM系統(tǒng)開(kāi)啟的時(shí)間t1和任務(wù)完成的時(shí)間t2，即可得到系統(tǒng)運(yùn)行的時(shí)間

T=t1-t2

(18)

在路程計(jì)算上，為了得到每個(gè)系統(tǒng)的行走路程，可以在一段很短的時(shí)間Δt內(nèi)近似子系統(tǒng)的SLAM運(yùn)行軌跡為直線。將每個(gè)系統(tǒng)的軌跡分成了Nt段

(19)

計(jì)算每個(gè)子端移動(dòng)的平均距離為

(20)

其中，M表示子系統(tǒng)的個(gè)數(shù)；xi,j表示子系統(tǒng)i的第j個(gè)狀態(tài)。

通過(guò)對(duì)每個(gè)子系統(tǒng)性能、后端融合優(yōu)化后的全局狀態(tài)以及多體協(xié)同SLAM系統(tǒng)的工作效率三個(gè)方面的評(píng)估，可以從系統(tǒng)的精確性、魯棒性、實(shí)時(shí)性以及多體協(xié)同效率對(duì)其做出更加準(zhǔn)確和全面的評(píng)估。

5 結(jié)論

針對(duì)SLAM領(lǐng)域目前缺少定量準(zhǔn)確的評(píng)估體系，本文提出了一類USME框架，從指標(biāo)體系、數(shù)據(jù)集及評(píng)測(cè)方法三個(gè)維度為各種SLAM方法的性能度量及比較研究提供基準(zhǔn)，主要的貢獻(xiàn)如下：

1)在指標(biāo)體系中，通過(guò)絕對(duì)軌跡誤差和相對(duì)位姿誤差對(duì)SLAM的定位精度進(jìn)行評(píng)估；通過(guò)幾何精度、3D重構(gòu)能力、點(diǎn)云稀疏程度和噪聲程度對(duì)地圖構(gòu)建進(jìn)行評(píng)估；通過(guò)跟蹤的魯棒性和重定位時(shí)間對(duì)魯棒性進(jìn)行評(píng)估；以及通過(guò)內(nèi)存使用和能耗等對(duì)SLAM性能進(jìn)行評(píng)估，從而構(gòu)成了SLAM系統(tǒng)的全面評(píng)估體系。

2)基于3D仿真平臺(tái)生成自己的仿真數(shù)據(jù)集，以對(duì)SLAM系統(tǒng)在不同場(chǎng)景中的表現(xiàn)、長(zhǎng)時(shí)間運(yùn)行時(shí)的漂移量、閉環(huán)檢測(cè)的能力、相機(jī)遮擋及光照和運(yùn)動(dòng)物體對(duì)其性能的影響等方面進(jìn)行測(cè)試。

3)通過(guò)對(duì)多體協(xié)同SLAM系統(tǒng)每個(gè)子系統(tǒng)的性能、經(jīng)過(guò)服務(wù)器后端融合優(yōu)化后的全局狀態(tài)以及多體協(xié)同SLAM系統(tǒng)的工作效率三個(gè)方面進(jìn)行評(píng)估。

希望該評(píng)估框架可加快SLAM領(lǐng)域在定位精度、地圖構(gòu)建、魯棒性以及多體協(xié)同SLAM方向的發(fā)展，同時(shí)推動(dòng)視覺(jué)導(dǎo)航定位的實(shí)際應(yīng)用。