Abstract
We designed a distributed collision-free formation flight control law in the framework of nonlinear model predictive control. Formation configuration is determined in the virtual reference point coordinate system. Obstacle avoidance is guaranteed by cost penalty, and intervehicle collision avoidance is guaranteed by cost penalty combined with a new priority strategy.
1. Introduction
Extensive research has been conducted on cooperative control for multiagent systems in the recent years. One motivator for the growing interest is the application of distributed multiple UAVs for distributed sensing and collaborative operations [1]. Among the main subproblems of multiple UAVs cooperative control problem, formation flight is of great interest and widely researched [2, 3]. The main goal of formation flight of multiple UAVs is to achieve a desired group formation shape while controlling the overall behavior of the group [2].
Various control schemes have been proposed for UAV formation flight, such as PID [3], potential method [4, 5], constraint forces [2], adaptive output feedback approach [6], sliding mode approach [7], and consensus-based method [8]. But those methods cannot consider constraints explicitly, such as stall velocity of fixed wing UAV, angular turn rate constraints, and control input saturation constraints. Optimization-based method can deal with the constraints appropriately, and it has proven to be one of the more successful methods for addressing formation control problems. Among the more popular optimization-based approaches is model predictive control (MPC) method.
Model predictive control, or receding horizon control (RHC), is a feedback control scheme in which a trajectory optimization is solved at each time step. The first control input of the optimal sequence is applied, and the optimization is repeated at each subsequent step [9]. It is nowadays a very active research area and a thorough survey of this method is given by Mayne et al. in [10]. It has been widely used in systems with relatively slow dynamics, such as chemical processes. With the advent of faster modern computers, its application areas are expanding to multiagent control and large-scale distributed control problems. A motivation for its wide use is the hard constraints on controls and states that are difficult to handle by other methods. Centralized MPC has been applied to the cooperative control of multiple vehicles [11]. But the computational effort required for a single optimization can become prohibitive and scales very poorly with the size of the number of unmanned vehicles. To address this problem, decentralized model predictive control (DMPC) method [12] is proposed by breaking the optimization into smaller subproblems.
Formation control strategy is important for the formation control problem. In the literature, there are mainly three information structure approaches to the formation control problem, namely, leader-follower [13], virtual structure [14], and behavioral approach [15]. Most of the multiagent formation control researches are performed in leader-follower structure, where some vehicles are designed as leaders while others are designed as followers. It is easy to understand and implement. However, this approach is not robust with respect to leader’s failure. Although virtual leader strategy is proposed to improve its robustness, the chain structure leads to a poor disturbance rejection property [13]. In the virtual structure approach, the entire formation is treated as a single virtual rigid body structure. Rather than following a path, each vehicle follows a moving point, which allows the virtual structure to potentially be attached to another vehicle [12]. The guidance of a group is easier than the other approaches since all agents in the formation are treated as a single object. But the formation can only perform synchronized maneuvers, and it is difficult to consider obstacle avoidance [16]. In the behavior approach, several desired behaviors are prescribed for each vehicle, including formation keeping, goal seeking, and collision/obstacle avoidance. The control action of each vehicle is a weighted average of the control for each behavior [17]. It is suitable for uncertain environments, but lack of a rigorous theoretic analysis.
MPC-based multivehicle formation control problem has been widely studied, such as [18–21]. In [20], a dual mode MPC method was used for robot formation control. To guarantee the stability, the dual mode controller has to switch from an MPC control to a terminal state controller. Several researchers exclusively studied UAV formation flight problem in the framework of MPC method [22–28]. Among those papers, [22, 24] mainly studied tight formation flight problem. [25–28] only use the UAV linear dynamical model in MPC problem formulation. [23] uses the UAV nonlinear dynamical model in MPC problem formulation and leader-follower structure to design formation flight controller. It uses Karush-Kuhn-Tucker (KKT) variables to achieve collision avoidance maneuver. But it needs to dynamically choose suitablevariables to determine the tradeoff between tracking and collision avoidance.
Sequential quadratic program (SQP) is one of the most effective methods for solving nonlinear programming (NLP) problem. It uses penalty or merit functions to enforce global convergence. However, it is usually difficult to choose suitable penalty parameters in practice. To avoid the practical problems associated with the setting of the penalty parameter, Fletcher and Leyffer [29] introduced a filter for SQP trust region algorithm to promote global convergence.
In this paper, we design a distributed UAV formation flight control law in the framework of nonlinear MPC. A virtual reference point control strategy is used to determine the formation configuration. The main contribution of the paper is that obstacle avoidance is guaranteed by a new cost penalty. Intervehicle collision avoidance is guaranteed by cost function combined with a priority strategy, using the delayed neighboring information. For simplicity, it is assumed that all data used in the formation flight are not corrupted by both the process and measurement noise.
The rest of the paper is organized as follows. Section 2 gives the problem formulation. Nonlinear model predictive control-based collision-free formation flight control law is designed in Section 3. Section 4 shows the simulation results and compares the algorithm with other approaches in terms of performance. Finally, concluding remarks and future work are given in Section 5.
2. Problem Formulation
2.1. 2D UAV Dynamical Models
A common control system for an unmanned aerial vehicle is a two-loop structure where the attitude dynamics are controlled by an inner loop, and the position dynamics are controlled by an outer loop. In the context of a group of UAVs in formation, the outer loop also contains a controller that can achieve and maintain the given formation configuration. For simplicity, the two-dimension motion of UAVs [30] in a horizontal plan is analyzed and the inner loop dynamic of the UAV is modeled as a first-order model: where and are UAV ’s inertial position, heading angle, velocity, and roll angle, respectively. and are the commanded velocity and roll angle to UAV ’s autopilots; is the gravitational constant. and are positive constants.
Generally, there is a reference trajectory for UAVs to flight in formation. Dynamic and kinematics constraints prohibit unmanned aerial vehicles from following arbitrary reference trajectories. Enlightened from [31], we assume that the reference trajectory generated by a formation flight trajectory generator satisfies the following equations: where and are piecewise continuous and uniformly bounded, and they satisfy the following constraints:
2.2. Formation Control Strategy
In this paper, virtual point tracking strategy is used to achieve the desired formation. Assume that there is a moving reference point representing a UAV following a predesigned reference trajectory. The real-time movement of the reference point can be known in advance or in-flight through wireless communication by each UAV. Each UAV must try to keep the prescribed relative distance and angle from this reference point. In essence, it is similar to virtual leader method, but there is no error propagation between UAVs.
Figure 1 illustrates that reference point coordinate system is attached to the reference point . It is uniquely determined by reference point’s position and velocity direction. Formation configuration can be defined in . The desired position of each UAV can be determined by the following equation: where and are the coordinates and course of the virtual reference point in XOY.
2.3. Control Objectives
Consider a team of homogenous UAVs. For simplifying the notation, we can represent the ith UAV’s dynamical model using a nonlinear discrete state space form as where and are the ith UAV’s state and control input at time , respectively. is a nonlinear continuous function.
Define the ith UAV’s tracking state error vector and control inputs error vector as where .
So, the UAV formation flight problem can be transformed to find control law such that
3. Control Law Design
The main idea of the MPC approach is to obtain the control action by repeatedly solving an optimal control problem online. Since each UAV has the ability of computation, we can design distributed formation flight control law in the framework of MPC. Each UAV calculates its own control inputs based on local states and neighboring UAV’s state information.
3.1. Virtual Point Tracking
From (2.5), the prediction of the ith UAV’s dynamics by itself can be obtained as where is the predictive horizon and indicates the ith UAV’s state predicted at time and . Correspondingly, is the predicted control inputs at time and .
Firstly, without considering obstacle avoidance and intervehicle collision avoidance problem, at time step , the cost function is defined as where , , and are positive-definite symmetric matrices, .
3.2. Obstacle Avoidance
In obstacle-rich environment, UAV must be able to avoid obstacles automatically. Traditionally, inequality constraints are added to the MPC optimization problem directly to realize obstacle avoidance [24]. It is easy to formulate the problem, but difficult to solve, because of its nonconvex property. Moreover, this type of distance-based obstacle avoidance method usually leads to unwanted avoidance and frequent maneuver.
Here, we proposed a new effective method to guarantee obstacle avoidance. As depicted in Figure 2, when the shortest distance between UAV and obstacle is less than the dangerous distance , the position and velocity orientation of the UAV are used to predict if the shortest distance between them is less than the minimal allowable distance in the near future. If so, a cost function is added to the UAV’s objective function and UAV starts to avoid it while guaranteeing not deviating too far away from the previous desired trajectory to achieve formation. For simplicity, we only consider static and circular obstacles and assume that the position and the radius of the obstacle can be obtained in real time.
At time , the obstacle avoidance cost function of the ith UAV is chosen as where
, are positive constants, and is a positive parameter. and are the Cartesian coordinates and the radius of the obstacle, respectively.
3.3. Intervehicle Collision Avoidance
Intervehicle collision avoidance is also an important aspect in formation flight. Similar to obstacle avoidance problem, some papers ensure collision avoidance, in the framework of MPC, by adding inequality constraints to the optimization problem [23]. However, too many constraints usually make the optimization problem become difficult to solve, especially in large-scale formation flight application. Moreover, UAVs involved with collision avoidance may maneuver simultaneously to avoid collision, which leads to an undesired chain reaction in the formation, especially in close formation flight.
Here we proposed a new method to ensure intervehicle collision avoidance through cost function with priority strategy. Firstly, UAVs involved with collision avoidance are tagged with a priority level according to its current relative position and mission at each sampling instant. When the relative distance between two UAVs is less than the safe separation, UAV with lower priority level should take the UAV with higher priority level as a moving obstacle to avoid. This strategy can avoid undesired chain maneuver because less UAVs need to maneuver for collision avoidance.
The policy of tagging priority level is as follows:•UAVs with smaller tracking error have higher priority level than those with larger tracking error;•UAVs with emergent mission, such as obstacle avoidance, have the highest priority.
Different from obstacle avoidance, we assume that each UAV has a circular protected zone with radius and circular collision zone with radius , as depicted in Figure 3. When the protected zones of two UAVs intersect, UAV with lower priority should take the other UAV as a moving obstacle to avoid. When the collision zones of two UAVs intersect, collision will happen.
In this paper, we assume that the intervehicle wireless communication network is always available but introduces a random time delay in the information flow. It is assumed that is bounded, that is, So, at time step , the ith UAV can obtain the jth UAV’s future position vector predicted at time step , that is, .
The intervehicle collision avoidance cost function of the ith UAV is chosen as where
are positive constant, is a positive parameter, and and are the priority level, at time , of the ith UAV and the jth UAV, respectively.
Since, at time step , the ith UAV can only obtain the UAV’s predicted position from time step to , positions of the jth UAV from time to should be recurred by the ith UAV according to the delayed information.
For simplicity, we take the linear recurrence method to predict the jth UAV’s positions from time step to as follows:
All UAV’s priority level should be calculated in a distributed way. Since there exist a random communication delay, the ith UAV calculates the priority level according to the neighboring jth UAV’s distance error vector and its current distance error .
3.4. Optimization Problem
To achieve collision-free formation flight, at time , the ith UAV needs to solve the following optimization problem: subject to
The optimal input for the current sampling interval is applied to the ith UAV. At time , repeat this procedure again with updated information and shifted horizon.
4. Simulation
In order to illustrate the feasibility and effectiveness of the designed formation flight control law, a simulation is presented in this section. Five UAVs are required to achieve the desired formation configuration from random initial positions and maintain the configuration while tracking the formation reference trajectory. Collision avoidance and obstacle avoidance should always be guaranteed in the whole process. The simulation is carried out in MATLAB. The optimization problem is solved by filterSQP function in TOMLAB [32].
4.1. Simulation Parameters
The total simulation time is 200 seconds. The predictive control horizon and control time interval of the MPC are selected as and s.
The desired UAV formation configuration is an arrow with virtual points located at in the virtual reference point coordinate system. Three static obstacles located at and with the radius of 1 km, 0.5 km, and 0.4 km, respectively. km, km, =2 km, ,, , , and rad/s (Table 1).
The reference trajectory of the reference point is illustrated as
4.2. Simulation Results
Figure 4 shows that five UAVs can rendezvous to a desired hexagonal configuration in about 100 s. The arrows show the position and heading of each UAV at snapshots of time, specifically at , and 200 seconds. In the process of formation achievement, UAV4 can avoid two obstacles automatically. After formation achievement, UAVs can track the formation reference trajectory and keep the formation configuration while maneuvering. Figure 5 illustrates UAV4’s control input commands and tracking error history. We can see that the control inputs are bounded in the constraints and tracking error can be gradually controlled to zero. Figure 6 shows distance error of each UAV, and Figure 7 shows the relative distance between UAVs. The simulation results show that the MPC-based designed formation flight control law can work well in obstacle-rich environment.
(a) velocity commands and real velocity
(b) roll angle commands and real roll angle
(c) position tracking error
(d) course angle tracking error
Figure 8 shows that the configuration of the formation cannot be maintained in the process of sharp turn. Some UAVs cannot track the virtual reference point with the prescribed offset because of its inherent control input saturation constraints. Figures 9–11 show that although UAV4’s roll angle is the maximum but the angular velocity cannot reach the desired position in the process of sharp turn.
(a) velocity commands and real velocity
(b) roll angle commands and real roll angle
(c) position tracking error
(d) course angle tracking error
5. Conclusion and Future Work
In this paper, a collision-free UAV formation flight controller is designed in the framework of MPC. The formation configuration is determined in the virtual reference point coordinate system which is uniquely determined by virtual reference point’s position and velocity direction. Then a distributed formation flight control law is designed in the frame work of MPC, which considers the nonlinear dynamical model of UAV, state and control input constraints. Obstacle avoidance is guaranteed by cost penalty. Intervehicle collision avoidance is guaranteed by collision cost penalty, using the delayed neighboring information, combined with a new priority strategy. Simulation results show that the designed controller is capable of achieving and maintaining the formation along the desired reference trajectory while avoiding obstacles and intervehicle collision. In the future, we will investigate the effects of communication delay on the proposed formation flight controller and formation reconfiguration control problem.
Acknowledgment
Research is supported by the National Science Foundation of China (NSFC) under Grant no. 6001203.