Point cloud map construction method, device and system, engineering machinery and program product

Abstract

The invention relates to a point cloud map construction method, device and system, engineering machinery and a program product. The method comprises the following steps: acquiring IMU data and laser radar point cloud data; performing forward propagation integration on the IMU data, performing timestamp accumulation on the laser radar point cloud data, and generating an IMU data integration result and a point cloud data sequence which are aligned in time sequence; correcting the distortion of the point cloud data according to the back propagation of the IMU data integration result, extracting geometric features from the distortion-removed point cloud data, calculating the point-surface residual error, and constructing a state estimation constraint; aiming at a map matching failure scene, optimizing the contour residual error; fusing multiple types of residual errors to construct a residual error set; and iteratively correcting the system state through a first predetermined algorithm, and synchronously generating an odometer and a map of the underground roadway. According to the method, the IMU and laser radar data can be cooperatively processed in stages, and the forward and back propagation algorithms and multi-residual joint optimization are combined, so that high-precision state estimation and map construction are realized.

Description

Technical Field

[0001] The present disclosure relates to the field of intelligent unmanned engineering machinery and robot positioning, and in particular to a point cloud map construction method, device and system, engineering machinery and program product. Background Art

[0002] Intelligent, unmanned transportation is crucial for improving the safety and efficiency of underground tunnel transportation. Simultaneous Localization and Mapping (SLAM) technology also enables mobile robots to locate themselves in real time in unknown environments. Summary of the Invention

[0003] Through research, the inventors found that the harsh production environment in underground tunnels, complex terrain, narrow tunnels, environmental degradation, and gradual disappearance or instability of features increase the difficulty of applying SLAM technology underground.

[0004] In view of at least one of the above technical problems, the present disclosure provides a point cloud map construction method, device and system, engineering machinery and program product, which realize high-precision state estimation and map construction by coordinating IMU (Inertial Measurement Unit) and lidar data in stages, combining forward and back propagation algorithms and multi-residual joint optimization.

[0005] According to one aspect of the present disclosure, a point cloud map construction method is provided, comprising:

[0006] Obtain inertial measurement unit (IMU) data and lidar point cloud data;

[0007] Perform forward propagation integration on the IMU data, accumulate timestamps on the lidar point cloud data, and generate time-aligned IMU data integration results and point cloud data sequences;

[0008] Correct point cloud data distortion based on backpropagation of IMU data integration results, extract geometric features from the dedistorted point cloud data, calculate point-surface residuals, and construct state estimation constraints; optimize contour residuals for map matching failure scenarios; and fuse multiple types of residuals to construct a residual set.

[0009] The system state is iteratively corrected through a first predetermined algorithm, and an odometer and a map of the underground tunnel are synchronously generated.

[0010] In some embodiments of the present disclosure, for map matching failure scenarios, optimizing the contour residual includes:

[0011] For map matching failure scenarios, geometric observability analysis and information entropy evaluation are integrated to determine whether there is degradation;

[0012] In the case of degradation, the contour residual is optimized.

[0013] In some embodiments of the present disclosure, fusing geometric observability analysis with information entropy evaluation to determine whether degradation occurs includes:

[0014] Linearize the residual equation at the predicted optimal value and perform eigenvalue decomposition on the generated information matrix;

[0015] When the minimum eigenvalue is less than a predetermined threshold, the minimum eigenvalue is directly associated with a system degradation sensitive direction.

[0016] In some embodiments of the present disclosure, directly associating the minimum eigenvalue with a system degradation sensitive direction includes:

[0017] In the underground well working scenario, it is determined that there is no point cloud data along the longitudinal direction of the corridor;

[0018] In the case of point-to-plane registration, it is determined that the obtained plane normal vector disappears in the longitudinal direction, resulting in longitudinal degradation.

[0019] In some embodiments of the present disclosure, the IMU data includes acceleration and angular velocity data of the IMU.

[0020] In some embodiments of the present disclosure, performing a forward propagation integration on the IMU data includes:

[0021] Continuously integrate the IMU acceleration and angular velocity data at a predetermined frequency;

[0022] The optimized odometer is used as the starting position, and an IMU data integration result is output, wherein the IMU data integration result includes at least one of the integrated position, velocity, and attitude angle parameters.

[0023] In some embodiments of the present disclosure, the lidar point cloud data is lidar single-frame point cloud data.

[0024] In some embodiments of the present disclosure, accumulating timestamps on lidar point cloud data to generate a time-aligned IMU data integration result and point cloud data sequence includes:

[0025] Based on the timestamp, the lidar single-frame point cloud data and the IMU integration results are synchronized in time and space;

[0026] A sliding window algorithm is used to accumulate multi-frame point cloud data to construct a globally consistent point cloud data sequence.

[0027] In some embodiments of the present disclosure, correcting point cloud data distortion based on backpropagation of IMU data integration results includes:

[0028] Based on the IMU integration results, reversely calculate the sensor motion trajectory at each moment in the lidar scanning cycle;

[0029] fitting a continuous motion model using a predetermined interpolation algorithm;

[0030] An inverse transformation is applied point by point to the accumulated point cloud data to eliminate the non-rigid distortion caused by the high-speed motion of the sensor and generate dedistorted point cloud data.

[0031] In some embodiments of the present disclosure, extracting geometric features from dedistorted point cloud data and calculating point-surface residuals, and constructing state estimation constraints include:

[0032] Extract local planar features from dedistorted point cloud data;

[0033] Using a second predetermined algorithm to accelerate the nearest neighbor search, calculating the Euclidean distance from each point to the corresponding plane as a residual;

[0034] Outliers are eliminated through a third predetermined algorithm to generate a weighted point-surface residual set as an observation constraint for state estimation.

[0035] In some embodiments of the present disclosure, optimizing the contour residual includes:

[0036] Based on the point-surface residuals, the environmental line features and contour features are extracted;

