航位推算在導(dǎo)航定位中的應(yīng)用

航位推算在導(dǎo)航定位中的應(yīng)用摘要：只限于GPS導(dǎo)航往往會有盲點，DR導(dǎo)航的自主性恰能彌補(bǔ)GPS導(dǎo)航的不足。本文闡述了在得到一組GPS導(dǎo)航數(shù)據(jù)的基礎(chǔ)上如何利用航位推算進(jìn)行導(dǎo)航。通過卡爾曼濾波對航向角數(shù)據(jù)進(jìn)行處理，比較濾波前后載體的運動軌跡，分析產(chǎn)生誤差的主要原因。結(jié)果表明：matlab仿真出的載體運動軌跡與實驗時的羅盤方向軌跡相同，利用卡爾曼濾波能夠有效減小誤差。1引言DR導(dǎo)航時一種自主式導(dǎo)航，成本相對GPS較低，在短時間內(nèi)能夠提供比較精確的導(dǎo)航參數(shù)，抗干擾能力強(qiáng)，隱蔽性好。但是誤差隨導(dǎo)航時間積累是DR導(dǎo)航的致命缺點，因此DR導(dǎo)航不適合長時間的單獨導(dǎo)航。GPS導(dǎo)航精度高、功能強(qiáng)、使用方便，但在應(yīng)用于車輛導(dǎo)航時，當(dāng)車輛在樹蔭下、涵洞、隧道里、深山峽谷內(nèi)，特別在城市里行駛時，高樓大廈會擋住衛(wèi)星的信號，使衛(wèi)星導(dǎo)航接收機(jī)不能正常接收衛(wèi)星信號，不能正常定位［1］。由此可見，將兩種導(dǎo)航方式結(jié)合起來共同完成導(dǎo)航功能是提高導(dǎo)航精度的一個主要手段，具有重大意義?？柭鼮V波在導(dǎo)航方面有很重要的作用，最傳統(tǒng)的導(dǎo)航融合是集中式Kalman濾波，現(xiàn)在應(yīng)用比較廣泛的是自適應(yīng)擴(kuò)展卡爾曼濾波，利用系統(tǒng)預(yù)測量、系統(tǒng)狀態(tài)噪聲量、觀測量與觀測噪聲幾個量實現(xiàn)濾波，在信息融合中，利用信息分配原理，自適應(yīng)調(diào)整GPS和DR觀測量在濾波中的權(quán)重。自適應(yīng)濾波根據(jù)自適應(yīng)因子作用范圍不同分為單因子自適應(yīng)濾波和多因子自適應(yīng)濾波⑵。圖1航位推算原理2航位推算原理航位推算原理是利用載體的速度、航向以及上一時刻的位置來估計下一時刻載體的位置。利用載體的速度和航向能夠得出速度在當(dāng)?shù)厮阶鴺?biāo)軸上分向速度，將分向速度和載體所經(jīng)過的時間相乘便可以得到載體在坐標(biāo)軸上增加的坐標(biāo)值，與前一時刻的坐標(biāo)值求和便可以得到此時的坐標(biāo)值。原理圖如圖1所示。推算如公式(1)。錯誤！未找到引用源。（1）其中，（錯誤！未找到引用源。）為載體在錯誤！未找到引用源。時刻的初始位置，（錯誤！未找到引用源。）為載體在錯誤！未找到引用源。時刻位置，錯誤！未找到引用源。、錯誤！未找到引用源。為載體在錯誤！未找到引用源。時刻的速度與航向角（航向角為載體運動方向與正東方向的夾角）。錯誤！未找到引用源。為羅盤相鄰兩次推算所用的時間差。3坐標(biāo)系之間的轉(zhuǎn)化3.1所涉坐標(biāo)系的種類3.1.1當(dāng)?shù)厮阶鴺?biāo)系當(dāng)?shù)厮阶鴺?biāo)系又名東北天坐標(biāo)系，它的坐標(biāo)原點為載體的質(zhì)心，X軸指向當(dāng)?shù)乇弊游缇€，Y軸垂直X軸指向東，Z軸垂直XOY面構(gòu)成右手直角坐標(biāo)系。3.1.2大地坐標(biāo)系地球橢球的中心與地球質(zhì)心重合，橢球的短軸與地球的自轉(zhuǎn)軸重合，大地緯度B為地面點的橢球法線與橢球赤道面的夾角，大地經(jīng)度L為過地面點的橢球子午面與格林威治起始子午面的夾角，大地高H為地面點沿橢球法線至橢球面的距離。地面一點可表示為（L,B,H）。如圖3.1所示。圖3.1大地坐標(biāo)系B:大地緯度，通過觀測站的參考橢球面的法線與赤道面的夾角。L:大地經(jīng)度，通過觀測站的大地子午面與本初子午面得夾角。H:大地高度，觀測站沿法線到參考橢球面的距離。3.1.3WGS-84坐標(biāo)系GPS使用的坐標(biāo)系，該坐標(biāo)系屬于協(xié)議地球坐標(biāo)系，其原點位于地球質(zhì)心，Z軸指向BIH1984.0定義的協(xié)議地級方向，X軸指向BIH1984.0的零子午面和CTP赤道的交點，Y軸與Z軸構(gòu)成右手坐標(biāo)系⑶。GPS定位結(jié)果屬于協(xié)議地球地

