CN118067102A - Laser radar static point cloud map construction method and system based on viewpoint visibility - Google Patents

Abstract

The invention provides a laser radar static point cloud map construction method and system based on viewpoint visibility. The laser radar static point cloud map construction method based on the viewpoint visibility comprises the following steps: collecting laser radar point cloud data and IMU information, and preprocessing; performing point cloud registration on multi-frame point cloud scanning near the currently scanned laser radar point cloud to generate a local point cloud sub-map; removing the currently scanned laser radar point cloud and the ground point cloud in the local point cloud map to obtain a non-ground point cloud; performing spherical projection on the non-ground point cloud to generate a corresponding distance image; differentiating the distance images, and screening dynamic points in each local point cloud sub map; and splicing all the local point cloud sub-maps in the scene based on the dynamic points in all the local point cloud sub-maps to generate a static map of the scene. The method is optimized based on the defects of the visibility method, and has better applicability in high-dynamic traffic scenes.

Description

Laser radar static point cloud map construction method and system based on viewpoint visibility

Technical Field

The present invention relates to the technical field of simultaneous positioning and mapping based on laser radar, and in particular to a laser radar static point cloud map construction method based on viewpoint visibility.

Background technique

As one of the key technologies in the field of autonomous driving and intelligent transportation, the accuracy, real-time performance and robustness of mapping algorithms in dynamic traffic scenes are still in urgent need of further optimization. The sensors relied on for high-precision map construction include LiDAR, industrial cameras, GNSS, IMU, etc. Among them, LiDAR can provide massive point cloud data for point cloud map construction. The existing LiDAR-based simultaneous positioning and mapping algorithms are based on the assumption of a static environment. However, there are a large number of moving cars, pedestrians and cyclists in the traffic environment where autonomous vehicles are driving. During the map construction process, the above dynamic objects will affect the accuracy of the mapping algorithm and the quality of the constructed map.

First, dynamic point clouds will affect the registration effect of the lidar odometer. When the number of dynamic point clouds in the scene is too large, the point cloud registration may even fail. Secondly, dynamic objects leave traces of movement in the final constructed point cloud map, changing the spatial structure of the point cloud map, and these traces will be treated as obstacles in the map, resulting in reduced map accuracy and positioning accuracy, and reducing the efficiency of downstream tasks of autonomous driving. At the same time, due to the continuous development of autonomous driving related technologies in recent years, high-dynamic scenes have become the main application scenarios for autonomous driving vehicles. Real-time and accurate construction of maps of static structures in scenes is of great significance for achieving real-time positioning and downstream path planning tasks for high-level autonomous driving.

Summary of the invention

In view of this, the present invention provides a laser radar static point cloud map construction method based on viewpoint visibility to solve the above problems.

According to a first aspect of the present invention, a method for constructing a laser radar static point cloud map based on viewpoint visibility is provided, comprising: collecting laser radar point cloud data and IMU information, and performing preprocessing; performing point cloud registration on multiple frame point cloud scans near a currently scanned laser radar point cloud, and generating a local point cloud submap; removing the ground point cloud in the currently scanned laser radar point cloud and the local point cloud submap to obtain a non-ground point cloud; performing spherical projection on the non-ground point cloud to generate a corresponding distance image; performing differentiation on the distance image to filter dynamic points in each local point cloud submap; and splicing each local point cloud submap in the scene based on the dynamic points in each local point cloud submap to generate a static map of the scene.

In another implementation of the present invention, preprocessing includes data analysis, IMU pre-integration, radar motion compensation, point cloud segmentation and feature extraction; the preprocessed data includes valid lidar point cloud data containing available features and the initial pose output by the IMU itself.

In another implementation of the present invention, point cloud registration is performed on multiple frame point cloud scans near the currently scanned laser radar point cloud to generate a local point cloud submap, including: selecting multiple frame point cloud scans near the currently scanned laser radar point cloud; performing point cloud registration by minimizing the distance between line features and surface features in different point cloud scans; and generating a local point cloud submap based on the registered point cloud scans.

In another implementation of the present invention, a spherical projection is performed on the non-ground point cloud to generate a corresponding distance image, and the pixel value of each point is calculated by the following formula:

in, Represents the distance from point p to the local coordinate system of the kth keyframe.

In another implementation of the present invention, the range image is differentiated to screen the dynamic points in each local point cloud submap, including: the pixel values of the local point cloud submap and the point cloud in the current scan are subtracted by their matrix elements:

The calculated pixel value Compared with the threshold τ, if it is greater than τ, the corresponding point is a dynamic point. The specific definition of the dynamic point is as follows:

τ＝γdist(p)

Among them, γ is the sensitivity relative to the point distance, and dist(·) is the distance value of the corresponding point.

In another implementation of the present invention, local point cloud submaps in the scene are spliced based on the dynamic points in each local point cloud submap to generate a static map of the scene, including: fusing the ground point cloud with the non-ground point cloud from which the dynamic point cloud is filtered out, generating the current scan and point cloud submaps, and outputting a static map of the scene after the point cloud submaps are aligned.

According to the second aspect of the present invention, there is provided a laser radar static point cloud map construction system based on viewpoint visibility, characterized in that it includes: a data preprocessing module: used to collect laser radar point cloud data and IMU information, and perform preprocessing. A feature extraction module: used to perform point cloud registration on multiple frame point cloud scans near the currently scanned laser radar point cloud to generate a local point cloud submap. A ground fitting module: used to remove the ground point cloud in the currently scanned laser radar point cloud and the local point cloud submap to obtain a non-ground point cloud. A spherical projection is performed on the non-ground point cloud to generate a corresponding distance image; the distance image is differentiated to filter the dynamic points in each local point cloud submap. A mapping module: used to splice the local point cloud submaps in the scene based on the dynamic points in each local point cloud submap to generate a static map of the scene.

The method for constructing a laser radar static point cloud map based on viewpoint visibility of the present invention inputs non-ground point clouds for dynamic point cloud removal, thereby solving the problem that the method based on viewpoint visibility misjudges ground point clouds as dynamic points; by screening out non-ground point clouds, and after the dynamic point removal module completes the processing of the non-ground point clouds, the ground points are restored, so that the complete ground features are retained in the finally constructed point cloud map, and the accuracy of dynamic point cloud screening is improved; the screened ground point clouds are not subjected to dynamic point cloud removal, which reduces the amount of data processed by the dynamic point cloud removal module and improves the real-time performance of dynamic point cloud removal; the mapping algorithm proposed by the present invention optimizes the defects of the visibility-based method and has better applicability in high-dynamic traffic scenes.

BRIEF DESCRIPTION OF THE DRAWINGS

In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following briefly introduces the drawings required for use in the embodiments or the prior art description. By reading the detailed description of the following implementation, the advantages and benefits of the solutions become clear to those skilled in the art. The drawings are only used to illustrate the preferred implementation and are not considered to be limitations of the present invention. In the drawings:

FIG1 is a schematic flow chart of the steps of a method for constructing a static point cloud map of a laser radar based on viewpoint visibility according to an embodiment of the present invention.

FIG. 2 is a schematic diagram of a workflow for ground point cloud screening according to an embodiment of the present invention.

FIG. 3 is a diagram showing the projection of a point cloud sphere onto a range image according to an embodiment of the present invention.

FIG. 4 is a point cloud map constructed by a mapping module according to an embodiment of the present invention when there is no dynamic point cloud removal module.

FIG5 is a point cloud map constructed after a dynamic point cloud removal module is added to a mapping module according to an embodiment of the present invention.

Detailed ways

In order to enable those skilled in the art to better understand the technical solutions in the embodiments of the present invention, the technical solutions in the embodiments of the present invention will be described clearly and in detail below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. All other embodiments obtained by ordinary technicians in the field based on the embodiments in the embodiments of the present invention should fall within the scope of protection of the embodiments of the present invention.

FIG1 is a flowchart of a method for constructing a static point cloud map using a laser radar based on viewpoint visibility according to an embodiment of the present invention. As shown in FIG1 , this embodiment mainly includes the following steps:

S101, collect laser radar point cloud data and IMU information, and perform preprocessing.

S102, performing point cloud registration on multiple frame point cloud scans near the currently scanned laser radar point cloud to generate a local point cloud submap.

S103: remove the currently scanned laser radar point cloud and the ground point cloud in the local point cloud submap to obtain a non-ground point cloud.

S104: Perform spherical projection on the non-ground point cloud to generate a corresponding distance image.

