CN114993305A - 一种基于发射坐标系的制导炮弹组合导航方法 - Google Patents

Abstract

本发明公开了一种基于发射坐标系的制导炮弹组合导航方法，涉及制导炮弹组合导航领域，从发射坐标系捷联惯导/卫星量测方程改进制导炮弹组合导航方法，解决组合导航偏航角观测性较弱问题，同时又满足制导炮弹组合导航的要求。引入捷联惯导、卫星输出的偏航角之差作为量测量，拓展传统的量测方程，可以减小偏航角误差。

Description

一种基于发射坐标系的制导炮弹组合导航方法

技术领域

本发明涉及制导炮弹组合导航领域，具体涉及一种基于发射坐标系的制导炮弹组合导航方法。

背景技术

捷联惯导/卫星组合导航多采用位置+速度组合模式，选取捷联惯导、卫星输出的位置、速度之差作为量测量，采用卡尔曼滤波设计组合导航算法。组合导航系统中的偏航角可观测性较弱，同时，由于惯性器件的误差较大，偏航角误差通常较大甚至可能发散。因此，研究制导炮弹组合导航时，解决组合导航偏航角观测性较弱问题是该领域的技术难点。

针对偏航角观测性较弱问题，一些制导炮弹通过引入磁力计的偏航角信息，但是磁力计误差较大，易受外界环境干扰，精度较低。一些制导炮弹利用双天线提供偏航角信息，但制导炮弹上难以安装相隔距离较远的两个天线。为了解决组合导航偏航角观测性较弱问题，同时又满足制导炮弹组合导航的要求，本发明提出的一种基于发射坐标系的制导炮弹组合导航方法，是从发射坐标系捷联惯导/卫星量测方程进行改进。引入捷联惯导、卫星输出的偏航角之差作为量测量，拓展传统的量测方程，可以减小偏航角误差。

发明内容

针对现有技术中的上述不足，本发明提供了一种基于发射坐标系的制导炮弹组合导航方法。

为了达到上述发明目的，本发明采用的技术方案为：

一种基于发射坐标系的制导炮弹组合导航方法，包括如下步骤：

S1、制导炮弹升空完成姿态辨识之后，对制导炮弹的状态方程和量测方程进行离散化；

S2、利用滤波递推算法计算制导炮弹下一时刻的状态，并重复滤波直至收敛状态得到估算误差值；

S3、将计算得到的估算误差值修正至导航解算值中，完成制导炮弹的组合导航。

进一步的，所述S1中状态方程的离散化表示为：

X_k＝Φ_k/k-1X_k-1+Γ_k/k-1W_k-1

其中，X_k是k时刻的系统状态；X_k-1是k-1时刻的系统状态；W_k-1为k-1时刻的噪声；Φ_k,k-1与Γ_k,k-1为状态方程和噪声驱动矩阵的离散化。

进一步的，所述S1中量测方程的离散化表示为：

其中，X为制导炮弹的系统状态，H_vpψ为量测矩阵，V_vpψ为输出误差，V_I为惯性导航的制导炮弹速度，V_S为卫星输出的制导炮弹速度，P_I为惯性导航的制导炮弹的位置，P_S为卫星输出的制导炮弹的位置，ψ_I为惯性导航的制导炮弹的偏航角，ψ_S为卫星输出的制导炮弹的偏航角。

进一步的，所述S2中利用滤波递推算法计算制导炮弹下一时刻的状态具体方式为：

其中，

更新的状态估计，

为一步状态预测，H_k为k时刻的量测矩阵，Z_k为k时刻的离散化量测方程，K_k为滤波增益矩阵。

进一步的，所述S2中估算误差值包括制导炮弹的姿态、速度、位置、陀螺漂移和加速度计漂移的误差值。

进一步的，所述S3具体包括：

S31、通过导航算法计算得到姿态信息

速度为

和位置

S32、通过S2得到的估算误差值分别对姿态信息、速度和位置进行修正，完成组合导航。

