A 3D Reconstruction Method and System for Narrow Lanes

Abstract

This application discloses a 3D reconstruction method and system suitable for narrow alleyways. The method includes: accumulating point clouds using mechanical lidar and solid-state lidar to obtain mechanical lidar point cloud data and solid-state lidar point cloud data; performing forward propagation using an IMU to predict the robot's pose and obtain the IMU-estimated pose transformation; selecting feature points from the mechanical lidar point cloud data using mechanical lidar, and projecting the mechanical lidar point cloud data and solid-state lidar point cloud data according to the IMU-estimated pose transformation to form a point cloud frame; filtering and optimizing the point cloud frame and the IMU-estimated pose transformation to obtain the optimal pose estimate; updating the mechanical lidar local map using the optimal pose estimate, generating and updating the solid-state lidar global map, and forming a 3D reconstruction model of the alleyway. This invention uses solid-state lidar as the main component, supplemented by mechanical lidar and IMU as constraints, which can obtain more details of the alleyway.

Description

Technical Field

[0001] This application relates to the field of 3D reconstruction, and more specifically to a 3D reconstruction method based on the fusion of mechanical lidar, solid-state lidar and IMU (IMU) for the 3D reconstruction of narrow alleyways. Background Technology

[0002] Roadways are various passages drilled between the surface and the ore body, used for ore transportation, ventilation, drainage, pedestrian access, and various necessary preparatory works for new ore extraction by metallurgical equipment. Because roadways are generally underground, they are subject to the pressure of rock and soil masses and are easily affected during mining operations, potentially leading to safety issues. Therefore, roadway mapping is crucial. Roadway mapping technology originates from mine surveying, and traditional methods use total stations and 3D laser scanners to obtain environmental information about the roadways. However, traditional roadway mapping processes are time-consuming, involve excessive personnel transfers, are slow to update maps, and total stations and high-precision 3D scanners are expensive. With the improvement in sensor accuracy and the increase in the types of sensors in recent years, sensor fusion SLAM has gradually been applied to underground roadway mapping technology. According to the literature reviewed so far, only one lidar is typically used in mapping, combined with other sensors for fusion. LiDAR is divided into mechanical LiDAR and solid-state LiDAR. Mechanical LiDAR has the advantage of a large horizontal field of view, providing more environmental feature information, but the resulting point cloud is relatively sparse. Solid-state LiDAR, on the other hand, can obtain very dense point clouds, but its field of view is smaller, capturing less environmental feature information and making it prone to drift during mapping. Furthermore, since there is no GPS signal in underground tunnels, IMUs are often used as attitude estimation sensors to improve positioning information.

[0003] Considering that the 3D reconstruction of tunnels requires a drift-free and dense point cloud model, this invention integrates mechanical LiDAR, solid-state LiDAR, and IMU, mounting them on a mobile robot chassis to form a novel tightly coupled multi-sensor fusion SLAM method for 3D tunnel reconstruction. This method yields a more robust and denser 3D model compared to single-LiDAR SLAM reconstruction, and is significantly less costly and requires far less manpower than traditional methods using total stations and 3D scanners. Summary of the Invention

[0004] This application discloses a three-dimensional reconstruction method suitable for tunnels. By fusing mechanical lidar, solid-state lidar and IMU and mounting them on a mobile robot chassis, a new tightly coupled multi-sensor fusion SLAM method for tunnel three-dimensional reconstruction is formed.

[0005] To achieve the above objectives, this application provides the following solutions:

[0006] A three-dimensional reconstruction method and system suitable for narrow alleyways, the method comprising:

[0007] S1. Accumulate point clouds using mechanical lidar and solid-state lidar respectively to obtain mechanical lidar point cloud data and solid-state lidar point cloud data.

[0008] S2. Use the IMU for forward propagation to predict the robot's pose and obtain the pose transformation estimated by the IMU.

