CN111238474A - 基于倾斜坐标系的捷联导引头非奇异视线角速度提取方法 - Google Patents

Abstract

本发明记载一种基于倾斜坐标系的捷联导引头非奇异视线角速度提取方法，属于制导与控制技术领域，在传统坐标系的基础上，设计了倾斜地面坐标系、倾斜视线坐标系、倾斜弹体坐标系，并推导了坐标系间方向余弦矩阵，建立倾斜视线坐标系下的弹目相对运动方程，对弹目相对运动方程两次求导获得倾斜视线角速度的二阶导数与弹目相对加速度的关系方程，建立非奇异滤波状态方程；根据倾斜弹体系到倾斜地面系的坐标转换，推导由倾斜视线角结合弹体姿态信息，得到倾斜弹体系下体视线角的转换方程，建立观测方程，避免由奇异引起的偏航角和滚转角发散问题，同时保证垂直打击时惯性视线角速度的估计精度。

Description

基于倾斜坐标系的捷联导引头非奇异视线角速度提取方法

技术领域

本发明属于制导与控制技术领域，具体涉及基于倾斜坐标系的捷联导引头非奇异视线角速度提取方法。

背景技术

捷联导引头相比于平台导引头，去除了陀螺、万向支架和复杂的伺服机构等感应器，能够降低导引头的设计复杂度，具有结构简单、可靠性高、体积小以及质量轻等优点，同时成本得以大幅度降低，因此近年来捷联导引头在制导武器中得到越来越多的应用。由于捷联导引头直接与弹体固联，导引头所测得的视线角中耦合有弹体的姿态信息，需要设计解耦算法以实现弹体姿态运动的解耦。

考虑捷联导引头制导武器的实际作战需求，遭遇目标时的大落角可以在很大程度上提高毁伤效果。如在打击地面加固目标及地下设施时，需要能以近乎90°的角度实现侵彻打击，在打击装甲运动目标时，需要能够垂直命中前装甲，或者以较大落角命中薄弱的顶装甲以取得最大穿深。打击运动目标，不依赖视线角速度的视线角型制导律落角偏差和脱靶量均较大，因此需要依赖视线角速度信息。然而，捷联导引头无法直接提供制导律所需要的惯性视线角速率信息，需要建立状态方程和量测方程进行滤波估计。

在传统视线角速度滤波估计的状态方程中，分母含有高低视线角的余弦项，在飞行器垂直打击目标时，会引起奇异导致滤波输出的视线角和视线角速度发散、同时导致偏航角和滚转角发散的问题，因此使捷联导引头制导武器难以实现大角度垂直打击，打击角度包络和打击效能大幅降低。因此，解决捷联导引头制导武器垂直打击目标的奇异问题对于捷联制导系统的应用具有重要意义。

发明内容

本发明目的是为了解决现有捷联式导引头制导武器垂直打击目标时滤波发散和姿态奇异的问题，提供了一种基于倾斜坐标系的捷联导引头非奇异视线角速度提取方法。

为了实现上述目的，本发明采取的技术方案如下：

基于倾斜坐标系的捷联导引头非奇异视线角速度提取方法，包括以下步骤：

步骤一：建立倾斜地面坐标系、倾斜视线坐标系、倾斜弹体坐标系，推导三个坐标系间方向余弦矩阵；

步骤二：通过捷联导引头量测获得弹体系下的体视线角；

步骤三：建立倾斜视线坐标系下的弹目相对运动方程，对弹目相对运动方程两次求导获得倾斜视线角速度的二阶导数与弹目相对加速度的关系方程，建立非奇异滤波状态方程；

步骤四：根据倾斜弹体系到地面坐标系的坐标转换，推导由倾斜视线角结合弹体姿态信息，得到倾斜弹体坐标系下体视线角的转换方程，建立观测方程；

步骤五：应用无迹卡尔曼滤波提取倾斜坐标系下的捷联导引头制导信息；

步骤六：根据倾斜视线坐标系到地面坐标系的坐标转换，得到地面坐标系下的捷联导引头制导信息；

本实施方式中，步骤六获得的结果作为制导信息送至制导控制系统。

