Optimization Based Refinement Stage Planner for an Autonomous Vehicle

Document Sample
Optimization Based Refinement Stage Planner for an Autonomous Vehicle Powered By Docstoc
					     Optimization-Based Refinement Stage Planner for an
       Autonomous Vehicle in Dynamic Environments

                         Martin O. Larsson, Lund University
                         Faculty Mentor: Richard M. Murray

                                   September 21, 2006

         In November 2007 Caltech intends to participate in the DARPA Urban Chal-
     lenge, a competition where autonomous vehicles are to complete a race in an urban
     environment. A crucial component of an autonomous system is the path planner,
     which generates feasible trajectories for the vehicle to follow. The purpose of this
     project is to draw upon Team Caltechs earlier experience of autonomous driving and
     investigate the possibility of reusing and adapting existing technology to an urban
         The main conclusion is that the approach should not be ruled out, although a
     number of issues need to be addressed. The deterministic nature of the algorithm
     makes the strong dependence on prediction of other vehicles’ motion potentially
     problematic. A separate module would likely be necessary to encode logics, such as
     traffic laws, in terms of costs and constraints in the optimization problem. If the
     approach is used, the implementation should not build on the existing code; the
     lack of documentation and the fact that the code was optimized for the 2005 Grand
     Challenge makes it difficult to extend in a reliable way.

1    Introduction

In 2005, Caltech participated in the DARPA Grand Challenge (DGC) with the objective
to conceive, develop and build an autonomous ground vehicle capable of traversing a 175
mile desert race course in 10 hours. In May 2006 DARPA announced a new competition,
the Urban Challenge, due to take place in November 2007. This time the participating
vehicles must be able not only to traverse a race course, but also avoid moving obstacles,
obey traffic laws and employ defensive driving.
This SURF project is intended as part of an effort to investigate technologies that may
prove useful in extending the functionality of Alice, Team Caltech’s entry in the 2005
DGC. The goal is to enable Alice to function in dynamic environments, where moving
obstacles have to be avoided and traffic laws obeyed. A crucial part of the system is
Alice’s ability to choose an optimal path from point A to point B. The existing path

planner assumes static surroundings, which is clearly not a valid assumption in an urban
environment. The planner therefore has to be modified to account for moving obstacle

Figure 1: In a dynamic environment the path planner must take the moving obstacles
into account when planning a trajectory

This problem – trajectory planning in dynamic environments – is not a new one and
although not entirely solved, a number of different approaches have been proposed.
For instance, Fiorini and Schiller [1] compute a preliminary trajectory by searching a
tree where the nodes correspond to spatial locations and the edges represent avoidance
maneuvers. This trajectory is then refined so as to fulfill additional constraints, such
as actuator constraints. In Hsu et al. [2] the nodes are chosen in a randomized fashion
according to the so called Probabilistic Roadmap Method (PRM), which is useful to
keep computational time reasonably low when the vehicle/robot has many degrees of
Alice’s current trajectory planning module uses another technique, namely an optimization-
based method in a receding horizon framework. This means that the problem is posed as
an optimization problem, where the computed trajectory is chosen so as to minimize a
cost function (typically traversal time) subject to speed limits and the vehicle’s dynam-
ical constraints. Receding horizon means that the planner only produces trajectories
from the vehicle’s present position up to a horizon some distance ahead (see Figure 2).
The approach relies on earlier work by Milam [3], where receding horizon control in air
vehicle maneuvering is implemented on the Caltech Ducted Fan testbed.

2    Objectives and Approach

The goal of this project is to investigate whether the optimization-based, receding horizon
approach is suitable for the new situations Alice will encounter. The general approach

   field. This is inadequate for our purpose,
we want to be able to plan through both                Since the DGC race is over 100 miles long, it is both
  edances, where one could potentially travel,     computationally prohibitive and unnecessary to plan the ve-
d speed. Another well-researched approach to hicle’s trajectory all the way to the finish line at all times. A
  em is a graph search. This is a scheme where good compromise is to run the planner in a receding horizon
