An unmanned aerial vehicle cluster obstacle avoidance method based on field of view and adaptive obstacle avoidance radius

Abstract

The application provides a UAV cluster obstacle avoidance method based on a field of view and an adaptive obstacle avoidance radius, the method acquires a reference path in a prediction time domain according to a current position of a UAV; designs a cluster adaptive communication topology mechanism according to a field of view method; calculates an optimal obstacle avoidance radius based on a current speed direction of the UAV and a relative position relationship with an obstacle according to an adaptive obstacle avoidance radius method; constructs an objective function according to a desired behavior of the UAV, including tracking of the reference path, distance keeping with surrounding neighbors, obstacle avoidance, etc., and gives corresponding weights; based on a distributed model prediction control, converts the obstacle avoidance problem of the UAV cluster into a local optimal control problem in a limited time domain of a single UAV, and solves the control input of the UAV; the method enables the UAV cluster to safely pass through a complex obstacle environment, and timely restores the cluster form after obstacle avoidance, and realizes the obstacle avoidance of a fixed-wing UAV cluster under a reference route.

Description

Technical Field

[0001] This invention belongs to the field of unmanned aerial vehicle (UAV) swarm technology, and particularly relates to an obstacle avoidance method for UAV swarms based on field of view and adaptive obstacle avoidance radius. Background Technology

[0002] Faced with the information-scarce and complex, ever-changing external environment of future battlefields, drone swarms, with their low-cost, large-scale swarm configuration, will compensate for the limited operational range and high mission failure rate of individual drones, becoming an important means of combat against the enemy. Flexible and efficient obstacle avoidance flight, however, becomes a key technical challenge in ensuring the flight safety of drone swarms.

[0003] Based on the information exchange methods among swarm drones, swarms can be divided into centralized and distributed types. Centralized swarms have a simple obstacle avoidance principle and are easy to optimize globally, but they require high communication bandwidth, making them unsuitable for combat environments. Furthermore, a malfunction in the central drone can paralyze the entire swarm, posing a significant risk to swarm flight. In a distributed swarm, each drone can be considered a relatively independent intelligent agent. Drones in the swarm interact with their neighbors through a local communication network, making decisions collaboratively and independently. A malfunction in one drone does not affect the flight safety of the entire swarm.

[0004] When drone swarms attempt obstacle avoidance, it often requires disrupting the swarm's formation. During this process, drones must not only maintain a certain distance from obstacles but also consider the risk of collisions with other drones within the swarm. The design and coordination of individual drone obstacle avoidance mechanisms and internal collision avoidance mechanisms within the swarm is a crucial research area in swarm obstacle avoidance. Summary of the Invention

[0005] Purpose of the invention: The technical problem to be solved by this invention is to address the shortcomings of existing technologies by providing a drone swarm obstacle avoidance method based on field of view and adaptive obstacle avoidance radius, comprising the following steps:

[0006] Step 1: Based on the UAV motion model and current position, obtain the reference path in the prediction time domain;

[0007] Step 2: Design a cluster adaptive communication topology mechanism based on the vision method;

[0008] Step 3: According to the adaptive obstacle avoidance radius method, when the distance between the UAV and the obstacle meets the triggering condition of the adaptive obstacle avoidance radius method, calculate the optimal obstacle avoidance radius for the current obstacle.

[0009] Step 4: Construct an objective function based on the expected behavior of the UAV and assign corresponding weights;

[0010] Step 5: Based on distributed model predictive control, the obstacle avoidance problem of the UAV swarm is transformed into a local optimal control problem of a single UAV in a finite time domain, and the control input of the UAV is solved.

[0011] Step 1 includes: introducing the simplified motion model of the UAV:

[0012]

[0013] Where [x,y,z] are the three-dimensional spatial coordinates of UAV i. Let x, y, and z be the first derivatives, respectively, and V be the velocity scalar. Let χ, γ, ω, and ρ be the first derivatives, respectively, where χ is the azimuth angle, γ is the inclination angle, ω is the azimuth angular velocity, ρ is the inclination angular velocity, η is the rate of change of the azimuth angular velocity, and μ is the rate of change of the inclination angular velocity.

[0014] η and μ are the control variables of the UAV. Discretizing η and μ yields the difference equations for the UAV state variables:

[0015]

[0016] And it satisfies the motion constraints of fixed-wing UAVs:

[0017]

[0018] Where x(k), y(k), z(k), χ(k), γ(k), ω(k), ρ(k), η(k), and μ(k) are the corresponding representations of x, y, z, χ, γ, ω, ρ, η, and μ in the discrete time domain, respectively. Let k represent the current time, then k+1 represents the next time, where ΔT is the time interval between adjacent time points; γ sat ω sat ρ sat η sat μ sat These represent the maximum climb angle, maximum azimuth velocity, maximum tilt velocity, rate of change of maximum azimuth velocity, and rate of change of maximum tilt velocity of the UAV, respectively.

[0019] Based on the current position of the UAV, the reference flight path information in the prediction time domain is obtained. Let the current position of the UAV be (x, y, z), then let x l =x, to obtain the reference information of the corresponding point on the given route as (x l ,y l ,z l ,χ l ,γ l ), x l y l z l , χl γ l These are the x-coordinate, y-coordinate, z-axis coordinate, azimuth angle, and inclination angle of the reference point at the reference route corresponding to the current time.

