A method for accurate positioning of a moving target in a field environment

Abstract

The application discloses a kind of accurate positioning method of mobile target under field environment, control unmanned plane searches target terminal wireless signal, and real-time direction finding approximation is carried out according to signal strength variation;In signal coverage range, unmanned plane obtains own coordinate and wireless ranging value with target terminal, simultaneously receives the motion parameter and inertial prediction position that target terminal returns;According to ranging residual, carry out anomaly detection, when abnormal, based on the comprehensive cost optimization retest navigation point, when normal, construct non-synchronous measurement motion compensation model, and fold ranging value at each sampling time to unified reference time, based on motion compensation residual and signal quality index, construct error weighted matrix to solve spatial intersection equation obtained;When unmanned plane and coarse positioning coordinate distance is less than threshold value, identify target heat source feature and obtain high-precision geographic coordinate.The application effectively improves the search efficiency and positioning accuracy of mobile target in complex environment, and does not need multi-machine cooperation or ground base station.

Description

Technical Field

[0001] This invention relates to positioning and navigation technology, specifically to a method for accurately locating a moving target in a field environment. Background Technology

[0002] Accurate positioning of moving targets such as grazing animals and field workers has always been a challenge in intelligent management in complex terrain environments such as mountainous and forested areas. Traditional positioning methods mainly rely on satellite navigation systems (such as BeiDou and GPS), but in areas with severe signal obstruction such as canyons and dense forests, satellite signals attenuate drastically, resulting in a significant decrease in positioning accuracy, or even signal loss.

[0003] In existing technologies, some solutions employ a wired + BeiDou fusion positioning method: when the target is within a terrestrial network coverage area, wired backhaul or ground base station assistance is used to correct the BeiDou positioning results. This method offers high accuracy but is expensive to deploy and unsuitable for vast uninhabited areas. Other solutions use a purely wireless positioning method combining wireless and ranging. However, its accuracy is significantly affected by multipath effects and non-line-of-sight propagation. Furthermore, in the case of a single UAV, if the target location is unknown, the initial search lacks guidance, resulting in low search efficiency. Additionally, existing infrared positioning methods have limited range, leading to errors exceeding 50 meters in single-base station or single-UAV scenarios. Moreover, while multi-UAV collaborative positioning or joint positioning of UAVs and ground base stations improves accuracy, the systems are complex and costly, making them difficult to implement in remote areas.

[0004] Existing research includes similar technologies such as airborne multi-source data fusion positioning schemes and multi-information fusion medical search and rescue UAV positioning methods. Airborne multi-source data fusion schemes utilize UAVs to collect multi-source data such as spectrum and images, and perform cross-validation on the ground to achieve approximate location of radiation sources. However, they rely on ground monitoring stations for real-time calculation and verification, and their search path planning uses a regional coverage method, limiting real-time tracking accuracy when the target is moving. Medical search and rescue UAV methods achieve high-precision positioning through the fusion of multiple sensors, including GPS, inertial measurement, optical flow, ultrasound, and infrared thermal imaging. However, their application scenarios are biased towards static or semi-static targets, and the systems are complex, lacking sufficient initial detection capability for large-scale unknown targets in the field. Therefore, existing schemes still have significant shortcomings in achieving low-cost, high-precision positioning of moving targets in the field: first, the initial target detection capability is limited, relying heavily on prior information or static search areas; second, insufficient compensation for moving target motion and handling of asynchronous sampling errors lead to decreased positioning accuracy; and third, multi-source fusion schemes lack real-time performance and low power consumption in complex environments, hindering long-term continuous tracking. Summary of the Invention

[0005] The technical problem to be solved by this invention: 1. Existing single-UAV multi-point hovering or continuous sampling positioning results in low positioning accuracy due to the accumulation of errors caused by asynchronous measurement.

[0006] 2. The initial target detection capability is limited, relying heavily on prior information or static search areas.

[0007] 3. Multipath effects in the field and non-line-of-sight propagation can cause abnormal jumps or temporary loss of ranging values. Relying solely on wireless ranging can lead to unstable positioning results.

[0008] 4. Multi-source fusion solutions lack real-time performance and low power consumption in complex environments, which is not conducive to long-term continuous tracking.

[0009] To address the aforementioned problems of existing technologies, a method for precise positioning of moving targets in the field is provided. This method covers the entire process from target discovery to precise positioning, features low power consumption, adapts to complex field environments, and improves search efficiency and positioning accuracy.

[0010] To solve the above-mentioned technical problems, the technical solution adopted by the present invention is as follows: A method for accurately locating moving targets in a field environment includes the following steps: S1: Control the drone to fly along the first path to capture the wireless signal of the target terminal device; after capturing the wireless signal, guide the drone to fly along the second path to approach the target terminal device through radio direction finding; S2: During the flight of the UAV within the signal coverage area of ​​the target terminal device, the UAV's own coordinates and the wireless ranging value with the target terminal device are acquired in real time and added to the sampling dataset. The inertial prediction position transmitted back by the target terminal device is also acquired. S3: Calculate the ranging residual based on the wireless ranging value, the UAV's own coordinates, and the inertial predicted position at the same time; perform ranging anomaly detection based on the ranging residual; if an anomaly exists, generate a retest waypoint centered on the inertial predicted position, control the UAV to fly to the retest waypoint with the lowest overall cost, and return to S2; otherwise, use the previous coarse positioning coordinates of the target terminal device as the estimated position of the target terminal device, estimate the target motion state based on the estimated position of the target terminal device, use the motion compensation model to convert the estimated position of the target terminal device at each sampling time to the reference time based on the target motion state, calculate the compensated equivalent ranging value, establish a spatial intersection equation based on the equivalent ranging value and the corresponding UAV own coordinates, predict the position of the target terminal device at the reference time based on the estimated position of the target terminal device and the target motion state, determine the motion compensation residual based on the predicted position of the target terminal device and the compensated equivalent ranging value, construct an error weighting matrix based on the motion compensation residual and signal quality indicators, solve the spatial intersection equation, and obtain the current coarse positioning coordinates of the target terminal device; S4: Determine whether the distance between the drone and the current coarse positioning coordinates is greater than the distance threshold. If yes, control the drone to fly toward the current coarse positioning coordinates and repeat S2; otherwise, activate the infrared thermal imaging module on the drone to obtain high-precision geographic coordinates by identifying the target heat source features and obtain the final positioning result.

[0011] Furthermore, the first path is a spiral or concentric circle expansion path from the inside out. If the approximate direction of the target is known, the first path is a semi-circular expansion path from the inside out. The second path is a straight path or an adaptive path dynamically generated based on changes in signal strength, changes in target direction, and obstacle information.

[0012] Furthermore, the inertial prediction position is the target position obtained by the target terminal device through short-time inertial prediction, and its mathematical expression is as follows:

[0013]

[0014] in, For the first The inertial predicted position at each sampling time. For the first The inertial prediction velocity at each sampling time. For the first The inertial predicted position at each sampling time. For the first The inertial prediction velocity at each sampling time. The time interval between adjacent sampling times. This represents the inertial predicted acceleration.

[0015] Furthermore, when performing ranging anomaly detection based on the ranging residual, the square of the ranging residual is divided by the variance of the ranging residual to obtain the anomaly detection statistic. If the anomaly detection statistic is greater than a preset threshold, an anomaly exists; otherwise, no anomaly exists. The mathematical expression for the anomaly detection statistic is as follows:

[0016] in, For the distance measurement residual, Let V be the variance of the distance measurement residual. The mathematical expression for the distance measurement residual is as follows:

[0017] in, For the first The location of the drone's own coordinates at each sampling time. For the first Wireless ranging values ​​at each sampling time, For the first The inertial predicted position at each sampling time.

[0018] Furthermore, when selecting the retest waypoint with the minimum overall cost based on the geometric accuracy factor and signal quality index of each candidate waypoint, the mathematical expression for the overall cost is as follows:

[0019] in, Indicates the first The overall cost of retesting each candidate waypoint According to the first The geometric accuracy factor is calculated based on the spatial geometric distribution of each candidate waypoint relative to existing sampled points. and The first The received signal strength indicator quality factor and signal-to-noise ratio quality factor of the target terminal equipment for each candidate waypoint. and This is the signal quality impact coefficient.

[0020] Furthermore, when using a motion compensation model to calculate the compensated equivalent ranging value after converting the estimated position of the target terminal device at each sampling time to a reference time based on the target motion state, the motion compensation model calculates the target terminal device position converted to a unified reference time based on the target motion state at each sampling time. Then, it calculates the compensated equivalent ranging value based on the converted target terminal device position and the UAV's own coordinates at each sampling time, wherein: The mathematical expression for the converted target terminal device location is as follows: If the target's motion is uniform, then we have

[0021] If the target motion is uniformly accelerated, then we have

