基于擴展卡爾曼濾波的三維風速在線估計方法

涂海峰 賈生偉 陽豐俊 呂 瑞

中國運載火箭技術(shù)研究院，北京100076

風作為飛行器的重要外部環(huán)境因素，對飛行器的飛行起著至關(guān)重要的作用，尤其是小型飛行器，如小型無人機、四旋翼等，很容易受到風的影響[1-3]。風具有較強的隨機性，很難通過建模的方式計算地球任何一點的風速信息[4]，工程上通常采用機載設(shè)備的測量數(shù)據(jù)實現(xiàn)風速的估算。在國內(nèi)外已經(jīng)有學者展開通過飛行器有限的飛行數(shù)據(jù)對風場進行估計的研究。在國外，Brezoescu等[2]采用運動方程計算得到的飛行數(shù)據(jù)與傳感器的測量數(shù)據(jù)的差估計風速，同時提出用在線自適應(yīng)技術(shù)處理傳感器噪聲。Jack等[5]研究采用GPS/INS、空速等測量數(shù)據(jù)實現(xiàn)小型或微型飛行器周圍的風速和風速梯度的估計。LEFAS等[6]采用空速和雷達測量數(shù)據(jù)實現(xiàn)了風速的估計，Harish等[7]采用飛行數(shù)據(jù)通過無跡卡爾曼濾波實現(xiàn)了有航向角和無航向角觀測數(shù)據(jù)情況下的風速估計。在國內(nèi)，劉林[8]通過風速、空速、地速的三角關(guān)系構(gòu)建了風場估計模型，并通過卡爾曼濾波實現(xiàn)風場的估計。屈耀紅[4]在其博士論文中根據(jù)風速的矢量關(guān)系提出了多步長法，并采用卡爾曼濾波器實現(xiàn)風場的估計。周偉靜等[9]采用粒子濾波算法實現(xiàn)對一階時變自回歸風速模型的估計。這些學者進行風速估計時采用空速管的數(shù)據(jù)近似為飛行器的真實空速，忽略了攻角和側(cè)滑角的影響。

Cho等[10]利用GPS和空速管測量數(shù)據(jù)估算水平面內(nèi)的風向和風速，在建立風速估計模型時考慮到了攻角和側(cè)滑角的影響，并通過飛行試驗進行了驗證。何波等[11]也采用類似方法進行風速和風向的估計。他們僅實現(xiàn)了水平風場信息的估計，沒有對鉛垂面內(nèi)的風進行建模和估計，而對于在對流層內(nèi)飛行的巡飛彈藥來說，鉛垂面內(nèi)的風也需要考慮。

在線估計風向和風速通常建立狀態(tài)空間模型，通過卡爾曼濾波實現(xiàn)，然而能否在飛行中估計出精確結(jié)果，取決于系統(tǒng)是否可觀測。對系統(tǒng)可觀測性的分析已經(jīng)有較多的研究，錢偉行、劉建業(yè)等[12]運用譜條件數(shù)方法分析了載體在轉(zhuǎn)動基座初始對準系統(tǒng)中各狀態(tài)量的可觀測度；高偉熙等[13]將陀螺角速度誤差作為觀測量引入靜基座對準系統(tǒng)，通過可觀測性的定性分析和可觀測度的定量計算后發(fā)現(xiàn)可提高系統(tǒng)的可觀測度；游金川、秦永元等[14]利用分段定常系統(tǒng)可觀測性分析方法分析了加速度計的尺寸效應(yīng)估計系統(tǒng)狀態(tài)的可觀測性；喬國棟等[15]基于能觀度分析，提出了一種地月轉(zhuǎn)移軌道的自主導航算法。

考慮巡飛彈由于空間和成本的限制，無法安裝攻角和側(cè)滑角傳感器，因此采用彈載的GPS/MIMU組合導航系統(tǒng)的輸出數(shù)據(jù)與空速管測量的動壓值，通過擴展卡爾曼濾波實現(xiàn)地面坐標系下三維風速的在線估計，并利用數(shù)值仿真試驗進行驗證。

1 風速估計原理

在地面坐標系中，巡飛彈藥相對于地面的速度矢量Vg，相對于空氣的速度矢量Va以及空氣相對于地面的速度矢量Vwg構(gòu)成三角關(guān)系[8](見圖1)，則有：

Vg=Va+Vwg

(1)

圖1 風速的三角關(guān)系圖

其中，Vg可以通過巡飛彈藥中的GPS/MIMU組合導航系統(tǒng)計算得到，空速可通過圖2所示的方式轉(zhuǎn)換到地面坐標系中，其計算公式為：

(2)

式中，L(ψ,θ,φ)為彈體系到地面系的轉(zhuǎn)換矩陣，L(-α,β)T為速度坐標系到彈體坐標系的轉(zhuǎn)換矩陣，Va為速度坐標系下的空速值。

圖2 空速的分解圖

本文所研究的巡飛彈藥中并未安裝用于測量攻角和側(cè)滑角的傳感器，只在軸向上安裝了空速管用于測量軸向上的動壓值，可算得巡飛彈藥縱向軸的空速，不能得到攻角和側(cè)滑角信息。因此，不能通過式(2) 的實時計算得到空速在地面坐標系中的分量，進而計算出風場信息。然而空速管計算得到的軸向空速可用于空速大小的估算?？账僭跈C體坐標系下的投影為：

(3)

式中，Vab為空速在彈體坐標系下的矢量。因此空速管測得的空速Vpitot與真實空速的關(guān)系如下：

(4)

巡飛彈藥中的空速管可以測量動壓，根據(jù)伯努利方程，其空速測量原理為[10-11]：

(5)

式中，ΔP表示動壓，ρ為空氣密度，K為校正因子，用于校正由溫度、空氣黏度、不穩(wěn)定氣流以及安裝誤差等引起的空速管測量誤差。式(4)代入式(5)可得：

(6)

2 基于EKF的三維風速在線估計方法

空速管在測量風速時會存在較大的高頻噪聲[8]，為了獲得平滑、精度較高的風速估計值，采用EKF實現(xiàn)風速的非線性估計。式(6)可作為風速估計的量測方程，因此還需建立風速估計的狀態(tài)方程。風的模型有很多種，如定常風、突風(陣風)、風切變和湍流等。為了不失一般性，本文只關(guān)注定常風。風速在地面坐標系中的分解如圖3所示。

圖3 空速和地速在地面坐標系中的分解圖

需要估計得到的狀態(tài)量為：

(7)

本文主要針對常值風進行估計，其在空中緩慢變化，因此根據(jù)離散化狀態(tài)轉(zhuǎn)移模型，加入白噪聲便可建立系統(tǒng)的狀態(tài)方程[11]，如式(8)所示：

x(k+1)=Fx(k)+wk

(8)

式中，

(9)

其中，k表示第k次迭代值。系統(tǒng)方程已經(jīng)得到，下面還需要對量測方程進行適當?shù)淖冃?。根?jù)風速的三角形關(guān)系，可有：

(10)

式中，Vgx，Vgy和Vgz可由GPS/MIMU組合導航系統(tǒng)輸出，ΔP可由空速管實時測量得到，可以作為系統(tǒng)的量測信息，因此可由式(10)得到系統(tǒng)的量測方程為：

(11)

其中，υk∈N(0,R)為量測白噪聲。該量測方程為非線性方程，需要求取其Jacobian矩陣得到線性觀測矩陣為：

(12)

線性化的狀態(tài)方程和觀測方程都已經(jīng)求得，便可以用EKF進行迭代求解得到風速的估計值。

該方法要實現(xiàn)風速值的估計，還需要滿足可觀測性的要求，即需要滿足狀態(tài)量完全可觀測。系統(tǒng)的可觀測性可通過求取可觀測矩陣的秩，如果為滿秩矩陣則完全可觀測，不滿足則不完全可觀測[18]。

根據(jù)式(8)和(12)建立可觀測矩陣為：

(13)

矩陣F為單位矩陣，則上式可變?yōu)椋?/p>

(14)

顯然可觀測矩陣的秩rank(O)=4，觀測矩陣為滿秩矩陣，因此系統(tǒng)完全可以觀測，即通過擴展卡爾曼濾波實現(xiàn)風速的估計是可行的。

在進行濾波估計之前，還需要確定濾波的初始值，以便于卡爾曼濾波能快速收斂。根據(jù)風速的三角形公式，可通過濾波初始時刻的巡飛彈藥的初始地速和空速管測量的空速通過下式計算得到近似的風速值。

(15)

其中，下標“0”表示初始時刻。過程噪聲需要根據(jù)天氣情況進行設(shè)定，而量測噪聲可以通過實驗測得。在實際應(yīng)用中，雖然可觀測矩陣為滿秩，還需有足夠的激勵信息才能實現(xiàn)風速的估計。對三維風速信息的估算，既需要通過水平方向的機動使得測量數(shù)據(jù)中包含足夠的信息以實現(xiàn)水平方向常值風速的準確估計，還需要在鉛垂方向進行必要的機動實現(xiàn)縱向風速的估計。

3 風速估計的數(shù)值仿真試驗驗證

使用巡飛彈藥仿真平臺，在仿真中加入常值風的干擾。為實現(xiàn)三維風速的估計，設(shè)定如圖4(a)中的1-4點為航路點，并將其中1和3的目標飛行高度設(shè)置為700m，2和4點的目標高度設(shè)置為600m，飛行中每隔300s變換一次三向風速。仿真飛行的平面軌跡如圖4(a)所示，三維軌跡如圖4(b)所示。

圖4 飛行軌跡圖

仿真中用到的狀態(tài)初值由式(15)確定，其他初值設(shè)定如下：

(16)

三維風速的估計結(jié)果如圖5所示，三維風速的估計值與真值的誤差如圖6所示?？梢钥闯?，在300s內(nèi)，所提算法能準確地估計出三向的風速值和比例因子。估計穩(wěn)定之后三向風速的估計結(jié)果如表1所示，水平風速誤差不大于0.1m/s，垂直方向的風速誤差不超過0.4m/s，估計精度較高。

圖5 濾波估計結(jié)果與真值的對比

圖6 濾波結(jié)果與真值的誤差

風場變化次數(shù)Vwgx真值Vwgx估計值Vwgy真值Vwgy估計值Vwgz真值Vwgz估計值155．04354．98421．88521010．0432．95954．631332．99688．09511．1124-2-2．0091110．991-2-1．828

4 結(jié)論

由于成本和空間的限制，本文研究的巡飛彈藥沒有安裝測量攻角和側(cè)滑角傳感器，只能通過估算得到。然而在有風的情況下，攻角和側(cè)滑角的估算精度低。因此通過構(gòu)建風速估計模型，采用擴展卡爾曼濾波算法，利用巡飛彈藥中的GPS/MIMU和空速管的輸出在線估計出風速信息。仿真試驗表明：當濾波收斂后，水平風速精度小于0.1m/s，鉛垂方向風速精度小于0.4m/s。

參 考 文 獻

[1] 高社生, 張極, 黨進偉. 風場估計算法及其在組合導航中的應(yīng)用[J]. 飛行力學, 2013, 31(5):467-471. (Gao Shesheng, Zhang Ji, Dang Jinwei. Wind Field Estimation Algorithm and Its Application in Integrated Navigation[J]. Flight Dynamics, 2013, 31(5):467-471.)

[2] Brezoescu A, Castillo P, Lozano R. Wind Estimation for Accurate Airplane Path Following Applications[J]. Journal Intelligent and Robotic Systems, 2014, 73(4): 823-831.

[3] Morelli E A. Real-Time Aerodynamic Parameter Estimation without Air Flow Angle Measurements[J]. Journal of Aircraft, 2012, 49(4): 1064-1074.

[4] 屈耀紅. 小型無人機航跡規(guī)劃及組合導航關(guān)鍵技術(shù)研究[D]. 西安: 西北工業(yè)大學, 2006.(Qu Yaohong. Study on the Key Techniques of Trajectory Planning and Integrated Navigation of UAV[D]. Xi’an，Northwestern Polytechnical University, 2006.)

[5] Langelaan J W, Alley N, Neidhoefer J. Wind Field Estimation for Small Unmanned Aerial Vehicles[J]. Journal of Guidance, Control, and Dynamics, 2011, 34(4): 1016-1030.

