CN101603800B - 一种半捷联寻的导引头的制导信息构造方法 - Google Patents

Abstract

本发明涉及一种导弹制导信息构造方法，特别涉及一种半捷联寻的导引头制导信息构造方法，属于导弹制导系统设计技术领域。本发明提出一种新的半捷联寻的导引头制导信息构造方法，该方法基于Unscented卡尔曼滤波器(UKF)进行框架角速率估计，然后进行制导信息构造，有效提高了制导信息的构造精度，防止了直接对框架角微分得到角速率的方法所造成的误差放大，而且比采用EKF滤波构造得到的制导信息精度高，能在干扰情况下控制导弹按比例导引飞向目标。可广泛应用于各类半捷联寻的制导武器中，具有重要的军事应用前景。

Description

一种半捷联寻的导引头的制导信息构造方法

技术领域

本发明涉及一种导弹制导信息构造方法，特别涉及一种半捷联寻的导引头制导信息构造方法，属于导弹制导控制系统设计技术领域。

背景技术

半捷联寻的导引头的特点是，将常平架式导引头上的惯性测量元件从平台上取下来，直接捷联安装在弹体上，测量弹体三个方向上的角速度，平台的基座与弹体刚性连接。半捷联寻的导引头的优点是减小了平台安装空间，机械结构比较简单，易于小型化，成本低。缺点是没有机械独立的稳定平台系统实现对弹体运动的隔离，所以弹体运动会耦合作用到光轴上，不能由机械装置直接给出比例导引所需制导信息。

一般半捷联寻的导引头由探测器、框架、框架上的位置传感器、和弹上的惯性测量组件构成。探测器用于测量光轴指向与弹目视线之间的误差角，而光轴相对于弹体的俯仰框架角和方位框架角由框架上的位置传感器测量，惯性测量组件既可用于隔离弹体对导引头的扰动，也可用于导弹自动驾驶仪。

半捷联寻的导引头应用于导弹制导必须解决的关键技术之一是制导信息构造问题。框架角速率信号是制导信息的重要组成部分之一，目前获取框架角速率信号的方法主要有以下几种：

1)对框架角直接微分得到。这种方法在理论上可行，但在工程实践中微分运算会对噪声起放大作用，影响在工程实践中的应用，不能满足制导精度要求。

2)采用EKF滤波对半捷联寻的导引头的框架角速率进行估计。由于EKF滤波估计精度不高，导致制导信息的构造精度不理想。

3)直接测量获取，但这样会增加导引头硬件成本，增大导引头体积。

发明内容

本发明的目的是为了解决上述半捷联寻的导引头制导信息构造中的问题，提出了一种新的半捷联寻的导引头制导信息构造方法。

本发明的目的是通过下述技术方案实现的。

本发明的一种半捷联寻的导引头制导信息构造方法，该方法是为了控制带有半捷联寻的导引头的导弹准确命中目标，需要构造制导信息；框架角速率信息是制导信息的重要组成部分，本发明采用Unscented卡尔曼滤波器UKF(Unscented Kalman Fi lter)得到框架角速率比较准确的估计值；为了采用UKF进行滤波估计，首先需要建立半捷联寻的导引头框架动力学数学模型，然后建立框架动力学的滤波状态方程和测量方程，进行框架角速率信息的递推滤波估计。

具体实现步骤如下：

1)建立半捷联寻的导引头框架动力学数学模型

根据动量矩定理，建立两框架半捷联寻的导引头的框架动力学模型如下：

${\stackrel{\·\·}{\mathrm{\λ}}}_z=\frac{T_{\mathrm{oz}}-\frac{{\mathrm{\β}}_3}{{\mathrm{\α}}_3}{T}_{\mathrm{oy}}-[({\mathrm{\β}}_1+{\mathrm{\δ}}_1)-\frac{{\mathrm{\β}}_3({\mathrm{\α}}_1+{\mathrm{\γ}}_1)}{{\mathrm{\α}}_3}]{\stackrel{\·}{\mathrm{\λ}}}_y-[({\mathrm{\β}}_2+{\mathrm{\δ}}_2)-\frac{{\mathrm{\β}}_3({\mathrm{\α}}_2+{\mathrm{\γ}}_2)}{{\mathrm{\α}}_3}]{\stackrel{\·}{\mathrm{\λ}}}_z-{\mathrm{\δ}}_4{\stackrel{\·}{\mathrm{\λ}}}_y{\stackrel{\·}{\mathrm{\λ}}}_z-{\mathrm{\δ}}_3{{\stackrel{\·}{\mathrm{\λ}}}_y}^2-[({\mathrm{\β}}_5+{\mathrm{\δ}}_5)-\frac{{\mathrm{\β}}_3({\mathrm{\α}}_5+{\mathrm{\γ}}_3)}{{\mathrm{\α}}_{\text{3}}}]}{{\mathrm{\β}}_4-\frac{{\mathrm{\α}}_4{\mathrm{\β}}_3}{{\mathrm{\α}}_3}}$

${\stackrel{\·\·}{\mathrm{\λ}}}_y=\frac{1}{{\mathrm{\α}}_3}[T_{\mathrm{oy}}-({\mathrm{\α}}_1+{\mathrm{\γ}}_1){\stackrel{\·}{\mathrm{\λ}}}_y-({\mathrm{\α}}_2+{\mathrm{\γ}}_2){\stackrel{\·}{\mathrm{\λ}}}_z-{\mathrm{\α}}_4{\stackrel{\·\·}{\mathrm{\λ}}}_z-{\mathrm{\α}}_5-{\mathrm{\γ}}_3]$

其中：

${\mathrm{\α}}_1={\stackrel{\·}{I}}_y^{\mathrm{in}/0}$ ${\mathrm{\α}}_2={\stackrel{\·}{I}}_{\mathrm{yz}}^{\mathrm{in}/0}$ ${\mathrm{\α}}_3=I_y^{\mathrm{in}/0}+I_y^0$ ${\mathrm{\α}}_4=I_{\mathrm{yz}}^{\mathrm{in}/0}$

${\mathrm{\α}}_5=\stackrel{\·}{a}(I_{\mathrm{xy}}^0+I_{\mathrm{xy}}^{\mathrm{in}/0})+a{\stackrel{\·}{I}}_{\mathrm{xy}}^{\mathrm{in}/0}+\stackrel{\·}{b}(I_y^0+I_y^{\mathrm{in}/0})+b{\stackrel{\·}{I}}_y^{\mathrm{in}/0}+\stackrel{\·}{c}(I_{\mathrm{yz}}^0+I_{\mathrm{yz}}^{\mathrm{in}/0})+{c\stackrel{\·}{I}}_{\mathrm{yz}}^{\mathrm{in}/0}$

