CN111256688A - Pre-integration algorithm of inertial navigation system - Google Patents

Pre-integration algorithm of inertial navigation system Download PDF

Info

Publication number
CN111256688A
CN111256688A CN201911152293.4A CN201911152293A CN111256688A CN 111256688 A CN111256688 A CN 111256688A CN 201911152293 A CN201911152293 A CN 201911152293A CN 111256688 A CN111256688 A CN 111256688A
Authority
CN
China
Prior art keywords
time
integration
navigation system
calculating
inertial
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.)
Pending
Application number
CN201911152293.4A
Other languages
Chinese (zh)
Inventor
张一博
谢阳光
王国庆
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Xian Flight Automatic Control Research Institute of AVIC
Original Assignee
Xian Flight Automatic Control Research Institute of AVIC
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 Xian Flight Automatic Control Research Institute of AVIC filed Critical Xian Flight Automatic Control Research Institute of AVIC
Priority to CN201911152293.4A priority Critical patent/CN111256688A/en
Publication of CN111256688A publication Critical patent/CN111256688A/en
Pending legal-status Critical Current

Links

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

Abstract

The invention discloses a pre-integration algorithm of an inertial navigation system, which comprises the following steps: step 1: initializing definition; step 2: calculating a pre-integration result; and step 3: calculating an equivalent rotation vector; and 4, step 4: calculating the rotation relation of a machine system between two continuous moments; and 5: the inertial state is resolved, and the pre-integration algorithm of the inertial navigation system can reduce the inertial resolution frequency, reduce the operation time and improve the real-time performance of the algorithm on the premise of not losing inertial data in an underwater integrated navigation method.

Description

