A laser slam method for suppressing high drift of odometry

Abstract

The application provides a laser SLAM method for inhibiting high drift of an odometer, comprising: obtaining various types of point clouds after preprocessing, optimizing displacement by using Kalman filtering, determining radar odometer pose according to the optimized radar odometer displacement, and optimizing the radar odometer with high drift by using a slope limiting algorithm; converting the various types of point clouds and odometer information to a point cloud map to be established, extracting key frames, performing feature extraction on the point clouds of the key frames, performing a smoothing operation on the height by using historical poses after solving the key frame odometer pose, extracting a loop detection factor in a Scan Context loop detection method based on radius search, and obtaining global laser radar odometer 6-DOF pose information and a three-dimensional point cloud map by performing graph optimization on a factor graph. The application takes the slope limiting algorithm and the height smoothing algorithm as cores, and realizes the effects of high-precision odometer pose and a point cloud map without distortion.

Description

Technical Field

[0001] This invention belongs to the field of autonomous driving technology, specifically relating to a laser SLAM method for suppressing odometry altitude drift. Background Technology

[0002] With the rapid development of autonomous driving technology, more and more companies are applying it in various scenarios. High-precision maps are a crucial issue that must be addressed to achieve autonomous driving, as they not only help vehicles perform autonomous driving functions but also improve vehicle safety. To build high-precision maps, Simultaneous Localization and Mapping (SLAM) technology has become a core focus in the industry. Currently, mainstream high-precision map construction methods use sensors such as monocular cameras, binocular cameras, depth cameras, LiDAR, and millimeter-wave radar. Among these, LiDAR has advantages over other sensors, including strong anti-interference capabilities, long detection range, high accuracy, and good real-time performance, thus it is widely used in high-precision map reconstruction. However, existing SLAM algorithms relying solely on LiDAR also exhibit low vertical resolution and are prone to height drift on undulating roads and even flat terrain.

[0003] To address the problems of traditional SLAM algorithms, Zhang et al. proposed the loam algorithm, which employs a two-dimensional LiDAR and an inertial measurement unit (IMU) for environmental acquisition. Point cloud matching utilizes a matching method based on corner, point, and planar features, improving mapping accuracy and matching efficiency compared to traditional point cloud registration algorithms such as iterative closet point (ICP) and normal distributions transform (NDT). Furthermore, the loam algorithm uses a uniform velocity model for motion compensation of the point cloud and improves LiDAR odometry accuracy by fusing data from two types of sensors in a loosely coupled manner. However, it does not consider preprocessing the initial LiDAR point cloud data, nor does it incorporate loop closure detection for pose correction in the backend, resulting in weak robustness and susceptibility to drift and ghosting during map creation. Shan et al. proposed the lightweight lego-loam algorithm, which first preprocesses the point cloud at the front end. It segments ground points and non-ground points by detecting the angle of the LiDAR beam and performs clustering operations on the non-ground points, significantly improving point cloud registration efficiency and reducing computational load. Subsequently, a loop closure detection algorithm based on radius search was added to the backend to correct some incorrect point cloud poses. However, in real-world environments, it suffers from low loop closure detection efficiency, weak robustness, and odometry height drift. Summary of the Invention

[0004] In view of the above-mentioned shortcomings of the prior art, the present invention provides a laser SLAM method for suppressing odometer height drift, so as to solve the above-mentioned technical problems.

[0005] This invention provides a laser SLAM method for suppressing odometry altitude drift, comprising:

[0006] We acquire various preprocessed point clouds, extract features and calculate the displacement of the lidar odometry, and use Kalman filtering to optimize the displacement. Based on the optimized displacement, we determine the pose of the lidar odometry. We use a slope limiting algorithm to optimize the lidar odometry that has experienced height drift, and obtain a more accurate lidar odometry pose.

[0007] The various point cloud and odometer information are converted onto the point cloud map to be built. Key frames are extracted and feature extraction is performed on the point cloud of the key frames. After solving the odometer pose of the key frames, the height is smoothed using the historical pose. The radar odometer pose and point cloud map are optimized. The key frame information and odometer pose are saved and added to the factor map to obtain the key frame odometer pose factor.

