Underground multi-source fusion positioning method and system

Abstract

The invention discloses an underground multi-source fusion positioning method and system. The method comprises the following steps that initialization is carried out based on a known initial pose of underground equipment during starting; acquiring environment point cloud data scanned by a vehicle-mounted laser radar of underground equipment and motion information acquired by an inertial measurement unit; registration is carried out based on the environment point cloud data and a pre-constructed high-precision three-dimensional map, and first pose estimation is obtained by using an OpenMP-based parallel normal distribution transformation algorithm; using the motion information to obtain second pose estimation through integral operation; inputting the first pose estimation and the second pose estimation into a Kalman filter for loose coupling fusion, and outputting the fused equipment pose; and outputting the final pose as a positioning result of underground equipment for vehicle navigation and pose control. The method effectively improves the positioning precision of underground equipment.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of mine positioning and navigation, and particularly relates to an underground multi-source fusion positioning method and system. BACKGROUND

[0002] In recent years, mine automatic driving technology has developed rapidly, and accurate positioning and navigation of underground environment has become one of the key technologies. Since underground mines, tunnels and other scenes belong to typical GNSS signal-free environments, laser radar (LiDAR) positioning technology has become the main solution in this field. The current mainstream LiDAR positioning technology is usually based on the method of laser point cloud matching, that is, by registering the real-time collected environment point cloud data with the pre-constructed high-precision three-dimensional map to estimate the accurate position and attitude information of the underground equipment.

[0003] Among many point cloud matching algorithms, ICP (Iterative Closest Point) and NDT (Normal Distributions Transform) algorithms are widely used. The basic idea of ICP algorithm is to minimize the distance between corresponding points of two frames of point clouds through iteration to obtain the best rigid transformation. ICP algorithm performs well in the case of rich feature points and accurate initial attitude estimation. However, in actual application, especially when dealing with high-density point clouds or large-scale point clouds, ICP algorithm needs to perform a large number of nearest neighbor search and point-to-point distance calculation, which has high computational complexity, resulting in increased iteration times, slower convergence speed, and difficulty in meeting the real-time positioning requirements.

[0004] NDT algorithm converts target point cloud data into multiple local Gaussian distribution models and matches and registers source point cloud data in a probabilistic model framework, which can obtain more stable matching results in scenes with sparse features or more environmental occlusions. However, the traditional NDT algorithm still has obvious deficiencies in actual application: the fitting process of Gaussian distribution and the construction of grid structure itself have large computational overhead, the matching process is time-consuming, and the parallel computing capability of modern computers is not fully utilized, so the overall running efficiency is still greatly restricted.

[0005] In the closed and complex environment of underground mines, tunnels and other scenes, due to the complex spatial structure, the point cloud data is easily occluded, the loop is frequent and there are many interference sources, and the deficiencies of these existing technologies are particularly obvious. The real-time performance and stability of the positioning algorithm in the underground scene are put forward with higher requirements, and the current point cloud matching technology needs to significantly improve the computing efficiency and robustness while maintaining the positioning accuracy to meet the urgent needs of high-frequency and high-precision real-time positioning for underground equipment automatic driving and remote operation. SUMMARY

[0006] The present application aims to solve the problems in the prior art and provide a downhole multi-source fusion positioning method and system, which effectively improve the positioning accuracy of downhole equipment.

[0007] The technical scheme adopted by the present application is a downhole multi-source fusion positioning method, comprising the following steps: Initialization based on the known initial pose of the downhole equipment when starting; Obtain the environmental point cloud data scanned by the vehicle-mounted laser radar of the downhole equipment and the motion information collected by the inertial measurement unit; Register the environmental point cloud data with the pre-constructed high-precision three-dimensional map, and obtain the first pose estimate using the OpenMP-based parallel normal distribution transformation algorithm; Obtain the second pose estimate by integral operation using the motion information; Input the first pose estimate and the second pose estimate into the Kalman filter for loose coupling fusion, and output the fused device pose; Output the final pose as the positioning result of the downhole equipment for vehicle navigation and attitude control.

[0008] In the above technical scheme, the voxel filtering algorithm is used to downsample the environmental point cloud data scanned and collected by the vehicle-mounted laser radar for first pose estimation, so as to filter out outliers and sensor interference in the downhole environment, and improve the signal-to-noise ratio and feature fidelity of the point cloud data.

[0009] In the above technical scheme, the second pose estimate obtained by the inertial measurement unit through inertial navigation solution is used as the prior pose for point cloud matching, so as to reduce the search range of the OpenMP-based parallel normal distribution transformation algorithm matching.

[0010] In the above technical scheme, in the fusion process based on Kalman filtering, the first pose obtained by laser radar point cloud matching is used as the observation value to correct the second pose obtained by the inertial measurement unit, and the Kalman filtering suppresses the cumulative error of the inertial measurement unit through iterative prediction and update.

[0011] In the above technical scheme, when preprocessing the environmental point cloud data, the dust and water mist interference in the downhole mine environment are filtered and denoised to enhance the quality of the point cloud data.

[0012] In the technical solution, the process of constructing the high-precision three-dimensional map in advance comprises: collecting dense three-dimensional point clouds in the underground roadway by using a mobile surveying device, and performing global optimization by a simultaneous localization and mapping algorithm to obtain a reference point cloud map of the underground environment; dividing the reference point cloud map into three-dimensional voxel grids according to a preset voxel resolution, counting the mean vector and the covariance matrix of the internal point cloud of each voxel, and generating a corresponding normal distribution voxel unit; and storing the voxel model containing the mean vector and the covariance matrix in a binary form in a map database as the high-precision three-dimensional map.

[0013] The application provides an underground multi-source fusion positioning system, comprising: a laser radar unit configured to scan an underground environment and acquire three-dimensional point cloud data; an inertial measurement unit configured to collect acceleration and angular velocity data of the device; a high-precision map storage unit configured to store an underground three-dimensional map containing a voxelized normal distribution model, wherein the model records a mean vector and a covariance matrix for each voxel; a point cloud processing module communicatively connected to the laser radar unit and configured to receive and perform voxel filtering and denoising processing on the point cloud data; a map matching module communicatively connected to the point cloud processing module and the high-precision map storage unit and configured to perform a parallel normal distribution transform algorithm based on the point cloud data and the voxel model to obtain a first pose estimate; an inertial navigation module communicatively connected to the inertial measurement unit and configured to perform integral operation on the collected motion information to obtain a second pose estimate; a fusion positioning module communicatively connected to the map matching module and the inertial navigation module, configured to input the first pose estimate and the second pose estimate into a Kalman filter for loosely coupled fusion, and output a fused device pose; a positioning result output unit configured to output the fused device pose for underground vehicle navigation and attitude control.

[0014] In the technical solution, in the fusion process based on the Kalman filter, the first pose obtained by matching the laser radar point cloud is used as an observation value to correct the second pose obtained by the inertial measurement unit, and the Kalman filter suppresses the cumulative error of the inertial measurement unit through iterative prediction and update.

[0015] The application provides a non-transitory computer-readable storage medium storing an executable program, wherein the program is executed by a processor to sequentially execute the method steps of the technical solution.

[0016] The application provides a mine transport vehicle configured to be used in an underground mine environment without GNSS signals, wherein the vehicle is provided with: Lidar, for real-time scanning of surrounding tunnels and obtaining point cloud data; Inertial measurement unit, for real-time acquisition of vehicle motion information; Vehicle-mounted controller, the controller is pre-installed with a program module for executing the method of the above technical solution; The vehicle-mounted controller calls the above program module to sequentially execute point cloud preprocessing, map matching based on parallel normal distribution transformation algorithm, inertial navigation solution and Kalman filter fusion positioning, and uses the obtained high-precision pose for vehicle navigation and attitude control.

[0017] The advantages of the present application are: through the fusion of LiDAR point cloud and IMU motion information, the advantages of the two sensors are fully complementary - LiDAR provides high-precision environment observation, IMU provides high-frequency motion prediction, after coupling and fusion in Kalman filter, the positioning accuracy (typical error reduced to 0.05-0.15m) and stability can be significantly improved, and reliable pose can be continuously obtained in the underground environment without GNSS signal.

[0018] Further, the present application introduces voxel filtering downsampling, effectively removes underground dust and sensor noise, while reducing the amount of point cloud data, thereby reducing the computational overhead of subsequent NDT registration, speeding up the registration speed (single frame time can be reduced to 15-35ms), and improving the real-time performance of the overall positioning system.

[0019] Further, the present application uses the pose prediction of IMU inertial navigation solution as the prior of NDT point cloud matching, which can greatly reduce the search space, reduce the number of matching iterations, improve the convergence speed and robustness of the algorithm, and reduce the risk of matching failure caused by environmental occlusion or sparse texture.

[0020] Further, the present application uses Kalman filter iteration correction to use LiDAR absolute pose observation to suppress IMU cumulative drift, taking into account the characteristics of high-frequency prediction and high-precision measurement, effectively suppressing error growth, so that the positioning result remains stable and reliable in long-time continuous operation.

[0021] Further, the present application increases special filter denoising in the preprocessing stage for the unique dust and humid environment of underground mines, further improves the clarity and reliability of point cloud features, makes the subsequent registration and fusion process more robust, and avoids mismatching or positioning jump.

[0022] Further, the present application constructs a voxelized NDT model offline, pre-stores the mean vector and covariance matrix, significantly shortens the online loading and query time, reduces the runtime calculation pressure; at the same time, ensures that the NDT algorithm can be directly called under multi-thread parallel, improves the response speed and real-time performance of the whole system.

[0023] Further, the present application integrates each functional module of the algorithm into the positioning system hardware platform: LiDAR, IMU, map storage and processing unit work together to form an integrated device, which can be directly deployed on mine vehicles and other underground equipment, simplifying system construction and maintenance, improving the feasibility and reliability of engineering application.

[0024] Further, the present application further limits the correction strategy of Kalman fusion at the system level, so that the observation fusion and error suppression mechanism are specifically guaranteed by hardware implementation, enhancing the adaptive correction ability of the system to IMU drift and ensuring stable positioning in severe dynamic motion or complex terrain.

[0025] Further, the present application protects the algorithm implementation process in the form of a non-transitory storage medium, covering the software product form, facilitating the release, upgrade and remote update of the method in the form of program modules, and improving the legal protection strength of the patent for software implementation to prevent simple code deployment.

[0026] Further, the present application specifies mine transport vehicles to integrate the positioning scheme, directly embeds the algorithm into the vehicle controller, deeply couples with vehicle navigation and attitude control, realizes a "software and hardware integrated" solution, enhances the completeness and practicality of the application, and directly meets the field requirements of underground autonomous driving or assisted driving. BRIEF DESCRIPTION OF DRAWINGS

[0027] Figure 1 is a method flowchart of the present application; Figure 2 is a high-precision map matching flowchart of a specific embodiment; Figure 3 is a high-precision map matching interface schematic diagram of a specific embodiment; wherein the red color represents a high-precision point cloud map, and the white color is the registration point cloud after the current radar point cloud and map matching; Figure 4 is an IMU pose prediction flowchart of a specific embodiment; Figure 5 is a loose coupling flowchart based on Kalman of a specific embodiment. DETAILED DESCRIPTION

[0028] The present application will be further described in detail below in combination with the drawings and specific embodiments, so as to facilitate a clear understanding of the present application, but they do not constitute a limitation on the present application.

[0029] Embodiment 1 As shown in the figure, the present application provides an underground multi-source fusion positioning method, comprising the following steps: Figure 1 Initialization based on the known initial pose of the underground equipment at startup; ​Acquire environment point cloud data of vehicle-mounted lidar scanning of downhole equipment and motion information collected by an inertial measurement unit; Register the environment point cloud data with a pre-constructed high-precision three-dimensional map, and obtain a first pose estimate using an OpenMP-based parallel normal distribution transform algorithm; Obtain a second pose estimate by integral operation using the motion information; Input the first pose estimate and the second pose estimate into a Kalman filter for loosely coupled fusion, and output the fused device pose; Output the final pose as the positioning result of the downhole equipment for vehicle navigation and attitude control.

[0030] The embodiment provides a downhole multi-source fusion positioning method based on Lidar and IMU. Before real-time positioning starts, first, off-line construction of a high-precision map is performed: a laser radar installed on a downhole vehicle is used to scan and measure the mine environment, three-dimensional point cloud data of the global environment are collected, and a voxel modeling process is performed on the point cloud using an NDT (Normal Distributions Transform) method.

[0031] Specifically, the environment space is divided into a plurality of voxel grid units, the distribution characteristics (such as mean and covariance) of the point cloud data falling into the same voxel are calculated, a Gaussian distribution model of the corresponding voxel is generated, and thus a high-precision three-dimensional map containing detailed features such as downhole roadway structure, equipment contour and landmarks is constructed.

[0032] Preferably, in the map construction process, the parallel computing capability of the ndt_omp algorithm can be used to improve the efficiency and accuracy of point cloud registration, and the global optimization step (such as loop correction) can be combined to improve the consistency of the map. The constructed high-precision map is stored in the storage medium of the system for use in subsequent real-time positioning process.

[0033] After obtaining the environment high-precision map, the downhole multi-source fusion positioning method mainly includes the following steps: 1. Initialize the pose input: when the system starts, initialize the positioning system by the known initial position and attitude. That is, set the initial pose (including position coordinates and heading attitude) of the vehicle relative to the coordinate system of the high-precision map as the starting reference of the positioning algorithm. For example, the initial pose can be obtained by manual measurement, downhole measurement mark or previous positioning result. Initialization provides a reference coordinate frame for subsequent calculation, ensuring that the subsequent sensor data fusion has a reliable reference starting point. At the same time, the calibration parameters of the sensor (such as the relative installation position and attitude of the laser radar and the IMU coordinate system) are also read in the initialization process to ensure that the multi-sensor data is fused and calculated in the same coordinate system.

[0034] 2. Lidar point cloud data collection and preprocessing: The lidar sensor continuously scans the surrounding environment of the underground, and obtains a large amount of three-dimensional point cloud data near the current position of the vehicle in real time, as shown in FIG. 2. The lidar preferably adopts a 3D lidar with multi-beam 360° rotary scanning, such as a 16-line or 32-line lidar, and the typical scanning frequency is about 10 Hz, and the ranging radius can reach dozens of meters (for example, 100 meters), so as to fully cover the space of the tunnel. For each frame of laser point cloud data collected in real time, first, preprocessing filtering is performed to remove noise points and invalid points, so as to improve the accuracy and reliability of the data. Figure 2

[0035] In this embodiment, the voxel filtering (Voxel Grid filtering) method is used to downsample the point cloud: the point cloud is divided into a fixed-size voxel grid according to space, and all points in each voxel are replaced by a representative point. Through voxel filtering, the key feature points of the environment can be effectively retained, and the data amount can be reduced, and the subsequent calculation overhead can be reduced.

