Graph optimization GNSS and IMU integrated navigation method based on dynamic window

Abstract

The invention discloses a dynamic window-based graph optimization GNSS (Global Navigation Satellite System) and IMU (Inertial Measurement Unit) integrated navigation method, which improves the adaptability, calculation efficiency and positioning accuracy of a system in a dynamic environment by introducing a dynamic window strategy and a key frame mechanism. According to the method, the size of a sliding window is dynamically adjusted according to a real-time motion state, environment change and computing resources: when the environment is complex or high precision is needed, the window is enlarged to contain more historical information; when the environment is stable or computing resources are limited, windows are reduced to improve computing efficiency. Meanwhile, threshold values of position and posture changes are set, and only when the variable quantity of the current state and the latest key frame state exceeds the threshold values, new key frame nodes are added, key motion information is captured, and redundant calculation is avoided; according to the method, the optimal balance between the positioning precision and the calculation efficiency in a complex dynamic environment is realized, the method is suitable for an embedded platform with limited resources, and the requirement of high-precision real-time navigation is met.

Description

Technical Field

[0001] The present invention belongs to the field of GNSS navigation technology, and in particular relates to a dynamic window-based graph-optimized GNSS and IMU combined navigation method. Background Art

[0002] In today's navigation system applications, the integration of GNSS (Global Navigation Satellite System) and IMU (Inertial Measurement Unit) has become a key technology for achieving high-precision and high-reliability positioning. GNSS can provide absolute position information worldwide, but its signals are susceptible to obstruction and interference, especially in complex environments such as urban canyons, tunnels, and forests, resulting in reduced positioning accuracy or even loss of positioning. IMUs, on the other hand, use accelerometers and gyroscopes to provide high-frequency attitude and motion information. However, due to accumulated errors, long-term use can lead to positioning drift. The integration of GNSS and IMUs can complement each other's strengths and improve the overall performance of navigation systems.

[0003] Traditional GNSS / IMU integrated navigation methods primarily use Kalman filters for data fusion. The Kalman filter is a recursive estimation method suitable for linear Gaussian systems. However, real-world navigation systems are highly nonlinear, and noise may not strictly follow a Gaussian distribution, which limits the performance of the Kalman filter in certain situations. Furthermore, while the recursive nature of the Kalman filter is suitable for real-time updates, it may not fully utilize historical information for global optimization when processing long-term data series.

[0004] To address these issues, GNSS / IMU fusion methods based on graph optimization have received widespread attention in recent years. Graph optimization methods construct factor graphs, represent state variables as nodes, and represent measurements and constraints as factors (edges), and solve for optimal state estimation by minimizing a global error function. Compared to filtering methods, graph optimization can better handle nonlinear problems, provide globally consistent estimation results, and improve positioning accuracy. However, existing graph optimization methods still face some challenges in practical applications, mainly reflected in the following aspects:

[0005] First, there are the limitations of fixed-size sliding windows. In practical applications, to control computational complexity and memory usage, graph optimization is often performed within a fixed-size sliding window, meaning only a certain number of recent state and measurement data are considered. However, fixed-size sliding windows cannot adapt to varying needs in dynamically changing environments:

[0006] When the environment changes dramatically or the motion state is complex, such as vehicles entering crowded urban roads, drones flying in complex terrain, wires dancing to monitor electromagnetic noise, or drastic weather changes, a larger window is needed to include more historical information to improve positioning accuracy and system robustness.

[0007] When the environment is stable or the motion state is smooth, such as a vehicle traveling at a constant speed on a highway, a fixed-size window may contain too much redundant information, increasing unnecessary computational effort and reducing computational efficiency.

[0008] Therefore, the fixed sliding window cannot dynamically adjust according to changes in the environment and motion state, and cannot achieve the best balance between accuracy and efficiency.

[0009] Secondly, during navigation, data at different points in time contribute differently to positioning. In situations like straight-line driving and constant motion, the changes between successive states are minimal, resulting in high information redundancy. However, during acceleration, deceleration, or significant environmental changes, state variations are significant, requiring more nodes to accurately describe the trajectory. Existing methods typically fail to tailor these different scenarios. They add necessary nodes at critical moments to capture key motion information and ensure accurate trajectory estimation. Reducing nodes at less critical moments avoids processing large amounts of redundant information and conserves computing resources. This results in inefficient utilization of computing resources, impacting the system's real-time performance and positioning accuracy. Furthermore, navigation systems often run on resource-constrained embedded platforms, such as wire-waving monitoring modules, drones, robots, and vehicles, which place stringent demands on computational efficiency and real-time performance. Traditional graph optimization methods require processing a large number of variables and constraints, resulting in high computational complexity and difficulty meeting the demands of high-frequency data updates and real-time computation. This is particularly problematic in applications with high-speed motion or high update rates. Summary of the Invention

[0010] The technical problem to be solved by the present invention is to provide a dynamic window-based graph optimization GNSS and IMU combined navigation method to solve the technical problems that the existing technology uses a fixed sliding window that cannot be dynamically adjusted according to changes in the environment and motion state, and cannot achieve the best balance between accuracy and efficiency; the traditional graph optimization method needs to process a large number of variables and constraints, has high computational complexity, and is difficult to meet the needs of high-frequency data updates and real-time calculations.

[0011] Technical solution of the present invention:

[0012] A dynamic window-based graph-optimized GNSS and IMU integrated navigation method improves the system's adaptability, computational efficiency, and positioning accuracy in dynamic environments by introducing a dynamic window strategy and keyframe mechanism. The sliding window size is dynamically adjusted according to the real-time motion state, environmental changes, and computing resources. Thresholds for position and attitude changes are set, and new keyframe nodes are added only when the difference between the current state and the most recent keyframe state exceeds the threshold, capturing critical motion information and avoiding redundant calculations.

[0013] The method specifically includes:

[0014] Step S01: Set the maximum window size N max and the minimum window size N min , set the key frame determination threshold δ key , initialize the state estimate And covariance matrix P0, initialize the key frame state

[0015] Step S02: Obtain the IMU measurement data x at the current time k k and GNSS measurement data z k ;

[0016] Step S03: Use IMU data to predict the state and obtain a priori estimate

[0017] Step S04: Calculate the state change between the current prior state and the state of the most recent key frame. If the change exceeds the key frame determination threshold, add a key frame at the current time step.

[0018] Step S05: Adjust the sliding window size N according to whether to add key frames k ;

[0019] Step S06: Construct a factor graph in the sliding window, including variable nodes and factors. The variable nodes include state estimation where i∈[kN k +1,k]; the factors include IMU factor, GNSS pseudorange factor and virtual pseudorange factor. The IMU factor is used to connect the receiver status at adjacent moments, the GNSS pseudorange factor connects the receiver status with the satellite position at the corresponding moment, and the virtual pseudorange factor connects the receiver status with the predicted satellite position.

[0020] Step S07: construct a total residual function including all factors;

[0021] Step S08: Use the Gauss-Newton iteration method to iteratively update the parameter estimation value to minimize the total residual function;

[0022] Step S09: If a key frame is added, update the key frame state to the current optimized state estimate;

[0023] Step S10: When the sliding window moves, the old states beyond the window are marginalized, and the influence of the expired states is integrated into the remaining states using linearization and Schur complementation methods.

[0024] Step S11: Repeat steps S02 to S10 to process new data and continuously update the state estimation.

[0025] Step S03 uses IMU data to predict the state and obtain a priori estimate The specific method is:

[0026]

[0027] Among them, x k-1 represents the state vector at time k-1; Represents the prior estimate of the position vector at time k; represents the prior estimate of the velocity vector at time k; Represents the prior estimate of the IMU attitude rotation matrix at time k; represents the prior estimate of the accelerometer bias at time k; represents the prior estimate of the gyroscope bias at time k; Δt represents the time interval between consecutive moments; exp(·) represents the exponential mapping from the rotation vector to the rotation matrix; and denote the process noise of accelerometer and gyroscope bias, respectively, and a k-1 represents the acceleration measured by the IMU at time k-1; ω k-1 Indicates the angular velocity measured by the IMU at time k-1.