[0037] The fourth predetermined algorithm is used to compare the current frame contour with the map contour features, calculate the line matching residual and the contour matching residual, and expand the geometric constraint dimension of the optimization problem.

[0038] In some embodiments of the present disclosure, fusing multiple types of residuals to construct a residual set includes:

[0039] Dynamically adjust optimization weights when degradation of environmental characteristics is detected;

[0040] The point-surface residuals, line residuals, contour residuals and IMU pre-integration residuals are combined into a multi-source residual set.

[0041] In some embodiments of the present disclosure, iteratively correcting the system state through a first predetermined algorithm to synchronously generate an odometer and a map of the underground tunnel includes:

[0042] Constructing a tightly coupled nonlinear least squares problem, and iteratively solving the optimal state estimate using a first predetermined algorithm;

[0043] Use the optimized odometer as the starting pose for the next IMU forward propagation;

[0044] Based on the optimized pose, the dedistorted point cloud data is projected into the global coordinate system, and the fifth predetermined algorithm is used to construct a dense map.

[0045] According to another aspect of the present disclosure, there is provided a point cloud map construction device, comprising:

[0046] A data acquisition module is configured to acquire inertial measurement unit (IMU) data and lidar point cloud data;

[0047] A data processing module is configured to perform forward propagation integration on the IMU data, accumulate timestamps on the lidar point cloud data, and generate a time-aligned IMU data integration result and point cloud data sequence;

[0048] The state estimation module is configured to correct point cloud data distortion based on backpropagation of the IMU data integration results, extract geometric features from the dedistorted point cloud data, calculate point-surface residuals, and construct state estimation constraints; optimize contour residuals for map matching failure scenarios; and fuse multiple types of residuals to construct a residual set;

[0049] The joint optimization module is configured to iteratively correct the system state through a first predetermined algorithm and synchronously generate an odometer and a map of the underground tunnel.

[0050] According to another aspect of the present disclosure, there is provided a point cloud map construction device, comprising:

[0051] Memory; and

[0052] A processor coupled to the memory, wherein the processor is configured to execute the point cloud map construction method as described in any of the above embodiments based on instructions stored in the memory.

[0053] According to another aspect of the present disclosure, a point cloud map construction system is provided, comprising a data acquisition device and a point cloud map construction device as described in any one of the above embodiments.

[0054] According to another aspect of the present disclosure, there is provided an engineering machine comprising a point cloud map construction system as described in any one of the above embodiments.

[0055] According to another aspect of the present disclosure, a computer-readable storage medium is provided, wherein the computer-readable storage medium stores computer instructions, and when the instructions are executed by a processor, the point cloud map construction method as described in any of the above embodiments is implemented.

[0056] According to another aspect of the present disclosure, a computer program product is provided, including a computer program, wherein when the computer program is executed by a processor, the point cloud map construction method as described in any of the above embodiments is implemented.

[0057] The present disclosure can achieve high-precision state estimation and map construction by co-processing IMU and lidar data in stages, combining forward and back propagation algorithms and multi-residual joint optimization. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0059] Figure 1 Schematic diagram of some embodiments of the point cloud map construction method disclosed in the present invention.

[0060] Figure 2 Schematic diagrams of other embodiments of the point cloud map construction method disclosed in the present invention.

[0061] Figure 3 Schematic diagrams of some embodiments of the point cloud map construction device disclosed herein.

[0062] Figure 4 Schematic diagrams of the structures of other embodiments of the point cloud map construction device disclosed in the present invention. DETAILED DESCRIPTION

[0063] The following will be combined with the drawings in the embodiments of the present disclosure to clearly and completely describe the technical solutions in the embodiments of the present disclosure. Obviously, the described embodiments are only part of the embodiments of the present disclosure, rather than all the embodiments. The following description of at least one exemplary embodiment is actually only illustrative and is in no way intended to limit the present disclosure and its application or use. Based on the embodiments in the present disclosure, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present disclosure.

[0064] Unless specifically stated otherwise, the relative arrangement of components and steps, the numerical expressions and numerical values ​​set forth in these embodiments do not limit the scope of the present disclosure.

[0065] At the same time, it should be understood that for the convenience of description, the sizes of the various parts shown in the drawings are not drawn according to the actual proportional relationship.

[0066] Technologies, methods and equipment known to ordinary technicians in the relevant art may not be discussed in detail, but where appropriate, such technologies, methods and equipment should be considered part of the authorization specification.

[0067] In all examples shown and discussed herein, any specific values ​​should be interpreted as merely exemplary and not limiting. Therefore, other examples of the exemplary embodiments may have different values.

[0068] It should be noted that like reference numerals and letters refer to like items in the following figures, and therefore, once an item is defined in one figure, it need not be further discussed in subsequent figures.

[0069] Through research, the inventors discovered a related technology that uses virtual point prediction to enhance features. Feature matching and registration are performed by combining feature points and integrating the pose matrix pre-integrated by the IUM. A factor graph optimization algorithm is introduced at the backend to improve mapping and positioning accuracy. This related technology requires the construction of pseudo point cloud features.

[0070] Another related technology uses multiple sensors, including LiDAR, IMU, and UWB, and employs a multi-sensor fusion positioning algorithm based on a factor graph to obtain real-time pose estimates for unmanned vehicles, improving vehicle positioning accuracy. This technology requires the deployment of UWB.

[0071] Another related technology extracts point clouds from the tunnel sidewalls and ground and performs plane fitting to create environmental constraints. It also uses factor graph optimization to tightly couple the LiDAR and IMU, enabling high-precision reconstruction of the tunnel's three-dimensional scene. This technology requires extracting the orientation of the sidewalls and ground, but imposes fewer longitudinal constraints.