[0008] The loop closure detection factor is extracted from the Scan Context loop closure detection method based on radius search and added to the ISAM2 factor graph along with the keyframe odometry pose factor. The odometry pose and point cloud map are corrected by graph optimization of the factor graph, and the global lidar odometry 6-DOF pose information and three-dimensional point cloud map are obtained.

[0009] Furthermore, the acquisition of preprocessed point clouds, feature extraction, and calculation of lidar odometry displacement includes:

[0010] Feature extraction is performed on various types of preprocessed point cloud data. Planar points and edge points are extracted using a smoothness principle. The smoothness c of the local surface to be evaluated, used to distinguish between planar points and edge points, is:

[0011]

[0012] In the above formula, S is the set of points in the neighborhood of the calculated point, and r j r i Let p be the midpoint of set S. j p i The Euclidean distance from the corresponding pixel to the sensor;

[0013] The odometer's [Δt] at time k relative to time k-1 is calculated by examining the changes in the characteristics of the planar points. z ,Δθ roll ,Δθ pich ], where Δt z Δθ is the unit time increment of displacement along the Z-axis.roll Δθ is the roll angle increment per unit time. pich The pitch angle is incremented per unit time; [Δt] is solved by considering the changes in edge point features. x ,Δt y ,Δθ yaw ], where Δt x Δt is the increment of displacement per unit time along the x-axis. y Δθ is the increment of displacement per unit time along the y-axis. yaw The incremental 6-DOF pose of the yaw angle is obtained by incrementing the yaw angle per unit time.

[0014] Furthermore, the step of optimizing the displacement using Kalman filtering and determining the radar odometer pose based on the optimized radar odometer displacement includes:

[0015] For highly correlated Δt z , Δθ roll , Δθ pich Kalman filter optimization is performed, where the state equation of the Kalman filter system is x k =x k-1 +u k-1 +w k-1 The Kalman filter observation equation is: z k =x k +v k ;

[0016] x k It is the state vector, i.e., the pose Δt before filtering. z , Δθ roll , Δθ pich u k-1 It is the state control vector at time k-1, w k-1 The noise at time k of the system follows a Gaussian distribution: w k-1 ~N(0,Q), where Q is the system noise covariance matrix; z k It is the observation vector at time k, i.e., the filtered pose output value, v k The noise observed at time k follows a Gaussian distribution: v k ~N(0,R), where R is the covariance matrix of the observation noise;

[0017] The optimized odometer incremental 3-DOF pose [Δt] is used to determine the odometer increment 3-DOF pose. x ,Δt y ,Δt z Convert to translation matrix, 3-DOF pose [Δθ] roll ,Δθ pich ,Δθ yaw Convert to a rotation matrix;

[0018] By continuously accumulating data, the rotation matrix of the 3-DOF pose of the odometry at any moment in the world coordinate system can be obtained:

[0019]

[0020] in, Let be the rotation matrix of the incremental 3-DOF pose of the odometer at time k in radar coordinate system L relative to time k-1. Let be the rotation matrix of the incremental 3-DOF pose of the odometry at time k in the world coordinate system W, relative to the origin. Rotation matrix of incremental 3-DOF pose of odometry at time k-1 in world coordinate system W relative to the origin;

[0021] By inverse solution Obtain the 3-DOF pose of the odometry at time k in the world coordinate system. Simultaneously, by combining the cumulative translation matrix, we can obtain...

[0022] Furthermore, the slope-limiting method is used to optimize the radar odometry that experiences altitude drift, resulting in a more accurate radar odometry pose, including:

[0023] The gradient G of the odometer from time k-1 to time k is calculated using the following formula. r :

[0024]

[0025] Obtain the road design slope of the map to be built and

[0026] By determining the slope G of the odometer from time k-1 to time k. r Road design slope in the drawing to be constructed The size of the value determines whether altitude drift has occurred, where, When altitude drift is determined to occur; when It was confirmed that no altitude drift had occurred.

[0027] In the event that altitude drift has occurred, To impose restrictions, let G r equal Then solve G in reverse. r Get new and use new replace

[0028] Furthermore, after solving the keyframe odometry pose, the elevation is smoothed using historical poses to optimize the radar odometry pose and point cloud map, including:

[0029] Optimizing odometry pose using planar features from keyframes Odometry pose optimization using edge features

