A fusion positioning method and system for dynamic environment of underground mines

Abstract

The present invention proposes a fusion positioning method and system for the dynamic environment of underground mining, the method comprising: establishing a priori map for positioning; obtaining the first real-time point cloud data of the target vehicle, and registering the point clouds that do not meet the conditions in the first real-time point cloud data; judging the dynamics of the first real-time point cloud data, eliminating the dynamic points of the first real-time point cloud data to obtain the second real-time point cloud data; obtaining the first real-time IMU data, the first chassis speed information and the first slam posture of the target vehicle; using the initial posture and the unit time posture transformation to superimpose to obtain the updated odometer information; fusing the updated odometer information, the second real-time point cloud data and the first real-time IMU data to output the final fusion positioning information. Based on this method, a fusion positioning system for the dynamic environment of underground mining is also proposed. The present invention eliminates the influence of dynamic obstacles in the environment on positioning in a long corridor environment, and improves the accuracy and robustness of positioning.

Description

Technical Field

[0001] The present invention belongs to the technical field of autonomous driving vehicle positioning, and in particular relates to a fusion positioning method and system for dynamic environments of underground mines. Background Art

[0002] In a mining environment, the application of autonomous driving technology can greatly improve the efficiency and safety of mining operations. However, the mining environment is complex, including long corridors, forks and other complex terrains, which puts higher requirements on autonomous driving technology. At the same time, positioning technology is also an important part of autonomous driving technology. It determines the position and posture of the autonomous driving vehicle in the environment, thus affecting the safety and accuracy of autonomous driving.

[0003] In the existing technology, laser SLAM (Simultaneous Localization and Mapping) is usually used for mapping and positioning. Laser SLAM is a technology that obtains environmental information through laser radar and simultaneously performs positioning and map construction. However, the underground environment is complex, the long corridor effect is very obvious, and the dynamic points have a great impact on positioning. Secondly, Odom (odometer), IMU (inertial measurement unit), Lidar (laser radar) and UWB fusion positioning will also be used. The odometer information is parsed by CAN message data, the speed information of the left and right wheels is read, the model is simplified, and the rear wheel differential model is constructed, thereby deriving the odometer model centered on the midpoint of the rear wheel axle. However, the existing positioning technology generally uses multi-sensor fusion positioning of IMU, Odom, Lidar, and UWB. Odom comes from chassis information and has a large cumulative error. Therefore, how to achieve dynamic point elimination of autonomous driving in underground mining environments, reduce the cumulative error of input, and improve the accuracy and stability of positioning is a problem that needs to be solved urgently. Summary of the invention

[0004] In order to solve the above technical problems, the present invention proposes a fusion positioning method and system for the dynamic environment of underground mines, which eliminates the influence of dynamic obstacles in the environment on positioning in a long corridor environment and improves the accuracy and robustness of positioning.

[0005] To achieve the above object, the present invention adopts the following technical solutions:

[0006] A fusion positioning method for a dynamic environment of an underground mine comprises the following steps:

[0007] After the historical LiDAR data, historical IMU data and historical odometer data of the target vehicle are respectively time synchronized and spatially calibrated, SLAM mapping is performed to obtain a point cloud map; the point cloud map is preprocessed to obtain a priori map for positioning;

[0008] Acquire first real-time point cloud data of the target vehicle, and align point clouds in the first real-time point cloud data that are not in the same coordinate system; then use a clustering algorithm to determine the dynamics of the first real-time point cloud data, and remove dynamic points from the first real-time point cloud data to obtain second real-time point cloud data;

[0009] Acquire the first real-time IMU data, the first chassis speed information and the first slam posture of the target vehicle; acquire the heading angle from the first real-time IMU data, and acquire the initial posture from the first slam posture; and obtain the updated odometer information by continuously superimposing the initial posture and the unit time posture transformation;

[0010] The updated odometer information, the second real-time point cloud data and the first real-time IMU data are Kalman fused to output the final fused positioning information.

[0011] Furthermore, the time synchronization process includes: using the ptp time synchronization strategy to synchronize the historical lidar data and the historical IMU data.

[0012] Furthermore, the process of performing SLAM mapping to obtain a point cloud map includes:

[0013] The calibrated historical lidar data, historical IMU data, and historical odometer data are all input into the real-time positioning and map building algorithm to obtain a point cloud map.