[0028] The implementation method of step S04 includes: calculating the state change between the current prior state and the state of the most recent key frame:

[0029]

[0030] Determine Δx k Is it greater than the key frame judgment threshold δ key , if Δx k >δ key , then add a keyframe.

[0031] The sliding window size N in step S05 k Adjust by:

[0032]

[0033] Among them, ΔN represents the increment of window size, N k-1 Indicates the sliding window size at the previous moment, N max Indicates the maximum value of the sliding window, N min Indicates the minimum value of the sliding window.

[0034] The calculation method of IMU factor is:

[0035]

[0036] Among them, x k represents the state vector of the IMU at time k, e IMU,k Represents the IMU factor at time k.

[0037] The calculation method of GNSS pseudorange factor is:

[0038]

[0039] in, represents the Euclidean distance between the receiver position and the position of the i-th satellite, b k represents the receiver clock deviation at time k, e GNSS,k,i represents the pseudorange factor of the receiver at time k to the i-th satellite, ρ k,i represents the pseudorange of the receiver to the i-th satellite at time k, p k represents the receiver position vector.

[0040] The calculation method of the virtual pseudorange factor is:

[0041]

[0042] in, represents the predicted position vector of the i-th satellite at time k, represents the virtual pseudorange of the i-th satellite at time k.

[0043] Virtual pseudorange The calculation method is:

[0044]

[0045] Among them, ρ k-1,i represents the pseudorange measurement value of the i-th satellite at time k, represents the pseudorange change rate of the i-th satellite at time k-1, Δt represents the time interval between consecutive moments, P k-1 represents the position vector of the receiver at time k, and v represents the velocity vector of the receiver.

[0046] The total residual function is constructed as follows,

[0047]

[0048] Where E represents the total cost function, ∑ IMU,k Represents the covariance matrix of the IMU error function, ∑ GNSS,k,i represents the covariance matrix of the GNSS pseudorange error function, ∑ VC,k,i Represents the covariance matrix of the virtual pseudorange error function.

[0049] Beneficial effects of the present invention:

[0050] The present invention adds keyframe nodes only when the motion state changes significantly based on preset thresholds (such as position and posture changes). It captures important motion information at critical moments and avoids adding unnecessary nodes when the state changes are small or when there is a lot of redundant information. This reduces the number of nodes and edges in the optimization graph, reduces computational complexity, and saves computing resources. While ensuring positioning accuracy, it improves computational efficiency and meets the performance requirements of real-time navigation systems.

[0051] This invention introduces a dynamic window strategy that adjusts the sliding window size in real time based on motion status, environmental changes, and computing resources. When the system detects drastic environmental changes or complex motion, it automatically enlarges the sliding window to include more historical status and measurement information, providing richer data to support optimization algorithms and improve positioning accuracy. When the environment is stable or computing resources are limited, the window size is reduced to reduce the computational burden. Through dynamic adjustment, the system can adapt to different environments and needs, achieving an optimal balance between accuracy and efficiency, significantly enhancing the adaptability of the navigation system.

[0052] This invention integrates the influence of expired states outside the sliding window into the current optimization through marginalization techniques. Using linearization and Schur complementation to process expired states, the constraints and information on the system are effectively integrated into the remaining states, avoiding information loss. This not only reduces memory usage and maintains the scalability of the optimization problem, but also preserves the contribution of historical information to the current state estimate, maintaining the system's global consistency and positioning accuracy.

[0053] Through dynamic windowing strategies and keyframe mechanisms, the system intelligently adjusts computational load based on real-time computing resource availability. When computing resources are limited, the system automatically reduces the window size and number of nodes to reduce computational effort. When resources are sufficient, the system increases the number of windows and keyframes to improve positioning accuracy. This optimized resource utilization ensures the system maintains excellent real-time performance under all conditions, making it suitable for resource-constrained embedded platforms.

[0054] In complex environments or with highly variable motion states, the present invention's dynamic windowing strategy and keyframe mechanism allow the system to incorporate more effective information for optimization. By incorporating more historical states and measurement constraints at critical moments, the optimization algorithm can more accurately estimate the current state and reduce positioning errors. This enhances the system's positioning accuracy and robustness in complex environments, meeting the requirements of high-precision navigation.

