分布式多車協(xié)同視覺SLAM系統(tǒng)*

蔣朝陽，蘭天然，鄭曉妮，高九龍，葉學(xué)通

（北京理工大學(xué)機械與車輛學(xué)院，北京 100081）

前言

定位與導(dǎo)航是自動駕駛技術(shù)的關(guān)鍵組成部分［1］。視覺SLAM技術(shù)能在沒有環(huán)境先驗信息的情況下，建立環(huán)境模型，同時估計運動［2］。目前相關(guān)研究成果豐富［3-5］，已經(jīng)應(yīng)用于智能車輛領(lǐng)域［6］。但該方案的誤差累計問題嚴重，難以長期保證準(zhǔn)確定位。基于多車協(xié)同［7-8］的視覺SLAM技術(shù)相較于單車SLAM定位更加精準(zhǔn)，在大型場景、長時間定位任務(wù)中具有廣泛應(yīng)用價值。

視覺傳感器因其硬件廉價、結(jié)構(gòu)簡單、安裝方便、體積小、質(zhì)量輕、特征豐富等優(yōu)點成為SLAM系統(tǒng)的重要傳感器之一。目前已經(jīng)有眾多視覺SLAM方案被提出，例如ORB-SLAM3［9］、VINS-Mono［10］等?；谝曈X的協(xié)同SLAM也得到了一定的研究和發(fā)展，然而大多數(shù)研究方案為了減輕帶寬要求往往采用集中式框架，例如CCM-SLAM［11］、CVI-SLAM［12］、COVINS［13］等。其劣勢在于數(shù)據(jù)隱私性差，對服務(wù)端的計算能力要求較高。這種框架不適用于自動駕駛車輛獨立完成定位建圖任務(wù)。分布式系統(tǒng)能夠更好地解決集中式系統(tǒng)所存在的問題，對于單個節(jié)點的加入與退出均具有較強的魯棒性和適應(yīng)性。自2013年以來，分布式協(xié)同SLAM已經(jīng)取得了部分研究成果［14-15］，但仍存在受限于帶寬的問題［16］。

現(xiàn)有的協(xié)同SLAM方案通常利用多車間的共視區(qū)域識別（visual place recognition，VPR）實現(xiàn)數(shù)據(jù)關(guān)聯(lián)。詞袋法被廣泛應(yīng)用于該項任務(wù)，但視覺傳感器易受光線、環(huán)境等影響，基于詞袋的識別方法容易產(chǎn)生假陽性閉環(huán)。Cieslewski等［17］利用基于端到端訓(xùn)練的NetVLAD描述子［18］實現(xiàn)了共視區(qū)域識別，提升了系統(tǒng)對于光照變化的魯棒性，降低了協(xié)同SLAM系統(tǒng)的數(shù)據(jù)交換量。該方案在DOOR-SLAM［19］中也得到了應(yīng)用。

多車共視區(qū)域識別通過當(dāng)前關(guān)鍵幀與多車關(guān)鍵幀庫中圖像描述子的匹配實現(xiàn)。面對具有大量相同特征的環(huán)境，基于圖像相似性的識別方法易出現(xiàn)假陽性閉環(huán)。因此眾多研究者通過閉環(huán)離群值剔除，保證正確數(shù)據(jù)關(guān)聯(lián)。Agarwal等［20］為協(xié)方差矩陣增添尺度因子實現(xiàn)協(xié)方差矩陣動態(tài)縮放，改變閉環(huán)在優(yōu)化過程中所占的比重。一種尋找一致性最大團的離 群 值 剔 除 算 法［21］在DOOR-SLAM［19］、DiSCo-SLAM［22］等分布式協(xié)同SLAM系統(tǒng)中均有體現(xiàn)，但是相對位姿變換估計精度有限，錯誤的識別結(jié)果也可能滿足一致性。

