AIAA Guidance, Navigation and Control Conference and Exhibit AIAA 2008-7134

18 - 21 August 2008, Honolulu, Hawaii

Collision-free Multi-UAV Optimal Path Planning and

Cooperative Control for Tactical Applications

Kevin P. Bollino*
Naval Postgraduate School, Monterey, CA 93943

and

L. Ryan Lewis†
U.S. Navy, Washington D.C. 20003

Stemming from previous work that addressed the optimal path planning of an unmanned
aerial vehicle (UAV) in obstacle-rich environments, this paper demonstrates the approach’s
scalability to that of a multi-UAV application. The proposed concept, based on optimal
control techniques and pseudospectral methods, offers the improved system flexibility and
autonomy demanded by UAV tactical missions in urban areas. As demonstrated, employing
optimal control methods for path planning problems provides a simplistic yet powerful
capability of flight trajectory optimization that includes simultaneous collision avoidance
between vehicles and terrain obstacles. Departing from traditional techniques that harbor
non-optimal architectures, the employed method facilitates real-time, onboard computations
that may potentially improve overall system performance. Recent developments in the field
of optimal control theory point at an emerging paradigm shift that may involve less
dependency on the typical inner-loop control. Extending these developments, this paper
provides not only a fresh perspective, but also illustrates a viable technique for efficiently
generating maneuvering flight trajectories for single vehicles or multiple vehicle sorties.

I. Introduction

PRIMARILY driven by advancements in computer processing power and electronic miniaturization, the
capabilities of unmanned vehicles have made significant progress within the last 10 years. Accelerated by the
National Defense Authorization Act of 20011 (and again in 2006), the United States has since placed significant
emphasis on the research and development of unmanned and autonomous vehicle technologies. The targeted
technology areas include, but are not limited to: mission planning and reasoning, sensing, mapping, recognition,
path planning, trajectory following, and motion control. Although all of these areas may be considered equally
important, this paper focuses mostly on that of path planning for Unmanned Aerial Vehicle (UAV) applications. The
methods demonstrated herein prove viable for future UAV implementation and ultimately providing greater
autonomy with optimum performance. Even if this technology is incorporated into a remotely controlled
(teleoperated) vehicle, greater vehicle autonomy facilitates less human supervision and thereby permitting a
reduction in operational costs – a ubiquitous goal sought by industries around the world.

A. The Need for Autonomy

UAVs are already integral assets to military commanders in the battlefield. Their popularity has inspired
significant research and development of new systems such as the Army’s Future Combat System (FCS) as well as
upgrades to the Air Force’s Predator and Global Hawk. Most UAVs operate at altitudes above terrain obstacles such
as buildings, but this is driven by their large size and remote nature of control. In addition, currently deployed UAVs
operate in mostly teleoperated mode, some semiautonomous leader-follower mode, and some semi-autonomous
route-following mode that can provide manned vehicles with a leader-follower capability. Teleoperation capabilities
enable the warfighter to conduct standoff operations and thereby reduce or remove operator risks in highly stressful

*
Research Associate, Department of Mechanical and Astronautical Engineering, bollinok@verizon.net, AIAA Member.
†
Engineer, llewis@vt.edu.

1
American Institute of Aeronautics and Astronautics
This material is declared a work of the U.S. Government and is not subject to copyright protection in the United States.
and dangerous environments. However, teleoperation capabilities alone do little to reduce operator task loading or
the ratio of operators to platforms. This is partly due to the fact that such missions require (1) meeting total mission
requirements for mobility and communications in advance, and (2) reporting critical new information to the operator
for decision making. Even when part of the operator’s role is transferred to traditional feedback controls that are not
based on optimality considerations, the performance of the system will be limited to well under its true capabilities
as evidently possible from the physics of the problem. Inevitably, unmanned vehicles must not only be capable of
independently moving safely in their environment in order to relieve the operator from the burden of constant
supervision, but also do it in an optimal manner. Furthermore, it is now well established that traditional non-optimal
feedback control laws diminish safety margins. Therefore, it is of imminent need for future UAV platforms to
exhibit not only autonomous capabilities but also optimal performance, mobility, navigation, and mission
accomplishment. To fully realize optimal capabilities, fully autonomous systems must be developed that
demonstrate increasingly tactical human-like behaviors in optimal route planning and execution, obstacle avoidance,
and mission performance. Thus, compared to the Global Hawk class of UAVs, personnel-carried UAVs with
autonomous guidance, navigation, and control (GNC) systems would allow greater combat flexibility and are more
inline with future employment concepts. These future systems will demand the ability to accomplish high-level
goals (strategic level) by automatically generating the required low-level tasking via an autonomous GNC system,
thereby removing the need for extensive human interaction. A driving requirement is to provide the operational
advantage of tactical intelligence without putting pilots or expensive aircraft at risk.
As emphasized by Chun2, current research focuses on “vehicle-centric control,” but there is still a need for
selective and supervised autonomy, necessitated by the requirement to have at least one human in the loop for
override/emergency situations. Although there are different levels of autonomy, the standard notion of autonomy
currently depends too much on pre-determined tactics/behaviors that generate heavy workloads on the vehicle(s) and
the operator(s) and limits overall capability (e.g. unexpected situations, “pop-up” obstacles, loss of link profiles,
time-sensitive targets, defensive tactics, etc.). Therefore, semi-automatic, onboard route planning and re-planning
beyond line-of-sight (BLOS) is critical particularly for missions involving search and recovery, scout
reconnaissance, and target acquisition and tracking. Overall, this capability provides greater adaptability to
unforeseen situations and flexibility for targeting both static and dynamic objects – ideal for tactical missions in
urban environments.
In addition, no single battlefield commander can control the detailed actions of a large number of subordinates,
especially where geographically dispersed. Therefore, an inherent benefit of injecting more autonomy into a single
UAV is that centralized command and decentralized execution is more readily achievable. That is, the ability of a
single UAV to automatically generate the detailed “execution” plans lends itself well to controlling teams of
vehicles from a single command center (i.e. commanding top-level mission goals only).

B. Path Planning Background

Path planning problems have been extensively explored for several decades. In general, path planning problems
are underdetermined and often nonlinear in their truest form. For these reasons, planning methods fall into four
generic categories including: completeness, optimality, computational complexity, and scalability.3-5 Completeness
refers to a method’s ability to guarantee a successful solution if one does exist, and likewise report if no solution is
possible. Optimality specifies the ability of a method to select a path that best fits a predetermined criterion.
Computational complexity provides a relative value for the amount of time a method requires to determine a
solution, and this tends to delineate the method’s ability to be used in an online or offline manner. Scalability is a
method’s portability to different or more dimensionally complex systems. Permissible problem complexity is a
characteristic implicit to completeness and optimality. In a simple example where a method is incapable of handling
a nonlinear system of equations, completeness and optimality designations for that method carry less weight due to
the system’s inaccuracy in problem representation.
In the past, real-life applications of path planning methods led to one last defining characteristic, the ability to
function with local information or the restriction to global information. These terms have often experienced usage in
parallel to the “offline” versus “online” classification, due to a relatively longer computation time. The
distinctiveness of global and local methods has blurred with the progression of modern computing. If an offline,
global information planner can run at a sufficient speed, it can be implemented in a feedback loop to provide an
updated solution as new sensor information becomes available. This concept, exemplified in Ref. [6] with an
autonomous ground vehicle implementation, is the centerpiece of this research. Of course the ability to utilize this
technique is reliant equally upon the idiosyncrasies of the system controlled, the speed of the planner, and the speed
of the computer.

2
American Institute of Aeronautics and Astronautics
 Historically, the use of optimal control techniques in path planning problems was limited due to the
aforementioned reasons of impracticality and uncertainty. The computational complexity, convergence issues, and
burden of an adequate guess made the application of these techniques intractable despite the desirability of the
solutions. Further, the model, sensor, and navigational uncertainty marginalized the benefits in favor of less complex
architectures. The work presented here does not develop a new control technique; rather, it simply makes use of
advances in computing and optimal control algorithms to make this particular application possible.
Recent research7-11 has demonstrated optimal control’s application to online control functions for complex
systems including spacecraft attitude control and atmospheric reentry of a reusable launch vehicle. These papers
have shown the capability to utilize optimal control in a feedback form thereby accounting for disturbances in the
vehicle’s environment and removing the need for a trajectory tracker in the overall control scheme. The obvious
conclusion that can be drawn from these most recent studies is that the method limitations lie not in optimality,
scalability, or completeness, rather it lies in computational complexity and even this argument is proving to be weak.
In general, the use of optimal control methods is the focus of this research not only for the advantages mentioned
above but also the lack of previous exploration in the application of these methods to unmanned vehicle control in
general. Therefore, it is the intent of this paper to advance the understanding of the capability of optimal control
methods and by using a powerful optimization tool, demonstrate the means to employ these methods in real-world
UAV missions.

II. The Optimal Path Planning Approach

Given the progress made in applying optimal control methods to online aerospace applications12 and being aware
of the ability to eliminate the associated computational burden13,14, this paper focuses on applying these techniques
to the endemic tactical UAV problem, sharing attributes with both aerospace and robotics applications. As such, the
research presented herein investigates the concept of trajectory planning for tactical UAVs using optimal control
methods.

A. The Optimal Control Problem

The autonomous UAV path planning example, to follow, is first formulated into a standard optimal control
problem (OCP) definition detailed in Refs [15] and [16], but repeated here, Eq. (1), for completeness.
 τ

Minimize J ( x ( ⋅) , u ( ⋅) ,τ 0 ,τ f ) = E ( x (τ 0 ) , x (τ f ) ,τ 0 ,τ f ) + ∫ F ( x (τ ) , u (τ ) ,τ ) dτ
f

 τ0


Subject to: x (τ ) = f ( x (τ ), u (τ ),τ )
 h L ≤ h ( x (τ ), u (τ ),τ ) ≤ hU
(OCP)  (1)

 (
e L ≤ e x (τ 0 ) , x (τ f ) ,τ 0 ,τ f ≤ e U )

 x L ≤ x (τ ) ≤ xU
 u L ≤ u (τ ) ≤ u U


The general idea of optimal control is to generate an optimum control trajectory u (τ ) that drives the system from
an initial condition [ x (τ 0 ),τ 0 ] to a final condition [ x (τ f ),τ f ] while minimizing (or maximizing) a performance
index, represented as a Bolza cost functional J ( x ( ⋅) , u ( ⋅) ,τ 0 ,τ f ) , subject to constraints. The cost can be
minimized with respect to the states, x , the controls, u , and/or the clock times, τ 0 and τ f , subject to the constraint
equations that include nonlinear dynamics x (τ ) , paths constraints h (⋅) , endpoint conditions e (⋅) , and limits on
states and controls as depicted in Eq. (1).
Once in numerical form, the problem must be scaled and balanced to ensure that the optimization tool will not
suffer from numerical instability. While this particular activity is crucial to a successful solution, details are not
provided within this paper; however, scaling examples can be found in Refs. [15-17] and references therein.
For multi-UAV applications, individual UAVs can be considered components of one unified system that work
together to accomplish the mission objectives. Note that this cooperative behavior does not restrict them to
performing the same tasks as they may “divide and conquer”, yet have an overall synergistic effect. For example,

