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

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

Info

Publication number
CN112710298A
CN112710298A CN202011375192.6A CN202011375192A CN112710298A CN 112710298 A CN112710298 A CN 112710298A CN 202011375192 A CN202011375192 A CN 202011375192A CN 112710298 A CN112710298 A CN 112710298A
Authority
CN
China
Prior art keywords
projectile
missile
geomagnetic
dynamic model
satellite
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202011375192.6A
Other languages
English (en)
Other versions
CN112710298B (zh
Inventor
龙达峰
孙俊丽
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Hengzhun Times Technology Co ltd
Original Assignee
Huizhou University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Huizhou University filed Critical Huizhou University
Priority to CN202011375192.6A priority Critical patent/CN112710298B/zh
Publication of CN112710298A publication Critical patent/CN112710298A/zh
Application granted granted Critical
Publication of CN112710298B publication Critical patent/CN112710298B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
    • G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/52—Determining velocity
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/53—Determining attitude
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/20—Design optimisation, verification or simulation
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F2119/00—Details relating to the type or aim of the analysis or the optimisation
    • G06F2119/14—Force analysis or force optimisation, e.g. static or dynamic forces

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Theoretical Computer Science (AREA)
  • Geology (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • General Life Sciences & Earth Sciences (AREA)
  • Computer Hardware Design (AREA)
  • Evolutionary Computation (AREA)
  • Geometry (AREA)
  • General Engineering & Computer Science (AREA)
  • Environmental & Geological Engineering (AREA)
  • Navigation (AREA)
  • Feedback Control In General (AREA)
  • Aiming, Guidance, Guns With A Light Source, Armor, Camouflage, And Targets (AREA)

Abstract

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

Description

基于动力学模型辅助的旋转弹用地磁卫星组合导航方法
技术领域
本发明涉及飞行器或者旋转弹飞行姿态、速度和位置的测量方法技术领域，尤其是涉及基于动力学模型辅助的旋转弹用地磁卫星组合导航方法。
背景技术
由于旋转弹发射高过载、高自旋转和高动态的“三高”特殊的应用环境，现有成熟的弹载测量系统无法直接移植应用于旋转弹的测试应用，存在可靠性差、飞行参数测试不全或测量精度较低等问题。因此，高速旋转弹高精度姿态测量技术是其制导化改造的难点所在，亟需寻求一种低成本、高精度、适用于旋转弹飞行姿态测量技术，对解决旋转弹药制导化改造中空中飞行姿态测量难题具有重要的理论价值与现实意义。
发明内容
针对上述问题，本发明内容为一种新的适用于旋转弹飞行全参数的组合导航解算方法，具体为基于动力学模型辅助的旋转弹用地磁卫星组合导航方法，具体包括：
S1：利用弹体外弹道动力学模型和卫星导航系统测量方程构建滤波模型，由EKF滤波算法完成弹体位置
Figure BDA0002812731750000011
和速度的估算
Figure BDA0002812731750000012
S2：利用弹体滤波器估算的速度信息
Figure BDA0002812731750000013
和弹载三轴地磁传感器测量的信息，由迭代算法完成弹体三维姿态最优解算
Figure BDA0002812731750000014
其中，所述S1还包括，构建弹体外弹道动力学模型：
选取弹体速度(vx,vy,vz)和三维位置(x,y,z)作为滤波器的状态变量X(t)＝[vx,vy,vz,x,y,z]T；
构建弹体外弹道动力学模型作为系统的状态方程，表示为：
Figure BDA0002812731750000015
上式中，f[X(t),t]为非线性方程，
Figure BDA0002812731750000016
w(t)是为系统的零均值系统白噪声，满足均值E[w(t)]＝0，方差为E[w(t),wT(τ)]＝Q(t)，Q(t)为系统噪声方差阵。
其中，c为弹道系数，用于表示空气阻力对弹道特性影响程度；H(y)为空气密度，其经验为H(y)＝(20000-y)/(20000+y)；x,y,z为发射系下的弹体三维位置；vx、vy和vz为发射系下的弹体速度分量；G(vr,cs)为阻力函数，
Figure BDA0002812731750000021
其中：K为阻力系数，
Figure BDA0002812731750000022
cs为声速，ρon为标准空气密度值，Cx0标准阻力系数值，弹体总速度
Figure BDA0002812731750000023
g为重力加速度。
进一步的，所述S1还包括，所述卫星导航系统测量方程为：
选取弹载卫星组合导航系统测量的速度和位置信息作为量测值Z(t)，则滤波器的量测方程可表示为：
Z(t)＝[vx,vy,vz,x,y,z]T+v(t)，
进一步简写为：Z(t)＝h[X(t),t]+v(t)，其中，h[X(t),t]＝[vx,vy,vz,x,y,z]T，
上式中，v(t)为零均值量测白噪声，满足均值E[v(t)]＝0，方差E[v(t),vT(τ)]＝R(t)，R(t)为量测噪声方差阵。
基于上述，进一步构建滤波模型，公式为：
Figure BDA0002812731750000024
对其进行线性化处理和离散化处理，得其线性干扰方程为：
Figure BDA0002812731750000025
进行离散化处理后，得：
Figure BDA0002812731750000026
上式中，
Figure BDA0002812731750000027
是雅克比矩阵；观测矩阵H(t)＝I6×6为6阶单位矩阵；观测矩阵Hk＝I6×6，状态转移阵Φk,k-1＝I6×6+F(tk-1)T，T为采样时间，F(tk-1)为F(t)在采样时刻tk-1的值。
进一步的，所述EKF滤波算法，包括：时间更新和量测更新两个过程，其中，
时间更新，公式为：
Figure BDA0002812731750000031
量测更新，公式为：
Figure BDA0002812731750000032
进而计算得：状态变量
Figure BDA0002812731750000033
的实时估计
Figure BDA0002812731750000034
上式中Kk为滤波增益阵；Φk,k-1为状态转移阵；Hk为量测矩阵；Rk为等效量测噪声阵；Qk-1为前一时刻等效系统噪声阵，Qk为等效系统噪声阵；Pk-1为前一时刻系统估计方差阵；Pk,k-1为系统方差预测阵；Pk为当前时刻系统方差阵；
Figure BDA0002812731750000035
分别为弹体导航坐标系下X、Y和Z轴向的速度分量估计值，
Figure BDA0002812731750000036
分别为弹体导航坐标系下X、Y和Z轴向的位置分量估计值。
进一步的，所述S2还包括，计算弹体的俯仰角，公式为：
Figure BDA0002812731750000037
上式中，
Figure BDA0002812731750000038
为弹体的俯仰角解算值。
进一步的，所述S2还包括，计算弹体的偏航角和滚转角，公式为
根据地磁场的投影关系得到弹体姿态解算公式为：
Figure BDA0002812731750000039
上式中，
Figure BDA00028127317500000310
和
Figure BDA00028127317500000311
为弹载三轴地磁传感器的测量的磁测输出值，
Figure BDA00028127317500000312
和
Figure BDA00028127317500000313
分别为发射坐标系下的地磁场三分量；ψ、θ和γ分别弹体的偏航角、俯仰角和滚转角。
进一步通过迭代法解算出弹体的偏航角和滚转角的第一个近似解
Figure BDA0002812731750000041
为：
Figure BDA0002812731750000042
利用当前迭代所得到的近似解
Figure BDA0002812731750000043
作为下一次迭代计算的初始值Xatt0，即令
Figure BDA0002812731750000044
执行下一次迭代，并判断迭代解算结果
Figure BDA0002812731750000045
是否收敛，当ΔX最大值Max||ΔXatt||≤δ，则判断为收敛；否则，重复迭代计算，所述δ为预设的迭代解算误差值；
其中，
Figure BDA0002812731750000046
Figure BDA0002812731750000047
和γ0分别为偏航角和滚转角的初始值；
Figure BDA0002812731750000048
和
Figure BDA0002812731750000049
分别为偏航角和滚转角的近似解。
计算所述第一个近似解
Figure BDA00028127317500000410
还包括：
将所述弹体姿态解算公式在近似解
Figure BDA00028127317500000411
处进行一阶Taylor的展开，并进一步整理后为：
Figure BDA00028127317500000412
上式中，令
Figure BDA00028127317500000413
则得：Y＝A×ΔXatt，
计算得到ΔX的最小二乘解为：
ΔXatt＝(ATA)-1ATY；
上式中，AT为矩阵A的转置形式，fi(Xatt),i＝1,2,3在X0处的表示为：
Figure BDA0002812731750000051
Figure BDA0002812731750000052
Figure BDA0002812731750000053
而
Figure BDA0002812731750000054
分别表示为fi(Xatt)在
Figure BDA0002812731750000055
处的所求得的一阶偏导数。
综上所述，本发明提供基于动力学模型辅助的旋转弹用地磁卫星组合导航方法，给出了基于动力学模型辅助的旋转弹用地磁/卫星组合解算方案，通过利用弹体动力学模型和卫星导航系统测量方程共同构造滤波模型，由EKF滤波算法完成弹体位置和速度的估算；进而利用速度信息实时估算弹体俯仰角，最终由迭代算法完成弹体偏航角和滚转角姿态最优解算，从而有效提高了测试旋转弹的精确度和可靠性。
附图说明
图1为一实施例中的基于动力学模型辅助的地磁/卫星组合导航解算方案示意图。
图2为如图1所示的解算方案的坐标系定义示意图。
具体实施方式
下面将结合具体实施例及附图对本发明的基于动力学模型辅助的旋转弹用地磁卫星组合导航方法，作进一步详细描述。
如图1所示为本发明提供的基于动力学模型辅助的地磁/卫星组合导航解算方案示意图，其中，弹载传感器由三轴地磁传感器和卫星导航系统组成，本发明所述的基于动力学模型辅助的旋转弹用地磁卫星组合导航方法，主要包括如下两个过程：
(1)利用弹体动力学模型和卫星导航系统测量方程构造滤波模型，由EKF滤波算法完成弹体位置
Figure BDA0002812731750000056
和速度的估算
Figure BDA0002812731750000057
(2)利用弹体滤波器估算的速度信息
Figure BDA0002812731750000058
和弹载三轴地磁传感器测量的信息，由迭代算法完成弹体三维姿态最优解算
Figure BDA0002812731750000059
本发明选取发射坐标系为导航参考坐标系(0-xyz)，弹体坐标系(0-xbybzb),各坐标系的定义如图2所示。
作为优选的，本发明所述方法通过构建弹体速度与位置滤波模型进行计算弹体速度与位置参数，具体为：
在标准气象条件下，不考虑旋转弹的攻角情况下，弹体质点外弹道动力学方程可以简化为公式1：
Figure BDA0002812731750000061
上式中，c为弹道系数，反应了空气阻力对弹道特性影响程度。H(y)为空气密度公式，其经验为H(y)＝(20000-y)/(20000+y)；x,y,z为发射系下的弹体三维位置；vx、vy和vz为发射系下的弹体速度分量；G(vr,cs)为阻力函数，
Figure BDA0002812731750000062
其中：K为阻力系数，
Figure BDA0002812731750000063
cs为声速，ρon为标准空气密度值，Cx0标准阻力系数值，弹体总速度
Figure BDA0002812731750000064
g为重力加速度。
本发明选取弹体速度(vx,vy,vz)和三维位置(x,y,z)作为滤波器的状态变量X(t)＝[vx,vy,vz,x,y,z]T。选取公式1所描述的弹体外弹道动力学模型作为系统的状态方程，其可简化表示为公式2：
Figure BDA0002812731750000065
上式中，f[X(t),t]为非线性方程，
Figure BDA0002812731750000071
w(t)是为系统的零均值系统白噪声，满足均值E[w(t)]＝0，方差为E[w(t),wT(τ)]＝Q(t)，Q(t)为系统噪声方差阵。
选取弹载卫星组合导航系统测量的速度和位置信息作为量测值Z(t)，则滤波器的量测方程可表示为公式3：
Z(t)＝[vx,vy,vz,x,y,z]T+v(t)
上式中v(t)为零均值量测白噪声，满足均值E[v(t)]＝0，方差E[v(t),vT(τ)]＝R(t)。R(t)为量测噪声方差阵。
再把量测公式3简写为一般形式，如公式4：
Z(t)＝h[X(t),t]+v(t)
上式中，h[X(t),t]＝[vx,vy,vz,x,y,z]T。
进一步的，进行弹体速度与位置滤波算法，具体为：
综合前述建立的系统状态公式2观测公式4共同组成滤波器的滤波模型，如公式5：
Figure BDA0002812731750000072
对其进行线性化处理和离散化处理，可得其线性干扰方程，如公式6：
Figure BDA0002812731750000073
上式中，
Figure BDA0002812731750000074
是雅克比矩阵；观测矩阵H(t)＝I6×6为6阶单位矩阵。在此基础上，再对公式6进行离散化处理后可表示为以下形式，表示为公式7：
Figure BDA0002812731750000081
上式中，观测矩阵Hk＝I6×6，状态转移阵Φk,k-1＝I6×6+F(tk-1)T，T为采样时间，F(tk-1)为F(t)在采样时刻tk-1的值。
因此，基于EKF的速度与位置滤波算法为，其包括时间更新和量测更新两个过程：
1)时间更新，如公式8：
Figure BDA0002812731750000082
2)量测更新，如公式9：
Figure BDA0002812731750000083
上式中Kk为滤波增益阵；Φk,k-1为状态转移阵；Hk为量测矩阵；Rk为等效量测噪声阵；Qk-1为前一时刻等效系统噪声阵，Qk为等效系统噪声阵；Pk-1为前一时刻系统估计方差阵；Pk,k-1为系统方差预测阵；Pk为当前时刻系统方差阵。
基于弹道模型，由上述滤波算法完成状态变量
Figure BDA0002812731750000084
的实时估计
Figure BDA0002812731750000085
基于上述，进一步执行弹体姿态解算，具体为：
(1)弹体俯仰角解算
考虑到旋转弹飞行时，弹体攻角通常情况下都很小，弹体的俯仰角可以按如下公式10进行解算：
Figure BDA0002812731750000086
上式中，
Figure BDA0002812731750000087
为弹体的俯仰角解算值；
Figure BDA0002812731750000088
分别为弹体导航坐标系下X、Y和Z轴向的速度分量估计值，由前述弹体滤波器估计得到。
(2)弹体偏航角和滚转角解算
根据地磁场的投影关系可得到弹体姿态解算公式为：
Figure BDA0002812731750000091
上式中，
Figure BDA0002812731750000092
和
Figure BDA0002812731750000093
为弹载三轴地磁传感器的测量的磁测输出值，
Figure BDA0002812731750000094
和
Figure BDA0002812731750000095
分别为发射坐标系下的地磁场三分量。ψ、θ和γ分别弹体的偏航角、俯仰角和滚转角。
因此，首先利用公式10解算出弹体的俯仰角
Figure BDA0002812731750000096
然后根据磁测解算公式11，通过迭代法解算出解算弹体的偏航角和滚转角。
令
Figure BDA0002812731750000097
为方程组(11)中的偏航角
Figure BDA0002812731750000098
和滚转角
Figure BDA0002812731750000099
的近似解，
Figure BDA00028127317500000910
为方程组(11)迭代解算的偏航角
Figure BDA00028127317500000911
和滚转角(γ0)的初始值。再将解算方程组(11)在近似解
Figure BDA00028127317500000912
处进行一阶Taylor的展开，表示为公式12：
Figure BDA00028127317500000913
上式中fi(Xatt),i＝1,2,3在X0处的表示为：
Figure BDA00028127317500000914
Figure BDA00028127317500000915
Figure BDA00028127317500000916
而
Figure BDA00028127317500000917
分别表示为fi(Xatt)在
Figure BDA00028127317500000918
处的所求得的一阶偏导数。
因此，再把公式12作进一步整理可得公式13：
Figure BDA0002812731750000101
上式中，令
Figure BDA0002812731750000102
则公式13可进一步简写为如下公式14：
Y＝A×ΔXatt。
由上公式14可以计算得到ΔX的最小二乘解为公式15：
ΔXatt＝(ATA)-1ATY
上式中，AT为矩阵A的转置形式。因此，方程组(12)中的偏航角
Figure BDA0002812731750000103
和滚转角
Figure BDA0002812731750000104
的第一个近似解
Figure BDA0002812731750000105
为公式16：
Figure BDA0002812731750000106
然后，利用当前迭代所得到的近似解
Figure BDA0002812731750000107
作为下一次迭代计算的初始值Xatt0，即令
Figure BDA0002812731750000108
重复利用上述迭代计算过程，并判断迭代解算结果
Figure BDA0002812731750000109
是否收敛，收敛条件为ΔX最大值Max||ΔXatt||≤δ，这里δ为设定的迭代解算误差。若当次不满足收敛条件，再重复上述迭代计算过程直到满足精度，完成弹体偏航角和滚转角最优解算。
综上所述，本发明所述的基于动力学模型辅助的旋转弹用地磁卫星组合导航方法，利用弹体动力学模型和卫星导航系统测量方程共同构造滤波模型，由EKF滤波算法完成弹体位置和速度的估算，以及通过速度信息实时估算弹体俯仰角，最终由迭代算法完成弹体偏航角和滚转角姿态最优解算，实现的旋转弹用地磁/卫星在飞行状态下的位置参数和速度参数，有效保证了飞行参数检测数据的真实和可靠性。
以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。

