基于視覺延時補償?shù)臒o人機室內(nèi)實時導(dǎo)航系統(tǒng)

許承宇 徐紹凱

（1.中國航空工業(yè)集團公司洛陽電光設(shè)備研究所，洛陽 471000）（2.普聯(lián)技術(shù)有限公司，杭州 310000）

引言

近幾十年來，隨著傳感器技術(shù)的發(fā)展和控制理論的完善，無人機的應(yīng)用越來越廣泛.導(dǎo)航定位的高精度和高性能是實現(xiàn)無人機（Unmanned Aerial Vehicle，UAV）廣泛應(yīng)用的重要前提，在導(dǎo)航系統(tǒng)中，通常需要使用多種類型的傳感器來實現(xiàn)不同的運動感知目的.因此，傳感器數(shù)據(jù)融合是必不可少的，但同時在融合的過程中也存在傳感器輸出數(shù)據(jù)延時的問題.

對于無人機的室內(nèi)導(dǎo)航系統(tǒng)而言，慣性測量單元（Inertial measurement unit，IMU）能夠提供慣性信息[1]，是無人機最常用的傳感器之一.另外，由于視覺傳感器可以提供豐富的信息量，近年來得到了廣泛應(yīng)用，視覺數(shù)據(jù)中不僅包括彩色圖像信息，還可以計算出包括位移、速度、目標(biāo)位置等運動信息.將視覺傳感器與慣性傳感器相結(jié)合，組成視覺輔助慣性導(dǎo)航系統(tǒng)，是目前無人機導(dǎo)航十分重要的方法.但是慣性傳感器和視覺傳感器需要做預(yù)先的同步和融合才能作為控制系統(tǒng)的輸入，因此必須解決每個傳感器之間的時間延時差問題.

傳感器延時主要是指視覺傳感器的輸出延時.一方面，因為視覺信息通常需要經(jīng)過復(fù)雜的操作處理，比如在用到實時定位和地圖重建（Simultaneous localization and mapping，SLAM）[2]等技術(shù)時，需要進行大量圖像數(shù)據(jù)計算工作，耗費巨大的計算資源，在硬件性能有限時會有很大的時間延遲.另一方面，如果無人機機載系統(tǒng)無法滿足圖像處理的硬件要求，就必須通過無線模塊將視覺數(shù)據(jù)發(fā)送回地面站，這就需要一定的通信時間，不可避免地導(dǎo)致更大的延時.

為了實現(xiàn)合理的傳感器數(shù)據(jù)融合，必須準(zhǔn)確估計這幾個傳感器之間的輸出延時差.對于視覺與IMU相結(jié)合的傳感器系統(tǒng)而言，視覺數(shù)據(jù)延時通常難以準(zhǔn)確估算[3]，使用擴展卡爾曼濾波器（Extended Kalman filter，EKF）創(chuàng)建了一種視覺輔助慣性導(dǎo)航系統(tǒng)（Visual-Inertial navigcation system，V-INS），它可以有效地融合IMU和視覺數(shù)據(jù)，但是并沒有考慮到視覺延時[4].提出了一種改進的卡爾曼濾波器，在假設(shè)延時為常數(shù)的情況下補償了延時，并且構(gòu)造了周期函數(shù)作為測量協(xié)方差，用以提高融合精度.在文獻[5]中開發(fā)了一種名為時間延時迭代最臨近點（Time Delay Iterative Closest Point，TD-ICP）的方法，該方法在3-D空間中找到兩條曲線的對應(yīng)關(guān)系以找到最臨近點，將其中的時間差看作視覺數(shù)據(jù)延時.然而，TD-ICP需要大量的計算時間，并且需要精確的校準(zhǔn)和初始化，本文參考了該方法并加以改進.針對分布式網(wǎng)絡(luò)控制系統(tǒng)[6]，提出了一種延時補償方法，但在整體化模型中應(yīng)用效果較差[7].在視覺延時估計中使用了積分方法，為本文提供了靈感，但是在使用傳感器計算運動速度時，它沒有正確地整合加速度計數(shù)據(jù)和視覺數(shù)據(jù)，估計精度還有改進的空間.

本文提出了迭代最鄰近點（Tterative Closest Point，ICP）的一種變體算法來估計延時，可以簡化傳統(tǒng)算法并大大減少計算時間.在此估計的時間延時內(nèi)，根據(jù)加速度計的測量結(jié)果，預(yù)測出延時補償后無人機的速度，并進一步估計出運動的位移.在統(tǒng)一好各傳感器的坐標(biāo)系方向后，使用EKF將IMU測量值和補償后的視覺數(shù)據(jù)融合在一起，用于無人機的定位和導(dǎo)航.

