魏 強(qiáng) 張冬梅 范勇生

(1.中國空間技術(shù)研究院, 北京 100098; 2.中國東方紅衛(wèi)星股份有限公司, 北京 100081;3.上海宇航系統(tǒng)工程研究所, 上海 201108)

1 引言

近年來,隨著機(jī)器人領(lǐng)域技術(shù)的不斷發(fā)展,以地面無人車輛和無人機(jī)為代表的智能無人系統(tǒng)在軍事、民用等領(lǐng)域中的應(yīng)用逐漸成為了研究熱點(diǎn),智能無人系統(tǒng)不僅能夠完成澆灌、測繪、巡檢等民用任務(wù),還能有效完成偵察、打擊、追蹤等作戰(zhàn)任務(wù)。 然而隨著任務(wù)需求的增高,對無人系統(tǒng)能力的需求也愈來愈高。

導(dǎo)航系統(tǒng)是無人系統(tǒng)功能實(shí)現(xiàn)的關(guān)鍵組成部分,負(fù)責(zé)向無人系統(tǒng)提供其在參考坐標(biāo)系下的位置、速度、姿態(tài)等關(guān)鍵信息,引導(dǎo)無人系統(tǒng)按照既定路線行進(jìn),導(dǎo)航系統(tǒng)的精確程度和穩(wěn)定性直接影響了后續(xù)任務(wù)規(guī)劃和控制的能力。 傳統(tǒng)的導(dǎo)航系統(tǒng)多依賴于全球?qū)Ш叫l(wèi)星系統(tǒng)(Global Navigation Satellite System,GNSS)和機(jī)載慣性導(dǎo)航元件之間的組合導(dǎo)航,然而,在室內(nèi)環(huán)境或復(fù)雜室外環(huán)境中GNSS 系統(tǒng)會受高大障礙物遮擋以及多路徑效應(yīng)的影響,導(dǎo)致信號解析錯(cuò)誤或信號中斷。 同時(shí)定位與建圖技術(shù)(Simultaneous Localization and Mapping, SLAM)[1]使智能體獲得自定位的能力,以保證在GPS 拒止的情況下無人系統(tǒng)能夠完成自主導(dǎo)航,同時(shí)SLAM 技術(shù)能夠建立一個(gè)全局一致表征的環(huán)境地圖,可用于環(huán)境探索等任務(wù),因此,SLAM 技術(shù)是在GNSS 拒止情況下,使無人系統(tǒng)能夠完成自主導(dǎo)航的關(guān)鍵技術(shù)之一。 按照使用的傳感器不同,SLAM 技術(shù)主要可以分為激光雷達(dá)SLAM 和視覺SLAM 2 種:激光雷達(dá)誤差模型簡單,對于距離估計(jì)更為準(zhǔn)確,并且不受弱光照等環(huán)境因素的影響,但是價(jià)格昂貴,重量大;相較之下,視覺傳感器價(jià)格低廉,設(shè)備輕便,應(yīng)用前景更加廣泛。 目前小范圍環(huán)境下的SLAM 問題研究較為透徹,但單一無人系統(tǒng)在大范圍場景下進(jìn)行探索時(shí)效率較低,并且由于地圖數(shù)據(jù)量的不斷增長,計(jì)算時(shí)間和內(nèi)存占用都會對機(jī)載計(jì)算機(jī)的能力提出嚴(yán)苛的要求。

多機(jī)協(xié)同SLAM 可以將問題分而治之,將整個(gè)任務(wù)的計(jì)算/存儲壓力以及計(jì)算量分?jǐn)偟絽⑴c協(xié)同的各個(gè)無人系統(tǒng)上,大幅提高任務(wù)效率。 因此,多機(jī)協(xié)同SLAM 逐漸成為了當(dāng)前的研究重點(diǎn)。目前多機(jī)協(xié)同視覺SLAM 系統(tǒng)多數(shù)為中心式架構(gòu)[2],中心式架構(gòu)將智能體分為子端和中心端,子端和中心端直接進(jìn)行信息交互,子端智能體執(zhí)行部分SLAM 任務(wù),完成狀態(tài)估計(jì)并建立局部地圖;中心端利用子端傳輸?shù)臄?shù)據(jù),對地圖進(jìn)行融合和優(yōu)化,創(chuàng)建全局一致性表示的全局地圖。 2008年,Castle 等[3]在Klein 等[4]的基礎(chǔ)上使用了2 個(gè)單目視覺傳感器,利用2 個(gè)子端產(chǎn)生的關(guān)鍵幀共同完成地圖構(gòu)建。 基于相似的思路,Zou 等[5]提出了CoSLAM,將多視覺傳感器協(xié)同的數(shù)量規(guī)模擴(kuò)張到了12 個(gè)。 然而隨著參與協(xié)同的傳感器數(shù)目增多,CoSLAM[5]的算力壓力較大,并且對通信系統(tǒng)的帶寬具有較高的要求。 Riazuelo 等[6]通過引入云端服務(wù)器,使子端將關(guān)鍵幀上傳至云端,在云端服務(wù)器上完成局部地圖的構(gòu)建和子端地圖融合,改善了系統(tǒng)的計(jì)算壓力和帶寬壓力。 Forster等[7]使用了減少數(shù)據(jù)傳輸量的方法來緩解通訊壓力,只發(fā)送檢測出的特征點(diǎn)和關(guān)鍵幀幀間的相對位姿。 通過比較子端和中心端計(jì)算的關(guān)鍵幀位姿之間的差異來恢復(fù)絕對位姿。

本文提出了一種大范圍場景下的多無人系統(tǒng)協(xié)同定位與建圖方法,利用多個(gè)無人系統(tǒng)進(jìn)行探索,同時(shí)每個(gè)子端進(jìn)行同時(shí)建圖與定位過程,建立稀疏點(diǎn)云地圖。 之后,根據(jù)區(qū)域重疊等公共信息進(jìn)行局部地圖的融合,建立全局的稀疏點(diǎn)云地圖,完成協(xié)同定位與建圖功能,在提升定位精度與效率的基礎(chǔ)上建立一個(gè)全局一致的環(huán)境地圖表示,以提升無人系統(tǒng)集群的智能程度。

2 多無人系統(tǒng)協(xié)同定位與建圖系統(tǒng)

2.1 系統(tǒng)整體框架

采用中心式架構(gòu),將系統(tǒng)分為兩部分:子端和中心端。 每個(gè)子端搭載一臺單目相機(jī)、通信單元和機(jī)載計(jì)算機(jī);中心端和子端進(jìn)行數(shù)據(jù)交互,接收子端傳輸?shù)年P(guān)鍵幀,地圖點(diǎn)等數(shù)據(jù),完成回環(huán)檢測,地圖融合,全局優(yōu)化等非實(shí)時(shí)操作;并將優(yōu)化后的數(shù)據(jù)發(fā)送回子端,指導(dǎo)子端的導(dǎo)航系統(tǒng)。 整體的系統(tǒng)結(jié)構(gòu)如圖1 所示。

圖1 算法框架圖Fig.1 Diagram of the algorithm framework

1)所有智能體子端都運(yùn)行一個(gè)獨(dú)立的前端視覺里程計(jì),維持一個(gè)規(guī)模較小的局部地圖并將關(guān)鍵幀和地圖點(diǎn)發(fā)送給中心端,中心端接收各子端信息,形成各子端的歷史地圖。

2)中心端通過從所有子端處獲得的關(guān)鍵幀增量式構(gòu)建了一個(gè)關(guān)鍵幀數(shù)據(jù)庫。 用于存儲所有接收到的關(guān)鍵幀的信息,中心端還建立了一個(gè)包含了所有子端局部地圖的地圖堆棧,不同子端地圖的構(gòu)建獨(dú)立進(jìn)行,互不干擾。 重疊區(qū)域檢測過程不斷進(jìn)行,新接收的關(guān)鍵幀通過基于字典機(jī)制的相似度檢測算法不斷檢測子端是否位于過去到達(dá)過的區(qū)域,重疊區(qū)域包括該子端過去到達(dá)過的區(qū)域和其他子端到達(dá)過的區(qū)域。 子端內(nèi)部重疊,則進(jìn)行回環(huán)檢測過程;子端之間重疊,則通過匹配的關(guān)鍵幀建立兩子端局部地圖間的數(shù)據(jù)關(guān)聯(lián),進(jìn)一步進(jìn)行地圖融合,地圖融合后這2 個(gè)地圖被合并,原有的局部地圖從地圖堆棧中刪除,而由它們的融合產(chǎn)生的一個(gè)新的全局地圖將被添加到地圖堆棧中,并與2 個(gè)子端直接關(guān)聯(lián)。 最后根據(jù)得到的全局地圖進(jìn)行全局優(yōu)化。