[0036] For example, the voxel filtering size can be set to about 0.5 meters (adjusted according to the complexity of the environment and the required accuracy), so that isolated and scattered noise points (such as outliers caused by underground dust or water mist) can be smoothed out, and the number of point cloud points can be greatly reduced to improve the matching operation speed.

[0037] 3. Point cloud matching based on high-precision map: The filtered current frame of lidar point cloud data is matched and registered with the pre-constructed high-precision underground environment map (see the flow shown in FIG. 3). The high-precision map contains detailed information of the boundaries of the underground tunnel, the outlines of the equipment and facilities, and the typical landmark features, and has been stored in the system through the offline process described above. Figure 2

[0038] The matching process adopts a high-performance ndt_omp point cloud matching algorithm: that is, the point cloud of each voxel unit in the high-precision map is represented as a Gaussian distribution model, and the OpenMP parallel optimization is used to position and match the current frame of filtered point cloud.

[0039] Specifically, the estimated pose at the last moment of the vehicle or the IMU predicted pose is taken as the initial alignment, and the pose transformation of the current point cloud relative to the map is continuously iteratively optimized in the map coordinate system, so that the current point cloud and the voxel distribution at the corresponding position in the map are best matched. The optimal pose transformation of the current lidar point cloud relative to the map coordinate system is calculated through the ndt_omp algorithm, and then the accurate position and attitude estimation value of the vehicle in the map is obtained. The ndt_omp algorithm greatly improves the matching efficiency by using multi-thread parallel calculation, and can meet the real-time requirements of underground positioning while ensuring the accuracy. Figure 3 ​​The matching effect of point cloud in the real vehicle environment under the mine is shown, wherein the red point cloud is the high-precision map, and the white point cloud is the point cloud scanned by the current laser radar after registration. It can be seen that the two can be well overlapped, proving accurate matching.

[0040] 4. IMU inertial data integration and pose prediction: IMU sensors measure kinematic data such as three-axis acceleration and three-axis angular velocity of the vehicle in real time, as shown in FIG. 4. The preferred IMU output frequency is high, for example, 100 Hz or even higher, to obtain fine-grained motion information. Since there are errors such as zero drift and random noise in IMU measurement, the error model parameters are usually obtained through calibration before use, and are compensated in the fusion algorithm. Figure 4

[0041] In the positioning system, the integral operation of IMU data is calculated by using the classical strapdown INS (strapdown inertial navigation system): the angular velocity is integrated to update the vehicle attitude (for example, the current attitude angle is calculated by recursively calculating the quaternion or direction cosine matrix), and the acceleration data is transformed to the global coordinate system through the current attitude, and the acceleration is integrated after compensating for the gravity effect to update the velocity and position. By combining the integral process with the initial pose obtained in the previous step, the IMU data can calculate the motion increment of the vehicle relative to the initial position in real time, thereby obtaining a high-frequency pose prediction result. In simple terms, the IMU provides the motion change amount of the vehicle within a short time between two laser scans, and the continuously integrated output pose is used as the predicted value of the vehicle motion at the current time. The predicted pose can be used as the prior initial value for laser point cloud matching, thereby narrowing the search range of the matching algorithm, improving the matching efficiency and stability.

[0042] 5. Loosely coupled fusion based on Kalman filter: the discrete low-frequency pose measurement obtained by the above laser radar point cloud matching is fused with the continuous high-frequency pose prediction obtained by the IMU inertial integration, and a Kalman filter algorithm with a loosely coupled architecture is used to realize the fusion estimation of multi-source information, as shown in FIG. 5. Figure 5

[0043] The fusion process includes two stages of prediction and update: In the prediction stage, the pose calculated by the IMU integration is used as the state prediction value, and the state covariance is also predicted; In the update stage, when the new Lidar matching pose measurement arrives, it is regarded as an observation of the true pose of the vehicle, the difference (innovation) between the observation and the prediction is calculated, and the Kalman gain is updated using the sensor error model (such as the IMU noise drift model and the radar matching error), and then the prior state estimation is corrected.

[0044] ​​Through repeated prediction-update cycles, the Kalman filter continuously corrects the errors accumulated by the IMU due to drift, and takes full advantage of the high-precision but low-frequency measurements of the laser radar, to achieve the complementary advantages of the data of the two sensors. In the loosely coupled scheme, the state quantity of the Kalman filter can include the current position, speed, attitude of the vehicle, and the bias error of the IMU sensor, etc.; the measurement quantity is the vehicle position and attitude calculated by laser matching. After fusion calculation, the optimal vehicle pose estimation with the same frequency as the IMU and the optimized error correction can be output. Compared with a single sensor, this fusion method significantly improves the accuracy and stability of the positioning result: the predicted pose provided by the IMU ensures the smoothness and continuity of short-time motion estimation, while the laser radar matching result corrects the drift error accumulated by the inertial navigation over time, ensuring that the positioning does not diverge in the long term.

