A laser SLAM mapping method based on adaptive filter and GNSS factor

Abstract

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

[0001] The present invention relates to the technical field of laser radar positioning and map construction, and in particular to a laser SLAM mapping method based on an adaptive filter and a GNSS factor. Background Art

[0002] With the rapid development of 3D laser scanning technology, laser SLAM (Simultaneous Localization and Mapping, SLAM) technology has been widely used in the fields of robots, autonomous vehicles, etc. Through the laser SLAM algorithm, autonomous vehicles can realize incremental map construction by sequentially registering point cloud data collected at different positions, and then match the environment scanned in real time at their own position with the map information provided by GNSS to achieve the purpose of optimizing the current pose estimation and map.

[0003] During the LiDAR data collection process, the LiDAR scans the surrounding environment and generates a large number of data points, which constitute the so-called point cloud. Due to the wide scanning range and high resolution of LiDAR, it can capture very detailed information in the environment, but this also means that a large amount of data is generated, which needs to be processed by computers. The process of processing this large amount of data consumes a lot of computing resources, thereby reducing the processing speed.

[0004] To solve this problem, it is usually necessary to downsample the point cloud data, that is, select a portion of the data points from the original data for processing to reduce the amount of data, thereby increasing the processing speed. However, traditional voxel filters (a commonly used downsampling method) usually have a fixed grid size and cannot adaptively adjust the filter parameters according to the density of the current point cloud. This means that even when the point cloud data is already sparse, the filter may still use the same grid size for filtering, which may cause important feature points to be incorrectly filtered out, thereby losing important information in the point cloud data.

[0005] In addition, when using the Global Navigation Satellite System (GNSS) factor to optimize the lidar data, the GNSS factor represents the positioning information of the robot or vehicle at a specific moment. If there is a large time difference between the time when the GNSS factor is obtained and the time of the lidar key frame, then this GNSS factor may not accurately reflect the pose at the key frame moment. This is because during this period of time, the robot or vehicle may have moved a certain distance, resulting in a mismatch between the GNSS factor and the pose of the key frame. This mismatch will affect the accuracy of the pose estimation during the optimization process, thereby affecting the overall accuracy of the vehicle trajectory. Summary of the invention

[0006] Purpose of the invention: The purpose of the present invention is to provide a laser SLAM mapping method based on adaptive filters and GNSS factors with low computational complexity and high mapping accuracy.

[0007] Technical solution: To achieve the above purpose, the laser SLAM mapping method based on adaptive filter and GNSS factor described in the present invention comprises the following steps:

[0008] Step 1: Obtain multi-frame 3D lidar point cloud data generated by the lidar in the scanning area;

[0009] Step 2: After ground segmentation, the point cloud data is divided into a ground point set and a non-ground point set;

[0010] Step 3: Preprocess the point cloud data of the docking point set and the non-grounding point set respectively;

[0011] Step 4: Select the plane point set and the corner point set from the preprocessed ground point set and the non-ground point set;

[0012] Step 5: Based on the plane point set and the corner point set, the LOAM algorithm is used to recursively solve the pose between adjacent frames to obtain the lidar odometer data;

[0013] Step 6: Integrate the lidar odometer data and the map information data provided by GNSS to generate a factor graph, optimize the factor graph using a graph optimization method to obtain a final pose estimate, and generate a three-dimensional point cloud map of the scanning area based on the final pose estimate.

[0014] The method for segmenting the ground into a ground point set and a non-ground point set in step 2 is as follows: according to the height difference Hz between point A and point B located on two adjacent 3D laser radar scanning lines, the plane Euclidean distance D ab , calculate the horizontal angle between the two points: θ = arctan(H z / D ab ); Calculate the horizontal angle of the point cloud data according to the vertical angle of the laser radar beam from small to large. If the horizontal angle is greater than the set value, the point cloud data within the current vertical angle belongs to the ground point, otherwise it belongs to the non-ground point, and finally realize the ground segmentation.

[0015] Among them, the preprocessing described in step 3 refers to performing adaptive voxel filtering on the point cloud data in the ground point concentration, and first performing point cloud clustering and segmentation processing based on Euclidean distance on the point cloud data in the non-ground point concentration, and then performing adaptive voxel filtering after being divided into different point sets.

[0016] The method for performing adaptive voxel filtering on the point cloud data is as follows:

[0017] Step 301: According to the maximum distance D of the input point cloud max, calculate the initial voxel edge length Step1, the grid voxel length Step after iterative growth n ;