[0022] in, Indicates reference time The converted target terminal device location, Indicates the sampling time The estimated location of the target terminal device. and These represent the estimated values ​​of the target terminal device's velocity and acceleration, respectively. Indicates from the sampling time to reference time The time interval; The mathematical expression for the compensated equivalent distance measurement value is as follows:

[0023] in, This represents the coordinates of the UAV at the i-th sampling time. This represents the target terminal device's estimated location at the i-th sampling time, after being converted to a unified reference time.

[0024] Furthermore, the target motion parameters include the velocity and acceleration of the target terminal device. When estimating the target motion state based on the estimated position of the target terminal device, specifically, the estimated position of the target terminal device is used as an observation. The target motion state vector containing position and velocity components is estimated and updated using Kalman filtering. The velocity component is extracted from the updated target motion state vector as the target motion state. When predicting the target terminal device position at a reference time based on the estimated position and target motion state, specifically, the target motion state vector obtained after the most recent Kalman filter update is used as the state prediction input. A state transition matrix is ​​constructed based on the time interval between the most recent estimated time and the reference time corresponding to the target motion state vector. The target motion state is recursively derived from the most recent estimated time to the reference time through the Kalman filter state prediction equation to obtain the target motion state vector at the reference time. The position component is extracted from it as the predicted position of the target terminal device at the reference time.

[0025] Furthermore, based on the predicted position of the target terminal device and the compensated equivalent ranging value, the motion compensation residual is determined. An error weighting matrix is ​​constructed based on the motion compensation residual and signal quality indicators. Solving the spatial intersection equation includes the following steps: The motion compensation residual is determined based on the predicted position of the target terminal device at the reference time, the compensated equivalent ranging value, and the UAV's own coordinates at each sampling time. The mathematical expression is as follows:

[0026] in, This represents the coordinates of the UAV at the i-th sampling time. This represents the equivalent ranging value at the i-th sampling time. This indicates the predicted location of the target terminal device at the reference time. Calculate the current received signal strength and signal-to-noise ratio (SNR) of the target terminal device's wireless signal, and then normalize the current received signal strength and SNR to calculate the received signal strength quality factor. With signal-to-noise ratio quality factor ; The equivalent measurement variance of the distance measurements is constructed, and its mathematical expression is as follows:

[0027] in, This represents the equivalent measurement variance at the i-th sampling time. The baseline distance measurement variance, and These are the weighting coefficients for received signal strength and signal-to-noise ratio, respectively. The influence coefficient of motion compensation residuals. To prevent extremely small positive numbers with a denominator of zero; The ranging weights are calculated based on the equivalent measurement variance, and the error weighting matrix is ​​constructed from the ranging weights of each sampling point. The mathematical expression for the ranging weights is as follows:

[0028] in, This represents the ranging weight at the i-th sampling time. Based on the error weighting matrix, a weighted nonlinear least squares objective function is established to solve the spatial intersection equation. The mathematical expression of the weighted nonlinear least squares objective function is as follows:

[0029] in, These are the coarse positioning coordinates of the target terminal device to be solved. It is the residual of the spatial intersection equation. This represents the coordinates of the UAV at the i-th sampling time. This represents the equivalent distance measurement value at the i-th sampling time.

[0030] Furthermore, when obtaining high-precision geographic coordinates by identifying target heat source features, the following steps are included: Candidate search areas are determined in the infrared images acquired by the infrared thermal imaging module based on the coarse positioning coordinates, and the average background temperature within the candidate search areas is calculated. and background temperature standard deviation The target temperature difference threshold is calculated using the following mathematical expression:

[0031] in, This is the threshold adjustment coefficient, which can be set according to changes in temperature difference in the field environment; Pixels with temperatures higher than the adaptive temperature difference threshold and a temperature difference greater than the preset temperature difference threshold are extracted as candidate target pixels. Connectivity analysis is performed on the candidate target pixels, and the connected regions with areas within a preset range and closest to the coarse positioning coordinate projection position are selected as the final target regions. Temperature difference weighted centroid calculation is performed on the pixels within the final target area to obtain the coordinates of the target center pixel; Based on the infrared lens parameters, the target center pixel coordinates are converted into a unit line-of-sight vector in the camera coordinate system. Combined with the UAV attitude angle and gimbal attitude angle, the unit line-of-sight vector is converted into the local navigation coordinate system. Based on the UAV's current coordinates, flight altitude, and target ground elevation, the target's local coordinates are calculated through the intersection of the ray and the ground. The target's local coordinates are then converted into high-precision geographic coordinates.

[0032] Furthermore, after obtaining high-precision geographic coordinates by identifying the characteristics of the target heat source, the process also includes: The high-precision geographic coordinates and the pseudo coordinates calculated from the wireless ranging values ​​are used as multi-source observations for the same target terminal device location. They are unified into the same target state model and jointly solved by extended Kalman filtering or weighted least squares to obtain the fused target geographic coordinates and the final positioning result.

[0033] Compared with the prior art, the advantages of the present invention are as follows: Through a closed-loop collaboration of wireless signal discovery, direction finding guidance, motion compensation coarse positioning, infrared precise positioning, and anomaly backtracking and replanning, the UAV can continuously search and accurately locate targets even when their initial position is unknown, they are continuously moving, and there are obstructions in mountainous or forested areas. Compared to solutions that rely solely on wireless ranging, simple infrared identification, or pre-defined area search and rescue, this invention uses wireless signal quality, motion compensation residuals, inertial prediction results, and infrared heat source identification results for path adjustment and coordinate correction. This reduces the impact of asynchronous measurement errors and abnormal ranging values, improving the accuracy, stability, and infrared identification success rate of locating moving targets in the field. Furthermore, the infrared module is only activated at close range, and the target terminal employs a low-power, rechargeable design, meeting the needs of long-term deployment and low-cost applications in remote field environments. Specifically: (1) To address the problems of asynchronous measurement error accumulation and low positioning accuracy, this invention obtains the target motion parameters and uses a motion compensation model to convert the ranging values ​​at different sampling times to a unified reference time, thereby eliminating the systematic error introduced by the continuous movement of the target and the misalignment of the sampling time, so that the ranging values ​​at different times meet the geometric consistency of spatial intersection under a unified time reference; at the same time, by jointly constructing an error weighting matrix by motion compensation residual and signal quality index, the influence of abnormal ranging values ​​on the solution results is suppressed, the asynchronous measurement error is reduced, and the positioning accuracy is improved.

[0034] (2) In view of the problem of limited initial target discovery capability and lack of effective guidance, the present invention controls the UAV to fly along the first path to conduct a search without prior knowledge. After capturing the signal, the UAV is guided to fly along the second path to approach the target terminal device through radio direction finding. In the absence of prior location information, a closed-loop guidance from wide-area search to directional approach is realized, which improves the initial target discovery capability and search efficiency without relying on prior location or ground base station.

[0035] (3) To address the problem of abnormal ranging jumps or loss caused by multipath and non-line-of-sight propagation in the field, this invention uses the inertial prediction position returned by the target terminal to calculate the ranging residual for anomaly detection; when there is an anomaly, a remeasured waypoint is generated and selected with the inertial prediction position as the center, and a stable ranging connection is re-established; when there is a normal situation, a weighted solution is performed based on the motion compensation residual and the signal quality index, so that the inertial prediction result and the wireless signal quality are used together for the path adjustment and coordinate correction of the UAV, thereby improving the reliability of continuous tracking.

[0036] (4) In response to the problems of insufficient real-time performance and low power consumption of multi-source fusion schemes, this invention adopts a two-stage strategy of long-distance wireless coarse positioning and short-distance infrared precise positioning. The infrared module is only activated after approaching the threshold. While meeting the long-term deployment requirements, it obtains high-precision coordinates through infrared heat source identification, thereby improving the infrared identification success rate. Attached Figure Description

[0037] Figure 1 This is a system architecture diagram of an embodiment of the present invention.

[0038] Figure 2 This is a flowchart of a method according to an embodiment of the present invention.

[0039] Figure 3 This is a specific example flowchart in an embodiment of the present invention. Detailed Implementation

[0040] The present invention will be further described below with reference to the accompanying drawings and specific preferred embodiments, but this does not limit the scope of protection of the present invention.

[0041] Example 1 To address the problems of low positioning accuracy (typically deviation > 50 meters) caused by the accumulation of errors in existing single-UAV multi-point hovering positioning methods due to asynchronous measurement, this embodiment proposes a precise positioning method for moving targets in the field environment. This method effectively improves the search efficiency and positioning accuracy of moving targets in complex environments, and does not require multi-UAV collaboration or ground base stations. It provides a theoretically feasible new path for low-cost and high-precision positioning of moving targets in complex environments such as mountains and forests.

[0042] The method in this embodiment is applied to Figure 1The system shown is a precise positioning system for moving targets in a field environment, such as... Figure 1 As shown, the system includes a UAV 100 and a target terminal device 200. Through a joint algorithm combining radio signal detection, direction finding guidance, segmented approach, and infrared precise positioning, it achieves efficient searching and high-precision positioning of moving targets in complex environments. Specifically: The drone 100 is equipped with: The Beidou positioning module 110 is used to obtain the real-time coordinates of the UAV; The wireless signal receiving and direction finding module 120 is used to search for target signals and determine direction. Infrared thermal imaging module 140 is used for accurate identification of target heat source characteristics at close range; The flight control unit 130 is used to control the flight path of the UAV and perform motion compensation and coordinate calculation.

[0043] The target terminal device 200 is equipped with: The wireless signal transmitting module 210 is used to transmit wireless signals that can be detected by drones. Motion sensor 220 (such as a six-axis inertial measurement unit) is used to detect the target's motion state and transmit the target's motion parameters to the drone via a wireless link; The solar energy acquisition module 230, including a flexible solar panel and a high-efficiency power management unit, provides supplemental power to the wireless signal transmission module, motion sensor, and inertial measurement unit in field environments. Combined with a low-power energy storage battery, it enables long-term autonomous power supply, significantly extending the device's continuous operating time and preventing positioning interruptions due to power depletion. This design is particularly suitable for applications where frequent battery replacements are difficult, such as in vast uninhabited areas and grazing scenarios. The inertial measurement unit 240 (which can also be integrated with a motion sensor) is used to perform short-term dead reckoning based on the target's previous reliable position, velocity state, and inertial measurement data during periods of abnormal or abrupt changes in the wireless ranging signal. The predicted inertial position is then transmitted to the UAV via a wireless link. The UAV uses Kalman filtering to fuse the wireless ranging values ​​and inertial data, suppressing positioning deviations caused by ranging anomalies or motion model mismatches, and outputting a smooth and stable target trajectory.

[0044] The BeiDou positioning module 250 is used to output BeiDou observation coordinates when a signal is available.

[0045] It should be noted that although the target terminal device is equipped with a BeiDou module and can output BeiDou observation coordinates when the signal is available, relying directly on the target terminal to transmit BeiDou coordinates for positioning in complex field environments has significant limitations: First, mountainous, forested, and canyon terrain severely obstructs satellite signals, often resulting in signal loss, accuracy attenuation (errors can reach tens to hundreds of meters), or short-term drift in the target terminal's BeiDou positioning, making it impossible to guarantee continuous and reliable high-precision coordinate output; Second, when the target terminal is deployed in the field for a long time, continuously keeping the BeiDou positioning module running consumes a lot of power, which is not conducive to achieving long-term autonomous power supply in conjunction with low-power energy storage batteries and solar energy acquisition modules. Therefore, the goal of this embodiment is for the UAV to independently complete target search and high-precision positioning even when the target terminal's BeiDou signal is unavailable or insufficient in accuracy, through a full-process active detection design of "wireless signal discovery—direction finding guidance—segmented approach—infrared precise positioning".

[0046] like Figure 2 As shown, the method in this embodiment includes the following steps: S1: The flight control unit 130 controls the UAV 100 to fly along the first path to capture the wireless signal of the target terminal device 200; after capturing the wireless signal, the wireless signal receiving and direction finding module 120 performs radio direction finding to guide the UAV to fly along the second path to approach the target terminal device. S2: During the flight of UAV 100 within the signal coverage area of ​​target terminal device 200, the UAV 100 obtains its own coordinates in real time through Beidou positioning module 110, calculates the wireless ranging value with target terminal device 200 and adds it to the sampling dataset, and also obtains the inertial prediction position transmitted back by target terminal device 200. S3: The flight control unit 130 calculates the ranging residual based on the wireless ranging value, the UAV's own coordinates, and the inertial predicted position at the same time. It then performs ranging anomaly detection based on the ranging residual. If an anomaly is found, a retest waypoint is generated centered on the inertial predicted position. The UAV is then controlled to fly to the retest waypoint with the lowest overall cost and returns to S2. Otherwise, the previous coarse positioning coordinates of the target terminal device are used as the estimated position of the target terminal device. The target motion state is estimated based on the estimated position of the target terminal device. A motion compensation model is used to convert the estimated position of the target terminal device at each sampling time to the reference time based on the target motion state, and then the compensated equivalent ranging value is calculated. A spatial intersection equation is established based on the equivalent ranging value and the corresponding UAV own coordinates. The target terminal device position at the reference time is predicted based on the estimated position of the target terminal device and the target motion state. The motion compensation residual is determined based on the predicted position of the target terminal device and the compensated equivalent ranging value. An error weighting matrix is ​​constructed based on the motion compensation residual and signal quality indicators. The spatial intersection equation is solved to obtain the current coarse positioning coordinates of the target terminal device. S4: The flight control unit 130 determines whether the distance between the UAV and the current coarse positioning coordinates is greater than the distance threshold. If so, it controls the UAV to fly toward the current coarse positioning coordinates and repeats S2. Otherwise, it activates the infrared thermal imaging module 140 to obtain high-precision geographic coordinates by identifying the target heat source features and obtain the final positioning result.

[0047] The above steps, through a complete process design of "wireless signal discovery—direction finding guidance—segmented approximation—infrared precise positioning," overcome the shortcomings of traditional methods that rely on the target's prior position and are prone to blind initial searches, achieving a seamless transition from wide-area search to local precise positioning. The straight-line flight and motion compensation model ensure that the geometric configuration between adjacent sampling points can be pre-optimized, while continuous sampling allows for continuous tracking of the target's motion state, providing sufficient time redundancy for motion compensation. The significance of this complete process design lies in: constructing an independent positioning capability that does not rely on the target terminal's satellite receiving environment, solving the positioning blind spots and insufficient accuracy problems caused by relying solely on BeiDou in field environments; and achieving higher positioning accuracy and stronger environmental adaptability than target terminal-only BeiDou positioning through active direction finding approximation, motion compensation calculation, and infrared heat source identification at the UAV end.

[0048] The following provides a detailed explanation of each step.

[0049] In step S1 of this embodiment, the first path is a spiral or concentric circle expansion path from the inside out; if the approximate direction of the target is known, the first path is a semi-circular expansion path from the inside out to shorten the flight distance; the second path is a straight path or an adaptive path dynamically generated based on changes in signal strength, changes in target direction, and obstacle information. The specific implementation process is as follows: Initial Search: The UAV 100 is controlled to fly in a spiral path expanding from the inside out to capture the wireless signal transmitted by the target terminal device 200. The flight path can be pre-planned based on the target's last known location (if any) or terrain data, the flight speed is set to a constant value, and the sampling time interval is fixed. During the flight, the UAV 100 flies in a spiral path with the starting point as the center, and the radius gradually increases until the target signal is captured for the first time.

[0050] Radio direction finding guidance: After receiving the target signal, the RSSI comparison method is used: the UAV makes small oscillations or flies along an arc in the horizontal plane, compares the signal strength received in different directions, and takes the direction with the maximum value as the approach heading, gradually reducing the distance to the target. In this embodiment, the radio direction finding guidance step specifically adopts the following method: The drone continuously measures the Received Signal Strength Indication (RSSI) of the signal transmitted by the target terminal using its onboard wireless signal receiving module. Assume the drone is located at... The measured signal strength is At adjacent sampling points The measured signal strength is .like If the signal strength is within a certain range, the drone is determined to be approaching the target and continues to maintain its current heading; otherwise, the drone performs a yaw maneuver (such as hovering, rotating, or flying in a small-radius arc), records the maximum RSSI value in different directions, and takes that direction as the new approach heading. This process is repeated until the distance between the drone and the target is less than a preset threshold, or the signal strength reaches saturation. Alternatively, the drone can be equipped with a directional antenna array. By measuring the phase difference of the signal reaching different antenna elements, the direction of arrival (AOA) can be calculated, directly guiding the drone to fly in that direction.

[0051] In step S2 of this embodiment, within the signal coverage area, the UAV flies along a straight line or an adaptive path. During the flight, the UAV acquires its own BeiDou coordinates Pi (Xi, Yi, Zi) and the wireless ranging value di with the target terminal device at multiple consecutive sampling times, where i = 1, 2, ..., n, n ≥ 3.

[0052] Specifically, when the UAV flies along a straight path, its flight speed remains constant or varies according to a preset speed curve, and the sampling time interval is fixed. The spatial distance between adjacent sampling points satisfies the geometric configuration optimization conditions, minimizing the geometric precision factor (DOP) formed by each ranging point, thereby improving the stability of the solution.

