Long-distance laser radar high-precision matching method based on joint probability weight iteration

Abstract

The invention belongs to the field of laser radar point cloud matching, and particularly relates to a long-distance laser radar high-precision matching method based on joint probability weight iteration, which comprises the following steps of: 1) preprocessing and calibrating acquired point cloud data, and converting the preprocessed and calibrated point cloud data into a global coordinate system taking a main laser radar as a reference; 2) processing the point cloud data to obtain a screened plane point cloud; 3) refining the screened plane point cloud, and optimizing the coefficient of a plane equation to obtain the plane equation; 4) selecting a splicing point cloud, and matching the point cloud data through a joint probability weight normalization method in the splicing process; and 5) performing iteration matching and splicing for multiple times through a joint probability weight normalization method to obtain an accurate change matrix, and constructing a unified global point cloud model in combination with the internal reference of the calibrated main laser radar. According to the method, an automatic feature matching mechanism is included, and large-area and flat plane features in different laser radar point clouds can be automatically recognized.

Description

Technical Field

[0001] The present invention belongs to the field of laser radar point cloud matching, and specifically relates to a long-distance laser radar high-precision matching method based on joint probability weight iteration. Background Art

[0002] In the field of automation and robotics, in order to realize the intelligent operation of large equipment, it is crucial to accurately obtain the surrounding three-dimensional environmental information and the posture and position information of the equipment. However, the existing three-dimensional environmental scanning systems are mostly suitable for small scenes, and lack multi-sensor data fusion functions. They cannot adapt to harsh environments, nor can they meet the requirements of real-time detection and high-precision positioning of equipment postures. Especially in long-distance working scenarios, when two lidar devices exist in a common viewing area, the stitching accuracy of point cloud data becomes a technical problem. Since the measurement accuracy of lidar decreases with increasing distance, the traditional ICP (iterative closest point) algorithm and NDT (normal distribution transform) algorithm have a greater impact when stitching long-distance point cloud data due to the global error minimization strategy. As a result, the accuracy of the stitched model is not high.

[0003] Therefore, the present invention proposes a "long-distance laser radar high-precision matching method based on joint probability weighted iteration of the nearest point". A distance-based weight function is used in the scheme. The farther the point cloud is, the smaller the weight is, thereby enhancing the positive impact of short-distance point clouds on the stitching algorithm. It aims to achieve high-precision point cloud data stitching capabilities in large scene environments, thereby improving the accuracy and reliability of intelligent operations of large equipment. Summary of the invention

[0004] The purpose of this invention is to provide a high-precision matching method for long-distance LiDAR based on joint probability weight iteration, aiming to solve the problem of splicing accuracy caused by measurement accuracy attenuation when splicing LiDAR point cloud data at long distances and large scenes. By automatically selecting matching feature points and introducing a distance weighting mechanism, the traditional ICP algorithm is optimized, so that the splicing algorithm can process long-distance point cloud data more accurately, and improve the accuracy and reliability of the spliced ​​model.

[0005] The technical solution adopted by the present invention to achieve the above-mentioned purpose is: a high-precision matching method for long-distance laser radar based on joint probability weight iteration, comprising the following steps:

[0006] 1) Deploy laser radars in the target workshop, collect point cloud data and send them to the PCL point cloud processing library of the terminal. After preprocessing and calibration, all point cloud data acquired by the laser radars are converted to the global coordinate system based on the main laser radar;

[0007] 2) Processing the point cloud data under the global coordinate system global, extracting the plane point cloud, and filtering the plane point cloud to obtain the filtered plane point cloud;

[0008] 3) The screened plane point cloud is refined by the least squares method, the coefficients of the plane equation are optimized, and the plane equation of the point cloud data is fitted;

[0009] 4) Select the spliced ​​point cloud, and during the splicing process, match the laser radar point cloud data by the joint probability weight normalization method;

[0010] 5) The point cloud data screened by multiple lidars are gradually matched and spliced ​​with the main lidar point cloud for multiple iterations to obtain an accurate change matrix, and combined with the internal parameters of the calibrated main lidar to construct a unified global point cloud model.

[0011] The step 1) comprises the following steps:

[0012] 1-1) Multiple laser radars are evenly distributed in the workshop to enable the laser radar to scan the entire workshop, and the point cloud data obtained by all laser radars are sent to the PCL point cloud processing library of the terminal;

[0013] 1-2) PCL point cloud processing library performs preprocessing steps of denoising and filtering on raw point cloud data to improve data accuracy and reliability;

