A 2D map optimization method based on Gaussian process

Abstract

The application provides a 2D map optimization method based on a Gaussian process, comprising: obtaining current frame original point cloud data; determining a plurality of map test positions to be optimized based on the current frame original point cloud data; taking the current frame original point cloud data as Gaussian process training data, calculating predicted coordinates and corresponding variances of all test positions through a Gaussian process conditional distribution; screening target coordinates from the predicted coordinates of all test positions based on the variances to obtain a current frame predicted point set; adopting a target registration algorithm to register the current frame predicted point set with a historical world map to solve a coordinate transformation matrix, and aligning the current frame predicted point set to a global coordinate system by using the transformation matrix; and performing map fusion on the aligned current frame predicted point set and the historical world map to obtain an updated world map. Through implementation of the application, the global spatial consistency of the map can be improved, and ghosting interference can be reduced.

Description

Technical Field

[0001] This invention belongs to the field of map optimization technology, specifically relating to a 2D map optimization method based on Gaussian processes. Background Technology

[0002] In industrial settings, the construction of 2D point cloud maps often relies on single-line LiDAR. Mainstream algorithms such as gmapping, karto, and cartographer can all construct 2D point cloud maps. These existing algorithms all use occupancy grid maps as the map representation, abstracting the environment map as a random variable field composed of uniformly spaced grids. Since the advent of cartographer, occupancy grid-based 2D mapping schemes have formed a mature system. However, under the high-speed, bumpy operating conditions of mobile robots equipped with single-line LiDAR, this type of scheme has inherent bottlenecks in both accuracy and computational efficiency, making it difficult to generate compact, detailed point cloud maps. This manifests as blurred map environment outlines and noticeable ghosting from multiple scans of the same area.

[0003] Meanwhile, the flatness of the industrial site is difficult to guarantee. Uneven ground will cause irregular bumps during the robot's movement, causing instantaneous shifts in the LiDAR observation pose, further aggravating the map ghosting problem, and seriously damaging the global spatial consistency of the entire map. Summary of the Invention

[0004] In view of this, the purpose of this invention is to provide a 2D map optimization method based on Gaussian process to meet the need to improve the global spatial consistency of the map.

[0005] To achieve the above objectives, the present invention provides the following technical solution: According to a first aspect, the present invention provides a 2D map optimization method based on Gaussian processes, comprising: acquiring raw point cloud data of the current frame; determining multiple map test locations to be optimized based on the raw point cloud data of the current frame; using the raw point cloud data of the current frame as training data for the Gaussian process, calculating the predicted coordinates and corresponding variances of all test locations through the conditional distribution of the Gaussian process; filtering target coordinates from the predicted coordinates corresponding to all test locations based on the variance to obtain a predicted point set for the current frame; employing a target registration algorithm to register the predicted point set of the current frame with the historical world map and solve for the coordinate transformation matrix, aligning the predicted point set of the current frame to the global coordinate system using the transformation matrix; and performing map fusion with the aligned predicted point set of the current frame and the historical world map to obtain an updated world map.

[0006] According to a second aspect, embodiments of the present invention provide an electronic device, the device comprising: a memory, a processor, and a computer program stored in the memory and executable on the processor, the processor performing the steps of the 2D map optimization method based on Gaussian process described in the first aspect or any embodiment of the first aspect.

[0007] According to a third aspect, embodiments of the present invention provide a computer storage medium storing computer instructions that, when executed by a processor, implement the steps of the 2D map optimization method based on a Gaussian process as described in the first aspect or any embodiment of the first aspect.

[0008] This embodiment provides a 2D map optimization method based on Gaussian processes. It uses Gaussian processes to predict and reconstruct the original laser point cloud, selecting high-confidence prediction points based on the prediction variance to form a prediction point set. This filters out noise, outliers, and low-confidence observations, reducing map ghosting and blurring caused by bumps and jitter at the source, and improving the density of 2D point cloud map contour details. During the mapping process, GP reconstruction effectively addresses map ghosting caused by unstable external environments such as uneven ground and vehicle bumps, resulting in a more continuous and compact map.

[0009] Other advantages, objectives, and features of the invention will be set forth in the following description and will be apparent to those skilled in the art in some respects, or may be learned by practice of the invention. The objectives and other advantages of the invention can be realized and obtained through the following description. Attached Figure Description

[0010] To make the objectives, technical solutions, and beneficial effects of this invention clearer, the following figures are provided for illustration: Figure 1 This is a flowchart illustrating a specific example of a 2D map optimization method based on Gaussian processes in this invention. Figure 2 This is an example diagram of map rasterization and direction selection in this invention; Figure 3 This is a schematic diagram of bidirectional Gaussian fitting in this invention; Figure 4 This is a schematic diagram of the predicted selection region in this invention; Figure 5 This is a schematic diagram of the iterative GP point set registration method in this invention; Figure 6 This is a schematic block diagram of a specific example of an electronic device in an embodiment of the present invention. Detailed Implementation

[0011] The technical solution of the present invention will now be clearly and completely described with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0012] In the description of this invention, it should be noted that, unless otherwise explicitly specified and limited, the terms "installation," "connection," and "linking" should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral connection; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium; they can also refer to the internal connection of two components; and they can refer to a wireless connection or a wired connection. Those skilled in the art can understand the specific meaning of the above terms in this invention based on the specific circumstances.

[0013] Furthermore, the technical features involved in the different embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0014] This invention provides a 2D map optimization method based on Gaussian processes, such as... Figure 1 As shown, it includes: S101, Obtain the original point cloud data of the current frame; S102, Based on the original point cloud data of the current frame, determine multiple map test locations to be optimized; S103: Using the original point cloud data of the current frame as Gaussian process training data, calculate the predicted coordinates and corresponding variances of all test locations through Gaussian process conditional distribution. S104, based on variance, filter the target coordinates from the predicted coordinates corresponding to all test positions to obtain the prediction point set of the current frame; S105, using the target registration algorithm, registers the current frame prediction point set with the historical world map to solve the coordinate transformation matrix, and uses the transformation matrix to align the current frame prediction point set to the global coordinate system; S106, the aligned current frame prediction point set is merged with the historical world map to obtain the updated world map.

[0015] For example, the raw point cloud data of the current frame can be acquired by scanning with a mobile robot equipped with LiDAR.

[0016] Next, based on the original point cloud data of the current frame, the original map extent is determined. Then, as Figure 2As shown, the original map area is divided into multiple evenly spaced and non-overlapping regions. Specifically, the original map area is divided into... Decomposed into multiple sub-regions This sub-region can be a square sub-region, where the decomposition rules are determined by the original map extent. The size and side length of the square region are determined by the original map area. The selection is limited to a rectangle that completely covers all points, with its boundaries set to an integer multiple of the grid size. This rectangle is then divided into evenly spaced, non-intersecting square sub-regions, represented by grayscale lines in the image. After division, regions with more than a threshold of original point cloud data in the current frame are selected as target regions. The threshold can be 2, meaning sub-regions containing no points or fewer than 2 points are deleted. Regions with more than 2 points are selected as target regions. These target regions are used for map construction. See [link to map description]. Figure 2 The blue area in the image.

[0017] Next, principal component analysis is used to extract the normal vectors of the original point cloud data in each target region; based on the angle between the normal vectors and the two-dimensional coordinate axes, the sampling direction of the test position in each target region is determined; based on the sampling direction of the test position in each target region, sampling is performed on the corresponding coordinates of each target region to obtain the map test position.

[0018] Specifically, set It belongs to A subset of point set data for The j-th point cloud is defined. Then, Principal Component Analysis (PCA) is used to find a normal direction for each subset of points, where the center of each subset of points is... Defined as:

[0019] in, express The total number of point clouds.

[0020] Covariance matrix of point locations It can be represented as:

[0021] matrix The eigenvalues ​​are decomposed into:

[0022] in , yes eigenvalues, , This is the corresponding normalized eigenvector. The smallest eigenvalue is taken. Corresponding feature vector As a point The normal vector of the subset. The angles of the normal direction relative to the x-axis and y-axis are:

[0023] in, for The set of eigenvectors corresponding to the smallest eigenvalue. It is a two-dimensional unit vector basis matrix.

