基于小波濾波的無人旋翼機高度信息融合

白 浪 雷旭升 盛 蔚 杜玉虎

(北京航空航天大學 儀器科學與光電工程學院，北京100191)

小型無人旋翼機(SUAR，Small Unmanned Aerial Rotorcraft)具有體積小、垂直起降、機動性好等特點，可以執(zhí)行監(jiān)測、精確定位等任務(wù)，在軍事、民用方面具有廣闊的應用前景［1］.

由于SUAR受自身載重、體積等限制，其高度測量單元常采用體積小、功耗低的器件，如基于氣壓-電壓型硅壓阻式傳感器的氣壓高度計;能輸出定位高度的全球定位系統(tǒng)(GPS，Global Positioning System)模塊;超聲波測高模塊等.

為實現(xiàn)SUAR高精度自主飛行，飛行控制系統(tǒng)需要準確獲取高精度的自身飛行狀態(tài)信息.高度作為信息源的重要組成部分，對系統(tǒng)的控制性能具有重要影響［2］.受各傳感器自身特點和測量條件限制，目前高度測量精度和可信度不能滿足SUAR各種飛行狀態(tài)下的高度測量精度要求.因此需要采用數(shù)據(jù)濾波和信息融合技術(shù)以提高測量精度.

本文針對SUAR系統(tǒng)高度測量單元精度低的問題，通過基于小波提升算法的小波分解重構(gòu)方法對原始測量數(shù)據(jù)進行濾波處理，根據(jù)多源傳感器信號特性，利用自適應卡爾曼濾波(adaptive kalman filter)的方法對濾波數(shù)據(jù)進行信息融合，最終獲取高精度SUAR高度狀態(tài)信息，并通過實際飛行試驗驗證了上述方法的有效性.

1 高度測量單元

圖1為搭載實驗室自研飛行控制系統(tǒng)的SUAR平臺.圖1中SUAR的主旋翼直徑1.46 m、機身長1.3 m、總重約5 kg((包括飛行器本體、飛行控制系統(tǒng)、GPS接收機、通訊系統(tǒng)、供電系統(tǒng)及500 mL燃料).高度測量單元有氣壓高度計、差分GPS和超聲波高度測量模塊構(gòu)成.

圖1 SUAR試驗平臺

1.1 高度測量傳感器

基于SUAR平臺，在微風情況下執(zhí)行懸停任務(wù)(定高13.5 m)，各類傳感數(shù)據(jù)試驗結(jié)果及數(shù)據(jù)分析如下.

1.1.1 氣壓高度計

SUAR采用硅壓阻式傳感器MXP6115A，根據(jù)大氣壓強和高度之間的關(guān)系，通過測量大氣靜壓間接測量得到氣壓高度.氣壓高度的計算公式為

式中，h為測試點相對于基準點的高度;T0=288.15 K為基準溫度;P0=101 325 Pa為基準壓強;Ps為實測壓力值;β為溫度變化系數(shù);R為空氣氣體常數(shù);g為自由落體加速度.

實際飛行中，通過計算起飛點和SUAR實時相對于基準點的高度，從而得出SUAR相對于地面的高度.

式中，h1為SUAR起飛點相對于基準點的高度;P1為起飛點實測壓力值.

式中，h2為SUAR飛行過程中實時的相對于基準點的高度;P2為起飛點實測壓力值.

式中，ha為SUAR實際飛行中相對于地面起飛點的高度.

氣壓高度計的主要測量誤差源包括:器件自身的零點漂移、溫度漂移誤差以及外界干擾引起的誤差，如旋翼下洗氣流和風擾等.懸停時的氣壓高度計測量數(shù)據(jù)如圖2所示.

圖2 氣壓高度計測量數(shù)據(jù)

由圖2可得實測的氣壓高度值波動較大(均方差為0.4156)，屬高頻噪聲.氣壓高度計測量數(shù)據(jù)雖然數(shù)據(jù)連續(xù)，但精度較低，且易受外界環(huán)境干擾.直接采用此測量值進行高度信息融合，將會在融合信息中引入高頻噪聲.

1.1.2 差分GPS定位高度

SUAR采用加拿大NovAtel公司的差分GPS.由GPS接收機接收3顆以上已知坐標的衛(wèi)星來交匯出測量點的三維坐標，通過計算和轉(zhuǎn)換得到定位高度.差分GPS可以消除GPS定位帶來的絕大部分常值誤差［3］.懸停時的GPS定位高度信息如圖3所示.

圖3 差分GPS定位高度測量數(shù)據(jù)

由圖3可得差分GPS測量信息受GPS星數(shù)波動影響很大，GPS定位高度短期波動要比氣壓高度計測量值好.

NovAtel差分GPS的輸出頻率為1～10 Hz可配置，SUAR機體響應頻率一般為20Hz，所以GPS高度測量信息的最大輸出頻率不能滿足SUAR飛行的動態(tài)響應要求.實際中采用以慣性測量單元信息和GPS測量信息進行組合的50 Hz導航高度信息hn，如圖4所示.

圖4 組合導航高度數(shù)據(jù)

由圖4可得組合導航解算的高度信息輸出曲線整體比較平滑，但受GPS定位高度影響，有一些短期的波動.

1.1.3 超聲波高度測量模塊

SUAR采用的超聲波高度測量模塊為靜電超聲波變送器 Mini-S，測量范圍為 0.15 ～3.05 m.測量精度為1‰量程，可達3 mm.圖5為一次飛行試驗中超聲波測高模塊輸出的高度信息.

圖5 超聲波測高模塊數(shù)據(jù)

由圖5可得出超聲波高度測量模塊在低空可以實現(xiàn)精確的高度信息測量，主要為SUAR實現(xiàn)自主起降功能提供保障，另外還需要對氣壓高度測量信息和組合導航高度信息進行初步校正.但超過量程范圍后，即高空飛行時測量信息明顯不可信.

SUAR系統(tǒng)的高度傳感器測量數(shù)據(jù)易受外界干擾，包含有不少噪聲，因此必須對原始數(shù)據(jù)先濾波處理，然后再進行高度信息融合.

1.2 高度測量數(shù)據(jù)分析

基于SUAR平臺進行實際飛行試驗，通過飛行控制系統(tǒng)中的Flash存儲模塊獲取50 Hz的高度測量數(shù)據(jù)，并對高度測量數(shù)據(jù)進行傅里葉變換的頻譜分析.結(jié)果如圖6所示.

由圖6可得高度測量數(shù)據(jù)的有效值主要分布在低頻階段，干擾噪聲主要分布在高頻階段.設(shè)置所選取低通濾波器的截止頻率為0.1 Hz.

圖6 高度測量信息頻譜分析

2 基于小波分析的高度濾波方法

常用的數(shù)據(jù)濾波方法有數(shù)據(jù)平滑濾波、無限沖擊響應(IIR，Infinite Impulse Response)濾波，有限沖擊響應(FTR，F(xiàn)inite Impulse Response)濾波、小波濾波等.

1)數(shù)據(jù)平滑濾波是一種簡單的濾波處理方法，即在高采樣速率條件下，對采樣數(shù)據(jù)進行平均后輸出，可以有效削弱測量野值和噪聲，但對傳感器數(shù)據(jù)采集系統(tǒng)的要求比較高;

