一种半捷联雷达导引头视线角速度提取方法

摘要

本发明涉及一种半捷联雷达导引头视线角速度提取方法，在天线坐标系下建立目标运动微分方程，进而得到目标视线角速度信息，回避了旋转运动与转动次序相关问题，简化了处理过程，从而获取目标视线角速度信息。

说明书

技术领域

本发明属于半捷联雷达制导领域，主要涉及视线角速度信号提取及相关跟踪控制 技术。

背景技术

毫米波导引头主要用于近程对地、反舰导弹精确制导，对导引头体积、成本都有 严格要求，比例导引系统对视线角速度品质有着较高要求，常规雷达导引头采用两轴 速率陀螺稳定结构实现，利用陀螺的空间测速反馈实现稳定跟踪，为此要求伺服系统 必须选择高品质速率陀螺，给导引头小型化、低成本实现都带来相当困难。20世纪末 以来，国外开始出现半捷联制导体制导引头，如美国的AIM-9X空空导弹、德国的 IRIS-T导弹等，见参考文献“毛峡,张俊伟:半捷联导引头光轴稳定的研究,红外与 激光工程,Vol.36No.1,2007:9～12”。这种半捷联导引头伺服系统不包含速率陀螺，利用 弹体陀螺实现空间稳定，由于省去了高品质速率陀螺，有利于减小伺服系统载荷，利 于小型化实现，同时还可以大幅度降低导引头伺服系统的成本。与光学导引头相比， 雷达导引头还可以获得弹目相对距离，综合距离与角度跟踪信息可以进一步提高信号 输出品质，周瑞青等人提出一种用于半捷联雷达导引头的视线角速度提取方法，见参 考文献“周瑞青等,捷联导引头稳定与跟踪技术，北京：国防工业出版社，2010： 156～165”。

半捷联式天线平台的稳定控制环可以是位置环，也可以是速度环，对于比例导引 系统要求输出目标视线角速度信号，需要在速度环补偿弹体扰动以输出视线角速度信 号。

半捷联导引头的动力学数学模型非线性比较严重，EKF(ExtendedKalmanFilter) 滤波不能满足高精度制导要求，宋建梅等提出采用UKF(UncentedKalmanFilter)滤 波方法对框架角速率进行估计，见参考文献“宋建梅等，半捷联图像寻的制导系统导 引信息构造方法，兵工学报，vol(31),No.12,2010:1573～1579”，同时结合弹上IMU测量 得到的弹体角速率信息以及图像探测器处理得到的目标在像平面上的位置信息，构造 视线角速度信息。这种半捷联制导技术可以用于光学制导系统，由于雷达制导系统相 比光学系统还可以获取目标与导弹之间的距离信息，而上述系统则无法综合距离信息 改善系统输出。周瑞青、贾筱媛等提出基于Coriolis定理在惯性坐标系建立目标视线 角速度运动的微分动态方程，见参考文献“贾筱媛,半捷联稳定控制方案与制导信息 提取方法,红外与激光工程,Vol.40No.12,2011:2474～2479”，进而建立滤波方程提取目 标视线角速度，遗憾的是，这种滤波算法存在“先天不足”：滤波参数中不利用弹体扰 动信息，也没有传感器获取天线相对惯性空间的运动信息，竟然能够得到目标相对惯 性空间的视线角速度信息。这种处理的合理与否可以通过把问题简化到单平面情形来 验证，事实上，对于单平面情形很容易看出这种处理是不适用的，造成这种错误的根 本原因是忽略了有限转动的不可交换性引起的复杂性，见参考文献“秦永元，惯性导 航，北京：科学出版社，2006:8”。

目前，光学半捷联制导系统已有应用报道，半捷联雷达制导技术尚未见到应用报 道。本发明将在天线坐标系下建立目标视线运动微分方程，进而结合导弹弹体扰动信 息、天线相对弹体运动信息形成视线角速度信号，用于比例制导系统。

发明内容

要解决的技术问题

为了解决现有技术的不足，本发明提出了一种半捷联雷达导引头视线角速度提取 方法。

技术方案

一种半捷联雷达导引头视线角速度提取方法，其特征在于步骤如下：

步骤1：建立目标视线相对天线坐标运动的角速度和角误差微分方程：

$\stackrel{·}{\theta }={\omega }_y$

$\stackrel{·}{\psi }={\omega }_z$

${\stackrel{·}{\omega }}_y=-2\stackrel{·}{r}/{\mathrm{r\omega }}_y-{\mathrm{\psi a}}_{Ax}/r$

${\stackrel{·}{\omega }}_z=-2\stackrel{·}{r}/{\mathrm{r\omega }}_z+{\mathrm{\theta a}}_{Ax}/r$

其中，θ和ψ分别为俯仰和方位角误差，和分别为俯仰和方位角误差求微分， ω_y和ω_z分别为视线角速度在天线坐标系沿y、z轴分量，r为导引头与目标的接近距离， a_Ax为导引头横向加速度；

步骤2：将角速度和角误差微分方程转化为卡尔曼滤波消息模型，以矩阵形式表 达：

$\left(\begin{array}{c}\stackrel{·}{\theta }\\ \stackrel{·}{\psi }\\ {\omega }_y\\ {\omega }_z\end{array}\right)=\left(\begin{array}{cccc}0& 0& 1& 0\\ 0& 0& 0& 1\\ 0& -\frac{a_{Ax}}{r}& \frac{-2v}{r}& 0\\ \frac{a_{Ax}}{r}& 0& 0& \frac{-2v}{r}\end{array}\right)\left(\begin{array}{c}\theta \\ \psi \\ {\omega }_y\\ {\omega }_z\end{array}\right)+\left(\begin{array}{cc}1& 0\\ 0& 1\\ 0& 0\\ 0& 0\end{array}\right)\left(\begin{array}{c}{w}_y\\ w_z\end{array}\right)$

转化为卡尔曼滤波状态方程和测量方程：

$\stackrel{·}{x}=A\left(t\right)x\left(t\right)+Gw\left(t\right)$

z(t)＝Hx(t)+v(t)

其中，$\stackrel{·}{x}\left(t\right)={\left(\begin{array}{cccc}\stackrel{·}{\theta }& \stackrel{·}{\psi }& {\omega }_y& {\omega }_z\end{array}\right)}^T,$$A\left(t\right)=\left(\begin{array}{cccc}0& 0& 1& 0\\ 0& 0& 0& 1\\ 0& -\frac{a_{Ax}}{r}& \frac{-2v}{r}& 0\\ \frac{a_{Ax}}{r}& 0& 0& \frac{-2v}{r}\end{array}\right),G=\left(\begin{array}{cc}1& 0\\ 0& 1\\ 0& 0\\ 0& 0\end{array}\right),w\left(t\right)=\left(\begin{array}{c}{w}_y\\ w_z\end{array}\right),$$H=\left(\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right),v\left(t\right)=\left(\begin{array}{c}{v}_y\left(t\right)\\ v_z\left(t\right)\end{array}\right),$w_y和w_z分别为俯仰和方位两个方向角误差策动噪声， v_y和v_z分别为y和z两个通道的测量噪声；

步骤3：对卡尔曼滤波状态方程和测量方程进行离散化：

$\left(\begin{array}{c}{X}_{k+1}={\phi }_{k+1,k}{X}_k+{\Gamma }_{k+1,k}{W}_k\\ Z_{k+1}=H_{k+1}{X}_{k+1}+V_{k+1}\end{array}\right)$

其中：

$\left(\begin{array}{c}{\phi }_{k+1,k}=\left(\begin{array}{cccc}1& 0& T& 0\\ 0& 1& 0& T\\ 0& -\frac{a_{Ax}}{T}& 1-\frac{2v}{r}T& 0\\ \frac{a_{Ax}}{r}T& 0& 0& 1-\frac{2v}{r}T\end{array}\right)\\ {\Gamma }_{k+1,k}=\left(\begin{array}{cc}T& 0\\ 0& T\\ 0& -\frac{a_{Ax}}{2r}\\ \frac{a_{Ax}}{2r}{T}^2& 0\end{array}\right)\\ H_{K+1}=\left(\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right)\end{array}\right)$

T是采样周期，V_k+1为离散化的测量噪声；

采用自适应卡尔曼滤波算法求取视线相对于天线坐标系运动的目标视线角速度 ω_y和ω_z；

步骤4：分别计算俯仰和方位两个方向的视线角速度：

$\stackrel{·}{q}={\omega }_p^{B/I}+{\omega }_p^{O/B}+{\omega }_p+{\omega }_y$

$\stackrel{·}{q}={\omega }_p^{B/I}+{\omega }_p^{O/B}+{\omega }_p+{\omega }_z$

其中，是弹体相对惯性系旋转角速度在天线坐标系的投影，计算公式：

$\left(\begin{array}{c}{\omega }_P^{B/I}=T_{OP}{T}_{BO}{\omega }_B^{B/I}=\left(\begin{array}{ccc}{\mathrm{cos\lambda }}_z& {\mathrm{sin\lambda }}_z& 0\\ -{\mathrm{sin\lambda }}_z& {\mathrm{cos\lambda }}_z& 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{ccc}{\mathrm{cos\lambda }}_y& 0& -{\mathrm{sin\lambda }}_y\\ 0& 1& 0\\ {\mathrm{sin\lambda }}_y& 0& {\mathrm{cos\lambda }}_y\end{array}\right)\left(\begin{array}{c}{\omega }_{Bx}\\ {\omega }_{By}\\ {\omega }_{Bz}\end{array}\right)\\ =\left(\begin{array}{c}{\omega }_{Bx}{\mathrm{cos\lambda }}_z{\mathrm{cos\lambda }}_y+{\omega }_{By}{\mathrm{sin\lambda }}_z-{\omega }_{Bz}{\mathrm{cos\lambda }}_z{\mathrm{sin\lambda }}_y\\ -{\omega }_{Bx}{\mathrm{sin\lambda }}_z{\mathrm{cos\lambda }}_y+{\omega }_{By}{\mathrm{cos\lambda }}_z+{\omega }_{Bz}{\mathrm{sin\lambda }}_z{\mathrm{sin\lambda }}_y\\ {\omega }_{Bx}{\mathrm{sin\lambda }}_y+{\omega }_{Bz}{\mathrm{cos\lambda }}_y\end{array}\right)\end{array}\right)$

${\omega }_B^{B/I}={\left(\begin{array}{ccc}{\omega }_{Bx}& {\omega }_{By}& {\omega }_{Bz}\end{array}\right)}^T$为导弹相对于惯性空间的旋转角速率在弹体坐标系内的向 量，λ_y为外框相对于弹体的框架角，λ_z为内框相对于外框的框架角；