[0014] Furthermore, the process of acquiring first real-time point cloud data of the target vehicle and registering point clouds in the first real-time point cloud data that are not in the same coordinate system includes:

[0015] Obtain continuous scanning frames of the laser radar, and each frame includes the same number of points;

[0016] The iterative closest point algorithm is used to align point cloud data that are not in the same coordinate system to ensure accurate alignment between point cloud data.

[0017] Furthermore, the specific process of using the iterative closest point algorithm to align point cloud data in different coordinate systems includes:

[0018] Compute the Euclidean distance between adjacent points:

[0019] ;

[0020] in, For the The location of the scanned point cloud; For the The location of the scanned point cloud;

[0021] If the total number of scans in a time window is ,point The number of occurrences is , and the position change at each occurrence is lower than the position threshold, then the consistency score of a time window is calculated as:

[0022] ;

[0023] If the consistency score Less than the scoring threshold, calculate the standard deviation of its occurrence position:

[0024] ;

[0025] in, Yes of The average position of the occurrences, is the standard deviation;

[0026] Determine the dynamics of a point using the decision formula:

[0027] ;

[0028] in, is the minimum threshold of standard deviation; Maximum threshold for consistency scoring.

[0029] Furthermore, the specific process of using the clustering algorithm to determine the dynamics of the first real-time point cloud data includes:

[0030] ;

[0031] in, For real-time point cloud data in time The cluster centroid of For real-time point cloud data in time The cluster centroid of is the centroid offset of the point cloud data at adjacent moments;

[0032] ;

[0033] in, For real-time point cloud data in time Clustered shape descriptors; For real-time point cloud data in time Clustered shape descriptors; It is the difference of real-time point cloud data in consecutive time frames.

[0034] Furthermore, the process of performing dynamic point removal on the first real-time point cloud data includes:

[0035] ;

[0036] in, is the preset threshold for centroid shift; A preset threshold for shape change;

[0037] If the centroid offset is greater than the preset threshold of the centroid offset, or the shape change is greater than one of the preset thresholds of the shape change, it is identified as a dynamic point and marked as 1, otherwise it is a non-dynamic point and marked as 0;

[0038] The statistical outlier removal algorithm is used to remove dynamic points.

[0039] Furthermore, the process of obtaining updated odometer information by continuously superimposing the initial posture and the unit time posture transformation includes: multiplying the initial posture with the unit time posture transformation to obtain complete position information, and publishing the odometer to a complete coordinate position relationship to obtain updated odometer information.

[0040] Furthermore, the process of performing Kalman fusion positioning on the updated odometer information, the second real-time point cloud data and the first real-time IMU data includes:

[0041] The first real-time IMU data or the updated odometer information is used as the predicted value, the second real-time point cloud data and the point cloud map output by the prior map are used as the NDT scanning matching result as the observation value, and the fusion positioning outputs the final fusion positioning information.

[0042] The present invention also proposes a fusion positioning system for a dynamic environment of underground mines, comprising a preprocessing module, a registration and rejection module, an updating module and a fusion positioning module;

[0043] The preprocessing module is used to perform time synchronization and spatial calibration on the acquired historical laser radar data, historical IMU data and historical odometer data of the target vehicle, and then perform SLAM mapping to obtain a point cloud map; and obtain a priori map for positioning after preprocessing the point cloud map;

[0044] The registration and elimination module is used to obtain first real-time point cloud data of the target vehicle, and to register point clouds in the first real-time point cloud data that are not in the same coordinate system; then a clustering algorithm is used to determine the dynamics of the first real-time point cloud data, and dynamic point elimination is performed on the first real-time point cloud data to obtain second real-time point cloud data;

[0045] The updating module is used to obtain the first real-time IMU data, the first chassis speed information and the first slam posture of the target vehicle; obtain the heading angle from the first real-time IMU data, and obtain the initial posture from the first slam posture; and obtain the updated odometer information by continuously superimposing the initial posture and the unit time posture transformation;

[0046] The fusion positioning module is used to perform Kalman fusion positioning on the updated odometer information, the second real-time point cloud data and the first real-time IMU data to output final fusion positioning information.

[0047] The effects provided in the content of the invention are only the effects of the embodiments, not all the effects of the invention. One of the above technical solutions has the following advantages or beneficial effects:

[0048] The invention proposes a fusion positioning method and system for a dynamic environment of an underground mine, belonging to the technical field of positioning of an autonomous driving vehicle. The method comprises the following steps: performing time synchronization and spatial calibration on the acquired historical laser radar data, historical IMU data and historical odometer data of a target vehicle, respectively, and then performing slam mapping to obtain a point cloud map; performing preprocessing on the point cloud map to obtain a priori map for positioning; obtaining first real-time point cloud data of the target vehicle, and aligning point clouds in the first real-time point cloud data that are not in the same coordinate system; then using a clustering algorithm to judge the dynamics of the first real-time point cloud data, and performing dynamic point elimination on the first real-time point cloud data to obtain second real-time point cloud data; obtaining first real-time IMU data, first chassis speed information and first slam posture of the target vehicle; obtaining a heading angle from the first real-time IMU data, and obtaining an initial posture from the first slam posture; obtaining updated odometer information by continuously superimposing the initial posture and the unit time posture transformation; performing Kalman fusion positioning on the updated odometer information, the second real-time point cloud data and the first real-time IMU data to output final fused positioning information. Based on a fusion positioning method for a dynamic environment of an underground mine, a fusion positioning system for a dynamic environment of an underground mine is also proposed. The invention eliminates the influence of dynamic obstacles in the environment on positioning in a long corridor environment, thereby improving the accuracy and robustness of positioning.

[0049] The present invention fuses IMU data with vehicle speed information and publishes it as an odometer, avoiding the cumulative error caused by the traditional vehicle speed odometer model, and the odometer posture can be adjusted according to actual conditions. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] Figure 1 A flow chart of a fusion positioning method for a dynamic environment of an underground mine proposed in Example 1 of the present invention;

[0051] Figure 2 This is a schematic diagram of a framework obtained by obtaining a priori map proposed in Example 1 of the present invention;

[0052] Figure 3 This is a schematic diagram of the time synchronization framework proposed in Example 1 of the present invention;

[0053] Figure 4A schematic diagram of a framework for dynamic point detection, point cloud matching and fusion positioning proposed in Example 1 of the present invention;

[0054] Figure 5 This is a schematic diagram of a framework for obtaining updated odometer information proposed in Embodiment 1 of the present invention;

[0055] Figure 6 This is a schematic diagram of a fusion positioning system for dynamic environments in underground mines proposed in Example 2 of the present invention. DETAILED DESCRIPTION

[0056] In order to clearly illustrate the technical features of the present solution, the present invention is described in detail below through specific implementation methods and in conjunction with the accompanying drawings. The disclosure below provides many different embodiments or examples for realizing different structures of the present invention. In order to simplify the disclosure of the present invention, the components and settings of specific examples are described below. In addition, the present invention may repeat reference numbers and / or letters in different examples. This repetition is for the purpose of simplification and clarity, and does not itself indicate the relationship between the various embodiments and / or settings discussed. It should be noted that the components illustrated in the accompanying drawings are not necessarily drawn to scale. The present invention omits the description of known components and processing techniques and processes to avoid unnecessary limitations on the present invention.

[0057] Example 1

[0058] Embodiment 1 of the present invention proposes a fusion positioning method for a dynamic environment of an underground mine, which is used to solve the technical problems existing in the fusion positioning of a dynamic environment of an underground mine in the prior art.

[0059] Figure 1 A flow chart of a fusion positioning method for a dynamic environment of an underground mine proposed in Example 1 of the present invention;

[0060] In step S1, the historical lidar data, historical IMU data and historical odometer data of the acquired target vehicle are respectively time synchronized and spatially calibrated, and then SLAM mapping is performed to obtain a point cloud map; the point cloud map is preprocessed to obtain a priori map for positioning. Figure 2 This is a schematic diagram of a framework obtained by obtaining a priori map proposed in Example 1 of the present invention;

[0061] The time synchronization process includes: using the ptp time synchronization strategy to synchronize the historical lidar data and the historical IMU data. Figure 3This is a schematic diagram of the framework of time synchronization proposed in Example 1 of the present invention; when performing time synchronization, a domain controller is used, and the domain controller is connected to the laser radar and IMU through PTP signals. The domain controller is connected to RADAR through CAN signals; the domain controller is connected to the ARS548 millimeter wave radar through gPTP. The domain controller is also connected to the chassis of the car through the central gateway domain controller. The domain controller is connected to the IMU through PPS and NMEA respectively.