${\mathrm{\γ}}_1=c(I_{\mathrm{xy}}^0+I_{\mathrm{xy}}^{\mathrm{in}/0})-a(I_{\mathrm{yz}}^0+I_{\mathrm{yz}}^{\mathrm{in}/0})$ ${\mathrm{\γ}}_2=c{I}_{\mathrm{xz}}^{\mathrm{in}/0}-a{I}_z^{\mathrm{in}/0}$

${\mathrm{\γ}}_3=\mathrm{ac}((I_x^0+I_x^{\mathrm{in}/0})-(I_z^0+I_z^{\mathrm{in}/0}))+\mathrm{bc}(I_{\mathrm{xy}}^0+I_{\mathrm{xy}}^{\mathrm{in}/0})+(c^2-a^2)(I_{\mathrm{xz}}^0+I_{\mathrm{xz}}^{\mathrm{in}/0})-\mathrm{ab}(I_{\mathrm{yz}}^0+I_{\mathrm{yz}}^{\mathrm{in}/0})$

${\mathrm{\β}}_1={\stackrel{\·}{I}}_y^{\mathrm{in}/0}$ ${\mathrm{\β}}_2={\stackrel{\·}{I}}_z^{\mathrm{in}/0}$ ${\mathrm{\α}}_3=I_y^{\mathrm{in}/0}$ ${\mathrm{\β}}_4=I_z^{\mathrm{in}/0}$

${\mathrm{\β}}_5=\stackrel{\·}{a}{I}_{\mathrm{xz}}^{\mathrm{in}/0}+a{\stackrel{\·}{I}}_{\mathrm{xz}}^{\mathrm{in}/0}+\stackrel{\·}{b}{I}_{\mathrm{yz}}^{\mathrm{in}/0}+n{\stackrel{\·}{I}}_{\mathrm{yz}}^{\mathrm{in}/0}+\stackrel{\·}{c}{I}_z^{\mathrm{in}/0}+c{\stackrel{\·}{I}}_z^{\mathrm{in}/0}$

${\mathrm{\δ}}_1=a(I_y^{\mathrm{in}/0}+I_x^{\mathrm{in}/0})-2b{I}_{\mathrm{xy}}^{\mathrm{in}/0}-c{I}_{\mathrm{xz}}^{\mathrm{in}/0}$ ${\mathrm{\δ}}_2=a{I}_{\mathrm{yz}}^{\mathrm{in}/0}-b{I}_{\mathrm{xz}}^{\mathrm{in}/0}$

${\mathrm{\δ}}_3=-I_{\mathrm{xy}}^{\mathrm{in}/0}$ ${\mathrm{\δ}}_4=-I_{\mathrm{xz}}^{\mathrm{in}/0}$

${\mathrm{\δ}}_5=(a^2-b^2)I_{\mathrm{xy}}^{\mathrm{in}/0}+\mathrm{ab}(I_y^{\mathrm{in}/0}-I_x^{\mathrm{in}/0})+\mathrm{ac}{I}_{\mathrm{yz}}^{\mathrm{in}/0}-\mathrm{bc}{I}_{\mathrm{xz}}^{\mathrm{in}/0}$

$\left[\begin{array}{c}a\\ b\\ c\end{array}\right]=\left[\begin{array}{c}{W}_{\mathrm{mx}}\cos {\mathrm{\λ}}_y-W_{\mathrm{mz}}\sin {\mathrm{\λ}}_y\\ W_{\mathrm{my}}\\ W_{\mathrm{mx}}\sin {\mathrm{\λ}}_y+W_{\mathrm{mz}}\cos {\mathrm{\λ}}_y\end{array}\right]$

$\left[\begin{array}{c}\stackrel{\·}{a}\\ \stackrel{\·}{b}\\ \stackrel{\·}{c}\end{array}\right]\left[\begin{array}{c}{\stackrel{\·}{W}}_{\mathrm{mx}}\cos {\mathrm{\λ}}_y-W_{\mathrm{mx}}{\stackrel{\·}{\mathrm{\λ}}}_y\sin {\mathrm{\λ}}_y-{\stackrel{\·}{W}}_{\mathrm{mz}}\sin {\mathrm{\λ}}_y-W_{\mathrm{mz}}{\stackrel{\·}{\mathrm{\λ}}}_y\cos {\mathrm{\λ}}_y\\ {\stackrel{\·}{W}}_{\mathrm{my}}\\ {\stackrel{\·}{W}}_{\mathrm{mx}}\sin {\mathrm{\λ}}_y+W_{\mathrm{mx}}{\stackrel{\·}{\mathrm{\λ}}}_y\cos {\mathrm{\λ}}_y+{\stackrel{\·}{W}}_{\mathrm{mz}}\cos {\mathrm{\λ}}_y-W_{\mathrm{mz}}{\stackrel{\·}{\mathrm{\λ}}}_y\sin {\mathrm{\λ}}_y\end{array}\right]$

$\left[\begin{array}{ccc}{I}_x^{\mathrm{in}}& I_{\mathrm{xy}}^{\mathrm{in}}& I_{\mathrm{xz}}^{\mathrm{in}}\\ I_{\mathrm{xy}}^{\mathrm{in}}& I_y^{\mathrm{in}}& I_{\mathrm{yz}}^{\mathrm{in}}\\ I_{\mathrm{xz}}^{\mathrm{in}}& I_{\mathrm{yz}}^{\mathrm{in}}& I_z^{\mathrm{in}}\end{array}\right]$ 为内框连同光电负载的转动惯量；

$\left[\begin{array}{ccc}{I}_x^o& I_{\mathrm{xy}}^o& I_{\mathrm{xz}}^o\\ I_{\mathrm{xy}}^o& I_y^o& I_{\mathrm{yz}}^o\\ I_{\mathrm{xz}}^o& I_{\mathrm{yz}}^o& I_z^o\end{array}\right]$ 为外框转动惯量；

λ_z，λ_y分别为俯仰框架角和偏航框架角；

W_mx、W_my、W_mz为导弹旋转角速度；

T_oz为内框力矩电机产生的驱动力矩；

T_oy为外框力矩电机产生的驱动力矩。

2)建立半捷联寻的导引头框架动力学的滤波状态方程和测量方程定义状态变量

x₁＝λ_z

$x_2={\stackrel{\·}{\mathrm{\λ}}}_z$

x₃＝λ_y

$x_4={\stackrel{\·}{\mathrm{\λ}}}_y$

控制变量为：u＝[T_oz T_oy]^T

则其状态方程和测量方程为：

$\stackrel{\·}{x}=F(x,u)+w$

y＝[λ_z λ_y]^T＝Hx+v

w为状态噪声，v为测量噪声

滤波前，首先将上述微分方程转化为等效离散模型。

3)UKF滤波器初始化

x₀＝E(x₀)，P₀＝E[(x₀-x₀)(x₀-x₀)^T]

$W_0^{\left(m\right)}=\mathrm{\τ}/(\mathrm{\τ}+L)$

$W_0^{\left(p\right)}=\mathrm{\τ}/(\mathrm{\τ}+L)+1-{\mathrm{\α}}^2+\mathrm{\β}$

