多無人系統協同視覺SLAM 算法

魏 強 張冬梅 范勇生

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

1 引言

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

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

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

本文提出了一種大范圍場景下的多無人系統協同定位與建圖方法,利用多個無人系統進行探索,同時每個子端進行同時建圖與定位過程,建立稀疏點云地圖。 之后,根據區域重疊等公共信息進行局部地圖的融合,建立全局的稀疏點云地圖,完成協同定位與建圖功能,在提升定位精度與效率的基礎上建立一個全局一致的環境地圖表示,以提升無人系統集群的智能程度。

2 多無人系統協同定位與建圖系統

2.1 系統整體框架

采用中心式架構,將系統分為兩部分:子端和中心端。 每個子端搭載一臺單目相機、通信單元和機載計算機;中心端和子端進行數據交互,接收子端傳輸的關鍵幀,地圖點等數據,完成回環檢測,地圖融合,全局優化等非實時操作;并將優化后的數據發送回子端,指導子端的導航系統。 整體的系統結構如圖1 所示。

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

1)所有智能體子端都運行一個獨立的前端視覺里程計,維持一個規模較小的局部地圖并將關鍵幀和地圖點發送給中心端,中心端接收各子端信息,形成各子端的歷史地圖。

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

3)在全局優化結束后,中心端會將優化后的相機位姿發送回對應的子端,子端會更新位姿并將其進行局部優化,從而更精確的完成協同SLAM 過程,并獲得更準確的導航結果。

2.2 子端算法

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

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

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

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

2.3 中心端算法

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

圖4 參考坐標系轉換關系Fig.4 Conversion relation of reference coordinate system

由于局部地圖之間重疊部分的地圖點所對應的特征描述子之間相似度較高。 而系統創建了一個關鍵幀數據庫,其中存儲了所有子端發送的關鍵幀,因此,可以為關鍵幀數據庫建立一個字典樹,在整個地圖堆棧中檢索有無關鍵幀匹配,檢測到匹配后利用匹配關系解算地圖間的相對位姿關系,實現全局地圖融合與優化。 下面介紹基于重疊區域檢測的地圖融合優化算法。

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

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

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

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

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

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

3 實驗結果

3.1 數據集和測試指標

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

圖6 廠房圖Fig.6 Machine hall overview

SLAM 系統性能測試中常用的精度指標為絕對軌跡誤差(Absolute Trajectory Error,ATE),本文選取絕對軌跡誤差ATE 作為評測指標。

3.2 實驗結果

本文首先建立系統中心端和3 個無人系統子端,分別接收MH01,MH02,MH03 等3 組數據集發送的圖像數據,SLAM 如圖7 所示。 其中綠色的軌跡和點云是MH01 中相機的軌跡估計結果和構建的點云,白色的軌跡和點云是MH02 中相機的軌跡估計結果和構建的點云,藍色的軌跡和點云是MH03 中相機的軌跡估計結果和構建的點云。 可以發現,3 組獨立的飛行過程通過本文的協同SLAM 系統完成了數據關聯,得到了各自軌跡的相對位姿關系,并共同構建了一個全局一致性表征的稀疏點云地圖。 最終的軌跡估計結果如圖8 所示。

圖7 SLAM 結果圖Fig.7 Diagram of SLAM result

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

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

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

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

表1 MH01 實驗結果Table 1 Results of MH01

表3 MH03 實驗結果Table 3 Results of MH03

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

表4 不同N 值下的實時性測試實驗結果Table 4 Real time test results for different N values

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

4 結論

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

2) 完成了數據集實驗測試。 證明了本方法的精度略優于ORB-SLAM2 單機SLAM 的精度,證明了本方法可以在大范圍環境下完成無人系統協同視覺SLAM。