一種用于導(dǎo)航系統(tǒng)的姿態(tài)及航向角的校正方法
【專利摘要】本發(fā)明公布了一種用于導(dǎo)航系統(tǒng)的姿態(tài)及航向角的校正方法，包括提出了基于改進擴展卡爾曼濾波的橢球擬合方法進行磁強計誤差補償、姿態(tài)角的誤差二級建模和校正以及實現(xiàn)整個系統(tǒng)誤差校正的有效方法。本發(fā)明用于由慣性測量單元(IMU)和磁強計等組成的慣性組合導(dǎo)航與定位系統(tǒng)中的姿態(tài)及航向角的誤差校正。磁場補償方法從二維橢圓擬合拓展到三維橢球體擬合，利用新的橢球模型和改進擴展卡爾曼濾波方法進行橢球擬合，該方法可以有效地實現(xiàn)載體動態(tài)實時地自身三維磁場干擾的補償，提高地磁場的測量精度，從而提高載體航向角的精度；對導(dǎo)航系統(tǒng)輸出的姿態(tài)角信息進行誤差二級建模，然后對其補償以實時提高姿態(tài)角的精度。
【專利說明】一種用于導(dǎo)航系統(tǒng)的姿態(tài)及航向角的校正方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及一種導(dǎo)航校正方法，特別涉及一種用于導(dǎo)航系統(tǒng)的姿態(tài)及航向角的校正方法。
【背景技術(shù)】
[0002]在導(dǎo)航系統(tǒng)應(yīng)用中，導(dǎo)航系統(tǒng)能否提供載體的準(zhǔn)確姿態(tài)及航向角信息至關(guān)重要。對于僅僅使用加速度計和陀螺儀組成慣性測量單元(mu)，由于加速度計和陀螺儀自身的誤差及漂移使得MU輸出的姿態(tài)和航向參數(shù)精度不能滿足一些導(dǎo)航系統(tǒng)的要求。由慣性測量單元(IMU)和磁強計組成的姿態(tài)及航向測量系統(tǒng)，可以提高姿態(tài)角及航向角的精度。
[0003]用地磁場進行導(dǎo)航定位，具有無源、無輻射、抗干擾、全天時、全天候、體積小、能耗低的優(yōu)點，因此在飛機、艦船和潛艇等領(lǐng)域得到廣泛應(yīng)用。導(dǎo)航載體通過磁傳感器測量空間的磁場信息，這些磁場信息不僅包括導(dǎo)航定位所用的地磁場信息，也包括載體自身的干擾磁場信息。高精度的地磁導(dǎo)航過程中需要對磁傳感器的觀測磁場信息進行處理，實時對載體磁場干擾進行補償，提高地磁導(dǎo)航精度。
[0004]目前，有關(guān)對導(dǎo)航系統(tǒng)載體姿態(tài)角的誤差建模方法已經(jīng)很普遍，也很成熟，可以達到的一定的高精度要求。但是，對慣性測量器件直接建立模型處于一級建模階段，如果對誤差一級模型得到的姿態(tài)角數(shù)據(jù)再進行二級建模這樣可以大大提高系統(tǒng)的精度。
[0005]載體自身干擾磁場的實時補償是提高磁傳感器測量精度的關(guān)鍵技術(shù)。

【發(fā)明內(nèi)容】