[0009] S3. Using the mechanical lidar, planar feature points and edge feature points are selected from the mechanical lidar point cloud data. Based on the pose transformation estimated by the IMU, the mechanical lidar point cloud data and the solid-state lidar point cloud data are projected to form a point cloud frame.

[0010] S4. Filter and optimize the pose transformation of the point cloud frame and the IMU-estimated pose to obtain the optimal pose estimate;

[0011] S5. The optimal pose estimation is used to update the mechanical laser local map, generate and update the solid-state laser global map, and finally form a dense three-dimensional reconstruction model of the tunnel.

[0012] Preferably, the process of obtaining the mechanical laser point cloud data and the solid-state laser point cloud data specifically includes:

[0013] The data collected from the high-frequency sampling of the mechanical lidar and the solid-state lidar within a preset time period are processed to obtain the mechanical lidar point cloud data and the solid-state lidar point cloud data.

[0014] Preferably, the specific process for obtaining the pose transformation estimated by the IMU includes:

[0015] The IMU is turned on, and forward propagation is performed using the IMU to predict the robot's pose at different times, thus obtaining the pose transformation estimated by the IMU.

[0016] Preferably, the IMU forward propagation process specifically includes:

[0017] The data from the six-axis IMU is used for forward propagation to obtain real-time linear and angular accelerations. This IMU data is then used for forward propagation to estimate the robot's real-time pose changes. The formula for IMU forward propagation is as follows:

[0018]

[0019] Where, x i That is, the robot's state as defined above, u i The input is Δt, where Δt is the interval between two IMU frames, and 0 indicates that the process noise is set to 0.

[0020] Preferably, the specific process of filtering out planar feature points and edge feature points includes:

[0021] From the original radar data center, feature points are selected for subsequent state estimation. Using five points before and after a random point in the point cloud data as a group, the curvature is calculated:

[0022]

[0023] Where c is the curvature, S is a set of random points, i is the currently centered point, k is the point in the k-th frame, and L is the coordinate system of the mechanical lidar. These are the coordinates of a point in the mechanical lidar coordinate system.

[0024] Preferably, the process of performing filter optimization specifically includes:

[0025] An extended Kalman filter is used to optimize the pose estimation equation, including the prediction model and the correction model.

[0026] The prediction model specifically includes: prior estimation, the formula of which is: The covariance matrix of the prior error is given by the following formula: in, Let f(·) be the predicted robot pose, and x be the state equation. k-1 u represents the robot's state determined in the previous moment. k-1 For input, 0 indicates that we still consider process noise to be 0, and Q is the noise covariance that follows a Gaussian distribution.

[0027] The correction model includes: Kalman gain, the formula of which is: The posterior estimate is given by the formula: The covariance matrix of the prediction error is given by the following formula: in, h(·) is the output equation.

[0028] Preferably, the formation of a dense three-dimensional reconstruction model of the tunnel specifically includes:

[0029] Based on the optimized pose estimation equation obtained by the extended Kalman filter, the point cloud frames obtained from the mechanical laser point cloud data and the solid-state laser point cloud data at different times are projected according to the pose transformation equation to update the mechanical laser local map, generate and update the solid-state laser global map, and finally form a dense three-dimensional reconstruction model of the tunnel.

[0030] A three-dimensional reconstruction method suitable for narrow alleyways also includes a three-dimensional reconstruction system suitable for narrow alleyways, comprising a point cloud accumulation module, a forward propagation module, a filtering module, a filtering module, and a model building module;

[0031] The point cloud accumulation module is used to accumulate point clouds using mechanical lidar and solid-state lidar respectively, to obtain mechanical lidar point cloud data and solid-state lidar point cloud data.

[0032] The forward propagation module is used to perform forward propagation using the IMU to predict the robot's pose and obtain the pose transformation estimated by the IMU.

[0033] The filtering module is used to filter out planar feature points and edge feature points from the mechanical laser point cloud data using the mechanical lidar, and to project the mechanical laser point cloud data and the solid-state laser point cloud data according to the pose transformation estimated by the IMU to form a point cloud frame;

