A method for establishing an elevation map

Abstract

This invention relates to a method for constructing an elevation map, belonging to the field of autonomous navigation technology, and solves the problem of driving safety of existing unmanned tracked vehicles in complex environments. The method includes: determining the vehicle's first pose information using point cloud data; determining the vehicle's second pose information using inertial navigation data; extracting a global descriptor from the point cloud data and performing loop closure detection to obtain the vehicle's third pose information; constructing and solving a factor graph with the first, second, and third pose information as observation factors to obtain the vehicle's pose information; stitching the point cloud data based on the pose information to obtain a point cloud map; clustering the point clouds in the point cloud map and constructing an elevation map based on the clustering results.

Description

Technical Field

[0001] This invention relates to the field of autonomous navigation technology, and in particular to a method for creating an elevation map. Background Technology

[0002] With the development of unmanned technology, unmanned tracked platforms have always been a focus of attention as ground-based unmanned platforms due to their superior performance, high safety, strong carrying capacity, and adaptability to complex and harsh environments. In recent years, unmanned tracked platforms have seen tremendous development, and related technologies have been widely applied in various scenarios, such as disaster relief and border patrol.

[0003] When unmanned tracked vehicles navigate complex environments, especially off-road conditions, the significant elevation changes and vehicle attitude variations can lead to unsafe behavior if the three-dimensional environmental features are not considered and 3D motion planning and control are not implemented. This can result in the vehicle exceeding safety limits and posing a substantial threat to driving safety. Therefore, accurate and real-time pose estimation and elevation mapping of unmanned vehicles are essential to maximize driving safety and improve system passability. Summary of the Invention

[0004] Based on the above analysis, the embodiments of the present invention aim to provide a method for establishing elevation maps to solve the problem of driving safety of existing unmanned tracked vehicles in complex environments.

[0005] On one hand, embodiments of the present invention provide a method for creating an elevation map, comprising the following steps:

[0006] The first pose information of the vehicle is determined by the point cloud data of the vehicle.

[0007] The vehicle's second pose information is determined using the vehicle's inertial navigation data;

[0008] Global descriptors are extracted from the point cloud data and loop closure detection is performed to obtain the vehicle's third pose information;

[0009] A factor graph is constructed and solved using the first pose information, the second pose information, and the third pose information as observation factors to obtain the pose information of the vehicle.

[0010] The point cloud data is stitched together based on the pose information to obtain a point cloud map;

[0011] Clustering is performed on the point cloud in the point cloud map, and an elevation map is built based on the clustering results.

[0012] Further improvements to the above method also include:

[0013] The elevation map is input into a pre-trained elevation map completion model to complete the elevation map.

[0014] Based on a further improvement to the above method, the step of extracting global descriptors from the point cloud data and performing loop closure detection to obtain the vehicle's third pose information includes:

[0015] The point cloud data is projected into a distance image;

[0016] The distance image is input into the feature extraction model to obtain the global descriptor of the distance image;

[0017] The K-NN algorithm is used to perform nearest neighbor matching between the global descriptor at the current time and the global descriptors at historical time steps to obtain the vehicle's third pose information.

[0018] Based on a further improvement of the above method, the feature extraction model includes a Transformer neural network and a multilayer perceptron. The Transformer neural network is used to extract feature information from the distance image, and the multilayer perceptron is used to aggregate the feature information into a global descriptor representing whether the distance image at different times represents the vehicle in the same pose.

[0019] Based on a further improvement of the above method, the observation factors of the factor map also include the vehicle's fourth pose information, which is determined by the vehicle's Global Navigation Satellite System (GNSS) data. Furthermore, the initial factors of the factor map are determined by the vehicle's initial pose calculated from the inertial navigation data.

[0020] A further improvement to the above method, after clustering the point clouds in the point cloud map and before building an elevation map based on the clustering results, includes:

[0021] Statistically analyze the distribution information of each type of point cloud in the point cloud map;

[0022] Outliers whose nearest neighbor Euclidean distance is greater than a preset threshold are considered as smoke and filtered out.

[0023] A further improvement to the above method, the step of stitching together the point cloud data based on the pose information to obtain a point cloud map includes:

[0024] The position change information and the second attitude change information are determined by the pose information, wherein the second attitude change information includes heading angle change information.

[0025] The point cloud data is stitched together based on the position change information and the attitude change information to obtain a point cloud map.

