一種用于導(dǎo)航系統(tǒng)航向角直接校正補(bǔ)償方法
【專利摘要】本發(fā)明公開了一種用于導(dǎo)航系統(tǒng)航向角直接校正補(bǔ)償方法，屬于導(dǎo)航系統(tǒng)定位【技術(shù)領(lǐng)域】。首先根據(jù)磁力計計算航向角的基本原理，拓展了磁力計的誤差模型，其次再根據(jù)地磁場的分布特征得出進(jìn)行參數(shù)估計的模型，最后利用兩步總體最小二乘法對拓展的磁力計的誤差模型的參數(shù)進(jìn)行估計，最終可以實(shí)現(xiàn)在系統(tǒng)級對導(dǎo)航系統(tǒng)航向角進(jìn)行直接校正。本發(fā)明可以應(yīng)用于由慣性測量單元(IMU)和磁力計(或電子羅盤)等組成的慣性組合導(dǎo)航與定位系統(tǒng)中航向角的誤差校正及補(bǔ)償。本發(fā)明能夠直接對導(dǎo)航系統(tǒng)航向角進(jìn)行校正、魯棒性強(qiáng)，可靠性強(qiáng)、成本低、精度高、計算過程處理簡潔。同時本發(fā)明能夠?qū)崿F(xiàn)高精度定位和導(dǎo)航，并取得良好效果。
【專利說明】一種用于導(dǎo)航系統(tǒng)航向角直接校正補(bǔ)償方法

【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及一種用于導(dǎo)航系統(tǒng)航向角直接校正及補(bǔ)償方法，屬于導(dǎo)航系統(tǒng)定位【技術(shù)領(lǐng)域】。

【背景技術(shù)】
[0002]在導(dǎo)航系統(tǒng)的發(fā)展過程中，導(dǎo)航系統(tǒng)一個最重要的功能就是提供載體的準(zhǔn)確姿態(tài)及航向角信息。對于僅僅使用加速度計和陀螺儀組成慣性測量單元(MU)，由于加速度計和陀螺儀自身的誤差及漂移使得MU輸出的姿態(tài)和航向參數(shù)精度不能滿足一些導(dǎo)航系統(tǒng)的要求。由慣性測量單元(IMU)和磁力計構(gòu)成的導(dǎo)航系統(tǒng)，可以提高姿態(tài)角及航向角的精度。磁力計在國外應(yīng)用比較廣泛，但在磁場補(bǔ)償方面仍然存在一定問題，致力于尋找一種高精度磁力計的校正和補(bǔ)償方法是磁力計校正及補(bǔ)償?shù)碾y點(diǎn)所在。
[0003]用地磁場進(jìn)行導(dǎo)航定位，具有無源、無輻射、抗干擾、全天時、全天候、體積小、能耗低的優(yōu)點(diǎn)，因此在飛機(jī)、艦船和潛艇等領(lǐng)域得到廣泛應(yīng)用。導(dǎo)航載體通過磁傳感器測量空間的磁場信息，這些磁場信息不僅包括導(dǎo)航定位所用的地磁場信息，也包括載體自身的干擾磁場信息。高精度的地磁導(dǎo)航過程中需要對磁傳感器的觀測磁場信息進(jìn)行處理，實(shí)時對載體磁場干擾進(jìn)行補(bǔ)償，提高地磁導(dǎo)航精度。
[0004]目前，有關(guān)對導(dǎo)航系統(tǒng)中的磁力計進(jìn)行校正及補(bǔ)償方法已經(jīng)很普遍，部分方法也很成熟，可以達(dá)到的一定的精度要求。但是，對于高精度要求的場合現(xiàn)有方法還不能滿足，同時現(xiàn)有的方法需要將磁力計的誤差模型轉(zhuǎn)化為橢球模型，估計橢球模型的參數(shù)再轉(zhuǎn)化為誤差模型，這樣處理方法不夠簡潔。


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

