A coal bunker modeling method based on three-dimensional positioning and two-dimensional mapping

Abstract

A coal bunker modeling method based on three-dimensional positioning and two-dimensional mapping. To improve the efficiency of bunker cleaning at the dock, sensors are used to obtain the internal information of the coal bunker. The three-dimensional lidar information and inertial measurement unit are used to perform real-time positioning on the push rake used for bunker cleaning operations. A fixed rotation sensor composed of a single-line radar and a DC servo motor is used to sense the coal bunker environment. The front-end positioning and serial port rotation angle are used as the basis for single-frame matching to obtain a complete point cloud inside the coal bunker. The superimposed point clouds are subjected to semantic fusion segmentation to classify the coal material point clouds, complete the outer boundary of the point clouds, fill the internal void point clouds, and calculate the volume of the coal material based on the three-dimensional grid point cloud of the coal material. The calculated information is fed back into the control logic of the upper computer to improve the efficiency of the entire push rake bunker cleaning operation.

Description

Technical Field

[0001] The present invention relates to a three-dimensional modeling method for a coal bunker of a coal ship in a port, mainly using positioning based on three-dimensional laser and rotating two-dimensional modeling technology of a single-line radar to perform point cloud reconstruction and analysis on the bunker environment. Background Art

[0002] In recent years, with the change of the coal supply and demand pattern in China, the scope of the sea coal transportation service of "domestic coal transported from the north to the south + imported foreign coal" in China has expanded from the seven southern provinces to the Bohai Rim region and the four provinces in the middle reaches of the Yangtze River. Facing the increasing intensity of energy structure adjustment, the port coal transportation still maintains stable growth, and the pattern of coal discharging ports is accelerating adjustment, which effectively guarantees the national energy demand and the healthy development of economic construction. Under the existing coal transportation mode, as a very important transfer node, the port plays an important role in coal transportation. At the same time, facing the pressure of coal transportation, the port operation capacity is severely tested.

[0003] Currently, the main tools for the main hold cleaning operation in terminal operations are ship unloaders and push rakes. Generally speaking, first, the ship unloader operated by manual grabs the coal pile in the coal bunker onto the belt conveyor, and the surrounding coal materials of the grabbed part will slide down freely due to their angle of repose; until the coal materials accumulate into an environment with a relatively large slope, at this time, the crane lifts the push rake into the bunker, and the push rake operated by manual pushes out the surrounding coal materials with a high slope and aggregates them at the fixed center, and then the ship unloader further grabs them; finally, until the last stage, the push rake conducts a full hold cleaning, and the remaining coal materials are grabbed by the ship unloader operated by manual.

[0004] In the above hold cleaning operation, the staff of the ship unloader mainly repeats the grabbing and raking of the central area inside the bunker based on visual inspection, and the scattered coal materials in other areas are gathered by the push rake working inside the bunker. Moreover, due to the limitations of non-closed-loop operation and the ability of manual visual inspection, the entire push rake operation does not have the ability to detailedly perceive the internal environment of the bunker and three-dimensional reconstruction, resulting in the inability to calculate the volume of the remaining coal materials and the remaining working time of the hold cleaning operation. On the other hand, due to the influence of weather such as rainy, snowy, and icy days, and the requirements of full-day operation, the accuracy of the perception method based on manual visual inspection is reduced, and the efficiency will also be further reduced. <B

[0005] Therefore, in order to solve the three-dimensional positioning problem, Yu Xinghu proposed a three-dimensional positioning method based on a rotating two-dimensional lidar (Yu Xinghu, Meng Lingbo. Three-dimensional point cloud reconstruction method based on lidar [P]. Chinese Patent: CN110223379A, 2019-09-10). However, in the working environment of the coal bunker, most of the environment is coal materials that are constantly moving with the operation. Therefore, this method does not handle the influence of dynamic objects on the positioning and matching, and is not applicable to working conditions with large movement amplitude and long working hours.

[0006] Meanwhile, to solve the problem of three-dimensional reconstruction, Li Chang'an proposed a three-dimensional reconstruction technology based on a rotating two-dimensional lidar (Li Chang'an, Yao Tongjian, Shi Wei, Pan Pan, Li Jingyu, Ma Lei. A method and device for stockpile modeling [P]. Chinese Patent: CN106094702B, 2020-07-07). This method is based on three-dimensional reconstruction on a plane under a fixed environment. However, due to the unfixed sizes and quantities of coal ship coal bunkers at the dock, it is impossible to install a fixed rotating two-dimensional lidar. Summary of the Invention

[0007] The present invention overcomes the problems in the prior art and proposes a three-dimensional modeling of a coal bunker based on three-dimensional positioning and a rotating single-line lidar.

[0008] The technical solution adopted by the present invention to solve the problems in the prior art is as follows:

[0009] A three-dimensional reconstruction method based on three-dimensional positioning and a rotating single-line lidar, which includes the following steps:

[0010] 1) Three-dimensional positioning: For the complex cabin environment and high-intensity working mode, a positioning unit of a 32-line lidar and an IMU inertial navigation is used to perform real-time optimization calculation on the pose of the carrier, specifically including:

[0011] 1.1) First, perform distortion removal on the input point cloud, match the time stamps of the motion estimation and lidar information input by the IMU sensor, and then perform interpolation processing and correction on the lidar point cloud according to the uniform motion model.

[0012]

[0013] P k+i ,P k+i ' represents the i-th distorted point and corrected point in the k-th frame, w, j are the number of motion estimation frames of all IMUs in the k-th frame and the j-th frame where the current point is located, T k+j ,T k+j+1 is the change matrix of the current k-th frame superimposed with the change matrix of the current IMU motion estimation and the change matrix of the next frame.

[0014] 1.2) Then, divide the space of the 32-line lidar point cloud input by the lidar to generate a Scan Context, and divide the spatial plane coordinates {hor, ver} to which each point belongs, reducing the three-dimensional point cloud space to a two-dimensional data format.

[0015]

[0016]

[0017] where x i,y i ,z i is the three-dimensional coordinate of the i-th point in the current frame, Δα, Δβ are the horizontal and vertical division resolutions respectively, hor i ,ver i The coordinates of the plane where the i-th point in the current frame belongs after processing.

[0018] 1.3) Traverse the two-dimensional plane space and determine the category of the point by calculating the curvature of the neighborhood around the current point and the angle between the horizontal line and the vertical line, and setting the corresponding angle.

[0019]

[0020] Where P i,j Represents the three-dimensional coordinate point of the spatial plane coordinate {i, j}, n and k represent the number of points in the surrounding area and the cumulative subscript, and the category of the current point is determined by comparing θ with the set threshold.

[0021] 1.4) The classified point cloud is then clustered based on category and Euclidean distance: one of the unvisited points is selected as the initial point, and a search is started from this point for a nearby point cloud within a certain radius. Points that meet the preset category and Euclidean distance conditions are marked as points of this category and further clustered with the neighboring points as the center of the circle. Otherwise, they are marked as noise points. At this time, the number of marked points is determined. If it is less than the threshold, it is discarded. If it is greater than the threshold, a new starting point is selected from the unmarked points to start a new round of clustering.

[0022] 1.5) In the feature extraction and matching step, the XY plane is divided into six equal parts. The main plane feature points of each part are selected from the pre-processed static wall point cloud according to the following curvature calculation formula.

[0023]

[0024] Where c represents the domain curvature of the current feature point, n represents the number of domain points, Represents the i-th 3D point under the k-th laser beam in the current L point cloud frame.

[0025] 1.6) After extracting the feature points, the distance between the feature points is determined by matching the feature points between the two frames. A nonlinear constraint equation system is constructed by combining these equations, and this equation system is solved using the Levenberg-Marquardt method to obtain the solved radar pose. Furthermore, a nonlinear constraint equation system is constructed by matching the frames with the map, and this is again solved using the Levenberg-Marquardt method to obtain the optimized motion estimate.

[0026] 1.7) Save the key frames selected from the historical point cloud information as two-dimensional coordinates into the historical information, and delete the adjacent historical information near the current pose according to the front-end matching result each time, so as to remove the outdated information of the historical frames in the dynamically changing scene and ensure the dynamic update of the historical frames.

[0027] 2) Two-dimensional reconstruction, using the pose calculated in the rotating motor, the calibrated single-line radar, and the three-dimensional positioning, specifically including:

[0028] 2.1) Due to the superposition of pose attitudes, it is impossible to only use IMU data to interpolate and de-distort the point cloud. Therefore, according to the uniform motion model, the pose change matrix calculated by the multi-line radar optimization formula below is used to de-distort the single-line radar point cloud.

[0029]

[0030] Where k+i P k represents the point coordinate with subscript i in the current k-th frame, k P k ' represents the point coordinate of subscript i corrected to the starting time of the k-th frame, represents the inter-frame pose change matrix from the starting subscript of the k-th frame to subscript i calculated by the multi-line radar.

[0031] 2.2) Since the vehicle body motion distortion has been removed in the previous step, the second part is to remove the single-line radar distortion problem caused by the uniform rotation of the motor. By synchronously feeding back data through the serial port, the pose transformation matrix between the end of the current frame and the start of the next frame can be estimated, and then interpolation processing is performed on each point through the time stamp. The rotation matrix corresponding to each point within the frame can be obtained by the following formula, and through matrix correction, the point is projected onto the absolute world coordinate system:

[0032]

[0033] P k,i represents the point with subscript i in the k-th frame of the single-line radar, P k,i ' represents the point with subscript i in the k-th frame corrected to the starting time of the k-th frame, represents the correction matrix for rotating angle around the rotation axes of the motor and the radar.