本文在第1部分中，提供了IMU的數(shù)據(jù)模型；在第2部分中給出了ICP方法的簡化變體，運用迭代求解的方法估算出視覺數(shù)據(jù)的延時；在第3部分中，本文使用EKF融合了IMU數(shù)據(jù)和補償后的視覺數(shù)據(jù)；第4部分是實驗和分析，在這部分中使用了不同數(shù)據(jù)源的輸入進行對比，給出了幾組對照實驗的結(jié)果；第5部分總結(jié)了本文.

1 IMU模型

IMU包含加速度計和陀螺儀，分別提供加速度和角速度.在無人機航姿參考系統(tǒng)（Attitude and Heading Reference System，AHRS）[8]中可以通過融合加速度(ax，ay，az)和角速度(?，θ，ψ)來估計無人機的姿態(tài).

1.1 加速度計測量模型

加速度計可以測量物體的線性加速度.加速度計的輸出通常包括瞬時加速度以及無法避免的偏差和噪聲，可表示為

其中，a′是實際的加速度，而a是傳感器測量出的加速度；Ka是一個比例參數(shù)，為常數(shù)，由傳感器自身特性決定；ba是偏差項，包含零偏和由溫度變化引起的偏差；na是高斯白噪聲.通?？杉俣╞a中的零偏為常數(shù)，因此可以在初始化期間對其進行補償.溫度變化引起的偏差會隨環(huán)境溫度而變化，因此該偏差可以通過測溫模塊進行校準(zhǔn)；g是重力加速度，在計算運動加速度時，首先應(yīng)消除重力影響；Τα是一個旋轉(zhuǎn)矩陣，用于對齊加速度計和無人機機體的正方向.

1.2 陀螺儀數(shù)據(jù)模型

陀螺儀可測量繞三個正交軸上的角速度.與加速度計類似，陀螺儀的數(shù)據(jù)模型可表示為

這里，ω′是實際角速度，而ω是測量出的結(jié)果；bω是偏差；nω是高斯白噪聲；同樣地，Kω和Τω表示比例參數(shù)和相應(yīng)的旋轉(zhuǎn)矩陣.

1.3 IMU測量延時

由于IMU是微型機械電子系統(tǒng)（Micro Electro Mechanical System，MEMS）[9]，在測量過程中會因為自身的機電構(gòu)造而存在固有的輸出延時.并且通常原始的IMU輸出噪聲較大，需要在使用過程中進行濾波處理，這樣更會導(dǎo)致延時變大.加速度計和陀螺儀的數(shù)據(jù)輸出延時可以從IMU的數(shù)據(jù)手冊中找到，以目前常用的慣性傳感器MPU6000為例，可以從其數(shù)據(jù)手冊中找到如表1所示的輸出延時.其中tda和tdω分別代表加速度計和陀螺儀的輸出延時.從表1中可以看出，如果選擇了相同的濾波參數(shù)fp，則tda和tdω之間只有很小的差異.

隨著計算機硬件性能的迅猛發(fā)展，尤其是CPU中央運算處理器的發(fā)展,計算機視覺技術(shù)和生物識別技術(shù)得到了飛速的發(fā)展，各種各樣的人機交互軟件不斷推陳出新，生物特征識別技術(shù)也逐漸應(yīng)用到智能門禁系統(tǒng)中。當(dāng)下、各小區(qū)車庫道閘淘汰射頻卡技術(shù)，進出小區(qū)只需刷車臉，道閘均會自動起降。究其原因是利用圖像識別技術(shù)，性能穩(wěn)定可靠?；谌四樧R別(刷臉識別)的智能門禁系統(tǒng)由于友好的用戶體驗以及成熟的技術(shù)積淀而受到廣泛認(rèn)可。

表1 MPU6000數(shù)據(jù)輸出延時Table 1 MPU6000 data output delay

2 視覺數(shù)據(jù)模型

通過使用恰當(dāng)?shù)囊曈X里程計或SLAM算法[10]，可以利用視覺傳感器系統(tǒng)來較準(zhǔn)確地計算出無人機的實時位姿.本文利用ORB-SLAM2[11]算法來計算視覺位姿，該算法可以通過估計兩個連續(xù)圖像幀之間的姿態(tài)變化來跟蹤攝像機的運動，并將關(guān)鍵幀存儲起來以創(chuàng)建本地地圖.在經(jīng)過ORB-SLAM2算法的處理后，可以由圖像數(shù)據(jù)計算出無人機的運動狀態(tài)量Χt，Χt中包括位置坐標(biāo)信息和無人機的姿態(tài)角信息，如下所示：

其中，Τv是旋轉(zhuǎn)矩陣，用于統(tǒng)一相機坐標(biāo)系和無人機機體坐標(biāo)系，可表示為：

在上式中，β表示兩坐標(biāo)軸間的旋轉(zhuǎn)，γ表示兩個姿態(tài)軸之間的旋轉(zhuǎn).

2.1 視覺數(shù)據(jù)延時

本文使用的延時估計算法是基于ICP的一種簡化變體.我們將所有由視覺數(shù)據(jù)計算出的姿態(tài)角信息保存為數(shù)據(jù)集Vt，將所有由AHRS估計出的無人機姿態(tài)角數(shù)據(jù)保存為數(shù)據(jù)集At，使用迭代的方法求解數(shù)據(jù)集Vt與數(shù)據(jù)集At之間對應(yīng)的最鄰近點.需要注意的是數(shù)據(jù)集中每項數(shù)據(jù)都包含時間信息，也就是數(shù)據(jù)集Vt和At之間的延時差可以視為時間軸上的平移運動，如果不考慮兩個數(shù)據(jù)集間存在旋轉(zhuǎn)關(guān)系，就可以將延時TD初步表示為：

上式中，NA代表At中有效數(shù)據(jù)的個數(shù)，下標(biāo)t和t+TD表示數(shù)據(jù)采集的時刻.求解上式需要運用ICP的思路，首先分別計算兩個數(shù)據(jù)集的去質(zhì)心點集，數(shù)據(jù)集Vt和At中的姿態(tài)角用歐拉角的形式表示為如下的3×3矩陣：

分別計算出兩個數(shù)據(jù)集的質(zhì)心，用μV和μA表示

則數(shù)據(jù)集的去質(zhì)心點集為

接下來將去質(zhì)心點集代入公式（3）中，就可以使用奇異值分解的方法搜索最優(yōu)解：

至此一次估計過程結(jié)束，如果Vt+TD，i和At，i之間的差E(V，A)大于閾值（Threshold，TH），所有點將根據(jù)TD進行更新，算法將再次搜索對應(yīng)的點，直到E(V，A)

圖1 延時標(biāo)定算法邏輯圖Fig.1 Logic diagram of delay calibration algorithm

2.2 視覺延時補償

AHRS的姿態(tài)角輸出本身就具有內(nèi)部延時tdF，但是由于TD比tdF和tda都要大得多，并且EKF的方法可認(rèn)為是實時更新了來自AHRS的數(shù)據(jù)，因此AHRS的輸出可以被大致認(rèn)為是IMU數(shù)據(jù)的無延時估計，也就是tdF≈tda≈tdω，各傳感器數(shù)據(jù)在時間軸上的關(guān)系如圖2所示.根據(jù)圖2可以看出AHRS估計的數(shù)據(jù)相對于IMU數(shù)據(jù)有微小的延時，而視覺數(shù)據(jù)的延時遠遠大于AHRS估計數(shù)據(jù)所產(chǎn)生的延時，因此可以得到如下的關(guān)系式：

圖2 延時示意圖Fig.2 Time delay diagram

其中，td1表示視覺數(shù)據(jù)滯后于慣性傳感器數(shù)據(jù)輸出的延時，而TD+tdF是視覺數(shù)據(jù)滯后于實際運動發(fā)生時刻的實際延時，通過減去加速度計的輸出延時，求得的td1在涉及加速度的控制閉環(huán)中基本滿足準(zhǔn)確度要求.使用td1進行延時補償，可以將視覺數(shù)據(jù)補償?shù)脚cIMU數(shù)據(jù)基本相同的時間點.

在td1這段時間內(nèi)，無人機的實際運動由加速度計實時獲取，但由于視覺數(shù)據(jù)的延時，此時并未獲取到由視覺傳感器計算得到的運動數(shù)據(jù).根據(jù)運動定理，可以將線性加速度的積分視為無人機的速度va，視覺數(shù)據(jù)的位置微分也可被視為速度vb.相對而言，va具有良好的實時響應(yīng)，但由于誤差在積分過程中會產(chǎn)生漂移，尤其是對于長時間操作漂移會更加明顯.而vb在整個飛行過程中相對準(zhǔn)確，但其實時性能卻很差.

因此，融合va和vb以獲得更為準(zhǔn)確的速度數(shù)據(jù)是非常有必要的.將va作為td1時間內(nèi)的有效實時速度，并利用vb在td1時刻之后的高可靠性，可以得到的補償后速度v′為：

其中，ts是傳感器的采樣周期，而xt是來自Xt中的位置坐標(biāo).假設(shè)在飛行過程中環(huán)境溫度沒有變化，在初始化過程中無人機處于靜止?fàn)顟B(tài)，則取前200次加速度數(shù)據(jù)的平均值來作為加速度計的零偏：

同樣的，位移的實際延時td2≈td1，由速度積分得到的位移數(shù)據(jù)在td2這段時間內(nèi)具有更好的動態(tài)性能，而在td2時刻之后，視覺數(shù)據(jù)可以給出更準(zhǔn)確的位移信息.因此，可以通過融合來自這兩個數(shù)據(jù)源的位移數(shù)據(jù)來計算延時補償后的位置：