[0014] 1-3) Make a rough estimate of the position of each lidar and preliminarily determine the rotation matrix R of each lidar relative to the master lidar master i and the translation vector T i , where i represents the number of the laser radar;

[0015] 1-4) Convert the point cloud data obtained by all laser radars to the global coordinate system based on the main laser radar, that is:

[0016]

[0017] Among them, (x i ,y i ,z i ) is the coordinate value of LiDAR i in its own radar coordinate system local, (x ic ,y ic ,z ic ) is the coordinate value of the main lidar coordinate global.

[0018] The step 2) comprises the following steps:

[0019] 2-1) Downsampling processing: The point cloud data is downsampled through the Voxel Grid filter in the PCL point cloud processing library, and the voxel size is set to m meters to reduce the amount of calculation and improve the processing speed;

[0020] 2-2) The PCL point cloud processing library calculates the normal and curvature of each point, retains and identifies points with curvature less than k;

[0021] 2-3) Use the RANSAC algorithm to fit and extract the plane, set the number of iterations and distance threshold, extract the plane through the segment_plane method and obtain the internal point index;

[0022] 2-4) Filter the extracted planes, set an area filtering threshold, and only retain plane point clouds with an area greater than the threshold.

[0023] The step 3) comprises the following steps:

[0024] 3-1) Define the plane equation as:

[0025] ax+by+cz+d=0

[0026] Among them, (a, b, c) is the normal vector of the plane, and d is the distance from the plane to the origin;

[0027] 3-2) For each point P in the plane point cloud after RANSAC screening i (x i ,y i ,z i ), construct the linear equation as: ax i +by i +cz i +d=0;

[0028] 3-3) Use the least squares method to minimize the sum of the squares of the perpendicular distances of all points to the plane, that is:

[0029]

[0030] Where N is the number of points in the plane point cloud;

[0031] 3-4) Calculate the partial derivatives of E, a, b, c, and d respectively, and set them equal to zero. Express them in matrix form, that is, the linear equation system is:

[0032]

[0033] Among them, i represents the index of the point in the plane point cloud, which is used to traverse all points in the plane point cloud; x i ,y i ,z irepresents the coordinates of the i-th point, and N represents the total number of points in the plane point cloud;

[0034] 3-5) Solve the linear equations to obtain the optimal values ​​of a, b, c, and d, and fit the plane equation of the point cloud data.

[0035] The selecting and splicing point cloud is specifically as follows:

[0036] The preset distance τ, τ<0.5 meters, and combined with the optimized plane equation ax+by+cz+d=0, the matching points are re-screened, and the point cloud data with a matching degree greater than the set value with the plane equation is retained.

[0037] The joint probability weight normalization method comprises the following steps:

[0038] 4-1) For each laser radar point, measure the distance d in its own radar coordinate system i for:

[0039]

[0040] Where i represents the index of the point cloud in the laser radar, (x i ,y i ,z i ) is the coordinate value of the laser radar i in its own radar coordinate system local;

[0041] 4-2) Use the weight function to calculate the weight:

[0042]

[0043] Among them, the value range of the weight function is (0~1), λ is the coefficient that controls the rate of change of the weight, and d ref is the preset reference distance;

[0044] 4-3) Joint probability weight calculation:

[0045] For each pair of matching points (p 1i ,p 2i ), and their joint probability weight is calculated as:

[0046] w(p 1i ,p 2i )=w1(d 1i )·w2(d 2i )

[0047] Among them, w(p 1i ,p 2i ) are the joint weights of the matching point pairs in the two lidars, d 1i and d 2iis the distance from the matching point to each lidar. The farther the distance, the lower the weight.

[0048] 4-4) Weight normalization:

[0049] Calculate the joint probability weight sum W of all matching point pairs as:

[0050]

[0051] The joint probability weight of each matching point pair is normalized, that is:

[0052]

[0053] Among them, w ′ (p 1i ,p 2i ) is the normalized joint probability weight of the i-th pair of matching points of LiDAR 1 and LiDAR 2.

[0054] The step 5) is specifically:

[0055] 5-1) Get the point cloud of the main lidar master , and find the Cloud master Recent LiDAR Point Cloud nearest ;

[0056] 5-2) For each point p in the lidar point cloud i , calculate the distance between its own coordinate system and the main coordinate system; and normalize the distance after weighting to obtain the minimum weighted distance;