esented as a graph with nodes corresponding framework. A receding horizon scenario is one where plans
 ns, and the edges representing to the cost are computed not to the final goal, but to a point a set horizon
 m one location to another. A well-known (spatial or temporal) ahead on some rough estimate of the path
   type is A*. It can plan an optimal path towards the goal (Figure 3).
mapped environment, but this information is
 nown a priori [1]. D* is an extension of                                                      Trackline path
amically and efficiently modify the plan if                  Vehicle
                                                                       Obstacle Target point
n obstacle is encountered [2]. Unfortunately,
 raph is searched, D* and A* are insufficient
uld include no dynamics and would operate
                                                                        Final plan           Sensor range
  A car-like robot includes non-holonomic
 h would render unfollowable many spatial Fig. 3. An illustration of the receding horizon framework. A very inaccurate
  that employ A* to search an extended graph (but2: Receding horizon control (picture taken afrom [4])
                                              Figure easily computable) path is used as the global plan. Here this is spline
  e trajectories exist ( [3], [4]). Unfortunately, based on the trackline. The solver that produces a higher-quality plan performs
                                                   its computation to reach a point on the lower quality path, some distance
oo slow or simplify the problem too much, ahead. In this figure, that distance (the horizon) is set to the range of the
 acles.                                            sensors of the vehicle.
                            was to extend the current software with moving obstacle avoidance capability. In the old
 efforts compute the optimal spatial path, and
                            planner, obstacle avoidance is accounted for through the speed limit constraint in the
e optimal velocity profile that is feasible for a
                             separation of spatial A. Vehicle the
e that path [5], [6]. This optimization problem. Sincemodelenvironment was assumed to be static, the speed limit
                            depended of space only. A straightforward way, at least in principle, of incorporating
nning, however, is a potential source on non-
                                                       As mentioned earlier, the planner is designed to generate
                            moving the fastest
 he shortest path is not necessarilyobstacle is to add time dependency to the speed limit constraint. All this is laid
                                                   trajectories that are dynamically feasible in respect to some
                                                   model of the vehicle. The model chosen solving the problem at hand is thus
                            out in more detail in Appendix A. The approach for here is the kinematic
                             done to find ways to
 e issues, work has been to implement this seemingly simple extension to the existing software.
                                                   bicycle model (Figure 4):.
 ally feasible trajectories around obstacles in
                             database of precom-
 f those algorithms use a A key issue is the interfaces with other relevant modules on Alice, including designing
                                                                                N = v cos θ
                            a computations [7].
  ves to speed up the online suitable representation of the moving obstacles (in collaboration with other SURF
                                                                                E = v sin θ
  the drawback of not necessarily producing                                                                                    (1)
                                                                                 v = toadifferent parts of the functionality were
  d making it difficult toprojects). Also, test scenarios corresponding v
                              specify constraints.                               ˙
                                                                                 θ = L tan φ
                            created with the
  from the discrete nature of the precomputed objective of testing them in simulations and in the field. The perfor-
                                                   where to an E are the Northing and about 5 plans per
                            mance objective was set N and update frequency of Easting coordinates of second, which was
mization method to produce the plans would the middle of the rear axle, v is the scalar speed of the center
                            the rate during the 2005 Grand Challenge.
 ems. An optimization problem could be set of the rear axle, θ is the yaw of the vehicle, a is the scalar
 ptimal plans that also It has not been a part of this project to consider howisto generate a suitable seed, or
                            avoid obstacles and longitudinal acceleration of the vehicle, L                     the wheelbase
                            initial guess, for of the vehicle, This is the the words ”Refinement
 easibility constraints. An optimization-based the optimizer. and φ is whysteering angle. This model isStage” are present
                               temporal planning derived
  ormed the spatial andin the title of this report.by assuming that the two bicycle wheels do not slip on
  sed its spatial plans off D* was attempted the ground, which implies that each wheel is always tangent
                        Figure 3 illustrates the structure of the dynamic planner module and how it relates to the
                        modules with which it communicates. The red box and its subcomponents are described
                        in a more detailed fashion in Appendix B.

                        2.1       The Dynamic Map and Map Accessor

                        The dynamic map (CDynMap) was implemented from scratch1 and contains a vectorized
                        representation of the present state and geometry, i.e. position, velocity, orientation and
                        side lengths (all moving obstacles are assumed to be rectangular). This is the same
                        representation used by the moving obstacle detection and tracking module. The old
                        software uses a grid-based representation of the environment, and to be able to reuse as
                        much as possible of the existing code, CDynMap was equipped with functions converting
                        from the vectorized to a grid-based representation. The result is essentially a binary
                        space-time map, where a zero indicates the presence of a moving obstacle.
                               Part of this work was done by David Bolin

                                      Terrain sensors

                           Moving obstacle
                                                 Dynamic Fusion
                            detection and

                                 Dynamic Planner Module
                              Dynamic Map         Static Map
                               (CDynMap)           (CMap)

         RDDF corridor         Dynamic planner (CDynPlanner)      Vehicle state

                                    Trajectory Follower