[6] Lefas C C. Real-Time Wind Estimation and Tracking with Transponder Downlinked Airspeed and Heading Data[J]. IEEE Transactions on Aerospace and Electronic Systems, 1987, 23(2): 169-174.

[7] Palanthandalam-Madapusi H J, Girard A, Bernstein D S. Wind-Field Reconstruction Using Flight Data[C]. 2008 American Control Conference. Washington, USA, 2008.

[8] 劉林. 在飛行中進行風場估計[J]. 飛行力學, 1992, 10(3): 41-48.(Liu Lin. Wind Estimation in Flight[J]. Flight Dynamics, 1992, 10(3): 41-48.)

[9] 周偉靜, 達興亞, 沈懷榮. 基于粒子濾波算法的風場估計方法[J]. 系統(tǒng)仿真學報, 2009, 21(2): 38-42.(Zhou Weijing, Da Xingya, Shen Huairong. Research of Wind Estimation Based on Particle Filter[J]. Journal of System Simulation, 2009, 21(2): 38-42.)

[10] Cho A, Kim J, Lee S, et al. Wind Estimation and Airspeed Calibration Using a UAV with a Single-Antenna GPS Receiver and Pitot Tube[J]. IEEE Transactions on Aerospace and Electronic Systems, 2011, 47(1): 109-117.

[11] 何波, 李榮冰, 劉建業(yè)等. 基于擴展卡爾曼濾波的風速估計算法研究[J]. 電子測量技術(shù), 2014, 37(6): 34-37.(He Bo, Li Rongbing, Liu Jianye, et al. Estimation Algorithm for Airborne Wind Based on Extended Kalman Filter [J]. Electronic Measurement Technology, 2014, 37(6): 34-37.)

[12] 錢偉行, 劉建業(yè), 趙偉, 趙文芳. 基于轉(zhuǎn)動基座的SINS初始對準方法研究[J]. 宇航學報, 2008, 29(3): 928-932.(Qian Weihang, Liu Jianye, Zhao Wei, Zhao Wenfang. SINS Initial Alignment Based on Revolving Base [J]. Journal of Astronautic, 2008, 29(3): 928-932.)

[13] 高偉熙, 繆玲娟, 倪茂林. 一種引入陀螺角速度信息的快速對準方法[J]. 宇航學報, 2010, 31(6): 1596-1601.(Gao Weixi, Miao Lingjuan, Ni Maolin. A Fast Initial Alignment Method with Gyro Angular Rate Information[J]. Journal of Astronautic, 2010, 31(6): 1596-1601.)

[14] 游金川, 秦永元, 楊鵬翔, 嚴恭敏. 捷聯(lián)慣導加速度計尺寸效應(yīng)誤差建模及標定[J]. 宇航學報, 2012, 33(3): 311-317.(You Jinchuan, Qin Yongyuan, Yang Pengxiang, Yan Gongmin. Modeling and Calibration of the Accelerometer Size Effect Error of the SINS[J]. Journal of Astronautic, 2012, 33(3): 311-317.)

[15] 喬國棟, 倪茂林, 李鐵壽. 基于能觀度分析的信息融合自主導航算法[J]. 宇航學報, 2008, 29(3): 883-888.(Qiao Guodong, Ni Maolin, Li Tieshou. Observability-Based Information Fusion Autonomous Navigation Algorithm[J]. Journal of Astronautic, 2008, 29(3): 883-888.)

[16] Rhudy M, Larrabee T, Chao H, et al. UAV Attitude, Heading, and Wind Estimation Using GPS/INS and Air Data System[C]. AIAA Guidance, Navigation, and Control Conference. Boston, MA, 2013.

[17] Petrich J, Subbarao K. On-Board Wind Speed Estimation for UAVs[C]. AIAA Guidance, Navigation, and Control Conference. Portland, Oregon, 2011.

[18] 楊曉霞, 陰玉梅. 可觀測度的探討及其在捷聯(lián)慣導系統(tǒng)可觀測性分析中的應(yīng)用[J]. 中國慣性技術(shù)學報, 2012, 20(4): 405-409.(Yang Xiaoxia, Yin Yumei. Discussions on Observability and Its Applications in SINS[J]. Journal of Chinese Inertial Technology, 2012, 20(4): 405-409.)