[發明專利]動態干擾條件下捷聯慣導系統的緯度未知自對準方法

有效

• 申請號: 201610540157.2
• 申請日: 2016-07-10
• 公開（公告）號: CN106123921B
• 公開（公告）日: 2019-05-24
• 發明（設計）人: 裴福俊;智巖;梁青琳;魏曉麗
• 申請（專利權）人: 北京工業大學
• 主分類號: G01C25/00
• 分類號: G01C25/00;G01C21/16
• 代理公司: 北京思海天達知識產權代理有限公司 11203
• 代理人: 沈波

1.動態干擾條件下捷聯慣導系統的緯度未知自對準方法，其特征在于：

坐標系定義如下：地球坐標系e系，原點選取地球中心，X軸位于赤道平面內，從地心指向載體所在點經線，Z軸沿地球自轉軸方向，隨地球自轉而轉動，X軸、Y軸和Z軸構成右手坐標系，隨地球自轉而轉動；地心慣性坐標系i系，是在粗對準起始時刻將地球坐標系e系慣性凝固后形成的坐標系；導航坐標系n系，即導航基準的坐標系，導航相關運算都在該坐標系下進行，原點位于艦載機重心，X軸指向東向，Y軸指向北向，Z軸指向天向；載體坐標系b系，原點位于艦載機重心，X軸、Y軸、Z軸分別沿艦載機機體橫軸指向右、沿縱軸指向前、沿立軸指向上；載體慣性坐標系ib0系，是在對準初始時刻將載體坐標系經慣性凝固后的坐標系；

步驟1：系統準備階段，捷聯慣導系統進行預熱準備，上位導航計算機開啟；

步驟2：導航算法初始化，正確設置相關參數；

步驟3：上位導航計算機控制導航系統向導航系統發送命令協議，采集慣性測量單元IMU中光纖陀螺和加速度計的輸出數據，光纖陀螺儀的零點偏置穩定性是0.5度/h，零點偏置重復性是0.5度/h，加速度計的零點偏置小于1mg，零點偏置穩定性小于1mg；

步驟4：得到光纖陀螺儀的三軸測量值和加速度計的三軸測量值f^b后，利用重力加速度積分所得速度信息不同時刻在慣性坐標系下的投影，構建幾何解析公式，求解載體所在位置的緯度信息，完成存在動態干擾的環境下的緯度求解過程；然后對緯度誤差進行分析求解，證明本緯度計算方法的可行性；

(1)動態干擾情況下求解緯度信息過程

光纖陀螺儀的三軸測量值和加速度計的三軸測量值f^b采集成功后，進入載體晃動基座下緯度信息解算過程，將重力加速度積分所得速度值投影到慣性坐標系下得到利用兩個不同時刻慣性坐標系下的速度投影之間的夾角求取緯度，隔離晃動干擾對緯度求解的影響；

載體坐標系b系隨地球一起轉動，慣性坐標系i系和ib0系沒有轉動，保持不變；由于地球的轉動，重力加速度g在慣性空間ib0系內的方向投影由t₁時刻的g^ib0(t₁)變成了t₂時刻的g^ib0(t₂)，設g^ib0(t₁)與g^ib0(t₂)之間的夾角為θ，地球在這段時間內轉過角度為α；緯度計算，夾角θ、夾角α和緯度L三個角度之間存在幾何關系，因此通過求取θ和α，間接地求取緯度信息L，下面敘述具體過程；

因為A與B在同一緯度，所以有式：

|AO'|＝|BO'|＝|AO|cosL (1)

由幾何定理知：

將公式(1)帶入(2)得：

同理可得：

由公式(3)和公式(4)得緯度的求解公式為：

cosL＝sin(θ/2)/sin(α/2) (5)

即

確定起始時刻t₁和緯度估計時間t₂，ω_ie為地球坐標系e系相對于地心慣性坐標系i系的地球自轉角速度大小，在t₁、t₂時間差內地球轉過的角度α求得：

α＝ω_ie(t₂-t₁) (7)

在本方法基于慣性空間算法中，重力加速度g在慣性空間ib0系內的方向投影計算方式如公式(8)所示：

