[發明專利]一種基于地球重力場與地磁場序貫修正的姿態解算方法

有效

• 申請號: 201810384477.2
• 申請日: 2018-04-26
• 公開（公告）號: CN108871319B
• 公開（公告）日: 2022-05-17
• 發明（設計）人: 李志;王錄;汪璟;倪瑞
• 申請（專利權）人: 李志;王錄;汪璟;倪瑞
• 主分類號: G01C21/08
• 分類號: G01C21/08
• 代理公司: 暫無信息
• 代理人: 暫無信息

本發明提供一種基于地球重力場與地磁場序貫修正的姿態解算方法，其步驟包括如下：(1)根據航姿系統中陀螺儀采集的角速率計算系統模型的狀態轉移矩陣，并利用離散化的四元數微分方程更新載體姿態四元數；(2)根據航姿系統中加速度計采集的載體重力加速度對步驟(1)中更新得到的姿態四元數由均方誤差最小原則進行修正；(3)根據航姿系統中磁力計采集的載體地磁場矢量對步驟(2)中修正得到的姿態四元數由均方誤差最小原則進行第二次修正。本發明利用地球物理場信息對更新得到的姿態四元數進行了兩次序貫最優估計，在該算法中，不需向系統提供先驗地磁場信息，只需提供先驗重力加速度矢量，克服了上述地磁場在應用中的不足。

技術領域

本發明屬于定位導航技術領域，具體涉及一種基于地球重力場與地磁場序貫修正的姿態解算方法。

技術背景

航姿參考系統(Attitude and Heading Reference System，AHRS)中集成了MEMS傳感器(陀螺儀、加速度計、磁力計)，其有成本低、集成度高以及體積小等優點，但是在進行姿態解算時，卻存在輸出噪聲過大、零點漂移無法完全消除、角速率輸出易受干擾等所導致的姿態解算精度低、累計誤差大等問題。因此，通常情況下，需要采用多個傳感器經過信息融合算法實現高精度的姿態解算。相較于利用歐拉角進行姿態解算，四元數作為一種非常有用的數學工具，由于其參數少且避免了歐拉角產生的奇異，在現代姿態解算應用中，得到了廣泛的應用。地磁場作為地球固有的物理特征，在導航中的應用越來越廣泛，但是地磁場易受周圍建筑物及鋼性結構的影響，在地球表面的不同位置，地磁場矢量各不相同且未知，大大限制的地磁場在導航中的應用。

發明內容

為解決以上技術問題本發明提供一種基于地球重力場與地磁場序貫修正的姿態解算方法。

本發明的技術方案是：一種基于地球重力場與地磁場序貫修正的姿態解算方法，包括如下三個步驟：

(1)根據航姿系統(AHRS)中MEMS陀螺儀采集的角速率計算系統模型的狀態轉移矩陣，并利用離散化的四元數微分方程更新載體姿態四元數；

(2)根據航姿系統(AHRS)中MEMS加速度計采集的載體重力加速度對步驟(1)中更新得到的姿態四元數由均方誤差最小原則進行修正；

(3)根據航姿系統(AHRS)中MEMS磁力計采集的載體地磁場矢量對步驟(2)中修正得到的姿態四元數由均方誤差最小原則進行第二次修正。

詳細地，步驟(1)具體包括：

a.建立陀螺儀動力學模型：

即

其中，X＝[X₀ X₁ X₂ X₃]^T表示全局坐標系G到載體坐標系B的旋轉四元數，表示在載體坐標系中測量的MEMS陀螺角速度信息，且

利用四元數微分方程的比卡求解法，使上式離散化可得

其中，X_k-1為t_k-1時刻全局坐標系G到載體坐標系B的旋轉四元數，X_k為t_k時刻全局坐標系G到載體坐標系B的旋轉四元數，

且

b.獲得載體坐標系B下的載體角速度信息

c.根據所述載體角速度信息計算系統狀態轉移矩陣

c.載體姿態四元數狀態一步預測