High-precision matching method for long-distance laser radar based on joint probability weight iteration

Abstract

The present application 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, which comprises the following steps: 1) converting the collected point cloud data to a global coordinate system global with the main laser radar as a reference after pretreatment and calibration; 2) processing the point cloud data to obtain screened plane point clouds; 3) performing fine processing on the screened plane point clouds to optimize the coefficients of the plane equation and obtain the plane equation; 4) selecting spliced point clouds, and matching the point cloud data by means of a joint probability weight normalization method in the splicing process; and 5) performing multiple iteration matching and splicing by means of the joint probability weight normalization method to obtain an accurate change matrix, and combining the internal parameters of the main laser radar after calibration to construct a unified global point cloud model. The method of the present application comprises an automatic feature matching mechanism, and can automatically identify large-area and flat plane features in different laser radar point clouds.

Description

Technical Field

[0001] This invention belongs to the field of lidar point cloud matching, specifically a high-precision matching method for long-range lidar based on joint probability weight iteration. Background Technology

[0002] In the fields of automation and robotics, accurately acquiring information about the surrounding 3D environment, as well as the equipment's attitude and position, is crucial for enabling intelligent operation of large equipment. However, existing 3D environment scanning systems are mostly suitable for small-scale scenarios and lack multi-sensor data fusion capabilities, making them unsuitable for harsh environments and unable to meet the requirements for real-time equipment attitude detection and high-precision positioning. Particularly in long-distance working scenarios, the accuracy of point cloud data stitching becomes a technical challenge when two LiDAR devices share a common field of view. Since LiDAR measurement accuracy decreases with increasing distance, traditional ICP (Iterative Closest Point) and NDT (Normal Distribution Transform) algorithms, when stitching long-distance point cloud data, suffer from low accuracy due to the global error minimization strategy; larger measurement errors have a greater impact.

[0003] Therefore, this invention proposes a "high-precision matching method for long-range lidar based on joint probability weighting and iterative nearest point matching." The method uses a distance-based weighting function; the greater the distance between point clouds, the smaller the weight, thereby enhancing the positive impact of short-range point clouds on the stitching algorithm. The aim is to achieve high-precision point cloud data stitching capabilities in large-scale environments, thereby improving the accuracy and reliability of intelligent operations for large equipment. Summary of the Invention

[0004] The purpose of this invention is to provide a high-precision matching method for long-range LiDAR based on joint probability weight iteration, aiming to solve the stitching accuracy problem caused by the attenuation of measurement accuracy when stitching LiDAR point cloud data in long-distance, large-scene situations. By automatically selecting matching feature points and introducing a distance weighting mechanism, the traditional ICP algorithm is optimized, enabling the stitching algorithm to process long-distance point cloud data more accurately and improving the accuracy and reliability of the stitched model.

[0005] The technical solution adopted by this invention to achieve the above objectives is: a high-precision matching method for long-range lidar based on joint probability weight iteration, comprising the following steps:

[0006] 1) Deploy lidar in the target workshop, collect point cloud data and send it to the PCL point cloud processing library of the terminal. After preprocessing and calibration, convert all the point cloud data acquired by lidar into the global coordinate system based on the main lidar.

[0007] 2) Process the point cloud data in the global coordinate system to extract the planar point cloud, and then filter the planar point cloud to obtain the filtered planar point cloud.

[0008] 3) The filtered planar point cloud is refined using the least squares method to optimize the coefficients of the planar equation and fit the planar equation of the point cloud data.

[0009] 4) Select the stitched point cloud and match the point cloud data of the lidar using the joint probability weight normalization method during the stitching process;

[0010] 5) The point cloud data selected from multiple lidars are iteratively matched and stitched with the point cloud of the main lidar to obtain an accurate change matrix. Combined with the intrinsic parameters of the calibrated main lidar, a unified global point cloud model is constructed.

[0011] Step 1) includes the following steps:

[0012] 1-1) Distribute multiple lidars evenly in the workshop to enable the lidars to achieve full coverage scanning of the workshop, and send the point cloud data acquired by all lidars to the PCL point cloud processing library of the terminal.

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

[0014] 1-3) Perform a rough estimate of the pose of each lidar and preliminarily determine the rotation matrix R of each lidar relative to the master lidar. i Translation vector T i , where i represents the lidar number;

[0015] 1-4) Convert all point cloud data acquired by the lidar to the global coordinate system (global) with the main lidar as the reference, i.e.:

[0016]

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

[0018] Step 2) includes the following steps:

[0019] 2-1) Downsampling: The point cloud data is downsampled using the Voxel Grid filter in the PCL point cloud processing library. The voxel size is set to m meters to reduce computation and improve processing speed.

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

[0021] 2-3) The RANSAC algorithm is used for plane fitting and extraction. The number of iterations and distance thresholds are set, and the plane is extracted and the inlier index is obtained by segment_plane method.

[0022] 2-4) Filter the extracted planes by setting an area filtering threshold and retaining only the plane point cloud with an area greater than the threshold.

[0023] Step 3) includes the following steps:

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

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

[0026] Where (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 planar point cloud after RANSAC filtering i (x i ,y i ,z i The linear equation is constructed as: ax i +by i +cz i +d = 0;

[0028] 3-3) Minimize the sum of squares of the perpendicular distances from all points to the plane using the least squares method, i.e.:

[0029]

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

[0031] 3-4) Take the partial derivatives with respect to E, a, b, c, and d respectively, and set them equal to zero. Represent the system of linear equations in matrix form as follows:

[0032]

[0033] Where i represents the index of a point in the planar point cloud, used to traverse all points in the planar point cloud; x i ,y i ,z iLet represent the coordinates of the i-th point, and N represent the total number of points in the planar point cloud;

[0034] 3-5) Solve the system of 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 selection of the spliced ​​point cloud specifically refers to:

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

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

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

[0039]

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

[0041] 4-2) Calculate the weights using a weighting function:

[0042]

[0043] Wherein, the range of the weighting function is (0~1), λ is the coefficient controlling the rate of change of the weights, and d ref This is a preset reference distance;

[0044] 4-3) Calculation of joint probability weights:

[0045] For each pair of matched points (p) from the two lidars respectively 1i ,p 2i ), calculate their joint probability weights as follows:

[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, and d are the weights of the matching point pairs in the two lidars. 1i and d 2iIt is the distance from the matching point to its respective LiDAR; the farther the distance, the lower the weight.

[0048] 4-4) Weight normalization:

[0049] The sum of the joint probability weights W for all matching point pairs is calculated as follows:

[0050]

[0051] The joint probability weights for each matching point pair are normalized, i.e.:

[0052]

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

[0054] Step 5) specifically includes:

[0055] 5-1) Obtain the point cloud of the main LiDAR. master and find with 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 principal coordinate system; and after weighting and normalizing the distance, obtain the minimum weighted distance.

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

[0058] 5-4) Point Cloud Merging: Merging the matched and stitched cloud points. nearest Merge into Cloud master To form a new main point cloud. master ;

[0059] 5-5) Select the next nearest LiDAR point cloud for calculation, and repeat the matching and stitching process from 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] Step 5-2) specifically involves:

[0061] (1) For each point p in the lidar point cloud i Its coordinate system is:

[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 of that point in its own coordinate system to the distance in the principal 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, i.e.:

[0071]

[0072] Where i represents the index of the LiDAR, used to distinguish different LiDAR point clouds; j represents the index of a point in the j-th LiDAR point cloud, used to traverse all points in each LiDAR point cloud. If the j-th LiDAR point cloud has N points after filtering... i There are several points, j will range from 1 to N. i Change; w normalized,i,j d represents the normalized weight of the j-th point in the i-th lidar; global,i,j This represents the distance of the j-th point in the i-th lidar in the global coordinate system. The weighted sum of the global distances from all points to the main lidar; min i This means finding the lidar point cloud with the minimum weighted global distance sum among all lidar point clouds.

[0073] Step 5-3) specifically involves:

[0074] For the point cloud of the main LiDAR master And the nearest LiDAR point cloud Cloud to the main LiDAR point cloud nearest The weighted ICP algorithm is executed for matching and splicing; in the weighted ICP algorithm, an accurate transformation T is estimated. * =(R * ,t * This minimizes the weighted error.

[0075] The weighted error E is:

[0076]

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

[0078] Minimize the weighted error E to obtain the exact transformation matrix T * for:

[0079] The construction of the global point cloud model of coordinate system one is specifically as follows:

[0080] The rotation matrix R of each lidar relative to the master lidar is... i Translation vector T i By fusing with the exact transformation matrix, the final transformation matrix T is obtained. final ,Right now:

[0081] T final =T i ·T *

[0082] Among them, T final It is the final transformation matrix, T i It is a coarse intrinsic parameter calibration transformation, T * It is a precise transformation.

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

[0084] 1. This invention employs automatic stitching technology: The method of this invention achieves automatic stitching, enabling the automatic alignment and fusion of point cloud data from multiple LiDAR systems to form a unified, global point cloud model. This process requires no 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, flat planar features in different lidar point clouds.

[0086] 3. Distance Weighting Function: The method of this invention innovatively introduces a distance-based weighting function, which dynamically adjusts the weight of each point according to the distance between the point cloud data points and the LiDAR. This weighting function gives higher weights to points that are closer to the data and gradually reduces the weights of points that are farther away, thus optimizing the point cloud stitching process.

[0087] 4. Joint Probabilistic ICP Algorithm: The method of this 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, optimizing the registration process by calculating the probability of each matching point pair. This method improves the accuracy and robustness of registration, especially when noisy data is present. Attached Figure Description

[0088] Figure 1 This embodiment presents a schematic diagram of the scenario and equipment deployment.

[0089] Figure 2 This embodiment presents a network deployment diagram;

[0090] Figure 3 Flowchart of the high-precision matching method for long-range lidar of the present invention;

[0091] Figure 4 A schematic diagram of the actual point cloud of this invention;

[0092] Figure 5 This is a schematic diagram of the weights of the point cloud in their respective lidar sensors in this embodiment;

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

[0094] Figure 7 This is a schematic diagram of the splicing result in this embodiment;

[0095] Figure 8a This is a point cloud diagram of LiDAR A in this embodiment;

[0096] Figure 8b This is a point cloud diagram of LiDAR B in this embodiment;

[0097] Figure 8c This is a point cloud diagram of the lidar C in this embodiment;

[0098] Figure 8d This is a schematic diagram of the point cloud after merging the algorithms in this embodiment. Detailed Implementation

[0099] The present invention will now be described in further detail with reference to the accompanying drawings and embodiments.

[0100] like Figure 1The diagram shown is a schematic of the scenario and equipment deployment in this embodiment. This embodiment aims to achieve comprehensive real-time monitoring, precise measurement and positioning of a large tailings workshop by deploying 6 lidars, providing high-precision real-time data for data fusion and 3D modeling, and improving the management level and production efficiency of the mine.

[0101] 1. Scene and Equipment Deployment

[0102] (1) Scene description:

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

[0104] Monitoring areas: tailings pile, track system, work platform and crane operating area.

[0105] (2) LiDAR deployment:

[0106] One lidar unit is deployed in each of the southwest, northwest, north, south, southeast, and northeast corners of the workshop to ensure comprehensive coverage.

[0107] (3) Host deployment:

[0108] An x86 host computer is used as the device for algorithm computation. The host computer is running Linux and is equipped with the PCL point cloud processing library.

[0109] (4) Figure 2 The diagram shown is a network deployment schematic for this embodiment. Specifically:

[0110] A high-speed local area network (LAN) is built using gigabit industrial switches and wired connections to meet the throughput requirements for real-time point cloud data acquisition.

[0111] like Figure 1 The diagram shown illustrates the scene and equipment deployment. The laser equipment and configuration in this embodiment are detailed below:

[0112] (1) Equipment configuration:

[0113] Each lidar unit 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 point cloud capacity of a single lidar unit is 240K / S, and the data volume is 4MB / S.

[0115] (2) Data preprocessing:

[0116] Preprocessing steps such as denoising and filtering are performed on the raw point cloud data to improve the accuracy and reliability of the data.

[0117] like Figure 3The diagram shown is a flowchart of the high-precision matching method for long-range lidar according to the present invention. In this embodiment, the specific steps are as follows:

[0118] Step 1): Unify the lidar coordinate system:

[0119] like Figure 4 As shown in the figure, the common field of view of the actual LiDAR scan is displayed. L1 and L2 represent two LiDARs. A, B, and C are three feature planes. A1, B1, and C1 represent the scan point cloud of LiDAR L1. A2, B2, and C2 represent the scan point cloud of LiDAR L2.

[0120] Based on the construction drawings or the actual on-site installation conditions, the pose of each lidar is roughly estimated, and the rotation matrix R of each lidar relative to the master lidar is initially determined. i Translation vector T i Where i represents the lidar number. The point cloud data acquired by all lidars is transformed into a global coordinate system based on the main lidar, i.e.:

[0121]

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

[0123] Step 2): Downsampling: Use the Voxel Grid filter from the PCL library to downsample the original point cloud, setting the voxel size to 0.5 meters to reduce computation and improve processing speed.

[0124] Step 3): Filter by point cloud curvature: Calculate the normal and curvature of each point using the NormalEstimation class in the PCL library, and retain points with curvature less than 0.01, as these points 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 10000 and the distance threshold to 0.1 meters, and extract the plane and obtain the interior point index using the segment_plane method.

[0126] Step 5): Area filtering: Filter the extracted planes, set the area filtering threshold to 10㎡, and only retain the point cloud of planes with an area greater than this threshold.

[0127] Step 6): Least Squares Optimization of the Plane: The plane point cloud after RANSAC filtering 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 plane equation 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) Constructing a system of linear equations: For each point P in the planar point cloud after RANSAC filtering... 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 squares of the perpendicular distances from all points to the plane, i.e.:

[0131]

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

[0133] 6.4) Solving for the optimal parameters: To minimize (a,b,c,d) of E, we need to take 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 system of equations in matrix form and solve for the optimal parameters:

[0136]

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

[0138] Step 7): Precise selection of stitched point clouds:

[0139] Based on the optimized plane and the preset distance τ (τ < 0.5 meters), matching points are re-selected, and point cloud data with high matching degree with the plane equation are retained.

[0140] Step 8): Normalize the joint probability weights:

[0141] To balance the weights of near 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 lidar point, the distance d in its own lidar coordinate system is measured. i , where i represents the index of the point cloud in the lidar, The weights are calculated using the weighting function:

[0144]

[0145] The weighting function has a range of (0 to 1), and λ is a coefficient that controls the rate of change of the weights; in this example, it is set to 0.1. ref This is a preset reference distance; in this example, it's set to 75 meters, which is half of the maximum measurement distance of the lidar (150 meters). This piecewise weighting function ensures that at distance d... i Less than d ref When the distance is d, the weight is 1, meaning these points have the greatest influence during the stitching process. i Greater than or equal to d ref The weights are calculated using an exponential decay function, which gradually reduces the influence of distant points during the stitching process. This design can better handle the stitching problem of distant point cloud data, improving the accuracy and reliability of the stitching.

[0146] 8.2) Joint probability weight calculation:

[0147] For each pair of matching points (p) 1i ,p 2i (From two lidar units respectively), calculate their joint probability weight: 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, and d are the weights of the matching point pairs in the two lidars. 1i and d 2i This refers to the distance from each device to its respective lidar sensor. The greater the distance, the lower the weight, and vice versa. For example... Figure 5 As shown in the figure, the weights of point cloud A in LiDAR sensors L1 and L2 are displayed. Since the weight of W1 is higher than that of W2, the stitching of this point should favor the data from L1.

[0148] 8.3) Weight normalization:

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

