Self-adaptive laser mapping method fusing historical pose and point cloud characteristics

Abstract

The invention discloses a self-adaptive laser mapping method fusing historical poses and point cloud characteristics, and belongs to the technical field of robot positioning and map construction, and the method comprises the steps: reading point cloud data of a laser radar sensor, and carrying out the preprocessing; modeling the change trend of the obtained historical pose as a Lie algebraic vector field, and constructing a dynamic correction guide field in combination with historical error information; classifying the point cloud into planar points and non-planar points; calculating adaptive weights of different classification points; and respectively establishing point-to-point and point-to-plane residual errors according to point cloud classification, solving the pose of the robot through an adaptive weighted Gauss-Newton optimization method, and updating a local map and a historical pose sequence. According to the method, the historical motion trend and real-time deviation correction are fused, and a geometric feature self-adaptive registration strategy is combined, so that the pose estimation precision and robustness in a complex motion mode and a feature degradation scene are remarkably improved, and an effective solution is provided for reliable navigation of a robot in a GPS denial environment.

Description

Technical Field

[0001] This invention belongs to the technical field of robot localization and mapping, specifically relating to an adaptive laser mapping method that integrates historical pose and point cloud characteristics. Background Technology

[0002] In today's rapidly developing robotics and autonomous driving technologies, Simultaneous Localization and Mapping (SLAM) technology is crucial. SLAM enables robots to determine their own position in a dynamic environment in real time and simultaneously build a map of their surroundings, which is the foundation for autonomous navigation and task execution. In SLAM systems, the choice of sensors plays a decisive role. Sensor performance directly affects the accuracy and reliability of the SLAM algorithm, while also influencing system cost and resource consumption. LiDAR sensors offer high-precision distance measurement and significant advantages in low-light environments; therefore, LiDAR has become an indispensable sensor in autonomous driving and robotics systems and is used for odometry calculations.

[0003] Iterative Closest Point (ICP) and its variants are core to pose estimation in laser SLAM, essentially calculating the robot's pose by minimizing the residuals between corresponding points. Most advanced odometry algorithms use point-to-point or point-to-plane error metrics. Existing lidar odometry systems typically rely on a single error metric. The Adaptive Point Cloud Registration Algorithm (KISS-ICP) employs an adaptive thresholding method to perform point-to-point ICP directly, processing point cloud data and leveraging sparsity optimization to excel in feature-sparse environments. Considering the universality of planar features, Fast Direct Laser Inertial Odometry (FAST-LIO2), Continuous Time Radar Odometry (CT-ICP), and Robust Point Cloud Registration Algorithm (MAD-ICP) utilize point-to-plane models to construct geometric constraints for the point cloud, enabling the algorithms to adapt to different types of lidar. However, relying on a single metric inherently limits these methods in complex real-world environments: in highly structured environments (such as cities), planar features are rich and stable, and point-to-plane metrics perform well; but in unstructured environments (such as forests), features are sparse and poorly structured, making normal vector estimation unreliable or even meaningless, in which case directly processing point cloud data is more advantageous. To ensure the robustness of laser SLAM algorithms, the Generalized Iterative Closest Point (GICP) algorithm combines point-to-point and point-to-plane error metrics within a probabilistic framework. The Degenerate Point Cloud Registration (GenZ-ICP) algorithm uses both point-to-point and point-to-plane metrics simultaneously, adjusting the weights of the two metric errors in the optimization problem based on the ratio of planar to non-planar points, thus enhancing adaptability to different environments. Direct LiDAR Odometry (DLO) matches downsampled point clouds with maps and uses GICP errors to calculate odometry, obtaining fast and accurate results. However, existing hybrid methods fail to adequately consider the degree of matching between the local geometric distribution attributes of each point and the residual metric used.

