Mine unmanned aerial vehicle UWB-SLAM positioning and path correction method

Abstract

The invention relates to the technical field of coal mine safety monitoring and robot autonomous navigation, and discloses a mine unmanned aerial vehicle UWB-SLAM positioning and path correction method, which comprises the following steps: firstly, collecting airborne inertial measurement unit and ultra wide band radio frequency data, constructing observability indexes based on a radio frequency road sign, and determining the UWB-SLAM positioning and path correction parameters according to the observability indexes; and when it is judged that a geometric degradation area is entered, an expected trajectory of superposition excitation maneuver is actively generated. Then, the expected trajectory is utilized to cooperatively carry out dynamic baseline correction, a distance-Doppler image is generated through an inverse synthetic aperture radar technology, and environmental characteristics are extracted. And finally, constructing a radio frequency-inertial odometer containing distance and Doppler double constraints, correcting the pose of the unmanned aerial vehicle by using an observation residual error, and executing closed-loop control. According to the invention, the environment observability is enhanced through active maneuvering, the problem of positioning drift caused by high dust interference and geometric degradation of a long straight roadway is effectively solved by utilizing deep cooperation of perception and control, and high-precision autonomous positioning in a base-station-free environment is realized.

Description

Technical Field

[0001] This invention relates to the field of coal mine safety monitoring and robot autonomous navigation technology, specifically a UWB-SLAM positioning and path correction method for mine unmanned aerial vehicles (UAVs). Background Technology

[0002] Unmanned aerial vehicles (UAVs) in mines play an irreplaceable role in disaster relief, environmental monitoring, and hazard identification. High-precision, robust autonomous positioning and navigation technology is the core foundation for ensuring their safe operation. Due to the enclosed and unique environment of underground mines, signals from commonly used global navigation satellite systems are completely blocked, preventing UAVs from directly obtaining absolute position information. Therefore, they must rely on their own sensor systems to achieve autonomous positioning.

[0003] Existing downhole positioning technologies are mainly divided into two categories: infrastructure-based positioning and autonomous sensing-based positioning. Infrastructure-based solutions typically involve pre-laying numerous ultra-wideband base stations or Wi-Fi beacons on the tunnel walls and performing triangulation by measuring the distance or angle between the drone and the base stations. However, this method is heavily reliant on infrastructure, resulting in high deployment costs, difficult maintenance, and inability to operate in unknown post-disaster areas or newly excavated tunnels due to the lack of pre-installed base stations, severely limiting the drone's operational range and flexibility.

[0004] To reduce reliance on infrastructure, real-time localization and mapping (SLAM) technologies based on lidar or visual sensors have become a research hotspot. However, these optical sensors face severe challenges in actual mine operations. Firstly, mine tunnels are often filled with high concentrations of coal dust and water mist, and lighting conditions are extremely poor. This leads to severe scattering and attenuation of the laser beam, resulting in blurred or feature-lost visual images, significantly reducing the measurement accuracy of optical sensors or even causing them to fail completely. Secondly, and more significantly, mine tunnels are typically long, structurally simple, straight tubular tunnels lacking rich longitudinal geometric features. In this extreme environment, SLAM algorithms are prone to geometric degradation, the so-called "long corridor effect," which results in extremely low observability of the system in the forward direction, failing to effectively constrain the cumulative drift of the inertial navigation system, ultimately causing localization divergence and navigation failure. Summary of the Invention

[0005] To address the shortcomings of existing technologies, this invention provides a UWB-SLAM positioning and path correction method for mine unmanned aerial vehicles (UAVs), thus solving the problems.

[0006] To achieve the above objectives, the present invention provides the following technical solution: The first aspect of the present invention provides a UWB-SLAM positioning and path correction method for mine unmanned aerial vehicles (UAVs), comprising the following steps:

[0007] S1. State initialization and raw data acquisition.

[0008] Initialize the UAV's pose state and acquire raw waveform data of the UAV's onboard inertial measurement unit and the channel impulse response from the ultra-wideband radio frequency front-end in real time. Use the inertial measurement unit data to recursively deduce the UAV's pose state.

[0009] Wherein, the state vector x of the UAV at time k k Defined as:

[0010]

[0011] In the formula, p k Let v be the position vector. k Let q be the velocity vector. k Let b be a quaternion of attitude. a,k For zero bias of the accelerometer, b g,k This is for zero bias of the gyroscope.

[0012] S2, Observability assessment and proactive path decision-making.

[0013] Based on the currently extracted radio frequency landmarks, an observability index is constructed, and the need for active path correction is determined according to the observability index.

[0014] Specifically, using the geometric distribution information of the N radio frequency landmarks extracted at the current moment, a Fisher information matrix J is constructed:

[0015] JH T R -1 H;

[0016] In the formula, H is the measurement Jacobian matrix, and R is the measurement noise covariance matrix. The minimum eigenvalue or determinant of the Fisher information matrix J is selected as the observability index η of the current system. The calculated observability index η is compared with the preset observability threshold η. th Compare them. If η ≥ η th Maintain the current nominal path p nom (t) is the expected trajectory; if η < η h If the target is found to be in a geometrically degenerate region, a desired trajectory p superimposed with an active excitation maneuver is generated. ref (t):

[0017] p ref (t)=p nom (t)+n·Asin(2πf m t+φ0);

[0018] In the formula, n is a lateral unit vector perpendicular to the forward direction, A is the maneuver amplitude, and f mLet f be the maneuver frequency, and φ0 be the initial phase. The maneuver amplitude A and maneuver frequency f are also mentioned. m It has a positive correlation with the degree of decline in observability indicators.

[0019] S3. ISAR dynamic baseline correction based on desired trajectory coordination.

[0020] The desired trajectory generated in step S2 is used as a priori motion model to perform dynamic baseline correction on the original waveform data of the channel impulse response collected in step S1, so as to eliminate the influence of UAV micro-motion error on synthetic aperture imaging.

[0021] Specifically, within the synthetic aperture time window, the prior position and the desired trajectory p recursively obtained by the inertial measurement unit are calculated. ref The baseline deviation Δd(t) between the two signals is used to construct a phase compensation factor to correct the original UWB echo signal s(t,r), thus obtaining the corrected channel impulse response data s. comp (t,r):

[0022]

[0023] In the formula, f c Here, c is the center frequency of the UWB signal, c is the speed of light, and j is the imaginary unit. By introducing deterministic trajectory constraints with active control, the smoothness of the phase history is improved.

[0024] S4. Generate UWB-ISAR radio frequency images and extract features. For the corrected channel impulse response data s comp (t,r) is used for inverse synthetic aperture radar imaging processing.

[0025] Specifically, for s comp (t,r) performs a Fast Fourier Transform (FFT) in the fast time dimension to obtain the distance dimension information, and performs a FFT in the slow time dimension to obtain the Doppler dimension information, generating a generator containing distance information τ and Doppler information f. d Two-dimensional distance-Doppler image I(r,f) d ).

[0026] The constant false alarm rate (CFAR) detection algorithm is used to traverse I(r,f) d Strong scattering points are extracted as radio frequency (RF) landmarks. For the i-th RF landmark, its measurement vector is denoted as z. RF,i =[R i ,v D,i ] T , where R i To measure distance, v D,i To measure Doppler velocity.

[0027] S5. Radio Frequency-Inertial Odometry (RFIO) State Update. An observation model based on RF landmarks is constructed, observation residuals are calculated, and the UAV pose state is corrected and updated using these residuals. For the i-th RF landmark, its position is denoted as... The observation model is constructed as follows:

[0028] Distance observation model:

[0029]

[0030] Doppler observation model:

[0031]

[0032] Calculate the observation residuals A cost function containing prior residuals and observation residuals is constructed. This cost function is then minimized using an error-state Kalman filter or a sliding window optimization algorithm to obtain the corrected high-precision positioning state.

[0033] S6, Closed-loop control execution.

[0034] Based on high-precision positioning status and expected trajectory p ref (t) generates control commands to drive the UAV's flight, completing the closed loop of positioning and path correction. The active maneuvering flight performed by the UAV will provide the necessary synthetic aperture baseline and Doppler shift for the next ISAR imaging step.

[0035] A second aspect of the present invention provides a UWB-SLAM positioning and path correction system for mining unmanned aerial vehicles (UAVs), comprising:

[0036] The airborne sensing module is configured to acquire in real time data from the UAV's inertial measurement unit and raw waveform data of the channel impulse response from the ultra-wideband radio frequency front end;

[0037] The observability analysis and path decision module is configured to calculate observability indicators based on radio frequency landmarks and generate a desired trajectory superimposed with active excitation maneuvers when the indicators are below a preset threshold.

[0038] The collaborative imaging processing module is configured to use the desired trajectory as prior information to perform dynamic baseline correction on the raw waveform data of the channel impulse response, generate a range-Doppler image and extract radio frequency landmarks;

[0039] The state estimation module is configured to use radio frequency landmarks to build a distance and Doppler observation model, calculate the observation residuals, and correct and update the UAV pose state.

[0040] The flight control module is configured to generate motor control commands based on the corrected UAV attitude state and desired trajectory, and control the UAV to perform straight flight or S-shaped active maneuver flight.

[0041] This invention provides a UWB-SLAM positioning and path correction method for unmanned aerial vehicles (UAVs) used in mining operations. It offers the following advantages:

[0042] 1. This invention combines the inverse synthetic aperture radar imaging principle with a UWB radio frequency front-end, and utilizes the motion synthesis virtual large aperture antenna during the flight of the UAV to transform multipath reflection signals in mine tunnels, which are originally considered as interference, into radio frequency landmarks containing rich environmental geometric information. This allows the system to achieve high-precision positioning without the need to pre-deploy and maintain expensive active UWB base stations in the mine, relying solely on the passive environmental characteristics of the mine itself. This significantly reduces the deployment cost and maintenance complexity of the system, and is particularly suitable for long-distance, unknown mine tunnel detection.