其中，m代表均值，p代表协方差，L为状态维数

τ＝α²(L+κ)-L，τ是一个比例缩放参数，通常取一个很小的正值；κ为辅助尺度因子，应确保矩阵

为半正定矩阵；β用于体现x的分布，若x为高斯分布，β＝2。

4)计算2L+1个sigma点χ_i和它们的权值

${\mathrm{\χ}}_{k-1}=[{\stackrel{\‾}{x}}_{k-1},{\stackrel{\‾}{x}}_{k-1}-\mathrm{\γ}\sqrt{P_{k-1}},{\stackrel{\‾}{x}}_{k-1}+\mathrm{\γ}\sqrt{P_{k-1}}]$

$W_i^{\left(m\right)}=W_i^{\left(p\right)}=1/\left[2(L+\mathrm{\τ})\right],$ i＝1，2，…，2L

下标i代表第几个采样点； $\mathrm{\γ}=\sqrt{L+\mathrm{\τ}}$ 是比例因子；

是矩阵(L+τ)P_x的平方根，它是对称的正定矩阵。

5)滤波器时间更新

χ_k/k-1＝F(χ_k-1，u_k-1，w_k-1)

${\stackrel{\‾}{x}}_k^{-}=\underset{i=0}{\overset{2L}{\mathrm{\Σ}}}{W}_i^{\left(m\right)}{\mathrm{\χ}}_{i,k/k-1}$

$P_k^{-}=\underset{i=0}{\overset{2L}{\mathrm{\Σ}}}{W}_i^{\left(p\right)}({\mathrm{\χ}}_{i,k/k-1}-{\stackrel{\‾}{x}}_k^{-}){({\mathrm{\χ}}_{i,k/k-1}-{\stackrel{\‾}{x}}_k^{-})}^T+Q$

Y_k/k-1＝H(χ_k/k-i，v_k)

${\stackrel{\‾}{y}}_k^{-}=\underset{i=0}{\overset{2L}{\mathrm{\Σ}}}{W}_i^{\left(m\right)}{Y}_{i,k/k-1}$

式中，Q为系统状态噪声的协方差矩阵。

6)滤波器测量更新

$P_{{\stackrel{\‾}{y}}_k{\stackrel{\‾}{y}}_k}=\underset{i=0}{\overset{2L}{\mathrm{\Σ}}}{W}_i^{\left(p\right)}(Y_{i,k/k-1}-{\stackrel{\‾}{y}}_k^{-}){(Y_{i,k/k-1}-{\stackrel{\‾}{y}}_k^{-})}^T+R$

$P_{{\stackrel{\‾}{x}}_k{\stackrel{\‾}{y}}_k}=\underset{i=0}{\overset{2L}{\mathrm{\Σ}}}{W}_i^{\left(p\right)}({\mathrm{\χ}}_{i,k/k-1}-{\stackrel{\‾}{x}}_k^{-}){({\mathrm{\χ}}_{i,k/k-1}-{\stackrel{\‾}{y}}_k^{-})}^T$

$K_k=P_{{\stackrel{\‾}{x}}_k{\stackrel{\‾}{y}}_k}{P}_{{\stackrel{\‾}{y}}_k{\stackrel{\‾}{y}}_k}^{-1}$

${\stackrel{\‾}{x}}_k={\stackrel{\‾}{x}}_k^{-}+K_k(y_k-{\stackrel{\‾}{y}}_k^{-})$

$P_k=P_k^{-}-K_k{P}_{{\stackrel{\‾}{y}}_k{\stackrel{\‾}{y}}_k}{K}_k^T$

式中，R为测量噪声的协方差矩阵。

这样，就得到了k时刻的状态x_k和协方差P_x，其中x_k的第二和第四项即为俯仰和偏航框架角速率。

7)构造制导信息，对导弹进行导引

半捷联寻的导引头的制导信息由弹体旋转角速率、框架角速度和目标在像平面的移动角速度组成。其中框架角速率由上述UKF滤波得到，弹体旋转角速率由弹上惯性测量组件得到，目标在像平面的移动角速度由探测器测得的误差角经滤波微分得到。

根据自动驾驶仪要求，将制导信息转换到所需要的坐标系中，即对导弹进行导引飞行。

8)判断是否已进入导引头盲区，若是则停止滤波，导弹导引信息归零；否则转到4)。

至此，通过上述8个步骤完成了基于UKF的半捷联寻的导引头制导信息的构造，构造流程如图1所示。

有益效果

本发明的一种半捷联寻的导引头的制导信息构造方法，在框架角速率不可测情况下，构造了半捷联寻的制导系统制导信息，构造精度较高，能在干扰情况下控制导弹按比例导引飞向目标。该方法成本低，防止了直接对框架角微分得到角速率的方法所造成的误差放大，而且比采用EKF滤波得到的制导信息构造精度高。可广泛应用于各类半捷联寻的制导武器中，具有重要的军事应用前景。

附图说明

图1本发明的总体框图；

图2-图6为采用本发明的构造方法得到制导信息，对导弹进行比例导引仿真，得到的各特征变量变化曲线；

图2俯仰框架角速率变化；

图3弹体系下的偏航框架角速率变化；

图4弹体姿态变化；

图5导弹俯仰平面飞行轨迹；

图6导弹偏航平面飞行轨迹；

图7-图11为采用EKF滤波进行制导信息构造，并对导弹进行比例导引仿真，得到各特征变量变化曲线；

图7俯仰框架角速率变化；

图8弹体系下的偏航框架角速率变化；

图9弹体姿态变化；

图10导弹俯仰平面飞行轨迹；

图11导弹偏航平面飞行轨迹；

具体实施方式

下面结合附图对本发明的技术方案做进一步描述。

本发明包括下述步骤：(如图1所示)

1)建立半捷联寻的导引头框架动力学数学模型

根据动量矩定理，建立两框架半捷联寻的导引头的框架动力学模型如下：

${\stackrel{\·\·}{\mathrm{\λ}}}_z=\frac{T_{\mathrm{oz}}-\frac{{\mathrm{\β}}_3}{{\mathrm{\α}}_3}{T}_{\mathrm{oy}}-[({\mathrm{\β}}_1+{\mathrm{\δ}}_1)-\frac{{\mathrm{\β}}_3({\mathrm{\α}}_1+{\mathrm{\γ}}_1)}{{\mathrm{\α}}_3}]{\stackrel{\·}{\mathrm{\λ}}}_y-[({\mathrm{\β}}_2+{\mathrm{\δ}}_2)-\frac{{\mathrm{\β}}_3({\mathrm{\α}}_2+{\mathrm{\γ}}_2)}{{\mathrm{\α}}_3}]{\stackrel{\·}{\mathrm{\λ}}}_z-{\mathrm{\δ}}_4{\stackrel{\·}{\mathrm{\λ}}}_y{\stackrel{\·}{\mathrm{\λ}}}_z-{\mathrm{\δ}}_3{{\stackrel{\·}{\mathrm{\λ}}}_y}^2-[({\mathrm{\β}}_5+{\mathrm{\δ}}_5)-\frac{{\mathrm{\β}}_3({\mathrm{\α}}_5+{\mathrm{\γ}}_3)}{{\mathrm{\α}}_{\text{3}}}]}{{\mathrm{\β}}_4-\frac{{\mathrm{\α}}_4{\mathrm{\β}}_3}{{\mathrm{\α}}_3}}$

