Model predictive control and improved artificial potential field method for multi-UAV path planning

Abstract

This invention relates to path planning technology for unmanned aerial vehicles (UAVs), proposing a trajectory planning method combining Model Predictive Control (MPC) and Advanced Artificial Potential Field (APF) to achieve path planning for micro-UAV swarms in complex obstacle environments. To this end, the technical solution adopted in this invention is a multi-UAV path planning method using Model Predictive Control and an improved artificial potential field method. A linear point model of the UAVs is established, and constraints and cost functions are constructed. The optimization problem is solved using MPC to obtain offline planned trajectories. When encountering dynamic obstacles, the artificial potential field method (APF) is used for local online planning to achieve dynamic obstacle avoidance, thereby enabling the swarm of UAVs to safely fly through obstacles and reach the target point. This invention is mainly applied to multi-UAV flight control scenarios.

Description

Technical Field

[0001] This invention relates to path planning technology for unmanned aerial vehicles (UAVs), and more specifically, to a multi-UAV path planning method based on model predictive control and an improved artificial potential field method. Background Technology

[0002] Model predictive control (MPC) has been applied by researchers to UAV path planning. Under a set of pre-designed constraints, a cost function is designed based on path planning requirements. Optimization methods are used to minimize the cost of the planned path, thus solving for a flight path that meets the requirements. However, MPC requires predicting the UAV's state over a certain time domain, calculating and predicting the state at multiple future moments from the current moment, and performing rolling optimization. This significantly increases the computational load of MPC. Especially in multi-UAV path planning, more UAVs bring more state variables, making the computation even more massive. This huge computational load makes MPC difficult to apply in practice. For example, MPC has a low solution frequency, and real-time control performance is not ideal. Artificial potential field (APF) is a classic UAV obstacle avoidance method. Its principle is to set up a virtual force field in the UAV's flight space, with the force field of obstacles set as repulsive and the force field of the target point set as attractive. The UAV moves according to the resultant force of the repulsive and attractive forces, thus reaching the target point while avoiding obstacles. Summary of the Invention

[0003] This invention aims to propose a trajectory planning method combining MPC and APF to achieve path planning for micro-UAV swarms in complex obstacle environments. To this end, the technical solution adopted in this invention is a multi-UAV path planning method using Model Predictive Control and an improved artificial potential field method. A linear point model of the UAVs is established, and constraints and cost functions are constructed. The optimization problem is solved using Model Predictive Control (MPC) to obtain offline planned trajectories. When encountering dynamic obstacles, the Artificial Potential Field Method (APF) is used for local online planning to achieve dynamic obstacle avoidance, thereby enabling the swarm of UAVs to safely fly through obstacles and reach the target point.

[0004] The specific steps are as follows:

[0005] Step 1) Establish a linear point model of the UAV.

[0006] Consider a swarm of N drones. The discrete linear model for each drone is as follows:

[0007] x i (k+1)=A i x i (k)+B i u i (k) (1)

[0008] Where A i and B i It is a constant matrix. Let be the position and velocity of the i-th UAV at time k, where i is the UAV number, and be the control input. This refers to the reference position information of the UAV; the subscript i refers to the i-th UAV.

[0009] Step 2) Establishing Constraints

[0010] The time domain of MPC is discrete, and its overall prediction time domain is a constant T. p =Pt p , where t p Let the step size be the time step. N is the number of steps. + For positive natural numbers, define For prediction at time k The predicted time value, the planned trajectory of the drone is represented by a Bézier curve, the drone at T l The trajectory within a time period is formed by stitching together multiple third-order Bézier curves. Each Bézier curve used for stitching has an order of r and consists of r+1 three-dimensional control points. Defined as follows: the column vector formed by the control points of each curve segment is... After each prediction step in MPC, a curve composed of m segmented trajectories will be obtained. That is, each step calculates m×(r+1) control points. At time k, the trajectory control points of the i-th UAV are represented by a column vector. To indicate;

[0011] The segmented trajectory stitching of the UAV requires that the second derivative of the two Bézier curves be continuous between their first and last connection points. Based on the continuity requirement, the continuity constraints of the trajectory are constructed as follows:

[0012]

[0013] in Let represent the (r+1)th control point on the 0th derivative of the μ-th Bézier curve of the i-th UAV. In the above equation, the left side represents the last control point of the previous Bézier curve segment, and the right side represents the first control point of the Bézier curve. According to the above constraints, the second derivative can be guaranteed to be continuous after the segmented trajectory is spliced. According to the basic properties of Bézier curves, its higher-order control points can be represented by a linear combination of lower-order control points, that is:

[0014]

[0015] Based on this property of the control points of the Bézier curve, equation (2) can be rewritten in the following matrix form:

[0016]

[0017] Equation (4) represents the trajectory continuity constraint condition for the i-th UAV at time k, where and All are constants;

[0018] To limit the speed and acceleration of the drone, the following dynamic constraints are established:

[0019]

[0020] in All are constants;

