高艷輝，李志宇，肖前貴

(南京航空航天大學(xué) 中小型無(wú)人機(jī)先進(jìn)技術(shù)工信部重點(diǎn)實(shí)驗(yàn)室，南京 210016)

基于GPS、磁羅盤與大氣數(shù)據(jù)計(jì)算機(jī)的無(wú)人機(jī)風(fēng)估計(jì)

高艷輝，李志宇，肖前貴

(南京航空航天大學(xué) 中小型無(wú)人機(jī)先進(jìn)技術(shù)工信部重點(diǎn)實(shí)驗(yàn)室，南京 210016)

針對(duì)無(wú)人機(jī)實(shí)時(shí)航路規(guī)劃及適應(yīng)環(huán)境變化的自主能力發(fā)展需求，提出了一種新的風(fēng)估計(jì)與空速校準(zhǔn)的方法;該方法基于GPS接收機(jī)、大氣計(jì)算機(jī)和磁羅盤等傳感器實(shí)現(xiàn);針對(duì)定常風(fēng)模型，風(fēng)速、風(fēng)向能夠利用地速、風(fēng)速和空速之間的速度矢量三角形關(guān)系計(jì)算得到；采用無(wú)導(dǎo)擴(kuò)展卡爾曼濾波(DEKF)，估計(jì)風(fēng)場(chǎng)信息以及真空速的比例校準(zhǔn)系數(shù)；利用某型無(wú)人機(jī)數(shù)字仿真平臺(tái)，在2D定常風(fēng)條件下進(jìn)行了全過(guò)程自主飛行仿真；仿真結(jié)果表明：該方法在航路跟隨的直線段、轉(zhuǎn)彎段均能準(zhǔn)確估計(jì)。

風(fēng)估計(jì)；空速校準(zhǔn)；無(wú)人機(jī)；無(wú)導(dǎo)擴(kuò)展卡爾曼濾波

0 引言

無(wú)人機(jī)(UAV)的機(jī)動(dòng)性與經(jīng)濟(jì)性使它在空投、目標(biāo)跟蹤、地理定位等軍事、民用領(lǐng)域得到了大量應(yīng)用。準(zhǔn)確的風(fēng)估計(jì)可以大大提高無(wú)人機(jī)適應(yīng)環(huán)境變化的自主能力。Sohn[1]研究表明風(fēng)場(chǎng)信息可以提供地面目標(biāo)的精確定位信息。此外，估計(jì)風(fēng)過(guò)程中，同時(shí)得到的偏航角有助于提高無(wú)人機(jī)在航路跟隨、著陸任務(wù)等方面的控制性能。事實(shí)上，風(fēng)估計(jì)本身已經(jīng)成為氣象無(wú)人機(jī)的一項(xiàng)重要應(yīng)用[2]。

本文提出了利用GPS接收機(jī)、大氣數(shù)據(jù)計(jì)算機(jī)和磁羅盤輸出的測(cè)量信息來(lái)估計(jì)風(fēng)速、風(fēng)向的一種新方法。該方法同時(shí)也對(duì)空速進(jìn)行了比例校準(zhǔn)，便于應(yīng)用于低成本無(wú)人機(jī)。該方法不同于以前使用的技術(shù)[3-5]，它不需要無(wú)人機(jī)以特定的機(jī)動(dòng)動(dòng)作飛行，在航路跟隨的直線段、轉(zhuǎn)彎段均能準(zhǔn)確估計(jì)。

1 風(fēng)估計(jì)算法設(shè)計(jì)與實(shí)現(xiàn)

1.1 風(fēng)估計(jì)算法設(shè)計(jì)

在風(fēng)場(chǎng)中無(wú)人機(jī)飛行運(yùn)動(dòng)學(xué)滿足速度矢量三角形[3]，如圖1所示。即：

(1)

圖1 速度矢量三角形

