Mining equipment positioning system and method based on radar, ultra wide band and inertial navigation

Abstract

The invention discloses a mining equipment positioning system and method based on radar, ultra wide band and inertial navigation, and the system is characterized in that a multi-source sensor unit, a data synchronization and collection module and a preprocessing module are connected in sequence, and a state fusion module is connected with the preprocessing module, a UWB ranging error compensation module, a radar motion estimation module and a pose output module; the upper computer is connected with the pose output module; the method comprises the steps of collecting multi-source data; timestamp alignment and format conversion processing are carried out, and feature and speed constraint information extraction is carried out on the point cloud data; fitting correction is carried out on the ranging data by adopting a weighted least square method, and position information is obtained through an error weighted average algorithm; feature matching is carried out, and in combination with a KISS-ICP point cloud registration method, a speed estimation result is obtained; carrying out data fusion, and obtaining a pose estimation result by adopting an extended Kalman filtering algorithm; and outputting equipment position and attitude information. According to the invention, high-precision real-time estimation of the attitude and position information of the mining equipment can be realized.

Description

Technical Field

[0001] The present invention belongs to the field of intelligent control technology, and in particular relates to a mining equipment positioning system and method based on radar, ultra-wideband and inertial navigation. Background Art

[0002] With the continuous advancement of intelligent coal mine construction, the automation and intelligence levels of mining equipment are constantly improving, placing higher demands on the accuracy, safety, and real-time performance of underground operations. As one of the key technologies for achieving autonomous and precise equipment operation, high-precision positioning and navigation systems are playing an increasingly important role in coal mine production. Because coal mine operating environments are typically characterized by closed spaces, insufficient lighting, complex electromagnetic environments, and high levels of dust and vibration, traditional positioning methods that rely on global navigation satellite systems or vision are difficult to adapt to and cannot provide stable, continuous, and high-precision position information.

[0003] Inertial navigation systems (INS) are irreplaceable in underground operating environments due to their complete autonomy and independence from external signals. INS continuously outputs the device's position, velocity, and attitude information, offering a fast response time. However, they also suffer from significant limitations: errors accumulate rapidly over time, particularly when affected by initial alignment errors or sensor drift, significantly reducing the system's overall navigation accuracy. Therefore, relying solely on INS for positioning and navigation fails to meet the high-precision operational demands of underground equipment.

[0004] In recent years, research on the application of ultra-wideband ranging technology and 4D millimeter-wave radar in underground positioning has continued to deepen, offering new possibilities for improving positioning accuracy. UWB, with its strong anti-interference capabilities and high ranging accuracy, demonstrates excellent adaptability in short-range positioning and multi-point deployment underground. 4D millimeter-wave radar not only provides target relative velocity but also outputs real-time spatial structural characteristics of the target, including four-dimensional information such as distance, velocity, angle, and reflection intensity. It possesses the ability to operate stably in harsh environments and can assist equipment in achieving precise navigation and obstacle avoidance. However, both UWB and millimeter-wave radar have inherent limitations: UWB suffers from large ranging errors in scenarios with multipath and severe occlusion, and the positioning granularity of millimeter-wave radar is limited by the sparsity of static point clouds, making it incapable of providing continuous, accurate position tracking over long periods of time. Therefore, using either sensor alone is difficult to achieve high-precision, robust positioning of underground mining equipment.

[0005] To this end, the current trend in underground positioning and navigation technology is the integration of multiple sensor information, particularly the continuous performance of INS, the high-precision ranging capabilities of UWB, and the dynamic environmental perception capabilities of 4D millimeter-wave radar. By optimizing and integrating multi-source information using advanced data fusion algorithms, various error interferences can be effectively suppressed, enhancing the system's robustness and adaptability, and further improving the autonomous positioning capabilities of mining equipment.

[0006] Therefore, there is an urgent need to provide a mining equipment positioning system and method based on the fusion of 4D millimeter-wave radar, ultra-wideband and inertial navigation system to achieve high-precision, continuous and robust positioning of mining equipment in complex coal mine environments, so as to meet the higher requirements of modern intelligent mining operations for safety and automation. Summary of the Invention

[0007] In response to the problems of the above-mentioned prior art, the present invention provides a mining equipment positioning system and method based on radar, ultra-wideband, and inertial navigation. The system has a simple structure and low manufacturing cost. It fully integrates the structural perception and relative motion estimation capabilities of 4D millimeter-wave radar, the high-precision ranging capabilities of UWB, and the short-term and high-frequency attitude solution advantages of the inertial navigation system, and can efficiently and accurately obtain the position and attitude information of mining equipment during mining operations. The method is simple and efficient to implement, with a clear fusion algorithm structure and high positioning accuracy. It can achieve high-precision real-time estimation of the attitude and position information of mining equipment, significantly improving the autonomous positioning capability of mining equipment in harsh conditions such as no GNSS and low line of sight, and effectively improving the stability and reliability of the positioning system under variable working conditions.

[0008] To achieve the above-mentioned object, the present invention provides a mining equipment positioning system based on radar, ultra-wideband and inertial navigation, comprising a multi-source sensor unit, a data synchronization and acquisition module, a preprocessing module, a UWB ranging error compensation module, a radar motion estimation module, a state fusion module, a posture output module and a host computer;

[0009] The multi-source sensor unit includes a 4D millimeter-wave radar, an ultra-wideband positioning module, and an inertial navigation system. The 4D millimeter-wave radar and the ultra-wideband positioning module are installed on the body of the mining equipment, and the inertial navigation system is installed inside the mining equipment. The multi-source sensor unit is used to collect multi-source monitoring data in real time during the operation of the mining equipment.

[0010] The data synchronization and acquisition module is connected to the multi-source sensor unit and is used to receive point cloud data from the 4D millimeter wave radar, ranging data from the ultra-wideband positioning module, and acceleration and angular velocity data from the inertial navigation system;

[0011] The pre-processing module is connected to the data synchronization and acquisition module, and is used to perform time sequence alignment and format conversion processing on various types of data, and extract features and speed constraint information from the radar point cloud;

[0012] The UWB ranging error compensation module is connected to the preprocessing module and is used to perform fitting correction on the ranging data using a weighted least squares algorithm;

[0013] The radar motion estimation module is connected to the preprocessing module and is used to estimate the Doppler velocity information of the mining equipment through feature matching between point cloud data frames and the KISS-CIP point cloud registration method;

[0014] The state fusion module is respectively connected to the UWB ranging error compensation module, the preprocessing module and the radar motion estimation module, and is used to fuse data using the extended Kalman filter method;

[0015] The posture output module is connected to the state fusion module and the host computer respectively, and is used to output the real-time updated position information and posture information of the mining equipment and provide it to the host computer for real-time display;

[0016] The host computer is connected to the mining equipment and is used to display the position information and posture information of the mining equipment in real time through the display module, and to control the mining equipment in real time according to the position information and posture information of the mining equipment.

[0017] Preferably, the ultra-wideband positioning module integrates four fixed base stations and two mobile tags, symmetrically distributed around the mining equipment. This dual-tag data fusion approach can offset single-point measurement deviations and improve anti-interference capabilities.

[0018] In the present invention, 4D millimeter wave radar, ultra-wideband positioning module and inertial navigation system are deployed on the mining equipment at the same time, which can facilitate the synchronous collection of original monitoring data from the three types of sensors. By setting the data synchronization and acquisition module, it is convenient to synchronously receive the original monitoring data from the three types of sensors. By setting the preprocessing module, it is not only convenient to timestamp align the original monitoring data from the three types of sensors, but also to eliminate the time deviation caused by hardware delay or sampling rate difference, ensuring that the running state of the dynamic target is accurately portrayed at the same time, which can effectively avoid the situation of trajectory distortion, but also can reduce the spatial dislocation caused by the difference in coordinate system through format conversion processing, which is conducive to improving the geometric consistency of environmental expression. In addition, by extracting feature information from the radar point cloud, the ability to detect the edge points of low-reflectivity objects can be improved, which significantly improves the monitoring accuracy. Furthermore, by extracting speed constraint information, it is easy to identify the running trend of dynamic objects, and provide time-consistent motion parameters for trajectory prediction. The UWB ranging error compensation module facilitates non-line-of-sight error detection and compensation for ranging data, significantly improving ranging accuracy and enabling more precise vehicle position information. The radar motion estimation module facilitates the use of the KISS-CIP algorithm to significantly improve point cloud registration speed and reduce hardware resource consumption, thereby meeting the real-time requirements of high-speed mining equipment motion scenarios and efficiently and accurately obtaining vehicle velocity estimates. The state fusion module fully integrates multi-source data. Through efficient point cloud processing and multi-source data synergy, it effectively addresses the motion perception bottleneck of mining equipment in various complex working conditions and ensures reliable pose reference information. The pose output module facilitates the transmission of real-time updated position and pose information to a host computer for display, enabling the host computer to control the mining equipment's movements in real time based on the updated position and pose information. The system proposed in the present invention has the advantages of compact structure, high fusion efficiency and strong adaptability. It can effectively solve the problems existing in the existing mining equipment positioning methods, such as large positioning drift and susceptibility to environmental interference. It is particularly suitable for complex working scenarios such as underground mines and tunnel excavation where GNSS is unavailable.

[0019] The system has a simple structure and low manufacturing cost. It can fully integrate the structural perception and relative motion estimation capabilities of 4D millimeter-wave radar, the high-precision ranging capabilities of UWB, and the short-time and high-frequency attitude solution advantages of the inertial navigation system to build a fusion positioning framework for the collaboration of heterogeneous sensors. It can efficiently and accurately obtain the position and attitude information of mining equipment during mining operations.

[0020] The present invention also provides a method for positioning mining equipment based on radar, ultra-wideband and inertial navigation, which uses a mining equipment positioning system based on radar, ultra-wideband and inertial navigation, including the following steps:

[0021] Step 1: Install 4D millimeter wave radar, ultra-wideband positioning module and inertial navigation system on the mining equipment;

[0022] Step 2: During the operation of the mining equipment, the 4D millimeter-wave radar, ultra-wideband positioning module, and inertial navigation system are used to synchronously collect raw point cloud data, raw ranging data, raw acceleration, and angular velocity data in real time. The data synchronization and acquisition module is used to synchronously receive the real-time output data of the 4D millimeter-wave radar, ultra-wideband positioning module, and inertial navigation system, respectively, to obtain raw point cloud data, raw ranging data, raw acceleration, and angular velocity data;

[0023] Step 3: Use the preprocessing module to align the timestamps and convert the format of the raw data, and perform feature extraction and speed constraint information extraction on the point cloud data. The specific process is as follows:

[0024] S31: Align the timestamps of the raw data collected from the 4D millimeter-wave radar, ultra-wideband positioning module, and inertial navigation system, and use the nearest neighbor method to achieve data time domain synchronization;

[0025] S32: Convert the original point cloud data into a vehicle coordinate system, complete the coordinate system transformation based on the sensor external parameters, use the random sample consistency algorithm to fit the ground and plane structures in the point cloud data, and remove outliers that do not meet the model constraints;

[0026] S33: Use a feature extraction algorithm based on the joint analysis of local curvature changes and density gradients of point clouds to identify weak corners and edge points in the boundary area of ​​the structure, and construct a descriptor set based on geometric quantities for each feature point to form a feature matching candidate set;

[0027] S34: Perform a sliding window matching process between adjacent frame point clouds to establish a set of point pairs with consistent temporal order based on the feature descriptor matching results and the Euclidean distance change and normal angle change constraints between point pairs;

[0028] Step 4: Use the UWB ranging error compensation module to fit and correct the ranging data using the weighted least squares method to obtain compensated dual-tag ranging data, and obtain the carrier position information through the error weighted average algorithm;

[0029] Step 5: Use the radar motion estimation module to perform feature matching on the point cloud data frames, and combine it with the KISS-ICP point cloud registration method to obtain the carrier speed estimation result of the mining equipment;

[0030] Step 6: Use the state fusion module to fuse the output data of the preprocessing module, the output data of the UWB ranging error compensation module, and the output data of the radar motion estimation module, and use the extended Kalman filter algorithm to obtain a robust pose estimation result;

[0031] Step 7: Output the real-time updated mining equipment position information and posture information through the posture output module, and transmit it to the host computer for visual display.

[0032] Furthermore, in order to efficiently obtain high-precision, low-latency perception data, in the timestamp alignment process in S31, the inertial navigation system is used as the main clock reference, and the low-frequency data of the ultra-wideband positioning module and 4D millimeter-wave radar are aligned to the sampling time point of the inertial navigation system through linear interpolation. The specific process is as follows:

[0033] For any inertial navigation system timestamp t k ∈[t u1 ,t u2 ], according to formula (1), the interpolation observation value z corresponding to the ultra-wideband positioning module is obtained u (t k ); For point cloud frame data, the nearest frame retention strategy is adopted to obtain the interpolated observation value z of the point cloud data according to formula (2) r (t k );

[0034]

[0035] z r (t k )=z r (t r ) (2);

[0036] Where, t k ∈[t r ,t r+1 ), t r The starting time of the current frame.

[0037] Through the technical solution of high-frequency benchmark combined with lightweight interpolation, sub-millisecond synchronization of multi-source data can be achieved and the accuracy of perception data can be ensured.

[0038] Furthermore, in order to ensure the spatial alignment of multi-source monitoring data and provide highly reliable data input for multi-sensor fusion positioning, the specific process of removing outliers that do not meet the model constraints in S32 is as follows:

[0039] First, the original point cloud data is converted from the radar coordinate system to the vehicle coordinate system; first, use formula (3) to express the homogeneous coordinate P of any point in the original point cloud in the radar coordinate system:L , and then use formula (4) to express the coordinate P of any point in the original point cloud in the vehicle coordinate system B ;