[0026] A further improvement to the above method includes, before stitching the point cloud data based on the pose information, the following steps are also included:

[0027] Acquire the point cloud data corresponding to the key frame, wherein the key frame is a frame in which the second pose information change of the vehicle satisfies a preset threshold;

[0028] The point cloud data corresponding to the key frame is downsampled to obtain the sparse point cloud of the key frame;

[0029] Furthermore, the step of stitching together the point cloud data based on the position change information and the attitude change information to obtain a point cloud map includes:

[0030] The sparse point cloud of the keyframe is stitched together based on the position change information and the attitude change information to obtain a sparse point cloud map.

[0031] Based on a further improvement of the above method, the method for establishing the factor graph includes:

[0032] Frames in which the second pose information of the vehicle changes to a preset threshold are set as keyframes.

[0033] A sliding window is established using the R most recent keyframes, where R is an integer greater than 1;

[0034] The first pose information, second pose information, third pose information, and fourth pose information of the vehicle are incrementally inserted into the factor map.

[0035] Based on further improvements to the above method, the elevation map completion model is a U-Net neural network model based on encoder-decoder.

[0036] Compared with the prior art, the present invention can achieve at least one of the following beneficial effects:

[0037] 1. This invention can establish high-quality elevation maps in real time in scenarios where GNSS signals are lost or abruptly changed, or where lidar observations are severely degraded or interfered with by smoke and dust.

[0038] 2. This invention improves the real-time performance of the system and enables environmental perception capabilities for predicting unknown areas by constructing datasets offline and training elevation map completion models using masks.

[0039] In this invention, the above-described technical solutions can be combined with each other to achieve more preferred combinations. Other features and advantages of this invention will be set forth in the following description, and some advantages may become apparent from the description or be learned by practicing the invention. The objects and other advantages of this invention can be realized and obtained from what is particularly pointed out in the description and drawings. Attached Figure Description

[0040] The accompanying drawings are for illustrative purposes only and are not intended to limit the invention. Throughout the drawings, the same reference numerals denote the same parts.

[0041] Figure 1 This is a flowchart illustrating a method for creating an elevation map according to an embodiment of the present invention.

[0042] Figure 2 A schematic diagram illustrating the principle of deep learning-based loop closure detection according to an embodiment of the present invention is shown.

[0043] Figure 3 A flowchart illustrating a training method for an elevation map completion model according to an embodiment of the present invention is shown.

[0044] Figure 4 A schematic diagram of masking a elevation map according to an embodiment of the present invention is shown.

[0045] Figure 5 A schematic diagram of a neural network structure for an elevation map completion model according to an embodiment of the present invention is shown.

[0046] Figure 6 A schematic diagram of a neural network structure of an improved ViT module according to an embodiment of the present invention is shown.

[0047] Figure 7 A schematic diagram of a deep convolutional feedforward network in an improved ViT module according to an embodiment of the invention is shown. Detailed Implementation

[0048] Preferred embodiments of the present invention will now be described in detail with reference to the accompanying drawings, which form part of this application and are used together with the embodiments of the present invention to illustrate the principles of the present invention, but are not intended to limit the scope of the present invention.

[0049] Figure 1 This is a flowchart illustrating a method for creating an elevation map according to an embodiment of the present invention.

[0050] The following is combined with Figure 1 An embodiment of the present invention will be described.

[0051] like Figure 1 As shown, the method for creating this elevation map includes the following steps:

[0052] Step 101: Determine the first pose information of the vehicle using the vehicle's point cloud data.

[0053] The point cloud data here can be point cloud data obtained through LiDAR.

[0054] In this embodiment, line and surface features can be extracted from point cloud data first, and then the vehicle's pose change can be obtained based on inter-frame matching. The process will be described below with reference to a specific embodiment.

[0055] First, the point cloud P can be projected into a distance image R1, represented as: Each pixel in the distance image R1 corresponds to a point in the point cloud. Specifically, for a point p in the point cloud... i = (x,y,z)P, can be transformed using Equation 1.1 to obtain the pixel (u) representing distance information in the distance image. n ,v n ):

[0056]

[0057] In equation 1.1, ξ=|φ up |+|φ down | represents the vertical field of view of the lidar, φ up and φ down The upward and downward tilt angles of the lidar, (x, y, z) represent the three-dimensional coordinates of the point cloud, x n y n z n Let r represent the 3D coordinates of the nth point cloud. n Let w represent the radius from the nth point cloud to the lidar, w represent the width of the distance image, and h represent the height of the distance image.