其中，是外框架相对于弹体旋转角速度在内框架坐标系的投影，计算公式：

${\omega }_p^{O/B}=\left(\begin{array}{c}{\omega }_{Ax}\\ {\omega }_{Ay}\\ {\omega }_{Az}\end{array}\right)=T_{OP}\left(\begin{array}{c}0\\ {\stackrel{·}{\lambda }}_y\\ 0\end{array}\right)=\left(\begin{array}{ccc}{\mathrm{cos\lambda }}_z& {\mathrm{sin\lambda }}_z& 0\\ -{\mathrm{sin\lambda }}_z& {\mathrm{cos\lambda }}_z& 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{c}0\\ {\stackrel{·}{\lambda }}_y\\ 0\end{array}\right)$

其中，${\omega }_O^{O/B}={\left(\begin{array}{ccc}0& {\stackrel{·}{\lambda }}_y& 0\end{array}\right)}^T$为外框相对于弹体的旋转角速率在外框坐标系的向量；

其中，ω_p是内框架相对于外框架的旋转角速度，计算公式：

${\omega }_p={\left(\begin{array}{ccc}0& 0& {\stackrel{·}{\lambda }}_z\end{array}\right)}^T$

${\omega }_O^{P/O}={\left(\begin{array}{ccc}0& 0& {\stackrel{·}{\lambda }}_z\end{array}\right)}^T$为内框相对于外框的旋转角速率在外框坐标系的向量。

有益效果

本发明提出的一种半捷联雷达导引头视线角速度提取方法，采用半捷联制导方式 可以省去昂贵的高精度速率陀螺，并有利于减小体积，在雷达导引头中结合距离测量 信息，由于雷达的距离测量精度很高，综合利用距离信息以后有利于提高输出信号精 度，已公开的雷达导引头半捷联制导技术存在理论上缺陷，无法用于实际系统，本发 明在天线坐标系下建立目标运动微分方程，进而得到目标视线角速度信息，回避了旋 转运动与转动次序相关问题，简化了处理过程，从而获取目标视线角速度信息。

附图说明

图1单平面跟踪制导

图2天线坐标系与伺服系统内外框架之间的关系

具体实施方式

现结合实施例、附图对本发明作进一步描述：

首先考虑最简单的单平面制导情形，如图1所示：

ψ_S目标相对惯性系的视线角

ψ_B导弹相对于惯性系的姿态角

ψ_L目标框架角

ψ_L＝ψ_S-ψ_B(1)

对上式两边求导：

ω_L＝ω_S-ω_B(2)

ω_L是相对弹体的天线转速，ω_S是目标视线角速度，ω_B是导弹弹体相对惯性空间旋 转速度。为了求解ω_S需要测量出ω_B、ω_L。

对于三维空间问题变得十分复杂：目标视线角速度不再是弹体姿态角速度和天线 相对弹体相对速度的简单相加。弹体相对惯性空间存在绕三个轴向的转动，导引头一 般采用两轴框架结构天线相对弹体有方位和俯仰转动，当这些转角不都是小角度时， 对应于不同的旋转次序，对应的空间指向位置是不同的。举例来说，天线先在方位上 转30°再转俯仰20°与先转俯仰20°后转方位30°空间指向是不同的。因此，为了 达到相同的指向，由于旋转次序不同也会导致多种旋转方式的存在。这意味着，即使 可以利用矩阵运算来描述弹体转动对目标视线运动的影响，求解也会变得十分困难。 (考虑到导弹弹体旋转与天线旋转，可能的旋转次序多达120种！)。半捷联处理算法 的有效性的前提是把问题简化到单平面，需要满足上面的公式(2)，事实上，周瑞青 等人的算法简化到单平面以后是无法满足(2)式的。

当转角为小量时，旋转后坐标系的最终角位置与旋转次序无关，这就是所说的无 限转动与旋转次序无关^[5]，围绕x,y,z三个轴向旋转角决定的坐标变换矩阵具有唯一性， 而与旋转次序无关：

$T=\left(\begin{array}{ccc}1& \psi & -\theta \\ -\psi & 1& \phi \\ \theta & -\phi & 1\end{array}\right)---\left(3\right)$

这里φ,θ,ψ分别为绕x,y,z轴的微小转角。

由于目标跟踪过程中天线指向和目标真实视线之间的夹角总是很小，为了简化问 题求解，考虑在天线坐标系下建立目标视线角动态微分方程，简化问题求解。

在天线坐标系下考察视线角速度信号组成，包括以下分量：

1)弹体相对惯性空间的旋转角速度在天线坐标系的投影；

2)外框架相对弹体角速度对天线坐标系的投影；

3)内框架相对外框架角速度；

4)目标视线相对天线坐标系角速度，即：

$\stackrel{·}{q}={\omega }_p^{B/I}+{\omega }_p^{O/B}+{\omega }_p+{\omega }_p^{S/P}---\left(4\right)$

这里是弹体相对惯性系旋转角速度在天线坐标系的投影，是外框架相对 于弹体旋转角速度在内框架坐标系的投影，ω_p是内框架相对于外框架的旋转角速度， 是视线相对于内框架坐标系的旋转角速度。

为了定量描述弹体、内外框架以及目标运动引入以下坐标系：

弹体坐标系B：坐标原点O在天线旋转中心(通常定义为导弹质心，这里为简 化问题定义在天线旋转中心)，x_b轴与弹体纵轴重合，指向导弹头部；y_b轴位于弹体 纵向对称面内并垂直与x_b轴，指向上方，z_b轴与上两轴垂直并构成右手坐标系。

外框架坐标系O(偏航)：坐标原点为天线旋转中心O，x_o轴垂直与外框架平面， 指向目标；y_o轴与弹体坐标系y_b轴重合；z_o轴与其它两轴构成右手坐标系。

内框架坐标系P(俯仰，即天线坐标系)：坐标原点为天线旋转中心O，x_p轴与内 框架平面垂直，z_p轴与外框架坐标系z_o轴重合，y_p与其它两轴构成右手坐标系。

天线坐标系A：坐标原点O在天线旋转中心，x_a沿天线的法线方向，y_a和z_a分别 对应于单脉冲天线的俯仰、方位角误差对称轴线，为了简化问题，这里假定y_a和y_p、 z_a和z_p重合，实际中可以通过天线安装予以保证。这样天线坐标系实际上与内框架坐 标系是一致的。

视线坐标系S：坐标原点O在天线旋转中心，x_s沿目标视线方向，y_s和z_s在与x_s垂 直的平面内，分别由天线坐标系的y_a绕z_a轴旋转ψ角，z_a绕y_a旋转θ角获得，这里ψ， θ均为小角度，因而可以不考虑转动次序。

为了简化问题，下面的讨论中假定天线电轴与机械轴重合(通常二者之间误差很 小，故此处予以忽略)。

外框相对于弹体的框架角用λ_y表示，内框相对于外框的框架角为λ_z。

由弹体坐标系到外框坐标系的变换矩阵为：

$T_{BO}=\left(\begin{array}{ccc}{\mathrm{cos\lambda }}_y& 0& -{\mathrm{sin\lambda }}_y\\ 0& 1& 0\\ {\mathrm{sin\lambda }}_y& 0& {\mathrm{cos\lambda }}_y\end{array}\right)---\left(5\right)$

外框到内框的坐标变换矩阵为：

$T_{OP}=\left(\begin{array}{ccc}{\mathrm{cos\lambda }}_z& {\mathrm{sin\lambda }}_z& 0\\ {\mathrm{sin\lambda }}_z& {\mathrm{cos\lambda }}_z& 0\\ 0& 0& 1\end{array}\right)---\left(6\right)$

导弹相对于惯性空间的旋转角速率在弹体坐标系内的向量表示为：

${\omega }_B^{B/I}={\left(\begin{array}{ccc}{\omega }_{Bx}& {\omega }_{By}& {\omega }_{Bz}\end{array}\right)}^T---\left(7\right)$

外框相对于弹体的旋转角速率在外框坐标系的向量表示为：

${\omega }_O^{O/B}={\left(\begin{array}{ccc}0& {\stackrel{·}{\lambda }}_y& 0\end{array}\right)}^T---\left(8\right)$

内框相对于外框的旋转角速率在外框坐标系的向量表示为：

${\omega }_O^{P/O}={\left(\begin{array}{ccc}0& 0& {\stackrel{·}{\lambda }}_z\end{array}\right)}^T---\left(9\right)$

弹体相对于惯性空间的旋转角速率在内框坐标系下的向量表示为：

$\begin{array}{c}{\omega }_P^{B/I}=T_{OP}{T}_{BO}{\omega }_B^{B/I}=\left(\begin{array}{ccc}{\mathrm{cos\lambda }}_z& {\mathrm{sin\lambda }}_z& 0\\ -{\mathrm{sin\lambda }}_z& {\mathrm{cos\lambda }}_z& 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{ccc}{\mathrm{cos\lambda }}_y& 0& -{\mathrm{sin\lambda }}_y\\ 0& 1& 0\\ {\mathrm{sin\lambda }}_y& 0& {\mathrm{cos\lambda }}_y\end{array}\right)\left(\begin{array}{c}{\omega }_{Bx}\\ {\omega }_{By}\\ {\omega }_{Bz}\end{array}\right)\\ =\left(\begin{array}{c}{\omega }_{Bx}{\mathrm{cos\lambda }}_z{\mathrm{cos\lambda }}_y+{\omega }_{By}{\mathrm{sin\lambda }}_z-{\omega }_{Bz}{\mathrm{cos\lambda }}_z{\mathrm{sin\lambda }}_y\\ -{\omega }_{Bx}{\mathrm{sin\lambda }}_z{\mathrm{cos\lambda }}_y+{\omega }_{By}{\mathrm{cos\lambda }}_z+{\omega }_{Bz}{\mathrm{sin\lambda }}_z{\mathrm{sin\lambda }}_y\\ {\omega }_{Bx}{\mathrm{sin\lambda }}_y+{\omega }_{Bz}{\mathrm{cos\lambda }}_y\end{array}\right)\end{array}---\left(10\right)$

天线外框架角速率在内框坐标系的投影为：

