Combined navigation method based on key frame dynamic grading sliding window

Abstract

The invention is applicable to the technical field of satellite navigation, and provides an integrated navigation method based on a key frame dynamic grading sliding window, which comprises the following steps of: dynamically adjusting the length of the sliding window through a GNSS (Global Navigation Satellite System) state score; the method comprises the following steps: introducing a hierarchical composition mechanism of a frame A (an anchor point key frame triggered by a high-precision GNSS state and fixed in pose) and a frame B (a common key frame inserted according to IMU drift or time interval), and constructing a local subgraph taking the frame A as a reference; scan-to-Submap matching is adopted to obtain pose constraints, multi-sensor data fusion is realized through factor graph optimization (a fixed A frame and an optimized B frame), and sub-graph construction and optimization decoupling are carried out to improve real-time performance. The method effectively solves the problems that in the prior art, a sliding window is fixed, a key frame lacks anchor points, and composition and optimization coupling exist, is suitable for a weak GNSS environment, and can give consideration to positioning precision and system efficiency.

Description

Technical Field

[0001] The invention belongs to the technical field of satellite navigation, and in particular relates to a combined navigation method based on key frame dynamic hierarchical sliding windows. Background Art

[0002] With the rapid development of intelligent connected vehicles and advanced autonomous driving technologies, the demand for high-precision, robust, and real-time positioning capabilities is increasing. Traditional single sensors (such as GNSS, IMU, or LiDAR) are unable to meet the continuous, stable, and high-precision positioning requirements of autonomous driving systems due to poor environmental adaptability, susceptibility to interference, and large long-term accumulated errors. Therefore, multi-sensor fusion positioning technology has gradually become mainstream, with the combination of GNSS (Global Navigation Satellite System), IMU (Inertial Measurement Unit), and LiDAR (Lidar) widely used in autonomous driving navigation systems. GNSS / IMU / LiDAR fusion positioning systems combine the strengths of different sensors: GNSS provides global position information, IMUs offer short-term continuity and high frequency, and LiDAR provides high-precision environmental structure information. These systems compensate for the shortcomings of single sensors due to occlusion, failure, and error accumulation, and have become a key infrastructure for achieving automotive-grade positioning. Current mainstream research methods include loosely coupled and tightly coupled fusion methods, factor graph-based sliding window optimization technology, key frame composition and sub-graph matching strategies, etc.

[0003] The existing GNSS / IMU / LiDAR fusion navigation technology has the following technical problems in practical applications: The sliding window size is fixed and cannot be adaptively adjusted based on GNSS availability or environmental changes. This can easily introduce abnormal constraints in scenarios with poor GNSS signals and lead to computational redundancy in scenarios with good GNSS signals, making it difficult to balance accuracy and efficiency. The keyframe composition mechanism is simple and lacks anchor point references. Most maps use pure radar composition or keyframe update strategies triggered by IMU mileage accumulation. The "anchor point" composition concept is not introduced when the GNSS signal is good. This makes it difficult to utilize the global constraints of GNSS on map construction, which can easily lead to global drift. Composition and optimization are highly coupled. Point cloud processing, factor graph construction, and sliding window optimization are triggered synchronously during the key frame composition stage. There is a lack of a decoupling mechanism. In a dynamic environment with high lidar frequency or frequent fluctuations in GNSS status, it is difficult to ensure high-frequency updates of the system, and delay accumulation is prone to occur, which restricts real-time performance. Summary of the Invention

[0004] The application aims to provide a combined navigation method based on a key frame dynamic hierarchical sliding window, aiming to solve the technical problems existing in the prior art determined in the background art.

[0005] The application is implemented as a combined navigation method based on a key frame dynamic hierarchical sliding window, comprising: 1. Time stamp synchronization of GNSS receiver, IMU unit, LiDAR sensor data, and generation of time sequence fusion frames Each frame contains: GNSS pseudorange and solution state, IMU raw acceleration and angular velocity, and LiDAR point cloud data.

[0006] 2. Calculate the current frame GNSS state score: ; Wherein: is the GNSS solution mode score, is the visible satellite number score, is the HDOP score, is the solution residual score, and are the weight coefficients of the solution mode, the number of visible satellites, HDOP and the solution residual, respectively; According to the score, the sliding window length is dynamically set: .