多車協(xié)同SLAM利用共視區(qū)域識別結(jié)果實現(xiàn)多車數(shù)據(jù)關(guān)聯(lián)，增加多車間相對位姿約束，實現(xiàn)協(xié)同定位精度增強。目前廣泛應(yīng)用于分布式協(xié)同SLAM位姿圖優(yōu)化的算法是由Choudhary等［23］所提出的兩階段高斯賽德爾算法（DGS），該方案第一階段估計旋轉(zhuǎn)分量，第二階段利用第一階段的估計結(jié)果恢復(fù)完整的位姿。但該方案需要求解非凸優(yōu)化問題，算法收斂速度受初始值的精確性影響，同時該方案僅考慮唯一的相對位姿變換作為約束，在車輛數(shù)量增加以及位姿求解不準(zhǔn)確時，定位精度和一致性會受到影響。Huang等［22］提出了一種兩階段全局-局部優(yōu)化算法，該方案提高了整體定位估計的一致性，但是在共視閉環(huán)約束數(shù)量較少時無法進行優(yōu)化。

針對自動駕駛應(yīng)用場景，為了解決現(xiàn)有協(xié)同視覺SLAM中存在的環(huán)境適應(yīng)性差、假陽性閉環(huán)多以及多車協(xié)同優(yōu)化約束單一等問題，本文提出了一種分布式、數(shù)據(jù)傳輸輕量、魯棒的多車協(xié)同視覺SLAM系統(tǒng)。該系統(tǒng)有如下特點：

（1）通過引入NetVLAD全局圖像描述子，實現(xiàn)多車間共視區(qū)域識別，提升視覺系統(tǒng)對于光照變化的魯棒性，減輕數(shù)據(jù)交換量，降低分布式系統(tǒng)對于帶寬的要求；

（2）提出并引入一種新的基于數(shù)據(jù)相似性和結(jié)構(gòu)一致性的多車間閉環(huán)離群值剔除算法，保證協(xié)同定位過程的正確數(shù)據(jù)關(guān)聯(lián)；

（3）在現(xiàn)有的DGS優(yōu)化算法［23］的基礎(chǔ)上，提出了一種改進的DGS優(yōu)化算法（multi-closure DGS，MC-DGS）。該算法基于共視區(qū)域閉環(huán)構(gòu)建多車間相對位姿變換約束，在閉環(huán)數(shù)量增多時，利用歷史閉環(huán)信息構(gòu)建里程計約束，通過位姿圖優(yōu)化求解，提升了多車協(xié)同定位效果；

（4）通過自動駕駛測試平臺和開源數(shù)據(jù)集KITTI進行實驗，驗證了系統(tǒng)的有效性。

1 分布式協(xié)同視覺SLAM系統(tǒng)概述

多車協(xié)同視覺SLAM定位增強系統(tǒng)原理如圖1所示，該系統(tǒng)能夠?qū)崿F(xiàn)以下功能：（1）每輛無人車搭載雙目相機，接收圖像信息并運行雙目視覺里程計；（2）當(dāng)多車間滿足通信范圍時，交互圖像全局描述子信息并構(gòu)建多車間共視區(qū)域識別閉環(huán)約束；（3）對產(chǎn)生多車間閉環(huán)約束的車輛進行位姿圖優(yōu)化，提升系統(tǒng)的定位效果。

圖1 多車協(xié)同定位增強系統(tǒng)原理圖

圖2展示了本文所提出的分布式多車協(xié)同視覺SLAM系統(tǒng)的結(jié)構(gòu)。首先，每輛無人車通過車載的雙目相機獲取圖像，采用ORB-SLAM2的雙目模式作為視覺里程計進行位姿估計。與此同時，提取圖像的NetVLAD描述子，發(fā)送至閉環(huán)檢測模塊實現(xiàn)單車SLAM閉環(huán)檢測、多車共視區(qū)域識別閉環(huán)檢測，獲得滿足數(shù)據(jù)相似性和結(jié)構(gòu)一致性的閉環(huán)集合。然后，利用多車通信，交換多車間共視區(qū)域識別閉環(huán)關(guān)鍵幀的圖像特征、特征描述子集合以及由建圖模塊獲得的3D點位置集合，實現(xiàn)多車間相對位姿變換估計。最后，根據(jù)由視覺里程計構(gòu)建的單車視覺里程計約束和由共視區(qū)域匹配所產(chǎn)生的多車間位姿約束，執(zhí)行分布式位姿圖優(yōu)化模塊。

圖2 分布式多車協(xié)同視覺SLAM系統(tǒng)框圖