3)在全局優(yōu)化結(jié)束后,中心端會將優(yōu)化后的相機(jī)位姿發(fā)送回對應(yīng)的子端,子端會更新位姿并將其進(jìn)行局部優(yōu)化,從而更精確的完成協(xié)同SLAM 過程,并獲得更準(zhǔn)確的導(dǎo)航結(jié)果。

2.2 子端算法

子端的主要功能是運(yùn)行視覺里程計(jì),并將跟蹤的路標(biāo)點(diǎn)進(jìn)行存儲,組成局部地圖,按照設(shè)定好的數(shù)據(jù)結(jié)構(gòu)將信息發(fā)送給中心端。 這部分基于ORB-SLAM2[8]進(jìn)行開發(fā),如圖 2 所示。 由于子端局部地圖的大小會隨著運(yùn)行時(shí)間而增長,大范圍協(xié)同SLAM 一般都需要長時(shí)間運(yùn)行,這就對子端的存儲能力提出了挑戰(zhàn)。 由于中心端的地圖堆棧中包含了每個(gè)子端的完整地圖,因此在通信系統(tǒng)暢通的情況下,子端無需保留過多地圖信息。 本方法對各子端建立的局部地圖中關(guān)鍵幀的數(shù)量進(jìn)行限制,以降低在大范圍探索任務(wù)中子端機(jī)載處理器的存儲壓力和計(jì)算壓力。

圖2 跟蹤線程框架圖Fig.2 Block diagram of the tracking process

設(shè)定子端局部地圖中關(guān)鍵幀數(shù)目的上限為N,一旦關(guān)鍵幀數(shù)目超過N,就去除舊的關(guān)鍵幀,同時(shí)將新創(chuàng)建的關(guān)鍵幀添加進(jìn)局部地圖,為了防止出現(xiàn)中心端沒有接收到被刪除的關(guān)鍵幀而造成數(shù)據(jù)丟失的現(xiàn)象,在刪除關(guān)鍵幀之前,子端會和中心端進(jìn)行一次數(shù)據(jù)交互,確認(rèn)中心端是否接收到該關(guān)鍵幀,確認(rèn)后再進(jìn)行刪除,流程如圖3 所示。

圖3 子端局部建圖流程Fig.3 Process of the local mapping

2.3 中心端算法

不同子端局部地圖之間的參考坐標(biāo)系關(guān)系如圖4 所示,假設(shè)有子端a,子端b,子端a 的參考坐標(biāo)系為worlda,子端b 的參考坐標(biāo)系為worldb,ki為子端a 創(chuàng)建的一個(gè)關(guān)鍵幀,它在子端a 的局部地圖中的位姿為Ta,它在子端b 的局部地圖中的位姿Tb=TabTa。 因此要完成子端a,b 之間的地圖融合,就需要得知從worlda到worldb的位姿轉(zhuǎn)換關(guān)系Tab。 如果子端a 和子端b 在觀測過一處公共區(qū)域,那么2 個(gè)局部地圖之間就存在著重疊區(qū)域,可以利用其完成地圖融合與全局優(yōu)化,然而,若2 個(gè)子端沒有觀測過公共區(qū)域,那么在沒有其他信息的情況下無法完成兩地圖之間的數(shù)據(jù)融合。

圖4 參考坐標(biāo)系轉(zhuǎn)換關(guān)系Fig.4 Conversion relation of reference coordinate system

由于局部地圖之間重疊部分的地圖點(diǎn)所對應(yīng)的特征描述子之間相似度較高。 而系統(tǒng)創(chuàng)建了一個(gè)關(guān)鍵幀數(shù)據(jù)庫,其中存儲了所有子端發(fā)送的關(guān)鍵幀,因此,可以為關(guān)鍵幀數(shù)據(jù)庫建立一個(gè)字典樹,在整個(gè)地圖堆棧中檢索有無關(guān)鍵幀匹配,檢測到匹配后利用匹配關(guān)系解算地圖間的相對位姿關(guān)系,實(shí)現(xiàn)全局地圖融合與優(yōu)化。 下面介紹基于重疊區(qū)域檢測的地圖融合優(yōu)化算法。

首先,進(jìn)行重疊區(qū)域檢測。 遍歷新關(guān)鍵幀k的所有共視關(guān)鍵幀,計(jì)算k與每個(gè)共視關(guān)鍵幀的詞袋向量相似度,并得到當(dāng)前幀與其共視關(guān)鍵幀相似度的最低分minScore,在關(guān)鍵幀數(shù)據(jù)庫中尋找與k相似度超過minScore的關(guān)鍵幀,稱為候選匹配關(guān)鍵幀,接下來在候選匹配關(guān)鍵幀中檢測具有連續(xù)性的候選幀,連續(xù)次數(shù)大于3 則認(rèn)為成功匹配,將所有符合條件的關(guān)鍵幀組合起來,就得到了候選關(guān)鍵幀組。

然后,利用求解出的匹配關(guān)系進(jìn)行地圖之間相對位姿的計(jì)算。 ①遍歷匹配候選關(guān)鍵幀集合,篩選出與當(dāng)前幀的匹配特征點(diǎn)大于20 的候選幀集合;②每一個(gè)候選幀都構(gòu)造一個(gè)位姿求解器進(jìn)行迭代求解;③本文的位姿求解算法使用三維相似變換算法(Similarity,Sim3)[9],常用于求取2 組匹配點(diǎn)之間的位姿變換關(guān)系。

利用Sim3 轉(zhuǎn)換通過匹配的地圖點(diǎn)之間的關(guān)系求解出兩關(guān)鍵幀間的相似變換關(guān)系,假設(shè)當(dāng)前幀到匹配幀的坐標(biāo)變換矩陣為Tmc,逆變換矩陣為Tcm,當(dāng)前關(guān)鍵幀坐標(biāo)系下的地圖點(diǎn)Pc對應(yīng)的圖像像素點(diǎn)為pc,同理,有候選幀坐標(biāo)系下與地圖點(diǎn)Pc匹配的地圖點(diǎn)Pl及其對應(yīng)的圖像像素點(diǎn)pl,則重投影誤差為:

設(shè)置兩個(gè)重投影誤差閾值分別為E1和E2,若elc ＜E1且ecl ＜E2,則該組匹配構(gòu)建的三維點(diǎn)被稱為內(nèi)點(diǎn)。 當(dāng)內(nèi)點(diǎn)數(shù)量滿足要求時(shí),視為該候選幀匹配成功。 當(dāng)有一個(gè)候選幀匹配成功,成功解算出相對位姿關(guān)系后,為了優(yōu)化這個(gè)初步解算出的位姿轉(zhuǎn)換關(guān)系,通過將匹配關(guān)鍵幀以及其相連關(guān)鍵幀的地圖點(diǎn)都投影到當(dāng)前關(guān)鍵幀進(jìn)行匹配,以構(gòu)造更多的匹配關(guān)系來進(jìn)一步優(yōu)化Sim3 變換,直到優(yōu)化后的內(nèi)點(diǎn)數(shù)目符合要求后,則視為相似變換解算成功。

最后,進(jìn)行地圖融合與優(yōu)化。 由于上一步中計(jì)算Sim3 轉(zhuǎn)換改變了當(dāng)前關(guān)鍵幀的地圖點(diǎn),因此需要根據(jù)共視關(guān)系更新當(dāng)前幀與其他關(guān)鍵幀之間的連接以及其他關(guān)鍵幀的地圖點(diǎn),對沖突的地圖點(diǎn)進(jìn)行替換或填補(bǔ)。 然后將匹配關(guān)鍵幀組的地圖點(diǎn)投影到局部關(guān)鍵幀組上進(jìn)行地圖點(diǎn)檢查與替換,更新關(guān)鍵幀之間的共視相連關(guān)系,得到因匹配時(shí)地圖點(diǎn)融合而新得到的連接關(guān)系。 接下來進(jìn)行本質(zhì)圖優(yōu)化,優(yōu)化因匹配而更改的連接關(guān)系,最后添加當(dāng)前幀與閉環(huán)匹配幀之間的邊,進(jìn)行全局優(yōu)化,基于地圖點(diǎn)到關(guān)鍵幀的投影關(guān)系,優(yōu)化所有的地圖點(diǎn)和關(guān)鍵幀的位姿。 算法流程圖如圖5所示。

