A Hierarchical Optimization Method and System for the Back-end Module Suitable for Laser SLAM

Abstract

A hierarchical optimization method for the backend module applicable to laser SLAM, comprising the following steps: constructing the first layer of the pyramid, with the input of multiple consecutive frames of point clouds and the poses corresponding to each frame of point cloud as the input of the first layer of the pyramid; using the BA module to perform ICP error constraint and gravitational acceleration constraint on the data of each window in each layer of the pyramid respectively; performing layer-by-layer optimization on the pyramid through the BA module, where the optimized output result in the pyramid structure is used as the input of the next layer until the BA module optimizes to the top layer of the pyramid; constructing a global PG0 residual model, using the global PG0 residual model to constrain the relative pose transformation of adjacent key frames in the same layer within the pyramid. Through the hierarchical design, the optimization problem is decomposed into multiple sub-problems that can be calculated in parallel at multiple layers. Each sub-problem involves fewer pose variables, thereby reducing the computational complexity and resource consumption, improving the convergence speed of solving the optimization problem, and being able to reduce the possibility of the constructed map diverging.

Description

Technical Field

[0001] The present invention relates to the technical field of laser SLAM, and particularly to a hierarchical optimization method and system for a backend module applicable to laser SLAM. Background Art

[0002] A SLAM (Simultaneous Localization and Mapping) system is a technology that uses sensors (vision cameras / lidar) to achieve autonomous positioning and three-dimensional reconstruction of an unknown environment. According to the different sensors, it can be divided into visual SLAM and laser SLAM. A complete laser SLAM system usually consists of a sensor data preprocessing module, a front-end module, and a back-end module. The system finally outputs pose estimation and a point cloud map. Among them, the back-end module is responsible for processing data from the front end (such as pose estimation, lidar point cloud), re-optimizing the pose estimation within a certain time range, or jointly optimizing it with the point cloud map. There are various technical routes for the back-end module of the current laser SLAM system. For example, the method based on graph optimization is a widely used one currently.

[0003] In the method based on graph optimization, by considering multiple constraints simultaneously, all poses within a certain time window are individually optimized or jointly optimized with the point cloud map, so as to reduce the cumulative error and achieve the estimation of the robot's motion pose and the construction of the three-dimensional map of the environment. Although the method based on graph optimization has good accuracy and robustness, it also has the following two significant disadvantages:

[0004] a. High computational complexity and long time consumption: The graph optimization method processes all data within all time ranges simultaneously. The huge amount of data makes the dimension of the optimization problem very high, and the problem-solving speed is relatively slow. The computing performance of systems such as drones and robots, which are the main deployment objects of SLAM, is generally low. Computational complexity and resource consumption are a major bottleneck. Especially when operating in a large-scale scenario, it is impossible to output the optimized pose estimation and point cloud map in time, which affects the normal operation of the SLAM system and is difficult to meet the requirements.

[0005] b. The optimization model often ignores the constraint on gravitational acceleration. The current mainstream SLAM algorithms either assume that the carrier is already aligned with the world system during the system initialization process, that is, by default, the gravitational acceleration in the carrier system is also (0, 0, -9.81), or directly ignore the constraint on gravitational acceleration, resulting in the carrier system not being correctly aligned to the world system. At the same time, the incorrect estimation of gravity will also lead to inaccurate initialization estimation of the zero bias, thereby affecting the mapping quality. Especially for scenarios with more movement in the Z-axis direction and larger scale, the constructed map has problems such as local collapse and deformation. Summary of the Invention

[0006] In view of the above defects, the object of the present invention is to propose a hierarchical optimization method and system for the back-end module applicable to laser SLAM, so as to solve the problems of slow convergence speed of the optimization problem and low mapping quality.

[0007] To achieve this purpose, the present invention adopts the following technical solutions: A hierarchical optimization method for the back-end module applicable to laser SLAM, comprising the following steps:

[0008] Step S1: Construct the first layer of the pyramid, using the input multi-frame continuous point clouds and the poses corresponding to each frame of point cloud as the input of the first layer of the pyramid;

[0009] Step S2: Construct a BA module, where the BA module is composed of an ICP error model and a gravitational acceleration error model, and the BA module is respectively used to perform ICP error constraint and gravitational acceleration constraint on the data of each window of each layer of the pyramid;

[0010] Step S3: Optimize the pyramid layer by layer through the BA module, where the optimized output result in the pyramid structure is used as the input of the next layer until the BA module optimizes to the top layer of the pyramid;

[0011] Step S4: Construct a global PG0 residual model, and use the global PG0 residual model to constrain the relative pose transformation of adjacent key frames in the same layer of the pyramid.

[0012] Preferably, the first layer of the pyramid is set as follows:

[0013] Divide the continuous key frames of the first layer of the pyramid into multiple local windows according to the local window size w and the local window step size s;

[0014] Set the first frame of the first layer of the pyramid not to fall into the first local window division;

[0015] Input point cloud data, establish a world coordinate system with the pose of the first frame of point cloud as a reference, convert all point cloud data to the world coordinate system, downsample the point cloud according to the set voxel resolution to obtain the first data, calculate the hash key according to the three-dimensional coordinates of the point cloud and store it in the hash table;

[0016] Traverse the hash table, judge whether the point cloud stored in each hash value meets the plane feature requirements, if it meets, retain the first data, if it does not meet, delete the first data.

[0017] Preferably, the steps for judging whether the first data meets the requirements of the plane feature are as follows:

[0018] Convert the point cloud corresponding to the first data into point clusters;

[0019] Obtain the covariance matrix of the point clusters;

[0020] Perform eigenvalue decomposition on the covariance matrix of the point cluster to obtain a number of eigenvectors and the corresponding eigenvalues; obtain the ratio of the minimum eigenvalue to the second minimum eigenvalue as the first ratio.

[0021] Determine whether the first ratio is less than the ratio threshold. If it is less, it meets the requirements of the plane feature; if it is greater, it does not meet the requirements of the plane feature.

[0022] Preferably, the ICP error model is specifically as follows:

[0023] ;

[0024] where is the number of poses of the key frame, is the number of plane geometric features, is the number of points in the point cloud set, is the Euclidean distance between the z-th observation point of the x-th plane feature in the y-th key frame and the x-th plane feature;

[0025] The gravity acceleration error model is specifically as follows:

[0026] ;

[0027] where is the gravity acceleration constraint of the y-th key frame;

[0028] where the BA module is the model obtained by weighting the ICP error model and the gravity acceleration error model, and the BA module is specifically as follows:

[0029] ;

[0030] where is the covariance of pose estimation, is the covariance of gravity estimation.

[0031] Preferably, the global PG0 residual model is specifically as follows:

[0032] ;

[0033] where is the relative pose constraint between the j-th key frame and the j + 1-th key frame, is the pose of the j-th key frame and the pose of the j + 1-th key frame the correlation between them, is the relative pose constraint between the s·j-th key frame and the s·j + 1-th key frame, is the pose of the s·j-th key frame and the pose of the s·(j + 1)-th key frame The relevance between them is the ·j-th key frame and the ·(j + 1)-th key frame relative pose constraint is the ·j-th key frame pose and the ·(j + 1)-th key frame pose The relevance between them. All relevances are optimized by the BA module in the previous stage.

[0034] A hierarchical optimization system for the backend module of laser SLAM, using the above-mentioned hierarchical optimization method for the backend module of laser SLAM, includes: a first construction module, a BA optimization module, a pyramid optimization module, and a global PGO module;

[0035] Wherein the first construction module is used for: constructing the first layer of the pyramid, using the input multi-frame continuous point clouds and the pose corresponding to each frame of point cloud as the input of the first layer of the pyramid;

[0036] The BA optimization module is used to construct a BA module composed of an ICP error model and a gravitational acceleration error model;

[0037] The pyramid optimization module is used to optimize the pyramid layer by layer through the BA module, where the optimized output result in the pyramid structure is used as the input of the next layer until the BA module optimizes to the top layer of the pyramid;

[0038] The global PGO module is used to construct a global PG0 residual model, and constrain the relative pose transformation of adjacent key frames in the same layer of the pyramid through the global PG0 residual model.

[0039] Preferably, the first construction module includes a division unit, a rejection unit, a transfer unit, and a calculation unit;

[0040] The division unit is used to divide the continuous key frames of the first layer of the pyramid into multiple local windows according to the local window size w and the local window step;

[0041] The rejection unit is used to set that the first frame of the first layer of the pyramid does not fall into the first local window division;

[0042] The transfer unit is used to input point cloud data, establish a world coordinate system with the pose of the first frame of point cloud as a reference, convert all the point cloud data into the world coordinate system, downsample the point cloud according to the set voxel resolution to obtain the first data, calculate the hash key based on the three-dimensional coordinates of the point cloud and store it in the hash table.

[0043] The calculation unit is used to traverse the hash table, determine whether the point cloud stored in each hash value meets the requirements of the plane feature. If it meets the requirements, the first data is retained; if it does not meet the requirements, the first data is deleted.

[0044] One of the technical solutions in the above technical solutions has the following advantages or beneficial effects: 1. Through the hierarchical design, the optimization problem is decomposed into multiple sub-problems that can be calculated in parallel in multiple layers. Each sub-problem involves fewer pose variables, thereby reducing the computational complexity and resource consumption, improving the convergence speed of solving the optimization problem, and reducing the possibility of divergence in map construction.

[0045] 2. The BA optimization model designed in the present invention adds a gravitational acceleration constraint, enabling the vehicle coordinate system to be correctly aligned to the world coordinate system, especially in large-scale scenarios and scenarios with large movements in the Z-axis direction. Compared with the existing methods, it effectively reduces the cumulative error and distortion problems of the map, improves the global consistency and accuracy of the point cloud map. At the same time, it can also improve the initial estimation accuracy of the zero bias of the IMU accelerometer, improve the accuracy of the IMU observation data, and provide a higher-precision optimization result for the SLAM system. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] Figure 1 is a flowchart of an embodiment of the method of the present invention.

[0047] Figure 2 is a schematic structural diagram of an embodiment of the system of the present invention.

[0048] Figure 3 is a schematic diagram of a unit sphere manifold in an embodiment of the present invention.

[0049] Figure 4 is a schematic diagram of the pyramid structure being compressed into a single-layer structure in an embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0050] The following details the embodiments of the present invention. The examples of the embodiments are shown in the drawings, where the same or similar reference numerals represent the same or similar elements or elements with the same or similar functions throughout. The embodiments described below with reference to the drawings are exemplary only for explaining the present invention and should not be construed as limiting the present invention.

[0051] In the description of the embodiments of the present invention, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the quantity of the indicated technical features. Thus, the features defined with "first" and "second" may explicitly or implicitly include one or more of the said features. In the description of the embodiments of the present invention, "a plurality of" means two or more unless otherwise specifically defined.

[0052] In addition, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the quantity of the indicated technical features. Thus, the features defined with "first" and "second" may explicitly or implicitly include one or more of such features. In the description of the present invention, unless otherwise stated, "a plurality of" means two or more. For those of ordinary skill in the art, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0053] As Figures 1 to 4 shown, a hierarchical optimization method for the back-end module applicable to laser SLAM includes the following steps:

[0054] Step S1: Construct the first layer of the pyramid, using the input multi-frame continuous point clouds and the poses corresponding to each frame of point clouds as the input of the first layer of the pyramid;

[0055] Step S2: Construct a BA module, where the BA module is composed of an ICP error model and a gravitational acceleration error model, and the BA module is respectively used to perform ICP error constraint and gravitational acceleration constraint on the data of each window of each layer of the pyramid;

[0056] Step S3: Optimize the pyramid layer by layer through the BA module, where in the pyramid structure, the optimized output result is used as the input of the next layer until the BA module optimizes to the top layer of the pyramid;

[0057] The specific method of the said Step S3 is to aggregate the point cloud data of all key frames within a local window into one frame as one of the nodes of the next layer, as shown in the formula:

[0058] ;

[0059] The meaning of the above formula is to transform the coordinates of all key frames within a window to the coordinate system of the first key frame of the window, and then directly accumulate to obtain the . is the j-th key frame in the (i + 1)-th layer; is the relative pose transformation matrix, which describes the pose transformation from the s·j-th key frame to the (s·j) + k-th key frame in the i-th layer; is the (s·j)+k-th key frame in the i-th layer; means that is transformed to the coordinate system of, i.e., with the first key frame within the current window as the origin.

