Inertia pre-integration adjustment method for autonomous navigation

Abstract

The invention discloses an inertia pre-integration adjusting method for autonomous navigation, which belongs to the field of autonomous navigation, and comprises the following steps: receiving output data of an inertial sensor, and calculating and measuring time change based on the output data of the inertial sensor; determining a pre-integration adjustment condition based on the measurement time change; constructing a corresponding inertia pre-integration rapid adjustment module based on the pre-integration adjustment condition; the inertia pre-integration quick adjustment module is used for quickly adjusting the inertia pre-integration information by using a Jacobian matrix to obtain the adjusted inertia pre-integration information; and constructing a Jacobian matrix corresponding to the adjusted inertia pre-integration information, and updating an inertia pre-integration factor. According to the method, the consumption of computing resources is greatly reduced, and the real-time performance and the response speed of the system are improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of autonomous navigation, and in particular relates to an inertial pre-integration adjustment method for autonomous navigation. Background Art

[0002] Navigation and positioning technology is a core enabling factor for autonomous driving, drone inspections, industrial robot control, and other fields. Faced with typical application requirements such as dynamic obstacle avoidance in complex urban scenarios, continuous positioning in underground spaces without satellite signals, and accurate capture of human motion, navigation systems must not only overcome the performance limitations of a single sensor but also achieve coordinated optimization of robustness, real-time performance, and accuracy through multi-source information fusion mechanisms.

[0003] Inertial navigation systems calculate the position change of the carrier by integrating the information output by inertial sensors (such as accelerometers and gyroscopes) and achieve continuous positioning through recursion. However, since their output contains errors, which increase rapidly during the integration process, their errors need to be suppressed to meet navigation requirements. Multi-source fusion architectures represented by factor graph optimization have gradually become an important technical path for improving the error suppression capabilities of navigation systems because they can fully exploit sensor observation information at historical moments for global state estimation. Compared with the traditional Kalman filter's recursive processing mode that only considers the optimal value at the previous moment, factor graphs construct nonlinear optimization problems and simultaneously optimize information at multiple historical moments, thereby obtaining more stable pose estimation results in complex interference environments. However, the output of high-frequency inertial devices participates in factor graph optimization through integration, and the inertia at historical moments needs to be re-integrated when estimating the error. This computationally intensive process seriously restricts the system's real-time computing capabilities.

[0004] Existing inertial pre-integration methods can achieve error re-estimation without re-integration by solving the Jacobian matrix of the inertial integral and the error, showing good computational efficiency. However, their mathematical models are usually constructed based on the assumption of a fixed time window. When the measurement interval of the system changes due to factors such as environmental disturbances, communication delays or dynamic scheduling, traditional methods need to repeatedly integrate historical pre-integration data to re-establish the constraint relationship. This repeated calculation not only consumes computing resources, but also causes a rapid increase in computing load when triggered at high frequencies, seriously restricting the system's real-time response capability under complex working conditions such as clock asynchronous. Therefore, it is necessary to propose a fast adjustment method for inertial pre-integration to achieve rapid adjustment of pre-integration parameters under dynamic measurement intervals, reduce computational complexity while maintaining the error suppression effect, and break through the key technical bottleneck of improving the robustness and universality of inertial-based fusion navigation systems. Summary of the Invention

[0005] In order to solve the above technical problems, the present invention proposes an inertial pre-integration adjustment method for autonomous navigation to solve the problems existing in the above-mentioned prior art.

[0006] To achieve the above object, the present invention provides an inertial pre-integration adjustment method for autonomous navigation, comprising:

[0007] receiving inertial sensor output data, and calculating a measurement time change based on the inertial sensor output data;

[0008] determining a pre-integration adjustment condition based on the measured time change;

[0009] Constructing a corresponding inertia pre-integration fast adjustment module based on the pre-integration adjustment situation;

[0010] The inertia pre-integration fast adjustment module uses the Jacobian matrix to quickly adjust the inertia pre-integration information to obtain adjusted inertia pre-integration information;

[0011] Construct the Jacobian matrix corresponding to the adjusted inertia pre-integration information to update the inertia pre-integration factor.

[0012] Optionally, the pre-integration adjustment conditions include: reducing inertia data at the head, increasing inertia data at the head, reducing inertia data at the tail, and increasing inertia data at the tail.

[0013] Optionally, the inertia pre-integration fast adjustment module under the head inertia reduction data includes: a posture pre-integration fast adjustment module, a velocity pre-integration fast adjustment module, a position pre-integration fast adjustment module and a covariance pre-integration fast adjustment module;

[0014] Among them, the expression of the attitude pre-integration fast adjustment module is:

[0015]

[0016] Where R i+n,j The rotation matrix representing the change from time j to time i+n, represents the output of the gyroscope at time k between two zero-speed points, represents the zero bias of the gyroscope at time k, represents the random noise of the gyroscope at time k; Exp represents the mapping of the equivalent rotation vector to the rotation matrix, and Δt represents the inertial sensor output interval;

[0017] The expression of the speed pre-integration fast adjustment module is:

[0018]

[0019] Where Δv i+n,jis the velocity change from time i+n to time j relative to the body coordinate system at time i+n, is the output of the accelerometer at time k between two zero-speed points, is the zero bias of the accelerometer at time k, is the random noise of the accelerometer at time k;

[0020] The expression of the position pre-integration fast adjustment module is:

[0021]

[0022] Where Δp i+n,j is the position change from time i+n to time j relative to the body coordinate system at time i+n;

[0023] The expression of the covariance pre-integration fast adjustment module is:

[0024]

[0025] Where, Σ i+n,j is the covariance from time i+n to time j, represents the transformation matrix that removes the covariance at time i, represents the transformation matrix for removing system noise at time i, Σ η is the system noise matrix.

[0026] Optionally, the process of constructing the Jacobian matrix corresponding to the adjusted inertia pre-integration information includes:

[0027] Based on the Jacobian matrix of the zero bias of the MEMS-IMU relative to the state quantity, the Jacobian matrix of the residual relative to the state quantity is constructed. The Jacobian matrix of the residual relative to the state quantity includes the attitude residual Jacobian matrix, the velocity residual Jacobian matrix, the position residual Jacobian matrix and the zero bias residual Jacobian matrix.

[0028] Optionally, the posture residual Jacobian matrix is:

[0029]

[0030] Where, e R is the rotation matrix residual, R i is the rotation matrix at time i, v i is the speed at time i, p i is the position at time i, is the gyro bias at time i, is the acceleration bias at time i, J r (·) is the attitude right Jacobian update matrix, is the inverse of the attitude right Jacobian update matrix, is the Jacobian matrix of attitude change relative to gyroscope bias in inertial pre-integration.

[0031] Optionally, the velocity residual Jacobian matrix is:

[0032]

[0033] Where, is the Jacobian matrix of the MEMS-IMU gyroscope bias relative to the state, e v represents the velocity residual, Represents the velocity information in the world coordinate system at time i, are the Jacobian matrices of velocity change relative to gyroscope bias and accelerometer bias in inertial pre-integration, respectively.

[0034] Optionally, the position residual Jacobian matrix is:

[0035]

[0036] Where, e p represents the position residual, Represents the rotation matrix from the machine system to the world coordinate system at time i, Indicates the position at time j in the world coordinate system, Δt ij represents the time from time i to time j, Represents the rotation matrix from the machine system to the world coordinate system at time j, They are the Jacobian matrices of the position relative to the gyroscope bias and accelerometer bias in inertial pre-integration.

[0037] Optionally, the zero-bias residual Jacobian matrix is:

[0038]

[0039] Where, e b represents the deviated residual.

[0040] Optionally, updating the inertial pre-integration factor includes updating the residual value based on the change of the zero bias and the Jacobian matrix of the MEMS-IMU zero bias relative to the state quantity.

[0041] Compared with the prior art, the present invention has the following advantages and technical effects:

[0042] The inertial pre-integration adjustment method for autonomous navigation of the present invention has significant technical effects. By receiving the output data of the inertial sensor and calculating the measurement time change, the pre-integration adjustment situation can be accurately determined, thereby constructing an efficient inertial pre-integration fast adjustment module. This module uses the Jacobian matrix to achieve rapid adjustment, effectively improving the update efficiency of the inertial pre-integration information. At the same time, by constructing the Jacobian matrix corresponding to the adjusted inertial pre-integration information, the update process of the inertial pre-integration factor is further optimized, and the stability and reliability of the system are enhanced. This method greatly reduces the consumption of computing resources, improves the real-time performance and response speed of the system, and is particularly suitable for inertial navigation systems with high requirements for accuracy and efficiency, and provides strong support for high-precision positioning and navigation in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] The accompanying drawings, which constitute part of this application, are intended to provide a further understanding of this application. The exemplary embodiments and descriptions of this application are intended to explain this application and do not constitute an improper limitation on this application. In the accompanying drawings:

[0044] Figure 1 This is a graph showing the time consumption for integral adjustment under a single measurement change according to an embodiment of the present invention;

[0045] Figure 2 A graph showing the time consumption for integral adjustment under multiple measurement changes according to an embodiment of the present invention;

[0046] Figure 3 Flowchart of a method according to an embodiment of the present invention. DETAILED DESCRIPTION

[0047] It should be noted that, in the absence of conflict, the embodiments and features of the embodiments in this application can be combined with each other. The present application will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0048] It should be noted that the steps shown in the flowcharts of the accompanying drawings can be executed in a computer system such as a set of computer-executable instructions, and that, although a logical order is shown in the flowcharts, in some cases, the steps shown or described can be executed in an order different from that shown here.

[0049] Example 1

[0050] In robot state estimation, inertial pre-integration can achieve rapid fusion of inertial data and sensor measurement data with different sampling periods. Figure 3 As shown, this embodiment provides an efficient adjustment method when the measurement time changes lead to a reduction in the inertial pre-integration head data. The present invention constructs a fast calculation method for the state changes and the corresponding Jacobian matrix when the pre-integration head and tail increase or decrease, and obtains the adjusted inertial pre-integration information.

[0051] The present invention constructs an inertial pre-integration Jacobian matrix, including inertial pre-integration attitude, velocity, and position adjustment Jacobian matrices, as well as covariance adjustment Jacobian matrices. By rapidly stripping inertial data from the Jacobian matrix or adding it to the existing inertial pre-integration, the disclosed method can significantly improve the real-time performance of inertial-based integrated navigation under varying measurement delays.

[0052] This example provides an efficient adjustment method for the case where the inertial pre-integration header data is reduced due to measurement time changes, which specifically includes the following steps:

[0053] Step 1. In the constructed inertial pre-integration, the current measurement time changes from moment i to moment i. c At the moment, calculate the amount of inertial measurement n that the head reduces;

[0054]

[0055] Where Δt is the output interval of the inertial sensor.

[0056] Step 2. The change of inertial pre-integration when the head reduces n inertial measurement data is as follows;

[0057] Since inertial integration involves multiple coordinate systems, the coordinate system used must be defined first. The outputs of the gyroscope and accelerometer are based on the IMU coordinate system, which is recorded as b system. The three axes of the IMU coordinate system point to the right, front, and top respectively, and the IMU coordinate system at time t is recorded as b t The navigation results of the carrier are expressed in the world coordinate system, which is recorded as the w system. The origin of the world coordinate system coincides with the IMU coordinate system at the initial moment, and its three axes point to the east, north, and sky respectively.

[0058] Since the interval between zero-speed points is short, the error characteristic of the inertial sensor changes little in a short time, so the error between two consecutive sampling time points t i With t j The zero bias of the MEMS-IMU between the two zero-speed points can be regarded as a constant. The output of the gyroscope and accelerometer at time k between the two zero-speed points and as follows:

[0059]

[0060] in, and They represent the actual angular velocity of the machine system and the actual acceleration of the world coordinate system at time k respectively. represents the rotation from the world coordinate system to the body coordinate system at time k. and represents the zero bias of the gyroscope and accelerometer at time k. and represents the random noise of the gyroscope and accelerometer at time k.

[0061] Pre-integration quick adjustment includes four situations: reducing inertia data at the head, increasing key data at the head, reducing inertia data at the tail, and increasing inertia data at the tail. The specific situations are as follows:

[0062] In the traditional pre-integration of inertia, it is necessary to re-perform pre-integration based on the reduced inertia data. The reduction of the pre-integration head is as follows:

[0063]

[0064] Where Exp represents the mapping of the equivalent rotation vector to the rotation matrix. For ease of understanding, the state quantity without a superscript represents the body coordinate system at the moment before the subscript, j is the corresponding moment of the next measurement, ΔR i+n,j is the rotation matrix from time i+n to time j relative to the body coordinate system at time i+n, where R in the present invention is ·，* The parameters of the format represent the rotation matrix of the change from time * to time ·, ΔR ·，* The parameters of the format represent the rotation matrix from time · to time * relative to the body coordinate system at time ·, Δv i+n,j is the velocity change from time i+n to time j relative to the body coordinate system at time i+n, where Δv ·，* The parameter in the format represents the velocity change from time · to time * relative to the body coordinate system at time ·, Δp i+n,j is the position change from time i+n to time j relative to the body coordinate system at time i+n, where Δp ·，* The parameter of the format represents the position change from time · to time * relative to the body coordinate system at time ·, Σ i+n,j A is the covariance from time i+n to time j, which can be regarded as a confidence reference. j-1 is the state transition matrix, B j-1 is the noise transfer matrix, which is as follows:

[0065]

[0066] The increase in the pre-integration head is as follows:

[0067]

[0068] The increase in the pre-integration tail is as follows:

[0069]

[0070] The reduction of the pre-integration tail is as follows:

[0071]

[0072] Step 3. Construct an inertia pre-integration fast adjustment module when the head reduces n inertial measurement data, including an attitude pre-integration fast adjustment module, a velocity pre-integration fast adjustment module, a position pre-integration fast adjustment module, and a covariance pre-integration fast adjustment module;

[0073] The module for rapid posture adjustment in the case of pre-integrated head reduction is as follows:

[0074]

[0075] Where R i+n,j The rotation matrix representing the change from time j to time i+n, represents the output of the gyroscope at time k between two zero-speed points, represents the zero bias of the gyroscope at time k, represents the random noise of the gyroscope at time k; Exp represents the mapping of the equivalent rotation vector to the rotation matrix, and Δt represents the inertial sensor output interval.

[0076] The module for rapid speed adjustment in the case of pre-integration head reduction is as follows:

[0077]

[0078] Where Δv i+n,j is the velocity change from time i+n to time j relative to the body coordinate system at time i+n, is the output of the accelerometer at time k between two zero-speed points, is the zero bias of the accelerometer at time k, is the random noise of the accelerometer at time k.

[0079] The module for rapid position adjustment in the case of pre-integration head reduction is as follows:

[0080]

[0081] Where Δp i+n,j is the position change from time i+n to time j relative to the body coordinate system at time i+n.

[0082] The covariance fast adjustment module in the case of pre-integration head reduction is as follows:

[0083]

[0084] Where, Σ i+n,j is the covariance from time i+n to time j, represents the transformation matrix that removes the covariance at time i, represents the transformation matrix for removing system noise at time i, Σ η is the system noise matrix.

[0085] in, Represents the transformation matrix that removes the covariance at time i, with the goal of transforming the coordinate system within the covariance. Represents the transformation matrix for removing system noise at time i, as follows:

[0086]

[0087] Among them, since the zero bias estimates of the previous and next main events may vary, it is necessary to estimate based on the zero bias that constitutes this pre-integration when adjusting the head data. Represents zero bias The state of represents the right Jacobian update matrix of the posture at time k.

[0088] Then the adjusted Jacobian matrix is ​​calculated to reconstruct the inertia pre-integration module.

[0089] Step 4. Construct the Jacobian matrix corresponding to the inertial pre-integration residual when the head reduces n inertial measurement data, including the Jacobian matrix of attitude, velocity, position and zero bias residual;

[0090] For the inertial pre-integration based on MEMS-IMU, assuming that the zero bias remains unchanged between the two main events, the residual E imuij It can be expressed as:

[0091]

[0092] in, Indicates that when the zero bias is equal to The change of state. a and δb g are the differences between the bias in the current iteration and the initial value of the optimization process. is the Jacobian matrix of the MEMS-IMU zero bias relative to the state, which can be used to efficiently adjust the pre-integration changes caused by the zero bias, as shown in Equation (14).

[0093]

[0094] Based on the Jacobian matrix of the state quantity relative to the MEMS-IMU error, the Jacobian matrix of the residual relative to the state quantity can be constructed, which is specifically listed according to attitude, velocity, position, and error. The attitude is as shown in formula (15):

[0095]

[0096] Where, e R is the rotation matrix residual, R i is the rotation matrix at time i, v i is the speed at time i, p i is the position at time i, is the gyro bias at time i, is the acceleration bias at time i, J r (·) is the attitude right Jacobian update matrix, is the inverse of the attitude right Jacobian update matrix, is the Jacobian matrix of attitude change relative to gyroscope bias in inertial pre-integration.

[0097] The speed is as shown in formula (16):

[0098]

[0099] Where, is the Jacobian matrix of the MEMS-IMU gyroscope bias relative to the state, e v represents the velocity residual, Represents the velocity information in the world coordinate system at time i, are the Jacobian matrices of velocity change relative to gyroscope bias and accelerometer bias in inertial pre-integration, respectively.

[0100] The position is as shown in formula (17):

[0101]

[0102] Where, e p represents the position residual, Represents the rotation matrix from the machine system to the world coordinate system at time i, Indicates the position at time j in the world coordinate system, Δt ij represents the time from time i to time j, Represents the rotation matrix from the machine system to the world coordinate system at time j, They are the Jacobian matrices of the position relative to the gyroscope bias and accelerometer bias in inertial pre-integration.

[0103] The zero bias is as shown in formula (18):

[0104]

[0105] Where, e b represents the deviated residual.

[0106] After reducing n inertial data from the original pre-integration head, the residual is as shown in formula (19):

[0107]

[0108] From the Jacobian matrix of the residual to the state, it can be seen that except for the Jacobian matrix of the MEMS-IMU zero bias relative to the state, which needs to be obtained recursively, the remaining elements can be adjusted through linear operation graphs with low complexity. This section constructs an efficient method for adjusting the Jacobian matrix of the MEMS-IMU zero bias relative to the state to further reduce the amount of calculation, as shown in (20):

[0109]

[0110]

[0111] By substituting the adjusted matrix into the original Jacobian matrix, the Jacobian matrix after reducing n inertial data from the original pre-integrated head can be quickly calculated.

[0112] Through the above adjustment method, the state quantity, residual, and Jacobian matrix can be updated after the original pre-integration head reduces n inertial data, thereby realizing the update of the inertial pre-integration factor.

[0113] The final experimental results are as follows Figure 1 、 Figure 2 shown.

[0114] A single measurement experiment compared the re-integration time required for 10 inertial measurement changes, 0.05 seconds after the last zero-velocity measurement. The time required to adjust for a single inertial measurement change is shown in the figure. Both pre-integration and the EKF require approximately 2.30ms. The proposed method only takes 0.36ms to 1.88ms for an increase in the number of IMU measurement changes from 1 to 9.

[0115] Multiple measurement experiments compared the time required for re-integration when the measurement changes every ten steps, ranging from 1 to 9. Because factor graph optimization fusion uses sliding window optimization techniques, this situation is more common in practice. As shown in the figure, the traditional EKF-based method incurs significant time consumption, ranging from 2.25ms to 69.72ms, the pre-integration method consumes time from 2.27ms to 15.50ms, and the proposed method consumes only 0.75ms to 6.74ms.

[0116] Comparing the adjustment time under two different measurement variations reveals that the proposed algorithm reduces computational effort by 70% compared to traditional algorithms. Factor graph optimization fusion requires dozens of iterations, each subject to the potential for measurement variation. On high-performance embedded systems like the Raspberry Pi, this can save 2-10ms per iteration, and a single optimization can reduce computational time by up to hundreds of milliseconds. On more common embedded systems like the STM32, this can potentially shave off several seconds of computation time, a crucial factor in ensuring real-time performance.

[0117] In addition, more accurate measurement construction will enable more precise zero-bias estimation, further improving positioning estimation accuracy. Through the efficient pre-integration adjustment method proposed in this invention, a factor graph optimization fusion framework for measurement closed-loop optimization can be constructed to achieve higher-precision state estimation.

[0118] This invention discloses an inertial pre-integration adjustment method for autonomous navigation. This method constructs an inertial pre-integration Jacobian matrix, including adjustments to the Jacobian matrix for attitude, velocity, and position, as well as a covariance adjustment Jacobian matrix. By rapidly stripping inertial data from the Jacobian matrix or adding it to the existing inertial pre-integration, the real-time performance of inertial-based integrated navigation is improved under varying measurement delays. A factor graph optimization fusion framework for closed-loop measurement optimization can be further constructed to achieve even higher-precision state estimation.

[0119] The above are merely preferred embodiments of the present application, but the scope of protection of the present application is not limited thereto. Any changes or substitutions that can be easily conceived by a person skilled in the art within the technical scope disclosed in this application should be included in the scope of protection of the present application. Therefore, the scope of protection of the present application should be based on the scope of protection of the claims.

Claims

1. A method for adjusting inertial pre-integration for autonomous navigation, characterized in that: The following steps are involved: receiving inertial sensor output data, and calculating a measurement time change based on the inertial sensor output data; determining a pre-integration adjustment condition based on the measured time change; Constructing a corresponding inertia pre-integration fast adjustment module based on the pre-integration adjustment situation; The inertia pre-integration fast adjustment module uses the Jacobian matrix to quickly adjust the inertia pre-integration information to obtain adjusted inertia pre-integration information; Construct the Jacobian matrix corresponding to the adjusted inertia pre-integration information to update the inertia pre-integration factor.

2. The inertial pre-integration adjustment method for autonomous navigation according to claim 1, characterized in that: The pre-integration adjustment conditions include: reducing inertia data at the head, increasing inertia data at the head, reducing inertia data at the tail, and increasing inertia data at the tail.

3. The inertial pre-integration adjustment method for autonomous navigation according to claim 1, characterized in that: The inertia pre-integration fast adjustment module under the head-reduced inertia data includes: attitude pre-integration fast adjustment module, velocity pre-integration fast adjustment module, position pre-integration fast adjustment module and covariance pre-integration fast adjustment module; Among them, the expression of the attitude pre-integration fast adjustment module is: Where R i+n,j The rotation matrix representing the change from time j to time i+n, represents the output of the gyroscope at time k between two zero-speed points, represents the zero bias of the gyroscope at time k, represents the random noise of the gyroscope at time k; Exp represents the mapping of the equivalent rotation vector to the rotation matrix, and Δt represents the inertial sensor output interval; The expression of the speed pre-integration fast adjustment module is: Where Δv i+n,j is the velocity change from time i+n to time j relative to the body coordinate system at time i+n, is the output of the accelerometer at time k between two zero-speed points, is the zero bias of the accelerometer at time k, is the random noise of the accelerometer at time k; The expression of the position pre-integration fast adjustment module is: Where Δp i+n,j is the position change from time i+n to time j relative to the body coordinate system at time i+n; The expression of the covariance pre-integration fast adjustment module is: Where, Σ i+n,j is the covariance from time i+n to time j, represents the transformation matrix that removes the covariance at time i, represents the transformation matrix for removing system noise at time i, Σ η is the system noise matrix.

4. The inertial pre-integration adjustment method for autonomous navigation according to claim 3, characterized in that: The process of constructing the Jacobian matrix corresponding to the adjusted inertia pre-integration information includes: Based on the Jacobian matrix of the zero bias of the MEMS-IMU relative to the state quantity, the Jacobian matrix of the residual relative to the state quantity is constructed. The Jacobian matrix of the residual relative to the state quantity includes the attitude residual Jacobian matrix, the velocity residual Jacobian matrix, the position residual Jacobian matrix and the zero bias residual Jacobian matrix.

5. The inertial pre-integration adjustment method for autonomous navigation according to claim 4, characterized in that: The attitude residual Jacobian matrix is: Where, e R is the rotation matrix residual, R i is the rotation matrix at time i, v i is the speed at time i, p i is the position at time i, is the gyro bias at time i, is the acceleration bias at time i, J r (·) is the attitude right Jacobian update matrix, is the inverse of the attitude right Jacobian update matrix, is the Jacobian matrix of attitude change relative to gyroscope bias in inertial pre-integration.

6. The inertial pre-integration adjustment method for autonomous navigation according to claim 5, characterized in that: The velocity residual Jacobian matrix is: Where, is the Jacobian matrix of the MEMS-IMU gyroscope bias relative to the state, e v represents the velocity residual, Represents the velocity information in the world coordinate system at time i, are the Jacobian matrices of velocity change relative to gyroscope bias and accelerometer bias in inertial pre-integration, respectively.

7. The inertial pre-integration adjustment method for autonomous navigation according to claim 6, characterized in that: The position residual Jacobian matrix is: Where, e p represents the position residual, Represents the rotation matrix from the machine system to the world coordinate system at time i, Indicates the position at time j in the world coordinate system, Δt ij represents the time from time i to time j, Represents the rotation matrix from the machine system to the world coordinate system at time j, They are the Jacobian matrices of the position relative to the gyroscope bias and accelerometer bias in inertial pre-integration.

8. The inertial pre-integration adjustment method for autonomous navigation according to claim 7, characterized in that: The zero-bias residual Jacobian matrix is: Where, e b represents the deviated residual.

9. The inertial pre-integration adjustment method for autonomous navigation according to claim 1, characterized in that: The updating of the inertial pre-integration factor includes updating the residual value based on the change of the zero bias and the Jacobian matrix of the MEMS-IMU zero bias relative to the state quantity.

Images