3
American Institute of Aeronautics and Astronautics
consider a mission requiring a team of UAVs to rapidly acquire and track a suspect time-sensitive target. In order to
effectively accomplish the objective of rapid acquisition, the vehicles would need to get to the known target region
in minimum time without colliding into each other or any obstacles in their path. This scenario is illustrated in Fig.
1. Kaminer et al. demonstrates a similar time-critical mission scenario with multiple UAVs, except that non-optimal
“virtual arc” trajectories are generated that are not necessarily capable of avoiding terrain obstacles.18

Target Region
UAV1

UAV2

UAV3

Figure 1. Multi-UAV Path Planning Concept.

Although they are separate vehicles, each vehicle will require knowledge of the operating environment such as
terrain features, obstacles, and each other (e.g. location or relative position). The successive re-computations of all
vehicles’ optimal trajectories allows for obstacle-avoidance dictated by changes in the trajectory and reconstruction
of new solutions that complete the mission. By considering all system components as one system-of-systems during
successive control trajectory updates, the system will have the capability of autonomously changing other vehicle’s
trajectories and completing the mission in case of vehicle or actuator failures – similar to control allocation but in a
broader sense, vehicle/sortie allocation.

B. The Problem Solver

To solve the nonlinear UAV problem, the optimization tool of choice was DIDO16, a software package based on
pseudospectral (PS) methods19,20 and sequential quadratic programming that runs within the MATLAB environment.
The fundamental premise of the employed PS method is to discretize the problem and approximate the states, co-
states and control variables using Lagrange interpolating polynomials where the unknown coefficient values are
obtained at the Legendre-Gauss-Lobatto (LGL)20 node points. The embedded nonlinear programming (NLP) solver
SNOPT21, based on sequential quadratic programming, then solves a sequence of finite-dimensional optimization
problems.
DIDO has been successfully used for solving a large number of optimal control problems to include recent
demonstrations of a PS-feedback approach that consists of computing real-time solutions for generating closed-loop
performance22,23. In fact, a technique called Pseudospectral Real-Time Optimal Control (PS-RTOC) has been
developed as a prototype for an onboard, real-time algorithm for autonomous guidance and control applications
requiring computational efficiency and guaranteed convergence. 23-25
Being a preliminary feasibility study, robustness is not proven in this paper; however, a low level of uncertainty
is considered in that this work assumes a perfect UAV model but imperfect sensor measurements manifested as
inherent numerical errors. Again, the primary thrust of this work is to provide a simple demonstration of employing
PS-methods for the optimization of kinematic trajectories for multiple UAVs within an obstacle-rich environment
representative of an urban city. The following section defines the UAV kinematic path-planning problem within the
framework of an OCP.

4
American Institute of Aeronautics and Astronautics
 III. Problem Formulation

A. Kinematic UAV Model

The UAV problem formulation used here includes simple flight kinematics. This, in part, reflects the fact that
future tactical UAVs may not necessarily resemble contemporary airplane designs and therefore have complex,
unconventional dynamics. In addition, although vehicle dynamics and other practical limitations have yet to be
incorporated in this study, based on previous related work26-29, there is no reason to believe that this approach will
not scale to the larger problem and future work intends to prove this.
As shown in Fig. 2, the coordinate system for the UAV kinematics is represented by the standard flight path
angles in the local horizontal, local vertical (LHLV) reference frame. There are three state variables and three
control variables. The state variables represent the three spatial degrees of freedom. The three control variables are
the vehicle velocity,ν , the flight path angle, γ , and the heading angle, ξ . The flight path angle is measured from
the local horizontal to the velocity vector, and the heading angle is measured from a reference heading (e.g. due
North) to the velocity vector.

Local Horizontal
Heading
γ Reference

V
ξ V

Figure 2. UAV Kinematic Diagram.

By developing optimal kinematic trajectories, a conventional inner-loop tracking controller could follow the
generated trajectories. However, recent work has indicated that inner-loop tracking control may not be preferable
and that the best choice may employ a single, integrated G&C loop as discussed in Ref. [26]. Note that details of this
integrated G&C concept are not modeled in this work, but it will be the focus of future studies along with the
incorporation of dynamical equations and eventually a higher fidelity environmental model. After presenting the
kinematic and path constraints, the OCP can be stated mathematically as in Eq. (1).
Conceptually, the problem is defined as the generation of a time-optimal trajectory that enables the UAV (or
UAVs as discussed later) to travel from some initial condition to some final condition while satisfying the kinematic
constraints and path constraints. Before proceeding, it is important to remember that the OCP is solved with a
numerical optimization tool and for that reason good scaling is necessary.15-17
Mathematically, the problem variables and general OCP formulation are presented in an unscaled form as shown
in Equation Sets (2) and (3), respectively.

 x(t )   v(t )   vmin ≤ v(t ) ≤ vmax 

 
x =  y (t )  ∈ R 3 ;
  
u = γ (t )  ∈U = γ min ≤ γ (t ) ≤ γ max  (2)
 z (t )  ξ (t )   ξ (t ) ∈ R 
 

5
American Institute of Aeronautics and Astronautics
 Minimize J  x(⋅), u(⋅), t f  = t f
  
Subject to:

  v(t ) ⋅ cos γ (t ) ⋅ cos ξ (t ) 
 x =  v(t ) ⋅ cos γ (t ) ⋅ sin ξ (t )  ;
UAV OCP  (3)
 v(t ) ⋅ sin γ (t ) 

 xT (to ) = [ xi , y i , z i ] , to = 0

 xT (t f ) = [ x f , y f , z f ]

 hi ( x(t ), y (t ), z (t ) ) ≥ 0

The OCP given by Eq. (3), specifies, in order of presentation, the cost function, the kinematic constraints, the
starting position, the ending position, and the path constraints. As this study is designed to be a proof of concept, the
kinematic constraints are simplistic and the cost function considers only the final time; several of the studies
mentioned previously have shown success with considerably more complex problems. The starting and ending
conditions are specified later, and the derivation of path constraints is detailed in the next section. Finally, as
mentioned previously, the only activity not shown here is the scaling and balancing of the OCP.