[0057] 5-3) using the weighted point cloud data to perform an iterative closest point algorithm to minimize the stitching error and improve the stitching accuracy of the point cloud data;

[0058] 5-4) Point cloud merging: Merge the matched and spliced ​​Cloud nearest Merge to Cloud master , forming a new main point cloud Cloud master ;

[0059] 5-5) Select the next nearest LiDAR point cloud for calculation, and repeat the matching and stitching process of step 5-2) to step 5-4) until all LiDAR point clouds are merged to complete the construction of the global point cloud model of coordinate system one.

[0060] The step 5-2) is specifically:

[0061] (1) For each point p in the lidar point cloud i , which is in its own coordinate system:

[0062]

[0063] Point p i The distance in the principal coordinate system is:

[0064]

[0065] (2) Calculate the weight of each point based on the ratio of the distance between the point in its own coordinate system and the main coordinate system:

[0066]

[0067] (3) To ensure that the sum of the weights of all points is 1, the calculated weights need to be normalized, that is:

[0068]

[0069] Where N is the number of points in the point cloud.

[0070] (4) Determine the minimum weighted distance: Find the lidar point cloud with the smallest weighted distance as the nearest point cloud, that is:

[0071]

[0072] Where i represents the index of the laser radar, which is used to distinguish different laser radar point clouds; j represents the index of the point in the jth laser radar point cloud, which is used to traverse all points in each laser radar point cloud. If the jth laser radar filtered point cloud has N i points, j will range from 1 to N i Change; w normalized,i,j represents the normalized weight of the jth point in the i-th laser radar; d global,i,j Indicates the distance of the jth point in the ith laser radar in the global principal coordinate system; is the weighted sum of the global distances of all points to the main lidar; min i It means finding the lidar point cloud with the minimum sum of weighted global distances among all lidar point clouds.

[0073] The step 5-3) is specifically:

[0074] Point Cloud for the main lidar master The closest LiDAR point cloud to the main LiDAR point cloud nearest , perform the weighted ICP algorithm for matching and splicing; in the weighted ICP algorithm, an accurate transformation T is estimated * =(R * ,t * ), so that the weighted error is minimized;

[0075] The weighted error E is:

[0076]

[0077] Among them, R * and t * are the rotation matrix and translation vector, w i ′ is the normalized joint probability weight of the i-th pair of matching points of two lidars, p ref,i is the point cloud of matching points, p tgt,i is the point cloud of the target point;

[0078] Minimize the weighted error E and get the exact transformation matrix T * for:

[0079] The global point cloud model of the coordinate system 1 is constructed as follows:

[0080] The rotation matrix R of each laser radar relative to the master laser radar master i and the translation vector T i Fuse with the precise transformation matrix to obtain the final transformation matrix T final ,Right now:

[0081] T final =T i ·T *

[0082] Among them, T final is the final transformation matrix, T i is a rough internal calibration transformation, T * is an exact transformation.

[0083] The present invention has the following beneficial effects and advantages:

[0084] 1. The present invention uses automatic stitching technology: The method of the present invention implements automatic stitching technology, which can automatically align and fuse point cloud data from multiple laser radars to form a unified, global point cloud model. This process does not require manual intervention, greatly improving the efficiency and accuracy of data processing.

[0085] 2. Automatic feature matching: The method of the present invention includes an automatic feature matching mechanism, which can automatically identify large and flat planar features in different lidar point clouds.

[0086] 3. Distance weight function: The method of the present invention innovatively introduces a distance-based weight function, which dynamically adjusts the weight of each point according to the distance between the point cloud data point and the lidar. This weight function makes the points at close distance have higher weights, while the weights of the points at long distances gradually decrease, which optimizes the point cloud stitching process.

[0087] 4. Joint Probabilistic ICP Algorithm: The method of the present invention introduces the Joint Probabilistic ICP algorithm, which is an extension of the traditional ICP algorithm. The Joint Probabilistic ICP algorithm considers the probability distribution of point cloud data when matching point clouds, and optimizes the registration process by calculating the probability of each set of matching point pairs. This method improves the accuracy and robustness of the registration, especially when there is noisy data. BRIEF DESCRIPTION OF THE DRAWINGS

[0088] Figure 1 Schematic diagram of the scenario and equipment deployment of this embodiment;

[0089] Figure 2 A schematic diagram of network deployment of this embodiment;

[0090] Figure 3 A method flow chart of a high-precision matching method for a long-distance laser radar according to the present invention;

[0091] Figure 4 The actual point cloud schematic diagram of the present invention;

