基于擴(kuò)展卡爾曼濾波的三維風(fēng)速在線估計(jì)方法

涂海峰 賈生偉 陽(yáng)豐俊 呂 瑞

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

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

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

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

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

1 風(fēng)速估計(jì)原理

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

Vg=Va+Vwg

(1)

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

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

(2)

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

圖2 空速的分解圖

本文所研究的巡飛彈藥中并未安裝用于測(cè)量攻角和側(cè)滑角的傳感器，只在軸向上安裝了空速管用于測(cè)量軸向上的動(dòng)壓值，可算得巡飛彈藥縱向軸的空速，不能得到攻角和側(cè)滑角信息。因此，不能通過(guò)式(2) 的實(shí)時(shí)計(jì)算得到空速在地面坐標(biāo)系中的分量，進(jìn)而計(jì)算出風(fēng)場(chǎng)信息。然而空速管計(jì)算得到的軸向空速可用于空速大小的估算。空速在機(jī)體坐標(biāo)系下的投影為：

(3)

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

(4)

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

(5)

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

(6)

2 基于EKF的三維風(fēng)速在線估計(jì)方法

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

圖3 空速和地速在地面坐標(biāo)系中的分解圖

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

(7)

本文主要針對(duì)常值風(fēng)進(jìn)行估計(jì)，其在空中緩慢變化，因此根據(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)得到，下面還需要對(duì)量測(cè)方程進(jìn)行適當(dāng)?shù)淖冃巍８鶕?jù)風(fēng)速的三角形關(guān)系，可有：

(10)

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

(11)

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

(12)

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

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

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

(13)

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

(14)

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

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

(15)

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

3 風(fēng)速估計(jì)的數(shù)值仿真試驗(yàn)驗(yàn)證

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

圖4 飛行軌跡圖

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

(16)

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

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

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

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

4 結(jié)論

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

參 考 文 獻(xiàn)

[1] 高社生, 張極, 黨進(jìn)偉. 風(fēng)場(chǎng)估計(jì)算法及其在組合導(dǎo)航中的應(yīng)用[J]. 飛行力學(xué), 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] 屈耀紅. 小型無(wú)人機(jī)航跡規(guī)劃及組合導(dǎo)航關(guān)鍵技術(shù)研究[D]. 西安: 西北工業(yè)大學(xué), 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ìn)行風(fēng)場(chǎng)估計(jì)[J]. 飛行力學(xué), 1992, 10(3): 41-48.(Liu Lin. Wind Estimation in Flight[J]. Flight Dynamics, 1992, 10(3): 41-48.)

[9] 周偉靜, 達(dá)興亞, 沈懷榮. 基于粒子濾波算法的風(fēng)場(chǎng)估計(jì)方法[J]. 系統(tǒng)仿真學(xué)報(bào), 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è)等. 基于擴(kuò)展卡爾曼濾波的風(fēng)速估計(jì)算法研究[J]. 電子測(cè)量技術(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)動(dòng)基座的SINS初始對(duì)準(zhǔn)方法研究[J]. 宇航學(xué)報(bào), 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] 高偉熙, 繆玲娟, 倪茂林. 一種引入陀螺角速度信息的快速對(duì)準(zhǔn)方法[J]. 宇航學(xué)報(bào), 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] 游金川, 秦永元, 楊鵬翔, 嚴(yán)恭敏. 捷聯(lián)慣導(dǎo)加速度計(jì)尺寸效應(yīng)誤差建模及標(biāo)定[J]. 宇航學(xué)報(bào), 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] 喬國(guó)棟, 倪茂林, 李鐵壽. 基于能觀度分析的信息融合自主導(dǎo)航算法[J]. 宇航學(xué)報(bào), 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] 楊曉霞, 陰玉梅. 可觀測(cè)度的探討及其在捷聯(lián)慣導(dǎo)系統(tǒng)可觀測(cè)性分析中的應(yīng)用[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 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.)