S105: Differentiate the distance image to filter dynamic points in each local point cloud submap.

S106: splicing the local point cloud submaps in the scene based on the dynamic points in the local point cloud submaps to generate a static map of the scene.

The method for constructing a laser radar static point cloud map based on viewpoint visibility of the present invention inputs non-ground point clouds for dynamic point cloud removal, thereby solving the problem that the method based on viewpoint visibility misjudges ground point clouds as dynamic points; by screening out non-ground point clouds, and after the dynamic point removal module completes the processing of the non-ground point clouds, the ground points are restored, so that the complete ground features are retained in the finally constructed point cloud map, and the accuracy of dynamic point cloud screening is improved; the screened ground point clouds are not subjected to dynamic point cloud removal, which reduces the amount of data processed by the dynamic point cloud removal module and improves the real-time performance of dynamic point cloud removal; the mapping algorithm proposed by the present invention optimizes the defects of the visibility-based method and has better applicability in high-dynamic traffic scenes.

In another implementation of the present invention, preprocessing includes data analysis, IMU pre-integration, radar motion compensation, point cloud segmentation and feature extraction; the preprocessed data includes valid lidar point cloud data containing available features and the initial pose output by the IMU itself.

Exemplarily, the data preprocessing module performs data analysis, IMU pre-integration, radar motion compensation, point cloud segmentation and feature extraction on the collected lidar point cloud data and inertial navigation data (IMU). The preprocessed data includes valid point cloud data containing available features and the initial pose output by the IMU itself.

In another implementation of the present invention, point cloud registration is performed on multiple frame point cloud scans near the currently scanned laser radar point cloud to generate a local point cloud submap, including: selecting multiple frame point cloud scans near the currently scanned laser radar point cloud; performing point cloud registration by minimizing the distance between line features and surface features in different point cloud scans; and generating a local point cloud submap based on the registered point cloud scans.

Exemplarily, the preprocessed lidar point cloud is input into a feature extraction module to extract features and register to generate a local point cloud submap.

Preferably, 5 frames of scanning near the current scanning of the laser radar are selected, and the line features and surface features of the current scanning are selected to be expressed by the following formula:

{ε _k-2 ,ε _k-1 ,ε _k ,ε _k+1 ,ε _k+2 }

{Η _k-2 Η _k-1 Ｈ _k Ｈ _k+1 Ｈ _k+2 }

Among them, ε _k represents the set of line features extracted from the k-th laser radar scan, and Ｈ _k represents the set of surface features extracted from the k-th laser radar scan.

Preferably, point cloud registration is performed by minimizing the distance between line features and surface features in different LiDAR scans. The distance between line features and surface features between different scans is expressed as follows:

Generate local point cloud submaps from registered LiDAR scans.

In another implementation of the present invention, as shown in FIG2 , the processed lidar point cloud is input into a ground fitting module to remove the ground point cloud and filter out the non-ground point cloud.

S1031, partition the input point cloud by polar coordinates, and the partition formula is as follows:

Among them, C represents the entire area, _Zm represents the mth area, and _Nz represents the number of areas. According to experience, _Nz is set to 4. _Zm is expressed as follows:

Z _m = {p _k ∈ p | L _{min, m} ≤ ρ _k ＜ L _{max, m} }

Among them, _Lmin,m and Lmax _,m respectively represent the minimum and maximum radial boundaries of _Zm ; then, _Zm is also divided into _Nr,m ×Nθ _,m bins, where each region has a different bin size.

S1032, the set of data points in each bin is named _Sn , and the total number of bins is _Nc. The specific formula is as follows:

Wherein, N _r,m represents the number of regions divided radially in the m region, and N _θ,m represents the number of regions divided circumferentially in the m region.

S1033, using the principal component analysis method, extract the third feature representing the height direction, select the point with the lowest height as the seed point, and let is the average value of the total number of seed points selected, N _seed , then the initial ground point cloud is estimated as follows:

Among them, z(·) returns the height value of the point, and z _seed represents the height threshold.

S1034, the ground point obtained by l iterations is The normal vector of the ground point at the lth iteration is/>

S1035, calculate the ground coefficient using the ground point normal vector of the previous iteration, the formula is as follows:

in, is the average value of all points classified as ground in the first iteration.

S1036, the second iteration formula of the ground point is as follows:

The kth data point is represented by _Md represents the distance threshold set by the ground of the point.

S1037, the likelihood of the ground is estimated by integrating the probabilities of verticality, height, and flatness. If the product is greater than 0.5, it is considered to be the ground. The formula for estimating the likelihood of the ground is as follows:

Where f(χ _n |θ _n ) is calculated by the following formula:

in, r _n , σ _n represent the average z value, the distance between the origin and the center of mass _{of Sn,} and the surface variable, respectively.

Furthermore, the steps for calculating verticality, height, and flatness are as follows:

S10371, the verticality of the plane is calculated by using the third-dimensional feature vector _v3 extracted by the principal component analysis method for each plane, and the formula is as follows:

Among them, z represents [0, 0, 1] and θ _τ represents the verticality threshold.

S10372, use the following formula to estimate the situation where the upper data point in the occluded space is considered to be the ground:

where κ(·) represents an adaptive midpoint function that grows exponentially according to r _n .

S10373, based on the previous height judgment result, use the following formula to calculate the flatness to reduce the misjudgment of the uphill plane:

In another implementation of the present invention, a spherical projection is performed on the non-ground point cloud to generate a corresponding distance image, and the pixel value of each point is calculated by the following formula:

in, Represents the distance from point p to the local coordinate system of the kth keyframe.

Exemplarily, as shown in FIG3 , the point cloud is converted into a range image by spherical projection, and the projection coordinates of each point are calculated by the following formula:

The current scan point cloud and the radar local map are projected into a sphere to generate the corresponding distance image. The pixel value of each point is calculated by the following formula:

in, Represents the distance from point p to the local coordinate system of the kth keyframe.

In another implementation of the present invention, the range image is differentiated to screen the dynamic points in each local point cloud submap, including: the pixel values of the local point cloud submap and the point cloud in the current scan are subtracted by their matrix elements:

The calculated pixel value Compared with the threshold τ, if it is greater than τ, the corresponding point is a dynamic point. The specific definition of the dynamic point is as follows:

τ＝γdist(p)

Among them, γ is the sensitivity relative to the point distance, and dist(·) is the distance value of the corresponding point.

In another implementation of the present invention, local point cloud submaps in the scene are spliced based on the dynamic points in each local point cloud submap to generate a static map of the scene, including: fusing the ground point cloud with the non-ground point cloud from which the dynamic point cloud is filtered out, generating the current scan and point cloud submaps, and outputting a static map of the scene after the point cloud submaps are aligned.

Exemplarily, the generated point cloud map is shown in Figures 4 and 5, wherein Figure 4 is a schematic diagram of a point cloud map constructed by a mapping module of an embodiment of the present invention when there is no dynamic point cloud removal module; Figure 5 is a schematic diagram of a point cloud map constructed by a mapping module of an embodiment of the present invention after adding a dynamic point cloud removal module.

According to a second aspect of the present invention, a laser radar static point cloud map construction system based on viewpoint visibility is provided, characterized in that it includes:

Data preprocessing module: used to collect lidar point cloud data and IMU information and perform preprocessing.

Feature extraction module: used to perform point cloud registration on multiple frame point cloud scans near the currently scanned lidar point cloud to generate a local point cloud submap.

Ground fitting module: used to remove the ground point cloud in the currently scanned lidar point cloud and the local point cloud submap to obtain the non-ground point cloud.

Perform spherical projection on the non-ground point cloud to generate the corresponding distance image; perform difference on the distance image to filter the dynamic points in each local point cloud submap.

Mapping module: used to stitch together the local point cloud submaps in the scene based on the dynamic points in each local point cloud submap to generate a static map of the scene.

Preferably, a dynamic point cloud removal module may be provided in the mapping module.

Thus far, specific embodiments of the present invention have been described. Other embodiments are within the scope of the appended claims. In some cases, the actions recited in the claims may be performed in a different order and still achieve the desired results. Additionally, the processes depicted in the accompanying drawings do not necessarily require the specific order or sequential order shown to achieve the desired results. In some embodiments, multitasking and parallel processing may be advantageous.

It should be noted that all directional indications in the embodiments of the present invention (such as up, down, left, right, back, etc.) are only used to explain the relative position relationship, movement status, etc. between the components under a certain specific posture (as shown in the accompanying drawings). If the specific posture changes, the directional indication will also change accordingly.

In the description of the present invention, the terms "first" and "second" are only used to facilitate the description of different components or names, and cannot be understood as indicating or implying a sequential relationship, relative importance, or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" and "second" may explicitly or implicitly include at least one of the features.

Unless otherwise defined, all technical and scientific terms used herein have the same meaning as those commonly understood by those skilled in the art of the present invention. The terms used in the specification of the present invention herein are only for the purpose of describing specific embodiments and are not intended to limit the present invention.

It should be noted that although the specific embodiments of the present invention are described in detail in conjunction with the accompanying drawings, it should not be understood as limiting the scope of protection of the present invention. Within the scope described in the claims, various modifications and variations that can be made by those skilled in the art without creative work still belong to the scope of protection of the present invention.

The examples of the embodiments of the present invention are intended to concisely illustrate the technical features of the embodiments of the present invention so that those skilled in the art can intuitively understand the technical features of the embodiments of the present invention, and are not intended to be improper limitations of the embodiments of the present invention.

Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims (7)

1. A laser radar static point cloud map construction method based on viewpoint visibility is characterized by comprising the following steps:

collecting laser radar point cloud data and IMU information, and preprocessing;

performing point cloud registration on multi-frame point cloud scanning near the currently scanned laser radar point cloud to generate a local point cloud sub-map;

removing the currently scanned laser radar point cloud and the ground point cloud in the local point cloud map to obtain a non-ground point cloud;

Performing spherical projection on the non-ground point cloud to generate a corresponding distance image;

Differentiating the distance images, and screening dynamic points in each local point cloud sub map;

and splicing all the local point cloud sub-maps in the scene based on the dynamic points in all the local point cloud sub-maps to generate a static map of the scene.

2. The method of claim 1, wherein the preprocessing comprises data parsing, IMU pre-integration, radar motion compensation, point cloud segmentation and feature extraction;

the preprocessed data includes valid lidar point cloud data including available features and its own initial pose output by the IMU.

3. The method of claim 1, wherein the performing point cloud registration of the multi-frame point cloud scan around the currently scanned laser radar point cloud to generate the local point cloud map comprises:

selecting multi-frame point cloud scanning near the currently scanned laser radar point cloud;

Performing point cloud registration by minimizing distances between line features and plane features in different point cloud scans;

and generating a local point cloud sub-map according to the registered point cloud scanning.

4. The method of claim 1, wherein the spherically projecting the non-ground point cloud generates a corresponding range image, and wherein the pixel value of each point is calculated by:

wherein, Representing the distance of point p to the local coordinate system of the kth keyframe.

5. The method of claim 4, wherein differentiating the distance image to screen dynamic points in each local point cloud map comprises:

the pixel values of the local point cloud sub-map and the point cloud in the current scan are subtracted by their matrix elements:

To calculate the pixel value Comparing with the threshold value tau, if the value tau is larger than the threshold value tau, the corresponding point is a dynamic point, and the specific dynamic point is defined as follows:

τ＝γdist(p)

Where γ is sensitivity to the point distance, and dist () is a distance value taking the corresponding point.

6. The method of claim 5, wherein the generating a static map of the scene based on stitching each partial point cloud sub-map of the scene with a dynamic point in each partial point cloud sub-map, comprises:

And fusing the ground point cloud with the non-ground point cloud with the dynamic point cloud filtered out, generating a current scanning and point cloud sub-map, and outputting a scene static map after the point cloud sub-map registration.

7. A laser radar static point cloud map construction system based on viewpoint visibility is characterized by comprising:

And a data preprocessing module: the method comprises the steps of collecting laser radar point cloud data and IMU information, and preprocessing;

And the feature extraction module is used for: the method comprises the steps of performing point cloud registration on multi-frame point cloud scanning near a currently scanned laser radar point cloud to generate a local point cloud map;

And a ground fitting module: the method comprises the steps of removing the currently scanned laser radar point cloud and the ground point cloud in the local point cloud map to obtain a non-ground point cloud; performing spherical projection on the non-ground point cloud to generate a corresponding distance image; differentiating the distance images, and screening dynamic points in each local point cloud sub map;

and (3) a drawing building module: and the method is used for splicing the local point cloud sub-maps in the scene based on the dynamic points in the local point cloud sub-maps to generate a static map of the scene.