B. The Multi-UAV System

Extending the previous formulation of the simple kinematic UAV model, additional vehicles can be added via a
state-space system-of-systems representation.

 x UAV1 
   xUAVn 
 x UAV2  N
 
xsys =  ∈R ; x UAVn =  yUAVn  ∈ R 3 ; (4)
   
 x UAVn   zUAVn 

Likewise, the controls can be represented as

 u UAV1 
   vUAVn   vmin ≤ v(t ) ≤ vmax 
u UAV2     
usys = N
∈R ; u UAVn = γ UAVn  ∈U 3 = γ min ≤ γ (t ) ≤ γ max  (5)
     ξ (t ) ∈ R 
u UAVn  ξ UAVn   

For a multiple UAV system with similar mission goals, the only additional requirement is that the vehicles do
not collide. The system-of-systems modeling approach described by Eqs. (4) and (5) conveniently enables collision-
free paths via the addition of a simple path constraint – that the UAVs cannot occupy the same position at the same
time for any given time such that

x UAV1 (ti ) ≠ x UAV2 (ti ), ∀ti ∈ [t0 , t f ] (6)

Mathematically, this is interpreted as the UAVs not having the same Cartesian position vector, R , at any given
time. Or more appropriately, this can be modeled as an inequality constraint on the position of each UAV by a
vector difference such that

R UAV1 (ti ) − R UAV2 (ti ) > 0, ∀ti ∈ [t0 , t f ] (7)

6
American Institute of Aeronautics and Astronautics
 In addition to the constraint on UAV position for collision avoidance, the UAVs should maintain some nominal
spacing as dictated by missions requiring cooperative efforts. For example, Fig. 3 illustrates how the relative
position of two UAVs could be bounded while still enabling minimum time-to-target.

Target Region
xUAV1 (t f )
xUAV2 (t f )
xUAV1 (t0 )

xUAV2 (t0 )

Figure 3. Multi-UAV Path Constraint Concept.

Figure 4 provides a notional spacing constraint concept. Note that although only two UAVs are depicted in the
figure, the concept can be extended to any number by sequentially spacing from one UAV to the next. This
presumes that the UAVs know each other’s position which would allow them to select their closest neighbor for
spacing determination and maintenance.

rmax r (ti ) r (ti+1 ) path1

r (ti+2 )
rmin
path2

Figure 4. Multi-UAV Spacing Constraint Concept.

For the spacing constraint depicted in Fig. 4, the distance between the two UAVs is bounded by some predetermined
minimum proximity rmin (perhaps based on a collision safety buffer, maneuver agility, worst-case turbulence, etc)
and a maximum separation distance rmax (perhaps dictated by a nominal communication range). This bound is
simply modeled as

rmin ≤ r (t ) ≤ rmax , ∀t ∈ [t0 +δ t , t f ] (8)

where r (t ) is the Euclidean distance between the two UAVs at each time instance and given as

r (t ) = ( xUAV2 − xUAV1 ) 2 + ( yUAV2 − yUAV1 ) 2 + ( zUAV2 − zUAV1 ) 2 (9)

Allowing the separation range to vary within the prescribed bounds allows some flexibility for UAV maneuvering
which may still be required for obstacle avoidance. Additionally, this spacing constraint, Eq. (8), simultaneously
serves the purpose of Eq. (6) or (7) – restricting the UAVs from occupying the same position at the same time. The
only foreseeable disadvantage this may cause is related to conflicting constraints. In the case where a single vehicle

7
American Institute of Aeronautics and Astronautics
cannot maneuver around an obstacle without violating the spacing constraint, then the path planning algorithm will
have to compensate by cooperatively maneuvering both vehicles to avoid the obstacle. This will ultimately reduce
the time-to-target unless the vehicles were not limited to a specific separation distance.

C. Note on Architecture
The system-of-systems modeling approach, represented by Eqs. (4) and (5) in Sec. B, lends itself well for an
architecture involving an offboard computational engine such as an in-theater, high-altitude platform responsible for
supervising (monitoring and high-level decision making and commanding) the tactical UAV operations. As such,
this so called “commander” can perform the aforementioned path-planning computations and send waypoints via a
communications link to its “subordinates.” This may facilitate the use of less sophisticated waypoint guidance and
tracking control algorithms; hence, relaxing requirements on the individual UAV’s processor capabilities.
Although not a focus of this paper, a notional architecture is depicted in Fig. 5. As shown, a separate platform
(i.e. commander) with an onboard computational engine (CE) maintains near-real time continuous communications
with the fleet of UAVs (i.e. subordinates) via their general purpose computers (GPC).

x UAV1 x UAV N
,
, ...
, x UA
V2

x UAV1

Figure 5. Notional System-of-Systems Network Architecture.

In Fig. 5, xUAV1 represents the feedback of UAV-1’s measured position and velocity data, or any other pertinent
mission data as needed by the commander. Note that in this architecture the commander may have global knowledge
while the subordinates have only local knowledge. Therefore, depending on the system requirements, it may be that
the UAVs do not need to know information about each other. Furthermore, if communication directly between
UAVs is not needed, this could potentially relax the maximum spacing constraint, rmax , given in Eq. (8) and allow
more maneuvering flexibility. However, other communications issues must still be addressed with this type of
architecture to include, at the very least, required signal strength/range, throughput, and latency.

D. Obstacles as Path Constraints

Path constraints for the UAV problem can entail several aspects of the control problem including: vehicle
kinematic (or dynamic) limitations, flight profile requirements, and physical obstacles. This section discusses the
techniques used to model the physical obstacles the UAV must avoid while moving towards a target in a simulated
“downtown” city environment.
While several approaches exist for algebraically modeling physical obstacles, the technique used here was
implemented due to its simplicity. By means of a p-norm, Eq. (10), simple shapes such as squares, diamonds,
circles, rectangles and ellipses can be modeled efficiently. For example, in Eq. (10), if p = 2 , then the resultant is a
sphere; however, if p = ∞ (or p 0 ) the resultant is a square prism.

