基于載體系速度匹配的容積卡爾曼非線性組合導(dǎo)航方法
【專利摘要】本發(fā)明提供了基于載體系速度匹配的容積卡爾曼非線性組合導(dǎo)航方法。主要步驟包括：初始化組合導(dǎo)航系統(tǒng)；建立直接以導(dǎo)航參數(shù)為狀態(tài)量以及以載體系速度為量測量的非線性濾波模型及其離散化；構(gòu)建狀態(tài)方程和量測方程均為非線性的容積卡爾曼濾波算法，實現(xiàn)組合導(dǎo)航的信息融合和精確的導(dǎo)航定位。優(yōu)點在于本發(fā)明建立的非線性模型以及使用的非線性濾波算法，直接輸出的是導(dǎo)航參數(shù)，不需要進行誤差修正，且捷聯(lián)慣導(dǎo)系統(tǒng)參數(shù)更新與濾波器的時間更新同步實現(xiàn)，算法簡單，為以捷聯(lián)慣性導(dǎo)航系統(tǒng)為主的組合導(dǎo)航系統(tǒng)信息融合提供了一種新方案。
【專利說明】基于載體系速度匹配的容積卡爾曼非線性組合導(dǎo)航方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及一種捷聯(lián)慣性導(dǎo)航系統(tǒng)與提供載體系速度的輔助裝置(如多普勒測速儀、里程計等導(dǎo)航設(shè)備)構(gòu)成組合導(dǎo)航系統(tǒng)時的信息融合技術(shù)，屬于導(dǎo)航、制導(dǎo)與控制【技術(shù)領(lǐng)域】。
【背景技術(shù)】
[0002]捷聯(lián)慣性導(dǎo)航系統(tǒng)中慣性測量單元主要包含三只陀螺儀和三只加速度計，通過對陀螺儀測得的角速度信息和加速度計測得的線加速度信息進行積分來獲得載體位置、速度和姿態(tài)等導(dǎo)航信息，具有信息完備、自主性強，不依賴于任何外界信息而可以獨立工作，短時精度高等優(yōu)點，在很多情況下，成為運載體的首選導(dǎo)航方式。但是，捷聯(lián)慣性導(dǎo)航系統(tǒng)的誤差隨著時間積累，長時間導(dǎo)航時需要其它傳感器輔助，因此常與其他導(dǎo)航設(shè)備構(gòu)成組合導(dǎo)航系統(tǒng)。
[0003]目前以捷聯(lián)慣性導(dǎo)航為主的組合導(dǎo)航系統(tǒng)的濾波模型大多是基于誤差模型，在誤差視為小量的情況下，將誤差模型線性化，然后采用卡爾曼濾波對誤差進行估計，而捷聯(lián)慣性導(dǎo)航系統(tǒng)的誤差模型本質(zhì)是非線性的，在做線性化近似時會引入額外誤差；采用速度匹配方式時，量測量通常為導(dǎo)航系中的速度誤差，而實際情況下量測量通常都是載體系的速度，這需要將載體系速度轉(zhuǎn)換到導(dǎo)航系中；采用誤差模型時通常為兩套算法，一套為捷聯(lián)慣性導(dǎo)航系統(tǒng)參數(shù)更新算法，另一套為濾波算法，估計導(dǎo)航參數(shù)誤差，并且需對導(dǎo)航參數(shù)進行誤差修正。本發(fā)明中狀態(tài)量直接采用導(dǎo)航參數(shù)，而不是誤差，量測量采用載體系速度，而不是常用的導(dǎo)航系速度誤差，模型不做任何近似和約束，更為準(zhǔn)確；采用容積卡爾曼非線性濾波算法，不需要線性假設(shè)，更符合慣導(dǎo)系統(tǒng)的非線性本質(zhì)，適用于狀態(tài)方程和量測方程均為非線性的情況；捷聯(lián)慣導(dǎo)系統(tǒng)的導(dǎo)航參數(shù)更新與濾波過程的時間更新同步實現(xiàn)，且濾波輸出為導(dǎo)航參數(shù)，不需要進行誤差修正，算法簡單。

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

