Dynamic sliding window optimization method for 3D laser radar inertial SLAM system

Abstract

The invention discloses a dynamic sliding window optimization method for a 3D laser radar inertial SLAM system, relates to the technical field of SLAM, and aims to improve the precision of pose estimation of an existing laser inertial SLAM system in a complex motion state of a robot. According to the method, the key frame is selected according to the displacement difference and the rotation difference, the key frame is further divided into the strong key frame and the weak key frame, when the window slides, a multi-slide-in and multi-slide-out dynamic optimization strategy of the strong and weak key frames is designed, and the dynamic optimization of the strong and weak key frames is realized in combination with double standards of whether the window meets optimization conditions or not. And the SLAM system is endowed with the capability of dynamically adjusting the window size according to different motion states of the robot. Besides, a two-time optimization scheme of key frame optimization in the sliding window and local common frame optimization in the sliding window is designed, a relatively accurate optimization reference can be provided for common frame optimization after key frame optimization, and more reliable marginalized prior information can be provided for next key frame optimization after common frame optimization. Therefore, the pose estimation precision of the SLAM system in the complex motion state of the robot is improved.

Description

Technical Field

[0001] The invention belongs to the technical field of SLAM (Simultaneous Localization and Mapping), and relates to a SLAM system integrating two sensors, a laser radar and an IMU, and in particular to a dynamic sliding window optimization method with a variable length. Background Art

[0002] SLAM technology enables mobile robots to achieve autonomous navigation and planning, and to sense the environment and measure posture through sensors, thereby completing posture tracking and environmental map construction. The SLAM system is divided into the front end and the back end: the front end is responsible for environmental perception, posture tracking and loop detection to achieve real-time data updates; the back end performs posture optimization and map construction to eliminate accumulated errors.

[0003] In the SLAM backend processing, the sliding window method is a classic local optimization method. This method optimizes the local area by setting a sliding window with a fixed number of key frames to establish a constraint relationship model between the lidar point cloud and the inertial measurement unit. This method can reduce cumulative errors and improve real-time performance in general scenarios. However, a fixed-size sliding window may no longer be applicable in complex robot motion states, resulting in a decrease in the accuracy of pose optimization. Therefore, the traditional sliding window optimization method has certain limitations when dealing with complex robot motion states.

[0004] In response to the above problems, Chinese patent CN117782063A "A multi-sensor fusion positioning method based on graph optimization and variable sliding window" proposes to repeatedly fill in key frame information in the case of small displacement, so that the window size can be variable, ensuring the real-time performance of the vehicle in fast-moving and rapidly changing environments, and improving the environmental adaptability of the system. However, repeated filling will lead to information redundancy and reduce optimization efficiency. Too frequent filling of key frame information with small displacement may cause the trajectory optimization to converge to a local optimal solution, affecting the accuracy of the system's pose estimation and map construction.

[0005] The paper "Variable Sliding Window Surround View SLAM Algorithm for Automatic Parking" proposes a variable sliding window algorithm to improve the efficiency of loop detection and relative pose solution. The sliding window is dynamically adjusted based on the vehicle speed and the number of point clouds. When the vehicle speed is low or the number of point clouds is small, it is adjusted to a smaller sliding window. However, such an adjustment strategy may result in too little point cloud information and inability to accurately optimize; when the vehicle speed is fast or the number of point clouds is large, it is adjusted to a larger sliding window. However, such an adjustment strategy may result in redundant point clouds and reduce optimization efficiency. Summary of the invention

[0006] The present invention aims to solve the problem that the existing 3D laser radar inertial SLAM system back-end sliding window optimization method has low applicability when the robot is in a complex motion state. A dynamic sliding window optimization method for a 3D laser radar inertial SLAM system is provided, which comprises the following steps:

[0007] S1: The SLAM system receives the point cloud data collected by the 3D laser radar, as well as the acceleration data and angular velocity data collected by the IMU, extracts the feature information of the point cloud from the received laser radar data, including corner points and surface points, and then executes S2;

[0008] S2: Obtain the relative transformation relationship between the two frames by matching the laser radar point cloud of the current frame with the laser radar point cloud of the previous key frame, including the displacement difference and the rotation difference, and then execute S3;

[0009] S3: Determine whether the displacement difference or rotation difference obtained in S2 reaches the corresponding threshold to determine whether the current frame is a key frame. If not, retain the IMU relative transformation relationship and return to S1. If so, execute S4;

[0010] For the content of S3, including the key frame selection strategy of the present invention, when the displacement difference or rotation difference from the previous key frame to the current frame reaches the corresponding threshold, the current frame is judged to be a key frame. After the key frames are selected by this method, the key frames are further subdivided into strong key frames and weak key frames. The judgment criteria of strong key frames and weak key frames are given below.

[0011] Strong keyframe judgment criteria: If the displacement difference reaches the displacement threshold and the rotation difference is not zero, the keyframe is marked as a strong keyframe; if the displacement difference and rotation difference both reach the corresponding thresholds, the keyframe is marked as a strong keyframe; if the rotation difference reaches the rotation threshold and the displacement difference is not zero, the keyframe is marked as a strong keyframe. Weak keyframe judgment criteria: If the rotation difference reaches the rotation threshold but the displacement difference is zero, the keyframe is marked as a weak keyframe; if the displacement difference reaches the displacement threshold but the rotation difference is zero, the keyframe is marked as a weak keyframe.

[0012] S4: Determine whether the sliding window is full. If the sliding window is not full, execute S5. If the sliding window is full, execute S6.

[0013] For the content of S4, there are two criteria for judging whether the sliding window is full: first, whether the accumulated displacement of the robot from the oldest key frame to the latest key frame in the sliding window reaches the preset distance threshold; second, whether the number of key frames in the sliding window reaches the maximum number threshold. If either of these two conditions is met, it can be judged that the sliding window is full.

[0014] S5: Add the latest key frame to the sliding window, transform the point cloud of the latest key frame to the map coordinate system and add it to the local map, and then return to S1;

[0015] For the content in S5, the global map coordinate system of the present invention is established based on the coordinate system of the first IMU frame. The local map corresponds to the point cloud of all key frames in the sliding window. The local map is part of the global map and shares the coordinate system reference.

[0016] S6: Use linear interpolation to ensure the timestamp synchronization between the IMU frame and the lidar keyframe, and then execute S7;