[0040] P L =[x L ,y L ,z L ,1] T (3);

[0041] P B =T BL ·P L (4);

[0042] Where, T BL The extrinsic parameter transformation matrix from the radar coordinate system to the vehicle coordinate system;

[0043] Secondly, the RANSAC algorithm is used to fit the ground and plane structures on the transformed vehicle coordinate system point cloud data. First, the mathematical model of the fitting plane is established according to formula (5), and then the RANSAC algorithm is used to randomly select the minimum sample set for plane fitting in multiple iterations, and the algebraic distance from the remaining points to the fitting plane is calculated. If the point p i If the conditions in formula (6) are met, it is determined to be an outlier that does not meet the model constraints and is removed;

[0044] n T ·p+d=0 (5);

[0045] Where n∈R 3 is the unit normal vector of the plane, p∈R 3 is the spatial point coordinate in the point cloud, d∈R is the plane intercept distance;

[0046] |n T ·p i +d|>∈ (6);

[0047] Where, ∈ is the set distance threshold.

[0048] Furthermore, in order to effectively improve the accuracy and robustness of point cloud feature extraction, in S33, the process of forming the feature matching candidate set is as follows:

[0049] S33-1: For each point p in the point cloud frame i , extract its neighborhood point set N i , construct the local covariance matrix C according to formula (7) i , and the local covariance matrix C i Perform eigenvalue decomposition and obtain eigenvalues ​​λ0≤λ1≤λ2;

[0050]

[0051] Where, For p i The neighborhood centroid of the local covariance matrix is ​​decomposed into eigenvalues ​​to obtain eigenvalues ​​λ0≤λ1≤λ2;

[0052] S33-2: Define the local curvature K of the point according to formula (8) i , when K i When the set threshold is exceeded, the decision point p i is a corner point or edge point;

[0053]

[0054] S33-3: Introducing density gradient based on kernel density estimation As shown in formula (9), the curvature index and density change trend are jointly analyzed to achieve accurate identification of weak structural feature points;

[0055]

[0056] S33-4: For the selected feature points, a descriptor set of geometric quantities based on the local normal, the distribution of distances between points, and the direction of the local principal curvature is constructed to form a set of candidate feature points.

[0057] Furthermore, in order to significantly improve the matching accuracy and robustness, in S34, the process of establishing a set of point pairs with consistent time sequence is as follows:

[0058] S34-1: For each pair of adjacent frames P t , P t+Δt The candidate feature points in are matched by Euclidean distance based on their descriptor vectors D(·) to obtain preliminary matching pairs, as shown in formula (10);

[0059]

[0060] S34-2: Introduce geometric consistency constraints, set the matching point to (p i ,q j ), when the conditions in formula (11) are met, it is judged as a valid matching pair;

[0061]

[0062] Where, are the normal vectors of the feature points, δ d , δ θ are the set distance and angle thresholds respectively.

[0063] Furthermore, in order to assign different weights to different positioning results and improve positioning accuracy by fusing multi-source positioning information, in step 4, the error weighted average algorithm is shown in formula (12);

[0064]

[0065] Where, X i represents the positioning result of the i-th tag at the current moment; w i is the weight of the i-th label, and σ i It is the empirical standard deviation preset based on the actual installation location and environmental characteristics.

[0066] Furthermore, in order to significantly improve the accuracy and robustness of motion estimation, in step 5, the carrier velocity estimation process is as follows:

[0067] S51: Use the KISS-ICP point cloud registration method to estimate the pose transformation matrix T of the current frame relative to the previous frame k , where T k By the rotation matrix R k and the translation vector t k Composition; minimize the sum of squares of the Euclidean distances between corresponding points in adjacent frames and solve the optimal R k With t k ;

[0068] S52: Extract the radial velocity observation value v of each scattering point r,i and its corresponding unit observation direction vector n i , and establish a velocity observation model according to formula (13). At the same time, the observation values ​​of N scattering points collected are combined into a matrix form according to formula (14);

[0069]

[0070] Where, ε i is the Gaussian noise term;

[0071] v r =Nv+ε (14);

[0072] Where, v r =[v r,1 ,...,v r,N ] T , ε=[ε1,...,ε N ] T ;

[0073] S53: Estimate the carrier velocity vector provided by the radar using the least squares method according to formula (15)

[0074]

[0075] Furthermore, in order to achieve high-precision pose estimation, in step 6, the EKF method is used to fuse the output data of the preprocessing module, the output data of the UWB ranging error compensation module, and the output data of the radar motion estimation module to obtain the pose estimation result. The specific process is as follows:

[0076] S61: Establish the state vector x according to formula (16) i ;

[0077] x i =[p i v i q i b a b g ] T (16);

[0078] Where p i Indicates position, v i Indicates speed, q i is the attitude representation in quaternion form, b a , b g are the bias errors of the accelerometer and gyroscope, respectively, and are modeled using first-order Markov models;

[0079] S62: constructing a nonlinear state transition model of the system using the measurement value of the inertial navigation system according to formula (17);

[0080] x i|i-1 =f(x i-1 ,u i-1 )+w i-1 (17);

[0081] Where u i-1 is the measured value of acceleration and angular velocity data, w i-1 is the process noise;

[0082] S63: Fusion of ultra-wideband positioning module observations and 4D millimeter-wave radar observation information, constructing a joint observation vector z according to formula (18) i

[0083] z i =[z uwb ,z radar ] T (18);

[0084] Where z uwb =H i,uwb x i,uwb +εuwb , z radar =H i,radar x i,radar +ε radar , H i,uwb 、H i,radar are the observation matrices of the ultra-wideband positioning module and 4D millimeter-wave radar at the i-th moment, ε uwb , ε radar They are the noise errors of the ultra-wideband positioning module and the 4D millimeter-wave radar respectively;

[0085] S64: Predict the state vector x at time i according to formula (19) and formula (20) respectively i|i-1 and the state covariance matrix P i|i-1 ;

[0086]

[0087] Where, is the state transfer Jacobian matrix, Q i-1 is the process noise covariance matrix;

[0088] S65: Calculate the observation residual y at the i-th moment according to formula (21) and formula (22) respectively i and the Kalman gain K i

[0089]

[0090] Where R i is the observation noise covariance matrix;

[0091] S66: Calculate the optimized state vector at the i-th moment according to formula (23)

[0092]

[0093] S67: If the i-th moment is the last moment, complete the fusion estimation of all states; otherwise, update the state covariance matrix, return to step S64 and continue to enter the prediction and update step of the next moment, and update the state covariance matrix according to formula (24);

[0094]

[0095] Where I is the identity matrix.

