基于小波濾波的無(wú)人旋翼機(jī)高度信息融合

白 浪 雷旭升 盛 蔚 杜玉虎

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

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

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

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

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

1 高度測(cè)量單元

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

圖1 SUAR試驗(yàn)平臺(tái)

1.1 高度測(cè)量傳感器

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

1.1.1 氣壓高度計(jì)

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

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

實(shí)際飛行中，通過(guò)計(jì)算起飛點(diǎn)和SUAR實(shí)時(shí)相對(duì)于基準(zhǔn)點(diǎn)的高度，從而得出SUAR相對(duì)于地面的高度.

式中，h1為SUAR起飛點(diǎn)相對(duì)于基準(zhǔn)點(diǎn)的高度;P1為起飛點(diǎn)實(shí)測(cè)壓力值.

式中，h2為SUAR飛行過(guò)程中實(shí)時(shí)的相對(duì)于基準(zhǔn)點(diǎn)的高度;P2為起飛點(diǎn)實(shí)測(cè)壓力值.

式中，ha為SUAR實(shí)際飛行中相對(duì)于地面起飛點(diǎn)的高度.

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

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

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

1.1.2 差分GPS定位高度

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

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

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

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

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

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

1.1.3 超聲波高度測(cè)量模塊

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

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

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

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

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

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

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

圖6 高度測(cè)量信息頻譜分析

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

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

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

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

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

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

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

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

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

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

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

2.2 小波變換快速算法

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

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

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

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

將多相位矩陣進(jìn)行因子分解如下:

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

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

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

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

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

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

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

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

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

3.2 觀(guān)測(cè)模型

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

式中，v1為量測(cè)噪聲.

組合導(dǎo)航輸出高度的觀(guān)測(cè)方程為

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

基于式(12)、式(13)構(gòu)建SUAR觀(guān)測(cè)方程為

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

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

由圖3中GPS輸出高度信息分析可得，當(dāng)GPS搜星數(shù)目波動(dòng)時(shí)，會(huì)造成GPS輸出的高度信息波動(dòng)，最終引起組合導(dǎo)航輸出的高度波動(dòng).因此可以通過(guò)GPS搜星情況來(lái)評(píng)估組合導(dǎo)航的高度測(cè)量信息.

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

自適應(yīng)Kalman濾波的時(shí)間更新如下:

自適應(yīng)Kalman濾波的量測(cè)更新如下:

當(dāng) nk=nk+1，k=1，2，…，n 時(shí)有

當(dāng) nk≠nk+1，k=1，2，…，n 時(shí)有

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

修正后的組合導(dǎo)航高度量測(cè)誤差為

通過(guò)α可以實(shí)現(xiàn)組合導(dǎo)航高度信息量測(cè)誤差的自適應(yīng)調(diào)整，當(dāng)組合導(dǎo)航高度不準(zhǔn)確時(shí)，系統(tǒng)依舊可以獲得高精度高度信息.

圖9中比較了原始?xì)鈮焊叨刃畔ⅰ⑷诤咸幚砗蟮母叨刃畔⒑托〔V波后的融合高度信息.

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

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

4 飛行試驗(yàn)驗(yàn)證

4.1 定點(diǎn)懸停試驗(yàn)

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

圖10 定點(diǎn)懸停試驗(yàn)高度數(shù)據(jù)

4.2 三維航跡跟蹤飛行試驗(yàn)

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

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

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

5 結(jié)論

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

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

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

由于在分析SUAR系統(tǒng)高度測(cè)量信息時(shí)，主要是基于懸停飛行的數(shù)據(jù);試驗(yàn)驗(yàn)證主要是在懸停飛行和低速前飛下進(jìn)行，未考慮高度大機(jī)動(dòng)等狀態(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］朱曉娟，陳欣.一種無(wú)人機(jī)高度傳感器信息融合方法［J］.航空學(xué)報(bào)，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］黃曉瑞，崔平遠(yuǎn)，催祜濤.差分GPS定位方法與應(yīng)用研究［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］彭玉華.小波變換與工程應(yīng)用［M］.北京:科學(xué)出版社，2005 Peng Yuhua.Wavelet transform and engineering application［M］.Beijing:Science Press，2005(in Chinese)

［5］孫延奎.小波分析及應(yīng)用［M］.北京:機(jī)械工業(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］程正興，楊守志，馮曉霞.小波分析的理論、算法、進(jìn)展和應(yīng)用［M］.北京:國(guó)防工業(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］劉金琨.機(jī)器人控制系統(tǒng)的設(shè)計(jì)與MATLAB仿真［M］.北京:清華大學(xué)出版社，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