[0034] The filtering module is used to filter and optimize the pose transformation of the point cloud frame and the IMU-estimated pose to obtain the optimal pose estimate;

[0035] The model building module uses the optimal pose estimation to update the mechanical laser local map, generate and update the solid-state laser global map, and finally form a dense three-dimensional reconstruction model of the tunnel.

[0036] The beneficial effects of this application are as follows:

[0037] This application discloses a three-dimensional reconstruction method suitable for tunnels. The advantages of the mapping method based on mechanical lidar, solid-state lidar, and IMU in this application are:

[0038] (1) 3D reconstruction based on solid-state lidar has a denser point cloud, which can obtain more tunnel details.

[0039] (2) The robustness of 3D reconstruction is improved by using mechanical lidar and IMU as constraints.

[0040] (3) The cost is greatly reduced compared to using a total station for surveying;

[0041] (4) It has a higher degree of automation compared to manual measurement methods;

[0042] (5) The tightly coupled three-dimensional reconstruction method based on mechanical lidar, solid-state lidar and IMU combines the advantages of both and overcomes their disadvantages, and can obtain a highly stable and dense three-dimensional reconstruction model of the tunnel. Attached Figure Description

[0043] To more clearly illustrate the technical solutions of this application, the drawings used in the embodiments are briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0044] Figure 1 This is a step diagram of a three-dimensional reconstruction method for roadways according to an embodiment of this application;

[0045] Figure 2 This is a flowchart of a three-dimensional reconstruction method for roadways according to an embodiment of this application;

[0046] Figure 3 This is a system structure diagram of a three-dimensional reconstruction system suitable for roadways, as described in Embodiment 2 of this application.

[0047] Figure 4 This is a physical image of a three-dimensional reconstruction system suitable for tunnels, as described in Embodiment 2. Detailed implementation method:

[0048] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0049] To make the above-mentioned objectives, features and advantages of this application more apparent and understandable, the application will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0050] Example 1

[0051] In this first embodiment, as Figure 1-2 As shown, a three-dimensional reconstruction method suitable for tunnels specifically includes:

[0052] After all devices (radar and IMU) are turned on, the lidar begins point cloud accumulation, while the IMU directly starts forward propagation to predict the robot's pose. Next, the mechanical lidar filters planar and edge feature points from the raw radar data, denoted as {Lj}, for subsequent state estimation and other processes. Since the previously received lidar point clouds come from different times, we need to project the point clouds from different times into frame-by-frame point cloud frames using the pose transformation estimated by the IMU. The feature points after motion compensation are denoted as {Lk}. However, because the previously extracted features are from different times, they still have errors compared to the map. Therefore, an iterative Kalman filter is then used to optimize the pose estimation. When the filter converges, the optimal pose estimate is obtained, which is the mechanical lidar odometry. This is added to the IMU's forward propagation to correct the IMU's predicted pose transformation. The optimal pose estimate is also used to update the local mechanical map. Figure 1 On one hand, the filtering process is re-integrated to further optimize pose estimation; on the other hand, it is added to the filtering process of the solid-state lidar. Since solid-state lidar can extract fewer environmental features, the feature extraction step for solid-state lidar is omitted here. Instead, backpropagation is performed for motion compensation, and residual calculations are performed between the solid-state lidar point cloud frames and the local map of the mechanical lidar (the lidars have the same start-up time and operating frequency, so at any given time, the mechanical lidar local map will contain the solid-state lidar point cloud frames) to obtain the pose estimation for the solid-state lidar portion. (If the equipment is fixed or there are no large or sharp turns, the pose estimation and odometer results should be the same as those obtained from the mechanical lidar portion). Then, the optimized pose estimation allows the point cloud frames to be registered in the global map. The final solid-state lidar global map is the 3D reconstruction model of the alleyway we need.