[0058] Then, feature extraction of lines and surfaces is performed on the distance image. Specifically, let S be the set of consecutive points in the same row of the distance image, and the roughness c can be calculated using Equation 1.2:

[0059]

[0060] In equation 1.2, r i ,r j The distance is the pixel value of the image, which corresponds to the distance from the point cloud to the LiDAR. |S| represents the number of points in set S, and the double vertical lines ‖ represent the modulus.

[0061] Next, the roughness c of the points in each row of the distance image is sorted and then used as a threshold c. th Distinguish between line and surface features. For example, if the roughness c of a point is greater than a threshold c... th If the point roughness c is less than the threshold c, then the point segmentation is a line feature; th Then the points are segmented into surface features.

[0062] Finally, the vehicle's pose change is calculated using lidar. Specifically, the cloud centerline feature E for the next time step is first established using Equation 1.3. k+1Kneading feature H k+1 Line features reprojected onto the point cloud coordinate system of the previous time step Kneading characteristics The relationship between them:

[0063]

[0064] In equation 1.3, X (k+1,i) It is E k+1 or H k+1 The coordinates of feature point i in the middle. yes or The coordinates of the corresponding point, T (k+1,i) (1:3) is the transformation matrix T (k+1,i) =[t x ,t y ,t z ,θ x ,θ y ,θ z ] Τ In the first three rows, R is the rotation matrix defined by the Rodriguez formula (i.e., Equation 1.4):

[0065]

[0066] In equation 1.4, θ = ||T (k+1,i) (4:6)|| represents the last three rows of the transformation matrix; ω is the unit vector of the rotation axis. Then, for each feature point X... (k+1,i) In E k+1 or H k+1 In all cases, a nonlinear function can be established (e.g., Equation 1.5):

[0067]

[0068] In equation 1.5, The normalized feature point distance, n T For line feature E k+1 Directional unit vector or surface feature H k+1 The normal unit vector. Then, the Levenberg-Marquard algorithm is used to optimize the corresponding distance d, making it approach 0, thereby solving for the rotation matrix R and the transformation matrix T. (k+1,i) (1:3). Finally, the solved rotation matrix R and transformation matrix T are used. (k+1,i) (1:3), based on the pose information of the previous moment, the pose information of the vehicle at the next moment is obtained, which is the first pose information of the vehicle in this invention. Step 102: Determine the second pose information of the vehicle through the vehicle's inertial navigation data.

[0069] In this embodiment, the vehicle's pose information can be inferred by pre-integration using inertial navigation data from different times. Specifically, the vehicle's attitude, displacement, and velocity information at time j are inferred using Equation 1.6 using inertial navigation data from frame i to frame j.

[0070]

[0071]

[0072]

[0073] In equation 1.6, q i The quaternion representation q of the vehicle attitude corresponding to the i-th frame i (α i ,β i ,γ i ), α i Represents the pitch angle, β i Represents the roll angle, γ i p represents the heading angle. i V represents the vehicle position corresponding to the i-th frame. i R represents the vehicle speed corresponding to the i-th frame. k For q k The matrix representation, where Δt represents the time interval between two adjacent frames of inertial navigation data, and ω k b represents the angular velocity measured by the gyroscope. g a represents the angular velocity offset. k b represents the acceleration measured by the accelerometer. a This is the acceleration offset.

[0074] Using wheel odometers for pose estimation is prone to significant errors in off-road environments due to factors such as ground undulations, slippage, and rotation. Using the results of inertial navigation data pre-integration to estimate vehicle pose can avoid this problem.

[0075] Step 103: Extract global descriptors from the point cloud data and perform loop closure detection to obtain the vehicle's third pose information.

[0076] Loop closure detection refers to the process by which an autonomous vehicle (RV) can identify and use the loop closure information when it returns to a previously visited location (forming a closed loop) within a certain period of time to correct accumulated positioning errors. Loop closure detection methods include feature-based methods, appearance-based methods, topology-based methods, and consistency-based methods. In feature-based methods, features are extracted from image or sensor data, and a feature database is built. When the RV returns to a previously visited location, loop closures are detected by matching features in the current image with features in the database.

[0077] A global descriptor is a feature vector used in computer vision to represent the content of an image, capturing overall information about the image. The purpose of global descriptors is to enable computers to perform high-level understanding of images, such as identifying scenes, object categories, or comparing the similarity between images. Methods for generating global descriptors include histogram statistics, feature aggregation, and deep learning. In histogram statistics, features describing a scene can be statistically analyzed by manually designing global descriptors.

[0078] In some embodiments, a global descriptor for the distance image R1 can be extracted using a deep learning-based method, and loop closure detection can be performed. Figure 2 A schematic diagram illustrating the principle of deep learning-based loop closure detection according to an embodiment of the present invention is shown below. Figure 2 Please provide an explanation.

[0079] like Figure 2 As shown, frames with a 1m change in position or a 10° change in pose can be designated as keyframes. The corresponding distance image R1 is used to extract global descriptors (also known as "global descriptor vectors") using a deep neural network with a "Transformer + Multilayer Perceptron (MLP)" architecture. Specifically, firstly, the hierarchical Transformer extracts high-dimensional features of the distance image R1 through local and global receptive fields at different spatial resolutions; then, the Multilayer Perceptron (MLP) aggregates the high-dimensional features of the distance image R1 into a compact global descriptor vector. The aggregated global descriptor can characterize whether the distance images R1 at two different times correspond to the same pose; finally, K-NN nearest neighbor matching is performed on the global descriptors of the current frame and historical frames to obtain loop closure detection factors, thereby obtaining the vehicle's pose estimation information.

[0080] In this embodiment, compared to the original point cloud and distance image R1, the global descriptor vector has a lower dimension, resulting in a smaller data volume. Using it for loop closure detection can greatly improve the system's real-time performance.

[0081] Step 104: Establish and solve a factor graph with the first pose information, the second pose information and the third pose information as observation factors to obtain the vehicle pose information.

[0082] A factor graph is a one-way graphical model used to represent the dependencies between variables and factors in a probabilistic model. In this embodiment, the factor graph can be initialized using pose information derived from inertial navigation data (i.e., the second pose information in this invention) to obtain initial factors. Frames with a 1m change in position or a 10° change in attitude are designated as keyframes, and a sliding window is established using the 30 most recent keyframes. Then, the first pose information, second pose information, and third pose information are incrementally inserted into the factor graph for solution, and the following Bayesian network is used for smoothing and mapping:

[0083]

[0084] In this formula, x1, x2, and x3 are three example keyframes, φ(x1,l1), φ(x2,l1), and φ(x3,l2) are three observation factors (i.e., first pose information, second pose information, and third pose information), φ(x1,x2), and φ(x2,x3) are optimized odometry factors, and φ(x1) is the initial factor.

[0085] Based on Bayes' theorem, the probability formula for the most likely pose information of the vehicle can be derived from the formula. Then, the probability problem is transformed into a nonlinear least squares problem, thereby finding a set of pose information that matches all observation factors as closely as possible.

[0086] In some embodiments, Global Navigation Satellite System (GNSS) data of the vehicle can be received, and the position data in longitude, latitude, and altitude coordinate systems can be transformed into position data in eastward, northward, and celestial coordinate systems. Then, the GNSS data is incrementally inserted into a factor graph for solution, and smoothing and mapping are performed using the Bayesian network shown in Equation 1.7. In this embodiment, the GNSS data is inserted into the factor graph as the fourth node, and φ(x1,l1), φ(x2,l1), and φ(x3,l2) in Equation 1.7 are the observation factors for four pose information types.

[0087] It should be noted that the smooth pose information obtained according to Equation 1.7 is the pose information in the embodiments of the present invention.

[0088] In the above embodiments, the factor graph can be used to fuse data from multiple frames together, thereby smoothing and optimizing the vehicle pose information, increasing the perception range and reducing blind spots.

[0089] Step 105: Based on the pose information, stitch together the point cloud data to obtain a point cloud map.

[0090] In this embodiment, based on the smooth pose obtained in step 104, the original point cloud data corresponding to the keyframes in the sliding window can be rotated and translated using Equation 1.8 to obtain the coordinates of the vehicle in the current k-th frame, thereby stitching together the point cloud map:

[0091]

[0092] In equation 1.8, The point cloud map is stitched together for the k-th frame, where N is the total number of frames in the sliding window, and R... k-i and T k-i (1:3) is the rotation and translation matrix.

[0093] In some embodiments, step 105 includes:

[0094] Step S30: Determine position change information and second attitude change information through the pose information, wherein the second attitude change information includes heading angle change information.

[0095] Step S40: The point cloud data is stitched together according to the position change information and the attitude change information to obtain a point cloud map.

[0096] Since the stitched point cloud map is intended for online inference, a sparse representation of the original point cloud data (e.g., a downsampled sparse point cloud) can be used to ensure the real-time performance of the online inference. In this case, the sparse point cloud corresponding to the keyframes in the sliding window can be rotated and translated to the coordinate system of the vehicle's current k-th frame using Equation 1.8, thereby stitching together a sparse point cloud map. In this embodiment, the rotation matrix in Equation 1.8 only includes pitch angle change information, excluding roll angle change information and yaw angle change information. It should be noted that in this embodiment, omitting the rotation of the vehicle's pitch and roll angles ensures that the stitched point cloud map is aligned with the horizontal plane.

[0097] Step 106: Perform clustering processing on the point cloud in the point cloud map and establish an elevation map based on the clustering results.

[0098] In this embodiment, a clustering algorithm can be used to cluster the point clouds in the stitched point cloud map. The clustering algorithm here can be the KD-Tree algorithm. The KD-Tree algorithm, also known as the K-Dimensional Tree algorithm, is a method for partitioning high-dimensional data spaces. It is a binary tree structure where each node represents a partition in K-dimensional space. By recursively selecting one dimension to partition the space, a tree structure is constructed.

[0099] In this embodiment, the elevation map can be stored in the form of a raster map, where the size of each raster represents a fixed length in reality. For example, the elevation map has 224×224 raster cells, and the side length of each raster cell represents 0.2m in reality. In this case, the elevation map can represent the elevation information of 44.8m to the left and right of the vehicle, 67.2m in front, and 22.4m behind.

[0100] In this embodiment, since the elevation map represents the ground portion within its range (i.e., the height information of the ground relative to the sea level or other reference datum), after clustering the point clouds in the stitched point cloud map using a clustering algorithm, other types of point clouds can be discarded, and only the type of point cloud with the lowest cluster center height can be retained.

[0101] In some embodiments, after clustering the point clouds in the point cloud map and before building an elevation map based on the clustering results, the method for building the elevation map further includes: statistically analyzing the distribution information of each type of point cloud in the point cloud map; and treating outliers whose nearest neighbor Euclidean distance is greater than a preset threshold as dust and filtering them out.

[0102] To address the issue of smoke and dust generated by unmanned vehicles driving in off-road environments, this embodiment proposes that after clustering the point cloud in the stitched point cloud map, statistical distribution information can be collected within each cluster, and outliers with a nearest neighbor Euclidean distance greater than a preset threshold are considered smoke and dust and filtered out. For example, the statistical distribution information within each cluster is determined by Equations 1.9 and 1.10:

[0103]

[0104]

[0105] At this point, the preset threshold can be 1.96σ.

[0106] In this embodiment, after the smoke and dust are filtered out, the average height of the lowest N points (e.g., N is 10) in the point cloud retained in each grid can be used as the elevation information of that grid.

[0107] In some embodiments, Figure 1 The methods for creating elevation maps in China include:

[0108] Step 107: Input the elevation map into the pre-trained elevation map completion model to complete the elevation map.

[0109] The training process of the elevation map completion model is explained below.

[0110] Example 2

[0111] Figure 3A flowchart illustrating a training method for an elevation map completion model according to an embodiment of the present invention is shown. Figure 3 As shown, the training method for this elevation map completion model includes the following steps:

[0112] Step 301: Perform random masking on the elevation maps in the training dataset to obtain elevation map masks.

[0113] In this embodiment, the elevation map in the training dataset can be stored in the form of a raster map, where the size of each raster represents a fixed length in reality. For example, the elevation map has 224×224 raster cells, and the side length of each raster cell represents 0.2m in reality. In this case, the elevation map can represent the elevation information of 44.8m to the left and right, 67.2m in front, and 22.4m behind the vehicle.

[0114] In some embodiments, the elevation maps in the training dataset may be obtained in an offline environment based on the elevation maps obtained in steps 101 to 106.

[0115] In the above embodiments, the elevation map in the training dataset can be randomly masked to obtain an elevation map mask. Specifically, the elevation values ​​within a portion of the raster within a preset area of ​​the elevation map can be randomly masked to obtain the elevation map mask.

[0116] Figure 4 A schematic diagram illustrating the masking process of an elevation map according to an embodiment of the present invention is shown. Figure 4 As shown, within each 7×7 grid window, 75% of the elevation values ​​within the grid can be randomly masked to obtain an elevation map mask.

[0117] Step 302: Using the elevation map mask as input and the elevation map before random masking as the desired output, train the initial model of the elevation map completion model to obtain the trained elevation map completion model.

[0118] In this embodiment, the elevation map completion model can be a U-Net neural network model based on an encoder-decoder.

[0119] Figure 5 A schematic diagram of a neural network structure for an elevation map completion model according to an embodiment of the present invention is shown below. Figure 5 The neural network structure of the elevation map completion model in this embodiment of the invention will be described.

[0120] like Figure 5As shown, this elevation map completion model includes an encoder and a decoder. The encoder comprises a block location encoding layer and four feature extraction networks. Each feature extraction network includes a 2D convolutional layer with 3×3 convolutional kernels and an improved ViT module. Here, ViT refers to the Vision Transformer model. The Vision Transformer (ViT) model is a deep learning model with a Transformer architecture used for computer vision tasks. It segments an image into a series of image patches and passes each patch as an input sequence to the Transformer encoder. Each image patch is transformed into a vector representation through a linear projection layer and combined with a location embedding vector, then the output is fed into the Transformer encoder for processing. The Transformer encoder includes multiple self-attention layers and feedforward neural network layers to learn global and local features in the image. Finally, a linear classifier splits the encoder output.

