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

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

3、算原理2 航位推算原理航位推算原理是利用載體的速度、航向以及上一時刻的位置來估計下一時刻載體的位置。利用載體的速度和航向能夠得出速度在當地水平坐標軸上分向速度，將分向速度和載體所經過的時間相乘便可以得到載體在坐標軸上增加的坐標值，與前一時刻的坐標值求和便可以得到此時的坐標值。原理圖如圖1所示。推算如公式(1)。.(1)其中，()為載體在時刻的初始位置，()為載體在時刻位置，、為載體在時刻的速度與航向角（航向角為載體運動方向與正東方向的夾角）。為羅盤相鄰兩次推算所用的時間差。3 坐標系之間的轉化3.1 所涉坐標系的種類3.1.1 當地水平坐標系當地水平坐標系又名東北天坐標系，它的坐標原點為載體的

4、質心，X軸指向當地北子午線，Y軸垂直X軸指向東，Z軸垂直XOY面構成右手直角坐標系。3.1.2大地坐標系地球橢球的中心與地球質心重合，橢球的短軸與地球的自轉軸重合，大地緯度B為地面點的橢球法線與橢球赤道面的夾角，大地經度L為過地面點的橢球子午面與格林威治起始子午面的夾角，大地高H為地面點沿橢球法線至橢球面的距離。地面一點可表示為( L,B,H)。如圖3.1所示。圖3.1 大地坐標系B:大地緯度，通過觀測站的參考橢球面的法線與赤道面的夾角。L:大地經度，通過觀測站的大地子午面與本初子午面得夾角。H:大地高度，觀測站沿法線到參考橢球面的距離。3.1.3 WGS-84坐標系GPS使用的坐標系，該坐標

5、系屬于協議地球坐標系，其原點位于地球質心，Z軸指向BIH1984.0定義的協議地級方向，X軸指向BIH1984.0的零子午面和CTP赤道的交點，Y軸與Z軸構成右手坐標系3。GPS定位結果屬于協議地球地心坐標系，即84坐標系，且通常以空間直角坐標（X，Y，Z），或以橢球大地坐標（B,L,H）的形式給出。而實用的常規地面測量成果或是屬于國家的參心大地坐標系，或是屬于地方獨立坐標系。因此必須實現GPS成果的坐標系的轉換。3.2 坐標系轉換3.2.1 當地水平坐標系轉化為空間直角坐標系當地水平坐標系與84坐標系同屬于直角坐標系，故相互轉化時需要利用地心大地坐標系的參數，將84坐標系下（以大地坐標（B,

6、L,H）測得的GPS量作為載體的質心，然后經過旋轉矩陣將東北天坐標系（當地水平坐標系、站心坐標系）轉化為空間直角坐標系。旋轉點作為載體的質心，以（B,L,H）的值為起點。將坐標系分別繞Z、X、Y軸逆時針旋轉、角所使用的旋轉矩陣如下：=.(2)= (3)= .(4)由于航位推算是在東北天坐標系下進行的，所以需要需變換到大地坐標系下，顯示經緯度信息，先將東北天坐標系轉換到直角坐標系下，然后再有直角坐標系轉化為大地坐標系，將在84坐標系（大地坐標（B、L、H）形式表示）下測得的GPS值轉化為直角坐標系（X、Y、Z）下表示，作為直角坐標系下初值，轉化公式如下：.(5)現將Y軸反向得軸。繞軸逆時針旋轉)

7、，再繞Z軸旋轉)，即可將東北天坐標系（地平站心坐標系）轉化為站心赤道直角坐標系，公式如下：.(6)R=*=所以空間直角坐標系和站心赤道直角坐標系之間有簡單的平移關系，所以將站心赤道坐標系轉化為空間直角坐標系之間的轉化關系為：=.(7)所以與東北天坐標系之間的轉化關系為：=(8) 3.2.2 直角坐標系轉化為大地坐標系直角坐標系轉化為大地坐標系的轉化公式為：.(8)其中：，N為該點的卯酉圈曲率半徑；，a、b、e分別為該大地坐標系對應參考橢球的長半軸、短半軸和第一偏心率。長半軸a=63781372m，短半軸b=6356.7523142km，。此方法H和B需要迭代求得，而且應用條件受到很大限制，如要