[0053] In this embodiment, the so-called adaptive path refers to the UAV dynamically generating the next course and flight distance based on continuously sampled signal strength index (RSSI), changes in target orientation, and obstacle information. Specific implementation steps include: a) Measure the target signal strength and direction at the initial point; b) Calculate the signal gradient and determine the optimal heading; c) Make minor adjustments to the course to avoid obstacles or areas that cannot be flown over; d) Repeat the above steps at the next sampling point to form a continuous dynamic adjustment track and realize real-time adaptive path planning.

[0054] In a further embodiment, if the UAV fails to establish a wireless connection with the terminal device at multiple consecutive sampling points during spiral search or straight flight, the UAV automatically switches to spiral extended search path (or expands the search radius) until the signal connection is restored and it re-enters the direction finding guidance mode. This mechanism can automatically expand the search range when the target deviates from the preset area, improving system robustness.

[0055] Through the above steps, the method of this embodiment does not rely on a preset path library, but uses real-time measurement data to calculate the trajectory, ensuring effective approximation of moving targets in unknown environments.

[0056] In this embodiment, when calculating the wireless ranging value with respect to the target terminal device 200, the distance is measured using the Time Difference of Arrival (TOA) or Signal Strength Indication (RSSI) of the received signal. If estimated using an algorithm, the ranging formula is:

[0057] in Let i be the distance measurement value from the i-th sample. For signal propagation speed, For the target launch time, For drone reception time, As a measurement error correction term, continuous ranging values ​​can be corrected using Kalman filtering or weighted least squares methods to achieve high-precision estimation.

[0058] In this embodiment, the inertial predicted position is the target position obtained by the target terminal device through short-time inertial prediction. The inertial measurement unit of the target terminal device includes an accelerometer and a gyroscope, which is used to perform short-time dead reckoning based on the target's previous reliable position, velocity state, and inertial measurement data during periods of abnormal wireless ranging signal, brief loss, or jumps in ranging values. The reckoning results are then transmitted to the UAV via a wireless link. The UAV automatically determines whether to adjust its flight path based on the inertial reckoning results, wireless ranging residuals, and positioning uncertainty. If necessary, it performs approach flight, fan-shaped remeasurement, or spiral expansion remeasurement centered on the predicted target position to increase effective sampling points, improve ranging geometry, and re-establish a stable ranging connection.

[0059] Specifically, the target terminal device periodically collects acceleration and angular velocity data output by the inertial measurement unit and corrects for zero bias. After updating the target terminal's attitude based on the gyroscope output, the acceleration is converted to the navigation coordinate system to obtain the target acceleration aNk in the navigation coordinate system. Based on the previous reliable target position and velocity, short-time inertial prediction is performed according to the following formula, the mathematical expression of which is as follows:

[0060]

[0061] in, For the first The inertial predicted position at each sampling time. For the first The inertial prediction velocity at each sampling time. For the first The inertial predicted position at each sampling time. For the first The inertial prediction velocity at each sampling time. The time interval between adjacent sampling times. This represents the inertial predicted acceleration.

[0062] In step S3 of this embodiment, based on the UAV flight speed, sampling time interval, and target motion state estimation, a motion compensation model under asynchronous measurement is constructed. The ranging values ​​at different sampling times are converted to a unified reference time to obtain the compensated equivalent ranging value. Based on the compensated ranging value and the corresponding UAV coordinates, a spatial intersection equation system is established, and the target absolute coordinates are calculated using nonlinear least squares or extended Kalman filtering. The specific implementation process is as follows: 1) Perform ranging anomaly detection; if anomalies are found, perform waypoint remeasurement and optimization.

[0063] When the drone receives new wireless ranging values At that time, the wireless ranging value is compared with the inertial predicted position to obtain the ranging residual.

[0064] Distance measurement residual formula

[0065] in, For the first The location of the drone's own coordinates at each sampling time. For the first Wireless ranging values ​​at each sampling time, For the first The inertial predicted position at each sampling time. This represents the residual between the wireless ranging value and the inertial prediction result. To determine whether the wireless ranging is abnormal, an anomaly detection statistic is calculated based on the ranging residual and its variance. Specifically, anomaly detection is performed by dividing the square of the ranging residual by its variance. If the anomaly detection statistic is greater than a preset threshold, an anomaly exists; otherwise, no anomaly is found. The mathematical expression for the anomaly detection statistic is as follows:

[0066] in, For the distance measurement residual, The variance of the ranging residual can be determined jointly by the variance of the wireless ranging noise and the inertial prediction uncertainty. Greater than the preset threshold When an abnormal wireless ranging value is detected, and the wireless signal is lost or abnormalities occur in multiple consecutive sampling periods, the UAV will not directly use the abnormal ranging value to update the target coordinates, but will instead prioritize using the inertial prediction position. As a temporary target center.

[0067] When the wireless ranging residual does not exceed the limit, the UAV performs an approach flight along the predicted target position direction, and the approach heading angle is calculated according to the following formula.

[0068] Formula for approaching the heading angle of a drone

[0069] in, To determine the heading angle for the next flight of the drone, , To fuse or predict the target horizontal coordinates, , This represents the current horizontal coordinates of the UAV. The UAV generates the next target waypoint based on the given heading angle.

[0070] Formula for drones to approach flight target waypoints

[0071] in, To adjust the step size for a single path, To determine the next waypoint for the drone.

[0072] If an anomaly is detected, candidate retest waypoints are generated centered on the inertial prediction location. Based on the geometric accuracy factor and signal quality index of each candidate waypoint, the waypoint with the lowest overall cost is selected, and the UAV is controlled to fly to that waypoint. Specifically, when wireless ranging experiences continuous anomalies or a brief loss of wireless connection, the UAV generates candidate retest waypoints centered on the inertial prediction target location and selects the waypoint with the lowest overall cost as the next target waypoint.

[0073] Formula for generating waypoints for retesting

[0074] Formula for comprehensive cost of re-surveying waypoints

[0075] in, For the first One candidate waypoint for retesting For the remeasured radius, For safe flight altitude, The azimuth of the candidate waypoints. Indicates the first The overall cost of retesting each candidate waypoint According to the first The geometric accuracy factor is calculated based on the spatial geometric distribution of each candidate waypoint relative to existing sampled points. and The first The received signal strength indicator quality factor and signal-to-noise ratio quality factor of the target terminal equipment for each candidate waypoint. and This represents the signal quality impact coefficient. The overall cost for the UAV is selected. The smallest candidate waypoint will be selected as the next target waypoint, i.e., the corresponding... Assigned value .

[0076] In this way, when the wireless ranging value is normal, the system integrates the wireless ranging and inertial prediction results to output a stable target trajectory; when the wireless ranging is abnormal or lost, the system uses the inertial measurement unit to perform short-term dead reckoning, and triggers the UAV to approach, fan-shaped remeasurement or spiral expansion remeasurement with the predicted target position as the center, thereby realizing positioning deviation suppression and adaptive adjustment of flight path.

[0077] 2) If no anomalies are found, construct a motion compensation model.

[0078] After obtaining the UAV coordinates and target wireless ranging values, the target's velocity and acceleration are estimated in real time using historical position sequences and Kalman filtering. The specific steps are as follows: a) Record the UAV position Pi(Xi,Yi,Zi) and the ranging value di at continuous sampling times; b) Constructing a motion compensation model for asynchronous measurements:

[0079]

[0080] in The target direction vector is obtained from the previous position calculations. c) Use Kalman filtering or extended Kalman filtering to predict the state of historical target position sequences and estimate the current velocity. With acceleration ; d) The predicted target motion state is used to calculate the ranging values ​​at different sampling times to a unified reference time, thereby achieving asynchronous error compensation.

[0081] Through the above steps, the UAV can achieve continuous dynamic tracking and high-precision positioning without relying on the prior position of the target.

[0082] The compensation model in this embodiment is as follows: assuming the target is at the sampling time... The actual location is ,satisfy Assuming the target moves at a constant or approximately constant speed within adjacent sampling intervals, its velocity v is not directly assumed but estimated in real-time using Kalman filtering based on the target's coarse positioning coordinates from previous samplings. Specifically, within each solution cycle j, the target's coarse positioning coordinates are first calculated using the aforementioned spatial intersection equations as the estimated position of the target terminal device. The j-th coarse target location coordinates:

[0083] Where (_j) represents the approximate target position obtained in the (j)th iteration by solving the wireless ranging and spatial intersection equations. For cases where the number of initial calculations is insufficient, the initial coarse positioning coordinates are obtained by directly solving the spatial intersection equations using the data from the first two sampling iterations, serving as the initial input for subsequent Kalman filter state estimation.

[0084] Subsequently, using the target position and velocity as the Kalman filter state variables, the following target motion state vector is constructed:

[0085] in,( ) represents the coordinates, i.e., the position components, of the target in the (j)th solution cycle. ) represents the velocity components of the target in the three coordinate directions.

