本發(fā)明屬于飛行器組合導(dǎo)航技術(shù)領(lǐng)域，特別涉及一種可配置的慣性/天文/北斗多組合導(dǎo)航系統(tǒng)及其導(dǎo)航方法。

背景技術(shù)：

北斗導(dǎo)航系統(tǒng)是我國自主研發(fā)的衛(wèi)星導(dǎo)航系統(tǒng)，其在國防安全及國民經(jīng)濟上起著不可或缺的作用。另外，近年我國遠程長航時飛行器發(fā)展迅速，遠程長航時飛行器對其導(dǎo)航系統(tǒng)的高精度、高可靠性提出了較高的要求，由于天文導(dǎo)航不受電磁干擾且精度高，有著其獨特的優(yōu)勢。因此慣性/天文/北斗組合導(dǎo)航方法的研究有著較高的軍用價值。

目前大部分單線程導(dǎo)航程序移植性差，在跨平臺移植過程中浪費大量的物力和人力，單線程模式對導(dǎo)航計算機的利用率低，從而造成導(dǎo)航解算實時性低。鑒于我國導(dǎo)航系統(tǒng)的迅速發(fā)展及飛行器對可配置導(dǎo)航裝置的需求，可配置的慣性/天文/北斗多組合導(dǎo)航系統(tǒng)還需要進一步的研究。

技術(shù)實現(xiàn)要素：

本發(fā)明的目的，在于提供一種可配置的慣性/天文/北斗多組合導(dǎo)航系統(tǒng)及導(dǎo)航方法，其可有效解決導(dǎo)航方法的可移植性、實時性、精確性等問題，為教學(xué)、科研、多組合導(dǎo)航系統(tǒng)的工程化和產(chǎn)品化提供有益參考。

為了達成上述目的，本發(fā)明的解決方案是：

一種可配置的慣性/天文/北斗多組合導(dǎo)航系統(tǒng)，包括導(dǎo)航計算機模塊、慣性元件、北斗接收機、星敏感器和上位機，慣性元件、北斗接收機和星敏感器將數(shù)據(jù)送入導(dǎo)航計算機模塊，導(dǎo)航計算機模塊進行處理后將導(dǎo)航解算結(jié)果送入上位機實時顯示，上位機同時對導(dǎo)航計算機模塊進行控制。

上述導(dǎo)航計算機模塊采用pc機、工作站、基于linux系統(tǒng)或windows系統(tǒng)的嵌入式模塊。

上述導(dǎo)航計算機模塊與慣性元件、北斗接收機、星敏感器、上位機之間通過rs232串口或usb接口進行通信。

上述慣性元件采用imu慣性元件或mems慣性元件。

一種可配置的慣性/天文/北斗多組合導(dǎo)航方法，包括如下步驟：

步驟1，多組合導(dǎo)航系統(tǒng)慣導(dǎo)誤差狀態(tài)量方程建模

對于可配置的慣性/天文/北斗多組合導(dǎo)航系統(tǒng)，速度計算公式為：

式中，為載體相當(dāng)于地理系的加速度在載體上的投影，為載體相當(dāng)于地理系的速度在載體上的投影，為機體系到導(dǎo)航坐標(biāo)系的姿態(tài)轉(zhuǎn)移矩陣，為載體相當(dāng)于地理坐標(biāo)系的速度在導(dǎo)航坐標(biāo)系上的投影，其中，地理坐標(biāo)系即為導(dǎo)航坐標(biāo)系；

位置計算公式為：

用四元數(shù)描述載體的姿態(tài)運動，計算公式為：

用矩陣表示為：

式中，q為姿態(tài)四元數(shù)，為q對時間的導(dǎo)數(shù)，矢量為四元數(shù)形式，表示載體坐標(biāo)系相對導(dǎo)航坐標(biāo)系在載體坐標(biāo)系上的投影；

在上述基礎(chǔ)上，建立組合導(dǎo)航系統(tǒng)狀態(tài)方程為：

其中，狀態(tài)變量為：

表示捷聯(lián)慣性導(dǎo)航在地里坐標(biāo)系下東、北、天方向平臺誤差角，δve,δvn,δvu表示慣性導(dǎo)航系統(tǒng)在地理坐標(biāo)系下東、北、天方向的速度誤差；δl,δλ,δh表示慣性導(dǎo)航系統(tǒng)在地里坐標(biāo)系下緯度、經(jīng)度、高度的誤差；εbx,εby,εbz表示陀螺隨機常數(shù)誤差；εrx,εry,εrz表示陀螺一階馬爾可夫過程隨機誤差；▽x,▽y,▽z表示加速度計的一階馬爾可夫過程隨機誤差；a(t)18×18為系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣；g(t)18×9為噪聲系數(shù)矩陣；w(t)9×1為系統(tǒng)的白噪聲矢量；

步驟2，組合導(dǎo)航系統(tǒng)量測方程建模

利用北斗導(dǎo)航系統(tǒng)的位置信息對慣導(dǎo)的位置、速度信息進行濾波估計，利用天文姿態(tài)信息對慣導(dǎo)的姿態(tài)信息進行濾波估計。

上述步驟2中，在sins/cns子系統(tǒng)中，定義觀測量為星敏感器測得的載體姿態(tài)角與捷聯(lián)慣導(dǎo)的姿態(tài)角之差，量測方程如下：

zs(t)＝hs(t)x(t)+vs(t)(9)

由于組合導(dǎo)航系統(tǒng)狀態(tài)方程中的誤差角狀態(tài)量為慣性導(dǎo)航平臺誤差角，它描述的是數(shù)學(xué)平臺與地里面坐標(biāo)系之間的誤差角關(guān)系，因此需要將慣性姿態(tài)誤差角轉(zhuǎn)化到平臺誤差角，在平臺誤差角為小量時，量測矩陣由以下公式表示：

因此，量測方程表示為：

式中，γ、θ、ψ分別為橫滾角、俯仰角和航向角，vs(t)為姿態(tài)誤差角量測噪聲。

上述步驟2中，對于慣性/北斗、慣性/天文濾波模塊，各濾波模塊過程如下：

其中，表示tk-1時刻的狀態(tài)對tk時刻的狀態(tài)的最優(yōu)估計值，φk/k-1表示tk-1時刻至tk時刻系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣，表示tk-1時刻的系統(tǒng)狀態(tài)估計值，kk表示增益矩陣，hk表示濾波模塊tk時刻的觀測系數(shù)矩陣，pk/k-1表示最優(yōu)預(yù)測估計誤差協(xié)方差陣，pk/k表示濾波模塊tk時刻的系統(tǒng)誤差協(xié)方差陣，qk-1表示tk-1時刻的噪聲方差矩陣，γk/k-1表示tk-1時刻的噪聲矢量對tk時刻狀態(tài)矢量影響的噪聲系數(shù)矩陣，rk表示tk時刻的量測方差矩陣，i為單位矩陣。

采用上述方案后，本發(fā)明的有益效果如下：

(1)整個導(dǎo)航程序采用的是多線程并發(fā)模式，保證了導(dǎo)航解算的實時性和導(dǎo)航模塊的可擴展性。同一導(dǎo)航程序可以跨平臺使用，增強了系統(tǒng)的可移植性，減少了程序開發(fā)的成本。

(2)導(dǎo)航計算機和傳感器之間支持熱插拔模式，導(dǎo)航計算機對可用串口實時監(jiān)測并自動識別鏈接，保證了傳感器的即插即用，增加了導(dǎo)航裝置使用的便利性。

(3)不同線程在訪問臨界資源時采用加鎖的保護方式，保證了導(dǎo)航結(jié)果的正確性，避免了程序因訪問臨界資源而崩潰的現(xiàn)象。

(4)整個導(dǎo)航模式可以通過上位機在線配置，選擇不同的解算周期及組合模式，增強了系統(tǒng)的適用性。

附圖說明

圖1是本發(fā)明導(dǎo)航系統(tǒng)的整體架構(gòu)圖。

具體實施方式

以下將結(jié)合附圖，對本發(fā)明的技術(shù)方案進行詳細說明。

