CN115560756A - Miniature self-seeking missile strapdown navigation method under launching coordinate system - Google Patents

Miniature self-seeking missile strapdown navigation method under launching coordinate system Download PDF

Info

Publication number
CN115560756A
CN115560756A CN202211034272.4A CN202211034272A CN115560756A CN 115560756 A CN115560756 A CN 115560756A CN 202211034272 A CN202211034272 A CN 202211034272A CN 115560756 A CN115560756 A CN 115560756A
Authority
CN
China
Prior art keywords
coordinate system
missile
geographic
initial
axis
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
CN202211034272.4A
Other languages
Chinese (zh)
Other versions
CN115560756B (en
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.)
Kaifeng Navigation Control Technology Co ltd
Original Assignee
Beijing Kaikai Hangyu Navigation Control Technology Co ltd
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 Beijing Kaikai Hangyu Navigation Control Technology Co ltd filed Critical Beijing Kaikai Hangyu Navigation Control Technology Co ltd
Priority to CN202211034272.4A priority Critical patent/CN115560756B/en
Publication of CN115560756A publication Critical patent/CN115560756A/en
Application granted granted Critical
Publication of CN115560756B publication Critical patent/CN115560756B/en
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/20—Instruments for performing navigational calculations
    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

The invention provides a miniature self-seeking missile strapdown navigation method under a launching coordinate system, which comprises the following steps: establishing a geographic coordinate system, a carrier coordinate system and a transmitting coordinate system; calculating an initial attitude matrix and a quaternion, and establishing an initial state of a strapdown navigation algorithm; measuring angular rate and specific force in real time; updating the attitude in the geographic coordinate system, and obtaining the attitude in the emission coordinate system through a coordinate conversion matrix; and updating the speed and the position under the geographic coordinate system, and obtaining the speed and the position under the emission coordinate system through a coordinate transformation matrix. The invention takes a mature geographic coordinate system as a navigation coordinate system, the calculation formula does not need to be deduced again, the labor cost is greatly reduced, and the attitude, the speed and the position under the geographic coordinate system are converted into the attitude, the speed and the position under the emission coordinate system only by a coordinate conversion matrix on the basis.

Description

Miniature self-seeking missile strapdown navigation method under launching coordinate system
Technical Field
The invention relates to the technical field of missile strapdown navigation, in particular to a miniature self-seeking missile strapdown navigation method under a launching coordinate system.
Background
The miniature self-seeking missile is suitable for modern war, even the development trend of future war, and complies with the historical trend. The miniature homing missile creatively adopts a knapsack type design, and a single-soldier knapsack type memory similar to a knapsack improves the dressing speed of soldiers in a battlefield, thereby improving the operation efficiency. The miniature self-seeking missile system is flexible, portable, can realize no matter what the missile system needs after being launched, does not need laser irradiation, greatly reduces personnel required by a battlefield, and reduces the exposure risk on the battlefield.
And the miniature self-seeking missile sampling and launching coordinate system is used as a navigation coordinate system. In a conventional strapdown navigation positioning method, a geographic coordinate system is generally a navigation coordinate system. The emission coordinate system is inconsistent with the geographic coordinate system, and the navigation resolving formula under the conventional geographic coordinate system needs to be deduced again.
Disclosure of Invention
In view of the above, the present invention has been developed to provide a method for miniature homing missile strapdown navigation in a launch coordinate system that overcomes or at least partially solves the above-mentioned problems.
According to one aspect of the invention, the method for the miniature self-seeking missile strapdown navigation under the emission coordinate system comprises the following steps: step 100: establishing a geographic coordinate system, a terrestrial coordinate system, a missile coordinate system and a transmitting coordinate system;
step 200: according to initial information of the miniature self-seeking missile, establishing navigation initial information by taking a northeast geographic coordinate system as a navigation coordinate system, wherein the navigation initial information comprises an initial direction cosine matrix between the emission coordinate system and the geographic coordinate system
Figure BDA0003818564210000021
And an initial quaternion Q;
step 300: according to the emission point O of the initial binding f Position and azimuth angle alpha, and obtaining a direction cosine matrix between the terrestrial coordinate system and the transmitting coordinate system
Figure BDA0003818564210000022
Step by step calculation of commanded angular rate
Figure BDA0003818564210000023
Step 400: according to the initial direction cosine matrix
Figure BDA0003818564210000024
Determining an attitude transformation matrix
Figure BDA0003818564210000025
And obtaining the projection of the angular rate from the geographic coordinate system to the inertial system on the carrier coordinate system
Figure BDA0003818564210000026
And the projection of the angular rate of the carrier coordinate system to the geographic coordinate system onto the carrier coordinate system
Figure BDA0003818564210000027
Step 500: projecting on the carrier coordinate system according to the angular rate from the carrier coordinate system to the geographic coordinate system
Figure BDA0003818564210000028
Updating the quaternion Q;
step 600: calculating the initial direction cosine matrix according to the quaternion Q
Figure BDA0003818564210000029
Step 700: deducing a direct conversion relation between the transmitting coordinate system and the carrier coordinate system through 3 times of coordinate rotation according to a matrix conversion principle
Figure BDA00038185642100000210
Step 800: deducing speed updating;
step 900: obtaining the speed under the emission coordinate system according to the northeast speed under the geographic coordinate system; the conversion formula is as follows:
Figure BDA00038185642100000211
step 1000: deducing location updating; the formula is as follows:
Figure BDA0003818564210000031
step 1100: and calculating to obtain the position under the terrestrial coordinate system according to the position under the geographic coordinate system, thereby obtaining the speed under the emission coordinate system.
Optionally, the step 100: establishing a geographic coordinate system, a terrestrial coordinate system, a projectile coordinate system and a launching coordinate system comprises the following steps:
establishing a geographic coordinate system n, OX by using the mass center of the miniature self-seeking missile as the origin n Axis pointing to east, OY n Axis pointing to true north, OZ n The shaft is vertical to the local horizontal plane and is upward along the local vertical line; a geographic coordinate system n is adopted as a navigation coordinate system;
establishing an earth coordinate system e, OX by using the center of the earth as an origin e Axis and OY e Axis in the equatorial plane of the earth, OX e Axis pointing to the principal meridian, OZ e The axis being the earth's rotation axis, OY e Shaft and OX e Axis, OZ e The shaft forms a right-hand coordinate system, and an earth coordinate system e is fixedly connected with the earth;
establishing a missile coordinate system b, OX by using the mass center of the missile as the origin b Axis coincident with the longitudinal axis of the projectile body and pointing to the positive head, OY b The axis being in the plane of symmetry of the longitudinal axis of the projectile, perpendicular to OX b Axial, positive upward, OZ b Axis perpendicular to X b OY b Plane, the direction is determined according to the right-hand rule;
the missile control system uses parameters under a launching coordinate system as control parameters, and establishes a launching coordinate system f by taking a launching point as an origin, wherein OX f The axis is the line from the launch point to the target point, pointing in the target direction, OY f Axial origin O f With the vertical line pointing upwards, OZ f Axis perpendicular to X f OY f Plane, direction determined according to the right-hand rule, OX f The angle between the axis and north is defined as the azimuth angle alpha, along OY f Viewed in the positive direction of the axis, counterclockwisePositive and negative clockwise, the real-time speed and position required by the missile-borne control system are the speed under the emission coordinate
Figure BDA0003818564210000032
And position (X) f ,Y f ,Z f )。
Optionally, the step 200: according to the initial information of the miniature self-seeking missile, taking the northeast geographic coordinate system as a navigation coordinate system, and establishing the navigation initial information specifically comprises the following steps:
before missile launching, 2-second static data are collected, and the average values of the obtained accelerometers are respectively
Figure BDA0003818564210000041
Obtaining an initial pitch angle theta from static data 0 And roll angle gamma 0 Respectively as follows:
Figure BDA0003818564210000042
Figure BDA0003818564210000043
initial yaw angle
Figure BDA0003818564210000044
Initial values can be obtained through the binding of the pop-up control system, and the initial direction cosine matrix is known according to three initial attitude angles
Figure BDA0003818564210000045
And the initial quaternion Q;
Figure BDA0003818564210000046
Figure BDA0003818564210000047
quaternion normalization, as follows:
Figure BDA0003818564210000048
optionally, the step 300: according to the emission point O of the initial binding f Position and azimuth angle alpha, and obtaining a direction cosine matrix between the terrestrial coordinate system and the transmitting coordinate system
Figure BDA0003818564210000051
Step by step calculation of commanded angular rate
Figure BDA0003818564210000052
The method specifically comprises the following steps:
the emission point O of the initial binding f The positions include: longitude λ in geographic coordinate system 0 Latitude L 0 Height H 0 ；
Figure BDA0003818564210000053
Knowing the geographic coordinate system position (L, lambda, H) of a certain point, the position (X) of the earth coordinate system e is calculated e ,Y e ,Z e )：
Figure BDA0003818564210000054
In the formula, R N Is the curvature radius of the unitary point-fourth-element ring,
Figure BDA0003818564210000055
the earth ellipsoid model adopts a WGS-84 earth coordinate system and a long half shaft R a =6378137m, semi-axis short R b =6356752.314m, global oblateness
Figure BDA0003818564210000056
Figure BDA0003818564210000057
Square of first eccentricity
Figure BDA0003818564210000058
Obtaining a position conversion formula from a geographic coordinate system to a transmitting coordinate system;
Figure BDA0003818564210000059
wherein (X) of ,Y of ,Z of ) Is an emission point O f (ii) coordinates in the terrestrial coordinate system of (X) e ,Y e ,Z e ) Is the coordinate of the missile body in the earth coordinate system of the real-time position (X) f ,Y f ,Z f ) The coordinates of the real-time position of the projectile body in the launching coordinate system are obtained through conversion according to a formula (7).
Optionally, the step 400: according to the initial direction cosine matrix
Figure BDA0003818564210000061
Determining an attitude transformation matrix
Figure BDA0003818564210000062
And obtaining the projection of the angular rate from the geographic coordinate system to the inertial system on the carrier coordinate system
Figure BDA0003818564210000063
And the projection of the angular rate of the carrier coordinate system to the geographic coordinate system onto the carrier coordinate system
Figure BDA0003818564210000064
The method specifically comprises the following steps:
step by step calculation of commanded angular rate
Figure BDA0003818564210000065
And through the attitude transformation matrix
Figure BDA0003818564210000066
To obtain
Figure BDA0003818564210000067
And
Figure BDA0003818564210000068
angular rate of rotation of the earth
Figure BDA0003818564210000069
The angular rate of the terrestrial coordinate system relative to the geographic coordinate system is
Figure BDA00038185642100000610
Then the angular rate is commanded
Figure BDA00038185642100000611
In the formula, meridian plane curvature radius
Figure BDA00038185642100000612
By attitude transformation matrix
Figure BDA00038185642100000613
To obtain
Figure BDA00038185642100000614
And
Figure BDA00038185642100000615
Figure BDA00038185642100000616
Figure BDA00038185642100000617
in the formula, an attitude transformation matrix
Figure BDA00038185642100000618
Is that
Figure BDA00038185642100000619
By means of, i.e.
Figure BDA00038185642100000620
Figure BDA00038185642100000621
Is a projection of the angular velocity of the geographic coordinate system to the inertial system onto the carrier coordinate system,
Figure BDA00038185642100000622
is the projection of the angular velocity of the carrier coordinate system to the geographic coordinate system onto the carrier coordinate system.
Optionally, the step 500: projecting on the carrier coordinate system according to the angular rate from the carrier coordinate system to the geographic coordinate system
Figure BDA00038185642100000623
Updating the quaternion Q specifically includes:
the quaternion differential equation is as follows:
Figure BDA00038185642100000624
the differential equation is calculated using the fourth-order Longkuta method, as follows
Figure BDA0003818564210000071
Figure BDA0003818564210000072
Figure BDA0003818564210000073
Figure BDA0003818564210000074
Figure BDA0003818564210000075
And further carrying out normalization processing on the quaternion obtained by calculation.
Optionally, the step 600: calculating the initial direction cosine matrix according to the quaternion Q
Figure BDA0003818564210000076
The method specifically comprises the following steps:
computing an attitude matrix from quaternions
Figure BDA0003818564210000077
Figure BDA0003818564210000078
Optionally, the step 700: deducing a direct conversion relation between the transmitting coordinate system and the carrier coordinate system through 3 times of coordinate rotation according to a matrix conversion principle
Figure BDA0003818564210000079
The method specifically comprises the following steps:
Figure BDA00038185642100000710
in the formula (I), the compound is shown in the specification,
Figure BDA00038185642100000711
θ f 、γ f the attitude angles of the carrier coordinate system relative to the transmitting coordinate system are respectively a course angle, a pitch angle and a roll angle;
at the same time, the derivation is obtained,
Figure BDA0003818564210000081
according to the above two formulae
Figure BDA0003818564210000082
The three attitude angles of the carrier coordinate system relative to the emission coordinate system are calculated as follows:
yaw angle
Figure BDA0003818564210000083
Pitch angle
Figure BDA0003818564210000084
Figure BDA0003818564210000085
Roll angle gamma f ＝atan(F 32 /F 33 ) (17)
Figure BDA0003818564210000086
Optionally, the step 800: the deriving speed update specifically includes:
Figure BDA0003818564210000087
in the formula, f n Is the specific force of the transformation of the carrier coordinate system into the geographic coordinate system, i.e.
Figure BDA0003818564210000088
g n Is a representation of gravitational acceleration in a geographic coordinate system, g n ＝[0 0 g z ] T Wherein, in the step (A),
g z ＝9.78049×(1+0.005288(sinL) 2 )-3.0855e -6 ×H。
optionally, the navigation method further includes:
calculating an initial attitude matrix and a quaternion according to the data of the missile at the power-on static moment, establishing an initial state of a strapdown navigation algorithm, and obtaining a plurality of attitude matrices;
measuring the projection of the angular rate of the missile relative to the inertial space in a carrier coordinate system and the projection of the specific force relative to the inertial space in the carrier coordinate system in real time;
converting the angular rate and the specific force into physical quantities under the geographic coordinate system through an attitude matrix;
in an angular rate integral loop, a quaternion is calculated by using the measured angular velocity and a fourth-order Longkuta method, and a coordinate conversion matrix from the carrier coordinate system to the geographic coordinate system is obtained
Figure BDA0003818564210000091
Thereby obtaining a coordinate transformation matrix from the carrier coordinate system to the transmission coordinate system
Figure BDA0003818564210000092
Thereby calculating to obtain three attitude angles;
in an acceleration integration loop, using
Figure BDA0003818564210000093
And converting the measured value of the sensor into the geographic coordinate system, compensating the gravity acceleration, and obtaining the speed and the position under the transmitting coordinate system through the coordinate conversion matrix.
The invention provides a simple method for miniature self-seeking missile strapdown navigation under a launching coordinate system, which comprises the following steps: establishing a geographic coordinate system, a carrier coordinate system and a transmitting coordinate system; calculating an initial attitude matrix and a quaternion, and establishing an initial state of a strapdown navigation algorithm; measuring angular rate and specific force in real time; updating the posture under the geographic coordinate system, and obtaining the posture under the emission coordinate system through a coordinate conversion matrix; and updating the speed and the position under the geographic coordinate system, and obtaining the speed and the position under the emission coordinate system through a coordinate transformation matrix. The invention takes a mature geographic coordinate system as a navigation coordinate system, the calculation formula does not need to be deduced again, the labor cost is greatly reduced, and the attitude, the speed and the position under the geographic coordinate system are converted into the attitude, the speed and the position under the emission coordinate system through a coordinate conversion matrix on the basis.
The foregoing description is only an overview of the technical solutions of the present invention, and the embodiments of the present invention are described below in order to make the technical means of the present invention more clearly understood and to make the above and other objects, features, and advantages of the present invention more clearly understandable.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings needed to be used in the description of the embodiments are briefly introduced below, and it is obvious that the drawings in the following description are only some embodiments of the present invention, and it is obvious for those skilled in the art to obtain other drawings based on these drawings without creative efforts.
FIG. 1 is a block diagram of the layout of strapdown navigation algorithms of the present invention.
FIG. 2 is a flow chart of the strapdown navigation algorithm of the present invention.
Detailed Description
Exemplary embodiments of the present disclosure will be described in more detail below with reference to the accompanying drawings. While exemplary embodiments of the present disclosure are shown in the drawings, it should be understood that the present disclosure may be embodied in various forms and should not be limited to the embodiments set forth herein. Rather, these embodiments are provided so that this disclosure will be thorough and complete, and will fully convey the scope of the disclosure to those skilled in the art.
The terms "comprises" and "comprising," and any variations thereof, in the described embodiments of the invention and in the claims and drawings, are intended to cover a non-exclusive inclusion, such as, for example, a list of steps or elements.
The technical solution of the present invention is further described in detail with reference to the accompanying drawings and embodiments.
As shown in FIGS. 1-2, the invention provides a simple strapdown navigation method for a miniature homing missile in a launching coordinate system, and a strapdown navigation algorithm actually adopted by a QN-XXX miniature homing missile is taken as an example for explanation.
The coordinate system related to the invention is as follows:
establishing a geographic coordinate system n, OX by using the mass center of the miniature self-seeking missile as an origin n The axis pointing to the east, OY n Axis pointing to true north, OZ n The axis is perpendicular to the local horizontal plane and up the local vertical line. And adopting a geographic coordinate system n as a navigation coordinate system.
Establishing an earth coordinate system e, OX by using the center of the earth as an origin e Shaft and OY e Axis in the equatorial plane of the earth, OX e Axis pointing to the principal meridian, OZ e The axis being the earth's rotation axis, OY e Shaft and OX e Axis, OZ e The shaft forms a right-hand coordinate system, and an earth coordinate system e is fixedly connected with the earth.
Establishing a missile coordinate system b, OX by using the mass center of the missile as the origin b Axis coincident with the longitudinal axis of the projectile body and pointing to the positive head, OY b The axis being in the plane of symmetry of the longitudinal axis of the projectile, perpendicular to OX b Axial, positive upward, OZ b Axis perpendicular to X b OY b Plane, direction is determined according to the right hand rule.
The missile control system uses parameters in a launching coordinate system as control parameters. Establishing an emission coordinate system f, OX by using the emission point as an origin f The axis is the line from the launch point to the target point, pointing in the target direction, OY f Axial origin O f With the vertical line pointing upwards, OZ f Axis perpendicular to X f OY f Plane, direction is determined according to the right hand rule. OX f The angle between the axis and north is defined as the azimuth angle alpha, along OY f The counterclockwise direction is positive and the clockwise direction is negative when viewed from the positive direction. The real-time speed and position required by the missile-borne control system are both the speed under the emission coordinate
Figure BDA0003818564210000111
And position (X) f ,Y f ,Z f )。
The specific algorithm arrangement of the invention on the QN-XXX miniature homing missile is carried out according to the figure 1. The strapdown navigation algorithm arrangement method comprises the following steps: calculating an initial attitude matrix and quaternion according to the data of the missile at the power-on static moment, establishing an initial state of a strapdown navigation algorithm, and obtaining several important attitude matrices; measuring the projection of the angular rate of the missile relative to the inertial space in a carrier coordinate system and the projection of the specific force relative to the inertial space in the carrier coordinate system in real time; converting the angular rate and the specific force into physical quantities under a geographic coordinate system through the attitude matrix; in an angular rate integral loop, a quaternion is calculated by using the measured angular velocity and a fourth-order Longkuta method, and a coordinate conversion matrix from a carrier coordinate system to a geographic coordinate system is obtained
Figure BDA0003818564210000121
Thereby obtaining a coordinate transformation matrix from the carrier coordinate system to the emission coordinate system
Figure BDA0003818564210000122
Thereby calculating to obtain three attitude angles; in an acceleration integration loop, using
Figure BDA0003818564210000123
And converting the measured value of the sensor into a geographical coordinate system, compensating the gravity acceleration, and obtaining the speed and the position under the transmitting coordinate system through a coordinate conversion matrix.
The specific process of the invention is carried out according to figure 2, comprising the following steps:
before missile launching, 2-second static data are collected, and the average values of the obtained accelerometers are respectively
Figure BDA0003818564210000124
Obtaining an initial pitch angle theta from static data 0 And roll angle gamma 0 Respectively as follows:
Figure BDA0003818564210000125
Figure BDA0003818564210000126
initial yaw angle
Figure BDA0003818564210000127
The initial value can be obtained by the on-board control system binding. According to three initial attitude angles, an initial direction cosine matrix is known
Figure BDA0003818564210000128
And an initial quaternion Q.
Figure BDA0003818564210000129
Figure BDA0003818564210000131
Quaternions require normalization as follows:
Figure BDA0003818564210000132
step three: according to the emission point O of the initial binding f Location (longitude λ in geographic coordinate system) 0 Latitude L 0 Height H 0 ) And the azimuth angle alpha to obtain a direction cosine matrix between the earth coordinate system and the transmitting coordinate system
Figure BDA0003818564210000133
Knowing the geographic coordinate system position (L, lambda, H) of a certain point, the position (X) of the earth coordinate system e is calculated e ,Y e ,Z e )：
Figure BDA0003818564210000134
In the formula, R N Is the curvature radius of the unitary point-mortise ring.
Figure BDA0003818564210000135
The earth ellipsoid model adopts a WGS-84 earth coordinate system and a long half shaft R a =6378137m, semi-axis short R b =6356752.314m, global oblateness
Figure BDA0003818564210000136
Square of first eccentricity
Figure BDA0003818564210000137
And obtaining a position conversion formula from the geographic coordinate system to the transmitting coordinate system.
Figure BDA0003818564210000141
In the formula (X) of ,Y of ,Z of ) Is an emission point O f (ii) coordinates in the terrestrial coordinate system of (X) e ,Y e ,Z e ) Is the coordinate of the missile body in the earth coordinate system of the real-time position (X) f ,Y f ,Z f ) The coordinates of the real-time positions of the projectiles in the launching coordinate system are obtained through conversion according to the formula.
Step four: step by step calculation of commanded angular rate
Figure BDA0003818564210000142
And through the attitude transformation matrix
Figure BDA0003818564210000143
To obtain
Figure BDA0003818564210000144
And
Figure BDA0003818564210000145
angular rate of rotation of the earth
Figure BDA0003818564210000146
The angular rate of the terrestrial coordinate system relative to the geographic coordinate system is
Figure BDA0003818564210000147
Then the angular rate is commanded
Figure BDA0003818564210000148
Wherein the meridian plane radius of curvature R M ＝R a (1-e(2-3sinλ 2 ))。
Step five: by attitude transformation matrix
Figure BDA0003818564210000149
To obtain
Figure BDA00038185642100001410
And
Figure BDA00038185642100001411
Figure BDA00038185642100001412
Figure BDA00038185642100001413
in the formula, an attitude transformation matrix
Figure BDA00038185642100001414
Is that
Figure BDA00038185642100001415
By means of, i.e.
Figure BDA00038185642100001416
Figure BDA00038185642100001417
Is the projection of the angular velocity of the geographic coordinate system to the inertial system onto the carrier coordinate system,
Figure BDA00038185642100001418
is the projection of the angular velocity of the carrier coordinate system to the geographical coordinate system onto the carrier coordinate system.
By passing
Figure BDA00038185642100001419
Updating quaternion Q, and carrying out quaternion differential equation as follows:
Figure BDA00038185642100001420
the differential equation is calculated using the fourth-order Longkuta method, as follows
Figure BDA0003818564210000151
Figure BDA0003818564210000152
Figure BDA0003818564210000153
Figure BDA0003818564210000154
Figure BDA0003818564210000155
And further carrying out normalization processing on the quaternion obtained by calculation.
Step six: computing an attitude matrix from quaternions
Figure BDA0003818564210000156
Figure BDA0003818564210000157
Step seven: the difference from the standard navigation algorithm is that the direct conversion relationship between the transmit coordinate system and the carrier coordinate system is derived from the matrix conversion principle by 3 coordinate rotations
Figure BDA0003818564210000158
In the formula (I), the compound is shown in the specification,
Figure BDA0003818564210000159
θ f 、γ f the attitude angle of the carrier coordinate system relative to the emission coordinate system is respectively a course angle, a pitch angle and a roll angle.
At the same time, the derivation is carried out,
Figure BDA00038185642100001510
according to the above two formulae
Figure BDA00038185642100001511
Calculates three attitude angles of the carrier coordinate system relative to the transmitting coordinate system as:
yaw angle
Figure BDA0003818564210000161
Pitch angle
Figure BDA0003818564210000162
Figure BDA0003818564210000163
Roll angle gamma f ＝atan(F 32 /F 33 ) (17)
Figure BDA0003818564210000164
Step eight: deriving a velocity update comprising the steps of:
Figure BDA0003818564210000165
in the formula (f) n Is the specific force of the transformation of the carrier coordinate system into the geographic coordinate system, i.e.
Figure BDA0003818564210000166
g n Is a representation of the acceleration of gravity in a geographical coordinate system, g n ＝[0 0 g z ] T Wherein, in the process,
z 2 -6 g＝9.78049×(1+0.005288(sinL))-3.0855e×H。
step nine: the difference from the standard navigation algorithm lies in that the velocity in the transmitting coordinate system can be obtained according to the velocity in the northeast of the earth in the geographic coordinate system, and the conversion formula is as follows:
Figure BDA0003818564210000167
step ten: derive location updates, the formula is as follows:
Figure BDA0003818564210000171
step eleven: the difference from the standard navigation algorithm is that the position in the terrestrial coordinate system is calculated by using formula (7) according to the position in the geographic coordinate system, so as to obtain the velocity in the transmitting coordinate system.
The strapdown navigation algorithm arrangement method under the geographic coordinate system comprises the following steps: calculating initial attitude matrix sum according to the static moment data of the missileQuaternion, establishing an initial state of a strapdown navigation algorithm, and obtaining several important attitude matrixes; measuring the projection of the angular rate of the missile relative to the inertial space in a carrier coordinate system and the projection of the specific force relative to the inertial space in the carrier coordinate system in real time; converting the angular rate and the specific force into physical quantities under a geographic coordinate system through the attitude matrix; in an angular rate integral loop, a quaternion is calculated by using the measured angular velocity and a fourth-order Longkuta method, and a coordinate conversion matrix from a carrier coordinate system to a geographic coordinate system is obtained
Figure BDA0003818564210000172
Thereby obtaining a coordinate transformation matrix from the carrier coordinate system to the emission coordinate system
Figure BDA0003818564210000173
Thereby calculating to obtain three attitude angles; in an acceleration integration loop, using
Figure BDA0003818564210000174
And converting the measured value of the sensor into a geographic coordinate system, compensating the gravity acceleration so as to obtain the speed and the position under the geographic coordinate system, and obtaining the speed and the position under the transmitting coordinate system through a coordinate conversion matrix.
The resolving process comprises the following steps: in the initialization stage, three initial attitude angles are obtained, and navigation parameters are initialized; calculating initial quaternion and initial attitude matrix, and calculating coordinate transformation matrix
Figure BDA0003818564210000175
Figure BDA0003818564210000176
According to a set navigation resolving period, sequentially updating quaternion and attitude matrix by taking a geographic coordinate system as a navigation coordinate system according to angular velocity and specific force information measured by a sensor and subjected to calibration compensation, and calculating the velocity and position under the geographic coordinate system; according to the coordinate transformation matrix, sequentially obtaining an attitude angle, a speed and a position under a transmitting coordinate system; then, in the next navigation resolving period, circulation is carried out。
The resolved main processing chip is a domestic megaprocessing chip GD32F103TBU6.
Has the advantages that: although the parameters required by the control system are parameters under the emission coordinate system, the method is still based on a mature geographic coordinate system as a navigation coordinate system, a calculation formula does not need to be deduced again, the labor cost is greatly reduced, and the attitude, the speed and the position under the geographic coordinate system are converted into the attitude, the speed and the position under the emission coordinate system through a coordinate conversion matrix on the basis.
The above embodiments are provided to further explain the objects, technical solutions and advantages of the present invention in detail, it should be understood that the above embodiments are merely exemplary embodiments of the present invention and are not intended to limit the scope of the present invention, and any modifications, equivalents, improvements and the like made within the spirit and principle of the present invention should be included in the scope of the present invention.