[0043] 2. This invention establishes a deep collaborative mechanism between the desired trajectory and ISAR dynamic baseline correction. The deterministic desired trajectory generated by the path planning layer is used as prior information and fed back to the signal processing layer for phase history compensation of the UWB original waveform. This design uses the known input of the control system to help correct the motion error of the sensing system, effectively solving the problem that traditional ISAR imaging relies heavily on high-precision inertial navigation and is easily affected by IMU drift, resulting in defocusing.

[0044] 3. This invention enables dynamic flight strategy switching by setting an observability threshold, allowing the UAV to adaptively switch between nominal straight-line cruise and high-precision maneuver positioning modes. The system only triggers energy-consuming active S-shaped maneuvers when the environmental geometry is insufficient to support the positioning accuracy requirements, while maintaining efficient straight-line flight in feature-rich areas. Under the premise of ensuring positioning robustness, the system optimizes the flight efficiency and energy utilization of the UAV to the greatest extent, meeting the needs of long-term operation in mines. Attached Figure Description

[0045] Figure 1 This is a perspective view of the present invention;

[0046] Figure 2 This is a schematic diagram of the present invention. Detailed Implementation

[0047] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0048] Example:

[0049] Please see the appendix Figure 1 This invention provides a UWB-SLAM positioning and path correction method for mine unmanned aerial vehicles (UAVs), comprising the following steps:

[0050] S1. Real-time acquisition of inertial measurement unit data and raw waveform data of channel impulse response of UAV from ultra-wideband radio frequency front end, and recursion of UAV attitude state using inertial measurement unit data.

[0051] In this embodiment, step S1 mainly performs state initialization and raw data acquisition operations, providing basic data support for subsequent positioning calculation and path correction. Specifically, this step uses onboard multi-source sensors to perceive the UAV's motion state and the radio frequency characteristics of the environment in real time.

[0052] In this embodiment, the UAV system first performs state initialization before starting operations. Initialization includes determining the UAV's initial position, initial velocity, and initial attitude in the world coordinate system. In the absence of an external positioning reference, the initial position is typically set to the coordinate origin, and the initial attitude is obtained through gravity alignment or magnetometer-assisted alignment by the onboard inertial measurement unit in a stationary state. Simultaneously, the system initializes the covariance matrix required by the state estimator and sets the initial uncertainty range.

[0053] In this embodiment, the inertial measurement unit (IMU) onboard the UAV serves as the core body perception sensor, responsible for acquiring the UAV's angular velocity and specific force data in real time at a high sampling frequency. The angular velocity data reflects the instantaneous rate of rotation of the body around each axis, while the specific force data reflects the non-gravitational acceleration experienced by the body in each axis.

[0054] In this embodiment, the acquired IMU data is used to perform a short-term recursive calculation of the UAV's pose state using a standard inertial navigation mechanical orchestration algorithm. Specifically, the UAV state vector x at time k is... k Defined as containing position vector p k Velocity vector v k , attitude quaternion q k accelerometer zero bias b a,k and gyroscope zero bias b g,k High-dimensional vectors:

[0055]

[0056] The state recursion process involves using the current IMU measurement value, combined with the state estimate from the previous time step, to predict the current state through integration. This process provides high-frequency motion prior information for subsequent radio frequency imaging, especially during the synthetic aperture time required for imaging, where the displacement information provided by the IMU is crucial for motion compensation.

[0057] In this embodiment, the UAV is also equipped with an ultra-wideband radio frequency front-end module for detecting passive features in the mining environment. This module preferably employs a single-transmitter, dual-receiver antenna layout to provide basic angular resolution. During flight, the UWB module periodically transmits extremely short pulse signals into the surrounding space.

[0058] In this embodiment, the UWB receiver does not perform traditional time-of-flight ranging calculations, but instead directly acquires and records the raw waveform data of the channel impulse response (CIR). This CIR data includes echo signals generated by various reflectors encountered by the electromagnetic wave during its propagation in space.

[0059] In this embodiment, the acquired CIR data can be represented as a two-dimensional function s(t,τ) with respect to slow time t and fast time τ. Slow time t corresponds to the pulse transmission moment of the UAV flying along its flight path, reflecting the UAV's motion sequence in space; fast time τ corresponds to the sampling moment after a single pulse is emitted, reflecting the propagation delay distance of the signal from transmission to return to the receiver via the reflector.

[0060] In this embodiment, by synchronously acquiring high-frequency motion data from the IMU and radio frequency waveform data from the UWB, and strictly aligning the two on the time axis, the system can construct a raw dataset containing complete motion-sensing information, thus laying the data foundation for subsequent steps to use synthetic aperture technology to convert multipath reflection signals into environmental geometric features.

