A factor graph optimization combined navigation method

Abstract

The present invention discloses a factor graph optimization combined navigation method, which uses the ISAM2 optimization algorithm to incrementally solve the state variables in the factor graph in the flow space, and compensates for the earth's rotation and harmful acceleration based on the current state and the pre-integration increment. Then, as the system operation time increases, the factor graph optimization scale gradually expands, and the ISMA2 algorithm is used to repeatedly re-compensate the historically affected moments. When a certain number of times is reached, the historical nodes tend to be stable, and the ISAM2 algorithm no longer calls the compensation algorithm to re-compensate them. The present invention uses a high-precision IMU pre-integration algorithm in the flow space, and its positioning accuracy is improved compared to the traditional pre-integration algorithm. At the same time, due to the factor graph optimization method, it has strong scalability and can realize plug-and-play of multiple sensors. In addition, the algorithm using the ISAM2 has a small amount of calculation and is highly practical.

Description

Technical Field

[0001] The present invention belongs to the technical field of integrated navigation and relates to a factor graph optimization integrated navigation method, in particular to a factor graph optimization integrated navigation method that integrates ISAM2 (Incremental Smoothing and Mapping Using the Bayes Tree) with high-precision inertial measurement unit (IMU) pre-integration in manifold space. Background Art

[0002] With the rapid development of navigation technology in recent years, the variety of sensors used in integrated navigation has gradually increased. Traditional Kalman filtering-based methods have limited applicability in some scenarios. Furthermore, filtering-based techniques do not fully utilize existing data and are susceptible to data contamination during filter fusion, hindering further accuracy improvements. The emergence of factor graph optimization algorithms effectively addresses these issues, further improving the positioning accuracy of integrated navigation. However, the high frequency of IMU measurements makes real-time computation difficult for factor graph optimization algorithms. To address this high frequency, international scholar T. Lupton first proposed an IMU pre-integration algorithm in 2012. However, current factor graph optimization-based algorithms primarily utilize low-precision MEMS (Microelectro Mechanical Systems) sensors, and the pre-integration algorithm employed during fusion simplifies some aspects to account for the low precision of IMUs. High-precision IMUs (typically navigation-grade or higher) are often pre-integrated without detailed processing. Consequently, factor graph optimization algorithms haven't been developed to fully leverage the precision of high-precision IMUs when combined with them. This results in the use of traditional low-precision IMU pre-integration factor graph optimization algorithms for high-precision IMU measurements. Consequently, factor graph optimization algorithms fail to achieve the expected navigation accuracy when combining high-precision IMUs with other sensors. However, some specific scenarios, such as fully enclosed underground coal mines, rely heavily on high-precision IMUs due to limitations in other sensors. Therefore, a pre-integration algorithm suitable for high-precision IMUs is urgently needed.

[0003] Traditional factor graph optimization algorithms use a pre-integration algorithm to accumulate IMU output data between two adjacent optimization moments. This primarily aims to obtain relative motion constraints between these two moments. This avoids the need to re-arrange the IMU mechanically to recursively recurse to the current state during subsequent optimizations, thus avoiding extensive recursive calculations. However, traditional IMU pre-integration algorithms ignore the Earth's angular rate, the navigation system rotation caused by the carrier's motion, and the position-dependent acceleration of gravity, and assume a flat Earth. This results in overly coarse constraints between the two moments, leading to suboptimal positioning accuracy. Subsequent optimizations can introduce variations in the past, but the past states are not further compensated. Furthermore, factor graph optimization uses only the current moment for compensation, resulting in significant differences between the current state and the results of multiple optimizations. This applies when there are no outliers. When outliers are present, the results of the current optimization result contain undetermined errors. In reality, using the current optimization result is always unreliable, and compensating for harmful acceleration and gravity using the current state is crude. Compensating for state variables with large errors will not improve the estimated state accuracy, and may even reduce it. Since the high-precision positioning results of factor graph optimization are produced after multiple optimizations, compensation at all moments in factor graph optimization is a key factor in determining whether the high-precision IMU pre-integration algorithm can further improve the overall optimization accuracy in factor graph optimization. Summary of the Invention

