阅读说明：本技术 一种基于弹道模型约束的弹载组合导航方法 (Missile-borne integrated navigation method based on ballistic model constraint ) 是由 朱建良 许越 薄煜明 吴盘龙 赵高鹏 王超尘 邹卫军 王军 于 2021-08-27 设计创作，主要内容包括：本发明公开了一种基于弹道模型约束的弹载组合导航方法,该方法为：根据炮弹飞行过程中受到的各种扰动,构建初始弹道模型得到炮弹的飞行轨迹,通过弹道解算得到弹丸的瞬时姿态、速度以及位置信息；根据惯导数据进行惯导解算,得到弹丸瞬时姿态、速度以及位置信息；将得到的瞬时姿态、速度以及位置信息作差,得到弹丸的加速度误差、速度误差、位置误差和姿态角误差；在卫星信号失常时,根据所得误差构建卡尔曼滤波误差方程,对导航状态量进行修正,实现状态最优估计；每隔设定时间段,以当前修正后的状态量作为初始量重新构建弹道模型,重复导航修正过程。本发明在卫星信号不同状况下都可以较好的约束导航误差,得到精确的导航定位数据。(The invention discloses a missile-borne integrated navigation method based on ballistic model constraint, which comprises the following steps: according to various disturbances suffered by the cannonball in the flying process, an initial ballistic model is built to obtain the flying track of the cannonball, and the instantaneous attitude, speed and position information of the cannonball are obtained through ballistic calculation; performing inertial navigation resolving according to the inertial navigation data to obtain instantaneous attitude, speed and position information of the projectile; subtracting the obtained instantaneous attitude, speed and position information to obtain an acceleration error, a speed error, a position error and an attitude angle error of the projectile; when the satellite signal is abnormal, constructing a Kalman filtering error equation according to the obtained error, and correcting the navigation state quantity to realize the optimal state estimation; and reconstructing a ballistic model by taking the current corrected state quantity as an initial quantity every set time period, and repeating the navigation correction process. The invention can better restrain navigation errors under different conditions of satellite signals to obtain accurate navigation positioning data.)

1. A missile-borne integrated navigation method based on ballistic model constraint is characterized by comprising the following steps:

step 1, constructing an initial ballistic model according to various disturbances suffered by a shell in a flying process to obtain a flying track of the shell, and calculating by ballistic to obtain the instantaneous attitude, speed and position information of a projectile;

step 2, performing inertial navigation resolving according to the inertial navigation data to obtain instantaneous attitude, speed and position information of the projectile;

step 3, subtracting the instantaneous attitude, speed and position information obtained in the step 1 and the step 2 to obtain an acceleration error, a speed error, a position error and an attitude angle error of the projectile;

step 4, when the satellite signal is abnormal, constructing a Kalman filtering error equation according to the acceleration error, the speed error, the position error and the attitude angle error, and correcting the navigation state quantity to realize the optimal state estimation;

and 5, reconstructing a ballistic model by taking the current corrected state quantity as an initial quantity every set time period, and repeating the navigation correction process of the steps 1-4.

2. The ballistic model constraint-based missile-borne integrated navigation method according to claim 1, wherein in the step 1, an initial ballistic model is constructed according to various disturbances suffered by the cannonball during the flying process to obtain the flying trajectory of the cannonball, and the instantaneous attitude, speed and position information of the cannonball are obtained through ballistic solution, which is as follows:

an improved ballistic equation system is adopted to establish an outer ballistic model, and a ground coordinate system is defined firstlyO-xyzMissile body coordinate systemO- x ₁ y ₁ z ₁And ballistic coordinate systemO-x ₂ y ₂ z ₂；

Aiming at the selected cannonball, the quality of the cannonball is set asmThe flying speed at the time of launch isThe moment of momentum of the projectile relative to the center of mass isH(ii) a Comprehensively considering an atmospheric density model and a pneumatic model, wherein the air density changes along with the change of the altitude in the flight process, and the pneumatic coefficient changes along with the change of the speed in the flight process, so that an external ballistic six-degree-of-freedom model equation set is established;

equation of motion of center of massIn ballistic coordinate systemO-x ₂ y ₂ z ₂The components on the middle three axes are as shown in formula (1)The system of the cardiac mechanics equations is shown as:

(1)