[0034] 2.3) Finally, perform coordinate transformation according to the de-distorted point cloud and the motion estimation matched with the time stamp, and output the superimposed point cloud map.

[0035] P t,i = T t P t,i (8)

[0036] Pt,i and P t,i ' represents the pose and the transformed pose of the i-th laser point, T t represents the interpolation transformation matrix of the relative time t corresponding to the i-th laser point.

[0037] 3) Back-end analysis: Perform back-end processing on the single-line lidar point cloud map after superposition correction to achieve semantic recognition of the point cloud, filling of coal material, and calculation of volume parameters; specifically including:

[0038] 3.1) First, since the point cloud data is huge and a dense three-dimensional space of points needs to be searched quickly, first construct a KD tree data structure for the input point cloud.

[0039] 3.2) Due to the influence of hardware and environmental factors on the imaging results, use the Moving Least Squares method to smooth each point.

[0040] 3.3) Select an appropriate resolution, assign spatial plane coordinates {hor, ver} to each point in the dense point cloud, and sort the entire point cloud according to the plane coordinates.

[0041] 3.4) Traverse the two-dimensional plane space, use the prior knowledge that the angle of repose of the coal material does not exceed 45 degrees to set an appropriate angle threshold, and classify the current point by calculating the relationship between the normal angle of the neighborhood around point P i,j and the Euclidean distance in the XY plane, that is, wall points, top layer points, coal material points, and noise points.

[0042] 3.5) Because the resolution of the single-line lidar is very high and the distribution of coal material is random, use a clustering method based on category and Euclidean distance in the two-dimensional plane to correct the category error of the current point: Set the first point of the current vel as the starting point, mark this point as a clustering point according to the category and Euclidean distance, and continue to judge downward from this point; if the conditions are not met, judge whether the number of clustering points meets the clustering number threshold requirement. If not, discard this clustering; after completing the clustering operation, judge the category of the neighborhood of each clustering result by the following formula weight * number to determine the category of all points in the current clustering.

[0043]

[0044] where w i represents the basis of the clustering weight of the current i-th point, S i,w represents calculating the weight of the current i-th point using w, and k represents the weight coefficient.

[0045] 3.6) The category of a point is determined using the information of all points on a single line. Next, the category of each point is determined using the entire point cloud information: all points are traversed, and based on the constructed KD-tree data structure, the surrounding neighborhood points of the point are searched using a three-dimensional spatial structure within a certain range, and the belonging category of the point is corrected through the neighborhood information.

[0046] 3.7) The processing of the occupancy grid map mainly uses the mapping method of the grid map to determine the current state of the grid, so as to achieve the effects of dynamically updating the point cloud map, removing the ghost points generated by dynamic objects, and reducing the error of point cloud mapping. After obtaining the superimposed and classified point cloud, each frame of point cloud is used as prior data and input into the grid map:

[0047] data = {x1, T1, x2, T2, …, x n , T n} (10)

[0048] data represents the information of the current point cloud frame, and x n , T n represents the coordinate and pose of the nth point.

[0049] To generate an occupancy grid map that maximizes the probability of conforming to the information data of the current frame and historical frames:

[0050] m * = arg max m P(m|data) (11)

[0051] where P(m|data) is the probability of the occupancy grid map under the current prior data data, and m * represents the map that maximizes the probability of conforming to the current frame and historical frames.

[0052] 3.8) When superimposing the two-dimensional radar information, the probability value of the grid belonging to the occupancy grid map is updated simultaneously:

[0053]

[0054] where is the model observation value, is the fixed update step size, S - and S + represent the prior grid probability and the updated grid probability. The maximum likelihood estimate of each grid in the occupancy grid map is updated synchronously according to formula (12), the current frame point cloud data in formula (10) is updated simultaneously, and the maximum grid probability map in formula (11) is constructed.

[0055] 3.9) After updating the grid map constructed by using the current frame information to update the historical frame information, set the occupancy probability threshold, and determine whether the points in the occupied grid belong to the occupied points through the grid probabilities in the grid map. If so, save them as points of the corresponding category. If not, mark the points in the idle grid as noise points and process and remove them.

[0056] 3.10) However, since the laser emitted by the single-line radar is in the shape of a beam, when encountering coal material with a certain height slope, a laser scanning blind area will be formed behind it. Therefore, the overlapping point cloud of the coal material is not complete and needs to be further filled. Before filling, it is necessary to determine the outer boundary of the point cloud. Use formula (2) to determine the outer boundary search range, and use KNN outlier processing + mean filtering to process the size of the outer boundary and fill the missing part of the boundary to obtain a complete outer boundary.

