CN118209096B - Laser SLAM mapping method based on self-adaptive filter and GNSS factors - Google Patents
Laser SLAM mapping method based on self-adaptive filter and GNSS factors Download PDFInfo
- Publication number
- CN118209096B CN118209096B CN202410254101.5A CN202410254101A CN118209096B CN 118209096 B CN118209096 B CN 118209096B CN 202410254101 A CN202410254101 A CN 202410254101A CN 118209096 B CN118209096 B CN 118209096B
- Authority
- CN
- China
- Prior art keywords
- point cloud
- point
- gnss
- data
- voxel
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 62
- 238000013507 mapping Methods 0.000 title claims abstract description 14
- 238000005457 optimization Methods 0.000 claims abstract description 18
- 230000003044 adaptive effect Effects 0.000 claims abstract description 16
- 230000011218 segmentation Effects 0.000 claims abstract description 8
- 238000007781 pre-processing Methods 0.000 claims abstract description 7
- 238000001914 filtration Methods 0.000 claims description 32
- 238000012545 processing Methods 0.000 claims description 11
- 238000004364 calculation method Methods 0.000 claims description 7
- 238000005259 measurement Methods 0.000 claims description 6
- 238000012216 screening Methods 0.000 claims description 5
- 230000005484 gravity Effects 0.000 claims description 3
- 230000010354 integration Effects 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 238000005516 engineering process Methods 0.000 abstract description 4
- 238000010276 construction Methods 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000018109 developmental process Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 230000009897 systematic effect Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
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/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
-
- 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- 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
-
- 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/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
-
- 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
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- Optical Radar Systems And Details Thereof (AREA)
Abstract
本发明公开了一种基于自适应滤波器和GNSS因子的激光SLAM建图方法，包括获取扫描区域内激光雷达生成的多帧三维激光雷达点云数据；进行地面分割后划分为接地点集和非接地点集；对接地点集和非接地点集分别进行点云数据预处理；从接地点集中选出平面点集和非接地点集中选出角点集，利用LOAM算法递推求解出相邻帧之间的位姿，得到激光雷达里程计数据；将激光雷达里程计数据和GNSS提供的地图信息数据整合生成因子图，使用图优化方法对因子图进行优化，获得最终位姿估计，并基于最终位姿估计生成所述扫描区域的三维点云地图。本发明解决了现有激光SLAM技术在复杂环境或动态场景中定位和建图精度不足的问题。
The present invention discloses a laser SLAM mapping method based on an adaptive filter and GNSS factors, including obtaining multi-frame three-dimensional laser radar point cloud data generated by a laser radar in a scanning area; dividing the data into a ground point set and a non-ground point set after ground segmentation; performing point cloud data preprocessing on the ground point set and the non-ground point set respectively; selecting a plane point set from the ground point set and a corner point set from the non-ground point set, recursively solving the pose between adjacent frames using the LOAM algorithm, and obtaining laser radar odometer data; integrating the laser radar odometer data and the map information data provided by GNSS to generate a factor graph, optimizing the factor graph using a graph optimization method, obtaining a final pose estimate, and generating a three-dimensional point cloud map of the scanning area based on the final pose estimate. The present invention solves the problem of insufficient positioning and mapping accuracy of existing laser SLAM technology in complex environments or dynamic scenes.
Description
Technical Field
The invention relates to the technical field of laser radar positioning and map construction, in particular to a laser SLAM map construction method based on a self-adaptive filter and GNSS factors.
Background
With the rapid development of three-dimensional laser scanning technology, laser SLAM (Simultaneous Localization AND MAPPING, SLAM) technology has been widely used in the fields of robots, automated driving vehicles, and the like. Through a laser SLAM algorithm, an automatic driving vehicle can realize incremental construction of a map by sequentially registering point cloud data collected at different positions, and then the aim of optimizing current pose estimation and a map is fulfilled by matching an environment scanned in real time at the position of the automatic driving vehicle with map information provided by a GNSS.
During data acquisition, the lidar scans the surrounding environment and generates a large number of data points, which constitute a so-called point cloud. Since lidar has a wide scanning range and high resolution, it can capture very detailed information in the environment, but this also means that a large amount of data is generated, which data needs to be processed by a computer. The process of processing such large amounts of data consumes a significant amount of computing resources, thereby reducing processing speed.
To address this problem, it is often desirable to downsample the point cloud data, i.e., select a portion of the data points from the raw data for processing, to reduce the amount of data and thereby increase the processing speed. However, conventional voxel filters (a common downsampling method) typically have a fixed grid size and cannot adaptively adjust the filtering parameters according to the density of the current point cloud. This means that even in the case where the point cloud data is already sparse, the filter may still filter using the same grid size, which may result in important feature points being incorrectly filtered out, thereby losing important information in the point cloud data.
In addition, when the lidar data is optimized using Global Navigation Satellite System (GNSS) factors, the GNSS factors represent positioning information of a robot or vehicle at a particular moment. If there is a large time difference between the time of acquisition of the GNSS factor and the time of the laser radar key frame, this GNSS factor may not accurately reflect the pose of the key frame time. Because the robot or vehicle may have moved a distance during this time, the GNSS factors may not match the pose of the key frames. Such a mismatch can affect the accuracy of pose estimation during the optimization process, thereby affecting the overall accuracy of the vehicle trajectory.
Disclosure of Invention
The invention aims to provide a laser SLAM mapping method based on an adaptive filter and GNSS factors, which has small calculation amount and high mapping precision.
In order to achieve the above purpose, the laser SLAM mapping method based on the adaptive filter and the GNSS factors comprises the following steps:
step 1, acquiring multi-frame three-dimensional laser radar point cloud data generated by a laser radar in a scanning area;
Step 2, dividing the point cloud data into a grounding point set and a non-grounding point set after ground segmentation;
step 3, respectively preprocessing point cloud data of the grounding point set and the non-grounding point set;
Step 4, selecting a plane point set and an angle point set from the non-grounding point set from the preprocessed grounding point set;
step 5, recursively solving the pose between adjacent frames by utilizing an LOAM algorithm based on the plane point set and the corner set to obtain laser radar odometer data;
And 6, integrating the laser radar odometer data and the map information data provided by the GNSS to generate a factor graph, optimizing the factor graph by using a graph optimization method to obtain final pose estimation, and generating a three-dimensional point cloud map of the scanning area based on the final pose estimation.
The method for dividing the ground into a grounding point set and a non-grounding point set in the step 2 comprises the steps of calculating a horizontal included angle between two points according to a height difference Hz and a plane Euclidean distance D ab between the point A and the point B on two adjacent three-dimensional laser radar scanning lines, calculating the horizontal included angle from small to large according to the vertical angle of laser radar beams, if the horizontal included angle is larger than a set value, the point cloud data within the current vertical angle belong to the grounding point, otherwise belong to the non-grounding point, and finally realizing the ground division.
The preprocessing in step 3 is to perform adaptive voxel filtering on the ground point concentrated point cloud data, and the non-ground point concentrated point cloud data is first subjected to point cloud clustering segmentation processing based on Euclidean distance, and then subjected to adaptive voxel filtering after being divided into different point sets.
The method for performing self-adaptive voxel filtering on the point cloud data comprises the following steps:
step 301, calculating an initial voxel side length Step 1 and a grid voxel length Step n after iterative growth according to a maximum distance D max of the input point cloud;
302, substituting a parameter Step 1,Stepn into a voxel filter for filtering processing to obtain a point number P n after downsampling, and if the point cloud number P n is more than or equal to P g and the iteration number is less than n max, recalculating the current grid voxel side length Step n and substituting the current grid voxel side length Step into the voxel filter again;
step 303, repeating the above steps until the current point cloud number P n is smaller than P g or the iteration number is larger than n max;
Step 304, based on step 303, if the current point cloud number P n is smaller than the downsampled minimum point cloud number P min, using P n-1 to replace P n as the current point cloud number, and simultaneously using the point cloud obtained through n-1 times of filtering to replace the point cloud obtained through n times of filtering and output as the target point cloud, and if P n is larger than P min, outputting the target point cloud obtained through n times of filtering to ensure that the output target point cloud number is always larger than P min.
The calculation process of the maximum distance D max in step 301 is as follows:
solving the maximum value x max,ymax,zmax and the minimum value x min,ymin,zmin of the point cloud data x, y and z on three coordinate axes, and calculating the side length of the point cloud bounding box to ：Ix＝xmax-xmin,Iy＝ymax-ymin,Iz＝zmax-zmin;
The method for calculating the initial voxel side length Step 1 and the iteratively-increased grid voxel length Step n comprises the following steps:
Wherein D max is the maximum distance in the point cloud set, step 1 is the grid voxel side length of initial downsampling, step n is the grid voxel length after n-th iteration growth, n is the current iteration number, n max is the maximum iteration number, P g is the target point number after downsampling, P min is the minimum point number after downsampling, P n is the point number after n-time filtering, f (·) represents a filter, and K is the Step size coefficient.
The process of substituting the parameter Step 1、Stepn into the voxel filter for filtering in Step 302 is as follows:
Step 3021, setting the side length of each grid voxel as r, and dividing the point cloud data into a D x×Dy×Dz voxel grid:
Wherein, I x、Iy、Iz is the side length of the point cloud bounding box on three coordinate axes of x, y and z, and x max、ymax、zmax and x min、ymin、zmin are the maximum value and the minimum value of the point cloud data of x, y and z on the three coordinate axes respectively;
step 3022, calculating an Index for each point within the voxel grid:
Step 3023, sorting the points in the Index from small to large to calculate the barycentric point of each voxel grid, and replacing all the points in the voxel grid with the barycentric point, wherein the calculation process of the barycentric point is as follows:
Wherein X, Y, Z is the center of gravity point coordinate, m is the number of point clouds in the grid, x i、yi、zi is the point cloud coordinate in the grid, and i is the point cloud sequence number in the grid.
The method for recursively solving the pose between adjacent frames by using the LOAM algorithm in the step 5 comprises the steps of performing feature matching between continuous frames by taking corner points and plane points as feature points, namely, matching the feature points of the current frame with the feature points in historical frames to obtain feature point pairs, constructing a nonlinear optimization problem about the pose by using the feature point pairs, solving the problem to obtain the pose between the adjacent frames, and forming laser radar odometry data by pose data of all the adjacent frames, wherein the laser radar odometry data describe the position and the orientation change of the three-dimensional laser radar in a map coordinate system along with time.
In step 6, integrating the laser radar odometer data and the map information data provided by the GNSS to generate a factor graph, and optimizing the factor graph by using a graph optimization method to obtain a final pose estimation method, which comprises the following steps:
Step 601, data integration, namely performing timestamp matching on laser radar odometer data and GNSS data;
Step 602, constructing a factor graph, wherein each laser radar measurement data point and GNSS data point corresponds to a factor, the factors represent different types of observation information, and edges in the factor graph represent the dependency relationship among different observation information;
and 603, minimizing the observation error in the factor graph by using a graph optimization method, wherein in the optimization process, the pose in the laser radar measurement data is adjusted according to the dependency relationship between the observation information in the factor graph and different observation information until the convergence condition is met, and outputting final pose estimation.
In the optimizing process in step 603, the GNSS data are optimized by screening and interpolating GNSS factors to reduce errors in acquiring the GNSS data, i.e. a frame of image is selected as a key frame at intervals, wherein each key frame should include longitude, latitude and altitude information of the GNSS factors before and after the GNSS factors, if no GNSS factor is selected after the key frame is selected, waiting for the next new GNSS data input, otherwise comparing the timestamp of the key frame with the timestamp of the previous and next GNSS factors, if the difference between the timestamps is smaller than the set value, linearly interpolating the longitude, latitude and altitude of the previous and next GNSS factors, taking the longitude, latitude and altitude obtained by interpolation as the optimized GNSS factors, otherwise waiting for the new GNSS data input, and after the interpolation is finished, transferring the optimized GNSS factors under the current map coordinate system and inserting the optimized GNSS factors into the global factor map.
The method for generating the three-dimensional point cloud map of the scanning area in the step 6 includes the steps of constructing a pose map according to pose estimation, wherein nodes in the pose map represent laser radar poses of different frames, edges represent transformation relations between adjacent frames, and the pose map is used for matching point cloud data of different frames, so that the point cloud data of different frames are integrated, and a continuous three-dimensional point cloud map is generated.
The self-adaptive voxel filter has the advantages that 1, the self-adaptive voxel filter adopted by the method can dynamically adjust the grid size according to the local density of point cloud data, in a region with denser point cloud, smaller grid size can be used for reserving more details, in a region with sparser point cloud, larger grid size can be used for reducing the calculated amount and filtering noise, and the self-adaptive adjustment is beneficial to reducing unnecessary data processing and reducing the calculated amount while reserving characteristic points in important point cloud data;
2. The method firstly compares the GNSS factors with the time stamps of the laser radar key frames, ensures that the time difference between the GNSS factors and the key frame time is in an acceptable range, ensures that the GNSS factors can accurately reflect the pose of the key frame time, processes the GNSS factors with larger time difference between the time stamps and the key frame time by adopting a linear interpolation technology, and can obtain an approximate GNSS pose at the key frame time by adopting the method, thereby reducing the problem of pose mismatch caused by the time difference and improving the accuracy of vehicle tracks.
Drawings
FIG. 1 is a schematic flow chart of the method of the present invention;
FIG. 2 is a schematic diagram of points A and B on two adjacent three-dimensional lidar scan lines;
FIG. 3 is a flow chart of adaptive voxel filtering;
FIG. 4 is a flow chart for optimizing GNSS data by screening and interpolating GNSS factors;
Fig. 5 is a graph of point cloud data before and after filtering using an adaptive voxel filter.
Detailed Description
The technical scheme of the present invention will be described in detail with reference to the following examples and the accompanying drawings.
As shown in fig. 1, a laser SLAM mapping method based on an adaptive filter and GNSS factors includes the following steps:
step 1, acquiring multi-frame three-dimensional laser radar point cloud data generated by a laser radar in a scanning area;
The method comprises the steps of dividing point cloud data into a grounding point set and a non-grounding point set after ground segmentation, calculating a horizontal included angle between two points according to a height difference Hz and a plane Euclidean distance D ab between the point A and the point B respectively positioned on two adjacent three-dimensional laser radar scanning lines, calculating the horizontal included angle according to laser radar beam vertical angles from small to large and point cloud data, wherein if the horizontal included angle is larger than a set value, the point cloud data belong to the grounding point within the current vertical angle, otherwise belong to the non-grounding point, and finally realizing ground segmentation, as shown in figure 2.
And 3, respectively preprocessing point cloud data of a grounding point set and a non-grounding point set, wherein the preprocessing refers to self-adaptive voxel filtering of the point cloud data obtained by scanning each frame of the laser radar, the self-adaptive voxel filtering is directly carried out on the grounding point set point cloud data, the non-grounding point set point cloud data is subjected to point cloud clustering segmentation processing based on Euclidean distance, and the point cloud data is divided into different point sets and then subjected to self-adaptive voxel filtering.
As shown in fig. 3, the method for performing adaptive voxel filtering on the point cloud data includes:
step 301, calculating an initial voxel side length Step 1 and a grid voxel length Step n after iterative growth according to a maximum distance D max of the input point cloud;
302, substituting a parameter Step 1,Stepn into a voxel filter for filtering processing to obtain a point number P n after downsampling, and if the point cloud number P n is more than or equal to P g and the iteration number is less than n max, recalculating the current grid voxel side length Step n and substituting the current grid voxel side length Step into the voxel filter again;
step 303, repeating the above steps until the current point cloud number P n is smaller than P g or the iteration number is larger than n max;
Step 304, based on step 303, if the current point cloud number P n is smaller than the downsampled minimum point cloud number P min, using P n-1 to replace P n as the current point cloud number, and simultaneously using the point cloud obtained through n-1 times of filtering to replace the point cloud obtained through n times of filtering and output as the target point cloud, and if P n is larger than P min, outputting the target point cloud obtained through n times of filtering to ensure that the output target point cloud number is always larger than P min.
The calculation process of the maximum distance D max in step 301 is as follows:
solving the maximum value x max,ymax,zmax and the minimum value x min,ymin,zmin of the point cloud data x, y and z on three coordinate axes, and calculating the side length of the point cloud bounding box to ：Ix＝xmax-xmin,Iy＝ymax-ymin,Iz＝zmax-zmin;
The method for calculating the initial voxel side length Step 1 and the iteratively-increased grid voxel length Step n comprises the following steps:
Wherein D max is the maximum distance in the point cloud set, step 1 is the grid voxel side length of initial downsampling, step n is the grid voxel length after n-th iteration growth, n is the current iteration number, n max is the maximum iteration number, P g is the target point number after downsampling, P min is the minimum point number after downsampling, P n is the point number after n-time filtering, f (·) represents a filter, and K is the Step size coefficient.
The process of substituting the parameter Step 1、Stepn into the voxel filter for filtering in Step 302 is as follows:
Step 3021, setting the side length of each grid voxel as r, and dividing the point cloud data into a D x×Dy×Dz voxel grid:
Wherein, I x、Iy、Iz is the side length of the point cloud bounding box on three coordinate axes of x, y and z, and x max、ymax、zmax and x min、ymin、zmin are the maximum value and the minimum value of the point cloud data of x, y and z on the three coordinate axes respectively;
step 3022, calculating an Index for each point within the voxel grid:
Step 3023, sorting the points in the Index from small to large to calculate the barycentric point of each voxel grid, and replacing all the points in the voxel grid with the barycentric point, wherein the calculation process of the barycentric point is as follows:
Wherein X, Y, Z is the center of gravity point coordinate, m is the number of point clouds in the grid, x i、yi、zi is the point cloud coordinate in the grid, and i is the point cloud sequence number in the grid.
And 4, selecting a plane point set and an angle point set from the non-grounding point set from the preprocessed grounding point set.
The method comprises the steps of carrying out feature matching between continuous frames by using corner points and plane points as feature points, namely matching the feature points of the current frame with the feature points in historical frames to obtain feature point pairs, constructing a nonlinear optimization problem about pose by using the feature point pairs, solving the problem to obtain pose between adjacent frames, and constructing laser radar odometer data by pose data of all adjacent frames, wherein the position and orientation of the laser radar odometer data describe the change of a three-dimensional laser radar in a map coordinate system along with time.
Step 6, integrating laser radar odometer data and map information data provided by GNSS to generate a factor graph, optimizing the factor graph by using a graph optimization method to obtain final pose estimation, and generating a three-dimensional point cloud map of the scanning area based on the final pose estimation;
The method for integrating the laser radar odometer data and the map information data provided by the GNSS to generate a factor graph, and optimizing the factor graph by using a graph optimization method comprises the following steps of:
Step 601, data integration, namely performing timestamp matching on laser radar odometer data and GNSS data;
Step 602, constructing a factor graph, wherein each laser radar measurement data point and GNSS data point corresponds to a factor, the factors represent different types of observation information, and edges in the factor graph represent the dependency relationship among different observation information;
and 603, minimizing the observation error in the factor graph by using a graph optimization method, wherein in the optimization process, the pose in the laser radar measurement data is adjusted according to the dependency relationship between the observation information in the factor graph and different observation information until the convergence condition is met, and outputting final pose estimation.
In the optimization process, the GNSS data are optimized by screening and interpolating GNSS factors to reduce errors of acquired GNSS data, as shown in fig. 4, namely, a frame of image is selected at intervals to serve as a key frame, wherein each key frame is provided with longitude, latitude and altitude information of the GNSS factors, if no GNSS factors exist after the selected key frame, the next new GNSS data input is waited, otherwise, the timestamp of the key frame is compared with the timestamp of the front GNSS factor and the timestamp of the rear GNSS factor, if the difference value of the timestamp is smaller than a set value, such as 0.5 seconds, the longitude, latitude and altitude of the front GNSS factor and the rear GNSS factor are linearly interpolated, the obtained longitude, latitude and altitude are used as optimized GNSS factors, otherwise, the new GNSS data input is waited, and after the interpolation is finished, the optimized GNSS factors are transferred to a current map coordinate system and are inserted into a global factor graph.
The method for generating the three-dimensional point cloud map of the scanning area comprises the steps of constructing a pose map according to pose estimation, wherein nodes in the pose map represent laser radar poses of different frames, edges represent transformation relations between adjacent frames, and the pose map is used for matching point cloud data of different frames, so that the point cloud data of different frames are integrated, and a continuous three-dimensional point cloud map is generated.
To verify the effectiveness of the method of the present invention, systematic evaluations were performed in this example using sequences 00 and 07 in the KITTI dataset, all tests were performed based on the Robot Operating System (ROS). Specifically, the parameter P g is set to 11,000, P min is set to 10000, and K is set to 0.2. And meanwhile, the accuracy of the vehicle track obtained by the method is compared by using a classical LEGO-LOAM algorithm.
As shown in fig. 4, as a result of downsampling a frame of point cloud in KITTI data sets by an adaptive voxel filter, the downsampled point cloud greatly reduces the data amount while still retaining most of key points capable of describing the form of the original point cloud.
The vehicle track error results based on sequences 00 and 07 are shown in tables 1 and 2. Therefore, the method provided by the invention adopts the self-adaptive voxel filter, and can effectively reduce the error of the obtained track after optimizing the GNSS data by screening and interpolating GNSS factors.
Table 1 shows an error comparison based on KITTI dataset 01 sequence
Table 2 shows an error comparison based on KITTI dataset 07 sequences
Claims (6)
1. The laser SLAM mapping method based on the adaptive filter and the GNSS factors is characterized by comprising the following steps of:
step 1, acquiring multi-frame three-dimensional laser radar point cloud data generated by a laser radar in a scanning area;
Step 2, dividing the point cloud data into a grounding point set and a non-grounding point set after ground segmentation;
step 3, respectively preprocessing point cloud data of the grounding point set and the non-grounding point set;
Step 4, selecting a plane point set and an angle point set from the non-grounding point set from the preprocessed grounding point set;
step 5, recursively solving the pose between adjacent frames by utilizing an LOAM algorithm based on the plane point set and the corner set to obtain laser radar odometer data;
Step 6, integrating laser radar odometer data and map information data provided by GNSS to generate a factor graph, optimizing the factor graph by using a graph optimization method to obtain final pose estimation, and generating a three-dimensional point cloud map of the scanning area based on the final pose estimation;
The method for dividing the ground into a grounding point set and a non-grounding point set comprises the steps of calculating a horizontal included angle between two points according to a height difference Hz and a plane Euclidean distance D ab between the point A and the point B respectively positioned on two adjacent three-dimensional laser radar scanning lines, calculating the horizontal included angle from small to large according to the vertical angle of laser radar beams, and if the horizontal included angle is larger than a set value, the current vertical angle is less than the internal point cloud data and belongs to the grounding point, otherwise belongs to the non-grounding point, and finally realizing the ground division;
Step 3, preprocessing refers to self-adaptive voxel filtering of the grounding point concentrated point cloud data, wherein non-grounding point concentrated point cloud data is subjected to point cloud clustering segmentation processing based on Euclidean distance, and is divided into different point sets and then subjected to self-adaptive voxel filtering;
The method for recursively solving the pose of the adjacent frames by using the LOAM algorithm comprises the steps of performing feature matching between the continuous frames by taking corner points and plane points as feature points, namely, matching the feature points of the current frame with the feature points in the historical frames to obtain feature point pairs, constructing a nonlinear optimization problem about the pose by using the feature point pairs, and solving the problem to obtain the pose between the adjacent frames, wherein pose data of all the adjacent frames form laser radar odometry data, and the laser radar odometry data describe the position and orientation change of the three-dimensional laser radar in a map coordinate system along with time;
And 6, integrating the laser radar odometer data and the map information data provided by the GNSS to generate a factor graph, and optimizing the factor graph by using a graph optimization method, wherein the method for obtaining the final pose estimation comprises the following steps of:
step 601, data integration, namely performing timestamp matching on laser radar odometer data and GNSS data;
Step 602, constructing a factor graph, wherein each laser radar measurement data point and GNSS data point corresponds to a factor, the factors represent different types of observation information, and edges in the factor graph represent the dependency relationship among different observation information;
and 603, minimizing the observation error in the factor graph by using a graph optimization method, wherein in the optimization process, the pose in the laser radar measurement data is adjusted according to the dependency relationship between the observation information in the factor graph and different observation information until the convergence condition is met, and outputting final pose estimation.
2. The laser SLAM mapping method based on the adaptive filter and the GNSS factors of claim 1, wherein the method for performing adaptive voxel filtering on the point cloud data is as follows:
step 301, calculating an initial voxel side length Step 1 and a grid voxel length Step n after iterative growth according to a maximum distance D max of the input point cloud;
302, substituting a parameter Step 1,Stepn into a voxel filter for filtering processing to obtain a point number P n after downsampling, and if the point cloud number P n is more than or equal to P g and the iteration number is less than n max, recalculating the current grid voxel side length Step n and substituting the current grid voxel side length Step into the voxel filter again;
step 303, repeating the above steps until the current point cloud number P n is smaller than P g or the iteration number is larger than n max;
Step 304, based on step 303, if the current point cloud number P n is smaller than the downsampled minimum point cloud number P min, using P n-1 to replace P n as the current point cloud number, and simultaneously using the point cloud obtained through n-1 times of filtering to replace the point cloud obtained through n times of filtering and output as the target point cloud, and if P n is larger than P min, outputting the target point cloud obtained through n times of filtering to ensure that the output target point cloud number is always larger than P min.
3. The adaptive filter and GNSS factor based laser SLAM mapping method of claim 2, wherein the maximum distance D max calculation in step 301 is:
solving the maximum value x max,ymax,zmax and the minimum value x min,ymin,zmin of the point cloud data x, y and z on three coordinate axes, and calculating the side length of the point cloud bounding box to ：Ix＝xmax-xmin,Iy＝ymax-ymin,Iz＝zmax-zmin;
The method for calculating the initial voxel side length Step 1 and the iteratively-increased grid voxel length Step n comprises the following steps:
Wherein D max is the maximum distance in the point cloud set, step 1 is the grid voxel side length of initial downsampling, step n is the grid voxel length after n-th iteration growth, n is the current iteration number, n max is the maximum iteration number, P g is the target point number after downsampling, P min is the minimum point number after downsampling, P n is the point number after n-time filtering, f (·) represents a filter, and K is the Step size coefficient.
4. The adaptive filter and GNSS factor-based laser SLAM mapping method of claim 2, wherein the Step 302 of substituting the parameter Step 1、Stepn into the voxel filter for filtering is:
Step 3021, setting the side length of each grid voxel as r, and dividing the point cloud data into a D x×Dy×Dz voxel grid:
Wherein, I x、Iy、Iz is the side length of the point cloud bounding box on three coordinate axes of x, y and z, and x max、ymax、zmax and x min、ymin、zmin are the maximum value and the minimum value of the point cloud data of x, y and z on the three coordinate axes respectively;
step 3022, calculating an Index for each point within the voxel grid:
Step 3023, sorting the points in the Index from small to large to calculate the barycentric point of each voxel grid, and replacing all the points in the voxel grid with the barycentric point, wherein the calculation process of the barycentric point is as follows:
Wherein X, Y, Z is the center of gravity point coordinate, m is the number of point clouds in the grid, x i、yi、zi is the point cloud coordinate in the grid, and i is the point cloud sequence number in the grid.
5. The method of claim 1, wherein in the optimizing step 603, the GNSS data is optimized by screening and interpolating the GNSS factors to reduce errors in acquiring the GNSS data, i.e. selecting a frame of image as a key frame at intervals, wherein each key frame should include GNSS factors before and after each key frame, longitude, latitude and altitude information included in the GNSS factors, waiting for a new GNSS data input if no GNSS factor is selected after the key frame is selected, otherwise comparing the timestamp of the key frame with the timestamp of the front and rear GNSS factors, linearly interpolating the longitude, latitude and altitude of the front and rear GNSS factors if the difference between the timestamps is smaller than a set value, taking the interpolated longitude, latitude and altitude as the optimized GNSS factors, otherwise waiting for a new GNSS data input, and transferring the optimized GNSS factors under a current map coordinate system and inserting the optimized GNSS factors into a global factor after the interpolation.
6. The adaptive filter and GNSS factor-based laser SLAM mapping method of claim 1, wherein in step 6, a pose graph is constructed according to pose estimation, nodes in the pose graph represent laser radar poses of different frames, and the pose graph is used for matching point cloud data of different frames while representing a transformation relationship between adjacent frames, so that the point cloud data of different frames are integrated to generate a continuous three-dimensional point cloud map.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410254101.5A CN118209096B (en) | 2024-03-06 | 2024-03-06 | Laser SLAM mapping method based on self-adaptive filter and GNSS factors |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410254101.5A CN118209096B (en) | 2024-03-06 | 2024-03-06 | Laser SLAM mapping method based on self-adaptive filter and GNSS factors |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN118209096A CN118209096A (en) | 2024-06-18 |
| CN118209096B true CN118209096B (en) | 2024-12-13 |
Family
ID=91456627
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410254101.5A Active CN118209096B (en) | 2024-03-06 | 2024-03-06 | Laser SLAM mapping method based on self-adaptive filter and GNSS factors |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN118209096B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN119600068B (en) * | 2025-02-10 | 2025-07-25 | 浙江工业大学 | Point cloud registration method for positioning bulk cargo wharf engineering equipment and application |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113569915A (en) * | 2021-06-30 | 2021-10-29 | 广西大学 | Multi-strategy rail transit obstacle identification method based on laser radar |
| CN116452763A (en) * | 2023-04-17 | 2023-07-18 | 浙江大学 | Three-dimensional point cloud map construction method based on error Kalman filtering and factor graph |
| CN117269977A (en) * | 2023-11-23 | 2023-12-22 | 湖南大学 | Laser SLAM implementation method and system based on vertical optimization |
-
2024
- 2024-03-06 CN CN202410254101.5A patent/CN118209096B/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113569915A (en) * | 2021-06-30 | 2021-10-29 | 广西大学 | Multi-strategy rail transit obstacle identification method based on laser radar |
| CN116452763A (en) * | 2023-04-17 | 2023-07-18 | 浙江大学 | Three-dimensional point cloud map construction method based on error Kalman filtering and factor graph |
| CN117269977A (en) * | 2023-11-23 | 2023-12-22 | 湖南大学 | Laser SLAM implementation method and system based on vertical optimization |
Also Published As
| Publication number | Publication date |
|---|---|
| CN118209096A (en) | 2024-06-18 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113516664B (en) | Visual SLAM method based on semantic segmentation dynamic points | |
| CN114862932B (en) | BIM global positioning-based pose correction method and motion distortion correction method | |
| CN114659514B (en) | A LiDAR-IMU-GNSS fusion positioning method based on voxel-based precise registration | |
| CN118209096B (en) | Laser SLAM mapping method based on self-adaptive filter and GNSS factors | |
| CN116433737A (en) | Method and device for registering laser radar point cloud and image and intelligent terminal | |
| CN114563000B (en) | Indoor and outdoor SLAM method based on improved laser radar odometer | |
| CN115222884A (en) | Space object analysis and modeling optimization method based on artificial intelligence | |
| CN117078870A (en) | Road environment three-dimensional reconstruction method integrating high-precision map and laser sparse point cloud | |
| CN115128628A (en) | Construction method of road grid map based on laser SLAM and monocular vision | |
| CN116385892A (en) | Digital elevation model extraction method based on target context convolution neural network | |
| CN117392237A (en) | A robust lidar-camera self-calibration method | |
| CN117471439A (en) | External parameter calibration method for ship lock forbidden region non-overlapping vision field laser radar | |
| CN108595373B (en) | An uncontrolled DEM registration method | |
| CN110688440B (en) | A Map Fusion Method Applicable to Less Submap Overlaps | |
| CN114549779B (en) | Scene model reconstruction method, device, electronic device and storage medium | |
| CN114137562B (en) | Multi-target tracking method based on improved global nearest neighbor | |
| CN118518093B (en) | Laser SLAM method, device and medium based on multi-frame space occupancy | |
| CN117710603B (en) | Unmanned aerial vehicle image three-dimensional building modeling method under constraint of linear geometry | |
| CN117541614B (en) | Space non-cooperative target close-range relative pose tracking method based on improved ICP algorithm | |
| CN118674790A (en) | Laser radar-camera online calibration method based on cross-mode mask matching | |
| CN118887263A (en) | Automatic combined point cloud registration system and method | |
| Meng et al. | Section-LIO: A high accuracy LiDAR-inertial odometry using undistorted sectional point | |
| CN114742898B (en) | A laser radar and camera joint calibration method and system | |
| CN116168219A (en) | High-precision automatic matching method for remote sensing images | |
| CN115719370A (en) | Public road laser feature matching positioning method, system and storage medium |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |