CN111238535A - IMU error online calibration method based on factor graph - Google Patents
IMU error online calibration method based on factor graph Download PDFInfo
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 52
- 238000005457 optimization Methods 0.000 claims abstract description 14
- 239000011159 matrix material Substances 0.000 claims description 102
- 230000010354 integration Effects 0.000 claims description 74
- 238000005259 measurement Methods 0.000 claims description 33
- 230000001133 acceleration Effects 0.000 claims description 21
- 230000007704 transition Effects 0.000 claims description 21
- 238000005070 sampling Methods 0.000 claims description 18
- 238000004364 calculation method Methods 0.000 claims description 17
- 238000012546 transfer Methods 0.000 claims description 14
- 238000009434 installation Methods 0.000 claims description 13
- 230000000007 visual effect Effects 0.000 claims description 11
- 238000004094 preconcentration Methods 0.000 claims description 9
- 230000008569 process Effects 0.000 claims description 7
- 238000013519 translation Methods 0.000 claims description 6
- 239000004576 sand Substances 0.000 claims description 3
- 230000009897 systematic effect Effects 0.000 claims description 3
- 238000006243 chemical reaction Methods 0.000 claims description 2
- 230000008859 change Effects 0.000 abstract description 3
- 230000009467 reduction Effects 0.000 abstract description 2
- 230000002411 adverse Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 238000009825 accumulation Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 239000012855 volatile organic compound Substances 0.000 description 1
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
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:
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,showing the mounting error of the accelerometer in the X-axis and the Y-axis,showing the mounting error of the accelerometer in the X-axis and Z-axis,showing the installation error of the Y axis and the Z axis of the accelerometer;showing the mounting error of the gyroscope X-axis and Y-axis,showing the mounting error of the gyroscope X-axis and Z-axis,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:
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:
wherein, bkDenotes the initial tkA coordinate system of the body at a moment,respectively representing the sampling time t of two adjacent inertia frames i and i +1iAnd ti+1The measured value of the accelerometer is measured,respectively represent tiAnd ti+1The measurement value of the gyroscope at the moment, δ t represents the sampling period of the inertial sensor;respectively represent ti+1The time, the position pre-integration value, the speed pre-integration value and the angle pre-integration value of the carrier,respectively represent tiTime, position preconcentration value, velocity preconcentration value and angle preconcentration value of carrier, initial timeAndis 0, since gamma is a rotation expressed by a quaternion,is a unit quaternion; r (y) denotes the conversion of quaternions into rotation matrices,i.e. representing t in terms of quaternioniThe moment carrier angle pre-integration value is converted into a rotation matrix,i.e. representing t in terms of quaternioni+1Converting the carrier angle pre-integration value into a rotation matrix at the moment;respectively represent tiThe acceleration at that moment is offset from zero of the gyroscope,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;are respectively shown at tiA matrix of the acceleration at the moment and the error coefficient of the gyroscope, 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:
Wherein,representing t by quaternioniThe moment carrier angle pre-integration value is converted into a rotation matrix,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:
wherein,are respectively shown at tiA matrix of the acceleration at the moment and the error coefficient of the gyroscope,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
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;
Wherein, N is the noise,represents tiThe position of the carrier at the moment pre-integrates the error state,represents tiTime of dayThe angular pre-integration error state of the carrier,represents tiThe speed of the carrier at the moment pre-integrates the error state,respectively represent tiZero offset error states of the accelerometer and gyroscope of the time carrier,respectively represent tiError state of error coefficient matrix of the carrier at moment;represents tiWhite noise from the gyroscope and accelerometer at the moment,represents ti+1White noise from the gyroscope and accelerometer at the moment,it is indicated that the accelerometer has zero-bias white noise,representing white zero bias noise of the gyroscope;a Jacobian matrix of system states at an initial time, andq is a covariance matrix of noise N, with an initial time
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 compensatedThe calculation formula of (a) is as follows:
wherein,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,respectively represent tkError states of error coefficient matrixes of an accelerometer and a gyroscope of a time carrier,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:
wherein,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
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;
therefore, the inertia pre-integral error calculation formula is as follows:
wherein, the attitude of the carrier is expressed by quaternion,representing the imaginary part of the extracted quaternion,the angular pre-integration value of the carrier after the error is compensated,is tkA rotation matrix from the time world coordinate system to the body system,respectively represent tkThe position, the speed and the attitude of the time carrier under a world coordinate system,respectively represent tk+1The position, the speed and the attitude of the time carrier under a world coordinate system,respectively represent tkTime and tk+1The zero offset value of the accelerometer at the moment,respectively represent tkTime and tk+1The zero-bias value of the gyroscope at that time,respectively represent tkTime and tk+1The matrix of error coefficients of the accelerometer at the moment,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:
wherein,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λ]
representing inertial pre-integration errorAnd (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:
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;indicating the relative translation of the other navigation sensors and the IMU,indicating the amount of relative rotation of the other navigation sensors with the IMU;
inertial pre-integration errorJacobian matrix of state variables X to be optimizedThe calculating method comprises the following steps:
error pre-integration by inertiaThe 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.:
the remaining variables to be optimized, namely the position, speed and attitude information and IMU error information of the carrier The method is divided into four groups:
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:
j2, J3 are calculated as follows:
calculating each element in the matrix separately to obtain the numerical expression form of J0, J1, J2, J3:
wherein I represents an identity matrix
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:
and combining the error terms provided by the other navigation sensors to construct an objective function:
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;for inertial pre-integration error, B is the set of all IMU measurements,is the covariance of the inertial pre-integration error;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
Step two: accelerometer measurementsAnd gyroscope measurementsAnd performing pre-integration propagation on the IMU.
1) The error coefficient matrix of the accelerometer and the gyroscope is calculated as follows:
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,showing the mounting error of the accelerometer in the X-axis and the Y-axis,showing the mounting error of the accelerometer in the X-axis and Z-axis,showing the installation error of the Y axis and the Z axis of the accelerometer;showing the mounting error of the gyroscope X-axis and Y-axis,showing the mounting error of the gyroscope X-axis and Z-axis,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:
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:
wherein, bkDenotes the initial tkA coordinate system of the body at a moment,respectively representing the sampling time t of two adjacent inertia frames i and i +1iAnd ti+1The time-of-flight data of the accelerometer,respectively represent tiAnd ti+1Data of the gyroscope at the moment, wherein deltat represents the sampling period of the inertial sensor; 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,respectively represent tiThe time, the position pre-integration value, the speed pre-integration value and the angle pre-integration value of the carrier,is a unit quaternion; gamma is the rotation expressed in quaternions, R (gamma) expression converts quaternions into a rotation matrix,representing t by quaternioniThe moment carrier angle pre-integration value is converted into a rotation matrix,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,andis a non-volatile organic compound (I) with a value of 0,respectively represent tiThe acceleration at that moment is offset from zero of the gyroscope,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;are respectively shown at tiA matrix of the acceleration at the moment and the error coefficient of the gyroscope,are respectively shown at ti+1And (3) a matrix of the acceleration at the moment and the error coefficient of the gyroscope. (ii) aAnd the interval between two adjacent IMU frames is kept unchanged.
Step three: utilizing inertial sensor dataAndan 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:
wherein,representing t by quaternioniThe moment carrier angle pre-integration value is converted into a rotation matrix,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:
wherein,are respectively shown at tiA matrix of the acceleration at the moment and the error coefficient of the gyroscope,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
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;
Wherein, N is the noise,represents tiThe position of the carrier at the moment pre-integrates the error state,represents tiThe speed of the carrier at the moment pre-integrates the error state,represents tiThe angular pre-integration error state of the time carrier,Respectively represent tiZero offset error states of the accelerometer and gyroscope of the time carrier,respectively represent tiError state of error coefficient matrix of the carrier at moment;represents tiWhite noise from the gyroscope and accelerometer at the moment,represents ti+1White noise from the gyroscope and accelerometer at the moment,it is indicated that the accelerometer has zero-bias white noise,representing white zero bias noise of the gyroscope;a Jacobian matrix of system states at an initial time, andq 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 The calculation is performed as follows:
wherein,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.Respectively represent tkError states of error coefficient matrixes of an accelerometer and a gyroscope of a time carrier,respectively represent tkZero offset error states of an accelerometer and a gyroscope of the time carrier.
Each jacobian matrix is calculated as follows:
wherein,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:
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;
thus, the inertial pre-integration error is calculated as follows:
wherein, the attitude of the carrier is expressed by quaternion,representing the imaginary part of the extracted quaternion,the angular pre-integration value of the carrier after the error is compensated,is tkA rotation matrix from the time world coordinate system to the body system,respectively represent tkThe position, the speed and the attitude of the time carrier under a world coordinate system,respectively represent tk+1The position, the speed and the attitude of the time carrier under a world coordinate system,respectively represent tkTime and tk+1The zero offset value of the accelerometer at the moment,respectively represent tkTime and tk+1The zero-bias value of the gyroscope at that time,respectively represent tkTime and tk+1The matrix of error coefficients of the accelerometer at the moment,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:
wherein,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]
representing inertial pre-integration errorA 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]
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 errorJacobian matrix of state variables X to be optimizedThe calculation is performed as follows:
error pre-integration by inertiaCan know the relative state variable x to be optimizedsAnd xλThe jacobian matrix of (a) is 0, i.e.:
remaining variables to be optimized The method is divided into four groups: 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):
j2, J3 are calculated as follows:
calculating each element in the matrix separately to obtain the numerical expression form of J0, J1, J2, J3:
wherein I represents an identity matrix
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:
4) and (3) combining the reprojection error provided by the visual navigation sensor to construct an objective function as follows:
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;for inertial pre-integration error, B is the set of all IMU measurements,is the covariance of the inertial pre-integration error;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:
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,showing the mounting error of the accelerometer in the X-axis and the Y-axis,showing the mounting error of the accelerometer in the X-axis and Z-axis,showing the installation error of the Y axis and the Z axis of the accelerometer;showing the mounting error of the gyroscope X-axis and Y-axis,showing the mounting error of the gyroscope X-axis and Z-axis,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:
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:
wherein, bkDenotes the initial tkA coordinate system of the body at a moment,respectively representing the sampling time t of two adjacent inertia frames i and i +1iAnd ti+1The measured value of the accelerometer is measured,respectively represent tiAnd ti+1The measurement value of the gyroscope at the moment, δ t represents the sampling period of the inertial sensor;respectively represent ti+1The time, the position pre-integration value, the speed pre-integration value and the angle pre-integration value of the carrier,respectively represent tiThe time, the position pre-integration value, the speed pre-integration value and the angle pre-integration value of the carrier,is a unit quaternion; r (y) denotes the conversion of quaternions into rotation matrices,i.e. representing t in terms of quaternioniThe moment carrier angle pre-integration value is converted into a rotation matrix,i.e. representing t in terms of quaternioni+1Converting the carrier angle pre-integration value into a rotation matrix at the moment;respectively represent tiThe acceleration at that moment is offset from zero of the gyroscope,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;are respectively shown at tiA matrix of the acceleration at the moment and the error coefficient of the gyroscope,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:
Wherein,representing t by quaternioniThe moment carrier angle pre-integration value is converted into a rotation matrix,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:
wherein,are respectively shown at tiA matrix of the acceleration at the moment and the error coefficient of the gyroscope,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
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;
Wherein, N is the noise,represents tiThe position of the carrier at the moment pre-integrates the error state,represents tiThe angular pre-integration error state of the carrier at the moment,represents tiThe speed of the carrier at the moment pre-integrates the error state,respectively represent tiZero offset error states of the accelerometer and gyroscope of the time carrier,respectively represent tiError state of error coefficient matrix of the carrier at moment;represents tiWhite noise from the gyroscope and accelerometer at the moment,represents ti+1White noise from the gyroscope and accelerometer at the moment,it is indicated that the accelerometer has zero-bias white noise,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 compensatedThe calculation formula of (a) is as follows:
wherein,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,respectively represent tkError states of error coefficient matrixes of an accelerometer and a gyroscope of a time carrier,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:
wherein,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
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;
therefore, the inertia pre-integral error calculation formula is as follows:
wherein, the attitude of the carrier is expressed by quaternion,representing the imaginary part of the extracted quaternion,the angular pre-integration value of the carrier after the error is compensated,is tkA rotation matrix from the time world coordinate system to the body system,respectively represent tkThe position, the speed and the attitude of the time carrier under a world coordinate system,respectively represent tk+1The position, the speed and the attitude of the time carrier under a world coordinate system,respectively represent tkTime and tk+1The zero offset value of the accelerometer at the moment,respectively represent tkTime and tk+1The zero-bias value of the gyroscope at that time,respectively represent tkTime and tk+1The matrix of error coefficients of the accelerometer at the moment,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:
wherein,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λ]
representing inertial pre-integration errorAnd (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:
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;indicating the relative translation of the other navigation sensors and the IMU,indicating the amount of relative rotation of the other navigation sensors with the IMU;
inertial pre-integration errorJacobian matrix of state variables X to be optimizedThe calculating method comprises the following steps:
error pre-integration by inertiaThe 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.:
the remaining variables to be optimized, namely the position, speed and attitude information and IMU error information of the carrier The method is divided into four groups:
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:
calculating each element in the matrix separately to obtain the numerical expression form of J0, J1, J2, J3:
wherein I represents an identity matrix
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:
and combining the error terms provided by the other navigation sensors to construct an objective function:
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;for inertial pre-integration error, B is the set of all IMU measurements,is the covariance of the inertial pre-integration error;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.
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)
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)
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 |
-
2020
- 2020-02-03 CN CN202010078321.9A patent/CN111238535B/en active Active
Patent Citations (4)
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)
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 |