[0096] The present invention provides a mining equipment positioning method based on radar, ultra-wideband, and inertial navigation. First, a pre-processing module is used to align the timestamps of the raw monitoring data from the three types of sensors, thereby eliminating the time deviation caused by hardware delay or sampling rate differences, ensuring that the operating state of the dynamic target is accurately portrayed at the same time, effectively avoiding trajectory distortion, and reducing the spatial dislocation caused by coordinate system differences through format conversion processing, which is conducive to improving the geometric consistency of environmental expression. In addition, by extracting feature information from the radar point cloud, the edge points of low-reflectivity objects can be detected, significantly improving monitoring accuracy. Furthermore, by extracting speed constraint information, the operating trend of dynamic objects can be easily identified, and temporal consistency motion parameters can be provided for trajectory prediction. Then, a UWB ranging error compensation module is used to detect and compensate for non-line-of-sight errors in the ranging data, greatly improving the accuracy of ranging and obtaining more accurate carrier position information. Furthermore, when using the radar motion estimation module to match data features between point cloud data frames, the KISS-ICP point cloud registration method is combined. Because the KISS-ICP algorithm uses binary coding to optimize nearest neighbor search and multi-threaded parallel computing, it significantly improves the speed of point cloud registration, meeting the real-time requirements of high-speed mining equipment motion scenarios and more efficiently and accurately obtaining carrier velocity estimates. Then, during the state fusion module's fusion of multi-source data, an extended Kalman filter algorithm is used to obtain pose estimation results, effectively removing noise information from the monitoring data and thus obtaining high-precision multi-dimensional pose information. This invention fully utilizes the complementary advantages of 4D millimeter-wave radar, ultra-wideband positioning module, and inertial navigation system in spatial positioning and dynamic estimation, constructing a state estimation and optimization mechanism based on collaborative modeling of multi-source feature information. By introducing the KISS-ICP algorithm, rapid registration and velocity calculation of 4D millimeter-wave radar point clouds are achieved. Combined with UWB ranging error compensation, the stability of position estimation is significantly improved. The inertial navigation system provides a continuous attitude reference, enabling tightly coupled estimation of attitude and velocity in complex environments. During the fusion process, the extended Kalman filter algorithm is used to dynamically fuse multi-source heterogeneous data, construct a unified state space, and achieve high-precision estimation of multi-dimensional pose information.

[0097] The implementation process of this method is simple and efficient, the fusion algorithm structure is clear, and the positioning accuracy is high. It comprehensively considers the performance complementarity of different sensors in complex environments, designs the spatiotemporal alignment and feature constraint mechanism of multi-source observation information, and introduces a state estimation strategy based on extended Kalman filtering, which realizes high-precision real-time estimation of the posture and position information of mining equipment, significantly improves the autonomous positioning capability of mining equipment under harsh conditions such as no GNSS and weak line of sight, effectively improves the stability and reliability of the positioning system under variable working conditions, and solves the problem of inaccurate positioning of mining equipment in complex underground environments due to the susceptibility of a single sensor to interference and error accumulation, reliably ensuring the efficient and safe progress of mining operations. BRIEF DESCRIPTION OF THE DRAWINGS

[0098] Figure 1 It is a principle block diagram of the system part of the present invention;

[0099] Figure 2 is a flow chart of the method portion of the present invention;

[0100] Figure 3 This is a schematic diagram of the assembly of the multi-source sensor unit on the mining equipment in the present invention. DETAILED DESCRIPTION

[0101] The present invention will be further described below with reference to the accompanying drawings.

[0102] like Figures 1 to 3 As shown, the present invention provides a mining equipment positioning system based on radar, ultra-wideband and inertial navigation, including a multi-source sensor unit, a data synchronization and acquisition module, a preprocessing module, a UWB ranging error compensation module, a radar motion estimation module, a state fusion module, a posture output module and a host computer;

[0103] The multi-source sensor unit includes a 4D millimeter-wave radar, an ultra-wideband (UWB) positioning module, and an inertial navigation system (INS). The 4D millimeter-wave radar and the ultra-wideband positioning module are installed on the body of the mining equipment, and the inertial navigation system is installed inside the mining equipment. The multi-source sensor unit is used to collect multi-source monitoring data in real time during the operation of the mining equipment.

[0104] The data synchronization and acquisition module is connected to the multi-source sensor unit and is used to receive point cloud data from the 4D millimeter wave radar, ranging data from the ultra-wideband positioning module, and acceleration and angular velocity data from the inertial navigation system;

[0105] The pre-processing module is connected to the data synchronization and acquisition module, and is used to perform time sequence alignment and format conversion processing on various types of data, and extract features and speed constraint information from the radar point cloud;

[0106] The UWB ranging error compensation module is connected to the preprocessing module and is used to perform fitting correction on the ranging data using a weighted least squares algorithm (WLS);

[0107] The radar motion estimation module is connected to the preprocessing module and is used to estimate the Doppler velocity information of the mining equipment through feature matching between point cloud data frames and the KISS-CIP point cloud registration method;

[0108] The state fusion module is respectively connected to the UWB ranging error compensation module, the preprocessing module and the radar motion estimation module, and is used to fuse data using the extended Kalman filter (EKF) method;

[0109] The posture output module is connected to the state fusion module and the host computer respectively, and is used to output the real-time updated mining equipment position information and posture information to the host computer;

[0110] The host computer is connected to the mining equipment and is used to display the position information and posture information of the mining equipment in real time through the display module, and to control the mining equipment in real time according to the position information and posture information of the mining equipment.

[0111] Preferably, the ultra-wideband positioning module integrates four fixed base stations and two mobile tags. The two mobile tags are symmetrically distributed around the mining equipment and migrate with it in real time. This dual-tag data fusion approach can offset deviations in single-point measurements and improve anti-interference capabilities.

[0112] In the present invention, 4D millimeter wave radar, ultra-wideband positioning module and inertial navigation system are deployed on the mining equipment at the same time, which can facilitate the synchronous collection of original monitoring data from the three types of sensors. By setting the data synchronization and acquisition module, it is convenient to synchronously receive the original monitoring data from the three types of sensors. By setting the preprocessing module, it is not only convenient to timestamp align the original monitoring data from the three types of sensors, but also to eliminate the time deviation caused by hardware delay or sampling rate difference, ensuring that the running state of the dynamic target is accurately portrayed at the same time, which can effectively avoid the situation of trajectory distortion, but also can reduce the spatial dislocation caused by the difference in coordinate system through format conversion processing, which is conducive to improving the geometric consistency of environmental expression. In addition, by extracting feature information from the radar point cloud, the ability to detect the edge points of low-reflectivity objects can be improved, which significantly improves the monitoring accuracy. Furthermore, by extracting speed constraint information, it is easy to identify the running trend of dynamic objects, and provide time-consistent motion parameters for trajectory prediction. The UWB ranging error compensation module facilitates non-line-of-sight error detection and compensation for ranging data, significantly improving ranging accuracy and enabling more precise vehicle position information. The radar motion estimation module facilitates the use of the KISS-CIP algorithm to significantly improve point cloud registration speed and reduce hardware resource consumption, thereby meeting the real-time requirements of high-speed mining equipment motion scenarios and efficiently and accurately obtaining vehicle velocity estimates. The state fusion module fully integrates multi-source data. Through efficient point cloud processing and multi-source data synergy, it effectively addresses the motion perception bottleneck of mining equipment in various complex working conditions and ensures reliable pose reference information. The pose output module facilitates the transmission of real-time updated position and pose information to a host computer for display, enabling the host computer to control the mining equipment's movements in real time based on the updated position and pose information. The system proposed in the present invention has the advantages of compact structure, high fusion efficiency and strong adaptability. It can effectively solve the problems existing in the existing mining equipment positioning methods, such as large positioning drift and susceptibility to environmental interference. It is particularly suitable for complex working scenarios such as underground mines and tunnel excavation where GNSS is unavailable.

[0113] The system has a simple structure and low manufacturing cost. It can fully integrate the structural perception and relative motion estimation capabilities of 4D millimeter-wave radar, the high-precision ranging capabilities of UWB, and the short-time and high-frequency attitude solution advantages of the inertial navigation system to build a fusion positioning framework for the collaboration of heterogeneous sensors. It can efficiently and accurately obtain the position and attitude information of mining equipment during mining operations.

[0114] The present invention also provides a method for positioning mining equipment based on radar, ultra-wideband and inertial navigation, which uses a mining equipment positioning system based on radar, ultra-wideband and inertial navigation, including the following steps:

[0115] Step 1: Install a 4D millimeter-wave radar, an ultra-wideband positioning module, and an inertial navigation system on the mining equipment. The 4D millimeter-wave radar and ultra-wideband positioning module are fixedly installed on the outside of the mining equipment, while the inertial navigation system is installed inside the mining equipment.

[0116] Step 2: During the operation of the mining equipment, the 4D millimeter-wave radar, ultra-wideband positioning module, and inertial navigation system are used to synchronously collect raw point cloud data, raw ranging data, raw acceleration, and angular velocity data in real time. The data synchronization and acquisition module is used to synchronously receive the real-time output data of the 4D millimeter-wave radar, ultra-wideband positioning module, and inertial navigation system, respectively, to obtain raw point cloud data, raw ranging data, raw acceleration, and angular velocity data;

[0117] Step 3: Use the preprocessing module to align the timestamps and convert the format of the raw data, and perform feature extraction and speed constraint information extraction on the point cloud data. The specific process is as follows:

[0118] S31: Align the timestamps of the raw data collected from the 4D millimeter-wave radar, ultra-wideband positioning module, and inertial navigation system, and use the nearest neighbor method to achieve data time domain synchronization;

[0119] S32: Convert the original point cloud data into a vehicle coordinate system, complete the coordinate system transformation based on the sensor extrinsic parameters, use the Random Sample Consensus (RANSAC) algorithm to fit the ground and plane structures in the point cloud data, and remove outliers that do not meet the model constraints;

[0120] S33: Use a feature extraction algorithm based on the joint analysis of local curvature changes and density gradients of point clouds to identify weak corners and edge points in the boundary area of ​​the structure, and construct a descriptor set based on geometric quantities for each feature point to form a feature matching candidate set;

[0121] S34: Perform a sliding window matching process between adjacent frame point clouds to establish a set of point pairs with consistent temporal order based on the feature descriptor matching results and the Euclidean distance change and normal angle change constraints between point pairs;

[0122] Step 4: Use the UWB ranging error compensation module to fit and correct the ranging data using the weighted least squares method to obtain the compensated dual-tag (UWB tag 1 and UWB tag 2) ranging data, and obtain the carrier position information through the error weighted average algorithm;

[0123] Step 5: Use the radar motion estimation module to perform feature matching on the point cloud data frames, and combine it with the KISS-ICP point cloud registration method to obtain the carrier speed estimation result of the mining equipment;

[0124] Step 6: Use the state fusion module to fuse the output data of the preprocessing module, the output data of the UWB ranging error compensation module, and the output data of the radar motion estimation module, and use the extended Kalman filter algorithm to obtain a robust pose estimation result to obtain the posture and position information of the mining equipment;

[0125] Step 7: Output the real-time updated mining equipment position information and posture information through the posture output module, and transmit it to the host computer for visual display.

[0126] In order to efficiently obtain high-precision, low-latency perception data, during the timestamp alignment process in S31, the inertial navigation system is used as the main clock reference, and the low-frequency data of the ultra-wideband positioning module and 4D millimeter-wave radar are aligned to the sampling time point of the inertial navigation system through linear interpolation. The specific process is as follows:

[0127] For any inertial navigation system timestamp t k ∈[t u1 ,t u2 ], according to formula (1), the interpolation observation value z corresponding to the ultra-wideband positioning module is obtained u (t k ); For point cloud frame data, the nearest frame retention strategy is adopted to obtain the interpolated observation value z of the point cloud data according to formula (2) r (t k );

[0128]

[0129] z r (t k )=z r (t r ) (2);

[0130] Where, t k ∈[t r ,t r+1 ), t r The starting time of the current frame.

[0131] Through the technical solution of high-frequency benchmark combined with lightweight interpolation, sub-millisecond synchronization of multi-source data can be achieved and the accuracy of perception data can be ensured.

[0132] In order to ensure the spatial alignment of multi-source monitoring data and provide highly reliable data input for multi-sensor fusion positioning, the specific process of removing outliers that do not meet the model constraints in S32 is as follows:

[0133] First, the original point cloud data is converted from the radar coordinate system to the vehicle coordinate system; first, use formula (3) to express the homogeneous coordinate P of any point in the original point cloud in the radar coordinate system: L , and then use formula (4) to express the coordinate P of any point in the original point cloud in the vehicle coordinate system B ;

[0134] P L =[x L ,y L ,z L ,1] T (3);

[0135] P B =T BL ·P L (4);

[0136] Where, T BL The extrinsic transformation matrix from the radar coordinate system to the vehicle coordinate system can be obtained through offline calibration and contains rotation and translation information;

[0137] Secondly, the RANSAC algorithm is used to fit the ground and plane structures on the transformed vehicle coordinate system point cloud data. First, a mathematical model of the fitting plane is established according to formula (5). Then, based on the above model, the RANSAC algorithm is used to randomly select the minimum sample set for plane fitting in multiple iterations, and the algebraic distances of the remaining points to the fitting plane are calculated. If the point p i If the conditions in formula (6) are met, it is determined to be an outlier point that does not meet the model constraints and is removed. All points that meet the conditions will be removed to eliminate the interference of noise and abnormal structures on the system positioning accuracy;

[0138] n T ·p+d=0 (5);

[0139] Where n∈R 3 is the unit normal vector of the plane, p∈R 3 is the spatial point coordinate in the point cloud, d∈R is the plane intercept distance;

[0140] |n T ·p i +d|>∈ (6);

[0141] Where, ∈ is the set distance threshold.

[0142] In order to effectively improve the accuracy and robustness of point cloud feature extraction, in S33, the process of forming the feature matching candidate set is as follows:

[0143] S33-1: For each point p in the point cloud framei , extract its neighborhood point set N i , construct the local covariance matrix C according to formula (7) i , and the local covariance matrix C i Perform eigenvalue decomposition and obtain eigenvalues ​​λ0≤λ1≤λ2;

[0144]

[0145] Where, For p i The neighborhood centroid of the local covariance matrix is ​​decomposed into eigenvalues ​​to obtain eigenvalues ​​λ0≤λ1≤λ2;

[0146] S33-2: Define the local curvature K of the point according to formula (8) i , when K i When the set threshold is exceeded, the decision point p i is a corner point or edge point;

[0147]

[0148] S33-3: To enhance the robustness of boundary point extraction, density gradient based on kernel density estimation is introduced As shown in formula (9), the curvature index and density change trend are jointly analyzed to achieve accurate identification of weak structural feature points;

[0149]

[0150] S33-4: For the selected feature points, a descriptor set of geometric quantities based on the local normal, the distribution of distances between points, and the direction of the local principal curvature is constructed to form a set of candidate feature points.

[0151] By jointly analyzing the curvature index and the density change trend, accurate identification of weak structural feature points can be achieved.

[0152] In order to significantly improve matching accuracy and robustness, in S34, the process of establishing a set of point pairs with consistent timing is as follows:

[0153] S34-1: For each pair of adjacent frames P t , P t+Δt The candidate feature points in are matched by Euclidean distance based on their descriptor vectors D(·) to obtain preliminary matching pairs, as shown in formula (10);

[0154]

[0155] S34-2: Introduce geometric consistency constraints, set the matching point to (p i ,q j), when the conditions in formula (11) are met, it is judged as a valid matching pair;

[0156]

[0157] Where, are the normal vectors of the feature points, δ d , δ θ are the set distance and angle thresholds respectively.

[0158] In order to assign different weights to different positioning results and improve positioning accuracy by fusing multi-source positioning information, in step 4, the error weighted average algorithm is shown in formula (12);

[0159]

[0160] Where, X i represents the positioning result of the i-th tag at the current moment; w i is the weight of the i-th label, and σ i It is the empirical standard deviation preset based on the actual installation location and environmental characteristics.

[0161] In order to significantly improve the accuracy and robustness of motion estimation, in step 5, the carrier velocity estimation process is as follows:

[0162] S51: Use the KISS-ICP point cloud registration method to estimate the pose transformation matrix T of the current frame relative to the previous frame k , where T k By the rotation matrix R k and the translation vector t k Composition; minimize the sum of squares of the Euclidean distances between corresponding points in adjacent frames and solve the optimal R k With t k ;

[0163] S52: Extract the radial velocity observation value v of each scattering point r,i and its corresponding unit observation direction vector n i , and establish a velocity observation model according to formula (13). At the same time, the observation values ​​of N scattering points collected are combined into a matrix form according to formula (14);

[0164]

[0165] Where, ε i is the Gaussian noise term;

[0166] v r =Nv+ε (14);

[0167] Where, vr =[v r,1 ,...,v r,N ] T , ε=[ε1,...,ε N ] T ;

[0168] S53: Estimate the carrier velocity vector provided by the radar using the least squares method according to formula (15)

[0169]

[0170] In order to achieve high-precision pose estimation, in step six, the EKF method is used to fuse the output data of the preprocessing module, the output data of the UWB ranging error compensation module, and the output data of the radar motion estimation module to obtain the pose estimation result. The specific process is as follows:

[0171] S61: Establish the state vector x according to formula (16) i ;

[0172] x i =[p i v i q i b a b g ] T (16);

[0173] Where p i Indicates position, v i Indicates speed, q i is the attitude representation in quaternion form, b a , b g are the bias errors of the accelerometer and gyroscope, respectively, and are modeled using first-order Markov models;

[0174] S62: constructing a nonlinear state transition model of the system using the measurement value of the inertial navigation system according to formula (17);

[0175] x i|i-1 =f(x i-1 ,u i-1 )+w i-1 (17);

[0176] Where u i-1 is the measured value of acceleration and angular velocity data, w i-1 is the process noise;

[0177] S63: Fusion of ultra-wideband positioning module observations and 4D millimeter-wave radar observation information, constructing a joint observation vector z according to formula (18)i

[0178] z i =[z uwb ,z radar ] T (18);

[0179] Where z uwb =H i,uwb x i,uwb +ε uwb , z radar =H i,radar x i,radar +ε radar , H i,uwb 、H i,radar are the observation matrices of the ultra-wideband positioning module and 4D millimeter-wave radar at the i-th moment, ε uwb , ε radar They are the noise errors of the ultra-wideband positioning module and the 4D millimeter-wave radar respectively;

[0180] S64: Predict the state vector x at time i according to formula (19) and formula (20) respectively i|i-1 and the state covariance matrix P i|i-1 ;

[0181]

[0182] Where, is the state transfer Jacobian matrix, Q i-1 is the process noise covariance matrix;

[0183] S65: Calculate the observation residual y at the i-th moment according to formula (21) and formula (22) respectively i and the Kalman gain K i

[0184]

[0185] Where R i is the observation noise covariance matrix;

[0186] S66: Calculate the optimized state vector at the i-th moment according to formula (23)

[0187]

[0188] S67: If the i-th moment is the last moment, complete the fusion estimation of all states; otherwise, update the state covariance matrix, return to step S64 and continue to enter the prediction and update step of the next moment, and update the state covariance matrix according to formula (24);

[0189]

[0190] Where I is the identity matrix.

[0191] The present invention provides a mining equipment positioning method based on radar, ultra-wideband, and inertial navigation. First, a pre-processing module is used to align the timestamps of the raw monitoring data from the three types of sensors, thereby eliminating the time deviation caused by hardware delay or sampling rate differences, ensuring that the operating state of the dynamic target is accurately portrayed at the same time, effectively avoiding trajectory distortion, and reducing the spatial dislocation caused by coordinate system differences through format conversion processing, which is conducive to improving the geometric consistency of environmental expression. In addition, by extracting feature information from the radar point cloud, the edge points of low-reflectivity objects can be detected, significantly improving monitoring accuracy. Furthermore, by extracting speed constraint information, the operating trend of dynamic objects can be easily identified, and temporal consistency motion parameters can be provided for trajectory prediction. Then, a UWB ranging error compensation module is used to detect and compensate for non-line-of-sight errors in the ranging data, greatly improving the accuracy of ranging and obtaining more accurate carrier position information. Furthermore, when using the radar motion estimation module to match data features between point cloud data frames, the KISS-ICP point cloud registration method is combined. Because the KISS-ICP algorithm uses binary coding to optimize nearest neighbor search and multi-threaded parallel computing, it significantly improves the speed of point cloud registration, meeting the real-time requirements of high-speed mining equipment motion scenarios and more efficiently and accurately obtaining carrier velocity estimates. Then, during the state fusion module's fusion of multi-source data, an extended Kalman filter algorithm is used to obtain pose estimation results, effectively removing noise information from the monitoring data and thus obtaining high-precision multi-dimensional pose information. This invention fully utilizes the complementary advantages of 4D millimeter-wave radar, ultra-wideband positioning module, and inertial navigation system in spatial positioning and dynamic estimation, constructing a state estimation and optimization mechanism based on collaborative modeling of multi-source feature information. By introducing the KISS-ICP algorithm, rapid registration and velocity calculation of 4D millimeter-wave radar point clouds are achieved. Combined with UWB ranging error compensation, the stability of position estimation is significantly improved. The inertial navigation system provides a continuous attitude reference, enabling tightly coupled estimation of attitude and velocity in complex environments. During the fusion process, the extended Kalman filter algorithm is used to dynamically fuse multi-source heterogeneous data, construct a unified state space, and achieve high-precision estimation of multi-dimensional pose information.

