Joint initialization method of multi-sensor SLAM system for 3D laser radar visual inertia

Abstract

The invention provides an initialization method of an SLAM system for multi-sensor fusion, relates to three sensors of a 3D laser radar, a camera and an IMU, and aims to improve the accuracy and robustness of initialization of the SLAM system. The SLAM initialization process is divided into five modules, a multivariable optimization problem is converted into a multi-batch optimization problem, multivariable optimization is performed from easy to difficult, convergence difficulty is reduced, convergence speed is increased, and the position, attitude, speed and IMU offset of each key frame and deviation between the Z axis of a world coordinate system and the gravity direction are estimated progressively; converting a multi-variable multi-batch optimization problem into a joint optimization problem, taking a batch optimization result as a variable initial value, and improving the precision of a state estimation result through joint optimization; a gravity direction deviation optimization module is designed, and it is guaranteed that the deviation between a world coordinate system and the gravity direction can still be corrected under the condition that inertial parameters do not have good observability due to slow movement of the robot.

Description

Technical Field

[0001] The present invention belongs to the field of SLAM (Simultaneous Localization and Mapping) technology, and relates to three sensors: 3D laser radar, camera and IMU, and in particular to a joint initialization method for a multi-sensor SLAM system for 3D laser radar visual inertia. Background Art

[0002] SLAM is a crucial technology for enabling autonomous navigation of unmanned systems in unknown environments. It is widely used in robotics, autonomous driving, augmented reality, and other fields. This technology enables mobile robots to achieve localization and mapping using their sensors without any prior information. SLAM initialization is a crucial step in system startup, laying the foundation for subsequent pose tracking and back-end optimization. The quality of SLAM initialization directly impacts the accuracy and robustness of subsequent localization and mapping, ultimately determining the success of these processes. Currently, SLAM technology is developing towards multi-source heterogeneous sensor fusion. This approach, by simultaneously acquiring and fusing information from multiple sensors, overcomes the limitations of single sensor acquisition. However, this also increases the difficulty of SLAM system initialization. Sensor intrinsic parameters can be estimated through self-calibration methods, such as the Zhang Zhengyou calibration method for camera intrinsic parameters. Extrinsic parameters between sensors can be estimated through joint calibration of multiple sensors, such as the LiDAR and IMU extrinsic parameters obtained using the Joint Calibration Toolbox.

[0003] In the field of SLAM research, there is no effective initialization scheme for the three sensors. In addition to the intrinsic parameters of the sensors and the extrinsic parameters between them, the initialization process also requires aligning the Z-axis of the world coordinate system established by the SLAM system with the direction of gravity. This problem can only be solved when the unmanned system is in motion to estimate the deviation in the direction of gravity in the world coordinate system. Furthermore, while the unmanned system is in motion, the SLAM initialization process must also estimate the bias of the IMU's accelerometer and gyroscope, as well as the scale of the monocular camera and the velocity, position, and attitude of the unmanned system at each keyframe sampling moment. With so many variables to be estimated, achieving both accuracy and speed in the initialization process is challenging. The core difficulty of multi-sensor SLAM initialization lies in the fact that the system must simultaneously complete a preliminary estimate of its own motion and a preliminary perception of the environment's structure without any prior information. This challenges it faces from the physical limitations of the sensors, the complexity of the environment, and the constraints of algorithmic theory. These difficulties make the design of a fast, robust, and automatic initialization scheme for various scenarios a key research topic. Summary of the Invention

[0004] The purpose of the present invention is to provide a fast and accurate initialization method for the existing multi-sensor fusion SLAM system. It involves three sensors: a 3D laser radar, an IMU, and a camera. Based on the factor graph optimization theory, the multi-sensor joint initialization is realized, so that the IMU obtains a good zero-bias initial value and the gyroscope zero-bias initial value, and the camera and 3D laser radar establish a world coordinate system with a unified scale, and the Z axis of the world coordinate system is pulled to a state parallel to the direction of gravity, thereby ensuring that subsequent multi-source data can be effectively integrated and improving the accuracy, speed and robustness of the positioning and mapping of the SLAM system.

[0005] The present invention includes five modules: a data preprocessing module, a visual laser radar combined maximum a posteriori estimation module, a pure inertial maximum a posteriori estimation module, a visual inertial laser radar combined tuning module, and a gravity direction deviation optimization module.

[0006] The data preprocessing module is used to receive and preprocess the data collected by each sensor, as well as select key frames and construct a point cloud map for initialization. By performing data-level fusion on the image data and the laser point cloud data, dynamic feature points in the image and the dynamic point cloud of the LiDAR are removed. The laser point cloud data is used as a depth reference to estimate the accurate depth values ​​of the static feature points in the image. Key frames are selected based on the distance the robot has moved and a key frame queue for initialization is established for subsequent initialization operations. The module includes the following steps:

[0007] S1.1: The SLAM system receives image data captured by the camera, point cloud data collected by the 3D lidar, and acceleration and angular velocity data collected by the IMU, synchronizes the timestamps of the image data, lidar data, and IMU data, and then executes S1.2.

[0008] S1.2: Use a classic image feature point extraction method (such as ORB, SIFT, etc.) to extract image feature points; use the camera distortion coefficient to perform camera imaging distortion correction on the extracted image feature points, and then execute S1.3;

[0009] S1.3: Remove outliers from the LiDAR point cloud, use IMU measurement data to correct motion distortion on the LiDAR point cloud, downsample the point cloud data using voxel filtering, and extract feature information from the point cloud data, including corner points and surface points, and then execute S1.4;

[0010] Regarding the content of S1.3, the use of IMU measurement data to correct motion distortion of the lidar point cloud means using the robot posture information calculated by the IMU within a lidar scanning frame time to accurately obtain the position information corresponding to the point cloud at a certain moment in the scanning frame time through the linear interpolation method, and then use the position information to correct the distortion of the point cloud at that moment caused by the robot motion.

[0011] For the content of S1.3, the point cloud curvature is calculated to extract the corner points and surface points of the point cloud. The simulation calculation expression of the point cloud curvature is as follows:

[0012]

[0013] Where c i is the curvature of point i, S is the set of points in the neighborhood of point i, |S| represents the number of points in set S, represents the distance from point i to the radar, Indicates the distance from the point to the lidar, Represents the distance from point j to the radar in the neighborhood of point i.

[0014] S1.4: Use the extrinsic matrix between the LiDAR and the camera to achieve data-level matching and alignment between the image and the laser point cloud. Perform 2D object detection on the image and 3D object detection on the laser point cloud to achieve object-level matching and alignment between the image and the laser point cloud. Then, remove dynamic feature points from the image and the laser point cloud, and then execute S1.5.

[0015] Regarding the content of S1.4, the target-level matching and alignment of image and point cloud data refers to performing 2D target detection on the image to generate an image mask of the dynamic object; performing 3D target detection on the point cloud data, and projecting the detection results onto the image normalization plane to generate a mask of the point cloud target object; by comparing the masks of the image and the point cloud, retaining the overlapping areas of the two and deleting the non-overlapping areas, objects covered by both masks are identified as dynamic objects, and feature points and point clouds covered by overlapping masks are identified as dynamic feature points and dynamic point clouds, which are removed from the feature point set of the image and the point cloud set of the lidar, respectively.

[0016] S1.5: Use the static feature points in the LiDAR point cloud auxiliary image to obtain accurate depth information, delete the static feature points that do not have a depth value, and then execute S1.6;

[0017] S1.6: Determine whether the keyframe queue is empty. If so, set the current frame as a keyframe and insert it into the keyframe queue. Then, execute S1.7. If not, perform ICP matching of feature points on the laser point cloud of the current frame and the laser point cloud of the previous keyframe to obtain the translation and rotation information between the two frames. Determine whether the current frame is a keyframe based on whether the obtained translation reaches the distance threshold. If so, insert the keyframe into the keyframe queue and execute S1.7. If not, return to S1.1.

[0018] For the content of S1.6, the ICP matching of feature points between the laser point cloud of the current frame and the laser point cloud of the previous key frame refers to the scan-to-scan pose estimation of the feature points of the point cloud between the current key frame and the point cloud data of the previous key frame. The target optimization function is constructed based on the corner point residuals and the surface point residuals. The corner point residuals refer to the matching errors from the corner points in the laser point cloud of the current frame to the line features of the laser point cloud of the previous key frame. The surface point residuals refer to the matching errors from the surface points in the laser point cloud of the current frame to the surface features of the laser point cloud of the previous key frame. Assume that the relative rotation matrix and relative translation vector between the current frame and the previous key frame are Δq and Δp respectively, and the target function is constructed based on the corner point residuals and surface point residuals of the point cloud. The expression is as follows:

[0019] e l (x k ,x′)=r1+r2 (2)

[0020] Where x′ and x k They represent the pose states of the current frame and the previous key frame respectively, r1 represents the corner residual of the lidar, and r2 represents the surface residual of the lidar.

[0021] For the corner residual, the specific expression is:

[0022]

[0023] Where, is the corresponding corner point in the current frame lidar coordinate system, It is the coordinate representation of the corner point of the current frame in the lidar coordinate system of the previous key frame. and is the value in the previous keyframe The coordinates of the two matching corner points, represents the cumulative sum of all corner residuals in the current frame, and ||·|| represents the modulo operation.

[0024] For the surface point residual, the specific expression is:

[0025]

[0026] Where p l is the corresponding surface point in the current frame laser radar coordinate system, p m is the coordinate representation of the surface point of the current frame in the lidar coordinate system of the previous key frame, and n is the coordinate of p in the previous key frame. m The normal vector of the matching plane, It represents the cumulative sum of all surface residuals in the current frame, and ||·|| represents the modulo operation.

[0027] S1.7: Determine whether the current keyframe is the first keyframe in the keyframe queue. If so, establish a world coordinate system based on the lidar coordinate system of the current keyframe as the reference coordinate system, and then execute S1.8. If not, transform the laser point cloud of the current keyframe to the world coordinate system, and establish an image feature point matching relationship between the current keyframe and the adjacent keyframe based on the visual common view relationship between the current keyframe and the previous keyframe, and then execute S1.8.

[0028] S1.8: Determine whether the key frame queue is full of 10 key frames. If not, return to S1.1. If so, further determine whether the pure inertial maximum a posteriori estimation failure flag is set. If not, enter the visual lidar joint maximum a posteriori estimation module. If it is set, enter the gravity direction deviation optimization module.

[0029] Regarding the content of S1.8, the function of the flag is to mark whether the pure inertia maximum a posteriori estimation module can be successfully executed. If the pure inertia maximum a posteriori estimation module cannot be successfully executed within the specified time or the specified number of times, then the flag is set, indicating that the robot is in a slow motion state, resulting in the inertia parameters being unobservable and difficult to converge. The subsequent initialization process only optimizes the gravity direction deviation matrix.

[0030] The visual lidar combined with the maximum a posteriori estimation module is used to estimate the position and rotation state of the mobile robot at each keyframe. The relative pose quantities from point cloud matching between adjacent keyframes are used to derive the robot's pose quantities in the world coordinate system, which serve as node information in the factor graph. The matching relationship between image feature points between adjacent keyframes is used to establish visual reprojection constraints, which serve as factor information in the factor graph. The nodes and factors are then combined to form a factor graph optimization model, with the pose state of each keyframe being used as the optimization variable to obtain the robot's state information. Specifically, the model includes the following steps:

[0031] S2.1: Based on the relative pose states between the key frames obtained by the data preprocessing module, obtain the state information of the robot at each key frame in the world coordinate system, including position and rotation, and then execute S2.2;

[0032] For the content of S2.1, the relative pose states between the key frames are obtained by the scan-to-scan matching method of the lidar point cloud, and the relative pose states are transformed into absolute pose states, such as Figure 2 As shown, assuming that the relative transformation matrix between the i-1th key frame and the i-th key frame is expressed as The pose state T of the key frame of the i-th frame i Get the pose state T of the key frame i-1 i-1 , the expression is as follows:

[0033]

[0034] S2.2: Based on the image feature point matching relationship between adjacent key frames in the key frame queue, use the visual perspective n-point projection algorithm to estimate the visual relative pose constraint relationship between each key frame, and then execute S2.3;

[0035] S2.3: Construct the state information of each key frame and the constraint relationship between the key frames into a factor graph, establish an optimization function based on the factor graph, and then execute S2.4;

[0036] For the content of S2.3, the visual constraint factor is constructed based on the reprojection error. When a map point in space is projected onto a keyframe where the map point can be observed, the coordinates of the projected point should coincide with the observed coordinates of the map point. The reprojection error between the two types of coordinates is the residual of the visual constraint factor. Assume that there is a map point P in space, its index number is l, and its projection coordinates on the normalized plane of the kth keyframe are The observation coordinates at the kth key frame are Then there is the visual constraint residual e C (x k-1 ,x k ) is expressed as follows:

[0037]

[0038] Where, and x k Respectively represent the robot state information corresponding to the k-1th key frame and the kth key frame, including position, attitude, velocity, IMU accelerometer bias and gyroscope bias; It can be estimated by the visual perspective projection geometric model, and the expression is:

[0039]

[0040] Where, represents the observation value of the map point P on the normalized plane at the k-1th key frame, and They represent the translation and rotation from the normalized coordinate system of the k-1th key frame to the normalized coordinate system of the kth key frame, respectively.

[0041] Regarding the content of S2.3, the state information of each key frame and the constraint relationship between key frames are constructed as a factor graph, such as Figure 3 A schematic diagram of the factor graph structure is provided. The constraint relationships between keyframes refer to the constraint factors between keyframes obtained by the PnP algorithm and established based on the visual reprojection error. The state information of each keyframe refers to the relative pose relationship obtained from the laser point cloud and then converted to the absolute pose state. The optimization objective function is constructed based on the established factor graph, and the expression is as follows:

[0042]

[0043] Where, e C (x k-1 ,x k ) represents the visual constraint residual, χ c Represents all variables to be optimized of the objective function. There are 10 key frames in the key frame queue for initialization in the present invention, so N=10.

[0044] Regarding the content of S2.3, the state information of the key frame includes the rotation state quantity and the translation state quantity. Taking the k-th key frame as an example, the variables to be optimized of the k-th key frame are written in the form of a vector, which can be expressed as:

[0045]

[0046] Where p k and q k Represent the position and posture of the kth key frame respectively. All variables to be optimized in the objective function are represented as vector χ c , of the following form:

[0047]

[0048] In the above two equations, setting N=10 indicates that there are 10 key frames in the key frame queue used for initialization.

[0049] S2.4: Using the state information of each key frame as the optimization variable, perform iterative nonlinear optimization to determine whether the optimization result converges to the threshold neighborhood. If so, enter the pure inertial maximum a posteriori estimation module. If not, execute S2.5.

[0050] Regarding the content of S2.4, the iterative optimization of the objective function using nonlinear optimization, taking the Levenberg-Marquadt nonlinear optimization 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 problem of non-singularity and ill-conditioning is avoided, thus providing a more stable and accurate state increment. The objective function is constructed in the form of a state increment equation:

[0051] (H h +μI)Δχ=g (11)

[0052] Where 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 through iterative optimization;

[0053] S2.5: Determine whether the optimization time or number of iterations exceeds the time threshold or the number threshold. If neither exceeds the time threshold or the number threshold, return to S2.4 to continue nonlinear iterative optimization until any threshold is exceeded. Then, pop the first element in the key frame queue, update the factor graph model and point cloud map, and then return to the data preprocessing module.

[0054] The pure inertial maximum a posteriori estimation module adds an IMU pre-integration constraint factor to the factor graph constructed by the visual lidar combined with the maximum a posteriori estimation module. The optimization function is then reconstructed from the factor graph. The position and posture at each key frame are considered as known quantities. The velocity at each key frame, as well as the deviation matrix between the Z axis of the world coordinate system and the direction of gravity, the accelerometer bias and gyroscope bias of the IMU are estimated in a nonlinear iterative optimization manner. Finally, the world coordinate system is adjusted using the optimized gravity direction deviation matrix. The specific steps are as follows:

[0055] S3.1: Based on the IMU data of each keyframe in the keyframe queue, construct an IMU pre-integration constraint factor between each keyframe, add the IMU pre-integration constraint factor to the factor graph, establish an optimization function based on the factor graph, and then execute S3.2;

[0056] For the content of S3.1, the factor graph model includes visual reprojection constraint factors and IMU pre-integration constraint factors, such as Figure 4 As shown in the figure, a schematic diagram of the factor graph model constructed by the pure inertial maximum a posteriori estimation module is given. According to the factor graph model, the optimization objective function is established, and the expression is as follows:

[0057]

[0058] Where x k-1 and xk They represent the robot posture state information corresponding to the k-1th key frame and the kth key frame, respectively, and e B (x k-1 ,x k ) represents the IMU pre-integration constraint residual, e C (x k-1 ,x k ) represents the visual constraint residual. There are 10 key frames in the key frame queue used for initialization in the present invention, so N=10, χ imu Represents the variable to be optimized in the objective function.

[0059] S3.2: Using the velocity, gravity deviation matrix, and IMU bias (including acceleration bias and gyroscope bias) of each keyframe as optimization variables, perform iterative nonlinear optimization to determine whether the optimization result converges to the threshold neighborhood. If not, execute S3.3. If converged, use the optimized gravity deviation matrix to adjust the world coordinate system so that its Z axis is parallel to the gravity direction, and then enter the visual-inertial lidar joint tuning module.

[0060] For the content of S3.2, the position and attitude of each key frame have been optimized in the visual lidar combined maximum a posteriori estimation module. In the pure inertial maximum a posteriori estimation module, the variables to be optimized for each key frame only include velocity v, IMU acceleration bias and gyroscope b. a Offset b ω , taking the kth key frame as an example, the variables to be optimized are written in the form of vectors, which can be expressed as:

[0061]

[0062] Where, v k 、 are the velocity of the kth key frame, the acceleration bias of the IMU, and the gyroscope bias, respectively. In addition, the deviation between the Z axis of the world coordinate system and the direction of gravity needs to be considered. The gravity direction deviation matrix G is optimized together as the optimization variable. In the pure inertial maximum a posteriori estimation module, the variable to be optimized of the objective function can be expressed as χ imu , of the following form:

[0063]

[0064] In the above two equations, setting N=10 indicates that there are 10 key frames in the key frame queue used for initialization.

[0065] S3.3: Determine whether the optimization time or the number of iterations of the nonlinear optimization exceeds the time threshold or the number threshold. If neither exceeds the time threshold or the number threshold, return to S3.2 to continue the nonlinear iterative optimization until either threshold is exceeded, and then execute S3.4;

[0066] S3.4: Determine whether the initialization execution time exceeds 20 seconds or the number of times the pure inertial maximum a posteriori estimation module has been executed exceeds 20 times. If neither exceeds, pop the first element in the keyframe queue, update the factor graph model and point cloud map, and then return to the data preprocessing module. If it exceeds, set the pure inertial maximum a posteriori estimation failure flag and return to the data preprocessing module.

[0067] The visual-inertial lidar joint optimization module still uses the factor graph model established by the pure inertial maximum a posteriori estimation module and the objective function constructed based on this model. The results obtained by iterative optimization of the visual lidar joint maximum a posteriori estimation module and the pure inertial maximum a posteriori estimation module are used as initial values. All optimization variables of these two modules are re-optimized, and the batched, progressive multivariable optimization is transformed into a multivariable joint optimization to further improve the accuracy of the estimation results. The specific steps are as follows:

[0068] S4.1: Using the robot's position, attitude, and velocity at each keyframe, the IMU's accelerometer bias, gyroscope bias, and gravity deviation matrix at each keyframe as optimization variables, perform iterative nonlinear optimization on the optimization function established in the pure inertial maximum a posteriori estimation module. The optimization results obtained by the combined visual lidar maximum a posteriori estimation module and the pure inertial maximum a posteriori estimation module are used as the initial values ​​of the optimization variables, and then execute S4.2.

[0069] For the content of S4.1, the factor graph model established by the pure inertial maximum a posteriori estimation module is still used. This factor graph model includes the visual reprojection constraint factor and the IMU pre-integration constraint factor. The position, attitude, velocity, IMU acceleration bias and gyroscope bias of each key frame are optimized simultaneously. Taking the kth key frame as an example, the variables to be optimized are written in the form of vectors, which can be expressed as:

[0070]

[0071] Where p k ,q k 、v k 、 Represent the position, attitude, velocity, IMU acceleration bias and gyroscope bias of the kth key frame respectively. In addition, the gravity direction deviation matrix G needs to be optimized. The initial values ​​of all optimization variables are the optimization results of the visual lidar joint maximum a posteriori estimation module and the pure inertial maximum a posteriori estimation module. All variables to be optimized in the objective function are represented as vectors χ cil , of the following form:

[0072]

[0073] In the above two equations, setting N=10 indicates that there are 10 key frames in the key frame queue used for initialization.

[0074] S4.2: Determine whether the optimization result converges within the threshold neighborhood. If so, execute S4.3. If not, determine whether the optimization time or the number of iterations exceeds the time threshold or the number threshold. If neither exceeds, return to S4.1 to continue nonlinear iterative optimization until any threshold is exceeded. Then determine whether the initialization time exceeds 100 seconds. If not, pop out the first element in the initialization keyframe queue, update the factor graph model and point cloud map, and then return to the data preprocessing module. If it exceeds 100 seconds, end the initialization program.

[0075] S4.3: Use the optimized gravity direction deviation matrix to adjust the world coordinate system again so that its Z axis is parallel to the gravity direction. Then, clear the keyframe queue, delete the factor graph model, retain the point cloud map created according to the keyframes during the initialization process, and finally end the initialization process.

