CN111238535B - 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
CN111238535B
CN111238535B CN202010078321.9A CN202010078321A CN111238535B CN 111238535 B CN111238535 B CN 111238535B CN 202010078321 A CN202010078321 A CN 202010078321A CN 111238535 B CN111238535 B CN 111238535B
Authority
CN
China
Prior art keywords
error
time
gyroscope
carrier
matrix
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202010078321.9A
Other languages
Chinese (zh)
Other versions
CN111238535A (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

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 the traditional IMU error calibration methods are offline calibration, and IMU errors can be changed 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, which is essentially a dead reckoning system, 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 an Inertial sensor, and carrying out pre-integration propagation on an IMU (Inertial measurement unit) by adopting the measurement values.
And S2, calculating the pre-integral error transfer equation by using the measured value.
And S3, collecting and utilizing measured values of other navigation sensors, namely a visual image sensor, a GPS receiver and a laser radar to solve an inertial pre-integration error, optimally solving carrier navigation information and IMU errors by combining error items provided by the other navigation sensors, and circularly executing S1-S3.
Further, in 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 is a1 、M a2 、M a3 Error coefficients, M, representing the X, Y, Z axes of the accelerometer, respectively ω1 、M ω2 、M ω3 Respectively representing error coefficients of X, Y and Z axes of the gyroscope,
Figure BDA0002379242030000023
represents the installation error of the X axis and the Y axis of the accelerometer>
Figure BDA0002379242030000031
Represents the installation error of the X axis and the Z axis of the accelerometer>
Figure BDA0002379242030000032
Showing the installation error of the Y axis and the Z axis of the accelerometer; />
Figure BDA0002379242030000033
Represents the installation error of the gyroscope X axis and Y axis>
Figure BDA0002379242030000034
Represents the installation error of the X axis and the Z axis of the gyroscope>
Figure BDA0002379242030000035
The installation error of a Y axis and a Z axis of the gyroscope is represented;
the elements at the diagonal positions of the error coefficient matrix are calculated according to the following matrix:
Figure BDA0002379242030000036
Figure BDA0002379242030000037
wherein K ax 、K ay 、K az Scale factor errors in three axes of the accelerometer are respectively represented; k ωx 、K ωy 、K ωz Scale factor errors in three axes of the gyroscope are respectively represented;
s12, aiming at two adjacent IMU sampling time t i And t i+1 Calculating t i+1 The value of the moment inertia pre-integral is calculated by the following formula:
Figure BDA0002379242030000038
Figure BDA0002379242030000039
Figure BDA00023792420300000310
wherein, b k Represents the initial t k A coordinate system of the body at a moment,
Figure BDA00023792420300000311
respectively representing the sampling time t of two adjacent inertia frames i and i +1 i And t i+1 Based on the measurement value of the accelerometer, is taken>
Figure BDA00023792420300000312
Respectively represent t i And t i+1 Measuring values of the gyroscope at the moment, wherein deltat represents a sampling period of an inertial sensor; />
Figure BDA00023792420300000313
Respectively represent t i+1 Based on the time, the pre-integration value of position, the pre-integration value of speed and the pre-integration value of angle of the support>
Figure BDA00023792420300000314
Respectively represent t i Time, pre-integration value of position, pre-integration value of speed and pre-integration value of angle of carrier, and initial time->
Figure BDA0002379242030000041
And &>
Figure BDA0002379242030000042
Is 0, since γ is a rotation expressed in quaternions, based on the number of revolutions in the image sensor>
Figure BDA0002379242030000043
Is a unit quaternion; r (gamma) represents the conversion of a quaternion into a rotation matrix, based on the quaternion>
Figure BDA0002379242030000044
I.e. representing t in terms of quaternion i The pre-integration value of the instant carrier angle is converted into a rotation matrix, which is then evaluated>
Figure BDA0002379242030000045
I.e. representing t in terms of quaternion i+1 Converting the carrier angle pre-integration value into a rotation matrix at the moment; />
Figure BDA0002379242030000046
Respectively represent t i The acceleration at that moment is offset from zero of the gyroscope,
Figure BDA0002379242030000047
respectively represent t i+1 The 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 t i The time acceleration and the error coefficient matrix of the gyroscope are/is>
Figure BDA0002379242030000049
Figure BDA00023792420300000410
Are respectively shown at t i+1 And (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 system i And noise state transition matrix G i ：
Figure BDA00023792420300000411
Figure BDA00023792420300000412
Wherein the content of the first and second substances,
Figure BDA00023792420300000413
representing t by quaternion i The moment carrier angle pre-integration value is converted into a rotation matrix,
Figure BDA0002379242030000051
representing t by quaternion i+1 Converting the angular pre-integral value of the time carrier 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 F i And noise state transition matrix G i Element f in (1) mn 、g mn Wherein, 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 the content of the first and second substances,
Figure BDA00023792420300000512
are respectively shown at t i The acceleration at the moment and the error coefficient matrix of the gyroscope are combined>
Figure BDA00023792420300000513
Respectively represent t i Moment acceleration and zero offset of gyroscope, a i And a i+1 Respectively represent t i Time and t i+1 Time of day the output value of the accelerometer, ω i And omega i+1 Respectively represent t i Time and t i+1 Time of day output of the gyroscopeThe value is obtained.
S22, calculating sampling time t of two adjacent IMUs i And t i+1 Error transfer equation, jacobian matrix and covariance of the system, t i+1 The error transfer equation, the jacobian matrix and the covariance at the moment are respectively:
A i+1 ＝F i A i +G i N
Figure BDA0002379242030000061
P i+1 ＝F i P i F i T +G i QG i T
wherein t is i The error transfer equation at a time is A i ，t i The Jacobian of the time is J i bk ，t i Covariance of time of day is P i ；
Figure BDA0002379242030000062
Figure BDA0002379242030000063
Wherein, N is the noise,
Figure BDA0002379242030000064
represents t i Position pre-integration error state of the time carrier>
Figure BDA0002379242030000065
Denotes t i Angular pre-integration error state of time carrier>
Figure BDA0002379242030000066
Represents t i Speed pre-integration error state for time-of-day carrier>
Figure BDA0002379242030000067
Respectively represent t i Zero offset error state of accelerometer and gyroscope of moment carrier>
Figure BDA0002379242030000068
Respectively represent t i Error state of error coefficient matrix of the carrier at moment; />
Figure BDA0002379242030000069
Represents t i White noise in the gyroscope and accelerometer at times>
Figure BDA00023792420300000610
Represents t i+1 White noise at moment gyroscope and accelerometer>
Figure BDA00023792420300000611
Represents the accelerometer has zero offset white noise->
Figure BDA00023792420300000612
Representing white zero bias noise of the gyroscope;
Figure BDA00023792420300000613
a Jacobian matrix which is the state of the system at the initial moment and has ^ er>
Figure BDA00023792420300000614
Q is a covariance matrix of noise N, with an initial time
Figure BDA00023792420300000615
Further, in S3, a solution method of the carrier navigation information and the IMU error is as follows:
collecting the measured values S (k) of the other navigation sensors, and converting the current t into the current t k+1 The time system is marked as b k+1 At t k Time t k+1 At 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, the first and the second end of the pipe are connected with each other,
Figure BDA0002379242030000072
respectively represent t k Time t k+1 Position pre-integration value, speed pre-integration value, angle pre-integration value, and combination value of carrier without error compensation at any time>
Figure BDA0002379242030000073
Respectively represent t k Error state of the error coefficient matrix of the accelerometer or gyroscope of the time-of-day carrier>
Figure BDA0002379242030000074
Respectively represent t k Zero 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, the first and the second end of the pipe are connected with each other,
Figure BDA0002379242030000078
respectively represent t k Time t k+1 At 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;
t k+1 time error state transfer equation a l Is 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, F i For systematic error state transition matrix, G i As a noisy state transition matrix, G l-1 Is t k To t k+1 A 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
represents the imaginary part of the extracted quaternion, and->
Figure BDA0002379242030000083
Angle pre-integration value compensated for faulty carriers, based on a characteristic value determined in advance in the evaluation unit>
Figure BDA0002379242030000084
Is t k A rotation matrix from the time world coordinate system to the body system,
Figure BDA0002379242030000085
respectively represent t k The position, the speed and the posture quantity of the moment carrier under the world coordinate system>
Figure BDA0002379242030000086
Respectively represent t k+1 The position, the speed and the posture quantity of the moment carrier under the world coordinate system>
Figure BDA0002379242030000087
Respectively represent t k Time and t k+1 The zero offset value of the accelerometer at that moment>
Figure BDA0002379242030000088
Respectively represent t k Time and t k+1 The zero-bias value of the gyroscope at that time,
Figure BDA0002379242030000089
respectively represent t k Time and t k+1 An error coefficient matrix for the time accelerometer, based on the time of day>
Figure BDA00023792420300000810
Respectively represent t k Time and t k+1 An 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, the first and the second end of the pipe are connected with each other,
Figure BDA00023792420300000812
represents t k Time t k+1 Covariance matrix of the system at time, deltax represents the error state quantity in the optimization process,including error amounts deltax of relative translation and rotation of other navigation sensors and IMU s Error 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＝[δx 0 ,δx 1 ,…,δx n ,δx s ,δx λ ]
Figure BDA0002379242030000091
Figure BDA0002379242030000092
Figure BDA0002379242030000093
representing inertial pre-integration error pick-up>
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＝[x 0 ,x 1 ,…,x n ,x s ,x λ ]
wherein x is 0 ,x 1 ,…,x n Representing the carrier-related state variable to be optimized, x, in n +1 measurement frames within the sliding window s Representing 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 x 0 ,x 1 ,…,x n A certain determined state quantity x of k And x s Comprises the following steps:
Figure BDA0002379242030000095
Figure BDA0002379242030000096
x k each 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 scale factor error and installation error, and the value of k is 0,1,2 \8230n;
Figure BDA0002379242030000097
indicates the relative translation amount of the other navigation sensor 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
The Jacobian matrix which corresponds to the state variable 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 inertial pre-integration error treats the optimized state variable x s And 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 comprises four groups: />
Figure BDA0002379242030000102
Figure BDA0002379242030000103
Labeling the Jacobian matrixes of the inertia pre-integral error and the variables to be optimized as J [0], J [1], J [2] and J [3];
the calculation modes of J [0], J [1], J [2] and J [3] 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 quaternion q = [ x y z λ =]＝[ω λ]The significance of L and R is left and right [. Degree] L And [ · C] R The 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, | | r p || 2 For 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, B is @>
Figure BDA0002379242030000144
For inertial pre-integrationCovariance of the 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 beneficial effects of the invention are:
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 online calibration method has the following 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 defects of single consideration on IMU device errors, simple IMU modeling, and only zero offset which is an IMU error is considered, and only partial compensation can be performed on the IMU error; 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 (5) 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 is a1 、M a2 、M a3 Respectively representing the error coefficients, M, of the X, Y, Z axes of the accelerometer ω1 、M ω2 、M ω3 Error coefficients of X, Y and Z axes of the gyroscope are respectively represented,
Figure BDA0002379242030000167
represents the installation error of the X axis and the Y axis of the accelerometer>
Figure BDA0002379242030000168
Represents the installation error of the X axis and the Z axis of the accelerometer>
Figure BDA0002379242030000169
Showing the installation error of the Y axis and the Z axis of the accelerometer; />
Figure BDA00023792420300001610
Represents the installation error of the gyroscope X axis and Y axis>
Figure BDA00023792420300001611
Represents the installation error of the gyroscope in the X axis and the Z axis and is combined with the gyroscope>
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 K ax 、K ay 、K az Scale factor errors in three axes of the accelerometer are respectively represented; k is ωx 、K ωy 、K ωz Respectively representing scale factor errors in three axes of the gyroscope.
2) Sampling time t for two adjacent IMUs i And t i+1 Calculate t as follows i+1 Value of the moment inertia pre-integral:
Figure BDA0002379242030000173
Figure BDA0002379242030000174
Figure BDA0002379242030000175
wherein, b k Denotes the initial t k A coordinate system of the body at a moment,
Figure BDA0002379242030000176
representing two adjacent inertial frames i and i +1, respectivelySampling time t i And t i+1 Data of the time accelerometer->
Figure BDA0002379242030000177
Respectively represent t i And t i+1 Data of a gyroscope at the moment, delta t represents a sampling period of an inertial sensor; />
Figure BDA0002379242030000178
Figure BDA0002379242030000179
Respectively represent t i+1 Based on the time, the pre-integration value of position, the pre-integration value of speed and the pre-integration value of angle of the support>
Figure BDA00023792420300001710
Respectively represent t i Based on the time, the pre-integration value of position, the pre-integration value of speed and the pre-integration value of angle of the support>
Figure BDA00023792420300001711
Is a unit quaternion; γ is the rotation expressed in quaternion, R (γ) expresses the conversion of quaternion into rotation matrix, and/or ` R `>
Figure BDA00023792420300001712
Representing t by quaternion i The moment carrier angle pre-integration value is converted into a rotation matrix,
Figure BDA00023792420300001713
representing t by quaternion i+1 Converting the carrier angle pre-integration value into a rotation matrix at the moment; at an initial moment in time>
Figure BDA00023792420300001714
And &>
Figure BDA00023792420300001715
Is 0 and/or>
Figure BDA00023792420300001716
Respectively represent t i Acceleration at moment and zero offset of gyroscope>
Figure BDA00023792420300001717
Respectively represent t i+1 The 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 t i The acceleration at the moment and the error coefficient matrix of the gyroscope are combined>
Figure BDA0002379242030000182
Are respectively shown at t i+1 And (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 system i And noise state transition matrix G i The calculation is performed as follows:
Figure BDA0002379242030000186
Figure BDA0002379242030000187
wherein, the first and the second end of the pipe are connected with each other,
Figure BDA0002379242030000188
to representT is expressed by quaternion i The moment carrier angle pre-integration value is converted into a rotation matrix,
Figure BDA0002379242030000189
representing t by quaternion i+1 Converting the angular pre-integral value of the time carrier 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 F i And noise state transition matrix G i Element f in (1) mn 、g mn Wherein, 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 the content of the first and second substances,
Figure BDA00023792420300001911
are respectively shown at t i The time acceleration and the error coefficient matrix of the gyroscope are/is>
Figure BDA00023792420300001912
Respectively represent t i Zero offset of moment acceleration and gyroscope, a i And a i+1 Respectively represent t i Time and t i+1 Time of day the output value of the accelerometer, ω i And omega i+1 Respectively represent t i Time and t i+1 The output value of the gyroscope at the moment.
2) Sampling time t for two adjacent IMUs i And t i+1 The error transfer equation, jacobian matrix and covariance of the system are calculated as follows:
A i+1 ＝F i A i +G i N
Figure BDA00023792420300001913
P i+1 ＝F i P i F i T +G i QG i T
wherein t is i The error transfer equation at a time is A i ，t i The Jacobian at a time is J i bk ，t i Covariance of time of day is P i ；
Figure BDA00023792420300001914
Figure BDA0002379242030000201
Wherein, N is the noise,
Figure BDA0002379242030000202
denotes t i Position pre-integration error state for time carrier>
Figure BDA0002379242030000203
Represents t i The speed pre-integration error status of the moment carrier>
Figure BDA0002379242030000204
Represents t i Angle pre-integration error status of time carrier>
Figure BDA0002379242030000205
Respectively represent t i Zero-offset error state of accelerometer and gyroscope of moment carrier>
Figure BDA0002379242030000206
Respectively represent t i Error state of error coefficient matrix of the carrier at moment; />
Figure BDA0002379242030000207
Represents t i White noise in the gyroscope and accelerometer at times>
Figure BDA0002379242030000208
Represents t i+1 White noise at moment gyroscope and accelerometer>
Figure BDA0002379242030000209
Represents the accelerometer has zero offset white noise->
Figure BDA00023792420300002010
Representing white gyroscope zero bias noise; />
Figure BDA00023792420300002011
A Jacobian matrix which is the state of the system at the initial moment and has ^ er>
Figure BDA00023792420300002012
Q is the covariance matrix of the noise N, initially ` at>
Figure BDA00023792420300002013
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 acquired, the key frame t is recorded k+1 The time machine body is b k+1 At t k Time t k+1 At the time, there are a total of one IMU data, the position preconcentration value, the speed preconcentration value and the angle preconcentration value of the carrier after the error compensation
Figure BDA00023792420300002014
Figure BDA00023792420300002015
The calculation is performed as follows:
Figure BDA00023792420300002016
Figure BDA00023792420300002017
Figure BDA00023792420300002018
wherein the content of the first and second substances,
Figure BDA00023792420300002019
respectively represent t k Time to t k+1 And 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 t k Error state of the error coefficient matrix of the accelerometer or gyroscope of the time-of-day carrier>
Figure BDA0002379242030000211
Respectively represent t k Zero 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, the first and the second end of the pipe are connected with each other,
Figure BDA0002379242030000215
respectively represent t k Time t k+1 At 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 recurred from the IMU data of the first frame by the error states of the inertia pre-integral of the two adjacent IMU frames given by the step threeThe IMU data of the first frame are calculated, specifically, the following:
Figure BDA0002379242030000216
A l is t k+1 The 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 equation i For systematic error state transition matrix, G i As a noisy state transition matrix, G l-1 Is t k To t k+1 A 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
represents the imaginary part of the extracted quaternion>
Figure BDA0002379242030000223
Angle pre-integration value of the compensated carrier after an error>
Figure BDA0002379242030000224
Is t k A rotation matrix from the time world coordinate system to the body system,
Figure BDA0002379242030000225
respectively represent t k The position, the speed and the posture quantity of the moment carrier under the world coordinate system>
Figure BDA0002379242030000226
Respectively represent t k+1 The position, the speed and the posture quantity of the moment carrier under the world coordinate system>
Figure BDA0002379242030000227
Respectively represent t k Time and t k+1 The zero offset value of the accelerometer at that moment>
Figure BDA0002379242030000228
Respectively represent t k Time and t k+1 The zero-bias value of the gyroscope at that time,
Figure BDA0002379242030000229
respectively represent t k Time and t k+1 The error coefficient matrix of the time accelerometer, ->
Figure BDA00023792420300002210
Respectively represent t k Time and t k+1 And (5) an error coefficient matrix of the moment gyroscope.
2) The incremental equation for the inertial pre-integration error is calculated as follows:
Figure BDA00023792420300002211
wherein the content of the first and second substances,
Figure BDA00023792420300002212
represents t k Time t k+1 And 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 an error state quantity in the optimization process, including an error quantity δ X of relative translation and rotation of the visual navigation sensor and the IMU s Inverse depth of m +1 visual feature points in the sliding window, error state quantity of carrier position, velocity, quaternion, accelerometer and gyroscope zero bias of n +1 measurement frames, and accelerometer including scale factor error and installation errorError state quantity of the error coefficient matrix of the gyroscope:
δX＝[δx 0 ,δx 1 ,…,δx n ,δx s ,δλ 0 ,δλ 1 ,...δλ m ]
Figure BDA0002379242030000231
Figure BDA0002379242030000232
Figure BDA0002379242030000233
representing inertial pre-integration error pick-up>
Figure BDA0002379242030000234
A jacobian matrix corresponding to the state variable X to be optimized, specifically, the state variable X to be optimized is: />
X＝[x 0 ,x 1 ,…,x n ,x s ,λ 0 ,λ 1 ,...,λ m ]
Figure BDA0002379242030000235
Figure BDA0002379242030000236
Wherein x 0 ,x 1 ,…,x n Representing the carrier-related state variable to be optimized, x, in n +1 measurement frames within the sliding window n Representing the position, the speed, the quaternion, the zero offset of an accelerometer and a gyroscope of a certain measuring 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 of s Representing the relative translation and rotation of the visual navigation sensor and the IMU; lambda [ alpha ] m Indicates a certainThe inverse depth of the visual feature point.
3) Inertial pre-integration error
Figure BDA0002379242030000237
Jacobian matrix in combination with a state variable 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 optimized s And x λ Has a jacobian matrix of 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
The Jacobian matrix for recording the relative inertial pre-integral error to the variables to be optimized is J [0]]、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 quaternion q = [ x y z λ =]＝[ω λ]The significance of L and R is left and right [. Degree] L And [. C] R The 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, | | r p || 2 For 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, B is @>
Figure BDA0002379242030000266
Covariance as 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 (1)

1. An IMU error online calibration method based on a factor graph is characterized by comprising the following steps: s1, collecting 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, wherein the pre-integration propagation method comprises the following steps:
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 FDA0004074028160000011
Figure FDA0004074028160000012
wherein, M a1 、M a2 、M a3 Respectively representing the error coefficients, M, of the X, Y, Z axes of the accelerometer ω1 、M ω2 、M ω3 Error coefficients of X, Y and Z axes of the gyroscope are respectively represented,
Figure FDA0004074028160000013
represents the installation error of the X axis and the Y axis of the accelerometer>
Figure FDA0004074028160000014
Represents the installation error of the X axis and the Z axis of the accelerometer>
Figure FDA0004074028160000015
Showing the installation error of the Y axis and the Z axis of the accelerometer; />
Figure FDA0004074028160000016
Represents the installation error of the gyroscope X axis and Y axis>
Figure FDA0004074028160000017
Represents the installation error of the gyroscope in the X axis and the Z axis and is combined with the gyroscope>
Figure FDA0004074028160000018
The installation error of a Y axis and a Z axis of the gyroscope is represented;
the elements at the diagonal positions in the error coefficient matrix are calculated according to the following matrix:
Figure FDA0004074028160000019
Figure FDA00040740281600000110
wherein K ax 、K ay 、K az Scale factor errors representing three axes of the accelerometer, respectively; k ωx 、K ωy 、K ωz Scale factor errors in three axes of the gyroscope are respectively represented;
s12, aiming at two adjacent IMU sampling moments t i And t i+1 Calculating t i+1 The value of the moment inertia pre-integral is calculated by the following formula:
Figure FDA0004074028160000021
Figure FDA0004074028160000022
Figure FDA0004074028160000023
wherein, b k Represents the initial t k A coordinate system of the body at a moment,
Figure FDA0004074028160000024
respectively representing the sampling time t of two adjacent inertia frames i and i +1 i And t i+1 A measurement of the accelerometer, based on a time of day>
Figure FDA0004074028160000025
Respectively represent t i And t i+1 The measurement value of the gyroscope at the moment, δ t represents the sampling period of the inertial sensor; />
Figure FDA0004074028160000026
Respectively represent t i+1 Time, pre-integration value for position, pre-integration value for speed and pre-integration value for angle of carrier, and method for determining the location and/or speed of a vehicle based on the pre-integration value>
Figure FDA0004074028160000027
Respectively represent t i Based on the time, the pre-integration value of position, the pre-integration value of speed and the pre-integration value of angle of the support>
Figure FDA0004074028160000028
Is a unit quaternion; r (y) denotes the conversion of quaternions into rotation matrices,
Figure FDA0004074028160000029
i.e. representing t expressed by quaternions i The pre-integration value of the instant carrier angle is converted into a rotation matrix, which is then evaluated>
Figure FDA00040740281600000210
I.e. representing t expressed by quaternions i+1 Converting the carrier angle pre-integration value into a rotation matrix at the moment; />
Figure FDA00040740281600000211
Respectively represent t i Acceleration at moment and zero offset of gyroscope>
Figure FDA00040740281600000212
Individual watchShow t i+1 The zero offset of the moment acceleration and the gyroscope, and the derivative of the moment acceleration and the gyroscope is white noise; />
Figure FDA00040740281600000213
Are respectively shown at t i The time acceleration and the error coefficient matrix of the gyroscope are/is>
Figure FDA00040740281600000214
Are respectively shown at t i+1 A time acceleration and an error coefficient matrix of the gyroscope;
s2, calculating a pre-integral error transfer equation by using the measured value, wherein the method specifically comprises the following steps:
s21, calculating an error state transition matrix F of the system i And noise state transition matrix G i ：
Figure FDA0004074028160000031
Figure FDA0004074028160000032
Wherein, the first and the second end of the pipe are connected with each other,
Figure FDA0004074028160000033
representing t by quaternion i The pre-integration value of the instant carrier angle is converted into a rotation matrix, which is then evaluated>
Figure FDA0004074028160000034
Representing t by quaternion i+1 Converting 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 F i And noise state transition matrix G i Element f in (1) mn 、g mn Wherein the values of m and n are positive integers as follows: />
Figure FDA0004074028160000041
Figure FDA0004074028160000042
Figure FDA0004074028160000043
Figure FDA0004074028160000044
Figure FDA0004074028160000045
Figure FDA0004074028160000046
Figure FDA0004074028160000047
Figure FDA0004074028160000048
Figure FDA0004074028160000049
Figure FDA00040740281600000410
Wherein, the first and the second end of the pipe are connected with each other,
Figure FDA00040740281600000411
are respectively shown at t i The time acceleration and the error coefficient matrix of the gyroscope are/is>
Figure FDA00040740281600000412
Respectively represent t i Moment acceleration and zero offset of gyroscope, a i And a i+1 Respectively represent t i Time and t i+1 Time of day the output value of the accelerometer, ω i And omega i+1 Respectively represent t i Time and t i+1 The output value of the gyroscope at the moment;
s22, calculating sampling time t of two adjacent IMUs i And t i+1 Error transfer equation, jacobian matrix and covariance of the system, t i+1 The error transfer equation, the Jacobian matrix and the covariance at the moment are respectively:
A i+1 ＝F i A i +G i N
Figure FDA00040740281600000413
P i+1 ＝F i P i F i T +G i QG i T
wherein t is i The error transfer equation at a time is A i ，t i The Jacobian of the time is J i bk ，t i Covariance of time of day is P i ；
Figure FDA00040740281600000414
Figure FDA0004074028160000051
Wherein, N is the noise,
Figure FDA0004074028160000052
denotes t i Position pre-integration error state of the time carrier>
Figure FDA0004074028160000053
Represents t i Angular pre-integration error state of time carrier>
Figure FDA0004074028160000054
Represents t i The speed pre-integration error status of the moment carrier>
Figure FDA0004074028160000055
Respectively represent t i Zero-offset error state of accelerometer and gyroscope of moment carrier>
Figure FDA0004074028160000056
Respectively represent t i Error state of error coefficient matrix of the carrier at moment; />
Figure FDA0004074028160000057
Represents t i White noise in the gyroscope and accelerometer at times>
Figure FDA0004074028160000058
Denotes t i+1 White noise at moment gyroscope and accelerometer>
Figure FDA0004074028160000059
Represents the accelerometer has zero offset white noise->
Figure FDA00040740281600000510
Representing white zero bias noise of the gyroscope; />
S3, collecting and utilizing the measurement values of the visual image sensor, the GPS receiver and the laser radar to solve an inertial pre-integration error, combining error items provided by navigation sensors except the visual image sensor, the GPS receiver, the laser radar and the inertial sensor to optimally solve carrier navigation information and IMU error, and circularly executing S1-S3, wherein the solving method of the carrier navigation information and the IMU error comprises the following steps:
collecting the measured values S (k) of the visual image sensor, the GPS receiver and the laser radar, and comparing the current t k+1 The organism system at time is marked as b k+1 At t, at k Time t k+1 At 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 FDA00040740281600000511
The calculation formula of (a) is as follows:
Figure FDA00040740281600000512
Figure FDA00040740281600000513
Figure FDA00040740281600000514
wherein the content of the first and second substances,
Figure FDA00040740281600000515
respectively represent t k Time t k+1 Position pre-integration value, speed pre-integration value, angle pre-integration value, and combination value of carrier without error compensation at any time>
Figure FDA00040740281600000516
Respectively represent t k Error coefficient matrix of accelerometer and gyroscope of time carrierError state>
Figure FDA0004074028160000061
Respectively represent t k Zero 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 FDA0004074028160000062
Figure FDA0004074028160000063
Figure FDA0004074028160000064
wherein, the first and the second end of the pipe are connected with each other,
Figure FDA0004074028160000065
respectively represent t k Time t k+1 In 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;
t k+1 time error state transfer equation A l Is composed of
Figure FDA0004074028160000066
Wherein, the value range of i is 1 to l, N is noise, F i For systematic error state transition matrix, G i As a noisy state transition matrix, G l-1 Is t k To t k+1 A noise state transition matrix at the sampling time of the l-1 th IMU data between the moments;
Figure FDA0004074028160000067
therefore, the inertia pre-integral error calculation formula is as follows:
Figure FDA0004074028160000068
wherein, the attitude of the carrier is expressed by quaternion,
Figure FDA0004074028160000071
represents the imaginary part of the extracted quaternion>
Figure FDA0004074028160000072
Angle pre-integration value of the compensated carrier after an error>
Figure FDA0004074028160000073
Is t k A rotation matrix from the time world coordinate system to the body system,
Figure FDA0004074028160000074
respectively represent t k The position, the speed and the posture quantity of the moment carrier under the world coordinate system>
Figure FDA0004074028160000075
Respectively represent t k+1 Position, speed and posture quantity of the time carrier under the world coordinate system>
Figure FDA0004074028160000076
Respectively represent t k Time and t k+1 The zero offset value of the accelerometer at that moment>
Figure FDA0004074028160000077
Respectively represent t k Time and t k+1 The zero-bias value of the gyroscope at that time,
Figure FDA0004074028160000078
respectively represent t k Time and t k+1 The error coefficient matrix of the time accelerometer, ->
Figure FDA0004074028160000079
Respectively represent t k Time and t k+1 An error coefficient matrix of the moment gyroscope;
the calculation formula of the increment equation of the inertia pre-integration error is as follows:
Figure FDA00040740281600000710
wherein the content of the first and second substances,
Figure FDA00040740281600000711
represents t k Time t k+1 A covariance matrix of the system between the moments, wherein deltaX represents an error state quantity in the optimization process, and comprises an error quantity deltax of relative translation and rotation of the measurement values of the visual image sensor, the GPS receiver and the laser radar and the IMU s Error state quantity deltax provided by measurement values of the vision image sensor, the GPS receiver and the laser radar λ Error state quantities of carrier position, velocity, quaternion, accelerometer and gyroscope zero bias of n +1 measurement frames in the sliding window, and error state quantities of error coefficient matrixes of accelerometer and gyroscope including scale factor error and installation error:
δX＝[δx 0 ,δx 1 ,…,δx n ,δx s ,δx λ ]
Figure FDA00040740281600000712
Figure FDA00040740281600000713
Figure FDA00040740281600000714
representing inertial pre-integration error pick-up>
Figure FDA00040740281600000715
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＝[x 0 ,x 1 ,…,x n ,x s ,x λ ]
wherein x 0 ,x 1 ,…,x n Represents the state variable to be optimized related to the carrier in n +1 measurement frames in the sliding window, x s Represents a state variable to be optimized, x, constructed by the navigation sensor and IMU except the visual image sensor, the GPS receiver, the laser radar and the inertial sensor λ Representing the state variables to be optimized provided by the measurements of the visual image sensor, of the GPS receiver, of the lidar, in particular for x 0 ,x 1 ,…,x n A certain determined state quantity x of k And x s Comprises the following steps:
Figure FDA0004074028160000081
/>
Figure FDA0004074028160000082
x k each 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 \8230n; p is a radical of s b Representing the relative translation of the visual image sensor, the GPS receiver, the lidar, a navigation sensor other than the inertial sensor and the IMU,
Figure FDA0004074028160000083
representing the relative rotation amount of the visual image sensor, the GPS receiver, the laser radar and a navigation sensor except the inertial sensor and the IMU;
inertial pre-integration error
Figure FDA0004074028160000084
The Jacobian matrix which corresponds to the state variable X to be optimized>
Figure FDA0004074028160000085
The calculating method comprises the following steps:
error pre-integration by inertia
Figure FDA0004074028160000086
The expression of (2) shows that the inertia pre-integral error treats the optimized state variable x s And x λ The jacobian matrix of (a) is 0, i.e.:
Figure FDA0004074028160000087
the remaining variables to be optimized, namely the position, speed and attitude information and IMU error information of the carrier
Figure FDA0004074028160000088
Figure FDA0004074028160000089
The method is divided into four groups: />
Figure FDA00040740281600000810
Figure FDA00040740281600000811
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 FDA0004074028160000091
/>
Figure FDA0004074028160000092
Figure FDA0004074028160000101
/>
Figure FDA0004074028160000102
calculating each element in the matrix separately to obtain the numerical expression form of J0, J1, J2, J3:
Figure FDA0004074028160000111
Figure FDA0004074028160000112
/>
Figure FDA0004074028160000113
Figure FDA0004074028160000114
wherein I represents an identity matrix
Figure FDA0004074028160000115
Q = [ xyz λ for quaternion]＝[ωλ]The significance of L and R is left and right [. Degree] L And [ · C] R The left and right operators, respectively, representing the quaternion q may be expressed in particular as:
Figure FDA0004074028160000121
Figure FDA0004074028160000122
and combining error terms provided by navigation sensors except the visual image sensor, the GPS receiver, the laser radar and the inertial sensor to construct an objective function:
Figure FDA0004074028160000123
wherein, | | r p || 2 For 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 FDA0004074028160000124
for inertial pre-integration error, B is the set of all IMU measurements, based on>
Figure FDA0004074028160000125
Covariance as inertial pre-integration error; />
Figure FDA0004074028160000126
Error terms provided for the measurement values of the visual image sensor, the GPS receiver and the laser radar;
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 CN111238535A (en) 2020-06-05
CN111238535B true 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)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112284379B (en) * 2020-09-17 2023-09-22 江苏大学 Inertial pre-integration method of 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
CN113175933B (en) * 2021-04-28 2024-03-12 南京航空航天大学 Factor graph integrated navigation method based on high-precision inertial pre-integration
CN113776543B (en) * 2021-07-27 2023-11-28 武汉中海庭数据技术有限公司 Vehicle fusion positioning method, system, electronic equipment and storage medium
CN115615437B (en) * 2022-09-20 2024-04-30 哈尔滨工程大学 Factor graph integrated navigation method

Citations (3)

* 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

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10687046B2 (en) * 2018-04-05 2020-06-16 Fyusion, Inc. Trajectory smoother for generating multi-view interactive digital media representations

Patent Citations (3)

* 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

Also Published As

Publication number Publication date
CN111238535A (en) 2020-06-05

Similar Documents

Publication Publication Date Title
CN111238535B (en) IMU error online calibration method based on factor graph
CN107655493B (en) SINS six-position system-level calibration method for fiber-optic gyroscope
CN107941217B (en) Robot positioning method, electronic equipment, storage medium and device
CN110631574B (en) inertia/odometer/RTK multi-information fusion method
CN111156997B (en) Vision/inertia combined navigation method based on camera internal parameter online calibration
CN113175933B (en) Factor graph integrated navigation method based on high-precision inertial pre-integration
CN113311411A (en) Laser radar point cloud motion distortion correction method for mobile robot
CN110702113B (en) Method for preprocessing data and calculating attitude of strapdown inertial navigation system based on MEMS sensor
CN108507572B (en) Attitude positioning error correction method based on MEMS inertial measurement unit
CN114216456A (en) Attitude measurement method based on IMU and robot body parameter fusion
CN115046539A (en) Dynamic calibration method for MEMS electronic compass
CN115200578A (en) Polynomial optimization-based inertial-based navigation information fusion method and system
CN113008229B (en) Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor
CN113203429B (en) Online estimation and compensation method for temperature drift error of gyroscope
CN114526731A (en) Inertia combination navigation direction positioning method based on moped
CN113074753A (en) Star sensor and gyroscope combined attitude determination method, combined attitude determination system and application
CN111637892A (en) Mobile robot positioning method based on combination of vision and inertial navigation
CN116929335A (en) Radiation field map construction method based on multisource information perception
CN116338719A (en) Laser radar-inertia-vehicle fusion positioning method based on B spline function
CN115452003A (en) Online estimation method for nonlinear bias of micro-electromechanical gyroscope
CN114184190A (en) Inertial/odometer integrated navigation system and method
CN114397642A (en) Three-dimensional laser radar and IMU external reference calibration method based on graph optimization
Liu et al. Eccentric Optimization of Multi-sensor for SLAM Integrated Navigation
CN112697142A (en) Inertia/wheel speed odometer fusion positioning and parameter optimization method based on pre-integration theory
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