[0086] Let the time interval between two consecutive solutions be ( = Given that the target moves at approximately a constant velocity over a short period of time, the state transition matrix is:

[0087] This leads to the state prediction equation:

[0088] in, This represents the current state predicted based on the filtering results from the previous time step. This represents the state estimate result that has been updated in the previous time step.

[0089] Finally, using the estimated position of the target terminal device as an observation, the target motion state vector containing position and velocity components is estimated and updated using Kalman filtering. The velocity component is then extracted from the updated target motion state vector as the target motion state. The coarse positioning coordinates of the target obtained from the (j)th solution are used as the observations:

[0090] The observation matrix is ​​defined as:

[0091] The observation residual is then:

[0092] The Kalman gain is calculated based on the state prediction error covariance and the observation noise covariance. First, covariance prediction is performed:

[0093] in, For the prediction error covariance matrix, Let Q be the error covariance matrix updated at the previous time step, and let Q be the process noise covariance matrix.

[0094] Then calculate the Kalman gain:

[0095] Where R is the observation noise covariance matrix, the size of which can be determined based on signal quality, ranging residuals, or experimental calibration results.

[0096] The state vector is updated according to the Kalman filter update formula:

[0097] After the filtering update is completed, the target velocity component is extracted from the state vector:

[0098] in, This refers to the target velocity estimated by Kalman filtering from the results of the previous calculations. If the number of calculations is insufficient in the initial stage, the difference between two adjacent coarse positioning coordinates can be used as the initial velocity value.

[0099] After obtaining the target's velocity, the target position at each sampling time is converted to a unified reference time. ).

[0100] The motion compensation model calculates the target terminal device position at a unified reference time based on the target motion state at each sampling time, and then calculates the compensated equivalent ranging value based on the converted target terminal device position and the UAV's own coordinates at each sampling time. If a uniform velocity model is used, the target's position at the reference time is calculated as follows:

[0101] If the target's acceleration is not negligible, then the acceleration estimate can be obtained from two consecutive velocity estimates:

[0102] A uniform acceleration model was used for position calculation.

[0103] in, Indicates reference time The converted target terminal device location, Indicates the sampling time The estimated location of the target terminal device. and These represent the estimated values ​​of the target terminal device's velocity and acceleration, respectively. Indicates from the sampling time to reference time The time interval; The mathematical expression for the compensated equivalent distance measurement value is as follows:

[0104] in, This represents the coordinates of the UAV at the i-th sampling time. This represents the target terminal device's estimated location at the i-th sampling time, after being converted to a unified reference time. .

[0105] This compensation eliminates the system error caused by the asynchronous movement of the target and the sampling by the UAV.

[0106] 3) Perform error-weighted solution.

[0107] In this embodiment, an error weighting matrix is ​​introduced into the nonlinear least squares solution. The weights are dynamically adjusted based on the signal quality indicators (RSSI, signal-to-noise ratio) and target motion compensation residuals of each ranging value. Ranging values ​​significantly affected by multipath effects, non-line-of-sight propagation, or wireless interference are assigned lower weights to suppress the influence of abnormal ranging values ​​on the solution results. Specifically, let the coordinates of the UAV at the i-th sampling time be... The equivalent distance measurement value after motion compensation is The coordinates of the target to be determined are: .

[0108] The spatial intersection equation established based on the equivalent ranging value and the corresponding UAV coordinates includes: Theoretical distance measurement value:

[0109] in, Represents target coordinates Coordinates of UAV sampling points The theoretical distance between them.

[0110] The residual corresponding to the i-th distance measurement value:

[0111] in, This represents the difference between the theoretical distance measurement value and the equivalent distance measurement value after motion compensation.

[0112] To construct the error weighting matrix, the received signal strength RSSI and signal-to-noise ratio SNR are first normalized.

[0113] RSSI normalization formula:

[0114] SNR normalization formula:

[0115] in, This is the normalized received signal strength quality factor. This is the normalized signal-to-noise ratio quality factor. If the normalization result is less than 0, then take 0; if the normalization result is greater than 1, then take 1.

[0116] Furthermore, the predicted target position at the reference time is obtained based on the target motion compensation model. Specifically, it is the target motion state vector obtained after Kalman filtering update. As input for state prediction, the target motion state vector includes the most recent estimated time. Target terminal device estimated position components and velocity components ; Calculate the most recent estimated time Reference time The time interval between According to the time interval Construct the state transition matrix from the most recent estimated time to the reference time. State prediction equations using Kalman filtering The target's motion state is adjusted from the most recent estimated time. The target motion state vector at the reference time is obtained. The position components are extracted from these components and used as the reference for predicting the target terminal device's position at the specified time. .

[0117] Then, the motion compensation residual corresponding to the i-th ranging value is calculated. Specifically, the motion compensation residual is determined based on the predicted position of the target terminal device at the reference time, the compensated equivalent ranging value, and the UAV's own coordinates at each sampling time. The mathematical expression is as follows:

[0118] in, This represents the coordinates of the UAV at the i-th sampling time. This represents the equivalent ranging value at the i-th sampling time. This indicates the predicted location of the target terminal device at the reference time. It is used to characterize the degree of inconsistency between the distance measured after motion compensation and the predicted target position.

[0119] Taking into account RSSI, signal-to-noise ratio, and motion-compensated residuals, the equivalent measurement variance of the i-th ranging value is constructed, and its mathematical expression is as follows:

[0120] in, This represents the equivalent measurement variance at the i-th sampling time. The baseline distance measurement variance, and These are the weighting coefficients for the received signal strength and signal-to-noise ratio, respectively, and they satisfy the following: , The influence coefficient of motion compensation residuals. To prevent extremely small positive numbers with a denominator of zero.

[0121] Based on the equivalent measurement variance The ranging weights are calculated using the following mathematical expression:

[0122] in, This represents the ranging weight at the i-th sampling time, which is determined by the ranging weights of each sampling point. The error weighting matrix is ​​constructed as follows:

[0123] Finally, based on the aforementioned error weighting matrix Establish a weighted nonlinear least squares objective function for the spatial intersection equation and The problem of solving for the target coordinates is transformed into the following weighted nonlinear least squares optimization problem, and the mathematical expression of the weighted nonlinear least squares objective function is as follows:

[0124] in, These are the coarse positioning coordinates of the target terminal device to be solved. It is the residual of the spatial intersection equation. This represents the coordinates of the UAV at the i-th sampling time. This represents the equivalent distance measurement value at the i-th sampling time.

[0125] Therefore, when the RSSI corresponding to a certain ranging value is low, the signal-to-noise ratio is low, or the motion compensation residual is large, its equivalent measurement variance is high. Increase, corresponding weight Automatically reduced; when the RSSI corresponding to a certain ranging value is high, the signal-to-noise ratio is high, and the motion compensation residual is small, its weight... Automatic weighting. This dynamic weighting method reduces the impact of abnormal ranging values ​​on target coordinate calculation results, improving the stability and accuracy of moving target localization in complex field environments.

[0126] In step S4 of this embodiment, when the distance between the UAV and the target is less than a preset threshold, the infrared thermal imaging module 140 is activated to scan the ground. By identifying the target's heat source characteristics, the pixel coordinates of the target in the infrared image are obtained. Combined with the UAV's current coordinates and the gimbal attitude angle, the high-precision geographic coordinates of the target are calculated. After positioning is completed, the infrared module is automatically turned off to save power. If infrared tracking fails (e.g., no heat source is detected), the coarse positioning coordinates are retained as the positioning result.

[0127] Specifically, obtaining high-precision geographic coordinates by identifying target heat source features includes the following steps: S201: Based on the coarse positioning coordinates, determine the candidate search region in the infrared image acquired by the infrared thermal imaging module, statistically analyze the mean and standard deviation of the background temperature within the candidate search region, and calculate the target temperature difference threshold. Specifically, the infrared image acquired by the infrared thermal imaging module is a temperature matrix. ,in These are the image pixel coordinates. The UAV determines candidate search regions in the infrared image based on the coarse positioning coordinates and calculates the average background temperature within these candidate search regions. and background temperature standard deviation To distinguish the target heat source from the background environment, the target temperature difference threshold is set as follows:

[0128] in, This is the threshold adjustment coefficient, which can be set according to changes in temperature difference in the field environment; S202: Extract pixels whose temperature is higher than the adaptive temperature difference threshold and whose temperature difference with the background is greater than a preset temperature difference threshold as candidate target pixels. Perform connected component analysis on the candidate target pixels and select the connected component with an area within a preset range and closest to the coarse positioning coordinate projection position as the final target region. Specifically, when the temperature of a certain pixel meets the threshold... And its temperature difference with the background temperature is not less than the preset temperature difference threshold. When this happens, the pixel is recorded as a candidate target pixel. Connectivity analysis is then performed on the candidate target pixels to obtain the candidate target region. The selection criteria are:

[0129] in, For the first The area of ​​each connected region. and These are the minimum and maximum area thresholds preset according to the target type. If multiple candidate target regions exist, the connected region with the largest average temperature difference, area that meets the target size range, and closest to the coarse positioning projection position is selected as the final target region. .

[0130] S203: Perform temperature-weighted centroid calculation on the pixels within the final target area to obtain the target center pixel coordinates. Compared to directly taking the geometric center, this method can reduce the impact of edge noise and local occlusion on pixel center extraction. Specifically, in determining the final target area... Then, the temperature difference-weighted pixel center of the target heat source in this area is calculated to obtain the pixel coordinates of the target in the infrared image. :

[0131]

[0132] S204: Based on the infrared lens parameters, the target center pixel coordinates are converted into a unit line-of-sight vector in the camera coordinate system. Combined with the UAV attitude angle and gimbal attitude angle, the unit line-of-sight vector is converted into the local navigation coordinate system. Based on the UAV's current coordinates, flight altitude, and target ground elevation, the target local coordinates are calculated through the intersection of the ray and the ground. The target local coordinates are then converted into high-precision geographic coordinates.

[0133] Specifically, based on the intrinsic parameters of the infrared lens (focal length) , and principal point coordinates , ), converting pixel coordinates to normalized imaging coordinates in the camera coordinate system:

[0134] in, , These are the equivalent focal lengths of the camera in the horizontal and vertical pixel directions, respectively. , These are the coordinates of the principal point of the image.

[0135] The unit line-of-sight vector of the target direction in the camera coordinate system is obtained from the normalized imaging coordinates. :

[0136] By combining the UAV's attitude angles (current roll, pitch, and yaw), gimbal pitch and yaw angles, a rotation matrix is ​​used to transform the line-of-sight vector in the camera coordinate system into the line-of-sight vector in the local navigation coordinate system. :

[0137] in, This is the rotation matrix from the unmanned aerial vehicle system to the local navigation coordinate system. Let be the rotation matrix from the gimbal coordinate system to the UAV coordinate system. This is the installation calibration matrix from the camera coordinate system to the gimbal coordinate system; , , These are the drone's roll angle, pitch angle, and yaw angle, respectively. , These are the gimbal pitch angle and yaw angle, respectively.

[0138] Convert the drone's current BeiDou coordinates to local East-North-Sky coordinates. And determine the ground elevation of the target location based on the flight altitude or terrain elevation. The local coordinates of the target are calculated by the intersection of the ray and the ground plane. Let the position of the UAV after local projection transformation be the real-time BeiDou coordinates. ,in , and Let the coordinates represent the eastward, northward, and elevation coordinates, respectively; and let the ground elevation of the target location be . When a digital elevation model is unavailable, the elevation can be approximated by the drone's flight altitude or ranging altitude. .like Then the parameters of the intersection point between the target point and the ground are:

[0139] The position of the target point in the local navigation coordinate system is:

[0140] Right now:

[0141] Finally, the target position in the local navigation coordinate system is converted to latitude and longitude coordinates. The target's local coordinates are then converted to latitude and longitude coordinates. Convert to BeiDou latitude and longitude coordinates This is then output as the infrared precise positioning result. Let the current BeiDou latitude and longitude of the UAV be... The target's eastward and northward offsets relative to the UAV are respectively , Then the target latitude and longitude can be approximately expressed as:

[0142]

[0143] in, The radius of curvature of the meridian at the current latitude. This represents the radius of curvature of the tropic curve at the current latitude. Through the above steps, the target pixel coordinates in the infrared image can be converted into the target's precise geographic coordinates. The data is then transmitted back to the user terminal via the drone's communication link. The final positioning result is also transmitted back to the user terminal via the drone's communication link. The system supports fully automated execution of the above steps on the drone side, or receiving real-time data via a mobile app and generating a visual flight trajectory for manual reference and control.

[0144] If the target coordinates change less than a preset threshold in several consecutive frames of infrared images, the infrared positioning is considered stable. If the coordinate jump is too large or the target area is lost, the system reverts to the coarse wireless positioning result and controls the UAV to change its altitude or observation angle to rescan. If infrared recognition fails due to reasons such as target obstruction, ambient temperature close to body temperature, or candidate target area not meeting the area threshold, the system automatically reverts to the coarse coordinates calculated in the first stage and triggers the UAV to adjust its flight path, such as reducing altitude, changing the observation angle, or performing a small-scale circling scan around the coarse positioning point before retrying infrared tracking.

[0145] like Figure 3 As shown, in a specific example, a two-stage localization strategy combining coarse and fine localization is adopted based on the above steps. The entire localization process is executed in a closed-loop iterative manner, specifically including the following steps: Phase 1 (Coarse Positioning): Corresponding Figure 3 The process involves a series of steps, from "straight-line flight sampling" and "constructing a motion compensation model" to "establishing a set of intersection equations and calculating the absolute coordinates." In this stage, the UAV first performs straight-line flight sampling (corresponding to step S2), continuously acquiring its own BeiDou coordinates and wireless ranging values ​​with the target terminal device within the signal coverage area. Then, it constructs a motion compensation model (corresponding to step S3), converting the estimated position of the target terminal device at each sampling time to a unified reference time based on the target's motion state and calculating the compensated equivalent ranging value, thereby establishing a set of spatial intersection equations. By introducing an error weighting matrix for solution, the coarse absolute coordinates of the target are obtained, completing the coarse positioning.

[0146] Figure 3After "establishing the intersection equation system and calculating the absolute coordinates", the process enters the judgment box "whether the infrared verification exceeds the set threshold". This judgment box corresponds to the distance threshold judgment in step S4, which is used to measure whether the spatial distance between the UAV and the coarse positioning coordinates meets the start condition for close-range fine positioning. If the judgment result is "yes" (i.e., exceeding the set threshold, the distance is still far), the process returns to the "straight flight sampling" node along the loop arrow, corresponding to "control the UAV to fly towards the coarse positioning coordinates and repeat S2" in step S4. The UAV continues to perform straight flight and sampling, iteratively optimizes the coarse positioning result and gradually approaches the target; if the judgment result is "no" (i.e., not exceeding the set threshold, the distance meets the requirements), the process enters the precise positioning stage along the "no" branch.

[0147] Phase Two (Precise Positioning): Corresponding Figure 3 The positioning process following the "No" branch. At this point, the drone automatically flies to the approximate coordinates and activates the infrared thermal imaging module to scan the ground. By identifying the temperature difference characteristics between the target (such as livestock or people) and the background environment, the pixel coordinates of the target in the infrared image are extracted. Combining the drone's real-time BeiDou coordinates, gimbal pitch angle, gimbal yaw angle, flight altitude, and lens parameters such as lens focal length and principal point coordinates, the pixel coordinates are converted into precise geographic coordinates through geometric calculations, which serve as the final positioning result.

[0148] After accurate positioning is completed, the process enters the "data back to user terminal" node, where the final positioning result obtained by the solution is transmitted back to the user terminal via the UAV communication link, and then enters the "positioning end" node, completing a single positioning task.

[0149] Example 2 This embodiment is basically the same as Embodiment 1, except that, in the field environment, the BeiDou signal of the target terminal may be affected by mountains, forests, and slopes, resulting in short-term drift or jumps. Relying solely on BeiDou coordinates, wireless ranging, or infrared images may lead to unstable positioning results. Therefore, this embodiment does not use the BeiDou coordinates of the target terminal or the high-precision geographic coordinates of Embodiment 1 as the sole or primary positioning basis. Instead, it uses them as one of the multi-source fusion observations, incorporating them into the fusion when BeiDou signals are available to further improve positioning robustness.

[0150] This embodiment unifies the target terminal's BeiDou coordinates, wireless direction finding / ranging data, and infrared image calculation coordinates into a multi-source observation for the same target location, and performs joint calculation through extended Kalman filtering or weighted least squares.

[0151] Specifically, in step S4 of this embodiment, after obtaining high-precision geographic coordinates by identifying the characteristics of the target heat source, the following steps are also included: The high-precision geographic coordinates and the pseudo-coordinates calculated from wireless ranging values ​​are used as multi-source observations for the same target terminal device location. These are unified into a single target state model, and jointly solved using extended Kalman filtering or weighted least squares to obtain the fused target geographic coordinates, thus yielding the final positioning result. The specific process is as follows: Constructing the fused state vector, in this embodiment, the result obtained by the multi-source fusion strategy is the fused geographic coordinates and motion state estimate of the target at the current moment, denoted as:

[0152] in, For the target's fused geographic coordinates at the current moment, The target's current velocity is used as the coordinates. The fused target coordinates serve two purposes: firstly, as the update value of the coarse positioning result in step S2 of the next cycle, used for motion compensation and subsequent tracking path optimization; secondly, as the guiding coordinates for infrared precise positioning in step S3, enabling the UAV to activate the infrared module after flying to the target's approximate area. When infrared recognition is successful, the infrared image calculated coordinates participate in the final fusion as high-precision observations. When infrared recognition fails, the fused coarse positioning coordinates are output as backup positioning results, and the process proceeds to step S4 for result feedback or to trigger the UAV to readjust its flight path.

[0153] In practical implementation, wireless direction finding / ranging data is first converted into target pseudo-coordinates. Let the first... The drone's own BeiDou coordinates are The wireless ranging value is The unit direction vector obtained by wireless direction finding is Then the wireless pseudo-coordinates are:

[0154] in, This represents the target position observation value obtained by wireless direction finding / range finding. The infrared image undergoes temperature difference identification, pixel coordinate extraction, and geometric projection to obtain the infrared calculated coordinates. The target terminal's BeiDou module outputs BeiDou observation coordinates when signal is available. Therefore, a joint observation vector is constructed:

[0155] For the above observations, the observation model can be written as:

[0156] in, For the observation matrix, To account for observation noise. Since BeiDou coordinates, wireless pseudo-coordinates, and infrared calculated coordinates all directly observe the target position, their individual observation matrices can be taken as:

[0157] The observation matrix corresponding to multiple observation sources can be obtained from Obtained by stacking rows.

[0158] In the extended Kalman filter fusion process, the current state is first predicted based on the target state at the previous time step:

[0159] in, Here is the state transition matrix. Predict the state at the current moment.

[0160] Then, the multi-source observation residuals were calculated:

[0161] To suppress anomalous jumps, Mahalanobis distance is introduced to detect anomalies in the observation residuals:

[0162] in, Let be the residual covariance matrix. When the value exceeds a preset threshold, it is determined that there is an abnormal jump in the current observation. In this case, the weight of the observation source is reduced or its observation noise variance is increased. If the value is less than or equal to the preset threshold, the observation source will be retained to participate in the fusion normally.

[0163] Fusion update via Kalman gain:

[0164] in, For Kalman gain, This represents the estimated target state after fusion. The final output of the fused target's geographic coordinates is:

[0165] This fused coordinate system is used to replace the output of a single sensor, thereby reducing positioning jumps caused by BeiDou drift, wireless multipath errors, and infrared false detections.

[0166] As an alternative implementation, weighted least squares can be used for joint solution. Let BeiDou observation, wireless pseudo-coordinate observation, and infrared observation be respectively... , and Their corresponding weights are respectively , and The target location can then be obtained through the following optimization problem:

[0167] in, The coordinates of the target are to be determined. The weights can be dynamically adjusted based on the reliability of each sensor: when the number of BeiDou satellites is insufficient, the wireless RSSI or signal-to-noise ratio is low, or the confidence level of the infrared target is low, the corresponding weight is reduced; when a certain observation source is stable and reliable, its corresponding weight is increased. This allows for adaptive selection of more reliable observation data under different environmental conditions.

[0168] The multi-source fusion strategy in this embodiment does not simply display multiple types of data side-by-side, but rather unifies BeiDou coordinates, wireless direction finding / ranging results, and infrared image geometric calculation results into a single target state model for joint estimation. The output directly affects motion compensation and coarse positioning, infrared fine positioning guidance, final result transmission, and UAV flight path adjustment in abnormal situations. Simultaneously, the infrared module only activates after the fused coordinates indicate the UAV has entered the near-range threshold range and shuts down after positioning is complete, thereby reducing overall power consumption while maintaining positioning accuracy.

[0169] Example 3 This embodiment is basically the same as Embodiment 2, except that this embodiment also includes a process for automatically adjusting the flight path of the UAV using an inertial measurement unit. This process is executed in parallel with steps S1-S4 and is used to provide real-time path adjustment and anomaly recovery during continuous UAV tracking. The target terminal device continuously collects and predicts inertial measurement data. The UAV performs anomaly monitoring based on the ranging residual in each control cycle and adjusts the flight path accordingly, including the following steps: Step A1: The target terminal device periodically collects accelerometer and gyroscope data, and corrects the accelerometer zero bias and gyroscope zero bias. For details of the implementation of this step, please refer to the corresponding content of step S2 in Embodiment 1. Step A2: The target terminal device updates its own attitude according to the gyroscope output and converts the accelerometer data to the navigation coordinate system to obtain the target's motion acceleration in the navigation coordinate system. For details of the implementation of this step, please refer to the corresponding content of step S2 in Embodiment 1. Step A3: The target terminal device or UAV terminal recursively derives the predicted target position based on the previous reliable target position, the previous reliable target velocity, and the acceleration in the navigation coordinate system. and prediction speed For details on the specific implementation of this step, please refer to the corresponding content of step S2 in Embodiment 1; Step A4: Acquire wireless ranging values ​​on the drone end Then, calculate the wireless ranging value. With inertial prediction position Distance residuals between And based on the ranging residual Determine whether the current ranging is abnormal. For details on the implementation of this step, please refer to the corresponding content of step S3 in Example 1. Step A5: When the ranging residual If the preset threshold is not exceeded, the UAV uses the fused target location (i.e., fused geographic coordinates) or the inertial predicted target location as the pointing center to generate the approach heading and the next target waypoint. For details on the specific implementation of this step, please refer to the corresponding content of step S3 in Embodiment 1; Step A6: When the ranging residual exceeds the preset threshold, or when a stable ranging connection cannot be established for multiple consecutive sampling periods, the UAV generates multiple candidate remeasurement waypoints centered on the inertial prediction target position. For details on the specific implementation of this step, please refer to the corresponding content of step S3 in Embodiment 1; Step A7: The UAV compares the geometric accuracy factor and signal quality index of each candidate waypoint for retesting, and selects the candidate waypoint with the lowest overall cost. As the next destination And automatically adjust the flight path. For details of the implementation of this step, please refer to the corresponding content of step S3 in Embodiment 1. Step A8: After the UAV reaches the new retest waypoint, it will perform wireless ranging and RSSI direction finding again. If a stable ranging connection is restored, return to the continuous ranging and coarse positioning steps (i.e., return to step S2); if a stable connection is still not restored, expand the retest radius or switch to the spiral extended search path (i.e., return to step S1).

[0170] Through the above steps, the inertial measurement unit is not only used to provide short-term continuous position estimation during wireless ranging anomalies, but also to provide target center prediction basis for the UAV to automatically adjust its flight path. This enables the system to suppress positioning deviation in forest, mountainous, obstructed and multipath environments, and improve the reliability of continuous tracking of moving targets.

[0171] In summary, this invention proposes a method for precise positioning of moving targets in outdoor environments, designing a complete process of "wireless signal discovery—direction finding guidance—segmented approach—infrared precise positioning": The UAV first uses an inward-outward spiral or concentric circle expansion strategy to search for the target's emitted wireless signal; upon receiving the signal, it performs real-time direction finding based on signal strength changes, guiding the UAV to gradually approach the target; within close range, it activates an infrared thermal imaging module for precise positioning, while simultaneously employing a low-power strategy to activate infrared only when necessary. This invention effectively improves the search efficiency and positioning accuracy of moving targets in complex environments, and eliminates the need for multi-UAV collaboration or ground base stations, providing a theoretically feasible new path for low-cost, high-precision positioning of moving targets in complex environments such as mountainous and forested areas.

[0172] Those skilled in the art will understand that embodiments of this application can be provided as methods, systems, or computer program products. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application can take the form of a computer program product embodied on one or more computer-readable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code. This application is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this application. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create a machine for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to operate in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The functions specified in one or more boxes. These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable apparatus for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0173] The above description is merely a preferred embodiment of the present invention. The scope of protection of the present invention is not limited to the above embodiments. All technical solutions falling within the scope of the present invention's concept are within the scope of protection of the present invention. It should be noted that for those skilled in the art, any improvements and modifications made without departing from the principles of the present invention should also be considered within the scope of protection of the present invention.

Claims

