A multi-modal based loop-back optimization method

Abstract

The application discloses a kind of based on multi-modal loop optimization method, based on GPS as prior constraint carries out loop frame screening, if there is no signal drift or signal disappears, current point cloud frame is used with historical point cloud frame to carry out point cloud registration, to obtain corresponding heading angle;Otherwise, mode switching is carried out, and Scan Context is used to supplement detection optimization loop in point cloud map, and then the angle initial value of the relative pose of the two frames required for subsequent calculation loop frame is provided.The application provides a kind of based on multi-modal loop optimization method, for the problem that existing loop detection algorithm robustness is not high, GPS information is introduced into Scan Context, the advantages of Scan Context and GPS are combined, loop detection is optimized, and the robustness thereof is improved.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving localization and mapping technology. More specifically, this invention relates to a multimodal loop closure optimization method. Background Technology

[0002] With the rapid development of artificial intelligence technology, research on autonomous driving technology is becoming increasingly popular. High-precision maps are one of the key technologies for realizing autonomous driving. A crucial step in building a high-precision map is collecting data and processing it using algorithms to remove semantics, resulting in an initial point cloud map. The accuracy of the initial point cloud map directly affects the accuracy of the final high-precision map generated by software processing.

[0003] Simultaneous Localization and Mapping (SLAM) is currently the mainstream method for building point cloud maps. The SLAM front-end estimates the current state of the autonomous vehicle by registering consecutive point cloud frames. However, over time, the relative pose constraints obtained solely from laser odometry and IMU pre-integration cannot eliminate accumulated errors. Therefore, a module is typically added to SLAM systems to increase system constraints and eliminate this accumulated error; this is usually a loop closure detection module.

[0004] Loop closure detection determines whether an autonomous vehicle has previously visited a given location. Successful loop closure detection and the addition of loop closure constraints provide the system with robust absolute observation constraints. Adding constraints to discontinuous frames through loop closure detection effectively optimizes the trajectory and corrects accumulated errors from odometry. However, in practical engineering, loop closure detection in laser SLAM typically relies on GPS. But GPS's inherent limitations (susceptibility to obstructions and signal instability) often affect loop closure detection. Furthermore, Scan Context technology has very low loop closure efficiency. Therefore, it is crucial to avoid the adverse effects of relying solely on GPS and Scan Context for loop closure in existing technologies and to improve the robustness of loop closure detection. Summary of the Invention

[0005] One object of the present invention is to solve at least the above-mentioned problems and / or defects, and to provide at least the advantages described below.

[0006] To achieve these objectives and other advantages of the present invention, a multimodal loop closure optimization method is provided, which uses GPS as a priori constraint to filter loop closure frames. If there is no signal drift or signal disappearance, the current point cloud frame is used to perform point cloud registration with the historical point cloud frame to obtain the corresponding heading angle.

[0007] Otherwise, a mode switch is performed, and Scan Context is used to supplement and optimize loop closures in the point cloud map, thereby providing initial angle values ​​for the relative pose of the two loop closure frames required for subsequent calculations.

[0008] Preferably, the process for determining whether signal drift or signal loss exists includes:

[0009] The GPS information is converted from latitude, longitude, and altitude data to the map coordinate system to obtain the GPS data for the current frame;

[0010] The difference between the GPS data of the current frame and the GPS data of the previous frame is calculated to obtain the difference between the two frames of point cloud data. This difference is compared with a pre-set threshold I. If the difference is greater than threshold I, it is considered that a serious drift has occurred and the GPS data of the current frame is unusable; otherwise, the GPS data is considered to be usable.

[0011] Preferably, when GPS data is available, the point cloud frame corresponding to the GPS data is searched within a predetermined radius. If pose data can be found, Scan Context is used to calculate the similarity score between the current frame and historical frames.

[0012] When GPS data is unavailable, Scan Context is used to traverse previous keyframes to find the optimal loopback point cloud frame, and the similarity score between the loopback point cloud frame and the current frame is calculated.

[0013] Preferably, if the calculated similarity score is greater than threshold II, the current frame is used as the corresponding heading angle, and the difference is used as the initial value for ICP matching.

[0014] If the error after ICP matching is less than the predetermined threshold Ⅲ, the loop closure is considered successful and the corresponding loop closure constraint is constructed and added to the graph optimization window, thereby optimizing the graph construction result through loop closure.

[0015] Otherwise, it is considered a failed loop.

