CN114581619A - Coal bunker modeling method based on three-dimensional positioning and two-dimensional mapping - Google Patents
Coal bunker modeling method based on three-dimensional positioning and two-dimensional mapping Download PDFInfo
- Publication number
- CN114581619A CN114581619A CN202210177469.7A CN202210177469A CN114581619A CN 114581619 A CN114581619 A CN 114581619A CN 202210177469 A CN202210177469 A CN 202210177469A CN 114581619 A CN114581619 A CN 114581619A
- Authority
- CN
- China
- Prior art keywords
- point
- dimensional
- grid
- frame
- points
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 239000003245 coal Substances 0.000 title claims abstract description 93
- 238000000034 method Methods 0.000 title claims abstract description 37
- 238000013507 mapping Methods 0.000 title claims abstract description 14
- 239000000463 material Substances 0.000 claims abstract description 31
- 238000004140 cleaning Methods 0.000 claims abstract description 8
- 238000005259 measurement Methods 0.000 claims abstract description 6
- 230000011218 segmentation Effects 0.000 claims abstract description 4
- 230000004927 fusion Effects 0.000 claims abstract description 3
- 239000011159 matrix material Substances 0.000 claims description 34
- 238000012545 processing Methods 0.000 claims description 21
- 238000004364 calculation method Methods 0.000 claims description 16
- 230000008859 change Effects 0.000 claims description 16
- 230000033001 locomotion Effects 0.000 claims description 15
- 238000012937 correction Methods 0.000 claims description 12
- 230000009466 transformation Effects 0.000 claims description 12
- 238000004458 analytical method Methods 0.000 claims description 6
- 238000005457 optimization Methods 0.000 claims description 5
- 230000001360 synchronised effect Effects 0.000 claims description 5
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 claims description 3
- 230000001186 cumulative effect Effects 0.000 claims description 3
- 230000007613 environmental effect Effects 0.000 claims description 3
- 238000000605 extraction Methods 0.000 claims description 3
- 238000001914 filtration Methods 0.000 claims description 3
- 238000003384 imaging method Methods 0.000 claims description 3
- 238000012163 sequencing technique Methods 0.000 claims description 3
- 230000003068 static effect Effects 0.000 claims description 3
- 238000012417 linear regression Methods 0.000 claims description 2
- 238000005192 partition Methods 0.000 claims 1
- 238000007781 pre-processing Methods 0.000 claims 1
- 241000273930 Brevoortia tyrannus Species 0.000 description 17
- 238000010586 diagram Methods 0.000 description 5
- 230000000007 visual effect Effects 0.000 description 3
- 238000007476 Maximum Likelihood Methods 0.000 description 2
- 238000010276 construction Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000007774 longterm Effects 0.000 description 2
- 238000010521 absorption reaction Methods 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000018109 developmental process Effects 0.000 description 1
- 230000008676 import Effects 0.000 description 1
- AFCCDDWKHLHPDF-UHFFFAOYSA-M metam-sodium Chemical compound [Na+].CNC([S-])=S AFCCDDWKHLHPDF-UHFFFAOYSA-M 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
- G06T17/05—Geographic models
-
- G06T3/06—
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
- G06T7/74—Determining position or orientation of objects or cameras using feature-based methods involving reference images or patches
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10028—Range image; Depth image; 3D point clouds
Abstract
A coal bunker modeling method based on three-dimensional positioning and two-dimensional mapping is characterized in that in order to accelerate wharf cabin cleaning efficiency, a sensor is used for obtaining internal information of a coal bunker, and a three-dimensional laser radar information and an inertia measurement unit are used for positioning a rake pusher for carrying out cabin cleaning operation in real time; sensing the coal bunker environment by using a fixed rotation sensor consisting of a single-line radar and a direct-current servo motor, and taking front-end positioning and a serial port rotation angle as a single-frame matching basis to obtain complete point cloud inside the coal bunker; and performing semantic fusion and segmentation on the superposed point clouds, classifying the point clouds of the coal materials, completing the outer boundaries of the point clouds, filling the internal vacancy point clouds, and calculating the volume of the coal materials according to the three-dimensional grid point clouds of the coal materials. And the calculated information is fed back to the control logic of the upper computer, so that the working efficiency of the whole push rake cleaning cabin is improved.
Description
Technical Field
The invention relates to a three-dimensional modeling method for a coal bunker of a port coal ship, which mainly utilizes a three-dimensional laser-based positioning and single-line radar-based rotating two-dimensional modeling technology to carry out point cloud reconstruction and analysis on a coal bunker environment.
Background
In recent years, with the change of the coal supply and demand pattern in China, the marine coal service range of 'domestic coal north coal south transportation + foreign trade coal import' in China is expanded from seven southern provinces to four provinces in Bohai region and midstream Yangtze river. In the face of increasing the adjustment force of the energy structure, port coal transportation still keeps stable growth, and the adjustment of the port layout under coal is accelerated, so that the national energy demand and the healthy development of economic construction are effectively guaranteed. Under the existing coal transportation mode, the port is used as a very important transfer node, has an important role in coal transportation, and is subjected to severe examination under the pressure of coal transportation.
The main cabin cleaning tools for wharf operations today are ship unloaders and rakers. Generally, a manually operated ship unloader grabs coal piles into a coal ship cabin and then the coal piles are piled on a belt conveyor, and the coal around the grabbed part can freely slide down due to the self angle of repose; until the coal material is accumulated into an environment with a larger gradient, the pushing and raking machine is lifted into the cabin by the crane, the manually operated pushing and raking machine pushes and rakes the coal material with high gradient around, the coal material is gathered at the fixed center and is further grabbed by the ship unloader; and finally, performing one-time full-cabin cleaning by the pushing and raking machine until the final stage, and grabbing the residual coal by a manually operated ship unloader.
In the above-mentioned cleaning operation, the ship unloader worker repeatedly grabs and harrows the central area inside the cabin mainly based on a visual mode, and scattered coal in other areas is gathered by the pushing and harrowing machine working inside the coal bunker, and as the limitation of the capability of non-closed-loop operation and manual visual, the whole pushing and harrowing operation does not have the capability of sensing the internal environment of the coal bunker in detail and three-dimensional reconstruction, so that the volume of the residual coal and the residual working time of the cleaning operation cannot be calculated. On the other hand, due to the influence of weather such as rainy, icy and snowy days and the requirement of operation all day, the accuracy of the sensing method based on artificial vision is reduced, and the efficiency is further reduced.
Therefore, in order to solve the problem of three-dimensional positioning, a three-dimensional positioning method based on a rotating two-dimensional laser radar is proposed in Xinghu (Monam in Xinghu. three-dimensional point cloud reconstruction method based on laser radar [ P ]. Chinese patent No. CN110223379A, 2019-09-10). However, in the working environment of the coal bunker, most of the environments are coal materials which continuously move along with operation, so the method does not process the influence of dynamic objects on positioning matching, and is not suitable for working conditions with large movement amplitude and long working time.
Meanwhile, in order to solve the problem of three-dimensional reconstruction, Lichangan provides a three-dimensional reconstruction technology based on a rotary two-dimensional laser radar (Lichangan, Yao Co-construction, Shimawei, Panpan, Lijing, Mayu. A material pile modeling method and a material pile modeling device [ P ] China patent: CN106094702B, 2020-07-07). The method is based on three-dimensional reconstruction on a plane in a fixed environment, but the size and the number of coal bunkers of a wharf coal ship are not fixed, so that a fixed rotary two-dimensional radar cannot be installed.
Disclosure of Invention
The invention overcomes the problems in the prior art and provides a coal bunker three-dimensional modeling based on three-dimensional positioning and rotating single-line radar.
The technical scheme adopted by the invention for solving the problems in the prior art is as follows:
a three-dimensional reconstruction method based on three-dimensional positioning and rotating single line radar comprises the following steps:
1) three-dimensional positioning: aiming at a complex cabin environment and a high-intensity working mode, a 32-line laser radar and IMU inertial navigation positioning unit is adopted to perform real-time optimization calculation on the pose of a carrier, and the method specifically comprises the following steps:
1.1) firstly carrying out distortion removal processing on an input point cloud, carrying out timestamp matching on motion estimation and laser radar information input by an IMU sensor, and then carrying out interpolation processing correction on the radar point cloud according to a uniform velocity model.
Pk+i,Pk+i' represents the ith distortion point and correction point in the k frame, w, j is the motion estimation frame number of all IMUs in the k frame and the jth frame where the current point is located, Tk+j,Tk+j+1The change matrix of the current k-th frame is superimposed with the change matrix of the motion estimation of the current IMU and the change matrix of the next frame.
1.2) then, dividing a space for 32-line radar point cloud input by a laser radar, generating a Scan Context, dividing a space plane coordinate { hor, ver } to which each point belongs, and reducing the dimension of the three-dimensional point cloud space to a two-dimensional data format.
Wherein x isi,yi,ziIs the three-dimensional coordinate of the ith point of the current frame, and the Deltaalpha and the Deltabeta are respectively the horizontal and vertical division resolution, hori,veriAnd processing the coordinates of the attribution plane for the ith point of the current frame.
1.3) traversing the two-dimensional plane space, and judging the category of the point by calculating the included angle between the curvature of the surrounding neighborhood of the current point and the vertical line of the horizontal line and setting a corresponding angle.
Wherein P isi,jThree-dimensional coordinate points representing spatial plane coordinates { i, j }, n and k represent the number of surrounding field points and the accumulated subscript, and the category of the current point is determined by comparing theta with a set threshold.
1.4) then carrying out clustering processing based on the category and Euclidean distance on the classified point cloud: selecting one of the points which are not accessed as an initial point, starting to search the adjacent point cloud with a certain radius by using the point, marking the point cloud as a category point after the preset category and Euclidean distance conditions are met, further clustering by using the adjacent point as a circle center, otherwise marking the point cloud as a noise point, judging the marked number, if the marked number is smaller than a threshold value, abandoning the point cloud, and if the marked number is larger than the threshold value, selecting a new initial point from the unmarked point to start a new round of clustering operation.
1.5) in the step of feature extraction and matching, dividing the XY plane into six equal parts on the plane, wherein the main plane feature point of each part is selected from the preprocessed static wall point cloud according to the following curvature calculation formula.
Where c represents the domain curvature of the current feature point, n represents the number of domain points,
and representing the ith three-dimensional point under the kth laser beam of the current L point cloud frame.
1.6) after extracting the feature points, obtaining the distance between the feature points through the feature point matching between two frames, constructing a nonlinear constraint equation set in a simultaneous manner, and solving the equation set through an LM (Linear modeling) method to obtain the solved radar pose; and simultaneously constructing a nonlinear constraint equation set of the frame and the map through matching of the frame and the map, and solving by using an LM (Linear modeling) method again to obtain the optimized motion estimation.
1.7) storing the two-dimensional coordinates of the key frames selected from the historical point cloud information into the historical information, and deleting the adjacent historical information near the current posture according to each front end matching result, so that the outdated information of the historical frames can be removed in a dynamic changing scene, and the dynamic updating of the historical frames is ensured.
2) Two-dimensional reconstruction, namely utilizing a rotating motor, a single-line radar which is calibrated and installed and a pose calculated in three-dimensional positioning, and specifically comprising the following steps of:
2.1) due to the superposition of pose and posture, point cloud interpolation and distortion removal can not be carried out only by utilizing IMU data, so that the pose change matrix of the optimized calculation of the multi-line radar of the following formula is adopted to carry out distortion removal treatment on the point cloud of the single line radar according to the uniform velocity model.
Whereink+iPkRepresenting the coordinates of the point in the current k frame with index i,kPk' points representing the index i are corrected to the coordinates of the points at the start time of k frames,
and representing the position and posture change matrix between frames from the k frame start subscript to the i subscript calculated by the multi-line radar.
2.2) because the vehicle body motion distortion has been removed in the previous step, so the second part is to remove the single line radar distortion problem brought by the uniform rotation of the motor, through synchronous serial port feedback data, can predict the posture transformation matrix between the frame end of the current frame and the frame head of the next frame, and then interpolation processing is carried out to each point through the time stamp, and the rotation matrix corresponding to each point in the frame can be obtained through the following formula, and through matrix correction, the point is projected under the absolute world coordinate system:
Pk,irepresenting a point of the single-line radar with a k-th frame index i, Pk,i' represents the point at which the k frame index i is corrected to the point at the start time of the k frame,
representing rotation about motor and radar axes of rotation
A correction matrix of angles.
And 2.3) finally, carrying out coordinate system transformation according to the motion estimation matched with the undistorted point cloud and the timestamp, and outputting a superposed point cloud map.
Pt,i＝TtPt,i (8)
Pt,iAnd Pt,i' represents the pose of the ith laser spot and the pose after transformation, TtAnd an interpolation transformation matrix representing the relative time t corresponding to the ith laser point.
3) Performing back-end analysis, namely performing back-end processing on the single-line radar point cloud map after superposition correction to realize semantic identification of the point cloud, filling of coal materials and calculation of volume parameters; the method specifically comprises the following steps:
3.1) firstly constructing point cloud data to be huge, and needing to quickly search dense three-dimensional space points, so that firstly constructing a KD tree data structure by using input point cloud.
3.2) because hardware and environmental factors influence the imaging result, each point is smoothed by a sliding Least square method (Moving Least Squares).
3.3) selecting proper resolution, distributing space plane coordinates { hor, ver } to each point in the dense point cloud, and sequencing the whole point cloud according to the plane coordinates.
3.4) traversing the two-dimensional plane space, setting a proper angle threshold value by using the priori knowledge that the repose angle of the coal material does not exceed 45 degrees, and calculating a point Pi,jThe surrounding neighborhood normal angle and the Euclidean distance relation of the XY plane to attribute the category of the current pointNamely wall points, roof points, coal points and noise points.
3.5) because the resolution ratio of the single-line laser radar is high and the coal distribution has randomness, correcting the category error of the current point by using a clustering method based on the category and Euclidean distance in a two-dimensional plane: setting a first point of the current vel as a starting point, marking the point as a clustering point according to the category and the Euclidean distance, and continuously judging downwards by using the point; if the condition is not met, judging whether the number of the clustering points meets the threshold requirement of the number of the clusters, and if the condition is not met, discarding the clusters; and after the clustering operation is finished, judging the neighborhood category of each clustering result by the following formula of weight and number to determine the categories of all points in the current clustering.
Wherein wiThe clustering weight basis, S, representing the current i pointi,wRepresents calculating the weight of the current i point by using w, and k represents a weight coefficient.
3.6) the information of all points on the single line is used to determine the category of the points, and then the information of the whole point cloud is used to determine the category of each point: and traversing all the points, searching surrounding neighborhood points of the points in a three-dimensional space structure of a certain range based on the constructed KD tree data structure, and correcting the attribution type of the points through neighborhood information.
3.7) the covering grid map processing mainly utilizes the mapping mode of the grid map to determine the current state of the grid, thereby achieving the effects of dynamically updating the point cloud map, removing ghost points generated by dynamic objects and reducing point cloud mapping errors. After the point clouds after the overlapping classification are obtained, inputting each frame of point cloud into a grid map as prior data:
data＝{x1,T1,x2,T2,…,xn,Tn} (10)
data represents current point cloud frame information, xn,TnRepresenting the coordinates and pose of the nth point.
To generate a grid map that conforms to the maximum probability of the current frame and historical frame information data:
m*＝arg maxmP(m|data) (11)
where P (m | data) is the grid map probability under the current a priori data, m*Representing the most probable map that fits the current frame and the historical frame.
3.8) when the two-dimensional radar information is superposed, and the probability value of the grid to which the grid map belongs is updated at the same time:
wherein
Is the model observed value, is the fixed update step size, S-And S+Representing the prior grid probabilities and the updated grid probabilities. The maximum likelihood estimation of each grid in the grid map is synchronously updated by formula (12), the current frame point cloud data in formula (10) is simultaneously updated, and the maximum grid probability map in formula (11) is constructed.
And 3.9) after the grid map constructed by the historical frame information is updated by using the current frame information, setting an occupation probability threshold, judging whether points in an occupied grid belong to occupied points or not according to the grid probability in the grid map, if so, storing the points as corresponding category points, otherwise, marking the points in the idle grid as noise points, and processing and removing the noise points.
3.10) but because the laser emitted by the single-line radar is in a beam shape, when the coal material with a certain height and gradient is encountered, a laser scanning blind area is formed at the rear part of the coal material, so that the superposed point cloud of the coal material is not complete and needs to be further filled. Before filling, the point cloud outer boundary needs to be determined, an outer boundary searching range is determined by using a formula (2), the size of the outer boundary is processed by using KNN outlier processing and mean filtering, and a partial missing boundary is filled, so that a complete outer boundary is obtained.
3.11) after obtaining the point cloud with optimized classificationConstructing a two-dimensional grid matrix, projecting all coal points into the two-dimensional grid matrix, saving Z values of all points in the same grid as a one-dimensional array, setting the number nums and variance threshold values of neighbor points, searching the neighbor points in the nums boundaries through a KD tree, and calculating the mean value d of the sum of the distances from the current point to all the neighbor pointsiAnd standard deviation stddev:
by judging di>threshold stddev to determine the point as an outlier, marking the point, removing the point from the one-dimensional array, and then determining the minimum Z value inside the grid as the height value of the current grid;
3.12) searching the adjacent points of the blind area grid by using a CV operator of the two-dimensional grid, and filling the height value of the blind area grid by using the distance weight of the formula (9) and the height value of the grid of the adjacent points:
Si,drepresenting the calculation of the weight of the current point i by means of the distance d, ZnumsRepresenting the grid height of the current neighbor point, and E representing the grid height value of the blind area.
3.13) after the treatment, projecting the two-dimensional grid back to the three-dimensional space again, and performing grid treatment on the three-dimensional point cloud by using KD tree space division. And accumulating the volume of all the grids to obtain the two-dimensional grid height data of the complete coal material, namely the total volume of the coal material:
wherein sizex,sizeyRepresentative gridResolution of division, EiRepresents the height of the current i-grid and V represents the cumulative grid volume.
The invention has the advantages that:
1. positioning advantages. An absolute positioning mode taking a Global Positioning System (GPS) as an example has larger measurement error, is not friendly to low-level signals and cannot meet the engineering requirement of high precision. And the long-term accumulated errors in the relative positioning such as a wheel type encoder and an IMU sensor are easily influenced by a magnetic field, and the long-term positioning requirement cannot be well met. Therefore, the relative positioning mode of sensing the coal bunker environment by the multi-line laser radar, synchronously positioning and establishing the image can well meet the engineering requirements of high-precision closed-loop calculation.
2. And (5) establishing a map. Due to the low reflection and high absorption characteristics of the coal material, the point cloud obtained by scanning the multi-line radar can be partially absorbed by the coal material, so that the whole point cloud is incomplete, and the multi-line radar point cloud is sparse for the whole coal bunker because the resolution of the multi-line radar in the SCAN dimension is too low. The three-dimensional reconstruction adopts a rotating single-line radar, the horizontal resolution can be adjusted through the angular speed of a motor, the vertical resolution can reach 0.06, and a point cloud map of the whole coal bunker can be completely reconstructed in a three-dimensional manner.
3. Back end analysis. The back-end analysis mainly adopts point cloud classification, fusion and grid processing, converts the three-dimensional reconstructed point cloud map into a semantic map, removes the influence of dynamic object noise through grid map processing, further optimizes the error of point cloud and pose matching, and realizes long-time dynamic map updating. And the volume, the high-level map and other visual information required by the upper computer are calculated through the processed point cloud, so that the bin clearing operation efficiency of the ship unloader operated by the upper computer is improved.
Drawings
FIG. 1 is a schematic view of the flow structure of the present invention.
Fig. 2 is a three-dimensional cloud point diagram of a distortion-removed warehouse established by the present invention.
FIG. 3 is a three-dimensional semantic graph of a coal bunker established by the invention.
FIG. 4 is a schematic view of the distribution of the outer boundary of the coal bunker of the present invention
FIG. 5 is a diagram of outer boundary correction according to the present invention
FIG. 6 is a cloud point diagram of coal material cut by the present invention.
FIG. 7 is a three-dimensional grid diagram of the coal material constructed by the present invention.
Detailed Description
Referring to fig. 1-7, a coal bunker modeling method based on three-dimensional positioning and two-dimensional mapping, as shown in fig. 1, receives two radar sensing information and IMU measurement information through a serial port and a network port, and obtains a real-time pose of a rake through three-dimensional mapping positioning by utilizing three-dimensional laser information and IMU real-time measurement information of an Ouster radar; the internal environment of the coal bunker of the coal ship is constructed by utilizing 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 and the IMU real-time measurement information acquired by Modbus communication, and a point cloud map about the coal warehouse is established.
The method comprises the following specific steps:
1) three-dimensional positioning: the method adopts a 32-line laser radar and IMU inertial navigation positioning unit to perform real-time optimization calculation on the pose of the carrier, and specifically comprises the following steps:
1.1) removing the influence of dynamic distortion by utilizing synchronous IMU interpolation according to a uniform velocity model:
Pk+i,Pk+irepresents the ith distortion point and correction point in the k-th frame, w, j is the motion estimation frame number of all IMUs in the k-th frame and the jth frame where the current point is located, Tk+j,Tk+j+1The change matrix of the current k-th frame is superimposed with the change matrix of the motion estimation of the current IMU and the change matrix of the next frame.
1.2) projecting the three-dimensional space point to a two-dimensional plane, and carrying out plane division on the laser radar point cloud:
wherein xi,yi,ziIs the three-dimensional coordinate of the ith point of the current frame, and the Deltaalpha and the Deltabeta are respectively the horizontal and vertical division resolution, hori,veriAnd processing the coordinates of the attribution plane for the ith point of the current frame.
1.3) traversing a two-dimensional plane space, and calculating the category of the attribution point of the included angle between the neighborhood curvature and the horizontal line vertical line:
wherein P isi,jThree-dimensional coordinate points representing space plane coordinates { i, j }, n and k represent the number of surrounding field points and the accumulated subscript, and the category of the current point is determined by comparing theta with a set threshold.
1.4) then carrying out clustering processing based on the category and Euclidean distance on the classified point cloud in a three-dimensional space. Thus, noise points are removed according to the category and the Euclidean distance condition, and the category of each point can be corrected based on category clustering.
1.5) in the step of feature extraction and matching, dividing the XY plane into six equal parts on the plane, removing the factor of uneven feature points caused by multiple states of the coal environment, wherein the main plane feature points of each part are selected from the preprocessed static wall point cloud according to the following curvature calculation formula.
Where c represents the domain curvature of the current feature point, n represents the number of domain points,
the ith laser beam under the kth laser beam representing the current L point cloud frameThree-dimensional points.
1.6) after extracting the feature points, obtaining the distance between the feature points through the feature point matching between two frames, simultaneously constructing a nonlinear constraint equation set, and solving the equation set through an LM (linear regression) method to obtain the solved radar pose; and through matching of the frame and the map, establishing a nonlinear constraint equation set of the frame and the map in a simultaneous manner, and solving by using an LM method again to obtain the optimized motion estimation.
1.7) storing the two-dimensional coordinates of the key frame selected from the historical point cloud information into the historical information, and deleting the adjacent historical information near the current posture according to each front end matching result to ensure the dynamic update of the historical frame.
2) Two-dimensional reconstruction, namely constructing a dense point cloud map in the coal bunker by using the information of the rotating single line radar and the pose calculated in three-dimensional positioning, and specifically comprising the following steps:
2.1) due to the superposition of pose and attitude, point cloud interpolation and distortion removal can not be carried out only by utilizing IMU data, so that the pose change matrix of the optimized calculation of the multi-line radar of the following formula is adopted to carry out distortion removal treatment on the point cloud of the single line radar according to the uniform velocity model:
whereink+iPkRepresenting the coordinates of the point in the current k frame with index i,kPk' points representing the index i are corrected to the coordinates of the points at the start time of k frames,
and representing the position and posture change matrix between frames from the k frame start subscript to the i subscript calculated by the multi-line radar.
2.2) through synchronous serial port feedback data, predicting a posture transformation matrix between the frame end of the current frame and the frame head of the next frame, and obtaining a rotation matrix corresponding to each point in the frame by utilizing interpolation processing:
Pk,irepresenting a point of the single-line radar with a k-th frame index i, Pk,i' represents the point at which the k frame index i is corrected to the point at the start time of the k frame,
representing rotation about motor and radar axes of rotation
A correction matrix of angles.
And 2.3) finally, carrying out coordinate system transformation according to the motion estimation matched with the undistorted point cloud and the timestamp, and outputting a superposed point cloud map.
Pt,i’＝TtPt,i (8)
Pt,iAnd Pt,i' represents the pose of the ith laser spot and the pose after transformation, TtAnd an interpolation transformation matrix representing the relative time t corresponding to the ith laser point.
After distortion is removed, a warehouse point cloud map is obtained by using motor rotation as shown in fig. 2, and after the motor rotation reaches a certain speed, the whole constructed point cloud map still keeps an accurate superposition form.
3) Back end analysis, back end processing is carried out on the single line radar point cloud map after superposition correction, semantic recognition, coal filling and volume parameter calculation of the point cloud are realized, and the method specifically comprises the following steps:
3.1) because the constructed point cloud data is huge and needs to search dense three-dimensional space points quickly, firstly constructing a KD tree data structure by the input point cloud.
3.2) because hardware and environmental factors influence the imaging result, each point is smoothed by a sliding Least square method (Moving Least Squares).
3.3) selecting proper resolution, distributing space plane coordinates { hor, ver } to each point in the dense point cloud according to the formulas (2) and (3), and sequencing the whole point cloud according to the plane coordinates.
3.4) by calculating the point Pi,jThe classification of the current point, namely a wall point, a top layer point, a coal material point and a noise point, is attributed according to the surrounding neighborhood normal angle and the Euclidean distance relation of the XY plane.
3.5) finishing clustering by a method based on category and Euclidean distance on a two-dimensional plane, and judging the following formula weight and clustering quantity of the neighborhood categories of each clustering result to correct the category error of the current point.
Wherein wiThe clustering weight basis, S, representing the current i pointi,wRepresents calculating the weight of the current i point by using w, and k represents a weight coefficient.
3.6) based on the constructed KD tree data structure, correcting the attribution type of the point by using the global point cloud type information through the domain type information.
3.7) optimizing the point cloud map by using the grid map, and after the point clouds after the overlapping classification are obtained, inputting each frame of point cloud into the grid map as prior data:
data＝{x1,T1,x2,T2,…,xn,Tn} (10)
data represents current point cloud frame information, xn,TnRepresenting the coordinates and pose of the nth point.
To generate a grid map that conforms to the maximum probability of the current frame and historical frame information data:
m*＝arg maxmP(m|data) (11)
where P (m | data) is the grid map probability under the current a priori data, m*Representing the most probable map that fits the current frame and the historical frame.
3.8) when the two-dimensional radar information is superposed, and the probability value of the grid to which the grid map belongs is updated at the same time:
wherein
Is the model observation, is a fixed update step, S-And S+Representing the prior grid probability and the updated grid probability. The maximum likelihood estimation of each grid in the grid map is synchronously updated by formula (12), the current frame point cloud data in formula (10) is simultaneously updated, and the maximum grid probability map in formula (11) is constructed.
3.9) removing noise points formed by mapping due to the matching error of the front-end odometer by using the grid probability, and correcting all grid class points. Finally, a semantic segmentation graph is obtained as shown in fig. 3, wherein red point cloud represents an upper plane, green point cloud represents a cabin wall, and purple point cloud represents coal bunker.
3.10) but because the laser emitted by the single-line radar is in a beam shape, when the single-line radar meets coal materials with certain height and gradient, a laser scanning blind area is formed at the rear part of the single-line radar, so that the point cloud of a lower red area is incomplete and needs to be further filled as shown in fig. 3. Before filling, the point cloud outer boundary needs to be determined, and the outer boundary searching range is determined by using formula (2), as shown in the point cloud outer boundary line diagram of fig. 4, the outer boundary range is also incomplete and has many burrs. The complete outer boundary is obtained by utilizing KNN outlier processing + mean filtering optimization errors and filling partial missing boundaries, and the complete outer boundary effect of FIG. 5 is realized.
3.11) constructing a two-dimensional grid matrix, projecting all coal points into the two-dimensional grid matrix, setting the number nums of neighboring points and a variance threshold, and calculating the mean value d of the distance sum from the current point to all the neighboring pointsiAnd standard deviation stddev:
by judging di>threshold stddev to determine the point as an outlier, mark the 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) searching the adjacent points of the blind area grid by using the CV operator of the two-dimensional grid, and filling the height value of the blind area grid by using the distance weight of the formula (9) and the height value of the adjacent point grid:
Si,drepresenting the calculation of the weight of the current point i by means of the distance d, ZnumsRepresenting the grid height of the current neighbor point, and E representing the grid height value of the blind area.
The resulting complete coal cut is shown in fig. 6, where the coal area is complete and smooth.
3.13) after the treatment, projecting the two-dimensional grid back to the three-dimensional space again, and performing grid treatment on the three-dimensional point cloud by using KD tree space division. The results obtained are shown in FIG. 7. And accumulating the volume of all the grids to obtain the three-dimensional grid height data of the complete coal material, namely the total volume of the coal material:
wherein sizex,sizeyRepresenting the resolution of the grid division, EiRepresents the height of the current i-grid and V represents the cumulative grid volume.
The embodiment of the invention is not limited to the three-dimensional modeling of the wharf cabin, and is also suitable for modeling scenes such as indoor warehouses, underground coal mines and the like.
Claims (4)
1. A coal bunker modeling method based on three-dimensional positioning and two-dimensional mapping is characterized in that: the coal bunker modeling method comprises the following steps:
1) three-dimensional positioning: the method adopts a 32-line laser radar and IMU inertial navigation positioning unit to perform real-time optimization calculation on the pose of the carrier, and specifically comprises the following steps:
1.1) removing the influence of dynamic distortion by utilizing synchronous IMU interpolation according to a uniform velocity model:
Pk+i，Pk+i' represents the ith distortion point and correction point in the k frame, w, j is the motion estimation frame number of all IMUs in the k frame and the jth frame where the current point is located, Tk+j，Tk+j+1The change matrix of the current k-th frame is superimposed with the change matrix of the motion estimation of the current IMU and the change matrix of the next frame.
1.2) projecting the three-dimensional space point to a two-dimensional plane, and carrying out plane division on the laser radar point cloud:
wherein xi，yi，ziFor the three-dimensional coordinates of the ith point of the current frame, Δ α, Δ β are the horizontal and vertical partition resolutions, hor, respectivelyi，veriAnd processing the coordinates of the attribution plane for the ith point of the current frame.
1.3) traversing a two-dimensional plane space, and calculating the category of the attribution point of the included angle between the neighborhood curvature and the horizontal line vertical line:
wherein P isi，jThree-dimensional coordinate points, n and j, representing spatial plane coordinates { i, j }k represents the number of surrounding field points and the accumulated index, and the category of the current point is determined by comparing theta with the set threshold.
1.4) then carrying out clustering processing based on the category and Euclidean distance on the classified point cloud: selecting one of the points which are not accessed as an initial point, starting to search the adjacent point cloud with a certain radius by using the point, marking the point cloud as a category point after the preset category and Euclidean distance conditions are met, further clustering by using the adjacent point as a circle center, otherwise marking the point cloud as a noise point, judging the marked number, if the marked number is smaller than a threshold value, abandoning the point cloud, and if the marked number is larger than the threshold value, selecting a new initial point from the unmarked point to start a new round of clustering operation.
Thus, noise points are removed according to the category and the Euclidean distance condition, and the category of each point can be corrected based on category clustering.
1.5) in the step of feature extraction and matching, dividing the XY plane into six equal parts on the plane, removing the factor of uneven feature points caused by multiple states of the coal environment, wherein the main plane feature points of each part are selected from the preprocessed static wall point cloud according to the following curvature calculation formula.
Where c represents the domain curvature of the current feature point, n represents the number of domain points,
and representing the ith three-dimensional point under the kth laser beam of the current L point cloud frame.
1.6) after extracting the feature points, obtaining the distance between the feature points through the feature point matching between two frames, simultaneously constructing a nonlinear constraint equation set, and solving the equation set through an LM (linear regression) method to obtain the solved radar pose; and through matching of the frame and the map, establishing a nonlinear constraint equation set of the frame and the map in a simultaneous manner, and solving by using an LM method again to obtain the optimized motion estimation.
1.7) storing the two-dimensional coordinates of the key frame selected from the historical point cloud information into the historical information, and deleting the adjacent historical information near the current posture according to each front end matching result to ensure the dynamic update of the historical frame.
2) Two-dimensional reconstruction, namely constructing a dense point cloud map in the coal bunker by using the information of the rotating single line radar and the pose calculated in three-dimensional positioning, and specifically comprising the following steps:
2.1) due to the superposition of pose and attitude, point cloud interpolation and distortion removal can not be carried out only by utilizing IMU data, so that the pose change matrix of the optimized calculation of the multi-line radar of the following formula is adopted to carry out distortion removal treatment on the point cloud of the single line radar according to the uniform velocity model:
whereink+iPkRepresenting the coordinates of the point in the current k frame with index i,kPk' points representing the index i are corrected to the coordinates of the points at the start time of k frames,
and representing the position and posture change matrix between frames from the k frame start subscript to the i subscript calculated by the multi-line radar.
2.2) through synchronous serial port feedback data, predicting a posture transformation matrix between the frame end of the current frame and the frame head of the next frame, and obtaining a rotation matrix corresponding to each point in the frame by utilizing interpolation processing:
Pk，irepresents the point with index i of the k-th frame of the single-line radar, Pk，i' represents the point at which the k frame index i is corrected to the point at the start time of the k frame,
representing wound motors and minesTo the rotation axis
A correction matrix of angles.
2.3) finally, carrying out coordinate system transformation according to the motion estimation matched with the undistorted point cloud and the time stamp, and outputting a superposed point cloud map:
Pt，i′＝TtPt，i (8)
Pt，iand Pt，i' represents the pose of the ith laser spot and the pose after transformation, TtAnd an interpolation transformation matrix representing the relative time t corresponding to the ith laser point.
3) Back end analysis, back end processing is carried out on the single line radar point cloud map after superposition correction, semantic recognition, coal filling and volume parameter calculation of the point cloud are realized, and the method specifically comprises the following steps:
3.1) because the constructed point cloud data is huge and needs to quickly search dense three-dimensional space points, firstly constructing a KD tree data structure by the input point cloud.
3.2) because hardware and environmental factors influence the imaging result, each point is smoothed by a sliding Least square method (Moving Least Squares).
3.3) selecting proper resolution, distributing space plane coordinates { hor, ver } to each point in the dense point cloud according to the formulas (2) and (3), and sequencing the whole point cloud according to the plane coordinates.
3.4) passing the calculated point Pi，jThe class of the current point, namely a wall point, a top layer point, a coal material point and a noise point, is attributed according to the relation between the normal angle of the surrounding neighborhood and the Euclidean distance of an XY plane.
3.5) finishing clustering by a method based on category and Euclidean distance on a two-dimensional plane, and judging the following formula weight and clustering quantity of the neighborhood categories of each clustering result to correct the category error of the current point.
Wherein wiThe clustering weight basis, S, representing the current i pointi，wRepresents calculating the weight of the current i point by using w, and k represents a weight coefficient.
3.6) based on the constructed KD tree data structure, correcting the attribution type of the point by using the global point cloud type information through the domain type information. And traversing all the points, searching surrounding neighborhood points of the points in a three-dimensional space structure of a certain range based on the constructed KD tree data structure, and correcting the attribution type of the points through neighborhood information.
3.7) optimizing the point cloud map by using the grid map, and after the point clouds after the overlapping classification are obtained, inputting each frame of point cloud into the grid map as prior data:
data＝{x1，T1，x2，T2，...，xn，Tn} (10)
data represents current point cloud frame information, xn，TnRepresenting the coordinates and pose of the nth point.
To generate a grid map which conforms to the maximum probability of the information data of the current frame and the historical frame:
m*＝arg maxm P(m|data) (11)
where P (m | data) is the grid map probability under the current a priori data, m*Representing the most probable map that fits the current frame and the historical frame.
3.8) when the two-dimensional radar information is superposed, and the probability value of the grid to which the grid map belongs is updated at the same time:
wherein
Is the model observation, is a fixed update step, and S-and S + represent the prior grid probability and the updated grid probability. Synchronously updating each grid in the grid map by formula (12)The current frame point cloud data in the formula (10) is updated at the same time, and the maximum grid probability map in the formula (11) is constructed.
3.9) removing noise points formed by mapping due to the matching error of the front-end odometer by using the grid probability, and correcting all grid class points. And after the grid map constructed by historical frame information is updated by using the current frame information, setting an occupation probability threshold, judging whether points in an occupied grid belong to occupied points or not according to the grid probability in the grid map, if so, storing the points as corresponding category points, otherwise, marking the points in an idle grid as noise points, and removing the noise points.
3.10) but because the laser emitted by the single-line radar is in a beam shape, when the coal material with a certain height gradient is encountered, a laser scanning blind area is formed behind the laser scanning blind area, and further filling is needed. Before filling, the outer boundary of the point cloud needs to be determined, and the outer boundary searching range is determined by using formula (2). And acquiring a complete outer boundary by utilizing KNN outlier processing + mean filtering optimization errors and filling partial missing boundaries.
3.11) constructing a two-dimensional grid matrix, projecting all coal points into the two-dimensional grid matrix, setting the number nums of neighboring points and a variance threshold, and calculating the mean value d of the distance sum from the current point to all the neighboring pointsiAnd standard deviation stddev:
by judging diGreater than threshold stddev to determine that the point is an outlier, mark the 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) searching the adjacent points of the blind area grid by using the CV operator of the two-dimensional grid, and filling the height value of the blind area grid by using the distance weight of the formula (9) and the height value of the adjacent point grid:
Si，drepresenting the calculation of the weight of the current point i by means of the distance d, ZnumsRepresenting the grid height of the current neighbor point, and E representing the grid height value of the blind area.
3.13) after the treatment, projecting the two-dimensional grid back to the three-dimensional space again, and performing grid treatment on the three-dimensional point cloud by utilizing KD tree space division. And accumulating the volume of all the grids to obtain the three-dimensional grid height data of the complete coal material, namely the total volume of the coal material:
wherein sizex，sizeyRepresenting the resolution of the grid division, EiRepresents the height of the current i-grid and V represents the cumulative grid volume.
2. The coal bunker modeling method based on three-dimensional positioning and two-dimensional mapping as claimed in claim 1, characterized in that three-dimensional laser radar information and an inertia measurement unit are used for sensing coal bunker environment, feature semantic segmentation and clustering preprocessing are performed on the information, and a global map is updated in real time by using pose Euclidean distance as a basis in historical information, so that a rake grader performing cleaning operation is positioned in real time.
3. The coal bunker modeling method based on three-dimensional positioning and two-dimensional mapping as claimed in claim 1, wherein a fixed rotation sensor composed of a single-line radar and a direct current servo motor is used for sensing the coal bunker environment, and the pose result of front-end three-dimensional positioning and the rotation angle fed back by a serial port are used as a single-frame matching basis to obtain complete point cloud inside the coal bunker.
4. The coal bunker modeling method based on three-dimensional positioning and two-dimensional mapping as claimed in claim 1, wherein the result of the point cloud map is subjected to semantic fusion segmentation, coal material point clouds are classified, the outer boundary of the point clouds is completed, the internal vacant point clouds are filled, and the volume of the coal material is calculated according to the three-dimensional grid map information of the coal material.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210177469.7A CN114581619A (en) | 2022-02-25 | 2022-02-25 | Coal bunker modeling method based on three-dimensional positioning and two-dimensional mapping |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210177469.7A CN114581619A (en) | 2022-02-25 | 2022-02-25 | Coal bunker modeling method based on three-dimensional positioning and two-dimensional mapping |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114581619A true CN114581619A (en) | 2022-06-03 |
Family
ID=81773226
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210177469.7A Pending CN114581619A (en) | 2022-02-25 | 2022-02-25 | Coal bunker modeling method based on three-dimensional positioning and two-dimensional mapping |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114581619A (en) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111932624A (en) * | 2020-08-13 | 2020-11-13 | 秦皇岛燕大滨沅科技发展有限公司 | Angle of repose detection method based on gradient image segmentation |
| CN115937294A (en) * | 2022-12-30 | 2023-04-07 | 重庆大学 | Method for predicting height of goaf collapse zone after ground fracturing of coal mine |
| CN116499364A (en) * | 2023-06-30 | 2023-07-28 | 济南作为科技有限公司 | Method and system for cloud adjustment distortion of three-dimensional laser point of coal-coiling instrument |
-
2022
- 2022-02-25 CN CN202210177469.7A patent/CN114581619A/en active Pending
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111932624A (en) * | 2020-08-13 | 2020-11-13 | 秦皇岛燕大滨沅科技发展有限公司 | Angle of repose detection method based on gradient image segmentation |
| CN111932624B (en) * | 2020-08-13 | 2023-08-18 | 秦皇岛燕大滨沅科技发展有限公司 | Inclination image segmentation-based repose angle detection method |
| CN115937294A (en) * | 2022-12-30 | 2023-04-07 | 重庆大学 | Method for predicting height of goaf collapse zone after ground fracturing of coal mine |
| CN115937294B (en) * | 2022-12-30 | 2023-10-27 | 重庆大学 | Prediction method for goaf caving zone height after coal mine ground fracturing |
| CN116499364A (en) * | 2023-06-30 | 2023-07-28 | 济南作为科技有限公司 | Method and system for cloud adjustment distortion of three-dimensional laser point of coal-coiling instrument |
| CN116499364B (en) * | 2023-06-30 | 2023-09-12 | 济南作为科技有限公司 | Method and system for cloud adjustment distortion of three-dimensional laser point of coal-coiling instrument |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN114581619A (en) | Coal bunker modeling method based on three-dimensional positioning and two-dimensional mapping | |
| CN111486855B (en) | Indoor two-dimensional semantic grid map construction method with object navigation points | |
| CN110717983B (en) | Building elevation three-dimensional reconstruction method based on knapsack type three-dimensional laser point cloud data | |
| CN113436260B (en) | Mobile robot pose estimation method and system based on multi-sensor tight coupling | |
| CN102506824B (en) | Method for generating digital orthophoto map (DOM) by urban low altitude unmanned aerial vehicle | |
| CN109242862B (en) | Real-time digital surface model generation method | |
| CN109186608B (en) | Repositioning-oriented sparse three-dimensional point cloud map generation method | |
| CN103020342A (en) | Method for extracting contour and corner of building from ground LiDAR data | |
| CN114543666B (en) | Stockpile face prediction method based on mine field environment perception | |
| CN113781582A (en) | Synchronous positioning and map creating method based on laser radar and inertial navigation combined calibration | |
| CN111553292A (en) | Rock mass structural plane identification and occurrence classification method based on point cloud data | |
| CN114419130A (en) | Bulk cargo volume measurement method based on image characteristics and three-dimensional point cloud technology | |
| CN111192328A (en) | Two-dimensional laser radar-based point cloud processing method for three-dimensional scanning system of compartment container | |
| CN114004869A (en) | Positioning method based on 3D point cloud registration | |
| CN114998338A (en) | Mining quantity calculation method based on laser radar point cloud | |
| CN113985429A (en) | Unmanned aerial vehicle environment scanning and reconstructing method based on three-dimensional laser radar | |
| CN114526745A (en) | Drawing establishing method and system for tightly-coupled laser radar and inertial odometer | |
| CN110095108B (en) | Surveying and mapping device and method based on BIM unmanned aerial vehicle | |
| CN111721279A (en) | Tail end path navigation method suitable for power transmission inspection work | |
| CN114283070B (en) | Method for manufacturing terrain section by fusing unmanned aerial vehicle image and laser point cloud | |
| CN115496792A (en) | Point cloud semantic SLAM method, system and device based on human-in-the-loop optimization | |
| CN115128628A (en) | Road grid map construction method based on laser SLAM and monocular vision | |
| CN116912443A (en) | Mining area point cloud and image fusion modeling method using unmanned aerial vehicle aerial survey technology | |
| CN116380039A (en) | Mobile robot navigation system based on solid-state laser radar and point cloud map | |
| CN113947665B (en) | Method for constructing map of spherical hedge trimmer based on multi-line laser radar and monocular vision |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |