Laser radar synchronous positioning mapping method and system based on improved NDT odometer

Abstract

The invention belongs to the technical field of positioning and mapping (SLAM), and particularly relates to a laser radar synchronous positioning and mapping method based on an improved normal distribution transformation (NDT) odometer. The method comprises the steps of point cloud preprocessing, optimized segmentation and extraction, noise and other redundancy removal, and feature point cloud forming; calculating by using a LiDAR odometer, discretizing the point cloud by using NDT and modeling, and determining a transformation matrix to calculate a pose track; closed-loop detection is carried out, and in combination with Radius Search and Scan Context, poses are weighted and combined according to detection results; global optimization is carried out, closed-loop constraint is introduced, a factor graph is used for optimizing a map and a track, and feature point cloud registration is combined with full-point cloud projection. The method has the obvious advantages that the efficiency and precision are improved by unifying the feature point cloud framework; the closed-loop detection module is good in improvement effect; the global optimization strategy is good; experiments prove that the method is excellent in registration speed, precision, real-time performance and the like, is suitable for complex environments, and provides a reliable positioning and mapping scheme for related fields.

Description

Technical Field

[0001] The present invention relates to the technical field of positioning and mapping, and in particular to a laser radar synchronous positioning and mapping method and system based on an improved NDT odometer. Background Art

[0002] In key areas such as autonomous driving, drone navigation and robot positioning, LiDAR has become the core sensor for achieving real-time mapping (SLAM) due to its high accuracy, strong environmental adaptability and ability to generate dense point cloud data. However, the efficient operation of SLAM systems faces many challenges, mainly concentrated in the two key links of point cloud registration and closed-loop detection.

[0003] Traditional point cloud registration algorithms, such as the iterative closest point (ICP) algorithm, have simple principles but significant defects when dealing with complex scenes. They have high computational complexity, slow convergence speed, and are very easy to fall into local optimal solutions. They also require extremely high accuracy of the initial pose, which makes them difficult to be competent in application scenarios that require high efficiency and accuracy. Although the normal distribution transform (NDT) algorithm has improved computational efficiency to a certain extent, it still has the problem of excessive consumption of computing resources in the face of large-scale and dense point cloud data, especially in autonomous driving and drone applications with extremely high real-time requirements.

[0004] To address these challenges, feature extraction-based methods have emerged. For example, LOAM and its derivative algorithms simplify the point cloud registration process by extracting plane and edge features in the environment. However, in large-scale and complex environments, these methods still do not achieve an ideal balance between accuracy and efficiency, and are difficult to meet the strict requirements of practical applications.

[0005] In terms of closed-loop detection, traditional methods based on odometer information are seriously affected by cumulative errors, and the detection accuracy drops significantly over time. Although the introduction of geometric methods such as Scan Context has improved the detection accuracy to a certain extent, there is still a risk of misjudgment in complex environments. Although deep learning methods show high accuracy and adaptability, they are difficult to be widely used in different LiDAR models and sensor configurations due to their high training cost and limited generalization ability. Summary of the invention

[0006] To solve the above problems, the core goal of the present invention is to provide an innovative LiDAR-SLAM framework, which effectively overcomes the shortcomings of the prior art in terms of large-scale point cloud processing efficiency, closed-loop detection accuracy and overall system robustness, achieves high-precision, high-real-time positioning and mapping, and improves the comprehensive performance of the SLAM system in complex environments. The present invention provides a laser radar synchronous positioning and mapping method based on an improved NDT odometer, comprising the following steps:

[0007] The robot is configured with a laser radar to scan and obtain the original environmental point cloud data, and then pre-processed to extract environmental features to obtain effective feature point clouds;

[0008] Use feature point cloud for NDT registration to calculate the relative motion and position of the robot and generate the pose trajectory;

[0009] A parallel hybrid method combining radius search and scan context is used to perform closed loop detection, correct the accumulated error, and update the above pose to obtain a more accurate pose.

