本发明属于制导与控制技术领域，具体涉及基于倾斜坐标系的捷联导引头非奇异视线角速度提取方法。
背景技术：
捷联导引头相比于平台导引头，去除了陀螺、万向支架和复杂的伺服机构等感应器，能够降低导引头的设计复杂度，具有结构简单、可靠性高、体积小以及质量轻等优点，同时成本得以大幅度降低，因此近年来捷联导引头在制导武器中得到越来越多的应用。由于捷联导引头直接与弹体固联，导引头所测得的视线角中耦合有弹体的姿态信息，需要设计解耦算法以实现弹体姿态运动的解耦。
考虑捷联导引头制导武器的实际作战需求，遭遇目标时的大落角可以在很大程度上提高毁伤效果。如在打击地面加固目标及地下设施时，需要能以近乎90°的角度实现侵彻打击，在打击装甲运动目标时，需要能够垂直命中前装甲，或者以较大落角命中薄弱的顶装甲以取得最大穿深。打击运动目标，不依赖视线角速度的视线角型制导律落角偏差和脱靶量均较大，因此需要依赖视线角速度信息。然而，捷联导引头无法直接提供制导律所需要的惯性视线角速率信息，需要建立状态方程和量测方程进行滤波估计。
在传统视线角速度滤波估计的状态方程中，分母含有高低视线角的余弦项，在飞行器垂直打击目标时，会引起奇异导致滤波输出的视线角和视线角速度发散、同时导致偏航角和滚转角发散的问题，因此使捷联导引头制导武器难以实现大角度垂直打击，打击角度包络和打击效能大幅降低。因此，解决捷联导引头制导武器垂直打击目标的奇异问题对于捷联制导系统的应用具有重要意义。
技术实现要素：
本发明目的是为了解决现有捷联式导引头制导武器垂直打击目标时滤波发散和姿态奇异的问题，提供了一种基于倾斜坐标系的捷联导引头非奇异视线角速度提取方法。
为了实现上述目的，本发明采取的技术方案如下：
基于倾斜坐标系的捷联导引头非奇异视线角速度提取方法，包括以下步骤：
步骤一：建立倾斜地面坐标系、倾斜视线坐标系、倾斜弹体坐标系，推导三个坐标系间方向余弦矩阵；
步骤二：通过捷联导引头量测获得弹体系下的体视线角；
步骤三：建立倾斜视线坐标系下的弹目相对运动方程，对弹目相对运动方程两次求导获得倾斜视线角速度的二阶导数与弹目相对加速度的关系方程，建立非奇异滤波状态方程；
步骤四：根据倾斜弹体系到地面坐标系的坐标转换，推导由倾斜视线角结合弹体姿态信息，得到倾斜弹体坐标系下体视线角的转换方程，建立观测方程；
步骤五：应用无迹卡尔曼滤波提取倾斜坐标系下的捷联导引头制导信息；
步骤六：根据倾斜视线坐标系到地面坐标系的坐标转换，得到地面坐标系下的捷联导引头制导信息；
本实施方式中，步骤六获得的结果作为制导信息送至制导控制系统。
本发明的优点：本发明设计了一种基于倾斜坐标系的捷联导引头非奇异视线角速度提取方法。针对捷联式导引头制导武器垂直打击目标时俯仰角接近90度导致的奇异问题，该发明定义了倾斜地面坐标系、倾斜视线坐标系、倾斜弹体坐标系，并推导了坐标系间方向余弦矩阵，推导了相应的非奇异滤波状态方程和非奇异姿态运动学方程，避免由奇异引起的偏航角和滚转角发散问题，同时保证垂直打击时惯性视线角速度的估计精度。本发明基于倾斜坐标系设计非奇异视线角速度提取方法，有效扩展了捷联导引头制导武器的打击角度包络，并改善了大角度接近垂直打击时的制导精度，在捷联式导引头制导武器领域有广阔的应用前景。
具体实施方式
具体实施方式一：
本实施方式所述基于倾斜坐标系的捷联导引头非奇异视线角速度提取方法，它包括以下步骤：
步骤一：建立倾斜地面坐标系、倾斜视线坐标系、倾斜弹体坐标系，推导三个坐标系间方向余弦矩阵；
步骤二：通过捷联导引头量测获得弹体系下的体视线角；
步骤三：建立倾斜视线坐标系下的弹目相对运动方程，对弹目相对运动方程两次求导获得倾斜视线角速度的二阶导数与弹目相对加速度的关系方程，建立非奇异滤波状态方程；
步骤四：根据倾斜弹体坐标系到地面坐标系的坐标转换，推导由倾斜视线角结合弹体姿态信息，得到倾斜弹体坐标系下体视线角的转换方程，建立观测方程；
步骤五：应用无迹卡尔曼滤波提取倾斜坐标系下的捷联导引头制导信息；
步骤六：根据倾斜视线坐标系到地面坐标系的坐标转换，得到地面坐标系下的捷联导引头制导信息；
本实施方式中，步骤六获得的结果作为制导信息送至制导控制系统。
具体实施方式二：
下面说明本实施方式，本实施方式对实施方式一作进一步说明，
基于倾斜坐标系的捷联导引头非奇异视线角速度提取方法，具体步骤如下：
步骤一中倾斜地面坐标系axiyizi定义如下：将地面坐标系axyz绕az轴逆时针转动一定的角度δ，(考虑到垂直打击需求，-π/4＜δ＜0)，得坐标系axiyizi，则axiyizi同axyz的关系为
倾斜视线坐标系
倾斜地面坐标系首先绕着ayi轴旋转qλ角度，然后再绕着
其中：
倾斜弹体坐标系
倾斜地面坐标系首先通过绕ayi轴旋转ψi角，然后绕azi轴旋转θi角，最后再绕
式中：
步骤三中弹目相对运动方程的获得过程为：
根据导弹和目标的空间几何关系和运动学关系建立以下关系式：
式中r为地面坐标系下导弹与目标的相对位置矢量，rt为目标在地面坐标系下的位置矢量，rm为导弹在地面坐标系下的位置矢量，v为导弹与目标在地面坐标系下的相对速度矢量，vt为目标在地面坐标系下的速度矢量，vm为导弹在地面坐标系下的速度矢量，a为地面坐标系下导弹与目标的相对加速度矢量，at为目标在地面坐标系下的加速度矢量，am为导弹在地面坐标系下的加速度矢量；
倾斜视线坐标系相对倾斜地面坐标系的旋转角速度ωs为：
qγ为倾斜高低视线角，qλ为倾斜方位视线角，is为倾斜视线坐标系的x轴单位向量，js为倾斜视线坐标系的y轴单位向量，ks为倾斜视线坐标系的z轴单位向量；
在倾斜视线坐标系中，r＝ris，其中，r为地面坐标系下导弹与目标的相对位置标量；
is的导数为
同理：js，ks的导数分别为
则对r求导，得到：
进一步对v求导，得到
其中，
基于弹目相对运动关系，得到弹目相对运动方程：
式中axs为导弹相对目标的加速度在倾斜视线坐标系的
导弹与目标的相对位置矢量r的估计方法如下：
式中，ym为弹载gps输出的高度，yt为目标点高程；
弹目距离变化率
其中，
取状态量为
式中x1～x4为状态量x中第1～4个元素，
步骤四中建立观测方程的具体方法为：
取观测变量为导引头量测的体高低视线角qα和导引头量测的体方位视线角qβ，即z＝[qαqβ]t，建立观测方程如下：
式中z1～z2为观测变量z中第1～2个元素；
其中，上式中的rbc为地面坐标系到倾斜弹体坐标系的转换矩阵cd1中第b行第c列的元素，v1为体高低视线角的噪声信号，v2为体方位视线角的噪声信号。根据倾斜地面坐标系和地面坐标系的坐标转换关系，地面坐标系到倾斜弹体坐标系的转换矩阵cd1为
cd1＝ci1lz(δ)。
步骤五中应用无迹卡尔曼滤波提取捷联导引头制导信息的具体方法为：
(一)设定初始参数：
设定滤波初始时刻系统状态值x0，初始时刻状态误差协方差阵为p0，系统噪声协方差阵q，量测噪声协方差阵r；
设置滤波参数采样点权值
其中，n为状态方程的维数，在本问题中n＝4，λ＝3α2-n为比例因子，α,β为常值，决定采样点的分布状态，
(二)时间更新：
1)计算采样点
其中，
2)通过状态方程式(4)传播采样点
其中，f(·)为状态方程。
计算k时刻状态的一步预测值
其中，q为系统噪声协方差阵。
(三)量测更新：
1)通过量测方程式(5)计算采样点的量测预测值
其中，h(·)为量测方程。
2)计算量测预测加权和
3)计算增益矩阵kk：
4)计算状态估计值
其中，zk为k时刻的实际量测值。
步骤六中根据倾斜视线坐标系到倾斜地面坐标系的坐标转换，得到地面坐标系下的捷联导引头制导信息的具体方法为；
进一步经过坐标转换，获得从地面坐标系到倾斜视线坐标系的坐标转换矩阵。
cds＝cis(lz(δ))
其中，cds为从地面坐标系到倾斜视线坐标系的坐标转换矩阵，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为：
进一步获得地面坐标系下的捷联导引头制导信息
本发明针对现有捷联式导引头制导武器垂直打击目标时滤波发散和姿态奇异问题的不足而提出，在地面坐标系的基础上，设计了倾斜地面坐标系、倾斜视线坐标系、倾斜弹体坐标系，并推导了坐标系间方向余弦矩阵，建立倾斜视线坐标系下的弹目相对运动方程，对弹目相对运动方程两次求导获得倾斜视线角速度的二阶导数与弹目相对加速度的关系方程，建立非奇异滤波状态方程；根据倾斜弹体系到倾斜地面系的坐标转换，推导由倾斜视线角结合弹体姿态信息，得到倾斜弹体系下体视线角的转换方程，建立观测方程，避免由奇异引起的偏航角和滚转角发散问题，同时保证垂直打击时惯性视线角速度的估计精度。
本发明基于倾斜坐标系设计非奇异视线角速度提取方法，有效扩展了捷联导引头制导武器的打击角度包络，并改善了大角度接近垂直打击时的制导精度，在捷联式导引头制导武器领域有广阔的应用前景。
1.基于倾斜坐标系的捷联导引头非奇异视线角速度提取方法，其特征在于，包括以下步骤：
步骤一：建立倾斜地面坐标系、倾斜视线坐标系、倾斜弹体坐标系，推导三个坐标系间方向余弦矩阵；
步骤二：通过捷联导引头量测获得弹体系下的体视线角；
步骤三：建立倾斜视线坐标系下的弹目相对运动方程，对弹目相对运动方程两次求导获得倾斜视线角速度的二阶导数与弹目相对加速度的关系方程，建立非奇异滤波状态方程；
步骤四：根据倾斜弹体坐标系到地面坐标系的坐标转换，推导由倾斜视线角结合弹体姿态信息，得到倾斜弹体坐标系下体视线角的转换方程，建立观测方程；
步骤五：应用无迹卡尔曼滤波提取倾斜坐标系下的捷联导引头制导信息；
步骤六：根据倾斜视线坐标系到地面坐标系的坐标转换，得到地面坐标系下的捷联导引头制导信息。
2.根据权利要求1所述的基于倾斜坐标系的捷联导引头非奇异视线角速度提取方法，其特征在于：步骤一中，倾斜地面坐标系axiyizi定义如下：将传统地面坐标系axyz绕az轴逆时针转动一定的角度δ，得坐标系axiyizi；倾斜视线坐标系
3.根据权利要求2所述的基于倾斜坐标系的捷联导引头非奇异视线角速度提取方法，其特征在于：步骤三中，弹目相对运动方程的获得过程为：根据导弹和目标的空间几何关系和运动学关系建立以下关系式：
式中r为地面坐标系下导弹与目标的相对位置矢量，rt为目标在地面坐标系下的位置矢量，rm为导弹在地面坐标系下的位置矢量，v为导弹与目标在地面坐标系下的相对速度矢量，vt为目标在地面坐标系下的速度矢量，vm为导弹在地面坐标系下的速度矢量，a为地面坐标系下导弹与目标的相对加速度矢量，at为目标在地面坐标系下的加速度矢量，am为导弹在地面坐标系下的加速度矢量；
倾斜视线坐标系相对倾斜地面坐标系的旋转角速度ωs为：
qγ为倾斜高低视线角，qλ为倾斜方位视线角，is为倾斜视线坐标系的x轴单位向量，js为倾斜视线坐标系的y轴单位向量，ks为倾斜视线坐标系的z轴单位向量；
在倾斜视线坐标系中，r＝ris，其中，r为地面坐标系下导弹与目标的相对位置标量；
is的导数为
同理：js，ks的导数分别为
则对r求导，得到：
进一步对v求导，得到
其中，
基于弹目相对运动关系，得到弹目相对运动方程：
式中axs为导弹相对目标的加速度在倾斜视线坐标系的
导弹与目标的相对位置矢量r的估计方法如下：
式中，ym为弹载gps输出的高度，yt为目标点高程；
弹目距离变化率
其中，
取状态量为
式中x1～x4为状态量x中第1～4个元素，
4.根据权利要求3所述的基于倾斜坐标系的捷联导引头非奇异视线角速度提取方法，其特征在于：步骤四中建立观测方程的具体方法为：取观测变量为导引头量测的体高低视线角qα和导引头量测的体方位视线角qβ，即z＝[qαqβ]t，建立观测方程如下：
式中z1～z2为观测变量z中第1～2个元素；
其中，上式中的rbc为地面坐标系到倾斜弹体坐标系的转换矩阵cd1中第b行第c列的元素，v1为体高低视线角的噪声信号，v2为体方位视线角的噪声信号，根据倾斜地面坐标系和地面坐标系的坐标转换关系，地面坐标系到倾斜弹体坐标系的转换矩阵cd1为
cd1＝ci1lz(δ)。
5.根据权利要求4所述的基于倾斜坐标系的捷联导引头非奇异视线角速度提取方法，其特征在于：步骤五中应用无迹卡尔曼滤波提取捷联导引头制导信息的具体方法为：
(一)设定初始参数：
设定滤波初始时刻系统状态值x0，初始时刻状态误差协方差阵为p0，系统噪声协方差阵q，量测噪声协方差阵r；
设置滤波参数采样点权值
其中，n为状态方程的维数，λ＝3α2-n为比例因子，α,β为常值，决定采样点的分布状态，
(二)时间更新：
1)计算采样点
其中，
2)通过状态方程式(4)传播采样点
其中，f(·)为状态方程；
计算k时刻状态的一步预测值
其中，q为系统噪声协方差阵；
(三)量测更新：
1)通过量测方程式(5)计算采样点的量测预测值
其中，h(·)为量测方程；
2)计算量测预测加权和
3)计算增益矩阵kk：
4)计算状态估计值
其中，zk为k时刻的实际量测值；
6.根据权利要求1所述的基于倾斜坐标系的捷联导引头非奇异视线角速度提取方法，其特征在于：步骤六中根据倾斜视线坐标系到倾斜地面坐标系的坐标转换，得到地面坐标系下的捷联导引头制导信息的具体方法为；
进一步经过坐标转换，获得从地面坐标系到倾斜视线坐标系的坐标转换矩阵：
cds＝cis(lz(δ))
其中，cds为从地面坐标系到倾斜视线坐标系的坐标转换矩阵，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为：
进一步获得地面坐标系下的捷联导引头制导信息
