本發(fā)明公開了一種基于李群濾波的捷聯(lián)慣性導(dǎo)航初始對準(zhǔn)方法，該方法屬于導(dǎo)航方法及應(yīng)用技術(shù)領(lǐng)域。

背景技術(shù)：

所謂導(dǎo)航，就是正確地引導(dǎo)載體沿著預(yù)定的航線、以要求的精度、在指定的時(shí)間內(nèi)將載體引導(dǎo)至目的地的過程。慣性導(dǎo)航系統(tǒng)根據(jù)自身傳感器的輸出，以牛頓第二定律為理論基礎(chǔ)，對載體的各項(xiàng)導(dǎo)航參數(shù)進(jìn)行解算。它是一種自主式的導(dǎo)航系統(tǒng)，在工作時(shí)不依靠外界信息，也不向外界輻射任何能量，隱蔽性好、抗擾性強(qiáng)，能夠全天時(shí)、全天候?yàn)檩d體提供完備的運(yùn)動(dòng)信息。

早期的慣導(dǎo)系統(tǒng)以平臺(tái)慣導(dǎo)為主，隨著慣性器件的成熟和計(jì)算機(jī)技術(shù)的發(fā)展，上世紀(jì)60年代開始出現(xiàn)了慣性器件與載體直接固聯(lián)的捷聯(lián)慣導(dǎo)系統(tǒng)。與平臺(tái)慣導(dǎo)相比，捷聯(lián)慣導(dǎo)系統(tǒng)省去了復(fù)雜的實(shí)體穩(wěn)定平臺(tái)，具有成本低、體積小、重量輕、可靠性高等優(yōu)點(diǎn)。近年來，捷聯(lián)慣導(dǎo)系統(tǒng)日趨成熟，精度逐步提高，應(yīng)用范圍也逐漸擴(kuò)大。捷聯(lián)式慣性導(dǎo)航技術(shù)將陀螺儀和加速度計(jì)直接安裝在載體上，得到載體系下的加速度和角速度，通過導(dǎo)航計(jì)算機(jī)將測得的數(shù)據(jù)轉(zhuǎn)換至導(dǎo)航坐標(biāo)系完成導(dǎo)航，它不需要實(shí)體的穩(wěn)定平臺(tái)，成本低、體積小、重量輕、可靠性高。

捷聯(lián)式慣性導(dǎo)航系統(tǒng)進(jìn)入導(dǎo)航任務(wù)前要先進(jìn)行初始對準(zhǔn)，目的是建立精確的初始姿態(tài)矩陣，從而得到載體相對空間的姿態(tài)。對準(zhǔn)時(shí)間和對準(zhǔn)精度是初始對準(zhǔn)的兩個(gè)重要指標(biāo)。對準(zhǔn)時(shí)間反映系統(tǒng)的快速反應(yīng)能力，對準(zhǔn)精度反映系統(tǒng)的導(dǎo)航性能。作為慣導(dǎo)系統(tǒng)的一個(gè)關(guān)鍵技術(shù)，初始對準(zhǔn)是國內(nèi)外學(xué)者多年來的研究熱點(diǎn)。慣導(dǎo)系統(tǒng)的初始對準(zhǔn)分為粗對準(zhǔn)和精對準(zhǔn)兩個(gè)階段。

比較傳統(tǒng)的初始對準(zhǔn)方法，僅適用于靜止或微幅晃動(dòng)的對準(zhǔn)環(huán)境。對于工作在復(fù)雜環(huán)境中的載體，諸如發(fā)動(dòng)機(jī)處于高頻振動(dòng)的汽車、格斗狀態(tài)下的戰(zhàn)斗機(jī)、浪涌下的艦船等，載體的角振動(dòng)和線振動(dòng)會(huì)導(dǎo)致初始對準(zhǔn)的精度和穩(wěn)定性下降。捷聯(lián)慣導(dǎo)系統(tǒng)在自對準(zhǔn)過程中的姿態(tài)時(shí)刻都在發(fā)生變化，初始對準(zhǔn)的時(shí)間和精度會(huì)受到影響。因此，在運(yùn)載體晃動(dòng)干擾環(huán)境下完成初始對準(zhǔn)過程，就必須屏蔽這些無法消除的擾動(dòng)影響。對準(zhǔn)領(lǐng)域的研究重點(diǎn)是動(dòng)態(tài)情況下捷聯(lián)式慣性導(dǎo)航系統(tǒng)的初始對準(zhǔn)，致力于研究出抗擾能力強(qiáng)、對準(zhǔn)精度高、適應(yīng)于各種復(fù)雜惡劣環(huán)境的對準(zhǔn)方法。本發(fā)明將姿態(tài)矩陣的求解問題轉(zhuǎn)化為初始時(shí)刻慣性坐標(biāo)系下的求解問題，可有效地解決晃動(dòng)條件下初始對準(zhǔn)的問題。

四元數(shù)的表示方法彌補(bǔ)了歐拉角的的不足，計(jì)算過程中不存在奇異點(diǎn)的問題，但是四元數(shù)存在約束條件，在采用四元數(shù)描述姿態(tài)運(yùn)動(dòng)時(shí)，在濾波過程中由于難以滿足其約束條件而出現(xiàn)問題。并且，采用四元數(shù)的描述方式在計(jì)算過程中表述復(fù)雜，而且難以避免的存在計(jì)算誤差，導(dǎo)致姿態(tài)解算存在偏差。

針對上述問題，本發(fā)明將姿態(tài)矩陣的求解問題轉(zhuǎn)化為初始時(shí)刻慣性坐標(biāo)系下的求解問題，可有效地解決晃動(dòng)條件下初始對準(zhǔn)的問題。應(yīng)用李群濾波器進(jìn)行遞歸迭代，大大的簡化了計(jì)算過程?；赟O(3)群的概率分布函數(shù)推導(dǎo)，由于SO(3)群是緊的，李代數(shù)濾波器有效的避免了姿態(tài)解算中的奇異之問題，可以對姿態(tài)進(jìn)行全局表示?；诶钊好枋龅臑V波運(yùn)算，也大大減少了矩陣和向量轉(zhuǎn)化中的計(jì)算誤差，提升了計(jì)算速度和計(jì)算精度。減輕了晃動(dòng)干擾對初始對準(zhǔn)的影響，使在晃動(dòng)條件下仍然能夠快速、精確地實(shí)現(xiàn)初始對準(zhǔn)。該方法不依賴于系統(tǒng)的誤差模型，且無需在粗對準(zhǔn)的基礎(chǔ)上進(jìn)行精對準(zhǔn)即可完成晃動(dòng)基座下的初始對準(zhǔn)。

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

基于李群濾波的初試對準(zhǔn)方法是應(yīng)用李群和李代數(shù)替代傳統(tǒng)的四元數(shù)方法來實(shí)現(xiàn)捷聯(lián)慣導(dǎo)的解算過程，并與最優(yōu)姿態(tài)估計(jì)的初始對準(zhǔn)思想相結(jié)合，將初始對準(zhǔn)轉(zhuǎn)化為姿態(tài)估計(jì)的問題，構(gòu)建了基于李群結(jié)構(gòu)的載體晃動(dòng)干擾情況下捷聯(lián)慣導(dǎo)系統(tǒng)初始對準(zhǔn)模型，應(yīng)用李群濾波器進(jìn)行遞歸迭代快速有效地實(shí)現(xiàn)精對準(zhǔn)。姿態(tài)矩陣的求解問題轉(zhuǎn)化為初始時(shí)刻慣性坐標(biāo)系下SO(3)群的估計(jì)問題，將初始姿態(tài)矩陣分解為三個(gè)SO(3)群的連續(xù)相乘，通過慣性元件的積分計(jì)算和地理位置信息，建立新的觀測模型。采用李群和李代數(shù)描述捷聯(lián)解算，應(yīng)用李群濾波器進(jìn)行遞歸迭代，求出姿態(tài)矩陣。本發(fā)明實(shí)時(shí)地反映載體在晃動(dòng)干擾下的姿態(tài)變化，減輕了晃動(dòng)干擾對初始對準(zhǔn)的影響，使在晃動(dòng)條件下仍然能夠快速、精確地實(shí)現(xiàn)初始對準(zhǔn)，不依賴于系統(tǒng)的誤差模型，且無需在粗對準(zhǔn)的基礎(chǔ)上進(jìn)行精對準(zhǔn)即可完成晃動(dòng)基座下的初始對準(zhǔn)，計(jì)算簡單，適應(yīng)性強(qiáng)，能夠用于復(fù)雜的隨機(jī)系統(tǒng)，確保對準(zhǔn)精度的同時(shí)提高了對準(zhǔn)速度，在實(shí)際工程中具有良好的應(yīng)用前景。