${\stackrel{\·\·}{\mathrm{\λ}}}_y=\frac{1}{{\mathrm{\α}}_3}[T_{\mathrm{oy}}-({\mathrm{\α}}_1+{\mathrm{\γ}}_1){\stackrel{\·}{\mathrm{\λ}}}_y-({\mathrm{\α}}_2+{\mathrm{\γ}}_2){\stackrel{\·}{\mathrm{\λ}}}_z-{\mathrm{\α}}_4{\stackrel{\·\·}{\mathrm{\λ}}}_z-{\mathrm{\α}}_5-{\mathrm{\γ}}_3]$

其中：

${\mathrm{\α}}_1={\stackrel{\·}{I}}_y^{\mathrm{in}/0}$ ${\mathrm{\α}}_2={\stackrel{\·}{I}}_{\mathrm{yz}}^{\mathrm{in}/0}$ ${\mathrm{\α}}_3=I_y^{\mathrm{in}/0}+I_y^0$ ${\mathrm{\α}}_4=I_{\mathrm{yz}}^{\mathrm{in}/0}$

${\mathrm{\α}}_5=\stackrel{\·}{a}(I_{\mathrm{xy}}^0+I_{\mathrm{xy}}^{\mathrm{in}/0})+a{\stackrel{\·}{I}}_{\mathrm{xy}}^{\mathrm{in}/0}+\stackrel{\·}{b}(I_y^0+I_y^{\mathrm{in}/0})+b{\stackrel{\·}{I}}_y^{\mathrm{in}/0}+\stackrel{\·}{c}(I_{\mathrm{yz}}^0+I_{\mathrm{yz}}^{\mathrm{in}/0})+{c\stackrel{\·}{I}}_{\mathrm{yz}}^{\mathrm{in}/0}$

${\mathrm{\γ}}_1=c(I_{\mathrm{xy}}^0+I_{\mathrm{xy}}^{\mathrm{in}/0})-a(I_{\mathrm{yz}}^0+I_{\mathrm{yz}}^{\mathrm{in}/0})$ ${\mathrm{\γ}}_2=c{I}_{\mathrm{xz}}^{\mathrm{in}/0}-a{I}_z^{\mathrm{in}/0}$

${\mathrm{\γ}}_3=\mathrm{ac}((I_x^0+I_x^{\mathrm{in}/0})-(I_z^0+I_z^{\mathrm{in}/0}))+\mathrm{bc}(I_{\mathrm{xy}}^0+I_{\mathrm{xy}}^{\mathrm{in}/0})+(c^2-a^2)(I_{\mathrm{xz}}^0+I_{\mathrm{xz}}^{\mathrm{in}/0})-\mathrm{ab}(I_{\mathrm{yz}}^0+I_{\mathrm{yz}}^{\mathrm{in}/0})$

${\mathrm{\β}}_1={\stackrel{\·}{I}}_{\mathrm{yz}}^{\mathrm{in}/0}$ ${\mathrm{\β}}_2={\stackrel{\·}{I}}_z^{\mathrm{in}/0}$ ${\mathrm{\β}}_3=I_{\mathrm{yz}}^{\mathrm{in}/0}$ ${\mathrm{\β}}_4=I_z^{\mathrm{in}/0}$

${\mathrm{\β}}_5=\stackrel{\·}{a}{I}_{\mathrm{xz}}^{\mathrm{in}/0}+a{\stackrel{\·}{I}}_{\mathrm{xz}}^{\mathrm{in}/0}+\stackrel{\·}{b}{I}_{\mathrm{yz}}^{\mathrm{in}/0}+b{\stackrel{\·}{I}}_{\mathrm{yz}}^{\mathrm{in}/0}+\stackrel{\·}{c}{I}_z^{\mathrm{in}/0}+c{\stackrel{\·}{I}}_z^{\mathrm{in}/0}$

${\mathrm{\δ}}_1=a(I_y^{\mathrm{in}/0}-I_x^{\mathrm{in}/0})-2b{I}_{\mathrm{xy}}^{\mathrm{in}/0}-c{I}_{\mathrm{xz}}^{\mathrm{in}/0}$ ${\mathrm{\δ}}_2=a{I}_{\mathrm{yz}}^{\mathrm{in}/0}-b{I}_{\mathrm{xz}}^{\mathrm{in}/0}$

${\mathrm{\δ}}_3=-I_{\mathrm{xy}}^{\mathrm{in}/0}$ ${\mathrm{\δ}}_4=-I_{\mathrm{xz}}^{\mathrm{in}/0}$

${\mathrm{\δ}}_5=(a^2-b^2)I_{\mathrm{xy}}^{\mathrm{in}/0}+\mathrm{ab}(I_y^{\mathrm{in}/0}-I_x^{\mathrm{in}/0})+\mathrm{ac}{I}_{\mathrm{yz}}^{\mathrm{in}/0}-\mathrm{bc}{I}_{\mathrm{xz}}^{\mathrm{in}/0}$

$\left[\begin{array}{c}a\\ b\\ c\end{array}\right]=\left[\begin{array}{c}{W}_{\mathrm{mx}}\cos {\mathrm{\λ}}_y-W_{\mathrm{mz}}\sin {\mathrm{\λ}}_y\\ W_{\mathrm{my}}\\ W_{\mathrm{mx}}\sin {\mathrm{\λ}}_y+W_{\mathrm{mz}}\cos {\mathrm{\λ}}_y\end{array}\right]$

$\left[\begin{array}{c}\stackrel{\·}{a}\\ \stackrel{\·}{b}\\ \stackrel{\·}{c}\end{array}\right]\left[\begin{array}{c}{\stackrel{\·}{W}}_{\mathrm{mx}}\cos {\mathrm{\λ}}_y-W_{\mathrm{mx}}{\stackrel{\·}{\mathrm{\λ}}}_y\sin {\mathrm{\λ}}_y-{\stackrel{\·}{W}}_{\mathrm{mz}}\sin {\mathrm{\λ}}_y-W_{\mathrm{mz}}{\stackrel{\·}{\mathrm{\λ}}}_y\cos {\mathrm{\λ}}_y\\ {\stackrel{\·}{W}}_{\mathrm{my}}\\ {\stackrel{\·}{W}}_{\mathrm{mx}}\sin {\mathrm{\λ}}_y+W_{\mathrm{mx}}{\stackrel{\·}{\mathrm{\λ}}}_y\cos {\mathrm{\λ}}_y+{\stackrel{\·}{W}}_{\mathrm{mz}}\cos {\mathrm{\λ}}_y-W_{\mathrm{mz}}{\stackrel{\·}{\mathrm{\λ}}}_y\sin {\mathrm{\λ}}_y\end{array}\right]$