1. A method for accurately locating a moving target in a field environment, characterized in that, Includes the following steps: S1: Control the drone to fly along the first path to capture the wireless signal of the target terminal device; after capturing the wireless signal, guide the drone to fly along the second path to approach the target terminal device through radio direction finding; S2: During the flight of the UAV within the signal coverage area of ​​the target terminal device, the UAV's own coordinates and the wireless ranging value with the target terminal device are acquired in real time and added to the sampling dataset. The inertial prediction position transmitted back by the target terminal device is also acquired. S3: Calculate the ranging residual based on the wireless ranging value, the UAV's own coordinates, and the inertial predicted position at the same time; perform ranging anomaly detection based on the ranging residual; if an anomaly exists, generate a retest waypoint centered on the inertial predicted position, control the UAV to fly to the retest waypoint with the lowest overall cost, and return to S2; otherwise, use the previous coarse positioning coordinates of the target terminal device as the estimated position of the target terminal device, estimate the target motion state based on the estimated position of the target terminal device, use the motion compensation model to convert the estimated position of the target terminal device at each sampling time to the reference time based on the target motion state, calculate the compensated equivalent ranging value, establish a spatial intersection equation based on the equivalent ranging value and the corresponding UAV own coordinates, predict the position of the target terminal device at the reference time based on the estimated position of the target terminal device and the target motion state, determine the motion compensation residual based on the predicted position of the target terminal device and the compensated equivalent ranging value, construct an error weighting matrix based on the motion compensation residual and signal quality indicators, solve the spatial intersection equation, and obtain the current coarse positioning coordinates of the target terminal device; S4: Determine whether the distance between the drone and the current coarse positioning coordinates is greater than the distance threshold. If yes, control the drone to fly toward the current coarse positioning coordinates and repeat S2; otherwise, activate the infrared thermal imaging module on the drone to obtain high-precision geographic coordinates by identifying the target heat source features and obtain the final positioning result.

2. The method for precise positioning of a moving target in a field environment according to claim 1, characterized in that, The first path is a spiral or concentric circle expansion path from the inside out. If the approximate direction of the target is known, the first path is a semi-circular expansion path from the inside out. The second path is a straight path or an adaptive path dynamically generated based on changes in signal strength, changes in target direction, and obstacle information.

3. The method for precise positioning of a moving target in a field environment according to claim 1, characterized in that, The inertial prediction position is the target position obtained by the target terminal device through short-time inertial prediction, and its mathematical expression is as follows: in, For the first The inertial predicted position at each sampling time. For the first The inertial prediction velocity at each sampling time. For the first The inertial predicted position at each sampling time. For the first The inertial prediction velocity at each sampling time. The time interval between adjacent sampling times. This represents the inertial predicted acceleration.

4. The method for precise positioning of a moving target in a field environment according to claim 1, characterized in that, When performing ranging anomaly detection based on ranging residuals, the anomaly detection statistic is obtained by dividing the square of the ranging residuals by the variance of the ranging residuals. If the anomaly detection statistic is greater than a preset threshold, an anomaly exists; otherwise, no anomaly exists. The mathematical expression for the anomaly detection statistic is as follows: in, For the distance measurement residual, Let V be the variance of the distance measurement residual. The mathematical expression for the distance measurement residual is as follows: in, For the first The location of the drone's own coordinates at each sampling time. For the first Wireless ranging values ​​at each sampling time, For the first The inertial predicted position at each sampling time.

5. The method for precise positioning of a moving target in a field environment according to claim 1, characterized in that, When selecting the retest waypoint with the minimum overall cost based on the geometric accuracy factor and signal quality index of each candidate waypoint, the mathematical expression for the overall cost is as follows: in, Indicates the first The overall cost of retesting each candidate waypoint According to the first The geometric accuracy factor is calculated based on the spatial geometric distribution of each candidate waypoint relative to existing sampled points. and The first The received signal strength indicator quality factor and signal-to-noise ratio quality factor of the target terminal equipment for each candidate waypoint. and This is the signal quality impact coefficient.

6. The method for precise positioning of a moving target in a field environment according to claim 1, characterized in that, When using a motion compensation model to calculate the compensated equivalent ranging value after converting the estimated position of the target terminal device at each sampling time to a reference time based on the target motion state, the motion compensation model calculates the target terminal device position converted to a unified reference time based on the target motion state at each sampling time. Then, it calculates the compensated equivalent ranging value based on the converted target terminal device position and the UAV's own coordinates at each sampling time, wherein: The mathematical expression for the converted target terminal device location is as follows: If the target's motion is uniform, then we have If the target motion is uniformly accelerated, then we have in, Indicates reference time The converted target terminal device location, Indicates the sampling time The estimated location of the target terminal device. and These represent the estimated values ​​of the target terminal device's velocity and acceleration, respectively. Indicates from the sampling time to reference time The time interval; The mathematical expression for the compensated equivalent distance measurement value is as follows: in, This represents the coordinates of the UAV at the i-th sampling time. This represents the target terminal device's estimated location at the i-th sampling time, after being converted to a unified reference time.

7. The method for precise positioning of a moving target in a field environment according to claim 1, characterized in that, The target motion parameters include the velocity and acceleration of the target terminal device. When estimating the target motion state based on the estimated position of the target terminal device, the estimated position of the target terminal device is used as the observation. The Kalman filter is used to estimate and update the target motion state vector containing position and velocity components. The velocity component is extracted from the updated target motion state vector as the target motion state. When predicting the target terminal device's position at a reference time based on the estimated position and motion state of the target terminal device, the target motion state vector obtained after the most recent Kalman filter update is used as the state prediction input. A state transition matrix is ​​constructed based on the time interval between the most recent estimated time and the reference time corresponding to the target motion state vector. The target motion state is then recursively derived from the most recent estimated time to the reference time through the Kalman filter state prediction equation to obtain the target motion state vector at the reference time. The position component is then extracted from this vector as the predicted position of the target terminal device at the reference time.

8. The method for precise positioning of a moving target in a field environment according to claim 1, characterized in that, Based on the predicted position of the target terminal device and the compensated equivalent ranging value, the motion compensation residual is determined. An error weighting matrix is ​​constructed based on the motion compensation residual and signal quality indicators. Solving the spatial intersection equation includes the following steps: The motion compensation residual is determined based on the predicted position of the target terminal device at the reference time, the compensated equivalent ranging value, and the UAV's own coordinates at each sampling time. The mathematical expression is as follows: in, This represents the coordinates of the UAV at the i-th sampling time. This represents the equivalent ranging value at the i-th sampling time. This indicates the predicted location of the target terminal device at the reference time. Calculate the current received signal strength and signal-to-noise ratio (SNR) of the target terminal device's wireless signal, and then normalize the current received signal strength and SNR to calculate the received signal strength quality factor. With signal-to-noise ratio quality factor ; The equivalent measurement variance of the distance measurements is constructed, and its mathematical expression is as follows: in, This represents the equivalent measurement variance at the i-th sampling time. The baseline distance measurement variance, and These are the weighting coefficients for received signal strength and signal-to-noise ratio, respectively. The influence coefficient of motion compensation residuals. To prevent extremely small positive numbers with a denominator of zero; The ranging weights are calculated based on the equivalent measurement variance, and the error weighting matrix is ​​constructed from the ranging weights of each sampling point. The mathematical expression for the ranging weights is as follows: in, This represents the ranging weight at the i-th sampling time. Based on the error weighting matrix, a weighted nonlinear least squares objective function is established to solve the spatial intersection equation. The mathematical expression of the weighted nonlinear least squares objective function is as follows: in, These are the coarse positioning coordinates of the target terminal device to be solved. It is the residual of the spatial intersection equation. This represents the coordinates of the UAV at the i-th sampling time. This represents the equivalent distance measurement value at the i-th sampling time.

9. The method for precise positioning of a moving target in a field environment according to claim 1, characterized in that, When obtaining high-precision geographic coordinates by identifying target heat source features, the following steps are included: Candidate search areas are determined in the infrared images acquired by the infrared thermal imaging module based on the coarse positioning coordinates, and the average background temperature within the candidate search areas is calculated. and background temperature standard deviation The target temperature difference threshold is calculated using the following mathematical expression: in, This is the threshold adjustment coefficient, which can be set according to changes in temperature difference in the field environment; Pixels with temperatures higher than the adaptive temperature difference threshold and a temperature difference greater than the preset temperature difference threshold are extracted as candidate target pixels. Connectivity analysis is performed on the candidate target pixels, and the connected regions with areas within a preset range and closest to the coarse positioning coordinate projection position are selected as the final target regions. Temperature difference weighted centroid calculation is performed on the pixels within the final target area to obtain the coordinates of the target center pixel; Based on the infrared lens parameters, the target center pixel coordinates are converted into a unit line-of-sight vector in the camera coordinate system. Combined with the UAV attitude angle and gimbal attitude angle, the unit line-of-sight vector is converted into the local navigation coordinate system. Based on the UAV's current coordinates, flight altitude, and target ground elevation, the target's local coordinates are calculated through the intersection of the ray and the ground. The target's local coordinates are then converted into high-precision geographic coordinates.

10. The method for precise positioning of a moving target in a field environment according to claim 1, characterized in that, After obtaining high-precision geographic coordinates by identifying the characteristics of the target heat source, the process also includes: The high-precision geographic coordinates and the pseudo coordinates calculated from the wireless ranging values ​​are used as multi-source observations for the same target terminal device location. They are unified into the same target state model and jointly solved by extended Kalman filtering or weighted least squares to obtain the fused target geographic coordinates and the final positioning result.

Images