甘 雨,隋立芬,劉長(zhǎng)建,董 明
信息工程大學(xué)地理空間信息學(xué)院,河南 鄭州450000
GNSS測(cè)姿具有成本低、初始化快、誤差相對(duì)穩(wěn)定等特點(diǎn),已成為GNSS應(yīng)用研究的熱點(diǎn)。目前在雙天線單瞬時(shí)定向方面的研究較為深入,一般通過(guò)約束條件輔助單基線求解,再基于基線結(jié)果計(jì)算姿態(tài),這一領(lǐng)域已有豐富成果[1-4]。在多基線方面,有學(xué)者研究了整體解算方法[5-6],但是需先解基線再解姿態(tài),無(wú)法直接利用原始觀測(cè)值測(cè)姿。文獻(xiàn)[7]提出在多基線解算過(guò)程中固定模糊度,再將模糊度回代求解四元數(shù)參數(shù),以充分利用觀測(cè)信息,但本質(zhì)上仍屬基線解算模型。文獻(xiàn)[8]以方向余弦陣為姿態(tài)參數(shù),利用原始觀測(cè)值計(jì)算方向余弦和模糊度浮點(diǎn)解,將方向余弦的正交約束引入到模糊度搜索中,利用MCLAMBDA方法(multivariate constrained LAMBDA)進(jìn)行搜索。由于約束信息的引入,使得模糊度的搜索空間不再為橢球體,整周模糊度的搜索負(fù)擔(dān)遠(yuǎn)高于一般的LAMBDA方法[9],且方向余弦陣含9個(gè)參數(shù),解算效率偏低,影響實(shí)時(shí)應(yīng)用。目前未形成可靠的由載波相位觀測(cè)解算歐拉角或四元數(shù)固定解的理論。
現(xiàn)有測(cè)姿方法使用最小二乘進(jìn)行單歷元解算以避免周跳探測(cè)問(wèn)題,歷史信息沒(méi)有得到充分利用。使用自適應(yīng)選權(quán)濾波[10]、自適應(yīng)抗差濾波[11-12]等方法均能高效地平衡歷史信息和當(dāng)前觀測(cè),其中自適應(yīng)抗差濾波具有抵御粗差干擾的能力,在城區(qū)及海上姿態(tài)測(cè)量等應(yīng)用中能發(fā)揮優(yōu)勢(shì)。實(shí)際上,只要不使用模糊度歷史信息,使用濾波的方式也無(wú)須探測(cè)周跳,可實(shí)現(xiàn)瞬時(shí)定姿。
本文推導(dǎo)了基于失準(zhǔn)角及乘性誤差四元數(shù)的載波相位觀測(cè)模型,分別建立有外部角速度傳感器和無(wú)外部傳感器輔助下姿態(tài)參數(shù)估計(jì)的狀態(tài)模型;使用自適應(yīng)抗差濾波估計(jì)姿態(tài),借鑒分類自適應(yīng)因子的思想,對(duì)模糊度參數(shù)和姿態(tài)誤差參數(shù)分別自適應(yīng),通過(guò)模糊度自適應(yīng)因子控制模糊度先驗(yàn)值的信任度,若選擇極小的自適應(yīng)因子,可在濾波框架內(nèi)免去探測(cè)周跳過(guò)程;根據(jù)Ratio值確定姿態(tài)參數(shù)的自適應(yīng)因子。自適應(yīng)抗差濾波能夠充分利用約束信息和歷史信息,將其融合在浮點(diǎn)解計(jì)算過(guò)程中,極大提高模糊度浮點(diǎn)解精度及其協(xié)方差的結(jié)構(gòu),在此基礎(chǔ)上使用LAMBDA方法即能快速搜索出固定解。采用實(shí)測(cè)艦載GNSS 3天線測(cè)姿算例證明了本文方法的有效性。
設(shè)在主天線i和天線j同步觀測(cè)到衛(wèi)星p、q,可組站星雙差觀測(cè)方程
將式(1)在基線初值展開(kāi)并忽略大氣折射延遲項(xiàng),有
式中,bij為基線參數(shù)和為方向余弦矢量;
整理可得基線觀測(cè)模型為
由于GNSS觀測(cè)方程為非線性,姿態(tài)參數(shù)之間的關(guān)系也為非線性,因此,姿態(tài)估計(jì)中一般計(jì)算名義姿態(tài)中包含的姿態(tài)誤差,再對(duì)名義姿態(tài)進(jìn)行誤差修正。這里分別從失準(zhǔn)角和乘性四元數(shù)這兩種姿態(tài)誤差參數(shù)的角度推導(dǎo)相應(yīng)的載波相位觀測(cè)方程。
設(shè)載體坐標(biāo)系為b系,當(dāng)?shù)厮阶鴺?biāo)系為n系,載體姿態(tài)可用姿態(tài)矩陣表示。設(shè)名義當(dāng)?shù)厮阶鴺?biāo)系為n′系,定義n系到n′系的失準(zhǔn)角為φ,它表示實(shí)際當(dāng)?shù)厮较祅和名義當(dāng)?shù)厮较祅′之間的旋轉(zhuǎn)矢量,可理解為名義系n′的誤差。若失準(zhǔn)角較小,其近似等于俯仰、橫滾和航向的歐拉角誤差,這里為了保證嚴(yán)密性,依舊稱之失準(zhǔn)角而非歐拉角誤差。n和n′之間的關(guān)系可用式(6)的姿態(tài)矩陣的形式表示[13]
式中,(φ×)為φ的叉乘矩陣,為一反對(duì)稱陣。
失準(zhǔn)角、名義姿態(tài)陣、姿態(tài)陣之間的關(guān)系如式(7)所示
式(3)中的bij為地固系e系中的基線,加上上標(biāo)為,由坐標(biāo)旋轉(zhuǎn)關(guān)系,有
將式(8)代入式(3),得
整理可得基于失準(zhǔn)角參數(shù)的載波相位觀測(cè)方程
對(duì)于天線j1和主天線i,按照式(11)可由所有觀測(cè)衛(wèi)星得到矩陣形式的觀測(cè)方程為
若有天線(j1,j2,…,jm),則基于失準(zhǔn)角和模糊度參數(shù)(略去雙差算子)的載波相位觀測(cè)模型為
前面推導(dǎo)了基于失準(zhǔn)角表示姿態(tài)誤差的觀測(cè)方程,也可采用乘性誤差四元數(shù)的形式。在誤差角為小量下,有[13]
僅需將式(12)稍作改動(dòng)即可得到基于乘性誤差四元數(shù)的載波相位觀測(cè)方程
若GNSS姿態(tài)測(cè)量系統(tǒng)含m條基線,使用基線解算模型需解算3m個(gè)基線參數(shù),而使用基于失準(zhǔn)角或誤差四元數(shù)的觀測(cè)值模型僅解算3個(gè)姿態(tài)參數(shù),效率更高。
四元數(shù)微分方程為[7]
由姿態(tài)矩陣與姿態(tài)四元數(shù)之間的關(guān)系可得乘性誤差四元數(shù)δQ滿足
將式(18)、式(19)代入式(21)
根據(jù)四元數(shù)乘法準(zhǔn)則,得
由式(16),得
式(25)、式(26)的右端第2項(xiàng)即為狀態(tài)噪聲。
若有外部角速度傳感器如陀螺儀的輔助,則可直接使用式(25)或式(26)建立狀態(tài)方程,否則,無(wú)法獲得角速度,此時(shí)將式(26)改化為
將ω?cái)U(kuò)展為狀態(tài)參數(shù)并用隨機(jī)游走模型表示,納入式(27)并離散化
即為無(wú)外部角速度傳感器輔助下的姿態(tài)估計(jì)狀態(tài)模型。
綜合式(13)、式(28)可得基于載波相位觀測(cè)值估計(jì)姿態(tài)的濾波模型為
式(30)中的A與式(13)略有不同,為
使用姿態(tài)誤差而非姿態(tài)本身作為濾波中的姿態(tài)參數(shù),可以減小觀測(cè)方程線性化引起的模型誤差。設(shè)計(jì)算的名義姿態(tài)陣,濾波估計(jì)得到的失準(zhǔn)角,代入式(6)、式(7),即可對(duì)名義姿態(tài)進(jìn)行修正,修正后的即為濾波解。在k時(shí)刻,以k-1時(shí)刻的濾波解作為名義解。若k時(shí)刻成功固定模糊度即得到失準(zhǔn)角固定解時(shí),應(yīng)在完成姿態(tài)修正后將失準(zhǔn)角置零后傳遞至下一歷元。
若前一時(shí)刻姿態(tài)估計(jì)精度高,則本時(shí)刻名義解較為可靠,應(yīng)信任狀態(tài)預(yù)測(cè)信息,若名義解精度不高,則使用自適應(yīng)因子減小狀態(tài)信息的貢獻(xiàn)。
姿態(tài)誤差、角速度、模糊度屬不同類型的參數(shù),很難采用統(tǒng)一的自適應(yīng)因子進(jìn)行調(diào)節(jié),分類因子自適應(yīng)濾波可對(duì)不同的分量選取不同的自適應(yīng)因子。分類因子自適應(yīng)抗差濾波解為[14-15]
式中,為抗差等價(jià)權(quán)矩陣,可由Huber函數(shù)確定[15]
式中,為標(biāo)準(zhǔn)化殘差;c為常量,可取1.0~1.5。
為狀態(tài)預(yù)測(cè)的自適應(yīng)權(quán)矩陣,有
式中,αk為自適應(yīng)因子矩陣,即
由于ω為間接觀測(cè)量,這里不作自適應(yīng),有
αN可調(diào)整模糊度先驗(yàn)信息的權(quán)重,若進(jìn)行周跳探測(cè)得到各衛(wèi)星中最大周跳估值為dN,顧及周跳探測(cè)量自身的噪聲,可選取自適應(yīng)因子為
式中,σˉN為模糊度預(yù)測(cè)標(biāo)準(zhǔn)差,主要由設(shè)定的模糊度狀態(tài)噪聲決定;σDT為周跳探測(cè)量的中誤差,周跳估值越大,模糊度自適應(yīng)因子越小,一般αN<1。
若要免去周跳探測(cè)步驟以便瞬時(shí)解算,則可取αN為一極小值,如αN=1.0e-8,此時(shí),模糊度先驗(yàn)信息的權(quán)重降至極小,不受周跳干擾。
自適應(yīng)濾波常用的誤差判別統(tǒng)計(jì)量包括狀態(tài)不符值或預(yù)測(cè)殘差等,GNSS定姿中,在模糊度固定前,無(wú)法獲得足夠精度的上述統(tǒng)計(jì)量??紤]姿態(tài)誤差的預(yù)測(cè)性能本質(zhì)上由名義解即前一歷元姿態(tài)濾波解的精度決定,其對(duì)應(yīng)的Ratio值能夠在一定程度上反映其可靠程度,αφ由三段函數(shù)表示
式中,Rat為Ratio值,一般取c0=2、c1=3。
本文算例使用實(shí)測(cè)艦載GPS 3個(gè)天線姿態(tài)測(cè)量雙頻數(shù)據(jù)進(jìn)行計(jì)算和比較分析,3個(gè)天線構(gòu)成兩條獨(dú)立基線,基線的載體系坐標(biāo)分別為[0.0 15.0 0.0]T、[14.5 25.1 0.0]T。由于海面環(huán)境復(fù)雜,加之天線間距離較遠(yuǎn),雙差后部分觀測(cè)值仍受到較為嚴(yán)重的多路徑效應(yīng)影響,以單個(gè)歷元而言它們表現(xiàn)出粗差性質(zhì)。姿態(tài)解算中模糊度固定解的搜索均采用LAMBDA方法,若Ratio值超過(guò)3,則視相應(yīng)歷元成功固定。設(shè)計(jì)以下4種方案計(jì)算浮點(diǎn)解進(jìn)行對(duì)比。
方案1:?jiǎn)螝v元最小二乘;
方案2:?jiǎn)螝v元抗差估計(jì);
方案3:自適應(yīng)濾波;
方案4:自適應(yīng)抗差濾波。
方案3和方案4中,取模糊度自適應(yīng)因子αN=1.0e-8,即完全不使用模糊度先驗(yàn)信息,以和方案1、方案2形成對(duì)照,因此4個(gè)方案均無(wú)須主探測(cè)周跳。方案3中僅使用了自適應(yīng)因子,未使用抗差等價(jià)權(quán)控制粗差干擾。共有1800個(gè)觀測(cè)歷元,表1所示為各方案的模糊度固定結(jié)果比較,圖1為觀測(cè)衛(wèi)星數(shù)及PDOP值,圖2—圖5分別為各方案的航向角估計(jì)結(jié)果,其中固定解用直線繪制,浮點(diǎn)解用“*”表示。圖6—圖9分別為各方案的模糊度精度衰減因子ADOP值結(jié)果。ADOP能有效表征模型幾何強(qiáng)度,可對(duì)模糊度固定成功率進(jìn)行近似[16]。雖沒(méi)有外部姿態(tài)結(jié)果作為參考真值,但只要模糊度固定正確,4種方案的姿態(tài)結(jié)果基本一致,而姿態(tài)浮點(diǎn)解相對(duì)固定解會(huì)有較大突變,這點(diǎn)可由圖1—圖4看出,因此,主要從模糊度固定成功率的角度評(píng)價(jià)4種方案。采用“逐歷元L+MCLABMDA”的方法也進(jìn)行了解算,但MCLAMBDA算法無(wú)法使用Ratio檢驗(yàn)等方法評(píng)價(jià)固定效果,對(duì)比他們的模糊度固定解,MCLAMBDA算法中1783個(gè)歷元的解與本文自適應(yīng)抗差濾波法完全相同,說(shuō)明兩種方法的正確率相當(dāng),這里主要比較它與本文方法的效率差異,結(jié)果如表2所示(計(jì)算平臺(tái):HP dv4-3124tx)。
表1 模糊度固定結(jié)果Tab.1 Results of Ambiguity Fixing
圖1 觀測(cè)衛(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)解算總耗時(shí)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)相對(duì)于最小二乘,抗差估計(jì)能夠較好地抵制粗差干擾,提高測(cè)姿的可靠性,但是由于未充分利用歷史信息,僅由當(dāng)前觀測(cè)值定姿的成功率不高??共罟烙?jì)結(jié)果的ADOP結(jié)果較最小二乘略差,因?yàn)榭共罟烙?jì)對(duì)粗差觀測(cè)值進(jìn)行降權(quán),觀測(cè)值整體利用率更低。
圖8 方案3的ADOP值Fig.8 ADOP of scheme 3
圖9 方案4的ADOP值Fig.9 ADOP of scheme 4
(2)自適應(yīng)濾波通過(guò)自適應(yīng)因子合理利用歷史約束信息,改善觀測(cè)結(jié)構(gòu),提高固定率,并具備一定的抗粗差能力,但整體上仍受到粗差觀測(cè)值影響。
(3)自適應(yīng)抗差濾波能夠合理利用姿態(tài)歷史信息,改善整體的結(jié)構(gòu),顯著減小ADOP值,且很好地抵御了粗差干擾,大大提高模糊度固定成功率。實(shí)際上9個(gè)未成功固定的歷元中,有4個(gè)歷元固定解正確而未通過(guò)Ratio檢驗(yàn),而在另外5個(gè)歷元處存在較多粗差,同時(shí)自適應(yīng)因子為0,觀測(cè)結(jié)構(gòu)較差,無(wú)法得到可靠的浮點(diǎn)解。
(4)由于在搜索過(guò)程中引入約束條件,MCLAMBDA的計(jì)算效率較低,耗時(shí)遠(yuǎn)高于自適應(yīng)抗差濾波方法。通常自適應(yīng)抗差濾波比最小二乘更為耗時(shí)(如浮點(diǎn)解計(jì)算階段),但由于自適應(yīng)抗差濾波得到的浮點(diǎn)解精度高于最小二乘,因此在模糊度搜索過(guò)程中更快,整體上效率稍高。
目前沒(méi)有可靠的由GNSS載波相位觀測(cè)值直接解算歐拉角或四元數(shù)的理論。基于原始觀測(cè)值直接定姿的難點(diǎn)在于如何合理利用約束信息,自適應(yīng)抗差Kalman濾波能將姿態(tài)約束信息通過(guò)狀態(tài)模型與當(dāng)前的觀測(cè)模型融合起來(lái),本文通過(guò)建立合適的狀態(tài)模型和調(diào)節(jié)自適應(yīng)因子,最大限度地發(fā)揮歷史信息的作用,同時(shí)通過(guò)抗差等價(jià)權(quán)抵制粗差觀測(cè)值的影響,實(shí)現(xiàn)可靠、高效的姿態(tài)確定。除自適應(yīng)因子減小同時(shí)又出現(xiàn)粗差這類情況,本文方法均能實(shí)現(xiàn)瞬時(shí)姿態(tài)解算。通過(guò)模糊度參數(shù)的自適應(yīng),可以結(jié)合周跳探測(cè)結(jié)果調(diào)整模糊度歷史信息的權(quán)重,并可轉(zhuǎn)換為無(wú)須周跳探測(cè)的模式,增加了數(shù)據(jù)處理的靈活性。
本文建立姿態(tài)估計(jì)狀態(tài)模型可以用于陀螺等外部傳感器和GNSS進(jìn)行組合測(cè)姿的情況,因此自適應(yīng)抗差Kalman濾波實(shí)現(xiàn)瞬時(shí)姿態(tài)確定的方法能用于任何含多天線GNSS的姿態(tài)測(cè)量系統(tǒng)中,具有廣闊的應(yī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.(劉志儉,劉毅.一種基于非專用接收機(jī)的GPS實(shí)時(shí)定姿算法[J].測(cè)繪學(xué)報(bào),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單歷元定向和測(cè)姿算法及其精度分析[J].武漢大學(xué)學(xué)報(bào):信息科學(xué)版,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.(李征航,劉萬(wàn)科,樓益棟,等.基于雙頻GPS數(shù)據(jù)的單歷元定向算法研究[J].武漢大學(xué)學(xué)報(bào):信息科學(xué)版,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)星導(dǎo)航系統(tǒng)單歷元定向算法研究[J].武漢大學(xué)學(xué)報(bào):信息科學(xué)版,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解算載體測(cè)姿的算法研究[J].武漢大學(xué)學(xué)報(bào):信息科學(xué)版,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ǎo)航精度的理論與方法研究[D].武漢:中國(guó)科學(xué)院測(cè)量與地球物理研究所,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.(楊元喜,何海波,徐天河.論動(dòng)態(tài)自適應(yīng)濾波[J].測(cè)繪學(xué)報(bào),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.(崔先強(qiáng),楊元喜.分類因子自適應(yīng)抗差濾波[J].自然科學(xué)進(jìn)展,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.