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

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

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