Figure 3: The dynamic planner structure and the system framework. C++ class names
are given in parentheses. Moving obstacle detection and tracking as well as the dynamic
map were not present in the old architecture.

In the planner code (MapAccess.h/.cc) the map accessor was extended with calls to
the dynamic map and the subsequent fusion of the static and dynamic maps, simply
by element-wise multiplication. This happens before the interpolation and smoothing
needed to make the speed map differentiable as required by the optimizer (cf. Kogan
[4]), implying that nothing of that had to be re-implemented for the dynamic parts of
the map.

2.2   The Constraint Function

Evaluating the new constraint function involved calling the new map accessor, which
required implementing a function computing the time coordinate at each collocation
point. The computation was carried out by subsequent integration of

                                t(s) = t(0) + Sf                                      (1)
                                                        0       v(τ ),

where t(0) is the time at the start of the trajectory (”now”), Sf is the trajectory length
and v(τ ) is the speed along the trajectory as a function of arc length. This eventually
amounts to evaluating the integral

                                    0       v0 + (v1 − v0 )s

which was done by integrating the fifth order Taylor expansion of the integrand around
s = 0.5 (cf. Kogan [4]).
The computation of the new Jacobian of the constraint function was not implemented
due to the complexity of the previous code and the lack of documentation (see Appendix
A for details), but this doesn’t seem to have had any significant impact on the result.
The numerical solver continually monitors the provided Jacobians and outputs warning
messages if they don’t seem reasonable. No such messages appeared.

2.3   Corridor and Target Point

The old planner uses a so-called RDDF (Route Definition Data File) to specify the
maximum speed and the corridor in which the vehicle must remain. This representation
is not suitable for urban environments, but to be able to test the extended optimizing
algorithm without implementing a lot of peripheral functionality and integrate it with
the planner code, the RDDF format was reused. The trajectory target point is chosen
along the trackline path, as illustrated in Figure 2.

2.4   Computing a Seed

As mentioned above, the problem of finding a suitable seed for the optimizer was not
a part of this project. However, an initial guess still has to be provided to the solver.
Two qualitatively different approaches were tested. The first was simply to use a smooth
curve from the current position to a position sufficiently far ahead along the RDDF. The
curve was generated using the existing class CPath (see the Alice documentation). The
second approach was to seed the subsequent planning cycle with the previously computed
trajectory extended with the CPath curve. See Figure 4 for a schematic illustration of
the two approaches.



Figure 4: Two qualitatively different ways of generating an initial guess for the numerical

3     Results

Testing was mostly performed in simulations, where simple scenarios were executed and
the behavior observed visually. The performance in terms of computation time was
logged during each run. See appendix C for a description of the system setup. The
different scenarios are presented in Table 1, and in Figure 5 subsequent screenshots from
scenario 3 are shown.
The dynamic planner was tested in the field on Alice on one occasion, although with-
out any moving obstacles. The planner worked as expected, but since no moving ob-
stacles were present the initial objective of testing the scenarios in the field was not
In the presence of moving obstacles, the update frequency of the planner was normally
about 0.3 – 0.5 seconds per plan, i.e. 2 – 3 Hz. This does not quite fulfill the objective
of 0.2 seconds per plan, but is still acceptable at moderate speeds. Furthermore, these
numbers come from the lab computers, whereas testing revealed that the planner gen-

