孫海文，歐陽(yáng)中輝，王 彥

(海軍航空工程學(xué)院兵器科學(xué)與技術(shù)系，山東煙臺(tái)264001)

0 引言

現(xiàn)在利用載波相位技術(shù)進(jìn)行定位是十分活躍的研究領(lǐng)域，由于GPS L1載波波長(zhǎng)很短，只有19 cm，載波相位測(cè)量噪聲很小，因此測(cè)量精度高，能達(dá)到很高的定位精度。對(duì)此許多人作了研究，并將此技術(shù)應(yīng)用到姿態(tài)測(cè)量領(lǐng)域［1－3］。

利用載波相位GPS進(jìn)行測(cè)姿已逐漸應(yīng)用于衛(wèi)星、導(dǎo)彈、飛機(jī)、船舶、汽車等高動(dòng)態(tài)的載體上［4－6］。隨著載波相位 GPS測(cè)量精度的不斷提高，利用其動(dòng)態(tài)測(cè)量精度高、實(shí)時(shí)性好、受環(huán)境影響小的特點(diǎn)［7－8］，可以對(duì)艦船進(jìn)行海上動(dòng)態(tài)標(biāo)校。目前對(duì)于姿態(tài)解算算法有很多，例如九參數(shù)最小二乘法、三參數(shù)迭代最小二乘法、直接算法［8］等。前2種算法的優(yōu)點(diǎn)是，可利用多條基線的冗余信息，解算精度較好，但計(jì)算過程復(fù)雜，實(shí)時(shí)性差;直接算法通過代數(shù)方法求解載體的姿態(tài)角，其原理簡(jiǎn)單易懂、計(jì)算速度快、實(shí)時(shí)性好，在單基線解算中，其解算精度較好，但在解算中沒有對(duì)粗大誤差和GPS信號(hào)缺失進(jìn)行處理，解算結(jié)果誤差較大。

1 艦船姿態(tài)角定義

艦船姿態(tài)角一般是指船舶甲板坐標(biāo)系相對(duì)當(dāng)?shù)厮阶鴺?biāo)系之間關(guān)系的角度，姿態(tài)角，為縱搖角(P)、橫搖角(R)、航向角(Y)。用坐標(biāo)系O－XYZ來(lái)表示水平坐標(biāo)系［9］，姿態(tài)角定義如圖1所示。

圖1 姿態(tài)角示意圖Fig.1 The attitude angle diagram

艦船相對(duì)于水平坐標(biāo)系X軸的相對(duì)旋轉(zhuǎn)角度定義為縱搖角，右手定則X軸正方向沿著手指方向轉(zhuǎn)為正，否則為負(fù);Y軸的相對(duì)旋轉(zhuǎn)角度定義為橫搖角，右手定則Y軸正方向沿著手指方向轉(zhuǎn)為正，否則為負(fù);Z軸的相對(duì)旋轉(zhuǎn)角度定義為航向角，右手定則Z軸正方向沿著手指方向轉(zhuǎn)為正，否則為負(fù)。

2 直接算法

直接計(jì)算法是根據(jù)基線的當(dāng)?shù)厮阶鴺?biāo)值和艦船甲板坐標(biāo)值及兩坐標(biāo)系間的轉(zhuǎn)換關(guān)系直接求解載體姿態(tài)角的算法。在單基線測(cè)量時(shí)只能求解出航向角和縱搖角。在船的首尾分別安放GPS接收機(jī)，尾部的接受機(jī)為基準(zhǔn)站天線1，首部的接收機(jī)為用戶站天線2，GPS接收機(jī)架設(shè)坐標(biāo)系如圖2所示，天線1和天線2之間的連線經(jīng)過甲板坐標(biāo)系原點(diǎn)且位于艦船甲板坐標(biāo)系的Y軸上。天線1和天線2之間的連線向量記作L12，這條基線的長(zhǎng)度和方向可精確測(cè)量。

圖2 甲板GPS架設(shè)示意圖Fig.2 The deck GPSerection diagram

直接計(jì)算法的具體計(jì)算過程如下:

第1步:根據(jù)GPS精密單點(diǎn)定位方式測(cè)量出基準(zhǔn)站的地心坐標(biāo)，同時(shí)利用GPS的載波相位差分方式測(cè)量出用戶站相對(duì)于主天線在地心坐標(biāo)系下的基線解:

其中E為地心坐標(biāo)系。

