CN110645998A - Dynamic object-free map segmentation establishing method based on laser point cloud - Google Patents

Dynamic object-free map segmentation establishing method based on laser point cloud Download PDF

Info

Publication number
CN110645998A
CN110645998A CN201910851167.1A CN201910851167A CN110645998A CN 110645998 A CN110645998 A CN 110645998A CN 201910851167 A CN201910851167 A CN 201910851167A CN 110645998 A CN110645998 A CN 110645998A
Authority
CN
China
Prior art keywords
map
point cloud
sub
frame
submap
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201910851167.1A
Other languages
Chinese (zh)
Other versions
CN110645998B (en
Inventor
吴晓东
林挺
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shanghai Jiaotong University
Original Assignee
Shanghai Jiaotong University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shanghai Jiaotong University filed Critical Shanghai Jiaotong University
Priority to CN201910851167.1A priority Critical patent/CN110645998B/en
Publication of CN110645998A publication Critical patent/CN110645998A/en
Application granted granted Critical
Publication of CN110645998B publication Critical patent/CN110645998B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching
    • G01C21/32—Structuring or formatting of map data
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/4808—Evaluating distance, position or velocity data

Abstract

The invention discloses a dynamic object-free map segmentation establishing method based on laser point cloud, which relates to the field of maps and comprises the following steps: 1. collecting data by using a laser radar to obtain point cloud data; the point cloud data is multi-frame data; 2. playing the collected point cloud data by using an industrial personal computer, and creating a map without dynamic objects in an off-line segmentation manner; carrying out voxel grid downsampling on each frame of point cloud data and transmitting the frame to a map creation method; 3. initializing a map, and creating an initial map without dynamic objects; 4. creating and updating the sub-map, and removing the dynamic object; 5. carrying out loop detection on the map, and calculating the error of the map; 6. optimizing the error of the map; 7. and outputting the error-optimized map. According to the invention, through a series of optimization, the efficiency is prevented from being excessively reduced in the map building process, loop detection and loop error optimization are simultaneously carried out, and the problem that the motion track of a dynamic object appears on the map in the whole process is solved.

Description

Dynamic object-free map segmentation establishing method based on laser point cloud
Technical Field
The invention relates to the field of high-precision positioning maps, in particular to a dynamic object-free map segmentation establishing method based on laser point cloud.
Background
With the deep development of the unmanned technology, people have higher and higher requirements on the precision and stability of the autonomous positioning of the vehicle. The autonomous positioning of the vehicle is a precondition for autonomous navigation of the vehicle, autonomous navigation and movement of the vehicle are possible only when the vehicle can obtain an accurate position and posture of the vehicle in real time, and the accuracy are guaranteed by the safety of an unmanned vehicle.
The positioning technology based on the environmental feature map generally obtains the position of the vehicle on the map by matching the currently scanned laser point cloud or image with the established map. On the premise of higher map accuracy, the positioning technology can often realize centimeter-level positioning accuracy. Currently, maps of this type are mostly created by the slam (simultaneous Localization And mapping) method. The SLAM technology calculates a current position of a vehicle on a map by matching environmental information obtained by a sensor with map information, and at the same time, updates the map by loading the current environmental information onto the map based on the calculated vehicle position. Positioning alternates with map updating, which is also the source of the SLAM technology name. The SLAM technology is based on a sensor type, and a portion using a camera is referred to as a visual SLAM, and a portion using a laser radar is referred to as a laser SLAM. The laser SLAM has the advantages of the laser radar: the stability is high, the precision is high and the influence of illumination is avoided; at the same time, the following disadvantages are also present: the method is influenced by extreme weather, the cost is high, and pattern information cannot be obtained. The camera-based visual SLAM method includes: the cost is low, and the detected environmental information is rich, especially pattern texture information; there are also disadvantages of cameras: the influence of illumination is large, the calculation amount is large, and the calculation precision and the stability often cannot meet the automatic driving requirement. With each of these advantages and disadvantages, both SLAM technologies have evolved to some extent in recent years.
Laser SLAM technology can distribute two modules: matching the frame information of the front end and optimizing the back end. Front-end registration algorithms are mainly classified into two types: based on ICP type registration algorithm between points; and (4) registering algorithm based on potential field class, such as NDT registering. The ICP type registration algorithm mainly forms a matching point pair by searching a matching point between a target point cloud and an input point cloud, and reduces the distance between the matching point pair as much as possible by a certain method. The potential field type registration algorithm is that a potential field is generated in a target point cloud, then an input point cloud is projected in a target point cloud space based on a conversion matrix, each projection result obtains a score to represent the matching degree, and then the projection scores are optimized through a certain optimization algorithm, so that an optimal conversion matrix is obtained. Because the NDT method uses a probability density function generation based on statistics, the method has better performance on robustness and fault tolerance of a three-dimensional environment and a non-structural environment, and is more suitable for an environment outside an inner chamber of a campus.
Front-end matching acts as an odometer, but like other odometers, this displacement-based map update method has cumulative errors. Therefore, a loop back error inevitably occurs when the map is closed. Loop detection and loop error elimination are important tasks for back-end optimization.
The prior art has the following defects:
1. in the SLAM mapping process, registration operation needs to be performed on each frame of laser point cloud information, so that the current position of the vehicle is obtained. The complexity of the generation of the probability density function in the NDT registration algorithm is proportional to the number of points of the target point cloud (the complexity of other registration algorithms is also positively correlated with the number of points of the target point cloud), i.e., o (n), where n is the number of points of the target point cloud. As the map grows, the time consumed by NDT registration becomes larger. Therefore, when a map of a large environment is built, the map building efficiency is relatively low;
2. because the laser point cloud is a sparse point cloud structure, the realization of loop detection by the SLAM technology based on the laser radar is difficult, and thus loop errors cannot be eliminated. In an outdoor open environment, the elimination of the loop error is necessary;
3. in the scanning process of the laser radar, dynamic objects such as pedestrians and other vehicles can be scanned by the laser radar to form point clouds, and the point cloud information can be continuously put into a map in the map building process. Since the positions of the dynamic objects in different point cloud scanning frames are different, information similar to the motion tracks of the dynamic objects appears in the final map, and the information is unnecessary redundant information in the map. Such accumulation of erroneous information may affect the accuracy of the final map and the positioning accuracy of the following vehicles.
Accordingly, those skilled in the art have endeavored to develop a new map building method that overcomes the above-mentioned shortcomings of the prior art.
Disclosure of Invention
In view of the above-mentioned defects in the prior art, the technical problem to be solved by the present invention is how to use a single laser radar, apply the laser SLAM technology to construct an environment map that can realize high-precision (centimeter level) and high-frequency (10Hz) positioning of a vehicle, and how to prevent the map-constructing efficiency from excessively decreasing in the laser SLAM process and simultaneously perform loop detection and loop error elimination through a series of optimizations. Meanwhile, the technical problem to be solved by the invention also includes how to solve the problem that the motion trail of the dynamic object appears on the map due to the multiple appearance of the dynamic object at different positions in the process of establishing the map.
The specific explanation of the related terms is as follows:
point cloud: a data structure. A set of points describing the location information of the points. The addition of the point clouds refers to the union of the two point sets.
NDT (see reference: Biber P. the Normal Distributions Transform: A New Approach to Laser Scan Matching [ J ]. Proc. of IROS2003,2003.): a registration algorithm based on meshing, probability density function generation and maximum likelihood;
loop detection: the method is also called closed-loop detection, and refers to that a vehicle is identified to reach a certain scene in the whole map building process, so that the map is subjected to closed-loop detection work; the position error occurring between sub-maps in the loop detection is also called a loop error.
A kdTere query method comprises the following steps: a method for efficient query using a tree structure. The number of points in the point cloud, which are less than a certain threshold value from a certain coordinate P, can be obtained, and the found points are called r neighborhood points of the P points;
FPFH characteristics (see references: Rusu R B. Semantic 3D Object Maps for Evaryday management in Human Living Engineers [ J ]. KI-Kunstliche Intellignenz, 2010, 24(4):345-348. P57-P61.): the fast point feature histogram is used for describing the geometric features of the point cloud in a local space;
SAC-IA algorithm (see references: Rusu R B. Semantic 3D Object Maps for evolution management [ J ]. KI-Kunstliche Intellignez, 2010, 24(4):345-348. P57-P61.): and obtaining a characteristic-based matrix transformation relation between the two point clouds by a sampling consistency initial registration algorithm in a sampling mode. The results from this approach tend to be less accurate and require further precision calculations, but may give a value closer to the correct result. In this method, a lower score indicates a better result.
In order to achieve the aim, the invention provides a dynamic object-free map segmentation establishing method based on laser point cloud, which comprises the following steps:
step 1, collecting data by using a laser radar to obtain point cloud data; the point cloud data is multi-frame data;
step 2, playing the collected point cloud data by using an industrial personal computer, and creating a map offline; carrying out voxel grid downsampling on each frame of the point cloud data and then transmitting the frame of the point cloud data to the map creation method;
the preset parameters include:
nframethe serial number of a certain frame data in the point cloud data;
ninitinitializing frame number;
shiftaddedupdating the distance for the map;
r is the query radius of kdTree;
nstablea stable number threshold for "new points";
ndeletea disappearance number threshold for the "new point";
nbuffercaching the frame number of the seed map;
nsubmapthe number of frames of the sub map;
nstartBufferto cache the seed map start frame number, nstartBuffer＝nsubmap-nbuffer；
Step 3, initializing the map without dynamic objects;
step 4, establishing and updating the sub-map without dynamic objects;
step 5, carrying out loop detection on the map, and calculating the error of the map;
step 6, optimizing the error of the map;
and 7, outputting the map subjected to error optimization.
Further, the lidar is located at a top dead center of the vehicle.
Further, the step 3 comprises:
step 3.1, recording first frame information of the point cloud data as frame first, taking a coordinate system of the laser radar when the first frame information is scanned as a coordinate system of the map, and taking the position of the laser radar when the first frame information is scanned as an initial position;
3.2, using an NDT (normalized difference test) registration method, taking the frame first as a target point cloud, and taking each frame of scanning information of the laser radar as an input point cloud, and obtaining the displacement shift of the vehicle relative to the initial position;
step 3.3 if nframe≥ninitOr shift is greater than or equal to shiftaddedUsing kdTeree to perform mutual query between the registration result scanAligned and the frameFirst of the current frame; removing points which belong to scanAligned and can not be inquired into r neighborhood points in the frameFirst; removing points which belong to the frameFirst but can not inquire r neighborhood points in the scanAligned to obtain frameFirstFiltered and scanAligned filtered;
step 3.4, adding the frameFirstFiltered and scanalignedFiltered to obtain a final initial map, namely mapinitAnd completing the initialization of the map.
Further, the step 4 comprises:
step 4.1, let submap ═ mapinitWherein the submap is the current sub-map;
step 4.2, setting a cache seed mapbuffer；
Step 4.3, setting variable naddedIndicating the number of map updates under the current sub-map;
step 4.4, setting a data structure 'new point' pointNew, comprising members: coordinate information: x, y, z; the appearance time of the "new point" is: t is texistInitialized to 0; the number of times the "new point" is seen: n isseeAnd is initialized to 0.
Simultaneously setting a container PointCloudNew for storing the new point;
step 4.5, acquiring the position of the vehicle by using the NDT registration method and using the submap as a target point cloud and the scanning information scan of the laser radar as an input point cloud, and further calculating the displacement shift of the vehicle relative to the last map updating time; the point cloud of the registration result is scanAligned;
step 4.6, if shift is larger than or equal to shiftaddedAdding map information is needed, wherein n isaddedThe value of (a) is increased by 1;
step 4.7, updating the state of the pointNew in the container pointCloudNew; if the PointCloudNew is not empty, querying the point in the scanAligned, the queried pointNew, n thereof on the PointCloudNewseeThe value is increased by 1; if n issee>nstableIf the pointNew is stable, adding the pointNew into the submap, and deleting the pointNew from the container pointCloudNew; t of the pointNew will remainexistIncrease by 1 if texist>ndeleteIf the pointNew is considered as a temporary non-stable point, deleting the pointNew from the container pointCloudNew;
step 4.8, searching a new point in the scanalign; inquiring a point in the scanalign in the submap, regarding the point in the scanalign which is not inquired to the r neighborhood point as a newly appeared point, using the coordinates of the point to create the pointNew, adding the pointNew into the container PointCloudNew, and deleting the point from the scanalign;
step 4.9, adding the data points in the scanalign to the submap, namely, the submap and the scanalign;
step 4.10, if nadded≥nstartBufferAdding the scanAligned to the cache seed mapbufferIn (i) mapbuffer＝mapbuffer+scanAligned；
Step 4.11, if nadded＝nsubmapIf the current sub-map is built, the sub-map is stored locally; resetting multiple parameters simultaneously, resetting the number of map updates nadded0; using the cache seed map as the current sub-map ═ mapbuffer(ii) a Clearing the cache seed map;
and 4.12, introducing the point cloud information of the next frame, and repeating the operations from the step 4.5 to the step 4.11 until all frames of the point cloud data are finished.
Further, the step 5 comprises:
step 5.1, each time a submap of the sub-map is establishedcurrentMaking a decision on other established sub-maps, if the sub-map and sub-map arecurrentAnd when the distance is less than a certain threshold value and the difference between the sequence numbers established by the two is greater than a certain threshold value, adding the sub-map into the loop detection sub-map set.
Step 5.2, loop detection of FPFH rough registration is carried out;
and 5.3, calculating the error of NDT fine registration.
Further, the step 5.2 comprises:
step 5.2.1, respectively calculating the submaps of the sub-mapscurrentAnd the sub maptestA normal characteristic of (d);
step 5.2.2, respectively calculating the submaps of the sub-map based on the normal featurescurrentAnd the sub maptestThe FPFH characteristic of (1);
step 5.2.3, based on the FPFH characteristics, using a SAC-IA algorithm to perform loop coarse registration;
step 5.2.4, the minimum score of the loop detection is taken as the optimal coarse matching, if the optimal coarse registration score is smaller than a certain threshold value, and the quasi score is smaller than a certain threshold value, the loop detection is considered to be successful, and the serial numbers of the detected sub-map and the current sub-map are respectively assigned to nstartAnd nend。
Further, the step 5.3 comprises:
step 5.3.1, accurately registering the coarse registration results of the two point clouds by using an NDT (normalized difference test) registration method based on the optimal result of loop detection success of the FPFH (field programmable gate flash) coarse registration;
step 5.3.2, obtaining a final registration result transformation matrix Terror(ii) a The transformation matrix is the loop back error generated by the map creation method.
Further, the step 6 comprises:
step 6.1, flattening the loop error; the calculation sequence number is at nstartAnd nendAverage error between every two adjacent sub-maps: t iserrorSubmap＝Terror/(nend-nstart)；
And 6.2, correcting the position by taking the sub map as a unit.
Further, in said step 6.2, for the sequence number i (n)start≤i≤nend) Sub-map of (2), perform Ti＝(i- nstart)×TerrorSubmapPoint cloud transformation of (2); for each sub-map, there are: submapcorrect_i＝submapi×Ti。
Further, the step 7 includes:
step 7.1, sub-map overlay, i.e.
7.2, performing map downsampling, and performing voxel grid downsampling on the map;
and 7.3, outputting and storing the complete map.
Other multiline lidar may also be used in embodiments of the present invention.
In another embodiment of the present invention, the point cloud inter-frame registration method does not necessarily use the NDT method, and other inter-frame registration algorithms such as the ICP method and the PL-ICP method may also be used.
In another embodiment of the present invention, the loop detection does not necessarily use the coarse registration method of FPFH + SAC-IA, and other registration methods based on point cloud features may also be used.
The method for establishing the map without the dynamic object by the segmentation based on the laser point cloud has the following technical effects:
1. the map is gradually saved to the local, so that huge calculation pressure brought to an industrial personal computer by excessive increase of the map is avoided; through the optimization, the algorithm has the capability of establishing a large environment laser point cloud map. If the map which is kept complete is used for updating in the whole process, the complexity of the probability density function generation in the registration algorithm is as follows: o (n), wherein n is a map point number; the invention limits the maximum complexity value generated by the probability density function by gradually saving the map: o (K), wherein K is a constant related to a preset value;
2. aiming at the problem of loop error, the loop error is detected and eliminated through FPFH feature calculation and an initial registration algorithm based on the FPFH feature, and the normal use of the method is ensured.
3. Another point of optimization brought by the sub-map method is that the too large map can bring influence to the internal memory of the computer and even cause the crash of the industrial personal computer. A map with the final size of 100M can be divided into a plurality of sub-maps of 2-5M to be built respectively, and the method is very suitable for building the map in a large environment and in a long distance.
4. By setting the cache container, points to be added into the map are firstly placed in the container, and the points are considered to be stable points in the map when the points appear for enough times, so that the operation effectively filters dynamic objects (such as vehicles, pedestrians and the like) in the environment, the established map is closer to the original scene of the real environment, and the final positioning is facilitated.
The conception, the specific structure and the technical effects of the present invention will be further described with reference to the accompanying drawings to fully understand the objects, the features and the effects of the present invention.
Drawings
FIG. 1 is a general block diagram of a map building method;
FIG. 2 is a schematic diagram of map initialization;
FIG. 3 is a schematic diagram of a map update;
FIG. 4 is a schematic diagram of loop detection and error optimization;
fig. 5 is a graph of the filtering effect of the motion trail of the dynamic object.
Detailed Description
The technical contents of the preferred embodiments of the present invention will be more clearly and easily understood by referring to the drawings attached to the specification. The present invention may be embodied in many different forms of embodiments and the scope of the invention is not limited to the embodiments set forth herein.
As shown in fig. 2, the method for building a map without dynamic objects based on laser point cloud disclosed by the present invention comprises the following steps:
step 1, collecting laser radar data;
step 2, data down-sampling;
step 3, initializing a map without dynamic objects;
step 4, no dynamic object is created and updated on the sub-map;
step 5, loop detection and error calculation;
step 6, error optimization;
and 7, outputting the map.
The preset parameters include:
nframethe serial number of a certain frame data in the point cloud data;
ninitinitializing frame number;
shiftaddedupdating the distance for the map;
r is the query radius of kdTree;
nstableis "newPoint "threshold of stable times;
ndeletea disappearance number threshold of "new points";
nbuffercaching the frame number of the seed map;
nsubmapthe number of frames of the sub map;
nstartBufferto cache the seed map start frame number, nstartBuffer＝nsubmap-nbuffer；
The method comprises the following steps that 1, data acquisition is carried out through a laser radar, and point cloud data are obtained; the point cloud data is multi-frame data.
Step 2, playing the collected point cloud data by using an industrial personal computer, and creating a map offline; and carrying out voxel grid downsampling on each frame of point cloud data and then transmitting the frame to a map creation method.
Wherein, the laser radar is positioned at the top right middle part of the vehicle.
As shown in fig. 3, the map initialization of step 3 includes the following steps:
step 3.1, recording first frame information of the point cloud data as frame first, taking a coordinate system of the laser radar when the first frame information is scanned as a coordinate system of a map, and taking the position of the laser radar when the first frame information is scanned as an initial position;
3.2, using an NDT (normalized difference test) registration method, taking the frame first as a target point cloud and the scanning information scan of the laser radar as an input point cloud, and obtaining the displacement shift of the vehicle relative to the initial position;
step 3.3 if nframe≥ninitOr shift is greater than or equal to shiftaddedUsing kdTeree to perform mutual query between the registration result scanAligned and the frameFirst of the current frame; removing points which belong to scanAligned and can not be inquired into r neighborhood points in the frameFirst; removing points which belong to the frameFirst but can not inquire r neighborhood points in the scanAligned to obtain frameFirstFiltered and scanAligned filtered;
step 3.4, adding the frameFirstFiltered and scanalignedFiltered to obtain a final initial map, namely mapinit＝frameFirstFiltered+scanAlignedFiAnd (4) filtered, completing the initialization of the map.
As shown in fig. 4, the map update of step 4 includes the following steps:
step 4.1, let submap ═ mapinitWherein the submap is the current sub-map;
step 4.2, setting a cache seed mapbuffer；
Step 4.3, setting variable naddedIndicating the number of map updates under the current sub-map;
step 4.4, setting a data structure 'new point' pointNew, comprising members: coordinate information: x, y, z; appearance time of "new spot": t is texistInitialized to 0; number of times "new point" is seen: n isseeInitialized to 0; simultaneously, a container PointCloudNew is arranged for storing 'new points';
step 4.5, acquiring the position of the vehicle by using an NDT (normalized difference test) registration method and taking the submap as a target point cloud and the scanning information scan of the laser radar as an input point cloud, and further calculating the displacement shift of the vehicle relative to the last map updating time; the point cloud of the registration result is scanAligned;
step 4.6, if shift is larger than or equal to shiftaddedThen the map information addition is needed, naddedThe value of (a) is increased by 1;
step 4.7, updating the state of the pointNew in the container pointCloudNew; if the PointCloudNew is not empty, querying the point in the scanAligned, the queried pointNew, n thereof on the PointCloudNewseeThe value is increased by 1; if n issee>nstableIf the pointNew is stable, adding the pointNew into the submap, and deleting the pointNew from the container pointCloudNew; t of the pointNew will remainexistIncrease by 1 if texist>ndeleteIf the pointNew is considered as a temporary non-stable point, deleting the pointNew from the container pointCloudNew;
step 4.8, searching a new point in the scanAligned; inquiring a point in scanalign in the submap, regarding the point in scanalign which is not inquired to the r neighborhood point as a newly appeared point, creating pointNew by using the coordinates of the point, adding the pointNew into a container PointCloudNew, and deleting the point from scanalign;
step 4.9, adding the data points in the scanalign into submap, namely, submap + scanalign;
step 4.10, if nadded≥nstartBufferThen adding scanAligned to the cache seed mapbufferIn (i) mapbuffer＝mapbuffer+scanAligned；
Step 4.11, if nadded＝nsubmapIf the current sub-map is built, the sub-map is stored locally; resetting multiple parameters simultaneously, resetting the number of map updates nadded0; using the cache seed map as the current sub-map ═ mapbuffer(ii) a Clearing the cache seed map;
and 4.12, transmitting the point cloud information of the next frame, and repeating the operations from the step 4.5 to the step 4.11 until all frames of the point cloud data are finished.
As shown in fig. 5, the loop detection and error calculation of step 5 includes the following steps:
step 5.1, each time a submap of the sub-map is establishedcurrentMaking a decision on other established sub-maps, if the sub-map and sub-map arecurrentAnd when the distance is less than a certain threshold value and the difference between the sequence numbers established by the two is greater than a certain threshold value, adding the sub-map into the loop detection sub-map set.
Step 5.2, loop detection of FPFH rough registration is carried out;
and 5.3, calculating the error of NDT fine registration.
Wherein, step 5.2 includes:
step 5.2.1, respectively calculating the submaps of the sub-mapscurrentAnd sub maptestA normal characteristic of (d);
step 5.2.2, respectively calculating sub map based on normal featurescurrentAnd sub maptestThe FPFH characteristic of (1);
step 5.2.3, based on the FPFH characteristics, using SAC-IA algorithm to perform coarse registration;
step 5.2.4, the minimum score of the loopback detection is taken as the optimal coarse matching, if the score of the optimal coarse registration is smaller than a certain threshold value, the loopback detection is considered to be successful, and the serial numbers of the detected sub-map and the current sub-map are respectively assigned to nstartAnd nend。
Wherein, step 5.3 includes:
step 5.3.1, using NDT (normalized difference transmit) registration method based on loop detection success result of FPFH (field programmable gate flash) coarse registration, and using nstartAs a target point cloud, nendAccurately registering two point clouds for input point clouds;
step 5.3.2, obtaining a final registration result transformation matrix Terror(ii) a The transformation matrix is the loop error generated by map building.
The error optimization of the step 6 comprises the following steps:
step 6.1, flattening the loop error; the calculation sequence number is at nstartAnd nendAverage error between every two adjacent sub-maps: t iserrorSubmap＝Terror/(nend-nstart)；
And 6.2, correcting the position by taking the sub map as a unit.
Wherein, in step 6.2, the sequence number is i (n)start≤i≤nend) Sub-map of (2), perform Ti＝(i- nstart)×TerrorSubmapPoint cloud transformation of (2); for each sub-map, there are: submapcorrect_i＝submapi×Ti。
The map output of step 7 comprises the following steps:
step 7.1, sub-map overlay, i.e.
Figure RE-GDA0002284182220000081
7.2, map downsampling, namely downsampling a map voxel grid;
and 7.3, outputting and storing the complete map.
The foregoing detailed description of the preferred embodiments of the invention has been presented. It should be understood that numerous modifications and variations could be devised by those skilled in the art in light of the present teachings without departing from the inventive concepts. Therefore, the technical solutions available to those skilled in the art through logic analysis, reasoning and limited experiments based on the prior art according to the concept of the present invention should be within the scope of protection defined by the claims.

Claims (10)

1. A dynamic object-free map segmentation establishing method based on laser point cloud is characterized by comprising the following steps:
step 1, collecting data by using a laser radar to obtain point cloud data; the point cloud data is multi-frame data;
step 2, playing the collected point cloud data by using an industrial personal computer, and creating a map offline by using a SLAM method; carrying out voxel grid downsampling on each frame of the point cloud data and then transmitting the mapping module;
the preset parameters include:
nframethe serial number of a certain frame data in the point cloud data;
ninitinitializing frame number;
shiftaddedupdating the distance for the map;
r is the query radius of kdTree;
nstablea stable number threshold for "new points";
ndeletea disappearance number threshold for the "new point";
nbuffercaching the laser point cloud frame number of the seed map;
nsubmapthe number of laser point cloud frames contained in each sub-map is set;
nstartBufferto cache the seed map start frame number, nstartBuffer＝nsubmap-nbuffer；
Step 3, initializing the map without dynamic objects;
step 4, establishing and updating the sub-map without dynamic objects;
step 5, carrying out loop detection on the map, and calculating the error of the map;
step 6, optimizing the error of the map;
and 7, outputting the map subjected to error optimization.
2. The method of claim 1, wherein the lidar is positioned at a top center of the vehicle.
3. The method for building a map segment without dynamic objects based on laser point cloud as claimed in claim 2, wherein the step 3 comprises:
step 3.1, recording first frame information of the point cloud data as frame first, taking a coordinate system of the laser radar when the first frame information is scanned as a coordinate system of the map, and taking the position of the laser radar when the first frame information is scanned as an initial position;
3.2, using an NDT (normalized difference test) registration method, taking the frame first as a target point cloud, and taking each frame of scanning information of the laser radar as an input point cloud, and obtaining the displacement shift of the vehicle relative to the initial position;
step 3.3 if nframe≥ninitOr shift is greater than or equal to shiftaddedUsing kdTeree to perform mutual query between the registration result scanAligned and the frameFirst of the current frame; removing points which belong to scanAligned and can not be inquired into r neighborhood points in the frameFirst; removing points which belong to the frameFirst but can not inquire r neighborhood points in the scanAligned to obtain frameFirstFiltered and scanAligned filtered;
step 3.4, the frameFirstFiltered and scanalignedFiltered are overlapped to obtain a final initial map, namely mapinitAnd completing the initialization of the map.
4. The method for building a map segment without dynamic objects based on laser point cloud as claimed in claim 3, wherein the step 4 comprises:
step 4.1, let submap ═ mapinitWherein the submap is the current sub-map;
step 4.2, setting a cache seed mapbufferFor the production of the next sub-map;
step 4.3, setting variable naddedIndicating the number of map updates under the current sub-map;
step 4.4, setting a data structure 'new point' pointNew, comprising members: coordinate information: x, y, z; the appearance time of the "new point" is: t is texistInitialized to 0; the number of times the "new point" is seen: n isseeInitialized to 0;
simultaneously setting a container PointCloudNew for storing the new point;
step 4.5, acquiring the position of the vehicle by using the submap as a target point cloud and the scanning frame scan of the laser radar as an input point cloud by using the NDT registration method, and further calculating the displacement shift of the vehicle relative to the last map updating time; the point cloud of the registration result is scanAligned;
step 4.6, if shift is larger than or equal to shiftaddedThen the map information needs to be updated, naddedThe value of (a) is increased by 1;
step 4.7, updating the state of the pointNew in the container pointCloudNew; if the PointCloudNew is not empty, querying the point in the scanAligned, the queried pointNew, n thereof on the PointCloudNewseeThe value is increased by 1; if n issee>nstableIf the pointNew is stable, adding the pointNew into the submap, and deleting the pointNew from the container pointCloudNew; t of the pointNew will remainexistIncrease by 1 if texist>ndeleteIf the pointNew is considered as a temporary non-stable point, deleting the pointNew from the container pointCloudNew;
step 4.8, searching a new point in the scanalign; inquiring a point in the scanalign in the submap, regarding the point in the scanalign which is not inquired to the r neighborhood point as a newly appeared point, using the coordinates of the point to create the pointNew, adding the pointNew into the container PointCloudNew, and deleting the point from the scanalign;
step 4.9, adding the data points in the scanalign to the submap, namely, the submap and the scanalign;
step 4.10, if nadded≥nstartBufferAdding the scanAligned to the cache seed mapbufferIn (i) mapbuffer＝mapbuffer+scanAligned；
Step 4.11, if nadded＝nsubmapIf the current sub-map is built, the sub-map is stored locally; resetting multiple parameters simultaneously, resetting the number of map updates nadded0; using the cache seed map as the current sub-map ═ mapbuffer(ii) a Clearing the cache seed map;
and 4.12, introducing the point cloud information of the next frame, and repeating the operations from the step 4.5 to the step 4.11 until all frames of the point cloud data are finished.
5. The method for building a map segment without dynamic objects based on laser point cloud as claimed in claim 4, wherein the step 5 comprises:
step 5.1, each time a submap of the sub-map is establishedcurrentMaking a decision on other established sub-maps, if the sub-map and sub-map arecurrentAnd when the distance is less than a certain threshold value and the difference between the sequence numbers established by the two is greater than a certain threshold value, adding the sub-map into the loop detection sub-map set.
Step 5.2, loop detection of FPFH rough registration is carried out;
and 5.3, calculating the error of NDT fine registration.
6. The method for building a map segment without dynamic objects based on laser point cloud as claimed in claim 5, wherein said step 5.2 comprises:
step 5.2.1, respectively calculating the submaps of the sub-mapscurrentAnd the loopback test sub-map submaptestA normal characteristic of (d);
step 5.2.2, respectively calculating the submaps of the sub-map based on the normal featurescurrentAnd the loopback test sub-map submaptestThe FPFH characteristic of (1);
step 5.2.3, based on the FPFH characteristics, using a SAC-IA algorithm to perform loop coarse registration;
step 5.2.4, the minimum score of the loopback detection is taken as the optimal coarse matching, if the score of the optimal coarse registration is smaller than a certain threshold value, the loopback detection is considered to be successful, and the serial numbers of the detected sub-map and the current sub-map are respectively assigned to nstartAnd nend。
7. The method for building a map segment without dynamic objects based on laser point cloud as claimed in claim 6, wherein said step 5.3 comprises:
step 5.3.1, accurately registering the coarse registration results of the two point clouds by using an NDT (normalized difference test) registration method based on the loop detection success result of the FPFH (field programmable gate flash) coarse registration;
step 5.3.2, obtaining a final registration result transformation matrix Terror(ii) a The transformation matrix is the loop error generated by mapping by the SLAM method.
8. The method for building a map segment without dynamic objects based on laser point cloud as claimed in claim 7, wherein said step 6 comprises:
step 6.1, flattening the loop error; the calculation sequence number is at nstartAnd nendAverage error between every two adjacent sub-maps: t iserrorSubmap＝Terror/(nend-nstart)；
And 6.2, correcting the position by taking the sub map as a unit.
9. As claimed in claimThe method for establishing the map without the dynamic object based on the laser point cloud is characterized in that in the step 6.2, the serial number is i (n)start≤i≤nend) Sub-map of (2), perform Ti＝(i-nstart)×TerrorSubmapPoint cloud transformation of (2); for each sub-map, there are: submapcorrect_i＝submapi×Ti。
10. The method for building a map segment without dynamic objects based on laser point cloud as claimed in claim 8, wherein said step 7 comprises:
step 7.1, sub-map overlay, i.e.
Figure FDA0002196982190000031
7.2, map downsampling, namely downsampling a map voxel grid;
and 7.3, outputting and storing the complete map.
CN201910851167.1A 2019-09-10 2019-09-10 Dynamic object-free map segmentation establishing method based on laser point cloud Active CN110645998B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910851167.1A CN110645998B (en) 2019-09-10 2019-09-10 Dynamic object-free map segmentation establishing method based on laser point cloud

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910851167.1A CN110645998B (en) 2019-09-10 2019-09-10 Dynamic object-free map segmentation establishing method based on laser point cloud

