黃 鶴，劉一恒，趙 熙，許 哲，郭 璐

（1. 長安大學 電子與控制工程學院，西安 710064；2. 北京航空航天大學，北京 100083；3. 中電科第20研究所，西安 710068；4. 無人機系統(tǒng)國家工程研究中心，西安 710072）

無人機測控技術是指對無人機進行遙控、遙測、跟蹤定位和信息傳輸，不僅體現(xiàn)在在較大空域條件下對飛機飛行狀態(tài)和設備狀態(tài)的控制，而且還需要對其參數(shù)進行測量，其中，準確的高度測量數(shù)據(jù)是復雜環(huán)境下低空自主飛行和懸?？刂葡到y(tǒng)的必要輸入條件[1]。機載測高傳感器通常包括氣壓計、GPS、加速度計和無線電高度表等。大量試驗表明：氣壓計數(shù)據(jù)連續(xù)且分辨率高，但受大氣壓力影響，測量誤差隨著高度降低而增大，難以滿足測高精度[2-4]；GPS定位準確度高，但其被動定位特性會使其具有較低的測量數(shù)據(jù)更新率，易受 GPS碼型改變或系統(tǒng)關閉影響出現(xiàn)定位故障[5]；加速度計可以測量高度方向的速度變化，短時間內(nèi)具有高精度，但隨著時間增長，高度變化量的誤差會累積，致使測高數(shù)據(jù)發(fā)散；無線電高度表測量精度較高，通常裝備在大型固定翼飛機和無人直升機上，獲取飛機與地面或海平面的相對高度，受地形狀況影響較大，應用范圍有限，在地面海拔高度未知的情況下無法獲取飛機的絕對高度，且四旋翼無人機載荷有限，一般不會裝備。由此可見，以上各傳感器受自身測量特性和應用場景的限制，依靠單一傳感器很難滿足無人機的高度精確測量需求。

針對上述問題，文獻[6-10]中的學者研究了各類傳感器的測量特性、數(shù)學特性，嘗試將不同類型傳感器進行信息融合。本文綜合氣壓計、GPS和加速度計三種傳感器的特點和信息數(shù)據(jù)資源，實現(xiàn)優(yōu)勢互補。無人機信息融合測高系統(tǒng)工作流程如圖1所示。當系統(tǒng)中的個別傳感器失效或出現(xiàn)故障時，可利用其它正常工作的傳感器采集信息，綜合計算獲取量測信息，具有較強的容錯性。同時，可充分利用多傳感器協(xié)調和性能互補[11]，依據(jù)設計的準則將多傳感器的數(shù)據(jù)信息資源合理支配和使用，進行篩選、分析和處理，利用從不同信息源得到的在時間和空間上可冗余或互補的信息進行組合、推理和集成，以獲得高度信息的一致性解釋和描述，克服單一傳感器的不確定性和局限性，提高整個系統(tǒng)的魯棒性，全面準確的描述被測對象[12]。

圖1 無人機信息融合測高系統(tǒng)Fig.1 AUV height measurement by information fusion

一步延遲時間融合算法是文獻[13]提出的一種應用在雷達信號處理過程中的信息融合算法，本文首先將其引入應用在無人機測高過程中，但精度提升有限。針對這個問題，提出了一種多層多源信息融合懸停旋翼無人機測高算法，應用在懸停狀態(tài)的無人機測高過程中。首先提出一種基于二步延遲自適應時空算法，作為第一層融合，對氣壓計和GPS測得的歷史數(shù)據(jù)進行處理，提升融合精度。將基于參數(shù)辨識的自適應互補濾波用于第二層融合，利用加速度計數(shù)據(jù)的輔助測高，組建多步時空雙重融合模型，相對于一步延遲融合方法，進一步提升旋翼無人機測高數(shù)據(jù)的準確性。

1 多傳感器測高模型

1.1 氣壓計

氣壓計是利用大氣氣壓會隨著高度的增加而降低的原理來測量高度。以標準海平面作為基準面，當高度在11 km以下時，標準氣壓高度公式為：

式中：PH和Pb分別是高度為H和Hb時對應的氣壓（Pa）；Tb是高度為Hb時的溫度（K）；β是溫度垂直變化率（K/m）；R是空氣專用氣體常數(shù)（m2/k·s2）;g是重力加速度常數(shù)（m/s2）。氣壓計測高模型為：

