A SLAM method for Lidar-IMU fusion of ground mobile robots

Abstract

The present invention belongs to the field of simultaneous positioning and mapping (SLAM), and discloses a SLAM method for Lidar-IMU fusion of a ground mobile robot, comprising the following steps: Step 1: Data preprocessing; Step 2: Constructing a submap; Step 3: Creating a map and an optimization factor graph; Step 4: Optimizing a pose graph. The present invention can add ground constraints to the robot on an undulating road surface, increase the constraints in the vertical direction, and can well reduce the cumulative error. The entire experimental robot trajectory is about 1000m long. Compared with the traditional algorithm, the method of the present invention has a significant improvement in accuracy, especially in the z direction. The translation deviation (root mean square error) of loam is 18.461m, the translation deviation of lio_sam is 15.672m, and the translation deviation of the algorithm of the present invention is 8.331m. The trajectory accuracy obtained by the algorithm has been greatly improved.

Description

Technical Field

[0001] The present invention belongs to the field of simultaneous localization and mapping (SLAM), and in particular relates to a SLAM method for Lidar-IMU fusion of a ground mobile robot. Background Art

[0002] Simultaneous localization and environmental mapping are prerequisites for autonomous robot navigation. With the recent development of the robotics industry, state estimation, localization, and mapping have become fundamental prerequisites for intelligent mobile robots to perform feedback control, obstacle avoidance, and planning. In practical applications, vision sensors or lidar sensors are generally used to achieve high-performance simultaneous localization and mapping capabilities for robots. Vision-based methods typically use a monocular or stereo camera and triangulate features on consecutive images to determine camera motion. Although vision-based methods are particularly well-suited for position recognition, their sensitivity to initialization, illumination, and distance makes them difficult to use solely for autonomous navigation systems. LiDAR-based methods, on the other hand, are unaffected by changes in illumination. Recently, with the emergence of long-range, high-resolution 3D lidar and the application of solid-state lidar, lidar has become more suitable for directly capturing details of the environment in 3D space.

[0003] Over the past two decades, numerous lidar-based state estimation and mapping methods have been proposed. Among them, the LOAM algorithm, suitable for low-drift and real-time state estimation and mapping, is one of the most widely used methods. This method uses lidar and an inertial measurement unit (IMU) and employs point-to-line and point-to-plane constraints to achieve excellent performance. However, the IMU is only used for point cloud dedistortion and does not participate in optimization. In contrast, the Lio_sam algorithm utilizes factor graph optimization to achieve a tight fusion of the IMU and lidar. While these traditional algorithms have achieved great success, they can suffer from significant drift in large scenes and GPS denial conditions. This is because they often indiscriminately extract feature points from the point cloud as matching points, without considering ground constraints. For ground mobile robots, feature points primarily come from feedback from surrounding objects. These feature points can only constrain the horizontal dimension, while vertical dimensions can only be extracted from the ground. This results in insufficient vertical constraints and is prone to drift in large outdoor scenes. Some algorithms, while considering ground constraints, simply treat the ground as a plane. In a real environment, it is impossible for a robot to always run on the same ground plane.

[0004] Unlike conventional 6-DOF aircraft, ground-moving robots are always subject to ground constraints. If state estimation is performed directly according to the traditional SE(3) pose, since the LiDAR primarily collects laser points above the road surface, this can easily lead to significant drift in the z direction during point cloud matching optimization. Furthermore, on undulating roads, while ground-moving robots are subject to ground constraints, they may be subject to different ground planes. If we assume that the robot always operates on the same ground plane and add ground constraints based on this, the accuracy of state estimation will be affected. Summary of the Invention

[0005] The purpose of the present invention is to provide a SLAM method for Lidar-IMU fusion of ground mobile robots to solve the above-mentioned technical problems.

[0006] To solve the above technical problems, the specific technical solution of the present invention for the SLAM method of Lidar-IMU fusion for ground mobile robots is as follows:

[0007] A SLAM method for Lidar-IMU fusion of ground mobile robots includes the following steps:

[0008] Step 1: Data preprocessing; including point cloud dedistortion and feature extraction;

[0009] Step 2: Keyframe selection and submap construction: Introducing the keyframe concept in visual SLAM, using the lidar information between two keyframes to construct a local submap;

[0010] Step 3: Factor graph optimization of keyframes: associate the keyframes with the world coordinate system through factor graph optimization, and construct a factor graph to jointly optimize the lidar keyframes and IMU pre-integration;

[0011] Step 4: Pose graph optimization; a globally consistent environment map is obtained through pose graph optimization.

[0012] Furthermore, the step 1 includes point cloud dedistortion and feature extraction;

[0013] The point cloud dedistortion is as follows: in the factor optimization process, the IMU pre-integration provides the pre-integration factor, and the IMU deviation in the optimization process will also participate in the optimization as an optimization parameter. The IMU data corrected by the obtained IMU deviation is used to estimate the position of the carrier at any time, and the laser point is transformed into a unified coordinate system to perform point cloud distortion correction.

[0014] The feature extraction is as follows: extracting feature point clouds from the dedistorted point clouds, extracting edge points and surface points as matching feature points. For any point p, find 10 consecutive points from the same row scan as p, represented by S, evenly distributed on both sides of p. The roughness of p can be calculated by the following formula:

[0015]

[0016] Where |S| is the number of points in S, which is 10, r i Indicates the depth of the i-th point;

[0017] Select the point with smaller roughness value as the plane point and the point with larger roughness value as the edge point. The feature point obtained from any laser radar scan is expressed as represents a plane point, Indicates edge points.

[0018] Furthermore, the step 2 includes the selection of key frames and the construction of submaps;

[0019] The selection of the key frame: The key frame concept commonly used in visual SLAM is introduced. The lidar information between two key frames is used to construct a local submap. Assuming that the ground mobile robot is a rigid body, the changes in the xy plane of the robot body coordinate system at the time of adjacent lidar scans can reflect the changes in the ground plane. When a new lidar scan arrives, the ground normal vector e3 of the current frame is converted to the coordinate system e′ of the previous frame. 3i , then the angle between the ground planes scanned by adjacent lidars is:

[0020]

[0021] In the formula is the transposed matrix of e3;

[0022] If θ is greater than the set threshold θ th , then it is considered that the slope of the ground plane has changed, and the robot is no longer on the same ground plane as the previous scan, and this scan will be selected as the key frame;

[0023] The construction of the submap: If the new Lidar scan is a key frame, a new submap is created and the key frame is used as the reference frame of the submap. At this time, the robot's body coordinate system is the coordinate system of the submap, and M is used. i Then, a maximum a posteriori estimate including the lidar odometry constraint and the ground constraint is constructed to optimize the pose transformation of the subsequent ordinary radar frame relative to the submap;

[0024] When the new LiDAR normal frame F jWhen it arrives, its feature points are obtained after preprocessing Combine it with the submap M i Match, at this point, the local submap has the following lidar frame:

[0025] M i ={L i ,F i,0 ,F i,1 …F i,j-1}

[0026] Where L i Represents submap M i The reference key frame, F i,k, k=1,2,…j-1 represents the existing common frame in the submap. For the scan matching method, the same method as loam is used. First, the feature points of this common frame are converted into the submap where it is located. The initial transformation is obtained by using the predictions from the calibrated IMU, for For each feature point in , find the corresponding edge point and plane point in the submap, and then minimize the distance from the feature point to its corresponding edge and plane to obtain the most optimal transformation:

[0027]

[0028] In the formula

[0029]

[0030]

[0031]

[0032]

[0033] In the formula They are the edge feature points and surface feature points of the current frame, yes The point obtained after conversion to the local coordinate system, Is the local map The corresponding matching edge points, Is the local map Corresponding matching face points;

