一種基于單目視覺的無人機室內(nèi)定位方法

聶 偉 文懷志 謝良波 楊小龍 周 牧

(重慶郵電大學(xué)通信與信息工程學(xué)院 重慶 400065)

1 引言

近年來，無人機技術(shù)得到飛速發(fā)展[1]。軍事上，無人機的出現(xiàn)改變了現(xiàn)代戰(zhàn)爭的面貌。2020年9月，亞美尼亞和阿塞拜疆在納卡地區(qū)爆發(fā)軍事沖突。其中，阿塞拜疆使用軍用無人機對亞美尼亞的軍事設(shè)施實行定點清除的畫面令人印象深刻。民用方面，以大疆等品牌為代表的無人機以其體積小、操作方便、機動靈活的特點在防災(zāi)減災(zāi)、土地調(diào)查、環(huán)境監(jiān)測等方面得到廣泛應(yīng)用[2]。

在室外，無人機通過GPS(Global Positioning System)、北斗等定位技術(shù)可以精確地獲得自身的定位信息[3]。然而在室內(nèi)環(huán)境中，由于建筑物當(dāng)中的鋼筋混凝土結(jié)構(gòu)容易屏蔽衛(wèi)星信號，GPS容易受到干擾，信號弱且不穩(wěn)定，使其無法向無人機提供實時準確的定位信息，這將極大地限制無人機在室內(nèi)環(huán)境下的應(yīng)用，例如室內(nèi)安防防控以及自主巡檢等。眾所周知，常見的室內(nèi)定位方法包括激光雷達、運動捕捉系統(tǒng)(Motion Capture System,MCS)、WiFi、藍牙等定位技術(shù)[4]。市面上常見的一些如Gmapping、赫克托等基于激光雷達的定位方案雖然定位精度比較高，但設(shè)備的重量往往比較大而且價格昂貴，并不適合用在一些輕型的民用無人機上面。運動捕捉系統(tǒng)通過部署多個高速攝像機獲取目標在空間中的相對位置，這種方式的缺點是需要提前部署設(shè)備，而且能夠?qū)崿F(xiàn)定位的區(qū)域范圍有限。另外，在像防災(zāi)救險、危險區(qū)域探測這種無人機應(yīng)用場景中提前部署設(shè)備是不現(xiàn)實的做法。UWB (Ultra Wide Band)定位主要采用藍牙、WiFi、基站定位等多種方式融合成一套定位體系[5]。但是該類方法受環(huán)境的影響比較大，而且往往需要部署多個設(shè)備。此外，這種方式只能得到遠近信息，無法知道定位目標在空間中的姿態(tài)。

視覺SLAM(Simultaneous Localization And Mapping)為無人機在沒有GPS信號的情況下的定位提供了一種解決方案[6]。它不需要部署額外的設(shè)備，僅僅依靠自身攜帶的俯視相機，利用SLAM算法，就能實現(xiàn)自身的定位。SLAM算法主要分為兩部分：定位與建圖。定位功能主要是根據(jù)相機輸入的圖像，利用多視圖幾何的原理，計算無人機在當(dāng)前環(huán)境中的位置和姿態(tài)。建圖的目的是建立周邊環(huán)境與空間的對應(yīng)關(guān)系。視覺SLAM算法根據(jù)運動估計和場景結(jié)構(gòu)恢復(fù)的方式可以分為兩類：特征點法與直接法，這兩種方法各有優(yōu)缺點。特征點法在每個圖像上提取特征點，然后進行相鄰圖像間的特征匹配，最后根據(jù)匹配好的特征點來估計相機的運動。這種方法的優(yōu)點是理論成熟，由于穩(wěn)定的視覺特征描述，所以其不易受到光照的影響。但這種方法在弱紋理環(huán)境下會出現(xiàn)特征點匹配困難，從而導(dǎo)致定位失敗。直接法通過最小化相鄰圖像間的光度誤差來優(yōu)化相機的位姿，優(yōu)點是圖像的利用率高，不需要特征點匹配[7]。同時它的缺點也很明顯，由于灰度值不變假設(shè)的原因，直接法對圖像光度變化比較敏感。2007年，文獻[8]提出了一種可以實時運行的單目SLAM系統(tǒng)。它以擴展卡爾曼濾波為后端，追蹤前端非常稀疏的特征點，但存在特征點容易丟失的情況，導(dǎo)致系統(tǒng)運行不穩(wěn)定甚至中斷。2008年，文獻[9]提出了一種小型化的SLAM系統(tǒng)，使用兩個線程實現(xiàn)了定位與建圖過程的并行化，運行速度高，但它的缺陷也比較明顯：場景小，跟蹤容易丟失。對此，文獻[10]在2015年提出了一種改進的SLAM系統(tǒng)，它采用3線程結(jié)構(gòu)：跟蹤線程、局部優(yōu)化線程、全局優(yōu)化線程，通過提取圖像的ORB(Oriented FAST and Rotated BRIEF)特征來估計相機的位姿，同時采用非線性優(yōu)化的方式來進行后端優(yōu)化。運行穩(wěn)定，定位精度高，且支持單目、雙目、RGB-D3種模式。由于它采用特征點法，需要對每幅圖像都計算一遍ORB特征和特征匹配，這是非常耗時的，所以它并不適用于計算資源有限且高速運動的普通小型無人機上面。這是采用特征點法的SLAM系統(tǒng)要想用在無人機上面需要解決的首要問題。2014年，文獻[11]提出了一種基于單目直接法的SLAM，它不需要提取特征點，還能構(gòu)建半稠密的地圖，但需要GPU加速，增加了成本而且耗時比較大。