[0017] S7: Construct a lidar constraint factor based on the matching relationship between the feature points of the local map point cloud and the feature points of the key frame point cloud in the sliding window; construct an IMU pre-integration constraint factor based on the IMU pre-integration theory; construct a marginalization prior constraint factor based on the marginalization processing result of the updated sliding window. Add the lidar constraint factor, IMU pre-integration constraint factor and marginalization prior constraint factor to the sliding window key frame factor graph optimization model to construct a sliding window key frame optimization objective function, and then execute S8;

[0018] For the content of S7, the state variable x to be optimized of the laser radar constraint factor l for:

[0019]

[0020] in, is the position of the kth key frame in the map coordinate system, It is the attitude value of the kth key frame in the map coordinate system.

[0021] For the content of S7, the variable to be optimized x of the IMU pre-integration constraint factor imu for:

[0022]

[0023] in, and t i Time and t j The position of the key frame at the moment, and t i Time and t j The rotation amount of the keyframe at the moment, and They are t i Time and t j The speed of the keyframe at the moment, and ti Time and t j The accelerometer bias at the key frame at time, and t i Time and t j The gyroscope bias of the key frame at time t i Time and t j The moments are aligned with the k-1th frame and the kth frame of the lidar key frame respectively.

[0024] For the content of S7, the sliding window key frame optimization objective function expression is as follows:

[0025]

[0026] Among them, χ is the state variable of all key frames in the sliding window, and χ=[x0,x1,...,x n ], the total number of all key frames in the sliding window is n+1, e l (x k ,m)、e B (x k-1 ,x k ) and e M They represent the lidar constraint residual, IMU pre-integrated constraint residual and marginalized prior constraint residual respectively, and m is the local map point cloud set corresponding to the current sliding window. For the kth key frame, k∈[0,n], the state vector in the map coordinate system can be expressed as and are the position, velocity and posture of the kth key frame respectively, and are the accelerometer bias and gyroscope bias of the kth key frame respectively.

[0027] For the content of S7, the lidar constraint residual e l (x k ,m)The expression is as follows:

[0028] e l (x k ,m)=r1+r2 (17)

[0029] Among them, r1 represents the corner point residual of the laser radar, and its expression is:

[0030]

[0031] in, is the corresponding corner point coordinate in the k-th frame laser radar coordinate system, is the coordinate representation of the corner point coordinates of the kth frame in the map coordinate system, and Is the local map The coordinates of the two matching corner points, represents the cumulative sum of all corner point residuals in the kth frame, and ||·|| represents the modulus operation.

[0032] r2 represents the surface point residual of the laser radar, and its expression is:

[0033]

[0034] in, is the corresponding surface point coordinate in the k-th frame laser radar coordinate system, is the coordinate representation of the surface point coordinates of the kth frame in the map coordinate system, n k Is the local map The normal vector of the matching plane, represents the cumulative sum of all surface point residuals in the kth frame, and ||·|| represents the modulus operation.

[0035] For the content of S7, the IMU pre-integration constraint residual e B (x k-1 ,x k ) is expressed as:

[0036]

[0037] Among them, r p 、r q 、r v , They are the position residual, attitude residual, velocity residual, accelerometer residual and gyroscope residual from the k-1th key frame to the kth key frame respectively.

[0038] For the content of S7, the marginalized prior constrained residual e M The expression is:

[0039] e M =b p -H p δχ 1:N (twenty one)

[0040] Among them, δχ 1:N is the state increment of all key frames in the sliding window,

[0041]

[0042] Among them, H a is the state vector δχ to be marginalized d The covariance matrix, H c is the retained state vector δχ s The covariance matrix of Indicates H b The transpose of H b is δχ d With δχ s The covariance matrix between d and b s are δχ d and δχ s A constant vector of .

[0043] S8: Optimize the sliding window key frame optimization objective function using a nonlinear optimization algorithm (such as the Gauss-Newton method, the Levenberg-Marquadt method, etc.), update the positions of all key frames in the sliding window, complete the key frame sliding window optimization, and then execute S9.

[0044] S9: Take the oldest frame in the sliding window and the oldest strong key frame other than the oldest frame as the reference frames. If there are several weak key frames between the two reference frames, these weak key frames are also used as reference frames. If there are no strong key frames other than the oldest frame in the sliding window, and all are weak key frames, then all weak key frames in the sliding window are used as reference frames. Construct relative motion constraint factors of all common frames between these reference frames, add all relative motion constraint factors to the optimization model of the factor graph of common frames in the sliding window, thereby establishing a target optimization function for nonlinear optimization, and then execute S10.

[0045] For the content of S9, the optimization of a part of the common frames in the sliding window aims to provide more reliable marginalization prior information for the optimization of the next sliding window key frame. According to the factor graph optimization model of some common frames in the sliding window, the optimization objective function of the common frames is established. By minimizing the sum of the relative motion constraint residuals, the maximum a posteriori probability estimate of the state is obtained. The state variables of all the common frames to be optimized in the sliding window are set as χ o , and there is χ o =[x0,x1,...,x H ], H+1 is the total number of all common frames to be optimized in the sliding window, and the objective function expression of the common frame is as follows:

[0046]

[0047] Among them, e l (x h-1 ,x h ) represents the relative motion constraint residual of the ordinary frame. Let the state vector of the h-th ordinary frame in the map coordinate system be and are the position and posture of the hth normal frame respectively.

[0048] For the content of S9, the relative motion constraint residual e of the normal framel (x h-1 ,x h ) is expressed as:

[0049]

[0050] in, and They represent the rotation and translation transformation from the hth ordinary frame to the h-1th ordinary frame, respectively, which are obtained by ICP matching of the feature information between the two frames of laser point clouds; and They represent the postures of the h-1th normal frame and the hth normal frame in the map coordinate system respectively. and Respectively represent the positions of the h-1th normal frame and the hth normal frame in the map coordinate system, [·] xyz It means that the imaginary part of the quaternion is used to form a three-dimensional vector. The posture and position of the ordinary frame are recursively obtained from the posture of the adjacent benchmark key frame. If the ordinary frame to be optimized is closer to the previous benchmark key frame than the next benchmark key frame, it is recursively obtained from the posture of the previous benchmark key frame, and vice versa.

[0051] If the normal frame to be optimized is closer to its previous reference key frame, its pose recursion is based on the pose of the previous frame and is recursively obtained according to the motion model and pose transformation relationship. The expression is as follows:

[0052]

[0053] If the normal frame to be optimized is closer to the next benchmark key frame, its pose is recursively deduced based on the pose of the next frame, and the expression is as follows based on the motion model and pose transformation relationship:

[0054]

[0055] In the above two formulas, p h-1 , R h-1 、v h-1 ,ω h-1 and a h-1 represents the position, attitude, velocity, angular velocity and acceleration of the h-1th normal frame in the map coordinate system, p h and R h represents the position and posture of the h-th ordinary frame in the map coordinate system; Δt represents the time difference between the two frames, R(θ) represents the rotation transformation from the h-1-th ordinary frame to the h-th ordinary frame; I represents the unit matrix, θ represents the rotation vector, and K represents the antisymmetric matrix of the rotation vector.

[0056] S10: optimizing the objective function of some common frames in the sliding window by using a nonlinear optimization algorithm, updating the poses of all optimized common frames in the sliding window, and then executing S11;

[0057] S11: Marginalize the oldest keyframe in the sliding window, delete all weak keyframes between the oldest keyframe and the first strong keyframe after it; if there is no strong keyframe after the oldest frame in the sliding window, but only a few keyframes, keep the latest 5 weak keyframes and delete the other weak keyframes. Update the local map, remove the point cloud corresponding to the deleted keyframe from the local map, reconstruct the constraint relationship in the sliding window, and then execute S12;

[0058] For the content of S11, marginalizing the oldest key frame refers to deleting the IMU constraint relationship and the lidar constraint relationship corresponding to the oldest key frame in the sliding window, and retaining the relative motion constraint relationship between the oldest key frame and other key frames in the sliding window. The present invention also deletes the weak key frame after the oldest key frame to achieve the maximum number of strong key frames in the sliding window during each optimization. Therefore, when the sliding window is updated, the number of key frames that slide out is not fixed, so the number of key frames in the sliding window is dynamically variable, that is, the size of the sliding window is dynamically variable.

[0059] S12: Determine whether the optimization thread of the SLAM system backend is terminated. If not, return to S1; if terminated, end.

[0060] The beneficial effects of the invention are as follows:

[0061] The present invention proposes a dynamic sliding window optimization method for a 3D lidar inertial SLAM system, aiming to improve the performance of the SLAM system in complex motion states of the robot.

[0062] The present invention selects key frames according to displacement difference and rotation difference, and further divides key frames into strong key frames and weak key frames. When the window slides, a dynamic optimization strategy of multiple sliding in and out of strong and weak key frames is designed, and combined with the dual standards of whether the window meets the optimization conditions, the SLAM system is given the ability to dynamically adjust the window size according to different motion states of the robot. This helps to improve the adaptability of the SLAM system in the case of complex robot motion, and can also ensure the real-time processing capability of sliding window optimization and reduce data redundancy in the sliding window.

[0063] Furthermore, the present invention constructs relative motion constraint factors of some common frames in the sliding window based on the optimized key frames, which are used to optimize the posture of the common frames in the sliding window. The optimization of key frames in the sliding window can provide a more accurate optimization benchmark for the optimization of common frames, and the optimization of some common frames in the sliding window can provide more reliable marginalization prior information for the optimization of the next key frame, thereby improving the positioning accuracy of the robot in complex motion conditions and reducing the drift of the robot's posture estimation in complex motion conditions. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] Figure 1 A flowchart of a dynamic sliding window optimization method for a 3D laser radar inertial SLAM system described in a specific implementation manner;

[0065] Figure 2 A schematic diagram of the dynamic sliding window sliding update of the 3D LiDAR inertial SLAM system;

[0066] Figure 3 Schematic diagram of the sliding window keyframe factor graph optimization model for the 3D LiDAR inertial SLAM system;

[0067] Figure 4 Schematic diagram of the optimization model of some common frame factor graphs in the sliding window of the 3D lidar inertial SLAM system;

[0068] Figure 5 A schematic diagram of the dynamic sliding window update of the 3D LiDAR inertial SLAM system;

[0069] Figure 6 A schematic diagram showing the trajectory comparison between a dynamic sliding window optimization method for 3D lidar inertial SLAM system and LIO-SAM on the KITTI 07 dataset;

[0070] Figure 7 A schematic diagram showing the comparison of the X, Y, and Z axis errors of a dynamic sliding window optimization method for a 3D lidar inertial SLAM system and the LIO-SAM trajectory on the KITTI 07 dataset.

[0071] Figure 8 Comparison of the APE error of the trajectories estimated by LIO-SAM and our-LIO-SAM on the KITTI 07 dataset under different error evaluation indicators. DETAILED DESCRIPTION

[0072] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, rather than all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work belong to the scope of protection of the present invention. It should be noted that the embodiments of the present invention and the features in the embodiments can be combined with each other without conflict.

[0073] Reference Figures 1 to 8 Specifically describing this embodiment, a dynamic sliding window optimization method for a 3D laser radar inertial SLAM system described in this embodiment includes the following steps:

[0074] S1: The SLAM system receives the point cloud data collected by the 3D laser radar, as well as the acceleration data and angular velocity data collected by the IMU, extracts the feature information of the point cloud from the received laser radar data, including corner points and surface points, and then executes S2;

[0075] S2: Obtain the relative transformation relationship between the two frames by matching the laser radar point cloud of the current frame with the laser radar point cloud of the previous key frame, including the displacement difference and the rotation difference, and then execute S3;

[0076] S3: Determine whether the displacement difference or rotation difference obtained in S2 reaches the corresponding threshold to determine whether the current frame is a key frame. If not, retain the IMU relative transformation relationship and return to S1. If so, execute S4;

[0077] For the content of S3, including the key frame selection strategy of the present invention, when the displacement difference or rotation difference from the previous key frame to the current frame reaches the corresponding threshold, the current frame is judged to be a key frame, the displacement threshold is set to 1m, and the rotation threshold is set to 10 ° After the key frames are selected by this method, they are further subdivided into strong key frames and weak key frames. The judgment criteria of strong key frames and weak key frames are given below.

[0078] Strong keyframe judgment criteria: If the displacement difference reaches the displacement threshold and the rotation difference is not zero, the keyframe is marked as a strong keyframe; if the displacement difference and rotation difference both reach the corresponding thresholds, the keyframe is marked as a strong keyframe; if the rotation difference reaches the rotation threshold and the displacement difference is not zero, the keyframe is marked as a strong keyframe. Weak keyframe judgment criteria: If the rotation difference reaches the rotation threshold but the displacement difference is zero, the keyframe is marked as a weak keyframe; if the displacement difference reaches the displacement threshold but the rotation difference is zero, the keyframe is marked as a weak keyframe.