wherein the content of the first and second substances,is the acceleration vector of the projectile and,is the velocity vector of the projectile and,vis a scalar quantity of the speed of the projectile,θ ₂in order to form the inclination angle of the trajectory,θ _a is the ballistic declination;

F _{x 2} ，F _{y 2} ，F _{z 2}the projectile is respectively in a trajectory coordinate systemO-x ₂ y ₂ z ₂The components on the three coordinate axes neglect the influence of earth rotation, and are specifically expressed as shown in formula (2):

(2)

in the formula (I), the compound is shown in the specification,θ ₂in order to form the inclination angle of the trajectory,γ _v is a velocity ramp angle;

X、Y、Zrespectively aerodynamic drag, magnus force and lift, as shown in equation (3):

(3)

wherein the content of the first and second substances,ρin order to be the density of the air,vin order to determine the speed of the projectile,sthe area of the shot is the area of the shot,c _x ，c _y ，c _z respectively is the aerodynamic drag coefficient and the horse grid-a nusse force coefficient and a lift coefficient;

when the wind speed is influenced, the speed of the projectile is changed,is the relative speed of the projectile or pellets,W _x2，W _y2，W _z2the components of the atmospheric wind speed on three axes of a ballistic coordinate system are respectively;

the system of kinetic equations for motion around the centroid is as follows:

(4)

in the formula (I), the compound is shown in the specification,J _x1，J _y1，J _z1in the body coordinate system for the moment of inertia of the projectile, respectivelyO-x ₁ y ₁ z ₁The components of the three coordinate axes are divided into three coordinate axes,ω _x1，ω _y1，ω _z1respectively a projectile rolling angular velocity, a pitching angular velocity and a yaw angular velocity under a projectile coordinate system,M _x1，M _y1，M _z1roll moment, pitch moment and yaw moment under the elastic body coordinate system respectively;

the ball being subjected to a rolling momentM _x1Pitching momentM _y1And yaw momentM _z1Respectively shown in formula (5):

(5)

wherein the content of the first and second substances,ρin order to be the density of the air,vin order to determine the speed of the projectile,sthe area of the shot is the area of the shot,lis the length of the projectile;

in the ground coordinate systemO-xyzIn (3), the kinematic equation of the projectile is shown in formula (6):

(6)

wherein the content of the first and second substances,x、y、zrespectively the components of the position of the projectile in the ground coordinate system in three axial directions,tis time;

the motion of the projectile around the center of mass can be decomposed into rotary motion around three coordinate axes in a ground coordinate systemO-xyzThen, the kinematic equation of the projectile rotating around the center of mass is shown as formula (7):

(7)

in the formula (I), the compound is shown in the specification,θ、γ、Ψrespectively representing the pitch angle, the roll angle and the yaw angle of the projectile;ω _x 、ω _y 、ω _z the components of the rotating angular speed of the projectile on three coordinate axes of a ground coordinate system are shown;

the equations form a mathematical model of the projectile outer trajectory, a first-order differential equation is solved by using a Runge Kutta method, instantaneous attitude, speed and position information of the projectile is obtained, and the instantaneous attitude, speed and position information is provided for an inertial navigation system to carry out error correction.

3. The missile-borne integrated navigation method based on ballistic model constraints of claim 2, wherein the ground coordinate systemO-xyzMissile body coordinate systemO-x ₁ y ₁ z ₁And ballistic coordinate systemO-x ₂ y ₂ z ₂Specifically, the following are defined:

(a) ground coordinate systemO-xyz: the ground coordinate system is fixedly connected with the earth surface and is an inertial system, and the cannonball launching point is taken as the origin of the coordinate systemOCoordinate axisOx、Oy、OzPointing to east, north and sky respectively;

(b) projectile coordinate systemO-x ₁ y ₁ z ₁: origin of projectile coordinate systemOIs the mass center of the cannonball,Ox ₁the axis is consistent with the longitudinal central axis of the projectile body, and the front part of the projectile body is taken as the positive;Oy ₁axis perpendicular toOx ₁The longitudinal section of the projectile in which the shaft is located,Oz ₁axis perpendicular toO-x ₁ y ₁ Plane, with a direction positive, andO-x ₁ y ₁ z ₁forming a right-hand rectangular coordinate system;

(c) ballistic coordinate systemO-x ₂ y ₂ z ₂: ballistic coordinate system and velocityVIs connected to, at the origin ofOIs the instantaneous center of mass of the projectile,Ox ₂velocity vector of shaft and projectile all the timeVThe two layers are overlapped with each other,Oz ₂the axis being located containing the velocity vectorVIn the vertical plane and perpendicular toOx ₂Axis, positive upward;Ox ₂、Oy ₂、Oz ₂the three axes form a right-hand rectangular coordinate system.

4. The missile-borne integrated navigation method based on ballistic model constraints according to claim 2 or 3, wherein in step 4, when the satellite signal is abnormal, a Kalman filtering error equation is constructed according to the acceleration error, the velocity error, the position error and the attitude angle error, and specifically the Kalman filtering error equation is as follows:

the cannonball state equation is composed of an acceleration error, a speed error, a position error and an attitude angle error equation, and the state equation is shown in the formula (8):

(8)

wherein the content of the first and second substances,Fin order to be a state transition matrix,Gin order to be a system noise transfer matrix,Wfor system noise, state variablesXAs shown in formula (9):

(9)

where the variable is the position errorδPSpeed errorδVAttitude angle errorδAAnd accelerometer null shift ∇, gyroscope null shiftε；

(10)

(11)

(12)

In the formula (I), the compound is shown in the specification,is the accelerometer error;is the gyroscope drift error;O _3×3a zero matrix with 3 rows and 3 columns;I _3×3an identity matrix of 3 rows and 3 columns;is the attitude transformation moment from the carrier system to the navigation system;f _n x is an antisymmetric matrix of specific forces in the navigation system;B ₁andB ₂respectively, time constant matrixes of zero-bias instability of the accelerometer and the gyroscope;

when the satellite signal is absent, the current motion state value is used as an initial quantity to construct a trajectory, the position and the speed under a navigation coordinate system are obtained through calculation and coordinate conversion, at the moment, the observed quantity selects a position error and a speed error, and a measurement equation is shown as a formula (13):

(13)

in the formula,. DELTA.p _x、Δp _y、Δp _zRespectively, the components of the position error of the projectile in the navigation system on three coordinate axes, deltav _x、Δv _y、Δv _zRespectively the components of the speed of the projectile under the navigation system on three coordinate axes,Xas is the current state quantity, the state quantity,Vto measure noise;

Hto measure the transition matrix, as shown in equation (14):

(14)

the system equation and the measurement equation required for constructing the Kalman filter are described above;

for a ballistic assisted inertial navigation system, the kalman filter equation takes the form:

after each measurement update, the inertial navigation system is corrected using the existing error estimates for optimal position, velocity, and attitude, and the covariance matrix is forward predicted in time, as shown in equation (15):

(15)

wherein phi_kIs obtained by discretizing the system error equationt _k A time system transfer matrix;P _{k /k+1}is represented byt _k Predicted time of dayt _k+1The expectation of the covariance matrix at the moment of time,P _k/k is represented byt _k A covariance matrix of the time state estimate;Q _k setting a desired noise level for the system noise matrix based on inertial measurements of acceleration and angular rate;

the error estimation of the inertial navigation system state is derived by the equation (16):

(16)

wherein the content of the first and second substances,δx _{k+ /k+11}is an error estimate of the state of the system,K _k+1is composed oft _k+1The gain of the kalman filter at a time,δz _k+1is an observed value of the systematic error;

the covariance matrix is updated according to equations (17) and (18):

(17)

(18)

in the formula (I), the compound is shown in the specification,P _{k+ /k+11}is represented byt _k+1A covariance matrix of the time state estimate;Ris the measurement noise, and the elements of the matrix are set according to the expected gyroscope and accelerometer measurement noise levels.

5. The missile-borne integrated navigation method based on ballistic model constraints according to claim 4, wherein the navigation state quantity is corrected in step 4 to achieve optimal state estimation, specifically, after each measurement value is updated, the inertial navigation state quantity is corrected by using the current optimal error estimation value, and the correction equation is as follows:

1) speed and position correction

The velocity and position are corrected by subtracting the estimate error from the estimate of these two quantities by the inertial system, as shown in equation (19):

(19)

in the formula (I), the compound is shown in the specification,x _c as the state quantities of the speed and the position,for the purpose of the velocity and position estimates,δxvelocity and position estimate errors;

2) attitude correction

Modified directional cosine matrixC _cAs shown in equation (20):

(20)

in the formula (I), the compound is shown in the specification,Cfor the direction cosine matrix of the one-step prediction,Iis an identity matrix of 3 rows and 3 columns,an antisymmetric matrix for attitude angle estimation errors;

will be provided withC _cAndCthe component forms are written as a function of the corrected quaternion parameter and the estimated quaternion parameter, respectivelya _c b _c c _c d _c ]Andexpressed, the estimated quaternion parameters are directly modified by equation (21):

(21)

wherein the content of the first and second substances,δα、δβ、δγin order to be an estimation error of the attitude angle,a _c 、b _c 、c _c 、d _c for modified directional cosine matricesC _cCorresponding to the four components of the quaternion,directional cosine matrix for one-step predictionCCorresponding toFour components of a quaternion are estimated.

Technical Field

The invention relates to the technical field of ballistic construction and integrated navigation, in particular to a ballistic model constraint-based missile-borne integrated navigation method.

Background

The guided projectile is a weapon with long attack distance, strong lethality and wide killing range, has great strategic significance and puts forward higher requirements on the stability and the accuracy of a navigation system. Nowadays, the combined navigation mode which is widely applied and has mature technology is inertial navigation/satellite combined navigation, and has better navigation and positioning effects.

The inertial navigation/satellite combined navigation system performs data fusion through a Kalman filter, and the filter can output satellite data and inertial navigation data on the premise of receiving the satellite data and the inertial navigation data. The Strapdown Inertial Navigation System (SINS) requires low cost, can work independently, and can obtain more detailed Navigation information. However, the positioning data obtained by inertial navigation calculation has accumulated errors, and the accumulated errors can be dispersed along with time, so that the long-term working state is not facilitated.

A Global Navigation Satellite System (GNSS) just makes up for the disadvantages of the inertial Navigation System, and not only does the error not diverge with time, but also the accuracy of the data is relatively high. At present, the Beidou Navigation Satellite System (BDS) in China realizes global networking and can provide positioning data with certain precision. However, in the high dynamic environment of cannonball launching, satellite signals are extremely easy to be interfered or can be in an unlocked state for a long time, and the combined navigation system cannot work normally.

Aiming at the condition that the satellite signal is unlocked in the flying process of the cannonball, a BP neural network model and an Elman neural network model are respectively designed for Zhao-Xuefeng masters of the university of electronic science and technology, the output error of the inertial navigation system is predicted, and the inertial navigation system is corrected by utilizing the predicted value to obtain the positioning data under the satellite signal loss. A Radial Basis Function (RBF) neural network is constructed by Loringa Semeniuk and Aboelmagd nourdedin of the Canada royal academy of military affairs to predict the inertial navigation error, and an input delay dynamic neural network is designed to train the inertial navigation output error, so that the position prediction precision is improved. However, the neural network method requires a lot of training and prediction offline, and is poor in real-time performance. The xu soldier doctor of Nanjing university of science and technology proposes a vector frequency tracking algorithm assisted by a projectile expectation trajectory, which can inhibit the short-time interruption of satellite signals, but the expectation trajectory is generated in advance by software, and the influence of trajectory deviation caused by actual conditions on combined navigation is not considered, so that the algorithm is low in precision and poor in reliability.

Disclosure of Invention

The invention aims to provide a missile-borne integrated navigation method based on ballistic model constraint, which can ensure that higher integrated navigation precision can be realized under the condition of satellite signal loss.

The technical solution for realizing the purpose of the invention is as follows: a missile-borne integrated navigation method based on ballistic model constraint comprises the following steps:

step 1, constructing an initial ballistic model according to various disturbances suffered by a shell in a flying process to obtain a flying track of the shell, and calculating by ballistic to obtain the instantaneous attitude, speed and position information of a projectile;

step 2, performing inertial navigation resolving according to the inertial navigation data to obtain instantaneous attitude, speed and position information of the projectile;

step 3, subtracting the instantaneous attitude, speed and position information obtained in the step 1 and the step 2 to obtain an acceleration error, a speed error, a position error and an attitude angle error of the projectile;

step 4, when the satellite signal is abnormal, constructing a Kalman filtering error equation according to the acceleration error, the speed error, the position error and the attitude angle error, and correcting the navigation state quantity to realize the optimal state estimation;

