A distributed adaptive bin map implementation method for improving the performance of laser inertial odometry

Abstract

The present invention discloses a distributed adaptive bin map implementation method for improving the performance of a laser inertial odometry (LIO). The method comprises the following steps: uniformly dividing the map space into a grid of voxels, storing pointers to sampling points used to construct the bin map in the voxels, and projecting the voxels storing the sampling point pointers into a one-dimensional array using a spatial hash function, thereby forming a voxel hash data structure; the sampling points are randomly generated in a spherical space with a radius of r, with the center point of the spherical space serving as the location identifier of the sampling points. The spherical space serves as the statistical range of point cloud data obtained by laser radar scanning, thereby modeling the point cloud as a Gaussian distribution; each sampling point on the bin map corresponds to a bin, and the pointer to each sampling point is stored in the voxel where the center point of the sampling point is located. The bin map can adaptively adjust the position of the bin sampling points according to the distribution of the point cloud, reducing the risk of improper point cloud division and improving the accuracy of plane estimation.

Description

Technical Field

[0001] The present invention relates to the field of path planning, and in particular to a distributed adaptive bin map implementation method for improving the performance of a laser inertial odometry. Background Art

[0002] Point cloud registration is the core algorithm module of the laser inertial odometry, and its accuracy and efficiency are crucial. The ICP method is still the main method of point cloud registration, 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 issue is how to quickly obtain accurate local plane references. There are currently two most popular methods. The first is to use a point cloud map, represented by Fast-LIO2. When constructing the point-to-plane distance cost factor, Fast-LIO2 obtains a plane reference by fitting neighboring map points into a plane. This method has the following disadvantages:

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

[0004] 2. Local plane fitting must be performed each time the cost factor is constructed. Frequent local plane fitting results in a large amount of computational redundancy, which affects computational efficiency.

[0005] Another method is to create a surface element map, which describes the environment as independent surfaces, each of which corresponds to a plane. Compared with point cloud maps, surface element maps 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 a plane reference, eliminating the need for frequent plane fitting, thereby improving the efficiency of cost factor construction. The mainstream method for creating surface element maps is voxel extraction:

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

[0007] ② The map continuously receives input from LiDAR scan frames and adds them to the corresponding voxel grid according to the scan point position;

[0008] ③ Update the plane parameters of the voxel grid with new point input.

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

[0010] But this voxel-surface map also has disadvantages:

[0011] 1. Fixed-distribution voxel grids are difficult to cope with randomly distributed point clouds, which will result in small point cloud volumes in some voxels and anisotropic polygonal shapes, affecting the estimation accuracy of plane parameters.

[0012] 2. The size and density of voxels are coupled to each other. While maintaining an appropriate voxel size to support good plane estimation, it is not possible to increase the detail of the environment description by increasing the voxel density.

[0013] 3. During odometry, voxels need to update their plane parameters when receiving new points. Each update requires recounting all point cloud information and fitting the plane, resulting in inefficient surface element updates.

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

[0015] To address the current voxel-surface maps, which suffer from low bin accuracy and density, and thus fail to provide accurate and detailed environmental descriptions, and their inefficiency in bin update methods, significantly reducing mapping efficiency, this paper proposes a new bin map format to address these issues while maintaining the advantages of bin maps over point cloud maps, ultimately improving the performance of laser inertial odometry.

[0016] In order to achieve the above technical objectives, the technical solution of the present invention is:

[0017] A distributed adaptive bin map implementation method for improving the performance of a laser inertial odometry comprises the following steps:

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

[0019] The sampling point is defined by defining elements, and the center point of the sampling point is the center of the circle and the radius is The spherical space formed is used as the statistical range of the point cloud data within the sampling point, and all point cloud data within the statistical range are modeled as a Gaussian distribution, that is, a surface element; the pointer of each sampling point is 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 by the lidar scan, one of the scan points is randomly selected as the center point to establish the first sampling point and initialize the definition element information. Then, the remaining scan points are traversed to perform a voxel-based neighbor search to find the neighboring sampling points. When a scan point does not find a neighboring sampling point, or a neighboring sampling point is found but the distance is greater than a preset distance threshold, a new sampling point is created with the scan point and the definition element information is initialized. When a scan point is within the statistical range of any neighboring sampling point, the scan point is attributed to the neighboring sampling point and the definition element information of the neighboring sampling point is incrementally updated, thereby establishing a surface element map.

[0021] Subsequently, when the point cloud data of the new scanning frame is received, the neighbor search is continued in the same way for each scanning point to find the neighboring sampling points and determine whether to establish a new sampling point or return it to the established sampling point. Then, the definition element information calculation is initialized or incrementally updated to update the polygon element map.

[0022] Furthermore, the voxels storing the sampling point pointers are mapped into a one-dimensional array through a spatial hash function, and the spatial hash function used is:

[0023] ;

[0024] in Represents the coordinates of the sampling points stored in the voxel after being mapped to a 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; Indicates exclusive OR operation; mod indicates remainder calculation; It is the size of the hash table, which is determined by the size of the map space.

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

[0026] Center Point , point cloud cumulative value within the statistical range of sampling points , used to calculate the covariance matrix Auxiliary variables , point cloud mean within the statistical range of sampling points , plane normal vector , eigenvalue vector , from the creation of the sampling point to the last update 、 and The number of scan points accumulated within the sampling point statistics range , since the last update 、 and The number of scan points currently accumulated within the sampling point statistics range ;in and All are made by Perform feature decomposition to obtain;

[0027] The point cloud data within the statistical range is modeled as a Gaussian distribution, which means establishing a Gaussian distribution. ,in is and Calculated.

[0028] Furthermore, the and is defined as:

[0029] ;

[0030] ;

[0031] in To fall at the sampling point The point cloud collection within the statistical range is:

[0032] ;

[0033] The superscript T in denotes the matrix transpose;

[0034] According to and , calculated by the following formula :

[0035] .

[0036] Furthermore, the nearest neighbor search is:

[0037] For each scan point, perform the following steps Search for As all sampling points within the radius, that is, surface elements, The minimum distance interval between preset sampling points:

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

[0039] ②Traverse the search space Calculate the center point of each face element in arrive The distance between the points and Compare the sizes and keep the distance less than The surface element is taken as the nearest sampling point;

[0040] The side length of a single voxel is greater than , and the mentioned As the radius range, it refers to and The larger value is used as the radius range.

[0041] Furthermore, when creating a new sampling point, the initialization definition element information includes setting And initialize 、 、 、 、 、 and Value:

[0042] The new scanning point Set to ; Initialized to , Initialized to , the superscript T represents the matrix transpose, Initialized to 1, Initialized to 0;

[0043] 、 and When creating a new sampling point, it is first set to 0 and the new sampling point is used as the As the formal initialization condition, until it is met, the following formula is used to adjust 、 and Perform formal initialization:

[0044] ;

[0045] ;

[0046] ;

[0047] ;

[0048] in The superscript T in denotes the matrix transpose;

[0049] Then 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] Furthermore, the step of completing the point-to-plane ICP algorithm based on the surfel map to obtain the lidar pose is also included:

[0062] Obtain point-surface matching through point-surface search ,in Indicates the scanning point for point-surface matching in the current scanning frame, Represents Successfully matched facets;

[0063] Then construct the cost factor of the distance from the point to the plane :

[0064] ;

[0065] in is the pose transformation matrix from the lidar coordinate system to the map coordinate system, and its initial value is obtained by integrating the constant velocity model of the odometer or the inertial data; is the observation noise; Representing a facet The plane normal vector of Representing a facet The mean value of the point cloud within the statistical range of the sampling points;

[0066] Reformulate the least squares problem:

[0067] ;

[0068] in is the posture state variable of the lidar, express The L2 norm of , and then solve this least squares problem to obtain the position and posture of the lidar.

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

[0070] 1) Divide each voxel into 8 even regions and then determine The area where you are located, and The 7 directly adjacent voxels that are directly in contact with the area, and these 8 voxels form a cube-shaped search space ;

[0071] 2) Traversal The center point of the face element is calculated to The distance between the surface element and the minimum distance is retained and recorded. distance ;like If there is no facet in No matching facets found, and the process ends. Point-surface matching process;

[0072] 3) Calculation arrive Minimum distance to surface ,when When , end the search process and jump to step 5, otherwise go to step 4;

[0073] 4) Search for other face elements in the 19 sub-adjacent voxels. If no other face elements are found, go to step 5; otherwise, calculate the difference between the found face element and The distance and Compare, then retain the minimum distance surface element and go to step 5; the next adjacent voxel refers to the The voxel where it is located is the center and surrounded by Around the voxel and remove All voxels except

[0074] 5) The currently retained minimum distance surface element is The distance is compared with the preset distance threshold. If it is greater than the distance threshold, it is determined No matching facets found, and the process ends. Otherwise, the currently retained minimum distance surface element is used as Matched facets .

[0075] Based on the above steps, the present invention provides a distribution-adaptive surfel map (DASM). This surfel map adaptively adjusts the locations of surfel sampling points based on the distribution of the point cloud, reducing the risk of improper point cloud segmentation and improving plane estimation accuracy. Furthermore, the surfels in the DASM are compactly arranged to provide a more detailed description of the environment.

[0076] To address the problem that traditional facet updating methods require recalculating all information and are therefore inefficient, the present invention also provides a highly efficient facet incremental updating method, which models the point cloud within the facet as a normal distribution and performs incremental updates. It performs eigendecomposition on the covariance matrix in the normal distribution to iteratively update the plane parameters, greatly reducing the computational cost of facet updating and improving the efficiency of mapping.

[0077] The present invention organizes the unordered surfels in the surfel map into an ordered voxel hash data structure, thereby enabling fast map access. The present invention also provides a fast neighbor search method to accelerate map creation and map query.

[0078] The technical effect of the present invention is that the distributed adaptive facet map provided by the present invention can provide a more accurate and detailed description of the environment, improve the performance of the point cloud registration algorithm, and ultimately improve the positioning accuracy of the laser inertial odometry. The present invention also provides a highly efficient facet incremental update method, avoiding the disadvantage of requiring recalculation every time a facet is updated, greatly increasing the efficiency of mapping. 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 faster search efficiency and can effectively accelerate map creation and map query. BRIEF DESCRIPTION OF THE DRAWINGS

[0079] Figure 1 This is a structural diagram of the distributed adaptive facet map of the present invention.

[0080] Figure 2 Schematic diagram of neighbor search according to the present invention.

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

[0082] Figure 4 Schematic diagram of the map updating process in an embodiment of the present invention. DETAILED DESCRIPTION

[0083] See also Figure 1 , the bin map provided in this embodiment , which is composed of a large number of sampling points Composition: Among them , each sampling point corresponds to a surface element. Each sampling point is defined by the following elements:

[0084] Center Point , point cloud cumulative value within the statistical range of sampling points , used to calculate the covariance matrix Auxiliary variables , point cloud mean within the statistical range of sampling points , plane normal vector , eigenvalue vector , , } ( < < ), from the creation of the sampling point to the last update 、 and The number of scan points accumulated within the sampling point statistics range , since the last update 、 and The number of scan points currently accumulated within the sampling point statistics range .in in Indicates real numbers, superscript Indicates a dimension of 3.

[0085] Center Point It is the unique location identifier of the sampling point and remains unchanged after initialization. The spherical space with a center and a radius of r is the statistical range of the sampling point. The sampling point will model all points within the statistical range as a Gaussian distribution. . The covariance matrix It is not stored in the sampling point, but is generated by and Calculated.

[0086] Assumptions Is located at a sampling point The point cloud set within the statistical range. 、 The definition is as follows:

[0087] .

[0088] .

[0089] It should be mentioned here that, in this embodiment, whether in establishing a new sampling point pair and When initializing or updating the sampling points incrementally, there are corresponding more efficient expressions to and When performing specific calculations, the final results are the same as the above definition expressions. Therefore, the definition expressions here are only used for definition, and the above two expressions will not be used for calculations in actual implementation.

[0090] From the historical point of view definition:

[0091] .

[0092] .

[0093] Eigenvalue vector and normal vector Through Perform eigendecomposition to obtain the normal vector for The eigenvector corresponding to the minimum feature of It is caused by The vector consisting of the three eigenvalues ​​of .

[0094] Since the generation of sampling points is random, this disorder will bring some difficulties to map access. In order to improve the efficiency of map access, the disordered sampling points will be 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 at their center points. The voxel where the sampling point is located (i.e. the center point of the sampling point) The voxels within the statistical range of this voxel will be projected into a one-dimensional array through the spatial hash function. The spatial hash function is expressed as:

[0095] ;

[0096] In the above formula, Represents the coordinates of the sampling points stored in the voxel after being mapped to a one-dimensional array. 、 where p1, p2, and p3 represent the coordinates of the sampling point stored in the voxel on the X, Y, and Z axes in the three-dimensional coordinate system. p1, p2, and p3 are large, unequal prime numbers. In this embodiment, p1, p2, and p3 are 73,856,093, 19,349,669, and 83,492,791, respectively. These values ​​may be determined based on specific implementation requirements. Indicates exclusive-or operation. mod indicates remainder operation. It is the size of the hash table, which is determined by the size of the map space.

[0097] Map creation process:

[0098] When receiving the first frame of point cloud data obtained by the LiDAR scan, one of the scan points in the scan frame is randomly selected as the center point to establish the first sampling point and calculate the defining element information. Next, a voxel-based nearest neighbor search is performed on the remaining scan points to find neighboring sampling points. If a scan point does not find a neighboring sampling point, or a neighboring sampling point is found but the distance between them is greater than a preset distance threshold, a new sampling point is created based on that scan point and the defining element information is calculated. If a scan point is within the statistical range of any neighboring sampling point, the scan point is assigned to that neighboring sampling point and the defining element information of that neighboring sampling point is incrementally updated, thus establishing a surface element map.

[0099] In this example, the position of the sampling points is determined by the actual scanning points. This ensures that the sampling points are close to the surface of the actual object and that the spherical space expanded by the sampling points can better encompass the point cloud, thus avoiding the inclusion of the point clouds of adjacent objects to the greatest extent possible.

[0100] The neighbor search mentioned above can be found in Figure 2 , is for each scanning point Search for As all sampling points within the radius, that is, surface elements, The minimum distance between the preset sampling points is set by changing The size of can adjust the density of the surface elements in the map, That means and The larger value is used as the radius range, and in order to ensure that the query point The spherical space within the radius is the search space A subset of , the side length of a single voxel should be greater than The specific steps of nearest neighbor search include:

[0101] ① Divide each voxel into 8 even regions, namely Figure 2 Each small blue cube on the left represents a divided area, and then determine The area is shown in red in the figure. This red area and the other 7 blue areas form Then determine the voxel where The 7 directly adjacent voxels that are directly in contact with the area are in Figure 2 The middle part is represented by a transparent cube. You can see that these 7 transparent cubes are in direct contact with In the region, including Finally, these 8 voxels form a cube-shaped search space. .

[0102] ②Next, traverse the search space Calculate the center point of each face element in arrive The distance between the points and Compare the sizes and keep the distance less than The surface element is taken as the nearest neighbor sampling point.

[0103] When creating a new sampling point, the initialization definition element information includes setting the sampling point , and initialize 、 、 、 、 、 and Value:

[0104] The new scanning point Set to ; Initialized to , Initialized to , the superscript T represents the matrix transpose, Initialized to 1, Initialized to 0; at the same time 、 and When creating a new sampling point, it is first set to 0 as a temporary initialization value.

[0105] Then, as the neighbor search is performed on each scanning point in the current laser radar scanning frame, when 3 or more scanning points are included in this sampling point, that is, Then, the following calculation steps are used to calculate 、 and Perform formal initialization:

[0106] ;

[0107] ;

[0108] ;

[0109] ;

[0110] in The superscript T in denotes the matrix transpose;

[0111] Then 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 .

[0112] If the new sampling point fails to reach This formal initialization condition, then keep 、 and These three parameters are reset to 0, and after a new scan frame arrives, they continue to check whether the formal initialization conditions are met, until they are met in a certain scan frame, and then formal initialization is performed again. The formal initialization of these three parameters is performed only once, and then they are updated according to the update rules described in the incremental update below. However, if these three parameters have already been formally initialized in the same scan frame, even if the incremental update rules are met again, these three parameters will not be updated again, and will continue to be updated after the arrival of subsequent scan frames.

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

[0114] ;

[0115] ;

[0116] ;

[0117] And when 、 After the update, As 、 and The update rules of these three parameters are that only when When the following formula is used to calculate 、 and The information is updated:

[0118] ;

[0119] ;

[0120] ;

[0121] ;

[0122] 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 is obtained, low-frequency updates are performed to improve efficiency. This incremental update strategy does not require storing large amounts of point cloud information and recalculating, which can greatly improve the update efficiency of the surface element.

[0123] Here for 、 and The update of these three parameters is also performed only once in the same frame 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 no longer be updated, that is, in the same frame scan frame, these three parameters are updated at most once.

[0124] Map update process:

[0125] See also Figure 4The surface element map of this embodiment continuously receives new scan point cloud input and updates itself. That is, when a new scan frame arrives, a neighbor search is performed for each scan point in the same manner as described above to find neighboring sampling points. A decision is then made as to whether to establish a new sampling point based on this scan point or to include this scan point in an existing sampling point. The surface element map is then initialized or incrementally updated to define element information, thereby updating the surface element map.

[0126] Map applications:

[0127] The surfel 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 laser inertial odometry.

[0128] The key to the point-to-plane ICP algorithm is to construct a cost factor for the distance from point to plane. Given a LiDAR scan frame, each scan point in the scan frame searches for the corresponding surface element through a point-to-surface search. If the search is successful, an effective point-to-surface matching is achieved. ,in Indicates the scanning point for point-surface matching in the current scanning frame, Represents Successfully matched face element. Then for valid point-face matching To construct the point-to-plane distance cost factor :

[0129] ;

[0130] in is the pose transformation matrix from the lidar coordinate system to the map coordinate system. The initial value is generally obtained by integrating the constant velocity model of the odometer or the inertial data. is the observation noise.

[0131] Finally, construct the least squares problem:

[0132] ;

[0133] is the posture state variable of the lidar. express The L2 norm of , and then solve this least squares problem to obtain the position and posture of the lidar.

[0134] See also Figure 2Map updates and point-to-surface matching both rely on efficient nearest neighbor searches. The nearest neighbor search algorithm in this embodiment divides each voxel into eight evenly spaced regions, each with a corresponding grid of seven immediate neighbors and 19 next-nearest neighbors. Given a scan point as a query point, the query point is first determined within the voxel region, and based on this, the immediate and next-nearest neighbors are identified.

[0135] In the point-surface matching step, for each query point Search for the nearest surface element. The search process is as follows:

[0136] 1) Divide each voxel into 8 even regions and then determine The area where you are located, and The 7 directly adjacent voxels that are directly in contact with the area, and these 8 voxels form a cube-shaped search space ;

[0137] 2) Traversal The center point of the face element is calculated to The distance between the surface element and the minimum distance is retained and recorded. distance ;like If there is no facet in No matching facets found, and the process ends. Point-surface matching process;

[0138] 3) Calculation arrive Minimum distance to surface ,when When , the search process ends and jumps to step 5, thus completing the first stage of the search process, otherwise go to step 4 to perform the second stage of the search;

[0139] 4) Search for other face elements in the 19 sub-adjacent voxels. If no other face elements are found, go to step 5; otherwise, calculate the difference between the found face element and The distance and Then keep the smallest distance surface element and go to step 5. The voxel where it is located is the center and surrounded by Around the voxel and remove All voxels except . For details, see Figure 2 On the right, the green cubes are the next-neighbor voxels.

[0140] 5) The currently retained minimum distance surface element is The distance is compared with the preset distance threshold. If it is greater than the distance threshold, it is determined No matching facets found, and the process ends. Otherwise, the currently retained minimum distance surface element is used as Matched facets .

[0141] The 8-voxel nearest neighbor search method in the map update step and the two-stage 19-voxel nearest neighbor search method in the point-surface matching step both outperform the traditional 27-voxel nearest neighbor search method. The 27-voxel nearest neighbor search method treats directly adjacent voxels and sub-adjacent voxels as equivalent, resulting in the need to visit 27 intra-voxel face elements each time. The 8-voxel nearest neighbor search method, however, significantly reduces the number of voxels that need to be visited at the expense of increasing the size of the voxel grid, thereby improving search efficiency overall. The search method implemented by the two-stage point-surface matching refines the search process, allowing most searches to be completed in the first stage, saving search time.

[0142] To explore the performance improvement of laser inertial odometry achieved by the Distributed Adaptive Surf Map (DASM) proposed in this example, this example replaces the point cloud map in the Fast-LIO2 laser inertial odometry with DASM, implementing a fast and accurate laser odometry, DASM-LIO. Four advanced laser inertial odometry systems, including Fast-LIO2, Faster-LIO, VoxelMap, and SLICT, were selected for comparison with DASM-LIO. Fast-LIO2 and Faster-LIO both use point cloud maps, obtaining a plane reference by fitting map points in a local area to a plane. VoxelMap and SLICT, representative of the use of voxel-surf maps, both use multi-resolution voxels to extract surfels.

[0143] Precision analysis:

[0144] Table 1 below compares the absolute trajectory error (RMSE) of various methods across various datasets, including those from NCLT, Newer College, and Botanic Garden. Thanks to the precise and detailed environmental descriptions provided by DASM, DASM-LIO achieves the lowest absolute trajectory error in most sequences. Data in the table are in meters.

[0145] .

[0146] Figure 3 The comparison of the effects of DASM and voxel-surface map is shown. It can be seen that the map built by DASM is more accurate and detailed.

[0147] Next, this example analyzes the computational efficiency of DASM-LIO in "Single" and "Paral" modes. "Single" mode restricts the program to running on a single CPU core and is used to test the algorithm's actual computational overhead. "Paral" mode allows for parallel acceleration and is used to test the algorithm's parallel acceleration performance.

[0148] As shown in Table 2 below, the datasets used are still from NCLT, Newer College, and Botanic Garden. DASM-LIO in this embodiment achieves the highest computational efficiency, saving 44%-67% of runtime in "Paral" mode compared to the second-place algorithm, Faster-LIO, and 68%-73% of runtime in "Single" mode compared to the second-place algorithm, Faster-LIO2. Data in the table are all in milliseconds (ms).

[0149] .

Claims

1. A distributed adaptive bin map implementation method for improving the performance of a laser inertial odometry, characterized in that: The following steps are involved: The map space is evenly divided into grid-like voxels to store pointers to the sampling points that constitute the surface map; 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; The sampling point is defined by defining elements, and the center point of the sampling point is the center of the circle and the radius is The spherical space formed is used as the statistical range of the point cloud data within the sampling point, and all point cloud data within the statistical range are modeled as a Gaussian distribution, that is, a surface element; the pointer of each sampling point is 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 the lidar scan, one of the scan points is randomly selected as the center point to establish the first sampling point and initialize the definition element information. Then, the remaining scan points are traversed to perform a voxel-based neighbor search to find the nearest neighbor sampling points. When a scan point fails to find a neighboring sampling point, or finds a neighboring sampling point but the distance between them is greater than a preset distance threshold, a new sampling point is created with the scan point and the definition element information is initialized. When a scan point is within the statistical range of any neighboring sampling point, the scan point is assigned to the neighboring sampling point and the definition element information of the neighboring sampling point is incrementally updated, thereby establishing a bin map. Subsequently, when the point cloud data of the new scanning frame is received, the neighbor search is continued in the same way for each scanning point to find the neighboring sampling points and determine whether to establish a new sampling point or return it to the established sampling point. Then, the definition element information is initialized or incrementally updated to update the polygon 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 hash function. The spatial hash function used is: ； in Represents the coordinates of the sampling points stored in the voxel after being mapped to a 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; Indicates exclusive OR operation; mod indicates remainder calculation; It is the size of the hash table, which is determined by the size of the map space.

3. The method according to claim 1, characterized in that The definition element information of the sampling point includes: Center Point , point cloud cumulative value within the statistical range of sampling points , used to calculate the covariance matrix Auxiliary variables , point cloud mean within the statistical range of sampling points , plane normal vector , eigenvalue vector , from the creation of the sampling point to the last update 、 and The number of scan points accumulated within the sampling point statistics range , since the last update 、 and The number of scan points currently accumulated within the sampling point statistics range ;in and All are made by Perform feature decomposition to obtain; The point cloud data within the statistical range is modeled as a Gaussian distribution, which means establishing a Gaussian distribution. ,in is and Calculated.

4. The method according to claim 3, characterized in that described and is defined as: ； ； in To fall at the sampling point The point cloud collection within the statistical range is: ； The superscript T in represents the matrix transpose; According to and , calculated by the following formula : 。 5. The method according to claim 4, characterized in that The nearest neighbor search is: For each scan point, perform the following steps Search for As all sampling points within the radius, that is, surface elements, The minimum distance interval between preset sampling points: ① Divide each voxel into 8 even regions, and then determine The area where you are located, and The 7 directly adjacent voxels that are directly in contact with the area, and these 8 voxels form a cube-shaped search space ; ②Traverse the search space Calculate the center point of each face element in arrive The distance between the points and Compare the sizes and keep the distance less than The surface element is taken as the neighboring sampling point; The side length of a single voxel is greater than ,by As the radius range, it refers to and The larger value is used as the radius range.

6. The method according to claim 4, characterized in that When creating a new sampling point, the initial definition element information includes setting And initialize 、 、 、 、 、 and Value: The new scanning point Set to ; Initialized to , Initialized to , the superscript T represents the matrix transpose, Initialized to 1, Initialized to 0; 、 and When creating a new sampling point, it is first set to 0 and the new sampling point is used as the As the formal initialization condition, until it is met, the following formula is used to adjust 、 and Perform formal initialization: ； ； ； ； in The superscript T in represents the matrix transpose; Then 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 .

7. The method according to claim 6, characterized in that The new scan 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: ； ； ； when 、 After the update, only when When the following formula is used to calculate 、 and The information is updated: ； ； ； ； 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.

8. The method according to claim 7, characterized in that 、 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.

9. The method according to claim 5, characterized in that It also includes the steps of completing the point-to-plane ICP algorithm based on the surfel map to obtain the lidar pose: Obtain point-surface matching through point-surface search ,in Indicates the scanning point for point-surface matching in the current scanning frame, Represents Successfully matched facets; Then construct the cost factor of the distance from the point to the plane : ； in is the pose transformation matrix from the lidar coordinate system to the map coordinate system, and its initial value is obtained by integrating the constant velocity model of the odometer or the inertial data; is the observation noise; Representing a facet The plane normal vector of Representing a facet The mean value of the point cloud within the statistical range of the sampling points; Reformulate the least squares problem: ； in is the posture state variable of the lidar, express The L2 norm of , and then solve this least squares problem to obtain the position and posture of the lidar.

10. The method according to claim 9, characterized in that The point-plane search comprises the following steps: 1) Divide each voxel into 8 even regions and then determine The area where you are located, and The 7 directly adjacent voxels that are directly in contact with the area, and these 8 voxels form a cube-shaped search space ; 2) Traversal The center point of the face element is calculated to The distance between the surface element and the minimum distance is retained and recorded. distance ;like If there is no facet in No matching facets found, and the process ends. Point-surface matching process; 3) Calculation arrive Minimum distance to surface ,when When , end the search process and jump to step 5, otherwise go to step 4; 4) Search for other face elements in the 19 sub-adjacent voxels. If no other face elements are found, go to step 5; otherwise, calculate the difference between the found face element and The distance and Compare, then retain the minimum distance surface element and go to step 5; the next adjacent voxel refers to the The voxel where it is located is the center and surrounded by Around the voxel and remove All voxels except 5) Combine the currently retained minimum distance surface element with The distance is compared with the preset distance threshold. If it is greater than the distance threshold, it is determined No matching facets found, and the process ends. Otherwise, the currently retained minimum distance surface element is used as Matched facets .

Images