[0004] For pure LiDAR SLAM systems, especially those employing point cloud registration algorithms, a sufficiently accurate initial pose estimate is crucial. When the initial pose error is large, the registration algorithm is likely to converge to a local minimum, leading to severe pose estimation failures. An accurate initial estimate can limit the search space of the data association to a local neighborhood near the true optimum, reducing the number of convergence iterations, alleviating computational burden, increasing the probability of convergence to the global optimum, and obtaining a more accurate and robust final pose estimate. Many state-of-the-art laser odometry systems integrate modules to predict the initial pose of the current scan. Real-time laser odometry with mapping algorithms (LOAM), lightweight laser odometry (Lego-LOAM), and KISS-ICP employ the most common and straightforward constant-velocity model, which assumes that the sensor's motion remains constant between adjacent optimization frames. Some more complex frameworks utilize continuous-time trajectories to improve motion prediction; CT-ICP uses linear interpolation to parameterize continuous-time trajectories, and MAD-ICP uses a B-spline model to fit recent trajectories, thus providing smoother, kinematically more consistent predictions of the initial pose. Although existing prediction methods have achieved good results, constant velocity models are inherently too simple to capture complex motions, while continuous time models, although improved, still lack the ability to predict dynamically changing motions.

[0005] Existing LiDAR odometry methods have significant limitations in initial pose prediction and feature weighting. Iterative nearest-point registration methods typically employ constant velocity or continuous-time assumptions for motion prediction. However, constant velocity models are too simplistic to handle complex motions, while continuous-time models are still susceptible to error accumulation and sudden motion changes, leading to excessively large initial pose deviations and registration converging to local optima. Furthermore, existing methods often rely on a single error metric for point-to-point or point-to-plane measurements, or, while employing hybrid metrics, fail to dynamically allocate weights based on the local geometric properties of points, resulting in a significant decrease in registration accuracy in feature-degraded environments. Existing technologies have shortcomings in fixed weight strategies and initial pose prediction. There is an urgent need for a method that can dynamically adjust point cloud weights based on local geometric characteristics and use historical information to provide reliable initial poses in order to meet the high-precision positioning requirements in complex motion and feature degradation scenarios. Summary of the Invention

[0006] The purpose of this invention is to address the above-mentioned shortcomings in the prior art by providing an adaptive laser mapping method that integrates historical pose and point cloud characteristics, in order to solve the problems of inaccurate initial pose prediction in complex motion scenarios and insufficient registration robustness in feature degradation environments.

[0007] To achieve the above objectives, the technical solution adopted by the present invention is as follows: An adaptive laser mapping method that integrates historical pose and point cloud characteristics includes the following steps: S1. Read the point cloud data from the lidar sensor and preprocess it to obtain the source point cloud of the current frame; S2. The historical pose change trend is modeled as a Lie algebra vector field, and a dynamic correction guidance field is constructed by combining historical error information to obtain the predicted initial pose of the current frame. S3. Calculate the covariance matrix of each point in the source point cloud, and classify the point cloud into planar points and non-planar points based on the eigenvalue decomposition results of the covariance matrix. S4. Calculate the adaptive weights for different classification points: For non-planar points, calculate the point-to-point weights based on distance and covariance; for planar points, calculate the point-to-plane weights based on curvature and KL divergence. S5. Based on the predicted initial pose, the source point cloud is transformed to the map coordinate system. The point-to-point residual and the point-to-plane residual are calculated according to the point cloud classification. The objective function is constructed by combining the adaptive weights. The robot pose is obtained by iteratively solving the objective function, and the local map and historical pose sequence are updated.

[0008] Furthermore, step S2 includes the following sub-steps: S201. Obtain the historical pose sequence, model the historical pose changes as a vector field on Lie algebra, and calculate the average motion trend at the current moment based on the vector field. S202. Define the difference between the initial motion prediction and the final pose estimation result of each frame as the prediction deviation, and fit the deviation correction amount based on the prediction deviation. S203. Construct a dynamic correction guidance field based on the average motion trend and deviation correction amount, and convert the dynamic correction guidance field back to the pose space as the prediction initial pose of the current frame.