[0055] By introducing a dynamic window strategy and keyframe mechanism, this invention improves the system's adaptability, computational efficiency, and positioning accuracy in dynamic environments. This method dynamically adjusts the size of the sliding window based on real-time motion state, environmental changes, and computing resources. When the environment is complex or high precision is required, the window is increased to include more historical information; when the environment is stable or computing resources are limited, the window is reduced to improve computational efficiency. Furthermore, a threshold for position and posture changes is set. New keyframe nodes are added only when the difference between the current state and the most recent keyframe state exceeds the threshold, capturing critical motion information and avoiding redundant computation. This method achieves an optimal balance between positioning accuracy and computational efficiency in complex dynamic environments, making it suitable for resource-constrained embedded platforms and meeting the requirements of high-precision real-time navigation. It addresses technical issues such as the inability of existing fixed sliding windows to dynamically adjust to changes in the environment and motion state, thus failing to achieve the optimal balance between accuracy and efficiency; and the difficulty of traditional graph optimization methods in meeting the requirements of high-frequency data updates and real-time computation, due to the large number of variables and constraints they require to process. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] Figure 1 It is a schematic diagram of the process of the present invention. DETAILED DESCRIPTION

[0057] Working principle of the present invention:

[0058] The key technology of this invention is to dynamically adjust the sliding window and key frame selection strategy to adapt to different environments and requirements:

[0059] Dynamic sliding window adjustment: The system adjusts the sliding window size in real time by monitoring environmental complexity, motion status, and computing resource availability. This process involves sensing the environment and motion status, such as using sensor data to analyze changes in motion trajectory and determine whether the window size needs to be adjusted.

[0060] Keyframe selection mechanism: Set a threshold for position and pose changes, and only add a new keyframe node when the change exceeds the threshold. This requires real-time calculation and comparison of position and pose data to ensure that the addition of keyframes accurately reflects significant changes in motion state.

[0061] Marginalization of Expired States: For states outside the sliding window, the system uses marginalization techniques to incorporate their impact on the system into the current optimization. This process requires precise mathematical processing to ensure information retention and efficient calculation.

[0062] Dynamic management of computing resources: The system intelligently allocates computing tasks, adjusts window sizes, and adjusts the number of keyframes based on real-time computing resource availability. This involves monitoring system resources and implementing management strategies to ensure optimal performance under various resource conditions.

[0063] The specific implementation of the present invention includes:

[0064] A dynamic window-based graph optimization GNSS and IMU integrated navigation method, including (see Figure 1 ):

[0065] S01. Set the maximum window size N max and the minimum window size N min , set the key frame determination threshold δ key , initialize the state estimate And covariance matrix P0, initialize the key frame state

[0066] S02. Get the IMU measurement data x at the current time k k and GNSS measurement data z k ;

[0067] S03. Use IMU data to predict the state and obtain a priori estimates

[0068] S04, calculating the state change between the current prior state and the state of the most recent key frame, and if the change exceeds the key frame determination threshold, adding a key frame at the current time step;

[0069] S05. Adjust the sliding window size N based on whether to add key frames k ;

[0070] S06. Construct a factor graph within the sliding window, including variable nodes and factors. The variable nodes include state estimation where i∈[kN k +1,k]; the factors include IMU factor, GNSS pseudorange factor and virtual pseudorange factor. The IMU factor is used to connect the receiver status at adjacent moments, the GNSS pseudorange factor connects the receiver status with the satellite position at the corresponding moment, and the virtual pseudorange factor connects the receiver status with the predicted satellite position.

[0071] S07. Construct a total residual function that includes all factors;

[0072] S08, using the Gauss-Newton iteration method to iteratively update the parameter estimates to minimize the total residual function;

[0073] S09. If a key frame is added, update the key frame state to the current optimized state estimate;

[0074] S10. When the sliding window moves, the old states beyond the window are marginalized, and the effects of the expired states are incorporated into the remaining states using linearization and Schur complementation.

[0075] S11. Repeat steps 2 to 10 to process new data and continuously update the state estimate.