[0030] After continuously adding optimized point cloud and odometry information to the map queue, iterative optimization is performed using a scan-to-map approach. The optimization method involves smoothing the height of keyframes using historical keyframes, specifically including: collecting 6-DOF pose information from historical keyframe odometry. Add it to the map queue, and intervene when the height difference between the odometry pose at time k-1 and time k exceeds the preset value. Use the following formula to adjust the odometry height at time k. It consists of the current keyframe height and the historical keyframe height:

[0031]

[0032] Where η = a + b + c + d, and a, b, c, and d are weight parameters.

[0033] Furthermore, the extraction of loop closure detection factors from the radius-search-based Scan Context loop closure detection method includes:

[0034] After receiving a complete scan data frame from the radar, check if it is a key frame, then query whether there are any historical key frames within the radius of the current key frame. If there are, they are used as candidate historical key frames. Extract the radius search loop factor for this process.

[0035] The 3D point cloud of each frame is unfolded into a 2D image, and then a Scan Context is established. A two-stage hierarchical search is performed by introducing a rotation-invariant descriptor, Ring Key, to construct a KD tree for scanning index Key. The nearest neighbor search algorithm is used to quickly find candidate historical keyframes and match them with the current keyframe, calculate their Ring Key similarity, find the most similar candidate historical keyframe, and extract the Scan Context loop closure factor in this process.

[0036] Furthermore, the odometry pose and point cloud map are corrected by performing graph optimization on the factor graph to obtain global lidar odometry 6-DOF pose information and a 3D point cloud map, including:

[0037] The constraints for optimizing pose and trajectory are constructed. The pose transformation and incremental map registration of the current keyframe and closed-loop keyframe are iterated through ICP. The pose estimation is expressed as a maximum a posteriori problem. Then, based on Gaussian noise covariance, the problem is transformed into a nonlinear least squares problem. Based on Bayesian tree, ISAM2 is used to smooth and linearize some nonlinear factors such as trajectory and pose to obtain the final accurate global trajectory and pose.

[0038] The beneficial effects of this invention are as follows: The laser SLAM method for suppressing odometry altitude drift provided by this invention addresses the odometry altitude drift and map distortion phenomena that easily occur during the mapping process of existing laser SLAM algorithms. It can achieve high-precision mapping in complex terrain using only LiDAR. Through algorithm optimization, it improves and achieves high-precision odometry pose and distortion-free point cloud maps, rather than using the traditional multi-sensor fusion approach. With slope limiting algorithm and height smoothing algorithm as the core, it integrates Kalman filtering, loop closure detection and graph optimization methods, providing a new solution for the SLAM field.

[0039] Furthermore, this invention enables the acquisition of surrounding environmental information at low cost in environments such as factories, eliminating the need for sensors like IMUs and cameras, and relying solely on LiDAR, which is beneficial for the widespread application of robots. In addition, the invention has a reliable design principle, a simple structure, and very broad application prospects. Attached Figure Description

[0040] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, for those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0041] Figure 1 This is a schematic flowchart of a method according to an embodiment of the present invention.

[0042] Figure 2 This is a schematic flowchart of a data processing method according to an embodiment of the present invention.

[0043] Figure 3 This is a schematic flowchart of the front-end radar odometer pose calculation according to an embodiment of the present invention.

[0044] Figure 4 This is a schematic diagram of the point cloud segmentation algorithm provided in an embodiment of the present invention.

[0045] Figure 5 This is a schematic diagram of the odometer pose slope provided in an embodiment of the present invention.

[0046] Figure 6This is a schematic flowchart illustrating the backend radar mapping process according to an embodiment of the present invention. Detailed Implementation

[0047] To enable those skilled in the art to better understand the technical solutions of this invention, the technical solutions of the embodiments of this invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this invention, and not all embodiments. Based on the embodiments of this invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of this invention.

[0048] Currently, multi-line LiDAR has low vertical resolution. Existing SLAM algorithms are prone to odometry height drift when mapping on undulating outdoor roads or even flat roads, leading to map distortion and ghosting. To address this, this invention proposes a LiDAR SLAM method to suppress odometry height drift. Figure 1 As shown, the method includes:

[0049] S1. Data Preprocessing: After obtaining the environmental data collected by the lidar, the ground point cloud and non-ground point cloud are marked by the segmentation algorithm. Then, the non-ground point cloud is clustered by the clustering algorithm. Finally, the segmented ground point cloud and the clustered non-ground point cloud are transmitted to S2.

