基于GNSS的飛行器姿態(tài)確定算法研究

劉帥趙國(guó)榮王元鑫曹唯一

（1.海軍航空工程學(xué)院研究生管理大隊(duì)煙臺(tái)264001）（2.海軍航空工程學(xué)院控制工程系煙臺(tái)264001）（3.海軍航空工程學(xué)院學(xué)員一旅煙臺(tái)264001）

基于GNSS的飛行器姿態(tài)確定算法研究

劉帥1趙國(guó)榮2王元鑫1曹唯一3

（1.海軍航空工程學(xué)院研究生管理大隊(duì)煙臺(tái)264001）（2.海軍航空工程學(xué)院控制工程系煙臺(tái)264001）（3.海軍航空工程學(xué)院學(xué)員一旅煙臺(tái)264001）

論文對(duì)基于GNSS的飛行器姿態(tài)測(cè)量進(jìn)行研究。考慮姿態(tài)測(cè)量系統(tǒng)基線長(zhǎng)度固定的特點(diǎn)，建立了單基線測(cè)量的載波相位差分姿態(tài)方程。在解算姿態(tài)方程時(shí)，為克服可能出現(xiàn)的載波相位觀測(cè)值異常情況，提出了基于抗差卡爾曼濾波的飛行器姿態(tài)解算算法。仿真結(jié)果表明，該算法具有比最小二乘法更高的姿態(tài)解算精度和穩(wěn)定性。

姿態(tài)測(cè)量；載波相位單差方程；抗差卡爾曼濾波

Class NumberTP301

1 引言

飛行器的姿態(tài)測(cè)量系統(tǒng)是其飛行控制系統(tǒng)的重要組成部分，姿態(tài)測(cè)量精度的提高對(duì)提高控制系統(tǒng)的穩(wěn)定性和準(zhǔn)確性具有重要意義。傳統(tǒng)的姿態(tài)測(cè)量系統(tǒng)主要包括光學(xué)測(cè)姿系統(tǒng)、地磁導(dǎo)航系統(tǒng)、慣性導(dǎo)航系統(tǒng)等，都有其各自的局限性［1～2］。基于GNSS（全球?qū)Ш叫l(wèi)星系統(tǒng)）的姿態(tài)測(cè)量是近年來(lái)發(fā)展迅速的研究領(lǐng)域，GNSS測(cè)姿具有適用性強(qiáng)、性?xún)r(jià)比高、無(wú)累積誤差等優(yōu)點(diǎn)，既可以作為獨(dú)立的導(dǎo)航系統(tǒng)應(yīng)用到飛行器的導(dǎo)航中，也可以與慣性導(dǎo)航系統(tǒng)等組合，進(jìn)行組合導(dǎo)航的應(yīng)用，具有廣闊的應(yīng)用前景。

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

2 載波相位差分測(cè)姿模型的建立

高精度的GNSS姿態(tài)測(cè)量，需要借助載波相位差分技術(shù)來(lái)實(shí)現(xiàn)。圖1為單差載波相位示意圖。

圖1 單差載波相位示意圖

假設(shè)某一時(shí)刻A、B兩個(gè)天線同時(shí)接受衛(wèi)星j的信號(hào)，在基線較短的情況下，可認(rèn)為兩天線的對(duì)流層延時(shí)等效距離誤差和電離層延時(shí)等效距離誤差相等，而接收機(jī)鐘差引起的誤差也可以通過(guò)共用接收機(jī)等方式加以消除。因此以A、B兩個(gè)天線為端點(diǎn)的基線關(guān)于衛(wèi)星j的單差載波相位方程可寫(xiě)成下列形式：

根據(jù)圖1中的幾何關(guān)系有：

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

假設(shè)衛(wèi)星j的方向角和高度角分別為βj和 αj，載體的方位角和撫養(yǎng)角分別為ψ和θ，則有下式成立

將式（2）、（3）帶入式（1）可得單差觀測(cè)方程的表達(dá)式：

假設(shè)姿態(tài)角的初值為(ψ0，θ0)，應(yīng)用泰勒展開(kāi)對(duì)式（4）進(jìn)行線性化，有［8］：

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

式中：

3 基于抗差卡爾曼濾波的姿態(tài)解算

3.1 系統(tǒng)狀態(tài)方程和觀測(cè)方程的建立

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

結(jié)合式（6），可以得到如下的觀測(cè)方程：

式中，Zk為觀測(cè)矩陣，Vk為測(cè)量噪聲序列，

假設(shè)采用常速度模型，則狀態(tài)轉(zhuǎn)移方程為

其中，Γk，k-1為系統(tǒng)噪聲驅(qū)動(dòng)陣，Wk為系統(tǒng)噪聲序列，Φk，k-1為k-1時(shí)刻到k時(shí)刻的狀態(tài)轉(zhuǎn)移矩陣，且有：

式（7）和式（8）共同組成了GNSS姿態(tài)測(cè)量系統(tǒng)的濾波方程：

一般情況下，將測(cè)量噪聲和系統(tǒng)噪聲視為不相關(guān)的高斯白噪聲，即有以下關(guān)系成立：

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

3.2 姿態(tài)解算過(guò)程

濾波方程建立后，若使用經(jīng)典卡爾曼濾波算法進(jìn)行姿態(tài)解算，算法描述為［9］

狀態(tài)一步預(yù)測(cè)：

狀態(tài)估計(jì)：

一步預(yù)測(cè)誤差方差陣：

估計(jì)誤差方差陣：

在給定初始值X?0和P0的情況下，可以通過(guò)上述公式遞推得到各個(gè)時(shí)刻的解算結(jié)果。

經(jīng)典的卡爾曼濾波在處理只含有偶然誤差的問(wèn)題時(shí)，能夠取得很好的效果，但對(duì)于存在粗差的情況，則可能出現(xiàn)結(jié)果偏離很大的情況。在基于GNSS的姿態(tài)測(cè)量中，在特定的時(shí)間、特定的地點(diǎn)，可能會(huì)由于多路徑效應(yīng)等因素導(dǎo)致觀測(cè)值異常。因此，需要尋找一種能夠有效規(guī)避異常觀測(cè)值的方

濾波增益矩陣：法，抗差卡爾曼濾波算法采用抗差估計(jì)原理，通過(guò)引入降權(quán)因子改變了濾波增益矩陣，較好地解決了這一問(wèn)題。

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

式（15）中的Rˉk為觀測(cè)序列Zk的抗差等價(jià)協(xié)方差，假設(shè)觀測(cè)序列的權(quán)矩陣為B=diag(b1，b2，…，bn)，則Rˉk可以通過(guò)以下方法求得：

式中，c0和c1為常數(shù)，且c0取值范圍一般為1.5～2.0，c1的取值范圍一般為3.0～8.5，||v?i為Zk的標(biāo)準(zhǔn)化殘差［10］。

根據(jù)調(diào)整后的濾波增益矩陣抗，可得抗差卡爾曼濾波算法的估計(jì)誤差方差陣：

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

4 仿真及結(jié)果分析

使用單基線GNSS姿態(tài)測(cè)量系統(tǒng)對(duì)本文提出的算法進(jìn)行仿真驗(yàn)證，基線長(zhǎng)度設(shè)為3m，采集200個(gè)測(cè)量歷元的載波相位觀測(cè)數(shù)據(jù)，分別用最小二乘法和本文提出的抗差卡爾曼濾波算法進(jìn)行姿態(tài)解算，解算結(jié)果如圖3和圖4所示。

圖3 俯仰角測(cè)量結(jié)果

圖4 偏航角測(cè)量結(jié)果

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

表1 兩種方法的姿態(tài)解算效果匯總表

5 結(jié)語(yǔ)

本文針對(duì)基于GNSS的飛行器姿態(tài)測(cè)量問(wèn)題，結(jié)合載體的運(yùn)動(dòng)特點(diǎn)建立系統(tǒng)狀態(tài)方程和量測(cè)方程，采用抗差卡爾曼濾波算法解決測(cè)量過(guò)程中可能出現(xiàn)的載波相位觀測(cè)值異常問(wèn)題，仿真結(jié)果表明算法具有較高的解算精度和可靠性，具有較好的工程應(yīng)用的前景。

［1］許江寧，朱濤，卞鴻巍.GPS姿態(tài)測(cè)量技術(shù)綜述［J］.海軍工程大學(xué)學(xué)報(bào)，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］李躍，邱致和.導(dǎo)航與定位［M］.北京：國(guó)防工業(yè)出版社，2008.

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

［3］T.Scatahlini，F(xiàn).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載波相位多天線實(shí)時(shí)測(cè)姿系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)［J］.測(cè)繪科學(xué)技術(shù)學(xué)報(bào)，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的航天器姿態(tài)確定研究［D］.哈爾濱：哈爾濱工業(yè)大學(xué)，2009.

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

［7］王冰，隋立芬，張清華.利用GPS解算載體測(cè)姿的算法研究［J］.武漢大學(xué)學(xué)報(bào)（信息科學(xué)版），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］王永泉.長(zhǎng)航時(shí)高動(dòng)態(tài)條件下GPS/GLONASS姿態(tài)測(cè)量研究［D］.上海：上海交通大學(xué)，2008.

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

［9］卞鴻巍，李安，覃方君，等.現(xiàn)代信息融合技術(shù)在組合導(dǎo)航中的應(yīng)用［M］.北京：國(guó)防工業(yè)出版社，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］閆欣.基于抗差估計(jì)的GPS/MIMU組合導(dǎo)航濾波算法研究［D］.哈爾濱：哈爾濱工程大學(xué)，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日

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