$\left[\begin{array}{ccc}{I}_x^{\mathrm{in}}& I_{\mathrm{xy}}^{\mathrm{in}}& I_{\mathrm{xz}}^{\mathrm{in}}\\ I_{\mathrm{xy}}^{\mathrm{in}}& I_y^{\mathrm{in}}& I_{\mathrm{yz}}^{\mathrm{in}}\\ I_{\mathrm{xz}}^{\mathrm{in}}& I_{\mathrm{yz}}^{\mathrm{in}}& I_z^{\mathrm{in}}\end{array}\right]$ 为内框连同光电负载的转动惯量；

$\left[\begin{array}{ccc}{I}_x^o& I_{\mathrm{xy}}^o& I_{\mathrm{xz}}^o\\ I_{\mathrm{xy}}^o& I_y^o& I_{\mathrm{yz}}^o\\ I_{\mathrm{xz}}^o& I_{\mathrm{yz}}^o& I_z^o\end{array}\right]$ 为外框转动惯量；

λ_z，λ_y分别为俯仰框架角和偏航框架角；

W_mx、W_my、W_mz为导弹旋转角速度；

T_oz为内框力矩电机产生的驱动力矩；

T_oy为外框力矩电机产生的驱动力矩。

2)建立半捷联寻的导引头框架动力学的滤波状态方程和测量方程定义状态变量

x₁＝λ_z

$x_2={\stackrel{\·}{\mathrm{\λ}}}_z$

x₃＝λ_y

$x_4={\stackrel{\·}{\mathrm{\λ}}}_y$

控制变量为：u＝[T_ox T_oy]^T

则其状态方程和测量方程为：

$\stackrel{\·}{x}=F(x,u)+w$

y＝[λ_x λ_y]^T＝Hx+v

其中

$F(x,u)=\left[\begin{array}{c}{x}_2\\ {\mathrm{part}}_1+{\mathrm{part}}_2\\ x_4\\ \frac{1}{{\mathrm{\α}}_3^f}[T_{\mathrm{oy}}-({\mathrm{\α}}_1^f+{\mathrm{\γ}}_1^f)x_4-({\mathrm{\α}}_2^f+{\mathrm{\γ}}_2^f)x_2-{\mathrm{\α}}_4^f{\stackrel{\·\·}{\mathrm{\λ}}}_z^f-{\mathrm{\α}}_5^f-{\mathrm{\γ}}_3^f\end{array}\right]$

$H=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right]$

${\mathrm{part}}_1=\frac{T_{\mathrm{oz}}-\frac{{\mathrm{\β}}_3^f}{{\mathrm{\α}}_3^f}{T}_{\mathrm{oy}}-[({\mathrm{\β}}_1^f+{\mathrm{\δ}}_1^f)-\frac{{\mathrm{\β}}_3^f({\mathrm{\α}}_1^f+{\mathrm{\γ}}_1^f)}{{\mathrm{\α}}_3^f}]x_4}{{\mathrm{\β}}_4^f-\frac{{\mathrm{\α}}_4^f{\mathrm{\β}}_3^f}{{\mathrm{\α}}_3^f}}$

${\mathrm{part}}_2=\frac{-[({\mathrm{\β}}_2^f+{\mathrm{\δ}}_2^f)-\frac{{\mathrm{\β}}_3^f({\mathrm{\α}}_2^f+{\mathrm{\γ}}_2^f)}{{\mathrm{\α}}_3^f}]x_2-{\mathrm{\δ}}_4^f{x}_2{x}_4-{\mathrm{\δ}}_3^f{x_4}^2-[({\mathrm{\β}}_5^f+{\mathrm{\β}}_5^f)-\frac{{\mathrm{\β}}_3^f({\mathrm{\α}}_5^f+{\mathrm{\γ}}_3^f)}{{\mathrm{\α}}_3^f}]}{{\mathrm{\β}}_4^f-\frac{{\mathrm{\α}}_4^f{\mathrm{\β}}_3^f}{{\mathrm{\α}}_3^f}}$

w为状态噪声，v为测量噪声；上标f表示该参数是采用滤波估计值或测量值计算得到的；滤波前，首先将上述微分方程转化为等效离散模型。

3)UKF滤波器初始化

x₀＝E(x₀)，P₀＝E[(x₀-x₀)(x₀-x₀)^T]

$W_0^{\left(m\right)}=\mathrm{\τ}/(\mathrm{\τ}+L)$

$W_0^{\left(p\right)}=\mathrm{\τ}/(\mathrm{\τ}+L)+1-{\mathrm{\α}}^2+\mathrm{\β}$

其中，m代表均值，p代表协方差，L为状态维数；

τ＝α²(L+κ)-L，τ是一个比例缩放参数，通常取一个很小的正值；κ为辅助尺度因子，应确保矩阵

为半正定矩阵；β用于体现x的分布，若x为高斯分布，β＝2。

4)计算2L+1个sigma点χ_i和它们的权值

${\mathrm{\χ}}_{k-1}=[{\stackrel{\‾}{x}}_{k-1},{\stackrel{\‾}{x}}_{k-1}-\mathrm{\γ}\sqrt{P_{k-1}},{\stackrel{\‾}{x}}_{k-1}+\mathrm{\γ}\sqrt{P_{k-1}}]$

$W_i^{\left(m\right)}=W_i^{\left(p\right)}=1/\left[2(L+\mathrm{\τ})\right],$ i＝1，2，…，2L

下标i代表第几个采样点；其中

是比例因子；

是矩阵(L+τ)P_x的平方根，它是对称的正定矩阵。

5)滤波器时间更新

χ_k/k-1＝F(χ_k-1，u_k-1，w_k-1)

${\stackrel{\‾}{x}}_k^{-}=\underset{i=0}{\overset{2L}{\mathrm{\Σ}}}{W}_i^{\left(m\right)}{\mathrm{\χ}}_{i,k/k-1}$

$P_k^{-}=\underset{i=0}{\overset{2L}{\mathrm{\Σ}}}{W}_i^{\left(p\right)}({\mathrm{\χ}}_{i,k/k-1}-{\stackrel{\‾}{x}}_k^{-}){({\mathrm{\χ}}_{i,k/k-1}-{\stackrel{\‾}{x}}_k^{-})}^T+Q$

Y_k/k-1＝H(χ_k/k-1，v_k)

${\stackrel{\‾}{y}}_k^{-}=\underset{i=0}{\overset{2L}{\mathrm{\Σ}}}{W}_i^{\left(m\right)}{Y}_{i,k/k-1}$

式中，Q为系统状态噪声的协方差矩阵。

6)滤波器测量更新

$P_{{\stackrel{\‾}{y}}_k{\stackrel{\‾}{y}}_k}=\underset{i=0}{\overset{2L}{\mathrm{\Σ}}}{W}_i^{\left(p\right)}(Y_{i,k/k-1}-{\stackrel{\‾}{y}}_k^{-}){(Y_{i,k/k-1}-{\stackrel{\‾}{y}}_k^{-})}^T+R$

$P_{{\stackrel{\‾}{x}}_k{\stackrel{\‾}{y}}_k}=\underset{i=0}{\overset{2L}{\mathrm{\Σ}}}{W}_i^{\left(p\right)}({\mathrm{\χ}}_{i,k/k-1}-{\stackrel{\‾}{x}}_k^{-}){({\mathrm{\χ}}_{i,k/k-1}-{\stackrel{\‾}{y}}_k^{-})}^T$

$K_k=P_{{\stackrel{\‾}{x}}_k{\stackrel{\‾}{y}}_k}{P}_{{\stackrel{\‾}{y}}_k{\stackrel{\‾}{y}}_k}^{-1}$

${\stackrel{\‾}{x}}_k={\stackrel{\‾}{x}}_k^{-}+K_k(y_k-{\stackrel{\‾}{y}}_k^{-})$

$P_k=P_k^{-}-K_k{P}_{{\stackrel{\‾}{y}}_k{\stackrel{\‾}{y}}_k}{K}_k^T$

式中，R为测量噪声的协方差矩阵。

这样，就得到了k时刻的状态x_k和协方差P_k，其中x_k的第二和第四项即为俯仰和偏航框架角速率。

7)构造制导信息，对导弹进行导引

半捷联寻的导引头的制导信息由弹体旋转角速率、框架角速度和目标在像平面的移动角速度组成。其中框架角速率由上述UKF滤波得到，弹体旋转角速率由弹上惯性测量组件得到，目标在像平面的移动角速度由探测器测得的误差角经滤波微分得到。

根据自动驾驶仪要求，将制导信息转换到所需要的坐标系中，即对导弹进行导引飞行。

8)判断是否已进入导引头盲区，若是则停止滤波，导弹导引信息归零；否则转到4)。

9)利用上述形成的制导信息，对导弹进行比例导引，进行某导弹末段寻的制导仿真，验证制导信息的构造精度。

仿真中加入框架角度测量噪声均方差为0.5度，弹体角速度测量噪声均方差为0.5度/秒，转动惯量拉偏10％，目标向负z轴方向机动，速度为10m/s，导弹初始位置为(0，500，0)，目标初始位置为(1000，0，500)。

采用本发明的构造方法得到制导信息，对导弹进行比例导引仿真，得到各特征变量变化曲线如图2-图6所示，脱靶量小于0.5米。

采用EKF滤波进行制导信息构造，并对导弹进行比例导引仿真，得到各特征变量变化曲线如图7-图11所示，脱靶量大于8米。由图可见，采用本发明构造制导信息，导引导弹，打击精度较高。

Claims (1)

1.一种半捷联寻的导引头的制导信息构造方法，其特征在于：

该方法是为了控制带有半捷联寻的导引头的导弹准确命中目标，需要构造制导信息；框架角速率信息是制导信息的重要组成部分，采用Unscented卡尔曼滤波器UKF（Unscented KalmanFilter）得到框架角速率比较准确的估计值；为了采用UKF进行滤波估计，首先需要建立半捷联寻的导引头框架动力学数学模型，然后建立框架动力学的滤波状态方程和测量方程，进行框架角速率信息的递推滤波估计；具体实现步骤如下：

1）建立半捷联寻的导引头框架动力学数学模型

根据动量矩定理，建立两框架半捷联寻的导引头的框架动力学模型如下：

${\stackrel{\·\·}{\mathrm{\λ}}}_z=\frac{T_{\mathrm{oz}}-\frac{{\mathrm{\β}}_3}{{\mathrm{\α}}_3}{T}_{\mathrm{oy}}-[({\mathrm{\β}}_1+{\mathrm{\δ}}_1)-\frac{{\mathrm{\β}}_3({\mathrm{\α}}_1+{\mathrm{\γ}}_1)}{{\mathrm{\α}}_3}]{\stackrel{\·}{\mathrm{\λ}}}_y-[({\mathrm{\β}}_2+{\mathrm{\δ}}_2)-\frac{{\mathrm{\β}}_3({\mathrm{\α}}_2+{\mathrm{\γ}}_2)}{{\mathrm{\α}}_3}]{\stackrel{\·}{\mathrm{\λ}}}_z-{\mathrm{\δ}}_4{\stackrel{\·}{\mathrm{\λ}}}_y{\stackrel{\·}{\mathrm{\λ}}}_z-{\mathrm{\δ}}_3{{\stackrel{\·}{\mathrm{\λ}}}_y}^2-[({\mathrm{\β}}_5+{\mathrm{\δ}}_5)-\frac{{\mathrm{\β}}_3({\mathrm{\α}}_5+{\mathrm{\γ}}_3)}{{\mathrm{\α}}_3}]}{{\mathrm{\β}}_4-\frac{{\mathrm{\α}}_4{\mathrm{\β}}_3}{{\mathrm{\α}}_3}}$

${\stackrel{\·\·}{\mathrm{\λ}}}_y=\frac{1}{{\mathrm{\α}}_3}[T_{\mathrm{oy}}-({\mathrm{\α}}_1+{\mathrm{\γ}}_1){\stackrel{\·}{\mathrm{\λ}}}_y-({\mathrm{\α}}_2+{\mathrm{\γ}}_2){\stackrel{\·}{\mathrm{\λ}}}_z-{\mathrm{\α}}_4{\stackrel{\·\·}{\mathrm{\λ}}}_z-{\mathrm{\α}}_5-{\mathrm{\γ}}_3]$

其中：

${\mathrm{\α}}_1={\stackrel{\·}{I}}_y^{\mathrm{in}/0}$ ${\mathrm{\α}}_2={\stackrel{\·}{I}}_{\mathrm{yz}}^{\mathrm{in}/0}$ ${\mathrm{\α}}_3=I_y^{\mathrm{in}/0}+I_y^0$ ${\mathrm{\α}}_4=I_{\mathrm{yz}}^{\mathrm{in}/0}$

${\mathrm{\α}}_5=\stackrel{\·}{a}(I_{\mathrm{xy}}^0+I_{\mathrm{xy}}^{\mathrm{in}/0})+a{\stackrel{\·}{I}}_{\mathrm{xy}}^{\mathrm{in}/0}+\stackrel{\·}{b}(I_y^0+I_y^{\mathrm{in}/0})+b{\stackrel{\·}{I}}_y^{\mathrm{in}/0}+\stackrel{\·}{c}(I_{\mathrm{yz}}^0+I_{\mathrm{yz}}^{\mathrm{in}/0})+c{\stackrel{\·}{I}}_{\mathrm{yz}}^{\mathrm{in}/0}$

