一種多無人機協同定位與稠密地圖構建算法

唐嘉寧, 陳偉, 陳云浩*, 李玉亭, 胡敏森, 許俊鋒

(1.云南民族大學電氣信息工程學院, 昆明 650000; 2.云南民族大學無人自主系統研究所, 昆明 650000)

同時定位與地圖構建(simultaneous localization and mapping, SLAM)指的是無人機在未知環境中運動時增量地構建自身周圍的環境地圖,同時使用已構建好的地圖來評估無人機自身的位姿[1],該技術在增強現實(augmented reality,AR)、三維重建、無人機、無人車等領域得到越來越廣泛的應用[2-3]。視覺SLAM(visual SLAM,VSLAM)指的是以攝像頭作為無人機的傳感器,相比于傳統的激光雷達等傳感器,攝像頭以其價格便宜、重量較輕等優點受到廣大研究者的青睞[4]。

VSLAM比較成熟的算法主要包括ORB-SLAM2[5](oriented fast and rotated brief-SLAM2)、VINS-Mono[6](Monoculor visual-inertial state) 、Kimera[7]、ORB-SLAM3[8]等,這些算法在單無人機的應用中表現出了較高的精度,但多數尚未應用到多無人機協同定位與地圖構建中。其中,VINS-MONO在眾多的SLAM算法中具有定位精度較高、支持相機與imu融合、支持回環檢測與重定位等優點,但也存在著建圖效率不高、構建的稀疏點云地圖對環境的描述信息較少的缺點。

在復雜的環境下,單無人機工作的效率和魯棒性往往達不到要求,此時需要多無人機協同完成工作,因此多無人機SALM逐漸成為研究熱點[9]。CCM-SLAM[10]系統采用集中式的架構,服務器與客戶端通過無線網絡進行連接,每個無人機作為客戶端可獨立進行定位與建圖,直到識別到重疊區域,服務器才進行地圖融合、全局優化等工作。CORB-SLAM[11]系統采用集中式框架,每個機器人都會單獨執行ORB-SLAM2算法來建立各自的局部地圖并傳送給中央處理器。中央處理器會融合來自機器人的局部地圖,然后把融合后的全局一致性地圖傳送回每個機器人。Schmuck 等[12]提出了基于ORB-SLAM3的中心化協同SLAM框架,可以用于多達十二架無人機。以上多無人機SLAM算法在定位精度方面顯著提升,但是其構建的稀疏點云地圖并不能滿足無人機的自主導航等需求。

稀疏點云地圖對環境的描述在一些數據集中可以滿足無人機的定位要求,但是無法應用于無人機的自主導航。稠密點云地圖雖可以滿足無人機的自主導航的要求,但是稠密點占用的存儲過大,并且沒有距離信息。歐幾里得符號距離場(euclidean signed distance functions,ESDF)地圖中的每個體素都包含距離信息和梯度信息,在無人機在線運動規劃中具有重要作用。Voxblox[13]算法中提出了一種從TSDFs(truncated signed distance function)增量的構建ESDFs((euclidean signed distance functions)的方法,其在單個CPU上驗證了方法的實時性。Bartolomei等[14]提出了一種分層的多無人機全局規劃算法,其將Voxblox算法做出改進并應用在所提出的多無人機全局規劃算法中。

針對單無人機工作效率低、魯棒性低且多無人機SLAM系統構建的稀疏點云無法用于自主導航問題,現提出一種集中式多無人機協同定位與稠密地圖構建算法(多無人機協同SLAM算法)。算法以VINS-MONO算法作為多無人機協同SLAM算法框架的客戶端,以Voxblox建圖模塊和地圖融合與位姿圖優化模塊組成多無人機協同SLAM算法框架的服務端。為了建立各個客戶端之間的連接,建立了統一的世界坐標系,并使用回環閉合獲得了多無人機之間的額外約束。提出使用雙向檢測匹配法對關鍵幀的特征點做匹配,并使用PROSAC算法剔除特征點的誤匹配,從而提高回環閉合中特征匹配的準確度,以提升多無人機之間的定位精度。進一步結合Voxblox算法,構建了可供五架無人機協同導航的全局稠密地圖。

1 VINS-MONO算法

VINS-MONO算法是由Qin等[6]提出的一種魯棒且通用單目視覺慣性狀態估計器,算法框架如圖1所示。算法主要包括四個大方面:初始化模塊、觀測預處理模塊、視覺慣性里程計(visual inertial odometry,VIO)融合模塊和全局優化模塊。觀測預處理模塊是同時對單目相機和慣性測量單元(intermediate maintenance unit,IMU)進行觀測數據的預處理,單目視覺預處理是利用光流法對上一幀與當前幀之間的特征點追蹤,并于當前幀提取新的特征點和挑選新的關鍵幀,IMU預處理是對IMU預積分進行計算。初始化模塊主要分為視覺慣導對齊和純視覺初始化。VIO融合模塊是當初始化模塊完成后,用輸入的觀測數據構建新的約束,即局部建圖過程,同時采用滑窗機制來管理局部建圖的節點(新節點加入,最早進入的節點將被刪除)。全局優化模塊是對算法在連續建圖過程累計的誤差進行消除。

2 改進的多無人機協同SLAM算法

2.1 系統總體框架

算法的系統架構由多個無人機代理和一個中央服務器組成,如圖2所示。每個代理均為攜帶相機和IMU傳感器的無人機,可以分別運行VINS-MONO算法,同時立體攝像機可以構建立體點云。為了減輕向服務器傳輸立體點云的帶寬需求,通過體素濾波融合從后續幀獲取的立體點云以對立體點云進行壓縮。然后,將里程計、關鍵幀和關鍵幀中的三維地標以及立體點云一起發送給服務器。服務器負責部分計算成本更高的任務,如基于全局優化的傳感器融合、使用Voxblox模塊構建全局稠密地圖等任務。

2.2 地圖融合與位姿圖優化

對于每個代理,都有獨立的地圖坐標系,且在其獨立坐標系下執行SLAM過程,當檢測到回環閉合時執行全局優化。為了建立多個代理之間的聯系,使用四種不同類型的坐標系來表示坐標轉換,如圖3所示。圖3中世界坐標系在所有代理中都是唯一不變的,對于每個代理,有一個局部坐標系和里程計坐標系,用于VIO的漂移校正。

圖1 VINS-MONO算法框架圖Fig.1 VINS-MONO algorithm block diagram

圖2 系統總體框架圖Fig.2 System overall framework diagram

圖3 坐標變換圖Fig.3 Coordinate transformation diagram

2.2.1 殘差計算

為了優化坐標系之間的變換,根據變換表達測量值,以形成殘差。算法中使用的第一類殘差為先驗殘差,其定義式為

(1)

(2)

算法中使用的第二類殘差是相對測量約束,其代表兩個關鍵幀之間相對測量位姿與基于狀態變量的預測轉換之間的誤差,定義式為

(3)

式(3)中:TMS為關鍵幀坐標系到局部地圖坐標系的轉換;TOS為關鍵幀坐標系到視覺慣性里程計坐標系的約束;j、p為不同關鍵幀。關鍵幀位姿與視覺慣性里程計位姿之間的關系為

(4)

式(4)中:TMO為視覺慣性里程計坐標系到局部地圖坐標系的轉換。

2.2.2 回環閉合

為了在代理與代理之間軌跡建立額外的約束,利用視覺詞袋模型(bags of binary words,BoW)[15]執行視覺回環閉合檢測。為了可以在代理與代理之間執行回環閉合檢測,需要創建一個在所有代理之間共享單詞的數據庫,通過查詢數據庫中相似的候選對象來檢測新的循環閉包,并對最佳候選對象進行基于特征(binary robust indenpendent elementary features,BRIEF)描述符的雙向檢測匹配法進行匹配,并使用漸進一致采樣法(progressive sampling consensus,PROSAC)算法剔除誤匹配。如果找到足夠的內點數,則在不同代理之間進行回路閉合檢測的情況下,通過最小化已建立對應的重投影誤差來優化相對位姿。

2.2.3 改進的多無人機協同全局優化

針對多無人機協同工作時的全局優化,提出當在多個代理之間的軌跡檢測到回環閉合時,全局優化會優化所有連接的位姿和地圖轉換。如在一個包含三個代理的算法中,如果在代理1和代理2之間的軌跡中檢測到回環閉合,則與代理1和代理2相關的所有變換都會得到優化,而代理3內的變換保持獨立。全局優化的目標表達式為

(5)

式(5)針對室外環境,在視覺的基礎上融合了全球定位系統(global positioning system,GPS)以增加定位精度。本文算法主要針對室內環境下的定位與建圖,室內環境下無GPS信號,考慮到增加GPS后在室內環境下并不能增加定位精度并增加計算量,對公式做出了改進,剔除掉融合的GPS部分減少計算量,公式為

(6)

優化變量x表示為

x=[x1,x2,…,x|Z|]

(7)

每個x包含如下轉換

(8)

式(8)中:W代表世界坐標系;M代表地圖坐標系;S代表關鍵幀坐標系。回環閉合約束的殘差計算公式為

(9)

2.3 多無人機協同建圖

立體點云計算模塊接收數據集的圖像后,將計算得到的立體點云傳輸給數據處理模塊,數據處理模塊負責將立體點云壓縮,并將VINS-MONO算法得到的關鍵幀和位姿傳輸給服務端,服務端的地圖融合與位姿圖優化模塊對位姿進行優化,之后Voxblox模塊利用立體點云構建全局一致的TSDF地圖和ESDF地圖。

3 改進的回環閉合算法

傳統的回環閉合的方案使用單向檢測的匹配算法加隨機一致性采樣(random sample consensus,RANSAC)算法對特征點進行處理,其準確性不夠高。因此提出使用雙向檢測匹配法對特征點進行匹配,并結合PROSAC算法剔除誤匹配,來提高特征點匹配的準確度,以提升回環閉合的準確性。

第一步：利用雙向檢測匹配法對特征點進行匹配。首先是前向匹配:計算第一幀圖像中某一個特征點xi描述子與第二幀圖像所有特征點y=(y1,y2,…,ym)描述子之間的漢明距離,對所有的距離進行排序,取距離最近的一個作為匹配點。然后是后向匹配:計算第二幀圖像的某一個特征點yi與第一幀圖像所有特征點x=(x1,x2,…,xn)描述子之間的漢明距離,對所有的距離進行排序,取距離最近的一個作為匹配點。

只有當前向匹配和后向匹配得到的為同一點對才認為匹配正確。計算兩幀圖像的描述符之間的漢明距離,其表達式為

(10)

式(10)中:a、b代表兩幀圖像的所有特征點描述子;⊕為計算特征描述子之間的直線距離。

第二步：利用PROSAC算法剔除特征點的誤匹配。PROSAC算法是對RANSAC(random sample consensus)算法的優化,不同于RANSAC算法,PROSAC算法會對樣本點進行降序排序,然后選取四組匹配質量最高的匹配點,計算內點數量,因為樣本點為相似性更高的點,所以得到的更可能為內點,并且采樣點的減少使得算法計算速度更快。算法引入質量函數q(uj),數據點集合為uN,數據點降序滿足公式

ui,uj∈uN,i

(11)

式(11)中:“:”代表并列,即ui、uj屬于集合uN且i

數據點為內點的概率p(uj)公式為

p(ui)≥p(uj)

(12)

設β為匹配的特征點描述子歐式距離的比值,對質量函數進行評估,表達式為

(13)

式(13)中:D1代表特征點描述子歐式距離最小值;D2代表特征點描述子歐氏距離次小值。比值越小,代表匹配的準確度越高。

通過引入質量因子γ來衡量匹配點對的質量好壞,其值越大,代表匹配質量越高,其計算公式為

(14)

改進后的回環閉合算法流程圖如圖4所示,首先使用視覺詞袋模型進行視覺回環閉合檢測,然后判斷是否滿足預先設置的閾值,滿足閾值以后進行特征描述符的計算,使用雙向檢測 匹配算法進行特征點匹配,其次使用PROSAC算法進行特征點誤匹配的剔除,最后執行回環閉合優化,整個算法流程不斷循環,直到系統結束。

圖4 改進后的回環閉合算法流程圖Fig.4 Improved loop closure algorithm flow chart

4 實驗結果分析

實驗在Ubuntu18.04環境下進行,所用計算機CPU為2.30 GHz Intel i7,運行內存為16 GB。實驗數據集來自瑞士蘇黎世聯邦理工學院制作的EuRoc數據集,取其中MH_01_easy、MH_02_easy、MH_03_medium、MH_04_difficult和MH_05_difficult序列。在實驗中對提出的多無人機協同SLAM算法中的特征點匹配做出分析,并且與VINS-MONO的定位精度做了對比,同時分析了多無人機協同SLAM算法構建的TSDF地圖與ESDF地圖的效果。

4.1 特征點匹配分析

實驗選取了EuRoc數據集中的MH_01_easy序列中的兩幅圖片,分別比較了不同情況下匹配效果,如圖5所示傾斜綠色匹配線代表匹配錯誤的點。

從圖5中看出匹配錯誤的線逐漸減少,說明匹配的準確率逐漸上升,在四種方法中使用雙向檢測匹配法加PROSAC算法匹配的結果達到最優,匹配的準確率最高。

圖5 不同算法的匹配結果圖Fig.5 The matching result chart of different algorithms

4.2 定位精度分析

為了驗證所提算法定位精度的提升,在EuRoc數據集下的五個序列中與VINS-MONO算法做了多次對比定位精度實驗,取其中一組對比實驗進行分析,結果如圖6、圖7所示。

圖6 兩種算法的軌跡對比圖Fig.6 The trajectory comparison chart of the two algorithms

從圖6可以看出,相對于圖6(a)與圖6(b)的軌跡,圖6(c)中多無人機協同SLAM算法在MH_03_medium數據集下的軌跡與真實軌跡更加貼近。

評估算法定位精度,可以采用真實軌跡與實際運動軌跡的絕對軌跡誤差(absolute trajectory error,ATE)進行比較。ATE越小,算法的精度越高,計算式為

(15)

將算法估計的軌跡與數據集真實軌跡對比,使用EVO工具繪制絕對軌跡誤差圖如圖7所示,圖中APE代表絕對位姿誤差。

由圖7可以看出,多無人機協同SLAM算法相較于VINS-MONO算法的定位精度有了明顯提升。利用絕對軌跡誤差進行比較,表1為兩種算法在五個不同數據集下的最大軌跡誤差對比,其中最大絕對軌跡誤差對應絕對軌跡誤差圖中折線的最高值。

定位精度提升公式為

(16)

式(16)中:Z1為VINS-MONO算法在有回環閉合時的運行軌跡誤差;Z2為多無人機協同SLAM算法在有回環閉合時的運行軌跡誤差。

表1 最大絕對軌跡誤差Table 1 Maximum absolute trajectory error

從表1中可以看出,VINS-MONO算法在有回環閉合的情況下比無回環閉合的情況的最大絕對軌跡誤差減小,這說明在有回環閉合時VINS-MONO算法的定位精度更高,而多無人機協同SLAM算法相較于VINS-MONO算法在有回環閉合時的最大絕對軌跡誤差較小,這表示多無人機協同SLAM算法的定位精度較VINS-MONO算法精度更高。

根據式(16)計算,多無人機協同SLAM算法相比于VINS-MONO算法在有回環閉合時,在五個數據集下的誤差皆減小,在MH_01_easy、MH_02_easy、MH_03_medium、MH_04_difficult和MH_05_difficult數據集下,誤差分別減小了34%、26%、42%、24%、19%。

4.3 多無人機協同稠密地圖構建對比

圖8中所有圖像均來自Ubuntu18.04環境下的ROS(機器人操作系統)中的Rviz(三維可視化平臺)。

本文算法使用五個無人機代理協同構建稠密地圖,與文獻[14]采用三個無人機代理協同構建稠密地圖進行對比分析,其中三個代理采用MH_01-MH_03的三個數據集進行實驗,五個代理采用MH_01-MH_05的五個數據集進行實驗,如圖8所示。

從圖8(f)左圖與圖8(f)右圖的虛線框對比中可以看到,增加代理后,圖8(f)右圖中構建的TSDF地圖和ESDF地圖具有更高的完整度。圖8(f)右圖中在構建TSDF地圖和ESDF地圖時增加了代理四與代理五,所構建的地圖可滿足五架無人機協同導航需求,雖然相較于三個代理在構建TSDF地圖和ESDF地圖時的時間增加了3%左右,但仍可以滿足在構建全局一致的TSDF地圖與ESDF地圖時的實時性。

5 結論

(1)改進了VINS-MONO算法,提出了一種集中式多無人機協同定位與稠密地圖構建算法。該算法將協同的所有無人機都轉換到統一的世界坐標系下,不需要在地圖融合時執行額外的位姿轉換。

(2)改進了回環閉合算法。使用雙向檢測匹配法對關鍵幀的特征點做匹配,并使用PROSAC算法剔除誤匹配,為多無人協同定位提供了一種新的回環閉合方法。

(3)通過實驗驗證,提出的多無人機協同SLAM算法與VINS-MONO算法相比定位精度平均提高了約38%,同時在構建全局一致地圖時的追蹤定位實時性較好,在保證實時性的情況下構建了可供五架無人機協同導航使用的全局一致TSDF地圖與ESDF地圖。

(4)后續可考慮應用到真實無人機,但該算法需要進行大量的計算和數據處理,對計算機的計算能力要求較高,還需要考慮降低運算量和數據傳輸的問題。

圖8 多無人機協同構建稠密地圖對比圖Fig.8 Multiple UAVs cooperate to build dense map comparison map