[0072] In view of at least one of the above technical problems, the present disclosure provides a point cloud map construction method, device, and system, engineering machinery, and program product. The above embodiments of the present disclosure propose a method and device for constructing underground tunnel point cloud maps based on tunnel contour feature enhancement. For degraded scenarios where underground tunnel point cloud mapping is insufficiently constrained, contour feature enhancement and registration can be used when degradation is determined based on information entropy evaluation, thereby improving odometry accuracy and providing the following advantages:

[0073] The above-mentioned embodiment of the present disclosure determines whether degradation occurs by fusing geometric observability analysis and information entropy evaluation, and uses contour feature registration when degradation occurs, thereby improving the odometer accuracy.

[0074] The above-mentioned embodiments of the present disclosure achieve high-precision state estimation and map construction by co-processing IMU and lidar data in stages, combining forward / backward propagation algorithms and multi-residual joint optimization.

[0075] The present disclosure is described below through specific embodiments.

[0076] Figure 1 Schematic diagram of some embodiments of the point cloud map construction method disclosed in the present invention. Figure 2 Schematic diagrams of other embodiments of the point cloud map construction method disclosed herein. Preferably, Figure 1 or Figure 2 The embodiment can be executed by the point cloud map construction device of the present disclosure, the point cloud map construction system of the present disclosure, or the engineering machinery of the present disclosure. Figure 1 and Figure 2 As shown, the point cloud map construction method disclosed in the present invention may include at least one step from step 100 to step 400.

[0077] In step 100, inertial measurement unit (IMU) data and lidar point cloud data are acquired.

[0078] In some embodiments of the present disclosure, the IMU data may include acceleration and angular velocity data of the IMU.

[0079] In some embodiments of the present disclosure, the lidar point cloud data may be lidar single-frame point cloud data.

[0080] In step 200, forward propagation integration is performed on the IMU data, and timestamp accumulation is performed on the lidar point cloud data to generate a time-aligned IMU data integration result and point cloud data sequence.

[0081] In some embodiments of the present disclosure, Figure 2 As shown, Figure 1 or Figure 2 Step 200 of an embodiment may include: data processing.

[0082] In some embodiments of the present disclosure, Figure 2 As shown, Figure 1 or Figure 2 Step 200 of the embodiment may include at least one of steps 210 to 220 .

[0083] In step 210, the IMU integral is propagated forward.

[0084] In some embodiments of the present disclosure, step 210 may include: performing forward propagation integration on the IMU raw data.

[0085] In some embodiments of the present disclosure, step 210 may include: continuously integrating the acceleration and angular velocity data of the IMU at a predetermined frequency; taking the optimized odometer as the starting position, outputting the IMU data integration result, wherein the IMU data integration result includes at least one of the integrated position, velocity and attitude angle parameters.

[0086] In some embodiments of the present disclosure, the predetermined frequency may be 100 Hz.

[0087] In some embodiments of the present disclosure, step 210 may include: receiving acceleration and angular velocity data from the IMU in real time, using a quaternion integration algorithm to continuously integrate discrete data at a predetermined frequency (e.g., 100 Hz), using the optimized odometer as the starting position, and outputting the integrated position, velocity, and attitude angle parameters.

[0088] The above-mentioned embodiments of the present disclosure can eliminate the drift error caused by sensor noise through the forward propagation mechanism, and provide low-latency motion state prior information for subsequent modules.

[0089] In step 220 , point clouds are accumulated.

[0090] In some embodiments of the present disclosure, step 220 may include: accumulating timestamps on the lidar point cloud data to generate a time-aligned IMU data integration result and a point cloud data sequence.

[0091] In some embodiments of the present disclosure, step 220 may include: accumulating timestamps on the lidar point cloud to generate time-aligned state parameters and point cloud sequences.

[0092] In some embodiments of the present disclosure, step 210 may include: synchronizing the single-frame point cloud data of the lidar with the IMU integration results in time and space according to the timestamp; accumulating multiple frames of point cloud data using a sliding window algorithm to construct a globally consistent point cloud data sequence.

[0093] The above-mentioned embodiments of the present disclosure can reduce the cumulative error caused by the change of the sensor posture within the scanning interval by introducing a motion compensation factor.

[0094] In step 300, the point cloud data distortion is corrected based on the back propagation of the IMU data integration results, geometric features are extracted from the dedistorted point cloud data, and point-surface residuals are calculated to construct state estimation constraints. For map matching failure scenarios, the contour residuals are optimized, and multiple types of residuals are fused to construct a residual set.

[0095] In some embodiments of the present disclosure, step 300 may include: correcting point cloud distortion based on backpropagation of IMU integration results, extracting geometric features and calculating point-surface residuals, constructing state estimation constraints, triggering a contour residual optimization strategy to maintain system stability for map matching failure scenarios; and fusing multiple types of residuals to construct a residual set.

[0096] In some embodiments of the present disclosure, Figure 2 As shown, Figure 1 or Figure 2 Step 300 of an embodiment may include: state estimation.

[0097] In some embodiments of the present disclosure, Figure 2 As shown, Figure 1 or Figure 2 Step 300 of the embodiment may include at least one of steps 310 to 350 .

[0098] In step 310 , the point cloud is back-propagated for dedistortion.

[0099] In some embodiments of the present disclosure, step 310 may include: correcting the point cloud data distortion based on the back propagation of the IMU data integration result.

[0100] In some embodiments of the present disclosure, step 310 may include: based on the IMU integration results, reversely solving the sensor motion trajectory at each moment in the lidar scanning cycle; using a predetermined interpolation algorithm to fit the continuous motion model; applying an inverse transformation to the accumulated point cloud data point by point to eliminate the non-rigid distortion caused by the high-speed movement of the sensor, and generating dedistorted point cloud data.

[0101] In some embodiments of the present disclosure, the predetermined interpolation algorithm may be a B-spline interpolation algorithm.

[0102] In some embodiments of the present disclosure, step 310 may include: based on the pose sequence output by the IMU integration, inversely solving the sensor motion trajectory at each moment in the lidar scanning cycle; using the B-spline interpolation algorithm to fit the continuous motion model, applying an inverse transformation to the accumulated point cloud point by point, eliminating the non-rigid distortion caused by the high-speed movement of the sensor, and generating a dedistorted point cloud.

[0103] In step 320, the point-surface residuals are obtained.

[0104] In some embodiments of the present disclosure, step 320 may include: extracting geometric features from the dedistorted point cloud data and calculating point-to-surface residuals to construct state estimation constraints.

[0105] In some embodiments of the present disclosure, step 320 may include: extracting local plane features from the dedistorted point cloud data; using a second predetermined algorithm to accelerate the nearest neighbor search, and calculating the Euclidean distance from each point to the corresponding plane as the residual; eliminating outliers through a third predetermined algorithm, and generating a weighted point-surface residual set as an observation constraint for state estimation.

[0106] In some embodiments of the present disclosure, the second predetermined algorithm may be an ikd-Tree algorithm.

[0107] In some embodiments of the present disclosure, the third predetermined algorithm may be a RANSAC (Random Sample Consensus) algorithm.

[0108] In some embodiments of the present disclosure, step 320 may include extracting local planar features from the dedistorted point cloud, using an ikd-Tree accelerated nearest neighbor search, and calculating the Euclidean distance from each point to the corresponding plane as a residual. Outliers are removed using a RANSAC algorithm to generate a weighted set of point-surface residuals, which serve as observation constraints for state estimation.

[0109] In step 330 , it is determined whether degradation has occurred.

[0110] In some embodiments of the present disclosure, step 330 may include: for a map matching failure scenario, fusing geometric observability analysis with information entropy evaluation to determine whether degradation occurs.

[0111] In some embodiments of the present disclosure, step 330 may include: linearizing the residual equation at the predicted optimized value, performing eigenvalue decomposition on the generated information matrix; and when the minimum eigenvalue is less than a predetermined threshold, directly associating the minimum eigenvalue with the system degradation sensitive direction.

[0112] In some embodiments of the present disclosure, step 330 may include: fusing geometric observability analysis and information entropy evaluation to determine whether degradation has occurred, linearizing the residual equation at the predicted optimized value, performing eigenvalue decomposition on the generated information matrix, and if its minimum eigenvalue is less than a certain threshold, then the eigenvalue is directly related to the system degradation sensitive direction.

[0113] In some embodiments of the present disclosure, the step of directly associating the minimum eigenvalue with the system degradation sensitive direction includes: in the underground well working scenario, determining that there is no point cloud data along the longitudinal line direction of the corridor; in the case of point-to-surface registration, determining that the obtained plane normal vector disappears in the longitudinal direction, and longitudinal degradation occurs.

[0114] For example, in an underground well working scenario, there is no point cloud along the longitudinal direction of the corridor, which causes the plane normal vector obtained during point-to-surface registration to disappear in the longitudinal direction, resulting in longitudinal degradation.

[0115] In step 340, line residuals and / or contour residuals.

[0116] In some embodiments of the present disclosure, step 340 may include: optimizing the contour residual when degradation is determined.

[0117] In some embodiments of the present disclosure, step 340 may include: extracting environmental line features and contour features based on point-surface residuals; calculating line matching residuals and contour matching residuals based on the current frame contour and map contour features through a fourth predetermined algorithm, thereby expanding the geometric constraint dimension of the optimization problem.

[0118] In some embodiments of the present disclosure, the fourth predetermined algorithm may be an ICP (Iterated Closest Points) algorithm.

[0119] In some embodiments of the present disclosure, step 340 may include: extracting environmental line features (such as corners and edges) and contour features (such as object boundaries) based on point-surface residuals, matching the current frame contour with the map contour features through the ICP algorithm, calculating the line / contour matching residuals, and expanding the geometric constraint dimension of the optimization problem.

[0120] The above embodiments of the present disclosure improve odometer accuracy through contour feature registration.

[0121] The above-mentioned embodiment of the present disclosure determines whether degradation occurs by fusing geometric observability analysis and information entropy evaluation, and uses contour feature registration when degradation occurs, thereby improving the odometer accuracy.

[0122] In step 350, the residual set.

[0123] In some embodiments of the present disclosure, step 350 may include: fusing multiple types of residuals to construct a residual set.

[0124] In some embodiments of the present disclosure, step 350 may include: dynamically adjusting the optimization weights when degradation of environmental features is detected; and merging point-surface residuals, line residuals, contour residuals, and IMU pre-integration residuals into a multi-source residual set.

[0125] In some embodiments of the present disclosure, environmental features are degraded, such as a long corridor structure causing line features to be missing.

[0126] In step 400, the system state is iteratively corrected using a first predetermined algorithm, and an odometer and a map of the underground tunnel are synchronously generated.

[0127] In some embodiments of the present disclosure, the first predetermined algorithm may be an LM (Levenberg-Marquardt) algorithm, which is a numerical optimization method widely used in nonlinear least squares problems.

[0128] In some embodiments of the present disclosure, step 400 may include: iteratively correcting the system state through a nonlinear optimization algorithm, and synchronously generating a high-precision odometer and a map.

[0129] In some embodiments of the present disclosure, Figure 2 As shown, Figure 1 or Figure 2 Step 400 of an embodiment may include: joint optimization.

[0130] In some embodiments of the present disclosure, Figure 2 As shown, Figure 1 or Figure 2 Step 400 of the embodiment may include at least one of steps 410 to 420 .

[0131] In step 410 , the odometer is used.

[0132] In some embodiments of the present disclosure, step 410 may include: constructing a tightly coupled nonlinear least squares problem, using a first predetermined algorithm to iteratively solve the optimal state estimate; and using the optimized odometer as the starting pose for the next IMU forward propagation.