and 5, reconstructing a ballistic model by taking the current corrected state quantity as an initial quantity every set time period, and repeating the navigation correction process of the steps 1-4.

Compared with the prior art, the invention has the following remarkable advantages: (1) by utilizing the constructed ballistic model, more navigation data observation values can be obtained on the basis of not increasing hardware; (2) the ballistic model is adopted for assistance, so that higher navigation precision can be still obtained under the condition that the satellite signal is unlocked; (3) and reconstructing the trajectory by taking the current corrected state quantity as an initial quantity at intervals, so that the high reliability of the trajectory data can be ensured.

Drawings

FIG. 1 is a block diagram of the combined SINS/BDS/ballistic navigation system of the present invention.

FIG. 2 is a graph showing simulated north position errors of the method and the conventional method when the simulated satellite signals are out-of-lock.

FIG. 3 is a simulation graph of east position error of the method and the conventional method under the condition of loss of lock of the simulated satellite signal.

FIG. 4 is a simulation graph of the position error in the sky between the present method and the conventional method when the simulated satellite signal is out of lock.

FIG. 5 is a simulation curve diagram of pitch angle error between the method and the conventional method under the condition of loss of lock of the simulated satellite signal.

FIG. 6 is a graph showing the simulation of roll angle error in the present method and the conventional method under the condition of loss of lock of the simulated satellite signal.

FIG. 7 is a simulation graph of the course angle error of the method and the conventional method under the condition of loss of lock of the simulated satellite signals.

FIG. 8 is a graph showing a simulation curve of north direction velocity error of the method and the conventional method under the condition of loss of lock of the simulated satellite signal.

FIG. 9 is a simulation graph of east direction velocity error of the method and the conventional method under the condition of loss of lock of the simulated satellite signal.

FIG. 10 is a simulation graph of the direction-of-the-sky velocity error of the present method versus the conventional method under the condition of loss of lock of the simulated satellite signal.

Detailed Description

Under the high dynamic environment of cannonball launching, satellite signals are extremely easy to be interfered or can be in an unlocked state for a long time, and the precision of the integrated navigation system is greatly reduced. Based on the ballistic reconstruction technology, the combined navigation is carried out by adopting ballistic model assistance under the condition of satellite signal loss, so that the system can still obtain relatively accurate positioning data when the satellite signal is lost.

With reference to fig. 1, the invention provides a missile-borne integrated navigation method based on ballistic model constraints, which includes the following steps:

step 1, constructing an initial ballistic model according to various disturbances suffered by a shell in a flying process to obtain a flying track of the shell, and calculating by ballistic to obtain the instantaneous attitude, speed and position information of a projectile;

step 2, performing inertial navigation resolving according to the inertial navigation data to obtain instantaneous attitude, speed and position information of the projectile;

step 3, subtracting the instantaneous attitude, speed and position information obtained in the step 1 and the step 2 to obtain an acceleration error, a speed error, a position error and an attitude angle error of the projectile;

step 4, when the satellite signal is abnormal, constructing a Kalman filtering error equation according to the acceleration error, the speed error, the position error and the attitude angle error, and correcting the navigation state quantity to realize the optimal state estimation;

and 5, reconstructing a ballistic model by taking the current corrected state quantity as an initial quantity every set time period, and repeating the navigation correction process of the steps 1-4.

As a specific implementation manner, in step 1, an initial ballistic model is constructed according to various disturbances suffered by the cannonball during the flying process to obtain the flying trajectory of the cannonball, and the instantaneous attitude, speed and position information of the cannonball is obtained through ballistic solution, which is specifically as follows:

an improved ballistic equation system is adopted to establish an outer ballistic model, and a ground coordinate system is defined firstlyO-xyzMissile body coordinate systemO-x ₁ y ₁ z ₁And ballistic coordinate systemO-x ₂ y ₂ z ₂；

Aiming at the selected cannonball, the quality of the cannonball is set asmThe flying speed at the time of launch isThe moment of momentum of the projectile relative to the center of mass isH(ii) a Comprehensively considering an atmospheric density model and a pneumatic model, wherein the air density changes along with the change of the altitude in the flight process, and the pneumatic coefficient changes along with the change of the speed in the flight process, so that an external ballistic six-degree-of-freedom model equation set is established;

