王小剛，胡智勇，于 洋，秦武韜

（1. 哈爾濱工業(yè)大學(xué) 航天學(xué)院，哈爾濱 150001；2. 空軍大連通信士官學(xué)校，大連 116600；3. 國營第六二四廠，哈爾濱 150030）

基于魯棒濾波的捷聯(lián)導(dǎo)引頭視線角速度估計方法

王小剛1，胡智勇2，于 洋3，秦武韜1

（1. 哈爾濱工業(yè)大學(xué) 航天學(xué)院，哈爾濱 150001；2. 空軍大連通信士官學(xué)校，大連 116600；3. 國營第六二四廠，哈爾濱 150030）

針對捷聯(lián)導(dǎo)引頭無法直接獲取視線角速度等信息的問題，研究了魯棒濾波在大氣層外飛行器捷聯(lián)導(dǎo)引頭視線角速度估計中的應(yīng)用。為了建立非線性濾波估計模型，考慮目標(biāo)視線角速度的慢變特性，采用一階馬爾科夫模型建立了狀態(tài)方程；推導(dǎo)了視線角速度的解耦模型，并建立了量測方程；考慮到實際應(yīng)用中存在系統(tǒng)噪聲統(tǒng)計特性失準(zhǔn)的問題，基于Huber-Based魯棒濾波方法，設(shè)計了視線角速度濾波器，并完成了基于 Huber-Based濾波方法和擴展卡爾曼濾波方法的數(shù)學(xué)仿真。仿真結(jié)果表明Huber-Based濾波方法的視線角、視線角速度及視線角加速度估計精度分別達到 0.1140′、0.1423′/s、0.0203′/s2，而擴展卡爾曼濾波方法的視線角、視線角速度及視線角加速度估計精度僅分別為0.6577′、0.6415′/s、0.0979′/s2。仿真結(jié)果證明了該方法可以有效地估計出相對視線角速度等信息，并且在非高斯噪聲的條件下，依然可獲得較高的估計精度，具有一定的魯棒性。

捷聯(lián)導(dǎo)引頭；視線角速度；一階馬爾科夫模型；非線性濾波方法；Huber-based濾波

為了應(yīng)對彈道導(dǎo)彈的威脅，美俄等軍事強國先后開展了導(dǎo)彈防御系統(tǒng)[1]（Missile Defense System, MDS）的相關(guān)技術(shù)研究。特別是美國已經(jīng)建立了一套多基、分層導(dǎo)彈防御系統(tǒng)，其核心組成部分之一就是大氣層外攔截器[2]，而高精度末制導(dǎo)律設(shè)計是保證其攔截精度的關(guān)鍵技術(shù)[3]。傳統(tǒng)的大氣層外攔截器末制導(dǎo)依賴于紅外導(dǎo)引頭，制導(dǎo)律采用比例導(dǎo)引法。然而，隨著捷聯(lián)導(dǎo)引頭的出現(xiàn)，成本低、重量輕等一系列優(yōu)勢使其逐漸成為軍事領(lǐng)域的研究熱點[4]。然而捷聯(lián)導(dǎo)引頭只能測量目標(biāo)的相對視線角，無法直接獲得視線角速率，導(dǎo)致比例導(dǎo)引等傳統(tǒng)制導(dǎo)律無法應(yīng)用。因而，捷聯(lián)導(dǎo)引頭的末制導(dǎo)律設(shè)計問題逐漸成為當(dāng)前的一個研究熱點問題。