Publications (2)

Publication Number Publication Date
CN110645998A true CN110645998A (en) 2020-01-03
CN110645998B CN110645998B (en) 2023-03-24

Family

ID=68991721

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910851167.1A Active CN110645998B (en) 2019-09-10 2019-09-10 Dynamic object-free map segmentation establishing method based on laser point cloud

Country Status (1)

Country Link
CN (1) CN110645998B (en)

Cited By (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111275763A (en) * 2020-01-20 2020-06-12 深圳市普渡科技有限公司 Closed loop detection system, multi-sensor fusion SLAM system and robot
CN111311684A (en) * 2020-04-01 2020-06-19 亮风台（上海）信息科技有限公司 Method and equipment for initializing SLAM
CN111553937A (en) * 2020-04-23 2020-08-18 东软睿驰汽车技术（上海）有限公司 Laser point cloud map construction method, device, equipment and system
CN111723173A (en) * 2020-06-15 2020-09-29 中国第一汽车股份有限公司 Vehicle-mounted map making method and device, electronic equipment and storage medium
CN111765882A (en) * 2020-06-18 2020-10-13 浙江大华技术股份有限公司 Laser radar positioning method and related device thereof
CN111795687A (en) * 2020-06-29 2020-10-20 深圳市优必选科技股份有限公司 Robot map updating method and device, readable storage medium and robot
CN112184539A (en) * 2020-11-27 2021-01-05 深兰人工智能(深圳)有限公司 Point cloud data processing method and device
CN112668653A (en) * 2020-12-30 2021-04-16 北京百度网讯科技有限公司 Loop detection method, device, equipment and medium based on laser radar map
CN112907491A (en) * 2021-03-18 2021-06-04 中煤科工集团上海有限公司 Laser point cloud loopback detection method and system suitable for underground roadway
CN113093221A (en) * 2021-03-31 2021-07-09 东软睿驰汽车技术（沈阳）有限公司 Generation method and device of grid-occupied map
CN113625301A (en) * 2021-09-03 2021-11-09 国网山东省电力公司济宁供电公司 Method and system for enlarging mapping view field based on single scanning head laser radar
CN114371485A (en) * 2022-03-21 2022-04-19 中汽研（天津）汽车工程研究院有限公司 Obstacle prediction and tracking method based on ICP and multi-feature data association

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106767827A (en) * 2016-12-29 2017-05-31 浙江大学 A kind of mobile robot point cloud map creating method based on laser data
CN108320329A (en) * 2018-02-02 2018-07-24 维坤智能科技（上海）有限公司 A kind of 3D map creating methods based on 3D laser
CN109214248A (en) * 2017-07-04 2019-01-15 百度在线网络技术（北京）有限公司 The method and apparatus of the laser point cloud data of automatic driving vehicle for identification
CN109556611A (en) * 2018-11-30 2019-04-02 广州高新兴机器人有限公司 A kind of fusion and positioning method based on figure optimization and particle filter
CN109598670A (en) * 2018-11-14 2019-04-09 广州广电研究院有限公司 EMS memory management process, device, storage medium and the system of cartographic information acquisition
CN109887053A (en) * 2019-02-01 2019-06-14 广州小鹏汽车科技有限公司 A kind of SLAM map joining method and system
CN109887057A (en) * 2019-01-30 2019-06-14 杭州飞步科技有限公司 The method and apparatus for generating high-precision map
CN109991984A (en) * 2019-04-22 2019-07-09 上海蔚来汽车有限公司 For generating the method, apparatus and computer storage medium of fine map
CN110084272A (en) * 2019-03-26 2019-08-02 哈尔滨工业大学（深圳） A kind of cluster map creating method and based on cluster map and the matched method for relocating of location expression
CN110109134A (en) * 2019-05-05 2019-08-09 桂林电子科技大学 A method of the broken line based on 2D laser radar range extracts Maximum-likelihood estimation

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106767827A (en) * 2016-12-29 2017-05-31 浙江大学 A kind of mobile robot point cloud map creating method based on laser data
CN109214248A (en) * 2017-07-04 2019-01-15 百度在线网络技术（北京）有限公司 The method and apparatus of the laser point cloud data of automatic driving vehicle for identification
CN108320329A (en) * 2018-02-02 2018-07-24 维坤智能科技（上海）有限公司 A kind of 3D map creating methods based on 3D laser
CN109598670A (en) * 2018-11-14 2019-04-09 广州广电研究院有限公司 EMS memory management process, device, storage medium and the system of cartographic information acquisition
CN109556611A (en) * 2018-11-30 2019-04-02 广州高新兴机器人有限公司 A kind of fusion and positioning method based on figure optimization and particle filter
CN109887057A (en) * 2019-01-30 2019-06-14 杭州飞步科技有限公司 The method and apparatus for generating high-precision map
CN109887053A (en) * 2019-02-01 2019-06-14 广州小鹏汽车科技有限公司 A kind of SLAM map joining method and system
CN110084272A (en) * 2019-03-26 2019-08-02 哈尔滨工业大学（深圳） A kind of cluster map creating method and based on cluster map and the matched method for relocating of location expression
CN109991984A (en) * 2019-04-22 2019-07-09 上海蔚来汽车有限公司 For generating the method, apparatus and computer storage medium of fine map
CN110109134A (en) * 2019-05-05 2019-08-09 桂林电子科技大学 A method of the broken line based on 2D laser radar range extracts Maximum-likelihood estimation

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
XIAQING DIN,ET AL: "Multi-session Map Construction in Outdoor Dynamic Environment", 《PROCEEDINGS OF THE 2018 IEEE INTERNATIONAL CONFERENCE ON REAL-TIME COMPUTING AND ROBOTICS》 *
翁潇文等: "基于图优化的二维激光SLAM研究", 《自动化与仪表》 *

Cited By (18)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111275763A (en) * 2020-01-20 2020-06-12 深圳市普渡科技有限公司 Closed loop detection system, multi-sensor fusion SLAM system and robot
CN111275763B (en) * 2020-01-20 2023-10-13 深圳市普渡科技有限公司 Closed loop detection system, multi-sensor fusion SLAM system and robot
CN111311684A (en) * 2020-04-01 2020-06-19 亮风台（上海）信息科技有限公司 Method and equipment for initializing SLAM
CN111311684B (en) * 2020-04-01 2021-02-05 亮风台（上海）信息科技有限公司 Method and equipment for initializing SLAM
CN111553937A (en) * 2020-04-23 2020-08-18 东软睿驰汽车技术（上海）有限公司 Laser point cloud map construction method, device, equipment and system
CN111553937B (en) * 2020-04-23 2023-11-21 东软睿驰汽车技术（上海）有限公司 Laser point cloud map construction method, device, equipment and system
CN111723173A (en) * 2020-06-15 2020-09-29 中国第一汽车股份有限公司 Vehicle-mounted map making method and device, electronic equipment and storage medium
CN111765882A (en) * 2020-06-18 2020-10-13 浙江大华技术股份有限公司 Laser radar positioning method and related device thereof
CN111795687B (en) * 2020-06-29 2022-08-05 深圳市优必选科技股份有限公司 Robot map updating method and device, readable storage medium and robot
CN111795687A (en) * 2020-06-29 2020-10-20 深圳市优必选科技股份有限公司 Robot map updating method and device, readable storage medium and robot
CN112184539A (en) * 2020-11-27 2021-01-05 深兰人工智能(深圳)有限公司 Point cloud data processing method and device
CN112668653A (en) * 2020-12-30 2021-04-16 北京百度网讯科技有限公司 Loop detection method, device, equipment and medium based on laser radar map
CN112907491B (en) * 2021-03-18 2023-08-22 中煤科工集团上海有限公司 Laser point cloud loop detection method and system suitable for underground roadway
CN112907491A (en) * 2021-03-18 2021-06-04 中煤科工集团上海有限公司 Laser point cloud loopback detection method and system suitable for underground roadway
CN113093221A (en) * 2021-03-31 2021-07-09 东软睿驰汽车技术（沈阳）有限公司 Generation method and device of grid-occupied map
CN113625301A (en) * 2021-09-03 2021-11-09 国网山东省电力公司济宁供电公司 Method and system for enlarging mapping view field based on single scanning head laser radar
CN113625301B (en) * 2021-09-03 2024-01-19 国网山东省电力公司济宁供电公司 Method and system for expanding view field of building map based on single-scanning-head laser radar
CN114371485A (en) * 2022-03-21 2022-04-19 中汽研（天津）汽车工程研究院有限公司 Obstacle prediction and tracking method based on ICP and multi-feature data association

Also Published As

Publication number Publication date
CN110645998B (en) 2023-03-24

Similar Documents

Publication Publication Date Title
CN110645998B (en) Dynamic object-free map segmentation establishing method based on laser point cloud
CN105160702B (en) The stereopsis dense Stereo Matching method and system aided in based on LiDAR point cloud
CN113436260B (en) Mobile robot pose estimation method and system based on multi-sensor tight coupling
CN113269837B (en) Positioning navigation method suitable for complex three-dimensional environment
CN109186608B (en) Repositioning-oriented sparse three-dimensional point cloud map generation method
CN106599108A (en) Method for constructing multi-mode environmental map in three-dimensional environment
CN112767490B (en) Outdoor three-dimensional synchronous positioning and mapping method based on laser radar
Zhou et al. S4-SLAM: A real-time 3D LIDAR SLAM system for ground/watersurface multi-scene outdoor applications
CN113706710A (en) Virtual point multi-source point cloud fusion method and system based on FPFH (field programmable gate flash) feature difference
CN113516664A (en) Visual SLAM method based on semantic segmentation dynamic points
CN116449384A (en) Radar inertial tight coupling positioning mapping method based on solid-state laser radar
CN116817887B (en) Semantic visual SLAM map construction method, electronic equipment and storage medium
CN117053779A (en) Tightly coupled laser SLAM method and device based on redundant key frame removal
CN116608873A (en) Multi-sensor fusion positioning mapping method for automatic driving vehicle
Xue et al. Real-time 3D grid map building for autonomous driving in dynamic environment
CN114563000A (en) Indoor and outdoor SLAM method based on improved laser radar odometer
CN114565726A (en) Simultaneous positioning and mapping method in unstructured dynamic environment
CN111127474B (en) Airborne LiDAR point cloud assisted orthophoto mosaic line automatic selection method and system
Guo et al. 3D Lidar SLAM Based on Ground Segmentation and Scan Context Loop Detection
CN116660916B (en) Positioning method, mapping method and electronic equipment for orchard mobile robot
Xie et al. Semi-direct visual SLAM algorithm based on improved keyframe selection
Ahat et al. Obstacle Detection in Off-road Environments Based on LiDAR
Shen et al. P‐2.11: Research on Scene 3d Reconstruction Technology Based on Multi‐sensor Fusion
Liu et al. Semantic-Assisted LIDAR Tightly Coupled SLAM for Dynamic Environments
CN117419738A (en) Based on visual view and D * Path planning method and system of Lite algorithm

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant