趙佩銘,張月超

(1.上海市政工程設計研究總院(集團)有限公司,上海200082;2.中國石油集團工程技術(shù)研究院,天津 300451)

0 引 言

單歷元定位定姿的特征是僅利用當前觀測歷元的GPS觀測數(shù)據(jù),其前提是要有高精度的載波相位觀測值[1-2]。在利用GPS觀測值做單歷元定位定姿解算中,無論是通過相位平滑偽距差分解算還是利用載波相位解算,傳統(tǒng)的方法并未考慮載體前一歷元時刻的姿態(tài)及其變化率對當前時刻姿態(tài)的影響,只利用當前時刻的歷元數(shù)據(jù)求得最小二乘解,因此在解算結(jié)果的平滑性上并不理想;另外,在單歷元定姿中容易受到噪聲值的影響,使得解算結(jié)果出現(xiàn)較大跳變影響定姿結(jié)果的穩(wěn)定性，綜合來講,現(xiàn)有的GPS定姿直接解算方法,大致存在以下幾種缺陷:1)不具有抗差性;2)對運動載體而言,沒有充分利用運動模型的信息;3)當一些歷元的實際觀測值少于必要觀測數(shù)時,會出現(xiàn)無解的情況[3]。

Kalman濾波作為一種新的重要的最優(yōu)估計理論已被廣泛應用于各種測量數(shù)據(jù)的處理[4]。在Kalman濾波估計理論中引入了狀態(tài)空間的概念,借助系統(tǒng)的狀態(tài)轉(zhuǎn)移方程根據(jù)前一時刻的狀態(tài)估值和當前時刻的觀測值遞推估計新的狀態(tài)估值[5]，與最小二乘法比較,Kalman濾波更適合于GPS動態(tài)定位數(shù)據(jù)處理[5]，在消除噪聲影響方面,近幾年基于Kalman濾波的抗差估計已有廣泛的研究[6]。對于GPS單歷元定姿數(shù)據(jù)中噪聲值的影響,本文基于穩(wěn)健估計理論在Kalman濾波方程的解算過程中加入抗差因子,以消除或減弱粗差或較大誤差項的影響，在實際解算中驗證了此方法的正確性,具有較好的應用效果。

1 姿態(tài)計算原理和方法

1.1 姿態(tài)解算中的坐標系

若能確定平臺上不在同一直線上的三個點的精確相對位置,那么就能確定該平臺的姿態(tài)[1]。在姿態(tài)測量中,表述一個運動載體的瞬時特征要涉及到兩個獨立的坐標系:載體坐標系和當?shù)厮阶鴺讼?通常利用表述兩個坐標系關(guān)系的3個歐拉角描述載體在當?shù)厮阶鴺讼抵械淖藨B(tài)[7]。GPS直接測量值是在WGS-84坐標系下的位置信息,WGS-84坐標系屬于地心地固坐標系。

測站水平坐標系(LLS)即站心坐標系:以測站為原點,測站上的法線(或垂線)為Z軸方向,北方向為X軸,東方向為Y軸,建立坐標系[8]。

載體坐標系(BFS):一般將載體坐標系的原點與當?shù)厮阶鴺讼档脑c重合,定義Y軸與載體運動方向的中心線重合,正向指向載體的運動方向,X軸垂直于Y軸并位于同一平面內(nèi),Z軸垂直于X,Y軸構(gòu)成的平面成右手坐標系。

當?shù)厮阶鴺讼蹬c載體坐標系之間的三個旋轉(zhuǎn)角分別用y,p,r表示航向角,俯仰角和滾動角。如圖1所示,假設12方向為參考方向,在計算姿態(tài)角時要首先將地固地心系下的坐標轉(zhuǎn)換為以1號點為坐標原點的站心坐標,設2、3號點的站心坐標分別為(x2,y2,z2)和(x3,y3,z3),則有[1-3]:

航向角:

(1)

俯仰角:

(2)

翻滾角:

(3)

其中,S12,S13分別表示1、2點和1、3點間的距離。

圖1 站心坐標系中表示姿態(tài)角

1.2 GPS單歷元定姿解算

