Method for planning and safety control of unmanned aerial vehicle formation movement layout under multiple constraints

Abstract

The application discloses a kind of multi-constraint conditions under unmanned plane formation movement layout planning and safety control method, adopts jump point search algorithm to combine safe flight corridor and constructs global feasible path and external constraint;Based on control barrier function, establish dynamic constraint adjustment rhythm, realize the self-adapting expansion and contraction of formation mode to environment boundary;Utilize Venn diagram to realize ellipse formation internal space division and member distribution;For each unmanned plane, establish pre-stable cascade closed-loop tracking control system, realize stable tracking to reference instruction, and construct Lyapunov function for representing system tracking error energy;Introduce explicit reference management mechanism, with dynamic safety margin and navigation field as core, realize the asymptotic advance of reference trajectory and constraint feasibility by Lyapunov constraint.

Description

Technical Field

[0001] This invention relates to the field of UAV cooperative control and path planning technology, specifically to a method for planning and controlling the formation motion layout of UAVs under multiple constraints, which is particularly applicable to the safe maneuvering and adaptive morphological adjustment of multiple UAVs in complex and restricted environments. Background Technology

[0002] In recent years, drone swarms have demonstrated enormous application potential in areas such as regional patrols, material delivery, and disaster relief. Unlike the motion planning of a single drone, the core of drone swarm movement lies in simultaneously maintaining the overall orderliness of the formation and the safe distance constraints between individual drones.

[0003] Traditional formation control methods often employ a leader-follower or rigid virtual structural framework, focusing on maintaining fixed relative positions between individuals. While this approach is beneficial for achieving stable formations, it has significant limitations in actively adapting to environmental constraints in complex and obstacle-filled confined environments (such as narrow corridors or dense building complexes): when the width of the environmental passage is less than the width of the rigid formation, the formation is highly susceptible to collisions with obstacles.

[0004] To maintain the safe passage of the entire formation in confined environments, a planning method capable of proactively sensing the environment and adaptively changing the formation shape is urgently needed. Existing methods utilize model predictive control to achieve shape changes, but due to the need for online rolling optimization in multi-dimensional space, the computational burden is extremely high, making it difficult to meet the real-time requirements of UAV formations. Furthermore, simple path planning often ignores the control response latency and physical actuator saturation issues of the underlying UAVs, failing to provide strict safety guarantees for the UAV formation system during dynamic deformation processes.

[0005] Therefore, how to establish a unified mapping relationship between outer trajectory planning, internal formation layout and dynamic constraint control, and how to use low-computation and high-security control strategies such as control obstacle functions and explicit reference management to achieve adaptive scaling of formation to environmental boundaries and strict maintenance of safety feasibility are key technical problems that urgently need to be solved in the field of multi-UAV cooperative control. Summary of the Invention

[0006] Purpose of the invention: This invention provides a method for planning and controlling the formation motion layout of UAVs under multiple constraints. The formation is abstracted as a variable ellipse with a constant area. By using the obstacle control function and explicit reference management, a joint dynamic adjustment law of morphological adaptation and trajectory advancement is established. Under the premise of ensuring physical constraints and obstacle avoidance constraints, the formation can achieve efficient and safe cooperative maneuvering in complex and restricted environments.

[0007] Technical solution: The present invention provides a method for planning and controlling the formation motion of unmanned aerial vehicles (UAVs) under multiple constraints, characterized by comprising the following steps:

[0008] S1. Based on the jump point search algorithm combined with the safe flight corridor, construct a globally feasible path for the formation center to provide external safe passage constraints for the formation.

[0009] S2. Based on the control obstacle function, establish a dynamic constraint adjustment law for the position of the ellipse center and the aspect ratio of the major and minor axes to realize the adaptive scaling of the formation shape to the environmental boundary.

[0010] S3. Based on the Veno diagram, realize the spatial division and member position distribution within the elliptical formation motion layout to maintain topological connectivity and spatial uniformity during formation flight.

[0011] S4. Establish a pre-stabilized cascaded closed-loop tracking control system for each UAV in the formation to achieve stable tracking of reference commands, and construct a Lyapunov function to characterize the tracking error energy of the system.

[0012] S5. Taking the constructed global dynamic safety margin and navigation field as the core, the final command is generated through an explicit reference management mechanism, and the asymptotic advancement of the reference trajectory and the maintenance of multi-constraint feasibility are realized by combining Lyapunov constraints.

[0013] The specific implementation process of S1 is as follows:

[0014] By using a jump point search algorithm to search for jump points and forced neighbors in an environmental grid map, a collision-free discrete segmented path connecting the starting point and the target point is generated.

[0015] Constructing a safe flight corridor around each line segment of the discrete segmented path involves: fitting an initial ellipsoid based on the path line segments, and shortening the lengths of the remaining axes while maintaining alignment of the major axis along the line segment direction until the interior of the ellipsoid contains no obstacle points.

[0016] A half-space constraint is generated by creating a tangent plane at the contact point between the ellipsoid and the obstacle. After removing the contact point, the ellipsoid is expanded and a new contact point is searched repeatedly to obtain a series of half-space sequences.

[0017] The intersection of the above half-space sequences forms a convex polyhedron containing the path segment. Several adjacent and overlapping convex polyhedra are pieced together to form a safe flight corridor, thereby transforming environmental obstacle avoidance into an explicit linear inequality constraint of the polyhedron:

[0018]

[0019] in, The location is the center of the ellipse. To characterize the ratio of the major and minor axes of an ellipse With the positive definite matrix of attitude, The outer normal vector of the safe half-plane, This is the intercept.

[0020] The dynamic constraint adjustment law for establishing the ellipse center position and the aspect ratio of the major and minor axes based on the control obstacle function, as described in S2, specifically includes:

[0021] The overall envelope of the formation is abstracted into a parameterized elliptical virtual structure with a constant coverage area, and the ratio of the major and minor axes of the ellipse is adjusted. To achieve the expansion and compression of formation patterns;

[0022] Calculate the distance from each hyperplane on the boundary of the safe flight corridor to the nearest point of the formation ellipse envelope. Furthermore, a smoothing factor is introduced to fuse all nearest point distances into a smoothed shortest distance, which is then used to construct a control barrier function. :

[0023]

[0024] in, As a smoothing factor, The distance from each hyperplane on the boundary of the safe flight corridor to the nearest point of the formation ellipse envelope, Represents the smooth shortest distance. The corresponding number of safe half-spaces; to obtain the time derivative of the control obstacle function along the trajectory of the UAV formation system, the control obstacle function is calculated respectively. Reference value for the center position of the ellipse partial derivatives and reference value for the ratio of major to minor axis derivative Furthermore, a dynamic safety set is established to characterize the safety margin between the formation envelope and the global feasible domain boundary, and a dynamic constraint adjustment law is constructed based on this.

[0025] The execution logic of the dynamic constraint adjustment law is as follows:

[0026] To ensure that the state of the UAV formation system always remains within the safe set, so that the control obstacle function satisfies ,in For the control barrier function The time first derivative of the UAV formation system trajectory is used to introduce a safety correction term for the center position of the ellipse. Morphological safety correction term relative to major and minor axis ratio ;

[0027] When the drone formation system approaches the corridor boundary, the constraints tighten, i.e., the initial function... When calculating dynamic constraint adjustment compensation, its analytical form is as follows:

[0028]

[0029]

[0030] in, This indicates a safety correction term for the center position of the ellipse. This represents a morphological safety correction term for the ratio of the major and minor axes of an ellipse. For adaptive adjustment class functions, , This represents the initial time derivative of the control barrier function driven by the nominal reference instruction. , and This represents the regularization parameter introduced to prevent singularities in numerical computation;

[0031] By correcting the negative drift caused by the center motion and suppressing unfavorable shape changes, the formation shape can adaptively expand and contract to the environmental boundary.

[0032] The implementation process of S3 is as follows:

[0033] A large number of sampling points are generated within the elliptical virtual structure. The Euclidean distance from the sampling point to the projected position of the formation member is calculated and assigned to the nearest member to form discrete Vino units.

[0034] Calculate the geometric centroid of the corresponding Vino element for each member, and update the projected position of each member using an under-relaxation iterative method until the preset centroid Vino equilibrium convergence condition is met, thus obtaining the uniform spatial distribution of each member within the elliptical safe boundary.

[0035] The implementation process of S4 is as follows:

[0036] The kinematic and dynamic model of the UAV is established as follows:

[0037]

[0038] in, , , , The first The first derivatives of the drone's position, velocity, attitude angle, and angular velocity. , The first The position and speed of the drone For quality, For total thrust, For rotation matrix, It is the acceleration due to gravity. It is a unit vector. For attitude angle, For the first The angular velocity of the drone Let the moment of inertia of the drone be... To control the torque, External disturbance;

[0039] A dual closed-loop control structure for position and attitude is adopted, wherein the outer position controller and the inner attitude controller are designed as follows:

[0040]

[0041] in, This is the output of the outer position controller. The reference position command assigned to the UAV. , This is the positive definite gain matrix of the position outer layer controller; , To fuse the observed states of the extended state observer, , Here is the positive definite gain matrix of the inner-loop attitude controller. For attitude error, the fused extended state observer is defined as follows:

[0042]

[0043] In the formula, The rate of change of the attitude error estimate. The observation error is the attitude error. This is the estimated attitude error value. and For the first derivative of the state of the fused extended state observer, , , For observer gain, , The exponential parameter representing the nonlinear gain range. , This represents the smoothing threshold parameter that determines the linear interval. To The smoothing improvement of a function is defined as follows:

[0044]

[0045] in, To ensure a sufficiently small quantity; As the independent variable, Let be the integration variable in the Gaussian convolution integral process, β be the nonlinear exponential parameter, δ be the linear interval threshold parameter, and sgn(·) be the standard sign function; construct a state including position and velocity tracking error states. Laypunov quadratic function ,in Here is the Lyapunov weight matrix. This serves as an energy representation of the current tracking error in the pre-stabilized cascaded closed-loop tracking control system, and is used as a direct input variable for subsequent calculations of the dynamic safety margin.

[0046] The process of constructing the global dynamic safety margin described in S5 is as follows:

[0047] First, a multi-constraint model of the drone formation is established, including:

[0048] Thrust saturation constraint: ,in This represents the maximum thrust available to the drone.

[0049] Internal collision avoidance constraints: ,in , Let the positions of any two drones be given. To ensure the safe collision avoidance radius for drones;

[0050] Error tracking constraints: ,in The maximum allowable position tracking error;

[0051] Safe flight corridor constraints: This is used to ensure that the formation as a whole does not collide with external obstacles;

[0052] For the aforementioned multiple constraints, calculate the corresponding Lyapunov safety thresholds. :

[0053] Threshold for thrust saturation constraint in operational constraints :

[0054]

[0055] in, This represents the thrust constraint coefficient vector. , , This is the thrust direction vector;

[0056] Tracking error threshold in operational constraints :

[0057]

[0058] in, For matrix The first element;

[0059] Safe corridor threshold in environmental constraints :

[0060]

[0061] in, Let be the relative position vector of the member in the virtual coordinate system. Characterizes the attitude rotation of an ellipse;

[0062] The Lyapunov threshold values ​​for each constraint minus the Lyapunov function values ​​of the current state of the pre-stabilized cascaded closed-loop tracking control system. The global dynamic security margin is calculated. :

[0063]

[0064] in, The global dynamic security margin is a set of integer indices. As a continuous dynamic gain coefficient, it directly determines the update rate of the reference instruction; specifically, the update rate of the reference instruction is related to... The numerical values ​​are positively correlated: when At that time, the drone swarm system was in a safe state, and The larger the value, the faster the reference instruction update rate; when When the value approaches 0, it indicates that the drone formation system is approaching the constraint boundary. The update rate of the reference command then decreases proportionally to 0, thereby automatically slowing down and eventually stopping the update of the reference command, waiting for the underlying drones to catch up with the reference trajectory.

[0065] The process of generating the final instruction through the explicit reference management mechanism described in S5 is as follows:

[0066] Design attraction fields pointing towards the target point respectively With form attraction field :

[0067]

[0068]

[0069] in, For the local target path points of the current safe corridor segment, This is a reference value for the ratio of major to minor axis. This is a reference value for the center position of the ellipse. As a smoothing factor, For the desired nominal shape reference value, For shape tracking gain;

[0070] Global dynamic security margin As a nonlinear gain, combined with the target attraction field and obstacle avoidance correction field The superimposed total navigation field:

[0071]

[0072] Finally, the dynamic regulation equation for the ERG output expansion state is as follows:

[0073]

[0074] in, and These are the update rates of the reference instructions for the center position of the output ellipse and the reference instructions for the major and minor axis ratio parameters, respectively. For the overall location navigation field, For the overall shape navigation field, The global path points are planned by the jump point search algorithm. It is a positive definite adjustment coefficient.

[0075] Beneficial effects: Compared with the prior art, the beneficial effects of the present invention are as follows:

[0076] 1. This invention overcomes the defect of traditional rigid formations being prone to collisions in confined spaces. By introducing a parametric elliptical virtual structure with variable major and minor axes and using a centroid Venn diagram to achieve a uniform spatial distribution of members, the formation can freely extend and compress in narrow corridors.

[0077] 2. This invention creatively applies the control obstacle function to the adjustment of the geometric shape parameters of the formation. Through rigorous mathematical derivation, a dynamic adjustment law of the shape parameters is established. Once the formation envelope is detected to be approaching the boundary of the safe flight corridor, the UAV formation system can automatically output a safety correction gradient to guide the formation to stretch and narrow to pass safely, thus ensuring zero collision from the theoretical level.

[0078] 3. Based on the framework of Control Barrier Function (CBF) and explicit reference management technology, this invention transforms multidimensional constraints into a scalar of dynamic safety margin and a safety correction term in analytical form; the calculation process involves only algebraic operations and gradient solving, which greatly reduces the computational burden of the airborne computer and meets the requirements of high-frequency real-time control of UAVs.

[0079] 4. The exponential logarithmic summation mechanism was used to handle the distance abrupt change and non-differentiability problem at the intersection of multiple boundaries, ensuring the smoothness of the constraint adjustment process; at the same time, the Lyapunov constraint was combined to limit the forward speed of the reference trajectory, avoiding the saturation and instability of the UAV actuators caused by the rapid change of commands. Attached Figure Description

[0080] Figure 1 This is a flowchart of the present invention;

[0081] Figure 2 Diagram for constructing a safe flight corridor;

[0082] Figure 3 Simulation results of formation movement layout planning;

[0083] Figure 4 The diagram shows the formation movement layout; where (a) is the initial state; (b) is moving towards the first channel; (c) is passing through the first channel; and (d) is leaving the first channel.

[0084] Figure 5 This is a graph showing the change in the ratio of the major and minor axes of the ellipse during operation.

[0085] Figure 6 The graph shows the derivative of CBF with respect to the ratio of major to minor axes.

[0086] Figure 7 This is a graph showing the global dynamic safety margin changes. Detailed Implementation

[0087] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0088] like Figure 1 As shown, this invention provides a method for planning and controlling the motion layout of UAV formations under multiple constraints. Taking an abstract model of the entire formation as the starting point, it unifies outer path search, internal morphology adjustment, and dynamic safety control within a single analytical control framework. The specific implementation process mainly includes the following steps:

[0089] Step S1: Construct a globally feasible path based on the jump point search algorithm combined with the safe flight corridor.

[0090] After determining the internal layout, it is necessary to plan an external safety passage for the formation center.

[0091] First, a jump point search algorithm is employed in a known 3D or 2D raster map. This algorithm optimizes the traditional A* algorithm by pruning, and quickly generates a collision-free discrete segmented path connecting the start and end points by identifying "jump points" and "forced neighbors" in the map.

[0092] Subsequently, a continuous safe flight corridor is constructed around this discrete path. Specifically, an initial ellipsoid is fitted based on each line segment of the path. While keeping the major axis direction along the line segment unchanged, the lengths of the other axes are gradually shortened until the ellipsoid no longer contains any obstacle points. A tangent plane is then drawn at the contact point between the ellipsoid and the obstacle, thus generating a half-space constraint. ; Let be a vector at any position in space; Let this be the external normal vector of the tangent plane; This is the intercept of the tangent plane. Remove the current contact point, continue expanding the ellipsoid, and repeat the search for new contact points, resulting in a series of half-space sequences.

[0093] Combining the intersections of these half-spaces forms a convex polyhedron containing the path segment. Several overlapping convex polyhedra are then joined together to form a safe flight corridor. For any side interface of the safe flight corridor... The constraints on the virtual structure of the formation ellipse can be explicitly expressed as:

[0094]

[0095] Here, It is a positive definite matrix, representing the attitude rotation of the ellipse. and by scaling matrix It determines the characteristics of the major and minor axes. The physical meaning of this inequality is: the center of the ellipse... In the normal vector The projection onto the ellipse, plus the maximum extension of the ellipse in that direction, must be less than the intercept of the boundary. .

[0096] Step S2: Establish a dynamic constraint adjustment law based on the control barrier function (CBF) to achieve adaptive morphological scaling.

[0097] The above inequalities are merely static geometric constraints. To actively drive changes in formation configuration to adapt to the environment during the dynamic flight of the UAV, this invention creatively introduces a control obstacle function.

[0098] Set member position distribution (Assuming the formation flies at a constant altitude, primarily considering deformation of the horizontal plane), the radii of the major and minor axes of the ellipse are respectively... and To ensure that the coverage area remains constant, an area invariance constraint is introduced:

[0099]

[0100] in, These are preset constants. The aspect ratios of the major and minor axes of the ellipse are further defined. .when At that time, the formation is stretched along the flight direction, and the coverage area increases along the heading direction. At this time, the formation compresses laterally to adapt to narrow passages or areas with dense obstacles, thereby achieving adaptive adjustment of the formation shape.

[0101] For multiple boundary surfaces of a convex polyhedral corridor, the distance from each hyperplane of the safe flight corridor boundary surface to the nearest point of the formation ellipse envelope is... There are multiple candidate values. If we directly take the minimum value... When switching polygons, the distance function becomes non-differentiable, causing severe oscillations in the underlying control law. Therefore, this invention introduces a smoothing factor. The smooth shortest distance is constructed using the Log-Sum-Exp form and used as the control barrier function. :

[0102]

[0103] in, Represents the smooth shortest distance. The distance from each hyperplane on the boundary of the safe flight corridor to the nearest point of the formation ellipse envelope, Given the corresponding number of safe half-spaces, this function maintains global differentiability while preserving conservative safety.

[0104] To establish a dynamic regulation law, it is necessary to obtain... First-order time derivative along the trajectory of the UAV formation system:

[0105]

[0106] in, This is a reference value for the center position of the ellipse. This serves as a reference value for the ratio of the major axis to the minor axis. The corresponding partial derivatives are obtained using the central difference method or the analytical chain rule.

[0107] The dynamic constraint adjustment law is a safety correction feedback mechanism built based on the dynamic safety set and derivative information. Its specific execution logic is as follows: According to the CBF safety forward invariance theorem, as long as the UAV formation system satisfies... The drone swarm system will remain permanently in a safe assembly state.

[0108] When the environment is open, the drone swarm system advances according to the nominal target; when the swarm approaches the corridor boundary, it causes... At that time, this method actively calculates a gradient-based safety correction term:

[0109]

[0110]

[0111] in, This indicates a safety correction term for the center position of the ellipse. This represents a morphological safety correction term for the ratio of the major and minor axes of an ellipse. For adaptive adjustment class functions, This represents the initial time derivative of the control barrier function driven by the nominal reference instruction. This represents the initial time derivative of the control barrier function driven by the nominal reference instruction. , and This represents the regularization parameter introduced to prevent singularities in numerical computation. It provides a repulsive field, pushing the formation center away from obstacles; and Then, a shape change occurs; when the channel narrows, it forces the ratio of the major axis to the minor axis. The increased size narrows the formation, allowing it to pass through smoothly.

[0112] Step S3: Based on the Venn diagram, realize the spatial division and member position distribution within the elliptical formation motion layout.

[0113] In order to deploy within the ellipse This invention employs a self-organizing distribution model based on centroid Vino partitioning for unmanned aerial vehicles (UAVs). Specifically:

[0114] Generate a large number of sampling points within a given parametric elliptical safety boundary, and calculate the value of each sampling point. Projected positions of each member The Euclidean distance (at the k-th iteration position) divides the space into non-overlapping Vino units. Then, the geometric centroid of each element is calculated. The member positions are updated using the Lloyd iteration algorithm and an under-relaxation strategy:

[0115]

[0116] in , Indicates the under-relaxation coefficient. This represents the position in the (k+1)th iteration. The system converges when the position of each member coincides with the centroid of its control domain, at which point the formation members achieve optimal spatial uniformity within the elliptical envelope.

[0117] Step S4: Establish a pre-stabilized cascaded closed-loop tracking control system for each UAV in the formation to achieve stable tracking of reference commands, and construct a Lyapunov function to characterize the tracking error energy of the pre-stabilized cascaded closed-loop tracking control system.

[0118] The operation of an explicit reference management mechanism requires a stable underlying system. This step aims to provide interference-resistant low-level flight control for each UAV within the formation, ensuring its stable tracking of reference commands issued from the upper level.

[0119] Specifically, the kinematic and dynamic model of the UAV is established as follows:

[0120]

[0121] in, , , , The first The first derivatives of the drone's position, velocity, attitude angle, and angular velocity. , The first The position and speed of the drone For quality, For total thrust, For rotation matrix, It is the acceleration due to gravity. It is a unit vector. For attitude angle, For the first The angular velocity of the drone Let the moment of inertia of the drone be... To control the torque, This is an external disturbance.

[0122] A dual closed-loop control structure for position and attitude is adopted, wherein the outer position controller and the inner attitude controller are designed as follows:

[0123]

[0124] In the position controller, This is the output of the outer position controller. The reference position command assigned to the UAV. , This is the positive definite gain matrix of the position outer layer controller; , To fuse the observed states of the extended state observer, , Here is the positive definite gain matrix of the inner-loop attitude controller. For attitude error, the fused extended state observer is defined as follows:

[0125]

[0126] In the formula, The rate of change of the attitude error estimate. The observation error is the attitude error. This is the estimated attitude error value. and For the first derivative of the state of the fused extended state observer, , , For observer gain, , The exponential parameter representing the nonlinear gain range. , This represents the smoothing threshold parameter that determines the linear interval. To The smoothing improvement of a function is defined as follows:

[0127]

[0128] in, To ensure a sufficiently small quantity; As the independent variable, Let be the integration variable in the Gaussian convolution integral process, β be the nonlinear exponential parameter, δ be the linear interval threshold parameter, and sgn(·) be the standard sign function; construct a state including position and velocity tracking error states. Laypunov quadratic function ,in Here is the Lyapunov weight matrix. This serves as an energy representation of the current tracking error in the pre-stabilized cascaded closed-loop tracking control system, and is used as a direct input variable for subsequent calculations of the dynamic safety margin.

[0129] Step S5: Introduce an explicit reference governor (ERG) mechanism centered on dynamic safety margin (DSM) and navigation field, and combine it with Lyapunov constraints to achieve asymptotic advancement of the reference trajectory and maintain the feasibility of multiple constraints.

[0130] While CBF addresses collision avoidance in the environmental space, the underlying limitations of UAVs include thrust saturation, attitude tracking errors, and collision avoidance among crew members. To achieve global coordination, this invention introduces an explicit reference management mechanism at the reference trajectory layer.

[0131] Specifically, based on the Lyapunov function mentioned above Derive the Lyapunov safety threshold under various constraints. First, a multi-constraint model is established. In a confined space, the safe flight of a drone formation must simultaneously satisfy operational and environmental constraints. The specific modeling is as follows:

[0132] Thrust saturation constraint: Due to the physical performance limitations of the motor and blades, the total thrust of a single unit has an upper limit of saturation, i.e.

[0133]

[0134] in This represents the maximum thrust available to the drone.

[0135] Internal collision avoidance constraints: Within the formation, each drone must maintain sufficient spatial separation to prevent collisions. For any two drones... It should satisfy:

[0136]

[0137] in , Let the positions of any two drones be given. The safe collision avoidance radius for drones.

[0138] Error tracking constraint: Considering the limited reference update speed and closed-loop transient response characteristics, the deviation between the actual position and the reference position needs to be limited, i.e.:

[0139]

[0140] in This represents the maximum allowable position tracking error.

[0141] Safe flight corridor constraints: To ensure that the formation as a whole does not collide with external obstacles, the elliptical envelope must be constrained within the convex polyhedron of the safe flight corridor, i.e., satisfying:

[0142]

[0143] For the aforementioned multiple constraints, calculate the corresponding Lyapunov safety thresholds. :

[0144] Threshold for thrust saturation constraint in operational constraints :

[0145]

[0146] in, This represents the thrust constraint coefficient vector. , , This is the thrust direction vector.

[0147] Tracking error threshold in operational constraints :

[0148]

[0149] in, For matrix The first element.

[0150] Safe corridor threshold in environmental constraints :

[0151]

[0152] in, Let be the relative position vector of the member in the virtual coordinate system. Characterizes the orientation rotation of an ellipse.

[0153] The minimum dynamic safety margin of each member in the formation is defined as the global dynamic safety margin, from which we can obtain:

[0154]

[0155] in, The global dynamic security margin is a set of integer indices. As a continuous dynamic gain coefficient, it directly determines the update rate of the reference instruction; specifically, the update rate of the reference instruction is related to... The numerical values ​​are positively correlated: when At that time, the drone system was in a safe state, and The larger the value, the faster the reference instruction update rate; when When the value approaches 0, it indicates that the UAV system is approaching the constraint boundary. The update rate of the reference command then decreases proportionally to 0, thereby automatically slowing down and eventually stopping the update of the reference command, waiting for the underlying UAV to catch up with the reference trajectory.

[0156] To drive the formation toward the finish line, a positional attraction field and a shape attraction field need to be designed separately:

[0157] For the center of the ellipse, a segmented local target tracking strategy is adopted. Let the local target pathpoint of the current safe corridor segment be... The position attraction field is defined as:

[0158]

[0159] in, As a smoothing factor, This is a reference value for the center position of the ellipse. ( When the threshold for switching is reached, the target point is automatically switched to the next path point. .

[0160] For the ratio of the major and minor axes of an ellipse, let its expected nominal shape reference be... The shape attraction field is then defined as:

[0161]

[0162] in, For shape tracking gain, This is a reference value for the ratio of the major axis to the minor axis.

[0163] The target attraction field obtained above Compared with the obstacle avoidance and repulsion correction term obtained based on CBF in step S3 By superimposing the data, a navigation field is formed:

[0164]

[0165] Finally, the dynamic adjustment equation for the ERG output expansion state is as follows:

[0166]

[0167] in, and These are the update rates of the reference instructions for the center position of the output ellipse and the reference instructions for the major and minor axis ratio parameters, respectively. For the overall location navigation field, For the overall shape navigation field, The global path points are planned by the jump point search algorithm. The coefficient is positive definite. The above ordinary differential equation not only gives the formation center command... The update speed also provides formation commands. The update speed.

[0168] In actual flight, the onboard computer performs numerical integration calculations on the above ordinary differential equations at an extremely high frequency. Compared to traditional Model Predictive Control (MPC), which requires online solving of complex quadratic or nonlinear programming problems, the formulas in this invention are all explicit algebraic analytical expressions, consuming extremely low computational resources. Meanwhile, the DSM term... As an adaptive gain, it fundamentally eliminates the phenomenon of drone loss of control caused by sudden command changes.

[0169] To verify the effectiveness of this invention, a complex and constrained simulation environment was set up, containing multiple static obstacles forming narrow passages and corners. In this complex environment, a discrete path was first planned using a jump-point search algorithm, and based on this, a system was constructed as follows: Figure 2 The safety flight corridor shown. From Figure 2 As can be seen, the corridor is composed of a series of overlapping convex polyhedra, enclosing a collision-free free space and providing a safe external passage for the formation. Based on this corridor constraint, the final simulation results of the formation motion layout planning are as follows: Figure 3 As shown. From Figure 3 As can be seen, the formation's center trajectory was smooth and strictly confined within the safe flight corridor throughout the entire process. The formation successfully traversed the complex obstacle area from the starting point to the target point, verifying the effective combination of global path planning and low-level control. To more intuitively demonstrate the formation's adaptive deformation capability, Figure 4 The evolution of the formation's motion configuration in different flight phases is presented. Figure 4 (a) shows the initial state; (b) shows the state heading towards the first channel; (c) shows the state passing through the first channel; and (d) shows the state leaving the first channel. When the formation enters the narrow channel, the elliptical envelope adaptively stretches and compresses, and the internal UAV members are reconstructed in real time based on the Venn diagram and maintain a uniform distribution. After leaving the narrow channel, the formation smoothly returns to its initial desired shape without any collisions with external obstacles or between internal members. To further verify the dynamic adjustment performance of the system from the underlying data, the curves showing the change in the elliptical major-minor axis ratio and the curves showing the change in the derivative of the CBF with respect to the major-minor axis ratio during operation are shown below. Figure 5 and Figure 6 As shown. Combined with Figure 5 and Figure 6 It can be seen that when the formation approaches the corridor boundary and the constraints become tighter, the control barrier function responds quickly and actively outputs a safety correction gradient. This gradient directly drives the ratio of the major and minor axes of the ellipse to increase rapidly, forcing the formation to narrow so as to safely pass through the restricted area. This fully demonstrates the effectiveness and sensitivity of the morphological adaptive adjustment law based on CBF. Figure 7 The invention demonstrates the changes in the global dynamic safety margin. Throughout the flight and dynamic deformation process, the global dynamic safety margin, which incorporates multiple constraints such as thrust saturation, tracking error, and corridor boundaries, remains positive. The method of this invention guides efficient coordinated maneuvering of the formation while strictly ensuring that the UAV system does not violate any physical operational constraints or environmental spatial constraints. In summary, this invention not only guarantees the absolute safety and topological connectivity of UAV formations in complex environments from the underlying control level, but also significantly improves the smoothness of coordinated maneuvering and overall passage efficiency.

[0170] This document uses specific examples to illustrate the principles and implementation methods of this application. The descriptions of the above embodiments are only for the purpose of helping to understand the methods and core ideas of this application. Furthermore, those skilled in the art will recognize that, based on the ideas of this application, there will be changes in the specific implementation methods and application scope. Therefore, the content of this specification should not be construed as a limitation of this application.

Claims

1. A method for planning and controlling the formation motion layout of unmanned aerial vehicles (UAVs) under multiple constraints, characterized in that, Includes the following steps: S1. Based on the jump point search algorithm combined with the safe flight corridor, construct a globally feasible path for the formation center to provide external safe passage constraints for the formation. S2. Based on the control obstacle function, establish a dynamic constraint adjustment law for the position of the ellipse center and the aspect ratio of the major and minor axes to realize the adaptive scaling of the formation shape to the environmental boundary. S3. Based on the Veno diagram, realize the spatial division and member position distribution within the elliptical formation motion layout to maintain topological connectivity and spatial uniformity during formation flight. S4. Establish a pre-stabilized cascaded closed-loop tracking control system for each UAV in the formation to achieve stable tracking of reference commands, and construct a Lyapunov function to characterize the tracking error energy of the system. S5. Taking the constructed global dynamic safety margin and navigation field as the core, the final command is generated through an explicit reference management mechanism, and the asymptotic advancement of the reference trajectory and the maintenance of multi-constraint feasibility are realized by combining Lyapunov constraints. The dynamic constraint adjustment law for the ellipse center position and the aspect ratio of the major and minor axes, as described in S2, specifically includes: The overall envelope of the formation is abstracted into a parameterized elliptical virtual structure with a constant coverage area, and the ratio of the major and minor axes of the ellipse is adjusted. To achieve the expansion and compression of formation patterns; Calculate the distance from each hyperplane on the boundary of the safe flight corridor to the nearest point of the formation ellipse envelope. Furthermore, a smoothing factor is introduced to fuse all nearest point distances into a smoothed shortest distance, which is then used to construct a control barrier function. : ； in, As a smoothing factor, The distance from each hyperplane on the boundary of the safe flight corridor to the nearest point of the formation ellipse envelope, Represents the smooth shortest distance. The corresponding number of safe half-spaces; to obtain the time derivative of the control obstacle function along the trajectory of the UAV formation system, the control obstacle function is calculated respectively. Reference value for the center position of the ellipse partial derivatives and reference value for the ratio of major to minor axis derivative Furthermore, a dynamic safety set is established to characterize the safety margin between the formation envelope and the global feasible domain boundary, and a dynamic constraint adjustment law is constructed based on this. The execution logic of the dynamic constraint adjustment law is as follows: To ensure that the state of the UAV formation system always remains within the safe set, so that the control obstacle function satisfies ,in For the control barrier function The time first derivative of the UAV formation system trajectory is used to introduce a safety correction term for the center position of the ellipse. Morphological safety correction term relative to major and minor axis ratio ; When the drone formation system approaches the corridor boundary, the constraints tighten, i.e., the initial function... When calculating dynamic constraint adjustment compensation, its analytical form is as follows: ； ； in, This indicates a safety correction term for the center position of the ellipse. This represents a morphological safety correction term for the ratio of the major and minor axes of an ellipse. For adaptive adjustment class functions, , This represents the initial time derivative of the control barrier function driven by the nominal reference instruction. , and This represents the regularization parameter introduced to prevent singularities in numerical computation; By correcting the negative drift caused by the center movement and suppressing unfavorable shape changes, the formation shape can be adaptively stretched and contracted to the environmental boundary. The S3 implementation process is as follows: A large number of sampling points are generated within the elliptical virtual structure. The Euclidean distance from the sampling point to the projected position of the formation member is calculated and assigned to the nearest member to form discrete Vino units. Calculate the geometric centroid of the corresponding Vino element for each member, and update the projected position of each member using an under-relaxation iterative method until the preset centroid Vino equilibrium convergence condition is met, thus obtaining the uniform spatial distribution of each member within the elliptical safe boundary.

2. The method for planning and controlling the formation motion of unmanned aerial vehicles (UAVs) under multiple constraints as described in claim 1, characterized in that, The specific implementation process of S1 is as follows: By using a jump point search algorithm to search for jump points and forced neighbors in an environmental grid map, a collision-free discrete segmented path connecting the starting point and the target point is generated. Constructing a safe flight corridor around each line segment of the discrete segmented path involves: fitting an initial ellipsoid based on the path line segments, and shortening the lengths of the remaining axes while maintaining alignment of the major axis along the line segment direction until the interior of the ellipsoid contains no obstacle points. A half-space constraint is generated by creating a tangent plane at the contact point between the ellipsoid and the obstacle. After removing the contact point, the ellipsoid is expanded and a new contact point is searched repeatedly to obtain a series of half-space sequences. The intersection of the above half-space sequences forms a convex polyhedron containing the path segment. Several adjacent and overlapping convex polyhedra are pieced together to form a safe flight corridor, thereby transforming environmental obstacle avoidance into an explicit linear inequality constraint of the polyhedron: ； in, The location is the center of the ellipse. To characterize the ratio of the major and minor axes of an ellipse With the positive definite matrix of attitude, The outer normal vector of the safe half-plane. This is the intercept.

3. The method for planning and controlling the formation motion of unmanned aerial vehicles (UAVs) under multiple constraints as described in claim 1, characterized in that, The S4 implementation process is as follows: The kinematic and dynamic model of the UAV is established as follows: ； in, , , , The first The first derivatives of the drone's position, velocity, attitude angle, and angular velocity. , The first The position and speed of the drone For quality, For total thrust, For rotation matrix, It is the acceleration due to gravity. It is a unit vector. For attitude angle, For the first The angular velocity of the drone Let the moment of inertia of the drone be... To control the torque, External disturbance; A dual closed-loop control structure for position and attitude is adopted, wherein the outer position controller and the inner attitude controller are designed as follows: ； in, This is the output of the outer position controller. The reference position command assigned to the UAV. , This is the positive definite gain matrix of the position outer layer controller; , To fuse the observed states of the extended state observer, , Here is the positive definite gain matrix of the inner-loop attitude controller. For attitude error, the fused extended state observer is defined as follows: ； In the formula, The rate of change of the attitude error estimate. The observation error is the attitude error. This is the estimated attitude error value. and For the first derivative of the state of the fused extended state observer, , , For observer gain, , The exponential parameter representing the nonlinear gain range. , This represents the smoothing threshold parameter that determines the linear interval. To The smoothing improvement of a function is defined as follows: ； in, To ensure a sufficiently small quantity; As the independent variable, Let be the integration variable in the Gaussian convolution integral process, β be the nonlinear exponential parameter, δ be the linear interval threshold parameter, and sgn(·) be the standard sign function; construct a state including position and velocity tracking error states. Laypunov quadratic function ,in Here is the Lyapunov weight matrix. This serves as an energy representation of the current tracking error in the pre-stabilized cascaded closed-loop tracking control system, and is used as a direct input variable for subsequent calculations of the dynamic safety margin.

4. The method for planning and controlling the formation motion of unmanned aerial vehicles under multiple constraints as described in claim 1, characterized in that, The process for constructing the global dynamic safety margin described in S5 is as follows: First, a multi-constraint model of the drone formation is established, including: Thrust saturation constraint: ,in This represents the maximum thrust available to the drone. Internal collision avoidance constraints: ,in , Let the positions of any two drones be given. To ensure the safe collision avoidance radius for drones; Error tracking constraints: ,in The maximum allowable position tracking error; Safe flight corridor constraints: This is used to ensure that the formation as a whole does not collide with external obstacles; For the aforementioned multiple constraints, calculate the corresponding Lyapunov safety thresholds. : Threshold for thrust saturation constraint in operational constraints : ； in, This represents the thrust constraint coefficient vector. , , This is the thrust direction vector; Tracking error threshold in operational constraints : ； in, For matrix The first element; Safe corridor threshold in environmental constraints : ； in, Let be the relative position vector of the member in the virtual coordinate system. Characterizes the attitude rotation of an ellipse; The Lyapunov threshold values ​​for each constraint minus the Lyapunov function values ​​of the current state of the pre-stabilized cascaded closed-loop tracking control system. The global dynamic security margin is calculated. : ； in, The global dynamic security margin is a set of integer indices. As a continuous dynamic gain coefficient, it directly determines the update rate of the reference instruction; specifically, the update rate of the reference instruction is related to... The numerical values ​​are positively correlated: when At that time, the drone swarm system was in a safe state, and The larger the value, the faster the reference instruction update rate; when When the value approaches 0, it indicates that the drone formation system is approaching the constraint boundary. The update rate of the reference command then decreases proportionally to 0, thereby automatically slowing down and eventually stopping the update of the reference command, waiting for the underlying drones to catch up with the reference trajectory.

5. The method for planning and controlling the formation motion of unmanned aerial vehicles (UAVs) under multiple constraints according to claim 1, characterized in that, The process of generating final instructions through the explicit reference management mechanism described in S5 is as follows: Design attraction fields pointing towards the target point respectively With form attraction field : ； ； in, For the local target path points of the current safe corridor segment, This is a reference value for the ratio of major to minor axis. This is a reference value for the center position of the ellipse. As a smoothing factor, For the desired nominal shape reference value, For shape tracking gain; Global dynamic security margin As a nonlinear gain, combined with the target attraction field and obstacle avoidance correction field The superimposed total navigation field: ； Finally, the dynamic regulation equation for the ERG output expansion state is as follows: ； in, and These are the update rates of the reference instructions for the center position of the output ellipse and the reference instructions for the major and minor axis ratio parameters, respectively. For the overall location navigation field, For the overall shape navigation field, The global path points are planned by the jump point search algorithm. It is a positive definite adjustment coefficient.

Images