基于ROS的無人機占據地圖構建方案設計

林曉華，王茂森，戴勁松

( 南京理工大學 機械工程學院， 南京 210094)

隨著人工智能技術的快速發展，使得小型無人機在軍用和民用兩個領域均表現出非常重要的應用價值，包括偵察、搜救、探測、追蹤以及監控等[1-4]。為了達到無人機自主飛行控制的目的，需要完成兩個前提任務，即定位與建圖[5]。最早源于機器人領域，Smith和Cheeseman便提出了同時定位與建圖(Simultaneous Localization and Mapping, SLAM)[6]，其目的是當機器人在一個未知的環境中移動時能夠做到實時重建環境的三維結構并同時估計自身的狀態和位置信息。

現有的無人機定位方式多依賴于全球定位系統(Global Positioning System，GPS)，倘若在GPS信號遮蔽嚴重的區域,這種方法便不再可靠甚至完全失效。但隨著視覺技術的不斷發展，基于視覺技術的定位方式愈發成為熱門的研究方向。文獻[7]第一個提出基于EKF的實時單目SLAM系統，成為V-SLAM研究的先驅，但實時運行時間較短且無法處理無人機的速度突變。文獻[8]提出了一種基于特征點與直接法混合的半直接法，處理速度快，但缺少回環檢測和后端優化部分，使得位姿估計累計誤差較大。文獻[9]中將直接法應用于半稠密建圖的V-SLAM系統中，使得系統可以實時運行在大場景環境中，構建半稠密的地圖，但對光線比較敏感。文獻[10]提出一種融合IMU的單目SLAM系統，克服了單目SLAM的固有弊端，目前在多旋翼無人機平臺上得到較多的運用。除此之外，由于視覺傳感器具有重量輕、功耗低、小范圍內定位精度高、硬件成本低廉、獲取信息更為豐富等優點，使得越來越多的視覺同時定位與地圖構建[11](Visual Simultaneous Localization and Mapping，V-SLAM)方案得到開發與應用。本文主要針對無人機在室內無GPS信號情況下無法完成自身定位導航的問題，提出一種基于ROS框架下運行雙目SLAM算法進而構建無人機環境占據地圖方案，并在計算資源有限的情況下，僅采用關鍵幀信息進行建圖以減少對CPU的占用率。

1 平臺搭建

本文方案搭建的硬件平臺基于Linux內核，使用ubuntu16.04操作系統，處理器型號為Intel Corei5-8300H，RAM為16.0 GB，能夠滿足SLAM算法以及地圖構建等任務需求。

傳感器硬件模塊為兩個平行放置的杰銳微通HF867- 480P型USB高清攝像頭，如圖1所示。該攝像頭支持UVC通信協議，幀率為30幀/秒，適用于Linux內核下的嵌入式開發。工作電壓為5V，無人機搭載的鋰電池能夠滿足其與飛控板的基本載荷需求。

圖1 雙目攝像頭硬件

本文方案的軟件模塊主要依托于機器人操作系統[12](Robot Operating System,ROS)，建立雙相機圖像獲取模塊、算法運行模塊以及建圖模塊之間的數據通信。

ROS是一個適用于機器人的開源的元操作系統。它采用的是一種分布式框架，提供了操作系統應有的服務，包括硬件抽象，底層設備控制，常用函數的實現，進程間消息傳遞，以及包管理。它也提供用于獲取、編譯、編寫、和跨計算機運行代碼所需的工具和庫函數。其具有代碼復用率高、多語言支持、精簡集成化、系統模塊化以及便于測試等優點。基于ROS這種分布式框架的優點，本方案將構建地圖分為3個節點。

第1個節點：首先通過改寫USB相機驅動并調用USB相機功能包，使其能夠同時同步調用兩個相機以獲取雙目圖像信息，然后利用ROS的“話題發布-訂閱”模型，將兩個相機所獲取的圖像數據消息同步發布到usb_cam1/image和usb_cam2/image兩個話題。

第2個節點：算法模塊通過訂閱第一節點所發布的兩個相機圖像話題，將圖像數據、相機參數、詞典文件作為輸入參數，運行SLAM算法，實現相機軌跡定位及跟蹤。此外，還增加點云計算函數，并將點云信息發布到pointcloud_outout話題，同時也將位姿估計信息發布到camera_pose話題。

第3個節點：建圖模塊通過訂閱pointcloud_outout話題以及camera_pose話題，得到點云信息與相機位姿信息，利用Octomap工具構建出占據地圖，完成任務。節點關系如圖2所示。

圖2 三節點關系示意圖

2 ORB-SLAM算法框架探析