無(wú)人機(jī)導(dǎo)航一般采用地面坐標(biāo)系，因此，本文選擇在地面坐標(biāo)系進(jìn)行計(jì)算。其中，GPS接收機(jī)輸出的地速Vg、天向速度Vd和航跡角χg信息為地面坐標(biāo)系。本文主要考慮2-D定常風(fēng)模型，包括風(fēng)速Vw，風(fēng)向χw兩個(gè)參數(shù)，同樣采用地面坐標(biāo)系。大氣數(shù)據(jù)計(jì)算機(jī)輸出的真空速Va，坐標(biāo)系為氣流坐標(biāo)系，因此，需要經(jīng)過(guò)機(jī)體坐標(biāo)系、地面坐標(biāo)系的轉(zhuǎn)換。小型無(wú)人機(jī)一般采用協(xié)調(diào)轉(zhuǎn)彎飛行，方向舵主要用于消除側(cè)滑角，因此，假定β≈0。在無(wú)人機(jī)穩(wěn)定配平飛行時(shí)，迎角一般較小，為計(jì)算方便，假定α≈θ。因此，

(2)

(3)

(4)

一般情況下，無(wú)人機(jī)平飛迎角非常小，可以假設(shè)sinθ≈0。因此，在二維水平面上，得到兩個(gè)方程：

Vacos2θcosψ=Vgcosχg-Vwcosχw

(5)

Vacos2θsinψ=Vgsinχg-Vwsinχw

(6)

將公式(5)、(6)計(jì)算平方和，整理得到：

(7)

大氣數(shù)據(jù)計(jì)算機(jī)輸出的空速V利用空速管測(cè)量的總壓、靜壓根據(jù)伯努利方程計(jì)算得出[4]。

空速管頭部和靜壓孔測(cè)得的壓力值不完全與真實(shí)值相等。假設(shè)空速管的迎角較小，那么空速管測(cè)量的總壓誤差就非常小。然而，靜壓誤差卻會(huì)很大，這是因?yàn)殪o壓孔附近的氣流由于壓力傳感器本身和飛機(jī)結(jié)構(gòu)的原因受到了干擾。同時(shí)對(duì)于低成本無(wú)人機(jī)，空速管每一次的安裝位置、方向可能均有不同，導(dǎo)致安裝誤差變化較大。因此，針對(duì)實(shí)際大氣環(huán)境、安裝等誤差而設(shè)置的補(bǔ)償系數(shù)sf。若有迎角、側(cè)滑角存在，無(wú)人機(jī)真空速與Vpitot之間的關(guān)系如下：

(8)

將公式(7)代入公式(8)中，整理得到：

(9)

將公式(6)與公式(5)相除得到：

(10)

ψ=mod(atan2(Vgsinχg-Vwsinχw,Vgcosχg

-Vwcosχ),2π)

(11)

航向角由磁羅盤測(cè)量得到。磁羅盤測(cè)得的是無(wú)人機(jī)縱軸以磁北方向?yàn)榛鶞?zhǔn)的磁方位, 這個(gè)磁方位再加上一個(gè)修正量(即磁偏角)，才能得到無(wú)人機(jī)縱軸以正北方向?yàn)榛鶞?zhǔn)的真方位角。假設(shè)無(wú)人機(jī)飛行區(qū)域的磁偏角變化較小, 只要在飛行前根據(jù)起飛方向?qū)Υ牌沁M(jìn)行校正，就可以忽略磁偏角角對(duì)風(fēng)速測(cè)量精度的影響。

1.2DEKF算法

擴(kuò)展卡爾曼濾波(EKF)基于一階泰勒級(jí)數(shù)公式展開(kāi)式實(shí)現(xiàn)，是一種廣泛應(yīng)用于狀態(tài)估計(jì)的技術(shù)。EKF算法的一個(gè)主要缺點(diǎn)是它的實(shí)現(xiàn)。非線性方程所描述的模型需要求導(dǎo)和模型導(dǎo)數(shù)(或雅可比矩陣)計(jì)算。M.Q.Brendan[11]提出了一種新穎的無(wú)導(dǎo)擴(kuò)展卡爾曼濾波(DEKF)方法。DEKF算法假設(shè)狀態(tài)分布的高階矩非常小，利用n+1個(gè)n維狀態(tài)向量集來(lái)表示狀態(tài)估計(jì)和狀態(tài)的不確定性。極限情況下，該算法數(shù)學(xué)上等價(jià)于EKF。

本文在此簡(jiǎn)要描述無(wú)導(dǎo)擴(kuò)展卡爾曼濾波(DEKF)，具體參見(jiàn)文獻(xiàn)[11-12]。DEKF同傳統(tǒng)卡爾曼濾波器一樣分為兩個(gè)階段，時(shí)間更新階段與測(cè)量更新階段，為清晰起見(jiàn)，下文省去時(shí)間步長(zhǎng)索引。

(13)

(14)

(15)

(16)

(17)

(18)

(19)

(20)

1.3 風(fēng)估計(jì)算法實(shí)現(xiàn)

針對(duì)2D定常風(fēng)模型，本文選擇DEKF方法同時(shí)估計(jì)風(fēng)速、風(fēng)向和空速校準(zhǔn)系數(shù)。在某些飛行時(shí)間，這些變量基本保持固定不變，因此，可以采用隨機(jī)游走過(guò)程建模[4]。

狀態(tài)變量x=[Vwχwsf]T，系統(tǒng)動(dòng)力學(xué)描述如下：

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

(21)

其中:F為單位矩陣，w～N(0,Qk)。

在上述公式中，地速、航跡角由GPS接收機(jī)測(cè)量輸出，俯仰角由垂直陀螺測(cè)量。這些變量的精度一般較高，在每一個(gè)時(shí)刻可以當(dāng)做已知變量處理。測(cè)量變量zk=[Vaψ]，非線性觀測(cè)方程表示為：

(22)

其中:vk～N(0,Rk)，Va參見(jiàn)公式(9),ψ參見(jiàn)公式(11)。

在仿真時(shí)，測(cè)量噪聲、過(guò)程噪聲根據(jù)實(shí)際試飛數(shù)據(jù)統(tǒng)計(jì)得到。風(fēng)速、風(fēng)向的初值可以根據(jù)速度矢量三角形計(jì)算得到。

2 仿真驗(yàn)證

2.1 某型無(wú)人機(jī)仿真平臺(tái)簡(jiǎn)介

本文采用某型無(wú)人機(jī)數(shù)字仿真平臺(tái)進(jìn)行算法的仿真驗(yàn)證[13]。該平臺(tái)采用層次化、模塊化設(shè)計(jì)思想構(gòu)建，總體結(jié)構(gòu)如圖2所示。

圖2 無(wú)人機(jī)數(shù)字仿真平臺(tái)總體框圖

無(wú)人機(jī)模型接收常規(guī)執(zhí)行機(jī)構(gòu)作為輸入，完成無(wú)人機(jī)空氣動(dòng)力特性、發(fā)動(dòng)機(jī)推力特性、質(zhì)量特性和大氣特性等模塊的計(jì)算，然后求解六自由度非線性全量運(yùn)動(dòng)方程，模擬無(wú)人機(jī)的飛行狀態(tài)變化。同時(shí)也接收來(lái)自外部環(huán)境模型(例如：定常風(fēng))的變化輸入。在每一個(gè)仿真步長(zhǎng)更新的飛行參數(shù)，將傳遞給可視化模塊(航跡顯示、參數(shù)顯示)，用于顯示、存儲(chǔ)仿真數(shù)據(jù)，便于數(shù)據(jù)分析。

火箭作用力利用火箭助推器彈道性能預(yù)估曲線(推力與時(shí)間的關(guān)系曲線)模擬計(jì)算得到，并通過(guò)Stateflow精確控制火箭的作用時(shí)間。

傳感器反饋模塊用于將飛行參數(shù)轉(zhuǎn)換為真實(shí)的模擬信號(hào)輸出，這些信號(hào)將反饋至飛控系統(tǒng)用于無(wú)人機(jī)的軌跡控制。傳感器模擬模塊由AeroSimBlockset提供，根據(jù)實(shí)際傳感器參數(shù)進(jìn)行設(shè)置。

飛控系統(tǒng)仿真包含模態(tài)管理和控制器兩個(gè)模塊，其中控制器采用embeddedSimulinkFunction的方式來(lái)實(shí)現(xiàn)。模態(tài)管理根據(jù)遙控指令以及預(yù)規(guī)劃航路等輸入信息，實(shí)現(xiàn)無(wú)人機(jī)的導(dǎo)航與制導(dǎo)、飛行模態(tài)的邏輯控制與切換等功能?？刂破鞲鶕?jù)輸入的飛行參數(shù)、縱向給定量、橫航向給定量、縱向飛行模態(tài)、橫航向飛行模態(tài)進(jìn)行控制律解算，輸出升降舵、副翼舵、方向舵和油門信息用來(lái)控制無(wú)人機(jī)實(shí)現(xiàn)相應(yīng)的飛行模態(tài)。

