基于理想彈道魯棒容積卡爾曼濾波視線角估計

劉振亞, 高 敏, 程 呈

(陸軍工程大學導彈工程系, 河北 石家莊 050003)

0 引 言

全捷聯導引頭相比于傳統平臺式導引頭具有體積小、質量輕、可靠性高[1]等特點,滿足精確打擊武器低成本、小型化的要求,被廣泛應用于現代化局部戰爭中。由于全捷聯導引頭將探測器與彈體固連,彈目視線角與彈體姿態角相互耦合,導致探測器不能直接輸出制導控制系統所需的彈目視線角以及彈目視線角速率。通過建立坐標系轉換關系,可將探測器測量得到的體視線角解耦得到彈目視線角以及彈體姿態角,將慣性測量組件測量得到的彈體姿態角代入解耦方程中,即可得到彈目視線角。而導引頭探測元件受環境擾動影響大,導致測量得到的體視線角很可能偏離真實值。同時,慣性測量組件測量得到的彈體姿態角也具有一定的噪聲,使得直接解耦得到的彈目視線角誤差較大。

目前,國內外學者對于全捷聯彈目視線角信息估計問題已經研究的比較深入,針對系統具有非線性的特點,主要利用非線性卡爾曼濾波對彈目視線角進行估計。文獻[2]針對非線性目標運動模型,利用擴展卡爾曼濾波的方法對目標運動信息進行了估計;文獻[3]針對濾波器初始值不準確以及異常值的存在,提出了一種魯棒參數化范圍容積卡爾曼濾波(cubature Kalman filter,CKF)的純方位跟蹤方法;文獻[4]通過對擴展卡爾曼濾波、無跡卡爾曼濾波、中心差分卡爾曼濾波以及粒子濾波在目標跟蹤問題中的性能進行了分析,從而為在實際應用中如何選擇非線性濾波器提供了理論依據。文獻[5]分別利用微分網絡以及卡爾曼濾波對彈目視線角進行仿真,表明卡爾曼濾波具有更小的延遲與噪聲水平;文獻[6]為分析全捷聯圖像導引頭視線角速率精度影響因素,提出了視線角速率解耦算法,并根據誤差理論詳細分析了各誤差源的誤差靈敏度;文獻[7]以捷聯紅外導引頭的工程應用為研究背景,針對刻度尺誤差帶來的隔離度問題,提出一種基于多模型的隔離度在線補償方法。

在工程應用中,所建立系統與實際模型存在誤差,且噪聲的統計特性未知,造成估計結果誤差較大甚至發散。針對上述問題,許多學者對卡爾曼濾波器進行了改進。文獻[8]針對離散線性系統狀態方程以及量測方程具有不確定性問題,將估計誤差協方差有界作為條件,得到協方差最小時的增益矩陣;文獻[9]針對將模型最大不確定度作為條件濾波結果保守程度大的問題,利用帕累托效率權衡濾波器的濾波性能以及魯棒性能;文獻[10]針對具有模型不確定性的非線性系統,將非線性系統線性化,得到魯棒擴展卡爾曼濾波算法。文獻[11]針對具有多傳感器的模型不確定系統,將噪聲與參數不確定性統一轉化為噪聲不確定性,利用極大極小魯棒估值原理,提出一種魯棒協方差交叉融合穩態卡爾曼濾波器,文獻[12]通過對卡爾曼濾波器的實時性能評價,提出了一種基于非線性映射的自適應調節權值混合Kalman/H∞濾波器;文獻[13]為減小測量異常誤差對非線性目標跟蹤系統的影響,提出了一種基于廣義M估計的魯棒容積卡爾曼濾波(robust cubature Kalman filter,RCKF)算法。

上述彈目視線角估計方法都需要慣性元件對彈體運動參數進行測量,但目前的測量元件成本高,抗高過載能力低,精度受環境影響較大,因此其在低成本全捷聯制導彈藥中的應用難度極大。本文針對全捷聯制導彈藥飛行彈道基本固定的特點,將理想彈道參數視為帶不確定性的模型參數,提出了一種基于理想彈道的魯棒容積卡爾曼濾波(ideal trajectory robust cubature Kalman filter,ITRCKF)的彈目視線角估計算法。

