[發明專利]基于李群最優估計的捷聯慣性導航晃動基座粗對準方法

有效

• 申請號: 202010973054.1
• 申請日: 2020-09-16
• 公開（公告）號: CN112229421B
• 公開（公告）日: 2023-08-11
• 發明（設計）人: 裴福俊;楊肅;尹舒男;朱德森;張恒銘
• 申請（專利權）人: 北京工業大學
• 主分類號: G01C25/00
• 分類號: G01C25/00
• 代理公司: 北京思海天達知識產權代理有限公司 11203
• 代理人: 沈波

1.基于李群最優估計的捷聯慣性導航晃動基座粗對準方法，其特征在于，該方法通過下述步驟實現：

步驟(1)：SINS捷聯慣性導航系統進行系統預熱準備，啟動系統，獲得載體所在位置的經度λ、緯度L、當地重力加速度在導航系下的投影gⁿ基本信息，采集慣性測量單元IMU中陀螺儀輸出的載體系相對于慣性系的旋轉角速率信息在載體系的投影和加速度計輸出的載體系加速度信息f^b；

步驟(2)：對采集到的陀螺儀和加速度計的數據進行預處理，基于李群微分方程，建立基于李群描述的線性晃動基座自對準系統模型：

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

根據SINS捷聯慣性導航系統原理，SINS晃動基座自對準問題轉化為姿態估計問題，姿態變換為兩個坐標系之間的旋轉變換，導航的姿態矩陣用一個3×3的正交變換矩陣表示；該正交變換矩陣符合李群的特殊正交群SO(n)的性質，構成三維旋轉群SO(3)：

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

晃動基座自對準姿態估計問題轉化為對基于李群描述的姿態矩陣R的求解問題；根據基于李群描述的姿態矩陣的鏈式法則，將導航姿態矩陣分解為三個矩陣的乘積形式：

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

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

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

由公式(2)-(5)可知，和由IMU傳感器數據實時計算得到，而表示初始時刻的姿態矩陣，其不隨時間變化；因此，SINS自對準過程中姿態矩陣的求解問題，轉化為對基于李群描述的初始姿態矩陣的求解問題；

根據捷聯慣性導航晃動基座粗對準原理以及公式(2)，基于李群描述的晃動基座粗對準模型可以寫成如下形式：

其中，Vⁿ(t)和V^b(t)分別表示載體在n系下的速度和b系下的速度，Vⁿ(t)和V^b(t)可通過下式求得：

并且由于初始旋轉矩陣是常數矩陣，由此晃動基座粗對準模型寫為如下形式：

其中R(t_k)表示t_k時刻的

步驟(3)：根據李代數的物理定義求解誤差李代數，并利用李群與李代數之間的指數映射對R(t_k)進行估計；

根據旋轉矩陣的定義和公式(9)，Vⁿ(t)和R(t_k)V^b(t)是n系下的同一個矢量，向量積為0；然而，由于誤差，估計的旋轉矩陣與實際的旋轉矩陣R(t_k)之間存在偏差，這導致估計的旋轉矩陣是b系到n′系的姿態矩陣，既：

根據鏈式法則，為補償估計的旋轉矩陣，我們只需要計算n′系與n系之間的旋轉關系，也就是

根據羅德里格斯的旋轉公式：

R＝exp(φ)＝exp(θa)＝(cosθ)I+(1-cosθ)aa^T+(sinθ)a_×???????(11)

任意的旋轉矩陣可以通過起對應的旋轉李代數φ求得，并且任意旋轉李代數(旋轉向量)φ由轉角θ以及轉軸a通過下式求得：

φ＝θa???????????????????(12)

根據坐標系變換的基本原理，兩坐標系之間的旋轉關系等價于兩個系下同一矢量的旋轉關系，由此n′系與n系之間的旋轉關系，也就是等價與求解觀測矢量Vⁿ(t)和預測矢量之間的旋轉關系，因此根據式(11)和式(12)只要確定Vⁿ(t)和之間的轉軸和轉角確定Vⁿ(t)和之間的轉軸和轉角以及旋轉關系；

轉軸a通過對單位化來求得：

轉角θ則通過下式求得：

于是誤差李代數寫成：

于是李群最優估計的更新方程可建立如下：

2.根據權利要求1所述的基于李群描述的捷聯慣性導航動基座初始對準方法，其特征在于，步驟(3)中，根據李代數的物理定義求解誤差李代數，并利用李群與李代數之間的指數映射對R(t_k)進行估計；

步驟(3.1)通過用李群的左乘不變性以及李群與李代數之間的指數映射分離狀態相關噪聲的方式為：

根據旋轉矩陣的定義和公式(9)，Vⁿ(t)和R(t_k)V^b(t)是n系下的同一個矢量，其向量積為0；然而，由于誤差，估計的旋轉矩陣與實際的旋轉矩陣R(t_k)之間存在偏差，這導致估計的旋轉矩陣是b系到n′系的姿態矩陣：

根據鏈式法則，為補償估計的旋轉矩陣，只需要計算n′系與n系之間的旋轉關系，也就是

根據羅德里格斯的旋轉公式：

R＝exp(φ)＝exp(θa)＝(cosθ)I+(1-cosθ)aa^T+(sinθ)a_×??????(18)

任意的旋轉矩陣通過起對應的旋轉李代數φ求得，并且任意旋轉李代數φ由轉角θ以及轉軸a通過下式求得：

φ＝θa??????????????????(19)

根據坐標系變換的基本原理，兩坐標系之間的旋轉關系等價于兩個系下同一矢量的旋轉關系，由此n′系與n系之間的旋轉關系，也就是等價與求解觀測矢量Vⁿ(t)和預測矢量之間的旋轉關系，因此根據式(18)和式(19)只要確定Vⁿ(t)和之間的轉軸和轉角就確定Vⁿ(t)和之間的轉軸和轉角以及旋轉關系；

由此轉軸a通過對單位化來求得：

轉角θ則通過下式求得：

于是誤差李代數寫成：

于是李群最優估計的更新方程可建立如下：