[0192] The implementation process of this method is simple and efficient, the fusion algorithm structure is clear, and the positioning accuracy is high. It comprehensively considers the performance complementarity of different sensors in complex environments, designs the spatiotemporal alignment and feature constraint mechanism of multi-source observation information, and introduces a state estimation strategy based on extended Kalman filtering, which realizes high-precision real-time estimation of the posture and position information of mining equipment, significantly improves the autonomous positioning capability of mining equipment under harsh conditions such as no GNSS and weak line of sight, effectively improves the stability and reliability of the positioning system under variable working conditions, and solves the problem of inaccurate positioning of mining equipment in complex underground environments due to the susceptibility of a single sensor to interference and error accumulation, reliably ensuring the efficient and safe progress of mining operations.

Claims

1. A mining equipment positioning system based on radar, ultra-wideband and inertial navigation, characterized in that: It includes multi-source sensor unit, data synchronization and acquisition module, pre-processing module, UWB ranging error compensation module, radar motion estimation module, state fusion module, posture output module and host computer; The multi-source sensor unit includes a 4D millimeter-wave radar, an ultra-wideband positioning module, and an inertial navigation system. The 4D millimeter-wave radar and the ultra-wideband positioning module are installed on the body of the mining equipment, and the inertial navigation system is installed inside the mining equipment. The multi-source sensor unit is used to collect multi-source monitoring data in real time during the operation of the mining equipment. The data synchronization and acquisition module is connected to the multi-source sensor unit and is used to receive point cloud data from the 4D millimeter wave radar, ranging data from the ultra-wideband positioning module, and acceleration and angular velocity data from the inertial navigation system; The pre-processing module is connected to the data synchronization and acquisition module, and is used to perform time sequence alignment and format conversion processing on various types of data, and extract features and speed constraint information from the radar point cloud; The UWB ranging error compensation module is connected to the preprocessing module and is used to perform fitting correction on the ranging data using a weighted least squares algorithm; The radar motion estimation module is connected to the preprocessing module and is used to estimate the Doppler velocity information of the mining equipment through feature matching between point cloud data frames and the KISS-CIP point cloud registration method; The state fusion module is respectively connected to the UWB ranging error compensation module, the preprocessing module and the radar motion estimation module, and is used to fuse data using the extended Kalman filter method; The posture output module is connected to the state fusion module and the host computer respectively, and is used to output the real-time updated mining equipment position information and posture information to the host computer; The host computer is connected to the mining equipment and is used to display the position information and posture information of the mining equipment in real time through the display module, and to control the mining equipment in real time according to the position information and posture information of the mining equipment.

2. The mining equipment positioning system based on radar, ultra-wideband and inertial navigation according to claim 1 is characterized in that: The ultra-wideband positioning module integrates four groups of fixed base stations and two groups of mobile tags, and the two groups of mobile tags are symmetrically distributed about the body of the mining equipment.

3. A method for positioning mining equipment based on radar, ultra-wideband and inertial navigation, using a mining equipment positioning system based on radar, ultra-wideband and inertial navigation as claimed in claim 1 or 2, characterized in that: The following steps are involved: Step 1: Install 4D millimeter wave radar, ultra-wideband positioning module and inertial navigation system on the mining equipment; Step 2: During the operation of the mining equipment, the 4D millimeter-wave radar, ultra-wideband positioning module, and inertial navigation system are used to synchronously collect raw point cloud data, raw ranging data, raw acceleration, and angular velocity data in real time. The data synchronization and acquisition module is used to synchronously receive the real-time output data of the 4D millimeter-wave radar, ultra-wideband positioning module, and inertial navigation system, respectively, to obtain raw point cloud data, raw ranging data, raw acceleration, and angular velocity data; Step 3: Use the preprocessing module to align the timestamps and convert the format of the raw data, and perform feature extraction and speed constraint information extraction on the point cloud data. The specific process is as follows: S31: Align the timestamps of the raw data collected from the 4D millimeter-wave radar, ultra-wideband positioning module, and inertial navigation system, and use the nearest neighbor method to achieve data time domain synchronization; S32: Convert the original point cloud data into a vehicle coordinate system, complete the coordinate system transformation based on the sensor external parameters, use the random sample consistency algorithm to fit the ground and plane structures in the point cloud data, and remove outliers that do not meet the model constraints; S33: Use a feature extraction algorithm based on the joint analysis of local curvature changes and density gradients of point clouds to identify weak corners and edge points in the boundary area of ​​the structure, and construct a descriptor set based on geometric quantities for each feature point to form a feature matching candidate set; S34: Perform a sliding window matching process between adjacent frame point clouds to establish a set of point pairs with consistent temporal order based on the feature descriptor matching results and the Euclidean distance change and normal angle change constraints between point pairs; Step 4: Use the UWB ranging error compensation module to fit and correct the ranging data using the weighted least squares method to obtain compensated dual-tag ranging data, and obtain the carrier position information through the error weighted average algorithm; Step 5: Use the radar motion estimation module to perform feature matching on the point cloud data frames, and combine it with the KISS-ICP point cloud registration method to obtain the carrier speed estimation result of the mining equipment; Step 6: Use the state fusion module to fuse the output data of the preprocessing module, the output data of the UWB ranging error compensation module, and the output data of the radar motion estimation module, and use the extended Kalman filter algorithm to obtain a robust pose estimation result; Step 7: Output the real-time updated mining equipment position information and posture information through the posture output module, and transmit it to the host computer for visual display.

4. The method for positioning mining equipment based on radar, ultra-wideband and inertial navigation according to claim 3, characterized in that: During the timestamp alignment process in S31, the inertial navigation system is used as the main clock reference, and the low-frequency data of the ultra-wideband positioning module and 4D millimeter-wave radar are aligned to the sampling time point of the inertial navigation system through linear interpolation. The specific process is as follows: For any inertial navigation system timestamp t k ∈[t u1 ,t u2 ], according to formula (1), the interpolation observation value z corresponding to the ultra-wideband positioning module is obtained u (t k ); For point cloud frame data, the nearest frame retention strategy is adopted to obtain the interpolated observation value z of the point cloud data according to formula (2) r (t k ); z r (t k )=z r (t r ) (2); Where, t k ∈[t r ,t r+1 ), t r The starting time of the current frame.