[0021] During the flight of a drone swarm, collision avoidance between drones is achieved by establishing relevant constraints. The drones are considered as rigid spherical bodies with a collision radius of r. c Define the safe flight distance d between the two drones. safe >r c The collision avoidance constraint is applied using an event-triggered mechanism. Specifically, when MPC predicts an impending collision between two drones, collision avoidance constraints are applied; otherwise, no constraints are applied. The braking time T for a drone when there is a collision threat is defined. b =Bt p ≤T p This refers to the time required for the drone to brake from its maximum speed to a complete stop with maximum acceleration. When a collision threat is detected, as long as the drone can maintain a sufficient safe distance within the braking time, a collision will not occur. The continuous collision avoidance constraints are constructed as follows:

[0022] ||p i (k+φ|k)-p j (k+φ|k)||2≥d safe -ω ij (k+φ|k) (6)

[0023] d safe ω is the predefined safe distance between drones. ij ≥0 represents the degree of relaxation of the constraints, the subscript j indicates that it is the j-th drone, which is different from the i-th drone, ω ij The larger the value, the smaller the safe distance for the drone. The constraint can be rewritten in matrix form:

[0024]

[0025] Where A a-saf b a-saf All are constant matrices, Ω i Since the vector is constant and the drone swarm does not have a fixed formation, it is necessary to constrain the maximum distance between drones and their neighbors. The swarm compaction constraint is constructed as follows:

[0026]

[0027] Where A a-co b a-co Both are constant matrices, X i As constant vectors, the static obstacle avoidance behavior of the UAV is similar to that of inter-UAV collision avoidance. An event-triggered mechanism is adopted, and obstacle avoidance constraints are applied when a collision is predicted to occur. The static obstacle avoidance constraints are constructed as follows:

[0028]

[0029] Where A o-saf b o-saf All are constant matrices, Λ i Given a constant vector, the collision avoidance cost function is constructed as follows:

[0030]

[0031] Where h a-saf and q a-saf It is the weighting coefficient, ω ij The relaxation factors are constants. The cost function is in quadratic form and solved as a QP problem. The compact cost function is constructed as follows:

[0032]

[0033] Where h a-saf and q a-saf It is the weighting coefficient, χ. ij Let be relaxation factors, and be constants. The obstacle avoidance cost function of the cluster is designed as follows:

[0034]

[0035] Where h o-saf and q o-saf It is the weighting coefficient, λ io Let be relaxation factors, and be constants. The objective error cost function is designed as follows:

[0036]

[0037] in Let q be the target point. tar The weighting coefficients and the control input cost function are designed as follows:

[0038]

[0039] Where q con This is the weighting constant;

[0040] Step 3) Solve the optimization problem using MPC and obtain the offline planning trajectory.

[0041] To plan the desired trajectory of the drones, each drone i needs to solve a QP problem at time k, and the flight space of the drones is defined as P. v This is equivalent to the flight area of ​​a drone, which stipulates that the drone can only fly within a designated area. The flight speed space of the drone is defined as V. v ={v|v min ≤v≤v max Define the feasible state space χ for the UAV. i ={x i (k)=[p i (k) T ,v i (k) T ] T |p i (k)∈P v ,v i (k)∈V v Define the feasible control input space U for the UAV. i ={p refi (k)|p refi (k)∈P v Combining the above constraints and cost function, the QP problem for obtaining the desired trajectory is as follows:

[0042]

[0043] And it satisfies the following constraints:

[0044]

[0045] Simultaneously satisfying the following dynamic equations:

[0046]

[0047] Step 4) Use APF for local online planning to perform dynamic obstacle avoidance.

[0048] When performing dynamic obstacle avoidance based on the artificial potential field method, the UAV's flight altitude is constant, and the Z-axis is no longer considered. The dynamic obstacle avoidance problem is treated as a problem in the XY two-dimensional plane, and the gravitational potential field function is established as follows:

[0049] U at (X p ) = 0.5k a ρ 2 (X p ,X tar )+f at ρ(X p ,X tar (18)

[0050] Where ρ is a function measuring the distance between two points, the gravitational function is obtained by taking the negative gradient of the potential function as follows:

[0051] F at (X p )=-k a ρ(X p ,X tar )-f at (19)

[0052] in f represents the location of the drone and the target point, respectively. at Assuming the numbers are positive constants, the repulsive field function is established as follows:

[0053]

[0054] The repulsive force function is further obtained as follows:

[0055]

[0056] in ρ0 represents the position of the obstacle, and ρ0 represents the range of the repulsive field of the obstacle. Beyond this distance, the repulsive field no longer has an effect.

[0057] When the distance between the drone and a dynamic obstacle is less than the collision distance, the drone will deviate from the planned trajectory to begin obstacle avoidance. After completing obstacle avoidance, it will re-enter the planned trajectory. In order to make the drone re-enter the trajectory more smoothly, a circle with the center of the obstacle as the center and the collision distance as the radius intersects at a certain point on the future planned trajectory. This point is the trajectory recovery point.

[0058] The features and beneficial effects of this invention are:

[0059] 1. By combining the offline multi-UAV MPC global path planning algorithm with the online improved artificial potential field dynamic obstacle avoidance method, the shortcomings of MPC such as large real-time computation and slow computation are overcome. While obtaining a safe and smooth global planning trajectory, the obstacle avoidance capability of UAV swarm under dynamic obstacle interference is improved.

[0060] 2. A strategy for trajectory change and trajectory recovery of the UAV during flight along a predetermined trajectory was designed. Based on an event-triggered method, the UAV performs dynamic obstacle avoidance behavior only when necessary, maximizing the use of the original planned trajectory.

[0061] 3. When the artificial potential field method is applied in confined environments, the target's gravitational and repulsive points are too close, causing the gravitational force at the target point to approach zero. This can easily lead to the UAV hovering at low speed around the target point. When multiple UAVs use the artificial potential field method, they may become trapped at force equilibrium points. Considering the above problems, this invention proposes an improved multi-UAV artificial potential field dynamic obstacle avoidance method, which effectively solves the problems of low-speed hovering and being trapped at force equilibrium points. Attached image description:

[0062] Figure 1 This is a flowchart of the system workflow of the present invention.

[0063] Figure 2 This is the MPC global planning trajectory diagram of the present invention; x and y are the horizontal and vertical coordinates, respectively.

[0064] Figure 3 This is the inter-drone distance curve of the present invention; d ij d represents the distance between drones i and j. coh d represents the farthest aggregation distance between drones. safety d represents the safe distance. coll The value represents the collision distance, mean represents the average value, min and max represent the maximum and minimum values ​​respectively, and Time represents the time.

[0065] Figure 4 This is the distance curve between the UAV and the obstacle in this invention.

[0066] Figure 5 This is the speed curve of the UAV of this invention; v represents speed.

[0067] Figure 6 This is the dynamic obstacle avoidance trajectory at t = 7.4s; t represents time.

[0068] Figure 7 It is the dynamic obstacle avoidance trajectory at t=8.4s.

[0069] Figure 8 It is the dynamic obstacle avoidance trajectory at t=9.4s.

[0070] Figure 9 The dynamic obstacle avoidance trajectory is at t = 10.4 s. Detailed Implementation

[0071] To overcome the shortcomings of existing technologies, this invention aims to propose a trajectory planning method that combines MPC and APF to realize path planning for micro UAV swarms in complex obstacle environments.

[0072] The technical solution adopted in this invention is a multi-UAV path planning method based on model predictive control and improved artificial potential field method. It establishes a linear property point model of the UAV, constructs constraints and cost functions, solves the optimization problem through MPC and obtains offline planned trajectories, and uses APF for local online planning to achieve dynamic obstacle avoidance.

[0073] The specific steps are as follows:

[0074] Step 1) Establish a linear point model of the UAV;

[0075] Consider a swarm of N drones. The discrete linear model for each drone is as follows:

[0076] x i (k+1)=A i x i (k)+B i u i (k) (1)

[0077] This model is derived from modeling the dynamic characteristics of a UAV with a low-level position controller. Where A... i and B i It is a constant matrix. Let represent the position and velocity of the i-th UAV at time k, where i is the UAV's number. Control input This provides the reference position information for the drones. In the following text, the subscript i of each variable character refers to the i-th drone, and the subscript j refers to the j-th drone, which is different from the i-th drone.

[0078] Step 2) Establishing Constraints

[0079] The time domain of MPC is discrete, and its overall prediction time domain is a constant T. p =Pt p , where t p Let the step size be the time step. N is the number of steps. + It is a positive natural number. (Definition) For prediction at time k Predicted value at any given time.

[0080] In this invention, the planned trajectory of the UAV is represented by a Bézier curve. The UAV at T... l The trajectory within a time period can be constructed by stitching together multiple third-order Bézier curves. Based on the mathematical properties of Bézier curves, the continuous input trajectory of a UAV can be defined by a finite set of control points. Each segment of the Bézier curve used for stitching has an order of r and consists of r+1 three-dimensional control points. Definition. The column vector formed by the control points of each curve segment is... After each prediction step in MPC, a curve composed of m segmented trajectories is obtained. That is, each step calculates m × (r+1) control points. The trajectory control points of the i-th UAV at time k are represented by a column vector. To express.

[0081] The segmented trajectory stitching of a UAV requires a smooth trajectory at the stitching point, meaning the second derivative must be continuous between the beginning and end points of the two Bézier curves. The continuity constraints of the trajectory are constructed based on this requirement as follows:

[0082]

[0083] in Let r represent the (r+1)th control point on the 0th derivative of the μ-th Bézier curve of the i-th UAV. In the above equation, the left side represents the last control point of the previous Bézier curve segment, and the right side represents the first control point of the Bézier curve. According to the above constraints, the second derivative can be guaranteed to be continuous after the segmented trajectory is spliced. According to the basic properties of Bézier curves, its higher-order control points can be represented by a linear combination of lower-order control points, that is:

[0084]

[0085] Based on this property of the control points of the Bézier curve, equation (2) can be rewritten in the following matrix form:

[0086]

[0087] Equation (4) represents the trajectory continuity constraint condition for the i-th UAV at time k. Wherein... and All are constants.

[0088] In addition to ensuring trajectory continuity, the dynamic feasibility of the UAV in the actual physical environment must also be satisfied, i.e., restrictions must be placed on the UAV's speed and acceleration. Following the model for constructing continuity constraints, the dynamic constraints are constructed as follows:

[0089]

[0090] in All are constants.

[0091] During the flight of a drone swarm, collision avoidance between drones is achieved by establishing relevant constraints. The drones are considered as rigid spherical bodies with a collision radius of r. c To allow for a margin of safety during flight, the safe flight distance d between the two drones is defined. safe >r cThe principle of collision avoidance is to predict the flight status of the drone and its neighbors, optimize its flight trajectory, and thus avoid future collisions. Collision avoidance constraints are applied using an event-triggered mechanism; that is, when the MPC predicts that two drones are about to collide, collision avoidance constraints are applied to both drones. No constraints are applied when there is no collision threat. This approach relaxes the constraints, expands the solution space, and makes it easier for the MPC to find more feasible solutions. The braking time T of the drone when there is a collision threat is defined. b =Bt p ≤T p This refers to the time required for the drone to brake from its maximum speed to a complete stop with maximum acceleration. When a collision threat is detected, flight is considered safe as long as the drone can maintain a sufficient safe distance within the braking time to avoid a collision. This means ensuring the drone can brake in time before a collision. The continuous collision avoidance constraints are constructed as follows:

[0092] ||p i (k+φ|k)-p j (k+φ|k)||2≥d safe -ω ij (k+φ|k) (6)

[0093] d safe ω is the predefined safe distance between drones. ij ≥0 represents the degree of relaxation of the constraints. ω ij The larger the value, the smaller the safe distance for the drone, the looser the constraints, and the larger the solution space. The constraints can be rewritten in matrix form:

[0094]

[0095] Where A a-saf b a-saf All are constant matrices, Ω i This is a constant vector. The drone swarm does not have a fixed formation, but to ensure swarm compactness, the maximum distance between drones and their neighbors needs to be constrained. Otherwise, the drone flight paths will be distributed along the map edges, and the drones will be too loosely spaced to form a swarm. The swarm compactness constraint is constructed as follows:

[0096]

[0097] Where A a-co b a-co Both are constant matrices, X i This is a constant vector. The drone swarm needs to traverse a map with numerous static obstacles, therefore static obstacle avoidance constraints need to be imposed on the drones. The static obstacle avoidance behavior of the drones is similar to inter-drone collision avoidance, employing an event-triggered mechanism to apply obstacle avoidance constraints when a collision is predicted to occur. The static obstacle avoidance constraints are constructed as follows:

[0098]

[0099] Where A o-saf b o-saf All are constant matrices, Λ i Let be a constant vector. The optimization objective of MPC is to minimize the cost function. The design of the cost function must consider collision avoidance cost, obstacle avoidance cost, cluster compaction cost, target error cost, and control input cost. The collision avoidance cost function is constructed as follows:

[0100]

[0101] Where h a-saf and q a-saf It is the weighting coefficient, ω ij Let be relaxation factors, all of which are constants. The cost function takes the form of a quadratic form and can be solved as a QP problem. The compact cost function is constructed as follows:

[0102]

[0103] Where h a-saf and q a-saf It is the weighting coefficient, χ. ij Let be relaxation factors, all of which are constants. The obstacle avoidance cost function for the cluster is designed as follows:

[0104]

[0105] Where h o-saf and q o-saf It is the weighting coefficient, λ io Let be relaxation factors, all of which are constants. The objective error cost function is designed as follows:

[0106]

[0107] in Let q be the target point. tar These are the weighting coefficients. The control input cost function is designed as follows:

[0108]

[0109] Where q con This is the weighting constant.

[0110] Step 3) Solve the optimization problem using MPC and obtain the offline planning trajectory.