[0053] The following sections will explain each step. However, since the processing procedures of the two lidars overlap to some extent, they will not be explained again.

[0054] Specifically as follows:

[0055] (i) Point cloud accumulation: Since the sampling frequency of the raw data of the LiDAR is very high, at 200kHz, it is impossible to process each sampled point directly in real time. Instead, it is necessary to accumulate data points within a certain period of time and then process them all at once. The accumulation time is selected as 20ms, which is the frequency of 50Hz. It is also at this frequency that the odometer and map are output.

[0056] (II) Feature Extraction: From numerous raw radar data centers, feature points (planar feature points and edge feature points) that can be used for subsequent state estimation and other processes are selected {L}. jThe specific feature extraction method references the LOAM feature extraction method, which uses five points before and after a given point as a group to calculate its curvature. The calculation formula is as follows:

[0057]

[0058] Where c is the curvature, S is the set of selected points, i represents the current center point, k is the point in the kth frame, and L is the coordinate system of the mechanical lidar.

[0059] (III) IMU Forward Propagation: A six-axis IMU can obtain the robot's real-time linear and angular accelerations. Therefore, the IMU data can be used for forward propagation to estimate the robot's real-time pose transformation. This should be referenced in the definition in Fastlio. and The specific definitions are as follows:

[0060]

[0061]

[0062]

[0063]

[0064] Where R, R1, R2∈SO(3), a, The exponent is mapped as follows:

[0065]

[0066] Meanwhile, the robot's pose x can be defined as follows, where M refers to the manifold:

[0067]

[0068] in, This refers to the attitude and position of MU within the global framework. For the speed of motion at this time, and For IMU bias, As the gravity vector, its differential dynamic model is as follows:

[0069] (p GI )′=v GI

[0070] (v GI )′=R GI (a m -b a -n a )+g G

[0071]

[0072] (b ω )′=n bω

[0073] (b a )′=n ba

[0074] Where, n bω n ba The noise is Gaussian, and (·)′ represents differentiation, with the symbol ∧ indicating a slant-symmetric matrix. The robot pose transformation derived from the current IMU can be obtained by inversely deriving the integral formula from the differential equation.

[0075] Therefore, the formula for IMU forward propagation can be obtained as follows:

[0076]

[0077] Where, x i For the robot's state defined above, u i Given the input Δt, the interval between two IMU frames, and 0 for process noise set to 0, the equation f is defined as follows:

[0078]

[0079] (iv) Backpropagation: Since the previous points were measured at different times, there is relative motion between them. Therefore, backpropagation is needed to compensate for the motion. The linear interpolation formula for the i-th point in the k-th frame is:

[0080]

[0081] Therefore, combined with the rotation matrix R (which is the R obtained from the IMU forward propagation) GI Then the formula for projection can be obtained, that is:

[0082]

[0083] This transformation converts point clouds at different times into individual point cloud frames. (V) Extended Kalman Filter Module: Since the extracted feature points and the estimates obtained through IMU prediction always have some deviations, a filtering module is needed to optimize the pose estimation to obtain the optimal robot pose estimation. Here, we use an extended Kalman filter to perform this task. The application of extended Kalman filtering is similar to that of Kalman filtering in general, but extended Kalman filtering is suitable for nonlinear systems and consists of two steps: prediction and correction.

[0084] The predictions are as follows:

[0085] ① Prior estimation:

[0086] ② Covariance matrix of prior error:

[0087] in, Let f(·) be the predicted robot pose, and x be the state equation. k-1 u represents the robot's state determined in the previous moment. k-1 For input, 0 indicates that we still consider process noise to be 0, and Q is the noise covariance that follows a Gaussian distribution.

[0088] The correction section is as follows:

[0089] ① Kalman gain:

[0090] ② Posterior estimation:

[0091] ③ Covariance matrix of prediction error:

[0092] in, h(·) represents the output equation.

[0093] Therefore, we can derive the optimal pose estimate of the robot at this time.