[0133] In some embodiments of the present disclosure, step 410 may include: constructing a tightly coupled nonlinear least squares problem, using the LM algorithm to iteratively solve the optimal state estimate; and using the optimized odometer as the starting pose for the next IMU forward propagation.

[0134] In step 420, map.

[0135] In some embodiments of the present disclosure, step 420 may include: projecting the dedistorted point cloud data to a global coordinate system based on the optimized pose, and constructing a dense map using a fifth predetermined algorithm.

[0136] In some embodiments of the present disclosure, the fifth predetermined algorithm may be a voxel grid filtering algorithm.

[0137] In some embodiments of the present disclosure, step 420 may include: projecting the dedistorted point cloud to a global coordinate system based on the optimized pose, and constructing a dense map using a voxel grid filtering algorithm. The map produced by the above embodiments of the present disclosure is used for degraded contour registration.

[0138] The above-mentioned embodiments of the present disclosure provide a method for constructing a point cloud map of an underground tunnel based on tunnel contour feature enhancement.

[0139] The above-mentioned embodiment of the present disclosure determines whether degradation occurs by fusing geometric observability analysis and information entropy evaluation, and uses contour feature registration when degradation occurs, thereby improving the odometer accuracy.

[0140] The above-mentioned embodiments of the present disclosure achieve high-precision state estimation and map construction by coordinating the processing of IMU and lidar data in stages, combining forward / backward propagation algorithms and multi-residual joint optimization.

[0141] Figure 3 Schematic diagram of some embodiments of the point cloud map construction device disclosed in the present invention. Figure 3 As shown, the point cloud map construction device of the present disclosure may include a data acquisition module 31 , a data processing module 32 , a state estimation module 33 and a joint optimization module 34 .

[0142] The data acquisition module 31 is configured to acquire inertial measurement unit IMU data and lidar point cloud data.

[0143] In some embodiments of the present disclosure, the IMU data includes acceleration and angular velocity data of the IMU.

[0144] In some embodiments of the present disclosure, the lidar point cloud data is lidar single-frame point cloud data.

[0145] The data processing module 32 is configured to perform forward propagation integration on the IMU data, accumulate timestamps on the lidar point cloud data, and generate time-aligned IMU data integration results and point cloud data sequences.

[0146] In some embodiments of the present disclosure, the data processing module 32 can be configured to perform forward propagation integration on the IMU raw data, accumulate timestamps on the lidar point cloud, and generate time-aligned state parameters and point cloud sequences.

[0147] In some embodiments of the present disclosure, the data processing module 32 may include a forward propagation IMU integration unit and a point cloud accumulation unit.

[0148] In some embodiments of the present disclosure, the forward propagation IMU integration unit can be configured to continuously integrate the acceleration and angular velocity data of the IMU at a predetermined frequency; with the optimized odometer as the starting position, output the IMU data integration result, wherein the IMU data integration result includes at least one of the integrated position, velocity and attitude angle parameters.

[0149] In some embodiments of the present disclosure, the forward propagation IMU integration unit can be configured to receive the acceleration and angular velocity data of the IMU in real time, use a quaternion integration algorithm to continuously integrate the discrete data at a preset frequency (such as 100 Hz), use the optimized odometer as the starting position, and output the integrated position, velocity, and attitude angle parameters.

[0150] The above-mentioned embodiments of the present disclosure eliminate the drift error caused by sensor noise through the forward propagation mechanism, and provide low-latency motion state prior information for subsequent modules.

[0151] In some embodiments of the present disclosure, the point cloud accumulation unit can be configured to synchronize the single-frame point cloud data of the lidar with the IMU integration results in time and space according to the timestamp; use a sliding window algorithm to accumulate multiple frames of point cloud data to construct a globally consistent point cloud data sequence.

[0152] In some embodiments of the present disclosure, the point cloud accumulation unit can be configured to receive single-frame point cloud data from a lidar, perform spatiotemporal synchronization based on the timestamp and the IMU integration result, accumulate multiple frames of point clouds using a sliding window algorithm, and construct a globally consistent point cloud sequence.

[0153] The above-mentioned embodiments of the present disclosure reduce the cumulative error caused by the change of the sensor posture within the scanning interval by introducing a motion compensation factor.

[0154] The state estimation module 33 is configured to correct the distortion of the point cloud data based on the back propagation of the IMU data integration results, extract geometric features from the dedistorted point cloud data and calculate the point-surface residuals to construct state estimation constraints; optimize the contour residuals for map matching failure scenarios; and fuse multiple types of residuals to construct a residual set.

[0155] In some embodiments of the present disclosure, the state estimation module 33 can be configured to correct point cloud distortion based on backpropagation of IMU integration results, extract geometric features and calculate point-surface residuals, construct state estimation constraints, trigger contour residual optimization strategies to maintain system stability for map matching failure scenarios, and fuse multiple types of residuals to construct a residual set.

[0156] In some embodiments of the present disclosure, the state estimation module 33 may include a back-propagation point cloud dedistortion unit, a point-surface residual calculation unit, a degradation detection unit, a line / contour residual calculation unit, and a residual set determination unit.

[0157] In some embodiments of the present disclosure, the back-propagation point cloud dedistortion unit can be configured to reversely solve the sensor motion trajectory at each moment in the lidar scanning cycle based on the IMU integration result; fit the continuous motion model using a predetermined interpolation algorithm; apply an inverse transformation to the accumulated point cloud data point by point to eliminate the non-rigid distortion caused by the high-speed movement of the sensor, and generate dedistorted point cloud data.

[0158] In some embodiments of the present disclosure, the backpropagation point cloud dedistortion unit can be configured to reversely calculate the sensor motion trajectory at each moment within the lidar scanning cycle based on the pose sequence output by the IMU integration. A continuous motion model is fitted using a B-spline interpolation algorithm, and an inverse transform is applied point by point to the accumulated point cloud to eliminate non-rigid distortion caused by the high-speed motion of the sensor, generating dedistorted point cloud data.