心坐標(biāo)系，即84坐標(biāo)系，且通常以空間直角坐標(biāo)（X,Y,Z）,或以橢球大地坐標(biāo)（B,L,H）的形式給出。而實用的常規(guī)地面測量成果或是屬于國家的參心大地坐標(biāo)系，或是屬于地方獨立坐標(biāo)系。因此必須實現(xiàn)GPS成果的坐標(biāo)系的轉(zhuǎn)換。3.2坐標(biāo)系轉(zhuǎn)換3.2.1當(dāng)?shù)厮阶鴺?biāo)系轉(zhuǎn)化為空間直角坐標(biāo)系當(dāng)?shù)厮阶鴺?biāo)系與84坐標(biāo)系同屬于直角坐標(biāo)系，故相互轉(zhuǎn)化時需要利用地心大地坐標(biāo)系的參數(shù)，將84坐標(biāo)系下（以大地坐標(biāo)（B,L,H））測得的GPS量作為載體的質(zhì)心，然后經(jīng)過旋轉(zhuǎn)矩陣將東北天坐標(biāo)系（當(dāng)?shù)厮阶鴺?biāo)系、站心坐標(biāo)系）轉(zhuǎn)化為空間直角坐標(biāo)系。旋轉(zhuǎn)點作為載體的質(zhì)心，以（B,L,H）的值為起點。將坐標(biāo)系分別繞Z、X、Y軸逆時針旋轉(zhuǎn)a、0、y角所使用的旋轉(zhuǎn)矩陣如下：TOC\o"1-5"\h\z錯誤！未找到引用源。=錯誤！未找到引用源。（2）錯誤！未找到引用源。=錯誤！未找到引用源。（3）錯誤！未找到引用源。=錯誤！未找到引用源。（4）由于航位推算是在東北天坐標(biāo)系下進(jìn)行的，所以需要需變換到大地坐標(biāo)系下，顯示經(jīng)緯度信息，先將東北天坐標(biāo)系轉(zhuǎn)換到直角坐標(biāo)系下，然后再有直角坐標(biāo)系轉(zhuǎn)化為大地坐標(biāo)系，將在84坐標(biāo)系（大地坐標(biāo)（B、L、H）形式表示）下測得的GPS值轉(zhuǎn)化為直角坐標(biāo)系（X、Y、Z）下表示，作為直角坐標(biāo)系下初值，轉(zhuǎn)化公式如下：錯誤！未找到引用源。（5）現(xiàn)將Y軸反向得錯誤！未找到引用源。軸。繞錯誤！未找到引用源。軸逆時針旋轉(zhuǎn)錯誤！未找到引用源。），再繞Z軸旋轉(zhuǎn)錯誤！未找到引用源。），即可將東北天坐標(biāo)系（地平站心坐標(biāo)系）轉(zhuǎn)化為站心赤道直角坐標(biāo)系，公式如下：錯誤！未找到引用源。⑹R=錯誤！未找到引用源。=錯誤！未找到引用源。*錯誤！未找到引用源。*錯誤！未找到引用源。=錯誤！未找到引用源。-sinBcasL—sinBsinLcosB—sinL