8
American Institute of Aeronautics and Astronautics
 p p p
 x(t ) − xc   y (t ) − yc   z (t ) − zc 
hi ( x(t ), y (t ), z (t ) ) =   +  +  −d
p
(10)
 a   b   c 

Equation (10) maps the current location of the UAV to a scalar value whose sign describes whether the UAV is
inside or outside the obstacle.
Unfortunately, problems arise during closed-loop, path constraint modeling of physical obstacles due to the
numerical uncertainty in position. This numerical uncertainty is due to round-off errors, propagation errors,
interpolation errors, etc., and it can result in the vehicle’s position within an obstacle – an infeasible location which
is a product of numerical simulation. Examples of this adverse behavior are clearly illustrated in Ref. [27]. To
prevent this, a distance-dependant buffer is added to the exterior of the obstacle during simulation. While the buffer
is an artificial fabrication, its similarity to actual control scenarios is realistic as the uncertainty on actual obstacle
size, location, and shape is greater as distance from the obstacle increases. Figure 6 gives a conceptual diagram of
how a buffer would work in two dimensions. Like the effects of numerical uncertainty, more details on this buffer
technique are given in Ref. [27].

Obstacle
with
Actual Buffer
Obstacle
Vehicle
⊗
dbuffer Position 2
di
Vehicle
⊗ Position 1

Figure 6. Range-Dependent Buffer Concept.

For this work, an exponent-based buffer, Eq. (11), was used to ensure that the buffer did not greatly affect optimal
control solutions at great distances; however, a non-smooth function could have been adopted to treat the near and
far situations separately.

1
dbuffer = a1 ( di + a2 ) a3
+ a4 (11)

In Eq. (11), dbuffer is the buffer size from the edge of the obstacle; di is the distance of the vehicle from the
obstacle; and a1 through a4 are all constants affecting the buffer characteristics. Ideally, this same buffer logic could
be implemented as a spacing constraint for the case of multiple vehicles, but as mentioned, a Euclidean distance
serves multiple purposes.

IV. Solving the Optimal Control Problem

Proving that the results of an OCP are indeed globally optimal is intractable, but several criteria can be
demonstrated as evidence for the extremal nature of the solution. The criteria used here include satisfaction of the
necessary optimality conditions and verification of solution feasibility. Feasibility is verified by propagating the
system kinematics from the initial condition using the calculated optimal control trajectory and MATLAB’s Runge-
Kutta algorithm (i.e. “ode45”). The necessary conditions for optimality, presented below, were verified, where
possible, against the solution received from DIDO.
By creating the control Hamiltonian, the OCP is rewritten as Equation Set (12).

9
American Institute of Aeronautics and Astronautics
 Minimize H ( λ , x,u, t ) = λx v cos ( γ ) cos (ξ ) +
λ y v cos ( γ ) sin (ξ ) + λz v sin ( γ ) (12)
 vmin ≤ v ( t ) ≤ vmax 
 
Subject to: u ∈ U = γ min ≤ γ ( t ) ≤ γ max  ; hi ( x, y, z ) ≥ 0
 ξ ∈R 
 

where λ T = λx , λ y , λz  are the Lagrange multipliers.

The Lagrangian of the Hamiltonian, Eq. (13), incorporates the control and path constraints.

H ( λ, x,u, t ) = λx v cos ( γ ) cos (ξ ) + λ y v cos ( γ ) sin (ξ ) +

(13)
λz v sin ( γ ) + µv v + µγ γ + µξ ξ + µ h ,i hi + …

Again, additional details of the derivations can be found in Ref. [27]. The Karush-Kuhn-Tucker (KKT) conditions
for the Hamiltonian minimization condition are given in Equation Set (14)

 λx cos ( γ ) cos (ξ ) + λ y cos ( γ ) sin (ξ ) + λz sin ( γ ) + µv 

∂H  
= 0 =  −λx v sin ( γ ) cos (ξ ) − λ y v sin ( γ ) sin (ξ ) + λz v cos ( γ ) + µγ 
∂u 

−
λx v cos ( γ ) sin (ξ ) + λ y v cos ( γ ) cos (ξ ) 

 ≤ 0 for v(t ) = vmin

µv  = 0 for vmin < v(t ) < vmax
≥ 0 for v(t ) = v (14)
 max

 ≤ 0 for γ (t ) = γ min

µγ  = 0 for γ min < γ (t ) < γ max
≥ 0 for γ (t ) = γ
 max

≤ 0 for h ( x, y , z ) = 0
µ h ,i 
= 0 for h ( x, y , z ) > 0
The UAV will most often fly at a non-maximal flight path angle; therefore, µγ is expected to equal zero
throughout most trajectories. On the contrary, the maximum velocity is expected throughout the trajectory for the
time-optimal problem formulation, and this should force µv to be greater than zero. Equation (15) shows the
derivation of the adjoint equations, and this in conjunction with the KKT conditions implies that the costates remain
constant unless the trajectory touches an obstacle.

 ∂hi 
 µ h ,i ∂x
+ …
 λx   
∂H    ∂hi 
λ=− = λ y = µ h ,i + … (15)
∂x    ∂y
 λz   
   ∂hi 
 µh ,i ∂z
+ …


The terminal transversality condition,

10
American Institute of Aeronautics and Astronautics
 ∂E
λ (t f ) = (16)
∂x f
( ) ( ) (
where E ν, x f , t f = E x f , t f + ν T e x f , t f ) and ν T
= ν x ν y ν z  adds no new boundary condition information,
but the Hamiltonian value condition,