本发明的优点：本发明设计了一种基于倾斜坐标系的捷联导引头非奇异视线角速度提取方法。针对捷联式导引头制导武器垂直打击目标时俯仰角接近90度导致的奇异问题，该发明定义了倾斜地面坐标系、倾斜视线坐标系、倾斜弹体坐标系，并推导了坐标系间方向余弦矩阵，推导了相应的非奇异滤波状态方程和非奇异姿态运动学方程，避免由奇异引起的偏航角和滚转角发散问题，同时保证垂直打击时惯性视线角速度的估计精度。本发明基于倾斜坐标系设计非奇异视线角速度提取方法，有效扩展了捷联导引头制导武器的打击角度包络，并改善了大角度接近垂直打击时的制导精度，在捷联式导引头制导武器领域有广阔的应用前景。

具体实施方式

具体实施方式一：

本实施方式所述基于倾斜坐标系的捷联导引头非奇异视线角速度提取方法，它包括以下步骤：

步骤一：建立倾斜地面坐标系、倾斜视线坐标系、倾斜弹体坐标系，推导三个坐标系间方向余弦矩阵；

步骤二：通过捷联导引头量测获得弹体系下的体视线角；

步骤三：建立倾斜视线坐标系下的弹目相对运动方程，对弹目相对运动方程两次求导获得倾斜视线角速度的二阶导数与弹目相对加速度的关系方程，建立非奇异滤波状态方程；

步骤四：根据倾斜弹体坐标系到地面坐标系的坐标转换，推导由倾斜视线角结合弹体姿态信息，得到倾斜弹体坐标系下体视线角的转换方程，建立观测方程；

步骤五：应用无迹卡尔曼滤波提取倾斜坐标系下的捷联导引头制导信息；

步骤六：根据倾斜视线坐标系到地面坐标系的坐标转换，得到地面坐标系下的捷联导引头制导信息；

本实施方式中，步骤六获得的结果作为制导信息送至制导控制系统。

具体实施方式二：

下面说明本实施方式，本实施方式对实施方式一作进一步说明，

基于倾斜坐标系的捷联导引头非奇异视线角速度提取方法，具体步骤如下：

步骤一中倾斜地面坐标系Ax^Iy^Iz^I定义如下：将地面坐标系Axyz绕Az轴逆时针转动一定的角度δ，(考虑到垂直打击需求，-π/4＜δ＜0)，得坐标系Ax^Iy^Iz^I，则Ax^Iy^Iz^I同Axyz的关系为

倾斜视线坐标系

定义如下：原点O位于弹体质心。

轴由导弹指向目标，过

轴相对倾斜地面坐标系Ax^Iy^Iz^I中的平面Ax^Iz^I作垂直平面，在此垂直平面内过点O做

轴的垂线，即

轴，

轴与

轴、

轴成右手直角坐标系。

与Ax^Iy^Iz^I的关系用倾斜高低视线角qγ与倾斜方位视线角qλ表示。qγ为

轴与Ax^Iz^I平面的夹角，qλ为

轴在Ax^Iz^I平面上的投影与Ax^I轴的夹角。

倾斜地面坐标系首先绕着Ay^I轴旋转q_λ角度，然后再绕着

轴旋转q_γ角，即可与倾斜视线坐标系重合。写成矩阵形式有：

其中：

倾斜弹体坐标系

定义如下：原点O位于弹体质心。

轴与导弹体轴矢量重合，且方向一致。过

轴相对倾斜地面坐标系Ax^Iy^Iz^I中的平面Ax^Iz^I作垂直平面，在此垂直平面内过点O做

轴的垂线，即

轴，

轴与

轴、

轴成右手直角坐标系。

与Ax^Iy^Iz^I的关系用倾斜俯仰角θ^I、倾斜偏航角ψ^I与倾斜滚转角γ^I表示。θ^I为

轴与Ax^Iz^I平面的夹角，ψ^I为

轴在Ax^Iz^I平面上的投影与Ax^I轴的夹角。

倾斜地面坐标系首先通过绕Ay^I轴旋转ψ^I角，然后绕Az^I轴旋转θ^I角，最后再绕

轴旋转γ^I角。由此可以得到矩阵转换关系式有：

式中：

步骤三中弹目相对运动方程的获得过程为：

根据导弹和目标的空间几何关系和运动学关系建立以下关系式：

式中r为地面坐标系下导弹与目标的相对位置矢量，r_T为目标在地面坐标系下的位置矢量，r_M为导弹在地面坐标系下的位置矢量，V为导弹与目标在地面坐标系下的相对速度矢量，V_T为目标在地面坐标系下的速度矢量，V_M为导弹在地面坐标系下的速度矢量，a为地面坐标系下导弹与目标的相对加速度矢量，a_T为目标在地面坐标系下的加速度矢量，a_M为导弹在地面坐标系下的加速度矢量；