5. The method for positioning mining equipment based on radar, ultra-wideband and inertial navigation according to claim 4, characterized in that: In S32, the specific process of removing outliers that do not meet the model constraints is as follows: First, the original point cloud data is converted from the radar coordinate system to the vehicle coordinate system; first, use formula (3) to express the homogeneous coordinate P of any point in the original point cloud in the radar coordinate system: L , and then use formula (4) to express the coordinate P of any point in the original point cloud in the vehicle coordinate system B ; P L =[x L ,y L ,z L ,1] T (3); P B =T BL ·P L (4); Where, T BL The extrinsic parameter transformation matrix from the radar coordinate system to the vehicle coordinate system; Secondly, the RANSAC algorithm is used to fit the ground and plane structures on the transformed vehicle coordinate system point cloud data. First, the mathematical model of the fitting plane is established according to formula (5), and then the RANSAC algorithm is used to randomly select the minimum sample set for plane fitting in multiple iterations, and the algebraic distance from the remaining points to the fitting plane is calculated. If the point p i If the conditions in formula (6) are met, it is determined to be an outlier that does not meet the model constraints and is removed; n T ·p+d=0 (5); Where n∈R 3 is the unit normal vector of the plane, p∈R 3 is the spatial point coordinate in the point cloud, d∈R is the plane intercept distance; |n T ·p i +d|>∈ (6); Where, ∈ is the set distance threshold.

6. The method for positioning mining equipment based on radar, ultra-wideband and inertial navigation according to claim 5, characterized in that: In S33, the process of forming the feature matching candidate set is as follows: S33-1: For each point p in the point cloud frame i , extract its neighborhood point set N i , construct the local covariance matrix C according to formula (7) i , and the local covariance matrix C i Perform eigenvalue decomposition and obtain eigenvalues ​​λ0≤λ1≤λ2; Where, For p i The neighborhood centroid of the local covariance matrix is ​​decomposed into eigenvalues ​​to obtain eigenvalues ​​λ0≤λ1≤λ2; S33-2: Define the local curvature K of the point according to formula (8) i , when K i When the set threshold is exceeded, the decision point p i is a corner point or edge point; S33-3: Introducing density gradient ▽ρ based on kernel density estimation i , as shown in formula (9), the curvature index and density change trend are jointly analyzed to achieve accurate identification of weak structural feature points; S33-4: For the selected feature points, a descriptor set of geometric quantities based on the local normal, the distribution of distances between points, and the direction of the local principal curvature is constructed to form a set of candidate feature points.

7. The method for positioning mining equipment based on radar, ultra-wideband and inertial navigation according to claim 6, characterized in that: In S34, the process of establishing a time-consistent point pair set is as follows: S34-1: For each pair of adjacent frames P t , P t+Δt The candidate feature points in are matched by Euclidean distance based on their descriptor vectors D(·) to obtain preliminary matching pairs, as shown in formula (10); S34-2: Introduce geometric consistency constraints, set the matching point to (p i ,q j ), when the conditions in formula (11) are met, it is judged as a valid matching pair; Where, are the normal vectors of the feature points, δ d , δ θ are the set distance and angle thresholds respectively.

8. The method for positioning mining equipment based on radar, ultra-wideband and inertial navigation according to claim 7, characterized in that: In step 4, the error weighted average algorithm is shown in formula (12); Where, X i represents the positioning result of the i-th tag at the current moment; w i is the weight of the i-th label, and σ i It is the empirical standard deviation preset based on the actual installation location and environmental characteristics.

9. The method for positioning mining equipment based on radar, ultra-wideband and inertial navigation according to claim 8, characterized in that: In step 5, the process of carrier velocity estimation is as follows: S51: Use the KISS-ICP point cloud registration method to estimate the pose transformation matrix T of the current frame relative to the previous frame k , where T k By the rotation matrix R k and the translation vector t k Composition; minimize the sum of squared Euclidean distances between corresponding points in adjacent frames and solve the optimal R k With t k ; S52: Extract the radial velocity observation value v of each scattering point r,i and its corresponding unit observation direction vector n i , and establish a velocity observation model according to formula (13). At the same time, the observation values ​​of N scattering points collected are combined into a matrix form according to formula (14); Where, ε i is the Gaussian noise term; v r =Nv+ε (14); where \(v\) r = \([v r,1 ,\cdots,v r,N T , \(\varepsilon = [\varepsilon_1,\cdots,\varepsilon N T ;\)​​ S53: Estimate the carrier velocity vector provided by the radar using the least squares method according to formula (15) 10. The method for positioning mining equipment based on radar, ultra-wideband and inertial navigation according to claim 9, characterized in that: In step 6, the EKF method is used to fuse the output data of the preprocessing module, the output data of the UWB ranging error compensation module, and the output data of the radar motion estimation module to obtain the pose estimation result. The specific process is as follows: S61: Establish the state vector x according to formula (16) i ; x i =[p i v i q i b a b g ] T (16); Where p i Indicates position, v i Indicates speed, q i is the attitude representation in quaternion form, b a , b g are the bias errors of the accelerometer and gyroscope, respectively, and are modeled using first-order Markov models; S 62: According to formula (17), the nonlinear state transition model of the system is constructed using the measurement value of the inertial navigation system; x i|i-1 =f(x i-1 ,u i-1 )+w i-1 (17); Where u i-1 is the measured value of acceleration and angular velocity data, w i-1 is the process noise; S63: Fusion of ultra-wideband positioning module observations and 4D millimeter-wave radar observation information, constructing a joint observation vector z according to formula (18) i With i =[z uwb ,With radar ] T (18); Where z uwb =H i,uwb x i,uwb +ε uwb , z radar =H i,radar x i,radar +ε radar , H i,uwb 、H i,radar are the observation matrices of the ultra-wideband positioning module and 4D millimeter-wave radar at the i-th moment, ε uwb , ε radar They are the noise errors of the ultra-wideband positioning module and the 4D millimeter-wave radar respectively; S 64: Predict the state vector x at time i according to formula (19) and formula (20) respectively i|i-1 and the state covariance matrix P i|i-1 ; Where, is the state transfer Jacobian matrix, Q i-1 is the process noise covariance matrix; S65: Calculate the observation residual y at the i-th moment according to formula (21) and formula (22) respectively i and the Kalman gain K i Where R i is the observation noise covariance matrix; S66: Calculate the optimized state vector at the i-th moment according to formula (23) S67: If the i-th moment is the last moment, complete the fusion estimation of all states; otherwise, update the state covariance matrix, return to step S64 and continue to enter the prediction and update step of the next moment, and update the state covariance matrix according to formula (24); Where I is the identity matrix.

Images