[0092] Figure 5 Schematic diagram of the weights of the point clouds in the respective laser radars of this embodiment;

[0093] Figure 6 This is a schematic diagram of the traditional ICP splicing result;

[0094] Figure 7 Schematic diagram of the splicing result of this embodiment;

[0095] Figure 8a Schematic diagram of the point cloud of the laser radar A in this embodiment;

[0096] Figure 8b Schematic diagram of the point cloud of the laser radar B in this embodiment;

[0097] Figure 8c Schematic diagram of the point cloud of the laser radar C in this embodiment;

[0098] Figure 8d Schematic diagram of the point cloud after being merged by the algorithm of this embodiment. DETAILED DESCRIPTION

[0099] The present invention is further described in detail below in conjunction with the accompanying drawings and embodiments.

[0100] like Figure 1As shown, this is a schematic diagram of the scene and equipment deployment of this embodiment. This embodiment aims to achieve all-round real-time monitoring, precise measurement and positioning of a large-scale tailings workshop by deploying 6 laser radars, provide high-precision real-time data for data fusion and 3D modeling, and improve the management level and production efficiency of the mine.

[0101] 1. Scenarios and device deployment

[0102] (1) Scenario description:

[0103] Workshop size: 140 meters long and 30 meters wide.

[0104] Monitoring area: tailings pile, track system, working platform and overhead crane operation area.

[0105] (2) LiDAR deployment:

[0106] A laser radar is deployed at the southwest corner, northwest corner, middle north side, middle south side, southeast corner and northeast corner of the workshop to ensure full coverage.

[0107] (3) Host deployment:

[0108] An X86 host is used as the device for algorithm calculation. The host is a Linux system equipped with a PCL point cloud processing library.

[0109] (4) Figure 2 The network deployment diagram of this embodiment is shown in FIG. 1 , specifically:

[0110] Use gigabit industrial switches and wired connections to build a high-speed LAN to meet the throughput requirements for real-time point cloud data acquisition.

[0111] like Figure 1 The scene and equipment deployment diagram shown in the figure, the laser equipment and configuration in this embodiment are as follows:

[0112] (1) Equipment configuration:

[0113] Each laser radar is configured with a static IP address and connected to a gigabit switch.

[0114] The laser scanning field of view covers 70 degrees vertically and horizontally. The number of point clouds of a single laser radar is 240K / S, and the data volume is 4MB / S.

[0115] (2) Data preprocessing:

[0116] Perform pre-processing steps such as denoising and filtering on the original point cloud data to improve data accuracy and reliability.

[0117] like Figure 3As shown, it is a method flow chart of the long-distance laser radar high-precision matching method of the present invention. In this embodiment, the specific steps are as follows:

[0118] Step 1): Unify the laser radar coordinate system:

[0119] like Figure 4 As shown in the figure, the common view area of ​​the actual laser radar scan is shown. In the figure, L1 and L2 are two laser radars. A, B, and C are three feature planes. A1, B1, and C1 are the scan point clouds of the L1 laser radar. A2, B2, and C2 are the scan point clouds of the L2 laser radar.

[0120] According to the construction drawings or the actual situation of on-site installation, the position and posture of each lidar are roughly estimated, and the rotation matrix R of each lidar relative to the master lidar is preliminarily determined. i and the translation vector T i , where i represents the number of the laser radar. The point cloud data obtained by all laser radars are converted to the global coordinate system global based on the main laser radar, that is:

[0121]

[0122] Among them, (x i ,y i ,z i ) is the coordinate value of LiDAR i in its own radar coordinate system local, (x ic ,y ic ,z ic ) is the coordinate value of the main lidar coordinate global.

[0123] Step 2): Downsampling processing: Use the Voxel Grid filter in the PCL library to downsample the original point cloud, and the voxel size is set to 0.5 meters to reduce the amount of calculation and improve the processing speed.

[0124] Step 3): Screening based on point cloud curvature: Use the NormalEstimation class in the PCL library to calculate the normal and curvature of each point, and retain points with a curvature less than 0.01, which are more likely to belong to the plane.

[0125] Step 4): Plane extraction: Apply the RANSAC algorithm for plane fitting and extraction, set the number of iterations to 10,000 and the distance threshold to 0.1 meters, extract the plane through the segment_plane method and obtain the inner point index.

[0126] Step 5): Area screening: Filter the extracted planes, set the area filtering threshold to 10㎡, and only retain plane point clouds with an area larger than the threshold.

