Methods and systems for cooperative localization and target guidance of unmanned aerial vehicle (UAV) swarms

Abstract

This invention provides a method and system for collaborative localization and target guidance of unmanned aerial vehicle (UAV) swarms, relating to the field of UAV swarm positioning and navigation. It solves problems such as insufficient positioning accuracy, high cumulative error, and poor adaptability of path planning in existing technologies. The method first constructs a feature point cloud and descriptor library for the target area. Multiple observation UAVs, based on images of the target area captured by their own cameras, calculate and determine their own pose data in real time under the Reloc3r localization framework. A mapping relationship between the pixel coordinates of the target area and the world coordinate system is established to obtain the individual positioning coordinates of each observation UAV. After clustering and fusion, the target coordinate data at the mission location is obtained. Finally, the executing UAV generates a flight path from its own position to the target position based on the target coordinate data, constrained by an obstacle point cloud map. This invention improves positioning consistency and supports precise guidance requirements in complex scenarios, significantly increasing the success rate of collaborative guidance.

Description

Technical Field

[0001] This invention relates to the field of unmanned aerial vehicle (UAV) swarm positioning and navigation technology, specifically to a method and system for UAV swarm collaborative positioning and target guidance. Background Technology

[0002] In collaborative drone swarm missions, achieving high-precision target positioning and effective guidance is a crucial direction for current technological development. With the increasing prevalence of drones in various application scenarios, the demand for multi-drone collaborative missions is growing, making positioning accuracy and guidance reliability key factors affecting overall performance. However, existing technologies still face numerous challenges in this field, failing to meet the practical needs of complex and dynamic environments.

[0003] In terms of localization, traditional visual localization methods mainly rely on feature matching techniques between consecutive frames to estimate the real-time pose of the UAV. However, when the UAV performs high-speed maneuvers, such as rapid dives or sharp turns, these methods are prone to significant accumulation of pose estimation errors due to unstable feature tracking. This error not only amplifies over time but may also cause the localization system to fail, thus affecting the accurate determination of the target's location. Furthermore, environmental factors such as changes in lighting or occlusion problems further exacerbate the uncertainty of localization, limiting the applicability of existing methods in dynamic scenes.

[0004] For multi-drone cooperative positioning, existing technologies often focus on the independent positioning of individual UAVs, lacking effective swarm coordination mechanisms. Because the positioning data from individual UAVs are not fully integrated, the overall positioning error of the swarm tends to diverge over time, making it difficult to maintain consistency and synchronization in cooperative tasks. This error accumulation problem not only reduces the reliability of positioning but may also affect the coordinated actions between multiple UAVs, thus limiting the overall task execution efficiency. Therefore, how to achieve effective integration and error control of multi-drone positioning data is a pressing technical challenge that needs to be addressed.

[0005] Furthermore, common path planning and guidance algorithms exhibit insufficient robustness when dealing with complex environments. These algorithms are typically based on static or simplified environmental models and fail to fully integrate real-time environmental perception data, such as information on dynamic changes in obstacles. In scenarios with buildings, trees, or other complex obstacles, path planning is prone to producing unstable results, increasing the risk of collisions and thus affecting the reliability and safety of the guidance process. Existing methods also lack adaptability when dynamically adjusting paths, making it difficult to ensure the continuity and accuracy of guidance in changing environments.

[0006] It is evident that existing technologies have significant limitations in collaborative localization and target guidance for UAV swarms, including insufficient positioning accuracy in dynamic environments, error accumulation issues in multi-UAV collaboration, and poor robustness of path planning. These limitations restrict the widespread application of UAV swarms in complex tasks, necessitating the development of more efficient and reliable technical solutions to improve overall performance and adaptability. Summary of the Invention

[0007] The purpose of this invention is to address the limitations of existing UAV swarm collaborative positioning and guidance processes, such as insufficient positioning accuracy, high cumulative errors, and poor path planning adaptability. Therefore, this invention proposes a method and system for UAV swarm collaborative positioning and target guidance. The core of this invention is the construction of a complete process method and system including "prior map - single-unit positioning - multi-unit fusion - collaborative guidance." Based on the Reloc3r framework, it achieves high-precision positioning, improves positioning consistency, and supports precise guidance requirements in complex scenarios, significantly increasing the success rate of collaborative guidance in specific tasks.

[0008] The present invention employs the following technical solutions to achieve its objective: A method for cooperative localization and target guidance of a drone swarm, wherein the drone swarm includes multiple observation drones and at least one execution drone; the method includes the following steps: S1. At least one observation drone pre-collects image sequences and point cloud data of the target area at the mission location, and constructs a feature point cloud and descriptor library of the target area containing static feature points; S2. Multiple observation drones, based on the feature point cloud and descriptor library of the target area, as well as the target area images collected in real time by their own cameras, calculate and determine their own drone pose data in real time under the action of their respective Reloc3r positioning frameworks. S3. Multiple observation drones establish a mapping relationship between pixel coordinates in the real-time image of the target area and the preset world coordinate system coordinates based on their own drone pose data, and obtain their own individual positioning coordinates of the target. At the same time, they form matching feature point pairs of multiple observation drones in the feature point cloud and descriptor library. S4. Cluster and fuse the individual positioning coordinates and matching feature point pairs corresponding to multiple observation drones to obtain the target coordinate data at the mission location after fusion. S5. The UAV receives the target coordinate data and converts it into its own local coordinate system. Under the constraints of the obstacle point cloud map determined in real time, it plans and generates flight path instructions from its own position to the target position and executes them, thus completing the collaborative positioning and target guidance process.