进一步的，所述S32中，对姿态信息的修正方式为：

其中，

为计算得到的姿态四元数，

为修正后的姿态四元数，Q_k为姿态误差φ^g对应的四元数；

对速度的修正方式为：

其中，V^g为修正后的速度，

为计算得到的速度，δV^g为速度估算误差值。

对位置的修正方式为：

P^g为修正后的位置，

为计算得到的位置，δP^g为位置估算误差值。

本发明具有以下有益效果：

引入捷联惯导、卫星输出的偏航角之差作为量测量，拓展传统的量测方程，减小偏航角误差，解决了组合导航偏航角观测性较弱问题，同时又满足制导炮弹组合导航的要求。

附图说明

图1为本发明基于发射坐标系的制导炮弹组合导航方法流程示意图。

图2为本发明实施例中发射坐标系与地球之间的关系示意图。

具体实施方式

下面对本发明的具体实施方式进行描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。

为了实现上述目的，本发明提出以下方案：

(1)本发明使用到以下坐标系及坐标系转换

1)地心惯性坐标系(i系)

地心惯性坐标系(Earth-Centered-Inertial frame)，坐标系原点Oⁱ为地球质心，zⁱ轴沿地球自转轴指向协议地极，xⁱ轴在赤道平面上并指向春分点，yⁱ轴满足右手定则。

2)地心地固坐标系(e系)

地心地固坐标系(Earth-Centered Earth-Fixed frame)，坐标系原点O^e为地球中心，x^e轴在赤道平面内并且指向本初子午线，z^e轴为地球自转轴并且指向北极，y^e轴在赤道平面内并且与x^e轴、z^e轴构成右手直角坐标系。

3)发射坐标系(g系)

发射坐标系(Launch-Centered Earth-Fixed frame)，坐标系原点O^g为发射点，x^g轴在发射点水平面内并且指向发射瞄准方向，y^g轴垂直于发射点水平面并且指向上方，z^g轴与x^g轴、y^g轴构成右手直角坐标系，发射坐标系与地球固连。发射点的地理纬度B₀、经度λ₀、高度H₀和发射方位角A₀确定了发射坐标系与地球之间的关系，如图2所示。

4)发射惯性坐标系(a系)

发射惯性坐标系(Launch-Centered Inertial Frame)，在发射瞬间与发射坐标系相重合，之后发射惯性系保持在惯性空间不变，不随地球一起旋转。发射惯性系在发射时刻适用发射坐标系定义，其原点相对地心惯性系保持不动。

5)弹体坐标系(b系)

弹体坐标系(Body frame)，坐标系原点O^b为弹体质心，x^b轴沿弹体纵轴指向弹体正前，y^b轴在弹体主对称轴平面内指向弹体上方，z^b轴与x^b轴、y^b轴构成右手直角坐标系指向弹体右方。

6)发射坐标系与地心地固坐标系之间的转换关系

地心地固坐标系到发射坐标系的方向余弦矩阵为

地心地固坐标系旋转到发射坐标系，由三次旋转获得，涉及飞行器初始经度λ₀、地理纬度B₀和航向A₀，得转换矩阵

如下：

7)发射坐标系与弹体坐标系之间的转换关系

制导炮弹在发射坐标系相对弹体坐标系的姿态角由俯仰角

偏航角ψ和滚转角γ三个欧拉角描述，发射坐标系到弹体坐标系，按照先绕z轴旋转俯仰角

再绕y轴旋转偏航角ψ、后绕x轴旋转滚转角γ的3-2-1旋转顺序，得转换矩阵

如下

转换矩阵

基于上述原理，本发明提供一种基于发射坐标系的制导炮弹组合导航方法，如图1所示，包括如下步骤：

S1、制导炮弹升空完成姿态辨识之后，对制导炮弹的状态方程和量测方程进行离散化；

发射坐标系捷联惯导/卫星松耦合状态方程

卡尔曼滤波状态方程为

1)状态矢量X为

状态矢量包括发射坐标系下的姿态矢量误差φ^g、速度误差矢量δV^g、位置误差矢量δP^g、陀螺仪误差矢量δε^b和加速度计误差矢量

2)状态转移矩阵F为

式(5)中，

为地球自转角速度在发射坐标系下的投影

的反对称矩阵。

分别为

式(6)中，ω_ie为地球的自转引起的e系相对于i系的角速度，

为对应的角速度矢量

3)噪声驱动矩阵G为

4)过程噪声矢量W为

式(10)中，w_g为陀螺仪白噪声，w_a为加速度计白噪声。

传统的发射坐标系捷联惯导/卫星松耦合量测方程以速度、位置为观测量，卡尔曼滤波量测方程为：

式(11)中，V_I、P_I，V_S、P_S分别为惯导和卫星输出的速度、位置，量测矩阵为

为速度、位置输出误差。

卫星输出的偏航角ψ_S

卫星接收机可以直接得到东北天坐标系速度矢量V^l＝[V_E V_N V_U]^T。根据制导炮弹的特点，弹体在无控静稳定状态时攻角和侧滑角都很小，可以用弹道偏角作为偏航角的近似值，因此用卫星接收机提供的东北天速度分量V_E、V_N解算制导炮弹的偏航角，计算公式如下

ψ_S≈ψ_v＝arctan 2(-V_E,V_N) (12)

惯导输出的偏航角ψ_I

计算得到的转换矩阵

可以写为

其中，

Ψ^g为φ^g的反对称矩阵,φ^g＝[φ_x φ_y φ_z]^T为姿态误差矢量，有

由式(13)得

式(16)中，

为测量矩阵理论值，由式(2)得

式(17)中，ψ、

γ分别是偏航角、俯仰角、滚转角的理论值，且有

计算得到的转换矩阵

为

式(18)中，

分别是偏航角、俯仰角、滚转角的测量值。且有

偏航角、俯仰角、滚转角测量值与理论值的关系为

式(19)中，δψ、

δγ分别是偏航角、俯仰角、滚转角的测量误差。

将式(17)和式(19)代入式(16)，得

由式(15)和式(17)得

由式(19)、式(20)和式(21)知，式(14)左边第三行第一列元素为-sin(ψ_I+δψ_I)+sinψ_I，式(14)右边第三行第一列元素为

因此有

对小量X，有

sin X＝X (23)

ψ_I为小量，式(22)可化为

偏航角的量测矩阵为

Z_ψ＝ψ_I-ψ_S＝δψ_I-δψ_s (25)

式(25)中，ψ_I、ψ_S，δψ_I、δψ_S分别为惯导和卫星输出的偏航角、偏航角误差。

将式(24)代入式(25)，得

其中

记

则有

Z_ψ＝H_ψX+V_ψ (29)

以速度、位置、偏航角为观测量，卡尔曼滤波量测方程为：

式(11)中，V_I、P_I、ψ_I，V_S、P_S、ψ_S分别为惯导和卫星输出的速度、位置、偏航角。量测矩阵为

为速度、位置输出误差。

基于此，状态方程和量测方程的离散化分别为

X_k＝Φ_k/k-1X_k-1+Γ_k/k-1W_k-1 (31)

Z_k＝H_kX_k+V_k (32)

式中，X_k是k时刻的系统状态；Φ_k/k-1与Γ_k/k-1为状态方程和噪声驱动矩阵的离散化。

S2、利用滤波递推算法计算制导炮弹下一时刻的状态，并重复滤波直至收敛状态得到姿态、速度、位置、陀螺仪漂移和加速度计漂移的估算误差值；

在本实施例里，状态一步预测：

X_k/k-1＝Φ_k/k-1X_k-1 (35)

状态估计

式中，K_k称作滤波增益矩阵，是观测信息在状态更新时的权重

式中，P_k/k-1称作一步预测均方误差阵，其对角线元素是各个状态估计的方差，可以表示估计的不确定度

估计均方误差矩阵为P_k