如圖1所示，本發(fā)明提供一種可配置的慣性/天文/北斗多組合導(dǎo)航系統(tǒng)及其導(dǎo)航方法，包括軟件部分和硬件部分，軟件部分是基于qtcreator開發(fā)的，采用多線程并發(fā)的模式，導(dǎo)航程序由捷聯(lián)慣導(dǎo)解算線程、sins/bd組合濾波線程、sins/天文組合濾波線程、數(shù)據(jù)通信解碼線程、上位機顯示及模塊配置線程組成，導(dǎo)航程序易于裁剪，實時性高。硬件部分由導(dǎo)航計算機、北斗接收機、imu慣性元件、天文仿真器、上位機等組成，導(dǎo)航計算機可由pc機、工作站、基于linux系統(tǒng)或windows系統(tǒng)的嵌入式模塊承擔(dān)。傳感器與導(dǎo)航計算機之間通過rs232串口或usb接口進行通信，上位機可以實時顯示導(dǎo)航解算結(jié)果，同時上位機還可以對導(dǎo)航線程進行配置，選擇不同的組合模式，增強導(dǎo)航程序?qū)Σ煌h(huán)境下的適應(yīng)性。

導(dǎo)航系統(tǒng)采用傳感器熱插拔技術(shù)。傳感器與導(dǎo)航計算機之間的通信波特率一般是固定的，導(dǎo)航計算機實時查詢可用串口，導(dǎo)航計算機針對不同傳感器的數(shù)據(jù)幀頭進行識別，若識別通過則鎖定串口，該傳感器即進入工作狀態(tài)。數(shù)據(jù)整個導(dǎo)航裝置的傳感器即插即用，保證整個導(dǎo)航裝置的便利性與智能性。

在實現(xiàn)導(dǎo)航程序不同線程之間的同步時，捷聯(lián)慣導(dǎo)頻率為可設(shè)置為50hz、100hz和200hz，慣性/北斗組合頻率可設(shè)置1-10hz，慣性/天文組合頻率可設(shè)置1-2hz，各線程之間同步是通過周期控制線程來協(xié)調(diào)的，周期控制線程嚴(yán)格監(jiān)視整個導(dǎo)航程序的運行時間，并可以在固定間隔時間發(fā)出信號來喚醒相應(yīng)的線程。

所述導(dǎo)航系統(tǒng)采用多線程并發(fā)模式，不同的線程會發(fā)生訪問臨界資源沖突的現(xiàn)象，若不加保護措施會導(dǎo)致導(dǎo)航解算結(jié)果發(fā)生嚴(yán)重錯誤甚至?xí)?dǎo)致整個程序的崩潰。因此本導(dǎo)航裝置在訪問臨界資源的代碼段采用加鎖的保護模式，以保證同一時間段內(nèi)只能有一個線程訪問臨界資源，從而保證整個程序的流暢穩(wěn)定運行。

本發(fā)明還公開一種可配置的慣性/天文/北斗多組合導(dǎo)航方法，原理是：分別建立捷聯(lián)慣導(dǎo)解算線程、sins/cns濾波線程、sins/bd濾波線程，具體包括如下步驟：

步驟1，多組合導(dǎo)航系統(tǒng)慣導(dǎo)誤差狀態(tài)量方程建模

對于可配置的慣性/天文/北斗多組合導(dǎo)航系統(tǒng)，其捷聯(lián)慣導(dǎo)解算過程如下，速度計算公式為：

式中，為載體相當(dāng)于地理系的加速度在載體上的投影，為載體相當(dāng)于地理系的速度在載體上的投影，為機體系到導(dǎo)航坐標(biāo)系的姿態(tài)轉(zhuǎn)移矩陣，為載體相當(dāng)于地理坐標(biāo)系的速度在導(dǎo)航坐標(biāo)系上的投影，本模型的地理坐標(biāo)系即為導(dǎo)航坐標(biāo)系。

位置計算公式為：

用四元數(shù)描述載體的姿態(tài)運動，計算公式為：

用矩陣表示為：

式中，q為姿態(tài)四元數(shù)，為q對時間的導(dǎo)數(shù)，矢量為四元數(shù)形式，表示載體坐標(biāo)系相對導(dǎo)航坐標(biāo)系在載體坐標(biāo)系上的投影。

在上述基礎(chǔ)上，建立多組合導(dǎo)航系統(tǒng)狀態(tài)方程為：

其中，狀態(tài)變量為：

表示捷聯(lián)慣性導(dǎo)航在地里坐標(biāo)系下東、北、天方向平臺誤差角，δve,δvn,δvu表示慣性導(dǎo)航系統(tǒng)在地理坐標(biāo)系下東、北、天方向的速度誤差；δl,δλ,δh表示慣性導(dǎo)航系統(tǒng)在地里坐標(biāo)系下緯度、經(jīng)度、高度的誤差；εbx,εby,εbz表示陀螺隨機常數(shù)誤差；εrx,εry,εrz表示陀螺一階馬爾可夫過程隨機誤差；▽x,▽y,▽z表示加速度計的一階馬爾可夫過程隨機誤差；a(t)18×18為系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣；g(t)18×9為噪聲系數(shù)矩陣；w(t)9×1為系統(tǒng)的白噪聲矢量。

