一種基于卡爾曼濾波的動態(tài)相對位置解算算法*

孫海文，歐陽中輝

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

?

一種基于卡爾曼濾波的動態(tài)相對位置解算算法*

孫海文，歐陽中輝

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

摘要：在艦船海上校飛過程中，測量解算飛機(jī)相對艦船的位置會產(chǎn)生一定的誤差。采用直接算法在解算中沒有對粗大誤差和GPS信號缺失進(jìn)行處理，解算結(jié)果誤差較大。提出了一種改進(jìn)的相對位置解算算法。通過仿真實驗，從方位角、俯仰角和距離3個方面，對改進(jìn)算法的解算精度和直接算法的解算精度進(jìn)行分析比較，仿真結(jié)果表明，改進(jìn)算法能很好的提高相對位置解算精度。

關(guān)鍵詞：差分GPS；載波相位；動態(tài)相對定位；直接算法；基于卡爾曼濾波的相對位置解算算法；測量精度

0引言

在GPS定位研究的發(fā)展初期，主要使用C/A碼偽距和P碼偽距進(jìn)行定位，其數(shù)學(xué)原理簡單，定位速度可達(dá)到實時水平，但其定位精度不高。使用載波相位相對定位技術(shù)，因其高精度的定位能力，在國際上得到廣泛的研究[1-4]。

在進(jìn)行海上動態(tài)校飛真值獲取時，不關(guān)心飛機(jī)在大地坐標(biāo)系下的絕對位置，我們只關(guān)心飛機(jī)用戶站待定坐標(biāo)點相對于艦船基準(zhǔn)站已知坐標(biāo)點的位置，同時還要保證測量的高精度，這里使用載波相位相對定位技術(shù)進(jìn)行坐標(biāo)點位置獲取，然后對目標(biāo)相對位置的方位角、俯仰角及相對距離進(jìn)行解算，采用傳統(tǒng)的直接算法進(jìn)行解算，不能處理粗大誤差和GPS信號短暫缺失的問題，因此本文提出了一種基于卡爾曼濾波的動態(tài)相對位置解算算法。

1載波相位相對定位分析

利用載波相位DGPS進(jìn)行相對定位的原理是：通過單點精密定位獲取基準(zhǔn)站的坐標(biāo)，基準(zhǔn)站將采集的載波相位觀測值傳送給用戶站，然后進(jìn)行雙差解算獲取用戶站的坐標(biāo)[5-8]。

如圖1所示A，B為GPS的2個接收天線，把連接這2個天線的線段稱為基線，稱這個帶有方向的基線為基線向量，記作P。在GPS姿態(tài)測量系統(tǒng)中，基線長度一般為幾米到十幾千米，而接收機(jī)與GPS衛(wèi)星的距離約為20 000 km，所以由天線A和B指向同一顆GPS衛(wèi)星的單位矢量可以看作是相同的，記為e。于是GPS信號的波前可以看作平面波。

圖1　姿態(tài)測量示意圖Fig.1　Sketch map of attitude determination

從圖1中的矢量三角形可以看出：

(1)

基準(zhǔn)站首先把測得的GPS信號載波相位廣播出來，用戶接收到之后，和自己測得的對同一顆衛(wèi)星的信號進(jìn)行作差得

(2)

由于接收機(jī)A和B的距離相對于站星距來說很近，從衛(wèi)星1發(fā)射出的電磁波到達(dá)接收機(jī)A與到達(dá)接收機(jī)B的傳播環(huán)境是一樣的，所受的電離層和對流層延遲也是相同的，進(jìn)而式(2)可以簡寫為

(3)

同理，對于GPS衛(wèi)星2有如下等式：

(4)

如果GPS接收機(jī)A和B能同時觀測到的衛(wèi)星數(shù)目大于或等于5顆(這里取5顆星，設(shè)為衛(wèi)星1,2，3，4，5)，同時設(shè)衛(wèi)星1為主星，如果在用戶接收機(jī)內(nèi)把不同衛(wèi)星的單差結(jié)果相互之間求差一次，便形成所謂的雙差[10]，則可以得到由4個載波相位雙差方程組成的方程組，忽略殘差的影響并寫成矩陣的形式有

