A Constraint-Based Approach for Multiple
Non-Holonomic Vehicle Coordination in Industrial Scenarios
Federico Pecora and Marcello Cirillo
Center for Applied Autonomous Sensor Systems ¨
Orebro University, SE-70182 Sweden <name>.<surname>@oru.se
Abstract
Autonomous vehicles are already widely used in industrial lo-gistic settings. However, applications still lack flexibility, and many steps of the deployment process are hand-crafted by specialists. Here, we preset a new, modular paradigm which can fully solve logistic problems for AGVs, from high-level task planning to vehicle control. In particular, we focus on a new method for multi-robot coordination which does not rely on pre-defined traffic rules and in which feasible and collision-free trajectories are calculated for every vehicle ac-cording to mission specifications. Also, our solutions can be adapted on-line to exogenous events, control failures, or changes in mission requirements.
Introduction
Industrial actors involved in the development of autonomous vehicles (e.g., autonomous forklifts for warehouses) are con-stantly interested in decision support tools which could im-prove the flexibility and the performance of their products. Atlas-Copco1 (Larsson, Appelgren, and Marshall, 2010), Kiva Systems2, INRO3 (Thomson and Graham, 2011) and Kollmorgen4, among others, aim to achieve complete au-tomation in Autonomous Ground Vehicle (AGV) deploy-ments. Although it is current practice to employ automated solutions in several aspects of logistics automation, many key parts of the deployment phase are still ad-hoc and man-ual. For instance, the definition of AGV paths is often done off-line, and these paths are hand crafted for each different setting. Also, large-scale industrial deployments of AGVs rarely include more than very crude heuristics to optimize mission scheduling. Another limitation of current industrial solutions is the resolution of spatial conflicts, which is of-ten performed off-line through manually synthesized traffic rules, whose correctness cannot be formally proved. Other fallacies of real systems include the lack of support for re-sources and an often only partial support for on-line mis-sion constraint posting (e.g., changed deadlines, new re-quirements, collapses of resource availability).
When automated solving components are used in indus-trial AGV deployments, these are usually not integrated. For
1 http://www.atlascopco.com 2http://www.kivasystems.com/ 3http://www.inro.co.nz/ 4 http://www.kollmorgen.com
instance, it is often the case that path planning is de-coupled from trajectory generation, or that the allocation of vehicles to destinations does not depend on the trajectory that will ac-tually be followed by the vehicles. This leads to inefficien-cies in the quality of the solutions and reduced flexibility in dealing with contingencies. Furthermore, the methodol-ogy for assessing how many AGVs are necessary for a par-ticular deployment typically consists of what-if analyses on simulated scenarios. This analysis becomes more cumber-some and, especially, less accurate if many de-coupled solv-ing modules are employed.
In this paper, we introduce a system which strives to facil-itate all phases of deployment of AGVs in real settings. Our approach is modular, in that it can be applied “partially” or in “pieces”, depending on the requirements of the particu-lar deployment at hand. For instance, AGV paths may be automatically generated by a path planner (as is the case in this paper), or the routes could be manually decided by a field specialist (as is often the case in industrial settings). The same principle applies to task planning, which can be automated or manually decided by human operators (in this paper, the specific task planning algorithm is omitted). To achieve this, the modules rely on a shared, constraint-based representation of the overall problem, and each module re-fines this representation from “its own” point of view.
Related Work
Many of the problems underlying the automation of task and motion planning for industrial vehicles have been addressed in research. As a result, important advancements have been achieved in addressing separate parts of the overall prob-lem. Algorithms such as M∗ (Wagner and Choset, 2011), an extension of the classical A∗to multi-robot systems, and the work of Luna and Bekris (2011), whose focus is a new method for multi-robot path planning which is computation-ally efficient and complete, are recent examples of promis-ing theoretical results. A new system for the coordination of large multi-robot teams has been presented by Kleiner, Sun, and Meyer-Delius (2011). The authors propose a system that generates an overall, optimal road map configuration. How-ever, in this work the agents are assumed as moving on a grid, and the local motions are calculated for each robot in-dependently from the motions of other robots.
usually guarantees fast results is the assignment of prior-ity levels to different robots. This can be seen as an im-proved version of hand-coded traffic rules, but cannot ensure deadlock-free situations. An example of an algorithm which relies on this paradigm is presented by ter Mors (2011). The overall system can find optimal, conflict-free routes in low polynomial time, but relies on a pre-defined roadmap shared by all agents for path planning. Desaraju and How (2011) further extend the idea of prioritized path planning, by substituting the pre-defined priority levels with a merit based token, which is passed among agents. Once a robot has planned its own path, it circulates it to the other team members, which in turn update their trajectories.
In recent years, a number of approaches to multi-robot coordination have been presented which rely on pre-defined paths. Examples include the work of Kleiner, Sun, and Meyer-Delius (2011), whose algorithm is resolution com-plete and can be easily applied to situations in which a large number of agents is moving. However, the overall coordi-nated motions lack flexibility, as time is considered only im-plicitly in configurations along the paths. Therefore, the fi-nal result cannot take into account motion delays, or explicit temporal constraints imposed on the single agents and their positions over time.
A Constraint-Based Approach
A trajectory is a sequence of points and an associated tem-poral profile, which specifies exactly when the vehicle will be in a certain point. Instead of reasoning in terms of one trajectory, in our approach we reason in terms of tempo-raland spatial constraints on trajectories. The collection of spatial and temporal constraints on one vehicle’s trajectory is called a trajectory envelope. We can describe the overall mission planning problem (which will be defined precisely shortly) as a Constraint Satisfaction Problem (Tsang, 1993, CSP) where variables represent vehicles, their values repre-sent possible trajectories that they should execute, and con-straints are spatial and temporal requirements on these tra-jectories. A solution to the overall problem is therefore an assignment of trajectories to vehicles such that none of the requirements on trajectories is violated.
Trajectory envelopes are the key representational ele-ments used to express and solve our problem. More pre-cisely, (see also figure 1):
Definition 1. A trajectory envelope is a triple hS, D, Oi where
• S = {S1, . . . Sn} is a set of linear spatial constraints in the formAix + Biy ≤ Ci; each set Si of spatial con-straints specifies a convex region in the map within which the vehicle must be contained;
• D = {D1, . . . Dn} is a set of linear temporal constraints in the formli ≤ tei − tsi ≤ ui, wheretsi (tei) represents the time at which the vehicle enters (exits) the area spec-ified by the set of spatial constraintsSi; these constraints provide bounds on when the vehicle is within the convex region specified bySi;
• O = {O1, . . . On−1} is a set of linear temporal con-straints in the form li≤ tei− t
s
i+1≤ ui; these constraints
tsi tei li ≤ tei− tsi≤ ui Di tsi+1 Di+1 tei+1 li+1 ≤ tei+1− tsi+1≤ ui+1
time
Oi x
y
Aix + Biy ≤ Ci Ai+1x + Bi+1y ≤ Ci+1 Si+1
Si
li ≤ tei− tsi+1≤ ui
Figure 1:A trajectory envelope consisting of two spatio-temporal polygons.
provide bounds on when the vehicle is within the (convex) spatial overlap betweenSiandSi+1.
Problem Statement
Given N vehicles, we define the overall mission planning problem in our scenario as follows:
Definition 2. A mission planning problem is a tuple hM, I, G, T , Vi, where
• M is a metric map of the environment;
• I = {l1, . . . lN} is a set of coordinates in the map speci-fying theinitial location of all vehicles;
• G = {G1, . . . Gm} is a set of goals in the form hk, s, gi, each specifying thatk loads must be transported from lo-cations to location g;
• T = {T1, . . . Tn} is a set of temporal constraints on G∪I; these constraints are in the formli≤ tx− ty ≤ ui, where txandty are start/end timepoints of a goal or initial po-sition;
• V = {V1, . . . VN} are the capacities of the vehicles (max-imum amount∈ N of load each vehicle can carry). Finding a solution to a mission planning problem is decom-posable into three parts; first, compute an allocation of ve-hicles to goals which achieves the necessary displacement of loads to places; second, compute the trajectory envelopes E for each vehicle; third, synthesize a set of temporal con-straints Tcol imposing that spatio-temporal polygons inter-sect either only in space, or only in time, or not at all. Note that a solution to a mission planning problem in fact repre-sents sets of possible trajectories for each vehicle.
Our approach is based on four functional modules, namely task planning, trajectory planning, trajectory scheduling, and control. All modules output constraints cap-turing some aspect of the mission’s requirements. Together, these constraints define trajectory envelopes for all vehicles, and provide a global representation of the mission, from its high-level goals to the specific trajectories vehicles must ex-ecute to achieve these goals. Each of the four modules pro-gressively refines the representation, imposing increasingly specific requirements:
πk Task planning Si Trajectory scheduling Trajectory planning V T I G M
Spatio-temporal constraint database
Tcol Control Tcon [li, ui] c Ti πk Si Di Oi [li, ui]
Temporal constraint propagation (STP)
Figure 2:Overall information flow of the four solving modules.
1. Task planning decides goal locations for currently avail-able vehicles, therefore constraining the start and end points of vehicle trajectories. Also, task planning may im-pose quantified temporal requirements on these trajecto-ries, e.g., “vehicle A must reach goal (x, y) before time t”, or “vehicle A must reach its goal before vehicle B”. 2. Trajectory planning decides locations that should be
vis-ited in-between goals for each vehicle. Crucially, these are not simple paths between the initial and goal positions, rather they are trajectory envelopes, i.e., temporally-constrained sets of spatial constraints.
3. Trajectory scheduling imposes further temporal con-straints which ensure that the trajectory envelopes of dif-ferent vehicles do not intersect in time and space. As ex-plained below, this is a hybrid form of spatio-temporal reasoning based on state-of-the-art scheduling techniques. 4. The Control module employs a kinematic and geomet-ric model of a vehicle to generate and follow one specific trajectory that lies within the trajectory envelope obtained as a result of the above modules. This results in appropri-ate control signals for the vehicle or in the selection of a new trajectory within the trajectory envelope in the case that the currently selected trajectory does not adhere to the constraints.
Note that commitment to a specific trajectory in the above scheme is performed only at the control level, and that the controller is given the constraints within which it can se-lect one of many trajectories to follow. As we will show, the way in which these constraints are posted and propa-gated by the first three modules ensures that for each vehi-cle there exists a trajectory, within the constraints, which is feasible with respect to all vehicles. Also, due to the particu-lar type of hybrid temporal-spatial scheduling performed by the trajectory scheduling module, the selection of mutually-compatible trajectories by all vehicles can be done in poly-nomial time.
All communication between the four modules occurs through a spatio-temporal constraint database (see figure 2), which contains variables and constraints defining an over-all CSP. The variables of this problem are spatio-temporal polygons, and the constraints are spatial or temporal rela-tions defining the trajectory envelopes. Note that the tem-poral constraints in D and O constitute a Simple Temtem-poral
Problem (Dechter, Meiri, and Pearl, 1991, STP), which is solvable in cubic time through the Floyd-Warshall (Floyd, 1962) temporal constraint propagation algorithm. The algo-rithm computes the lower and upper bounds [li, ui] of all timepoints given the constraints in the database, and is trig-gered every time a constraint is added to the database. Thus, through temporal constraint propagation, the bounds of all timepoints tiof the spatio-temporal polygons are maintained at all times consistent with the temporal constraints that are present in the spatio-temporal database. Note also that if a temporal constraint is added which invalidates previously existing temporal constraints, the constraint database can de-tect this though a propagation failure, and thus rejects this constraint. This feature of temporal constraint propagation is employed in the trajectory scheduling to search for tem-poral constraints that avoid collisions between vehicles.
For convenience, we will refer to the sets S and D ∪ O as the spatial and temporal envelopes of a trajectory, respec-tively. Also, we refer to the i-th set of spatial and tempo-ral constraints {Si∪ Di∪ Oi} on a trajectory envelope as a spatio-temporal polygon. Two spatio-temporal polygons i and j are spatially overlapping if Si∩ Sj 6= ∅. Tempo-ral overlap is less straightforward: since the underlying STP maintains bounds on the timepoints of spatio-temporal poly-gons, temporal overlap must be assessed by choosing an ear-liest start time for the timepoints. Specifically, two spatio-temporal polygons i and j are said to be spatio-temporally overlap-ping in the earliest start time solutionif [ls
i, lei]∩lsj, lej 6= ∅. Note that two spatially and temporally overlapping polygons belonging to trajectory envelopes of different vehicles entail that the vehicles may collide.
Solving a Mission Planning Problem
It is often the case in real-world deployments that a partic-ular task allocation strategy, i.e., a task planning module, is given and cannot be substituted. This is due to the often very domain-specific objective functions, preferences and char-acteristics of the application scenario (e.g., a milk packag-ing factory vs. an underground mine). For this reason, in the following sections we omit details about task planning and focus on modules (2–4). Consequently, we assume for the purposes of the following description that a task planner has decided, for each goal G = hs, g, ki, a high-level plan that achieves the displacement of k loads from s to g by an
ap-propriate set of vehicles. Each element of this plan is in the form πk = hi, f, s, gi, indicating that vehicle i should load an amount f ≤ Vi of load in s and transport it to g. Obvi-ously, f can also be equal to 0, when s represents the initial position of a vehicle and g its first load pick-up location.
We can now define the solution to the mission planning problem as follows:
Definition 3. A solution to a mission planning problem hM, I, G, C, Vi with N vehicles is a triple hΠ, E, Ci where • Π = {π1, . . . , πp} is a set of high-level plans which
achieve the goals inG;
• E = {hS1, D1, O1i . . . hSN, DN, ONi} is a set of trajec-tory envelopes where
– Siis a set of spatial envelopes for the trajectory of ve-hiclei;
– for every hi, f, s, gi in the high-level plan Π, Si con-tains a sequence of spatial polygons hS1, . . . , Sni whereS1contains locations, Sncontains locationg, and eachSjspatially overlapsSj+1;
– DiandOiare sets of temporal constraints defining the temporal envelope of the trajectory for vehiclei; these constraints impose that overlapping spatial polygons also overlap in time;
• C = T ∪ Tcolis a set of temporal constraints between the start/end timepoints of any pair of spatio-temporal poly-gons inE; this set contains the constraints T expressing the initial temporal requirements of the mission planning problem, as well as a set of constraintsTcolwhich ensures that the intersection of spatio-temporal polygons for dif-ferent vehicles is either only spatial, or only temporal, or neither (i.e., these constraints disallow collisions).
Trajectory Planning
In order to obtain trajectory envelopes, we first employ a lattice-based planner to generate kinematically feasible paths for the (non-holonomic) vehicles in the mission plan-ning problem. A lattice can be seen as a generalization of a grid: instead of using perpendicular lines, the state-space is discretized by repeating the same primitive set of connect-ing edges. We start from a set of kinematically acceptable motion primitives which can be repeated over and over to obtain a directed graph. Obviously, the graph need not be completely specified from the start, and can be progressively built during search. The graph is then efficiently explored using deterministic, theoretically sound algorithms. In our case, we chose to rely on the classic A∗(Hart, Nilsson, and Raphael, 1968) for optimal path generation5, and on one of its most efficient anytime versions, ARA∗(Likhachev, Gor-don, and Thrun, 2003), which can provide provable bounds on sub-optimality.
Our approach is inspired by existing lattice-based path planners, as the ones successfully used in real world ap-plication by Pivtoraiko, Knepper, and Kelly (2009) and by Urmson, Anhalt, and others (2008).
5
The resulting path is optimal wrt the choice of the set of prim-itive motions and to the granularity with which the lattice is built.
Each vertex of the lattice represents a pose of the vehi-cle in the form hx, y, θi, where x and y are coordinates on a grid of a pre-determined resolution, and θ ∈ Θ is the ve-hicle orientation, where Θ is the set of pre-selected possible orientations for a specific vehicle model. For instance, in the experimental runs presented in this paper, the grid resolution is always equal to 0.2 meters, Θ is a set of 16 angles, equally spaced between π and −π, and each vertex is connected to 15 others through pre-calculated, kinematically feasible mo-tion primitives. In our setup, the cost is based on the distance covered by each edge of the lattice, multiplied by a cost fac-tor that penalizes backwards and turning motions.
Using off-line computation, it is possible to speed up the exploration of the lattice in environments with obstacles in two ways. First, as each edge is the instantiation of a pre-calculated motion template, we can pre-compute for each primitive the cells which the vehicle will partially or to-tally occupy during the motion. This way, obstacle detec-tion can be efficiently performed on-line, by checking the occupancy level of each cell in the grid-partitioned environ-ment. Second, a more informed heuristic function can be pre-computed and stored in a lookup table (Knepper and Kelly, 2006) by saving the minimum cost to connect two poses in a specific range (10 meters, in the experimental runs presented in this paper). This proves to be a much more effi-cient heuristic than simple Euclidean distance, as it uses the kinematic model of the non-holonomic vehicle to factor in maneuvering costs. Both functions are however admissible, and they entail optimal solutions when used with A∗.
When the environment presents obstacles, a third heuristic function is also used. Regardless of the pose of the vehicle, each cell in the environment is associated with a value repre-sented by the distance from the goal in a 8-connected graph. All three heuristic functions are evaluated when a new ver-tex of the lattice is expanded, and we always use the higher in value. Clearly, the resulting heuristic function is not ad-missible in environments with obstacles. This, however, is not a big drawback in practical applications, where our goal is to obtain drivable and kinematically feasible, albeit sub-optimal, paths. Also, in real settings (as the one described below), we preferably employ ARA∗ to explore the lattice, in order to speed up the computation, therefore relinquishing optimality anyway.
Recall that a trajectory envelope is defined as a set of tem-porally constrained spatio-temporal polygons. The starting point for computing the spatial constraints Sifor vehicle i’s trajectory envelope is a path obtained through the path plan-ning strategy outlined above. Then, the computed spatial en-velope is used together with the path and the minimum and maximum speeds of the particular vehicle to determine the temporal envelope of the trajectory (i.e., the temporal con-straints Di and Oi). These two procedures are described in the following paragraphs.
Spatial envelope generation. For each vehicle, waypoints are sampled along the path obtained by the path planning al-gorithm. The sampling procedure is incremental, and works as follows:
1. select the first two points of the path;
2. build a convex polygon around the vector defined by these two points whose shape is the bounding box of the vehi-cle, centered in the first point;
3. grow the sides of the polygon outwards, stopping the growth of each side when it intersects with an obstacle or when a threshold on growth has been reached; 4. select two points along the path immediately outside the
polygon, and go to step (2).
The resulting sequence of polygons is such that (1) the path is completely covered by polygons, and (2) each polygon intersects the next one6. The resulting polygons are used to define the spatial envelope Sifor each vehicle i. All spatial envelopes are then added to the spatio-temporal constraint database.
Temporal envelope generation. Again starting from the first point along a vehicle’s path, the path is traversed to compute the distance covered by the vehicle while traveling in each spatial polygon Sj ∈ Si. These distances are used to compute the temporal bounds within which the vehicle can possibly occupy each polygon and each area of polygon intersection. For this computation, we employ two constant speeds (vmin, vmax) corresponding to the minimum and max-imum desired speeds for the vehicle. For each spatial poly-gon Sj we thus obtain a pair of bounds [lj, uj] restricting the temporal distance between its start and end timepoints (ts
j, tej), as well as a pair of bounds lj0, u0j restricting the distance between the end time te
j of spatial polygon Sjand the start tsj+1 of spatial polygon Sj+1. Together, all these constraints constitute the spatial envelope Di ∪ Oi of the trajectory of the i-th vehicle, and are added to the spatio-temporal constraint database.
Trajectory Scheduling
The spatio-temporal polygons generated by the trajectory planning module impose vehicles to be in certain (convex) regions within certain temporal bounds. In order to complete the synthesis of the solution to the mission planning prob-lem, further constraints must be added (the set Tcol) in order to prune out of the solution trajectory envelopes in E those trajectories that lead to collisions. This problem is cast as a CSP whose variables are sets of spatio-temporal polygons which have a non-empty spatial and temporal intersection. The values of these variables are temporal constraints which separate these temporally concurrent, spatially overlapping polygons in time. In other words, the trajectory schedul-ing module resolves concurrent use of floor space by al-tering when different vehicles occupy spatially overlapping polygons. This results in temporal constraints that disallow the concurrent occupation of overlapping polygons by more than one vehicle at a time.
The reduction of the trajectory scheduling problem to a CSP is inspired by the ESTA precedence-constraint post-ing algorithm (Cesta, Oddi, and Smith, 2002) for resource
6
Polygon intersection is not guaranteed with this procedure, which, however, gives very good results in practice.
Function SolveESTA(E): success or failure static Tcol← ∅ 1 repeat 2 conflicts ←h(tsi, tei), (tsj, tej)i ∈ E : 3 [ls i, lei] ∩lsj, lej 6= ∅ ∧ Si∩ Sj6= ∅ 4 if conflicts 6= ∅ then 5 MCS ← Choose(conflicts, hvar) 6 resolvers ←(te i, tsj) : Di, Dj6=i∈ MCS 7 while resolvers 6= ∅ do 8 (tei, tsj) ← Choose(resolvers, hval) 9 resolvers ← resolvers \ (tei, tsj) 10 STP ← STP ∪ (0 ≤ tsj− tei ≤ ∞) 11 if STP is consistent then 12 Tcol← Tcol∪ (0 ≤ tsj− tei ≤ ∞) 13
if SolveESTA(E) = failure then
14
Tcol← Tcol\ (0 ≤ tsj− tei ≤ ∞)
15
else return success
16
until conflicts = ∅
17
scheduling. The algorithm is a CSP-style backtracking search (see algorithm SolveESTA()). It starts by collect-ing all pairs of spatio-temporal polygons that overlap both spatially and temporally (line 3–4). These conflicts are the variables of the CSP, and as usual in CSP search, ordered ac-cording to a most-constrained-first variable ordering heuris-tic (hvar) — the rationale being that it is better to fail sooner rather than later so as to prune large parts of the search tree. Once a conflict is chosen, its possible resolvers are identi-fied (line 7). These are values of the CSP’s variables, and each is a temporal constraint to be imposed between the pair of spatio-temporal polygons that would eliminate their tem-poral overlap. Note that since conflicts are pairs of spatio-temporal polygons, there are only two ways to resolve the temporal overlap, namely imposing that the end time of one spatio-temporal polygon is constrained to occur before the start time of the other, or vice-versa. Again as is common practice in constraint-based reasoning, the resolver to at-tempt first is chosen (line 9) according to a least constrain-ing value orderconstrain-ing heuristic (hval) — the rational being that the value which leaves most options open for future choices should be given precedence. The algorithm then attempts to post the chosen resolving constraint into the spatio-temporal constraint database (line 11). If the underlying STP is still consistent, then the procedure goes on to identify and re-solve another conflict through a recursive call (line 14). In case of failure (line 15), the chosen value is retracted from the spatio-temporal constraint database and another value is attempted.
Clearly, the efficiency of the search for resolving con-straints depends on how well-informed the value and vari-able ordering heuristics are. In our specific case, we employ two heuristics which take into account both the temporal and the spatial features of the trajectory scheduling prob-lem. The heuristic hvaremployed for variable ordering gives preference to the pairs of spatio-temporal polygons that are spatially closer to other conflicting pairs. The idea of this
heuristic is that conflicts that are “close” to other conflicts are more likely to be the most difficult to solve, as the possi-ble choices for resolving these conflicts will depend on how other conflicts are resolved.
As a value ordering heuristic, we follow the method used by Cesta, Oddi, and Smith (2002), whereby the temporal bounds hls/ei , us/ei i andhls/ej , us/ej i of the start/end time-points of the chosen pair of spatio-temporal polygons are analyzed to determine which ordering least restricts the tem-poral slack of the intervals.
From Envelopes to Vehicle Control
The trajectory scheduling module performs the last step in defining trajectory envelopes that solve the mission plan-ning problem. Every vehicle’s control module must at this point select one particular trajectory (i.e., a path and a speed profile) within the vehicle’s trajectory envelope to execute. However, it is important to note that the particular trajec-tory chosen by each vehicle’s controller depends on which trajectory other vehicles have chosen, as trajectories of dif-ferent vehicles are temporally dependent.
The presence of temporal dependencies between trajecto-ries entails that vehicle controllers must communicate their choice to other controllers. This choice can be seen as a set of temporal constraints Tcon, which is added to the shared constraint database. Here, we leverage an important feature of the STP underlying our constraint database: in a fully propagated and consistent STP, i.e., one in which the bounds [li, ui] of all timepoints ti have been updated to reflect the constraints, there exist two specific assignments of times to timepoints that are temporally consistent, namely the earli-est time assignment (ET)and the latest time assignment (LT). The former is obtained by choosing the lower bound lifor all timepoints, and the latter by choosing the upper bound ui for all timepoints. Therefore, we can immediately obtain the fastest and slowest speed profiles for all vehicles.
Our vehicle control scheme consists in a model predictive controller (Qin and Badgwell, 2003) which synthesizes con-trol outputs to the vehicle. These outputs enable the vehicle to follow the given trajectory, both with respect to the spa-tial constraints Si and with respect to a particular solution to the temporal constraints in {Di∪ Oi}. Having selected a particular speed profile, this means that a controller must en-ter and exit spatial polygons exactly at the times prescribed in the particular speed profile selected for that vehicle (e.g., the fastest speed profile). Whenever these times cannot be achieved, vehicle controllers must revise their trajectories and compute new control outputs. Fortunately, to compute an allocation of times which is different from the ET or LT, it is sufficient to impose one constraint which models the desired allocation of one timepoint, and this can be achieved in polynomial time. As a result of propagation, the new ET allocation will clearly be temporally feasible. Indeed, even more interestingly, numerous alternative, globally consistent speed profiles can be computed before hand, each of which reflects one specific time in which vehicles should enter and exit each spatial polygon on their trajectory.
Experimental Evaluation
We now present an experimental validation of the two cen-tral modules of our approach, namely trajectory planning and trajectory scheduling. We validate the modules both qualitatively and quantitatively, with a special focus, in the quantitative analysis, on the performance of the trajectory scheduling algorithm. All test runs were performed in simu-lation. The kinematic model employed in all the experiments is that of a Linde H50D forklift.
Qualitative Evaluation
A single run in an industrial scenario was performed to qualitatively assess the feasibility of the approach in a re-alistic setting. For this purpose, we used a real map of an underground mine (courtesy of Atlas-Copco Drilling Ma-chines, see figure 3), where we deployed 7 identical ve-hicles with pre-assigned tasks. Each task consisted in an initial and final pose for one of the vehicles, in the form {hxi, yi, θii, hxf, yf, θfi}.
The overall run consisted of three phases. First, our lattice-based path planner generated in parallel individual kinematically feasible paths. Second, the paths were sam-pled to calculate the spatial envelopes for each vehicle. Assuming that all the forklifts would start moving at the same time, and defining the minimum and maximum desired speed in the tunnels (vmin,vmax), we thus obtained a temporal envelope for each vehicle. The SolveESTA() algorithm was then invoked to generate a solution to the mission plan-ning problem. As explained above, the algorithm identified all the conflicts in space and time over the temporally and spatially constrained polygons, and solved them by impos-ing additional temporal constraints Tcol.
In this specific run, considering the initial temporal and spacial envelopes for each single vehicle, the scheduler iden-tified three groups of conflicting polygons (shaded in fig-ure 3). Each conflict reflects the fact that, with only the tem-poral constraints stemming from the desired vmin and vmax of each vehicle along its nominal path, two or more vehicles would be “allowed” to be in overlapping areas at the same time, if they chose some particular velocity profiles.
The scheduler’s solution consisted of 13 temporal con-straints. This resulted in revised bounds for each of the spatio-temporal polygons such that in any consistent execu-tion (e.g., the earliest start time, or fastest, execuexecu-tion) vehi-cles yield to each other appropriately in order to avoid colli-sions.
Extracting a specific trajectory for execution other than the earliest and latest time trajectories takes about 250 mil-liseconds. The total time required to generate the scheduled trajectory envelopes was less than 40 seconds: the paths were generated using the ARA∗ algorithm, with a cut-off time of 5 seconds, and then used to grow a total of 140 poly-gons for the 7 vehicles, while trajectory scheduling took less than 34 seconds.
Quantitative Evaluation
To evaluate our approach in a more thorough and quan-titative way, we generated a benchmark set of 900 tra-jectory scheduling problems. On an obstacle free map of
Figure 3:A solution to a mission planning problem involving seven vehicles in an underground mine. Spatio-temporal polygons involved in critical sets during trajectory scheduling are shaded.
width and length of 50 meters, we pre-defined 80 poses {hx1, y1, θ1i, . . . , hx80, y80, θ80i}, where the (x, y) coordi-nates of each pose correspond to one of 10 points spatially distributed on a circle, 40 meters in diameter, and where the orientation θ is one of 8 pre-determined angles. Each pose could be chosen as initial of final pose for a vehicle, with the only constraint that the (x, y) coordinates of the two poses should be different.
The experimental evaluation was performed by defining 9 test sets, each corresponding to an increasing number of vehicles concurrently deployed in the environment, from 2 (the minimum number of vehicles whose spatial and tem-poral envelopes could generate conflicts) to a maximum of 10. For each set, we performed 100 test runs, as follows. In each run, we randomly chose initial and final poses for the number of vehicles required, only avoiding that two or more vehicles had the same starting or final (x, y) positions. Once generated, the paths were used to obtain the spatial and tem-poral envelopes with (vmin, vmax) = (0.05, 15) meters per second. In order to make the problems difficult to solve for the scheduler, we also added temporal constraints imposing that the temporal distance between all initial spatio-temporal polygons is zero, thus forcing all vehicles to start moving at the same time. This, combined with a non-zero minimum speed for all vehicles, is what allows some benchmark prob-lems to be unsatisfiable.
Again, we focused on analyzing the trajectory scheduling efficiency, so we measured the time required by the sched-uler to find a conflict-free solution for each run, or to identify the problem as unsolvable. The results are shown in figure 4. As expected, scheduling time grows exponentially with the number of vehicles involved.
Two features of these results are interesting. First, note that problem difficulty in this benchmark is somewhat artifi-cially inflated as all vehicles are constrained to operate in an area which is 40-meter diameter circle at roughly the same
1 10 1000 10000 100000 1e+06 1e+07 1e+08 2 3 4 5 6 7 8 9 10 100
Scheduling time [msec]
Number of vehicles
UNSAT SAT All
Figure 4:Quantitative evaluation of the trajectory scheduler.
time. Moreover, all vehicles are constrained to start moving at the same time, as all starting spatio-temporal polygons are constrained to occur at the same time and a minimum velocity of zero is not possible. Even under these rather un-likely circumstances, the average resolution time remains under one second up to problems in which we deployed 8 vehicles. Second, recall that the solutions obtained through the scheduling procedure represent a trajectory envelope for each vehicle. Two single trajectories, the earliest time and latest time trajectory, can be extracted for all vehicles in lin-ear time (in the number of polygons). This is, even for the most difficult problem, an operation which takes less than 10 milliseconds. Furthermore, even if one vehicle controller decides it must stray from its current chosen trajectory, the calculation of another trajectory for all other vehicles is also a matter of milliseconds, as it can be done in cubic time. Specifically the most challenging problem of our benchmark contains 94 polygons, and we can extract a new trajectory
for all vehicles in less than 50 milliseconds.
Comparing heuristics. In a comparison against a random choice variable and/or value ordering heuristic, the proposed hvar and hval lead to dramatically better performance. We have also compared hvar to a heuristic commonly used in scheduling which employs only temporal features of the constraint network to determine the most constrained vari-able (Cesta, Oddi, and Smith, 2002). Our spatio-temporal heuristic lead to better performance of the backtracking search algorithm, although a complete comparison is nec-essary to establish whether the effect is due to the particular problem structure of the benchmark.
Conclusions and Future Work
This paper presents a new approach to multiple vehicle coor-dination in industrial environments. The framework is com-posed of four different modules for solving logistics problem for AGVs. The modules progressively refine a constraint-based representation of the overall problem, taking into ac-count high-level task planning goals and temporal require-ments to ultimately obtain commands for vehicle control. Our approach is engineered in a way that single modules can be used independently, thus providing the flexibility re-quired in industrial settings.
We have focused on the two central modules of our frame-work, namely trajectory planning and trajectory schedul-ing. Our main contribution lies in multi-robot system coor-dination: instead of relying on ad-hoc traffic rules, or pre-defined priority levels, we used an on-line scheduler to syn-chronize the movements of the AGVs. Our scheduler allows maximum flexibility, as vehicle trajectories can be globally adapted to exogenous events, control failures, or changed mission requirements. The two modules are evaluated both qualitatively and quantitatively, proving that our approach can be used on-line, and that the results can be immediately employed by low-level controllers.
Our future work will focus on the full development of the remaining two modules, the task planner and a robust model predictive controller, for one or more specific industrial set-tings. Also, we will explore the use of different heuristics for trajectory scheduling. Finally, we intend to test the full framework and/or parts of it in real industrial scenarios to demonstrate the benefit of our new paradigm in terms of de-ployability and efficiency.
Acknowledgments. This work is supported by project “Safe Autonomous Navigation” (SAUNA), funded by the Swedish Knowledge Foundation (KKS). The Authors wish to thank Dimiter Driankov, Dimitar Dimitrov and Simone Fratini for their support and useful comments.
References
Cesta, A.; Oddi, A.; and Smith, S. F. 2002. A constraint-based method for project scheduling with time windows. Journal of Heuristics8(1):109–136.
Dechter, R.; Meiri, I.; and Pearl, J. 1991. Temporal con-straint networks. Artificial Intelligence 49(1-3):61–95. Desaraju, V., and How, J. 2011. Decentralized path
plan-ning for multi-agent teams in complex environments us-ing rapidly-explorus-ing random trees. In Proc. of the IEEE Int. Conf. on Robotics and Automation (ICRA).
Floyd, R. W. 1962. Algorithm 97: Shortest path. Communi-cation of the ACM5:345–348.
Hart, P.; Nilsson, N.; and Raphael, B. 1968. A formal ba-sis for the heuristic determination of minimum cost paths. IEEE Transactions on Systems Science and Cybernetics 4(2):100–107.
Kleiner, A.; Sun, D.; and Meyer-Delius, D. 2011. Armo: Adaptive road map optimization for large robot teams. In Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS).
Knepper, R. A., and Kelly, A. 2006. High performance state lattice planning using heuristic look-up tables. In Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS).
Larsson, J.; Appelgren, J.; and Marshall, J. 2010. Next generation system for unmanned lhd operation in under-ground mines. In Proc. of the Annual Meeting and Exhibi-tion of the Society for Mining, Metallurgy & ExploraExhibi-tion (SME).
Likhachev, M.; Gordon, G.; and Thrun, S. 2003. ARA*: Anytime A* with provable bounds on sub-optimality. Ad-vances in Neural Information Processing Systems16. Luna, R., and Bekris, K. 2011. Efficient and complete
centralized multi-robot path planning. In Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS).
Pivtoraiko, M.; Knepper, R. A.; and Kelly, A. 2009. Differ-entially constrained mobile robot motion planning in state lattices. Journal of Field Robotics 26(3):308–333. Qin, S., and Badgwell, T. 2003. A survey of industrial model
predictive control technology. Control Engineering Prac-tice11:733–764.
ter Mors, A. 2011. Conflict-free route planning in dynamic environments. In Proc. of the IEEE/RSJ Int. Conf. on In-telligent Robots and Systems (IROS).
Thomson, J., and Graham, A. 2011. Efficient scheduling for multiple automated non-holonomic vehicles using a coordinated path planner. In Proc. of the IEEE Int. Conf. on Robotics and Automation (ICRA).
Tsang, E. 1993. Foundations of Constraint Satisfaction. Academic Press, London and San Diego.
Urmson, C.; Anhalt, J.; et al. 2008. Autonomous driving in urban environments: Boss and the urban challenge. Jour-nal of Field Robotics25(8):425–466.
Wagner, G., and Choset, H. 2011. M*: A complete multi-robot path planning algorithm with performance bounds. In Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS).
