Hybrid decentralized receding horizon control of vehicle formations Tam´as Keviczky, B´alint Vanek, Francesco Borrelli, Gary J. Balas Abstract— A hybrid rule-based extension of a recently proposed decentralized Receding Horizon Control (RHC) scheme is presented for vehicle formation control. The scheme makes use of logic rules which improve stability and feasibility of the decentralized method by enforcing coordination. The decentralized control laws which respect the rules are computed using hybrid control design techniques. The basic ideas are illustrated on a two-vehicle formation flight example. A more complex simulation involving six vehicles is shown as well to demonstrate the potential benefits of using rules for coordination within the RHC scheme.
I. I NTRODUCTION Motivated by control of vehicle formations, in this paper we study hybrid decentralized receding horizon control of decoupled systems. Using the approach first presented in [1], we focus on the formation control problem, which has the following characteristics: (i) it involves large number of subsystems (order of hundreds) which can be independently actuated, (ii) the subsystems are dynamically decoupled, (iii) the control objective can only be achieved through a collective behavior, and (iv) the feasible set of states of each subsystem is a function of other subsystems’ states. Applications sharing these features fall under the general class of optimal control problems for a set of decoupled dynamical systems where cost function and constraints couple the dynamical behavior of the systems. The problem of decentralized control for decoupled systems can be formulated as follows. A dynamical system (e.g. group of vehicles) is composed of distinct dynamical subsystems that can be independently actuated. The subsystems are dynamically decoupled but have common objectives and constraints which make them interact between each other. Typically the interaction is local, i.e. the objective and the constraints of a subsystem are function of only a subset of other subsystems’ states. A decentralized control scheme consists of distinct controllers, one for each subsystem, where the inputs to each subsystem are computed only based on local information, i.e. on the states of the subsystem and its neighbors. Due to the complexity of the problem, control of large scale systems is usually approached using decentralization. Examples of applications and current approaches for decentralized control design include [2]–[4]. In a recent paper [5] we have proposed a method for designing decentralized receding horizon controllers (RHC) for a certain class of large scale systems. A centralized RHC controller is broken into distinct RHC controllers of smaller sizes. Each RHC T. Keviczky, B. Vanek and G. J. Balas are with the Department of Aerospace Engineering and Mechanics, University of Minnesota, Minneapolis, MN 55455, USA, {keviczky,balas}@aem.umn.edu F. Borrelli is with the Dipartimento di Ingegneria, Universit`a degli Studi del Sannio, 82100 Benevento, Italy,
[email protected]
controller is associated with a different subsystem and computes the local control inputs based only on the states of the subsystem and of its neighbors. Along with the benefits of a decentralized design, one has to face inherent issues such as difficulties in ensuring stability and feasibility of the system. In [1] it was shown how coordination rules can be included in the decentralized control design by using hybrid system techniques. Such rules improve the overall behavior of the systems and make the control subproblems feasible where traditional design is either infeasible or too conservative. Theoretical proofs of stability and feasibility in such design schemes are treated in [6], [7] for special cases, but in general difficult to give. This paper highlights the benefits and practicality of these hybrid decentralized RHC techniques by describing their application for the control of vehicle formations, specifically Organic Air Vehicle (OAV) formation flight. OAVs are autonomous hovering ducted-fan vehicles currently used at Honeywell Laboratories. Section II introduces the decentralized control scheme used to approach a certain class of large scale control problems, including formation flight. Benefits of applying hybrid control techniques and possible ways of incorporating logic-based coordination within the decentralized framework are highlighted in Section III. Simulation results of the OAV formation flight example concludes the paper in Sections IV-V. II. P ROBLEM FORMULATION A concise description of the decentralized RHC scheme proposed in [5] follows. Consider a set of Nv linear decoupled dynamical systems, the i-th system being described by the discrete-time time-invariant state equation: xik+1 = f i (xik , uik ) i
i
i
(1) i
i
where xik ∈ Rn , uik ∈ Rm , f i : Rn ×Rm → Rn are state, input and state update function of the i-system, respectively. i i Let X i ⊆ Rn and U i ⊆ Rm denote the polytopic set of feasible states and inputs of the i-th system, respectively. We will refer to the set of Nv constrained systems as a i i ˜k ∈ RNv ×m be the team system. Let x ˜k ∈ RNv ×n and u vectors which collect the states and inputs of the team system at time k. We denote by (xie , uie ) the equilibrium pair of the i-th system and (˜ xe ,˜ ue ) the corresponding equilibrium for the team system. So far the individual systems belonging to the team system are completely decoupled. We consider an optimal control problem for the team system where cost function and constraints couple the dynamic behavior of individual systems. Using a graph to represent the coupling, we associate the i-th system to the i-th node. If an edge (i, j) connecting the i-th and j-th node is present, then the cost and the constraints of
the optimal control problem will have a component which is a function of both xi and xj . We define the interconnection graph as G(t) = {V, A(t)}, where V is the set of nodes V = {1, . . . , Nv } and A(t) ⊆ V × V the set of time-varying arcs (i, j) with i ∈ V, j ∈ V. Using the graph structure defined above, the optimization problem is formulated as follows. Denote with x ˜ik the states of all neighbors of the i-th system at time k, i.e. x ˜ik = {xjk ∈ P i j ˜ ik = j|(i,j)∈A(k) njk . ˜ik ∈ Rn˜ k with n Rn |(i, j) ∈ A(k)}, x i ˜k Analogously, u ˜ik ∈ Rm denotes the inputs to all the neighbors of the i-th system at time k. Let g i,j (xi , xj ) ≤ 0 define interconnection constraints between the i,j i-th and the i j j-th systems, with g i,j : Rn × Rn → Rnc . We will also use the following shorter form of the interconnection constraints defined between the i-th system and all its i i i,k neighbors: gki (xik , x ˜ik ) ≤ 0 with gki : Rn × Rn˜ k → Rnc . the overall cost function l(˜ x, u ˜) = PConsider Nv i n ˜ ik mi i i i i i ni × × R × R l (x , u , x ˜ , u ˜ ), where l : R k k i=1 k m ˜ ik R → R is the cost associated with the i-th system and is a function of its states and the states of its neighbor nodes. Assume that l is a positive convex function with l(˜ xe , u ˜e ) = 0. In [5], the complexity associated with a centralized optimal control design for such class of large scale systems is tackled by formulating Nv decentralized finite time optimal control problems, each one associated with a different node and implemented in a receding horizon fashion as detailed next. Let the following finite time optimal control problem Pi (t) i∗ i ˜it ) be associated with with optimal value function JN (xt , x the i-th system at time t min ˜i U t
N −1 X
i
uik,t
i
∈U , ∈X , k = 1, . . . , N − 1 xjk+1,t = f j (xjk,t , ujk,t ), (i, j) ∈ A(t),
(2c)
III. A PPLICATION OF HYBRID THEORY IN DECENTRALIZED CONTROL
(2d)
(2e) xjk,t ∈ X i , ujk,t ∈ U j , (i, j) ∈ A(t), k = 1, . . . , N − 1 (2f) g i,j (xik,t , uik,t , xjk,t , ujk,t ) ≤ 0, (i, j) ∈ A(t), k = 1, . . . , N − 1 q,r q (2g) g (xk,t , uqk,t , xrk,t , urk,t ) ≤ 0, (q, r) ∈ A(t), (i, q) ∈ A(t), (i, r) ∈ A(t), k = 1, . . . , N − 1 (2h) xiN,t ∈ Xfi , xjN,t ∈ Xfj , (i, j) ∈ A(t) ˜it ˜i0,t = x xi0,t = xit , x
(3)
(2b)
k=0
xik,t
uit = u∗i 0,t .
4) Each node repeats steps 1 to 4 at time t + 1, based on the new state information xit+1 , x ˜it+1 . In order to solve problem Pi (t) each node needs to know its current states, its neighbors’ current states, its terminal region, its neighbors’ terminal regions and models and constraints of its neighbors. Based on such information each node computes its optimal inputs and its neighbors’ optimal inputs assuming a constant set of neighbors over the horizon. The input to the neighbors will only be used to predict their trajectories and then discarded, while the first component of the i-th optimal input of problem Pi (t) will be implemented on the i-th node. The solution of the i-th subproblem will yield a control policy for the i-th node of the form uit = cit (xit , x ˜it ). Even if we assume N to be infinite, the decentralized RHC approach described so far does not guarantee that solutions computed locally are globally feasible and stable. This is due to the fact that the trajectory of xj predicted by problem Pi (t) and the one predicted by problem Pj (t), based on the same initial conditions, are different (since in general, Pi (t) and Pj (t) will be different). In the following section, incorporation of coordination rules within the decentralized control strategy presented above is shown using hybrid system techniques based on [1].
i ˜iN,t ) (2a) (xiN,t , x ˜ik,t ) + lN ˜ik,t , u lti (xik,t , uik,t , x
subj. to xik+1,t = f i (xik,t , uik,t ),
We will define the following decentralized RHC scheme. At time t 1) Compute graph connection A(t) according to a chosen policy. 2) Each node i solves problem Pi (t) based on measurements of its state xit and the states of all its neighbors x ˜it . ˜ i∗ 3) Each node i implements the first sample of U t
(2i)
i i ˜ti , [ui , u ˜iN −1,t ]0 ∈ Rs , s , where U 0,t ˜0,t , . . . , uN −1,t , u i i (m ˜ + m )N denotes the optimization vector, xik,t denotes the state vector of the i-th node predicted at time t + k obtained by starting from the state xit and applying to system (1) the input sequence ui0,t , . . . , uik−1,t . The tilded vectors denote the prediction vectors associated with the neighboring systems assuming a constant interconnection graph.
In general, it is very difficult to provide feasibility guarantees in a constrained decentralized control problem. Nevertheless everyday life is full of decentralized control problems. Although feasible solutions are not always found or even possible at all, these problems are solved day-by-day relying on certain rules that help coordinate the single subsystem efforts. Examples range from traffic laws to behavior of individuals in a community. This suggests that it can be beneficial to make use of coordination rules in some decentralized engineering control problems as well. Such rules can also be used to influence the information exchange policy. Hybrid control design techniques are able to cope with the hybrid nature of a problem governed by differential equations and logic rules. For this reason it is worthwhile to investigate the benefits of hybrid system techniques in implementing coordination rules within the decentralized control framework presented in Section II. A more formal discussion follows. We define a rule element to be a Boolean-valued function operating on the states of a node and its neighbors’ states % : (xi , x ˜i ) → X,
X = {true, f alse} .
(4)
We define a rule to be a propositional logic statement involving rule elements R : (%1 , %2 , . . . , %n−1 ) → X,
X = {true, f alse} . (5)
The logic statement R is a combination of “not” (¬), “and” (∧), “or” (∨), “exclusive or” (⊕), “implies” (→), and “iff” (↔) operators. For instance, the following logic expression of R (X1 , . . . , Xn−1 ) ↔ Xn (6) involving Boolean variables X1 , . . . , Xn can be expressed equivalently with its conjunctive normal form (CNF) k ^ _ _ _ Xi ¬Xi , Nj , Pj ⊆ {1, . . . , n}. j=1
i∈Pj
i∈Nj
(7) The rule holds and its value is “true” if the statement is evaluated as true based on the rule elements. The rule is not respected and its value is “false” when the underlying statement is false. We introduce two abstract function classes called coordinating functions, which operate on a set of rules and the states of a node and its neighbors ˜i ) → R FcC : ( mδij
TABLE I U SE OF BINARY VARIABLES TO EXPRESS DISJUNCTIONS
relative distance and define the set A(t) as A(t) = {(i, j) ∈ V × V | kxit,pos − xjt,pos k ≤ dmin }, (12) that is the set of all the arcs, which connect two vehicles whose distance is less than or equal to dmin . A detailed description of the problem formulation for this particular application example can be found in [7]. The above choice of dynamics, linear cost and constraints allow us to rewrite problem (2) as a Mixed Integer Linear Program (MILP) [11], [12], for which efficient branch-and-bound solvers are available [13]. Note that any other linear or piecewise linear formulation of constraints, cost and dynamics can be cast as an MILP [11]. The resulting local MILPs can be implemented on the vehicle platform by evaluation of an equivalent lookup table obtained by means of parametric programming [14].
for the purpose of describing the idea behind implementing inter-vehicle coordination rules in two dimensions. The superscripts “E, W, N, S” stand for “east”, “west”, “north” and “south” corresponding to positive x, negative x, positive y and negative y directions, respectively. For instance, the N value of the δi,j variable is true if the i-th vehicle is “north” of the j-th vehicle, or in other words if yi − p2 > yj + p2 . Using the binary variables introduced in Table I, the condition to be satisfied for avoiding collision is S N W E OR δij OR δij OR δij δij
(13)
In order to establish desired coordination rules, we add a linear term of the binary variables in the cost function that penalizes certain undesired relative positions between the ith vehicle and its neighbors determined by the disjunction associated with a particular binary variable.
A. Inter-vehicle coordination rules N −1 X In order to improve coordination and the likelihood of i bin i ˜iN,t ) (xiN,t , x ˜ik,t ) + lN ˜ik,t , u ˜it ) , lti (xik,t , uik,t , x (xt , x JN feasibility of the decentralized scheme, different “right-ofk=0 way” priorities can be introduced which allows to have better N −1 X X prediction about neighbors’ trajectories. This can be easily cδi,j δi,j (k) (14) + achieved if protection zones around vehicles are modeled as k=0 j|(i,j)∈A(t) parallelepipeds and the disjunctions are modeled as binary E W N S 0 variables [15]. “Right-of-way” priorities can be translated where δi,j = [δi,j δi,j δi,j δi,j ] in the two-dimensional into weights and constraints on the binary variables which case. describe the location of a vehicle with respect to a paralFor instance, if we would like to penalize having neighbors lelepipedal protection zone of another vehicle (six binary to the “east” side of the i-th vehicle during a maneuver, variables in three dimensions for each vehicle couple). then the corresponding term in the binary variable coefficient The main idea behind inter-vehicle coordination is to vector should be a non-zero positive number cδi,j = [∗ 0 0 0] make use of “preferred” decisions in the hybrid control for all j|(i, j) ∈ A(t). problem that arises due to the non-convex collision avoidance V. S IMULATION EXAMPLES constraints. For illustration, consider the following scenario. Simulations involving two vehicles will be used for illusAssume that protection zones around vehicles are specified as square exclusion regions centered around each vehicle’s tration of the basic idea. A more complex example with six position as depicted in Table I. Collision avoidance can be vehicles will be shown as well for further demonstration. represented by introducing binary decision variables associ- Consider first a simple scenario, where two vehicles have to ated with the feasibility of linear inequalities defined over reach a specified location, and along the way they need to the system states. Disjunctions of the protection zones can get around each other in order to achieve the absolute and then be easily described by propositional logic statements relative position references. This setup intends to mimic a involving the binary decision variables. This mixture of logic typical “challenging” conflict scenario where vehicles have states and dynamics is then modeled in the MLD framework to reach their final targets by crossing each others paths. [11] by translating logic relations into mixed-integer linear Although the vehicles can both sense each other, they make inequalities. Part of this translation is illustrated in Table I decisions independently whether to avoid the other vehicle by
turning to the right or left. Since the local cost functions are not assumed to be strictly convex and the collision avoidance constraints are concave (which lead to multiple optima), the local optimal control problems may lead to conflicting solutions. In order to help influence their decentralized decisions and increase the accuracy of their predictions about each other, we include a set of “rules” using the technique presented above. The decentralized scheme (2)-(3) is applied to the problem using a prediction horizon length of 14 steps (2.8 seconds). Snapshots of the simulation is shown in Figure 1 indicating that although a feasible solution is found in a decentralized way, the two vehicles seem to have difficulty in deciding which way to avoid each other. Although both vehicles solve a centralized problem, they arrive at independent optimal solutions to implement, which are different from each other due to multiple optima. Since there is no coordination among the two vehicles, conflicting trajectories shown in this example could arise. Figure 2 shows the resulting trajectories when vehicle #1 penalized binary variables that indicated being on the “west” or “south” side of any neighbor. Correspondingly, vehicle #2 penalized being on the “east” or “north” side of any neighbor. This translates into a coordination policy, where vehicle #1 avoids others by going around them in the “north” direction, while vehicle #2 evades neighbors from the “south”. The corresponding binary variable coefficient vectors were selected as cδ1,2 = [0 500 0 500] and cδ2,1 = [500 0 500 0]. The “preferred” direction of evasion can be flipped if we change the “north-south” binary variable weights to penalize opposite sides of the protection zones. This results in an alternative set of coordination rules, and the vehicles avoid each other by turning into the opposite direction [16]. Simulation results of a six-vehicle scenario is shown in Figures 3-4. The vehicles are initially placed in a hexagonal formation and their objective is to reach the location of the vehicle on the opposite side relative to the formation’s center. Essentially, each vehicle has to “swap” places with its mirror image with respect to the formation center. Each vehicle is connected to its two closest spatial neighbors, leading to a time-varying and directed interconnection graph. Figure 3 shows snapshots of the simulation without any coordination rules. Vehicles reach their final objectives but not without any difficulty or collision. The conflicting decentralized trajectories that arise from this scenario can be planned more easily if coordination rules are introduced as illustrated in Figure 4. These rules are pre-determined based on the final objective and establish preferred relative positions between each vehicle pair during the entire simulation, similarly to the two-vehicle example. Localized, time-varying coordination rules based on consistent “rightof-way” agreements (e.g. yield to the right) can be defined as well. These rules are activated only when neighboring vehicles get closer than a certain threshold and a conflict situation arises. Simulation results of the presented scenarios can also be found at [16].
VI. C ONCLUSIONS AND FUTURE WORK A hybrid decentralized receding horizon framework has been presented, which enables the incorporation of rules by means of coordinating functions to improve feasibility of the distributed optimization problems with coupling constraints. Application of the proposed scheme was shown in the case of formation flight maneuvers with collision avoidance formulated as polyhedral protection zones. The proposed set of coordinating functions can be employed either in the local cost functions, where trajectories violating rules are penalized, or within constraints leading to a reduction of the local domains of feasibility to a set that encompasses only those trajectories, which enforce the rules. Note that these practical techniques will not imply feasibility by themselves but help in avoiding undesirable formation behavior as illustrated by numerical simulations of unmanned hovering vehicles. Proper selection of rules in such decentralized scenarios is an open problem, but results such as [17] for a provably safe coordinated strategy in distributed air traffic conflict resolution or other examples from current adhoc, everyday approaches to constrained large-scale control problems usually provide a good starting point. R EFERENCES [1] F. Borrelli, T. Keviczky, G. J. Balas, G. Stewart, K. Fregene, and D. Godbole, “Hybrid decentralized control of large scale systems,” in Hybrid Systems: Computation and Control, ser. Lecture Notes in Computer Science. Springer Verlag, Mar. 2005. ˇ [2] D. D. Siljak, Decentralized Control of Complex Systems. Academic Press, 1990. [3] A. Bicchi and L. Pallottino, “On optimal cooperative conflict resolution for air traffic management systems,” IEEE Trans. Intelligent Transportation Systems, vol. 1, no. 4, pp. 221–231, December 2000. ¨ [4] P. Ogren, M. Egerstedt, and X. Hu, “A control lyapunov function approach to multi-agent coordination,” IEEE Trans. on Robotics and Automation, vol. 18, no. 5, pp. 847–851, Oct. 2002. [5] T. Keviczky, F. Borrelli, and G. J. Balas, “A study on decentralized receding horizon control for decoupled systems,” in Proc. American Contr. Conf., 2004. [6] ——, “Stability analysis of decentralized RHC for decoupled systems,” in 44th IEEE Conf. on Decision and Control, and European Control Conf., Seville, Spain, Dec. 2005. [7] T. Keviczky, “Decentralized receding horizon control of large scale dynamically decoupled systems,” Ph.D. dissertation, Control Science and Dynamical Sys. Center, Univ. of Minnesota, Minneapolis, 2005. [8] D. Stipanovic, G. Inalhan, R. Teo, and C. J. Tomlin, “Decentralized overlapping control of a formation of unmanned aerial vehicles,” Automatica, vol. 40, no. 8, pp. 1285–1296, 2004. [9] A. Richards, J. Bellingham, M. Tillerson, and J. P. How, “Coordination and control of multiple UAVs,” in AIAA Guidance, Navigation, and Control Conference, Monterey, CA, 2002. [10] W. B. Dunbar and R. M. Murray, “Model predictive control of coordinated multi-vehicle formation,” in Proc. 41th IEEE Conf. on Decision and Control, 2002. [11] A. Bemporad and M. Morari, “Control of systems integrating logic, dynamics, and constraints,” Automatica, vol. 35, no. 3, pp. 407–427, Mar. 1999. [12] A. Richards and J. P. How, “Aircraft trajectory planning with collision avoidance using mixed integer linear programming,” in Proc. American Contr. Conf., 2002. [13] ILOG, Inc., CPLEX 7.0 User Manual, Gentilly Cedex, France, 2000. [14] F. Borrelli, Constrained Optimal Control of Linear and Hybrid Systems, ser. Lecture Notes in Control and Information Sciences. Springer, 2003, vol. 290. [15] T. Schouwenaars, B. D. Moor, E. Feron, and J. How, “Mixed integer programming for multi-vehicle path planning,” in Proc. European Control Conf., 2001. [16] http://www.aem.umn.edu/people/students/keviczky/Simulations.html. [17] G. Dowek, C. A. Mu˜noz, and V. A. Carre˜no, “Provably safe coordinated strategy for distributed conflict resolution,” in AIAA Guidance, Navigation, and Control Conf., Aug. 2005.
(a)
(b) Fig. 1.
(a)
(c)
(d)
Conflict resolution without coordination rules using the decentralized scheme.
(b)
(c)
(d)
Fig. 2. Conflict resolution using coordination rules. These snapshots illustrate the scenario, when vehicle #2 prefers being on the “north” side, vehicle #2 being on the “south” side.
(a)
(b) Fig. 3.
(a)
(d)
Decentralized conflict resolution without coordination rules in the six-vehicle example.
(b) Fig. 4.
(c)
(c)
Decentralized conflict resolution using coordination rules in the six-vehicle example.
(d)