[0009] Furthermore, in S203, a dynamic correction guidance field is constructed based on the average motion trend and the deviation correction amount, which is expressed as: In the formula, To dynamically correct the guiding field. For the average trend of motion, Adaptive weights for bias correction; Let k be the deviation correction amount at time k. This represents the deviation correction amount at time k-1. For norm, is the scale factor.

[0010] Furthermore, step S3 includes the following sub-steps: S301. Form a point set by combining each point in the source point with multiple points of its nearest neighbors, and calculate the covariance of the point set. S302. Perform singular value decomposition on the covariance matrix to derive multiple eigenvalues; calculate the flatness and curvature of the nearest neighbor distribution based on the multiple eigenvalues, and classify the point cloud into planar points and non-planar points based on the flatness and curvature. Flatness is expressed as: Curvature is expressed as: In the formula, For flatness, For curvature, , , are eigenvalues, and .

[0011] Furthermore, in step S302, classifying the point cloud into planar points and non-planar points based on the flatness and curvature includes: If flatness Greater than the planar feature threshold and curvature Greater than the planar feature threshold If the condition is met, the extracted points are planar points; otherwise, the extracted points are non-planar points.

[0012] Furthermore, step S4 includes the following sub-steps: S401. For non-planar points, the point-to-point weights are calculated based on distance and covariance, and are expressed as follows: In the formula, For adaptive weights of point-to-point residuals, The distance between the point and the lidar sensor. This represents the maximum effective sampling distance of the lidar sensor. For covariance; S402, For plane points, based on curvature The KL divergence is used to calculate the point-to-plane weights, which are expressed as: In the formula, For adaptive weights of point-to-plane residuals, Let KL divergence be denoted as KL divergence.

[0013] Furthermore, in S402, the KL divergence Represented as: In the formula, The trace of the matrix, , These are the neighborhood distribution covariances of the source and target points, respectively. for The inverse matrix, for The inverse matrix; , These are the centroids of the neighborhood distribution of the source and target points, respectively.

[0014] Furthermore, step S5 includes the following sub-steps: S501. Calculate the point-to-point residual and the point-to-plane residual based on the point cloud classification: = In the formula, For point-to-point residuals, For point-to-plane residuals, For non-planar points in the source point cloud, For planar points in the source point cloud, In the target point cloud The nearest point, In the target point cloud The nearest point, The normal vector of the current plane point. and They represent the transformation parameters respectively. The rotation matrix and translation vector; S502, based on point-to-point residuals and point-to-plane residuals, and combined with adaptive weights for point-to-point residuals. Adaptive weights of point-to-plane residuals Construct the Hessian matrix H and the gradient vector b; S503. The pose optimization problem is transformed into a linear least squares problem, and its objective function equation is expressed as: In the formula, For pose increment; For the variable to be solved, It is the set of real numbers; Solve the objective function equation to obtain the pose increment. The objective function equation is solved repeatedly during the iterative process of the nearest point algorithm until convergence. The pose estimate is then calculated using the pose increment after convergence. In the formula, For attitude estimation, exp represents performing an exponential operation; Based on attitude estimation The latest attitude estimate is expressed as: = In the formula, Let k be the robot's global pose at time k. This represents the robot's global pose at time k-1. The relative posture is used for motion prediction.

[0015] Furthermore, in S502, the Hessian matrix H and the gradient vector b are expressed as: In the formula, To determine the number of point pairs for applying point-to-point residuals, The number of point pairs for applying the point-to-plane residual; , These are the indices of non-planar points and planar points, respectively; , They are respectively and Jacobian matrix with respect to transformation parameters; For point-to-point residuals, The point-to-plane residual; , for , The transpose of .

[0016] The adaptive laser mapping method that integrates historical pose and point cloud characteristics provided by this invention has the following beneficial effects: This invention addresses the problems of inaccurate initial pose estimation, poor error measurement, and poor adaptation to local geometry in existing LiDAR odometry scenarios with complex motion and geometric degradation, and proposes a method accordingly. This invention constructs a dynamically corrected guidance field, using historical pose sequences and error statistics to predict and correct the initial pose of the current frame, effectively reducing initial registration bias and preventing the iterative closest point algorithm from getting trapped in local optima.