[0062] The data collected by the laser radar sensor is stored in the form of a bag package, and each data is input into the real-time positioning and map construction algorithm for SLAM mapping to obtain a point cloud map in pcd format. In this application, the real-time positioning and map construction algorithm adopts the fast-lio algorithm.

[0063] The process of performing SLAM mapping to obtain a point cloud map includes: inputting the calibrated historical lidar data, historical IMU data, and historical odometer data into the real-time positioning and map construction algorithm to obtain a point cloud map.

[0064] The point cloud map is processed by downsampling, radius filtering, ground point cloud filtering, noise processing, etc. to obtain a point cloud map with higher accuracy as a priori map for positioning.

[0065] Figure 4 A schematic diagram of a framework for dynamic point detection, point cloud matching and fusion positioning proposed in Example 1 of the present invention;

[0066] In step S2, first real-time point cloud data of the target vehicle is obtained, and point clouds in the first real-time point cloud data that are not in the same coordinate system are registered; then a clustering algorithm is used to determine the dynamics of the first real-time point cloud data, and dynamic points of the first real-time point cloud data are removed to obtain second real-time point cloud data;

[0067] First, collect continuous scan frames from the LiDAR. Each frame should contain the same number of points. If the point cloud data is not in the same coordinate system, data registration is required first. The ICP (Iterative Closest Point) algorithm registration method can be used to ensure accurate alignment between point clouds.

[0068] The specific process of using the iterative closest point algorithm to align point cloud data in different coordinate systems includes:

[0069] Compute the Euclidean distance between adjacent points:

[0070] Compute the Euclidean distance between adjacent points:

[0071] ;

[0072] in, For the The location of the scanned point cloud; For the The location of the scanned point cloud;

[0073] If the total number of scans in a time window is ,point The number of occurrences is , and the position change at each occurrence is lower than the position threshold, then the consistency score of a time window is calculated as:

[0074] ;

[0075] If the consistency score Close to 1, indicating a point remains static throughout the time window; if If it is lower, it indicates that the point is changing dynamically.

[0076] Across multiple observations, more sophisticated statistical analysis can be performed on the consistency of each point.

[0077] If the consistency score Less than the scoring threshold, calculate the standard deviation of its occurrence position:

[0078] ;

[0079] in, Yes of The average position of the occurrences, is the standard deviation; standard deviation The smaller it is, the more stable the point position is.

[0080] Based on the above statistical analysis, a decision formula can be defined to finally determine the dynamics of the point. If the value of is greater than a certain threshold or the consistency score C is lower than a certain threshold, the point is considered dynamic:

[0081] Determine the dynamics of a point using the decision formula:

[0082] ;

[0083] in, is the minimum threshold of standard deviation; Maximum threshold for consistency scoring.

[0084] The specific process of using the clustering algorithm to determine the dynamics of the first real-time point cloud data includes:

[0085] ;

[0086] in For real-time point cloud data in time The cluster centroid of For real-time point cloud data in time The cluster centroid of is the centroid offset of the point cloud data at adjacent moments;

[0087] ;

[0088] in, For real-time point cloud data in time Clustered shape descriptors; For real-time point cloud data in time Clustered shape descriptors; It is the difference of real-time point cloud data in consecutive time frames.

[0089] The process of dynamic point removal for the first real-time point cloud data includes:

[0090] ;

[0091] in, is the preset threshold for centroid shift; A preset threshold for shape change;

[0092] If the centroid offset is greater than the preset threshold of the centroid offset, or the shape change is greater than one of the preset thresholds of the shape change, it is identified as a dynamic point and marked as 1, otherwise it is a non-dynamic point and marked as 0;

[0093] According to the above scoring or judgment results, the dynamic points are removed by using the statistical outlier removal algorithm. The statistical outlier removal algorithm (Statistical Outlier Removal) is used for filtering to obtain the filtered second real-time point cloud data.

[0094] In step S3, the first real-time IMU data, the first chassis speed information and the first slam posture of the target vehicle are obtained; the heading angle is obtained from the first real-time IMU data, and the initial posture is obtained from the first slam posture; the updated odometer information is obtained by continuously superimposing the initial posture and the unit time posture transformation;

[0095] Figure 5 This is a schematic diagram of a framework for obtaining updated odometer information proposed in Embodiment 1 of the present invention;

[0096] The yaw value is read from the IMU data as the heading angle, the initial attitude is multiplied by the unit time attitude transformation, and the position information is continuously superimposed to obtain the complete position information, and the coordinate transformation relationship from odom to base_link (vehicle body) is published. The frequency is 100hz and the frequency is adjustable.

[0097] In step S4, the updated odometer information, the second real-time point cloud data and the first real-time IMU data are subjected to Kalman fusion positioning to output final fused positioning information.

[0098] The Kalman fusion process includes: using the first real-time IMU data or the updated odometer information as the predicted value, using the second real-time point cloud data and the point cloud map output by the prior map to perform NDT scanning matching to obtain the result as the observation value, and fusing the positioning to output the final fused positioning information.

[0099] In the underground environment, due to the obvious long corridor effect, the odom and IMU weight values ​​in the EKF are larger, which is a more accurate result of track calculation.

[0100] Embodiment 1 of the present invention proposes a fusion positioning method for a dynamic environment of an underground mine, which eliminates the influence of dynamic obstacles in the environment on positioning in a long corridor environment, thereby improving the accuracy and robustness of positioning.

[0101] Embodiment 1 of the present invention proposes a fusion positioning method for the dynamic environment of underground mining, which fuses IMU data with vehicle speed information and publishes it as an odometer, avoiding the cumulative error caused by the traditional vehicle speed odometer model, and the odometer posture can be adjusted according to actual conditions.

[0102] Example 2

[0103] Based on the fusion positioning method for the dynamic environment of underground mining proposed in Example 1 of the present invention, Example 2 of the present invention further proposes a fusion positioning system for the dynamic environment of underground mining. Figure 6 A schematic diagram of a fusion positioning system for a dynamic environment of an underground mine proposed in Embodiment 2 of the present invention, the system comprising a preprocessing module, a registration and rejection module, an updating module and a fusion positioning module;

[0104] The preprocessing module is used to perform time synchronization and spatial calibration on the acquired historical laser radar data, historical IMU data and historical odometer data of the target vehicle, and then perform SLAM mapping to obtain a point cloud map; and obtain a priori map for positioning after preprocessing the point cloud map;

[0105] The registration and elimination module is used to obtain first real-time point cloud data of the target vehicle, and to register point clouds in the first real-time point cloud data that are not in the same coordinate system; then a clustering algorithm is used to determine the dynamics of the first real-time point cloud data, and dynamic point elimination is performed on the first real-time point cloud data to obtain second real-time point cloud data;

[0106] The updating module is used to obtain the first real-time IMU data, the first chassis speed information and the first slam posture of the target vehicle; obtain the heading angle from the first real-time IMU data, and obtain the initial posture from the first slam posture; and obtain the updated odometer information by continuously superimposing the initial posture and the unit time posture transformation;

[0107] The fusion positioning module is used to perform Kalman fusion positioning on the updated odometer information, the second real-time point cloud data and the first real-time IMU data to output final fusion positioning information.

[0108] During the execution of the preprocessing module, the time synchronization process includes: using the ptp time synchronization strategy to synchronize the historical lidar data and the historical IMU data.

[0109] The process of performing SLAM mapping to obtain a point cloud map includes: inputting the calibrated historical lidar data, historical IMU data, and historical odometer data into the real-time positioning and map construction algorithm to obtain a point cloud map.

[0110] During the execution of the registration and rejection module, the process of obtaining the first real-time point cloud data of the target vehicle and registering the point clouds in the first real-time point cloud data that are not in the same coordinate system includes:

[0111] Obtain continuous scanning frames of the laser radar, and each frame includes the same number of points;

[0112] The iterative closest point algorithm is used to align point cloud data that are not in the same coordinate system to ensure accurate alignment between point cloud data.

[0113] The specific process of using the iterative closest point algorithm to align point cloud data in different coordinate systems includes:

[0114] Compute the Euclidean distance between adjacent points:

[0115] Compute the Euclidean distance between adjacent points:

[0116] ;

[0117] in, For the The location of the scanned point cloud; For the The location of the scanned point cloud;

[0118] If the total number of scans in a time window is ,point The number of occurrences is , and the position change at each occurrence is lower than the position threshold, then the consistency score of a time window is calculated as:

[0119] ;

[0120] If the consistency score If it is smaller than the scoring threshold, the standard deviation of its occurrence position is calculated;

[0121] ;

[0122] in, Yes of The average position of the occurrences, is the standard deviation;

[0123] Determine the dynamics of a point using the decision formula:

[0124] ;

