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

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

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

?

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

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

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

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

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

0引言

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

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

1載波相位相對(duì)定位分析

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

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

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

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

(1)

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

(2)

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

(3)

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

(4)

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

(5)

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

以用戶(hù)站天線(xiàn)坐標(biāo)為例

(6)

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

(7)

式中：

(8)

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

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

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

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

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

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

(9)

觀測(cè)方程：Zk=HkXk+Vk，

(10)

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

(11)

干擾矩陣Γk-1為

(12)

(13)

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

(14)

(15)

(16)

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

(17)

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

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

(18)

(19)

(20)

(21)

4仿真試驗(yàn)

4.1仿真條件建立

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

(22)

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

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

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

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

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

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

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

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

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

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

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

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

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

5結(jié)束語(yǔ)

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

參考文獻(xiàn)：

[1]劉大杰，施一民，過(guò)靜珺．全球定位系統(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)星測(cè)量原理與應(yīng)用 [M]．修訂版．北京：測(cè)繪出版社，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測(cè)量原理及應(yīng)用[M]．北京：科學(xué)出版社，2005．

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

[4]胡伍生，高成發(fā)．GPS測(cè)量原理及其應(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]王愛(ài)朝，張琰．GPS動(dòng)態(tài)定位的初始化[J]．測(cè)繪通報(bào)，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動(dòng)態(tài)定位的理論研究[D]．長(zhǎng)沙：中南大學(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]．北京：國(guó)防工業(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]．北京：國(guó)防工業(yè)出版社，2004：101-120．

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

[11]葉曉清．機(jī)載GPS實(shí)時(shí)精密定位技術(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]．北京：測(cè)繪出版社，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]傅夢(mèng)?。甂alman濾波理論及其在導(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)南，杜道生．卡爾曼濾波可靠性分析及其在動(dòng)態(tài)GPS定位中的應(yīng)用[J]．武漢測(cè)繪科技大學(xué)學(xué)報(bào)，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

作者簡(jiǎn)介：孫海文(1990-)，男，山東煙臺(tái)人。碩士生，研究方向?yàn)镚PS標(biāo)校。

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

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

中圖分類(lèi)號(hào)：TN967.1；TN713；TP312

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

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

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