[0076] The gravity direction deviation optimization module is designed to improve the robustness of SLAM system initialization. When the mobile robot moves slowly, the inertial parameters are unobservable, which easily leads to the difficulty of convergence of the pure inertial maximum a posteriori estimation module, resulting in the failure of the initialization process. To address this problem, and considering that the accuracy of the world coordinate system is crucial to the SLAM system, the present invention designs a gravity direction deviation optimization module. This module reconstructs the factor graph and reconstructs the target optimization function. Only the gravity direction deviation matrix is ​​used as the optimization variable, and other state quantities are not involved in the optimization. The specific steps are as follows:

[0077] S5.1: Match the laser point cloud data of each key frame in the key frame queue with the point cloud map, estimate the state information of the robot at each key frame (including translation and rotation), and then execute S5.2;

[0078] For the content of S5.1, the laser point cloud data of each key frame is matched with the point cloud map respectively. The scan-to-map method is used to perform ICP matching on the point cloud of each key frame and the point cloud map in the world coordinate system. The corner points in the key frame are matched with the line features in the point cloud map, and the surface points in the key frame are matched with the surface features in the point cloud map. The principle is the same as the scan-to-scan matching principle in step S1.6, and will not be repeated here.

[0079] S5.2: Based on the image feature point matching relationship between adjacent keyframes in the keyframe queue, use the visual perspective n-point projection algorithm to estimate the relative pose relationship between the keyframes, thereby constructing the visual reprojection constraint factor between the keyframes, and then execute S5.3;

[0080] Regarding the content of S5.2, the use of the visual perspective n-point projection algorithm to estimate the relative pose relationship between each key frame is the same as the method in step S2.2, and will not be repeated here.

[0081] S5.3: Based on the IMU data of each keyframe in the keyframe queue, construct an IMU pre-integration constraint factor between each keyframe, construct the state information of each keyframe, the visual reprojection constraint factor between each keyframe, and the IMU pre-integration constraint factor into a factor graph, then establish a target optimization function based on the factor graph, and then execute S5.4;

[0082] Regarding the content in S5.3, the IMU pre-integration constraint factor between each key frame is constructed. Considering that the inertial parameters do not have good observability due to the slow movement of the robot, in order to ensure the robustness of the initialization process and complete the initialization basic goal of aligning the Z axis of the world coordinate system with the direction of gravity, this step does not consider the optimization of the IMU acceleration bias and gyroscope bias. Therefore, the form of the IMU pre-integration constraint residual constructed in step S3.1 is different from that of the IMU pre-integration constraint residual, and is as follows:

[0083]

[0084] Where r p 、r v 、r q are the k-1th key frame (t k-1 time) to the kth key frame (t k The position residual, velocity residual, and attitude residual of the k-th frame are respectively represented by the IMU measurement data and the rotation change, velocity change, and position change between the k-1th frame and the kth frame calculated by the pre-integration formula. It represents the rotation matrix from the body coordinate system to the world coordinate system based on the IMU. G represents the gravity direction deviation matrix. and Respectively represent the positions of the k-1th frame and the kth frame in the world coordinate system, and It represents the velocity between the k-1th frame and the kth frame in the world coordinate system, and g represents the gravity vector.

[0085] Regarding the content in S5.3, the factor graph model includes visual reprojection constraint factors and IMU pre-integration constraint factors. The factor graph structure is similar to the factor graph structure constructed in step S3.1, with two differences: first, the expression of the IMU pre-integration constraint residual is different; second, the method of obtaining the node state in the factor graph is different. The specific differences have been described above and will not be repeated here. Based on the factor graph model, the optimization objective function is established, and the expression is as follows:

[0086]

[0087] Where x k-1 and x k They represent the robot posture state information corresponding to the k-1th key frame and the kth key frame, e′ B (x k-1 ,x k ) represents the IMU pre-integration constraint residual, e C (x k-1 ,x k ) represents the visual constraint residual, χ g Represents the variable to be optimized in the objective function.

[0088] S5.4: Using the gravity direction deviation matrix as the optimization variable, perform iterative nonlinear optimization on the target optimization function to determine whether the optimization result converges to the threshold neighborhood. If so, execute S5.5. If not, determine whether the optimization time or number of iterations exceeds the time threshold or the number threshold. If neither exceeds, continue nonlinear iterative optimization until any threshold is exceeded. Then, pop the first element in the keyframe queue, update the factor graph model and point cloud map, and then return to the data preprocessing module.

[0089] Regarding the content of S5.4, considering that the pure inertial maximum a posteriori estimation failure flag has been set, indicating that the robot is in a state of slow motion, so that the inertial parameters do not have good observability, the reason for the failure of the pure inertial maximum a posteriori estimation to converge is that the accelerometer bias and gyroscope bias of the IMU diverge during the optimization process, while the visual lidar combined maximum a posteriori estimation module is generally not affected by the slow motion of the robot and can estimate the pose of each key frame through the objective function. In order to ensure the robustness of the SLAM system initialization, the present invention designs a gravity direction deviation optimization module, which abandons the optimization of the inertial parameters and only completes the optimization of the deviation matrix G between the Z axis of the world coordinate system and the gravity direction. Therefore, all the variables to be optimized in the objective function are written in the form of vectors, which are expressed as follows:

[0090] χ g =[G] (19)

[0091] In the formula, setting N = 10 means that there are 10 key frames in the key frame queue used for initialization, v k Indicates the velocity of the kth keyframe.

[0092] Regarding the content of S5.4, the iterative nonlinear optimization of the objective function still uses the Levenberg-Marquadt nonlinear optimization method, which is consistent with the method in step S2.4 and will not be repeated here.

[0093] S5.5: Use the obtained gravity direction deviation matrix to adjust the world coordinate system so that its Z axis is parallel to the gravity direction. Then determine whether more than 100 key frames have been processed cumulatively or more than 100 seconds have passed since the initialization program was started. If any of the conditions is met, clear the initialization queue, delete the point cloud map, delete the factor graph model, and end the initialization program. If both conditions are not met, pop out the first element in the key frame queue, update the factor graph model and point cloud map, and then return to the data preprocessing module.

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

[0095] This paper proposes a joint initialization method for a SLAM system that integrates multiple sensors, including 3D lidar, camera, and IMU. The goal is to improve the accuracy of SLAM system initialization results and enhance the robustness of SLAM system initialization when the robot is moving slowly.

[0096] This method preprocesses the three heterogeneous sensor data before initialization, aligns the image and point cloud at the data and target levels, removes dynamic feature points in the image and the lidar dynamic point cloud, and ensures that the data used in the initialization process is accurate; maintains a point cloud map based on the key frame queue, and updates the feature point matching relationship between key frames to ensure that the reference coordinate system and constraint relationship used in the initialization process are accurate.

[0097] The present invention decomposes the SLAM initialization process into multiple progressive sub-modules. Through such hierarchical decomposition, the multi-variable (key frame position, posture, speed, gravity direction deviation matrix, IMU acceleration bias and gyroscope bias) optimization problem is transformed into a multiple batch optimization problem. The convergence difficulty of the multi-variable optimization is reduced and the convergence speed of the multi-variable optimization is improved by a progressive optimization method from easy to difficult. During the initialization process, if any sub-module has low convergence accuracy or fails to converge within a timeout, it is considered that there is a problem with the initialization data, and the optimization iteration process is stopped. The key frame queue and its related constraints are updated and the initialization operation is performed again, so as to avoid the problem of low optimization result accuracy due to inaccurate data and ensure the accuracy and speed of initialization.

[0098] The visual-inertial lidar joint tuning module of the present invention further optimizes all optimization variables, transforming the multi-variable multiple batch optimization problem into a multi-variable joint optimization problem, and using the convergence values ​​of multiple batch optimizations as the initial values ​​of the variables, thereby ensuring the convergence speed of the joint optimization and further improving the estimation accuracy of all optimization variables.

[0099] The present invention addresses the phenomenon that the inertial parameters of the robot are not well observable due to slow movement, which causes the SLAM system to repeatedly initialize. The reason is that the pure inertial maximum a posteriori estimation module has difficulty converging. To address this problem, the present invention designs a gravity direction deviation optimization module, and only optimizes the gravity direction deviation matrix, ensuring that the Z axis of the world coordinate system is quickly aligned with the gravity direction, thereby improving the robustness of the SLAM system initialization. BRIEF DESCRIPTION OF THE DRAWINGS

[0100] Figure 1 It is a flowchart of a joint initialization method for a 3D lidar visual-inertial multi-sensor SLAM system described in a specific embodiment;

[0101] Figure 2 It is a schematic diagram of the transformation of the relative posture relationship between adjacent key frames into the absolute posture state of the key frames;

[0102] Figure 3 Schematic diagram of the factor graph model constructed by the visual laser radar combined with the maximum a posteriori estimation module in the present invention;

[0103] Figure 4 Schematic diagram of the factor graph model constructed by the pure inertial maximum a posteriori estimation module in the present invention;

[0104] Figure 5 This is a comparison chart of the trajectories of the three initialization methods within a period of initialization;

[0105] Figure 6 This is a graph showing the relationship between the displacement of the trajectory on the X, Y, and Z axes and time for the three initialization methods within a period of initialization. DETAILED DESCRIPTION

[0106] The following will be combined with the accompanying 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 embodiments described are only part of the embodiments of the present invention, not 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 are within 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 in the absence of conflict.

[0107] Reference Figures 1 to 6 Specifically describe this embodiment. Figure 1 A flowchart of a joint initialization method for a 3D lidar visual-inertial multi-sensor SLAM system described in the present invention is given. The method includes five modules: a data preprocessing module, a visual lidar joint maximum a posteriori estimation module, a pure inertial maximum a posteriori estimation module, a visual-inertial lidar joint tuning module, and a gravity direction deviation optimization module.

[0108] The data preprocessing module is used to receive and preprocess the data collected by each sensor, as well as select key frames and construct a point cloud map for initialization. By performing data-level fusion on the image data and the laser point cloud data, dynamic feature points of the image and the dynamic point cloud of the LiDAR are removed. The laser point cloud data is used as a depth reference to estimate the accurate depth values ​​of the static feature points of the image. Key frames are selected based on the distance traveled by the vehicle and a key frame queue for initialization is established for subsequent initialization operations. The module includes the following steps:

[0109] S1.1: The SLAM system receives image data captured by the camera, point cloud data collected by the 3D lidar, and acceleration and angular velocity data collected by the IMU, synchronizes the timestamps of the image data, lidar data, and IMU data, and then executes S1.2.

[0110] S1.2: Use a classic image feature point extraction method (such as ORB, SIFT, etc.) to extract image feature points; use the camera distortion coefficient to perform camera imaging distortion correction on the extracted image feature points, and then execute S1.3;

[0111] Regarding the content of S1.2, the camera distortion coefficient is used to correct the camera imaging distortion. Before the distortion correction is performed, at any point p on the normalized image plane, the coordinates of the point in the camera normalized coordinates are expressed as [x, y] Τ , perform radial distortion and tangential distortion correction on the point to obtain the coordinates of the point after correction. The expression is as follows:

[0112]

[0113] In the formula, [x distorted ,y distorted ] Τ is the coordinate of point p after distortion correction, k1, k2 and k3 are radial distortion correction parameters, p1 and p2 are tangential distortion correction parameters, and r is the distance from point p to the origin of the camera normalized coordinate system, i.e. r 2 =x 2 +y 2 .

[0114] S1.3: Remove outliers from the LiDAR point cloud, use IMU measurement data to correct motion distortion on the LiDAR point cloud, downsample the point cloud data using voxel filtering, and extract feature information from the point cloud data, including corner points and surface points, and then execute S1.4;

[0115] Regarding the content of S1.3, the use of IMU measurement data to correct motion distortion of the lidar point cloud means using the robot posture information calculated by the IMU within a lidar scanning frame time to accurately obtain the position information corresponding to the point cloud at a certain moment in the scanning frame time through the linear interpolation method, and then use the position information to correct the distortion of the point cloud at that moment caused by the robot motion.

[0116] Assuming that the point cloud data obtained by the laser radar scanning strictly follows the time sequence, within a laser radar scanning cycle, t s Indicates the scanning start time, t e Indicates the end time of scanning, t c Represents the current scanning time, and satisfies t s <t c ≤t e ,θ c t c The emission angle of the laser beam corresponding to the moment, θ e t e The laser beam emission angle corresponding to the moment, the expression of the relationship between the current scanning time and the laser radar scanning rotation angle is as follows:

[0117]

[0118] In one scanning cycle, in the world coordinate system, assuming t s Time and t e The robot posture information corresponding to the moment is and It can be expressed as follows:

[0119]

[0120] Where p x 、p y 、p z is the position of the robot, γ, θ, ψ are the posture values ​​of the robot expressed using the Euler angle method. s Time and t e The robot posture information corresponding to the moment t is accurately obtained by linear interpolation method c The robot posture information corresponding to the moment is calculated using the following formula:

[0121]

[0122] Where, t c The position and posture of the point cloud at each moment.

[0123] The specific calculation formula for the distortion of the current point cloud relative to the starting time is:

[0124]

[0125] Where, is the information to be processed of the current point cloud in the world coordinate system, and is the position and posture of the point cloud in the world coordinate system at the initial moment, is the distortion of the converted point cloud relative to the initial scanning time. Subtracting the current point cloud from the transformed point cloud after distortion removal can effectively offset the point cloud distortion caused by robot motion.

[0126] For the content of S1.3, the point cloud curvature is calculated to extract the corner points and surface points of the point cloud. The simulation calculation expression of the point cloud curvature is as follows:

[0127]

[0128] Where c i is the curvature of point i, S is the set of points in the neighborhood of point i, |S| represents the number of points in set S, represents the distance from point i to the radar, Indicates the distance from the point to the lidar, Represents the distance from point j to the radar in the neighborhood of point i.

[0129] S1.4: Use the extrinsic matrix between the LiDAR and the camera to achieve data-level matching and alignment between the image and the laser point cloud. Perform 2D object detection on the image and 3D object detection on the laser point cloud to achieve object-level matching and alignment between the image and the laser point cloud. Then, remove dynamic feature points from the image and the laser point cloud, and then execute S1.5.

[0130] Regarding the content of S1.4, the data-level matching and alignment of the image and the laser point cloud is achieved by transforming the point cloud data from the radar coordinate system to the camera coordinate system through the external parameter matrix between the laser radar and the camera, and then projecting the point cloud in the camera coordinate system onto the image normalization plane through the camera perspective projection relationship. Assume that in the radar coordinate system, the coordinates of a point in the point cloud are P(p x p y p z ), then the coordinates of the point are transformed to the normalized coordinate system (u,v) T It can be expressed as:

[0131]

[0132] In the above formula, z represents the depth value of point P after transforming it from the radar coordinate system to the camera coordinate system, K is the camera intrinsic parameter matrix, It is the extrinsic parameter matrix from the lidar to the camera.

[0133] Regarding the content of S1.4, the target-level matching and alignment of image and point cloud data refers to performing 2D target detection on the image to generate an image mask of the dynamic object; performing 3D target detection on the point cloud data, and projecting the detection results onto the image normalization plane to generate a mask of the point cloud target object; by comparing the masks of the image and the point cloud, retaining the overlapping areas of the two and deleting the non-overlapping areas, objects covered by both masks are identified as dynamic objects, and feature points and point clouds covered by overlapping masks are identified as dynamic feature points and dynamic point clouds, which are removed from the feature point set of the image and the point cloud set of the lidar, respectively.

[0134] S1.5: Use the static feature points in the LiDAR point cloud auxiliary image to obtain accurate depth information, delete the static feature points that do not have a depth value, and then execute S1.6;

[0135] For the content of S1.5, obtaining accurate depth information from static feature points in the LiDAR point cloud auxiliary image means that for each static feature point on the camera normalized plane, a neighborhood box is taken with the feature point as the center. Assume that the total number of projected LiDAR points in the neighborhood box is N. all , calculate the mean μ and variance σ of the depth values ​​of all LiDAR points in the neighborhood box, then group the depth values ​​of all LiDAR points in the neighborhood box into statistical histograms, and count the number of LiDAR points N within the range of twice the mean μ and variance σ μ , if the expression is satisfied It is assumed that the depth values ​​of the LiDAR points in the neighborhood box are approximately Gaussian distributed. μ The depth values ​​of the LiDAR points are calculated, and the mean depth value is recalculated as the depth value of the (static) feature point; if the above expression is not satisfied, it is considered that the feature point is near the fault plane, and the point cloud within the neighborhood box is not sufficient to estimate the accurate depth value of the feature point, so the feature point is deleted.

[0136] S1.6: Determine whether the keyframe queue is empty. If so, set the current frame as a keyframe and insert it into the keyframe queue. Then, execute S1.7. If not, perform ICP matching of feature points on the laser point cloud of the current frame and the laser point cloud of the previous keyframe to obtain the translation and rotation information between the two frames. Determine whether the current frame is a keyframe based on whether the obtained translation reaches the distance threshold. If so, insert the keyframe into the keyframe queue and execute S1.7. If not, return to S1.1.

[0137] For the content of S1.6, the ICP matching of feature points between the laser point cloud of the current frame and the laser point cloud of the previous key frame refers to the scan-to-scan pose estimation of the feature points of the point cloud between the current key frame and the point cloud data of the previous key frame. The target optimization function is constructed based on the corner point residuals and the surface point residuals. The corner point residuals refer to the matching errors from the corner points in the laser point cloud of the current frame to the line features of the laser point cloud of the previous key frame. The surface point residuals refer to the matching errors from the surface points in the laser point cloud of the current frame to the surface features of the laser point cloud of the previous key frame. Assume that the relative rotation matrix and relative translation vector between the current frame and the previous key frame are Δq and Δp respectively, and the target function is constructed based on the corner point residuals and surface point residuals of the point cloud. The expression is as follows:

[0138] e l (x k ,x′)=r1+r2 (8)

[0139] Where x′ and x k They represent the pose states of the current frame and the previous key frame respectively, r1 represents the corner residual of the lidar, and r2 represents the surface residual of the lidar.

[0140] For the corner residual, the specific expression is:

[0141]

[0142] Where, is the corresponding corner point in the current frame lidar coordinate system, It is the coordinate representation of the corner point of the current frame in the lidar coordinate system of the previous key frame. and is the value in the previous keyframe The coordinates of the two matching corner points, represents the cumulative sum of all corner residuals in the current frame, and ||·|| represents the modulo operation.

[0143] For the surface point residual, the specific expression is:

[0144]

[0145] Where p l is the corresponding surface point in the current frame laser radar coordinate system, p m is the coordinate representation of the surface point of the current frame in the lidar coordinate system of the previous key frame, and n is the coordinate of p in the previous key frame. m The normal vector of the matching plane, It represents the cumulative sum of all surface residuals in the current frame, and ||·|| represents the modulo operation.

[0146] S1.7: Determine whether the current keyframe is the first keyframe in the keyframe queue. If so, establish a world coordinate system based on the lidar coordinate system of the current keyframe as the reference coordinate system, and then execute S1.8. If not, transform the laser point cloud of the current keyframe to the world coordinate system, and establish an image feature point matching relationship between the current keyframe and the adjacent keyframe based on the visual common view relationship between the current keyframe and the previous keyframe, and then execute S1.8.

[0147] For the content of S1.7, the image feature point matching relationship between adjacent key frames is realized using the fast nearest neighbor search (FLANN) matching method. For a feature point of the current key frame, the Euclidean distance between the feature point and each feature point in the feature point set of the adjacent key frame is calculated, and these Euclidean distances are stored through the KD-Tree structure. The matching point pair set is recursively searched from top to bottom to obtain the minimum Euclidean distance, thereby obtaining the best matching point pair for the feature point. This operation is performed on each feature point of the current key frame, and then a set of image feature point matching points between the current key frame and the adjacent key frames can be obtained.

[0148] S1.8: Determine whether the key frame queue is full of 10 key frames. If not, return to S1.1. If so, further determine whether the pure inertial maximum a posteriori estimation failure flag is set. If not, enter the visual lidar joint maximum a posteriori estimation module. If it is set, enter the gravity direction deviation optimization module.

[0149] Regarding the content of S1.8, the function of the flag is to mark whether the pure inertia maximum a posteriori estimation module can be successfully executed. If the pure inertia maximum a posteriori estimation module cannot be successfully executed within the specified time or the specified number of times, then the flag is set, indicating that the robot is in a slow motion state, resulting in the inertia parameters being unobservable and difficult to converge. The subsequent initialization process only optimizes the gravity direction deviation matrix.

[0150] The visual lidar combined with the maximum a posteriori estimation module is used to estimate the position and rotation state of the mobile robot at each keyframe. The relative pose quantities from point cloud matching between adjacent keyframes are used to derive the robot's pose quantities in the world coordinate system, which serve as node information in the factor graph. The matching relationship between image feature points between adjacent keyframes is used to establish visual reprojection constraints, which serve as factor information in the factor graph. The nodes and factors are then combined to form a factor graph optimization model, with the pose state of each keyframe being used as the optimization variable to obtain the robot's state information. Specifically, the model includes the following steps:

[0151] S2.1: Based on the relative pose states between the key frames obtained by the data preprocessing module, obtain the state information of the robot at each key frame in the world coordinate system, including position and rotation, and then execute S2.2;

[0152] For the content of S2.1, the relative pose states between the key frames are obtained by the scan-to-scan matching method of the lidar point cloud, and the relative pose states are transformed into absolute pose states, such as Figure 2 As shown, assuming that the relative transformation matrix between the i-1th key frame and the i-th key frame is expressed as The pose state T of the key frame of the i-th frame i Get the pose state T of the key frame i-1 i-1 , the expression is as follows:

[0153]

[0154] S2.2: Based on the image feature point matching relationship between adjacent key frames in the key frame queue, use the visual perspective n-point projection algorithm to estimate the visual relative pose constraint relationship between each key frame, and then execute S2.3;

[0155] Regarding the content of S2.2, the visual perspective n-point projection (PnP) algorithm estimates the visual relative pose constraint relationship between each key frame, which means that a set of 3D spatial points and their projection points on the 2D image plane are known, and the camera posture, including translation and rotation, is estimated through the corresponding matching relationship between the spatial points and the projection points.

[0156] According to the perspective projection geometric relationship of camera imaging, a three-dimensional point P in space is established. i The corresponding two-dimensional projection point u i The projection point is the image feature point, and the relationship between the pixel coordinates of the spatial point projection and the coordinates of the spatial point is:

[0157] s i u i =KTP i (12)

[0158] Where u i Represents the pixel coordinates of the projection, P i represents the three-dimensional coordinates of the spatial point, K represents the camera internal parameter, T represents the relative pose relationship to be determined, s i Represents the depth information of the projected point. Since the observed projected points are noisy, an error term is established based on the reprojection error, and the objective function is established based on the least squares optimization problem. The expression is as follows:

[0159]

[0160] Where n represents the number of matching pairs between spatial points and projection points, T * Represents the camera pose transformation matrix that minimizes the reprojection error objective function.

[0161] S2.3: Construct the state information of each key frame and the constraint relationship between the key frames into a factor graph, establish an optimization function based on the factor graph, and then execute S2.4;

[0162] For the content of S2.3, the visual constraint factor is constructed based on the reprojection error. When a map point in space is projected onto a keyframe where the map point can be observed, the coordinates of the projected point should coincide with the observed coordinates of the map point. The reprojection error between the two types of coordinates is the residual of the visual constraint factor. Assume that there is a map point P in space, its index number is l, and its projection coordinates on the normalized plane of the kth keyframe are The observation coordinates at the kth key frame are Then there is the visual constraint residual e C (x k-1 ,x k )The expression is as follows:

[0163]

[0164] Where x k-1 and x k Respectively represent the robot state information corresponding to the k-1th key frame and the kth key frame, including position, attitude, velocity, IMU accelerometer bias and gyroscope bias; It can be estimated by the visual perspective projection geometric model, and the expression is:

[0165] Where, represents the observation value of the map point P on the normalized plane of the k-1th key frame, and They represent the translation and rotation from the normalized coordinate system of the k-1th key frame to the normalized coordinate system of the kth key frame, respectively.

[0166] Regarding the content of S2.3, the state information of each key frame and the constraint relationship between key frames are constructed as a factor graph, such as Figure 3 A schematic diagram of the factor graph structure is provided. The constraint relationships between keyframes refer to the constraint factors between keyframes obtained by the PnP algorithm and established based on the visual reprojection error. The state information of each keyframe refers to the relative pose relationship obtained from the laser point cloud and then converted to the absolute pose state. The optimization objective function is constructed based on the established factor graph, and the expression is as follows:

[0167]

[0168] Where, e C (x k-1 ,x k ) represents the visual constraint residual, χ c Represents all variables to be optimized of the objective function. There are 10 key frames in the key frame queue for initialization in the present invention, so N=10.

[0169] Regarding the content of S2.3, the state information of the key frame includes the rotation state quantity and the translation state quantity. Taking the k-th key frame as an example, the variables to be optimized of the k-th key frame are written in the form of a vector, which can be expressed as:

[0170]

[0171] Where p k and q k Represent the position and posture of the kth key frame respectively. All variables to be optimized in the objective function are represented as vector χ c , of the following form:

[0172]

[0173] In the above two equations, setting N=10 indicates that there are 10 key frames in the key frame queue used for initialization.

[0174] S2.4: Using the state information of each key frame as the optimization variable, perform iterative nonlinear optimization to determine whether the optimization result converges to the threshold neighborhood. If so, enter the pure inertial maximum a posteriori estimation module. If not, execute S2.5.

[0175] Regarding the content of S2.4, the iterative optimization of the objective function using nonlinear optimization, taking the Levenberg-Marquadt nonlinear optimization 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 problem of non-singularity and ill-conditioning is avoided, thus providing a more stable and accurate state increment. The objective function is constructed in the form of a state increment equation:

[0176] (H h +μI)Δχ=g (19)

[0177] Where 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 through iterative optimization;

[0178] S2.5: Determine whether the optimization time or number of iterations exceeds the time threshold or the number threshold. If neither exceeds the time threshold or the number threshold, return to S2.4 to continue nonlinear iterative optimization until any threshold is exceeded. Then, pop the first element in the key frame queue, update the factor graph model and point cloud map, and then return to the data preprocessing module.

[0179] The pure inertial maximum a posteriori estimation module adds an IMU pre-integration constraint factor to the factor graph constructed by the visual lidar combined with the maximum a posteriori estimation module. The optimization function is then reconstructed from the factor graph. The position and posture at each key frame are considered as known quantities. The velocity at each key frame, as well as the deviation matrix between the Z axis of the world coordinate system and the direction of gravity, the accelerometer bias and gyroscope bias of the IMU are estimated in a nonlinear iterative optimization manner. Finally, the world coordinate system is adjusted using the optimized gravity direction deviation matrix. The specific steps are as follows:

[0180] S3.1: Based on the IMU data of each keyframe in the keyframe queue, construct an IMU pre-integration constraint factor between each keyframe, add the IMU pre-integration constraint factor to the factor graph, establish an optimization function based on the factor graph, and then execute S3.2;

[0181] For the content of S3.1, the IMU pre-integration constraint factor is used to describe the relative posture change relationship of the IMU measurement information between the time difference of two key frames. Assume that the robot posture state information corresponding to the k-1 key frame and the k key frame are x k-1 and x k , the IMU pre-integration constraint factor is determined by the IMU pre-integration constraint residual e B (x k-1 ,x k ) is constructed, and its expression is as follows:

[0182]

[0183] Where r p 、r v 、r q 、 are the k-1th key frame (t k-1 time) to the kth key frame (t k The position residual, velocity residual, attitude residual, accelerometer residual and gyroscope residual of the k-1th frame are obtained by IMU measurement data and pre-integration formula, respectively. ΔR, Δv and Δp represent the rotation change, velocity change and position change between the k-1th frame and the kth frame, respectively. It represents the rotation matrix from the body coordinate system to the world coordinate system based on the IMU. G represents the gravity direction deviation matrix. and Respectively represent the positions of the k-1th frame and the kth frame in the world coordinate system, and Indicates the speed of the k-1th frame and the kth frame in the world coordinate system, g represents the gravity vector, b a represents the accelerometer bias, and represents the accelerometer bias of the k-1th frame and the kth frame, b ω represents the gyroscope bias, and Indicates the gyroscope bias of the k-1th frame and the kth frame.

[0184] For the content of S3.1, the factor graph model includes visual reprojection constraint factors and IMU pre-integration constraint factors, such as Figure 4 As shown in the figure, a schematic diagram of the factor graph model constructed by the pure inertial maximum a posteriori estimation module is given. According to the factor graph model, the optimization objective function is established, and the expression is as follows:

[0185]

[0186] Where x k-1 and x k They represent the robot posture state information corresponding to the k-1th key frame and the kth key frame, respectively, and e B (x k-1 ,x k ) represents the IMU pre-integration constraint residual, e C (x k-1 ,x k ) represents the visual constraint residual. There are 10 key frames in the key frame queue used for initialization in the present invention, so N=10, χ imu Represents the variable to be optimized in the objective function.

[0187] S3.2: Using the velocity, gravity deviation matrix, and IMU bias (including acceleration bias and gyroscope bias) of each keyframe as optimization variables, perform iterative nonlinear optimization to determine whether the optimization result converges to the threshold neighborhood. If not, execute S3.3. If converged, use the optimized gravity deviation matrix to adjust the world coordinate system so that its Z axis is parallel to the gravity direction, and then enter the visual-inertial lidar joint tuning module.

[0188] For the content of S3.2, the position and attitude of each key frame have been optimized in the visual lidar combined maximum a posteriori estimation module. In the pure inertial maximum a posteriori estimation module, the variables to be optimized for each key frame only include velocity v, IMU acceleration bias and gyroscope b. a Offset b ω , taking the kth key frame as an example, the variables to be optimized are written in the form of vectors, which can be expressed as:

[0189]

[0190] Where, v k 、 are the velocity of the kth key frame, the acceleration bias of the IMU, and the gyroscope bias, respectively. In addition, the deviation between the Z axis of the world coordinate system and the direction of gravity needs to be considered. The gravity direction deviation matrix G is optimized together as the optimization variable. In the pure inertial maximum a posteriori estimation module, the variable to be optimized of the objective function can be expressed as χ imu , of the following form:

[0191]

[0192] In the above two equations, setting N=10 indicates that there are 10 key frames in the key frame queue used for initialization.

[0193] S3.3: Determine whether the optimization time or the number of iterations of the nonlinear optimization exceeds the time threshold or the number threshold. If neither exceeds the time threshold or the number threshold, return to S3.2 to continue the nonlinear iterative optimization until either threshold is exceeded, and then execute S3.4;

[0194] S3.4: Determine whether the initialization execution time exceeds 20 seconds or the number of times the pure inertial maximum a posteriori estimation module has been executed exceeds 20 times. If neither exceeds, pop the first element in the keyframe queue, update the factor graph model and point cloud map, and then return to the data preprocessing module. If it exceeds, set the pure inertial maximum a posteriori estimation failure flag and return to the data preprocessing module.

[0195] The visual-inertial lidar joint optimization module still uses the factor graph model established by the pure inertial maximum a posteriori estimation module and the objective function constructed based on this model. The results obtained by iterative optimization of the visual lidar joint maximum a posteriori estimation module and the pure inertial maximum a posteriori estimation module are used as initial values. All optimization variables of these two modules are re-optimized, and the batched, progressive multivariable optimization is transformed into a multivariable joint optimization to further improve the accuracy of the estimation results. The specific steps are as follows:

[0196] S4.1: Using the robot's position, attitude, and velocity at each keyframe, the IMU's accelerometer bias, gyroscope bias, and gravity deviation matrix at each keyframe as optimization variables, perform iterative nonlinear optimization on the optimization function established in the pure inertial maximum a posteriori estimation module. The optimization results obtained by the combined visual lidar maximum a posteriori estimation module and the pure inertial maximum a posteriori estimation module are used as the initial values ​​of the optimization variables, and then execute S4.2.

[0197] For the content of S4.1, the factor graph model established by the pure inertial maximum a posteriori estimation module is still used. This factor graph model includes the visual reprojection constraint factor and the IMU pre-integration constraint factor. The target optimization function expression obtained by the factor graph model is the same as formula (21). In this step, the position, attitude, velocity, IMU acceleration bias and gyroscope bias of each key frame are optimized simultaneously. Taking the kth key frame as an example, the variables to be optimized are written in the form of vectors, which can be expressed as:

[0198]

[0199] Where p k ,q k 、v k 、 Represent the position, attitude, velocity, IMU acceleration bias and gyroscope bias of the kth key frame respectively. In addition, the gravity direction deviation matrix G needs to be optimized. The initial values ​​of all optimization variables are the optimization results of the visual lidar joint maximum a posteriori estimation module and the pure inertial maximum a posteriori estimation module. All variables to be optimized in the objective function are represented as vectors χ cil , of the following form:

[0200]

[0201] In the above two equations, setting N=10 indicates that there are 10 key frames in the key frame queue used for initialization.

[0202] S4.2: Determine whether the optimization result converges within the threshold neighborhood. If so, execute S4.3. If not, determine whether the optimization time or the number of iterations exceeds the time threshold or the number threshold. If neither exceeds, return to S4.1 to continue nonlinear iterative optimization until any threshold is exceeded. Then determine whether the initialization time exceeds 100 seconds. If not, pop out the first element in the initialization keyframe queue, update the factor graph model and point cloud map, and then return to the data preprocessing module. If it exceeds 100 seconds, end the initialization program.

[0203] S4.3: Use the optimized gravity direction deviation matrix to adjust the world coordinate system again so that its Z axis is parallel to the gravity direction. Then, clear the keyframe queue, delete the factor graph model, retain the point cloud map created according to the keyframes during the initialization process, and finally end the initialization process.

[0204] The gravity direction deviation optimization module is designed to improve the robustness of SLAM system initialization. When the mobile robot moves slowly, the inertial parameters are unobservable, which easily leads to the difficulty of convergence of the pure inertial maximum a posteriori estimation module, resulting in the failure of the initialization process. To address this problem, and considering that the accuracy of the world coordinate system is crucial to the SLAM system, the present invention designs a gravity direction deviation optimization module. This module reconstructs the factor graph and reconstructs the target optimization function. Only the gravity direction deviation matrix is ​​used as the optimization variable, and other state quantities are not involved in the optimization. The specific steps are as follows:

[0205] S5.1: Match the laser point cloud data of each key frame in the key frame queue with the point cloud map, estimate the state information of the robot at each key frame (including translation and rotation), and then execute S5.2;

[0206] For the content of S5.1, the laser point cloud data of each key frame is matched with the point cloud map respectively. The scan-to-map method is used to perform ICP matching on the point cloud of each key frame and the point cloud map in the world coordinate system. The corner points in the key frame are matched with the line features in the point cloud map, and the surface points in the key frame are matched with the surface features in the point cloud map. The principle is the same as the scan-to-scan matching principle in step S1.6, and will not be repeated here.

[0207] S5.2: Based on the image feature point matching relationship between adjacent keyframes in the keyframe queue, use the visual perspective n-point projection algorithm to estimate the relative pose relationship between the keyframes, thereby constructing the visual reprojection constraint factor between the keyframes, and then execute S5.3;

[0208] Regarding the content of S5.2, the use of the visual perspective n-point projection algorithm to estimate the relative pose relationship between each key frame is the same as the method in step S2.2, and will not be repeated here.

[0209] S5.3: Based on the IMU data of each keyframe in the keyframe queue, construct an IMU pre-integration constraint factor between each keyframe, construct the state information of each keyframe, the visual reprojection constraint factor between each keyframe, and the IMU pre-integration constraint factor into a factor graph, then establish a target optimization function based on the factor graph, and then execute S5.4;

[0210] Regarding the content in S5.3, the IMU pre-integration constraint factor between each key frame is constructed. Considering that the inertial parameters do not have good observability due to the slow movement of the robot, in order to ensure the robustness of the initialization process and complete the initialization basic goal of aligning the Z axis of the world coordinate system with the direction of gravity, this step does not consider the optimization of the IMU acceleration bias and gyroscope bias. Therefore, the form of the IMU pre-integration constraint residual constructed in step S3.1 is different from that of the IMU pre-integration constraint residual, and is as follows:

[0211]

[0212] Where r p 、r v 、r q are the k-1th key frame (t k-1 time) to the kth key frame (t k The position residual, velocity residual, and attitude residual of the k-th frame are respectively represented by the IMU measurement data and the rotation change, velocity change, and position change between the k-1th frame and the kth frame calculated by the pre-integration formula. It represents the rotation matrix from the body coordinate system to the world coordinate system based on the IMU. G represents the gravity direction deviation matrix. and Respectively represent the positions of the k-1th frame and the kth frame in the world coordinate system, and Represents the velocity of the k-1th frame and the kth frame in the world coordinate system, and g represents the gravity vector. It should be noted that the above formula does not construct the IMU bias residual. It is not regarded as zero, but as a constant during the initialization period. The IMU bias is a time-varying quantity. Treating it as a constant will certainly lead to a larger system error, but it can ensure the robustness and rapidity of the SLAM system initialization under slow robot motion, and complete the estimation of the gravity direction deviation matrix. The estimation of the IMU bias will continue in the posture tracking module of the SLAM system.

[0213] Regarding the content in S5.3, the factor graph model includes visual reprojection constraint factors and IMU pre-integration constraint factors. The factor graph structure is similar to the factor graph structure constructed in step S3.1, with two differences: first, the expression of the IMU pre-integration constraint residual is different; second, the method of obtaining the node state in the factor graph is different. The specific differences have been described above and will not be repeated here. Based on the factor graph model, the optimization objective function is established, and the expression is as follows:

[0214]

[0215] Where x k-1 and x k They represent the robot posture state information corresponding to the k-1th key frame and the kth key frame, e′ B (x k-1 ,x k ) represents the IMU pre-integration constraint residual, e C (x k-1 ,x k ) represents the visual constraint residual, χ g Represents the variable to be optimized in the objective function.

[0216] S5.4: Using the gravity direction deviation matrix as the optimization variable, perform iterative nonlinear optimization on the target optimization function to determine whether the optimization result converges to the threshold neighborhood. If so, execute S5.5. If not, determine whether the optimization time or number of iterations exceeds the time threshold or the number threshold. If neither exceeds, continue nonlinear iterative optimization until any threshold is exceeded. Then, pop the first element in the keyframe queue, update the factor graph model and point cloud map, and then return to the data preprocessing module.

[0217] Regarding the content of S5.4, considering that the pure inertial maximum a posteriori estimation failure flag has been set, indicating that the robot is in a state of slow motion, so that the inertial parameters do not have good observability, the reason for the failure of the pure inertial maximum a posteriori estimation to converge is that the accelerometer bias and gyroscope bias of the IMU diverge during the optimization process, while the visual lidar combined maximum a posteriori estimation module is generally not affected by the slow motion of the robot and can estimate the pose of each key frame through the objective function. In order to ensure the robustness of the SLAM system initialization, the present invention designs a gravity direction deviation optimization module, which abandons the optimization of the inertial parameters and only completes the optimization of the deviation matrix G between the Z axis of the world coordinate system and the gravity direction. Therefore, all the variables to be optimized in the objective function are written in the form of vectors, which are expressed as follows:

[0218] χ g =[G] (28)

[0219] Regarding the content of S5.4, the iterative nonlinear optimization of the objective function still uses the Levenberg-Marquadt nonlinear optimization method, which is consistent with the method in step S2.4 and will not be repeated here.

[0220] S5.5: Use the obtained gravity direction deviation matrix to adjust the world coordinate system so that its Z axis is parallel to the gravity direction. Then determine whether more than 100 key frames have been processed cumulatively or more than 100 seconds have passed since the initialization program was started. If any of the conditions is met, clear the initialization queue, delete the point cloud map, delete the factor graph model, and end the initialization program. If both conditions are not met, pop out the first element in the key frame queue, update the factor graph model and point cloud map, and then return to the data preprocessing module.

[0221] This embodiment proposes a joint initialization method for a 3D lidar visual-inertial multi-sensor SLAM system. Through the design of a joint optimization scheme under multi-source heterogeneous sensor information fusion, it aims to improve the accuracy and robustness of the SLAM system initialization. The present invention divides the initialization of the multi-sensor fusion SLAM system into five modules: data preprocessing, visual lidar joint maximum a posteriori estimation, pure inertial maximum a posteriori estimation, visual inertial lidar joint tuning, and gravity direction deviation optimization module. The data preprocessing module realizes the data-level and target-level matching alignment of point clouds and images, removes dynamic feature points in images and dynamic point clouds in point clouds, and ensures the accuracy of sensor data used for initialization; the visual lidar joint maximum a posteriori estimation module constructs visual constraint factors based on the visual reprojection error between adjacent key frames, and constructs nodes based on the relative pose relationship of the lidar transformed into the absolute pose state, and then establishes a factor graph based on the factors and nodes to estimate the position state and rotation state of the robot at each key frame; the pure inertial maximum a posteriori estimation module adds an IMU pre-integration constraint factor to the factor graph to reconstruct the optimization objective function. The module is used to estimate the robot's velocity, IMU bias, and the deviation matrix between the Z axis of the world coordinate system and the direction of gravity at each key frame. The visual-inertial lidar joint optimization module does not change the factor graph structure, but transforms the batched and progressive multivariable optimization into a multivariable joint optimization. The convergence value of multiple batch optimizations is used as the initial value for optimization to further improve the accuracy of the state estimation results. The gravity direction deviation optimization module is designed to improve the robustness of the SLAM system initialization, ensuring that the world coordinate system deviation can be corrected even when the inertial parameters are not well observable due to the slow movement of the robot.

[0222] Implementation and verification of the present invention:

[0223] The present invention proposes a joint initialization method for a multi-sensor SLAM system for 3D lidar visual inertia, which is implemented using C++ programming. The present invention uses the KITTI dataset, one of the world's largest autonomous driving scene evaluation datasets, as a verification dataset. The KITTI dataset is collected during vehicle driving. The data collection work is jointly organized by the Karlsruhe Institute of Technology in Germany and Toyota Research Institute of America. Its data acquisition platform includes 4 optical lenses (2 grayscale cameras, 2 color cameras), 1 Velodyne 64-line 3D lidar, and 1 GPS / IMU positioning and navigation system.

[0224] Taking the 07 subset of the KITTI dataset as an example, the collection of this subset includes various motion states of the vehicle during driving, including vehicle acceleration and deceleration, speed change and turning, braking for obstacle avoidance, uphill and downhill driving, meeting and overtaking, etc., and also includes dynamic objects such as pedestrians and other moving vehicles in the road environment. In the field of SLAM, since there are currently no typical schemes for comparing three sensor initializations, this paper selects the initialization schemes of VINS-Mono and ORB-SLAM3 as comparison methods. The initialization schemes of these two methods are quite representative. Other open source SLAMs basically follow the initialization schemes of these two methods. VINS-Mono uses an analytical method to estimate the initialization state quantity, while ORB-SLAM3 uses an optimized method to estimate the initialization state quantity. Figure 5 A trajectory comparison diagram of the three initialization methods within a period of initialization is given. Figure 6 The relationship between the displacement of the trajectory on the X, Y, and Z axes and time during the initialization period of the three initialization methods is given. In the figure, the initialization scheme of the present invention is recorded as ours, the initialization scheme of VINS-Mono is recorded as VINS, and the initialization scheme of ORB-SLAM3 is recorded as ORB. Table 1 shows the initialization time and initialization average trajectory error of the three methods, t init Indicates the preprocessing time before initialization, t tol Represents the total initialization time. Although the initialization scheme proposed in the present invention consumes slightly more time than the other two methods, it can provide higher initialization accuracy.

[0225] Table 1 Initialization time and initialization average trajectory error

[0226]

[0227] In summary, the present invention proposes a joint initialization method for a SLAM system with multiple sensor fusion, involving three heterogeneous sensors: 3D lidar, camera, and IMU. By designing a joint initialization scheme for the fusion of information from the three sensors, the accuracy and robustness of the initialization results are improved while ensuring the rapid initialization of the SLAM system. The present invention decomposes the SLAM initialization process into multiple progressive submodules. Through such hierarchical decomposition, the multivariable optimization problem is transformed into a multiple batch optimization problem. The convergence difficulty of the multivariable optimization is reduced and the convergence speed of the multivariable optimization is improved by progressive optimization from easy to difficult. The visual inertial lidar joint tuning module of the present invention further optimizes all optimization variables, transforms the multivariable multiple batch optimization problem into a multivariable joint optimization problem, and uses the converged values ​​of the multiple batch optimizations as the initial values ​​of the variables, thereby ensuring the convergence speed of the joint optimization and further improving the estimation accuracy of all optimization variables. In order to solve the problem that the inertial parameters of the robot do not have good observability due to slow movement, which leads to the failure of SLAM system initialization, the present invention designs a gravity direction deviation optimization module to ensure that the Z axis of the world coordinate system is quickly aligned with the gravity direction, thereby improving the robustness of the SLAM system initialization.

[0228] Although the present invention is described herein with reference to specific embodiments, it should be understood that these embodiments are merely illustrative of the principles and applications of the invention. It should be understood that many modifications may be made to the illustrative embodiments, and that other arrangements may be devised, without departing from the spirit and scope of the invention as defined by the appended claims. It should be understood that the various dependent claims and features described herein may be combined in ways other than those described in the original claims. It should also be understood that features described in conjunction with individual embodiments may be used in conjunction with other described embodiments.

Claims

1. A joint initialization method for a multi-sensor SLAM system for 3D lidar visual inertial, characterized in that 5 modules: data preprocessing module, visual lidar joint maximum a posteriori estimation module, pure inertial maximum a posteriori estimation module, visual inertial lidar joint tuning module, gravity direction deviation optimization module, including the following steps: The data preprocessing module is used to receive and preprocess the data collected by each sensor, as well as select key frames and construct a point cloud map for initialization. By performing data-level fusion on the image data and the laser point cloud data, dynamic feature points of the image and the dynamic point cloud of the LiDAR are removed. The laser point cloud data is used as a depth reference to estimate the accurate depth values ​​of the static feature points of the image. Key frames are selected based on the distance traveled by the vehicle and a key frame queue for initialization is established for subsequent initialization operations. The module includes the following steps: S1.1: The SLAM system receives image data captured by the camera, point cloud data collected by the 3D lidar, and acceleration and angular velocity data collected by the IMU, synchronizes the timestamps of the image data, lidar data, and IMU data, and then executes S1.

2. S1.2: Use a classic image feature point extraction method (such as ORB, SIFT, etc.) to extract image feature points; use the camera distortion coefficient to perform camera imaging distortion correction on the extracted image feature points, and then execute S1.3; S1.3: Remove outliers from the LiDAR point cloud, use IMU measurement data to correct motion distortion on the LiDAR point cloud, downsample the point cloud data using voxel filtering, and extract feature information from the point cloud data, including corner points and surface points, and then execute S1.4; S1.4: Use the extrinsic matrix between the LiDAR and the camera to achieve data-level matching and alignment between the image and the laser point cloud. Perform 2D object detection on the image and 3D object detection on the laser point cloud to achieve object-level matching and alignment between the image and the laser point cloud. Then, remove dynamic feature points from the image and the laser point cloud, and then execute S1.

5. S1.5: Use the static feature points in the LiDAR point cloud auxiliary image to obtain accurate depth information, delete the static feature points that do not have a depth value, and then execute S1.6; S1.6: Determine whether the keyframe queue is empty. If so, set the current frame as a keyframe and insert it into the keyframe queue. Then, execute S1.

7. If not, perform ICP matching of feature points on the laser point cloud of the current frame and the laser point cloud of the previous keyframe to obtain the translation and rotation information between the two frames. Determine whether the current frame is a keyframe based on whether the obtained translation reaches the distance threshold. If so, insert the keyframe into the keyframe queue and execute S1.

7. If not, return to S1.

1. For the content of S1.6, the ICP matching of feature points between the laser point cloud of the current frame and the laser point cloud of the previous key frame refers to the scan-to-scan pose estimation of the feature points of the point cloud between the current key frame and the point cloud data of the previous key frame. The target optimization function is constructed based on the corner point residuals and the surface point residuals. The corner point residuals refer to the matching errors from the corner points in the laser point cloud of the current frame to the line features of the laser point cloud of the previous key frame. The surface point residuals refer to the matching errors from the surface points in the laser point cloud of the current frame to the surface features of the laser point cloud of the previous key frame. Assume that the relative rotation matrix and relative translation vector between the current frame and the previous key frame are Δq and Δp respectively, and the target function is constructed based on the corner point residuals and surface point residuals of the point cloud. The expression is as follows: e l (x k ,x′)=r1+r2 (1) Where x′ and x k They represent the pose states of the current frame and the previous key frame respectively, r1 represents the corner residual of the lidar, and r2 represents the surface residual of the lidar. For the corner residual, the specific expression is: Where, is the corresponding corner point in the current frame lidar coordinate system, It is the coordinate representation of the corner point of the current frame in the lidar coordinate system of the previous key frame. and is the value in the previous keyframe The coordinates of the two matching corner points, represents the cumulative sum of all corner residuals in the current frame, and ||·|| represents the modulo operation. For the surface point residual, the specific expression is: Where p l is the corresponding surface point in the current frame laser radar coordinate system, p m is the coordinate representation of the surface point of the current frame in the lidar coordinate system of the previous key frame, and n is the coordinate of p in the previous key frame. m The normal vector of the matching plane, It represents the cumulative sum of all surface residuals in the current frame, and ||·|| represents the modulo operation. S1.7: Determine whether the current keyframe is the first keyframe in the keyframe queue. If so, establish a world coordinate system based on the lidar coordinate system of the current keyframe as the reference coordinate system, and then execute S1.

8. If not, transform the laser point cloud of the current keyframe to the world coordinate system, and establish an image feature point matching relationship between the current keyframe and the adjacent keyframe based on the visual common view relationship between the current keyframe and the previous keyframe, and then execute S1.

8. For the content of S1.7, the image feature point matching relationship between adjacent key frames is realized using the fast nearest neighbor search (FLANN) matching method. For a feature point of the current key frame, the Euclidean distance between the feature point and each feature point in the feature point set of the adjacent key frame is calculated, and these Euclidean distances are stored through the KD-Tree structure. The matching point pair set is recursively searched from top to bottom to obtain the minimum Euclidean distance, thereby obtaining the best matching point pair for the feature point. This operation is performed on each feature point of the current key frame, and then a set of image feature point matching points between the current key frame and the adjacent key frames can be obtained. S1.8: Determine whether the key frame queue is full of 10 key frames. If not, return to S1.

1. If so, further determine whether the pure inertial maximum a posteriori estimation failure flag is set. If not, enter the visual lidar joint maximum a posteriori estimation module. If it is set, enter the gravity direction deviation optimization module. Regarding the content of S1.8, the function of the flag is to mark whether the pure inertia maximum a posteriori estimation module can be successfully executed. If the pure inertia maximum a posteriori estimation module cannot be successfully executed within the specified time or the specified number of times, then the flag is set, indicating that the robot is in a slow motion state, resulting in the inertia parameters being unobservable and difficult to converge. The subsequent initialization process only optimizes the gravity direction deviation matrix. The visual lidar combined with the maximum a posteriori estimation module is used to estimate the position and rotation state of the mobile robot at each keyframe. The relative pose quantities from point cloud matching between adjacent keyframes are used to derive the robot's pose quantities in the world coordinate system, which serve as node information in the factor graph. The matching relationship between image feature points between adjacent keyframes is used to establish visual reprojection constraints, which serve as factor information in the factor graph. The nodes and factors are then combined to form a factor graph optimization model, with the pose state of each keyframe being used as the optimization variable to obtain the robot's state information. Specifically, the model includes the following steps: S2.1: Based on the relative pose states between the key frames obtained by the data preprocessing module, obtain the state information of the robot at each key frame in the world coordinate system, including position and rotation, and then execute S2.2; S2.2: Based on the image feature point matching relationship between adjacent key frames in the key frame queue, use the visual perspective n-point projection algorithm to estimate the visual relative pose constraint relationship between each key frame, and then execute S2.3; Regarding the content of S2.2, the visual perspective n-point projection (PnP) algorithm estimates the visual relative pose constraint relationship between each key frame, which means that a set of 3D spatial points and their projection points on the 2D image plane are known, and the camera posture, including translation and rotation, is estimated through the corresponding matching relationship between the spatial points and the projection points. S2.3: Construct the state information of each key frame and the constraint relationship between the key frames into a factor graph, establish an optimization function based on the factor graph, and then execute S2.4; For the content of S2.3, the visual constraint factor is constructed based on the reprojection error. When a map point in space is projected onto a keyframe where the map point can be observed, the coordinates of the projected point should coincide with the observed coordinates of the map point. The reprojection error between the two types of coordinates is the residual of the visual constraint factor. Assume that there is a map point P in space, its index number is l, and its projection coordinates on the normalized plane of the kth keyframe are The observation coordinates at the kth key frame are Then there is the visual constraint residual e C (x k-1 ,x k )The expression is as follows: Where x k-1 and x k Respectively represent the robot state information corresponding to the k-1th key frame and the kth key frame, including position, attitude, velocity, IMU accelerometer bias and gyroscope bias; It can be estimated by the visual perspective projection geometric model, and the expression is: Where, represents the observation value of the map point P on the normalized plane at the k-1th key frame, and They represent the translation and rotation from the normalized coordinate system of the k-1th key frame to the normalized coordinate system of the kth key frame, respectively. Regarding the content of S2.3, the state information of each keyframe and the constraint relationship between keyframes are constructed as a factor graph. Figure 3 shows the structural diagram of the factor graph. The constraint relationship between keyframes refers to the constraint factors between keyframes obtained by the PnP algorithm based on the visual reprojection error. The state information of each keyframe refers to the relative pose relationship obtained from the laser point cloud and then converted to the absolute pose state. The optimization objective function is constructed based on the established factor graph, and the expression is as follows: Where, e C (x k-1 ,x k ) represents the visual constraint residual, χ c Represents all variables to be optimized of the objective function. There are 10 key frames in the key frame queue for initialization in the present invention, so N=10. Regarding the content of S2.3, the state information of the key frame includes the rotation state quantity and the translation state quantity. Taking the k-th key frame as an example, the variables to be optimized of the k-th key frame are written in the form of a vector, which can be expressed as: Where p k and q k Represent the position and posture of the kth key frame respectively. All variables to be optimized in the objective function are represented as vector χ c , of the following form: In the above two equations, setting N=10 indicates that there are 10 key frames in the key frame queue used for initialization. S2.4: Using the state information of each key frame as the optimization variable, perform iterative nonlinear optimization to determine whether the optimization result converges to the threshold neighborhood. If so, enter the pure inertial maximum a posteriori estimation module. If not, execute S2.

5. S2.5: Determine whether the optimization time or number of iterations exceeds the time threshold or the number threshold. If neither exceeds the time threshold or the number threshold, return to S2.4 to continue nonlinear iterative optimization until any threshold is exceeded. Then, pop the first element in the key frame queue, update the factor graph model and point cloud map, and then return to the data preprocessing module. The pure inertial maximum a posteriori estimation module adds an IMU pre-integration constraint factor to the factor graph constructed by the visual lidar combined with the maximum a posteriori estimation module. The optimization function is then reconstructed from the factor graph. The position and posture at each key frame are considered as known quantities. The velocity at each key frame, as well as the deviation matrix between the Z axis of the world coordinate system and the direction of gravity, the accelerometer bias and gyroscope bias of the IMU are estimated in a nonlinear iterative optimization manner. Finally, the world coordinate system is adjusted using the optimized gravity direction deviation matrix. The specific steps are as follows: S3.1: Based on the IMU data of each keyframe in the keyframe queue, construct an IMU pre-integration constraint factor between each keyframe, add the IMU pre-integration constraint factor to the factor graph, establish an optimization function based on the factor graph, and then execute S3.2; Regarding the content of S3.1, the factor graph model includes the visual reprojection constraint factor and the IMU pre-integration constraint factor. As shown in Figure 4, a schematic diagram of the factor graph model constructed by the pure inertial maximum a posteriori estimation module is given. Based on the factor graph model, the optimization objective function is established, and the expression is as follows: Where x k-1 and x k They represent the robot posture state information corresponding to the k-1th key frame and the kth key frame, respectively, and e B (x k-1 ,x k ) represents the IMU pre-integration constraint residual, e C (x k-1 ,x k ) represents the visual constraint residual. There are 10 key frames in the key frame queue used for initialization in the present invention, so N=10, χ imu Represents the variable to be optimized in the objective function. S3.2: Using the velocity, gravity deviation matrix, and IMU bias (including acceleration bias and gyroscope bias) of each keyframe as optimization variables, perform iterative nonlinear optimization to determine whether the optimization result converges to the threshold neighborhood. If not, execute S3.

3. If converged, use the optimized gravity deviation matrix to adjust the world coordinate system so that its Z axis is parallel to the gravity direction, and then enter the visual-inertial lidar joint tuning module. For the content of S3.2, the position and attitude of each key frame have been optimized in the visual lidar combined maximum a posteriori estimation module. In the pure inertial maximum a posteriori estimation module, the variables to be optimized for each key frame only include velocity v, IMU acceleration bias and gyroscope b. a Offset b ω , taking the kth key frame as an example, the variables to be optimized are written in the form of vectors, which can be expressed as: Where, v k 、 are the velocity of the kth key frame, the acceleration bias of the IMU, and the gyroscope bias, respectively. In addition, the deviation between the Z axis of the world coordinate system and the direction of gravity needs to be considered. The gravity direction deviation matrix G is optimized together as the optimization variable. In the pure inertial maximum a posteriori estimation module, the variable to be optimized of the objective function can be expressed as χ imu , of the following form: In the above two equations, setting N=10 indicates that there are 10 key frames in the key frame queue used for initialization. S3.3: Determine whether the optimization time or the number of iterations of the nonlinear optimization exceeds the time threshold or the number threshold. If neither exceeds the time threshold or the number threshold, return to S3.2 to continue the nonlinear iterative optimization until either threshold is exceeded, and then execute S3.4; S3.4: Determine whether the initialization execution time exceeds 20 seconds or the number of times the pure inertial maximum a posteriori estimation module has been executed exceeds 20 times. If neither exceeds, pop the first element in the keyframe queue, update the factor graph model and point cloud map, and then return to the data preprocessing module. If it exceeds, set the pure inertial maximum a posteriori estimation failure flag and return to the data preprocessing module. The visual-inertial lidar joint optimization module still uses the factor graph model established by the pure inertial maximum a posteriori estimation module and the objective function constructed based on this model. The results obtained by iterative optimization of the visual lidar joint maximum a posteriori estimation module and the pure inertial maximum a posteriori estimation module are used as initial values. All optimization variables of these two modules are re-optimized, and the batched, progressive multivariable optimization is transformed into a multivariable joint optimization to further improve the accuracy of the estimation results. The specific steps are as follows: S4.1: Using the robot's position, attitude, and velocity at each keyframe, the IMU's accelerometer bias, gyroscope bias, and gravity deviation matrix at each keyframe as optimization variables, perform iterative nonlinear optimization on the optimization function established in the pure inertial maximum a posteriori estimation module. The optimization results obtained by the combined visual lidar maximum a posteriori estimation module and the pure inertial maximum a posteriori estimation module are used as the initial values ​​of the optimization variables, and then execute S4.

2. For the content of S4.1, the factor graph model established by the pure inertial maximum a posteriori estimation module is still used. This factor graph model includes the visual reprojection constraint factor and the IMU pre-integration constraint factor. The position, attitude, velocity, IMU acceleration bias and gyroscope bias of each key frame are optimized simultaneously. Taking the kth key frame as an example, the variables to be optimized are written in the form of vectors, which can be expressed as: Where p k ,q k 、v k 、 Represent the position, attitude, velocity, IMU acceleration bias and gyroscope bias of the kth key frame respectively. In addition, the gravity direction deviation matrix G needs to be optimized. The initial values ​​of all optimization variables are the optimization results of the visual lidar joint maximum a posteriori estimation module and the pure inertial maximum a posteriori estimation module. All variables to be optimized in the objective function are represented as vectors χ cil , of the following form: In the above two equations, setting N=10 indicates that there are 10 key frames in the key frame queue used for initialization. S4.2: Determine whether the optimization result converges within the threshold neighborhood. If so, execute S4.

3. If not, determine whether the optimization time or the number of iterations exceeds the time threshold or the number threshold. If neither exceeds, return to S4.1 to continue nonlinear iterative optimization until any threshold is exceeded. Then determine whether the initialization time exceeds 100 seconds. If not, pop out the first element in the initialization keyframe queue, update the factor graph model and point cloud map, and then return to the data preprocessing module. If it exceeds 100 seconds, end the initialization program. S4.3: Use the optimized gravity direction deviation matrix to adjust the world coordinate system again so that its Z axis is parallel to the gravity direction. Then, clear the keyframe queue, delete the factor graph model, retain the point cloud map created according to the keyframes during the initialization process, and finally end the initialization process. The gravity direction deviation optimization module is designed to improve the robustness of SLAM system initialization. When the mobile robot moves slowly, the inertial parameters are unobservable, which easily leads to the difficulty of convergence of the pure inertial maximum a posteriori estimation module, resulting in the failure of the initialization process. To address this problem, and considering that the accuracy of the world coordinate system is crucial to the SLAM system, the present invention designs a gravity direction deviation optimization module. This module reconstructs the factor graph and reconstructs the target optimization function. Only the gravity direction deviation matrix is ​​used as the optimization variable, and other state quantities are not involved in the optimization. The specific steps are as follows: S5.1: Match the laser point cloud data of each key frame in the key frame queue with the point cloud map, estimate the state information of the robot at each key frame (including translation and rotation), and then execute S5.2; For the content of S5.1, the laser point cloud data of each key frame is matched with the point cloud map respectively. The scan-to-map method is used to perform ICP matching on the point cloud of each key frame and the point cloud map in the world coordinate system. The corner points in the key frame are matched with the line features in the point cloud map, and the surface points in the key frame are matched with the surface features in the point cloud map. S5.2: Based on the image feature point matching relationship between adjacent keyframes in the keyframe queue, use the visual perspective n-point projection algorithm to estimate the relative pose relationship between the keyframes, thereby constructing the visual reprojection constraint factor between the keyframes, and then execute S5.3; S5.3: Based on the IMU data of each keyframe in the keyframe queue, construct an IMU pre-integration constraint factor between each keyframe, construct the state information of each keyframe, the visual reprojection constraint factor between each keyframe, and the IMU pre-integration constraint factor into a factor graph, then establish a target optimization function based on the factor graph, and then execute S5.4; Regarding the content in S5.3, the factor graph model includes visual reprojection constraint factors and IMU pre-integration constraint factors. The factor graph structure is similar to the factor graph structure constructed in step S3.1, with two differences: first, the expression of the IMU pre-integration constraint residual is different; second, the method of obtaining the node state in the factor graph is different. The specific differences have been described above and will not be repeated here. Based on the factor graph model, the optimization objective function is established, and the expression is as follows: Where x k-1 and x k They represent the robot posture state information corresponding to the k-1th key frame and the kth key frame, e′ B (x k-1 ,x k ) represents the IMU pre-integration constraint residual, e C (x k-1 ,x k ) represents the visual constraint residual, χ g Represents the variable to be optimized in the objective function. S5.4: Using the gravity direction deviation matrix as the optimization variable, perform iterative nonlinear optimization on the target optimization function to determine whether the optimization result converges to the threshold neighborhood. If so, execute S5.

5. If not, determine whether the optimization time or number of iterations exceeds the time threshold or the number threshold. If neither exceeds, continue nonlinear iterative optimization until any threshold is exceeded. Then, pop the first element in the keyframe queue, update the factor graph model and point cloud map, and then return to the data preprocessing module. Regarding the content of S5.4, in order to ensure the robustness of the SLAM system initialization, the present invention designs a gravity direction deviation optimization module. This module abandons the optimization of inertial parameters and only optimizes the deviation matrix G between the Z axis of the world coordinate system and the gravity direction. Therefore, all variables to be optimized in the objective function are written in the form of vectors, which are expressed as follows: x g =[G] (15) S5.5: Use the obtained gravity direction deviation matrix to adjust the world coordinate system so that its Z axis is parallel to the gravity direction. Then determine whether more than 100 key frames have been processed cumulatively or more than 100 seconds have passed since the initialization program was started. If any of the conditions is met, clear the initialization queue, delete the point cloud map, delete the factor graph model, and end the initialization program. If both conditions are not met, pop out the first element in the key frame queue, update the factor graph model and point cloud map, and then return to the data preprocessing module.

Images