CN111238535A - IMU error online calibration method based on factor graph - Google Patents

IMU error online calibration method based on factor graph Download PDF

Info

Publication number
CN111238535A
CN111238535A CN202010078321.9A CN202010078321A CN111238535A CN 111238535 A CN111238535 A CN 111238535A CN 202010078321 A CN202010078321 A CN 202010078321A CN 111238535 A CN111238535 A CN 111238535A
Authority
CN
China
Prior art keywords
error
gyroscope
time
carrier
moment
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
CN202010078321.9A
Other languages
Chinese (zh)
Other versions
CN111238535B (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.)
Nanjing University of Aeronautics and Astronautics
Original Assignee
Nanjing University of Aeronautics and Astronautics
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 Nanjing University of Aeronautics and Astronautics filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN202010078321.9A priority Critical patent/CN111238535B/en
Publication of CN111238535A publication Critical patent/CN111238535A/en
Application granted granted Critical
Publication of CN111238535B publication Critical patent/CN111238535B/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
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices

Landscapes

  • Engineering & Computer Science (AREA)
  • Manufacturing & Machinery (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Navigation (AREA)

Abstract

The invention discloses an IMU error online calibration method based on a factor graph, which relates to the field of integrated navigation. And obtaining the error coefficient of the IMU after the optimization solution. Most of traditional IMU error calibration methods are offline calibration, and IMU errors can change due to mechanical impact or other factors caused by carrier motion, so that the problem of reduction of navigation precision is caused. The invention can effectively solve the problem of reduced precision of the navigation system caused by the change of IMU errors, and can be used in various integrated navigation systems based on inertial navigation.

Description

IMU error online calibration method based on factor graph
Technical Field
The invention relates to the field of integrated navigation, in particular to an IMU error online calibration method based on a factor graph.
Background
The inertial navigation system has the advantages of strong autonomy, good continuity and high stability, so that the combined navigation system mainly based on the inertial navigation system is a research hotspot in the technical field of navigation. However, the inertial navigation system is a dead reckoning system in nature, and has a problem that errors accumulate over time. The error sources affecting the navigation accuracy are various, and the inertial device itself has errors such as: the influence of zero offset, scale factor error, installation error and the like on the navigation precision is very large, and accurate calibration is required.
In order to obtain various error coefficients of an IMU (Inertial measurement unit), there are two main methods at present:
the method comprises the following steps: the method comprises the steps of utilizing a high-precision double-shaft rotary table of a laboratory to carry out off-line calibration, standing the IMU on the high-precision double-shaft rotary table, applying different control inputs to the rotary table, simultaneously collecting IMU data for a period of time, and obtaining various errors of the IMU through a data processing means subsequently.
The method 2 comprises the following steps: the traditional inertial pre-integration method is used for online calibration, and only the zero offset error of the IMU can be obtained.
However, the method 1 has the disadvantages that errors of the IMU cause deviation between an offline calibration result and an error when the IMU is actually used due to random starting uncertainty and the error existing in calibration, and adverse effect is brought to real-time high-precision navigation; the method 2 has the defect that the IMU error can be only calibrated in comparison with a single IMU error consideration method, and the influence of the scale factor error and the installation error on the navigation precision is ignored, so that the high-precision navigation is also adversely affected. In summary, the IMU error calibration method in the prior art is weak in applicability and cannot complete online calibration of IMU scaling factor errors and installation errors, thereby affecting system accuracy.
Disclosure of Invention
The invention provides an IMU error online calibration method based on a factor graph, which can optimally solve carrier navigation information and IMU errors by combining inertial residual errors and residual errors provided by other navigation sensors, improves the precision of real-time high-precision navigation, and has low cost and strong applicability.
In order to achieve the purpose, the invention adopts the following technical scheme:
an IMU error online calibration method based on a factor graph comprises the following steps:
s1, collecting the measurement values of an accelerometer and a gyroscope in the Inertial sensor, and carrying out pre-integration propagation on an IMU (Inertial measurement unit) by using the measurement values.
And S2, calculating the pre-integration error transfer equation by using the measured value.
And S3, collecting and utilizing the measured values of other navigation sensors, namely a visual image sensor, a GPS receiver and a laser radar to solve the inertial pre-integration error, combining the error items provided by the other navigation sensors to optimally solve the carrier navigation information and the IMU error, and executing S1-S3 in a circulating manner.
Further, in the S1, the pre-integration propagation method includes:
s11, calculating error coefficient matrixes of the accelerometer and the gyroscope by using the measured values, wherein the error coefficient matrixes of the accelerometer and the gyroscope at the moment t are respectively as follows:
Figure BDA0002379242030000021
Figure BDA0002379242030000022
wherein M isa1、Ma2、Ma3Error coefficients, M, representing the X, Y, Z axes of the accelerometer, respectivelyω1、Mω2、Mω3Respectively representing the error coefficients of the X, Y, Z axes of the gyroscope,
Figure BDA0002379242030000023
showing the mounting error of the accelerometer in the X-axis and the Y-axis,
Figure BDA0002379242030000031
showing the mounting error of the accelerometer in the X-axis and Z-axis,
Figure BDA0002379242030000032
showing the installation error of the Y axis and the Z axis of the accelerometer;
Figure BDA0002379242030000033
showing the mounting error of the gyroscope X-axis and Y-axis,
Figure BDA0002379242030000034
showing the mounting error of the gyroscope X-axis and Z-axis,
Figure BDA0002379242030000035
showing the installation error of the Y axis and the Z axis of the gyroscope;
the elements at the diagonal positions of the error coefficient matrix are calculated according to the following matrix:
Figure BDA0002379242030000036
Figure BDA0002379242030000037
wherein Kax、Kay、KazScale factor errors representing three axes of the accelerometer, respectively; kωx、Kωy、KωzScale factor errors in three axes of the gyroscope are respectively represented;
s12, sampling time t for two adjacent IMUsiAnd ti+1Calculating ti+1The value of the moment inertia pre-integral is calculated by the following formula:
Figure BDA0002379242030000038
Figure BDA0002379242030000039
Figure BDA00023792420300000310
wherein, bkDenotes the initial tkA coordinate system of the body at a moment,
Figure BDA00023792420300000311
respectively representing the sampling time t of two adjacent inertia frames i and i +1iAnd ti+1The measured value of the accelerometer is measured,
Figure BDA00023792420300000312
respectively represent tiAnd ti+1The measurement value of the gyroscope at the moment, δ t represents the sampling period of the inertial sensor;
Figure BDA00023792420300000313
respectively represent ti+1The time, the position pre-integration value, the speed pre-integration value and the angle pre-integration value of the carrier,
Figure BDA00023792420300000314
respectively represent tiTime, position preconcentration value, velocity preconcentration value and angle preconcentration value of carrier, initial time
Figure BDA0002379242030000041
And
Figure BDA0002379242030000042
is 0, since gamma is a rotation expressed by a quaternion,
Figure BDA0002379242030000043
is a unit quaternion; r (y) denotes the conversion of quaternions into rotation matrices,
Figure BDA0002379242030000044
i.e. representing t in terms of quaternioniThe moment carrier angle pre-integration value is converted into a rotation matrix,
Figure BDA0002379242030000045
i.e. representing t in terms of quaternioni+1Converting the carrier angle pre-integration value into a rotation matrix at the moment;
Figure BDA0002379242030000046
respectively represent tiThe acceleration at that moment is offset from zero of the gyroscope,
Figure BDA0002379242030000047
respectively represent ti+1The zero offset of the moment acceleration and the gyroscope, and the derivative of the moment acceleration and the gyroscope is white noise;
Figure BDA0002379242030000048
are respectively shown at tiA matrix of the acceleration at the moment and the error coefficient of the gyroscope,
Figure BDA0002379242030000049
Figure BDA00023792420300000410
are respectively shown at ti+1And (3) a matrix of the acceleration at the moment and the error coefficient of the gyroscope.
Further, the S2 includes:
s21, calculating an error state transition matrix F of the systemiAnd noise state transition matrix Gi：
Figure BDA00023792420300000411
Figure BDA00023792420300000412
Wherein,
Figure BDA00023792420300000413
representing t by quaternioniThe moment carrier angle pre-integration value is converted into a rotation matrix,
Figure BDA0002379242030000051
representing t by quaternioni+1Converting the pre-integral value of the carrier angle at the moment into a rotation matrix, wherein I is an identity matrix, delta t represents the sampling period of the inertial sensor, and the error state transition matrix FiAnd noise state transition matrix GiPart (A) hasElement f of volume representationmn、gmnWherein, the values of m and n are positive integers as follows:
Figure BDA0002379242030000052
Figure BDA0002379242030000053
Figure BDA0002379242030000054
Figure BDA0002379242030000055
Figure BDA0002379242030000056
Figure BDA0002379242030000057
Figure BDA0002379242030000058
Figure BDA0002379242030000059
Figure BDA00023792420300000510
Figure BDA00023792420300000511
wherein,
Figure BDA00023792420300000512
are respectively shown at tiA matrix of the acceleration at the moment and the error coefficient of the gyroscope,
Figure BDA00023792420300000513
respectively represent tiZero offset of moment acceleration and gyroscope, aiAnd ai+1Respectively represent tiTime and ti+1Time of day the output value of the accelerometer, ωiAnd omegai+1Respectively represent tiTime and ti+1The output value of the gyroscope at the moment.
S22, calculating two adjacent IMU sampling time tiAnd ti+1Error transfer equation, Jacobian matrix and covariance of the system, ti+1The error transfer equation, the Jacobian matrix and the covariance at the moment are respectively:
Ai+1＝FiAi+GiN
Figure BDA0002379242030000061
Pi+1＝FiPiFi T+GiQGi T
wherein t isiThe error transfer equation at a time is Ai，tiThe Jacobian of the time is Ji bk，tiCovariance of time of day is Pi；
Figure BDA0002379242030000062
Figure BDA0002379242030000063
Wherein, N is the noise,
Figure BDA0002379242030000064
represents tiThe position of the carrier at the moment pre-integrates the error state,
Figure BDA0002379242030000065
represents tiTime of dayThe angular pre-integration error state of the carrier,
Figure BDA0002379242030000066
represents tiThe speed of the carrier at the moment pre-integrates the error state,
Figure BDA0002379242030000067
respectively represent tiZero offset error states of the accelerometer and gyroscope of the time carrier,
Figure BDA0002379242030000068
respectively represent tiError state of error coefficient matrix of the carrier at moment;
Figure BDA0002379242030000069
represents tiWhite noise from the gyroscope and accelerometer at the moment,
Figure BDA00023792420300000610
represents ti+1White noise from the gyroscope and accelerometer at the moment,
Figure BDA00023792420300000611
it is indicated that the accelerometer has zero-bias white noise,
Figure BDA00023792420300000612
representing white zero bias noise of the gyroscope;
Figure BDA00023792420300000613
a Jacobian matrix of system states at an initial time, and
Figure BDA00023792420300000614
q is a covariance matrix of noise N, with an initial time
Figure BDA00023792420300000615
Further, in S3, the solving method of the carrier navigation information and the IMU error is as follows:
collecting the sameThe measured value S (k) of his navigation sensor, and the current tk+1The time system is marked as bk+1At tkTime tk+1At the time, the carrier collects a total of I IMU data, and the position preconcentration value, the speed preconcentration value and the angle preconcentration value of the carrier after the error is compensated
Figure BDA00023792420300000616
The calculation formula of (a) is as follows:
Figure BDA00023792420300000617
Figure BDA00023792420300000618
Figure BDA0002379242030000071
wherein,
Figure BDA0002379242030000072
respectively represent tkTime tk+1A carrier position pre-integration value, a velocity pre-integration value, an angle pre-integration value, which are not subjected to error compensation at a time,
Figure BDA0002379242030000073
respectively represent tkError states of error coefficient matrixes of an accelerometer and a gyroscope of a time carrier,
Figure BDA0002379242030000074
respectively represent tkZero offset error states of an accelerometer and a gyroscope of a time carrier;
wherein, the calculation formula of each Jacobian matrix is as follows:
Figure BDA0002379242030000075
Figure BDA0002379242030000076
Figure BDA0002379242030000077
wherein,
Figure BDA0002379242030000078
respectively represent tkTime tk+1At the moment, the error states of the position pre-integral quantity, the speed pre-integral quantity and the angle pre-integral quantity of the carrier are obtained;
tk+1time error state transfer equation alIs composed of
Figure BDA0002379242030000079
Wherein, the value range of i is 1 to l, l is the number of IMU acquired data, N is noise, FiFor systematic error state transition matrix, GiAs a noisy state transition matrix, Gl-1Is tkTo tk+1A noise state transition matrix at the sampling time of the first-1 IMU data at the time;
Figure BDA00023792420300000710
therefore, the inertia pre-integral error calculation formula is as follows:
Figure BDA0002379242030000081
wherein, the attitude of the carrier is expressed by quaternion,
Figure BDA0002379242030000082
representing the imaginary part of the extracted quaternion,
Figure BDA0002379242030000083
the angular pre-integration value of the carrier after the error is compensated,
Figure BDA0002379242030000084
is tkA rotation matrix from the time world coordinate system to the body system,
Figure BDA0002379242030000085
respectively represent tkThe position, the speed and the attitude of the time carrier under a world coordinate system,
Figure BDA0002379242030000086
respectively represent tk+1The position, the speed and the attitude of the time carrier under a world coordinate system,
Figure BDA0002379242030000087
respectively represent tkTime and tk+1The zero offset value of the accelerometer at the moment,
Figure BDA0002379242030000088
respectively represent tkTime and tk+1The zero-bias value of the gyroscope at that time,
Figure BDA0002379242030000089
respectively represent tkTime and tk+1The matrix of error coefficients of the accelerometer at the moment,
Figure BDA00023792420300000810
respectively represent tkTime and tk+1An error coefficient matrix of the moment gyroscope;
the calculation formula of the increment equation of the inertia pre-integration error is as follows:
Figure BDA00023792420300000811
wherein,
Figure BDA00023792420300000812
represents tkTime tk+1Covariance matrix of system between time, and delta X represents error state quantity in optimization processIncluding error amounts deltax of relative translation and rotation of other navigation sensors and IMUsError state quantity deltax provided by said other navigation sensorsλError state quantities of carrier position, velocity, quaternion, accelerometer and gyroscope zero bias for n +1 measurement frames in the sliding window, and error state quantities of the accelerometer and gyroscope error coefficient matrices containing scale factor errors and mounting errors:
δX＝[δx0,δx1,…,δxn,δxs,δxλ]
Figure BDA0002379242030000091
Figure BDA0002379242030000092
Figure BDA0002379242030000093
representing inertial pre-integration error
Figure BDA0002379242030000094
And (3) relative to a Jacobian matrix of the state variable X to be optimized, the state variable X to be optimized is as follows:
X＝[x0,x1,…,xn,xs,xλ]
wherein x0,x1,…,xnRepresenting the carrier-related state variable to be optimized, x, in n +1 measurement frames within the sliding windowsRepresenting the state variable to be optimized, x, constructed by other navigation sensors and IMUλRepresenting state variables to be optimized provided by other navigation sensors, in particular for x0,x1,…,xnA certain state quantity x ofkAnd xsComprises the following steps:
Figure BDA0002379242030000095
Figure BDA0002379242030000096
xkeach element in the matrix respectively represents the carrier position, speed, quaternion, zero offset of an accelerometer and a gyroscope of a specific measurement frame in the sliding window, and an error coefficient matrix of the accelerometer and the gyroscope of a scale factor error and an installation error, and the value of k is 0, 1,2 … n;
Figure BDA0002379242030000097
indicating the relative translation of the other navigation sensors and the IMU,
Figure BDA0002379242030000098
indicating the amount of relative rotation of the other navigation sensors with the IMU;
inertial pre-integration error
Figure BDA0002379242030000099
Jacobian matrix of state variables X to be optimized
Figure BDA00023792420300000910
The calculating method comprises the following steps:
error pre-integration by inertia
Figure BDA00023792420300000911
The expression of (2) shows that the inertia pre-integral error treats the optimized state variable xsAnd xλThe jacobian matrix of (a) is 0, i.e.:
Figure BDA00023792420300000912
the remaining variables to be optimized, namely the position, speed and attitude information and IMU error information of the carrier
Figure BDA00023792420300000913
Figure BDA0002379242030000101
The method is divided into four groups:
Figure BDA0002379242030000102
Figure BDA0002379242030000103
marking the Jacobian matrixes of the inertial pre-integral error relative to the variables to be optimized as J0, J1, J2 and J3;
the calculation modes of J0, J1, J2 and J3 are as follows:
Figure BDA0002379242030000104
Figure BDA0002379242030000111
j2, J3 are calculated as follows:
Figure BDA0002379242030000112
Figure BDA0002379242030000121
calculating each element in the matrix separately to obtain the numerical expression form of J0, J1, J2, J3:
Figure BDA0002379242030000122
Figure BDA0002379242030000131
Figure BDA0002379242030000132
Figure BDA0002379242030000133
wherein I represents an identity matrix
Figure BDA0002379242030000134
For quaternions q ═ x y z λ]＝[ω λ]L, R are characterized by the meanings left and right [. cndot]LAnd [. C]RThe left and right operators, respectively, representing the quaternion q may be expressed in particular as:
Figure BDA0002379242030000135
Figure BDA0002379242030000141
and combining the error terms provided by the other navigation sensors to construct an objective function:
Figure BDA0002379242030000142
wherein, | | rp||2For the prior constraint of marginalization, only a small amount of measurement and states are reserved in the optimization, and other measurement and states are marginalized and converted into prior;
Figure BDA0002379242030000143
for inertial pre-integration error, B is the set of all IMU measurements,
Figure BDA0002379242030000144
is the covariance of the inertial pre-integration error;
Figure BDA0002379242030000145
error terms provided for other navigation sensors;
and (3) using a Gaussian Newton nonlinear optimization method, stopping optimization when an error convergence state is reached or the iteration number reaches a threshold value, and outputting the carrier navigation information and the IMU error.
The invention has the beneficial effects that:
the method can solve the problem that the navigation precision is reduced because the IMU error is changed due to mechanical impact or other factors caused by carrier motion.
Compared with the traditional IMU offline error calibration method, the method has the following two main advantages: 1. the off-line calibration generally aims to verify whether each error of the IMU meets the specification index, only can provide a certain reference for a navigation positioning algorithm of a carrier, and cannot solve the problem of reduction of navigation precision caused by the change of the IMU error. The invention takes IMU error as the state quantity to be estimated, and can estimate IMU error in real time and compensate the navigation algorithm, thereby solving the problem and improving the navigation precision. 2. The off-line calibration process has longer time period, larger economic investment and strict environmental requirements, and the invention has low cost and strong environmental applicability.
Compared with an online calibration system represented by a Kalman filter, the method has the following two main advantages: 1. the Kalman filter only uses the current measurement information to carry out resolving, and the obtained result is only the optimal solution at the current moment; the factor nodes established in the factor graph optimization method adopted by the invention comprise all information in the whole time period of the system or a certain sliding window time period, and the global optimal solution in the whole time period is obtained, so that the estimation result of the IMU error is more accurate. 2. Aiming at the nonlinear part of the carrier in the actual motion process, the Kalman filtering algorithm only carries out linear processing on the nonlinear part once, the linear error is large, and the accumulation of the error is generated along with the calculation process; however, the gauss-newton iteration method adopted in the factor graph optimization of the invention can carry out linearization for many times when estimating the navigation solution, and can effectively reduce the linearization error, so that the invention has more accurate estimation result of IMU error.
Compared with an online calibration system using a traditional pre-integration method, the method has the main advantages that: the traditional online calibration system using the traditional pre-integration method has the advantages that the error of an IMU device is considered only singly, the IMU modeling is simple, only the IMU error of zero offset is considered, and the IMU error can be partially compensated; the invention introduces the scale factor error and the installation error of the IMU into the inertial pre-integration process and the factor node construction process, so the invention has more comprehensive compensation on the IMU error and more obvious improvement effect on the navigation precision.
In conclusion, the invention further improves the precision of real-time high-precision navigation, and has low cost and strong applicability.
Drawings
In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the drawings needed to be used in the embodiments will be briefly described 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 that other drawings can be obtained according to the drawings without creative efforts.
FIG. 1 is a flow chart of a method of an embodiment.
Detailed Description
In order that those skilled in the art will better understand the technical solutions of the present invention, the present invention will be further described in detail with reference to the following detailed description.
The embodiment of the invention provides an IMU (Inertial measurement unit) error online calibration method based on a factor graph, wherein the flow is shown in a figure 1 and comprises the following steps:
the method comprises the following steps: collecting accelerometer and gyroscope measurements in inertial sensors
Figure BDA0002379242030000161
Figure BDA0002379242030000162
Step two: accelerometer measurements
Figure BDA0002379242030000163
And gyroscope measurements
Figure BDA0002379242030000164
And performing pre-integration propagation on the IMU.
1) The error coefficient matrix of the accelerometer and the gyroscope is calculated as follows:
Figure BDA0002379242030000165
Figure BDA0002379242030000166
wherein M isa1、Ma2、Ma3Error coefficients, M, representing the X, Y, Z axes of the accelerometer, respectivelyω1、Mω2、Mω3Respectively representing the error coefficients of the X, Y, Z axes of the gyroscope,
Figure BDA0002379242030000167
showing the mounting error of the accelerometer in the X-axis and the Y-axis,
Figure BDA0002379242030000168
showing the mounting error of the accelerometer in the X-axis and Z-axis,
Figure BDA0002379242030000169
showing the installation error of the Y axis and the Z axis of the accelerometer;
Figure BDA00023792420300001610
showing the mounting error of the gyroscope X-axis and Y-axis,
Figure BDA00023792420300001611
showing the mounting error of the gyroscope X-axis and Z-axis,
Figure BDA00023792420300001612
showing the mounting error of the gyroscope Y axis and Z axis.
The elements at the diagonal positions of the two matrices are calculated as follows:
Figure BDA0002379242030000171
Figure BDA0002379242030000172
wherein Kax、Kay、KazScale factor errors representing three axes of the accelerometer, respectively; kωx、Kωy、KωzRespectively representing scale factor errors in three axes of the gyroscope.
2) Sampling time t for two adjacent IMUsiAnd ti+1Calculating t as followsi+1Value of the moment inertia pre-integral:
Figure BDA0002379242030000173
Figure BDA0002379242030000174
Figure BDA0002379242030000175
wherein, bkDenotes the initial tkA coordinate system of the body at a moment,
Figure BDA0002379242030000176
respectively representing the sampling time t of two adjacent inertia frames i and i +1iAnd ti+1The time-of-flight data of the accelerometer,
Figure BDA0002379242030000177
respectively represent tiAnd ti+1Data of the gyroscope at the moment, wherein deltat represents the sampling period of the inertial sensor;
Figure BDA0002379242030000178
Figure BDA0002379242030000179
respectively represent ti+1Time of day, loadA position pre-integration value, a velocity pre-integration value and an angle pre-integration value of the body,
Figure BDA00023792420300001710
respectively represent tiThe time, the position pre-integration value, the speed pre-integration value and the angle pre-integration value of the carrier,
Figure BDA00023792420300001711
is a unit quaternion; gamma is the rotation expressed in quaternions, R (gamma) expression converts quaternions into a rotation matrix,
Figure BDA00023792420300001712
representing t by quaternioniThe moment carrier angle pre-integration value is converted into a rotation matrix,
Figure BDA00023792420300001713
representing t by quaternioni+1Converting the carrier angle pre-integration value into a rotation matrix at the moment; at the initial moment in time of the day,
Figure BDA00023792420300001714
and
Figure BDA00023792420300001715
is a non-volatile organic compound (I) with a value of 0,
Figure BDA00023792420300001716
respectively represent tiThe acceleration at that moment is offset from zero of the gyroscope,
Figure BDA00023792420300001717
respectively represent ti+1The zero offset of the moment acceleration and the gyroscope, and the derivative of the moment acceleration and the gyroscope is white noise;
Figure BDA0002379242030000181
are respectively shown at tiA matrix of the acceleration at the moment and the error coefficient of the gyroscope,
Figure BDA0002379242030000182
are respectively shown at ti+1And (3) a matrix of the acceleration at the moment and the error coefficient of the gyroscope. (ii) a
Figure BDA0002379242030000183
And the interval between two adjacent IMU frames is kept unchanged.
Step three: utilizing inertial sensor data
Figure BDA0002379242030000184
And
Figure BDA0002379242030000185
an inertial pre-integration error transfer equation is calculated.
1) Error state transition matrix F of the systemiAnd noise state transition matrix GiThe calculation is performed as follows:
Figure BDA0002379242030000186
Figure BDA0002379242030000187
wherein,
Figure BDA0002379242030000188
representing t by quaternioniThe moment carrier angle pre-integration value is converted into a rotation matrix,
Figure BDA0002379242030000189
representing t by quaternioni+1Converting the pre-integral value of the carrier angle at the moment into a rotation matrix, wherein I is an identity matrix, delta t represents the sampling period of the inertial sensor, and the error state transition matrix FiAnd noise state transition matrix GiElement f in (1)mn、gmnWherein, the values of m and n are positive integers as follows:
Figure BDA0002379242030000191
Figure BDA0002379242030000192
Figure BDA0002379242030000193
Figure BDA0002379242030000194
Figure BDA0002379242030000195
Figure BDA0002379242030000196
Figure BDA0002379242030000197
Figure BDA0002379242030000198
Figure BDA0002379242030000199
Figure BDA00023792420300001910
wherein,
Figure BDA00023792420300001911
are respectively shown at tiA matrix of the acceleration at the moment and the error coefficient of the gyroscope,
Figure BDA00023792420300001912
respectively represent tiZero offset of moment acceleration and gyroscope, aiAnd ai+1Respectively represent tiTime and ti+1Time of day the output value of the accelerometer, ωiAnd omegai+1Respectively represent tiTime and ti+1The output value of the gyroscope at the moment.
2) Sampling time t for two adjacent IMUsiAnd ti+1The error transfer equation, the jacobian matrix and the covariance of the system are calculated as follows:
Ai+1＝FiAi+GiN
Figure BDA00023792420300001913
Pi+1＝FiPiFi T+GiQGi T
wherein t isiThe error transfer equation at a time is Ai，tiThe Jacobian of the time is Ji bk，tiCovariance of time of day is Pi；
Figure BDA00023792420300001914
Figure BDA0002379242030000201
Wherein, N is the noise,
Figure BDA0002379242030000202
represents tiThe position of the carrier at the moment pre-integrates the error state,
Figure BDA0002379242030000203
represents tiThe speed of the carrier at the moment pre-integrates the error state,
Figure BDA0002379242030000204
represents tiThe angular pre-integration error state of the time carrier,
Figure BDA0002379242030000205
Respectively represent tiZero offset error states of the accelerometer and gyroscope of the time carrier,
Figure BDA0002379242030000206
respectively represent tiError state of error coefficient matrix of the carrier at moment;
Figure BDA0002379242030000207
represents tiWhite noise from the gyroscope and accelerometer at the moment,
Figure BDA0002379242030000208
represents ti+1White noise from the gyroscope and accelerometer at the moment,
Figure BDA0002379242030000209
it is indicated that the accelerometer has zero-bias white noise,
Figure BDA00023792420300002010
representing white zero bias noise of the gyroscope;
Figure BDA00023792420300002011
a Jacobian matrix of system states at an initial time, and
Figure BDA00023792420300002012
q is a covariance matrix of noise N, with an initial time
Step four: when the measurement data S (k) of the visual image sensor, the GPS receiver and the laser radar are acquired, solving the inertial pre-integration error and optimally solving the carrier navigation information and the IMU error by combining a reprojection error item provided by the visual navigation sensor.
1) When the visual navigation sensor data S (k) is collected, the key frame t is recordedk+1The time machine body is bk+1At tkTime tk+1At a time, all togetherI IMU data, position pre-integral value, speed pre-integral value and angle pre-integral value of the carrier after error compensation
Figure BDA00023792420300002014
Figure BDA00023792420300002015
The calculation is performed as follows:
Figure BDA00023792420300002016
Figure BDA00023792420300002017
Figure BDA00023792420300002018
wherein,
Figure BDA00023792420300002019
respectively represent tkTime tk+1And step two, carrying out recursive calculation on the IMU data of the first frame from the IMU data of the first frame to the IMU data of the first frame by using the inertia pre-integration propagation equation between two adjacent IMU frames, wherein the carrier position pre-integration value, the speed pre-integration value and the angle pre-integration value are not subjected to error compensation in the time.
Figure BDA00023792420300002020
Respectively represent tkError states of error coefficient matrixes of an accelerometer and a gyroscope of a time carrier,
Figure BDA0002379242030000211
respectively represent tkZero offset error states of an accelerometer and a gyroscope of the time carrier.
Each jacobian matrix is calculated as follows:
Figure BDA0002379242030000212
Figure BDA0002379242030000213
Figure BDA0002379242030000214
wherein,
Figure BDA0002379242030000215
respectively represent tkTime tk+1At the time, the error states of the position pre-integral quantity, the speed pre-integral quantity and the angle pre-integral quantity of the carrier are calculated by recursion from the IMU data of the first frame to the IMU data of the l frame according to the error states of the inertia pre-integral of the two adjacent IMU frames given in the step three, and specifically, the method comprises the following steps:
Figure BDA0002379242030000216
Alis tk+1The value range of i is 1 to l, l is the number of IMU acquired data, N is noise, and F is the time error state transfer equationiFor systematic error state transition matrix, GiAs a noisy state transition matrix, Gl-1Is tkTo tk+1A noise state transition matrix at the sampling time of the first-1 IMU data at the time;
Figure BDA0002379242030000217
thus, the inertial pre-integration error is calculated as follows:
Figure BDA0002379242030000221
wherein, the attitude of the carrier is expressed by quaternion,
Figure BDA0002379242030000222
representing the imaginary part of the extracted quaternion,the angular pre-integration value of the carrier after the error is compensated,
Figure BDA0002379242030000224
is tkA rotation matrix from the time world coordinate system to the body system,
Figure BDA0002379242030000225
respectively represent tkThe position, the speed and the attitude of the time carrier under a world coordinate system,
Figure BDA0002379242030000226
respectively represent tk+1The position, the speed and the attitude of the time carrier under a world coordinate system,
Figure BDA0002379242030000227
respectively represent tkTime and tk+1The zero offset value of the accelerometer at the moment,
Figure BDA0002379242030000228
respectively represent tkTime and tk+1The zero-bias value of the gyroscope at that time,
Figure BDA0002379242030000229
respectively represent tkTime and tk+1The matrix of error coefficients of the accelerometer at the moment,
Figure BDA00023792420300002210
respectively represent tkTime and tk+1And (4) an error coefficient matrix of the time gyroscope.
2) The incremental equation for the inertial pre-integration error is calculated as follows:
Figure BDA00023792420300002211
wherein,
Figure BDA00023792420300002212
represents tkTime tk+1And 4, the covariance matrix of the time system is obtained by recursion calculation of the covariance matrix of the two adjacent IMU frames given in the step three.
δ X represents the error state quantity in the optimization process, including the error quantity δ X of the relative translation and rotation of the visual navigation sensor and the IMUsThe inverse depth of m +1 visual feature points in the sliding window, the error state quantity of the carrier position, the speed, the quaternion, the accelerometer and the gyroscope zero offset of n +1 measurement frames, and the error state quantity of the error coefficient matrix of the accelerometer and the gyroscope containing the scale factor error and the installation error:
δX＝[δx0,δx1,…,δxn,δxs,δλ0,δλ1,...δλm]
Figure BDA0002379242030000231
Figure BDA0002379242030000232
Figure BDA0002379242030000233
representing inertial pre-integration error
Figure BDA0002379242030000234
A jacobian matrix corresponding to the state variable X to be optimized, specifically, the state variable X to be optimized is:
X＝[x0,x1,…,xn,xs,λ0,λ1,...,λm]
Figure BDA0002379242030000235
Figure BDA0002379242030000236
wherein x0,x1,…,xnRepresenting the carrier-related state variable to be optimized, x, in n +1 measurement frames within the sliding windownRepresenting the position, the speed, the quaternion and the zero offset of the accelerometer and the gyroscope of a certain measurement frame in the sliding window, and an error coefficient matrix of the accelerometer and the gyroscope containing scale factor errors and installation errors; x is the number ofsRepresenting the relative translation and rotation of the visual navigation sensor and the IMU; lambda [ alpha ]mRepresenting the inverse depth of a certain visual feature point.
3) Inertial pre-integration error
Figure BDA0002379242030000237
Jacobian matrix of state variables X to be optimized
Figure BDA0002379242030000238
The calculation is performed as follows:
error pre-integration by inertia
Figure BDA0002379242030000239
Can know the relative state variable x to be optimizedsAnd xλThe jacobian matrix of (a) is 0, i.e.:
Figure BDA00023792420300002310
remaining variables to be optimized
Figure BDA00023792420300002311
Figure BDA00023792420300002312
The method is divided into four groups:
Figure BDA00023792420300002313
Figure BDA00023792420300002314
recording the Jacobian matrix of the relative inertial pre-integral error to the to-be-optimized variable as J0]、J[1]、J[2]、J[3]Each gives J [0]]、J[1]、J[2]、J[3]The calculation method of (2):
Figure BDA0002379242030000241
Figure BDA0002379242030000242
j2, J3 are calculated as follows:
Figure BDA0002379242030000243
Figure BDA0002379242030000251
calculating each element in the matrix separately to obtain the numerical expression form of J0, J1, J2, J3:
Figure BDA0002379242030000252
Figure BDA0002379242030000253
Figure BDA0002379242030000254
Figure BDA0002379242030000255
wherein I represents an identity matrix
Figure BDA0002379242030000261
For quaternions q ═ x y z λ]＝[ω λ]L, R listThe significance of the symbols are left and right [ · respectively]LAnd [. C]RThe left and right operators, respectively, representing the quaternion q may be expressed in particular as:
Figure BDA0002379242030000262
Figure BDA0002379242030000263
4) and (3) combining the reprojection error provided by the visual navigation sensor to construct an objective function as follows:
Figure BDA0002379242030000264
wherein, | | rp||2For the prior constraint of marginalization, only a small amount of measurement and states are reserved in the optimization, and other measurement and states are marginalized and converted into prior;
Figure BDA0002379242030000265
for inertial pre-integration error, B is the set of all IMU measurements,
Figure BDA0002379242030000266
is the covariance of the inertial pre-integration error;
Figure BDA0002379242030000267
reprojection errors provided for visual navigation sensors. And (3) using a Gaussian Newton nonlinear optimization method, stopping optimization when an error convergence state is reached or the iteration number reaches a threshold value, and outputting navigation information and an error coefficient matrix of the carrier.
Step five: outputting carrier navigation information and IMU errors, and circularly executing the first step to the fifth step.
The above description is only for the specific embodiment of the present invention, but the scope of the present invention is not limited thereto, and any changes or substitutions that can be easily conceived by those skilled in the art within the technical scope of the present invention are included in the scope of the present invention. Therefore, the protection scope of the present invention shall be subject to the protection scope of the claims.

Claims (4)

1. An IMU error online calibration method based on a factor graph is characterized by comprising the following steps:
s1, collecting the measurement values of an accelerometer and a gyroscope in an Inertial sensor, and carrying out pre-integration propagation on an IMU (Inertial measurement unit) by adopting the measurement values;
s2, calculating the pre-integration error transfer equation by using the measured value;
and S3, collecting and utilizing the measured values of other navigation sensors, namely a visual image sensor, a GPS receiver and a laser radar to solve the inertial pre-integration error, combining the error items provided by the other navigation sensors to optimally solve the carrier navigation information and the IMU error, and executing S1-S3 in a circulating manner.
2. The factor-graph-based IMU error online calibration method according to claim 1, wherein in S1, the pre-integration propagation method comprises:
s11, calculating error coefficient matrixes of the accelerometer and the gyroscope by using the measured values, wherein the error coefficient matrixes of the accelerometer and the gyroscope at the moment t are respectively as follows:
Figure FDA0002379242020000011
Figure FDA0002379242020000012
wherein M isa1、Ma2、Ma3Error coefficients, M, representing the X, Y, Z axes of the accelerometer, respectivelyω1、Mω2、Mω3Respectively representing the error coefficients of the X, Y, Z axes of the gyroscope,
Figure FDA0002379242020000013
showing the mounting error of the accelerometer in the X-axis and the Y-axis,
Figure FDA0002379242020000014
showing the mounting error of the accelerometer in the X-axis and Z-axis,
Figure FDA0002379242020000015
showing the installation error of the Y axis and the Z axis of the accelerometer;
Figure FDA0002379242020000016
showing the mounting error of the gyroscope X-axis and Y-axis,
Figure FDA0002379242020000017
showing the mounting error of the gyroscope X-axis and Z-axis,
Figure FDA0002379242020000018
showing the installation error of the Y axis and the Z axis of the gyroscope;
the elements at the diagonal positions of the error coefficient matrix are calculated according to the following matrix:
Figure FDA0002379242020000021
Figure FDA0002379242020000022
wherein Kax、Kay、KazScale factor errors representing three axes of the accelerometer, respectively; kωx、Kωy、KωzScale factor errors in three axes of the gyroscope are respectively represented;
s12, sampling time t for two adjacent IMUsiAnd ti+1Calculating ti+1The value of the moment inertia pre-integral is calculated by the following formula:
Figure FDA0002379242020000023
Figure FDA0002379242020000024
Figure FDA0002379242020000025
wherein, bkDenotes the initial tkA coordinate system of the body at a moment,
Figure FDA0002379242020000026
respectively representing the sampling time t of two adjacent inertia frames i and i +1iAnd ti+1The measured value of the accelerometer is measured,
Figure FDA0002379242020000027
respectively represent tiAnd ti+1The measurement value of the gyroscope at the moment, δ t represents the sampling period of the inertial sensor;
Figure FDA0002379242020000028
respectively represent ti+1The time, the position pre-integration value, the speed pre-integration value and the angle pre-integration value of the carrier,
Figure FDA0002379242020000029
respectively represent tiThe time, the position pre-integration value, the speed pre-integration value and the angle pre-integration value of the carrier,
Figure FDA00023792420200000210
is a unit quaternion; r (y) denotes the conversion of quaternions into rotation matrices,
Figure FDA00023792420200000211
i.e. representing t in terms of quaternioniThe moment carrier angle pre-integration value is converted into a rotation matrix,
Figure FDA00023792420200000212
i.e. representing t in terms of quaternioni+1Converting the carrier angle pre-integration value into a rotation matrix at the moment;
Figure FDA00023792420200000213
respectively represent tiThe acceleration at that moment is offset from zero of the gyroscope,
Figure FDA0002379242020000031
respectively represent ti+1The zero offset of the moment acceleration and the gyroscope, and the derivative of the moment acceleration and the gyroscope is white noise;
Figure FDA0002379242020000032
are respectively shown at tiA matrix of the acceleration at the moment and the error coefficient of the gyroscope,
Figure FDA0002379242020000033
are respectively shown at ti+1And (3) a matrix of the acceleration at the moment and the error coefficient of the gyroscope.
3. The factor-graph-based IMU error online calibration method according to claim 1, wherein the S2 includes:
s21, calculating an error state transition matrix F of the systemiAnd noise state transition matrix Gi：
Figure FDA0002379242020000034
Figure FDA0002379242020000035
Wherein,
Figure FDA0002379242020000036
representing t by quaternioniThe moment carrier angle pre-integration value is converted into a rotation matrix,
Figure FDA0002379242020000037
representing t by quaternioni+1Converting the pre-integral value of the carrier angle at the moment into a rotation matrix, wherein I is an identity matrix, delta t represents the sampling period of the inertial sensor, and the error state transition matrix FiAnd noise state transition matrix GiElement f in (1)mn、gmnWherein, the values of m and n are positive integers as follows:
Figure FDA0002379242020000041
Figure FDA0002379242020000042
Figure FDA0002379242020000043
Figure FDA0002379242020000044
Figure FDA0002379242020000045
Figure FDA0002379242020000046
Figure FDA0002379242020000047
Figure FDA0002379242020000048
Figure FDA0002379242020000049
Figure FDA00023792420200000410
wherein,
Figure FDA00023792420200000411
are respectively shown at tiA matrix of the acceleration at the moment and the error coefficient of the gyroscope,
Figure FDA00023792420200000412
respectively represent tiZero offset of moment acceleration and gyroscope, aiAnd ai+1Respectively represent tiTime and ti+1Time of day the output value of the accelerometer, ωiAnd omegai+1Respectively represent tiTime and ti+1The output value of the gyroscope at the moment;
s22, calculating two adjacent IMU sampling time tiAnd ti+1Error transfer equation, Jacobian matrix and covariance of the system, ti+1The error transfer equation, the Jacobian matrix and the covariance at the moment are respectively:
Ai+1＝FiAi+GiN
Figure FDA00023792420200000413
Pi+1＝FiPiFi T+GiQGi T
wherein t isiThe error transfer equation at a time is Ai，tiThe Jacobian of the time is Ji bk，tiCovariance of time of day is Pi；
Figure FDA00023792420200000414
Figure FDA0002379242020000051
Wherein, N is the noise,
Figure FDA0002379242020000052
represents tiThe position of the carrier at the moment pre-integrates the error state,
Figure FDA0002379242020000053
represents tiThe angular pre-integration error state of the carrier at the moment,
Figure FDA0002379242020000054
represents tiThe speed of the carrier at the moment pre-integrates the error state,
Figure FDA0002379242020000055
respectively represent tiZero offset error states of the accelerometer and gyroscope of the time carrier,
Figure FDA0002379242020000056
respectively represent tiError state of error coefficient matrix of the carrier at moment;
Figure FDA0002379242020000057
represents tiWhite noise from the gyroscope and accelerometer at the moment,
Figure FDA0002379242020000058
represents ti+1White noise from the gyroscope and accelerometer at the moment,
Figure FDA0002379242020000059
it is indicated that the accelerometer has zero-bias white noise,
Figure FDA00023792420200000510
representing white zero bias noise for the gyroscope.
4. The factor-graph-based IMU error online calibration method of claim 1, wherein in S3, the solution method of the carrier navigation information and IMU error is: collecting the measured values S (k) of the other navigation sensors, and comparing the current tk+1The time system is marked as bk+1At tkTime tk+1At the time, the carrier collects a total of I IMU data, and the position preconcentration value, the speed preconcentration value and the angle preconcentration value of the carrier after the error is compensated
Figure FDA00023792420200000517
The calculation formula of (a) is as follows:
Figure FDA00023792420200000511
Figure FDA00023792420200000512
Figure FDA00023792420200000513
wherein,
Figure FDA00023792420200000514
respectively represent tkTime tk+1A carrier position pre-integration value, a velocity pre-integration value, an angle pre-integration value, which are not subjected to error compensation at a time,
Figure FDA00023792420200000515
respectively represent tkError states of error coefficient matrixes of an accelerometer and a gyroscope of a time carrier,
Figure FDA00023792420200000516
respectively represent tkZero offset error states of an accelerometer and a gyroscope of a time carrier;
wherein, the calculation formula of each Jacobian matrix is as follows:
Figure FDA0002379242020000061
Figure FDA0002379242020000062
Figure FDA0002379242020000063
wherein,
Figure FDA0002379242020000064
respectively represent tkTime tk+1At the moment, the error states of the position pre-integral quantity, the speed pre-integral quantity and the angle pre-integral quantity of the carrier are obtained;
tk+1time error state transfer equation alIs composed of
Figure FDA0002379242020000065
Wherein, the value range of i is 1 to l, N is noise, FiFor systematic error state transition matrix, GiAs a noisy state transition matrix, Gl-1Is tkTo tk+1A noise state transition matrix at the sampling time of the l-1 th IMU data between the times;
Figure FDA0002379242020000066
therefore, the inertia pre-integral error calculation formula is as follows:
Figure FDA0002379242020000067
wherein, the attitude of the carrier is expressed by quaternion,
Figure FDA0002379242020000068
representing the imaginary part of the extracted quaternion,
Figure FDA0002379242020000069
the angular pre-integration value of the carrier after the error is compensated,
Figure FDA00023792420200000610
is tkA rotation matrix from the time world coordinate system to the body system,
Figure FDA00023792420200000611
respectively represent tkThe position, the speed and the attitude of the time carrier under a world coordinate system,
Figure FDA0002379242020000071
respectively represent tk+1The position, the speed and the attitude of the time carrier under a world coordinate system,
Figure FDA0002379242020000072
respectively represent tkTime and tk+1The zero offset value of the accelerometer at the moment,
Figure FDA0002379242020000073
respectively represent tkTime and tk+1The zero-bias value of the gyroscope at that time,
Figure FDA0002379242020000074
respectively represent tkTime and tk+1The matrix of error coefficients of the accelerometer at the moment,
Figure FDA0002379242020000075
respectively represent tkTime and tk+1An error coefficient matrix of the moment gyroscope;
the calculation formula of the increment equation of the inertia pre-integration error is as follows:
Figure FDA0002379242020000076
wherein,
Figure FDA0002379242020000077
represents tkTime tk+1A covariance matrix of the system between the moments, δ X representing an error state quantity in the optimization process, including an error quantity δ X of relative translation and rotation of the other navigation sensors and the IMUsError state quantity deltax provided by the other navigation sensorsλError state quantities of carrier position, velocity, quaternion, accelerometer and gyroscope zero bias for n +1 measurement frames in the sliding window, and error state quantities of the accelerometer and gyroscope error coefficient matrices containing scale factor errors and mounting errors:
δX＝[δx0,δx1,…,δxn,δxs,δxλ]
Figure FDA0002379242020000078
Figure FDA0002379242020000079
Figure FDA00023792420200000710
representing inertial pre-integration error
Figure FDA00023792420200000711
And (3) relative to a Jacobian matrix of the state variable X to be optimized, the state variable X to be optimized is as follows:
X＝[x0,x1,…,xn,xs,xλ]
wherein x0,x1,…,xnRepresenting the carrier-related state variable to be optimized, x, in n +1 measurement frames within the sliding windowsRepresenting the state variable to be optimized, x, constructed by other navigation sensors and IMUλRepresenting state variables to be optimized provided by other navigation sensors, in particular for x0,x1,…,xnA certain state quantity x ofkAnd xsComprises the following steps:
Figure FDA0002379242020000081
Figure FDA0002379242020000082
xkeach element in the matrix respectively represents the carrier position, speed, quaternion, zero offset of an accelerometer and a gyroscope of a specific measurement frame in the sliding window, and an error coefficient matrix of the accelerometer and the gyroscope of a scale factor error and an installation error, and the value of k is 0, 1,2 … n;
Figure FDA0002379242020000083
indicating the relative translation of the other navigation sensors and the IMU,
Figure FDA0002379242020000084
indicating the amount of relative rotation of the other navigation sensors with the IMU;
inertial pre-integration error
Figure FDA0002379242020000085
Jacobian matrix of state variables X to be optimized
Figure FDA0002379242020000086
The calculating method comprises the following steps:
error pre-integration by inertia
Figure FDA0002379242020000087
The expression of (2) shows that the inertia pre-integral error treats the optimized state variable xsAnd xλThe jacobian matrix of (a) is 0, i.e.:
Figure FDA0002379242020000088
the remaining variables to be optimized, namely the position, speed and attitude information and IMU error information of the carrier
Figure FDA0002379242020000089
Figure FDA00023792420200000810
The method is divided into four groups:
Figure FDA00023792420200000811
Figure FDA00023792420200000812
marking the Jacobian matrixes of the inertial pre-integral error relative to the variables to be optimized as J0, J1, J2 and J3;
the calculation modes of J0, J1, J2 and J3 are as follows:
Figure FDA0002379242020000091
Figure FDA0002379242020000092
Figure FDA0002379242020000101
Figure FDA0002379242020000102
calculating each element in the matrix separately to obtain the numerical expression form of J0, J1, J2, J3:
Figure FDA0002379242020000111
Figure FDA0002379242020000112
Figure FDA0002379242020000113
Figure FDA0002379242020000114
wherein I represents an identity matrix
Figure FDA0002379242020000115
For quaternions q ═ x y z λ]＝[ω λ]L, R are characterized by the meanings left and right [. cndot]LAnd [. C]RThe left and right operators, respectively, representing the quaternion q may be expressed in particular as:
Figure FDA0002379242020000121
Figure FDA0002379242020000122
and combining the error terms provided by the other navigation sensors to construct an objective function:
Figure FDA0002379242020000123
wherein, | | rp||2For marginalized a priori constraints, only guarantees are made in the optimizationLeaving a small number of measurements and states, while other measurements and states are marginalized and converted to priors;
Figure FDA0002379242020000124
for inertial pre-integration error, B is the set of all IMU measurements,
Figure FDA0002379242020000125
is the covariance of the inertial pre-integration error;
Figure FDA0002379242020000126
error terms provided for other navigation sensors;
and (3) using a Gaussian Newton nonlinear optimization method, stopping optimization when an error convergence state is reached or the iteration number reaches a threshold value, and outputting the carrier navigation information and the IMU error.
CN202010078321.9A 2020-02-03 2020-02-03 IMU error online calibration method based on factor graph Active CN111238535B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010078321.9A CN111238535B (en) 2020-02-03 2020-02-03 IMU error online calibration method based on factor graph

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010078321.9A CN111238535B (en) 2020-02-03 2020-02-03 IMU error online calibration method based on factor graph

Publications (2)

Publication Number Publication Date
CN111238535A true CN111238535A (en) 2020-06-05
CN111238535B CN111238535B (en) 2023-04-07

Family

ID=70868802

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010078321.9A Active CN111238535B (en) 2020-02-03 2020-02-03 IMU error online calibration method based on factor graph

Country Status (1)

Country Link
CN (1) CN111238535B (en)

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112611361A (en) * 2020-12-08 2021-04-06 华南理工大学 Method for measuring installation error of camera of airborne surveying and mapping pod of unmanned aerial vehicle
CN113175933A (en) * 2021-04-28 2021-07-27 南京航空航天大学 Factor graph combined navigation method based on high-precision inertia pre-integration
CN113776543A (en) * 2021-07-27 2021-12-10 武汉中海庭数据技术有限公司 Vehicle fusion positioning method and system, electronic equipment and storage medium
WO2022057350A1 (en) * 2020-09-17 2022-03-24 江苏大学 Inertial pre-integration method for combined motion measurement system based on nonlinear integral compensation
CN112762938B (en) * 2020-12-24 2022-08-09 哈尔滨工程大学 Factor graph co-location method based on rotation matrix
CN115507848A (en) * 2022-11-09 2022-12-23 苏州华米导航科技有限公司 Positioning method based on wheel speed inertial navigation combined pre-integration and RTK tight combination
CN115615437A (en) * 2022-09-20 2023-01-17 哈尔滨工程大学 Factor graph combined navigation method

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103245359A (en) * 2013-04-23 2013-08-14 南京航空航天大学 Method for calibrating fixed errors of inertial sensor in inertial navigation system in real time
CN104019828A (en) * 2014-05-12 2014-09-03 南京航空航天大学 On-line calibration method for lever arm effect error of inertial navigation system in high dynamic environment
CN104764467A (en) * 2015-04-08 2015-07-08 南京航空航天大学 Online adaptive calibration method for inertial sensor errors of aerospace vehicle
US20190313085A1 (en) * 2018-04-05 2019-10-10 Fyusion, Inc. Trajectory smoother for generating multi-view interactive digital media representations

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103245359A (en) * 2013-04-23 2013-08-14 南京航空航天大学 Method for calibrating fixed errors of inertial sensor in inertial navigation system in real time
CN104019828A (en) * 2014-05-12 2014-09-03 南京航空航天大学 On-line calibration method for lever arm effect error of inertial navigation system in high dynamic environment
CN104764467A (en) * 2015-04-08 2015-07-08 南京航空航天大学 Online adaptive calibration method for inertial sensor errors of aerospace vehicle
US20190313085A1 (en) * 2018-04-05 2019-10-10 Fyusion, Inc. Trajectory smoother for generating multi-view interactive digital media representations

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2022057350A1 (en) * 2020-09-17 2022-03-24 江苏大学 Inertial pre-integration method for combined motion measurement system based on nonlinear integral compensation
CN112611361A (en) * 2020-12-08 2021-04-06 华南理工大学 Method for measuring installation error of camera of airborne surveying and mapping pod of unmanned aerial vehicle
CN112762938B (en) * 2020-12-24 2022-08-09 哈尔滨工程大学 Factor graph co-location method based on rotation matrix
CN113175933A (en) * 2021-04-28 2021-07-27 南京航空航天大学 Factor graph combined navigation method based on high-precision inertia pre-integration
CN113175933B (en) * 2021-04-28 2024-03-12 南京航空航天大学 Factor graph integrated navigation method based on high-precision inertial pre-integration
CN113776543A (en) * 2021-07-27 2021-12-10 武汉中海庭数据技术有限公司 Vehicle fusion positioning method and system, electronic equipment and storage medium
CN113776543B (en) * 2021-07-27 2023-11-28 武汉中海庭数据技术有限公司 Vehicle fusion positioning method, system, electronic equipment and storage medium
CN115615437A (en) * 2022-09-20 2023-01-17 哈尔滨工程大学 Factor graph combined navigation method
CN115615437B (en) * 2022-09-20 2024-04-30 哈尔滨工程大学 Factor graph integrated navigation method
CN115507848A (en) * 2022-11-09 2022-12-23 苏州华米导航科技有限公司 Positioning method based on wheel speed inertial navigation combined pre-integration and RTK tight combination