在本發(fā)明方法的詳細(xì)描述中坐標(biāo)系定義如下：地球坐標(biāo)系e系，原點(diǎn)選取地球中心，X軸位于赤道平面內(nèi)，從地心指向載體所在點(diǎn)經(jīng)線，Z軸沿地球自轉(zhuǎn)軸方向，隨地球自轉(zhuǎn)而轉(zhuǎn)動(dòng)，X軸、Y軸和Z軸構(gòu)成右手坐標(biāo)系，隨地球自轉(zhuǎn)而轉(zhuǎn)動(dòng)；地心慣性坐標(biāo)系i系，是在粗對準(zhǔn)起始時(shí)刻將地球坐標(biāo)系e系慣性凝固后形成的坐標(biāo)系；導(dǎo)航坐標(biāo)系n系，即導(dǎo)航基準(zhǔn)的坐標(biāo)系，導(dǎo)航相關(guān)運(yùn)算都在該坐標(biāo)系下進(jìn)行，原點(diǎn)位于艦載機(jī)重心，X軸指向東向E，Y軸指向北向N，Z軸指向天向U；載體坐標(biāo)系b系，原點(diǎn)位于艦載機(jī)重心，X軸、Y軸、Z軸分別沿艦載機(jī)機(jī)體橫軸指向右、沿縱軸指向前、沿立軸指向上。

在本方法中，根據(jù)現(xiàn)有的捷聯(lián)慣性導(dǎo)航系統(tǒng)初始對準(zhǔn)中的問題，提出了基于李群濾波的捷聯(lián)慣性導(dǎo)航初試對準(zhǔn)方法。

為實(shí)現(xiàn)方法流程，本發(fā)明采用的技術(shù)方案為基于李群濾波的捷聯(lián)慣性導(dǎo)航初始對準(zhǔn)方法，該方法通過下述流程實(shí)現(xiàn)，

(1)捷聯(lián)慣導(dǎo)系統(tǒng)進(jìn)行預(yù)熱準(zhǔn)備，啟動(dòng)系統(tǒng)，獲得載體所在位置的經(jīng)度λ、緯度L的基本信息，采集慣性測量單元IMU中陀螺的輸出角速度信息和加速度計(jì)的輸出信息f^b等，總體實(shí)物圖如圖1所示；

(2)對采集到的陀螺和加速度計(jì)的數(shù)據(jù)進(jìn)行處理，應(yīng)用李群濾波方法解算姿態(tài)矩陣。

方法的整體計(jì)算流程如2所示。

將初始對準(zhǔn)轉(zhuǎn)化為姿態(tài)估計(jì)的問題，姿態(tài)變換為兩個(gè)坐標(biāo)系之間的旋轉(zhuǎn)變換，導(dǎo)航的姿態(tài)表示用一個(gè)3×3的正交變換矩陣來表示，如圖3所示。導(dǎo)航的正交變換矩陣符合李群3維特殊正交群SO(3)的性質(zhì)，構(gòu)成了SO(3)群：

其中，任意轉(zhuǎn)動(dòng)群R∈SO(3)對應(yīng)了特定的導(dǎo)航姿態(tài)矩陣，表示3×3的向量空間，上標(biāo)T表示矩陣的轉(zhuǎn)置，I表示三維單位矩陣，det(R)表示為矩陣R的行列式。

姿態(tài)矩陣的求解問題轉(zhuǎn)化為初始時(shí)刻慣性坐標(biāo)系下的求解問題，將姿態(tài)矩陣分解為三個(gè)部分，姿態(tài)矩陣的分解形式如下：