[0007] 3. Key frame insertion and sliding window update: A frame (anchor key frame) insertion condition: ; Distance from the last A frame time interval ; Insertion process: Mark the current frame as an A frame and record its GNSS pose; Project all B frame poses to the new A frame coordinate system: Clear the association relationship of the old A frame.

[0008] B frame (ordinary key frame) insertion condition: IMU integral drift exceeds threshold value; Or the time interval from the last key frame is

[0009] Insertion process: Mark the current frame as a B frame; Record point cloud, time stamp, pose ; Join the sliding window and keep the latest N B frames.

[0010] 4. Construct a subgraph based on the sliding window: Project all B-frame point clouds to the current A-frame coordinate system according to their poses; Construct local map submaps through voxel filtering or raster fusion for scan-to-submap matching.

[0011] 5. Factor graph optimization, including: Optimization variables: Assume that the current sliding window contains N B frames, and the optimization variable set is constructed as: ; in: Indicates the pose of the i-th B frame relative to the current A frame; the current A frame are fixed values ​​or assigned non-adjustable constant weights; Residual factor composition: ; in, is the laser confinement factor, and: is the IMU pre-integration factor, is the GNSS DD factor, is the measurement covariance matrix of the laser factor, is the covariance matrix of the IMU pre-integration factor, is the covariance matrix of GNSS measurements, is the prior residual term.

[0012] Laser matching factor: ; For the Frame The residual of surf points, For the Frame indivual The residual of the point, is the covariance matrix of the point-to-line residuals, is the covariance matrix of the point-to-plane residual, is the covariance matrix, which is used to adjust the weights of each residual.

[0013] IMU pre-integration factor: ; in, is the IMU pre-integration factor, is the attitude residual, is the position residual, is the velocity residual, is the bias residual, constraining the smoothness of the accelerometer bias at adjacent moments, is the bias residual, which constrains the smoothness of the gyroscope bias.

[0014] GNSS DD Factor: ; ; Among them, is the GNSS DD factor, For the The satellites observed between the receiver and the reference receiver Double difference observations with the reference satellite, is the double-difference observation direction vector of the satellite-receiver, For receiver The coordinates in the navigation coordinate system, is the known baseline vector of the receiver antenna phase center relative to the vehicle coordinate system, is the rotation matrix between the receiver antenna phase center and the vehicle body coordinate system, is the measurement error.

[0015] A priori residuals: ; in, is the stack of all state variables in the current sliding window, is the residual vector after marginalization, is the corresponding covariance information matrix.

[0016] Generated by marginalizing historical B-frames.

[0017] 6. Pose output and update Calculate the current frame pose and publish it to the navigation module; predict the initial value of the next frame based on the current pose and IMU integral.

[0018] The beneficial effects of the present invention are: This paper proposes a GNSS-IMU-LiDAR integrated navigation method based on a dynamic hierarchical sliding window of keyframes. By incorporating a GNSS status scoring mechanism to dynamically adjust the sliding window length, the method extends the window to enhance constraints when the GNSS signal is good and reduces it to improve real-time performance when the signal is poor. This method effectively balances positioning accuracy and system efficiency. A hierarchical mapping mechanism of A-frames (anchor keyframes) and B-frames (normal keyframes) utilizes high-precision GNSS status to trigger A-frames as global references for local subgraphs, stabilizing map coordinates, enhancing global consistency, and suppressing trajectory drift. Subgraph construction is decoupled from factor graph optimization. During the mapping phase, only keyframes are cached and matching subgraphs are constructed. During the optimization phase, only B-frames are solved, with the A-frame pose fixed. This significantly reduces the computational burden and improves the system's deployment adaptability and real-time responsiveness on embedded platforms. Furthermore, the fusion optimization of multimodal factors (laser matching, IMU pre-integration, and GNSS pseudorange) further enhances positioning accuracy and robustness in complex urban environments. This method is particularly suitable for intelligent vehicle navigation and positioning tasks in weak GNSS environments such as urban canyons, tunnels, and obstructions. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1 A flow chart of the combined navigation method provided by an embodiment of the present invention; Figure 2 This is a flow chart of the A / B key frame insertion mechanism provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0020] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0021] like Figure 1 As shown, a combined navigation method based on a key frame dynamic hierarchical sliding window, the method comprising: Step 1: Synchronize the data of GNSS receiver, IMU unit and LiDAR sensor to generate time series fusion frame Each frame contains: GNSS pseudorange and solution status, IMU raw acceleration and angular velocity, LiDAR point cloud data, and each data frame records the timestamp ; Step 2: Calculate the GNSS status score of the current frame , and dynamically set the sliding window length N according to the current frame GNSS status score ( ); Calculating the current frame GNSS status score , specifically: ; in: Rate the GNSS solution mode, Score the number of visible satellites, Rate HDOP, To solve the residual score, are the weight coefficients of solution mode, number of visible satellites, HDOP and solution residual; Define the sliding window length N( ) is: .

