A combined navigation method based on key frame dynamic hierarchical sliding window

Abstract

The application is suitable for the field of satellite navigation technology, and provides a combined navigation method based on a key frame dynamic hierarchical sliding window. The method dynamically adjusts the sliding window length through GNSS state scoring, introduces a hierarchical framing mechanism of A frames (anchor key frames triggered by high-precision GNSS states, with fixed poses) and B frames (ordinary key frames inserted according to IMU drift or time interval), and constructs a local submap with the A frames as references. The method adopts Scan-to-Submap matching to obtain pose constraints, realizes multi-sensor data fusion through factor graph optimization (fixed A frames and optimized B frames), and decouples submap construction and optimization to improve real-time performance. The method effectively solves the problems of fixed sliding window, lack of anchor points for key frames, and coupling of framing and optimization in the prior art, is suitable for weak GNSS environments, and can balance positioning accuracy and system efficiency.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of satellite navigation, and particularly relates to a combined navigation method based on a key frame dynamic hierarchical sliding window. BACKGROUND

[0002] With the rapid development of intelligent networked vehicles and high-level automatic driving technology, vehicles in various complex environments such as cities, mountainous areas, tunnels and parks have put forward higher and higher requirements for positioning capabilities with high precision, strong robustness and good real-time performance. Traditional single sensors (such as GNSS, IMU or LiDAR) are difficult to meet the needs of the automatic driving system for continuous, stable and high-precision positioning due to poor environmental adaptability, susceptibility to interference, large long-term cumulative error and other problems. Therefore, multi-sensor fusion positioning technology has gradually become mainstream, and the combination of GNSS (Global Navigation Satellite System), IMU (Inertial Measurement Unit) and LiDAR (Laser Radar) is widely used in automatic driving navigation systems. The GNSS / IMU / LiDAR fusion positioning system combines the advantages of different sensors - GNSS provides global position information, IMU has strong short-term continuity and high frequency, and LiDAR can provide high-precision environmental structure information, thereby making up for the shortcomings of single sensors in terms of shielding, failure or error accumulation, and has become one of the key infrastructures for realizing vehicle-level positioning. Current mainstream research methods include loose and tight coupling fusion methods, sliding window optimization technology based on factor graphs, key frame mapping and subgraph matching strategies, etc.

[0003] The existing GNSS / IMU / LiDAR fusion navigation technology has the following technical problems in actual application:

[0004] The sliding window size is fixed and cannot be adaptively adjusted according to GNSS availability or environmental changes, which easily introduces abnormal constraints in poor GNSS signal scenarios and exists calculation redundancy in good GNSS signal scenarios, making it difficult to balance precision and efficiency;

[0005] The key frame mapping mechanism is single and lacks anchor reference, and pure radar mapping or key frame update strategy based on IMU mileage accumulation is mostly used, without introducing the "anchor" mapping idea when GNSS signal is good, which makes it difficult to utilize the global constraint of GNSS on map construction and easily leads to global drift;

[0006] The mapping and optimization are highly coupled, and the point cloud processing, factor graph construction and sliding window optimization are triggered simultaneously in the key frame mapping stage, lacking a decoupling mechanism, which makes it difficult to guarantee high-frequency updates of the system in dynamic environments with high laser radar frequency or frequent GNSS state fluctuations, and easily accumulates delay, thereby restricting real-time performance. SUMMARY

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

[0008] The application is implemented as a combined navigation method based on key frame dynamic hierarchical sliding window, comprising:

[0009] 1. Time stamp synchronization of GNSS receiver, IMU unit, LiDAR sensor data, and generation of time sequence fusion frame Each frame contains: GNSS pseudorange and solution state, IMU raw acceleration and angular velocity, and LiDAR point cloud data.

[0010] 2. Calculate the current frame GNSS state score:

[0011] ;

[0012] 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;

[0013] According to the score, the sliding window length is dynamically set:

[0014] .

[0015] 3. Key frame insertion and sliding window update:

[0016] A frame (anchor key frame) insertion condition:

[0017] ;

[0018] Distance from the last A frame time interval ;

[0019] Insertion process:

[0020] Mark the current frame as an A frame and record its GNSS pose;

[0021] Project all B frame poses to the new A frame coordinate system:

[0022] Clear the association relationship of the old A frame.

[0023] B frame (ordinary key frame) insertion condition:

[0024] IMU integral drift exceeds the threshold value;

