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

背景技術(shù)：

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

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

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

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

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

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

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

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

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

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

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

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

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

方法的整體計算流程如2所示。

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

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

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

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

取地理坐標系為導航坐標系，慣性導航的基本方程，載體速度微分方程表示為：

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

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

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

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

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

式(5)的左邊展開為：

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

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

簡化表示為：

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

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

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

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

矢量α(t)近似為：

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

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

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

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

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

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

β_n＝R_nα_n+Q_v (19)

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

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

R_n＝R_n-1 (20)

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

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

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

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

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

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

附圖說明

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

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

圖3：導航坐標系到機體坐標系的一般運動；

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

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

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

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

具體實施方式

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

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

步驟2：通過更新計算

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

其中

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

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

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

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

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

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

取地理坐標系為導航坐標系，慣性導航的基本方程，速度微分方程表示為：

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

整理得：

積分得：

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

簡化表示為：

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

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

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

化簡計算得：

化簡計算得：

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

β_n＝R_nα_n+Q_v (35)

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

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

R_n＝R_n-1 (36)

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

步驟5：采用李群濾波估計

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

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

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

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

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

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

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

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

(1)在以下仿真環(huán)境下，對該方法進行仿真實驗：

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

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

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

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

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

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

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

赤道半徑：R_e＝6378165.0m；

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

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

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

常數(shù)：π＝3.1415926；

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

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

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

系統(tǒng)實驗結(jié)果如下：

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