其中，t表示時(shí)間變量，n(t)表示t時(shí)刻的導(dǎo)航坐標(biāo)系，n(0)表示t₀時(shí)刻的導(dǎo)航坐標(biāo)系，b(t)表示t時(shí)刻的載體坐標(biāo)系，b(0)表示t₀時(shí)刻的載體坐標(biāo)系，和分別為導(dǎo)航坐標(biāo)系和機(jī)體坐標(biāo)系下從初始t₀時(shí)刻到t時(shí)刻的姿態(tài)轉(zhuǎn)換矩陣。和由陀螺和加速度計(jì)的信息計(jì)算得到。那么，初始對準(zhǔn)的任務(wù)由求解姿態(tài)矩陣的問題轉(zhuǎn)化為求解初始姿態(tài)陣的問題。為初始t₀時(shí)刻的機(jī)體系與導(dǎo)航系之間的姿態(tài)轉(zhuǎn)換矩陣，是一個(gè)常值矩陣。

取地理坐標(biāo)系為導(dǎo)航坐標(biāo)系，慣性導(dǎo)航的基本方程，載體速度微分方程表示為：

其中，vⁿ表示相對于地球的載體速度；fⁿ表示為比力在導(dǎo)航系下的投影；f^b表示為比力在載體系下的投影，由加速度計(jì)測量得到；表示為地球坐標(biāo)系相對于慣性坐標(biāo)系的角速率；表示為導(dǎo)航坐標(biāo)系相對于地球坐標(biāo)系的角速率；gⁿ表示重力。

將式(2)代入式(3)得：

上式兩邊同時(shí)左乘姿態(tài)轉(zhuǎn)換矩陣則有：

上式經(jīng)整理，得到：

對式(5)的兩邊進(jìn)行積分，得到：

式(5)的左邊展開為：

其中，vⁿ(0)為初始t₀時(shí)刻的速度。

將式(8)和式(7)帶入式(6)得：

簡化表示為：

式(10)就表示為速率方程在慣性坐標(biāo)系的積分形式，將姿態(tài)矩陣的求解問題轉(zhuǎn)化為初始時(shí)刻慣性坐標(biāo)系下的求解問題。

式(12)是關(guān)于初始姿態(tài)陣的數(shù)學(xué)方程。α(t)、β(t)表示如上，由加速度計(jì)和陀螺的輸出計(jì)算得到。

這樣給出的是α(t)、β(t)實(shí)現(xiàn)的連續(xù)形式，通過α(t)、β(t)對應(yīng)的積分迭代算法結(jié)解算α和β具體的值。由于在角晃動(dòng)或線晃動(dòng)激烈頻繁的環(huán)境下，載體作姿態(tài)更新時(shí)，單子樣旋轉(zhuǎn)矢量法對有限轉(zhuǎn)動(dòng)引起的不可交換誤差的補(bǔ)償程度不夠，造成算法漂移十分嚴(yán)重。而多子樣旋轉(zhuǎn)矢量法能實(shí)現(xiàn)對不可

交換誤差的有效補(bǔ)償，算法簡單，易于操作，工程上非常實(shí)用。子樣數(shù)越高，算法的精度越高，但計(jì)算量也越大。綜合考慮精度要求和計(jì)算量，選擇雙子樣旋轉(zhuǎn)矢量算法對α(t)、β(t)進(jìn)行積分迭代計(jì)算。

矢量α(t)近似為：

對式(13)右邊的積分部分采用雙子樣旋轉(zhuǎn)矢量法進(jìn)行計(jì)算：

式(14)帶入到式(13)簡化得：

式(12)右邊的最后一項(xiàng)為：

假設(shè)速度在[t_k,t_k+1]內(nèi)呈線性變化，式(12)右邊的倒數(shù)第二項(xiàng)近似為：

把式(16)和式(17)代入式(12)，化簡得：

根據(jù)式(10)-式(18)，建立起系統(tǒng)的觀測方程：

β_n＝R_nα_n+Q_v (19)

Q_v為系統(tǒng)觀測噪聲協(xié)方差陣。觀測方程是由實(shí)測數(shù)據(jù)運(yùn)算所得，存在誤差項(xiàng)。

由于將求解姿態(tài)矩陣的問題轉(zhuǎn)化為求解初始姿態(tài)陣的問題，在整個(gè)初始對準(zhǔn)過程中為常值，建立系統(tǒng)狀態(tài)方程如下：

R_n＝R_n-1 (20)

根據(jù)上述內(nèi)容，將姿態(tài)矩陣的求解問題轉(zhuǎn)化為初始時(shí)刻慣性坐標(biāo)系下的求解問題，建立起了具有李群結(jié)構(gòu)的系統(tǒng)方程，表示為：

采用李群的結(jié)構(gòu)表示，避免了四元數(shù)的描述方式在計(jì)算過程中表述復(fù)雜和存在計(jì)算誤差，并且在計(jì)算過程中也不存在奇異點(diǎn)的問題。但是矩陣形式的表示并不適用于常規(guī)濾波方法。采用矩陣奇異值分解(SVD)的方法求解姿態(tài)陣，但是由于矩陣奇異值分解的方法固定，靈活性和適應(yīng)性較差，而且強(qiáng)烈的依賴于樣本范圍，精確性較差，受傳感器數(shù)據(jù)精度影響較大，計(jì)算精度差。也可以采用最優(yōu)姿態(tài)方法，建立初始對準(zhǔn)問題與最優(yōu)姿態(tài)確定問題之間的聯(lián)系，運(yùn)用Wahba姿態(tài)確定問題將對準(zhǔn)問題轉(zhuǎn)化為最小化求解問題，改變觀測方程結(jié)構(gòu)，建立拉格朗日方程，求解對應(yīng)最小特征值的特征向量作為最優(yōu)解，解決姿態(tài)求解問題。但是最優(yōu)姿態(tài)方法計(jì)算量，變換過程表述復(fù)雜，存在計(jì)算誤差，雖然一定程度上提升了計(jì)算精度和適應(yīng)性，但是還是存在缺陷。采用李群濾波方式，切合系統(tǒng)整體結(jié)構(gòu)，大量減少計(jì)算誤差，以最小均方誤差為估計(jì)為估計(jì)準(zhǔn)則，可以快速有效地估計(jì)出系統(tǒng)姿態(tài)矩陣。

設(shè)計(jì)李群濾波器精確估計(jì)兩個(gè)慣性系之間的關(guān)系，進(jìn)而得到捷聯(lián)姿態(tài)矩陣，完成初始對準(zhǔn)。

建立系統(tǒng)的李群濾波方程：

其中，Q_w為系統(tǒng)狀態(tài)噪聲協(xié)方差陣，由于在整個(gè)初始對準(zhǔn)過程中為常值，Q_w＝0_3×3；Q_v為系統(tǒng)觀測噪聲協(xié)方差陣；H_ξ為系統(tǒng)量測矩陣，H_ξ＝[α×]；為李群濾波一步預(yù)測估值；P_n為誤差協(xié)方差矩陣；Δx為系統(tǒng)偏差量，以李群結(jié)構(gòu)更新姿態(tài)陣。

根據(jù)以上述李群濾波方法進(jìn)行遞歸迭代，求出再根據(jù)式(2)求解從而完成捷聯(lián)慣導(dǎo)系統(tǒng)初試對準(zhǔn)過程。

附圖說明

圖1：捷聯(lián)慣性導(dǎo)航系統(tǒng)裝置總體簡圖；

圖2：捷聯(lián)慣導(dǎo)系統(tǒng)流程圖；

圖3：導(dǎo)航坐標(biāo)系到機(jī)體坐標(biāo)系的一般運(yùn)動(dòng)；

圖4：李群濾波方法流程圖；

圖5：仿真數(shù)據(jù)結(jié)果圖；其中，(a)為航向誤差角；(b)為俯仰誤差角；(c)為橫滾誤差角；

圖6：實(shí)測數(shù)據(jù)結(jié)果圖；其中，(a)為航向誤差角；(b)為俯仰誤差角；(c)為橫滾誤差角；

圖7：上位機(jī)采集真實(shí)姿態(tài)信息頁面示意圖；

具體實(shí)施方式

本發(fā)明是基于李群濾波的捷聯(lián)慣性導(dǎo)航系統(tǒng)初試對準(zhǔn)設(shè)計(jì)，下面結(jié)合本發(fā)明系統(tǒng)流程圖對本發(fā)明的具體實(shí)施步驟進(jìn)行詳細(xì)的描述：

步驟1：系統(tǒng)準(zhǔn)備階段，導(dǎo)航系統(tǒng)初始化，獲得載體所在位置的經(jīng)度λ、緯度L的基本信息，采集慣性測量單元IMU中陀螺的輸出角速度信息和加速度計(jì)的輸出信息f^b等；