[0061] S2. Construct observability indicators based on the currently extracted radio frequency landmarks, and determine whether active path correction is needed based on the observability indicators; if the observability indicators are lower than the preset threshold, generate the desired trajectory superimposed with active excitation maneuvers.

[0062] In this embodiment, step S2 mainly performs observability assessment and active path decision-making operations, aiming to monitor the geometric stability of the positioning system in real time and dynamically adjust the UAV's flight strategy accordingly. This step transforms traditional passive positioning into an active perception process by introducing information theory indicators.

[0063] In this embodiment, the system first needs to quantitatively evaluate the richness of geometric features in the current positioning environment. In the fields of SLAM and state estimation, the solvability of the system state is usually measured by observability metrics. For landmark-based positioning systems, the geometric distribution of landmarks directly determines the strength of the constraint equations.

[0064] In this embodiment, to accurately assess this potential risk, the system constructs a Fisher information matrix using the N radio frequency landmarks successfully tracked within the current field of view. The Fisher information matrix is ​​a symmetric positive definite matrix describing the amount of information about state parameters carried by the observed data. Specifically, based on the measurement Jacobian matrix H and the measurement noise covariance matrix R, a FIM matrix J is constructed:

[0065] J = H T R -1 H;

[0066] Among them, the Jacobian matrix H reflects how a small change in the state variable causes a change in the observed value, and the noise covariance matrix R reflects the uncertainty of the sensor itself. The size of matrix J and the distribution of its eigenvalues ​​directly characterize the accuracy limit of the state estimation.

[0067] In this embodiment, the smallest eigenvalue λ of the Fisher information matrix J is selected. min The determinant det(J) serves as the comprehensive observability index η of the system. This index is a scalar value that intuitively reflects the uncertainty of the current positioning system in the worst-case dimension. When the value of η decreases significantly, it means that the system is in or about to enter the geometric degradation region, and the positioning accuracy faces the risk of decline.

[0068] In this embodiment, the system has a preset empirical observability threshold η. th This threshold is pre-calibrated based on the positioning accuracy requirements and the sensor noise level. The system compares the real-time calculated index η with this threshold as the logical basis for path decision-making.

[0069] In this embodiment, if the comparison result shows η≥η th This indicates that the current environmental features are well-distributed, and the system has sufficient observability to maintain high-precision positioning. Under these circumstances, no additional path intervention is required, and the expected trajectory p... ref (t) Maintain the nominal path p issued by the task planning layer. nom (t). The nominal path is typically a straight or smooth curve trajectory along the centerline of the roadway, designed to perform inspection tasks and ensure flight efficiency.

[0070] In this embodiment, if the comparison result shows η<η thIf the system enters the geometrically degenerate region, it is determined that the system has entered the degenerate region. To prevent positioning drift, the system triggers an active path correction mechanism, generating a desired trajectory p superimposed with active excitation maneuvers. ref (t). The purpose of this trajectory is to artificially create lateral variations in the observation perspective and diversity in Doppler shift.

[0071] In this embodiment, the generated desired trajectory is specifically based on the original nominal path, superimposed with a transverse sinusoidal motion component perpendicular to the direction of travel. Its mathematical expression is as follows:

[0072] p ref (t)=p nom (t)+n·Asin(2πf m t+φ0);

[0073] Where n is the horizontal unit vector, used to specify the direction of the maneuver; A represents the amplitude of the sinusoidal maneuver; f m Represents the frequency of maneuvers.

[0074] In this embodiment, in order to achieve an adaptive response to different degrees of degradation, the maneuver amplitude A and the maneuver frequency f m It is not a fixed value, but rather a value that is positively correlated with the degree of decline in observability. In other words, the more severe the decline in observability, the greater the amplitude and the faster the frequency of the resulting S-shaped trajectory. This dynamic adjustment mechanism ensures that additional energy is consumed only when necessary for maneuvering, thereby achieving an optimal balance between positioning accuracy and flight energy consumption.

[0075] S3. Using the desired trajectory generated in step S2 as a priori motion model, perform dynamic baseline correction on the original channel impulse response waveform data collected in step S1 to obtain the corrected channel impulse response data.

[0076] In this embodiment, step S3 mainly performs ISAR dynamic baseline correction based on desired trajectory coordination. This step is a key collaborative link connecting upper-layer path planning and lower-layer signal processing. Its core idea is to use the deterministic input of the control system to help correct the motion error of the sensing system, thereby improving imaging quality.

[0077] In this embodiment, traditional synthetic aperture radar (SAR) or inverse synthetic aperture radar (ISAR) imaging techniques heavily rely on precise knowledge of the carrier's trajectory. During imaging, any uncompensated minute displacement will cause phase distortion of the echo, resulting in a defocused and blurred image. In conventional solutions, motion compensation data typically comes only from inertial measurement units (IMUs). However, under long-term operation or severe maneuvers, low-cost IMUs inevitably suffer from integral drift, making it difficult to meet the requirements of high-precision coherent imaging.

