Priority-based intersection management with kinodynamic constraints  Jean Gr ́ egoire ?   Silv` ere Bonnabel ?   Arnaud de La Fortelle ? †  Abstract — We consider the problem of coordinating a col- lection of robots at an intersection area taking into account dynamical constraints due to actuator limitations. We adopt the coordination space approach, which is standard in multiple robot motion planning. Assuming the priorities between robots are assigned in advance and the existence of a collision-free trajectory respecting those priorities, we propose a provably safe trajectory planner satisfying kinodynamic constraints. The algorithm is shown to run in real time and to return safe (collision-free) trajectories. Simulation results on synthetic data illustrate the benefits of the approach.  I. I NTRODUCTION  A. Motivation  Human error is the sole cause in 57% of all road accidents and is a contributing factor in over 90% [1], [2]. More- over, traffic congestion motivates the research to improve intersection traffic flow. Intelligent transportation systems are expected to tackle both safety and efficiency issues in the near future. Many systems have been proposed and they have proved their ability to increase traffic efficiency – particularly compared to traffic light systems – and to reduce the risk of road accidents [3], [4], [5], [6], [7], [8]. Furthermore, more generally, automated conflict management opens new perspectives to improve railway [9] and air transportation systems [10] efficiency. In transportation systems, safety is usually centralized (e.g. air traffic control, rail management systems) or at least managed locally in a centralized way (e.g. traffic lights). In the future, we anticipate there will be locally full information, e.g. through car-to-car commu- nication being currently standardized. Obviously there will be non-communicating entities, sometimes delays or sensing errors, but our aim is to go from a centralized system in full information down to more reactive schemes, ensuring safety first.  B. Related work  The standard approach to multi robot motion planning is to decompose the problem into two parts, as initiated in [11]. As presented in [12], the first one consists of determining fixed paths along which robots cross the intersection. The second one consists of computing the velocity profile of each robot along its path: this is a well-known problem studied for applications in automated guided vehicles (AGVs) and robot manipulators.  ?   MINES ParisTech, Center for Robotics, 60 Bd St Michel 75272 Paris Cedex 06, France  †   Inria   Paris   -   Rocquencourt,   RITS   team,   Domaine   de   Voluceau   - Rocquencourt, B.P. 105 - 78153 Le Chesnay, France  As first introduced in [13], [14], the path-velocity de- composition   enables   to   introduce   an   abstract   space:   the coordination space. It is a standard approach to robot motion planning [15], [16], and the motion planning problem in the real space boils down to finding an optimal trajectory in the coordination space that is collision-free with respect to an obstacle region. The coordination space is a   n -dimensional space (where   n   denotes the number of robots in the intersec- tion) and the obstacle-region has a cylindrical structure [17]. In [12], we have revisited the notion of priorities to propose a novel framework for automated intersection management based on priority assignment. It is a very intuitive notion: the priority graph indicates the relative order of robots. Our framework enables to decompose the motion planning prob- lem problem in the coordination space into a combinatorial problem: priority assignment, and a continuous problem: finding an efficient trajectory with assigned priorities. The ambition of this framework is to enable more robust- ness and distribution in future automated intersection man- agement systems. Indeed, existing intersection management systems such as proposed in [6], [3], [8] plan the complete trajectories of robots through the intersection and ensuring safety requires robots to follow precisely the planned trajec- tory. By contrast, if priorities only are planned, the priority graph can be conserved even if some unpredictable event requires a robot to slow down for some time. It is now clear that the combinatorial problem of assigning efficient priorities is inherently difficult, as noticed in [18] and developed in the priority-based framework in [12]. As a result, we will only consider in the present paper the issue of planning ”good” trajectories for already assigned priorities. When the robots can start and stop instantaneously, it is rela- tively easy to define an optimal trajectory for fixed priorities. This trajectory is referred to as the left-greedy trajectory [18], [12]. However, taking into account acceleration (and higher derivatives) constraints turns the optimization problem into a ”highly non-trivial” problem (as suggested in the conclusion of the paper [18]). In the present paper, we address the challenging problem of finding safe trajectories that respect this type of constraints. In [19], the problem is formulated as a mixed integer nonlinear programming problem, and the solution proposed is suitable only for a ”reasonable” and fixed number of robots. Moreover, priority assignment and trajectory planning are not decoupled. In the present paper, we focus on a low complexity solution to the trajectory planning problem with assigned priorities which is applicable for a large and potentially varying number of robots.  arXiv:1310.5828v2 [cs.RO] 5 May 2014
C. Contributions  We introduce a theoretical tool: the braking trajectory, which is a virtual trajectory obtained letting all robots slow- ing down as much as possible to stop. The key idea of the paper is to ensure that at every time-step, the (virtual) braking trajectory is collision-free. With the proposed planner, robots are maximally aggressive, i.e. always maximize the distance travelled at every time-step. However, they do not accelerate if the virtual braking trajectory becomes unsafe or violates a priority, i.e. they ensure the existence of a failsafe maneuver for the system of robots at any time. We present a trajectory planner with assigned priorities that consists of just-in-time braking and is proved to return collision-free trajectories respecting the assigned priorities. Section II and III present the modelling assumptions and recall the basics of the priority-based framework of [12]. Section IV introduces the motion planner algorithm along with   its   safety   guarantees.   Finally,   simulation   results   of Section V illustrate the efficiency of the approach. II. M ODELLING ASSUMPTIONS  A. The coordination space  We assume that robots are constrained to follow prede- fined paths to go through the intersection. The paths are not necessarily straight lines: robots are just considered as driving along fixed tracks. This can be achieved by a low- level controller. This standard assumption [13], [20], [21], [22], [7] fits well intersections in a road network, where robots travel along lanes. Every robot   i   follows a particular path   γ i   and we denote  x i   ∈   R   its   curvilinear   coordinate   along   the   path.   The configuration of the system of robots is   x   = ( x i ) i ∈{ 1 ...n }   and we denote   x ( t )   the evolution of   x   through time   t   ∈   [0 , T   ] . Figure 1 illustrates the path following assumption. γ   1   =   γ   2   =   γ   3   =   γ   4  γ   5   =   γ   6  γ   7   =   γ   8  γ   11   =   γ   12  γ   9   =   γ   10 8 10 9   7 12 11 1 2   3   4 5 6  Fig. 1.   The path following assumption. All robots in the same lane (depicted with the same color) travel along the same geometric path with independent velocity profiles.  The configuration space   χ   is known as the coordination space [23], [17], [13]. In the rest of the paper,   { e i } 1 ≤ i ≤ n  denotes the canonical basis of   χ . The use of the coordination space and the results of this paragraph are standard [17]. As every robot occupies a non-empty geometric region, some configurations must be excluded to avoid collisions between robots. The obstacle region   χ obs   is the open set of all collision configurations.   χ free   =   χ \ χ obs   denotes the obstacle-free space. A collision occurs when two robots occupy a common region of space, so that the obstacle region can be described as the union of   n ( n − 1) / 2   open cylinders   χ obs  ij   corresponding to as many collision pairs:   χ obs   =   ∪ i>j   χ obs  ij   . Each cylinder  χ obs  ij   is assumed to have an open bounded convex cross- section (in the plane generated by   e i   and   e j   ). Figure 2 displays the obstacle region and a collision configuration for a two-path intersection. We assume   χ obs   6   =   ∅   (otherwise, coordination is not required), so the boundedness condition ensures that   inf   χ obs   and   sup   χ obs   both exist. x1 x2 χ   obs Υ   1 Υ   2  Fig. 2.   The left drawing depicts two robots in collision in a 2-path- intersection. The right drawing depicts the corresponding configuration in the coordination space that belongs to the obstacle region.  B. Kinodynamic constraints  In this paper, we propose to take into account the technical constraints   of   the   robots   at   the   motion   planning   phase. These   include   kinematic   constraints   (maximum   velocity, maximum curve radius, etc.) and dynamic constraints (lim- ited acceleration, adherence, jerk, etc.). Let   p   denote the degree of the constraints and   n   the number of robots. Let  s ( t ) = ( x x ′   · · ·   x ( p ) )( t )   ∈   R n × ( p +1)   denote the state of the system. We let   x ( t )   =   π ( s ( t ))   denote the first column of the state   s ( t ) , that is the position of all robots. We say a trajectory   x   respects the kinodynamic constraints   C  if:   ∀ i   ∈ { 1 ...n } ,   ∀ t   ∈   [0 , T   ] ,   we   have   s i ( t )   ∈   C i   with  C i   ⊂   R p +1   representing the constraints for robot   i   and  C   =   ∏  i ∈{ 1 ...n }   C i   ⊂   R n × ( p +1) . Note that every robot can have different constraints   C i , and   C i   can not necessarily be expressed in a product form (for example, the constraint on the velocity can depend on the position). We assume that the kinodynamic constraints are such that the set of reachable positions from a given state in finite time is bounded. More precisely, the set of reachable positions from state   s 0   in a time-length   t :  χ reach ( s 0 , t ) =  {  x ( t )  ∣ ∣ ∣ ∣   x   respects the constraints   C s (0) =   s 0  }  (1) is assumed to be continuous with respect to   s 0   and to be a bounded hypercube of   x 0   +   R n  + . Note that, the above assumptions imply in particular that: 1) robots cannot travel backwards in the intersection,
2) and from a given state   s 0 , the set of reachable positions in finite time is bounded, and the bounds depend on the state   s 0   of the robots (position, velocity, acceleration, etc.). III. T HE PRIORITY - BASED FRAMEWORK  In this section, we recall the basics of priority-based intersection management introduced in our previous work [12].  A. The priority graph  Consider the region   χ obs  i  j   defined as follows and depicted in Figure 3:  χ obs  i  j   =   χ obs  ij   −   R + e i   +   R + e j   (2) xi xi xj  χ   obs xi xj  χ   obs j>i x i xj  χ   obs i>j xi>j   (t) xj  χ   obs xj>i   (t) xi>j   (t) xj>i   (t)  Fig. 3.   The top drawings represent in the plane   ( x i , x j   )   the obstacle region   χ obs , a collision-free trajectory   x i  j   respecting priority   i      j  and a collision-free trajectory   x j  i   respecting priority   j      i . The bottom drawings depict   χ obs  i  j   and   χ obs  j  i .  We define a natural binary relation corresponding to pri- ority relations between robots. A collision-free trajectory   x  induces a binary relation      on the set   { 1 ...n }   as follows. For  i   6   =   j   s.t.   χ obs  ij   6   =   ∅ ,   i      j   if   x   is collision-free with   χ obs  i  j   . The priority relation can be described by a graph   G  with nodes   { 1 ...n } , where each edge represents the relative priority of a pair of robots. Given a collision-free trajectory  x , the priority graph is defined as the oriented graph   G   whose vertices are   V   ( G ) =   { 1 ...n }   and such that there is an edge from   i   to   j   if   i      j , we write   ( i, j )   ∈   E ( G )   where   E ( G )  denotes the edge set. An example of a priority graph for 3 robots along 3 distinct paths is described in Figure 4.  B. Problem formulation  The initial state of the robots is   s init , and the goal region is   χ goal   =   ( sup   χ obs   +   R n  +  )   ⊂   χ free . A feasible trajectory for the considered problem is a trajectory   x   : [0 , T   ]   →   χ free  respecting constraints   C   such that   s (0) =   s init   and   x ( T   )   ∈ 3 1 2 3   1 2 3 1 2 3   1 2  Fig. 4.   Two representations of priority relations. Robots along a path in foreground have priority over robots along a path in background.  χ goal . The multiple robot motion planning problem consists of finding a feasible trajectory. The benefit of the priority- based approach is that the priority graph captures the discrete part of the problem that consists of assigning the relative order of robots through the intersection. When the priority graph   G   is fixed, for all   ( i, j )   ∈   E ( G ) , the trajectory must be collision-free with regards to   χ obs  i  j   . Given, a priority graph   G , the collision region with regards to priorities   G   is merely defined as:  χ obs  G   =   ⋃  ( i,j ) ∈ E ( G )  χ obs  i  j   (3) It is natural to define   χ free  G   =   χ   \   χ obs  G   , so that   { χ free  G   , χ obs  G   }  form a partition of   χ . In this paper, we focus on the problem on finding a feasible trajectory respecting assigned priorities, i.e. a trajectory   x   : [0 , T   ]   →   χ free  G   respecting constraints   C  such that   s (0) =   s init   and   x ( T   )   ∈   χ goal . IV. M OTION PLANNER WITH ASSIGNED PRIORITIES  The key idea is that if robots wait to be at the boundary of the collision region to brake (as it is the case without dynamic constraints in [12]), collisions will occur because robots   can   not   stop   instantly.   That   is   why   we   need   to anticipate the approach of the collision region. This can be done introducing two virtual trajectories as follows.  A. Introducing maximal and minimal trajectories  The minimal (resp. maximal) trajectory from state   s 0 , denoted   x ( s 0 , t )   (resp.   x ( s 0 , t ) ), are defined bellow:  x ( s 0 , t )   =   min χ reach ( s 0 , t )  x ( s 0 , t )   =   max χ reach ( s 0 , t )  These are the lower and upper bounds of the hypercube  χ reach ( s 0 , t ) . One can view the minimal trajectory as a brak- ing trajectory, and the maximal trajectory as an accelerating trajectory. The concepts are illustrated by Figure 5 where the kinodynamic constraints have the special following form:  C acc  i   =  {  ( x i , x ′  i , x ′′  i   )  ∣ ∣ ∣ ∣   0   ≤   x ′  i   ≤   v max  i  a min  i   ≤   x ′′  i   ≤   a max  i  }  (4)
x'   i x"   i vmax a   max a   min C   i xi t xi   (t) t   0 x(s(t0),t-t   0   ) x(s(t0),t-t   0   ) Fig. 5.   The left drawing depicts an example of kinodynamic constraints where robots have uniform minimal/maximal velocity and acceleration along their paths. The right drawing depicts the corresponding minimal/maximal trajectories   x ( s ( t 0 ) , t )   and   x ( s ( t 0 ) , t ) .  B. The motion planner  The time   t   is first discretized, and the trajectory of the robots   x ( t )   is   computed   iteratively   as   described   in   the following Algorithm 1. Indeed, at every time-step   t , the trajectory up to time   t   + ∆ T   can be computed as follows:  •   Cycling through all robots, we select a particular robot  i   (line 5);  •   We compute a virtual path   χ virtual   (line 12) that would be followed by the robots if:  –   in the next time step, robot   i   accelerates as much as possible while all other robots decelerate as much as possible (lines 6-11);  –   afterwards, all robots including   i   brake as much as possible (see the second term of the concatenation at line 12)  •   If this virtual path is such that no collision and priorities are respected, it means there exists a failsafe maneuver such that robot   i   accelerates as much as possible in the next time step, and we let it do so. Otherwise, robot  i   must brake (lines 14-18). Thus, at each time-step   t , each robot   i   exclusively follows its maximal or minimal trajectory in the next time-step. The defined trajectory thus appears as a natural extension of the left-greedy trajectory introduced in [18], in the sense that in the absence of kinodynamic constraints ( p   = 1 ), it coincides with it. Indeed, in this case the robots can stop instantly and the block from Line 6 to Line 17 simply consists of checking that maximum speed during the next time-step is safe: if it is not the case the robot is stopped. Note also that if the state   s ( t )   is such that the braking trajectory from   s ( t )   is collision-free, the state   s ( t )   is not an ”Inevitable Collision State” (ICS), as defined in [24] because the braking trajectory is collision-free, that is, there exists a particular collision-free trajectory starting from state   s ( t ) .  C. Safety guarantees  The theorem below exhibits the safety guarantee provided by the proposed motion planner.  Theorem 1   (Safety guarantees) .   Assume that there exists some feasible trajectory respecting priorities defined by   G  and the initial state   s init   is such that the initial braking  Algorithm 1   The motion planner with assigned priorities  Input:   s init ,   feasible priority graph   G  function   MAXIMALLY A GRESSIVE T RAJECTORY  T   ←   0  s (0)   ←   s init  while   x ( T   )   / ∈   χ goal   do  5:   for   i   ∈ { 1 ...n }   do for   t   ∈   [0 ,   ∆ T   ]   do for   j   6   =   i   do   ̃ s j   ( t )   ←   s j   ( s ( T   ) , t )  end for  10:    ̃ s i ( t )   ←   s i ( s ( T   ) , t )  end for  χ virtual   ←   π ( ̃ s ([0 ,   ∆ T   ]))   ∪   x ( ̃ s (∆ T   ) ,   R + )  if   ∃ ( j, i )   ∈   E ( G )   s.t.   χ virtual   ∩ χ obs  j  i   6   =   ∅   then  s i ( T   + ∆ T   )   ←   s i ( s ( T   ) ,   ∆ T   )  15:   else  s i ( T   + ∆ T   )   ←   s i ( s ( T   ) ,   ∆ T   )  end if end for  T   ←   T   + ∆ T  20:   end while return   ( x ( t )) t =0 ··· T  end function  trajectory   x ( s init , t )   is collision-free. Then, for sufficiently small   ∆ T   , Algorithm 1 terminates and returns a (collision- free) feasible trajectory respecting priorities   G . Proof.   It is assumed that that there exists some feasible trajectory respecting priorities defined by   G , so that   G   is a feasible priority graph as defined in [12], and the trajectory cannot reach a deadlock configuration (for sufficiently small  ∆ T   ). Until   χ goal   is reached, at any time there is at least one robot   i 0   at a coordinate lower than   sup { x i 0   :   x   ∈   χ obs }  moving forward. Indeed if this was not true it would mean that the robots have reached a deadlock configuration. There is thus a lower bound, say   μ , for the distance travelled by some of the robots in a time-length   ∆ T   , depending on the constraints. This implies   x   necessarily reaches   χ goal   in finite time (of order at most   O ( n/μ ) ) and Algorithm 1 terminates. Now we prove that, at every time step, the braking trajec- tory from the current state is collision-free. We begin with a preliminary useful property, that is a direct consequence of the definition of   χ obs  j  i   and is easily seen on Figure 3.  Property 1.   Given   i, j   ∈ { 1 ...n }   and two configurations  x, y   ∈   χ   satisfying   y j   ≥   x j   and   y i   ≤   x i , we have:  x   ∈   χ free  j  i   ⇒   y   ∈   χ free  j  i   (5) The initial braking trajectory   x ( s init , t )   is assumed to be collision-free. Now, assume that for some   t 0   =   k ∆ T   ,  x ( s ( t 0 ) , t )   is collision-free. In the next time step, for any priority   ( j, i )   ∈   E ( G ) , there are two options for the robot with lower priority:
•   either   i   brakes as much as possible. The fact that  x ( s ( t 0 ) , t )   is   collision-free   with   χ obs  j  i   implies   that  x ( s ( t 0   + ∆ T   ) , t )   is also collision-free by Property 1, since we have for all   t   ≥   0 :  x i ( s ( t 0   + ∆ T   ) , t )   =   x i ( s ( t 0 ) , t   + ∆ T   )   (6)  x j   ( s ( t 0   + ∆ T   ) , t )   ≥   x j   ( s ( t 0 ) , t   + ∆ T   )   (7)  •   or   i   accelerates as much as possible; in this case, the virtual path   χ virtual   is collision-free with respect to  χ obs  j  i . Then, consider the state    ̃ s (∆ T   )   defined as:   ̃ s j   (∆ T   )   =   s j   ( s ( t 0 ) ,   ∆ T   )   (8)   ̃ s i (∆ T   )   =   s i ( s ( t 0 ) ,   ∆ T   )   (9) Since the virtual path   χ virtual   is collision-free with  χ obs  j  i ,   x ( ̃ s (∆ T   ) , t )   is also collision-free (see the second term in the concatenation at Line 12). It implies that  x ( s ( t 0   + ∆ T   ) , t )   is also collision-free by Property 1, since we have for all   t   ≥   0 :  x i ( s ( t 0   + ∆ T   ) , t )   =   x i ( ̃ s (∆ T   ) , t )   (10)  x j   ( s ( t 0   + ∆ T   ) , t )   ≥   x j   ( ̃ s (∆ T   ) , t )   (11) Hence,   at   every   time   step,   the   braking   trajectory   is collision-free.   Now,   we   prove   that   there   is   no   collision between time steps. Again, at every time step   t 0   =   k ∆ T   , the are two options:  •   either   i   brakes as much as possible. The fact that  x ( s ( t 0 ) , t )   is collision-free with   χ obs  j  i   implies that for  t   ∈   [ t 0 , t 0   + ∆ T   ] ,   s ( t )   is also collision-free by Prop- erty 1, since we have for all   t   ∈   [ t 0 , t 0   + ∆ T   ] :  x i ( t )   =   x i ( s ( t 0 ) , t )   (12)  x j   ( t )   ≥   x j   ( s ( t 0 ) , t )   (13)  •   or   i   accelerates as much as possible; in this case, the virtual path   χ virtual   is collision-free with respect to  χ obs  j  i . Then, consider the trajectory    ̃ x ( t )   for   t   ∈   [0 ,   ∆ T   ]  defined as:   ̃ x j   ( t )   =   x j   ( s ( t 0 ) , t )   (14)   ̃ x i ( t )   =   x i ( s ( t 0 ) , t )   (15) Since   χ virtual   is collision-free with   χ obs  j  i ,    ̃ x ( t )   is also collision-free (see the first term in the concatenation at Line 12). It implies that   s ( t )   is also collision-free for  t   ∈   [ t 0 , t 0   + ∆ T   ]   by Property 1, since we have:  s i ( t )   =    ̃ s i ( t   −   t 0 )   (16)  s j   ( t )   ≥    ̃ s j   ( t   −   t 0 )   (17) As a result,   x   is collision-free with respect to   χ obs  G   at every time-step and reaches   χ goal : it is a collision-free feasible trajectory respecting priorities   G . V. S IMULATIONS  The algorithms presented in this paper have been imple- mented into a simulator coded in Java. Our algorithms have proved their ability to run in real-time.  A. Setting and results  Only straight paths are implemented (for simplicity’s sake) and   all   robots   are   supposed   to   be   circle-shaped   with   a common radius   R . The kinodynamic constraints of the robots concern only the maximal velocity and minimal/maximal acceleration.   Moreover,   all   robots   are   supposed   to   have identical kinodynamic constraints.  ∀ i   ∈ { 1 ...n } , C acc  i   =  {  ( x i , x ′  i , x ′′  i   )  ∣ ∣ ∣ ∣   0   ≤   x ′  i   ≤   v max  a min   ≤   x ′′  i   ≤   a max  }  . (18) In the simulation results presented in this section, we take as priority assignment policy the maximally aggressive priority assignment policy that consists for every robot of taking priority over another robot if it reaches the conflicting region first. This priority assignment policy can lead to deadlock configurations (see [12]), but with a very small probability in case of low traffic density as in the presented simulations. This policy is used for the sake of simplicity, the priority assignment policy not being the focus of this paper. Simulations   have   been   carried   out   for   the   4-path- intersection depicted in Figure 7. At full speed, the distance travelled in one time-step is   R   and at full acceleration, 20 time-steps are required for the robots to reach full speed. Figure 6 depicts the increase in travel time for different traffic densities. The increase in travel time is the delay due to coordination, i.e. the difference with the ideal travel time which is the travel time of robots in the absence of other robots. It is expressed in percentage of the ideal travel time. The increase in travel time vanishes as the density approaches 0 since it becomes very unlikely that they need to coordinate to avoid collisions. The traffic density in percentage is the ratio between the actual traffic density and the maximum traffic density (continuous flow of robots). The robots are generated randomly at a constant rate over time. The video of the simulation for a traffic density of 10% is available at  http://youtu.be/bJHdf3AbIlI . 10.0% 15.0%  Increase in travel time (%)  0.0% 5.0% 0.0%   2.0%   4.0%   6.0%   8.0%   10.0%  Increase in travel time (%)  Traffic density (%) Traffic density (%)  Fig. 6.   Simulation results: plot of the averaged increase in travel time against the traffic density for the intersection of Figure 7
Fig. 7.   The 4-path-intersection used for simulations  B. Comments  First of all, our algorithm succeeds to work in real time, and one can observe in simulations (notably on the video) that collisions never occur. This confirms the fact that the planner guarantees safety under dynamic constraints. One can see in Figure 6 that at a traffic density of   10%   on each path, the increase in travel time due to coordination to avoid other robots is less than   15%   which seems a low price to pay to ensure safe coordination. Note that we do not present simulation results at higher traffic densities because it would require to define a more complex priority assignment policy (at least to avoid deadlocks), which is a challenge in itself, and beyond the scope of the present paper. VI. C ONCLUSIONS AND DISCUSSION  The   results   presented   in   this   paper   prove   that   when priorities are assigned, it is possible to plan a safe and quite efficient trajectory respecting the priority graph and the dynamic constraints of the robots. The use of the braking trajectory enables to anticipate the need to brake just-in-time, and as a byproduct provides robustness guarantees since there exists a collision-free braking maneuver at any time. If the robots drift from the planned trajectory but if no priority has been violated, it is possible to run the motion planner   from   a   new   initial   state   to   get   a   new   feasible trajectory respecting the assigned priorities. This reflects that the method proposed in this paper is inherently a feedback motion planning approach (see [17], chapter 8). We are currently turning the planning algorithms of this paper into a feedback control law that aims at coordinating robots with assigned priorities. The idea is to define a control law   g G ( s )  that maps every state   s   to the control to apply in the next time step. The control law   g G   is in charge of coordination, ensuring that collisions are avoided and that priorities   G   are respected. Robots do not have to follow precisely a planned trajectory, they just have to be aware of the priorities and to respect the control law. The benefit of the approach is that it ensures safe coordination as long as priorities   G  are respected, which is much easier to robustly ensure than following precisely a planned trajectory. This opens avenues to build multiple robot coordination systems much more robust with regards to uncertainty in control and sensing. R EFERENCES [1]   J. Treat, N. Castellan, R. Stansifer, R. Mayer, R. Hume, D. Shinar, S. McDonald, and N. Tumbas,   Tri-level Study of the Causes of Traffic Accidents: Final Report. Volume I: Causal Factor Tabulations and Assessments , 1977. [2]   NCSA, “National center for statistics and analysis, traffic safety facts 2003,” U.S. DOT, Washington, DC, Tech. Rep., 2004. [3]   K.   Dresner   and   P.   Stone,   “Multiagent   traffic   management:   A reservation-based intersection control mechanism,” in   Proceedings of the Third International Joint Conference on Autonomous Agents and Multiagent Systems-Volume 2 , july 2004, pp. 530 –537. [4]   ——, “Mitigating catastrophic failure at intersections of autonomous vehicles,” in   AAMAS Workshop on Agents in Traffic and Transporta- tion , Estoril, Portugal, May 2008, pp. 78–85. [5]   A. Colombo and D. D. Vecchio, “Efficient algorithms for collision avoidance at intersections,”   Hybrid Systems: Computation and Control , 2012. [6]   I. Zohdy and H. Rakha, “Optimizing driverless vehicles at intersec- tions,” in   10th ITS World Congress Vienna, Austria , October 2012. [7]   H. Kowshik, D. Caveney, and P. Kumar, “Provable systemwide safety in intelligent intersections,”   IEEE Transactions on Vehicular Technol- ogy , vol. 60, no. 3, pp. 804 –818, march 2011. [8]   O. Mehani and A. De La Fortelle, “Trajectory planning in a cross- roads for a fleet of driverless vehicles,” in   Proceedings of the 11th international   conference   on   Computer   aided   systems   theory ,   ser. EUROCAST’07.   Berlin, Heidelberg: Springer-Verlag, 2007, pp. 1159–1166. [9]   Ismail and Sahin, “Railway traffic control and train scheduling based oninter-train conflict management,”   Transportation Research Part B: Methodological , vol. 33, no. 7, pp. 511 – 534, 1999. [10]   C. Tomlin, G. Pappas, and S. Sastry, “Conflict resolution for air traffic management: a study in multiagent hybrid systems,”   IEEE Transactions on Automatic Control , vol. 43, no. 4, pp. 509 –521, apr 1998. [11]   K. Kant and S. W. Zucker, “Toward efficient trajectory planning: The path-velocity decomposition,”   International Journal of Robotics Research , vol. 5, no. 3, pp. 72–89, 1986. [12]   J. Gregoire, S. Bonnabel, and A. De La Fortelle, “Optimal cooper- ative motion planning for vehicles at intersections,” in   Navigation, Perception, Accurate Positioning and Mapping for Intelligent Vehicles, Workshop, 2012 IEEE Intelligent Vehicles Symposium , 2012. [13]   S. Leroy, J. P. Laumond, and T. Simeon, “Multiple path coordination for mobile robots: A geometric algorithm,” in   Proceedings of the International Joint Conference on Artificial Intelligence (IJCAI) , 1999, pp. 1118–1123. [14]   S. LaValle and S. Hutchinson, “Optimal motion planning for multiple robots having independent goals,” in   Proceedings of the IEEE Inter- national Conference on Robotics and Automation, 1996 , vol. 3, apr 1996, pp. 2847 –2852 vol.3. [15]   J.-C. Latombe,   Robot Motion Planning .   Norwell, MA, USA: Kluwer Academic Publishers, 1991. [16]   T. Lozano-Perez, “Spatial planning: A configuration space approach,” 1980. [17]   S. M. LaValle,   Planning Algorithms .   Cambridge, U.K.: Cambridge University Press, 2006, available at http://planning.cs.uiuc.edu/. [18]   R. Ghrist and S. M. Lavalle, “Nonpositive curvature and pareto optimal coordination of robots,”   SIAM Journal on Control and Optimization , vol. 45, pp. 1697–1713, November 2006. [19]   J. Peng and S. Akella, “Coordinating multiple robots with kinody- namic constraints along specified paths,”   The International Journal of Robotics Research , vol. 24, no. 4, pp. 295–310, 2005. [20]   T. Fraichard and C. Laugier, “Planning movements for several coordi- nated vehicles,” in   IEEE/RSJ International Workshop on Intelligent Robots and Systems ’89. The Autonomous Mobile Robots and Its Applications. IROS ’89. , sep 1989, pp. 466 –472. [21]   C.   Hafner,   Cunningham   and   D.   Vechhio,   “Automated   vehicle-to- vehicle collision avoidance at intersections,” 2011. [22]   S. Akella and S. Hutchinson, “Coordinating the motions of multiple robots with specified trajectories,” in   Proceedings of the IEEE Inter- national Conference on Robotics and Automation, 2002. ICRA ’02. , vol. 1, 2002, pp. 624 – 631 vol.1. [23]   P. O’Donnell and T. Lozano-Periz, “Deadlock-free and collision-free coordination of two robot manipulators,” in   Proceedings of the IEEE International Conference on Robotics and Automation, 1989. , may 1989, pp. 484 –489 vol.1. [24]   T. Fraichard and H. Asama, “Inevitable collision states - a step towards safer robots?”   Advanced Robotics -Utrecht- , vol. 18, no. 10, pp. 1001– 1024, 2004.