[0034] All lidar frames in the submap are located on the same ground plane. Therefore, a plane constraint is added to each lidar normal frame using the SE(3) pose-SE(2) constraint method. Ideally, in the submap, the robot moves in a plane, and the transformation between the normal frame and the submap is parameterized as the SE(2) pose, whose Lie algebra is expressed as v = [v1 v2φ], where φ is the rotation component and (v1, v2) represents the translation component. In the actual environment, due to the rugged terrain and the shaking of the robot itself during the robot's movement, the robot's posture has motion interference in dimensions other than SE(2). Therefore, instead of directly parameterizing the robot posture on SE(2), the posture is restored from SE(2) to SE(3) through Lie algebra:

[0035] ξ=[v1 v2 0 0 0 φ] T

[0036] At the same time, the Gaussian model is used to model the errors of motion disturbances in the other three dimensions, expressing The amplitude of the motion perturbations allowed in the other three dimensions, which is determined by the terrain conditions and the robot structure;

[0037] Therefore, the residual of the ground plane constraint is defined as:

[0038]

[0039] in, is the measurement of the planar constraint, which can be obtained by projecting the current pose estimate into SE(2) space and then recovering it into SE(3):

[0040]

[0041] The Jacobian matrix and covariance matrix of the plane constraint are:

[0042]

[0043]

[0044] In the formula are the variances of the robot states in the z, roll, and pitch directions, respectively.

[0045] Furthermore, step 2 adopts a heuristic method for the case where the robot moves on the same ground plane for too long and a key frame cannot be selected. Compared with the state of the previous frame, the robot rotates or translates beyond a defined threshold at this time, and this scan is considered a key frame. If the current scan does not meet the above two conditions, the scan is a normal frame. The key frame will then be used for joint optimization with the IMU, while the normal frame is used to build the submap.

[0046] Furthermore, the step 3 includes the following specific steps:

[0047] The keyframes are associated with the world coordinate system through factor graph optimization, and a factor graph is constructed to jointly optimize the lidar keyframes and the IMU pre-integration. The factor graph includes the following three types of factors:

[0048] LiDAR Odometry Factor: When the new keyframe L i When it arrives, after preprocessing, the edge point is obtained and plane points At this point, this new state node is added to the factor graph, and this keyframe is associated with the previous submap M i-1 For matching, the submap contains the following lidar frames:

[0049] M i-1 ={L i-1 ,F i-1,0 ,F i-1,1 ,F i-1,2 …F i-1,n}

[0050] Where n is the submap M i-1 The number of common frames in the matrix is ​​calculated. The matching method is the same as that described in step 3. The relative pose transformation between adjacent key frames is obtained by matching. At this time, the new key frame pose is used as a node, and the relative pose is added to the factor graph as a factor.

[0051] IMU pre-integration factor: The IMU measurement model is as follows:

[0052]

[0053]

[0054] IMU data are measured in their body coordinate system and are subject to additive noise n a , n w , acceleration deviation and gyro bias The effect of,the bias is modeled as a random walk whose derivative is Gaussian, and the additive noise of the accelerometer and,gyroscope is assumed to be Gaussian distributed;

[0055] This gives the final pre-integration form between two adjacent keyframes:

[0056]

[0057]

[0058]

[0059] In the formula

[0060]

[0061]

[0062]

[0063] In addition to efficiency, the application of IMU pre-integration provides a factor for relative pose constraints to the factor graph;

[0064] Loop closure factor: Using the loop closure detection method based on Euclidean distance, the Euclidean distance is used as the prior, and the key frame closest to the current key frame and whose relative transformation of the pose is less than the set threshold is taken as the candidate frame. Then, the candidate frame and the n key frames before and after it and their corresponding local submaps are used as the matching point cloud of the current key frame to obtain the relative transformation between the current frame and the candidate frame. This transformation is added to the factor graph as a loop closure factor.

[0065] Furthermore, step 4 includes the following specific steps:

[0066] Sliding window-based pose optimization is used to fix the ordinary frame to the world coordinate system and correct the key frame pose to reduce the accumulation of errors. In the process of constructing the local submap, the relative pose of the ordinary frame and the submap is obtained. Through factor graph optimization, the pose of the key frame in the world coordinate system is obtained. In the sliding window, the relative pose of the ordinary frame and the absolute pose of the key frame are optimized together to perform pose graph optimization. During the optimization process, the pose of the earliest key frame or loop key frame is fixed. During marginalization, the key frame and ordinary frame in the submap that enters the pose graph optimization earliest are directly deleted. Finally, through the above optimization, we obtain a globally consistent environment map.

[0067] The present invention's SLAM method for Lidar-IMU fusion of ground mobile robots has the following advantages: the present invention can add ground constraints to the robot on undulating roads, increase vertical constraints, and effectively reduce cumulative errors. The entire experimental robot trajectory is about 1000m long. Compared with traditional algorithms, the method of the present invention has greatly improved accuracy, especially in the z direction. The translation deviation (RMS error) of loam is 18.461m, the translation deviation of lio_sam is 15.672m, and the translation deviation of the algorithm of the present invention is 8.331m. The trajectory accuracy obtained by this algorithm has been greatly improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0068] Figure 1 This is a diagram of the Lidar-IMU fusion framework in the method of the present invention;

[0069] Figure 2 Schematic diagram of three optimization methods in the method of the present invention;

[0070] Figure 3 Schematic diagram of the projection of the trajectories obtained by various algorithms on the same dataset in the xy direction;

[0071] Figure 4 Schematic diagram of the projection of the trajectories obtained by various algorithms on the same dataset in the z direction. DETAILED DESCRIPTION

[0072] In order to better understand the purpose, structure and function of the present invention, the following is a further detailed description of the SLAM method of Lidar-IMU fusion for ground mobile robots in conjunction with the accompanying drawings.

[0073] like Figure 1As shown in the figure, a SLAM method for Lidar-IMU fusion for ground mobile robots consists of four parts. After receiving measurement data from the Lidar and IMU, the system first enters the point cloud preprocessing module, where the point cloud is dedistorted using the IMU's estimated state to obtain an undistorted point cloud. The point cloud is then segmented using depth map projection, and the ground plane is fitted. Edge feature points and surface feature points are extracted from the segmented point cloud as matching point clouds. Next, the Lidar scan is divided into keyframes and normal frames based on ground changes. Ground changes can be measured using the IMU. Ground mobile robots are generally considered rigid bodies. The ground plane changes between the two frames can be obtained by measuring the pose changes between the previous and next IMU frames. If the ground plane change exceeds a set threshold, the Lidar frame is selected as a keyframe; otherwise, it is a normal frame. If the scan is a normal frame, the system enters the local map construction module. In this module, a maximum a posteriori estimate (MAP) is constructed, including Lidar frame constraints and ground constraints. The transformation between the current frame and the local map is optimized and the point cloud of the frame is converted to the local map. If the scan is a keyframe, the local map creation and factor graph optimization module proceeds. This module primarily performs two tasks: first, a new local map is created. The keyframe serves as the reference frame for the new local map, and its body coordinate system is the local map's coordinate system. Subsequent normal frames are transformed into this local map through maximum posterior estimation. Simultaneously, the factor graph optimization module is initiated to determine the pose of the keyframe in the world coordinate system. Normal frames are not subject to this factor graph optimization. This factor graph includes three types of factors: an IMU pre-integration factor derived from IMU measurements between two keyframes using pre-integration techniques; a lidar odometry factor, which represents the relative pose of the current and previous frames obtained through point cloud matching; and a loop detection factor. At this point, the relative pose between the normal frame and the local map is determined through maximum a posteriori estimation in the local map. The keyframe pose is then optimized to its world coordinate pose through factor graph optimization. Finally, these two poses enter the final module, the sliding window-based pose graph optimization module. This module uses the pose graph to optimize the poses described above. Further optimization is performed within a sliding window, associating the common frames with the world coordinate system to achieve a globally consistent map. During the sliding optimization process, the pose of the oldest keyframe in the sliding window is always fixed. Because we use pose optimization, the pose of each frame is only related to the measurements of the two frames before and after it. During marginalization, we can simply discard the information of the oldest frame. This module not only associates the pose of the common frames with the world coordinate system, but also corrects the pose of the keyframes, reducing error accumulation.

[0074] The present invention provides a SLAM method for Lidar-IMU fusion of a ground mobile robot, comprising the following steps:

[0075] 1) Data preprocessing

[0076] Point cloud distortion: Point cloud distortion occurs when the laser radar captures data, but the motion of the carrier causes the points in a frame of the point cloud to be acquired at different times. This means that the coordinate systems of different laser points are inconsistent. During factor optimization, the IMU pre-integration provides pre-integration factors, and the IMU deviation is also included in the optimization process as an optimization parameter. The IMU data corrected for IMU deviations can be used to estimate the carrier's pose at any time, transform the laser points into a unified coordinate system, and perform point cloud distortion correction. The pose obtained from the IMU also provides a good initial value for inter-frame matching.

[0077] Feature Extraction: Next, we extract feature point clouds from the dedistorted point cloud. We extract edge points and surface points as matching feature points. For any point p, we find 10 consecutive points, denoted by S, from the same scan line as p, evenly spaced on both sides of p. The roughness of p can then be calculated using the following formula:

[0078]

[0079] Where |S| is the number of points in S, which is 10 in this invention, r i Indicates the depth of the i-th point.

[0080] We select points with smaller roughness values ​​as plane points and points with larger roughness values ​​as edge points. The feature points obtained from any LiDAR scan can be expressed as represents a plane point, Indicates edge points.

[0081] 2) Keyframe selection and sub-map construction

[0082] Keyframe selection: To ensure the real-time performance of the algorithm, we introduced the keyframe concept commonly used in visual SLAM. But the difference is that instead of discarding the lidar information between two keyframes, we use them to construct a local submap. In the algorithm, we assume that the ground mobile robot is a rigid body, so the changes in the xy plane of the robot's body coordinate system at the moment of adjacent lidar scanning can reflect the changes in the ground plane. When a new lidar scan arrives, we convert the ground normal vector e3 of the current frame to e′ in the coordinate system of the previous frame. 3i , then the angle between the ground planes scanned by adjacent lidars is:

[0083]

[0084] In the formula is the transposed matrix of e3.

[0085] If θ is greater than the threshold θ we set th, the slope of the ground plane is considered to have changed. The robot is no longer on the same ground plane as the previous scan. This scan will be selected as a keyframe. We also use a simple but effective heuristic method for the case where the robot moves on the same ground plane for too long to select a keyframe. Compared to the state of the previous frame, the robot rotation or translation exceeds the threshold we define, and we consider this scan to be a keyframe. If the current scan does not meet the above two conditions, then the scan is a normal frame. Figure 2 As shown, the keyframes are then used for joint optimization with the IMU, while the normal frames are used to build the submap.

[0086] Submap construction: If the new Lidar scan is a keyframe, we need to create a new submap and use the keyframe as the reference frame of the submap. At this time, the robot's body coordinate system is the coordinate system of the submap, and M is used to represent the submap. i Indicates that Figure 2 Then, we construct a maximum a posteriori estimate that includes the lidar odometry constraint and the ground constraint, and optimize the pose transformation of the subsequent ordinary radar frame relative to the submap.

[0087] When the new LiDAR normal frame F j When it arrives, we obtain its feature points after preprocessing We compare it with the submap M i At this point, the local submap has the following lidar frames:

[0088] M i ={L i ,F i,0 ,F i,1 …F i,j-1}

[0089] Where L i Represents submap M i The reference key frame, F i,k, k=1,2,…j-1 represents the common frames in the submap. For the scan matching method, we use the same method as loam because it is computationally efficient and robust in various challenging environments. We first convert the feature points of this common frame into the submap where it is located to obtain The initial transformation is obtained by using the predictions from the calibrated IMU. For each feature point in , we find the corresponding edge point and plane point in the submap. Then, we minimize the distance from the feature point to its corresponding edge and plane to obtain the most optimal transformation:

[0090]

[0091] In the formula

[0092]

[0093]

[0094]

[0095]

[0096] In the formula They are the edge feature points and surface feature points of the current frame respectively. yes The point obtained after transformation to the local coordinate system. Is the local map The corresponding matching edge points. Is the local map The corresponding matching points.

[0097] All lidar frames in the submap are located on the same ground plane. Therefore, we use the SE(3) pose-SE(2) constraint method to add a plane constraint to each lidar normal frame. Ideally, in the submap, the robot moves in a plane. We can parameterize the transformation between the normal frame and the submap as an SE(2) pose, whose Lie algebra can be expressed as v = [v1 v2 φ], where φ is the rotation component and (v1, v2) represents the translation component. However, in the actual environment, due to the rough terrain and the shaking of the robot itself during the robot's movement, the robot's posture has motion interference in dimensions other than SE(2). Therefore, instead of directly parameterizing the robot posture on SE(2), we recover the posture from SE(2) to SE(3) through Lie algebra:

[0098] ξ=[v1 v2 0 0 0 φ] T

[0099] At the same time, we did not ignore the motion disturbances in the other three dimensions, but used Gaussian models to model their errors, expressing The amplitude of the motion perturbations allowed in the other three dimensions is determined by the terrain conditions and the robot structure.

[0100] Therefore, the residual of the ground plane constraint is defined as:

[0101]

[0102] in, is the measurement of the planar constraint, which can be obtained by projecting the current pose estimate into SE(2) space and then recovering it into SE(3):

[0103]

[0104] The Jacobian matrix and covariance matrix of the plane constraint are:

[0105]

[0106]

[0107] In the formula are the variances of the robot states in the z, roll, and pitch directions, respectively.

[0108] 3) Factor graph optimization of key frames

[0109] In this section, we associate keyframes with the world coordinate system through factor graph optimization. We will construct a factor graph to jointly optimize the lidar keyframes and the IMU pre-integration. This factor graph includes the following three types of factors.

[0110] LiDAR Odometry Factor: When the new keyframe L i When it arrives, after preprocessing, we get the edge point and plane points At this point, we need to add this new state node to the factor graph. We combine this keyframe with the previous submap M i-1 (rather than the global map) for matching. The submap contains the following lidar frames:

[0111] M i-1 ={L i-1 ,F i-1,0 ,F i-1,1 ,F i-1,2 …F i-1,n}

[0112] Where n is the submap M i-1 The number of common frames in the graph. The matching method is the same as above. Through matching, we obtain the relative pose transformation between adjacent keyframes. At this time, the new keyframe pose is added as a node, and the relative pose is added as a factor in the factor graph.

[0113] IMU pre-integration factor: The IMU measurement model is as follows:

[0114]

[0115]

[0116] IMU data are measured in their body coordinate system and are subject to additive noise n a , n w , acceleration deviation and gyro bias In general, we model the bias as a random walk whose derivative is Gaussian, and the additive noise of the accelerometer and gyroscope is assumed to be Gaussian.

[0117] Here, we give the final pre-integration form between two adjacent keyframes:

[0118]

[0119]

[0120]

[0121] In the formula

[0122]

[0123]

[0124]

[0125] In addition to efficiency, the application of IMU pre-integration also naturally provides a factor for relative pose constraints to the factor graph.

[0126] Loop closure factor: This paper uses a loop closure detection method based on Euclidean distance. We use the Euclidean distance as a prior and select the keyframe closest to the current keyframe whose relative pose transformation is less than a set threshold as a candidate frame. We then use the candidate frame, its preceding and following n keyframes, and their corresponding local submaps as the matching point cloud for the current keyframe. Using this method, we obtain the relative transformation between the current and candidate frames, which we add to the factor graph as a loop closure factor.

[0127] 4) Pose graph optimization

[0128] In this section, the pose optimization based on sliding window is used to fix the normal frame to the world coordinate system and correct the key frame pose to reduce the accumulation of errors. In the process of constructing the local submap, we get the relative pose of the normal frame and the submap. Through factor graph optimization, we get the pose of the key frame in the world coordinate system. Figure 1 and Figure 2 As shown, in a sliding window, the relative poses of regular frames and the absolute poses of keyframes are combined to perform pose graph optimization. During optimization, the pose of the earliest keyframe or looped keyframe is fixed. During marginalization, we simply delete the keyframe and regular frame from the submap that enters pose graph optimization earliest, as the submap optimization is only relevant to the two frames before and after. Finally, through these optimizations, we obtain a globally consistent environment map.

[0129] The present invention can add ground constraints to the robot on undulating roads, increase vertical constraints, and effectively reduce cumulative errors. Figure 3 , Figure 4 The entire experimental robot trajectory is approximately 1000 meters long. Compared with the traditional algorithm, the proposed method significantly improves accuracy, especially in the z-direction. The translational deviation (RMS error) of loam is 18.461 meters, the translational deviation of lio_sam is 15.672 meters, and the translational deviation of the proposed algorithm is 8.331 meters. This significantly improves the trajectory accuracy obtained by the proposed algorithm.

[0130] It will be understood that the present invention is described by way of some embodiments, and it will be appreciated by those skilled in the art that various changes or equivalent substitutions may be made to these features and embodiments without departing from the spirit and scope of the present invention. In addition, under the teachings of the present invention, these features and embodiments may be modified to adapt to specific circumstances and materials without departing from the spirit and scope of the present invention. Therefore, the present invention is not limited to the specific embodiments disclosed herein, and all embodiments falling within the scope of the claims of this application are intended to be protected by the present invention.

Claims