[0016] Preferably, before traversing previous keyframes using Scan Context, the method further includes using the Scan Context algorithm to perform coarse localization of the point cloud, wherein the coarse localization is performed as follows:

[0017] Divide the point cloud space into N equal parts along the direction of increasing radius. r If there are several rings, then the width d of each ring is... r for:

[0018]

[0019] In the above formula, L max This indicates the maximum scanning distance of the lidar;

[0020] Each resulting ring is cut into N parts. s Divide the data into equal parts, and the resulting set of laser points can be represented as:

[0021]

[0022] In the above formula, P ij This represents the set of key points of the dividing units of the i-th annulus and j-th sector;

[0023] Generate Scan Context matrix I, where each value is the maximum value among the points in the point cloud formed by the corresponding ring ID and the corresponding sector ID;

[0024] The formula for calculating the similarity of the Scan Context matrix between two point clouds is:

[0025]

[0026] Among them, I q ,I c This is the Scan Context matrix for the current frame and historical frames. and is the j-th column vector in the corresponding matrix.

[0027] The present invention has at least the following beneficial effects: In view of the problem of low robustness of existing loop closure detection algorithms, the present invention introduces GPS information into the Scan Context to combine the advantages of Scan Context and GPS, thereby optimizing loop closure detection and improving its robustness.

[0028] Other advantages, objectives and features of the present invention will become apparent in part from the following description, and in part from those skilled in the art through study and practice of the invention. Attached Figure Description

[0029] Figure 1 This is a schematic diagram of the algorithm processing flow of the present invention;

[0030] Figure 2 This is a schematic diagram illustrating the point cloud spatial partitioning process of the present invention;

[0031] Figure 3 This is a schematic diagram of the actual trajectory of the vehicle during data collection in a predetermined environment.

[0032] Figure 4 for Figure 3 Trajectory graph without loop closure detection;

[0033] Figure 5 for Figure 3 Track map using pure GPS loop closure detection;

[0034] Figure 6 for Figure 3 Trajectory graph for loop closure detection using descriptors;

[0035] Figure 7 for Figure 3 The trajectory diagram of loop closure detection using the method of the present invention. Detailed Implementation

[0036] The present invention will now be described in further detail with reference to the accompanying drawings, so that those skilled in the art can implement it based on the description.

[0037] The overall idea of ​​this invention is to use GPS as a priori constraint to replace Euclidean distance for loop closure frame screening, and then use the current point cloud frame to perform point cloud registration with these point cloud frames. This invention combines and improves the advantages of the two loop closure detection schemes by adding reliable GPS information to constrain the loop closure search range and find the nearest key frame. GPS is an absolute positioning sensor. Although there will be measurement errors, because the measurements of the previous and next frames are independent, the state of the current frame will not be affected by the error of the previous frame, so there will be no cumulative error. However, when constructing an occluded environment, GPS signal drift or signal loss may occur. In order to prevent the loop closure detection system from failing, the Scan Context algorithm is added for supplementary detection to prevent the loop from completely failing when the GPS signal disappears. At the same time, it can provide initial angle values ​​for subsequent calculation of the relative pose of two loop closure frames.

[0038] Therefore, the technical solution of this invention is a GPS-based loop closure optimization method, which aims to improve the robustness of loop closure detection and solve the problem of loop closure drift in point cloud maps. The specific implementation process includes the following steps:

[0039] S1. Convert GPS information from latitude, longitude, and altitude data to map coordinate system.

[0040] (1) WGS-84 coordinate system - ECEF coordinate system

[0041] Assume the coordinates of the carrier in the WGS84 coordinate system and the ECEF coordinate system are respectively Given (X,Y,Z), the transformation from the WGS84 coordinate system to ECEF is as follows:

[0042]

[0043] Where λ is longitude, R is latitude, h is altitude, and R is latitude. N Let R be the radius of the Earth's geoid, and e be the eccentricity. N The solution for e is as follows:

[0044]

[0045]

[0046] Where a represents the Earth's major axis and b represents the Earth's minor axis.

[0047] The transformation from the ECEF coordinate system to the WGS84 coordinate system is mainly achieved through the following formula:

[0048]

[0049]

[0050]

[0051] The above equation is a high-order equation and cannot be solved directly. However, iterative numerical solutions can be obtained using the advantages of computers.

[0052] (2) WGS-84 coordinate system - ENU coordinate system