[0057] 3.11) After obtaining the classified and optimized point cloud, construct a two-dimensional grid matrix, project all coal material points into the two-dimensional grid matrix, save the Z values of all points inside the same grid as a one-dimensional array, set the number of neighboring points nums and the variance threshold threshold, search for the neighboring points inside the boundaries of nums through the KD tree, and calculate the mean d of the sum of the distances from the current point to all neighboring points i and the standard deviation stddev:

[0058]

[0059]

[0060] By judging d i > threshold * stddev to determine that this point is an outlier, mark this point, and remove it from the one-dimensional array, and then determine the minimum Z value inside the grid as the current grid height value;

[0061] 3.12) Use the CV operator of the two-dimensional grid to search for the neighboring points of the blind area grid, and fill the height value of the blind area grid with the distance weight * neighboring point grid height value using formula (9):

[0062]

[0063] S i,d represents using the distance d to calculate the weight of the current i point, Z nums represents the height of the current neighboring point grid, and E represents the height value of the blind area grid.

[0064] 3.13) After processing, project the two-dimensional grid back into the three-dimensional space, and use the KD tree space division to perform grid processing on the three-dimensional point cloud. For the obtained two-dimensional grid height data of the complete coal material, the total volume of the coal material is the sum of the volumes of all grids:

[0065]

[0066] where size x , size y represents the resolution of grid division, and E i represents the height of the current i-th grid, and V represents the accumulated grid volume.

[0067] The advantages of the present invention are as follows:

[0068] 1. Positioning advantage. The absolute positioning method taking the Global Positioning System (GPS) as an example has relatively large measurement errors, is not friendly to low-level signals, and cannot meet the high-precision engineering requirements. The relative positioning taking wheel encoders and IMU sensors as examples has problems such as long-term cumulative errors and is easily affected by magnetic fields, etc., and cannot well solve the long-time positioning requirements. Therefore, the relative positioning method using a multi-line lidar to sense the coal bunker environment and synchronously position and map can well meet the engineering requirements of high precision and closed-loop calculation.

[0069] 2. Mapping advantage. Due to the characteristics of low reflectivity and high absorption of coal materials, part of the point cloud obtained by multi-line radar scanning will be absorbed by the coal materials, resulting in an incomplete overall point cloud. And because the resolution of the multi-line radar in the SCAN dimension is too low, the multi-line radar point cloud is sparse for the overall coal bunker. The three-dimensional reconstruction uses a rotating single-line radar, the horizontal resolution can be adjusted by the angular velocity of the motor, and the vertical resolution can reach 0.06, and it can completely three-dimensionally reconstruct the point cloud map of the overall coal bunker.

[0070] 3. Back-end analysis. In the back-end analysis, point cloud classification, fusion, and grid processing are mainly used to convert the three-dimensional reconstructed point cloud map into a semantic map, and the influence of dynamic object noise is removed through grid map processing, further optimizing the error of point cloud and pose matching, and realizing long-term dynamic map update. And the volume, elevation map and other visualization information required by the upper computer are calculated through the processed point cloud, improving the operation efficiency of the upper computer for the ship unloader to clear the bunker. Brief Description of the Drawings

[0071] Figure 1 is a schematic diagram of the process structure of the present invention.

[0072] Figure 2 is the de-distorted three-dimensional point cloud map of the warehouse established by the present invention.

[0073] Figure 3 is the three-dimensional semantic map of the coal bunker established by the present invention.

[0074] Figure 4 is the distribution map of the outer boundary of the coal bunker of the present invention

[0075] Figure 5It is the outer boundary correction diagram of the present invention

[0076] Figure 6 It is the point cloud diagram of the coal material obtained by cutting the present invention

[0077] Figure 7 It is the three-dimensional grid diagram of the coal material established by the present invention Specific implementation manner

[0078] Referring to Figures 1 to 7 , a coal bunker modeling method based on three-dimensional positioning and two-dimensional mapping, as Figure 1 shown, receive two radar sensing information and IMU measurement information through the serial port and network port, utilize the three-dimensional laser information of the Ouster radar and the IMU real-time measurement information, and obtain the real-time pose of the pusher through three-dimensional mapping and positioning; utilize the three-dimensional laser information of the Quanergy radar, the real-time pose calculated by three-dimensional scanning and mapping, the rotation angle of the YZ-ACSD608 motor obtained by Modbus communication, and the IMU real-time measurement information to construct the internal environment of the coal ship coal bunker and establish a point cloud map of the coal material warehouse

[0079] The specific steps are as follows

[0080] 1) Three-dimensional positioning: Adopt a positioning unit of a 32-line lidar and IMU inertial navigation to perform real-time optimization calculation on the pose of the carrier, specifically including

[0081] 1.1) Remove the influence of dynamic distortion by using synchronous IMU interpolation according to the uniform motion model

[0082]