Figure 5: Screenshot from simulation of scenario 3. The current implementation of the
graphical user interface causes the other car to leave a trace behind it. The diagrams on
the right illustrate the intended speed profiles along the respective trajectories

 Scenario                          Tested functionality               Result
 1. Drive across an open field      Ability to navigate an empty       The planner avoids the obsta-
 (zone) with no static obstacles   parking lot (or other open         cle. Sometimes indecisive be-
 and another car driving across    area) with another moving ve-      havior; cannot decide whether
 our path                          hicle                              to pass in front of or behind
                                                                      the car.
 2. Drive across an open field      Ability to navigate an empty       In some cases the planner
 (zone) with no static obsta-      parking lot (or other open         avoids both obstacles. How-
 cles and two other cars cross-    area) with two other moving        ever, it relatively often fails to
 ing our path                      vehicles.                          converge.
 3. Drive down a lane behind a     Ability to pass a slow-moving      The planner performs the
 slow-moving car with the cor-     vehicle                            passing maneuver, sometimes
 ridor wide enough to allow for                                       passing on the left side, some-
 a passing maneuver.                                                  times on the right.
 4. Drive down a lane behind       Ability to follow a lead vehicle   The planner is unable to slow
 another car                                                          down and follow the other car.
                                                                      It either fails to converge, or
                                                                      tries to go outside the RDDF.

Table 1: List of scenarios with description, purpose in terms of which functionality is
tested, and outcome from simulations