Pre-integration algorithm of inertial navigation system
Technical Field
The invention belongs to the technical field of inertial navigation, and particularly relates to a pre-integration algorithm of a strapdown inertial navigation system.
Background
In an underwater inertial navigation system, the output frequency of carrier inertial data is high and is about 200 Hz. However, the frequently used acoustic sensor data in underwater integrated navigation is updated at a low frequency, namely, the data is updated once in about 10 s. In the multi-sensor data fusion optimization process, a method for solving the maximum posterior probability of the system is often used for navigation positioning of the carrier, and if a conventional inertia factor is added into the data fusion process at the rate of inertia measurement, the data fusion calculation load is increased, and the real-time performance is deteriorated.
For the problem, the common mode is to acquire effective data once every several beats in the inertial data and to use the extracted data to resolve at a lower frequency. In an environment where long-time navigation is needed underwater, the requirement on navigation accuracy is often high, and therefore a calculation method capable of reducing the calculation complexity in a data fusion algorithm and making full use of inertia measurement data is needed.
Disclosure of Invention
The technical problems solved by the invention are as follows: a pre-integration algorithm of an inertial navigation system is provided, and in an underwater integrated navigation method, the inertia resolving frequency is reduced on the basis of not losing inertia data, so that the real-time performance of the algorithm is improved.
The technical scheme of the invention is as follows:
a pre-integration algorithm for an inertial navigation system, comprising the steps of:
step 1: initializing definition;
step 2: calculating a pre-integration result;
and step 3: calculating an equivalent rotation vector;
and 4, step 4: calculating the rotation relation of a machine system between two continuous moments;
and 5: the inertial state is resolved.
Initialization definition in step 1, specifically, initializing position information of a moving carrier
Figure RE-RE-GDA0002469504350000021
Speed information
Figure RE-RE-GDA0002469504350000022
And attitude information
Figure RE-RE-GDA0002469504350000023
Wherein:
Figure RE-RE-GDA0002469504350000024
in the formula of △ Px、△Py、△PzX, Y, Z, position change amount in three directions, △ Vx、△Vy、△VzIs X, Y, Z the amount of speed change in three directions,
Figure RE-RE-GDA0002469504350000025
is tiTime ti+1Amount of change in attitude at time, tiIs the initial moment of carrier motion and I is the identity matrix.
The pre-integration calculation result in the step 2 specifically comprises: when there is new measurement data at tj+1Calculating the variation of the updated position when the mobile terminal is added to the system at any time
Figure RE-RE-GDA0002469504350000026
Update rate variation
Figure RE-RE-GDA0002469504350000027
Updating attitude variance
Figure RE-RE-GDA0002469504350000028
Compute pre-integral △ xi→j+1Wherein t isj+1＝tj+△t，tjIs the motion carrier pre-integration calculation time, △ t is the time interval when new measurement data arrives.
The calculated updated position variation amount
Figure RE-RE-GDA0002469504350000031
The calculation formula is as follows:
Figure RE-RE-GDA0002469504350000032
calculating the change amount of the update speed
Figure RE-RE-GDA0002469504350000033
The calculation formula is as follows:
Figure RE-RE-GDA0002469504350000034
in the formula fjNewly adding inertia measurement;
calculating an updated attitude change amount
Figure RE-RE-GDA0002469504350000035
The calculation formula is as follows:
Figure RE-RE-GDA0002469504350000036
in the formula
Figure RE-RE-GDA0002469504350000037
Is tjTime tj+1Attitude information of the moment;
compute pre-integral △ xi→j+1The calculation formula is as follows:
Figure RE-RE-GDA0002469504350000038
the step 3 of calculating the equivalent rotation vector specifically includes: in a speed update period T ═ Tj+1-tjIn the shorter case, the equivalent rotation vector Φ is calculated as:
Figure RE-RE-GDA0002469504350000039
wherein the content of the first and second substances,
Figure RE-RE-GDA00024695043500000310
in the formula (I), the compound is shown in the specification,
Figure RE-RE-GDA00024695043500000311
is the angular velocity of the navigation system relative to the body,
Figure RE-RE-GDA00024695043500000312
is the angular velocity of the inertial system relative to the machine system,
Figure RE-RE-GDA00024695043500000313
is tiThe attitude matrix of the motion carrier at a moment,
Figure RE-RE-GDA00024695043500000314
is the angular velocity of the navigation system relative to the inertial system.
Step 4, calculating the rotation relationship of the machine system between two consecutive moments, wherein the calculation formula is as follows:
Figure RE-RE-GDA00024695043500000315
step 5 inertial state solution, including position of navigation system
Figure RE-RE-GDA00024695043500000316
Speed of rotation
Figure RE-RE-GDA00024695043500000317
And posture
Figure RE-RE-GDA00024695043500000318
The calculation formula is respectively as follows:
Figure RE-RE-GDA0002469504350000041
Figure RE-RE-GDA0002469504350000042
is tiTime tjThe attitude rotation matrix at a time of day,
Figure RE-RE-GDA0002469504350000043
is tiThe location of the time of day navigation system,
Figure RE-RE-GDA0002469504350000044
is tiThe attitude matrix of the time-machine system,
Figure RE-RE-GDA0002469504350000045
is tiThe speed of the moment in time is,
Figure RE-RE-GDA0002469504350000046
is tiThe acceleration of gravity at the location of the moment,
Figure RE-RE-GDA0002469504350000047
is tiThe angular velocity of the moving carrier at that moment.
Figure RE-RE-GDA0002469504350000048
Figure RE-RE-GDA0002469504350000049
And ending the pre-integration process, selecting the reference coordinate system as the body coordinate system at the moment when the next pre-integration is started, and initializing the following steps: order to
Figure RE-RE-GDA00024695043500000410
And entering the next pre-integration process.
The invention has the beneficial effects that: the pre-integration algorithm of the inertial navigation system can reduce the inertial resolving frequency, reduce the operation time and improve the real-time performance of the algorithm on the premise of not losing inertial data in an underwater integrated navigation method.
Detailed Description
The following further details the examples of the invention:
a pre-integration algorithm for an inertial navigation system, comprising the steps of:
step 1: initializing definition;
step 2: calculating a pre-integration result;
and step 3: calculating an equivalent rotation vector;
and 4, step 4: calculating the rotation relation of a machine system between two continuous moments;
and 5: the inertial state is resolved.
Initialization definition in step 1, specifically, initializing position information of a moving carrier
Figure RE-RE-GDA0002469504350000051
Speed information
Figure RE-RE-GDA0002469504350000052
And attitude information
Figure RE-RE-GDA0002469504350000053
Wherein:
Figure RE-RE-GDA0002469504350000054
in the formula of △ Px、△Py、△PzX, Y, Z, position change amount in three directions, △ Vx、△Vy、△VzIs X, Y, Z the amount of speed change in three directions,
Figure RE-RE-GDA0002469504350000055
is tiTime ti+1Amount of change in attitude at time, tiIs the initial moment of carrier motion and I is the identity matrix.
The pre-integration calculation result in the step 2 specifically comprises: when there is new measurement data at tj+1Calculating the variation of the updated position when the mobile terminal is added to the system at any time
Figure RE-RE-GDA0002469504350000056
Update rate variation
Figure RE-RE-GDA0002469504350000057
Updating attitude variance
Figure RE-RE-GDA0002469504350000058
Compute pre-integral △ xi→j+1Wherein t isj+1＝tj+△t，tjIs the moment of the pre-integration calculation of the moving carrier,△tis the time interval in which new metrology data arrives.
The calculated updated position variation amount
Figure RE-RE-GDA0002469504350000059
The calculation formula is as follows:
Figure RE-RE-GDA00024695043500000510
calculating the change amount of the update speed
Figure RE-RE-GDA00024695043500000511
The calculation formula is as follows:
Figure RE-RE-GDA00024695043500000512
in the formula fjNewly adding inertia measurement;
calculating an updated attitude change amount
Figure RE-RE-GDA00024695043500000513
The calculation formula is as follows:
Figure RE-RE-GDA00024695043500000514
in the formula
Figure RE-RE-GDA00024695043500000515
Is tjTime tj+1Attitude information of the moment;
compute pre-integral △ xi→j+1The calculation formula is as follows:
Figure RE-RE-GDA0002469504350000061
the step 3 of calculating the equivalent rotation vector specifically includes: in a speed update period T ═ Tj+1-tjIn the shorter case, the equivalent rotation vector Φ is calculated as:
Figure RE-RE-GDA0002469504350000062
wherein the content of the first and second substances,
Figure RE-RE-GDA0002469504350000063
in the formula (I), the compound is shown in the specification,
Figure RE-RE-GDA0002469504350000064
is the angular velocity of the navigation system relative to the body,
Figure RE-RE-GDA0002469504350000065
is the angular velocity of the inertial system relative to the machine system,
Figure RE-RE-GDA0002469504350000066
is tiThe attitude matrix of the motion carrier at a moment,
Figure RE-RE-GDA0002469504350000067
is the angular velocity of the navigation system relative to the inertial system.
Step 4, calculating the rotation relationship of the machine system between two consecutive moments, wherein the calculation formula is as follows:
Figure RE-RE-GDA0002469504350000068
step 5 inertial state solution, including position of navigation system
Figure RE-RE-GDA0002469504350000069
Speed of rotation
Figure RE-RE-GDA00024695043500000610
And posture
Figure RE-RE-GDA00024695043500000611
The calculation formula is respectively as follows:
Figure RE-RE-GDA00024695043500000612
Figure RE-RE-GDA00024695043500000613
is tiTime tjThe attitude rotation matrix at a time of day,
Figure RE-RE-GDA00024695043500000614
is tiThe location of the time of day navigation system,
Figure RE-RE-GDA00024695043500000615
is tiThe attitude matrix of the time-machine system,
Figure RE-RE-GDA00024695043500000616
is tiThe speed of the moment in time is,
Figure RE-RE-GDA00024695043500000617
is tiThe acceleration of gravity at the location of the moment,
Figure RE-RE-GDA00024695043500000618
is tiThe angular velocity of the moving carrier at that moment.
Figure RE-RE-GDA00024695043500000619
Figure RE-RE-GDA0002469504350000071
The pre-integration process is finished, and when the next pre-integration is started, the reference coordinate system is selected as the body coordinate system at the moment, and the following initialization is carried out: order to
Figure RE-RE-GDA0002469504350000072
And entering the next pre-integration process.
When the pre-integration step length is 50 steps, the error of the equivalent inertia calculation method is larger than that of the traditional binary sample algorithm, although the equivalent inertia calculation finally has larger error in simulation data of about 7 hours, the error of the equivalent algorithm and the traditional algorithm are kept in the same order of magnitude, and the equivalent algorithm is an effective approximation of the traditional algorithm. Meanwhile, the two methods are compared in simulation time, 253.780516s is used in the traditional calculation method, 185.32451s is used in the equivalent calculation method, and therefore the equivalent calculation method is better in calculation efficiency. In the practical application process, the relation between the calculation precision and the calculation time is balanced, and the calculation can be performed by adopting an equivalent inertia calculation method under the conditions that the precision requirement is high, the data volume is large, and the real-time performance of the calculation is seriously influenced.
In the three-dimensional simulation, the pre-integration algorithm times at several different step sizes are compared, as shown in the following table:
Figure RE-RE-GDA0002469504350000073
as can be seen from the table, in the pre-integration-based inertial solution algorithm, the operation time gradually shortens as the integration step increases. The longer the pre-integration time, the lower the accuracy of the inertial solution algorithm, but the better the real-time performance of the algorithm. Therefore, in practical application, on the premise of ensuring that the pre-integration time is shorter, the step length of the pre-integration is reasonably set to avoid overlarge resolving error.
The above-described embodiments are merely illustrative of one of the preferred embodiments of the present invention and do not limit the spirit and scope of the present invention. Various changes and modifications of the technical solution of the present invention should fall within the protection scope of the present invention without departing from the concept of the present invention, and the technical contents of the present invention are all recorded in the claims.

Claims (8)

1. A pre-integration algorithm for an inertial navigation system, comprising: the method comprises the following steps:
step 1: initializing definition;
step 2: calculating a pre-integration result;
and step 3: calculating an equivalent rotation vector;
and 4, step 4: calculating the rotation relation of a machine system between two continuous moments;
and 5: the inertial state is resolved.
2. The pre-integration algorithm of an inertial navigation system according to claim 1, wherein: initialization definition in step 1, specifically, initializing position information of a moving carrier
Figure FDA0002283061870000011
Speed information
Figure FDA0002283061870000012
And attitude information
Figure FDA0002283061870000013
Wherein:
Figure FDA0002283061870000014
in the formula,. DELTA.Px、ΔPy、ΔPzX, Y, Z is the position variation in three directions, Δ Vx、ΔVy、ΔVzIs X, Y, Z the amount of speed change in three directions,
Figure FDA0002283061870000015
is tiTime ti+1Amount of change in attitude at time, tiIs the initial moment of carrier motion and I is the identity matrix.
3. The pre-integration algorithm of an inertial navigation system according to claim 1, wherein: the pre-integration calculation result in the step 2 specifically comprises: when there is new measurement data at tj+1Calculating the variation of the updated position when the mobile terminal is added to the system at any time
Figure FDA0002283061870000016
Update rate variation
Figure FDA0002283061870000017
Updating attitude variance
Figure FDA0002283061870000018
Calculating a pre-integral Deltaxi→j+1Wherein t isj+1＝tj+Δt，tjIs the moment of the pre-integration calculation of the moving carrier, and Δ t is the time interval of the arrival of new measurement data.
4. The pre-integration algorithm for computing an inertial navigation system according to claim 3, wherein: the calculated updated position variation amount
Figure FDA0002283061870000019
The calculation formula is as follows:
Figure FDA00022830618700000110
calculating the change amount of the update speed
Figure FDA0002283061870000021
The calculation formula is as follows:
Figure FDA0002283061870000022
in the formula fjNewly adding inertia measurement;
calculating an updated attitude change amount
Figure FDA0002283061870000023
The calculation formula is as follows:
Figure FDA0002283061870000024
in the formula
Figure FDA0002283061870000025
Is tjTime tj+1Attitude information of the moment;
calculating a pre-integral Deltaxi→j+1The calculation formula is as follows:
Figure FDA0002283061870000026
5. the pre-integration algorithm of an inertial navigation system according to claim 1, wherein: the step 3 of calculating the equivalent rotation vector specifically includes: in a speed update period T ═ Tj+1-tjIn the shorter case, the equivalent rotation vector Φ is calculated as:
Figure FDA0002283061870000027
wherein the content of the first and second substances,
Figure FDA0002283061870000028
in the formula (I), the compound is shown in the specification,
Figure FDA0002283061870000029
is the angular velocity of the navigation system relative to the body,
Figure FDA00022830618700000210
is the angular velocity of the inertial system relative to the machine system,
Figure FDA00022830618700000211
is tiThe attitude matrix of the motion carrier at a moment,
Figure FDA00022830618700000212
is the angular velocity of the navigation system relative to the inertial system.
6. The pre-integration algorithm of an inertial navigation system according to claim 1, wherein: step 4, calculating the rotation relationship of the machine system between two consecutive moments, wherein the calculation formula is as follows:
Figure FDA00022830618700000213
7. the pre-integration algorithm of an inertial navigation system according to claim 1, wherein: step 5 inertial state solution, including position of navigation system
Figure FDA00022830618700000214
Speed of rotation
Figure FDA00022830618700000215
And posture
Figure FDA00022830618700000216
The calculation formula is respectively as follows:
Figure FDA00022830618700000217
Figure FDA00022830618700000218
is tiTime tjThe attitude rotation matrix at a time of day,
Figure FDA00022830618700000219
is tiThe location of the time of day navigation system,
Figure FDA00022830618700000220
is tiThe attitude matrix of the time-machine system,
Figure FDA00022830618700000221
is tiThe speed of the moment in time is,
Figure FDA00022830618700000222
is tiThe acceleration of gravity at the location of the moment,
Figure FDA00022830618700000223
is tiThe angular velocity of the moving carrier at the moment;
after the pre-integration process is finished, when the next pre-integration is started, selecting the reference coordinate system as the body coordinate system at the moment, and initializing the following steps: order to
Figure FDA0002283061870000031
And entering the next pre-integration process.
8. The pre-integration algorithm of an inertial navigation system according to claim 7, wherein: t is tiVelocity of time of day
Figure FDA0002283061870000032
The calculation formula is as follows:
Figure FDA0002283061870000033
tiattitude matrix of time machine system
Figure FDA0002283061870000034
The calculation formula is as follows:
Figure FDA0002283061870000035
CN201911152293.4A 2019-11-21 2019-11-21 Pre-integration algorithm of inertial navigation system Pending CN111256688A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911152293.4A CN111256688A (en) 2019-11-21 2019-11-21 Pre-integration algorithm of inertial navigation system

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911152293.4A CN111256688A (en) 2019-11-21 2019-11-21 Pre-integration algorithm of inertial navigation system

Publications (1)

Publication Number Publication Date
CN111256688A true CN111256688A (en) 2020-06-09

Family

ID=70950173

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911152293.4A Pending CN111256688A (en) 2019-11-21 2019-11-21 Pre-integration algorithm of inertial navigation system

Country Status (1)

Country Link
CN (1) CN111256688A (en)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112146653A (en) * 2020-08-03 2020-12-29 河北汉光重工有限责任公司 Method for improving integrated navigation resolving frequency
CN112284379A (en) * 2020-09-17 2021-01-29 江苏大学 Inertia pre-integration method of combined motion measurement system based on nonlinear integral compensation
CN112464432A (en) * 2020-10-27 2021-03-09 江苏大学 Inertial pre-integration optimization method based on double-speed numerical integration structure

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
郭凯祥等: "一种快速的基于预积分的SINS等价解算方法", 《舰船电子对抗》 *

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112146653A (en) * 2020-08-03 2020-12-29 河北汉光重工有限责任公司 Method for improving integrated navigation resolving frequency
CN112284379A (en) * 2020-09-17 2021-01-29 江苏大学 Inertia pre-integration method of combined motion measurement system based on nonlinear integral compensation
WO2022057350A1 (en) * 2020-09-17 2022-03-24 江苏大学 Inertial pre-integration method for combined motion measurement system based on nonlinear integral compensation
CN112284379B (en) * 2020-09-17 2023-09-22 江苏大学 Inertial pre-integration method of combined motion measurement system based on nonlinear integral compensation
CN112464432A (en) * 2020-10-27 2021-03-09 江苏大学 Inertial pre-integration optimization method based on double-speed numerical integration structure