[0020] For the reference waypoint information in the prediction time domain Np, the y, z, χ, and γ values ​​corresponding to the route information are obtained by updating the position of x. The method for updating the position of x is as follows:

[0021]

[0022]

[0023]

[0024] ...

[0025]

[0026]

[0027]

[0028] Then according to The sequence determines the reference waypoint information, where The reference x-axis coordinates for the first step in the time domain are used for prediction; To and The corresponding reference track inclination angle at that location, and γ l equal; To and The corresponding reference track azimuth angle at that location, and χ l equal; To predict the reference x-axis coordinates at step l in the time domain, To and The corresponding reference track inclination angle, To and The corresponding reference track azimuth angle.

[0029] Step 2 includes:

[0030] Step 2-1: Convert the heading information into a velocity vector and calculate the angle θ between UAV j and UAV i's direction of motion. ij :

[0031]

[0032] Where acos represents the arccosine of the trigonometric function p i Let p be the position vector of UAV i. j Let v be the position vector of the drone j. i Let i be the velocity vector of the drone.

[0033] Step 2-2, determine the included angle θ ij whether it belongs to range:

[0034]

[0035] wherein, is the field of view of the UAV, which diffuses around based on the individual speed angle; if the condition is satisfied then UAV j and UAV i satisfy the neighbor relationship in terms of angle, and step 2-3 is executed; otherwise, it is determined that UAV j is not a neighbor of UAV i, and no distance-based judgment is required anymore;

[0036] Step 2-3, determine whether the distance between UAV j and UAV i is less than the communication distance r:

[0037] ||p j -p i ||<r

[0038] if ||p j -p i ||<r is satisfied, then UAV j is a neighbor of UAV i, can establish a communication relationship, which facilitates inter-UAV data exchange;

[0039] Step 2-4, the neighbor set N i is:

[0040]

[0041] wherein r is the communication distance of the UAV, and during cluster flight N is the set of all UAVs;

[0042] Obstacle avoidance mode: the individual's forward perception viewing angle is reduced to and the neighbor viewing angles on both sides of the current obstacle relative to the UAV direction are increased so that the UAV pays attention to neighbors on the side close to the obstacle;

[0043] The neighbor judgment steps based on obstacle field of view include:

[0044] Step a1, calculate the included angle θ of UAV j relative to individual i to the center position O of the obstacle iOj :

[0045]

[0046] Step a2, determine whether the included angle θ iOj belongs to range:

[0047]

[0048] Wherein is the field of view in the obstacle avoidance mode of the UAV, if the following condition is satisfied then UAV j and UAV i satisfy the neighbor relationship from the perspective of obstacle-based field of view, and step a3 is executed; otherwise, it is determined that UAV j is not a neighbor of UAV i, and no further distance-based judgment is required;

[0049] Step a3, judging whether the distance between UAV j and UAV i is less than the communication distance r:

[0050] ||p j -p i ||<r

[0051] If UAV j and UAV i satisfy the neighbor relationship from the perspective of obstacle-based field of view, and satisfy ||p j -p i ||<r, then UAV j is a neighbor of UAV i based on obstacle field of view;

[0052] Step a4, obstacle avoidance expands the neighbor set is:

[0053]

[0054] The final neighbor set in obstacle avoidance mode is

[0055] Step 3 comprises:

[0056] Step 3-1, when the distance between the UAV and the obstacle surface is less than the obstacle avoidance trigger distance, construct a linear equation based on the current position according to the speed direction of the UAV, and convert the linear equation into the following standard form:

[0057] l:ax l +by l +cz l +d=0;

[0058] wherein a, b, c, d are parameters to be solved of the straight line l, x l , y l , z l are coordinates of points satisfying the linear equation;

[0059] Step 3-2, calculate the intersection points of the straight line l and the obstacle spherical surface, connect the center of the obstacle and the first intersection point to form a straight line l1, denote the included angle between the straight line l1 and the current speed direction of the UAV as the obstacle avoidance threat angle α, the optimal obstacle avoidance radius is proportional to α:

[0060]

[0061] in:

[0062]

[0063] In the formula, R O r is the radius of the current obstacle. min r is the minimum obstacle avoidance radius of the current obstacle. max This represents the maximum obstacle avoidance radius for the current obstacle.

[0064] Step 4 includes: establishing the following objective function J:

[0065] minJ(X(k),U(k),X j (k))=min{L p (X(k))+L u (U(k))+L on (X(k),X j (k))+L ob (X(k))+L s (X(k))+L n (X j (k))}

[0066] in,

[0067] X(k)=[x(k) y(k) z(k) χ(k) γ(k) ω(k) ρ(k)] T

[0068] U(k)=[η(k) μ(k)] T

[0069] X(k) is the state matrix composed of the UAV's discrete state variables x(k), y(k), z(k), χ(k), γ(k), ω(k), and ρ(k), while U(k) is the control matrix composed of the UAV's discrete control variables η(k) and μ(k); X j (k) is the state matrix of the drone's neighbors obtained through communication;

[0070]

[0071]

[0072]

[0073] Where X r According to The sequence yields the reference waypoints of the UAV in the predicted time domain, where T represents the matrix transpose and C... Q It is the state selection matrix, multiplied by X to obtain the result with X. rThe corresponding state variable matrix; L p (X(k)) is the path tracking term, representing the error cost between the predicted position of the UAV and the reference position in the prediction time domain. r (k+i|k) represents the reference path information at the corresponding time point, and Q is the diagonal weight matrix. k1 Q k2 Q k3 Q k4 Q k5 The weight parameters are set;