[0050] S2. Front-end radar odometer pose calculation: Acquire various preprocessed point clouds, extract features and calculate the displacement of the lidar odometer, and use Kalman filtering to optimize the displacement. Determine the radar odometer pose based on the optimized radar odometer displacement, and use a slope limiting algorithm to optimize the radar odometer that has experienced height drift, so as to obtain a more accurate radar odometer pose.

[0051] S3. Backend radar mapping: Convert the various point cloud and odometer information to the point cloud map to be built, extract key frames and perform feature extraction on the point cloud of the key frames, use historical poses to smooth the height after solving the key frame odometer pose, optimize the radar odometer pose and point cloud map, save the key frame information and odometer pose and add them to the factor map to obtain the key frame odometer pose factor.

[0052] S4. Loop closure detection and factor graph optimization: Extract the loop closure detection factor from the Scan Context loop closure detection method based on radius search, and add it to the ISAM2 factor graph along with the keyframe odometry pose factor. Graph optimization of the factor graph is then performed to correct the odometry pose and point cloud map, resulting in global lidar odometry 6-DOF pose information and a 3D point cloud map.

[0053] The method provided in this invention includes data preprocessing, front-end radar odometer, back-end radar mapping, and loop closure detection. This method uses slope limiting algorithm and height smoothing algorithm as its core, and integrates Kalman filtering, loop closure detection, and graph optimization methods to optimize the lidar odometer that has experienced height drift, so that the system can output high-precision odometer pose and distortion-free point cloud map.

[0054] To facilitate understanding of the present invention, the following description will elaborate on S1 to S4 above, and further describe the laser SLAM method for suppressing odometry height drift provided by the present invention.

[0055] like Figure 2 As shown, data preprocessing includes:

[0056] S11: Acquire environmental data collected by the lidar;

[0057] S12: The ground point cloud and non-ground point cloud of the environmental data are marked by the segmentation algorithm;

[0058] S13: Clustering operation is performed on the non-ground point cloud using a clustering algorithm;

[0059] S14: The segmented ground point cloud and the clustered non-ground point cloud are used as the preprocessed point clouds of various types.

[0060] Specifically, since the market uses multi-line LiDAR systems that are fixedly mounted on vehicles, the height of the LiDAR above the ground is constant. Therefore, the segmentation algorithm can be implemented using the number of LiDAR lines. For example... Figure 3 As shown, the RS-16 lidar used in this embodiment has 16 transmitters in the vertical direction, with a vertical angular resolution of 0.2° and a horizontal field of view of 360°, with a horizontal angular resolution of 0.2°. Therefore, each transmitter returns 1800 points per lap. Let P be the point cloud acquired at time t. t ={p1,p2,p3,...,p n}, p i The point cloud P at time t t A point cloud is defined as 1 ≤ i ≤ n. Projecting this point cloud onto a distance image yields a resolution of 1800 × 16. The vertical scanning range of the RS-16 lidar is [-15°, 15°], so the ground appears between [-15°, -1°]. In this embodiment, the ground is set to appear below the 7th line. The pitch angle between the upper and lower lines is calculated for the point cloud of the 0-7 line bundles. Figure 3The point cloud with angle α is used to estimate the ground plane. In this embodiment, point clouds with angle α within 10° are defined as ground points, and other point clouds are defined as non-ground points. Thus, the environmental data collected by the lidar is acquired, and the ground point cloud and non-ground point cloud of the environmental data are marked by a segmentation algorithm.

[0061] The clustering operation uses the Breadth-First-Search (BFS) clustering algorithm to cluster non-ground points, with p i The search proceeds outward from the center. In this embodiment, if the clustered point cloud contains more than 30 points, it is designated as a usable cluster; if the clustered point cloud contains fewer than 30 points but greater than or equal to 5, the number of points in its vertical direction is counted, and if it is greater than 3, it is also designated as a usable cluster. Clustering effectively reduces interference from dynamic objects (pedestrians, vehicles) and small objects such as leaves in the point cloud. Then, the clustered non-ground points and the segmented ground points can be further processed. Therefore, the data preprocessing method provided in this embodiment can reduce the amount of point cloud data and improve the computation speed of subsequent algorithms.

