段勝青,熊 智,2,趙 耀,崔雨晨,周帥琳
(1.南京航空航天大學(xué)自動化學(xué)院,南京 211106;2.先進(jìn)飛行器導(dǎo)航、控制與健康管理工業(yè)和信息化部重點實驗室,南京 211106)
多機(jī)器人系統(tǒng)相比于單個機(jī)器人系統(tǒng)在執(zhí)行任務(wù)時具備更高的效率和魯棒性,可應(yīng)用于森林/城市救火、自然災(zāi)害搜救、水下/空間探測和安全巡檢等任務(wù)[1]。多機(jī)器人同步定位與構(gòu)圖(Multi-Robot Simultaneous Localization and Mapping,MR SLAM)技術(shù)作為多機(jī)器人系統(tǒng)的重要組成部分,承擔(dān)著在全球定位系統(tǒng)(Global Positioning System,GPS)衛(wèi)星拒止環(huán)境下的定位和導(dǎo)航的關(guān)鍵作用。由于相機(jī)具有成本低、質(zhì)量小等特點,以相機(jī)為核心的視覺SLAM技術(shù)近年來得到了廣泛的研究和應(yīng)用。目前,單個機(jī)器人SLAM技術(shù)已經(jīng)相對成熟,多機(jī)器人SLAM的主要瓶頸在于通信帶寬限制和計算負(fù)載,而多機(jī)器人SLAM的數(shù)據(jù)交換和計算主要集中在回環(huán)檢測和后端優(yōu)化階段。
回環(huán)檢測是多機(jī)器人SLAM的重要基礎(chǔ)。在SLAM研究的古典時期(classical age)[2],通常采用激光角點作為路標(biāo),并采用最大似然估計的方式對路標(biāo)進(jìn)行數(shù)據(jù)關(guān)聯(lián)[3-4],此類方法需要持續(xù)共享信息矩陣,傳輸數(shù)據(jù)量與狀態(tài)量個數(shù)呈平方增長,數(shù)據(jù)傳輸量大;此外,由于路標(biāo)特征簡單,容易發(fā)生錯誤關(guān)聯(lián),因此此類方法適用于人工環(huán)境,對于復(fù)雜的自然環(huán)境魯棒性較差。A.Howard[5]和L.Carlone[6]等采用機(jī)器人攜帶人工標(biāo)簽的方式實現(xiàn)多機(jī)器人的檢測識別和相對位置量測,特殊的人工標(biāo)簽?zāi)苡行Ы鉀Q回環(huán)檢測中錯誤關(guān)聯(lián)的問題,且僅在檢測到回環(huán)時交換數(shù)據(jù),但要求機(jī)器人在執(zhí)行任務(wù)過程中必須相遇,具有較大使用限制。隨著圖像處理技術(shù)的進(jìn)步,基于特征點的回環(huán)檢測被應(yīng)用于單機(jī)器人SLAM和多機(jī)器人SLAM。P.M.Newman在2006年提出了一種基于尺度不變特征變換(Scale-Invariant Feature Transform,SIFT)特征點的回環(huán)檢測方法[7],通過聚類的方法,采用視覺單詞對場景進(jìn)行描述,從而實現(xiàn)實時地回環(huán)檢測。D.Gálvez-López提出了一種基于FAST(Features from Accelerated Segment Test)特征點和BRIEF(Binary Robust Independent Elementary Features)描述子的詞袋模型[8],實現(xiàn)了更高的實時性,被應(yīng)用于一些主流的SLAM系統(tǒng)[9-10]?;谠~袋模型的回環(huán)檢測雖然能夠減少數(shù)據(jù)的傳輸,但準(zhǔn)確率有待提高。近年來,神經(jīng)網(wǎng)絡(luò)憑借其更好的準(zhǔn)確性和魯棒性開始被應(yīng)用于SLAM回環(huán)檢測。S.Choudhary等通過YOLO(You Look Only Once)神經(jīng)網(wǎng)絡(luò)生成視覺單詞進(jìn)行回環(huán)檢測[11],但以YOLO為代表的用于分類識別的模型主要關(guān)注圖片的局部特征,信息利用率較低。此外,劉強(qiáng)等指出圖片中的環(huán)境建筑等背景對于現(xiàn)有網(wǎng)絡(luò)模型通常屬于非感興趣區(qū)域[12],不利于回環(huán)檢測。R.Arandjelovic等根據(jù)傳統(tǒng)方法中的局部聚合描述子向量(Vector of Local Aggregated Descriptors,VLAD)設(shè)計了一種 NetVLAD神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)[13],并以GPS信息為標(biāo)簽進(jìn)行弱監(jiān)督訓(xùn)練,所生成的描述子包含了圖片的全局信息,相比于傳統(tǒng)方法能以較短的描述子實現(xiàn)更高的準(zhǔn)確率,可有效降低數(shù)據(jù)傳輸,比其他網(wǎng)絡(luò)更加適用于多機(jī)器人SLAM系統(tǒng)。因此,本文將NetVLAD應(yīng)用于多機(jī)器人回環(huán)檢測,但NetVLAD對硬件計算能力有較高要求,難以適用于低成本圖形處理器(Graphics Processing Unit,GPU),硬件資源占用和實時性有待進(jìn)一步優(yōu)化。
多機(jī)器人SLAM按照后端優(yōu)化算法可分為基于濾波的多機(jī)器人SLAM和基于非線性優(yōu)化的多機(jī)器人SLAM。基于濾波的多機(jī)器人SLAM常用的濾波方法有卡爾曼濾波[4,14]、信息濾波[3]和粒子濾波[5-6],但對于以相機(jī)為主要傳感器的視覺SLAM系統(tǒng),大量量測數(shù)據(jù)造成協(xié)方差矩陣和粒子數(shù)目增大,數(shù)據(jù)傳輸增大,降低了算法的實時性。H.Strasdat等指出,在視覺SLAM領(lǐng)域非線性優(yōu)化方法相比于濾波方法具有更好的精度和實時性[15]。對于多機(jī)器人系統(tǒng),理論上可以使每個機(jī)器人收集其他所有機(jī)器人的量測信息,然后通過非線性優(yōu)化估計出所有狀態(tài)量,但實際應(yīng)用中通信帶寬和計算資源都是有限的,因此該方法并不實用。A.Cunningham等提出了一種具備分布式數(shù)據(jù)融合(Decentralized Data Fusion,DDF)功能的平滑與構(gòu)圖方法DDF-SAM[16],該方法通過高斯消元降低了通信和計算負(fù)載。T.Cieslewski等提出了一種基于Gauss-Seidel算法的分布式非線性優(yōu)化方法[17],以解決多機(jī)器人SLAM通信帶寬受限的情況。但以上兩種方法本質(zhì)上是對非線性優(yōu)化過程中求解正規(guī)方程的改進(jìn),而SLAM系統(tǒng)仍需要多次傳輸數(shù)據(jù)和迭代才能逼近最優(yōu)估計值,考慮到網(wǎng)絡(luò)傳輸?shù)难舆t,將會降低系統(tǒng)的實時性和魯棒性。目前,位姿圖優(yōu)化方法因其數(shù)據(jù)少和計算快的優(yōu)點,在多機(jī)器人SLAM領(lǐng)域得到了較多的研究和應(yīng)用[7,18]。
綜上,在多機(jī)器人SLAM領(lǐng)域的回環(huán)檢測和后端優(yōu)化方面,盡管已有很多方法被提出,但無法完全滿足實際場景對系統(tǒng)實時性、魯棒性以及帶寬限制的要求。為此,本文將引入NetVLAD神經(jīng)網(wǎng)絡(luò)模型,在NetVLAD的基礎(chǔ)上提出了一種低數(shù)據(jù)傳輸?shù)膶崟r多機(jī)器人回環(huán)檢測方法,并根據(jù)現(xiàn)有的非線性優(yōu)化理論對多機(jī)器人后端位姿圖優(yōu)化方法進(jìn)行分析和改進(jìn),以減少通信和計算資源占用,并以O(shè)RB-SLAM2為前端實現(xiàn)了一個實時多機(jī)器人SLAM系統(tǒng)。
本文提出系統(tǒng)如圖1所示,分為前端、后端和NetVLAD三部分。前端部分機(jī)器人本地圖片采用ORB SLAM2[9]前端處理,對于外部圖片消息采用4.2節(jié)算法初始匹配后再采用ORB SLAM2前端處理。后端本地位姿圖優(yōu)化采用ORB SLAM2方法,多機(jī)器人位姿圖優(yōu)化和共享采用第5節(jié)所述方法。NetVLAD模塊用于計算關(guān)鍵幀的局部特征聚合描述符(Vector of Local Aggregated Descriptors,VLAD)向量,根據(jù)VLAD的相似度(4.1節(jié))進(jìn)行多機(jī)器人回環(huán)幀匹配。
圖1 本文系統(tǒng)流程框圖
通常SLAM問題通過概率模型來求解。設(shè)狀態(tài)量X,量測量Z,則有
P(X|Z)=P(Z|X)P(X)/P(Z)
(1)
先驗分布P(X)未知時,可看作實數(shù)域上的均勻分布,故可忽略。X的最優(yōu)估計即為
(2)
為了方便描述和分析問題,通常將式(1)和式(2)用因子圖的形式描述,節(jié)點(Vertex)表示待估計狀態(tài)量,邊(Edge)表示量測約束P(zij|xi,xj)。當(dāng)式(2)中的概率分布P(zij|xi,xj)服從正態(tài)分布時,可通過非線性最小二乘方法進(jìn)行求解。SLAM系統(tǒng)中的BA優(yōu)化和位姿圖優(yōu)化均是基于上述原理。
記α、β、γ為機(jī)器人的序號,并且α、β、γ可指代任意同一個機(jī)器人或不同機(jī)器人,狀態(tài)量xγ0αi定義為關(guān)鍵幀F(xiàn)αi在機(jī)器人γ初始關(guān)鍵幀F(xiàn)γ0對應(yīng)相機(jī)坐標(biāo)系下的位姿,記作
(3)
量測量zαiβj定義為當(dāng)前機(jī)器人β的關(guān)鍵幀F(xiàn)βj相對于機(jī)器人α歷史關(guān)鍵幀F(xiàn)αi的位姿,記作
(4)
當(dāng)α=β時,zαiβj采用ORB SLAM2前端進(jìn)行求解;當(dāng)α≠β時,求解方法見本文4.3節(jié)。
用ρ(·)表示李群到李代數(shù)的運(yùn)算,量測約束概率模型定義如下
(5)
其中,Σ表示量測模型的方差;eαiβj對應(yīng)因子圖中的一條邊。此時
(6)
NetVLAD是一種專用于位置識別的神經(jīng)網(wǎng)絡(luò)模型,相比其他神經(jīng)網(wǎng)絡(luò)模型具有更高的準(zhǔn)確率,但對GPU資源要求較高。為了減少該神經(jīng)網(wǎng)絡(luò)模型對GPU資源占用、提高回環(huán)檢測速度,本文直接對文獻(xiàn)[13]訓(xùn)練完成的NetVLAD模型做出參數(shù)調(diào)整而無需重新訓(xùn)練。
4.1.1 conv1_1卷積層合并
NetVLAD網(wǎng)絡(luò)conv1_1層卷積核的維度為3×3×3×64,conv1_1層輸出可表示如下
(7)
其中,Oi表示卷積層第i個輸出通道;Ic對應(yīng)輸入圖像的3個通道;ac∈R為預(yù)處理時減去的一個常數(shù);Kic為卷積核;bi為偏置項;?表示卷積運(yùn)算。本文以灰度圖作為輸入,I1=I2=I3,因此
(8)
可以對conv1_1層進(jìn)行合并
(9)
最終conv1_1卷積核維度變?yōu)?×3×1×64。
4.1.2 WPCA層裁剪
NetVLAD模型將vgg16網(wǎng)絡(luò)的輸出經(jīng)過NetVLAD core處理后,通過加權(quán)主成分分析(Weighted Principal Component Analysis,WPCA)將輸出降維至4096維[13]??紤]到PCA矩陣中越靠后的列包含信息越少,本文將WPCA層32768×4096的矩陣裁剪為32768×128維,只保留矩陣的前128列,并將偏置項裁剪為128×1維,從而極大地減少參數(shù)數(shù)目,降低計算量。
4.1.3 相似度計算
圖片U和V根據(jù)上面所述改進(jìn)NetVLAD進(jìn)行計算,得到歸一化128維向量u和v,相似度定義如下
(10)
S(u,v)越大,圖片越可能構(gòu)成回環(huán)。
為減少數(shù)據(jù)傳輸,本文所述方法僅發(fā)送左目相機(jī)中圖片特征點像素坐標(biāo)和對應(yīng)單詞Id,以量測相對于其他機(jī)器人歷史關(guān)鍵幀的位姿。
機(jī)器人在接收到上述圖片信息后,由于缺失特征點的描述子信息,采用ORB SLAM2前端會產(chǎn)生大量誤匹配(見第6節(jié)圖4),導(dǎo)致相對位姿解算失敗。為此,本文提出了以下方法用于解決描述子缺失情況下的特征匹配。
設(shè)圖片i寬和高分別為W和H,詞袋為Bi,特征點集合為Pi,對于特征點p∈Pi,對應(yīng)單詞為wp,像素坐標(biāo)為cp=[xp,yp]T,記τd=(W2+H2)/4,匹配算法描述如下:
通過上述方法可以得到誤匹配較少的匹配結(jié)果,此后通過RANSAC PnP(Random Sample Consensus Perspective-n-Point)方法求解初始位姿,再由初始位姿通過反投影方式增加匹配點對。
為減少數(shù)據(jù)傳輸,回環(huán)檢測分為五步:
1)計算機(jī)器人α新增關(guān)鍵幀圖片U的NetVLAD特征向量u,并廣播給其他機(jī)器人;
2)任意其他機(jī)器人β收到u后,與本地NetVLAD特征庫進(jìn)行對比,如果存在特征向量為v的關(guān)鍵幀圖片V滿足S(u,v)>τmatch(τmatch為相似度閾值),則將圖片V的特征點信息(坐標(biāo)、單詞)發(fā)送給機(jī)器人α,否則結(jié)束回環(huán)檢測;
3)采用算法1進(jìn)行特征點匹配;
4)機(jī)器人α用RANSAC PnP方法求解U與V的相對位姿zαUβV,如果求解成功,則認(rèn)為U和V對應(yīng)同一地點,并廣播機(jī)器人α和β的關(guān)鍵幀匹配消息;
5)每個機(jī)器人通過接收關(guān)鍵幀匹配消息判斷自身是否與其他機(jī)器人軌跡構(gòu)成回環(huán)或間接回環(huán)(例如A?B?C?A)。
先驗分布P(X)未知時,因子圖只存在雙邊約束,可將因子圖按照下面的規(guī)則分割為多個子圖:
1)構(gòu)成一個環(huán)路的節(jié)點和邊屬于同一子圖;
2)有至少一條公共邊的2個子圖應(yīng)合并為一個子圖;
3)不經(jīng)過任何環(huán)路的邊和其連接2個節(jié)點構(gòu)成一個子圖。
約定依照上述規(guī)則所得的子圖稱為回環(huán)子圖。記完整因子圖為G,回環(huán)子圖Si∈G,經(jīng)過上述分割,子圖間無法再形成環(huán)路,本文參考iSAM2[19]用貝葉斯樹表示各個子圖的鄰接關(guān)系(如圖2所示)。相比于iSAM2的貝葉斯樹,本文中樹的節(jié)點為非完全子圖,無需三角化,每次檢測到新的回環(huán)時只需通過并查集操作合并子圖,從而簡化計算。
圖2 位姿圖(左)和貝葉斯樹(右)
觀察式(5)發(fā)現(xiàn)
=ρ(zβjαi·(Txγ0αi)-1·(Txγ0βj))
(11)
可知式(6)的最優(yōu)解位姿變換之后仍為它的一組最優(yōu)解。因此可通過3個步驟求解Xopt:
1)根據(jù)上述規(guī)則將因子圖劃分為回環(huán)子圖;
阿奇霉素與紅霉素有交叉耐藥性。對阿奇霉素耐藥最常見的機(jī)制是通過甲基化修飾23S rRNA靶點。核糖體修飾可能決定對其他大環(huán)內(nèi)酯類、林可酰胺類和鏈陽菌素B(MLS B表型)的交叉耐藥性。
2)根據(jù)式(6)求解各個子圖的最優(yōu)估計;
3)根據(jù)式(11)特性,按貝葉斯樹從根節(jié)點向下的順序依此將各個子圖的解做整體位姿變換,以確保相連子圖的共享變量數(shù)值一致性,即可求得整個因子圖G的最優(yōu)解。
以圖2為例,左圖為原始因子圖,右圖為所得貝葉斯樹,右圖每個節(jié)點表示一個回環(huán)子圖,每條邊上的字母表示2個相鄰子圖的共有節(jié)點。分別求出子圖ab、bcdef、ci、fgh、ij的最優(yōu)解,再依次對bcdef、ci、ij、fgh進(jìn)行位姿變換,以保證狀態(tài)變量b、c、f、i的一致性,即可求得因子圖最優(yōu)解。
根據(jù)上述特性,在多機(jī)器人位姿圖優(yōu)化時,只需要發(fā)送與其他機(jī)器人構(gòu)成回環(huán)子圖的部分邊和節(jié)點即可,從而實現(xiàn)了節(jié)省帶寬的目的。
多機(jī)器人后端優(yōu)化采用如下策略:
1)當(dāng)檢測到自身與其他機(jī)器人構(gòu)成回環(huán)時,廣播位姿圖給其他機(jī)器人,且僅廣播與其他機(jī)器人構(gòu)成回環(huán)子圖的邊和節(jié)點;
2)廣播時,如果在上次廣播到本次廣播期間機(jī)器人沒有發(fā)生內(nèi)部閉環(huán)和BA優(yōu)化,僅廣播新增位姿圖節(jié)點和邊,已廣播的節(jié)點不需要再傳輸;
3)當(dāng)機(jī)器人發(fā)生內(nèi)部閉環(huán)和BA優(yōu)化時,對內(nèi)部閉環(huán)優(yōu)化影響到回環(huán)子圖上的邊和節(jié)點進(jìn)行重新廣播;
本文系統(tǒng)前端和后端部分由C++編程開發(fā),NetVLAD采用Python3和Tensorflow實現(xiàn),每個機(jī)器人都運(yùn)行有NetVLAD回環(huán)檢測和SLAM前后端2個進(jìn)程,進(jìn)程間通過機(jī)器人操作系統(tǒng)(Robot Operating System, ROS)收發(fā)消息,機(jī)器人間也通過ROS通信,并通過廣播的方式進(jìn)行信息發(fā)送,從而實現(xiàn)一對多信息傳遞。
采用混淆矩陣的方式檢驗回環(huán)檢測效果,并將混淆矩陣用灰度圖的形式顯示,矩陣的行列對應(yīng)KITTI00數(shù)據(jù)集中圖片編號(0~4540),圖片相似度越高,對應(yīng)像素的顏色越黑。圖3(a)中矩陣計算規(guī)則如下
(12)
ti和tj表示圖片對應(yīng)的真實位置,即二者距離大于4m時aij為0(白色),否則為1(黑色)。圖3(b)表示原始NetVLAD網(wǎng)絡(luò)計算所得特征向量根據(jù)式(10)計算所得的混淆矩陣,圖3(c)表示根據(jù)第5節(jié)修改后NetVLAD網(wǎng)絡(luò)計算所得特征向量根據(jù)式(10)計算所得的混淆矩陣,圖3(d)為對圖3(c)的結(jié)果設(shè)置0.85閾值的二值化處理結(jié)果,此時圖3(d)的準(zhǔn)確率為99.44%,召回率為39.16%,準(zhǔn)確度能夠滿足SLAM需求,召回率雖然偏低,但SLAM系統(tǒng)在一段連續(xù)圖片序列中只需要找到一個回環(huán)幀即可,因此仍能滿足回環(huán)檢測的需求。
(a)
分別用不同顯卡設(shè)備搭載NetVLAD網(wǎng)絡(luò)處理KITTI00數(shù)據(jù)集(4541張圖片),對比結(jié)果如表1和表2所示。
表1 Nvidia GTX1650Ti 4G測試結(jié)果
表2 GTX965M 2G 測試結(jié)果
通過上述結(jié)果可知,改進(jìn)后的NetVLAD擁有更快的計算速度和更少的資源占用,可應(yīng)用于低成本GPU,同時能夠滿足回環(huán)檢測準(zhǔn)確率和實時性需求。
圖4和圖5所示分別為ORB SLAM2和本文算法1對KITTI00數(shù)據(jù)集圖片000391和002442的匹配結(jié)果,可知ORB SLAM2前端匹配算法在無描述子情況下,當(dāng)2幅待匹配圖片存在視角偏移時會產(chǎn)生較多誤匹配,影響機(jī)器人的定位結(jié)果(參見圖6),而算法1僅存在少量誤匹配,不影響系統(tǒng)后續(xù)運(yùn)行。
圖4 ORB SLAM2 特征匹配結(jié)果
圖5 本文方法特征匹配結(jié)果
將KITTI00數(shù)據(jù)集按照圖片序號平均劃分為4段,分別配置在4臺i7-6700HQ/Nvidia GTX965M 2G筆記本電腦上,配置4個多機(jī)器人SLAM系統(tǒng)節(jié)點,并且機(jī)器人初始位置未知,最終運(yùn)行結(jié)果如圖6、圖7和表3、表4所示。
圖6 匹配失敗情況下的MR SLAM軌跡
圖7 本文方案軌跡
表3 本文方案與ORB SLAM2精度對比
表4 本文方案與ORB SLAM2實時性對比
通過以上數(shù)據(jù)可以看出,本文提出的多機(jī)器人SLAM系統(tǒng)能夠?qū)崿F(xiàn)與ORB SLAM2系統(tǒng)同樣數(shù)量級的定位精度和實時性,多機(jī)器人SLAM的定位精度要略高于ORB SLAM2,其原因來自于多機(jī)器人SLAM僅進(jìn)行了位姿圖優(yōu)化,而未進(jìn)行BA優(yōu)化,由于ORB SLAM2前端特征點匹配存在誤差,BA優(yōu)化帶來了過擬合效應(yīng)。表4中圖片處理時間均值小于表2中的均值,原因是本文方案中NetVLAD僅對關(guān)鍵幀進(jìn)行處理。
圖8所示為部署的4個機(jī)器人節(jié)點在運(yùn)行期間的數(shù)據(jù)傳輸統(tǒng)計圖,可見在未檢測到回環(huán)幀時,帶寬保持在10kb/s以下,在發(fā)現(xiàn)回環(huán)時發(fā)送圖片數(shù)據(jù)會占用25kb/s的帶寬,位姿圖占用帶寬與機(jī)器人所經(jīng)過路徑長度有關(guān),145s時刻由于多個機(jī)器同時構(gòu)成回環(huán),造成了位姿圖流量高峰,但不超過40kb/s。整個過程傳輸數(shù)據(jù)總量為1.18MB,位姿圖消息105.16kB,圖片消息265.22kB(11張圖片),其他消息(主要為NetVLAD)814.13kB,證明了本文提出方法可有效降低帶寬負(fù)載。
圖8 運(yùn)動過程中每秒數(shù)據(jù)傳輸量
本文提出了一種低帶寬占用的多機(jī)器人SLAM系統(tǒng)。試驗結(jié)果表明:
1)本文對NetVLAD神經(jīng)網(wǎng)絡(luò)的改進(jìn)可有效提高NetVLAD計算速度和回環(huán)檢測實時性,并降低顯存占用和通信帶寬占用。
2)本文提出的圖片消息壓縮方法和對應(yīng)的特征點匹配算法能極大地減少數(shù)據(jù)發(fā)送,并保證匹配和量測的魯棒性。
3)本文提出的多機(jī)器人視覺SLAM系統(tǒng)能夠達(dá)到和單機(jī)器人ORB SLAM2同樣的精度和實時性,同時有效降低了機(jī)器人之間的數(shù)據(jù)傳輸。
4)本文的圖片消息采用發(fā)送所有特征點的方式,對于通信帶寬限制較大的場景,應(yīng)該對特征點進(jìn)行進(jìn)一步篩選。位姿圖共享也需要進(jìn)行合理規(guī)劃,分批發(fā)送,避免造成流量高峰時的擁堵情況。