宋金來高延浩張 軍彌 曼(北京航天微系統(tǒng)研究所北京100094)

基于UKF自主定姿方法研究

宋金來，高延浩，張 軍，彌 曼
(北京航天微系統(tǒng)研究所，北京100094)

輕小型飛行器在飛行中衛(wèi)星導(dǎo)航失效時，余度控制回路要求導(dǎo)航系統(tǒng)具有自主確定姿態(tài)的能力。提出了基于IMU的輸出確定水平姿態(tài)的方法，并采用UKF實現(xiàn)飛行中的實時濾波估計。對某無人機(jī)實際飛行的MEMS型IMU數(shù)據(jù)進(jìn)行了仿真，結(jié)果表明該方法給出的姿態(tài)角信息滿足控制精度需求。將UKF與EKF濾波估計結(jié)果進(jìn)行比較，UKF更具有優(yōu)越性。

導(dǎo)航；自主定姿；MEMS；UKF

0 引言

在輕小型無人機(jī)、巡航彈等飛行器的控制系統(tǒng)研制中，MINS(MEMS型INS)＋BD2／GPS組合導(dǎo)航系統(tǒng)以體積小、功耗低、質(zhì)量輕、精度高等特點，成為導(dǎo)航系統(tǒng)的最佳選擇方案。在飛行控制中，由MINS＋BD2／GPS組合導(dǎo)航系統(tǒng)實時輸出速度、位置、姿態(tài)、加速度、角速度等信息，飛控系統(tǒng)利用這些信息通過控制回路，實現(xiàn)對飛行器的飛行穩(wěn)定與控制。

MINS＋BD2／GPS組合導(dǎo)航［1?2］方案，利用了二者的互補(bǔ)性，保證了飛行器長航時導(dǎo)航系統(tǒng)輸出的精度。然而，在衛(wèi)星導(dǎo)航失效狀態(tài)下，MINS輸出將無法長時間保持飛行控制系統(tǒng)所需要的信息精度。這樣，隨著速度、位置、姿態(tài)誤差的增加，UAV飛行將偏離規(guī)劃的航跡，也將導(dǎo)致失穩(wěn)情況的發(fā)生。為了確保飛行器飛行安全，當(dāng)此情況發(fā)生時，常用的飛行模式是：控制飛行器穩(wěn)定飛行，按指令返航或在目標(biāo)上空盤旋等待衛(wèi)星導(dǎo)航有效恢復(fù)，這是飛行故障自動診斷與處理的常用策略。對于中型無人機(jī)等飛行器而言，利用上垂直陀螺輸出的俯仰角、滾轉(zhuǎn)角姿態(tài)信息，可以通過俯仰、滾轉(zhuǎn)回路的控制實現(xiàn)飛行器的穩(wěn)定飛行。

本文研究了在衛(wèi)星導(dǎo)航失效時利用MEMS慣性儀表的輸出(加速度、角速度)實現(xiàn)自主定姿的方法。利用此方法實現(xiàn)垂直陀螺的功能，即實時獲得飛行器的兩個水平姿態(tài)信息，為飛行器的穩(wěn)定飛行提供保障。自主定姿問題的解決使輕小型無人機(jī)導(dǎo)航系統(tǒng)實現(xiàn)了自身有效的余度設(shè)計，增強(qiáng)了飛行器故障自動檢測與隔離及控制律重構(gòu)的功能。理論上，在飛行器處于巡航飛行時，利用加速度計的輸出，可以實時確定兩個水平姿態(tài)。但是，由于飛行中受發(fā)動機(jī)等因素的影響，MIMU (MEMS型IMU)的輸出信息被噪聲污染，直接解算姿態(tài)的結(jié)果將無法滿足飛行控制的使用要求。本文給出的自主定姿方法是借助飛行器姿態(tài)運動學(xué)方程，利用加速度計調(diào)平原理，通過濾波估計器，實時確定出滿足穩(wěn)定控制精度要求的兩個水平姿態(tài)角。由于姿態(tài)運動學(xué)方程以及量測方程均呈非線性，因此，采用了UKF濾波［3?4］估計方法。和常用的EKF濾波［5］方法相比較，UKF不需要求導(dǎo)計算Jacobian矩陣，穩(wěn)定性較強(qiáng)，二者整體運算量相差不多。這種定姿方法不存在誤差積累，其精度主要取決于加速度計的精度，模擬飛行仿真結(jié)果表明了此方法的有效性。

1 自主定姿模型

在衛(wèi)星導(dǎo)航失效狀態(tài)下，飛行器將按指令保持當(dāng)前高度或返航飛行，并保持姿態(tài)平穩(wěn)、避免機(jī)動。

一般地，小型飛行器巡航飛行速度不超過200km／h，飛行高度不超過5km，穩(wěn)定控制回路對姿態(tài)角要求為精度優(yōu)于0.5°。

設(shè)飛行器的俯仰角為θ，滾轉(zhuǎn)角為γ，偏航角為ψ。

1.1 姿態(tài)運動學(xué)方程