[0017] This invention proposes an adaptive weighting mechanism based on local geometric distribution. It classifies point clouds into planar features and point features, and uses point-to-planar and point-to-point error metrics respectively. The contribution weight of each point in the optimization is dynamically assigned based on its local geometric characteristics, enabling the registration process to adapt to different environmental structures. By fusing historical motion trends and real-time deviation correction, combined with a geometrically adaptive registration strategy, the pose estimation accuracy and system robustness are significantly improved in complex motion patterns and feature degradation scenarios, providing an effective solution for reliable navigation of autonomous robots in GPS-denied environments. Attached Figure Description

[0018] Figure 1 This is a flowchart of an adaptive laser mapping method that integrates historical pose and point cloud characteristics, as described in an embodiment of the present invention.

[0019] Figure 2 This is a real-world image of a long corridor environment in which an unmanned ground vehicle equipped with a lidar sensor is located, as described in an embodiment of the present invention.

[0020] Figure 3 This embodiment of the invention illustrates the maintenance of the local point cloud map during algorithm execution.

[0021] Figure 4 This is an example of feature classification in a frame of point cloud scanned by a lidar in an embodiment of the present invention.

[0022] Figure 5 This is an embodiment of the invention showing the allocation of point-to-point optimization contribution weights.

[0023] Figure 6 This embodiment of the invention illustrates the allocation of contribution weights for point-to-plane optimization.

[0024] Figure 7 This invention provides a comparison of the point cloud map construction results of the proposed algorithm with those of other advanced algorithms in a long corridor scenario.

[0025] Figure 8 This is the trajectory positioning result on the collected data at a distance of 6.5 km in this embodiment of the invention.

[0026] Figure 9 This is the trajectory positioning result on the collected data at a distance of 3.2 km in this embodiment of the invention. Detailed Implementation

[0027] The specific embodiments of the present invention are described below to enable those skilled in the art to understand the present invention. However, it should be understood that the present invention is not limited to the scope of the specific embodiments. For those skilled in the art, various changes are obvious as long as they are within the spirit and scope of the present invention as defined and determined by the appended claims. All inventions utilizing the concept of the present invention are protected.

[0028] This embodiment of the adaptive laser mapping method, which integrates historical pose and point cloud characteristics, first downsamples the input point cloud and classifies it into planar distribution points and point distribution points based on their local geometric characteristics. Then, it analyzes historical pose data to obtain motion trends, calculates prediction deviations using historical error information, and constructs a dynamically corrected guidance field to generate a reliable initial pose for registration, reducing the initial error between the current point cloud and the historical local map. In the point cloud registration stage, for planar distribution points, a planar consistency weight is calculated and a point-to-planar error metric is used; for point distribution points, a point association weight is calculated and a point-to-point error metric is used. An adaptive weighting mechanism achieves accurate alignment between the source and target point clouds, reducing the impact of degradation features. Finally, based on the optimization results, the odometry pose and local map are updated, and historical pose sequences are added to provide rich prior information for the next frame.

[0029] The ICP (Iterative Closest Point) algorithm is the core of LiDAR odometry, and providing an accurate initial pose is crucial for ICP. Therefore, this embodiment uses the robot's global pose at time k. The estimation problem of ∈SE(3) is defined as the point cloud registration problem and the initial pose prediction problem, and its core expression is as follows: = in, It is used as the relative pose for motion prediction, and the pose estimation result of inter-frame registration is... Using rotation matrix ∈SO(3) and translation vector ∈ express; Let be the global pose of the robot at time k-1; SE(3) is a special Euclidean group, and SO(3) is a special orthogonal group.

[0030] Specifically, firstly, the point cloud in the radar coordinate system Based on the pose of the previous moment (The robot's global pose at time k-1) and the relative pose used for motion prediction Convert to map coordinate system , Source cloud was established and target point cloud The alignment results are obtained. In point cloud registration, the pose transformations of the source and target point clouds are iteratively solved by minimizing the cost function. The cost function is the residual between the source and target point clouds. Currently, many advanced LiDAR SLAM systems apply point-to-point or point-to-plane residual metrics. This embodiment adaptively combines two types of residuals to participate in ICP optimization, and the minimization problem is defined as: in, For adaptive weights of point-to-point residuals, Adaptive weights for point-to-plane residuals; To determine the number of point pairs for applying point-to-point residuals, The number of point pairs for applying the point-to-plane residual; , These are the optimal rotation matrix and optimal translation vector obtained through optimization. and They represent the transformation parameters respectively. The rotation matrix and translation vector; , These are the indices of non-planar points and planar points, respectively. For point-to-point residuals, The point-to-plane residual; norm Based on this, refer to Figure 1 This embodiment specifically includes the following: S1. Read the point cloud data from the lidar sensor and preprocess it to obtain the raw ranging data. Then, downsample the point cloud data to obtain the source point cloud of the current frame. S2. The obtained historical pose change trend is modeled as a Lie algebra vector field, and a dynamically corrected guiding field is constructed by combining historical error information to obtain the predicted initial pose of the current frame. This specifically includes the following sub-steps: S201. Obtain the historical pose sequence, model the historical pose changes as a vector field on a Lie algebra, and define the motion trend as follows: In the formula, For the trend of sports, This is the pose estimation result for the previous frame. This is the final pose estimation result; In the tangent space, it is represented as: In the formula, For the representation of the motion trend in the tangent space, log represents performing a logarithmic operation; The relative motion trend of the most recent m frames is selected, and then the average motion trend is calculated, which is expressed as: In the formula, For the average trend of motion, The currently selected motion frame; S202, Predict the initial motion for each frame. Compared with the final pose estimation results The difference between them is defined as the prediction bias. It is represented as: The deviation correction amount is obtained by fitting the prediction deviation across multiple frames: In the formula, Let k be the deviation correction amount at time k. As a smoothing factor, it determines the degree of dependence on recent forecast bias. This is the deviation correction amount at time k-1; S203. Construct a dynamic correction guidance field based on the average motion trend and deviation correction amount, and convert the dynamic correction guidance field back to the pose space as the prediction initial pose of the current frame. Among them, a dynamic correction guidance field is constructed based on the average motion trend and the deviation correction amount, which is expressed as: In the formula, To dynamically correct the guiding field. Adaptive weights for bias correction; For norm, As a scale factor, when the motion changes drastically, through The value is increased to increase the correction strength.

[0031] S3. Calculate the covariance matrix of each point in the source point cloud. Based on the eigenvalue decomposition results of the covariance matrix, classify the point cloud into planar points and non-planar points. This includes the following sub-steps: S301, transfer each point in the source point Form a point set with its m nearest neighbors, and calculate the covariance of this point set: In the formula, Let the covariance of the point set be... For point The k-th nearest neighbor, It is a point The mean of and its neighboring points; S302. Perform singular value decomposition on the covariance matrix to derive multiple eigenvalues; calculate the flatness and curvature of the nearest neighbor distribution based on the multiple eigenvalues, and classify the point cloud into planar points and non-planar points based on the flatness and curvature. Flatness is expressed as: Curvature is expressed as: In the formula, For flatness, For curvature, , , are eigenvalues, and .

[0032] Define the criteria for evaluating flatness: In the formula, This is a planarity discriminant function used to determine whether the local geometric properties of a point are planar. If flatness Greater than the planar feature threshold and curvature Greater than the planar feature threshold If the condition is met, the extracted points are planar points; otherwise, the extracted points are non-planar points.

[0033] S4. Calculate the adaptive weights for different classification points: For non-planar points, calculate the point-to-point weights based on distance and covariance; for planar points, calculate the point-to-plane weights based on curvature and KL divergence, which includes the following sub-steps: S401. For non-planar points, the point-to-point weights are calculated based on distance and covariance, and are expressed as follows: In the formula, For adaptive weights of point-to-point residuals, The distance between the point and the lidar sensor is the coordinate measurement uncertainty. The farther the point is, the higher the uncertainty of its coordinate measurement. This represents the maximum effective sampling distance of the lidar sensor. Covariance reflects the sparsity of a point's neighborhood; the more dispersed the distribution, the smaller the weight. S402, For plane points, based on curvature The KL divergence is used to calculate the point-to-plane weights, which are expressed as: In the formula, For adaptive weights of point-to-plane residuals, Let KL divergence be denoted as KL divergence.

[0034] KL divergence Represented as: In the formula, The trace of the matrix, , These are the neighborhood distribution covariances of the source and target points, respectively. for The inverse matrix, for The inverse matrix; , These are the centroids of the neighborhood distributions of the source and target points, respectively. The smaller the curvature, the closer the local manifold is to a Euclidean subspace, and the more reliable the estimate of its normal vector, the higher its weight will be assigned. It captures the inconsistencies between the source and target manifolds during local alignment. The smaller the value, the more similar the distributions of the two local manifolds are, and the more reliable the match of the point pair.

[0035] S5. Based on the predicted initial pose, transform the source point cloud to the map coordinate system, calculate the point-to-point residual and the point-to-plane residual according to the point cloud classification, construct an objective function in combination with the adaptive weights, obtain the robot pose by iteratively solving the objective function, and update the local map and historical pose sequence; specifically including the following sub-steps: S501. Calculate the point-to-point residual and the point-to-plane residual based on the point cloud classification: = In the formula, For point-to-point residuals, For point-to-plane residuals, For non-planar points in the source point cloud, For planar points in the source point cloud, In the target point cloud The nearest point, In the target point cloud The nearest point, The normal vector of the current plane point. and They represent the transformation parameters respectively. The rotation matrix and translation vector; S502, based on point-to-point residuals and point-to-plane residuals, and combined with adaptive weights for point-to-point residuals. Adaptive weights of point-to-plane residuals Construct the Hessian matrix H and the gradient vector b; Wherein, the Hessian matrix H and the gradient vector b are expressed as: In the formula, To determine the number of point pairs for applying point-to-point residuals, The number of point pairs for applying the point-to-plane residual; , These are the indices of non-planar points and planar points, respectively; , They are respectively and Jacobian matrix with respect to transformation parameters; For point-to-point residuals, The point-to-plane residual; , for , transpose; S503. The pose optimization problem is transformed into a linear least squares problem, and its objective function equation is expressed as: In the formula, For pose increment; For the variable to be solved, It is the set of real numbers; Solve the objective function equation to obtain the pose increment. The objective function equation is solved repeatedly during the iterative process of the nearest point algorithm until convergence. The pose estimate is then calculated using the pose increment after convergence. In the formula, For attitude estimation, exp represents performing an exponential operation; Based on attitude estimation The latest attitude estimate is expressed as: = In this example, such as Figure 2 As shown, the algorithm is tested in a long corridor environment with feature degradation, using an unmanned ground vehicle as the carrier of the lidar sensor. Red represents the X-axis, green represents the Y-axis, and blue represents the Z-axis.

[0036] In this example, Figure 3 The algorithm maintains the local map, which is used for point-to-map matching. Green points are planar points, blue points are non-planar points, and the local point cloud map is represented by gray points.

[0037] In this example, such as Figure 4 As shown, by analyzing the feature values ​​of the point cloud, the point cloud is divided into planar points and non-planar points, where planar points are green points and non-planar points are blue points.

[0038] In this example, such as Figure 5 As shown, for non-planar points, the point-to-point optimization contribution weight is determined by the variance of the source point and the distance of the point relative to the sensor. The point-to-point optimization contribution weight value is visualized in blue, and the brighter the blue color, the greater the contribution weight.

[0039] In this example, such as Figure 6 As shown, for a plane point, the optimization contribution weight from the point to the plane is determined by the curvature of the source point itself and the simplified KL divergence between the source point and the target point. The optimization weight from the point to the plane is visualized in green, and the brighter the green color, the greater the contribution weight.

[0040] In this example, such as Figure 7 As shown, the long corridor environment is a typical structural degradation scenario. The binding constraints along the corridor direction are weak, and the laser SLAM algorithm is prone to matching and cumulative drift. In the diagram, the blue point cloud is the real reference point cloud, and the red point cloud is the point cloud map result constructed by the algorithm. The areas where the algorithm has obvious degradation are covered by gray, and the key comparison areas are magnified for display. The algorithm proposed in this invention adjusts the weights by analyzing the local distribution characteristics of the points, thereby achieving higher accuracy. It models and corrects motion prediction from historical trajectories, suppresses ghosting in the point cloud map, and obtains clearer geometric boundaries.

[0041] In this example, a mobile robot equipped with LiDAR collected data over a distance of 6.5 km. The actual trajectory was obtained using RTK (Real-Time Kinematics). Figure 8 As shown, CT-ICP using the continuous-time model method struggles to accurately model large variations in angular velocity and is difficult to self-correct after accumulating errors. The algorithm proposed in this invention most closely approximates the actual trajectory.

[0042] In this example, a mobile robot equipped with a LiDAR sensor collected data over a distance of 3.2 km. The actual trajectory was acquired using RTK (Real-Time Kinematics). Figure 9 As shown, the DLO algorithm performs registration directly on the point cloud, resulting in insufficient registration constraints. FLOAM employs a simplified feature extraction and fast registration strategy, sacrificing robustness. Furthermore, when the uniform velocity assumption fails, the initial registration value deviation is large, making it prone to getting trapped in erroneous local optima, leading to complete trajectory drift. Example results demonstrate that the algorithm proposed in this invention exhibits high reliability in long-term operating scenarios.

[0043] Although specific embodiments of the invention have been described in detail with reference to the accompanying drawings, this should not be construed as limiting the scope of protection of this patent. Various modifications and variations that can be made by a person skilled in the art without inventive effort within the scope described in the claims still fall within the scope of protection of this patent.

Claims

1. An adaptive laser mapping method that integrates historical pose and point cloud characteristics, characterized in that, Includes the following steps: S1. Read the point cloud data from the lidar sensor and preprocess it to obtain the source point cloud of the current frame; S2. The historical pose change trend is modeled as a Lie algebra vector field, and a dynamic correction guidance field is constructed by combining historical error information to obtain the predicted initial pose of the current frame. S3. Calculate the covariance matrix of each point in the source point cloud, and classify the point cloud into planar points and non-planar points based on the eigenvalue decomposition results of the covariance matrix. S4. Calculate the adaptive weights for different classification points: For non-planar points, calculate the point-to-point weights based on distance and covariance. For plane points, calculate the point-to-plane weights based on curvature and KL divergence; S5. Based on the predicted initial pose, the source point cloud is transformed to the map coordinate system. The point-to-point residual and the point-to-plane residual are calculated according to the point cloud classification. The objective function is constructed by combining the adaptive weights. The robot pose is obtained by iteratively solving the objective function, and the local map and historical pose sequence are updated.

2. The adaptive laser mapping method for fusing historical pose and point cloud characteristics according to claim 1, characterized in that, S2 includes the following sub-steps: S201. Obtain the historical pose sequence, model the historical pose changes as a vector field on Lie algebra, and calculate the average motion trend at the current moment based on the vector field. S202. Define the difference between the initial motion prediction and the final pose estimation result of each frame as the prediction deviation, and fit the deviation correction amount based on the prediction deviation. S203. Construct a dynamic correction guidance field based on the average motion trend and deviation correction amount, and convert the dynamic correction guidance field back to the pose space as the prediction initial pose of the current frame.

3. The adaptive laser mapping method for fusing historical pose and point cloud characteristics according to claim 2, characterized in that, In step S203, a dynamic correction guidance field is constructed based on the average motion trend and the deviation correction amount, which is expressed as follows: In the formula, To dynamically correct the guiding field. For the average trend of motion, Adaptive weights for bias correction; Let k be the deviation correction amount at time k. This is the deviation correction amount at time k-1. For norm, is the scale factor.

4. The adaptive laser mapping method for fusing historical pose and point cloud characteristics according to claim 1, characterized in that, S3 includes the following sub-steps: S301. Form a point set by combining each point in the source point with multiple points of its nearest neighbors, and calculate the covariance of the point set. S302. Perform singular value decomposition on the covariance matrix to derive multiple eigenvalues; The planarity and curvature of the nearest neighbor distribution are calculated based on multiple feature values, and the point cloud is classified into planar points and non-planar points based on the planarity and curvature. Flatness is expressed as: Curvature is expressed as: In the formula, For flatness, For curvature, , , are eigenvalues, and .

5. The adaptive laser mapping method for fusing historical pose and point cloud characteristics according to claim 4, characterized in that, In step S302, the point cloud is classified into planar points and non-planar points based on the flatness and curvature, including: If flatness Greater than the planar feature threshold and curvature Greater than the planar feature threshold If the condition is met, the extracted points are planar points; otherwise, the extracted points are non-planar points.

6. The adaptive laser mapping method for fusing historical pose and point cloud characteristics according to claim 1, characterized in that, S4 includes the following sub-steps: S401. For non-planar points, the point-to-point weights are calculated based on distance and covariance, and are expressed as follows: In the formula, For adaptive weights of point-to-point residuals, The distance between the point and the lidar sensor. This represents the maximum effective sampling distance of the lidar sensor. For covariance; S402, For plane points, based on curvature The KL divergence is used to calculate the point-to-plane weights, which are expressed as: In the formula, For adaptive weights of point-to-plane residuals, Let KL divergence be denoted as KL divergence.

7. The adaptive laser mapping method for fusing historical pose and point cloud characteristics according to claim 6, characterized in that, In S402, the KL divergence Represented as: In the formula, The trace of the matrix, , These are the neighborhood distribution covariances of the source and target points, respectively. for The inverse matrix, for The inverse matrix; , These are the centroids of the neighborhood distribution of the source and target points, respectively.

8. The adaptive laser mapping method for fusing historical pose and point cloud characteristics according to claim 1, characterized in that, S5 includes the following sub-steps: S501. Calculate the point-to-point residual and the point-to-plane residual based on the point cloud classification: = In the formula, For point-to-point residuals, For point-to-plane residuals, For non-planar points in the source point cloud, For planar points in the source point cloud, In the target point cloud The nearest point, In the target point cloud The nearest point, The normal vector of the current plane point. and They represent the transformation parameters respectively. The rotation matrix and translation vector; S502, based on point-to-point residuals and point-to-plane residuals, and combined with adaptive weights for point-to-point residuals. Adaptive weights of point-to-plane residuals Construct the Hessian matrix H and the gradient vector b; S503. The pose optimization problem is transformed into a linear least squares problem, and its objective function equation is expressed as: In the formula, For pose increment; For the variable to be solved, It is the set of real numbers; Solve the objective function equation to obtain the pose increment. The objective function equation is solved repeatedly during the iterative process of the nearest point algorithm until convergence. The pose estimate is then calculated using the pose increment after convergence. In the formula, For attitude estimation, exp represents performing an exponential operation; Based on attitude estimation The latest attitude estimate is expressed as: = In the formula, Let k be the robot's global pose at time k. This represents the robot's global pose at time k-1. The relative posture is used for motion prediction.

9. The adaptive laser mapping method for fusing historical pose and point cloud characteristics according to claim 8, characterized in that, In S502, the Hessian matrix H and the gradient vector b are represented as follows: In the formula, To determine the number of point pairs for applying point-to-point residuals, The number of point pairs for applying the point-to-plane residual; , These are the indices of non-planar points and planar points, respectively; , They are respectively and Jacobian matrix with respect to transformation parameters; For point-to-point residuals, The point-to-plane residual; , for , The transpose of .

Images