[發(fā)明專利]一種捷聯(lián)慣性導(dǎo)航的誤差校正方法及裝置

在審

• 申請?zhí)枺?/td>: 201710131897.5
• 申請日: 2017-03-07
• 公開（公告）號: CN108571980A
• 公開（公告）日: 2018-09-25
• 發(fā)明（設(shè)計）人: 汪勇;胡祝銀;林威;胡樹立;章小兵
• 申請（專利權(quán)）人: 深圳市博安智控科技有限公司
• 主分類號: G01C25/00
• 分類號: G01C25/00
• 代理公司: 重慶百潤洪知識產(chǎn)權(quán)代理有限公司 50219
• 代理人: 劉立春

本發(fā)明涉及導(dǎo)航定位技術(shù)領(lǐng)域，公開了一種捷聯(lián)慣性導(dǎo)航的誤差校正方法及裝置，通過對加速度計，磁力計進(jìn)行姿態(tài)解算，以獲取終端的第一姿態(tài)數(shù)據(jù)，根據(jù)上一次的第一姿態(tài)數(shù)據(jù)與當(dāng)前的第一姿態(tài)數(shù)據(jù)，計算第一姿態(tài)變化率，將所述第一姿態(tài)數(shù)據(jù)作為陀螺儀的初始姿態(tài)，采用四元數(shù)進(jìn)行陀螺儀姿態(tài)解算，以獲取第二姿態(tài)數(shù)據(jù)，根據(jù)上一次的第二姿態(tài)數(shù)據(jù)與當(dāng)前的第二姿態(tài)數(shù)據(jù)，計算第二姿態(tài)變化率，跟蹤偏差率的周期性變化，當(dāng)所述偏差率達(dá)到極值時，根據(jù)所述第一姿態(tài)數(shù)據(jù)、所述第二姿態(tài)數(shù)據(jù)及所述姿態(tài)可信度，計算第三姿態(tài)數(shù)據(jù)，并通過所述第三姿態(tài)數(shù)據(jù)初始化陀螺儀姿態(tài)，以校正陀螺儀積分誤差。本發(fā)明解算的終端姿態(tài)能避免陀螺儀因長時間運動模式積分誤差帶來的終端姿態(tài)失效問題。

技術(shù)領(lǐng)域

本發(fā)明涉及導(dǎo)航定位技術(shù)領(lǐng)域，尤其涉及一種捷聯(lián)慣性導(dǎo)航的誤差校正方法及裝置。

背景技術(shù)

捷聯(lián)慣性導(dǎo)航系統(tǒng)(Strapdown Inertial Navigation System簡稱SINS)是根據(jù)牛頓提出的相對慣性空間的力學(xué)定律獲得載體導(dǎo)航信息的裝置。它利用磁力計、陀螺儀、加速度計等慣性元件感受運行體運動信息，然后通過計算機(jī)進(jìn)行積分運算得到運動體的姿態(tài)、速度和位置等導(dǎo)航參數(shù)。捷聯(lián)慣性導(dǎo)航可實現(xiàn)無外部全球定位信號如GPS、北斗，或其他定位技術(shù)(如wifi，RFID)時，僅僅通過載體對相關(guān)傳感器采集數(shù)據(jù)的計算進(jìn)行自主導(dǎo)航。

SINS信息豐富，信號頻率高，工作方式隱蔽，但SINS中磁力計容易受到外部磁場干擾，且響應(yīng)水平方向旋轉(zhuǎn)遲緩，無法實時準(zhǔn)確測定終端水平方向；受陀螺儀漂移誤差累積的影響，系統(tǒng)精度會隨著時間的推移而降低；加速度計運動狀態(tài)下，因外部作用力的影響，難以確定姿態(tài)；現(xiàn)有技術(shù)中，采用融合姿態(tài)解算方法提高精確度，例如：

1、通過加速度計與磁力計，計算方向角，與陀螺儀計算方向角進(jìn)行卡爾曼濾波獲得濾波后姿態(tài)。

2、通過加速度計與磁力計，計算方向角，與陀螺儀計算方向角進(jìn)行一階、二階互補(bǔ)濾波后姿態(tài)。

3、采用低通濾波和高通濾波的結(jié)合方式，以實現(xiàn)加速度計、磁力計、與陀螺儀的參數(shù)融合解算。

以上幾種方案能獲得矯正后姿態(tài)，最終姿態(tài)收斂，但中間姿態(tài)可能出現(xiàn)較大誤差，如外部磁場干擾，或移動終端持續(xù)單向加速將對中間姿態(tài)產(chǎn)生較大誤差。以上幾種濾波方式，主要考慮對白噪聲，以及頻率特性進(jìn)行濾波，未考慮移動終端的運動特性引入的隨機(jī)干擾。因此，基于以上算法獲得的姿態(tài)，進(jìn)行運動加速度積分得到的運動軌跡存在較大誤差。

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

本發(fā)明提供一種捷聯(lián)慣性導(dǎo)航的誤差校正方法及裝置，解決現(xiàn)有技術(shù)中運動特性引入的隨機(jī)干擾導(dǎo)致運動加速度積分得到的運動軌跡存在較大誤差的技術(shù)問題。

本發(fā)明的目的是通過以下技術(shù)方案實現(xiàn)的：

一種捷聯(lián)慣性導(dǎo)航的誤差校正方法，包括：

對加速度計，磁力計進(jìn)行姿態(tài)解算，以獲取終端的第一姿態(tài)數(shù)據(jù)；

根據(jù)上一次的第一姿態(tài)數(shù)據(jù)與當(dāng)前的第一姿態(tài)數(shù)據(jù)，計算第一姿態(tài)變化率；

將所述第一姿態(tài)數(shù)據(jù)作為陀螺儀的初始姿態(tài)，采用四元數(shù)進(jìn)行陀螺儀姿態(tài)解算，以獲取第二姿態(tài)數(shù)據(jù)；

根據(jù)上一次的第二姿態(tài)數(shù)據(jù)與當(dāng)前的第二姿態(tài)數(shù)據(jù)，計算第二姿態(tài)變化率；

跟蹤偏差率的周期性變化，當(dāng)所述偏差率達(dá)到極值時，根據(jù)所述第一姿態(tài)數(shù)據(jù)、所述第二姿態(tài)數(shù)據(jù)及所述姿態(tài)可信度，計算第三姿態(tài)數(shù)據(jù)，并通過所述第三姿態(tài)數(shù)據(jù)初始化陀螺儀姿態(tài)，以校正陀螺儀積分誤差，其中，所述姿態(tài)可信度由所述偏差率及修正參數(shù)計算獲得。

一種捷聯(lián)慣性導(dǎo)航的誤差校正裝置，包括：

第一姿態(tài)計算模塊，用于對加速度計，磁力計進(jìn)行姿態(tài)解算，以獲取終端的第一姿態(tài)數(shù)據(jù)；