得益于分布式的框架，車輛可以自由地加入、退出協(xié)同定位系統(tǒng)。引入通過端到端訓(xùn)練的NetVLAD圖像描述子，系統(tǒng)能夠一定程度適應(yīng)光照變化，并且傳輸一幀圖像描述子的大小僅為1 Kb，減輕了帶寬負擔(dān)。閉環(huán)檢測模塊同時檢驗數(shù)據(jù)相似性和結(jié)構(gòu)一致性，避免了只考慮單一因素所造成的假陽性閉環(huán)，保證正確數(shù)據(jù)關(guān)聯(lián)。分布式位姿圖優(yōu)化模塊利用本文所提出的MC-DGS算法，在單車里程計約束的基礎(chǔ)上，增加了多車間共視區(qū)域閉環(huán)約束以及兩次閉環(huán)所構(gòu)成的里程計約束，優(yōu)化求解位姿，得到更加精準(zhǔn)的位姿估計。

2 多車間共視區(qū)域識別模塊

為了保證在有可能感知混疊的情況下實現(xiàn)更加精準(zhǔn)的多車間共視區(qū)域識別，本文采用了對光照魯棒的、輕量的全局圖像描述子NetVLAD。此外，提出一種基于數(shù)據(jù)相似性和結(jié)構(gòu)一致性的閉環(huán)離群值剔除方法，由此保證多車間數(shù)據(jù)關(guān)聯(lián)的魯棒性。

2.1 基于NetVLAD描述子的共視區(qū)域識別模型

圖像特征對于光照的魯棒性由圖像描述子決定。NetVLAD全局圖像描述子是基于卷積神經(jīng)網(wǎng)絡(luò)實現(xiàn)場景識別的先進性結(jié)果。通過端到端訓(xùn)練，能夠在一定程度上應(yīng)對光線變化，彌補傳統(tǒng)圖像描述子的不足，提高視覺系統(tǒng)對于光照變化的魯棒性。與此同時，NetVLAD描述子十分緊湊，采用描述子前128維描述一幀圖像即可良好地完成共視區(qū)域識別任務(wù)，大小僅占用1 Kb。應(yīng)用于本系統(tǒng)中，能夠降低數(shù)據(jù)交換量，適用于帶寬受限的分布式協(xié)同SLAM系統(tǒng)。

基于文獻［17］的研究結(jié)果，本文采用128維的NetVLAD描述子實現(xiàn)多車間的共視區(qū)域識別功能。對于i時刻來自車α的圖像描述子vαi以及j時刻來自車β的圖像描述子vβj，滿足成為多車間共視區(qū)域閉環(huán)匹配候選集的條件是描述子間的歐式距離小于相似度閾值τNetVLAD：

相似閾值越低，本步驟所獲得的閉環(huán)候選集中的匹配對數(shù)量就越少。考慮到本文引入了閉環(huán)離群值剔除模塊，因此可以設(shè)置較為寬松的相似度閾值，在本文實驗中，該閾值被設(shè)置為0.2。

2.2 幾何驗證模型

幾何驗證模型用于初步實現(xiàn)多車間閉環(huán)離群值剔除以及多車間的相對位姿變換計算。對于候選閉環(huán)關(guān)鍵幀匹配對，提取視覺特征、視覺特征描述子以及相應(yīng)的3D點位置估計信息，利用PNP算法以及RANSAC算法計算產(chǎn)生閉環(huán)匹配的兩車間的相對位姿變換矩陣，同時返回一組內(nèi)點。如果內(nèi)點的數(shù)量足夠大，認為本次閉環(huán)是比較成功的。通過設(shè)定內(nèi)點數(shù)量閾值可以初步篩選部分閉環(huán)，在本文實驗中，該項閾值被設(shè)定為20。由幾何驗證步驟可以計算出在多車間的相對位姿變換估計值，因此本文所提出的多車協(xié)同定位系統(tǒng)無需事先測量多車間的初始位置關(guān)系。

2.3 閉環(huán)離群值剔除模型

