[發明專利]一種基于磁強計和加速度計的快速對準方法

有效

• 申請號: 201811327470.3
• 申請日: 2018-11-08
• 公開（公告）號: CN109631939B
• 公開（公告）日: 2020-07-28
• 發明（設計）人: 崔蒞杭;諶浩然
• 申請（專利權）人: 湖北三江航天紅峰控制有限公司
• 主分類號: G01C25/00
• 分類號: G01C25/00
• 代理公司: 武漢東喻專利代理事務所(普通合伙) 42224
• 代理人: 李佑宏

1.一種基于磁強計和加速度計的快速對準方法，其特征在于，具體步驟為：

S1.依據上一時刻的姿態矩陣以及三軸磁強計和三軸加速度計的測量值計算量測矩陣；

S2.依據上一時刻的姿態矩陣、當地地理坐標系下的地球磁場強度的反對稱矩陣和地球重力加速度的反對稱矩陣計算量測模型矩陣；

S3.當前計算次數t大于等于預設的計算次數閾值n時，采用迭代最小二乘估計并利用量測矩陣和量測模型矩陣計算旋轉矢量；否則采用單次最小二乘估計并利用量測矩陣和量測模型矩陣計算旋轉矢量；

S4.依據旋轉矢量計算旋轉矩陣和當前時刻的姿態矩陣；

S5.根據當前時刻的姿態矩陣解算出載體當前俯仰角、橫滾角及航向角，從而在確保對準精度的前提下實現基于磁強計和加速度計的低精度慣性導航系統的快速對準。

2.根據權利要求1所述的一種基于磁強計和加速度計的快速對準方法，其特征在于，當前計算次數t的計算方法為：初始時刻設置為t＝0，間隔一個預設的采樣周期計算次數t增加一次，從t＝1開始計算載體當前俯仰角、橫滾角及航向角信息。

3.根據權利要求2所述的一種基于磁強計和加速度計的快速對準方法，其特征在于，步驟S1具體為：

選擇東北天當地地理坐標系作為導航坐標系，載體坐標系三個軸系定義為載體右-前-上，旋轉順序為3-1-2；

間隔一個預設的采樣周期利用三軸磁強計和三軸加速度計采集當前時刻載體的磁強計信息M(t)和加速度計信息Acc(t)；

計算載體坐標系下的地球磁場強度投影M^b(t)：

計算載體坐標系下的地球重力加速度投影Acc^b(t)：

其中，b表示載體坐標系，n表示導航坐標系；其中為上一時刻姿態矩陣的轉置矩陣，表示當前時刻的姿態矩陣，初始值設置為Mⁿ為當地地球磁場強度；g為當地地球重力加速度；

依據磁強計信息M(t)、載體坐標系下的地球磁場強度投影M^b(t)、加速度計信息Acc(t)和載體坐標系下的地球重力加速度投影Acc^b(t)計算出量測矩陣Z(t)：

4.根據權利要求3所述的一種基于磁強計和加速度計的快速對準方法，其特征在于，步驟S2具體為：

當地地理坐標系下的地球磁場強度的反對稱矩陣Mⁿ(×)為：

當地地理坐標系下的地球重力加速度的反對稱矩陣Accⁿ(×)為：

依據上一時刻的姿態矩陣當地地理系下的地球磁場強度的反對稱矩陣Mⁿ(×)和地球重力加速度的反對稱矩陣Accⁿ(×)計算量測模型矩陣H(t)，

5.根據權利要求4所述的一種基于磁強計和加速度計的快速對準方法，其特征在于，步驟S3具體為：

tn時，采用單次最小二乘估計計算姿態旋轉矢量φ(t)為：

φ(t)＝[H^T(t)*H(t)]^-1*H^T(t)*Z(t)

其中，H^T(t)為量測模型矩陣H(t)的轉置矩陣；

當t≥n時，采用迭代最小二乘估計計算旋轉矢量φ(t)為：依據前一時刻的增益矩陣K(t-1)、姿態等效旋轉矢量φ(t-1)和估計誤差協方差陣P(t-1)依次計算當前時刻的增益矩陣K(t)、姿態等效旋轉矢量φ(t)和估計誤差協方差陣P(t)，表達式為：

K(t)＝P(t-1)*H^T(t)*[H(t)*P(t-1)*H^T(t)+R]^-1

φ(t)＝K(t)*[Z(t)-K(t)*H(t)]*P(t-1)

其中，P(t)初始值設置為R表示量測噪聲陣，其為對角線矩陣且其對角線的值分別為三軸磁強計的三個磁強計精度值和三軸加速度計的三個加速度計精度值。