[0045] 6. Positioning result output: finally, the vehicle attitude and position obtained through the above sensor fusion filtering are output as the positioning result. The positioning result is usually given in the form of three-dimensional coordinates and heading angle of the vehicle in the high-precision map coordinate system, and the output frequency can be consistent with the IMU update frequency (for example, 100 Hz), thereby providing real-time high-precision position information support for the automatic driving system, remote control platform or monitoring display terminal of the underground equipment. In this embodiment, the positioning result can be sent to the vehicle control system through the communication interface of the vehicle-mounted computer for navigation and obstacle avoidance decision, or recorded in the data storage device for subsequent analysis.

[0046] The above steps 1 to 6 are executed in sequence and continuously, forming a real-time positioning closed loop: the system performs point cloud matching and fusion update once every time the laser radar scan arrives, while the IMU integration and state prediction are continuously running at a higher frequency, thereby realizing continuous, high-frequency and accurate positioning of the vehicle during driving.

[0047] It should be noted that in actual engineering applications, the above process can be extended and optimized as needed. For example, when the underground environment changes greatly, the data can be re-collected and the high-precision map can be updated offline, and the updated map data can be loaded into the system to maintain the accuracy of the positioning reference; for example, to improve the robustness of the system, a fault detection and relocation module can be added in engineering - when the laser radar matching confidence is low or the positioning result appears abnormal jump, start the backup positioning algorithm (for example, use the last few frames of point cloud for local re-matching or calculate the short-time position by odometer) to recalibrate the pose.

[0048] In addition, in the map building stage, the original point cloud can be filtered and denoised and globally optimized (such as loop closure optimization) as preprocessing to obtain a higher quality map for positioning. The above additional modules are only enabled under certain working conditions to enhance the stability of the system in long-term operation and complex dynamic environment. Overall, the method provided in the embodiment covers the complete technical process from initialization to data acquisition and then to fusion output, and a person skilled in the art can directly develop a multi-source fusion positioning software algorithm for a mine underground vehicle based on the method.

[0049] Embodiment 2 The application provides an underground multi-source fusion positioning system, comprising: a laser radar unit for scanning the underground environment and acquiring three-dimensional point cloud data; an inertial measurement unit for collecting acceleration and angular velocity data of the device; a high-precision map storage unit for storing an underground three-dimensional map comprising a voxelized normal distribution model, the model recording a mean vector and a covariance matrix for each voxel record; a point cloud processing module in communication connection with the laser radar unit, receiving and performing voxel filtering and denoising processing on the point cloud data; a map matching module in communication connection with the point cloud processing module and the high-precision map storage unit, performing a parallel normal distribution transform algorithm based on the point cloud data and the voxel model to obtain a first pose estimate; an inertial navigation module in communication connection with the inertial measurement unit, performing integral operation on the collected motion information to obtain a second pose estimate; a fusion positioning module in communication connection with the map matching module and the inertial navigation module, inputting the first pose estimate and the second pose estimate into a Kalman filter for loosely coupled fusion, and outputting a fused device pose; a positioning result output unit for outputting the fused device pose for underground vehicle navigation and attitude control.

[0050] The embodiment provides an underground multi-source fusion positioning device, which can be regarded as a functional implementation unit division of the method flow of embodiment 1. The device can be realized in the form of combination of software and hardware, comprising the following functional modules: a laser radar unit for acquiring three-dimensional point cloud data of the underground environment. Preferably, a 360° rotating laser radar sensor suitable for mine environment (such as a multi-line laser radar) is used, which is fixedly installed on the underground vehicle to ensure that the field of view covers the space in front of and around the vehicle. The laser radar can withstand the underground dust and humid environment, has high ranging accuracy and reliability.

[0051] For example, in one implementation, a 16-line laser radar is selected, with a horizontal field of view of 360 degrees, a vertical field of view covering a sufficient range to scan the profile of the roadway, a scanning frequency of about 10 Hz, and a single scanning radius of up to 100 meters. The laser radar transmits the collected point cloud data to the vehicle-mounted computing device through a high-speed communication interface (such as Ethernet or CAN bus) for processing.

[0052] Inertial Measurement Unit (IMU): used to measure the acceleration and angular velocity of the vehicle motion. The IMU is also mounted on the vehicle, preferably fixed at a solid position of the vehicle chassis or body (e.g. near the center of gravity of the vehicle) to reduce the influence of vibration and ensure that its measurement axes are aligned with the vehicle coordinate system. The IMU can output three-axis acceleration and three-axis angular velocity data at a high frequency, for example, a typical output frequency of 100 Hz or higher. In order to obtain stable inertial measurement in harsh environments, a high-precision MEMS IMU module or fiber-optic gyroscope IMU can be selected. The inherent error types of the IMU include gyroscope zero bias drift, accelerometer zero bias and noise, etc. In actual deployment, the error parameters should be obtained through factory calibration or field calibration, and these errors should be compensated and estimated in the fusion algorithm. The IMU continuously sends data to the vehicle-mounted computing device through a communication interface (such as CAN, serial port, etc.). In the system, the time reference of the IMU is synchronized with the laser radar or aligned through time stamping to ensure that the vehicle motion state corresponding to the same time is obtained when the data of the two sensors are fused.