最大成對一致性（PCM）［21］在協(xié)同SLAM閉環(huán)離群值剔除領(lǐng)域得到了廣泛的使用和認可，但是基于兩幀圖像所計算的相對位姿變換并不完全準(zhǔn)確，錯誤的閉環(huán)也可能具有成對一致性。因此本文提出了一種基于數(shù)據(jù)相似性和結(jié)構(gòu)一致性的多車間共視區(qū)域閉環(huán)離群值剔除方法。利用ORB-SLAM2在視覺里程計部分所構(gòu)成的共視幀關(guān)系，驗證閉環(huán)幀的數(shù)據(jù)相似性，利用PCM算法，驗證閉環(huán)幀的結(jié)構(gòu)一致性，篩選同時具備兩項性質(zhì)的閉環(huán)關(guān)鍵幀，避免產(chǎn)生由于感知混疊和位姿估計不準(zhǔn)所導(dǎo)致的假陽性閉環(huán)，保證多車間實現(xiàn)穩(wěn)健的數(shù)據(jù)關(guān)聯(lián)。這里，可以直觀地認為相似度高的圖像成為閉環(huán)的可能性更大。

由PCM算法［21］可知，對于兩車之間所構(gòu)成的共視區(qū)域閉環(huán)幀應(yīng)滿足以下關(guān)系：

式中：‖‖?Σ代表馬氏距離；zαiαk表示無人車α由k幀到i幀的相對位姿變換矩陣；zαk βl代表由無人車β坐標(biāo)系下的l幀到無人車α坐標(biāo)系下的k幀的相對位姿變換矩陣，其他符號具有類似的含義；γ代表PCM成對一致性檢測閾值。閾值越低表明對于閉環(huán)的要求越嚴格，反之越寬松。

記c(zi，zk)表示為一對多車共視區(qū)域閉環(huán)的結(jié)構(gòu)一致性評分，評分越高，代表閉環(huán)的結(jié)構(gòu)一致性越強。假設(shè)當(dāng)前車輛為α，其中zi表示當(dāng)前車輛在i幀所產(chǎn)生的多車間閉環(huán)<αi，βj>，即車輛α在i幀與車輛β在j幀經(jīng)過了同一地點，構(gòu)成了多車間閉環(huán)。同理，zk表示當(dāng)前車輛在k幀所產(chǎn)生的多車閉環(huán)<αk，βl>。

ORB-SLAM2前端將關(guān)鍵幀以簇的形式進行管理。當(dāng)兩個關(guān)鍵幀所觀測到的相同地圖點滿足一定數(shù)量要求時，構(gòu)成共視關(guān)鍵幀的關(guān)系。其中，共視程度最高的共視關(guān)鍵幀被稱為該關(guān)鍵幀的父關(guān)鍵幀。同時，單車內(nèi)閉環(huán)關(guān)鍵幀與當(dāng)前幀的相似度應(yīng)高于當(dāng)前幀與其父關(guān)鍵幀間的相似度?；谏鲜鲫P(guān)系，文中假設(shè)正確的多車間閉環(huán)具有以下特征：與當(dāng)前幀具有較高的相似度，與圖像的父關(guān)鍵幀（或者單車內(nèi)閉環(huán)關(guān)鍵幀）也具有較高的相似度，其關(guān)系如圖3所示。

圖3 基于數(shù)據(jù)相似性計算相似度評分原理

假設(shè)當(dāng)前車輛為α，對于多車間閉環(huán)約束zi=<αi，βj>，計算關(guān)鍵幀βj與當(dāng)前車輛閉環(huán)關(guān)鍵幀αi的父關(guān)鍵幀（單車內(nèi)閉環(huán)關(guān)鍵幀）之間的相似度評分s(zi)：

同理，對于多車間閉環(huán)約束zk=<αk，βl>，可以計算得到s(zk)。相似度評分越高，代表圖像越相似，正確閉環(huán)概率就越高。

綜合考慮數(shù)據(jù)相似性和結(jié)構(gòu)一致性，定義多車間共視區(qū)域閉環(huán)質(zhì)量評分，由一對無人車間閉環(huán)候選可計算出閉環(huán)質(zhì)量評分m(zi，zk)：

圖4（a）表示無人車α與無人車β間的閉環(huán)候選集合，圖4（b）表示閉環(huán)兩兩間質(zhì)量評分所構(gòu)成的評分矩陣，該鄰接矩陣是一個對稱矩陣，圖4（c）表示由閉環(huán)關(guān)系和質(zhì)量評分所產(chǎn)生的無向加權(quán)圖，處于最大團中的閉環(huán)將被最終確定為用于位姿圖優(yōu)化的正確多車間閉環(huán)。