[0004] In view of the above-mentioned prior art, the technical problem to be solved by the present invention is to provide a factor graph optimization combined navigation method that integrates ISAM2 (Incremental Smoothing and Mapping Using the Bayes Tree) with the pre-integration of a high-precision inertial measurement unit (IMU) in manifold space, so as to give full play to the characteristics of the high-precision IMU, improve the calculation accuracy, reduce the amount of calculation, and expand the scope of use of the high-precision IMU in factor graph optimization.

[0005] To solve the above technical problems, the present invention provides a factor graph optimization combined navigation method, comprising the following steps:

[0006] Step 1: Determine whether the code is executing the first loop. If so, obtain and save the k-time data output by the IMU and and k-1 moment data and And initialize the system; otherwise, obtain and save the k-time data output by the current k-time IMU and in are the angular increment and velocity increment of the carrier relative to the inertial system measured by the IMU at time k in the carrier coordinate system b; the starting time of pre-integration is defined as time i, and 0≤i≤k-1;

[0007] Step 2: Use the zero bias of the gyroscope and accelerometer at time i to align and Make the correction and then increase the speed after correction Compensate for the rotation error of the velocity at time k-1 in the b frame The rowing error compensation at time k-1 under the b system of the previous cycle and the single sample Finally, we get the result of k-1 moment in the compensated b system. The angle increment after correction Perform single sample addition and coning error compensation of the previous cycle to obtain the angle increment after coning motion compensation at time k-1

[0008] Step 3: and Divide by the sampling time interval of the IMU to obtain the average acceleration a at time k-1 and time k under the load system b(k) and the average angular velocity w b(k) , and then calculate the angle pre-integration increment φ from time k-1 to time k k-1,k , position pre-integration increment Velocity pre-integration increment Then, the angle pre-integrated increment from time i to time k is obtained in the flow space Position pre-integration increment Velocity pre-integration increment And calculate the error transfer matrix;

[0009] Step 4: Determine whether there is input from other sensors besides the IMU. If so, proceed to step 5. Otherwise, k=k+1, and loop through steps 1 to 4.

[0010] Step 5: Create a pre-integration factor based on the pre-integration increment from time i to time k in step 3 above, set the pre-integration end time j = k, and add the pre-integration factor between the state variables at time i and time j in the factor graph;

[0011] Step 6: In the flow space, Compensate for the Earth's rotation angular rate and the navigation system's rotation angular rate caused by the carrier's motion, respectively. and Compensate for gravitational acceleration, Coriolis force, and the centripetal force generated by the motion of the carrier on the Earth's surface. Then model the Earth as an ellipsoid, recursively calculate the state at time j and add it to the factor graph.

[0012] Step 7: Perform ISAM2 optimization, calculate the pre-integration residuals at time i and time j and add them to the factor graph. Assume that the time to be linearized is set to i and the next time is j, and calculate the pre-integration increment Perform first-order recalibration of accelerometer and gyro bias;

[0013] Step 8: In the flow space, first perform the pre-integration increment in step 7 and Update and compensate for gravity, and then Compensate for the Earth's rotation angular rate and the navigation system's rotation angular rate caused by the carrier's motion, and finally and Compensate for the Coriolis force and centripetal force, recursively calculate the state at time j, and update the pre-integrated residual between the states at time i and j;

[0014] Step 9: Based on the ISAM2 judgment rule, determine whether the status of all affected historical moments has been recalculated. If so, execute step 10; otherwise, continue to loop through steps 7 and 9.

[0015] Step 10: Let i=j, output navigation information and return to step 1.

[0016] Furthermore, the result of the k-1 moment in the b system after compensation in step 2 is and the angle increment after cone motion compensation at time k-1 Specifically:

[0017]

[0018]

[0019] Furthermore, the angle pre-integration increment from time i to time k in step 3 is Position pre-integration increment Velocity pre-integration increment Specifically:

[0020]

[0021]

[0022]

[0023] in, is the last angle pre-integration increment, p k-1,kis the position pre-integration increment from time k-1 to time k, v k-1,k is the velocity pre-integral increment from time k-1 to time k, satisfy:

[0024]

[0025] Furthermore, the error transfer matrix in step 3 is specifically:

[0026]

[0027] Among them, Σ k is the error transfer matrix at time k, Σ k-1 is the error transfer matrix at time k-1, which is a 9×9 square matrix. is the measurement noise of the accelerometer at time k, The noise introduced by measuring the angular velocity at time k;