[0025] Or time interval from last key frame

[0026] Insertion flow:

[0027] Mark current frame as B-frame;

[0028] Record point cloud, timestamp, pose ;

[0029] Join sliding window and keep latest N B-frames.

[0030] 4. Construct submap based on sliding window:

[0031] Project all B-frame point clouds to current A-frame coordinate system by pose;

[0032] Construct local map submap by voxel filter or raster fusion for Scan-to-Submap matching.

[0033] 5. Factor graph optimization, including:

[0034] Optimization variables:

[0035] Let N B-frames be contained in current sliding window, construct optimization variable set as:

[0036] ;

[0037] Where: represents the pose of the i-th B-frame relative to the current A-frame; the current A-frame is a fixed value or is assigned an unadjustable constant weight;

[0038] Residual factors consist of:

[0039] ;

[0040] Where, is the laser constraint 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 the GNSS measurement, is the prior residual term.

[0041] Laser matching factor:

[0042] ;

[0043] is the i-th frame in the The residual of each surf point For the first The first frame indivual Point residuals, Let be the covariance matrix of the residuals from the point to the line. Let be the covariance matrix of the point-to-plane residuals. This is the covariance matrix, used to adjust the weights of each residual.

[0044] IMU pre-integration factor:

[0045] ;

[0046] in, For IMU pre-integration factor, For posture residuals, For positional residuals, For the velocity residual, To constrain the smoothness of the accelerometer bias between adjacent time points, the bias residual is used. To constrain the smoothness of the gyroscope bias for the bias residual.

[0047] GNSS DD factor:

[0048] ;

[0049] ;

[0050] Among them, GNSS DD factor, Satellites observed jointly by the receiver and the reference receiver With reference satellite Double-difference observations For satellite With reference satellite Relative to the receiver's double-difference observation direction vector, For receiver Coordinates in the navigation coordinate system Let be the known baseline vector of the receiver antenna phase center relative to the vehicle coordinate system. For receiver The rotation matrix between the antenna phase center and the vehicle body coordinate system. This represents measurement error.

[0051] Prior residuals:

[0052] ;

[0053] in, for the stack of all state variables within the current sliding window, for the marginalized residual vector, for the corresponding covariance information matrix.

[0054] generated from the marginalized history B frames.

[0055] 6. Pose output and update

[0056] Compute current frame pose, publish to navigation module; predict next frame initial value based on current pose and IMU integration.

[0057] The beneficial effects of the present application are:

[0058] The GNSS-IMU-LiDAR integrated navigation method based on key frame dynamic hierarchical sliding window provided by the present application dynamically adjusts the sliding window length by introducing a GNSS state scoring mechanism, lengthens the window when the GNSS signal is good to enhance the constraint, and reduces the window when the signal is poor to improve the real-time performance, effectively balancing the positioning accuracy and system efficiency; through the hierarchical framing mechanism of A frames (anchor key frames) and B frames (ordinary key frames), the high-precision GNSS state triggers the A frame as the global reference of the local subgraph, stabilizes the map coordinates, enhances the global consistency, and suppresses the trajectory drift; the subgraph construction and factor graph optimization are decoupled, the key frames are only cached and the matching subgraph is constructed in the construction stage, and the A frame pose is fixed in the optimization stage, which significantly reduces the computational burden and improves the deployment adaptability and real-time response capability of the system on the embedded platform. At the same time, the fusion optimization of multi-modal factors (laser matching, IMU pre-integration, GNSS pseudorange) further improves the positioning accuracy and robustness in complex urban environments, and is especially suitable for intelligent vehicle navigation and positioning tasks in weak GNSS environments such as urban canyons, tunnels, and occlusions. BRIEF DESCRIPTION OF DRAWINGS

[0059] Figure 1 The integrated navigation method flowchart provided by the embodiment of the present application;

[0060] Figure 2 The A / B key frame insertion mechanism flowchart provided by the embodiment of the present application. DETAILED DESCRIPTION

[0061] In order to make the purpose, technical scheme and advantages of the present application clearer, the present application will be further described in detail below in combination with the drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and do not limit the present application.

[0062] As shown in Figure 1 , an integrated navigation method based on key frame dynamic hierarchical sliding window, the method comprises:

[0063] Step 1, time synchronization of data of GNSS receiver, IMU unit and LiDAR sensor, generation of time sequence fusion frame , each frame containing: GNSS pseudorange and solution state, IMU original acceleration and angular velocity, LiDAR point cloud data, and each data frame recording time stamp ;

[0064] Step 2, calculating the current frame GNSS state score , and dynamically setting the sliding window length N according to the current frame GNSS state score ;

[0065] The calculation of the current frame GNSS state score , specifically:

[0066] ;

[0067] Wherein: is the GNSS solution mode score, is the visible satellite number score, is the HDOP score, is the solution residual score, respectively, the weight coefficients of the solution mode, the number of visible satellites, HDOP and the solution residual;

[0068] The decision threshold of the sliding window length N is defined as:

[0069] .

[0070] Updating the sliding window, including: triggering A frame insertion based on GNSS state score, triggering B frame insertion based on IMU integral drift amount / key time interval;

[0071] Step 3, A / B key frame insertion mechanism, as shown in Figure 2 :

[0072] The A frame is used to provide a high-precision anchor point reference coordinate system, and the condition for triggering the A frame insertion is that:

[0073] The current frame GNSS state score is greater than the set anchor point threshold ;

[0074] The time interval from the last A frame exceeds the preset minimum interval ;

[0075] The A frame insertion process includes:

[0076] Marking the current frame as an A frame;​

[0077] Record the current frame of GNSS high-precision pose as a new anchor point;

[0078] Reset the sliding window, project all B frames to the new A frame coordinate system, and update ;

[0079] Clear the old A frame and invalid associations, and set the current A frame as the current anchor point.

[0080] The condition for triggering B frame insertion is to meet any of the following:

[0081] The drift of IMU integral calculation exceeds the set threshold from the last key frame;

[0082] The time interval between the current frame and the last key frame exceeds the fixed interval .

[0083] The B frame insertion process includes:

[0084] Mark the current frame as a B frame;

[0085] Record the current frame point cloud, timestamp, and initial estimated pose , reference the current A frame coordinate system;

[0086] Add the current frame to the current sliding window, and keep at most N B frames (determined dynamically according to GNSS score);

[0087] If the sliding window exceeds the upper limit, remove the oldest B frame and keep the current A frame fixed.

[0088] Step 4, take the current A frame as the anchor point, project all B frame point clouds in the sliding window according to their poses to the current A frame coordinate system, and construct a subgraph;

[0089] Adopt the anchor point dominant sliding window mapping strategy, each time the map is updated by the current anchor A frame and its associated N B frames to construct a subgraph for laser point cloud matching and state optimization.

[0090] (1) Sliding window composition

[0091] The key frame set in the sliding window is:

[0092] ;

[0093] Where:

[0094] : Current activated anchor frame, i.e. A-frame, corresponds to GNSS high quality state

[0095] : The latest N B-frames in the sliding window

[0096] N: The score of the current frame GNSS state Dynamic decision.

[0097] (2) Sliding window update rule

[0098] Every time a frame of data is processed, the sliding window is dynamically adjusted according to the A / B frame insertion situation, and the rules are as follows:

[0099] Case 1: New A-frame insertion

[0100] Take this A-frame as the new anchor point, and the original A-frame is discarded;

[0101] Transform the pose of all B-frames in the sliding window to the coordinate system of the new A-frame:

[0102] ;

[0103] : Pose of the old anchor A-frame in the world coordinate system.

[0104] : Pose of a certain B-frame in the world coordinate system.

[0105] : Convert the pose of the B-frame in the world coordinate system to the coordinate system of the new anchor A-frame.

[0106] : Pose of the B-frame relative to the old anchor A-frame.

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

[0108] Case 2: B-frame insertion

[0109] Add this frame to the B-frame set in the current sliding window;

[0110] If the number of B-frames in the sliding window exceeds the current set window length N, remove the earliest B-frame (keep the number of B-frames in the sliding window unchanged);

[0111] The current A-frame remains unchanged.

[0112] Case 3: Normal frame (not inserted A / B)

[0113] The current frame is not included in the key frame set, only as a to-be-matched frame, participates in Scan-to-Submap matching, but does not affect the sliding window construction.

[0114] (3) Submap map construction

[0115] Based on the sliding window key frame set , in the current A frame coordinate system to construct a local map subgraph: each B frame point cloud according to its pose Project into the A frame coordinate system; voxel filtering, splicing or grid fusion is performed on the N B frame point clouds to construct a dense or sparse map; the current frame point cloud Will be matched with the subgraph to obtain the inter-frame pose constraint.

