An Optimal Control Approach to Flocking
Logan E. Beaver Chris Kroninger Andreas A. Malikopoulos

	
	 SIMEX Estimation in Parametric Modal Regression with Measurement Error
    Weixing Songcor
    March 8, 2003
=======================================================================


	    Flocking behavior has attracted considerable attention in multi-agent systems. The structure of flocking has been predominantly studied through the application of artificial potential fields coupled with velocity consensus.
	    These approaches, however, do not consider the energy cost of the agents during flocking, which is especially important in large-scale robot swarms.
	    This paper introduces an optimal control framework to induce flocking in a group of agents.
	    Guarantees of energy minimization and safety are provided, along with a decentralized algorithm that satisfies the  optimality conditions and can be realized in real time.
	    The efficacy of the proposed control algorithm is evaluated through simulation in both MATLAB and Gazebo.
	
	
	

§ INTRODUCTION

	
    

 §.§ Background


    Complex systems consist of many interdependent, independent agents that are connected to each other in both space and time <cit.>. The individual interactions between agents may result in unpredictable emergent behavior at a large scale. As the world becomes increasingly complex <cit.>, modern control approaches will be required to optimize individual agents with an eye on overall system behavior <cit.>.
    
    Multi-agent systems have attracted considerable attention in many applications due to their natural parallelization, general adaptability, and ability to self-organize <cit.>.
    This has proven useful in many applications, such as transportation <cit.>, construction <cit.>, and surveillance <cit.>.
    Controlling emergent flocking behavior has been of particular interest to robotics researchers since the seminal paper by Reynolds <cit.>, which introduced three heuristic rules for flocking in digital animation: move toward neighboring flockmates, avoid collisions, and match velocity with neighbors.
    Flocking has many practical applications, such as mobile sensing networks, coordinated delivery, reconnaissance, and surveillance  <cit.>.
    
 
    



    Currently, the most popular approach to impose flocking in multi-agent systems is the use of artificial potential fields and velocity consensus <cit.>.
    In these approaches, flock aggregation and collision avoidance are both handled by the potential field, the design of which is still an open question <cit.>.
    Several theoretical guarantees have been proven for these types of flocking models <cit.>; however, minimum-energy flocking still remains relatively unexplored.
    
    In this paper, we propose an extension of our previous work on energy-optimal trajectories for formations <cit.> and apply it to the problem of flocking. We provide the following three contributions:
	(1) a continuous decentralized optimal control framework for minimum-energy flocking, (2) a closed-form solution for the energy-minimizing control input in the centralized case, and (3) an adaptation of the centralized optimal solution to the decentralized case.
    
    There have been several approaches in the literature that have considered optimal flocking using dynamic programming for aircraft aggregation <cit.>, and optimal control over discrete time <cit.>. In contrast, our approach is continuous, has an analytical solution, and allows flexibility in the flock shape.
	Gomez et al. <cit.> used a centralized controller to derive optimal trajectories for teams of agents to flock. In contrast, our approach adopts the centralized solution to a decentralized system without requiring a central computer to plan trajectories.
	The framework we present in this paper is related to the robot ecology paradigm for long-duration autonomy <cit.>. However, we apply optimal control over a planning horizon, rather than reacting to the environment with gradient flow.

	   



	




	
	

	

	
	
	The structure of the paper is as follows. We formulate the problem in Section <ref>. In Section <ref>, we solve the optimization problem for the unconstrained and constrained cases. In Section <ref>, we provide two sets of simulation results and discuss the observed emergent behavior. In the first set, we use MATLAB to demonstrate the viability of the proposed control scheme; in the second set, we use Gazebo to validate a minimum-communication approach, which reformulates the control method to use pure sensing. Finally, we draw conclusions and discuss future work in Section <ref>.
	
	

