[發明專利]一種在地磁異常情況下應用AHRS測量姿態的方法

有效

• 申請號: 201711082420.9
• 申請日: 2017-11-07
• 公開（公告）號: CN107860382B
• 公開（公告）日: 2021-04-06
• 發明（設計）人: 孫鋒;唐國峰;季和平;鄭玲玲;原杰;朱亞東洋;陳祖斌;張彥峰
• 申請（專利權）人: 吉林大學
• 主分類號: G01C21/00
• 分類號: G01C21/00;G01C21/16;G01C1/00;G01C25/00
• 代理公司: 長春吉大專利代理有限責任公司 22201
• 代理人: 張巖;王立文

1.一種在地磁異常情況下應用AHRS測量姿態的方法，其特征在于，包括以下步驟：

A、基于無跡卡爾曼濾波算法的磁力計異常矯正：

A1、磁異常的判斷：通過對三軸磁力計的輸出值求出總磁通量和磁傾角，當其偏離當地磁通量和磁傾角時則此時刻磁力計處于磁異常狀態；

A2、為卡爾曼濾波選定狀態量其含義為方位角偏離值和磁力計異常值；

A3、利用狀態量的運動關系推導出狀態方程，并求出狀態矩陣A和狀態估計值X_t；

A4、通過對方向向量開微分方程的改進，在其原來旋轉矩陣的基礎上，推導出連續時刻磁力計輸出的關系，進而推導出測量方程，并求解出測量矩陣Z和測量估計值z_t，測量方程：Z_t＝Z·X_t+r_t，式中Z為測量矩陣；z_t為測量值，其為t時刻磁力計的輸出值；r_t為測量噪聲；當磁力計有異常情況時表述如下：

其中M_(t)表示t時刻磁力計的輸出值，表示t-1時刻磁力計修正后的輸出值；

A5、基于上述狀態方程和測量方程，通過無跡卡爾曼濾波算法對磁力計輸出的異常值進行修正；

A6、通過迭代算法用上一時刻磁力計的輸出值連續校準下一時刻磁力計的輸出值使每一個異常值多次參加卡爾曼濾波過程，消除極端異常值對整個過程的影響；

B、求解方位角方法：

B1、根據四元數的微分方程推算出姿態運動學方程作為卡爾曼濾波狀態方程，代入磁力計修正值和加速度計值求出方位角測量值；

B2、通過擴展卡爾曼濾波融合AHRS數據求出姿態角。

2.根據權利要求1所述的一種在地磁異常情況下應用AHRS測量姿態的方法，其特征在于：步驟B2，所述AHRS包括磁力計、加速度計和陀螺儀。

3.根據權利要求1所述的一種在地磁異常情況下應用AHRS測量姿態的方法，其特征在于：步驟B2，所述姿態角包括航向角、俯仰角和橫滾角。