基于GNSS的飛行器姿態確定算法研究

劉帥趙國榮王元鑫曹唯一

（1.海軍航空工程學院研究生管理大隊煙臺264001）（2.海軍航空工程學院控制工程系煙臺264001）（3.海軍航空工程學院學員一旅煙臺264001）

基于GNSS的飛行器姿態確定算法研究

劉帥1趙國榮2王元鑫1曹唯一3

（1.海軍航空工程學院研究生管理大隊煙臺264001）（2.海軍航空工程學院控制工程系煙臺264001）（3.海軍航空工程學院學員一旅煙臺264001）

論文對基于GNSS的飛行器姿態測量進行研究。考慮姿態測量系統基線長度固定的特點，建立了單基線測量的載波相位差分姿態方程。在解算姿態方程時，為克服可能出現的載波相位觀測值異常情況，提出了基于抗差卡爾曼濾波的飛行器姿態解算算法。仿真結果表明，該算法具有比最小二乘法更高的姿態解算精度和穩定性。

姿態測量；載波相位單差方程；抗差卡爾曼濾波

Class NumberTP301

1 引言

飛行器的姿態測量系統是其飛行控制系統的重要組成部分，姿態測量精度的提高對提高控制系統的穩定性和準確性具有重要意義。傳統的姿態測量系統主要包括光學測姿系統、地磁導航系統、慣性導航系統等，都有其各自的局限性［1～2］。基于GNSS（全球導航衛星系統）的姿態測量是近年來發展迅速的研究領域，GNSS測姿具有適用性強、性價比高、無累積誤差等優點，既可以作為獨立的導航系統應用到飛行器的導航中，也可以與慣性導航系統等組合，進行組合導航的應用，具有廣闊的應用前景。

現有的GNSS姿態解算算法主要可以分為三類：直接計算法、最小二乘法和卡爾曼濾波法。直接計算法是一種經典的方法，算法運算簡單，已被廣泛地應用于GNSS姿態測量系統的設計與研制中［3～4］，但該方法在處理多基線系統時不能充分利用多基線提供的冗余信息，且解算結果容易受觀測噪聲影響。最小二乘法是一種經典的最優估計方法，在利用載波相位信息進行姿態解算時，如果整周模糊度已確定，則觀測方程的數量往往大于未知數個數，此時運用最小二乘法可以很好地利用所有觀測信息，盡可能地減少測量噪聲的影響，得到最優估計［5～7］。卡爾曼濾波法是20世紀最偉大的學術成果之一，已經被廣發地應用于導航、定位、信號處理、自動控制等多個領域，基于卡爾曼濾波的姿態解算可以有效地利用載體的運動特性，實現姿態的最優估計，但其結論是在系統噪聲統計特性已知的前提下得到的，因而很難有效解決測量過程中可能出現的觀測值異常問題。本文將在研究GNSS姿態測量原理的基礎上，對傳統的卡爾曼濾波算法進行改進，通過引入抗差因子，建立抗差卡爾曼濾波方程對狀態向量進行估計，實現動態條件下的載體姿態快速解算。

2 載波相位差分測姿模型的建立

高精度的GNSS姿態測量，需要借助載波相位差分技術來實現。圖1為單差載波相位示意圖。

圖1 單差載波相位示意圖

假設某一時刻A、B兩個天線同時接受衛星j的信號，在基線較短的情況下，可認為兩天線的對流層延時等效距離誤差和電離層延時等效距離誤差相等，而接收機鐘差引起的誤差也可以通過共用接收機等方式加以消除。因此以A、B兩個天線為端點的基線關于衛星j的單差載波相位方程可寫成下列形式：

根據圖1中的幾何關系有：

式中：b為由主天線相位中心指向輔助天線相位中心的基線矢量；sj為衛星Sj的視線矢量。

假設衛星j的方向角和高度角分別為βj和 αj，載體的方位角和撫養角分別為ψ和θ，則有下式成立

將式（2）、（3）帶入式（1）可得單差觀測方程的表達式：

