一種抑制掠海飛行中多種擾動(dòng)的高度組合濾波方法

李金晟，莊 凌，宋加洪，陳 剛，胡 建

（北京航天長(zhǎng)征飛行器研究所，北京，100076）

掠海飛行器在海上執(zhí)行各種任務(wù)時(shí)，為提高生存概率，通常需要進(jìn)行超低空飛行。為實(shí)現(xiàn)該目的，需為這類飛行器設(shè)計(jì)一個(gè)性能可靠的高度控制系統(tǒng)，解決飛行過程中的海浪擾動(dòng)及傳感器（主要是雷達(dá)高度表及加速度表）測(cè)量誤差兩大難題，從而滿足掠海低飛安全性的要求。高度控制系統(tǒng)的設(shè)計(jì)核心在于選擇合理的濾波估計(jì)器及補(bǔ)償器，為滿足實(shí)時(shí)性及魯棒性的要求，這二者通?！耙粌?yōu)一簡(jiǎn)”，由此形成兩種設(shè)計(jì)理念[1,2]。

目前國(guó)內(nèi)外研究人員多采用優(yōu)化濾波簡(jiǎn)化補(bǔ)償?shù)乃悸?，設(shè)計(jì)高度控制系統(tǒng)，實(shí)現(xiàn)對(duì)掠海低飛過程中海浪擾動(dòng)及傳感器測(cè)量誤差的實(shí)時(shí)抑制或消除。文獻(xiàn)[3]設(shè)計(jì)了一種能對(duì)海浪擾動(dòng)進(jìn)行實(shí)時(shí)補(bǔ)償?shù)臄U(kuò)展?fàn)顟B(tài)觀測(cè)器（Extended State Observer，ESO），并對(duì)比分析了ESO、卡爾曼濾波（Kalman Filter，KF）和預(yù)測(cè)濾波（Predictive Filter）在高度控制系統(tǒng)中的應(yīng)用效果。文獻(xiàn)[4]基于等價(jià)干擾思想，提出一種能將作用于輸出的海浪擾動(dòng)等價(jià)為輸入擾動(dòng)的實(shí)時(shí)估計(jì)器。上述方法對(duì)海浪擾動(dòng)具有較強(qiáng)魯棒性，但其都沒考慮傳感器的固有噪聲問題。為此文獻(xiàn)[5]設(shè)計(jì)了一種三階組合高度測(cè)量系統(tǒng)，重點(diǎn)解決海浪擾動(dòng)及加速度表的測(cè)量誤差，但其為提高實(shí)時(shí)性，采用基于固定增益的KF 方法，故對(duì)各種擾動(dòng)的魯棒性較差，且沒有考慮高度表常值誤差的影響。文獻(xiàn)[6]提出擴(kuò)展卡爾曼濾波（Extended Kalman Filter，EKF），其通過采用狀態(tài)擴(kuò)增法，成功解決了加速度表常值漂移的影響。但當(dāng)高度表存在常值誤差或加速度表的慢變漂移較顯著時(shí)，其估計(jì)結(jié)果存在較大偏差。

綜上所述，針對(duì)掠海飛行器，目前國(guó)內(nèi)外尚沒有一種基于多源高度信息融合的濾波方案，可同時(shí)有效地解決海浪擾動(dòng)和高度表、加速度表的各種測(cè)量誤差問題。為此，本文提出一種將卡爾曼濾波、預(yù)測(cè)濾波及有色噪聲處理方法進(jìn)行結(jié)合的新型濾波方案，其核心思想如下：基于線性卡爾曼濾波，采用狀態(tài)擴(kuò)展法和量測(cè)差分法來(lái)解決高度表和加速度表的常值偏差；引入預(yù)測(cè)濾波實(shí)時(shí)估計(jì)修正模型偏差，補(bǔ)償加速度表誤差建模的偏差；基于貝葉斯理論，給予修正模型先驗(yàn)估計(jì)值更高的權(quán)重來(lái)處理海浪擾動(dòng)及高度表的測(cè)量白噪聲。

1 掠海低飛的主要擾動(dòng)建模及特性分析