[0083] P k+i ,P k+i represent the i-th distorted point and corrected point in the k-th frame, w, j are the number of motion estimation frames of all IMUs in the k-th frame and the j-th frame where the current point is located, T k+j ,T k+j+1 is the change matrix of the current k-th frame superimposed with the change matrix of the current IMU motion estimation and the change matrix of the next frame

[0084] 1.2) Project the three-dimensional space points onto a two-dimensional plane and divide the lidar point cloud

[0085]

[0086]

[0087] where x i ,y i ,z iis the three-dimensional coordinate of the i-th point in the current frame, Δα, Δβ are the horizontal and vertical division resolutions respectively, hor i ,ver i The coordinates of the plane where the i-th point in the current frame belongs after processing.

[0088] 1.3) Traverse the two-dimensional plane space and calculate the angle between the neighborhood curvature and the horizontal and vertical lines to classify the points:

[0089]

[0090] Where P i,j Represents the three-dimensional coordinate point of the spatial plane coordinate {i, j}, n and k represent the number of points in the surrounding area and the cumulative subscript, and the category of the current point is determined by comparing θ with the set threshold.

[0091] 1.4) The classified point cloud is then clustered in three dimensions based on category and Euclidean distance. This removes noise points based on category and Euclidean distance, and the category of each point can be corrected based on category clustering.

[0092] 1.5) In the feature extraction and matching steps, the XY plane is divided into six equal parts to remove the uneven feature points caused by the polymorphism of the coal material environment. The main plane feature points of each part are selected from the preprocessed static wall point cloud according to the curvature calculation formula below.

[0093]

[0094] Where c represents the domain curvature of the current feature point, n represents the number of domain points, Represents the i-th 3D point under the k-th laser beam in the current L point cloud frame.

[0095] 1.6) After extracting the feature points, the distance between the feature points is determined by matching the feature points between the two frames. A nonlinear constraint equation system is constructed by combining these equations, and this equation system is solved using the Levenberg-Marquardt method to obtain the solved radar pose. Furthermore, a nonlinear constraint equation system is constructed by matching the frames with the map, and this is again solved using the Levenberg-Marquardt method to obtain the optimized motion estimate.

[0096] 1.7) The keyframes selected from the historical point cloud information are saved as 2D coordinates in the historical information, and the adjacent historical information near the current posture is deleted based on each front-end matching result to ensure the dynamic update of the historical frame.

[0097] 2) 2D reconstruction: Using the information from the rotating single-line radar and the pose calculated in 3D positioning, a dense point cloud map of the coal bunker interior is constructed. This includes:

[0098] 2.1) Due to the superposition of pose and attitude, it is impossible to use only IMU data to interpolate and distort the point cloud. Therefore, according to the uniform motion model, the pose change matrix calculated by the multi-line radar optimization formula below is used to correct the distortion of the single-line radar point cloud:

[0099]

[0100] Where k+i P k represents the coordinate of the point with subscript i in the current k-th frame, k P k ' represents the coordinate of the point with subscript i corrected to the starting time of the k-th frame, represents the inter-frame pose change matrix from the starting subscript to the i-th subscript of the k-th frame calculated by the multi-line radar.

[0101] 2.2) By feeding back data through the synchronous serial port, estimate the pose transformation matrix between the end of the current frame and the start of the next frame. The rotation matrix corresponding to each point within the frame can be obtained using interpolation processing:

[0102]

[0103] P k,i represents the point with subscript i in the k-th frame of the single-line radar, P k,i ' represents the point with subscript i in the k-th frame corrected to the starting time of the k-th frame, represents the correction matrix for rotating by angle around the motor and radar rotation axes.

[0104] 2.3) Finally, perform coordinate transformation based on the motion estimation of the undistorted point cloud and timestamp matching, and output the superimposed point cloud map.

[0105] P t,i ’ = T t P t,i (8)

[0106] P t,i and P t,i ' represent the pose and the transformed pose of the i-th laser point, T t represents the interpolation transformation matrix corresponding to the relative time t of the i-th laser point.

[0107] After removing the distortion, the point cloud map of the warehouse is obtained by the rotation of the motor as shown in Figure 2 . It can be seen that after the motor rotates to a certain speed, the entire constructed point cloud map still maintains an accurate superimposed form.

[0108] 3) Back-end analysis: Perform back-end processing on the superimposed and corrected single-line radar point cloud map to achieve semantic recognition of the point cloud, filling of coal materials, and calculation of volume parameters. Specifically, it includes:

[0109] 3.1) Since the constructed point cloud data is huge and it is necessary to quickly search for dense three-dimensional space points, the input point cloud is first used to construct a KD tree data structure.

[0110] 3.2) Due to the influence of hardware and environmental factors on the imaging results, the moving least squares method is used to smooth each point.

[0111] 3.3) Select an appropriate resolution, and assign spatial plane coordinates {hor, ver} to each point in the dense point cloud according to formulas (2) and (3), and sort the entire point cloud according to the plane coordinates.