圖4 閉環(huán)質(zhì)量評分矩陣與最大團原理

多車間閉環(huán)產(chǎn)生于經(jīng)過同一地點，因此閉環(huán)關(guān)鍵幀需具備數(shù)據(jù)相似性。同時，由于視覺里程計約束，閉環(huán)幀需要具備結(jié)構(gòu)一致性。因此，正確的多車間閉環(huán)兼?zhèn)鋬煞N性質(zhì)，考慮滿足兩種性質(zhì)的可能性，能夠提高篩選閉環(huán)關(guān)鍵幀的正確率，優(yōu)于考慮單一因素。

設(shè)置m(zi，zk)閾值可以篩選同時具備數(shù)據(jù)相似性和結(jié)構(gòu)一致性條件的閉環(huán)，閾值越大，閉環(huán)要求越嚴格，數(shù)量越少，正確率越高。該方案容易計算，并且在不需要額外通信負擔(dān)的情況下，綜合利用了視覺里程計前端提供的數(shù)據(jù)相似性結(jié)果以及多車間閉環(huán)所構(gòu)成的結(jié)構(gòu)一致性結(jié)果。

3 分布式位姿圖優(yōu)化模塊

這里提出一種改進的DGS算法MC-DGS實現(xiàn)多車協(xié)同定位優(yōu)化。在構(gòu)建多車間相對位姿變換約束的基礎(chǔ)上，考慮歷史多車間閉環(huán)信息，構(gòu)建基于兩次閉環(huán)過程的里程計約束，降低不精準(zhǔn)的相對位姿變換估計對優(yōu)化過程的影響，進一步提高位姿優(yōu)化精度和估計一致性。

3.1 單車SLAM優(yōu)化建模

首先，考慮單車視覺SLAM問題的表現(xiàn)形式，定義時間0到t范圍內(nèi)6自由度姿態(tài)為X={x0，x1，???，xt}，X?SE(3)。定義觀測相對位姿變換矩陣zij和期望相對位姿變換矩陣?之間的誤差eij為

式中：I4×4表示4階單位矩陣；Rij表示旋轉(zhuǎn)分量；tij表示平移分量；‖‖?F表示Frobenius范數(shù)。則單車SLAM問題可以簡化為一個最小二乘問題：

式中：Fij表示位姿xi與xj之間的約束；Ωij表示協(xié)方差矩陣。通過求解式（9），得到一組最優(yōu)的位姿軌跡，可以實現(xiàn)無人車輛的自主定位。

3.2 多車協(xié)同SLAM優(yōu)化建模

假設(shè)多車協(xié)同SLAM中有N輛無人車參與，N={1，2，…，n}。對于無人車α?N，其位姿可以表示為Xa，所有車輛的位姿集合表示為Xall={Xa∣α∈N}。對于多車協(xié)同SLAM系統(tǒng)，希望解決以下問題：

式中：Fintra(Xall)表示所有單車SLAM約束所構(gòu)成的代價；Finter(Xall)表示所有多車間約束所構(gòu)成的代價，通過最小化兩種代價，獲得多車間兩兩一致的位姿估計結(jié)果，提高定位精度。

式中：Cα表示無人車α內(nèi)部約束的集合；Cαβ表示無人車α和β之間的多車間約束集合；表示產(chǎn)生約束的兩個關(guān)鍵幀。因此對于每一輛參與協(xié)同定位的無人車，如無人車α，可以構(gòu)建包含內(nèi)部約束以及外部約束的優(yōu)化問題，如式（13）所示：

區(qū)別于單車SLAM，本系統(tǒng)考慮多車間約束，建立以下模型：

式中：R∈和t∈分別代表旋轉(zhuǎn)和平移噪聲，并假設(shè)平移噪聲符合信息矩陣為零均值的高斯分布，旋轉(zhuǎn)噪聲遵循參數(shù)為的馮-米塞斯分布；‖‖?F表 示Frobenius范數(shù)［24］。

3.3 MC-DGS優(yōu)化算法

區(qū)別于DGS算法［23］，本文考慮歷史閉環(huán)信息對于提高優(yōu)化精度的作用，提出了一種改進的DGS算法，即MC-DGS。當(dāng)一對無人車間存在不止一對閉環(huán)時，該方案能夠利用歷史閉環(huán)信息，提高位姿估計的一致性和精準(zhǔn)性。