§ PROBLEM FORMULATION
 
	
	Consider a flock of  N∈ℕ agents indexed by the set 𝒜 = { 1, 2, …, N}. Each agent i∈𝒜 follows the double integrator dynamics,
	
    𝐩̇_i(t)    = 𝐯_i(t), 
    𝐯̇_i(t)    = 𝐮_i(t),

	where t∈ℝ_≥0 is the time, and 𝐩_i(t), 𝐯_i(t), 𝐮_i(t) ∈ℝ^2 are the position, velocity, and control input, respectively. Each agent occupies a closed disk of radius R∈ℝ_>0. The state of each agent is given by
	
    𝐱_i(t) = 
    	[ 𝐩_i(t); 𝐯_i(t) ].

	The speed and control input are constrained such that
	
    ||𝐯_i(t)|| ≤ v_i^max, 
    
    	||𝐮_i(t)|| ≤ u_i^max,

	for all t∈ℝ_≥0.
	
	For any pair of agents i,j∈𝒜, the relative displacement between them is described by the vector
	
    𝐬_ij(t) = 𝐩_j(t) - 𝐩_i(t),    i,j∈𝒜.
 
	To guarantee safety within the system, we impose the following constraints:
	
    𝐬_ij(t)·𝐬_ij(t)    ≥ 4R^2,   ∀  j∈𝒜,   ∀ t∈ℝ_≥0,
    
    		h    > 2R,

	where (<ref>) guarantees collision avoidance and (<ref>) is a system-level constraint which allows collisions to be detected before they occur. 
	Each agent has also a sensing/communicating distance, h∈ℝ_>0, which is used to define its neighborhood.
	 
	The neighborhood of each agent i∈𝒜, is defined by the set
	
    𝒩_i(t) = { j ∈𝒜 |   ||𝐬_ij(t)|| < h },

	where ||·|| is the Euclidean norm.
	
	The neighborhood is allowed to switch over time, while i∈𝒩_i(t) always holds. Agent i is able to communicate with any agent j∈𝒩_i(t) and sense its current state, 𝐱_j(t). A schematic of the system is presented in Fig. <ref>.
	
	
	
	
	Finally, every agent is also equipped with a long-range sensor, which can estimate the centroid of the flock, given by
	
    𝐩_cg(t) = 1/|𝒩_i(t)|∑_i∈𝒜𝐩_i(t),

	where |·| denotes set cardinality.
	The purpose of (<ref>) is to drive isolated agents back toward the flock. This can reasonably be achieved with inexpensive audio or visual sensors in the case where a majority of the agents have already formed an aggregate. However, for the case where there is no clear flock, it may be challenging to determine (<ref>) in practice. 
	
	In order to guarantee safety (<ref>), and to calculate (<ref>), agent i∈𝒜 requires information about the trajectories of all agents j∈𝒩_i(t). One potential approach to handle this is to impose a priority ordering on the agents; in this case, higher priority agents act first while lower priority agents must steer to avoid them <cit.>. Our approach is for each agent to apply model predictive control. Each agent generates an initial unconstrained trajectory for a given time interval t∈[t_0, t_f], where t_f - t_0 > 0 is the horizon. Then, agent i follows this generated trajectory for some period Δ T, at which point the trajectory is recalculated over the new interval t∈[t_0 + Δ T, t_f + Δ T].
	
	In this case, a longer time interval corresponds to a larger planning space at the cost of increased computational complexity. A decrease in the replanning period, Δ T,  provides agent i with a better estimate of its neighbors' trajectories at the cost of more frequent calculations. The trajectory generated by each agent is the outcome of minimizing the flocking error function, defined next.
	 
		For all agents i∈𝒜, the flocking error is defined by the scalar function
		
    Φ(𝐱_i, t_f)    =  w_1 ϕ_d(𝐱_i, t_f) +
    		w_2 ϕ_v(𝐱_i, t_f) 
       + w_3 ϕ_a(𝐱_i, t_f), 
    ϕ_d(𝐱_i, t_f)    = ||𝐯_i(t_f) - 𝐯_d(t_f) ||^2,
    		
    ϕ_v(𝐱_i, t_f)    = ||𝐯_i(t_f) - 𝐯_avg(t_f)||^2,
    		
    ϕ_a(𝐱_i, t_f)    = 
    		
    		(||𝐩_cg(t_f) - 𝐩_i(t_f)|| - D)^2,  |𝒩_i(t_0)| = 1,
    j∈𝒩_i(t_0)∑( 𝐬_ij(t_f)·p̂_ij(t_f)-D  )^2, 
     |𝒩_i(t_0)| > 1,

		where w_1, w_2, and w_3 are normalizing parameters which weight the influence of velocity control ϕ_d(𝐱_i, t_f), velocity matching ϕ_v(𝐱_i, t_f), and aggregation ϕ_a(𝐱_i, t_f) in the overall system behavior. The system parameters 𝐯_d(t_f) and D set the desired flock velocity and separating distance between agents. The average agent speed, 𝐯_avg(t_f), is given by
		
    𝐯_avg(t_f) = 𝐩̇_cg(t_f),       |𝒩_i(t_0)| = 1,
    1/|𝒩_i(t_0)|∑_j∈𝒩_i(t_0)𝐯_j(t_f),      |𝒩_i(t_0)| > 1.

	
	
	The cases in (<ref>) and (<ref>) when |𝒩_i(t_0)| = 1 are equivalent to placing a virtual agent at the centroid of the flock for any agent which becomes isolated. This will drive isolated agents toward the centroid by the construction of the flocking error. However, this does not guarantee that a disconnected group of agents will return to the flock if separation occurs.
	
	Each agent generates its energy-optimal trajectory by solving the following decentralized optimal control problem.
	
	(Minimum-energy flocking)
	
	For every agent i∈𝒜,
	
    min_𝐮_i(t)   {Φ(𝐱_i, t_f) + ∫_t_0^t_f ||𝐮_i(t)||^2 dt }
    subject to:   (<ref>), (<ref>), (<ref>), (<ref>), (<ref>),  𝐱_i(t_0) = 𝐱_i^0,

	where Φ(𝐱_i, t_f) is given by Definition <ref> and 𝐱_i^0 is the initial state of agent i.
	
	By minimizing the L^2 norm of the control input we expect to see a proportional reduction in energy consumption.
	To solve Problem <ref>, we impose the following assumptions.
	
        There are no external disturbances or obstacles.
    
    Assumption <ref> is imposed to evaluate the idealized performance of the proposed algorithm. 
    This assumption may be relaxed by introducing a measure of robustness into Problem <ref>.
    
	
	    There are no errors or delays with respect to communication and sensing.
	
    The strength of Assumption <ref> is application dependent. In general, it has been shown that sparse updates to model predictive control may be sufficient <cit.>. 
    
    
        The flock is low-density (only two agents i,j∈𝒜 ever come within distance |𝐬_ij| = 2R of each other).
    
	Assumption <ref> may be strong, but it is imposed to simplify the solution to the optimal speed profile for two agents that are safety constrained.
	This assumption may be removed if an order is imposed on the agents rather than using model predictive control <cit.>, and it may be relaxed in cases when a numerical solver can generate the safety-constrained trajectory in real-time.
	
	
	