[0127] Step 6): Least squares method to optimize the plane: The plane point cloud after RANSAC screening is refined using the least squares method to optimize the coefficients of the plane equation. This can improve the accuracy of plane fitting.

[0128] 6.1) Definition of plane equation: The equation of a plane is usually expressed as ax+by+cz+d=0, where (a, b, c) is the normal vector of the plane and d is the distance from the plane to the origin.

[0129] 6.2) Construct a linear equation system: For each point P in the plane point cloud after RANSAC screening i (x i ,y i ,z i ), we can construct the following linear equation: ax i +by i +cz i +d=0;

[0130] 6.3) Objective function: The least squares method is used to minimize the sum of the squares of the vertical distances from all points to the plane, that is:

[0131]

[0132] Where N is the number of points in the planar point cloud.

[0133] 6.4) Solve for the optimal parameters: To solve (a, b, c, d) that minimizes E, we need to find the partial derivatives of E with respect to a, b, c, and d respectively, and set them equal to zero:

[0134]

[0135] 6.5) Matrix representation and solution: Represent the above equations in matrix form and solve them to obtain the optimal parameters:

[0136]

[0137] Where i represents the index of the point in the plane point cloud, which is used to traverse all points in the plane point cloud. i ,y i ,z i : represents the coordinates of the i-th point. N: represents the total number of points in the plane point cloud. By solving the above linear equations, we can get the optimal values ​​of a, b, c, d, which define the plane equation that best fits the point cloud data.

[0138] Step 7): Accurate selection of stitching point cloud:

[0139] According to the optimized plane and the preset distance τ, τ < 0.5 m, the matching points are re-screened and the point cloud data with high matching degree with the plane equation is retained.

[0140] Step 8): Joint probability weight normalization:

[0141] In order to balance the weights of close and far points, the point cloud weights of the two lidars that need to be matched are combined to ensure that the sum of the weights of all points is 1 during the stitching process.

[0142] 8.1) Weight calculation:

[0143] For each laser radar point, the distance d in its own radar coordinate system is measured i , i represents the index of the point cloud in the lidar, where The weight is calculated using the weight function:

[0144]

[0145] The value range of the weight function is (0-1), and λ is the coefficient that controls the rate of change of the weight, which is set to 0.1 in this example. ref is the preset reference distance, which is set to 75 meters in this example, half of the maximum measurement distance of the LiDAR (150 meters). This piecewise weight function ensures that at distance d i Less than d ref When the distance d is , the weight is 1, that is, these points have the greatest influence in the splicing process. i Greater than or equal to d ref When , the weight is calculated according to the exponential decay function, so that the influence of distant points in the stitching process gradually decreases. This design can better handle the stitching problem of distant point cloud data and improve the accuracy and reliability of stitching.

[0146] 8.2) Joint probability weight calculation:

[0147] For each pair of matching points (p 1i ,p 2i )(from two laser radars respectively), calculate their joint probability weight: w(p 1i ,p 2i )=w1(d 1i )·w2(d 2i ) where w(p 1i ,p 2i ) are the joint weights of the matching point pairs in the two lidars, d 1i and d 2i is the distance from each laser radar. The farther the distance, the lower the weight, and vice versa. Figure 5 As shown in the figure, the weights of point cloud A in L1 and L2 of the laser radar are shown. Since the weight of W1 is higher than that of W2, the data of L1 should be more inclined in the splicing of this point.

[0148] 8.3) Weight Normalization:

[0149] Calculate the joint probability weighted sum W of all matching point pairs:

[0150] Normalize the joint probability weight of each matching point pair:

[0151]

[0152] Among them, w ′ (p 1i ,p 2i ) is the normalized joint probability weight of the i-th pair of matching points of lidars 1 and 2.

[0153] Step 9): Iterate matching and splicing step by step:

[0154] The point cloud data filtered by multiple lidars are gradually matched and spliced ​​with the main lidar point cloud, and finally a unified global point cloud model is constructed.

[0155] 9.1) Initialization: Get the point cloud of the main lidar master .

[0156] 9.2) Closest radar match: Find the master Recent LiDAR Point Cloud nearest .

[0157] 9.2.1) Point cloud distance calculation: For each point p in the LiDAR point cloud i , calculate the distance between its own coordinate system and the main coordinate system:

[0158]

[0159] 9.2.2) Distance weighting: Calculate the weight of each point based on the ratio of the distance between the point in its own coordinate system and the main coordinate system:

[0160]

[0161] 9.2.3) Normalized weights: In order to ensure that the sum of the weights of all points is 1, the calculated weights need to be normalized:

[0162]

[0163] Where N is the number of points in the point cloud.

[0164] 9.2.4) Determine the minimum weighted distance: Find the lidar point cloud with the smallest weighted distance as the "closest" point cloud:

[0165]

[0166] Wherein, i represents the index of the laser radar, which is used to distinguish different laser radar point clouds. In this embodiment, there are 6 laser radars, and i will vary from 1 to 6.

[0167] j represents the index of the point in the jth lidar point cloud, which is used to traverse all points in each lidar point cloud. For example, if the jth lidar filtered point cloud has N i points, j will range from 1 to N i change.

[0168] w normalized,i,j Represents the normalized weight of the jth point in the ith lidar.

[0169] d global,i,j Indicates the distance of the jth point in the ith laser radar in the global principal coordinate system.

[0170] It means that for each lidar point cloud, the weighted sum of the global distances of all its points to the main lidar is calculated. This sum is obtained by calculating the global distance of each point (d global,i,j ) multiplied by its corresponding weight (w normalized,i,j ) obtained.

[0171] min i Indicates that among all the lidar point clouds, find the lidar point cloud with the minimum sum of weighted global distances. This lidar point cloud is considered to be the "closest" to the main lidar point cloud.

[0172] 9.3) Weighted ICP algorithm: Use weighted point cloud data to perform iterative closest point algorithm to minimize the stitching error and improve the stitching accuracy of point cloud data. master and Cloud nearest , perform the weighted ICP algorithm for matching and splicing. In the weighted ICP algorithm, an accurate transformation T is estimated * =(R * ,t * ), so that the weighted error is:

[0173]

[0174] Among them, R * and t * are the rotation matrix and translation vector, w i ′ is the normalized joint probability weight of the i-th pair of matching points of two lidars, p ref,i is the point cloud of matching points, p tgt,iis the point cloud of the target point;

[0175] Minimize the weighted error E and get the exact transformation matrix T * for:

[0176] 9.4) Merge point clouds: Merge the matched and spliced ​​point clouds nearest Merge to Cloud master , forming a new main point cloud Cloud master .

[0177] 9.5) Iterative process: Select the next nearest LiDAR point cloud for calculation, and repeat the matching and stitching process from 9.2) to 9.5) above until all LiDAR point clouds are merged.

[0178] 9.6) Final transformation fusion: The rough intrinsic calibration transformation estimated in step 1) is fused with the precise transformation obtained in step 9) to obtain the final transformation matrix: T final =T i ·T * , where T final is the final transformation matrix, T i is a rough internal calibration transformation, T * is an exact transformation.

[0179] like Figure 6 As shown in the figure, the stitching result of the traditional ICP algorithm is shown. Since the farther the laser radar is, the greater the measurement error is, the stitching effect and accuracy are much lower than the actual situation. The stitching result of this implementation is shown in Figure 7 As shown in the figure, the result of the stitching algorithm of the present invention is shown. First, through automatic calculation, many interfering point clouds are filtered out, and finally, a distance-based weight function is introduced, and joint probability is used to reduce the influence of distant point clouds. The effect and accuracy after stitching are closer to the actual situation.

[0180] 10) Verify the accuracy and integrity of the spliced ​​point cloud data to ensure the consistency between the 3D model and the actual environment. Evaluate the performance of the system through actual monitoring data, including monitoring coverage, data transmission speed and accuracy. Figures 8a to 8d shown.

[0181] The above embodiment demonstrates how to apply the high-precision matching method of long-distance lidar based on the joint probability weighted iterative nearest point to a large-scale tailings workshop, realizing all-round monitoring and precise measurement and positioning, providing high-precision real-time data for data fusion and three-dimensional modeling, and improving the management level and production efficiency of the mine.

[0182] Those skilled in the art will appreciate that the above are only preferred embodiments of the present invention, and the various embodiments of the present disclosure and / or the features described in the claims may be combined or combined in various ways, even if such combinations or combinations are not explicitly described in the present disclosure. It is not intended to limit the present invention. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art may still modify the technical solutions described in the aforementioned embodiments, or perform equivalent substitutions on some of the technical features therein. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention shall be included in the protection scope of the present invention.

[0183] Although preferred embodiments of the present invention have been described, additional changes and modifications may be made to these embodiments by those skilled in the art once the basic inventive concepts are known. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of the present invention. Obviously, those skilled in the art may make various changes and modifications to the present invention without departing from the spirit and scope of the present invention. Thus, if these modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalents, the present invention is also intended to include these modifications and variations.

Claims

1. A high-precision matching method for long-distance laser radar based on joint probability weight iteration, characterized in that: The following steps are involved: 1) Deploy laser radars in the target workshop, collect point cloud data and send them to the PCL point cloud processing library of the terminal. After preprocessing and calibration, all point cloud data acquired by the laser radars are converted to the global coordinate system based on the main laser radar; 2) Processing the point cloud data under the global coordinate system global, extracting the plane point cloud, and filtering the plane point cloud to obtain the filtered plane point cloud; 3) The screened plane point cloud is refined by the least squares method, the coefficients of the plane equation are optimized, and the plane equation of the point cloud data is fitted; 4) Select the spliced ​​point cloud, and during the splicing process, match the laser radar point cloud data by the joint probability weight normalization method; 5) The point cloud data screened by multiple lidars are gradually matched and spliced ​​with the main lidar point cloud for multiple iterations to obtain an accurate change matrix, and combined with the internal parameters of the calibrated main lidar to construct a unified global point cloud model.

2. The long-distance laser radar high-precision matching method based on joint probability weight iteration according to claim 1 is characterized in that: The step 1) comprises the following steps: 1-1) Multiple laser radars are evenly distributed in the workshop to enable the laser radar to scan the entire workshop, and the point cloud data obtained by all laser radars are sent to the PCL point cloud processing library of the terminal; 1-2) PCL point cloud processing library performs preprocessing steps of denoising and filtering on raw point cloud data to improve data accuracy and reliability; 1-3) Make a rough estimate of the position of each lidar and preliminarily determine the rotation matrix R of each lidar relative to the master lidar master i and the translation vector T i , where i represents the number of the laser radar; 1-4) Convert the point cloud data obtained by all laser radars to the global coordinate system based on the main laser radar, that is: Among them, (x i ,y i ,z i ) is the coordinate value of LiDAR i in its own radar coordinate system local, (x ic ,y ic ,z ic ) is the coordinate value of the main lidar coordinate global.

3. The long-distance laser radar high-precision matching method based on joint probability weight iteration according to claim 1 is characterized in that: The step 2) comprises the following steps: 2-1) Downsampling processing: The point cloud data is downsampled through the Voxel Grid filter in the PCL point cloud processing library, and the voxel size is set to m meters to reduce the amount of calculation and improve the processing speed; 2-2) The PCL point cloud processing library calculates the normal and curvature of each point, retains and identifies points with curvature less than k; 2-3) Use the RANSAC algorithm to fit and extract the plane, set the number of iterations and distance threshold, extract the plane through the segment_plane method and obtain the internal point index; 2-4) Filter the extracted planes, set an area filtering threshold, and only retain plane point clouds with an area greater than the threshold.

4. The long-distance laser radar high-precision matching method based on joint probability weight iteration according to claim 1 is characterized in that: The step 3) comprises the following steps: 3-1) Define the plane equation as: ax+by+cz+d=0 Among them, (a, b, c) is the normal vector of the plane, and d is the distance from the plane to the origin; 3-2) For each point P in the plane point cloud after RANSAC screening i (x i ,y i ,z i ), construct the linear equation as: ax i +by i +cz i +d=0; 3-3) Use the least squares method to minimize the sum of the squares of the perpendicular distances of all points to the plane, that is: Where N is the number of points in the plane point cloud; 3-4) Calculate the partial derivatives of E, a, b, c, and d respectively, and set them equal to zero. Express them in matrix form, that is, the linear equation system is: Among them, i represents the index of the point in the plane point cloud, which is used to traverse all points in the plane point cloud; x i ,y i ,z i represents the coordinates of the i-th point, and N represents the total number of points in the plane point cloud; 3-5) Solve the linear equations to obtain the optimal values ​​of a, b, c, and d, and fit the plane equation of the point cloud data.

5. The long-distance laser radar high-precision matching method based on joint probability weight iteration according to claim 1 is characterized in that: The selecting and splicing point cloud is specifically as follows: The preset distance τ, τ<0.5 meters, and combined with the optimized plane equation ax+by+cz+d=0, the matching points are re-screened, and the point cloud data with a matching degree greater than the set value with the plane equation is retained.