[0111] To plan the desired trajectory of the drones, each drone i needs to solve a QP problem at time k. Let P be the flight space of the drones. vThis is equivalent to the drone's flight area, which stipulates that the drone can only fly within a designated space. The drone's flight speed space is defined as V. v ={v|v min ≤v≤v max Define the feasible state space χ for the UAV. i ={x i (k)=[p i (k) T ,v i (k) T ] T |p i (k)∈P v ,v i (k)∈V v Define the feasible control input space U for the UAV. i ={p refi (k)|p refi (k)∈P v Combining the above constraints and cost function, the QP problem for obtaining the desired trajectory is as follows:

[0112]

[0113] And it satisfies the following constraints:

[0114]

[0115] Simultaneously satisfying the following dynamic equations:

[0116]

[0117] Given the global position information of static obstacles, each UAV solves the aforementioned optimization problem in the prediction time domain. This ultimately yields a globally planned trajectory leading to the vicinity of the target point, with its smoothness and safety guaranteed by constraints and optimization functions. The UAV then flies along this pre-planned global trajectory to reach the vicinity of the target point. However, this trajectory is planned based on known global static obstacle information. If unknown dynamic obstacles are introduced to interfere, the UAV is likely to collide during flight. To compensate for this deficiency and enhance trajectory safety, this invention proposes an online local dynamic obstacle avoidance method based on the artificial potential field method.

[0118] Step 4) Use APF for local online planning to perform dynamic obstacle avoidance.

[0119] Since the global trajectory flight altitude of the UAV swarm planned by MPC is relatively fixed, with significant changes only during takeoff and landing, the UAV flight altitude is constant when performing dynamic obstacle avoidance based on the artificial potential field method. Therefore, the Z-axis is no longer considered, and the dynamic obstacle avoidance problem is treated as a problem in the XY two-dimensional plane. The gravitational potential field function is established as follows:

[0120] U at (X p ) = 0.5k a ρ 2 (X p ,X tar )+f at ρ(X p ,X tar (18)

[0121] Where ρ is a function measuring the distance between two points. Taking the negative gradient of the potential function yields the gravitational function as follows:

[0122] F at (X p )=-k a ρ(X p ,X tar )-f at (19)

[0123] Where X p , These represent the locations of the drone and the target point, respectively. at It is a positive constant. The repulsive field function is established as follows:

[0124]

[0125] The repulsive force function is further obtained as follows:

[0126]

[0127] in ρ represents the position of the obstacle, and ρ0 represents the range of the repulsive field of the obstacle. Beyond this distance, the repulsive field no longer has an effect.

[0128] When the distance between the drone and a dynamic obstacle is less than the collision distance, the drone will deviate from the planned trajectory to begin obstacle avoidance, and then re-enter the planned trajectory after completing obstacle avoidance. To ensure a smoother re-entry, a circle with the obstacle's center point as the center and the collision distance as the radius intersects at a point on the future planned trajectory; this point is the trajectory recovery point. Since the obstacle's direction of motion is perpendicular to the drone's overall direction of motion, this chosen recovery point is roughly symmetrical to the change point during obstacle avoidance, and the trajectory is relatively smooth.

[0129] The present invention will now be described in further detail with reference to the accompanying drawings. The technical problem this invention aims to solve is that MPC (Multi-Process Control) involves a large amount of computation, making it difficult to control UAV flight in real time; offline MPC methods struggle to achieve dynamic obstacle avoidance, thus requiring the integration of other obstacle avoidance methods.

[0130] The technical solution adopted in this invention is:

[0131] A linear point model of the UAV is established, constraints and cost functions are constructed, the optimization problem is solved using MPC to obtain the offline planned trajectory, and APF is used for local online planning to achieve dynamic obstacle avoidance. The specific steps are as follows:

[0132] Step 1) Establish a linear point model of the UAV;

[0133] Consider a swarm of N drones. The discrete linear model for each drone is as follows:

[0134] x i (k+1)=A i x i (k)+B i u i (k) (1)

[0135] This model is derived from modeling the dynamic characteristics of a UAV with a low-level position controller. Where A... i and B i It is a constant matrix. Let represent the position and velocity of the i-th UAV at time k, where i is the UAV's number. Control input This provides the reference position information for the drones. In the following text, the subscript i of each variable character refers to the i-th drone, and the subscript j refers to the j-th drone, which is different from the i-th drone.

[0136] Step 2) Establishing Constraints

[0137] The time domain of MPC is discrete, and its overall prediction time domain is a constant T. p =Pt p , where t p Let the step size be the time step. N is the number of steps. + It is a positive natural number. (Definition) For prediction at time k Predicted value at any given time.

[0138] In this invention, the planned trajectory of the UAV is represented by a Bézier curve. The UAV at T... lThe trajectory within a time period can be constructed by stitching together multiple third-order Bézier curves. Based on the mathematical properties of Bézier curves, the continuous input trajectory of a UAV can be defined by a finite set of control points. Each segment of the Bézier curve used for stitching has an order of r and consists of r+1 three-dimensional control points. Definition. The column vector formed by the control points of each curve segment is... After each prediction step in MPC, a curve composed of m segmented trajectories is obtained. That is, each step calculates m × (r+1) control points. The trajectory control points of the i-th UAV at time k are represented by a column vector. To express.

[0139] The segmented trajectory stitching of a UAV requires a smooth trajectory at the stitching point, meaning the second derivative must be continuous between the beginning and end points of the two Bézier curves. The continuity constraints of the trajectory are constructed based on this requirement as follows:

[0140]

[0141] in Let r represent the (r+1)th control point on the 0th derivative of the μ-th Bézier curve of the i-th UAV. In the above equation, the left side represents the last control point of the previous Bézier curve segment, and the right side represents the first control point of the Bézier curve. According to the above constraints, the second derivative can be guaranteed to be continuous after the segmented trajectory is spliced. According to the basic properties of Bézier curves, its higher-order control points can be represented by a linear combination of lower-order control points, that is:

[0142]

[0143] Based on this property of the control points of the Bézier curve, equation (2) can be rewritten in the following matrix form:

[0144]

[0145] Equation (4) represents the trajectory continuity constraint condition for the i-th UAV at time k. Wherein... and All are constants.

[0146] In addition to ensuring trajectory continuity, the dynamic feasibility of the UAV in the actual physical environment must also be satisfied, i.e., restrictions must be placed on the UAV's speed and acceleration. Following the model for constructing continuity constraints, the dynamic constraints are constructed as follows:

[0147]

[0148] in All are constants.

[0149] During the flight of a drone swarm, collision avoidance between drones is achieved by establishing relevant constraints. The drones are considered as rigid spherical bodies with a collision radius of r. c To allow for a margin of safety during flight, the safe flight distance d between the two drones is defined. safe >r c The principle of collision avoidance is to predict the flight status of the drone and its neighbors, optimize its flight trajectory, and thus avoid future collisions. Collision avoidance constraints are applied using an event-triggered mechanism; that is, when the MPC predicts that two drones are about to collide, collision avoidance constraints are applied to both drones. No constraints are applied when there is no collision threat. This approach relaxes the constraints, expands the solution space, and makes it easier for the MPC to find more feasible solutions. The braking time T of the drone when there is a collision threat is defined. b =Bt p ≤T p This refers to the time required for the drone to brake from its maximum speed to a complete stop with maximum acceleration. When a collision threat is detected, flight is considered safe as long as the drone can maintain a sufficient safe distance within the braking time to avoid a collision. This means ensuring the drone can brake in time before a collision. The continuous collision avoidance constraints are constructed as follows:

[0150] ||p i (k+φ|k)-p j (k+φ|k)||2≥d safe -ω ij (k+φ|k) (6)

[0151] d safe ω is the predefined safe distance between drones. ij ≥0 represents the degree of relaxation of the constraints. ω ij The larger the value, the smaller the safe distance for the drone, the looser the constraints, and the larger the solution space. The constraints can be rewritten in matrix form:

[0152]

[0153] Where A a-saf b a-saf All are constant matrices, Ω i This is a constant vector. The drone swarm does not have a fixed formation, but to ensure swarm compactness, the maximum distance between drones and their neighbors needs to be constrained. Otherwise, the drone flight paths will be distributed along the map edges, and the drones will be too loosely spaced to form a swarm. The swarm compactness constraint is constructed as follows:

[0154]

[0155] Where A a-co b a-co Both are constant matrices, Xi This is a constant vector. The drone swarm needs to traverse a map with numerous static obstacles, therefore static obstacle avoidance constraints need to be imposed on the drones. The static obstacle avoidance behavior of the drones is similar to inter-drone collision avoidance, employing an event-triggered mechanism to apply obstacle avoidance constraints when a collision is predicted to occur. The static obstacle avoidance constraints are constructed as follows:

[0156]

[0157] Where A o-saf b o-saf All are constant matrices, Λ i Let be a constant vector. The optimization objective of MPC is to minimize the cost function. The design of the cost function must consider collision avoidance cost, obstacle avoidance cost, cluster compaction cost, target error cost, and control input cost. The collision avoidance cost function is constructed as follows:

[0158]

[0159] Where h a-saf and q a-saf It is the weighting coefficient, ω ij Let be relaxation factors, all of which are constants. The cost function takes the form of a quadratic form and can be solved as a QP problem. The compact cost function is constructed as follows:

[0160]

[0161] Where h a-saf and q a-saf It is the weighting coefficient, χ. ij Let be relaxation factors, all of which are constants. The obstacle avoidance cost function for the cluster is designed as follows:

[0162]

[0163] Where h o-saf and q o-saf It is the weighting coefficient, λ io Let be relaxation factors, all of which are constants. The objective error cost function is designed as follows:

[0164]

[0165] in Let q be the target point. tar These are the weighting coefficients. The control input cost function is designed as follows:

[0166]

[0167] Where q con This is the weighting constant.

[0168] Step 3) Solve the optimization problem using MPC and obtain the offline planning trajectory.