[0010] The complete original environment point cloud data is used to construct the environment map, and the environment map and posture are optimized to ensure global consistency, generate high-precision maps and trajectories, and realize mapping.

[0011] Furthermore, it is used to process the original laser scanning data, including removing ground points, noise points and small target point clouds, dividing the point clouds into scan lines according to vertical angles, calculating the smoothness of the points and classifying them into edge points and plane points, and combining the ground points to form a feature point cloud to reduce redundant data and retain key features, thereby reducing computational complexity.

[0012] Furthermore, the point cloud is discretized into a grid through the NDT method, and the point cloud distribution in the grid is modeled using Gaussian distribution. The point cloud registration is transformed into a statistical optimization problem, and the transformation matrix that matches the source point cloud and the target point cloud distribution is determined. The pose changes between consecutive frames are calculated, and the robot's global pose trajectory is iteratively updated.

[0013] Furthermore, a parallel hybrid method combining radius search and scan context is adopted for loop closure detection. Specifically, Radius Search uses KD-tree to search historical frames near the current key frame and perform ICP registration. Scan Context converts the point cloud of the current frame into polar coordinates to calculate eigenvalues ​​for global matching. According to the detection results and confidence scores of the two methods, a weighted combination is performed to obtain the updated pose of the current frame to improve the robustness and accuracy of loop closure detection.

[0014] Furthermore, after completing the point cloud registration and closed-loop detection, the relative pose obtained by the closed-loop detection is introduced as a constraint into the back-end optimization, the factor graph model is used to optimize the map and trajectory, and the feature point cloud is used for inter-frame registration to reduce the computational complexity. The complete point cloud is then projected to the global coordinate system to generate a map. By minimizing the weighted square error of the pose constraint, a globally consistent map and trajectory are generated to ensure global consistency and accuracy.

[0015] Furthermore, the so-called point cloud preprocessing is to deeply optimize the original laser scanning data to reduce data redundancy and reduce the computational burden of subsequent point cloud registration. Its core technology draws on cutting-edge research results and adopts efficient point cloud segmentation and feature extraction strategies.

[0016] First, a specific algorithm is used to accurately extract ground points and mark them as P ground , so that it does not participate in the subsequent segmentation process. For the remaining non-ground points P non_ground , according to the vertical angle, based on N scanning lines, use filtering technology to remove outliers and small target points to ensure the validity and stability of the data. Then perform feature extraction, for the i-th point p i , by calculating its smoothness c with adjacent points i , and its calculation formula is:

[0017]

[0018] Where S is point p i The set of adjacent points in the same row, r i For the i-th point p i The position vector of the image is obtained by comparing the smoothness with the preset threshold c, and the points are accurately classified into edge points and plane points. Finally, the plane points, edge points and ground points are organically combined to form a feature point cloud. This processing method not only greatly reduces the amount of data, but also effectively retains the key features of the environment, laying the foundation for subsequent processing and significantly improving the processing efficiency and accuracy of the system.

[0019] Furthermore, the NDT method is used to calculate the relative motion and position of the robot in the environment. The core principle of the NDT method is to discretize the point cloud into grids and use Gaussian distribution to model the point cloud distribution in each grid. The calculation formulas for the mean μ and covariance Σ are as follows:

[0020]

[0021] Where n is the number of points in the grid, p i is the coordinate of the i-th point.

[0022] Based on the above statistics, the point cloud distribution of each grid cell can be expressed as a Gaussian distribution probability density function in the following form:

[0023]

[0024] Among them, x is the point to be evaluated in the point cloud, D is the dimension of the point cloud, ∑ is the covariance matrix, |Σ| is the determinant of the covariance matrix Σ, and μ is the mean of the points in the grid. The point cloud is converted into a continuous mathematical representation through this probability distribution. In the registration process, the transformation T that maximizes the likelihood of matching the distribution of the target point cloud with the transformed source point cloud is sought.

[0025] In the process of point cloud registration, it is transformed into a complex statistical optimization problem. By maximizing the likelihood of distribution matching between the source point cloud and the target point cloud after transformation, the transformation relationship T that makes the two match is accurately determined. On this basis, the posture changes between consecutive frames are calculated, and the global posture trajectory of the robot in three-dimensional space is generated in real time through an iterative update mechanism.

[0026] Furthermore, in order to address the limitations of traditional NDT methods, a parallel hybrid strategy combining radius search (RadiusSearch) and scan context (Scan Context) is adopted. Radius Search uses an efficient KD-tree data structure to quickly search for historical key frames near the current key frame based on a carefully set radius threshold, and uses it as a high-quality target point cloud for ICP registration. Scan Context converts the point cloud information of the current frame into polar coordinates through a unique algorithm, accurately calculates the Ring value, and then performs an efficient search based on the descriptor in the KD-tree to complete the closed loop detection. In the closed loop detection, if Radius Search successfully detects a closed loop (C RS =1), then use the transformation matrix obtained Update the pose; if Radius Search does not detect a closed loop (C RS = 0) but Scan Context detected a closed loop (C SC =1), then use the transformation matrix obtained by Scan Context Correction pose, updated pose of current frame k The calculation formula is as follows:

[0027]

[0028] Among them C RS and C SC Represents the confidence score of the two detection results, C RS ∈{0,1},C SC ∈{0,1}. If C RS =1, then use the transformation matrix obtained by Radius Search Update the pose; if and C SC = 1, then the Scan Context is used to obtain Correction posture, Tk is the pose before transformation of the current frame.

[0029] This dual-loop detection strategy fully utilizes the advantages of the two closed-loop detection methods, effectively improving the accuracy and robustness of closed-loop detection, and ensuring that the system can operate stably even in complex environments.

[0030] Furthermore, after completing the point cloud registration and closed-loop detection, the global optimization module begins to play a role. After the closed-loop detection successfully identifies the number of closed loops, the relative pose of the current frame and the historical frame will be calculated and output, and introduced into the back-end optimization process as a key constraint. During the back-end optimization process, the factor graph model is used to accurately model the map and trajectory. Among them, the point cloud frame pose is used as the key vertex, and the relative transformation between frames and the constraints generated by closed-loop detection are used as important edges. In the point cloud mapping process, the feature point cloud is fully utilized for efficient inter-frame registration, effectively reducing the computational complexity. Subsequently, the complete point cloud is projected into the global coordinate system according to the precise pose transformation matrix to generate a high-precision map. This process can be expressed by the following formula:

[0031] P map ={T f P f |f=1,2,...,N}

[0032] Among them, P map Represents the final point cloud map, T f represents the transformation matrix of the point cloud of the f-th frame, P f Represents the point cloud of the fth frame. By minimizing the weighted square error of all pose constraints, the global consistency optimization of the system is achieved, ensuring that the final generated map and robot motion trajectory have high accuracy and reliability.

[0033] According to another aspect of the present invention, a laser radar synchronous positioning and mapping system based on an improved NDT odometer is provided, comprising:

[0034] Point cloud preprocessing module, used to preprocess the original environmental point cloud data obtained by scanning with any configuration of the robot laser radar, extract environmental features to obtain effective feature point cloud;

[0035] The LiDAR odometer calculation module uses the feature point cloud for NDT registration to calculate the relative motion and position of the robot and generate the pose trajectory;

[0036] The closed loop detection module uses a parallel hybrid method combining radius search and scan context to perform closed loop detection, correct the accumulated error, and update the above pose to obtain a more accurate pose;

[0037] The global optimization module uses complete point cloud data to build maps, optimizes environmental maps and poses, ensures global consistency, generates high-precision maps and trajectories, and realizes mapping.

[0038] The laser radar synchronous positioning and mapping method based on the improved NDT odometer of the present invention effectively improves the SLAM performance in complex environments through the above-mentioned point cloud preprocessing, laser radar odometer calculation, loop detection and global optimization and other specific implementation methods. It has higher computing efficiency, alignment accuracy and loop detection accuracy when processing large-scale point clouds, and can achieve reliable pose estimation and map construction to meet the needs of fields such as autonomous driving, drone navigation and robot positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] Figure 1 It is a schematic diagram of the process of the present invention;

[0040] Figure 2 It is a schematic diagram of plane feature and edge feature point extraction;

[0041] Figure 3 It is a schematic diagram of feature point cloud;

[0042] Figure 4 It is a schematic diagram of the map construction effect. DETAILED DESCRIPTION

[0043] The principles and features of the present invention are described below in conjunction with the accompanying drawings. The examples given are only used to explain the present invention and are not used to limit the scope of the present invention.

[0044] refer to Figure 1 ,refer to Figure 1 The present invention provides a laser radar synchronous positioning and mapping method based on an improved NDT odometer to solve the problems of low computational efficiency, insufficient registration accuracy, and lack of loop detection faced by the existing NDT laser radar SLAM system when processing large-scale point clouds. It includes the following steps:

[0045] The robot is configured with a laser radar to scan and obtain the original environmental point cloud data, and then pre-processed to extract environmental features to obtain effective feature point clouds;

[0046] Use feature point cloud for NDT registration to calculate the relative motion and position of the robot and generate the pose trajectory;

[0047] A parallel hybrid method combining radius search and scan context is used to perform closed loop detection, correct the accumulated error, and update the above pose to obtain a more accurate pose.

[0048] Use the complete point cloud data to build the map, optimize the environment map and pose, ensure global consistency, generate high-precision maps and trajectories, and realize mapping.

[0049] 1. Point cloud preprocessing

[0050] Point cloud preprocessing focuses on reducing the redundancy of raw laser scanning data and lowering the computational burden in the point cloud registration process, thereby improving the accuracy and robustness of the registration process.

[0051] First, for the point cloud acquired by the lidar at time t According to the structure of the laser radar, it is divided into N scan lines. Before segmentation, the ground points are extracted and marked as P ground , and exclude these points from the subsequent segmentation process. The remaining non-ground points P non-ground For object segmentation, clustering is used to group points and assign specific categories to each group, while filtering outliers and small point clouds, retaining only point clouds corresponding to larger objects.

[0052] Furthermore, feature extraction is performed on non-ground points. Figure 2 The schematic diagram of extracting plane feature points and edge feature points is shown in Figure 1. Let S be point p i For a set of adjacent points in the same row, the smoothness c of the point is calculated by the following formula i :

[0053]

[0054] Among them, r i It's point p i The position vector, r j is the position vector of the adjacent point in S. t Medium smoothness c i Points greater than a given threshold c are classified as edge points, and points less than c are classified as plane points. After sorting the smoothness values, the plane and edge features are combined with the ground points to form the required feature point cloud. The effect can be referred to Figure 3 Schematic diagram of feature point cloud. This feature extraction method is different from the traditional downsampling method. It retains key features while reducing the number of point clouds, reduces computational costs and improves matching robustness during registration.

[0055] 2. LiDAR Odometry

[0056] After point cloud preprocessing, the characteristic point cloud of the environment is used to calculate the relative motion and position of the robot in the environment. This process adopts the NDT method.

[0057] NDT is a probabilistic method whose core principle is to discretize the point cloud into a grid and model the point distribution in each cell as a Gaussian distribution. In this embodiment, the environmental feature point cloud is divided into multiple grid cells, each cell contains a set of points, and the points in the cell are modeled using a Gaussian distribution. The calculation formulas for the mean μ and covariance Σ are as follows:

[0058]

