IEEE Robotics & Automation Magazine - March 2015 - 71

lattice, scaled by a cost factor that penalizes backward and
turning motions. These paths can be seen as an initial, very
tight collection of convex polyhedra, which is then relaxed to
obtain a larger spatial envelope for each vehicle.
The planner uses a set of predefined, kinematically feasible
motion primitives, which are repeatedly applied to obtain a
directed graph that covers the state space. The graph is then
explored using A ) [20], or anytime repairing A ) (ARA )) (an
efficient anytime variant) [21], which can provide provable
bounds on suboptimality. Effective heuristic functions [22], as
well as offline computations for collision detection, are
employed to speed up exploration of the lattice. Our approach
is inspired by existing lattice-based path planners [23] that are
successfully used in real-world applications. It extends existing work by providing the possibility to compute paths for
multiple robots jointly. This ensures that the computed spatial
envelopes provide the opportunity for vehicles to yield to others. All paths are generated such that there exists a time profile that yields feasible trajectories (V1, 2), under the assumption of car-like and waist-actuated vehicles.
Coordination
The feasibility of a set of trajectory envelopes cannot be ascertained without checking global constraints such as floor
space. In particular, two trajectory envelopes (of different
vehicles) that overlap in both time and space imply the possibility of a collision. The temporal and spatial overlap defines a
conflict set, i.e., a set of pairs of spatial polyhedra with nonempty intersections and whose associated temporal constraints also intersect (i.e., it is possible for the vehicles' reference points to be in a common area at the same time).
Note that since trajectory envelopes constitute both a
temporal and a spatial CSP, it is sufficient to eliminate solutions from these CSPs that entail the possibility of both temporal and spatial overlap. In other words, we are interested in
enforcing the feasibility of the set of all trajectory envelopes
for all vehicles (AP3). The problem of finding an overall set
of trajectory envelopes that is feasible requires a significant
computational overhead. Several strategies are possible, one
being to refine only the spatial envelopes of spatiotemporally
intersecting trajectory envelopes to eliminate the spatial
overlap. Another possibility is to add temporal constraints
that eliminate temporal overlap. The third option is to perform one or both refinements, depending on some particular heuristic indicating the impact of the refinement on the
feasible trajectories. In SAUNA, we have explored the second
option, i.e., the trajectory coordination algorithm resolves
concurrent use of floor space by altering when different
vehicles occupy spatially overlapping polyhedra. More precisely, the algorithm refines the temporal envelopes by adding temporal constraints Ta. This yields a feasible set of trajectory envelopes.
Finding a set of additional constraints that make the set of
trajectory envelopes feasible can itself be cast as a CSP. The
variables of this CSP are conflict sets, i.e., pairs of polyhedra
that intersect and whose associated temporal variables may

overlap. The values of these variables are temporal constraints
Ta that eliminate this temporal overlap. It can be shown that
a solution to this CSP prunes out of the trajectory envelopes
those trajectories that lead to a collision between controlled
vehicles (ES2). In addition, the identified temporal constraints
guarantee the absence of deadlocks (Dep3).
In real deployments of AGVs, it is common practice to
dynamically impose deadlines on vehicles reaching their
destinations (AP2). Accounting for such constraints renders
the previously mentioned
trajectory coordination
The key industrial
problem NP-hard. Note
also that the minimizing
requirements of efficiency
makespan (i.e., the total
completion time of all
and safety pose significant
trajectories) is equivalent
to resolving the problem
challenges, especially for
with increasingly tight
deadlines on all vehicles
perception.
and is therefore NP-hard.
The (centralized) coordination algorithm proposed in SAUNA, detailed in [1], employs a powerful heuristically guided, systematic CSP search to find the
resolving temporal constraints Ta. The search employs a
spatial heuristic for deciding which pair of spatiotemporally overlapping polyhedra to separate in time and a well
known temporal heuristic [24] to decide which vehicle
should take precedence.
Collision Prediction
To address the collision prediction challenge, we focus on
(noncontrollable) human-driven vehicles (ES4) and go
beyond reactive collision-avoidance solutions to account
for possible collisions before they happen. The key idea is
to make vehicles proactive, i.e., able to adapt their motion
as early as possible to minimize the risk of collision while
still moving toward their targets. To do so, our solution
uses the continuous flow of refinements to the trajectory
envelopes provided by the perception and coordination
modules. Each obstacle detected by perception is described
by a set of probabilistic attributes that includes an oriented
bounding box and color information. Such perceptual
information is used to perform short-term tracking of
nearby perceived moving objects, with the aim of extracting information about their motion, i.e., position and velocity. Motion information is then used to compute probabilistic estimates of the future positions of each of the tracked
objects. Possible future collision with a specific object is
estimated as the probability of future intersection between
the bounding box of that object and the spatial envelope of
a vehicle. In our current implementation, we compute the
probability of intersections using sequential Monte Carlo
estimation techniques [25]. For each tracked object, we create a particle filter that estimates the future positions of the
object. The probability of collision is calculated as the ratio
march 2015

IEEE ROBOTICS & AUTOMATION MAGAZINE

Table of Contents for the Digital Edition of IEEE Robotics & Automation Magazine - March 2015