[0079] S4: Determine whether the sliding window is full. If the sliding window is not full, execute S5. If the sliding window is full, execute S6.

[0080] For the content of S4, there are two criteria for judging whether the sliding window is full: first, whether the accumulated displacement of the robot movement from the oldest key frame to the latest key frame in the sliding window reaches a preset distance threshold, which is set to 10m in the present invention; second, whether the number of key frames in the sliding window reaches a maximum number threshold, which is 50. If any of these two conditions is met, it can be judged that the sliding window is full.

[0081] S5: Add the latest key frame to the sliding window, transform the point cloud of the latest key frame to the map coordinate system and add it to the local map, and then return to S1;

[0082] For the content in S5, the global map coordinate system of the present invention is established based on the coordinate system of the first IMU frame. The local map corresponds to the point cloud of all key frames in the sliding window. The local map is part of the global map and shares the coordinate system reference. Figure 2 A schematic diagram of the dynamic sliding window sliding-in update of a 3D lidar inertial SLAM system is given. It should be noted that, taking into account the complex motion state of the robot, if the robot rotates in place, then according to the judgment standard of strong and weak key frames of the present invention, the sliding window only contains weak key frames, and these weak key frames have no cumulative displacement, then the sliding window cannot always meet the optimization condition of the distance threshold, resulting in a large number of key frames accumulated in the sliding window and unable to be optimized. In order to avoid this situation, the present invention stipulates a maximum limit on the number of key frames in the sliding window.

[0083] S6: Use linear interpolation to ensure the timestamp synchronization between the IMU frame and the lidar keyframe, and then execute S7;

[0084] S7: Construct a lidar constraint factor based on the matching relationship between the feature points of the local map point cloud and the feature points of the key frame point cloud in the sliding window; construct an IMU pre-integration constraint factor based on the IMU pre-integration theory; construct a marginalization prior constraint factor based on the marginalization processing result of the updated sliding window. Add the lidar constraint factor, IMU pre-integration constraint factor and marginalization prior constraint factor to the sliding window key frame factor graph optimization model to construct a sliding window key frame optimization objective function, and then execute S8;

[0085] For the content of S7, the three constraint factors are constructed as a sliding window keyframe factor graph optimization model, and then the optimization objective function is established. By minimizing the sum of the lidar constraint residual, the IMU pre-integrated constraint residual, and the marginalized prior constraint residual, the maximum a posteriori probability estimate of the state estimation is obtained. Figure 3 A schematic diagram of the sliding window keyframe factor graph optimization model constructed by three constraint factors is given.

[0086] The state variables of all key frames in the sliding window are expressed as χ = [x0, x1, ..., x n ], the total number of all key frames in the sliding window is n+1, and its number will not exceed the maximum key frame number threshold set by the sliding window. The sliding window key frame optimization objective function expression is as follows:

[0087]

[0088] Among them, e l (x k ,m)、e B (x k-1 ,x k ) and e M They represent the lidar constraint residual, IMU pre-integrated constraint residual and marginalized prior constraint residual respectively, and m is the local map point cloud set corresponding to the current sliding window. For the kth key frame, k∈[0,n], the state vector in the map coordinate system can be expressed as and are the position, velocity and posture of the kth key frame respectively, and are the accelerometer bias and gyroscope bias of the kth key frame respectively.

[0089] The laser radar constraint factor is constructed by the corner point residual and surface point residual of the point cloud. The corner point residual refers to the matching error between the corner point in the latest key frame laser point cloud and the line feature of the local map, and the surface point residual refers to the matching error between the surface point in the latest key frame laser point cloud and the plane feature of the local map. In the present invention, the state variable to be optimized of the laser radar constraint factor is l It is expressed as:

[0090]

[0091] in, is the position of the kth key frame in the map coordinate system, is the attitude of the kth key frame in the map coordinate system. LiDAR constraint residual e l (x k ,m) is as follows:

[0092] e l (x k ,m)=r1+r2 (3)

[0093] Among them, r1 represents the corner point residual of the laser radar, and its expression is:

[0094]

[0095] in, is the corresponding corner point coordinate in the k-th frame laser radar coordinate system, is the coordinate representation of the corner point coordinates of the kth frame in the map coordinate system, and Is the local map The coordinates of the two matching corner points, represents the cumulative sum of all corner point residuals in the kth frame, and ||·|| represents the modulus operation.

[0096] r2 represents the surface point residual of the laser radar, and its expression is:

[0097]

[0098] in, is the corresponding surface point coordinate in the k-th frame laser radar coordinate system, is the coordinate representation of the surface point coordinates of the kth frame in the map coordinate system, n k Is the local map The normal vector of the matching plane, represents the cumulative sum of all surface point residuals in the kth frame, and ||·|| represents the modulus operation.

[0099] The IMU pre-integration constraint factor is used to describe the relative position change relationship of the IMU measurement information between the time difference of two key frames. i Time and t j The time is aligned with the k-1th frame and the kth frame of the lidar key frame, and the coordinate system is aligned with the map coordinate system. The variable to be optimized of the IMU pre-integration factor is expressed as:

[0100]

[0101] in, and t i Time and t j The position of the key frame at the moment, and t i Time and t j The rotation amount of the keyframe at the moment, and They are t i Time and t j The speed of the keyframe at the moment, and t i Time and t j The accelerometer bias at the key frame at time, and t i Time and tj The gyroscope bias of the key frame at time t i Time and t j The moments are aligned with the k-1th frame and the kth frame of the lidar key frame respectively.

[0102] t j The position of the keyframe in the map coordinate system at this moment speed and posture Obtained by the following formula:

[0103]

[0104] Where Δt is t i Time and t j The time interval between moments, t i ≤t≤t j , is the rotation matrix in the map coordinate system, indicating the posture, g m is the gravity vector in the map coordinate system, δt is the time interval between two adjacent IMU measurement data, is the IMU at time t relative to t i The amount of rotation at a moment, and are the accelerometer bias and gyroscope bias of the IMU at time t, and Respectively represent the acceleration and angular velocity output by the IMU at time t, and Respectively represent the acceleration noise and angular velocity noise of the IMU measurement information at time t, and assume that they are white noises that obey Gaussian distribution. Transform the above equation and multiply both sides of the equations for position, velocity, and rotation by We can get:

[0105]

[0106] in, That is, the IMU pre-integrated variables of position, velocity, and attitude, and the expressions are as follows:

[0107]

[0108] It can be seen that the t j The state of the key frame at time t is only related to the measurement value of the IMU and will not be affected by the measurement value of the key frame at time t. i The influence of the key frame state quantity at the moment is eliminated, thereby ensuring that the measurement value is independent of the state quantity and avoiding repeated integration. Then there is the IMU pre-integration constraint residual e B (x k-1 ,x k) is expressed as follows:

[0109]

[0110] Among them, r p 、r q 、r v , t i Time to t j The position residual, attitude residual, velocity residual, accelerometer residual and gyroscope residual of the key frame at the moment t i Time and t j The times are aligned with the k-1th frame and the kth frame of the lidar key frame, respectively. is the quaternion multiplication symbol, [·] xyz It means taking the imaginary part of the quaternion to form a three-dimensional vector. t i The rotation amount of the key frame in the map coordinate system at the moment, indicating the posture, From t j Time to t i The rotation amount for the keyframe at the moment.

[0111] The marginalization prior constraint factor is the constraint relationship that is retained after removing the marginalized keyframes when updating the sliding window. The least squares expression is constructed using the keyframe factor graph optimization model, and the state vector χ is set as a node. The marginalized state vector χ d The pose information in the map points and the constraint relationship are added to the retained state vector χ in the form of prior information. s In the optimization relationship of , the loss of constraint information corresponding to the key frames that are marginalized after updating the sliding window is avoided. After marginalization, the prior matrix H p and the corresponding prior residual b p , which is consistent with the marginalization expression in S11, which prior constrains the residual e M The expression is:

[0112] e M =b p -H p δχ 1:N (11)

[0113] Among them, δχ 1:N is the state increment of all key frames in the sliding window,

[0114]

[0115] Among them, H a is the state vector δχ to be marginalized d The covariance matrix, Hc is the retained state vector δχ s The covariance matrix of Indicates H b The transpose of H b is δχ d With δχ s The covariance matrix between d and b s are δχ d and δχ s A constant vector of .

[0116] S8: Optimize the sliding window key frame optimization objective function using a nonlinear optimization algorithm (such as the Gauss-Newton method, the Levenberg-Marquadt method, etc.), update the positions of all key frames in the sliding window, complete the key frame sliding window optimization, and then execute S9.

[0117] Taking the Levenberg-Marquadt method as an example, by setting a confidence region for the state increment, the approximation failure caused by large changes in the state increment is avoided, and the coefficient matrix of the linear equation system is avoided. The non-singular and ill-conditioned problems can be avoided, so a more stable and accurate state increment can be provided. The objective function is constructed in the form of a state increment equation:

[0118] (H h +μI)Δχ=g (13)

[0119] Among them, H h is the Hessian matrix, μ is the Lagrange multiplier, and the accuracy of the nonlinear quadratic approximation model is reflected by adjusting the size of the parameter μ, and then the optimal solution is obtained by iterative optimization.

[0120] S9: Take the oldest frame in the sliding window and the oldest strong key frame other than the oldest frame as the reference frames. If there are several weak key frames between the two reference frames, these weak key frames are also used as reference frames. If there are no strong key frames other than the oldest frame in the sliding window, and all are weak key frames, then all weak key frames in the sliding window are used as reference frames. Construct relative motion constraint factors of all common frames between these reference frames, add all relative motion constraint factors to the optimization model of the factor graph of common frames in the sliding window, thereby establishing a target optimization function for nonlinear optimization, and then execute S10.

[0121] For the content of S9, the optimization of a part of the common frames in the sliding window is intended to provide more reliable marginalization prior information for the optimization of the next sliding window key frame. Figure 4 A schematic diagram of the optimization model of the common frame factor graph constructed by the relative motion constraint factors of a part of the common frames in the sliding window is given. Figure 4In , the case where there is a weak key frame between the oldest frame in the sliding window and the first strong key frame thereafter is given. According to the factor graph optimization model of some common frames in the sliding window, the optimization objective function of common frames is established. By minimizing the sum of the relative motion constraint residuals, the maximum a posteriori probability estimate of the state is obtained. Let the state variables of all common frames to be optimized in the sliding window be χ o , and there is χ o =[x0,x1,...,x H ], H+1 is the total number of all common frames to be optimized in the sliding window, and the objective function expression of the common frame is as follows:

[0122]

[0123] Among them, e l (x h-1 ,x h ) represents the relative motion constraint residual of the ordinary frame. Let the state vector of the h-th ordinary frame in the map coordinate system be and are the position and posture of the hth normal frame respectively.

[0124] The relative motion constraint residual e in the objective function of the normal frame l (x h-1 ,x h ) is expressed as:

[0125]

[0126] in, and They represent the rotation and translation transformation from the hth ordinary frame to the h-1th ordinary frame, respectively, which are obtained by ICP matching of the feature information between the two frames of laser point clouds; and They represent the postures of the h-1th normal frame and the hth normal frame in the map coordinate system respectively. and Respectively represent the positions of the h-1th normal frame and the hth normal frame in the map coordinate system, [·] xyz It means that the imaginary part of the quaternion is used to form a three-dimensional vector. The posture and position of the ordinary frame are recursively obtained from the posture of the adjacent benchmark key frame. If the ordinary frame to be optimized is closer to the previous benchmark key frame than the next benchmark key frame, it is recursively obtained from the posture of the previous benchmark key frame, and vice versa.

[0127] If the normal frame to be optimized is closer to its previous reference key frame, its pose recursion is based on the pose of the previous frame and is recursively obtained according to the motion model and pose transformation relationship. The expression is as follows:

[0128]

[0129] If the normal frame to be optimized is closer to the next benchmark key frame, its pose is recursively deduced based on the pose of the next frame, and the expression is as follows based on the motion model and pose transformation relationship:

[0130]

[0131] In equations (16) and (17), p h-1 , R h-1 、v h-1 ,ω h-1 and a h-1 represents the position, attitude, velocity, angular velocity and acceleration of the h-1th normal frame in the map coordinate system, p h and R h represents the position and posture of the hth ordinary frame in the map coordinate system; Δt represents the time difference between two frames, R(θ) represents the rotation transformation from the h-1th ordinary frame to the hth ordinary frame; I represents the unit matrix, θ represents the rotation vector, θ x ,θ y and θ z are the Euler angles of the x-axis, y-axis, and z-axis respectively; K represents the antisymmetric matrix of the rotation vector. The expression relationship between the rotation vector and its antisymmetric matrix is ​​as follows:

[0132]

[0133] S10: optimizing the objective function of some common frames in the sliding window by using a nonlinear optimization algorithm, updating the poses of all optimized common frames in the sliding window, and then executing S11;

[0134] S11: Marginalize the oldest keyframe in the sliding window, delete all weak keyframes between the oldest keyframe and the first strong keyframe after it; if there is no strong keyframe after the oldest frame in the sliding window, but only weak keyframes, keep the latest 5 weak keyframes and delete other weak keyframes. Update the local map, remove the point cloud corresponding to the deleted keyframe from the local map, reconstruct the constraint relationship in the sliding window, and then execute S12;

[0135] For the content of S11, marginalizing the oldest key frame refers to deleting the IMU constraint relationship and the lidar constraint relationship corresponding to the oldest key frame in the sliding window, and retaining the relative motion constraint relationship between the oldest key frame and other key frames in the sliding window. The present invention also deletes the weak key frame after the oldest key frame to achieve the maximum number of strong key frames in the sliding window during each optimization. Therefore, when the sliding window is updated, the number of key frames that slide out is not fixed, so the number of key frames in the sliding window is dynamically variable, that is, the size of the sliding window is dynamically variable. Figure 5A schematic diagram of the dynamic sliding window update of the 3D lidar inertial SLAM system is given.

[0136] The marginalization of the oldest frame in the sliding window is usually implemented by the Schur complement method. The state variables χ of all key frames in the sliding window are constructed in the sliding window with the least squares expression and simplified to the form of Hδχ=b, where H is the system covariance matrix, δχ is the increment of the state quantity, and b is a constant vector. The state variable χ is divided into two categories, namely, the state quantity δχ to be marginalized d And the state quantity δχ that needs to be retained s . Split the system covariance matrix H into four block matrices H a , H c , H b , H a is δχ d The covariance matrix of H is the marginalized variable; c is δχ s The covariance matrix of , that is, the covariance matrix of the retained state variables; Indicates H b The transpose of H b is δχ d With δχ s The covariance matrix between them. The state variable χ is expressed as an incremental equation as follows:

[0137]

[0138] Among them, b d and b s are δχ d and δχ s According to the Schur complement theory, without calculating the marginalized state quantity δχ d In the case of s , and perform Gaussian elimination to obtain:

[0139]

[0140] in, Indicates H a In H b According to the above formula, we can deduce δχ s for:

[0141]

[0142] The state quantity is only δχ s, so that the constraint information of the marginalized keyframes can be retained, and the state quantity of the marginalized keyframes can be eliminated without losing useful information. In the marginalization process, although the state variables are constantly updated, the linearization point is fixed by optimizing the consistency strategy, that is, when calculating the marginalized Jacobian matrix, the value of the derivative variable is fixed, instead of using the state variables updated after each iteration to calculate the Jacobian, so as to ensure the unobservable dimension of the system and avoid changes in the null space of the covariance matrix.

[0143] S12: Determine whether the optimization thread of the SLAM system backend is terminated. If not, return to S1; if terminated, end.

[0144] This implementation method selects key frames according to displacement difference and rotation difference, and further divides the key frames into strong key frames and weak key frames. When the window slides, a dynamic optimization strategy of multiple sliding in and out of strong and weak key frames is designed, and combined with the dual standards of whether the window meets the optimization conditions, the SLAM system is given the ability to dynamically adjust the window size according to different motion states of the robot, which helps to improve the adaptability of the SLAM system in complex robot motion conditions, and at the same time can also ensure the real-time processing capability of sliding window optimization and reduce data redundancy in the sliding window; the present invention constructs relative motion constraint factors of ordinary frames based on key frames after sliding window optimization, which are used to optimize the posture of some ordinary frames in the sliding window. The optimization of key frames can provide a more accurate optimization benchmark for the optimization of ordinary frames, and the optimization of some ordinary frames in the sliding window can provide more reliable marginalization prior information for the next key frame optimization, thereby improving the positioning accuracy of the robot in complex motion conditions and reducing the drift of the robot's posture estimation in complex motion conditions.

[0145] Implementation and verification of the present invention:

[0146] The dynamic sliding window optimization method for a 3D laser radar inertial SLAM system proposed in the present invention is implemented using C++ language programming, and the beneficial effects of the present invention are verified by evaluating the accuracy of the motion trajectory of the mobile robot estimated by the SLAM system. Based on the 3D laser radar inertial SLAM system-LIO-SAM framework, the SLAM system using the improved sliding window optimization method proposed in the present invention is named our-LIO-SAM. The present invention uses the open source KITTI dataset as a verification dataset. The KITTI dataset was jointly founded by the Karlsruhe Institute of Technology in Germany and Toyota America Technical Research Institute, and is currently the largest evaluation dataset for autonomous driving scenarios in the world. The data acquisition platform of the KITTI dataset is equipped with 2 grayscale cameras, 2 color cameras, a Velodyne 64-line 3D laser radar, 4 optical lenses, and 1 GPS navigation system.

[0147] Take the KITTI 07 data subset in the KITTI dataset as an example. The KITTI 07 data subset is road scene information collected during the driving of the car. In this data subset, the car driving on the road includes various complex motion states such as acceleration and deceleration, speed change and turning, braking and obstacle avoidance, up and down road breaking, meeting and overtaking. LIO-SAM and our-LIO-SAM are used to estimate the movement trajectory of the car in the dataset respectively, and then different evaluation indicators are used to evaluate the accuracy of the two trajectories. Figure 6 The plane trajectory diagram of the car estimated by LIO-SAM and our-LIO-SAM on the KITTI 07 dataset is given and compared with the true trajectory value. It can be seen from the figure that the trajectory of our-LIO-SAM has a higher degree of overlap with the true trajectory value; Figure 7 Given Figure 6 The relationship between the displacement change of the car trajectory on the three coordinate axes of X, Y, and Z and time. From the figure, it can be seen that the estimates of our-LIO-SAM and LIO-SAM on the X and Y axes have a high degree of coincidence with the true values, and there is a drift on the Z axis. However, the offset of our-LIO-SAM on the Z axis is significantly smaller than that of LIO-SAM; Figure 8 The following is a comparison of the APE (absolute pose error index) error of the trajectories estimated by LIO-SAM and our-LIO-SAM on the KITTI 07 dataset under different error evaluation indicators. It can be seen from the figure that the APE error of our-LIO-SAM under the min (minimum error), max (maximum error value), std (error standard deviation), median (error median), mean (error mean), and rmse (root mean square error) indicators is smaller than that of LIO-SAM, which indicates that our-LIO-SAM has improved in accuracy, stability, and overall performance, and estimates the trajectory more accurately. Overall, these results show that the improvements in positioning accuracy and robustness of our-LIO-SAM are effective.