§ SOLUTION APPROACH
 

    To derive an analytical solution for Problem <ref> we use Hamiltonian Analysis.
    As the case where the state and control constraints, (<ref>) and (<ref>), are active is well studied in the literature <cit.> we will only consider the safety constrained case.
	
	As a first step, for an agent i∈𝒜, the safety constraint, (<ref>), must be derived until the control input, 𝐮_i(t), appears,
	
    𝐍_i(t) = [               4R^2 - 𝐬_ij(t)·𝐬_ij(t);                    -𝐬_ij(t)·𝐬̇_ij(t); -𝐬_ij(t)·𝐬̈_ij - 𝐬̇_ij(t)
    	·𝐬̇_ij(t) ]≤0.

    The Hamiltonian is then augmented by the final row of 𝐍_i(t), which yields
	
    H_i = ||𝐮_i(t)||^2 + λ_i^p(t)·𝐯_i(t) + λ_i^v(t)·𝐮_i(t) 
    
    	- ∑_j∈𝒩_iμ_i,j(t) ( 𝐬_ij(t) ·𝐬̈_ij(t) + 𝐬̇_ij(t)·𝐬̇_ij(t)  ),

	where λ_i^p(t), λ_i^v(t) are the position and velocity covectors, and μ_ij(t) is the Lagrange multiplier with values
	
    μ_ij(t) = 
    		≥ 0,     if  𝐬_ij(t) ·𝐬̈_ij(t) + 𝐬̇_ij(t)·𝐬̇_ij(t) = 0, 
    
    			0,     if  𝐬_ij(t) ·𝐬̈_ij(t) + 𝐬̇_ij(t)·𝐬̇_ij(t) > 0.


	To solve (<ref>) for agent i∈𝒜 we consider that (i) all agents j∈𝒩_i(t) satisfy μ_i,j = 0 or 
	(ii) any agent j∈𝒩_i(t) satisfies μ_i,j > 0. We then piece the constrained and unconstrained arcs together to arrive at a piecewise-continuous, energy-optimal trajectory. Next, we present the unconstrained motion and boundary conditions followed by our algorithm for constructing an energy-optimal trajectory in real-time.	
	
	

 §.§ Unconstrained Motion
 
	The energy-optimal unconstrained trajectories for the position, speed, acceleration, and covectors resulting from (<ref>) over an interval t∈[t_1, t_2]⊂ℝ_≥0 are <cit.>
	
    𝐩_i(t) =    1/6𝐚_i t^3 + 1/2𝐛_i t + 𝐜_i t + 𝐝_i,
    𝐯_i(t) =    1/2𝐚_i t^2 + 𝐛_i t + 𝐜_i ,
    𝐮_i(t) =    𝐚_i t + 𝐛_i ,
    λ^p_i(t) =    𝐚_i , 
    λ^v_i(t) =     -𝐚_i t - 𝐛_i,

	where 𝐚_i, 𝐛_i, 𝐜_i, 𝐝_i ∈ℝ^2 are constants of integration. Equations (<ref>) - (<ref>) contain 8 unknowns, which are solved using 8 boundary conditions;
	
    𝐱_i(t_0)    = 𝐱_i^0, 
    λ_i(t_f)    = ∂Φ(𝐱_i,t)/∂𝐱_i|_t_f.

	
	Solving (<ref>) yields
	
    λ_i^p(t_f)    =
    	-2w_3 ∑_j∈𝒩_i(t_0)( ( ||𝐬_ij(t_f)||-D )  𝐬_ij(t_f) ), 
    λ_i^v(t_f)    = 2w_2(𝐯_i(t_f)-𝐯_avg)  + 2w_1(𝐯_i(t_f)-𝐯_d).

	We then substitute (<ref>), (<ref>), and (<ref>) into (<ref>) and (<ref>) to solve for 𝐩_i(t_f) and 𝐮_i(t_f). This yields two equations
	
    𝐩_i(t_f)    = 𝐚_i/2w_3∑_j(||𝐬_ij(t_f)|| - D) + ∑_j∈𝒩_i𝐩_j(t_f),  
    𝐮_i(t_f)    = -2w_2(𝐯_i(t_f)-𝐯_avg)  - 2w_1(𝐯_i(t_f)-𝐯_d),

	where 𝐚_i = 0 if the right-hand side of (<ref>) is ever zero.
	
	Equations (<ref>), (<ref>), and (<ref>) give three conditions to solve for the constants 𝐛_i, 𝐜_i, and 𝐝_i in (<ref>) - (<ref>). The value of 𝐚_i can then be found by the solution of (<ref>), which must be computed numerically.
	
	
	

 §.§ Constrained Motion

	To generate the safety-constrained motion of agent i∈𝒜, we require agent i to cooperate with all agents j∈𝒩_i(t) to solve the centralized optimal control problem whenever the collision avoidance constraint becomes active. The agents will then all employ the centralized solution to guarantee collision avoidance. 
	
	For any agent i∈𝒜, we define the set 𝒱_i as
	
    𝒱_i(t) {j∈𝒩_i(t)  | μ_ij(t) > 0,   j≠ i  }.

	Thus, when 𝒱_i ≠∅ agent i must follow a safety-constrained trajectory. Application of the Euler-Lagrange equations yields
	
    𝐮_i(t) =     -λ_i^v(t) - ∑μ_ij(t)𝐬_ij(t), 
    
    	-λ̇_i^v(t) =    λ_i^p(t) + ∑μ_ij(t)𝐬̇_ij(t), 
    
    	-λ̇_i^p(t) =    ∑μ_ij(t)𝐬̈_ij(t),


	which, in general, must be solved numerically. However, under Assumption <ref>, we may consider the case where only two agents interact. 
	We define
	
    a_ij(t)  ||𝐬̇_ij(t)||,

	which is the relative speed between the two constrained agents. 
	We may then construct a new basis for ℝ^2 which we define next.
	
	The orthonormal contact basis for any two agents in contact, i∈𝒜,   j ∈𝒱_i, with a nonzero relative speed, is defined as
	
    p̂_ij(t)     𝐬_ij(t)/||𝐬_ij(t)|| = 𝐬_ij(t)/2R,
    q̂_ij(t)     𝐬̇_ij(t)/||𝐬̇_ij(t)|| = 𝐬̇_ij(t)/a_ij(t),

	where p̂_ij(t)·q̂_ij(t) = 0 by (<ref>).
	
	
	
    To solve the (<ref>) - (<ref>) we will project 𝐬̈_ij(t) onto the contact basis (Definition <ref>). From (<ref>) we can project 𝐬̈_ij onto p̂_ij by
	
    𝐬̈_ij(t)·𝐬_ij(t) = -ṡ_ij(t)·ṡ_ij(t) = -a_ij^2(t).









  
  The projection of 𝐬̈_ij(t) onto q̂_ij(t) can be calculated by applying integration by parts, which yields <cit.>,
    
    𝐬̈_ij(t)·𝐬̇_ij(t) = a_ij(t)·ȧ_ij(t).

    Therefore, the projection of 𝐬̈_ij onto the contact basis is
    
    𝐬̈_ij(t) =
            [ -a_ij^2(t)1/2R;        ȧ_ij(t) ].

    Finally, we can use (<ref>) to solve for the time derivatives of (<ref>) and (<ref>). First we have
    
    d/dtp̂_ij(t) = 𝐬̇_ij(t)/2R = a(t)/2Rq̂_ij(t).

    Taking the time derivative of q̂_ij(t) and substituting (<ref>) in the numerator yields <cit.>,
    
        
    d/dtq̂_ij(t) 
            
            
            
            = -a(t)/2Rp̂_ij(t).

    
	
	
	By (<ref>), we may now write 𝐬̈_ij(t) projected on to the contact basis (Definition <ref>) as
	
    𝐬̈_ij(t) =    𝐮_j(t) - 𝐮_i(t) = -𝐋_ij^v(t) + m_ij(t)𝐬_ij(t) 
    
    	=    -𝐋_ij^v(t) ·[ p̂_ij(t); q̂_ij(t) ] + m_ij(t) ·[ 2R;  0 ],

	where m_ij(t) = μ_ij(t) + μ_ji(t), and 𝐋_ij^x(t) = λ_j^x(t) - λ_i^x(t). Next we substitute (<ref>) and (<ref>) into (<ref>) and rewrite it as a system of scalar equations,
	
    𝐋_ij^v(t)·p̂_ij(t)    = a_ij^2(t)/2R + 2R  m_ij(t), 
    𝐋_ij^v(t)·q̂_ij(t)    = -ȧ_ij(t).

	Taking a time derivative yields
	
    𝐋̇_ij^v(t)·p̂_ij(t) + 𝐋_ij^v(t)·ṗ̂̇_ij(t)    = a_ij(t)ȧ_ij(t)/R + 2R ṁ_ij(t), 
    𝐋̇_ij^v(t)·q̂_ij(t) + 𝐋_ij^v(t)·q̇̂̇_ij(t)    = -ä_ij(t).

	Then we substitute (<ref>), (<ref>), and (<ref>) into (<ref>) and (<ref>) which simplifies to
	
    𝐋_ij^p(t)·p̂_ij(t)    = -2R ṁ_ij(t) - 3/2Ra_ij(t)ȧ_ij(t), 
    
            𝐋_ij^p(t)·q̂_ij(t)    = ä_ij(t) - a_ij^3(t)/4R^2.

	
    
    Repeating this process of deriving and substituting on (<ref>) and (<ref>) yields a pair of coupled nonlinear second-order ordinary differential equations,
    
    4a_ij(t)ä_ij(t)/2R + 3ȧ^2_ij(t)/2R   + 2Rm̈_ij(t) = a_ij^4(t)/8R^3
         + m_ij(t)a_ij^2(t)/2R, 
    
        m_ij(t)ȧ_ij(t) + ṁ_ij(t) a_ij(t)    + 6a_ij^2(t)ȧ_ij(t)/4R^2 = ⃛a_ij(t).

    Thus, for any constrained trajectory to be energy-optimal it must be a solution of (<ref>) and (<ref>) while also satisfying the boundary conditions (<ref>) and (<ref>).
    In general this is difficult, as both equations are nonlinear and (<ref>) is third order.
    
    An alternative solution is to impose a_ij(t) = 0 over any nonzero interval where the safety constraint is active. This implies 𝐬̇_ij(t) = 𝐯_j(t) - 𝐯_i(t) = 0. Additionally, as 𝐬̇_ij(t) is constant, its derivative 𝐬̈_ij(t) = 0. Thus we may select 𝐯_i(t) = 𝐯_j(t) and 𝐮_i(t) = 𝐮_j(t) as a “reigning optimal” solution <cit.>.
    
    
	
	    
	    
	
	
	
	
	

	
	
	
	
	
	
	
	
	
	
	
	
	To generate a final energy-optimal trajectory, each agent will piece together unconstrained and constrained arcs during each trajectory update. In the following section, we present the decentralized strategy used by each agent to generate collision-free by piecing together energy-optimal motion primitives.
	
	

 §.§ Decentralized Trajectory Generation
 
	
	To generate the trajectory of each agent we will use the following definition of contact intervals.
	 
		For each agent i∈𝒜 we define g∈ℕ contact intervals indexed by k = 1, 2, ..., g. These g intervals must satisfy
		
    τ_i^k    ⊂ [t_0, t_f]  such that 
    𝒱_i(t_a)    = 𝒱_i(t_b) ≠∅  ∀  t_a, t_b ∈τ_i^k,

		where for any two intervals p,q∈ℕ,   p≠ q, we have t_p < t_q for t_p∈τ_i^p and t_q∈τ_i^q. The contact intervals correspond to the nonzero and non-overlapping intervals of time where 𝒱_i(t) is invariant.
	
	The contact intervals correspond to the instances in time when each agent i∈𝒜 would violate the safety constraint when traveling along an unconstrained trajectory. To guarantee safety, we enforce the centralized solution to the safety-constrained case over these intervals. We then piece these constraints together with the initial and final conditions using unconstrained trajectory segments. Each agent i∈𝒜 performs the following steps simultaneously:
	
		
  * Generate an unconstrained trajectory for t∈[t_0, t_f].
		
  * Exchange unconstrained trajectories with all j∈𝒩_i(t).
		
  * Calculate 𝒱_i(t).
		
  * Generate a constrained arc for every contact interval (Definition <ref>) and fix the entry and exit states for each interval.
		
  * Piece together all constrained arcs using unconstrained trajectories and continuity in 𝐱_i at the boundaries.
		
  * Generate initial and final unconstrained arcs to satisfy 𝐱_i(t_0) and 𝐱_i(t_f).
		
  * Generate escape arcs, i.e., agent i may exit to an unconstrained trajectory early if it does not violate any safety constraints.
	
	The above steps will be performed by all agents simultaneously and repeated with a period of Δ T per our control framework. 
	
	
	
	
	
	
	
	
	
	
	
	
	
	
	
	
	
	
	
	
	
	
	
	
	
	
	

	
	
	
	
	
	
	
	
	
	
	
	
	
	

