UAV cluster positioning method and system based on collaborative SLAM algorithm and distributed optimization

Abstract

The present invention discloses a method and system for positioning a cluster of unmanned aerial vehicles (UAVs) using a collaborative SLAM algorithm and distributed optimization, comprising: initializing the UAV cluster and synchronizing data from multi-source heterogeneous sensors; performing multi-machine feature point matching and initial pose joint optimization on each UAV to construct an initial pose graph; constructing a dynamic hierarchical communication topology for the UAV cluster; building a two-level pose graph optimization based on the hierarchical communication topology to obtain a unified global scene; utilizing a lightweight deep learning model to detect and remove dynamic obstacles in the global scene in real time; dynamically allocating computing resources for each UAV, quickly taking over and predicting recovery for failed nodes; and displaying the global scene and the trajectory of the UAV cluster in real time using a three-dimensional visualization tool. By integrating lightweight perception, distributed optimization, and dynamic resource scheduling, the present invention significantly improves the positioning accuracy, collaborative efficiency, and system robustness of UAV clusters in complex dynamic environments.

Description

Technical Field

[0001] The present invention relates to the field of robot positioning technology, and in particular to a drone cluster positioning method and system that collaborates with a SLAM algorithm and distributed optimization. Background Art

[0002] Collaborative localization and mapping of drone swarms aims to build a globally consistent, high-precision scene in an unknown environment in real time by sharing sensory information among multiple drones, while also achieving precise positioning of each drone. With the widespread application of unmanned platforms in industrial inspection, military reconnaissance, smart cities, and other fields, collaborative SLAM technology has become a core technology supporting the autonomous execution of complex tasks. However, existing collaborative localization and mapping methods still face many bottlenecks:

[0003] (1) Insufficient adaptability to dynamic environments: Traditional centralized SLAM architectures rely on high-bandwidth communications and central computing nodes, and are unable to effectively cope with the high-frequency information update requirements caused by dynamic obstacle interference and local scene mutations, which can easily lead to scene drift or positioning inaccuracy. In addition, uncertain factors such as complex lighting and weather changes further exacerbate the noise interference of perception data.

[0004] (2) Inefficient large-scale cluster collaboration: As the number of drones increases, existing optimization algorithms need to handle multi-dimensional data fusion and pose graph optimization problems in global scenarios, resulting in a nonlinear increase in computational complexity. Graph optimization strategies that rely solely on a single node are unable to meet the real-time requirements of highly dynamic scenarios and are subject to communication bandwidth bottlenecks and single-point failure risks.

[0005] (3) Robustness challenges of distributed collaboration: Under bandwidth-limited or intermittent communication conditions, it is difficult to achieve efficient sparsification and consistent alignment of local observation data of a drone cluster, which can easily lead to the accumulation of errors in the splicing of multi-machine scenes.

[0006] Although some progress has been made in recent years by introducing lightweight SLAM front-ends (such as point cloud registration based on visual inertial odometry or lidar) and improving back-end optimization algorithms (such as incremental nonlinear optimization), existing methods still cannot take into account the real-time, accuracy and scalability of multi-machine collaboration. Especially when facing heterogeneous UAV clusters, the spatiotemporal synchronization and collaborative construction of heterogeneous sensors are crucial. Figure 1 There is still a lack of efficient solutions for consistency assurance.

[0007] In summary, to meet the real-time collaborative perception needs of large-scale drone swarms in dynamic, unknown environments, there is an urgent need to design an efficient and robust distributed collaborative SLAM framework. This framework must overcome the communication and computational bottlenecks of traditional architectures and integrate lightweight front-end perception, distributed optimization, and adaptive resource scheduling technologies to achieve low latency, high accuracy, and strong robustness for multi-machine collaborative localization and mapping in complex scenarios. This present invention is based on this goal and provides a new technical approach for autonomous collaborative tasks in drone swarms. Summary of the Invention

[0008] In response to the problems of poor dynamic adaptability, low collaborative efficiency and insufficient communication robustness in the existing technology, the present invention proposes a drone cluster positioning method and system based on collaborative SLAM algorithm and distributed optimization. By constructing a lightweight front-end perception module, dynamic hierarchical communication topology and incremental distributed optimization mechanism, efficient collaborative positioning and global consistent scene construction of drone clusters in dynamic environments are achieved.