[0053] Assume the initial coordinates of ECEF are (X0, Y0, Z0), and the coordinates of the ENU coordinate system are (X... enu ,Y enu Z enu The transformation from WGS84 coordinate system to ENU coordinate system is as follows:

[0054]

[0055] in, Let be the transformation matrix from the ECEF coordinate system to the navigation coordinate system. Since it is an orthogonal matrix, according to the properties of orthogonal matrices, the transformation matrix from the ENU coordinate system to the WGS84 coordinate system can be written as:

[0056]

[0057] (3) ENU coordinate system - carrier coordinate system

[0058] The ENU coordinate system and the carrier coordinate system can be transformed based on their own attitude angles. When the attitude angles are (φ, θ, ψ), the transformation matrix from the ENU coordinate system to the carrier coordinate system can be written as:

[0059]

[0060] in The transformation matrix from the vehicle coordinate system to the ENU navigation coordinate system, satisfying the orthogonal matrix condition, is as follows:

[0061]

[0062] S2, Transfer the GPS data of the current frame and the previous frame of GPS data The difference is calculated by subtracting the two frames of point cloud data.

[0063] S3. Judge the difference obtained. If the difference is greater than a certain threshold, it is considered that a serious drift has occurred and the GPS data is unusable. Proceed to S6. If the GPS data is available, construct a kd tree with all frame IDs and the corresponding GPS data converted to map coordinate system. Search for all historical frame IDs within a certain range. If the ID difference is greater than 50, it is considered that the frame can be added to the descriptor matching pair.

[0064] S4. Match the current frame with the historical frame ID to find the optimal score and the heading angle at the optimal match. Use the difference between the heading angle and the GPS coordinates as the initial value to construct the ICP matching model and perform optimization calculations.

[0065] Define two sets of point clouds as follows:

[0066]

[0067] Where n is the number of matching point pairs in the two sets of point clouds, the pose transformation relationship between the two sets of point clouds can be represented by the following equation for the iterative nearest point method:

[0068]

[0069] In the above formula, R is an orthogonal matrix with a determinant of 1, called the rotation matrix. It is a displacement vector;

[0070]

[0071] Define the centroids of the two point clouds as follows:

[0072]

[0073] Substituting least squares

[0074]

[0075] Define the centroid-free coordinates of each point in the two sets of point clouds as follows: and

[0076]

[0077] In the above formula

[0078] It can be rewritten as:

[0079] In the formula R * For the rotation matrix to be solved, further expansion yields:

[0080]

[0081] In the above formula It is unrelated to R. Similarly, it is unrelated to R. It can be written as:

[0082] For an orthogonal matrix, when W is full rank, the optimal rotation matrix can be solved: R = UVT;

[0083] After calculating the rotation, substitute it into the following formula:

[0084]

[0085] Later orders The displacement transformation between two frames of point clouds can then be calculated.

[0086] S5. After solving for the rotation and translation, perform a rigid body transformation on the point cloud, and then build an ICP model for the transformed point cloud for another iteration, followed by nonlinear optimization to continue solving.

[0087] Optimization methods are common means of estimation. Optimization leverages the speed of computer computation to iteratively optimize variables to maximize or minimize the residual function. Common optimization algorithms include Newton's method, Gauss-Newton's method, and steepest descent. To minimize nonlinear errors in nonlinear optimization, second-order linear expansions are often used to reduce the nonlinearity caused by Taylor expansions. Steepest descent primarily uses first-order derivatives for iterative optimization. Therefore, for SLAM problems, Newton's method and Gauss-Newton's method are the best choices. Newton's method performs a second-order linear expansion and directly solves for the second derivative of the matrix. Gauss-Newton's method uses Jacobian matrix multiplication to fit the Hessian matrix, which can result in a non-positive definite Hessian matrix, making optimization impossible. The Levenberg-Marquardt (LM) algorithm combines the advantages of both steepest descent and Gauss-Newton methods and is therefore often used to solve nonlinear optimization problems in SLAM.

[0088] The main purpose of nonlinear optimization is to adjust variables to minimize the nonlinear residual function. Assume f i (x) is a nonlinear function. The optimization algorithm mainly adjusts the variable x = (x1, x2, ..., x...). n ) TTo minimize the total residual function F(x), the problem can be described as:

[0089]

[0090] In the process of optimizing a nonlinear function, it is first converted into a linear function for optimization, where x = x k f i Taylor expansion of (x) yields:

[0091]

[0092] The function after ignoring higher-order terms is represented by φ. i (x) represents, that is:

[0093]

[0094] Then φ i (x) is f i (x) at x = x k A linear function at a given point, approximately f i (x), then let ψ(x) approximates F(x), therefore, by f i The first-order Taylor expansion of (x) can be obtained as follows:

[0095]

[0096] Then we have:

[0097]

[0098] make

[0099] Therefore:

[0100]

[0101] Differentiating the expression and setting the derivative to 0, we get:

[0102] J k T J k x = J k T (J k x k -f k ).

[0103] Assume the Jacobian matrix J k If the columns are of full rank, then the Hessian matrix H = J. k T J k If it is reversible, then:

[0104] xxk =-(J k T J k ) -1 J k T f k .

[0105] The above is the optimization idea of ​​the Gauss-Newton method. Iterative optimization of the above equation yields the optimal variable, with the variable being -(J) in each iteration. k T J k ) -1 J k T f k However, when the Jacobian matrix is ​​not of full rank, the above equation cannot yield an exact solution, and the optimization algorithm fails to converge. The LM algorithm adds a small positive definite diagonal column matrix λ to the Jacobian matrix to limit the range of the descent gradient. To make the Hessian matrix positive definite, the above equation is transformed as follows:

[0106] xx k =-(J k T J k +λI) -1 J k T f k .

[0107] In the above formula, λ is a positive real number, and I is the identity matrix.

[0108] As can be seen from the above formula, when λ is very small, the LM algorithm can be approximated as the Gauss-Newton method, and when λ is very large, the LM algorithm can be approximated as the minimum descent method. In SLAM algorithms, to ensure accuracy while avoiding the occurrence of a non-positive definite Hess matrix, λ is usually set to 0.01.

[0109] S6. If GPS data is unavailable, use the Scan Context algorithm to perform coarse localization of the point cloud.

[0110] First, such as Figure 2 As shown, the point cloud space is divided into N equal parts along the direction of increasing radius. r If there are 3 rings, then the width of each ring is:

[0111]

[0112] In the above formula, L max This indicates the maximum scanning distance of the lidar.

[0113] Then, each of the resulting rings is cut into N pieces. sAfter being divided into equal parts, the set of laser points can be represented as:

[0114]

[0115] In the above formula, P ij This represents the set of key points of the dividing unit of the i-th annulus and j-th sector.

[0116] Generate Scan Context matrix I, where each value is the maximum value among the point cloud points formed by the corresponding ring ID and the corresponding sector ID.

[0117] The formula for calculating the similarity of the Scan Context matrix between two point clouds is:

[0118]

[0119] Among them, I q ,I c This is the Scan Context matrix for the current frame and historical frames. and is the j-th column vector in the corresponding matrix.

[0120] S7. Traverse the previous keyframes, find the best frame and perform ICP point cloud registration. If the point cloud registration score is less than the threshold, the loop closure is considered successful.

[0121] S8. Add the loop closure matching results as constraints to the graph optimization window to perform loop closure optimization on the graph construction results.

[0122] In this invention, mode one uses GPS for coarse matching to narrow down the fine matching range, and mode two uses descriptors for coarse matching to provide an initial pose for fine matching. The fine matching in mode one uses the ICP registration method, specifically, GPS is used for subtraction in ICP registration, and the subtraction result is used to determine whether mode switching is required.

[0123] The following uses, as Figure 3 The indoor and outdoor environmental diagrams serve as environmental documentation to demonstrate the superiority of the invention:

[0124] Comparative Example 1

[0125] right Figure 3 Data was collected for SLAM experiments. Mapping was performed without loop closure detection, and trajectory data was analyzed to obtain results such as... Figure 4 The trajectory graph without loop closure detection constraints.

[0126] Comparative Example 2

[0127] Using only GPS loop closure constraints, the collected data is analyzed. A GPS signal is added during the loop closure, but it is partially obstructed to make the signal unstable. Mapping experiments are conducted to simulate GPS obstruction in a real-world environment. Analysis of the trajectory data reveals the following results when only GPS loop closure constraints are used and the signal is unstable: Figure 5 The trajectory diagram.

[0128] Comparative Example 3

[0129] The Scan Context algorithm was used as the loop closure constraint to analyze the collected data. The Scan Context algorithm was used as the constraint when loops closed to conduct mapping experiments and analyze the trajectory data to obtain the trajectory map for loop closure detection using descriptors, as shown in Figure 6.

