基于直接導(dǎo)航模型的無(wú)跡卡爾曼非線性初始對(duì)準(zhǔn)方法
【專利摘要】本發(fā)明提供了一種基于直接導(dǎo)航模型的無(wú)跡卡爾曼非線性初始對(duì)準(zhǔn)方法。主要步驟包括：凝固粗對(duì)準(zhǔn)；建立基于導(dǎo)航參數(shù)微分方程（即直接導(dǎo)航模型）以及以導(dǎo)航系速度為量測(cè)量的非線性濾波模型及其離散化；構(gòu)建時(shí)間更新和量測(cè)更新不同步的簡(jiǎn)化無(wú)跡卡爾曼濾波算法，實(shí)現(xiàn)捷聯(lián)慣性導(dǎo)航初始對(duì)準(zhǔn)，為捷聯(lián)慣性導(dǎo)航初始對(duì)準(zhǔn)提供了一種新方案。優(yōu)點(diǎn)在于直接導(dǎo)航模型非線性模型更為準(zhǔn)確，簡(jiǎn)化的無(wú)跡卡爾曼非線性濾波更具有普適性，濾波過程中只需要一套濾波算法同時(shí)實(shí)現(xiàn)濾波與姿態(tài)更新過程，而且濾波后直接輸出導(dǎo)航參數(shù)，不需要進(jìn)行誤差修正，算法更簡(jiǎn)單。
【專利說明】基于直接導(dǎo)航模型的無(wú)跡卡爾曼非線性初始對(duì)準(zhǔn)方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及一種捷聯(lián)慣性導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)技術(shù)，可用于捷聯(lián)慣性系統(tǒng)的初始姿態(tài)的確定，屬于導(dǎo)航、制導(dǎo)與控制【技術(shù)領(lǐng)域】。
【背景技術(shù)】
[0002]捷聯(lián)慣性導(dǎo)航系統(tǒng)沒有實(shí)體平臺(tái)，在導(dǎo)航定位解算前必須初始對(duì)準(zhǔn)，確定初始的位置、速度及姿態(tài)信息，其中初始姿態(tài)信息的確定是捷聯(lián)慣性導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)的關(guān)鍵內(nèi)容。捷聯(lián)慣性導(dǎo)航系統(tǒng)的慣性器件直接安裝在載體上，載體的晃動(dòng)干擾直接加給慣性器件，因此捷聯(lián)慣性導(dǎo)航系統(tǒng)通常工作在具有各種噪聲的動(dòng)態(tài)環(huán)境中，初始對(duì)準(zhǔn)的環(huán)境并不理想。[0003]現(xiàn)有的捷聯(lián)慣性導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)分為粗對(duì)準(zhǔn)和精對(duì)準(zhǔn)兩個(gè)階段。粗對(duì)準(zhǔn)階段，依靠重力矢量和地球速率矢量的測(cè)量值，粗略估算出載體坐標(biāo)系到導(dǎo)航參考坐標(biāo)系的變換矩陣，與真實(shí)的姿態(tài)矩陣存在偏差，尤其是在外部干擾比較大的情況下。精對(duì)準(zhǔn)階段，常利用系統(tǒng)誤差模型估計(jì)出地理坐標(biāo)系與真實(shí)地理坐標(biāo)系之間的失準(zhǔn)角，并補(bǔ)償誤差，提高對(duì)準(zhǔn)精度。捷聯(lián)慣性導(dǎo)航系統(tǒng)誤差模型為一組非線性微分方程，本質(zhì)是非線性的，濾波估計(jì)的是導(dǎo)航參數(shù)誤差，具體應(yīng)用時(shí)往往需要進(jìn)行簡(jiǎn)化、近似或者加入約束條件，而受到很多限制，例如小失準(zhǔn)角時(shí)常將系統(tǒng)誤差方程近似為線性誤差模型而使用線性卡爾曼濾波，要求粗對(duì)準(zhǔn)的精度高，失準(zhǔn)角很小，然而這在對(duì)準(zhǔn)環(huán)境惡劣情況中往往達(dá)不到。采用系統(tǒng)誤差模型濾波時(shí)通常為兩套算法，一套為姿態(tài)更新算法，另一套為誤差估計(jì)濾波算法，待誤差估計(jì)后，對(duì)導(dǎo)航參數(shù)進(jìn)行誤差修正，算法復(fù)雜。本發(fā)明采用直接導(dǎo)航模型，而不是系統(tǒng)誤差模型，量測(cè)量采用東、北、天三個(gè)方向的速度，而不是速度的誤差，沒有任何近似和約束條件，模型更為準(zhǔn)確；濾波過程中時(shí)間更新過程與姿態(tài)更新過程合并完成，待有量測(cè)數(shù)據(jù)時(shí)進(jìn)行量測(cè)更新，濾波輸出為導(dǎo)航參數(shù)，不需要進(jìn)行誤差修正，算法更簡(jiǎn)單，采用無(wú)跡卡爾曼非線性濾波算法，不受限于線性模型，應(yīng)用更廣泛，具有普適性。

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