Claims (8)

1.基于动力学模型辅助的旋转弹用地磁卫星组合导航方法，其特征在于，包括：
S1：利用弹体外弹道动力学模型和卫星导航系统测量方程构建滤波模型，由EKF滤波算法完成弹体位置
Figure FDA0002812731740000011
和速度的估算
Figure FDA0002812731740000012
S2：利用弹体滤波器估算的速度信息
Figure FDA0002812731740000013
和弹载三轴地磁传感器测量的信息，由迭代算法完成弹体三维姿态最优解算
Figure FDA0002812731740000014
2.根据权利要求1所述的基于动力学模型辅助的旋转弹用地磁卫星组合导航方法，其特征在于，所述S1还包括，构建弹体外弹道动力学模型：
选取弹体速度(vx,vy,vz)和三维位置(x,y,z)作为滤波器的状态变量X(t)＝[vx,vy,vz,x,y,z]T；
构建弹体外弹道动力学模型作为系统的状态方程，表示为：
Figure FDA0002812731740000015
上式中，f[X(t),t]为非线性方程，
Figure FDA0002812731740000016
w(t)是为系统的零均值系统白噪声，满足均值E[w(t)]＝0，方差为E[w(t),wT(τ)]＝Q(t)，Q(t)为系统噪声方差阵；
其中，c为弹道系数，用于表示空气阻力对弹道特性影响程度；H(y)为空气密度，其经验为H(y)＝(20000-y)/(20000+y)；x,y,z为发射系下的弹体三维位置；vx、vy和vz为发射系下的弹体速度分量；G(vr,cs)为阻力函数，
Figure FDA0002812731740000017
其中：K为阻力系数
Figure FDA0002812731740000018
cs为声速，ρon为标准空气密度值，Cx0标准阻力系数值，弹体总速度
Figure FDA0002812731740000019
g为重力加速度。
3.根据权利要求1所述的基于动力学模型辅助的旋转弹用地磁卫星组合导航方法，其特征在于，所述S1还包括，所述卫星导航系统测量方程为：
选取弹载卫星组合导航系统测量的速度和位置信息作为量测值Z(t)，则滤波器的量测方程可表示为：
Z(t)＝[vx,vy,vz,x,y,z]T+v(t)，
进一步简写为：Z(t)＝h[X(t),t]+v(t)，其中，h[X(t),t]＝[vx,vy,vz,x,y,z]T，
上式中，v(t)为零均值量测白噪声，满足均值E[v(t)]＝0，方差E[v(t),vT(τ)]＝R(t)，R(t)为量测噪声方差阵。
4.根据权利要求1-3任一项所述的基于动力学模型辅助的旋转弹用地磁卫星组合导航方法，其特征在于，还包括，构建滤波模型，公式为：
Figure FDA0002812731740000021
对其进行线性化处理和离散化处理，得其线性干扰方程为：
Figure FDA0002812731740000022
进行离散化处理后，得：
Figure FDA0002812731740000023
上式中，
Figure FDA0002812731740000024
是雅克比矩阵；观测矩阵H(t)＝I6×6为6阶单位矩阵；观测矩阵Hk＝I6×6，状态转移阵Φk,k-1＝I6×6+F(tk-1)T，T为采样时间，F(tk-1)为F(t)在采样时刻tk-1的值。
5.根据权利要求1所述的基于动力学模型辅助的旋转弹用地磁卫星组合导航方法，其特征在于，所述EKF滤波算法，包括：时间更新和量测更新两个过程，其中，
时间更新，公式为：
Figure FDA0002812731740000025
量测更新，公式为：
Figure FDA0002812731740000031
进而计算得：状态变量
Figure FDA0002812731740000032
的实时估计
Figure FDA0002812731740000033
上式中Kk为滤波增益阵；Φk,k-1为状态转移阵；Hk为量测矩阵；Rk为等效量测噪声阵；Qk-1为前一时刻等效系统噪声阵，Qk为等效系统噪声阵；Pk-1为前一时刻系统估计方差阵；Pk,k-1为系统方差预测阵；Pk为当前时刻系统方差阵；
Figure FDA0002812731740000034
分别为弹体导航坐标系下X、Y和Z轴向的速度分量估计值，
Figure FDA0002812731740000035
分别为弹体导航坐标系下X、Y和Z轴向的位置分量估计值。
6.根据权利要求1所述的基于动力学模型辅助的旋转弹用地磁卫星组合导航方法，其特征在于，所述S2还包括，计算弹体的俯仰角，公式为：
Figure FDA0002812731740000036
上式中，
Figure FDA0002812731740000037
为弹体的俯仰角解算值。
7.根据权利要求1所述的基于动力学模型辅助的旋转弹用地磁卫星组合导航方法，其特征在于，所述S2还包括，计算弹体的偏航角和滚转角，公式为:
根据地磁场的投影关系得到弹体姿态解算公式为：
Figure FDA0002812731740000038
上式中，
Figure FDA0002812731740000039
和
Figure FDA00028127317400000310
为弹载三轴地磁传感器的测量的磁测输出值，
Figure FDA00028127317400000311
和
Figure FDA00028127317400000312
分别为发射坐标系下的地磁场三分量；ψ、θ和γ分别弹体的偏航角、俯仰角和滚转角；
进一步通过迭代法解算出弹体的偏航角和滚转角的第一个近似解
Figure FDA00028127317400000313
为：
Figure FDA00028127317400000314
利用当前迭代所得到的近似解
Figure FDA00028127317400000315
作为下一次迭代计算的初始值Xatt0，令
Figure FDA0002812731740000041
执行下一次迭代，并判断迭代解算结果
Figure FDA0002812731740000042
是否收敛，当ΔX最大值Max||ΔXatt||≤δ，则判断为收敛；否则，重复迭代计算，所述δ为预设的迭代解算误差值；
其中，
Figure FDA0002812731740000043
Figure FDA0002812731740000044
和γ0分别为偏航角和滚转角的初始值；
Figure FDA0002812731740000045
和
Figure FDA0002812731740000046
分别为偏航角和滚转角的近似解。
8.根据权利要求7所述的基于动力学模型辅助的旋转弹用地磁卫星组合导航方法，其特征在于，计算所述第一个近似解
Figure FDA0002812731740000047
还包括：
将所述弹体姿态解算公式在近似解
Figure FDA0002812731740000048
处进行一阶Taylor的展开，并进一步整理后为：
Figure FDA0002812731740000049
上式中，令
Figure FDA00028127317400000410
则得：Y＝A×ΔXatt，
计算得到ΔX的最小二乘解为：
ΔXatt＝(ATA)-1ATY；
上式中，AT为矩阵A的转置形式，fi(Xatt),i＝1,2,3在X0处的表示为：
Figure FDA00028127317400000411
Figure FDA00028127317400000412
Figure FDA00028127317400000413
而
Figure FDA0002812731740000051
分别表示为fi(Xatt)在
Figure FDA0002812731740000052
处的所求得的一阶偏导数。
CN202011375192.6A 2020-12-02 2020-12-02 基于动力学模型辅助的旋转弹用地磁卫星组合导航方法 Active CN112710298B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011375192.6A CN112710298B (zh) 2020-12-02 2020-12-02 基于动力学模型辅助的旋转弹用地磁卫星组合导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011375192.6A CN112710298B (zh) 2020-12-02 2020-12-02 基于动力学模型辅助的旋转弹用地磁卫星组合导航方法