[0006]發(fā)明目的:針對上述現(xiàn)有技術(shù)，本發(fā)明根據(jù)橢圓假設(shè)磁補償方法的思路，一種用于導(dǎo)航系統(tǒng)的姿態(tài)及航向角的校正方法，克服由于載體自身干擾磁場的影響，提高導(dǎo)航精度。
[0007]技術(shù)方案:一種用于導(dǎo)航系統(tǒng)的姿態(tài)及航向角的校正方法，載體航向角的校正包括下列步驟:
[0008]步驟(I)，三軸磁強計誤差建模:
[0009]選取導(dǎo)航坐標(biāo)系為東北天(ENU)、載體坐標(biāo)系的Y軸為載體的前進方向，X軸為與載體前進方向垂直向右，Z軸與X軸、Y軸垂直向上滿足右手定則；將磁強計的三軸測量值投影到二維水平面的X、Y軸，分別為Hx、Hy ;三軸磁強計的誤差模型為:
[0010]Hffl = CisCn (CsHe+bn)+b0 (I)
[0011]式中:Cis為靈敏度誤差矩陣；Cn為非正交誤差矩陣；CS為軟磁誤差矩陣；bn為硬磁誤差矢量為偏置誤差矢量；H6為地磁場強度；Hm為磁強計測量值；
[0012]將所述三軸磁強計的誤差模型改寫為:
[0013]Hffl = CaHe+b (2)
[0014]式中:Ca是總誤差矩陣；b為總偏置矢量；
[0015]由式⑵可得地磁場矢量He為:
[0016]He = C(HnTb) (3)；[0017]步驟(2)，改進的地球磁場的橢球模型建模:
[0018]地球磁場的橢球模型為:
[0019]AX2+BY2+CZ2+2EXY+2FXZ+2GYZ+2QX+2SY+2TZ+D = 0 (4)
[0020]式中，A、B、C、E、F、G、Q、S、T、D是橢球模型中的各項參數(shù)；X、Y、Z分別是磁強計的
三軸X、Y、Z的測量值；[0021]對所述橢球模型進行改進，得到如下橢球模型:
[0022]a (x+y+p) 2+b (x+z+q) 2+c (y+z+r)2 = 0 (5)
[0023]式中，(x, y, z)是磁強計的三軸測量值；a、b、c、p、q、r為改進橢球模型的各項參數(shù)；
[0024]其中，式(5)與式⑷中參數(shù)的對應(yīng)關(guān)系為:
[0025]A = a+b
[0026]B = a+c
[0027]C = b+c
[0028]E = a
[0029]F = b
[0030]G = c
[0031]Q = ap+bq
[0032]S = ap+cr
[0033]T = bq+cr
[0034]D = ap2+bq2+cr2 ；
[0035]步驟(3)，利用改進擴展卡爾曼濾波器求解橢球模型的參數(shù):
[0036]改進的擴展卡爾曼濾波器，直接把狀態(tài)的更新值作為觀測值進行下一步更新；利用改進擴展卡爾曼濾波器擬合橢球時，狀態(tài)向量是所述式(5)中橢球參數(shù)估計值與真實值的偏差，利用改進擴展卡爾曼濾波對橢球參數(shù)的偏差進行估計，得出橢球模型的參數(shù)；
[0037]步驟(4)，由所述橢球模型參數(shù)求得所述式(3)中磁強計誤差模型參數(shù)；
[0038]步驟(5)，根據(jù)所述式(2)的三軸磁強計誤差模型，對三軸磁強計輸出磁通量補償；
[0039]步驟(6)，根據(jù)補償后的三軸磁強計輸出以及姿態(tài)角，計算得到校正后的載體航向角。
[0040]作為本發(fā)明的改進，在載體航向角校正之前，還包括姿態(tài)角校正步驟，包括如下具體步驟:
[0041]步驟(1)，對導(dǎo)航系統(tǒng)的經(jīng)過一級誤差補償后的姿態(tài)角進行誤差二級建模:
[0042]建立俯仰角誤差模型:yP = mpxp+np，其中，yP表示俯仰角誤差值，所述俯仰角誤差值為俯仰角與轉(zhuǎn)臺角度差值，Xp表示測量值，mP、nP是誤差模型的系數(shù)；
[0043]建立橫滾角誤差模型:yK = mQXK+nQ，其中，yE表示橫滾角誤差值,所述橫滾角誤差值為橫滾角與轉(zhuǎn)臺角度差值，xK表示測量值，mQ、nQ是誤差模型的系數(shù)；
[0044]步驟(2)，導(dǎo)航系統(tǒng)三軸轉(zhuǎn)臺進行試驗，上位機采集數(shù)據(jù):
[0045]首先，三軸轉(zhuǎn)臺初始水平校正；然后，保持導(dǎo)航系統(tǒng)橫滾角不變，調(diào)整轉(zhuǎn)臺角度，上位機采集經(jīng)過一級誤差補償后的導(dǎo)航系統(tǒng)俯仰角測量值，并記錄所述俯仰角測量值與轉(zhuǎn)臺角度差值；
[0046]將三軸轉(zhuǎn)臺恢復(fù)初始水平設(shè)置，保持導(dǎo)航系統(tǒng)俯仰角不變，調(diào)整轉(zhuǎn)臺角度，上位機采集經(jīng)過一級誤差補償后的導(dǎo)航系統(tǒng)橫滾角測量值，并記錄所述橫滾角與轉(zhuǎn)臺角度差值；
[0047]步驟(3)，根據(jù)俯仰角誤差模型，利用最佳平方逼近對所述采集的俯仰角與轉(zhuǎn)臺角度差值進行計算后得到所述誤差系數(shù)mP、nP的值，從而得到俯仰角誤差模型；
[0048]根據(jù)橫滾角誤差模型，利用最小二乘法對所述采集的橫滾角與轉(zhuǎn)臺角度差值進行計算后得到所述誤差系數(shù)mQ、nQ的值，從而得到橫滾角誤差模型；
[0049]步驟(4)，利用所述俯仰角誤差模型對經(jīng)過一級誤差補償后的導(dǎo)航系統(tǒng)俯仰角進行補償，得到補償后的俯仰角P為:P = Yp-Xp ;利用所述橫滾角誤差模型對經(jīng)過一級誤差補償后的導(dǎo)航系統(tǒng)橫滾角進行補償，得到補償后的橫滾角R為:R = Ye-Xro
[0050]有益效果:為了提高導(dǎo)航系統(tǒng)的精度，本發(fā)明通過建立姿態(tài)角的輸出誤差二級模型用以校正姿態(tài)角，并且在航向角的計算原理上尋找提高精度的方法，從而提出了新的橢球模型和基于改進擴展卡爾曼濾波的橢球擬合方法進行磁場補償。該校正方法可以有效的提高導(dǎo)航系統(tǒng)的精度，進而實現(xiàn)高精度定位和導(dǎo)航，并取得良好效果。
[0051]利用三軸磁強計計算載體的航向角的主要原理是，由磁強計測量出的地磁場強度值，然后進行相應(yīng)的計算，最后得到航向角。對于如圖2所示的二維系統(tǒng)，定義載體前進方向與磁北的夾角為地磁航向角P，其與地理北極的夾角為地理航向角P，由圖示可知:
【權(quán)利要求】
1.一種用于導(dǎo)航系統(tǒng)的姿態(tài)及航向角的校正方法，其特征在于，載體航向角的校正包括下列步驟: 步驟(1)，三軸磁強計誤差建模: 選取導(dǎo)航坐標(biāo)系為東北天(ENU)、載體坐標(biāo)系的Y軸為載體的前進方向，X軸為與載體前進方向垂直向右，Z軸與X軸、Y軸垂直向上滿足右手定則；將磁強計的三軸測量值投影到二維水平面的X、Y軸，分別為Hx、Hy ;三軸磁強計的誤差模型為:
Hm = CisCn (CsHe+bn)+b0 (I) 式中=Cis為靈敏度誤差矩陣；Cn為非正交誤差矩陣；CS為軟磁誤差矩陣；bn為硬磁誤差矢量為偏置誤差矢量；H6為地磁場強度；Hm為磁強計測量值； 將所述三軸磁強計的誤差模型改寫為:
Hm= CaHe+b (2) 式中:Ca是總誤差矩陣；b為總偏置矢量； 由式(2)可得地磁場矢量H6為:
He = Ca 1 (Hm-b) (3)； 步驟(2)，改進的地球磁場的橢球模型建模: 地球磁場的橢球模型為:
AX2+BY2+CZ2+2EXY+2FXZ+2GYZ+2QX+2SY+2TZ+D = O (4) 式中，A、B、C、E、F、G、Q、S、T、D是橢球模型中的各項參數(shù)；X、Y、Z分別是磁強計的三軸X、Y、Z的測量值； 對所述橢球模型進行改進，得到如下橢球模型: a (x+y+p) 2+b (x+z+q) 2+c (y+z+r)2 = O (5) 式中，(x，y，z)是磁強計的三軸測量值A(chǔ)、B、C、p、q、r為改進橢球模型的各項參數(shù)； 其中，式(5)與式(4)中參數(shù)的對應(yīng)關(guān)系為:
A = a+b
B = a+c
C = b+c
E = a
F = b
G = c
Q = ap+bq
S = ap+cr
T = bq+cr
D = ap2+bq2+cr2 ； 步驟(3)，利用改進擴展卡爾曼濾波器求解橢球模型的參數(shù): 改進的擴展卡爾曼濾波器，直接把狀態(tài)的更新值作為觀測值進行下一步更新；利用改進擴展卡爾曼濾波器擬合橢球時，狀態(tài)向量是所述式(5)中橢球參數(shù)估計值與真實值的偏差，利用改進擴展卡爾曼濾波對橢球參數(shù)的偏差進行估計，得出橢球模型的參數(shù)； 步驟(4)，由所述橢球模型參數(shù)求得所述式(3)中磁強計誤差模型參數(shù)； 步驟(5)，根據(jù)所述式(2)的三軸磁強計誤差模型，對三軸磁強計輸出磁通量補償；步驟(6)，根據(jù)補償后的三軸磁強計輸出以及姿態(tài)角，計算得到校正后的載體航向角。
2.根據(jù)權(quán)利要求1所述的一種用于導(dǎo)航系統(tǒng)的姿態(tài)及航向角的校正方法，其特征在于，在載體航向角校正之前，還包括姿態(tài)角校正步驟，包括如下具體步驟: 步驟(1)，對導(dǎo)航系統(tǒng)的經(jīng)過一級誤差補償后的姿態(tài)角進行誤差二級建模: 建立俯仰角誤差模型:yP = mPxP+nP，其中，yP表示俯仰角誤差值,所述俯仰角誤差值為俯仰角與轉(zhuǎn)臺角度差值，Xp表示測量值，mP、nP是誤差模型的系數(shù)； 建立橫滾角誤差模型= mQxK+nQ，其中，yK表示橫滾角誤差值，所述橫滾角誤差值為橫滾角與轉(zhuǎn)臺角度差值，xK表示測量值，mQ、nQ是誤差模型的系數(shù)； 步驟(2)，導(dǎo)航系統(tǒng)三軸轉(zhuǎn)臺進行試驗，上位機采集數(shù)據(jù): 首先，三軸轉(zhuǎn)臺初始水平校正；然后，保持導(dǎo)航系統(tǒng)橫滾角不變，調(diào)整轉(zhuǎn)臺角度，上位機采集經(jīng)過一級誤差補償后的導(dǎo)航系統(tǒng)俯仰角測量值，并記錄所述俯仰角測量值與轉(zhuǎn)臺角度差值; 將三軸轉(zhuǎn)臺恢復(fù)初始水平設(shè)置，保持導(dǎo)航系統(tǒng)俯仰角不變，調(diào)整轉(zhuǎn)臺角度，上位機采集經(jīng)過一級誤差補償后的導(dǎo)航系統(tǒng)橫滾角測量值，并記錄所述橫滾角與轉(zhuǎn)臺角度差值； 步驟(3)，根據(jù)俯仰角誤差模型，利用最佳平方逼近對所述采集的俯仰角與轉(zhuǎn)臺角度差值進行計算后得到所述誤差系數(shù)mP、nP的值，從而得到俯仰角誤差模型； 根據(jù)橫滾角誤差模型，利用最小二乘法對所述采集的橫滾角與轉(zhuǎn)臺角度差值進行計算后得到所述誤差系數(shù)HVne的值，從而得到橫滾角誤差模型； 步驟(4)，利用所述俯仰角誤差模型`對經(jīng)過一級誤差補償后的導(dǎo)航系統(tǒng)俯仰角進行補償，得到補償后的俯仰角P為:P = Yp-Xp ;利用所述橫滾角誤差模型對經(jīng)過一級誤差補償后的導(dǎo)航系統(tǒng)橫滾角進行補償，得到補償后的橫滾角R為:R = yE-xRO
【文檔編號】G01C21/20GK103630137SQ201310633933
【公開日】2014年3月12日 申請日期:2013年12月2日 優(yōu)先權(quán)日:2013年12月2日
【發(fā)明者】陳熙源, 呂才平, 黃浩乾, 湯傳業(yè), 宋銳, 何昆鵬, 方琳 申請人:東南大學(xué)