[0074]

[0075] in:

[0076]

[0077] L u (U(k)) is the input penalty term, R is the diagonal weight matrix, and Rk is the weight matrix. k1 R k2 The weight parameters are set;

[0078]

[0079] in,

[0080]

[0081] L p (ΔU(k)) is the penalty term for the input change, and S is the diagonal weight matrix. k1 S k2 The weight parameters are set;

[0082]

[0083] Where L on (X(k),X j (k) is to avoid neighbor terms. x is the location information of neighbors obtained through communication. p (k+i|k) represents its own position information, N n d represents the number of neighbors. e Let L be the expected distance between neighbors, which is a constant; the cost term L is... on (x(k),x j (k) ensures that the predicted trajectory of the drone maintains a safe distance from its neighbors, i.e. K on It is a positive scalar; e is the natural constant;

[0084]

[0085] Lob (X(k)) represents the obstacle avoidance term, ensuring the drone maintains a safe distance from the obstacle surface; O j Let j be the center position of the detected obstacle. x is the optimal obstacle avoidance radius obtained in step 4. p (k+i|k) represents the predicted future location information at the current moment, where K... ob It is a positive scalar;

[0086]

[0087] in,

[0088]

[0089] L s (X(k)) is a constraint term that penalizes violations of state quantity restrictions; σ represents any term in [γ(k)ω(k)ρ(k)];

[0090]

[0091] in,

[0092]

[0093]

[0094] L n (X(k),X j (k) is related to the drone's field of vision. The average track angle cooperative term of the neighbors, n is the number of those belonging to Total number of neighboring drones within the field of view, L n (X(k),X j (k) is optimized only during path tracking and is not considered when the drone is in obstacle avoidance mode. n Let K be the weight matrix. n1 K n2 C represents the weight parameters set; C is also the state selection matrix.

[0095] Step 5 includes:

[0096] Step 5-1, Initialization,

[0097] Step 5-2, iterative loop.

[0098] Step 5-1 specifically includes:

[0099] Step 5-1-1: Set the initial position of each drone i to be appropriate, the spacing between drones to be appropriate, and ensure that the drones fly in the current state, with no collisions within the cluster and no collisions with obstacles, i = 1, 2... N;

[0100] Step 5-1-2: Based on the flight path information and its own position, the UAV uses MATLAB to construct an optimization problem for the objective function J, and solves for minJ(X). i (k),U i (k),X j (k)), thus obtaining the initialization control sequence U. i '(k), calculate the initial predicted trajectory information X i '(k), initializing X during the first solution j (k) The information is empty;

[0101] Step 5-1-3: Determine the neighbor set and communicate with it, sending your own predicted trajectory information X. i '(k), Receive neighbor predicted trajectory information X' j (k);

[0102] Step 5-1-4, predict trajectory information X' based on neighbor information. j (k) Given its own state, flight path information, and obstacle information, solve the finite-time local optimal control problem: min J(X) i '(k),X i '(k),X' j (k)), thus obtaining the feasible control sequence Ui(k) and the predicted trajectory information X at time k+1. i (k+1);

[0103] Step 5-1-5: Communicate with neighbors and send the actual predicted trajectory information X for the next time step. i (k+1), receive feasible predicted trajectory information X from neighbors. j (k+1), and the feasible control sequence U i The first term of (k) is applied to the system as a control input.

[0104] Step 5-2 specifically includes:

[0105] Step 5-2-1, k = k + 1;

[0106] Step 5-2-2: For each drone i, based on the feasible trajectory prediction information X from its neighbors received in the previous time step... j (k) Given its own state, flight path information, and obstacle information, solve the finite-time local optimal control problem: min J(X) i (k),U i (k),X j (k)) yields a feasible control sequence U i (k), and the predicted trajectory information X at time k+1. i (k+1);

[0107] Step 5-2-3: Communicate with neighbors and send your own feasible predicted trajectory information X. i (k+1), receive feasible predicted trajectory information X from neighbors. j (k+1), and the feasible control sequence U i The first term of (k) is applied to the system as a control input;

[0108] Step 5-2-4, t = t + Δt, return to step 5-2-1, until t = t stop Δt is the set simulation step size, t stop This is the total duration of the simulation;

[0109] At this point, the control input for each individual in the cluster is obtained.

[0110] The present invention also provides a storage medium, characterized in that it stores a computer program or instructions, which, when the computer program or instructions are run, implement the UAV swarm obstacle avoidance method based on field of view and adaptive obstacle avoidance radius.

[0111] Beneficial effects: 1. The present invention provides a drone swarm obstacle avoidance method based on field of view and adaptive obstacle avoidance radius. When the distance between the drone and the obstacle meets the triggering condition, the adaptive obstacle avoidance radius method is used to obtain the optimal obstacle avoidance radius of the current obstacle, so that the innermost drone closest to the obstacle can safely pass through the obstacle area.

[0112] 2. The present invention provides a drone swarm obstacle avoidance method based on field of view and adaptive obstacle avoidance radius. By designing a swarm adaptive communication topology mechanism through the field of view method, the outer drones can communicate with the inner drones closest to the obstacle through the field of view topology to coordinate obstacle avoidance.