2)IIR低通濾波器雖然可以有效地濾除高頻干擾，但會引起明顯的相位延遲;

管理云以秒級連續(xù)采集ONT和ONT上關(guān)聯(lián)的終端（STA）的各種數(shù)據(jù)，包括ONT在線情況、STA連接協(xié)商速率、信號強度、流量等。通過云端軟件統(tǒng)一分析，可做到以下幾個功能：

3)FIR低通濾波器也能有效削弱噪聲信號的能量，但細節(jié)處的高頻干擾濾除還是不理想;

4)小波濾波是一種利用小波變換的解相關(guān)特性，通過分析處理小波系數(shù)進行濾波的方法.基于適合小波基函數(shù)的小波分解-重構(gòu)實現(xiàn)信號濾波，可以有效地濾除噪聲，而且濾波后信號的相位延遲很小.

2.1 小波基函數(shù)選擇

小波基函數(shù)的選取主要是根據(jù)待濾波信號的形式相似性和工程應用的方便性.一般情況，小波基函數(shù)選擇應該考慮的特性有［4］:①緊支性;②對稱性;③正交性;④消失矩.

根據(jù)以上小波特性，本文中采用Daubechies小波族函數(shù)，簡記為DbN(N=1，2，…)其中N為對應的濾波器長度，對應的緊支撐區(qū)域為(－N+1，N)，消失矩為 2N.除 Db1(Haar小波)外，其它D小波沒有顯式表達式，只有轉(zhuǎn)換函數(shù)的平方模是確定的［5］.

由上式構(gòu)建D小波的模平方和滿足:

2.2 小波變換快速算法

小波變換的快速算法主要有Mallat算法和提升小波變換算法.Mallat算法是基于濾波器組實現(xiàn)小波變換，涉及離散序列的卷積.提升小波變換方法比原有基于卷積的Mallat小波變換算法計算量少，復雜度低，且在運算過程中可實現(xiàn)原位計算，存儲空間需求少，常在工程應用中采用.

SUAR飛行控制系統(tǒng)中采用AT91RM9200做核心處理器，主要涉及飛行控制、傳感器信號采集、與導航計算機的交互、與地面站的交互等任務(wù).從節(jié)約飛行控制系統(tǒng)資源的角度出發(fā)，本文采用提升小波變換的方法實現(xiàn)SUAR高度信息濾波.

2.3 小波函數(shù)算法流程

由上濾波器參數(shù)可以確定多相位矩陣如下:

將多相位矩陣進行因子分解如下:

基于多相位矩陣分解因子的小波分解與重構(gòu)提升算法流程圖如圖7所示.

圖7 小波分解和重構(gòu)的提升算法流程圖

利用上面的小波分解和重構(gòu)算法，對SUAR系統(tǒng)中的高度信息進行基于Db4小波基函數(shù)的3次分解重構(gòu)濾波，結(jié)果如圖8所示.由圖8可以得出基于D小波的高度數(shù)據(jù)濾波，可以有效地濾除高頻干擾，而且濾波造成的相位延遲比較小.

圖8 利用小波濾波處理高度測量數(shù)據(jù)

3 高度數(shù)據(jù)融合方法

氣壓高度計的高度測量結(jié)果經(jīng)過數(shù)據(jù)濾波處理后，仍不能避免低頻零位漂移、溫度漂移等干擾，而且這些干擾產(chǎn)生的誤差還會隨時間積累.GPS輸出定位高度雖然受GPS搜星數(shù)目和搜星質(zhì)量的影響，但高度輸出數(shù)據(jù)的誤差不會隨時間積累.因此很有必要將濾波后的氣壓高度計信息和組合導航高度信息進行數(shù)據(jù)融合，以進一步提高高度信息的可信度.

3.1 飛行系統(tǒng)高度狀態(tài)模型

SUAR系統(tǒng)高度主要通過主槳總距控制，受俯仰角和主槳轉(zhuǎn)速的影響.因此，構(gòu)建SUAR高度動力學模型為［8］用于和w也有控制關(guān)系;ai可以通過模型參數(shù)辨識得到.由式(10)可得SUAR的高度的狀態(tài)方程為

可以結(jié)合高度測量信息和高度狀態(tài)方程，應用Kalman濾波估計理論對高度信息進行融合、估計和修正［9］.

3.2 觀測模型

在測量對地高度時，氣壓高度計測量的高度值ha經(jīng)過小波濾波后，在很大程度上削弱了高頻干擾噪聲，即其在高頻范圍內(nèi)是一種有用信號，但其穩(wěn)態(tài)值與地面氣壓和溫度有關(guān)，故這種信號中含有零位誤差he，考慮該誤差來源因素以及測量干擾，建立氣壓高度傳感器的觀測方程為

式中，v1為量測噪聲.

組合導航輸出高度的觀測方程為

hn主要受差分GPS的定位高度影響;v2為量測噪聲，主要與GPS搜星數(shù)目和搜星質(zhì)量相關(guān).

基于式(12)、式(13)構(gòu)建SUAR觀測方程為

式中，h為高度;w為主槳轉(zhuǎn)速;θ為旋翼槳葉的總距俯仰角;輸入u=(u1，u2)T，油門控制輸入u1作用于w·，對w和θ都有控制關(guān)系;總距控制u2作

3.3 自適應Kalman濾波數(shù)據(jù)融合

由圖3中GPS輸出高度信息分析可得，當GPS搜星數(shù)目波動時，會造成GPS輸出的高度信息波動，最終引起組合導航輸出的高度波動.因此可以通過GPS搜星情況來評估組合導航的高度測量信息.

據(jù)此，引入誤差評價因素α對傳統(tǒng)Kalman濾波中的量測誤差陣Rk+1進行校正，構(gòu)建自適應Kalman濾波估計方法對h，he進行估計，得到高度的無偏估計和高度氣壓計的常值偏差估計

自適應Kalman濾波的時間更新如下:

自適應Kalman濾波的量測更新如下:

當 nk=nk+1，k=1，2，…，n 時有

當 nk≠nk+1，k=1，2，…，n 時有

式中，nk(k=1，2，…，n)為第 k次采樣中的 GPS搜星數(shù)目;τ為根據(jù)GPS搜星數(shù)目數(shù)據(jù)特性所取的星數(shù)平均穩(wěn)定時間相關(guān)參數(shù).

修正后的組合導航高度量測誤差為

通過α可以實現(xiàn)組合導航高度信息量測誤差的自適應調(diào)整，當組合導航高度不準確時，系統(tǒng)依舊可以獲得高精度高度信息.

圖9中比較了原始氣壓高度信息、融合處理后的高度信息和小波濾波后的融合高度信息.

圖9 利用小波濾波處理高度融合數(shù)據(jù)

