利用改進(jìn)的Kalman濾波算法進(jìn)行GNSS單歷元姿態(tài)測(cè)量

趙佩銘,張?jiān)鲁?/p>

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

0 引 言

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

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

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

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

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

測(cè)站水平坐標(biāo)系(LLS)即站心坐標(biāo)系:以測(cè)站為原點(diǎn),測(cè)站上的法線(或垂線)為Z軸方向,北方向?yàn)閄軸,東方向?yàn)閅軸,建立坐標(biāo)系[8]。

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

當(dāng)?shù)厮阶鴺?biāo)系與載體坐標(biāo)系之間的三個(gè)旋轉(zhuǎn)角分別用y,p,r表示航向角,俯仰角和滾動(dòng)角。如圖1所示,假設(shè)12方向?yàn)閰⒖挤较?在計(jì)算姿態(tài)角時(shí)要首先將地固地心系下的坐標(biāo)轉(zhuǎn)換為以1號(hào)點(diǎn)為坐標(biāo)原點(diǎn)的站心坐標(biāo),設(shè)2、3號(hào)點(diǎn)的站心坐標(biāo)分別為(x2,y2,z2)和(x3,y3,z3),則有[1-3]:

航向角:

(1)

俯仰角:

(2)

翻滾角:

(3)

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

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

1.2 GPS單歷元定姿解算

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

(4)

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

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

(5)

由觀測(cè)數(shù)據(jù)計(jì)算得出載體在WGS-84坐標(biāo)系下的基線向量值dXWGS-84,將空間坐標(biāo)系下的基線向量轉(zhuǎn)換到當(dāng)?shù)厮阶鴺?biāo)系中[8]:

(6)

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

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

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

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

(7)

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

假設(shè)系統(tǒng)具有隨機(jī)統(tǒng)計(jì)特性

(8)

進(jìn)行推導(dǎo),得出Kalman濾波的遞推解[4]為

(9)

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

DX(k+1/k)，

(10)

其中,濾波估計(jì)值為

(11)

其協(xié)方差陣

(12)

系統(tǒng)增益矩陣

(13)

濾波解的另一種直接表達(dá)形式為

(14)

(15)

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

(16)

式中,Δt為前后歷元時(shí)間間隔。

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

(17)

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

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

(18)

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

(19)

(20)

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

(k+1/k)],

(21)

(22)

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

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

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

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

3 實(shí)驗(yàn)計(jì)算與分析

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

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

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

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

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

4 結(jié)束語

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

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

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

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

[1]劉根友,歐吉坤.GPS單歷元定向和測(cè)姿算法及其精度分析[J].武漢大學(xué)學(xué)報(bào)·信息科學(xué)版,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非差相位精密單點(diǎn)定位中的應(yīng)用[D].武漢：武漢大學(xué)測(cè)繪學(xué)院,2003.

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

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

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

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