Jean Gregoire
email: jean.gregoire@mines-paristech.fr

Silvère Bonnabel
email: silvere.bonnabel@mines-paristech.fr

Arnaud De

La Fortelle

Priority-based coordination of robots

Keywords: multiple robots, coordination, motion planning, control, cooperative, robustness, priority graph

This paper addresses the problem of coordinating multiple robots travelling through an intersection along fixed paths with positive velocities and kinodynamic constraints. The approach relies on a novel tool: a priority graph that encodes the relative order of the robots at the intersection. The overall planning approach can be decomposed into two key components as follows. The entry of robots into the intersection is managed by an intersection controller that assigns priorities. Within the intersection area, robots are controlled by a control law that preserves assigned priorities, avoids collisions, and is robust to unexpected decelerations of some robots occurring randomly.

Introduction

We consider the problem of coordinating a collection of cooperative robots at an intersection area, motivated by applications such as coordinating a fleet of automated guided vehicles in a factory, or automated cooperative vehicles in a fully automated transportation system. Due to the promises in autonomous cars design, automated intersection management has attracted much interest recently [START_REF] Zohdy | Optimizing driverless vehicles at intersections[END_REF][START_REF] Dresner | A multiagent approach to autonomous intersection management[END_REF]. Two main goals motivate the research in this topic. The first one is to avoid accidents due to collisions that occur particularly at intersections and because of human error (the leading factor in most of road accidents). The second one is to enhance road traffic efficiency, given that intersections represent bottlenecks in the traffic network. Intelligent cooperative vehicles are expected to reduce congestion, which is one of the major problems in today's metropolitan transportation networks.

Literature review

Multiple robot coordination with unconstrained paths is a problem of high combinatorial complexity. In [START_REF] Kant | Toward efficient trajectory planning: The pathvelocity decomposition[END_REF], a path-velocity decomposition allowing to reduce the problem's complexity was first proposed. In this setting, each robot is assumed to move along a predefined path and then the velocity profiles of the robots along their assigned paths are optimized. The problem is thus decomposed into a trajectory tracking problem via a low-level controller, and a high-level planning problem, the latter leading to the realm of fixed-path coordination algorithms (see, e.g., [START_REF] Lavalle | Planning Algorithms[END_REF]). The configuration of each robot boils down to its curvilinear position on its path and the configuration space of the whole system is then called the coordination space. It is a n-dimensional space where n denotes the number of robots going through the intersection. To prevent collisions between robots, some configurations of the coordination space must be excluded: they constitute the so-called obstacle region. The approaches based on the configuration space turn the motion planning problem into the geometric problem of searching a collision-free path for a composite robot in a n-dimensional space. The approach has since become standard in motion planning, see e.g., [START_REF] Leroy | Multiple path coordination for mobile robots: A geometric algorithm[END_REF][START_REF] Lozano-Perez | Spatial planning: A configuration space approach[END_REF][START_REF] Fraichard | Planning movements for several coordinated vehicles, in: Intelligent Robots and Systems '89[END_REF]. In the coordination problem, the obstacle region has a cylindrical shape [START_REF] Lavalle | Planning Algorithms[END_REF][START_REF] Leroy | Multiple path coordination for mobile robots: A geometric algorithm[END_REF]. Reference [START_REF] Ghrist | Nonpositive curvature and pareto optimal coordination of robots[END_REF] studies the problem of finding Paretooptimal trajectories for the coordination problem. It highlights the existence of locally Pareto-optimal trajectories in each homotopy class of trajectories. However, enumerating locally optima in each homotopy class to find a globally optimal trajectory is a problem of high combinatorial complexity. That complexity led researchers to develop the so-called prioritized motion planning method.

First introduced in [START_REF] Erdmann | On multiple moving objects[END_REF], prioritized motion planning avoids the complexity of searching a trajectory in the n-dimensional coordination space. Instead, it consists of planning the trajectory of each robot sequentially: robots for which motion has already been planned are considered as dynamic obstacles. The approach has been widely and successfully utilized since then, see e.g., [START_REF] Van Den | Prioritized motion planning for multiple robots[END_REF]. Even if prioritized motion planning is not explicitly mentioned, the approach of [START_REF] Dresner | A multiagent approach to autonomous intersection management[END_REF][START_REF] Mehani | Trajectory planning in a crossroads for a fleet of driverless vehicles[END_REF] for autonomous intersection management also belongs to the family of prioritized motion planning, because the trajectory of robots are planned sequentially. The collision-time approach of [START_REF] Peng | Coordinating multiple robots with kinodynamic constraints along specified paths[END_REF] is an extension for robots constrained to follow paths with a fixed velocity profile.

In motion planning, the planning phase and the control phase are generally decoupled, as a low-level feedback controller is assumed to track the trajectory computed by the planner. Uncertainty is thus often handled during the control phase. Feedback motion planning is a way to take into account uncertainty in an implicit way at the planning level (see [START_REF] Lavalle | Planning Algorithms[END_REF], chapter 8). The motion plan is defined by the feedback plan that returns the action to apply given the current state of the system. The approach has been widely used for multiple robot motion planning using potential field functions from which the control vector field is derived (see, e.g. [START_REF] Loizou | Closed loop navigation for multiple holonomic vehicles[END_REF]). Even when global convergence to destination can be guaranteed, the weakness of the method resides in its reactive nature that can lead the system to configurations that are not efficient in terms of travel time.

Finally, the theory of multiagent systems also discusses the problem of multiagent coordination [START_REF] Vlassis | A concise introduction to multiagent systems and distributed artificial intelligence[END_REF]. The multiple robot motion planning problem can be considered as a coordination problem where robots are collaborative agents. Individual decisions (planned trajectories) should be taken in a manner that results in a good joint utility for the fleet of robots. The key tool introduced in the theory of multi agent systems is the coordination graph. It represents, locally, the interaction between the actions of robots in the utility function. The coordination problem then boils down to solving several smaller subproblems. The approach is utilized in [START_REF] Bekris | Safe and distributed kinodynamic replanning for vehicular networks[END_REF] for multiple robot distributed coordination. These methods highlight the benefit of defining a graph to represent the interaction between robots and defining relative orders. The relevance of coordination graphs combined with the cylindrical structure of the coordination space led us to develop the priority-based framework exploited in the present paper.