8、求大地高度需要小于1000m時，才能使B、L迭代四次之后精度分別達到和0.001m，所以本文一種計算精度高，公式形式簡便的方法來計算B和L4。.(9)4 電子羅盤HMR30004.1 概述HMR3000包含三個磁阻傳感器和一個雙軸的液體橫滾傳感器，并帶有A/D轉換電路和微處理器。微處理器控制傳感器的測量順序，控制器操作的參數存儲在EEPROM中，輸出3類NMEA0813標準語句(HDG、HDT和XDR)，3類專用語句(HPR、RCD和CCD)，HPR是最常用的格式，采用RS232接口功能模塊如圖4-1所示。圖4-1 HMR3000功能框圖4.2 HMR3000特性數字電子羅盤HMR3000是H

9、oneywell公司的產品。它可以為導航定位系統提供航向、俯仰、橫滾等數據。其中航向的精度為±，分辨率可達±。橫滾和俯仰的精度為±，分辨率為±，輸出端口采用RS232。HMR3000的串行通信是根據NMEA0183標準制定的簡單、異步的ASCII協議5。ASCII碼的傳輸和接收使用1位起始位、8位數據位，無奇偶校驗位和1位停止位。每一個碼有10位。波特率可選擇1200,2400,4800,9600或38400。HMR3000支持NMEA0183和專用的信息。在測量模式中有HMR3000按照EEPROM中的編程的速率主動發送NMEA信息。本次試驗輸出是專用

10、HPR句子，它包含了航向、俯仰、橫滾等信息。格式如下：HPR Heading, Pitch & Roll（航向，俯仰和橫滾）$PTNTHPR,x.x,a,x.x,a,x.x,a*hh <cr> <lf>這個句子把HMR3000的三個重要的測量結果和有用的狀態、俯仰、俯仰狀態、橫滾、橫滾狀態。如本次試驗的一個句子為：$PTNTHPR,199.3,N,0.3,N,0.2,N*37此句說明：羅盤所測航向角為：，正常狀態，俯仰角為，正常狀態，橫滾角為，正常狀態。5 卡爾曼濾波1960年，R.E.Kalman首次提出的卡爾曼濾波是一種線性最小方差估計。卡爾曼濾波理論成功地

11、解決了美國阿波羅登月中的多導航傳感器組合導航的難題，引起工程界的廣泛重視。之后，卡爾曼濾波理論開始在導航系統中得到普遍的應用。卡爾曼濾波的基本思想：以最小均方誤差為估計準則，采用信號與誤差的狀態空間模型，利用前一時刻的估計值和當前的觀測值來更新對狀態變量的估計，求出當前時刻的估計值，算法根據建立的系統方程和觀測方程對需要處理的信號做出滿足最小均方誤差的的估計6。由于卡爾曼濾波方程具有易編程的優點，所以卡爾曼濾波器在導航方面應用十分廣泛，且能夠很好地提高導航的精度?？柭鼮V波是一種最優化自回歸數據處理方式，在了解狀態方程、觀測方程以及系統噪聲的情況下，通過卡爾曼濾波的五個公式實現羅盤航向角的最優

12、化回歸處理，將k-1時刻的預測值以及預測偏差和k時刻的量測值以及量測偏差結合起來，通過計算卡爾曼增益將這5個式子結合起來實現遞推。下面詳細介紹一下這五個式子的含義：首先我們要根據k-1時刻的航向角來預測k時刻的航向角，根據k-1時刻的狀態方程可以得到k時刻的預測航向角如公式(10)：(10)其中是根據k-1時刻航向角預測k時刻的航向角，A、B為系統參數矩陣，為k-1時刻的航向角，為控制量，此時為零向量。我們用(covariance)來表示通過k-1時刻方差陣預測的k時刻方差陣（預測值的偏差），表示k-1時刻的航向角的偏差，他們的關系如公式(11)：(11)Q為過程噪聲的covariance，假

13、設k時刻航向角的量測值為，現在我們可以得到k時刻的最優化航向角估計值如公式(12)：(12)H為測量系統參數，為k時刻的卡爾曼增益，求解如公式13：(13)R為觀測噪聲的covariance，為了能夠以此往下面迭代，故必須求出k時刻航向角的偏差，即求出。如公式(14)：(1-)*(14)然后再根據k時刻的參數值求解k+1時刻的航向角，這樣就可以實時求解航向角的大小。至于編程時初始參數的確定可以任意確定一個就行，因為卡爾曼濾波算法會使這個參數逐漸收斂的，但是最好別取零，防止卡爾曼濾波把這個當成最優解。實驗中，采用卡爾曼濾波器對羅盤輸出的航向值進行濾波，濾波器的輸出如圖5.1所示。圖5.1 航向角

14、濾波前后對比濾波后航向角的相對誤差如圖5.2所示。圖5.2 濾波數據與原始數據相對誤差從圖5.1可以看出，在濾波時給出的初值和剛開始的航向角值差別比較大，卡爾曼濾波能很快收斂到精確的航向角，在羅盤每次改變方向的時候，卡爾曼濾波因為要根據預測量和測量值來估計下一時刻值，所以濾波時航向角失調量要大些。6 實驗、數據處理及其仿真6.1 實驗內容（1）用NT3000GPS接收板卡測得五分鐘的數據，得到一組實驗場地的經緯度信息，然后取平均得到一組精度相對較高的初始經緯度信息。通過串口調試助手觀測并保存GPS數據為“txt”格式。（2）用HMR3000電子羅盤繞一周測4個方向的航向角信息，每個方向大概測5

15、分鐘，通過串口調試助手觀測并保存羅盤數據為“txt”格式。6.2 數據處理（1）將GPS測得經緯度信息取平均得到初始B=32.0304，L=118.8574，H=5.3。（2）用matlab讀取羅盤信息，在東北天坐標系下進行航位推算，因為磁偏角的存在使電子羅盤存在誤差，所以將測得的航向角加上當地的磁偏角便得到實際的航向角（南京的磁偏角為5°），我們這組我取速度為4 m/s,采樣時間用四組的平均采用時間，即/4.(15) =(0.1170+0.1157+0.1172+0.1170)/4=0.1167按DR算法可以得到如圖6.1所示運動軌跡圖：圖6.1 東北天坐標系下運動軌跡從上圖可以看

16、出用濾波后的數據進行航位推算得到的軌跡與原始數據進行航位推算得到的軌跡大致相同。說明在短時間內羅盤的導航精度很高。（3）將東北天坐標系轉化為直角坐標系，在直角坐標系中推算的軌跡如圖6.2所示，因為X-Y面更能清楚的反映運動情況，故采用平面直角坐標系。圖6.2 直角空間坐標系X-Y面然后再將直角空間坐標系轉換為大地坐標系，在大地坐標經緯度面的軌跡如圖6.3所示。圖6.3 大地坐標系經緯度面在大地坐標系（B,L,H）下的軌跡如圖6.4所示。圖6.4 大地坐標系X-Y-Z7 誤差分析7.1電子羅盤誤差電子羅盤的誤差具體可以分為兩類：系統誤差和外界環境誤差。系統誤差包括羅盤自身的器件精度誤差以及安裝誤

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

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

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

20、ing;m,n=size(Z);T=0.1167;A=1 T;0 1;%系統參數矩陣（狀態轉移矩陣）H=1 0; %測量系統參數X=100;0; %狀態變量初值C=diag(1,1);%誤差協方差初值Rk=diag(1);%量測噪聲方差Ta=T;1;%狀態過程噪聲陣sigam=1;Qk=Ta*Ta'*sigam2;%過程噪聲方差初值f=Z(:,1);for k=1:m Xk=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*

21、Xk);%狀態更新 C=Ck-K*H*Ck;%方差更新 Xe(:,k)=X;%濾波輸出 endfigure(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 short e1=(63781402-63567552)/63781402;N=6378140/(sqrt(1-e1*(sind(B)2); X=(N+H)*cosd(B)*cosd(L);Y=(N+H)*cosd(B)*sin

22、d(L);Z=(N*(1-e1)+H)*sind(B); c=Xe(1,:);n3 n4=size(Xe);for i=1:(n4+1) if i=1 x2(i)=0; y2(i)=0; else d1=c(i-1)+5; x2(i)=x2(i-1)+4*0.1167*cosd(d1); y2(i)=y2(i-1)+4*0.1167*sind(d1); end endfor i=1:(n4+1) if i=1 x3(i)=0; y3(i)=0; else d2=f(i-1)-5; x3(i)=x3(i-1)+4*0.1167*cosd(d2); y3(i)=y3(i-1)+4*0.1167*sind(d2); end end for i=1:(n4)d3(:,i)=abs(f(i)-c(i)/f(i);endfigure(2)plot(d3(1,:),'r');grid on;hold on;figure(3) plot(x2,y2,x3,y3,'r')hold on;xlabel('正東/(m)');ylabel('正北/(m)')