[0053] High-precision map storage unit: used to store and provide high-precision map data of the underground environment. This module loads the offline constructed three-dimensional point cloud map or NDT grid map at the start of the device, and internally stores the feature parameters (mean, covariance, etc.) of each voxel grid. The map storage module provides the map data for the subsequent matching module for query. Preferably, the map data is aligned with the positioning coordinate system, and the necessary conversion parameters are stored in this module, so that other modules can easily associate sensor data with the map coordinate system. The map storage module can be realized by non-volatile memory in the device, such as flash memory or solid state disk, and the data therein can be updated and upgraded by the user according to the mine environment.

[0054] Initialization module: used for initial pose setting when the positioning device is powered on or reset. This module reads the initial position and attitude parameters input externally, and sets the position state inside the device as the initial reference pose of the vehicle in the map coordinate system. For example, before the mine car is ready to start autonomous driving, its initial absolute coordinates and orientation in the tunnel coordinate system are obtained through manual measurement or pre-setting and written into the system by the initialization module. At the same time, the initialization module calls the pre-stored sensor installation calibration parameters to incorporate the conversion relationship between the sensor coordinate system and the map coordinate system into the initial conditions. After initialization, other modules of the device will take this initial state as the reference to start subsequent positioning calculations. This module ensures that the positioning process starts from a known and reliable starting point, and provides an initial attitude reference for IMU integration operations.

[0055] Point cloud processing module: used for processing real-time point cloud data streams from the laser radar sensor. This module is connected to the interface of the laser radar and receives the three-dimensional point cloud generated by each frame of scanning. The module first performs denoising and downsampling on the point cloud data, for example, using a voxel filtering algorithm to downsample the point cloud according to a predetermined grid size to filter out isolated noise points and reduce data volume. In the filtering process, statistical filtering methods can be used to remove abnormal points that deviate significantly from the environment structure, improving the signal-to-noise ratio of the point cloud. The processed and simplified point cloud data retains the main geometric features of the environment and is output to the map matching module for further registration calculation. This module can be implemented in software by Point Cloud Library, etc., or can use FPGA / DSP hardware acceleration to meet real-time requirements. After processing by this module, the point cloud data entering the matching calculation is both accurate and efficient.

[0056] Map matching module: used for matching the current frame of filtered point cloud from the point cloud processing module with the high-precision map provided by the map storage module. This module implements the function of the ndt_omp point cloud registration algorithm: based on the stored map voxel Gaussian distribution model, a set of pose transformation parameters is found to make the current point cloud best match the map. The module uses the prior value of the current vehicle attitude (for example, the result from the IMU prediction module) as the initial estimate, and solves the pose correction amount of the vehicle relative to the map through iterative optimization. In each iteration, the likelihood value of each point in the current point cloud falling into the map voxel is calculated, and the pose estimate is adjusted according to the gradient descent or Newton method until convergence to obtain the optimal match. The introduction of parallel technologies such as OpenMP enables this module to fully utilize multi-core processors to improve calculation speed and meet the real-time needs of underground positioning. The map matching module outputs the calculated pose of the vehicle in the map coordinate system (including position coordinates and orientation angle) along with its covariance and other uncertainty information to the fusion module as observation input.

[0057] Inertial navigation module: used to receive high-speed data of IMU sensors and perform inertial calculation, output short-period prediction pose of vehicle motion. This module continuously reads accelerometer and gyroscope data from the IMU interface and performs integral operation according to the inertial navigation algorithm: real-time update of the attitude matrix (or quaternion), velocity and position of the vehicle. In order to reduce the influence of noise, the module can use sliding average or digital filtering to smooth the acceleration value during integration, and compensate and correct the gyroscope zero offset. Because there is cumulative error in IMU measurement, the module will also periodically apply the correction value output by the Kalman fusion module to its own speed and position to limit drift divergence. The IMU data processing and prediction module runs at a frequency higher than the frame rate of the laser radar, for example, updating the pose prediction result once every 0.01 seconds. This prediction result, as a priori estimate of the system state, is used as the initial value for the map matching module, and is input into the fusion module along with its covariance prediction for state update.

[0058] Fusion positioning module: used to fuse the measurement results of the map matching module and the inertial calculation results of the IMU prediction module, output the final optimal positioning solution. This module uses a Kalman filter (such as Extended Kalman Filter EKF or Unscented Kalman Filter UKF) to realize loose coupling fusion: the vehicle's pose and speed are taken as the system state, and the pose obtained by laser radar matching is taken as the observation, and the state is predicted and updated in the time axis.