一般的姿態(tài)測量系統(tǒng)的硬件采用多個天線共一個接收機的方式,顯然兩個天線之間的單差觀測值中消除了與接收機和衛(wèi)星相關(guān)的誤差[5]。在這種情況下,使用單差觀測值解算姿態(tài)角,其解的強度要比雙差方法高。現(xiàn)有研究已經(jīng)證明,主天線坐標誤差在100 m時,其對定姿結(jié)果的影響僅為毫米級,因此利用偽距單點定位方式確定主天線位置可以滿足定姿精度的要求[3]。假設現(xiàn)有1、2、3三個GPS天線,將天線1作為主天線,GPS觀測值組成的雙差觀測方程為

(4)

式中：Δφ為雙差相位觀測量;R表示接收機與衛(wèi)星間的幾何距離;N為整周模糊度; 1,2表示接收機編號；i,j為衛(wèi)星編號。采用雙差觀測值能夠有效的消除接收機和衛(wèi)星鐘差,削弱電離層、對流層的影響。對于相位觀測值而言,同時也使得模糊度具有整周特性,有利于后面的模糊度解算。

對式(4)做整理變換,得到誤差方程式

(5)

由觀測數(shù)據(jù)計算得出載體在WGS-84坐標系下的基線向量值dXWGS-84,將空間坐標系下的基線向量轉(zhuǎn)換到當?shù)厮阶鴺讼抵衃8]:

(6)

式中,B,L為主天線點的大地經(jīng)緯度。

2 姿態(tài)濾波平滑及穩(wěn)健參數(shù)估計

2.1 Kalman濾波在姿態(tài)解算中的應用

為了有效利用歷元間的狀態(tài)信息,同時要消除誤差的影響,本文提出采用Kalman濾波進行姿態(tài)參數(shù)的抗差估計。在狀態(tài)空間下的Kalman濾波的狀態(tài)方程和觀測方程為[9-11]

(7)

式中:Xk+1為系統(tǒng)狀態(tài)向量;Lk+1為觀測值;Φk+1為狀態(tài)轉(zhuǎn)移矩陣;Uk、Zk+1為非隨機控制向量;Wk為系統(tǒng)動態(tài)噪聲;Vk+1為觀測誤差;Υk+1、Ψk+1、Bk+1為相應的系數(shù)項,在一般系統(tǒng)中不考慮非隨機控制量Uk和Zk+1,本文亦是如此。

假設系統(tǒng)具有隨機統(tǒng)計特性

(8)

進行推導,得出Kalman濾波的遞推解[4]為

(9)

DX(k+1/k+1)= (E-Jk+1Bk+1)

DX(k+1/k)，

(10)

其中,濾波估計值為

(11)

其協(xié)方差陣

(12)

系統(tǒng)增益矩陣

(13)

濾波解的另一種直接表達形式為

(14)

(15)

以系統(tǒng)的姿態(tài)角及其變化速率作為狀態(tài)向量,即X=[yprvyvpvr]T,則其狀態(tài)轉(zhuǎn)移矩陣為

(16)

式中,Δt為前后歷元時間間隔。

在姿態(tài)解算中,通常假定姿態(tài)角加速度為高斯白噪聲,對各姿態(tài)角的系統(tǒng)噪聲認為其方差為[3]

(17)

2.2 改進Kalman濾波穩(wěn)健估計算法推導及程序?qū)崿F(xiàn)

在2.1小節(jié)中論述的是經(jīng)典Kalman濾波的基本過程及其如何應用在GNSS單歷元姿態(tài)測量中。對于在計算過程中碰到可能出現(xiàn)的粗差問題,本文提出一種穩(wěn)健Kalman濾波修正方法以期消除或削弱粗差對計算結(jié)果的影響。根據(jù)抗差M估計原理,引入降權(quán)因子γ,對于濾波計算過程中的新息殘差向量:

(18)

結(jié)合Huber權(quán)函數(shù),γ降權(quán)因子可由下式確定:

(19)

(20)

式中，c為預先設定的參數(shù)。引入單位權(quán)陣,則抗差Kalman濾波解為

(k+1/k)],

(21)

(22)

2)引入調(diào)節(jié)因子γ,設初始值為單位陣;