[0076] In step S03, the state is predicted using IMU data to obtain a priori estimation. The specific method is:

[0077]

[0078] Among them, x k-1 represents the state vector at time k-1; Represents the prior estimate of the position vector at time k; represents the prior estimate of the velocity vector at time k; Represents the prior estimate of the IMU attitude rotation matrix at time k; represents the prior estimate of the accelerometer bias at time k; represents the prior estimate of the gyroscope bias at time k; Δt represents the time interval between consecutive moments; exp(·) represents the exponential mapping from the rotation vector to the rotation matrix; and denote the process noise of accelerometer and gyroscope bias, respectively, and a k-1 represents the acceleration measured by the IMU at time k-1; ω k-1 Indicates the angular velocity measured by the IMU at time k-1.

[0079] The step S04 is specifically as follows:

[0080] Calculate the state change between the current prior state and the state of the most recent keyframe

[0081]

[0082] Determine Δx k Is it greater than the key frame judgment threshold δ key , if Δx k >δ key , then add a keyframe.

[0083] The sliding window size N in step S05 kAdjust by:

[0084]

[0085] Among them, ΔN represents the increment of window size, N k-1 Indicates the sliding window size at the previous moment, N max Indicates the maximum value of the sliding window, N min Indicates the minimum value of the sliding window.

[0086] The calculation method of the IMU factor is:

[0087]

[0088] Among them, x k represents the state vector of the IMU at time k, e IMU,k Represents the IMU factor at time k.

[0089] The calculation method of the GNSS pseudorange factor is:

[0090]

[0091] in, represents the Euclidean distance between the receiver position and the position of the i-th satellite, b k represents the receiver clock deviation at time k, e GNSS,k,i represents the pseudorange factor of the receiver at time k to the i-th satellite, ρ k,i represents the pseudorange of the receiver to the i-th satellite at time k, p k represents the receiver position vector.

[0092] The calculation method of the virtual pseudorange factor is:

[0093]

[0094] in, represents the predicted position vector of the i-th satellite at time k, represents the virtual pseudorange of the i-th satellite at time k.

[0095] Furthermore, the virtual pseudorange The calculation method is:

[0096]

[0097] Among them, ρ k-1,i represents the pseudorange measurement value of the i-th satellite at time k, represents the pseudorange change rate of the i-th satellite at time k-1, Δt represents the time interval between consecutive moments, P k-1represents the position vector of the receiver at time k, and v represents the velocity vector of the receiver.

[0098] The total residual function is constructed by the following method,

[0099]

[0100] Where E represents the total cost function, ∑ IMU,k Represents the covariance matrix of the IMU error function, ∑ GNSS,k,i represents the covariance matrix of the GNSS pseudorange error function, ∑ VC,k,i Represents the covariance matrix of the virtual pseudorange error function.

Claims

1. A dynamic window-based graph-optimized GNSS and IMU integrated navigation method, characterized in that: By introducing dynamic window strategy and key frame mechanism, the system's adaptability, computational efficiency and positioning accuracy in dynamic environments are improved; the size of the sliding window is dynamically adjusted according to the real-time motion state, environmental changes and computing resources; at the same time, the threshold of position and posture change is set, and only when the change between the current state and the most recent key frame state exceeds the threshold, a new key frame node is added to capture key motion information and avoid redundant calculations.

2. The method for GNSS and IMU integrated navigation based on dynamic window graph optimization according to claim 1, characterized in that: The specific implementation steps of the method include: Step S01, set the maximum window size N max and the minimum window size N min , set the key frame determination threshold δ key , initialize the state estimate And covariance matrix P0, initialize the key frame state Step S02: Obtain the IMU measurement data x at the current time k k and GNSS measurement data k ; Step S03: Use IMU data to predict the state and obtain a priori estimate Step S04, calculating the state change between the current prior state and the state of the most recent key frame, and if the change exceeds the key frame determination threshold, adding a key frame at the current time step; Step S05: Adjust the sliding window size N according to whether to add a key frame k ; Step S06: Construct a factor graph in the sliding window, including variable nodes and factors. The variable nodes include state estimation where i∈[kN k +1,k]; the factors include IMU factor, GNSS pseudorange factor and virtual pseudorange factor. The IMU factor is used to connect the receiver status at adjacent moments, the GNSS pseudorange factor connects the receiver status with the satellite position at the corresponding moment, and the virtual pseudorange factor connects the receiver status with the predicted satellite position. Step S07, constructing a total residual function including all factors; Step S08, using the Gauss-Newton iteration method to iteratively update the parameter estimation value to minimize the total residual function; Step S09: if a key frame is added, update the key frame state to the current optimized state estimate; Step S10: When the sliding window moves, the old states beyond the window are marginalized, and the influence of the expired states is integrated into the remaining states using the linearization and Schur complement method; Step S11, repeat steps S02 to S10, process new data, and continuously update the state estimation.

