微型無人機(jī)視覺定位與環(huán)境建模研究

呂 科，施澤南，李一鵬

(1. 中國科學(xué)院大學(xué)工程科學(xué)學(xué)院 北京 石景山區(qū) 100049；2. 清華大學(xué)信息科學(xué)技術(shù)學(xué)院 北京 海淀區(qū) 100084)

微型無人機(jī)視覺定位與環(huán)境建模研究

呂 科1，施澤南1，李一鵬2

(1. 中國科學(xué)院大學(xué)工程科學(xué)學(xué)院 北京 石景山區(qū) 100049；2. 清華大學(xué)信息科學(xué)技術(shù)學(xué)院 北京 海淀區(qū) 100084)

同步定位與環(huán)境建模(SLAM)是實(shí)現(xiàn)無人機(jī)自主飛行和智能導(dǎo)航的關(guān)鍵技術(shù)。該文提出了適用于微型無人機(jī)的視覺定位與環(huán)境建模方法，針對RGB-D傳感器使用point-plane ICP點(diǎn)云匹配算法實(shí)現(xiàn)視覺自主定位，利用并行計(jì)算加速以滿足無人機(jī)控制的實(shí)時(shí)性要求；采用TSDF算法融合多幀觀測的點(diǎn)云數(shù)據(jù)，實(shí)現(xiàn)了無人機(jī)對未知目標(biāo)環(huán)境區(qū)域的模型重建；將視覺SLAM系統(tǒng)與無人機(jī)載IMU傳感器融合，進(jìn)一步提升了自主定位和建模精度。實(shí)際搭建了微型無人機(jī)視覺與環(huán)境建模驗(yàn)證系統(tǒng)，室內(nèi)環(huán)境下可以達(dá)到0.092 m的定位偏差和60 Hz的更新速率，滿足了無人機(jī)控制的精度和實(shí)時(shí)性要求，驗(yàn)證了該方法的有效性。

環(huán)境建模; 微型無人機(jī); 多傳感器; 同步定位與環(huán)境建模; 視覺定位

近年來，無人機(jī)以其低成本、良好的機(jī)動(dòng)性和環(huán)境適應(yīng)能力強(qiáng)等特點(diǎn)，越來越多地被應(yīng)用于航拍、測繪、電力巡檢[1]、災(zāi)害檢測、地質(zhì)勘探等特種行業(yè)。世界主要國家都非常重視無人機(jī)的發(fā)展[2]，隨著技術(shù)的演進(jìn)，人們不滿足于無人機(jī)依賴人和外部設(shè)備的指引來完成任務(wù)，如何使其具有更高的智能、更強(qiáng)的自主性乃至多機(jī)組成機(jī)群協(xié)同完成任務(wù)[3-4]成為了研究的重點(diǎn)。

自主定位和環(huán)境感知能力是移動(dòng)機(jī)器人實(shí)現(xiàn)自主和智能的關(guān)鍵，相關(guān)技術(shù)研究具有重要的理論和應(yīng)用價(jià)值。與地面機(jī)器人相比，無人機(jī)的運(yùn)動(dòng)范圍更廣闊，6自由度運(yùn)動(dòng)模式需要構(gòu)建三維的環(huán)境模型來完成飛行任務(wù)，此類問題被稱為6自由度的同步定位與環(huán)境建模問題(SLAM)[5-6]。應(yīng)用于無人機(jī)系統(tǒng)的SLAM算法，在魯棒性、精確性等諸多方面面臨著重大挑戰(zhàn)。特別對于微型無人機(jī)而言，更需要考慮負(fù)重、體積和飛行時(shí)間，對于自主定位和環(huán)境模型構(gòu)建的計(jì)算復(fù)雜性和實(shí)時(shí)性提出了更高的要求。國內(nèi)外目前典型的SLAM研究工作多停留在二維定位和建圖上，典型的工作如文獻(xiàn)[7]提出的利用下視相機(jī)完成地面二維地圖繪制和定位，文獻(xiàn)[8]提出的利用激光掃描儀實(shí)現(xiàn)的平面環(huán)境的建圖和定位，文獻(xiàn)[9]提出的使用視覺里程計(jì)的機(jī)器人定位等。