[0022] Update the sliding window, including triggering A-frame insertion based on GNSS status score and triggering B-frame insertion based on IMU integrated drift / critical time interval; Step 3, A / B key frame insertion mechanism, such as Figure 2 As shown: The A frame is used to provide a high-precision anchor reference coordinate system. The conditions for triggering the insertion of the A frame are: Current frame GNSS status score Greater than the set anchor threshold ; The time interval from the last A frame exceeds the preset minimum interval ; The A-frame insertion process includes: Set the current frame Marked as A frame; Record current frame GNSS high-precision pose , as the new anchor point; Reset the sliding window, project all B frames to the new A frame coordinate system, and update ; Clear the old A-frame and invalid associations, and set the current A-frame as the current anchor point.

[0023] The conditions for triggering B frame insertion are any of the following: Since the last keyframe, the drift of the IMU integral calculation exceeds the set threshold; The time interval between the current frame and the previous key frame exceeds the fixed interval .

[0024] The B-frame insertion process includes: Set the current frame Marked as B frame; Record the current frame point cloud, timestamp, and initial estimated pose , refer to the current A-frame coordinate system; Set the current frame Join the current sliding window, up to N B frames (determined dynamically according to GNSS score) are retained; If the sliding window exceeds the upper limit, the oldest B frame is removed, and the current A frame is kept fixed.

[0025] Step 4, with the current A frame as the anchor point, all B frame point clouds in the sliding window are projected into the current A frame coordinate system according to their poses to construct a subgraph; The anchor point dominated sliding window mapping strategy is adopted, and each time the map is updated, the current anchor A frame and its associated N B frames jointly constitute a subgraph for laser point cloud matching and state optimization.

[0026] (1) Sliding window composition The key frame set in the sliding window is: ; Among them: : the current active anchor frame (A frame), corresponding to the GNSS high-quality state; : the latest N B frames in the sliding window; N: determined dynamically by the current frame GNSS state score .

[0027] (2) Sliding window update rule Every time a frame of data is processed, the sliding window is dynamically adjusted according to the A / B frame insertion, and the rules are as follows: Case 1: New A frame insertion Take this A frame as the new anchor point, and the original A frame is discarded; Transform the poses of all B frames in the sliding window to the coordinate system of the new A frame: ; : the pose of the old anchor A frame in the world coordinate system.

[0028] : the pose of a B frame in the world coordinate system.

[0029] : convert the pose of the B frame in the world coordinate system to the coordinate system of the new anchor A.

[0030] : the pose of the B frame relative to the old anchor A.

[0031] Reset the sliding window to the new A frame + the latest N B frames.

[0032] Case 2: B frame insertion Join this frame to the B frame set in the current sliding window; If the number of B frames in the sliding window exceeds the currently set window length N, the earliest B frame is removed (keeping the number of B frames in the sliding window unchanged); The current A-frame remains unchanged.

[0033] Case 3: Normal frame (no A / B inserted) The current frame is not included in the keyframe set and is only used as a frame to be matched. It participates in Scan-to-Submap matching but does not affect the sliding window composition.

[0034] (3) Subgraph map construction Sliding window keyframe collection , construct a local map subgraph in the current A frame coordinate system: the point cloud of each B frame is based on its pose Project to the A frame coordinate system; perform voxel filtering, splicing or raster fusion on N B frame point clouds to build a dense or sparse map; the current frame point cloud Scan-to-Submap matching will be performed with the submap to obtain the inter-frame pose constraints.

[0035] (4) Optimize window and composition window The composition window (Submap) is used only for point cloud map stitching and laser matching (strong real-time performance). The optimization window (factor graph) is used for graph optimization and only solves the B frame in the sliding window, while the A frame remains fixed (see step 5). The two are relatively decoupled: the composition response is fast and delayed processing is optimized to ensure system real-time performance.

[0036] (5) Composition coordinate system maintenance strategy Each composition always uses the current A-frame coordinate system as the unified reference system to ensure: Each B-frame point cloud is superimposed in the same reference system; Matching calculations will not introduce cross-coordinate system errors; When the sliding window is reset, the states of all B frames only need to be changed once.

[0037] Step 5: Fix the A-frame pose in the factor graph optimization, and optimize the poses of all B-frames in the sliding window by combining the Scan-to-Submap matching factor, IMU pre-integration factor, and GNSS DD factor. (1) Optimize variable definition Assume that the current sliding window contains N B frames, and the optimization variable set is constructed as: ; in: Indicates the pose of the i-th B frame relative to the current A frame; the current A frame are fixed values ​​or assigned non-adjustable constant weights; (2) Residual factor composition The objective function is defined as the sum of all residuals in the sliding window. The objective function of factor graph optimization is: ; in, is the laser confinement factor, and: is the IMU pre-integration factor, is the GNSS DD factor, is the measurement covariance matrix of the laser factor, is the covariance matrix of the IMU pre-integration factor, is the covariance matrix of GNSS measurements, is the prior residual term.

[0038] Each of these has the following meaning: 1) Laser Constraint Factor (Scan-to-Submap): ; in, For the Frame The residual of surf points, For the Frame indivual The residual of the point, is the covariance matrix of the point-to-line residuals, is the covariance matrix of the point-to-plane residual, is the covariance matrix, which is used to adjust the weights of each residual.

[0039] 2) IMU pre-integration factor (IMUPre-integrationFactor): ①State variable definition In the sliding window, each key frame of type B The state variables are defined as: Location

[0040] speed

[0041] Posture (rotation)

[0042] Accelerometer bias

[0043] Gyroscope bias

[0044] ②IMU observation model Given time interval , the original IMU observation is acceleration , angular velocity , plus bias and noise modeling as follows: ; in: : gravity vector : Gaussian white noise ③ Pre-integrated measurement prediction item According to the pre-integration theory, the integrated relative motion is estimated as: ; in Represents the time from time t to the start frame rotation.

[0045] ④Definition of IMU pre-integration residual term Finally, the IMU residual term consists of six parts, recorded as: ; in, is the IMU pre-integration factor, is the attitude residual, is the position residual, is the velocity residual, is the bias residual, constraining the smoothness of the accelerometer bias at adjacent moments, is the bias residual, constraining the smoothness of the gyroscope bias, is the rotation increment obtained by IMU pre-integration, 、 For the moment and The rotation matrix of the IMU coordinate system relative to the navigation coordinate system, 、 is the IMU position vector in the navigation coordinate system, is the IMU velocity in the navigation system, is the time interval between two IMU frames, is the representation of gravitational acceleration in the navigation system, is the position increment calculated by pre-integration, 、 For the moment and The velocity state variable, is the velocity increment obtained by pre-integration calculation, 、 For the moment 、 The accelerometer bias, 、 For the moment 、 The gyroscope bias.

[0046] The explanation is as follows: a. Rotational residual (attitude): The error is mapped to Lie algebra via Log(R), which is suitable for nonlinear minimization; b. Position and velocity residuals: reflect the deviation between the estimated trajectory and the IMU integral; c. Bias drift residual: controls the rate of bias change.

[0047] 3) GNSS DD factor ; ; Among them, is the GNSS DD factor, For the The satellites observed between the receiver and the reference receiver Double difference observations with the reference satellite, is the double-difference observation direction vector of the satellite-receiver, For receiver The coordinates in the navigation coordinate system, is the known baseline vector of the receiver antenna phase center relative to the vehicle coordinate system, is the rotation matrix between the receiver antenna phase center and the vehicle body coordinate system, is the measurement error.

[0048] 4) Prior residual term To achieve continuous optimization of the state within the sliding window, the present invention marginalizes the historical B-frame state outside the window after each round of optimization, compressing its historical observation information into a priori residual term to guide the subsequent optimization process. Its expression is: ; in: is the stack of all state variables in the current sliding window; is the residual vector after marginalization; is the corresponding covariance information matrix.

[0049] This factor enables the system to maintain trajectory coherence and global consistency in long-term operation, while effectively utilizing historical information to avoid state drift.

[0050] (3) Optimize the execution process 1) Construct factor graph nodes and edges: All sliding window B frames are graph nodes, and Scan-to-Submap, IMU constraints, and GNSS constraints are added as edges; 2) A-frame nodes are fixed: the A-frame pose is not derived, and is only used as a reference coordinate; 3) Solver optimization: Use an efficient nonlinear optimizer (CeresSolver) to jointly minimize the above residuals to obtain the optimized poses of all B-frames in the sliding window; 4) Update status: Use the optimized pose for subsequent sliding window composition, next frame prediction, and release.

[0051] Step 6: Output the current frame pose and update the state.

[0052] After completing the sliding window factor graph optimization, the final pose estimation is output for the current data frame, and the subsequent states are updated and transmitted to ensure the real-time and continuity of the system.

[0053] (1) Current frame pose calculation For the current input frame , its final estimated pose is obtained by combining Scan-to-Submap matching + sliding window optimization results, which is expressed as: The pose in the current anchor point A-frame coordinate system:

[0054] The current A-frame's position in the world coordinate system is:

[0055] The final estimated pose of the current frame in the world coordinate system is:

[0056] (2) Posture publishing and trajectory recording The system estimates the pose of the current frame Publish to the navigation module or visualization module and store historical track points for post-processing or map reconstruction.

[0057] (3) State propagation is used for the initial value of the next frame To improve the next frame To achieve the best matching efficiency and optimized convergence speed, the "state prediction + optimization feedback" mechanism is adopted: Use the estimated pose of the current frame and the IMU integral value to predict the pose of the next frame as the initial value; like If it becomes a B-frame key frame, the initial value is used as the prior estimate of its addition to the sliding window; If it is a normal frame, it is used for Scan-to-Submap matching initial pose estimation.

[0058] (4) Sliding window state update logic After completing the current frame processing, the system updates the sliding window content based on whether frame A or frame B is inserted: like For A frame: Set it as new anchor A; Project all B frame poses to new coordinate system; Reconstruct sliding window map; If it is B frame: Add it to current B frame queue; If it exceeds window length N, remove the earliest B frame; If it is normal frame: not enter sliding window, only participate in current frame matching, no state variable is generated.

[0059] The technical features of the above-described embodiments can be combined in any manner. To make the description concise, all possible combinations of the technical features in the above-described embodiments are not described, but as long as the combinations of the technical features do not contradict, they should be considered within the scope of the present disclosure.

[0060] The above-described embodiments only express several embodiments of the present application, and the description is more specific and detailed, but it should not be understood as limiting the scope of the present patent. It should be pointed out that for ordinary skilled in the art, without departing from the concept of the present application, a number of modifications and improvements can be made, which are within the scope of the present application. Therefore, the scope of protection of the present patent should be subject to the appended claims.

[0061] The above-described only the preferred embodiments of the present application, and not to limit the present application, any modification, equivalent replacement and improvement within the spirit and principles of the present application, should be included within the scope of the present application.

Claims