[0059] Specifically, the fusion positioning module performs prediction at each IMU period, updates the current state estimate with the high-frequency pose increment provided by the IMU module, and advances the state error covariance; when the new pose observation of laser radar matching arrives, perform a measurement update to correct the previous state prediction with the map matching observation. During filtering, the module uses the pre-established sensor error model matrix (including IMU noise variance, map matching error variance, etc.) to calculate the Kalman gain, so as to correct in the direction of higher confidence data. The fusion filtering module estimates and compensates the error items such as IMU zero offset, so even if there is drift in the long-term operation of the IMU, it can be corrected by periodic laser observation. The module finally outputs the fused vehicle state, including high-precision current attitude, position and corresponding confidence evaluation. The fusion module can be implemented in software form, for example, using existing filter library to configure the corresponding state equation and observation equation; for high real-time requirement scenarios, the filter operation can also be implemented on a special hardware.

[0060] The positioning result output unit is used to output the vehicle positioning information obtained by the fusion filtering module to the user or other systems. The module obtains the updated vehicle position coordinates and attitude angle data from the fusion module, and arranges and publishes them in the required format. The output form can be various, such as real-time display of the vehicle's position and orientation on the map, and the driving trajectory on the display screen interface; or the data is packaged and sent to the vehicle automatic driving system through the communication bus for calling. In the unmanned driving application, the output module sends the positioning result to the navigation control unit, so that the vehicle can execute path following and steering control according to its accurate position. At the same time, the module can record the positioning data to the local storage for offline analysis or operation log. If there is an abnormal sensor data or positioning failure, the output module can also generate an alarm signal to notify the upper system or operator. Through the positioning result output module, the positioning information of the whole device can be utilized, so as to complete the function closed loop of the positioning device supporting the autonomous driving and remote monitoring of the mine car.

[0061] The above functional modules work together to achieve accurate positioning of the underground equipment according to the process described in Embodiment 1. Each module of the device can be realized in the form of software and hardware combination (for example, each module is composed of a software function executed by a processor, and the modules share data through memory), or can be divided into independent circuit units on hardware. For example, the laser radar point cloud processing and map matching module can be realized by software threads of the same computing platform, and the IMU solving and fusion filtering module can also be integrated in the computing platform for running; or when high parallel computing is required, FPGA chips can be used to realize point cloud matching acceleration, and microprocessors can be used to realize Kalman fusion, and the two cooperate to form a three-dimensional device architecture. These specific implementation methods will not affect the functional essence of the device of the present application.

[0062] It is worth mentioning that, in order to ensure the consistency of data between modules, the device sets a time synchronization mechanism and a unified coordinate system in the system design: all sensor data is stamped with a unified time stamp by the time synchronization unit before entering the processing flow, and each module reads data based on the unified time axis when processing; at the same time, the calibration parameters provided by the initialization module are used to make the laser radar point cloud, IMU solving pose and map coordinate system maintain consistent reference frame. Through the above measures, the data output by each module can be seamlessly connected, ensuring the accuracy and effectiveness of the fusion calculation.

[0063] In summary, the positioning device described in the embodiment covers all functional units from sensing information acquisition, data preprocessing, to multi-source information fusion and result output. Based on existing hardware platforms and development tools, those skilled in the art can program and realize the functional modules, and integrate them into devices such as underground mine cars to form a complete real-time positioning device. After deployment, the device can independently run on unmanned mine vehicles to achieve continuous and high-precision perception of the vehicle's position and attitude, providing key positioning support for mine automatic driving and remote control.

[0064] Embodiment 3 The application provides a non-transitory computer-readable storage medium storing an executable program, which, when executed by a processor, sequentially executes the method steps in the technical solution.

[0065] Embodiment 4 The application provides a mine transport vehicle used in a mine environment without GNSS signals, which is provided with: a laser radar for real-time scanning of surrounding tunnels and acquisition of point cloud data; an inertial measurement unit for real-time acquisition of vehicle motion information; a vehicle-mounted controller, which is pre-stored with a program module for executing the method in the technical solution; The vehicle-mounted controller sequentially executes point cloud preprocessing, map matching based on a parallel normal distribution transform algorithm, inertial navigation calculation, and Kalman filter fusion positioning by calling the program module, and uses the obtained high-precision position and attitude for vehicle navigation and attitude control.

[0066] Those skilled in the art will understand that the embodiments of the application can be provided as a method, a system, or a computer program product. Therefore, the application can take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware aspects. Moreover, the application can take the form of a computer program product implemented on one or more computer usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer usable program code.

[0067] The application is described with reference to flowcharts and / or block diagrams according to the embodiments of the application. It should be understood that each flow and / or block in the flowcharts and / or block diagrams, and the combination of the flows and / or blocks in the flowcharts and / or block diagrams can be implemented by computer program instructions. These computer program instructions can be provided to the processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device produce a device that implements the functions described in the flowcharts and / or block diagrams.Figure 1 one or more blocks and / or steps in a flowchart. Figure 1 one or more blocks and / or steps in a flowchart.

[0068] These computer program instructions can also be stored in a computer readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer readable memory produce an article of manufacture including instructions which implement the flowchart Figure 1 one or more blocks and / or steps in a flowchart. Figure 1 one or more blocks and / or steps in a flowchart.

[0069] These computer program instructions can also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the flowchart Figure 1 one or more blocks and / or steps in a flowchart. Figure 1 one or more blocks and / or steps in a flowchart.

[0070] The embodiments of the present application described above are merely intended to illustrate the present application, but not to limit the present application. The above specific embodiments are merely illustrative, but not restrictive, and those skilled in the art can make many modifications without departing from the spirit and scope of the present application, which are protected by the claims.

[0071] The contents not described in detail in the specification belong to the prior art known to those skilled in the art.

Claims

1. A downhole multi-source fusion positioning method, characterized in that, Includes the following steps: Initialization is performed at startup based on the known initial pose of the downhole equipment. Acquire environmental point cloud data from the vehicle-mounted lidar scanning of the downhole equipment and motion information collected by the inertial measurement unit; Based on the environmental point cloud data, registration is performed with a pre-built high-precision 3D map, and the first pose estimate is obtained using a parallel normal distribution transformation algorithm based on OpenMP. The second pose estimate is obtained by integral calculation using the motion information. The first pose estimate and the second pose estimate are input into a Kalman filter for loose coupling fusion, and the fused device pose is output. The final pose is output as the positioning result of the downhole equipment for vehicle navigation and attitude control.

2. The method according to claim 1, characterized in that: A voxel filtering algorithm is used to downsample the environmental point cloud data collected by the vehicle-mounted lidar for first pose estimation, in order to filter out outliers and sensor interference in the downhole environment and improve the signal-to-noise ratio and feature fidelity of the point cloud data.

3. The method according to claim 1, characterized in that: The second pose estimate obtained by the inertial measurement unit through inertial navigation calculation is used as the prior pose for point cloud matching, so as to narrow the search range of the OpenMP-based parallel normal distribution transformation algorithm.

4. The method according to claim 1, characterized in that: In the Kalman filter-based fusion process, the first pose obtained by matching the lidar point cloud is used as the observation value to correct the second pose calculated by the inertial measurement unit. The Kalman filter suppresses the cumulative error of the inertial measurement unit through iterative prediction and updating.

5. The method according to claim 2, characterized in that: During the preprocessing of environmental point cloud data, filtering and noise reduction are performed to address dust and water mist interference in the underground mining environment, thereby enhancing the quality of the point cloud data.

6. The method according to claim 1, characterized in that: The process of pre-constructing a high-precision 3D map includes: collecting dense 3D point clouds in underground tunnels using a mobile surveying device, and performing global optimization through synchronous positioning and map building algorithms to obtain a baseline point cloud map of the underground environment; dividing the baseline point cloud map into 3D voxel grids according to a preset voxel resolution, calculating the mean vector and covariance matrix of the point cloud within each voxel to generate a corresponding normally distributed voxel unit; and storing the voxel model containing the mean vector and covariance matrix in binary form in a map database as a high-precision 3D map.

7. A downhole multi-source fusion positioning system, characterized in that, include: The lidar unit is used to scan the downhole environment and acquire three-dimensional point cloud data. An inertial measurement unit (IMU) is used to collect acceleration and angular velocity data from the device. A high-precision map storage unit is used to store a downhole 3D map containing a voxelized normal distribution model, wherein the model records the mean vector and covariance matrix for each voxel; The point cloud processing module is communicatively connected to the lidar unit, and receives and performs voxel filtering and noise reduction processing on the point cloud data. The map matching module is communicatively connected to the point cloud processing module and the high-precision map storage unit. Based on the point cloud data and the voxel model, it performs a parallel normal distribution transformation algorithm to obtain the first pose estimate. An inertial navigation module, which is communicatively connected to the inertial measurement unit, performs integral calculations on the collected motion information to obtain a second pose estimate; The fusion positioning module is communicatively connected to the map matching module and the inertial navigation module. It inputs the first pose estimate and the second pose estimate into a Kalman filter for loose coupling fusion and outputs the fused device pose. The positioning result output unit is used to output the fused equipment pose for downhole vehicle navigation and attitude control.

8. A downhole multi-source fusion positioning system according to claim 7, characterized in that: In the fusion positioning module based on Kalman filtering, the first pose obtained by matching the lidar point cloud is used as the observation value to correct the second pose calculated by the inertial measurement unit. The Kalman filter suppresses the cumulative error of the inertial measurement unit through iterative prediction and updating.

9. A non-transitory computer-readable storage medium storing an executable program, characterized in that: After the program is executed by the processor, the method steps described in any one of claims 1-6 are executed sequentially.

10. A mining transport vehicle, characterized in that, For use in underground mining environments without GNSS signals, the vehicle is equipped with: LiDAR is used to scan the surrounding alleyways in real time and acquire point cloud data; An inertial measurement unit is used to collect vehicle motion information in real time. An on-board controller, wherein the controller is pre-installed with a program module that executes the method according to any one of claims 1-6; The vehicle controller, by calling the aforementioned program modules, sequentially executes point cloud preprocessing, map matching based on the parallel normal distribution transformation algorithm, inertial navigation calculation, and Kalman filter fusion positioning, and uses the obtained high-precision pose for vehicle navigation and attitude control.

Images