由多車間閉環(huán)約束<αi，βj>、<αk，βl>可以建立以下模型：

每確定一個多車間閉環(huán)可以計算出兩幀間的相對 位 姿 變 換 矩 陣，如Rαi βj、Rβlαk。對 于 閉 環(huán) 約 束<αk，βl>利用已優(yōu)化的Rαi、Rβj構(gòu)造視覺里程計約束<αi，αk>。對于無人車α和無人車β，分別解決以下線性最小二乘問題，完成算法第一階段旋轉(zhuǎn)估計：

算法第二階段代入旋轉(zhuǎn)估計并求解式（16），獲得完整位姿估計。當(dāng)不存在兩次多車間閉環(huán)時，Rαi、Rβj、Rαi βj置位為單位矩陣，第一階段變?yōu)榍蠼馐剑?1），與DGS算法的第一階段相同，不影響定位優(yōu)化過程。

良好的初始值以及優(yōu)化順序?qū)涌烨蠼獾倪^程。本文采用一種選擇性排序的方式加快求解。系統(tǒng)根據(jù)此次通信過程中多車間閉環(huán)的數(shù)量進行排序，首先解決閉環(huán)約束少的無人車定位優(yōu)化問題，再解決閉環(huán)約束多的無人車定位優(yōu)化問題，為優(yōu)化過程提供良好的初始值。

4 實驗

4.1 實車測試

利用自主搭建的自動駕駛測試平臺實現(xiàn)校園內(nèi)道路環(huán)境數(shù)據(jù)采集。通過雙目相機采集圖像，利用一個高精度、實時的GNSS/INS組合導(dǎo)航系統(tǒng)（Novatel PwrPak7D-E1）采集車輛運動軌跡作為定位結(jié)果參考值，構(gòu)建多車協(xié)同SLAM數(shù)據(jù)集，對本文所提出的系統(tǒng)進行驗證。圖5所示為本文所采集的校園道路環(huán)境場景示例。

圖5 校園道路環(huán)境真實數(shù)據(jù)

在實驗環(huán)節(jié)，本文利用Oxford RobotCar數(shù)據(jù)集中的部分序列進行NetVLAD特征提取，用于完成NetVLAD描述子聚類，實現(xiàn)單車閉環(huán)檢測以及多車間的共視區(qū)域識別。實驗過程采用128維的NetVLAD描述子描述圖像，其表現(xiàn)形式如圖6所示。由于本文重點驗證所提出系統(tǒng)的定位效果提升性能，因此實驗環(huán)節(jié)采用離線的方式獲得定位優(yōu)化結(jié)果，不考慮多車通信時延對定位效果的影響。

圖6 圖像的NetVLAD描述子表現(xiàn)形式

圖7展示了不同方法下的3車協(xié)同定位結(jié)果。虛線代表軌跡真值，實線代表估計軌跡。不同顏色的線段分別代表不同車輛的運行軌跡。本文系統(tǒng)的估計軌跡閉環(huán)狀態(tài)良好，估計誤差小，與真實軌跡基本重合，體現(xiàn)出更好的定位精度。

圖7 校園道路環(huán)境下不同協(xié)同定位方法比較

針對不同長度的校園真實數(shù)據(jù)測試序列進行測試，圖8所示為兩段校園真實數(shù)據(jù)在衛(wèi)星地圖上的投影以及基于3車協(xié)同的定位結(jié)果。從圖中可以看出，估計軌跡與真實軌跡基本重合，定位精度高，驗證了系統(tǒng)的有效性。

圖8 校園道路數(shù)據(jù)集衛(wèi)星投影及協(xié)同定位結(jié)果

由于真實數(shù)據(jù)采集場景限制以及實驗硬件平臺數(shù)量有限，本文所提出的協(xié)同視覺SLAM系統(tǒng)相較于單車SLAM以及其他協(xié)同SLAM系統(tǒng)所體現(xiàn)的定位精度優(yōu)勢并不突出，因此，利用開源自動駕駛數(shù)據(jù)集KITTI進一步實驗，證明本文系統(tǒng)的精度效果、輕量性以及環(huán)境適應(yīng)性。

4.2 KITTI數(shù)據(jù)集測試