第2步:按照式(2)把式(1)地心坐標(biāo)系下的解轉(zhuǎn)換成當(dāng)?shù)厮阶鴺?biāo)系下的解:

式中:L為當(dāng)?shù)厮阶鴺?biāo)系;l0和b0分別為艦船甲板坐標(biāo)系原點(diǎn)的經(jīng)度和緯度。

第3步:根據(jù)當(dāng)?shù)厮阶鴺?biāo)系與艦船甲板坐標(biāo)系之間的轉(zhuǎn)換關(guān)系［10－11］，如圖3所示，可以得到坐標(biāo)轉(zhuǎn)換關(guān)系式:

其中D為艦船甲板坐標(biāo)系。

圖3 當(dāng)?shù)厮阶鴺?biāo)系和艦船甲板坐標(biāo)系間的轉(zhuǎn)化示意圖Fig.3 Conversion diagram of the local level coordinate system and the ship deck coordinate system

第4步:把基L12線的當(dāng)?shù)厮阶鴺?biāo)系的解d XLL=［d xL，2d yL，2d zL，2］T和艦船甲板坐標(biāo)系的解 d XDD=［0 L120］T同時(shí)代入式(4)，得:

由式(6)可解得:

對(duì)式(7)進(jìn)行微分，得到:

航向角的方差為

上式化成不等式為

σdxL，2，σdyL，2為 GPS 接收機(jī)的測(cè)距精度，統(tǒng)一用σΔGPS來(lái)表示，即為:

由上述公式分析可知，航向角的精度與定義航向的基線L12的長(zhǎng)度成反比，即基線長(zhǎng)度越長(zhǎng)，精度越高，此結(jié)論只適用于短基線 (一般在10 km范圍內(nèi))。

在解算縱搖角時(shí)基線在Z軸的投影長(zhǎng)度較短，因此其解算精度較低。

3 基于自適應(yīng)卡爾曼濾波的姿態(tài)解算算法

為滿足艦船姿態(tài)的實(shí)時(shí)測(cè)量需求，本節(jié)提出一種不受粗大誤差干擾和GPS信號(hào)短暫缺失的GPS定姿算法。該方法的基本原理和解算過程如下:

在姿態(tài)測(cè)量開始前，通過GPS靜態(tài)相對(duì)定位方式確定出用戶天線相對(duì)于基準(zhǔn)天線在載體坐標(biāo)系下的基線d XkD2，其中k為歷元號(hào);

艦船姿態(tài)測(cè)量過程中，根據(jù)偽距單點(diǎn)定位方式或偽距差分方式獲得基準(zhǔn)天線的地心坐標(biāo)值。同時(shí)，利用載波相位差分方式獲得用戶天線相對(duì)于基準(zhǔn)天線的基線向量(地心坐標(biāo)系)，見式(1);利用旋轉(zhuǎn)矩陣將上述地心坐標(biāo)系下的解轉(zhuǎn)換成當(dāng)?shù)厮阶鴺?biāo)系下的解:

再利用旋轉(zhuǎn)矩陣Tkdl，得到基線當(dāng)?shù)厮阶鴺?biāo)系下的解與載體坐標(biāo)系下解得函數(shù)關(guān)系式:

其中r，p，γ分別為翻滾角、俯仰角和航向角。

式(15)是以r，p，γ為未知量的非線性表達(dá)式。用自適應(yīng)卡爾曼濾波進(jìn)行參數(shù)估計(jì)前需先進(jìn)行線性化處理，把式(14)進(jìn)行泰勒級(jí)數(shù)展開并保留第一項(xiàng)，可得到式(14)的線性化表達(dá)式:

式中:∑dXkL，2為的協(xié)方差矩陣;Vk為殘差。由于是單基線測(cè)量，在解算過程中橫搖角r會(huì)為0。利用直接算法獲取初始解p0和γ0。

對(duì)于單基線，則其最小二乘解及協(xié)方差為:

在艦船的姿態(tài)測(cè)量過程中，艦船始終處于搖擺狀態(tài)，所以可根據(jù)前一歷元的信息和艦船的搖擺方程，來(lái)預(yù)測(cè)和估計(jì)下一歷元的信息。由于艦船的搖擺運(yùn)動(dòng)是不規(guī)則的曲線，采用加速度模型時(shí)會(huì)出現(xiàn)周期的間斷點(diǎn)，所以此處采用常速度模型，有:

其中，

式中:Xk和分別為歷元k的真值和估計(jì)值;Φk/k－1為狀態(tài)轉(zhuǎn)移矩陣;Wk為動(dòng)態(tài)噪聲;Δt為歷元間隔;ω為姿態(tài)角變化的速度;Q為姿態(tài)角變化的速度功率譜密度。

那么，基于常規(guī)卡爾曼濾波的遞推解為:

進(jìn)一步，可寫出基于自適應(yīng)卡爾曼濾波的解為:

其中α為自適應(yīng)因子，它們的計(jì)算公式如下:

式中:c0和c1為常數(shù)，一般取值范圍為c0=1.0～1.5，c1=3.0～4.5。ΔXt按下式求取:

4 仿真試驗(yàn)

4.1 仿真條件建立

假設(shè)2個(gè)組GPS數(shù)據(jù)基準(zhǔn)站為 (E37.94169444，N121.6988889)、 用 戶 站 為 (E37.94166666，N121.6975000)、高度為15 m，坐標(biāo)原點(diǎn)位于用戶站與基準(zhǔn)站之間連線上坐標(biāo)為 (E37.94168750，N121.6985417)。

建立艦艇縱搖角模型

航向角模型

縱搖角速度模型

航向角速度模型

式中:Ap，Aγ分別為艦船的縱搖角幅度值和航向角幅度值;Tp，Tγ分別為艦船縱搖周期、艦船偏航周期;p0，k0分別為艦船縱搖角初始相位、艦船偏航角初始相位;γ0為艦船航向角;ˉωp，ˉωγ為艦船引入誤差后的角速度。

4.2 縱搖角和航向角誤差比較

圖4是引入誤差后，縱搖角和航向角的測(cè)量誤差和標(biāo)準(zhǔn)差的曲線圖。

圖4 縱搖角誤差和航向角誤差曲線圖Fig.4 The curves of the pitch angle error and the path angle error

從圖4可看出，航向角誤差值的標(biāo)準(zhǔn)差小于縱搖角誤差值的標(biāo)準(zhǔn)差，而且相差一個(gè)數(shù)量級(jí)，因此得出結(jié)論，在單基線測(cè)量過程中縱搖角測(cè)量精度明顯低于航向角測(cè)量精度。

4.3 航向角誤差與基線長(zhǎng)度的關(guān)系分析

通過經(jīng)緯度數(shù)據(jù)可計(jì)算出2個(gè)GPS天線之間的距離為122 m;保持緯度不變，縮小經(jīng)度之間的差值，基線長(zhǎng)度分別為73 m和24 m。仿真結(jié)果如圖5所示。

圖5 不同基線長(zhǎng)度下誤差情況曲線圖Fig.5 The curves of different baseline length errors

從圖5可看出，隨著基線長(zhǎng)度縮短，測(cè)量誤差的標(biāo)準(zhǔn)差逐漸增大。因此可知，在短基線 (一般在10 km以內(nèi))情況下，航向角的測(cè)量精度跟基線長(zhǎng)度有關(guān)系，基線越長(zhǎng)，精度越高。

4.4 直接姿態(tài)解算算法和基于自適應(yīng)卡爾曼濾波的姿態(tài)解算算法比較及分析

第1步:仿真出用直接算法解算出的航向角與真實(shí)航向角對(duì)比曲線圖，并仿真出誤差曲線圖如圖6所示。

圖6 直接算法解算與真值比較及誤差曲線圖Fig.6 The comparison of the solution of the direct attitude algorithm and the true value curves and the error curves

第2步:仿真出改進(jìn)算法解算出的航向角與真實(shí)航向角對(duì)比曲線圖，并仿真出誤差曲線圖如圖7所示。

圖7 改進(jìn)算法解算與真值比較及誤差曲線圖Fig.7 The comparison of the solution of the improved algorithm and the true value curves and the error curves

第3步:為了更直觀地看出改進(jìn)算法的優(yōu)勢(shì)，將改進(jìn)算法、直接算法與真值進(jìn)行比較，如圖8所示。

圖8 改進(jìn)算法、直接算法與真值比較圖Fig.8 The comparison diagram of the improved algorithm，the direct algorithm and the true value

第4步:對(duì)2種算法的誤差進(jìn)行仿真比較，繪出誤差曲線并計(jì)算出誤差的標(biāo)準(zhǔn)差，如圖9所示。