掠海飛行器通常搭載慣性測(cè)量單元及雷達(dá)高度表兩種導(dǎo)航設(shè)備。高度表主要用于校正加速度表的測(cè)量誤差，但由于其測(cè)量的是飛行器與海面的實(shí)際相對(duì)高度，故使得海浪高度成為飛行過程中的一種重要擾動(dòng)，同時(shí)也引入了自身的測(cè)量誤差。本文重點(diǎn)研究?jī)?nèi)容是對(duì)掠海低飛過程中主要擾動(dòng)的抑制問題，故選擇的研究對(duì)象為已具有自動(dòng)駕駛儀及補(bǔ)償器的高度控制系統(tǒng)，其基本結(jié)構(gòu)如圖1所示[3]。圖中hcom、h、hm及分別代表期望高度、相對(duì)平均海平面高度、高度表實(shí)際測(cè)量高度及估計(jì)高度。hwave是相對(duì)平均海平面的海浪高度。

為測(cè)試基于不同濾波方法的高度控制系統(tǒng)是否能解決上述各種擾動(dòng)問題，需對(duì)這些擾動(dòng)特定進(jìn)行建模分析，具體內(nèi)容如下。

1 .1 雷達(dá)高度表引入的擾動(dòng)

雷達(dá)高度表提供飛行器高度和高度速率測(cè)量信息，可按式(1)進(jìn)行建模：

式中：nh_bias、為高度及高度速率的常值誤差；nh_white、為相應(yīng)的測(cè)量隨機(jī)噪聲，是高斯白噪聲，hwave為海浪擾動(dòng)。對(duì)于海浪擾動(dòng)目前已有許多建模方法，其中隨機(jī)過程法是將海浪視為平穩(wěn)隨機(jī)過程，采用譜分析技術(shù)進(jìn)行建模，其精度較高，應(yīng)用最為廣泛。本文采用ITTC 雙參數(shù)海浪譜S(ω)模型，當(dāng)考慮多普勒效應(yīng)時(shí)，高度表實(shí)際感受到的海浪譜Sm(ωm)如下：

式中：V為飛行器運(yùn)動(dòng)速度，β為航向角，，有效波高Hsig及平均周期T可由海況等級(jí)進(jìn)行查表確定[2]。

有了海浪譜，便可基于隨機(jī)過程理論進(jìn)行海浪建模，具體又可分為有理譜法[5]和基于Pierson 模型[6]的疊加法兩種。有理譜法采用逼近理論，存在一定的誤差，為此本文采用基于Pierson 模型的疊加法，即認(rèn)為海浪由無(wú)限多個(gè)振幅、頻率、相位均不相同的余弦波組成，由此構(gòu)建的二維海浪模型如下：

式中：x，t分別為海面的位置及測(cè)量時(shí)刻；N為頻譜間隔數(shù)，即擾動(dòng)波的總數(shù)；ai，ωi，ki分別為第i種擾動(dòng)波的幅值，頻率及權(quán)重，相關(guān)關(guān)系如下：

海浪運(yùn)動(dòng)模擬表明，高速掠海飛行器實(shí)際感受到的海浪擾動(dòng)可近似為白噪聲，假設(shè)服從高斯分布。

1.2 加速度表引入的擾動(dòng)

加速度表在高度控制系統(tǒng)中主要提供縱向加速度信息，誤差模型如下：

式中：am為加速度表的測(cè)量值；ay為真實(shí)值；abias、amarkov及awhite分別為逐次啟動(dòng)漂移、慢變漂移及快變漂移，本文分別采用隨機(jī)常數(shù)、一階馬爾科夫過程及高斯白噪聲近似描述，其滿足如下關(guān)系：

式中：α= 5，wdrive是高斯白噪聲；δkj為Kronecker符號(hào)。

2 高度組合濾波方案

高度控制系統(tǒng)中的濾波估計(jì)器是一個(gè)線性系統(tǒng)，其輸入量為加速度表測(cè)量信息，觀測(cè)量為高度表測(cè)量信息，具體的狀態(tài)方程和量測(cè)方程如下：