[0159] In some embodiments of the present disclosure, the point-surface residual calculation unit can be configured to extract local plane features from dedistorted point cloud data; use a second predetermined algorithm to accelerate the nearest neighbor search, and calculate the Euclidean distance from each point to the corresponding plane as the residual; eliminate outliers through a third predetermined algorithm, and generate a weighted point-surface residual set as an observation constraint for state estimation.

[0160] In some embodiments of the present disclosure, the point-surface residual calculation unit can be configured to extract local planar features from the dedistorted point cloud, use an ikd-Tree accelerated nearest neighbor search, and calculate the Euclidean distance from each point to the corresponding plane as the residual. Outliers are removed using the RANSAC algorithm to generate a weighted set of point-surface residuals, which serve as observation constraints for state estimation.

[0161] In some embodiments of the present disclosure, the degradation detection unit may be configured to determine whether degradation occurs by fusing geometric observability analysis with information entropy evaluation for map matching failure scenarios.

[0162] In some embodiments of the present disclosure, the degradation detection unit can be configured to linearize the residual equation at the predicted optimization value and perform eigenvalue decomposition on the generated information matrix; when the minimum eigenvalue is less than a predetermined threshold, the minimum eigenvalue is directly associated with the system degradation sensitive direction.

[0163] In some embodiments of the present disclosure, the degradation detection unit can also be configured to determine that there is no point cloud data along the longitudinal line direction of the corridor in an underground well working scenario; in the case of point-to-surface registration, it is determined that the obtained plane normal vector disappears in the longitudinal direction, and longitudinal degradation occurs.

[0164] In some embodiments of the present disclosure, the degradation detection unit can be configured to fuse geometric observability analysis with information entropy assessment to determine degradation. The residual equation is linearized at the predicted optimized value, and the resulting information matrix is ​​subjected to eigenvalue decomposition. If the minimum eigenvalue is less than a certain threshold, this eigenvalue is directly associated with the system's degradation-sensitive direction. For example, in an underground well operating scenario, there is no point cloud along the longitudinal direction of the corridor. This causes the plane normal vector obtained during point-to-surface registration to disappear in the longitudinal direction, resulting in longitudinal degradation.

[0165] In some embodiments of the present disclosure, the line / contour residual calculation unit may be configured to optimize the contour residual when degradation is determined.

[0166] In some embodiments of the present disclosure, the line / contour residual calculation unit can be configured to extract environmental line features and contour features based on point-surface residuals; calculate line matching residuals and contour matching residuals through the current frame contour and map contour features through a fourth predetermined algorithm, thereby expanding the geometric constraint dimension of the optimization problem.

[0167] In some embodiments of the present disclosure, the line / contour residual calculation unit can be configured to extract environmental line features (such as corners and edges) and contour features (such as object boundaries) based on point-surface residuals, match the current frame contour with the map contour features through the ICP algorithm, calculate the line / contour matching residual, and expand the geometric constraint dimension of the optimization problem.

[0168] The above embodiments of the present disclosure improve odometer accuracy through contour feature registration.

[0169] In some embodiments of the present disclosure, the residual set determination unit can be configured to dynamically adjust the optimization weights when environmental feature degradation is detected; and merge the point surface residuals, line residuals, contour residuals and IMU pre-integration residuals into a multi-source residual set.

[0170] In some embodiments of the present disclosure, the residual set determination unit can be configured to dynamically adjust the optimization weights when environmental feature degradation is detected (such as the loss of line features due to a corridor structure), and merge the point-surface residuals, line / contour residuals and IMU pre-integration residuals into a multi-source residual set.

[0171] The joint optimization module 34 is configured to iteratively correct the system state through a first predetermined algorithm and synchronously generate an odometer and a map of the underground tunnel.

[0172] In some embodiments of the present disclosure, the joint optimization module 34 may be configured to iteratively correct the system state through a nonlinear optimization algorithm and synchronously generate a high-precision odometer and map.

[0173] In some embodiments of the present disclosure, the joint optimization module 34 may include an odometer optimization unit and a map optimization unit.

[0174] In some embodiments of the present disclosure, the odometer optimization unit can be configured to construct a tightly coupled nonlinear least squares problem, use a first predetermined algorithm to iteratively solve the optimal state estimate, and use the optimized odometer as the starting pose for the next IMU forward propagation.

[0175] In some embodiments of the present disclosure, the odometry optimization unit can be configured to construct a tightly coupled nonlinear least squares problem and iteratively solve the optimal state estimate using the LM algorithm. The optimized odometry serves as the starting pose for the next IMU forward propagation.

[0176] In some embodiments of the present disclosure, the map optimization unit may be configured to project the dedistorted point cloud data into a global coordinate system based on the optimized posture, and construct a dense map using a fifth predetermined algorithm.

[0177] In some embodiments of the present disclosure, the map optimization unit may be configured to project the dedistorted point cloud into a global coordinate system based on the optimized pose, and construct a dense map using a voxel grid filtering algorithm.

[0178] The maps produced by the above embodiments of the present disclosure can be used for degraded contour registration.

[0179] In some embodiments of the present disclosure, the point cloud map construction device of the present disclosure may also be configured to execute the point cloud map construction method described in any of the above embodiments of the present disclosure.

[0180] The point cloud map construction device disclosed in the present invention consists of a data processing module, a state estimation module and a joint optimization module. Through the bidirectional optimization mechanism of forward propagation and backpropagation, it solves the problems of point cloud distortion compensation, motion state estimation and robust mapping in environmental degradation scenarios.

[0181] Figure 4Schematic diagram of the structure of some other embodiments of the point cloud map construction device disclosed in the present invention. Figure 4 As shown, the point cloud map construction device includes a memory 41 and a processor 42.

[0182] The memory 41 is used to store instructions, the processor 42 is coupled to the memory 41, and the processor 42 is configured to execute the point cloud map construction method described in any embodiment of the present disclosure based on the instructions stored in the memory.

[0183] like Figure 4 As shown, the point cloud map construction device further includes a communication interface 43 for exchanging information with other devices. At the same time, the point cloud map construction device further includes a bus 44, through which the processor 42, the communication interface 43, and the memory 41 communicate with each other.

[0184] Memory 41 may include high-speed RAM memory or non-volatile memory, such as at least one disk storage device. Memory 41 may also be a memory array. Memory 41 may also be divided into blocks, and the blocks may be combined into virtual volumes according to certain rules.

[0185] Furthermore, the processor 42 may be a central processing unit (CPU), or may be an application-specific integrated circuit (ASIC), or may be configured to implement one or more integrated circuits of the present disclosure.

[0186] In response to the technical problems of related technologies, the present disclosure proposes a method and device for constructing underground tunnel point cloud maps based on tunnel contour feature enhancement. For degraded scenarios with insufficient constraints on underground tunnel point cloud mapping, contour feature enhancement and registration are used when judging degradation based on information entropy evaluation, thereby improving the odometer accuracy.

[0187] According to another aspect of the present disclosure, a point cloud map construction system is provided, comprising a data acquisition device and a point cloud map construction device as described in any one of the above embodiments.

[0188] In some embodiments of the present disclosure, the data acquisition device may include an IMU and a lidar.

[0189] In some embodiments of the present disclosure, the point cloud map construction system may be an unmanned operating system for a shovel loader in an underground mining scenario.

[0190] According to another aspect of the present disclosure, there is provided an engineering machine comprising a point cloud map construction system as described in any one of the above embodiments.

[0191] In some embodiments of the present disclosure, the engineering machine may be an underground scraper.

[0192] In some embodiments of the present disclosure, the engineering machinery may be a shovel loader for underground mining scenarios.

[0193] According to another aspect of the present disclosure, a computer program product is provided, including a computer program, wherein when the computer program is executed by a processor, the point cloud map construction method as described in any of the above embodiments is implemented.

[0194] According to another aspect of the present disclosure, a computer-readable storage medium is provided, wherein the computer-readable storage medium stores computer instructions, and when the instructions are executed by a processor, the point cloud map construction method as described in any of the above embodiments is implemented.

[0195] In some embodiments of the present disclosure, the computer-readable storage medium may be a non-transitory computer-readable storage medium.

[0196] Through research, the inventors found that the underground tunnel point cloud map construction method in the related art did not specifically consider the degradation of detection point-to-surface registration and did not use the contour registration method.

[0197] The above embodiment of the present disclosure targets degraded scenarios where underground tunnel point cloud mapping is insufficiently constrained. It uses contour feature enhancement registration when judging degradation based on information entropy evaluation. Compared with related technologies:

[0198] The above-mentioned embodiment of the present disclosure determines whether degradation occurs by fusing geometric observability analysis and information entropy evaluation, and uses contour feature registration when degradation occurs, thereby improving the odometer accuracy.

[0199] The above-mentioned embodiments of the present disclosure achieve high-precision state estimation and map construction by co-processing IMU and lidar data in stages, combining forward / backward propagation algorithms and multi-residual joint optimization.

[0200] Those skilled in the art will appreciate that embodiments of the present disclosure may be provided as methods, apparatuses, or computer program products. Thus, the present disclosure may take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present disclosure may take the form of a computer program product implemented on one or more computer-usable, non-transitory storage media (including, but not limited to, magnetic disk storage, CD-ROMs, optical storage, etc.) containing computer-usable program code.

[0201] The present disclosure is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present disclosure. It should be understood that each process and / or block in the flowcharts and / or block diagrams, as well as combinations of processes and / or blocks in the flowcharts and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowcharts and / or block diagrams. Figure 1 a process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0202] These computer program instructions may also be stored in a computer readable memory that can direct a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 a process or multiple processes and / or boxes Figure 1 The function specified in one or more boxes.

[0203] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operational steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing the instructions executed on the computer or other programmable device for implementing the process. Figure 1 a process or multiple processes and / or boxes Figure 1 A step that specifies a function in one or more boxes.

[0204] The point cloud map construction device, data acquisition module, data processing module, state estimation module and joint optimization module described above can be implemented as a general-purpose processor, programmable logic controller (PLC), digital signal processor (DSP), application-specific integrated circuit (ASIC), field programmable gate array (FPGA) or other programmable logic device, discrete gate or transistor logic device, discrete hardware component or any appropriate combination thereof for performing the functions described in this application.

[0205] The present disclosure has been described in detail. To avoid obscuring the concept of the present disclosure, some details known in the art have not been described. Based on the above description, those skilled in the art can fully understand how to implement the technical solutions disclosed herein.

[0206] Those skilled in the art will understand that all or part of the steps to implement the above embodiments may be accomplished by hardware, or may be accomplished by instructing the relevant hardware through a program, and the program may be stored in a non-transitory computer-readable storage medium, and the above-mentioned storage medium may be a read-only memory, a disk, or an optical disk, etc.

[0207] The description of the present disclosure is provided for purposes of illustration and description and is not intended to be exhaustive or to limit the disclosure to the form disclosed. Many modifications and variations will be apparent to those skilled in the art. The embodiments are selected and described in order to better illustrate the principles and practical applications of the present disclosure and to enable those skilled in the art to understand the present disclosure and design various embodiments with various modifications suitable for specific applications.

Claims