首先針對不同算法步驟下的協(xié)同定位進行實驗，證明本文系統(tǒng)每個模塊的有效性。其次，基于多個序列測試，對比現(xiàn)有的單車視覺SLAM方法以及協(xié)同視覺SLAM方法，證明本文所提出的系統(tǒng)具有更好的精度效果和環(huán)境適應(yīng)性。然后，測試協(xié)同定位過程中的數(shù)據(jù)交換量，證明系統(tǒng)的輕量性。最后，提升測試協(xié)同車輛數(shù)量，證明系統(tǒng)的通用性和可擴展性。

為了模擬測試多車協(xié)同視覺SLAM過程，對KITTI數(shù)據(jù)集的原始序列進行了一定的處理，以適應(yīng)本文所述的系統(tǒng)。根據(jù)需要模擬的無人車協(xié)同數(shù)量，將原始數(shù)據(jù)進行分割和修改，時間戳調(diào)整為部分重疊以形成共視區(qū)域。例如，構(gòu)建3車協(xié)同測試數(shù)據(jù)，將原始序列分割成長度大致相同的3段，每段數(shù)據(jù)在時間上保證部分重疊。上述方法在DOORSLAM［19］和DiSCo-SLAM［22］中均有體現(xiàn)，被廣泛應(yīng)用于協(xié)同SLAM測試。

圖9所示為不同算法步驟下的3車協(xié)同定位結(jié)果。圖9（a）為僅進行DGS優(yōu)化的軌跡估計結(jié)果，由于未進行多車間閉環(huán)離群值剔除，定位軌跡合并錯誤，誤差較高。圖9（b）為PCM閉環(huán)離群值剔除后再進行DGS優(yōu)化的軌跡估計結(jié)果，軌跡合并正確，但部分軌跡出現(xiàn)明顯誤差。圖9（c）為PCM閉環(huán)離群值剔除后再進行本文所提出的MC-DGS位姿圖優(yōu)化算法所得到的估計軌跡，進一步減小了定位誤差。圖9（d）為本文系統(tǒng)的完整實現(xiàn)，軌跡真值和估計軌跡基本重合，軌跡合并效果較好，定位精度高。表1為4種不同算法步驟處理下的軌跡絕對誤差（APE）最大值、平均值以及RMSE誤差結(jié)果。通過比較可以得出，在KITTI 00序列上本文所提出的協(xié)同視覺SLAM系統(tǒng)具有優(yōu)越的定位精度結(jié)果。

圖9 不同算法步驟處理下的協(xié)同定位結(jié)果

表1 不同算法步驟下的APE誤差對比

基于城鎮(zhèn)道路環(huán)境的KITTI 00序列，本文系統(tǒng)與兩種主流視覺SLAM系統(tǒng)以及分布式視覺協(xié)同SLAM系統(tǒng)進行了絕對位姿誤差的RMSE誤差對比，測試均采用雙目視覺模式。由于多數(shù)協(xié)同SLAM算法并不開源，因此僅從論文中獲得了算法在KITTI 00數(shù)據(jù)集上的定位精度，誤差比較結(jié)果如表2所示，本文所提出的系統(tǒng)具有更高的定位精度。

表2 現(xiàn)有單車、協(xié)同視覺SLAM算法誤差對比

帶寬受限是分布式協(xié)同SLAM系統(tǒng)中的關(guān)鍵問題。為了減輕帶寬要求，除了采用輕量的NetVLAD描述子外，也限制了兩次多車間相對位姿變換計算的間隔幀數(shù)，這意味著部分閉環(huán)因為帶寬受限而被舍棄。經(jīng)過實驗表明，在降低了數(shù)據(jù)傳輸?shù)那闆r下，該系統(tǒng)仍然能夠保證較高的定位精度。圖10所示為本系統(tǒng)3個主要功能模塊數(shù)據(jù)傳輸量隨運行時間的變化。

圖10 系統(tǒng)各個模塊的數(shù)據(jù)傳輸量變化過程

基于KITTI 00序列測試，由多車分布式位姿圖優(yōu)化所產(chǎn)生的數(shù)據(jù)傳輸量為0.96 MB，由共視區(qū)域識別所產(chǎn)生的數(shù)據(jù)傳輸量為0.61 MB，由多車間相對位姿估計所產(chǎn)生的數(shù)據(jù)傳輸量為1.19 MB，總計2.76 MB，大大降低了分布式系統(tǒng)的帶寬限制。