[0062] like Figure 4 As shown, the main purpose of the front-end radar odometer stage is to extract the 6-DOF pose data. Specifically, this includes:

[0063] S21: Feature extraction is performed using the smoothness principle.

[0064] First, feature extraction is performed on various types of preprocessed point cloud data. Based on the smoothness principle, the feature points are divided into two categories: planar points and edge points. Therefore, this invention uses the smoothness principle to extract planar points and edge points of the point cloud when performing feature extraction.

[0065] Wherein, the smoothness c of the local surface to be evaluated, used to distinguish between planar points and edge points, is:

[0066]

[0067] In the above formula, S is the set of points in the neighborhood of the calculated point; in this embodiment, S is set to 10. j r i Let p be the midpoint of set S. j p i The Euclidean distance from the corresponding pixel to the sensor.

[0068] S22: Calculate the incremental 6-DOF pose of the odometer using planar point features and edge point features.

[0069] Then, by analyzing the changes in the characteristics of the planar points, the [Δt] of the odometer at time k relative to time k-1 is calculated. z ,Δθ roll ,Δθ pich], where Δt z Δθ is the increment of displacement per unit time along the z-axis. roll Δθ is the roll angle increment per unit time. pich The pitch angle increment per unit time is calculated by solving for [Δt] using the changes in edge point features. x ,Δt y ,Δθ yaw ], where Δt x Δt is the increment of displacement per unit time along the x-axis. y Δθ is the increment of displacement per unit time along the y-axis. yaw The yaw angle increment per unit time allows us to obtain the odometry increment 6-DOF pose [Δt]. z ,Δt x ,Δt y ,Δθ yaw ,Δθ roll ,Δθ pich ].

[0070] S23: Perform Kalman filtering optimization on the incremental 6-DOF pose of the odometer.

[0071] Since it is a simple coarse matching of feature points, and the error of the lidar on the horizontal plane is relatively small, with the main drift being in height, the horizontal position-related Δt is not considered. x Δt y , Δθ yaw Optimize only for highly correlated Δt z , Δθ roll , Δθ pich Perform Kalman filter optimization:

[0072] x k =x k-1 +u k-1 +w k-1 (2)

[0073] z k =x k +v k (3)

[0074] Wherein, equation (2) is the state equation of the Kalman filter system, and equation (3) is the observation equation of the Kalman filter. k It is the state vector at time k, i.e., the pose Δt before filtering. z , Δθ roll , Δθ pich u k-1 It is the state control vector at time k-1, u k-1 It is the state control vector at time k-1, w k-1 The noise of the control system at time k-1 follows a Gaussian distribution: wk-1 ~N(0,Q), where Q is the system noise covariance matrix, obtained offline through sensor calibration. k It is the observation vector at time k, i.e., the filtered pose output value, v k The noise observed at time k follows a Gaussian distribution: v k ~N(0,R), where R is the covariance matrix of the observation noise.

[0075] This embodiment is set to apply Δt z When processing, Q is 0.5 and R is 1; for Δθ roll , Δθ pich During processing, Q is 0.001 and R is 0.005.

[0076] S24: Calculate the odometer 6-DOF pose based on the odometer incremental 3-DOF pose optimized by Kalman filtering.

[0077] The odometer increment 3-DOF pose [Δt] optimized by Kalman filtering is used to determine the odometer increment 3-DOF pose. x ,Δt y ,Δt z Convert the 3-DOF pose [Δθ] to a translation matrix. roll ,Δθ pich ,Δθ yaw [Convert to rotation matrix. Since the starting point of the odometer is the origin of the world coordinate system, the rotation matrix of the odometer's 3-DOF pose at any moment in the world coordinate system can be obtained by continuously accumulating formula (4):]

[0078]

[0079] in, Let be the rotation matrix of the incremental 3-DOF pose of the odometer at time k in radar coordinate system L relative to time k-1. Let be the rotation matrix of the incremental 3-DOF pose of the odometry at time k in the world coordinate system W, relative to the origin. The rotation matrix of the incremental 3-DOF pose of the odometry at time k-1 in world coordinate system W relative to the origin; obtained by inverse k-1 solution. Obtain the 3-DOF pose of the odometry at time k in the world coordinate system. Simultaneously, by combining the cumulative translation matrix, we can obtain... Thus, the 6-DOF pose of the front-end odometer is obtained. The laser SLAM method provided in this embodiment of the invention solves the problem of odometer drift in height and reduces the pose error of the lidar odometer.

[0080] However, the pose predicted by the front-end odometer is easily affected by the terrain and will drift in altitude. In order to reduce the altitude drift of the odometer, a series of construction indicators are usually specified when the road is built. Gradient is an important indicator. The maximum gradient of ordinary highways is 5%, while the gradient value of the trajectory of the front-end odometer after altitude drift is outside the range of the indicator.

[0081] To address the above problems, embodiments of the present invention further include:

[0082] S25: The odometer measurement process during mapping is optimized using a slope constraint method. Details are as follows:

[0083] like Figure 5 As shown, point A is the odometry pose at time k-1. Point B represents the pose at time k. BC represents the vertical displacement of the odometer, and AC represents the horizontal displacement.

[0084] Road surface slope is the percentage of the vertical height ΔH and horizontal distance ΔL of the slope, denoted by G. r express:

[0085]

[0086] Combination Figure 5 The slope of the odometer from time k-1 to time k can be obtained using formula (5).

[0087]

[0088] in, Let k be the x-axis pose. The x-axis pose is at time k-1; the z-axis and y-axis poses are similar and will not be described further here.

[0089] Obtain the road design slope of the map to be built and By determining the slope G of the odometer from time k-1 to time k. r Road design slope in the drawing to be constructed The size of the value determines whether altitude drift has occurred, where, When altitude drift is determined to occur; when If it is determined that no altitude drift has occurred; if it is determined that altitude drift has occurred, then... To impose restrictions, let G r equal Then solve G in reverse. r Get new and use new replace

[0090] like Figure 6As shown, the purpose of backend radar mapping is to create a map and determine map factors. Specifically, this includes:

[0091] S31: Extract keyframes and obtain the odometry pose of the keyframes.

[0092] First, the preprocessed point cloud data and front-end odometry information are converted onto the map to be built. Then, keyframes are extracted based on a series of historical radar frames. Finally, the planar features of the keyframes are used to optimize the odometry pose. z ,θ roll ,θ pich Information, optimized using edge features [t] x ,t y ,θ yaw ]information.

[0093] After continuously adding optimized point cloud and odometry information to the map queue, a scan-to-map approach is used for iterative optimization. Although the odometry pose after scan-to-map optimization is more accurate than that of the front-end odometry, this result is based on the premise that the front-end odometry does not experience altitude drift. If the data provided by the front end is incorrect, even after scan-to-map optimization, the odometry will still exhibit altitude drift. Therefore, this embodiment of the invention employs the idea of ​​smoothing filtering to smoothly optimize the odometry portion that experiences altitude drift.

[0094] S32: The odometry pose of the keyframes is smoothed and optimized using a height smoothing filter algorithm. Specifically, the height smoothing filter algorithm is as follows:

[0095] First, it is necessary to continuously collect the 6-DOF pose information of the odometry in historical keyframes and add it to the map queue. Then, after discovering that the height difference between the odometry pose at time k-1 and time k exceeds 0.1m, intervention is carried out, and the height of the odometry at time k is adjusted using formula (7). It consists of the current frame height and the historical frame height:

[0096]

[0097] Where η = a + b + c + d.

[0098] Regarding the values ​​of weights a, b, c, and d, multiple experiments revealed that selecting different weight parameters yielded the best mapping results under different road environments. In relatively flat road environments, a, b, c, and d were set to 9, 5, 3, and 1; in more rugged road environments, a, b, c, and d were set to 3, 5, 3, and 1. After height smoothing filtering, odometry height drift can be suppressed, thereby reducing map distortion. Finally, the optimized keyframes and odometry factors are saved for subsequent loop closure detection and factor map optimization methods.

[0099] The loop closure detection method and factor map optimization method provided in this invention are mainly aimed at suppressing the odometry height drift phenomenon that occurs during SLAM, so as to achieve loop closure detection and efficient and stable mapping even in complex environments.