1 坐標系定義及轉換

(1) 如圖1(a)所示,O-xgygzg為基準坐標系,坐標系原點O為彈體質心位置,Oxg軸沿水平線指向射擊方向,Oyg軸鉛直向上,Ozg根據右手法則垂直于Oxgyg平面指向右方。

(3) 如圖1(a)所示,O-xqyqzq為視線坐標系,坐標原點O為彈體質心位置,Oxq軸與彈目連線重合并指向目標,Ozq軸在基準系Oxgzg平面內且與Oxq軸垂直,Oyq軸按照右手法則與Oxqzq平面垂直向上。Oxq軸與水平面夾角為彈目視線傾角qγ,其水平面投影與基準坐標系Oxg軸夾角為彈目視線偏角qλ。

(5) 如圖1(b)所示,O-xlylzl為體視線(body line-of-sight,BLOS)坐標系,坐標原點O為彈體質心位置,Oxl軸與彈目視線重合指向目標,Ozl軸在彈體坐標系Oxbzb平面內且與Oxl軸垂直,Oyl根據右手法則與Oxlzl平面垂直向上。Oxl軸與彈體坐標系Oxbzb平面夾角為體視線傾角qα,其在Oxbzb平面的投影與彈體坐標系Oxb軸的夾角為體視線偏角qβ。

圖1 坐標系定義Fig.1 Definition of coordinate

坐標系之間的關系可由各坐標系之間的夾角確定,利用坐標系連續旋轉的方法即可得到各坐標系之間的轉換關系以及轉換矩陣。圖2為上述5種坐標系之間的轉換關系,其中L(·)為各坐標系之間的轉換矩陣。

圖2 坐標系轉換關系Fig.2 Relationships of coordinate systems

2 濾波模型建立

2.1 彈目相對運動模型

設彈目相對運動矢量r,彈體速度矢量vm,目標運動矢量為vt,其變化規律可表示為

(1)

根據制導彈藥目標為固定點可得:vt=0;將彈體速度矢量vm投影到視線坐標系Oxq、Oyq與Ozq三軸,根據彈目相對運動角速度與線速度的關系,可得彈目相對運動速度表達式為

(2)

式中,vx、vy與vz為彈體速度在視線坐標系下的投影。

已知彈體速度在彈道坐標系下的坐標為(v,0,0),為求得彈體速度矢量在視線坐標系下的投影,根據圖2中坐標系間的轉換關系,可得到基準系下彈體以及目標的速度矢量在視線坐標系下坐標:

(3)

將坐標系轉換矩陣代到式(3)中,結合式(2),最終可得到彈目相對運動速度的表達式為

(4)

2.2 濾波器觀測模型

(5)

最終化簡得到濾波器觀測模型為

(6)

式中,Rij為基準坐標系至彈體坐標系轉換矩陣中的元素；v1和v2為導引頭量測噪聲。因此,濾波模型由狀態方程(4)和量測模型(6)組成,其中狀態變量為彈目相對運動信息,量測值為探測器測量得到的體視線角。

3 魯棒容積卡爾曼濾波

3.1 非線性系統線性化

建立具有不確定參數的離散非線性系統:

(7)

(8)

利用泰勒公式將非線性系統狀態方程以及量測方程在濾波器狀態估計點處展開,得到的線性系統:

(9)

3.2 非線性不確定參數線性化

由式(9)可以看出,利用泰勒公式展開級數的一階項系數為關于不確定參數的非線性函數。現將狀態方程一階項不確定系數表示為

(10)

將該系數利用泰勒公式展開可得

(11)

(12)

式(12)右邊項中的不確定度完全由非線性系統中參數不確定度ΔAk決定,因此可設定

高中物理實驗教學中通過合作式的學習方法幫助學生培養合作思維能力.在一個物理實驗過程中往往有許多實驗步驟需要幾人合作完成，通過合作式的學習方法讓學生體會到合作的重要性，在今后的學習和工作過程中能夠明白通過與他人合作解決問題或是實現目標.高中物理教育中物理實驗的合作式學習方法能夠有效地培養學生的合作思維能力.

(13)

同理,設定量測方程一階不確定系數為

(14)

根據式(9)～式(14),最終得到帶不確定參數線性化系統表達式:

(15)

3.3 誤差協方差矩陣

(16)

利用CKF算法[14]可得系統狀態的一步預測估計值為

(17)

將狀態變量線性化表達式(9)以及CKF得到一步預測狀態量式(17)代入一步預測誤差表達式,可得

(18)

根據狀態預測誤差協方差定義,可得一步預測誤差協方差表達式為

Pk+1|k=(F1k+M1kΓ1kN1k)Pk|k(F1k+M1kΓ1kN1k)T+Q

(19)

系統觀測估計值為

(20)

將系統觀測值代入至輸出預測誤差表達式(16)中,可得

(21)

輸出預測誤差協方差為

Py,k+1=

(F2k+M2kΓ2kN2k)Pk+1|k(F2k+M2kΓ2kN2k)T+R

(22)

設定狀態變量量測更新表達式為

(23)

易得狀態誤差協方差表達式

(24)

(25)

(26)

3.4 RCKF的設計

引理1[15]存在具有合適維度的矩陣A,B,C和D,并且滿足CCT≤I。若任意給定一個實正定矩陣以及一個正常數κ,并且滿足κI-DUDT>0;那么有式(27)成立

(A+BCD)U(A+BCD)T≤

A(U-1-κ-1DTD)-1AT+κBBT

(27)

根據引理1對式(19)及式(22)進行變換,得到帶不確定參數的一步預測誤差協方差以及輸出預測誤差協方差的上界Σk+1|k和Σy,k+1，其表達式為

(28)

(29)

當一步預測協方差滿足Pk+1|k≤Σk+1|k,則狀態誤差協方差滿足Pk+1≤Σk+1[16]。因此,只需選擇合適的增益矩陣將誤差協方差矩陣上界的跡最小化,即將式中的P用Σ代替,就可得到參數不確定系統狀態估計的誤差協方差最小上界。

(30)

(31)

根據式(30)與式(31),利用球面徑向準則計算容積點Xi與Yi,最終可得

(32)

(33)

(34)

將上述協方差矩陣上界代入第3.3節的結論中,可以得到RCKF算法:

(1) RCKF初始化

(35)

(2) 計算容積點

(36)

(37)

式中,n表示狀態變量的個數;[1]i表示一個全對陣點集。例如,當n=2時,全對陣點集可表示為

(3) 容積點傳播

Xi,k+1/k=f(Xi,k/k,uk)

(38)

(4) 狀態預測以及誤差協方差

(39)

計算容積點

(40)

(5) 容積點傳播

Yi,k+1/k=g(Xi,k+1/k,uk)

(41)

(6) 量測值估計、量測協方差以及狀態量測交叉協方差

(42)

(7) 濾波更新方程

(43)

3.5 不確定性參數κ的選擇

引理1中用含不確定性參數γ的協方差矩陣表示系統含參數不確定性的狀態誤差協方差矩陣的上界。因此,選擇合適的不確定參數對于濾波性能的好壞以及濾波穩定性能有著至關重要的作用。

在RCKF算法中,式(30)與式(31)限制了κ的下限,κ必須大于矩陣N-1Σ(NT)-1的最大特征值,一般矩陣N取單位陣,即:

κ>max(eig(Σ))

(44)

(1) 當κ?max(eig(Σ))時,滿足算法中的條件。但這時(Σ-1-κ-1NTN)-1近似不變,而κMMT趨于無限大,這時系統誤差協方差矩陣對于系統的不確定性沒有了限制,該算法失效。

(2) 當κ=max(eig(Σ))+δ時,其中δ為任意小量,這時max((eig(Σ-1-κ-1NTN)-1))遠大于max((eigΣ-1)),隨著迭代步驟的增加,誤差協方差矩陣趨于發散,并且發散速率隨著δ值的減小逐漸加快。

(3) 當系統中n個狀態變量的協方差矩陣之間數量級相差較大,而κ需要滿足條件κ>max(eig(Σ)),由不等式(28)可知,較大的κ為數量級較小的狀態變量協方差定義了較大的上界,從而使算法對于協方差數量級較小的狀態變量估計值具有較大的保守性,導致在濾波結果中,某些元素的精度比其他元素要高。

4 基于理想彈道參數的彈目視線角估計算法

針對制導彈藥彈體運動彈道基本固定的特點,可將理想彈道的彈體運動參數作為帶有不確定性的實際飛行運動參數,利用RCKF算法對彈目視線角進行估計,可將導引頭內的慣性測量組件減少或去掉,進一步降低了全捷聯制導彈藥的成本,提高其可靠性能。

4.1 實際運動參數誤差分析

由式(13)可知,非線性濾波系統式(4)和式(6)不確定度僅與系統中參數不確定度有關。因此,當實際彈體運動參數與理想彈道參數誤差越小,濾波系統不確定度對于濾波結果的影響越小。現以初始射角為85°對某型迫擊炮彈進行1 000次蒙特卡羅模擬打靶實驗,分析實際彈道參數相對于理想彈道參數誤差的大小。需要注意的是,由于僅對彈目視線角進行估計,因此可將彈目相對距離作為系統參數。

通過對實驗結果的擬合,得到除滾轉角外的彈體運動參數服從以理想彈體運動參數為均值的正態分布,其標準差變化曲線如圖3所示。

圖3 彈體運動參數誤差標準差曲線Fig.3 Variance of projectile motion parameter error

4.2 基于理想彈道參數的濾波器設計

根據式(4)及式(6),可分別得到非線性系統系統狀態方程以及量測方程對于狀態變量的雅克比矩陣

(45)

式中,f1和f2分別表示為系統狀態方程中關于彈目視線角的方程;g1和g2分別表示系統量測方程中的兩個方程。

可將矩陣U和V看作線性化系統系數,可得狀態方程與量測方程線性化系數對于系統不確定參數(即彈體運動參數)的雅克比矩陣

(46)

根據式(12),可得濾波系統不確定度

(47)

式中,M1Γ1N1為狀態方程不確定度;M2Γ2N2為量測方程不確定度;Δξ和Δη為實際彈道參數與理想彈道參數之間的偏差值。在設計濾波器時,Δξ和Δη取利用蒙特卡羅仿真的方法計算每條實際彈道與理想彈道在對應時刻誤差的標準差,作為系統不確定度的上界。

根據所建立的濾波器模型,最終得到基于理想彈道參數的彈目視線角濾波估計算法。為區分滾轉角,將不確定參數設定為κ,其算法過程如圖4所示。

圖4 彈目視線角估計算法流程圖Fig.4 Estimation algorithm flow of LOS angle

5 實驗驗證

5.1 數字仿真實驗

為驗證彈目視線角估計精度以及算法的性能,首先構建圖5所示的仿真驗證模型。其中,彈體六自由度運動模型根據某型迫擊炮彈體氣動參數以及實際打靶測得的射擊環境構建;彈目相對運動模型以及導引頭模型分別由系統狀態方程(4)與量測方程(6)建立;目標模型設定為目標點位置(1 000,0,0),濾波估計模型由RCKF算法構建,其初始值為導引頭開始工作時刻的理想彈道彈目視線角。在滿足不等式(44)的條件下,設定魯棒濾波器中不確定參數為

κ=100max(eig(Σ))

(48)

圖5 仿真實驗模型示意圖Fig.5 Simulation model sketch

彈道參數不確定性主要由彈體質量、轉動慣量、氣動參數、發射角偏差、初始速度偏差以及風速風向擾動因素造成。由于彈體質量、轉動慣量以及氣動參數不確定范圍較小,因此在本次實驗中忽略這三種擾動因素,分別設定小擾動條件與大擾動條件如表1所示。

表1 實驗條件

其中Δv表示初始速度偏差,Δangle表示射角偏差,ΔAwind表示風向擾動偏差,Δwind表示風速系數(即風速擾動=發射環境測量得到的風速×風速系數)。將某型迫彈作為實驗對象,其在彈道末段的速度約為200 m/s,而激光導引頭的探測距離約為2 km,因此設定濾波器工作時間為10 s,設定初始射角為85°,分別在上述小擾動以及大擾動條件下進行數字仿真實驗。

5.2 實驗結果分析

實驗結果如圖6、圖7及表2所示,僅利用理想彈道參數與CKF對彈目視線角進行濾波估計的實驗結果,將理想彈道參數作為濾波器系數對彈目視線角進行估計的結果誤差并沒有很大,主要是由于制導彈藥彈道基本固定,并且參數不確定性較小,在彈道末端,無論系統參數是否確定,其彈目視線傾角基本保持在接近于-90°的范圍內并最終迅速趨于0°。因此隨著時間變化其誤差逐漸趨于零;而彈目視線偏角在彈道末端隨著彈目相對距離的減小,其值逐漸增大并逐漸趨于落點與目標點的夾角,因此隨著彈目相對距離的接近,理想彈道偏角與實際彈道偏角的誤差越來越大。

當彈體受到小擾動的實驗結果如圖6所示,對比其中僅依靠CKF的彈目視線偏角估計值曲線與ITRCKF彈目視線估計值曲線,可以看到:ITRCKF對于彈目視線傾角估計效果相對于CKF并不是很明顯,其主要原因是在小擾動條件下彈目視線傾角在彈道末端的變化范圍小,不確定性對其影響較小;而ITRCKF對于彈目視線偏角估計值有著較好的抗系統不確定性,由表2中結果可知,ITRCKF濾波估計最大誤差值較CKF下降了85.57%,誤差均方根下降了81.93%。

圖6 小擾動條件實驗結果Fig.6 Experiment result in small disturbance

圖7 大擾動實驗結果Fig.7 Experiment result in large disturbance

條件CKF最大誤差/(°)RCKF最大誤差/(°)CKF誤差均方根/(°)RCKF誤差均方根/(°)小擾動qγ0.260.230.180.17大擾動qγ3.782.582.331.25小擾動qλ2.910.421.660.30大擾動qλ2.342.961.422.67

對比在大擾動條件下ITRCKF對于彈目視線傾角的估計結果,可以看到在大擾動的條件下ITRCKF具有更好的濾波結果,其平均誤差較CKF下降了31.64%,誤差均方根下降了46.39%;而對于視線偏角的估計效果并不明顯,主要是由于為保證視線傾角估計精度,濾波系統取較大的κ值,使視線偏角估計具有較大的保守性,其濾波結果沒有CKF效果好。

綜上所述,ITRCKF在對于彈目視線角估計時,對于受擾動影響較大的角度有著明顯的魯棒性能,同時也為另一角度帶來了保守性;但相對于CKF濾波結果,其保守性對于濾波精度影響較小。因此,利用ITRCKF能夠得到精確的彈目視線角,并且具有較好的魯棒性能。

6 結 論

本文提出了一種基于理想彈道參數的彈目視線角估計方法,利用理想彈道參數替換慣性元件測量值,減小了導引頭體積,降低了導引頭成本。針對所建立非線性濾波系統參數具有不確定性,提出了帶不確定參數的RCKF方法,將帶不確定性系統狀態估計問題轉化為求解帶參數κ的狀態變量誤差協方差矩陣上界的最小值問題,作為最終得到具有一定精度的彈目視線角,利用數字仿真證明了該算法的穩定性與可行性,并分析了該算法對于彈目視線角抗系統不確定性的有效性與保守性,為低成本全捷聯制導彈藥制導信息估計提供了理論依據。

[1] JAMES M M. Line of sight rate estimation for guided projectiles with strapdown seekers[J].AIAA Guidance, Navigation and Control Conference, 2015，32(6):2346-2351.

[2] VADDI S S, MENON P K. Target state estimation for integrated guidance-control of missiles[C]∥Proc.of the AIAA Guidance, Navigation and Control Conference and Exhibit, 2007: 1-22.