Claims (10)

1. A miniature self-seeking missile strapdown navigation method under a launching coordinate system is characterized by comprising the following steps:
step 100: establishing a geographic coordinate system, a terrestrial coordinate system, a missile coordinate system and a transmitting coordinate system;
step 200: according to initial information of the miniature self-seeking missile, establishing navigation initial information by taking a northeast geographic coordinate system as a navigation coordinate system, wherein the navigation initial information comprises an initial direction cosine matrix between the emission coordinate system and the geographic coordinate system
Figure FDA0003818564200000011
And an initial quaternion Q;
step 300: according to the emission point O of the initial binding f Position and azimuth angle alpha, and obtaining a direction cosine matrix between the terrestrial coordinate system and the transmitting coordinate system
Figure FDA0003818564200000012
Step by step calculation of commanded angular rate
Figure FDA0003818564200000013
Step 400: according to the initial direction cosine matrix
Figure FDA0003818564200000014
Determining an attitude transformation matrix
Figure FDA0003818564200000015
And obtaining the projection of the angular rate from the geographic coordinate system to the inertial system on the carrier coordinate system
Figure FDA0003818564200000016
And the projection of the angular rate of the carrier coordinate system to the geographic coordinate system on the carrier coordinate system
Figure FDA0003818564200000017
Step 500: projecting on the carrier coordinate system according to the angular rate from the carrier coordinate system to the geographic coordinate system
Figure FDA0003818564200000018
Updating the quaternion Q;
step 600: calculating the initial direction cosine matrix according to the quaternion Q
Figure FDA0003818564200000019
Step 700: deducing a direct conversion relation between the transmitting coordinate system and the carrier coordinate system through 3 times of coordinate rotation according to a matrix conversion principle
Figure FDA00038185642000000110
Step 800: deducing speed updating;
step 900: obtaining the speed under the emission coordinate system according to the northeast speed under the geographic coordinate system; the conversion formula is as follows:
Figure FDA00038185642000000111
step 1000: deducing location updating; the formula is as follows:
Figure FDA0003818564200000021
step 1100: and calculating to obtain the position under the terrestrial coordinate system according to the position under the geographic coordinate system, thereby obtaining the speed under the transmitting coordinate system.
2. The method for navigating the miniature homing missile strapdown according to the claim 1, wherein the step 100: establishing a geographic coordinate system, a terrestrial coordinate system, a projectile coordinate system and a launching coordinate system comprises the following steps:
establishing a geographic coordinate system n, OX by using the mass center of the miniature self-seeking missile as an origin n Axis pointing to east, OY n Axis pointing to true north, OZ n The shaft is vertical to the local horizontal plane and is upward along the local vertical line; adopting a geographic coordinate system n as a navigation coordinate system;
establishing an earth coordinate system e, OX by using the center of the earth as an origin e Axis and OY e Axis in the equatorial plane of the earth, OX e Axis pointing to the principal meridian, OZ e The axis being the earth's rotation axis, OY e Shaft and OX e Axis, OZ e The shaft forms a right-hand coordinate system, and an earth coordinate system e is fixedly connected with the earth;
establishing a missile coordinate system b, OX by using the mass center of the missile as the origin b Axis coincident with the longitudinal axis of the projectile body and pointing to the positive head, OY b The axis being in the plane of symmetry of the longitudinal axis of the projectile, perpendicular to OX b Axial, positive upward, OZ b Axis perpendicular to X b OY b Plane, the direction is determined according to the right-hand rule;
the missile control system uses a launching coordinate systemThe parameters are used as control parameters, and a transmitting coordinate system f, OX is established by taking the transmitting point as an origin f The axis is the line from the emitting point to the target point, pointing to the target direction, OY f Axis along origin O f A vertical line of (A) is directed upwards, OZ f Axis perpendicular to X f OY f Plane, direction determined according to the right-hand rule, OX f The angle between the axis and north is defined as the azimuth angle alpha, along OY f Viewed from the positive direction of the axis, the anticlockwise direction is positive, the clockwise direction is negative, and the real-time speed and the position required by the missile-borne control system are both the speed under the emission coordinate
Figure FDA0003818564200000022
And position (X) f ,Y f ,Z f )。
3. The method for miniature homing missile strapdown navigation in a launch coordinate system of claim 1, wherein the steps 200: according to the initial information of the miniature self-seeking missile, taking the northeast geographic coordinate system as a navigation coordinate system, and specifically establishing the navigation initial information comprises the following steps:
before missile launching, 2-second static data are collected, and the average values of the obtained accelerometers are respectively
Figure FDA0003818564200000031
Obtaining an initial pitch angle theta from static data 0 And roll angle γ 0 Respectively as follows:
Figure FDA0003818564200000032
Figure FDA0003818564200000033
initial yaw angle
Figure FDA0003818564200000034
Can pass through the bulletBinding by an upper control system to obtain an initial value, and knowing the initial direction cosine matrix according to three initial attitude angles
Figure FDA0003818564200000035
And the initial quaternion Q;
Figure FDA0003818564200000036
Figure FDA0003818564200000037
quaternion normalization, as follows:
Figure FDA0003818564200000038
4. the method for navigating the miniature homing missile strapdown according to the claim 1, wherein the step 300: according to the emission point O of the initial binding f Position and azimuth angle alpha, and obtaining a direction cosine matrix between the terrestrial coordinate system and the transmitting coordinate system
Figure FDA0003818564200000039
Step by step calculation of commanded angular rate
Figure FDA00038185642000000310
The method specifically comprises the following steps:
the emission point O of the initial binding f The positions include: longitude λ in geographic coordinate system 0 Latitude L 0 Height H 0 ；
Figure FDA0003818564200000041
Knowing the geographic coordinate system position (L, lambda, H) of a certain point, the position (X) of the earth coordinate system e is calculated e ,Y e ,Z e )：
Figure FDA0003818564200000042
In the formula, R N Is the curvature radius of the unitary point-mortise ring,
Figure FDA0003818564200000043
the earth ellipsoid model adopts a WGS-84 earth coordinate system and a long semi-axis R a =6378137m, semi-axis short R b =6356752.314m, global oblateness
Figure FDA0003818564200000044
Square of first eccentricity
Figure FDA0003818564200000045
Obtaining a position conversion formula from a geographic coordinate system to a transmitting coordinate system;
Figure FDA0003818564200000046
in the formula (X) of ，Y of ，Z of ) Is an emission point O f (ii) coordinates in the terrestrial coordinate system of (X) e ，Y e ，Z e )
Is the coordinate of the missile body in the earth coordinate system of the real-time position (X) f ，Y f ，Z f )
The coordinates of the real-time position of the projectile body in the launching coordinate system are obtained through conversion according to a formula (7).
5. The method as claimed in claim 1, wherein the method comprises the following stepsCharacterized in that, the step 400: according to the initial direction cosine matrix
Figure FDA0003818564200000047
Determining an attitude transformation matrix
Figure FDA0003818564200000048
And obtaining the projection of the angular rate from the geographic coordinate system to the inertial system on the carrier coordinate system
Figure FDA0003818564200000049
And the projection of the angular rate of the carrier coordinate system to the geographic coordinate system onto the carrier coordinate system
Figure FDA00038185642000000410
The method specifically comprises the following steps:
step by step calculation of commanded angular rate
Figure FDA0003818564200000051
And through the attitude transformation matrix
Figure FDA0003818564200000052
To obtain
Figure FDA0003818564200000053
And
Figure FDA0003818564200000054
angular rate of rotation of the earth
Figure FDA0003818564200000055
The angular rate of the terrestrial coordinate system relative to the geographic coordinate system is
Figure FDA0003818564200000056
Then the angular rate is commanded
Figure FDA0003818564200000057
Wherein the meridian plane radius of curvature R M ＝R a (1-e(2-3sinλ 2 ))；
By attitude transformation matrix
Figure FDA0003818564200000058
To obtain
Figure FDA0003818564200000059
And
Figure FDA00038185642000000510
Figure FDA00038185642000000511
Figure FDA00038185642000000512
in the formula, an attitude transformation matrix
Figure FDA00038185642000000513
Is that
Figure FDA00038185642000000514
By means of, i.e.
Figure FDA00038185642000000515
Figure FDA00038185642000000516
Is a projection of the angular velocity of the geographic coordinate system to the inertial system onto the carrier coordinate system,
Figure FDA00038185642000000517
is the projection of the angular velocity of the carrier coordinate system to the geographic coordinate system onto the carrier coordinate system.
6. The method for miniature homing missile strapdown navigation according to claim 1, wherein the steps 500: projecting on the carrier coordinate system according to the angular rate of the carrier coordinate system to the geographic coordinate system
Figure FDA00038185642000000518
Updating the quaternion Q specifically includes:
the quaternion differential equation is as follows:
Figure FDA00038185642000000519
the differential equation is calculated using the fourth-order Longkuta method, as follows
Figure FDA00038185642000000520
Figure FDA00038185642000000521
Figure FDA00038185642000000522
Figure FDA00038185642000000523
Figure FDA0003818564200000061
And further carrying out normalization processing on the quaternion obtained by calculation.
7. The method of claim 1A method for micro self-seeking missile strapdown navigation in a launching coordinate system, the method comprising the steps of 600: calculating the initial direction cosine matrix according to the quaternion Q
Figure FDA0003818564200000062
The method specifically comprises the following steps:
computing an attitude matrix from quaternions
Figure FDA0003818564200000063
Figure FDA0003818564200000064
8. The method for navigating the miniature homing missile strapdown according to the claim 1, wherein the step 700 is as follows: deducing a direct conversion relation between the transmitting coordinate system and the carrier coordinate system through 3 times of coordinate rotation according to a matrix conversion principle
Figure FDA0003818564200000065
The method specifically comprises the following steps:
Figure FDA0003818564200000066
in the formula (I), the compound is shown in the specification,
Figure FDA0003818564200000067
θ f 、γ f the attitude angle of a carrier coordinate system relative to a transmitting coordinate system is respectively a course angle, a pitch angle and a roll angle;
at the same time, the derivation is carried out,
Figure FDA0003818564200000068
according to
Figure FDA0003818564200000069
The three attitude angles of the carrier coordinate system relative to the emission coordinate system are calculated as follows:
yaw angle
Figure FDA00038185642000000610
Pitch angle
Figure FDA0003818564200000071
Figure FDA0003818564200000072
Roll angle gamma f ＝atan(F 32 /F 33 ) (17)
Figure FDA0003818564200000073
9. The method for navigating the miniature homing missile strapdown according to the claim 1, wherein the step 800 is as follows: the deriving speed update specifically includes:
Figure FDA0003818564200000074
in the formula (f) n Is the specific force converted from a carrier coordinate system to a geographic coordinate system,
Figure FDA0003818564200000075
g n is a representation of gravitational acceleration in a geographic coordinate system, g n ＝[0 0 g z ] T Wherein, in the step (A),
g z ＝9.78049×(1+0.005288(sinL) 2 )-3.0855e -6 ×H。
10. the method for navigating the miniature homing missile strapdown according to the claim 1, wherein the method further comprises the following steps:
calculating an initial attitude matrix and a quaternion according to the data of the missile at the power-on static moment, establishing an initial state of a strapdown navigation algorithm, and obtaining a plurality of attitude matrices;
measuring the projection of the angular rate of the missile relative to an inertia space in a carrier coordinate system in real time and the projection of the specific force relative to the inertia space in the carrier coordinate system;
converting the angular rate and the specific force into physical quantities under the geographic coordinate system through an attitude matrix;
in an angular rate integral loop, a quaternion is calculated by using the measured angular velocity and a fourth-order Longkuta method, and a coordinate conversion matrix from the carrier coordinate system to the geographic coordinate system is obtained
Figure FDA0003818564200000076
Thereby obtaining a coordinate transformation matrix from the carrier coordinate system to the transmission coordinate system
Figure FDA0003818564200000081
Thereby calculating to obtain three attitude angles;
in an acceleration integration loop, using
Figure FDA0003818564200000082
And converting the measured value of the sensor into the geographic coordinate system, compensating the gravity acceleration, and obtaining the speed and the position under the transmitting coordinate system through the coordinate conversion matrix.
CN202211034272.4A 2022-08-26 2022-08-26 Miniature self-seeking missile strapdown navigation method under emission coordinate system Active CN115560756B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211034272.4A CN115560756B (en) 2022-08-26 2022-08-26 Miniature self-seeking missile strapdown navigation method under emission coordinate system

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211034272.4A CN115560756B (en) 2022-08-26 2022-08-26 Miniature self-seeking missile strapdown navigation method under emission coordinate system

Publications (2)

Publication Number Publication Date
CN115560756A true CN115560756A (en) 2023-01-03
CN115560756B CN115560756B (en) 2023-07-04

Family

ID=84739867

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211034272.4A Active CN115560756B (en) 2022-08-26 2022-08-26 Miniature self-seeking missile strapdown navigation method under emission coordinate system

Country Status (1)

Country Link
CN (1) CN115560756B (en)

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105180937A (en) * 2015-10-15 2015-12-23 常熟理工学院 Initial alignment method for MEMS-IMU
RU168214U1 (en) * 2016-08-08 2017-01-24 Акционерное общество "Ульяновское конструкторское бюро приборостроения" (АО "УКБП") Strap-on integrated inertial heading vertical
CN107478223A (en) * 2016-06-08 2017-12-15 南京理工大学 A kind of human body attitude calculation method based on quaternary number and Kalman filtering
CN109141410A (en) * 2018-07-25 2019-01-04 深圳市集大自动化有限公司 The Multi-sensor Fusion localization method of AGV integrated navigation
CN111721291A (en) * 2020-07-17 2020-09-29 河北斐然科技有限公司 Engineering algorithm for strapdown inertial navigation under launching system
CN113587925A (en) * 2021-07-16 2021-11-02 湖南航天机电设备与特种材料研究所 Inertial navigation system and full-attitude navigation resolving method and device thereof

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105180937A (en) * 2015-10-15 2015-12-23 常熟理工学院 Initial alignment method for MEMS-IMU
CN107478223A (en) * 2016-06-08 2017-12-15 南京理工大学 A kind of human body attitude calculation method based on quaternary number and Kalman filtering
RU168214U1 (en) * 2016-08-08 2017-01-24 Акционерное общество "Ульяновское конструкторское бюро приборостроения" (АО "УКБП") Strap-on integrated inertial heading vertical
CN109141410A (en) * 2018-07-25 2019-01-04 深圳市集大自动化有限公司 The Multi-sensor Fusion localization method of AGV integrated navigation
CN111721291A (en) * 2020-07-17 2020-09-29 河北斐然科技有限公司 Engineering algorithm for strapdown inertial navigation under launching system
CN113587925A (en) * 2021-07-16 2021-11-02 湖南航天机电设备与特种材料研究所 Inertial navigation system and full-attitude navigation resolving method and device thereof

