[发明专利]一种半捷联雷达导引头视线角速度提取方法

权利要求书

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

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

θ·=ωy]]>

ψ·=ωz]]>

ω·y=-2r·/rωy-ψaAx/r]]>

ω·z=-2r·/rωz+θaAx/r]]>

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

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

θ·ψ·ωyωz=001000010-aAxr-2vr0aAxr00-2vrθψωyωz+10010000wywz]]>

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

x·(t)=A(t)x(t)+Gw(t)]]>

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

其中，w_y和w_z分别为俯仰和方位两个方向角误差策动噪声，v_y(t)和v_z(t)分别为y和z两个通道的测量噪声；

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

Xk+1=φk+1,kXk+Γk+1,kWkZk+1=Hk+1Xk+1+Vk+1]]>

其中：

φk+1,k=10T0010T0-aAxrT1-2vrT0aAxrT001-2vrTΓk+1,k=T00T0-aAx2rT2aAx2rT20HK+1=10000100]]>

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

采用自适应卡尔曼滤波算法求取视线角速度在天线坐标系沿y、z轴分量ω_y和ω_z；

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

q·y=ωpB/I+ωpO/B+ωP+ωy]]>

q·z=ωpB/I+ωpO/B+ωP+ωz]]>

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

ωPB/I=TOPTBOωBB/I=cosλzsinλz0-sinλzcosλz0001cosλy0-sinλy010sinλy0cosλyωBxωByωBz=ωBxcosλzcosλy+ωBysinλz-ωBzcosλzsinλy-ωBxsinλzcosλy+ωBycosλz+ωBzsinλzsinλyωBxsinλy+ωBzcosλy]]>

为导弹相对于惯性空间的旋转角速率在弹体坐标系内的向量，λ_y为外框相对于弹体的框架角，λ_z为内框相对于外框的框架角；

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

ωpO/B=ωAxωAyωAz=TOP0λ·y0=cosλzsinλz0-sinλzcosλz00010λ·y0]]>

其中，为外框相对于弹体的旋转角速率在外框坐标系的向量；其中，ω_p是内框架相对于外框架的旋转角速度，计算公式：

ωp=00λ·zT]]>

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