自適應抗差Kalman濾波在多天線原始觀測值瞬時姿態(tài)確定中的應用

甘 雨，隋立芬，劉長建，董 明

信息工程大學地理空間信息學院，河南 鄭州450000

1 引 言

GNSS測姿具有成本低、初始化快、誤差相對穩(wěn)定等特點，已成為GNSS應用研究的熱點。目前在雙天線單瞬時定向方面的研究較為深入，一般通過約束條件輔助單基線求解，再基于基線結(jié)果計算姿態(tài)，這一領域已有豐富成果［1－4］。在多基線方面，有學者研究了整體解算方法［5－6］，但是需先解基線再解姿態(tài)，無法直接利用原始觀測值測姿。文獻［7］提出在多基線解算過程中固定模糊度，再將模糊度回代求解四元數(shù)參數(shù)，以充分利用觀測信息，但本質(zhì)上仍屬基線解算模型。文獻［8］以方向余弦陣為姿態(tài)參數(shù)，利用原始觀測值計算方向余弦和模糊度浮點解，將方向余弦的正交約束引入到模糊度搜索中，利用MCLAMBDA方法（multivariate constrained LAMBDA）進行搜索。由于約束信息的引入，使得模糊度的搜索空間不再為橢球體，整周模糊度的搜索負擔遠高于一般的LAMBDA方法［9］，且方向余弦陣含9個參數(shù)，解算效率偏低，影響實時應用。目前未形成可靠的由載波相位觀測解算歐拉角或四元數(shù)固定解的理論。

現(xiàn)有測姿方法使用最小二乘進行單歷元解算以避免周跳探測問題，歷史信息沒有得到充分利用。使用自適應選權濾波［10］、自適應抗差濾波［11－12］等方法均能高效地平衡歷史信息和當前觀測，其中自適應抗差濾波具有抵御粗差干擾的能力，在城區(qū)及海上姿態(tài)測量等應用中能發(fā)揮優(yōu)勢。實際上，只要不使用模糊度歷史信息，使用濾波的方式也無須探測周跳，可實現(xiàn)瞬時定姿。

本文推導了基于失準角及乘性誤差四元數(shù)的載波相位觀測模型，分別建立有外部角速度傳感器和無外部傳感器輔助下姿態(tài)參數(shù)估計的狀態(tài)模型；使用自適應抗差濾波估計姿態(tài)，借鑒分類自適應因子的思想，對模糊度參數(shù)和姿態(tài)誤差參數(shù)分別自適應，通過模糊度自適應因子控制模糊度先驗值的信任度，若選擇極小的自適應因子，可在濾波框架內(nèi)免去探測周跳過程；根據(jù)Ratio值確定姿態(tài)參數(shù)的自適應因子。自適應抗差濾波能夠充分利用約束信息和歷史信息，將其融合在浮點解計算過程中，極大提高模糊度浮點解精度及其協(xié)方差的結(jié)構，在此基礎上使用LAMBDA方法即能快速搜索出固定解。采用實測艦載GNSS 3天線測姿算例證明了本文方法的有效性。

2 基于姿態(tài)誤差和模糊度參數(shù)的載波相位觀測模型

設在主天線i和天線j同步觀測到衛(wèi)星p、q，可組站星雙差觀測方程

將式（1）在基線初值展開并忽略大氣折射延遲項，有

式中，bij為基線參數(shù)和為方向余弦矢量；

整理可得基線觀測模型為

由于GNSS觀測方程為非線性，姿態(tài)參數(shù)之間的關系也為非線性，因此，姿態(tài)估計中一般計算名義姿態(tài)中包含的姿態(tài)誤差，再對名義姿態(tài)進行誤差修正。這里分別從失準角和乘性四元數(shù)這兩種姿態(tài)誤差參數(shù)的角度推導相應的載波相位觀測方程。

設載體坐標系為b系，當?shù)厮阶鴺讼禐閚系，載體姿態(tài)可用姿態(tài)矩陣表示。設名義當?shù)厮阶鴺讼禐閚′系，定義n系到n′系的失準角為φ，它表示實際當?shù)厮较祅和名義當?shù)厮较祅′之間的旋轉(zhuǎn)矢量，可理解為名義系n′的誤差。若失準角較小，其近似等于俯仰、橫滾和航向的歐拉角誤差，這里為了保證嚴密性，依舊稱之失準角而非歐拉角誤差。n和n′之間的關系可用式（6）的姿態(tài)矩陣的形式表示［13］

式中，（φ×）為φ的叉乘矩陣，為一反對稱陣。

失準角、名義姿態(tài)陣、姿態(tài)陣之間的關系如式（7）所示

式（3）中的bij為地固系e系中的基線，加上上標為，由坐標旋轉(zhuǎn)關系，有

將式（8）代入式（3），得

整理可得基于失準角參數(shù)的載波相位觀測方程

對于天線j1和主天線i，按照式（11）可由所有觀測衛(wèi)星得到矩陣形式的觀測方程為

若有天線（j1，j2，…，jm），則基于失準角和模糊度參數(shù)（略去雙差算子）的載波相位觀測模型為

前面推導了基于失準角表示姿態(tài)誤差的觀測方程，也可采用乘性誤差四元數(shù)的形式。在誤差角為小量下，有［13］

僅需將式（12）稍作改動即可得到基于乘性誤差四元數(shù)的載波相位觀測方程

若GNSS姿態(tài)測量系統(tǒng)含m條基線，使用基線解算模型需解算3m個基線參數(shù)，而使用基于失準角或誤差四元數(shù)的觀測值模型僅解算3個姿態(tài)參數(shù)，效率更高。

3 姿態(tài)估計的狀態(tài)模型

四元數(shù)微分方程為［7］

由姿態(tài)矩陣與姿態(tài)四元數(shù)之間的關系可得乘性誤差四元數(shù)δQ滿足

將式（18）、式（19）代入式（21）

根據(jù)四元數(shù)乘法準則，得

由式（16），得

式（25）、式（26）的右端第2項即為狀態(tài)噪聲。

若有外部角速度傳感器如陀螺儀的輔助，則可直接使用式（25）或式（26）建立狀態(tài)方程，否則，無法獲得角速度，此時將式（26）改化為

將ω擴展為狀態(tài)參數(shù)并用隨機游走模型表示，納入式（27）并離散化

即為無外部角速度傳感器輔助下的姿態(tài)估計狀態(tài)模型。

4 自適應抗差濾波的應用及自適應因子的確定

綜合式（13）、式（28）可得基于載波相位觀測值估計姿態(tài)的濾波模型為

式（30）中的A與式（13）略有不同，為

使用姿態(tài)誤差而非姿態(tài)本身作為濾波中的姿態(tài)參數(shù)，可以減小觀測方程線性化引起的模型誤差。設計算的名義姿態(tài)陣，濾波估計得到的失準角，代入式（6）、式（7），即可對名義姿態(tài)進行修正，修正后的即為濾波解。在k時刻，以k－1時刻的濾波解作為名義解。若k時刻成功固定模糊度即得到失準角固定解時，應在完成姿態(tài)修正后將失準角置零后傳遞至下一歷元。

若前一時刻姿態(tài)估計精度高，則本時刻名義解較為可靠，應信任狀態(tài)預測信息，若名義解精度不高，則使用自適應因子減小狀態(tài)信息的貢獻。

姿態(tài)誤差、角速度、模糊度屬不同類型的參數(shù)，很難采用統(tǒng)一的自適應因子進行調(diào)節(jié)，分類因子自適應濾波可對不同的分量選取不同的自適應因子。分類因子自適應抗差濾波解為［14－15］

式中，為抗差等價權矩陣，可由Huber函數(shù)確定［15］

式中，為標準化殘差；c為常量，可取1.0～1.5。

為狀態(tài)預測的自適應權矩陣，有

式中，αk為自適應因子矩陣，即