[0078] In this embodiment, to overcome the above limitations, the present invention innovatively introduces a desired trajectory cooperative mechanism. Since in step S2, the system has already generated a clear desired trajectory p containing active maneuvering features based on observability requirements. ref The desired trajectory (t) will be sent to the flight controller for execution. Therefore, this desired trajectory can be regarded as a strong prior constraint model on the actual motion of the UAV. Compared with IMU integral data that has random drift, the desired trajectory has higher determinism and smoothness in macroscopic shape.

[0079] In this embodiment, before performing ISAR imaging processing, the system first defines a synthetic aperture time window. Within this time window, the system acquires two trajectory data in parallel: one is a priori position sequence recursively obtained from the IMU, and the other is a desired trajectory sequence generated by the control layer. The system calculates the difference between these two trajectories in the imaging baseline direction to obtain the baseline deviation Δd(t). This deviation reflects the inconsistency between the sensor measurement value and the control target value.

[0080] In this embodiment, based on the calculated baseline deviation, the system constructs a compensation factor to correct the phase. According to electromagnetic wave propagation theory, changes in distance cause signal phase rotation. Therefore, the baseline deviation can be mapped to a correction term in the phase domain. Specifically, for each pulse time t, a complex-form phase compensation factor Ψ is constructed. comp (t):

[0081]

[0082] In the formula, f c Here, is the center frequency of the UWB signal, and c is the speed of light. This factor acts to counteract the phase shift caused by non-ideal motion.

[0083] In this embodiment, the phase compensation factor constructed above is applied point by point to the original channel impulse response waveform data s(t,τ) acquired in step S1. Through complex multiplication, the phase history of the original signal is pre-corrected to obtain the corrected signal data s. comp (t,r).

[0084] In this embodiment, this dynamic baseline correction process effectively achieves the closed-loop gain of control-assisted sensing. By forcibly aligning the signal processing reference to the desired trajectory of the control system, the degradation of imaging coherence by IMU high-frequency noise and low-frequency drift is effectively suppressed. This allows subsequent imaging algorithms to still obtain well-focused, high signal-to-noise ratio range-Doppler images even when the UAV performs large S-shaped maneuvers, providing high-quality data input for accurate extraction of radio frequency features.

[0085] S4. Perform inverse synthetic aperture radar imaging processing on the corrected channel impulse response data to generate a range-Doppler image, and extract strong scattering points from the range-Doppler image as radio frequency landmarks.

[0086] In this embodiment, step S4 mainly performs the operation of generating UWB-ISAR radio frequency images and extracting features, aiming to convert the phase-corrected time-domain echo signal into geometric feature points that can be identified and tracked by a computer, i.e., radio frequency landmarks. This step utilizes the imaging principle of inverse synthetic aperture radar to separate significant target scattering centers from the cluttered echoes of the mine environment.

[0087] In this embodiment, the channel impulse response data s output in step S3 after dynamic baseline correction is... comp (t,r) undergoes two-dimensional frequency domain transformation. Since the original data is a time-domain pulse sequence, to obtain the target's range and relative velocity information, spectral analysis needs to be performed in both the fast and slow time dimensions. Specifically, the system first performs a Fast Fourier Transform on the fast time dimension τ of each pulse. This process maps the time delay information to range information, thereby achieving pulse compression in the range direction and improving range resolution.

[0088] In this embodiment, after range compression, the system then performs a Fast Fourier Transform (FFT) on the data matrix in the slow-time t dimension. Because of the relative motion between the UAV and environmental feature points during flight, this relative motion introduces a Doppler frequency shift in the echo signal. The slow-time FFT processing can resolve the signal's phase change rate into Doppler frequencies, thereby achieving azimuth focusing.

[0089] In this embodiment, after the above two dimensional transformations, a value containing distance information r and Doppler information f is generated. d Two-dimensional distance-Doppler image I(r,f) d The process of generating this image can be described by a mathematical expression:

[0090] I(r,f d ) = F slow {F fast {s comp (t,r)}};

[0091] Among them, F fast and F slow These represent the Fourier transform operators for the fast and slow time dimensions, respectively. In this image, the horizontal axis represents the radial distance between the target and the drone, and the vertical axis represents the radial velocity of the target relative to the drone.

[0092] In this embodiment, the generated RD image typically contains a large amount of background noise, clutter, and sidelobe interference. To extract stable and reliable environmental features for subsequent localization, a robust detection algorithm is required. Preferably, this invention employs a two-dimensional constant false alarm rate (CFAR) detection algorithm for feature extraction. This algorithm estimates the local background noise statistical characteristics by sliding a detection window across the image and utilizing reference units around the window, and dynamically calculates the detection threshold based on a preset false alarm probability.

