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

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

前言

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

視覺傳感器因其硬件廉價(jià)、結(jié)構(gòu)簡單、安裝方便、體積小、質(zhì)量輕、特征豐富等優(yōu)點(diǎn)成為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ù)端的計(jì)算能力要求較高。這種框架不適用于自動駕駛車輛獨(dú)立完成定位建圖任務(wù)。分布式系統(tǒng)能夠更好地解決集中式系統(tǒng)所存在的問題，對于單個節(jié)點(diǎn)的加入與退出均具有較強(qiáng)的魯棒性和適應(yīng)性。自2013年以來，分布式協(xié)同SLAM已經(jīng)取得了部分研究成果［14-15］，但仍存在受限于帶寬的問題［16］。

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

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

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

（1）通過引入NetVLAD全局圖像描述子，實(shí)現(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ǔ)上，提出了一種改進(jìn)的DGS優(yōu)化算法（multi-closure DGS，MC-DGS）。該算法基于共視區(qū)域閉環(huán)構(gòu)建多車間相對位姿變換約束，在閉環(huán)數(shù)量增多時(shí)，利用歷史閉環(huán)信息構(gòu)建里程計(jì)約束，通過位姿圖優(yōu)化求解，提升了多車協(xié)同定位效果；

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

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

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

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

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

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

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

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

為了保證在有可能感知混疊的情況下實(shí)現(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ò)實(shí)現(xiàn)場景識別的先進(jìn)性結(jié)果。通過端到端訓(xùn)練，能夠在一定程度上應(yīng)對光線變化，彌補(bǔ)傳統(tǒng)圖像描述子的不足，提高視覺系統(tǒng)對于光照變化的魯棒性。與此同時(shí)，NetVLAD描述子十分緊湊，采用描述子前128維描述一幀圖像即可良好地完成共視區(qū)域識別任務(wù)，大小僅占用1 Kb。應(yīng)用于本系統(tǒng)中，能夠降低數(shù)據(jù)交換量，適用于帶寬受限的分布式協(xié)同SLAM系統(tǒng)。

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

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

2.2 幾何驗(yàn)證模型

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

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

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

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

ORB-SLAM2前端將關(guān)鍵幀以簇的形式進(jìn)行管理。當(dāng)兩個關(guān)鍵幀所觀測到的相同地圖點(diǎn)滿足一定數(shù)量要求時(shí)，構(gòu)成共視關(guān)鍵幀的關(guān)系。其中，共視程度最高的共視關(guān)鍵幀被稱為該關(guān)鍵幀的父關(guān)鍵幀。同時(shí)，單車內(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ù)相似性計(jì)算相似度評分原理

假設(shè)當(dāng)前車輛為α，對于多車間閉環(huán)約束zi=<αi，βj>，計(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>，可以計(jì)算得到s(zk)。相似度評分越高，代表圖像越相似，正確閉環(huán)概率就越高。

綜合考慮數(shù)據(jù)相似性和結(jié)構(gòu)一致性，定義多車間共視區(qū)域閉環(huán)質(zhì)量評分，由一對無人車間閉環(huán)候選可計(jì)算出閉環(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)圖，處于最大團(tuán)中的閉環(huán)將被最終確定為用于位姿圖優(yōu)化的正確多車間閉環(huán)。

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

多車間閉環(huán)產(chǎn)生于經(jīng)過同一地點(diǎn)，因此閉環(huán)關(guān)鍵幀需具備數(shù)據(jù)相似性。同時(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í)具備數(shù)據(jù)相似性和結(jié)構(gòu)一致性條件的閉環(huán)，閾值越大，閉環(huán)要求越嚴(yán)格，數(shù)量越少，正確率越高。該方案容易計(jì)算，并且在不需要額外通信負(fù)擔(dān)的情況下，綜合利用了視覺里程計(jì)前端提供的數(shù)據(jù)相似性結(jié)果以及多車間閉環(huán)所構(gòu)成的結(jié)構(gòu)一致性結(jié)果。

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

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

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

首先，考慮單車視覺SLAM問題的表現(xiàn)形式，定義時(shí)間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)的位姿軌跡，可以實(shí)現(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)成的代價(jià)；Finter(Xall)表示所有多車間約束所構(gòu)成的代價(jià)，通過最小化兩種代價(jià)，獲得多車間兩兩一致的位姿估計(jì)結(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)化精度的作用，提出了一種改進(jìn)的DGS算法，即MC-DGS。當(dāng)一對無人車間存在不止一對閉環(huán)時(shí)，該方案能夠利用歷史閉環(huán)信息，提高位姿估計(jì)的一致性和精準(zhǔn)性。

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

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

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

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

4 實(shí)驗(yàn)

4.1 實(shí)車測試

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

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

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

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

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

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

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

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

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

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

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

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

圖9所示為不同算法步驟下的3車協(xié)同定位結(jié)果。圖9（a）為僅進(jìn)行DGS優(yōu)化的軌跡估計(jì)結(jié)果，由于未進(jìn)行多車間閉環(huán)離群值剔除，定位軌跡合并錯誤，誤差較高。圖9（b）為PCM閉環(huán)離群值剔除后再進(jìn)行DGS優(yōu)化的軌跡估計(jì)結(jié)果，軌跡合并正確，但部分軌跡出現(xiàn)明顯誤差。圖9（c）為PCM閉環(huán)離群值剔除后再進(jìn)行本文所提出的MC-DGS位姿圖優(yōu)化算法所得到的估計(jì)軌跡，進(jìn)一步減小了定位誤差。圖9（d）為本文系統(tǒng)的完整實(shí)現(xiàn)，軌跡真值和估計(jì)軌跡基本重合，軌跡合并效果較好，定位精度高。表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)進(jìn)行了絕對位姿誤差的RMSE誤差對比，測試均采用雙目視覺模式。由于多數(shù)協(xié)同SLAM算法并不開源，因此僅從論文中獲得了算法在KITTI 00數(shù)據(jù)集上的定位精度，誤差比較結(jié)果如表2所示，本文所提出的系統(tǒng)具有更高的定位精度。

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

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

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

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

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

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

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

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

5 結(jié)論

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

（1）本文利用NetVLAD描述子實(shí)現(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è)計(jì)添加了基于數(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ù)量增多時(shí)，能夠明顯提高協(xié)同系統(tǒng)的定位精度，優(yōu)于現(xiàn)有的DGS優(yōu)化算法。

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