由于ω為間接觀測量，這里不作自適應，有

αN可調(diào)整模糊度先驗信息的權重，若進行周跳探測得到各衛(wèi)星中最大周跳估值為dN，顧及周跳探測量自身的噪聲，可選取自適應因子為

式中，σˉN為模糊度預測標準差，主要由設定的模糊度狀態(tài)噪聲決定；σDT為周跳探測量的中誤差，周跳估值越大，模糊度自適應因子越小，一般αN＜1。

若要免去周跳探測步驟以便瞬時解算，則可取αN為一極小值，如αN＝1.0e－8，此時，模糊度先驗信息的權重降至極小，不受周跳干擾。

自適應濾波常用的誤差判別統(tǒng)計量包括狀態(tài)不符值或預測殘差等，GNSS定姿中，在模糊度固定前，無法獲得足夠精度的上述統(tǒng)計量?？紤]姿態(tài)誤差的預測性能本質(zhì)上由名義解即前一歷元姿態(tài)濾波解的精度決定，其對應的Ratio值能夠在一定程度上反映其可靠程度，αφ由三段函數(shù)表示

式中，Rat為Ratio值，一般取c0＝2、c1＝3。

5 計算與比較

本文算例使用實測艦載GPS 3個天線姿態(tài)測量雙頻數(shù)據(jù)進行計算和比較分析，3個天線構成兩條獨立基線，基線的載體系坐標分別為［0.0 15.0 0.0］T、［14.5 25.1 0.0］T。由于海面環(huán)境復雜，加之天線間距離較遠，雙差后部分觀測值仍受到較為嚴重的多路徑效應影響，以單個歷元而言它們表現(xiàn)出粗差性質(zhì)。姿態(tài)解算中模糊度固定解的搜索均采用LAMBDA方法，若Ratio值超過3，則視相應歷元成功固定。設計以下4種方案計算浮點解進行對比。

方案1：單歷元最小二乘；

方案2：單歷元抗差估計；

方案3：自適應濾波；

方案4：自適應抗差濾波。

方案3和方案4中，取模糊度自適應因子αN＝1.0e－8，即完全不使用模糊度先驗信息，以和方案1、方案2形成對照，因此4個方案均無須主探測周跳。方案3中僅使用了自適應因子，未使用抗差等價權控制粗差干擾。共有1800個觀測歷元，表1所示為各方案的模糊度固定結(jié)果比較，圖1為觀測衛(wèi)星數(shù)及PDOP值，圖2—圖5分別為各方案的航向角估計結(jié)果，其中固定解用直線繪制，浮點解用“＊”表示。圖6—圖9分別為各方案的模糊度精度衰減因子ADOP值結(jié)果。ADOP能有效表征模型幾何強度，可對模糊度固定成功率進行近似［16］。雖沒有外部姿態(tài)結(jié)果作為參考真值，但只要模糊度固定正確，4種方案的姿態(tài)結(jié)果基本一致，而姿態(tài)浮點解相對固定解會有較大突變，這點可由圖1—圖4看出，因此，主要從模糊度固定成功率的角度評價4種方案。采用“逐歷元L＋MCLABMDA”的方法也進行了解算，但MCLAMBDA算法無法使用Ratio檢驗等方法評價固定效果，對比他們的模糊度固定解，MCLAMBDA算法中1783個歷元的解與本文自適應抗差濾波法完全相同，說明兩種方法的正確率相當，這里主要比較它與本文方法的效率差異，結(jié)果如表2所示（計算平臺：HP dv4－3124tx）。

表1 模糊度固定結(jié)果Tab.1 Results of Ambiguity Fixing

圖1 觀測衛(wèi)星數(shù)及PDOP值Fig.1 Observed satellite number and PDOP

圖2 方案1航向角Fig.2 Yaw of scheme 1

圖3 方案2航向角Fig.3 Yaw of scheme 2

圖4 方案3航向角Fig.4 Yaw of scheme 3