為解決上述問題，研究人員提出了兩種技術(shù)途徑：一種是設(shè)計與捷聯(lián)導(dǎo)引頭測量特性相適應(yīng)，只利用視線角信息進行制導(dǎo)的捷聯(lián)制導(dǎo)算法。文獻[5]以經(jīng)典制導(dǎo)律為基礎(chǔ)，提出了多種捷聯(lián)律設(shè)計方案，并采用姿態(tài)角反饋自動駕駛儀和過載反饋自動駕駛儀構(gòu)造了捷聯(lián)制導(dǎo)系統(tǒng)，并進行了仿真驗證。然而，該研究以反輻射導(dǎo)彈為應(yīng)用背景，對于大氣層外飛行器的高速、大過載情況，其適應(yīng)性尚未得到驗證。另外一種技術(shù)途徑是基于捷聯(lián)導(dǎo)引頭的測角信息，提取目標(biāo)視線角速率，進而采用比例導(dǎo)引法完成制導(dǎo)任務(wù)。由于捷聯(lián)導(dǎo)引頭的測角信息中包含噪聲，采用直接微分的方法會放大噪聲，淹沒角速率信息[6]，因而目前常用的方法是非線性濾波估計法。文獻[7]提出了一種基于粒子濾波的捷聯(lián)導(dǎo)引頭視線角速率估計方法。文獻[8]設(shè)計了一種H∞平方根濾波器，估計出彈目相對運動等信息，可有效對付機動目標(biāo)。文獻[9]針對某型制導(dǎo)彈藥末制導(dǎo)問題，采用強跟蹤卡爾曼濾波方法提取相對運動信息，用于最優(yōu)導(dǎo)引律實現(xiàn)。綜上，在采用非線性濾波方法估計視線角速率信息時，其關(guān)鍵問題包含如下三個方面：一是非線性濾波算法的選取，要兼顧考慮估計精度和計算量兩個因素；二是視線角速度率信息重構(gòu)，即建立視線角與實現(xiàn)角速度率之間的關(guān)系，該問題的研究目前已相對成熟；三是狀態(tài)方程的建立，需要結(jié)合具體問題進行設(shè)計。文獻[7-9]的狀態(tài)方程基于彈目相對運動關(guān)系，假設(shè)目標(biāo)運動加速度符合某種統(tǒng)計規(guī)律，如一階馬爾科夫模型。因而，該方法僅適用于機動性較差的地面慢速目標(biāo)。本文將利用一階馬爾科夫模型建立系統(tǒng)的狀態(tài)方程，結(jié)合 Huber-Based濾波算法以及視線角速度解耦模型實現(xiàn)對目標(biāo)視線角等信息的有效估計

1 Huber-Based濾波算法

魯棒性是指當(dāng)參數(shù)與假設(shè)分布存在偏差時，系統(tǒng)對偏差不敏感的特性。在實際應(yīng)用中，由于內(nèi)外部不確定因素的影響會造成系統(tǒng)噪聲的先驗統(tǒng)計特性不準(zhǔn)確或噪聲特性不穩(wěn)定，進而影響整個系統(tǒng)的精度。為解決這個問題，Huber提出了廣義極大似然估計[10]，即 M 估計。本文將濾波系統(tǒng)的量測更新轉(zhuǎn)化為用Huber估計的方法[11]，具體如下：

假設(shè)有如下離散系統(tǒng)：

式(1)為系統(tǒng)的狀態(tài)方程，式(2)為系統(tǒng)的量測方程，其中，為第k-1時刻到第k時刻的一步轉(zhuǎn)移矩陣，Γk-1為系統(tǒng)噪聲驅(qū)動矩陣，xk和zk分別為系統(tǒng)的狀態(tài)值和量測值，wk為系統(tǒng)激勵噪聲序列，vk為量測噪聲序列。

1.1 狀態(tài)預(yù)測

將第k時刻系統(tǒng)狀態(tài)的估計值記為x?k，狀態(tài)估計誤差協(xié)方差陣記為Pk，根據(jù)系統(tǒng)方程，第k時刻預(yù)測的狀態(tài)估計值和協(xié)方差為

1.2 量測更新

對于給定的量測值zk-1，濾波更新是所構(gòu)造的線性回歸問題的解，因此，將濾波更新問題轉(zhuǎn)換為線性回歸問題，有：

定義如下變量：

Huber-Based濾波的量測更新值可以通過求解如下指標(biāo)函數(shù)獲得：

式中，ζi為ζ的第i個元素，且為損失函數(shù)（或目標(biāo)函數(shù)）。

盡管ρ函數(shù)在一定程度上可以選擇，但為保證式(7)有解，ρ函數(shù)須滿足以下三個條件：

1）ρ在R中處處連續(xù)；

3）矩陣M滿秩。

Huber給出了如下ρ函數(shù)形式：

式中，γ為調(diào)節(jié)參數(shù)。

式(8)中ρ函數(shù)是l1范數(shù)最小和l2范數(shù)最小的混合方程。調(diào)節(jié)參數(shù)γ→0，式(8)為l1范數(shù)最小；調(diào)節(jié)參數(shù)γ→∞，式(8)為l2范數(shù)最小。Huber已證明，當(dāng)ρ函數(shù)選擇式(8)的形式，Huber-Based濾波對受污染的高斯分布具有漸進最優(yōu)魯棒性[12]。

而最小化指標(biāo)函數(shù)(7)的解滿足如下方程：

通過迭代可求出式(10)的解：

圖 1 Huber-Based濾波流程圖Fig.1 Flow chart of Huber-based filter

Huber-Based濾波的流程圖如圖1所示。

2 捷聯(lián)成像導(dǎo)引頭視線角速率估計

2.1 坐標(biāo)系及其轉(zhuǎn)換

為研究捷聯(lián)成像導(dǎo)引頭視線角速率估計算法，引入發(fā)射慣性坐標(biāo)系、彈體坐標(biāo)系、視線坐標(biāo)系和體視線坐標(biāo)系。各坐標(biāo)系定義如下：

其轉(zhuǎn)換關(guān)系如圖2所示。圖2中：

上述諸式中，θ、ψ、γ分別為俯仰、滾轉(zhuǎn)、偏航角，qα為體視線高低角，qβ為體視線方位角，qγ為視線高低角，qλ為視線方位角，qc為視線變換角，s、c分別表示sin和cos。

圖2 坐標(biāo)系間轉(zhuǎn)換關(guān)系Fig.2 Transformation relationship between coordinate systems

2.2 系統(tǒng)狀態(tài)模型

狀態(tài)方程的連續(xù)形式為

其離散形式為

式(21)(22)中， f為式(13)～(20)的聯(lián)立形式。

2.3 系統(tǒng)量測模型

考慮到捷聯(lián)成像導(dǎo)引頭可直接測量體視線高低角qα及qβ，故將其作為量測量，但仍需建立狀態(tài)量與其之間的聯(lián)系，而目標(biāo)在視線坐標(biāo)系和體視線坐標(biāo)系中的坐標(biāo)均為則目標(biāo)在體坐標(biāo)系和發(fā)射慣性坐標(biāo)系中的坐標(biāo)分別為

聯(lián)立式(23)，有：

由式(26)，可以解出：

式中：Cij為從發(fā)射慣性坐標(biāo)系到彈體坐標(biāo)系的轉(zhuǎn)換矩陣的第i行第j列元素；s、 c、t分別表示sin、cos和tan。

由此，可得系統(tǒng)的量測方程為

式中：Cij為從發(fā)射管坐標(biāo)系到彈體坐標(biāo)系的轉(zhuǎn)換矩陣的第i行第j列元素；ωα和 ωβ為體視線高低角的量測噪聲；s、c、t分別表示sin、cos和tan。

3 濾波器設(shè)計

根據(jù) 2.2節(jié)選取的狀態(tài)量和建立的狀態(tài)方程(13)～(20)，計算雅克比矩陣如下：

則狀態(tài)轉(zhuǎn)移矩陣Fk為

式中，T為采樣周期。

然后依據(jù)式(3)和式(4)進行狀態(tài)估計值和協(xié)方差陣的一步預(yù)測。再由 2.3節(jié)所建立的系統(tǒng)量測模型，求出系統(tǒng)的量測矩陣Hk為

式中，

其中，Cij為從發(fā)射慣性坐標(biāo)系到彈體坐標(biāo)系的轉(zhuǎn)換矩陣bC 的第i行第j列元素。

通過已經(jīng)求解出的狀態(tài)轉(zhuǎn)移矩陣Fk和量測矩陣f將量測更新問題轉(zhuǎn)換為求解線性回歸問題，依據(jù)1.2節(jié)及1.3節(jié)所述方法，通過迭代求解出量測更新后的狀態(tài)估計值和協(xié)方差陣，作為下一步濾波計算的初值。

4 仿真分析

4.1 仿真初始條件

捷聯(lián)成像導(dǎo)引頭系統(tǒng)量測噪聲為高斯白噪聲，其標(biāo)準(zhǔn)差為0.06°，然而，高斯白噪聲為理想模型，實際噪聲特性一般為受污染的白噪聲。理想的高斯白噪聲受到污染后的概率分布表達式如式(32)所示，且本文的仿真中認為污染分布也符合高斯分布，并且方差是原高斯分布的5倍。

式中：ε為污染率；σ1和σ2分別為捷聯(lián)成像導(dǎo)引頭系統(tǒng)量測噪聲標(biāo)準(zhǔn)差和污染噪聲標(biāo)準(zhǔn)差。

初始彈目距離為30 km，仿真時間為20 s，目標(biāo)運動特性如圖3所示。

圖3 目標(biāo)運動特性Fig.3 Moving characteristics of target

4.2 仿真結(jié)果

取污染率ε=0.2進行仿真，得到仿真結(jié)果如圖4～圖8所示。圖4和圖5分別給出了Huber-Based濾波算法估計出的視線角和視線角速率。可以看出，該算法的視線角估計精度在0.05°以內(nèi)，視線角速率估計精度在0.05 (°)/s以內(nèi)，具有估計精度高、收斂速度快的特點。圖7和圖8分別給出了擴展卡爾曼濾波和Huberbased濾波算法的視線角估計誤差和視線角速率估計誤差（25次仿真的平均情況）的對比，可以看出，對于量測噪聲受污染的高斯白噪聲情況，Huber-Based濾波估計精度高于擴展卡爾曼濾波。

圖4 視線角估計誤差Fig.4 Estimation error of LOS angle

圖5 視線角速率估計誤差Fig.5 Estimation error of LOS rate

圖6 視線角加速度估計誤差Fig.6 Estimation error of LOS acceleration

圖7 兩種濾波算法視線角估計誤差對比Fig.7 Estimation error of LOS angle of two algorithms

圖8 兩種濾波算法視線角速率估計誤差對比Fig.8 Estimation error of LOS rate of two algorithms

5 結(jié) 論

本文研究捷聯(lián)成像導(dǎo)引頭系統(tǒng)輸出的量測噪聲不符合理想高斯分布時，進行視線角、視線角速率以及視線角加速度估計的方法，將 Huber-based濾波應(yīng)用于捷聯(lián)成像導(dǎo)引頭視線角及視線角速率估計，有效地估計出了視線角、視線角速率以及視線角加速度。與擴展卡爾曼濾波相比，在導(dǎo)引頭設(shè)備輸出為受污染的高斯分布時，該估計方法具有一定的魯棒性。

圖9 兩種濾波算法視線角加速度估計誤差對比Fig.9 Estimation error of LOS acceleration of two algorithms

（References）：

[1] Mirmina S A. The ballistic missile defense system and its effects on the outer space environment[J]. J. Space L, 2005, 31: 287-297.

[2] Arcus P. American national missile defense system[J]. Space Policy, 2003, 19(1): 7-13.

[3] Nazaroff G. An optimal terminal guidance law[J]. IEEE Transactions on Automatic Control, 1976, 21(3): 407-408.

[4] Ehrich R D, Vergez P. Strapdown seeker technology for the terminal guidance of tactical weapons[C]//11th International Conference on Control, Automation and Systems. 2011: 969-972.

[5] 楚德強. 某型導(dǎo)彈捷聯(lián)末制導(dǎo)律設(shè)計及制導(dǎo)精度分析[D]. 西北工業(yè)大學(xué), 2007. Chu De-qiang. A certain type of missile strapdown terminal guidance law design and guidance accuracy analysis[D]. Northwestern Polytechnical University, 2007.

[6] 王佩, 張科, 呂梅柏. 跟蹤-微分器在全捷聯(lián)制導(dǎo)中的應(yīng)用分析[J]. 西北工業(yè)大學(xué)學(xué)報, 2014, 32(5): 817-821. Wang Pei, Zhang Ke, Lv Mei-bai. The analysis of the application of tracking-differentiator in strapdown guidance[J]. Journal of Northwestern Polytechnical University, 2014, 32(5): 817-821.

[4] Wen Shi-ping, Zeng Zhi-gang, Huang Ting-wen. Reliable H∞filtering for neutral systems with mixed delays and multiplicative noises[J]. Signal Processing, 2013, 94(1): 23-32.

[5] Sangsuk S, Bullock T E. Analysis of discrete-time Kalman filtering under incorrect noise covariances[J]. IEEE Transactions on Automatic Control, 1990, 35(12): 1304-1309.

[6] Chai Qiang-long, Bai Yu-long, Dong Cun-hui. Radar target tracking error research using Kalman filter-based algorithms[J]. Applied Mechanics and Materials, 2013, 239: 1184-1187.

[7] Pedro B,Carlos S, Paulo O. Partial attitude and rate gyro bias estimation: Observability analysis, filter design, and performance evaluation[J]. International Journal of Control, 2011, 84(5): 895-903.

[8] Julier D S, Uhlmann J, Durrant-Whyte H F. A new method for the nonlinear transformation of means and covariances in filters and estimators[J]. IEEE Transactions on Automatic Control, 2000, 45(3): 2253-2261.

[9] Xiong K, Zhang H Y, Chan C W. Performance evaluation of UKF-based nonlinear filtering[J]. Automatica, 2006, 42: 261-270.

[10] 周朝霞, 任家棟, 曾慶雙, 等. 測量最小分辨率特性下自主相對導(dǎo)航設(shè)計方法[J]. 哈爾濱工業(yè)大學(xué)學(xué)報, 2015, 47(3): 49-53. Zhou Chao-xia, Ren Jia-dong, Zeng Qing-shuang, et al. Autonomous relative navigation constrained by microwave radar minimum measure resolution[J]. Journal of Harbin Institute of Technology, 2015, 47(3): 49-53.

[11] 任家棟, 曾慶雙, 豐保民. 基于地平儀兩軸姿態(tài)測量的衛(wèi)星自主相對導(dǎo)航[J]. 中國慣性技術(shù)學(xué)報, 2015, 23(5): 597-601. Ren Jia-dong, Zeng Qing-shuang, Feng Bao-min. Autonomous relative navigation with two-axis attitude measurements only infrared earth sensor[J]. Journal of Chinese Inertial Technology, 2015, 23(5): 597-601.

[7] 李璟璟, 伊國興, 張迎春. 基于粒子濾波的捷聯(lián)成像導(dǎo)引頭視線角速率估計[J]. 彈箭與制導(dǎo)學(xué)報, 2009, 29(2): 91-94. Li Jing-jing, Yi Guo-xing, Zhang Ying-chun. Strapdown imaging seeker LOS rate estimation based on particle filter [J]. Journal of Projectiles, Rockets, Missiles and Guidance, 2009, 29(2): 91-94.

[8] 宋建梅, 鄭建強. 平方根濾波在捷聯(lián)尋的制導(dǎo)系統(tǒng)中的應(yīng)用研究[J]. 北京理工大學(xué)學(xué)報, 2006, 26(6): 476-478. Song Jian-mei, Zheng Jian-qiang. Application of H∞square root filter for strapdown homing guidance system [J]. Transactions of Beijing Institute of Technology, 2006, 26(6): 476-478.

[9] 李全運, 宋建梅. 捷聯(lián)尋的制導(dǎo)系統(tǒng)濾波器設(shè)計[J].兵工學(xué)報, 2005, 26(2): 216-219. Li Quan-yun, Song Jian-mei. Strapdown homing guidance system filter design[J]. Acta Armamentarii, 2005, 26(2): 216-219.

[10] Huber P J. Robust estimation of a location parameter[J]. Annuals of Mathematical Statistics, 1964, 3(2): 73-101.

[11] Petrus P. Robust Huber adaptive filter[J]. IEEE Transactions on Signal Processing, 1999, 47(4): 1129-1133.

[12] Kumar T A, Rao K D, Swamy M N S. A robust technique for multiuser detection in non Gaussian channels[C]//The 2004 47th Midwest Symposium on Circuits and Systems. 2004: 247-250.

Line-of-sight angular-rate estimation of strapdown seeker based on robust filter

WANG Xiao-gang1, HU Zhi-yong2, YU Yang3, QIN Wu-tao1

(1. Department of Astronautics, Harbin Institute of Technology, Harbin 150001, China; 2. Dalian Communication Sergeant School of Air Force, Dalian 116600, China; 3. State-owned No.624 Factory, Harbin 150001, China)

In view of the problem that strapdown seeker can’t directly access to the line-of-sight (LOS) angular velocity, the application of robust filter in LOS rate estimation of exo-atmospheric vehicle is studied. In order to establish a nonlinear filtering estimation model, the first-order Markov model state equation is established based on the slowly varying properties of LOS rate. The LOS rate decoupling model is derived as the measurement equation. In view that some factors in the practical application can cause the noise be inaccurate in statistical properties, this paper discusses the Huber-based robust filtering method and designs the LOS rate filter. The simulation based on Huber-Based filter and extended Kalman filter is completed under the condition of non-Gaussian noise. Simulation results show that the LOS angle, LOS rate and LOS acceleration estimation precisions based on Huber-based filter method reach 0.1140′, 0.1423′/s, 0.0203′/s2, respectively, but the LOS angle, LOS rate and LOS acceleration estimation precisions based on an extended Kalman filter method only reach 0.6577′, 0.6415′/s, 0.0979′/s2, respectively. These prove that the method can effectively estimate the relative LOS rate information, and can also obtain higher estimation precision under the condition of non-Gaussian noise, showing its good robustness.

strapdown seeker, LOS Rate, first-order Markov model, nonlinear filter, Huber-based filter

V249.32

A

1005-6734(2016)02-0251-06

10.13695/j.cnki.12-1222/o3.2016.02.021

2016-01-13

2016-03-30

國家自然科學(xué)基金（61304236）

王小剛（1980—），男，副教授，研究方向為非線性濾波理論及應(yīng)用。E-mail: wangxiaogang@hit.edu.cn