無人機(jī)對定位要求較高，容錯(cuò)率低，一旦失去定位會(huì)面臨失控甚至墜毀的風(fēng)險(xiǎn)，因此應(yīng)用于無人機(jī)的SLAM算法需要充分考慮系統(tǒng)的穩(wěn)定性和可靠性。相關(guān)工作中通常采用多種傳感器數(shù)據(jù)融合的方法以提升整體的穩(wěn)定性和抗干擾能力[10-11]。

本文以RGB-D相機(jī)為主要視覺傳感器，首先使用point-plane ICP[12]點(diǎn)云匹配算法實(shí)現(xiàn)無人機(jī)的視覺自主定位，通過并行計(jì)算加速以滿足實(shí)時(shí)性要求；然后在自主定位的基礎(chǔ)上，通過對所觀測的環(huán)境三維點(diǎn)云進(jìn)行融合，實(shí)現(xiàn)了無人機(jī)對未知目標(biāo)環(huán)境區(qū)域的模型重建；最后將視覺SLAM系統(tǒng)和無人機(jī)飛行控制系統(tǒng)整合，提出了視覺定位及無人機(jī)傳感器融合以提高定位和導(dǎo)航精度的方法，并在實(shí)際搭建的四旋翼無人機(jī)系統(tǒng)上驗(yàn)證了該算法的有效性和精確性。

1 視覺定位系統(tǒng)

1.1 傳感器選擇

本文選取RGB-D相機(jī)作為主要的視覺傳感器。RGB-D相機(jī)可直接獲取環(huán)境深度信息，從而計(jì)算出環(huán)境表面的三維點(diǎn)云坐標(biāo)，相比于一般的攝像頭，RGB-D相機(jī)對環(huán)境紋理不敏感，且對光照條件依賴性較小，因此適用作微型無人機(jī)的定位傳感器，具有較強(qiáng)的適應(yīng)性和可靠性。

目前商用消費(fèi)級RGB-D相機(jī)，如Microsoft kinect和華碩Xtion,，重量約為400～500 g，攝像頭分辨率約為640×480，深度測量精度達(dá)到厘米級，而功耗僅為5 W，適合用作無人機(jī)機(jī)載傳感器。

1.2 Poing-plane ICP定位算法

在定位系統(tǒng)中，使用point-plane ICP算法[12]來匹配不同位置所觀測的點(diǎn)云數(shù)據(jù)，從而實(shí)現(xiàn)定位。Point-plane ICP是一個(gè)直接匹配算法，即不對圖像進(jìn)行任何的稀疏化處理，如提取特征點(diǎn)、提取邊緣等方法，而是直接用圖像的全部信息進(jìn)行計(jì)算。因充分利用了全部的信息，在精度、可靠性、和魯棒性上相比基于特征點(diǎn)和特征信息的匹配算法具有相當(dāng)優(yōu)勢。

記相機(jī)第k幀觀測的深度圖像為Zk:Ω→R+，其中Ω為像素集合，使用相機(jī)小孔成像模型，已知相機(jī)內(nèi)參矩陣為：

可以計(jì)算當(dāng)前觀測點(diǎn)云Vk:Ω→R3和法向量Nk:?→R3分別為：

k時(shí)刻相機(jī)的6自由度位姿可用齊次坐標(biāo)變換表示為：

式中，Rk∈ SO(3)，tk∈R3。該位姿變換對應(yīng)的基準(zhǔn)坐標(biāo)系為世界坐標(biāo)系，以符號g表示，點(diǎn)云坐標(biāo)也以齊次坐標(biāo)表示。k時(shí)刻所觀測的點(diǎn)云和法向量投影至世界坐標(biāo)系下為：

不失一般性，假設(shè)Tk?1已知，point-plane ICP算法認(rèn)為Tk的最佳估計(jì)值將k?1幀點(diǎn)云映射到第k幀之后，使得對應(yīng)點(diǎn)之間的誤差會(huì)達(dá)到最小。這里使用L2范數(shù)作為誤差函數(shù)，則將計(jì)算k時(shí)刻位姿Tk的問題轉(zhuǎn)化成最小化誤差的問題，有：

