Laser radar positioning and three-dimensional mapping method based on implicit neural field

Abstract

The invention discloses a laser radar positioning and three-dimensional mapping method based on an implicit neural field, and belongs to the technical field of positioning and mapping. The method comprises the following steps: for first frame data in radar point cloud data, converting an input sampling point from a sensor coordinate system to a world coordinate system by using a unit matrix or a constant matrix to initialize a map; the hierarchical potential features are optimized in a self-supervision mode, an octree data structure is used for storing the optimized potential features to generate a potential feature set, and features in the potential feature set are decoded into symbol distance values through a shallow multilayer perceptron (MLP); starting from a second frame of data of the radar point cloud data, obtaining a predicted pose through a matching mode from the frame to the implicit map every time a frame is input; converting a new input point into a world coordinate system by using the obtained predicted pose, and determining whether a new potential feature needs to be added or not according to a Morton value of the new input point; and completing the construction of the NSDF by repeating the operations of predicting the pose and expanding the NSDF.

Description

Technical Field

[0001] The present invention relates to the technical field of positioning and map construction, and in particular to a laser radar positioning and three-dimensional mapping method based on implicit neural fields. Background Art

[0002] SLAM (Simultaneous Localization and Mapping) is a technology that integrates positioning and map construction, and is mainly used in fields such as mobile robots and autonomous vehicles. The goal of SLAM is to estimate the pose (position and attitude) of a mobile robot in six degrees of freedom (6-DoF) and build a map around the mobile robot in the location environment. LiDAR Odometry is a technology that estimates the pose change (position and orientation) of a robot or vehicle by using LiDAR data.

[0003] In terms of map reconstruction, most existing 3D reconstruction works are developed for RGB-D cameras and indoor environments, which are not suitable for large-scale outdoor environments. For large-scale mapping, laser radar (LiDAR) plays a key role due to its ability to provide accurate distance measurement. In the positioning method using LiDAR, most of the existing technologies are explicit methods, that is, they directly rely on the point cloud data itself, or extract point features, surface features, edge features, etc. from the point cloud data to obtain the pose by finding corresponding points or features in adjacent frames to minimize the Euclidean distance. Most of the existing deep learning methods are also supervised training to obtain the pose, that is, additional real poses are required during the training process, which requires a large amount of real data from the environment. Since LiDAR data may contain noise, especially in severe weather conditions (such as rain, snow, fog, etc.), these noises will affect the feature extraction and matching process, resulting in increased pose estimation errors and high computational overhead. In real-time applications, processing speed may become a bottleneck, especially when the point cloud density is high, and the initial pose estimation is highly dependent. If the initial guess deviation is too large, the algorithm may converge to a local optimal solution, resulting in a large error, being unable to cope with dynamic environments and being easily affected by interference from dynamic objects, which may affect the position estimation. Summary of the invention

[0004] The present invention provides a method for laser radar positioning and three-dimensional mapping based on implicit neural fields, which can solve the problems existing in the existing SLAM technology, such as sensitivity to noise, high computational overhead, strong dependence on initial pose estimation, and inability to cope well with dynamic environments. The present invention aims to provide a new solution that is more intelligent, efficient and adaptable to outdoor environments, so as to effectively improve the accuracy and reliability of laser radar in positioning and mapping.

[0005] The present invention solves the above technical problems and proposes a lidar positioning and three-dimensional mapping method based on implicit neural field, which includes the following steps:

[0006] S1 initializes the map and the neural signed distance field NSDF:

[0007] S1.1 Initialize the map: For the first frame of radar point cloud data, use the unit matrix or constant matrix to transform the input sampling points from the sensor coordinate system to the world coordinate system, and use the points transformed into the world coordinate system to initialize the map. During the map initialization process, randomly generate hierarchical potential features through Gaussian distribution;

[0008] S1.2 Initialize the neural signed distance field NSDF: optimize the hierarchical latent features in a self-supervised manner, and use the octree data structure to store the optimized latent features to generate a latent feature set F. The features in the latent feature set F are decoded into signed distance values ​​through a shallow multi-layer perceptron MLP to complete the initialization of NSDF.

