A method and system for collaborative positioning of unmanned aerial vehicles and ground sensors

Abstract

This invention relates to the field of unmanned aerial vehicles (UAVs) and discloses a method and system for collaborative positioning between a UAV and ground sensors. The method includes: the UAV measuring the distance to a ground base station via a UWB module and transmitting the ranging data to the ground base station to obtain the initial position of the UAV relative to the base station; the UAV performing pre-correction on visual pose estimation and outputting a visual position update result Ps; the UAV detecting the environmental state based on the lidar; when the environment is detected to be in a first state, matching the geomagnetic data detected by the UAV's geomagnetic sensor array with a pre-stored geomagnetic spectrum; obtaining a geomagnetic matching correction result based on the matching degree; and performing data fusion based on a hierarchical Kalman filter to obtain the UAV's position coordinates. This method has the following advantages: it effectively suppresses the errors and limitations of a single sensor, thereby obtaining high-precision and high-reliability UAV position coordinates.

Description

Technical Field

[0001] This invention relates to the field of unmanned aerial vehicles (UAVs), and more particularly to a method and system for collaborative positioning of UAVs and ground sensors. Background Technology

[0002] With the widespread application of drones in surveying and inspection, logistics delivery, disaster relief, and other fields, high-precision and high-reliability positioning technology has become a core element to ensure their safe and stable operation. Traditional drone positioning methods mostly rely on a single sensor, such as the Global Navigation Satellite System (GNSS). However, in environments with severe satellite signal obstruction or interference, such as urban canyons or indoor spaces, positioning accuracy drops significantly or even fails. While inertial navigation systems (INS) do not rely on external signals, they suffer from the problem of error accumulation over time. Therefore, a single sensor is insufficient to meet the positioning needs of drones in complex environments.

[0003] Solving the above-mentioned technical problems is a technical challenge that needs to be overcome by those skilled in the art. Summary of the Invention

[0004] This invention provides a method and system for collaborative positioning of unmanned aerial vehicles (UAVs) and ground sensors, which at least partially solves the above-mentioned technical problems.

[0005] In a first aspect, to solve the above-mentioned technical problems, the present invention provides a method for cooperative positioning of unmanned aerial vehicles (UAVs) and ground sensors, comprising:

[0006] The UAV measures the distance between itself and the ground base station via a UWB module and transmits the ranging data to the ground base station. The ground base station processes the received ranging data based on pre-stored environmental map data and historical positioning data to obtain the initial position of the UAV relative to the base station and transmits the initial position back to the UAV.

[0007] The UAV obtains an environmental state assessment result based on the image information collected by the camera and the obstacle detection results collected by the lidar. Based on the environmental state assessment result, key frames are generated to construct a local map. The visual pose estimation is pre-corrected using inertial measurement unit data, and the visual position update result Ps is output.

[0008] The UAV detects the environmental state based on the lidar; when the environment is detected to be in a first state, the UAV matches the geomagnetic data detected by the UAV's geomagnetic sensor array with a pre-stored geomagnetic map; a geomagnetic matching correction result is obtained based on the degree of matching; when the environment is detected to be in a second state, geomagnetic matching is paused.

[0009] Data fusion is performed based on a hierarchical Kalman filter; wherein, the first layer filter filters the initial position and the visual position update result Ps respectively; the second layer filter fuses the output of the first layer filter with the geomagnetic matching correction result obtained in the geomagnetic matching correction step to obtain the UAV position coordinates.

[0010] In one optional implementation, the environmental state assessment includes an obstacle movement index calculation step, a visual feature stability calculation step, and an environmental state determination step.

[0011] The steps for calculating the obstacle movement index include:

[0012] Clustering is performed on two frames of point cloud data to identify obstacles in the environment;

[0013] Based on location distance and shape similarity, an association matching method is used to match obstacles between two frames;

[0014] Calculate the position change vector for each successfully associated obstacle;

[0015] The mean square error of all position change vectors is calculated to obtain the obstacle movement index.

[0016] The steps for calculating visual feature stability include:

[0017] Extract feature points from adjacent frames and calculate descriptors;

[0018] Feature point matching is performed to obtain initial matching point pairs;

[0019] Remove false matching pairs and calculate the number of valid matching pairs;

[0020] The stability of visual features is obtained by calculating the statistical average of the feature point matching success rate over at least 3 consecutive frames.

[0021] The environmental condition determination steps include:

[0022] The obstacle movement index is compared with a first preset threshold T1, and the visual feature stability is compared with a second preset threshold T2. When the obstacle movement index is less than T1 and the visual feature stability is greater than T2, the environment is determined to be in the first state. When the obstacle movement index is not less than T1 or the visual feature stability is less than T2, the environment is determined to be in the second state.

[0023] In one optional implementation, geomagnetic data detected by the UAV's geomagnetic sensor array is matched with a pre-stored geomagnetic map; a geomagnetic matching correction result is obtained based on the degree of matching, including:

[0024] Convert real-time geomagnetic data detected by the geomagnetic sensor array into feature vectors;

[0025] Calculate the cosine similarity between the feature vector and the feature vector of the candidate location in the pre-stored geomagnetic map, and use it as the matching score S; when the matching score S exceeds the third preset threshold T3, the match is determined to be successful.

[0026] For each successfully matched feature point, calculate the difference between its coordinates in the geomagnetic map and the predicted coordinates of the corresponding point under the current UAV position estimation; determine the weight of each difference based on the matching degree score, with higher matching degree resulting in greater weight;

[0027] Sum all the weighted differences to obtain the position adjustment vector;

[0028] The geomagnetic matching correction results are calculated based on the predicted coordinates and position adjustment vector of the corresponding point under the current UAV position estimation.

[0029] In one optional implementation, the first layer filter filters the initial position and the visual position update result Ps respectively, including:

[0030] The predicted position of the drone at the current moment is calculated based on the position, velocity, and acceleration data of the drone at the previous moment.

[0031] Compare the predicted location with the UWB measured location;

[0032] The predicted position is corrected based on the magnitude of the difference to obtain the filtered UWB position; where the correction magnitude is reduced when obstacles in the environment move frequently.

[0033] Based on the angular velocity and acceleration data collected in real time by the inertial measurement unit on the UAV, the visual pose prediction value at the current moment is calculated through integral calculation;

[0034] Compare the difference between the predicted pose and the visual position update result Ps;

[0035] The predicted pose is corrected based on the difference magnitude to obtain the filtered visual position; whereby, when the success rate of image feature point matching is low, the influence of visual data on the correction is reduced.

[0036] In one optional implementation, the second-layer filter fuses the output of the first-layer filter with the geomagnetic matching correction result obtained in the geomagnetic matching correction step to obtain the UAV's position coordinates, including:

[0037] Check the validity of the UWB position, visual position, and geomagnetic matching correction results of the first layer filter output: if the filtering error of the UWB position exceeds the fourth preset threshold, the UWB data is determined to be unreliable.

[0038] If the success rate of feature point matching at the visual location is lower than the fifth preset threshold, the visual data is deemed unreliable; if the environmental state is in the second state or the geomagnetic matching score does not exceed the third preset threshold, the geomagnetic data is deemed unreliable.

[0039] Based on the environmental condition assessment results, weights are assigned to each sensor: when the environment is in the first state, the weight of geomagnetic data is set to the first weight value, and the weights of UWB and visual data are set to the second and third weight values, respectively; when the environment is in the second state, the weight of geomagnetic data is reduced to zero, and the weights of UWB and visual data are adjusted to the fourth and fifth weight values, respectively; among them, the fourth weight value is greater than the second weight value, and the fifth weight value is greater than the third weight value.

[0040] For UWB data, the weights are adjusted inversely based on the magnitude of the filtering error; for visual data, the weights are adjusted directly based on the feature point matching success rate; for geomagnetic data, the weights are adjusted directly based on the matching score S.

[0041] The final position coordinates are calculated based on the weights of each sensor. If all sensor data are valid, the final position coordinates are calculated as follows: Final position = (UWB weight × UWB filter position) + (Visual weight × Visual filter position) + (Geomagnetic weight × Geomagnetic matching correction result). If the geomagnetic data is invalid, the final position coordinates are calculated as follows: Final position = (UWB weight × UWB filter position) + (Visual weight × Visual filter position).

[0042] In one optional implementation, the pre-stored geomagnetic spectrum undergoes gridded preprocessing, specifically including the following steps:

[0043] The geomagnetic map is divided into multiple non-overlapping sub-regions according to a preset grid size. Each sub-region includes: the coordinates of the region boundary, a statistical description of the geomagnetic feature vectors within the region, and the topological connection relationship between adjacent regions.

[0044] Construct a geomagnetic feature index for each sub-region;

[0045] Based on environmental characteristics, matching priorities are assigned to each sub-region:

[0046] When geomagnetic matching begins, a set of candidate sub-regions within a preset range around the current location is calculated based on the estimated current location of the UAV. The candidate regions are sorted according to their matching priority, and high-priority regions are processed first. For each candidate region, the similarity between its feature descriptor and the real-time geomagnetic data is calculated, and regions with similarity exceeding the seventh preset threshold are selected as high-probability matching regions.

[0047] Matching is performed only on feature points within high-probability matching areas: the cosine similarity between real-time geomagnetic data and feature points within the area is calculated, and a matching score is generated; if the highest matching score exceeds the third preset threshold, the match is considered successful, and the position correction amount for the corresponding area is output.

[0048] In one alternative implementation, the collected point cloud data further includes, before being used for environmental condition assessment:

[0049] Based on the clock on the drone, record the timestamps of the data collected by each sensor;

[0050] By utilizing the attitude and position information provided by the UAV's inertial measurement unit, the point cloud data of the lidar and the image data of the camera are uniformly converted to the same world coordinate system;

[0051] For data that is out of time, linear interpolation is used to synchronize it to the reference time point, so that the image information and point cloud data are aligned in the spatiotemporal dimension.

[0052] Secondly, the present invention provides a UAV-ground sensor cooperative positioning system, based on any of the above-mentioned positioning methods, characterized in that it includes:

[0053] The drone is equipped with a camera, a UWB module, a lidar, and a geomagnetic sensor. The camera collects image information of the surrounding environment; the UWB module is used to measure the distance information of the ground base station; the geomagnetic sensor is used to measure geomagnetic data; and the lidar is used to measure environmental obstacle information.

[0054] Ground base station; the ground base station communicates with the UAV.

[0055] Compared with the prior art, the present invention has at least the following beneficial effects: it effectively suppresses the errors and limitations of a single sensor, thereby obtaining high-precision and high-reliability UAV position coordinates. Attached Figure Description

[0056] Figure 1 This is a schematic diagram of a method for collaborative positioning of a UAV and ground sensors provided in the first embodiment of the present invention. Detailed Implementation

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

[0058] Reference Figure 1The first embodiment of the present invention provides a method for cooperative positioning of unmanned aerial vehicles (UAVs) and ground sensors, comprising the following steps:

[0059] S101, the UAV measures the distance to the ground base station via the UWB module and transmits the ranging data to the ground base station. The ground base station processes the received ranging data based on pre-stored environmental map data and historical positioning data to obtain the UAV's initial position relative to the base station, and then transmits the initial position back to the UAV. Specifically, the UWB module (Ultra-Wideband) transmits data by sending and receiving extremely narrow pulses with sub-nanosecond or microsecond durations, achieving high-precision distance measurement. In this scheme, it is used to measure the distance between the UAV and the ground base station. The ground base station is a communication device fixed to the ground. It receives the ranging data transmitted by the UAV and calculates the UAV's initial position relative to the base station based on pre-stored environmental map data and historical positioning data. The environmental map data is pre-collected and stored digitized data about the geographic information of the target area, including terrain, buildings, roads, etc., used to assist the ground base station in processing the ranging data to determine the UAV's initial position. Historical positioning data records the UAV's position information during past operations, which, combined with the environmental map data, helps the ground base station more accurately calculate the UAV's current initial position.

[0060] S102, the UAV obtains an environmental state assessment result based on the image information collected by the camera and the obstacle detection results collected by the lidar. Based on the environmental state assessment result, keyframes are generated to construct a local map. The visual pose estimation is pre-corrected using inertial measurement unit (IMU) data, and the visual position update result Ps is output. Specifically, the camera is used to collect image information of the surrounding environment and provides the UAV with visual environmental perception data through image analysis. The lidar detects the environment by emitting laser beams and receiving reflected light, generating high-precision point cloud data for detecting obstacle information in the environment. The inertial measurement unit (IMU), composed of sensors such as accelerometers and gyroscopes, measures the UAV's acceleration, angular velocity, and other motion parameters in real time, and is used for pre-correction of the visual pose estimation.

[0061] S103, the UAV detects the environmental state based on the lidar; when the environment is detected to be in a first state, the UAV matches the geomagnetic data detected by its geomagnetic sensor array with a pre-stored geomagnetic map; a geomagnetic matching correction result is obtained based on the degree of matching; when the environment is detected to be in a second state, geomagnetic matching is paused. Specifically, the geomagnetic sensor array consists of multiple geomagnetic sensors used to detect geomagnetic field data in the environment. By matching it with a pre-stored geomagnetic map, the UAV is assisted in position correction. The pre-stored geomagnetic map is a digital map formed by measuring and recording the geomagnetic field distribution in a specific area. It contains geomagnetic feature information of different locations and is used to match the geomagnetic data detected by the UAV in real time to achieve positioning correction.

[0062] S104, data fusion is performed based on a hierarchical Kalman filter; wherein, the first layer filter filters the initial position and the visual position update result Ps respectively; the second layer filter fuses the output result of the first layer filter with the geomagnetic matching correction result obtained in the geomagnetic matching correction step to obtain the UAV position coordinates.

[0063] Specifically, the UWB module works in conjunction with a ground base station to determine the initial position using environmental maps and historical positioning data. The camera and LiDAR work together to assess the environmental state, enabling real-time perception of environmental changes and generating keyframes to construct a local map. Based on this, IMU data is used to pre-correct visual pose estimation, improving the accuracy of visual positioning. The LiDAR detects the environmental state and controls the activation and deactivation of geomagnetic matching, ensuring that geomagnetic matching only occurs in suitable environments. This avoids potential errors in geomagnetic matching under complex interference conditions, improving its effectiveness. A hierarchical Kalman filter processes the data in two layers. The first layer filters the initial position and visual position update results to remove noise. The second layer incorporates the geomagnetic matching correction results. Taking into account the characteristics and reliability of multi-sensor data, the filter organically integrates data from different sources through dynamic weight adjustments, effectively suppressing the errors and limitations of single sensors. This results in high-precision, high-reliability UAV position coordinates, achieving stable and accurate positioning in complex environments and meeting the application needs of UAVs in various scenarios.

[0064] In one embodiment, environmental condition assessment includes an obstacle movement index calculation step, a visual feature stability calculation step, and an environmental condition determination step.

[0065] The steps for calculating the obstacle movement index include:

[0066] Clustering is performed on two frames of point cloud data to identify obstacles in the environment;

[0067] Based on location distance and shape similarity, an association matching method is used to match obstacles between two frames;

[0068] Calculate the position change vector for each successfully associated obstacle;

[0069] The mean square error of all position change vectors is calculated to obtain the obstacle movement index.

[0070] The steps for calculating visual feature stability include:

[0071] Extract feature points from adjacent frames and calculate descriptors;

[0072] Feature point matching is performed to obtain initial matching point pairs;

[0073] Remove false matching pairs and calculate the number of valid matching pairs;

[0074] The stability of visual features is obtained by calculating the statistical average of the feature point matching success rate over at least 3 consecutive frames.

[0075] The environmental condition determination steps include:

[0076] The obstacle movement index is compared with a first preset threshold T1, and the visual feature stability is compared with a second preset threshold T2. When the obstacle movement index is less than T1 and the visual feature stability is greater than T2, the environment is determined to be in the first state. When the obstacle movement index is not less than T1 or the visual feature stability is less than T2, the environment is determined to be in the second state.

[0077] Specifically, point cloud data is environmental data collected by devices such as LiDAR and presented as a set of three-dimensional coordinate points. Each point contains information such as its location (X, Y, Z coordinates) and reflection intensity, which is used to describe the spatial distribution and shape characteristics of objects in the environment.

[0078] Clustering is a data processing algorithm that divides points in point cloud data that are spatially close and have similar attributes into the same set (cluster), thereby separating different objects or regions from a large amount of point cloud data and achieving preliminary identification of obstacles in the environment.

[0079] Positional distance and shape similarity are two key metrics used to measure whether obstacles in two frames of point cloud data belong to the same object. Positional distance refers to the difference in the spatial position of an obstacle at different times; shape similarity helps to determine whether obstacles in two frames correspond by calculating the similarity of the geometric features of the obstacle (such as outline and volume).

[0080] Association matching is based on location distance and shape similarity, which pairs the obstacles identified in two frames of point cloud data to determine the state changes of the same obstacle at different times.

[0081] The position change vector is a vector used to describe the direction and distance of the obstacle's position movement between two adjacent frames. It is obtained by calculating the coordinate difference of the obstacle in three-dimensional space after association matching, reflecting the obstacle's movement trend.

[0082] Feature points can be understood as pixels in an image that have unique attributes (such as corner points and edge points). These points have high stability in image transformations (such as rotation and scaling) and are key information carriers for image matching and analysis.

[0083] A descriptor generates a set of feature vectors for each feature point, which are used to describe the local features (such as texture, gradient direction, etc.) of the image region around the feature point. Feature point matching can be achieved by comparing the similarity between descriptors.

[0084] Incorrectly matched point pairs occur during feature point matching when, due to factors such as image noise and lighting changes, feature points that do not belong to the same object are mistakenly identified as matching point pairs. These pairs need to be removed by algorithms to ensure the accuracy of the matching results.

[0085] The number of valid matching point pairs can be understood as the number of matching point pairs that truly reflect the same object or region in the image after removing false matching point pairs. It is an important indicator for measuring the quality of image matching.

[0086] Feature point matching success rate can be understood as the ratio of the number of effective matching point pairs to the number of initial matching point pairs, reflecting the reliability of image feature matching. Calculating the statistical average of the feature point matching success rates over at least three consecutive frames provides a more stable assessment of the stability of visual features.

[0087] The first preset threshold T1 is a pre-set critical value used to measure the obstacle movement index. When the calculated obstacle movement index is less than this threshold, it indicates that the obstacle in the environment is moving slowly or is relatively stationary; otherwise, it indicates that the environment is dynamically changing.

[0088] The second preset threshold T2 is a pre-set critical value used to measure the stability of visual features. When the stability of visual features is greater than this threshold, it indicates that the image feature matching effect is good and the visual positioning is reliable; otherwise, it indicates that the reliability of visual data is reduced.

[0089] In the obstacle movement index calculation step, point cloud data collected by LiDAR is used to identify obstacles through clustering. Two frames of obstacles are matched based on positional distance and shape similarity. The position change vector is calculated, and the mean square error is determined to obtain a quantified obstacle movement index, reflecting the degree of obstacle movement in the environment. In the visual feature stability calculation step, feature points are extracted from the images collected by the camera, and descriptors are calculated. After feature point matching, mismatched point pairs are eliminated. The average feature point matching success rate of at least three consecutive frames is calculated based on the number of valid matching point pairs to evaluate the stability of the visual features. In the environment state determination step, the obstacle movement index is compared with a first preset threshold T1, and the visual feature stability is compared with a second preset threshold T2. If the obstacle movement index is less than T1 and the visual feature stability is greater than T2, the environment is determined to be in the first state, i.e., a relatively stable state. If the obstacle movement index is not less than T1 or the visual feature stability is less than T2, the environment is determined to be in the second state, i.e., a state with greater dynamic changes. When the environment is in state one, it indicates a relatively stable environment, and the UAV can fully utilize high-precision positioning methods such as geomagnetic matching to improve positioning accuracy. When in state two, the system suspends geomagnetic matching to avoid interference and instead relies on other more reliable sensor data, such as visual and UWB data. This effectively reduces positioning errors and the risk of failure caused by environmental changes.

[0090] In one implementation, geomagnetic data detected by the UAV's geomagnetic sensor array is matched with a pre-stored geomagnetic map; a geomagnetic matching correction result is obtained based on the degree of matching, including:

[0091] Convert real-time geomagnetic data detected by the geomagnetic sensor array into feature vectors;

[0092] Calculate the cosine similarity between the feature vector and the feature vector of the candidate location in the pre-stored geomagnetic map, and use it as the matching score S; when the matching score S exceeds the third preset threshold T3, the match is determined to be successful.

[0093] For each successfully matched feature point, calculate the difference between its coordinates in the geomagnetic map and the predicted coordinates of the corresponding point under the current UAV position estimation; determine the weight of each difference based on the matching degree score, with higher matching degree resulting in greater weight;

[0094] Sum all the weighted differences to obtain the position adjustment vector;

[0095] The geomagnetic matching correction results are calculated based on the predicted coordinates and position adjustment vector of the corresponding point under the current UAV position estimation.

[0096] Specifically, the geomagnetic data records various parameters of the geomagnetic field at the current location of the UAV, including the components of the magnetic field intensity on different coordinate axes and the magnetic field direction angle, which are key data reflecting the geomagnetic field characteristics of the UAV's location.

[0097] Eigenvectors are a set of representative values ​​obtained after processing raw geomagnetic data, reflecting the characteristics of the geomagnetic field in terms of intensity, direction, etc. In the matching process, eigenvectors serve as the basic unit for data comparison.

[0098] Cosine similarity is a mathematical method used to measure the degree of similarity between two vectors. In this scheme, the degree of similarity between the feature vector converted from real-time geomagnetic data and the feature vector of candidate positions in the pre-stored geomagnetic map is calculated to determine the similarity between the two. The closer the value is to 1, the higher the similarity.

[0099] The matching score is a quantitative indicator calculated based on cosine similarity, used to intuitively represent the degree of matching between real-time geomagnetic data and candidate locations in pre-stored geomagnetic maps. The higher the score, the more similar the geomagnetic characteristics of the two are, and the greater the probability that the UAV is located at that candidate location.

[0100] The third preset threshold T3 is a pre-set critical value used to determine whether the geomagnetic matching is successful. When the matching score exceeds this threshold, it is considered that the real-time geomagnetic data has successfully matched a certain location in the pre-stored geomagnetic map, and subsequent location corrections can be made accordingly.

[0101] The position adjustment vector is obtained by weighted summing the differences between the geomagnetic coordinates of successfully matched feature points and the estimated coordinates of the UAV's current position. This vector contains direction and magnitude information, indicating the direction and distance that the UAV's current position estimate needs to be adjusted.

[0102] Current UAV position estimation: The initial position of the UAV, determined by other positioning methods (such as UWB positioning, visual positioning, etc.) before geomagnetic matching, is used as the benchmark for calculating position adjustment.

[0103] In this application, the UAV's geomagnetic sensor array collects geomagnetic data of its location and converts it into a feature vector. This feature vector is then compared with the feature vectors of each candidate location in a pre-stored geomagnetic map, and a matching score is obtained by calculating the cosine similarity. If the matching score exceeds a third preset threshold T3, the candidate location is considered a successful match. For each successfully matched feature point, the difference between its coordinates in the geomagnetic map and the predicted coordinates of the corresponding point under the current UAV position estimate is calculated, and different weights are assigned according to the matching score, with higher matching scores having higher weights. All weighted differences are summed to obtain a position adjustment vector. Combining the predicted coordinates of the corresponding point under the current UAV position estimate and the position adjustment vector, the geomagnetic matching correction result is calculated, achieving precise adjustment of the UAV's position. Converting geomagnetic data into feature vectors effectively extracts key features of the geomagnetic field, reduces data redundancy, and improves matching calculation efficiency. Weighting the differences of successfully matched feature points allows feature points with high matching scores to play a greater role in position adjustment, ensuring that the position correction result is more consistent with reality. By summing the results to obtain the position adjustment vector and correcting it based on the current UAV position estimate, this method can provide additional position correction references for the UAV under suitable environmental conditions, leveraging high-precision information from geomagnetic maps. This compensates for the shortcomings of other positioning methods (such as UWB and visual positioning) in certain scenarios, improving positioning accuracy. Simultaneously, its rigorous matching judgment mechanism and weighted correction strategy effectively enhance the reliability of the positioning system, reducing positioning errors caused by geomagnetic data fluctuations or interference, enabling the UAV to maintain stable and accurate positioning even in complex environments.

[0104] In one implementation, the first layer filter filters the initial position and the visual position update result Ps respectively, including:

[0105] The predicted position of the drone at the current moment is calculated based on the position, velocity, and acceleration data of the drone at the previous moment.

[0106] Compare the predicted location with the UWB measured location;

[0107] The predicted position is corrected based on the magnitude of the difference to obtain the filtered UWB position; where the correction magnitude is reduced when obstacles in the environment move frequently.

[0108] Based on the angular velocity and acceleration data collected in real time by the inertial measurement unit on the UAV, the visual pose prediction value at the current moment is calculated through integral calculation;

[0109] Compare the difference between the predicted pose and the visual position update result Ps;

[0110] The predicted pose is corrected based on the difference magnitude to obtain the filtered visual position; whereby, when the success rate of image feature point matching is low, the influence of visual data on the correction is reduced.

[0111] Specifically, the position prediction value is the theoretical value of the current position obtained by recursively calculating the position, velocity, and acceleration data of the previous moment, reflecting the continuity and regularity of the UAV's motion.

[0112] UWB-measured position is calculated by measuring the distance between the UAV and the ground base station using an ultra-wideband module, and then combining this distance with the base station's coordinates to determine the UAV's real-time position. The deviation between the predicted position and the UWB-measured position reflects the inconsistency between the model's prediction and the actual measurement.

[0113] The correction magnitude is the degree to which the predicted position is adjusted based on the magnitude of the difference. When obstacles in the environment move frequently (such as in the second state), the reliability of sensor data decreases, and the correction magnitude needs to be reduced to avoid introducing too much noise.

[0114] Visual pose prediction is the current pose (position and attitude) of the UAV calculated by integrating angular velocity and acceleration data collected by the inertial measurement unit (IMU) (such as integrating acceleration to obtain velocity and integrating velocity to obtain displacement), and is used to predict the visual positioning result.

[0115] The visual position update result Ps is the pre-corrected visual pose estimate constructed using camera images and LiDAR data to build keyframes.

[0116] In this application, the first-layer filter filters the initial UWB position and the visual position update results through two parallel paths. UWB position filtering: Based on the UAV's position, velocity, and acceleration at the previous moment, the current position prediction is calculated using a kinematic model; the predicted value is compared with the UWB measured position, and the difference is calculated; the predicted position is corrected according to the magnitude of the difference to obtain the filtered UWB position. When obstacles in the environment move frequently (i.e., the environment state is determined to be in the second state), it is considered that the UWB measurement may be subject to dynamic interference. In this case, the correction magnitude is reduced, and more reliance is placed on historical prediction values ​​to maintain filtering stability. Visual position filtering: Using the angular velocity and acceleration data collected in real time by the IMU, the current visual pose prediction is calculated through integration; the predicted value is compared with the visual position update result Ps, and the difference is calculated; the predicted pose is corrected according to the magnitude of the difference to obtain the filtered visual position. When the image feature point matching success rate is low (e.g., insufficient lighting or areas with repetitive textures), it indicates a decrease in the reliability of the visual data. In this case, the influence of visual data on the correction is reduced to avoid filtering deviations due to mismatches.

[0117] The first-layer filter achieves hierarchical suppression and dynamic adaptive adjustment of multi-source sensor noise by independently filtering UWB and visual data. Utilizing the optimal estimation characteristics of Kalman filtering, it effectively reduces UWB measurement noise and visual positioning noise, making the filtered position data closer to the true value. For example, correction of UWB data can eliminate random ranging errors, and integration of visual data can smooth high-frequency pose fluctuations. The correction strategy is dynamically adjusted based on environmental conditions and sensor data quality. When obstacles move frequently or visual feature matching is unstable, the filter automatically reduces its reliance on real-time measurement data, instead relying on historical motion models or inertial data to avoid positioning failures due to sensor malfunctions. For example, reducing the UWB correction magnitude in the second state can prevent positioning jumps caused by dynamic obstacle interference; reducing the weight of visual data can avoid erroneous corrections under low matching success rates. The filtered UWB and visual positions have eliminated most of the noise while retaining the effective features of the sensors, meeting the real-time positioning requirements of UAVs.

[0118] In one implementation, the second-layer filter fuses the output of the first-layer filter with the geomagnetic matching correction result obtained in the geomagnetic matching correction step to obtain the UAV's position coordinates, including:

[0119] Check the validity of the UWB position, visual position, and geomagnetic matching correction results of the first layer filter output: if the filtering error of the UWB position exceeds the fourth preset threshold, the UWB data is determined to be unreliable.

[0120] If the success rate of feature point matching at the visual location is lower than the fifth preset threshold, the visual data is deemed unreliable; if the environmental state is in the second state or the geomagnetic matching score does not exceed the third preset threshold, the geomagnetic data is deemed unreliable.

[0121] Based on the environmental condition assessment results, weights are assigned to each sensor: when the environment is in the first state, the weight of geomagnetic data is set to the first weight value, and the weights of UWB and visual data are set to the second and third weight values, respectively; when the environment is in the second state, the weight of geomagnetic data is reduced to zero, and the weights of UWB and visual data are adjusted to the fourth and fifth weight values, respectively; among them, the fourth weight value is greater than the second weight value, and the fifth weight value is greater than the third weight value.

[0122] For UWB data, the weights are adjusted inversely based on the magnitude of the filtering error; for visual data, the weights are adjusted directly based on the feature point matching success rate; for geomagnetic data, the weights are adjusted directly based on the matching score S.

[0123] The final position coordinates are calculated based on the weights of each sensor. If all sensor data are valid, the final position coordinates are calculated as follows: Final position = (UWB weight × UWB filter position) + (Visual weight × Visual filter position) + (Geomagnetic weight × Geomagnetic matching correction result). If the geomagnetic data is invalid, the final position coordinates are calculated as follows: Final position = (UWB weight × UWB filter position) + (Visual weight × Visual filter position).

[0124] Specifically, in the hierarchical Kalman filter architecture, the second layer filter is a processing module responsible for fusing the output results of the first layer filter (UWB position, visual position) with the geomagnetic matching correction results, and obtaining the final positioning coordinates through weight allocation and weighted calculation.

[0125] The fourth preset threshold is a pre-set critical value for UWB filtering error. When the actual error exceeds this threshold, the UWB data is determined to be unreliable due to excessive noise or environmental interference, and its weight in the fusion process needs to be reduced or discarded.

[0126] The fifth preset threshold is a pre-defined critical value for feature point matching success rate. Below this threshold, it indicates that the visual data is significantly affected by changes in lighting, texture loss, etc., and its reliability is insufficient. For UWB locations, if the filtering error exceeds the fourth preset threshold, the data is deemed unreliable. For visual locations, if the feature point matching success rate is below the fifth preset threshold, the data is deemed unreliable. For geomagnetic data, if the environment is in the second state or the matching score does not exceed the third preset threshold, the data is deemed unreliable.

[0127] Weight is a numerical value that measures the importance of each sensor's data in the fusion calculation. The higher the weight, the greater the impact of that sensor's data on the final positioning result.

[0128] In the first state, the geomagnetic data is reliable and is assigned the first weight value, while UWB and visual data are assigned the second and third weight values, respectively. In the second state, the geomagnetic data is invalid and its weight is reduced to zero, while the weights of UWB and visual data are increased to the fourth and fifth weight values, respectively.

[0129] Data quality fine-tuning can be understood as follows: UWB weights are adjusted inversely to the filtering error (the larger the error, the lower the weight); visual weights are adjusted directly to the feature point matching success rate; and geomagnetic weights are adjusted directly to the matching score.

[0130] If all sensor data is valid, the final coordinates are calculated using the formula: Final Position = (UWB Weight × UWB Filter Position) + (Visual Weight × Visual Filter Position) + (Geomagnetic Weight × Geomagnetic Matching Correction Result). If the geomagnetic data is invalid, the geomagnetic term is removed, and only UWB and visual data are fused.

[0131] In this application, by checking the validity of data through preset thresholds, interference from sensor data (such as UWB affected by multipath effects and visual data affected by changes in illumination) can be identified and weakened in a timely manner. For example, when the UWB filtering error is too large, its weight is reduced to prevent erroneous data from dominating the positioning results; geomagnetic data is automatically downweighted in dynamic environments to prevent errors caused by geomagnetic matching failure.

[0132] The system dynamically adjusts weights based on environmental conditions, allowing each sensor to leverage its strengths in different scenarios. In the first state, geomagnetic data, with its high accuracy, is given a high weight, and combined with UWB and visual data, it achieves high-precision positioning. In the second state, geomagnetic data is unreliable, so the system relies heavily on UWB and visual data to ensure the continuity and stability of the positioning process.

[0133] UWB provides absolute position reference, visual data supplements relative pose information, and geomagnetic data corrects accumulated errors in stable environments. Through weight adjustments, these three elements complement each other in different environments. For example, visual data receives increased weight in feature-rich scenes, while UWB receives increased weight in open spaces, ultimately achieving overall optimization of positioning accuracy.

[0134] Furthermore, even if a single sensor fails (such as a geomagnetic matching failure), the system can still maintain its positioning function using other valid data, preventing positioning failure due to a single point of failure. For example, when geomagnetic data is invalid, it automatically switches to UWB and visual fusion mode to ensure that the drone continuously acquires reliable location information.

[0135] In one embodiment, the pre-stored geomagnetic spectrum undergoes gridded preprocessing, specifically including the following steps:

[0136] The geomagnetic map is divided into multiple non-overlapping sub-regions according to a preset grid size. Each sub-region includes: the coordinates of the region boundary, a statistical description of the geomagnetic feature vectors within the region, and the topological connection relationship between adjacent regions.

[0137] Construct a geomagnetic feature index for each sub-region;

[0138] Based on environmental characteristics, matching priorities are assigned to each sub-region:

[0139] When geomagnetic matching begins, a set of candidate sub-regions within a preset range around the current location is calculated based on the estimated current location of the UAV. The candidate regions are sorted according to their matching priority, and high-priority regions are processed first. For each candidate region, the similarity between its feature descriptor and the real-time geomagnetic data is calculated, and regions with similarity exceeding the seventh preset threshold are selected as high-probability matching regions.

[0140] Matching is performed only on feature points within high-probability matching areas: the cosine similarity between real-time geomagnetic data and feature points within the area is calculated, and a matching score is generated; if the highest matching score exceeds the third preset threshold, the match is considered successful, and the position correction amount for the corresponding area is output.

[0141] Specifically, gridded preprocessing can be understood as a preprocessing method that divides a pre-stored geomagnetic map into multiple grid-like sub-regions according to certain rules. In this way, large-scale geomagnetic data is structurally segmented, which facilitates rapid retrieval and matching in the future.

[0142] A sub-region can be understood as an independent small area formed after the geomagnetic map is gridded, and each sub-region contains geomagnetic information within a specific geographical range.

[0143] Regional boundary coordinates are used to define the geographical coordinates (such as latitude, longitude, and altitude) of a sub-region, clarifying the sub-region's location in the actual environment.

[0144] The geomagnetic eigenvector is a mathematical description of the geomagnetic field characteristics in each sub-region, including key parameters such as geomagnetic field strength and direction.

[0145] Topological connectivity is used to describe the adjacency relationships between sub-regions, recording which sub-regions are directly connected to each other, which helps to quickly expand the search scope during the matching process.

[0146] The geomagnetic feature index is an index structure established for the geomagnetic feature vectors of each sub-region, similar to a book catalog. Through the index, geomagnetic feature data of a specific sub-region can be quickly located and accessed, improving matching efficiency.

[0147] Matching priority is a matching order index set for each sub-region based on environmental characteristics (such as geomagnetic feature stability, regional importance, etc.). Sub-regions with higher priority will participate in geomagnetic matching calculations first.

[0148] The candidate region set can be understood as: based on the estimated current location of the UAV, a set of sub-regions within a certain spatial range that may contain the actual location of the UAV, which is the preliminary screening result for geomagnetic matching.

[0149] Feature descriptors are representative feature data extracted from the geomagnetic feature vectors of sub-regions. They are used to calculate similarity with geomagnetic data collected in real time by UAVs, simplifying the data comparison process.

[0150] The seventh preset threshold is a pre-set similarity threshold used to filter candidate regions. When the similarity between the feature descriptor of a sub-region and the real-time geomagnetic data exceeds this threshold, the sub-region is identified as a high-probability matching region.

[0151] High-probability matching regions can be understood as sub-regions that, after screening, are considered to be highly similar to the geomagnetic characteristics of the drone's current location. Further finer feature point matching will be performed in these regions.

[0152] The position correction amount can be understood as: after successful geomagnetic matching, the direction and distance that need to be adjusted based on the current position of the drone calculated from the matching results are used to correct the drone's positioning coordinates.

[0153] In this application, the geomagnetic map is divided into multiple sub-regions according to a preset grid size. Each sub-region records its boundary coordinates, statistical description of geomagnetic feature vectors, and topological connections between adjacent regions. A geomagnetic feature index is also constructed for each sub-region. Matching priorities are assigned to each sub-region based on environmental characteristics, determining the matching order. When geomagnetic matching begins, a preset surrounding range is defined based on the estimated current position of the UAV, a set of candidate regions is selected, and these regions are sorted according to priority. The similarity between the feature descriptors of the candidate regions and the real-time geomagnetic data is calculated, and regions exceeding a seventh preset threshold are identified as high-probability matching regions. Only within high-probability matching regions are detailed cosine similarity calculations performed on feature points to generate a matching score. If the score exceeds a third preset threshold, a successful match is determined, and the corresponding region's position correction is output, achieving precise calibration of the UAV's position.

[0154] By dividing the geomagnetic map into sub-regions and constructing an index, the massive geomagnetic data is transformed into ordered, structured data, avoiding global traversal matching and significantly reducing data processing volume. Matching priorities are determined based on environmental characteristics, prioritizing regions with stable geomagnetic features and high relevance to the current environment. This ensures that in complex and changing environments, the system can quickly focus on effective data, reducing unnecessary computation and improving the targeting of the match. Candidate regions are screened based on UAV position estimation, and high-probability matching regions are determined by setting a similarity threshold, further narrowing the matching range. This allows the system to perform fine-grained matching only on the most likely regions, ensuring the accuracy of the matching results while reducing the risk of errors caused by data redundancy. This effectively improves the positioning accuracy and reliability of UAVs in complex environments, meeting the application requirements of UAVs in high-precision positioning scenarios.

[0155] In one implementation, the collected point cloud data further includes, before being used for environmental condition assessment:

[0156] Based on the clock on the drone, record the timestamps of the data collected by each sensor;

[0157] By utilizing the attitude and position information provided by the UAV's inertial measurement unit, the point cloud data of the lidar and the image data of the camera are uniformly converted to the same world coordinate system;

[0158] For data that is out of time, linear interpolation is used to synchronize it to the reference time point, so that the image information and point cloud data are aligned in the spatiotemporal dimension.

[0159] The second embodiment of the present invention provides a UAV-ground sensor cooperative positioning system, based on any of the above-mentioned positioning methods, characterized in that it includes:

[0160] The drone is equipped with a camera, a UWB module, a lidar, and a geomagnetic sensor. The camera collects image information of the surrounding environment; the UWB module is used to measure the distance information of the ground base station; the geomagnetic sensor is used to measure geomagnetic data; and the lidar is used to measure environmental obstacle information.

[0161] Ground base station; the ground base station communicates with the UAV.

[0162] The specific embodiments described above further illustrate the purpose, technical solution, and beneficial effects of the present invention. It should be understood that the above descriptions are merely specific embodiments of the present invention and are not intended to limit the scope of protection of the present invention. In particular, it should be noted that any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention for those skilled in the art.

Claims

1. A method for cooperative positioning of unmanned aerial vehicles (UAVs) and ground sensors, characterized in that, include: The UAV measures the distance between itself and the ground base station via a UWB module and transmits the ranging data to the ground base station. The ground base station processes the received ranging data based on pre-stored environmental map data and historical positioning data to obtain the initial position of the UAV relative to the base station and transmits the initial position back to the UAV. The drone obtains environmental status assessment results based on image information collected by camera and obstacle detection results collected by lidar, and generates keyframes based on the environmental status assessment results to construct a local map; The visual pose estimation is pre-corrected using inertial measurement unit data, and the visual position update result Ps is output. The UAV detects the environmental state based on the lidar; when the environment is detected to be in a first state, the UAV matches the geomagnetic data detected by the UAV's geomagnetic sensor array with a pre-stored geomagnetic map; a geomagnetic matching correction result is obtained based on the degree of matching; when the environment is detected to be in a second state, geomagnetic matching is paused. Data fusion is performed based on a hierarchical Kalman filter; wherein, the first layer filter filters the initial position and the visual position update result Ps respectively; the second layer filter fuses the output of the first layer filter with the geomagnetic matching correction result obtained in the geomagnetic matching correction step to obtain the UAV position coordinates; Environmental condition assessment includes steps for calculating obstacle mobility index, calculating visual feature stability, and determining environmental condition. The steps for calculating the obstacle movement index include: Clustering is performed on two frames of point cloud data to identify obstacles in the environment; Based on location distance and shape similarity, an association matching method is used to match obstacles between two frames; Calculate the position change vector for each successfully associated obstacle; The mean square error of all position change vectors is calculated to obtain the obstacle movement index. The steps for calculating visual feature stability include: Extract feature points from adjacent frames and calculate descriptors; Feature point matching is performed to obtain initial matching point pairs; Remove false matching pairs and calculate the number of valid matching pairs; The stability of visual features is obtained by calculating the statistical average of the feature point matching success rate over at least 3 consecutive frames. The environmental condition determination steps include: The obstacle movement index is compared with a first preset threshold T1, and the visual feature stability is compared with a second preset threshold T2. When the obstacle movement index is less than T1 and the visual feature stability is greater than T2, the environment is determined to be in the first state. When the obstacle movement index is not less than T1 or the visual feature stability is less than T2, the environment is determined to be in the second state.

2. The method for cooperative positioning of UAV and ground sensors according to claim 1, characterized in that, Match the geomagnetic data detected by the drone's geomagnetic sensor array with the pre-stored geomagnetic map; The geomagnetic matching correction results are obtained based on the degree of matching, including: Convert real-time geomagnetic data detected by the geomagnetic sensor array into feature vectors; Calculate the cosine similarity between the feature vector and the feature vector of the candidate location in the pre-stored geomagnetic map, and use it as the matching score S; when the matching score S exceeds the third preset threshold T3, the match is determined to be successful. For each successfully matched feature point, calculate the difference between its coordinates in the geomagnetic map and the predicted coordinates of the corresponding point under the current UAV position estimation; determine the weight of each difference based on the matching degree score, with higher matching degree resulting in greater weight; Sum all the weighted differences to obtain the position adjustment vector; The geomagnetic matching correction result is calculated by combining the predicted coordinates of the corresponding point and the position adjustment vector under the current UAV position estimation.

3. The method for cooperative positioning of UAV and ground sensors according to claim 2, characterized in that, The first layer filter filters the initial position and the visual position update result Ps respectively, including: The predicted position of the drone at the current moment is calculated based on the position, velocity, and acceleration data of the drone at the previous moment. Compare the predicted location with the UWB measured location; The predicted position is corrected based on the magnitude of the difference to obtain the filtered UWB position; where the correction magnitude is reduced when obstacles in the environment move frequently. Based on the angular velocity and acceleration data collected in real time by the inertial measurement unit on the UAV, the visual pose prediction value at the current moment is calculated through integral calculation; Compare the difference between the predicted pose and the visual position update result Ps; The predicted pose is corrected based on the difference magnitude to obtain the filtered visual position; whereby, when the success rate of image feature point matching is low, the influence of visual data on the correction is reduced.

4. The method for cooperative positioning of UAV and ground sensors according to claim 3, characterized in that, The second-layer filter fuses the output of the first-layer filter with the geomagnetic matching correction result obtained in the geomagnetic matching correction step to obtain the UAV's position coordinates, including: Check the validity of the UWB position, visual position, and geomagnetic matching correction results of the first layer filter output: if the filtering error of the UWB position exceeds the fourth preset threshold, the UWB data is determined to be unreliable. If the success rate of feature point matching at the visual location is lower than the fifth preset threshold, the visual data is deemed unreliable; if the environmental state is in the second state or the geomagnetic matching score does not exceed the third preset threshold, the geomagnetic data is deemed unreliable. Based on the environmental condition assessment results, weights are assigned to each sensor: when the environment is in the first state, the weight of geomagnetic data is set to the first weight value, and the weights of UWB and visual data are set to the second and third weight values, respectively; when the environment is in the second state, the weight of geomagnetic data is reduced to zero, and the weights of UWB and visual data are adjusted to the fourth and fifth weight values, respectively; among them, the fourth weight value is greater than the second weight value, and the fifth weight value is greater than the third weight value. For UWB data, the weights are adjusted inversely based on the magnitude of the filtering error; for visual data, the weights are adjusted directly based on the feature point matching success rate; for geomagnetic data, the weights are adjusted directly based on the matching score S. The final position coordinates are calculated based on the weights of each sensor. If all sensor data are valid, the final position coordinates are calculated as follows: Final position = (UWB weight × UWB filter position) + (Visual weight × Visual filter position) + (Geomagnetic weight × Geomagnetic matching correction result). If the geomagnetic data is invalid, the final position coordinates are calculated as follows: Final position = (UWB weight × UWB filter position) + (Visual weight × Visual filter position).

5. The method for cooperative positioning of UAV and ground sensors according to claim 4, characterized in that, The pre-stored geomagnetic spectrum is preprocessed using a gridded method, specifically including the following steps: The geomagnetic map is divided into multiple non-overlapping sub-regions according to a preset grid size. Each sub-region includes: the coordinates of the region boundary, a statistical description of the geomagnetic feature vectors within the region, and the topological connection relationship between adjacent regions. Construct a geomagnetic feature index for each sub-region; Based on environmental characteristics, matching priorities are assigned to each sub-region: When geomagnetic matching begins, a set of candidate sub-regions within a preset range around the current location is calculated based on the estimated current location of the UAV. The candidate regions are sorted according to their matching priority, and high-priority regions are processed first. For each candidate region, the similarity between its feature descriptor and the real-time geomagnetic data is calculated, and regions with similarity exceeding the seventh preset threshold are selected as high-probability matching regions. Matching is performed only on feature points within high-probability matching areas: the cosine similarity between real-time geomagnetic data and feature points within the area is calculated, and a matching score is generated; if the highest matching score exceeds the third preset threshold, the match is considered successful, and the position correction amount for the corresponding area is output.

6. The method for cooperative positioning of unmanned aerial vehicles (UAVs) and ground sensors according to claim 5, characterized in that, Before being used for environmental condition assessment, the collected point cloud data also includes: Based on the clock on the drone, record the timestamps of the data collected by each sensor; By utilizing the attitude and position information provided by the UAV's inertial measurement unit, the point cloud data of the lidar and the image data of the camera are uniformly converted to the same world coordinate system; For data that is out of time, linear interpolation is used to synchronize it to the reference time point, so that the image information and point cloud data are aligned in the spatiotemporal dimension.

7. A UAV and ground sensor cooperative positioning system, based on the positioning method according to any one of claims 1-6, characterized in that, include: The drone is equipped with a camera, a UWB module, a lidar, and a geomagnetic sensor. The camera collects image information of the surrounding environment; the UWB module is used to measure the distance information of the ground base station; the geomagnetic sensor is used to measure geomagnetic data; and the lidar is used to measure environmental obstacle information. Ground base station; the ground base station communicates with the UAV.

Images