式中：h1是氣壓計的測量高度；h是真實高度；ε1是氣壓計測量噪聲。

1.2 GPS

GPS測高可以得到無人機在WGS-84坐標系中的準確位置，經(jīng)過坐標系轉換，可以求得大地高HG，即以橢球面為基準時無人機的高度；實際測量需要正常高H，它是以似大地水準面為基準測得的高度；橢球面和似大地水準面之間的高度差稱為高程異常ξ。它們之間的關系可以表示為：

GPS測高模型為：

式中：h2是GPS的測量高度；h是真實高度；ε2是GPS測量噪聲。

1.3 加速度計

加速度計可以測量出無人機高度方向的速度變化，通過對速度積分，短時間內(nèi)可以得到精度較高的高度變化量，但長時間會使誤差累積，最終導致測高數(shù)據(jù)發(fā)散。無人機相對地面的高度H、垂向速度Vg和垂向加速度ag之間的關系為：

加速度計測高模型為：

式中：h3是加速度計的測量高度；h是真實高度；ε3是加速度計的測量噪聲。

由此可見，各類傳感器檢測結果受到自身測量特性的限制，無法滿足無人機對高度的精準測量要求。本文算法從氣壓計、GPS和加速度計的測量特性出發(fā)，進行高度測量信息的融合，達到精確測量的目的。

2 一步延遲時間融合算法

文獻[8]提出了一步延遲時間融合算法，主要應用在雷達信號和數(shù)據(jù)過程處理中，通過遞推估計實現(xiàn)單一雷達傳感器在不同時刻的融合估計，減小或者消除其在測量過程中的不確定性，通過前后時間鄰域點測量數(shù)據(jù)的重復運算，提升結果的正確率，獲取可靠性更高的測量數(shù)據(jù)。

設采用多傳感器（個數(shù)為d）對某一被測參數(shù)進行測量，在某一時間間隔內(nèi)，每個傳感器測量到n個數(shù)據(jù)Zi(1),Zi(2), …,Zi(n)，i=1, 2, …,d，然后采用時間融合估計。對于真值X，第i個傳感器前兩個時間點測量可得：

V1(1)、V1(2)為符合正態(tài)分布的測量噪聲，假定其均值均等于零，方差分別為δi2(1)、δi2(2)。假定X、V1(1)、V1(2)都是相互獨立的隨機變量。第1個時間點采用同一傳感器獲取X估計值的均值和方差：

利用時間遞推估計理論推出第2個時間點的最優(yōu)估計值Zi+(2)和方差Pi+(2)為[8]：

傳感器采集高度的量測數(shù)據(jù)后，將每次估計值Zi+和方差Pi+作為下一次采集測量的統(tǒng)計特性，將新的測量數(shù)據(jù)用于修正依次遞推計算。可得同一傳感器k次測量后的時間融合估計值Zi+(k)和方差Pi+(k)為：

在此基礎上，測量方差可以通過單一傳感器的自協(xié)方差因子和互協(xié)方差因子計算獲取。

3 多層信息融合無人機測高算法

3.1 多層信息融合總體框架

本文設計的無人機測高信息融合模型由兩層組成，其系統(tǒng)框架如圖2所示。第一層為二步延遲時空融合，包含二步延遲時間融合和空間融合兩個子層。第二層為基于參數(shù)識別的自適應互補濾波。

圖2 多源多層融合系統(tǒng)框圖Fig.2 System diagram of multi-layer and multi-source information fusion

通常情況下，氣壓計和GPS的噪聲較大，長時間工作時，其數(shù)據(jù)較為穩(wěn)定，能夠保持在高度真值附近波動。加速度計的噪聲較小，經(jīng)過兩次積分，可以轉化為位移，在短時間內(nèi)較為精確，但開機時間長了，會產(chǎn)生積分漂移，誤差較大，因此僅僅設計一層時空融合算法是不夠的，無法將加速度計數(shù)據(jù)與氣壓計和GPS數(shù)據(jù)同時進行融合。這里必須要另外設計一層信息融合方法，實現(xiàn)加速度計的輔助測高，在較長時間工作條件下，用GPS和氣壓計的時空融合值對加速度計的積分值進行緩慢的修正，抑制積分漂移現(xiàn)象。這里第二層信息融合采用互補濾波。在做第一層信息融合過程中，當GPS和氣壓計的數(shù)據(jù)噪聲較大時，時空融合算法計算出的融合高度數(shù)據(jù)會出現(xiàn)一些尖銳的毛刺，當其進入到濾波后的波形中，將會使無人機的高度數(shù)據(jù)產(chǎn)生誤差，而采用常規(guī)互補濾波算法無法徹底濾除這些毛刺。針對這個問題，本文設計了一種基于參數(shù)識別的自適應互補濾波算法，應用在第二層融合中。

