Unmanned aerial vehicle formation cooperative obstacle avoidance method based on distributed model predictive control

Abstract

Disclosed in the present invention is an unmanned aerial vehicle formation cooperative obstacle avoidance method based on distributed model predictive control. The method comprises the following steps: establishing an unmanned aerial vehicle motion model to predict a control state of a single unmanned aerial vehicle; on the basis of the unmanned aerial vehicle motion model and control objectives of reaching a target state and maintaining formation, establishing a cost function; setting a constraint, updating the control state in a prediction time domain under the condition that the cost function is minimum, and obtaining the control state of each unmanned aerial vehicle in a subsequent period of time; and each unmanned aerial vehicle flying in the subsequent period of time on the basis of the control state, so as to realize cooperative obstacle avoidance. The unmanned aerial vehicle formation cooperative obstacle avoidance method based on distributed model predictive control disclosed in the present invention has the advantages of high stability, low operation amount, low delay and the like.

Description

Cooperative obstacle avoidance method for UAV formation based on distributed model predictive control Technical Field

[0001] The present invention relates to a UAV formation collaborative obstacle avoidance method based on distributed model predictive control, belonging to the field of guidance control. Background Art

[0002] Traditional UAV formation collaborative obstacle avoidance generally adopts centralized control or decentralized control. In centralized control, the output information and status information of all subsystems are analyzed and processed by a central controller. During the control iteration process, the computational burden is too heavy. When a subsystem fails, it will cause the entire system to crash. In decentralized control, multiple controllers and subsystems are included. The subsystems are arbitrarily coupled with each other, but the upper-level controllers cannot communicate and coordinate with each other. When the coupling between subsystems is strong, the system may be unable to be stably controlled.

[0003] Therefore, in view of the problems of slow calculation speed and easy falling into local optimum of traditional obstacle avoidance algorithms, it is necessary to further study the collaborative obstacle avoidance method of UAV formation to solve the above problems.

[0004] Summary of the Invention

[0005] To overcome the above problems, the inventors conducted in-depth research and proposed a UAV formation collaborative obstacle avoidance method based on distributed model predictive control, which includes the following steps:

[0006] S1. Establish a UAV motion model to predict the control state of a single UAV;

[0007] S2, establish a cost function by combining the UAV motion model, the control objectives of reaching the target state and maintaining the formation;

[0008] S3. Set constraints to update the control state in the prediction time domain under the condition of minimizing the cost function, and obtain the control state of each UAV for a period of time.

[0009] S4. Each UAV flies for a period of time according to the control state to achieve collaborative obstacle avoidance.

[0010] Furthermore, the drone control state includes the drone state and the drone input parameters, the drone state includes the drone position and speed, and the drone input parameters are the drone acceleration.

[0011] Preferably, the UAV motion model is expressed as: i (k+1)=Az i (k)+Bu i (k),i∈{1,2,…,N a}

[0012] Among them, z i represents the state of drone i, z i =[x i ,y i ,z i ,v xi ,v yi ,v zi ] T ,(x i ,y i , z i ) represents the position of UAV i, (v xi , v yi , v zi ) represents the speed of UAV i, k represents the time, z i (k) represents the state of the UAV at time k, A and B are coefficient matrices, and u i Represents the input parameters of UAV i, u i =[a xi ,a yi ,a zi ] T ,(a xi , a yi , a zi ) represents the acceleration of UAV i.

[0013] Preferably, in S2, the cost function is expressed as:

[0014] Among them, the subscript i represents the serial number of the UAV, l represents different prediction durations, and N represents the total prediction duration, that is, the prediction time domain; α i , β i , ρ i is the weight parameter, z i Indicates the status of drone i, represents the target state of UAV i, z i (k+l|k) represents the state of drone i at time k+l predicted at time k, is the neighbor set of drone i, subscript j represents the drone number in the neighbor set of drone i, d ij represents the distance between UAV i and UAV j, represents the ideal formation state between UAVs i and j, u i Represents the input parameters of UAV i, u i (k+l-1|k) represents the input parameters of UAV i at time k+l predicted at time k.

[0015] Preferably, in S3, the constraint is expressed as:

[0016] in, represents the state set of UAV i, represents the input set of drone i, X i (k+l|k) represents the position of UAV i at time k+l predicted at time k, R is the safe distance between UAVs and between UAVs and obstacles, Represents all drones in the drone formation, represents all drones in the drone formation except drone i, is the location of the obstacle, is a collection of obstacles.

[0017] Preferably, in S3, during the control state update process, the UAV obtains the control state predicted by its neighboring UAVs.