[0060] Perform the above operations on all local windows of the first layer of the pyramid to construct the second layer of the pyramid. Continue to divide the key frames of this layer into multiple local windows according to parameters w and s, and then perform local BA optimization concurrently. The obtained results are used to construct the key frames of the third layer (the top layer), and then BA is performed on all the key frames of the entire top layer. Note that the BA at this time is already global BA because the key frames of the top layer are obtained by aggregating the point clouds of all the input key frames layer by layer. For the global BA performed on the top layer, its specific working principle is no different from that of local BA, except for the different data ranges of the input.

[0061] Step S4: Construct a global PG0 residual model to constrain the relative pose transformation of adjacent key frames in the same layer within the pyramid. After constructing the residual model of the global PGO, call graph optimization libraries such as GTSAM / G2O to complete the solution.

[0062] The backend module designed in the present invention based on the pyramid structure can improve the accuracy and real-time performance of pose estimation and map construction, especially for large-scale scenes, and provides a reliable low-computation-cost solution for the autonomous navigation and map construction of the vehicle in an unfamiliar environment. Compared with the existing methods, the hierarchical design decomposes the optimization problem into multiple sub-problems that can be computed in parallel at multiple layers. Each sub-problem involves fewer pose variables, thereby reducing the computational complexity and resource consumption, increasing the convergence speed of solving the optimization problem, and being able to reduce the possibility of divergence in map construction.

[0063] In addition, since the current mainstream solution optimizes the input data within a single window and only considers the constraints of pose estimation and point cloud map, and only the ICP error is added to the optimization model, there are problems such as slow working speed and easy distortion of the constructed point cloud map in the backend module, especially for the operation tasks in large-scale scenes. There is an urgent need to improve the accuracy, timeliness, and global consistency of the system's pose estimation and map construction.

[0064] Therefore, a gravity acceleration constraint is added to the BA optimization model designed in the present invention, enabling the vehicle coordinate system to be correctly aligned to the world coordinate system, especially in large-scale scenes and scenes with large movements in the Z-axis direction. Compared with the existing methods, it effectively reduces the cumulative error and distortion problems of the map, improves the global consistency and accuracy of the point cloud map. At the same time, it can also improve the initial estimation accuracy of the zero bias of the IMU accelerometer, improve the accuracy of the IMU observation data, and provide a higher-precision optimization result for the SLAM system.

[0065] Preferably, the first layer of the pyramid is set as follows:

[0066] Divide the consecutive key frames of the first layer of the pyramid into multiple local windows according to the local window size w and the local window step size s;

[0067] Set the first frame of the first layer of the pyramid not to fall into the first local window division;

[0068] Input the point cloud data, establish the world coordinate system with the pose of the first frame of the point cloud as a reference, convert all the point cloud data to the world coordinate system, downsample the point cloud according to the set voxel resolution to obtain the first data, calculate the hash key according to the three-dimensional coordinates of the point cloud and store it in the hash table;

[0069] Traverse the hash table, judge whether the point cloud stored in each hash value meets the plane feature requirements. If it meets, retain the first data. If it does not meet, delete the first data.

[0070] In fact, the output result of the existing SLAM is the relative pose, which will cause the problem of degrees of freedom when there is no absolute reference system. To avoid this problem, in the present invention, the first frame of the key frames input to the first layer of the pyramid is used as a reference. Then all the point clouds and pose coordinates of other key frames are transferred to the coordinate system with the pose of the first frame as the origin. At this time, since a unified relative coordinate system is established throughout the SLAM process, it eliminates the cumulative error of the translational degrees of freedom generated due to inaccurate initial pose estimation between different frames, thus solving the problem of degrees of freedom.

[0071] Preferably, the steps to judge whether the first data meets the plane feature requirements are as follows:

[0072] Convert the point cloud corresponding to the first data into point clusters;

[0073] Specifically, the conversion into point clusters can be carried out through the following formula:

[0074] , where P and v are intermediate variables, , , represents the Kth point cloud, and N is the number of point clouds;

[0075] Obtain the covariance matrix of the point clusters;

[0076] Specifically, the formula for calculating the covariance matrix of this point cluster is as follows:

[0077] ;

[0078] Perform eigenvalue decomposition on the covariance matrix of the point cluster to obtain a number of eigenvectors and the corresponding eigenvalues; perform eigenvalue decomposition on the covariance matrix to obtain a number of eigenvectors and the corresponding eigenvalues, which is achieved by calling the Eigen library function in C++ language.

[0079] Obtain the ratio of the minimum eigenvalue to the second minimum eigenvalue as the first ratio;

[0080] Determine whether the first ratio is less than the ratio threshold. If it is less, it meets the requirements of the plane feature; if it is greater, it does not meet the requirements of the plane feature.

[0081] The eigenvalue describes the degree of dispersion of the point cluster in the direction of the corresponding eigenvector. Usually, the magnitude of the eigenvalue of the covariance matrix is related to the distribution width of the point cluster in the corresponding direction. The direction corresponding to the minimum eigenvalue can be understood as the normal vector of the plane. If the ratio of the minimum eigenvalue to the second minimum eigenvalue is very small, it means that the distribution of the point cluster in the direction of the eigenvector corresponding to the minimum eigenvalue is relatively concentrated, while the distribution in other directions is relatively divergent. Therefore, when the first ratio is less than the ratio threshold, it can be considered that the point cluster is close to a plane.

[0082] Preferably, let the set of observation points of the x-th plane feature in the y-th key frame of this window be , and the number of points in this point set is . Let the z-th point of the point set be . By left-multiplying, the point in the local coordinate system is transformed to the world coordinate system to obtain ,

[0083] , represents the relative pose of the j-th key frame in the i-th layer;

[0084] The Euclidean distance from this point to the x-th plane feature can be obtained by the following formula:

[0085] , where is the normal vector of this plane, is an arbitrary point on the plane;

[0086] The sum of the Euclidean distances between all observation points matching the x-th plane feature in the y-th key frame can be obtained as follows:

[0087] ;

[0088] The sum of the Euclidean distances between all observation points matching the x-th plane feature for the poses of all key frames in this local window can be obtained:

[0089] ;

[0090] The Euclidean distance between all the observed points of all planar feature matches for all key - frame poses accumulated within this local window can be obtained as follows:

[0091] ;

[0092] Theoretically, after pose estimation and transformation, the obtained , will coincide with the corresponding planar feature in space, that is, the Euclidean distance is zero. However, in reality, due to various errors and noise effects, the relative pose and the planar feature are not necessarily accurate. Therefore, the Euclidean distance is always non - zero. The direct purpose of ICP (Iterative Closest Point) is to find a suitable pose estimation and planar feature to minimize the Euclidean distance. To sum up, the ICP error model is specifically as follows:

[0093] ;

[0094] where is the number of poses of key frames, is the number of planar geometric features, is the number of points in the point cloud set, is the Euclidean distance between the z - th observed point of the x - th planar feature in the y - th key frame and the x - th planar feature;

[0095] However, the ICP error model only considers the information of the estimated pose and the observed planar feature, and is prone to cumulative drift in the case of large - scale translational motion of the carrier, especially in the Z - axis direction. Therefore, the present invention considers the constraint based on gravitational acceleration. The gravitational acceleration constraint provides an absolute vertical reference, significantly improving the estimation accuracy in the Z - axis direction.

[0096] In the front - end odometer, when adding a new key frame (corresponding to a key frame), an estimation of gravitational acceleration and covariance matrix in the world coordinate system will be obtained. The estimation of gravitational acceleration and its covariance can be transformed from the world coordinate system to the vehicle coordinate system using the result of point cloud registration, as shown in the formula:

[0097] ;

[0098] On the other hand, obviously, the direction of gravitational acceleration is vertically downward in the world coordinate system. Therefore, we define the standard gravitational acceleration vector , multiply the pose homogeneous matrix on the left to obtain the standard gravitational acceleration vector in the vehicle coordinate system :

[0099] ;

[0100] When there is no error in the estimation of gravitational acceleration by the front-end odometer, the following equation should hold:

[0101] ;

[0102] However, due to the existence of IMU internal parameters and measurement noise, there will always be errors in the estimation of gravitational acceleration by the front-end odometer. Therefore, we need to establish a model to measure the errors.

[0103] Considering that the accuracy of the standard gravitational acceleration (9.80665 m / ) is already quite high and can meet the mission requirements at most locations on the earth, the magnitude of the gravitational acceleration vector is set as a constant, always equal to 9.80665. Based on the fact that the magnitude of the gravitational acceleration is always a constant, we can use a two-dimensional vector to represent the three-dimensional gravitational acceleration vector, and then express the gravitational acceleration vector on a unit sphere manifold . Let the state variable of the manifold be D, and let ε be the identity element of the manifold. As Figure 3 shown, it points vertically from the center of the sphere to the sphere surface.

[0104] Since the state variable on this manifold is a two-dimensional vector, but the magnitude of the gravitational acceleration has been restricted to be a constant, the gravitational acceleration vector can be represented by a two-dimensional vector (three-dimensional). Similarly, the standard gravitational acceleration vector is also represented by (three-dimensional). Let D be any point on the manifold. The relationship between the manifold and the tangent space is given by the logarithmic mapping:

[0105] , where is a set of orthonormal bases in the tangent space of ε;

[0106] Let be any point in the tangent space of the identity element ε. The relationship between the tangent space and the manifold is given by the exponential mapping:

[0107] , where is the two-dimensional vector where

[0108] Based on the logarithmic mapping and the exponential mapping, we can construct a model for measuring the difference between two gravitational accelerations on the manifold. Let , are two gravitational acceleration vectors, and are respectively and in the manifold representation, the following model can be constructed:

[0109] ;

[0110] The above equation represents the difference between the gravitational acceleration and . If the gravitational acceleration vector g2 is equal to the gravitational acceleration vector , then the projection point of coincides with the origin of the tangent space, and are also equal, and according to the logarithmic mapping relationship, the value is zero, where Log() is a custom function. At this time, the gravitational acceleration residual equation of the y-th key frame in this window can be constructed:

[0111] ;

[0112] is the attitude matrix of the world system in the vehicle system, left-multiplying can convert the standard gravitational acceleration from the world system to the vehicle system, is the estimate of the gravitational acceleration in the vehicle system. If this estimate is equal to the standard gravitational acceleration, then the above equation has a value of zero.

[0113] One key frame corresponds to one gravitational acceleration constraint. Summing up all key frames can obtain the gravitational acceleration error model within this window. The gravitational acceleration error model is specifically as follows:

[0114] ;

[0115] where is the gravitational acceleration constraint of the y-th key frame;

[0116] where the BA module is the model obtained by weighting the ICP error model and the gravitational acceleration error model. The BA module is specifically as follows:

[0117] ;

[0118] where is the covariance of the pose estimate, is the covariance of the gravity estimate.

[0119] At this time, the solution of the BA module is a standard least squares problem, which can be solved by the LM (Levenberg-Marquardt) method. Thus, the construction of the local BA model is completed, and the C++ standard library TBB (Thread Building Blocks) is called to concurrently execute the local BA optimization for all local windows in the first layer of the pyramid.

[0120] Preferably, due to the previous hierarchical BA, the relative pose transformations of adjacent key frames are obtained in different layers, so there are also multi-layer constraints in PGO. In this example, there are three layers of constraints in total.

[0121] On the other hand, for the j-th key frame in the (i + 1)-th layer , and the s·j-th key frame in the i-th layer are essentially the same. Therefore, the pyramid structure of the present application can be compressed into a single-layer structure during PGO optimization, and the result is as Figure 4 shown.

[0122] Obviously, the absolute poses of any two adjacent key frames , , and the relative pose transformation between them have been obtained in the previous BA process. If the values obtained by BA optimization have no deviation, then for the relative pose transformation between the j-th key frame and the (j + 1)-th key frame, there is the following relationship:

[0123] ;

[0124] However, in fact, even if the BA optimization effect is very good, there will inevitably be errors. By transposing and taking the logarithm of the above formula, we can get:

[0125] ;

[0126] That is the relative pose constraint error of a single pair of key frames. We can obtain the specific global PG0 residual model by weighted accumulation of the relative pose constraint errors of all key frames in all layers as follows:

[0127] ;

[0128] Where is the relative pose constraint between the j-th key frame and the (j + 1)-th key frame, is the pose of the j-th key frame and the pose of the (j + 1)-th key frame is the correlation between them, is the relative pose constraint between the s·j-th key frame and the (s·j + 1)-th key frame, is the pose of the s·j-th key frame The relevance between the pose of the s·(j + 1)-th key frame is the relative pose constraint between the ·j-th key frame and the ·(j + 1)-th key frame, which is ·j-th key frame and the ·(j + 1)-th key frame. All relevances are optimized by the BA module in the previous stage. which is the pose of the ·j-th key frame and the pose of the ·(j + 1)-th key frame between them. All relevances are optimized by the BA module in the previous stage.

[0129] A hierarchical optimization system for the back-end module of laser SLAM, using the above-mentioned hierarchical optimization method for the back-end module of laser SLAM, includes: a first construction module, a BA optimization module, a pyramid optimization module, and a global PGO module;

[0130] Wherein the first construction module is used for: constructing the first layer of the pyramid, using the input multi-frame continuous point clouds and the poses corresponding to each frame of point cloud as the input of the first layer of the pyramid;

[0131] The BA optimization module is used for constructing a BA module composed of an ICP error model and a gravitational acceleration error model;

[0132] The pyramid optimization module is used for layer-by-layer optimization of the pyramid through the BA module, where the optimized output result in the pyramid structure is used as the input of the next layer until the BA module optimizes to the top layer of the pyramid;

[0133] The global PGO module is used for constructing a global PG0 residual model, and constraining the relative pose transformation between adjacent key frames in the same layer of the pyramid through the global PG0 residual model.

[0134] Preferably, the first construction module includes a division unit, a rejection unit, a transfer unit, and a calculation unit;

[0135] The division unit is used for dividing the continuous key frames of the first layer of the pyramid into multiple local windows according to the local window size w and the local window step;

[0136] The rejection unit is used for setting that the first frame of the first layer of the pyramid does not fall into the first local window division;

[0137] The transfer unit is used for inputting point cloud data, setting up a world coordinate system with the pose of the first frame of point cloud as a reference, converting all point cloud data to the world coordinate system, downsampling the point cloud according to the set voxel resolution to obtain the first data, calculating a hash key based on the three-dimensional coordinates of the point cloud, and storing it in the hash table;

[0138] The calculation unit is used to traverse the hash table, determine whether the point cloud stored for each hash value meets the requirements of the plane feature. If it meets the requirements, the first data is retained; if it does not meet the requirements, the first data is deleted.

[0139] In the description of this specification, the descriptions with reference to the terms "one embodiment", "some embodiments", "illustrative embodiments", "examples", "specific examples", or "some examples", etc. mean that the specific features, structures, materials, or characteristics described in connection with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials, or characteristics described may be combined in any one or more embodiments or examples in a suitable manner.

[0140] Although the embodiments of the present invention have been shown and described, those of ordinary skill in the art can understand that various changes, modifications, substitutions, and variations can be made to these embodiments without departing from the principles and spirit of the present invention. The scope of the present invention is defined by the claims and their equivalents.

Claims

1. A back-end module hierarchical optimization method suitable for laser SLAM, characterized in that: The steps include: Step S1: construct the first layer of the pyramid, using the input of multiple frames of continuous point clouds and the pose corresponding to each frame of point clouds as the input of the first layer of the pyramid; Step S2: constructing a BA module, wherein the BA module is composed of an ICP error model and a gravity acceleration error model, and the BA module is used to constrain the ICP error and gravity acceleration of the data of each window of each layer of the pyramid respectively; The ICP error model is as follows: ； in is the number of keyframe poses, is the number of plane geometric features, is the number of points in the point cloud, is the Euclidean distance between the zth observation point of the xth plane feature and the xth plane feature in the yth key frame; The gravity acceleration error model is as follows: ； in is the gravity acceleration constraint of the y-th key frame; The BA module is a model obtained by weighting the ICP error model and the gravity acceleration error model. The specific details of the BA module are as follows: ； in is the covariance of pose estimation, is the covariance of the gravity estimate; Step S3: Optimize the pyramid layer by layer through the BA module, wherein the optimized output result is used as the input of the next layer in the pyramid structure, until the BA module optimizes to the top layer of the pyramid; Step S4: construct a global PG0 residual model, and constrain the relative pose transformation of adjacent key frames in the same layer in the pyramid through the global PG0 residual model.

2. A back-end module hierarchical optimization method suitable for laser SLAM according to claim 1, characterized in that: The first layer of the pyramid is set up as follows: The continuous key frames of the first layer of the pyramid are divided into multiple local windows according to the local window size w and the local window step size s; Setting the first frame of the first layer of the pyramid not to fall into the first local window division; Input point cloud data, establish a world coordinate system with the pose of the first frame of point cloud as a reference, convert all point cloud data to the world coordinate system, downsample the point cloud according to the set voxel resolution, obtain the first data, calculate the hash key according to the three-dimensional coordinates of the point cloud, and store it in the hash table; The hash table is traversed to determine whether the point cloud stored in each hash value meets the plane feature requirements. If so, the first data is retained; if not, the first data is deleted.

3. A back-end module hierarchical optimization method suitable for laser SLAM according to claim 2, characterized in that: The steps of judging whether the first data meets the requirements of the plane feature are as follows: Convert the point cloud corresponding to the first data into a point cluster; Get the covariance matrix of the point cluster; Perform eigendecomposition on the covariance matrix of the point cluster to obtain several eigenvectors and eigenvalues ​​corresponding to the eigenvectors; Obtaining the ratio of the smallest eigenvalue to the second smallest eigenvalue as the first ratio; It is determined whether the first ratio is less than a ratio threshold value. If so, the requirement of the plane feature is met; if so, the requirement of the plane feature is not met.

4. A back-end module hierarchical optimization method suitable for laser SLAM according to claim 1, characterized in that: The global PG0 residual model is as follows: ； in is the relative pose constraint between the jth key frame and the j+1th key frame, is the pose of the jth key frame The pose of the j+1th key frame The correlation between is the relative pose constraint between the s·jth key frame and the s·j+1th key frame, is the pose of the s jth key frame and the pose of the s·(j+1)th key frame The correlation between For the The jth key frame and the The relative pose constraints of the (j+1) keyframes, For the The poses of j keyframes With The poses of (j+1) keyframes The correlation between them, all correlations are obtained by optimizing the BA module in the previous stage.

5. A back-end module hierarchical optimization system suitable for laser SLAM, characterized in that: A back-end module hierarchical optimization method suitable for laser SLAM according to any one of claims 1 to 4, comprising: a first building module, a BA optimization module, a pyramid optimization module and a global PGO module; The first construction module is used to: construct the first layer of the pyramid, using the input of multiple frames of continuous point clouds and the pose corresponding to each frame of point clouds as the input of the first layer of the pyramid; The BA optimization module is used to construct a BA module consisting of an ICP error model and a gravity acceleration error model, wherein the ICP error model is specifically as follows: ； in is the number of keyframe poses, is the number of plane geometric features, is the number of points in the point cloud, is the Euclidean distance between the zth observation point of the xth plane feature and the xth plane feature in the yth key frame; The gravity acceleration error model is as follows: ； in is the gravity acceleration constraint of the y-th key frame; The BA module is a model obtained by weighting the ICP error model and the gravity acceleration error model. The specific details of the BA module are as follows: ； in is the covariance of pose estimation, is the covariance of the gravity estimate; The pyramid optimization module is used to optimize the pyramid layer by layer through the BA module, wherein the optimized output result is used as the input of the next layer in the pyramid structure until the BA module optimizes to the top layer of the pyramid; The global PGO module is used to construct a global PG0 residual model, and the relative pose transformation of adjacent key frames in the same layer in the pyramid is constrained by the global PG0 residual model.

6. A back-end module hierarchical optimization system suitable for laser SLAM according to claim 5, characterized in that: The first building module includes a division unit, a removal unit, a transfer unit and a calculation unit; The division unit is used to divide the continuous key frames of the first layer of the input pyramid into a plurality of local windows according to the local window size w and the local window step size; The elimination unit is used to set the first frame of the first layer of the pyramid not to fall into the first local window division; The transfer unit is used to input point cloud data, establish a world coordinate system with the pose of the first frame of point cloud as a reference, convert all point cloud data to the world coordinate system, downsample the point cloud according to the set voxel resolution to obtain first data, calculate a hash key according to the three-dimensional coordinates of the point cloud, and store it in a hash table; The calculation unit is used to traverse the hash table to determine whether the point cloud stored in each hash value meets the plane feature requirements, and if so, retain the first data; if not, delete the first data.

Images