1. A combined navigation method based on keyframe dynamic hierarchical sliding windows, characterized in that: The method comprises: Time synchronization of data from GNSS receivers, IMU units, and LiDAR sensors to generate time-series fusion frames Each frame contains: GNSS pseudorange and solution status, IMU raw acceleration and angular velocity, LiDAR point cloud data, and each data frame records the timestamp ; Calculate the GNSS status score of the current frame , and dynamically set the sliding window length N according to the current frame GNSS status score ( ); Update the sliding window, including triggering A-frame insertion based on GNSS status score and triggering B-frame insertion based on IMU integrated drift / critical time interval; Taking the current A frame as the anchor point, all B frame point clouds in the sliding window are aligned according to their poses. Project it into the current A-frame coordinate system and construct a sub-image; In the factor graph optimization, the A-frame pose is fixed, and the poses of all B-frames in the sliding window are optimized by combining the Scan-to-Submap matching factor, IMU pre-integration factor, and GNSSDD factor. Output the current frame pose and update the state.

2. The method according to claim 1, characterized in that Calculating the current frame GNSS status score , specifically: ； in: Rate the GNSS solution mode, Score the number of visible satellites, Rating HDOP, To solve the residual score, are the weight coefficients of solution mode, number of visible satellites, HDOP and solution residual; The setting sliding window length N( ), specifically: 。 3. The method according to claim 2, characterized in that The conditions for triggering A frame insertion are: Current frame GNSS status score Greater than the set anchor threshold ; The time interval from the last A frame exceeds the preset minimum interval ; The conditions for triggering B frame insertion are any of the following: Since the last keyframe, the drift of the IMU integral calculation exceeds the set threshold; The time interval between the current frame and the previous key frame exceeds the fixed interval .

4. The method according to claim 3, characterized in that In the update sliding window, the A frame insertion process includes: Set the current frame Marked as A frame; Record current frame GNSS high-precision pose , as the new anchor point; Reset the sliding window, project all B frames to the new A frame coordinate system, and update ; Clear the old A-frame and invalid associations, and set the current A-frame as the current anchor point; The B-frame insertion process includes: Set the current frame Marked as B frame; Record the current frame point cloud, timestamp, and initial estimated pose , refer to the current A-frame coordinate system; Set the current frame Add to the current sliding window and determine the retained B frames based on the GNSS score; If the sliding window exceeds the upper limit, the oldest B frame is removed and the current A frame is kept fixed.

5. The method according to claim 1, wherein The objective function of the factor graph optimization is: ； in, is the laser confinement factor, and: is the IMU pre-integration factor, is the GNSS DD factor, is the measurement covariance matrix of the laser factor, is the covariance matrix of the IMU pre-integration factor, is the covariance matrix of GNSS measurements, is the prior residual term; ； For the Frame The residual of surf points, For the Frame indivual The residual of the point, is the covariance matrix of the point-to-line residuals, is the covariance matrix of the point-to-plane residual, is the covariance matrix, which is used to adjust the weights of each residual.

6. The method according to claim 5, characterized in that The GNSS DD factor is defined as: ； ； Among them, is the GNSS DD factor, For the The satellites observed between the receiver and the reference receiver Double difference observations with the reference satellite, is the double-difference observation direction vector of the satellite-receiver, For receiver The coordinates in the navigation coordinate system, is the known baseline vector of the receiver antenna phase center relative to the vehicle coordinate system, is the rotation matrix between the receiver antenna phase center and the vehicle body coordinate system, is the measurement error.

7. The method according to claim 6, characterized in that The a priori residual term is generated by marginalization: ； in, is the stack of all state variables in the current sliding window, is the residual vector after marginalization, is the corresponding covariance information matrix.

8. The method according to claim 7, characterized in that The residual term included in the IMU pre-integration factor is: ； in, is the IMU pre-integration factor, is the attitude residual, is the position residual, is the velocity residual, is the bias residual, constraining the smoothness of the accelerometer bias at adjacent moments, is the bias residual, constraining the smoothness of the gyroscope bias, is the rotation increment obtained by IMU pre-integration, 、 For the moment and The rotation matrix of the IMU coordinate system relative to the navigation coordinate system, 、 is the IMU position vector in the navigation coordinate system, is the IMU speed in the navigation system, is the time interval between two IMU frames, is the representation of gravitational acceleration in the navigation system, is the position increment calculated by pre-integration, 、 For the moment and The velocity state variable, is the velocity increment obtained by pre-integration calculation, 、 For the moment 、 The accelerometer bias, 、 For the moment 、 The gyroscope bias.

Images