[0005]發(fā)明目的:為了克服現(xiàn)有技術(shù)中無法對磁力計進(jìn)行校正及補(bǔ)償以達(dá)到高精度和強(qiáng)魯棒性的情形，本發(fā)明提供一種用于導(dǎo)航系統(tǒng)航向角直接校正補(bǔ)償方法，該方法可以提高磁力計校正及補(bǔ)償?shù)木?，同時該方法魯棒性強(qiáng)，處理過程簡潔。
[0006]為實(shí)現(xiàn)上述目的，本發(fā)明采用的技術(shù)方案為:一種用于導(dǎo)航系統(tǒng)航向角直接校正補(bǔ)償方法，包括以下步驟:
[0007]步驟A，根據(jù)導(dǎo)航系統(tǒng)中影響磁力計三個軸輸出矢量的誤差源及其誤差源特性，建立磁力計輸出誤差模型；
[0008]步驟B，以載體的質(zhì)心為載體坐標(biāo)系的原點(diǎn)，ObYb軸指向載體前進(jìn)方向，ObXb軸與載體前進(jìn)方向垂直指向右，ObZb軸與0bYb，ObXb軸構(gòu)成右手定則建立載體坐標(biāo)系ObXbYbZb ;根據(jù)當(dāng)?shù)氐乩碜鴺?biāo)系建立當(dāng)?shù)厮阶鴺?biāo)系O1X1Y1Z1 ;利用載體坐標(biāo)系ObXbYbZb到當(dāng)?shù)厮阶鴺?biāo)系O1X1Y1Z1的變換矩陣，將磁力計三個軸的輸出矢量由載體坐標(biāo)系轉(zhuǎn)換到當(dāng)?shù)厮阶鴺?biāo)系，即對步驟A得出的磁力計的輸出誤差模型進(jìn)行拓展，得到拓展磁力計輸出誤差模型；
[0009]步驟C，根據(jù)磁力計三個軸輸出矢量在當(dāng)?shù)厮阶鴺?biāo)系中的分布的第一個特征簡化步驟B得到的拓展磁力計輸出誤差模型，進(jìn)而得到用于參數(shù)估計的誤差模型；所述第一個特征是指在理想的條件下磁力計三個軸輸出矢量在當(dāng)?shù)厮阶鴺?biāo)系中O1Z1軸的輸出值是一個恒定的量；所述理想條件是指磁力計不存在誤差干擾，輸出的磁場信息就是地磁場的真實(shí)信息。
[0010]步驟D，通過兩步總體最小二乘方法對步驟C得到的用于參數(shù)估計的誤差模型中的參數(shù)進(jìn)行估計，然后根據(jù)估計出的參數(shù)計算出導(dǎo)航系統(tǒng)航向角，完成導(dǎo)航系統(tǒng)航向角直接校正補(bǔ)償。
[0011]所述步驟A中建立磁力計輸出誤差模型為:
[0012]# = J/ (if ^ //)(I)
[0013]上中，#為校正后的磁力計的三軸輸出矢量，M = f為總誤差矩陣，f -*為總誤差矩陣的逆矩陣，和為磁力計的三軸輸出矢量，bb為零偏置誤差矢量。
[0014]所述步驟B中得出的拓展磁力計輸出誤差模型:
[0015]B1 = ClBb = C^V (3* - bb、⑵
[0016]其中，#是磁力計三個軸輸出矢量在當(dāng)?shù)厮阶鴺?biāo)系中的矢量值，(I是載體坐標(biāo)系到當(dāng)?shù)厮阶鴺?biāo)系的方向余弦矩陣，I表示當(dāng)?shù)厮阶鴺?biāo)系，b表示載體坐標(biāo)系，Jf為校正后的磁力計的三軸輸出矢量，M = M-1, M為總誤差矩陣，M-1為總誤差矩陣的逆矩陣,多為磁力計的三軸輸出矢量，bb為零偏置誤差矢量。
[0017]所述步驟C中得到的用于參數(shù)估計的誤差模型包括以下步驟:
[0018]步驟Cl，根據(jù)導(dǎo)航系統(tǒng)的姿態(tài)信息得到載體的俯仰角P和橫滾角R，利用俯仰角和橫滾角信息可以得到載體坐標(biāo)系到當(dāng)?shù)厮阶鴺?biāo)系的方向余弦矩陣G，而俯仰角P和橫滾角R由導(dǎo)航系統(tǒng)中的加速度計的輸出信息得到，將--代入表達(dá)式(2)可得:
[0019]