equation of motion of center of massIn ballistic coordinate systemO-x ₂ y ₂ z ₂The components on the middle three axes are shown in the dynamic equation set of the center of mass of the projectile in the formula (1):

(1)

wherein the content of the first and second substances,is the acceleration vector of the projectile and,is the velocity vector of the projectile and,vis a scalar quantity of the speed of the projectile,θ ₂in order to form the inclination angle of the trajectory,θ _a is the ballistic declination;

F _{x 2} ，F _{y 2} ，F _{z 2}the projectile is respectively in a trajectory coordinate systemO-x ₂ y ₂ z ₂The components on three coordinate axes neglect the influence of earth rotation, and are specifically expressed as the formula(2) Shown in the figure:

(2)

in the formula (I), the compound is shown in the specification,θ ₂in order to form the inclination angle of the trajectory,γ _v is a velocity ramp angle;

X、Y、Zrespectively aerodynamic drag, magnus force and lift, as shown in equation (3):

(3)

wherein the content of the first and second substances,ρin order to be the density of the air,vin order to determine the speed of the projectile,sthe area of the shot is the area of the shot,c _x ，c _y ，c _z respectively, aerodynamic drag coefficient, magnus force coefficient and lift coefficient;

when the wind speed is influenced, the speed of the projectile is changed,is the relative speed of the projectile or pellets,W _x2，W _y2，W _z2the components of the atmospheric wind speed on three axes of a ballistic coordinate system are respectively;

the system of kinetic equations for motion around the centroid is as follows:

(4)

in the formula (I), the compound is shown in the specification,J _x1，J _y1，J _z1in the body coordinate system for the moment of inertia of the projectile, respectivelyO-x ₁ y ₁ z ₁The components of the three coordinate axes are divided into three coordinate axes,ω _x1，ω _y1，ω _z1the rolling angular velocity, the pitch angular velocity and the yawing of the projectile under the projectile coordinate systemThe angular velocity of the light beam is measured,M _x1，M _y1，M _z1roll moment, pitch moment and yaw moment under the elastic body coordinate system respectively;

the ball being subjected to a rolling momentM _x1Pitching momentM _y1And yaw momentM _z1Respectively shown in formula (5):

(5)

wherein the content of the first and second substances,ρin order to be the density of the air,vin order to determine the speed of the projectile,sthe area of the shot is the area of the shot,lis the length of the projectile;

in the ground coordinate systemO-xyzIn (3), the kinematic equation of the projectile is shown in formula (6):

(6)

wherein the content of the first and second substances,x、y、zrespectively the components of the position of the projectile in the ground coordinate system in three axial directions,tis time;

the motion of the projectile around the center of mass can be decomposed into rotary motion around three coordinate axes in a ground coordinate systemO-xyzThen, the kinematic equation of the projectile rotating around the center of mass is shown as formula (7):

(7)

in the formula (I), the compound is shown in the specification,θ、γ、Ψrespectively representing the pitch angle, the roll angle and the yaw angle of the projectile;ω _x 、ω _y 、ω _z the components of the rotating angular speed of the projectile on three coordinate axes of a ground coordinate system are shown;

the equations form a mathematical model of the projectile outer trajectory, a first-order differential equation is solved by using a Runge Kutta method, instantaneous attitude, speed and position information of the projectile is obtained, and the instantaneous attitude, speed and position information is provided for an inertial navigation system to carry out error correction.

As an embodiment, a ground coordinate systemO-xyzMissile body coordinate systemO-x ₁ y ₁ z ₁And ballistic coordinate systemO-x ₂ y ₂ z ₂Specifically, the following are defined:

(a) ground coordinate systemO-xyz: the ground coordinate system is fixedly connected with the earth surface and is an inertial system, and the cannonball launching point is taken as the origin of the coordinate systemOCoordinate axisOx、Oy、OzPointing to east, north and sky respectively;

(b) projectile coordinate systemO-x ₁ y ₁ z ₁: origin of projectile coordinate systemOIs the mass center of the cannonball,Ox ₁the axis is consistent with the longitudinal central axis of the projectile body, and the front part of the projectile body is taken as the positive;Oy ₁axis perpendicular toOx ₁The longitudinal section of the projectile in which the shaft is located,Oz ₁axis perpendicular toO-x ₁ y ₁ Plane, with a direction positive, andO-x ₁ y ₁ z ₁forming a right-hand rectangular coordinate system;

