[發明專利]基于LGMKF的SINS捷聯慣性導航系統自對準方法

有效

• 申請號: 201910224807.6
• 申請日: 2019-03-24
• 公開（公告）號: CN109931957B
• 公開（公告）日: 2020-08-28
• 發明（設計）人: 裴福俊;朱德森;蔣寧;徐浩;尹舒男
• 申請（專利權）人: 北京工業大學
• 主分類號: G01C25/00
• 分類號: G01C25/00
• 代理公司: 北京思海天達知識產權代理有限公司 11203
• 代理人: 沈波

1.基于LGMKF的SINS捷聯慣性導航系統自對準方法，其特征在于，該方法通過下述步驟實現：

步驟(1)：SINS進行系統預熱準備，啟動系統，獲得載體所在位置的經度λ、緯度L基本信息，獲取傳感器實時數據，傳感器實時數據包括慣性測量單元IMU中陀螺儀輸出的載體系相對于慣性系的旋轉角速率信息在載體系的投影和加速度計輸出的載體系加速度信息f^b；

步驟(2)：對采集到的陀螺儀和加速度計的數據進行預處理，基于李群微分方程，建立基于李群描述的線性初始對準系統模型，所述線性初始對準系統模型包括系統線性狀態模型和線性量測模型：

本方法的詳細描述中坐標系定義如下：

地球坐標系e系，選取地球中心為原點，X軸位于赤道平面內，從地心指向本初子午線，Z軸從地心指向地理北極，X軸、Y軸和Z軸構成右手坐標系，隨地球自轉而轉動；

地心慣性坐標系i系，選取地球中心為原點，X軸位于赤道平面內，從地心指向春分點，Z軸從地心指向地理北極，X軸、Y軸和Z軸構成右手坐標系；

導航坐標系n系，表示載體所在位置的地理坐標系，選取艦載機重心為原點，X軸指向東向E，Y軸指向北向N，Z軸指向天向U；本方法中導航坐標系選取為地理坐標系；

載體坐標系b系，表示捷聯慣性導航系統三軸正交坐標系，選取艦載機重心為原點，X軸、Y軸、Z軸分別沿艦載機機體橫軸指向右、沿縱軸指向前、沿立軸指向上；

初始導航坐標系n(0)系，表示SINS開機運行時刻的導航坐標系，并在整個對準過程中相對于慣性空間保持靜止；

初始載體坐標系b(0)系，表示SINS開機運行時刻的載體坐標系，并在整個對準過程中相對于慣性空間保持靜止；

基于李群微分方程，建立基于李群描述的線性初始對準系統模型：

根據SINS初始對準原理，SINS的自對準問題，轉化為姿態變換的求解問題；姿態變換表示為兩個坐標系之間的旋轉變換，SINS慣性導航系統的姿態變換通過一個3×3的正交變換矩陣表示；根據李群定義及其性質，該正交變換矩陣符合李群的特殊正交群SO(n)的性質，構成了三維旋轉群SO(3)：

其中，三維旋轉群R∈SO(3)表示特定的導航姿態矩陣，表示3×3的向量空間，上標T表示矩陣的轉置，I表示三維單位矩陣，det(R)表示為矩陣R的行列式；

因此，SINS中姿態變換的求解問題，轉化為對基于李群描述的姿態矩陣R的求解問題；根據基于李群描述的姿態矩陣鏈式法則，導航姿態矩陣分解為三個矩陣乘積形式：

其中，t表示時間變量，表示當前載體系相對于當前導航系的姿態變換矩陣，表示初始導航系相對于當前導航系的姿態變換矩陣，初始姿態矩陣表示初始載體系相對于初始導航系的姿態變換矩陣，表示當前載體系相對于初始載體系的姿態變換矩陣；

根據李群微分方程，姿態矩陣和隨時間變化更新過程為：

其中，表示初始載體系相對于當前載體系的姿態變換矩陣，表示導航系相對于慣性系的旋轉角速率在導航系的投影，表示地球系相對于慣性系的旋轉角速率在導航系的投影，表示導航系相對于地球系的旋轉角速率在導航系的投影，表示陀螺儀輸出的載體系相對于慣性系的旋轉角速率在載體系的投影；符號(·×)表示將一個三維向量轉換成一個反對稱矩陣的運算，運算規則如下：

