航位推算在導航定位中的應用

1、航位推算在導航定位中的應用摘要：只限于GPS導航往往會有盲點，DR導航的自主性恰能彌補GPS導航的 不足。本文闡述了在得到一組GPS導航數(shù)據(jù)的基礎上如何利用航位推算進行導 航。通過卡爾曼濾波對航向角數(shù)據(jù)進行處理，比較濾波前后載體的運動軌跡， 分析產(chǎn)生誤差的主要原因。結(jié)果表明：matlab仿真出的載體運動軌跡與實驗時 的羅盤方向軌跡相同，利用卡爾曼濾波能夠有效減小誤差。1引言DR導航時一種自主式導航，成本相對GPS較低，在短時間內(nèi)能夠提供比 較精確的導航參數(shù)，抗干擾能力強，隱蔽性好。但是誤差隨導航時間積累是 DR導航的致命缺點，因此DR導航不適合長時間的單獨導航。GPS導航精度 高、功能強、使用

2、方便，但在應用于車輛導航時，當車輛在樹蔭下、涵洞、隧 道里、深山峽谷內(nèi)，特別在城市里行駛時，高樓大廈會擋住衛(wèi)星的信號，使衛(wèi) 星導航接收機不能正常接收衛(wèi)星信號，不能正常定位1。由此可見，將兩種導 航方式結(jié)合起來共同完成導航功能是提高導航精度的一個主要手段，具有重大 意義?？柭鼮V波在導航方面有很重要的作用，最傳統(tǒng)的導航融合是集中式 Kalman濾波，現(xiàn)在應用比較廣泛的是自適應擴展卡爾曼濾波，利用系統(tǒng)預測 量、系統(tǒng)狀態(tài)噪聲量、觀測量與觀測噪聲幾個量實現(xiàn)濾波，在信息融合中，利 用信息分配原理，自適應調(diào)整GPS和DR觀測量在濾波中的權(quán)重。自適應濾波 根據(jù)自適應因子作用范圍不同分為單因子自適應濾波和多因

3、子自適應濾波。圖1航位推算原理2航位推算原理航位推算原理是利用載體的速度、航向以及上一時刻的位置來估計下一時 刻載體的位置。利用載體的速度和航向能夠得出速度在當?shù)厮阶鴺溯S上分向 速度，將分向速度和載體所經(jīng)過的時間相乘便可以得到載體在坐標軸上增加的 坐標值，與前一時刻的坐標值求和便可以得到此時的坐標值。原理圖如圖1所 示。推算如公式(1)。錯誤！未找到引用源。 （1）其中，（錯誤！未找到引用源。）為載體在錯誤！未找到引用源。時刻的初始位 置，（錯誤！未找到引用源。）為載體在錯誤！未找到引用源。時刻位置，錯 誤！未找到引用源。、錯誤！未找到引用源。為載體在錯誤！未找到引用源。時刻的速度與航向角（

4、航向角為載體運動方向與正東方向的夾角）。錯誤！未找 到引用源。為羅盤相鄰兩次推算所用的時間差。3坐標系之間的轉(zhuǎn)化3.1所涉坐標系的種類3.1.1當?shù)厮阶鴺讼诞數(shù)厮阶鴺讼涤置麞|北天坐標系，它的坐標原點為載體的質(zhì)心，X軸指 向當?shù)乇弊游缇€，Y軸垂直X軸指向東，Z軸垂直XOY面構(gòu)成右手直角坐標 系。3.1.2大地坐標系地球橢球的中心與地球質(zhì)心重合，橢球的短軸與地球的自轉(zhuǎn)軸重合，大地 緯度B為地面點的橢球法線與橢球赤道面的夾角，大地經(jīng)度L為過地面點的橢 球子午面與格林威治起始子午面的夾角，大地高H為地面點沿橢球法線至橢球 面的距離。地面一點可表示為（L,B,H）。如圖3.1所示。圖3.1大地坐標系

5、B:大地緯度，通過觀測站的參考橢球面的法線與赤道面的夾角。L:大地經(jīng)度，通過觀測站的大地子午面與本初子午面得夾角。H:大地高度，觀測站沿法線到參考橢球面的距離。3.1.3 WGS-84 坐標系GPS使用的坐標系，該坐標系屬于協(xié)議地球坐標系，其原點位于地球質(zhì) 心，Z軸指向BIH1984.0定義的協(xié)議地級方向，X軸指向BIH1984.0的零子午面和 CTP赤道的交點，Y軸與Z軸構(gòu)成右手坐標系。GPS定位結(jié)果屬于協(xié)議地球地心坐標系，即84坐標系，且通常以空間直角坐標（X, Y, Z）,或以橢球大地 坐標（B,L,H ）的形式給出。而實用的常規(guī)地面測量成果或是屬于國家的參心大 地坐標系，或是屬于地方獨