5)計算殘差向量Vk,并判斷其是否符合要求;

6)根據(jù)新息殘差向量值,重新計算調(diào)節(jié)因子,由此新的調(diào)節(jié)因子γ,得到新的權(quán)陣,解算濾波值。

7)重復計算(2)到(6)步,直到殘差向量收斂為止。

3 實驗計算與分析

在建筑物頂架設載體,載體為三角框架結(jié)構(gòu),三個頂點上可以放置GPS接收機,中心點可以放置其他定姿儀器。實際數(shù)據(jù)采集時，在三個頂角上放置Leica GS15雙頻接收機,載體中心點上放置激光跟蹤儀的6維定姿模塊,載體底部放置IMU模塊。3臺GPS接收機進行同步觀測,觀測時的采樣率設置為1 s,靜置采集時間共20 min.

對姿態(tài)角進行直接解算,包括偽距解算和載波相位解算兩種情況;其次,采用改進的穩(wěn)健Kalman濾波算法解算,同樣包括偽距解和載波相位解。兩種方法的靜態(tài)相位解算結(jié)果與跟蹤儀定姿結(jié)果和IMU定姿結(jié)果相吻合。

另外,利用每一歷元的觀測值作虛擬動態(tài)定位,采用兩種方法分別計算出每一歷元的姿態(tài)參數(shù)。兩種方法計算結(jié)果的誤差統(tǒng)計表如表1所示。

從表1中的各姿態(tài)角平均值可以看出,無論是利用最小二乘方法還是利用改進的Kalman濾波方法,均能有效的解算出載體姿態(tài)角;通過比較最值可以看出改進Kalman濾波算法要比最小二乘算法的穩(wěn)定性好。采用改進Kalman濾波算法的解算結(jié)果中消除掉了大部分的噪聲值，使得定姿結(jié)果更趨于平穩(wěn)；在出現(xiàn)粗差項時，改進Kalman濾波算法也具有很好的抗差性。

表1 姿態(tài)結(jié)果差值的統(tǒng)計分析

4 結(jié)束語

基于Kalman濾波的GPS定姿算法能夠比較充分的利用前后歷元間的載體姿態(tài)狀態(tài)關(guān)系,提高定姿結(jié)果的平滑性和穩(wěn)定性。本文提出的利用改進的Kalman濾波算法進行GNSS單歷元姿態(tài)測量在抗差性上有了比較好的提高。通過對解算結(jié)果的統(tǒng)計分析及繪制出的折線圖,可以看出:

1)在單歷元動態(tài)定姿解算中,采用Kalman濾波進行姿態(tài)解算的解算結(jié)果要優(yōu)于采用最小二乘間接平差的姿態(tài)角直接解算的結(jié)果;

2)對于出現(xiàn)的跳變數(shù)據(jù)或噪聲項,Kalman濾波本身就具有一定的抵抗性,并且在加入抗差因子后,改進Kalman濾波算法能夠較好的消除跳變值的影響。

如何更好的提高算法效率及更有效的解決單歷元解算中模糊度解算問題是今后深入研究解決的問題。

[1]劉根友,歐吉坤.GPS單歷元定向和測姿算法及其精度分析[J].武漢大學學報·信息科學版,2003,28(6):732-735.

[2]ZHEN D, KNEDLIK S, LOFFELD O,etal. A matlab toolbox for attitude determination with GPS multi-antenna systems[J].GPS Solut,2009,13(3):241-248.

[3]祁 芳.卡爾曼濾波算法在GPS非差相位精密單點定位中的應用[D].武漢：武漢大學測繪學院,2003.

[4]齊公玉,邱衛(wèi)寧,花向紅.卡爾曼濾波粗差修正方法應用[J].測繪工程,2010, 19 (2):50-52.

[5]WELCH G, BISHOP G. An introduction to Kalman filter[M]. ACM,INC,2001.

[6]張朝玉.卡爾曼濾波在多維AR序列建模中的應用[J].大地測量與地球動力學,2003(2):92-95.

[7]陳萬通,秦紅磊.一種新的GPS單頻單歷元定姿算法研究[J].上海航天,2012,29(3):11-14.