[0148] In summary, the present invention proposes a dynamic sliding window optimization method for a 3D laser radar inertial SLAM system, aiming to improve the performance of the system in complex robot motion states. The present invention selects key frames according to displacement difference and rotation difference, and further divides the key frames into strong key frames and weak key frames. When the window slides, a dynamic optimization strategy of multiple sliding in and out of strong and weak key frames is designed, and combined with the dual standards of whether the window meets the optimization conditions, the SLAM system is given the ability to dynamically adjust the window size according to different motion states of the robot; in addition, the present invention constructs the relative motion constraint factor of the ordinary frame based on the optimized key frame, which is used to optimize the posture of the ordinary frame in the sliding window. The optimization of the key frame can provide a more accurate optimization benchmark for the optimization of the ordinary frame, and the optimization of some ordinary frames in the sliding window can provide more reliable marginalization prior information for the optimization of the next key frame, thereby improving the positioning accuracy of the robot in complex motion conditions and reducing the drift of the posture estimation of the robot in complex motion conditions.

[0149] Although the present invention is described herein with reference to specific embodiments, it should be understood that these embodiments are merely examples of the principles and applications of the present invention. It should therefore be understood that many modifications may be made to the exemplary embodiments and that other arrangements may be devised without departing from the spirit and scope of the present invention as defined by the appended claims. It should be understood that the various dependent claims and features described herein may be combined in a manner different from that described in the original claims. It should also be understood that the features described in conjunction with a single embodiment may be used in other described embodiments.

Claims