圖5 基于重疊區(qū)域檢測的地圖融合優(yōu)化算法流程Fig.5 Algorithm flowchart of Map Fusion and optimization based on overlapping region Detection

3 實(shí)驗(yàn)結(jié)果

3.1 數(shù)據(jù)集和測試指標(biāo)

由于目前少有針對多機(jī)SLAM 的測試數(shù)據(jù)集,因此常應(yīng)用EuRoC、TUM 等單系統(tǒng)于單一環(huán)境以不同軌跡收集的多個(gè)數(shù)據(jù)集應(yīng)用于多機(jī)協(xié)同SLAM 的性能測試。 EuRoC[10]是SLAM 領(lǐng)域常用的測試數(shù)據(jù)集,常用于SLAM、VIO 等系統(tǒng)的性能測試。 EuRoC 中包含一個(gè)在小型工業(yè)廠房中收集的數(shù)據(jù)集,該廠房如圖 6 所示,廠房結(jié)構(gòu)較復(fù)雜,特征豐富,適合用來對SLAM 算法進(jìn)行性能測試。 該數(shù)據(jù)集中MH01、MH02 和MH03 是由無人機(jī)在光照條件較為良好的情況下進(jìn)行低速飛行采集到的3 組數(shù)據(jù)集,算法測試難度適中。 由于廠房面積較小,因此這3 組數(shù)據(jù)集存在大量的重疊關(guān)系,為多機(jī)協(xié)同SLAM 的地圖融合創(chuàng)造了條件。本文使用MH01,MH02 和MH03 數(shù)據(jù)集作為3 個(gè)無人系統(tǒng)子端對系統(tǒng)進(jìn)行測試,用Rviz[11]來進(jìn)行數(shù)據(jù)可視化。

圖6 廠房圖Fig.6 Machine hall overview

SLAM 系統(tǒng)性能測試中常用的精度指標(biāo)為絕對軌跡誤差(Absolute Trajectory Error,ATE),本文選取絕對軌跡誤差A(yù)TE 作為評測指標(biāo)。

3.2 實(shí)驗(yàn)結(jié)果

本文首先建立系統(tǒng)中心端和3 個(gè)無人系統(tǒng)子端,分別接收MH01,MH02,MH03 等3 組數(shù)據(jù)集發(fā)送的圖像數(shù)據(jù),SLAM 如圖7 所示。 其中綠色的軌跡和點(diǎn)云是MH01 中相機(jī)的軌跡估計(jì)結(jié)果和構(gòu)建的點(diǎn)云,白色的軌跡和點(diǎn)云是MH02 中相機(jī)的軌跡估計(jì)結(jié)果和構(gòu)建的點(diǎn)云,藍(lán)色的軌跡和點(diǎn)云是MH03 中相機(jī)的軌跡估計(jì)結(jié)果和構(gòu)建的點(diǎn)云。 可以發(fā)現(xiàn),3 組獨(dú)立的飛行過程通過本文的協(xié)同SLAM 系統(tǒng)完成了數(shù)據(jù)關(guān)聯(lián),得到了各自軌跡的相對位姿關(guān)系,并共同構(gòu)建了一個(gè)全局一致性表征的稀疏點(diǎn)云地圖。 最終的軌跡估計(jì)結(jié)果如圖8 所示。

圖7 SLAM 結(jié)果圖Fig.7 Diagram of SLAM result

圖8 SLAM 軌跡圖Fig.8 Diagram of SLAM trajectory

為了對比本文提出的協(xié)同SLAM 方法的性能,以O(shè)RB-SLAM2 作為基準(zhǔn)方法,對比協(xié)同情況下軌跡估計(jì)值的誤差和單獨(dú)進(jìn)行SLAM 情況下軌跡估計(jì)值的誤差。 使用SLAM 測評工具EVO 測試了軌跡跟蹤情況,使用ORB-SLAM2 單獨(dú)進(jìn)行SLAM 過程的軌跡跟蹤結(jié)果和應(yīng)用本文方法進(jìn)行協(xié)同SLAM 過程的軌跡跟蹤結(jié)果如圖9 所示,其中圖9(a),圖(b)是在MH01 數(shù)據(jù)集上的軌跡跟蹤結(jié)果,圖9(c),圖9(d)是在MH02 數(shù)據(jù)集上的軌跡跟蹤結(jié)果,圖9(e),圖9(f)是在MH03 數(shù)據(jù)集上的軌跡跟蹤結(jié)果,軌跡跟蹤情況較好。

