Moto
Moto
1
uncontrolled personal aviation aircraft. For a ability to increase performance by using as much as
military scenario, an intelligent adversarial aircraft possible of the previous plan when computing the
would have an even higher level of uncertainty. All new plan.
of these situations can be thought of as the same
In this paper we present a design for such a
general problem, but with different levels of growth
dynamic, stochastic path planner based on
of uncertainty.
evolutionary algorithmic (EA) techniques. The EA-
One approach to handling environmental based methods have a number of desirable features,
uncertainty is the Markov localization of Fox making them appropriate for this problem. One is
([6],[7]). For evolution-based methods, uncertainty their ability to consider non-linear performance
handling was presented in a simple fashion by metrics, which will allow for joint consideration of
Latourell [8]. A more complete approach for static geometric avoidance, time based dynamics, and
obstacles was presented by Rathbun [9]. vehicle performance limitations. Another desirable
feature is the EA structure of iterative solution
The uncertainty in an obstacle’s location will
improvements over an internal state. This will
typically grow as we try to predict further into the
allow for the condition at the end of one planning
future. Because of that growth, there will come a
cycle to more easily be used as a good starting
point during actuation (flying) of distant parts of a
condition for the next cycle.
planned path when the current (e.g. sensed)
uncertainty of an obstacle location will be smaller In section 2, we present the structure of the
than the uncertainty used in the earlier plan. EA-based path planner that we have chosen. In
Therefore, at some point it is appropriate to re-plan. section 3, we show how the planner deals with the
Taking this to the extreme (plan over 2000s, fly first uncertainty in the obstacle locations and how it
2s, plan over 2000s, fly 2s, ... etc.), we transform deals with the change in that uncertainty over time.
from a static planner into a dynamic planner. In section 4, we show how the results of a previous
planning solution can be incorporated into a re-
By considering this growth of uncertainty with
planning to form a true dynamic planner. In section
time in conjunction with re-planning, we are
5, we present some results of the planner, showing
predicting a motion so that the near term motion
its behavior while flying through a field of moving
will place the vehicle into a location so as to
obstacles whose future motion is uncertain.
maximize the likelihood of success of the far term
motion (e.g. maximize the likelihood that the plan
can be executed without change). This does not Structure of Path Planner
imply that such a planner would take the place of an
We will begin in this section with a description
ultra-fast, rule based, obstacle avoidance safety
of the static form (starting location and time does
system (i.e. TCAS). At low levels of integration,
not change) of an evolutionary algorithm (EA)
the path planner considers a long time horizon to
based method for solving the desired path planning
minimize the likelihood that the avoidance safety
problem. Those who desire a more in-depth
system is used. At higher levels of integration, the
description of general EA-based path search
avoidance safety system might condition its conflict
algorithms or their behaviors should see ([10],[11]).
resolution based on the “outer” loop path planner
downstream recommendations. EA-based path planning algorithms use a
population of solutions and random modifications
For faster updated rates of a dynamic planner,
to those solutions to form a structured, stochastic
performance (speed of response) may become an
search. See Figure 1. The population is seeded
issue. Ideally, the problem being solved is a nearby
randomly. The algorithm is then run in a loop. At
problem to the original planning problem –
each step (or generation), additional paths
potentially admitting a nearby solution (e.g. one
(members) are first added to the population by
that does not require a drastic change in UAV
modifying current members (a mutation) or by
trajectory). A desired feature of a path planner is the
2
Environment
Produce Vehicle
Offspring Capabilities
( mutation )
Evaluate Best
Path Encoding Population Decode Path
( fitness )
Selection Constraints
Goals
3
and the segment joined to are both Several of the mutation mechanisms are
chosen at random. See Figure 3(d). The enabled by a point-to-point-join function. This
solid line is the resultant path. The blue function takes a starting location (position and
segments are the mutated segments. The heading) and determines the parameters for two
red segments are the result of the point- segments that will link to an ending location, while
to-point-join function. meeting the continuity requirements. Without this
function, continuity would not be enforced. An
illustration of the nature of the geometric solution
required to make these joins is shown in Figure 4.
There are two possible solutions, depending on
whether the curvature of the two arc segments is in
(a) Mutate and Propagate the same or opposite directions. For creating a join,
both types are calculated and the one with the
shortest total length is used.
In the left-most figure, the construction begins
with lines emanating from the endpoints of each
path segment, perpendicular to the segment
direction. At a distance R from the segment end
point along these lines would be the join segment
(b) Crossover center points. The two resultant center points must
be a distance 2 R from each other. Solving this
geometrical constraint for R gives the solution.
The common endpoint for each join segment is the
point half way between each center point.
In the right-most figure, the construction
begins with the line connecting the two path
segment endpoints, and the two rays tangent to the
(a) Go to Goal path segment endpoints. Moving a distance A
along one ray and then a distance A along a line
parallel to the connection line gives the possible
common endpoint to the join segments. Doing the
same from the other endpoint with a distance of B
must give the same common point. This
geometrical constraint can be solved for A and B .
The center of the arc is the point of intersection of a
line perpendicular to the connection line through
(a) Mutate and Match the common point, and a line perpendicular to one
of the segment endpoints. This solution is derived
Figure 3. Path Mutation Mechanisms from Kosugi ([12]).
Note that the solution must be constrained by
For each generation, one of the mutation the minimum turn radius of the vehicle. In the
mechanisms chosen at random is applied to each event that either radii for the join is beyond the
path in the population (the original path is retained vehicle capability, the join is considered to have
in the population). The population size is started at failed and a new mutation (chosen at random) is
20 paths, and then mutated up to 40 paths. The applied to the parent path.
broad range of mutation types is included in an
effort to give the path search algorithm an improved
chance of avoiding local minima.
4
continuity and motion constraints are enforced by
the path encoding and mutation mechanisms.
5
the distribution function used to construct the field flight following a known trajectory (perhaps badly),
as defined in equation (3), is given as, a better model might be an uncertainty which
grows, but is limited to some upper value. There are
r 2 / σ 2 r ≤σ many other possible ways to compute future
p(r , σ ) = (6)
1 r >σ uncertainty. While the structure of the EA-based
planner allows for inclusion of nearly any method,
One advantage of the approximation to the
we will not provide any further discussion of
probability of intersection of equation (2) is the ease
possible estimation techniques. For all the
with which it can be extended to consider moving
simulation results presented, the uncertainty
obstacles. It is the form of the solution – a
propagation model is that of equation (11).
summation over a parametrically defined function –
that allows for the simple inclusion of time into the If we can assume that the location of each
equations. Equation (2) simply becomes: obstacle is independent of the location of all other
k
( )
B2 (Pi , O j ) = ∏ β Pi (s k ) − C j (Tk ) , σ j (Tk ) ∆s (7)
obstacles, we can express the total probability of
intersection of a path with one or more obstacles as,
B1 (Pi ) = 1 − ∏ {1 - B 2 (Pi , O j )} (12)
where Tk is the time at which j
Figure 5 through Figure 7 illustrate the
P(s ) = P(s k ) (8) behavior of this static planner (captured via
To compute the obstacle location at a given time, snapshots at three different generations) as it
C (Tk ) , without any other indication of its intent, we considers the motion of the obstacle over time.
Here, the UAV (blue outlined vehicle) is heading to
will use a simple dynamic propagation the right, trying to reach the goal location (circle,
C (Tk ) = C (T0 ) + v (Tk − T0 ) (9) shown in green). The obstacle vehicle is heading
vertically up the page. The dashed circles
If some other indication of vehicle intent is known
correspond to the planner’s estimate of uncertainty
(filed flight plan, expectation of strategy from
at different times – which is seen to grow the
gaming theory, etc.) then that could be used in place
further the planner plans (e.g. as the obstacle
of Equation (9).
vehicle moves further up the page). In this case, if
There are a number of terms that must be the UAV were to travel straight to the goal, it would
considered for propagation of the location exactly collide with the expected center of the
uncertainty, σ (Tk ) . Since the current obstacle moving obstacle.
location and velocity are estimated quantities, there The fitness function for this simulation allows
will be an initial uncertainty in the position. The the UAV to use 7.2kg of fuel before incurring any
uncertainty in the initial velocity will also couple penalty, where the straight line path to the goal
through the position propagation via equation (9). would have required 7.9kg. With such a low penalty
So, for a vehicle with no action to change its on fuel usage, the UAV path planner is
trajectory from a straight line, the minimum conservative, choosing a path that has a minimal
uncertainty propagation becomes encroachment onto the obstacle uncertainty region.
σ (Tk ) = σ (T0 ) + τ 0 (Tk − T0 ) (10) Figure 8 and Figure 9 show the time history of
Additional terms need to be added to account for the motion after the planning has been completed.
action taken by the vehicle (whether intentional or The UAV initially heads to its right to come around
not). A simple model is to assume that the vehicle behind the moving obstacle. It then heads almost
can apply a fixed acceleration, a , in a random straight to the goal, in so doing only entering the
direction. The uncertainty would then grow as uncertainty region of the obstacle by a very small
amount.
a (11)
σ (Tk ) = σ (T0 ) + τ 0 (Tk − T0 ) + (Tk − T0 )2
2
This may be a good model for a hostile aircraft,
whose motion is unpredictable. For a commercial
6
3 3
2 2
1 1
0 0
-1 -1
-2 -2
-3 -3
-1 0 1 2 3 4 5 -1 0 1 2 3 4 5
Figure 5. Static path planning showing the effect Figure 8. Moving through time (time t1)
of moving, uncertain obstacles (at generation 20)
3
3
2
2
1
1
0
0
-1
-1
-2
-2
-3 -3
-1 0 1 2 3 4 5
-1 0 1 2 3 4 5
Figure 6. Static path planning showing the effect Figure 9. Moving through Time (time t2)
of moving, uncertain obstacles (at generation 40)
3 Re-Planning
Since the uncertainty in the location of
2 obstacles in the UAV’s environment grows over
time, after the path has been actuated for a period of
1 time, the measurements of the obstacle locations
will diverge from the predicted locations. This calls
0 for re-planning the vehicle path.
Planning over time requires an iterative
-1
solution with the following steps:
-2 1. Plan path with “best probability” of success
2. Move along path a given (small?) distance
-3
-1 0 1 2 3 4 5
3. Update estimates of obstacle locations and
Figure 7. Static path planning showing the effect motion. Update vehicle locations
of moving, uncertain obstacles (at generation 60)
7
4. Use the previous solution to seed initial parts of the green and blue paths in the
conditions for a new search left-most figure.
5. Re-plan path • Add to the path a join between the end
of S [1]i and the start of the (now
6. go to Step (2)
shorter) path. These are depicted as the
Step (4) is an attempt to save the knowledge gained red segments in the right most figure.
from the previous planning cycle for the next
search. If done well, it has the possibility of
improving the performance for some types of the
search algorithms. This is of particular importance
for EA-based planners, since they must converge to
an “acceptable” (e.g. flyable and collision-free)
solution in a finite time.
For the EA-based path planner, step (4) will be
a mapping of the population from the previous
cycle into a valid population for the next cycle:
Γi → Γi +1 (13)
The objective of the mapping is to create a
population of paths that meet the constraints of the
search algorithm; (a) all start from the same
location (position/heading), and (b) are made from
Figure 10. Reset of Path to New Search Criteria
segments that meet the vehicle performance
constraints – while retaining as much as possible of
the characteristics of the original population; (c) a
high level of fitness, and (d) wide “variability”. We Note that the plan update rate must be on the
will do so with the following steps (see Figure 10): order of that required to fly a given trajectory
segment. This implies a tradeoff between the
1. Choose the path from the population of numbers of segments describing a trajectory (and
iteration i with the best fitness value. This thus freedom and flexibility of possible motions)
is depicted as the black path in the left-most and computational time required to plan the
figure. remaining trajectory.
2. Remove the first segment from that path
S [1]i . This is the segment which the Example EA Simulation
vehicle will travel along while iteration In this section we present the results of
i + 1 is being computed. This is depicted simulations which demonstrate the behavior of the
as the magenta segment at the start of the EA-based path planner defined in this paper.
black path in both figures.
We initialize the simulation with a single UAV
3. The new start point for the population of heading towards its final goal location at a distance
iteration i + 1 is the end point of segment of 4km. There are two other aircraft in the area,
S [1]i . each considered as an obstacle for the UAV. One
obstacle is initially heading roughly parallel to the
4. For all other members of the population UAV, but later turns to cross over the UAVs
• Remove a number of segments from the straight line path to the goal. A second obstacle is
start of the path (equal to the number of heading crosswise to the UAVs path, and later
segments required to form a join plus shifts its path to directly follow the UAVs straight
one). These are depicted as the dashed line path to the goal. The UAV is restricted to
avoid the other aircraft by a distance of 0.3km.
8
The EA-based path planner is initialized with a obstacle locations to grow with time: the level of
set of random paths. It is given an initial 20 that uncertainty must be weighed against other
generations of evolution before time begins. costs. The uncertainty can grow very large, and
Thereafter, it is allowed 20 generations of evolution completely cover all paths to the goal location.
each iteration before it must provide a segment for Because of this, a simple solution of avoiding all
the UAV to fly. possible locations of the obstacle results in a
problem with no solution. The best solutions are
The measurement uncertainty for the location
those that can trade off a small probability of
of the obstacle aircraft is set to 0.1km. The
obstacle intersection against other performance
measurement uncertainty for the velocity is set to
constraints.
5km/s. It is assumed that no knowledge about the
intent of the obstacle is available, and the
uncertainty growth is set to 0.001 km/s/s. 3
center of the circle is derived from the current Figure 12. Simulation State at time T5
location and velocity of the obstacle with random
noise added to simulate estimation error. The
dotted, red lines represent the increase in
uncertainty of the obstacle locations for times past
the end of the planning cycle.
Figure 12 also displays a typical challenge for
a path planner that allows the uncertainty of
9
3 3
2 2
1 1
0 0
-1 -1
-2 -2
-3 -3
-1 0 1 2 3 4 5 6 -1 0 1 2 3 4 5 6
Figure 13. Simulation State at time T7 Figure 16. Simulation State at time T11
3 3
2 2
1 1
0 0
-1 -1
-2 -2
-3 -3
-1 0 1 2 3 4 5 6 -1 0 1 2 3 4 5 6
Figure 14. Simulation State at time T9 Figure 17. Simulation State at time T12
3 3
2 2
1 1
0 0
-1 -1
-2 -2
-3 -3
-1 0 1 2 3 4 5 6 -1 0 1 2 3 4 5 6
Figure 15. Simulation State at time T10 Figure 18. Moving Through Time (T4)
10
3 the future trajectories of the other aircraft. The
planner was able to effectively balance the
2 likelihood of collision against the fuel required to
implement overly cautious avoidance behavior.
1
0 Acknowledgments
This research for this paper was completed
-1 under funding from the DARPA Mixed Initiative
Control of Automa-Teams (MICA) project. The
-2 authors would like to thank both DARPA and our
contracting agency, SPAWAR Systems Center San
-3
-1 0 1 2 3 4 5 6 Diego, CA, (contract N66001-01-C-0089) for their
support that made this work possible.
Figure 19. Moving Through Time (T14)
References
Figure 18 and Figure 19 show the final path
[1]. Khatib, O., 1986, “Real-Time Obstacle
flown by the UAV (without depiction of any future
Avoidance for Manipulator and Mobile
uncertainty of the vehicles) at two different times. Robots, International Journal of Robotics
This is what would have been seen by an outside
Research, Vol. 5, No. 1, pp. 90-98.
observer. The dotted, red circles around the obstacle
vehicles represent the size of the restricted approach [2]. Mitchell, J.S.B., and D.M. Keirsey, 1984,
distance. As can be seen, the UAV successfully “Planning Strategic Paths through Variable
avoids the two obstacle UAVs without any Terrain Data”, Proc. of the SPIE Conference
knowledge of their future intent, while maintaining on Applications of Artificial Intelligence,
all vehicle speed and fuel use constraints. Vol. 485, Arlington, VA, 1984, pp. 172-179.
[3]. Stentz, A., 1994, “Optimal and Efficient Path
Summary and Conclusions Planning for Partially-Known
Environments”, Proc. of the 1994
We have presented a simple framework for
International Conference on Robotics and
path planning using evolution based algorithms,
Automation, Vol. 4, Los Alamitos, CA., pp.
which allows for consideration of uncertainty in
3310-3317.
estimates of the parameters describing obstacles in
the environment, as well as changes in those [4]. Fogel, D.B., and L.J. Fogel, 1990, “Optimal
parameters over time. Routing of Multiple Autonomous
Underwater Vehicles through Evolutionary
We have presented a method for construction
Programming, Proc. of the 1990 Symposium
of a dynamic planner from the static version
on Autonomous Underwater Vehicle
through the addition of a state update function.
Technology, Washington, D.C., pp. 44-47.
The structure of the planner was such that the
[5]. Capozzi, B.J., and J. Vagners, 2001,
path generated was continuous in space and within
“Evolving (Semi)-Autonomous Vehicles”,
the speed and maneuverability constraints of the
Proc. of the 2001 AIAA Guidance,
UAV.
Navigation and Control Conference,
We demonstrated the behavior of the path Montreal, Canada.
planning algorithm by application to scenarios
[6]. Thrun, S., D. Fox, and W. Burgard, S. Thrun
where the UAV was required to modify its path to
and D. Fox and W. Burgard}, 1998, “A
avoid a number of other aircraft flying in its
Probabilistic Approach to Concurrent
vicinity. The UAV was able to successfully avoid
Mapping and Localization for Mobile
all other aircraft without the need for aggressive
avoidance maneuvers or any previous knowledge of
11
Robots”, Machine Learning and Autonomous
Robots, Vol. 31.
[7]. Fox, D., W. Burgard, and S. Thrun, 1999,
“Markov Localization for Mobile Robots in
Dynamic Environments”, Journal of
Artificial Intelligence Research, Vol. 11.
[8]. Latourell, J., B. Wallet, and B. Copeland,
1998, “Genetic Algorithm to Solve
Constrained Routing Problem with
Applications for Cruise Missile Routing “,
SPIE Proceedings, Applications and Science
of Computational Intelligence, Vol. 3390, pp.
490-500.
[9]. Rathbun, D., and B.J. Capozzi, 2002,
“Evolutionary Approaches to Path Planning
through Uncertain Environments”, Proc. of
AIAA's 1st Technical Conference and
Workshop on Unmanned Aerospace Vehicles,
Systems, Technologies, and Operations,
Portsmouth, VA.
[10]. Xiao, J. et al., 1997, “Adaptive Evolutionary
Planner/Navigator for Mobile Robots,” IEEE
Transactions on Evolutionary Computation,
Vol. 1, No. 1, pp. 18-28.
[11]. Capozzi, B.J., 2001, “Evolution-Based Path
Planning and Management for Autonomous
Vehicles”, Ph.D. Thesis, University of
Washington.
[12]. Kosugi, M. and T. Teranishi, 1977,
“Construction of a Curve Segment with Two
Circular Arcs”, Transactions of the IECE of
Japan, section E, vol. E60, No 11, p. 684
12