[0009] S2 predicted pose and extended NSDF:

[0010] S2.1 Predicted pose: Starting from the second frame of radar point cloud data, each time a frame is input, the predicted pose is obtained by matching the frame to the implicit map;

[0011] S2.2 Extended NSDF: Using the predicted pose, the new input point is converted to the world coordinate system, and the Morton value of the new input point is used to determine whether it is in the current map. If the new input point has a corresponding node, no new potential features need to be added; otherwise, the new potential features are also stored hierarchically in the potential feature set F based on the octree data structure;

[0012] By repeating the operations of predicting pose and expanding NSDF until all radar point cloud data are input, a complete map and the pose of all data frames are obtained, and NSDF construction is completed.

[0013] Beneficial effects: The present invention is based on a self-supervised training method, which does not require real posture training, and is an implicit method, which means that we do not need to find corresponding points or corresponding features in adjacent frames. This is an alignment method without corresponding relationships. It only needs to use hierarchical potential features based on the octree structure to obtain the posture of the current frame by minimizing the SDF value of the current frame. Since there is no need to find points or corresponding features in adjacent frames, the computational cost is also reduced. In addition, hierarchical feature fusion also reduces the impact of dynamic objects on positioning accuracy.

[0014] In terms of map construction, unlike traditional display reconstruction methods, an implicit way of representing maps is used, so the details are more realistic. The potential features based on octree data structure storage and index layering can not only reduce memory consumption when building maps on a large scale, but also improve the efficiency of data access. Multi-layer potential features make the map details richer, making large-scale 3D mapping more efficient and accurate. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] Figure 1 Schematic diagram of the Hi-LOAM system architecture in the present invention;

[0016] Figure 2 It is a flow chart of the lidar positioning and three-dimensional mapping method based on implicit neural field in the present invention. DETAILED DESCRIPTION

[0017] The present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments, but the protection scope of the present invention is not limited to the described contents.

[0018] The present invention proposes a lidar positioning and three-dimensional mapping method based on implicit neural fields, which integrates positioning and map construction into a unified implicit framework. In the use of lidar positioning methods, the existing technology usually adopts an explicit method, that is, directly relying on the point cloud data itself, or extracting point features, surface features, edge features, etc. from the point cloud data by finding corresponding points or features of adjacent frames to minimize the Euclidean distance to obtain the pose. Most of the existing deep learning methods are also supervised training to obtain the pose, that is, additional real pose is required during the training process.

[0019] A novel system architecture named Hi-LOAM is invented to implicitly construct a large-scale environment from only LiDAR data input. Figure 1 As shown in the figure, first, the points sampled along the ray are used to construct the octree-based neural signed distance field NSDF. Secondly, the endpoints of the ray are input into the neural implicit odometer to obtain the pose. Specifically, the pose of the previous frame in the video is used as the initial pose of the current frame. The pose is quickly indexed through the NSDF using a hash table to query the hierarchical latent features, and then the latent features of different levels are fused. The Levenberg-Marquardt method is used to minimize the loss function to obtain the predicted pose of the current frame. Then, the input point, i.e., the lidar data, is converted to global coordinates using the predicted pose, and the neural signed distance field NSDF is continued to be expanded. The above conversion and expansion are repeated until the map is built. At this time, the MarchingCubes algorithm is used to extract the isosurface to visualize the map. Multi-level latent features ensure the accuracy of positioning and the completeness of map details.

[0020] like Figure 2 As shown, the lidar positioning and three-dimensional mapping method based on implicit neural field in the present invention includes:

[0021] S1 initializes the map and the neural signed distance field NSDF:

[0022] S1.1 Initialize the map: For the first frame of data in the radar point cloud data, use the unit matrix or constant matrix to transform the input sampling points from the sensor coordinate system to the world coordinate system, and use the points transformed into the world coordinate system to initialize the map. During the map initialization process, hierarchical latent features are randomly generated through Gaussian distribution.