3. The method for combined GNSS and IMU navigation based on dynamic window graph optimization according to claim 2, characterized in that: Step S03 uses IMU data to predict the state and obtain a priori estimate The specific method is: Among them, x k-1 represents the state vector at time k-1; represents the prior estimate of the position vector at time k; represents the a priori estimate of the velocity vector at time k; Represents the prior estimate of the IMU attitude rotation matrix at time k; represents the a priori estimate of the accelerometer bias at time k; represents the a priori estimate of the gyroscope bias at time k; Δt represents the time interval between consecutive moments; exp(·) represents the exponential mapping of the rotation vector to the rotation matrix; and denote the process noise of the accelerometer and gyroscope bias, respectively, and a k-1 represents the acceleration measured by the IMU at time k-1; ω k-1 Represents the angular velocity measured by the IMU at time k-1.

4. The method of GNSS and IMU integrated navigation based on dynamic window graph optimization according to claim 2, characterized in that: The implementation method of step S04 includes: calculating the state change amount between the current prior state and the most recent key frame state: Determine Δx k Is it greater than the key frame determination threshold δ key , if Δx k >δ key , then add a keyframe.

5. The method for combined GNSS and IMU navigation based on dynamic window graph optimization according to claim 2, characterized in that: The sliding window size N in step S05 k Adjust by: Among them, ΔN represents the increment of window size, N k-1 Indicates the sliding window size at the previous moment, N max Indicates the maximum value of the sliding window, N min Indicates the minimum value of the sliding window.

6. The method of GNSS and IMU integrated navigation based on dynamic window graph optimization according to claim 2, characterized in that: The calculation method of IMU factor is: Among them, x k represents the state vector of the IMU at time k, e IMU,k Represents the IMU factor at time k.

7. The method of GNSS and IMU integrated navigation based on dynamic window graph optimization according to claim 2, characterized in that: The calculation method of GNSS pseudorange factor is: in, represents the Euclidean distance between the receiver position and the i-th satellite position, b k represents the receiver clock deviation at time k, e GNSS,k,i represents the pseudorange factor of the receiver at time k to the i-th satellite, ρ k,i represents the pseudorange of the receiver to the i-th satellite at time k, p k represents the receiver position vector.

8. The method of GNSS and IMU integrated navigation based on dynamic window graph optimization according to claim 2, characterized in that: The calculation method of the virtual pseudorange factor is: in, represents the predicted position vector of the i-th satellite at time k, represents the virtual pseudorange of the i-th satellite at time k.

9. The method of graph-optimized GNSS and IMU integrated navigation based on dynamic windows according to claim 2, characterized in that: Virtual Pseudorange The calculation method is: Among them, ρ k-1,i represents the pseudorange measurement value of the i-th satellite at time k, represents the pseudorange change rate of the i-th satellite at time k-1, Δt represents the time interval between consecutive moments, P k-1 represents the position vector of the receiver at time k, and v represents the velocity vector of the receiver.

10. The method of GNSS and IMU integrated navigation based on dynamic window graph optimization according to claim 2, characterized in that: The total residual function is constructed as follows, Where E represents the total cost function, ∑ IMU,k Represents the covariance matrix of the IMU error function, ∑ GNSS,k,i represents the covariance matrix of the GNSS pseudorange error function, ∑ VC,k,i Represents the covariance matrix of the virtual pseudorange error function.

Images