其中v′和x′分別是補償后的速度和位移，經(jīng)過補償?shù)乃俣群臀灰朴糜诤罄m(xù)數(shù)據(jù)融合濾波和導(dǎo)航，可以明顯提高狀態(tài)估計器的準(zhǔn)確性.

3 基于EKF的位置融合

4 實驗結(jié)果與驗證

本文的無人機實驗平臺如圖3中所示.圖中1部分所示為Intel電腦棒，用于傳輸視覺數(shù)據(jù)給地面站，并擔(dān)任部分圖像處理任務(wù)；2部分表示的是無人機的飛控單元，負責(zé)無人機的位姿控制，并擔(dān)任了傳感器數(shù)據(jù)融合的工作；3部分所示是無人機的IMU模塊；4部分是一雙目攝像頭，采集視覺數(shù)據(jù)通過1傳給地面站，執(zhí)行SLAM任務(wù).攝像頭和IMU相互獨立，二者之間無硬件同步.紅外測距模塊和其他輔助測試傳感器安裝在無人機的底部.

圖3 無人機實驗平臺Fig.3 UAV experimental platform

首先，運用本文的延時標(biāo)定和補償方法對視覺數(shù)據(jù)進行補償.通過融合在延時時間內(nèi)的加速度，可以大大提高補償后速度的估計精度.在圖4中，采用了一個紅外測距模塊作為對比傳感器，該模塊精度很高，并且僅有幾毫秒的延時，因此可以將紅外測量數(shù)據(jù)作為對比的真實值.可以觀察到，經(jīng)過補償后的速度v′和紅外數(shù)據(jù)之間只有很小的誤差，估計精度明顯優(yōu)于未經(jīng)過補償?shù)乃俣葀.精確估計的速度v′可進一步用于補償位移.如圖5所示，補償后的位移l非常接近真實位移，估計精度明顯優(yōu)于未經(jīng)過補償?shù)奈灰苐′.該實驗也有紅外測距模塊的測量數(shù)據(jù)做對比.

圖4 速度的延時補償Fig.4 Delay compensation of speed

圖5 位移的延時補償Fig.5 Delay compensation of displacement

圖4和圖5表明，本文描述的延時補償方法可以很好地將視覺數(shù)據(jù)延時補償?shù)秸_的時間點，這對于實時運動估計是非常有意義的，可以提高無人機實時室內(nèi)導(dǎo)航系統(tǒng)的控制穩(wěn)定性和普適性.

延時補償還有助于提高無人機的定位精度.無人飛行器沿著同一個預(yù)設(shè)矩形的邊緣飛行，實際飛行位置如圖6所示.圖6中三組獨立實驗分別使用了三個不同的數(shù)據(jù)輸入源，raw為使用原始視覺數(shù)據(jù)進行定位的飛行結(jié)果，L1為使用未經(jīng)延時補償?shù)娜诤蠑?shù)據(jù)進行定位的飛行結(jié)果，L2則為使用了補償后的融合數(shù)據(jù)的飛行結(jié)果.

圖6 無人機的實際飛行位置Fig.6 Actual flight position of UAV

運用軟件對三種數(shù)據(jù)源得到的飛行結(jié)果進行APE誤差分析，相對誤差表現(xiàn)如圖7所示.圖7中各線條按順序分別表示APE誤差、均方根誤差、誤差中位數(shù)、誤差均值和標(biāo)準(zhǔn)偏差.由圖可知，使用了補償后的融合數(shù)據(jù)的飛行結(jié)果相對誤差是三組實驗中最小的，可以證明補償后的數(shù)據(jù)具有更好的定位精度.具體誤差數(shù)值如表2所示.

圖7 三種數(shù)據(jù)源輸入的定位誤差對比Fig.7 Comparison of positioning errors of three data sources input

表2 三種數(shù)據(jù)源輸入的定位誤差數(shù)值Table 2 Positioning error values of three data sources

5 結(jié)論

本文提出了一種基于視覺系統(tǒng)延時補償?shù)臒o人機實時室內(nèi)導(dǎo)航方法.首先，通過ICP的簡化變體估算出IMU數(shù)據(jù)和視覺數(shù)據(jù)之間的延時.基于該估計的延時，使用加速度信息來補償視覺估計的速度.然后，對補償后的速度求積分，進一步融合積分結(jié)果和視覺估計的位置，得到補償后的位置信息.補償后的速度和位置在精度上有了很大的提高，并且使其與實際的速度和位移保持同步.最后，利用EKF將補償后的視覺數(shù)據(jù)與IMU數(shù)據(jù)融合，從而使定位性能顯著優(yōu)于原始視覺數(shù)據(jù).更重要的是，該方法不僅提高了定位精度，還滿足了室內(nèi)無人機控制的實時性要求.