cosLcos-sinBcasL—sinBsinLcosB—sinL

cosLcosBcostcosBsinLsinB錯誤！未找到引用源。=錯誤！未找到引用源。⑺所以與東北天坐標(biāo)系之間的轉(zhuǎn)化關(guān)系為：錯誤！未找到引用源。=錯誤！未找到引用源。（8）3.2.2直角坐標(biāo)系轉(zhuǎn)化為大地坐標(biāo)系直角坐標(biāo)系轉(zhuǎn)化為大地坐標(biāo)系的轉(zhuǎn)化公式為：錯誤！未找到引用源。（8）其中：錯誤！未找到引用源。，N為該點的卯酉圈曲率半徑；錯誤！未找到引用源。，a、b、e分別為該大地坐標(biāo)系對應(yīng)參考橢球的長半軸、短半軸和第一偏心率。長半軸a=6378137+2m，短半軸b=6356.7523142km，e2=0.00669437999013。此方法H和B需要迭代求得，而且應(yīng)用條件受到很大限制，如要求大地高度需要小于1000m時，才能使B、L迭代四次之后精度分別達(dá)到錯誤！未找到引用源。和0.001m，所以本文一種計算精度高，公式形式簡便的方法來計算B和L[4]。錯誤！未找到引用源。（9）4電子羅盤HMR30004.1概述HMR3000包含三個磁阻傳感器和一個雙軸的液體橫滾傳感器，并帶有A/D轉(zhuǎn)換電路和微處理器。微處理器控制傳感器的測量順序，控制器操作的參數(shù)存儲在EEPROM中，輸出3類NMEA0813標(biāo)準(zhǔn)語句（HDG、HDT和XDR），3類專用語句（HPR、RCD和CCD），HPR是最常用的格式，采用RS232接口功能模塊如圖4-1所示。圖4-1HMR3000功能框圖4.2HMR3000特性數(shù)字電子羅盤HMR3000是Honeywell公司的產(chǎn)品。它可以為導(dǎo)航定位系統(tǒng)提供航向、俯仰、橫滾等數(shù)據(jù)。其中航向的精度為土錯誤！未找到引用源。，分辨率可達(dá)土錯誤！未找到引用源。。橫滾和俯仰的精度為土錯誤！未找到引用源。，分辨率為土錯誤！未找到引用源。，輸出端口采用RS232。HMR3000的串行通信是根據(jù)NMEA0183標(biāo)準(zhǔn)制定的簡單、異步的ASCII協(xié)議⑸。ASCII碼的傳輸和接收使用1位起始位、8位數(shù)據(jù)位，無奇偶校驗位和1位停止位。每一個碼有10位。波特率可選擇1200,2400,4800,9600或38400。HMR3000支持NMEA0183和專用的信息。在測量模式中有HMR3000按照EEPROM中的編程的速率主動發(fā)送NMEA信息。本次試驗輸出是專用HPR句子，它包含了航向、俯仰、橫滾等信息。格式如下：HPRHeading,Pitch&Roll（航向，俯仰和橫滾）

$PTNTHPR,x.x,a,x.x,a,x.x,a*hh<cr><lf>這個句子把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首次提出的卡爾曼濾波是一種線性最小方差估計?？柭鼮V波理論成功地解決了美國阿波羅登月中的多導(dǎo)航傳感器組合導(dǎo)航的難題，引起工程界的廣泛重視。之后，卡爾曼濾波理論開始在導(dǎo)航系統(tǒng)中得到普遍的應(yīng)用?？柭鼮V波的基本思想：以最小均方誤差為估計準(zhǔn)則，采用信號與誤差的狀態(tài)空間模型，利用前一時刻的估計值和當(dāng)前的觀測值來更新對狀態(tài)變量的估計，求出當(dāng)前時刻的估計值，算法根據(jù)建立的系統(tǒng)方程和觀測方程對需要處理的信號做出滿足最小均方誤差的的估計⑹。由于卡爾曼濾波方程具有易編程的優(yōu)點，所以卡爾曼濾波器在導(dǎo)航方面應(yīng)用十分廣泛，且能夠很好地提高導(dǎo)航的精度?？柭鼮V波是一種最優(yōu)化自回歸數(shù)據(jù)處理方式，在了解狀態(tài)方程、觀測方程以及系統(tǒng)噪聲的情況下，通過卡爾曼濾波的五個公式實現(xiàn)羅盤航向角的最優(yōu)化回歸處理，將k-1時刻的預(yù)測值以及預(yù)測偏差和k時刻的量測值以及量測偏差結(jié)合起來，通過計算卡爾曼增益將這5個式子結(jié)合起來實現(xiàn)遞推。下面詳細(xì)介紹一下這五個式子的含義：

首先我們要根據(jù)k-1時刻的航向角來預(yù)測k時刻的航向角，根據(jù)k-1時刻的狀態(tài)方程可以得到k時刻的預(yù)測航向角如公式(10)：錯誤！未找到引用源。(10)其中錯誤！未找到引用源。是根據(jù)k-1時刻航向角預(yù)測k時刻的航向角，A、B為系統(tǒng)參數(shù)矩陣，錯誤！未找到引用源。為k-1時刻的航向角，錯誤！未找到引用源。為控制量，此時為零向量。我們用錯誤！未找到引用源。(covariance)來表示通過k-1時刻方差陣預(yù)測的k時刻方差陣(預(yù)測值的偏差)，錯誤！未找到引用源。表示k-1時刻的航向角的偏差，他們的關(guān)系如公式(11)：TOC\o"1-5"\h\z錯誤！未找到引用源。(11)Q為過程噪聲的covariance，假設(shè)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時刻的航向角，這樣就可以實時求解航向角的大小。至于編程時錯誤！未找到引用源。初始參數(shù)的確定可以任意確定一個就行，因為卡爾曼濾波算法會使這個參數(shù)逐漸收斂的，但是錯誤！未找到引用源。最好別取零，防止卡爾曼濾波把這個當(dāng)成最優(yōu)解。實驗中，采用卡爾曼濾波器對羅盤輸出的航向值進(jìn)行濾波，濾波器的輸出如圖5.1所示。350300250200150100500-50010002000300040005000600070008000900010000圖5.1航向角濾波前后對比濾波后航向角的相對誤差如圖5.2所示。18圖5.2濾波數(shù)據(jù)與原始數(shù)據(jù)相對誤差從圖5.1可以看出，在濾波時給出的初值和剛開始的航向角值差別比較大，卡爾曼濾波能很快收斂到精確的航向角，在羅盤每次改變方向的時候，卡爾曼濾波因為要根據(jù)預(yù)測量和測量值來估計下一時刻值，所以濾波時航向角失調(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.8574，H=5.3。（2）用matlab讀取羅盤信息，在東北天坐標(biāo)系下進(jìn)行航位推算，因為磁偏角的存在使電子羅盤存在誤差，所以將測得的航向角加上當(dāng)?shù)氐拇牌潜愕玫綄嶋H的航向角（南京的磁偏角為5°），我們這組我取速度為4m/s,采樣時間用四組的平均采用時間，即錯誤！未找到引用源。/4（15）錯誤！未找到引用源。=（0.1170+0.1157+0.1172+0.1170）/4=0.1167按DR算法可以得到如圖6.1所示運動軌跡圖：