[0059] Where n is the number of points in the grid, p i is the coordinate of the i-th point. Based on the above statistics, the point cloud distribution of each grid unit can be expressed as a probability density function in the following form:

[0060]

[0061] Among them, x represents a point to be evaluated, D is the spatial dimension, and |Σ| is the determinant of the covariance matrix Σ.

[0062] In the process of point cloud registration, the NDT method determines the transformation T by maximizing the likelihood of matching the transformed source point cloud with the target point cloud distribution. Specifically, the objective function is expressed by the following formula:

[0063]

[0064] in, represents the transformation parameters (including translation and rotation), n is the number of source point clouds, and p i is the i-th point in the source point cloud, T(ξ,p i ) indicates p i The position after the rigid body transformation parameter ξ is transformed, f(T(ξ,p i )) means p i The probability value under the probability distribution of the target point cloud, and T(ξ,p i )=R t p i +t, where R t The rotation component The derived rotation matrix, t is the translation vector between the target point and the source point.

[0065] After completing the point cloud registration, the lidar odometer generates the robot's posture trajectory in three-dimensional space by calculating the posture changes between consecutive frames. For each frame f, its transformation matrix T f It is expressed by the following formula:

[0066]

[0067] This includes the rotation matrix R t and the translation vector t fThe global pose of the robot is updated iteratively. Assuming that the global pose of the previous frame is Then the global pose of the current frame is This generates the robot's trajectory.

[0068] 3. Loop detection

[0069] Loop closure detection is a key component of this SLAM system, which aims to identify repeated positions in the robot trajectory and mitigate the accumulated error due to odometry drift.

[0070] The present invention adopts a method combining radius search and scan context. Radius search uses KD-tree to search for historical key frames near the current key frame according to the specified radius threshold, and uses it as the target point cloud for ICP registration. Scan Context converts the point cloud information of the current frame into polar coordinates for calculation and obtains the Ring value, and completes loop detection by searching based on descriptors in KD-tree.

[0071] In actual operation, C RS and C SC Represents the loop detection results of the two detection methods, where C RS ∈{0,1} represents the confidence score of Radius Search, that is, the reliability of the closest geometric match; C SC ∈{0,1} represents the confidence score of ScanContext, that is, the similarity of the global descriptor. If Radius Search successfully detects a loop (C RS =1), then use the transformation matrix obtained from Radius Search Update the pose; otherwise, if Radius Search does not detect a loop (C RS =0), but Scan Context detects (C SC =1), then the transformation matrix derived from Scan Context is applied Correct the pose. Update the pose of the current frame k It is a weighted combination of the transformations provided by the two methods and is expressed as follows:

[0072]

[0073] Where T k is the pose of the current frame before transformation, and These are the correction relationships of the poses obtained by the two methods. This confidence weighting mechanism ensures that the final pose correction in a given scenario gives priority to the method with higher reliability. By combining these two methods, the present invention can effectively deal with the loop detection problem in complex geometric shapes or large odometer drift environments, significantly improve the accuracy and robustness of loop detection, and is superior to traditional loop detection technology that relies on only a single method.

[0074] 4. Global Optimization

[0075] To ensure the global consistency of the point cloud map and improve the accuracy of trajectory estimation, the system map and pose optimization are performed after completing point cloud registration and loop closure detection.

[0076] In the loop detection phase, when a loop is successfully detected, the relative pose between the current frame and the historical frame is calculated using the registration algorithm and introduced as a new constraint into the backend optimization process. These constraints effectively compensate for the cumulative error in the trajectory and ground. Figure 1 The impact of consistency is significantly improved, especially in the long-term operation process.

[0077] In the backend optimization, the map and trajectory are modeled as factor graphs, with the poses of the point cloud frames as vertices, and the relative transformations between frames and the constraints generated by loop closure detection as edges. In the point cloud mapping process, feature point clouds (including edges and planar points) are used for inter-frame registration. Compared with directly using the complete point cloud, the sparsity of the feature point cloud significantly reduces the computational complexity of the registration process while maintaining a high matching accuracy. After completing the inter-frame registration, the pose transformation matrix derived from the registration is used to project the complete point cloud into the global coordinate system, effectively combining the computational efficiency of the feature point cloud and the rich details provided by the complete point cloud to generate a map that is both accurate and visually detailed. The mapping effect can be referred to. Figure 4 .

[0078] Specifically, the complete point cloud map is calculated by the following formula:

[0079] P map ={T f P f |f=1,2,...,N}

[0080] Where P f represents the point cloud of the fth frame, T f represents the transformation matrix corresponding to the fth frame. The core goal of the optimization is to generate a globally consistent map and trajectory by minimizing the weighted square error of all pose constraints. After the optimization is completed, the system reprojects the complete point cloud data according to the optimized pose to generate a globally consistent point cloud map, and uses the optimized pose to adjust the original trajectory estimate to obtain a globally consistent representation of the robot's motion path.

[0081] The laser radar synchronous positioning and mapping method based on the improved NDT odometer provided by the present invention has practical value in multiple actual scenarios. In the field of autonomous driving, automobile driving requires accurate environmental maps and positioning information to ensure safe navigation. The point cloud data collected by the on-board laser radar is processed by this method, and the point cloud preprocessing effectively filters out redundancy and interference. At the same time, the feature point cloud is used for inter-frame registration to improve the calculation efficiency, closed-loop detection corrects the odometer drift, and global optimization generates high-precision maps and trajectories. Compared with other methods, this method can better cope with complex road environments and ensure the safe and reliable operation of autonomous driving.

[0082] As for drone navigation, drones that perform surveying, inspection, and rescue missions use this method to process lidar point clouds. It can reduce the computational burden of the drone's computing unit, increase positioning accuracy in complex terrain (such as mountains, forests, cities, etc.), extend endurance, ensure accurate navigation, and effectively expand the scope of drone applications.

[0083] In the field of robot positioning and navigation, such as industry, logistics, and indoor environments, this method can reduce computational complexity, improve registration accuracy, and use closed-loop detection to correct accumulated errors, helping robots quickly perceive the environment and plan paths, effectively respond to complex and changeable indoor situations, and maintain long-term stable positioning and navigation performance.

[0084] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims

1. A laser radar synchronous positioning and mapping method based on an improved NDT odometer, characterized in that: The following steps are included: The robot is configured with a laser radar to scan and obtain the original environmental point cloud data, and then pre-process it to extract environmental features to obtain effective feature point clouds; Use feature point cloud for NDT registration to calculate the relative motion and position of the robot and generate the pose trajectory; A parallel hybrid method combining radius search and scan context is used to perform closed loop detection, correct the accumulated error, and update the above pose to obtain a more accurate pose. The complete original environment point cloud data is used to construct the environment map, and the environment map and posture are optimized to ensure global consistency, generate high-precision maps and trajectories, and realize mapping.

2. The laser radar synchronous positioning and mapping method based on the improved NDT odometer according to claim 1, characterized in that: It is used to process the original laser scanning data, including removing ground points, noise points and small target point clouds, dividing the point clouds into scan lines according to vertical angles, calculating the smoothness of the points and classifying them into edge points and plane points, and combining the ground points to form a feature point cloud.

3. The laser radar synchronous positioning and mapping method based on the improved NDT odometer according to claim 1, characterized in that: The point cloud is discretized into a grid through the NDT method, and the point cloud distribution in the grid is modeled using Gaussian distribution. The transformation matrix that matches the source point cloud with the target point cloud distribution is determined, the pose changes between consecutive frames are calculated, and the global pose trajectory of the robot is iteratively updated.

4. The laser radar synchronous positioning and mapping method based on the improved NDT odometer as claimed in claim 1, characterized in that: A parallel hybrid method combining radius search and scan context is adopted for closed-loop detection. Specifically, Radius Search uses KD-tree to search historical frames near the current key frame and perform ICP registration. Scan Context converts the point cloud of the current frame into polar coordinates to calculate eigenvalues ​​for global matching. According to the detection results and confidence scores of the two methods, a weighted combination is performed to obtain the updated pose of the current frame.

5. The laser radar synchronous positioning and mapping method based on the improved NDT odometer according to claim 1, characterized in that: After completing the point cloud registration and closed-loop detection, the relative pose obtained by the closed-loop detection is introduced as a constraint into the back-end optimization. The factor graph model is used to optimize the map and trajectory. The feature point cloud is used for inter-frame registration to reduce the computational complexity. The complete point cloud is then projected to the global coordinate system to generate a map. By minimizing the weighted square error of the pose constraint, a globally consistent map and trajectory are generated.

6. The laser radar synchronous positioning and mapping method based on the improved NDT odometer as claimed in claim 2, characterized in that: The method for calculating the smoothness of points in the point cloud preprocessing is: set point p i The set of adjacent points is S, r i For p i The position vector, r j is the position vector of the adjacent point in s, point p i The smoothness of is determined by the following formula: Points are classified as edge points or plane points based on the comparison of smoothness with the set threshold.

7. The laser radar synchronous positioning and mapping method based on the improved NDT odometer as claimed in claim 3, characterized in that: The Gaussian distribution probability density function is shown in the following formula: Among them, x is the point to be evaluated in the point cloud, D is the dimension of the point cloud, Σ is the covariance matrix, μ is the mean of the points in the grid, and the point cloud is converted into a continuous mathematical representation through this probability distribution. In the registration process, the transformation T that maximizes the likelihood of matching the distribution of the target point cloud with the transformed source point cloud is sought.

8. The laser radar synchronous positioning and mapping method based on the improved NDT odometer as claimed in claim 4, characterized in that: In the closed loop detection module, if Radius Search successfully detects a closed loop (C RS =1), then use the transformation matrix obtained Update the pose; if Radius Search does not detect a closed loop (C RS = 0) but Scan Context detected a closed loop (C SC =1), then use the transformation matrix obtained by Scan Context Correct the pose, and the updated pose of the current frame k is shown in the following formula: Among them, T k is the pose before transformation of the current frame.

9. The laser radar synchronous positioning and mapping method based on the improved NDT odometer as claimed in claim 5, characterized in that: During backend optimization, the map and trajectory are modeled as factor graphs, with the point cloud frame poses as constraint vertices, and the relative transformation between frames and closed-loop detection constraints as edges. Global optimization is achieved by minimizing the weighted square error of all pose constraints. The complete laser point cloud data is used to generate a globally consistent map and trajectory, and the point cloud data is reprojected according to the optimized pose. This process can be expressed by the following formula: P map ={T f P f ∣f=1,2,...,N} Among them, P map Represents the final point cloud map, T f represents the transformation matrix of the point cloud of the f-th frame, P f Represents the point cloud of the fth frame.

10. The laser radar synchronous positioning and mapping system based on the improved NDT odometer as claimed in claim 5, characterized in that: include, Point cloud preprocessing module, used to preprocess the original environmental point cloud data obtained by scanning with any configuration of the robot laser radar, extract environmental features to obtain effective feature point cloud; The LiDAR odometer calculation module uses the feature point cloud for NDT registration to calculate the relative motion and position of the robot and generate the pose trajectory; The closed loop detection module uses a parallel hybrid method combining radius search and scan context to perform closed loop detection, correct the accumulated error, and update the above pose to obtain a more accurate pose; The global optimization module uses the complete original environment point cloud data to build the environment map, and optimizes the environment map and posture to ensure global consistency, generate high-precision maps and trajectories, and realize mapping.

Images