[0113] 3. The present invention provides an obstacle avoidance method for UAV swarms based on field of view and adaptive obstacle avoidance radius. Based on distributed model predictive control, the obstacle avoidance problem of UAVs is transformed into a local optimal control problem in a finite time domain. By constructing an objective function to solve the optimal control quantity of UAVs, the obstacle avoidance problem of UAV swarms is solved, and the swarm can smoothly pass through the obstacle area. Attached Figure Description

[0114] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments, and the advantages of the present invention in the above and / or other aspects will become clearer.

[0115] Figure 1 This is a structural diagram of the cluster individual obstacle avoidance algorithm.

[0116] Figure 2 This is a schematic diagram of flight path tracking.

[0117] Figure 3 This is a schematic diagram of the field of view of an individual drone.

[0118] Figure 4 This is a schematic diagram of the obstacle field of view of an individual drone.

[0119] Figure 5 This is a schematic diagram of the minimum circumscribed sphere section of an obstacle.

[0120] Figure 6 This is a schematic diagram of adaptive obstacle avoidance radius calculation.

[0121] Figure 7 This is the obstacle avoidance trajectory diagram of the drone swarm in scenario 1.

[0122] Figure 8 This is the main view of the obstacle avoidance trajectory of the drone swarm in Scene 1.

[0123] Figure 9 This is a top-down view of the obstacle avoidance trajectory of the drone swarm in Scene 1.

[0124] Figure 10 This is the obstacle avoidance trajectory diagram of the drone swarm in scenario 2.

[0125] Figure 11 This is the main view of the obstacle avoidance trajectory of the drone swarm in Scene 2.

[0126] Figure 12 This is a top-down view of the obstacle avoidance trajectory of the drone swarm in Scene 2.

[0127] Figure 13 This is the obstacle avoidance trajectory diagram of the drone swarm in scenario 3.

[0128] Figure 14 This is the main view of the obstacle avoidance trajectory of the drone swarm in Scene 3.

[0129] Figure 15 This is a top-down view of the obstacle avoidance trajectory of the drone swarm in Scene 3.

[0130] Figure 16 This is a graph showing the change in the minimum distance between drone neighbors in scenario 3.

[0131] Figure 17 This is a graph showing the variation of the minimum distance between the drone and the surface of each obstacle in scenario 3. Detailed Implementation

[0132] This invention provides a method for obstacle avoidance in UAV swarms based on field of view and adaptive obstacle avoidance radius, as shown in the structural diagram below. Figure 1 As shown, the specific steps include the following:

[0133] Step 1: Based on the UAV motion model and current position, obtain the reference path in the prediction time domain.

[0134] In step 1, the simplified motion model of the UAV is introduced:

[0135]

[0136] In the formula, [x,y,z] represents the three-dimensional spatial coordinates of UAV i, V is the velocity scalar, χ is the azimuth angle, γ is the tilt angle, ω is the azimuth angular velocity, ρ is the tilt angular velocity, η is the rate of change of the azimuth angular velocity, μ is the rate of change of the tilt angular velocity, and η and μ are the control variables of the UAV. Discretizing them yields the difference equations for the UAV state variables:

[0137]

[0138] And it satisfies the dynamic constraints of fixed-wing UAVs:

[0139]

[0140] Based on the current position of the UAV, the reference flight path information in the prediction time domain is obtained. Assuming the current position of the UAV is (x, y, z), let x l =x, to obtain the reference information of the corresponding point on the given route as (x l ,y l ,z l ,χ l ,γ l ),like Figure 2 As shown. For the reference waypoint information within the prediction time domain Np, y, z, χ, and γ corresponding to the route information are obtained by updating the position of x. The method for updating the position of x is as follows:

[0141]

[0142]

[0143]

[0144] ...

[0145]

[0146]

[0147]

[0148] ΔT is the time interval between adjacent moments, and then according to The sequence determines the reference waypoint information.

[0149] Step 2: Design a cluster adaptive communication topology mechanism based on the vision method.

[0150] In step 2, firstly, to facilitate calculation, the heading information is converted into a velocity vector, and then the neighbors are calculated.

[0151] (1) Calculate the angle θ between UAV j and UAV i in the direction of motion. ij :

[0152]

[0153] (2) Determine whether the included angle θ belongs to Within the range:

[0154]

[0155] In the formula, The drone's field of view is defined by its individual speed and extends outwards in all directions. Angle, such as Figure 3 As shown.

[0156] (3) Determine whether the distance between UAV j and UAV i is less than the communication distance r:

[0157] p j -p i || <r

[0158] (4) The neighbor set is:

[0159]

[0160] Where r is the communication distance of the drones, during swarm flight. N is the set of all drones.

[0161] Obstacle avoidance mode: The individual's forward perception field of view is reduced to Simultaneously, it adds the neighboring perspectives of the current obstacle relative to both sides of the drone's direction. Make the drone focus on the neighboring side closest to the obstacle.

[0162] The steps for neighbor determination based on obstacle visibility include:

[0163] (1) Calculate the angle between UAV j and individual i and the center O of the obstacle.

[0164]

[0165] (2) Determine the included angle Does it belong to Within the range:

[0166]

[0167] in is the field of view in the obstacle avoidance mode of the unmanned aerial vehicle, if the condition is satisfied then UAV j and UAV i satisfy the neighbor relationship in terms of obstacle-based field of view, and proceed to the next step; otherwise, it is determined that UAV j is not a neighbor of UAV i, and no further distance-based judgment is required;