[0112] 3.4) By calculating the relationship between the normal angle of the neighborhood around point P i,j and the Euclidean distance from the XY plane, the category of the current point is attributed, that is, wall point, top layer point, coal material point, and noise point.

[0113] 3.5) Use a method based on category and Euclidean distance in the two-dimensional plane to complete clustering, and judge the weight * number of clusters of the neighborhood category of each clustering result to correct the category error of the current point.

[0114]

[0115] where w i represents the clustering weight basis of the current point i, S i,w represents the weight of the current point i calculated using w, and k represents the weight coefficient.

[0116] 3.6) Based on the constructed KD tree data structure, use the global point cloud category information to correct the attribution category of the point through the neighborhood category information.

[0117] 3.7) Use a grid map to optimize the point cloud map. After obtaining the point cloud after superposition classification, each frame of the point cloud is used as prior data and input into the grid map:

[0118] data = {x1, T1, x2, T2, …, x n , T n} (10)

[0119] data represents the current point cloud frame information, x n , T n represents the coordinate and pose of the nth point.

[0120] To generate a grid map that maximizes the probability of the data conforming to the current frame and historical frame information:

[0121] m * = arg maxm P(m|data) (11)

[0122] where P(m|data) is the grid map probability under the current prior data, and m * represents the maximum probability map that conforms to the current frame and historical frames.

[0123] 3.8) When superimposing two-dimensional radar information, simultaneously update the grid probability value of the grid to which the grid map belongs:

[0124]

[0125] where is the model observation value, and is a fixed update step size, S - and S + represent the prior grid probability and the updated grid probability. Synchronously update the maximum likelihood estimate of each grid in the grid map according to formula (12), simultaneously update the current frame point cloud data in formula (10), and construct the maximum grid probability map in formula (11).

[0126] 3.9) Use the grid probability to remove the noise points formed by the mapping due to the front-end odometer matching error, and correct all grid category points. Finally, obtain the semantic segmentation map as Figure 3 shown, where the red point cloud represents the upper plane, the green point cloud represents the cabin wall, and the purple point cloud represents the coal in the coal bunker.

[0127] 3.10) However, since the laser emitted by the single-line radar is in the shape of a beam, when encountering coal with a certain height gradient, a laser scanning blind area will be formed behind it. Therefore, as Figure 3 shown, the point cloud in the lower red area is incomplete and needs to be further filled. Before filling, it is necessary to determine the outer boundary of the point cloud. Use formula (2) to determine the outer boundary search range, as Figure 4 shown in the point cloud outer boundary point line diagram. The outer boundary range is also incomplete and has many burr lines. Use KNN outlier processing + mean filtering to optimize the error and fill the partially missing boundary to obtain a complete outer boundary and achieve Figure 5 the complete outer boundary effect.

[0128] 3.11) Construct a two-dimensional grid matrix, project all coal points into the two-dimensional grid matrix, set the number of neighboring points nums and the variance threshold threshold, and calculate the mean d i of the sum of the distances from the current point to all neighboring points and the standard variance stddev:

[0129]

[0130]

[0131] By judging that d i > threshold * stddev, it is determined that this point is an outlier, mark this point, and remove it from the one-dimensional array, then determine the minimum Z value inside this grid as the current grid height value.

[0132] 3.12) Use the CV operator of the two-dimensional grid to search for the neighboring points of the blind area grid, and fill the height value of the blind area grid with the formula (9) distance weight * neighboring point grid height value:

[0133]

[0134] S i,d represents calculating the weight of the current point i using the distance d, Z nums represents the height of the current neighboring point grid, and E represents the height value of the blind area grid.

[0135] Obtain the complete coal material segmentation Figure 6 As shown, the coal material area is complete and smooth.

[0136] 3.13) After processing, project the two-dimensional grid back into the three-dimensional space, and perform grid processing on the three-dimensional point cloud using KD-tree space partitioning. The obtained results are as Figure 7 shown. For the three-dimensional grid height data of the complete coal material obtained, the sum of the volumes of all grids is the total volume of the coal material:

[0137]

[0138] where size x , size y represents the resolution of the grid division, E i represents the height of the current i-th grid, and V represents the cumulative grid volume.

[0139] The embodiments of the present invention are not limited to the three-dimensional modeling of dock ship cabins, and are also applicable to scenarios such as indoor warehouses and underground coal mines.

Claims