倾斜视线坐标系相对倾斜地面坐标系的旋转角速度ω_s为：

q_γ为倾斜高低视线角，q_λ为倾斜方位视线角，i_s为倾斜视线坐标系的x轴单位向量，j_s为倾斜视线坐标系的y轴单位向量，k_s为倾斜视线坐标系的z轴单位向量；

分别代表倾斜高低视线角速度、倾斜方位视线角速度；

在倾斜视线坐标系中，r＝ri_s，其中，r为地面坐标系下导弹与目标的相对位置标量；

i_s的导数为

同理：j_s，k_s的导数分别为

则对r求导，得到：

进一步对V求导，得到

其中，

分别为r、q_γ、q_λ的二阶导数。

基于弹目相对运动关系，得到弹目相对运动方程：

式中a_xs为导弹相对目标的加速度在倾斜视线坐标系的

轴上的分量，a_ys为导弹相对目标的加速度在倾斜视线坐标系的

轴上的分量，a_zs为导弹相对目标的加速度在倾斜视线坐标系的

轴上的分量；

导弹与目标的相对位置矢量r的估计方法如下：

式中，y_m为弹载GPS输出的高度，y_t为目标点高程；

弹目距离变化率

由式(2)两边对t求导获得：

其中，

为弹载GPS输出的高度变化率，

为目标高度变化率，对地面目标近似为零。

取状态量为

由式(1)建立状态方程：

式中x₁～x₄为状态量x中第1～4个元素，

分别为x₁、x₂、x₃、x₄的一阶导数。

步骤四中建立观测方程的具体方法为：

取观测变量为导引头量测的体高低视线角q_α和导引头量测的体方位视线角q_β，即z＝[q_αq_β]^T，建立观测方程如下：

式中z₁～z₂为观测变量z中第1～2个元素；

其中，上式中的R_bc为地面坐标系到倾斜弹体坐标系的转换矩阵C_d1中第b行第c列的元素，v₁为体高低视线角的噪声信号，v₂为体方位视线角的噪声信号。根据倾斜地面坐标系和地面坐标系的坐标转换关系，地面坐标系到倾斜弹体坐标系的转换矩阵C_d1为

C_d1＝C_I1L_z(δ)。

步骤五中应用无迹卡尔曼滤波提取捷联导引头制导信息的具体方法为：

(一)设定初始参数：

设定滤波初始时刻系统状态值x₀，初始时刻状态误差协方差阵为P₀，系统噪声协方差阵Q，量测噪声协方差阵R；

设置滤波参数采样点权值

其中，n为状态方程的维数，在本问题中n＝4，λ＝3α²-n为比例因子，α,β为常值，决定采样点的分布状态，

为对采样点状态加权所用权值，

为对采样点协方差加权所用权值；

(二)时间更新：

1)计算采样点

其中，

为k-1时刻状态值，P_k-1为k-1时刻状态误差协方差阵；

2)通过状态方程式(4)传播采样点

其中，f(·)为状态方程。

计算k时刻状态的一步预测值

及k时刻状态误差协方差阵的一步预测值P_k|k-1：

其中，Q为系统噪声协方差阵。

(三)量测更新：

1)通过量测方程式(5)计算采样点的量测预测值

其中，h(·)为量测方程。

2)计算量测预测加权和

3)计算增益矩阵K_k：

4)计算状态估计值

和状态误差协方差阵P_k：

其中，z_k为k时刻的实际量测值。

的第一个变量即为提取得到的倾斜高低视线角q_γ，第三个变量为提取得到的倾斜方位视线角q_λ。第二个变量即为提取得到的倾斜高低视线角速度

第四个变量为提取得到的倾斜方位视线角速度

步骤六中根据倾斜视线坐标系到倾斜地面坐标系的坐标转换，得到地面坐标系下的捷联导引头制导信息的具体方法为；

进一步经过坐标转换，获得从地面坐标系到倾斜视线坐标系的坐标转换矩阵。

C_ds＝C_Is(L_z(δ))

其中，C_ds为从地面坐标系到倾斜视线坐标系的坐标转换矩阵，q_γd,q_λd分别为对应的高低视线角、方位视线角。虽然奇异状态下q_γd,q_λd的值无法直接求出，但sinq_γd,cosq_γd仍存在，表达式如下

sinq_γd＝cosq_γcosq_λsinδ+sinq_γcosδ

cosq_γd＝-sinq_γcosq_λsinδ+cosq_γcosδ

