許承宇 徐紹凱

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

引言

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

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

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

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

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

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

1 IMU模型

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

1.1 加速度計(jì)測量模型

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

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

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

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

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

1.3 IMU測量延時(shí)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2.2 視覺延時(shí)補(bǔ)償

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

圖2 延時(shí)示意圖Fig.2 Time delay diagram

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

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

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

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

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

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

3 基于EKF的位置融合

4 實(shí)驗(yàn)結(jié)果與驗(yàn)證

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

圖3 無人機(jī)實(shí)驗(yàn)平臺Fig.3 UAV experimental platform

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

圖4 速度的延時(shí)補(bǔ)償Fig.4 Delay compensation of speed

圖5 位移的延時(shí)補(bǔ)償Fig.5 Delay compensation of displacement

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

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

圖6 無人機(jī)的實(shí)際飛行位置Fig.6 Actual flight position of UAV

運(yùn)用軟件對三種數(shù)據(jù)源得到的飛行結(jié)果進(jìn)行APE誤差分析，相對誤差表現(xiàn)如圖7所示.圖7中各線條按順序分別表示APE誤差、均方根誤差、誤差中位數(shù)、誤差均值和標(biāo)準(zhǔn)偏差.由圖可知，使用了補(bǔ)償后的融合數(shù)據(jù)的飛行結(jié)果相對誤差是三組實(shí)驗(yàn)中最小的，可以證明補(bǔ)償后的數(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í)補(bǔ)償?shù)臒o人機(jī)實(shí)時(shí)室內(nèi)導(dǎo)航方法.首先，通過ICP的簡化變體估算出IMU數(shù)據(jù)和視覺數(shù)據(jù)之間的延時(shí).基于該估計(jì)的延時(shí)，使用加速度信息來補(bǔ)償視覺估計(jì)的速度.然后，對補(bǔ)償后的速度求積分，進(jìn)一步融合積分結(jié)果和視覺估計(jì)的位置，得到補(bǔ)償后的位置信息.補(bǔ)償后的速度和位置在精度上有了很大的提高，并且使其與實(shí)際的速度和位移保持同步.最后，利用EKF將補(bǔ)償后的視覺數(shù)據(jù)與IMU數(shù)據(jù)融合，從而使定位性能顯著優(yōu)于原始視覺數(shù)據(jù).更重要的是，該方法不僅提高了定位精度，還滿足了室內(nèi)無人機(jī)控制的實(shí)時(shí)性要求.