步驟2：通過更新計(jì)算

由于通常是緩慢變化的，則姿態(tài)矩陣近似為：

其中

那么得到姿態(tài)陣為：

步驟3：通過陀螺測量到的角速度更計(jì)算

姿態(tài)矩陣近似為：

其中，根據(jù)雙子樣算法得，

那么得到姿態(tài)陣為：

步驟4：建立相關(guān)的系統(tǒng)方程，即系統(tǒng)狀態(tài)方程和系統(tǒng)觀測方程。

取地理坐標(biāo)系為導(dǎo)航坐標(biāo)系，慣性導(dǎo)航的基本方程，速度微分方程表示為：

將式(2)代入式(26)，得：

整理得：

積分得：

其中，vⁿ(0)為初始t₀時(shí)刻的速度。

簡化表示為：

式(30)就表示為速率方程在慣性坐標(biāo)系的積分形式，將姿態(tài)矩陣的求解問題轉(zhuǎn)化為初始時(shí)刻慣性坐標(biāo)系下的求解問題。

式(30)是關(guān)于初始姿態(tài)陣的數(shù)學(xué)方程。α(t)、β(t)表示如上，由加速度計(jì)和陀螺的輸出計(jì)算得到。

這樣給出的是α(t)、β(t)實(shí)現(xiàn)的連續(xù)形式，通過α(t)、β(t)對應(yīng)的積分迭代算法結(jié)解算α和β具體的離散值。

化簡計(jì)算得：

化簡計(jì)算得：

根據(jù)上式，建立起系統(tǒng)的觀測方程：

β_n＝R_nα_n+Q_v (35)

Q_v為系統(tǒng)觀測噪聲協(xié)方差陣。觀測方程是由實(shí)測數(shù)據(jù)運(yùn)算所得，存在誤差項(xiàng)。

由于將求解姿態(tài)矩陣的問題轉(zhuǎn)化為求解初始姿態(tài)陣的問題，我們知道在整個(gè)初始對準(zhǔn)過程中為常值，建立系統(tǒng)狀態(tài)方程如下：

R_n＝R_n-1 (36)

根據(jù)上述內(nèi)容，將姿態(tài)矩陣的求解問題轉(zhuǎn)化為初始時(shí)刻慣性坐標(biāo)系下的求解問題，建立起了具有李群結(jié)構(gòu)的系統(tǒng)方程，表示為：

步驟5：采用李群濾波估計(jì)

采用李群的結(jié)構(gòu)表示，避免了四元數(shù)的描述方式在計(jì)算過程中表述復(fù)雜和存在計(jì)算誤差的，并且在計(jì)算過程中也不存在奇異點(diǎn)的問題。李群濾波方式，切合系統(tǒng)整體結(jié)構(gòu)，大量減少計(jì)算誤差，以最小均方誤差為估計(jì)為估計(jì)準(zhǔn)則，可以快速有效地估計(jì)出系統(tǒng)姿態(tài)矩陣。

建立系統(tǒng)的李群濾波方程：

其中，Q_w為系統(tǒng)狀態(tài)噪聲協(xié)方差陣，由于在整個(gè)初始對準(zhǔn)過程中為常值，Q_w＝0_3×3；Q_v為系統(tǒng)觀測噪聲協(xié)方差陣；H_ξ為系統(tǒng)量測矩陣，H_ξ＝[α×]；為李群濾波一步預(yù)測估值；P_n為誤差協(xié)方差矩陣；Δx為系統(tǒng)偏差量，以李群結(jié)構(gòu)更新姿態(tài)陣。最后求得的R_n就是我們需要的

根據(jù)以上述李群濾波方法進(jìn)行遞歸迭代，求出再根據(jù)式(2)求解從而完成捷聯(lián)慣導(dǎo)系統(tǒng)初試對準(zhǔn)過程。

步驟6：求解姿態(tài)陣解算姿態(tài)。

前文將姿態(tài)矩陣的求解問題轉(zhuǎn)化為初始時(shí)刻慣性坐標(biāo)系下的求解問題，將姿態(tài)矩陣分解為三個(gè)部分，姿態(tài)矩陣的分解形式如下：