[0093] In this embodiment, when the signal strength of the detection unit exceeds the calculated local noise threshold, the point is determined to be a strong scattering point, i.e., an effective radio frequency feature point. This mechanism can effectively adapt to the complex electromagnetic environment in mines, maintaining stable detection performance whether near strongly reflective metal supports or in weakly reflective rock wall areas, avoiding missed or false detections caused by fixed thresholds.

[0094] In this embodiment, for each extracted strong scattering point, the system reads its coordinate position in the RD image, thereby determining the measurement distance R of the feature point. i And measuring Doppler velocity v D,i These two physical quantities together constitute the measurement vector z of the i-th RF beacon. RF,i :

[0095] z RF,i =[R i ,v D,i ] T ;

[0096] This measurement vector includes not only the geometric position constraints found in traditional SLAM, but also motion state constraints. This is a significant feature that distinguishes the UWB-ISAR imaging scheme from traditional point cloud SLAM, providing higher-dimensional observation information for subsequent state estimation.

[0097] S5. Construct an observation model based on radio frequency landmarks, calculate the observation residuals, and use the observation residuals to correct and update the UAV pose state to obtain a high-precision positioning state.

[0098] In this embodiment, step S5 mainly performs the radio frequency-inertial odometry state update operation, which is the core calculation link of the entire positioning algorithm. Its main task is to use the high-precision radio frequency landmark observations extracted in step S4 to correct the prior pose state obtained by the IMU recursion, thereby eliminating the cumulative drift of the sensor and achieving stable positioning over a long period of time.

[0099] In this embodiment, the system first constructs an observation model for each successfully tracked radio frequency (RF) landmark. The position of the RF landmark in the world coordinate system is denoted as p. LiSince radio frequency imaging provides measurement information in two dimensions—range and Doppler velocity—it is necessary to establish corresponding observation equations for each dimension.

[0100] In this embodiment, a distance observation model is constructed, which describes the location p of the radio frequency beacon. Li With the drone's current location p k The geometric constraints between them. Specifically, the theoretical distance observations. Equal to the Euclidean distance between the two:

[0101]

[0102] This constraint ensures that the drone's position estimation is confined to a sphere with the landmark as the center and the measured distance as the radius.

[0103] In this embodiment, a Doppler observation model is constructed, which describes the relative motion constraints between the radio frequency beacon and the UAV. This is a key feature that distinguishes this invention from traditional pure range-based positioning. Theoretical Doppler velocity observation values... This equals the radial velocity component of the drone relative to the landmark. Mathematically, it can be represented as the direction of the relative position vector and the drone's velocity vector v. k dot product:

[0104]

[0105] This observation model directly introduces the velocity vector as a state variable, enabling the system to not only correct position drift but also effectively suppress velocity error divergence, which is particularly crucial for integral-based inertial navigation systems.

[0106] In this embodiment, after constructing the observation model, the system calculates the actual measurement vector z. RF,i The difference between the observed values ​​and the theoretical observations yields the observed residual vector r. i Subsequently, in order to obtain the optimal state estimate, the system constructs a nonlinear least squares cost function that includes prior residuals and observation residuals.

[0107] In this embodiment, the cost function is typically in the form of a weighted sum of squared Mahalanobis distances. Specifically, the optimization objective is to find the optimal state vector x that minimizes the following cost function:

[0108]

[0109] Among them, the first item Σ represents the prior constraints based on IMU integration. p The first term is the prior covariance matrix; the second term represents the observation constraints for all valid radio frequency landmarks. This is the noise covariance matrix for RF measurements, which is adaptively adjusted based on the signal-to-noise ratio during CFAR detection.

[0110] In this embodiment, to solve the aforementioned nonlinear optimization problem, an error-state Kalman filter or a factor graph optimization algorithm based on a sliding window is preferably used. During the iterative solution process, the system continuously adjusts the UAV's position, velocity, attitude, and the IMU's zero-bias parameters until the residual converges to a minimum. The final calculated state vector This is the high-precision positioning result at the current moment. This result is not only used for subsequent navigation control, but also serves as the starting state for the IMU recursion at the next moment, forming a closed-loop state estimation process.

[0111] S6. Based on the high-precision positioning status and the desired trajectory, generate control commands to drive the UAV to fly, completing the closed loop of positioning and path correction.

[0112] In this embodiment, step S6 mainly performs closed-loop control operations. This step is the final stage of converting the system's perception results into physical actions, and it is also the starting stage of the next round of active perception cycle. Through the precise execution of the flight control module, the perception-motion coordination strategy planned in the aforementioned steps is implemented in physical space.

[0113] In this embodiment, the flight control module first receives the high-precision state estimation results output from step S5, including the corrected UAV position, velocity, and attitude information. These state quantities have undergone rigorous calculation by radio frequency-inertial odometry, eliminating the cumulative drift of the inertial sensors and representing the UAV's closest physical state at the current moment. Simultaneously, the controller reads the real-time desired trajectory generated in step S2 based on the observability assessment results. This desired trajectory may be a smooth straight line for rapid passage, or it may be an S-shaped maneuver trajectory superimposed with sinusoidal fluctuations, specifically planned to enhance environmental feature extraction.