6、立坐標系。因此必須實現(xiàn) GPS成果的坐標系的轉(zhuǎn) 換。3.2坐標系轉(zhuǎn)換3.2.1當?shù)厮阶鴺讼缔D(zhuǎn)化為空間直角坐標系當?shù)厮阶鴺讼蹬c84坐標系同屬于直角坐標系，故相互轉(zhuǎn)化時需要利用地 心大地坐標系的參數(shù)，將84坐標系下（以大地坐標（B,L,H）測得的GPS量 作為載體的質(zhì)心，然后經(jīng)過旋轉(zhuǎn)矩陣將東北天坐標系（當?shù)厮阶鴺讼?、站?坐標系）轉(zhuǎn)化為空間直角坐標系。旋轉(zhuǎn)點作為載體的質(zhì)心，以（B,L,H）的值為 起點。將坐標系分別繞Z、X、Y軸逆時針旋轉(zhuǎn)a、0、y角所使用的旋轉(zhuǎn)矩陣如 下： TOC o 1-5 h z 錯誤！未找到引用源。=錯誤！未找到引用源。（2）錯誤！未找到引用源。=錯誤！未找到引用源。

7、（3）錯誤！未找到引用源。=錯誤！未找到引用源。（4）由于航位推算是在東北天坐標系下進行的，所以需要需變換到大地坐標系下， 顯示經(jīng)緯度信息，先將東北天坐標系轉(zhuǎn)換到直角坐標系下，然后再有直角坐標 系轉(zhuǎn)化為大地坐標系，將在84坐標系（大地坐標（B、L、H）形式表示）下測 得的GPS值轉(zhuǎn)化為直角坐標系（X、Y、Z）下表示，作為直角坐標系下初值， 轉(zhuǎn)化公式如下：錯誤！未找到引用源。（5）現(xiàn)將Y軸反向得錯誤！未找到引用源。軸。繞錯誤！未找到引用源。軸逆時針 旋轉(zhuǎn)錯誤！未找到引用源。），再繞Z軸旋轉(zhuǎn)錯誤！未找到引用源。），即可將 東北天坐標系（地平站心坐標系）轉(zhuǎn)化為站心赤道直角坐標系，公式如下：錯誤！未找

8、到引用源。R=錯誤！未找到引用源。=錯誤！未找到引用源。*錯誤！未找到引用源。*錯誤！未找到引用源。=錯誤！未找到引用源。-sinBcasL sinBsinL cosBsinLcosL-sinBcasL sinBsinL cosBsinLcosLcos B cost cosBsinL sinB空間直角坐標系和站心赤道直角坐標系之間有簡單的平移關系，所以將站心赤 道坐標系轉(zhuǎn)化為空間直角坐標系之間的轉(zhuǎn)化關系為：錯誤！未找到引用源。=錯誤！未找到引用源。所以與東北天坐標系之間的轉(zhuǎn)化關系為：錯誤！未找到引用源。=錯誤！未找到引用源。（8）3.2.2直角坐標系轉(zhuǎn)化為大地坐標系直角坐標系轉(zhuǎn)化為大地坐標系的

9、轉(zhuǎn)化公式為：錯誤！未找到引用源。（8）其中：錯誤！未找到引用源。，N為該點的卯酉圈曲率半徑；錯誤！未找到引 用源。，a、b、e分別為該大地坐標系對應參考橢球的長半軸、短半軸和第一偏 心率。長半軸 a=6378137 + 2m， 短半軸 b=6356.7523142km， e2 = 0.00669437999013。此方法H和B需要迭代求得，而且應用條件受到很大限制，如要求大地高 度需要小于1000m時，才能使B、L迭代四次之后精度分別達到錯誤！未找到 引用源。和0.001m，所以本文一種計算精度高，公式形式簡便的方法來計算B 和 L4。錯誤！未找到引用源。（9）4電子羅盤HMR30004.1概

10、述HMR3000包含三個磁阻傳感器和一個雙軸的液體橫滾傳感器，并帶有A/D 轉(zhuǎn)換電路和微處理器。微處理器控制傳感器的測量順序，控制器操作的參數(shù)存 儲在EEPROM中，輸出3類NMEA0813標準語句（HDG、HDT和XDR），3類 專用語句（HPR、RCD和CCD），HPR是最常用的格式，采用RS232接口功能模 塊如圖4-1所示。圖4-1 HMR3000功能框圖4.2 HMR3000 特性數(shù)字電子羅盤HMR3000是Honeywell公司的產(chǎn)品。它可以為導航定位系統(tǒng) 提供航向、俯仰、橫滾等數(shù)據(jù)。其中航向的精度為土錯誤！未找到引用源。，分 辨率可達土錯誤！未找到引用源。橫滾和俯仰的精度為土錯誤

11、！未找到引用 源。，分辨率為土錯誤！未找到引用源。，輸出端口采用RS232。HMR3000的 串行通信是根據(jù)NMEA0183標準制定的簡單、異步的ASCII協(xié)議。ASCII碼 的傳輸和接收使用1位起始位、8位數(shù)據(jù)位，無奇偶校驗位和1位停止位。每一 個碼有10位。波特率可選擇1200,2400,4800,9600或38400。HMR3000支持NMEA0183和專用的信息。在測量模式中有HMR3000按照 EEPROM中的編程的速率主動發(fā)送NMEA信息。本次試驗輸出是專用HPR句 子，它包含了航向、俯仰、橫滾等信息。格式如下：HPR Heading, Pitch & Roll （航向，俯仰和橫滾

12、）$PTNTHPR,x.x,a,x.x,a,x.x,a*hh 這個句子把HMR3000的三個重要的測量結(jié)果和有用的狀態(tài)、俯仰、俯仰狀 態(tài)、橫滾、橫滾狀態(tài)。如本次試驗的一個句子為：$PTNTHPR,199.3,N,0.3,N,0.2,N*37此句說明：羅盤所測航向角為：錯誤！未找到引用源。，正常狀態(tài)，俯仰角為 錯誤！未找到引用源。，正常狀態(tài)，橫滾角為錯誤！未找到引用源。，正常狀 態(tài)。5卡爾曼濾波1960年，R.E.Kalman首次提出的卡爾曼濾波是一種線性最小方差估計?？?爾曼濾波理論成功地解決了美國阿波羅登月中的多導航傳感器組合導航的難 題，引起工程界的廣泛重視。之后，卡爾曼濾波理論開始在導航

13、系統(tǒng)中得到普 遍的應用。卡爾曼濾波的基本思想：以最小均方誤差為估計準則，采用信號與 誤差的狀態(tài)空間模型，利用前一時刻的估計值和當前的觀測值來更新對狀態(tài)變 量的估計，求出當前時刻的估計值，算法根據(jù)建立的系統(tǒng)方程和觀測方程對需 要處理的信號做出滿足最小均方誤差的的估計。由于卡爾曼濾波方程具有易編程的優(yōu)點，所以卡爾曼濾波器在導航方面應 用十分廣泛，且能夠很好地提高導航的精度。卡爾曼濾波是一種最優(yōu)化自回歸 數(shù)據(jù)處理方式，在了解狀態(tài)方程、觀測方程以及系統(tǒng)噪聲的情況下，通過卡爾 曼濾波的五個公式實現(xiàn)羅盤航向角的最優(yōu)化回歸處理，將k-1時刻的預測值以 及預測偏差和k時刻的量測值以及量測偏差結(jié)合起來，通過計算

14、卡爾曼增益將 這5個式子結(jié)合起來實現(xiàn)遞推。下面詳細介紹一下這五個式子的含義：首先我們要根據(jù)k-1時刻的航向角來預測k時刻的航向角，根據(jù)k-1時刻的 狀態(tài)方程可以得到k時刻的預測航向角如公式(10)：錯誤！未找到引用源。(10)其中錯誤！未找到引用源。是根據(jù)k-1時刻航向角預測k時刻的航向角，A、B 為系統(tǒng)參數(shù)矩陣，錯誤！未找到引用源。為k-1時刻的航向角，錯誤！未找到 引用源。為控制量，此時為零向量。我們用錯誤！未找到引用源。(covariance) 來表示通過k-1時刻方差陣預測的k時刻方差陣(預測值的偏差)，錯誤！未 找到引用源。表示k-1時刻的航向角的偏差，他們的關系如公式(11)： T