[0121] In this embodiment, the original elevation map is divided into a series of image blocks ( Figure 5 (Not shown) After that, Figure 5 The block location encoding module in the image can encode a series of segmented image blocks to obtain the location embedding vector of each image block. Each image block is also encoded through a linear projection layer (...). Figure 5 (Not shown) is converted into a vector representation, and the vectors are combined and embedded at their positions before being output to the improved ViT module. In this embodiment, the improved ViT module refers to the improved Transformer encoder. Figure 6 A schematic diagram of a neural network structure for an improved ViT module according to an embodiment of the present invention is shown. Figure 6 As shown, along the input-to-output direction, the improved ViT module sequentially includes interconnected tensor deformation operation layers, multi-head self-attention layers, layer normalization layers, deep convolutional feedforward network layers, layer normalization layers, and tensor deformation operation layers. The output of the first tensor deformation operation layer is also interconnected with the first layer normalization layer, and the output of the first layer normalization layer is also interconnected with the second layer normalization layer.

[0122] It should be noted that the improved ViT module in this embodiment of the invention is mainly an improvement on the feedforward neural network layer in the Transformer encoder. Specifically, the improved ViT module in this invention uses a deep convolutional feedforward network. Figure 7 A schematic diagram of a deep convolutional feedforward network in an improved ViT module according to an embodiment of the invention is shown. Figure 7As shown, the deep convolutional feedforward network, along the input-to-output direction, sequentially includes an interconnected tensor deformation operation layer, a deep convolutional layer with 3×3 convolutional kernels, an activation layer (GeLU activation function), another tensor deformation operation layer, a linear layer, another activation layer (GeLU activation function), and another linear layer. The input of this deep convolutional feedforward network is XORed with the output of the second linear layer to obtain the output of the deep convolutional feedforward network. The improved ViT module proposed in this embodiment of the invention uses a deep convolutional feedforward network to extract information within nearby rasters of the elevation map, which can reduce computational costs and improve the perception capability of adjacent elevation rasters. The improved ViT module proposed in this embodiment of the invention employs a multi-head self-attention mechanism to aggregate long-range contextual information within the entire elevation map raster. Return to... Figure 5 The four feature extraction networks in the encoder can downsample the original elevation map (i.e., the elevation map in the training dataset in this paper) through convolution operations with 3x3 grid-sized convolution kernels, thereby mapping it into four feature maps with resolutions ranging from fine to coarse. Similarly, the four feature extraction networks in the decoder can deconvolve the elevation map output by the encoder through 3x3 grid-sized convolution kernels, mapping it into four feature maps with resolutions ranging from coarse to fine. Figure 5 In the example, the encoder and decoder also employ lateral cross-layer connections when working with deep features. Specifically, the output of the second feature extraction network's 2D convolutional layer in the encoder is input to the fourth feature extraction network in the decoder; the output of the third feature extraction network's 2D convolutional layer in the encoder is input to the third feature extraction network in the decoder; and the output of the fourth feature extraction network's 2D convolutional layer in the encoder is input to the first feature extraction network in the decoder.

[0123] It should be noted that, although Figure 5 An example of a neural network structure for an elevation map completion model according to an embodiment of the present invention is shown, but the present invention is not limited thereto. Specifically, the encoder and decoder can employ eight feature extraction networks. Regardless of the number of feature extraction networks employed, as long as the structure of the hierarchical feature extraction network proposed in this invention is adopted, it falls within the protection scope of this invention. The convolutional kernels of the encoder and decoder can also be convolutional kernels of other grid sizes (e.g., 9×9 grid size).

[0124] In this embodiment, a U-shaped encoder-decoder network can be constructed using pre-trained ViT neural network parameters. Then, the masked elevation map can be used as input to the U-shaped network, and the mean square error between the output elevation map and the unmasked elevation map can be used as the loss function for self-supervised fine-tuning of the model. Once training is complete, the elevation map completion model is obtained.

[0125] In some embodiments, if the elevation maps in the training dataset are obtained according to steps 101 to 106, data augmentation methods such as random rotation, random cropping, and random symmetry can be used to perform data augmentation processing on the elevation maps in the training dataset, thereby obtaining more elevation maps for training.

[0126] In some implementations, a learning rate adjustment strategy using linear warm-up and cosine annealing is employed to train the elevation map completion model.

[0127] In some embodiments, due to the obstruction of the laser by objects and terrain, some grids in the elevation map obtained according to steps 101 to 106 may lack elevation values. In this embodiment, bilinear interpolation can be performed near blank grids (referring to grids without elevation values ​​in the elevation map) according to Equation 1.11 (e.g., a 12×12 window around a blank grid) to complete the elevation information of the grids.

[0128]

[0129] In equation 1.11, Q ij Let the coordinates be (u,v) = (x i ,y j The elevation values ​​under (i,j∈(1,2)).

[0130] Next, the completed elevation map can be height-normalized, with height values ​​limited to [0~255], so that the highest point of the entire elevation map is 255 and the lowest point is 0. The normalized height values ​​are used as the RGB channel values, and the elevation map is stored as an image to construct the dataset required for self-supervised training.

[0131] Returning to step 107, during online operation, after obtaining the elevation map, the elevation map can be automatically completed based on the trained elevation map completion model, thereby predicting the elevation values ​​of environments that cannot be observed by lidar.

[0132] In some embodiments, to meet real-time requirements, the input elevation map in step 107 can be a sparse elevation map generated based on keyframe pose stitching of a sparse point cloud map. In this embodiment, parallel computation can be performed on the CPU and GPU, using the CPU to stitch the sparse point cloud and the GPU to efficiently infer the neural network model, thereby avoiding the extremely large computational load after stitching dense point clouds.

[0133] In some embodiments, when obtaining the elevation map of the training dataset according to steps 101 to 106, step 105 includes:

[0134] Step S10: Determine the position change information and the first attitude change information through the pose information, wherein the first attitude change information includes pitch angle change information, roll angle change information, and yaw angle change information.

[0135] Step S20: The point cloud data is stitched together according to the position change information and the attitude change information to obtain a point cloud map.

[0136] If the stitched point cloud map is intended for constructing a training dataset, dense raw point cloud data can be used. In this case, the raw point cloud data corresponding to the keyframes in the sliding window can be rotated and translated to the coordinate system of the vehicle's current k-th frame using Equation 1.8, thereby stitching together a dense point cloud map. In this embodiment, the rotation matrix in Equation 1.8 includes pitch angle change information, roll angle change information, and heading angle change information. In some embodiments, if the elevation map of the training dataset is obtained according to steps 101 to 106, after clustering the point clouds in the point cloud map and before establishing the elevation map based on the clustering results, the method for establishing the elevation map of the training dataset further includes: statistically analyzing the distribution information of each type of point cloud in the point cloud map; and treating outliers with a nearest neighbor Euclidean distance greater than a preset threshold as smoke and filtering them out.

[0137] To address the issue of smoke and dust generated by unmanned vehicles driving in off-road environments, this embodiment proposes that after clustering the point cloud in the stitched point cloud map, statistical distribution information can be collected in each class, and outliers with a nearest neighbor Euclidean distance greater than a preset threshold are considered as smoke and dust and filtered out. For example, the statistical distribution information in each class is determined by Equations 1.9 and 1.10 above.

[0138] Compared with the prior art, the embodiments of the present invention can achieve at least one of the following beneficial effects:

[0139] 1. This invention can establish high-quality elevation maps in real time in scenarios where GNSS signals are lost or abruptly changed, or where lidar observations are severely degraded or interfered with by smoke and dust.

[0140] 2. This invention improves the real-time performance of the system and enables environmental perception capabilities for predicting unknown areas by constructing datasets offline and training elevation map completion models using masks.

[0141] The above description is only a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for creating an elevation map, characterized in that, Includes the following steps: The first pose information of the vehicle is determined by the point cloud data of the vehicle. The vehicle's second pose information is determined using the vehicle's inertial navigation data; Global descriptors are extracted from the point cloud data and loop closure detection is performed to obtain the vehicle's third pose information; A factor graph is constructed and solved using the first pose information, the second pose information, and the third pose information as observation factors to obtain the pose information of the vehicle. The point cloud data is stitched together based on the pose information to obtain a point cloud map; Clustering is performed on the point cloud in the point cloud map, and an elevation map is built based on the clustering results; The step of extracting global descriptors from the point cloud data and performing loop closure detection to obtain the vehicle's third pose information includes: The point cloud data is projected into a distance image; The distance image is input into the feature extraction model to obtain the global descriptor of the distance image; The K-NN algorithm is used to perform nearest neighbor matching between the global descriptor at the current time and the global descriptors at historical time steps to obtain the vehicle's third pose information.

2. The method for establishing according to claim 1, characterized in that, Also includes: The elevation map is input into a pre-trained elevation map completion model to complete the elevation map.

3. The method for establishing according to claim 1, characterized in that, The feature extraction model includes a Transformer neural network and a multilayer perceptron. The Transformer neural network is used to extract feature information from the distance image, and the multilayer perceptron is used to aggregate the feature information into a global descriptor that represents whether the distance image at different times represents the vehicle in the same pose.

4. The method for establishing according to claim 1, characterized in that, The observed factors of the factor map also include the vehicle's fourth pose information, which is determined by the vehicle's Global Navigation Satellite System (GNSS) data. Furthermore, the initial factors of the factor map are determined by the vehicle's initial pose calculated from the inertial navigation data.

5. The method for establishing according to claim 1, characterized in that, After clustering the point clouds in the point cloud map and before building the elevation map based on the clustering results, the process also includes: Statistically analyze the distribution information of each type of point cloud in the point cloud map; Outliers whose nearest neighbor Euclidean distance is greater than a preset threshold are considered as smoke and filtered out.

6. The method for establishing according to claim 5, characterized in that, The step of stitching together the point cloud data based on the pose information to obtain a point cloud map includes: The position change information and the second attitude change information are determined by the pose information, wherein the second attitude change information includes heading angle change information. The point cloud data is stitched together based on the position change information and the attitude change information to obtain a point cloud map.

7. The method for establishing according to claim 6, characterized in that, Before stitching the point cloud data based on the pose information, the method further includes: Acquire the point cloud data corresponding to the key frame, wherein the key frame is a frame in which the second pose information change of the vehicle satisfies a preset threshold; The point cloud data corresponding to the key frame is downsampled to obtain the sparse point cloud of the key frame; Furthermore, the step of stitching together the point cloud data based on the position change information and the attitude change information to obtain a point cloud map includes: The sparse point cloud of the keyframe is stitched together based on the position change information and the attitude change information to obtain a sparse point cloud map.

8. The method for establishing according to claim 4, characterized in that, The method for establishing the factor graph includes: Frames in which the second pose information of the vehicle changes to a preset threshold are set as keyframes. A sliding window is established using the R most recent keyframes, where R is an integer greater than 1; The first pose information, second pose information, third pose information, and fourth pose information of the vehicle are incrementally inserted into the factor map.

9. The method for establishing according to claim 2, characterized in that, The elevation map completion model is a U-Net neural network model based on an encoder-decoder.

Images