[0018] Step 302: Set the parameters Step1, Step n Substitute into the voxel filter for filtering and obtain the number of points P after downsampling n , if the point cloud number P n Greater than or equal to P g And the number of iterations is less than n max , then recalculate the current grid voxel side length Step n , substituted into the voxel filter again;

[0019] Step 303: Repeat the above steps until the current point cloud number P n Less than P g or the number of iterations is greater than n max ;

[0020] Step 304: Based on step 303, if the current point cloud number P n Less than the minimum number of point clouds P after downsampling min , then use P n-1 Instead of P n As the current point cloud number, the point cloud obtained by filtering for n-1 times replaces the point cloud obtained by filtering for n times and outputs it as the target point cloud; if P n Greater than P min , then the target point cloud obtained by the nth filtering is output to ensure that the number of output target point clouds is always greater than P min .

[0021] Wherein, the maximum distance D in step 301 max The calculation process is:

[0022] Find the maximum value x on the three coordinate axes of point cloud data x, y, and z max ,y max , z max and the minimum value x min ,y min , z min ; Calculate the side length of the point cloud bounding box: I x =x max -x min , I y =y max -y min , I z =z max -z min ;but

[0023] The calculation of the initial voxel side length Step1 and the grid voxel length after iterative growth Step n The method is:

[0024]

[0025] Among them, D max is the maximum distance in the point cloud set; Step 1 is the length of the initial downsampled grid voxel; Step n is the length of the grid voxel after the nth iteration; n is the current iteration number; n max is the maximum number of iterations; P g is the target number of points after downsampling; P min is the minimum number of points after downsampling; P n is the number of points after n filtering; f(·) represents the filter; K is the step coefficient.

[0026] In step 302, the parameters Step1, Step n The process of substituting into the voxel filter for filtering is:

[0027] Step 3021: Set the side length of each grid voxel to r, and divide the point cloud data into D x ×D y ×D z Individual voxel grids:

[0028]

[0029] Among them, I x ,I y ,I z is the side length of the point cloud bounding box on the x, y, and z coordinate axes, x max ,y max 、z max and x min ,y min 、z min They are the maximum and minimum values ​​of the point cloud data x, y, and z on the three coordinate axes;

[0030] Step 3022: Calculate the index of each point in the voxel grid:

[0031]

[0032] Step 3023: Sort the points in the index Index from small to large to calculate the centroid of each voxel grid, and use the centroid to replace all points in the voxel grid. The calculation process of the centroid is:

[0033]

[0034] Where X, Y, and Z are the coordinates of the center of gravity, m is the number of point clouds in the grid, and x i ,y i 、z i is the point cloud coordinate within the grid, and i is the point cloud sequence number within the grid.

[0035] Among them, the method of using the LOAM algorithm to recursively solve the posture between adjacent frames in step 5 is: using corner points and plane points as feature points to perform feature matching between consecutive frames, that is, matching the feature points of the current frame with the feature points in the historical frame to obtain feature point pairs; using the feature point pairs to construct a nonlinear optimization problem about the posture, and by solving the problem, the posture between adjacent frames is obtained; the posture data of all adjacent frames constitute the lidar odometer data, and the lidar odometer data describes the changes in the position and orientation of the three-dimensional lidar in the map coordinate system over time.

[0036] Among them, in step 6, the laser radar odometer data and the map information data provided by GNSS are integrated to generate a factor graph, and the factor graph is optimized using a graph optimization method to obtain the final pose estimation method as follows:

[0037] Step 601: Data integration, matching the timestamps of the LiDAR odometer data and the GNSS data, that is;

[0038] Step 602: construct a factor graph, in which each laser radar measurement data point and GNSS data point corresponds to a factor, the factor represents different types of observation information, and the edges in the factor graph represent the dependency relationship between different observation information;

[0039] Step 603: Use a graph optimization method to minimize the observation error in the factor graph. During the optimization process, the pose in the lidar measurement data will be adjusted according to the observation information in the factor graph and the dependency between different observation information until the convergence condition is met, and the final pose estimate is output.

[0040] Among them, in the optimization process described in step 603, the GNSS data is optimized by screening and interpolating GNSS factors to reduce the error of obtaining GNSS data, that is, a frame of image is selected as a key frame at regular intervals, wherein each key frame should contain GNSS factors before and after, and the GNSS factors contain longitude, latitude and altitude information; if there is no GNSS factor after the selected key frame, wait for the next new GNSS data input; otherwise, compare the timestamp of the key frame with the timestamps of the previous and next GNSS factors, if the timestamp difference is less than the set value, then linearly interpolate the longitude, latitude and altitude of the previous and next GNSS factors, and use the interpolated longitude, latitude and altitude as the optimized GNSS factor; otherwise, wait for new GNSS data to be input; after the interpolation is completed, transfer the optimized GNSS factor to the current map coordinate system and insert it into the global factor graph.