本文提出了一種基于單目視覺的無人機室內(nèi)定位系統(tǒng)。結(jié)合特征點法和直接法各自的優(yōu)點，提取圖像的特征點，但不需要進行特征匹配，而是采用直接法的方式對無人機進行運動估計[12]。首先，對稀疏的特征塊使用直接法配準，獲取相機的初始位姿，再通過獲取的位姿預(yù)測參考幀中的特征塊在當(dāng)前幀中的位置，然后結(jié)合之前的關(guān)鍵幀對預(yù)測位置做進一步的優(yōu)化，從而得到準確的位置估計。最后根據(jù)新的特征位置，再次使用直接法，對相機位姿進行優(yōu)化。同時，對特征點進行深度估計，建立一個實時更新的稀疏路標地圖。

2 無人機位姿估計

本文提出的無人機位姿估計算法包括3個部分，算法的系統(tǒng)框圖如圖1所示。第1部分是基于稀疏模型的圖像對齊進行位姿初始化，相機相對于前一幀圖像的位姿可以通過最小化相同3D點投影位置之間的光度誤差得到；第2部分是幀間特征位置優(yōu)化，由于上一步的位姿估計發(fā)生在兩兩圖像之間，不可避免地帶入了累積誤差，所以關(guān)聯(lián)上之前的圖像，對特征點在圖像上的2D位置進行優(yōu)化；第3部分是位姿優(yōu)化，經(jīng)過上一步的幀間特征位置優(yōu)化，再次使用直接法，對相機位姿進行優(yōu)化，以最小化在前一個特征對齊步驟中建立的重投影誤差。為了提高算法的效率，在地圖上保留固定數(shù)量的關(guān)鍵幀。選取規(guī)則為：如果新幀相對于所有關(guān)鍵幀的歐氏距離超過平均場景深度的12%，則把它設(shè)為新的關(guān)鍵幀，同時刪除距離相機當(dāng)前位置最遠的那個關(guān)鍵幀。

圖1 位姿估計算法框圖

2.1 圖像稀疏對齊

圖2 稀疏圖像對齊

到目前為止，我們已經(jīng)得到了兩幅圖像之間的位姿，但是由于噪聲的存在，估計值與真實值之間會有誤差，需要進一步優(yōu)化。

2.2 幀間特征位置優(yōu)化

在上一個步驟當(dāng)中，通過直接法最小化相鄰圖像間特征塊的光度誤差求得了一個相對位姿Tk,k?1，但由于在位姿估計過程中不可避免地會存在誤差，而且這個過程只發(fā)生在兩兩圖像之間，誤差會逐漸累積。所以，為了減少這個偏移，相機的姿態(tài)應(yīng)該相對于地圖對齊，而不是與前一幀對齊。

具體做法是通過最小化當(dāng)前幀與之前關(guān)鍵幀中對應(yīng)特征塊之間的光度誤差來單獨優(yōu)化每個特征塊的2D位置，如圖3所示。

圖3 幀間特征位置優(yōu)化

2.3 位姿優(yōu)化

這是一個純運動BA(Bundle Adjustment)問題[14]，同樣可以使用像高斯牛頓法這樣的迭代非線性優(yōu)化算法來求解。到此，我們得到了一個比較準確的無人機位姿估計。

3 無人機單目建圖

在整個無人機室內(nèi)定位算法中，所建立的地圖集記錄了已經(jīng)收斂的路標點，這些路標點用來對無人機進行位姿估計，所以地圖集的精度決定了無人機室內(nèi)定位的精度。本文提出的無人機單目建圖算法流程如圖5所示。首先，判斷當(dāng)前幀是否為關(guān)鍵幀。如果是，提取當(dāng)前幀的FAST特征點[15,16]。為了避免提取的FAST角點過于密集，采用非極大值抑制的方法，只保留一定區(qū)域內(nèi)響應(yīng)值最大的角點。然后初始化一個深度濾波器，初始值設(shè)為當(dāng)前關(guān)鍵幀特征點對應(yīng)3維點的平均深度，通過極線搜索的方式確定匹配點。如果當(dāng)前幀不是關(guān)鍵幀，則更新深度濾波器。同時，如果濾波器收斂，則將估計的3D點加入系統(tǒng)地圖集中，用于后續(xù)的位姿估計或者其他用途。

圖5 無人機單目建圖算法框圖

3.1 FAST特征提取

在計算機視覺領(lǐng)域中，圖像特征點的種類較多。它們大多追求高精確性和高魯棒性，這些特征點在相機的運動和光照變化下仍具有相似的表達，著名的有Harris, SIFT[17,18]等。但需要比較大的計算量，耗費資源。

對快速運動且計算資源有限的無人機來說，使用這些特征點無疑是一種奢侈的行為，因為特征提取只占整個系統(tǒng)的一小部分。所以適當(dāng)降低精度和魯棒性，以提升計算的速度。FAST[15]是一種角點，主要檢測局部像素灰度變化明顯的地方，以計算量小、速度快著稱，適用于快速移動的無人機，如圖6所示。

圖6 FAST特征點

假設(shè)以像素點P為中心，它的亮度為Ip。以P為中心，選取半徑為3的圓上的1 6個像素點，設(shè)置一個閾值T,Sp→x表示像素點P相對于圓周上像素x的亮度大小

圖4 位姿優(yōu)化

3.2 深度濾波

給定強度圖像及其位姿，深度濾波算法估計2D特征點對應(yīng)的3D點的深度，如圖7所示。特征深度估計采用概率分布建模，每個深度濾波器都與一個參考關(guān)鍵幀相關(guān)聯(lián)，由于濾波器初始化時具有比較高的深度不確定性，我們把它的平均值設(shè)置為參考幀中的平均場景深度。對于后續(xù)的每幀圖像，在它的極線上尋找一個與在參考幀上相關(guān)度最高的特征塊，也就是所謂的極線搜索。極線可以通過這兩幀圖像的相對位姿T確定，下面是理論推導(dǎo)。

圖7 深度濾波

在深度濾波器更新的過程中，當(dāng)深度值的不確定性小于一定的閾值后，就可以認為深度數(shù)據(jù)已經(jīng)收斂，最后，把這個收斂的3D點加入到地圖集當(dāng)中。

4 實驗及分析

實驗平臺采用Intel(R) Core(TM) i3-8100 CPU 8核處理器，12GB RAM, 64位Ubuntu 16.04操作系統(tǒng)，開發(fā)軟件采用Visual Studio 2019和OpenCV 2.4.10, Sophus, Fast, G2O，編程采用C++實現(xiàn)。系統(tǒng)運行在ROS環(huán)境下，實驗數(shù)據(jù)采用由無人機俯視相機紀錄的數(shù)據(jù)集。另外，為了實驗效果的可視化，使用3維可視化工具RVIZ對真實環(huán)境進行仿真。實驗包括3部分，分別是FSAT特征提取、單目稠密建圖以及定位結(jié)果分析。

4.1 FSAT特征提取

特征提取算法常見的評價標準有：算法耗時、特征點的分布特性、不同紋理環(huán)境下的表現(xiàn)。采用前面提過的Harris特征點作為對比，設(shè)置每幀圖像提取的特征點數(shù)量上限為400。

為了測試FAST特征提取算法在不同室內(nèi)環(huán)境下的性能，分別在多紋理環(huán)境、少紋理環(huán)境、強光環(huán)境、弱光環(huán)境下測試。結(jié)果如圖8和表1所示，在各種室內(nèi)環(huán)境下，F(xiàn)AST特征提取算法均能提取穩(wěn)定數(shù)量的特征點，大約是Harris算法提取的特征點數(shù)量的3到5倍。而在無人機室內(nèi)定位算法中，只有在跟蹤足夠多的特征點情況下，才能保證位姿估計的精度，在這一點上面，顯然FAST特征點表現(xiàn)得更好。在時間效率上面，F(xiàn)AST可以做到毫秒級，而Harris內(nèi)部由于大量的導(dǎo)數(shù)計算導(dǎo)致只有秒級。綜上所述，本文所使用的FAST特征提取算法滿足了無人機室內(nèi)定位中實時性高、時間開銷少的要求。

