[發明專利]一種基于牛頓迭代法的炮彈姿態估計方法

有效

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

本發明公開一種基于牛頓迭代法的炮彈姿態估計方法，該方法包括以下步驟：(1)根據陀螺儀和GPS提供的炮彈速度和位置，計算t時刻載體慣性系i_b系相對于載體系b的炮彈姿態矩陣導航系n相對于導航慣性系i_n的炮彈姿態矩陣(2)由陀螺儀和加速度計測量值計算i_b系和i_n系下的炮彈速度v₁(t)和v₂(t)；(3)定義變量X＝[q^T u]^T，并構建非線性函數F(X)＝0；其中，q是4維列向量，代表i_n系至i_b系的變化四元數，u為待定系數；(4)通過求函數F(X)一階偏導數和二階偏導數,得到Jacobian矩陣和Hessian矩陣；(5)利用牛頓法迭代解出四元數q；根據計算出姿態角。

技術領域

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

背景技術

制導炮彈是指在傳統炮彈基礎上加裝制導系統和可供驅動的彈翼或尾艙等空氣動力裝置，以提高炮彈對目標打擊精度的一種低成本、小型化的精確制導武器。GPS/INS組合制導彈藥從平臺發射過程中，通常會承受高過載、高轉速的惡劣情況，在此高過載沖擊環境下陀螺儀、加速度計等彈上導航系統組件是無法正常上電工作的，所有彈上設備必須在經受此沖擊出管后上電工作，導航系統的初始化需要發射后在空中自主完成。且空中易受風力等氣象環境影響，彈體的姿態估算是后續導航系統工作的前提，也是當前的難點技術。彈體姿態探測的常用方法，主要包括采用地磁傳感器、GPS、慣性系統以及組合的姿態探測方法，根據載體繞質心運動的方程,利用陀螺測量角速度信息對姿態角進行估計等。

然而上述方法在實際使用時需要引入地磁傳感器，增大了成本，且一般的組合導航估計算法在高動態復雜的環境中效果不佳。利用陀螺測量角速度信息對滾轉角進行估計時將陀螺測量值近似為載體相對于導航系運動的角速度信息,在高速飛行下,這種近似將帶來很大的計算誤差，甚至不能滿足粗估計的要求。

發明內容

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

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

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

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

(3)定義變量X＝[q^T u]^T，并構建非線性函數F(X)＝0；其中，q是4維列向量，代表i_n系至i_b系的變化四元數，u為待定系數；

(4)通過函數F(X)一階偏導數和二階偏導數，求Jacobian矩陣和Hessian矩陣；

(5)利用牛頓法迭代解出四元數q，根據計算出姿態角。

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

設初始時刻t₀時，b系相對于i_b系的炮彈姿態矩陣為I為3×3的單位矩陣；

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