[0114] In this embodiment, the controller calculates the state deviation between the current actual state and the desired trajectory. Specifically, it calculates the position error vector and the velocity error vector. To achieve high dynamic response tracking of the desired trajectory, a cascaded PID control algorithm or a model predictive control algorithm is preferably used. Based on the state deviation and combined with the constraints of the UAV's dynamic model, the controller calculates the thrust vector required at the current moment and the desired adjustment amount of the aircraft's attitude.

[0115] In this embodiment, the calculated control quantities are mapped to execution commands for the underlying motors. Specifically, a hybrid control algorithm converts thrust and torque commands into speed setpoints for each rotor motor, thereby generating corresponding pulse width modulation signals to drive the electronic speed controller and control the changes in motor speed. This process drives the UAV to generate actual displacement and attitude changes in physical space, thereby minimizing the tracking error with the desired trajectory.

[0116] In this embodiment, the key technical feature of this step lies in the deep coupling between its physical execution action and the radio frequency imaging process. When the system determines in step S2 that active path correction is needed and an S-shaped trajectory is generated, the physical execution in step S6 is not only to move the UAV from one point to another, but also to actively create observation conditions conducive to perception.

[0117] In this embodiment, specifically, the lateral S-shaped maneuver performed by the UAV physically constructs a non-linear motion path. This path forms a virtual synthetic aperture baseline in space. It is precisely because step S6 precisely executes this maneuver that the onboard UWB sensor can observe the environment from different lateral positions at subsequent sampling times, thereby artificially creating variations in parallax and Doppler shift in the observation perspective.

[0118] In this embodiment, the physical motion introduced by the control action directly provides the necessary physical boundary conditions for the data acquisition in step S1 and the ISAR imaging in step S4 at the next moment. Without the precise execution of the maneuver trajectory in step S6, the subsequent dynamic baseline correction would lose its physical basis, and the radio frequency image would not be able to be effectively focused through the Doppler effect. Therefore, this step realizes a logical closed loop from perception-based control to control-based perception, ensuring the continuous robust operation of the system in a weakly textured mining environment.

[0119] Please see the appendix Figure 2 A UWB-SLAM positioning and path correction system for mining drones, comprising:

[0120] The airborne sensing module is configured to acquire in real time data from the UAV's inertial measurement unit and raw waveform data of the channel impulse response from the ultra-wideband radio frequency front end;

[0121] The observability analysis and path decision module is configured to calculate observability indices and generate a desired trajectory superimposed with active incentive maneuvers when the indices are low.

[0122] The collaborative imaging processing module is configured to perform dynamic baseline correction on the raw waveform data of the channel impulse response using the desired trajectory, and to generate a range-Doppler image and extract radio frequency landmarks;

[0123] The state estimation module is configured to use radio frequency landmarks to build an observation model and calculate the observation residuals to correct and update the UAV pose state;

[0124] The flight control module is configured to control the drone's flight based on the corrected drone attitude state and the desired trajectory.

[0125] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A mine unmanned aerial vehicle (UAV) UWB-SLAM positioning and path correction method, characterized in that, The method comprises the following steps: S1, collecting inertial measurement unit data and channel impulse response raw waveform data of an ultra-wideband radio frequency front end of the unmanned aerial vehicle in real time, and using the inertial measurement unit data to recursively determine the unmanned aerial vehicle pose state; S2, constructing an observability index based on the current extracted radio frequency landmarks, and determining whether active path correction is needed according to the observability index; If the observability index is lower than a preset threshold, a desired trajectory superimposed with active excitation maneuver is generated; S3, using the desired trajectory generated in step S2 as a prior motion model, performing dynamic baseline correction on the channel impulse response raw waveform data collected in step S1 to obtain corrected channel impulse response data; S4, performing inverse synthetic aperture radar imaging processing on the corrected channel impulse response data to generate a range-Doppler image, and extracting strong scattering points from the range-Doppler image as radio frequency landmarks; S5, constructing an observation model based on the radio frequency landmarks, calculating an observation residual, and using the observation residual to correct and update the unmanned aerial vehicle pose state to obtain a high-precision positioning state; S6, generating a control instruction to drive the unmanned aerial vehicle to fly according to the high-precision positioning state and the desired trajectory, and completing the closed loop of positioning and path correction.

2. The mine unmanned vehicle UWB-SLAM positioning and path correction method according to claim 1, characterized in that, The specific process of constructing the observability index and determining whether active path correction is needed in step S2 comprises: using the geometric distribution information of N radio frequency landmarks extracted at the current time to construct a Fisher information matrix; calculating the minimum eigenvalue or determinant of the Fisher information matrix as the observability index of the current system; comparing the calculated observability index with a preset observability threshold; if the observability index is greater than or equal to the preset observability threshold, the current nominal path is kept as the desired trajectory; if the observability index is less than the preset observability threshold, it is determined that the system has entered a geometric degradation region, and the desired trajectory superimposed with active excitation maneuver is triggered to be generated.

3. The mine unmanned vehicle UWB-SLAM positioning and path correction method according to claim 1, characterized in that, The desired trajectory superimposed with active excitation maneuver specifically refers to: superimposing a transverse sinusoidal motion trajectory perpendicular to the forward direction on the original nominal path of the unmanned aerial vehicle; wherein the maneuver amplitude and maneuver frequency of the sinusoidal motion trajectory are in a positive correlation mapping relationship with the degree of decline of the observability index, and the lower the observability index, the greater the generated maneuver amplitude and maneuver frequency.

4. The mine unmanned vehicle UWB-SLAM positioning and path correction method according to claim 1, characterized in that, The specific process of performing dynamic baseline correction on the channel impulse response raw waveform data in step S3 comprises: within the synthetic aperture time window, the prior position obtained by recursive calculation of the inertial measurement unit is compared with the desired trajectory generated in step S2 to calculate a baseline deviation; using the baseline deviation to construct a phase compensation factor; applying the phase compensation factor to the channel impulse response raw waveform data to pre-compensate the phase history of the raw waveform data, so as to eliminate the micro-motion error between the actual flight trajectory of the unmanned aerial vehicle and the ideal imaging trajectory.

5. The mine unmanned vehicle UWB-SLAM positioning and path correction method according to claim 1, characterized in that, The specific process of generating a range-Doppler image in step S4 comprises: performing fast Fourier transform on the channel impulse response data after dynamic baseline correction in the fast time dimension to obtain range dimension information; performing fast Fourier transform on the data after fast time dimension transformation in the slow time dimension to obtain Doppler dimension information; The data after two transformations are combined to generate a two-dimensional range-Doppler image containing range information and Doppler velocity information.

6. The mine unmanned vehicle UWB-SLAM positioning and path correction method according to claim 1, characterized in that, The step S4 of extracting a strong scattering point as a radio frequency landmark specifically refers to: A constant false alarm rate detection algorithm is used to traverse the range-Doppler image; A pixel point with a signal-to-noise ratio higher than a set threshold is identified as a strong scattering point; The distance value and the Doppler velocity value corresponding to the strong scattering point are obtained to form a radio frequency landmark measurement vector at the moment.

7. The mine unmanned vehicle UWB-SLAM positioning and path correction method according to claim 1, characterized in that, The step S5 of constructing an observation model based on the radio frequency landmark specifically includes: A range observation model is constructed, which describes the Euclidean distance relationship between the radio frequency landmark position and the unmanned aerial vehicle position; A Doppler observation model is constructed, which describes the relative radial velocity relationship between the radio frequency landmark and the unmanned aerial vehicle, specifically the projection of the difference vector between the radio frequency landmark position and the unmanned aerial vehicle position on the unmanned aerial vehicle velocity vector.

8. The mine unmanned vehicle UWB-SLAM positioning and path correction method according to claim 1, characterized in that, The step S5 of using observation residuals to correct and update the unmanned aerial vehicle pose state specifically refers to: The difference between the radio frequency landmark measurement vector and the theoretical observation value calculated by the observation model is calculated to obtain the observation residual; A cost function containing the prior residual and the observation residual is constructed; An error state Kalman filter or a sliding window optimization algorithm is used to solve the optimal state vector that minimizes the cost function, and the correction of the unmanned aerial vehicle position, velocity, attitude and sensor zero offset is completed.

9. The mine unmanned vehicle UWB-SLAM positioning and path correction method according to claim 1, characterized in that, The step S6 specifically includes: The high-precision positioning state output by step S5 is fed back to the flight controller; The flight controller calculates the deviation between the high-precision positioning state and the expected trajectory; According to the deviation, a motor control instruction is generated to drive the unmanned aerial vehicle to perform straight flight or S-shaped active maneuver flight.

10. The mine unmanned aerial vehicle UWB-SLAM positioning and path correction system according to any one of claims 1-9, characterized in that, The system includes: An airborne sensing module configured to collect inertial measurement unit data of the unmanned aerial vehicle and channel impulse response raw waveform data of the ultra-wideband radio frequency front end in real time; An observability analysis and path decision module configured to calculate an observability index and generate an expected trajectory superimposed with active excitation maneuvers when the index is low; A collaborative imaging processing module configured to perform dynamic baseline correction on the channel impulse response raw waveform data using the expected trajectory, and generate a range-Doppler image and extract a radio frequency landmark; A state estimation module configured to construct an observation model using the radio frequency landmark and calculate observation residuals to correct and update the unmanned aerial vehicle pose state; A flight control module configured to control the unmanned aerial vehicle flight according to the corrected unmanned aerial vehicle pose state and the expected trajectory.

Images