倾斜视线坐标系相对地面坐标系的旋转角速度ω_sd为：

进一步获得地面坐标系下的捷联导引头制导信息

本发明针对现有捷联式导引头制导武器垂直打击目标时滤波发散和姿态奇异问题的不足而提出，在地面坐标系的基础上，设计了倾斜地面坐标系、倾斜视线坐标系、倾斜弹体坐标系，并推导了坐标系间方向余弦矩阵，建立倾斜视线坐标系下的弹目相对运动方程，对弹目相对运动方程两次求导获得倾斜视线角速度的二阶导数与弹目相对加速度的关系方程，建立非奇异滤波状态方程；根据倾斜弹体系到倾斜地面系的坐标转换，推导由倾斜视线角结合弹体姿态信息，得到倾斜弹体系下体视线角的转换方程，建立观测方程，避免由奇异引起的偏航角和滚转角发散问题，同时保证垂直打击时惯性视线角速度的估计精度。

本发明基于倾斜坐标系设计非奇异视线角速度提取方法，有效扩展了捷联导引头制导武器的打击角度包络，并改善了大角度接近垂直打击时的制导精度，在捷联式导引头制导武器领域有广阔的应用前景。

Claims (6)

1.基于倾斜坐标系的捷联导引头非奇异视线角速度提取方法，其特征在于，包括以下步骤：

步骤一：建立倾斜地面坐标系、倾斜视线坐标系、倾斜弹体坐标系，推导三个坐标系间方向余弦矩阵；

步骤二：通过捷联导引头量测获得弹体系下的体视线角；

步骤三：建立倾斜视线坐标系下的弹目相对运动方程，对弹目相对运动方程两次求导获得倾斜视线角速度的二阶导数与弹目相对加速度的关系方程，建立非奇异滤波状态方程；

步骤四：根据倾斜弹体坐标系到地面坐标系的坐标转换，推导由倾斜视线角结合弹体姿态信息，得到倾斜弹体坐标系下体视线角的转换方程，建立观测方程；

步骤五：应用无迹卡尔曼滤波提取倾斜坐标系下的捷联导引头制导信息；

步骤六：根据倾斜视线坐标系到地面坐标系的坐标转换，得到地面坐标系下的捷联导引头制导信息。

2.根据权利要求1所述的基于倾斜坐标系的捷联导引头非奇异视线角速度提取方法，其特征在于：步骤一中，倾斜地面坐标系Ax^Iy^Iz^I定义如下：将传统地面坐标系Axyz绕Az轴逆时针转动一定的角度δ，得坐标系Ax^Iy^Iz^I；倾斜视线坐标系

定义如下：原点O位于弹体质心，

轴由导弹指向目标，过

轴相对倾斜地面坐标系Ax^Iy^Iz^I中的平面Ax^Iz^I作垂直平面，在此垂直平面内过点O做

轴的垂线，即

轴，

轴与

轴、

轴成右手直角坐标系；倾斜弹体坐标系

定义如下：原点O位于弹体质心，

轴与导弹体轴矢量重合，且方向一致，过

轴相对倾斜地面坐标系Ax^Iy^Iz^I中的平面Ax^Iz^I作垂直平面，在此垂直平面内过点O做

轴的垂线，即

轴，

轴与

轴、

轴成右手直角坐标系。

3.根据权利要求2所述的基于倾斜坐标系的捷联导引头非奇异视线角速度提取方法，其特征在于：步骤三中，弹目相对运动方程的获得过程为：根据导弹和目标的空间几何关系和运动学关系建立以下关系式：

式中r为地面坐标系下导弹与目标的相对位置矢量，r_T为目标在地面坐标系下的位置矢量，r_M为导弹在地面坐标系下的位置矢量，V为导弹与目标在地面坐标系下的相对速度矢量，V_T为目标在地面坐标系下的速度矢量，V_M为导弹在地面坐标系下的速度矢量，a为地面坐标系下导弹与目标的相对加速度矢量，a_T为目标在地面坐标系下的加速度矢量，a_M为导弹在地面坐标系下的加速度矢量；

倾斜视线坐标系相对倾斜地面坐标系的旋转角速度ω_s为：

q_γ为倾斜高低视线角，q_λ为倾斜方位视线角，i_s为倾斜视线坐标系的x轴单位向量，j_s为倾斜视线坐标系的y轴单位向量，k_s为倾斜视线坐标系的z轴单位向量；

分别代表倾斜高低视线角速度、倾斜方位视线角速度；