1. A point cloud map construction method, comprising: Obtain inertial measurement unit (IMU) data and lidar point cloud data; Perform forward propagation integration on the IMU data, accumulate timestamps on the lidar point cloud data, and generate time-aligned IMU data integration results and point cloud data sequences; Correct point cloud data distortion based on backpropagation of IMU data integration results, extract geometric features from the dedistorted point cloud data, calculate point-surface residuals, and construct state estimation constraints; optimize contour residuals for map matching failure scenarios; Fusion of multiple types of residuals to construct residual sets; The system state is iteratively corrected through a first predetermined algorithm, and an odometer and a map of the underground tunnel are synchronously generated.

2. The point cloud map construction method according to claim 1, wherein: For map matching failure scenarios, the optimization of contour residuals includes: For map matching failure scenarios, geometric observability analysis and information entropy evaluation are integrated to determine whether there is degradation; In the case of degradation, the contour residual is optimized.

3. The point cloud map construction method according to claim 2, wherein: The fusion of geometric observability analysis and information entropy evaluation to determine whether degradation occurs includes: Linearize the residual equation at the predicted optimal value and perform eigenvalue decomposition on the generated information matrix; When the minimum eigenvalue is less than a predetermined threshold, the minimum eigenvalue is directly associated with a system degradation sensitive direction.

4. The point cloud map construction method according to claim 3, wherein: Directly associating the minimum eigenvalue with the system degradation sensitive direction includes: In the underground well working scenario, it is determined that there is no point cloud data along the longitudinal direction of the corridor; In the case of point-to-plane registration, it is determined that the obtained plane normal vector disappears in the longitudinal direction, resulting in longitudinal degradation.

5. The point cloud map construction method according to any one of claims 1 to 4, wherein: IMU data includes IMU acceleration and angular velocity data, Performing the forward propagation integration on the IMU data involves: Continuously integrate the IMU acceleration and angular velocity data at a predetermined frequency; The optimized odometer is used as the starting position, and an IMU data integration result is output, wherein the IMU data integration result includes at least one of the integrated position, velocity, and attitude angle parameters.

6. The point cloud map construction method according to any one of claims 1 to 4, wherein: LiDAR point cloud data is single-frame point cloud data of LiDAR. Accumulate timestamps on the lidar point cloud data to generate time-aligned IMU data integration results and point cloud data sequences, including: Based on the timestamp, the lidar single-frame point cloud data and the IMU integration results are synchronized in time and space; A sliding window algorithm is used to accumulate multi-frame point cloud data to construct a globally consistent point cloud data sequence.

7. The point cloud map construction method according to any one of claims 1 to 4, wherein: The back propagation correction of point cloud data distortion based on the IMU data integration results includes: Based on the IMU integration results, reversely calculate the sensor motion trajectory at each moment in the lidar scanning cycle; fitting a continuous motion model using a predetermined interpolation algorithm; An inverse transformation is applied point by point to the accumulated point cloud data to eliminate the non-rigid distortion caused by the high-speed motion of the sensor and generate dedistorted point cloud data.

8. The point cloud map construction method according to any one of claims 1 to 4, wherein: Extracting geometric features from the dedistorted point cloud data and calculating point-surface residuals to construct state estimation constraints include: Extract local planar features from dedistorted point cloud data; Using a second predetermined algorithm to accelerate the nearest neighbor search, calculating the Euclidean distance from each point to the corresponding plane as a residual; Outliers are eliminated through a third predetermined algorithm to generate a set of point-surface residuals as observation constraints for state estimation.

9. The point cloud map construction method according to any one of claims 1 to 4, wherein: Optimizing the contour residual includes: Based on the point-surface residuals, the environmental line features and contour features are extracted; The fourth predetermined algorithm is used to compare the current frame contour with the map contour features, calculate the line matching residual and the contour matching residual, and expand the geometric constraint dimension of the optimization problem.

10. The point cloud map construction method according to any one of claims 1 to 4, wherein: The residual set constructed by fusing multiple types of residuals includes: Dynamically adjust optimization weights when degradation of environmental characteristics is detected; The point-surface residuals, line residuals, contour residuals and IMU pre-integration residuals are combined into a multi-source residual set.

11. The point cloud map construction method according to any one of claims 1 to 4, wherein: Iteratively correcting the system state through a first predetermined algorithm to synchronously generate an odometer and a map of the underground tunnel includes: Constructing a tightly coupled nonlinear least squares problem, and iteratively solving the optimal state estimate using a first predetermined algorithm; Use the optimized odometer as the starting pose for the next IMU forward propagation; Based on the optimized pose, the dedistorted point cloud data is projected into the global coordinate system, and the fifth predetermined algorithm is used to construct a dense map.

12. A point cloud map construction device, comprising: A data acquisition module is configured to acquire inertial measurement unit (IMU) data and lidar point cloud data; A data processing module is configured to perform forward propagation integration on the IMU data, accumulate timestamps on the lidar point cloud data, and generate a time-aligned IMU data integration result and point cloud data sequence; The state estimation module is configured to correct point cloud data distortion based on backpropagation of the IMU data integration results, extract geometric features from the dedistorted point cloud data, calculate point-surface residuals, and construct state estimation constraints; in the case of map matching failure, the contour residuals are optimized; Fusion of multiple types of residuals to construct residual sets; The joint optimization module is configured to iteratively correct the system state through a first predetermined algorithm and synchronously generate an odometer and a map of the underground tunnel.

13. A point cloud map construction device, comprising: Memory; as well as A processor coupled to the memory, wherein the processor is configured to execute the point cloud map construction method according to any one of claims 1 to 11 based on instructions stored in the memory.

14. A point cloud map construction system, comprising a data acquisition device and the point cloud map construction device according to claim 12 or 13.

15. An engineering machine comprising the point cloud map construction system according to claim 14.

16. A computer-readable storage medium, wherein: The computer-readable storage medium stores computer instructions, and when the instructions are executed by a processor, the point cloud map construction method according to any one of claims 1 to 11 is implemented.

17. A computer program product comprising a computer program, wherein When the computer program is executed by a processor, the point cloud map construction method according to any one of claims 1 to 11 is implemented.

Images