假設姿態角的初值為(ψ0，θ0)，應用泰勒展開對式（4）進行線性化，有［8］：

式（5）可以用下列矩陣形式表示：

式中：

3 基于抗差卡爾曼濾波的姿態解算

3.1 系統狀態方程和觀測方程的建立

用ψk、θk表示在第k個歷元的偏航角和俯仰角，用ψ?k、θ?k表示偏航角和俯仰角的變化率，則可構造狀態向量矩陣Xk=(ψk，θk，ψ?k，θ?k)T。

結合式（6），可以得到如下的觀測方程：

式中，Zk為觀測矩陣，Vk為測量噪聲序列，

假設采用常速度模型，則狀態轉移方程為

其中，Γk，k-1為系統噪聲驅動陣，Wk為系統噪聲序列，Φk，k-1為k-1時刻到k時刻的狀態轉移矩陣，且有：

式（7）和式（8）共同組成了GNSS姿態測量系統的濾波方程：

一般情況下，將測量噪聲和系統噪聲視為不相關的高斯白噪聲，即有以下關系成立：

上式中，Qk代表系統噪聲序列的方差陣，假設為非負定陣；Rk代表測量噪聲序列的方差陣，假設為正定陣［9］。δ為狄拉克函數，k=j時，δkj=1，k≠j時，δkj=0。

3.2 姿態解算過程

濾波方程建立后，若使用經典卡爾曼濾波算法進行姿態解算，算法描述為［9］

狀態一步預測：

狀態估計：

一步預測誤差方差陣：

估計誤差方差陣：

在給定初始值X?0和P0的情況下，可以通過上述公式遞推得到各個時刻的解算結果。

經典的卡爾曼濾波在處理只含有偶然誤差的問題時，能夠取得很好的效果，但對于存在粗差的情況，則可能出現結果偏離很大的情況。在基于GNSS的姿態測量中，在特定的時間、特定的地點，可能會由于多路徑效應等因素導致觀測值異常。因此，需要尋找一種能夠有效規避異常觀測值的方

濾波增益矩陣：法，抗差卡爾曼濾波算法采用抗差估計原理，通過引入降權因子改變了濾波增益矩陣，較好地解決了這一問題。

抗差卡爾曼濾波算法的濾波增益矩陣為

式（15）中的Rˉk為觀測序列Zk的抗差等價協方差，假設觀測序列的權矩陣為B=diag(b1，b2，…，bn)，則Rˉk可以通過以下方法求得：

式中，c0和c1為常數，且c0取值范圍一般為1.5～2.0，c1的取值范圍一般為3.0～8.5，||v?i為Zk的標準化殘差［10］。

根據調整后的濾波增益矩陣抗，可得抗差卡爾曼濾波算法的估計誤差方差陣：

抗差卡爾曼狀態估計：聯立式（10）、（12）、（15）、（19）和（20），便得到了基于抗差卡爾曼濾波的遞推方程，可根據初始值進行各個歷元的姿態解算。

4 仿真及結果分析

使用單基線GNSS姿態測量系統對本文提出的算法進行仿真驗證，基線長度設為3m，采集200個測量歷元的載波相位觀測數據，分別用最小二乘法和本文提出的抗差卡爾曼濾波算法進行姿態解算，解算結果如圖3和圖4所示。

圖3 俯仰角測量結果

圖4 偏航角測量結果

圖中淺色線表示最小二乘法的姿態解算結果，深色線表示抗差卡爾曼濾波法的解算結果。從圖中可以明顯看出抗差卡爾曼濾波算法的解算效果要好于最小二乘法。為了使結果更為直觀，對測量結果的最大值、最小值、方差進行統計，結果如表1所示。

表1 兩種方法的姿態解算效果匯總表

5 結語

本文針對基于GNSS的飛行器姿態測量問題，結合載體的運動特點建立系統狀態方程和量測方程，采用抗差卡爾曼濾波算法解決測量過程中可能出現的載波相位觀測值異常問題，仿真結果表明算法具有較高的解算精度和可靠性，具有較好的工程應用的前景。

