[發明專利]基于擴維模型的容積卡爾曼濾波提取捷聯導引頭制導信息的方法

有效

• 申請號: 201710448401.7
• 申請日: 2017-06-14
• 公開（公告）號: CN107255924B
• 公開（公告）日: 2018-07-17
• 發明（設計）人: 韋常柱;浦甲倫;許河川;何曉夫;韓業鵬
• 申請（專利權）人: 哈爾濱工業大學
• 主分類號: G05B13/04
• 分類號: G05B13/04
• 代理公司: 哈爾濱市陽光惠遠知識產權代理有限公司 23211
• 代理人: 鄧宇

1.一種基于擴維模型的容積卡爾曼濾波提取捷聯導引頭制導信息的方法，其特征在于，它包括以下步驟：

步驟一：通過捷聯導引頭量測獲得彈體系下的體視線角；

步驟二：建立彈目相對運動方程，對彈目相對運動方程兩次求導獲得慣性視線角速度的二階導數與彈目相對加速度的關系方程；

步驟三：利用狀態變量建立狀態方程；

步驟四：根據彈體系到地面系的坐標轉換，推導由慣性視線角結合彈體姿態信息，得到彈體系下體視線角的轉換方程；并推導彈目距離變化率與彈載GPS輸出的高度變化率的關系方程；將以上兩方程結合建立觀測方程；

步驟五：應用五階容積卡爾曼濾波提取捷聯導引頭制導信息；

步驟二中彈目相對運動方程的獲得過程為：

根據導彈和目標的空間幾何關系和運動學關系建立以下關系式：

式中r為慣性系下導彈與目標的相對位置矢量，r_T為目標在慣性系下的位置矢量，r_M為導彈在慣性系下的位置矢量，V為導彈與目標在慣性系下的相對速度矢量，V_T為目標在慣性系下的速度矢量，V_M為導彈在慣性系下的速度矢量，a為慣性系下導彈與目標的相對加速度矢量，a_T為目標在慣性系下的加速度矢量，a_M為導彈在慣性系下的加速度矢量；

視線坐標系相對慣性坐標系的旋轉角速度ω_s為：

q_γ為慣性視線高低角，q_λ為慣性視線方位角，i_s為視線坐標系的x軸單位向量，j_s為視線坐標系的y軸單位向量，k_s為視線坐標系的z軸單位向量；

在視線坐標系中，

同理：

則：

基于彈目相對運動關系，得到彈目相對運動方程：

式中a_xs為導彈相對目標的加速度在視線坐標系的Ox_s軸上的分量，a_ys為導彈相對目標的加速度在視線坐標系的Oy_s軸上的分量，a_zs為導彈相對目標的加速度在視線坐標系的Oz_s軸上的分量；

導彈與目標的相對位置矢量r的估計方法如下：

式中，y_m為彈載GPS輸出的高度，y_t為目標點高程；

彈目距離變化率由式(2)兩邊對t求導獲得：

其中，彈載GPS輸出的高度變化率，為目標高度變化率，對地面目標近似為零；

步驟三中，利用狀態變量建立狀態方程的具體方法為：

取狀態量為聯立式(1)～(3)，建立擴維狀態方程：

式中x₁～x₆為狀態量x中第1～6個元素。

2.根據權利要求1所述的基于擴維模型的容積卡爾曼濾波提取捷聯導引頭制導信息的方法，其特征在于，

步驟四中建立觀測方程的具體方法為：

根據彈目距離變化率與彈載GPS輸出的高度變化率的關系，取觀測變量為導引頭量測的體高低視線角q_α、導引頭量測的體方位視線角q_β和彈載GPS輸出的高度變化率即建立觀測方程如下：

式中z₁～z₃為觀測變量z中第1～3個元素；

其中，上式中的R_bc為地面系到彈體系的轉換矩陣L(γ,θ,ψ)中第b行第c列的元素，θ為俯仰角，ψ為偏航角，γ為滾轉角，v₁為體高低視線角的噪聲信號，v₂為體方位視線角的噪聲信號，v₃為彈載GPS高度變化率的測量噪聲信號。

3.根據權利要求2所述的基于擴維模型的容積卡爾曼濾波提取捷聯導引頭制導信息的方法，其特征在于，步驟五中應用五階容積卡爾曼濾波提取捷聯導引頭制導信息的具體方法為：

(一)設定初始參數：

設定濾波初始時刻系統狀態值x₀，初始時刻狀態誤差協方差陣為P₀，系統噪聲協方差陣Q，量測噪聲協方差陣R；

(二)時間更新：

1)假設k-1時刻狀態量真實值為x_k-1，估計值為狀態誤差協方差陣為P_k-1，其后驗分布概率已知，對P_k-1進行Cholesky分解：

2)計算容積點：

其中，i＝1,2,…,n，且j＝1,2,…,n(n-1)/2，w₀為第一類容積點的權值，w₁為第二類容積點的權值，w₂為第三類容積點的權值；

其中，e_p為第p個元素為1的n維單位向量；為e_p的正中點集合的第j個向量，為e_p的負中點集合的第j個向量；p＝1,2,…,n；

3)通過狀態方程公式(4)傳播容積點：

計算k時刻狀態的預測值及k時刻狀態誤差協方差陣的預測值P_k|k-1：

(三)量測更新：

1)對P_k|k-1進行Cholesky分解：

2)計算容積點：

其中，i＝1,2,…,n，j＝1,2,…,n(n-1)/2；

3)通過觀測方程公式(5)計算容積點量測值

其中，i＝1,2,…,n，j＝1,2,…,n(n-1)/2；

計算k時刻量測的預測值

4)計算自相關協方差陣P_zz,k|k-1和互相關協方差陣P_xz,k|k-1：

5)濾波增益計算：

6)計算狀態估計值和狀態誤差協方差陣P_k：

的第二個變量即為提取得到的高低慣性視線角速度，第四個變量為提取得到的方位慣性視線角速度，由此獲得捷聯導引頭制導信息。