(c) ballistic coordinate systemO-x ₂ y ₂ z ₂: ballistic coordinate system and velocityVIs connected to, at the origin ofOIs the instantaneous center of mass of the projectile,Ox ₂velocity vector of shaft and projectile all the timeVThe two layers are overlapped with each other,Oz ₂the axis being located containing the velocity vectorVIn the vertical plane and perpendicular toOx ₂Axis, positive upward;Ox ₂、Oy ₂、Oz ₂the three axes form a right-hand rectangular coordinate system.

As a specific implementation manner, in step 4, when the satellite signal is abnormal, a kalman filtering error equation is constructed according to the acceleration error, the velocity error, the position error, and the attitude angle error, which is specifically as follows:

the cannonball state equation is composed of an acceleration error, a speed error, a position error and an attitude angle error equation, and the state equation is shown in the formula (8):

(8)

wherein the content of the first and second substances,Fin order to be a state transition matrix,Gin order to be a system noise transfer matrix,Wfor system noise, state variablesXAs shown in formula (9):

(9)

where the variable is the position errorδPSpeed errorδVAttitude angle errorδAAnd accelerometer null shift ∇, gyroscope null shiftε；

(10)

(11)

(12)

In the formula (I), the compound is shown in the specification,is the accelerometer error;is the gyroscope drift error;O _3×3a zero matrix with 3 rows and 3 columns;I _3×3an identity matrix of 3 rows and 3 columns;is the attitude transformation moment from the carrier system to the navigation system;f _n x is an antisymmetric matrix of specific forces in the navigation system;B ₁andB ₂respectively, time constant matrixes of zero-bias instability of the accelerometer and the gyroscope;

when the satellite signal is absent, the current motion state value is used as an initial quantity to construct a trajectory, the position and the speed under a navigation coordinate system are obtained through calculation and coordinate conversion, at the moment, the observed quantity selects a position error and a speed error, and a measurement equation is shown as a formula (13):

(13)

in the formula,. DELTA.p _x、Δp _y、Δp _zRespectively, the components of the position error of the projectile in the navigation system on three coordinate axes, deltav _x、Δv _y、Δv _zRespectively the components of the speed of the projectile under the navigation system on three coordinate axes,Xas is the current state quantity, the state quantity,Vto measure noise;

Hto measure the transition matrix, as shown in equation (14):

(14)

the system equation and the measurement equation required for constructing the Kalman filter are described above;

for a ballistic assisted inertial navigation system, the kalman filter equation takes the form:

after each measurement update, the inertial navigation system is corrected using the existing error estimates for optimal position, velocity, and attitude, and the covariance matrix is forward predicted in time, as shown in equation (15):

(15)

wherein phi_kIs obtained by discretizing the system error equationt _k A time system transfer matrix;P _{k /k+1}is represented byt _k Predicted time of dayt _k+1The expectation of the covariance matrix at the moment of time,P _k/k is represented byt _k A covariance matrix of the time state estimate;Q _k setting a desired noise level for the system noise matrix based on inertial measurements of acceleration and angular rate;

the error estimation of the inertial navigation system state is derived by the equation (16):

(16)

wherein the content of the first and second substances,δx _{k+ /k+11}is an error estimate of the state of the system,K _k+1is composed oft _k+1The gain of the kalman filter at a time,δz _k+1is an observed value of the systematic error;

the covariance matrix is updated according to equations (17) and (18):

(17)

(18)

in the formula (I), the compound is shown in the specification,P _{k+ /k+11}is represented byt _k+1A covariance matrix of the time state estimate;Ris the measurement noise, and the elements of the matrix are set according to the expected gyroscope and accelerometer measurement noise levels.

As a specific implementation manner, the navigation state quantity is corrected in step 4 to realize the optimal state estimation, specifically, after each measurement value is updated, the inertial navigation state quantity is corrected by using the current optimal error estimation value, and the correction equation is as follows:

1) speed and position correction

The velocity and position are corrected by subtracting the estimate error from the estimate of these two quantities by the inertial system, as shown in equation (19):

(19)

in the formula (I), the compound is shown in the specification,x _c as the state quantities of the speed and the position,for the purpose of the velocity and position estimates,δxvelocity and position estimate errors;

2) attitude correction

