Laser SLAM mapping method based on improved Cartograph algorithm

Abstract

The invention provides a laser SLAM (Simultaneous Localization and Mapping) method based on an improved Cartograph algorithm, and aims to solve the problems of loopback detection and map construction in a dynamic environment. SURF feature extraction and generalized ICP fine registration are introduced, so that the precision and robustness of point cloud matching are improved; due to the design of the delay judgment module, false loopback misinformation is effectively reduced, and the consistency of a global map is ensured; and meanwhile, a multi-resolution search strategy is adopted, so that the loopback detection efficiency and precision are improved. Experimental results show that compared with Hector-SLAM (Simultaneous Localization and Mapping) and Cartographer, the algorithm disclosed by the invention shows higher map construction precision and consistency in a long straight corridor and a loopback environment, and successfully solves the problems of accumulative errors and wrong loopback. The algorithm is suitable for complex SLAM tasks, has wide engineering application value, and is especially suitable for the fields of robot navigation, autonomous driving and the like.

Description

Technical Field

[0001] The present invention relates to a laser SLAM mapping method based on an improved Cartographer algorithm, which relates to robot navigation and positioning technologies and is applicable to high-precision map construction and positioning in dynamic and complex environments. Background Art

[0002] The Simultaneous Localization and Mapping (SLAM) technology has become one of the important technical means for mobile robots, autonomous driving, and industrial automation due to its high efficiency and robustness. As an important algorithm for 2D laser SLAM, Cartographer has efficient scan matching and map optimization capabilities and shows excellent performance in static environments. This algorithm performs local registration of lidar point clouds through the front-end module, adds global constraints in the loop detection module, and generates a globally consistent trajectory through the back-end optimization module.

[0003] With the continuous increase in the requirements for dynamic environments and large-scale scenarios, the Cartographer algorithm still has deficiencies in dealing with problems such as feature extraction and matching, and loop detection. The traditional Cartographer algorithm uses Adaptive Voxel Filtering to process lidar point clouds, but it is prone to losing geometric detail features in the point clouds, resulting in a decrease in matching accuracy. Especially in long corridors or areas with dense corners, it is difficult to ensure stable feature point extraction by this method. On the other hand, the low-resolution CSM (Correlative Scan Matching) method used by the Cartographer algorithm for rough matching in loop detection has limited filtering ability for false loop points and low efficiency in large-scale scenarios. Summary of the Invention

[0004] The purpose of the present invention is to provide a laser SLAM mapping method based on an improved Cartographer algorithm, which effectively improves the robustness and accuracy of mapping in dynamic scenarios and large-scale scenarios, optimizes the loop detection efficiency, and significantly reduces errors and computational overhead.

[0005] To achieve the above technical objectives, the present invention will adopt the following technical solutions:

[0006] A laser SLAM mapping method based on an improved Cartographer algorithm includes a feature extraction and matching step and a loop detection step. It is characterized in that the feature extraction and matching step combines feature extraction based on the SURF algorithm and registration based on the GICP algorithm to achieve feature extraction and matching optimization of 2D laser point clouds.

[0007] Preferably, the feature extraction and matching steps specifically include:

[0008] Using the SURF algorithm to extract the significant feature points of the two-dimensional laser point cloud, and generating a feature descriptor for each significant feature point;

[0009] Calculating the Euclidean distance between the feature descriptor of the target point cloud and the feature descriptor of the source point cloud;

[0010] Screening the matching point pairs whose Euclidean distance meets the matching distance threshold;

[0011] According to the screened matching point pairs, estimating the initial pose transformation matrix of the point cloud;

[0012] Establishing an optimization objective function of the initial pose transformation matrix by minimizing the sum of the squares of the Euclidean distances of the matching point pairs:

[0013]

[0014] In the formula: T0 represents the initial pose transformation matrix of the target point cloud; p i , q i are the matching point pairs of the source point cloud and the target point cloud; R represents the rotation matrix of the pose transformation when the coordinate system where the target point cloud is located is transformed to the coordinate system where the source point cloud is located; t represents the translation vector of the pose transformation when the coordinate system where the target point cloud is located is transformed to the coordinate system where the source point cloud is located; N represents the number of matching point pairs formed by the source point cloud and the target point cloud; T represents the transformation matrix that transforms the coordinate system of the source point cloud to the coordinate system of the target point cloud;

[0015] Using singular value decomposition to solve the initial pose transformation matrix to obtain the initial pose estimate of the target point cloud;

[0016] Based on the initial pose estimate of the target point cloud, constructing an error function; the error function is defined as:

[0017] e(p i , q i ) = (q i - R·p i - t) T (∑ p + ∑ q ) -1 (q i - R·p i - t)

[0018] In the formula: Σp is the covariance matrix of point p i ; Σq is the covariance matrix of point q i ;