表2 所有歷元姿態(tài)解算總耗時Tab.2 Total time cost of attitude determination of all epochs s

圖5 方案4航向角Fig.5 Yaw of scheme 4

圖6 方案1的ADOP值Fig.6 ADOP of scheme 1

圖7 方案2的ADOP值Fig.7 ADOP of scheme 2

從以上結(jié)果可以看出：

（1）相對于最小二乘，抗差估計能夠較好地抵制粗差干擾，提高測姿的可靠性，但是由于未充分利用歷史信息，僅由當前觀測值定姿的成功率不高?？共罟烙嫿Y(jié)果的ADOP結(jié)果較最小二乘略差，因為抗差估計對粗差觀測值進行降權，觀測值整體利用率更低。

圖8 方案3的ADOP值Fig.8 ADOP of scheme 3

圖9 方案4的ADOP值Fig.9 ADOP of scheme 4

（2）自適應濾波通過自適應因子合理利用歷史約束信息，改善觀測結(jié)構，提高固定率，并具備一定的抗粗差能力，但整體上仍受到粗差觀測值影響。

（3）自適應抗差濾波能夠合理利用姿態(tài)歷史信息，改善整體的結(jié)構，顯著減小ADOP值，且很好地抵御了粗差干擾，大大提高模糊度固定成功率。實際上9個未成功固定的歷元中，有4個歷元固定解正確而未通過Ratio檢驗，而在另外5個歷元處存在較多粗差，同時自適應因子為0，觀測結(jié)構較差，無法得到可靠的浮點解。

（4）由于在搜索過程中引入約束條件，MCLAMBDA的計算效率較低，耗時遠高于自適應抗差濾波方法。通常自適應抗差濾波比最小二乘更為耗時（如浮點解計算階段），但由于自適應抗差濾波得到的浮點解精度高于最小二乘，因此在模糊度搜索過程中更快，整體上效率稍高。

6 結(jié) 論

目前沒有可靠的由GNSS載波相位觀測值直接解算歐拉角或四元數(shù)的理論。基于原始觀測值直接定姿的難點在于如何合理利用約束信息，自適應抗差Kalman濾波能將姿態(tài)約束信息通過狀態(tài)模型與當前的觀測模型融合起來，本文通過建立合適的狀態(tài)模型和調(diào)節(jié)自適應因子，最大限度地發(fā)揮歷史信息的作用，同時通過抗差等價權抵制粗差觀測值的影響，實現(xiàn)可靠、高效的姿態(tài)確定。除自適應因子減小同時又出現(xiàn)粗差這類情況，本文方法均能實現(xiàn)瞬時姿態(tài)解算。通過模糊度參數(shù)的自適應，可以結(jié)合周跳探測結(jié)果調(diào)整模糊度歷史信息的權重，并可轉(zhuǎn)換為無須周跳探測的模式，增加了數(shù)據(jù)處理的靈活性。

本文建立姿態(tài)估計狀態(tài)模型可以用于陀螺等外部傳感器和GNSS進行組合測姿的情況，因此自適應抗差Kalman濾波實現(xiàn)瞬時姿態(tài)確定的方法能用于任何含多天線GNSS的姿態(tài)測量系統(tǒng)中，具有廣闊的應用前景。

［1］LIU Zhijian，LIU Yi.Algorithm of a Real－time Attitude System Using Non－dedicated GPS Receivers［J］.Acta Geodaetica et Cartographica Sinica，2005，34（3）：213－217.（劉志儉，劉毅.一種基于非專用接收機的GPS實時定姿算法［J］.測繪學報，2005，34（3）：213－217.）

［2］LIU Genyou，OU Jikun.Determining Attitude with Single Epoch GPS Algorithm and Its Precision Analysis［J］.Geomatics and Information Science of Wuhan University，2003，28（6）：732－735.（劉根友，歐吉坤.GPS單歷元定向和測姿算法及其精度分析［J］.武漢大學學報：信息科學版，2003，28（6）：732－735.）

