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

有效

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

本發明公開一種基于牛頓迭代法的炮彈姿態和加速度計常值誤差估計方法，該方法包括以下步驟：(1)將初始時刻的載體系b和導航系n凝固為載體慣性系i_b和導航慣性系i_n，則t時刻的姿態矩陣可分解為由陀螺儀和加速度計測量值計算i_b系和i_n系下的速度v₁(t)和v₂(t)；(2)根據四元數相關性質，定義變量X＝[q^T b_a^T u]^T，并構建非線性函數F(X)＝0；(3)v₁(t)和v₂(t)，計算F(X)對應的Jacobian矩陣和Hessian矩陣，求函數F(X)一階偏導數和二階偏導數；(4)利用牛頓法迭代，解出對應的姿態角四元數q與加速度計常值誤差。

技術領域

本發明屬于導航技術領域，尤其涉及一種基于牛頓迭代法的炮彈姿態和加速度計常值誤差估計方法。

背景技術

制導炮彈是指在傳統炮彈基礎上加裝制導系統和可供驅動的彈翼或尾艙等空氣動力裝置，其上一般裝配有GPS/INS組合。炮彈的姿態估計是后續精確打擊的關鍵，但在制導彈藥從平臺發射過程中，由于環境惡劣，在高過載沖擊環境下陀螺儀、加速度計等彈上導航系統組件是無法正常上電工作的，導航系統的初始化需要發射后在空中自主完成。且空中易受風力等氣象環境影響，彈體的姿態估算也是當前的難點技術，提供儀表的常值誤差估計則對進一步提升對準精度有重要意義。

發明內容

發明目的：針對以上問題，本發明提出一種基于牛頓迭代法的炮彈姿態和加速度計常值誤差估計方法，該方法的目的在于僅利用陀螺儀，加速度計和GPS提供的信息，通過牛頓迭代法解出最優的炮彈姿態估計和加速度計常值誤差。

技術方案：為實現本發明的目的，本發明所采用的技術方案是：一種基于牛頓迭代法的炮彈姿態和加速度計常值誤差方法，該方法包括以下步驟：

(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對應的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系的變化：