∂E
H (t f ) + =0 ⇒ H ( t f ) = −1 (17)
∂t f

and the Hamiltonian evolution equation,

∂H
H= ⇒ H =0 (18)
∂t

combine to generate the Hamiltonian constraint presented as

H ( µ, λ, x,u, t ) = −1 (19)

Representative of the time-optimal problem, Eq. (19) indicates that the Hamiltonian must be a constant negative one
throughout the trajectory. This is a useful check for the numerical results as DIDO provides costate and Hamiltonian
information. Although not presented here in the interest of brevity, the same process can be applied to the multi-
UAV problem formulation.

V. Numerical Results
The following results demonstrate the simplicity and flexibility of the proposed approach by spanning a range of
applications: open and closed-loop path planning, dynamic replanning, obstacle avoidance, and collision-free multi-
UAV path planning. Only the starting and final configurations, local workspace obstacles at each instant, and
vehicle constraints were known to the optimal trajectory generation engine. The path constraints, as previously
explained, were modeled such that an obstacle of any shape or size can be accommodated. All of the results
presented below were generated using DIDO v2003e on a 1.60 GHz Intel Pentium M computer running Windows
XP and MATLAB R14.

A. Single-UAV Path Planning

As this paper is an extension of previous work30, some results are repeated here for completeness and to reiterate
the multi-purpose flexibility of the approach. Here it is assumed that the objective of the UAV problem is to plan a
time-optimal trajectory from the specified initial starting position to the designated time-sensitive target without
colliding with any obstacles. Furthermore, it is assumed that the complexity of this mission is only driven by the
obstacle-rich environment that the vehicle must circumnavigate in minimum time.
Consistent with Ref. [30], the initial and final conditions for the problem were

xi = [ 0 0 2.5] ; x f = [10 10 1]
T T
(20)

Figure 7 shows the DIDO-generated, time-optimal UAV trajectory for both open-loop and closed-loop runs.

11
American Institute of Aeronautics and Astronautics
 Start

Finish

Figure 7. Open-Loop and Closed-Loop UAV Kinematic Trajectories.

Closed-loop performance is generated by solving the open-loop problem in succession according to the supporting
PS-feedback theory presented in Ref. [23]. In general, closed-loop performance is improved over typical open-loop
performance due to the use of a previous near-optimal solution as the solution guess for the subsequent problem
iteration in accordance with Bellman’s principle of optimality. This is a manifestation of the fact that the new
Caratheodory- π trajectory is more accurate as a result of higher nodes.23
As expected, the cost (i.e. time to target) improves for the closed-loop solution (i.e. PS-feedback) – decreasing
from 14.94 seconds to 14.82 seconds. In accordance with theory, this is a benefit when realized by the PS-feedback
approach. As detailed in Ref. [30], approximately 76% of the computation time is a result of handling the path
constraints; therefore, it is evident that the obstacle models cause the largest burden on the optimization scheme. In
order to remove this burden from complicating the feasibility study, the successive clock speed was artificially fixed
to 0.5 seconds. The real mean computational feedback time was 2.35 seconds. Note that the closed-loop maneuver is
slightly different than the open-loop maneuver since state information is not known perfectly, but only within sensor
error tolerances. It is also important to note that the closed-loop maneuver is not tracking a trajectory, but rather a re-
generated series of open-loop solutions. This facilitates the ability to replan a flight path “on-the-fly” while
accounting for any changes in the environment (e.g. moving target, new obstacle, etc) or the vehicle (e.g. external
disturbance, control failure, etc).
In addition, without explicit constraints on control rates, it is possible to get rapid oscillations and sometimes
instantaneous controls. This can be remedied by the standard practice of including control rate constraints as well as
to employ smoothing filters to aid practical implementation. Again, for more details on these results, to include a
demonstration of feasibility and the verification of necessary conditions, refer to Ref. [30].

B. Automatic Route Re-Planning

This example illustrates the potential of using PS-feedback for dynamic replanning. The capability to
autonomously and rapidly replan and adapt to new objectives is particularly important in tactical operations due to
the often rapidly changing environment. The PS-feedback method has the inherent capability to recompute a new
flight trajectory “on-the-fly” in order to achieve a new mission objective (e.g. original target changes location or a
new target is specified while enroute) or perhaps even to help mitigate vulnerability to an enemy attack. As such, the
presented scenario is a simplified example of a new high-priority “pop-up” target that the UAV must plan to
intercept while approximately midcourse to a previously assigned target destination. As shown in Fig. 8, the UAV
successfully replans a time-optimal trajectory to the new target indicated by the blue circles. Note that if the area of
operations is expanded, additional known obstacle information must be provided as this work assumes a priori
knowledge of all obstacle geometries and locations.

12
American Institute of Aeronautics and Astronautics
 Start

Finish

Figure 8. Example of Midcourse Route Re-Planning.

Also, in general, the computational time for a new path plan is comparable to the original path generation for the
particular Bellman segment where the conditions change. However, it is possible that there may be noticeable
differences in computational time depending on how drastically the new objective changes as the previous solution
may be a poor guess to the new path generation. It is also obvious that this simplistic “proof-of-concept” problem,
lacking a mass rate model, will never result in an infeasible trajectory due to restrictions on endurance and range
(e.g. inadequate fuel availability). So, it is possible that a feasible solution does not exist for the replanning
conditions.

C. Collision-Free Multi-UAV Path Planning

For a multiple UAV system with similar mission objectives, the only additional requirement is that the vehicles
do not collide. Figure 9 shows the collision-free trajectories of two UAVs and their corresponding controls. Their
initial and final conditions are not collocated; however, they are within a prescribed region. These subtle differences
in conditions are the only factors contributing to the different time-optimal trajectories. Note that the controls are
plotted with scaled time and velocity.

Y
X

Figure 9. Two Collision-Free UAV Trajectories with No Spacing Constraint.

If it is required that the vehicles maintain a certain distance from one another, perhaps for communication
purposes, than this is easily accommodated by imposing the distance relationship constraint as presented in Eq. (8).
Figure 10 shows the resulting trajectories for the open-loop multi-UAV trajectories with an imposed spacing
constraint, 0.2 ≤ r (t ) ≤ 1.0 .

13
American Institute of Aeronautics and Astronautics
 Y
X
Figure 10. Two Collision-Free UAV Trajectories with Spacing Constraint.

Notice that there is a tighter grouping at the peak of the trajectory (+ Z axis), over the first green obstacle. Figure 11
plots the spacing distance between the two UAVs with and without the addition of spacing constraints. As shown,
the UAV LOS distances do not violate the spacing constraints except in case #5.

Figure 11. Line-of-Sight Distance between Two UAVs.

The minimum, maximum, and mean values for the five different spacing-constraint cases are given in Table 1. In
accordance with Fig. 11, the only case that violates the spacing constraint is case #5.

Table 1: Comparison of Different Spacing Constraints between UAV-1 and UAV-2.

Case rmin rmax LOSmin LOSmax LOSmean
1 ∞ ∞ 0.5601 1.1380 0.8097
2 0.2 1.0 0.6510 0.8660 0.7569
3 0.2 0.9 0.6435 0.8660 0.7717
4 0.2 0.866 0.6686 0.8922 0.7222
5 0.5 0.866 0.7939 1.0010 0.8898

Note that since the distance between the UAVs at the initial and final time is fixed at 0.8660 (in normalized distance
units) as a consequence of the desired endpoint conditions, the maximum separation distance is also limited to this

14
American Institute of Aeronautics and Astronautics
distance. Thus, it is assumed that the UAVs cannont exceed this distance based on some pre-defined mission
restrictions. Naturally, as the spacing constraint tightens, the separation distance between the UAVs shrinks;
however, based on obstacle location, there may be limitations on squeezing between obstacles based on the
minimum separation constraint between UAVs. Although the solution for case #5 is not reliable for analysis since it
was the result of infeasibility, it appears that it was attempting to use more maneuver space about 7 seconds into the
flight path trajectory. Therefore, this path planning approach proves valuable in exploring candidate missions that
require time-optimal solutions but with specific restrictions on UAV coordinated maneuvers such as tight constraints
on spacing.
Another example, plotted in Fig. 12, restricts the maximum altitude such that the UAVs cannot simply fly over
the buildings. This illustrates the ability to generate candidate flight paths in a more confined region without a
collision.

Y
X

Figure 12. Two Collison-Free UAV Trajectories with Altitude Constraint.

The same approach can be applied to a system of more than two UAVs. For example, Fig. 13 shows the same
problem, but with three UAVs all of which start and end their trajectories within a prescribed distance from each
other.

Figure 13. Three Collision-Free UAV Trajectories with Spacing Constraint.

Note that the initial and final conditions can be arbitrarily selected for each of the UAVs, but it is assumed that in a
real flight operation, for the same mission, they would be launched within a relatively close proximity and be
required to fly to the same target area within some prescribed region.

15
American Institute of Aeronautics and Astronautics
 VI. Conclusion
Based on preliminary results, this paper demonstrated how PS-based optimal control provides the capability of
generating optimal UAV flight-path trajectories suitable for tactical applications including collision-free
autonomous flights of multiple vehicles through cluttered urban environments. The trajectory re-planning problem
exemplifies the viability of this path-planning approach for onboard, autonomous UAV applications that demand
flexibility and a rapid response capability. Although additional work is required in the overall command and
control architecture, the same capability seems feasible for multiple UAV sorties which ultimately provide
another level of mission flexibility.

Acknowledgments
The authors would like to thank members of the research team from the Controls and Optimization Laboratories,
Naval Postgraduate School, to name a few: Prof. Michael Ross, Dr. Pooya Sekhavat, and Dr. Qi Gong, for their
ongoing contributions in developing the theory that supports the application of PS-optimal control to path-planning
for unmanned vehicles.