1. A SLAM method for Lidar-IMU fusion of ground mobile robots, characterized in that: The steps include: Step 1: Data preprocessing; including point cloud dedistortion and feature extraction; Step 2: Keyframe selection and submap construction: Introducing the keyframe concept in visual SLAM, using the lidar information between two keyframes to construct a local submap; Step 2 includes the selection of key frames and the construction of submaps; The selection of the key frame: the key frame concept in visual SLAM is introduced, and the lidar information between two key frames is used to construct a local submap. Assuming that the ground mobile robot is a rigid body, the change of the xy plane of the robot body coordinate system at the time of adjacent lidar scanning can reflect the change of the ground plane. When a new lidar scan arrives, the ground normal vector e3 of the current frame is converted to the coordinate system e′ of the previous frame. 3i , then the angle between the ground planes scanned by adjacent lidars is: In the formula is the transposed matrix of e3; If θ is greater than the set threshold θ th , then it is considered that the slope of the ground plane has changed, and the robot is no longer on the same ground plane as the previous scan, and this scan will be selected as the key frame; The construction of the submap: If the new Lidar scan is a key frame, a new submap is created and the key frame is used as the reference frame of the submap. At this time, the robot's body coordinate system is the coordinate system of the submap, and M is used. i Then, a maximum a posteriori estimate including the lidar odometry constraint and the ground constraint is constructed to optimize the pose transformation of the subsequent ordinary radar frame relative to the submap; When the new LiDAR normal frame F j When it arrives, its feature points are obtained after preprocessing Combine it with the submap M i Match, at this point, the local submap has the following lidar frame: M i ={L i ,F i,0 ,F i,1 …F i,j-1 } Where L i Represents submap M i The reference key frame, F i,k ,k=1,2,…j-1 represents the common frame in the submap. For the scan matching method, the same method as loam is used. First, the feature points of this common frame are converted into the submap where it is located. The initial transformation is obtained by using the predictions from the calibrated IMU, for For each feature point in , find the corresponding edge point and plane point in the submap, and then minimize the distance from the feature point to its corresponding edge and plane to obtain the most optimal transformation: In the formula In the formula They are the edge feature points and surface feature points of the current frame, yes The point obtained after conversion to the local coordinate system, Is the local map The corresponding matching edge points, Is the local map Corresponding matching face points; All lidar frames in the submap are located on the same ground plane. Therefore, a plane constraint is added to each lidar normal frame using the SE(3) pose-SE(2) constraint method. Ideally, in the submap, the robot moves in a plane, and the transformation between the normal frame and the submap is parameterized as the SE(2) pose, whose Lie algebra is expressed as v = [v1 v2 φ], where φ is the rotation component and (v1, v2) represents the translation component. In the actual environment, due to the rugged terrain and the shaking of the robot itself during the robot's movement, the robot's posture has motion interference in dimensions other than SE(2). Therefore, instead of directly parameterizing the robot posture on SE(2), the posture is restored from SE(2) to SE(3) through Lie algebra: ξ=[v1 v2 0 0 0 φ] T At the same time, the Gaussian model is used to model the errors of motion disturbances in the other three dimensions, expressing The amplitude of the motion perturbations allowed in the other three dimensions, which is determined by the terrain conditions and the robot structure; Therefore, the residual of the ground plane constraint is defined as: in, is the measurement of the planar constraint, obtained by projecting the current pose estimate into SE(2) space and then restoring it to SE(3): The Jacobian matrix and covariance matrix of the plane constraint are: In the formula are the variances of the robot states in the z, roll, and pitch directions respectively; Step 2 uses a heuristic method for the case where the robot moves on the same ground plane for too long and a keyframe cannot be selected. Compared with the state of the previous frame, the robot's rotation or translation exceeds a defined threshold at this time, and this scan is considered a keyframe. If the current scan does not meet the above two conditions, the scan is a normal frame. The keyframe will then be used for joint optimization with the IMU, while the normal frame is used to build the submap; Step 3: Factor graph optimization of keyframes: associate the keyframes with the world coordinate system through factor graph optimization, and construct a factor graph to jointly optimize the lidar keyframes and IMU pre-integration; Step 4: Pose graph optimization; obtain a globally consistent environment map through pose graph optimization; Sliding window-based pose optimization is used to fix the ordinary frame to the world coordinate system and correct the key frame pose to reduce the accumulation of errors. In the process of constructing the local submap, the relative pose of the ordinary frame and the submap is obtained. Through factor graph optimization, the pose of the key frame in the world coordinate system is obtained. In the sliding window, the relative pose of the ordinary frame and the absolute pose of the key frame are optimized together to perform pose graph optimization. During the optimization process, the pose of the earliest key frame or loop key frame is fixed. During marginalization, the key frame and ordinary frame in the submap that enters the pose graph optimization earliest are directly deleted. Finally, a globally consistent environment map is obtained through the above optimization.

2. The SLAM method for Lidar-IMU fusion of ground mobile robots according to claim 1 is characterized in that The step 1 includes point cloud dedistortion and feature extraction; Point cloud dedistortion: During the factor optimization process, the IMU pre-integration provides the pre-integration factor, and the IMU deviation in the optimization process will also participate in the optimization as an optimization parameter. The IMU data corrected by the obtained IMU deviation is used to estimate the position of the carrier at any time, and the laser points are transformed into a unified coordinate system to perform point cloud distortion correction; The feature extraction is as follows: extract the feature point cloud from the dedistorted point cloud, extract the edge points and surface points as matching feature points, and for any point p, find 10 consecutive points from the same row scan as p, represented by S, evenly distributed on both sides of p. The roughness of p is calculated by the following formula: Where |S| is the number of points in S, which is 10, r i Indicates the depth of the i-th point; Select the point with smaller roughness value as the plane point and the point with larger roughness value as the edge point. The feature point obtained from any laser radar scan is expressed as represents a plane point, Indicates edge points.

Images