Modified directional cosine matrixC _cAs shown in equation (20):

(20)

in the formula (I), the compound is shown in the specification,Cfor the direction cosine matrix of the one-step prediction,Iis an identity matrix of 3 rows and 3 columns,an antisymmetric matrix for attitude angle estimation errors;

will be provided withC _cAndCthe component forms are written as a function of the corrected quaternion parameter and the estimated quaternion parameter, respectivelya _c b _c c _c d _c ]Andexpressed, the estimated quaternion parameters are directly modified by equation (21):

(21)

wherein the content of the first and second substances,δα、δβ、δγin order to be an estimation error of the attitude angle,a _c 、b _c 、c _c 、d _c for modified directional cosine matricesC _cCorresponding to the four components of the quaternion,directional cosine matrix for one-step predictionCThe corresponding four components of the estimated quaternion.

The invention is described in further detail below with reference to the figures and the embodiments.

Examples

And carrying out simulation analysis on the navigation scheme designed by the patent by using MATLAB, and verifying the correctness and the effectiveness of the navigation scheme. In order to compare the effects of the SINS/BDS/ballistic combined navigation scheme and the SINS/BDS combined navigation scheme, BDS signals within 20-50 seconds are set to be in an abnormal state during simulation, and the two navigation schemes are used for simulation. When the scheme of the patent is used for simulation, the precision of the position and the speed provided by the trajectory can be reduced along with the time, so that the trajectory is updated every 5 seconds in the simulation. The specific process is as follows: firstly, after initial conditions are set, a generated ballistic model is resolved to obtain the instantaneous speed, position and attitude angle of a projectile, meanwhile, inertial navigation resolving is carried out according to inertial navigation data to obtain the instantaneous position, speed and attitude angle of the projectile, the position and speed values obtained in the two modes are subjected to difference and used as measurement values to carry out Kalman filtering, and then the position, speed and attitude are corrected. After 5 seconds, the trajectory is updated with the current optimum state quantity, and then the above process is repeated.

Initial position of simulation: a latitude of 32.028 ° N, a longitude of 118.8474 ° E; initial attitude: the roll angle and the course angle are both 0 degree, and the pitch angle is 45 degrees; initial speed: the size was 550 m/s.

Inertial device error: the zero offset stability of the SINS gyroscope is 1 degree/h, the random zero offset is 0.2 degree/h, the mean square error of the white noise driven by the first-order Markov noise is 0.2 degree/h, and the correlation time is 3600 s; the zero offset stability of the accelerometer is 80ug, the random offset is 40ug, the white noise mean square error is driven by first-order Markov noise is 40ug, the correlation time is 3600s, and the data output period of the inertial device is 0.01 s.

Fig. 2 to 4 are simulation diagrams of position error analysis of the SINS/BDS/ballistic method of the present invention and the conventional SINS/BDS method under the condition of simulated satellite signal loss of lock, where fig. 2 is a north position error simulation curve, fig. 3 is an east position error simulation curve, and fig. 4 is a sky position error simulation curve. Fig. 5-7 are simulation diagrams of the analysis of attitude errors of the SINS/BDS/ballistic method of the invention and the conventional SINS/BDS method under the condition of simulated satellite signal loss-of-lock, wherein fig. 5 is a pitch angle error simulation curve, fig. 6 is a roll angle error simulation curve, and fig. 7 is a course angle error simulation curve. Fig. 8-10 are simulation diagrams of velocity error analysis of the SINS/BDS/ballistic method of the present invention and the conventional SINS/BDS method under the condition of simulated satellite signal loss of lock, wherein fig. 8 is a north velocity error simulation curve, fig. 9 is an east velocity error simulation curve, and fig. 10 is a sky velocity error simulation curve. Therefore, the navigation scheme of the method has the error far smaller than that of the traditional SINS/BDS combined navigation scheme during the BDS signal aberration period, and the reliability and robustness of the original SINS/BDS combined navigation system are improved.

In conclusion, on the basis of adopting the initial ballistic constraint, the method and the device reconstruct the ballistic trajectory by taking the current corrected state quantity as the initial quantity at regular intervals for auxiliary combined navigation when the satellite loses the lock, obviously overcome the influence of various noise environments on the reference ballistic trajectory in the flight process, and greatly improve the precision and the practicability of the ballistic constraint algorithm. .