圖9 本方法和ORB-SLAM2 對各數(shù)據(jù)集的軌跡結(jié)果Fig.9 Trajectory results of ORB-SLAM2 and our methods for various datasets

ORB-SLAM2 算法和本方法在3 組數(shù)據(jù)集上的絕對軌跡誤差對比結(jié)果如表1～表3 所示。 由表可知,2 種算法在數(shù)據(jù)集中的誤差都較低,在MH01 數(shù)據(jù)集中ORB-SLAM2 的誤差均值為0.07 m,本方法的誤差均值為0.071 m,與ORBSLAM2 持平,在MH02 數(shù)據(jù)集中ORB-SLAM2 的誤差均值為0.042 m,本方法的誤差均值為0.035 m,比ORB-SLAM2 小0.007 m,在MH03 數(shù)據(jù)集中ORB-SLAM2 的誤差均值為0.088 m,本方法的誤差均值為0.076 m,比ORB-SLAM2 小0.008 m,但本方法在MH01 中出現(xiàn)了離群點(diǎn)現(xiàn)象,最大誤差較高。 最大誤差,最小誤差,誤差中值,均方誤差等其他各項(xiàng)指標(biāo)均與 ORB-SLAM2持平。 因此可以得出結(jié)論,利用本方法進(jìn)行協(xié)同SLAM 的效果與ORB-SLAM2 算法單獨(dú)進(jìn)行SLAM 的效果基本持平。

表1 MH01 實(shí)驗(yàn)結(jié)果Table 1 Results of MH01

表3 MH03 實(shí)驗(yàn)結(jié)果Table 3 Results of MH03

為驗(yàn)證本文對各子端局部地圖中關(guān)鍵幀的數(shù)量進(jìn)行限制的方法的有效性,本文通過設(shè)定不同的子端局部地圖中關(guān)鍵幀數(shù)目的上限值,進(jìn)行了實(shí)時(shí)性測試。 測試在處理相同數(shù)據(jù)時(shí),單幀跟蹤時(shí)間、建圖單幀迭代時(shí)間、單次數(shù)據(jù)通信時(shí)間在不同N值情況下的數(shù)值,仿真結(jié)果如表4 所示。

表4 不同N 值下的實(shí)時(shí)性測試實(shí)驗(yàn)結(jié)果Table 4 Real time test results for different N values

由實(shí)驗(yàn)結(jié)果可以看出,單幀跟蹤時(shí)間、建圖單幀迭代時(shí)間、單次數(shù)據(jù)通信時(shí)間都隨N值增加而增加,其中建圖時(shí)間增加幅度較大,這是因?yàn)镹值增加直接使得局部地圖規(guī)模增加,線性增加建圖成本。 跟蹤和通信時(shí)間由于子端建圖部分占用了更多的算力,產(chǎn)生了更多的數(shù)據(jù)交互而速度降低,因此驗(yàn)證了本文通過限制子端局部地圖中關(guān)鍵幀的數(shù)目可以有效提升算法的速度,減少子端的內(nèi)存壓力和計(jì)算壓力。

4 結(jié)論

1) 提出了一種協(xié)同視覺SLAM 系統(tǒng)。 采用中心式的架構(gòu)進(jìn)行設(shè)計(jì),對子端局部地圖的大小進(jìn)行限制,并設(shè)計(jì)了基于公共區(qū)域識別的地圖融合算法,最終得到誤差較小的位姿估計(jì)和全局一致性表示的全局地圖。

2) 完成了數(shù)據(jù)集實(shí)驗(yàn)測試。 證明了本方法的精度略優(yōu)于ORB-SLAM2 單機(jī)SLAM 的精度,證明了本方法可以在大范圍環(huán)境下完成無人系統(tǒng)協(xié)同視覺SLAM。