根據(jù)之前步驟求解的和即求解導(dǎo)航解姿態(tài)陣解算姿態(tài)信息。

本發(fā)明的有益效果如下：

(1)在以下仿真環(huán)境下，對該方法進(jìn)行仿真實(shí)驗(yàn)：

艦船受風(fēng)浪波動(dòng)影響，其航向角ψ、俯仰角θ、橫滾角γ作周期變化：

存在橫蕩、縱蕩和垂蕩引起的線速度：

ADx＝0.02m，ADy＝0.03m，ADz＝0.3m；ωDi＝2π/TDi,且TDx＝7s，TDy＝6s，TDz＝8s；為[0,2π]上服從均勻分布的隨機(jī)相位；

初始地理位置：東經(jīng)118°，北緯40°；

陀螺漂移：三個(gè)方向軸上的陀螺常值漂移為0.01°/h，隨機(jī)漂移為0.001°/h；

加速度計(jì)零位偏置：三個(gè)方向軸上的加速度計(jì)常值偏置為1×10^-4g，隨機(jī)偏置為

常數(shù)設(shè)置

赤道半徑：R_e＝6378165.0m；

橢球扁率：e＝3.352e-3；

由萬有引力得地球表面重力加速度：g₀＝9.78049；

地球自轉(zhuǎn)角速度(單位為rad/s)：

常數(shù)：π＝3.1415926；

方法仿真結(jié)果如下：

進(jìn)行了600s仿真，仿真結(jié)果如5所示。以姿態(tài)誤差角的估計(jì)誤差作為衡量精對準(zhǔn)的指標(biāo)?？梢钥吹贸?，俯仰姿態(tài)在180s內(nèi)完成精對準(zhǔn)，穩(wěn)定在0.02度；橫滾姿態(tài)在40s內(nèi)完成精對準(zhǔn)，穩(wěn)定在0.01度；航向姿態(tài)在180s內(nèi)完成精對準(zhǔn)，精度為0.02度以內(nèi)。由仿真結(jié)果可知，本方法可以快速有效的完成解算對準(zhǔn)過程，相比于四元數(shù)卡爾曼濾波方法，超調(diào)量明顯較小，收斂速度快，而且濾波精度較好。

(2)通過真實(shí)實(shí)驗(yàn)對本發(fā)明提出的基于李群濾波的捷聯(lián)慣導(dǎo)初試系統(tǒng)進(jìn)行驗(yàn)證。真實(shí)試驗(yàn)中，不提任何供外界輔助信息，系統(tǒng)裝置放置在車上，有人員上下車、開關(guān)車門、對車進(jìn)行晃動(dòng)等干擾。實(shí)驗(yàn)歷時(shí)600s，試驗(yàn)地點(diǎn)在北京工業(yè)大學(xué)羽毛球館南廣場位置。上位導(dǎo)航計(jì)算機(jī)控制導(dǎo)航系統(tǒng)，以100HZ的數(shù)據(jù)更新速率，115200bps的波特率，采集航向精度達(dá)0.1度、姿態(tài)精度達(dá)0.05度的實(shí)際三軸姿態(tài)信息，其上位機(jī)采集頁面示意圖如圖7。解算獲得的載體姿態(tài)信息與本步驟中得到的高精度真實(shí)載體姿態(tài)信息做比較，證明本方法和系統(tǒng)的可行性和有效性。

系統(tǒng)實(shí)驗(yàn)結(jié)果如下：

截取600s的實(shí)際數(shù)據(jù)，結(jié)果如圖6所示。以姿態(tài)誤差角的估計(jì)誤差作為衡量精對準(zhǔn)的指標(biāo)?？梢钥吹贸觯┭鲎藨B(tài)在180s內(nèi)完成精對準(zhǔn)，穩(wěn)定在0.01度；橫滾姿態(tài)在40s內(nèi)完成精對準(zhǔn)，穩(wěn)定在0.01度；航向姿態(tài)在180s內(nèi)完成精對準(zhǔn)，精度為0.03度以內(nèi)。本方法可以快速有效的完成解算對準(zhǔn)過程，相比于四元數(shù)卡爾曼濾波方法，超調(diào)量較小，收斂速度明顯較快，而且濾波精度較好。