Rear-end module layered optimization method and system suitable for laser SLAM

Abstract

A rear-end module layered optimization method suitable for laser SLAM comprises the following steps that a first layer of a pyramid is constructed, and input multi-frame continuous point clouds and poses corresponding to the point clouds of all the frames serve as input of the first layer of the pyramid; the BA module is used for carrying out data ICP error constraint and gravitational acceleration constraint on each window of each layer of the pyramid; the pyramid is optimized layer by layer through the BA module, and the optimized output result in the pyramid structure serves as the input of the next layer until the BA module is optimized to the topmost layer of the pyramid; according to the method, a global PG0 residual model is constructed, relative pose transformation of adjacent key frames on the same layer in a pyramid is constrained through the global PG0 residual model, an optimization problem is decomposed into a plurality of sub-problems which can be subjected to parallel computing in multiple layers through layered design, and pose variables related to a single sub-problem are few, so that the computing complexity and resource consumption are reduced, and the computing efficiency is improved. The convergence speed of solving the optimization problem is improved, and the possibility of divergence of map construction can be reduced.

Description

Technical Field

[0001] The present invention relates to the field of laser SLAM technology, and in particular to a back-end module hierarchical optimization method and system suitable for laser SLAM. Background Art

[0002] The SLAM (Simultaneous Localization and Mapping) system is a technology that uses sensors (visual cameras / lidar) to achieve autonomous positioning and 3D reconstruction of unfamiliar environments. It can be divided into visual SLAM and laser SLAM according to the different sensors. A complete laser SLAM system usually consists of a sensor data preprocessing module, a front-end module, and a back-end module. The system ultimately outputs pose estimation and a point cloud map. The back-end module is responsible for processing data from the front end (such as pose estimation, radar point cloud), re-optimizing the pose estimation within a certain time range, or jointly optimizing it with the point cloud map. The back-end module of the current laser SLAM system has a variety of technical routes, such as the graph optimization-based method, which is currently the most widely used one.

[0003] In the graph optimization-based method, by considering multiple constraints at the same time, all postures within a certain time window are optimized separately or jointly optimized with the point cloud map, thereby reducing the cumulative error, realizing the estimation of the robot's motion posture and the construction of the three-dimensional map of the environment. Although the graph optimization-based method 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 data within all time ranges simultaneously. The huge amount of data makes the optimization problem dimension very high and the problem solving speed is relatively slow. The computing performance of the main deployment objects of SLAM, such as drones, robots and other systems, is generally low. Computational complexity and resource consumption are a major bottleneck, especially in large-scale scene operations. 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. Optimization models often ignore the constraints on gravity acceleration. The current mainstream SLAM algorithms either assume that the carrier is aligned with the world system during system initialization, that is, the gravity acceleration is assumed to be (0, 0, -9.81) under the carrier system, or directly ignore the constraints on gravity acceleration, resulting in the carrier system not being correctly aligned to the world system. At the same time, incorrect estimation of gravity will also lead to inaccurate initialization estimation of zero bias, thus affecting the quality of mapping, especially for scenes 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-mentioned defects, the purpose of the present invention is to propose a back-end module hierarchical optimization method and system suitable for laser SLAM, so as to solve the problems of slow convergence speed and low mapping quality of the optimization problem.

[0007] To achieve this purpose, the present invention adopts the following technical solution: a back-end module hierarchical optimization method suitable for laser SLAM, comprising the following steps:

[0008] 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;

[0009] 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;

[0010] 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;

[0011] 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.

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

[0013] 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;

[0014] Setting 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, 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] 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.

[0017] Preferably, the steps of determining 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 a point cluster;

[0019] Get the covariance matrix of the point cluster;

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

[0021] 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.

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

[0023]

[0024] Among them, M p is the number of key frame poses, M f is the number of plane geometric features, N xy is the number of points in the point cloud, d xyz is the Euclidean distance between the zth observation point of the xth plane feature and the xth plane feature in the yth key frame;

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

[0026]

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

[0028] 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:

[0029]

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

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

[0032]

[0033] where r j，j+1 is the relative pose constraint between the jth key frame and the j+1th key frame, H j,j+1 is the pose T of the jth key frame j and the pose T of the j+1th key frame j+1 The correlation between s·j，j+1 is the relative pose constraint between the s·jth key frame and the s·j+1th key frame, H s·j，j+1is the pose T of the s jth key frame s·j and the pose T of the s·(j+1)th key frame s·j+1 The correlation between For the s 2 ·j keyframes and sth keyframe 2 Relative pose constraints of j+1 keyframes, For the s 2 The poses of j keyframes With the s 2 The poses of (j+1) keyframes The correlation between them, all correlations are obtained by optimizing the BA module in the previous stage.

[0034] A back-end module hierarchical optimization system applicable to laser SLAM, using the back-end module hierarchical optimization method applicable to laser SLAM, comprising: a first building module, a BA optimization module, a pyramid optimization module and a global PG0 module;

[0035] 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;

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

[0037] 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;

[0038] The global PG0 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.

[0039] Preferably, the first building module includes a division unit, a removal 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 input pyramid into a plurality of local windows according to the local window size w and the local window step size;

[0041] 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;

[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 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;

[0043] 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.

[0044] One of the above technical solutions has the following advantages or beneficial effects: 1. Through a layered design, the optimization problem is decomposed into multiple layers of sub-problems that can be calculated in parallel. A single sub-problem involves fewer posture variables, thereby reducing 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 by the present invention adds gravity acceleration constraints, so that the carrier system can be correctly aligned to the world system, especially in large-scale scenes and scenes with large movement in the Z-axis direction. Compared with the existing methods, it effectively reduces the cumulative error and distortion of the map, and improves the global consistency and accuracy of the point cloud map. At the same time, it can also improve the accuracy of the initial estimation of the zero bias of the IMU accelerometer, improve the accuracy of the IMU observation data, and provide a more accurate optimization result for the SLAM system. BRIEF DESCRIPTION OF THE DRAWINGS

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

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

[0048] Figure 3 Schematic diagram of a unit spherical manifold in one embodiment of the present invention.

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

[0050] The embodiments of the present invention are described in detail below, and examples of the embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals represent the same or similar elements or elements having the same or similar functions from beginning to end. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, and cannot be understood 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 are not to be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Thus, the features defined as "first" and "second" may explicitly or implicitly include one or more of the features. In the description of the embodiments of the present invention, the meaning of "plurality" is two or more, unless otherwise clearly and specifically defined.

[0052] In addition, the terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" and "second" may explicitly or implicitly include one or more of the features. In the description of the present invention, unless otherwise specified, the meaning of "plurality" is 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] like Figures 1 to 4 As shown, a back-end module hierarchical optimization method suitable for laser SLAM includes the following steps:

[0054] 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;

[0055] 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;

[0056] 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;

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

[0058]

[0059] The above formula means that the coordinates of all key frames in a window are converted to the coordinate system of the first key frame in the window, and then directly accumulated to get the coordinate system of the next layer. is the jth key frame in the i+1th layer; is the relative pose transformation matrix, which describes the pose transformation from the s·jth keyframe to the (s·j)+kth keyframe in the i-th layer; is the (s·j)+kth keyframe in the i-th layer; It means that Convert to In the coordinate system of the current window, the first key frame in the current window is taken as the origin.

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

[0061] Step S4: Construct a global PGO residual model, and constrain the relative pose transformation of adjacent key frames in the same layer of the pyramid through the global PGO residual model. After constructing the global PGO residual model, the solution can be completed by calling the GTSAM / G20 and other graph optimization libraries.

[0062] The pyramid-structured backend module designed by the present invention can improve the accuracy and real-time performance of pose estimation and map construction, especially for large-scale scenarios, and provide a reliable low-computational cost solution for autonomous navigation and map construction of the carrier in an unfamiliar environment. Compared with the existing methods, the layered design decomposes the optimization problem into multiple layers of sub-problems that can be calculated in parallel, and a single 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.

[0063] In addition, since the current mainstream solution is to optimize the input data simultaneously within a window and only consider the constraints of pose estimation and point cloud map, only ICP error is added to the optimization model, resulting in slow working speed of the back-end module and easy distortion of the constructed point cloud map. Especially for tasks in large-scale scenarios, there is an urgent need to improve the accuracy, timeliness and global consistency of the system's pose estimation and map construction.

[0064] To this end, the BA optimization model designed in the present invention adds a gravity acceleration constraint so that the carrier system can be correctly aligned to the world system, especially in large-scale scenes and scenes with large movement in the Z-axis direction. Compared with the existing methods, it effectively reduces the cumulative error and distortion of the map, and improves the global consistency and accuracy of the point cloud map. At the same time, it can also improve the accuracy of the initial estimation of the zero bias of the IMU accelerometer, improve the accuracy of the IMU observation data, and provide a more accurate optimization result for the SLAM system.

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

[0066] 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;

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

[0068] 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;

[0069] 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.

[0070] The result actually outputted by the existing SLAM is a relative pose, which leads to the problem of freedom when there is no absolute reference system. In order to avoid this problem, the first frame of the key frame input to the first layer of the pyramid is used as a reference in the present invention. Then all 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 the whole SLAM process has established a unified relative coordinate system, it eliminates the cumulative error of the translational freedom caused by the inaccurate initial pose estimation between different frames, thereby solving the problem of freedom.

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

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

[0073] Specifically, the transformation into point clusters can be performed using the following formula:

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

[0075] Get the covariance matrix of the point cluster;

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

[0077]

[0078] Perform eigendecomposition on the covariance matrix of the point cluster to obtain several eigenvectors and the eigenvalues ​​corresponding to the eigenvectors; perform eigenvalue decomposition on the covariance matrix to obtain several eigenvectors and the corresponding eigenvalues. This step is implemented by calling the Eigen library function in the C++ language.

[0079] Obtaining the ratio of the smallest eigenvalue to the second smallest eigenvalue as the first ratio;

[0080] 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.

[0081] The eigenvalue describes the degree of dispersion of the point cluster in the direction of the corresponding eigenvector. Usually, the size 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 smallest eigenvalue can be understood as the normal vector of the plane. If the ratio of the smallest eigenvalue to the second smallest eigenvalue is very small, it means that the distribution of the point cluster in the direction of the eigenvector corresponding to the smallest eigenvalue is more concentrated, and the distribution in other directions is more 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, the set of observation points of the yth key frame of the window for the xth plane feature is pf xy , the number of points in this point set is N xy , let the zth point of the point set be pf xyz , by left multiplying the point pf in the local coordinate system xyz Convert to the world system and get p xyz ,

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

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

[0085] Where n x is the normal vector of the plane, q x is any point on the plane;

[0086] The Euclidean distance between all observation points that match the xth plane feature in the yth keyframe can be obtained as follows:

[0087]

[0088] By accumulating the Euclidean distances between all observation points matching the x-th plane feature for all keyframe poses in the local window, we can get:

[0089]

[0090] By accumulating the Euclidean distances between all observation points matching all plane features for all keyframe poses in the local window, we can get:

[0091]

[0092] In theory, after pose estimation The converted p xyz , spatially, it will overlap with the corresponding plane feature, that is, the Euclidean distance is zero, but in fact, due to various errors and noise, the relative pose and the plane feature π x It is not necessarily accurate, so the Euclidean distance is never zero, and the direct purpose of ICP (Iterative Closest Point) is to find a suitable pose estimation and plane feature to minimize the Euclidean distance. In summary, the ICP error model is as follows:

[0093]

[0094] Among them, M p is the number of key frame poses, M f is the number of plane geometric features, N xy is the number of points in the point cloud, d xyz is the Euclidean distance between the zth observation point of the xth plane feature and the xth plane feature in the yth key frame;

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

[0096] In the front-end odometer, when a new key frame is added (corresponding to a key frame), an estimate of the gravitational acceleration in the world system is obtained. w and the covariance matrix C g , the result of point cloud registration can be used to transfer the estimation of gravitational acceleration and its covariance from the world system to the carrier system, as shown in the formula:

[0097]

[0098] On the other hand, it is obvious that the direction of gravity acceleration in the world system is vertically downward, so we define the standard gravity acceleration vector in the world system (map system) Multiply the pose homogeneous matrix on the left to obtain the standard gravitational acceleration vector under the load system

[0099]

[0100] When the front-end odometer estimates the gravity acceleration without error, 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 gravity acceleration by the front-end odometer, so we need to establish a model to measure the error.

[0103] Considering the standard gravitational acceleration (9.80665m / S 2 ) is already quite accurate and can meet the mission requirements at most locations on Earth, so the modulus of the gravity acceleration vector is set to a constant, which is always equal to 9.80665. On the basis that the modulus of gravity acceleration is always a constant, we can use a two-dimensional vector to express the three-dimensional gravity acceleration vector, and then pass the gravity acceleration vector through a unit spherical manifold S 2 Let D be the state variable of the manifold and let ε be the identity of the manifold, as Figure 3 As 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 modulus of gravitational acceleration has been restricted to a constant, the two-dimensional vector D B Represents the gravitational acceleration vector g B (3D), the same applies Represents the standard gravitational acceleration vector (3D). Let D be any point on the manifold, and use the logarithmic mapping to give the relationship between the manifold and the tangent space:

[0105] Where Bε is a set of standard orthogonal bases in the tangent space of ε;

[0106] Let μ be any point in the tangent space of the identity ε, and use the exponential map to give the relationship between the tangent space and the manifold:

[0107] Where R 2 is the two-dimensional vector where μ is located;

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

[0109]

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

[0111]

[0112] is the attitude matrix of the world system in the carrier system, Left multiplication The standard gravitational acceleration can be converted from the world system to the load system, D B is an estimate of the gravitational acceleration under the load system. If this estimate is equal to the standard gravitational acceleration, then The value of is zero.

[0113] One key frame corresponds to one gravity acceleration constraint. The gravity acceleration error model in the window can be obtained by accumulating all key frames. The gravity acceleration error model is as follows:

[0114]

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

[0116] 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:

[0117]

[0118] in is the covariance of pose estimation, 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. At this point, the construction of the local BA model is completed, and the C++ standard library TBB (Thread Building Blocks) is called to concurrently perform local BA optimization on all local windows in the first layer of the pyramid.

[0120] Preferably, due to the previous hierarchical BA, the relative pose transformation of adjacent key frames is obtained at different layers, so there are also multiple layers of constraints in PGO, and there are three layers of constraints in this example.

[0121] On the other hand, for the jth key frame of the i+1th layer and the s·jth key frame of the i-th layer The pyramid structure of the present application can be compressed into a single-layer structure during PGO optimization. Figure 4 shown.

[0122] Obviously, the absolute poses of any two adjacent key frames have been obtained in the previous BA process. And the relative pose transformation between them If the value obtained by BA optimization is without deviation, then the relative pose transformation between the jth key frame and the j+1th key frame has the following relationship:

[0123]

[0124] But in fact, even if the BA optimization effect is good, there are still errors. By shifting the terms 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 weight and accumulate the relative pose constraint errors of all key frames in all layers to obtain the global PGO residual model as follows:

[0127]

[0128] where r j，j+1 is the relative pose constraint between the jth key frame and the j+1th key frame, H j，j+1 is the pose T of the jth key frame j and the pose T of the j+1th key frame j+1 The correlation between s·j，j+1 is the relative pose constraint between the s·jth key frame and the s·j+1th key frame, H s·j，j+1 is the pose T of the s jth key frame s·j and the pose T of the s·(j+1)th key frame s·j+1 The correlation between For the s 2 ·j keyframes and sth keyframe 2 Relative pose constraints of j+1 keyframes, For the s 2 The poses of j keyframes With the s 2 The poses of (j+1) keyframes The correlation between them, all correlations are obtained by optimizing the BA module in the previous stage.

[0129] A back-end module hierarchical optimization system applicable to laser SLAM, using the back-end module hierarchical optimization method applicable to laser SLAM, comprising: a first building module, a BA optimization module, a pyramid optimization module and a global PGO module;

[0130] 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;

[0131] The BA optimization module is used to construct a BA module consisting of an ICP error model and a gravity acceleration error model;

[0132] 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;

[0133] 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.

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

[0135] 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;

[0136] 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;

[0137] 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;

[0138] 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.

[0139] In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "illustrative embodiments", "examples", "specific examples", or "some examples" means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representation of the above terms does 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, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that 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; 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 ICP error model is as follows: Among them, M p is the number of key frame poses, M f is the number of plane geometric features, N xy is the number of points in the point cloud, d xyz 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: where r y 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.

5. 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: where r j,j+1 is the relative pose constraint between the jth key frame and the j+1th key frame, H j,j+1 is the pose T of the jth key frame j and the pose T of the j+1th key frame j+1 The correlation between s·j,j+1 is the relative pose constraint between the s·jth key frame and the s·j+1th key frame, H s·j,j+1 is the pose T of the s jth key frame s·j and the pose T of the s·(j+1)th key frame s·j+1 The correlation between For the s 2 ·j keyframes and sth keyframe 2 Relative pose constraints of j+1 keyframes, For the s 2 The poses of j keyframes With the s 2 The poses of (j+1) keyframes The correlation between them, all correlations are obtained by optimizing the BA module in the previous stage.

6. 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 using any one of claims 1 to 5, 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 composed of an ICP error model and a gravity acceleration error model; 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.

7. A back-end module hierarchical optimization system suitable for laser SLAM according to claim 6, 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