References
1
U.S. Government, National Defense Authorization Act for Fiscal Year 2001, Public Law, 2001.
2
Chun, W.H., “Intelligent Systems Framework Development for Future Space Exploration,” Oral Presentation, AIAA
Infotech@Aerospace 2007 Conference, CA, May 2007.
3
Zefran, M., Continuous Methods for Motion Planning, pp.1-14, University of Pennsylvania, Dec 1996.
4
Choset, H. et al., Principles of Robot Motion: Theory, Algorithms, and Implementations, pp. 197-267 and 384-385, MIT
Press, 2005.
5
LaValle, S.M. and Kuffner, J.J., “Randomized Kinodynamic Planning,” International Journal of Robotics Research,” Vol.
20, No. 5, May 2001.
6
Hurni, M.A., Sekhavat, P., and Ross, I.M., “Autonomous Trajectory Planning Using Real-Time Information Updates,” Proc.
of the AIAA Guidance, Navigation and Control Conference, AIAA Paper No. 2008-6300, Honolulu, HI, 2008.
7
Sekhavat, P., Fleming A., and Ross, I.M., “Time-Optimal Nonlinear Feedback Control for the NPSAT1 Spacecraft," Proc. of
the 2005 IEEE/ASME International Conference on Advanced Intelligent Mechatronics, AIM 2005, Monterey, CA, Jul 2005.
8
Sekhavat, P., and Ross, I.M., “On Closed-Loop Spacecraft Attitude Maneuvers,” Proc. of the AAS/AIAA Astrodynamics
Specialists Conference, Mackinac Island, MI, 19-23 Aug. 2007.
9
Fleming, A., and Ross, I.M., “Minimum-Time Maneuvering of CMG-Driven Spacecraft,” Proc. of the AAS/AIAA
Astrodynamics Specialists Conference, Mackinac Island, MI, 19-23 Aug. 2007.
10
Bollino, K., Ross, I.M. and Doman, D., “Optimal Nonlinear Feedback Guidance for Reentry Vehicles," Proc. of the AIAA
Guidance, Navigation and Control Conference, AIAA Paper No. 2006-6074, Keystone, CO, 2006.
11
Bollino, K.P., and Ross, I.M., “A pseudospectral feedback method for real-time optimal guidance of reentry vehicles,” Proc.
of the 2007 IEEE American Control Conference, New York, NY, Jul 2007.
12
Gong, Q., Kang, W., Bedrossian, N.S., Fahroo, F., Sekhavat, P., and Bollino, K., “Pseudospectral Optimal Control for
Military and Industrial Applications,” Proc. of the 46th IEEE Conference on Decision and Control, Tutorial Session Paper
#1739, Dec 2007.
13
Ross, I. M. and Fahroo, F., “Issues in the Real-Time Computation of Optimal Control," Mathematical and Computer
Modeling, An International Journal, Vol. 43, Issues 9-10, May 2006, pp.1172-1188. (Special Issue: Optimization and Control for
Military Applications).
14
Ross, I. M. and Fahroo, F., “A Unified Framework for Real-Time Optimal Control," Proc. of the IEEE Conference on
Decision and Control, Maui, December, 2003.
15
Ross, I.M., Control and Optimization: An Introduction to Principles and Applications, Electronic Edition, Naval
Postgraduate School, Monterey, CA, Dec 2005.
16
Ross, I.M., A Beginner's Guide to DIDO: A MATLAB Application Package for Solving Optimal Control Problems, Elissar
Technical Report TR-711, http://www.elissar.biz, 2007.
17
Betts, J.T., Practical Methods for Optimal Control Using Nonlinear Programming, Society for Industrial and Applied
Mechanics (SIAM), Philadelphia, PA, 2001.
18
Kaminer, I., Yakimenko, O., Dohrokhodov, V., Pascoal, A., Hovakimyan, N., Patel, V., Cao, C., and Young. A.,
“Coordinated Path Following for Time-Critical Missions of Multiple UAVs via L1 Adaptive Output Feedback Controllers,”
Proc. of the AIAA Guidance, Navigation and Control Conference and Exhibit, Hilton Head, SC, Aug. 20-23, 2007.
19
Elnagar, J., Kazemi, M. A. and Razzaghi, M., “The Pseudospectral Legendre Method for Discretizing Optimal Control
Problems," IEEE Transactions on Automatic Control, Vol. 40, No. 10, 1995, pp. 1793-1796.
20
Ross, I. M., and Fahroo, F., “Legendre Pseudospectral Approximations of Optimal Control Problems," Lecture Notes in
Control and Information Sciences, Vol. 295, Springer-Verlag, New York, 2003.

16
American Institute of Aeronautics and Astronautics
 21
Gill, P.E., Murray, W., and Saunders, M.A., User’s Guide for SNOPT 5.3: A FORTRAN Package for Large-scale Nonlinear
Programming, Technical Report, Stanford Business Software, Inc., Palo Alto, CA, Jul 1998.
22
Bollino, K, Lewis, L.R., Sekhavat, P., and Ross, I.M., “Pseudospectral Optimal Control: A Clear Road for Autonomous
Intelligent Path Planning,” Proc. of the AIAA Infotech@Aerospace 2007 Conference, CA, May 2007.
23
Ross, I.M., Sekhavat, P., Fleming, A. and Gong, Q., “Optimal Feedback Control: Foundations, Examples and Experimental
Results for a New Approach,” Journal of Guidance, Control, and Dynamics, Vol. 31, No.2, Mar-Apr 2008, pp. 307-321.
24
Yan, H., Ross, I.M., and Alfriend, K.T., “Pseudospectral Feedback Control for Three-Axis Magnetic Attitude Stabilization in
Elliptic Orbits,” Journal of Guidance, Control, and Dynamics, Vol. 30, No.4, Jul-Aug 2007, pp. 1107-1115.
25
Fahroo, F., and Ross, I.M., “Pseudospectral Methods for Infinite-Horizon Optimal Control Problems,” Journal of Guidance,
Control, and Dynamics, Vol. 31, No.4, Jul-Aug 2008, pp. 927-936.
26
Bollino, K., High-Fidelity Real-Time Trajectory Optimization for Reusable Launch Vehicles, Dissertation, Naval
Postgraduate School, Monterey, CA, Dec 2006.
27
Lewis, L.R., Rapid Motion Planning and Autonomous Obstacle Avoidance for Unmanned Vehicles, Master’s Thesis, Naval
Postgraduate School, Monterey, CA, Dec 2006.
28
Geiger, B., Horn, J., Sinsley, G., Ross, J., Long, L., and Niessner, A., “Flight Testing a Real Time Implementation of a UAV
Path Planner Using Direct Collocation,” Proc. of the AIAA Guidance, Navigation and Control Conference and Exhibit, Hilton
Head, SC, Aug. 20-23, 2007.
29
Geiger, B., Horn, J., DeLullo, A., Niessner, A., and Long, L., “Optimal Path Planning of UAVs Using Direct Collocation
with Nonlinear Programming,” Proc. of the AIAA Guidance, Navigation, and Control Conference and Exhibit, Keystone, CO,
Aug. 21-24, 2006.
30
Bollino, K. and Lewis, L.R., “Optimal Path Planning and Control of Tactical Unmanned Aerial Vehicles in Urban
Environments,” Proc. of AUVSI's Unmanned Systems North America 2007 Conference, Washington D.C., Aug 2007.

17
American Institute of Aeronautics and Astronautics
 18
American Institute for Aeronautics and Astronautics