Distribution adaptive surface element map implementation method for improving performance of laser inertial odometer

Abstract

The invention discloses a distributed adaptive surface element map implementation method for improving the performance of a laser inertial odometer, which comprises the following steps of: uniformly dividing a map space into latticed voxels, and storing pointers for forming sampling points of a surface element map in the voxels, the voxels stored with the sampling point pointers are projected into a one-dimensional array through a spatial hash function, so that a voxel hash data structure is formed; a sampling point is a randomly generated spherical space with a radius of r, a central point of the spherical space is used as a position identifier of the sampling point, the spherical space is used as a statistical range of point cloud data obtained by laser radar scanning, and point cloud is modeled as Gaussian distribution; each sampling point on the surface element map corresponds to one surface element, and the pointer of each sampling point is stored in the voxel where the central point of the sampling point is located. According to the surface element map, the positions of the surface element sampling points can be adaptively adjusted according to the point cloud distribution condition, the risk of improper point cloud division is reduced, and the plane estimation precision is improved.

Description

Technical Field

[0001] The present invention relates to the field of path planning, and particularly to a method for implementing a distribution adaptive surface element map for improving the performance of a lidar inertial odometer. Background Art

[0002] Point cloud registration is the core algorithm module of a lidar inertial odometer, and its accuracy and efficiency are crucial. The ICP method is still the main method for point cloud registration at present, mainly including point-to-point ICP, point-to-plane (line) ICP, plane-to-plane ICP and other forms. The latter two ICP forms have become the first choice for many excellent works recently due to their excellent registration accuracy and robustness. In these two ICP forms, a key problem is how to quickly obtain an accurate local plane reference. Currently, there are two most popular methods. The first is to use a point cloud map, and the representative work is Fast-LIO2. When constructing the point-to-plane distance cost factor in Fast-LIO2, the local map points are fitted to a plane to obtain the plane reference. This method has the following disadvantages:

[0003] 1. To ensure locality and computational efficiency, usually only a small number of map points are used for plane fitting, and the estimation accuracy of the local plane is not high.

[0004] 2. Local plane fitting needs to be performed every time the cost factor is constructed, and there is a large amount of computational redundancy in frequent local plane fitting, which affects the computational efficiency.

[0005] Another method is to establish a surface element map. The surface element map describes the environment as independent surface elements, and each surface element corresponds to a plane. Compared with the point cloud map, the surface element map can make full use of the information of each scan and fit a more accurate plane from a large number of points. At the same time, the surface element can directly provide the plane reference without frequently performing plane fitting, thereby improving the efficiency of cost factor construction. The mainstream method for establishing a surface element map is the voxel extraction method:

[0006] ① Divide the map neatly into ordered voxel grids;

[0007] ② The map continuously receives lidar scan frame inputs, and the scan points are added to the corresponding voxel grids in sequence according to their positions;

[0008] ③ The voxel grids with new points input update the plane parameters.

[0009] ④ Each voxel grid corresponds to a surface element, and the ordered voxels support fast map access.

[0010] However, this voxel-surface element map also has disadvantages:

[0011] 1. The voxel grids with fixed distributions are difficult to handle randomly distributed point clouds, which may lead to small volumes of point clouds within some voxels and anisotropic polyhedrons in the shapes of point clouds, affecting the estimation accuracy of plane parameters.

[0012] 2. The size and density of voxels are coupled with each other. When maintaining a suitable voxel size to support good plane estimation, it is impossible to increase the voxel density to improve the detail level of environmental description.

[0013] 3. During the odometer movement, the plane parameters need to be updated when a voxel receives new points. Each time an update is performed, all point cloud information needs to be re - counted and the plane is fitted, resulting in low efficiency of facet update.

[0014] The first two points affect the positioning accuracy of the laser - inertial odometer, and the third point affects the mapping efficiency of the odometer. Summary of the Invention

[0015] To solve the problems that the current voxel - facet maps have low facet accuracy and density and cannot provide an accurate and detailed environmental description. At the same time, their facet update methods are inefficient, greatly reducing the mapping efficiency. The present invention proposes a new form of facet map to solve the problems of voxel - facet maps while maintaining the advantages of facet maps compared to point cloud maps, ultimately improving the performance of the laser - inertial odometer.

[0016] To achieve the above - mentioned technical objectives, the technical solution of the present invention is as follows:

[0017] A method for implementing a distribution - adaptive facet map for improving the performance of a laser - inertial odometer, comprising the following steps:

[0018] The map space is evenly divided into grid - like voxels to store pointers to the sampling points that make up the facet map; and the voxels storing the sampling point pointers are mapped into a one - dimensional array through a spatial hash function, thereby forming a voxel hash data structure;

[0019] The sampling points are defined by defined elements, and a spherical space formed with the center point of the sampling point as the center and a radius of is used as the statistical range of the point cloud data within the sampling point, and all the point cloud data within the statistical range is modeled as a Gaussian distribution, that is, a facet; the pointer of each sampling point is respectively stored in the voxel where the center point of the sampling point is located;

[0020] When receiving the first frame of point cloud data obtained from lidar scanning, randomly select one of the scanning points as the center point to establish the first sampling point and initialize the definition of element information, and then traverse the remaining scanning points to perform voxel-based nearest neighbor search to find the nearest neighbor sampling points; when a scanning point fails to find a nearest neighbor sampling point, or finds a nearest neighbor sampling point but the distance is greater than the preset distance threshold, then create a new sampling point with this scanning point and initialize the definition of element information; when a scanning point is within the statistical range of any nearest neighbor sampling point, then classify the scanning point into this nearest neighbor sampling point and perform incremental update on the definition of element information of this nearest neighbor sampling point, thereby establishing a surface element map;

[0021] Subsequently, when receiving the point cloud data of a new scanning frame, continue to perform nearest neighbor search for each scanning point in the same way to find the nearest neighbor sampling points and determine whether to establish a new sampling point or classify it into the established sampling points, and then initialize or perform incremental update on the calculation of the definition of element information, thereby updating the surface element map.

[0022] Furthermore, the voxel storing the sampling point pointer is mapped to a one-dimensional array through a spatial hash function, and the spatial hash function used is:

[0023] ;

[0024] where represents the coordinate after the sampling point stored in the voxel is mapped to the one-dimensional array, 、 respectively represent the coordinate values of the sampling point stored in the voxel on the X-axis, Y-axis, and Z-axis in the three-dimensional coordinate system; p1, p2, and p3 are unequal large prime numbers; represents the exclusive OR operation; mod represents the modulo operation; is the size of the hash table, which is determined according to the map space size.

[0025] Furthermore, the definition of element information of the sampling point includes:

[0026] center point 、the cumulative value of the point cloud within the statistical range of the sampling point 、the auxiliary variable used to calculate the covariance matrix 、the mean value of the point cloud within the statistical range of the sampling point 、plane normal vector 、eigenvalue vector 、from the creation of the sampling point to the previous update 、 and the number of scanning points accumulated within the statistical range of the sampling point when 、from the previous update 、 and functions as the number of scan points that have been accumulated within the statistical range of the sampling point ; where and are both obtained by performing eigen decomposition by ;

[0027] Said modeling all the point cloud data within the statistical range as a Gaussian distribution means establishing a Gaussian distribution , where is obtained by calculating with and ;

[0028] Furthermore, the definitions of said and are as follows:

[0029] ;

[0030] ;

[0031] where is the set of point clouds that fall within the sampling point statistical range, and there is:

[0032] ;

[0033] The superscript T of

[0034] denotes matrix transpose; and Then, according to , it is calculated through the following formula to obtain

[0035] ;

[0036] Furthermore, the nearest neighbor search is as follows:

[0037] For each scan point , search for all sampling points within the range with as the radius, that is, the bin, where is the preset minimum distance interval between sampling points:

[0038] ① Divide each voxel into 8 uniform regions, then determine the region where is located, and the 7 directly adjacent voxels that are in direct contact with the region where is located, and form a cube-shaped search space with these 8 voxels ;

[0039] ② Traverse the search space ​​​​​​​​​​​​For the surface elements therein, calculate the center point of each surface element to the distance of the point, and compare it with and keep the surface elements with a distance less than as the near-neighbor sampling points;

[0040] where the side length of a single voxel is greater than and the mentioned as the radius range means taking the larger value of and as the radius range.

[0041] Furthermore, when creating new sampling points, the initialization definition element information includes setting and initializing , , , , , and values:

[0042] where the new scan point is set to ; is initialized to , is initialized to , the superscript T represents matrix transpose, is initialized to 1, is initialized to 0;

[0043] , and are all set to 0 when creating new sampling points, and take the in the new sampling point as the formal initialization condition. Until it is satisfied, then use the following formula to formally initialize , and :

[0044] ;

[0045] ;

[0046] ;

[0047] ;

[0048] where the superscript T represents matrix transpose;

[0049] Then for Perform eigendecomposition, and according to the result of eigendecomposition, The eigenvector corresponding to the minimum feature of , and The vector composed of the three eigenvalues of .

[0050] Furthermore, the new scanning point The definition element information of the neighboring sampling point is incrementally updated, and the definition element information of the established sampling point is updated based on the following formula:

[0051] ;

[0052] ;

[0053] ;

[0054] when 、 After the update, only when When the following formula is used to calculate 、 and The information is updated:

[0055] ;

[0056] ;

[0057] ;

[0058] ;

[0059] This enables the establishment of sampling points at the initial stage and When the uncertainty of is high, high-frequency updates are performed to improve the estimation accuracy of the plane parameters; when the sampling points tend to be stable, that is, and When a good estimate has been obtained, less frequent updates are performed to improve efficiency.

[0060] Further, 、 and The only formal initialization must be performed after the formal initialization conditions are met, and then Updated when; and in the same scan frame, 、 and Only one formal initialization or one update is performed.

[0061] Further, it also includes the step of completing the point-to-plane ICP algorithm based on the voxel map to obtain the pose of the lidar:

[0062] Through point-plane search, point-plane matching is obtained , where represents the scan point for point-plane matching in the current scan frame, represents the plane element successfully matched with ;

[0063] Then, a cost factor for the point-to-plane distance is constructed :

[0064] ;

[0065] where is the pose transformation matrix from the lidar coordinate system to the map coordinate system, and the initial value is obtained from the constant velocity model of the odometer or inertial data integration; is the observation noise; represents the plane normal vector of the plane element ; represents the point cloud mean value of the plane element within the sampling point statistical range;

[0066] Next, a least squares problem is constructed:

[0067] ;

[0068] where is the pose state variable of the lidar, represents the L2 norm of, and then this least squares problem is solved to obtain the pose of the lidar.

[0069] Further, the point-plane search includes the following steps:

[0070] 1) Each voxel is divided into 8 uniform regions, and then the region where is located, and the 7 directly adjacent voxels directly in contact with the region where is located are determined, and an 8-voxel cube-shaped search space is formed with these 8 voxels ;

[0071] 2) Traverse the plane elements in , calculate the distance from the center point of the plane element to , retain the plane element with the minimum distance and record the distance from this plane element to ; If there is no plane element in , it is determined that has no matching plane element, and the process ends ; The point-plane matching process;

[0072] 3) Calculate to the minimum distance to the surface , when , end the search process and jump to step 5; otherwise, go to step 4;

[0073] 4) Search for other facets among 19 sub-adjacent voxels. If no other facets are found, go to step 5; otherwise, calculate the distance between the found facet and and compare it with , then retain the facet with the minimum distance obtained from the comparison and go to step 5; where the sub-adjacent voxels refer to all voxels that surround the voxel where is located, with the voxel where is located as the center, and excluding ;

[0074] 5) Compare the distance between the currently retained facet with the minimum distance and with a preset distance threshold. If it is greater than the distance threshold, it is determined that has no matching facet and end 's point-plane matching process; otherwise, use the currently retained facet with the minimum distance as the matching facet .

[0075] Based on the above steps, the present invention provides a distribution adaptive facet map (DASM). This facet map can adaptively adjust the positions of facet sampling points according to the point cloud distribution, reducing the risk of improper point cloud partitioning and improving the plane estimation accuracy. In addition, the facets in DASM can obtain a more detailed environmental description in a compact arrangement form.

[0076] Aiming at the problem that the traditional facet update method is inefficient because it needs to recalculate all information, the present invention also provides a highly efficient facet incremental update method. The point cloud within the facet is modeled as a normal distribution and incremental updates are performed. The covariance matrix in the normal distribution is subjected to eigenvalue decomposition to iteratively update the plane parameters, greatly reducing the computational cost of facet updates and improving the mapping efficiency.

[0077] In the facet map of the present invention, the disordered facets are organized in an ordered voxel hash data structure, enabling fast map access. At the same time, the present invention also provides a fast nearest neighbor search method to accelerate map building and map query.

[0078] The technical effect of the present invention is that the distribution - adaptive voxel map provided by the present invention can provide a more accurate and detailed environmental description, improve the performance of the point - cloud registration algorithm, and ultimately improve the positioning accuracy of the laser - inertial odometer. The present invention also provides a highly efficient voxel incremental update method, avoiding the drawback of having to recalculate every time the voxel is updated, and greatly increasing the mapping efficiency. In addition, the present invention also provides a fast nearest - neighbor search method. Compared with the traditional 27 - voxel nearest - neighbor search method, the present invention achieves a faster search efficiency, which can effectively accelerate map building and map querying. BRIEF DESCRIPTION OF THE DRAWINGS

[0079] Figure 1 It is a structural diagram of the distribution - adaptive voxel map of the present invention.

[0080] Figure 2 It is a schematic diagram of the nearest - neighbor search of the present invention.

[0081] Figure 3 It is a comparison diagram of the effects in the embodiment of the present invention.

[0082] Figure 4 It is a schematic diagram of the process of map update in the embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0083] Refer to Figure 1 , the voxel map provided by this embodiment is composed of a large number of sampling points : Among them , each sampling point corresponds to a voxel. Each sampling point is defined by the following elements:

[0084] Center point , the cumulative value of the point cloud within the statistical range of the sampling point , the auxiliary variable used to calculate the covariance matrix , , the mean value of the point cloud within the statistical range of the sampling point , plane normal vector , eigenvalue vector , , } ( < < ), from the creation of the sampling point to the last update , and the cumulative number of scanned points within the statistical range of the sampling point , from the last update , and to the current cumulative number of scanned points within the statistical range of the sampling point Among them in represents a real number, and the superscript indicates a dimension of 3.

[0085] Center point is the unique position identifier of the sampling point and remains unchanged after initialization. Taking as the center, the spherical space with a radius of r is the statistical range of the sampling point. The sampling point models all points within the statistical range as a Gaussian distribution . Among them, the covariance matrix is not stored in the sampling point, but is calculated by and each time the plane information needs to be updated.

[0086] Suppose is the point cloud set that falls within the statistical range of a certain sampling point . Then in this embodiment, , is defined as follows:

[0087] .

[0088] .

[0089] It should be mentioned here that in this embodiment, whether it is when initializing a new sampling point pair and , or when performing incremental updates on the sampling point, there are corresponding expressions with higher efficiency to specifically calculate and . The final results are the same as the above-defined expressions. Therefore, the defined expressions here are only for definition and will not be used to perform calculations with the above two expressions during actual implementation.

[0090] is then defined by the historical points :

[0091] .

[0092] .

[0093] Eigenvalue vector and normal vector are obtained by performing eigenvalue decomposition on . Among them, the normal vector is the eigenvector corresponding to the minimum eigenvalue of , and is the vector composed of the three eigenvalues of

[0094] Since the generation of sampling points is random, this disorder will cause certain difficulties for map access. To improve the efficiency of map access, the disordered sampling points are organized in a voxel hash data structure. Specifically, the map space is divided into neatly arranged voxel grids, and the pointers of the sampling points are stored in the voxels where their center points are located. The voxel where a sampling point exists (i.e., the center point of the sampling point is within the statistical range of this voxel) will be projected into a one-dimensional array through a spatial hash function. The spatial hash function is expressed as:

[0095] ;

[0096] In the above formula, represents the coordinate after the sampling point stored in the voxel is mapped to the one-dimensional array, , respectively represent the coordinate values of the sampling point stored in the voxel on the X-axis, Y-axis, and Z-axis in the three-dimensional coordinate system. p1, p2, and p3 are large prime numbers that are not equal to each other. In this embodiment, p1, p2, and p3 are 73856093, 19349669, and 83492791 respectively, and their values can be determined according to specific implementation requirements. represents the exclusive OR operation. mod represents the remainder calculation. is the size of the hash table, which is determined according to the size of the map space.

[0097] Map building process:

[0098] When receiving the first frame of point cloud data obtained by lidar scanning, randomly select one of the scanning points in the scanning frame as the center point to establish the first sampling point and calculate the defined element information. Next, traverse the remaining scanning points to perform voxel-based nearest neighbor search to find the nearest neighbor sampling points. When a scanning point fails to find a nearest neighbor sampling point, or finds a nearest neighbor sampling point but the distance is greater than the preset distance threshold, then create a new sampling point with this scanning point and calculate the defined element information. When a scanning point is within the statistical range of any nearest neighbor sampling point, then classify the scanning point into this nearest neighbor sampling point and perform incremental update on the defined element information of this nearest neighbor sampling point, thereby establishing a facet map.

[0099] In this example, the position of the sampling point is determined by the actual scanning points, which can ensure that the sampling points are close to the actual object surface, ensure that the spherical space expanded by the sampling points can better encompass the point cloud, and avoid including the point clouds of adjacent objects to the greatest extent.

[0100] The above-mentioned nearest neighbor search can be referred to Figure 2 , which is to search for each scanning point to All sampling points within the radius range are the bins, where is the minimum distance interval between preset sampling points. By changing the value, the density of the bins in the map can be adjusted. That is, the larger value of and is used as the radius range. To ensure that the spherical space within the radius of the query point is a subset of the search space the side length of a single voxel should be greater than . The specific steps of the nearest neighbor search include:

[0101] ① Divide each voxel into 8 equal regions, that is, Figure 2 each of the leftmost small blue cubes represents a divided region. Then determine the region where it is located, which is shown in red in the figure. This red region and the other 7 blue regions form the voxel where it is located. Then determine the 7 directly adjacent voxels that are directly in contact with the region where it is located. These 7 directly adjacent voxels are shown as transparent cubes in the Figure 2 middle part. It can be seen that all these 7 transparent cubes are directly in contact with the region where it is located, including the voxel that touches the region at the vertex. Finally, these 8 voxels form a cube-shaped search space .

[0102] ② Next, traverse the bins in the search space calculate the distance from the center point of each bin to the point, and compare it with . Retain the bins with a distance less than as the nearest neighbor sampling points.

[0103] When creating new sampling points, initializing the defined element information includes setting the of the sampling points, and initializing the , , , , , and values:

[0104] Among them, the new scan point is set to ; is initialized to , is initialized to , where the superscript T represents the matrix transpose, is initialized to 1, is initialized to 0; meanwhile and and are both set to 0 when creating a new sampling point as temporary initialization values.

[0105] Then, as the nearest neighbor search is sequentially performed on each scan point in the current lidar scan frame, when 3 or more scan points are included in this sampling point, i.e., at this time, the following calculation steps are used to and and perform formal initialization:

[0106] ;

[0107] ;

[0108] ;

[0109] ;

[0110] where the superscript T represents the matrix transpose;

[0111] Then is eigen-decomposed, and according to the result of the eigen-decomposition, the eigenvector corresponding to the smallest eigenvalue of is used as , and the vector composed of the three eigenvalues of is used as .

[0112] If the new sampling point in the current scan frame fails to meet this formal initialization condition, then and and these three parameters are kept as 0, and after a new scan frame arrives, it continues to check whether the formal initialization condition is met until it is met in a certain scan frame and then formal initialization is performed. At the same time, the formal initialization of these three parameters is only performed once, and then these three parameters are updated according to the update rules in the subsequent incremental update. However, in the same scan frame, if these three parameters have already been formally initialized, then even if the incremental update rule is met again, these three parameters are no longer updated, but continue to be updated after the subsequent scan frame arrives.

[0113] When performing incremental update, the defined element information of the established sampling points is updated based on the following formula:

[0114] ;

[0115] ;

[0116] ;

[0117] And when 、 after the update, take as 、 and the update rules for these three parameters, that is, only when is true, update the information of 、 and based on the following formulas:

[0118] ;

[0119] ;

[0120] ;

[0121] ;

[0122] Thus, at the initial stage of the establishment of the sampling points, and when the uncertainties are relatively high, perform high-frequency updates to improve the estimation accuracy of the plane parameters; when the sampling points tend to be stable, that is, and have been well estimated, perform low-frequency updates to improve efficiency. This incremental update strategy does not require storing a large amount of point cloud information and recalculation, and can greatly improve the update efficiency of the facets.

[0123] Here, for the update of 、 and these three parameters, it is also only executed once in the same scan frame, that is, if these three parameters have been updated once in the current scan frame, then even if the update rules are met again, these three parameters will not be updated again, that is, in the same scan frame, these three parameters are updated at most once.

[0124] The update process of the map:

[0125] See Figure 4 ​, the voxel map of this embodiment continuously receives new scanned point clouds as input and updates itself. That is, after the arrival of subsequent new scan frames, in the same manner as described above, nearest neighbor search is performed for each scanned point to find nearest neighbor sampling points, and it is determined whether to establish a new sampling point based on this scanned point or assign this scanned point to an established sampling point, and then the corresponding initialization or incremental update is performed to define element information, thereby updating the voxel map.

[0126] Application of the map:

[0127] The voxel map provided in this embodiment can provide an accurate plane reference for the point-to-plane ICP method, improve the performance of the point cloud registration algorithm, and ultimately improve the positioning accuracy of the lidar inertial odometer.

[0128] The key point of the point-to-plane ICP algorithm lies in constructing the cost factor of the point-to-plane distance. Given a lidar scan frame, each scanned point in the scan frame searches for the corresponding voxel through point-plane search. If the search is successful, an effective point-plane match is achieved. , where represents the scanned point for point-plane matching in the current scan frame, represents the voxel successfully matched with . Then, for the effective point-plane match to construct the point-to-plane distance cost factor :

[0129] ;

[0130] where is the pose transformation matrix from the lidar coordinate system to the map coordinate system, and its initial value is generally obtained from the constant velocity model of the odometer or inertial data integration, is the observation noise.

[0131] Finally, a least squares problem is constructed:

[0132] ;

[0133] is the pose state variable of the lidar. represents the L2 norm of, and then this least squares problem is solved to obtain the pose of the lidar.

[0134] See Figure 2, both map updating and point-plane matching rely on efficient nearest neighbor search. In the nearest neighbor search algorithm of this embodiment, each voxel is divided into eight uniform regions, and each region has its corresponding 7 directly adjacent voxels and 19 second-nearest neighbor voxel grids. Taking a scanning point as a given query point, first determine which region the query point is located in the voxel, and based on this, identify the directly adjacent and second-adjacent voxels.

[0135] And in the point-plane matching step, for each query point the nearest surface element needs to be searched. The search process is as follows:

[0136] 1) Divide each voxel into 8 uniform regions, and then determine the region where it is located, and the 7 directly adjacent voxels that are in direct contact with the region where it is located, and form a cubic search space with these 8 voxels ;

[0137] 2) Traverse the surface elements in it, calculate the distance from the center point of the surface element to , retain the surface element with the smallest distance and record the distance from this surface element to ; if there is no surface element in it, then determine that has no matching surface element, and end the point-plane matching process; ;

[0138] 3) Calculate the minimum distance from to the surface , when , end the search process and jump to step 5, thus completing the search process of the first stage, otherwise enter step 4 to perform the second-stage search;

[0139] 4) Search for other surface elements among the 19 second-adjacent voxels. If no other surface elements are found, enter step 5; otherwise, calculate the distance between the found surface element and and compare it with , then retain the surface element with the smallest distance compared, and enter step 5. The second-adjacent voxels refer to all the voxels that surround the voxel where is located and exclude the voxel where it is located. Specifically, refer to the right side of , and the green cube is the second-adjacent voxel. Figure 2 ;

[0140] 5) Compare the currently retained surface element with the smallest distance with Compare the distance with a preset distance threshold. If it is greater than the distance threshold, then determine that there is no matching surface element and end the point-plane matching process; otherwise, use the currently retained surface element with the minimum distance as the matching surface element .

[0141] The 8-voxel neighborhood search method in the map update step and the two-stage 19-voxel neighborhood search method in the point-plane matching step are both superior to the traditional 27-voxel neighborhood search method. The 27-voxel neighborhood search method treats directly adjacent voxels and secondarily adjacent voxels as equal relationships, resulting in the need to access the surface elements within 27 voxels each time. The 8-voxel neighborhood search method, at the cost of increasing the size of the voxel grid, greatly reduces the number of voxels that need to be accessed, generally achieving an improvement in search efficiency. The search method performed by the two-stage point-plane matching refines the search process, and most of the search process can end in the first stage, saving search time.

[0142] To explore the performance improvement of the distribution-adaptive surface element map (DASM) proposed in this embodiment on the laser inertial odometer. In this embodiment, the point cloud map in the laser inertial odometer Fast-LIO2 is replaced with DASM to implement a fast and accurate laser odometer DASM-LIO. Four advanced laser inertial odometers, namely Fast-LIO2, Faster-LIO, VoxelMap, and SLICT, are selected for comparison with DASM-LIO. Fast-LIO2 and Faster-LIO both use point cloud maps and obtain a plane reference by fitting the map points in the local area into a plane. VoxelMap and SLICT both use multi-resolution voxels to extract surface elements and are representative works using voxel-surface element maps.

[0143] Accuracy analysis:

[0144] Table 1 below shows the comparison of the absolute trajectory errors (RMSE) of each method in different datasets. The datasets used in the table are from datasets such as NCLT, Newer College, and BotanicGarden. Thanks to the accurate and detailed environment description provided by DASM, DASM-LIO achieves the lowest absolute trajectory error in most sequences. The unit of the data in the table is the length unit meter (m).

[0145] .

[0146] Figure 3 Shows the effect comparison between DASM and the voxel-surface element map. It can be seen that the map established by DASM is more accurate and detailed.

[0147] Next, this embodiment analyzes the computational efficiency of DASM-LIO in "Single" and "Paral" modes. The "Single" mode means that the program is restricted to run on a single CPU core, which is used to test the actual computational overhead of the algorithm. The "Paral" mode means that the program allows parallel acceleration, which is used to test the parallel acceleration performance of the algorithm.

[0148] As shown in Table 2 below, the datasets used in the table are still from datasets such as NCLT, Newer College, and BotanicGarden. The DASM-LIO of this embodiment has achieved the highest computational efficiency, saving 44%-67% of the running time compared to the second-best algorithm Faster-LIO in the "Paral" mode, and saving 68%-73% of the running time compared to the second-best algorithm Fast-LIO2 in the "Single" mode. The units of the data in the table are all time units of milliseconds (ms).

[0149] 。

Claims

1. A method for implementing a distributed adaptive voxel map to improve the performance of a laser inertial odometer, characterized in that Including the following steps: The map space is evenly divided into grid-like voxels to store pointers to the sampling points that make up the patch map; And the voxels storing the sampling point pointers are mapped into a one-dimensional array through a spatial hashing function, thereby forming a voxel hashing data structure; The sampling points are defined by defining elements, and with the center point of the sampling point as the center of a circle and a radius of The spherical space formed is used as the statistical range of the point cloud data within the sampling point, and all the point cloud data within the statistical range is modeled as a Gaussian distribution, that is, a surface element; the pointers of each sampling point are respectively stored in the voxel where the center point of the sampling point is located; When receiving the first frame of point cloud data obtained by lidar scanning, randomly select one of the scanning points as the center point to establish the first sampling point and initialize the definition element information, and then traverse the remaining scanning points to perform voxel-based nearest neighbor search to find the nearest neighbor sampling points; When a scanning point fails to find a nearest neighbor sampling point, or finds a nearest neighbor sampling point but the distance is greater than a preset distance threshold, then create a new sampling point with this scanning point and initialize the definition element information; when a scanning point is within the statistical range of any nearest neighbor sampling point, then classify the scanning point into the nearest neighbor sampling point and perform incremental update on the definition element information of the nearest neighbor sampling point, thereby establishing a patch map; Subsequently, when receiving the point cloud data of a new scan frame, continue to perform nearest neighbor search for each scanning point in the same way to find the nearest neighbor sampling points and determine whether to create a new sampling point or classify it into the established sampling points, and then initialize or incrementally update the definition element information, thereby updating the patch map.

2. The method according to claim 1, characterized in that, The voxels storing the sampling point pointers are mapped into a one-dimensional array through a spatial hashing function, and the spatial hashing function used is: ； Among them represents the coordinates after mapping the sampling points stored in the voxel to a one-dimensional array, and respectively represent the coordinate values of the sampling points stored in the voxel on the X-axis, Y-axis, and Z-axis in the three-dimensional coordinate system; p1, p2, and p3 are distinct large prime numbers; represents the exclusive OR operation; mod represents the remainder calculation; is the size of the hash table, which is determined according to the size of the map space.

3. The method according to claim 1, characterized in that, The definition element information of the sampling points includes: Center point 、Cumulative value of point cloud within the sampling point statistical range 、Auxiliary variable used to calculate the covariance matrix 、Mean value of point cloud within the sampling point statistical range 、Plane normal vector 、Eigenvalue vector 、From the creation of the sampling point to the previous update 、 And The number of scanned points accumulated within the sampling point statistical range at that time 、From the previous update 、 And The number of scanned points accumulated within the sampling point statistical range up to the current time ; where And Are both obtained by Performing eigen decomposition;​ Modeling all the point cloud data within the statistical range as a Gaussian distribution means establishing a Gaussian distribution , where is obtained by and calculation.

4. The method according to claim 3, wherein The said and are defined as: ； ； Among them is the point cloud set that falls within the statistical range of the sampling point There are: ； The superscript T represents matrix transpose; Then, according to and , it is calculated through the following formula to obtain : 。 5. The method according to claim 4, wherein The nearest neighbor search is: For each scan point, perform the following steps Search for All sampling points within the radius, i.e., the bins, where Is the preset minimum distance interval between sampling points: ①Divide each voxel into eight uniform regions, and then determine the region where it is located, and the seven directly adjacent voxels that are in direct contact with the region where is located, and form a cube-shaped search space with these eight voxels ; ② Traverse the search space for the facets therein, and calculate the center point of each facet to the point, compare the distances, and retain the facets with distances less than as the nearest neighbor sampling points; ​ wherein the side length of a single voxel is greater than , and taking as the radius range means taking the larger value of and as the radius range.

6. The method according to claim 4, wherein When creating a new sampling point, the initialization definition elements include setting and initializing , , , , , and values: Among them, the new scan point is set to ; is initialized to , is initialized to , where the superscript T represents the matrix transpose, is initialized to 1, is initialized to 0; , and When creating a new sampling point, they are all set to 0 first, and is used as the formal initialization condition. Until it is satisfied, the following formula is used to , and are formally initialized: ； ； ； ； wherein the superscript T represents matrix transpose; Then, perform eigen decomposition on . According to the result of eigen decomposition, use the eigenvector corresponding to the minimum eigenvalue of as , and use the vector composed of the three eigenvalues of as .

7. The method according to claim 6, characterized in that, The new scanning point is attributed to the neighboring sampling points and the definition element information of the neighboring sampling points is incrementally updated. The definition element information of the established sampling points is updated based on the following formula: ； ； ； When and are updated, only when , the information of , and will be updated based on the following formula: ； ； ； ； Thus, at the initial stage of the establishment of the sampling points, and when the uncertainties are relatively high, high-frequency updates are performed to improve the estimation accuracy of the plane parameters; when the sampling points tend to be stable, that is, and when good estimations have been obtained, low-frequency updates are executed to improve the efficiency.

8. The method according to claim 7, characterized in that , and must perform a unique formal initialization after meeting the formal initialization conditions, and then be updated according to the situation at ; and in the same scan frame, , and will only perform a formal initialization or an update once.

9. The method according to claim 5, wherein It also includes the step of completing the point-to-plane ICP algorithm based on the patch map to obtain the lidar pose: Obtain point-plane matching through point-plane search , where represents the scan point for point-plane matching in the current scan frame, represents the plane element that successfully matches with ; Then construct the cost factor of the distance from a point to a plane : ； wherein is the pose transformation matrix from the lidar coordinate system to the map coordinate system, and the initial value is obtained from the constant velocity model of the odometer or the integration of inertial data; is the observation noise; represents the surface element of the plane normal vector; represents the surface element of the point cloud mean within the sampling point statistical range; Then construct a least squares problem: ； wherein is the pose state variable of the lidar, denotes the L2 norm of, and then solve this least squares problem to obtain the pose of the lidar.

10. The method according to claim 9, wherein The point-plane search includes the following steps: 1) Divide each voxel into eight uniform regions, and then determine the region where it is located, and the seven directly adjacent voxels that are in direct contact with the region where is located, and form a cube-shaped search space with these eight voxels ; 2) Traverse the surface elements in, calculate the distance from the center point of the surface element to and retain the surface element with the minimum distance and record the distance from this surface element to ; If there are no surface elements in , it is determined that has no matching surface elements, and the point - surface matching process of is ended; 3) Calculate to the minimum distance to the surface When is satisfied, end the search process and jump to step 5; otherwise, go to step 4. 4) Search for other facets among the 19 sub-adjacent voxels. If no other facets are found, proceed to step 5; otherwise, calculate the distance between the found facet and and compare it with . Then retain the facet with the smallest distance obtained from the comparison and proceed to step 5. Here, the sub-adjacent voxels refer to all the voxels that surround the voxel where is located, with the voxel where is located at the center, excluding . 5) Compare the distance between the currently retained smallest-distance face element and with a preset distance threshold. If it is greater than the distance threshold, it is determined that there is no matching face element, and the point-face matching process ends; otherwise, use the currently retained smallest-distance face element as the matching face element .

Images