Publications (2)

Publication Number Publication Date
CN112710298A true CN112710298A (zh) 2021-04-27
CN112710298B CN112710298B (zh) 2022-04-01

Family

ID=75542279

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011375192.6A Active CN112710298B (zh) 2020-12-02 2020-12-02 基于动力学模型辅助的旋转弹用地磁卫星组合导航方法

Country Status (1)

Country Link
CN (1) CN112710298B (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113253327A (zh) * 2021-07-07 2021-08-13 湖南跨线桥航天科技有限公司 一种基于多普勒残差最小准则的自旋载体高精度测速方法
CN113418499A (zh) * 2021-05-13 2021-09-21 青岛杰瑞自动化有限公司 一种旋转飞行器滚转角解算方法及系统
CN113485095A (zh) * 2021-08-11 2021-10-08 中国科学院微小卫星创新研究院 北斗三号卫星处于地影期时预报姿态的方法

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102927861A (zh) * 2012-11-06 2013-02-13 中北大学 一种适用于高速旋转弹药的磁测姿态高精度解算方法
CN106885569A (zh) * 2017-02-24 2017-06-23 南京理工大学 一种强机动条件下的弹载深组合arckf滤波方法
CN106885570A (zh) * 2017-02-24 2017-06-23 南京理工大学 一种基于鲁棒sckf滤波的紧组合导航方法
CN107314718A (zh) * 2017-05-31 2017-11-03 中北大学 基于磁测滚转角速率信息的高速旋转弹姿态估计方法
CN109373832A (zh) * 2018-12-07 2019-02-22 惠州学院 基于磁测滚转的旋转弹炮口初始参数测量方法
CN109373833A (zh) * 2018-12-07 2019-02-22 惠州学院 适用于旋转弹初始姿态和速度联合测量方法
CN109596018A (zh) * 2018-12-07 2019-04-09 惠州学院 基于磁测滚转角速率信息的旋转弹飞行姿态高精度估计方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102927861A (zh) * 2012-11-06 2013-02-13 中北大学 一种适用于高速旋转弹药的磁测姿态高精度解算方法
CN106885569A (zh) * 2017-02-24 2017-06-23 南京理工大学 一种强机动条件下的弹载深组合arckf滤波方法
CN106885570A (zh) * 2017-02-24 2017-06-23 南京理工大学 一种基于鲁棒sckf滤波的紧组合导航方法
CN107314718A (zh) * 2017-05-31 2017-11-03 中北大学 基于磁测滚转角速率信息的高速旋转弹姿态估计方法
CN109373832A (zh) * 2018-12-07 2019-02-22 惠州学院 基于磁测滚转的旋转弹炮口初始参数测量方法
CN109373833A (zh) * 2018-12-07 2019-02-22 惠州学院 适用于旋转弹初始姿态和速度联合测量方法
CN109596018A (zh) * 2018-12-07 2019-04-09 惠州学院 基于磁测滚转角速率信息的旋转弹飞行姿态高精度估计方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
谢峰: "地磁辅助的卫星/惯性组合导航技术研究", 《中国优秀博硕士学位论文全文数据库（硕士）信息科技辑》 *
龙达峰等: "地磁辅助信息的旋转弹姿态估计方法", 《弹箭与制导学报》 *
龙达峰等: "高速旋转弹飞行姿态磁测解算方法", 《弹道学报》 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113418499A (zh) * 2021-05-13 2021-09-21 青岛杰瑞自动化有限公司 一种旋转飞行器滚转角解算方法及系统
CN113253327A (zh) * 2021-07-07 2021-08-13 湖南跨线桥航天科技有限公司 一种基于多普勒残差最小准则的自旋载体高精度测速方法
CN113485095A (zh) * 2021-08-11 2021-10-08 中国科学院微小卫星创新研究院 北斗三号卫星处于地影期时预报姿态的方法
CN113485095B (zh) * 2021-08-11 2022-09-13 中国科学院微小卫星创新研究院 北斗三号卫星处于地影期时预报姿态的方法

Also Published As

Publication number Publication date
CN112710298B (zh) 2022-04-01

Similar Documents

Publication Publication Date Title
CN112710298B (zh) 基于动力学模型辅助的旋转弹用地磁卫星组合导航方法
CN107544067B (zh) 一种基于高斯混合近似的高超声速再入飞行器跟踪方法
CN109933847B (zh) 一种改进的主动段弹道估计算法
CN107314718A (zh) 基于磁测滚转角速率信息的高速旋转弹姿态估计方法
CN104764467B (zh) 空天飞行器惯性传感器误差在线自适应标定方法
CN104778376B (zh) 一种临近空间高超声速滑翔弹头跳跃弹道预测方法
CN107255924A (zh) 基于扩维模型的容积卡尔曼滤波提取捷联导引头制导信息的方法
CN106500693A (zh) 一种基于自适应扩展卡尔曼滤波的ahrs算法
CN110044385B (zh) 一种大失准角情况下的快速传递对准方法
CN110672078B (zh) 一种基于地磁信息的高旋弹丸姿态估计方法
CN109375515B (zh) 一种垂直起降火箭在线轨迹规划的动力学特性在线辨识方法
CN105486307A (zh) 针对机动目标的视线角速率估计方法
CN109211231B (zh) 一种基于牛顿迭代法的炮弹姿态估计方法
CN111189442A (zh) 基于cepf的无人机多源导航信息状态预测方法
CN106200377A (zh) 一种飞行器控制参数的估计方法
CN108827345A (zh) 一种基于杆臂挠曲变形补偿的机载武器传递对准方法
CN109211232B (zh) 一种基于最小二乘滤波的炮弹姿态估计方法
CN113076634B (zh) 一种多机协同无源定位方法、装置及系统
CN116611160A (zh) 基于一段实测弹道参数的无控飞行器在线实时特征参数辨识与弹道预报方法
CN115906645A (zh) 一种基于快速传递结合lstm神经网络的轨迹预测方法及系统
CN109376364A (zh) 基于扩展卡尔曼滤波的高速旋转弹气动参数辨识方法
CN111239722B (zh) 临近空间高速机动目标机动突变的跟踪算法
CN114485675A (zh) 无人机航姿解算方法
CN117521561B (zh) 巡航飞行器的气动力与推力在线预示方法
CN112683265A (zh) 一种基于快速iss集员滤波的mimu/gps组合导航方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
TR01 Transfer of patent right
TR01 Transfer of patent right

Effective date of registration: 20230111

Address after: 100000 Room 8163, Building 3, No. 3, Xijing Road, Badachu High-tech Park, Shijingshan District, Beijing

Patentee after: Beijing HengZhun times Technology Co.,Ltd.

Address before: 516000 Da Dao Da Avenue, Huicheng District, Huizhou City, Guangdong Province, No. 46

Patentee before: HUIZHOU University