A method for indoor mapping and merging of laser point clouds based on multi-layer direct optimization

Abstract

The present invention relates to a method for indoor mapping and merging of laser point clouds based on multi-layer direct optimization, and belongs to the field of robot vision technology. The method mainly comprises the following steps: filtering out ground information; fragmenting point cloud sequences based on VFH comparison; estimating point cloud motion states based on a multi-layer direct method; and map building and fragment merging. This method does not rely on additional sensors. First, by filtering out invalid ground point cloud information, the quality of point cloud data is effectively improved. Then, based on the VFH comparison algorithm, the long point cloud sequence is divided into multiple segments to intermittently return the accumulated drift 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 correct the state estimation value based on the pixel brightness information between frames, thereby avoiding the complex point cloud local feature extraction and matching process, greatly reducing the complexity of the algorithm, and showing good real-time performance while maintaining high-precision mapping.

Description

Technical Field

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

[0002] Laser point cloud mapping is a 3D data representation widely used in fields such as robotic navigation, 3D reconstruction, and environmental perception. Accurately estimating inter-frame motion states is crucial for environmental modeling, real-time analysis, and accurate localization. Typically, point cloud pose estimation can be calculated using real-time data provided by sensors on a motion platform, such as an inertial measurement unit (IMU), global positioning system (GPS), and odometry. However, the introduction of additional sensors also presents a series of challenges: first, they increase system complexity and cost, making them uneconomical for many fixed, single-use scenarios. Second, data fusion and calibration between different sensors is a complex and error-prone process, potentially introducing new error sources. Furthermore, data processing and analysis from additional sensors require additional algorithms, further increasing the computational burden of the mapping system. These challenges pose significant challenges to algorithm development and parallel optimization in research efforts.

[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 often follow the research paradigm of: first, using scan matching algorithms such as CSM and ICP to perform preliminary point cloud registration to obtain the initial pose transformation; then, constructing a cost function based on the distance error between the predicted points and the actual points, and then performing nonlinear optimization. However, this idea still has several areas to be improved in indoor environments: (1) When processing indoor environments, algorithms such as CSM and ICP are not suitable for excessive downsampling due to the rich details of the point cloud. This results in a significant increase in the corresponding relationship search space of the CSM algorithm and a sharp increase in computational complexity; although the ICP algorithm has improved efficiency compared to CSM, it is more difficult to accurately match corresponding points in the case of complex indoor geometric features, which prolongs the algorithm's running time. (2) When performing nonlinear optimization, the indoor environment has rich texture, structure, and color changes. If only relying on the distance information between corresponding points for optimization, 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 and ceiling. From the perspective of spatial distribution, 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 errors in the direction of more critical vertical feature points. Summary of the Invention

[0004] In order to solve some of the 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 instead divides a long point cloud sequence into multiple segments, which can discontinuously return the accumulated error 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 performance of mapping.

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

[0006] A method for indoor mapping and merging of laser point clouds based on multi-layer direct optimization, comprising 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 construction 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 lidar as the coordinate origin O, where the XOY plane is parallel to the motion platform where the lidar 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 current frame point cloud, calculate the azimuth angle α of their projection on the XOY plane relative to the positive direction of the Y axis. If α < 0, then α = α + 2π. Set the angular resolution Δθ so that all point clouds fall into the 2π / Δθ angle interval centered at O. This means that the point clouds are clustered according to α. The cluster with index i is denoted 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 and project O onto the ground to 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 sort If C is the first point in cluster[i]′, calculate the pitch angle relative to O′ and the pitch angle β relative to 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 has been 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 mapping scene is indoors, then cluster[i]′ contains both ground and roof point cloud distributions. Step S15 can still be followed to filter out both.

[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 alignment in step S2 is specifically performed as follows:

[0025] S21. Define the viewpoint V(0,0,h) in the coordinate system O-XYZ, where h is equal to the maximum value z of the point cloud in the positive Z direction 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 , calculate the view feature histogram (VFH) of each frame with viewpoint V(0,0,0) and obtain a histogram array containing 308 elements;

[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 significantly different, and Cloud_rec V Truncated here; thus, q point cloud sequence segments can be formed. As a preference, k0 should not be too low, otherwise the q value will be too large, increasing the number of merging steps. Generally speaking, q≤5.

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

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

[0030] S32. Projecting the point cloud to create a brightness image;

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

[0032] Furthermore, the point cloud projection in step S32 is used to create 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 Perform voxelization with voxel width dd and voxel brightness as the average reflection intensity of each point in the grid. A two-dimensional brightness image will be obtained, denoted as IMG1, with size w·h, where Then any point (x v ,y v ,z v ) The corresponding pixel coordinates in its projected IMG1 (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] Furthermore, 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 derivative 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), which is:

[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) yields the result of equation (5), which is the form of the Jacobian matrix J of the error with respect to 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. 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 the problem, and the maximum number of iterations is set to 40. 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 preference, set the number of layers to 4, and the scaling coefficients from top to bottom to 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 for the lower layer optimization, and optimize again to improve the accuracy of rlt′ layer by layer, and 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 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. Transform 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] Furthermore, in step S43, the multi-map merging is completed based on the OPENGL mapping, and the specific merging steps are as follows:

[0062] S431. In OpenGL, the mapping results of each fragment are loaded as multiple layers. The coordinate systems of the layers are inconsistent, so they need to be converted to the same coordinate system. To do this, a layer is selected as the reference layer, denoted as layer0.

[0063] S432. Using the perspective projection matrix and vertex shader in OpenGL, along with mouse buttons and dragging, rotate and translate the current layer until it matches the point cloud of layer 0 to indicate registration. Record the starting coordinates M1 (m1, n1) and ending coordinates M2 (m2, n2) of the mouse movement in the software window coordinate system. Use the law of cosines 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.OPENGL agreed standardized device coordinate interval x, y∈[-1,1], 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 s / 2,h s / 2; the translation vector in the known window coordinate system is According to the horizontal and vertical scaling coefficients, 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 (usually denoted as MVP) can be determined and provided by the OPENGL camera position and target position, it is considered a known quantity, then:

[0065]

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

[0067]

[0068] S434. The current layer is passed 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 method for indoor mapping and merging of laser point clouds based on multi-layer direct optimization. This method does not rely on additional sensors and loop detection. By filtering out ground and roof information from the received three-dimensional point cloud, it effectively reduces noise interference and improves the quality of mapping. 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, ensuring the accuracy and stability of the mapping. When performing NDT alignment 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 alignment-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 following briefly introduces the drawings required for use in the embodiments. 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 any creative work.

[0072] Figure 1 It is a flowchart of the overall implementation of the present invention;

[0073] Figure 2 This is a flow chart of ground information filtering in 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 This 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 the 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.

[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 examples. It will be understood that the specific embodiments described herein are intended only to explain the relevant inventions and are not intended to limit the inventions. It should also be noted that, for ease of description, only portions relevant to the relevant inventions are shown in the accompanying drawings. It should be noted that, unless there is a conflict, the embodiments and features in the embodiments of this application may be combined with one another. The present application will be described in detail below with reference to the accompanying drawings and in conjunction with the examples.

[0081] As attached Figure 1 As shown in FIG, a 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 construction and segment merging.

[0086] The ground information filtering in step S1 specifically includes the following steps:

[0087] S11. Establish a spatial rectangular coordinate system O-XYZ with the center of the lidar as the coordinate origin O, where the XOY plane is parallel to the motion platform where the lidar 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 current frame point cloud, calculate the azimuth angle α of their projection on the XOY plane relative to the positive direction of the Y axis. If α < 0, then α = α + 2π. Set the angular resolution Δθ so that all point clouds fall into the 2π / Δθ angle interval centered at O. This means that the point clouds are clustered according to α. The cluster with index i is denoted 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 and project O onto the ground to 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 sort If C is the first point in cluster[i]′, calculate the pitch angle relative to O′ and the pitch angle β relative to 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 has been 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 mapping scene is indoors, then cluster[i]′ contains both ground and roof point cloud distributions. Step S15 can still be followed to filter out both.

[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 fragmenting the point cloud sequence based on VFH alignment in step S2 is as follows:

[0100] S21. Define the viewpoint V(0,0,h) in the coordinate system O-XYZ, where h is equal to the maximum value z of the point cloud in the positive Z direction 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 , calculate the view feature histogram (VFH) of each frame with viewpoint V(0,0,0) and obtain a histogram array containing 308 elements;

[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 significantly different, and Cloud_rec V Truncated here; thus, q point cloud sequence segments can be formed. As a preference, k0 should not be too low, otherwise the q value will be too large, increasing the number of merging steps. Generally speaking, q ≤ 5.

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

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

[0105] S32. Projecting the point cloud to create a brightness image;

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

[0107] Step S32, in which the point cloud is projected to create 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 Perform voxelization, with voxel width dd and voxel brightness as the average reflection intensity of each point in the grid, to obtain a two-dimensional brightness image, denoted as IMG1, with size w·h, where Then any point (x v ,y v ,z v ) The corresponding pixel coordinates in its projected IMG1 (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 derivative 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), which is:

[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) yields the result of equation (5), which is the form of the Jacobian matrix J of the error with respect to 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. 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 the problem, and the maximum number of iterations is set to 40. 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 preference, set the number of layers to 4, and the scaling coefficients from top to bottom to 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 for the lower layer optimization, and optimize again to improve the accuracy of rlt′ layer by layer, and 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 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. Transform 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 as multiple layers. The coordinate systems of the layers are inconsistent, so they need to be converted to the same coordinate system. To do this, a layer is selected as the reference layer, denoted as layer0.

[0138] S432. Using the perspective projection matrix and vertex shader in OpenGL, along with mouse buttons and dragging, rotate and translate the current layer until it matches the point cloud of layer 0 to indicate registration. Record the starting coordinates M1 (m1, n1) and ending coordinates M2 (m2, n2) of the mouse movement in the software window coordinate system. Use the law of cosines 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.OPENGL agreed standardized device coordinate interval x, y∈[-1,1], 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 s / 2,h s / 2; the translation vector in the known window coordinate system is According to the horizontal and vertical scaling coefficients, 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 (usually denoted as MVP) can be determined and provided by the OPENGL camera position and target position, it is considered a known quantity, then:

[0140]

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

[0142]

[0143] S434. The current layer is passed 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 RoboSense-Lidar-16 mechanical scanning laser radar is used to scan the corridors, stairwells, and elevators in the office area and complete the mapping according to steps S1 to S4.

[0146] Attachment 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 manual to complete the processing process. Specifically, in step S12, the angle resolution Δθ is set to π / 180, which can form Angle interval; in step S14, the pitch angle difference threshold value β1 between two adjacent points is 8°, and the pitch angle difference threshold value β2 between the current point and point O′ is 5°.

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

[0148] Attachment 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. In step S23, the threshold k0 is set to 12, and the number of sequence fragments q = 2 can be obtained.

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

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

[0151] S32. Projecting the point cloud to create a brightness image;

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

[0153] Attachment 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 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. Transform 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] Attachment Figure 7 The two frames of mapping results obtained after executing step S41 for the two point cloud sequence segments formed in this embodiment are a vertical view in OPENGL, with the camera 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; only LiDAR is needed to complete the mapping process efficiently and stably, 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 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 also taking into account good real-time performance;

[0163] (4) Using continuous frames instead of key frames, the system can maximize the capture of indoor environment details, including subtle structural features and texture changes, providing 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 method for indoor mapping and merging of laser point clouds 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; The ground information filtering in step S1 specifically includes the following steps: S11. Establish a spatial rectangular coordinate system O-XYZ with the center of the lidar as the coordinate origin O, where the XOY plane is parallel to the motion platform where the lidar 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 current frame point cloud, 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 angular resolution Δθ, and all point clouds will fall into the 2π / Δθ angle interval centered at O. This means that the point clouds are clustered according to α. The cluster with index number i 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 onto 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′ Compare With β1, there are two cases: S151, If C0 has been 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 mapping scene is indoors, then cluster[i]′ contains both ground and roof point cloud distributions. In step S15, both are filtered out. S17. Traverse all clusters to obtain a single-frame non-ground point cloud. For continuous multi-frame point clouds acquired or recorded in real time, follow steps S11 to S16 to obtain a continuous non-ground point cloud sequence, recorded as Cloud_rec. The specific process of the point cloud motion state estimation based on the multi-layer direct method optimization in step S3 includes: S31, for the adjacent previous frame point cloud f1 and the next frame point cloud f2 in the sequence segment, use the normal distribution change NDT to align from f2 to f1, and 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.

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 specific process of fragmenting the point cloud sequence based on VFH alignment in step S2 is as follows: S21. Define the viewpoint V(0,0,h) in the coordinate system O-XYZ, where h is equal to 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, the viewpoint feature histogram VFH is calculated with viewpoint V(0,0,0), and a histogram array containing 308 elements is obtained; 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 are formed.

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 point cloud projection in step S32 is used to create 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 number 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, f 1_2D Perform voxelization with voxel width dd and voxel brightness as the average reflection intensity of each point in the grid. A two-dimensional brightness image will be obtained, denoted as IMG1, with size w·h, where Then any point (x v ,y v ,z v ) The corresponding pixel coordinates (u v ,v v )for: In the formula, floor is the rounding operation; S323 . For the point cloud f2 , follow steps S321 to S322 to obtain a brightness image IMG2 .

4. The method for indoor mapping and merging of laser point clouds based on multi-layer direct optimization according to claim 3, 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); 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) of performing nonlinear optimization is 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 is converted into the derivative 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), which is: According to the projection relationship of formula (1), let the coordinates of point b1 be (x, y, z), then: According to the Lie algebra perturbation model, we have: where b1 ∧ represents the antisymmetric matrix corresponding to b1, i is the unit matrix; S333. Multiply (4), (5), and (6) to obtain the result of (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, that is, 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 in the upper image 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 ; S336, using the pcl::transformPointCloud function in the PCL library, transform f2 through rlt′ m Transform to the coordinate system of f1 to complete the mapping of adjacent frames.

5. 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 map creation and segment merging in step S4 includes: S41, according to step S3, using a sliding window method to obtain an optimized transformation 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.

6. The method for indoor mapping and merging of laser point clouds based on multi-layer direct optimization according to claim 5, 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 of each fragment are loaded into multiple layers. The coordinate systems of the layers are inconsistent, so they need to be converted to the same coordinate system. To this end, a layer needs to be selected as the 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 rotate and translate the current layer until it matches the point cloud of layer0 to indicate registration. Record the starting coordinates M1 (m1, n1) and ending coordinates M2 (m2, n2) of the mouse movement in the software window coordinate system; use the cosine theorem to calculate the rotation angle θ, and its corresponding rotation vector, rotate around the Z axis. The corresponding rotation matrix r is obtained by the Rodriguez formula; S433, OPENGL agreed standardized device coordinate interval x, y∈[-1,1], let the software window size be w s ·h s After mapping the window to the OPENGL coordinate system, the horizontal and vertical scaling factors are w s / 2,h s / 2; the translation vector in the known window coordinate system is According to the horizontal and vertical scaling coefficients, 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 is ​​recorded as MVP and is determined and provided by the OPENGL camera position and target position, it is considered a known quantity, so: Therefore, in the OPENGL coordinate system, we get the transformation matrix T caused by dragging the mouse gl Written as S434, 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