[0023] S1.2 Initialize the neural signed distance field NSDF: Use self-supervision to optimize the hierarchical latent features, and use the octree data structure to store the optimized latent features to generate a latent feature set F. Use a shallow multi-layer perceptron MLP to decode the features in the latent feature set F into signed distance values ​​to complete the initialization of NSDF.

[0024] Specifically, the latent feature is represented by F, F = {{F0}, {F1}, ..., {F(L-2)}, {F(L-1)}}, where L is the number of features. F is a one-dimensional set of feature vectors, each of which has a length of H1 and is stored in an octree.

[0025] Octree is a tree-like data structure that is often used to represent discrete structures in three-dimensional space. It recursively divides the three-dimensional space into eight cubes of equal size, each of which is called a node. This process can be recursively continued, subdividing each child node into eight child nodes until the required depth or condition is reached. An octree node consists of 8 corners. Each corner has a corresponding one-dimensional potential feature. Since the octree contains multiple levels, we store the corresponding potential features at multiple levels to form our layered potential features. The potential features at different levels express the map details from coarse to fine. Our method only stores the octree potential features of the last three levels. The level of the octree is defined as Level K, where K = 0, 1, 2, ..., k-1. It should be noted that the first level is defined as Level 0. In different levels, nodes without points will be pruned. The purpose of this pruning operation is to optimize the memory usage for large-scale scene construction.

[0026] Furthermore, when generating the potential feature set F in the octree data structure, a table N and a table G are generated at the same time, where the table N is represented by N={{N0},{N1},…,{N(L-2)},{N(L-1)}}, which is used to establish the relationship between the hierarchical nodes and the hierarchical potential features in the octree data structure, and the table G is represented by

[0027] G = {{G0}, {G1}, ..., {G(L-2)}, {G(L-1)}}, which is used to establish the relationship between the corner points of the hierarchical nodes and the hierarchical potential features; the three-dimensional coordinates are mapped to one-dimensional codes using Morton coding, and the hierarchical potential features are quickly indexed in combination with Table N and Table G. The Morton coding of the corner points of the nodes in Table G corresponds to the index of a single potential feature in Table F.

[0028] Morton coding is a method of mapping points in a multidimensional space to a one-dimensional space. It is often used to maintain locality in multidimensional data, that is, adjacent points remain adjacent in the one-dimensional code. The basic idea of ​​Morton coding is to interleave the multidimensional coordinates bit by bit to form a single integer code. The most common form is Z-shaped coding, in which adjacent points are also adjacent in the code. This helps to maintain locality in a one-dimensional sequence, thereby improving access efficiency. It is worth noting that NSDF is an ever-expanding process as the input increases.

[0029] S2 predicted pose and extended NSDF:

[0030] S2.1 Predicted pose: Starting from the second frame of radar point cloud data, for each input frame, the predicted pose is obtained by matching the frame to the implicit map scanto implicitmap.

[0031] The process of obtaining the predicted pose by matching the frame to the implicit map is called implicit neural odometry. This process is not performed in the first frame data. Starting from the second frame data, the implicit neural odometry will be used to predict the pose of each frame input.

[0032] Specifically, the predicted pose is obtained by matching the frame to the implicit map in the present invention by using the constant speed motion model to initialize the pose, which is specifically as follows: First, the pose of the first two frames is obtained by using formula (1) and Predict the initial pose of the current frame Formula (1) is: Secondly, based on the initial pose, the current frame is converted to the world coordinate system, and the SDF value of the input point in the current frame is queried through NSDF; finally, the Levenberg-Marquardt (LM) algorithm or Adam optimizer is used to minimize the SDF value to iteratively obtain the predicted pose of the current frame. is a transformation matrix expressed as follows: R is a 3X3 rotation matrix, and t is a 3X1 translation vector.

[0033] The Levenberg-Marquardt (LM) algorithm is a nonlinear least squares optimization algorithm. It combines the advantages of the gradient descent method and the Gauss-Newton method and performs well in dealing with nonlinear least squares problems. It is mainly used for data fitting problems such as fitting curves and surfaces, as well as parameter optimization in the training of machine learning models such as neural networks.

[0034] The specific solution process of minimizing the SDF value using the LM algorithm is:

[0035] Among them, p ct Represents the point cloud in the coordinate system under the sensor, p is any point in the coordinate system under the sensor, R, t are the initial poses The rotation matrix and translation vector in are Indicates that R when the minimum value is taken, t is the predicted pose R of the current frame * , t * .

[0036] In order to use the LM algorithm to solve the above equation and calculate the Jacobian matrix J, in the process of deriving the Jacobian matrix, we actually assume that the rotation angle is small. The following formula (3) can be derived from the Rodriguez formula. Due to the long derivation process, the derivation process is not given in detail here.

[0037] in represents the point cloud in world coordinates, σ is the scaling factor, θ is the rotation axis of the rotation vector, t is the translation vector, is the output of mlp, ×

[0038] represents the cross product, * represents the dot product, The neural distance field is The gradient of a point can usually be obtained through the automatic differentiation mechanism of the pytorch framework, so our method does not need to calculate the gradient separately compared to other methods. From the above formula (2), it can be seen that formula (2) does not show the correspondence between points or points to planes. Different from the traditional ICP method, our method is an implicit method without correspondence. Then we can approximate the Hessian matrix H = J T J and calculate the gradient of the objective function

[0039] g=J T b, b is the residual vector, which is the vector formed by subtracting the true sdf value from the predicted sdf value. From this, we can get the increment of each iteration process, as shown in the following formula (4):

[0040] Δ μ =(H+diag(H)) -1 g(4)

[0041] diag(H) extracts the diagonal elements of H and creates a diagonal matrix. After multiple iterations, when the increment Δ μ If it is small enough, we can get the final predicted pose, as shown in the following formula (5):

[0042] Among them, exp(Δ μ ) represents the antisymmetric matrix of the increment, R* is the predicted rotation matrix, and t* is the predicted translation vector.

[0043] S2.2 Extended NSDF: Using the predicted pose, the new input point is converted to the world coordinate system, and the Morton value of the new input point is used to determine whether it is in the current map. If the new input point has a corresponding node, no new potential features need to be added; otherwise, the new potential features are also hierarchically stored in the potential feature set F based on the octree data structure. Correspondingly, Table G and Table N are also extended.

[0044] In order to determine the size of the current map, the coordinates of the maximum node maxk and the minimum node mink of each level k of the octree are traversed, and the calculation formula is M = (maxk-mink) / Scube (6), where Scube represents the cube size of Marching Cubes, and M represents the number of cubes on the x, y and z axes of the entire map.

[0045] By repeating the operations of predicting pose and expanding NSDF until all frames in the radar point cloud data are input, a complete map and the pose of all data frames are obtained, thus completing the NSDF construction.

[0046] After the NSDF is constructed, the implicitly represented map is converted into a visual form using the Marching Cubes algorithm.

[0047] Optionally, the parameters in the above method are: for MLP, the number of hidden layers p of all MLPs is 2. scube is 0.1 meters, α is 0.05, the number of sampling points N of LiDAR rays is 6, the length of the feature vector H1 is 12, the octree level K = 15, and the scaling factor σ = 1 / 2 14 .

[0048] In summary, the present invention is based on a self-supervised training method, that is, it does not require real posture training, and it is an implicit method, that is, we do not need to find corresponding points or corresponding features of adjacent frames. This is a non-corresponding alignment method. It only needs to use the hierarchical potential features based on the octree structure to obtain the posture of the current frame by minimizing the SDF value of the current frame. Since there is no need to find the right point or corresponding feature in the adjacent frame, the computational consumption is also reduced. In addition, the hierarchical feature fusion also reduces the impact of dynamic objects on positioning accuracy. In terms of map construction, unlike the traditional display reconstruction method, the map is represented in an implicit way, so the details are more realistic. The data structure based on the octree can also reduce memory consumption when building maps on a large scale, and the multi-layer potential features make the map details richer. Therefore, the use of the octree data structure to store and index hierarchical potential features not only reduces memory consumption, but also improves the efficiency of data access, making large-scale 3D mapping more efficient and accurate.