3.2 第一層融合

在第一層融合過程中，結合無人機測高過程中數(shù)據(jù)量較小的特點，設計一種新的采用基于二步延遲自適應時空融合的融合算法，采用更多的歷史數(shù)據(jù)完成數(shù)據(jù)融合估計。

該算法首先利用高度傳感器在某一時間點的量測值與前兩個時間點的估計值進行融合，可計算得到同一傳感器在多時間點的融合估計值，然后將各異類傳感器同一時刻的估計值進行空間融合，從而得到最終的估計值。通過兩個子層的劃分將數(shù)據(jù)融合分解為兩次估計——基于二步延遲時間的最優(yōu)融合估計和基于空間的最優(yōu)融合估計。這兩個子層構建的數(shù)據(jù)融合算法不需要先驗信息，利用采集獲取的觀測數(shù)據(jù)精度來確定其對應的不同權值，從而計算出均方誤差最小的融合值。第一層融合過程如圖3所示。

圖3 第一層融合框圖Fig.3 Fusion process of the first layer

在高速數(shù)據(jù)量測過程中，在無人機的不同空間位置裝備d個異類高度傳感器，各個傳感器在不同時間和空間所獲取的無人機高度觀測數(shù)據(jù)是不相同的。d個傳感器在n個不同時間點的工作期間獲取高度值會出現(xiàn)d×n個觀測值。第一層信息融合分為兩個子層，第一子層是在一步延遲時間融合估計基礎上推導基于二步延遲時間的遞推估計融合估計，將各個傳感器分別進行基于兩個時間點延遲的高度數(shù)據(jù)的融合估計，通過結合歷史數(shù)據(jù)的修正消除每個傳感器依據(jù)時間獲取觀測數(shù)據(jù)的不確定性，提升單傳感器量測數(shù)據(jù)的精度和可靠性，將實時測量的高度數(shù)據(jù)進行第一步處理；第二子層將各個單傳感器處理后的高度數(shù)據(jù)進行自適應加權融合估計，實現(xiàn)高度數(shù)據(jù)多傳感器的空間融合。在此階段，每個傳感器所得到的同一時刻的估計值在完成了時間融合估計后，自適應確定最優(yōu)加權系數(shù)，使該時刻獲取的高度數(shù)據(jù)值的總均方差達到最小，從而實現(xiàn)最優(yōu)估計。分兩個階段進行信息融合是為了采用更多的觀測值集合Z，實現(xiàn)二步延遲時空融合，從而得到更精確的高度數(shù)據(jù)。

3.2.1 第一子層二步延遲時間融合

設無人機測高過程中共有d個高度傳感器，在某一時間間隔內(nèi)，單個高度傳感器獲取n個量測數(shù)據(jù)，即Z1(1),Z1(2), ...,Z1(n)，i=1, 2, ...,d，也就是說Zi(k)是第i個高度傳感器在k時間點的高度數(shù)據(jù)量測值，即觀測值Z={Zi(k)}(i=1, 2, ...,d;k=1, 2, ...,n)。首先，進行第一階段單個傳感器二步延遲高度信息時間融合估計，假設第i個傳感器獲取數(shù)據(jù)時，高度真值為X，前兩個時間點獲取的高度數(shù)據(jù)測量結果與公式(7)相同，單個高度傳感器在某一時間點測量之后計算出的X估計值的均值和方差與公式(8)相同。

通過二步延遲時間融合，推導出第3個時刻點測高的估計值和方差：

分別采用前兩個時間點的估計值和相應的方差作為歷史數(shù)據(jù)，進行單傳感器時間二步延遲融合計算。在計算過程中每一傳感器同一次只采用一個高度測量數(shù)據(jù)，將某一個點估計值及其方差作為下兩個時間點高度測量數(shù)據(jù)的統(tǒng)計特性參與計算，這樣每次估計值的求取都會用到本次的測量數(shù)據(jù)和前兩次的估計數(shù)據(jù)以及本次的方差和前兩次的方差。以此類推可得k次測量后單一測高傳感器融合估計和方差為：