［1］許江寧，朱濤，卞鴻巍.GPS姿態測量技術綜述［J］.海軍工程大學學報，2003，15（3）：19-20.

XU Jiangning，ZHU Tao，BIAN Hongwei.Review on GPS attitude determination［J］.Journal of Naval University of Engineering，2003，15（3）：19-20.

［2］李躍，邱致和.導航與定位［M］.北京：國防工業出版社，2008.

LI Yue，QIU Zhihe.Navigation and positioning［M］.Beijing：National Defense Industry Press，2008.

［3］T.Scatahlini，F.Pagola，J.Cogo and J.G Garcia.Attitude estimation using GPS carrier phase single differences［J］.IEEE LATIN AMERICA TRANSACTIONS，2014，12（5）：847-853.

［4］鄭坤，董緒榮，劉亞濤.GNSS載波相位多天線實時測姿系統的設計與實現［J］.測繪科學技術學報，2014，31（2）：136-140.

ZHENG Kun，DONG Xurong，LIU Yatao.Attitude Determination with A GNSS Multiple-Antenna System［J］.Journal of Geomatics Science and Technology，2014，31（2）：136-140.

［5］Teunissen P J G.A general multivariate formulation of the multi-antenna GNSS attitude determination problem［J］. Artificial Satellites，2007，42（2）：91-111.

［6］張健.基于GPS的航天器姿態確定研究［D］.哈爾濱：哈爾濱工業大學，2009.

ZHANG Jian.Study on attitude determination of spacecraft based on GPS［D］.Harbin：Harbin Institute of Technology，2009.

［7］王冰，隋立芬，張清華.利用GPS解算載體測姿的算法研究［J］.武漢大學學報（信息科學版），2013，38（12）：1392-1396.

WANG Bing，SUI Lifen，ZHANG Qinghua.Research on attitude determination algorithm using GPS［J］.Geomatics and Information Science of Wuhan University，2013，38（12）：1392-1396.

［8］王永泉.長航時高動態條件下GPS/GLONASS姿態測量研究［D］.上海：上海交通大學，2008.

WANG Yong-quan.GPS/GLONASS attitude determination research under long endurance and High Dynamic Conditions［D］.Shanghai：Shanghai Jiao Tong University，2008.

［9］卞鴻巍，李安，覃方君，等.現代信息融合技術在組合導航中的應用［M］.北京：國防工業出版社，2010：70-77.

BIAN Hongwei，LI An，TAN Fangjun.The use of modern information fusion technology in integrated navigation［M］.Beijing：National Defense Industry Press，2010：70-77.

［10］閆欣.基于抗差估計的GPS/MIMU組合導航濾波算法研究［D］.哈爾濱：哈爾濱工程大學，2010.

YAN Xin.Research on the filter algorithm of GPS/MIMU integrated navigation based on robust estimation［D］. Harbin：Harbin Engineering University，2010.

Aircraft Attitude Determination Method Based on GNSS

LIU Shuai1ZHAO Guorong2WANG Yuanxin1CAO Weiyi2
（1.Graduate Students'Brigade，Naval Aeronautical and Astronautical University，Yantai264001）（2.Department of Control Engineering，Naval Aeronautical and Astronautical University，Yantai264001）（3.Students'Brigade one，Naval Aeronautical and Astronautical University，Yantai264001）

This paper mainly studies on the attitude determination of aircraft based on GNSS.Considering the fact that the baseline of attitude determination system is fixed，the carrier phase single difference equation is build to compute the attitude.A method based on the robust kalman filtering is presented to solve the problem of gross error in measurement data.The simulation result shows that the precision and reliability of the presented method is better than the least squares method.

attitude determination，carrier phase single difference equation，robust kalman filtering

TP301

10.3969/j.issn.1672-9722.2017.06.014

2016年12月18日，

2017年1月23日

劉帥，男，碩士，研究方向：飛行器綜合導航。趙國榮，男，博士，教授，研究方向：導航制導與控制。王元鑫，男，碩士，研究方向：飛行器綜合導航。曹唯一，男，研究方向：飛行器導航與控制。