CN113587925B - Inertial navigation system and full-attitude navigation resolving method and device thereof - Google Patents
Inertial navigation system and full-attitude navigation resolving method and device thereof Download PDFInfo
- Publication number
- CN113587925B CN113587925B CN202110808144.XA CN202110808144A CN113587925B CN 113587925 B CN113587925 B CN 113587925B CN 202110808144 A CN202110808144 A CN 202110808144A CN 113587925 B CN113587925 B CN 113587925B
- Authority
- CN
- China
- Prior art keywords
- carrier
- conversion matrix
- coordinate system
- angle
- navigation
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 19
- 239000011159 matrix material Substances 0.000 claims abstract description 104
- 238000006243 chemical reaction Methods 0.000 claims abstract description 102
- 238000004364 calculation method Methods 0.000 claims abstract description 20
- 238000005096 rolling process Methods 0.000 claims abstract description 11
- 238000007796 conventional method Methods 0.000 claims abstract description 9
- 230000001133 acceleration Effects 0.000 claims description 23
- 230000005484 gravity Effects 0.000 claims description 11
- 230000008569 process Effects 0.000 claims description 6
- 230000009466 transformation Effects 0.000 claims description 5
- 230000004069 differentiation Effects 0.000 claims description 2
- 230000017525 heat dissipation Effects 0.000 claims 1
- 238000006073 displacement reaction Methods 0.000 abstract description 6
- 230000008859 change Effects 0.000 description 5
- 230000003068 static effect Effects 0.000 description 5
- 238000013459 approach Methods 0.000 description 4
- 230000008878 coupling Effects 0.000 description 2
- 238000010168 coupling process Methods 0.000 description 2
- 238000005859 coupling reaction Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005530 etching Methods 0.000 description 1
- 230000008676 import Effects 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
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/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 discloses an inertial navigation system and a full-gesture navigation resolving method and device thereof, wherein the inertial navigation system comprises initial alignment and calculation of an initial gesture conversion matrix; updating the speed, the position and the gesture of the carrier to obtain an updated gesture conversion matrix; when the absolute value of the 3 rd row and the 2 nd column elements in the updated posture conversion matrix is larger than a set threshold value, adopting full-posture navigation to calculate a pitch angle, a rolling angle and an azimuth angle, and when the absolute value of the 3 rd row and the 2 nd column elements in the updated posture conversion matrix is smaller than or equal to the set threshold value, adopting a conventional method to navigate and calculate the pitch angle, the rolling angle and the azimuth angle; the carrier attitude angle, the motion speed and the motion displacement under the full attitude are calculated through the triaxial accelerometer increment and the gyro increment, the azimuth angle when the absolute value of the pitch angle is more than 89.2 degrees can be tracked, and the problem that the azimuth angle attitude calculation is inaccurate when the absolute value of the pitch angle is more than 89.2 degrees is solved.
Description
Technical Field
The invention belongs to the technical field of inertial navigation, and particularly relates to an inertial navigation system and a full-attitude navigation resolving method and device thereof.
Background
In the inertial navigation attitude calculation process, when the pitch angle approaches +/-90 degrees (for example, the absolute value of the pitch angle is larger than 89.2 degrees), the calculation of the azimuth angle and the roll angle can generate coupling, so that the azimuth angle and the roll angle which are directly calculated are inconsistent with the actual situation. Although the speed and displacement navigation results are normal at this time, in some systems needing to monitor the navigation posture, the posture early warning system can be false-alarmed due to the fact that the calculated azimuth angle and the calculated roll angle are not in line with the reality.
Disclosure of Invention
The invention aims to provide an inertial navigation system and a full-attitude navigation resolving method and device thereof, which are used for solving the problem that the azimuth angle and the rolling angle which are directly resolved are inconsistent with the actual situation due to coupling when the pitch angle is close to +/-90 degrees.
The invention solves the technical problems by the following technical scheme: a full-attitude navigation resolving method of an inertial navigation system comprises the following steps:
step 1: carrying out initial alignment on the speed, the position and the gesture of a carrier carrying an inertial navigation system, and calculating an initial gesture conversion matrix;
step 2: updating the speed and the position of the carrier at the current moment k according to the initial posture conversion matrix in the step 1;
step 3: updating the posture of the carrier at the current moment k according to the initial posture conversion matrix in the step 1 and the updated speed and position in the step 2 to obtain an updated posture conversion matrix;
step 4: judging whether the absolute value of the 3 rd row and 2 nd column elements in the updated posture conversion matrix in the step 3 is larger than a set threshold value, if so, calculating the pitch angle, the roll angle and the azimuth angle of the carrier at the current moment k according to the following formula:
wherein θ k For pitch angle at current time k, γ k For the roll angle at the current instant k,azimuth angle ψ for current time k k Differential of->Posture conversion matrix for current time k>Element of row 3, column 2, of->Posture conversion matrix for current time k>Elements of row 1 and column 3 in ∈1,>posture conversion matrix for current time k>Elements of row 1, column 1, ">For the angular increment output of the Y-axis gyro at the current moment k,/->Outputting the angle increment of the Z-axis gyroscope at the current moment k;
otherwise, calculating the pitch angle, the rolling angle and the azimuth angle of the carrier at the current moment k according to a conventional method;
step 5: and (2) repeating the steps (2-4) to calculate the pitch angle, the rolling angle and the azimuth angle of the carrier at the next moment k+1 until the inertial navigation is finished.
Further, in the step 1, the calculation process of the initial posture conversion matrix is as follows:
calculating the local gravity acceleration g and the earth rotation angular velocity omega ie The projection on the navigation coordinate system n is specifically:
g n =[0 0 g] T
wherein g n For the projection of the local gravitational acceleration g on the navigation coordinate system n, L is the latitude of the current position of the carrier,is the rotation angular velocity omega of the earth ie Projection on a navigation coordinate system n;
calculating the local gravity acceleration g and the earth rotation angular velocity omega ie In the coordinates of the carrierProjection on line b;
based on the local gravitational acceleration g and the rotational angular velocity omega of the earth ie The coordinate conversion relation between the projection on the navigation coordinate system n and the projection on the carrier coordinate system b calculates an initial posture conversion matrix, and the expression of the initial posture conversion matrix is as follows:
wherein,,for initial pose transformation matrix g b For the projection of the local gravitational acceleration g on the carrier coordinate system b,is the rotation angular velocity omega of the earth ie Projection on the carrier coordinate system b.
Further, in the step 2, the updated velocity differential equation of the carrier is:
wherein R is the radius of the earth,and->Differentiation of east, north and sky speeds, V E(k) 、V N(k) And V U(k) The east speed, the north speed and the sky speed of the carrier at the current moment k are respectively L (k-1) And h (k-1) Respectively the latitude and the height f of the carrier at the last moment k-1 E(k) 、f N(k) And f U(k) The east-to-north and the heaven-to-earth forces, ω, of the carrier at the current moment k, respectively ie The rotation angular velocity of the earth, g is the local gravity acceleration; a, a x(k) 、a y(k) 、a z(k) The output of the triaxial accelerometer in X direction, Y direction and Z direction at the current moment k is respectively; />The gesture conversion matrix is the gesture conversion matrix at the current moment;
the updated position differential equation of the carrier is:
wherein,,for the longitude lambda of the carrier at the current moment k (k) Differential of->And->The latitude L of the carrier at the current moment k (k) And height h (k) Is a derivative of (a).
Further, in the step 3, the updated differential equation of the posture transformation matrix is:
wherein,,for the posture conversion matrix of the carrier coordinate system b relative to the navigation coordinate system n at the current moment k, +.>For the gesture conversion matrix->Differential of->For the projection vector of the angular velocity output by the current moment k carrier coordinate system b relative to the inertial coordinate system i on the carrier coordinate system b,/>The angular velocity output for the current moment k navigation coordinate system n relative to the inertial coordinate system i is derivedProjection vector on the aerial coordinate system n, +.>For projection vector +.>Is an antisymmetric matrix of>For the current moment k the earth rotation angular velocity omega ie Projection on navigation coordinate system n, +.>For the projection vector of the angular velocity output by the navigation coordinate system n relative to the geographic system e at the current moment k on the navigation coordinate system n, L (k-1) And h (k-1) The latitude and the height of the carrier at the last moment k-1 are respectively V E(k-1) 、V N(k-1) And V U(k-1) The east direction speed, the north direction speed and the heaven direction speed of the carrier at the last moment k-1 are respectively R M 、R N Is the radius of curvature of the earth.
Further, in the step 4, when the absolute value of the 3 rd row and the 2 nd column elements in the posture conversion matrix at the current time k is greater than the set threshold value and the absolute value of the pitch angle is greater than 89.2 °, the calculation formula of the azimuth angle at the current time k is:
wherein, psi is k Azimuth angle, ψ ', of current time k calculated for step 4' k An azimuth angle, a, of the current moment k when the absolute value of the pitch angle is larger than 89.2 DEG z(k) For the output of the Z-axis accelerometer at the current time k, a z(k-1) The output of the Z-axis accelerometer at the last time k-1.
Further, in the step 4, formulas for calculating the pitch angle, the roll angle and the azimuth angle of the carrier at the current moment k according to a conventional method are respectively as follows:
wherein,,posture conversion matrix for current time k>Element of row 3, column 1, respectively->Posture conversion matrix for current time k>Element of row 3, column 3, of->Posture conversion matrix for current time k>Elements of row 1, column 2, ">Posture conversion matrix for current time k>The elements of row 2 and column 2 in (a).
The invention also provides a full-attitude navigation solution device of the inertial navigation system, comprising:
the initial alignment unit is used for carrying out initial alignment on the speed, the position and the gesture of the carrier carrying the inertial navigation system and calculating an initial gesture conversion matrix;
a speed and position updating unit, configured to update the speed and position of the carrier at the current time k according to the initial posture conversion matrix;
the gesture updating unit is used for updating the gesture of the carrier at the current moment k according to the initial gesture conversion matrix, the updated speed and the updated position to obtain an updated gesture conversion matrix;
the resolving unit is used for judging whether the absolute value of the 3 rd row and the 2 nd column elements in the updated gesture conversion matrix is larger than a set threshold value, if so, resolving the pitch angle, the roll angle and the azimuth angle of the carrier at the current moment k according to the following formula:
wherein θ k For pitch angle at current time k, γ k For the roll angle at the current instant k,azimuth angle ψ for current time k k Differential of->Posture conversion matrix for current time k>Element of row 3, column 2, of->Posture conversion matrix for current time k>Elements of row 1 and column 3 in ∈1,>posture conversion matrix for current time k>Elements of row 1, column 1, ">For the angular increment output of the Y-axis gyro at the current moment k,/->Outputting the angle increment of the Z-axis gyroscope at the current moment k;
otherwise, the pitch angle, the roll angle and the azimuth angle of the carrier at the current moment k are calculated according to a conventional method.
The invention also provides an inertial navigation system, which comprises the full-attitude navigation solving device.
Advantageous effects
Compared with the prior art, the invention has the advantages that:
according to the inertial navigation system, the attitude angle, the movement speed and the movement displacement under the carrier inertial navigation algorithm under the full attitude are calculated through the triaxial accelerometer increment and the gyro increment, and the azimuth angle when the absolute value of the pitch angle is larger than 89.2 degrees can be tracked, so that the problem that in the traditional attitude calculation, the solution error is overlarge due to the fact that the denominator of a rolling angle calculation formula approaches 0 when the absolute value of the pitch angle approaches 90 degrees, and the problem that the attitude angle (or azimuth angle) calculation is inconsistent with the actual situation is solved; when the absolute value of the pitch angle is greater than 89.2 degrees, the pitch angle is calculated by means of the output of the actual gyro rate, a false alarm phenomenon cannot occur, and the problem that the azimuth attitude calculation is inaccurate when the absolute value of the pitch angle is greater than 89.2 degrees can be solved.
Drawings
In order to more clearly illustrate the technical solutions of the present invention, the drawings that are needed in the description of the embodiments will be briefly described below, it being obvious that the drawing in the description below is only one embodiment of the present invention, and that other drawings can be obtained according to these drawings without inventive effort for a person skilled in the art.
FIG. 1 is a flow chart of a full-pose navigation solution method in an embodiment of the invention;
FIG. 2 is a software interface diagram of a full-pose navigation solution in an embodiment of the invention.
Detailed Description
The following description of the embodiments of the present invention will be made more apparent and fully by reference to the accompanying drawings, in which it is shown, however, only some, but not all embodiments of the invention. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
The technical scheme of the present application is described in detail below with specific examples. The following embodiments may be combined with each other, and some embodiments may not be repeated for the same or similar concepts or processes.
As shown in fig. 1, the full-pose navigation resolving method of the inertial navigation system provided in the present embodiment includes the following steps:
step 1: and (3) carrying out initial alignment on the speed, the position and the posture of the carrier carrying the inertial navigation system, and calculating an initial posture conversion matrix.
Inputting three-axis accelerometer increment data and gyro increment data, wherein the data format requires a pure data text stored in a 6-column form, the first to three columns are respectively XYZ-axis acceleration increment data, and the fourth to six columns are respectively XYZ-axis gyro increment data; the local gravity acceleration g is input, the local longitude lambda (in degrees) is input, and the local latitude L (in degrees) is input; inputting a navigation start time, generally defaulting to 300s, the first 300s must be static data for initial alignment; the navigation solution result is output from 301 s.
The initial alignment is carried out by using static data of 300s before the carrier moves, and the coordinate conversion relation of the space vector alpha between the navigation coordinate system n and the carrier coordinate system b is as follows:
wherein alpha is n 、α b The space vector alpha is respectively represented by coordinates of a navigation coordinate system n and a carrier coordinate system b,is a gesture conversion matrix from the carrier coordinate system b to the navigation coordinate system n.
In the present embodiment, the local gravitational acceleration g and the earth rotation angular velocity ω are used ie The initial posture conversion matrix is calculated according to the coordinate conversion relation between the projection on the navigation coordinate system n and the projection on the carrier coordinate system b, and the specific calculation process is as follows:
1.1 calculating the local gravity acceleration g and the Earth rotation angular velocity omega ie The projection on the navigation coordinate system n is specifically:
g n =[0 0 g] T (2)
wherein g n For the projection of the local gravitational acceleration g on the navigation coordinate system n, L is the latitude of the current position of the carrier,is the rotation angular velocity omega of the earth ie Projection on the navigation coordinate system n.
1.2 calculating the local gravity acceleration g and the Earth rotation angular velocity omega ie Projection g on carrier coordinate system b b Andwherein g b For the mean value of the tri-axial accelerometer output in 300s static data,/o>Is the average value of the gyroscope output in the 300s static data.
Step 1.1 and step 1.2 may be performed simultaneously, without any sequence.
1.3 according to the local gravity acceleration g and the earth rotation angular velocity omega ie The coordinate conversion relation between the projection on the navigation coordinate system n and the projection on the carrier coordinate system b (i.e., expression (1)) calculates an initial posture conversion matrix whose expression is:
wherein,,for initial pose transformation matrix g b For the projection of the local gravitational acceleration g on the carrier coordinate system b,is the rotation angular velocity omega of the earth ie Projection on the carrier coordinate system b.
Step 2: and updating the speed and the position of the carrier at the current moment k according to the initial posture conversion matrix in the step 1.
After the initial gesture conversion matrix is obtained, the speed and the position at any moment can be updated. In this embodiment, the navigation coordinate system is the northeast coordinate system, and the updated velocity differential equation of the carrier at the current time k is:
wherein R is the radius of the earth,and->Respectively east velocity V E(k) North velocity V N(k) Sum of the upward velocity V U(k) Is the derivative of V E(k) 、V N(k) And V U(k) The east speed, the north speed and the sky speed of the carrier at the current moment k are respectively L (k-1) And h (k-1) Respectively the latitude and the height f of the carrier at the last moment k-1 E(k) 、f N(k) And f U(k) The east-to-north and the heaven-to-earth forces, ω, of the carrier at the current moment k, respectively ie The rotation angular velocity of the earth, g is the local gravitational acceleration.
The east-direction specific force, the north-direction specific force and the sky-direction specific force at the current moment k are obtained by an output left-hand gesture conversion matrix of the triaxial accelerometer, and specifically are as follows:
wherein a is x(k) 、a y(k) 、a z(k) The output of the triaxial accelerometer in X direction, Y direction and Z direction at the current moment k is respectively;the gesture conversion matrix is the gesture conversion matrix at the current moment.
Solving the velocity differential equation (5) to obtain the east velocity V of the carrier at the current moment k E(k) North velocity V N(k) Sum of the upward velocity V U(k) . The position update at the current instant k is based on a velocity update, in particular:
Wherein,,for the longitude lambda of the carrier at the current moment k (k) Differential of->And->The latitude L of the carrier at the current moment k (k) And height h (k) Is a derivative of (a). Solving the position differential equation (7) to obtain the longitude lambda of the carrier at the current moment k (k) Latitude L (k) And height h (k) 。
Step 3: and updating the posture of the carrier at the current moment k according to the initial posture conversion matrix in the step 1 and the updated speed and position in the step 2 to obtain an updated posture conversion matrix.
After the initial attitude conversion matrix and the updated speed and position are obtained, the attitude conversion matrix can be updated according to the output of the gyroscope measured by each frame, and the differential equation of the updated attitude conversion matrix is as follows:
wherein,,for the posture conversion matrix of the carrier coordinate system b relative to the navigation coordinate system n at the current moment k, +.>For the gesture conversion matrix->Differential of->For the projection vector of the angular velocity output by the current moment k carrier coordinate system b relative to the navigation coordinate system n on the carrier coordinate system b,/>X is projection vector +.>Is an anti-symmetric matrix of (a).
Since the gyroscope outputs the angular velocity of the carrier coordinate system b relative to the inertial coordinate system iAnd angular velocityCannot be obtained by direct measurement, and the differential equation of formula (8) needs to be transformed as follows:
wherein,,for the projection vector of the angular velocity output by the current moment k carrier coordinate system b relative to the inertial coordinate system i on the carrier coordinate system b,/>For the projection vector of the angular velocity output by the navigation coordinate system n relative to the inertial coordinate system i at the current moment k on the carrier coordinate system b,/>Navigating the coordinate system n relative to inertia for the current moment kProjection vector of angular velocity output by coordinate system i on navigation coordinate system n, +.>X is projection vector +.>Is an antisymmetric matrix of>For the gesture conversion matrix of the navigation coordinate system n relative to the carrier coordinate system b at the current moment k, it is known from its definition, +.>Is->Transposed matrix of (i.e.)>
Projection vectorI.e. the rotation of the navigation coordinate system n relative to the inertial coordinate system i, comprising two parts: navigation coordinate system n rotation caused by earth rotation>And the inertial navigation system moves the navigation coordinate system n rotation caused by the surface curvature of the earth near the surface of the earth +.>There is->
Wherein,,for the current moment k the earth rotation angular velocity omega ie Projection vector on navigation coordinate system n, < >>A projection vector of the angular velocity output by the navigation coordinate system n relative to the geographic system e at the current moment k on the navigation coordinate system n; v (V) E(k-1) 、V N(k-1) And V U(k-1) The east speed, the north speed and the sky speed of the carrier at the last moment k-1 are updated by the accelerometer output at the last moment k-1; r is R M 、R N Is the radius of curvature of the earth.
The attitude transformation matrix at the current moment k can be obtained by solving the differential equation of (9)
In this embodiment, navigation gesture calculation is performed based on a northeast navigation coordinate system, and a gesture conversion matrix is defined as follows:
according to the definition of the gesture conversion matrix, the specific value of each element in the gesture conversion matrix can be obtained by combining the solving result of the differential equation (9).
Step 4: judging whether the absolute value of the 3 rd row and 2 nd column elements in the updated posture conversion matrix in the step 3 is larger than a set threshold value, if so, calculating the pitch angle, the roll angle and the azimuth angle of the carrier at the current moment k according to the following formulas (12) and (13), wherein the specific formulas are as follows:
wherein θ k For pitch angle at current time k, γ k For the roll angle at the current instant k,azimuth angle ψ for current time k k Differential of->Posture conversion matrix for current time k>Element of row 3, column 2, of->Posture conversion matrix for current time k>Elements of row 1 and column 3 in ∈1,>posture conversion matrix for current time k>Elements of row 1, column 1, ">For the angular increment output of the Y-axis gyro at the current moment k,/->For Z-axis gyro at presentAnd outputting the angle increment of the etching k.
Otherwise, the pitch angle, the roll angle and the azimuth angle of the carrier at the current moment k are calculated according to a conventional method, and the specific formula is as follows:
wherein,,posture conversion matrix for current time k>Element of row 3, column 1, respectively->Posture conversion matrix for current time k>Element of row 3, column 3, of->Posture conversion matrix for current time k>Elements of row 1, column 2, ">Posture conversion matrix for current time k>The elements of row 2 and column 2 in (a).
That is, when the calculated pitch angle does not fall within the threshold range of ±90°, the pitch angle, roll angle and azimuth angle are calculated according to equation (14), and when the calculated pitch angle approaches the threshold range of ±90°, the pitch angle, roll angle and azimuth angle are calculated according to equations (12) and (13).
According to engineering experience, in the embodiment, the threshold value is set to be 0.9999, namelyIn this case, the pitch angle, roll angle and azimuth angle are calculated according to equations (12) and (13), and the azimuth angle can be calculated by solving the differential equation of equation (13).
In the process of gesture resolving, attention is required to be paid to the value range of resolving angles, theta k The value range is [ -90 degrees, 90 degrees), gamma k The value range of (a) is [ -180 degrees, 180 degrees), and (b) phi k Is [0 ], 360 °). When the carrier moves around the rolling axis or azimuth axis, the attitude resolution cannot track the azimuth angle of the carrier in real time. In this embodiment, under the initial attitude, the rotation of the carrier X axis corresponds to the change of the pitch angle, the rotation of the carrier Y axis corresponds to the change of the roll angle, and the rotation of the carrier Z axis corresponds to the change of the azimuth angle, and if the absolute value of the pitch angle is greater than 89.2 °, the change of the azimuth angle is affected by the change of angles in both the axial directions of the Y axis and the Z axis, and the specific calculation is as shown in formula (13).
It is noted that when the pitch angle is increased from 89.5 to 90 and then further increased by 0.5, the output of the pitch angle is still 89.5 due to the range limit of the pitch angle, but the azimuth angle is changed by 180. Therefore, in the azimuth angle solution when the absolute value of the pitch angle is greater than 89.2 °, a judgment formula (15) is added, namelyAnd when the absolute value of the pitch angle is larger than 89.2 degrees, the calculation formula of the azimuth angle at the current moment k is as follows:
wherein, psi is k Azimuth angle, ψ ', of current time k calculated for equation (13)' k Azimuth angle, a, of current time k calculated for equation (15) z(k) For the output of the Z-axis accelerometer at the current time k, a z(k-1) On Z-axis accelerometerAn output at time k-1.
Step 5: and (2) repeating the steps (2-4), and calculating the pitch angle, the rolling angle and the azimuth angle of the carrier at the next moment k+1 until the inertial navigation is finished or the data are output by the accelerometer and the gyroscope at the last frame.
According to the full-attitude navigation resolving method of the inertial navigation system, after the initial attitude conversion matrix of a moving body (carrier for short) carrying the inertial navigation system is obtained through initial alignment, the attitude conversion matrix at each moment is updated through the output of the triaxial accelerometer and the output of the gyroscope, so that the full-attitude navigation resolving problem can be solved pertinently, the practicability is high, the problem that the azimuth attitude resolving is inaccurate when the absolute value of the pitch angle is larger than 89.2 degrees is solved, and the problem that the attitude monitoring software is misreported is avoided.
The embodiment also provides a full-attitude navigation solution device of an inertial navigation system, including:
and the initial alignment unit is used for carrying out initial alignment on the speed, the position and the gesture of the carrier carrying the inertial navigation system, and calculating an initial gesture conversion matrix as shown in a formula (4).
Inputting three-axis accelerometer increment data and gyro increment data, wherein the data format requires a pure data text stored in a 6-column form, the first to three columns are respectively XYZ-axis acceleration increment data, and the fourth to six columns are respectively XYZ-axis gyro increment data; the local gravity acceleration g is input, the local longitude lambda (in degrees) is input, and the local latitude L (in degrees) is input; inputting a navigation start time, generally defaulting to 300s, the first 300s must be static data for initial alignment; the navigation solution result is output from 301 s. As shown in fig. 2, the tri-axial accelerometer incremental data and gyro incremental data required for navigation solution can be input by clicking "import navigation data".
And the speed and position updating unit is used for updating the speed and position of the carrier at the current moment k according to the initial posture conversion matrix, as shown in formulas (5) - (7).
And the gesture updating unit is used for updating the gesture of the carrier at the current moment k according to the initial gesture conversion matrix, the updated speed and the updated position to obtain an updated gesture conversion matrix, as shown in a formula (9).
The resolving unit is used for judging whether the absolute value of the 3 rd row and the 2 nd column elements in the updated gesture conversion matrix is larger than a set threshold value, if so, resolving the pitch angle, the roll angle and the azimuth angle of the carrier at the current moment k according to the following formula:
wherein θ k For pitch angle at current time k, γ k For the roll angle at the current instant k,azimuth angle ψ for current time k k Differential of->Posture conversion matrix for current time k>Element of row 3, column 2, of->Posture conversion matrix for current time k>Elements of row 1 and column 3 in ∈1,>posture conversion matrix for current time k>Elements of row 1, column 1, ">For the angular increment output of the Y-axis gyro at the current moment k,/->Outputting the angle increment of the Z-axis gyroscope at the current moment k;
otherwise, the pitch angle, the roll angle and the azimuth angle of the carrier at the current moment k are calculated according to a conventional method, as shown in a formula (14).
As shown in fig. 2, clicking the "navigation calculation" and waiting for popping up the "navigation calculation completed" window, a file named result. Txt can be found in the navigation data folder, that is, the navigation calculation result file is composed of nine columns of data, and the first column to the third column are respectively the navigation calculation pitch angle, the rolling angle and the azimuth angle; the fourth column to the sixth column are respectively an east speed error, a north speed error and an sky speed error which are calculated by navigation; the seventh to ninth columns are the east displacement error, the north displacement error and the sky displacement error of the navigation solution, respectively.
The foregoing disclosure is merely illustrative of specific embodiments of the present invention, but the scope of the present invention is not limited thereto, and any person skilled in the art will readily recognize that changes and modifications are possible within the scope of the present invention.
Claims (8)
1. The full-attitude navigation resolving method of the inertial navigation system is characterized by comprising the following steps of:
step 1: carrying out initial alignment on the speed, the position and the gesture of a carrier carrying an inertial navigation system, and calculating an initial gesture conversion matrix;
step 2: updating the speed and the position of the carrier at the current moment k according to the initial posture conversion matrix in the step 1;
step 3: updating the posture of the carrier at the current moment k according to the initial posture conversion matrix in the step 1 and the updated speed and position in the step 2 to obtain an updated posture conversion matrix;
step 4: judging whether the absolute value of the 3 rd row and 2 nd column elements in the updated posture conversion matrix in the step 3 is larger than a set threshold value, if so, calculating the pitch angle, the roll angle and the azimuth angle of the carrier at the current moment k according to the following formula:
wherein θ k For pitch angle at current time k, γ k For the roll angle at the current instant k,azimuth angle ψ for current time k k Differential of->Posture conversion matrix for current time k>Element of row 3, column 2, of->For the current time kIs>Elements of row 1 and column 3 in ∈1,>posture conversion matrix for current time k>Elements of row 1, column 1, ">For the angular increment output of the Y-axis gyro at the current moment k,/->Outputting the angle increment of the Z-axis gyroscope at the current moment k;
otherwise, calculating the pitch angle, the rolling angle and the azimuth angle of the carrier at the current moment k according to a conventional method;
step 5: and (2) repeating the steps (2-4) to calculate the pitch angle, the rolling angle and the azimuth angle of the carrier at the next moment k+1 until the inertial navigation is finished.
2. The full-pose navigation solution method according to claim 1, wherein in the step 1, the calculation process of the initial pose conversion matrix is:
calculating the local gravity acceleration g and the earth rotation angular velocity omega ie The projection on the navigation coordinate system n is specifically:
g n =[00g] T
wherein g n For the projection of the local gravitational acceleration g on the navigation coordinate system n, L is the latitude of the current position of the carrierThe degree of the heat dissipation,is the rotation angular velocity omega of the earth ie Projection on a navigation coordinate system n;
calculating the local gravity acceleration g and the earth rotation angular velocity omega ie Projection on the carrier coordinate system b;
based on the local gravitational acceleration g and the rotational angular velocity omega of the earth ie The coordinate conversion relation between the projection on the navigation coordinate system n and the projection on the carrier coordinate system b calculates an initial posture conversion matrix, and the expression of the initial posture conversion matrix is as follows:
3. The full-pose navigation solution method according to claim 1, wherein in the step 2, the updated velocity differential equation of the carrier is:
wherein R is the radius of the earth,and->Differentiation of east, north and sky speeds, V E(k) 、V N(k) And V U(k) The east speed, the north speed and the sky speed of the carrier at the current moment k are respectively L (k-1) And h (k-1) Respectively the latitude and the height f of the carrier at the last moment k-1 E(k) 、f N(k) And f U(k) The east-to-north and the heaven-to-earth forces, ω, of the carrier at the current moment k, respectively ie The rotation angular velocity of the earth, g is the local gravity acceleration; a, a x(k) 、a y(k) 、a z(k) The output of the triaxial accelerometer in X direction, Y direction and Z direction at the current moment k is respectively; />The gesture conversion matrix is the gesture conversion matrix at the current moment;
the updated position differential equation of the carrier is:
4. The full-pose navigation solution method according to claim 1, wherein in the step 3, the updated pose conversion matrix differential equation is:
wherein,,for the posture conversion matrix of the carrier coordinate system b relative to the navigation coordinate system n at the current moment k, +.>For the gesture conversion matrix->Differential of->For the projection vector of the angular velocity output by the current moment k carrier coordinate system b relative to the inertial coordinate system i on the carrier coordinate system b,/>For the projection vector of the angular velocity output by the navigation coordinate system n relative to the inertial coordinate system i at the current moment k on the navigation coordinate system n,/>For projection vector +.>Is an antisymmetric matrix of>For the current moment k the earth rotation angular velocity omega ie Projection on navigation coordinate system n, +.>For the projection vector of the angular velocity output by the navigation coordinate system n relative to the geographic system e at the current moment k on the navigation coordinate system n, L (k-1) And h (k-1) The latitude and the height of the carrier at the last moment k-1 are respectively V E(k-1) 、V N(k-1) And V U(k-1) The east direction speed, the north direction speed and the heaven direction speed of the carrier at the last moment k-1 are respectively R M 、R N Is the radius of curvature of the earth.
5. The full-pose navigation solution according to any one of claims 1 to 4, wherein in the step 4, when the absolute value of the 3 rd row and 2 nd column elements in the pose conversion matrix at the current time k is greater than a set threshold value and the absolute value of the pitch angle is greater than 89.2 °, the calculation formula of the azimuth angle at the current time k is:
wherein, psi is k Azimuth angle, ψ ', of current time k calculated for step 4' k An azimuth angle, a, of the current moment k when the absolute value of the pitch angle is larger than 89.2 DEG z(k) For the output of the Z-axis accelerometer at the current time k, a z(k-1) The output of the Z-axis accelerometer at the last time k-1.
6. The full-pose navigation solution according to any one of claims 1 to 4, wherein in the step 4, formulas for solving the pitch angle, the roll angle and the azimuth angle of the carrier at the current time k according to a conventional method are respectively:
wherein,,for the current time kIs>Element of row 3, column 1, respectively->Posture conversion matrix for current time k>Element of row 3, column 3, of->Posture conversion matrix for current time k>Elements of row 1, column 2, ">Posture conversion matrix for current time k>The elements of row 2 and column 2 in (a).
7. A full-pose navigation solution device of an inertial navigation system, comprising:
the initial alignment unit is used for carrying out initial alignment on the speed, the position and the gesture of the carrier carrying the inertial navigation system and calculating an initial gesture conversion matrix;
a speed and position updating unit, configured to update the speed and position of the carrier at the current time k according to the initial posture conversion matrix;
the gesture updating unit is used for updating the gesture of the carrier at the current moment k according to the initial gesture conversion matrix, the updated speed and the updated position to obtain an updated gesture conversion matrix;
the resolving unit is used for judging whether the absolute value of the 3 rd row and the 2 nd column elements in the updated gesture conversion matrix is larger than a set threshold value, if so, resolving the pitch angle, the roll angle and the azimuth angle of the carrier at the current moment k according to the following formula:
wherein θ k For pitch angle at current time k, γ k For the roll angle at the current instant k,azimuth angle ψ for current time k k Differential of->Posture conversion matrix for current time k>Element of row 3, column 2, of->Posture conversion matrix for current time k>Elements of row 1 and column 3 in ∈1,>posture conversion matrix for current time k>Elements of row 1, column 1, ">For the angular increment output of the Y-axis gyro at the current moment k,/->Outputting the angle increment of the Z-axis gyroscope at the current moment k;
otherwise, the pitch angle, the roll angle and the azimuth angle of the carrier at the current moment k are calculated according to a conventional method.
8. An inertial navigation system, characterized by: comprising the full-pose navigation solution according to claim 7.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110808144.XA CN113587925B (en) | 2021-07-16 | 2021-07-16 | Inertial navigation system and full-attitude navigation resolving method and device thereof |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110808144.XA CN113587925B (en) | 2021-07-16 | 2021-07-16 | Inertial navigation system and full-attitude navigation resolving method and device thereof |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113587925A CN113587925A (en) | 2021-11-02 |
CN113587925B true CN113587925B (en) | 2023-07-07 |
Family
ID=78247834
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110808144.XA Active CN113587925B (en) | 2021-07-16 | 2021-07-16 | Inertial navigation system and full-attitude navigation resolving method and device thereof |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113587925B (en) |
Families Citing this family (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114485641B (en) * | 2022-01-24 | 2024-03-26 | 武汉梦芯科技有限公司 | Attitude calculation method and device based on inertial navigation device navigation azimuth fusion |
CN115560756B (en) * | 2022-08-26 | 2023-07-04 | 北京开拓航宇导控科技有限公司 | Miniature self-seeking missile strapdown navigation method under emission coordinate system |
CN116048146B (en) * | 2023-03-31 | 2023-06-13 | 中国船舶集团有限公司第七〇七研究所 | Angular velocity smooth control method for inertial navigation of rotary fiber-optic gyroscope |
CN117091596B (en) * | 2023-10-09 | 2024-06-21 | 腾讯科技(深圳)有限公司 | Gesture information acquisition method and related equipment |
Family Cites Families (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104677358A (en) * | 2013-11-29 | 2015-06-03 | 哈尔滨恒誉名翔科技有限公司 | Micro-strapdown attitude heading reference system all-attitude controller |
CN105203098B (en) * | 2015-10-13 | 2018-10-02 | 上海华测导航技术股份有限公司 | Agricultural machinery all-attitude angle update method based on nine axis MEMS sensors |
CN106153073B (en) * | 2016-06-21 | 2018-10-12 | 东南大学 | A kind of nonlinear initial alignment method of full posture Strapdown Inertial Navigation System |
JP6922641B2 (en) * | 2017-10-13 | 2021-08-18 | 株式会社Jvcケンウッド | Angular velocity derivation device and angular velocity derivation method |
CN109115212B (en) * | 2018-10-30 | 2022-04-12 | 中国船舶重工集团公司第七0七研究所 | Method for extracting full-range attitude angle of inertial navigation system |
CN110132269A (en) * | 2019-06-10 | 2019-08-16 | 西北工业大学 | A kind of guided missile high-precision Vertical Launch initial attitude acquisition methods |
CN111780749B (en) * | 2020-05-26 | 2022-06-03 | 北京航天控制仪器研究所 | Attitude control method for full-attitude inertial navigation of orbital transfer maneuvering aircraft |
CN111721291B (en) * | 2020-07-17 | 2021-12-07 | 河北斐然科技有限公司 | Engineering algorithm for strapdown inertial navigation under launching system |
CN112378400A (en) * | 2020-10-30 | 2021-02-19 | 湖南航天机电设备与特种材料研究所 | Dual-antenna GNSS assisted strapdown inertial navigation integrated navigation method |
-
2021
- 2021-07-16 CN CN202110808144.XA patent/CN113587925B/en active Active
Also Published As
Publication number | Publication date |
---|---|
CN113587925A (en) | 2021-11-02 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN113587925B (en) | Inertial navigation system and full-attitude navigation resolving method and device thereof | |
KR102454882B1 (en) | Dead reckoning method and apparatus for vehicle, device and storage medium | |
CN110954102B (en) | Magnetometer-assisted inertial navigation system and method for robot positioning | |
CN105910606A (en) | Direction adjustment method based on angular velocity difference | |
CN106767767A (en) | A kind of micro-nano multimode star sensor system and its data fusion method | |
CN111189442B (en) | CEPF-based unmanned aerial vehicle multi-source navigation information state prediction method | |
CN110174121A (en) | A kind of aviation attitude system attitude algorithm method based on earth's magnetic field adaptive correction | |
CN114485641A (en) | Attitude calculation method and device based on inertial navigation and satellite navigation azimuth fusion | |
CN106370178A (en) | Mobile terminal equipment attitude measurement method and mobile terminal equipment attitude measurement apparatus | |
CN112880669A (en) | Spacecraft starlight refraction and uniaxial rotation modulation inertia combined navigation method | |
CN108871323B (en) | High-precision navigation method of low-cost inertial sensor in locomotive environment | |
CN108871319B (en) | Attitude calculation method based on earth gravity field and earth magnetic field sequential correction | |
CN115855048A (en) | Multi-sensor fusion pose estimation method | |
CN104121930A (en) | Compensation method for MEMS (Micro-electromechanical Systems) gyroscopic drifting errors based on accelerometer coupling | |
CN109029467A (en) | A kind of spacecraft high-precision angular movement measurement method based on rotator type gyro biorthogonal configuration | |
CN115655271B (en) | Large-range attitude angle extraction method under dynamic condition | |
CN111141283A (en) | Method for judging advancing direction through geomagnetic data | |
CN113447024B (en) | Inertial navigation attitude angle resolving method and system based on extended Krafft angle | |
CN113447025B (en) | Inertial navigation high-precision attitude angle resolving method and system based on Krafft angle | |
CN111649738B (en) | Method for calculating initial attitude of accelerometer under microgravity field | |
CN114526729A (en) | Course optimization method of MEMS inertial positioning system based on redundancy technology | |
WO2021012635A1 (en) | Gyroscope information-based inertial navigation method | |
CN113670297A (en) | Off-line positioning method based on MEMS and wheel type odometer | |
CN113051757B (en) | Strapdown inertial navigation generalized PSI angle error model construction method | |
CN118225086B (en) | Method and system for precisely integrating and calculating pose under inertial 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 |