[0116] (4) optimization window and composition window

[0117] Composition window (submap): only used for point cloud map splicing and laser matching (strong real-time); optimization window (factor graph): used for graph optimization, only B frames in the sliding window are solved, and the A frame is fixed (see step 5); the two are relatively decoupled: fast composition response, delayed optimization processing, ensuring system real-time performance.

[0118] (5) composition coordinate system maintenance strategy

[0119] Each time the composition always uses the current A frame coordinate system as the unified reference system, ensuring that:

[0120] Each B frame point cloud is superimposed in the same reference system;

[0121] Matching calculation will not introduce cross-coordinate system error;

[0122] When the sliding window is reset, all B frame states are transformed once.

[0123] Step 5, fix the A frame pose in the factor graph optimization, and jointly optimize the poses of all B frames in the sliding window using Scan-to-Submap matching factors, IMU pre-integration factors, and GNSS DD factors;

[0124] (1) optimization variable definition

[0125] Let the current sliding window contain N B frames, and the optimization variable set is constructed as follows:

[0126] ;

[0127] Among them: Xi represents the pose of the i-th B frame relative to the current A frame; the current A frame Is a fixed value or is given an unadjustable constant weight;

[0128] (2) residual factor composition

[0129] The objective function is defined as follows, which is the sum of all residuals in the sliding window, and the objective function of the factor graph optimization is:

[0130] ;

[0131] in, Let be the laser confinement factor, and: For IMU pre-integration factor, GNSS DD factor, The measurement covariance matrix of the laser factor is... The covariance matrix of the IMU preintegrating factor is... The covariance matrix of GNSS measurements. This is the a priori residual term.

[0132] The meaning of each item is as follows:

[0133] 1) Laser confinement factor (Scan-to-Submap):

[0134] ;

[0135] in, For the first The first frame The residual of each surf point For the first The first frame indivual Point residuals Let be the covariance matrix of the residuals from the point to the line. Let be the covariance matrix of the point-to-plane residuals. This is the covariance matrix, used to adjust the weights of each residual.

[0136] 2) IMU pre-integration factor:

[0137] ① Definition of state variables

[0138] In the sliding window, each Class B keyframe The state variables are defined as follows:

[0139] Location

[0140] speed

[0141] Posture (rotation)

[0142] Accelerometer zero bias

[0143] Gyroscope zero bias

[0144] ②IMU observation model

[0145] Given time interval , IMU raw observations are accelerations , angular velocities , plus bias and noise modeled as follows:

[0146] ;

[0147] where:

[0148] : gravity vector

[0149] : Gaussian white noise

[0150] Pre-integrated measurement prediction term

[0151] According to the pre-integration theory, the integrated relative motion estimate is:

[0152] ;

[0153] where denotes the rotation from time t to the start frame .

[0154] IMU pre-integrated residual term definition

[0155] Finally, the IMU residual term is composed of six parts, denoted as:

[0156] ;

[0157] where, is the IMU pre-integration factor, is the attitude residual, is the position residual, is the velocity residual, is the bias residual, constraining the accelerometer bias smoothness at adjacent time instants, is the bias residual, constraining the gyroscope bias smoothness, is the rotation increment obtained from the IMU pre-integration, , is the rotation matrix of the IMU coordinate frame with respect to the navigation coordinate frame at time and , , is the IMU position vector in the navigation coordinate system, is the IMU velocity in the navigation system, is the time interval between two frames of IMU, is the representation of the gravitational acceleration in the navigation system, position increments computed from pre-integration, , velocity state variables at time and , velocity increments computed from pre-integration, , accelerometer biases at time , , , gyroscope biases at time , .

[0158] The following is explained:

[0159] a. rotation residual (pose): error mapped to Lie algebra through Log(R), suitable for non-linear minimization;

[0160] b. position and velocity residuals: reflect the bias between the estimated trajectory and the IMU integrations;

[0161] c. bias drift residuals: control the bias rate of change.

[0162] 3) GNSS DD factor

[0163] ;

[0164] ;

[0165] wherein is the GNSS DD factor, is the double-difference observation of satellite and reference satellite commonly observed by the receiver and the reference receiver, is the double-difference observation direction vector of satellite and reference satellite with respect to the receiver, is the coordinate of the receiver in the navigation coordinate system, is the known baseline vector of the receiver antenna phase center with respect 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.

[0166] 4) Prior residual term