Similar Documents

Publication Publication Date Title
CN111207774B (en) Method and system for laser-IMU external reference calibration
CN111256688A (en) Pre-integration algorithm of inertial navigation system
CN109029448B (en) Monocular vision inertial positioning's IMU aided tracking model
CN110851776B (en) Attitude calculation method for high-dynamic variable-speed carrier
JP2012173190A (en) Positioning system and positioning method
CN103900614A (en) Method for compensating gravity of nine-accelerometer gyro-free inertial navigation system
CN110926499B (en) SINS strapdown inertial navigation system shaking base self-alignment method based on Liqun optimal estimation
CN110345936A (en) The track data processing method and its processing system of telecontrol equipment
CN114926547A (en) Calibration method of camera and IMU, electronic device and system
CN114018254B (en) SLAM method for integrating laser radar and rotary inertial navigation
CN112747770B (en) Speed measurement-based initial alignment method in carrier maneuvering
CN110440756A (en) A kind of inertial navigation system Attitude estimation method
CN117119586A (en) Indoor positioning fusion algorithm based on UWB and IMU
CN106092140B (en) A kind of gyroscope zero bias estimation method
CN113008229A (en) Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor
CN112378401A (en) Motion acceleration estimation method of inertial navigation system
CN107990893B (en) Detection method for sudden change of detection environment in two-dimensional laser radar S L AM
TW201314497A (en) Inertial sensing input apparatus, system and method thereof
CN114252073B (en) Robot attitude data fusion method
CN109459769A (en) A kind of autonomic positioning method and system
CN112008731B (en) Compliance control method, device, terminal, system and readable storage medium for aerial work robot
CN112325881B (en) Inertial navigation system attitude calculation method
CN110779552B (en) Self-adaptive alignment method under earth fixed connection coordinate system
CN114323011B (en) Kalman filtering method suitable for relative pose measurement
CN115615437B (en) Factor graph integrated navigation method

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