由公式(2)-(5)看出，和由傳感器數據實時計算得到，而初始姿態矩陣是一個常值矩陣；因此，SINS中姿態矩陣的求解問題，轉化為對基于李群描述的初始姿態矩陣的求解問題；

根據SINS初始對準原理，載體速度微分方程表示為：

其中，vⁿ表示相對于地球的載體速度信息，fⁿ表示比力在導航系下的投影，f^b表示比力在載體系下的投影即加速度計輸出的載體系加速度信息，gⁿ表示當地重力加速度在導航系下的投影；

將公式(2)代入公式(6)，可得：

對公式(7)兩邊同時左乘姿態變換矩陣整理后得：

在[0,t]上對公式(8)兩邊同時積分，整理后得：

其中，vⁿ(0)表示初始時刻載體的速度信息；

公式(9)簡化為：

其中

公式(10)表示為基于李群描述的SINS速度微分方程在慣性坐標系的積分形式，公式(11)和公式(12)可由傳感器數據實時計算得到；由于公式(10)看作是關于初始姿態矩陣的數學方程，且初始姿態矩陣為數值未知的常值矩陣，因此初始姿態矩陣的求解問題，轉化為初始姿態矩陣的最優估計問題；

建立系統線性狀態方程模型為：

建立系統線性量測方程模型為：

β(t)＝R(t)α(t) (14)

步驟(3)：將步驟(2)所得的基于李群描述的線性初始對準系統模型做離散化處理：

根據公式(13)，由于將SINS自對準問題轉化為對初始姿態矩陣的最優估計問題，且在整個自對準過程中為常值矩陣，因此建立離散形式的系統線性狀態方程為：

由于公式(14)中給出的和均為連續形式，在實際計算中需要對其作離散處理；通過α(t)和β(t)對應的積分迭代算法具體解算α和β的具體值；在載體作姿態更新過程中，由于處于角晃動或線晃動激烈、頻繁的外部環境時，單子樣旋轉矢量法對不可交換誤差的補償程度較低，會造成嚴重的算法漂移；多子樣旋轉矢量法可以有效避免該問題，子樣數越高，算法精度越高，但計算量也隨之增大；綜合考慮，選擇雙子樣旋轉矢量法對α(t)和β(t)進行積分迭代計算；

矢量在t_k時刻采用雙子樣旋轉矢量法求解，近似后得到：

矢量在t_k時刻采用雙子樣旋轉矢量法求解，近似后得到：

其中，M為系統循環次數，k＝0,1,2,...,M-1，Δv₁和Δv₂分別表示兩個相鄰半采樣周期內加速度計測量得到的速度變化量，Δθ₁和Δθ₂分別表示兩個相鄰半采樣周期內陀螺儀測量角度變化量，T為采樣周期，表示單位矩陣；

由于量測方程是根據傳感器實測數據計算所得，存在量測誤差；根據公式(14)、公式(16)和公式(17)，建立離散形式的系統線性量測方程為：

β(t_k)＝R(t_k)α(t_k)+Q(t_k) (18)

其中，Q(t_k)為系統量測噪聲協方差矩陣，α(t_k)和β(t_k)由采集到的陀螺儀和加速度計數據實時計算得到；

由于初始姿態矩陣為符合三維旋轉群SO(3)性質的李群矩陣，因此SINS中初始姿態矩陣的最優估計問題，轉化為對三維旋轉群SO(3)群的最優估計問題；根據上述內容，建立基于李群描述的線性初始對準濾波模型為：

基于李群描述初始姿態矩陣的SINS自對準系統，能夠避免基于四元數描述初始姿態矩陣的SINS自對準系統中由四元數構造初始姿態矩陣產生的非唯一性問題和非線性問題；

步驟(4)：根據LGMKF最優估計算法，直接對基于李群描述的初始姿態矩陣進行最優估計：

由于矩陣濾波是一種對矩陣形式狀態量的最小方差無偏估計方法，基于李群描述的線性初始對準濾波模型中狀態量為矩陣形式，因此結合李群特性和矩陣濾波原理設計了一種基于LGMKF的SINS慣性導航系統自對準方法；LGMKF最優估計算法中，濾波新息反映的是測量矩陣與估計矩陣之間的差值，噪聲矩陣的協方差矩陣可由其向量形式的協方差定義得到；

LGMKF最優估計算法時間更新方程為：

基于三維旋轉群SO(3)群的特性和矩陣濾波原理，狀態矩陣和誤差協方差矩陣的一步預測可以表示為：

P_k+1/k＝P_k/k+Q^w (21)

其中，表示k+1時刻的初始姿態矩陣的一步預測，表示k時刻的初始姿態矩陣的后驗估計，表示k+1時刻的誤差協方差矩陣的一步預測，表示k時刻的誤差協方差矩陣的后驗估計，表示k時刻的過程噪聲協方差矩陣；

LGMKF最優估計算法測量更新方程為：

LGMKF觀測方程中量測矩陣為矢量形式，在濾波過程中不能維持SO(3)群的特性同時不滿足矩陣乘法的維數要求，根據矩陣濾波原理，在保留原有量測信息基礎上將α_k+1擴維得到k+1時刻的量測矩陣H_k+1：

其中，表示k+1時刻由陀螺儀和加速度計輸出得到的狀態信息矩陣，表示單位矩陣，符號表示克羅內克積，運算規則如下：

根據矩陣濾波原理和克羅內克積特性，k+1時刻的LGMKF濾波增益矩陣K_k+1可以重構為：

其中，表示k+1時刻的誤差協方差矩陣的一步預測，表示k+1時刻的量測矩陣，表示k+1時刻的量測噪聲協方差矩陣，上標-1表示對矩陣進行求逆運算；

LGMKF中濾波新息定義為測量矩陣與估計測量矩陣之間的差值，k+1時刻的濾波新息矩陣表示為：

其中，和由陀螺儀和加速度計輸出解算得到，表示k+1時刻的初始姿態矩陣的一步預測；

由于LGMKF中濾波新息矩陣為矢量形式，不能滿足LGMKF濾波要求，為此在計算k+1時刻初始姿態矩陣的后驗估計時需要對和K_k+1做特殊處理：

其中，表示k+1時刻的初始姿態矩陣的一步預測，表示k+1時刻的濾波新息矩陣，定義如下：

根據矩陣濾波原理，k+1時刻的LGMKF誤差協方差矩陣的后驗估計可以表示為：

其中，表示單位陣，表示k+1時刻的濾波增益矩陣，表示k+1時刻的轉換矩陣，表示k+1時刻的誤差協方差矩陣的一步預測，表示k+1時刻的量測噪聲協方差矩陣；

因此，LGMKF最優估計算法歸納為：

基于LGMKF的SINS自對準方法，通過將初始姿態估計問題轉化為SO(3)群的最優估計問題，直接對初始姿態矩陣進行最優估計，實現了SINS一步直接自對準過程，不僅能夠大幅度縮短了對準時間，還能夠提高對準的可靠性；基于LGMKF的SINS自對準方法，能夠避免傳統四元數濾波過程中由四元數向初始姿態矩陣轉換時存在的復雜表述問題和大量計算誤差，能夠有效降低計算復雜度并提高對準精度；

步驟(5)：根據步驟(4)所得的基于李群描述的的初始姿態矩陣求解導航系統所需的姿態矩陣

根據之前步驟中求解得到的姿態變化矩陣和信息，通過公式(2)即可求解導航姿態矩陣

步驟(6)：自對準過程運行時間為MT，獲取傳感器實時數據的時刻為kT，T為傳感器采樣周期，k＝0,1,2,...,M，若k＝M，則輸出自對準解算結果，完成SINS自對準過程，若kM，表示自對準過程未完成，則重復上述驟(1)至步驟(5)，直至完成SINS自對準過程。