[0168] (3) Determine whether the distance between UAV j and UAV i is less than the communication distance r:

[0169] ||p j -p i ||<r

[0170] If UAV j and UAV i satisfy the neighbor relationship in terms of obstacle-based field of view, and satisfy ||p j -p i ||<r, then UAV j is a neighbor of UAV i based on obstacle field of view.

[0171] (4) The expanded neighbor set for obstacle avoidance is:

[0172]

[0173] The final neighbor set in obstacle avoidance mode is as shown in Figure 4 .

[0174] Step 3: According to the adaptive obstacle avoidance radius method, when the distance between the UAV and the obstacle satisfies the trigger condition of the adaptive obstacle avoidance radius method, calculate the optimal obstacle avoidance radius of the current obstacle;

[0175] In Step 3, first, the obstacle is subjected to convexification processing, and the minimum enclosing sphere of the obstacle is obtained as shown in Figure 5 , and the optimal obstacle avoidance radius is calculated

[0176] Steps for calculating adaptive obstacle avoidance radius:

[0177] (1) When the distance from the UAV to the obstacle surface is less than the obstacle avoidance trigger distance, construct a linear equation based on the velocity direction of the UAV and the current position, and convert it into a standard form, as follows:

[0178] l:ax l +by l +cz l +d=0

[0179] where a, b, c, d are parameters to be solved for the straight line l, x l , y l , z l are coordinates of points on the straight line that satisfy the linear equation;

[0180] (2) Calculate the intersection point of line l and the obstacle sphere. Connect the center of the obstacle circle with the first intersection point to form a straight line. Let the angle between this straight line and the current velocity direction of the UAV be the obstacle avoidance threat angle α. Figure 6 As shown. Adaptive obstacle avoidance radius. Proportional to α:

[0181]

[0182] in:

[0183]

[0184] In the formula, R O The radius of the current obstacle.

[0185] Step 4: Construct an objective function based on the expected behavior of the UAV, including tracking a given flight path, maintaining distance from surrounding neighbors, and avoiding obstacles, and assign corresponding weights.

[0186] In step 4, the desired behavior of the UAV is constructed in the form of an objective function, transforming the obstacle avoidance problem into a local optimal control problem for the UAV:

[0187] Factors affecting UAV flight include: positional deviation from the reference path, distance from obstacles, and distance from surrounding objects. The planned path of the UAV should minimize the deviation from the reference path while avoiding obstacles and surrounding objects; therefore, the objective function has the following form:

[0188] minJ(X(k),U(k),X j (k))=min{L p (X(k))+L u (U(k))+L on (X(k),X j (k))+L ob (X(k))+L s (X(k))+L n (X j (k))}

[0189] In the formula,

[0190] X(k)=[x(k) y(k) z(k) χ(k) γ(k) ω(k) ρ(k)] T

[0191] U(k)=[η(k) μ(k)] T

[0192] x j (k) Status information of drone neighbors, obtained through local communication.

[0193] in:

[0194]

[0195] In the formula:

[0196]

[0197]

[0198] Where X r According to The sequence yields the reference waypoints of the UAV in the predicted time domain, where T represents the matrix transpose and C... Q It is the state selection matrix, multiplied by X to obtain the result with X. r The corresponding state variable matrix; L p (X(k)) is the path tracking term, representing the error cost between the predicted position of the UAV and the reference position in the prediction time domain. r (k+i|k) represents the reference path information at the corresponding time point, and Q is the diagonal weight matrix. k1 Q k2 Q k3 Q k4 Q k5 The weight parameters are set;

[0199]

[0200] In the formula:

[0201]

[0202] L u (U(k)) is the input penalty term, which is used to penalize excessively large inputs, so that the UAV can complete the entire flight mission with small input costs, and at the same time, it can make the predicted trajectory smoother. R is the diagonal weight matrix.

[0203]

[0204] In the formula:

[0205]

[0206] L p (Δu(k)) is the input change penalty term, which is used to prevent large jumps in the input, which is more in line with the actual characteristics of the UAV and makes the UAV trajectory smoother. S is the diagonal weight matrix.

[0207]