[0125] in, is the minimum threshold of standard deviation; Maximum threshold for consistency scoring.

[0126] The specific process of using the clustering algorithm to determine the dynamics of the first real-time point cloud data includes:

[0127] ;

[0128] in, For real-time point cloud data in time The cluster centroid of For real-time point cloud data in time The cluster centroid of is the centroid offset of the point cloud data at adjacent moments;

[0129] ;

[0130] in, For real-time point cloud data in time Clustered shape descriptors; For real-time point cloud data in time Clustered shape descriptors; It is the difference of real-time point cloud data in consecutive time frames.

[0131] The process of dynamic point removal for the first real-time point cloud data includes:

[0132] ;

[0133] in, is the preset threshold for centroid shift; A preset threshold for shape change;

[0134] If the centroid offset is greater than the preset threshold of the centroid offset, or the shape change is greater than one of the preset thresholds of the shape change, it is identified as a dynamic point and marked as 1, otherwise it is a non-dynamic point and marked as 0;

[0135] The statistical outlier removal algorithm is used to remove dynamic points.

[0136] The update module executes the following steps: multiplying the initial posture by the unit time posture transformation to obtain complete position information, and publishing the odometer to the complete coordinate position relationship to obtain the updated odometer information.

[0137] In the fusion positioning module, the first real-time IMU data or the updated odometer information is used as the prediction value, and the second real-time point cloud data and the point cloud map output by the prior map are used as the NDT scanning matching result as the observation value, and the fusion positioning outputs the final fusion positioning information.

[0138] Embodiment 2 of the present invention proposes a fusion positioning system for a dynamic environment in an underground mine, which eliminates the influence of dynamic obstacles in the environment on positioning in a long corridor environment, thereby improving the accuracy and robustness of positioning.

[0139] Embodiment 2 of the present invention proposes a fusion positioning system for the dynamic environment of underground mines, which fuses IMU data with vehicle speed information and publishes it as an odometer, avoiding the cumulative error caused by the traditional vehicle speed odometer model, and the odometer posture can be adjusted according to actual conditions.

[0140] For the description of the relevant parts of a fusion positioning system for a dynamic environment of an underground mine proposed in Example 2 of the present invention, please refer to the detailed description of the corresponding parts of a fusion positioning method for a dynamic environment of an underground mine provided in Example 1 of the present application, which will not be repeated here.

[0141] It should be noted that, in this article, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply that there is any such actual relationship or order between these entities or operations. Moreover, the term "include", "comprise" or any other variant thereof is intended to cover non-exclusive inclusion, so that the process, method, article or equipment that includes a series of elements are inherent to the elements. In the absence of more restrictions, the elements limited by the sentence "comprise one..." do not exclude the presence of other identical elements in the process, method, article or equipment that includes the elements. In addition, the above-mentioned technical solution provided in the embodiment of the present application is consistent with the corresponding technical solution in the prior art in principle, and the part is not described in detail, so as not to repeat too much.

[0142] Although the above describes the specific implementation of the present invention in conjunction with the accompanying drawings, it is not intended to limit the scope of protection of the present invention. For those skilled in the art, other different forms of modifications or deformations can be made on the basis of the above description. It is not necessary and impossible to list all the implementation methods here. On the basis of the technical solution of the present invention, various modifications or deformations that can be made by those skilled in the art without creative work are still within the scope of protection of the present invention.

Claims