(5)

2坐標(biāo)轉(zhuǎn)換

以用戶站天線坐標(biāo)為例

(6)

圖2　轉(zhuǎn)換關(guān)系示意圖Fig.2　Sketch map of transformation relationship

(7)

式中：

(8)

3基于卡爾曼濾波的動態(tài)相對位置解算

建立水平坐標(biāo)系，假設(shè)在艦船艉部架設(shè)基準(zhǔn)站天線1，設(shè)為坐標(biāo)原點，飛機(jī)上架設(shè)用戶站天線2，建立如圖3所示坐標(biāo)系。

圖3　水平坐標(biāo)系下相對位置示意圖Fig.3　Sketch map of horizontal coordinates relative position

利用載波相位DGPS進(jìn)行動態(tài)相對定位解算時，采用直接法無法解決GPS信號缺失且不能反映出連續(xù)觀測時各歷元之間的關(guān)系，采用卡爾曼濾波[13-15]通過系統(tǒng)的狀態(tài)轉(zhuǎn)移方程可以反映出各歷元之間在觀測數(shù)據(jù)上的關(guān)系，并能解決GPS信號缺失問題。具體步驟如下：

假設(shè)飛機(jī)勻速飛行，建立卡爾曼濾波器基本模型，設(shè)狀態(tài)向量為

狀態(tài)方程：Xk=Φk|k-1Xk-1+Γk-1Wk-1，

(9)

觀測方程：Zk=HkXk+Vk，

(10)

狀態(tài)轉(zhuǎn)移矩陣Φk|k-1為

(11)

干擾矩陣Γk-1為

(12)

(13)

式中：ΔT為觀測歷元的時間間隔；Wk-1和Vk分別為系統(tǒng)激勵噪聲序列和系統(tǒng)噪聲驅(qū)動序列，它們的特性為

(14)

(15)

(16)

誤差協(xié)方差預(yù)測方程：

(17)

估計誤差協(xié)方差方程：

Pk=(I-KkHk)Pk|k-1.

(18)

(19)

(20)

(21)

4仿真試驗

4.1仿真條件建立

已知基準(zhǔn)站和用戶站的初始位置分別為(E37.941 694 44，N121.698 888 9，h15.134 256 72)、用戶站為(E37.943 333 33，N121.730 833 3，h263.726 856 5)，假設(shè)在水平坐標(biāo)系下飛機(jī)相對艦船的速度(單位：m/s)在x，y和z軸上為

(22)

式中：T1和T2，T3和T4，T5和T6分別控制飛機(jī)相對艦船的速度在各坐標(biāo)軸的變化周期。

4.2基于卡爾曼濾波的相對定位解算算法仿真結(jié)果分析

飛機(jī)與艦船相對位置隨時間的關(guān)系如圖4所示。從圖4中可以看出10 s內(nèi)飛機(jī)相對艦船俯仰角、方位角和距離的變化情況。

圖4　飛機(jī)與艦船相對位置隨時間的關(guān)系Fig.4　Relationship between the relative positionof aircraft and ships with time

從圖4中可知，由于飛機(jī)相對艦船運動速度時刻不規(guī)則變化，因此得到的俯仰角、方位角和距離的變化存在不規(guī)則性。

引入隨機(jī)噪聲后用直接算法解算動態(tài)相對位置與基于卡爾曼濾波的動態(tài)相對位置解算法比較仿真結(jié)果如圖5所示。

定性分析：從圖5可見，基于卡爾曼濾波的動態(tài)相對位置解算算法能很好地消除粗大誤差并能很好的補(bǔ)償缺失數(shù)據(jù)，測量的相對位置精度明顯提高。從圖5a)，5b)可知俯仰角和方位角時刻變化；從圖5c)可知飛機(jī)正在接近艦船。