${\omega }_p^{O/B}=\left(\begin{array}{c}{\omega }_L\\ {\omega }_{Ay}\\ {\omega }_{Az}\end{array}\right)=T_{OP}\left(\begin{array}{c}0\\ {\stackrel{·}{\lambda }}_y\\ 0\end{array}\right)=\left(\begin{array}{ccc}{\mathrm{cos\lambda }}_z& {\mathrm{sin\lambda }}_z& 0\\ -{\mathrm{sin\lambda }}_z& {\mathrm{cos\lambda }}_z& 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{c}0\\ {\stackrel{·}{\lambda }}_y\\ 0\end{array}\right)---\left(11\right)$

天线内框架相对外框架的转速为${\omega }_p={\left(\begin{array}{ccc}0& 0& {\stackrel{·}{\lambda }}_z\end{array}\right)}^T.$是对λ_z的求导

综合考虑内外框架影响则有：

$\left(\begin{array}{c}{\omega }_{Px}\\ {\omega }_{Py}\\ {\omega }_{Pz}\end{array}\right)=\left(\begin{array}{c}{\stackrel{·}{\lambda }}_{y}sin{\lambda }_z\\ {\stackrel{·}{\lambda }}_y{\mathrm{cos\lambda }}_z\\ {\stackrel{·}{\lambda }}_z\end{array}\right)---\left(12\right)$

目标视线角速度在天线坐标系的投影关系比较复杂，需要首先建立角误差、角速 度运动微分方程，通过求解微分方程组得到在天线坐标系的角速度运动信息。

1)角误差动态方程

导引头目标跟踪就是调整内框架坐标系S_P(r,e,d)(r,e,d为坐标系轴线方向)使输 出角误差尽可能小的过程。

由于雷达导引头的角误差测量是在天线坐标系内完成，而跟踪控制则是在弹体坐 标系内完成，严格意义上说天线测量的角误差是不能直接用于伺服控制的。在弹体坐 标内视线方向可以用下面的向量表示：

${\hat{i}}_s^B=E{\hat{i}}_s^e---\left(13\right)$

即弹体坐标系的单位向量与天线坐标系单位向量之间满足如下变换矩阵：

$\left(\begin{array}{c}{i}_s^b\\ j^b\\ k^b\end{array}\right)=E\left(\begin{array}{c}{i}_s^e\\ j_s^e\\ k_s^e\end{array}\right)---\left(14\right)$

利用T_BO,T_OP矩阵有：

$\begin{array}{c}E=\left(\begin{array}{ccc}{\mathrm{cos\lambda }}_y& 0& -{\mathrm{sin\lambda }}_y\\ 0& 1& 0\\ {\mathrm{sin\lambda }}_y& 0& {\mathrm{cos\lambda }}_y\end{array}\right)\left(\begin{array}{ccc}{\mathrm{cos\lambda }}_z& {\mathrm{sin\lambda }}_z& 0\\ -{\mathrm{sin\lambda }}_z& {\mathrm{cos\lambda }}_z& 0\\ 0& 0& 1\end{array}\right)\\ =\left(\begin{array}{ccc}{\mathrm{cos\lambda }}_y{\mathrm{cos\lambda }}_z& {\mathrm{cos\lambda }}_y{\mathrm{sin\lambda }}_z& -{\mathrm{sin\lambda }}_y\\ -{\mathrm{sin\lambda }}_z& {\mathrm{cos\lambda }}_z& 0\\ {\mathrm{sin\lambda }}_y{\mathrm{cos\lambda }}_z& {\mathrm{sin\lambda }}_y{\mathrm{sin\lambda }}_z& {\mathrm{cos\lambda }}_y\end{array}\right)\end{array}---\left(15\right)$

如果天线坐标系内，目标指向用天线电轴(假定机械轴与电轴重合)指向为在角误差为小量条件下，图中的俯仰和方位角误差分别有下式：

即目标视线方向单位矢量在方位、俯仰轴的投影。

弹体坐标系中目标指向估计值为(用天线坐标系分量表示)：

${\hat{i}}_s^b=\left(\begin{array}{ccc}cos{\hat{\lambda }}_{y}cos{\hat{\lambda }}_z& cos{\hat{\lambda }}_{y}sin{\hat{\lambda }}_z& -sin{\hat{\lambda }}_y\end{array}\right)\left(\begin{array}{c}{i}_s^e\\ j_s^e\\ k_s^e\end{array}\right)---\left(16\right)$

这里为目标在弹体坐标系下的视线方向估计。

弹体坐标系内天线实际指向由方位和俯仰轴决定：

$j_a^b=\left(\begin{array}{ccc}-{\mathrm{sin\lambda }}_z& {\mathrm{cos\lambda }}_z& 0\end{array}\right)\left(\begin{array}{c}{i}_s^e\\ j_s^e\\ k_s^e\end{array}\right)---\left(17\right)$

$k_a^b=\left(\begin{array}{ccc}{\mathrm{sin\lambda }}_y{\mathrm{cos\lambda }}_z& {\mathrm{sin\lambda }}_y{\mathrm{sin\lambda }}_z& {\mathrm{cos\lambda }}_y\end{array}\right)\left(\begin{array}{c}{i}_s^e\\ j_s^e\\ k_s^e\end{array}\right)---\left(18\right)$

天线的俯仰和方位角误差分别为：

$\begin{array}{c}{i}_s^b·j_a^b=-\cos {\hat{\lambda }}_y\cos {\hat{\lambda }}_z{\mathrm{sin\lambda }}_z+\cos {\hat{\lambda }}_y\sin {\hat{\lambda }}_z{\mathrm{cos\lambda }}_z\\ =\cos {\hat{\lambda }}_y[\sin {\hat{\lambda }}_z{\mathrm{cos\lambda }}_z-\cos {\hat{\lambda }}_z{\mathrm{sin\lambda }}_z]\\ =\cos {\hat{\lambda }}_y\sin ({\hat{\lambda }}_z-{\lambda }_z)\\ \approx \cos {\hat{\lambda }}_y({\hat{\lambda }}_z-{\lambda }_z)\\ =\theta \cos {\hat{\lambda }}_z\end{array}---\left(19\right)$

$\begin{array}{c}{i}_s^b·k_a^b=\cos {\hat{\lambda }}_y\cos {\hat{\lambda }}_z{\mathrm{sin\lambda }}_y{\mathrm{cos\lambda }}_z+\cos {\hat{\lambda }}_y\sin {\hat{\lambda }}_z{\mathrm{sin\lambda }}_y{\mathrm{sin\lambda }}_z-\sin {\hat{\lambda }}_y{\mathrm{cos\lambda }}_y\\ =\cos {\hat{\lambda }}_y{\mathrm{sin\lambda }}_y[{\mathrm{cos\lambda }}_z\cos {\hat{\lambda }}_z+\sin {\hat{\lambda }}_z{\mathrm{sin\lambda }}_z]-\sin {\hat{\lambda }}_y{\mathrm{cos\lambda }}_y\\ =\cos {\hat{\lambda }}_y{\mathrm{sin\lambda }}_y\cos ({\hat{\lambda }}_z-{\lambda }_z)-\sin {\hat{\lambda }}_y{\mathrm{cos\lambda }}_y\\ =\cos {\hat{\lambda }}_y{\mathrm{sin\lambda }}_y-\sin {\hat{\lambda }}_y{\mathrm{cos\lambda }}_y\\ =\sin ({\hat{\lambda }}_y-{\lambda }_y)\\ \approx \psi \end{array}---\left(20\right)$

上面的结果表明，角误差与方位俯仰框架控制量直接相关，可以直接用角误差控 制框架角转动。

对于两轴伺服平台可以忽略围绕视线轴的旋转运动(即绕x轴转角由天线坐标系到视线坐标系的坐标变换方程为：

$\left(\begin{array}{c}{S}_x\\ S_y\\ S_z\end{array}\right)=C_A^S\left(\begin{array}{c}{A}_x\\ A_y\\ A_z\end{array}\right)---\left(21\right)$

坐标变换矩阵：

上面的近似中忽略了高阶无穷小量。

由于天线坐标系和视线坐标系原点是相同的，可以用坐标的旋转角速度来表征相 对运动。ω_s是S_S相对于惯性空间的旋转角速度矩阵，ω_A是S_A相对于惯性空间的旋转 角速度矩阵。

${\omega }_S=\left(\begin{array}{ccc}0& {\omega }_{Sz}& -{\omega }_{Sy}\\ -{\omega }_{Sz}& 0& {\omega }_{Sx}\\ {\omega }_{Sy}& -{\omega }_{Sx}& 0\end{array}\right)---\left(23\right)$

${\omega }_A=\left(\begin{array}{ccc}0& {\omega }_{Az}& -{\omega }_{Ay}\\ -{\omega }_{Az}& 0& {\omega }_{Ax}\\ {\omega }_{Ay}& -{\omega }_{Ax}& 0\end{array}\right)---\left(24\right)$

目标跟踪过程中绕视线轴的角速度一般很小，可以忽略，即ω_Sx≈0，ω_Ax≈0速度 矩阵可以近似为：

${\omega }_S=\left(\begin{array}{ccc}0& {\omega }_{{\mathrm{Sz}}^{\prime }}& -{\omega }_{Sy}\\ -{\omega }_{Sz}& 0& 0\\ {\omega }_{Sy}& 0& 0\end{array}\right)---\left(25\right)$

${\omega }_A=\left(\begin{array}{ccc}0& {\omega }_{Az}& -{\omega }_{Ay}\\ -{\omega }_{Az}& 0& 0\\ {\omega }_{Ay}& 0& 0\end{array}\right)---\left(26\right)$

利用刚体绕定点运动的速度分布公式，变换矩阵

${\stackrel{·}{C}}_A^S={\omega }_S{C}_A^S-C_A^S{\omega }_A---\left(27\right)$

通常，跟踪误差很小，(22)式中的近似条件成立。

把(22)、(23)、(24)式代入(27)式，可以得到如下角误差微分方程：

$\stackrel{·}{\theta }\approx {\omega }_{Sy}-{\omega }_{Ay}---\left(28\right)$

$\stackrel{·}{\psi }\approx {\omega }_{Sz}-{\omega }_{Az}---\left(29\right)$

在天线坐标系中目标视线角速度为：

ω＝ω_S-ω_A，(30)

ω_x＝ω_Sx-ω_Ax(31)

ω_y＝ω_Sy-ω_Ay(32)

ω_z＝ω_Sz-ω_Az(33)

上式中ω_xω_yω_z分别为视线角速度在天线坐标系沿x，y，z轴分量。

根据方程(28)、(29)、(32)、(33)有：

$\stackrel{·}{\theta }={\omega }_y---\left(34\right)$

$\stackrel{·}{\psi }={\omega }_z---\left(35\right)$

2)角速度动态方程

根据目标在惯性坐标系中的加速度方程，在惯性坐标系下建立目标视线角速度动 态微分方程:

${\stackrel{·}{R}}_T={\stackrel{·}{R}}_S+\stackrel{·}{R}+{\omega }_S\times \stackrel{·}{R}---\left(36\right)$

目标在惯性坐标系的速度向量；

视线坐标系原点相对于惯性系的速度向量；

目标相对视线坐标系的速度矢量；

ω_S视线坐标系相对惯性系的角速度向量。

对(36)式求导，即可建立目标在惯性系的加速度公式，又称Coriolis方程

${\stackrel{··}{R}}_T={\stackrel{··}{R}}_S+\stackrel{··}{R}+{\stackrel{·}{\omega }}_S\times R+2{\omega }_S\times \stackrel{·}{R}+{\omega }_S\times ({\omega }_S\times R)---\left(37\right)$

将上式展成分量形式，并忽略高阶小量以及绕视线轴向转动可以得到如下微分方 程：

${\stackrel{·}{\omega }}_{Sy}=-(2{\stackrel{·}{r}}^{\prime },/r^{\prime }){\omega }_{Sy}-(1/r)(a_{{\mathrm{Tz}}^{\prime }}-a_{{\mathrm{Iz}}^{\prime }})---\left(38\right)$

${\stackrel{·}{\omega }}_{Sz}=-(2{\stackrel{·}{r}}^{\prime },/r^{\prime }){\omega }_{Sz}+(1/r)(a_{{\mathrm{Ty}}^{\prime }}-a_{{\mathrm{Iy}}^{\prime }})---\left(39\right)$

这里y',z'分别对应视线坐标系与视线方向垂直平面内两个坐标轴。

类似的，对于导引头天线轴线运动也可以在惯性坐标系中建立类似的微分方程：

${\stackrel{·}{\omega }}_{Ay}=-(2\stackrel{·}{r}/r){\omega }_{Ay}-(1/r)(a_{Tz}-a_{Iz})---\left(40\right)$

${\stackrel{·}{\omega }}_{Az}=-(2\stackrel{·}{r}/r){\omega }_{Sz}+(1/r)(a_{Ty}-a_{Iy})---\left(41\right)$

弹目相对加速度在视线坐标系下描述为：

a_Sz′＝a_Tz′-a_Iz′(42)

a_Sy′＝a_Ty′-a_Iy′(43)

在天线坐标系下描述为：

a_Az＝a_Tz-a_Iz(44)

a_Ay＝a_Ty-a_Iy(45)

视线坐标系只是一个假想的坐标系，无法直接测量，因此视线坐标系下的加速度 需要利用能够直接测量的天线坐标系分量进行描述：

$\left(\begin{array}{c}{a}_{{\mathrm{Sx}}^{\prime }}\\ a_{{\mathrm{Sy}}^{\prime }}\\ a_{{\mathrm{Sz}}^{\prime }}\end{array}\right)=\left(\begin{array}{ccc}1& \theta & -\psi \\ -\theta & 1& 0\\ \psi & 0& 1\end{array}\right)\left(\begin{array}{c}{a}_{Ax}\\ a_{Ay}\\ a_{Az}\end{array}\right)---\left(46\right)$

由于地面、海面目标加速度都是很小，可以忽略，因而上式中的加速度都是由弹 体加速度导致，可以直接测量得到。

由(38)、(39)与(40)、(41)式两边对应相减，并结合(32)、(33)式，可以 得到视线相对天线坐标系的动态微分方程：

${\stackrel{·}{\omega }}_y=-2\stackrel{·}{r}/{\mathrm{r\omega }}_y-{\mathrm{\psi a}}_{Ax}/r---\left(47\right)$

${\stackrel{·}{\omega }}_z=-2\stackrel{·}{r}/{\mathrm{r\omega }}_z+{\mathrm{\theta a}}_{Ax}/r---\left(48\right)$

定义状态向量x(t)＝[θψω_yω_z]^T

进一步可以写出Kalman滤波消息模型为：

$\stackrel{·}{\theta }\approx {\omega }_y+w_y---\left(49\right)$

$\stackrel{·}{\psi }\approx {\omega }_z+w_z---\left(50\right)$

${\stackrel{·}{\omega }}_y=-2\stackrel{·}{r}/{\mathrm{r\omega }}_y-{\mathrm{\psi a}}_{Ax}/r---\left(51\right)$

${\stackrel{·}{\omega }}_z=-2\stackrel{·}{r}/{\mathrm{r\omega }}_z+{\mathrm{\theta a}}_{Ax}/r---\left(52\right)$

其中w_y和w_z分别为俯仰和方位两个方向角误差策动噪声。(49～52)式改写为矩阵 形式：

$\left(\begin{array}{c}\stackrel{·}{\theta }\\ \stackrel{·}{\psi }\\ {\omega }_y\\ {\omega }_z\end{array}\right)=\left(\begin{array}{cccc}0& 0& 1& 0\\ 0& 0& 0& 1\\ 0& -\frac{a_{Ax}}{r}& \frac{-2v}{r}& 0\\ \frac{a_{Ax}}{r}& 0& 0& \frac{-2v}{r}\end{array}\right)\left(\begin{array}{c}\theta \\ \psi \\ {\omega }_y\\ {\omega }_z\end{array}\right)+\left(\begin{array}{cc}1& 0\\ 0& 1\\ 0& 0\\ 0& 0\end{array}\right)\left(\begin{array}{c}{w}_y\\ w_z\end{array}\right)---\left(53\right)$