[3] WU H, CHEN S X, YANG B F, et al. Robust range-parameterized cubature Kalman filter for bearings-only tracking[J]. Journal of Central South University, 2016, 23 (6): 1399-1405.

[4] KATKURI J R, JILKOV V P, Li X R. A comparative study of nonlinear filters for target tracking in mixed coordinates[C]∥Proc of the 42nd South Eastern Symposium on System Theory, 2010: 202-207.

[5] 王偉, 林德福, 徐萍. 捷聯導引頭彈目視線角速率估計[J]. 紅外與激光工程, 2015, 44 (10): 3066-3069.

WANG W,LIN D F,XU P.Strap-down seeker LOS angular rate estimation[J].Infrared and Laser Engineering,2015,44(10):3066-3069.

[6] 孫婷婷, 儲海榮, 郭立紅, 等. 全捷聯圖像導引頭視線角速率精度分析[J]. 儀器與儀表學報, 2015, 36(7): 1479-1487.

SUN T T, CHU H R, GUO L H, et al. Line-of-sight angular rate accuracy analysis of strapdown imaging seeker[J]. Chinese Journal of Scientific Instrument, 2015, 36(1): 1479-1487.

[7] 宗睿,林德福,王偉,等.基于多模型的捷聯紅外導引頭隔離度在線補償方法[J].系統工程與電子技術,2017,39(3):147-155.

ZONG R, LIN D F, WANG W, et al. Multiple model based online compensation for disturbance rejection rate of strapdown infrared seeker[J]. Systems Engineering and Electronics, 2017, 39(1),147-155.

[8] SHADY M K M, SAEID N. Robust finite-horizon Kalman filter for uncertain discrete-time systems with uncertain observations[J]. IEEE Trans.on Automatic Control, 2012, 57 (6):1548-1552.

[9] XU H, MANNOR S. A Kalman filter design based on the performance/robustness tradeoff[J]. IEEE Trans.on Automatic Control, 2009, 54 (5): 1171-1175.

[10] XIONG K, WEI C L, LIU L D. Rubust Kalman filter for discrete-time nonlinear systems with parameter uncertainties[J]. Aerospace Science and Technology, 2012, 18: 15-24.

[11] 王雪梅,劉文強,鄧自立.不確定系統魯棒交叉融合穩態Kalman濾波器[J]. 電子與信息學報, 2015, 37(8): 1900-1905.

WANG X M, LIU W Q, DENG Z L. Robust covariance intersection fusion steady-state Kalman filter for uncertain systems[J]. Journal of Electronics & Information Technology,2015,37(8): 1900-1905.

[12] 張勇剛, 黃玉龍, 李寧, 等. 基于非線性映射的自適應混合Kalman/H∞濾波器[J]. 系統工程與電子技術, 2013, 35(9): 1948-1953.

ZHANG Y G, HUANG Y L, LI N, et al. Adaptive hybrid Kalman/H∞ filter based on nonlinear mapping[J]. Systems Engineering and Electronics, 2013, 35(9): 1948-1953.

[13] 吳昊,陳樹新,楊賓峰,等.基于廣義M估計的魯棒容積卡爾曼濾波目標跟蹤算法[J]. 物理學報, 2015, 64(21): 1-7.

WU H, CHEN S X, YANG B F, et al. Robust cubature Kalman filter target tracking algorithmbased on genernalized M-estiamtion[J]. Acta Physica Sinica, 2015, 64(21): 1-7.

[14] ICCKARAN A, SIMON H. Cubature Kalman filters[J]. IEEE Trans.on Automatic Control, 2009, 54(6): 1254-1269.

[15] XIE L, SSOH Y C, DE S. Robust Kalman filter for uncertain discrete-time systems[J]. IEEE Trans.on Automatic Control, 1994, 39(6):1310-1314.

[16] THEODOR Y, SHAKED U. Robust discrete-time minmum-variance filtering[J].IEEE Trans.on Signal Process,1996,44(2): 181-189.