[0150] The joint probability weights for each matching point pair are normalized:

[0151]

[0152] 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 2.

[0153] Step 9): Iterative matching and splicing:

[0154] The point cloud data selected from multiple lidar sensors are gradually matched and stitched together with the point cloud of the main lidar sensor to ultimately construct a unified global point cloud model.

[0155] 9.1) Initialization: Obtain the point cloud (Cloud) of the main LiDAR. master .

[0156] 9.2) Recent Radar Match: Find matches with Cloud 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 its distance in its own coordinate system and the principal coordinate system:

[0158]

[0159] 9.2.2) Distance-weighted: Calculate the weight of each point based on the ratio of its distance in its own coordinate system to the distance in the principal coordinate system.

[0160]

[0161] 9.2.3) Normalized Weights: 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 "nearest" point cloud:

[0165]

[0166] Here, 'i' represents the index of the lidar, used to distinguish different lidar point clouds. In this embodiment, there are 6 lidars, and 'i' will vary from 1 to 6.

[0167] j represents the index of a point in the j-th lidar point cloud, used to traverse all points in each lidar point cloud. For example, if the j-th lidar point cloud has N points after filtering... i There are several points, j will range from 1 to N. i change.

[0168] w normalized,i,j This represents the normalized weight of the j-th point in the i-th lidar.

[0169] d global,i,j This represents the distance of the j-th point in the i-th lidar in the global coordinate system.

[0170] This represents the weighted sum of the global distances from all points in the LiDAR point cloud to the main LiDAR. This sum is calculated by taking the global distance (d) from each point. global,i,j Multiply by its corresponding weight (w) normalized,i,j ) obtained.

[0171] min i This indicates finding the lidar point cloud with the minimum weighted global distance sum among all lidar point clouds. This lidar point cloud is considered to be the "closest" to the main lidar point cloud.

[0172] 9.3) Weighted ICP Algorithm: This algorithm uses weighted point cloud data to perform an iterative nearest-point algorithm to minimize stitching errors and improve the stitching accuracy of point cloud data. For Cloud... master and Cloud nearest The weighted ICP algorithm is then executed 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 * These are the rotation matrix and the translation vector, w i ′ It is the normalized joint probability weight of the i-th pair of matching points between two lidars, p ref,i It is a point cloud of matching points, p tgt,iIt is the point cloud of the target point;

[0175] Minimize the weighted error E to obtain the exact transformation matrix T * for:

[0176] 9.4) Merging Point Clouds: Merging the matched and stitched point clouds nearest Merge into Cloud master To form a new main point 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 coarse intrinsic parameter 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 * , among which, T final It is the final transformation matrix, T i It is a coarse intrinsic parameter calibration transformation, T * It is a precise transformation.

[0179] like Figure 6 As shown in the figure, the stitching result is that of the traditional ICP algorithm. Since the measurement error increases with the distance of the LiDAR, the stitched result and accuracy are far lower than in reality. The stitching result of this implementation is as follows: Figure 7 As shown in the figure, the stitching algorithm of this invention is presented. First, through automatic calculation, many interfering point clouds are filtered out. Finally, a distance-based weighting function is introduced, employing joint probability to reduce the influence of distant point clouds. The stitched result has a higher accuracy and better approximation of the actual situation.

[0180] 10) Verify the accuracy and completeness of the stitched point cloud data to ensure consistency between the 3D model and the actual environment. Evaluate the system's performance using actual monitoring data, including monitoring coverage, data transmission speed, and accuracy. Its effects are as follows: Figures 8a-8d As shown.

[0181] The above embodiment demonstrates how to apply the long-range lidar high-precision matching method based on joint probability weight 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 3D modeling, and improving the management level and production efficiency of the mine.

[0182] Those skilled in the art will understand that the above description is merely a preferred embodiment of the present invention, and the features described in the various embodiments and / or claims of this disclosure can be combined or combined in various ways, even if such combinations or combinations are not explicitly described in this disclosure. This is not intended to limit the present invention. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art can still modify the technical solutions described in the foregoing embodiments or make equivalent substitutions for some of the technical features. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

[0183] Although preferred embodiments of the invention have been described, those skilled in the art, upon learning the basic inventive concept, can make other changes and modifications to these embodiments. Therefore, the appended claims are intended to be interpreted as including both the preferred embodiments and all changes and modifications falling within the scope of the invention. Clearly, those skilled in the art can make various alterations and modifications to the invention without departing from its spirit and scope. Thus, if these modifications and modifications of the invention fall within the scope of the claims and their equivalents, the invention is also intended to include these modifications and modifications.

Claims

1. A high-precision matching method for long-range lidar based on joint probability weight iteration, characterized in that, Includes the following steps: 1) Deploy lidar in the target workshop, collect point cloud data and send it to the PCL point cloud processing library of the terminal. After preprocessing and calibration, convert all the point cloud data acquired by lidar into the global coordinate system based on the main lidar. 2) Process the point cloud data in the global coordinate system to extract the planar point cloud, and then filter the planar point cloud to obtain the filtered planar point cloud; 3) The filtered planar point cloud is refined using the least squares method to optimize the coefficients of the planar equation and fit the planar equation of the point cloud data. 4) Select the stitched point cloud and match the point cloud data of the lidar using the joint probability weight normalization method during the stitching process; The joint probability weight normalization method includes the following steps: 4-1) For each lidar point, measure the distance in its own lidar coordinate system. for: ； Where i represents the index of the point cloud in the lidar, For lidar Coordinates in its own radar coordinate system (local); 4-2) Calculate the weights using a weighting function: ； The range of the weighting function is (0~1). The coefficient used to control the rate of change of the weights. This is a preset reference distance; 4-3) Calculation of joint probability weights: For each pair of matched points from two lidars respectively Calculate their joint probability weights as follows: ； in, These are the joint weights of the matching point pairs in the two lidar systems. and It is the distance from the matching point to its respective LiDAR; the farther the distance, the lower the weight. 4-4) Weight normalization: Calculate the sum of the joint probability weights for all matching point pairs. for: ； The joint probability weights for each matching point pair are normalized, i.e.: ； in, It is the first of LiDAR 1 and LiDAR 2 Normalized joint probability weights for matching points; 5) The point cloud data selected from multiple lidars are iteratively matched and stitched with the point cloud of the main lidar to obtain an accurate change matrix. Combined with the intrinsic parameters of the calibrated main lidar, a unified global point cloud model is constructed.

2. The high-precision matching method for long-range lidar based on joint probability weight iteration according to claim 1, characterized in that, Step 1) includes the following steps: 1-1) Distribute multiple lidars evenly in the workshop to enable the lidars to achieve full coverage scanning of the workshop, and send the point cloud data acquired by all lidars to the PCL point cloud processing library of the terminal. 1-2) The PCL point cloud processing library performs preprocessing steps such as denoising and filtering on the raw point cloud data to improve data accuracy and reliability; 1-3) Perform a rough estimate of the pose of each lidar and preliminarily determine the rotation matrix of each lidar relative to the master lidar. Translation vector ,in, Indicates the number of the lidar; 1-4) Convert all point cloud data acquired by the lidar to the global coordinate system (global) with the main lidar as the reference, i.e.: ； in, For lidar The coordinates in its own radar coordinate system (local). These are the coordinate values ​​under the global coordinate system of the main lidar.

3. The high-precision matching method for long-range lidar based on joint probability weight iteration according to claim 1, characterized in that, Step 2) includes the following steps: 2-1) Downsampling: The point cloud data is downsampled using the Voxel Grid filter in the PCL point cloud processing library. The voxel size is set to m meters to reduce computation and improve processing speed. 2-2) The PCL point cloud processing library calculates the normal and curvature of each point, and retains and identifies points with curvature less than k; 2-3) The RANSAC algorithm is used for plane fitting and extraction. The number of iterations and the distance threshold are set. The plane is extracted and the inlier index is obtained by using the segment_plane method. 2-4) Filter the extracted planes by setting an area filtering threshold and retaining only the point cloud of the planes with an area greater than the threshold.