圖6.1東北天坐標(biāo)系下運動軌跡從上圖可以看出用濾波后的數(shù)據(jù)進(jìn)行航位推算得到的軌跡與原始數(shù)據(jù)進(jìn)行航位推算得到的軌跡大致相同。說明在短時間內(nèi)羅盤的導(dǎo)航精度很高。（3）將東北天坐標(biāo)系轉(zhuǎn)化為直角坐標(biāo)系，在直角坐標(biāo)系中推算的軌跡如圖6.2所示，因為X-Y面更能清楚的反映運動情況，故采用平面直角坐標(biāo)系。4.7402-2.6128-2.6126-2.6124-2.6122-2.612-2.6118-2.6116-2.6114-2.6112-2.611x104.7402-2.6128-2.6126-2.6124-2.6122-2.612-2.6118-2.6116-2.6114-2.6112-2.611x106X/（m）圖6.2直角空間坐標(biāo)系X-Y面圖6.3大地坐標(biāo)系經(jīng)緯度面在大地坐標(biāo)系（B,L,H）下的軌跡如圖6.4所示。大地坐標(biāo)系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大地坐標(biāo)系X-Y-Z7誤差分析7.1電子羅盤誤差大地坐標(biāo)系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大地坐標(biāo)系X-Y-Z電子羅盤的誤差具體可以分為兩類：系統(tǒng)誤差和外界環(huán)境誤差。系統(tǒng)誤差包括羅盤自身的器件精度誤差以及安裝誤差，主要受羅盤內(nèi)部的三軸磁阻傳感器影響。外界環(huán)境誤差是指羅盤工作的環(huán)境所帶來的誤差，如載體傾斜帶來的誤差、磁羅差誤差等。本實驗中羅盤放在計算機(jī)、示波器和工控機(jī)旁邊易受到干擾，所以羅差對羅盤的精度有很大的影響。雖然濾波后得到的軌跡比濾波前精確些，但是羅盤每個方向的具體方位角并不精確，若想提高每個方向的方位角精度，最好能對羅盤進(jìn)行補(bǔ)償，可以在因為載體傾斜帶來的誤差和磁羅差方面分別進(jìn)行補(bǔ)償，因為本實驗中沒有理想的精確的方位角理論值，所以沒有對羅盤進(jìn)行補(bǔ)償，只對羅盤轉(zhuǎn)動的軌跡進(jìn)行了仿真。7.2航位推算本身誤差航位推算一個致命的弱點就是誤差隨著時間而積累。所以推算的航向角是發(fā)散的，航向角誤差越來越大。因此航位推算不能長時間的單獨工作，必須借助于其他的定位方式進(jìn)行組合導(dǎo)航，比如GPS/DR組合導(dǎo)航，航位推算借助于GPS定期傳來的載體位置進(jìn)行聯(lián)合導(dǎo)航，精度更加精確。8總結(jié)在得到一組GPS導(dǎo)航數(shù)據(jù)的基礎(chǔ)上利用航位推算進(jìn)行導(dǎo)航，通過卡爾曼濾波對航向角數(shù)據(jù)進(jìn)行處理，減小誤差。利用matlab仿真出分別在東北天坐標(biāo)系、直角坐標(biāo)系、大地坐標(biāo)系下的運動軌跡，載體運動軌跡與實驗時的羅盤向軌跡相同。最后分析了航位推算導(dǎo)航定位的誤差原因，并且希望以后在羅盤補(bǔ)償方面能做更進(jìn)一步的研究。參考文獻(xiàn)[1].孫希延，紀(jì)元法與施滸立,卡爾曼濾波實現(xiàn)車載GPS/DR組合導(dǎo)航.現(xiàn)代電子技術(shù),2006(11):第149-152頁.⑵.吳富梅，楊元喜與田育民,GPS/DR組合導(dǎo)航自適應(yīng)Kalman濾波算法.測繪科學(xué)技術(shù)學(xué)報,2008(3):第206-208+212頁..王珂,GPS測向算法與應(yīng)用研究,2009,重慶大學(xué)..張華海，鄭南山，王軍，李景芝.由空間直角坐標(biāo)計算大地坐標(biāo)的簡便公式.全球定位系統(tǒng).2002，4:9-12.彭樹生,數(shù)字電子羅盤HMR3000的特性及應(yīng)用.電子技術(shù),2004(8):第39-43頁..付夢印，鄧忠紅，張繼偉.Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用[M].北京:科學(xué)出版社，2003.附錄clearall;loadhead.matZ=heading;[m,n]=size(Z);T=0.1167;A=[1T;01];%系統(tǒng)參數(shù)矩陣(狀態(tài)轉(zhuǎn)移矩陣)H=[10];%測量系統(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);fork=1:mXk=A*X;%從k-1時刻預(yù)測k時刻Ck=A*C*A'+Qk;%k-1時刻預(yù)測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;%濾波輸出endfigure(1)plot(Xe(1,:),'r');holdon;plot(heading,'b');gridon;holdon;B=32.0304;L=118.8574;H=5.3;e2=0.00673949674227;formatshorte1=(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,:);[n3n4]=size(Xe);fori=1:(n4+1)ifi==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);endendfori=1:(n4+1)ifi==1x3(i)=0;y3(i)=0;elsed2=f(i-1)-5;x3(i)=x3(i-1)+4*0.1167*cosd(d2);y3(i)=y3(i-1)+4*0.1167*s