erally ran faster on Alice’s computers. Also, parts of the algorithm could potentially be
optimized, in particular functions associated with the dynamic map.
Considerable variations in update frequency were observed between different runs. Fig-
ure 6 shows the time for each planning cycle in two different runs of scenario 3. In these
diagrams the computation times span from 0.2 seconds up to more than 1 second per

                      29/-1:-.-,:;1"<1:,((22=2=>1!<13./62=                                18.,09,-,+9:0";09+''11<1<=0!;02-.51<
        #                                                                   #
        "                                                                   "
        !                                                                   !
       !"                                                                  !"
             !   "!           #!            $!            %!          &!         !   "!           #!            $!            %!      &!
                   ()*+,-.-/)01-/*213)412.(51+6.00/071(8(62                            '()*+,-,.(/0,.)102(301-'40*5-//./60'7'51
       !'%                                                                   #
       !'#                                                                   "
         !                                                                   !
             !   "!            #!          $!                %!       &!         !   "!            #!          $!                %!   &!
                          -/*21+241*.+1.((2::1?,*@                                            ,.)10*130)-*0-''1990>+)?
       #!!                                                                 #!!
       "!!                                                                 "!!
         !                                                                   !
             !   "!            #!             $!             %!       &!         !   "!            #!             $!             %!   &!

Figure 6: Logged performance from two different runs of scenario 3. The time per map
access is fairly constant once the moving obstacle appears, but the full computation time
varies considerably. In these examples, the planner almost never failed to converge.

4    Conclusions and Further Work

Most of the time, the planner was able to solve the basic task of computing a trajectory
while taking into account the motion of the other vehicles. However, there are some
situations where it fails completely or exhibits a behavior which by a human would
immediately be taken as unsafe or unpredictable. In particular in scenario 1 the planner
tends to switch rapidly between several radically different trajectories, not ”making up
its mind” until the last minute. Sometimes the planner fails to converge altogether.
By using a better seed and correcting the gradient of the constraint function the risk of
convergence failure or too long computation time can probably be reduced.
The large variations in computation time between different planning cycles are trouble-
some. It is not obvious what causes this, and it adds a very undesirable unpredictability
element to the algorithm.
Perfect knowledge about the future state of moving obstacles is not a realistic assump-
tion. For the algorithm to be useful in practice, the uncertainty in predicted motion of
other vehicles must somehow be accounted for.

When deciding on whether or not to use an optimization-based approach in the Urban
Challenge, there some issues that should be considered:
    • Logics, such as traffic laws, may be difficult to encode. This could for instance be
      solved by having a component upstream of the dynamic planner formulating the
      solver constraints at each planning cycle.
    • Provides that the situation can be encoded properly, the optimization approach is
      a very general way of planning a trajectory. The same framework can thus be used
      in parking lots and empty roads alike. However, many times the solution space
      is highly constrained ((for instance, the width of a lane is on the order of twice
      the cars width), and the approach may be unnecessarily complicated. It all comes
      down to how fast you can make the planner run.
    • Although the numerical optimization approach as such should not be ruled out,
      the implementation should not build on the existing planning software due to the
      lack of documentation and to the fact that the code was optimized for the 2005
      Grand Challenge.

5    Acknowledgments

I would like to thank the SURF 2006 Urban Challenge team, in particular David Bolin,
with whom I worked on the dynamic map, David Knowles, who also has been working on
path planning, and Laura Lindzey, whose knowledge about the existing system has been
absolutely indispensable. I also want to thank professor Anders Rantzer at the Depart-
ment of Automatic Control at Lund University for putting me in touch with Caltech,
and of course professor Richard Murray for making my SURF possible at all.


 [1] Paul Fiorini, Zvi Shiller. Time Optimal Trajectory Planning in Dynamic Environ-
     ments, Proceedings of the 1999 IEEE International Conference on Robotics and
     Automation, pp. 1553-1558, 1996
 [2] David Hsu et al. Randomized Kinodynamic Motion Planning with Moving Obstacles,
     International Journal of Robotics Research. Vol. 21, no. 3, pp. 233-255, March 2002
 [3] Mark B. Milam. Real-Time Optimal Trajectory Generation for Constrained Dynam-
     ical Systems, PhD thesis, California Institute of Technology, 2003
 [4] Dmitry L. Kogan. Realtime Path Planning through Optimization Methods, Califor-
     nia Institute of Technology, 2005

 [5] Philip E. Gill et al. User’s Guide for SNOPT 5.3: A Fortran Package for Large-
     Scale Nonlinear Programming, February 1999


A     Mathematical Details

To find the optimal trajectory, an optimization problem on the following general form
is constantly solved in real-time:

                               min J(x)    s.t. L ≤ g(x) ≤ U

Here, x ∈ Rn is a vector representing the trajectory and J : Rn −→ R is a cost function.
L, U ∈ Rm are lower and upper bounds on the constraint function g : Rn −→ Rm ,
where m is the number of constraints. The constraints are necessary to ensure that the
computed trajectory is dynamically feasible and obeys the speed limit. As described
below, they are also used for avoidance. Constraints are put on speed (v < vlimit ),
acceleration, steering angle, steering rate and lateral acceleration. See Kogan [4] for
The trajectory is fully determined by the yaw θ(s) and speed profile v(s) along the
trajectory (s is the normalized arc length), together with the trajectory length Sf . All
user variables (Northing and Easting coordinates, velocity, acceleration, yaw, etc.) can
be expressed in terms of these so-called flat variables (see Kogan [4]). The user variables
are necessary because it is for them the constraints are defined. Also, the Northing
and Easting coordinates are needed to compute the speed limit, since it depends on the
Now, the yaw and speed profiles belong to infinite dimensional spaces, which therefore
have to be approximated by a finite dimensional space Rn due to the finite nature of the
solver. This is done by sampling these two functions at equally spaced collocation points
along the trajectory. Hence the solver representation x of the trajectory is:

                                                                   1    2
         x = ( (θ(s))s∈Scoll (v(s))s∈Scoll Sf ),   Scoll = {0,       ,     , ..., 1},
                                                                 N −1 N −2

where N is the number of collocation points. The dimensionality n of the optimization
problem is thus n = 2N + 1. The constraints are only guaranteed to be satisfied at the
collocation points. It is therefore assumed that the collocation spacing is tight enough
for the constraints to hold also in between the collocation points.
Obstacle avoidance enters the problem through the speed limit constraint. In the old
planner the speed limit depended on the spatial location only, since the environment was

assumed to be static. To account for moving obstacles the speed limit function vlimit (x, y)
is replaced by vlimit (x, y, t), where t is the time when Alice would be at the location (x, y),
given the candidate solution currently under consideration by the solver.
An important observation is that the time coordinate t(s) along the trajectory can be
expressed in terms of the flat variables v(s) and Sf as in Equation (1) in Section 2.2.
Thus, no new solver variables have to be introduced, implying that the introduction of
a time dependent speed limit does not increase the dimensionality of the optimization
Since the numerical solver uses the Jacobian of the constraint function, this Jacobian
is implemented in the planner. The fact that new speed limit vlimit (x, y, t) now also
depends on t implies that a correction is required for the Jacobian to be accurate. The
division into a static and dynamic speed map, vlimit (x, y, t) = vstatic (x, y)vdynamic (x, y, t),
makes it possible to simplify the implementation. The partial derivatives with respect
to space (∂vlimit /∂x and ∂vlimit /∂y) are computed numerically as in the old algorithm
after the static and dynamic parts of the map have been multiplied together. As for the
time derivative, we have

                                  ∂vlimit           ∂vdynamic
                                          = vstatic           .
                                    ∂t                 ∂t

Computing ∂vdynamic /∂t involves smoothing and numerical differentiation in one dimen-
sion only. However, the solver needs the Jacobian of the constraint function g with
respect to the solver variables x. This requires more complicated modifications to the
existing code, and was not implemented due to the complexity and lack of documenta-
The cost function J is a linear combination of traversal time T , steering effort Eφ = φ 2
(φ is the steering angle) and acceleration effort Ea = a   2 . The steering and acceleration

terms are included to achieve a smoother driving behavior.
Once the optimization problem has been posed, a numerical solver is used to perform the
actual computations. The software used by the old planner, and also by the extended
planner, is SNOPT (cf. Kogan [4] and [5]), which is a Fortran-based commercial package
based on Mark Milams NTG software [3].

B     Module Description

Below is a slightly more detailed description of the dynamic planner module and its
subcomponents (see Figure 3) than was given in Section 2.
The (dynamic) planner module can be thought of as a container for the components
where the planner algorithm and its support functions actually lives. It is responsible for

all interfacing with the outside world (obstacle tracking, fusion mapper, state estimation)
through Skynet, the network communication software used on Alice. It continually
initializes and runs the planner algorithm and sends the results to the trajectory following
module. Two main changes were made to the old code; the calls to the planner were
extended with a parameter indicating the current time, and a loop listening to Skynet
messages on updates to the dynamic map was implemented.
The static map contains a grid-base representation of the static parts of the environ-
ment. It is constantly updated as new sensor readings of the environment are received.
The sensor data is processed and transformed into the appropriate form by the (dy-
namic) fusion mapper. The dynamic map contains a vectorized representation of the
moving obstacles and is kept up to date by the moving obstacle detection and tracking
The (dynamic) planner is where the planning algorithm is implemented. Apart from
the optimizer itself, the 2005 planner contains a seed generator (the so-called reactive
planner) that computes an initial guess for the optimizer. As mentioned in the Objectives
section, modifying this component has not been part of this project. The planner also
contains the optimizer (refinement stage planner) where the cost and constraint functions
are defined, as well as other support functions required by the numerical solver.
Figure 3 does not show the interface with the supervisory controller, SuperCon, which
in the old architecture was responsible for system diagnostics and contingency manage-
ment. The interface with SuperCon has not been part of this project, since the future
functionality and structure of this component was far from being finalized in the summer

C     Simulation Setup

Figure 7 shows the system setup for the simulation runs. The spoofCom component was
implemented in order to emulate the moving obstacle detection and tracking module
(cf. Figure 3). It continually sends Skynet messages about moving obstacles so that
the dynamic map can be properly updated. The planner and fusion mapper shared an
AMD64 computer, while the remaining modules were distributed over 2-3 Pentium 4
Each test scenario has a corresponding RDDF specifying the trackline and a scenario file
specifying the moving obstacles. The obstacles are always moving linearly with constant
speed, and their trajectories are assumed to be known exactly to the planner.

                                               trajectory                    commands
  Static speed map                Dynamic
                                                              trajFollower                 simulator
      (from file)               plannerModule

 spoofCom (spoofed
moving obstacle map)
                                  Dynamic                   map deltas

    = component have been written or modified
            as a part of this project

  Figure 7: The information flow between modules during simulations.