[0028]

[0029]

[0030]

[0031] Furthermore, in step six Compensate for the Earth's rotation angular rate and the navigation system's rotation angular rate caused by the carrier's motion, respectively. and Compensation for gravitational acceleration, Coriolis force, and the centripetal force generated by the movement of the carrier on the earth's surface is specifically:

[0032]

[0033]

[0034]

[0035] Among them, R n is the navigation system rotation compensation, which is based on w ie and w en It is calculated that w ie is the Earth's rotation angular rate in the northeast sky navigation system, w en is the angular rate of rotation of the navigation system caused by the motion of the carrier in the Northeast Navigation System, which is obtained by the pre-integrated increment between i and j. and the state matrix at time i Obtain the state at time j after compensating for gravitational acceleration, Coriolis force, navigation system rotation angular rate, and centripetal force where v midis the average velocity between i and j, where the gravitational acceleration g satisfies:

[0036] g=g0(1+5.27094e-3sin(lat) 2 +2.32718e-5sin(lat) 4 )-h(3.086e-6)

[0037] Among them, g0 is the standard acceleration of Earth's gravity.

[0038] Furthermore, the pre-integrated residual in step seven is specifically:

[0039]

[0040] in, is the state variable at time j calculated recursively, is the calculated state variable that already exists at time j.

[0041] Beneficial effects of the present invention: The present invention compensates for non-commutative errors with a multi-sample algorithm, adopts a single sample plus the previous cycle to compensate for non-commutative errors, considers the earth's rotation angular velocity rate, the rotation of the navigation system, the Coriolis force, the centripetal force, the gravity change, and the algorithm that models the earth as an ellipsoid according to WGS84 (World Geodetic System 1984), continuously adjusts the error compensation at each moment during the optimization process, and combines the ISAM2 algorithm to compensate for all states in the factor graph, so as to give full play to the characteristics of the high-precision IMU and expand the field of use of the high-precision IMU in factor graph optimization. At the IMU pre-integration in the factor graph optimization, the present invention performs rotation error compensation, paddling motion compensation and conical motion compensation on the pre-integration of the velocity, and pre-integrates the compensated angular velocity and acceleration to obtain the pre-integration value. When calculating each variable, the factor graph optimization uses the ISAM2 optimization algorithm to call the update algorithm described in steps five and six. It re-compensates the variables of all affected clusters in the Bayesian tree for the Earth's rotation, the Coriolis force, the rotation of the navigation system caused by the carrier's motion, and the acceleration of gravity. This not only further improves the compensation accuracy, but also prevents the amount of calculation from growing rapidly over time. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] Figure 1 It is the flow chart of the algorithm invented in this paper;

[0043] Figure 2 It is the true value diagram of the simulation trajectory in the three-dimensional plane;

[0044] Figure 3 is the position error of the traditional factor graph pre-integration algorithm;

[0045] Figure 4 is the position error of the factor graph algorithm after improved pre-integration;

[0046] Figure 5 is the position error of the high-precision Kalman filter algorithm;

[0047] Figure 6 It is the position error between high-precision Kalman filtering and traditional factor graph optimization algorithm;

[0048] Figure 7 It is the position error of high-precision Kalman filter and improved pre-integration factor graph optimization algorithm. DETAILED DESCRIPTION

[0049] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0050] In order to solve the problem that traditional pre-integration algorithms cannot adapt to high-precision IMUs, the present invention designs a set of high-precision pre-integration algorithms, which performs speed rotation error compensation, paddling motion compensation and coning motion compensation on the pre-integration. The ISAM2 optimization algorithm is used to perform multiple pre-integration re-compensation on the variables affected by the current variables in the factor graph at each optimization moment. Specifically, all affected variables are re-compensated for the rotation of the earth, Coriolis force, rotation of the navigation system caused by carrier motion, and gravitational acceleration. This not only further improves the compensation accuracy, but also prevents the amount of calculation from increasing over time. The specific implementation flow chart is as follows Figure 1 shown.

[0051] The present invention comprises the steps of:

[0052] Step 1: Determine whether the code is executing the first loop. If so, wait for the high-precision IMU to output two values ​​at time k and time k-1 respectively, and initialize the system. The value output by the high-precision IMU at time k is recorded as: and The value of the high-precision IMU output at time k-1 is recorded as: and Save the values ​​at time k and time k-1. If it is determined that this is not the first loop, obtain the acquisition result of the high-precision IMU at the current time (set as time k) and record it as: and above They are respectively the angular increment and velocity increment of the carrier relative to the inertial system measured by the IMU at time k-1 in the carrier coordinate system b. The angular increment and velocity increment of the carrier relative to the inertial frame, measured by the IMU at time k in the carrier coordinate system b. Time k and time k-1 are two moments between time i and time j, with 0 <= i <= k-1. Time i is defined as the starting time of each pre-integration, and time k and time k-1 are two adjacent moments between them.

[0053] Step 2: Put the k moment and k-1 moment and The value is corrected by the zero bias of the gyro and accelerometer at time i. Then the corrected velocity increment Compensate for the rotation error of the velocity at time k-1 in the b frame The rowing error compensation at time k-1 under the b system of the previous cycle and the single sample Finally, we get the result of k-1 moment in the compensated b system. The corrected angle increment Perform single sample addition and coning error compensation of the previous cycle to obtain the angle increment after coning motion compensation at time k-1

[0054] Step 3: By compensating the Divide by the IMU sampling time interval to obtain the average angular velocity w from time k-1 to time k under the load system b(k) ,right Divide by the IMU sampling time interval to obtain the average acceleration a from time k-1 to time k under the load system b(k) Then we can get the angle pre-integration increment φ from time k-1 to time k: k-1,k , position pre-integration increment Velocity pre-integration increment Then, the angle pre-integrated increment from time i to time k is obtained in the flow space Position pre-integration increment Velocity pre-integration increment And calculate the error transfer matrix.

[0055] Step 4: Determine whether there is input from other sensors besides the IMU. If so, proceed to step 5; otherwise, k=k+1, and loop through steps 1 to 4.

[0056] Step 5: Create a pre-integration factor based on the pre-integration increment from time i to time k in step 3 above. Set the pre-integration end time to time j, and assign the value at time k to time j (j=k). Add the pre-integration factor to the factor graph between the state variables at time i and time j.

[0057] Step 6: In the flow space, Compensate for the Earth's rotation angular rate and the navigation system's rotation angular rate caused by the carrier's motion, respectively. and Compensate for gravitational acceleration, Coriolis force, and the centripetal force generated by the movement of the carrier on the surface of the earth, then model the earth as an ellipsoid, recursively calculate the state at time j and add it to the factor graph.

[0058] Step 7: Perform ISAM2 optimization, calculate the pre-integration residuals at time i and time j and add them to the factor graph. According to the judgment rules of ISAM2, the time when the history needs to be re-linearized is obtained (if the time to be linearized is set to i, the next time is j), and the pre-integration increment is calculated. Perform first-order recalibration of accelerometer and gyro bias.

[0059] Step 8: Increment the pre-integration in step 7 and Update and compensate for gravity, and Compensation for the Earth's rotation angular rate and the navigation system's rotation angular rate caused by the carrier's motion. and Compensate for the Coriolis force and centripetal force, recursively calculate the state at time j, and update the pre-integrated residual between the states at time i and j.

[0060] Step 9: Based on the ISAM2 judgment rules, determine whether the status of all affected historical moments has been recalculated. If so, execute step 10; otherwise, continue to loop through steps 7 and 9.

[0061] Step 10: Let i = j, output navigation information and return to step 1. This forms a factor graph optimization combined navigation algorithm that integrates ISAM2 and high-precision IMU pre-integration in manifold space.

[0062] The present invention also includes the following structural features:

[0063] In the step 2, the specific implementation is as follows:

[0064]

[0065] Similarly, in step 2 and The zero bias compensation is calculated as follows:

[0066]

[0067] The speed rotation error compensation amount at time k-1 under the carrier system b in step 2 for:

[0068]

[0069] The single sample in step 2 plus the speed paddling error compensation at time k-1 in the previous cycle b system for:

[0070]

[0071] After speed rotation error compensation and single sample plus speed paddling error compensation of the previous cycle, we can get The calculation is as follows:

[0072]

[0073] The single sample in step 2 plus the coning error compensation of the previous cycle for:

[0074]

[0075] The specific implementation of step three is: in step three, after compensation, Divide by the IMU sampling time interval to obtain the angular velocity w at time k under the load system b(k) , taking the single sample plus the previous period as an example, the specific calculation is as follows:

[0076]

[0077] In step three Divide by the IMU sampling time interval to obtain the acceleration a at time k-1 under the load system b(k) , taking the single sample plus the previous period as an example, the specific calculation is as follows:

[0078]

[0079] The angle pre-integration increment φ from time k-1 to time k in step 3 k-1,k for:

[0080] φ k-1,k =w b(k) Δt k-1,k (9)

[0081] The position pre-integration increment from time k-1 to time k in step 3 for:

[0082]

[0083] The velocity pre-integration increment from time k-1 to time k in step 3 for:

[0084]

[0085] The angle pre-integration increment φ from time k-1 to time kk-1,k , calculate the angle pre-integration increment from time i to time k for:

[0086]

[0087] in is the last angle pre-integration increment, The calculation is as follows:

[0088]

[0089] The position pre-integrated increment p from time k-1 to time k k-1,k , calculate the position pre-integration increment from time i to time k for:

[0090]

[0091] The velocity pre-integrated increment v from time k-1 to time k k-1,k , calculate the velocity pre-integration increment from time i to time k for:

[0092]

[0093] The specific calculation of error transfer in step three is as follows:

[0094] First, you need to calculate the current pre-integration increment For the last pre-integration increment The partial derivative of , the derivative results are as follows:

[0095]

[0096] Then calculate the current pre-integration increment For acceleration a b(k) The partial derivative result is:

[0097]

[0098] Similarly, the current pre-integration increment is Angular velocity w b(k) The partial derivative result is:

[0099]

[0100] The error transfer matrix at the kth moment in step 3 can be calculated from the error transfer matrix at the k-1th moment. The error transfer calculation formula is as follows:

[0101]

[0102] where Σk is the error transfer matrix at time k, which is a 9×9 square matrix. k-1 is the error transfer matrix at time k-1, which is a 9×9 square matrix. is the measurement noise of the accelerometer at time k, is the noise introduced by measuring the angular velocity at time k.

[0103] The specific implementation of step 5 is as follows: through the pre-integration increment between i and j The pre-integration factor is constructed by the value of the pre-integration residual. However, the pre-integration residual is not used in step 5, but is calculated in the optimization process of step 7. Based on the state variable at time i in step 6, the state variable at time j is recursively calculated and recorded as Then calculate the state variables that already exist at time j and record them as The pre-integration residual is calculated as:

[0104]

[0105] Step 6 is implemented as follows, where the gravity update calculation is as follows:

[0106] g=g0(1+5.27094e-3sin(lat) 2 +2.32718e-5sin(lat) 4 )-h(3.086e-6) (21)

[0107] Where lat is the current latitude of the carrier, h is the current altitude of the carrier, and g0 is the standard Earth gravity acceleration value, which is taken as 9.780325. The current latitude of the carrier is obtained by inversely solving the value in the current world coordinate system. The compensation gravity acceleration, Coriolis force, navigation system rotation angular rate, and centripetal force in step 6 are calculated recursively as follows:

[0108]

[0109] Among them, R n is the navigation system rotation compensation, which is based on w ie and w en It is calculated that w ie is the Earth's rotation angular rate in the Northeast Sky navigation system. en The angular rate of rotation of the navigation system caused by the carrier motion in the Northeast Navigation System. and the state matrix at time i Obtain the state at time j after compensating for gravitational acceleration, Coriolis force, navigation system rotation angular rate, and centripetal force where v midis the average speed between i and j.

[0110] In step 7, the pre-integration increment (φ i,k 、 ) The specific implementation steps for the first-order re-correction of the accelerometer and gyroscope bias are as follows: by calculating the partial derivatives of the accelerometer bias and gyroscope bias for the pre-integrated increment, the Jacobian matrix of the pre-integrated increment relative to the accelerometer bias and gyroscope bias is obtained. Then, after obtaining the values ​​of the accelerometer and gyroscope bias estimated at the latest moment, the bias at the start of the pre-integration is subtracted from the latest bias to obtain the change in bias. The pre-integrated accelerometer bias correction is obtained by multiplying the Jacobian of the accelerometer bias by the change in bias, and the pre-integrated gyroscope bias correction is obtained by multiplying the Jacobian of the gyroscope bias by the change in bias. Finally, the pre-integrated increment φ i,k 、 The corrected pre-integrated value is obtained by integrating and adding the pre-integrated accelerometer bias correction value and the pre-integrated gyroscope bias correction value.