執(zhí)行機(jī)構(gòu)模塊用于模擬舵機(jī)的轉(zhuǎn)速、頻帶、零位、遲滯等特性。該模塊由AeroSimBlockset提供，根據(jù)實(shí)際舵機(jī)特性進(jìn)行參數(shù)設(shè)置。

2.2 仿真過(guò)程與結(jié)果分析

本文假定校準(zhǔn)系數(shù)初值為0.9，風(fēng)速設(shè)定為30m/s，風(fēng)向?yàn)?2.45°。仿真過(guò)程中DKF用到的參數(shù)包括：

某型無(wú)人機(jī)按照航線在無(wú)人機(jī)數(shù)字仿真平臺(tái)上進(jìn)行全過(guò)程自主飛行仿真。在平飛高度施加定常風(fēng)干擾，風(fēng)向與平飛段航線夾角為90°，為正側(cè)風(fēng)干擾。在無(wú)人機(jī)飛行穩(wěn)定后，接通DEKF濾波，風(fēng)估計(jì)、殘差以及空速校準(zhǔn)的仿真結(jié)果如圖3、圖4所示。

圖3 風(fēng)速、風(fēng)向、校準(zhǔn)系數(shù)仿真結(jié)果

圖4 殘差計(jì)算結(jié)果

圖3、圖4中，在300s時(shí)，仿真平臺(tái)自動(dòng)接通DEKF進(jìn)行風(fēng)估計(jì)。在300～650s，無(wú)人機(jī)處于直線自主飛行。在接通濾波20s后，風(fēng)速、風(fēng)向、校準(zhǔn)系數(shù)即可快速預(yù)測(cè)，風(fēng)速殘差小于5m/s,風(fēng)向殘差小于10°，校準(zhǔn)系數(shù)殘差小于0.02。在650～900s，無(wú)人機(jī)處于180°轉(zhuǎn)彎飛行，風(fēng)速殘差小于0.6m/s,風(fēng)向殘差小于9°,校準(zhǔn)系數(shù)殘差小于0.017。在900～1 100s，無(wú)人機(jī)處于直線自主飛行狀態(tài)，在1 100～1 350s無(wú)人機(jī)處于轉(zhuǎn)彎飛行狀態(tài)，經(jīng)過(guò)一圈飛行后，風(fēng)速殘差小于0.15m/s,風(fēng)向殘差小于2°,校準(zhǔn)系數(shù)殘差小于0.004 且估計(jì)精度進(jìn)一步提高。在整個(gè)飛行過(guò)程中，風(fēng)速、風(fēng)向、校準(zhǔn)系數(shù)的估計(jì)結(jié)果均滿足3-sigma邊界要求。仿真結(jié)果表明：DEKF估計(jì)結(jié)果可信。

3 結(jié)束語(yǔ)

本文提出了一種新的風(fēng)估計(jì)與空速校準(zhǔn)的方法。該方法基于GPS接收機(jī)、大氣計(jì)算機(jī)、磁羅盤和垂直陀螺等傳感器實(shí)現(xiàn)。設(shè)計(jì)的方法利用了地速、風(fēng)速和空速之間的速度矢量三角形關(guān)系，并不依賴于無(wú)人機(jī)的氣動(dòng)力模型。仿真試驗(yàn)結(jié)果表明：推薦的DEKF估計(jì)方法，只需要無(wú)人機(jī)進(jìn)行半圈自主轉(zhuǎn)彎飛行，就可以獲得合理的風(fēng)速、風(fēng)向信息。估計(jì)的校準(zhǔn)系數(shù)能夠修正真空速到一個(gè)合理的水平。因此，本文提出的方法能夠減少低成本無(wú)人機(jī)空速傳感器的復(fù)雜校準(zhǔn)操作，提高真空速的測(cè)量精度。同時(shí)該方法對(duì)于后續(xù)無(wú)人機(jī)在風(fēng)場(chǎng)中實(shí)時(shí)航路規(guī)劃提供了研究基礎(chǔ)。