Also Published As

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

Similar Documents

Publication Publication Date Title
CN111238535B (en) IMU error online calibration method based on factor graph
CN108731670B (en) Inertial/visual odometer integrated navigation positioning method based on measurement model optimization
CN113311411B (en) Laser radar point cloud motion distortion correction method for mobile robot
CN110631574B (en) inertia/odometer/RTK multi-information fusion method
CN110956665B (en) Bidirectional calculation method, system and device for turning track of vehicle
CN111156997B (en) Vision/inertia combined navigation method based on camera internal parameter online calibration
CN112697138B (en) Bionic polarization synchronous positioning and composition method based on factor graph optimization
CN113175933B (en) Factor graph integrated navigation method based on high-precision inertial pre-integration
CN111024070A (en) Inertial foot binding type pedestrian positioning method based on course self-observation
CN114526745A (en) Drawing establishing method and system for tightly-coupled laser radar and inertial odometer
CN104359496B (en) The high-precision attitude modification method compensated based on the deviation of plumb line
WO2022160391A1 (en) Magnetometer information assisted mems gyroscope calibration method and calibration system
CN113074753A (en) Star sensor and gyroscope combined attitude determination method, combined attitude determination system and application
CN114216456A (en) Attitude measurement method based on IMU and robot body parameter fusion
CN113514064A (en) Robust factor graph multi-source fault-tolerant navigation method
CN109387198B (en) Inertia/vision milemeter combined navigation method based on sequential detection
CN115200578A (en) Polynomial optimization-based inertial-based navigation information fusion method and system
CN113503872B (en) Low-speed unmanned aerial vehicle positioning method based on fusion of camera and consumption-level IMU
CN113008229B (en) Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor
CN114184190A (en) Inertial/odometer integrated navigation system and method
CN113340324A (en) Visual inertia self-calibration method based on depth certainty strategy gradient
CN113701755B (en) Optical remote sensing satellite attitude determination method without high-precision gyroscope
CN111637892A (en) Mobile robot positioning method based on combination of vision and inertial navigation
CN116338719A (en) Laser radar-inertia-vehicle fusion positioning method based on B spline function
CN114216480B (en) Precise alignment method of inertial navigation 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