[0169] To plan the desired trajectory of the drones, each drone i needs to solve a QP problem at time k. Let P be the flight space of the drones. v This is equivalent to the drone's flight area, which stipulates that the drone can only fly within a designated space. The drone's flight speed space is defined as V. v ={v|v min ≤v≤v max Define the feasible state space χ for the UAV. i ={x i (k)=[p i (k) T ,v i (k) T ] T |p i (k)∈P v ,v i (k)∈V v Define the feasible control input space U for the UAV. i ={p refi (k)|p refi (k)∈P v Combining the above constraints and cost function, the QP problem for obtaining the desired trajectory is as follows:

[0170]

[0171] And it satisfies the following constraints:

[0172]

[0173] Simultaneously satisfying the following dynamic equations:

[0174]

[0175] Given the global position information of static obstacles, each UAV solves the aforementioned optimization problem in the prediction time domain. This ultimately yields a globally planned trajectory leading to the vicinity of the target point, with its smoothness and safety guaranteed by constraints and optimization functions. The UAV then flies along this pre-planned global trajectory to reach the vicinity of the target point. However, this trajectory is planned based on known global static obstacle information. If unknown dynamic obstacles are introduced to interfere, the UAV is likely to collide during flight. To compensate for this deficiency and enhance trajectory safety, this invention proposes an online local dynamic obstacle avoidance method based on the artificial potential field method.

[0176] Step 4) Use APF for local online planning to perform dynamic obstacle avoidance.

[0177] Since the global trajectory flight altitude of the UAV swarm planned by MPC is relatively fixed, with significant changes only during takeoff and landing, the UAV flight altitude is constant when performing dynamic obstacle avoidance based on the artificial potential field method. Therefore, the Z-axis is no longer considered, and the dynamic obstacle avoidance problem is treated as a problem in the XY two-dimensional plane. The gravitational potential field function is established as follows:

[0178] U at (X p ) = 0.5k a ρ 2 (X p ,X tar )+f at ρ(X p ,X tar (18)

[0179] Where ρ is a function measuring the distance between two points. Taking the negative gradient of the potential function yields the gravitational function as follows:

[0180] F at (X p )=-k a ρ(X p ,X tar )-f at (19)

[0181] Where X p , These represent the locations of the drone and the target point, respectively. at It is a positive constant. The repulsive field function is established as follows:

[0182]

[0183] The repulsive force function is further obtained as follows:

[0184]

[0185] in ρ represents the position of the obstacle, and ρ0 represents the range of the repulsive field of the obstacle. Beyond this distance, the repulsive field no longer has an effect.

[0186] When the distance between the drone and a dynamic obstacle is less than the collision distance, the drone will deviate from the planned trajectory to begin obstacle avoidance, and then re-enter the planned trajectory after completing obstacle avoidance. To ensure a smoother re-entry, a circle with the obstacle's center point as the center and the collision distance as the radius intersects at a point on the future planned trajectory; this point is the trajectory recovery point. Since the obstacle's direction of motion is perpendicular to the drone's overall direction of motion, this chosen recovery point is roughly symmetrical to the change point during obstacle avoidance, and the trajectory is relatively smooth.

[0187] To verify the effectiveness of the multi-UAV path planning technology based on model predictive control and an improved artificial potential field method, a simulation experiment was designed to validate the invention. The workflow of this invention is as follows: Figure 1 As shown.

[0188] I. MPC Global Path Planning Simulation

[0189] Simulation results are as follows Figures 2 to 5 As shown. Figure 1 This image displays the global historical trajectories of four drones, showing only the spatial coordinates of the trajectory points. In the simulation, the drone trajectories are time-dependent, but the image is not suitable for displaying a timeline. Although the trajectories of some drones intersect spatially in the image, they are staggered temporally, and no collision occurs. Figure 2 It can be seen that the MPC-planned trajectory is relatively smooth and avoids many static obstacles to reach the vicinity of the target point. Note that the drone swarm cannot fly completely to the target point. This is because each drone flies towards the target point under the drive of the target error cost function designed by Equation (18), while the collision avoidance cost function requires the drones to maintain a safe distance from each other. Under the influence of these two cost functions, no drone can fly completely to the target, but rather they are distributed in a cluster within a relatively close range of the target point.

[0190] Figure 3 The diagram shows the distance curves between drones within the swarm. The two gray curves, one high and one low, represent the maximum and minimum collision avoidance distances between two drones, as defined by the MPC constraints. The red curve represents the actual collision distance. It can be seen that the drones maintain a consistent distance between each other, avoiding collisions while forming a sufficiently compact swarm. Overall, the average distance is approximately 0.5 meters. Figure 4 This demonstrates the minimum distance between the drones and obstacles within the cluster. The gray curve represents the obstacle avoidance safety distance defined by the MPC constraints, while the red curve represents the actual collision distance. After the 8th second, the drones essentially flew out of the obstacle cluster, hence the rapid rise of the curve. Overall, the drones consistently maintained a safe distance from the obstacles, avoiding collisions. This verifies the safety of the MPC-planned trajectory. Figure 5 The velocity curves of the drones show that their average speed is relatively stable, satisfying the dynamic feasibility constraints. The maximum and minimum speeds of the drones within the cluster at any given moment are generally similar and relatively stable.

[0191] II. Improved Simulation of Local Path Planning Using Artificial Potential Field Method

[0192] After verifying the safety and smoothness of the MPC trajectory, the planned trajectory can be used for further dynamic obstacle avoidance simulation verification. It is worth noting that the UAV does not obtain the trajectory expression information of the obstacles, but it can detect their position information within its own detection range. The simulation results are as follows... Figures 6-9 As shown. Figures 6-9 This demonstrates the dynamic obstacle avoidance process of a drone swarm. The black squares represent dynamic obstacles, and the historical trajectories of the four drones are marked on the graph. The rightmost circle for each trajectory represents the current position of the drone. Figure 6 It can be seen that at this time, the three UAVs, UAV1, UAV2 and UAV3, encountered dynamic obstacles in front of them, and their trajectories showed a clear downward trend to avoid them. Figure 7 With the obstacle gone and the avoidance complete, the three drones continued their movement toward the target point. Figure 8 A new obstacle appears below, and the UAV4 begins to move upwards to avoid it. Figure 9 With no further collision threat and having moved away from the obstacle, the four UAVs continued flying towards the target point. Overall, the drones were able to avoid dynamic obstacles, and the two drones maintained a certain distance from each other without colliding. The obstacle avoidance trajectories were very smooth, demonstrating good dynamic obstacle avoidance capabilities.

[0193] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A multi-UAV path planning method based on model predictive control and an improved artificial potential field, characterized in that, A linear point model of the UAV is established, and constraints and cost functions are constructed. The optimization problem is solved by model predictive control (MPC) to obtain offline planned trajectories. When encountering dynamic obstacles, the artificial potential field (APF) method is used for local online planning to achieve dynamic obstacle avoidance, enabling the swarm of UAVs to safely fly through obstacles and reach the target point. The specific steps are as follows: Step 1) Establish a linear point model of the UAV. Consider a swarm of N drones. The discrete linear model for each drone is as follows: （1） in and It is a constant matrix. For the first A drone in Position and velocity at any given time, where i is the UAV's ID, and control input. This provides reference location information for the drone; The subscript i refers to the i-th drone; Step 2) Establishing Constraints MPC is discrete in time, but its overall prediction time domain is constant. ,in Let the step size be the time step. For the number of steps, For positive natural numbers, define In order to be in Predicting in real time The predicted time value, the planned trajectory of the drone is represented by a Bézier curve, the drone in The trajectory within a time period is constructed by stitching together multiple third-order Bézier curves, with each Bézier curve used for stitching being of order 3. ,Depend on Three-dimensional control points Defined as follows: the column vector formed by the control points of each curve segment is... After each prediction step in MPC, a path will be obtained from... The curve is formed by piecing together segmented trajectories, i.e., each step calculates... One control point, at Time of the first The trajectory control points of the drone are composed of column vectors To indicate; The segmented trajectory stitching of the UAV requires that the second derivative of the two Bézier curves be continuous between their first and last connection points. Based on the continuity requirement, the continuity constraints of the trajectory are constructed as follows: （2） in Representing the The first drone The first Bézier curve The first derivative on the first derivative There are several control points. In the above equation, the left side represents the last control point of the previous Bézier curve segment, and the right side represents the first control point of the Bézier curve. According to the above constraints, the piecewise trajectory splicing ensures the continuity of the second derivative. Based on the fundamental properties of Bézier curves, their higher-order control points can be represented by a linear combination of lower-order control points, i.e.: （3） Based on this property of the control points of the Bézier curve, equation (2) can be rewritten in the following matrix form: （4） Equation (4) is the first A drone in The trajectory continuity constraint at time step, where and All are constants; To limit the speed and acceleration of the drone, the following dynamic constraints are established: （5） in , All are constants; During the flight of a drone swarm, collision avoidance between drones is achieved by establishing relevant constraints. The drones are considered as rigid spherical bodies with a collision radius of... Define the safe flight distance between two drones. The collision avoidance constraints are applied using an event-triggered mechanism. Specifically, when the MPC predicts an impending collision between two drones, collision avoidance constraints are applied; otherwise, no constraints are applied. The braking time for a drone when a collision threat is defined. This refers to the time required for the drone to brake from its maximum speed to a complete stop with maximum acceleration. When a collision threat is detected, as long as the drone can maintain a sufficient safe distance within the braking time, a collision will not occur. The continuous collision avoidance constraints are constructed as follows: （6） For the predefined safe distance between drones, This represents the degree of relaxation of the constraints, and the subscript j indicates that it is the j-th drone, which is different from the i-th drone. The larger the value, the smaller the safe distance for the drone. The constraint can be rewritten in matrix form: （7） in Since drone swarms do not have a fixed formation, it is necessary to constrain the maximum distance between drones and their neighbors. The swarm compactness constraint is constructed as follows: （8） in The static obstacle avoidance behavior of UAVs is similar to that of inter-UAV collision avoidance. An event-triggered mechanism is used to apply obstacle avoidance constraints when a collision is predicted to occur. The static obstacle avoidance constraints are constructed as follows: （9） in The collision avoidance cost function is constructed as follows: （10） in and These are weighting coefficients. Let be the relaxation factor, and be constants. The cost function is in quadratic form, and is treated as... To solve the problem, a compact cost function is constructed as follows: （11） in and These are weighting coefficients. Let be relaxation factors, and be constants. The obstacle avoidance cost function of the cluster is designed as follows: （12） in and These are weighting coefficients. Let be relaxation factors, and be constants. The objective error cost function is designed as follows: （13） in For the target point, The weighting coefficients and the control input cost function are designed as follows: （14） in This is the weighting constant; Step 3) Solve the optimization problem using MPC and obtain the offline planning trajectory. In order to plan the desired trajectory of the drone, each drone Need to Solving a QP problem at any time, defining the flight space of the UAV as... This is equivalent to the flight area of ​​a drone, stipulating that the drone can only fly within a designated area, and defining the drone's flight speed space as... Define the feasible state space of the drone. Define the feasible control input space for the drone. Combining the above constraints and cost function, the QP problem for obtaining the desired trajectory is as follows: （15） And it satisfies the following constraints: （16） Simultaneously satisfying the following dynamic equations: （17） Step 4) Use APF for local online planning to perform dynamic obstacle avoidance. When performing dynamic obstacle avoidance based on the artificial potential field method, the UAV's flight altitude is constant, and the Z-axis is no longer considered. The dynamic obstacle avoidance problem is treated as a problem in the XY two-dimensional plane, and the gravitational potential field function is established as follows: （18） in To measure the distance between two points, the gravitational function is obtained by taking the negative gradient of the potential field function as follows: （19） in , These are the locations of the drone and the target point, respectively. Assuming the numbers are positive constants, the repulsive field function is established as follows: （20） The repulsive force function is further obtained as follows: （21） in The location of the obstacle. This is the range of the repulsive force field of the obstacle; beyond this distance, the repulsive force field no longer has an effect. When the distance between the drone and a dynamic obstacle is less than the collision distance, the drone will deviate from the planned trajectory to begin obstacle avoidance. After completing obstacle avoidance, it will re-enter the planned trajectory. In order to make the drone re-enter the trajectory more smoothly, a circle with the center of the obstacle as the center and the collision distance as the radius intersects at a certain point on the future planned trajectory. This point is the trajectory recovery point.

Images