[0094] Specifically, map registration includes the formation of mechanical local maps and solid-state global maps. Based on the pose transformation equation optimized by extended Kalman filter, the point cloud frames obtained from different times of scanning can be projected according to the pose transformation equation to stitch them into local / global maps.

[0095] Odometry: Includes mechanical laser odometry and solid-state laser odometry. That is, pose information obtained through an extended Kalman filter.

[0096] It's worth noting that this application integrates mechanical LiDAR and solid-state LiDAR. Solid-state LiDAR generates very dense point clouds, but its field of view is smaller, only about 1 / 5 that of mechanical LiDAR. Mechanical LiDAR, on the other hand, has a wider field of view, reaching 360 degrees, enabling the extraction of more features. As shown in the flowchart, we do not use feature extraction in the solid-state LiDAR part. Instead, we filter the point cloud frames from the solid-state LiDAR and the local map from the mechanical LiDAR to optimize the pose. In other words, we trust the surrounding environment features extracted by the mechanical LiDAR, allowing them to help stabilize the dense map created by the solid-state LiDAR.

[0097] Example 2:

[0098] A three-dimensional reconstruction system suitable for narrow alleyways, such as Figure 3-4 As shown, it includes a point cloud accumulation module, a forward propagation module, a filtering module, a filtering module, and a model building module;

[0099] The point cloud accumulation module is used to accumulate point clouds using mechanical lidar and solid-state lidar respectively, to obtain mechanical lidar point cloud data and solid-state lidar point cloud data.

[0100] The forward propagation module is used to perform forward propagation using the IMU to predict the robot's pose and obtain the pose transformation estimated by the IMU.

[0101] The filtering module is used to filter out planar feature points and edge feature points from the mechanical laser point cloud data using the mechanical lidar, and to project the mechanical laser point cloud data and the solid-state laser point cloud data according to the pose transformation estimated by the IMU to form a point cloud frame;

[0102] The filtering module is used to filter and optimize the pose transformation of the point cloud frame and the IMU-estimated pose to obtain the optimal pose estimate;

[0103] The model building module uses the optimal pose estimation to update the mechanical laser local map, generate and update the solid-state laser global map, and finally form a dense three-dimensional reconstruction model of the tunnel.

[0104] The embodiments described above are merely preferred embodiments of this application and are not intended to limit the scope of this application. Any modifications and improvements made to the technical solutions of this application by those skilled in the art without departing from the spirit of this application shall fall within the protection scope defined by the claims of this application.

Claims

1. A three-dimensional reconstruction method suitable for narrow alleyways, characterized in that the method... include: S1. Accumulate point clouds using mechanical lidar and solid-state lidar respectively to obtain mechanical lidar point cloud data and solid-state lidar point cloud data. S2. Use the IMU for forward propagation to predict the robot's pose and obtain the pose transformation estimated by the IMU. S3. Using the mechanical lidar, planar feature points and edge feature points are selected from the mechanical lidar point cloud data. Based on the pose transformation estimated by the IMU, the mechanical lidar point cloud data and the solid-state lidar point cloud data are projected to form a point cloud frame. S4. Filter and optimize the pose transformation of the point cloud frame and the IMU-estimated pose to obtain the optimal pose estimate; S5. Update the mechanical laser local map using the optimal pose estimation, generate and update the solid-state laser global map, and finally form a dense three-dimensional reconstruction model of the tunnel. The process of filtering optimization specifically includes: An extended Kalman filter is used to optimize the pose estimation equation, including the prediction model and the correction model. The prediction model specifically includes: prior estimation, the formula of which is: The covariance matrix of the prior error is given by the following formula: ;in, For the predicted robot posture, The state equation is... This refers to the robot's state as determined in the previous moment. For input, 0 indicates that process noise is still considered to be 0, and Q is the noise covariance that follows a Gaussian distribution. W ; The correction model includes: Kalman gain, the formula of which is: The posterior estimate is given by the formula: The covariance matrix of the prediction error is given by the following formula: ;in, , , This is the output equation.