1. A fusion positioning method for dynamic environment of underground mines, characterized in that: The following steps are involved: After the historical LiDAR data, historical IMU data and historical odometer data of the target vehicle are respectively time synchronized and spatially calibrated, SLAM mapping is performed to obtain a point cloud map; the point cloud map is preprocessed to obtain a priori map for positioning; Acquire first real-time point cloud data of the target vehicle, and align point clouds in the first real-time point cloud data that are not in the same coordinate system; then use a clustering algorithm to determine the dynamics of the first real-time point cloud data, and remove dynamic points from the first real-time point cloud data to obtain second real-time point cloud data; The process of acquiring first real-time point cloud data of the target vehicle and registering point clouds in the first real-time point cloud data that are not in the same coordinate system includes: Obtain continuous scanning frames of the laser radar, and each frame includes the same number of points; The iterative closest point algorithm is used to align point cloud data that are not in the same coordinate system to ensure accurate alignment between point cloud data; The specific process of using the iterative closest point algorithm to align point cloud data in different coordinate systems includes: Compute the Euclidean distance between adjacent points: ； in, For the The location of the scanned point cloud; For the The location of the scanned point cloud; If the total number of scans in a time window is ,point The number of occurrences is , and the position change at each occurrence is lower than the position threshold, then the consistency score of a time window is calculated as: ； If the consistency score Less than the scoring threshold, calculate the standard deviation of its occurrence position: ； in, Yes of The average position of the occurrences, is the standard deviation; Determine the dynamics of a point using the decision formula: ； in, is the minimum threshold of standard deviation; The maximum threshold for consistency scoring; The specific process of using the clustering algorithm to determine the dynamics of the first real-time point cloud data includes: ； in, For real-time point cloud data in time The cluster centroid of For real-time point cloud data in time The cluster centroid of is the centroid offset of the point cloud data at adjacent moments; ； in, For real-time point cloud data in time Clustered shape descriptors; For real-time point cloud data in time Clustered shape descriptors; It is the difference of real-time point cloud data in consecutive time frames; The process of dynamic point removal for the first real-time point cloud data includes: if ; in, is the preset threshold for centroid shift; A preset threshold for shape change; If the centroid offset is greater than the preset threshold of the centroid offset, or the shape change is greater than one of the preset thresholds of the shape change, it is identified as a dynamic point and marked as 1, otherwise it is a non-dynamic point and marked as 0; The statistical outlier removal algorithm is used to remove dynamic points; Acquire the first real-time IMU data, the first chassis speed information and the first slam posture of the target vehicle; acquire the heading angle from the first real-time IMU data, and acquire the initial posture from the first slam posture; and obtain the updated odometer information by continuously superimposing the initial posture and the unit time posture transformation; The updated odometer information, the second real-time point cloud data and the first real-time IMU data are Kalman fused to output the final fused positioning information.

2. A fusion positioning method for dynamic environment of underground mines according to claim 1, characterized in that: The time synchronization process includes: using the ptp time synchronization strategy to synchronize the historical lidar data and the historical IMU data.

3. The fusion positioning method for dynamic environment of underground mines according to claim 1 is characterized in that: The process of performing SLAM mapping to obtain a point cloud map includes: The calibrated historical lidar data, historical IMU data, and historical odometer data are all input into the real-time positioning and map building algorithm to obtain a point cloud map.

4. The fusion positioning method for dynamic environment of underground mines according to claim 1 is characterized in that: The process of obtaining updated odometer information by continuously superimposing the initial posture and the unit time posture transformation includes: multiplying the initial posture with the unit time posture transformation to obtain complete position information, and publishing the odometer to a complete coordinate position relationship to obtain updated odometer information.

5. The fusion positioning method for dynamic environment of underground mines according to claim 1 is characterized in that: The process of Kalman fusion positioning of the updated odometer information, the second real-time point cloud data and the first real-time IMU data includes: The first real-time IMU data or the updated odometer information is used as the predicted value, the second real-time point cloud data and the point cloud map output by the prior map are used as the NDT scanning matching result as the observation value, and the fusion positioning outputs the final fusion positioning information.

6. A fusion positioning system for a dynamic environment of an underground mine, used to execute a fusion positioning method for a dynamic environment of an underground mine as claimed in any one of claims 1 to 5, characterized in that: It includes pre-processing module, registration and rejection module, updating module and fusion positioning module; The preprocessing module is used to perform time synchronization and spatial calibration on the acquired historical laser radar data, historical IMU data and historical odometer data of the target vehicle, and then perform SLAM mapping to obtain a point cloud map; and obtain a priori map for positioning after preprocessing the point cloud map; The registration and elimination module is used to obtain first real-time point cloud data of the target vehicle, and to register point clouds in the first real-time point cloud data that are not in the same coordinate system; then a clustering algorithm is used to determine the dynamics of the first real-time point cloud data, and dynamic point elimination is performed on the first real-time point cloud data to obtain second real-time point cloud data; The updating module is used to obtain the first real-time IMU data, the first chassis speed information and the first slam posture of the target vehicle; obtain the heading angle from the first real-time IMU data, and obtain the initial posture from the first slam posture; and obtain the updated odometer information by continuously superimposing the initial posture and the unit time posture transformation; The fusion positioning module is used to perform Kalman fusion positioning on the updated odometer information, the second real-time point cloud data and the first real-time IMU data to output final fusion positioning information.

Images