Contributions and organization

In the present paper, we aim at building a multiple robot coordination system at an intersection area that yields efficient trajectories in terms of travel time and is robust with respect to large deviations due to unexpected events. Prioritized motion planning methods build collision-free and efficient trajectories respecting kinodynamic constraints, but if there are large deviations from the planned trajectories, safety cannot be guaranteed any more: these methods require precise trajectory tracking control. We propose to use a prioritized motion planning approach only to plan an efficient relative order of the robots through the intersection. However, the precise trajectory is not planned in advance. A control law (mapping the current state to a control action) ensures collision avoidance and the preservation of the planned relative order: this is a feedback motion planning approach. It relies on a novel tool: a priority graph that encodes the relative order of the robots. The overall planning approach can be decomposed into two key components as follows. The entry of robots into the intersection is managed by an intersection controller that assigns priorities. Within the intersection area, robots are controlled by a control law that preserves assigned priorities, avoids collisions, and is robust to unexpected decelerations of some robots occurring randomly.

In Section 2, we recall the priority-based framework introduced in the preliminary conference paper [START_REF] Gregoire | Optimal cooperative motion planning for vehicles at intersections[END_REF]. The benefit of the approach is that it decomposes the coordination problem into a discrete problem (priority assignment, i.e. choosing a priority graph), and a continuous problem (controlling robots with assigned priorities). A solution to the continuous problem is provided in Section 3 for robots whose acceleration can be controlled. The control law ensures collision avoidance and preserves assigned priorities. Moreover, the state of the robots remains brake safe, which guarantees that at any point of time, a robot may brake independently from other robots' decisions without causing any collision. This latter fact is proved using the theory of monotone systems [START_REF] Angeli | Monotone control systems, Automatic Control[END_REF], and makes the coordination system robust to a large class of perturbations. The proposed control law can be viewed as an extension of the two-vehicle coordination system proposed in [START_REF] Del | A separation principle for a class of hybrid automata on a partial order[END_REF], made possible by the introduction of the priority graph. In Section 4, we propose an overall coordination system which priority assignment policy is inspired from the prioritized motion planning approaches. Robots are required to request an intersection controller the right to enter a control area. The intersection controller assigns priorities. Once accepted into the control area, robots are controlled by the control law of Section 3. To the authors' knowledge, this is the first multiple robot coordination system combining a global motion planning approach (for priority assignment) with a feedback control law (for control under assigned priorities). The theoretical benefits in terms of robustness are illustrated by qualitative simulation results presented in Section 5.

The priority-based framework

Consider the problem of coordinating the motion of a collection of robots R in a two-dimensional space. Every robot i ∈ R follows a particular path γ i ⊂ R 2 and we let x i ∈ R denote its curvilinear coordinate along the path (see Figure 1). x := (x i ) i∈R indicates the configuration of all robots. x ∈ χ := R n where n denotes the number of robots going through the intersection. The configuration space χ is known as the coordination space [START_REF] O'donnell | Deadlock-free and collision-free coordination of two robot manipulators[END_REF]. In the rest of the paper, {e i } 1≤i≤n denotes the canonical basis of χ. Some configurations must be excluded to avoid collisions between robots (see Figure 2). The obstacle region χ obs is the open set of all collision configurations. χ free := χ\χ obs denotes the obstacle-free space. Letting χ obs ij denote the set of configurations where i and j collide, it is easily seen that χ obs ij is an (open) cylinder, and the obstacle region merely appears as the union of n(n -1)/2 cylinders, that is, χ obs = ∪ {i,j} χ obs ij [START_REF] Lavalle | Planning Algorithms[END_REF]. Every cylinder χ obs ij is assumed to have an open bounded convex cross-section (in the plane generated by e i and e j ). The boundedness condition on χ obs is rather technical but ensures that the whole intersection lies in a bounded region: the intersection area. We assume χ obs = ∅ (otherwise, coordination is not required), so the boundedness condition ensures that inf χ obs and sup χ obs both exist. In this paper, we will only consider motions where all robots have a nonnegative velocity at all times: non-positive velocities would result in robots that move backwards in the intersection area. Paths in the coordination space satisfying the non-negative velocity assumptions, starting from an initial configuration lower than inf χ obs and ending at a final configuration greater than sup χ obs , will be called feasible. Because of the non-negative velocity constraint, for every couple of robots with a non-empty collision region, one of the robots necessarily passes "before" or "after" the other one. In the coordination space, a feasible path passes below or above the collision cylinder as depicted in Figure 3 where ϕ(t) denotes a feasible motion in the coordination space. This reflects the intuitive notion of priority. Let χ obs i j ⊂ χ denote the set defined below:

χ obs i j := χ obs ij -R + e i + R + e j (1)
Figure 3 illustrates the sets χ obs i j and χ obs j i . The geometry of the coordination space thus leads us to define a natural binary relation corresponding to priority relations between robots: a very familiar and intuitive notion in real life. Any feasible path induces a binary relation on the set R as follows. For i = j s.t. χ obs ij = ∅, i j if the path is collision-free with χ obs i j : we say robot i has priority over robot j. We let the priority graph be the oriented graph G whose vertices are V (G) := R 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. When priorities are assigned, for each priority i j, the fixed-priority collision cylinder χ obs i j must be avoided. Given a priority graph G, the collision region with regards to priorities defined by G is merely defined as:

χ obs G := (i,j)∈E(G) χ obs i j (2)
It is natural to define χ free i j := χ \ χ obs i j and χ free G := χ \ χ obs G , so that {χ obs G , χ free G } form a partition of χ. We say a feasible path respects priorities G if it takes values in χ free G .

A priority-preserving control law

In this section, we assume the acceleration of the robots can be controlled, and we propose a (centralized) control law aimed at coordinating multiple robots with assigned priorities and kinodynamic constraints in the intersection area. The method is inspired by the works [START_REF] Del | A separation principle for a class of hybrid automata on a partial order[END_REF][START_REF] Colombo | Efficient algorithms for collision avoidance at intersections[END_REF][START_REF] Kowshik | Provable systemwide safety in intelligent intersections[END_REF] for the coordination of two vehicles (the priority graph being then reduced to one oriented edge).

The multiple robot system as a monotone controlled system

Each robot i is modelled as a second-order controlled system with state

s i = (x i , v i ) ∈ S i := R × [0, v i ],
whose evolution is described by the differential equation:

ẋi (t) = v i (t) (3)
vi (t) = u i (t) δ(u i (t), v i (t)) (4)
where u i : R + → U i is the control of robot i and v i denotes the non-negative speed limit for robot i. We let U i := [u i , u i ] be the set of feasible control values. u i < 0 represents the maximum brake control value and u i > 0 represents the maximum throttle control value. δ is a binary function merely ensuring that

v i ∈ [0, v i ] at all times, that is, δ(u i (t), v i (t)) = 1 except for v i (t) = 0 and u i (t) < 0, and for v i (t) = v i and u i (t) > 0, where it vanishes.
The control is assumed to be updated in discrete time every ∆T > 0:

∀k ∈ N, ∀t ∈ [k∆T, (k + 1)∆T ), u i (t) ≡ u i (k∆T ) (5)
The time interval [k∆T, (k + 1)∆T ) will be referred to as (time) slot k. For the sake of simplicity we let ∆T := 1 in the sequel. We let U i denote the set of controls u i : R + → U i piecewise constant on intervals [k, k + 1), k ∈ N. We let t → Φ i (t, s i , u i ) denote the flow of the system starting at initial condition

s i ∈ S i with control u i ∈ U i .
We also define the vectorial state s := (s i ) i∈R ∈ S, the vectorial control u := (u i ) i∈R ∈ U := i∈R U i , and the vectorial flow: Φ(t, s, u) := (Φ i (t, s i , u i )) i∈R . We let u := (u i ) i∈R , u := (u i ) i∈R and we define the constant controls u(t) := u and u(t) := u. We introduce partial orders as follows:

∀u 1 i , u 2 i ∈ U i , u 1 i u 2 i if ∀t ≥ 0, u 1 i (t) ≤ u 2 i (t) (6) ∀s 1 i = (x 1 i , v 1 i ), s 2 i = (x 2 i , v 2 i ) ∈ S i , s 1 i s 2 i if x 1 i ≤ x 2 i and v 1 i ≤ v 2 i (7) ∀Φ 1 , Φ 2 : R + → S, Φ 1 Φ 2 if ∀t ≥ 0, Φ 1 (t) Φ 2 (t) (8)
The controlled system (3)-( 4) is a monotone control system [START_REF] Angeli | Monotone control systems, Automatic Control[END_REF] with regards to the relative orders defined above. More precisely, the following key property holds:

Property 1 (Order preservation). The flow t → Φ i (t, s i , u i ) is order-preserving with regards to s i and u i .

Note that in our open loop model, control u i only acts on robot i, that is, u is a collection of independent controls: it does not achieve any kind of coordination between the robots. The control law introduced in the sequel is precisely aiming at coordinating the robots to avoid collisions and respect priorities.

The proposed control law

We define projection operators as follows: π x (s) := x and π x,i (s) := π x,i (s i ) := x i . G denotes a given priority graph. Define the set of brake safe states as follows:

B G := {s ∈ S : π x (Φ (R + , s, u)) ⊂ χ free G } ⊂ S (9)
According to the above definition, a state s ∈ S is brake safe if, starting at initial condition s under maximum brake control, the system remains in χ free G . In particular, a state (x, 0) with x ∈ χ free G is brake safe, so B G is not empty provided χ free G is not empty. Brake safety is more conservative than remaining in the escape set proposed in [START_REF] Del | A separation principle for a class of hybrid automata on a partial order[END_REF], which includes all states from which there exists at least one control (not necessarily u) avoiding future collisions. It is also more conservative than not entering an inevitable collision state as defined in [START_REF] Fraichard | Inevitable collision states -a step towards safer robots?[END_REF] where neither the geometric path in R 2 nor the control to avoid collisions are fixed. In the context of multiple robot coordination, checking whether a state is an inevitable collision state, and computing the escape set are of high computational complexity [START_REF] Colombo | Efficient algorithms for collision avoidance at intersections[END_REF]. In contrast, verifying that a state is brake safe consists of computing a finite time single flow and checking if collisions occur for all pairs of robots, yielding a quadratic complexity. Moreover, constraining the state to be brake safe at all times comes with robustness properties, as at any time, all robots can be stopped with maximum brake control without colliding. Now, we propose to build a control law g G : S → U such that starting from an initial brake safe state in B G , the flow of the system controlled by the control law g G is ensured to remain in B G (thus being collision-free and respecting priorities G). In other words, using the terminology of [START_REF] Kerrigan | Robust constraint satisfaction: Invariant sets and predictive control[END_REF], B G shall be positively invariant for the system under control law g G . The liveness property of the control law, i.e. its ability to let each robot reach its goal will be studied in Section 4 and we first focus on collision avoidance.

The rationale for our control law is as follows. Consider a robot i and a robot j that has priority over i. Given an initial configuration of the two robots, the worst-case scenario is when j brakes whereas i accelerates in the next time slot. If the trajectory of the system in the next time slot under that worstcase scenario is collision-free and if the reached state is brake safe, robot i may accelerate in any case. Otherwise, it is required to brake. This is formalized below.

Let u impulse i ∈ U i denote the impulse control for robot i defined by:

u impulse i (k) := u i if k = 0 u i if k ≥ 1 (10)
Now let ũi denote the worst-case vectorial control with regards to i defined componentwise by:

ũi j := u impulse i if j = i u j if j = i (11)
The control law can then be formulated synthetically:

g G i (s) := u i if ∃(j, i) ∈ E(G) s.t. π x (Φ(R + , s, ũi )) ∩ χ obs j i = ∅ u i else. ( 12
)
This simply means that robot i applies maximum throttle command unless the worst-case flow t → Φ(t, s, ũi ) intersects χ obs G at some point of time t ≥ 0, in which case it applies maximum brake command. Now, in order to present our first main result, we need to introduce the following notation. Given a feedback control law h : S → U , with a slight abuse of notation we let t → Φ(t, s, h) denote the vectorial flow of the system starting at initial condition s ∈ S and controlled by u ∈ U satisfying:

∀k ∈ N, u(k) ≡ h(Φ(k, s, u)) (13)
Theorem 1 (Control law safety). The set of brake safe states B G is positively invariant (in discrete time) for the system under control law g G , i.e.:

∀s ∈ B G , ∀k ∈ N, Φ(k, s, g G ) ∈ B G (14)
Moreover, the configuration of the system remains in χ free G through time, i.e.:

∀s ∈ B G , ∀t ≥ 0, π x (Φ(t, s, g G )) ∈ χ free G ( 15
)
The above theorem asserts that under control law g G , provided the system starts in a brake safe state, the sequence of future states at the beginning of each time slot is a sequence of brake safe states (see Equation ( 14)). Moreover, the flow of the system remains in χ free G in continuous time (see Equation ( 15)), i.e. no collision occurs and priorities are preserved. It is a direct consequence of Theorem 2 and appears as a limiting case.

Robustness issues

The control law g G i returns the maximum control value that robot i can safely apply, but it is in fact always safe to apply a lower control value, including letting all vehicles brake as much as possible, i.e. leading to an emergency stop. This property stated in Theorem 2 below is very valuable because for applications in intelligent transportation systems, even without considering extreme situations such as emergency stops, it is very usual that a vehicle needs to brake because of an unpredictable event such as a pedestrian crossing the road, or delay or even loss of communication.

Theorem 2 (A broad class of priority-preserving controls). Given an initial condition s ∈ B G , and a control u ∈ U that satisfies:

∀k ∈ N, u(k) ≤ g G (Φ(k, s, u)) (16)
The set of brake safe states B G is positively invariant (in discrete time), i.e.:

∀k ∈ N, Φ(k, s, u) ∈ B G (17)
Moreover, the configuration of the system remains in χ free G through time, i.e.:

∀t ≥ 0, π x (Φ(t, s, u)) ∈ χ free G ( 18
)
Proof. By induction, it is sufficient to prove that given an initial condition s ∈ B G , the flow is collision-free for t ∈ [0, 1] and the reached state Φ(1, s, u) is brake safe. We begin with a preliminary useful property, that is a direct consequence of the definition of χ obs j i in Equation ( 1) and is easily seen on Figure 3. Property 2. Given i, j ∈ R 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 (19)
Now, we prove that the flow of Theorem 2 does not intersect χ obs G for t ∈ [0, 1]. Take arbitrary t ∈ [0, 1]: we have to prove that for all (j, i) ∈ E(G), π x (Φ(t, s, u)) ∈ χ free j i . By construction of g G , for each robot i, there are two cases:

• g G i (s) = u i : in this case, Φ i (t, s, u) = Φ i (t, s, u) (20)
and by order-preservation, for all robots j such that (j, i) ∈ E(G) we have:

Φ j (t, s, u) ≥ Φ j (t, s, u) (21)
Since s is brake safe, π x (Φ(t, s, u)) ∈ χ free j i . Hence, by Property 2, Equations ( 20) and ( 21) ensure that π x (Φ(t, s, u)) ∈ χ free j i as well.

• g G i (s) = u i : by construction of the control law, π x (Φ(t, s, ũi )) ∈ χ free G . By order-preservation, using ũi i (0) = u i , we obtain:

Φ i (t, s, ũi ) = Φ i (t, s, u) ≥ Φ i (t, s, u) (22)
For all robots j such that (j, i) ∈ E(G), using ũi j (0) = u j , we have:

Φ j (t, s, ũi ) = Φ j (t, s, u) ≤ Φ j (t, s, u) (23)
Since π x (Φ(t, s, ũi )) ∈ χ free G , π x (Φ(t, s, ũi )) ∈ χ free j i , and by Property 2, Equations ( 22) and ( 23) ensure that π x (Φ(t, s, u)) ∈ χ free j i as well.

As a final step, let us prove that the reached state s 1 := Φ(1, s, u) is brake safe. Take arbitrary t ≥ 0: we have to prove that for all (j, i) ∈ E(G), π x (Φ(t, s 1 , u)) ∈ χ free j i . As previously, there are two cases:

• g G i (s) = u i : then, s 1 i = Φ i (1, s, u) and we have:

Φ i (t, s 1 , u) = Φ i (1 + t, s, u) (24)
Moreover, by order-preservation, for all j such that (j, i) ∈ E(G): s 1 j ≥ Φ j (1, s, u). As a result, by order-preservation:

Φ j (t, s 1 , u) ≥ Φ j (1 + t, s, u) (25)
Since s is brake safe, π x (Φ(1 + t, s, u)) ∈ χ free j i . Hence, by Property 2, Equations ( 24) and ( 25) ensure that π x (Φ(t, s 1 , u)) ∈ χ free j i as well.

• g G i (s) = u i : then, by construction of the control law,

π x (Φ(1 + t, s, ũi )) ∈ χ free G . Define s1 := Φ(1, s, ũi ). We have ũi (1 + τ ) = u for τ ≥ 0. As a result, Φ(1 + t, s, ũi ) = Φ(t, s1 , u). Since π x (Φ(1 + t, s, ũi )) ∈ χ free G , π x (Φ(t, s1 , u)) ∈ χ free
G . By order-preservation, using ũi i (0) = u i , we obtain:

s1 i = Φ i (1, s, ũi ) = Φ i (1, s, u) ≥ Φ i (1, s, u) = s 1 i ( 26
)
For all robots j such that (j, i) ∈ E(G), using ũi j (0) = u j , we have:

s1 j = Φ j (1, s, ũi ) = Φ j (1, s, u) ≤ Φ j (1, s, u) = s 1 j (27)
Hence, by order-preservation, Equations ( 26) and ( 27) imply:

Φ i (t, s1 , u) ≥ Φ i (t, s 1 , u) (28)
Φ j (t, s1 , u) ≤ Φ j (t, s 1 , u) (29)
Since π x (Φ(t, s1 , u)) ∈ χ free G , π x (Φ(t, s1 , u)) ∈ χ free j i , and by Property 2, Equations ( 28) and ( 29) ensure that π x (Φ(t, s 1 , u)) ∈ χ free j i as well.

To illustrate the interest of Theorem 2, given priorities G and an initial condition s ∈ B G consider the two examples below.

Example 1 (Individual brake application). Consider a control u ∈ U satisfying:

∀k ∈ N, u i (k) = u i if k ∈ K g G i (Φ(k, s, u)) else. ( 30
) ∀j ∈ R, j = i, u j (k) = g G j (Φ(k, s, u)) (31)
i ∈ R is a particular robot and K ⊂ N is a subset of slots. Under the control described above, the system is perfectly controlled by the control law, except during slots K where the particular robot i brakes while other robots are still perfectly controlled by the control law. Such a scenario may arise, for instance, in case of a momentary communication/sensing failure for one robot: if the current state is not available, the control law cannot be applied, and a brake manoeuvre is performed instead. The condition of Theorem 2 is clearly respected since for j = i, u j (k) = g G j (Φ(k, s, u)) ≤ g G j (Φ(k, s, u)), and

u i (k) = g G i (Φ(k, s, u)) ≤ g G i (Φ(k, s, u)) or u i (k) = u i ≤ g G i (Φ(k, s, u)).
Hence, the flow t → Φ(t, s, u) is collision-free and preserves priorities G. This illustrates that the control law is robust with regards to an individual brake application of a particular robot for an arbitrary long time, yielding a deviated but still collision-free flow respecting the assigned priorities.

Example 2 (Simultaneous brake application). Consider a control u ∈ U satisfying:

∀k ∈ N, u(k) = u if k ∈ K g G (Φ(k, s, u)) else. ( 32
)
Again, K ⊂ N is a subset of slots. Under the control described above, the system is perfectly controlled by the control law, except during slots K where all robots brake simultaneously. It may arise in case of a global failure requiring an emergency brake to be performed. Again, the condition of Theorem 2 is clearly respected since u

(k) = g G (Φ(k, s, u)) ≤ g G (Φ(k, s, u)) or u(k) = u ≤ g G (Φ(k, s, u)).
It illustrates that the control law is robust with regards to a simultaneous brake application of all robots for an arbitrary long time, yielding again a deviated but still collision-free flow respecting the assigned priorities.

Overall coordination system

Given a priority graph, we know now how to coordinate the robots without violating the priorities encoded by the graph, and without colliding. However, finding a sensible graph is in fact far from trivial. Many graphs can be infeasible, that is, there is no trajectory respecting the priorities defined by it (that is, the so-called liveness property is not satisfied). Moreover, even in the case it is feasible, some unwise priorities can result in a very inefficient coordination (think of a line of robots in front an empty intersection waiting some far away robot to pass first). There are 2 n(n-1)/2 possible graphs, and enumerating them all soon become numerically prohibitive as the number of robots in the intersection grows [START_REF] Ghrist | Nonpositive curvature and pareto optimal coordination of robots[END_REF]. As a result, the present section proposes a simple yet sensible way to assign the priorities, inspired by the work [START_REF] Dresner | A multiagent approach to autonomous intersection management[END_REF], and proves that the obtained graphs are feasible. Because of the inherent complexity of the problem, and to the fact that the intersection is open, meaning the number of robots keep varying, the proposed overall coordination system proposed in Subsections 4.2 and 4.3 has a more engineering flavour than what has been done so far in the present paper.

Sufficient condition for liveness: acyclic priorities

In Section 3 we have focused on collision avoidance by building a control law under which B G is positively invariant. Another key property in motion planning is liveness, i.e. the guarantee that every robot eventually reaches its goal. In the particular case of the problem studied here, every robot is expected to exit the obstacle region. Since it is bounded, liveness is guaranteed if every robot i ∈ R eventually reaches position sup x∈χ obs x i . This can only happens if there are no deadlocks, i.e. situations where all robots are blocked in the intersection and can not move anymore. Our preliminary work already noticed the role of cycles in deadlocks [START_REF] Gregoire | Optimal cooperative motion planning for vehicles at intersections[END_REF]. The following theorem proves that when the graph G is acyclic, and under the proposed control law g G , liveness is guaranteed.

Theorem 3 (Sufficient condition for liveness). Given an acyclic priority graph G and an initial condition s ∈ B G , robots eventually exit the intersection under control law g G , i.e. the flow under control law g G satisfies:

∃T > 0 : ∀i ∈ R, π x,i (Φ(T, s, g G )) > sup x∈χ obs x i (33)
Proof. Consider the trajectory of the robots under control law g G . G being acyclic, there exists an extremal vertex i 1 ∈ R such that for all j ∈ R, (j, i 1 ) / ∈ E(G). As a result, under the control law g G , robot i 1 will always accelerate as much as possible and it will exit the intersection in finite time T 1 . Now, assume that at time T m , robots i 1 • • • i m have exited the intersection and m < n (there remain some robots). G being acyclic, there exists an extremal element for the remaining robots denoted

i m+1 ∈ R \ {i 1 • • • i m } such that for all j ∈ R \ {i 1 • • • i m }, (j, i m+1 ) / ∈ E(G).
Collisions occurring only with non exited robots, for t ≥ T m j will always accelerate and it will exit the intersection in finite time at instant T m+1 ≥ T m .

Iterating this process for m = 1

• • • n -1 yields a sequence (T 1 • • • T n
) and all robots have exited the intersection at time T := T n . We define the control area as a subset of the two-dimensional real space in which the obstacle region wholly resides (see Figure 4). For each path, an entry position and an exit position are defined. Let x entry i denote the entry position for robot i and x exit i its exit position. Robot i is in the control area if

A simple priority assignment policy

x i ∈ [x entry i , x exit i ].
Every robot is considered as an agent, and a control area controller, called the intersection controller, manages the intersection. It updates the set R of robots accepted in the control area and the priority graph G. Time is slotted, and at every time slot, the current state s of the system is available to the intersection controller and to all robots.

The entry of the control area is managed using requests sent by robots to the intersection controller. Each robot i that has not already been accepted in the control area checks at every time slot whether accelerating (or maintaining maximum velocity) during the next time slot will inevitably result in an entry into the control area. If this is the case, entry must be requested to the intersection controller, and if the entry is denied, robot i must brake. To formulate this mathematically, we compute the final (and maximal) position reached by robot i with initial state s i under impulse control defined in Equation ( 10):

x stop i (s i ) := max π x,i (Φ i (R + , s i , u impulse i )) (34)
The condition to request entry then simply becomes x stop i (s i ) > x entry i . Once requests are received, the intersection controller chooses an order and processes the requests one by one. The idea is to spend as little time as possible in the intersection area, as noticed in [START_REF] Dresner | A multiagent approach to autonomous intersection management[END_REF]. Thus a robot is accepted into the control area only if it can travel with maximum throttle command and with lowest priority. The second point is key: assigning every accepted robot the lowest priority with regards to robots already accepted into the control area leads to a necessarily acyclic graph, enforcing liveness (see Theorem 3). This can be formulated as follows. Consider a robot i that requests the entry of the control area. To decide to accept it or not, a virtual trajectory is built that consists of applying control u i constantly to robot i while robots j = i follow the trajectory that they would have followed in the absence of i. Let s(τ ) denote this virtual trajectory, for all τ ≥ 0:

si (τ ) := Φ i (τ, s i , u i ) (35) ∀j ∈ R, sj (τ ) := Φ j (τ, s, g G ) (36)
Define the virtual priority graph that consists of adding to G edges (j, i) for all robots j ∈ R. If all the states travelled along the virtual trajectory are brake safe with regards to the virtual priority graph, the request is accepted.

Otherwise, the request is rejected. When the request is accepted, the priority graph is updated, and the newly accepted robot is assigned the lowest priority (the virtual priority graph becomes the current priority graph). Moreover, the robot is added to the set of robots accepted into the control area. Note that the described algorithm ensures the priority relation to be a relative order, that is G to be a directed acyclic graph at all times. Figure 5 displays a scheme of the overall coordination system. Robots accepted in the control area are controlled by the control law g G according to priorities G assigned by the intersection controller. Each robot is sequentially accepted into the control area by the intersection controller if it can go through the intersection at maximum throttle command, and if the travelled states are brake safe. As a result, if there is no control/sensing uncertainty and no unexpected event, the control law will always return u. However, the purpose of the control law is precisely to handle uncertainty implicitly and to be able to decelerate at some point if necessary to avoid collisions or priority violation. This justifies feeding back into the intersection controller the current state of the robots, that can much deviate from the trajectories under perfect control law.

Proposed overall coordination system

Illustration of the results through simulations

The following simulations provide qualitative results on the benefits of the proposed priority-based coordination system which combines a global planning approach (for priority assignment) and a feedback control law (for control under assigned priorities). The implementation of the simulator is completely centralized: communication aspects are not considered here.

The simulation setting

The algorithms presented in this paper have been implemented into a simulator coded in Java. Only straight paths are implemented and all robots are supposed to be circle-shaped with a common diameter D. This allows an easy computation of the obstacle region. Note that there is a finite set of possible paths for robots. As a result, the collision region between each couple of paths can be precomputed once and for all during the design phase of the intersection controller. Robots are generated at the origin of each path randomly at a constant rate. When generated, a robot i is positioned with zero velocity at the coordinate 0 of the path, or if there is already a robot j at position x j ≤ D, i is positioned at the coordinate x j -D. As noted in [START_REF] Dresner | A multiagent approach to autonomous intersection management[END_REF], maximizing the velocity of robots in the intersection minimizes the time spent within the collision region, yielding a better performance. Hence, to ensure that robots have a maximum velocity within the collision region, the entry of the control area is defined to be far enough from the collision region (in the simulation videos we obviously see the robots that are not already accepted in the control area stop way before potential collision configurations). The intersection controller maintains a predicted trajectory for robots accepted in the control area. This predicted trajectory corresponds to the virtual trajectory defined in Equation (36). For each robot i, the predicted trajectory is initialized when it enters the control area. When the robot is accepted, its predicted trajectory assumes maximum throttle command. However, due to unpredicted events (e.g., a robot stops in the control area), the predicted trajectory may need to be updated. An update of the predicted trajectory of robots in the control area is carried out periodically (every 20 time slots in the presented simulations). If the current state of robots in the control area is s and the current priority graph is G, an update of the predicted trajectory consists of computing the trajectory τ → Φ(τ, s, g G ). This update process is key because the entry management is efficient only if the available predicted trajectories for robots already accepted in the control area are precise enough.

Simulations under deterministic control

The experimental intersection is depicted in Figure 6. It is composed of eight straights paths. The maximum velocity of robots is such that a robot at maximum velocity travels D/2 (one radius) during one slot. All robots share the same kinodynamic constraints with u = -u and 20 slots are necessary to go from stop to full speed (and conversely). Hence, to ensure that robots are at maximum velocity when they reach the first potential collision configuration, the entry position is fixed at a distance 6D from the first potential collision configuration. Symmetrically, the exit position is fixed at a distance 6D after the last potential collision configuration. A video capture of the simulation for an arrival rate of 0.08 robots per time slot on each path is available1 . One can observe that robots not accepted in the control area stop at a distance equivalent to 6 robots before the first potential collision configuration. In this simulation, there is no uncertainty, and the video capture confirms that in the absence of uncertainty, the presented algorithms result in robots always at maximum throttle command inside the control area. Finally, note that the entry management of the control area is not a first come first serve policy. Some robots requesting the entry before another robot may be accepted into the control area after that robot.

The latter phenomenon is more obvious in the video capture of the simulation for an arrival rate of 0.16 robots per time slot2 . At such an arrival rate, queues are formed at the entry of the control area, but the size of the queues are not considered for processing the requests. Finally, note that queues are stable at this arrival rate which denotes an ergodic dynamics of the system.

At this point, note that the benefit of the priority-based approach is not visible, because in the absence of uncertainty, the control law under assigned priorities always returns u. In the remaining simulations, we focus on particular uncertainties that may result in significant deviations from the predicted trajectory.

Simulations with unexpected events

Here, to illustrate the robustness of the proposed coordination system with respect to unexpected events, we consider a scenario in which robots may decide to brake within the control area unexpectedly. At the beginning of every time slot, each robot i may switch from a controlled regime under the control law g G to an unexpected deceleration under constant control u i , and vice versa, with probability transitions displayed in Figure 7. The probability values p, q are Robot controlled with control law g i G Robot brakes p = 0.001 u i = u i q = 0.03 Figure 7: Non-deterministic transitions between control regimes chosen arbitrarily, as the goal is is not to reproduce a realistic scenario but to test and validate the robustness of the approach. One may consider transitions to brake control regime as modelling some unexpected events subject to occur in applications to transportation systems such as a loss of communication abilities or a pedestrian crossing the road, both requiring the vehicle to slow down unexpectedly.

A video capture of the simulation for an arrival rate of 0.08 robots per time slot on each path is available here3 . Even if some robots stop within the control area, other robots adapt and brake if necessary thanks to the control law. In contrast with simulations under deterministic control, the control law is useful here and enables to handle robots slowing down unexpectedly. No collision occurs during the simulation, the control law is effectively safe and robust with regards to brake application. We see that the priorities are satisfied, that no collision occurs, and that all vehicles eventually exit the intersection, although the trajectory may be very far from the trajectory under perfect control law.

Conclusions and perspectives

The key contribution of the present work is the introduction of a prioritypreserving control law in Section 3. It both ensures collision avoidance and the respect of assigned priorities (see Theorem 1). Priorities need to be assigned: this is the topic of Section 4 where an overall coordination system is proposed. It is inspired from previous works on prioritized motion planning. Every robot is accepted into the control area sequentially in a way that maximizes its velocity through the intersection. When the robot is accepted, it is assigned the lowest priority among robots already accepted, which yields acyclic priorities, and thus ensures liveness as long as priorities are respected (see Theorem 3). The originality of the work is that the proposed coordination system combines a global motion planning approach for priority assignment, and a feedback motion planning approach for control under assigned priorities. The presented simulations illustrate the robustness of the algorithms with an example of unexpected events that make the robots drift far away from the trajectory that they would have followed in the absence of such events.

Finally, the presented work opens up new avenues. First of all, a possible distributed implementation of the proposed control law should be investigated. Indeed, for each robot, applying the control law only requires the knowledge of the state of neighboring robots. As a result, once priorities are assigned, robots could interact locally and there would be no need for large amounts of data being exchanged. This would be an interesting property from a practical viewpoint as in current wireless communication systems delays are unavoidable and the range is limited. The priority graph would then be seen as the minimal supervisory information ensuring that the individual actions of robots serve the overall goal.

Secondly, the priority assignment policy presented in Section 4 is rather naive. Indeed, queues of robots are not considered when processing the requests. Future work should focus on devising more complex priority assignment policies considering all robots at the proximity of the control area. This could be done adapting some of the many works already available on the optimization of prioritized motion planning methods (see e.g., [START_REF] Au | Enforcing liveness in autonomous traffic management[END_REF][START_REF] Akella | Coordinating the motions of multiple robots with specified trajectories[END_REF]). Moreover, in the context of a network of intersections in transportation networks, queueing network control systems already applied for traffic signal control (see, e.g., [START_REF] Varaiya | The max-pressure controller for arbitrary networks of signalized intersections[END_REF][START_REF] Wongpiromsarn | Distributed traffic signal control for maximum network throughput[END_REF][START_REF] Gregoire | Capacityaware back-pressure traffic signal control[END_REF]) should be investigated.

Lastly, future works should focus on the design of a control law that is aware of sensing and control uncertainty. [START_REF] Del | A separation principle for a class of hybrid automata on a partial order[END_REF] has considered a two-vehicles coordination system and has evidenced that the concept of non-deterministic information state [START_REF] Lavalle | Evaluating motion strategies under nondeterministic or probabilistic uncertainties in sensing and control[END_REF] (that consists of maintaining a set of possible states through time based on past history of observation and control) should prove useful for guaranteeing safety under sensing and control uncertainty. More precisely, a separation principle has been proved. We would like to extend this work to a multiple robot coordination system in the future.

γ 1 = 4 γ 5 = γ 6 γ 7 = γ 8 γ 11 = γ 12 γ 9 Figure 1 :

 14567111291 Figure 1: The fixed paths assumption. Every robot travels along an assigned path.

Figure 2 :

 2 Figure 2: The left drawing depicts two paths with two robots in collision in the current configuration. The left drawing shows the obstacle region associated to the two paths in the coordination space and the collision configuration (x i , x j ) ∈ χ obs ij corresponding to the collision of the left drawing.

Figure 3 :

 3 Figure3: The collision cylinders with assigned priorities χ obs i j and χ obs j i .

Figure 4 :

 4 Figure 4: The control area.

Figure 5 :

 5 Figure 5: Scheme of the overall coordination system.

Figure 6 :

 6 Figure 6: the intersection composed of eight straight paths used for simulations.

http://www.youtube.com/watch?v=DXPICp4n2cU

http://www.youtube.com/watch?v=fzgiXc6rgug

http://www.youtube.com/watch?v=2-90q9aiEfg


Jean Grégoire

Silvère Bonnabel

Arnaud De

La Fortelle

Priority-based intersection management with kinodynamic constraints

We consider the problem of coordinating a collection 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. INTRODUCTION

A. Motivation

Human error is the sole cause in 57% of all road accidents and is a contributing factor in over 90% [START_REF] Treat | Tri-level Study of the Causes of Traffic Accidents: Final Report[END_REF], [START_REF] Ncsa | National center for statistics and analysis, traffic safety facts 2003[END_REF]. Moreover, 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 [START_REF] Dresner | Multiagent traffic management: A reservation-based intersection control mechanism[END_REF], [START_REF]Mitigating catastrophic failure at intersections of autonomous vehicles[END_REF], [START_REF] Colombo | Efficient algorithms for collision avoidance at intersections[END_REF], [START_REF] Zohdy | Optimizing driverless vehicles at intersections[END_REF], [START_REF] Kowshik | Provable systemwide safety in intelligent intersections[END_REF], [START_REF] Mehani | Trajectory planning in a crossroads for a fleet of driverless vehicles[END_REF]. Furthermore, more generally, automated conflict management opens new perspectives to improve railway [START_REF] Ismail | Railway traffic control and train scheduling based oninter-train conflict management[END_REF] and air transportation systems [START_REF] Tomlin | Conflict resolution for air traffic management: a study in multiagent hybrid systems[END_REF] 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 communication 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 [START_REF] Kant | Toward efficient trajectory planning: The path-velocity decomposition[END_REF]. As presented in [START_REF] Gregoire | Optimal cooperative motion planning for vehicles at intersections[END_REF], 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.