表1 不同環(huán)境下兩種算法數(shù)據(jù)指標對比

圖8 特征提取結(jié)果對比

4.2 單目稠密建圖

為了驗證本文所提出的深度估計算法，在REMODE的測試數(shù)據(jù)集[19]上進行單目稠密建圖實驗。該數(shù)據(jù)集包含一架無人機采集的單目圖像，同時提供每張圖像的真實位姿(圖9)。下面在這些數(shù)據(jù)的基礎(chǔ)上估算第1幀圖像每個像素對應(yīng)的深度值，即進行單目稠密重建。實驗結(jié)果如圖10所示。

圖9 數(shù)據(jù)集圖示

圖10 算法運行結(jié)果

初始值使用了均值和方差均為3.0的高斯分布。程序輸出結(jié)果是數(shù)據(jù)集第1幀圖像像素的深度值乘以0.4后的結(jié)果，顏色越深表示深度值越小，也就是物體離我們更近。從算法運行結(jié)果可以看到，深度估計是一個動態(tài)過程，深度值從不確定狀態(tài)逐漸到收斂，當(dāng)?shù)螖?shù)到達30次左右時，深度圖趨于穩(wěn)定，可以大致看出桌子和地板的區(qū)別，而桌子上的物體深度則接近于桌子，說明深度估計基本正確。

4.3 定位結(jié)果分析

將系統(tǒng)部署在Ubuntu 16.04下的ROS環(huán)境中，實驗采用UZH-FPV Drone Racing的測試數(shù)據(jù)集，它提供了一架無人機在室內(nèi)環(huán)境下采集的單目俯視圖像。使用本文所提出的無人機室內(nèi)定位算法對數(shù)據(jù)集中的無人機進行實時軌跡跟蹤，同時建立一個用作路標的稀疏地圖。最后，利用3維可視化工具RVIZ對定位結(jié)果進行仿真，如圖11所示。

將每張圖像提取的最大特征點數(shù)設(shè)置為90，最大關(guān)鍵幀數(shù)設(shè)置為10。圖11顯示了系統(tǒng)運行時的實時特征提取以及無人機的飛行軌跡。從算法運行結(jié)果來看，在室內(nèi)環(huán)境下，無人機僅使用機載單目相機輸入的圖像和位姿估計算法實時估計出了無人機的位置和飛行路徑，并建立可實時更新的稀疏地圖。

圖11 算法運行截圖

下面對定位結(jié)果進行定量分析。將無人機真實軌跡數(shù)據(jù)和算法估計的軌跡數(shù)據(jù)導(dǎo)入到MATLAB中，畫出它們的軌跡對比圖，并計算其均方根誤差(Root Mean Square Error, RMSE)，其中，xi為第i個估計的3D坐標值，yi為真實坐標值。如式(22)所示

從圖12可以看出，估計的無人機軌跡和真實無人機軌跡基本相同。算法剛開始運行時，由于地圖中用于位姿優(yōu)化的路標點還比較少，所以定位誤差比較大。但隨著系統(tǒng)追蹤的特征點越來越多，定位誤差逐漸下降，最后穩(wěn)定在0.04 m左右。作為對比，將本文算法與經(jīng)典PTAM算法[9]在相同的室內(nèi)公共數(shù)據(jù)集序列下運行，結(jié)果如表2所示。本文算法的平均定位精度為0.048 m，PTAM的平均定位精度為0.053 m，兩者相差不大。但在算法耗時上面，由于本文算法不需要進行特征匹配，比PTAM算法的運行速度更快，更適合用在對實時性要求比較高的無人機室內(nèi)定位場景中。

表2 本文算法與PTAM算法對比

圖12 定位結(jié)果分析

5 結(jié)論

針對在GPS信號缺失的室內(nèi)環(huán)境下無人機無法定位的問題，本文提出了一種基于單目視覺的無人機室內(nèi)定位算法。首先，對稀疏的特征塊使用直接法配準，獲取相機的初始位姿，再通過獲取的位姿預(yù)測參考幀中的特征塊在當(dāng)前幀中的位置，然后結(jié)合之前的關(guān)鍵幀對預(yù)測位置做進一步的優(yōu)化，從而得到一個比較準確的位置估計。最后根據(jù)新的特征位置，再次使用直接法，對相機位姿進行優(yōu)化。同時，對特征點進行深度估計，建立一個實時更新的稀疏路標地圖。實驗表明，本文所提出的無人機定位算法在室內(nèi)環(huán)境下表現(xiàn)良好，定位誤差僅為0.04 m。