該問題為非線性優(yōu)化問題，采用局部線性化的高斯?牛頓法即可求解。高斯?牛頓法是迭代算法，每次迭代的增量只需計(jì)算目標(biāo)函數(shù)在當(dāng)前位置的一階微分，由于避免了求解目標(biāo)函數(shù)的二階微分，因此每次迭代均為線性計(jì)算，易于快速計(jì)算。

注意到該優(yōu)化問題的凸性只在局部滿足，當(dāng)初值距離最優(yōu)解較遠(yuǎn)時(shí)，可能收斂至錯(cuò)誤的局部極小值。為了進(jìn)一步提升算法的精度和可靠性，對每一幀觀測的深度圖均構(gòu)建3層的圖像金字塔結(jié)構(gòu)，每一層的分辨率為上一層的1/2，以此完成一個(gè)由粗糙至精細(xì)的迭代計(jì)算。

由于需要對所有的點(diǎn)云進(jìn)行計(jì)算，因此pointplane ICP算法雖然能夠提供較好的精度，但是難以滿足勻速速度的要求。考慮到迭代過程中每一步計(jì)算中各像素的數(shù)值是逐像素計(jì)算，該算法實(shí)際上具有高度并行化的特點(diǎn)，如果利用GPU進(jìn)行加速，可以大大提高運(yùn)算速度。本文采用CUDA對GPU進(jìn)行編程，將該算法利用GPU并行計(jì)算實(shí)現(xiàn)，并測試其計(jì)算時(shí)間(測試平臺(tái)：Nvidia-Geforce GTX 980)如表1所示?？梢钥闯?，經(jīng)CUDA加速后，該算法足夠讓定位系統(tǒng)以較高的幀率(＞60 Hz)運(yùn)行，并且相鄰兩幀點(diǎn)云匹配準(zhǔn)確度高。圖1展示了兩幀點(diǎn)云匹配的結(jié)果。

表1 point-plane ICP算法運(yùn)算時(shí)間測試結(jié)果

圖1 point-plane ICP算法匹配的兩幀點(diǎn)云

1.3 點(diǎn)云融合與場景重建

本文使用TSDF算法實(shí)現(xiàn)了多幀觀測點(diǎn)云數(shù)據(jù)的去噪、融合，并最終實(shí)現(xiàn)了對無人機(jī)所觀測的目標(biāo)場景較為精細(xì)的三維重建。SDF(signed distance function)模型由文獻(xiàn)[13]提出。該模型將空間分為密集的柵格，每個(gè)柵格的數(shù)值代表當(dāng)前格點(diǎn)到物體表面的距離，負(fù)值代表當(dāng)前格點(diǎn)在物體內(nèi)部(不可見)，正值代表當(dāng)前格點(diǎn)為空，而所有的過零點(diǎn)連接在一起即為所觀測的場景的可見表面的輪廓。在工程實(shí)際中，通常將SDF數(shù)值歸一化至(?1,1)，即為TSDF。基于TSDF的場景點(diǎn)云融合算法已在文獻(xiàn)[14-16]中得到成功應(yīng)用。圖2所示為TSDF模型表達(dá)下的某物體模型。

本文使用TSDF模型進(jìn)行點(diǎn)云融合時(shí)，使用高斯分布作為觀測的噪聲模型，因此在TSDF模型中的每一個(gè)柵格內(nèi)均記錄了SDF和該SDF的高斯分布權(quán)重(F(p),W(p))，p∈R3。第k幀觀測后，p點(diǎn)處的TSDF值使用式(6)和式(7)進(jìn)行融合更新：

圖2 TSDF模型示意圖

TSDF模型由于采用了高斯噪聲模型下的最優(yōu)估計(jì)，因此對三維點(diǎn)云的噪聲有很強(qiáng)的抑制作用，能夠很好地消除因無人機(jī)抖動(dòng)等因素帶來的測量誤差，具有非常好的魯棒性，并且能夠重建出比較精細(xì)的場景模型。

圖3a和圖3b分別展示了未經(jīng)TSDF模型融合處理的點(diǎn)云和經(jīng)TSDF融合后的點(diǎn)云?？梢钥闯觯诤狭硕鄮瑪?shù)據(jù)的TSDF模型比原始的場景模型更加精確和細(xì)致。

圖3 TSDF點(diǎn)云融合效果

2 無人機(jī)系統(tǒng)綜合架構(gòu)及其控制

在視覺定位和環(huán)境建模算法的基礎(chǔ)上，用四旋翼無人機(jī)搭建了完整的系統(tǒng)，實(shí)現(xiàn)了無人機(jī)在未知環(huán)境下的自主定位、導(dǎo)航和場景模型重建，驗(yàn)證了本文算法的有效性。另外，通過將視覺定位數(shù)據(jù)與無人機(jī)飛控所搭載的多種傳感器，如加速度計(jì)、陀螺儀等數(shù)據(jù)進(jìn)行融合，提升了整體測量和控制的可靠性。

2.1 視覺定位系統(tǒng)與機(jī)載傳感器的融合

為了保證無人機(jī)的穩(wěn)定飛行，無人機(jī)上通常搭載了高速、高精度的陀螺儀、用于姿態(tài)估計(jì)和自穩(wěn)的加速度計(jì)，以及用于飛行時(shí)的高度增穩(wěn)氣壓計(jì)、超聲等傳感器。在室內(nèi)飛行時(shí)不考慮外部定位設(shè)備(如GPS)，自主定位中數(shù)據(jù)融合對提升無人機(jī)飛行控制的精度及抗干擾能力非常重要[17-18]。其中常用的濾波算法有粒子濾波[8,19]和擴(kuò)展卡爾曼濾波(EKF)，其中粒子濾波雖然精度較高，但是運(yùn)算量偏大，不適合對實(shí)時(shí)性要求很高的無人機(jī)控制，因此本文研究工作中利用EKF中的最優(yōu)估計(jì)原理將視覺定位數(shù)據(jù)與其他傳感器的數(shù)據(jù)進(jìn)行融合，實(shí)現(xiàn)了以視覺定位為主，其他傳感器為輔的自主定位系統(tǒng)，增強(qiáng)了整體定位的精度、穩(wěn)定性和可靠性。同時(shí)該定位系統(tǒng)由于充分利用了陀螺儀、加速度計(jì)的數(shù)據(jù)(250～ 1 000 Hz)，填補(bǔ)了相機(jī)兩個(gè)采樣周期之間的空白，使得整體定位和控制效果更加流暢。

假設(shè)對于無人機(jī)速度、位置的測量和估計(jì)噪聲均滿足高斯分布，令表示地面坐標(biāo)系下第k次采樣時(shí)無人機(jī)速度矢量的分布，N(Xe(k),(k))表示位置矢量的分布。對于第i種傳感器，其測量的速度和位置矢量表示為和。由于加速度和陀螺儀的測量頻率遠(yuǎn)高于相機(jī)和其他傳感器的測量頻率，因此整體位置估計(jì)模型以加速度計(jì)的測量值為主要預(yù)測，并與其他傳感器進(jìn)行融合。

設(shè)無人機(jī)利用陀螺儀和加速度計(jì)做出的姿態(tài)估計(jì)矩陣為R，加速度計(jì)在t時(shí)刻觀測的機(jī)身坐標(biāo)系下的加速度矢量為a (t)=[ax(t),ay(t),az(t )]T，則速度估計(jì)模型和位置估計(jì)模型分別為：

2.2 系統(tǒng)整體架構(gòu)

圖4描述了結(jié)合視覺定位和場景重建的無人機(jī)系統(tǒng)算法架構(gòu)。在該系統(tǒng)中，圖像數(shù)據(jù)所提供的定位信息結(jié)合無人機(jī)載的多種傳感器數(shù)據(jù)，經(jīng)過融合、濾波，得到對無人機(jī)自身位置和姿態(tài)較為精確的估計(jì)。通過對環(huán)境的持續(xù)觀察，該系統(tǒng)可以構(gòu)建較為精確的環(huán)境模型，并以此模型為依據(jù)，在無人機(jī)飛行控制器的協(xié)同下，實(shí)現(xiàn)無人機(jī)的定位和導(dǎo)航。

圖4 系統(tǒng)軟件架構(gòu)框圖