[0130] Example 1

[0131] The optimization method of this invention is used to analyze the collected data, and the algorithm of this invention is used to constrain loop closures. Mapping experiments are conducted, and trajectory data is analyzed to obtain the following results: Figure 7 The loop closure detection trajectory diagram.

[0132] By comparing the obtained trajectory data, the optimization of loop closure in this invention is derived.

[0133] By comparing the trajectory data obtained from each pair of examples with those from the embodiments of the present invention, it can be seen from the trajectory diagram that the loop closure optimization of the present invention is superior to the current technology.

[0134] Furthermore, the optimization method of this invention is to add the Scan Context algorithm to the existing GPS loop closure algorithm for supplementary detection. Compared with the existing GPS loop closure algorithm, it uses reliable GPS information to constrain the loop closure search range and find the nearest key frame. This can effectively solve the problem of loop closure drift in point cloud maps. That is, the Scan Context algorithm is used in the case of drift to ensure the stability of the loop closure. At the same time, it can solve the problem of low processing efficiency when the existing Scan Context algorithm is applied.

[0135] The above solution is merely an illustration of a preferred example and is not limited thereto. When implementing this invention, appropriate substitutions and / or modifications can be made according to the user's needs.

[0136] The number of devices and processing scale described herein are for the purpose of simplifying the description of the invention. Applications, modifications, and variations of the invention will be readily apparent to those skilled in the art.

[0137] Although embodiments of the present invention have been disclosed above, they are not limited to the applications listed in the specification and embodiments. It can be applied to various fields suitable for the present invention. Other modifications can be readily made by those skilled in the art. Therefore, without departing from the general concept defined by the claims and their equivalents, the present invention is not limited to the specific details and examples shown and described herein.

Claims

1. A loop closure optimization method based on multimodal dynamics, characterized in that, Based on GPS as a priori constraint, loopback frames are filtered. If there is no signal drift or signal loss, the current point cloud frame is used to perform point cloud registration with the historical point cloud frame to obtain the corresponding heading angle. Otherwise, a mode switch is performed, and Scan Context is used to supplement and optimize loop closures in the point cloud map, thereby providing initial angle values ​​for the relative pose of the two loop closure frames required for subsequent calculations. The process for determining whether signal drift or signal loss exists includes: The GPS information is converted from latitude, longitude, and altitude data to the map coordinate system to obtain the GPS data for the current frame; The difference between the GPS data of the current frame and the GPS data of the previous frame is obtained to obtain the difference between the two frames of point cloud data. This difference is compared with a pre-set threshold I. If the difference is greater than threshold I, it is considered that a serious drift has occurred and the GPS data of the current frame is unusable; otherwise, the GPS data is considered to be usable. When GPS data is available, the point cloud frame corresponding to the GPS data is searched within a predetermined radius. If pose data can be found, Scan Context is used to calculate the similarity score between the current frame and historical frames. When GPS data is unavailable, Scan Context is used to traverse previous keyframes to find the optimal loopback point cloud frame, and the similarity score between the loopback point cloud frame and the current frame is calculated. If the calculated similarity score is greater than threshold II, then the current frame is used as the corresponding heading angle, and the difference is used as the initial value for ICP matching. If the error after ICP matching is less than the predetermined threshold Ⅲ, the loop closure is considered successful and the corresponding loop closure constraint is constructed and added to the graph optimization window, thereby optimizing the graph construction result through loop closure. Otherwise, it is considered a failed loop.

2. The loop closure optimization method based on multimodal operation as described in claim 1, characterized in that, Before using Scan Context to traverse previous keyframes, a coarse localization of the point cloud is performed using the Scan Context algorithm. The coarse localization method is as follows: Divide the point cloud space into equal parts along the direction of increasing radius. N r If there are several rings, then the width of each ring is... d r for: In the above formula, L max This indicates the maximum scanning distance of the lidar; Each of the resulting rings is cut into... N s Divide the data into equal parts, and the resulting set of laser points can be represented as: In the above formula, P ij Indicates the first i The ring of the first ring j A collection of key points in a sector-shaped segmentation unit; Generate Scan Context matrix I Each value in the matrix is ​​the maximum value among the points in the point cloud formed by the corresponding annular ID and the corresponding sector ID; The formula for calculating the similarity of the Scan Context matrix between two point clouds is: in, , This is the Scan Context matrix for the current frame and historical frames. and For the first in the corresponding matrix j Column vector.

Images