機(jī)體系(xb，yb，zb)與航跡系 (xg，yg，zg)的位置關(guān)系如圖1所示［6］。用表示機(jī)體系相對航跡系的角速度在機(jī)體系的分量構(gòu)成的列向量，有：

其中，、分別為地球的轉(zhuǎn)速、航跡系相對地面轉(zhuǎn)速在機(jī)體系的分量構(gòu)成的列向量，是機(jī)體系相對慣性系轉(zhuǎn)動角速度在機(jī)體系的分量構(gòu)成的列向量。

由圖1可知，和姿態(tài)角速率·θ、·γ、ψ·的關(guān)系為：

1.2 加速度與水平姿態(tài)的關(guān)系

飛行中，與加速度計輸出相關(guān)的比力方程［7］為：

將式(4)在機(jī)體系下展開并整理得到加速度與姿態(tài)的關(guān)系如下：

不難得知，在飛行器平穩(wěn)飛行時，3個加速度均為小量。假設(shè)水平飛行速度為200km／h，當(dāng)?shù)鼐暥葹?9°，由式(6)可計算得到3個加速度為0.005m／s2左右。

1.3 自主定姿模型

(1)狀態(tài)方程由式(1)可得：

由式(3)和式(8)，略去小量，得狀態(tài)方程：

寫成向量形式：

其中，

(2)量測方程

由式(5)和式(6)，略去小量，得量測方程為：

寫成向量的形式如下：

2 UKF濾波器設(shè)計

2.1 UT變換

假設(shè)一個非線性變換y＝f(x)。狀態(tài)變量x為n維隨機(jī)變量，并已知其均值x和方差Px，則可以通過下面的UT變換得到2n＋1個列向量χi(Sigma 點)和相應(yīng)的權(quán)值Wi來計算y的統(tǒng)計特性［8］：

這里λ＝α2（n＋κ）－n是一個比例因子，α決定x周圍Sigma點的分布狀態(tài)，通常選擇10－4≤α≤1。κ為常數(shù)，設(shè)置為0或3－n。參數(shù)β≥0，調(diào)節(jié)β可以提高方差的精度，β＝2最優(yōu)。i表示矩陣（n＋λ）Px的平方根的第i列或第i行。各個Sigma點的權(quán)值分別為：

其中，為均值的加權(quán)值，為方差的加權(quán)值。

為了得到輸出的均值和方差，將這些Sigma點分別通過非線性傳遞函數(shù)進(jìn)行非線性變換，得到變換后的各個Sigma點為：

則可以通過計算得到y(tǒng)的均值和方差為：

2.2 UKF算法實現(xiàn)

對于非線性動態(tài)系統(tǒng)，有：

式中，xk為nx維的系統(tǒng)狀態(tài)向量，yk為ny維的系統(tǒng)觀測向量。wk為系統(tǒng)噪聲，其協(xié)方差矩陣為Qk。νk為觀測噪聲，其協(xié)方差陣為Rk。假設(shè)wk、νk都為Gauss白噪聲，并且互不相關(guān)。

UKF濾波算法如下［9?10］：

(1)初始化

(2)UT變換

采用修正比例對稱分布采樣，得到k時刻狀態(tài)估計的Sigma點集｛χi，k｝，i＝1，2，…，L。其中，L＝2n＋1為采樣點數(shù)量。

(3)時間更新

(4)量測更新

3 仿真結(jié)果

利用某無人機(jī)上搭載飛行得到的MIMU數(shù)據(jù)進(jìn)行數(shù)值仿真。無人機(jī)共飛行了兩個多小時，飛行40min后給IMU上電，對IMU的輸出及組合導(dǎo)航結(jié)果進(jìn)行全過程記錄。選取IMU加電后130s～800s的數(shù)據(jù)進(jìn)行離線UKF自主確定姿態(tài)仿真。相應(yīng)的無人機(jī)飛行軌跡及飛行高度如圖2和圖3所示。

MIMU慣性儀表的輸出如圖4和圖5所示。

已知慣性儀表的精度為：陀螺零位漂移80(°)／h，加速度計零位偏置3×10－3g。

為了應(yīng)用UKF濾波估計方法，將式(10)進(jìn)行1階離散化處理，離散化步長及采樣間隔均為5ms。UKF濾波估計器初值的選?。籂顟B(tài)變量x的初值由無人機(jī)直接裝定，130s時無人機(jī)導(dǎo)航姿態(tài)為：

濾波器的其他參數(shù)為：α＝10－4，β＝2，κ＝0；Q＝diag ｛(20rad／h)2， (20rad／h)2｝，R＝ diag ｛(1.5m／s2)2，(1.5m／s2)2，(1.5m／s2)2｝P＝diag ｛(0.1rad)2，(0.1rad)2｝。

利用上述條件進(jìn)行UKF濾波估計仿真。以GPS／INS組合導(dǎo)航結(jié)果作為參照，將自主定姿結(jié)果與其比較，可知：UKF估計姿態(tài)的精度優(yōu)于0.5°，滿足余度穩(wěn)定控制回路的要求。俯仰角估計比較如圖6所示，滾轉(zhuǎn)角比較如圖7所示。