[0208] Where L on (X(k),Xj (k) is to avoid neighbor terms. x is the location information of neighbors obtained through communication. p (k+i|k) represents its own position information, N n d represents the number of neighbors. e Let L be the expected distance between neighbors, which is a constant; the cost term L is... on (x(k),x j (k) ensures that the predicted trajectory of the drone maintains a safe distance from its neighbors, i.e. K on It is a positive scalar; e is the natural constant;

[0209]

[0210] L ob (X(k)) represents the obstacle avoidance term, ensuring the drone maintains a safe distance from the obstacle surface. j Let j be the center position of the detected obstacle. x is the optimal obstacle avoidance radius obtained in step 4. p (k+i|k) represents the predicted future location information at the current moment, where K... ob It is a positive scalar.

[0211]

[0212] In the formula:

[0213]

[0214] L s (X(k)) is a constraint term that penalizes violations of state quantity restrictions, mainly constraining the state quantities mentioned in equation (13). Here, σ represents any term in [γ(k)ω(k)ρ(k)].

[0215]

[0216] In the formula:

[0217]

[0218]

[0219] L n (X(k),X j (k) is related to the drone's field of vision. The average track angle cooperative term of the neighbors, n is the number of those belonging to Total number of neighboring drones within the field of view, L n (X(k),X j(k) is optimized only during path tracking and is not considered when the drone is in obstacle avoidance mode. n Let K be the weight matrix. n1 K n2 C represents the weight parameters set; C is also the state selection matrix.

[0220] Step 5: Based on distributed model predictive control, the obstacle avoidance problem of the UAV swarm is transformed into a local optimal control problem of a single UAV in a finite time domain, and the control input of the UAV is solved.

[0221] In step 5, a distributed model predictive control algorithm is used to solve the finite-time local optimal control problem proposed in step 4.

[0222] Distributed model predictive control algorithm:

[0223] To enable coordinated flight of drones in the cluster, a distributed model predictive control framework is adopted based on the previous section. This framework consists of two parts: initialization and iterative loop. The specific steps are as follows:

[0224] Step 5-1, Initialization. Step 5-1 specifically includes:

[0225] Step 5-1-1: Set the initial position of each UAV i (i = 1, 2... N) to be appropriate, and the spacing between UAVs to be appropriate, so that the UAVs fly according to the current control input, and there is no collision within the cluster or with obstacles.

[0226] Step 5-1-2: Based on the flight path information and its own position, the UAV uses MATLAB to construct an optimization problem for the objective function J, and solves for minJ(X). i (k),U i (k),X j (k)), thus obtaining the initialization control sequence U. i '(k), calculate the initial predicted trajectory information X i '(k), initializing X during the first solution j (k) The information is empty;

[0227] Step 5-1-3: Determine the neighbor set and communicate with it, sending your own predicted trajectory information X. i '(k), Receive neighbor predicted trajectory information X' j (k);

[0228] Step 5-1-4, predict trajectory information X' based on neighbor information. j (k) Given its own state, flight path information, and obstacle information, solve the finite-time local optimal control problem: min J(X) i '(k),X i '(k),X' j(k)), thus obtaining the feasible control sequence Ui(k) and the predicted trajectory information X at time k+1. i (k+1);

[0229] Step 5-1-5: Communicate with neighbors and send the actual predicted trajectory information X for the next time step. i (k+1), receive feasible predicted trajectory information X from neighbors. j (k+1), and the feasible control sequence U i The first term of (k) is applied to the system as a control input.

[0230] Step 5-2, iterative loop, specifically includes the following steps:

[0231] Step 5-2-1, k = k + 1;

[0232] Step 5-2-2: For each drone i, based on the feasible trajectory prediction information X from its neighbors received in the previous time step... j (k) Given its own state, flight path information, and obstacle information, solve the finite-time local optimal control problem: min J(X) i (k),U i (k),X j (k)) yields a feasible control sequence U i (k), and the predicted trajectory information X at time k+1. i (k+1);

[0233] Step 5-2-3: Communicate with neighbors and send your own feasible predicted trajectory information X. i (k+1), receive feasible predicted trajectory information X from neighbors. j (k+1), and the feasible control sequence U i The first term of (k) is applied to the system as a control input;

[0234] Step 5-2-4, t = t + Δt, return to step 5-2-1, until t = t stop Δt is the set simulation step size, t stop This is the total duration of the simulation;

[0235] At this point, the control input for each individual in the cluster is obtained.

[0236] The drone swarm obstacle avoidance method of the present invention enables drones to follow a reference path and maintain a swarm formation when there are no obstacles. When encountering obstacles, they can safely pass through complex obstacle environments and restore the swarm formation in a timely manner after obstacle avoidance. In one embodiment of the present invention, a cluster of 20 drones is considered, with initial positions as follows: [245,196,411; 175,218,410; 176,115,412; 176,167,462; 176,168,358; 176,167,411; 107,217,407; 102,21,407; 105,118,505; 105,118,309; 106,167,455; 106,167,358; 104,69,455; 104,69,359; 105,118,407; 36,42,404; 36,94,403; 38,-8,404; 34,4...]. [5,455;34,45,351]m. Initial attitude (track azimuth, track inclination) are [0.26,0.01;0.50,0.03;0.32,0.07;0.45,0.07;0.45,0.00;0.35,0.06;0.60,0.04;0.78,0.04;0.66,-0.03;0.66,0.09;0.63,0.00;0.63,0.07;0.69,0.01;0.68,0.09;0.66,0.04;0.71,0.09;0.82,0.05;0.68,0.09;0.86,0.09;0.86,0.03]rad. The drone's speed is fixed at V = 30 m / s. Simulation scenario 1 is set up, with the obstacle center located on the preset flight path. This will affect the flight safety of each drone in the swarm and also further test the stability of the swarm flight, making it the most difficult situation in swarm obstacle avoidance flight scenarios. Figure 7 Provide the obstacle avoidance trajectory diagram for clustering in Scenario 1. Figure 8 Provides the main view of the obstacle avoidance trajectory for clustering in Scenario 1. Figure 9 A top-down view of the obstacle avoidance trajectory for Scenario 1 is provided. Simulation Scenario 2 is then set up: an obstacle appears on one side of the flight path, which will affect the flight safety of some drones. For drones unaffected by the obstacle, the collision problem between drones closer to the obstacle must be considered to further verify the collision avoidance performance of the swarm algorithm. Figure 10 Provide the obstacle avoidance trajectory diagram for scenario 2. Figure 11 Provide the main view of the obstacle avoidance trajectory for the cluster in Scenario 2. Figure 12Provide a top-down view of the obstacle avoidance trajectory for scenario 2 (cluster). Set up simulation scenario 3, designing multiple obstacles of different sizes and relative positions to the reference flight path, with alternating heights, to simulate continuous obstacle scenarios, single large obstacle scenarios, and parallel multi-obstacle scenarios encountered by the UAV during flight. Figure 13 Provide the obstacle avoidance trajectory diagram for scenario 3. Figure 14 Provide the main view of the obstacle avoidance trajectory for scenario 3. Figure 15 Provide a top-down view of the obstacle avoidance trajectory of the cluster in Scenario 3. Figure 16 Provide the minimum distance variation curves between neighboring drones in scenario 3. Figure 17 The curve showing the change in the minimum distance between the drone and the obstacle surface in scenario 3 is given. Figure 7 , Figure 8 , Figure 9 , Figure 10 , Figure 11 , Figure 12 , Figure 13 , Figure 14 , Figure 15 In the figure, x, y, and z represent the three-dimensional spatial coordinate axes of the UAV, and the unit is m (meter). Figure 16 , 17 The horizontal axis represents the simulation time in seconds (s), and the vertical axis represents the corresponding distance in meters (m).

[0237] This invention provides a drone swarm obstacle avoidance method based on field of view and adaptive obstacle avoidance radius. Many methods and approaches exist for implementing this technical solution; the above description is merely a preferred embodiment of the invention. It should be noted that those skilled in the art can make various improvements and modifications without departing from the principles of this invention, and these improvements and modifications should also be considered within the scope of protection of this invention. All components not explicitly stated in this embodiment can be implemented using existing technologies.

Claims

1. A method for obstacle avoidance in a UAV swarm based on field of view and adaptive obstacle avoidance radius, characterized in that, Includes the following steps: Step 1: Based on the UAV motion model and current position, obtain the reference path in the prediction time domain; Step 2: Design a cluster adaptive communication topology mechanism based on the vision method; Step 3: According to the adaptive obstacle avoidance radius method, when the distance between the UAV and the obstacle meets the triggering condition of the adaptive obstacle avoidance radius method, calculate the optimal obstacle avoidance radius for the current obstacle. Step 4: Construct an objective function based on the expected behavior of the UAV and assign corresponding weights; Step 5: Based on distributed model predictive control, the obstacle avoidance problem of the UAV swarm is transformed into a local optimal control problem of a single UAV in a finite time domain, and the control input of the UAV is solved. Step 2 includes: Step 2-1: Convert the heading information into a velocity vector and calculate the UAV's velocity. relative to drones Angle between the directions of motion : ， in Represents the arccosine in trigonometric functions. For drones The position vector, For drones The position vector, For drones The velocity vector; Step 2-2, determine the included angle Does it belong to Within the range: ， in, The drone's field of view is defined by its individual speed and extends outwards in all directions. Angle; if satisfied Then drone With drones If the neighbor relationship is satisfied from an angle, proceed to steps 2-3; otherwise, determine that it is a drone. Not a drone Neighbors no longer need to be judged based on distance; Steps 2-3: Determine if the drone is in use. With drones Is the distance between them less than the communication distance? : ， If satisfied Then drone It is a drone Neighbors, able to establish communication relationships; Steps 2-4, Neighbor Set for: ， in The communication range of drones during swarm flight. , A collection of all drones; Obstacle avoidance mode: The individual's forward perception field of view is reduced to At the same time, it increases the neighboring views of the current obstacle relative to both sides of the drone's direction. ; The steps for neighbor determination based on obstacle visibility include: Step a1, calculate the drone relative individual To the center of the obstacle The included angle : ， Step a2, determine the included angle Does it belong to Within the range: ， in The field of view in drone obstacle avoidance mode, if it meets the following conditions. Then drone With drones If the neighbor relationship is satisfied based on the obstacle's field of view, proceed to step a3; otherwise, determine that it is a drone. Not a drone Neighbors no longer need to be judged based on distance; Step a3, determine the drone With drones Is the distance between them less than the communication distance? : ， If drone With drones From the perspective of obstacle-based viewpoint, it satisfies neighbor relationships and satisfies Then drone It is a drone Neighbors based on obstacle view; Step a4, obstacle avoidance expands the neighbor set for: ， In obstacle avoidance mode, the final set of neighbors is ; Step 3 includes: Step 3-1: When the distance between the drone and the obstacle surface is less than the obstacle avoidance trigger distance, construct a straight line equation based on the drone's velocity direction and current position, and convert the straight line equation into the following standard form: ； in , , , It is a straight line With solution parameters, , , To satisfy the coordinates of points on the line equation; Step 3-2, calculate the straight line The point where the object intersects the spherical surface of the obstacle is connected to the center of the obstacle's circle, forming a straight line with the first intersection point. Let this line be... The angle between the drone's current speed direction and the obstacle avoidance threat angle is the obstacle avoidance threat angle. Optimal obstacle avoidance radius and Proportional: ， in: ， In the formula, The radius of the current obstacle. The minimum obstacle avoidance radius for the current obstacle. This represents the maximum obstacle avoidance radius for the current obstacle.