[0004]本發(fā)明的目的在于不采用傳統(tǒng)的捷聯(lián)慣性導(dǎo)航系統(tǒng)的誤差模型，而采用更簡(jiǎn)單的基于直接導(dǎo)航模型和實(shí)用性更為廣泛的非線性濾波方法，實(shí)現(xiàn)捷聯(lián)慣性導(dǎo)航系統(tǒng)的初始對(duì)準(zhǔn)，為捷聯(lián)慣性導(dǎo)航系統(tǒng)的初始對(duì)準(zhǔn)提供一種新途徑和新方案。
[0005]本發(fā)明的目的是這樣實(shí)現(xiàn)的，它包括以下步驟:
[0006]步驟1、根據(jù)慣性測(cè)量單元中陀螺儀測(cè)得的角速度信息和加速度計(jì)測(cè)得的線加速度信息，應(yīng)用基于凝固解析粗對(duì)準(zhǔn)算法完成捷聯(lián)慣性導(dǎo)航系統(tǒng)粗對(duì)準(zhǔn)，求得粗略的初始縱搖角0、橫搖角Y和航向角Ψ ;
[0007]步驟2、根據(jù)捷聯(lián)慣性導(dǎo)航系統(tǒng)的速度微分方程、歐拉角微分方程、傳感器模型，以導(dǎo)航系速度為量測(cè)量，建立基于導(dǎo)航參數(shù)微分方程的非線性濾波連續(xù)系統(tǒng)模型；
[0008]步驟3、將步驟2中建立的非線性連續(xù)系統(tǒng)模型離散化，形成非線性離散系統(tǒng)模型；
[0009]步驟4、根據(jù)步驟3中建立的非線性離散系統(tǒng)模型構(gòu)建用于初始對(duì)準(zhǔn)的簡(jiǎn)化無(wú)跡卡爾曼非線性濾波器，濾波算法中，時(shí)間更新與量測(cè)更新不同步；
[0010]步驟5、將步驟I提取的粗對(duì)準(zhǔn)姿態(tài)角以及陀螺儀和加速度計(jì)的輸出代入步驟4中，完成捷聯(lián)慣性導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)，直接輸出導(dǎo)航參數(shù)。
[0011]本發(fā)明還可以包括這樣一些特征:
[0012]1.所述的步驟2根據(jù)捷聯(lián)慣性導(dǎo)航系統(tǒng)的速度微分方程、歐拉角微分方程、傳感器模型，利用導(dǎo)航系速度為量測(cè)量，建立基于導(dǎo)航參數(shù)微分方程(即直接導(dǎo)航模型)的非線性濾波連續(xù)系統(tǒng)模型，具體為:
[0013]捷聯(lián)慣性導(dǎo)航系統(tǒng)的速度微分方程為
[0014]
【權(quán)利要求】
1.基于直接導(dǎo)航模型的無(wú)跡卡爾曼非線性初始對(duì)準(zhǔn)方法，包括下列步驟: 步驟1、根據(jù)慣性測(cè)量單元中陀螺儀測(cè)得的角速度信息和加速度計(jì)測(cè)得的線加速度信息，使用基于凝固解析粗對(duì)準(zhǔn)算法完成捷聯(lián)慣性導(dǎo)航系統(tǒng)粗對(duì)準(zhǔn)，求得粗略的初始縱搖角Θ、橫搖角Y和航向角Ψ ; 步驟2、根據(jù)捷聯(lián)慣性導(dǎo)航系統(tǒng)的速度微分方程、歐拉角微分方程、傳感器模型，以導(dǎo)航系速度為量測(cè)量，建立基于導(dǎo)航參數(shù)微分方程的非線性濾波連續(xù)系統(tǒng)模型； 步驟3、將步驟2中建立的非線性連續(xù)系統(tǒng)模型離散化，形成非線性離散系統(tǒng)模型；步驟4、根據(jù)步驟3中建立的非線性離散系統(tǒng)模型構(gòu)建用于初始對(duì)準(zhǔn)的簡(jiǎn)化無(wú)跡卡爾曼非線性濾波器，濾波算法中，時(shí)間更新與量測(cè)更新不同步； 步驟5、將步驟1提取的粗對(duì)準(zhǔn)姿態(tài)角以及陀螺儀和加速度計(jì)的輸出代入步驟4中，完成捷聯(lián)慣性導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)，直接輸出導(dǎo)航參數(shù)。
2.根據(jù)權(quán)利要求1所述的基于直接導(dǎo)航模型的無(wú)跡卡爾曼非線性初始對(duì)準(zhǔn)方法的步驟，其中所述的步驟2根據(jù)捷聯(lián)慣性導(dǎo)航系統(tǒng)的速度微分方程、歐拉角微分方程、傳感器模型，以導(dǎo)航系速度為量測(cè)量，建立基于導(dǎo)航參數(shù)微分方程(即直接導(dǎo)航模型)的非線性濾波連續(xù)系統(tǒng)模型，具體為: 捷聯(lián)慣性導(dǎo)航系統(tǒng)的速度微分方程為
3.根據(jù)權(quán)利要求1所述的基于直接導(dǎo)航模型的無(wú)跡卡爾曼非線性初始對(duì)準(zhǔn)方法的步驟，其中所述的步驟3中連續(xù)系統(tǒng)模型離散化后，非線性系統(tǒng)模型為
4.根據(jù)權(quán)利要求1所述的基于直接導(dǎo)航模型的無(wú)跡卡爾曼非線性初始對(duì)準(zhǔn)方法的步驟，其中所述的步驟4中用于初始對(duì)準(zhǔn)的簡(jiǎn)化無(wú)跡卡爾曼非線性濾波器的濾波步驟為 1)初始化狀態(tài)變量及其均方差