[0100] Specifically, after receiving a complete frame of environmental data from the LiDAR, the loop closure detection thread first checks if the frame is a keyframe, then queries the radius of the current keyframe (which can be 7m from the current keyframe or within the past 30s) to see if there are any historical keyframes. The 3D point cloud of each frame is unfolded into a 2D image, and a Scan Context is established. A two-stage hierarchical search is performed by introducing a rotation-invariant descriptor, the Ring Key. A KD tree is constructed for scanning the index Key. The nearest neighbor search algorithm is used to find candidate historical keyframes and match them with the current keyframe. By calculating their point cloud similarity (e.g., Ring Key similarity), the most similar keyframe is found, thus achieving loop closure detection.

[0101] The loop closure detection method provided in this invention differs from the historical keyframe radius search loop closure method, which can only achieve loop closures in simple environments. The Scan Context loop closure detection method can achieve loop closures in complex environments and is faster. This invention primarily uses Scan Context loop closure, supplemented by keyframe radius search loop closure. After matching using both methods, the two loop closure factors are added to the factor graph for further optimization. The laser SLAM method provided in this invention corrects some trajectory deviations through loop closure detection, resulting in less deviation in the global odometry and a higher degree of fit with the true trajectory.

[0102] The implementation of the factor graph optimization method first initializes the prior factors. Then, radar odometry factors, radius search loop closure factors, and Scan Context loop closure factors are gradually added to the factor graph to construct constraints for optimizing pose and trajectory. Next, ICP iterates the pose transformation and incremental map registration of the current keyframe and closed-loop keyframe, and the pose estimation is expressed as a maximum a posteriori (MAP) problem. Then, based on Gaussian noise covariance, the problem is transformed into a nonlinear least squares problem. Using ISAM2 based on Bayesian trees, some nonlinear factors such as trajectory and pose are smoothed and linearized to obtain the final accurate global trajectory and pose. Because this embodiment introduces the loop closure factor of the spatial point cloud descriptor Scan Context into the original loop closure detection part of Lego-Loam, it forms a loop closure detection algorithm based on historical keyframe radius search factors and Scan Context factors, and performs global consistency optimization using ISAM2.

[0103] Although the present invention has been described in detail with reference to the accompanying drawings and preferred embodiments, the invention is not limited thereto. Various equivalent modifications or substitutions can be made to the embodiments of the invention by those skilled in the art without departing from the spirit and essence of the invention, and such modifications or substitutions should all be within the scope of the invention. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in the invention should also be covered within the protection scope of the invention. Therefore, the protection scope of the invention should be determined by the scope of the claims.

Claims

1. A laser SLAM method for suppressing odometry height drift, characterized in that, include: We acquire various preprocessed point clouds, extract features and calculate the displacement of the lidar odometry, and use Kalman filtering to optimize the displacement. Based on the optimized displacement, we determine the pose of the lidar odometry. We use a slope limiting algorithm to optimize the lidar odometry that has experienced height drift, and obtain a more accurate lidar odometry pose. The various point cloud and odometer information are converted onto the point cloud map to be built. Key frames are extracted and feature extraction is performed on the point cloud of the key frames. After solving the odometer pose of the key frames, the height is smoothed using the historical pose. The radar odometer pose and point cloud map are optimized. The key frame information and odometer pose are saved and added to the factor map to obtain the key frame odometer pose factor. The loop closure detection factor in the Scan Context loop closure detection method based on radius search is extracted and added to the ISAM2 factor graph along with the key frame odometry pose factor. The odometry pose and point cloud map are corrected by graph optimization of the factor graph, and the global lidar odometry 6-DOF pose information and three-dimensional point cloud map are obtained. The process of smoothing the altitude using historical poses after solving for the keyframe odometry pose, and optimizing the radar odometry pose and point cloud map, includes: Optimizing odometry pose using planar features from keyframes Odometry pose optimization using edge features ; After continuously adding optimized point cloud and odometry information to the map queue, a scan-to-map approach is used for iterative optimization. The optimization method involves smoothing the height of keyframes using historical keyframes, specifically including: collecting 6-DOF pose information from historical keyframe odometry. Add it to the map queue. If the height difference between the odometry pose at time k-1 and time k exceeds a preset value, intervene. Use the following formula to adjust the odometry height at time k. It consists of the current keyframe height and the historical keyframe height: ； in a, b, c, and d are weight parameters.