[0041] Among them, the method for generating the three-dimensional point cloud map of the scanning area in step 6 is: constructing a pose graph based on pose estimation, the nodes in the pose graph represent the lidar poses of different frames, and the edges represent the transformation relationship between adjacent frames. The pose graph is used to match the point cloud data of different frames, thereby integrating the point cloud data of different frames to generate a continuous three-dimensional point cloud map.

[0042] Beneficial effects: The present invention has the following advantages: 1. The adaptive voxel filter used in the method can dynamically adjust the grid size according to the local density of the point cloud data. In areas with denser point clouds, a smaller grid size can be used to retain more details. In areas with sparser point clouds, a larger grid size can be used to reduce the amount of calculation and filter out noise. This adaptive adjustment helps to reduce unnecessary data processing and the amount of calculation while retaining the feature points in important point cloud data;

[0043] 2. This method first compares the GNSS factor and the timestamp of the lidar key frame to ensure that the time difference between the GNSS factor and the key frame moment is within an acceptable range, so as to ensure that the GNSS factor can accurately reflect the posture at the key frame moment. For those GNSS factors with a large time difference between the timestamp and the key frame moment, this method uses linear interpolation technology to process them. Through this method, an approximate GNSS posture can be obtained at the key frame moment, thereby reducing the posture mismatch problem caused by the time difference and improving the accuracy of the vehicle trajectory. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] Figure 1 It is a schematic diagram of the process of the present invention;

[0045] Figure 2 Schematic diagram of point A and point B on two adjacent 3D laser radar scanning lines;

[0046] Figure 3 is the adaptive voxel filtering flow chart;

[0047] Figure 4 A flowchart for optimizing GNSS data by screening and interpolating GNSS factors;

[0048] Figure 5 Figure 2 shows the point cloud data before and after filtering using an adaptive voxel filter. DETAILED DESCRIPTION

[0049] The technical solution of the present invention is described in detail below in conjunction with the embodiments and drawings.

[0050] like Figure 1 As shown, a laser SLAM mapping method based on an adaptive filter and a GNSS factor comprises the following steps:

[0051] Step 1: Obtain multi-frame 3D lidar point cloud data generated by the lidar in the scanning area;

[0052] Step 2: After ground segmentation, the point cloud data is divided into a ground point set and a non-ground point set; the method is: according to the height difference Hz between point A and point B located on two adjacent 3D laser radar scanning lines, the plane Euclidean distance D ab , calculate the horizontal angle between the two points: θ = arctan(H z / D ab ); Calculate the horizontal angle of the point cloud data according to the vertical angle of the laser radar beam from small to large. If the horizontal angle is greater than the set value, the point cloud data within the current vertical angle belongs to the ground point, otherwise it belongs to the non-ground point, and finally realize the ground segmentation, such as Figure 2 shown.

[0053] Step 3: Preprocess the point cloud data of the grounding point set and the non-grounding point set respectively; the preprocessing refers to adaptive voxel filtering of the point cloud data obtained by scanning each frame of the laser radar; the point cloud data in the grounding point set is directly subjected to adaptive voxel filtering, and the point cloud data in the non-grounding point set is first subjected to point cloud clustering and segmentation processing based on Euclidean distance, and then adaptive voxel filtering is performed after being divided into different point sets.

[0054] like Figure 3 As shown, the method for performing adaptive voxel filtering on the point cloud data is:

[0055] Step 301: According to the maximum distance D of the input point cloud max , calculate the initial voxel edge length Step1, the grid voxel length Step after iterative growth n ;

[0056] Step 302: Set the parameters Step1, Step nSubstitute into the voxel filter for filtering and obtain the number of points P after downsampling n , if the point cloud number P n Greater than or equal to P g And the number of iterations is less than n max , then recalculate the current grid voxel side length Step n , substituted into the voxel filter again;

[0057] Step 303: Repeat the above steps until the current point cloud number P n Less than P g or the number of iterations is greater than n max ;

[0058] Step 304: Based on step 303, if the current point cloud number P n Less than the minimum number of point clouds P after downsampling min , then use P n-1 Instead of P n As the current point cloud number, the point cloud obtained by filtering for n-1 times replaces the point cloud obtained by filtering for n times and outputs it as the target point cloud; if P n Greater than P min , then the target point cloud obtained by the nth filtering is output to ensure that the number of output target point clouds is always greater than P min .

[0059] Wherein, the maximum distance D in step 301 max The calculation process is:

[0060] Find the maximum value x on the three coordinate axes of point cloud data x, y, and z max ,y max , z max and the minimum value x min ,y min , z min ; Calculate the side length of the point cloud bounding box: I x =x max -x min , I y =y max -y min , I z =z max -z min ;but

[0061] The calculation of the initial voxel side length Step1 and the grid voxel length after iterative growth Step n The method is:

[0062]

[0063] Among them, D maxis the maximum distance in the point cloud set; Step 1 is the length of the initial downsampled grid voxel; Step n is the length of the grid voxel after the nth iteration; n is the current iteration number; n max is the maximum number of iterations; P g is the target number of points after downsampling; P min is the minimum number of points after downsampling; P n is the number of points after n filtering; f(·) represents the filter; K is the step coefficient.

[0064] In step 302, the parameters Step1, Step n The process of substituting into the voxel filter for filtering is:

[0065] Step 3021: Set the side length of each grid voxel to r, and divide the point cloud data into D x ×D y ×D z Individual voxel grids:

[0066]

[0067] Among them, I x ,I y ,I z is the side length of the point cloud bounding box on the x, y, and z coordinate axes, x max ,y max 、z max and x min ,y min 、z min They are the maximum and minimum values ​​of the point cloud data x, y, and z on the three coordinate axes;

[0068] Step 3022: Calculate the index of each point in the voxel grid:

[0069]

[0070] Step 3023: Sort the points in the index Index from small to large to calculate the centroid of each voxel grid, and use the centroid to replace all points in the voxel grid. The calculation process of the centroid is:

[0071]

[0072] Where X, Y, and Z are the coordinates of the center of gravity, m is the number of point clouds in the grid, and x i ,y i 、z i is the point cloud coordinate within the grid, and i is the point cloud sequence number within the grid.

[0073] Step 4: Select the plane point set and the corner point set from the preprocessed ground point set.

[0074] Step 5: Based on the plane point set and the corner point set, the LOAM algorithm is used to recursively solve the pose between adjacent frames to obtain the lidar odometer data; the method is: the corner points and plane points are used as feature points to perform feature matching between consecutive frames, that is, the feature points of the current frame are matched with the feature points in the historical frame to obtain feature point pairs; a nonlinear optimization problem about the pose is constructed using the feature point pairs, and the pose between adjacent frames is obtained by solving the problem; the pose data of all adjacent frames constitute the lidar odometer data, and the lidar odometer data describes the changes in the position and orientation of the three-dimensional lidar in the map coordinate system over time.

[0075] Step 6: Integrate the laser radar odometer data and the map information data provided by GNSS to generate a factor graph, optimize the factor graph using a graph optimization method to obtain a final pose estimate, and generate a three-dimensional point cloud map of the scan area based on the final pose estimate;

[0076] The method of 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, and obtaining the final pose estimation is as follows:

[0077] Step 601: Data integration, matching the timestamps of the LiDAR odometer data and the GNSS data, that is;

[0078] Step 602: construct a factor graph, in which each laser radar measurement data point and GNSS data point corresponds to a factor, the factor represents different types of observation information, and the edges in the factor graph represent the dependency relationship between different observation information;

[0079] Step 603: Use a graph optimization method to minimize the observation error in the factor graph. During the optimization process, the pose in the lidar measurement data will be adjusted according to the observation information in the factor graph and the dependency between different observation information until the convergence condition is met, and the final pose estimate is output.

[0080] In the optimization process, the GNSS data is optimized by screening and interpolating GNSS factors to reduce the error of obtaining GNSS data, such as Figure 4As shown, a frame of image is selected as a key frame at regular intervals, and each key frame should contain GNSS factors before and after, and the GNSS factors contain longitude, latitude and altitude information; if there is no GNSS factor after the selected key frame, wait for the next new GNSS data input; otherwise, compare the timestamp of the key frame with the timestamps of the previous and next GNSS factors. If the timestamp difference is less than the set value, such as 0.5 seconds, the longitude, latitude and altitude of the previous and next GNSS factors are linearly interpolated, and the interpolated longitude, latitude and altitude are used as the optimized GNSS factors; otherwise, wait for new GNSS data input; after the interpolation is completed, the optimized GNSS factors are transferred to the current map coordinate system and inserted into the global factor graph.

[0081] Among them, the method for generating the three-dimensional point cloud map of the scanning area is: constructing a pose graph based on pose estimation, the nodes in the pose graph represent the lidar poses of different frames, and the edges represent the transformation relationship between adjacent frames. The pose graph is used to match the point cloud data of different frames, thereby integrating the point cloud data of different frames to generate a continuous three-dimensional point cloud map.

[0082] To verify the effectiveness of the method of the present invention, a systematic evaluation was performed using sequences 00 and 07 in the KITTI dataset in this embodiment, and all tests were performed based on the Robot Operating System (ROS). Specifically, the parameter P is set g 11,000, P min is 10000, and K is 0.2. The classic LEGO-LOAM algorithm is also used to compare the accuracy of the vehicle trajectory obtained by the method of the present invention.

[0083] like Figure 4 As shown in Figure 1, a frame of point cloud in the KITTI dataset is downsampled by an adaptive voxel filter. The downsampled point cloud greatly reduces the amount of data while still retaining most of the key points that can describe the morphology of the original point cloud.

[0084] The vehicle trajectory error results based on sequences 00 and 07 are shown in Tables 1 and 2. It can be seen that the method proposed in the present invention can effectively reduce the error of the obtained trajectory by optimizing the GNSS data by using an adaptive voxel filter and screening and interpolating GNSS factors.

[0085] Table 1 shows the error comparison based on the KITTI dataset 01 sequence

[0086]

[0087] Table 2 shows the error comparison based on the KITTI dataset 07 sequence

[0088]

Claims

1. A laser SLAM mapping method based on adaptive filter and GNSS factor, characterized in that: The following steps are involved: Step 1: Obtain multi-frame 3D lidar point cloud data generated by the lidar in the scanning area; Step 2: After ground segmentation, the point cloud data is divided into a ground point set and a non-ground point set; Step 3: Preprocess the point cloud data of the docking point set and the non-grounding point set respectively; Step 4: Select the plane point set and the corner point set from the preprocessed ground point set and the non-ground point set; Step 5: Based on the plane point set and the corner point set, the LOAM algorithm is used to recursively solve the pose between adjacent frames to obtain the lidar odometer data; Step 6: Integrate the laser radar odometer data and the map information data provided by GNSS to generate a factor graph, optimize the factor graph using a graph optimization method to obtain a final pose estimate, and generate a three-dimensional point cloud map of the scan area based on the final pose estimate; The method for segmenting the ground into a ground point set and a non-ground point set in step 2 is: according to the height difference Hz between point A and point B located on two adjacent 3D laser radar scanning lines, the plane Euclidean distance D ab , calculate the horizontal angle between the two points: θ = arctan(H z / D ab ); Calculate the horizontal angle of the point cloud data according to the vertical angle of the laser radar beam from small to large. If the horizontal angle is greater than the set value, the point cloud data within the current vertical angle belongs to the ground point, otherwise it belongs to the non-ground point, and finally realize the ground segmentation; The preprocessing in step 3 refers to performing adaptive voxel filtering on the point cloud data in the ground point concentration, and performing point cloud clustering and segmentation processing based on the Euclidean distance on the point cloud data in the non-ground point concentration, and then performing adaptive voxel filtering after being divided into different point sets; The method of recursively solving the pose between adjacent frames using the LOAM algorithm in step 5 is: using corner points and plane points as feature points to perform feature matching between consecutive frames, that is, matching the feature points of the current frame with the feature points in the historical frame to obtain feature point pairs; A nonlinear optimization problem about the posture is constructed using the feature point pairs, and the posture between adjacent frames is obtained by solving the problem; the posture data of all adjacent frames constitute the laser radar odometer data, and the laser radar odometer data describes the change of the position and orientation of the three-dimensional laser radar in the map coordinate system over time; In step 6, the lidar odometer data and the map information data provided by GNSS are integrated to generate a factor graph, and the factor graph is optimized using a graph optimization method to obtain the final pose estimation method as follows: Step 601: Data integration, matching the timestamps of the LiDAR odometer data and the GNSS data; Step 602: construct a factor graph, in which each laser radar measurement data point and GNSS data point corresponds to a factor, the factor represents different types of observation information, and the edges in the factor graph represent the dependency relationship between different observation information; Step 603: Use a graph optimization method to minimize the observation error in the factor graph. During the optimization process, the pose in the lidar measurement data will be adjusted according to the observation information in the factor graph and the dependency between different observation information until the convergence condition is met, and the final pose estimate is output.

