A multi-platoon cooperative path planning method based on virtual reality

Abstract

The present application relates to unmanned aerial vehicle formation technology field, specifically is a kind of multi-formation cooperative path planning method based on virtual reality, comprising: according to the initial state vector of each formation, generate initial planning route and its constraint planning set;Collision response processing is carried out on flight trajectory based on initial planning path, collision response is expressed as trajectory response and direction response, collision risk zone is solved in virtual space;The flight trajectory of single unmanned aerial vehicle in collision risk zone is mapped as local coordinate offset with mass center coordinate as origin, the coordinates of each unmanned aerial vehicle in unmanned aerial vehicle formation are corrected;The sequence of corrected unmanned aerial vehicle coordinates is obtained, and all unmanned aerial vehicles are state coded according to the formation, to determine the path range of each unmanned aerial vehicle formation;The path range of antecedent formation and subsequent formation is taken as comparison object, the overlap degree of current formation and other formations is calculated, and the cooperative path of multi-unmanned aerial vehicle formation is determined. The flight safety and cooperation efficiency of unmanned aerial vehicle formation are improved.

Description

Technical Field

[0001] This invention relates to the field of unmanned aerial vehicle (UAV) formation technology, specifically a multi-formation cooperative path planning method based on virtual reality. Background Technology

[0002] With the development of drone swarm technology, multi-drone formation collaborative operations have been widely used in various scenarios such as regional inspection, security and surveillance, and surveying and modeling. In such applications, the single-formation operation mode is gradually evolving into the multi-formation collaborative operation mode. It is necessary to perform path and risk simulations for multiple formations to optimize the drone collaboration strategy and formation control.

[0003] For example, Chinese Patent Publication No. CN119937587A discloses a method for merging and changing UAV formation paths based on reachability sets, including: when the UAV formation path needs to merge or change, acquiring the flight status information, control information, and disturbance information of the interfering UAV and the evading UAV; constructing a system dynamic data set based on the above information; determining a safe region based on the game strategy of the evading UAV and the interfering UAV, and taking the zero sub-level set of the implicit surface function of the safe region as the target set; constructing a Hamilton-Jacobi partial differential equation based on the predefined cost-value function of reaching the target set from the current state and the system dynamic data set, taking the implicit surface function of the safe region as the termination condition, solving the Hamilton-Jacobi partial differential equation, and obtaining the optimal control strategy.

[0004] For example, Chinese Patent Publication No. CN119987428A discloses a method and system for collaborative formation control of UAV swarms based on unknown environments. This method sets the desired formation position based on the position of the tracked target and flexibly adjusts the specific position of each UAV within the formation according to its own position to construct the target point of each UAV. After constructing the optimal path from the UAV's position to the target point and smoothing the optimal path, the method controls the UAV to move along the trajectory after the optimal path is processed by a model predictive controller.

[0005] Existing technologies utilize state control of UAVs to construct optimal control strategies for UAV formations by solving implicit surface coordinates; or they define optimal obstacle avoidance and retraction paths through interference from neighboring UAVs. However, these technologies emphasize the strategy control of individual aircraft without considering the formation's direction of motion for motion prediction. This results in the unknowable movement states of individual UAVs within the formation, affecting the consistency and feasibility of formation planning. Summary of the Invention

[0006] To solve the above-mentioned technical problems, the technical solution adopted by the present invention is: a multi-formation cooperative path planning method based on virtual reality, including: S1, acquiring flight data of UAV formations, extracting the centroid coordinates of each formation from the flight data, and determining the initial state vector of each formation.

[0007] S2, based on the initial state vector of each formation, defines the initial state of the UAV formation as the planning starting point, and generates the initial planning route and its constraint planning set by referring to the path points and endpoints of the mission execution.

[0008] S3 processes the flight trajectory based on the initial planned path to perform collision response, representing the collision response as trajectory response and directional response, and solves the collision risk zone in virtual space.

[0009] S4 maps the flight trajectory of a single UAV within the collision risk zone to a local coordinate offset with the centroid coordinate as the origin, correcting the coordinates of each UAV in the UAV formation.

[0010] S5: Obtain the corrected UAV coordinate sequence, and encode the status of all UAVs according to their respective formations to determine the path range of each UAV formation.

[0011] S6 uses the path ranges of the preceding and following formations as comparison objects to calculate the overlap between the current formation and other formations, and determines the cooperative path of the multi-UAV formation.

[0012] The beneficial effects of this invention are as follows: First, this invention obtains the centroid coordinates by calculating the mean coordinates of all UAVs in the formation, establishes a global coordinate system for multiple formations, and defines the initial state vector of each formation in conjunction with the flight formation; at the same time, it quantifies the constraints of UAV planning by combining the minimum safe distance between formations and the allowable offset area of ​​a single UAV; it enables UAVs to take the collaborative formation as the analysis subject, ensuring the feasibility of route planning in virtual reality scenarios and avoiding planning conflicts caused by inconsistent constraints in multiple links.

[0013] II. This invention decomposes the collision response into two dimensions: trajectory response and directional response. By comparing the projected arc length of adjacent formations with the minimum safe longitudinal spacing, it identifies collision risk zones caused by longitudinal tracking deviations. Furthermore, it uses the formation's equivalent radius and altitude range to generate the formation's outer boundary, and identifies potential collision risk zones in the heading dimension by finding the intersection of these boundaries. This approach can identify collision risks caused by current trajectory deviations and, by combining the formation's orientation and spatial outer boundary, predict potential collision hazards within a future time domain. This improves the advance and comprehensiveness of risk identification and reduces the probability of emergency obstacle avoidance during flight.

[0014] Third, this invention maps the flight trajectory within the collision risk zone to a local coordinate offset with the center of mass as the origin, limiting obstacle avoidance adjustments to the relative offset level within the formation. The point with the smallest Euclidean distance is selected as the correction amount, preserving the overall reference trajectory and preset formation of the formation as much as possible, avoiding overall formation drift caused by directly modifying the global coordinates. Subsequently, if all points fall into the risk zone, the coordinates of the formation's center of mass are adjusted as a whole, and then synchronously mapped to the local positions of each individual aircraft. Obstacle avoidance feasibility is ensured by the overall translation of the center of mass, while maintaining the relative configuration within the formation.

[0015] Fourth, this invention extracts coordinate and velocity information from the corrected UAV coordinate sequence to construct a feature vector for a single UAV; it then performs aggregation operations on all feature vectors within the same formation to obtain the formation's feature vector; finally, it maps the formation's feature vector to a Gaussian probability density function in three-dimensional space, using the spatial region where the cumulative probability reaches a set threshold as the path range for the current formation's movement. By defining the spatial boundary of the formation's overall movement in the form of a probability distribution, it retains the flexibility of individual UAV position adjustments while constraining the overall movement range of the formation, preventing point-by-point adjustments by individual UAVs from causing the overall formation's path to deviate from the mission route, and improving the comparability and interpretability of the operational positions of each formation. Attached Figure Description

[0016] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0017] Figure 1 This is a flowchart illustrating a multi-formation collaborative path planning method based on virtual reality.

[0018] Figure 2 This is a flowchart illustrating step S3 of a multi-formation cooperative path planning method based on virtual reality.

[0019] Figure 3 This is a flowchart illustrating step S4 of a virtual reality-based multi-formation cooperative path planning method.

[0020] Figure 4 This is a flowchart illustrating step S5 of a multi-formation collaborative path planning method based on virtual reality. Detailed Implementation

[0021] The embodiments of the present invention are described in detail below. The embodiments described below are exemplary and are only used to explain the present invention, and should not be construed as limiting the present invention. Where specific techniques or conditions are not specified in the embodiments, they shall be performed in accordance with the techniques or conditions described in the literature in the art or in accordance with the product manual.

[0022] See Figure 1A multi-formation cooperative path planning method based on virtual reality includes: S1, acquiring flight data of UAV formations, extracting the centroid coordinates of each formation from the flight data, and determining the initial state vector of each formation; the flight data refers to the formation number, position, flight direction of the formation, and mission objective of the UAV formation; each UAV formation includes at least two UAVs, and flies according to a specific formation according to the mission objective of the current formation.

[0023] In one embodiment of the present invention, one implementation of step S1 includes: S11, calculating the centroid coordinates of each drone formation based on the average coordinates of all drones in the drone formation; wherein the centroid coordinates represent the average coordinates of the drone formation in three-dimensional space, and are used to explain the relative coordinate description of the entire formation.

[0024] S12. Establish a global coordinate system for multiple drone formations based on the centroid coordinates of each drone formation. When establishing the global coordinate system, input the centroid coordinates of multiple drone formations, define the spatial distribution of each formation based on the spatial distance between their centroid coordinates, and within each formation, define the relative coordinates of each drone based on the distance between its individual coordinates and the centroid coordinates. Synchronize these coordinates to the global three-dimensional coordinate system to form the global coordinate system of the drone formations.

[0025] S13 tracks the movement trajectory of each UAV formation in the global coordinate system and defines the initial state vector of each formation according to the flight formation corresponding to the movement trajectory.

[0026] When setting the initial state vectors for each formation, the formation's movement trajectory is tracked using centroid coordinates, the individual UAV's movement trajectory is tracked using relative coordinates, and the UAV's flight direction is recorded in conjunction with the formation's flight formation to complete the configuration of the UAV's initial state vectors. The initial state vectors are used to bind the UAVs' coordinates, directions, and formations, and represent the initial state when the formation performs its mission.

[0027] S2, based on the initial state vector of each formation, the initial state of the UAV formation is defined as the planning starting point. By referring to the path points and endpoints of the task execution, the initial planning route and its constraint planning set are generated. The initial planning route is used to explain the movement trajectory from the planning starting point to the task endpoint. When each task is generated, the starting point of the initial state, the path points and endpoints of the task execution are configured in the virtual space, so that the smooth curve connecting multiple points is used as the initial planning route.

[0028] In one embodiment of the present invention, one implementation of step S2 includes: S21, importing the initial state vector of each formation, taking the position corresponding to the initial state vector as the planning starting point, connecting each waypoint and the end point, and forming the initial planned route through smooth curve interpolation.

[0029] Import the current three-dimensional position of the UAV formation at the initial moment as the planning starting point of the formation. Then, read the waypoints and mission endpoints configured for the current formation in the virtual space, and use spline interpolation to generate a continuous spatial curve parameterized over time, which is the initial planned route.

[0030] It should be noted that the initial planned route is the reference trajectory for the entire formation, not the trajectory of each drone.

[0031] S22, using the centroid coordinates of the UAV formation to track the initially planned route, at any given time, obtain the relative position of each UAV in the formation with respect to the centroid coordinates at that time, construct the formation offset matrix, and determine the allowable offset range for each UAV; wherein, the formation offset matrix is ​​used to describe the offset of each UAV in the formation with respect to the centroid coordinates of the UAV formation, each row of the matrix corresponds to one UAV, storing the three-dimensional offset value of that UAV with respect to the centroid coordinates.

[0032] In a local coordinate system with the real-time centroid of the formation as the origin, a three-dimensional allowable offset interval is independently constructed for each UAV in the formation. This interval is a symmetrical closed interval on the xyz axes. The allowable offset interval is read from the formation's historical mission configuration data and represents the maximum allowable offset of the current formation. This value is strongly correlated with the mission objective, and the coordinates of all UAVs that are locally corrected must fall within this interval.

[0033] S23: Read the minimum safe distance between different formations, and output the minimum safe distance and allowable offset interval as a constraint planning set. Here, the safe distance is the minimum distance between formations, which can also be refined to the minimum safe distance between individual UAVs. In this step, the allowable offset interval and minimum safe distance are pre-set for the pre-planned initial planning path as constraints for path configuration. In subsequent processing, the allowable offset interval will serve as the standard for formation control; while the minimum safe distance will manage the collision risk between formations, quantifying the collaborative management between multiple formations.

[0034] S3 processes the flight trajectory for collision response based on the initial planned path, representing the collision response as trajectory response and directional response, and solves for the collision risk zone in virtual space. The trajectory response is used to identify the longitudinal tracking collision risk of the formation along the initial planned route, and the directional response is used to identify the potential collision risk of the formation's outer spatial boundary intersecting in the prediction time domain.

[0035] In one embodiment of the present invention, such as Figure 2As shown, one implementation of step S3 includes: S31, projecting the centroid coordinates of the current UAV formation onto the initial planned route to obtain the corresponding projection point; by determining the centroid coordinates of this formation, making a spatial vertical projection onto the initial planned route to obtain the projection point.

[0036] S32 detects the projected arc lengths of the current drone formation and adjacent formations on the same initial planned route. The distance from the projection point of the current drone formation to the projection point of the adjacent formation is the projected arc length at this location. When the projected arc length is less than the minimum safe longitudinal distance, the spatial region corresponding to the projected arc length is defined as the collision risk zone of the trajectory response. The minimum safe longitudinal distance represents the distance extended along the arc length of the initial planned route by the minimum safe distance of the constraint planning set. It is used to explain the safe distance between drones traveling on the same route to simulate whether a collision can occur on the same trajectory.

[0037] In this process, the vertical and horizontal flight of the drones is not considered, but the spacing between the formations along the flight path is emphasized. All drone formations are projected onto the same spline interpolation initial planned route to determine the spatial areas where collisions are likely to occur. After marking the corresponding areas, they serve as the collision risk zones during trajectory response.

[0038] In other words, when a drone formation is performing a mission, even if it is composed of 100 waypoints, the system will fit these points into a continuous and smooth initial planned route during initialization. At any given time, a projection point can be found on this route, and the distance between multiple drone formations during the mission can be checked based on the arc length corresponding to the projection point. The resulting collision risk zone is a longitudinal pipe region along the initial planned route. This pipe region is the area obtained by extending the minimum safe longitudinal distance forward from the current formation as the starting point, to identify the relative risk of spatial collision between the current formation and other formations. If the collision risk of drone formations in front and behind is considered, the length corresponding to the minimum safe longitudinal distance is extended sequentially from the current formation in the forward and backward directions to form a longitudinal pipe region. The longitudinal pipe region uses the maximum distance from each drone to the center of mass on the horizontal plane as the inner radius of the pipe, making it a specific pipe-shaped area on the horizontal plane.

[0039] S33. In the prediction time domain, for each centroid point, solve the outer boundary of the UAV formation at the corresponding time. The prediction time domain represents the outer boundary obtained by extending the UAV formation forward along the initial planned route in the directional response process by using the current position of the formation.

[0040] When solving for the outer boundary of the UAV formation at a given time, the initial planned route is sampled at a fixed time step to obtain discrete centroid positions. The fixed time step is 0.1s. After each fixed time step, the centroid coordinates of the formation are advanced along the initial planned route by an arc length increment corresponding to one step. Then, the three-dimensional coordinates of the centroid at the next time step are mapped to obtain the three-dimensional coordinates of the centroid. This calculated three-dimensional coordinate is the discrete centroid position.

[0041] Specifically, the centroid coordinates of the current UAV formation are extracted from the initial planned route. These centroid coordinates correspond to projection points. Over a fixed time step, the UAVs advance forward by a length corresponding to the fixed time step based on their current flight speed, thus revealing the projection points of the coordinate transformation and obtaining the arc length increment. Subsequently, the arc length increment is back-projected into specific three-dimensional coordinates according to the projection method of the projection points, thereby obtaining the centroid position corresponding to each fixed time step. The prediction time domain can then be set to 5-10 seconds to predict the spatial position of the UAVs within a short period.

[0042] For each centroid point, the formation attitude angles are calculated based on the tangential direction of the initially planned route at the current moment. The rotation matrix of the current formation is obtained based on the calculated attitude angles. The attitude angles refer to the rotation angles of the UAV around three orthogonal axes, which are used to explain the angles of the x, y, and z axes in the tangential direction, representing the flight path of the UAV at a specific point.

[0043] The equivalent radius for the horizontal dimension is set based on the maximum horizontal distance of all UAVs in the formation from the centroid point; the height range for the vertical dimension is set based on the maximum vertical distance of all UAVs in the formation from the centroid point. The equivalent radius refers to the radius of a circle with the centroid point as the center and the maximum horizontal distance as the length; similarly, the height range uses the maximum vertical distance as the upper and lower limits of the height range.

[0044] At this point, a circle with the equivalent radius is formed with the centroid as the center, serving as the boundary in the horizontal dimension. Then, the maximum vertical distance in the vertical direction is used to form the height range of the drone formation from top to bottom, so that it forms a specific cylinder with the horizontal circle. After that, this cylinder is synchronized to the three-dimensional space through rotation transformation according to the angle recorded by the rotation matrix to obtain the outer boundary of the drone formation at the current moment.

[0045] Based on the rotation matrix, the equivalent radius and altitude range are transformed by rotation to obtain the outer boundary of the UAV formation at the corresponding moment. The core of this step is to map the cylindrical boundary based on the centroid point to the global space through a three-dimensional rotation matrix to eliminate the approximation error in the level flight scenario and obtain an outer boundary that fits the spatial attitude of the formation. In addition to the above method, a convex hull polygon can also be obtained by rotating the local coordinates of all members in the UAV formation to serve as the current outer boundary.

[0046] S34, intersects the outer boundaries of each UAV formation. When there is an intersection on the outer boundaries, the spatial region corresponding to the intersection is regarded as the collision risk zone of the direction response. In this divided outer boundary, the boundary is to further identify the flight trajectories that intersect and overlap in space. This makes the UAV collision detection part extend from the longitudinal spacing of the trajectory response to the spatial intersection of the direction prediction, and finally achieves the effect of spatial attitude alignment of multiple UAV formations.

[0047] S4 maps the flight trajectory of a single UAV within the collision risk zone to a local coordinate offset with the centroid coordinate as the origin, correcting the coordinates of each UAV in the UAV formation; by transforming the flight trajectory within the collision risk zone into the spatial distribution of a single UAV relative to the centroid coordinate, it illustrates the spatial position of each UAV in the event of a collision risk; then, by adjusting the local coordinates of the UAVs, obstacle avoidance processing for multiple UAV formations is achieved.

[0048] In one embodiment of the present invention, such as Figure 3 As shown, one implementation of step S4 includes: S41, obtaining the centroid coordinates of the UAV formation at the current moment, applying a path offset to each UAV based on the centroid coordinates, wherein the path offset represents the relative position of the current UAV coordinates with respect to the centroid coordinates, used to explain the local position under the collision risk; by converting the flight trajectory within the collision risk zone into the path offset of the local position, the local position of the UAV in the formation local coordinate system is obtained; the local position describes which UAVs within the formation fall into the collision risk.

[0049] S42, determine whether the local position corresponding to the path offset falls into the collision risk zone, and set the correction amount of the local position based on the constraint planning set.

[0050] When determining whether the local position corresponding to the path offset falls into the collision risk zone, the local position of the UAV is synchronized to the local position of the collision risk zone. If any UAV's local position is located in the local position of the collision risk zone, it is considered that the local position corresponding to the path offset has fallen into the collision risk zone; otherwise, it is considered that the corresponding UAV has not fallen in. If it has not fallen in, it means that the UAV at the current moment only has a theoretical collision risk, and there is no intersection between the two in actual spatial position or the actual distance between the two is greater than the minimum safe distance.

[0051] If the collision occurs, examine the constraint planning set for each drone. If at least one of the currently plannable locations for the drone does not belong to the collision risk zone, calculate the Euclidean distance between the location and the drone's local location, and select the location with the smallest Euclidean distance as the correction amount for the local location.

[0052] In other words, by examining the allowable offset range of the constraint planning set, we can find all adjustable position points corresponding to the allowable offset range and the current local position. As long as there is a position point that does not belong to the collision risk zone, it means that a safe and feasible solution can be found within the allowable offset range. A feasible solution means that only the local position of a single UAV needs to be modified, without changing the centroid coordinates of the formation.

[0053] If all location points belong to the collision risk zone, the centroid coordinates of the current drone formation are adjusted, and the adjustment amount of the centroid coordinates is synchronized to the local position of each drone. The position solution of the location points and the collision risk zone is re-examined to see if there are any location points that do not belong to the collision risk zone, and the correction amount of the local position is obtained accordingly.

[0054] When all locations are within the collision risk zone, it means that all allowable offset intervals have been exhausted, indicating an infeasible solution where the collision risk cannot be avoided. An infeasible solution means that the formation centroid adjustment needs to be triggered, and the local positions of each UAV need to be re-determined to complete the collision coordinate correction.

[0055] Furthermore, when applying the centroid coordinate adjustment of the formation, the adjustment size is selected as the equivalent radius set by the collision risk zone, with 0.5-1.5 times the equivalent radius as the fine-tuning value. The entire UAV formation is translated and offset as a rigid whole. Then, it is determined whether there is a position point of the UAV in the formation that does not belong to the collision risk zone. The adjustment is continuously made until there is a feasible solution for the corresponding UAV formation. Based on the adjusted UAV formation, the coordinates of each UAV in the formation are identified simultaneously, and finally the correction amount of each UAV is set.

[0056] S43, based on the corrected local position and combined with the centroid coordinates at that moment, calculates the global coordinates of each UAV in the formation; for each UAV, the corrected local position is converted into global coordinates to identify the relative position of each UAV formation.

[0057] S44, based on the global coordinates of each UAV in the formation, compares the relative distances between UAVs at different time points, and regards the coordinates that meet the requirements of feasibility and safety as the corrected UAV coordinates.

[0058] Based on the adjusted global coordinates of the drone formation, check whether each drone formation has generated a new collision risk zone. Return the coordinates of these drones to the judgment process in step S2, and obtain the output drone coordinates when no collision risk zone is generated.

[0059] S5. Obtain the corrected UAV coordinate sequence and perform state encoding on all UAVs according to their formation to determine the path range of each UAV formation; where, state encoding refers to converting the coordinate changes of the UAVs into the form of specific feature vectors to record the movement state of each UAV.

[0060] In one embodiment of the present invention, the UAV coordinates after S4 verification are obtained and sorted chronologically to form a UAV coordinate sequence. Then, by adding a formation probability mapping, the overall path range of each formation is determined. Here, the path range refers to the spatial region where the cumulative probability of the probability density function reaches a specific threshold; it represents the set of spatial locations where the formation is highly likely to appear in the next moment or within a future period. Compared to the initial planned route that directly considers a single trajectory, this range incorporates the uncertainties of control errors, environmental disturbances, and obstacle avoidance residues, providing a more robust reference for subsequent UAV planning.

[0061] like Figure 4 As shown, one implementation of step S5 includes: S51, for the acquired UAV coordinate sequence, calculate the speed of each UAV, and use the coordinates and speed of the UAV at different positions as the feature vector of each UAV.

[0062] In this step, the drone coordinates after obstacle avoidance are obtained in step S4. The instantaneous velocity at each coordinate is calculated according to the path coordinates at each time period. The instantaneous velocity and feature vector are entered into the feature vector to record the relative state of the drone's flight.

[0063] S52 performs aggregation operations on the feature vectors within the same UAV formation to obtain the feature vectors of each UAV group.

[0064] For each UAV's feature vector, all UAVs are grouped according to the formation number stored in the flight data to form a data set of all UAVs in the same formation. Attention pooling, mean aggregation and other data aggregation operations are performed on the data in the data set to transform the recorded coordinate values ​​and velocity values ​​into high-dimensional feature vectors to record the newly formed cooperative relationships within the formation under the corrected coordinates.

[0065] Specifically, mean aggregation is achieved by taking the arithmetic mean of the feature vectors of all drones in the formation, resulting in the mean of the drone formation in the probability density operation. Attention pooling aggregation, on the other hand, calculates adaptive attention weights for the drone feature vectors and then averages them according to these weights to set a mean that includes attention. In this attention weighting method, the ratio of each feature to the sum of the features within the drone formation is used as the attention weight to obtain the mean that includes attention, thereby improving the sensitivity of path range estimation to risk.

[0066] S53, based on the feature vector of the UAV formation, maps and generates a probability density function, and solves the probability value of each point for all corresponding points in the virtual space of the formation movement.

[0067] Each drone formation's feature vector represents a set of overall state characteristics formed by the entire formation after obstacle avoidance adjustments. When calculating the probability value, the velocity and coordinate values ​​in the feature vector are first converted into a covariance matrix, and the probability value of all corresponding points in the formation within the virtual space is quantified according to the probability density function.

[0068] The covariance matrix is ​​expressed as: ;in, The covariance matrix is ​​calculated using the coordinates and velocities of the UAVs currently in the formation. Represents the variance along the x-axis. Represents the variance of the x and y axes. Represents the variance along the x and z axes; Represents the variance of the y-axis. Represents the variance along the y and z axes. The variances represent the z-axis variances, which indicate the correlation of positional deviations of the UAV formation across different coordinates.

[0069] Furthermore, since the covariance matrix is ​​derived from velocity and coordinate values, when setting the covariance matrix, it is necessary to obtain the eigenvectors corresponding to the coordinate and velocity values ​​through high-dimensional feature transformation, or to set multiple covariance matrices according to the combination of velocity and coordinate, including position-to-position, velocity-to-velocity, and position-to-velocity methods, to interpret the covariance matrix at the corresponding time.

[0070] ;in, Represents the covariance matrix of the entire state. The covariance matrix representing the intersection of positions is the dynamic position covariance in the current path planning scenario; This represents the covariance matrix of the velocity intersections. and This represents the covariance matrix of the position-velocity crossover.

[0071] In this scenario, the intersection of velocities and positions requires the application of a dynamically updated covariance matrix. The covariance matrix at the current moment is combined with the covariance matrix predicted for the next moment to update the probability density function at each moment.

[0072] During the update, a Kalman filter is used: ;in, The covariance matrix at time k+1 points to the predicted data content at the next time step. The covariance matrix at time k represents the relative values ​​at the current time. Let represent the Jacobian matrix, which is a matrix of partial derivatives with respect to the eigenvectors. Its function is to transfer the state from the previous time step to the current time step, thus recording the uncertainty of the multi-time step derivation process; T represents the matrix transpose operation. The process noise covariance matrix is ​​used to record the contribution of acceleration noise to the position covariance and velocity covariance, that is, by converting the random fluctuations of acceleration into additional uncertainties in position and velocity.

[0073] At this point, the dynamic covariance matrix formed by the intersection of positions and the covariance matrix formed by the intersection of velocities are set up, and these matrices are simplified and then entered into the probability density function.

[0074] Furthermore, when the feature vector corresponding to the location is used as input, the probability density function is expressed as: ;in, This represents the probability density function value, indicating the drone's position at a specific geometric point. The relative likelihood at the location; This represents an exponential function with base e. Represents the three-dimensional coordinates of a specific geometric point, where T represents the matrix transpose operation; Represents pi; This represents the mean of the feature vectors of the drones in the current formation. This completes the probability mapping of the drone states, and yields the probability values ​​for different virtual points under obstacle avoidance correction. In the calculation of the current probability density, the covariance and relative mean generated by the position are primarily used to statistically determine the probability density. When introducing feature values ​​corresponding to velocity and position, the calculation method is the same as described above; only the feature vectors representing the combination of position and velocity at geometric points need to be adjusted to complete the recording of the probability values ​​for each point.

[0075] S54, based on the probability values ​​of each point, takes the spatial region where the cumulative probability reaches the probability threshold as the path range of the current formation.

[0076] At this point, the probability value obtained from the probability density function is recorded as the cumulative probability within a specific spatial range, and then the path range for the current scene output is selected.

[0077] ;in, It represents the total probability that the drone's location falls within a certain spatial area, directly reflecting the probability value of the current drone's flight path; Representing geometric points In specific The probability density value at the coordinates; This represents a volume element in a three-dimensional rectangular coordinate system, representing a geometric point. The differential operation at the point. In this process, the path range to be solved is no longer a fixed range, but a soft boundary that changes dynamically. When the formation is in stable flight, the corresponding covariance is small, and the path range shrinks into a narrow channel. When the formation has just completed a severe obstacle avoidance correction, the corresponding uncertainty is large, and the path range expands, reminding subsequent formations to maintain a larger safe distance.

[0078] In one implementation, the probability threshold can be set to 95% or 99%. Based on the current path prediction requirements, the outer boundary of the current UAV is redefined, starting from its centroid, and used as the initial region. The probability values ​​of multiple points in the virtual space are then statistically analyzed. If the probability threshold is not reached, the outer boundary is extended proportionally outward along the initially planned route until the cumulative probability values ​​of multiple points in the virtual space reach the probability threshold. The probability values ​​statistically analyzed during this process represent the dispersion of the three-dimensional coordinates, which is the cumulative spatial uncertainty of the UAV formation throughout the entire flight mission.

[0079] S6 uses the path ranges of the preceding and following formations as comparison objects to calculate the overlap between the current formation and other formations, and determines the cooperative path of the multi-UAV formation.

[0080] In one embodiment of the present invention, step S6 is implemented by: calculating the overlap between adjacent formations based on the three-dimensional spatial volume corresponding to the path range, and setting the conflict risk level according to the value range of the overlap; wherein, the overlap represents the three-dimensional spatial intersection volume of the path ranges of two formations, and the intersection volume is divided by the union volume of the two path ranges to obtain the overlap of adjacent formations. At the same time, each overlap is bound to the distance between formations as an auxiliary indicator, and the overlap situation at the current moment is quantified by combining the minimum safe distance mentioned above.

[0081] The conflict risk level includes no conflict, low conflict risk, and high conflict risk; no conflict means that the overlap is 0, at which point the distance between the center of mass of the formation is greater than the minimum safe distance, and each UAV formation is completely separated within the obstacle avoidance path range.

[0082] Low collision risk means that the overlapping area is very small, and the overlap is less than or equal to the low overlap threshold. At this time, the path ranges of the two formations overlap only at a very small part of the edge. The real-time position spacing of all UAVs in the formation still meets the requirement that the distance between the formation centroids is greater than or equal to the minimum safe distance. The formation does not need to make large-scale changes to the path, but only needs to fine-tune the formation centroids so that the UAVs are closer to the original planned route in the space pointed to by the path range, thereby optimizing the centroid trajectory of the formation.

[0083] High collision risk occurs when the overlap is greater than the low overlap threshold, the path ranges of the two formations overlap significantly, and at least one pair of UAVs in adjacent formations have a real-time position distance less than the minimum safe distance, indicating a relative collision risk, requiring correction of the movement path range.

[0084] In one implementation, the low overlap threshold ranges from 0.05 to 0.1, which is used to explain scenarios with slight overlap of path ranges. When the value exceeds this range, the simulated path range is prone to drone collision risk at a specific time.

[0085] It should be noted that the path range is obtained by summarizing the relative probability values ​​of flight at each point in the virtual space during the process of finding the path range. Based on the update of the covariance matrix at each time step, the spatial range formed by the expansion along the initial trajectory is recorded. When performing intersection calculations, this spatial range will be used to perform intersection and union operations based on the spatial range corresponding to the cumulative probability at a certain time step, thereby determining the conflict risk at the corresponding time step. This conflict risk will be used as the predicted intersection and union situation at a specific time step to determine the conflict risk level of the UAV formation.

[0086] Based on the conflict risk level, the path range of each drone formation is modified, and the modified path is used as the output collaborative path.

[0087] When there is no conflict, it means that the current path does not need to be modified, and the current path range is directly used as the output cooperative path. In low conflict risk scenarios, the overlap is in the low threshold range, with only a small overlap at the edge of the path. The shape and size of the overall path range are not modified, but only the formation centroid is adjusted. The formation centroid of the path range is linearly fitted with the formation centroid of the initial planned route to make the formation centroid closer to the initial planned route range. Then, the finely adjusted formation centroid and path range are used as the output cooperative path.

[0088] When the risk of conflict is high, it is necessary to roll back to step S5 and reacquire the path range until the path range formed at least satisfies that the distance between the formations is greater than or equal to the minimum safe distance. The regenerated path range, together with the conflict risk level and the corresponding conflict risk level handling method, is used as the output cooperative path.

[0089] Although embodiments of the present invention have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those skilled in the art can make changes, modifications, substitutions and variations to the above embodiments within the scope of the present invention, which are still covered within the protection scope of the present invention.

Claims

1. A multi-formation cooperative path planning method based on virtual reality, characterized in that, include: S1. Obtain flight data of the UAV formation, extract the centroid coordinates of each formation from the flight data, and determine the initial state vector of each formation. S2, based on the initial state vector of each formation, define the initial state of the UAV formation as the planning starting point, and generate the initial planning route and its constraint planning set by referring to the path points and endpoints of the mission execution; S3, based on the initial planned path, performs collision response processing on the flight trajectory, representing the collision response as trajectory response and directional response, and solves the collision risk zone in virtual space; S4 maps the flight trajectory of a single UAV within the collision risk zone to a local coordinate offset with the centroid coordinate as the origin, correcting the coordinates of each UAV in the UAV formation. S5. Obtain the corrected UAV coordinate sequence, and encode the status of all UAVs according to their respective formations to determine the path range of each UAV formation. S6 uses the path ranges of the preceding and following formations as comparison objects to calculate the overlap between the current formation and other formations, and determines the cooperative path of the multi-UAV formation.

2. The multi-formation cooperative path planning method based on virtual reality according to claim 1, characterized in that, The initial state vector in step S1 is implemented in the following ways: S11, Calculate the centroid coordinates of each drone formation based on the average coordinates of all drones in the formation; S12, establish a global coordinate system for multiple drone formations based on the centroid coordinates of each drone formation; S13 tracks the movement trajectory of each UAV formation in the global coordinate system and defines the initial state vector of each formation according to the flight formation corresponding to the movement trajectory.

3. The multi-formation cooperative path planning method based on virtual reality according to claim 1, characterized in that, When generating the initial planned route and its constraint planning set in step S2, the implementation methods include: S21, import the initial state vectors of each formation, take the position corresponding to the initial state vector as the planning starting point, connect each waypoint and the end point, and form the initial planned route through smooth curve interpolation. S22: Track the initial planned route using the centroid coordinates of the drone formation. At any given time, obtain the relative position of each drone in the formation with respect to the centroid coordinates at that time, construct the formation offset matrix, and determine the allowable offset range for each drone. S23, read the minimum safe distance between different formations, and use the minimum safe distance and the allowable offset interval as the output constraint planning set.

4. The multi-formation cooperative path planning method based on virtual reality according to claim 1, characterized in that, The implementation methods for the collision risk zone in step S3 include: S31, Project the centroid coordinates of the current drone formation onto the initially planned route to obtain the corresponding projection point; S32 detects the projected arc length of the current drone formation and adjacent formations on the same initial planned route, and defines the spatial area corresponding to the projected arc length as the collision risk zone of the trajectory response when the projected arc length is less than the minimum safe longitudinal distance. S33, in the prediction time domain, for each centroid point, solve for the outer boundary of the UAV formation at the corresponding time; S34. The intersection of the outer boundaries of each UAV formation is calculated. When there is an intersection at the outer boundaries, the spatial region corresponding to the intersection is regarded as the collision risk zone for directional response.

5. The multi-formation cooperative path planning method based on virtual reality according to claim 4, characterized in that, When solving for the outer boundary of the UAV formation at a given time, the implementation methods include: The initial planned route is sampled at a fixed time step to obtain discrete centroid points; For each centroid point, calculate the formation's attitude angles based on the tangent direction of the initial planned route at the current moment; and obtain the rotation matrix of the current formation based on the calculated attitude angles. Set the equivalent radius of the horizontal dimension based on the maximum horizontal distance of all UAVs in the formation from the centroid point; set the height range of the vertical dimension based on the maximum vertical distance of all UAVs in the formation from the centroid point. Based on the rotation matrix, the equivalent radius and altitude range are rotated to obtain the outer boundary of the UAV formation at the corresponding moment.

6. The multi-formation cooperative path planning method based on virtual reality according to claim 1, characterized in that, When correcting the coordinates of each drone within the drone formation in step S4, the implementation method includes: S41, obtain the centroid coordinates of the UAV formation at the current moment, apply path offset to each UAV based on the centroid coordinates, and obtain the local position of the UAV in the local coordinate system of the formation; S42, determine whether the local position corresponding to the path offset falls into the collision risk zone, and set the correction amount of the local position in combination with the constraint planning set; S43, based on the corrected local position and combined with the centroid coordinates at that moment, calculate the global coordinates of each UAV in the formation; S44, based on the global coordinates of each UAV in the formation, compares the relative distances between UAVs at different time points, and regards the coordinates that meet the requirements of feasibility and safety as the corrected UAV coordinates.

7. The multi-formation cooperative path planning method based on virtual reality according to claim 6, characterized in that, When determining whether a local location corresponding to a path offset falls within a collision risk zone, the implementation methods include: If it falls into the collision risk zone, check the constraint planning set of each drone. If there is at least one location point that does not belong to the collision risk zone among the current plannable locations of the drone, calculate the Euclidean distance between the location point and the local location of the drone, and select the location point with the smallest Euclidean distance as the correction amount for the local location. If all locations are within the collision risk zone, the centroid coordinates of the current drone formation are adjusted, and the adjustment amount is synchronized to the local locations of each drone. The system then re-examines whether any locations are outside the collision risk zone and obtains the correction amount for the local locations.

8. The multi-formation cooperative path planning method based on virtual reality according to claim 1, characterized in that, The implementation methods for the path range in step S5 include: S51, For the acquired drone coordinate sequence, calculate the speed of each drone, and use the coordinates and speed of the drone at different positions as the feature vector of each drone; S52 performs aggregation operations on the feature vectors within the same UAV formation to obtain the feature vectors of each UAV group. S53, based on the feature vector of the UAV formation, maps and generates a probability density function, and solves the probability value of each point for all corresponding points in the virtual space of the formation movement. S54, based on the probability values ​​of each point, takes the spatial region where the cumulative probability reaches the probability threshold as the path range of the current formation.

9. A multi-formation cooperative path planning method based on virtual reality according to claim 8, characterized in that, The probability density function is expressed as: ； in, Represents the probability density function value. Represents the covariance matrix. This represents an exponential function with base e. Represents the three-dimensional coordinates of a geometric point, and T represents the matrix transpose operation; Represents pi; This represents the mean of the feature vectors of the drones in the current formation.

10. The multi-formation cooperative path planning method based on virtual reality according to claim 1, characterized in that, The implementation methods of the cooperative path in step S6 include: Based on the three-dimensional spatial volume corresponding to the path range, the overlap between adjacent formations is calculated, and the conflict risk level is set according to the value range of the overlap. Based on the conflict risk level, the path range of each drone formation is modified, and the modified path is used as the output collaborative path.

Images