一種無gps信號環(huán)境下的抗粗差組合導(dǎo)航方法
【專利摘要】本發(fā)明涉及一種無GPS信號環(huán)境下的抗粗差組合導(dǎo)航方法，包括以下步驟：SINS初始化；INS導(dǎo)航計算，獲得車輛當前時刻的速度、位置和姿態(tài)；卡爾曼濾波時間更新，得到系統(tǒng)的狀態(tài)一步預(yù)測值及相應(yīng)的均方誤差；計算濾波新息，并根據(jù)濾波新息判斷是否存在粗差，若是，則剔除相應(yīng)的觀測值，若否，則保留相應(yīng)的觀測值；將剔除粗差后的觀測值作為卡爾漫濾波器的輸入進行濾波量測更新，獲得狀態(tài)最優(yōu)估計值及相應(yīng)的均方誤差；利用狀態(tài)最優(yōu)估計值進行反饋校正，獲得修正后的車輛位置、速度和姿態(tài)矩陣。與現(xiàn)有技術(shù)相比，本發(fā)明具有低成本、導(dǎo)航穩(wěn)定、精度高等優(yōu)點。
【專利說明】一種無GPS信號環(huán)境下的抗粗差組合導(dǎo)航方法

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種車載導(dǎo)航方法，尤其是涉及一種無GPS信號環(huán)境下的抗粗差組合 導(dǎo)航方法。

【背景技術(shù)】
[0002]室內(nèi)和地下等無GPS(GlobalPositioningSystem,全球定位系統(tǒng)）信號環(huán)境下 的導(dǎo)航定位技術(shù)在我國具有重大需求，可應(yīng)用于室內(nèi)導(dǎo)航、室內(nèi)移動測圖系統(tǒng)、地下交通導(dǎo) 航定位、地下作業(yè)安全等領(lǐng)域，對于地下隧道開挖、礦產(chǎn)資源的勘探、礦山開采和事故應(yīng)急 等應(yīng)用具有重要意義。
[0003] 慣性導(dǎo)航系統(tǒng)（InertialNavigationSystem,INS)是一種自主導(dǎo)航系統(tǒng)，以牛頓 力學(xué)定律為工作原理，利用安裝在載體上的慣性測量元件（InertialMeasurementUnit, IMU)來敏感載體的角速度和加速度信息，通過積分運算得到載體的速度、位置和姿態(tài)等 導(dǎo)航信息，MU包括加速度計和陀螺。慣導(dǎo)系統(tǒng)按有無實體平臺，可以分為兩類：平臺式 慣性導(dǎo)航系統(tǒng)（GimbaledInertialNavigationSystem,GINS)和捷聯(lián)式慣性導(dǎo)航系統(tǒng) (StrapdownInertialNavigationSystem,SINS)。在捷聯(lián)慣導(dǎo)系統(tǒng)中，慣性測量元件直接 安裝在載體上，由于沒有實體平臺，捷聯(lián)慣導(dǎo)具有結(jié)構(gòu)簡單、體積小、成本低等優(yōu)點，被廣泛 應(yīng)用于車輛導(dǎo)航系統(tǒng)中。慣性導(dǎo)航系統(tǒng)在短時間內(nèi)具有高精度，但是由于積分原理，其誤差 隨時間累積，特別是車載導(dǎo)航系統(tǒng)中，考慮成本因素，通常采用的MU精度較低，其誤差累 積更快，長時間工作的精度較差。
[0004] 為了減少誤差累積，要引入外部觀測值來對慣導(dǎo)系統(tǒng)進行修正。里程計是測量車 輛運行距離/速度的裝置，其價格低、使用方便，是車輛導(dǎo)航中經(jīng)常采用的裝置，經(jīng)過刻度 因子的檢校和補償以后，其速度觀測值一般可以達到優(yōu)于1%的精度，因此可以用來修正慣 性導(dǎo)航系統(tǒng)的速度誤差。電子羅盤通過測量地球磁場在敏感軸上的分量來確定磁航向角， 經(jīng)過地磁偏角的補償以后，航向角的精度可以達到0. 5度以內(nèi)，將其安裝于車輛，可以提供 航向信息，用來修正慣性導(dǎo)航系統(tǒng)的姿態(tài)誤差。因此可以構(gòu)建捷聯(lián)慣導(dǎo)/里程計/電子羅 盤的組合導(dǎo)航系統(tǒng)，有效利用各自的優(yōu)點，在低成本的情況下實現(xiàn)無GPS信號環(huán)境下的自 主導(dǎo)航。
[0005] 在捷聯(lián)慣導(dǎo)/里程計/電子羅盤的組合導(dǎo)航系統(tǒng)中，采用卡爾曼濾波進行最優(yōu)數(shù) 據(jù)融合，將里程計的速度觀測值與電子羅盤的速度觀測值作為濾波器的部分輸入，通過濾 波獲得系統(tǒng)狀態(tài)（速度、位置、姿態(tài)）的最優(yōu)估計。
[0006] 然而，由于車輛運行環(huán)境的復(fù)雜性，經(jīng)常會受到各種局部和短暫的干擾。具體情況 有：車輛經(jīng)過松軟的沙土或者碰到地面隆起的障礙物，可能出現(xiàn)車輪打滑的現(xiàn)象；車輛運 行過程中，可能經(jīng)過鐵磁物質(zhì)附近，導(dǎo)致電子羅盤的輸出值異常。如果出現(xiàn)車輪打滑的情 況，則里程計輸出的速度觀測值將嚴重偏離車輛的真實速度；如果出現(xiàn)強磁場的干擾，則電 子羅盤輸出的航向角將嚴重偏離車輛的真實航向角。以上兩種情況下輸出的觀測值都可以 稱為"粗差"。粗差觀測值如果作為卡爾曼濾波器的輸入，將引起濾波器的震蕩，甚至導(dǎo)致濾 波器發(fā)散，這將影響整個系統(tǒng)的精度和穩(wěn)定性。因此本發(fā)明從濾波器新息的角度分析觀測 值的質(zhì)量，識別出粗差觀測值并剔除。


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