[0024] In this embodiment, the functional relationship between the x and y coordinates of each sub-region is determined based on these angles. For example, the angle of the normal direction relative to the y-axis being less than the angle of the x-axis exceeds a threshold. , such as in Figure 2 area a is composed of Since the functional relationship is determined, the x-coordinate is set as the training point. The position of region b is the opposite of region b, and the functional relationship is set as follows: The y-coordinate is set as the training point. If the angle difference between the two is less than the threshold, then... Then, two training point locations are set, and GP reconstruction is performed in both directions of the sub-region, such as... Figure 2 As shown. Once the relationships between each sub-region are determined, the test locations for the map to be optimized can be selected. ,like Figure 2 As shown, if the functional relationship is If the x-coordinates are uniformly distributed within a sub-region, then the test location of the map to be optimized is set as a series of uniformly distributed x-coordinates within that sub-region; otherwise, a series of uniformly distributed y-coordinates within that sub-region is selected as the test location of the map to be optimized. For example... Figure 3 As shown, if both directions are involved, the x and y coordinates are used as test locations on the map to be optimized, based on... Figure 3 As shown in the bidirectional Gaussian fitting, the matching point pairs formed by the original points and interpolated points can produce a smooth curve. Uniformly spaced sampling of test locations ensures a fixed correspondence between subsequent Gaussian training and matching point pair generation. This guarantees stable matching point pairs during iterative registration, preventing random jumps in the point correspondence that could lead to registration divergence and improving the stability of the registration iteration convergence.

[0025] In this embodiment, a Gaussian regression model is applied to the modification of point cloud data in map reconstruction. There are two objectives to be achieved in map reconstruction: the first is to downsample the original point data and convert it into a form suitable for map updates; the second is to adjust the newly formed points to the desired locations through state estimation. Considering the implementation process of these two objectives, this embodiment can be considered a type of interpolation. It pre-trains the Gaussian regression model with the aforementioned determined training points, enabling the model to learn the distribution pattern of continuous contours based on these real laser points, fit the environmental implicit function, and ultimately use the training location and predicted values ​​as the result of map construction.

[0026] Nonlinear regression problems are typically formulated as follows: Given a training dataset , For the training point location, To train the observation labels, test locations on the map to be optimized. It can be calculated In the Gaussian regression model, ,in, , for measuring noise.

[0027] Specifically, the original point cloud of the current frame is used as the training data for the Gaussian process to construct training position vectors and training observation vectors. The exponential covariance kernel function is used to solve the training covariance matrix using the training position vectors, and the observation covariance matrix is ​​obtained by superimposing the observation noise term. Combined with the test position, the cross kernel matrix between the test and training and the self-kernel matrix of the test position are calculated respectively. The observation covariance matrix, cross kernel matrix, test self-kernel matrix and training observation vector are substituted into the conditional prediction distribution calculation formula to obtain the predicted coordinates and corresponding variances of each test position. The conditional prediction distribution formula is derived based on the conditional distribution of the Gaussian process.

[0028] Define training observation sequence Training position and test location The joint distribution is obtained as follows:

[0029] Among them, kernel function , To train the observations, This is the test value, that is, the test location. The corresponding function value, which is the output that the model wants to predict, is known at the training points as s=f(l)+ At the test point, to predict =f( ), This is the average value. This is the noise diagonal matrix. To test the positional kernel matrix, This represents the cross-kernel matrix between testing and training. The kernel function is chosen using the exponential covariance function:

[0030] in It is a length scale parameter. Then in under conditions The predicted distribution is:

[0031] in, The predicted coordinates corresponding to the test location. This represents the corresponding variance.

[0032] Compared to the original data, the point cloud data reconstructed by GP is more regular and sparse, which makes the point cloud registration scheme more efficient and accurate in data association, thereby reducing the number of iterations and achieving higher registration accuracy while taking less time.

[0033] By uniformly distributing test locations, a Gaussian regression model can be used to calculate the predicted locations and their variance. To approximate continuity, each The point predictions all revolve around The model uses the average value. This embodiment uses the predicted locations and their test locations as prediction points. Each prediction is actually a linear combination of observations in that area. The weight of each element in the linear combination depends on the distance between the test location and the training location of the prediction point. Test points farther from the training location will always produce fuzzy predictions with larger biases (smaller weights); these predictions must be discarded to obtain an accurate map. Figure 4 As shown, the predicted points and their variance σ are used Figure 4 The region 'a' is represented by colored scatter plots. A latent, precise point-set map is hidden throughout the prediction. Figure 4 Regions b, c, and d illustrate the relationship between the original points and the predicted points, by selecting predicted points with variance σ below a certain threshold. Figure 4 The variance σ of region b is less than or equal to 0.7. Figure 4 The variance σ of region c is less than or equal to 0.4, ultimately resulting in a class point set mapping ( Figure 4 The d region), which is the current frame prediction point set, Figure 4 The results are shown with σ≤0.7, σ≤0.4, and σ≤0.1. If the map points are too sparse and the registration is unstable, the threshold should be increased. If there are many ghostings or noise, the threshold should be decreased.

[0034] In the above embodiments, determining the number of test locations prevents the map state size of each sub-region from exceeding a specified number during the SLAM process. Fixing the values ​​of the test locations ensures that the correspondence between the predicted point pairs used for state estimation is constant, thereby allowing the map state vector to be updated during the map update step.

[0035] Next, a target registration algorithm is used to register the current frame's predicted point set with the historical world map and solve for the coordinate transformation matrix. The transformation matrix is ​​then used to align the current frame's predicted point set to the global coordinate system. The target registration algorithm can be an ICP point cloud registration algorithm, or it can be a raster-based correlation-based brute-force scanning matching algorithm like the one used by Karto.

[0036] Finally, the aligned current frame prediction point set is merged with the historical world map to obtain the updated world map. Specifically, a recursive Gaussian process can be used to add the registered point cloud to the map.

[0037] This invention provides a 2D map optimization method based on Gaussian processes. It employs Gaussian processes to predict and reconstruct the original laser point cloud, selecting high-confidence prediction points based on the prediction variance to form a prediction point set. This filters out noise, outliers, and low-confidence observations, reducing map ghosting and blurring caused by bumps and jitter at the source, and improving the density of 2D point cloud map contour details. During the mapping process, Gaussian reconstruction effectively addresses map ghosting caused by unstable external environments such as uneven ground and vehicle bumps, resulting in a more continuous and compact map.

[0038] As an optional implementation, a target registration algorithm is used to register the current frame's predicted point set with the historical world map, solve for the coordinate transformation matrix, and then use the transformation matrix to align the current frame's predicted point set to the global coordinate system, including: The historical world map is divided into multiple uniformly spaced and non-overlapping sub-regions. Based on the target region where the test location is located in the current frame's predicted point set, the regions divided in the historical world map are matched to obtain a matching region. Matching point pairs are generated based on these matching regions. The Euclidean distance error is minimized based on the coordinates of the matching point pairs to obtain a coordinate transformation matrix. The original point cloud data of the current frame is transformed using the coordinate transformation matrix, and a new set of predicted points for the current frame is generated. A target error algorithm is used to determine the error between the coordinates of the new set of predicted points for the current frame and the coordinates of the corresponding matching points in the historical world map. If the error does not meet a preset condition, the steps of matching the target region where the test location is located in the current frame's predicted point set with the regions divided in the historical world map are repeated until the target error meets the preset condition.

[0039] For example, this embodiment uses point cloud registration for state estimation. Similar to the ICP method, a rigid transformation between corresponding point pairs is calculated from two frames of laser measurement data. Therefore, the registration scheme requires an initial value, which can be provided by other sensors. Assume two misaligned point sets Q and P. In a frame of reference, In the current coordinate system, where , Given the initial transformation, a point P in the current coordinate system can be roughly transformed to the reference coordinate system Q. The corresponding point pair... Determined based on two frames of point data.

[0040] Compared to ICP, which finds the nearest neighbor, this embodiment uses predicted point pairs with the same test location and variance below the same threshold. and .like Figure 5 As shown, Figure 5 Region a consists of two misaligned original point sets P (blue) and Q (red). Figure 5 The prediction point pair corresponding to region b and , Figure 5 Region c is the aligned prediction point pair and After 20 iterations, they almost overlapped. Figure 5 The d region is the original point after alignment.

[0041] The construction details corresponding to the points are as follows Figure 4 As shown: Assume that point Q in the reference frame and point P in the current coordinate system are in the same coordinate system after the initial transformation. First, the point correspondence of each sub-region is constructed by identifying overlapping regions. Therefore, the historical world map needs to be divided into multiple uniformly spaced and non-intersecting sub-regions according to the original map segmentation method. Based on the target region where the corresponding test position is located in the current frame prediction point set, the regions divided by the historical world map are matched to obtain the matching region. During the matching process, the following two requirements must be met: they have the same center within the square region and have the same functional relationship for them to be considered overlapping.

[0042] Once the overlapping sub-regions are identified, the test locations for the two sub-regions are considered identical, based on the test location selection rules introduced in the previous section. Predicted points with identical test locations are then considered corresponding point pairs.

[0043] Redefining in xy and for and Corresponding point pairs ( Figure 5Point pairs in region b have the same x-coordinate and the same y-coordinate. The rigid body transformation is obtained by solving the problem of minimizing the Euclidean distance error.

[0044] This can be solved analytically using singular value decomposition. More precisely, and These are calculated using the center calculation formula for each subset of points in the above embodiments. and If the center is , then the covariance is:

[0045] let The singular value decomposition of the optimal transformation H yields R and t as follows:

[0046] Where R is the rigid rotation matrix and t is the translation matrix, collectively referred to as the coordinate transformation matrix. These are the right singular vector matrix and the left singular vector matrix, respectively. In a given iteration, when calculating the transformation based on the corresponding predicted point pair, this transformation becomes the initial transformation for the next iteration. The mapping construction process of the current frame point data P is repeated to obtain better point pairs, and a new transformation is calculated from these, until the error is below a certain threshold.

[0047] Unlike ICP-based methods, error metrics cannot be assigned as the sum of squares of distances between corresponding points (the so-called "point-to-point" error metric) or the sum of squares of distances from each point to the plane perpendicular to the target normal (the so-called "point-to-plane" error metric). This is because these error metrics do not converge in a decreasing manner as the iterative registration process progresses. This occurs because the assumed points for alignment change with each iteration, as the point data P of the current frame is reconstructed using GP at the beginning of each iteration.

[0048] Therefore, this embodiment proposes a new error metric, called the "E" error metric, also known as the target error algorithm. The target error algorithm determines the error between the coordinates of the new current frame predicted point set and the corresponding matching point coordinates on the historical world map. This includes: calculating the first covariance matrix corresponding to the current frame predicted point set and the second covariance matrix corresponding to the corresponding matching point coordinates on the historical world map, based on the coordinates of the current frame predicted point set and the corresponding matching point coordinates on the historical world map; performing eigenvalue decomposition on the first and second covariance matrices respectively, obtaining two sets of eigenvalues ​​for the first and second covariance matrices; calculating the absolute difference between the eigenvalues ​​corresponding to the two sets of covariance matrices, and summing all the absolute differences to obtain the error.

[0049] Specifically, and These represent the predicted points in the current frame and their corresponding matching points on the historical world map. The first covariance matrix corresponding to the predicted point set in the current frame and the second covariance matrix corresponding to the coordinates of the matching points on the historical world map are calculated respectively. The first covariance matrix... Second covariance matrix The covariance matrix can still be calculated using the formula for solving the covariance matrix in the above embodiments. The first covariance matrix... Second covariance matrix The eigenvalues ​​are decomposed into:

[0050] in, , These are two sets of eigenvalues ​​of the first covariance matrix. , These are the two sets of eigenvalues ​​of the second covariance matrix. for The orthogonal eigenvector matrix, for The orthogonal eigenvector matrix.

[0051] Then, the absolute difference between the eigenvalues ​​is used as an error metric, which converges as the iteration process continues:

[0052] If the target error metric meets a preset condition, the iteration process will terminate. The preset condition is an error less than 0.006 or the process exceeding 20 iterations. The aligned predicted point pair registration result is then obtained as follows: Figure 5 As shown in region c. The conversion of R and t also calibrates the original laser measurements with high precision, such as... Figure 5 The d region is shown.

[0053] This embodiment quantifies the spatial distribution of point sets by using the eigenvalues ​​of the covariance matrix, and is not affected by the offset of single-point coordinates. It can still stably determine the registration convergence state even in scenarios such as robot bumps and local point cloud missingness.

[0054] This invention provides a 2D map optimization method based on Gaussian processes. It reuses the unified map partitioning rules to match historical map sub-regions, generates matching point pairs based on the same test location, and ensures that each matching point pair corresponds one-to-one, thereby reducing erroneous matching point pairs and registration errors. Furthermore, it employs an iterative optimization mechanism, updating the predicted point set and verifying the error after each registration until the error converges. This gradually corrects the instantaneous pose shift caused by robot jitter, improves the accuracy of solving the inter-frame coordinate transformation matrix, and weakens the impact of radar jitter on the overall map shift.

[0055] As an optional implementation, the aligned current frame prediction point set is fused with the historical world map to obtain an updated world map, including: If any point exists only in the historical world map, it is retained; if any point exists only in the current frame prediction point set, it is added to the historical world map; if any point is an overlap point between the current frame prediction point set and the historical world map, the target fusion algorithm is used to perform map fusion.

[0056] For example, this embodiment employs a scan-to-map matching strategy; when reading the first frame of data, it is initialized as a world map. Using the transformation calculated by the registration method, the data is aligned frame by frame with the world map, and then the data is reconstructed into a Gaussian prediction point map as the current map. In each loop, the current map should be merged with the world map to produce an updated world map.

[0057] The map update method is as follows: Test locations of Gaussian prediction points contained in the historical world map and the current map can be divided into three categories: locations contained only in the historical world map, locations contained only in the current map, and overlapping locations appearing in both maps. Predicted points for locations in the first category remain unchanged. For test locations in the second category, predicted points in the current map are directly added to the historical world map as a newly explored environment area. For overlapping areas, a target fusion algorithm is used for fusion, which can be a recursive estimation algorithm, to update the Gaussian map.

[0058] This invention provides a 2D map optimization method based on Gaussian processes, which classifies all points into three categories for differentiated processing: historically unique, current frame unique, and overlapping points. It fully preserves the historically detected environment, directly expands the map with newly added areas, and performs weighted fusion of overlapping areas, taking into account both map integrity and update accuracy, and avoiding contour distortion caused by the loss of old map information or direct overlay of new observations.

[0059] As an optional implementation method, a target fusion algorithm is used for map fusion, including: Extract overlapping point data from the historical world map and the overlapping point data from the current frame's predicted point set. The overlapping point data from the historical world map includes historical world map coordinates and historical observation variance, while the overlapping point data from the current frame's predicted point set includes current coordinates and current observation variance. Perform a fusion calculation on the overlapping point data from the historical world map and the overlapping point data from the current frame's predicted point set to solve for the fused updated coordinates and fused observation variance. Replace the overlapping points in the historical world map with the fused updated coordinates and fused observation variance to complete the map fusion.

[0060] For example, in sequence data processing, a common approach to solving this Gaussian regression problem is a recursive Gaussian process, which recursively updates the entire prediction distribution. However, this method requires calculating the covariance matrix. The inverse matrix of the matrix would reduce computational speed. In this embodiment, considering the efficiency of the proposed method, we choose to update each predicted mean at the selected test location instead of updating the posterior Gaussian distribution. Similar to the occupancy grid map, it is assumed that each predicted point in the Gaussian point map is independent of other points. Under this assumption, a map update algorithm is proposed that uses a recursive least squares estimation method to update overlapping regions. The recursive least squares estimation method is generally stated as follows: Assuming A is a constant vector and Z represents the first k observations of A, the measurement equation is:

[0061] in and H is the known measurement matrix, and C is the noise vector. The (k+1)th measurement is:

[0062] The weighted least squares estimate of A obtained from the first k observations is:

[0063] Where W is composed of:

[0064] set up For the reason The estimated covariance matrix is ​​calculated. Then, the (k+1)th weighted least squares estimate and its covariance matrix are calculated:

[0065] Then, recursive least squares estimation is performed on each prediction in the world map using the two formulas mentioned above.

[0066] Assume there exists any pair of overlapping prediction points and These belong to the world map and the current map, respectively. Here is the predicted location on the world map, where l represents the location. For the corresponding variance, This is the predicted location value on the current map, where l represents the location. This represents the corresponding variance.

[0067] The variables in the above formula take respectively Because a map update process is performed in each iteration of the SLAM process, the first k observations are reduced to 1. Least squares estimation and its covariance This is the value used for map updates. Under this condition, calculating the (k+1)th weighted least squares estimate and its covariance matrix formula is transformed into the following two formulas, similar to the standard binary state Bayes filtering method:

[0068] in, For the variance of the merged observations, For the updated coordinates after merging, For historical observation variance, The current observation variance, The current coordinates, For historical world map coordinates.

[0069] Finally, the single predicted GP points in the overlapping region are updated to , The test location prediction value is shown in the updated world map, where l represents the location to be predicted. This represents the corresponding variance.

[0070] The map update method guarantees local convergence of the predictions. The proof is as follows: Assume the covariance of a single predicted point in the first world map is... Its covariance in the i-th current frame is Because the prediction point selected in this embodiment Below the same threshold, .Then The calculation formula can be rewritten at step k of the SLAM process as follows: Therefore, as the SLAM process proceeds...

[0071]

[0072] The above formula shows that the covariance of the predicted points converges to a very small value after a few steps. From The calculation formula shows that, due to The coefficient is relatively large, and the updated Will tend to .

[0073] This invention provides a 2D map optimization method based on Gaussian process. The above formula can be used for fast calculation without complex matrix inversion operations, resulting in extremely low computational overhead. It avoids the computational bottleneck caused by the traditional recursive Gaussian process iterative inversion and can run in real time on embedded devices.

[0074] This application also provides an electronic device, such as... Figure 6 As shown, processor 501 and memory 502 are connected via a bus or other means.

[0075] Processor 501 can be a central processing unit (CPU). Processor 501 can also be other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, or combinations of the above types of chips.

[0076] The memory 502, as a non-transitory computer-readable storage medium, can be used to store non-transitory software programs, non-transitory computer-executable programs, and modules, such as the program instructions / modules corresponding to the 2D map optimization method based on Gaussian processes in this embodiment of the invention. The processor executes various functional applications and data processing by running the non-transitory software programs, instructions, and modules stored in the memory.

[0077] Memory 502 may include a program storage area and a data storage area. The program storage area may store the operating system and applications required for at least one function; the data storage area may store data created by the processor, etc. Furthermore, the memory may include high-speed random access memory and non-transitory memory, such as at least one disk storage device, flash memory device, or other non-transitory solid-state storage device. In some embodiments, memory 502 may optionally include memory remotely located relative to the processor, which can be connected to the processor via a network. Examples of such networks include, but are not limited to, the Internet, corporate intranets, local area networks, mobile communication networks, and combinations thereof.

[0078] The one or more modules are stored in the memory 502, and when executed by the processor 501, they perform actions such as... Figure 1 The 2D map optimization method based on Gaussian process in the illustrated embodiment.

[0079] For specific details regarding the aforementioned electronic devices, please refer to the relevant documentation. Figure 1 The relevant descriptions and effects in the illustrated embodiments are for understanding purposes only and will not be repeated here.

[0080] This embodiment also provides a computer storage medium storing computer-executable instructions that can execute the 2D map optimization method based on Gaussian processes in any of the above method embodiments. The storage medium can be a magnetic disk, optical disk, read-only memory (ROM), random access memory (RAM), flash memory, hard disk drive (HDD), or solid-state drive (SSD), etc.; the storage medium may also include combinations of the above types of memory.

[0081] Finally, it should be noted that the above preferred embodiments are only used to illustrate the technical solutions of the present invention and are not intended 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 to it in form and detail without departing from the scope defined by the claims of the present invention.

Claims

1. A 2D map optimization method based on Gaussian processes, characterized in that, include: Obtain the raw point cloud data of the current frame; Based on the current frame's raw point cloud data, multiple map test locations to be optimized are determined, including: determining the original map extent based on the current frame's raw point cloud data; dividing the original map extent into multiple evenly spaced and non-intersecting regions; selecting regions where the number of raw point cloud data in the current frame exceeds a threshold as target regions; using principal component analysis to extract the normal vectors of the raw point cloud data within each target region; determining the sampling direction of the test locations for each target region based on the angle between the normal vectors and the two-dimensional coordinate axes; and sampling at the corresponding coordinates of each target region based on the sampling direction of the test locations for each target region to obtain the map test locations. Using the original point cloud data of the current frame as the training data of the Gaussian process, the predicted coordinates and corresponding variances of all test locations are calculated through the conditional distribution of the Gaussian process. Based on variance, target coordinates are filtered from the predicted coordinates corresponding to all test locations to obtain the prediction point set for the current frame. A target registration algorithm is employed to register the current frame's predicted point set with the historical world map and solve for the coordinate transformation matrix. This transformation matrix is ​​then used to align the current frame's predicted point set to the global coordinate system. The process includes: dividing the historical world map into multiple uniformly spaced and non-overlapping sub-regions; matching the target region corresponding to the test location in the current frame's predicted point set with the regions defined in the historical world map to obtain a matching region; generating matching point pairs based on the matching regions; minimizing the Euclidean distance error based on the coordinates of the matching point pairs to obtain the coordinate transformation matrix; transforming the original point cloud data of the current frame using the coordinate transformation matrix and generating a new current frame's predicted point set; using a target error algorithm to determine the error between the coordinates of the new current frame's predicted point set and the coordinates of the corresponding matching points on the historical world map; if the error does not meet a preset condition, the steps of matching the target region corresponding to the test location in the current frame's predicted point set with the regions defined in the historical world map are repeated until the target error meets the preset condition. The aligned current frame prediction point set is then merged with the historical world map to obtain an updated world map.

2. The 2D map optimization method based on Gaussian process according to claim 1, characterized in that, Using the original point cloud data of the current frame as the training data for the Gaussian process, the predicted coordinates and corresponding variances for all test locations are calculated using the conditional distribution of the Gaussian process, including: Use the original point cloud of the current frame as training data for the Gaussian process to construct training position vectors and training observation vectors; The exponential covariance kernel function is used to solve the training covariance matrix using the training position vector, and the observation covariance matrix is ​​obtained by superimposing the observation noise term. Calculate the cross-kernel matrix between testing and training, and the self-kernel matrix at the testing location, based on the testing location. Substituting the observation covariance matrix, cross kernel matrix, test kernel matrix, and training observation vector into the conditional prediction distribution calculation formula, we obtain the predicted coordinates and corresponding variances for each test location. The conditional prediction distribution formula is derived based on the conditional distribution of Gaussian processes.

3. The 2D map optimization method based on Gaussian process according to claim 1, characterized in that, The target error algorithm is used to determine the error between the coordinates of the new current frame predicted point set and the coordinates of the corresponding matching points on the historical world map, including: Based on the coordinates of the predicted point set in the current frame and the coordinates of the corresponding matching points in the historical world map, calculate the first covariance matrix corresponding to the predicted point set in the current frame and the second covariance matrix corresponding to the coordinates of the corresponding matching points in the historical world map. Eigenvalue decomposition is performed on the first covariance matrix and the second covariance matrix respectively, yielding two sets of eigenvalues ​​for the first covariance matrix and two sets of eigenvalues ​​for the second covariance matrix. The absolute difference between the eigenvalues ​​corresponding to the two sets of covariance matrices is obtained, and all the absolute differences are summed to obtain the error.

4. The 2D map optimization method based on Gaussian process according to claim 1, characterized in that, The aligned current frame prediction point set is then merged with the historical world map to obtain an updated world map, including: If any point exists only on the historical world map, it is retained; If any point exists only in the current frame's predicted point set, then add that point to the historical world map; If any point is an overlap point between the current frame's predicted point set and the historical world map, then the target fusion algorithm is used to perform map fusion.

5. The 2D map optimization method based on Gaussian process according to claim 4, characterized in that, Map fusion is performed using a target fusion algorithm, including: Extract overlapping point data from the historical world map and overlapping point data from the current frame prediction point set. The overlapping point data from the historical world map includes the historical world map coordinates and historical observation variance, while the overlapping point data from the current frame prediction point set includes the current coordinates and current observation variance. The overlapping point data in the historical world map and the overlapping point data in the current frame prediction point set are fused together to solve for the updated coordinates and the observation variance after fusion. The map fusion is completed by replacing overlapping points in the historical world map with the updated coordinates and observed variance after fusion.

6. The 2D map optimization method based on Gaussian process according to claim 5, characterized in that, The overlapping point data in the historical world map and the overlapping point data in the current frame prediction point set are fused together to solve for the updated coordinates and the observed variance after fusion, including: ; ; in, For the variance of the merged observations, For the updated coordinates after merging, For historical observation variance, The current observation variance, The current coordinates, Coordinates for a historical world map.

7. An electronic device, the device comprising: A memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that the processor performs the steps of the 2D map optimization method based on a Gaussian process as described in any one of claims 1-6.

8. A computer storage medium storing computer instructions thereon, characterized in that, When executed by the processor, this instruction implements the steps of the 2D map optimization method based on Gaussian process as described in any one of claims 1-6.

Images