【權(quán)利要求】
1.一種用于導(dǎo)航系統(tǒng)航向角直接校正補(bǔ)償方法，其特征在于，包括以下步驟: 步驟A，根據(jù)導(dǎo)航系統(tǒng)中影響磁力計三個軸輸出矢量的誤差源及其誤差源特性，建立磁力計輸出誤差模型； 步驟B，以載體的質(zhì)心為載體坐標(biāo)系的原點(diǎn)，ObYb軸指向載體前進(jìn)方向，ObXb軸與載體前進(jìn)方向垂直指向右，ObZb軸與0bYb，ObXb軸構(gòu)成右手定則建立載體坐標(biāo)系ObXbYbZb ;根據(jù)當(dāng)?shù)氐乩碜鴺?biāo)系建立當(dāng)?shù)厮阶鴺?biāo)系O1X1Y1Z1 ;利用載體坐標(biāo)系ObXbYbZb到當(dāng)?shù)厮阶鴺?biāo)系O1X1Y1Z1的變換矩陣，將磁力計三個軸的輸出矢量由載體坐標(biāo)系轉(zhuǎn)換到當(dāng)?shù)厮阶鴺?biāo)系，即對步驟A得出的磁力計的輸出誤差模型進(jìn)行拓展，得到拓展磁力計輸出誤差模型； 步驟C，根據(jù)磁力計三個軸輸出矢量在當(dāng)?shù)厮阶鴺?biāo)系中的分布的第一個特征簡化步驟B得到的拓展磁力計輸出誤差模型，進(jìn)而得到用于參數(shù)估計的誤差模型；所述第一個特征是指在理想的條件下磁力計三個軸輸出矢量在當(dāng)?shù)厮阶鴺?biāo)系中O1Z1軸的輸出值是一個恒定的量； 步驟D，通過兩步總體最小二乘方法對步驟C得到的用于參數(shù)估計的誤差模型中的參數(shù)進(jìn)行估計，然后根據(jù)估計出的參數(shù)計算出導(dǎo)航系統(tǒng)航向角，完成導(dǎo)航系統(tǒng)航向角直接校正補(bǔ)償。
2.根據(jù)權(quán)利要求1所述的用于導(dǎo)航系統(tǒng)航向角直接校正補(bǔ)償方法，其特征在于:步驟A中建立磁力計輸出誤差模型為:
上中，r為校正后的磁力計的三軸輸出矢量，.= i為總誤差矩陣，為總誤差矩陣的逆矩陣，#為磁力計的三軸輸出矢量，bb為零偏置誤差矢量。
3.根據(jù)權(quán)利要求2所述的用于導(dǎo)航系統(tǒng)航向角直接校正補(bǔ)償方法，其特征在于:步驟B中得出的拓展磁力計輸出誤差模型:
其中，I是磁力計三個軸輸出矢量在當(dāng)?shù)厮阶鴺?biāo)系中的矢量值，G是載體坐標(biāo)系到當(dāng)?shù)厮阶鴺?biāo)系的方向余弦矩陣，I表示當(dāng)?shù)厮阶鴺?biāo)系，b表示載體坐標(biāo)系，i*為校正后的磁力計的三軸輸出矢量，M = f為總誤差矩陣，1-為總誤差矩陣的逆矩陣，P為磁力計的三軸輸出矢量，bb為零偏置誤差矢量。
4.根據(jù)權(quán)利要求3所述的用于導(dǎo)航系統(tǒng)航向角直接校正補(bǔ)償方法，其特征在于:步驟C中得到的用于參數(shù)估計的誤差模型包括以下步驟: 步驟Cl，根據(jù)導(dǎo)航系統(tǒng)的姿態(tài)信息得到載體的俯仰角P和橫滾角R，利用俯仰角和橫滾角信息可以得到載體坐標(biāo)系到當(dāng)?shù)厮阶鴺?biāo)系的方向余弦矩陣< ,而俯仰角P和橫滾角R由導(dǎo)航系統(tǒng)中的加速度計的輸出信息得到，將G代入表達(dá)式⑵可得:
(a' n? a1 12? 13，a' 21，a' 22，a' 23，a' 31，a' 32，a' 33)為矩陣 M 的各個分星，(bn, b12, b13, b21, b22, b23, b31, b32, b33)為矩陣('b 的各個分量，Oi1, n2, n3)為矢量 bb 的各個分量，(x, y, z)為矢量//的各個分量，(Xtl, J0, ζ0)為矢量/}-的各個分量。 步驟C2，根據(jù)磁力計三個軸輸出矢量在當(dāng)?shù)厮阶鴺?biāo)系中的分布的第一個特征對步驟Cl中的式(3)進(jìn)行化簡，可得用于參數(shù)估計的誤差模型:
Ciail+C2ai2+C3ai3+C4a21+C5a22+C6a23+C7a31+C8a32+C9a33+C10 Q +Cll ^ +C12、— !(4)
5.根據(jù)權(quán)利要求4所述的用于導(dǎo)航系統(tǒng)航向角直接校正補(bǔ)償方法，其特征在于:步驟D中兩步總體最小二乘方法對參數(shù)估計誤差模型中的參數(shù)進(jìn)行估計的方法，包括以下步驟:步驟D1，將磁力計三個軸輸出矢量代入步驟C2中得出的參數(shù)估計誤差模型，得到:
表不磁力計二個軸出矢里的第i組數(shù)據(jù)，η表示磁力計三個軸輸出矢量的第η組數(shù)據(jù)，總共η組數(shù)據(jù)；
馬來 D2，通過取小._?乘 /去估計參數(shù)(&11，^12^13 ? ^21^22 ? ^23^31 ? ^32 ? ^33 ?。，_β，Y )，首先，通過A的最佳逼近矩陣，將步驟Dl中的式(5)轉(zhuǎn)化為: A1 ο = (Α+Ε) σ = b (6)其中，A'為A的最佳逼近矩陣，A' =A+E，E為A中含有的系統(tǒng)誤差干擾； 然后，記，矩陣C= [A b]，對矩陣C進(jìn)行奇異值分解，可得:UTCV = E，其中，U、V均為正交矩陣，Σ為對角陣，上標(biāo)T表示矩陣轉(zhuǎn)置；記矩陣V= [V1 V^V12 V13],其中(V1, V2,…，V12,V13)分別為矩陣V的列向量； 最后，得到表達(dá)式(5)的解為:
該解為參數(shù)(a!!，^12，3^3，hi，&22? ^23? ^31? ^32? ^33? ^ ? β ? Y )的值，
步驟 D3,利用步驟 D2 得到的參數(shù)(an, a12, a13, a21, a22, a23, a31, a32, a33, α，β，y)計算參數(shù)Oi1, η2, η3),由式⑷可得:
式(J)是一組線性方程組，解上述線性方程組可以得到參數(shù)(ηι，η2，η3)的值。
6.根據(jù)權(quán)利要求5所述的用于導(dǎo)航系統(tǒng)航向角直接校正補(bǔ)償方法，其特征在于:所述理想條件是指磁力計不存在誤差干擾，輸出的磁場信息就是地磁場的真實(shí)信息。
【文檔編號】G01C25/00GK104165642SQ201410439211
【公開日】2014年11月26日 申請日期:2014年8月29日 優(yōu)先權(quán)日:2014年8月29日
【發(fā)明者】陳熙源, 呂才平, 黃浩乾, 方琳, 臧云歌 申請人:東南大學(xué)