Unmanned aerial vehicle cluster positioning method and system cooperating with SLAM algorithm and distributed optimization

Abstract

The invention discloses an unmanned aerial vehicle cluster positioning method and system cooperating with an SLAM algorithm and distributed optimization. The method comprises the following steps: carrying out initialization and multi-source heterogeneous sensor data synchronization on an unmanned aerial vehicle cluster; performing multi-machine feature point matching and initial pose joint optimization on each unmanned aerial vehicle to construct an initial pose map; constructing a dynamic hierarchical communication topology for the unmanned aerial vehicle cluster; on the basis of hierarchical communication topology, two-stage pose map optimization is constructed, and a unified global scene is obtained; a lightweight deep learning model is utilized to detect and remove dynamic obstacles in a global scene in real time; computing resources of each unmanned aerial vehicle are dynamically allocated, and failure nodes are quickly taken over and predicted and recovered; and displaying the global scene and the track of the unmanned aerial vehicle cluster in real time through a three-dimensional visualization tool. According to the invention, through fusion of lightweight sensing, distributed optimization and dynamic resource scheduling, the positioning precision, the cooperation efficiency and the robustness of the unmanned aerial vehicle cluster in a complex dynamic environment are significantly improved.

Description

Technical Field

[0001] The present invention relates to the technical field of robot positioning, and particularly relates to a method and system for positioning an unmanned aerial vehicle (UAV) cluster that combines a collaborative Simultaneous Localization and Mapping (SLAM) algorithm with distributed optimization. Background Art

[0002] The collaborative positioning and mapping of UAV clusters aims to construct a globally consistent and highly accurate environmental scene in real time in an unknown environment by sharing sensing information among multiple UAVs, and at the same time complete the precise positioning of each UAV. With the wide application of unmanned platforms in fields such as industrial inspection, military reconnaissance, and smart cities, the cluster collaborative SLAM technology has become the core technology to support the autonomous execution of complex tasks. However, existing collaborative positioning and mapping methods still face many bottleneck problems: (1) Insufficient adaptability to dynamic environments: Traditional centralized SLAM architectures rely on high-bandwidth communication and central computing nodes, and cannot effectively cope with the high-frequency information update requirements during dynamic obstacle interference and local scene mutations, which easily leads to scene drift or inaccurate positioning. In addition, uncertain factors such as complex lighting and weather changes further exacerbate the noise interference of sensing data.

[0003] (2) Low efficiency of large-scale cluster collaboration: As the number of UAVs increases, existing optimization algorithms face the problem of multi-dimensional data fusion and pose graph optimization of the global scene, resulting in a non-linear increase in computational complexity. Relying solely on the graph optimization strategy of a single node is difficult to meet the real-time requirements of high-dynamic scenes, and there are communication bandwidth bottlenecks and single-point failure risks.

[0004] (3) Robustness challenges in distributed collaboration: Under bandwidth-limited or intermittent communication conditions, it is difficult to efficiently sparsify and align the local observation data of UAV clusters, which easily leads to the accumulation of multi-aircraft scene stitching errors.

[0005] Although certain progress has been made in recent years by introducing lightweight SLAM front-ends (such as visual-inertial odometry or lidar point cloud registration) and improving backend optimization algorithms (such as incremental non-linear optimization), existing methods still cannot balance the real-time performance, accuracy, and scalability of multi-aircraft collaboration. Especially when facing heterogeneous UAV clusters, there is still a lack of an efficient solution for the spatio-temporal synchronization and collaborative mapping consistency guarantee of heterogeneous sensors. Figure 1 Consistency guarantee still lacks an efficient solution.

[0006] In summary, to meet the real-time collaborative perception requirements of large-scale UAV swarms in dynamic unknown environments, it is urgent to design an efficient and robust distributed collaborative SLAM framework. This framework needs to break through the communication and computing bottlenecks of traditional architectures, integrate lightweight front-end perception, distributed optimization, and adaptive resource scheduling technologies to achieve low latency, high precision, and strong robustness in multi-UAV collaborative positioning and mapping in complex scenarios. Based on this goal, the present invention is proposed, providing a new technical path for the autonomous collaborative tasks of UAV swarms. Summary of the Invention

[0007] Aiming at the problems of poor dynamic adaptability, low collaborative efficiency, and insufficient communication robustness existing in the prior art, the present invention proposes a UAV swarm positioning method and system based on collaborative SLAM algorithms and distributed optimization. By constructing a lightweight front-end perception module, a dynamic hierarchical communication topology, and an incremental distributed optimization mechanism, efficient collaborative positioning of UAV swarms in dynamic environments and the construction of a globally consistent scene are realized.