这里，$A\left(t\right)=\left(\begin{array}{cccc}0& 0& 1& 0\\ 0& 0& 0& 1\\ 0& -\frac{a_{Ax}}{r}& \frac{-2v}{r}& 0\\ \frac{a_{Ax}}{r}& 0& 0& \frac{-2v}{r}\end{array}\right),G=\left(\begin{array}{cc}1& 0\\ 0& 1\\ 0& 0\\ 0& 0\end{array}\right),w\left(t\right)=\left(\begin{array}{c}{w}_y\\ w_z\end{array}\right)$

进一步用向量表示为：

$\stackrel{·}{x}=A\left(t\right)x\left(t\right)+Gw\left(t\right)---\left(54\right)$

系统的测量方程为：

z(t)＝Hx(t)+v(t)(55)

其中，$H=\left(\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right)v\left(t\right)=\left(\begin{array}{c}{v}_y\left(t\right)\\ v_z\left(t\right)\end{array}\right)$

v_y,v_z分别为y和z两个通道的测量噪声。

Kalman滤波值为：

$\hat{x}=A\hat{X}+K_C(z-H\hat{x})---\left(56\right)$

其中Kalman滤波增益

K_c＝PH^TR^-1(57)

P满足下面的矩阵利卡提代数方程：

AP+PA^T+Q-PH^TR^-1HP＝0(58)

实际中还需要进一步把上述连续时间Kalman滤波模型离散化。

根据线性系统理论，(56)式中的状态方程的解为

$X\left(t\right)=\Phi (t,t_0)X\left(t_0\right)+\underset{t_0}{\overset{t}{\int }}\Phi (t,\tau )Gw\left(\tau \right)d\tau ---\left(59\right)$

上式的离散形式为：

$x\left(t_{k+1}\right)=\Phi (t_{k+1},t_k)x\left(t_k\right)+\underset{t_k}{\overset{t_{k+1}}{\int }}\Phi (t_{k+1},\tau )G\left(\tau \right)w\left(\tau \right)d\tau ---\left(60\right)$

其中，一步转移矩阵

$\stackrel{·}{\Phi }(t,t_k)=A\left(t\right)\Phi (t,t_k),---\left(61\right)$

Φ(t_k,t_k)＝I(62)

求解方程(61),(62)可以得到：

$\Phi \left(t_{k+1}{t}_k\right)=e^{{\int }_{t_k}^{t_{k+1}}A\left(t\right)dt}---\left(63\right)$

通常时间周期T＝t_k+1-t_k总是很短，可以近似认为状态参量为常数，即：

A(t)≈A(t_k)(t_k≤t≤t_k+1)(64)

$\Phi \left(t_{k+1}{t}_k\right)=e^{{\int }_{t_k}^{t_{k+1}}A\left(t\right)dt}\approx e^{TA\left(t_k\right)}\approx I+TA\left(t_k\right)+\frac{T^2}{2!}{A}^2\left(t_k\right)+...\left(65\right)$

$w\left(t_k\right)={\int }_{t_k}^{t_{k+1}}\Phi (t_{t+1},\tau )Gw\left(\tau \right)d\tau ---\left(66\right)$

经过进一步离散化得出离散化系统状态方程与测量方程：

$\left(\begin{array}{c}{X}_{k+1}={\phi }_{k+1,k}{X}_k+{\Gamma }_{k+1,k}{W}_k\\ Z_{k+1}=H_{k+1}{X}_{k+1}+V_{k+1}\end{array}\right)---\left(67\right)$

其中W_k为系统噪声；V_k+1为测量噪声；各系数矩阵为：

$\left(\begin{array}{c}{\phi }_{k+1,k}=\left(\begin{array}{cccc}1& 0& T& 0\\ 0& 1& 0& T\\ 0& -\frac{a_{Ax}}{T}& 1-\frac{2v}{r}T& 0\\ \frac{a_{Ax}}{r}T& 0& 0& 1-\frac{2v}{r}T\end{array}\right)\\ {\Gamma }_{k+1,k}=\left(\begin{array}{cc}T& 0\\ 0& T\\ 0& -\frac{a_{Ax}}{2r}\\ \frac{a_{Ax}}{2r}{T}^2& 0\end{array}\right)\\ H_{K+1}=\left(\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right)\end{array}\right)$

具体实施步骤如下：

(1)由式(4)可知，视线角速度包含弹体相对惯性空间转动角速度在天线坐标系的 投影、内外框架角运动在天线坐标系的投影、目标视线相对天线坐标系的运动等四部 分组成。其中，除目标视线相对天线坐标系的运动无法直接获取，其他分量参见文献 “宋建梅等，半捷联图像寻的制导系统导引信息构造方法，兵工学报， vol(31),No.12,2010:1573～1579”是可以通过测量直接得到的，对于雷达导引头，还可 以利用到距离信息，提高输出信号精度。

(2)对目标视线相对天线坐标运动建立角速度和角误差微分方程，从而建立卡尔曼滤 波状态方程(54)和测量方程(55)。

(3)根据离散化模型(67)，得到自适应卡尔曼滤波算法，从而求得视线相对于天线 坐标系运动的目标视线角速度，实现制导信息的提取。