${\mathrm{\γ}}_1=c(I_{\mathrm{xy}}^0+I_{\mathrm{xy}}^{\mathrm{in}/0})-a(I_{\mathrm{yz}}^0+I_{\mathrm{yz}}^{\mathrm{in}/0})$ ${\mathrm{\γ}}_2=c{I}_{\mathrm{xz}}^{\mathrm{in}/0}-a{I}_z^{\mathrm{in}/0}$

${\mathrm{\γ}}_3=\mathrm{ac}((I_x^0+I_x^{\mathrm{in}/0})-(I_z^0+I_z^{\mathrm{in}/0}))+\mathrm{bc}(I_{\mathrm{xy}}^0+I_{\mathrm{xy}}^{\mathrm{in}/0})+(c^2-a^2)(I_{\mathrm{xz}}^0+I_{\mathrm{xz}}^{\mathrm{in}/0})-\mathrm{ab}(I_{\mathrm{yz}}^0+I_{\mathrm{yz}}^{\mathrm{in}/0})$

${\mathrm{\β}}_1={\stackrel{\·}{I}}_{\mathrm{yz}}^{\mathrm{in}/0}$ ${\mathrm{\β}}_2={\stackrel{\·}{I}}_z^{\mathrm{in}/0}$ ${\mathrm{\β}}_3=I_{\mathrm{yz}}^{\mathrm{in}/0}$ ${\mathrm{\β}}_4=I_z^{\mathrm{in}/0}$

${\mathrm{\β}}_5=\stackrel{\·}{a}{I}_{\mathrm{xz}}^{\mathrm{in}/0}+a{\stackrel{\·}{I}}_{\mathrm{xz}}^{\mathrm{in}/0}+\stackrel{\·}{b}{I}_{\mathrm{yz}}^{\mathrm{in}/0}+b{\stackrel{\·}{I}}_{\mathrm{yz}}^{\mathrm{in}/0}+\stackrel{\·}{c}{I}_z^{\mathrm{in}/0}+c{\stackrel{\·}{I}}_z^{\mathrm{in}/0}$

${\mathrm{\δ}}_1=a(I_y^{\mathrm{in}/0}-I_x^{\mathrm{in}/0})-2b{I}_{\mathrm{xy}}^{\mathrm{in}/0}-c{I}_{\mathrm{xz}}^{\mathrm{in}/0}$ ${\mathrm{\δ}}_2=a{I}_{\mathrm{yz}}^{\mathrm{in}/0}-b{I}_{\mathrm{xz}}^{\mathrm{in}/0}$

${\mathrm{\δ}}_3=-I_{\mathrm{xy}}^{\mathrm{in}/0}$ ${\mathrm{\δ}}_4=-I_{\mathrm{xz}}^{\mathrm{in}/0}$

${\mathrm{\δ}}_5=(a^2-b^2)I_{\mathrm{xy}}^{\mathrm{in}/0}+\mathrm{ab}(I_y^{\mathrm{in}/0}-I_x^{\mathrm{in}/0})+\mathrm{ac}{I}_{\mathrm{yz}}^{\mathrm{in}/0}-\mathrm{bc}{I}_{\mathrm{xz}}^{\mathrm{in}/0}$

$\left[\begin{array}{c}a\\ b\\ c\end{array}\right]=\left[\begin{array}{c}{W}_{\mathrm{mx}}\cos {\mathrm{\λ}}_y-W_{\mathrm{mz}}\sin {\mathrm{\λ}}_y\\ W_{\mathrm{my}}\\ W_{\mathrm{mx}}\sin {\mathrm{\λ}}_y+W_{\mathrm{mz}}\cos {\mathrm{\λ}}_y\end{array}\right]$

$\left[\begin{array}{c}\stackrel{\·}{a}\\ \stackrel{\·}{b}\\ \stackrel{\·}{c}\end{array}\right]=\left[\begin{array}{c}{\stackrel{\·}{W}}_{\mathrm{mx}}\cos {\mathrm{\λ}}_y-W_{\mathrm{mx}}{\stackrel{\·}{\mathrm{\λ}}}_y\sin {\mathrm{\λ}}_y-{\stackrel{\·}{W}}_{\mathrm{mz}}\sin {\mathrm{\λ}}_y-W_{\mathrm{mz}}{\stackrel{\·}{\mathrm{\λ}}}_y\cos {\mathrm{\λ}}_y\\ {\stackrel{\·}{W}}_{\mathrm{my}}\\ {\stackrel{\·}{W}}_{\mathrm{mx}}\sin {\mathrm{\λ}}_y+W_{\mathrm{mx}}{\stackrel{\·}{\mathrm{\λ}}}_y\cos {\mathrm{\λ}}_y+{\stackrel{\·}{W}}_{\mathrm{mz}}\cos {\mathrm{\λ}}_y-W_{\mathrm{mz}}{\stackrel{\·}{\mathrm{\λ}}}_y\sin {\mathrm{\λ}}_y\end{array}\right]$

$\left[\begin{array}{ccc}{I}_x^{\mathrm{in}}& I_{\mathrm{xy}}^{\mathrm{in}}& I_{\mathrm{xz}}^{\mathrm{in}}\\ I_{\mathrm{xy}}^{\mathrm{in}}& I_y^{\mathrm{in}}& I_{\mathrm{yz}}^{\mathrm{in}}\\ I_{\mathrm{xz}}^{\mathrm{in}}& I_{\mathrm{yz}}^{\mathrm{in}}& I_z^{\mathrm{in}}\end{array}\right]$ 为内框连同光电负载的转动惯量；

$\left[\begin{array}{ccc}{I}_x^o& I_{\mathrm{xy}}^o& I_{\mathrm{xz}}^o\\ I_{\mathrm{xy}}^o& I_y^o& I_{\mathrm{yz}}^o\\ I_{\mathrm{xz}}^o& I_{\mathrm{yz}}^o& I_z^o\end{array}\right]$ 为外框转动惯量；

λ_z，λ_y分别为俯仰框架角和偏航框架角；

W_mx、W_my、W_mz为导弹旋转角速度；

T_oz为内框力矩电机产生的驱动力矩；

T_oy为外框力矩电机产生的驱动力矩；

2）建立半捷联寻的导引头框架动力学的滤波状态方程和测量方程定义状态变量

x₁＝λ_z

$x_2={\stackrel{\·}{\mathrm{\λ}}}_z$

x₃＝λ_y

$x_4={\stackrel{\·}{\mathrm{\λ}}}_y$

控制变量为：u＝[T_oz T_oy]^T

则其状态方程和测量方程为：

$\stackrel{\·}{x}=F(x,u)+w$

y＝[λ_z λ_y]^T＝H_x-y

w为状态噪声，v为测量噪声