§ SIMULATION RESULTS
 
	
	To validate our decentralized controller we developed two sets of simulations. First, we implemented the controller in MATLAB where the unconstrained and distance-constrained trajectories could be validated on double integrator agents.
	Next, the controller was applied to a set of AscTec quadrotors in Gazebo. The results of this simulation show that our optimal controller generates high-level trajectories which display emergent flocking behavior. These trajectories are also realizable by the dynamics and low-level flight controller of a commercially available quadrotor.
	
	
	





	














    

   
    First, we validated our proposed controller by placing 12 agents at feasible initial points randomly within the domain in MATLAB. The resultant flocking behavior if presented in Fig. <ref>; this shows that after a short transient period, a stable flock is formed which moves to the northeast as specified by 𝐯_d.
    
    
	
	
	
	









	Next, we implemented a sensing-based approximation of the derived optimal control algorithm in Gazebo. This approximation does sacrifice optimality; however, it does not require communication between the agents and it allows an explicit closed-form solution of the boundary conditions. The sensing-based approach for agent i∈𝒜 approximates (<ref>) as
	
    𝐩_i(t_f) =    ∑_j∈𝒩_i𝐩_j(t_0) + 𝐚_i/2w_3∑_j(||𝐬_ij(t_0)|| - D),

	where 𝐩_j(t_0),   j∈𝒩_i(t_0) must only be sensed by agent i. The expected flocking behavior still emerges and is presented in Fig. <ref>.
    We implemented our controller in Gazebo using the RotorS package <cit.> and six AscTec Hummingbirds operating in a horizontal plane. The parameters used for this simulation were: D = 0.25 m, 𝐯_d = (2.5, 0) m/s, and h = 4.5 m. The results of these simulations are visualized in Figs. <ref> and <ref>.
     In Fig. <ref>, agents initially coalesce into a hexagonal pattern before flocking along the direction of 𝐯_d (+x). The flock naturally forms a hexagonal near the end of the simulation due to the relative distance term in the cost function. Figure <ref> shows the energy consumption of the agents decaying despite the use of the sensing approximation to estimate neighbor trajectories.
    

	

	

    The Gazebo simulation shows an asymptotic reduction in energy consumption for agents with significantly more complicated dynamics, i.e., drones. Additionally, the sensing-only approximation (<ref>) does not seem to have a significant impact on the global structure of the flock and rate of energy consumption in this scenario.


    

    

    

    



    

§ CONCLUSION
 
    
    In this paper, we proposed an optimal control approach to realize flocking behavior in a group of cooperative agents. We presented the optimal trajectory in the form of a boundary value problem and provided a candidate solution which is locally optimal. 
    Some potential directions for future research include extending the obstacle avoidance constraint to include terrain and mobile obstacles, redefining the aggregation function to use a local description of flocking, and testing the optimal control algorithm on physical hardware in the University of Delaware's Scaled Smart City.
	
	

§ ACKNOWLEDGMENTS

	The authors would like to thank Michael Dorothy at Combat Capabilities Development Command, Army Research Laboratory, for his insightful discussions on optimal control.
	
    IEEEtran
    
	
	
	