式中，g^b(t)為t時刻重力加速度g在b系內的方向投影，f^b(t)為t時刻加速度計b系的三軸測量值；載體坐標系b系與載體慣性坐標系ib0系之間的轉動關系利用陀螺輸出的角速度信息進行更新解算，如式(9)所示：

其中初始時刻為單位陣；為光纖陀螺輸出值，表示b系相對于i系的載體運動角速率在b系內的投影；為向量的叉乘反對稱矩陣；

由此，θ由兩個時刻的重力加速度確定：

式中，<>為內積符號，||表示求取向量的模值；

考慮重力加速度單次量測值存在隨機誤差性；晃動基座下，加速度計輸出不僅受加速度計自身漂移誤差影響，還受到受量測噪聲、載體角振動、線振動的干擾；由于存在的載體線振動干擾，必然使得加速度計某時刻的輸出重力加速度值受到影響存在誤差項，以至于導致計算出來的θ角不準確，使得緯度計算值存在誤差項；更甚者，使得θ角計算嚴重不準確且大于α角，此時cosL>1，緯度值無法計算，出現計算的結果為奇異值；因此，本方法針對θ角計算不準確的問題，本方法對該算法進行改進；利用積分對數據誤差的平滑作用，通過對兩個時刻的重力加速度進行相同時間的積分平滑，平滑一段時間內某些時刻因受載體線振動的比力偏差點，抑制和補償搖擺基座載體線振動的干擾；

對兩個時刻的重力加速度進行相同時間的積分，積分時間t₀，得到慣性空間ib0系內的速度值以達到避免偶然的加速度計輸出帶來的緯度計算誤差的效果；

則θ由下式求得：

綜上所述，動態干擾情況下求解緯度信息可總結為如下步驟：

①根據公式(9)，更新計算坐標轉換矩陣；

②在時刻t₁和t₂根據公式(8)，分別將加速度計的輸出投影到i_b0坐標系，并利用公式(11)進行積分得到和積分時間為t₀；

③根據公式(7)計算在t₂-t₁時間內地球自轉轉過的角度α；

④根據公式(12)求出g^ib0(t₁)與g^ib0(t₂)之間的夾角θ；

⑤由公式求(6)出緯度L；

(2)對緯度誤差進行分析求解

設真實緯度為L，緯度誤差為δL，計算求出的緯度為則有

由于計算緯度過程中α是精確地，則緯度誤差主要來源于θ；設兩速度投影間的真實角度為θ，角度誤差為δθ，計算求出的角度為有

設t時刻的重力加速度在i_b0系下投影的誤差為載體坐標系與載體慣性坐標系之間轉換矩陣的誤差為光纖陀螺三軸b系輸出的誤差為δf^b(t)，則有：

即

展開得：

忽略二階小量得

由公式(13)(14)(15)(17)，則有：

將上式展開，近似得：

其中ε(t)×為陀螺誤差構成的叉乘反對稱矩陣，ε(t)很小，忽略二階小量得：

對緯度求解公式(6)求微分得：

結合公式(23)、公式(20)和公式(22)，得緯度誤差公式：

因為ε(t)很小，所以在公式中所占比例很小，則緯度誤差主要是由第二項和第四項決定，也就是誤差主要受加速度計輸出的影響，陀螺誤差帶來的影響比較小，而通過積分運算加速度計輸出的誤差也進一步降低，因此這種方法較好完成動基座下緯度的求解；

步驟5：在載體所在位置的緯度信息求解完成的基礎上，帶入計算求得的緯度值，基于雙矢量定姿原理，利用重力加速度在慣性空間方位的改變包含地球北向信息這一特性，求解慣性系下粗略的初始姿態陣，完成粗對準過程；

在慣性系下捷聯慣導系統的自對準粗對準算法中，以慣性空間為參考基準，導航坐標系n系與載體坐標系b系之間的初始姿態矩陣分解成4個矩陣求解，姿態矩陣的分解表達式為：

其中，載體坐標系b系與載體慣性坐標系為ib0系之間的轉換矩陣由公式(9)求解；導航坐標系n系與地心慣性坐標系i系之間的轉換矩陣由所求得的運載體所在對準點的地理位置緯度信息和初始對準時間確定：

導航坐標系n系與地球坐標系e系之間的轉換矩陣：

