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

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

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

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

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

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

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

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

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

0 引言

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

對無人機(jī)的飛行控制大部分還采用人工遠(yuǎn)程控制，法國派諾公司的ARDrone四旋翼[9]搭載視覺傳感器實現(xiàn)無人機(jī)室內(nèi)定位，通過Wi-Fi將圖像數(shù)據(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ī)半直接單目視覺里程計方法[11]，具有精度高和運(yùn)行速度快的特點(diǎn)，該方法也實現(xiàn)了在板嵌入式計算機(jī)的實時運(yùn)行，數(shù)據(jù)和控制不再依賴無線方式。但是在長距離和大尺度的環(huán)境中精度與魯棒性不高，目前還只是純粹的運(yùn)動估計，還沒有實現(xiàn)環(huán)境三維重建。

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

1 平臺設(shè)計

1.1 硬件

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

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

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

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

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

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

1.2 軟件

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

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

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

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

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

2.1 地圖初始化

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

(1)

(2)

采用如下公式的計算方法完成2個模型選擇：

(3)

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

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

2.2 跟蹤

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

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

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

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

局部地圖構(gòu)建線程主要對每個新的關(guān)鍵幀Ki完成局部地圖的構(gòu)建?？煞譃?個步驟完成，分別為：插入關(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)跟蹤該局部地圖，提高了位姿估計的精度。

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

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

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

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

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

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

3 實驗分析

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

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

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

驗證實驗為實驗室真實環(huán)境中的定點(diǎn)導(dǎo)航飛行，同時對環(huán)境進(jìn)行地圖構(gòu)建。無人機(jī)的飛行區(qū)域在實驗室中央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

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

圖6 地圖構(gòu)建環(huán)境部分真實圖像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é)束語

多旋翼無人機(jī)靈活機(jī)動、已經(jīng)廣泛應(yīng)用于軍事偵察和災(zāi)害救援等領(lǐng)域。在缺乏GPS信號的未知環(huán)境中，基于視覺的飛行器定位導(dǎo)航和地圖構(gòu)建逐漸成為研究熱點(diǎn)。本文主要研究自主的多旋翼無人機(jī)視覺定位與地圖構(gòu)建，利用視覺方法完成了無人機(jī)的定點(diǎn)導(dǎo)航和地圖構(gòu)建。實驗驗證結(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-)，男，江西吉安人。博士生，主要研究方向為機(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 北京市豐臺區(qū)杜家坎21號院

E-mail：18606921232@163.com