圖9 兩種算法的誤差對(duì)比圖Fug.9 The error comparison diagram of the two algorithms

從仿真圖中可看出，改進(jìn)后的姿態(tài)測(cè)量算法能很好地消除粗大誤差，并能很好地補(bǔ)償缺失數(shù)據(jù)，測(cè)量的姿態(tài)角精度明顯提高。計(jì)算出的改進(jìn)算法的誤差標(biāo)準(zhǔn)差=0.001 2，明顯小于直接算法的誤差標(biāo)準(zhǔn)差=0.034 4，相差一個(gè)數(shù)量級(jí)，解算精度得到明顯提高。

5 結(jié)語(yǔ)

本文對(duì)直接法求解姿態(tài)角進(jìn)行公式推導(dǎo)，因?yàn)橹苯铀惴ㄔ诮馑氵^程中，不能消除粗大誤差并且受GPS信號(hào)缺失的影響，因此提出了一種改進(jìn)的姿態(tài)角解算即基于自適應(yīng)卡爾曼濾波的姿態(tài)解算算法;通過仿真試驗(yàn)分析比較航向角與縱搖角的測(cè)量精度、航向角與基線長(zhǎng)度的關(guān)系、直接法與基于自適應(yīng)卡爾曼濾波的姿態(tài)解算算法的測(cè)量精度，仿真結(jié)果表明航向角的解算精度比縱搖角的解算精度高，且高出一個(gè)數(shù)量級(jí);基線越長(zhǎng)，航向角測(cè)量精度越高;改進(jìn)算法比直接算法的解算精度高，且高出一個(gè)數(shù)量級(jí)。

［1］ 李躍，邱致和．導(dǎo)航與定位［M］．北京:國(guó)防工業(yè)出版社，2008:192－193．

［2］ ELLIOTT D K．Understanding GPSprinciples and applications artsch house［D］．1996．

［3］ ZHU R，SUN D，ZHOU Z Y，et al．A linear fusion algorithm for attitude determination using lowcostMEMS－based sensors［J］．Measurement，2007，40(3):322 －328．

［4］ 韓慧群．GPS姿態(tài)測(cè)量系統(tǒng)研究與開發(fā)［D］．哈爾濱:哈爾濱工程大學(xué)，2005．

［5］ CHANG X W，YANG X，ZHOU T Y．et al．A modified lambda method for integer least-squares estimation［J］．Journal of Geodesy，2005，79(2):552 －565．

［6］ 黃顯亮，溫連峰，王強(qiáng)．基于多天線GPS艦船姿態(tài)測(cè)量系統(tǒng)研究［J］．艦船電子工程，2009，29(12):80 －82．HUANG Xian-liang，WEN Lian-feng，WANG Qiang．Study on multi antenna GPS ship attitude measurement system based on the［J］．Ship Electronic Engineering，2009，29(12):80－82．

［7］ 楊鐵軍．GPS實(shí)時(shí)姿態(tài)測(cè)量技術(shù)與多徑誤差研究［D］．成都:電子科技大學(xué)，2003．

［8］ 李俊毅．GPS姿態(tài)測(cè)量及相關(guān)技術(shù)的研究［D］．南京:信息工程大學(xué)，2004．

［9］ 李哲，蘇秀琴，喬永明，等．基于船姿測(cè)量的艦載光電經(jīng)緯儀視軸穩(wěn)定方法研究［J］．光子學(xué)報(bào)，2009，38(6):1553－1556．LI Zhe，SU Xiu-qin，QIAO Yong-ming．et al．Shipboard theodolite LOS stabilization based on ship attitude measurement［J］．Acta Photonica Sinica，2009，38(6):1553－1556．

［10］楊鐵軍，張曉玲，付毓生，等．基于DSP的GPS雙天線實(shí)時(shí)姿態(tài)測(cè)量系統(tǒng)實(shí)現(xiàn)［J］．電波科學(xué)學(xué)報(bào)，2002，17(6):661－666．YANG Tie-jun，ZHANG Xiao-ling，F(xiàn)U Yu-sheng，et al．Realize the real-time attitude determination system of GPS dual antenna based on DSP［J］．Chinese Journal of Radio Science，2002，17(6):661 －666．

［11］黃德武．GPS變形監(jiān)測(cè)中多路徑效應(yīng)影響研究［D］．成都:西南交通大學(xué)，2005．