基于上述原理，给定初值X₀和P₀，根据k时刻的量测值Z_k，递推求得k时刻的状态估计

经过重复计算n次后，可得出

的收敛值，即姿态，速度，位置，陀螺漂移，加速度计漂移的误差值。

S3、将计算得到的估算误差值修正至导航解算值中，完成制导炮弹的组合导航。

通过导航算法计算得到姿态信息

速度为

(1)姿态修正：

由四元数计算

式中，

为计算得到的姿态四元数，

为校正后的姿态四元数，Q_k为姿态误差φ^g对应的四元数。

(2)速度修正：

(3)位置修正：

本发明中应用了具体实施例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

本领域的普通技术人员将会意识到，这里所述的实施例是为了帮助读者理解本发明的原理，应被理解为本发明的保护范围并不局限于这样的特别陈述和实施例。本领域的普通技术人员可以根据本发明公开的这些技术启示做出各种不脱离本发明实质的其它各种具体变形和组合，这些变形和组合仍然在本发明的保护范围内。

Claims (7)

1.一种基于发射坐标系的制导炮弹组合导航方法，其特征在于，包括如下步骤：

S1、制导炮弹升空完成姿态辨识之后，对制导炮弹的状态方程和量测方程进行离散化；

S2、利用滤波递推算法计算制导炮弹下一时刻的状态，并重复滤波直至收敛状态得到姿态、速度、位置、陀螺仪漂移和加速度计漂移的估算误差值；

S3、将计算得到的估算误差值修正至导航解算值中，完成制导炮弹的组合导航。

2.根据权利要求1所述的基于发射坐标系的制导炮弹组合导航方法，其特征在于，所述S1中状态方程的离散化表示为：

X_k＝Φ_k/k-1X_k-1+Γ_k/k-1W_k-1

其中，X_k是k时刻的系统状态；X_k-1是k-1时刻的系统状态；W_k-1为k-1时刻的噪声；Φ_k,k-1与Γ_k,k-1为状态方程和噪声驱动矩阵的离散化。

3.根据权利要求1所述的基于发射坐标系的制导炮弹组合导航方法，其特征在于，所述S1中量测方程的离散化表示为：

其中，X为制导炮弹的系统状态，H_vpψ为量测矩阵，V_vpψ为输出误差，V_I为惯性导航的制导炮弹速度，V_S为卫星输出的制导炮弹速度，P_I为惯性导航的制导炮弹的位置，P_S为卫星输出的制导炮弹的位置，ψ_I为惯性导航的制导炮弹的偏航角，ψ_S为卫星输出的制导炮弹的偏航角。

4.根据权利要求1所述的基于发射坐标系的制导炮弹组合导航方法，其特征在于，所述S2中利用滤波递推算法计算制导炮弹下一时刻的状态具体方式为：

其中，

更新的状态估计，

为一步状态预测，H_k为k时刻的量测矩阵，Z_k为k时刻的离散化量测方程，X_k为滤波增益矩阵。

5.根据权利要求1所述的基于发射坐标系的制导炮弹组合导航方法，其特征在于，所述S2中估算误差值包括制导炮弹的姿态、速度、位置、陀螺漂移和加速度计漂移的误差值。

6.根据权利要求1所述的基于发射坐标系的制导炮弹组合导航方法，其特征在于，所述S3具体包括：

S31、通过导航算法计算得到姿态信息

速度

和位置

S32、通过S2得到的估算误差值分别对姿态信息、速度和位置进行修正，完成组合导航。

7.根据权利要求6所述的基于发射坐标系的制导炮弹组合导航方法，其特征在于，所述S32中，对姿态信息的修正方式为：

其中，

为计算得到的姿态四元数，

为修正后的姿态四元数，Q_k为姿态误差φ^g对应的四元数；

对速度的修正方式为：

其中，V^g为修正后的速度，

为计算得到的速度，δV^g为速度估算误差值。

对位置的修正方式为：

P^g为修正后的位置，

为计算得到的位置，δP^g为位置估算误差值。

Images