1. A dynamic sliding window optimization method for a 3D laser radar inertial SLAM system, characterized in that The following steps are involved: S1: The SLAM system receives the point cloud data collected by the 3D laser radar, as well as the acceleration data and angular velocity data collected by the IMU, extracts the feature information of the point cloud from the received laser radar data, including corner points and surface points, and then executes S2; S2: Obtain the relative transformation relationship between the two frames by matching the laser radar point cloud of the current frame with the laser radar point cloud of the previous key frame, including the displacement difference and the rotation difference, and then execute S3; S3: Determine whether the displacement difference or rotation difference obtained in S2 reaches the corresponding threshold to determine whether the current frame is a key frame. If not, retain the IMU relative transformation relationship and return to S1. If so, execute S4; For the content of S3, including the key frame selection strategy of the present invention, when the displacement difference or rotation difference from the previous key frame to the current frame reaches the corresponding threshold, the current frame is judged to be a key frame. After the key frames are selected by this method, the key frames are further subdivided into strong key frames and weak key frames. The judgment criteria of strong key frames and weak key frames are given below. Strong keyframe judgment criteria: If the displacement difference reaches the displacement threshold and the rotation difference is not zero, the keyframe is marked as a strong keyframe; if the displacement difference and rotation difference both reach the corresponding thresholds, the keyframe is marked as a strong keyframe; if the rotation difference reaches the rotation threshold and the displacement difference is not zero, the keyframe is marked as a strong keyframe. Weak keyframe judgment criteria: If the rotation difference reaches the rotation threshold but the displacement difference is zero, the keyframe is marked as a weak keyframe; if the displacement difference reaches the displacement threshold but the rotation difference is zero, the keyframe is marked as a weak keyframe. S4: Determine whether the sliding window is full. If the sliding window is not full, execute S5. If the sliding window is full, execute S6. For the content of S4, there are two criteria for judging whether the sliding window is full: first, whether the accumulated displacement of the robot from the oldest key frame to the latest key frame in the sliding window reaches the preset distance threshold; second, whether the number of key frames in the sliding window reaches the maximum number threshold. If either of these two conditions is met, it can be judged that the sliding window is full. S5: Add the latest key frame to the sliding window, transform the point cloud of the latest key frame to the map coordinate system and add it to the local map, and then return to S1; For the content in S5, the global map coordinate system of the present invention is established based on the coordinate system of the first IMU frame. The local map corresponds to the point cloud of all key frames in the sliding window. The local map is part of the global map and shares the coordinate system reference. S6: Use linear interpolation to ensure the timestamp synchronization between the IMU frame and the lidar keyframe, and then execute S7; S7: Construct a lidar constraint factor based on the matching relationship between the feature points of the local map point cloud and the feature points of the key frame point cloud in the sliding window; construct an IMU pre-integration constraint factor based on the IMU pre-integration theory; construct a marginalization prior constraint factor based on the marginalization processing result of the updated sliding window. Add the lidar constraint factor, IMU pre-integration constraint factor and marginalization prior constraint factor to the sliding window key frame factor graph optimization model to construct a sliding window key frame optimization objective function, and then execute S8; For the content of S7, the state variable x to be optimized of the laser radar constraint factor l for: in, is the position of the kth key frame in the map coordinate system, It is the attitude value of the kth key frame in the map coordinate system. For the content of S7, the variable to be optimized x of the IMU pre-integration constraint factor imu for: in, and t i Time and t j The position of the key frame at the moment, and t i Time and t j The rotation amount of the keyframe at the moment, and They are t i Time and t j The speed of the keyframe at the moment, and t i Time and t j The accelerometer bias at the key frame at time, and t i Time and t j The gyroscope bias of the key frame at time t i Time and t j The moments are aligned with the k-1th frame and the kth frame of the lidar key frame respectively. For the content of S7, the sliding window key frame optimization objective function expression is as follows: Among them, χ is the state variable of all key frames in the sliding window, and χ=[x0,x1,...,x n ], the total number of all key frames in the sliding window is n+1, e l (x k ,m)、e B (x k-1 ,x k ) and e M They represent the lidar constraint residual, IMU pre-integrated constraint residual and marginalized prior constraint residual respectively, and m is the local map point cloud set corresponding to the current sliding window. For the kth key frame, k∈[0,n], the state vector in the map coordinate system can be expressed as and are the position, velocity and posture of the kth key frame respectively, and are the accelerometer bias and gyroscope bias of the kth key frame respectively. For the content of S7, the lidar constraint residual e l (x k ,m)The expression is as follows: e l (x k ,m)=r1+r2 (4) Among them, r1 represents the corner point residual of the laser radar, and its expression is: in, is the corresponding corner point coordinate in the k-th frame laser radar coordinate system, is the coordinate representation of the corner point coordinates of the kth frame in the map coordinate system, and Is the local map The coordinates of the two matching corner points, represents the cumulative sum of all corner point residuals in the kth frame, and ||·|| represents the modulus operation. r2 represents the surface point residual of the laser radar, and its expression is: in, is the corresponding surface point coordinate in the k-th frame laser radar coordinate system, is the coordinate representation of the surface point coordinates of the kth frame in the map coordinate system, n k Is the local map The normal vector of the matching plane, represents the cumulative sum of all surface point residuals in the kth frame, and ||·|| represents the modulus operation. For the content of S7, the IMU pre-integration constraint residual e B (x k-1 ,x k ) is expressed as: Among them, r p 、r q 、r v , They are the position residual, attitude residual, velocity residual, accelerometer residual and gyroscope residual from the k-1th key frame to the kth key frame respectively. For the content of S7, the marginalized prior constrained residual e M The expression is: e M =b p -H p δχ 1:N (8) Among them, δχ 1:N is the state increment of all key frames in the sliding window, Among them, H a is the state vector δχ to be marginalized d The covariance matrix, H c is the retained state vector δχ s The covariance matrix of Indicates H b The transpose of H b is δχ d With δχ s The covariance matrix between d and b s are δχ d and δχ s A constant vector of . S8: Optimize the sliding window key frame optimization objective function using a nonlinear optimization algorithm (such as the Gauss-Newton method, the Levenberg-Marquadt method, etc.), update the positions of all key frames in the sliding window, complete the key frame sliding window optimization, and then execute S9. S9: Take the oldest frame in the sliding window and the oldest strong key frame other than the oldest frame as the reference frames. If there are several weak key frames between the two reference frames, these weak key frames are also used as reference frames. If there are no strong key frames other than the oldest frame in the sliding window, and all are weak key frames, then all weak key frames in the sliding window are used as reference frames. Construct relative motion constraint factors of all common frames between these reference frames, add all relative motion constraint factors to the optimization model of the factor graph of common frames in the sliding window, thereby establishing a target optimization function for nonlinear optimization, and then execute S10. For the content of S9, the optimization of a part of the ordinary frames in the sliding window aims to provide more reliable marginalization prior information for the optimization of the next sliding window key frame. According to the factor graph optimization model of some ordinary frames in the sliding window, the optimization objective function of ordinary frames is established. By minimizing the sum of the relative motion constraint residuals, the maximum posterior probability estimate of the state is obtained. The state variables of all ordinary frames to be optimized in the sliding window are set as χ o , and there is χ o =[x0,x1,...,x H ], H+1 is the total number of all common frames to be optimized in the sliding window, and the objective function expression of the common frame is as follows: Among them, e l (x h-1 ,x h ) represents the relative motion constraint residual of the ordinary frame. Let the state vector of the h-th ordinary frame in the map coordinate system be and are the position and posture of the hth normal frame respectively. For the content of S9, the relative motion constraint residual e of the normal frame l (x h-1 ,x h ) is expressed as: in, and They represent the rotation and translation transformation from the hth ordinary frame to the h-1th ordinary frame, respectively, which are obtained by ICP matching of the feature information between the two frames of laser point clouds; and They represent the postures of the h-1th normal frame and the hth normal frame in the map coordinate system respectively. and Respectively represent the positions of the h-1th normal frame and the hth normal frame in the map coordinate system, [·] xyz It means that the imaginary part of the quaternion is used to form a three-dimensional vector. The posture and position of the ordinary frame are recursively obtained from the posture of the adjacent benchmark key frame. If the ordinary frame to be optimized is closer to the previous benchmark key frame than the next benchmark key frame, it is recursively obtained from the posture of the previous benchmark key frame, and vice versa. If the normal frame to be optimized is closer to its previous reference key frame, its pose recursion is based on the pose of the previous frame and is recursively obtained according to the motion model and pose transformation relationship. The expression is as follows: If the normal frame to be optimized is closer to the next benchmark key frame, its pose is recursively deduced based on the pose of the next frame, and the expression is as follows based on the motion model and pose transformation relationship: In the above two formulas, p h-1 , R h-1 、v h-1 ,ω h-1 and a h-1 represents the position, attitude, velocity, angular velocity and acceleration of the h-1th normal frame in the map coordinate system, p h and R h represents the position and posture of the h-th ordinary frame in the map coordinate system; Δt represents the time difference between the two frames, R(θ) represents the rotation transformation from the h-1-th ordinary frame to the h-th ordinary frame; I represents the unit matrix, θ represents the rotation vector, and K represents the antisymmetric matrix of the rotation vector. S10: optimizing the objective function of some common frames in the sliding window by using a nonlinear optimization algorithm, updating the poses of all optimized common frames in the sliding window, and then executing S11; S11: Marginalize the oldest keyframe in the sliding window, delete all weak keyframes between the oldest keyframe and the first strong keyframe after it; if there is no strong keyframe after the oldest frame in the sliding window, but only a few keyframes, keep the latest 5 weak keyframes and delete the other weak keyframes. Update the local map, remove the point cloud corresponding to the deleted keyframe from the local map, reconstruct the constraint relationship in the sliding window, and then execute S12; For the content of S11, marginalizing the oldest key frame refers to deleting the IMU constraint relationship and the lidar constraint relationship corresponding to the oldest key frame in the sliding window, and retaining the relative motion constraint relationship between the oldest key frame and other key frames in the sliding window. The present invention also deletes the weak key frame after the oldest key frame to achieve the maximum number of strong key frames in the sliding window during each optimization. Therefore, when the sliding window is updated, the number of key frames that slide out is not fixed, so the number of key frames in the sliding window is dynamically variable, that is, the size of the sliding window is dynamically variable. S12: Determine whether the optimization thread of the SLAM system backend is terminated. If not, return to S1; if terminated, end.

Images