As first introduced in [START_REF] Leroy | Multiple path coordination for mobile robots: A geometric algorithm[END_REF], [START_REF] Lavalle | Optimal motion planning for multiple robots having independent goals[END_REF], the path-velocity decomposition enables to introduce an abstract space: the coordination space. It is a standard approach to robot motion planning [START_REF] Latombe | Robot Motion Planning[END_REF], [START_REF] Lozano-Perez | Spatial planning: A configuration space approach[END_REF], 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 intersection) and the obstacle-region has a cylindrical structure [START_REF] Lavalle | Planning Algorithms[END_REF]. In [START_REF] Gregoire | Optimal cooperative motion planning for vehicles at intersections[END_REF], 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 problem 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 robustness and distribution in future automated intersection management systems. Indeed, existing intersection management systems such as proposed in [START_REF] Zohdy | Optimizing driverless vehicles at intersections[END_REF], [START_REF] Dresner | Multiagent traffic management: A reservation-based intersection control mechanism[END_REF], [START_REF] Mehani | Trajectory planning in a crossroads for a fleet of driverless vehicles[END_REF] plan the complete trajectories of robots through the intersection and ensuring safety requires robots to follow precisely the planned trajectory. 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 [START_REF] Ghrist | Nonpositive curvature and pareto optimal coordination of robots[END_REF] and developed in the priority-based framework in [START_REF] Gregoire | Optimal cooperative motion planning for vehicles at intersections[END_REF]. 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 relatively easy to define an optimal trajectory for fixed priorities. This trajectory is referred to as the left-greedy trajectory [START_REF] Ghrist | Nonpositive curvature and pareto optimal coordination of robots[END_REF], [START_REF] Gregoire | Optimal cooperative motion planning for vehicles at intersections[END_REF]. 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 [START_REF] Ghrist | Nonpositive curvature and pareto optimal coordination of robots[END_REF]). In the present paper, we address the challenging problem of finding safe trajectories that respect this type of constraints. In [START_REF] Peng | Coordinating multiple robots with kinodynamic constraints along specified paths[END_REF], 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.

C. Contributions

We introduce a theoretical tool: the braking trajectory, which is a virtual trajectory obtained letting all robots slowing 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 [START_REF] Gregoire | Optimal cooperative motion planning for vehicles at intersections[END_REF]. 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. MODELLING ASSUMPTIONS

A. The coordination space

We assume that robots are constrained to follow predefined 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 lowlevel controller. This standard assumption [START_REF] Leroy | Multiple path coordination for mobile robots: A geometric algorithm[END_REF], [START_REF] Fraichard | Planning movements for several coordinated vehicles[END_REF], [START_REF] Hafner | Automated vehicle-tovehicle collision avoidance at intersections[END_REF], [START_REF] Akella | Coordinating the motions of multiple robots with specified trajectories[END_REF], [START_REF] Kowshik | Provable systemwide safety in intelligent intersections[END_REF] 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. 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.

γ 1 = γ 2 = γ 3 = γ 4 γ 5 = γ 6 γ 7 = γ 8 γ 11 = γ 12 γ 9 = γ 10
The configuration space χ is known as the coordination space [START_REF] Donnell | Deadlock-free and collision-free coordination of two robot manipulators[END_REF], [START_REF] Lavalle | Planning Algorithms[END_REF], [START_REF] Leroy | Multiple path coordination for mobile robots: A geometric algorithm[END_REF]. 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 [START_REF] Lavalle | Planning Algorithms[END_REF]. 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 crosssection (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 = ∅ (otherwise, coordination is not required), so the boundedness condition ensures that inf χ obs and sup χ obs both exist.

x 1 x 2 χ obs Υ 1 Υ 2 Fig. 2.
The left drawing depicts two robots in collision in a 2-pathintersection. 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 (limited acceleration, adherence, jerk, etc.). Let p denote the degree of the constraints and n the number of robots. Let 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

s(t) = (x x ′ • • • x (p) )(t) ∈ R n×(
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. THE PRIORITY-BASED FRAMEWORK

In this section, we recall the basics of priority-based intersection management introduced in our previous work [START_REF] Gregoire | Optimal cooperative motion planning for vehicles at intersections[END_REF].

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)
x i x i x j χ obs x i x j χ obs j>i x i x j χ obs i>j x i>j (t) x j χ obs x j>i (t)
x i>j (t)

x j>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 priority relations between robots. A collision-free trajectory x induces a binary relation ≻ on the set {1...n} as follows. For i = j s.t. χ obs ij = ∅, 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 ) ∈ χ goal . The multiple robot motion planning problem consists of finding a feasible trajectory. The benefit of the prioritybased 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. MOTION 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 [START_REF] Gregoire | Optimal cooperative motion planning for vehicles at intersections[END_REF]), 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 braking 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 v max a max a min C i x i t x i (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 [START_REF] Ghrist | Nonpositive curvature and pareto optimal coordination of robots[END_REF], 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 [START_REF] Fraichard | Inevitable collision states -a step towards safer robots?[END_REF] 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 MAXIMALLYAGRESSIVETRAJECTORY T ← 0 s(0) ← s init while x(T ) / ∈ χ goal do 5:

for i ∈ {1...n} do for t ∈ [0, ∆T ] do for j = i do sj (t) ← s j (s(T ), t) end for 10:

si (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 = ∅ 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 (collisionfree) 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 [START_REF] Gregoire | Optimal cooperative motion planning for vehicles at intersections[END_REF], 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 i0 : 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 trajectory 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 ) [START_REF] Kowshik | Provable systemwide safety in intelligent intersections[END_REF] • 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:

sj (∆T ) = s j (s(t 0 ), ∆T ) (8) si (∆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 Property 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:

xj (t) = x j (s(t 0 ), t) (14) xi (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) = si (t -t 0 ) (16) s j (t) ≥ sj (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. SIMULATIONS

The algorithms presented in this paper have been implemented 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 [START_REF] Gregoire | Optimal cooperative motion planning for vehicles at intersections[END_REF]), 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-pathintersection 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.

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. CONCLUSIONS 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 [START_REF] Lavalle | Planning Algorithms[END_REF], 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.

  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.

Fig. 4 .

 4 Fig. 4. Two representations of priority relations. Robots along a path in foreground have priority over robots along a path in background.

Fig. 6 .

 6 Fig.6. Simulation results: plot of the averaged increase in travel time against the traffic density for the intersection of Figure7

Fig. 7 .

 7 Fig. 7. The 4-path-intersection used for simulations