為了證明本文所提系統(tǒng)的有效性和環(huán)境適應(yīng)性，本文針對KITTI數(shù)據(jù)集不同環(huán)境、長度的序列進行了無協(xié)同模式以及3車協(xié)同模式定位精度對比測試，結(jié)果如表3所示。通過圖11，可以更直觀地看出本文所提出的協(xié)同SLAM系統(tǒng)相較于無協(xié)同SLAM系統(tǒng)的優(yōu)越性。

圖11 無協(xié)同模式與3車協(xié)同模式定位效果對比

表3 不同環(huán)境、長度測試序列RMSE定位誤差對比

為了測試面向更多數(shù)量車輛的協(xié)同SLAM系統(tǒng)定位表現(xiàn)，模擬了6輛無人車進行基于KITTI 00序列的協(xié)同定位，并繪制了每輛無人車的APE變化曲線。如圖12所示，該曲線變化與文獻［17］所述系統(tǒng)有類似的趨勢和結(jié)論。初始預(yù)測軌跡包含著軌跡的漂移誤差，隨著時間推移，預(yù)測軌跡誤差逐漸增大，當(dāng)多車間產(chǎn)生共視區(qū)域識別關(guān)聯(lián)時，定位軌跡合并、優(yōu)化，誤差逐漸降低，最終穩(wěn)定在2 m左右。由文獻［17］的結(jié)果可知，其APE變化峰值達到26 m左右，最后穩(wěn)定在4 m左右，平均值達到4.8 m。本文系統(tǒng)在相同序列上定位表現(xiàn)更加優(yōu)越，APE的變化峰值不超過8 m，最后穩(wěn)定在2 m左右，平均值為1.91 m。從精度評估上，本文所提出的系統(tǒng)顯然具有更好的定位精度。

圖12 每輛無人車定位誤差隨時間的變化

此后，在長序列KITTI 00、KITTI 08上分別采用了6、10輛無人車進行協(xié)同SLAM測試，其估計軌跡與真值軌跡的對比如圖13所示。本文所提出的協(xié)同視覺SLAM系統(tǒng)在長序列、協(xié)同車輛增多的情況下，估計軌跡與真實軌跡基本重合，定位精度較高，證明系統(tǒng)具有良好的通用性和可擴展性。

圖13 不同數(shù)量無人車協(xié)同軌跡估計結(jié)果

5 結(jié)論

本文提出了一種輕量化、魯棒、定位精度更高的分布式多車協(xié)同視覺SLAM系統(tǒng)。通過實車采集數(shù)據(jù)測試以及公開數(shù)據(jù)集測試，可以得出以下結(jié)論：

（1）本文利用NetVLAD描述子實現(xiàn)共視區(qū)域識別，能夠提升環(huán)境適應(yīng)性，降低數(shù)據(jù)傳輸量。系統(tǒng)在城鎮(zhèn)、高速、校園道路等多個環(huán)境測試中體現(xiàn)出良好的適應(yīng)性和定位精度。對于長度為3.7 km的軌跡降低APE誤差至2.25 m，總數(shù)據(jù)傳輸量僅為2.76 MB。

（2）本系統(tǒng)設(shè)計添加了基于數(shù)據(jù)相似性和結(jié)構(gòu)一致性的閉環(huán)離群值剔除模塊，相較于其他協(xié)同定位系統(tǒng)，保證了多車數(shù)據(jù)關(guān)聯(lián)的可靠性，能夠?qū)崿F(xiàn)正確的軌跡合并與優(yōu)化。

（3）本文所提出的結(jié)合歷史閉環(huán)信息的MCDGS算法，在多車間閉環(huán)數(shù)量增多時，能夠明顯提高協(xié)同系統(tǒng)的定位精度，優(yōu)于現(xiàn)有的DGS優(yōu)化算法。

（4）經(jīng)過實驗驗證，本系統(tǒng)在實車實驗以及KITTI數(shù)據(jù)集測試過程中，均體現(xiàn)了優(yōu)于現(xiàn)有單車SLAM以及協(xié)同SLAM系統(tǒng)的精度效果。