基于ROS的無人機(jī)占據(jù)地圖構(gòu)建方案設(shè)計(jì)

林曉華，王茂森，戴勁松

( 南京理工大學(xué) 機(jī)械工程學(xué)院， 南京 210094)

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

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

1 平臺(tái)搭建

本文方案搭建的硬件平臺(tái)基于Linux內(nèi)核，使用ubuntu16.04操作系統(tǒng)，處理器型號(hào)為Intel Corei5-8300H，RAM為16.0 GB，能夠滿足SLAM算法以及地圖構(gòu)建等任務(wù)需求。

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

圖1 雙目攝像頭硬件

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

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

第1個(gè)節(jié)點(diǎn)：首先通過改寫USB相機(jī)驅(qū)動(dòng)并調(diào)用USB相機(jī)功能包，使其能夠同時(shí)同步調(diào)用兩個(gè)相機(jī)以獲取雙目圖像信息，然后利用ROS的“話題發(fā)布-訂閱”模型，將兩個(gè)相機(jī)所獲取的圖像數(shù)據(jù)消息同步發(fā)布到usb_cam1/image和usb_cam2/image兩個(gè)話題。

第2個(gè)節(jié)點(diǎn)：算法模塊通過訂閱第一節(jié)點(diǎn)所發(fā)布的兩個(gè)相機(jī)圖像話題，將圖像數(shù)據(jù)、相機(jī)參數(shù)、詞典文件作為輸入?yún)?shù)，運(yùn)行SLAM算法，實(shí)現(xiàn)相機(jī)軌跡定位及跟蹤。此外，還增加點(diǎn)云計(jì)算函數(shù)，并將點(diǎn)云信息發(fā)布到pointcloud_outout話題，同時(shí)也將位姿估計(jì)信息發(fā)布到camera_pose話題。

第3個(gè)節(jié)點(diǎn)：建圖模塊通過訂閱pointcloud_outout話題以及camera_pose話題，得到點(diǎn)云信息與相機(jī)位姿信息，利用Octomap工具構(gòu)建出占據(jù)地圖，完成任務(wù)。節(jié)點(diǎn)關(guān)系如圖2所示。

圖2 三節(jié)點(diǎn)關(guān)系示意圖

2 ORB-SLAM算法框架探析

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

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

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

LocalMapping線程即局部地圖構(gòu)建線程，其主要任務(wù)是插入關(guān)鍵幀、拋棄外點(diǎn)、創(chuàng)建新的地圖點(diǎn)、局部BA(Buddle Adjustment)優(yōu)化以及淘汰局部關(guān)鍵幀。

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

LoopClosing線程即閉合環(huán)路檢測線程，其主要任務(wù)是檢測閉環(huán)以及計(jì)算閉環(huán)，最后對位姿圖進(jìn)行優(yōu)化，以達(dá)到全局一致性。

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

3 算法改進(jìn)

原有算法框架通過System∷TrackStereo()這個(gè)接口函數(shù)實(shí)現(xiàn)ORB-SLAM庫的調(diào)用，在System∷TrackStereo()中調(diào)用了Tracking∷GrabImageStereo()函數(shù)，最后在Tracking這個(gè)類中，mpTracker->GrabImageStereo()最終調(diào)用函數(shù)Tracking∷Track()計(jì)算相機(jī)位姿，同時(shí)調(diào)用NeedNewKeyFrame()函數(shù)，決定是否插入關(guān)鍵幀。

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

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

關(guān)鍵幀相當(dāng)于SLAM系統(tǒng)的骨架，是在局部一系列普通幀中作為局部幀的代表，記錄著局部信息。在相機(jī)保持靜止的狀態(tài)下，普通幀也會(huì)一直增加，而關(guān)鍵幀因?yàn)榭偸强吹皆瓐鼍?，因此不?huì)改變。所以在普通幀每兩幀之間會(huì)存在大量的信息冗余，如果所有幀都參與計(jì)算，不僅浪費(fèi)算力，對處理器內(nèi)存也是極大的考驗(yàn)。此外，關(guān)鍵幀選擇時(shí)還會(huì)對圖片質(zhì)量、特征點(diǎn)質(zhì)量等進(jìn)行考察，一定程度上發(fā)揮了濾波作用，可以防止無用或者錯(cuò)誤的信息進(jìn)入優(yōu)化過程而破壞定位建圖的準(zhǔn)確性。

因此，便可利用關(guān)鍵幀信息進(jìn)行占據(jù)地圖的構(gòu)建工作。在原有算法框架基礎(chǔ)上，增加關(guān)鍵幀判斷標(biāo)志位“isKeyframe”，加入到各層函數(shù)入口參數(shù)，在ROS節(jié)點(diǎn)訂閱源文件中使用帶有關(guān)鍵幀的庫調(diào)用入口，實(shí)現(xiàn)算法改進(jìn)。最終調(diào)用接口流程如圖3所示。

圖3 改進(jìn)后函數(shù)調(diào)用接口流程框圖

4 實(shí)驗(yàn)參數(shù)與結(jié)果分析

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

(1)

其中λ≠0為比例系數(shù)，矩陣M為相機(jī)內(nèi)參數(shù)矩陣，矩陣R和矩陣T為旋轉(zhuǎn)矩陣和平移矩陣。因?yàn)闃?biāo)定棋盤為一個(gè)平面，故把世界坐標(biāo)系構(gòu)建在Z=0的平面上，那么上式可改寫為如下形式：

(2)

式中r1、r2、r3分別代表旋轉(zhuǎn)矩陣的第一、二、三列。設(shè)

(3)

式中μ≠0為比例系數(shù)，因?yàn)閞1、r2為旋轉(zhuǎn)矩陣中的2個(gè)向量，因此r1和r2必正交，故推導(dǎo)得：

(4)

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

(5)

顯然矩陣A為一個(gè)對稱矩陣，現(xiàn)定義一個(gè)6維向量a來表示A，公式如下：

(6)

接著進(jìn)行下一步的數(shù)學(xué)化簡有：

(7)

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

(8)

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

(9)

整理后可得：

Vb=0

(10)

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

(11)

進(jìn)行標(biāo)定時(shí)，標(biāo)定界面如圖4所示，使用8×6標(biāo)定板，其每個(gè)方格邊長為24 mm，得到最終標(biāo)定結(jié)果如表1、表2所示。

圖4 雙目相機(jī)標(biāo)定界面

表1 右相機(jī)標(biāo)定參數(shù)

表2 左相機(jī)標(biāo)定參數(shù)

在對實(shí)際場景建圖前，首先采用TUM數(shù)據(jù)集的freiburg1_xyz序列對算法改進(jìn)前后的建圖效果，如圖5、圖6所示。其中兩幅圖的左半部分為改進(jìn)前的建圖效果，右半部分為改進(jìn)后的建圖效果。可見，算法改進(jìn)前后建圖效果總體上保持一致，僅在一些小細(xì)節(jié)上有細(xì)微差別，但對于構(gòu)建無人機(jī)環(huán)境地圖，這些細(xì)小的區(qū)別并不會(huì)產(chǎn)生實(shí)質(zhì)的影響。

實(shí)際室內(nèi)場景實(shí)驗(yàn)時(shí)，在ROS框架下依次啟動(dòng)相機(jī)驅(qū)動(dòng)模塊、算法模塊、建圖模塊3個(gè)節(jié)點(diǎn)，運(yùn)行改進(jìn)后的算法，在走廊、樓梯口實(shí)驗(yàn)場景中，僅對關(guān)鍵幀進(jìn)行跟蹤，構(gòu)建得到的效果圖與實(shí)際場景如圖7、圖8所示?？梢妼?shí)際場景中占據(jù)地圖構(gòu)建效果良好。

圖5 改進(jìn)前后點(diǎn)云地圖效果

圖6 改進(jìn)前后占據(jù)地圖效果

圖7 走廊場景建圖效果

圖8 樓梯場景建圖效果

在運(yùn)行整體方案的同時(shí)，利用系統(tǒng)命令Top，監(jiān)測各個(gè)進(jìn)程實(shí)時(shí)占用內(nèi)存以及CPU占用率，監(jiān)測界面如圖9所示。其中PID為進(jìn)程ID號(hào)，%CPU為當(dāng)前進(jìn)程的CPU占用率，%MEM為當(dāng)前進(jìn)程占用的物理內(nèi)存和總內(nèi)存的百分比，COMMAND為當(dāng)前進(jìn)程的啟動(dòng)命令名稱。

圖9 線程監(jiān)測界面

對于改進(jìn)前后算法，分別在實(shí)際場景中運(yùn)行并構(gòu)建占據(jù)地圖，同時(shí)在相機(jī)移動(dòng)時(shí)以及移動(dòng)后靜止時(shí)分別監(jiān)測建圖進(jìn)程的CPU占用率20 s，得到數(shù)據(jù)用軟件繪制得到如圖10、圖11所示。其中CPU超過100%的占用率是因?yàn)閷?shí)驗(yàn)中所使用的CPU具有四核，滿載負(fù)荷運(yùn)行理論上能夠達(dá)到400%。

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

圖10 移動(dòng)建圖時(shí)占用率比較

圖11 靜止建圖時(shí)占用率比較

5 結(jié)論

針對無人機(jī)在室內(nèi)無法使用GPS的情況，設(shè)計(jì)了一種僅利用關(guān)鍵幀進(jìn)行占據(jù)地圖構(gòu)建的VSLAM方案。利用ROS操作系統(tǒng)的分布式框架特點(diǎn)，設(shè)計(jì)了驅(qū)動(dòng)、算法、建圖3個(gè)信息節(jié)點(diǎn)接口用于信息交互，使用雙目相機(jī)作為環(huán)境信息采集入口，最終利用ORB-SLAM獲取的關(guān)鍵幀信息，完成占據(jù)地圖的構(gòu)建。實(shí)驗(yàn)結(jié)果表明，該設(shè)計(jì)方案能夠?qū)崿F(xiàn)無人機(jī)占據(jù)地圖構(gòu)建，并且可以有效地減少構(gòu)建地圖線程的CPU占用率，為無人機(jī)自主導(dǎo)航提供了一種參考方案。