[0007] 本發(fā)明的目的就是為了克服上述現(xiàn)有技術(shù)存在的缺陷而提供一種低成本、導(dǎo)航穩(wěn) 定和精度高的無GPS信號環(huán)境下的抗粗差組合導(dǎo)航方法。
[0008] 本發(fā)明的目的可以通過以下技術(shù)方案來實現(xiàn)：
[0009] 一種無GPS信號環(huán)境下的抗粗差組合導(dǎo)航方法，該方法通過SINS、里程計和電子 羅盤的組合對在無GPS信號環(huán)境下運作的車輛進行導(dǎo)航，具體包括以下步驟：
[0010]I)SINS初始化，獲得車輛的初始位置和初始姿態(tài)矩陣；
[0011] 2)根據(jù)IMU、里程計、電子羅盤的實時測量數(shù)據(jù)及步驟1)中的初始位置和初始姿 態(tài)矩陣，進行INS導(dǎo)航計算，獲得車輛當前時刻的速度、位置和姿態(tài)；
[0012] 3)卡爾曼濾波時間更新，得到系統(tǒng)的狀態(tài)一步預(yù)測值及相應(yīng)的均方誤差；
[0013] 4)根據(jù)步驟3)中系統(tǒng)的狀態(tài)一步預(yù)測值，以及里程計和電子羅盤的觀測值，計算 濾波新息，并根據(jù)濾波新息判斷是否存在粗差，若是，則剔除相應(yīng)的觀測值，若否，則保留相 應(yīng)的觀測值；
[0014] 5)將剔除粗差后的觀測值作為卡爾漫濾波器的輸入進行濾波量測更新，獲得狀態(tài) 最優(yōu)估計值及相應(yīng)的均方誤差；
[0015] 6)利用狀態(tài)最優(yōu)估計值進行反饋校正，獲得修正后的車輛位置、速度和姿態(tài)矩陣。
[0016] 所述的車輛的初始位置根據(jù)環(huán)境中的已知參考點獲得或通過全站儀測量得到。
[0017] 所述的車輛的初始姿態(tài)矩陣 < 為：

【權(quán)利要求】
1. 一種無 GPS信號環(huán)境下的抗粗差組合導(dǎo)航方法，其特征在于，該方法通過SINS、里程 計和電子羅盤的組合對在無 GPS信號環(huán)境下運作的車輛進行導(dǎo)航，具體包括以下步驟： 1. SINS初始化，獲得車輛的初始位置和初始姿態(tài)矩陣； 2) 根據(jù)IMU、里程計、電子羅盤的實時測量數(shù)據(jù)及步驟1)中的初始位置和初始姿態(tài)矩 陣，進行INS導(dǎo)航計算，獲得車輛當前時刻的速度、位置和姿態(tài)； 3) 卡爾曼濾波時間更新，得到系統(tǒng)的狀態(tài)一步預(yù)測值及相應(yīng)的均方誤差； 4) 根據(jù)步驟3)中系統(tǒng)的狀態(tài)一步預(yù)測值，以及里程計和電子羅盤的觀測值，計算濾波 新息，并根據(jù)濾波新息判斷是否存在粗差，若是，則剔除相應(yīng)的觀測值，若否，則保留相應(yīng)的 觀測值； 5) 將剔除粗差后的觀測值作為卡爾漫濾波器的輸入進行濾波量測更新，獲得狀態(tài)最優(yōu) 估計值及相應(yīng)的均方誤差； 6) 利用狀態(tài)最優(yōu)估計值進行反饋校正，獲得修正后的車輛位置、速度和姿態(tài)矩陣。
2. 根據(jù)權(quán)利要求1所述的一種無 GPS信號環(huán)境下的抗粗差組合導(dǎo)航方法，其特征在于， 所述的車輛的初始位置根據(jù)環(huán)境中的已知參考點獲得或通過全站儀測量得到。
3. 根據(jù)權(quán)利要求1所述的一種無 GPS信號環(huán)境下的抗粗差組合導(dǎo)航方法，其特征在于， 所述的車輛的初始姿態(tài)矩陣Ι?為：
式中，車輛坐標系為b系，SINS的導(dǎo)航坐標系選擇當?shù)厮阶鴺讼?，為L系，y為電子羅 盤觀測得的航向角，P、r為計算得的車輛俯仰角和翻滾角，且
其中，g為重力加速度，gf gff為車輛靜止設(shè)定時間內(nèi)，SINS中的加速度計 觀測得的比力向量均值。
4. 根據(jù)權(quán)利要求3所述的一種無 GPS信號環(huán)境下的抗粗差組合導(dǎo)航方法，其特征在于， 所述的INS導(dǎo)航計算具體包括以下步驟： 201) 姿態(tài)更新：利用SINS中的陀螺輸出的角速度觀測值，采用四元數(shù)法進行姿態(tài)更 新，然后將更新后的四元數(shù)轉(zhuǎn)化成姿態(tài)矩陣，記為祀; 202) 姿態(tài)提?。焊鶕?jù)更新后的姿態(tài)矩陣圮（A)中元素的值，計算車輛的姿態(tài)角（p，r， y)； 203) 比力分解：將加速度計輸出的比力觀測值fb由b系轉(zhuǎn)換到L系： 204) 加速度的修正：對產(chǎn)進行哥氏加速度修正和正常重力補償，得到車輛在地球表面 的加速度a : α = /1-(2Ω^ + Ω；,)νζ·+^ 式中，分別表示地球自轉(zhuǎn)角速度和導(dǎo)航系的轉(zhuǎn)移速率，兩者都表達在L系中， /為車輛上一時刻的速度，為重力加速度在L系中的分量； 205) 積分求取速度：利用加速度進行積分得到速度增量，進而得到k時刻的速度Vj^ ν* =ν^_1 + α·Δ? 式中，< :為k-1時刻的速度，Λ t為時間間隔； 206) 積分求取位置：利用速度進行積分得到位置增量，進而得到k時刻的位置·τ/;
式中，A為k-1時刻的位置。
5. 根據(jù)權(quán)利要求4所述的一種無 GPS信號環(huán)境下的抗粗差組合導(dǎo)航方法，其特征在于， 所述的卡爾曼濾波時間更新具體包括以下步驟： 301) 設(shè)定卡爾曼濾波器的狀態(tài)向量為 X=(5rL, δ vL, δ ε L, d, b, δ e) 式中，X為16階誤差向量，其中δΡ為SINS的三維位置誤差，δ/為SINS的三維速 度誤差，S 一為SINS的平臺失準角，d為三軸陀螺漂移，b為三軸加速度計零偏，Se為里 程計的刻度因子誤差； 302) 根據(jù)誤差向量建立SINS的狀態(tài)方程： Xk=Ou-Uru-IWk-I 式中，Xlrt和Xk分別表示SINSk-I和k時刻的狀態(tài)，OkI 1為狀態(tài)轉(zhuǎn)移矩陣，Wlrt為動態(tài) 噪聲，為噪聲輸入矩陣： 303) 根據(jù)狀態(tài)方程計算SINS的狀態(tài)一步預(yù)測值及相應(yīng)的均方誤差：
6. 根據(jù)權(quán)利要求5所述的一種無 GPS信號環(huán)境下的抗粗差組合導(dǎo)航方法，其特征在于， 所述的是否存在粗差具體指： 當速度誤差量測值Zv滿足以下關(guān)系時，判斷相應(yīng)的觀測值為粗差：
式中，Zv =C，F(xiàn)i為INS導(dǎo)航計算得到的車輛速度，C為里程計在L系中測得 的車輛速度，I I I I2表示2范數(shù)，Hv為速度觀測矩陣，i為大于0的比例系數(shù)，j為大于0的 常數(shù)； 當航向角誤差量測值Za滿足以下關(guān)系時，判斷相應(yīng)的觀測值為粗差：
式中，Za=yins-ymag，yins為INS導(dǎo)航計算得到的車輛航向角，y mag為電子羅盤測得的車輛 航向角，Ha為航向角觀測矩陣，δ yins是一定時間間隔內(nèi)INS導(dǎo)航計算得到的航向角變化值， δ ymag是一定時間間隔內(nèi)電子羅盤輸出的航向角變化值，為設(shè)定的閾值。
7. 根據(jù)權(quán)利要求5所述的一種無 GPS信號環(huán)境下的抗粗差組合導(dǎo)航方法，其特征在于， 所述的步驟6)中的反饋校正具體為： 位置、速度、姿態(tài)直接通過以下公式修正：
式中，產(chǎn)、鏟、分別為修正后的位置、速度和姿態(tài)矩陣，r￡、#是INS導(dǎo)航計算 得到的位置、速度和姿態(tài)矩陣，ε 1X)為三維反對稱陣； 里程計的刻度因子通過下式進行修正： 式中，I:是經(jīng)過修正后的里程計刻度因子，e是修正前的值； 陀螺漂移和加速度計零偏反饋到下一個周期的INS導(dǎo)航計算中。
8. 根據(jù)權(quán)利要求1所述的一種無 GPS信號環(huán)境下的抗粗差組合導(dǎo)航方法，其特征在于， 該方法還包括：在導(dǎo)航開始之前的檢校步驟，該檢校步驟包括里程計刻度因子檢校和電子 羅盤檢校。
9. 根據(jù)權(quán)利要求8所述的一種無 GPS信號環(huán)境下的抗粗差組合導(dǎo)航方法，其特征在于， 所述的里程計刻度因子e的檢校公式為：
1為設(shè)定的地面直線的長度，m為安裝有里程計的車輛沿地面直線運行時輸出的脈沖 數(shù)，η為實驗次數(shù)。
【文檔編號】G01C21/28GK104515527SQ201310451709
【公開日】2015年4月15日 申請日期:2013年9月27日 優(yōu)先權(quán)日:2013年9月27日
【發(fā)明者】周發(fā)根, 王錢佳 申請人:上海置微信息科技有限公司