[0009] Specifically, in step S1, the observation UAV that collects image sequences and point cloud data of the target area is equipped with an RTK-GPS module. During its collection flight, multiple main routes and multiple verification routes are preset. On each route, images of the corresponding target area are collected at preset trigger intervals. After preprocessing the collected target area images, the SIFT algorithm is used to extract features, obtain feature points in the images, and generate descriptors of preset dimensions. Through feature matching and UAV camera pose estimation, the three-dimensional coordinates of the feature points are reconstructed using triangulation to generate the initial point cloud of the target area. In the process of generating the initial point cloud, the bundle adjustment method is used to optimize it.

[0010] Preferably, after generating the initial point cloud of the target area, dynamic feature filtering and map optimization are performed on it; the ERASOR algorithm is used to perform consistency verification on multiple consecutive frames of point cloud, divide the point cloud into grids of a preset size and calculate the scan ratio of each grid, and remove grids with scan ratios greater than a preset threshold, that is, remove dynamic feature point areas; after performing planar fitting on the remaining grids, feature points with residuals greater than a preset threshold in the fitted point cloud are further removed; according to the preset point cloud retention densities for ground feature points and building feature points, the corresponding point cloud data is stored in the FAISS vector database, and descriptors of a preset dimension generated by the SIFT algorithm are also stored, thereby constructing the feature point cloud and descriptor library of the target area.

[0011] Specifically, in step S2, multiple observation drones preprocess the target area images acquired in real time by their cameras. First, a pre-deployed image detection model is used to detect and infer the target area images to obtain target detection boxes. Based on the preset input size requirements for target localization, the electronic zoom factor is calculated with the target center corresponding to the target detection box as the image center. A pixel area of ​​the corresponding size is cropped from the target area image and scaled to match the preset input size. Then, image contrast enhancement is performed, thereby completing the preprocessing of the target area image, which serves as the input basis for the Reloc3r localization framework.

[0012] Specifically, in step S2, the UAV pose data includes translation vectors. and rotation matrix Within the Reloc3r localization framework, these two elements constitute the 6-DOF pose of the camera corresponding to the UAV pose data. After receiving the target region image as input, the Reloc3r localization framework extracts the image coordinates and corresponding descriptors, performs feature matching, searches for similar regions in the feature point cloud and descriptor library, and then uses the EPnP algorithm to solve for the translation vector. and rotation matrix This allows us to obtain the drone pose data of the observation drone that acquires real-time images of the target area.

[0013] Specifically, in step S3, the Reloc3r positioning framework reverse-engineers the three-dimensional position of the target based on the mapping relationship between pixel coordinates in the real-time image of the target area and the preset world coordinate system coordinates. First, the target depth information is calculated based on the camera pitch angle and observation altitude of the observation UAV. Then, coordinate transformation is performed based on the target depth information to transform the camera coordinate system to the world coordinate system. The world coordinate system is further transformed into WGS84 format for storage. The stored WGS84 coordinates are the individual positioning coordinates used for clustering and fusion in step S4. Calculate the covariance matrix of individual positioning coordinates and estimate its uncertainty. At the same time, calculate the positioning confidence of matching feature point pairs of multiple observation UAVs. The estimated uncertainty and the calculated positioning confidence are used as auxiliary criterion inputs for cluster fusion in step S4.

[0014] Preferably, in step S4, after converting the individual positioning coordinates of multiple observation UAVs to UTM plane coordinates, the DBSCAN algorithm is used to detect outliers, and isolated points are marked as outliers and removed. Then, based on the positioning confidence of the matching feature point pairs of multiple observation UAVs, the weight of each observation UAV in the clustering and fusion process is calculated, and a weighted clustering and fusion process is performed.

[0015] Preferably, the average distance between the fused target coordinate data and the individual positioning coordinates of each observation UAV is calculated. If the calculated average distance is greater than a preset repositioning threshold, the repositioning process is triggered. During the clustering fusion process, the observation UAV with the highest positioning confidence of the matching feature point pair is used as the fusion node, which receives the individual positioning coordinates and positioning confidence of the other observation UAVs.

[0016] Specifically, in step S5, the UAV's local coordinate system is the ENU coordinate system; the UAV uses its onboard obstacle detection radar to determine the obstacle point cloud map within a preset range centered on itself in real time; when planning and generating flight path instructions from its own position to the target position, the UAV's current position is taken as the starting point, and the position corresponding to the preset direction and distance of the real-time target coordinate data is taken as the ending point, and path planning is performed under the constraints of preset flight attitude conditions, and the control law in the guidance process is determined by the proportional guidance method; as the flight path instructions are executed, when obstacles are detected on the path based on the real-time obstacle point cloud map, local path adjustment is triggered.

[0017] This invention also provides a UAV swarm cooperative positioning and target guidance system for implementing the above method, the system comprising the following functional modules: The prior map construction module is used to pre-collect image sequences and point cloud data of the target area at the task location, and construct a feature point cloud and descriptor library for the target area containing static feature points. The single-unit positioning module includes multiple observation drones. It is used to calculate and determine the drone pose data of each observation drone in real time based on the feature point cloud and descriptor library of the target area, as well as the target area images acquired in real time by the cameras of each observation drone, under the Reloc3r positioning framework. It is also used to establish the mapping relationship between the pixel coordinates in the real-time image of the target area and the coordinates of the preset world coordinate system, to obtain the corresponding target individual positioning coordinates based on the drone pose data, and at the same time to form the matching feature point pairs of each observation drone in the feature point cloud and descriptor library. The multi-drone fusion module is used to cluster and fuse the individual positioning coordinates and matching feature point pairs of multiple observation drones to obtain the target coordinate data at the mission location after fusion. The collaborative guidance module includes at least one execution drone, which receives target coordinate data and converts it into the execution drone's own local coordinate system, enabling the execution drone to plan and generate flight path instructions from its own position to the target position under the constraints of its own real-time determined obstacle point cloud map, and execute them.