[0008] In a first aspect, an embodiment of the present invention provides a UAV swarm positioning method based on collaborative SLAM algorithms and distributed optimization, which includes: S100, initialize the UAV swarm and synchronize multi-source heterogeneous sensor data.

[0009] S200, perform multi-UAV feature point matching and initial pose joint optimization between each UAV, and construct an initial pose graph through the VIO pre-integration and SVD decomposition algorithms.

[0010] S300, construct a dynamic hierarchical communication topology for the UAV swarm, dynamically elect a master node and divide communication subgroups, and process the transmission data of the hierarchical communication topology using adaptive block coding technology.

[0011] S400, based on the hierarchical communication topology, construct two-level pose graph optimization to obtain a unified global scene.

[0012] S500, use a lightweight deep learning model to detect and remove dynamic obstacles in the global scene in real time.

[0013] S600, based on load balancing, dynamically allocate computing resources for each UAV, and quickly take over and predict the recovery of failed nodes.

[0014] S700, real-time display the global scene and the trajectories of the UAV swarm through a 3D visualization tool, and synchronously monitor the positioning accuracy, communication delay, and load calculation.

[0015] Combined with the first aspect, the embodiments of the present invention provide a first possible implementation manner of the first aspect. Among them, in S100, the initialization of the UAV cluster and the synchronization of multi-source heterogeneous sensor data include: S110, externally calibrate the multi-source heterogeneous sensors and compensate for spatio-temporal offsets to establish a unified coordinate system.

[0016] S120, use a pose predictor based on Kalman filtering to perform multi-sensor data fusion on the multi-source heterogeneous sensors, and unify the sampling frequencies and time sequences of the multi-source heterogeneous sensors.

[0017] S130, each UAV performs environmental perception and clock synchronization through the multi-source heterogeneous sensors carried by itself.

[0018] S140, the multi-source heterogeneous sensors include at least one of a vision camera, a lidar, an inertial navigation system (IMU), and a millimeter-wave radar.

[0019] Combined with the first aspect, the embodiments of the present invention provide a second possible implementation manner of the first aspect. Among them, in S300, the construction of a dynamic hierarchical communication topology for the UAV cluster, the dynamic election of a master node, and the division of communication subgroups include: S310, construct a dynamic hierarchical communication topology with multiple subgroups for the UAV cluster.

[0020] S320, dynamically elect a master node in the hierarchical communication topology according to the relative distances, communication bandwidths, and task priorities among the UAVs.

[0021] S330, the master node is responsible for the fusion of the global scene and task scheduling.

[0022] S340, within each communication subgroup, the slave nodes perform local perception data generation and compressed transmission.

[0023] Combined with the first aspect, the embodiments of the present invention provide a third possible implementation manner of the first aspect. Among them, in S300, the processing of the transmission data of the hierarchical communication topology by using the adaptive block coding technology includes: S350, based on the local scene information constructed in the initial pose graph, evaluate the density distribution of the environmental features in each spatial region, divide the global scene into several transmission blocks, represent the sparse regions with low-resolution point clouds, and retain the complete feature descriptors in the dense regions.

[0024] S360, each transmission block is encoded by using a differential compression ratio strategy to obtain an encoded data packet, and the encoded data packet contains a hash check code.

[0025] In combination with the first aspect, an embodiment of the present invention provides a fourth possible implementation manner of the first aspect. In S400, the construction of a two-level pose graph optimization on the basis of the hierarchical communication topology to obtain a unified global scene includes: S410, the slave node uses the information matrix decomposition method to perform local graph optimization within the local sliding window and outputs a covariance matrix with confidence weights.

[0026] S420, the master node collects the pose estimates and covariance information uploaded by each slave node, parallelly solves the multi-robot global pose consistency constraint through the ADMM algorithm, filters dynamic interference, and feeds back the optimization result for the scene correction of the slave node to obtain a unified global scene.

[0027] In combination with the first aspect, an embodiment of the present invention provides a fifth possible implementation manner of the first aspect. In S500, the use of a lightweight deep learning model to detect and remove dynamic obstacles in the global scene in real time includes: S510, select a lightweight deep convolutional neural network model and use the fused multi-source perception data in the global scene as input.

[0028] S520, the output of the lightweight deep convolutional neural network model is an obstacle category probability map or a point cloud annotation result, and the point cloud or image area marked as dynamic is masked and filtered.

[0029] In combination with the first aspect, an embodiment of the present invention provides a sixth possible implementation manner of the first aspect. In S600, the dynamic allocation of computing resources for each drone based on load balancing includes: S610, in combination with the current processing task queue length, scene processing complexity, and communication status of each drone, perform real-time quota allocation of computing resources and communication bandwidth.

[0030] S620, when the confidence of the local scene is lower than a preset threshold, increase the resource priority of the associated drone and preferentially perform data feedback and pose optimization.

[0031] In combination with the first aspect, an embodiment of the present invention provides a seventh possible implementation manner of the first aspect. In S600, the quick takeover and predictive recovery of failed nodes includes: S630, when the edge communication of the slave node is interrupted, quickly take over the failed slave node, predict the current pose using the historical optimization result, and update the local scene.

[0032] S640, when a dense area of dynamic objects is detected, preferentially schedule the drone equipped with high-precision sensors to perform area revisit.

[0033] In a second aspect, an embodiment of the present invention further provides a UAV cluster positioning system that coordinates SLAM algorithms and distributed optimization, including: A master node server for performing global scene fusion and distributed optimization calculations.

[0034] UAV terminals equipped with heterogeneous sensors and edge computing modules for local feature extraction and pose optimization.

[0035] Communication relay devices for supporting dynamic block coding transmission and bandwidth adaptive allocation.

[0036] Combined with the second aspect, an embodiment of the present invention provides a first possible implementation manner of the second aspect, where the master node server includes: An initialization module for initializing the UAV cluster and synchronizing multi-source heterogeneous sensor data.

[0037] An initial pose graph construction module for performing multi-aircraft feature point matching and initial pose joint optimization between each of the UAVs, and constructing an initial pose graph through VIO pre-integration and SVD decomposition algorithms.

[0038] A hierarchical communication topology module for constructing a dynamic hierarchical communication topology for the UAV cluster, dynamically electing a master node and dividing communication subgroups, and processing the transmission data of the hierarchical communication topology using adaptive block coding technology.

[0039] A global scene unification module for constructing two-level pose graph optimization on the basis of the hierarchical communication topology to obtain a unified global scene.

[0040] A dynamic obstacle removal module for using a lightweight deep learning model to detect and remove dynamic obstacles in the global scene in real time.

[0041] A load balancing module for dynamically allocating computing resources for each UAV based on load balancing, and quickly taking over and predicting the recovery of failed nodes.

[0042] A real-time display module for real-time displaying the global scene and the trajectories of the UAV cluster through a 3D visualization tool, and synchronously monitoring positioning accuracy, communication latency, and load calculation.

[0043] The beneficial effects of the embodiments of the present invention are: (1) Through multi-source sensor fusion and dynamic obstacle filtering mechanisms, the present invention effectively copes with complex scenarios such as light changes and dynamic object interference, reduces the accumulation of positioning errors, and has strong adaptability to dynamic environments; (2) The present invention adopts a hierarchical optimization and distributed ADMM algorithm, reducing the cluster computing complexity to the order of O(NlogN) (N is the number of UAVs), supporting real-time mapping for clusters of up to a hundred UAVs, and significantly improving the cooperation efficiency. (3) Based on a dynamic block coding and edge caching strategy, the present invention ensures task continuity in a weak network environment and enhances communication robustness. (4) Through a unified spatio-temporal alignment interface, the present invention supports the plug-and-play deployment of different models of cameras, radars, and positioning modules, meeting the collaborative requirements of heterogeneous UAV clusters and having high compatibility for heterogeneous devices. Brief Description of the Drawings

[0044] To more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings required for use in the embodiments will be briefly introduced below. It should be understood that the following drawings only show some embodiments of the present invention and should not be regarded as limiting the scope. For those of ordinary skill in the art, other related drawings can be obtained based on these drawings without creative efforts.

[0045] Figure 1 It is a flowchart of the UAV cluster positioning method based on the collaborative SLAM algorithm and distributed optimization of the present invention. Detailed Embodiments

[0046] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are some, but not all, of the embodiments of the present invention. Usually, the components of the embodiments of the present invention described and shown in the accompanying drawings here can be arranged and designed in various different configurations.

[0047] Please refer to Figure 1, the first embodiment of the present invention provides a method for positioning an unmanned aerial vehicle (UAV) cluster by coordinating the SLAM algorithm and distributed optimization, which includes: S100, initializing the UAV cluster and synchronizing multi-source heterogeneous sensor data; S200, performing multi-UAV feature point matching and initial pose joint optimization among each of the UAVs, and constructing an initial pose graph through the VIO pre-integration and SVD decomposition algorithms; S300, constructing a dynamic hierarchical communication topology for the UAV cluster, dynamically electing a master node and dividing communication subgroups, and processing the transmission data of the hierarchical communication topology by using the adaptive block coding technology; S400, based on the hierarchical communication topology, constructing two-level pose graph optimization to obtain a unified global scene; S500, using a lightweight deep learning model to detect and remove dynamic obstacles in the global scene in real time; S600, based on load balancing, dynamically allocating the computing resources of each UAV, quickly taking over and predicting the recovery of failed nodes; S700, real-time displaying the global scene and the trajectory of the UAV cluster through a 3D visualization tool, and synchronously monitoring the positioning accuracy, communication delay, and load calculation.

[0048] Among them, in S100, the initialization of the UAV cluster and the synchronization of multi-source heterogeneous sensor data include: S110, externally calibrating the multi-source heterogeneous sensors and compensating for spatio-temporal offset to establish a unified coordinate system; S120, using a pose predictor based on the Kalman filter to perform multi-sensor data fusion on the multi-source heterogeneous sensors, and unifying the sampling frequencies and time sequences of the multi-source heterogeneous sensors; S130, each UAV performs environmental perception and clock synchronization through the multi-source heterogeneous sensors carried by itself; S140, the multi-source heterogeneous sensors include at least one of a visual camera, a lidar, an inertial navigation system (IMU), and a millimeter-wave radar.

[0049] Among them, the configuration of the communication protocol includes deploying a wireless ad-hoc network based on IEEE 802.11n, setting a bandwidth priority policy, with the minimum delay of critical data packets ≤ 20 ms, and using a double-check mechanism (CRC16 + parity check bit) to ensure reliable transmission.

[0050] The multi-sensor data fusion includes, aiming at the timing difference between a 30 Hz camera and a 200 Hz IMU, using a prediction compensation method based on linear interpolation to generate synchronous timestamps.

[0051] Through this step, the problem of timestamp jitter of heterogeneous sensor data can be solved, and the ghosting caused by timing misalignment during SLAM mapping can be avoided. The Python code for linearly interpolating and synchronizing IMU data is shown as follows: def interpolate_imu(cam_time, imu_queue): # Find the previous and next IMU samples in the queue for interpolation prev_imu = next(imu for imu in reversed(imu_queue) if imu.timestamp <= cam_time) next_imu = next(imu for imu in imu_queue if imu.timestamp >= cam_time) ratio = (cam_time - prev_imu.timestamp) / (next_imu.timestamp - prev_imu.timestamp) # Interpolate angular velocity and acceleration interpolated_gyro = (1 - ratio) * prev_imu.gyro + ratio * next_imu.gyro interpolated_accel = (1 - ratio) * prev_imu.accel + ratio * next_imu.accel return interpolated_gyro, interpolated_accel Calculate the extrinsic parameter matrix between the camera and the lidar by the checkerboard calibration method to align the point cloud and the pixel coordinate system; the dynamic obstacle feature extraction includes, combining the ORB feature point optical flow tracking results, filtering the regions where the optical flow magnitude exceeds the set threshold, such as the regions with a speed > 1 m / s, and determining them as dynamic target candidate regions.

[0052] The purpose of this step is to quickly screen the dynamic regions through the optical flow magnitude to reduce the interference of the static background on the SLAM scene. The Python code example for filtering by the optical flow threshold is as follows: def filter_dynamic_regions(flow, velocity_threshold=1.0): # Calculate the optical flow velocity magnitude of each pixel (unit: pixel / frame) magnitude = np.sqrt(flow[...,0]**2 + flow[...,1]**2) # Assume the camera frame rate is 30Hz and convert it to m / s (need to be calibrated according to the actual pixel scale) velocity_map = magnitude * 0.02 # Example scale: 1 pixel = 0.02m mask = velocity_map > velocity_threshold return cv2.connectedComponentsWithStats(mask.astype(np.uint8))[1]> 0 Among them, the initialization of the single-machine front-end perception module includes using VIO pre-integration to fuse visual and IMU data, parameterizing the pose through the Lie group SE(3), improving the efficiency of sliding window optimization, and initializing the odometer error within 0.2m horizontally and 0.1m vertically.

[0053] The initial alignment in the multi-machine scenario includes matching cross-machine feature points based on the Hamming distance of ORB descriptors. When the number of matching point pairs ≥ 15 groups, trigger the SVD to solve the initial pose transformation matrix, and remove the abnormal matches through RANSAC.

[0054] This step establishes a global coordinate system through cross-machine feature point matching to avoid overlapping conflicts caused by scene misalignment between multiple robots. The C++ code example for removing abnormal matches by RANSAC is as follows: void ransac_filter(std::vector <cv::point2f>& src_points, std::vector <cv::point2f>& dst_points, double threshold = 3.0) { cv::Mat mask; cv::findHomography(src_points, dst_points, cv::RANSAC, threshold, mask); / / Keep inliers std::vector <cv::point2f>inliers_src, inliers_dst; for (int i = 0; i < mask.rows; i++) { if (mask.at <uchar>(i)) { inliers_src.push_back(src_points[i]); inliers_dst.push_back(dst_points[i]); } } src_points = inliers_src; dst_points = inliers_dst; } The filtering optimization preprocessing includes constructing a joint optimization objective function that includes visual reprojection error and inertial error, initializing the outlier weights of the matching point pairs to 0.1, and retaining the constraints with high confidence for subsequent optimization.

[0055] Among them, in S300, the construction of a dynamic hierarchical communication topology for the UAV cluster and the dynamic election of the master node and division of communication subgroups include: S310, constructing a dynamic multi-subgroup hierarchical communication topology for the UAV cluster; S320, dynamically electing the master node in the hierarchical communication topology according to the relative distance, communication bandwidth, and task priority between the UAVs; S330, the master node is responsible for the fusion of the global scene and task scheduling; S340, inside each communication subgroup, the slave nodes execute local perception data generation and compressed transmission.

[0056] Among them, in S300, the processing of the transmission data of the hierarchical communication topology by using the adaptive block coding technology includes: S350, based on the local scene information constructed in the initial pose graph, evaluating the density distribution of the environmental features of each spatial region, dividing the global scene into several transmission blocks, representing the sparse regions with low-resolution point clouds to reduce redundant data, and retaining the complete feature descriptors in the dense regions to ensure the perception accuracy of the key regions; S360, each transmission block is encoded by using a differential compression ratio strategy to obtain an encoded data packet, and the encoded data packet contains a hash check code to support the local data retransmission request in the case of packet loss.

[0057] Among them, the master node election and subgroup division include real-time updating of the node scores based on a hybrid evaluation function. When the master node goes offline or the score is lower than the threshold, such as when the remaining battery power < 20%, the Bully algorithm is triggered to re-elect the master node; the subgroup uses the Ripple protocol to spread the topology change information.

[0058] The dynamic master node election is to avoid single point failure and ensure rapid broadcast to the cluster when the topology changes. The Python example of the Bully algorithm election logic is as follows: class Node: def __init__(self, id, battery): self.id = id self.battery = battery def bully_election(nodes): candidates = [node for node in nodes if node.battery >= 20] if not candidates: return None # Select the node with the largest ID as the leader node leader = max(candidates, key=lambda x: x.id) return leader.id Adaptive data compression and transmission includes dividing blocks according to the density of lidar point clouds, with a grid size of 1m × 1m. Sparse blocks have less than 50 points per grid and are compressed to 30% of the original data volume using Run-Length Encoding. Dense blocks retain the original accuracy, and data packets are attached with a hash check code (SHA-256) to ensure integrity.

[0059] Among them, in S400, on the basis of the hierarchical communication topology, constructing a two-level pose graph optimization to obtain a unified global scene includes: S410, the slave node uses the information matrix decomposition method to perform local graph optimization within the local sliding window and outputs a covariance matrix with confidence weights; S420, the master node collects the pose estimates and covariance information uploaded by each slave node, parallelly solves the multi-robot global pose consistency constraint through the ADMM algorithm, filters dynamic interference, and returns the optimization result for scene correction of the slave node to obtain a unified global scene.

[0060] Among them, local pose graph optimization includes adopting a sliding window mechanism with a window size of 10 frames, using sparse Cholesky decomposition to accelerate the Jacobian matrix operation, and only updating the affected variables when a new frame is added; global optimization includes the master node merging subgraphs through the ADMM algorithm, and the slave nodes alternately optimize local variables and submit them to the master node for conflict detection. The global optimization frequency is set to 5Hz to balance the computational load; outlier suppression includes reweighting the outliers whose visual matching residuals exceed 3 times the σ standard deviation, and the weights are iteratively adjusted according to the exponential decay function.

[0061] The purpose of this step is to reduce the impact of incorrect matching pairs on optimization through iterative weighting and improve the global scene consistency.

[0062] An example of Python code for adjusting the weights of residual outliers is as follows: def reweight_outliers(residuals, sigma): weights = np.ones_like(residuals) outlier_mask = np.abs(residuals) > 3 * sigma # Exponentially decaying weights: The weights of outliers gradually decrease from 0.1 weights[outlier_mask] = 0.1 * np.exp(-0.5 * (residuals[outlier_mask] / sigma)**2) return weights Among them, in S500, using the lightweight deep learning model to detect dynamic obstacles in the global scene in real time and remove them includes: S510, selecting a lightweight deep convolutional neural network model and using the fused multi-source perception data in the global scene as the input; S520, the output of the lightweight deep convolutional neural network model is the obstacle category probability map or the point cloud annotation result, and the point cloud or image area marked as dynamic is filtered by masking.

[0063] Among them, object detection includes the lightweight YOLOv5s network, with a model size of 14MB, which outputs the 2D bounding boxes of dynamic targets in real time, calculates the depth through binocular disparity, and the positioning accuracy error is <0.5m.

[0064] The lightweight model ensures real-time processing on edge devices, and binocular disparity improves the positioning accuracy of dynamic targets. An example of Python code for binocular depth calculation: def compute_depth(bbox_left, bbox_right, focal_length, baseline): # Calculate the disparity (taking the center point of the bounding box as an example) disparity = abs(bbox_left.center_x - bbox_right.center_x) depth = (focal_length * baseline) / max(disparity, 1e-5) # Avoid division by zero return depth Trajectory prediction includes fitting a polynomial model to the target motion trajectory within 5 consecutive frames, predicting the position probability distribution within the next 3 seconds, and a 95% confidence ellipse; Incremental update includes that the master node regularly triggers the merging of scene versions at intervals of 5 seconds, and removes obstacle data that has not been updated for more than 15 seconds according to the timestamp.

[0065] Among them, in S600, the dynamic allocation of computing resources for each of the drones based on load balancing includes: S610, performing real-time quota allocation of computing resources and communication bandwidth by combining the current processing task queue length, scene processing complexity, and communication status of each drone; S620, when the confidence level of the local scene is lower than a preset threshold, raising the resource priority of the associated drone and preferentially performing data backhaul and pose optimization.

[0066] Among them, in S600, the quick takeover and predictive recovery of the failed node includes: S630, when the edge communication of the slave node is interrupted, quickly taking over the failed slave node, predicting the current pose using the historical optimization results, and updating the local scene; S640, when a dense area of dynamic objects is detected, preferentially scheduling the drones equipped with high-precision sensors to perform area revisit.

[0067] This step avoids the loss of key frames in the scene through historical data interpolation and effectively improves the fault tolerance of the system. B-spline trajectory interpolation C++ code example: #include <bezier.h> std::vector <point3d>spline_interpolation(const std::vector <point3d>&points, int n = 10) { Bezier::Spline spline; spline.setControlPoints(points); return spline.eval(n); / / Generate interpolation points } Among them, task allocation includes real - time monitoring of the drone load, CPU utilization threshold of 70%, memory threshold of 80%, preferentially scheduling the node closest to the target point with sufficient resources, and the task allocation response delay in the dangerous area < 200ms; failure recovery includes that when adjacent nodes take over the failed area, interpolating and complementing the lost frames based on historical trajectory data, using the B - spline interpolation algorithm, and writing the interpolation result into the global scene after verification by the master node.

[0068] Among them, visualization includes rendering the global point cloud scene (Octomap format), the real - time trajectory of the drone (RGB color - coded), and the dynamic obstacle movement direction arrow markers through RViz2; performance monitoring includes recording key indicators such as communication delay (average < 50ms), positioning error (root - mean - square error < 0.15m), and mapping integrity (coverage rate ≥ 95%), and supports exporting logs in CSV format for offline analysis.

[0069] The second embodiment of the present invention provides a UAV swarm positioning system that combines collaborative SLAM algorithm and distributed optimization, which includes: a master node server for performing global scene fusion and distributed optimization calculations; UAV terminals equipped with heterogeneous sensors and edge computing modules for local feature extraction and pose optimization; communication relay devices for supporting dynamic block - coded transmission and bandwidth - adaptive allocation.

[0070] Among them, the master node server includes: an initialization module for initializing the UAV cluster and synchronizing multi-source heterogeneous sensor data; an initial pose graph construction module for performing multi-UAV feature point matching and initial pose joint optimization between each UAV, and constructing an initial pose graph through the VIO pre-integration and SVD decomposition algorithms; a hierarchical communication topology module for constructing a dynamic hierarchical communication topology for the UAV cluster, dynamically electing a master node and dividing communication subgroups, and processing the transmission data of the hierarchical communication topology using an adaptive block coding technique; a global scene unification module for constructing two-level pose graph optimization on the basis of the hierarchical communication topology to obtain a unified global scene; a dynamic obstacle removal module for using a lightweight deep learning model to detect and remove dynamic obstacles in the global scene in real time; a load balancing module for dynamically allocating computing resources for each UAV based on load balancing, quickly taking over failed nodes and predicting recovery; a real-time display module for real-time displaying the global scene and the trajectories of the UAV cluster through a 3D visualization tool, and synchronously monitoring the positioning accuracy, communication delay, and load calculation.

[0071] The computer program product of the UAV cluster positioning method and device based on the collaborative SLAM algorithm and distributed optimization provided by the embodiments of the present invention includes a computer-readable storage medium storing program code, and the instructions included in the program code can be used to execute the method in the foregoing method embodiments. For specific implementation, reference can be made to the method embodiments, which will not be elaborated here.

[0072] Specifically, the storage medium can be a general storage medium, such as a mobile disk, a hard disk, etc. When the computer program on the storage medium is run, it can execute the above-mentioned UAV cluster positioning method based on the collaborative SLAM algorithm and distributed optimization, thereby significantly improving the positioning accuracy, collaborative efficiency, and system robustness of the UAV cluster in a complex dynamic environment.

[0073] When the above-mentioned functions are implemented in the form of software functional units and sold or used as independent products, they can be stored in a non-volatile computer-readable storage medium executable by a processor. Based on such an understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or a part of the technical solution, can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes several instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the methods described in various embodiments of the present invention. The foregoing storage medium includes: various media such as USB flash drives, mobile hard disks, read-only memories (ROMs), random access memories (RAMs), magnetic disks, or optical discs that can store program codes.

[0074] Finally, it should be noted that the above-mentioned embodiments are only specific implementation manners of the present invention, used to illustrate the technical solutions of the present invention, rather than limiting them. The protection scope of the present invention is not limited thereto. Although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that: any person skilled in the art within the technical scope disclosed by the present invention can still modify the technical solutions recorded in the foregoing embodiments or easily conceive of changes, or perform equivalent replacements on some of the technical features; and these modifications, changes, or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present invention, and should all be covered by the protection scope of the present invention. Therefore, the protection scope of the present invention should be subject to the protection scope of the claims. < / uchar>

Claims

1. A method for positioning an unmanned aerial vehicle (UAV) cluster that coordinates a Simultaneous Localization and Mapping (SLAM) algorithm and distributed optimization, characterized in that, Including: S100, initializing the UAV swarm and synchronizing multi-source heterogeneous sensor data; S200, performing multi-UAV feature point matching and initial pose joint optimization between each of the UAVs, and constructing an initial pose graph through the VIO pre-integration and SVD decomposition algorithms; S300, constructing a dynamic hierarchical communication topology for the UAV swarm, dynamically electing a master node and dividing communication subgroups, and processing the transmission data of the hierarchical communication topology using an adaptive block coding technique; S400, constructing two-level pose graph optimization on the basis of the hierarchical communication topology to obtain a unified global scene; S500, using a lightweight deep learning model to detect and remove dynamic obstacles in the global scene in real time; S600, dynamically allocating computing resources for each of the UAVs based on load balancing, and quickly taking over and predicting the recovery of failed nodes; S700, real-time displaying the global scene and the trajectories of the UAV swarm through a 3D visualization tool, and synchronously monitoring the positioning accuracy, communication delay, and load calculation.

2. The collaborative SLAM algorithm and distributed optimization-based UAV cluster positioning method according to claim 1, characterized in that In S100, the initializing the UAV swarm and synchronizing multi-source heterogeneous sensor data includes: S110, externally calibrating the multi-source heterogeneous sensors and compensating for spatio-temporal offsets, and establishing a unified coordinate system; S120, using a pose predictor based on Kalman filtering to perform multi-sensor data fusion on the multi-source heterogeneous sensors, and unifying the sampling frequencies and time sequences of the multi-source heterogeneous sensors; S130, each UAV performs environmental perception and clock synchronization through the multi-source heterogeneous sensors carried by itself; S140, the multi-source heterogeneous sensors include at least one of a vision camera, a lidar, an inertial navigation system (IMU), and a millimeter-wave radar.

3. The collaborative SLAM algorithm and distributed optimization-based UAV cluster positioning method according to claim 1, wherein In S300, the constructing a dynamic hierarchical communication topology for the UAV swarm, dynamically electing a master node and dividing communication subgroups includes: S310, constructing a dynamic multi-subgroup hierarchical communication topology for the UAV swarm; S320, dynamically electing a master node in the hierarchical communication topology according to the relative distances, communication bandwidths, and task priorities between the UAVs; S330, the master node is responsible for the fusion of the global scene and task scheduling; S340, within each communication subgroup, the slave nodes perform local perception data generation and compressed transmission.

4. The collaborative SLAM algorithm and distributed optimization-based UAV cluster positioning method according to claim 3, wherein In S300, the processing the transmission data of the hierarchical communication topology using an adaptive block coding technique includes: S350, based on the local scene information constructed in the initial pose graph, evaluating the density distribution of the environmental features in each spatial region, dividing the global scene into several transmission blocks, representing the sparse regions with low-resolution point clouds, and retaining complete feature descriptors in the dense regions; S360, each of the transmission blocks is encoded using a differential compression ratio strategy to obtain an encoded data packet, and the encoded data packet contains a hash check code.

5. The collaborative SLAM algorithm and distributed optimization-based UAV cluster positioning method according to claim 3, characterized in that, In S400, the constructing two-level pose graph optimization on the basis of the hierarchical communication topology to obtain a unified global scene includes: S410. The slave node uses the information matrix decomposition method to perform local graph optimization within the local sliding window and outputs a covariance matrix with confidence weights. S420. The master node collects the pose estimates and covariance information uploaded by each slave node, parallelly solves the multi-robot global pose consistency constraint through the ADMM algorithm, filters dynamic interference, and transmits the optimization result back for the scene correction of the slave node to obtain a unified global scene.

6. The collaborative SLAM algorithm and distributed optimization-based UAV cluster positioning method according to claim 3, characterized in that In S500, the use of the lightweight deep learning model to detect and remove dynamic obstacles in the global scene in real time includes: S510. Select a lightweight deep convolutional neural network model and use the fused multi-source perception data in the global scene as the input. S520. The output of the lightweight deep convolutional neural network model is an obstacle category probability map or a point cloud annotation result, and the point cloud or image regions marked as dynamic are masked and filtered.

7. The collaborative SLAM algorithm and distributed optimization-based UAV cluster positioning method according to claim 4, wherein, In S600, the dynamic allocation of computing resources for each drone based on load balancing includes: S610. Combine the current processing task queue length, scene processing complexity, and communication status of each drone to perform real-time quota allocation of computing resources and communication bandwidth. S620. When the confidence of the local scene is lower than the preset threshold, increase the resource priority of the associated drone and give priority to data transmission and pose optimization.

8. The collaborative SLAM algorithm and distributed optimization-based UAV cluster positioning method according to claim 7, characterized in that, In S600, the rapid takeover and predictive recovery of failed nodes includes: S630. When the edge communication of the slave node is interrupted, quickly take over the failed slave node, predict the current pose using the historical optimization result, and update the local scene. S640. When a dense area of dynamic objects is detected, preferentially schedule the drones equipped with high-precision sensors to perform area revisit.

9. A UAV swarm positioning system that combines SLAM algorithm and distributed optimization, characterized in that, Including: A master node server for performing global scene fusion and distributed optimization calculations. Drone terminals equipped with heterogeneous sensors and edge computing modules for local feature extraction and pose optimization. Communication relay devices for supporting dynamic block coding transmission and bandwidth adaptive allocation.

10. The collaborative SLAM algorithm and distributed optimization-based UAV cluster positioning system according to claim 9, wherein, The master node server includes: An initialization module for initializing the drone cluster and synchronizing multi-source heterogeneous sensor data. An initial pose graph construction module for performing multi-robot feature point matching and initial pose joint optimization between each drone, and constructing an initial pose graph through the VIO pre-integration and SVD decomposition algorithms. A hierarchical communication topology module for constructing a dynamic hierarchical communication topology for the drone cluster, dynamically electing a master node and dividing communication subgroups, and processing the transmission data of the hierarchical communication topology using adaptive block coding technology. A global scene unification module for constructing two-level pose graph optimization based on the hierarchical communication topology to obtain a unified global scene. A dynamic obstacle removal module for using a lightweight deep learning model to detect and remove dynamic obstacles in the global scene in real time. A load balancing module for dynamically allocating computing resources for each drone based on load balancing, quickly taking over failed nodes, and performing predictive recovery. A real-time display module, which is used to display the global scene and the trajectories of the UAV cluster in real time through a 3D visualization tool, and synchronously monitor the positioning accuracy, communication latency, and load calculation.

Images