基于UKF自主定姿方法研究

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

基于UKF自主定姿方法研究

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

輕小型飛行器在飛行中衛星導航失效時，余度控制回路要求導航系統具有自主確定姿態的能力。提出了基于IMU的輸出確定水平姿態的方法，并采用UKF實現飛行中的實時濾波估計。對某無人機實際飛行的MEMS型IMU數據進行了仿真，結果表明該方法給出的姿態角信息滿足控制精度需求。將UKF與EKF濾波估計結果進行比較，UKF更具有優越性。

導航；自主定姿；MEMS；UKF

0 引言

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

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

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

1 自主定姿模型

在衛星導航失效狀態下，飛行器將按指令保持當前高度或返航飛行，并保持姿態平穩、避免機動。

一般地，小型飛行器巡航飛行速度不超過200km／h，飛行高度不超過5km，穩定控制回路對姿態角要求為精度優于0.5°。

設飛行器的俯仰角為θ，滾轉角為γ，偏航角為ψ。

1.1 姿態運動學方程

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

其中，、分別為地球的轉速、航跡系相對地面轉速在機體系的分量構成的列向量，是機體系相對慣性系轉動角速度在機體系的分量構成的列向量。

由圖1可知，和姿態角速率·θ、·γ、ψ·的關系為：

1.2 加速度與水平姿態的關系

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

將式(4)在機體系下展開并整理得到加速度與姿態的關系如下：

不難得知，在飛行器平穩飛行時，3個加速度均為小量。假設水平飛行速度為200km／h，當地緯度為39°，由式(6)可計算得到3個加速度為0.005m／s2左右。

1.3 自主定姿模型

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

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

寫成向量形式：

其中，

(2)量測方程

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

寫成向量的形式如下：

2 UKF濾波器設計

2.1 UT變換

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

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

其中，為均值的加權值，為方差的加權值。

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

則可以通過計算得到y的均值和方差為：

2.2 UKF算法實現

對于非線性動態系統，有：

式中，xk為nx維的系統狀態向量，yk為ny維的系統觀測向量。wk為系統噪聲，其協方差矩陣為Qk。νk為觀測噪聲，其協方差陣為Rk。假設wk、νk都為Gauss白噪聲，并且互不相關。

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

(1)初始化

(2)UT變換

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

(3)時間更新

(4)量測更新

3 仿真結果

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

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

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

為了應用UKF濾波估計方法，將式(10)進行1階離散化處理，離散化步長及采樣間隔均為5ms。UKF濾波估計器初值的選取：狀態變量x的初值由無人機直接裝定，130s時無人機導航姿態為：

濾波器的其他參數為：α＝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｝。

利用上述條件進行UKF濾波估計仿真。以GPS／INS組合導航結果作為參照，將自主定姿結果與其比較，可知：UKF估計姿態的精度優于0.5°，滿足余度穩定控制回路的要求。俯仰角估計比較如圖6所示，滾轉角比較如圖7所示。

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

4 結論

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

[1] 秦永元，張洪鉞，汪叔華.卡爾曼濾波與組合導航原理[M].西安:西北工業大學出版社，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] 曲蘊杰，孫堯，莫宏偉.小型無人機組合導航系統的平方根UKF技術研究[J].彈箭與制導學報，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組合導航中的應用分析[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傳感器姿態測量系統[J].傳感技術學報，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] 范曉蓉，張海，范耀祖，等.微捷聯姿態系統的一種擴展卡爾曼濾波方法[J].北京航空航天大學學報，2007，33(8):933?935. FAN Xiao?rong，ZHANG Hai，FAN 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] 趙漢元.飛行器再入動力學和制導[M].長沙:國防科技大學出版社，1997. ZHAO Han?yuan.Reentry dynamics and guidance[M]. Changsha:National University of Defense Technology Press，1997.

[7] 以光衢.慣性導航原理[M].北京:航空工業出版社，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

宋金來，男，博士，研究員，碩士生導師，研究方向為飛行器導航、控制技術應用。

2016?06?28