[0049] Finally, it should be noted that the above preferred embodiments are only used to illustrate the technical solutions of the present invention rather than to limit it. Although the present invention has been described in detail through the above preferred embodiments, those skilled in the art should understand that various changes can be made in form and details without departing from the scope defined by the claims of the present invention.

Claims

1. A lidar positioning and three-dimensional mapping method based on implicit neural field, characterized in that: The following steps are involved: S1 initializes the map and the neural signed distance field NSDF: S1.1 Initialize the map: For the first frame of radar point cloud data, use the unit matrix or constant matrix to transform the input sampling points from the sensor coordinate system to the world coordinate system, and use the points transformed into the world coordinate system to initialize the map. During the map initialization process, randomly generate hierarchical potential features through Gaussian distribution; S1.2 Initialize the neural signed distance field NSDF: optimize the hierarchical latent features in a self-supervised manner, and use the octree data structure to store the optimized latent features to generate a latent feature set F. The features in the latent feature set F are decoded into signed distance values ​​through a shallow multi-layer perceptron MLP to complete the initialization of NSDF. S2 predicted pose and extended NSDF: S2.1 Predicted pose: Starting from the second frame of radar point cloud data, each time a frame is input, the predicted pose is obtained by matching the frame to the implicit map; S2.2 Extended NSDF: Using the predicted pose, the new input point is converted to the world coordinate system, and the Morton value of the new input point is used to determine whether it is in the current map. If the new input point has a corresponding node, no new potential features need to be added; otherwise, the new potential features are also stored hierarchically in the potential feature set F based on the octree data structure; By repeating the operations of predicting pose and expanding NSDF until all radar point cloud data are input, a complete map and the pose of all data frames are obtained, and NSDF construction is completed.

2. The method for laser radar positioning and three-dimensional mapping based on implicit neural field according to claim 1, characterized in that: When generating a potential feature set F in an octree data structure, a table N and a table G are generated simultaneously; wherein the table N is represented as N={{N0},{N1},…,{N(L-2)},{N(L-1)}}, and is used to establish a relationship between hierarchical nodes and hierarchical potential features in the octree data structure; Table G is represented as G = {{G0}, {G1}, ..., {G(L-2)}, {G(L-1)}}, which is used to establish the relationship between the corner points of the hierarchical nodes and the hierarchical potential features; Use Morton encoding to map the three-dimensional coordinates to one-dimensional encoding, combine table N and table G to quickly index the hierarchical potential features, and the Morton code of the corner point of the node in table G corresponds to the index of a single potential feature in table F.

3. The method for laser radar positioning and three-dimensional mapping based on implicit neural field according to claim 1, characterized in that: The predicted pose is obtained by matching the frame to the implicit map, including: Use formula (1) to calculate the pose of the first two frames and Predict the initial pose of the current frame Formula (1) is: Based on the initial pose, the current frame is converted to the world coordinate system, and the SDF value of the input point in the current frame is queried through NSDF; Use the LM algorithm or Adam optimizer to minimize the SDF value to iteratively obtain the predicted pose of the current frame; in, is a transformation matrix expressed as follows: R is a 3X3 rotation matrix, and t is a 3X1 translation vector.

4. The method for laser radar positioning and three-dimensional mapping based on implicit neural field according to claim 1, characterized in that: Determining the size of the map includes: Traverse the maximum node coordinate maxk and the minimum node coordinate mink of each level of the octree; The map size of the current level is calculated according to the formula M = (maxk-mink) / Scube, where Scube is the cube size of MarchingCubes.

5. The method for laser radar positioning and three-dimensional mapping based on implicit neural field according to claim 1, characterized in that: The visualization of the map includes: The implicitly represented map is converted into a visual form using the Marching Cubes algorithm.

6. The method for laser radar positioning and three-dimensional mapping based on implicit neural field according to claim 1, characterized in that: The number of hidden layers of the MLP is 2, the length of the latent feature vector H1 is 12, and the number of octree levels K is 15.

Images