本文要解決的核心問題就是處理w及v中包含的多種擾動(dòng)誤差，解決思路如下：基于卡爾曼濾波能處理高斯白噪聲擾動(dòng)的特性，通過給予模型預(yù)測(cè)值較大的權(quán)重，實(shí)現(xiàn)對(duì)任意海況下海浪擾動(dòng)的抑制；引入有色噪聲處理思想，對(duì)卡爾曼濾波進(jìn)行改進(jìn)，解決規(guī)律明確的常值擾動(dòng)；對(duì)海浪擾動(dòng)的魯棒性取決于狀態(tài)模型的精度，而加速度的慢變漂移是模型偏差一部分，且其變化規(guī)律難以事先確定，為此采用預(yù)測(cè)濾波實(shí)時(shí)估計(jì)出慢變漂移，對(duì)模型進(jìn)行補(bǔ)償修正，從而獲得高精度的模型預(yù)測(cè)值。

2.1 改進(jìn)的卡爾曼濾波

目前對(duì)有色噪聲的濾波方法，可分為函數(shù)模型補(bǔ)償濾波及隨機(jī)模型補(bǔ)償濾波類[7,8]。基于簡(jiǎn)單有效的準(zhǔn)則，本文選擇第一類中的狀態(tài)擴(kuò)展及量測(cè)差分法，并將其與KF 進(jìn)行結(jié)合，實(shí)現(xiàn)對(duì)高度表及加速度表常值誤差的處理。首先對(duì)w、v進(jìn)行白化處理：

式中：F、G為單位陣；η、ζ為白噪聲，方差分別為Q、R。

采用狀態(tài)擴(kuò)增法來(lái)處理模型噪聲，擴(kuò)增狀態(tài)后的狀態(tài)方程和量測(cè)方程為：

其中，zk*、Hk*及vk*可由式(11)進(jìn)行計(jì)算：

式中：vk*是零均值的白噪聲，且與模型噪聲η k相關(guān)，具體統(tǒng)計(jì)特性如下：

綜上所述，經(jīng)由狀態(tài)擴(kuò)增法和量測(cè)差分法獲得的新狀態(tài)方程和量測(cè)方程分別是式(9)和式(10)。接下來(lái)基于卡爾曼濾波理論，可獲得狀態(tài)更新方程[9]：

2.2 預(yù)測(cè)濾波

預(yù)測(cè)濾波是一種基于最小模型誤差準(zhǔn)則的濾波算法，能實(shí)時(shí)對(duì)模型任意形式的誤差進(jìn)行估計(jì)，進(jìn)而修正系統(tǒng)模型[10]。本文將預(yù)測(cè)濾波與改進(jìn)KF 進(jìn)行結(jié)合，利用預(yù)測(cè)濾波的優(yōu)勢(shì)來(lái)實(shí)時(shí)修正擴(kuò)增狀態(tài)模型，從而解決加速度表的慢變漂移，其基本原理如式(14)所示：

式中：Gk為模型誤差擾動(dòng)矩陣；dk為模型誤差。

dk可通過極小化性能評(píng)價(jià)函數(shù)J(d)獲得，J(d)體現(xiàn)了預(yù)測(cè)濾波的核心思想，其是由測(cè)量輸出和預(yù)測(cè)輸出間殘差以及模型誤差修正項(xiàng)的加權(quán)平方和組成，表達(dá)式如下：

式中：W為正定權(quán)重矩陣；為預(yù)測(cè)輸出，為便于推導(dǎo)，首先給出的通用計(jì)算表達(dá)式：

式中：i= 1,2…m,m為量測(cè)向量維數(shù)；q為狀態(tài)向量維數(shù)；pi為dk出現(xiàn)在量測(cè)方程微分中的最低階數(shù)；Lkf(),Lgj()為相應(yīng)的李導(dǎo)數(shù)。

通過對(duì)J(d)求極值，并經(jīng)過相關(guān)推導(dǎo)，可得評(píng)價(jià)函數(shù)為極小值時(shí)的模型誤差dk計(jì)算表達(dá)式：

式中：I 為單位陣。

2.3 新型組合濾波流程

基于上述分析可得，本文所提出的新型組合濾波基本流程總結(jié)如下：

①計(jì)算初始狀態(tài)。利用初始時(shí)刻量測(cè)量z0獲得x0a的線性最小方差估計(jì)：

②計(jì)算模型誤差dk。本文引入dk主要用來(lái)補(bǔ)償加速度表的慢變漂移，故取Gk=Bka。此外，考慮到海浪擾動(dòng)主要體現(xiàn)在高度測(cè)量中，故為減小計(jì)算量，僅采用高度量測(cè)信息計(jì)算模型偏差，簡(jiǎn)化式(18)為：

式中：預(yù)測(cè)輸出經(jīng)(推導(dǎo)，可得如下近似關(guān)系：

③基于式(14)，計(jì)算狀態(tài)一步預(yù)測(cè)值。

④基于式(11)～(13)，計(jì)算狀態(tài)更新值及誤差方程陣Pk+1。由于此時(shí)模型已修正，故可基于貝葉斯理論將Q設(shè)置較小，給予修正模型估計(jì)值更大的權(quán)重，從而解決海浪擾動(dòng)及高度表白噪聲的影響。

至此，重復(fù)步驟②～④，便可完成對(duì)雷達(dá)高度表及加速度表的組合測(cè)量信息的濾波，從而估計(jì)出高度及高度速率。

3 仿真校驗(yàn)

接下來(lái)主要針對(duì)不同的擾動(dòng)誤差，對(duì)預(yù)測(cè)濾波[3]、ESO[3]、EKF[6]及所提出的組合濾波的濾波性能及其在高度控制系統(tǒng)中的應(yīng)用效果進(jìn)行對(duì)比分析。

表1 傳感器誤差特性Tab.1 Error characteristics of sensors

掠海飛行器一般需具備多高度下的平飛能力及不同高度間的轉(zhuǎn)換能力，為此本文假設(shè)飛行場(chǎng)景為：初始平飛高度45 m，要求在4 s 的響應(yīng)時(shí)間內(nèi)，進(jìn)入更低的飛行高度8 m，并在該高度平穩(wěn)飛行10 s，再快速提升至35 m 高度繼續(xù)平飛。仿真條件如下：①仿真時(shí)間t=30s、步長(zhǎng)T=0.01s 、傳感器采樣頻率為100 Hz；②初始高度及高度速率［ 45m,0m/s ］；③飛行器及自動(dòng)駕駛儀參數(shù)參考文獻(xiàn)[3]；基于構(gòu)建的傳感器誤差模型，結(jié)合當(dāng)前傳感器測(cè)量水平，設(shè)置誤差特性如表1所示。

3.1 高度估計(jì)結(jié)果

本節(jié)從濾波性能角度，評(píng)估不同擾動(dòng)因素對(duì)不同濾波方法的影響程度，相應(yīng)擾動(dòng)特性及高度估計(jì)偏差如圖2～5 所示。

圖2 海浪擾動(dòng)（海況7）的影響Fig.2 Influence of wave disturbance (sea state 7)

圖3 高度表常值誤差+白噪聲的影響Fig.3 The influence of altimeter constant value error and white noise

圖4 加速度常值漂移+白噪聲的影響Fig.4 The influence of acceleration constant drift and white noise

圖5 加速度慢變漂移+白噪聲的影響Fig.5 The influence of acceleration slow drift and white noise

具體估計(jì)精度可由實(shí)際高度與估計(jì)高度的均方根誤差（Root Mean Squared Error，RMSE）進(jìn)行量化表征，如表2所示。由于本文在仿真時(shí)，采用相同的計(jì)算步長(zhǎng)，故不同方法的實(shí)時(shí)性差異較小。

由圖2～5 及表2可得，預(yù)測(cè)濾波對(duì)于直接體現(xiàn)在高度觀測(cè)信息中的擾動(dòng)，即海浪擾動(dòng)和高度表常值誤差，具有最強(qiáng)的魯棒性，但無(wú)法處理作用于輸入信號(hào)的加速度測(cè)量誤差，其原因是預(yù)測(cè)濾波對(duì)狀態(tài)模型精度的依賴性非常高。實(shí)際應(yīng)用預(yù)測(cè)濾波時(shí)，可通過調(diào)節(jié)量測(cè)方差及狀態(tài)方差的大小來(lái)減小估計(jì)偏差，但調(diào)參過程較為繁瑣，且估計(jì)精度的提高會(huì)降低對(duì)海浪及高度表誤差的魯棒性；ESO 具有與預(yù)測(cè)濾波相似的特性，即基于對(duì)模型的強(qiáng)依賴性，獲得對(duì)海浪擾動(dòng)及高度表測(cè)量誤差的強(qiáng)魯棒性。但ESO 的估計(jì)精度取決于觀測(cè)器的極點(diǎn)位置，為使ESO 適用于0.01 s 的步長(zhǎng)，本文將其觀測(cè)器的極點(diǎn)配置于-200；基于狀態(tài)擴(kuò)展法的EKF，是已有濾波方法中估計(jì)精度最佳的方法，但難以解決高度表常值誤差及加速度表慢變漂移；組合濾波相比于EKF，對(duì)海浪擾動(dòng)及加速度常值漂移的抑制能力稍弱，但可有效地處理高度表常值漂移及加速度表的慢變漂移，RMSE 值較EKF 法進(jìn)一步降低了25%，故綜合擾動(dòng)處理能力最優(yōu)。

表2 濾波方法對(duì)擾動(dòng)因素的RMSE（單位：m）Tab.2 RMSE of filter method to disturbance factors(unit:m)

3.2 高度跟蹤結(jié)果

本節(jié)從指令跟蹤角度，分析不同濾波方法在圖1高度控制系統(tǒng)中的實(shí)時(shí)應(yīng)用效果?；诼雍ｏw行器的特點(diǎn)，高度控制系統(tǒng)應(yīng)具備如下特點(diǎn)：①無(wú)較大振蕩地保持飛行器在要求的掠海飛行高度；②不同平飛高度之間的轉(zhuǎn)換要快速；③全程沒有較大的超調(diào)量。

本文只給出在擾動(dòng)最嚴(yán)重的情況下，基于不同濾波方法的高度控制系統(tǒng)的控制性能，如圖6所示。

圖6 考慮所有擾動(dòng)因素時(shí)的高度控制系統(tǒng)性能對(duì)比Fig.6 Comparison of the performances of altitude controllers under the effect of all disturbance

由圖6可得，當(dāng)考慮所有擾動(dòng)因素時(shí)，僅有組合濾波方法能達(dá)到掠海低飛的控制目標(biāo)；由于存在加速度測(cè)量誤差，預(yù)測(cè)濾波和ESO 直接發(fā)散；EKF 雖能快速進(jìn)行高度轉(zhuǎn)換，但在8 m 高度階段出現(xiàn)與高度表常值誤差大小成正比的穩(wěn)態(tài)偏差，在35 m 階段穩(wěn)態(tài)偏差增大且進(jìn)一步產(chǎn)生了動(dòng)態(tài)振蕩，振蕩程度與加速度表的慢變漂移及白噪聲大小有關(guān)。

4 結(jié) 論

本文針對(duì)掠海飛行器高度控制系統(tǒng)設(shè)計(jì)時(shí)，無(wú)法同時(shí)解決海浪擾動(dòng)、雷達(dá)高度表及加速度表測(cè)量誤差對(duì)系統(tǒng)的控制精度影響問題，基于優(yōu)化濾波簡(jiǎn)化補(bǔ)償?shù)乃悸?，提出一種將卡爾曼濾波、預(yù)測(cè)濾波及有色噪聲處理方法進(jìn)行結(jié)合的高度組合濾波方法，實(shí)現(xiàn)了對(duì)掠海低飛過程中多種擾動(dòng)的實(shí)時(shí)抑制。仿真結(jié)果表明，相比于已有的預(yù)測(cè)濾波、ESO 及EKF 方法，組合濾波可以很好地解決混合擾動(dòng)問題，在多種擾動(dòng)同時(shí)存在的情況下，高度估計(jì)的RMSE 最低，并且在給定PD補(bǔ)償器的前提下，僅有組合濾波滿足掠海低飛高度控制系統(tǒng)的要求，具有一定的參考價(jià)值。