4. The high-precision matching method for long-range lidar based on joint probability weight iteration according to claim 1, characterized in that, Step 3) includes the following steps: 3-1) Define the plane equation as: ； in, It is the normal vector of the plane. It is the distance from the plane to the origin; 3-2) For each point in the planar point cloud after RANSAC filtering The linear equation is constructed as follows: ; 3-3) Minimize the sum of squares of the perpendicular distances from all points to the plane using the least squares method, i.e.: ； in, It is the number of points in a planar point cloud; 3-4) respectively , , , ,and Find the partial derivatives and set them equal to zero. The matrix representation is as follows: ； in, This represents the index of a point in a planar point cloud, used to traverse all points in the planar point cloud; Indicates the first The coordinates of the points This represents the total number of points in a planar point cloud. 3-5) Solve the system of linear equations to obtain... , , , The optimal value is obtained by fitting the plane equation of the point cloud data.

5. The high-precision matching method for long-range lidar based on joint probability weight iteration according to claim 1, characterized in that, The selection of the spliced ​​point cloud specifically refers to: A preset distance τ, where τ < 0.5 meters, is used, along with the optimized plane equations. Then, re-filter the matching points and retain the point cloud data whose matching degree with the plane equation is greater than the set value.

6. The high-precision matching method for long-range lidar based on joint probability weight iteration according to claim 1, characterized in that, Step 5) specifically involves: 5-1) Obtain the point cloud of the main lidar and find with Recent LiDAR point clouds ; 5-2) For each point p in the lidar point cloud i Calculate the distance between its own coordinate system and the principal coordinate system; and after weighting and normalizing the distance, obtain the minimum weighted distance. 5-3) Use weighted point cloud data to perform the iterative nearest point algorithm to minimize the stitching error and improve the stitching accuracy of point cloud data; 5-4) Point cloud merging: Merging the matched and stitched points... merge into A new principal point cloud is formed. ; 5-5) Select the next nearest LiDAR point cloud for calculation, and repeat the matching and stitching process from 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.

7. The high-precision matching method for long-range lidar based on joint probability weight iteration according to claim 6, characterized in that, Step 5-2) specifically involves: (1) For each point p in the lidar point cloud i Its coordinate system is: ； 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 of that point in its own coordinate system to the distance in the principal 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: ； in, It 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: ； in, This represents the index of the lidar, used to distinguish different lidar point clouds; Indicates the first The index of a point in a LiDAR point cloud is used to iterate through all points in each LiDAR point cloud. If the index of the first point is... After filtering by a lidar, the point cloud has One point, From 1 to change; Indicates the first The first of the lidar Normalized weights for each point; Indicates the first The first of the lidar The distance of each point in the main coordinate system global; This is the weighted sum of the global distances from all points to the main lidar. This means finding the lidar point cloud with the minimum weighted global distance sum among all lidar point clouds.

8. The high-precision matching method for long-range lidar based on joint probability weight iteration according to claim 6, characterized in that, Step 5-3) specifically involves: For the point cloud of the main lidar The point cloud of the lidar closest to the main lidar. The weighted ICP algorithm is then executed for matching and splicing; in the weighted ICP algorithm, an accurate transform is estimated. This minimizes the weighted error. Weighted error for: ； in, and These are the rotation matrix and the translation vector, respectively. It is one of two lidars. Normalized joint probability weights for matching points, It is a point cloud of matching points. It is the point cloud of the target point; Minimize weighted error To obtain the exact transformation matrix for: .

9. The high-precision matching method for long-range lidar based on joint probability weight iteration according to claim 6, characterized in that, The construction of the global point cloud model of coordinate system one is specifically as follows: The rotation matrix of each lidar relative to the master lidar Translation vector The final transformation matrix is ​​obtained by fusing it with the exact transformation matrix. ,Right now: ； in, It is the final transformation matrix. It is a coarse intrinsic parameter calibration transformation. It is a precise transformation.

Images