基于动力学模型辅助的旋转弹用地磁卫星组合导航方法技术

本发明专利技术提供基于动力学模型辅助的旋转弹用地磁卫星组合导航方法，给出了基于动力学模型辅助的旋转弹用地磁/卫星组合解算方案，通过利用弹体动力学模型和卫星导航系统测量方程共同构造滤波模型，由EKF滤波算法完成弹体位置和速度的估算；进而利用速度信息实时估算弹体俯仰角，最终由迭代算法完成弹体偏航角和滚转角姿态最优解算，从而有效提高了测试旋转弹的精确度和可靠性。弹的精确度和可靠性。弹的精确度和可靠性。

【技术实现步骤摘要】
基于动力学模型辅助的旋转弹用地磁卫星组合导航方法


[0001]本专利技术涉及飞行器或者旋转弹飞行姿态、速度和位置的测量方法
，尤其是涉及基于动力学模型辅助的旋转弹用地磁卫星组合导航方法。

技术介绍

[0002]由于旋转弹发射高过载、高自旋转和高动态的“三高”特殊的应用环境，现有成熟的弹载测量系统无法直接移植应用于旋转弹的测试应用，存在可靠性差、飞行参数测试不全或测量精度较低等问题。因此，高速旋转弹高精度姿态测量技术是其制导化改造的难点所在，亟需寻求一种低成本、高精度、适用于旋转弹飞行姿态测量技术，对解决旋转弹药制导化改造中空中飞行姿态测量难题具有重要的理论价值与现实意义。

技术实现思路

