Laser radar odometer and static map construction method and system

Abstract

The invention provides a laser radar odometer and static map construction method and system, and relates to the technical field of positioning and navigation.The method comprises the steps that semantic reasoning is used for obtaining point-by-point semantic tags for subsequent true dynamic point detection, and semantic constraints are provided for ICP registration; distortion removal is carried out based on motion prediction of a constant-speed motion model; performing dynamic point detection according to the shielding relation of the potential dynamic points; according to the method, multi-target tracking is carried out on a potential dynamic object to obtain state estimation, cross validation is carried out on the state estimation and a dynamic point detection result, instance-level dynamic objects based on priori pose estimation are accurately removed, unstable dynamic points are filtered out in the pre-registration stage, and the positioning accuracy and robustness of an odometer are improved. Semantic weights are introduced into ICP data association, pose estimation is obtained through robust optimization, and a static map is constructed. And high-precision and robust laser radar odometer and static map building is realized.

Description

Technical Field

[0001] The present invention relates to the technical field of positioning and navigation, and in particular, to a method and system for lidar odometry and static map construction. Background Art

[0002] Existing SLAM frameworks usually make static assumptions about the world model. Some early methods did not explicitly distinguish dynamic objects and only classified them into the categories of anomaly detection and outlier removal, using statistical methods to remove feature points that do not conform to the geometric model. Existing methods for constructing static maps by detecting and removing dynamic objects can be classified into three categories: ray-casting-based, visibility-based, and segmentation-based methods.

[0003] The ray-casting-based method assumes that the area through which the laser ray passes is free space, and the occupancy rate of the space is characterized by the voxel hit probability. Some methods use octrees to store the voxel occupancy probability in the form of logarithmic ratios, update the occupancy information of nodes by traversing voxels, and mark all non-empty voxels within the range of all laser ray crossings as dynamic. However, the probability update function of this method is too sensitive and is suitable for scenarios with sparse static objects. Moreover, such ray-casting-based methods highly depend on accurate positioning information. As the resolution increases, the computational complexity increases sharply, making it difficult to meet the real-time requirements.

[0004] The visibility-based method is similar to the ray-casting principle. It associates the query point and the map point within a narrow field of view. By comparing the differences between the query point and the local map, the area where the previous observation point is behind the current query point is marked as dynamic. Early visibility methods focused on iteratively updating the state of map points through multi-frame observations. Subsequently, related research integrated a dynamic point filtering mechanism based on adaptive multi-resolution distance images in the lidar-inertial SLAM framework. However, limited reference frames may lead to missed detection of dynamic points. Some solutions adopt a "delete first and then restore" strategy, strictly filtering dynamic points using high-resolution distance images and then backfilling static points through low-resolution images to reduce the false detection rate. The visibility-based method has a relatively small computational burden, but it cannot detect large-scale obstacles without reference points behind them, and is prone to misjudgment in scenarios with too large incident angles.

[0005] The ray-casting-based and visibility-based methods have some common defects: (1) Ground misjudgment problem: When the incident angle between the laser beam and the ground is large, the free space marker value will be updated when the light passes through the ground area, and it is easy to misjudge some ground points as dynamic; (2) Noise accumulation interference: During the multi-frame data fusion process, noise points below the ground will cause the previous area to be wrongly updated as free space, thus wrongly clearing the ground points that should have been retained; (3) Background missing residue: When there are no other objects behind the dynamic object, the algorithm cannot be verified by subsequent scans.

[0006] Early segmentation-based methods did not incorporate machine learning techniques. 2D bounding boxes were typically used to model the dynamic and geometric characteristics of the tracked vehicles, and Bayesian filters were used to estimate the state of each vehicle. Some segmentation methods assisted in differentiating moving objects through planar feature extraction and clustering, but their performance significantly degraded in high-dynamic environments. In recent years, deep neural networks have been used for three-dimensional object recognition and dynamic point regression prediction. Instead of directly detecting the actual motion of objects, some methods identify dynamic objects based on appearance features. Convolutional neural networks are combined to extract semantic information from lidar point clouds to construct a map with semantic labels. Appearance-based semantic segmentation of point cloud data can identify potential dynamic objects but cannot directly determine whether an object is in motion.

[0007] Therefore, how to achieve high-precision and robust positioning and mapping has become a technical problem to be solved. Summary of the Invention

[0008] The present invention aims to solve at least one of the technical problems existing in the prior art or related technologies, and discloses a lidar odometry and static map construction method and system. By fully utilizing the lidar point cloud semantic information provided by semantic reasoning and combining geometric detection rules based on dynamic point occlusion relationships, a semantic-assisted online lidar odometry and static map construction framework is constructed, which improves the positioning accuracy and robustness of the system in dynamic scenarios.

[0009] The first aspect of the present invention discloses a method for lidar odometry and static map construction, including: semantic segmentation: performing semantic segmentation on the point cloud data of the lidar to obtain the semantic labels of each point in the point cloud data, and determining potential dynamic points according to the semantic labels; distortion elimination: performing motion prediction on the robot based on a constant velocity motion model, and compensating for the motion of the sensor during the scanning process according to the point cloud timestamp to eliminate point cloud distortion; occlusion detection: converting the point cloud data with eliminated point cloud distortion into depth images frame by frame, and determining whether a point is a dynamic point according to the occlusion relationship between the current state and the previous state of the same potential dynamic point in different depth images to generate marked dynamic points; object detection: clustering the potential dynamic points to generate potential dynamic objects, and using a Kalman filter to perform multi-object tracking on the potential dynamic objects to obtain the state information of the potential dynamic objects; joint verification: determining that the potential dynamic objects are dynamic instances according to the state information of the potential dynamic objects, and filtering out the marked dynamic points and the dynamic points corresponding to the dynamic instances to reduce the interference of the dynamic point cloud on the lidar odometry and mapping accuracy; ICP data association: using semantic Euclidean distance and an adaptive threshold for ICP data association according to the semantic labels; point cloud registration and pose optimization: using an ICP iteration termination criterion based on the correction amount to converge the ICP algorithm, applying the pose correction calculated by the ICP algorithm to the intermediate point cloud, and adding the optimized point cloud to the voxel map to update the local map.

[0010] According to the lidar odometry and static map construction method disclosed in the above technical solution, preferably, the step of eliminating distortion specifically includes: Let be the original point in the k-th frame of lidar point cloud, and the timestamp of the original point relative to the start time of this frame is s i ∈[0,Δt], and the point after eliminating distortion The calculation process is as follows:

[0011]

[0012] where, exp(s i ω k ) is equivalent to performing spherical linear interpolation in the axis-angle space, v k is the sensor velocity predicted by the constant velocity motion model, and ω k is the sensor angular velocity predicted by the constant velocity motion model.

[0013] According to the lidar odometry and static map construction method disclosed in the above technical solution, preferably, the process of converting the point cloud data into depth images specifically includes:

[0014] To construct the depth image, the point W p in the global coordinate system needs to be converted to the lidar coordinate system: L p = R -1( W p - t), L p = L p x , L p y , L p z represents the coordinates of a point in the point cloud in the depth image coordinate system, and t is the position in the global coordinate system; for the point p in the lidar coordinate system, the global coordinates are obtained through calculation, where and represent the predicted values of the rotation and translation of the lidar pose in the k-th frame, and the predicted value of the lidar pose in the k-th frame is provided by the constant velocity motion model;

[0015] Calculate the spherical coordinates of the transformed point:

[0016]

[0017] where, θ, d are the azimuth angle, polar angle, and distance respectively;

[0018] Obtain the index of the point in the two-dimensional pixel plane: where, r h and r v are the horizontal and vertical resolutions of the depth image respectively, represents rounding down;

[0019] After determining the pixel position, save the spherical coordinates and semantic label of the point into the pixel.

[0020] According to the lidar odometry and static map construction method disclosed in the above technical solution, preferably, the process of determining the occlusion relationship between the current state and the previous state of the same potential dynamic point in different depth images specifically includes: The potential dynamic point in the current state is denoted as the current point, and the potential dynamic point in the previous state is denoted as the previous point. The method for detecting the occlusion relationship between the current point and the previous point based on the depth image of the previous point is as follows:

[0021] Project the current point onto the depth image of the previous point to obtain its pixel position and depth. If the depth of the current point is greater than the maximum depth saved by the corresponding pixel and its neighboring pixels, it is considered that the current point is occluded by the previous point in the depth image; if the depth of the current point is less than the minimum depth saved by the corresponding pixel and its neighboring pixels, it is considered that the current point is the occluding point of the previous point in the depth image; otherwise, it is considered that the occlusion relationship between the current point and the previous point in the depth image is not clear; The neighboring pixels are defined as the pixels whose distance from the current pixel is less than n h pixels in the horizontal direction and less than n v pixels in the vertical direction.

[0022] According to the lidar odometry and static map construction method disclosed in the above technical solution, preferably, the process of determining whether a potential dynamic point is a dynamic point specifically includes:

[0023] According to three occlusion rules, three independent tests are performed on the current point. If any test result is positive, the current point is marked as a dynamic point; the three occlusion rules are defined as follows: (1) For the detection of a target moving approximately perpendicular to the laser beam, project the current point p curr onto the depth images of the nearest N frames. If it occludes more than M1 frame background points, it is determined as a moving point; (2) For the detection of a target moving radially away from the laser beam, in this case, the moving object will be repeatedly occluded by its own points in the previous observations. Check whether the current point p curr is occluded by the points in the previous M2 frame depth images, and verify whether these points are occluded by the points in their subsequent frames. If all conditions are met, mark it as a moving point; (3) For the detection of a target moving radially closer to the laser beam, in this case, the moving object will repeatedly occlude its own points in the previous observations. Check whether the current point p curr continuously occludes the points in the previous M3 frame depth images, and further verify whether these points continuously occlude their subsequent points. If all conditions are met, mark it as a moving point.

[0024] According to the lidar odometry and static map construction method disclosed in the above technical solution, preferably, the steps of target detection specifically include:

[0025] According to the clustering result of the potential dynamic point P k,dyn , assuming that n k potential dynamic objects are detected, the motion probability Pr (k,i) of each potential dynamic object is:

[0026]

[0027] represents the set of dynamic points belonging to the i-th object detected based on the occlusion relationship;

[0028] Initialize the attributes of the tracking target, such as direction, speed, centroid, and bounding box size, according to the clustered point cloud. Use the Kalman filter as the probability inference model to predict and update the target state:

[0029] x k = F k x k-1 + u k + w k , z k = H k x k + v k ,

[0030] where x k represents the target state of the k-th frame, w k represents the process noise, v k represents the observation noise, F k and H k are the state transition matrix and the observation matrix respectively, u k is the input vector, z k is the observation value;

[0031] The target state x is represented by a six-dimensional vector, including the position and velocity characterized by the center of the bounding box: x = [x, y, z, v x , v y , v z ;

[0032] Assume that there are m tracking trajectories in the previous frame P k-1,dyn , and there are l candidate detections in the current frame P k-1,dyn , which are respectively defined as: O k represents the candidate detection;

[0033] Use the constant velocity model to predict the prior state estimate of each target, and then perform matching through the Hungarian algorithm to associate the candidate detections with the tracking targets.

[0034] According to the lidar odometry and static map construction method disclosed in the above technical solution, preferably, the steps of joint verification specifically include:

[0035] The instance dynamic probability is defined as:

[0036] where label represents the semantic category, Pr (k,i) is the motion probability;

[0037] When the calculated instance dynamic probability exceeds the preset probability value, the corresponding potential dynamic object is marked as a dynamic instance, or when the displacement of the potential dynamic object within the previous time window exceeds the preset displacement value, the corresponding potential dynamic object is marked as a dynamic instance.

[0038] According to the above technical solution, the estimation results of the dynamic point cloud P k,dyn and the static point cloud P k,sta for the current frame are obtained before point cloud registration, and the stable static point cloud P k,sta is used for odometry pose estimation:

[0039] Voxel-based point cloud downsampling is performed using a two-step downsampling strategy. The local map also takes the form of a voxel map as a representation, where the voxel size is set to v×v×v, and each voxel stores only a certain number of points. When processing each frame of lidar point cloud data, first, the point cloud P k,sta is initially downsampled using voxels with a side length of αv (α∈(0.0,1.0]), and only one point is retained in each voxel to generate an intermediate point cloud P′ k,sta . After the relative pose estimation is completed through ICP, this point cloud is used for map update. Since ICP requires lower-resolution data, the intermediate point cloud P′ k,sta is further downsampled: using a voxel size of βv (β∈(1.0,2.0]), and only one point is retained in each voxel to generate the final downsampled point cloud P″ k,sta ;

[0040] The source point set for registration is defined as:

[0041] S = {s i = T k-1 T pred,k p|p∈P″ k,sta},

[0042] where T k-1 is the pose estimation of the previous frame, and T pred,k is the predicted incremental pose. The data association of the point cloud depends on the nearest neighbor principle. Ideally, two associated points should share the same semantic label. However, in the actual running process, inaccurate pose prediction and imperfect semantic segmentation limit the effectiveness of semantic information in registration. In scenes with high noise, especially in places such as intersections, due to the existence of significant non-linear motion and dynamic objects, it is difficult to completely remove dynamic objects from the original point cloud, posing a severe challenge to data association. Considering these factors, the semantic Euclidean distance between the source point s and any point p s,n in its neighborhood is defined as: d se,n = η‖s - p s,n ‖2, where the coefficient is related to semantic information:

[0043]

[0044] where l(·) represents the semantic label, and l(·)=0 indicates that the annotation of this point is invalid. N s represents the number of points in the neighborhood with the same semantic label as s, and the constant μ (set to 0.1) controls the influence of the semantic information of the points in the neighborhood on the calculated semantic distance. The larger N s , the greater the probability of successful association of points with the same semantic label as s.

[0045] When performing associative search, a maximum distance threshold is usually set, which is equivalent to an outlier rejection mechanism. Matching points beyond this distance are considered outliers and ignored. The selection of the threshold τ depends on multiple factors, including the magnitude of the initial pose error, the number and type of dynamic objects in the scene, and sensor noise. This threshold is usually set based on experience. However, based on the aforementioned analysis of constant velocity motion prediction, by analyzing the degree to which the odometer deviates from the motion prediction over time, a reasonable upper limit can be estimated from the data. This deviation ΔT represents the local correction that ICP needs to apply relative to the predicted pose. Theoretically, the magnitude of the robot's acceleration affects the magnitude of ΔT. When the robot moves at a constant velocity, ΔT is usually small or even close to zero, indicating that the constant velocity motion assumption holds, and at this time, ICP does not require additional correction. Based on this, the ICP registration results successfully executed in the past can be integrated into the data associative search to estimate the possible displacement magnitude characterized by ΔT between corresponding points in adjacent frames in the presence of potential acceleration: δ(ΔT) = δ rot (ΔR) + δ trans (Δt), where δ rot (ΔR) represents the displacement that occurs at the maximum distance r max under the influence of the rotation ΔR. From the triangle inequality, the upper bound of the point displacement is: ‖ΔRp + Δt - p‖2 ≤ δ rot (ΔR) + δ trans (Δt). To calculate the threshold τ k for data association in the k-th frame, when the deviation exceeds the minimum distance δ min , that is, when it is considered that the robot's motion deviates from the constant velocity motion model, a Gaussian distribution is constructed based on the deviation δ(ΔT) in the robot's trajectory up to the current moment, and its standard deviation is calculated as follows: where the deviation set M k is defined as: M k = {i | i < k ∧ δ(ΔT i ) > δ min}, and the threshold τ k is calculated based on σ k = 3σ k . This threshold is used to reject outliers in the data associative search to ensure the reliability of the matching points.

[0046] In each ICP iteration, the correspondence between the point cloud S and the local map is obtained through the nearest neighbor search in the voxel map, and only point pairs with a distance between points less than the threshold τ are considered. To calculate the pose correction ΔT k in the j-th iteration, the pose is robustly optimized by minimizing the sum of the point-to-point residuals: est,j where C(τ k ​) is the set of nearest neighbor point pairs with a distance less than τ k , ρ is the Geman-McClure robust kernel function, and it is an M-estimator with strong outlier rejection characteristics.

[0047] Through the above process, the transformation T k = ΔT icp,k T k-1 T pred,k can be obtained, where ΔT icp,k = Π j ΔT est,j . Although the constant velocity motion model T pred,k is applied to the local coordinate system of lidar scans, the ICP correction ΔT icp,k is performed in the global coordinate system. Doing so can ensure that only the matched source point set S is transformed once in each ICP iteration, which helps improve efficiency. Therefore, the local pose deviation ΔT k of the k-th frame can be expressed as: ΔT k = (T k-1 T pred,k ) -1 ΔT icp,k T k-1 T pred,k . The present invention adopts an ICP iteration termination criterion based on the correction amount (stop iterating when the correction amount is less than the threshold γ), and does not set a maximum iteration limit. Finally, the pose correction calculated by ICP is applied to the intermediate point cloud P′ k,sta , and the optimized point cloud is added to the voxel map to update the local map.

[0048] The second aspect of the present invention also discloses a lidar odometer and static map construction system, including: a memory for storing program instructions; a processor for calling the program instructions stored in the memory to implement the lidar odometer and static map construction method provided by any of the above technical solutions.

[0049] The beneficial effects of the present invention at least include: Firstly, semantic inference is utilized to obtain per-point semantic tags for subsequent true dynamic point detection and to provide semantic constraints for ICP registration. Subsequently, in order to promote real-time performance, a motion point detection strategy based on occlusion relationships for potential dynamic points is proposed. In addition, multi-object tracking of potential dynamic objects is performed to obtain state estimates, which are cross-validated with the results of motion point detection, realizing precise removal of instance-level dynamic objects based on prior pose estimation, thereby filtering out unstable dynamic points in the pre-registration stage and improving the positioning accuracy and robustness of the odometer. During the registration and optimization process, a two-step downsampling strategy is adopted to downsample the point cloud based on voxels, obtaining point clouds with different resolutions for local map update and efficient registration respectively, and semantic weights are introduced in data association, and pose estimation is obtained through robust optimization and a static map is constructed. Through the present invention, robust and real-time positioning in a dynamic scene can be achieved, and a globally consistent static environment map can be constructed. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] Figure 1 FIG. shows a schematic flow chart of a lidar odometry and static map construction method according to an embodiment of the present invention.

[0051] Figure 2 FIG. shows a schematic block diagram of a lidar odometry and static map construction system according to an embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0052] In order to more clearly understand the above objects, features, and advantages of the present invention, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0053] In the following description, many specific details are set forth in order to fully understand the present invention. However, the present invention may also be implemented in other ways different from those described herein. Therefore, the present invention is not limited by the limitations of the specific embodiments disclosed below.

[0054] As Figure 1 shown, according to an embodiment of the present invention, a lidar odometry and static map construction method is disclosed, including:

[0055] Step 1: Perform semantic segmentation on lidar point cloud data;

[0056] Step 2: Predict the motion of the robot based on a constant velocity model, and compensate for the motion of the sensor during the scanning process according to the timestamps of the lidar points to eliminate point cloud distortion; specifically including: Let be the original point in the k-th frame of lidar point cloud, and its timestamp relative to the start time of this frame is s i ∈[0,Δt], the corrected point is calculated as follows: Among them is used to convert the axis-angle representation to a rotation matrix. exp(s i ω k ) is equivalent to performing spherical linear interpolation (SLERP) in the axis-angle space.

[0057] Step 3: Convert the historical frame into a series of depth images to achieve occlusion detection. Design three detection rules based on the occlusion relationship between the current point and the previous point in the depth image, perform three independent tests on the current potential dynamic points, and mark the moving points according to the test results. Specifically, let the prior pose provided by the constant velocity motion model for the k-th frame be To construct the depth image, the point W p in the global coordinate system needs to be transformed to the target lidar coordinate system: L p = R -1 ( W p - t), where L p = L p x , L p y , L p z represents the coordinates of the point in the target depth image coordinate system. For p ∈ P k L , its global coordinates are calculated through , where R and t correspond to the lidar pose (i.e., the previously estimated) of the target frame and Then calculate the spherical coordinates of the transformed point: Obtain the index of the point in the two-dimensional pixel plane: where r h and r v are the horizontal and vertical resolutions of the depth image respectively, denotes rounding down. After determining the pixel position, save the spherical coordinates, motion label and other information of the point into the pixel, and update the statistical information (number of points, maximum and minimum depth) of the pixel accordingly.

[0058] The moving point detection process focuses on potential dynamic points. Therefore, perform voxel downsampling on the stable point set P k,sta to obtain Then combine P k,dyn with to form the input point set P k of the depth image I k,det , that is

[0059] Based on the depth image representation of previous points, the occlusion relationship detection rules between the current point and the previous points in the depth image are as follows: Assume that the representation of the current point to be detected in the global coordinate system is W p, project it onto the depth image to obtain its pixel position and depth. If the depth of this point is greater than the maximum depth (threshold ε d ) saved by the current pixel and its neighboring pixels, then it is considered that this point is occluded by the points in the depth image; if the depth of this point is less than the minimum depth (threshold ε d ) saved by the current pixel and its neighboring pixels, then it is considered that this point is an occluding point in the depth image; otherwise, it is considered that the occlusion relationship between the current point and the points in the depth image is unclear. Neighboring pixels are defined as pixels whose distance from the current pixel is less than n h pixels in the horizontal direction and less than n v pixels in the vertical direction.

[0060] The moving point detection is based on three occlusion rules, and three independent tests are performed on the current point. If any test result is positive, it is marked as a moving point. The three occlusion rules are defined as follows: 1) Detection of targets moving approximately perpendicular to the laser beam. Project the current point p curr onto the depth images of the nearest N frames. If it occludes more than M1 frames (M1 ≤ N) of background points, it is determined as a moving point. 2) Detection of targets moving radially away from the laser beam. In this case, the moving object will be repeatedly occluded by its own points in the previous observations. Check whether p curr is occluded by the points in the depth images of the previous M2 frames (denoted as ), and verify whether these points are occluded by the points in their subsequent frames (i.e., for all i = 1,..., M2 - 1 and j = i + 1,..., M2, is occluded by ). If all conditions are met, it means that the current point p curr and the previous points are all on the object moving in the above way, so it is marked as a moving point. 3) Detection of targets moving radially closer to the laser beam. In this case, the moving object will repeatedly occlude its own points in the previous observations. Check whether p curr continuously occludes the points in the depth images of the previous M3 frames (denoted as ), and further verify whether these points continuously occlude their subsequent points (i.e., for all i = 1,..., M3 - 1 and j = i + 1,..., M3, occludes ). If all conditions are met, it means that the current point p curr and the previous points are all on the object moving in the above way, so it is marked as a moving point.

[0061] Step 4: Cluster the potential dynamic points, remove isolated dynamic points to improve the detection accuracy of moving points in the current frame, and use the Kalman filter to perform multi-object tracking on potential dynamic objects to obtain the state information of the objects; specifically including: Based on the clustering result of the potential dynamic points P in the k-th frame in Step 3 k,dyn assuming that n k objects are detected, the "motion probability" of each potential dynamic object can be expressed as: where i ∈ {1, 2,..., n k}, represents the set of moving points belonging to the i-th object detected based on the occlusion rule in Step 3. Initialize the attributes such as the direction, speed, centroid, and bounding box size of the tracking target according to the clustered point cloud. Use the Kalman filter as a probabilistic inference model to predict and update the target state: x k = F k x k-1 + u k + w k , z k = H k x k + v k , where the process noise w k ~ N(0, R), and the observation noise v k ~ N(0, Q). The target state is represented by a six-dimensional vector, including the position and speed characterized by the center of the bounding box: x = [x, y, z, v x , v y , v z . Assume that there are m tracking trajectories in the previous frame P k-1,dyn , and there are l candidate detections in the current frame P k-1,dyn , which are respectively defined as: The goal of the 3D multi-object tracking problem is to determine the posterior estimation sequence X0, X1,..., X k . Use the constant velocity model to predict the prior state estimation of each target, and then perform matching through the Hungarian algorithm to associate the candidate detections with the tracking targets.

[0062] Step 5: Jointly verify the geometric detection result based on the occlusion relationship and the speed result of multi-object tracking to achieve instance-level dynamic point filtering; specifically including:

[0063] The "dynamic probability" of an instance is defined as: where label represents the semantic category, and Pr (k,i) is the "motion probability" calculated in Step 4. When D (k,i),label > τ labelWhen this occurs, the instance is marked as a dynamic instance. Additionally, if the displacement of the object within the previous time window exceeds the threshold d th , it is also recognized as a dynamic instance. This strategy can effectively detect objects that have just stopped but may still affect mapping, such as a vehicle that suddenly brakes or a pedestrian who pauses briefly. Points belonging to true dynamic instances will be filtered out to reduce the interference of dynamic point clouds on lidar odometry and mapping accuracy, thereby improving the accuracy and robustness of subsequent pose estimation.

[0064] Step 6: Use semantic Euclidean distance and an adaptive threshold for ICP data association; specifically including:

[0065] After the processing of the above steps, the dynamic point cloud P k,dyn for the current frame and the estimated result of the static point cloud P k,sta are obtained before point cloud registration, and the stable static point cloud P k,sta is used for odometry pose estimation.

[0066] The present invention uses a two-step downsampling strategy for voxel-based point cloud downsampling. The local map also adopts the form of a voxel map as a representation, where the voxel size is set to v×v×v, and each voxel stores only a certain number of points. When processing each frame of lidar point cloud data, first, the point cloud P k,sta is initially downsampled using voxels with a side length of αv (α∈(0.0,1.0]), and only one point is retained in each voxel to generate an intermediate point cloud P′ k,sta . When the relative pose estimation is completed through ICP, this point cloud is used for map update. Since ICP requires data with a lower resolution, the intermediate point cloud P′ k,sta is further downsampled: using a voxel size of βv (β∈(1.0,2.0]), and only one point is retained in each voxel to generate the final downsampled point cloud P″ k,sta .

[0067] Define the source point set for registration as: S = {s i = T k-1 T pred,k p|p∈P″ k,sta}, where T k-1 is the pose estimation of the previous frame, and T pred,kis the predicted incremental pose. The data association of point clouds relies on the nearest neighbor principle. Ideally, two associated points should share the same semantic label. However, during actual operation, inaccurate pose prediction and imperfect semantic segmentation limit the effectiveness of semantic information in registration. In scenes with high noise, especially at intersections and other places, due to significant non-linear motion and dynamic objects, it is difficult to completely remove dynamic objects from the original point cloud, posing a severe challenge to data association. Considering these factors, the semantic Euclidean distance between the source point s and any point p within its neighborhood is defined as: s,n d se,n = η‖s - p s,n ‖², where the coefficient is related to semantic information:

[0068]

[0069] where l(·) represents the semantic label, l(·) = 0 indicates that the annotation of this point is invalid, N s represents the number of points within the neighborhood that have the same semantic label as s, and the constant μ (set to 0.1) controls the influence of the semantic information of points within the neighborhood on the calculated semantic distance. The larger N s , the greater the probability of successful association of points with the same semantic label as s.

[0070] When performing association search, a maximum distance threshold is usually set. This threshold is equivalent to an outlier rejection mechanism, and matching points beyond this distance will be regarded as outliers and ignored. The selection of the threshold τ depends on multiple factors, including the magnitude of the initial pose error, the number and type of dynamic objects in the scene, and sensor noise, etc. This threshold is usually set based on experience. However, based on the aforementioned analysis of constant velocity motion prediction, by analyzing the degree to which the odometer deviates from the motion prediction over time, a reasonable upper limit can be estimated from the data. This deviation ΔT represents the local correction that ICP needs to apply relative to the predicted pose. Theoretically, the magnitude of the robot's acceleration affects the magnitude of ΔT. When the robot moves at a constant speed, ΔT is usually small, even close to zero, indicating that the constant velocity motion assumption holds, and at this time ICP does not require additional correction. Based on this, the previously successfully executed ICP registration results can be integrated into the data association search to estimate the possible displacement magnitude characterized by ΔT between corresponding points in adjacent frames in the presence of potential acceleration: δ(ΔT) = δ rot (ΔR) + δ trans (Δt), where δ rot (ΔR) represents the displacement that occurs at the maximum distance r max under the influence of the rotation ΔR. According to the triangle inequality, the upper bound of the point displacement is: ‖ΔRp + Δt - p‖² ≤ δ rot (ΔR) + δ trans(Δt). To calculate the threshold τ associated with the data of the k-th frame k , when the deviation exceeds the minimum distance δ min , that is, when it is considered that the movement of the robot deviates from the constant velocity motion model, a Gaussian distribution is constructed based on the deviation δ(ΔT) in the trajectory of the robot up to the current moment, and its standard deviation is calculated as follows: where the deviation set M k is defined as: M k ={i|i < k ∧ δ(ΔT i ) > δ min}, and the threshold τ is calculated according to σ k : τ k = 3σ k . This threshold is used to eliminate outliers in the data association search to ensure the reliability of the matching points.

[0071] Step 7: Register the point cloud based on the ICP method and obtain the pose estimation through robust optimization. Specifically, it includes:

[0072] In each ICP iteration, the corresponding relationship between the point cloud S and the local map in the voxel map described in Step 6 is obtained through the nearest neighbor search, and only the point pairs with the distance between points less than the threshold τ are considered. To calculate the pose correction ΔT k in the j-th iteration, the pose is robustly optimized by minimizing the sum of the point-to-point residuals: est,j where C(τ ) is the set of nearest neighbor point pairs with the distance less than τ k , ρ is the Geman-McClure robust kernel function, and it is an M-estimator with strong outlier rejection characteristics. k Through the above process, the transformation T

[0073] can be obtained: T k = ΔT icp,k T k-1 T pred,k , where ΔT icp,k = Π j ΔT est,j . Although the constant velocity motion model T pred,k is applied to the local coordinate system of the lidar scan, the ICP correction ΔT icp,k is performed in the global coordinate system. This can make the matching source point set S be transformed only once in each ICP iteration, which helps to improve the efficiency. Therefore, the local pose deviation ΔT k of the k-th frame can be expressed as: ΔT k = (T k-1 T pred,k ) -1 ΔT icp,k Tk-1 T pred,k The present invention adopts an ICP iteration termination criterion based on the correction amount (stopping the iteration when the correction amount is less than the threshold γ), without setting a maximum iteration number limit. Finally, the pose correction calculated by ICP is applied to the intermediate point cloud P′ k,sta , and the optimized point cloud is added to the voxel map to update the local map.

[0074] As Figure 2 shown, according to another embodiment of the present invention, a lidar odometer and static map construction system 200 is also disclosed, including: a memory 201 for storing program instructions; a processor 202 for calling the program instructions stored in the memory to implement the lidar odometer and static map construction method as in the above embodiment.

[0075] The above are only the preferred embodiments of the present invention and are not intended to limit the present invention. For those skilled in the art, the present invention can have various changes and modifications. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A laser radar odometer and static map construction method, characterized in that: include: Semantic segmentation: semantically segment the point cloud data of the laser radar, obtain the semantic label of each point in the point cloud data, and determine the potential dynamic points according to the semantic label; Eliminate distortion: Predict the robot's motion based on a constant-speed motion model, and compensate for the sensor's motion during scanning based on the point cloud timestamp to eliminate point cloud distortion. Occlusion detection: Convert the point cloud data with point cloud distortion eliminated into depth images frame by frame, and determine whether the point is a dynamic point based on the occlusion relationship between the current state and the previous state of the same potential dynamic point in different depth images to generate marked dynamic points; Target detection: clustering potential dynamic points to generate potential dynamic objects, and using Kalman filter to perform multi-target tracking on the potential dynamic objects to obtain state information of the potential dynamic objects; Joint verification: determine that the potential dynamic object is a dynamic instance according to the state information of the potential dynamic object, and filter out the marked dynamic points and the dynamic points corresponding to the dynamic instance to reduce the interference of the dynamic point cloud on the laser radar odometer and mapping accuracy; ICP data association: Based on the semantic labels, ICP data association is performed using semantic Euclidean distance and adaptive threshold; Point cloud registration and pose optimization: The ICP iteration termination criterion based on the correction amount is used to make the ICP algorithm converge. The pose correction calculated by the ICP algorithm is applied to the intermediate point cloud, and the optimized point cloud is added to the voxel map to update the local map.

2. The laser radar odometer and static map construction method according to claim 1, characterized in that: The step of eliminating distortion specifically includes: set up is the original point in the kth frame of the laser radar point cloud. The timestamp of the original point relative to the start time of the frame is s i ∈[0,Δt], the point after distortion elimination The calculation is as follows: Among them, exp(s i ω k ) is equivalent to performing spherical linear interpolation in axis-angle space, v k is the sensor velocity predicted by the constant velocity motion model, ω k is the sensor angular velocity predicted by the constant velocity motion model.

3. The laser radar odometer and static map construction method according to claim 1, characterized in that: The process of converting point cloud data into depth images includes: To construct a depth image, the points in the global coordinate system need to be W p is transformed to the laser radar coordinate system: L p=R -1 ( W pt), L p=[ L p x , L p y , L p z ] represents the coordinates of the point in the point cloud in the depth image coordinate system, and t is the position in the global coordinate system; For a point p in the lidar coordinate system, the global coordinates are given by Calculate, where and represents the predicted values ​​of the rotation and translation of the LiDAR pose of the kth frame, and the predicted value of the LiDAR pose of the kth frame is provided by the constant speed motion model; Compute the spherical coordinates of the transformed point: in, θ and d are azimuth, polar angle and distance respectively; Get the index of a point in the 2D pixel plane: Among them, r h and r v are the horizontal and vertical resolutions of the depth image, Indicates rounding down; After determining the pixel location, the spherical coordinates of the point along with the semantic label are saved to the pixel.

4. The laser radar odometer and static map construction method according to claim 1, characterized in that: The process of determining the occlusion relationship between the current state and the previous state of the same potential dynamic point in different depth images specifically includes: The potential dynamic point of the current state is recorded as the current point, and the potential dynamic point of the previous state is recorded as the previous point. The occlusion relationship detection method between the current point and the previous point based on the depth image of the previous point is as follows: The current point is projected onto the depth image of the previous point to obtain its pixel position and depth. If the depth of the current point is greater than the maximum depth saved by the corresponding pixel and the neighboring pixels, the current point is considered to be occluded by the previous point in the depth image; if the depth of the current point is less than the minimum depth saved by the corresponding pixel and the neighboring pixels, the current point is considered to be an occluded point of the previous point in the depth image; otherwise, the occlusion relationship between the current point and the previous point in the depth image is considered to be unclear; the neighboring pixels are defined as the pixels whose horizontal distance from the current pixel is less than n. h pixels, less than n in the vertical direction v Pixels of pixels.

5. The laser radar odometer and static map construction method according to claim 4, characterized in that: The process of determining whether a potential dynamic point is a dynamic point specifically includes: According to the three occlusion rules, three independent tests are performed on the current point. If any test result is positive, the current point is marked as a dynamic point. The three occlusion rules are defined as follows: For target detection that moves approximately perpendicular to the laser beam, the current point p curr Projected to the most recent N frames of depth image, if its occlusion exceeds the background point of the M1 frame, it is judged as a moving point; for target detection moving away along the radial direction of the laser beam, in this case, the moving object will be repeatedly occluded by its own point in the previous observation, and check the current point p curr Whether it is blocked by the points in the previous M2 frame depth image, and verify whether these points are blocked by the points in its subsequent frame. If all conditions are met, it is marked as a moving point; for target detection approaching along the radial direction of the laser beam, in this case, the moving object will repeatedly block its own points in the previous observation, and check the current point p curr Whether the points in the previous M3 frame depth image are continuously occluded, and whether these points continue to occlude their subsequent points is further verified. If all conditions are met, it is marked as a moving point.

6. The laser radar odometer and static map construction method according to claim 1, characterized in that: The target detection step specifically includes: According to the potential dynamic point P k,dyn The clustering result, assuming that n k potential dynamic objects, the motion probability Pr of each potential dynamic object (k,i) for: in, represents the dynamic point set belonging to the i-th object detected based on the occlusion relationship; The attributes of the direction, speed, center of mass, and bounding box size of the tracked target are initialized according to the clustered point cloud, and the Kalman filter is used as the probabilistic inference model to predict and update the target state: x k =F k x k-1 +u k +w k ,z k =H k x k +v k , Among them, x k represents the target state of the kth frame, w k represents the process noise, v k represents the observation noise, F k With H k are the state transfer matrix and the observation matrix respectively, u k is the input vector, z k is the observed value; The target state x is represented by a six-dimensional vector, which contains the position and velocity represented by the center of the bounding box: x = [x, y, z, v x ,v y ,v z ]; Assume that the previous frame P k-1 ,d yn There are m tracking tracks in the current frame P k-1 ,d yn There are l candidate detections in , which are defined as: O k represents a candidate detection; A constant velocity model is used to predict a priori state estimates for each target, and matching is subsequently performed via the Hungarian algorithm to associate candidate detections with tracked targets.

7. The laser radar odometer and static map construction method according to claim 1, characterized in that: The steps of the joint verification specifically include: The instance dynamic probability is defined as: Among them, label represents the semantic category, Pr (k,i) is the probability of motion; When the calculated instance dynamic probability exceeds a preset probability value, the corresponding potential dynamic object is marked as a dynamic instance, or when the displacement of the potential dynamic object in the previous time window exceeds a preset displacement value, the corresponding potential dynamic object is marked as a dynamic instance.

8. A laser radar odometer and static map construction system, characterized in that: include: A memory for storing program instructions; A processor, configured to call the program instructions stored in the memory to implement the laser radar odometer and static map construction method as described in any one of claims 1 to 7.

Images