[0167] To realize the continuous optimization of the state in the sliding window, the application performs marginalization processing on the historical B frame state sliding out of the window after each round of optimization is completed, compresses the historical observation information into a prior residual term, and uses the prior residual term to guide the subsequent optimization process. The expression form is:

[0168] ;

[0169]

[0170] is a stack of all state variables in the current sliding window;

[0171] is a residual vector after marginalization;

[0172] is a corresponding covariance information matrix.

[0173] The factor enables the system to maintain trajectory continuity and global consistency in long-term operation, effectively utilizes historical information, and avoids state drift.

[0174] (3) Optimization execution process

[0175] 1) Building factor graph nodes and edges: all sliding window B frames are graph nodes, and Scan-to-Submap, IMU constraint, and GNSS constraint are added as edges;

[0176] 2) A frame node fixing: the A frame pose is not differentiated, and only serves as a reference coordinate;

[0177] 3) Solver optimization: using an efficient nonlinear optimizer (CeresSolver) to jointly minimize the above residual to obtain the optimized pose of all B frames in the sliding window;

[0178] 4) State update: using the optimized pose for subsequent sliding window graph construction, next frame prediction, and release.

[0179] Step 6, output the current frame pose and update the state.

[0180] After completing the sliding window factor graph optimization, the final pose estimate of the current data frame is output, and the subsequent state is updated and transmitted to ensure that the system has real-time performance and continuity.

[0181] (1) Current frame pose calculation

[0182] For the current input frame , the final estimated pose is obtained by combining Scan-to-Submap matching and sliding window optimization results, and is expressed as:

[0183] In the coordinate system of the current anchor A frame: ​

[0184] The pose of the current A-frame in the world coordinate system is:

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

[0186] (2) Pose publishing and trajectory recording

[0187] The system publishes the current frame estimated pose to the navigation module or visualization module, and stores the historical trajectory points for post-processing or map reconstruction.

[0188] (3) State propagation for next frame initial value

[0189] In order to improve the matching efficiency and optimization convergence speed of the next frame , the "state prediction + optimization feedback" mechanism is adopted:

[0190] The estimated pose of the current frame, IMU integral value, is used to predict the pose of the next frame as the initial value;

[0191] If becomes a B-frame key frame, then this initial value is used as its priori estimation when joining the sliding window;

[0192] If it is a normal frame, it is used for initial pose estimation of Scan-to-Submap matching.

[0193] (4) Sliding window state update logic

[0194] After completing the processing of the current frame, the system updates the sliding window content according to whether an A-frame or B-frame is inserted:

[0195] If it is an A-frame:

[0196] Set it as a new anchor point A;

[0197] Project all B-frame poses to the new coordinate system;

[0198] Reconstruct the sliding window map;

[0199] If it is a B-frame:

[0200] Add it to the current B-frame queue;

[0201] If it exceeds the window length N, remove the earliest B-frame;

[0202] If it is a normal frame: do not enter the sliding window, only participate in the current frame matching, and do not produce state variables.

[0203] Any technical features in the above-described embodiments can be combined in any manner, and for the sake of brevity, not all possible combinations are described, but it is understood that the scope of the present specification includes all possible combinations.

[0204] The above-described embodiments only express several implementation manners of the present application, and the description is relatively specific and detailed, but it should not be understood as a limitation on the patent scope of the present application. It should be pointed out that, for those skilled in the art, some modifications and improvements can be made without departing from the concept of the present application, and these all belong to the protection scope of the present application. Therefore, the protection scope of the present application patent should be subject to the appended claims.

[0205] The above-described embodiments only express several implementation manners of the present application, and the description is relatively specific and detailed, but it should not be understood as a limitation on the patent scope of the present application. It should be pointed out that, for those skilled in the art, some modifications and improvements can be made without departing from the concept of the present application, and these all belong to the protection scope of the present application. Therefore, the protection scope of the present application patent should be subject to the appended claims. The above-described embodiments only express several implementation manners of the present application, and the description is relatively specific and detailed, but it should not be understood as a limitation on the patent scope of the present application. It should be pointed out that, for those skilled in the art, some modifications and improvements can be made without departing from the concept of the present application, and these all belong to the protection scope of the present application. Therefore, the protection scope of the present application patent should be subject to the appended claims.

Claims