[0111] In the present invention, step eight is to call step six to recursively calculate the state at the next moment and update the pre-integration residual.

[0112] This paper invented a factor graph optimization combined navigation algorithm that integrates ISAM2 and high-precision IMU pre-integration in flow space. The experimental trajectory truth value is as follows Figure 2 The parameters of the high-precision inertial navigation simulation are as follows: the accelerometer zero bias is 50ug, the velocity random walk is The angular random walk of the gyroscope is The gyro's bias is The simulated GNSS positioning accuracy is 1 meter and the GNSS output frequency is 1 Hz. Figure 3 Error plot of the optimized positions versus the true values ​​for the traditional pre-integration factor plot. Figure 4 Error plot of the optimized position and true value for the improved pre-integrated factor graph. Figure 5 This is the position error diagram of the high-precision Kalman filter. There is only position error because the position error is the parameter that best reflects the accuracy of the system, while other errors are not. The high-precision IMU pre-integration algorithm invented in this article can effectively improve the accuracy of position, velocity, and attitude in the traditional pre-integration algorithm. The main reason is that the high-precision IMU algorithm can more effectively adapt to the high-precision IMU. Since the high-precision IMU can measure the angular rate of the earth and cannot ignore the influence of time-varying gravity and Coriolis force on its output, the traditional pre-integration algorithm ignores it, resulting in sudden changes in attitude, position, and velocity results. Furthermore, the present invention also compensates between adjacent state variables in the factor graph based on the pre-integration recursion, further improving the accuracy of the algorithm. Figure 6Comparison of position error between high-precision Kalman filter and traditional factor graph optimization algorithm, Figure 7 This figure compares the positioning errors of a high-precision Kalman filter and an improved factor graph optimization algorithm after pre-integration. The mean square error (MSE) results show that the positioning errors of the high-precision Kalman filter are 1.2504 meters horizontally and 0.9856 meters celestially. The positioning errors of the unimproved factor graph optimization are 0.3686 meters horizontally and 0.3293 meters celestially. The errors of the factor graph optimization after improved pre-integration are 0.3409 meters horizontally and 0.1514 meters celestially. This paper proposes a combined navigation algorithm based on factor graph optimization that integrates ISAM2 with high-precision IMU pre-integration in flow space. This algorithm performs real-time gravity updates, making it suitable for large-scale maneuvers. Furthermore, by compensating for factors such as Earth's rotation and the Coriolis force, it achieves higher positioning accuracy. Traditional pre-integration algorithms, which do not compensate for these factors, suffer from reduced accuracy. The high-precision Kalman filter, which only considers the current and previous states, is less accurate than the proposed algorithm. Simulation verification shows that the factor graph optimization combined navigation algorithm invented in this paper that integrates ISAM2 and high-precision IMU pre-integration in flow space can effectively improve the accuracy of high-precision IMU application scenarios.

Claims

1. A factor graph optimization combined navigation method, characterized in that: The following steps are involved: Step 1: Determine whether the code is executing the first loop. If so, obtain and save the k-time data output by the IMU and and k-1 moment data and And initialize the system; otherwise, obtain and save the k-time data output by the current k-time IMU and in are the angular increment and velocity increment of the carrier relative to the inertial system measured by the IMU at time k in the carrier coordinate system b; the starting time of pre-integration is defined as time i, and 0≤i≤k-1; Step 2: Use the zero bias of the gyroscope and accelerometer at time i to align and Make a correction and then increase the speed after correction Compensate for the rotation error of the velocity at time k-1 in the b frame The rowing error compensation at time k-1 under the b system of the previous cycle and the single sample Finally, we get the result of k-1 moment in the compensated b system. The corrected angle increment Perform single sample addition and coning error compensation of the previous cycle to obtain the angle increment after coning motion compensation at time k-1 Step 3: and Divide by the sampling time interval of the IMU to obtain the average acceleration a at time k-1 and time k under the load system b(k) and the average angular velocity w b(k) , and then calculate the angle pre-integration increment φ from time k-1 to time k k-1,k , position pre-integration increment Velocity pre-integration increment Then, the angle pre-integrated increment from time i to time k is obtained in the flow space Position pre-integration increment Velocity pre-integration increment And calculate the error transfer matrix; Step 4: Determine whether there is input from other sensors besides the IMU. If so, proceed to step 5. Otherwise, k=k+1, and loop through steps 1 to 4. Step 5: Create a pre-integration factor based on the pre-integration increment from time i to time k in step 3 above, set the pre-integration end time j = k, and add the pre-integration factor between the state variables at time i and time j in the factor graph; Step 6: In the flow space, Compensate for the Earth's rotation angular rate and the navigation system's rotation angular rate caused by the carrier's motion, respectively. and Compensate for gravitational acceleration, Coriolis force, and the centripetal force generated by the motion of the carrier on the Earth's surface. Then model the Earth as an ellipsoid, recursively calculate the state at time j and add it to the factor graph. Step 7: Perform ISAM2 optimization, calculate the pre-integration residuals at time i and time j and add them to the factor graph. Assume that the time to be linearized is set to i and the next time is j, and calculate the pre-integration increment Perform first-order recalibration of accelerometer and gyro bias; Step 8: In the flow space, first perform the pre-integration increment in step 7 and Update and compensate for gravity, and then Compensate for the Earth's rotation angular rate and the navigation system's rotation angular rate caused by the carrier's motion, and finally and Compensate for the Coriolis force and centripetal force, recursively calculate the state at time j, and update the pre-integrated residual between the states at time i and j; Step 9: Based on the ISAM2 judgment rule, determine whether the status of all affected moments in history have been recalculated. If so, proceed to step 10; otherwise, continue to loop through steps 7 and 9. Step 10: Let i=j, output navigation information and return to step 1.

2. The factor graph optimization combined navigation method according to claim 1, characterized in that: The result of the k-1 moment in the b system after compensation in step 2 and the angle increment after cone motion compensation at time k-1 Specifically:

3. The factor graph optimization combined navigation method according to claim 1, characterized in that: The angle pre-integration increment from time i to time k in step 3 Position pre-integration increment Velocity pre-integration increment Specifically: in, is the last angle pre-integration increment, is the position pre-integration increment from time k-1 to time k, is the velocity pre-integral increment from time k-1 to time k, satisfy:

4. The factor graph optimization combined navigation method according to claim 3, characterized in that: The error transfer matrix in step 3 is specifically: Among them, Σ k is the error transfer matrix at time k, Σ k-1 is the error transfer matrix at time k-1, which is a 9×9 square matrix. is the measurement noise of the accelerometer at time k, The noise introduced by measuring the angular velocity at time k; Where, Δt k-1,k is the IMU sampling time interval.

5. The factor graph optimization combined navigation method according to claim 1, characterized in that: Step 6 Compensate for the Earth's rotation angular rate and the navigation system's rotation angular rate caused by the carrier's motion, respectively. and Compensation for gravitational acceleration, Coriolis force, and the centripetal force generated by the movement of the carrier on the earth's surface is specifically: Among them, R n is the navigation system rotation compensation, which is based on w ie and w en It is calculated that w ie is the Earth's rotation angular rate in the northeast sky navigation system, w en is the angular rate of rotation of the navigation system caused by the motion of the carrier in the Northeast Navigation System, which is obtained by the pre-integrated increment between i and j. and the state matrix at time i Obtain the state at time j after compensating for gravitational acceleration, Coriolis force, navigation system rotation angular rate, and centripetal force where v mid is the average velocity between i and j, where the gravitational acceleration g satisfies: g=g0(1+5.27094e-3sin(lat) 2 +2.32718e-5sin(lat) 4 )-h(3.086e-6) Among them, g0 is the standard earth gravity acceleration, lat and h are the latitude and altitude values ​​of the carrier when calculating gravity.

6. The factor graph optimization combined navigation method according to claim 1, characterized in that: The pre-integration residual in step 7 is specifically: in, is the state variable at time j calculated recursively, is the calculated state variable that already exists at time j.

Images