由圖9可得小波濾波可以有效地濾除氣壓高度計測量數(shù)據(jù)中的高頻噪聲，而采用數(shù)據(jù)融合處理濾波后的高度信息可以校正氣壓高度計測量數(shù)據(jù)中常值誤差，進一步提高高度信息的可靠性.

4 飛行試驗驗證

4.1 定點懸停試驗

為驗證基于小波濾波的高度信息融合方法的有效性，基于SUAR飛行平臺進行了定點懸停試驗.目標高度為10 m，記錄飛行時間約30 s.飛行高度數(shù)據(jù)如圖10所示.由圖10可以得出，SUAR飛行高度比較平穩(wěn)，高度誤差小于0.6 m.

圖10 定點懸停試驗高度數(shù)據(jù)

4.2 三維航跡跟蹤飛行試驗

為進一步驗證基于小波濾波的高度信息融合方法的可靠性及動態(tài)響應特性，在河北涿州市郊外進行了三維航跡跟蹤飛行試驗.根據(jù)任務(wù)需求，SUAR進行三維航跡飛行，切換點高度19.5m，目標點高度20 m，前飛距離100 m，中間點高度最低為15 m，前飛過程中按照二次拋物線形進行高度控制，前飛速度1.5 m/s.實際飛行高度航跡和期望飛行高度航跡比對如圖11所示.

圖11 三維航跡跟蹤試驗高度數(shù)據(jù)

由圖11可得基于小波濾波的無人旋翼機高度信息融合方法，SUAR可實現(xiàn)高精度的變高跟蹤控制，全程高度誤差波動小于0.8 m，均方差為0.1563，能夠滿足系統(tǒng)在高度控制的動態(tài)響應.

5 結(jié)論

本文采用高度數(shù)據(jù)融合的處理方法解決SUAR系統(tǒng)高度測量信息可信度低、精度差的問題.

1)利用小波分解-重構(gòu)的濾波方法濾除SUAR實際飛行過程中高度測量信息的高頻干擾噪聲.

2)利用改進的自適應Kalman濾波數(shù)據(jù)融合方法，估計高度氣壓測量數(shù)據(jù)中的零位偏移.

由于在分析SUAR系統(tǒng)高度測量信息時，主要是基于懸停飛行的數(shù)據(jù);試驗驗證主要是在懸停飛行和低速前飛下進行，未考慮高度大機動等狀態(tài)下的SUAR系統(tǒng)特性.

References)

［1］De Voogt A J，Uitdewilligen S，Eremenko N.Safety in high-risk helicopter operations:the role of additional crew in accident prevention［J］.Safety Science，2009，47(5):717 －721

［2］朱曉娟，陳欣.一種無人機高度傳感器信息融合方法［J］.航空學報，2008，29(S1):S61 －S65 Zhu Xiaojuan，Chen Xin.An information fusion method of altitude sensors for UAV［J］.Acta Aeronautica et Astronautica Sinica，2008，29(S1):S61 － S65(in Chinese)

［3］黃曉瑞，崔平遠，催祜濤.差分GPS定位方法與應用研究［J］.航空兵器，2001(3):5 －8 Huang Xiaorui，Cui Pingyuan，Cui Hutao.Research on differential GPS positioning method and application ［J］.Journal of Aero Weaponry，2001(3):5 －8(in Chinese)

［4］彭玉華.小波變換與工程應用［M］.北京:科學出版社，2005 Peng Yuhua.Wavelet transform and engineering application［M］.Beijing:Science Press，2005(in Chinese)

［5］孫延奎.小波分析及應用［M］.北京:機械工業(yè)出版社，2005:28－30 Sun Yankui.Wavelat analyse and application［M］.Beijing:Mechanism Industry Press，2005:28 －30(in Chinese)

［6］Zhang Xi，Wang Wei，Yoshinori T.Design of IIR orthogonal wavelet filter banks using lifting scheme［J］.IEEE Transaction on Signal Processing，2006，54(7):2616 －2624

［7］程正興，楊守志，馮曉霞.小波分析的理論、算法、進展和應用［M］.北京:國防工業(yè)出版社，2007:223－227 Cheng Zhengxing，Yang Shouzhi，F(xiàn)eng Xiaoxia.The theory arithmetic evolve and application?of wavelat analyse［M］.Beijing:National Defence Industry Press，2007:223 －227(in Chinese)

［8］劉金琨.機器人控制系統(tǒng)的設(shè)計與MATLAB仿真［M］.北京:清華大學出版社，2008:505－507 Liu Jinkun.The design of robot control system and MATLAB simulink［M］.Beijing:Tsinghua University Press，2008:505 －507(in Chinese)

［9］Song Enbin，Zhu Yunmin，Zhou Jie，et al.Optimality of Kalman filtering fusion with cross-correlated sensor noises［J］.Automatica，2004，43(8):1450 －1456