6. The long-distance laser radar high-precision matching method based on joint probability weight iteration according to claim 1 is characterized in that: The joint probability weight normalization method comprises the following steps: 4-1) For each laser radar point, measure the distance d in its own radar coordinate system i for: Where i represents the index of the point cloud in the laser radar, (x i ,y i ,z i ) is the coordinate value of the laser radar i in its own radar coordinate system local; 4-2) Use the weight function to calculate the weight: Among them, the value range of the weight function is (0~1), λ is the coefficient that controls the rate of change of the weight, and d ref is the preset reference distance; 4-3) Joint probability weight calculation: For each pair of matching points (p 1i ,p 2i ), and their joint probability weights are calculated as: w(p 1i ,p 2i )=w1(d 1i )·w2(d 2i ) Among them, w(p 1i ,p 2i ) are the joint weights of the matching point pairs in the two lidars, d 1i and d 2i is the distance from the matching point to each lidar. The farther the distance, the lower the weight. 4-4) Weight normalization: Calculate the joint probability weight sum W of all matching point pairs as: The joint probability weight of each matching point pair is normalized, that is: Among them, w′(p 1i ,p 2i ) is the normalized joint probability weight of the i-th pair of matching points of LiDAR 1 and LiDAR 2.

7. The long-distance laser radar high-precision matching method based on joint probability weight iteration according to claim 1 is characterized in that: The step 5) is specifically: 5-1) Get the point cloud of the main lidar master , and find the Cloud master Recent LiDAR Point Cloud nearest ; 5-2) For each point p in the lidar point cloud i , calculate the distance between its own coordinate system and the main coordinate system; and normalize the distance after weighting to obtain the minimum weighted distance; 5-3) using the weighted point cloud data to perform an iterative closest point algorithm to minimize the stitching error and improve the stitching accuracy of the point cloud data; 5-4) Point cloud merging: Merge the matched and spliced ​​Cloud nearest Merge to Cloud master , forming a new main point cloud Cloud master ; 5-5) Select the next nearest LiDAR point cloud for calculation, and repeat the matching and stitching process of step 5-2) to step 5-4) until all LiDAR point clouds are merged to complete the construction of the global point cloud model of coordinate system one.

8. The long-distance laser radar high-precision matching method based on joint probability weight iteration according to claim 7 is characterized in that: The step 5-2) is specifically: (1) For each point p in the lidar point cloud i , which is in its own coordinate system: Point p i The distance in the principal coordinate system is: (2) Calculate the weight of each point based on the ratio of the distance between the point in its own coordinate system and the main coordinate system: (3) To ensure that the sum of the weights of all points is 1, the calculated weights need to be normalized, that is: Where N is the number of points in the point cloud. (4) Determine the minimum weighted distance: Find the lidar point cloud with the smallest weighted distance as the nearest point cloud, that is: Where i represents the index of the laser radar, which is used to distinguish different laser radar point clouds; j represents the index of the point in the jth laser radar point cloud, which is used to traverse all points in each laser radar point cloud. If the jth laser radar filtered point cloud has N i points, j will range from 1 to N i Change; w normalized,i,j represents the normalized weight of the jth point in the i-th laser radar; d global,i,j Indicates the distance of the jth point in the ith laser radar in the global principal coordinate system; is the weighted sum of the global distances of all points to the main lidar; min i It means finding the lidar point cloud with the minimum sum of weighted global distances among all lidar point clouds.

9. The long-distance laser radar high-precision matching method based on joint probability weight iteration according to claim 7 is characterized in that: The step 5-3) is specifically: Point Cloud for the main lidar master The closest LiDAR point cloud to the main LiDAR point cloud nearest , perform the weighted ICP algorithm for matching and splicing; in the weighted ICP algorithm, an accurate transformation T is estimated * =(R * ,t * ), so that the weighted error is minimized; The weighted error E is: Among them, R * and t * are the rotation matrix and translation vector, w′ i is the normalized joint probability weight of the i-th pair of matching points of the two lidars, p ref,i is the point cloud of matching points, p tgt,i is the point cloud of the target point; Minimize the weighted error E and get the exact transformation matrix T * for:

10. The long-distance laser radar high-precision matching method based on joint probability weight iteration according to claim 7 is characterized in that: The global point cloud model of the coordinate system 1 is constructed as follows: The rotation matrix R of each laser radar relative to the master laser radar master i and the translation vector T i Fuse with the precise transformation matrix to obtain the final transformation matrix T final ,Right now: T final =T i ·T * Among them, T final is the final transformation matrix, T i is a rough internal calibration transformation, T * is an exact transformation.

Images