[0018] In summary, due to the adoption of this technical solution, the beneficial effects of this invention are as follows: This invention significantly improves the overall performance and reliability of related systems by constructing a complete solution from prior maps to single-machine localization, multi-machine fusion, and collaborative guidance, providing strong support for collaborative tasks in complex environments. By employing the Reloc3r localization framework, this invention achieves rapid and accurate calculation of single-machine pose, greatly reducing error accumulation in dynamic environments. Furthermore, combined with an intelligent fusion algorithm for multi-machine localization data, through dynamic weight allocation and clustering processing, the consistency of UAV swarm localization is further enhanced, effectively controlling the overall localization error and thus establishing an accurate foundation for subsequent guidance tasks.

[0019] Regarding real-time performance, this invention optimizes the data processing flow, ensuring low latency in positioning and guidance calculations, and meeting the real-time requirements of high-speed UAV maneuvers. This enables the system to respond quickly to environmental changes, ensuring the continuity and timeliness of specific task execution. Simultaneously, this invention introduces dynamic path planning and collision avoidance technology, combined with real-time environmental perception data, significantly improving the success rate of collaborative guidance in complex obstacle scenarios. Through multi-segment path optimization and intelligent algorithms, the system can adaptively adjust the guidance path, effectively avoiding collision risks and improving the reliability of task completion.

[0020] This invention also possesses dynamic adaptability. Through a real-time update mechanism of the prior map, it can promptly respond to dynamic changes in the mission scenario, such as the addition of new building structural obstacles. Its flexibility ensures the system's continuous and effective operation in changing environments, thereby enhancing the stability and accuracy of the guidance process and providing comprehensive technical support for collaborative guidance missions involving UAV swarms. Attached Figure Description

[0021] The present invention is further described in detail with reference to the following figures, which include four figures as follows: Figure 1 This is a schematic diagram illustrating the overall process of the method of the present invention; Figure 2 This is a schematic diagram illustrating the functional module architecture relationship and collaboration logic of the system of the present invention; Figure 3 This is a simplified schematic diagram illustrating the contents of the four related data streams in the system of this invention; Figure 4 This is a schematic diagram of the functional execution flow of the stand-alone positioning module in the system of the present invention. Detailed Implementation

[0022] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, 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. The components of the embodiments of the present invention described and shown in the accompanying drawings can generally be arranged and designed in various different configurations.

[0023] Therefore, the following detailed description of the embodiments of the invention provided in the accompanying drawings is not intended to limit the scope of the claimed invention, but merely to illustrate selected embodiments of the invention. All other embodiments obtained by those skilled in the art based on the embodiments of the invention without inventive effort are within the scope of protection of the invention.

[0024] Example 1 A method for collaborative localization and target guidance of unmanned aerial vehicle (UAV) swarms, which can be viewed simultaneously. Figure 1 A simplified illustration of the method is provided, where the drone swarm includes multiple observation drones and at least one execution drone; the key steps of the method can be summarized as follows: S1. At least one observation drone pre-collects image sequences and point cloud data of the target area at the mission location, and constructs a feature point cloud and descriptor library of the target area containing static feature points; S2. Multiple observation drones, based on the feature point cloud and descriptor library of the target area, as well as the target area images collected in real time by their own cameras, calculate and determine their own drone pose data in real time under the action of their respective Reloc3r positioning frameworks. S3. Multiple observation drones establish a mapping relationship between pixel coordinates in the real-time image of the target area and the preset world coordinate system coordinates based on their own drone pose data, and obtain their own individual positioning coordinates of the target. At the same time, they form matching feature point pairs of multiple observation drones in the feature point cloud and descriptor library. S4. Cluster and fuse the individual positioning coordinates and matching feature point pairs corresponding to multiple observation drones to obtain the target coordinate data at the mission location after fusion. S5. The UAV receives the target coordinate data and converts it into its own local coordinate system. Under the constraints of the obstacle point cloud map determined in real time, it plans and generates flight path instructions from its own position to the target position and executes them, thus completing the collaborative positioning and target guidance process.

[0025] This embodiment will describe the details and preferred examples of each step in the order described above. First, the hardware configuration of the observation drone and the execution drone used in this embodiment will be introduced, wherein there are a total of 3 observation drones and 1 execution drone.

[0026] The observation drone and the execution drone share the same processor, the NVIDIA Jetson AGX Orin, which supports the computational requirements of the method, including real-time visual processing, AI inference, complex path planning, and obstacle avoidance. The observation drone's camera module uses a Sony IMX586, a 4K camera supporting electronic zoom up to 512×512 resolution. The observation drone's sensor is a built-in Bosch BMI088 IMU, and it also features an RTK-GPS module. The observation drone's communication module is a 5.8GHz data radio that supports Mesh self-organizing networking.

[0027] The execution UAV's navigation module employs an RTK-GPS+INS combined navigation system, with a positioning update frequency of 50Hz and an attitude update frequency of 200Hz. The execution UAV's communication module is identical to that of the observation UAV, and it also supports two-way data exchange with the ground station. The execution UAV is additionally equipped with a 16-line LiDAR as an obstacle detection radar, with a detection range of 100m, an angular resolution of 1 degree, and an update frequency of 10Hz, thus enabling the real-time obstacle detection required by the method.

[0028] The method first constructs a priori map in step S1, namely the feature point cloud and descriptor library of the target area. In this embodiment, the observation UAV equipped with an RTK-GPS module performs flight path planning at this stage, adopting a "grid-like" intersecting flight path to cover a 1km×1km target area. Three main flight paths are set with heading angles of 0°, 90° and 180°, respectively, as well as two verification flight paths; the distance between each flight path is 50m, the flight altitude is 100m, and the ground speed is 8m / s.

[0029] The drone's camera trigger mode is set to timed trigger, with a 1-second interval. During flight along the flight path and image acquisition, the image overlap rate is ensured to be ≥80% in the flight direction and ≥70% in the lateral direction. The camera's exposure time is set to 1 / 1000s, and the ISO is automatically adjusted within the range of 100-800 to ensure dynamic range coverage. RTK-GPS data is recorded simultaneously with image acquisition at 10 samples per second, and IMU data is also recorded at 200 samples per second; the camera shutter trigger timestamp is also recorded synchronously.

[0030] The target area image sequence acquired in this stage is preprocessed. Based on the camera intrinsic parameter matrix, distortion correction of the original image is performed using OpenCV. The camera intrinsic parameter matrix in this embodiment... as follows:

[0031] Next, the SIFT algorithm is used for feature extraction. In this embodiment, the contrast threshold of the algorithm is set to 0.03, the edge threshold is set to 10, and the maximum number of feature points is 5000 / image, thereby generating a 128-dimensional descriptor.

[0032] This embodiment uses feature matching and observation of UAV camera pose estimation to reconstruct the three-dimensional coordinates of feature points using triangulation, generating an initial point cloud of the target area. This is a three-dimensional reconstruction process. Specifically, COLMAP software is used for feature matching, with the FLANN matcher and a nearest neighbor ratio threshold of 0.8. The initial camera pose estimation uses a five-point algorithm to triangulate and generate the initial point cloud.

[0033] As a preferred embodiment, the initial point cloud is optimized using the bundle adjustment method, and the objective function used for optimization is as follows:

[0034] The optimization process iterates 50 times, with a convergence threshold of 1e-6; the objective function represents a minimization problem, where... It is a double summation symbol. The first one representing the observation drone One camera, Represents the first in the image captured by the camera Each three-dimensional point represents the sum of errors observed by all three-dimensional points across all cameras.

[0035] In the objective function, Representative at the In the images captured by the camera, the first... The actual observed pixel coordinates corresponding to each three-dimensional spatial point are two-dimensional coordinates, for example... Pixel value; Representing the The spatial coordinates of a three-dimensional point in a preset world coordinate system are given by, for example, three-dimensional coordinates. ; Representing the Rotation matrix of each camera, This describes the rotational relationship between the camera coordinate system and the world coordinate system. Representing the The translation vector of each camera. This describes the translation relationship between the camera coordinate system and the world coordinate system. It represents a real number.

[0036] This is a perspective projection function used to project points in a 3D camera coordinate system onto a 2D image plane; if the points in the camera coordinate system are... Then the pixel coordinates projected onto the image are:

[0037] In the formula, It is the camera's focal length; These are the coordinates of the camera's principal point, i.e., the intrinsic parameters.

[0038] symbol This represents the square of the L2 norm, i.e., the square of the Euclidean distance; for example, a two-dimensional point. and The norm square is .

[0039] After generating the initial point cloud of the target region, this embodiment performs dynamic feature filtering and map optimization to filter out targets such as pedestrians and vehicles in the image used to construct the prior map. First, the ERASOR algorithm is used to check the consistency of 10 consecutive frames of point cloud. The point cloud is divided into 1m×1m grids, and the scan ratio SR of each grid is calculated. Grids with SR>0.5 are removed, that is, dynamic feature point regions are removed.

[0040] After performing planar fitting on the remaining grid, feature points with residuals greater than 0.3m in the fitted point cloud are further removed. Different point cloud retention densities are preset for ground feature points and building feature points, with ground feature point density ≥ 100 points / m² and building feature point density ≥ 50 points / m². The corresponding point cloud data is stored in the FAISS vector database, along with corresponding 128-dimensional descriptors extracted using the SIFT algorithm. This constructs a feature point cloud and descriptor library for the target area, using the IVF1024, Flat index type.

[0041] The process then proceeds to step S2, the single-machine localization process. First, multiple observation drones preprocess the target area images acquired in real-time by their cameras. A pre-deployed YOLOv8n image detection model is then used to perform detection inference on the target area images, resulting in target bounding boxes. This model takes a 640×640 image as input, sets a confidence threshold of 0.5, and achieves 30 FPS inference on the Jetson platform before outputting the target bounding boxes. .

[0042] Based on the preset input size requirements for target localization, this embodiment uses 512×512, with the center of the target corresponding to the target detection box as the image center. Calculate the electron magnification factor ,as follows:

[0043] If the target size is too large, then take... ; To crop a pixel region of the corresponding size from the target region image, i.e. The pixel region is scaled to a size that matches the preset input size of 512×512 using bicubic interpolation, and then the CLAHE algorithm is applied to enhance the image contrast with a grid size of 8×8 and cliplimit=2.0. This completes the preprocessing of the target region image, which serves as the input basis for the Reloc3r positioning framework.

[0044] In this embodiment, the UAV pose data includes translation vectors. and rotation matrix Within the Reloc3r localization framework, these two elements constitute the 6-DOF pose of the camera corresponding to the UAV pose data. They are the core mathematical symbols describing the position and attitude of the UAV / camera in three-dimensional space, where: Translation vector , represents "three-dimensional real space", that is, a vector space consisting of three real number components. Translation vector. It is a three-dimensional column vector, in the form of:

[0045] in, These respectively represent the directions along the three-dimensional coordinate system. , , The translation of the axis is usually measured in meters, and the three-dimensional coordinate system is usually the world coordinate system or the camera coordinate system.

[0046] In drone positioning, This describes the position of the drone / camera in the world coordinate system. For example, if... This indicates that the drone / camera is located in the world coordinate system. 100-meter axis 200-meter axis At 50 meters from the axis, The axis is usually in the height direction.

[0047] Rotation matrix , It is a "special orthogonal group", representing the set of all 3×3 "rotation matrices"; it satisfies two core conditions: Orthogonality: The transpose of a matrix is ​​equal to its inverse, i.e. This ensures that the vector length remains unchanged after rotation; The determinant is 1: This ensures that the rotation is "right-handed" and there is no mirror flip.

[0048] Rotation matrix It is a 3×3 matrix. In UAV positioning, This describes the drone / camera's attitude, i.e., its orientation, and indicates the rotational relationship between the camera coordinate system and the world coordinate system; for example: like If it is an identity matrix, then the camera pose is completely consistent with the world coordinate system, with no rotation; if Including wrap The rotational component of the axis represents the change in the camera's pitch angle; the component around the axis... The component of the shaft corresponds to the roll angle, around The axis component corresponds to the yaw angle.

[0049] In this embodiment, the model selected for the deployment and optimization of the Reloc3r positioning framework is the DUSt3R-light model, which has 2.1M parameters, an input size of 512×512, is quantized to FP16 precision on TensorRT, and has an inference time of ≤15ms, meeting the application requirements.

[0050] After receiving the target region image as input, the Reloc3r localization framework extracts a 128-dimensional vector and performs L2 normalization for FAISS global retrieval. For local features, it extracts 800 feature points per frame, including coordinates. And a 128-dimensional descriptor.

[0051] When performing feature matching, the top-5 similar regions are retrieved from the feature point cloud and descriptor library in FAISS format to ensure that the cosine similarity is ≥0.85, thus narrowing the matching range to 50m×50m. For local feature matching, the FLANN matcher with KD-Tree index is used, with ≥500 initial matching point pairs. False matches are removed using the RANSAC algorithm, with an inlier threshold of 3 pixels and 1000 iterations, retaining ≥30 pairs of inliers.

[0052] Therefore, the EPNP algorithm can be used to solve for the translation vector. and rotation matrix This process yields the UAV pose data of the observation UAV that acquires real-time images of the target area. Nonlinear optimization is preferably performed during this process to minimize reprojection errors. Let be the objective function, where This is the square of the reprojection error. These are the actual observed coordinates of the feature points in the image. For perspective projection functions, For the world coordinates of a point in three-dimensional space, That is, the camera rotation matrix. That is, the camera translation vector, which can be understood by analogy with the formula; the Levenberg-Marquardt algorithm is used for iterative optimization, with 20 iterations and a convergence threshold set to 1e-4.

[0053] In step S3, the Reloc3r positioning framework reverse-engineers the three-dimensional position of the target based on the mapping relationship between the pixel coordinates in the real-time image of the target area and the preset world coordinate system coordinates.

[0054] First, based on the camera pitch angle of the observation drone... and observation altitude , The target depth can be calculated using a fusion of GPS and barometer data. ,as follows:

[0055]

[0056] In the formula, The target image y-coordinate is the vertical pixel coordinate of the target in the image. It comes from the target detection in the preprocessing stage, such as the target center y-coordinate output by YOLOv8n. Let y be the principal point of the camera. Let be the focal length of the camera in the y-direction; The target pitch offset angle.

[0057] Subsequently based on the target depth Perform a coordinate transformation to change the camera coordinate system to the world coordinate system, as shown in the following equation:

[0058]

[0059] In the formula, Represents the coordinates in the camera coordinate system. Represents the transformed world coordinate system coordinates; The x-coordinate of the target image; Let x be the principal point x-coordinate of the camera. Let x be the focal length of the camera in the x-direction.

[0060] The world coordinate system is further transformed into WGS84 format for storage. The transformation process is performed using UTM projection, and the formula is as follows:

[0061]

[0062] In the formula, Represents the target latitude. Represents the target longitude; Represents the latitude of the drone. Represents the longitude of the drone; Represents the eastern component of world coordinates. Representing the northward component of world coordinates, both are derived from the camera-to-world coordinate transformation results. ; The radius of the Earth's equator. This is the radius of the Earth's geocentric circle. The stored WGS84 coordinates are the individual positioning coordinates used for clustering and fusion in step S4.

[0063] As a preferred embodiment, the covariance matrix of the individual positioning coordinates is calculated. ,in The Jacobian matrix of the reprojection error with respect to the pose parameters is used to estimate its uncertainty; simultaneously, the positioning confidence of matched feature point pairs from multiple observation UAVs is calculated. As shown in the following formula:

[0064] In the formula, The rank of the matrix represents the sum of the diagonal elements of the covariance matrix, which reflects the total error energy. Represents the natural exponential function; covariance matrix It involves three-dimensional spatial coordinates, which is a The matrix, where " " That is, the number of dimensions of the three-dimensional spatial coordinates. This represents the unit standard deviation used as the confidence decay benchmark, i.e., one unit length error. The estimated uncertainty and the calculated location confidence are both used as auxiliary criterion inputs for cluster fusion in step S4.

[0065] In step S4, during data transmission and synchronization, the observation drone sends its own positioning results, including WGS84 coordinates, confidence level, timestamp, and other relevant information. The transmission process uses the Protobuf protocol serialization method, and the data size is ≤1KB / frame. Time synchronization is based on the IEEE1588PTP protocol, so that the clock synchronization error of each drone is ≤1ms. The transmission frequency is 10Hz, that is, the individual positioning coordinate results are sent once every 100ms.

[0066] Subsequently, the individual positioning coordinates of the three observation drones were converted to UTM plane coordinates to eliminate the influence of geographic coordinate nonlinearity. Then, the DBSCAN algorithm was used for outlier detection. The neighborhood radius of this algorithm was set to 2.5m and the minimum number of points was 2, so that isolated points were marked as outliers and removed. Isolated points are feature points whose distance from other points is greater than 3m.

[0067] Based on the location confidence of the matching feature point pairs of multiple observation drones, the weights of each observation drone in the clustering and fusion process are calculated accordingly. As shown in the following formula:

[0068] In the formula, , That is, the first frame, first The location confidence of matching feature point pairs from the observation drone is calculated. The weighted clustering fusion process is performed using the following formula:

[0069] In the formula, , and These are the target coordinate data after fusion. coordinate, coordinates and coordinate, The coordinates correspond to the eastward direction of the world coordinate system or the X-axis of the UTM plane; For the first The weights corresponding to the observation drones; , and The first The individual positioning coordinates of the observation drone coordinate, coordinates and coordinate.

[0070] As a preferred embodiment, the average distance between the fused target coordinate data and the individual positioning coordinates of each observation UAV is calculated. As shown in the following formula:

[0071] In the formula, The fused target's three-dimensional coordinates, i.e. ; For the first The individual positioning coordinates of the observation drone. If the average distance If the calculated value is greater than 2m, the relocation process is triggered.

[0072] During the clustering fusion process, the observation UAV with the highest location confidence among the matched feature point pairs is used as the fusion node. At the same time, it is advisable to require the best communication quality, with a packet loss rate of <5%, so that it can receive the individual positioning coordinates and positioning reliability of the other observation drones.

[0073] Finally, in step S5, the local coordinate system of the UAV is set to the ENU coordinate system, which is East X, North Y, and Sky Z. The origin of this coordinate system is the current position of the UAV.

[0074] The drone uses its onboard obstacle detection radar, or a 16-line lidar with a maximum detection range of 100 meters, to determine the obstacle point cloud map within a 50-meter range centered on itself in real time, thereby planning and generating flight path instructions from its own position to the target position.

[0075] Starting from the current position of the drone, and with the position corresponding to the preset direction and distance of the real-time target coordinate data as the endpoint, for example, 100 meters directly above the target, the improved A-satellite algorithm with a path resolution of 1m is used to plan the path under the constraints of preset flight attitude conditions. The flight attitude conditions can be constrained in advance, for example, requiring the drone to have a maximum climb angle of 30°, a maximum dive angle of 45°, and a minimum turning radius of 50m.

[0076] During path planning and execution, the control law for the guidance process is determined using the proportional guidance method, thereby calculating control commands, heading angle commands, pitch angle commands, and velocity commands. As flight path commands are executed, fused target coordinate data is received simultaneously at an update frequency of 5Hz. When an obstacle is detected on the path based on the real-time obstacle point cloud map, for example, if the obstacle is less than 20m from the planned path, the RRT star algorithm is triggered to perform local path adjustment. The time spent on local path adjustment planning should be constrained to within 100ms.

[0077] Example 2 Based on Example 1, this example provides a UAV swarm cooperative localization and target guidance system for implementing the method of Example 1. The system includes the following functional modules: The prior map construction module is used to pre-collect image sequences and point cloud data of the target area at the task location, and construct a feature point cloud and descriptor library for the target area containing static feature points. The single-unit positioning module includes multiple observation drones. It is used to calculate and determine the drone pose data of each observation drone in real time based on the feature point cloud and descriptor library of the target area, as well as the target area images acquired in real time by the cameras of each observation drone, under the Reloc3r positioning framework. It is also used to establish the mapping relationship between the pixel coordinates in the real-time image of the target area and the coordinates of the preset world coordinate system, to obtain the corresponding target individual positioning coordinates based on the drone pose data, and at the same time to form the matching feature point pairs of each observation drone in the feature point cloud and descriptor library. The multi-drone fusion module is used to cluster and fuse the individual positioning coordinates and matching feature point pairs of multiple observation drones to obtain the target coordinate data at the mission location after fusion. The collaborative guidance module includes at least one execution drone, which receives target coordinate data and converts it into the execution drone's own local coordinate system, enabling the execution drone to plan and generate flight path instructions from its own position to the target position under the constraints of its own real-time determined obstacle point cloud map, and execute them.

[0078] For the functional module architecture relationships and collaboration logic of the system, please refer to [link / reference]. Figure 2 The diagram illustrates that the system is divided into four modules: "data preparation → single-machine processing → multi-machine fusion → execution control," which are sequentially dependent. In the system, the RTK-GPS mapping equipment carried by the observation UAV interacts with the prior map building module, providing the raw data for map construction; the three observation UAVs serve as the specific carriers of the single-machine positioning module, and the real-time images they collect are the basic input for positioning calculations; the real-time obstacle point cloud map is generated by the lidar of the attack UAV, and together with the fused target coordinate data, it serves as the input to the collaborative guidance module, thereby supporting path planning.

[0079] like Figure 3 As shown, the system, according to its function, can form four time-correlated data streams, ultimately yielding execution instructions, including: Prior map construction flow: point cloud / image raw data → SIFT feature extraction → SfM structure of motion recovery and bundle adjustment (BA) 3D reconstruction → ERASOR algorithm dynamic feature filtering → FAISS vector database storage (feature point cloud + descriptor); this data flow is used to generate static, high-precision prior maps, providing a benchmark reference for localization.

[0080] Single-unit localization stream: Observe real-time UAV images → YOLOv8 target detection and image cropping → Reloc3r framework extracts global / local features → Match with prior map features (FAISS global retrieval + RANSAC removes mismatches) → EPnP+LM algorithm solves translation vectors and rotation matrices → UTM coordinate transformation; This data stream transforms image information into UAV 3D pose data, providing single-unit observations for multi-unit fusion.

[0081] Multi-UAV Fusion Stream: Pose and target localization data of 3 UAVs → Unified coordinate system alignment → DBSCAN algorithm outlier detection → Confidence-based dynamic weight allocation → Weighted fusion calculation → Output fused target coordinate data; This data stream eliminates single-machine errors and outputs high-precision target coordinates consistent across the cluster.

[0082] Collaborative guidance flow: Fusion of target coordinate data + LiDAR obstacle point cloud → Improved A algorithm path planning → Proportional guidance method to generate control commands (speed, heading angle, etc.) → Real-time updates every 200ms and local replanning for obstacle avoidance using RRT algorithm; This data flow transforms target coordinates into executable obstacle avoidance paths and control commands.

[0083] like Figure 4 As shown, this embodiment provides a detailed description of the internal functional units, algorithm components, and data processing links of the stand-alone positioning module; these include: Image preprocessing unit: Optimizes 4K images acquired by observation drones, including locking regions of interest through YOLOv8 target detection, uniformizing the size to 512×512 through electronic scaling cropping, and improving the robustness of feature extraction after using CLAHE contrast enhancement.

[0084] The Reloc3r framework and feature matching unit generate a 128-dimensional global feature vector, which is used for global retrieval of the FAISS library, narrowing the matching range to 50m×50m. At the same time, it extracts local feature points at a rate of 800 per frame, matches them with prior map features using the FLANN matcher, and then uses the RANSAC algorithm to remove mismatches, retaining ≥30 pairs of inliers.

[0085] Pose calculation unit: Based on the matched feature point pairs, the initial pose containing the rotation matrix and translation vector is first solved by the EPnP algorithm. Then, the pose is refined by the Levenberg-Marquardt nonlinear optimization method with the reprojection error as the objective function, thereby obtaining the corresponding target individual positioning coordinates. The positioning error is constrained to within 1.5m.

[0086] Coordinate transformation unit: Through the corresponding calculation of the rotation matrix transpose and translation vector, the pose data in the camera coordinate system is transformed into the world coordinate system, and finally transformed into WGS84 geographic coordinates through UTM projection, thereby meeting the coordinate system unification requirements of multi-machine collaboration and providing a guarantee for subsequent multi-machine clustering fusion and obtaining target coordinate data.

[0087] All processing steps in this single-unit positioning module are implemented based on the NVIDIA Jetson AGX Orin processor carried by each observation UAV, which can ensure that the single-frame processing latency is ≤20ms, thereby meeting the real-time requirement of 30FPS.

Claims

1. A method for cooperative localization and target guidance of unmanned aerial vehicle (UAV) swarms, characterized in that, The drone swarm comprises multiple observation drones and at least one execution drone; the method includes the following steps: S1. At least one observation drone pre-collects image sequences and point cloud data of the target area at the mission location, and constructs a feature point cloud and descriptor library of the target area containing static feature points; S2. Multiple observation drones, based on the feature point cloud and descriptor library of the target area, as well as the target area images collected in real time by their own cameras, calculate and determine their own drone pose data in real time under the action of their respective Reloc3r positioning frameworks. S3. Multiple observation drones establish a mapping relationship between pixel coordinates in the real-time image of the target area and the preset world coordinate system coordinates based on their own drone pose data, and obtain their own individual positioning coordinates of the target. At the same time, they form matching feature point pairs of multiple observation drones in the feature point cloud and descriptor library. S4. Cluster and fuse the individual positioning coordinates and matching feature point pairs corresponding to multiple observation drones to obtain the target coordinate data at the mission location after fusion. S5. The UAV receives the target coordinate data and converts it into its own local coordinate system. Under the constraints of the obstacle point cloud map determined in real time, it plans and generates flight path instructions from its own position to the target position and executes them, thus completing the collaborative positioning and target guidance process.

2. The UAV swarm cooperative positioning and target guidance method according to claim 1, characterized in that: In step S1, the observation drone that collects image sequences and point cloud data of the target area is equipped with an RTK-GPS module. During its collection flight, multiple main routes and multiple verification routes are preset. On each route, the corresponding target area image is collected according to the preset trigger interval. After preprocessing the collected target area image, the SIFT algorithm is used to extract features, obtain the feature points in the image, and generate a descriptor of preset dimension. By using feature matching and observation of UAV camera pose estimation, the three-dimensional coordinates of feature points are reconstructed using triangulation to generate an initial point cloud of the target area. In the process of generating the initial point cloud, the bundle adjustment method is used for optimization. The objective function used for optimization is shown below: The objective function represents a minimization problem, where It is a double summation symbol. The first one representing the observation drone One camera, Represents the first in the image captured by the camera A three-dimensional spatial point, which is represented as the sum of the errors of all three-dimensional spatial points observed by all cameras; In the objective function, Representative at the In the images captured by the camera, the first... The actual observed pixel coordinates corresponding to each three-dimensional spatial point; Representing the The spatial coordinates of a three-dimensional point in a preset world coordinate system; Representing the Rotation matrix of each camera; Representing the Translation vectors of each camera; This is a perspective projection function used to project points in a 3D camera coordinate system onto a 2D image plane; symbol This represents the square of the L2 norm.