因?yàn)槿S點(diǎn)云匹配算法需要依靠GPU并行計(jì)算加速來達(dá)到滿足無人機(jī)實(shí)時(shí)控制的要求，因此本文系統(tǒng)對于計(jì)算平臺(tái)的運(yùn)算性能要求較高，完整的無人機(jī)系統(tǒng)采用機(jī)載處理器與地面站協(xié)同計(jì)算的方案來實(shí)現(xiàn)。無人機(jī)機(jī)載飛行控制器選用Pixhawk飛控，在其開源控制算法的基礎(chǔ)上整合視覺定位及導(dǎo)航系統(tǒng)。無人機(jī)機(jī)載處理器通過無線局域網(wǎng)和地面站實(shí)現(xiàn)數(shù)據(jù)通信，將觀測數(shù)據(jù)發(fā)送至地面站，由地面站進(jìn)行計(jì)算后再發(fā)送回?zé)o人機(jī)，供無人機(jī)控制系統(tǒng)進(jìn)行位置和軌跡控制，如圖5所示。

圖5 無人機(jī)和地面站協(xié)同計(jì)算流程圖

3 實(shí)驗(yàn)結(jié)果

實(shí)驗(yàn)時(shí)使用微型四旋翼無人機(jī)作為實(shí)驗(yàn)平臺(tái)，搭載Microsoft kinect作為RGB-D相機(jī)，選用典型的室內(nèi)環(huán)境進(jìn)行自主定位和環(huán)境建模測試。將無人機(jī)放置在完全未知，并且沒有外部輔助定位的室內(nèi)環(huán)境下，由系統(tǒng)自動(dòng)完成無人機(jī)的自主定位和環(huán)境建模，在完全無人工干預(yù)的情況下實(shí)現(xiàn)對目標(biāo)區(qū)域的探索，并同時(shí)對環(huán)境進(jìn)行建模。測試中，選用不同的室內(nèi)環(huán)境，反復(fù)進(jìn)行多次實(shí)驗(yàn)，以驗(yàn)證其有效性和可靠性。圖6為無人機(jī)對目標(biāo)環(huán)境進(jìn)行自主探索，并重建出環(huán)境模型的過程。

圖6 無人機(jī)自主定位和環(huán)境建模過程

實(shí)際飛行結(jié)果顯示，該系統(tǒng)在不借助GPS等外部定位設(shè)備的幫助下，可以實(shí)現(xiàn)微型無人機(jī)的自主定位，并且可以自動(dòng)完成對目標(biāo)區(qū)域的自主探索。在飛行測試中，對室內(nèi)區(qū)域的10次探索均順利完成，顯示了該定位系統(tǒng)融合了無人機(jī)機(jī)載多傳感器的數(shù)據(jù)后，對實(shí)際飛行中偶爾遇到的氣流、振動(dòng)等因素導(dǎo)致的干擾具有一定抵抗能力，同時(shí)其對環(huán)境模型建立的稠密程度也能滿足微型無人機(jī)的導(dǎo)航需要。

為了進(jìn)一步驗(yàn)證定位算法和所建立模型的準(zhǔn)確性，使用公用測試數(shù)據(jù)集FR2-Desk檢驗(yàn)定位算法的偏差，并對比相似基于RGB-D相機(jī)的定位算法[14]，結(jié)果如表2所示?？梢钥闯觯琾oint-plane ICP算法由于充分利用了點(diǎn)云信息，精度更高，整體路徑偏差更小。

表2 定位偏差測試結(jié)果

圖7為了無人機(jī)對目標(biāo)空間探索后的重建的環(huán)境模型，由圖可以看出，重建的模型精度較高，人體表面及衣物輪廓均清晰可見，整體模型比較精確和細(xì)致。

圖7 無人機(jī)對目標(biāo)區(qū)域探索后建立的模型

4 結(jié) 束 語

實(shí)驗(yàn)結(jié)果表明，本文提出的搭載RGB-D相機(jī)作為主要傳感器，利用point-plane ICP算法實(shí)現(xiàn)的無人機(jī)視覺定位和建模系統(tǒng)，在定位速度、精度上均能滿足微型無人機(jī)飛行控制的需求。由于采用了合適的場景建模點(diǎn)云融合算法，對環(huán)境模型重建的精度較好，并且在實(shí)際飛行測試中，視覺定位和環(huán)境建模系統(tǒng)與無人機(jī)飛行控制器中的其他傳感器融合，整體控制的可靠性和穩(wěn)定性較好，能夠?qū)崿F(xiàn)微型無人機(jī)對室內(nèi)位置環(huán)境的自主定位、探索并最終重建出目標(biāo)環(huán)境的模型。本文研究中用于無人機(jī)定位和自主導(dǎo)航的主要傳感器為RGB-D相機(jī)，受RGB-D相機(jī)傳感器的距離限制，適合在較小范圍的場景中應(yīng)用?？紤]到工程和實(shí)際的結(jié)合，未來的研究工作可在本文提出的多傳感器融合的框架基礎(chǔ)上，通過多種傳感器結(jié)合和改進(jìn)算法的方式，拓展觀測距離，以適應(yīng)不同場景的需求，提升其實(shí)用性。

[1] 王淼, 杜毅, 張忠瑞. 無人機(jī)輔助巡視及絕緣子缺陷圖像識別研究[J]. 電子測量與儀器學(xué)報(bào), 2015, 29(12): 1862-1869.

WANG Miao, DU Yi, ZHANG Zhong-rui. Study on power transmission lines inspection using unmanned aerial vehicle and image recognition of insulator defect[J]. Journal of Electronic Measurement and Instrument, 2015, 29(12): 1862-1869.

[2] United States Department of Defense. Unmanned systems integrated roadmap[R]. [S.l.]: [s.n.], 2014: 22-47.

[3] 姚敏, 王緒芝, 趙敏. 多旋翼無人機(jī)群協(xié)同作戰(zhàn)任務(wù)分配方法研究[J]. 電子科技大學(xué)學(xué)報(bào), 2013, 42(5): 723-727.

YAO Min, WANG Xu-zhi, ZHAO Min. Coop erative combat task assignment optimization design for unmanned aerial vehicles cluster[J]. Journal of University of Electronic Science and Technology of China, 2013, 42(5): 723-727.

[4] 趙敏, 姚敏. 無人機(jī)群變航跡多任務(wù)綜合規(guī)劃方法研究[J]. 電子科技大學(xué)學(xué)報(bào), 2010, 39(4): 560-563.

ZHAO Min, YAO Min. Study on changeable path planning and multi-task assignment optimization design forunmanned aerial vehicles cluster[J]. Journal of University of Electronic Science and Technology of China, 2010, 39(4): 560-563.

[5] DURRANT-WHYTE H, BAILEY T. Simultaneous localization and mapping: Part I[J]. IEEE Robotics and Automation Magazine, 2006, 13(3): 99-108.

[6] DURRANT-WHYTE H, BAILEY T. Simultaneous localization and mapping: Part II[J]. IEEE Robotics and Automation Magazine, 2006, 13(3): 108-117.

[7] ACHTELIK M W, LYNEN S, WEISS S, et al. Visual-inertial SLAM for a small helicopter in large outdoor environments[C]//2012 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Vilamoura: IEEE, 2012: 2651-2652.

[8] GRISETTI G, STACHNISS C, BURGARD W. Improved techniques for grid mapping with rao-blackwellized particle filters[J]. IEEE Transactions on Robotics, 2007, 23(1): 34-46.

[9] 任沁源, 李平. 結(jié)合視覺里程計(jì)的微小型空中機(jī)器人SLAM研究[J]. 儀器儀表學(xué)報(bào), 2013, 34(2): 475-480.

REN Qin-yuan, LI Ping. The SLAM combing with visual odometry for small-scale aerial robots[J]. Chinese Journal of Scientific Instrument, 2013, 34(2): 475-480.

[10] 夏凌楠, 張波, 王營冠, 等. 基于慣性傳感器和視覺里程計(jì)的機(jī)器人定位[J]. 儀器儀表學(xué)報(bào), 2013, 34(1): 166-172.