[0009] In a first aspect, an embodiment of the present invention provides a method for positioning a drone cluster using a collaborative SLAM algorithm and distributed optimization, which includes:

[0010] S100 initializes the drone cluster and synchronizes multi-source heterogeneous sensor data.

[0011] S200, performing multi-machine feature point matching and initial pose joint optimization on each of the UAVs, and constructing an initial pose graph through VIO pre-integration and SVD decomposition algorithm.

[0012] S300, constructing a dynamic hierarchical communication topology for the drone cluster, dynamically electing a master node and dividing the communication subgroups, and using adaptive block coding technology to process the transmission data of the hierarchical communication topology.

[0013] S400: Based on the hierarchical communication topology, a two-level pose graph optimization is constructed to obtain a unified global scene.

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

[0015] S600, based on load balancing, dynamically allocates computing resources for each of the drones, and quickly takes over and predictively recovers failed nodes.

[0016] S700 displays the global scene and the trajectory of the drone cluster in real time through a three-dimensional visualization tool, and simultaneously monitors positioning accuracy, communication delay, and load calculation.

[0017] In combination with the first aspect, an embodiment of the present invention provides a first possible implementation of the first aspect, wherein in S100, initializing the drone cluster and synchronizing multi-source heterogeneous sensor data includes:

[0018] S110 , performing external calibration and time-space offset compensation on the multi-source heterogeneous sensors to establish a unified coordinate system.

[0019] S120 , using a Kalman filter-based pose predictor to perform multi-sensor data fusion on the multi-source heterogeneous sensors, and unify the sampling frequencies and timings of the multi-source heterogeneous sensors.

[0020] S130: Each of the drones performs environment perception and clock synchronization through the multi-source heterogeneous sensors carried by it.

[0021] S140, the multi-source heterogeneous sensor includes at least one of a visual camera, a lidar, an inertial navigation system (IMU), and a millimeter-wave radar.

[0022] In combination with the first aspect, an embodiment of the present invention provides a second possible implementation of the first aspect, wherein, in S300, the step of constructing a dynamic hierarchical communication topology for the drone cluster, dynamically electing a master node, and dividing the communication subgroups includes:

[0023] S310, constructing a dynamic multi-subgroup hierarchical communication topology for the drone cluster.

[0024] S320, dynamically electing a master node in the hierarchical communication topology according to the relative distance between the drones, the communication bandwidth, and the task priority.

[0025] S330, the master node is responsible for the integration of global scenes and task scheduling.

[0026] S340, within each communication subgroup, the slave node performs local sensing data generation and compression transmission.

[0027] In combination with the first aspect, an embodiment of the present invention provides a third possible implementation of the first aspect, wherein, in S300, the processing of the transmission data of the layered communication topology using the adaptive block coding technology includes:

[0028] S350, based on the local scene information constructed in the initial pose graph, evaluate the density distribution of environmental features in each spatial region, divide the global scene into several transmission blocks, use low-resolution point cloud representation for sparse areas, and retain complete feature descriptors for dense areas.

[0029] S360: Each of the transmission blocks is encoded using a differentiated compression ratio strategy to obtain an encoded data packet, wherein the encoded data packet includes a hash check code.

[0030] In combination with the first aspect, an embodiment of the present invention provides a fourth possible implementation of the first aspect, wherein in S400, constructing a two-level pose graph optimization based on the hierarchical communication topology to obtain a unified global scene includes:

[0031] S410: The slave node performs local graph optimization in a local sliding window using an information matrix decomposition method, and outputs a covariance matrix with confidence weights.

[0032] S420, the master node collects the pose estimation and covariance information uploaded by each slave node, solves the global pose consistency constraints of multiple machines in parallel through the ADMM algorithm, filters dynamic interference, and transmits the optimization results back for scene correction of the slave nodes to obtain a unified global scene.

[0033] In combination with the first aspect, an embodiment of the present invention provides a fifth possible implementation of the first aspect, wherein in S500, using a lightweight deep learning model to detect and remove dynamic obstacles in the global scene in real time includes:

[0034] S510: Select a lightweight deep convolutional neural network model and use the fused multi-source perception data in the global scene as input.

[0035] S520: The output of the lightweight deep convolutional neural network model is an obstacle category probability map or a point cloud annotation result, and mask filtering is performed on the point cloud or image area marked as dynamic.

[0036] In combination with the first aspect, an embodiment of the present invention provides a sixth possible implementation of the first aspect, wherein in S600, dynamically allocating computing resources for each drone based on load balancing includes:

[0037] S610, combines the current processing task queue length, scene processing complexity and communication status of each UAV to allocate computing resources and communication bandwidth in real time.

[0038] S620: When the confidence level of the local scene is lower than a preset threshold, the resource priority of the associated UAV is increased, and data transmission and posture optimization are performed first.

[0039] In combination with the first aspect, an embodiment of the present invention provides a seventh possible implementation of the first aspect, wherein in S600, the fast takeover and predictive recovery of the failed node includes:

[0040] S630: When the edge communication of the slave node is interrupted, quickly take over the failed slave node, use historical optimization results to predict the current posture, and update the local scene.

[0041] S640: When an area with dense dynamic objects is detected, the drone equipped with a high-precision sensor is preferentially dispatched to revisit the area.

[0042] In a second aspect, an embodiment of the present invention further provides a drone cluster positioning system that collaborates with a SLAM algorithm and distributed optimization, which includes:

[0043] The master node server is used to perform global scene fusion and distributed optimization calculations.

[0044] The drone terminal is equipped with heterogeneous sensors and edge computing modules for local feature extraction and posture optimization.

[0045] Communication relay equipment used to support dynamic block coding transmission and adaptive bandwidth allocation.

[0046] In combination with the second aspect, an embodiment of the present invention provides a first possible implementation of the second aspect, wherein the master node server includes:

[0047] The initialization module is used to initialize the drone cluster and synchronize multi-source heterogeneous sensor data.

[0048] The initial pose graph construction module is used to perform multi-machine feature point matching and initial pose joint optimization between each of the drones, and construct the initial pose graph through VIO pre-integration and SVD decomposition algorithm.

[0049] The hierarchical communication topology module is used to construct a dynamic hierarchical communication topology for the drone cluster, dynamically elect a master node and divide the communication subgroups, and use adaptive block coding technology to process the transmission data of the hierarchical communication topology.

[0050] The global scene unification module is used to construct a two-level pose graph optimization based on the hierarchical communication topology to obtain a unified global scene.

[0051] The dynamic obstacle removal module is used to detect and remove dynamic obstacles in the global scene in real time using a lightweight deep learning model.

[0052] The load balancing module is used to dynamically allocate computing resources for each drone based on load balancing, and to quickly take over and predictively recover failed nodes.

[0053] A real-time display module is used to display the global scene and the trajectory of the drone cluster in real time through a three-dimensional visualization tool, and to simultaneously monitor positioning accuracy, communication delay, and load calculation.

[0054] The beneficial effects of the embodiments of the present invention are:

[0055] (1) The present invention effectively copes with complex scenarios such as illumination changes and dynamic object interference through multi-source sensor fusion and dynamic obstacle filtering mechanism, reduces the accumulation of positioning errors, and has strong adaptability to dynamic environments;

[0056] (2) This invention adopts hierarchical optimization and distributed ADMM algorithm, reducing the cluster computing complexity to the O(NlogN) level (N is the number of drones), supporting real-time mapping of hundreds of drones, and significantly improving collaborative efficiency;

[0057] (3) The present invention is based on dynamic block coding and edge caching strategies to ensure task continuity in weak network environments and enhance communication robustness;

[0058] (4) The present invention supports plug-and-play deployment of different models of cameras, radars, and positioning modules through a unified spatiotemporal alignment interface, meeting the needs of heterogeneous UAV cluster collaboration and achieving high compatibility of heterogeneous devices. BRIEF DESCRIPTION OF THE DRAWINGS

[0059] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the following briefly introduces the drawings required for use in the embodiments. It should be understood that the following drawings only illustrate certain embodiments of the present invention and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other relevant drawings can be obtained based on these drawings without paying any creative work.

[0060] Figure 1 This is a flow chart of the UAV cluster positioning method based on collaborative SLAM algorithm and distributed optimization of the present invention. DETAILED DESCRIPTION

[0061] To make the objectives, technical solutions, and advantages of the embodiments of the present invention more clear, the technical solutions of the embodiments of the present invention will be clearly and completely described below in conjunction with the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Generally, the components of the embodiments of the present invention described and shown in the drawings herein can be arranged and designed in various different configurations.

[0062] Please refer to Figure 1The first embodiment of the present invention provides a UAV cluster positioning method using a collaborative SLAM algorithm and distributed optimization, which includes: S100, initializing the UAV cluster and synchronizing multi-source heterogeneous sensor data; S200, performing multi-machine 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; S300, constructing a dynamic hierarchical communication topology for the UAV cluster, dynamically electing a master node and dividing the communication subgroups, and using adaptive block coding technology to process the transmission data of the hierarchical communication topology; S400, constructing a two-level pose graph optimization based on 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, based on load balancing, dynamically allocating computing resources for each UAV, and quickly taking over and predicting recovery for failed nodes; S700, displaying the global scene and the trajectory of the UAV cluster in real time through a three-dimensional visualization tool, and synchronously monitoring positioning accuracy, communication delay and load calculation.

[0063] Among them, in S100, the initialization of the drone cluster and the synchronization of multi-source heterogeneous sensor data include: S110, external calibration and spatiotemporal offset compensation of the multi-source heterogeneous sensors to establish a unified coordinate system; S120, using a Kalman filter-based pose predictor to perform multi-sensor data fusion on the multi-source heterogeneous sensors to unify the sampling frequency and timing of the multi-source heterogeneous sensors; S130, each of the drones performs environmental perception and clock synchronization through the multi-source heterogeneous sensors carried by each of them; 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.

[0064] The configuration of the communication protocol includes deploying a wireless ad hoc network based on IEE 802.11n, setting a bandwidth priority policy, ensuring a minimum delay of ≤20ms for key data packets, and using a double check mechanism (CRC16 + parity bit) to ensure reliable transmission.

[0065] Multi-sensor data fusion includes generating synchronized timestamps using a linear interpolation-based prediction compensation method to account for the timing differences between the 30Hz camera and the 200Hz IMU.

[0066] This step can solve the problem of timestamp jitter in heterogeneous sensor data and avoid ghosting caused by timing misalignment during SLAM mapping. The Python code for linear interpolation synchronization of IMU data is shown below:

[0067] def interpolate_imu(cam_time, imu_queue):

[0068] # Find the two IMU samples before and after from the queue for interpolation

[0069] prev_imu = next(imu for imu in reversed(imu_queue) ifimu.timestamp <= cam_time)

[0070] next_imu = next(imu for imu in imu_queue if imu.timestamp >= cam_time)

[0071] ratio = (cam_time - prev_imu.timestamp) / (next_imu.timestamp -prev_imu.timestamp)

[0072] # Interpolate angular velocity and acceleration

[0073] interpolated_gyro = (1 - ratio) * prev_imu.gyro + ratio * next_imu.gyro

[0074] interpolated_accel = (1 - ratio) * prev_imu.accel + ratio * next_imu.accel

[0075] return interpolated_gyro, interpolated_accel

[0076] The extrinsic parameter matrices of the camera and radar are calculated using the checkerboard calibration method to align the point cloud with the pixel coordinate system. Dynamic obstacle feature extraction involves combining the optical flow tracking results of ORB feature points and filtering out areas where the optical flow amplitude exceeds a set threshold, such as areas with a speed >1m / s, to identify them as dynamic target candidates.

[0077] The purpose of this step is to quickly filter dynamic areas by optical flow amplitude and reduce the interference of static background on SLAM scene. The Python code example of optical flow threshold filtering is as follows:

[0078] def filter_dynamic_regions(flow, velocity_threshold=1.0):

[0079] # Calculate the optical flow velocity amplitude of each pixel (unit: pixel / frame)

[0080] magnitude = np.sqrt(flow[...,0]**2 + flow[...,1]**2)

[0081] # Assuming the camera frame rate is 30Hz, convert to m / s (needs to be calibrated according to the actual pixel scale)

[0082] velocity_map = magnitude * 0.02 # Example scale: 1 pixel = 0.02m

[0083] mask = velocity_map > velocity_threshold

[0084] return cv2.connectedComponentsWithStats(mask.astype(np.uint8))[1]> 0

[0085] 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 Lie group SE(3), improving the efficiency of sliding window optimization, and controlling the initialization odometer error within 0.2m horizontally and 0.1m vertically.

[0086] The initial alignment of multi-machine scenes includes matching cross-machine feature points based on the Hamming distance of the ORB descriptor. When there are ≥15 matching point pairs, SVD is triggered to solve the initial pose transformation matrix, and abnormal matches are eliminated through RANSAC.

[0087] This step establishes a global coordinate system by matching feature points across multiple robots, avoiding overlapping conflicts caused by misalignment between scenes. The C++ code example for RANSAC to eliminate abnormal matches is as follows:

[0088] void ransac_filter(std::vector <cv::point2f>& src_points,

[0089] std::vector <cv::point2f>& dst_points,

[0090] double threshold = 3.0) {

[0091] cv::Mat mask;

[0092] cv::findHomography(src_points, dst_points, cv::RANSAC, threshold, mask);

[0093] / / Keep inliers

[0094] std::vector <cv::point2f>inliers_src, inliers_dst;

[0095] for (int i=0; i<mask.rows; i++) {

[0096] if (mask.at <uchar>(i)) {

[0097] inliers_src.push_back(src_points[i]);

[0098] inliers_dst.push_back(dst_points[i]);

[0099] }

[0100] }

[0101] src_points = inliers_src;

[0102] dst_points = inliers_dst;

[0103] }

[0104] The filtering optimization preprocessing includes constructing a joint optimization objective function that includes visual reprojection error and inertial error, initializing the outlier weights of matching point pairs to 0.1, and retaining constraints with high confidence to participate in subsequent optimization.

[0105] Among them, in S300, the dynamic hierarchical communication topology is constructed for the drone cluster, the master node is dynamically elected and the communication subgroups are divided, including: S310, constructing a dynamic multi-subgroup hierarchical communication topology for the drone cluster; S320, dynamically electing a master node in the hierarchical communication topology based on the relative distance, communication bandwidth and task priority between the drones; S330, the master node is responsible for the fusion of the global scene and task scheduling; S340, within each communication subgroup, the slave node performs local perception data generation and compressed transmission.

[0106] Among them, in S300, the use of adaptive block coding technology to process the transmission data of the hierarchical communication topology includes: S350, based on the local scene information constructed in the initial pose graph, evaluating the density distribution of environmental features in each spatial area, dividing the global scene into several transmission blocks, using low-resolution point cloud representation for sparse areas to reduce redundant data, and retaining complete feature descriptors in dense areas to ensure the perception accuracy of key areas; S360, each of the transmission blocks is encoded using a differentiated compression ratio strategy to obtain an encoded data packet, which contains a hash check code to support local data retransmission requests in packet loss scenarios.

[0107] The master node election and subgroup division include real-time updating of node scores based on a hybrid evaluation function. When the master node is offline or the score is below a threshold, such as when the remaining power is less than 20%, the Bully algorithm is triggered to re-elect the master node. The subgroup uses the Ripple protocol to disseminate topology change information.

[0108] Dynamic master node election is designed to avoid single points of failure and ensure that topology changes are quickly propagated to the cluster. The following is a Python example of the Bully algorithm election logic:

[0109] class Node:

[0110] def __init__(self, id, battery):

[0111] self.id = id

[0112] self.battery = battery

[0113] def bully_election(nodes):

[0114] candidates = [node for node in nodes if node.battery >= 20]

[0115] if not candidates:

[0116] return None

[0117] # Select the node with the largest ID as the primary node

[0118] leader = max(candidates, key=lambda x: x.id)

[0119] return leader.id

[0120] Adaptive data compression transmission includes dividing the lidar point cloud into blocks based on its density. The grid size is 1m×1m, and sparse blocks have less than 50 points per grid. Run-Length Encoding is used to compress the data to 30% of the original data volume. Dense blocks retain the original accuracy, and the data packet is accompanied by a hash checksum (SHA-256) to ensure integrity.

[0121] Among them, in S400, based on the hierarchical communication topology, a two-level pose graph optimization is constructed to obtain a unified global scene, including: S410, the slave node uses the information matrix decomposition method to perform local graph optimization in the local sliding window, and outputs a covariance matrix with confidence weights; S420, the master node collects the pose estimation and covariance information uploaded by each of the slave nodes, solves the global pose consistency constraints of multiple machines in parallel through the ADMM algorithm, filters dynamic interference, and transmits the optimization results back for scene correction of the slave nodes to obtain a unified global scene.

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

[0123] The purpose of this step is to reduce the impact of mismatching on optimization through iterative weighting and improve global scene consistency.

[0124] The Python example of residual abnormal value weight adjustment is as follows:

[0125] def reweight_outliers(residuals, sigma):

[0126] weights = np.ones_like(residuals)

[0127] outlier_mask = np.abs(residuals) > 3 * sigma

[0128] # Exponential decay weight: the outlier weight gradually decreases from 0.1

[0129] weights[outlier_mask] = 0.1 * np.exp(-0.5 * (residuals[outlier_mask] / sigma)**2)

[0130] return weights

[0131] Among them, 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, selecting a lightweight deep convolutional neural network model, and using the fused multi-source perception data in the global scene as 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 mask filtering is performed on the point cloud or image area marked as dynamic.

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

[0133] The lightweight model ensures real-time processing on edge devices, and binocular parallax improves the accuracy of dynamic target positioning. Python code example for binocular depth calculation:

[0134] def compute_depth(bbox_left, bbox_right, focal_length, baseline):

[0135] # Calculate parallax (taking the center point of the bounding box as an example)

[0136] disparity = abs(bbox_left.center_x - bbox_right.center_x)

[0137] depth = (focal_length * baseline) / max(disparity, 1e-5) # avoid division by zero

[0138] return depth

[0139] 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 the master node regularly triggering scene version merging at intervals of 5 seconds, and removing obstacle data that has not been updated for more than 15 seconds based on the timestamp.

[0140] Among them, in S600, the dynamic allocation of computing resources for each drone based on load balancing includes: S610, real-time quota allocation of computing resources and communication bandwidth based on the current processing task queue length, scene processing complexity and communication status of each drone; S620, when the confidence of the local scene is lower than the preset threshold, the resource priority of the associated drone is increased, and data return and posture optimization are performed first.

[0141] Among them, in S600, the rapid takeover and predictive recovery of the failed node include: S630, when the edge communication of the slave node is interrupted, the failed slave node is quickly taken over, the current posture is predicted using historical optimization results, and the local scene is updated; S640, when a dense area of ​​dynamic objects is detected, the drone equipped with high-precision sensors is preferentially scheduled to perform area revisit.

[0142] This step avoids missing keyframes in the scene by interpolating historical data, effectively improving the system's fault tolerance. B-spline trajectory interpolation C++ code example:

[0143] #include<bezier.h>

[0144] std::vector <point3d>spline_interpolation(const std::vector <point3d>&points, int n=10) {

[0145] Bezier::Spline spline;

[0146] spline.setControlPoints(points);

[0147] return spline.eval(n); / / Generate interpolation points

[0148] }

[0149] Among them, task allocation includes real-time monitoring of drone load, CPU utilization threshold of 70%, memory threshold of 80%, priority scheduling of nodes closest to the target point and with sufficient resources, and response delay of task allocation in dangerous areas less than 200ms; failure recovery includes when a neighboring node takes over the failed area, interpolation based on historical trajectory data to supplement lost frames, using the B-spline interpolation algorithm, and the interpolation results are written to the global scene after verification by the master node.

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

[0151] The second embodiment of the present invention provides a drone cluster positioning system with a collaborative SLAM algorithm and distributed optimization, which includes: a master node server for performing global scene fusion and distributed optimization calculations; a drone terminal equipped with heterogeneous sensors and an edge computing module for local feature extraction and posture optimization; and a communication relay device for supporting dynamic block coding transmission and adaptive bandwidth allocation.

[0152] 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-machine feature point matching and initial pose joint optimization between each drone, and constructing an initial pose graph through 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 the communication subgroups, and using adaptive block coding technology to process the transmission data of the hierarchical communication topology; a global scene unification module for constructing a 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, and quickly taking over and predicting recovery of failed nodes; and a real-time display module for displaying the global scene and the trajectory of the drone cluster in real time using a three-dimensional visualization tool, and simultaneously monitoring positioning accuracy, communication delay, and load calculation.

[0153] The computer program product of the drone cluster positioning method and device based on collaborative SLAM algorithm and distributed optimization provided in the embodiment of the present invention includes a computer-readable storage medium storing program code. The instructions included in the program code can be used to execute the method in the previous method embodiment. The specific implementation can be found in the method embodiment, which will not be repeated here.

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

[0155] If the functions are implemented as software functional units and sold or used as independent products, they can be stored in a processor-executable, non-volatile, computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the portion that contributes to the prior art, or a portion of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes instructions for enabling a computer device (which can be a personal computer, server, or network device, etc.) to perform all or part of the steps of the methods described in various embodiments of the present invention. The aforementioned storage media include various media capable of storing program code, such as USB flash drives, mobile hard drives, read-only memories (ROMs), random access memories (RAMs), magnetic disks, or optical disks.

[0156] Finally, it should be noted that the above-described embodiments are only specific implementations of the present invention, which are used to illustrate the technical solutions of the present invention, rather than to limit them. The scope of protection of the present invention is not limited thereto. Although the present invention has been described in detail with reference to the above-described embodiments, those skilled in the art should understand that any person skilled in the art can modify or easily conceive of changes to the technical solutions described in the above-described embodiments within the technical scope disclosed by the present invention, or replace some of the technical features therein with equivalents. Such modifications, changes, or replacements do not deviate from the spirit and scope of the technical solutions of the embodiments of the present invention, and should be included in the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be based on the scope of protection of the claims. < / uchar>

Claims

1. A UAV cluster positioning method based on collaborative SLAM algorithm and distributed optimization, characterized in that: include: S100, initializes the drone cluster and synchronizes multi-source heterogeneous sensor data; S200, performing multi-machine feature point matching and initial pose joint optimization on each of the UAVs, and constructing an initial pose graph through VIO pre-integration and SVD decomposition algorithm; S300, constructing a dynamic hierarchical communication topology for the UAV cluster, dynamically electing a master node and dividing the communication subgroups, and using adaptive block coding technology to process the transmission data of the hierarchical communication topology; S400, based on the hierarchical communication topology, constructing a 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 allocates computing resources for each of the drones, and quickly takes over and predictively recovers failed nodes; S700 displays the global scene and the trajectory of the drone cluster in real time through a three-dimensional visualization tool, and simultaneously monitors positioning accuracy, communication delay, and load calculation.

2. The collaborative SLAM algorithm and distributed optimized UAV cluster positioning method according to claim 1 is characterized in that, In S100, the initialization of the drone cluster and synchronization of multi-source heterogeneous sensor data include: S110, performing external calibration and spatiotemporal offset compensation on the multi-source heterogeneous sensors to establish a unified coordinate system; S120, using a Kalman filter-based pose predictor to perform multi-sensor data fusion on the multi-source heterogeneous sensors, and unify the sampling frequency and timing of the multi-source heterogeneous sensors; S130, each of the drones performs environment perception and clock synchronization through the multi-source heterogeneous sensors carried by the drone; S140, the multi-source heterogeneous sensor includes at least one of a visual camera, a lidar, an inertial navigation system (IMU), and a millimeter-wave radar.

3. The collaborative SLAM algorithm and distributed optimized UAV cluster positioning method according to claim 1, characterized in that, In S300, the step of constructing a dynamic hierarchical communication topology for the drone cluster, dynamically electing a master node, and dividing the communication subgroups includes: S310, constructing a dynamic multi-subgroup hierarchical communication topology for the drone cluster; S320, dynamically electing a master node in the hierarchical communication topology based on the relative distance between the UAVs, the communication bandwidth, and the task priority; S330, the master node is responsible for global scene integration and task scheduling; S340, within each communication subgroup, the slave node performs local sensing data generation and compression transmission.

4. The collaborative SLAM algorithm and distributed optimized UAV cluster positioning method according to claim 3 is characterized in that, In S300, the processing of the transmission data of the layered 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 environmental features in each spatial region, dividing the global scene into a number of transmission blocks, using low-resolution point clouds to represent sparse areas, and retaining complete feature descriptors in dense areas; S360: Each of the transmission blocks is encoded using a differentiated compression ratio strategy to obtain an encoded data packet, wherein the encoded data packet includes a hash check code.

5. The collaborative SLAM algorithm and distributed optimized UAV cluster positioning method according to claim 3, characterized in that: In S400, based on the hierarchical communication topology, a two-level pose graph optimization is constructed to obtain a unified global scenario including: S410, the slave node performs local graph optimization in a local sliding window using an information matrix decomposition method, and outputs a covariance matrix with confidence weights; S420, the master node collects the pose estimation and covariance information uploaded by each slave node, solves the global pose consistency constraints of multiple machines in parallel through the ADMM algorithm, filters dynamic interference, and transmits the optimization results back for scene correction of the slave nodes to obtain a unified global scene.

6. The collaborative SLAM algorithm and distributed optimized UAV cluster positioning method according to claim 3 is characterized in that: 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, selecting a lightweight deep convolutional neural network model, and using the fused multi-source perception data in the global scene as 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 mask filtering is performed on the point cloud or image area marked as dynamic.

7. The collaborative SLAM algorithm and distributed optimized UAV cluster positioning method according to claim 4, characterized in that: In S600, dynamically allocating computing resources for each drone based on load balancing includes: S610, combines the current processing task queue length, scene processing complexity and communication status of each UAV to allocate computing resources and communication bandwidth in real time; S620: When the confidence level of the local scene is lower than a preset threshold, the resource priority of the associated UAV is increased, and data transmission and posture optimization are performed first.

8. The collaborative SLAM algorithm and distributed optimized UAV cluster positioning method according to claim 7, characterized in that: In S600, the rapid takeover and predictive recovery of the failed node includes: S630, when the edge communication of the slave node is interrupted, quickly take over the failed slave node, use historical optimization results to predict the current posture, and update the local scene; S640: When an area with dense dynamic objects is detected, the drone equipped with a high-precision sensor is preferentially dispatched to revisit the area.

9. A UAV cluster positioning system based on collaborative SLAM algorithm and distributed optimization, characterized in that: include: The master node server is used to perform global scene fusion and distributed optimization calculations, including: An initialization module is used to initialize the drone cluster and synchronize multi-source heterogeneous sensor data; an initial pose graph construction module is used to perform multi-machine feature point matching and initial pose joint optimization between each of the drones, and to construct an initial pose graph through VIO pre-integration and SVD decomposition algorithms; a hierarchical communication topology module is used to construct a dynamic hierarchical communication topology for the drone cluster, dynamically elect a master node and divide the communication subgroups, and use adaptive block coding technology to process the transmission data of the hierarchical communication topology; a global scene unification module is used to construct a two-level pose graph optimization based on the hierarchical communication topology to obtain a unified global scene; a dynamic obstacle removal module is used to use a lightweight deep learning model to detect and remove dynamic obstacles in the global scene in real time; a load balancing module is used to dynamically allocate computing resources for each drone based on load balancing, and to quickly take over and predictively recover failed nodes; a real-time display module is used to display the global scene and the trajectory of the drone cluster in real time through a three-dimensional visualization tool, and to simultaneously monitor positioning accuracy, communication delay and load calculation; UAV terminals, equipped with heterogeneous sensors and edge computing modules, are used for local feature extraction and pose optimization; Communication relay equipment used to support dynamic block coding transmission and adaptive bandwidth allocation.

Images