2. The method according to claim 1, characterized in that, Step 1 includes: introducing the simplified motion model of the UAV: ， in, For drones The three-dimensional spatial coordinates, , , They are respectively , , The first derivative, For velocity scalar, , , , They are respectively , , , The first derivative, The azimuth of the flight path. The inclination angle of the flight path. The azimuth angular velocity of the flight path. The angular velocity of the track tilt. The rate of change of the azimuth angular velocity of the flight path. The rate of change of the trajectory tilt angular velocity; , It is the control quantity of the drone, for , Discretization yields the difference equations for the UAV state variables: ， And it satisfies the motion constraints of fixed-wing UAVs: ， in , , , , , , , , They are respectively , , , , , , , , The corresponding representation in the discrete time domain is set as follows: To indicate the current time, then Indicates the next moment, where The time interval between adjacent moments; , , , , These represent the maximum climb angle, maximum azimuth velocity, maximum tilt velocity, rate of change of maximum azimuth velocity, and rate of change of maximum tilt velocity of the UAV, respectively. Based on the current position of the UAV, the reference flight path information within the predicted time domain is obtained, and the current position of the UAV is set as... Then let To obtain the reference information of the corresponding points on a given route. , , , , , These are the x-coordinate, y-coordinate, z-axis coordinate, azimuth angle, and inclination angle of the reference point at the reference route corresponding to the current time. For the prediction time domain Reference waypoint information within, through updating Location, obtain the corresponding route information , , and , The location update method is as follows: ， Then according to The sequence determines the reference waypoint information, where The reference x-axis coordinates for the first step in the time domain are used for prediction; To and The corresponding reference track inclination angle, and equal; To and The corresponding reference track azimuth angle, and equal; To predict the first in the time domain The reference x-axis coordinate of the step. To and The corresponding reference track inclination angle, To and The corresponding reference track azimuth angle.

3. The method according to claim 2, characterized in that, Step 4 includes: establishing the following objective function. : ， in, ， Discrete state variables of UAVs , , , , , , The state matrix formed, Discrete control quantities of unmanned aerial vehicles , The control matrix is ​​composed of; It is the state matrix of the drone's neighbors obtained through communication; ， ， ， in According to The sequence is used to predict the UAV's reference waypoints in the time domain, where T represents the matrix transpose. It is the state selection matrix, and Multiplying gives the result of multiplication with The corresponding state variable matrix; The path tracking term represents the error cost between the predicted position of the UAV and the reference position in the prediction time domain. For the reference path information at the corresponding time, It is a diagonal weight matrix. , , , , The weight parameters are set; ， in: ， It is an input penalty item. It is a diagonal weight matrix. , The weight parameters are set; ， in, ， It is a penalty term for input change. It is a diagonal weight matrix. , The weight parameters are set; ， in To avoid neighbor items, For neighbor location information obtained through communication, For its own location information, For the number of neighbors, The expected distance between neighbors is a constant; the cost term... To ensure that the predicted trajectory of the drone maintains a safe distance from its surroundings, i.e. , It is a positive scalar; e is the natural constant; ， To avoid obstacles, the drone must maintain a safe distance from the obstacle surface; For the detected first The center position of each obstacle The optimal obstacle avoidance radius obtained in step 4. This is the predicted future location information for the current moment. It is a positive scalar; ， in, ， It is a constraint that penalizes violations of state quantity restrictions; represent Any one of them; ， in, ， ， It is related to the field of vision of drones The average track angle of the neighbors' cooperative term, Belonging to Total number of neighboring drones within the field of view. Optimization is only performed during path tracking; it is not considered when the drone is in obstacle avoidance mode. This is the weight matrix. , The weight parameters are set; This is also a state selection matrix.

4. The method according to claim 3, characterized in that, Step 5 includes: Step 5-1, Initialization, Step 5-2, iterative loop.

5. The method according to claim 4, characterized in that, Step 5-1 specifically includes: Step 5-1-1, Configure each drone The initial position is appropriate, the spacing between drones is appropriate, ensuring that the drones fly in their current state, with no collisions within the cluster and with obstacles. ; Step 5-1-2: The UAV constructs an objective function based on the flight path information and its own position. The optimization problem is to solve it. The initialization control sequence is obtained. Calculate initial predicted trajectory information During the initialization of the first solution The information is empty; Step 5-1-3: Determine the neighbor set and communicate with it, sending your own predicted trajectory information. Receive neighbor predicted trajectory information ; Step 5-1-4: Based on the neighbor's predicted trajectory information Given its own state, flight path information, and obstacle information, the system solves a finite-time local optimal control problem. A feasible control sequence is obtained. ,as well as Time-based predicted trajectory information ; Step 5-1-5: Communicate with neighbors and send the actual predicted trajectory information for the next time step. Receive feasible predicted trajectory information from neighbors and feasible control sequences The first item is applied to the system as a control input.

6. The method according to claim 5, characterized in that, Step 5-2 specifically includes: Step 5-2-1, ; Step 5-2-2, each drone Based on the neighbor feasible trajectory information received in the previous moment, Given its own state, flight path information, and obstacle information, the system solves a finite-time local optimal control problem. A feasible control sequence is obtained. ,as well as Time-based predicted trajectory information ; Step 5-2-3: Communicate with neighbors and send information about your own feasible predicted trajectory. Receive feasible predicted trajectory information from neighbors and feasible control sequences The first item is applied to the system as a control input; Step 5-2-4, Return to step 5-2-1 until... , It's the simulation step size that's set. This is the total duration of the simulation; At this point, the control input for each individual in the cluster is obtained.

7. A storage medium, characterized in that, It stores a computer program or instructions that, when executed, implement the method as described in any one of claims 1 to 6.

Images