地球坐標系e系與地心慣性坐標系i系之間的轉換矩陣：

地心慣性坐標系i系與載體慣性坐標系ib0系之間的轉動關系是不隨時間變化的常值矩陣，其確定過程是姿態矩陣估算問題的關鍵，是初始對準的核心；

因為地心慣性坐標系i系內的速度投影Vⁱ與載體慣性坐標系ib0系內的速度投影有關系式因此可根據速度在兩慣性坐標系中的投影Vⁱ和構造矢量，采用雙矢量定姿法求解；

具體求解過程如下：

在粗對準過程中將加速度計的刻度系數誤差和安裝誤差忽略，則加速度計的測量值包含重力加速度g^b、載機垂蕩、縱蕩、橫蕩及振動而產生的干擾加速度桿臂干擾加速度和緯度誤差引起的重力加速度誤差δg^b，加表常值偏置向量為加速度計的常值偏置誤差，為高斯白噪聲；上述變量中，上標b表示變量在載體坐標系b系的投影；

設

積分得i_b0坐標系下的速度為：

由于為線振動干擾速度，與重力加速度積分的速度相比較小，粗對準過程中可以將其忽略；同理，由緯度誤差引起的重力加速度誤差可以忽略，故上式可化簡為：

設n系下的重力加速度值為g_n，則i系下的重力加速度值gⁱ為：

設Δt_k＝t_k-t₀，積分得i系下的速度投影Vⁱ：

根據式(30)可知，在tk1、tk2時刻有，

式中，t0<tk1<tk2，由上式得：

式中，Vⁱ(t_kj)(j＝1,2)由式(32)計算，按式(29)和式(30)計算；將計算得到的代入式(25)，即可計算出得到慣性系下粗略的初始姿態陣，完成捷聯慣導系統的粗對準；

步驟6：通過上述粗對準方法計算出姿態陣的粗略值，姿態誤差角可視為小角，在此基礎上利用基于新息的自適應濾波獲取載體的失準角，建立了緯度未知動態干擾條件下的精對準誤差模型，包含有垂蕩、縱蕩、橫蕩引起的干擾運動和由于緯度計算引起的誤差；然后采用基于新息的自適應濾波方法解算緯度誤差值和載體的失準角，用緯度誤差角補償緯度值，用失準角修正捷聯姿態矩陣，得到三軸姿態信息，實現精對準過程，完成初始對準，進入導航狀態；

(1)精對準誤差模型

在精對準階段，載體坐標系b系與導航坐標系n系之間的姿態矩陣可以由三個矩陣相乘得到：

式中，導航坐標系n系與地球坐標系e系之間的轉換矩陣和地球坐標系e系與地心慣性坐標系i系之間的轉換矩陣按照上述粗對準方案中的計算方法進行求取；因此，的確定關鍵在于地心慣性坐標系i系與載體坐標系b系之間的轉換矩陣的確定；

設i'系是由陀螺輸出計算得到的存在失準角的慣性坐標系，i系是真實的慣性坐標系，為解算得到的存在失準角的慣性坐標系i'系與真實的慣性坐標系i系之間的轉換關系；

式中，解算得到的存在失準角的慣性坐標系i'系與載體坐標系b系之間的轉換矩陣由下式計算：

其中，是通過粗對準過程建立的載體坐標系與計算慣性坐標系之間的轉換矩陣，作為精對準開始時的轉換矩陣；而轉換矩陣改變值可以由加速度計和陀螺儀的輸出計算得到；由于慣性器件存在各種誤差，導致得到的計算慣性坐標系i'與真實的慣性坐標系i之間存在誤差；設i'系與i系之間的失準角為三軸值分別表示為和

則從i系到i'系的坐標轉換矩陣為：

將公式(38)帶入公式(36)中即可求出綜上所述，精對準的主要目的就是求出失準角，用失準角修正捷聯姿態矩陣；

在精對準過程中，加速度計的刻度系數誤差和安裝誤差不可忽略；設加表刻度系數誤差矩陣為δK_A＝diag(δK_Ax,δK_Ay,δK_Az)，加表安裝誤差矩陣為則加速度計輸出的比力在i'系中的投影為：

忽略二階小項之后，可得：

將上式移項并整理，得：

式中，δaⁱ為等效干擾加速度，且

對上式積分，得：

設

設慣性系i系下的速度誤差為δVⁱ，δVⁱ的三軸值分別為和加速度計常值偏置誤差的三軸值分別為和高斯白噪聲的三軸值分別為和由式(48)得到慣性系i系下的速度誤差方程：

量測矢量表示為：

由姿態轉換陣和的微分方程，得到慣性系下的失準角方程：

式中，ε^b和ω^b分別為b系的陀螺的常值漂移和量測高斯白噪聲，ε^b的三軸值分別表示為和ω^b的三軸值分別表示為和

設地球半徑為Re，緯度誤差δL可由速度誤差求得：

δL＝δVx/Re (52)

根據慣性系下的速度誤差方程、失準角方程和緯度誤差方程，系統的狀態方程是如下形式：

其中，狀態矢量X和系統噪聲W表示為：

狀態轉移矩陣A和系統噪聲矩陣B表示為：

式中F＝[1/R_e 0 0]；

由公式(50)，得到系統的量測方程為：

其中，量測矩陣為H＝[I_3×3 0_3×10]；為不確定性量測干擾，主要由不同方向的干擾速度以及緯度誤差引起的誤差構成；V_w為量測高斯白噪聲；

由于存在計算的緯度誤差，導致計算結果存在影響；精對準過程中，通過濾波過程，可以估算出陀螺和加速度計的誤差，根據公式(24)計算出緯度誤差，實時對緯度進行補償，以降低緯度誤差對精對準的影響；

(2)采用基于新息的自適應濾波方法

系統離散模型的量測方程和狀態方程為：

式中，X_k為k時刻的狀態矢量；Z_k為k時刻的量測矢量；Ф_k,k-1為k-1時刻到k時刻的一步轉移矩陣；Γ_k-1為k-1時刻的系統噪聲矩陣；H_k為k時刻的量測矩陣；W_k-1和V_k為隨機干擾的白噪聲，U_k為不確定性量測干擾；

經典卡爾曼濾波采用新息的理論協方差計算濾波增益矩陣K_k，但是新息的理論協方差無法直接反映出外部量測噪聲的變化；本方法使用新息的實際協方差代替理論協方差來計算濾波增益矩陣K_k；這樣量測噪聲增加，濾波增益矩陣K_k隨之增加，反之亦然；

對新息進行加權求和：

由極大似然估計知系統噪聲協方差矩陣R_k的估計值為：

R_k的估計值代替經典卡爾曼公式中的R_k得到

不難發現，實際上使用實際協方差代替理論協方差，其原理在于使用極大似然估計值對R_k進行實時估算；

由于新息的不穩定性，使用實際新息的實際協方差進行計算時，由于其誤差大小的不確定性，可能導致濾波不穩定甚至發散；所以本方法對其采用自適應的方式進行計算，增加自適應因子實時調整新的新息在中所占比例；由此，可由公下式計算：

其中α為調節因子，用于調節新的新息在新息加權求和過程中所占的比例，可根據經驗選擇默認值；注意該算法當不存在調節因子α的時候，隨著k的增加，舊的新息比例會隨之減小；由濾波收斂判據知，當濾波器發散的時候，誤差協方差矩陣是無界的，這個時候實際協方差會比理論協方差大很多倍；以此為依據，當新的新息比新息邊界大的時候，認為此次的新息誤差過大，為降低其比例降低α，反之α采用默認值，以此抑制濾波器發散；

α的計算方法如公式如下：

其中為允許的誤差邊界；從公式可以看出，超過誤差允許邊界，距離越遠，α越小，則新的新息在計算過程中所占比例越小；

系統基于新息的自適應濾波方程如下：

狀態一步預測方程：

均方誤差一步預測方程：

濾波增益方程：

狀態估計方程：

均方誤差估計方程：P_k＝(I-K_kH_k)P_k|k-1 (64e)

的計算方程：

α的計算方程：

新息的計算方程：

利用基于新息的自適應濾波方法可估計出系統狀態矢量，系統狀態矢量的第四項、第五項、第六項即為載體的失準角

步驟7：終止程序，關閉系統。