由比較曲線可見，全過程中UKF自主確定水平姿態(tài)優(yōu)于EKF濾波的精度。綜合考慮，UKF在狀態(tài)估計應(yīng)用中，有比EKF方法更大的優(yōu)越性。

4 結(jié)論

本文研究了飛行器自主確定姿態(tài)的方法。針對定姿數(shù)學(xué)模型的非線性，采用了UKF濾波估計方法。仿真結(jié)果表明，UKF方法實用、有效，比EKF方法具有更大的優(yōu)勢。該自主定姿方法為長航時飛行器自主故障診斷與處理提供了一種有效途徑，具有一定的實際應(yīng)用價值。

[1] 秦永元，張洪鉞，汪叔華.卡爾曼濾波與組合導(dǎo)航原理[M].西安:西北工業(yè)大學(xué)出版社，1998.QIN Yong?yuan，ZHANG Hong?yue，WANG Shu?hua. Kalman filter and integrated navigation principle[M]. Xi'an:Northwest Industry University Press，1998.

[2] 曲蘊杰，孫堯，莫宏偉.小型無人機(jī)組合導(dǎo)航系統(tǒng)的平方根UKF技術(shù)研究[J].彈箭與制導(dǎo)學(xué)報，2009，29 (1):91?94. QU Yun?jie，SUN Yao，MO Hong?wei.Squre?root un?scented Kalman filter of small UAV integrated navigation system[J].Journal of Projectiles，Rockets，Missiles and Guidance，2009，29(1):91?94.

[3] 徐慧娟，吳美平.EKF和UKF在INS／GPS組合導(dǎo)航中的應(yīng)用分析[J].航天控制，2006，24(6):7?16. XU Hui?juan，WU Mei?ping.Analysis of extended and unscented Kalman filtering for INS／GPS integration[J]. Aerospace Control，2006，24(6):7?16.

[4] 趙鶴，王喆垚.基于UKF的MEMS傳感器姿態(tài)測量系統(tǒng)[J].傳感技術(shù)學(xué)報，2011，24(5):642?646. ZHAO He，WANG Zhe?yao.MEMS sensors based attitude measurement system using UKF[J].Chinese Journal of Sensors and Actuators，2011，24(5):642?646.

[5] 范曉蓉，張海，范耀祖，等.微捷聯(lián)姿態(tài)系統(tǒng)的一種擴(kuò)展卡爾曼濾波方法[J].北京航空航天大學(xué)學(xué)報，2007，33(8):933?935. FAN Xiao?rong，ZHANG Hai，F(xiàn)AN Yao?zu，et al.Ex?tended Kalman filter method for micro?inertial strapdown attitude determination system[J].Journal of Beijing Uni?versity of Aeronautics and Astronautics，2007，33(8): 933?935.

[6] 趙漢元.飛行器再入動力學(xué)和制導(dǎo)[M].長沙:國防科技大學(xué)出版社，1997. ZHAO Han?yuan.Reentry dynamics and guidance[M]. Changsha:National University of Defense Technology Press，1997.

[7] 以光衢.慣性導(dǎo)航原理[M].北京:航空工業(yè)出版社，1987. Yi Guang?qu.Inertial navigation principle[M].Beijing: Aviation Industry Press，1987.

[8] Julier S J，Uhlmann J K，Durrant?Whyte H F.A new ap?proach for filtering nonlinear systems[C].Proceedings of the American Control Conference，Washington，1995: 1628?1632.

[9] De Marina H G，Pereda F J，Giron?Sierra J M，et al. UAV attitude estimation using unscented Kalman filter and TRIAD[J].IEEE Transactions on Industrial Electronics，2012，59(11):4465?4474.

[10] Gustafsson F，Hendeby G.Some relations between ex?tended and unscented Kalman filters [J].IEEE Transactions on SignalProcessing，2012，60 (2): 545?555.

Autonomous Attitude Determination for Small Aircraft Based on UKF

SONG Jin?lai，GAO Yan?hao，ZHANG Jun，MI Man
(Beijing Aerospace Institute of Microsystems，Beijing 100094)

To satisfy the requirement of redundance control loop，navigation system needs to independently determi?nate the attitude of small craft in case of GPS loss.A method is proposed to determinate attitude based on the accelerometer and gyroscope output，and UKF is used to realize filter estimation in real time during the flight.MEMS IMU data of UAV (unmanned aerial vehicle)flight is simulated and the simulation results show that the attitude information calculated in this method meets the requirement of control system.Supposing that attitude given by SINS／GPS integrated navigation system is of high accuracy，simulation results comparisons which are made between UKF and EKF show UKF is of better perform?ance.

navigation;autonomous attitude determination;micro electro mechanical system(MEMS);unscented Kalman filter(UKF)

V249.3

A

1674?5558(2017)01?01287

10.3969／j.issn.1674?5558.2017.02.004

宋金來，男，博士，研究員，碩士生導(dǎo)師，研究方向為飛行器導(dǎo)航、控制技術(shù)應(yīng)用。

2016?06?28