[0004]本發(fā)明的目的在于狀態(tài)量直接采用導(dǎo)航參數(shù)、量測量采用載體系速度、利用容積卡爾曼非線性濾波方法來實現(xiàn)數(shù)據(jù)融合計算，為以捷聯(lián)慣性導(dǎo)航系統(tǒng)為主的組合導(dǎo)航系統(tǒng)信息融合提供一種新方案。
[0005]本發(fā)明主要包含以下步驟:
[0006]步驟1、初始化組合導(dǎo)航系統(tǒng)，為系統(tǒng)的導(dǎo)航參數(shù)更新提供初始值:使用位置傳感器獲取載體的初始位置參數(shù)；根據(jù)陀螺儀測得的角速度信息和加速度計測得的線加速度信息，以及速度傳感器測得的載體系速度，由捷聯(lián)慣性導(dǎo)航系統(tǒng)初始對準(zhǔn)算法獲取載體的初始姿態(tài)歐拉角信息和初始導(dǎo)航系速度信息；
[0007]步驟2、根據(jù)捷聯(lián)慣性導(dǎo)航系統(tǒng)的導(dǎo)航參數(shù)微分方程、慣性傳感器模型，以及以載體系速度為量測量的量測方程，選定10維系統(tǒng)狀態(tài)量和3維量測量，建立非線性連續(xù)系統(tǒng)模型；[0008]步驟3、將步驟2中建立的非線性連續(xù)系統(tǒng)模型離散化，形成非線性離散系統(tǒng)模型；
[0009]步驟4、根據(jù)步驟3中建立的非線性離散系統(tǒng)模型的狀態(tài)方程與量測方程均為非線性的特點，構(gòu)建容積卡爾曼非線性濾波算法，濾波過程中，時間更新周期與量測更新周期不同；
[0010]步驟5、將慣性傳感器的輸出以及輔助傳感器提供的載體系速度代入步驟4的濾波算法中，進行非線性濾波，直接輸出組合后的導(dǎo)航參數(shù)。
[0011]本發(fā)明還可以包括這樣一些特征:
[0012]1.所述的步驟2根據(jù)捷聯(lián)慣性導(dǎo)航系統(tǒng)的速度微分方程、歐拉角微分方程、慣性傳感器模型，以及以載體系速度為量測量的量測方程，選定10維狀態(tài)量和3維量測量，建立非線性連續(xù)系統(tǒng)模型，具體為:
[0013]以東北天(east-north-up)地理坐標(biāo)系作為導(dǎo)航坐標(biāo)系(η系)，以載體右前上(bx-by-bz)方向矢量右手定則構(gòu)成的坐標(biāo)系作為載體坐標(biāo)系(b系)。
[0014]捷聯(lián)慣性導(dǎo)航系統(tǒng)的速度微分方程為
【權(quán)利要求】
1.基于載體系速度匹配的容積卡爾曼非線性組合導(dǎo)航方法，其特征在于該方法包括下列步驟: 步驟1、初始化組合導(dǎo)航系統(tǒng)，為系統(tǒng)的導(dǎo)航參數(shù)更新提供初始值:使用位置傳感器獲取載體的初始位置參數(shù)；根據(jù)陀螺儀測得的角速度信息和加速度計測得的線加速度信息，以及速度傳感器測得的載體系速度，通過捷聯(lián)慣性導(dǎo)航系統(tǒng)使用初始對準(zhǔn)算法獲取載體的初始姿態(tài)歐拉角信息和初始導(dǎo)航系速度信息； 步驟2、根據(jù)捷聯(lián)慣性導(dǎo)航系統(tǒng)的導(dǎo)航參數(shù)微分方程、慣性傳感器模型，以及以載體系速度為量測量的量測方程，選定10維系統(tǒng)狀態(tài)量和3維量測量，建立非線性連續(xù)系統(tǒng)模型； 步驟3、將步驟2中建立的非線性連續(xù)系統(tǒng)模型離散化，形成非線性離散系統(tǒng)模型；步驟4、根據(jù)步驟3中建立的非線性離散系統(tǒng)模型的狀態(tài)方程與量測方程均為非線性的特點，構(gòu)建容積卡爾曼非線性濾波算法，濾波過程中，時間更新與量測更新周期不同；步驟5、將慣性傳感器的輸出以及輔助傳感器提供的載體系速度代入步驟4的濾波算法中，進行非線性濾波，直接輸出組合后的導(dǎo)航參數(shù)。
2.根據(jù)權(quán)利要求1所述的基于載體系速度匹配的容積卡爾曼非線性組合導(dǎo)航方法，其中所述的步驟2根據(jù)捷聯(lián)慣性導(dǎo)航系統(tǒng)的速度微分方程、歐拉角微分方程、慣性傳感器模型，以及以載體系速度為量測量的量測方程，選定10維系統(tǒng)狀態(tài)量和3維量測量，建立非線性連續(xù)系統(tǒng)模型，具體為: 以東北天地理坐標(biāo)系作為導(dǎo)航坐標(biāo)系-η系，以載體右前上方向矢量右手定則構(gòu)成的坐標(biāo)系作為載體坐標(biāo)系_b系； 捷聯(lián)慣性導(dǎo)航系統(tǒng)的速度微分方程為
3.根據(jù)權(quán)利要求1所述的基于載體系速度匹配的容積卡爾曼非線性組合導(dǎo)航方法，其中所述的步驟3中將步驟2中建立的非線性連續(xù)系統(tǒng)模型離散化，得到非線性離散系統(tǒng)模型。采用四階龍格庫塔方法將狀態(tài)方程離散化，非線性離散系統(tǒng)模型為
4.根據(jù)權(quán)利要求1所述的基于載體系速度匹配的容積卡爾曼非線性組合導(dǎo)航方法的步驟，其中所述的步驟3中容積卡爾曼非線性濾波器的濾波步驟為 1)初始化系統(tǒng)狀態(tài)向量及其方差矩陣
5.根據(jù)權(quán)利要求1所述的基于載體系速度匹配的容積卡爾曼非線性組合導(dǎo)航方法，其中所述的步驟5中由于系統(tǒng)狀態(tài)向量為導(dǎo)航參數(shù)，從而濾波輸出不需要進行誤差修正，直接為導(dǎo)航參數(shù)，即

Veast, Vnorth，^ ,
Y及Ψ。
【文檔編號】G01C21/20GK103727941SQ201410004374
【公開日】2014年4月16日 申請日期:2014年1月6日 優(yōu)先權(quán)日:2014年1月6日
【發(fā)明者】程向紅, 冉昌艷, 王磊, 陳紅梅, 周玲 申請人:東南大學(xué)