1. A coal bunker modeling method based on three-dimensional positioning and two-dimensional mapping, characterized in that: The coal bunker modeling method comprises the following steps: 1) 3D positioning: A 32-line laser radar and IMU positioning unit are used to perform real-time optimization calculations on the carrier's position and posture, including: 1.1) Using synchronized IMU interpolation to remove the effects of dynamic distortion based on the uniform velocity model: P k+i , P k+i ' represents the i-th distorted point and corrected point within the k-th frame, w, j are the number of motion estimation frames of all IMUs within the k-th frame and the j-th frame where the current point is located, T k+j , T k+j+1 is the change matrix of the current k-th frame superimposed with the change matrix of the motion estimation of the current IMU and the change matrix of the subsequent frame; 1.2) Project the 3D space points onto a 2D plane and perform plane segmentation on the LiDAR point cloud: where x i , y i , z i are the three-dimensional coordinates of the i-th point in the current frame, and Δα and Δβ are the horizontal and vertical division resolutions respectively, hor i , ver i are the coordinates of the belonging plane after processing the i-th point in the current frame; 1.3) Traverse the two-dimensional plane space and calculate the angle between the neighborhood curvature and the horizontal and vertical lines to classify the points: Where P i,，j represents the three-dimensional coordinate point of the spatial plane coordinates {i, j}, n and k represent the number of surrounding neighborhood points and the cumulative subscript, and the category of the current point is determined by comparing θ with the set threshold; 1.4) The classified point cloud is then clustered based on category and Euclidean distance: a point is selected from unvisited points as the initial point, and a search is started from this point within a certain radius of the adjacent point cloud. Points that meet the preset category and Euclidean distance conditions are marked as points of this category and further clustered with the neighboring points as the center. Otherwise, they are marked as noise points. The number of marked points is then determined. If it is less than a threshold, it is discarded. If it is greater than the threshold, a new starting point is selected from the unmarked points to start a new round of clustering. In this way, according to the category and Euclidean distance conditions, the noise points are removed, and the category of each point can be corrected based on category clustering; 1.5) In the feature extraction and matching step, the XY plane is divided into six equal parts to remove the uneven feature points caused by the multi-state coal environment. The main plane feature points of each part are selected from the pre-processed static wall point cloud according to the following curvature calculation formula; where c represents the local curvature of the current feature point, and n represents the number of local points. represents the i-th 3D point under the k-th laser beam of the current L point cloud frame; 1.6) After extracting the feature points, the distance between the feature points is determined by matching the feature points between the two frames. A nonlinear constraint system is constructed by this system, and this system is solved using the Levenberg-Marquardt method to obtain the solved radar pose. Furthermore, a nonlinear constraint system is constructed by matching the frames and the map, and this is again solved using the Levenberg-Marquardt method to obtain the optimized motion estimate. 1.7) The keyframes selected from the historical point cloud information are saved as 2D coordinates in the historical information, and the adjacent historical information near the current posture is deleted based on each front-end matching result to ensure the dynamic update of the historical frame; 2) 2D reconstruction: Using the information from the rotating single-line radar and the pose calculated in 3D positioning, a dense point cloud map of the coal bunker interior is constructed. This includes: 2.1) Due to the superposition of poses and attitudes, it is not possible to use only IMU data to interpolate and dedistort the point cloud. Therefore, based on the uniform velocity model, the pose change matrix calculated by multi-line radar optimization is used to dedistort the single-line radar point cloud: Among them k+i P k represents the coordinates of the point with subscript i in the current k-th frame, k P k ' represents the coordinates of the point with subscript i corrected to the starting time of the k-th frame, represents the inter-frame pose change matrix from the starting subscript of the k-th frame calculated by the multi-line radar to the subscript i; 2.2) Using the synchronous serial port feedback data, the attitude transformation matrix between the end of the current frame and the beginning of the next frame is estimated. Interpolation processing can be used to obtain the rotation matrix corresponding to each point in the frame: P k,i represents the point with subscript i in the k-th frame of the single-line radar, P k,i ' represents the point with subscript i in the k-th frame corrected to the point at the start time of the k-th frame, represents the rotation around the motor and the rotation axis of the radar angle correction matrix; 2.3) Finally, coordinate system transformation is performed based on the dedistorted point cloud and motion estimation of timestamp matching, and the superimposed point cloud map is output: P t,i ′ = T t P t,i (8) P t,i and P t,i ' represent the pose and the transformed pose of the i-th laser point, and T t represents the interpolation transformation matrix corresponding to the relative time t of the i-th laser point; 3) Back-end analysis: Back-end processing of the superimposed and rectified single-line radar point cloud map to achieve semantic recognition of the point cloud, coal filling, and volume parameter calculation, including: 3.1) Since the constructed point cloud data is huge and dense three-dimensional space points need to be searched quickly, the input point cloud is first used to construct a KD-tree data structure; 3.2) Due to the influence of hardware and environmental factors on the imaging results, the Moving Least Squares method is used to smooth each point; 3.3) Select an appropriate resolution, and assign spatial plane coordinates {hor, ver} to each point in the dense point cloud according to formulas (2) and (3), and sort the entire point cloud according to the plane coordinates; 3.4) By calculating the relationship between the normal angle of the neighborhood around point P i,j and the Euclidean distance in the XY plane, the category of the current point is attributed, namely, wall point, top layer point, coal material point, and noise point; 3.5) Use a method based on category and Euclidean distance in the two-dimensional plane to complete clustering, and judge the neighborhood category of each clustering result by the following formula: weight * number of clusters to correct the category error of the current point; where w i represents the clustering weight basis of the current i-th point, and S i,w represents the weight of the current i-th point calculated using w, and k represents the weight coefficient; 3.6) Based on the constructed KD-tree data structure, use the global point cloud category information to correct the belonging category of the point through the neighborhood category information; traverse all points, based on the constructed KD-tree data structure, search for the surrounding neighborhood points of the point in a three-dimensional space structure within a certain range, and correct the belonging category of the point through the neighborhood information; 3.7) Use a grid map to optimize the point cloud map. After obtaining the superimposed and classified point cloud, each frame of the point cloud is used as prior data and input into the grid map: data = {x1, T1, x2, T2, …, x n , T n} (10) The data represents the current point cloud frame information, x n , T n represents the coordinates and pose of the nth point; To generate a grid map that maximizes the probability of conforming to the information data of the current frame and historical frames: m * = arg max m P(m|data) (11) where P(m|data) is the grid map probability under the current prior data, and m * represents the maximum probability map that conforms to the current frame and historical frames; 3.8) When superimposing two-dimensional radar information, simultaneously update the grid probability value of the grid map; Among them is the model observation value, which is a fixed update step size, S - and S + represent the prior grid probability and the updated grid probability; synchronously update the maximum likelihood estimate of each grid in the grid map according to formula (12), update the current frame point cloud data in formula (10) at the same time, and construct the maximum grid probability map in formula (11); 3.9) Use the grid probability to remove the noise points formed in the mapping due to the matching error of the front-end odometer, and correct all grid category points; after updating the grid map constructed by the historical frame information using the current frame information, set an occupancy probability threshold, and use the grid probability in the grid map to judge whether the points in the occupied grid belong to the occupied points. If so, save them as corresponding category points. If not, mark the points in the idle grid as noise points and remove them; 3.10) However, since the laser emitted by the single-line radar is in the shape of a beam, when encountering coal material with a certain height gradient, a laser scanning blind area will be formed behind it, which needs to be further filled; before filling, the outer boundary of the point cloud needs to be determined, and the outer boundary search range is determined using formula (2); use KNN outlier processing + mean filtering to optimize the error and fill in some missing boundaries to obtain a complete outer boundary; 3.11) Construct a two-dimensional grid matrix, project all coal material points into the two-dimensional grid matrix, set the number of neighboring points nums and the variance threshold threshold, and calculate the mean value d of the sum of the distances from the current point to all neighboring points i and the standard deviation stddev: By judging that d i > threshold * stddev to determine that this point is an outlier, mark this point, and remove it from the one-dimensional array, and then determine the minimum Z value inside the grid as the current grid height value; 3.12) Use the CV operator of the two-dimensional grid to search for the neighboring points of the blind area grid, and fill the height value of the blind area grid with the distance weight * neighboring point grid height value using formula (9); S i,d represents calculating the weight of the current point i using the distance d, Z nums represents the grid height of the current neighboring point, and E represents the blind area grid height value; 3.13) After processing, project the two-dimensional grid back into the three-dimensional space, and use the KD-tree space division to perform grid processing on the three-dimensional point cloud; for the obtained three-dimensional grid height data of the complete coal material, the total volume of the coal material is obtained by accumulating the volumes of all grids: where size x , size y represents the resolution of the grid division, E i represents the height of the current i-th grid, and V represents the accumulated grid volume.

2. The coal bunker modeling method based on three-dimensional positioning and two-dimensional mapping according to claim 1 is characterized in that Three-dimensional lidar information and inertial measurement unit are used to perceive the coal bunker environment, perform feature semantic segmentation and clustering preprocessing on the information, and use the pose Euclidean distance as a basis in the historical information to update the global map in real time, so as to perform real-time positioning on the pusher for the bunker cleaning operation.

3. The coal bunker modeling method based on three-dimensional positioning and two-dimensional mapping according to claim 1, characterized in that A fixed rotation sensor composed of a single-line radar and a DC servo motor is used to sense the coal bunker environment. The pose result of the front-end three-dimensional positioning and the rotation angle fed back by the serial port are used as the basis for single-frame matching to obtain the complete point cloud inside the coal bunker.

4. The coal bunker modeling method based on three-dimensional positioning and two-dimensional mapping according to claim 1, characterized in that Semantic fusion segmentation is performed on the superimposed point cloud map results to classify the coal material point cloud, complete the outer boundary of the point cloud, fill the internal void point cloud, and calculate the volume of the coal material based on the three-dimensional grid map information of the coal material.

Images