2. The three-dimensional reconstruction method for narrow alleyways according to claim 1, characterized in that, The process of obtaining the mechanical laser point cloud data and the solid-state laser point cloud data specifically includes: The data collected from the high-frequency sampling of the mechanical lidar and the solid-state lidar within a preset time period are processed to obtain the mechanical lidar point cloud data and the solid-state lidar point cloud data.

3. The three-dimensional reconstruction method for narrow alleyways according to claim 1, characterized in that, The specific process of obtaining the pose transformation estimated by the IMU includes: The IMU is turned on, and forward propagation is performed using the IMU to predict the robot's pose at different times, thus obtaining the pose transformation estimated by the IMU.

4. The three-dimensional reconstruction method for narrow alleyways according to claim 3, characterized in that, The IMU forward propagation process specifically includes: The data from the six-axis IMU is used for forward propagation to obtain real-time linear and angular accelerations. This IMU data is then used for forward propagation to estimate the robot's real-time pose changes. The formula for IMU forward propagation is as follows: in, That is, the robot's state as defined above. For input, This represents the interval between two IMU frames; 0 indicates that the process noise is set to 0.

5. The three-dimensional reconstruction method for narrow alleyways according to claim 1, characterized in that, The specific process for filtering out planar feature points and edge feature points includes: From the original radar data center, feature points are selected for subsequent state estimation. Using five points before and after a random point in the point cloud data as a group, the curvature is calculated: Where c is the curvature, S is a set of random points, i is the currently centered point, k is the point in the k-th frame, and L is the coordinate system of the mechanical lidar. These are the coordinates of a point in the mechanical lidar coordinate system.

6. The three-dimensional reconstruction method for narrow alleyways according to claim 1, characterized in that, The formation of the dense three-dimensional reconstruction model of the tunnel specifically includes: Based on the optimized pose estimation equation obtained by the extended Kalman filter, the point cloud frames obtained from the mechanical laser point cloud data and the solid-state laser point cloud data at different times are projected according to the pose transformation equation to update the mechanical laser local map, generate and update the solid-state laser global map, and finally form a dense three-dimensional reconstruction model of the tunnel.

7. A three-dimensional reconstruction system suitable for narrow alleyways, characterized in that, It includes a point cloud accumulation module, a forward propagation module, a filtering module, a filtering module, and a model building module; The point cloud accumulation module is used to accumulate point clouds using mechanical lidar and solid-state lidar respectively, to obtain mechanical lidar point cloud data and solid-state lidar point cloud data. The forward propagation module is used to perform forward propagation using the IMU to predict the robot's pose and obtain the pose transformation estimated by the IMU. The filtering module is used to filter out planar feature points and edge feature points from the mechanical laser point cloud data using the mechanical lidar, and to project the mechanical laser point cloud data and the solid-state laser point cloud data according to the pose transformation estimated by the IMU to form a point cloud frame; The filtering module is used to filter and optimize the pose transformation of the point cloud frame and the IMU-estimated pose to obtain the optimal pose estimate; The model building module uses the optimal pose estimation to update the mechanical laser local map, generate and update the solid-state laser global map, and finally form a dense three-dimensional reconstruction model of the tunnel. The process of filtering optimization specifically includes: An extended Kalman filter is used to optimize the pose estimation equation, including the prediction model and the correction model. The prediction model specifically includes: prior estimation, the formula of which is: The covariance matrix of the prior error is given by the following formula: ;in, For the predicted robot posture, The state equation is... This refers to the robot's state as determined in the previous moment. For input, 0 indicates that process noise is still considered to be 0, and Q is the noise covariance that follows a Gaussian distribution. W ; The correction model includes: Kalman gain, the formula of which is: The posterior estimate is given by the formula: The covariance matrix of the prediction error is given by the following formula: ;in, , , This is the output equation.

Images