Laser point cloud indoor mapping and merging method based on multilayer direct optimization

Abstract

The invention relates to a laser point cloud indoor mapping and merging method based on multilayer direct optimization, and belongs to the technical field of robot vision. The method mainly comprises the following steps: filtering ground information; fragmenting the point cloud sequence based on VFH comparison; estimating a point cloud motion state based on a multilayer direct method; and map establishment and fragment combination. The method does not need to depend on an additional sensor, firstly, invalid ground point cloud information is filtered out, and the quality of point cloud data is effectively improved; then dividing the long point cloud sequence into a plurality of segments based on a VFH comparison algorithm to intermittently return accumulation drift to zero, thereby ensuring the accuracy and stability of mapping; when NDT registration and nonlinear optimization are executed in each segment, a multi-layer direct method is selected to correct a state estimation value based on inter-frame pixel brightness information, a complex point cloud local feature extraction and matching process is avoided, the complexity of an algorithm is greatly reduced, and good real-time performance is shown while high-precision mapping is kept.

Description

Technical Field

[0001] The invention relates to a laser point cloud indoor mapping and merging method based on multi-layer direct optimization, belonging to the technical field of robot vision. Background Art

[0002] Laser point cloud map is a 3D data representation widely used in robot navigation, 3D reconstruction and environmental perception. In the process of point cloud map building, accurate estimation of inter-frame motion state is crucial for environmental modeling, real-time analysis and positioning accuracy. Usually, point cloud pose estimation can be obtained by real-time data calculation provided by sensors on the motion platform, such as inertial measurement unit (IMU), global positioning system (GPS), odometer and other devices. However, the introduction of additional sensors will also bring a series of problems: first, additional sensors increase the complexity and cost of the system, which is not economical and practical for many fixed and single application scenarios; second, data fusion and calibration between different sensors is a complex and error-prone process, which may introduce new error sources; in addition, data processing and analysis of additional sensors also require additional algorithm support, which will further increase the computational burden of the mapping system. It can be seen that the above-mentioned problems have posed challenges to the algorithm development and parallel optimization difficulty in the research work.

[0003] In indoor environments that are relatively compact and do not have too many dynamic factors, a relatively easy-to-integrate single laser sensor lightweight mapping solution may also achieve ideal results. Generally speaking, pure laser point cloud mapping solutions mostly follow the research paradigm: first, use scanning matching algorithms such as CSM and ICP to perform preliminary point cloud registration to obtain the initial pose transformation; then, construct a cost function based on the distance error between the predicted point and the actual point, and then perform nonlinear optimization. However, this idea still has some room for improvement in indoor environments: (1) When processing indoor environments, algorithms such as CSM and ICP should not be over-sampled due to the rich details of the point cloud, which leads to a significant increase in the corresponding relationship search space of the CSM algorithm and a sharp increase in computational complexity; although the ICP algorithm is more efficient than the CSM algorithm, it is more difficult to accurately match corresponding points when the indoor geometric features are complex, which prolongs the running time of the algorithm. (2) When performing nonlinear optimization, the indoor environment has rich texture, structure, and color changes. If the optimization only relies on the distance information between corresponding points, these key visual information will be ignored, which may lead to insufficient accuracy or errors in the mapping results. (3) Existing solutions often fail to effectively filter out redundant point clouds on the ground, ceiling, etc. From the perspective of spatial distribution, objects with significant vertical features such as walls, stairs, tables and chairs in indoor environments are crucial for map construction and path planning for autonomous navigation. The nonlinear optimization process should be too inclined to reduce the error in the direction of more critical vertical feature points. Summary of the invention

[0004] In order to solve some problems existing in the above-mentioned laser point cloud indoor mapping process, the present invention proposes a laser point cloud indoor mapping and merging method based on multi-layer direct optimization. This method does not require the use of additional sensors, but divides a long point cloud sequence into multiple segments, so that the accumulated error can be discontinuously reset to zero; when performing point cloud registration and nonlinear optimization within each segment, the multi-layer direct method in camera vision SLAM is used for reference, and the pixel brightness information between consecutive frames is used to directly and quickly complete the nonlinear optimization of the state estimation value, thereby maintaining high-precision state estimation while taking into account the real-time nature of mapping.

[0005] To achieve the above object, the technical solution of the present invention is:

[0006] A laser point cloud indoor mapping and merging method based on multi-layer direct optimization includes the following steps:

[0007] S1. Ground information filtering;

[0008] S2. Point cloud sequence fragmentation based on VFH alignment;

[0009] S3. Point cloud motion state estimation based on multi-layer direct method optimization;

[0010] S4. Map building and segment merging.

[0011] Furthermore, the ground information is filtered out in step S1, and the specific process includes:

[0012] S11. Establish a spatial rectangular coordinate system O-XYZ with the center of the laser radar as the coordinate origin O, where the XOY plane is parallel to the motion platform where the laser radar is located, and the Z axis is perpendicular to the XOY plane; filter out point clouds with a distance less than 0.5m from O to prevent interference with mapping caused by the motion platform;

[0013] S12. For all points in the point cloud of the current frame, calculate the azimuth angle α of their projection on the XOY plane relative to the positive direction of the Y axis one by one. If α<0, then α=α+2π. Set the angle resolution Δθ, then all point clouds will fall into the 2π / Δθ angle interval centered at O, that is, the point cloud is clustered according to α. For the cluster with index number i, it is recorded as cluster[i];

[0014] S13. For each point in cluster[i], calculate the distance r of its projection on the XOY plane relative to O, and sort them from small to large according to r. The sorted cluster is recorded as cluster[i]′;

[0015] S14. Measure the height b of the laser radar from the ground, project O to the ground, and obtain point O′(0,0,-b); set two thresholds: one is the pitch angle difference β1 between two adjacent points in cluster[i]′, preferably, β1=8°; the other is the pitch angle difference β2 between the current point and point O′, preferably, β2=5°;

[0016] S15. For any point C(x,y,z) in cluster[i]′, calculate the pitch angle between it and the previous point C0(x0,y0,z0) in the sorting. If C is the first point in cluster[i]′, calculate the pitch angle relative to O′ and the pitch angle β with O′ CO′ As a preferred option, β CO′ It can be calculated as follows:

[0017]

[0018]

[0019] Compare With β1, there are two possibilities:

[0020] S151. If C0 is determined to be a ground point, then C is still determined to be a ground point; if C0 is a non-ground point but satisfies |β CO′ |<|β2|, then C is also determined to be a ground point;

[0021] S152. Set the distance threshold d, preferably, d = 0.2m; if the distance between C and C0 is d c >d, then the two are far apart, if |β CO′ |<|β2|, then C is determined to be a ground point;

[0022] S16. If the scene is indoors, then cluster[i]′ contains both ground and house top point cloud distributions. In this case, both can be filtered out according to step S15.

[0023] S17. Traverse all clusters to obtain a single-frame non-ground point cloud Cloud; for continuous multi-frame point clouds acquired or recorded in real time, a continuous non-ground point cloud sequence can be obtained according to steps S11 to S16, recorded as Cloud_rec.

[0024] Furthermore, the point cloud sequence fragmentation based on VFH comparison in step S2 is specifically performed as follows:

[0025] S21. Define the viewpoint V(0,0,h) in the coordinate system O-XYZ, h and the maximum value z of the point cloud in the positive direction of the Z axis in Cloud_rec max The relationship is: h = z max +10; translate the point cloud in Cloud_rec frame by frame to the coordinate system V-xyz with the observation point V as the origin. In this coordinate system, the coordinates of point V are (0, 0, 0), and the coordinates of any point in Cloud_rec are (x i ,y i ,z i ) is converted to (x i ,y i ,z i -h), the converted point cloud sequence is recorded as Cloud_rec V ;

[0026] S22. For Cloud_rec V For each frame of point cloud in , the viewpoint feature histogram (VFH) of each frame is calculated with viewpoint V(0,0,0), and a histogram array containing 308 elements is obtained;

[0027] S23. Use the knnSearch function in the flann function library to calculate the error k between the histograms of two adjacent frames. If k is greater than the set threshold k0, it is considered that the features between the two frames are quite different, and Cloud_rec V Truncated here; thus, q point cloud sequence segments can be formed. As a preference, k0 should not be selected too low, otherwise the q value will be too large, increasing the merging steps. Generally speaking, q≤5.

[0028] Furthermore, the point cloud motion state estimation based on multi-layer direct method optimization in step S3 specifically includes:

[0029] S31. For the adjacent previous frame point cloud f1 and next frame point cloud f2 in the sequence segment, use the normal distribution transformation (NDT) to align from f2 to f1, that is, transform f2 to the coordinate system where f1 is located, and obtain the initial matrix rlt of the inter-frame conversion;

[0030] S32. Point cloud projection to establish brightness image;

[0031] S33. Nonlinear optimization based on multi-level direct method.

[0032] Further, the point cloud projection in step S32 establishes a brightness image, which specifically includes the following steps:

[0033] S321. Project all points of f1 onto the plane z=-1: According to the similar triangle feature, for the point with index v in f1 (x v ,y v ,zv ), the projected coordinates The obtained planarized two-dimensional point cloud is denoted as f 1_2D , calculate the maximum value of its coordinates in the X and Y directions (x′ max ,y′ max ), minimum value (x′ min ,y′ min );

[0034] S322. 1_2D Voxelization is performed with voxel width dd and voxel brightness as the average reflection intensity of each point in the grid. A two-dimensional brightness image is obtained, denoted as IMG1, with a size of w·h, where Then any point (x v ,y v ,z v ) in its projected IMG1 corresponds to the pixel coordinates (u v ,v v )for:

[0035]

[0036] In the formula, floor is the rounding operation;

[0037] S323. For the point cloud f2, according to steps S321 to S322, a brightness image IMG2 can be obtained.

[0038] Further, the nonlinear optimization based on the multi-layer direct method in step S33 specifically includes the following steps:

[0039] S331. For any point b2(x2, y2, z2) in f2, its corresponding brightness in IMG2 is recorded as I2(b2), the estimated point coordinates of b2 in f1 are b1=T·b2, and the brightness in IMG1 is I1(b1); where T is a variable, representing the pose transformation matrix to be optimized, and its initial value is rlt described in step 31; then according to the photometric consistency assumption of the direct method, the error function e(T) for performing nonlinear optimization can be determined as:

[0040] e(T)=I2(b2)-I1(T·b2) (4)

[0041] S332. To minimize the error function, it is necessary to calculate the derivative of the error with respect to the posture T. According to the Lie algebra derivation and the perturbation model, it can be converted into the derivation of the perturbation amount ξ of T, that is:

[0042]

[0043] in:

[0044] m1(u,v) is the pixel point corresponding to b1 after being projected onto I1. Represents the brightness gradient of image I1 at pixel m1(u,v), we have:

[0045]

[0046] According to the projection relationship described in formula (3), assuming the coordinates of point b1 are (x, y, z), we have:

[0047]

[0048] According to the Lie algebra perturbation model, we have:

[0049]

[0050] where b1 ∧ represents the antisymmetric matrix corresponding to b1, and I is the unit matrix;

[0051] S333. Multiplying equations (6), (7), and (8) together, we can obtain the result of equation (5), which is the form of the Jacobian matrix J of the error for the Lie algebra:

[0052]

[0053] S334. Based on the G2O optimization library, the vertex type is BaseVertex<6, Sophus::SE3d>, the edge type is BaseUnaryEdge<1,double,VertexPose>, the Jacobian matrix J is set according to formula (9) in the linearizeOplus() function, and the error e(T) is set according to formula (4) in the computeError() function, and the initial value of T is specified as rlt;

[0054] As a preference, the optimization solver can be configured as follows: Select the 6*1 block solver, i.e. g2o::BlockSolver <g2o::BlockSolverTraits<6,1> >, indicating that the optimization variable dimension is 6 and the error value dimension is 1; then set the linear solver type to the pose matrix type, i.e. g2o::LinearSolverDense <blocksolvertype::posematrixtype>; During the optimization process, the Levenberg-Marquardt descent method, i.e. g2o::OptimizationAlgorithmLevenberg, is used for solving, and the maximum number of iterations is set to 40 times. The optimization algorithm is executed to obtain the optimized transformation matrix rlt′;

[0055] S335. Use multi-layer scaling to build an image pyramid. As a preferred method, set the number of layers to 4, and the scaling factors from top to bottom are 0.25, 0.5, 0.75, and 1 respectively; in the upper layer image, use the rlt′ obtained in S331 to S334 as the initial value of the lower layer optimization, and optimize again to improve the accuracy of rlt′ layer by layer to obtain the multi-layer optimization result rlt′ m ;

[0056] S336. Use the transformPointCloud function in the PCL library to transform f2 through rlt′ m Transform to the coordinate system of f1 to complete the mapping of adjacent frames.

[0057] Furthermore, the specific process of the map creation and segment merging in step S4 includes:

[0058] S41. According to step S3, a sliding window method is used to obtain an optimized conversion matrix between all adjacent frames in the sequence segment;

[0059] S42. transforming the point clouds of each frame in the segment into the coordinate system of the first frame to form a single-frame mapping result;

[0060] S43. Complete multi-map merging based on OPENGL mapping.

[0061] Further, in step S43, the merging of multiple maps is completed based on OPENGL mapping, and the specific merging steps are as follows:

[0062] S431. In OPENGL, the mapping results of each fragment are loaded into multiple layers. The coordinate systems of each layer are inconsistent, so they need to be converted to the same coordinate system. For this purpose, a layer needs to be selected as a reference layer, which is recorded as layer0;

[0063] S432. Use the perspective projection matrix and vertex shader in OPENGL, with the mouse button and drag, to achieve the rotation and translation of the current layer until it matches the layer0 part of the point cloud to indicate the registration, and record the starting coordinates M1 (m1, n1) and the ending coordinates M2 (m2, n2) of the mouse movement in the software window coordinate system; the cosine theorem can be used to calculate the rotation angle θ, and its corresponding rotation vector (rotation around the Z axis) The corresponding rotation matrix R can be obtained from the Rodriguez formula;

[0064] S433. The standardized device coordinate interval x, y∈[-1,1] agreed by OPENGL, assuming the software window size is w s ·h s After mapping the window to the OPENGL coordinate system, the horizontal and vertical scaling factors are w and s / 2,h s / 2; the translation vector in the known window coordinate system is According to the horizontal and vertical scaling factors, the translation vector in OPENGL is Because the positions of M1 and M2 are derived from the point cloud position selection on the screen, and the point cloud position displayed on the screen is obtained through OPENGL perspective projection, Inverse perspective projection is also required to obtain the true normalized translation vectors of M1 and M2 generated in OPENGL. Since the current perspective projection matrix (generally denoted as MVP) can be determined and provided by the OPENGL camera position and target position, it is considered a known quantity, and we have:

[0065]

[0066] Therefore, in the OPENGL coordinate system, we get the transformation matrix T caused by dragging the mouse gl It can be written as

[0067]

[0068] S434. Set the current layer through T gl Transform to layer0 to complete the merging of the current layer into the reference layer;

[0069] S435. Implement steps S432 to S434 for all layers to obtain the final merged complete map.

[0070] The present invention discloses a laser point cloud indoor mapping and merging method based on multi-layer direct optimization. The method does not need to rely on additional sensors and loop detection. By filtering out the ground and roof information received in the three-dimensional point cloud, noise interference is effectively reduced and the quality of mapping is improved. Then, based on the VFH comparison algorithm, the long sequence point cloud is divided into multiple segments, so that the accumulated drift is intermittently reset to zero, thereby ensuring the accuracy and stability of mapping. When performing NDT registration and nonlinear optimization within each segment, the multi-layer direct method is used to fine-tune and correct the initial motion state provided by the NDT algorithm based on the photometric consistency assumption. Since the above process does not require point cloud feature extraction and matching, the computational complexity is greatly reduced, and the registration-optimization work can be completed within the current frame duration, showing good real-time performance. Finally, a point cloud merging strategy based on OPENGL mapping is adopted to integrate the mapping results of the sequence segments to generate a complete and continuous point cloud map. BRIEF DESCRIPTION OF THE DRAWINGS

[0071] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0072] Figure 1 It is a general implementation flow chart of the present invention;

[0073] Figure 2 It is a flow chart of ground information filtering of the present invention;

[0074] Figure 3 This is the point cloud image of the present invention before filtering out the ground and roof information;

[0075] Figure 4 It is the point cloud image after filtering out the ground and roof information of the present invention;

[0076] Figure 5 is a flow chart of point cloud sequence fragmentation based on VFH alignment of the present invention;

[0077] Figure 6 It is a nonlinear optimization flow chart based on the multi-layer direct method of the present invention;

[0078] Figure 7 This is the effect of the two fragmented maps before merging in OPENGL of the present invention;

[0079] Figure 8 This is the result of merging two fragmented maps displayed in OPENGL according to the present invention. DETAILED DESCRIPTION

[0080] The present invention will be further described in detail below in conjunction with the accompanying drawings and embodiments. It will be appreciated that the specific embodiments described herein are only used to explain the relevant inventions and are not intended to limit the invention. It should also be noted that, for ease of description, only portions related to the relevant inventions are shown in the accompanying drawings. It should be noted that, in the absence of conflict, the embodiments in this application and the features in the embodiments may be combined with each other. The present application will be described in detail below with reference to the accompanying drawings and in conjunction with the embodiments.

[0081] As attached Figure 1 As shown, the laser point cloud indoor mapping and merging method based on multi-layer direct optimization of the present invention mainly includes the following four steps:

[0082] S1. Ground information filtering;

[0083] S2. Point cloud sequence fragmentation based on VFH alignment;

[0084] S3. Point cloud motion state estimation based on multi-layer direct method optimization;

[0085] S4. Map building and segment merging.

[0086] The ground information is filtered out in step S1, and the specific process includes:

[0087] S11. Establish a spatial rectangular coordinate system O-XYZ with the center of the laser radar as the coordinate origin O, where the XOY plane is parallel to the motion platform where the laser radar is located, and the Z axis is perpendicular to the XOY plane; filter out point clouds with a distance less than 0.5m from O to prevent interference with mapping caused by the motion platform;

[0088] S12. For all points in the point cloud of the current frame, calculate the azimuth angle α of their projection on the XOY plane relative to the positive direction of the Y axis one by one. If α<0, then α=α+2π. Set the angle resolution Δθ, then all point clouds will fall into the 2π / Δθ angle interval centered at O, that is, the point cloud is clustered according to α. For the cluster with index number i, it is recorded as cluster[i];

[0089] S13. For each point in cluster[i], calculate the distance r of its projection on the XOY plane relative to O, and sort them from small to large according to r. The sorted cluster is recorded as cluster[i]′;

[0090] S14. Measure the height b of the laser radar from the ground, project O to the ground, and obtain point O′(0,0,-b); set two thresholds: one is the pitch angle difference β1 between two adjacent points in cluster[i]′, preferably, β1=8°; the other is the pitch angle difference β2 between the current point and point O′, preferably, β2=5°;

[0091] S15. For any point C(x,y,z) in cluster[i]′, calculate the pitch angle between it and the previous point C0(x0,y0,z0) in the sorting. If C is the first point in cluster[i]′, calculate the pitch angle relative to O′ and the pitch angle β with O′ CO′ As a preferred option, β CO′ It can be calculated as follows:

[0092]

[0093]

[0094] Compare With β1, there are two possibilities:

[0095] S151. If C0 is determined to be a ground point, then C is still determined to be a ground point; if C0 is a non-ground point but satisfies |β CO′ |<|β2|, then C is also determined to be a ground point;

[0096] S152. Set the distance threshold d, preferably, d = 0.2m; if the distance between C and C0 is d c >d, then the two are far apart, if |β CO′ |<|β2|, then C is determined to be a ground point;

[0097] S16. If the scene is indoors, then cluster[i]′ contains both ground and house top point cloud distributions. In this case, both can be filtered out according to step S15.

[0098] S17. Traverse all clusters to obtain a single-frame non-ground point cloud Cloud; for continuous multi-frame point clouds acquired or recorded in real time, a continuous non-ground point cloud sequence can be obtained according to steps S11 to S16, recorded as Cloud_rec.

[0099] The specific process of the point cloud sequence fragmentation based on VFH comparison in step S2 is as follows:

[0100] S21. Define the viewpoint V(0,0,h) in the coordinate system O-XYZ, h and the maximum value z of the point cloud in the positive direction of the Z axis in Cloud_rec max The relationship is: h = z max +10; translate the point cloud in Cloud_rec frame by frame to the coordinate system V-xyz with the observation point V as the origin. In this coordinate system, the coordinates of point V are (0, 0, 0), and the coordinates of any point in Cloud_rec are (x i ,y i ,z i ) is converted to (x i ,y i ,z i -h), the converted point cloud sequence is recorded as Cloud_rec V ;

[0101] S22. For Cloud_rec V For each frame of point cloud in , the viewpoint feature histogram (VFH) of each frame is calculated with viewpoint V(0,0,0), and a histogram array containing 308 elements is obtained;

[0102] S23. Use the knnSearch function in the flann function library to calculate the error k between the histograms of two adjacent frames. If k is greater than the set threshold k0, it is considered that the features between the two frames are quite different, and Cloud_rec V Truncated here; thus, q point cloud sequence segments can be formed. As a preference, k0 should not be selected too low, otherwise the q value will be too large, increasing the merging steps. Generally speaking, q≤5.

[0103] The specific process of the point cloud motion state estimation based on multi-layer direct method optimization in step S3 includes:

[0104] S31. For the adjacent previous frame point cloud f1 and next frame point cloud f2 in the sequence segment, use the normal distribution transformation (NDT) to align from f2 to f1, that is, transform f2 to the coordinate system where f1 is located, and obtain the initial matrix rlt of the inter-frame conversion;

[0105] S32. Point cloud projection to establish brightness image;

[0106] S33. Nonlinear optimization based on multi-level direct method.

[0107] Step S32, in which the point cloud is projected to establish a brightness image, specifically includes the following steps:

[0108] S321. Project all points of f1 onto the plane z=-1: According to the similar triangle feature, for the point with index v in f1 (x v ,y v ,z v ), the projected coordinates The obtained planarized two-dimensional point cloud is denoted as f 1_2D , calculate the maximum value of its coordinates in the X and Y directions (x′ max ,y′ max ), minimum value (x′ min ,y′ min );

[0109] S322. 1_2D Voxelization is performed with voxel width dd and voxel brightness as the average reflection intensity of each point in the grid. A two-dimensional brightness image is obtained, denoted as IMG1, with a size of w·h, where Then any point (x v ,y v ,z v ) in its projected IMG1 corresponds to the pixel coordinates (u v ,v v )for:

[0110]

[0111] In the formula, floor is the rounding operation;

[0112] S323. For the point cloud f2, according to steps S321 to S322, a brightness image IMG2 can be obtained.

[0113] The nonlinear optimization based on the multi-layer direct method in step S33 specifically includes the following steps:

[0114] S331. For any point b2(x2, y2, z2) in f2, its corresponding brightness in IMG2 is recorded as I2(b2), the estimated point coordinates of b2 in f1 are b1=T·b2, and the brightness in IMG1 is I1(b1); where T is a variable, representing the pose transformation matrix to be optimized, and its initial value is rlt described in step 31; then according to the photometric consistency assumption of the direct method, the error function e(T) for performing nonlinear optimization can be determined as:

[0115] e(T)=I2(b2)-I1(T·b2) (4)

[0116] S332. To minimize the error function, it is necessary to calculate the derivative of the error with respect to the posture T. According to the Lie algebra derivation and the perturbation model, it can be converted into the derivation of the perturbation amount ξ of T, that is:

[0117]

[0118] in:

[0119] m1(u,v) is the pixel point corresponding to b1 after being projected onto I1. Represents the brightness gradient of image I1 at pixel m1(u,v), we have:

[0120]

[0121] According to the projection relationship described in formula (3), assuming the coordinates of point b1 are (x, y, z), we have:

[0122]

[0123] According to the Lie algebra perturbation model, we have:

[0124]

[0125] where b1 ∧ represents the antisymmetric matrix corresponding to b1, and I is the unit matrix;

[0126] S333. Multiplying equations (6), (7), and (8) together, we can obtain the result of equation (5), which is the form of the Jacobian matrix J of the error for the Lie algebra:

[0127]

[0128] S334. Based on the G2O optimization library, the vertex type is BaseVertex<6, Sophus::SE3d>, the edge type is BaseUnaryEdge<1,double,VertexPose>, the Jacobian matrix J is set according to formula (9) in the linearizeOplus() function, and the error e(T) is set according to formula (4) in the computeError() function, and the initial value of T is specified as rlt;

[0129] As a preference, the optimization solver can be configured as follows: Select the 6*1 block solver, i.e. g2o::BlockSolver <g2o::BlockSolverTraits<6,1> >, indicating that the optimization variable dimension is 6 and the error value dimension is 1; then set the linear solver type to the pose matrix type, i.e. g2o::LinearSolverDense <blocksolvertype::posematrixtype>; During the optimization process, the Levenberg-Marquardt descent method, i.e. g2o::OptimizationAlgorithmLevenberg, is used for solving, and the maximum number of iterations is set to 40 times. The optimization algorithm is executed to obtain the optimized transformation matrix rlt′;

[0130] S335. Use multi-layer scaling to build an image pyramid. As a preferred method, set the number of layers to 4, and the scaling factors from top to bottom are 0.25, 0.5, 0.75, and 1 respectively; in the upper layer image, use the rlt′ obtained in S331 to S334 as the initial value of the lower layer optimization, and optimize again to improve the accuracy of rlt′ layer by layer to obtain the multi-layer optimization result rlt′ m ;

[0131] S336. Use the transformPointCloud function in the PCL library to transform f2 through rlt′ m Transform to the coordinate system of f1 to complete the mapping of adjacent frames.

[0132] The specific process of the map creation and segment merging in step S4 includes:

[0133] S41. According to step S3, a sliding window method is used to obtain an optimized conversion matrix between all adjacent frames in the sequence segment;

[0134] S42. transforming the point clouds of each frame in the segment into the coordinate system of the first frame to form a single-frame mapping result;

[0135] S43. Complete multi-map merging based on OPENGL mapping.

[0136] Step S43 completes the merging of multiple maps based on OPENGL mapping, and the specific merging steps are as follows:

[0137] S431. In OPENGL, the mapping results of each fragment are loaded into multiple layers. The coordinate systems of each layer are inconsistent, so they need to be converted to the same coordinate system. For this purpose, a layer needs to be selected as a reference layer, which is recorded as layer0;

[0138] S432. Use the perspective projection matrix and vertex shader in OPENGL, with the mouse button and drag, to achieve the rotation and translation of the current layer until it matches the layer0 part of the point cloud to indicate the registration, and record the starting coordinates M1 (m1, n1) and the ending coordinates M2 (m2, n2) of the mouse movement in the software window coordinate system; the cosine theorem can be used to calculate the rotation angle θ, and its corresponding rotation vector (rotation around the Z axis) The corresponding rotation matrix R can be obtained from the Rodriguez formula;

[0139] S433. The standardized device coordinate interval x, y∈[-1,1] agreed by OPENGL, assuming the software window size is w s ·h s After mapping the window to the OPENGL coordinate system, the horizontal and vertical scaling factors are w and s / 2,h s / 2; the translation vector in the known window coordinate system is According to the horizontal and vertical scaling factors, the translation vector in OPENGL is Because the positions of M1 and M2 are derived from the point cloud position selection on the screen, and the point cloud position displayed on the screen is obtained through OPENGL perspective projection, Inverse perspective projection is also required to obtain the true normalized translation vectors of M1 and M2 generated in OPENGL. Since the current perspective projection matrix (generally denoted as MVP) can be determined and provided by the OPENGL camera position and target position, it is considered a known quantity, and we have:

[0140]

[0141] Therefore, in the OPENGL coordinate system, we get the transformation matrix T caused by dragging the mouse gl It can be written as

[0142]

[0143] S434. Set the current layer through T gl Transform to layer0 to complete the merging of the current layer into the reference layer;

[0144] S435. Implement steps S432 to S434 for all layers to obtain the final merged complete map.

[0145] In this embodiment, a mechanical scanning laser radar of the RoboSense-Lidar-16 model is used to scan the corridors, stairwells, and elevators in the office area and complete the mapping according to steps S1 to S4.

[0146] Attached Figure 2 This is a flowchart of the ground information filtering described in step S1. The preferred parameters can be set according to steps S11 to S17 in the specification to complete the processing process. Specifically, in step S12, the angle resolution Δθ=π / 180 is set, which can form Angle interval; in step S14, the pitch angle difference threshold β1 between two adjacent points is 8°, and the pitch angle difference threshold β2 between the current point and point O′ is 5°.

[0147] Attached Figure 3 , Attachment Figure 4 These are the point cloud images before and after filtering out the ground and roof information.

[0148] Attached Figure 5 This is a flowchart of the point cloud sequence fragmentation based on VFH comparison in step S2. The processing process can be completed according to steps S21 to S23 in the specification, wherein in step S23, a threshold k0=12 is set, and the number of sequence fragments q=2 can be obtained.

[0149] The point cloud motion state estimation based on multi-layer direct method optimization in step S3 specifically includes:

[0150] S31. For the adjacent previous frame point cloud f1 and next frame point cloud f2 in the sequence segment, use the normal distribution transformation (NDT) to align from f2 to f1, that is, transform f2 to the coordinate system where f1 is located, and obtain the initial matrix rlt of the inter-frame conversion;

[0151] S32. Point cloud projection to establish brightness image;

[0152] S33. Nonlinear optimization based on multi-level direct method.

[0153] Attached Figure 6 This is a nonlinear optimization flow chart based on the multi-layer direct method described in step S33. Specifically in this embodiment, the nonlinear optimization process can be completed according to steps S331 to S336 in the specification.

[0154] The specific process of the map creation and segment merging in step S4 includes:

[0155] S41. According to step S3, a sliding window method is used to obtain an optimized conversion matrix between all adjacent frames in the sequence segment;

[0156] S42. transforming the point clouds of each frame in the segment into the coordinate system of the first frame to form a single-frame mapping result;

[0157] S43. Complete multi-map merging based on OPENGL mapping.

[0158] Attached Figure 7 The two point cloud sequence segments formed in this embodiment are executed after step S41 to obtain the two frames of mapping results. In OPENGL, a vertical view angle is used, and the camera is located at point (0,0,2). Figure 7 It can be seen that there should be overlapping parts in the two frames of map, but because the coordinate system of the first frame point cloud is inconsistent when building the map, there is a certain rotation and translation between the two frames. After merging in step S43, the attached Figure 8 The final complete map shown.

[0159] The beneficial effects of the above embodiments include:

[0160] (1) No additional sensor equipment is required, and only LiDAR is needed to efficiently and stably complete the mapping process, significantly reducing system complexity and cost;

[0161] (2) No loop detection is required, which avoids the situation where the mobile platform repeatedly scans the same path in a compact indoor environment in order to eliminate accumulated errors through loop detection, improves mapping efficiency, and reduces unnecessary energy consumption;

[0162] (3) Direct nonlinear optimization can avoid complex feature calculation and matching processes, maintaining high-precision mapping while taking into account good real-time performance;

[0163] (4) Using continuous frames instead of key frames to maximize the capture of indoor environment details, including subtle structural features and texture changes, provides more accurate data support for subsequent navigation and positioning;

[0164] (5) It meets the needs of low-configuration and lightweight map construction in compact indoor spaces, and provides strong technical support for the widespread application of indoor service robots.

[0165] The above is a specific description of the preferred implementation of the present invention, but the invention is not limited to the embodiments. Those skilled in the art may make equivalent modifications or substitutions without violating the spirit of the present invention. These equivalent modifications and substitutions are all included in the scope defined by the claims of this application.< / blocksolvertype::posematrixtype> < / blocksolvertype::posematrixtype>

Claims

1. A laser point cloud indoor mapping and merging method based on multi-layer direct optimization, characterized in that: The following steps are involved: S1. Ground information filtering; S2. Point cloud sequence fragmentation based on VFH alignment; S3. Point cloud motion state estimation based on multi-layer direct method optimization; S4. Map building and segment merging.

2. The method for indoor mapping and merging of laser point clouds based on multi-layer direct optimization according to claim 1, characterized in that: The ground information is filtered out in step S1, and the specific process includes: S11. Establish a spatial rectangular coordinate system O-XYZ with the center of the laser radar as the coordinate origin O, where the XOY plane is parallel to the motion platform where the laser radar is located, and the Z axis is perpendicular to the XOY plane; filter out point clouds with a distance less than 0.5m from O to prevent interference with mapping caused by the motion platform; S12. For all points in the point cloud of the current frame, calculate their azimuth angle α in the XOY plane relative to the positive direction of the y-axis one by one. If α<0, then α=α+2π. Set the angle resolution Δθ, then all point clouds will fall into the 2π / Δθ angle interval centered at O, that is, the point cloud is clustered according to α. For the cluster with index number i, it is recorded as cluster[i]; S13. For each point in cluster[i], calculate its distance r relative to O in the XOY plane, and sort them from small to large according to r. The sorted cluster is recorded as cluster[i]′; S14. Measure the height b of the laser radar from the ground, project O to the ground, and obtain point O′(0, 0, -b); set two thresholds: one is the pitch angle difference β1 between two adjacent points in cluster[i]′, and the other is the pitch angle difference β2 between the current point and point O′; S15. For any point C in cluster[i]′, calculate the pitch angle between it and the previous point C0 (If C is the first point in cluster[i]′, calculate the pitch angle relative to O′), and the pitch angle β relative to O′ CO′ ;Compare With β1, there are two possibilities: S151. If C0 is determined to be a ground point, then C is still determined to be a ground point; if C0 is a non-ground point but satisfies |β CO ′|<|β2|, then C is also determined to be a ground point; S152. Set the distance threshold d. If the distance between C and C0 is d c >d, then the two are far apart, if |β CO ′|<|β2|, then C is determined to be a ground point; S16. If the scene to be mapped is indoors, then cluster[i]′ contains both ground and roof point cloud distributions, and both can still be filtered out according to step S15; S17. Traverse all clusters to obtain a single-frame non-ground point cloud; for continuous multi-frame point clouds obtained or recorded in real time, a continuous non-ground point cloud sequence can be obtained according to steps S11 to S16, which is recorded as Cloud_rec.

3. The method for indoor mapping and merging of laser point clouds based on multi-layer direct optimization according to claim 1, characterized in that: The specific process of the point cloud sequence fragmentation based on VFH comparison in step S2 is as follows: S21. Define the viewpoint V(0, 0, h) in the coordinate system O-XYZ, h and the maximum value z of the point cloud in the positive direction of the Z axis in Cloud_rec max The relationship is: h = z max +10; transform the point cloud coordinate system in Cloud_rec to the coordinate system V-XYZ with the observation point V as the origin. In this coordinate system, the coordinates of point V are (0, 0, 0), and the coordinates of any point in Cloud_rec are (x i ,y i , z i ) is converted to (x i ,y i , z i -h), the converted point cloud sequence is recorded as Cloud_rec V ; S22. For Cloud_rec V For each frame of point cloud in , the viewpoint feature histogram (VFH) is calculated with viewpoint V(0, 0, 0) to obtain a histogram array containing 308 elements; S23. Use the knnSearch function in the flann function library to calculate the error k between the histograms of two adjacent frames. If k is greater than the set threshold k0, it is considered that the features between the two frames are quite different, and Cloud_rec V Truncate here; thus, q point cloud sequence segments can be formed.

4. The method for indoor mapping and merging of laser point clouds based on multi-layer direct optimization according to claim 1, characterized in that: The specific process of the point cloud motion state estimation based on multi-layer direct method optimization in step S3 includes: S31. For the adjacent previous frame point cloud f1 and next frame point cloud f2 in the sequence segment, the normal distribution variation NDT is used to align from f2 to f1 to obtain the inter-frame conversion initial matrix rlt; S32. Point cloud projection to establish brightness image; S33. Nonlinear optimization based on multi-level direct method.

5. The method for indoor mapping and merging of laser point clouds based on multi-layer direct optimization according to claim 4, characterized in that: The point cloud projection in step S32 establishes a brightness image, and the specific process includes: S321. Project all points of f1 onto the plane z=-1: According to the similar triangle feature, for the point with index v in f1 (x v ,y v , z v ), the projected coordinates The obtained planarized two-dimensional point cloud is denoted as f 1_2D , calculate the maximum value of its coordinates in the X and Y directions (x′ max , y′ max ), minimum value (x′ min , y′ min ); S322. 1_2D Voxelization is performed, with a voxel width of dd and a voxel brightness of the average reflection intensity of each point in the grid. A two-dimensional brightness image is obtained, denoted as IMG1, with a size of w·h, where Then any point (x v ,y v , z v ) in its projected IMG1 corresponds to the pixel coordinates (u v , v v )for: In the formula, floor is the rounding operation; S323. For point cloud f2, according to steps S321 to S322, a brightness image IMG can be obtained. 2。 6. The method for indoor mapping and merging of laser point clouds based on multi-layer direct optimization according to claim 4, characterized in that: The nonlinear optimization based on the multi-layer direct method in step S33 specifically includes the following steps: S331. For any point b2 (x2, y2, z2) in f2, its corresponding brightness in IMG2 is recorded as I2(b2), the estimated point coordinates of b2 in f1 are b1=T·b2, and the brightness in IMG1 is I1(b1); wherein T is a variable, representing the pose transformation matrix to be optimized, and its initial value is rlt described in step 31; then according to the photometric consistency assumption of the direct method, the error function e(T) for performing nonlinear optimization can be determined as: e(T)=I2(b2)-I1(T·b2) (2) S332. To minimize the error function, it is necessary to calculate the derivative of the error with respect to the posture T. According to the Lie algebra derivation and the perturbation model, it can be converted into the derivation of the perturbation amount ξ of T, that is: in: m1(u, v) is the pixel point corresponding to b1 after being projected onto I1. Represents the brightness gradient of image I1 at pixel m1(u, v), we have: According to the projection relationship described in formula (1), assuming the coordinates of point b1 are (x, y, z), we have: According to the Lie algebra perturbation model, we have: Where b1^ represents the antisymmetric matrix corresponding to b1, and I is the unit matrix; S333. Multiplying equations (4), (5), and (6) together, we can obtain the result of equation (3), which is the form of the Jacobian matrix J of the error for the Lie algebra: S334. Based on the G2O optimization library, the vertex type is BaseVertex<6, Sophus::SE3d>, the edge type is BaseUnaryEdge<1, double, VertexPose>, the Jacobian matrix J is set according to formula (7) in the linearizeOplus() function, the error e(T) is set according to formula (2) in the computeError() function, the initial value of T is specified as rlt, and an appropriate solver is selected to solve the optimization problem, and the optimized transformation matrix rlt′ is obtained; S335. Use multi-layer scaling to build an image pyramid; use the rlt′ obtained in S331 to S334 as the initial value of the lower layer optimization in the upper layer image, and optimize again to improve the accuracy of rlt′ layer by layer to obtain the multi-layer optimization result rlt′ m ; S336. Use the pcl::transformPointCloud function in the PCL library to transform f2 through rlt′ m Transform to the coordinate system of f1 to complete the mapping of adjacent frames.

7. The method for indoor mapping and merging of laser point clouds based on multi-layer direct optimization according to claim 1, characterized in that: The specific process of the map creation and segment merging in step S4 includes: S41. According to step S3, a sliding window method is used to obtain an optimized conversion matrix between any adjacent frames in the sequence segment; S42. transforming the point clouds of each frame in the segment into the coordinate system of the first frame to form a single-frame mapping result; S43. Complete multi-map merging based on OPENGL mapping.

8. The method for indoor mapping and merging of laser point clouds based on multi-layer direct optimization according to claim 7, characterized in that: Step S43 completes the merging of multiple maps based on OPENGL mapping, and the specific merging steps are as follows: S431. In OPENGL, the mapping results formed by each fragment are loaded into multiple layers. The coordinate systems of each layer are inconsistent, so they need to be converted to the same coordinate system. For this purpose, a layer needs to be selected as a reference layer, which is recorded as layer0; S432. Use the perspective projection matrix and vertex shader in OPENGL, with the mouse button and drag, to achieve the rotation and translation of the current layer until it matches the layer0 part of the point cloud to indicate the registration, and record the starting coordinates M1 (m1, n1) and the ending coordinates M2 (m2, n2) of the mouse movement in the software window coordinate system; the cosine theorem can be used to calculate the rotation angle θ, and its corresponding rotation vector (rotation around the Z axis) The corresponding rotation matrix R can be obtained from the Rodriguez formula; S433. The standardized device coordinate interval x, y∈[-1,1] agreed by OPENGL, assuming the software window size is w s ·h s After mapping the window to the OPENGL coordinate system, the horizontal and vertical scaling factors are w and s / 2,h s / 2; the translation vector in the known window coordinate system is According to the horizontal and vertical scaling factors, the translation vector in OPENGL is Because the positions of M1 and M2 are derived from the point cloud position selection on the screen, and the point cloud position displayed on the screen is obtained through OPENGL perspective projection, Inverse perspective projection is also required to obtain the true normalized translation vectors of M1 and M2 generated in OPENGL. Since the current perspective projection matrix (generally denoted as MVP) can be determined and provided by the OPENGL camera position and target position, it is considered a known quantity, and we have: Therefore, in the OPENGL coordinate system, we get the transformation matrix T caused by dragging the mouse gl It can be written as S434. Set the current layer through T gl Transform to layer0 to complete the merging of the current layer into the reference layer; S435. Implement steps S432 to S434 for all layers to obtain the final merged complete map.

Images