Klein于2007年提出PTAM[13](Parallel Tracking and Mapping)并開源。ORB-SLAM作為對PTAM的改進[14-15]，被研究者稱為第一個現代SLAM方法，在PTAM分離了追蹤和地圖構建線程的基礎上，把回環檢測和世界地圖的優化從局部地圖優化中獨立出來，另辟一個線程，形成三線程并行運行結構，使得在線SLAM應用到更大的場景中，并可以應用到室外。

Tracking線程即跟蹤線程，主要涵蓋SLAM前端，也即視覺里程計(VO)部分。該線程的主要工作是提取ORB特征、初始跟蹤估計相機位姿、跟蹤局部地圖、決定新的關鍵幀以及當前幀相關信息保存，核心任務是根據圖像來估計相機運動。

ORB-SLAM的前端采用特征點法，基于ORB(Oriented FASTand Rotated BRIEF)特征進行特征點提取以及匹配。通過運動模型或者跟蹤參考關鍵幀得到每一幀對應的初始相機位姿估計，倘若跟蹤失敗，則通過重定位得到位姿估計。同時在該線程中還通過判斷是否需要插入新關鍵幀來更新全局關鍵幀。

LocalMapping線程即局部地圖構建線程，其主要任務是插入關鍵幀、拋棄外點、創建新的地圖點、局部BA(Buddle Adjustment)優化以及淘汰局部關鍵幀。

由于從雙目相機中不斷獲取得到新圖像傳入系統，并從Tracking線程中得到新的關鍵幀輸入，局部地圖中將會包含當前關鍵幀和與其成功匹配的其他關鍵幀，通過優化算法不斷剔除壞點以及誤差較大的關鍵幀，從而提高了位姿估計的精度。

LoopClosing線程即閉合環路檢測線程，其主要任務是檢測閉環以及計算閉環，最后對位姿圖進行優化，以達到全局一致性。

線程中，通過遍歷附近地圖中的關鍵幀，通過視覺詞袋向量，計算檢測出候選關鍵幀。再對候選關鍵幀生成Sim3求解器，計算其相似變換對當前關鍵幀的位姿進行修正，得到最佳的閉環匹配。最后，采用g2o(general graph optimization)框架[16]，對系統進行優化。

3 算法改進

原有算法框架通過System∷TrackStereo()這個接口函數實現ORB-SLAM庫的調用，在System∷TrackStereo()中調用了Tracking∷GrabImageStereo()函數，最后在Tracking這個類中，mpTracker->GrabImageStereo()最終調用函數Tracking∷Track()計算相機位姿，同時調用NeedNewKeyFrame()函數，決定是否插入關鍵幀。

對于關鍵幀的篩選，首先有3種直接不插入關鍵幀的情況：第1種情況，當前模式處于定位模式下，定位模式下只定位不建圖，僅利用已有的地圖點對當前相機位姿進行估計，并不更新地圖點，因此沒有插入關鍵幀的必要；第2種情況，局部地圖處于全部閉環條件下，當局部地圖被占用時，插入關鍵幀反而會影響全局閉環優化；第3種情況，距離上一次重定位較近的條件下，由于重定位成功后，剛剛插入了關鍵幀，所以不需要頻繁插入。

在排除了上述3種情況后，需要對關鍵幀進行進一步的檢測。首先需要保證內點數超過設定的最小閾值以保證跟蹤質量，同時避免引入過多的冗余信息，其次需要滿足以下3個條件之一：其一，距離上次插入關鍵幀已經過去了最大限定幀數，目的是防止跟蹤丟失；其二，距離上次插入關鍵幀至少過去了最小限定幀數，主要是為了防止圖像重疊度太高，并且使得局部建圖線程有足夠的空閑處理新的關鍵幀；其三，局部建圖線程中關鍵幀隊列中關鍵幀數量不超過3個，主要也是考慮到局部建圖線程的處理能力以及信息冗余度問題。

關鍵幀相當于SLAM系統的骨架，是在局部一系列普通幀中作為局部幀的代表，記錄著局部信息。在相機保持靜止的狀態下，普通幀也會一直增加，而關鍵幀因為總是看到原場景，因此不會改變。所以在普通幀每兩幀之間會存在大量的信息冗余，如果所有幀都參與計算，不僅浪費算力，對處理器內存也是極大的考驗。此外，關鍵幀選擇時還會對圖片質量、特征點質量等進行考察，一定程度上發揮了濾波作用，可以防止無用或者錯誤的信息進入優化過程而破壞定位建圖的準確性。

因此，便可利用關鍵幀信息進行占據地圖的構建工作。在原有算法框架基礎上，增加關鍵幀判斷標志位“isKeyframe”，加入到各層函數入口參數，在ROS節點訂閱源文件中使用帶有關鍵幀的庫調用入口，實現算法改進。最終調用接口流程如圖3所示。

圖3 改進后函數調用接口流程框圖

4 實驗參數與結果分析

首先采用張氏標定法對雙目相機進行標定。張氏標定法首先要從至少兩個以上角度對棋盤格進行拍攝，將棋盤格的角點作為標定點。將一個標定點記為Q，其在相機坐標系下的坐標記為Q=(x,y,z)，轉換成對應的齊次坐標形式為Q′=(x,y,z,1)，假設q為Q對應在相機成像平面的點，q在圖像坐標系下的齊次坐標值記為q=(m,n,1)。那么空間中點Q和成像平面上的q之間的轉換關系如下：

(1)

其中λ≠0為比例系數，矩陣M為相機內參數矩陣，矩陣R和矩陣T為旋轉矩陣和平移矩陣。因為標定棋盤為一個平面，故把世界坐標系構建在Z=0的平面上，那么上式可改寫為如下形式：

(2)

式中r1、r2、r3分別代表旋轉矩陣的第一、二、三列。設

(3)

式中μ≠0為比例系數，因為r1、r2為旋轉矩陣中的2個向量，因此r1和r2必正交，故推導得：

(4)

為方便求解上式，設一對稱矩陣A=M-TM-1如下：

(5)

顯然矩陣A為一個對稱矩陣，現定義一個6維向量a來表示A，公式如下：

(6)

接著進行下一步的數學化簡有：

(7)

將式(7)改為以a為未知數的2個其次方程：

(8)

迭代n組拍攝的圖像，可得以下方程組：

(9)

整理后可得：

Vb=0

(10)

式(10)中，V是一個2n×6的矩陣。當n≥3時，a是唯一的；當n=2時，可使A12=0。該式的解為VTV的最小特征值所對應的特征向量。根據求得的a可根據式(7)得到矩陣A，這樣就可以求得所需要的參數矩陣M。此外平移矩陣T和旋轉矩陣R可通過內參數矩陣M計算求得，公式如下：

(11)

進行標定時，標定界面如圖4所示，使用8×6標定板，其每個方格邊長為24 mm，得到最終標定結果如表1、表2所示。

圖4 雙目相機標定界面

表1 右相機標定參數

表2 左相機標定參數

在對實際場景建圖前，首先采用TUM數據集的freiburg1_xyz序列對算法改進前后的建圖效果，如圖5、圖6所示。其中兩幅圖的左半部分為改進前的建圖效果，右半部分為改進后的建圖效果。可見，算法改進前后建圖效果總體上保持一致，僅在一些小細節上有細微差別，但對于構建無人機環境地圖，這些細小的區別并不會產生實質的影響。

實際室內場景實驗時，在ROS框架下依次啟動相機驅動模塊、算法模塊、建圖模塊3個節點，運行改進后的算法，在走廊、樓梯口實驗場景中，僅對關鍵幀進行跟蹤，構建得到的效果圖與實際場景如圖7、圖8所示。可見實際場景中占據地圖構建效果良好。

圖5 改進前后點云地圖效果

圖6 改進前后占據地圖效果

圖7 走廊場景建圖效果

圖8 樓梯場景建圖效果

在運行整體方案的同時，利用系統命令Top，監測各個進程實時占用內存以及CPU占用率，監測界面如圖9所示。其中PID為進程ID號，%CPU為當前進程的CPU占用率，%MEM為當前進程占用的物理內存和總內存的百分比，COMMAND為當前進程的啟動命令名稱。

圖9 線程監測界面

對于改進前后算法，分別在實際場景中運行并構建占據地圖，同時在相機移動時以及移動后靜止時分別監測建圖進程的CPU占用率20 s，得到數據用軟件繪制得到如圖10、圖11所示。其中CPU超過100%的占用率是因為實驗中所使用的CPU具有四核，滿載負荷運行理論上能夠達到400%。

由圖10、圖11分析可知：無論是在相機移動時或者靜止時，本文所利用改進的僅跟蹤關鍵幀線程構建占據地圖方案總體上看對CPU占用率有較為明顯的降低，這為無人機板載計算機減負能夠起到一定的作用。

圖10 移動建圖時占用率比較

圖11 靜止建圖時占用率比較

5 結論

針對無人機在室內無法使用GPS的情況，設計了一種僅利用關鍵幀進行占據地圖構建的VSLAM方案。利用ROS操作系統的分布式框架特點，設計了驅動、算法、建圖3個信息節點接口用于信息交互，使用雙目相機作為環境信息采集入口，最終利用ORB-SLAM獲取的關鍵幀信息，完成占據地圖的構建。實驗結果表明，該設計方案能夠實現無人機占據地圖構建，并且可以有效地減少構建地圖線程的CPU占用率，為無人機自主導航提供了一種參考方案。