Also Published As

Publication number Publication date
CN115560756B (en) 2023-07-04

Similar Documents

Publication Publication Date Title
CN111721291B (en) Engineering algorithm for strapdown inertial navigation under launching system
CN105180728B (en) Front data based rapid air alignment method of rotary guided projectiles
CN108426575B (en) Strapdown inertial navigation polar region transverse navigation method improved by earth ellipsoid model
CN109059914B (en) Projectile roll angle estimation method based on GPS and least square filtering
CN106979781B (en) High-precision transfer alignment method based on distributed inertial network
CN107478110B (en) Rotating elastic attitude angle calculation method based on state observer
CN113050143B (en) Tightly-coupled navigation method under emission inertial coordinate system
CN110243362B (en) Medium-high altitude supersonic velocity target navigation method
CN113847913A (en) Missile-borne integrated navigation method based on ballistic model constraint
CN110398242B (en) Attitude angle determination method for high-rotation-height overload condition aircraft
CN110514200B (en) Inertial navigation system and high-rotation-speed rotating body attitude measurement method
CN114993305A (en) Guided projectile combination navigation method based on emission coordinate system
CN115560756A (en) Miniature self-seeking missile strapdown navigation method under launching coordinate system
CN114353784B (en) Guided projectile air attitude identification method based on motion vector
CN112182857B (en) Rocket-level debris falling point prediction method, rocket-level debris falling point prediction equipment and storage medium
CN114295145A (en) Design method for track generator of strapdown inertial navigation system based on vehicle-mounted launching platform
CN115060256B (en) Guided projectile air attitude identification method based on emission coordinate system
CN114234974A (en) Underwater vehicle navigation method based on emission coordinate system
CN113703019A (en) Fault processing method of navigation system, electronic equipment and storage medium
CN110906927A (en) Gravity acceleration simplified algorithm under solidification coordinate system
CN114383614B (en) Multi-vector air alignment method in ballistic environment
CN115359095B (en) Universal motion platform tracking and guiding calculation method
CN111765810B (en) Frame preset angle calculation method based on platform seeker gyroscope information
CN112747743B (en) Inertial vision integrated navigation method and device based on missile-borne infrared seeker
CN112179378B (en) Polarized light navigation-assisted transfer alignment system

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

Effective date of registration: 20230808

Address after: 230088 1056, North building, Hefei original animation Park Management Co., Ltd., No. 19, Tianzhi Road, high tech Zone, Hefei, Anhui Province

Patentee after: Kaifeng Navigation Control Technology Co.,Ltd.

Address before: 100000 Room 306, 3/F, Building 5, Yard 29, Kechuangqi Street, Beijing Economic and Technological Development Zone, Daxing District, Beijing

Patentee before: Beijing Kaikai Hangyu Navigation Control Technology Co.,Ltd.

TR01 Transfer of patent right