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

有效

• 申請號: 201811047725.0
• 申請日: 2018-09-07
• 公開（公告）號: CN109211231B
• 公開（公告）日: 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 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系的變化：

其中，為矩陣的變化率，“×”表示三維矢量對應的叉乘矩陣變換，設其中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)中，炮彈速度v₁(t)和v₂(t)計算方法如下：

其中，f^b(t)為t時刻炮彈上加速度計的輸出，表示炮彈三軸加速度，通過姿態矩陣將f^b(t)投影到i_b系中，得到

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

步驟(3)中，定義變量X＝[q^T u]^T，并構建非線性函數F(X)＝0，具體方法如下：

記四元數q＝[s η^T]^T，q^*＝[s -η^T]^T，s為q的標量部分，η為q的矢量部分，q^*表示q的共軛四元數，定義四元數q的變換矩陣如下：

其中，I為3×3單位矩陣，η×為η對應的叉乘矩陣；

將v₁(t)和v₂(t)擴充成零標量四元數，即定義V₁(t)＝[0 v₁(t)^T]^T，V₂(t)＝[0 v₂(t)^T]^T，定義W＝M(V₂(t))q-M'(V₁(t))q＝(M(V₂(t))-M'(V₁(t)))q，且對q進行模值約束qq^T＝1，引入拉格朗日乘子式，構造函數：

F(X)＝∑W^TW-u(q^Tq-1) (7)

令X＝[q^T u]^T；

步驟(4)中，通過Jacobian矩陣和Hessian矩陣，求函數F(X)一階偏導數和二階偏導數，具體方法如下：

其中，V＝M(V₂(t))-M'(V₁(t)),I_4×4為四階單位矩陣；

則Jacobian矩陣J和Hessian矩陣H可記為：

步驟(4)中，利用牛頓法迭代，解出四元數q，具體方法如下：

起始時，取X₀＝[1 0 0 0 0]^T，令k＝0,2,3,...,m-1，每次迭代時計算J與H；

X_k+1＝X_k-H^-1J (9)

由式(9)可不斷遞推X_k，直到所有數據全部解算完畢，從最終得到的X_k中取前4個元素組成q，即為i_n系至i_b系的變化四元數，記q＝[q₀ q₁ q₂ q₃]^T,q₀,q₁,q₂,q₃為q的四個元素；

步驟(5)中，計算方法如下：

計算

由下式得到

其中，為t時刻n系相對b系的姿態矩陣；為t時刻i_b系相對b系的姿態矩陣、為t時刻n系相對i_n系的姿態矩陣。

2.根據權利要求1所述的一種基于牛頓迭代法的炮彈姿態估計方法，其特征在于，步驟(5)中，t時刻炮彈姿態角計算方法如下：矩陣為3階方陣，其中各元素記為：

則t時刻炮彈的姿態角由下式解出：

φ，θ，γ分別是炮彈的航向角，縱搖角，橫滾角。