2. The laser SLAM mapping method based on adaptive filter and GNSS factor according to claim 1, characterized in that: The method for performing adaptive voxel filtering on the point cloud data is: Step 301: According to the maximum distance D of the input point cloud max , calculate the initial voxel edge length Step1, the grid voxel length Step after iterative growth n ; Step 302: Set the parameters Step1, Step n Substitute into the voxel filter for filtering and obtain the number of points P after downsampling n , if the point cloud number P n Greater than or equal to P g And the number of iterations is less than n max , then recalculate the current grid voxel side length Step n , substituted into the voxel filter again; Step 303: Repeat the above steps until the current point cloud number P n Less than P g or the number of iterations is greater than n max ; Step 304: Based on step 303, if the current point cloud number P n Less than the minimum number of point clouds P after downsampling min , then use P n-1 Instead of P n As the current point cloud number, the point cloud obtained by filtering for n-1 times replaces the point cloud obtained by filtering for n times and outputs it as the target point cloud; if P n Greater than P min , then the target point cloud obtained by the nth filtering is output to ensure that the number of output target point clouds is always greater than P min .

3. The laser SLAM mapping method based on adaptive filter and GNSS factor according to claim 2, characterized in that: The maximum distance D in step 301 max The calculation process is: Find the maximum value x on the three coordinate axes of point cloud data x, y, and z max ,y max , z max and the minimum value x min ,y min , z min ; Calculate the side length of the point cloud bounding box: I x =x max -x min , I y =y max -y min , I z =z max -z min ;but The calculation of the initial voxel side length Step1 and the grid voxel length after iterative growth Step n The method is: Among them, D max is the maximum distance in the point cloud set; Step 1 is the length of the initial downsampled grid voxel; Step n is the length of the grid voxel after the nth iteration; n is the current iteration number; n max is the maximum number of iterations; P g is the target number of points after downsampling; P min is the minimum number of points after downsampling; P n is the number of points after n filtering; f(·) represents the filter; K is the step coefficient.

4. The laser SLAM mapping method based on adaptive filter and GNSS factor according to claim 2, characterized in that: Step 302: Set the parameters Step1, Step n The process of substituting into the voxel filter for filtering is: Step 3021: Set the side length of each grid voxel to r, and divide the point cloud data into D x ×D y ×D z Individual voxel grids: Among them, I x ,I y ,I z is the side length of the point cloud bounding box on the x, y, and z coordinate axes, x max ,y max 、z max and x min ,y min 、z min They are the maximum and minimum values ​​of the point cloud data x, y, and z on the three coordinate axes; Step 3022: Calculate the index of each point in the voxel grid: Step 3023: Sort the points in the index Index from small to large to calculate the centroid of each voxel grid, and use the centroid to replace all points in the voxel grid. The calculation process of the centroid is: Where X, Y, and Z are the coordinates of the center of gravity, m is the number of point clouds in the grid, and x i ,y i 、z i is the point cloud coordinate within the grid, and i is the point cloud sequence number within the grid.

5. The laser SLAM mapping method based on adaptive filter and GNSS factor according to claim 1, characterized in that: In the optimization process of step 603, the GNSS data is optimized by screening and interpolating the GNSS factors to reduce the error of obtaining the GNSS data, that is, a frame of image is selected as a key frame at a certain interval, wherein each key frame should contain GNSS factors before and after, and the GNSS factors contain longitude, latitude and altitude information; if there is no GNSS factor after the selected key frame, wait for the next new GNSS data input; Otherwise, compare the timestamp of the key frame with the timestamps of the previous and next GNSS factors. If the timestamp difference is less than the set value, linearly interpolate the longitude, latitude and altitude of the previous and next GNSS factors, and use the interpolated longitude, latitude and altitude as the optimized GNSS factor; otherwise, wait for new GNSS data to be input; after the interpolation is completed, transfer the optimized GNSS factor to the current map coordinate system and insert it into the global factor graph.

6. The laser SLAM mapping method based on adaptive filter and GNSS factor according to claim 1, characterized in that: The method for generating the three-dimensional point cloud map of the scanning area described in step 6 is: constructing a pose graph based on pose estimation, the nodes in the pose graph represent the lidar poses of different frames, and the edges represent the transformation relationship between adjacent frames. The pose graph is used to match the point cloud data of different frames, thereby integrating the point cloud data of different frames to generate a continuous three-dimensional point cloud map.

Images