劉峰，呂強(qiáng)，郭峰，王國(guó)勝

(裝甲兵工程學(xué)院 控制工程系, 北京 100072)

導(dǎo)航、制導(dǎo)與控制

基于ROS的自主無人機(jī)VSLAM研究*

劉峰，呂強(qiáng)，郭峰，王國(guó)勝

(裝甲兵工程學(xué)院 控制工程系, 北京 100072)

針對(duì)未知復(fù)雜環(huán)境中無人機(jī)無法獲得外部輔助情況下自主導(dǎo)航所面臨的嚴(yán)峻問題，提出在ROS框架下在板運(yùn)行單目VSLAM算法的自主無人機(jī)方案，僅依靠自身攝像機(jī)自主地完成SLAM和導(dǎo)航任務(wù)。研究VSLAM算法原理與前沿算法ORB-SLAM，設(shè)計(jì)并搭建了自主導(dǎo)航無人機(jī)平臺(tái)，針對(duì)搭建的無人機(jī)平臺(tái)方案和特點(diǎn)完成視覺定位部分的改進(jìn)設(shè)計(jì)。實(shí)驗(yàn)表明，自主無人機(jī)能夠在未知環(huán)境中，自主實(shí)現(xiàn)同時(shí)定位和地圖構(gòu)建任務(wù)并完成精確的飛行控制與導(dǎo)航。

視覺同時(shí)定位于地圖構(gòu)建；自主無人機(jī)；ROS；視覺定位；位姿圖優(yōu)化；自主導(dǎo)航

0 引言

微小型空中無人平臺(tái)在軍事偵察、災(zāi)難救援、探測(cè)和監(jiān)控等領(lǐng)域都扮演著重要的角色，無人機(jī)的自主飛行控制對(duì)空中無人平臺(tái)走向智能化具有里程碑的意義。然而自主無人機(jī)在飛行過程中的最大挑戰(zhàn)是定位信息的獲取，目前僅僅依賴GPS的定位方式缺乏精度和可靠性，GPS信號(hào)在有覆蓋的環(huán)境中容易丟失。視覺傳感器能夠有效獲取周圍環(huán)境信息，重量輕，功耗低，所以采用視覺方法進(jìn)行無人機(jī)定位正逐漸成為研究熱點(diǎn)[1-6]，文獻(xiàn)[1]的方法被成功應(yīng)用于無人機(jī)中[7-8]。

對(duì)無人機(jī)的飛行控制大部分還采用人工遠(yuǎn)程控制，法國(guó)派諾公司的ARDrone四旋翼[9]搭載視覺傳感器實(shí)現(xiàn)無人機(jī)室內(nèi)定位，通過Wi-Fi將圖像數(shù)據(jù)傳回地面站計(jì)算機(jī)，地面站計(jì)算機(jī)完成數(shù)據(jù)處理并控制無人機(jī)的飛行。慕尼黑科技大學(xué)搭建四旋翼無人機(jī)[10]RGB-D攝像頭的稠密跟蹤與地圖構(gòu)建系統(tǒng)，獲取室內(nèi)環(huán)境3D模型完成無人機(jī)定位和自主飛行，算法依賴地面站GPU，需要無線模塊完成數(shù)據(jù)傳輸。這些無線通信的方式、數(shù)據(jù)和控制命令易受干擾，且存在一定的控制延遲。瑞士蘇黎世聯(lián)邦理工學(xué)院提出微型控制無人機(jī)半直接單目視覺里程計(jì)方法[11]，具有精度高和運(yùn)行速度快的特點(diǎn)，該方法也實(shí)現(xiàn)了在板嵌入式計(jì)算機(jī)的實(shí)時(shí)運(yùn)行，數(shù)據(jù)和控制不再依賴無線方式。但是在長(zhǎng)距離和大尺度的環(huán)境中精度與魯棒性不高，目前還只是純粹的運(yùn)動(dòng)估計(jì)，還沒有實(shí)現(xiàn)環(huán)境三維重建。

本文主要研究自主飛行的多旋翼無人機(jī)視覺定位與地圖構(gòu)建，設(shè)計(jì)了多旋翼無人機(jī)自主導(dǎo)航控制軟硬件平臺(tái)，研究視覺定位與地圖構(gòu)建算法，并成功利用該方法實(shí)現(xiàn)無人機(jī)的室內(nèi)視覺定位，完成無人機(jī)的室內(nèi)自主導(dǎo)航飛行。

1 平臺(tái)設(shè)計(jì)

1.1 硬件

本文搭建的多旋翼無人機(jī)平臺(tái)包括Pixhawk飛行控制板、視覺傳感器、低功耗在板主控計(jì)算機(jī)、電機(jī)和14.8 V鋰電池。能夠滿足基本的載荷要求，最短續(xù)航能力為20 min，最大直徑75 cm。圖1為該無人機(jī)平臺(tái)。

圖1 自主無人機(jī)平臺(tái)Fig.1 Autonomous UAV platform

在板計(jì)算機(jī)是多旋翼無人機(jī)視覺定位與地圖構(gòu)建以及自主飛行控制的核心。本文的在板計(jì)算機(jī)搭載Intel Core i7處理器，能夠滿足定位、地圖構(gòu)建、障礙物檢測(cè)和路徑規(guī)劃等高級(jí)任務(wù)需求，采用Ubuntu12.04操作系統(tǒng)，搭載Hydro機(jī)器人操作系統(tǒng)。該在板計(jì)算機(jī)的優(yōu)勢(shì)在于大小僅為25 mm×103 mm×113 mm，質(zhì)量為0.270 kg。

飛行控制板是執(zhí)行自主命令和控制飛行的樞紐，其內(nèi)部具有16位陀螺儀，一個(gè)STM公司的14位加速度計(jì)和磁力計(jì)，以及MS5611氣壓計(jì)。并提供了多個(gè)接口來連接外部傳感器，適用于各類無人機(jī)。為了增強(qiáng)系統(tǒng)的穩(wěn)定性和拓展性，本文還連接了數(shù)傳和GPS等模塊。

單場(chǎng)次洪水總量對(duì)比選取了1992—2016年系列中，實(shí)測(cè)流量最大年份1998年的最大實(shí)測(cè)洪水段，其實(shí)測(cè)最大流量為流量208 m3/s；汛期總量和年總量選取汛期徑流相對(duì)豐沛的2011年進(jìn)行對(duì)比分析計(jì)算；其中推算流量，高水部分采用1992-2016年歷年單值化關(guān)系線推算，中水部分多年單值化關(guān)系線推算，低枯水部分采用單年率定關(guān)系線推算，對(duì)比結(jié)果見表5。

飛行控制板上的串行通信接口與主控計(jì)算機(jī)的USB接口連接，建立了主控計(jì)算機(jī)與飛行控制板之間的數(shù)據(jù)橋梁。單目視覺傳感器通過USB接口直接連接在主控計(jì)算機(jī)上。

1.2 軟件

軟件主要基于機(jī)器人操作系統(tǒng)(ROS)[12]框架，建立各模塊通信、采集傳感器數(shù)據(jù)和設(shè)計(jì)自主位置控制。

機(jī)器人操作系統(tǒng)是一款新穎的主要應(yīng)用在機(jī)器人平臺(tái)操作與控制系統(tǒng)的軟件框架，其優(yōu)勢(shì)可以歸結(jié)為：點(diǎn)對(duì)點(diǎn)設(shè)計(jì)、工具包豐富、代碼復(fù)用率高、多語(yǔ)言支持(C++，Python，Java)、精簡(jiǎn)集成化、系統(tǒng)模塊化、免費(fèi)開源和便于測(cè)試。

通信接口節(jié)點(diǎn)是主控計(jì)算機(jī)與Pixhawk飛行控制板之間的信息傳輸樞紐，該節(jié)點(diǎn)是實(shí)現(xiàn)主控計(jì)算機(jī)實(shí)時(shí)獲取飛行器飛行狀態(tài)和對(duì)飛行狀態(tài)進(jìn)行自主控制的關(guān)鍵。該節(jié)點(diǎn)主要通過Mavlink協(xié)議接收多旋翼飛行器的心跳包、姿態(tài)角數(shù)據(jù)包和位置數(shù)據(jù)包等，并按類別以特定消息格式將數(shù)據(jù)發(fā)布到不同的主題上。同時(shí)訂閱了vision和position等主題，再以Mavlink協(xié)議的形式將這些主題數(shù)據(jù)發(fā)送到飛行控制板，獲取外部傳感器的數(shù)據(jù)并實(shí)現(xiàn)自主飛行控制。自主控制節(jié)點(diǎn)的設(shè)計(jì)原理可以總結(jié)為訂閱飛行器的當(dāng)前位置主題，提供位置反饋，再將位置控制命令發(fā)布到通信節(jié)點(diǎn)的/mavros/setpoint/local_position主題上。通過對(duì)無人飛行器狀態(tài)的實(shí)時(shí)監(jiān)測(cè)調(diào)整位置控制信息。

2 無人機(jī)視覺定位與地圖構(gòu)建

本節(jié)主要研究一種實(shí)時(shí)且高精度的視覺simultaneous localization and mapping (SLAM)算法[13]，該單目視覺定位與地圖構(gòu)建方法，能夠在室內(nèi)和室外，大規(guī)模和小規(guī)模實(shí)時(shí)運(yùn)行，可以完成閉環(huán)檢測(cè)和重定位，包括自動(dòng)的地圖初始化，且選取了最合適的方法來選擇特征點(diǎn)和關(guān)鍵幀。針對(duì)其特點(diǎn)，對(duì)算法完成改進(jìn)工作，并成功運(yùn)用在搭建的自主四旋翼無人機(jī)平臺(tái)，在無其它定位方法的情況下，完成四旋翼無人機(jī)的視覺定位與地圖構(gòu)建。

2.1 地圖初始化

使用標(biāo)準(zhǔn)的DLT和八點(diǎn)算法[14]并行計(jì)算單應(yīng)性矩陣Hc和基本矩陣Fc，然后對(duì)每個(gè)模型計(jì)算得

(1)

(2)

采用如下公式的計(jì)算方法完成2個(gè)模型選擇：

(3)

當(dāng)RH>0.45時(shí)即處理的是平面或者低視差的情形，選擇單應(yīng)性矩陣模型。否則選擇基本矩陣模型。

模型選擇完成后需要從模型中得到相關(guān)的運(yùn)動(dòng)估計(jì)。在單應(yīng)性矩陣的情況下，利用八點(diǎn)計(jì)算的運(yùn)動(dòng)假設(shè)方法處理，在視差較大的基本矩陣情況下，利用SVD方法計(jì)算4種運(yùn)動(dòng)假設(shè)完成估計(jì)。

2.2 跟蹤

跟蹤線程對(duì)每一幀圖像完成位姿估計(jì)，主要包含5個(gè)步驟：ORB特征提取、幀對(duì)幀的初始位姿估計(jì)、全局重定位的初始位姿估計(jì)、跟蹤局部地圖、確定新的關(guān)鍵幀。其框架如圖2所示。

圖2 跟蹤線程框架Fig.2 Tracking thread frame

采用幀對(duì)幀和利用關(guān)鍵幀進(jìn)行全局重定位2種位姿估計(jì)方法大大提高了圖像跟蹤的魯棒性。其次跟蹤線程中還需要一個(gè)插入關(guān)鍵幀的策略來更新全局關(guān)鍵幀。

2.3 局部地圖構(gòu)建

局部地圖構(gòu)建線程主要對(duì)每個(gè)新的關(guān)鍵幀Ki完成局部地圖的構(gòu)建?？煞譃?個(gè)步驟完成，分別為：插入關(guān)鍵幀，淘汰最近地圖點(diǎn)，創(chuàng)建新的地圖點(diǎn)，局部光束平差以及局部關(guān)鍵幀淘汰。局部地圖中包含當(dāng)前關(guān)鍵幀K1以及與其關(guān)聯(lián)的其他關(guān)鍵幀組K2(這組關(guān)鍵幀與當(dāng)前圖像幀有共同的地圖點(diǎn))中地圖點(diǎn)的集合。跟蹤線程中新的特征點(diǎn)跟蹤該局部地圖，提高了位姿估計(jì)的精度。

2.4 環(huán)路閉合檢測(cè)

環(huán)路閉合檢測(cè)線程主要是針對(duì)局部地圖構(gòu)建線程中最后處理的關(guān)鍵幀Ki來進(jìn)行。通過關(guān)鍵幀Ki的視覺詞袋向量計(jì)算其與系統(tǒng)中關(guān)鍵幀的相似性，完成候選的閉環(huán)檢測(cè)。倘若有多個(gè)位置的關(guān)鍵幀與Ki的場(chǎng)景外觀相似，則有多個(gè)閉環(huán)候選。

在確定了閉環(huán)后，則需要完成閉環(huán)修正，通過計(jì)算得到的相似性變換對(duì)當(dāng)前關(guān)鍵幀的位姿進(jìn)行修正，同時(shí)這個(gè)修正將影響到與當(dāng)前關(guān)鍵幀相鄰的關(guān)鍵幀節(jié)點(diǎn)。最后進(jìn)行位姿圖優(yōu)化達(dá)到全局一致性，優(yōu)化后，每個(gè)地圖點(diǎn)根據(jù)修正的關(guān)鍵幀進(jìn)行變換。系統(tǒng)中所有的優(yōu)化都采用了g2o(general graph optimization)框架[15]。

2.5 參數(shù)設(shè)定

針對(duì)本文的無人機(jī)平臺(tái)，需要對(duì)視覺定位與地圖構(gòu)建算法的相關(guān)參數(shù)進(jìn)行設(shè)置。具體參數(shù)的值如表1所示。參數(shù)的設(shè)置主要根據(jù)在板主控計(jì)算機(jī)的處理能力、實(shí)時(shí)性和精度等進(jìn)行綜合考慮，使系統(tǒng)的性能和穩(wěn)定性更高。其中ORB特征點(diǎn)數(shù)為每個(gè)圖像幀中特征點(diǎn)的個(gè)數(shù)，F(xiàn)AST檢測(cè)閾值保證了最低的檢測(cè)特征點(diǎn)數(shù)。

表1 算法參數(shù)設(shè)置Table 1 Algorithm parameter settings

3 實(shí)驗(yàn)分析

本節(jié)將對(duì)無人機(jī)視覺導(dǎo)航進(jìn)行實(shí)驗(yàn)分析，驗(yàn)證自主導(dǎo)航控制精度，并完成室內(nèi)環(huán)境的地圖構(gòu)建。

由于在ROS中rosrun命令只能單次運(yùn)行一個(gè)節(jié)點(diǎn)，實(shí)驗(yàn)平臺(tái)需要對(duì)多個(gè)節(jié)點(diǎn)同時(shí)運(yùn)行且完成相關(guān)參數(shù)設(shè)置，所以采用.launch啟動(dòng)文件來編排啟動(dòng)節(jié)點(diǎn)和初始化參數(shù)，同時(shí)還能運(yùn)行ROS內(nèi)核。圖3表示的是節(jié)點(diǎn)關(guān)系，其中圓圈表示主題。

圖3 節(jié)點(diǎn)關(guān)系圖Fig.3 Relationship diagram between nodes

驗(yàn)證實(shí)驗(yàn)為實(shí)驗(yàn)室真實(shí)環(huán)境中的定點(diǎn)導(dǎo)航飛行，同時(shí)對(duì)環(huán)境進(jìn)行地圖構(gòu)建。無人機(jī)的飛行區(qū)域在實(shí)驗(yàn)室中央3 m×6 m的無障礙區(qū)域。單目視覺傳感器的視野水平朝向飛行器側(cè)面且飛行過程中保持偏航角不變。自主導(dǎo)航飛行的高度設(shè)置為1.5 m。

圖4 自主導(dǎo)航水平位置精度Fig.4 Horizontal position accuracy of autonomous navigation

圖5 自主導(dǎo)航三維位置精度Fig.5 Three-dimensional positional accuracy of autonomous navigation

實(shí)驗(yàn)中地圖構(gòu)建的真實(shí)環(huán)境部分圖像如圖6所示。圖7為導(dǎo)航與地圖構(gòu)建實(shí)驗(yàn)關(guān)鍵幀的軌跡和構(gòu)建的真實(shí)環(huán)境三維地圖點(diǎn)，觀察視角為俯視圖，其中藍(lán)色的軌跡為算法關(guān)鍵幀軌跡，黑色的點(diǎn)為空間三維地圖點(diǎn)，紅色的點(diǎn)為當(dāng)前傳感器視野圖像中的空間地圖點(diǎn)，圖6中A和B位置分別對(duì)應(yīng)圖7中的2幅環(huán)境圖像。系統(tǒng)共構(gòu)建了5 722個(gè)環(huán)境地圖點(diǎn)，包含131幀圖像關(guān)鍵幀，飛行過程中平均每幀圖像跟蹤約150個(gè)特征點(diǎn)。三維環(huán)境地圖點(diǎn)構(gòu)建的區(qū)域約為10 m2的真實(shí)實(shí)驗(yàn)室辦公環(huán)境。

圖6 地圖構(gòu)建環(huán)境部分真實(shí)圖像Fig.6 Partially true image of maps built environment

圖7 關(guān)鍵幀軌跡和環(huán)境特征點(diǎn)地圖Fig.7 Keyframe track and environmental characteristics point map

4 結(jié)束語(yǔ)

多旋翼無人機(jī)靈活機(jī)動(dòng)、已經(jīng)廣泛應(yīng)用于軍事偵察和災(zāi)害救援等領(lǐng)域。在缺乏GPS信號(hào)的未知環(huán)境中，基于視覺的飛行器定位導(dǎo)航和地圖構(gòu)建逐漸成為研究熱點(diǎn)。本文主要研究自主的多旋翼無人機(jī)視覺定位與地圖構(gòu)建，利用視覺方法完成了無人機(jī)的定點(diǎn)導(dǎo)航和地圖構(gòu)建。實(shí)驗(yàn)驗(yàn)證結(jié)果表明本文的系統(tǒng)能夠在完全未知的環(huán)境中，利用視覺的方法自主完成精確的導(dǎo)航飛行控制和地圖構(gòu)建任務(wù)，不需要其他輔助定位系統(tǒng)，且具有較高的導(dǎo)航飛行控制精度。

[1] KLEIN G, MURRAY D. Parallel Tracking and Mapping for Small AR Workspaces[C]∥Mixed and Augmented Reality, 2007. ISMAR 2007. 6th IEEE and ACM International Symposium on. IEEE, 2007: 225-234.

[2] MUR-ARTAL R, TARDOS J D. ORB-SLAM: Tracking and Mapping Recognizable Features[C]∥Robotics: Science and Systems (RSS) Workshop on Multi View Geometry in Robotics，2014.

[3] CONCHA A, CIVERA J DPPTAM. Dense Piecewise Planar Tracking and Mapping from a Monocular Sequence[C]∥Intelligent Robots and Systems (IROS), 2015 IEEE/RSJ International Conference on. IEEE, 2015: 5686-5693.

[4] DAVISON A J, REID I D, MOLTON N D, et al. MonoSLAM: Real-Time Single Camera SLAM[J]. Pattern Analysis and Machine Intelligence, IEEE Transactions on, 2007, 29(6): 1052-1067.

[5] YANG S, SCHERER S A, SCHAUWECKER K, et al. Autonomous Landing of MAVs on an Arbitrarily Textured Landing Site Using Onboard Monocular Vision[J]. Journal of Intelligent & Robotic Systems, 2014, 74(1-2): 27-43.

[6] 林輝燦，呂強(qiáng)，張洋，等.稀疏和稠密的VSLAM的研究進(jìn)展[J].機(jī)器人，2016，38(5)：621-631. LIN Hui-can,Lü Qiang,ZHANG Yang,et al.The Sparse and Dense VSLAM:A Survey[J].Robot,2016,38(5):621-631.

[7] SCARAMUZZA D, ACHTELIK M C, DOITSIDIS L, et al. Vision-Controlled Micro Flying Robots: From System Design to Autonomous Navigation and Mapping in GPS-denied Environments[J]. Robotics & Automation Magazine, IEEE, 2014, 21(3): 26-40.

[8] ENGEL J, STURM J, CREMERS D. Scale-Aware Navigation of a Low-Cost Quadrocopter with a Monocular Camera[J]. Robotics and Autonomous Systems, 2014, 62(11): 1646-1656.

[10] ENGEL J, STURM J, CREMERS D. Camera-Based Navigation of a Low-Cost Quadrocopter[C]∥Intelligent Robots and Systems (IROS), 2012 IEEE/RSJ International Conference on. IEEE, 2012: 2815-2821.

[11] FORSTER C, PIZZOLI M, SCARAMUZZA D. SVO: Fast Semi-Direct Monocular Visual Odometry[C]∥Robotics and Automation (ICRA), 2014 IEEE International Conference on. IEEE, 2014: 15-22.

[12] MARTINEZ A,FERNANDEZ E.Learning ROS for Robotics Programming[M].Birmingham:Packt Publishing Ltd.,2013.

[13] MUR-ARTAL R, MONTIEL J M M, TARDOS J D. ORB-SLAM: A Versatile and Accurate Monocular SLAM System[J]. Robotics, IEEE Transactions on, 2015, 31(5): 1147-1163.

[14] HARTLEY R, ZISSERMAN A. Multiple View Geometry in Computer Vision[M].New York: Cambridge University Press, 2003.

[15] KüMMELE R, GRISETTI G, STRASDAT H, et al. g2o: A General Framework for Graph Optimization[C]∥Robotics and Automation (ICRA), 2011 IEEE International Conference on. IEEE, 2011: 3607-3613.

VSLAM of Autonomous UAV Based on ROS

LIU Feng，Lü Qiang，GUO Feng，WANG Guo-sheng

(Academy of Armored Forces Engineering,Department of Control Engineering, Beijing 100072，China)

Inunknown complex environments, unmanned aerial vehicles(UAV) faces the problem of autonomous navigation without external aid. To solve the problem, an autonomous UAV program with monocular visual simultaneous localization and mapping(VSLAM) algorithms running onboard under ROS framework is proposed. It can autonomously achieve SLAM and navigation task only relying on its own camera. The principle of VLSAM algorithms and the state-of-the-art ORB-SLAM algorithm are studied and a UAV autonomous navigation platform is designed and built. Aiming at the platform and its characteristics, the visual positioning design is improved. Experiments show that the autonomous UAV can achieve autonomous simultaneous positioningand complete map building task and precise flight control and navigation in unknown environments.

visual simultaneous localization and mapping(VSLAM)； autonomous unmanned aerial vehicles(UAV); robot operating system(ROS); visual localization; general graph optimization;autonomous navigation

2016-01-22；

2016-03-11

劉峰(1987-)，男，江西吉安人。博士生，主要研究方向?yàn)闄C(jī)器人控制，計(jì)算機(jī)視覺，SLAM。

10.3969/j.issn.1009-086x.2016.06.011

V279；V448.22+4；TP391.41

A

1009-086X(2016)-06-0061-06

通信地址：100072 北京市豐臺(tái)區(qū)杜家坎21號(hào)院

E-mail：18606921232@163.com