3. The UAV swarm cooperative positioning and target guidance method according to claim 2, characterized in that: After generating the initial point cloud of the target area, dynamic feature filtering and map optimization are performed on it. The ERASOR algorithm is used to perform consistency checks on multiple consecutive frames of point clouds. The point cloud is divided into grids of a preset size, and the scan ratio of each grid is calculated. Grids with scan ratios greater than a preset threshold are removed, thus eliminating dynamic feature point regions. After performing planar fitting on the remaining grids, feature points with residuals greater than a preset threshold in the fitted point cloud are further removed. According to different preset point cloud retention densities for ground feature points and building feature points, the corresponding point cloud data is stored in the FAISS vector database. At the same time, descriptors of a preset dimension generated by the SIFT algorithm are also stored, thereby constructing the feature point cloud and descriptor library of the target area.

4. The UAV swarm cooperative localization and target guidance method according to claim 1, characterized in that: In step S2, multiple observation drones preprocess the target area images acquired in real time by their cameras. First, a pre-deployed image detection model is used to detect and infer the target area images to obtain target detection boxes. Based on the preset input size requirements for target localization, the electronic zoom factor is calculated with the target center corresponding to the target detection box as the image center. A pixel area of ​​the corresponding size is cropped from the target area image and scaled to match the preset input size. Then, image contrast enhancement is performed, thus completing the preprocessing of the target area image, which serves as the input basis for the Reloc3r localization framework.

5. The UAV swarm cooperative positioning and target guidance method according to claim 1, characterized in that: In step S2, the UAV pose data includes translation vectors. and rotation matrix Within the Reloc3r localization framework, these two elements constitute the 6-DOF pose of the camera corresponding to the UAV pose data. After receiving the target region image as input, the Reloc3r localization framework extracts the image coordinates and corresponding descriptors, performs feature matching, searches for similar regions in the feature point cloud and descriptor library, and then uses the EPnP algorithm to solve for the translation vector. and rotation matrix This allows us to obtain the drone pose data of the observation drone that acquires real-time images of the target area.

6. The UAV swarm cooperative positioning and target guidance method according to claim 5, characterized in that: In step S3, the Reloc3r positioning framework reverse-engineers the three-dimensional position of the target based on the mapping relationship between pixel coordinates in the real-time image of the target area and the preset world coordinate system coordinates. First, the target depth information is calculated based on the camera pitch angle and observation altitude of the observation UAV. Then, coordinate transformation is performed based on the target depth information to transform the camera coordinate system to the world coordinate system. The world coordinate system is further transformed into WGS84 format for storage. The stored WGS84 coordinates are the individual positioning coordinates used for clustering and fusion in step S4. Calculate the covariance matrix of individual positioning coordinates and estimate its uncertainty. At the same time, calculate the positioning confidence of matching feature point pairs of multiple observation UAVs. The estimated uncertainty and the calculated positioning confidence are used as auxiliary criterion inputs for cluster fusion in step S4.

7. The UAV swarm cooperative localization and target guidance method according to claim 1, characterized in that: In step S4, after converting the individual positioning coordinates of multiple observation UAVs to UTM plane coordinates, the DBSCAN algorithm is used for outlier detection, marking isolated points as outliers and removing them. Subsequently, based on the location confidence of the matching feature point pairs of multiple observation UAVs, the weight of each observation UAV in the clustering fusion process is calculated, and the weighted clustering fusion process is performed using the following formula: In the formula, , and These are the target coordinate data after fusion. coordinate, coordinates and coordinate; For the first The weights corresponding to the observation drones; To observe the total number of drones; , and The first The individual positioning coordinates of the observation drone coordinate, coordinates and coordinate.

8. The UAV swarm cooperative positioning and target guidance method according to claim 7, characterized in that: Calculate the average distance between the fused target coordinate data and the individual positioning coordinates of each observation UAV. If the calculated average distance is greater than the preset relocation threshold, the relocation process is triggered. During the clustering fusion process, the observation UAV with the highest location confidence among the matching feature point pairs is used as the fusion node, which receives the individual location coordinates and location confidence of the other observation UAVs.

9. The UAV swarm cooperative localization and target guidance method according to claim 1, characterized in that: In step S5, the local coordinate system of the UAV is the ENU coordinate system; the UAV uses its own obstacle detection radar to determine the obstacle point cloud map within a preset range centered on itself in real time. When planning and generating flight path instructions from its own position to the target position, the current position of the executing UAV is taken as the starting point, and the position corresponding to the preset direction and distance of the real-time target coordinate data is taken as the ending point. The path planning is carried out under the constraints of preset flight attitude conditions, and the control law in the guidance process is determined by the proportional guidance method. As the flight path instructions are executed, when obstacles are detected on the path based on the real-time obstacle point cloud map, local path adjustment is triggered.

10. A UAV swarm cooperative localization and target guidance system for implementing the method of claim 1, characterized in that, The system includes the following functional modules: The prior map construction module is used to pre-collect image sequences and point cloud data of the target area at the task location, and construct a feature point cloud and descriptor library for the target area containing static feature points. The single-unit positioning module includes multiple observation drones. It is used to calculate and determine the drone pose data of each observation drone in real time based on the feature point cloud and descriptor library of the target area, as well as the target area images acquired in real time by the cameras of each observation drone, under the Reloc3r positioning framework. It is also used to establish the mapping relationship between the pixel coordinates in the real-time image of the target area and the coordinates of the preset world coordinate system, to obtain the corresponding target individual positioning coordinates based on the drone pose data, and at the same time to form the matching feature point pairs of each observation drone in the feature point cloud and descriptor library. The multi-drone fusion module is used to cluster and fuse the individual positioning coordinates and matching feature point pairs of multiple observation drones to obtain the target coordinate data at the mission location after fusion. The collaborative guidance module includes at least one execution drone, which receives target coordinate data and converts it into the execution drone's own local coordinate system, enabling the execution drone to plan and generate flight path instructions from its own position to the target position under the constraints of its own real-time determined obstacle point cloud map, and execute them.

Images