步驟2，多組合導(dǎo)航系統(tǒng)量測方程建模

在此所敘述的可配置的慣性/天文/北斗多組合導(dǎo)航系統(tǒng)，是利用北斗導(dǎo)航系統(tǒng)的位置信息對慣導(dǎo)的位置、速度信息進行濾波估計，利用天文姿態(tài)信息對慣導(dǎo)的姿態(tài)信息進行濾波估計。根據(jù)不同濾波線程的工作特性，建立各自濾波線程的量測方程。由于sins/bd濾波模塊和sins/cns濾波模塊的相似性，在此僅討論sins/cns濾波模塊的量測方程，如下：

在sins/cns子系統(tǒng)中，定義觀測量為星敏感器測得的載體姿態(tài)角與捷聯(lián)慣導(dǎo)的姿態(tài)角之差，量測方程如下：

zs(t)＝hs(t)x(t)+vs(t)(9)

由于多組合導(dǎo)航系統(tǒng)狀態(tài)方程中的誤差角狀態(tài)量為慣性導(dǎo)航平臺誤差角，它描述的是數(shù)學(xué)平臺與地里面坐標(biāo)系之間的誤差角關(guān)系，因此需要將慣性姿態(tài)誤差角轉(zhuǎn)化到平臺誤差角。在平臺誤差角為小量時，量測矩陣可以由以下公式表示：

因此，量測方程可以表示為：

式中，γ、θ、ψ分別為橫滾角、俯仰角和航向角，vs(t)為姿態(tài)誤差角量測噪聲。

對于慣性/北斗、慣性/天文濾波模塊，各濾波模塊過程如下：

上式中，表示tk-1時刻的狀態(tài)對tk時刻的狀態(tài)的最優(yōu)估計值，φk/k-1表示tk-1時刻至tk時刻系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣，表示tk-1時刻的系統(tǒng)狀態(tài)估計值，kk表示增益矩陣，hk表示濾波模塊tk時刻的觀測系數(shù)矩陣，pk/k-1表示最優(yōu)預(yù)測估計誤差協(xié)方差陣，pk/k表示濾波模塊tk時刻的系統(tǒng)誤差協(xié)方差陣，qk-1表示tk-1時刻的噪聲方差矩陣，γk/k-1表示tk-1時刻的噪聲矢量對tk時刻狀態(tài)矢量影響的噪聲系數(shù)矩陣，rk表示tk時刻的量測方差矩陣，i為單位矩陣。

需要說明的是，在此所敘述的多組合導(dǎo)航系統(tǒng)，組合模式及解算周期是可以在線配置的。通過上位機的配置界面，可以對整個導(dǎo)航系統(tǒng)的捷聯(lián)慣導(dǎo)解算周期、濾波周期進行配置，同時組合模式可配置為sins/cns、sins/bd和sins/cns/bd模式。

綜合上述，本發(fā)明導(dǎo)航方法包括以下步驟：以捷聯(lián)慣導(dǎo)解算主體，采用分布式濾波方案，通過上位機配置不同的組合模式及濾波時間。整個導(dǎo)航裝置可以選擇純捷聯(lián)解算、慣性/北斗、慣性/天文、慣性/天文/北斗等導(dǎo)航模式，便于傳感器模塊的裁剪與算法的驗證。本發(fā)明能夠有效實現(xiàn)捷聯(lián)慣性導(dǎo)航系統(tǒng)的穩(wěn)定工作及慣性/天文/北斗組合導(dǎo)航系統(tǒng)的信息融合處理，獲取高精度的導(dǎo)航結(jié)果。同時為教學(xué)、科研、多組合導(dǎo)航系統(tǒng)的工程化和產(chǎn)品化提供了有益參考。

以上實施例僅為說明本發(fā)明的技術(shù)思想，不能以此限定本發(fā)明的保護范圍，凡是按照本發(fā)明提出的技術(shù)思想，在技術(shù)方案基礎(chǔ)上所做的任何改動，均落入本發(fā)明保護范圍之內(nèi)。