定量分析：從圖6可見，直接算法計算出的方位角、俯仰角和距離的誤差標(biāo)準(zhǔn)差分別為0.032，0.033，1.868；基于卡爾曼濾波的姿態(tài)解算算法計算出的方位角、俯仰角和距離的誤差標(biāo)準(zhǔn)差分別為0.011，0.012，0.643。可以看出，基于卡爾曼濾波的動態(tài)相對位置解算精度明顯優(yōu)于直接法解算的精度。

利用Monte Carlo仿真方法，在隨機(jī)噪聲下進(jìn)行1 000次仿真試驗，分別得到改進(jìn)法和直接法的方位角、俯仰角和距離的誤差統(tǒng)計分布圖，如圖7所示。

圖5　2種算法動態(tài)相對位置仿真曲線圖Fig.5　Curve map of dynamic relative position on two kinds of algorithm

圖6　2種方法誤差仿真曲線圖Fig.6　Curve map of error of two methods

從圖7可見，在多次仿真試驗中，濾波法的解算誤差明顯小于直接法的解算誤差，由此可知基于卡爾曼濾波的姿態(tài)解算算法能很好地提高解算精度。

圖7　2種方法誤差統(tǒng)計分布圖Fig.7　Statistics chart of error distribution of two methods

5結(jié)束語

本文對載波相位相對定位技術(shù)的原理進(jìn)行闡述，再此基礎(chǔ)上對獲取的兩GPS接收機(jī)的坐標(biāo)點進(jìn)行算法解算，這里提出了一種改進(jìn)的動態(tài)相對位置解算算法即基于卡爾曼濾波的動態(tài)相對位置解算算法，它能消除粗大誤差并且受GPS信號缺失的影響。通過仿真試驗分析比較直接法與改進(jìn)算法的測量精度，仿真結(jié)果表明改進(jìn)算法比直接算法的解算精度高，解算效果好。

參考文獻(xiàn)：

[1]劉大杰，施一民，過靜珺．全球定位系統(tǒng)(GPS)的原理與數(shù)據(jù)處理[M]．上海：同濟(jì)大學(xué)出版社，1996：78-143．

LIU Da-jie，SHI Yi-min，GUO Jing-jun．The Global Positioning System (GPS) Theory and Data Processing [M]．Shanghai：Tongji University Press，1996：78-143．

[2]周忠謨，易杰軍，周琪．GPS衛(wèi)星測量原理與應(yīng)用 [M]．修訂版．北京：測繪出版社，1997：90-165．

ZHOU Zhong-mo，YI Jie-jun，ZHOU Qi. Measurement Principle and Application of GPS[M]．Revised ed．Beijing：Surveying and Mapping Press，1997：90-165．

[3]張勤，李家權(quán)．GPS測量原理及應(yīng)用[M]．北京：科學(xué)出版社，2005．

ZHANG Qin，LI Jia-quan．Principle and Application of GPS Measurement [M]．Beijing：Science Press，2005．

[4]胡伍生，高成發(fā)．GPS測量原理及其應(yīng)用[M]．北京：人民交通出版社，2002：75-78．

HU Wu-sheng，GAO Cheng-fa.GPS Measurement Principle and Application [M]．Beijing：China Communications Press，2002：75-78．

[5]劉基余．GPS衛(wèi)星導(dǎo)航定位原理與方法[M]．北京：科學(xué)出版社，2003：216-317．LIU Ji-yu．GPS Satellite Navigation and Positioning Principle and Method [M]．Beijing：Science Press，2003：216-317．

[6]李天文．GPS原理及應(yīng)用[M]．北京：科學(xué)出版社，2003：229-238．

LI Tian-wen．GPS Principle and Application [M]．Beijing：Science Press，2003：229-238．

[7]王愛朝，張琰．GPS動態(tài)定位的初始化[J]．測繪通報，1994，15(6)：3-6．

WANG Ai-zhao，ZHANG Yan．GPS Dynamic Positioning to Initialize [J]．Bulletin of Surveying and Mapping，1994，15(6)：3-6．

[8]王仁謙．GPS動態(tài)定位的理論研究[D]．長沙：中南大學(xué)，2004：7-10．

WANG Ren-qian．Theoretical Study of GPS Dynamic Positioning [D]．Changsha：Central South Univercity，2004：7-10．

[9]李躍，邱致和．導(dǎo)航與定位[M]．北京：國防工業(yè)出版社，2008：193．

LI Yue，QIU Zhi-he．The Navigation and Location [M]．Beijing：National Defence Industry Press，2008：193．

[10]張守信．GPS技術(shù)與應(yīng)用[M]．北京：國防工業(yè)出版社，2004：101-120．

ZHANG Shou-xin．GPS Technology and Application [M]．Beijing：National Defence Industry Press，2004：101-120．

[11]葉曉清．機(jī)載GPS實時精密定位技術(shù)研究[D]．西安：西北工業(yè)大學(xué)，2004：4-8．

YE Xiao-qing．Research on Real Time Precise Positioning Technology of Airborne GPS [D]．Xian Northwestern Polytechnical University，2004：4-8．

[12]程鵬飛，蔡艷輝，文漢江，等．全球衛(wèi)星導(dǎo)航系統(tǒng)GPS，GLONASS，Galileo及其他系統(tǒng)[M]．北京：測繪出版社，2007：209-211．

CHENG Peng-fei，CAI Yan-hui，WEN Han-jiang，et al．Global Navigation System GPS，GLONASS，Galileo and Other Systems[M]．Beijing: Mapping Press，2007：209-211．

[13]KALMAN R E．A New Approach to Linear Filtering and Prediction Problems[J]．Transaction of the ASME-Journal of Basic Engineering，1960，9(4)：35-45．

[14]傅夢印．Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用[M]．北京：科學(xué)出版社，2003．

FU Meng-yin．Kalman Filtering Theory and Its Application in Navigation System[M]．Beijing：Science Press，2003．

[15]柳響林，劉經(jīng)南，杜道生．卡爾曼濾波可靠性分析及其在動態(tài)GPS定位中的應(yīng)用[J]．武漢測繪科技大學(xué)學(xué)報，1997，22(3)：234-237．

LIU Xiang-lin，LIU Jing-nan，DU Dao-sheng．Reliability Analysis of Calman Filter and Its Application in the Dynamic GPS Positioning[J]．Journal of Wuhan University of Science and Technology of Surveying and Mapping，1997，22 (3)：234-237．

An Algorithm of Dynamic Relative Position Based on Calman Filtering

SUN Hai-wen，OU-YANG Zhong-hui

(Naval Aeronautical and Astronautical University，Department of Ordnance Science and Technology，Shandong Yantai 264001，China)

Abstract:In the process of ship flying school, to compute the relative position of aircraft and ships will have a certain error. Direct algorithm of gross error and loss of GPS signaling processing make error in the calculation results. A relative position improved algorithm is proposed. Through the simulation experiment, for the three aspects of the azimuth angle, pitching angle and distance, the precision of calculation accuracy and direct algorithm improved algorithm are analyzed and compared. The simulation results show that, the improved algorithm can improve the relative position accuracy very much.

Key words:DGPS；carrier phase；dynamic relative positioning；direct method；algorithm of dynamic relative position based on calman filtering；measurement accuracy

*收稿日期：2015-05-04；修回日期：2015-07-07

作者簡介：孫海文(1990-)，男，山東煙臺人。碩士生，研究方向為GPS標(biāo)校。

通信地址：264001山東煙臺芝罘區(qū)二馬路188號二系1007室E-mail：m15666156198@163.com

doi:10.3969/j.issn.1009-086x.2016.03.006

中圖分類號：TN967.1；TN713；TP312

文獻(xiàn)標(biāo)志碼：A

文章編號：1009-086X(2016)-03-0032-06

導(dǎo)航、制導(dǎo)與控制