[0018] Preferably, in S3, the control state update includes the following sub-steps:

[0019] S31. At the initial moment, each UAV i obtains initial input parameters and initial state through the UAV motion model;

[0020] S32. At the next moment, each drone i obtains the state z predicted by its neighbor drone j at that moment. j (k+l|k), combined with the initial state and initial input parameters of UAV i, minimize the cost function under the constraints and obtain the input parameters u of UAV i at the future time i (k+l|k) and state z i (k+l|k), and take the input parameters and state at l=1 as the input parameters and state at time k+1;

[0021] S33. Repeat S32 to obtain the input parameters and status of UAV i at different times over a subsequent period of time.

[0022] Preferably, in S32, the state z at time k predicted by the neighboring drone j is j (k+l|k) is obtained as follows:

[0023] UAV j takes the input parameter u at time k-1 j (k-1+l|k-1), l=0,1,…,N, the first state is removed, that is, u j (k|k-1) is removed, and a zero vector is added after the last state to obtain a new input parameter sequence. This input parameter sequence is input into the UAV motion model to obtain the state z predicted by the neighbor UAV j at time k. j (k+l|k).

[0024] The beneficial effects of the present invention include:

[0025] (1) The distributed model solves the computational complexity of existing formation problems. When one of the UAVs fails, trajectory planning can still be continued, ensuring stability.

[0026] (2) No complete global planning is required, but rather local trajectories are predicted in the time domain, which reduces the computational burden and lowers the performance requirements for the UAV’s onboard computer;

[0027] (3) Low computational complexity, fast computational speed, and low latency make it more suitable for avoiding dynamic obstacles. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] FIG1 shows a flow chart of a method for cooperative obstacle avoidance of UAV formations based on distributed model predictive control according to a preferred embodiment of the present invention;

[0029] FIG2 shows a simulation result diagram of each drone trajectory diagram in Example 1;

[0030] FIG3 shows a diagram of the simulation results of the input parameters of the UAV 1 in Example 1;

[0031] FIG4 shows a diagram of the simulation results of the input parameters of the UAV 2 in Example 1;

[0032] FIG5 shows a diagram of the simulation results of the input parameters of the UAV 3 in Example 1;

[0033] FIG6 shows a simulation result of the relative distance between drones in Example 1;

[0034] FIG7 shows a simulation result diagram of the distance between the UAV 1 and various obstacles in Example 1;

[0035] FIG8 shows a simulation result diagram of the distance between the UAV 2 and various obstacles in Example 1;

[0036] FIG9 shows a diagram of simulation results of the distances between the UAV 3 and various obstacles in Example 1. FIG. DETAILED DESCRIPTION

[0037] The present invention will be described in further detail below with reference to the accompanying drawings and examples, through which the features and advantages of the present invention will become more clearly understood.

[0038] The word "exemplary" is used exclusively herein to mean "serving as an example, example, or illustration." Any embodiment described herein as "exemplary" is not necessarily to be construed as preferred or advantageous over other embodiments. Although various aspects of the embodiments are shown in the drawings, the drawings are not necessarily drawn to scale unless otherwise indicated.

[0039] According to the present invention, a method for cooperative obstacle avoidance of a UAV formation based on distributed model predictive control is provided, as shown in FIG1 , comprising the following steps:

[0040] S1. Establish a UAV motion model to predict the control state of a single UAV;

[0041] S2, establish a cost function by combining the UAV motion model, the control objectives of reaching the target state and maintaining the formation;

[0042] S3. Set constraints to update the control state in the prediction time domain under the condition of minimizing the cost function, and obtain the control state of each UAV for a period of time.

[0043] S4. Each UAV flies for a period of time according to the control state to achieve collaborative obstacle avoidance.

[0044] The model predictive control is a dynamic system control problem with input and output constraints, and has the advantages of optimal control indicators, fast constraint processing capability, and excellent robustness.

[0045] According to the present invention, each drone is regarded as a subsystem. Through the distributed control, each subsystem can coordinate and communicate with each other in real time to ensure the timeliness and accuracy of effective information. Multiple subsystems perform iterative calculations simultaneously. When one of the subsystems reports an error, it can be identified and corrected in time. Complex calculation problems can be split into simple, precise, and small-scale calculation problems, which can reduce the calculation burden of the entire subsystem and improve calculation efficiency, thereby reducing the requirements for the drone's onboard computer, reducing the drone's control delay, and improving control accuracy.

[0046] Furthermore, in the present invention, the drone control state includes the drone state and the drone's input parameters. The drone state includes the drone's position and velocity, and the drone's input parameter is the drone's acceleration. This reduces the number of design parameters, reduces computational complexity, and improves efficiency.

[0047] In a preferred embodiment, the UAV motion model is expressed as: i (k+1)=Az i (k)+Bu i (k),i∈{1,2,…,N a}

[0048] Among them, z i represents the state of drone i, z i =[x i ,y i ,z i ,v xi ,v yi ,v zi ]T =[X i ,V i ] T , X i =[x i ,y i ,z i ] represents the position of UAV i, V i =[v xi ,v yi ,v zi ] represents the speed of drone i, k represents the time, z i (k) represents the state of the UAV at time k, A and B are coefficient matrices, and u i Represents the input parameters of UAV i, u i =[a xi ,a yi ,a zi ] T ,(a xi , a yi , a zi ) represents the acceleration of UAV i.

[0049] According to the present invention, based on the UAV motion model, the UAV state in the future can be predicted, which is expressed as:

[0050] Among them, u(k|k) is the input value at time k predicted at time k, and z(k+1|k) is the state value at time k+1 predicted at time k.

[0051] According to the present invention, the total prediction duration is predicted, i.e., the prediction time domain N The longer it is, the more accurately the model can describe the controlled system.

[0052] In a preferred embodiment, in S2, the cost function is expressed as:

[0053] Among them, the subscript i represents the serial number of the UAV, l represents different prediction durations, and N represents the total prediction duration, that is, the prediction time domain; α i , β i , ρ i is the weight parameter. The weight parameter of each UAV i can be freely set by technicians in this field according to actual needs. i Indicates the status of drone i, represents the target state of UAV i, z i (k+l|k) represents the state of drone i at time k+l predicted at time k, is the neighbor set of drone i, subscript j represents the drone number in the neighbor set of drone i, d ijRepresents the distance between drone i and drone j Indicates the state of UAVs i and j when they are in ideal formation, u i Represents the input parameters of UAV i, u i (k+l-1|k) represents the input parameters of UAV i at time k+l predicted at time k.

[0054] The ideal formation state is expressed as:

[0055] Among them, [x dij ,y dij ,z dij ] is d i j The weight, [v xi ,v yi ,v zi ] is the velocity direction component of UAV i, [v xj ,v yj ,v zj ] is the directional component of the velocity of UAV j.

[0056] The neighbor set is a range limitation commonly used in topological structures. In the present invention, there is no limitation on the setting range of the neighbor set of drone i, and those skilled in the art can freely set it according to actual needs.

[0057] According to the present invention, the cost function includes the target state part and maintain the control portion of the formation It not only simplifies the computational complexity, but also maintains optimal energy, making the drone flight more stable.

[0058] In S3, the cost function is minimized as follows:

[0059] Furthermore, in S3, the constraint is expressed as:

[0060] in, represents the state set of UAV i, represents the input set of drone i, X i (k+l|k) represents the position of UAV i at time k+l predicted at time k, R is the safe distance between UAVs and between UAVs and obstacles, Represents all drones in the drone formation, represents all drones in the drone formation except drone i, is the location of the obstacle, is a collection of obstacles.

[0061] Among them, the constraints All states and inputs must be within a feasible range. If this constraint is not added, the drone will be unable to travel along the predicted trajectory within adjacent sampling time periods due to excessive speed, which may result in the drone being stranded or colliding.

[0062] constraint It ensures that the UAV can avoid collisions and obstacles during its movement.

[0063] According to the present invention, in S3, during the control state update process, the drone obtains the control state predicted by its neighboring drones. Preferably, wireless communication can be performed between the drones, and the drone obtains the control state predicted by its neighboring drones through wireless communication.

[0064] According to the present invention, in S3, the weight parameter α i ,β i ,ρ i , the initial position of the UAV, the target position, the relative ideal distance between UAVs, the number and position of obstacles, the safety distance, and the total predicted duration are set before the control state is updated.

[0065] In a preferred embodiment, in S3, the control state update includes the following sub-steps:

[0066] S31. At the initial moment, i.e., moment k=0, each UAV i obtains initial input parameters and initial state through the UAV motion model;

[0067] The initial input parameter is denoted as u i (l|0), l=0,1,…,N;

[0068] The initial state is denoted as z i (l|0), l=0,1,…,N.

[0069] S32. At the next moment, each drone i obtains the state z predicted by its neighbor drone j at that moment. j (k+l|k), combined with the initial state and initial input parameters of UAV i, minimize the cost function under the constraints and obtain the input parameters u of UAV i at the future time i (k+l|k) and state z i (k+l|k), and take the input parameters and state at l=1 as the input parameters and state at time k+1;

