[發明專利]一種基于牛頓迭代法的炮彈姿態和加速度計常值誤差估計方法

有效

• 申請號: 201811041674.0
• 申請日: 2018-09-07
• 公開（公告）號: CN109211230B
• 公開（公告）日: 2022-02-15
• 發明（設計）人: 劉錫祥;楊文強;許廣富;郭小樂;馬曉爽
• 申請（專利權）人: 東南大學
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01S19/47
• 代理公司: 南京經緯專利商標代理有限公司 32200
• 代理人: 許方

1.一種基于牛頓迭代法的炮彈姿態和加速度計常值誤差方法，其特征在于，該方法包括以下步驟：

(1)根據陀螺儀和GPS提供的炮彈速度和位置，計算t時刻載體慣性系i_b系相對于載體系b的炮彈姿態矩陣導航系n相對于導航慣性系i_n的炮彈姿態矩陣

(2)由陀螺儀和加速度計測量值計算i_b系和i_n系下的炮彈速度v₁(t)和v₂(t)；

(3)定義變量X＝[q^T b_a^T u]^T，并構建非線性函數F(X)＝0，其中，q為i_n系至i_b系的變化四元數，b_a＝[0 Ba^T]^T，Ba為炮彈上三軸加速度計的常值誤差，u為待定系數；

(4)根據v₁(t)和v₂(t)，計算F(X)對應的Jacobian矩陣和Hessian矩陣，求函數F(X)一階偏導數和二階偏導數；

(5)利用牛頓法迭代法并根據計算對應的姿態角四元數q與加速度計常值誤差，為t時刻n系相對b系的姿態矩陣，為i_n系相對于i_b系的姿態矩陣；

步驟(1)中，炮彈姿態矩陣和計算方法如下：

設初始時刻t₀時，b系與i_b系間姿態矩陣為I為3×3的單位矩陣；

記t時刻陀螺儀的輸出值為即t時刻的b系相對于i_b系的角速度在b系上的投影值，由此跟蹤b系相對于i_b系的變化：

其中，為矩陣的變化率，“×”表示三維矢量對應的叉乘矩陣變換，設其中a，b，c分別表示炮彈沿三軸的旋轉角速度，則采用畢卡法解式(1)微分方程可得到式(2)：

記待解算數據時長為T，將時間段0～T以采樣周期dt為間隔劃分為多個時刻點t₀,t₁,t₂...t_m，k＝0,1,2,...,m，則式(2)中為t_k時刻的b系相對于t_k-1時刻的b系的姿態矩陣；為t_k-1時刻陀螺儀輸出，dt為采樣周期，最終

由GPS輸出的炮彈位置信息緯度L，東向、北向、天向速度分別為V_E,V_N,V_U，則n系相對i_n系的角速度可計算如下：

其中，R_N為地球子午圈曲率半徑，ω_ie為地球自轉角速度，R_E為地球卯酉圈半徑，根據公式(2)的計算方法，由可計算出

其中，為t_k時刻的n系相對于t_k-1時刻的n系的姿態矩陣；為t＝t_k-1時的值，dt為采樣周期，則

2.根據權利要求1所述的一種基于牛頓迭代法的炮彈姿態和加速度計常值誤差方法，其特征在于，步驟(2)中，炮彈速度v₁(t)和v₂(t)計算方法如下：

取t＝t_k，k＝12,3,...,m，f^b為炮彈加速度計輸出；將記為α₀，記為χ₀；

其中，V_n(t)為t時刻的n系炮彈速度，V_n(0)為起始時刻的n系炮彈速度，gⁿ＝[0 0 g]^T，g為重力加速度，ω_ie為地球自轉角速度。