[1] Sohn S, Lee B, Kim J, et al. Vision-based real-time target localization for single-antenna GPS-guided UAV[J]. IEEE Transactions on Aerospace and Electronic Systems, 2008, 44: 1342-1355.

[2] 周偉靜, 沈懷榮. 一種基于小型無(wú)人機(jī)的風(fēng)場(chǎng)測(cè)量方法[J]. 測(cè)試技術(shù)學(xué)報(bào), 2009, 23(4): 297-302.

[3] Hollister WM, Bradford ER, Welch JD. Using Aircraft Radar Tracks to Estimate Winds Aloft[J]. MIT Lincoln Laboratory Journal, 1989, 2:555-565.

[4] 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-115.

[5] Pachter M, Ceccarell N, Chandler PR. Estimating MAV's Heading and the Wind Speed and Direction Using GPS, Inertial, and Air Speed Measurements[A]. Proceedings of AIAA Guidance, Navigation, and Control Conference and Exhibit[C]. Honolulu, Hawaii, 2008.

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

[7] Ceccarelli N, Enright JJ, Frazzoli E, et al. Micro UAV Path Planning for Reconnaissance in Wind[A]. American Control Conference[C]. New York, NY, USA, 2007:5310-5315.

[8] Osborne J, Rysdyk R. Waypoint Guidance for Small UAVs in Wind[C]. AIAA Infotech@Aerospace, 2005.

[9] Palanthandalam-Madapusi HJ, Girard A, Bernstein DS. Wind-field Reconstruction Using Flight Data using an unbiased minimum-variance unscented filter[J]. Transactions of the Institute of Measurement and Control, 2008, 33(6):718-733.

[10] Phillips WF. Mechanics of Flight[M]. Canada: John Wiley & Sons, Inc. 2004.

[11] Brendan M Q. A derivative-free implementation of the extended Kalman filter[J]. Automatica, 2006, 42(11): 1927-1934.

[12] Xuedong Wu, Zhihuan Song. Online Chaotic Time-series Prediction with the Derivative-Free Extended Kalman Filter[C]. Proceedings of the 7th World Congress on Intelligent Control and Automation, Chongqing, Chian, 2008.

[13] 高艷輝, 肖前貴, 徐義華, 等. 無(wú)人機(jī)數(shù)字仿真平臺(tái)的設(shè)計(jì)與實(shí)現(xiàn)[J]. 指揮控制與仿真, 2014, 36(1): 104-110.

[14] 沈懷榮, 邵瓊玲, 王盛軍, 等. 無(wú)人機(jī)氣象探測(cè)技術(shù)[M]. 北京: 清華大學(xué)出版社, 2010.

Wind Estimation for UAV Based on GPS, Magnetic Compass and Air Data Computer

Gao Yanhui, Li Zhiyu, Xiao Qiangui

(Key Laboratory of Unmanned Aerial Vehicle Technology ( Nanjing University of Aeronautics and Astronautics), Ministry of Industry and Information Technology, Nanjing 210016, China)

Aiming at the requirements of real-time path planning and autonomous capability for adapting to the environmental changes of Unmanned Aerial Vehicle (UAV), a new method for wind estimation and airspeed calibration is proposed. Based on the information of GPS receiver, air data computer and magnetic compass, the method is implemented. Aiming at constant wind mode, the wind speed and wind direction can be estimated using velocity triangle vector between ground speed, wind speed and airspeed. A Derivative-free extended Kalman filter (DEKF) is applied to estimate wind parameters and scaling factor of airspeed. Using a digital simulation platform for Unmanned Aerial Vehicle (UAV), an entire autonomic flight simulation were achieved in 2D wind field. Simulations results show that wind speed and wind direction can be accurately estimated both in straight line and turning segment during the path tracking by using the method.

wind estimation; airspeed calibration; UAV; DEKF

2016-12-31；

2016-02-14。

中國(guó)人民解放軍裝備部預(yù)研基金項(xiàng)目(51325010601)。

高艷輝(1976-)，男，河北辛集人，碩士，助理研究員，主要從事無(wú)人機(jī)飛控系統(tǒng)方向的研究。

1671-4598(2017)03-0231-03DOI：10.16526/j.cnki.11-4762/tp

N945.13;V

A