[0070] S33. Repeat S32 to obtain the input parameters and status of UAV i at different times over a subsequent period of time.

[0071] In a preferred embodiment, in S32, the state z of the neighboring drone j at time k is predicted. j (k+l|k) is obtained as follows:

[0072] UAV j takes the input parameter u at time k-1 j (k-1+l|k-1), l=0,1,…,N, the first state is removed, that is, u j (k|k-1) is removed, and a zero vector is added after the last state to obtain a new input parameter sequence. This input parameter sequence is input into the UAV motion model to obtain the state z predicted by the neighbor UAV j at time k. j (k+l|k).

[0073] Example

[0074] Example 1

[0075] A simulation experiment was conducted to set up three UAVs (numbered UAV 1, UAV 2, and UAV 3) to perform UAV formation collaborative obstacle avoidance based on distributed model predictive control, including the following steps:

[0076] S1. Establish a UAV motion model to predict the control state of a single UAV;

[0077] S2, establish a cost function by combining the UAV motion model, the control objectives of reaching the target state and maintaining the formation;

[0078] S3. Set constraints to update the control state in the prediction time domain under the condition of minimizing the cost function, and obtain the control state of each UAV for a period of time.

[0079] S4. Each UAV flies for a period of time according to the control state to achieve collaborative obstacle avoidance.

[0080] The UAV motion model is expressed as: i (k+1)=Az i (k)+Bu i (k),i∈{1,2,…,N a}

[0081] In S2, the cost function is expressed as:

[0082] In S3, the constraint is expressed as:

[0083] In S3, the control state is updated, including the following sub-steps:

[0084] S31. At the initial moment, each UAV i obtains initial input parameters and initial state through the UAV motion model;

[0085] S32. At the next moment, each drone i obtains the state z predicted by its neighbor drone j at that moment. j (k+l|k), combined with the initial state and initial input parameters of UAV i, minimize the cost function under the constraints and obtain the input parameters u of UAV i at the future time i (k+l|k) and state z i (k+l|k), and take the input parameters and state at l=1 as the input parameters and state at time k+1;

[0086] S33. Repeat S32 to obtain the input parameters and status of UAV i at different times over a subsequent period of time.

[0087] In S3, set the parameters as: N=10,α i =1,β i =0.1,ρ i =0.1,R=0.25

[0088] The input parameter set is set to The initial state and target state of each UAV are shown in Table 1.

[0089] Table 1

[0090] The ideal distance between drones is shown in Table II.

[0091] During the simulation, five circular obstacles of random size and random position were set. The simulation results are shown in Figure 2-9, where:

[0092] Figure 2 shows the trajectory of each drone;

[0093] Figure 3 shows the input parameters of UAV 1, Figure 4 shows the input parameters of UAV 2, and Figure 5 shows the input parameters of UAV 3;

[0094] Figure 6 shows the relative distances between drones;

[0095] FIG7 shows the distances between UAV 1 and each obstacle, FIG8 shows the distances between UAV 2 and each obstacle, and FIG9 shows the distances between UAV 3 and each obstacle.

[0096] As can be seen from Figure 2, each drone can successfully avoid obstacles during its movement, and can maintain a safe distance even when adjacent drones are close to each other.

[0097] As can be seen from Figures 3-5, the input parameters of each drone are all within the set range of the input parameter set and gradually approach zero, indicating that the obstacle avoidance method is feasible and stable.

[0098] As can be seen from Figure 6, the distance between drones is greater than the safety distance of 0.5m during the movement, which can avoid collisions between drones.

[0099] As can be seen from Figures 7-9, the distances between different drones and all obstacles are greater than the safety distance of 0.5m, which means that all drones can achieve obstacle avoidance flight.

[0100] In the description of the present invention, it should be noted that the terms "upper," "lower," "inner," "outer," "front," and "rear" and the like, indicating positions or locations, are based on the operating state of the present invention and are intended solely to facilitate and simplify the description of the present invention. They are not intended to indicate or imply that the devices or components referred to must have, be constructed, or operate in a specific orientation, and therefore should not be construed as limitations on the present invention. Furthermore, the terms "first," "second," "third," and "fourth" are used for descriptive purposes only and should not be construed as indicating or implying relative importance.

[0101] In the description of the present invention, it should be noted that, unless otherwise expressly specified or limited, the terms "mounted," "connected," and "connected" should be understood in a broad sense. For example, they can refer to fixed, detachable, or integral connections; mechanical or electrical connections; direct or indirect connections through an intermediate medium; and internal communication between two components. Those skilled in the art will understand the specific meanings of the above terms in the present invention in specific contexts.

[0102] The present invention has been described above with reference to preferred embodiments, but these embodiments are merely exemplary and serve only as illustrations. On this basis, various replacements and improvements can be made to the present invention, all of which fall within the scope of protection of the present invention.

Claims

1. A UAV formation collaborative obstacle avoidance method based on distributed model predictive control, characterized in that: The following steps are involved: S1. Establish a UAV motion model to predict the control state of a single UAV; S2, establish a cost function by combining the UAV motion model, the control objectives of reaching the target state and maintaining the formation; S3. Set constraints to update the control state in the prediction time domain under the condition that the cost function is minimized, and obtain the control state of each UAV for a period of time; S4. Each UAV flies for a period of time according to the control status to achieve coordinated obstacle avoidance.

2. The method for cooperative obstacle avoidance of UAV formation based on distributed model predictive control according to claim 1 is characterized in that: The drone control state includes the drone state and the drone input parameters, the drone state includes the drone position and speed, and the drone input parameters are the drone acceleration.

3. The method for cooperative obstacle avoidance of UAV formation based on distributed model predictive control according to claim 1, characterized in that: The UAV motion model is expressed as: z i (k+1)=The i (k)+Bu i (k),i∈{1,2,…,N a } Among them, z i represents the state of drone i, z i =[x i ,y i ,z i ,v xi ,v yi ,v zi ] T =[X i ,V i ] T , X i =[x i ,y i ,z i ] represents the position of UAV i, V i =[v xi ,v yi ,v zi ] represents the speed of drone i, k represents the time, z i (k) represents the state of the drone at time k, A and B are coefficient matrices, and u i represents the input parameters of UAV i, u i =[a xi ,a yi ,a zi ] T , (a xi , a yi , a zi ) represents the acceleration of UAV i.

4. The method for cooperative obstacle avoidance of UAV formation based on distributed model predictive control according to claim 3 is characterized in that: In S2, the cost function is expressed as: Among them, the subscript i represents the serial number of the drone, l represents different prediction durations, N Represents the total prediction duration, that is, the prediction time domain; α i , β i , i is the weight parameter, z i Indicates the status of drone i, express The target state of drone i, z i (k+l|k) represents the state of drone i at time k+l predicted at time k, is the neighbor set of drone i, subscript j represents the drone number in the neighbor set of drone i, d ij represents the distance between UAV i and UAV j, represents the ideal formation state between UAVs i and j, u i represents the input parameters of UAV i, u i (k+l-1|k) represents the input parameters of UAV i at time k+l predicted at time k.

5. The method for cooperative obstacle avoidance of UAV formation based on distributed model predictive control according to claim 4 is characterized in that: In S3, the constraint is expressed as: in, represents the state set of UAV i, represents the input set of drone i, X i (k+l|k) represents the position of UAV i at time k+l predicted at time k, R is the safe distance between UAVs and between UAVs and obstacles, Represents all drones in the drone formation, represents all drones in the drone formation except drone i, is the location of the obstacle, is a collection of obstacles.

6. The method for cooperative obstacle avoidance of UAV formation based on distributed model predictive control according to claim 1, characterized in that: In S3, during the control state update process, the UAV obtains the control state predicted by its neighboring UAVs.

7. The method for cooperative obstacle avoidance of UAV formation based on distributed model predictive control according to claim 1, characterized in that: In S3, the control state is updated, including the following sub-steps: S31. At the initial moment, each UAV i obtains initial input parameters and initial state through the UAV motion model; S32. At the next moment, each drone i obtains the state predicted by its neighbor drone j at that moment. z j (k+l|k), combined with the initial state and initial input parameters of UAV i, minimize the cost function under the constraints and obtain the input parameters u of UAV i at the future time i (k+l|k) and state z i (k+l|k), and take the input parameters and state at l=1 as the input parameters and state at k+1; S33. Repeat S32 to obtain the input parameters and status of UAV i at different times in a subsequent period of time.

8. The method for cooperative obstacle avoidance of UAV formation based on distributed model predictive control according to claim 7, characterized in that: In S32, the state z of the neighboring drone j at time k is predicted j (k+l|k) is obtained by: UAV j takes the input parameter u at time k-1 j (k-1+l|k-1), l=0,1,…,N, the first state is removed, that is, u j (k|k-1) is removed, and a zero vector is added after the last state to obtain a new input parameter sequence, which is input into the UAV motion model to obtain the predicted state z of the neighbor UAV j at time k. j (k+l|k).

Images