單個高度傳感器每次測量方差為δi2(k)，可以通過高度傳感器的自協(xié)方差參數(shù)和互協(xié)方差參數(shù)的差算得。為了簡便計算，這里設有任意兩個不同傳感器p、q，其高度測量值分別為Xp(k)、Xq(k)，其測量誤差分別為Vp、Vq。p的自協(xié)方差為Rpp，p與q的互協(xié)方差為Rpq。通過時間域估計公式，可以得到：

其中，μ為采樣數(shù)據(jù)均值。

3.2.2 第二子層空間融合

通過式(15)(16)可得d個傳感器第k次測量估計值Z1+(k),Z2+(k), …,Zd+(k)，均方誤差值P1+(k),P2+(k), …,Pd+(k)，以及加權系數(shù)W1(k),W2(k), …,Wd(k)。利用圖4空間融合方法，計算可得異類傳感器空間最優(yōu)融合結果Z*(k)為：

由Z1+(k),Z2+(k), …,Zs+(k)相互獨立，得第k次多傳感器測量時空融合總均方誤差為：

圖4 空間融合過程Fig.4 Space fusion process

δ2(k)是二次函數(shù)，利用加權系數(shù)W1(k),W2(k), …,Wd(k)滿足約束條件的多元函數(shù)極值求取δ2(k)的最小值，得到最優(yōu)加權系數(shù)為：

最終總的均方誤差為：

由公式(23)可見，在第二階段進行多傳感器自適應加權空間融合后，獲得的均方誤差比單一傳感器采用二步延遲時間融合估計所獲得的均方誤差Pi+(k)更小，精度更高?；诳臻g最優(yōu)融合估計算法所得的均方誤差則：

通過式(24)，δi+(k)作為δi(k)的估計值，其值大于δmin(k)。因此，結合式(17)～(24)可以得到即自適應加權兩級融合算法均方誤差相對于基于空間融合有一定的減小。也就是說，經(jīng)過了第二階段自適應加權空間融合處理后，均方誤差變小了，相對于基于單個傳感器的時間融合或多傳感器的空間數(shù)據(jù)融合，其數(shù)據(jù)精度都有所提升。

3.2.3 計算步驟

以兩個傳感器測量數(shù)據(jù)樣本的融合估計為例，基于二步延遲的時空估計算法的計算步驟如下：

(a) 進行第一階段融合，利用公式組(17)～(20)進行計算，獲取測高傳感器采樣時間點Rpp(k)、Rpq(k)、Rqq(k)、Rqp(k)，通過計算可以得到測高采樣時間點的δp2(k)、δq2(k)；

(b) 分別將兩個傳感器的第一次測量值Zp(1)、Zq(1)和方差δp2(1)、δq2(1)作為系統(tǒng)初始參數(shù)，利用公式(5)計算得出每個測高傳感器采樣時間點基于二步延遲的時間融合估計值Zp+(1)、Zq+(1)和對應的融合方差Pp+(k)、Pq+(k)；

(c) 進行第二子層空間融合，利用公式(15)計算得到采樣時間點各個測高傳感器的最優(yōu)加權系數(shù)Wp(k)和Wq(k)；

(d) 利用公式(20)，獲取基于二步延遲時空融合估計結果Z(k)。

3.3 第二層融合

在第二層融合過程中，設計了一種基于參數(shù)辨識的自適應互補濾波算法，其融合過程如圖5所示。

圖5 第二層融合過程Fig.5 Fusion process of the second layer

通過該算法進行第二層融合，實現(xiàn)加速度計的輔助測高，在長時間工作狀態(tài)下，用GPS和氣壓計的時空融合值對加速度計的積分值進行緩慢的修正，抑制積分漂移現(xiàn)象。具體公式如下：

其中，v是垂直速度，a是垂直加速度，x是垂直位移，z是GPS和氣壓計時空融合的結果，T是時間常數(shù)，e是每次的修正量，k是自適應系數(shù)（開關量，只取 0和1），F(xiàn)是最終的融合結果。

融合過程中，T是可調節(jié)的參數(shù)。當T大時，更多地信任加速度計積分；而當T小時，更多地信任時空融合的結果。k的取值由修正量e決定，當e絕對值大于所設閾值時，k取0，否則，k取1，實現(xiàn)了對參數(shù)e的辨識，這樣就過濾了極端值，因為x在短時間內(nèi)是精確的，如果e過大，則證明z的取值不正常，此時，不把錯誤的e加進積分中，使波形參照加速度計的積分波形，不會出現(xiàn)大的波動，這樣就實現(xiàn)了互補濾波的自適應性。

4 實驗結果

為了驗證本文算法的有效性，實驗采用某無人機在240 m高空懸停時的實測數(shù)據(jù)，進行計算機仿真分析，氣壓計和GPS測量值如圖6～7所示。正如2.1節(jié)分析，測量誤差較大，圍繞240 m上下波動，但結果比較穩(wěn)定。

本實驗采用三軸MEMS數(shù)字加速度計MPU6050。先放置在水平桌面上，Z軸朝下，對X、Y軸進行零位校準；然后繞X軸旋轉90°，對Z軸進行零位校準；校準之后，對所測得的數(shù)據(jù)先進行卡爾曼濾波，得到靜止狀態(tài)下Z軸的數(shù)據(jù)為Zg（Zg表示靜止條件下重力加速度g對應的數(shù)據(jù)，為負數(shù)）。

圖6 氣壓計測量曲線Fig.6 Measurement data of the barometer

圖7 GPS測量曲線Fig.7 Measurement data of the GPS

在進行實驗時，將實時獲取的加速度計Z軸數(shù)據(jù)與Zg作差，得到無人機垂直向加速度。加速度計的測量數(shù)據(jù)如圖8～10所示，誤差相對比較小，但穩(wěn)定性不夠，工作時間過長會產(chǎn)生積分漂移，需要其他傳感器測高數(shù)據(jù)做修正。

圖8 加速度計的加速度測量值Fig.8 Acceleration measurement of the accelerometer

圖9 加速度計的速度估計曲線Fig.9 Estimation speed of the accelerometer

圖10 加速度計的測量高度估計曲線Fig.10 Estimation height of the accelerometer

圖11～12為采用氣壓計測高數(shù)據(jù)進行的一步延遲時間融合算法和二步延遲時間融合算法的誤差。結合表1（表1數(shù)據(jù)更直觀）和這兩張結果圖可以看出，二步延遲時間融合可以進一步減小均方誤差，這是由于計算過程中采用更多的歷史數(shù)據(jù)計算的原因。同時，測高剛開始的一段時間會產(chǎn)生大噪聲，也就是最大誤差，二步延遲時間融合算法有明顯改善。

圖13為多層融合誤差曲線，圖14為多種誤差對比圖。結合表1可知，在進行第二層融合之后，相比于一步延遲融合方法，高度估計值的均方根誤差減小43.6%，最大誤差減小53.6%，精度得到了大幅度的提升。

表1 實驗結果評價指標Tab.1 Evaluation of experimental results

圖11 氣壓計一步延遲時間融合算法誤差Fig.11 Barometer’s measurement error of the 1 step delay fusion algorithm

圖12 氣壓計二步延遲時間融合算法誤差Fig.12 Barometer’s measurement error of the 2 steps delay fusion algorithm

圖13 多層融合誤差曲線Fig.13 Measurement error of the multiple layer fusion

5 結 論

本文設計了一種基于多層多源信息融合的無人機測高算法，包含基于二步延遲自適應時空的第一層融合和基于參數(shù)辨識的自適應互補濾波的第二層融合，以克服現(xiàn)有技術存在的缺陷。

在借鑒雷達信號處理基于一步延遲自適應時空融合算法的基礎上，結合無人機測高過程中數(shù)據(jù)量不大的特點，實現(xiàn)了多步時間和空間雙重融合模型。采用更多的歷史數(shù)據(jù)完成數(shù)據(jù)處理，使融合精度更高。進行基于參數(shù)辨識的自適應互補濾波第二層融合，實現(xiàn)加速度計數(shù)據(jù)的輔助測高，使旋翼無人機在飛行和懸停過程中測高數(shù)據(jù)更加準確，保證飛行和任務執(zhí)行的穩(wěn)定性。同時，該算法總的計算量并不大，編程簡單，在硬件上不需要額外的設計，使系統(tǒng)在保證高可靠性的同時兼顧經(jīng)濟效益，是一種低成本、高準確度和可靠性的旋翼小型無人機高度測量方法。