[發明專利]基于分布式循環結構的飛行器自主姿態估計方法

有效

• 申請號: 202110261113.7
• 申請日: 2021-03-10
• 公開（公告）號: CN113029127B
• 公開（公告）日: 2023-05-09
• 發明（設計）人: 黃婧麗;趙育良;白玉山;欒曉菲;張翔宇;張玉葉;黎志強
• 申請（專利權）人: 中國人民解放軍海軍航空大學
• 主分類號: G01C21/00
• 分類號: G01C21/00;G01C21/16;G01C21/08;G01C21/20
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.基于分布式循環結構的飛行器自主姿態估計方法，其特征在于，包括以下步驟：

步驟一：利用離散四元數動力學方程建立飛行器姿態估計系統狀態方程和三軸磁強計測量模型建立姿態估計系統觀測方程，從而完整構建飛行器姿態估計系統，并求解該姿態估計系統，從而得到當前時刻的姿態四元數、姿態轉換矩陣、三軸姿態，利用當前時刻姿態矩陣將當地地磁場矢量理論值轉化到載體坐標系下，將轉化到載體坐標系下的當地地磁場矢量值與校正后的三軸磁強計測量值做差，得到地磁場殘差向量；地磁場殘差向量Δy_k為當地地磁場向量在機體坐標系下的理論值與經過誤差校正的三軸磁強計測量值之差；其中，為濾波后的四元數最優估計值，H_k為地理坐標系下磁場向量值，A(·)為從地理坐標系到載體坐標系的姿態轉換矩陣，B_k,m為磁強計在k時刻的測量值，d_k為零偏，s_k為尺度因數；

步驟二：利用加入測量噪聲的殘差向量構建三軸磁強計校正系統觀測方程，以及由尺度因素誤差和零偏兩個誤差參數作為系統狀態構建的三軸磁強計校正系統狀態方程，由此構建完整的三軸磁強計校正系統，并利用UKF濾波器估計該系統當前時刻的狀態，也即誤差參數；

步驟三：利用估計出的當前時刻的誤差參數校正三軸磁強計測量值，并將校正好的測量值代入飛行器姿態估計系統，重新估計三軸姿態；

步驟二中三軸磁強計校正系統觀測方程構建方法為：

令n為一個循環中積累的量測值數量，那么將觀測方程寫為

z_i＝Δy_i+r_i,i＝1,…,n

其中，zi是在一個循環內積累的第i個時刻的誤差，Δyi為地磁場殘差向量，ri是相應的噪聲；

步驟二中三軸磁強計校正系統狀態方程構建方法為：

其中，s_i為尺度因素，d_i為零偏，b_i是一個過程噪聲項，用來表示尺度因素和零偏的變化量；

尺度因數s_i和零偏d_i這兩個估計量用校正過的地面測試數據初始化，并且在載體運行階段通過傳感器校正過程進行修正，最終得到當時時刻的最優估計值

對三軸磁強計的測量值進行校正，方法如下：

得到校正后的三軸磁強計測量值B′_k,m，將該值代入姿態估計系統循環，估計飛行器姿態。

2.根據權利要求1所述的基于分布式循環結構的飛行器自主姿態估計方法，其特征在于，步驟一中姿態估計系統觀測方程構建方法為：

其中，為當前k時刻的姿態四元數一步預測值，A(·)為從地理坐標系到載體坐標系的姿態轉換矩陣，B_k為在載體坐標系下的三軸磁強計測量值，H_k為地理坐標系下磁場向量值，v_k為測量噪聲。

3.根據權利要求1所述的基于分布式循環結構的飛行器自主姿態估計方法，其特征在于，步驟一中飛行器姿態估計系統求解方法為：

將離散的飛行器姿態估計系統代入滾動時域估計模型，采用高斯-牛頓迭代法并結合MEMS速率陀螺和校正好的三軸磁強計測量值求解該滾動時域估計模型，從而得到當前時刻的姿態四元數、姿態轉換矩陣、三軸姿態。