1. A combined navigation method based on keyframe dynamic hierarchical sliding window, characterized in that, The method comprises: Time synchronization of GNSS receiver, IMU unit and LiDAR sensor data, generate time sequence fusion frame Each frame contains: GNSS pseudorange and solution state, IMU raw acceleration and angular velocity, LiDAR point cloud data, and each data frame records a timestamp ; calculating a current frame GNSS state score and dynamically setting the length of the sliding window in positive correlation with the GNSS state score ; updating a sliding window, comprising: triggering A-frame insertion based on GNSS state score, triggering B-frame insertion based on IMU integral drift amount / key time interval; 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; fixing A-frame pose in factor graph optimization, optimizing poses of all B-frames in the sliding window jointly with Scan-to-Submap matching factor, IMU pre-integral factor and GNSS DD factor; outputting current frame pose and updating state; The calculating the current frame GNSS state score comprises: Specifically, the calculating the current frame GNSS state score comprises: ； wherein: is a score for GNSS solution mode, is a score for number of visible satellites, is a score for HDOP, is a score for solution residual, are weight coefficients for solution mode, number of visible satellites, HDOP and solution residual, respectively. The set sliding window length Specifically, ； a key frame set in the sliding window is: ； wherein: : currently activated anchor frame, i.e. A-frame, corresponds to GNSS high quality state; : the latest N B-frames in the sliding window; N: Scored by current frame GNSS state Dynamic decision.

2. The method of claim 1, wherein, a condition for triggering A-frame insertion is satisfied: Current frame GNSS state score Greater than a set anchor point threshold ; the time interval since the last A-frame exceeds a preset minimum interval ; a condition for triggering B-frame insertion is satisfied as follows: from the last key frame, IMU integral drift exceeds a set threshold; The current frame is more than a fixed interval from the previous key frame .

3. The method of claim 1, wherein, in the updating of the sliding window, an A-frame insertion process comprises: marking the current frame as an A-frame; recording the current frame high-precision pose of the GNSS as a new anchor point; Reset the sliding window, project all B frames to the new A frame coordinate system, update ; clearing old A-frame and invalid association, setting the current A-frame as a current anchor point; a B-frame insertion process comprises: marking the current frame as a B-frame; record current frame point cloud, timestamp, initial estimated pose reference current A-frame coordinate system; adding the current frame adding the current sliding window, determining the retained B-frames according to the GNSS state score; if the sliding window exceeds an upper limit, removing the oldest B-frame and keeping the current A-frame fixed.

4. The method of claim 1, wherein, an objective function of the factor graph optimization is: ； wherein, is a stack of all state variables within the current sliding window, is a laser constraint factor, and: is an IMU pre-integration factor, is a GNSS DD factor, is a measurement covariance matrix for the laser factor, is a covariance matrix for the IMU pre-integration factor, is a covariance matrix for the GNSS measurements, is a prior residual term; ； For the first The first frame The residual of each surf point For the first The first frame indivual Point residuals, Let be the covariance matrix of the residuals from the point to the line. Let be the covariance matrix of the point-to-plane residuals. This is the covariance matrix, used to adjust the weights of each residual.

5. The method of claim 4, wherein, a definition of the GNSS DD factor is: ； ； wherein, wherein is a GNSS DD factor, is a satellite common to both the receiver and the reference receiver and the reference satellite , is a double difference observation vector of the satellite and the reference satellite with respect to the receiver, is a coordinate of the receiver in the navigation coordinate system, is a known baseline vector of the receiver antenna phase center with respect to the vehicle coordinate system, is a rotation matrix between the receiver antenna phase center and the vehicle body coordinate system, is a measurement error.

6. The method of claim 5, wherein, the prior residual term is generated by marginalization processing: ； wherein, is the edge-quantized residual vector, is the corresponding covariance information matrix.

7. The method of claim 6, wherein, a residual term contained in the IMU pre-integral factor is: ； wherein, 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 time instants, is the bias residual, constraining the smoothness of the gyroscope bias, is the rotation increment obtained from the IMU pre-integration, , is the rotation matrix of the IMU coordinate frame with respect to the navigation coordinate frame at time and , , is the IMU position vector in the navigation frame, is the IMU velocity in the navigation frame, is the time interval between two frames of IMU, is the representation of the gravity acceleration in the navigation frame, is the position increment obtained from the pre-integration calculation, , is the velocity state variable at time and , is the velocity increment obtained from the pre-integration calculation, , is the accelerometer bias at time , , , is the gyroscope bias at time , .

Images