15、OC o 1-5 h z 錯誤！未找到引用源。(11)Q為過程噪聲的covariance，假設k時刻航向角的量測值為錯誤！未找到引用 源。，現(xiàn)在我們可以得到k時刻的最優(yōu)化航向角估計值如公式(12)：錯誤！未找到引用源。(12)H為測量系統(tǒng)參數(shù)，錯誤！未找到引用源。為k時刻的卡爾曼增益，求解如公 式13：錯誤！未找到引用源。(13)R為觀測噪聲的covariance，為了能夠以此往下面迭代，故必須求出k時刻航向 角的偏差，即求出錯誤！未找到引用源。如公式(14)：錯誤！未找到引用源。(1-錯誤！未找到引用源。)*錯誤！未找到引用 源。(14)然后再根據(jù)k時刻的參數(shù)值求解k+1時刻的航向角，這樣就

16、可以實時求解 航向角的大小。至于編程時錯誤！未找到引用源。初始參數(shù)的確定可以任意確 定一個就行，因為卡爾曼濾波算法會使這個參數(shù)逐漸收斂的，但是錯誤！未找 到引用源。最好別取零，防止卡爾曼濾波把這個當成最優(yōu)解。實驗中，采用卡爾曼濾波器對羅盤輸出的航向值進行濾波，濾波器的輸出 如圖5.1所示。35030025020015010050 0-50010002000300040005000600070008000900010000圖5.1航向角濾波前后對比濾波后航向角的相對誤差如圖5.2所示。18圖5.2濾波數(shù)據(jù)與原始數(shù)據(jù)相對誤差從圖5.1可以看出，在濾波時給出的初值和剛開始的航向角值差別比較 大，卡爾

17、曼濾波能很快收斂到精確的航向角，在羅盤每次改變方向的時候，卡 爾曼濾波因為要根據(jù)預測量和測量值來估計下一時刻值，所以濾波時航向角失 調(diào)量要大些。6實驗、數(shù)據(jù)處理及其仿真6.1實驗內(nèi)容（1）用NT3000GPS接收板卡測得五分鐘的數(shù)據(jù)，得到一組實驗場地的經(jīng)緯度 信息，然后取平均得到一組精度相對較高的初始經(jīng)緯度信息。通過串口調(diào)試助 手觀測并保存GPS數(shù)據(jù)為“txt”格式。（2）用HMR3000電子羅盤繞一周測4個方向的航向角信息，每個方向大概測 5分鐘，通過串口調(diào)試助手觀測并保存羅盤數(shù)據(jù)為“txt”格式。6.2數(shù)據(jù)處理（1）將GPS測得經(jīng)緯度信息取平均得到初始B=32.0304，L=118.857

18、4，H=5.3。（2）用matlab讀取羅盤信息，在東北天坐標系下進行航位推算，因為磁偏角 的存在使電子羅盤存在誤差，所以將測得的航向角加上當?shù)氐拇牌潜愕玫綄?際的航向角（南京的磁偏角為5），我們這組我取速度為4 m/s,采樣時間用四 組的平均采用時間，即錯誤！未找到引用源。/4（15）錯誤！未找到引用源。=（0.1170+0.1157+0.1172+0.1170）/4=0.1167按DR算法可以得到如圖6.1所示運動軌跡圖：圖6.1東北天坐標系下運動軌跡從上圖可以看出用濾波后的數(shù)據(jù)進行航位推算得到的軌跡與原始數(shù)據(jù)進行 航位推算得到的軌跡大致相同。說明在短時間內(nèi)羅盤的導航精度很高。（3）將東

19、北天坐標系轉(zhuǎn)化為直角坐標系，在直角坐標系中推算的軌跡如圖6.2 所示，因為X-Y面更能清楚的反映運動情況，故采用平面直角坐標系。4.7402 -2.6128-2.6126-2.6124-2.6122-2.612 -2.6118-2.6116-2.6114-2.6112-2.611x 10 4.7402 -2.6128-2.6126-2.6124-2.6122-2.612 -2.6118-2.6116-2.6114-2.6112-2.611x 10 6X/（m）圖6.2直角空間坐標系X-Y面然后再將直角空間坐標系轉(zhuǎn)換為大地坐標系，在大地坐標經(jīng)緯度面的軌跡如圖 6.3所示。圖6.3大地坐標系經(jīng)緯度

20、面在大地坐標系（B,L,H）下的軌跡如圖6.4所示。大地坐標系X-Y-Z-1.1763-1.1764-1.1765-1.176732.04118.85118.845經(jīng)度緯度-1.1768118.865度高-1.1766大地坐標系X-Y-Z-1.1763-1.1764-1.1765-1.176732.04118.85118.845經(jīng)度緯度-1.1768118.865度高-1.176632.02532.0232.015圖6.4大地坐標系X-Y-Z7誤差分析7.1電子羅盤誤差電子羅盤的誤差具體可以分為兩類：系統(tǒng)誤差和外界環(huán)境誤差。系統(tǒng)誤差 包括羅盤自身的器件精度誤差以及安裝誤差，主要受羅盤內(nèi)部的三軸

21、磁阻傳感 器影響。外界環(huán)境誤差是指羅盤工作的環(huán)境所帶來的誤差，如載體傾斜帶來的 誤差、磁羅差誤差等。本實驗中羅盤放在計算機、示波器和工控機旁邊易受到 干擾，所以羅差對羅盤的精度有很大的影響。雖然濾波后得到的軌跡比濾波前 精確些，但是羅盤每個方向的具體方位角并不精確，若想提高每個方向的方位 角精度，最好能對羅盤進行補償，可以在因為載體傾斜帶來的誤差和磁羅差方 面分別進行補償，因為本實驗中沒有理想的精確的方位角理論值，所以沒有對 羅盤進行補償，只對羅盤轉(zhuǎn)動的軌跡進行了仿真。7.2航位推算本身誤差航位推算一個致命的弱點就是誤差隨著時間而積累。所以推算的航向角是 發(fā)散的，航向角誤差越來越大。因此航位推

22、算不能長時間的單獨工作，必須借 助于其他的定位方式進行組合導航，比如GPS/DR組合導航，航位推算借助于 GPS定期傳來的載體位置進行聯(lián)合導航，精度更加精確。8總結(jié)在得到一組GPS導航數(shù)據(jù)的基礎上利用航位推算進行導航，通過卡爾曼濾 波對航向角數(shù)據(jù)進行處理，減小誤差。利用 matlab仿真出分別在東北天坐標 系、直角坐標系、大地坐標系下的運動軌跡，載體運動軌跡與實驗時的羅盤向 軌跡相同。最后分析了航位推算導航定位的誤差原因，并且希望以后在羅盤補 償方面能做更進一步的研究。參考文獻1.孫希延，紀元法與施滸立,卡爾曼濾波實現(xiàn)車載GPS/DR組合導航.現(xiàn)代電子技 術(shù),2006(11):第 149-15

23、2頁.吳富梅，楊元喜與田育民,GPS/DR組合導航自適應Kalman濾波算法.測繪科學 技術(shù)學報,2008(3):第206-208+212頁.王珂,GPS測向算法與應用研究,2009,重慶大學.張華海，鄭南山，王軍，李景芝.由空間直角坐標計算大地坐標的簡便公式. 全球定位系統(tǒng).2002，4:9-12.彭樹生,數(shù)字電子羅盤HMR3000的特性及應用.電子技術(shù),2004(8):第39-43頁.付夢印，鄧忠紅，張繼偉.Kalman濾波理論及其在導航系統(tǒng)中的應用M. 北京:科學出版社，2003.附錄clear all;load head.matZ=heading;m,n=size(Z);T=0.116

24、7;A=1 T;0 1;%系統(tǒng)參數(shù)矩陣(狀態(tài)轉(zhuǎn)移矩陣)H=1 0; %測量系統(tǒng)參數(shù)X=100;0; %狀態(tài)變量初值C=diag(1,1);%誤差協(xié)方差初值Rk=diag(1);%量測噪聲方差Ta=T;1;%狀態(tài)過程噪聲陣sigam=1;Qk=Ta*Ta*sigam人2;%過程噪聲方差初值f=Z(:,1);for k=1:mXk=A*X;%從k-1時刻預測k時刻Ck=A*C*A+Qk;%k-1時刻預測k時刻方差K=Ck*H*inv(H*Ck*H+Rk);%kalman 增益X=Xk+K*(Z(k,:)-H*Xk);% 狀態(tài)更新C=Ck-K*H*Ck;% 方差更新Xe(:,k)=X;%濾波輸出en

25、dfigure(1)plot(Xe(1,:),r);hold on;plot(heading,b);grid on;hold on;B=32.0304;L=118.8574;H=5.3;e2=0.00673949674227;format shorte1=(6378140人2-6356755人2)/6378140人2;N=6378140/(sqrt(1-e1*(sind(B)人2);X=(N+H)*cosd(B)*cosd(L);Y=(N+H)*cosd(B)*sind(L);Z=(N*(1-e1)+H)*sind(B);c=Xe(1,:);n3 n4=size(Xe);for i=1:(n4+1)if i=1x2(i)=0;y2(i)=0;elsed1=c(i-1)+5;x2(i)=x2(i-1)+4*0.1167*cosd(d1);y2(i)=y2(i-1)+4*0.1167*sind(d1);endendfor i=1:(n4+1)if i=1x3(i)=0;y3(i)=0;el