在倾斜视线坐标系中，r＝ri_s，其中，r为地面坐标系下导弹与目标的相对位置标量；

i_s的导数为

同理：j_s，k_s的导数分别为

则对r求导，得到：

进一步对V求导，得到

其中，

分别为r、q_γ、q_λ的二阶导数；

基于弹目相对运动关系，得到弹目相对运动方程：

式中a_xs为导弹相对目标的加速度在倾斜视线坐标系的

轴上的分量，a_ys为导弹相对目标的加速度在倾斜视线坐标系的

轴上的分量，a_zs为导弹相对目标的加速度在倾斜视线坐标系的

轴上的分量；

导弹与目标的相对位置矢量r的估计方法如下：

式中，y_m为弹载GPS输出的高度，y_t为目标点高程；

弹目距离变化率

由式(2)两边对t求导获得：

其中，

为弹载GPS输出的高度变化率，

为目标高度变化率；

取状态量为

由式(1)建立状态方程：

式中x₁～x₄为状态量x中第1～4个元素，

分别为x₁、x₂、x₃、x₄的一阶导数。

4.根据权利要求3所述的基于倾斜坐标系的捷联导引头非奇异视线角速度提取方法，其特征在于：步骤四中建立观测方程的具体方法为：取观测变量为导引头量测的体高低视线角q_α和导引头量测的体方位视线角q_β，即z＝[q_α q_β]^T，建立观测方程如下：

式中z₁～z₂为观测变量z中第1～2个元素；

其中，上式中的R_bc为地面坐标系到倾斜弹体坐标系的转换矩阵C_d1中第b行第c列的元素，v₁为体高低视线角的噪声信号，v₂为体方位视线角的噪声信号，根据倾斜地面坐标系和地面坐标系的坐标转换关系，地面坐标系到倾斜弹体坐标系的转换矩阵C_d1为

C_d1＝C_I1L_z(δ)。

5.根据权利要求4所述的基于倾斜坐标系的捷联导引头非奇异视线角速度提取方法，其特征在于：步骤五中应用无迹卡尔曼滤波提取捷联导引头制导信息的具体方法为：

(一)设定初始参数：

设定滤波初始时刻系统状态值x₀，初始时刻状态误差协方差阵为P₀，系统噪声协方差阵Q，量测噪声协方差阵R；

设置滤波参数采样点权值

其中，n为状态方程的维数，λ＝3α²-n为比例因子，α,β为常值，决定采样点的分布状态，

为对采样点状态加权所用权值，

为对采样点协方差加权所用权值；

(二)时间更新：

1)计算采样点

其中，

为k-1时刻状态值，P_k-1为k-1时刻状态误差协方差阵；

2)通过状态方程式(4)传播采样点

其中，f(·)为状态方程；

计算k时刻状态的一步预测值

及k时刻状态误差协方差阵的一步预测值P_k|k-1：

其中，Q为系统噪声协方差阵；

(三)量测更新：

1)通过量测方程式(5)计算采样点的量测预测值

其中，h(·)为量测方程；

2)计算量测预测加权和

3)计算增益矩阵K_k：

4)计算状态估计值

和状态误差协方差阵P_k：

其中，z_k为k时刻的实际量测值；

的第一个变量即为提取得到的倾斜高低视线角q_γ，第三个变量为提取得到的倾斜方位视线角q_λ；第二个变量即为提取得到的倾斜高低视线角速度

第四个变量为提取得到的倾斜方位视线角速度

6.根据权利要求1所述的基于倾斜坐标系的捷联导引头非奇异视线角速度提取方法，其特征在于：步骤六中根据倾斜视线坐标系到倾斜地面坐标系的坐标转换，得到地面坐标系下的捷联导引头制导信息的具体方法为；

进一步经过坐标转换，获得从地面坐标系到倾斜视线坐标系的坐标转换矩阵：

C_ds＝C_Is(L_z(δ))

其中，C_ds为从地面坐标系到倾斜视线坐标系的坐标转换矩阵，q_γd,q_λd分别为对应的高低视线角、方位视线角，虽然奇异状态下q_γd,q_λd的值无法直接求出，但sin q_γd,cos q_γd仍存在，表达式如下

sin q_γd＝cos q_γcos q_λsinδ+sin q_γcosδ

cos q_γd＝-sin q_γcos q_λsinδ+cos q_γcosδ

倾斜视线坐标系相对地面坐标系的旋转角速度ω_sd为：

进一步获得地面坐标系下的捷联导引头制导信息