[0003]针对上述问题，本
技术实现思路
为一种新的适用于旋转弹飞行全参数的组合导航解算方法，具体为基于动力学模型辅助的旋转弹用地磁卫星组合导航方法，具体包括：
[0004]S1：利用弹体外弹道动力学模型和卫星导航系统测量方程构建滤波模型，由EKF滤波算法完成弹体位置和速度的估算
[0005]S2：利用弹体滤波器估算的速度信息和弹载三轴地磁传感器测量的信息，由迭代算法完成弹体三维姿态最优解算
[0006]其中，所述S1还包括，构建弹体外弹道动力学模型：
[0007]选取弹体速度(v
x
,v
y
,v
z
)和三维位置(x,y,z)作为滤波器的状态变量X(t)＝[v
x
,v
y
,v
z
>,x,y,z]T
；
[0008]构建弹体外弹道动力学模型作为系统的状态方程，表示为：
[0009][0010]上式中，f[X(t),t]为非线性方程，w(t)是为系统的零均值系统白噪声，满足均值E[w(t)]＝0，方差为E[w(t),w
T
(τ)]＝Q(t)，Q(t)为系统噪声方差阵。
[0011]其中，c为弹道系数，用于表示空气阻力对弹道特性影响程度；H(y)为空气密度，其经验为H(y)＝(20000
‑
y)/(20000+y)；x,y,z为发射系下的弹体三维位置；v
x
、v
y
和v
z
为发射
系下的弹体速度分量；G(v
r
,c
s
)为阻力函数，其中：K为阻力系数，c
s
为声速，ρ
on
为标准空气密度值，C
x0
标准阻力系数值，弹体总速度g为重力加速度。
[0012]进一步的，所述S1还包括，所述卫星导航系统测量方程为：
[0013]选取弹载卫星组合导航系统测量的速度和位置信息作为量测值Z(t)，则滤波器的量测方程可表示为：
[0014]Z(t)＝[v
x
,v
y
,v
z
,x,y,z]T
+v(t)，
[0015]进一步简写为：Z(t)＝h[X(t),t]+v(t)，其中，h[X(t),t]＝[v
x
,v
y
,v
z
,x,y,z]T
，
[0016]上式中，v(t)为零均值量测白噪声，满足均值E[v(t)]＝0，方差E[v(t),v
T
(τ)]＝R(t)，R(t)为量测噪声方差阵。
[0017]基于上述，进一步构建滤波模型，公式为：
[0018][0019]对其进行线性化处理和离散化处理，得其线性干扰方程为：
[0020][0021]进行离散化处理后，得：
[0022][0023]上式中，是雅克比矩阵；观测矩阵H(t)＝I6×6为6阶单位矩阵；观测矩阵H
k
＝I6×6，状态转移阵Φ
k,k
‑1＝I6×6+F(t
k
‑1)T，T为采样时间，F(t
k
‑1)为F(t)在采样时刻t
k
‑1的值。
[0024]进一步的，所述EKF滤波算法，包括：时间更新和量测更新两个过程，其中，
[0025]时间更新，公式为：
[0026][0027]量测更新，公式为：
[0028][0029]进而计算得：状态变量的实时估计
[0030]上式中K
k
为滤波增益阵；Φ
k,k
‑1为状态转移阵；H
k
为量测矩阵；R
k
为等效量测噪声阵；Q
k
‑1为前一时刻等效系统噪声阵，Q
k
为等效系统噪声阵；P
k
‑1为前一时刻系统估计方差阵；P
k,k
‑1为系统方差预测阵；P
k
为当前时刻系统方差阵；分别为弹体导航坐标系下X、Y和Z轴向的速度分量估计值，分别为弹体导航坐标系下X、Y和Z轴向的位置分量估计值。
[0031]进一步的，所述S2还包括，计算弹体的俯仰角，公式为：
[0032][0033]上式中，为弹体的俯仰角解算值。
[0034]进一步的，所述S2还包括，计算弹体的偏航角和滚转角，公式为
[0035]根据地磁场的投影关系得到弹体姿态解算公式为：
[0036][0037]上式中，和为弹载三轴地磁传感器的测量的磁测输出值，和分别为发射坐标系下的地磁场三分量；ψ、θ和γ分别弹体的偏航角、俯仰角和滚转角。
[0038]进一步通过迭代法解算出弹体的偏航角和滚转角的第一个近似解为：
[0039][0040]利用当前迭代所得到的近似解作为下一次迭代计算的初始值X
att0
，即令执行下一次迭代，并判断迭代解算结果是否收敛，当ΔX最大值Max||ΔX
att
||≤δ，则判断为收敛；否则，重复迭代计算，所述δ为预设的迭代解算误差值；
[0041]其中，其中，和γ0分别为偏航角和滚转角的初始值；和分别为偏航角和滚转角的近似解。
[0042]计算所述第一个近似解还包括：
[0043]将所述弹体姿态解算公式在近似解处进行一阶Taylor的展开，并进一步整理后为：
[0044][0045]上式中，令
[0046]则得：Y＝A
×
ΔX
att
，
[0047]计算得到ΔX的最小二乘解为：
[0048]ΔX
att
＝(A
T
A)
‑1A
T
Y；
[0049]上式中，A
T
为矩阵A的转置形式，f
i
(X
att
),i＝1,2,3在X0处的表示为：
[0050][0051][0052][0053]而分别表示为f
i
(X
att
)在处的所求得的一阶偏本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.基于动力学模型辅助的旋转弹用地磁卫星组合导航方法，其特征在于，包括：S1：利用弹体外弹道动力学模型和卫星导航系统测量方程构建滤波模型，由EKF滤波算法完成弹体位置和速度的估算S2：利用弹体滤波器估算的速度信息和弹载三轴地磁传感器测量的信息，由迭代算法完成弹体三维姿态最优解算2.根据权利要求1所述的基于动力学模型辅助的旋转弹用地磁卫星组合导航方法，其特征在于，所述S1还包括，构建弹体外弹道动力学模型：选取弹体速度(v
x
,v
y
,v
z
)和三维位置(x,y,z)作为滤波器的状态变量X(t)＝[v
x
,v
y
,v
z
,x,y,z]
T
；构建弹体外弹道动力学模型作为系统的状态方程，表示为：上式中，f[X(t),t]为非线性方程，w(t)是为系统的零均值系统白噪声，满足均值E[w(t)]＝0，方差为E[w(t),w
T
(τ)]＝Q(t)，Q(t)为系统噪声方差阵；其中，c为弹道系数，用于表示空气阻力对弹道特性影响程度；H(y)为空气密度，其经验为H(y)＝(20000
‑
y)/(20000+y)；x,y,z为发射系下的弹体三维位置；v
x
、v
y
和v
z
为发射系下的弹体速度分量；G(v
r
,c
s
)为阻力函数，其中：K为阻力系数c
s
为声速，ρ
on
为标准空气密度值，C
x0
标准阻力系数值，弹体总速度g为重力加速度。3.根据权利要求1所述的基于动力学模型辅助的旋转弹用地磁卫星组合导航方法，其特征在于，所述S1还包括，所述卫星导航系统测量方程为：选取弹载卫星组合导航系统测量的速度和位置信息作为量测值Z(t)，则滤波器的量测方程可表示为：Z(t)＝[v
x
,v
y
,v
z
,x,y,z]
T
+v(t)，进一步简写为：Z(t)＝h[X(t),t]+v(t)，其中，h[X(t),t]＝[v
x
,v
y
,v
z
,x,y,z]
T
，上式中，v(t)为零均值量测白噪声，满足均值E[v(t)]＝0，方差E[v(t),v
T
(τ)]＝R(t)，R(t)为量测噪声方差阵。4.根据权利要求1
‑
3任一项所述的基于动力学模型辅助的旋转弹用地磁卫星组合导航方法，其特征在于，还包括，构建滤波模型，公式为：
对其进行线性化处理和离散化处理，得其线性干扰方程为：进行离散化处理后，得：上式中，是雅克比矩阵；观测矩阵H(t)＝I6×6为6阶单位矩阵；观测矩阵H
k
＝I6...

【专利技术属性】
技术研发人员：龙达峰，孙俊丽，
申请(专利权)人：惠州学院，
类型：发明
国别省市：