[0019] Solving the error function to obtain the optimal pose transformation matrix.

[0020] Preferably, the SURF algorithm is used to extract the significant feature points of the two-dimensional laser point cloud, which specifically includes the following steps:

[0021] After graying the target point cloud, the projection density grid method is used to project the target point cloud into a two-dimensional grid map, so as to convert the sparse point cloud data into a two-dimensional grid map with intensity information, forming the input data for SURF feature extraction;

[0022] Estimate the pose relationship between the current laser frame and the two-dimensional grid map by optimizing the following objective function:

[0023]

[0024] In the formula, T is the pose transformation to be solved, including the rotation matrix R and the translation vector t; z i is the point of the current laser frame; m i is the point in the two-dimensional grid map; w i is the weight of the point pair;

[0025] Extract SURF feature points based on the Hessian matrix to obtain the significant feature points of the two-dimensional laser point cloud.

[0026] Preferably, in the loop detection step, a delay judgment module is introduced to optimize the loop detection before identifying the loop points.

[0027] Preferably, the delay judgment module optimizes the loop detection through the following steps:

[0028] Calculate the geometric consistency score of the candidate loop points, and directly eliminate the candidate loop points with low scores to obtain the initial loop point pairs a and b;

[0029] Adopt a delay judgment strategy to judge the validity of the candidate loop points: set a fixed time window t window , and continuously monitor whether new loop point pairs c and d are generated during this period; if so, geometrically verify the pose relationship of the loop points a, b, c, d; otherwise, eliminate the initial loop point pairs a and b;

[0030] When geometrically verifying the pose relationship of the loop points a, b, c, d, first verify whether the pose relationship of the loop points a, b, c, d satisfies the following closed-loop constraint:

[0031] T a→b ·T b→d ·T d→c ·T c→a =I

[0032] where I is the identity matrix; T a→b represents the relative pose between loop point a and loop point b; Tb→d Represents the relative pose of loop point b and loop point d; T d→c Represents the relative pose of loop point d and loop point c; T c→a Represents the relative pose of loop point c and loop point a.

[0033] When the verification result shows that the pose relationship of loop points a, b, c, and d does not fully satisfy the above-mentioned closed-loop constraint, calculate the matrix norm G = ||T a→b ·T b→d ·T d→c ·T c→a - I||, and through the error tolerance mechanism, determine whether the matrix norm G is less than the matrix norm threshold G0; if the judgment result shows that the matrix norm G is less than the matrix norm threshold G0, consider the loop detection to be credible, add the loop point to the optimization model, otherwise, remove the relevant loop points.

[0034] Preferably, the loop detection adopts a multi-resolution search strategy to accelerate the loop detection.

[0035] Preferably, the multi-resolution search strategy accelerates the loop detection through the following steps:

[0036] Low-resolution rough matching: In the low-resolution map G low Quickly screen candidate loop point c, and calculate the rough matching pose T low by maximizing the matching score between the point cloud P and G low ;

[0037] High-resolution fine matching: In the high-resolution map, with the low-resolution rough matching result as the initial value, further optimize the matching accuracy of the loop point.

[0038] Preferably, the rough matching pose T low is calculated by the following formula:

[0039]

[0040] In the formula: X represents an estimated pose of the robot in the low-resolution map, which is used for the preliminary matching of loop detection; represents all possible poses in the search space; Score low (P, G low , X) is the matching score of the point cloud P in the low-resolution grid, and the formula is:

[0041] Score low (P, G low , X) = ∑ p∈P Likelihood low (p, G low , X);

[0042] In the formula: the matching likelihood low The specific meaning of low is the matching probability value of point p in grid G

[0043] likelihood low (p, G low , X) = OccupancyProbability(g p )

[0044] The fine matching pose T high is calculated by the following formula:

[0045]

[0046] In the formula: represents the high-resolution search space defined based on the rough matching pose T low ; Score high (P, G high , X) represents the fine matching score function, and the formula is:

[0047] Score high (P, G high , X) = ∑ p∈P Likelihood high (p, G high , X);

[0048] In the formula: G high represents the high-resolution map. Similarly, the matching likelihood high is the occupancy probability value of point p in grid G high , and its calculation method is the same as that of the low resolution, but due to the high resolution capturing more environmental details, the matching accuracy is higher.

[0049] Another technical object of the present invention is to provide an electronic device, including a memory, a processor, and a computer program stored on the memory and running on the processor. The computer program runs to execute the above-mentioned laser SLAM mapping method based on the improved Cartographer algorithm.

[0050] Based on the above technical object, compared with the prior art, the present invention has the following advantages:

[0051] By optimizing point cloud registration, introducing a delay judgment module and a multi-resolution search strategy, the present invention significantly improves the performance of the laser SLAM system. In dynamic environments and large-scale scenes, the algorithm can efficiently construct a consistent map, and has good robustness and engineering application value. Description of the Drawings

[0052] Figure 1 It is a flowchart of the laser SLAM mapping method based on the improved Cartographer algorithm described in the present invention.

[0053] Figure 2 It is a schematic diagram of a simulated long straight corridor;

[0054] Figure 3 It is a schematic diagram of a simulated loop long corridor;

[0055] Figure 4 It is a comparison diagram of simulated maps obtained by different front-end algorithms;

[0056] Figure 5 It is a schematic diagram of selecting feature points in 10 long straight corridor environments;

[0057] Figure 6 It is a comparison diagram of map construction in a simulated loop long corridor environment (the left is the map constructed by the Cartographer algorithm, and the right is the map constructed by the improved algorithm of the present invention).

[0058] Figure 7 It is the mapping result of the actual environment experiment (the left is the map constructed by the Cartographer algorithm, and the right is the map constructed by the improved algorithm of the present invention) Detailed implementation manners

[0059] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all of the embodiments. The following description of at least one exemplary embodiment is actually only illustrative and in no way limits the present invention and its application or use. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts fall within the scope of the present invention. Unless otherwise specifically stated, the relative arrangements, expressions, and numerical values of the components and steps described in these embodiments do not limit the scope of the present invention. Technologies, methods, and devices known to those of ordinary skill in the relevant art may not be discussed in detail, but should be regarded as part of the specification when appropriate. In all the examples shown and discussed here, any specific value should be construed as merely exemplary, rather than as a limitation. Therefore, other examples of the exemplary embodiments may have different values.

[0060] As Figure 1 shown, the laser SLAM mapping method based on the improved Cartographer algorithm described in the present invention includes a feature extraction and matching optimization step and a loop detection optimization step, wherein:

[0061] The described feature extraction and matching optimization steps are based on SURF-based feature extraction for point cloud registration optimization. By combining feature extraction and optimized registration, the robustness and accuracy of point cloud matching are improved. The specific steps are as follows:

[0062] Step 1.1, SURF feature point extraction:

[0063] The accelerated robust features (SURF) algorithm is used to extract the significant feature points of the laser point cloud.

[0064] Since the laser point cloud is sparse and lacks texture information, the present invention first grayscales the laser point cloud and projects it onto a two-dimensional grid map. The gray value of the grid is generated by encoding the point cloud density or other features.

[0065] The generated grayscale image is adapted to the SURF algorithm to extract key feature points and generate descriptors. The SURF algorithm has strong scale and rotation robustness, effectively enhancing the reliability of point cloud registration.

[0066] To apply the SURF algorithm to the two-dimensional laser point cloud, it is first necessary to convert the sparse point cloud data into a two-dimensional spatial representation suitable for SURF operations. Using the method of projection density gridding, the laser point cloud is projected onto a two-dimensional grid map. The intensity value of each grid is determined by its point cloud density, defined as follows:

[0067]

[0068] where I(x,y) is the intensity value of the two-dimensional grid, count(p i ) represents the number of points p i falling into this grid, max_count is the maximum number of points in all grids for normalization operations, and grid(x,y) represents the grid cell at coordinates (x,y), that is, a certain grid in the two-dimensional grid map. Through this projection, the sparse point cloud is converted into a two-dimensional raster map with intensity information, forming the input data for SURF feature extraction.

[0069] In the feature extraction stage, Cartographer's scan matching module estimates the pose relationship between the current frame and the map by optimizing the following objective function:

[0070]

[0071] where T is the pose transformation to be solved, including the rotation matrix R and the translation vector t; z i is the point of the current laser frame; m i is the point in the reference map; w i is the weight of the point pair; z i represents the measurement value of the i-th point in the current laser frame.

[0072] The extraction of SURF feature points depends on the Hessian matrix, which measures the feature saliency of the local area and is defined as follows:

[0073]

[0074] In the matrix, I is the grayscale value of the image, and x and y are the spatial coordinates of the image. Each element in the matrix represents the second-order partial derivative of the image in the local area, and the specific explanations are as follows:

[0075] is the second-order derivative of the image in the x direction, representing the curvature of the image's change in the horizontal direction. It reflects the intensity of the edges or corners of the image in the x direction. is the second-order derivative of the image in the y direction, representing the curvature of the image in the vertical direction and reflecting the intensity of the edges or corners of the image in the y direction. is the mixed second-order derivative of the image in the x and y directions, representing the cross-change of the image in the x and y directions. It measures the slope change of the image, that is, the degree of the image's direction change.

[0076] Calculate the determinant of the Hessian matrix to judge the feature saliency:

[0077] det(H) = L xx ·L yy -(L xy ) 2 (4)

[0078] where L xx , L yy , and L xy are the second-order Gaussian derivatives of the image, representing respectively representing the curvatures in different directions.

[0079] Only when the determinant value det(H) exceeds the preset threshold τ can the point be selected as a candidate feature point. Finally, the SURF algorithm generates a feature descriptor for each feature point. By statistically analyzing the gradient direction and amplitude distribution in the neighborhood, a descriptor vector with a length of 64 or 128 is generated, laying a foundation for subsequent matching.

[0080] Step 1.2, Feature Point Matching and Initial Registration

[0081] After extracting the feature points, calculate the similarity between the feature points in the source point cloud P and the target point cloud Q using the SURF descriptor.

[0082] The similarity is measured by the Euclidean distance:

[0083] d(f i , f j ) = ||f i-f j || (5)

[0084] where f i and f j are the SURF descriptors of the feature points in the source point cloud and the target point cloud respectively. Set a matching distance threshold d th , and filter out the point pairs that meet the conditions:

[0085] d(f i , f j ) < d th (6)

[0086] According to these matching point pairs, use the least squares method to estimate the initial pose transformation matrix T0 = {R0, t0} of the point cloud. R0 represents the rotation transformation from the source point cloud (or the original coordinate system) to the target point cloud (or the target coordinate system), and t0 represents the translation transformation from the source point cloud to the target point cloud.

[0087] The optimization objective is to minimize the sum of the squares of the Euclidean distances of the matching point pairs:

[0088]

[0089] where p i and q i are the matching point pairs of the source point cloud and the target point cloud. Use SVD (Singular Value Decomposition) to solve the pose transformation matrix T0, providing an initial value for subsequent iterative optimization.

[0090] Step 1.3, GICP fine registration and pose update:

[0091] After the initial pose estimation is completed, use the GICP algorithm to further optimize the point cloud alignment. The core improvement of GICP lies in introducing the covariance information of points, and the error function is defined as:

[0092] e(p i , q i ) = (q i - R·p i - t) T (∑ p + ∑ q ) -1 (q i - R·p i - t) (8)

[0093] where ∑p and ∑q are the covariance matrices of points p i and q i respectively, used to describe the local distribution characteristics of points.

[0094] The optimization objective is to minimize the total error:

[0095]

[0096] The error function is iteratively solved using the Gauss - Newton method until convergence, and finally the optimal pose transformation matrix T = {R, t} is obtained. The optimized T is passed to the front - end module of the Cartographer algorithm to update the pose of the current laser frame.

[0097] It can be seen that the feature extraction and matching optimization steps of the present invention, combined with the robust feature extraction of SURF and the high - precision optimization of GICP, achieve efficient registration of point clouds in a dynamic environment, providing high - quality data for subsequent map construction.

[0098] The loop - closure detection optimization step is optimized by introducing a delay judgment module.

[0099] Loop - closure detection identifies loop - closure points in the robot's path, eliminates cumulative errors, and maintains consistency. However, noise and incorrect matches in a dynamic environment can easily lead to false loop - closures. The present invention proposes a delay judgment module to optimize the accuracy and reliability of loop - closure detection. Figure 1 Before entering loop - closure detection, the Cartographer algorithm matches the current frame data with existing sub - maps in the search window W through scan matching, obtains preliminary loop - closure point pairs a and b, and calculates their relative pose T

[0100] →T a →T a→b . To avoid misjudgment, T a →T a→b will not be immediately added to the global optimization at this time, but enters the delay judgment process.

[0101] The delay judgment module sets a fixed time window t, during which it continuously monitors whether new loop - closure points c and d are generated. If so, the module geometrically verifies the pose relationships of the loop - closure points a, b, c, d to ensure that the loop - closure constraint satisfies the following condition:

[0102] T a→b ·T b→d ·T d→c ·T c→a =I (10)

[0103] where I is the identity matrix. If the above condition is not fully met, the module, through an error tolerance mechanism, defines a matrix norm threshold G:

[0104] ||T a→b ·T b→d ·T d→c ·T c→a -I||<G (11)

[0105] If the norm is less than G, the loop detection is considered credible and the constraint is added to the optimization model; otherwise, the relevant constraint is removed.

[0106] In other words, if the pose relationship of the loop points a, b, c, d satisfies this condition (Formula 10 or Formula 11), the loop is considered valid; otherwise, the loop is invalid.

[0107] It can be seen that the specific implementation of the delay judgment module is as follows:

[0108] Step 2.1: During the loop detection process, first calculate the geometric consistency score of the candidate loop points to measure the credibility of their relative poses. The candidate points with low scores are directly removed to avoid false alarms.

[0109] Step 2.2: Delay judgment strategy. Set a fixed time window t window , after detecting the loop point, delay the judgment of its validity. Monitor whether there are other new loop points appearing within this window and comprehensively verify their geometric consistency. If no new loop points are detected within the time window, the initial candidate points are removed.

[0110] Step 2.3: Loop point verification. For multiple sets of loop points within the delay window, verify whether they satisfy the closed-loop constraint:

[0111] T a→b ·T b→d ·T d→c ·T c→a =I;

[0112] Considering the error tolerance range, the formula can be adjusted to:

[0113] ||T a→b ·T b→d ·T d→c ·T c→a -I||<G;

[0114] The loop points that meet the conditions are considered credible and added to the global optimization process; otherwise, they are removed. Through the delay judgment module, false loops are effectively reduced and the consistency of the global map is improved.

[0115] The loop detection optimization steps described in the present invention are implemented by adopting a multi-resolution search strategy. Loop detection is usually a computational bottleneck in SLAM, especially in large-scale scenarios where the search space is huge. The present invention proposes a multi-resolution search strategy, which significantly improves the efficiency and accuracy through hierarchical matching. The specific steps are as follows:

[0116] Step 3.1: Coarse matching at low resolution:

[0117] The grids of the low-resolution map are larger and cover a wider area, which can significantly reduce the computational amount. In the low-resolution map Glow Quickly screen candidate loop points c by maximizing the matching score between the point cloud P and the low-resolution map G low and calculate the rough matching pose T low :

[0118]

[0119] In the formula: represents the pose search space of the low-resolution map, that is, in the low-resolution map G low , the set of all possible candidate poses.

[0120] The low matching score function is defined as:

[0121] Score low (P, G low , X) = ∑ p∈P Likelihood low (p, G low , X) (13)

[0122] If the score exceeds the threshold, enter the next stage.

[0123] Step 3.2, High-resolution fine matching stage

[0124] In the high-resolution map G high , optimize with the low-resolution rough matching result T low as the initial value to obtain the fine matching pose T high :

[0125]

[0126] In the formula: represents the set of possible poses in the high-resolution map, given the low-resolution rough matching result T low . That is to say, is a region or range searched in the high-resolution map, and this range is based on the low-resolution rough matching T low obtained. It roughly indicates the pose space in the high-resolution map related to the low-resolution matching result. P represents a set of points that will be used when calculating the fine matching score.

[0127] The fine matching score function is defined as:

[0128] Score high (P, G low , X) = ∑ p∈P Likelihood high (p, G high , X) (15)

[0129] Where: p represents a specific point in the high-resolution map (such as a lidar data point or a feature point), which is used to calculate the score.

[0130] Through this strategy, the computational amount is effectively reduced and the matching accuracy is improved.

[0131] The present invention also provides a storage medium. When the program stored in the storage medium runs, it executes the above-mentioned laser SLAM mapping method based on the improved Cartographer algorithm.

[0132] The present invention also provides an electronic device, including a memory, a processor, and a computer program stored on the memory and executable on the processor. The processor runs through the computer program to execute the above-mentioned laser SLAM mapping method based on the improved Cartographer algorithm.

[0133] In the above embodiments of the present invention, the descriptions of the respective embodiments have their own emphases. For the parts not detailed in a certain embodiment, reference may be made to the relevant descriptions of other embodiments.

[0134] In the several embodiments provided by the present application, it should be understood that the disclosed technical content can be implemented in other ways. Among them, the device embodiments described above are only illustrative. For example, the division of the units can be a logical function division. In actual implementation, there can be other division methods. For example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the displayed or discussed coupling or direct coupling or communication connection between each other can be through some interfaces. The indirect coupling or communication connection of units or modules can be in an electrical or other form.

[0135] The units described as separate components may or may not be physically separated. The components displayed as units may or may not be physical units, that is, they can be located in one place, or they can be distributed to multiple units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0136] In addition, in each embodiment of the present invention, the functional units can be integrated in a processing unit, or each unit can exist physically alone, or two or more units can be integrated in one unit. The above-mentioned integrated units can be implemented in the form of hardware or in the form of software functional units.