其中 $H=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right]$

$F(x,u)=\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\λ}}}_z\\ \frac{T_{\mathrm{oz}}-\frac{{\mathrm{\β}}_3}{{\mathrm{\α}}_3}{T}_{\mathrm{oy}}-[({\mathrm{\β}}_1+{\mathrm{\δ}}_1)-\frac{{\mathrm{\β}}_3({\mathrm{\α}}_1+{\mathrm{\γ}}_1)}{{\mathrm{\α}}_3}]{\stackrel{\·}{\mathrm{\λ}}}_y-[({\mathrm{\β}}_2+{\mathrm{\δ}}_2)-\frac{{\mathrm{\β}}_3({\mathrm{\α}}_2+{\mathrm{\γ}}_2)}{{\mathrm{\α}}_3}]{\stackrel{\·}{\mathrm{\λ}}}_z-{\mathrm{\δ}}_4{\stackrel{\·}{\mathrm{\λ}}}_y{\stackrel{\·}{\mathrm{\λ}}}_z-{\mathrm{\δ}}_3{{\stackrel{\·}{\mathrm{\λ}}}_y}^2-[({\mathrm{\β}}_5+{\mathrm{\δ}}_5)-\frac{{\mathrm{\β}}_3({\mathrm{\α}}_5+{\mathrm{\γ}}_3)}{{\mathrm{\α}}_3}]}{{\mathrm{\β}}_4-\frac{{\mathrm{\α}}_4{\mathrm{\β}}_3}{{\mathrm{\α}}_3}}\\ {\stackrel{\·}{\mathrm{\λ}}}_y\\ \frac{1}{{\mathrm{\α}}_3}[T_{\mathrm{oy}}-({\mathrm{\α}}_1+{\mathrm{\γ}}_1){\stackrel{\·}{\mathrm{\λ}}}_y-({\mathrm{\α}}_2+{\mathrm{\γ}}_2){\stackrel{\·}{\mathrm{\λ}}}_z-{\mathrm{\α}}_4{\stackrel{\·\·}{\mathrm{\λ}}}_z-{\mathrm{\α}}_5-{\mathrm{\γ}}_3]\end{array}\right]$

滤波前，首先将上述微分方程转化为等效离散模型；

3）UKF滤波器初始化

${\stackrel{\‾}{x}}_0=E\left(x_0\right),P_0=E\left[(x_0-{\stackrel{\‾}{x}}_0){(x_0-{\stackrel{\‾}{x}}_0)}^T\right]$

$W_0^{\left(m\right)}=\mathrm{\τ}/(\mathrm{\τ}+L)$

$W_0^{\left(p\right)}=\mathrm{\τ}/(\mathrm{\τ}+L)+1-{\mathrm{\α}}^2+\mathrm{\β}$

其中，m代表均值，p代表协方差，L为状态维数

τ＝α²(L-κ)-L，τ是一个比例缩放参数，通常取一个很小的正值；κ为辅助尺度因子，应确保矩阵

为半正定矩阵；β用于体现x的分布，若x为高斯分布，β＝2；

4）计算2L+1个sigma点χ_i和它们的权值

${\mathrm{\χ}}_{k-1}=[{\stackrel{\‾}{x}}_{k-1},{\stackrel{\‾}{x}}_{k-1}-\mathrm{\γ}\sqrt{P_{k-1}},{\stackrel{\‾}{x}}_{k-1}+\mathrm{\γ}\sqrt{P_{k-1}}]$

$W_i^{\left(m\right)}=W_i^{\left(p\right)}=1/\left[2(L+\mathrm{\τ})\right],$ i＝1，2，…，2L

下标i代表第几个采样点；

是比例因子；

是矩阵(L+τ)P_x的平方根，它是对称的正定矩阵；

5）滤波器时间更新

${\text{\χ}}_{k/k-1}=F({\mathrm{\χ}}_{k-1},u_{k-1},w_{k-1})$

${\stackrel{\‾}{x}}_k^{-}\underset{i=0}{\overset{2L}{\mathrm{\Σ}}}{W}_i^{\left(m\right)}{\mathrm{\χ}}_{i,k/k-1}$

$P_k^{-}=\underset{i=0}{\overset{2L}{\mathrm{\Σ}}}{W}_i^{\left(p\right)}({\mathrm{\χ}}_{i,k/k-1}-{\stackrel{\‾}{x}}_k^{-}){({\mathrm{\χ}}_{i,k/k-1}-{\stackrel{\‾}{x}}_k^{-})}^T+Q$

$Y_{k/k-1}=H({\mathrm{\χ}}_{k/k-1},v_k)$

${\stackrel{\‾}{y}}_k^{-}=\underset{i=0}{\overset{2L}{\mathrm{\Σ}}}{W}_i^{\left(m\right)}{Y}_{i,k/k-1}$

式中，Q为系统状态噪声的协方差矩阵；

6)滤波器测量更新

$P_{y_k{y}_k}=\underset{i=0}{\overset{2L}{\mathrm{\Σ}}}{W}_i^{\left(p\right)}(Y_{i,k/k-1}-{\stackrel{\‾}{y}}_k^{-}){(Y_{i,k/k-1}-{\stackrel{\‾}{y}}_k^{-})}^T+R$

$P_{{\stackrel{\‾}{x}}_k{\stackrel{\‾}{y}}_k}=\underset{i=0}{\overset{2L}{\mathrm{\Σ}}}{W}_i^{\left(p\right)}({\mathrm{\χ}}_{i,k/k-1}-{\stackrel{\‾}{x}}_k^{-}){(Y_{i,k/k-1}-{\stackrel{\‾}{y}}_k^{-})}^T$

$K_k=P_{x_k{y}_k}{P}_{y_k{y}_k}^{-1}$

${\stackrel{\‾}{x}}_k={\stackrel{\‾}{x}}_k^{-}+k_k(y_k-{\stackrel{\‾}{y}}_k^{-})$

$P_k=P_k^{-}-K_k{P}_{{\stackrel{\‾}{y}}_k{\stackrel{\‾}{y}}_k}{K}_k^T$

式中，R为测量噪声的协方差矩阵。

这样，就得到了k时刻的状态和协方差P_k，其中的第二和第四项即为俯仰和偏航框架角速率；

7）构造制导信息，对导弹进行导引

半捷联寻的导引头的制导信息由弹体旋转角速率、框架角速度和目标在像平面的移动角速度组成；其中框架角速率由上述UKF滤波得到，弹体旋转角速率由弹上惯性测量组件得到，目标在像平面的移动角速度由探测器测得的误差角经滤波微分得到；

根据自动驾驶仪要求，将制导信息转换到所需要的坐标系中，即对导弹进行导引飞行；

8）判断是否已进入导引头盲区，若是则停止滤波，导弹导引信息归零；否则转到4）；

至此，通过上述8个步骤完成了基于UKF的半捷联寻的导引头制导信息的构造。

Images