［3］LI Zhenghang，LIU Wanke，LOU Yidong，et al.Heading Determination Algorithm with Single Epoch Dual－frequency GPS Data［J］.Geomatics and Information Science of Wuhan University，2007，32（9）：753－756.（李征航，劉萬科，樓益棟，等.基于雙頻GPS數(shù)據(jù)的單歷元定向算法研究［J］.武漢大學學報：信息科學版，2007，32（9）：753－756.）

［4］TANG Weiming，LI Di，CHI Fengming.Research on Single Epoch Orientation Algorithm of BeiDou Navigation Satellite System［J］.Geomatics and Information Science of Wuhan University，2013，38（9）：1014－1017.（唐衛(wèi)明，李笛，遲鳳明.北斗衛(wèi)星導航系統(tǒng)單歷元定向算法研究［J］.武漢大學學報：信息科學版，2013，38（9）：1014－1017.）

［5］LU Gang，CANNON M E.Attitude Determination Using a Multi－antenna GPS System for Hydrographic Applications［J］.Marine Geodesy，1994，17（4）：237－250.

［6］WANG C，LACHAPELLE G.GPS Attitude Determina－tion Reliability Performance Improvement Using Low Cost Receivers［C］∥Proceedings of the 15th International Technical Meeting of the Satellite Division of the Institute of Navigation.Portland，OR：［s.n.］，2002：1064－1074.

［7］SCHLEPPE J.Development of a Real－time Attitude System Using a Quaternion Parameterization and Non－dedicated GPS Receivers［D］.Alberta，Canada：The University of Calgary，1996.

［8］TEUNISSEN P J G.A General Multivariate Formulation of the Multi－antenna GNSS Attitude Determination Problem［J］.Artificial Satellites，2007，42（2）：97－111.

［9］W ANG Bing，SUI Lifen，ZHANG Qinghua，et al.Research on Attitude Determination Algorithms Using GPS［J］.Geomatics and Information Science of Wuhan University，2013，38（12）：1392－1395.（王冰，隋立芬，張清華，等.利用GPS解算載體測姿的算法研究［J］.武漢大學學報：信息科學版，38（12）：1392－1395.）

［10］CHAI Yanju.Theory and Method for Improving the Navigation Accuracy of GPS/INS Integration by Digging the Hidden Information［D］.Wuhan：Institute of Geodesy and Geophysics，Chinese Academy of Sciences，2008.（柴艷菊.挖掘信息提高GPS/INS導航精度的理論與方法研究［D］.武漢：中國科學院測量與地球物理研究所，2008.）

［11］YANG Yuanxi，HE Haibo，XU Guochang.A New Adaptively Robust Filtering for Kinematic Geodetic Positioning［J］.Journal of Geodesy，2001，75（2－3）：109－116.

［12］YANG Yuanxi，HE Haibo，XU Tianhe.Adaptive Robust Filtering for Kinematic GPS Positioning［J］.Acta Geodaetica et Cartographica Sinica，2001，30（4）：293－298.（楊元喜，何海波，徐天河.論動態(tài)自適應濾波［J］.測繪學報，2001，30（4）：293－298.）

［13］MARKLEY F L.Attitude Error Representations for Kalman Filtering［J］.Journal of Guidance，Control，and Dynamics，2003，26（2）：311－317.

［14］CUI Xianqiang，YANG Yuanxi.Adaptively Robust Filterring with Classified Adaptive Factors［J］.Progress in Natural Science，2006，16（4）：490－494.（崔先強，楊元喜.分類因子自適應抗差濾波［J］.自然科學進展，2006，16（4）：490－494.）

［15］YANG Yuanxi，CUI Xianqiang.Adaptively Robust Filter with Multi Adaptive Factors［J］.Survey Review，2008，40（309）：260－270.

［16］ODIJK D，TEUNISSEN PJG.ADOP in Closed Form for a Hierarchy of Multi－frequency Single－baseline GNSS Models［J］.Journal of Geodesy，2008，82（8）：473－492.