[0137] When the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or all or part of the technical solution, can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes several instructions for causing a computer device (which can be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present invention. The aforementioned storage medium includes: various media such as USB flash drives, read-only memories (ROMs), random access memories (RAMs), external hard drives, magnetic disks, or optical discs that can store program codes.

[0138] Experimental verification

[0139] To verify the method proposed in the present invention, simulation mapping experiments (corresponding to Sections 4.1 - 4.3) and actual mapping experiments (corresponding to Section 4.4) were respectively carried out to test the effectiveness of the proposed method.

[0140] The simulation experiment runs on the ROS (Robot Operating System) system of the Ubuntu 18.04.4 LTS operating system, with the version being Noetic. The open-source TurtleBot3 and Cartographer algorithm packages are used. The specific settings are as follows:

[0141] (1) Configuration file: Run turtlebot3_lds_2d_gazebo.lua in the TurtleBot3 package;

[0142] (2) Robot model: Select the Waffle version;

[0143] (3) Maximum linear velocity: Set to 0.5 m / s;

[0144] (4) Maximum angular velocity: Set to 0.2 rad / s;

[0145] (5) Visualization tool: Use Rviz to observe the map and save it after the map construction is completed.

[0146] Two simulation environments were built through Gazebo (version 11.11):

[0147] (1) Long straight corridor environment: The corridor length is 87.63 meters (as Figure 2 ); (2) Square-shaped loop corridor environment: Corridor a has a length of 63.25 meters, and corridor b has a length of 42.37 meters (as Figure 3 ).

[0148] 4.1 Front-end module performance simulation and verification experiment

[0149] To verify the improvement effect of the front-end module algorithm proposed in this paper and exclude the influence of the loop detection part on the mapping quality, the experiment was carried out in Figure 2 a simulated long straight corridor environment, and the following three algorithms were used for testing respectively:

[0150] (1) Hector-SLAM; (2) Cartographer; (3) The improved algorithm proposed in the present invention.

[0151] Each algorithm constructs the map multiple times from left to right. The experimental results are as Figure 4 shown. Observe the result graph:

[0152] Hector-SLAM: In the long corridor environment, the pose estimation error is large, and the map gradually deviates from the original direction. Refer to (a) in the appendix Figure 4 .

[0153] Cartographer: The pose estimation error is reduced, but the map still shows arc bending and even tilting. Refer to (b) in the appendix Figure 4 .

[0154] The method of the present invention: As the robot walks along the long straight corridor, the map edge basically remains a straight line. Refer to (c) in the appendix Figure 4 .

[0155] To further quantitatively analyze the accuracy of map construction, 10 feature points on the long straight corridor map (as Figure 5 shown) were selected, and the error between the map distance and the actual simulation environment distance was measured. MAE (Mean Absolute Error) is the average of the absolute differences between the predicted value and the actual value, which can robustly reflect the actual size of the model prediction error and avoid the excessive influence of outliers on the results.

[0156] Through the measurement function in the Rviz software, the key distance lengths were measured multiple times and the average values were calculated. The experimental results are recorded in Table 1.

[0157] Table 1 Data table of front-end simulation experiment results

[0158]

[0159] According to the data in Table 1: The MAE of Hector-SLAM is 0.0539 m; the MAE of Cartographer is 0.0426 m; the MAE of the algorithm proposed in the present invention is 0.0302 m.

[0160] By comparing the absolute errors of the three algorithms in each section, the following conclusions can be drawn: The improved algorithm proposed in this paper is significantly lower than Hector-SLAM and Cartographer in terms of the overall error; compared with Hector-SLAM, the MAE in the map construction process is reduced by 0.0237 meters; compared with Cartographer, the MAE is reduced by 0.0124 meters.

[0161] The above results show that the front-end module optimization method proposed in this paper is superior to Hector-SLAM and Cartographer in terms of mapping accuracy and has better performance.

[0162] 4.2 Performance Simulation and Verification Experiment of the Loop Detection Module

[0163] To verify the performance of the algorithm proposed in the present invention in the loop detection module, mapping tests were continued in a simulated loop environment. The experiment controlled the robot to map according to the route of Figure 3 A - B - C - D - E - B - F - G - E - A in the figure. The same route was repeated more than 3 times in the same experiment. To reduce the influence of the performance gap of the front-end modules of each algorithm, a large number of obstacles (corner points) were added in each long straight corridor of the loop environment. In this experimental stage, the Cartographer algorithm and the algorithm proposed in the present invention were used for mapping tests, and the mapping results and local enlarged comparison diagrams are as shown in Figure 6 shown.

[0164] Cartographer algorithm: In some complex areas, the cumulative error could not be completely eliminated, resulting in a consistency problem; Algorithm of the present invention: The constructed global map is basically consistent with the actual environment, successfully completing the optimization of the global map and effectively eliminating the influence of the cumulative error. Figure 1 The above results show that the improved algorithm proposed in the present invention reduces the loop error situation compared with Cartographer in the loop detection stage, significantly improving the global consistency and accuracy of map construction.

[0165] The above results show that the improved algorithm proposed in the present invention reduces the loop error situation compared with Cartographer in the loop detection stage, significantly improving the global consistency and accuracy of map construction.

[0166] 4.3 Actual Mapping Experiment

[0167] To verify the accuracy of the algorithm of the present invention in the actual environment, the Cartographer algorithm and the algorithm of the present invention were evaluated and compared using an actual AGV trolley system. The experimental environment was on the second floor of Teaching Building No. 9 of Nanjing Forestry University (hereinafter referred to as Nanjing Forestry University Teaching Building No. 9 for short). The corridor of Nanjing Forestry University Teaching Building No. 9 is a closed "day" - shaped area, and its general orientation is similar to Figure 3 shown. The software and hardware configurations used are shown in Table 2:

[0168] Table 2 Software and Hardware Equipment of AGV

[0169]

[0170] The mapping results of the corresponding autonomous navigation system are as follows Figure 7 shown. It can be seen from the figure that

[0171] For the Cartographer algorithm, referring to Figure 7 (a) in: There are obvious incorrect loops in the position of the map established within the box in the figure;

[0172] For the method of the present invention, referring to Figure 7 (b) in: Most of the "ghosting" caused by incorrect loops is eliminated.

[0173] Quantitatively analyze the mapping accuracy of the two SLAM systems in the real environment. Taking the lengths of the east-west corridor and the north-south corridor of a certain building as an example (corresponding to Figure 3 corridors a, b, c, and d in), compare the mapping accuracy of the Cartographer algorithm before and after optimization. Use the measurement tool in the Rviz platform to obtain Figure 7 the lengths of each corridor in the map shown (measure 3 times and take the average), and compare with the measured values of the corridor lengths. The results are shown in Table 3.

[0174] Table 3 Comparison of the actual mapping errors of the AGV autonomous navigation system before and after optimization

[0175]

[0176] According to the results in Table 3, it is calculated that the MAE of mapping with the Cartographer algorithm is 0.226 meters; the MAE of mapping with the algorithm of the present invention is 0.169 meters, and the minimum mapping error is only 0.132 meters, which is 0.055 meters less than the Cartographer algorithm. This shows that in the actual environment, the algorithm proposed by the present invention effectively improves the mapping accuracy, verifying that the optimization of the algorithm of the present invention is reasonable and feasible.

[0177] In summary: The present invention proposes a laser SLAM mapping method based on improved Cartographer, and optimizes the algorithm for the problems of map construction and positioning in dynamic environments and complex loop scenarios. The accuracy and robustness of point cloud matching are improved through SURF feature extraction and generalized ICP registration; the delay judgment strategy effectively reduces the false alarm rate of false loops and enhances the reliability of loop detection; the multi-resolution search strategy combines coarse matching and fine matching, greatly improving the detection efficiency and accuracy.

[0178] The experimental results show that in the long straight corridor and loop environment, the mean absolute error of the method in this paper is significantly lower than that of the Hector-SLAM and Cartographer algorithms. The constructed map is highly consistent with the actual environment, solving the problems of cumulative error and false loop. In summary, the present invention has been significantly improved in terms of accuracy, robustness and efficiency, and is applicable to scenarios such as robot navigation and autonomous driving, having important engineering application value.

[0179] The above shows and describes the basic principles, main features and advantages of the present invention. Those skilled in the art should understand that the above embodiments do not limit the present invention in any form. Any technical solution obtained by using equivalent replacement or equivalent transformation falls within the protection scope of the present invention.

Claims

1. A laser SLAM mapping method based on an improved Cartographer algorithm, comprising a feature extraction and matching step and a loop detection step, characterized in that: The feature extraction and matching steps combine feature extraction based on the SURF algorithm with registration based on the GICP algorithm to achieve feature extraction and matching optimization of the two-dimensional laser point cloud.

2. The laser SLAM mapping method based on the improved Cartographer algorithm according to claim 1 is characterized in that , The feature extraction and matching steps specifically include: The SURF algorithm is used to extract the significant feature points of the two-dimensional laser point cloud, and a feature descriptor is generated for each significant feature point; Calculate the Euclidean distance between the feature descriptor of the target point cloud and the feature descriptor of the source point cloud; Filter the matching point pairs whose Euclidean distance meets the matching distance threshold; According to the selected matching point pairs, the initial pose transformation matrix of the point cloud is estimated; The optimization objective function of the initial pose transformation matrix is ​​established by minimizing the sum of squared Euclidean distances of matching point pairs: Where: T0 represents the initial pose transformation matrix of the target point cloud; p i ,q i is the matching point pair of the source point cloud and the target point cloud; R represents the rotation matrix of the pose transformation when the coordinate system of the target point cloud is converted to the coordinate system of the source point cloud; t represents the translation vector of the pose transformation when the coordinate system of the target point cloud is converted to the coordinate system of the source point cloud; N represents the number of matching point pairs formed by the source point cloud and the target point cloud; T represents the transformation matrix, which transforms the coordinate system of the source point cloud to the coordinate system of the target point cloud; The initial pose transformation matrix is ​​solved by singular value decomposition to obtain the initial pose estimation of the target point cloud; Based on the initial pose estimation of the target point cloud, an error function is constructed; the error function is defined as: e(p i ,q i )=(q i -R·p i -t) T (∑ p +∑ q ) -1 (q i -R·p i -t) Where: ∑p is point p i The covariance matrix of i The covariance matrix of The error function is solved to obtain the optimal pose transformation matrix.

3. The laser SLAM mapping method based on the improved Cartographer algorithm according to claim 2, characterized in that: The SURF algorithm is used to extract the significant feature points of the two-dimensional laser point cloud, which specifically includes the following steps: After graying the target point cloud, the target point cloud is projected into a two-dimensional grid map using the projection density gridding method, so as to transform the sparse point cloud data into a two-dimensional grid map with intensity information to form the input data for SURF feature extraction; The pose relationship between the current laser frame and the 2D grid map is estimated by optimizing the following objective function: Where T is the pose transformation to be solved, including the rotation matrix R and the translation vector t; z i is the point of the current laser frame; m i is a point in the 2D grid map; w i is the weight of the point pair; SURF feature point extraction is performed based on the Hessian matrix to obtain the significant feature points of the two-dimensional laser point cloud.

4. The laser SLAM mapping method based on the improved Cartographer algorithm according to claim 1, characterized in that: In the loop detection step, a delay judgment module is introduced before identifying the loop point to optimize the loop detection.

5. The laser SLAM mapping method based on the improved Cartographer algorithm according to claim 4, characterized in that: The delay determination module specifically optimizes loop detection through the following steps: Calculate the geometric consistency scores of the candidate loop points and directly eliminate the candidate loop points with low scores to obtain the initial loop point pairs a and b; Use a delayed judgment strategy to determine the validity of candidate loop points: set a fixed time window t window During this period, it is continuously monitored whether there are new loop point pairs c and d generated; if so, the pose relationship of loop points a, b, c, d is geometrically verified; otherwise, the initial loop point pairs a and b are eliminated; When performing geometric verification on the pose relationship of loop points a, b, c, d, first verify whether the pose relationship of loop points a, b, c, d satisfies the following closed-loop constraints: T a→b ·T b→d ·T d→c ·T c→a =I Where I is the identity matrix; T a→b represents the relative position of loop point a and loop point b; T b→d represents the relative position of loop point b and loop point d; T d→c represents the relative position of loop point d and loop point c; T c→a Indicates the relative position of loop point c and loop point a; The verification results show that when the position relationship of the loop points a, b, c, d does not fully satisfy the above closure constraints, the matrix norm G is calculated as || T a→b ·T b→d ·T d→c ·T c→a -I||, and through the error tolerance mechanism, determine whether the matrix norm G is less than the matrix norm threshold G0; if the judgment result shows that the matrix norm G is less than the matrix norm threshold G0, the loop detection is considered to be credible, and the loop point is added to the optimization model, otherwise, the relevant loop point is removed.

6. The laser SLAM mapping method based on the improved Cartographer algorithm according to claim 1, characterized in that: The loop detection adopts a multi-resolution search strategy to accelerate the loop detection.

7. The laser SLAM mapping method based on the improved Cartographer algorithm according to claim 1, characterized in that: The multi-resolution search strategy described above accelerates loop detection by the following steps: Low-resolution coarse matching: In the low-resolution map G low In the process of quickly screening candidate loop points c, by maximizing the point cloud P and G low The matching score is calculated to calculate the rough matching pose T low ; High-resolution fine matching: In the high-resolution map, the low-resolution coarse matching result is used as the initial value to further optimize the matching accuracy of the loop points.

8. The laser SLAM mapping method based on the improved Cartographer algorithm according to claim 7, characterized in that: Rough matching pose T low Calculated by the following formula: Where: X represents an estimated pose of the robot in the low-resolution map, which is used for preliminary matching of loop detection; Represents all possible poses in the search space; Score low (P,G low ,X) is the matching score of point cloud P in the low-resolution grid, and the formula is: Score low (P,G low ,X)=Σ p∈P Likelihood low (p,G low ,X); Where: Matching likelihood Likelihood low The specific meaning is that point p is in the grid G low The matching probability value in is calculated as follows: Likelihood low (p,G low ,X)=OccupancyProbability(g p ) Fine matching pose T high Calculated by the following formula: Where: Represents the rough matching pose T low Limited high-resolution search space; Score high (P,G high ,X) represents the fine matching score function, the formula is: Score high (P,G high ,X)=∑ p∈P Likelihood high (p,G high ,X); Where: G high represents a high-resolution map. Similarly, the matching likelihood high For point p on the grid G high The occupancy probability value in is calculated in the same way as the low resolution, but because the high resolution captures more environmental details, the matching accuracy is higher.

9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and running on the processor, wherein the computer program runs to execute the laser SLAM mapping method based on the improved Cartographer algorithm as described in any one of claims 1 to 8.

Images