5.根據(jù)權(quán)利要求1所述的基于直接導(dǎo)航模型的無(wú)跡卡爾曼非線性初始對(duì)準(zhǔn)方法的步驟，其中所述的步驟4中簡(jiǎn)化無(wú)跡卡爾曼非線性濾波器在濾波過程中時(shí)間更新與量測(cè)更新不同步，濾波過程與姿態(tài)更新過程合并完成，時(shí)間更新周期與傳感器數(shù)據(jù)更新周期一致，即時(shí)間T，時(shí)間更新也是姿態(tài)更新過程；量測(cè)更新周期與量測(cè)量更新周期一致，為濾波周期，即時(shí)間Tfo
6.根據(jù)權(quán)利要求1所述的基于直接導(dǎo)航模型的無(wú)跡卡爾曼非線性初始對(duì)準(zhǔn)方法的步驟，其中所述的步驟5中濾波輸出即為導(dǎo)航參數(shù)，不需要進(jìn)行誤差修正，濾波輸出為vn,Vu, Θ，Y 及 ψ。
【文檔編號(hào)】G01C21/16GK103644913SQ201310724264
【公開日】2014年3月19日 申請(qǐng)日期:2013年12月25日 優(yōu)先權(quán)日:2013年12月25日
【發(fā)明者】冉昌艷, 程向紅, 王磊, 陳紅梅, 戴晨曦 申請(qǐng)人:東南大學(xué)