2. The method according to claim 1, characterized in that, The process of acquiring various preprocessed point clouds, extracting features, and calculating the displacement of the lidar odometry includes: Feature extraction is performed on various types of preprocessed point cloud data. Planar points and edge points are extracted using a smoothness principle. The smoothness c of the local surface to be evaluated, used to distinguish between planar points and edge points, is: ； In the above formula, S is the set of points in the neighborhood of the calculated point. , is the midpoint of set S The Euclidean distance from the corresponding pixel to the sensor; The odometer reading at time k relative to time k-1 is obtained by analyzing the changes in the characteristics of planar points. ,in, This represents the increment of the z-axis displacement per unit time. This represents the roll angle increment per unit time. This represents the pitch angle increment per unit time. Solving the problem by considering the changes in edge point features ,in, This represents the increment of x-axis displacement per unit time. This represents the increment of displacement per unit time along the y-axis. This represents the increment of the yaw angle per unit time.

3. The method according to claim 2, characterized in that, The process of optimizing the displacement using Kalman filtering and determining the radar odometer pose based on the optimized radar odometer displacement includes: Highly relevant , , Kalman filter optimization is performed, where the state equation of the Kalman filter system is: The Kalman filter observation equation is: ; It is the state vector at time k, i.e., the pose before filtering. , , , It is the state control vector at time k-1. The noise at time k of the system follows a Gaussian distribution: Q is the system noise covariance matrix; It is the observation vector at time k, i.e., the filtered pose output value. The noise observed at time k follows a Gaussian distribution: R is the covariance matrix of the observation noise; The optimized odometer incremental 3-DOF pose Convert to translation matrix, 3-DOF pose Convert to a rotation matrix; By continuously accumulating data, the rotation matrix of the 3-DOF pose of the odometry at any moment in the world coordinate system can be obtained: ； in, Let be the rotation matrix of the incremental 3-DOF pose of the odometer at time k in radar coordinate system L relative to time k-1. Let be the rotation matrix of the incremental 3-DOF pose of the odometry at time k in the world coordinate system W, relative to the origin. Rotation matrix of incremental 3-DOF pose of odometry at time k-1 in world coordinate system W relative to the origin; By inverse solution Obtain the 3-DOF pose of the odometry at time k in the world coordinate system. Simultaneously, by combining the cumulative translation matrix, we can obtain... .

4. The method according to claim 3, characterized in that, The method of optimizing radar odometry that experiences altitude drift using a slope limitation approach to obtain a more accurate radar odometry pose includes: The gradient of the odometer from time k-1 to time k is calculated using the following formula. : ； Obtain the road design slope of the map to be built ; By determining the slope of the odometer from time k-1 to time k. Road design slope in the drawing to be constructed The size of the value determines whether altitude drift has occurred, where, When altitude drift is determined to occur; when It was confirmed that no altitude drift had occurred. Given that a height drift has occurred, the z-axis displacement at time k To impose restrictions, equal Then reverse solve Obtain the new z-axis displacement at time k and with new replace .

5. The method according to claim 1, characterized in that, The extraction of loop closure detection factors from the radius-search-based Scan Context loop closure detection method includes: After receiving a complete scan data frame from the radar, check if it is a key frame, then query whether there are any historical key frames within the radius of the current key frame. If there are, they are used as candidate historical key frames. Extract the radius search loop factor for this process. The 3D point cloud of each frame is unfolded into a 2D image, and then a Scan Context is established. A two-stage hierarchical search is performed by introducing a rotation-invariant descriptor, Ring Key, to construct a KD tree for scanning index Key. The nearest neighbor search algorithm is used to quickly find candidate historical keyframes and match them with the current keyframe, calculate their Ring Key similarity, find the most similar candidate historical keyframe, and extract the Scan Context loop closure factor in this process.

6. The method according to claim 1, characterized in that, The method involves graph optimization of the factor graph to correct the odometry pose and point cloud map, resulting in global LiDAR odometry 6-DOF pose information and a 3D point cloud map, including: The constraints for optimizing pose and trajectory are constructed, and the pose transformation and incremental map registration of the current keyframe and closed-loop keyframe are iterated through ICP. The pose estimation is expressed as a maximum a posteriori problem, and then the problem is transformed into a nonlinear least squares problem based on Gaussian noise covariance. Based on Bayesian trees, ISAM2 is used to smooth and linearize the nonlinear factors of the trajectory and pose, so as to obtain the final accurate global trajectory and pose.

Images