XIA Ling-nan, ZHANG Bo, WANG Ying-guan, et al. Robot localization algorithm based on inertial sensor and video odometry[J]. Chinese Journal of Scientific Instrument, 2013, 34(1): 166-172.

[11] LYNEN S, ACHTELIK M W, WEISS S, et al. A robust and modular multi-sensor fusion approach applied to mav navigation[C]//2013 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Tokyo: IEEE, 2013: 3923-3929.

[12] BESL P, MCKAY N. A method for registration of 3D shapes[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence (PAMI), 1992, 14(2): 239-256.

[13] CURLESS B, LEVOY M. A volumetric method for building complex models from range images[J]. In ACM Transactions on Graphics (SIGGRAPH), 1996, 3: 303-312.

[14] WHELAN T, JOHANNSSON H, KAESS M, et al. Robust real-time visual odometry for dense RGB-D mapping[C]// 2013 IEEE International Conference on Robotics and Automation (ICRA). [S.l.]: IEEE, 2013: 5724-5731.

[15] NEWCOMBE R A, IZADI S, HILLIGES O, et al. KinectFusion: Real-time dense surface mapping and tracking[C]//2011 10th IEEE International Symposium on Mixed and Augmented Reality (ISMAR). [S.l.]: IEEE, 2011: 127-136.

[16] WHELAN T, KAESS M, FALLON M, et al. Kintinuous: spatially extended Kinect fusion[R/OL]. (2012-07-05) [2015-11-15]. https://www.researchgate.net/publication/ 279843674_Kintinuous_Spatially_Extended_KinectFusion.

[17] KIM J H, SALAH S. 6DoF SLAM aided GNSS/INS navigation in GNSS denied and unknown environments[J]. Journal of Global Positioning Systems 4, 2005, 1: 120-128.

[18] STRASDAT H, MONTIEL J M M, DAVISON A J. Real-time monocular SLAM: Why filter?[C]//IEEE International Conference on Robotics and Automation (ICRA). [S.l.]: IEEE, 2010, 58(8): 2657-2664.

[19] 劉志花, 李淑芬, 李寶安. 基于粒子濾波的無人機(jī)航跡預(yù)測方法研究[J]. 電子測量與儀器學(xué)報(bào), 2010, 24(7): 643-649.

LIU Zhi-hua, LI Shu-feng, LI Bao-an. Research of UAV track forecast based on particle filter[J]. Journal of Electronic Measurement and Instrument, 2010, 24(7): 643-649.

編 輯 漆 蓉

Visual Localization and Environment Mapping for Micro Aerial Vehicles

Lü Ke1, SHI Ze-nan1, and LI Yi-peng2

(1. School of Engineering Science, University of Chinese Academy of Sciences Shijingshan Beijing 100049; 2. School of Information Science and Technology, Tsinghua University Haidian Beijing 100084)

Simultaneous localization and mapping (SLAM) is a key technology for unmanned aerial vehicle (UAV) autonomous flight and navigation. This paper presents a visual localization and environment mapping method for micro UAV. In the method, the point-plane ICP point cloud matching algorithm is used to achieve visual localization based on RGB-D sensor, the parallel computing acceleration is applied to meet the real-time requirements of UAV control, then the truncated signed distance function (TSDF) algorithm is used for multi-frame point cloud data fusion to achieve environment model reconstruction for an unknown environment, and at last the fusion of visual SLAM system and UAV IMU sensor data is applied to enhance the precision of autonomous localization and model reconstruction. Finally, we set up a micro UAV system with full visual SLAM algorithm and test the whole system in a typical indoor environment. The result shows a 0.092 m positioning deviation and 60 Hz update rate, capable for real-time UAV controlling accuracy requirement, thus verifying the effectiveness of the method.

environment mapping; micro aerial vehicle; multi-sensor; simultaneous localization and mapping (SLAM); visual odometry

TP242.6

A

10.3969/j.issn.1001-0548.2017.03.011

2016 ? 03 ? 09；

2016 ? 06 ? 30

國家自然科學(xué)基金面上項(xiàng)目(61271268)

呂科(1971 ? )，男，博士，教授，主要從事遙感圖像處理、醫(yī)學(xué)影像處理和計(jì)算圖形學(xué)等方面的研究.