An Algorithm for Supervised Driving of Cooperative Semi-Autonomous Vehicles (Extended)

06/25/2017 ∙ by Florent Altche, et al. ∙ MINES ParisTech 0

Before reaching full autonomy, vehicles will gradually be equipped with more and more advanced driver assistance systems (ADAS), effectively rendering them semi-autonomous. However, current ADAS technologies seem unable to handle complex traffic situations, notably when dealing with vehicles arriving from the sides, either at intersections or when merging on highways. The high rate of accidents in these settings prove that they constitute difficult driving situations. Moreover, intersections and merging lanes are often the source of important traffic congestion and, sometimes, deadlocks. In this article, we propose a cooperative framework to safely coordinate semi-autonomous vehicles in such settings, removing the risk of collision or deadlocks while remaining compatible with human driving. More specifically, we present a supervised coordination scheme that overrides control inputs from human drivers when they would result in an unsafe or blocked situation. To avoid unnecessary intervention and remain compatible with human driving, overriding only occurs when collisions or deadlocks are imminent. In this case, safe overriding controls are chosen while ensuring they deviate minimally from those originally requested by the drivers. Simulation results based on a realistic physics simulator show that our approach is scalable to real-world scenarios, and computations can be performed in real-time on a standard computer for up to a dozen simultaneous vehicles.

READ FULL TEXT VIEW PDF
POST COMMENT

Comments

There are no comments yet.

Authors

page 1

page 2

page 3

page 4

This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

I Introduction

Advanced driver assistance systems (ADAS) are becoming increasingly complex as they spread across the automotive market. Although adaptive cruise control (ACC) [1] and automated emergency braking (AEB) [2] are the best-known examples of such systems, applications of ADAS have been broadened and now include pedestrian [3], traffic light [4] or obstacle detection [5] as well as lane keeping assistance [6]. The development of this new equipment allows drivers to delegate part of the driving task to their vehicles. As these systems keep getting more efficient and able to handle more complex situations, vehicles will gradually progress towards semi-autonomous driving, where drivers remain in charge of their own safety, while their errors can be seamlessly corrected to prevent potential accidents.

One of the challenges of semi-autonomous driving lies in efficiently handling vehicles on conflicting paths, for instance at an intersection or a highway entry lane. Traffic rules such as priority to the right can help determine whether to pass before or after another vehicle; however, many situations require driving experience to be handled efficiently. Learning-based approaches may eventually prove able to transfer driving experience to a computer, but such knowledge is very hard to implement in a safety system. In this article, we consider another possible solution, consisting in using vehicle-to-vehicle or vehicle-to-infrastructure communication for cooperative semi-autonomous driving. In this setting, vehicles negotiate with one another, or receive instructions from a centralized computer, allowing them to drive safely and efficiently.

In this article, we consider a method to ensure the safety of multiple semi-autonomous vehicles on conflicting paths, for instance crossing an intersection or entering a highway, while remaining compatible with the presence of human drivers. To this end, and inspired by earlier work in [7, 8], we propose a so-called Supervisor which monitors control inputs from each vehicle’s driver, and is able to override these controls when they would result in an unsafe situation. More specifically, the role of the supervisor is twofold: first, knowing the current states of the vehicles, the supervisor should determine if the controls requested by the drivers would lead the vehicles into unsafe inevitable collision states [9]. In this case, the second task of the supervisor is to compute safe controls – maintaining the vehicles in safe states – which are as close as possible to those actually requested by the drivers. We say that such a control is minimally deviating.

This paper provides two main contributions: from a practical standpoint, we design and implement a mathematical framework allowing to simultaneously perform the safety verification of target control inputs, and the computation of minimally deviating safe controls if target inputs are unsafe. From a theoretical standpoint, we formally prove that verifying safety over a finite time horizon is enough to ensure infinite horizon safety, and we provide a sufficient condition on the verification horizon for this property to hold. Unlike previous work focusing on specific situations such as intersections [7, 8], our framework can be applied to a wide variety of driving scenarios including intersections, merging lanes and roundabouts.

The rest of the article is structured as follows: in Section II, we provide a review of the related literature. In Section III, we present our modeling of semi-autonomous vehicles and introduce the Supervision problem of verifying the safety of drivers control inputs and finding a minimally deviating safe control if necessary. In Section IV, we present an infinite horizon formulation based on constraints programming to solve this problem. In Section V, we derive a finite horizon formulation which we prove is equivalent to the infinite horizon one. In Section VI, we use computer simulations to showcase the performance of the proposed supervisor in various driving situations. In Section VII, we present possible methods for real-world implementations of our approach. Finally, Section VIII concludes the study.

Ii Related Work

In the last decade, a lot of research has been focused on coordinating fully autonomous vehicles in challenging settings such as crossroads, roundabouts or merging lanes, with the ambition of improving both safety and traffic efficiency. Naumann et al. [10], followed by Dresner and Stone [11] have seemingly pioneered the work of adapting traffic intersections management methods to fully autonomous vehicles, designing so-called autonomous intersection management algorithms. They propose that each approaching autonomous vehicle reserves a time interval to cross the intersection; collisions are prevented by ensuring that conflicting vehicles are assigned non-overlapping crossing times. Subsequent studies on this particular problem have led to other approaches. In [12], vehicles choose their control inputs based on navigation functions which include a collision avoidance term, allowing vehicles to react to maneuvers from other traffic participants. In [13], collision avoidance is ensured by assigning relative crossing orders to incoming vehicles; each vehicle then uses model predictive control to plan collision-free trajectories respecting these priorities. Other authors have considered different driving situations for autonomous vehicles, such as cooperative merging on a highway [14, 15, 16], or entering a roundabout [17].

By contrast, relatively little work has considered semi-autonomous driving assistance, possibly because the presence of human drivers brings a lot of additional complexity. The goal of a semi-autonomous driving assistant is to help the driver avoid collisions, either by notifying of a potential danger [18] or by taking over vehicle control in dangerous situations [19, 20, 21]. To be accepted by human drivers, such systems should be as unobtrusive as possible, and in particular should only intervene when necessary. Most of the currently existing literature on semi-autonomous driving mostly focuses on highway driving [19, 20, 21], which presents relatively low difficulty as vehicles trajectories remain mostly parallel. The aim of this article is to bring semi-autonomy one step further, to allow cooperative driving between semi-autonomous vehicles in more complex conflict situations.

Some of these more complex problems have already been studied in the literature. In [22], the authors consider semi-autonomous driving at an intersection and propose that human drivers let an automated system control their vehicle while crossing said intersection. However, this scheme is rather intrusive as drivers completely relinquish control for a time, and handing back controls to a potentially distracted driver poses problems by itself. Colombo et al. [7, 8] introduced the idea of a supervisory instance (called supervisor) tasked with preventing the system of vehicles from entering undesirable states by overriding the controls of one or several vehicles. In this more human-friendly approach, overriding only occurs when necessary, i.e. if an absence of intervention would result in a crash. The question of determining whether overriding is needed or not, called verification problem, is NP-hard [23]; under several simplifying assumptions, it is shown in [7] to be equivalent to a scheduling problem. In this reformulation, vehicles are each assigned a time slot during which they are allowed inside the intersection, and assigned slots are mutually disjoint. If, due to vehicle dynamics, no feasible schedule exists, the initial state is deemed unsafe. This allows the authors to design a so-called least restrictive supervisor, which verifies the safety of the desired inputs and overrides them if necessary. However, the proposed supervisor is only suitable to simple intersection geometries with a single conflict point. Moreover, no additional property is required from the safe controls used for overriding, which can widely deviate from the desired ones.

Several variations have been proposed based on the equivalence demonstrated in [7]. Reference [24] designs a supervisor which is robust to bounded uncertainties by adding safety margins. Reference [25] leverages job-shop scheduling to develop a supervisor that considers several possible conflict points inside the intersection; however, vehicle dynamics are only modeled as first-order integrators, which is not realistic in a real-world setting. Campos et al. [8] proposed a Pareto-optimal supervisor leading to a minimally deviating formulation by recursively finding the most constrained vehicle, reserving its optimal crossing time, and scheduling the crossing of the remaining vehicles using the previous schedule as constraints. This method allows to minimize the deviation between the overridden and desired controls, but may be computationally intensive. Indeed, one of the major difficulties of performing optimization in this context lies in the necessity to consider all the possible orderings of the vehicles.

This problem is highly combinatorial; it has been shown that there exists up to orderings for vehicles [26]

. Moreover, it is generally ignored by most authors studying motion planning problems, who either use simple heuristics such as first-come, first-served 

[11, 27] or rely on exhaustive search [28, 8]. A possible method to handle the combinatorial explosion is to use pruning techniques such as branch-and-bound

, which avoid exploring branches of the decision tree that would provably yield suboptimal results. These methods are commonly used in mixed-integer linear (see,

e.g., [29, 30] for applications to motion planning) or quadratic programming (see, e.g., [31]) problems, which combine continuous and discrete optimization. More general nonlinear methods have also been used in motion planning [32, 33], although their high computational difficulty generally requires linearization for effective resolution, as illustrated in [34]. To the best of the authors’ knowledge, branch-and-bound methods have never been applied to semi-autonomous driving.

This article significantly differs from references [7, 8, 25]. Instead of using a scheduling approach, we formulate the supervision problem as a Mixed Integer Quadratic Programming (MIQP) problem, which can handle various geometries with multiple collision points such as multi-lane intersections, merging lanes or roundabouts. Our formulation only requires to consider a small, finite planning horizon, while previous approaches [7, 8, 25] needed to schedule the crossing of all the considered vehicles. Furthermore, the MIQP formulation is highly flexible, allowing to take into account various constraints (e.g., maximal turning speed) and different cost functions. Finally, the resolution of MIQP can leverage highly-optimized solvers [35], allowing real-time implementations even for a relatively large number of vehicles.

This article expands the results presented in the conference paper [36]; among the significant improvements made in this extension, we now give a more comprehensive model of our vision of semi-autonomous vehicles and adjust the modeling of the problem to handle bounded control errors. We provide a detailed discussion on how complex road geometries with multiply-intersecting paths can be handled, leading to a very versatile framework. Finally, we extend the theoretical results to continuous arrivals of vehicles, and provide possible ways for actual implementation as a roadside unit.

Iii Supervision problem

We consider the problem of safely coordinating multiple semi-autonomous vehicles on the road, in order to prevent collisions and deadlock situations where no vehicle is able to move forward. Since vehicles are human-driven, a form of outside supervision is necessary to prevent undesirable situations. This section presents our formulation of a so-called Supervision problem generalizing the work of Colombo et al. [7]; solving this problem yields a provably safe control, as close as possible to the original intentions of the drivers.

Iii-a Modeling

Iii-A1 Supervision area

We consider an isolated portion of a road infrastructure used by semi-autonomous vehicles, where some form of coordination is required to ensure vehicles safety. For instance, this could be a classical road intersection, a roundabout or an entry or weaving lane on a highway. We call this bounded portion of infrastructure the supervision area and we assume that vehicles can travel safely outside of the collision area using only their ACC capacities. In a real-world setting, different critical portions of infrastructure which are far enough apart can be considered individually, but need to be treated jointly if traffic from one can influence another. Figure 4 shows examples of roads configurations and the corresponding possible choice for a supervision area.

(a) Crossroads (b) Roundabout
(c) Highway merging
Fig. 4: Examples of considered road configurations, and corresponding supervision areas (interior of the dotted rectangles).

In this article, we present an embodiment of a Supervisor working over a spatially static supervision area over time, that can be thought of as a dedicated computer on the infrastructure or in the cloud. Vehicles are assumed to establish a connection to the supervisor when they enter the supervision area (using, for instance, V2I communication), and maintain it until they exit this region. We denote by the set of vehicles currently inside the supervision area at a time .

Iii-A2 Semi-autonomous vehicles

We consider semi-autonomous vehicles equipped with advanced driver assistance systems, many of which are already commercially available, and Vehicle to Infrastructure (V2I) communication capacities. In particular, vehicles are assumed to have advanced cruise control, automated braking and lane keeping assistance systems such that accelerating, braking and steering can be actuated by an on-board computer. Moreover, we suppose that vehicles have access to reliable cartographic data and are capable of precisely measuring their current position, orientation and velocity with reference to a unique global frame, for instance using GNSS and inertial navigation.

Since the vehicles are not assumed to have advanced environment-sensing capacities, for instance based on LIDAR data, they are not able to handle all situations and still require a human driver to safely navigate, for instance in the case of on-road obstacles or loss of GNSS signal. Moreover, lateral collisions or deadlock situations can happen due to human error, justifying the need for supervision.

Iii-A3 Parametrization

In the remainder of this article, we only consider the two-dimensional kinematics and dynamics of the vehicles. We denote by a bounding polygon for the shape of vehicle , and by the center of .

We assume that the geometry and lane markings of the roads inside the supervision area define a finite number of reference paths across this region, as exemplified in fig. 8. Due to the presence of a lane keeping assistance system, we assume that every vehicle is able to follow one of these reference paths with a small bounded lateral error. Noting the reference path of a vehicle , we assume that the distance of from is bounded from above by . Moreover, we assume that is at least -continuous, and that is small enough to ensure, for all ,

(1)

This condition allows to use the curvilinear position of the point of closest to to uniquely encode the longitudinal position of vehicle along . We denote by this curvilinear position, with the convention that when the front bumper of first enters the supervision area and increases when goes forward; we let be the longitudinal position at which the rear bumper of fully exits the supervision area.

Iii-A4 Vehicle dynamics

In this article, we mostly focus on the longitudinal dynamics of the vehicles, and we let be the state of vehicle , where and are respectively its longitudinal position and longitudinal speed. We assume that vehicles follow second-order integrator dynamics with a bounded longitudinal error, and that the control input corresponds to the longitudinal acceleration as:

(2)

where and . Since we mostly consider situations with conflicting vehicles, we assume that human drivers maintain a relatively low speed (compared to the curvature of their path), which allows neglecting lateral dynamics and slip [37].

To account for speed limitations on the vehicles, each vehicle is supposed to have a bounded non-negative velocity, so that (with ) at all times. Moreover, we assume that the acceleration of each vehicle is bounded as , with . These bounds can differ between vehicles, thus allowing heterogeneous vehicle performance. At a given time , we let be the set of admissible accelerations for the vehicles of . We denote bt boldface and the state and control for the system of vehicles.

In what follows, we let be a global upper bound for , a lower bound for and an upper bound for such that for all and all , and . Therefore, all vehicles are capable of braking with and accelerating with ; finally, we let be a global upper bound for .

Iii-A5 Collision regions

Finally, we assume that the angle between the orientation of vehicle and the tangent to at its point closest to is also bounded. With these hypotheses, for any pair of vehicles , we can compute the bounded set of curvilinear positions for which a collision could happen between and . Note that these sets are “inflated” to take into account the bounded control errors. We call the collision region between and ; fig. 8 shows examples of paths and corresponding computed collision regions for different driving situations. Note that collision regions can be empty or have one or multiple connected components. If , we say that vehicles and are conflicting; when has multiple connected components, we denote by its -th component, using the convention .

(a) Simple orthogonal intersection situation with example vehicle shapes
(b) Roundabout situation with multiple connected components in
(c) Highway merging situation
Fig. 8: Examples of paths (left) and corresponding collision regions (right) for vehicles with the polygonal shape shown in fig. (a)a.

Iii-A6 No-stop regions

To prevent creating deadlock situations, vehicles are not allowed to stop when doing so would block traffic in other directions. To this extent, we define a no-stop region (see fig. 9) for each vehicle as the smallest interval containing all for all , and all such that ; in this formula, is the projection operator on the first coordinate. The no-stop region corresponds to the part of the supervision area where a vehicle may have to yield to another; if contains , then either or enters the supervision area behind the other, in which case the relative ordering of the vehicles is given and the does not count in .

Note that, although this definition theoretically requires knowledge of all future vehicles, can be computed off-line as a finite intersection of intervals provided that there only exists a finite number of possible paths inside the supervision area. In what follows, we let be a minimum allowed speed for any vehicle inside its no-stop region, and we assume that for all vehicles.

For a no-stop region , we define the corresponding acceleration region such that, if vehicle is stopped at , it can reach a speed before reaching . More specifically, we require that for all . Inside the acceleration region, vehicles are only allowed to accelerate; this condition prevents vehicles from stopping right before the entrance of the no-stop region, leaving them unable to proceed forward due to the minimum speed requirement. Figure 9 illustrates an example of the no-stop regions and the corresponding acceleration regions.

Fig. 9: Illustration of the no-stop region and acceleration region inside the supervision area (dotted rectangle).

Iii-A7 Time discretization

Drivers continuously change the control input of their vehicle; however, due to computational and communication constraints, it is impractical to handle functions of a continuous variable. In the remainder of this article, we choose a constant time step duration , and we assume that all vehicles use piecewise-constant controls with step , typically . To simplify the formulation, we further assume that vehicles update their control simultaneously at times for , and we denote by the set of piecewise-constant admissible controls for the vehicles of . By definition, for all and all , .

Iii-B Problem statement

Before presenting the so-called supervision problem, we first define the safety criterion for the vehicles inside the supervision area at a given time.

Definition 1 (Safe state).

We say that the supervision area is in a safe state at time if there exists an admissible piecewise-constant control defined over such that, under this control and starting from , for all and all , . Such a control is said to be a safe control.

With this definition, the supervision area is in a safe state when all the vehicles inside this area can apply a dynamically admissible, infinite horizon control without a risk of collision. This safety condition corresponds to a contraposition of the notion of “inevitable collision state” proposed by Fraichard et al. [9]. In what follows, we denote by the set of safe and dynamically admissible piecewise-constant controls for the vehicles in ; by definition, a control is a piecewise-constant function from to . We now define the safety condition for vehicles entering the supervision area.

Definition 2 (Safe entry).

Consider a safe state at time and let be the first time at which a new vehicle enters the supervision area. We say that the vehicles of safely enter the supervision area with a margin if , or if any safe control :

  • keeps the system of the vehicles of safe at time and

  • remains safe over for the vehicles of ,

regardless of the control applied by the vehicles of over .

This definition ensures that a safe control computed for the vehicles of remains safe after new vehicles enter, i.e. the entry of new vehicles does not invalidate previously safe controls. Moreover, we assume that we can safely exclude vehicles departing the supervision area from the safety verification problem, i.e. that drivers are able to safely follow the previously departed vehicles without supervision. We will show in Section IV-C that these hypotheses allow discrete-time supervision with continuous vehicle arrival.

In the remainder of this article, we consider a centralized supervisor working in discrete time steps of duration , and we assume that new vehicles always enter safely with a margin . At the beginning of each time step , the supervisor receives an information about the desired longitudinal control of each vehicle for the next time step, denoted by . The collection of these desired controls for the vehicles of defines a constant desired system control defined over .

This control may, or may not, lead the system of vehicles into an unsafe state. The supervisor is tasked with preventing the system from entering an unsafe state, by overriding the desired control if necessary. To remain compatible with human drivers, it is desirable that the supervisor has several properties, namely being least restrictive and minimally deviating. Letting be the restriction of the functions of to , we define the least restrictive supervision problem:

Definition 3 (Least restrictive supervision).

Consider a safe state at time , a desired system control and assume that all new vehicles enter the supervision area safely with a margin . The least restrictive supervision problem () is that of finding a control such that if .

Note that this definition corresponds to that of [7] in our generalized setting. Such a supervisor is least restrictive because overriding only occurs if the initially requested control would lead the vehicles in an unsafe state. However, it is also desirable that the control used for overriding is chosen close to the drivers’ desired control. Extending the work in [8], we define the minimally deviating supervision problem as follows:

Definition 4 (Minimally deviating supervision).

Consider a safe state at time , a desired system control and assume that all new vehicles enter the supervision area safely with a margin . The minimally deviating supervision problem () is that of finding a constant control such that:

(3)

where is a norm defined over .

Note that, from this definition, any solution to is a solution to .

This concept of minimally deviating supervision follows a different fail-safety paradigm that could be found in, e.g., rail transportation where all trains in an area should perform an emergency braking when an incident occurs. The reasoning behind definition 4 is that, to improve efficiency without sacrificing safety, intervention is only performed on vehicles which are actually at risk, and does not necessarily result in a full stop. However, at individual vehicle level, the safe overriding control may differ greatly from the driver’s input, e.g. braking instead of accelerating.

Iv Infinite Horizon Formulation of the Supervision Problem

In this section, we present an extension of the work in [36] allowing to reformulate the generalized minimally deviating supervision problem using mixed-integer quadratic programming (MIQP) in Section IV-A. As the supervisor works in discrete time steps of duration , we consider the beginning of a step , corresponding to a time and formulate an infinite-horizon MIQP problem. Assuming the initial state is safe, we will show in Section IV-B that this formulation can be used to find a minimally deviating safe control for the vehicles in . We will show in Section IV-C that, if the vehicles of follow the corresponding control, our formulation expressed at remains feasible for the vehicles of , provided that all new vehicles enter safely with a margin . These properties ensure that our infinite horizon MIQP formulation can be solved in a receding horizon fashion, to ensure safety for all future vehicles.

Iv-a Model variables and constraints

In what follows, we present the variables and constraints used in our model. Unless specified otherwise, these constraints are enforced at all time steps , and for all vehicles of .

Iv-A1 Vehicle dynamics

When they evolve inside the supervision area, vehicles use a piecewise-constant control, which is updated every seconds. For a vehicle at step , we introduce the variables , and , respectively denoting its curvilinear position and longitudinal speed at , and longitudinal acceleration over . The following constraints enforce vehicle dynamics:

(4)
(5)

Iv-A2 Logical constraints

In [38], we showed that it is possible to enforce logical constraints on continuous and integer variables with linear inequalities using a “big-M” formulation. More specifically, if

is a binary variable and

a continuous or integer variable bounded so that , then the logical constraint: is equivalent to the linear inequality constraint . This method can be used to define indicator binary variables for a given semi-infinite interval: for a continuous variable and a constant , we denote by the constraints , where denotes the binary conjunction; we use to denote the binary negation.

Iv-A3 Collision avoidance

As presented in Section III-A3, the collision region between two vehicles , , can be computed off-line. As it was already presented in [38], it is possible to compute a minimal bounding convex polygon for each connected component of . A good compromise between accuracy and complexity is to use a bounding hexagon with edges either parallel to the , or lines; such a polygon is uniquely defined by six parameters, as shown in fig. 10.

Fig. 10: Minimum bounding hexagon for the collision region presented in fig. (a)a.

To ensure that vehicles do not enter any of the collision regions, we introduce a set of binary variables to encode the discrete decisions arising from the choice of an ordering of vehicles, as presented in [38]. For all conflicting vehicles , we let if vehicle passes the -th collision region before , and otherwise; moreover, we introduce the binary indicator variables for all :

(6)
(7)

We enforce the collision avoidance constraints for all conflicting vehicles and as:

(8)
(9)
(10)

where . Constraint (8) corresponds to “crossing situations”, where a vehicle has to wait for another to pass; constraints (9) and (10) correspond to “following situations”, where a vehicle needs to maintain a certain longitudinal distance from another.

Note that constraints (8) to (10) use the values of the indicator variables at step to force the positions of the vehicles at step in order to avoid a “corner cutting” phenomenon; the additional constraint (10) prevents collisions between two time steps. These constraints are very slightly stronger than that of collision avoidance, i.e. for all , . Consequently, the results in the rest of this article are to be understood replacing the exact collision avoidance constraints in definition 1 by conditions (8)-(10).

Finally, to ensure the consistency of the formulation, we add the mutual exclusion constraint for all conflicting vehicles :

(11)

Iv-A4 Deadlock avoidance

As described in Section III-A6, we require all vehicles to maintain a minimum speed inside their no-stop region . This requirement is enforced by defining additional binary variables, for all and all :

(12)
(13)
(14)
(15)

and using the constraints:

(16)
(17)

As long as the acceleration regions are large enough, constraint (16) prevents vehicles from remaining blocked due to the minimum speed requirement (17). We will show in the next section that these conditions effectively prevent deadlocks for all future times.

Iv-A5 Initial conditions

The supervision problem is used in a receding horizon fashion, and we consider that the state of each vehicle of at time is known before solving the problem. Therefore, we use the following initial condition for all :

(18)

Iv-B Objective function

Any piecewise-constant control verifying constraints (4) to (18) for all is dynamically admissible and prevents collisions for all future times, and is therefore in . To remain compatible with human driving, we now formulate an objective function allowing to find a least restrictive and minimally deviating control given a desired control . In what follows, we let be a set of strictly positive weights, be the tuple of all the problem variables, and we define:

(19)

Noting the projection operator such that , we deduce the following theorem:

Theorem 1.

The solution of the optimization problem:

(IH-SP)
subj. to

is a solution to the minimally deviating supervision problem at time , for the norm associated with .

Note that the weighting terms allow distinguishing between different types of agents, for instance to prioritize emergency services or high-occupancy vehicles. More complex cost functions can also be used, for instance to penalize a forced acceleration more than a forced braking.

Iv-C Receding horizon properties

We now assume that there exists a solution to IH-SP at time , that the vehicles of follow this solution control over , and that the vehicles of enter safely with a margin . From Definitions 2 and 1, we have the following theorem:

Theorem 2 (Recursive feasibility).

Let , , and . Assume that:

  • there exists a solution to IH-SP at time for the vehicles of ,

  • the vehicles of follow this solution control over ,

  • the vehicles of enter safely with a margin .

Then there exists a solution to IH-SP at time for the vehicles of .

Proof.

From definitions 2 and 1, and using theorem 1, we know that the first two hypotheses guarantee that the vehicles in are in a safe state at time . Moreover, the third hypothesis ensures that the vehicles in also are in a safe state at regardless of the control applied by the vehicles of up to time . By definition 1, there exists a feasible solution to IH-SP thus proving the theorem. ∎

We now state that the IH-SP formulation effectively prevents the apparition of deadlocksThe proof of this theorem can be found in Section A-A.

Theorem 3 (Deadlock avoidance).

Let and assume that, for all , the conditions of theorem 2 remain satisfied at time . There exists a feasible solution of IH-SP at time in which all the vehicles in exit the supervision area in finite time.

Note that theorem 3 only ensures that, at all times, there exists a solution where all the vehicles inside the supervision at this particular time eventually exit. However, there is no guarantee that such a solution will actually be selected, for instance if one driver wishes to stop although there is no other vehicle. There is also no fairness guarantee, i.e. it is possible that one vehicle is forced to remain stopped for an arbitrarily long time, for instance if there is a very heavy traffic coming from another direction. Future developments will focus devising more complex objectives function to take traffic efficiency and fairness into account.

Iv-D Multiple paths choices

The above formulation assumes that the path of each vehicle is known in advance. However, this may not be realistic in the context of semi-autonomous cars where drivers can decide to change paths, for instance to avoid an obstacle on the road or use another itinerary. Using additional variables to indicate the path to which a vehicle is assigned, our formulation can be extended to handle multiple possible paths for each vehicle. Due to length limitations, this extension will be detailed in future work.

V Finite Horizon Formulation

In Section IV-C, we presented an infinite horizon formulation to solve the minimally deviating supervision problem. However, due to the infinite number of variables, this formulation is not suitable for practical resolution. In this section, we derive an equivalent finite horizon formulation that can be implemented and solved using standard numerical techniques.

In what follows, we let and we denote by FH-SP the restriction of IH-SP at time to the variables at steps with , and we only consider the constraints (4) to (18) up to step . The objective function is unchanged. A solution to FH-SP at time allows to compute a control preventing collisions up to time ; however, due to the dynamics of the vehicles, the state reached at may not be safe. Since FH-SP only has a subset of the constraints of IH-SP, we can formulate the following proposition:

Proposition 1.

Let and let be a solution of IH-SP at step . The restriction of to the first time steps is a feasible solution to FH-SP.

Using the global bounds , , and defined in section III-A4, we will now prove a reciprocal implication to proposition 1: if is chosen large enough, any solution of FH-SP can be used to construct a solution of IH-SP.

As presented in [36], the key idea of the proof lies in the choice of a planning horizon long enough to allow any vehicle to fully stop. The structure of the demonstration is as follows: lemma 1 gives a lower bound on the time horizon to allow a single isolated vehicle to stop using discrete dynamics, although with a potential risk of rear-end collisions from following vehicles. In proposition 2, we give a slightly higher bound on the time horizon ensuring that all vehicles in a line can all safely stop without rear-end collisions. Finally, in proposition 3 we give a bound on ensuring the recursive feasibility of FH-SP; this allows formulating theorem 4, stating the equivalence of FH-SP and IH-SP. In this section, we only present sketches of proofs for each result; detailed demonstrations can be found in Section A-B.

Lemma 1.

At a time , consider a horizon with . Let be a vehicle for which there exists a piecewise-constant control such that, for all , , corresponding to a dynamically feasible trajectory over .

There exists a discrete control such that for all , and , and for which the corresponding dynamically feasible trajectory verifies and over .

Sketch of proof.

is an upper bound on the required time for any vehicle to stop by applying a control , which by definition is dynamically feasible. The additional accounts for the fact that we require at the first time step. ∎

In the following proposition and noting the ceiling function, we prove a bound ensuring that a line of vehicles can safely stop before the leader reaches its final computed position at the end of the time horizon, without risk of rear-end collisions:

Proposition 2.

At a time , suppose that vehicles of (denoted by from rear to front) are following one another. Consider a horizon , and assume that every vehicle has a safe discrete control such that, for all , . We let be the trajectory over for vehicle under control .

For all , there exists a safe discrete control such that for all , , and for which the corresponding dynamically feasible and safe trajectory verifies and over .

Sketch of proof.

The worst case that needs to be taken into account corresponds to a situation where the initial states of the vehicles require each of them to accelerate in order to avoid a rear-end collision from the vehicle behind. This rather extreme situation happens when a vehicle goes faster than the one it is following, and the two are too close to allow a safe deceleration. In this case, the rearmost vehicle can always brake with the control from lemma 1, until it decelerates below the speed of the vehicle in front of it. The second rearmost vehicle can then decelerate, then the third and up to the front-most vehicle. The term arises from the piecewise-constant control hypothesis, and vanishes as goes to . Note that the condition also provides the same guarantees; depending on the value of , this second bound might be more efficient. ∎

Remark 1.

The bound from proposition 2 depends on the number of vehicles in a line, and can become quite high when is large. It can be proven that the condition also provides the same guarantees; depending on the value of , this second bound might be more efficient.

We can now prove the recursive feasibility of FH-SP for a large enough , as follows:

Proposition 3.

Consider a time , and assume that at most vehicles are following one another at all times . We set and we let be the stopping horizon from proposition 2 for vehicles; moreover, we define . We assume that all vehicles of for all enter safely with a margin .

Problem FH-SP is recursively feasible under the hypotheses of theorem 2, i.e. if there exists a solution to FH-SP at time for the vehicles of , there exists a solution at for the vehicles of .

Sketch of proof.

The idea between the choice of is to ensure that each vehicle can either stop safely before entering its acceleration region (without generating rear-end collisions), or has already planned to exit its no-stop region safely. Moreover, the safe entering hypothesis ensures that the entry of new vehicles does not invalidate previously safe solutions, which can therefore be extended. ∎

We obtain the equivalence between IH-SP and FH-SP:

Theorem 4.

Problems IH-SP and FH-SP with are equivalent, i.e. an optimal solution to one is also an optimal solution to the other.

Proof.

Proposition 1 ensures that any optimal solution to IH-SP is a feasible solution of FH-SP. Proposition 3 shows that a solution to FH-SP (with ) can be recursively extended to a solution of IH-SP; therefore, the optimal solution of FH-SP is feasible for IH-SP. Using these two results, we deduce the stated theorem. ∎

An important corollary of theorems 4, 3 and 1 is that the control obtained by solving FH-SP with large enough is also a solution to the minimally deviating supervision problem, and ensures deadlock avoidance as well. Contrary to IH-SP, FH-SP is relatively easy to solve with dedicated mixed-integer quadratic programming solvers, as will be demonstrated in the following section.

Vi Simulation Results

Vi-a Simulation environment

The presented Supervisor framework has been validated using extensive computer simulations on various test scenarios. In the absence of standardized test situations and since no open-sourced implementation of comparable methods [7, 8] is available, this section does not aim at a quantitative comparison with existing algorithms. Since our Supervisor is by design guaranteed to output an optimal111Among the set of piecewise-constant controls with a given time step duration and in the sense of Definition 4 safe control, the major evaluation criterion is rather its ability to handle a wider variety of traffic scenarios than existing techniques, which is demonstrated in the rest of this section.

Due to implementation reasons, the resolution of the supervision problem is performed off-line and simulations are run in two successive phases. In the first phase, we define the geometry of the roads inside the supervision area and the corresponding possible paths, and compute the collision and acceleration regions information for each pair of paths. Since these sets only depend on the geometry of vehicles and paths, the corresponding parameters are computed off-line.

In the second phase, we run the simulation by coupling the high-fidelity vehicle physics simulator PreScan [39] with an external Python implementation of our supervisor. The actual resolution uses the commercial MIQP solver GUROBI [35]

; the Python program runs a coarse simulation over a set time horizon with a fixed time step duration. Vehicles are generated using random Poisson arrivals, with a predefined arrival rate for each possible path, while respecting the safe entering condition; the initial velocity of each generated vehicle is chosen randomly according to a truncated Gaussian distribution. At each time step, the finite horizon supervision problem FH-SP is solved for the vehicles inside the supervision area, and yields the best safe control for the set of vehicles. The state of these vehicles at the next time step is then computed according to equations (

4) and (5).

In parallel, we use PreScan to validate the consistency of this output: from the safe controls computed in the Python supervisor and knowing the reference paths of the vehicles, we compute a target state comprising a desired position, heading and longitudinal velocity for each vehicle. This target state is fed into a low-level controller which outputs a steering and an acceleration or braking control. The vehicle model used in the validation phase takes into account engine response as well as chassis and suspensions dynamics, but does not model road-tire friction. PreScan’s collision detection and visualization capacities are then used to validate the absence of collision or dangerous situations. Note that vehicles controllers are designed to ensure a bounded positioning error for any vehicle, relative to their prescribed path and velocity profile. This error is taken into account in the computation of the collision regions, so that the system is robust to control imperfections.

Vi-B Test scenarios

In the rest of this section, we consider three test scenarios – chosen to represent a wide variety of driving situations – consisting of merging on a highway, crossing an intersection or driving inside a roundabout. To showcase the performance of our framework in avoiding accidents and deadlocks, we assume that drivers are “oblivious” and focused on tracking a desired speed, regardless of the presence of other vehicles. A video of the presented simulations is available online222Available at https://youtu.be/JJZKfHMUeCI.

Vi-B1 Highway merging

We first consider a very simple highway merging scenario, where an entry lane merges into a single-lane road; the possible paths for the vehicles are the same as in fig. (c)c. The collision region between a vehicle in the entry lane and a vehicle on the highway have a single connected component given as and , taking control errors into account.

(a) Longitudinal positions; the shaded area is the collision region.
(b) Longitudinal velocities
Fig. 13: Vehicles positions and velocities in the merging scenario; solid lines correspond to vehicles on the entry lane, dashed lines to vehicles starting on the highway. The thick colored portions show overriding intervals.

To illustrate the action of the supervisor, we consider a set of six vehicles, three of which are on the highway and three on the entry lane. All vehicles are assumed to have “oblivious” drivers maintaining a constant speed, thus resulting in potential collisions. This admittedly unrealistic behavior has been chosen to generate a higher probability of collisions in absence of supervision.

Figure 13 shows the longitudinal trajectories of the supervised vehicles; colored (thick) portions of the lines represent intervals of time during which overriding occurs. The area in gray corresponds to the collision region between entering vehicles and vehicles on the highway; thanks to the action of the supervisor, all collisions are successfully avoided.

Vi-B2 Intersection crossing

(a) Longitudinal positions; the shaded area is the collision region.
(b) Longitudinal velocities
Fig. 16: Vehicles positions and velocities in the intersection crossing scenario; solid lines correspond to vehicles on the entry lane, dashed lines to vehicles starting on the highway. The thick colored portions show overriding intervals.

The second scenario is the crossing of a shaped intersection by a total of eight vehicles, with two vehicles per branch. In each branch, the front vehicle goes straight, and the rear vehicle turns left; moreover, all vehicles in front start at the same distance from the center of the intersection, and the same is true for the vehicles in the rear. This scenario illustrates the symmetry-breaking capacities of our framework, which handles this perfectly symmetrical scenario well, as shown in fig. 16. The area in gray corresponds to the collision region between vehicles on different branches. A video of a longer, one hour simulation is available also online333https://youtu.be/cl32nbceZvw.

Vi-B3 Roundabout driving

Fig. 17: Illustration of three possible classes of trajectories found by the solver, depending on the initial states of the vehicles. Trajectories and correspond to one vehicle passing the two collision points before the other. Trajectory corresponds to the case where the vehicle on the inner lane enters after the other, and overtakes it inside the roundabout.

Finally, the third scenario consists of vehicles driving inside a two-lanes roundabout. The particularity of this situation is that collision regions can have multiple connected components, for instance for the paths shown in fig. (b)b. Since our formulation explicitly distinguishes each of these connected components, the supervisor is able to choose an ordering for each point of conflict, as illustrated in fig. 17: depending on the initial states and control targets of the vehicles, a different class of solution is chosen. A video of a longer, one hour simulation is also available online444https://youtu.be/pLoG32wFnkE.

Vi-B4 Computation time

Due to the relatively short time horizon needed to ascertain infinite horizon safety, computation time remains reasonable despite the NP-hardness of the MIQP formulation. Figure 20 shows the evolution of the computation time in the intersection crossing and roundabout scenarios; the limited available space in the merging scenario does not allow enough vehicles for a similar diagram. These measurements have been obtained on a computer equipped with an Intel Core i7-6700K CPU clocked at with of RAM, using the GUROBI solver in version 7.0. It can be seen that computation time remains below the duration of a time step in of cases for up to approximately ten simultaneous vehicles, thus allowing real-time computation at .

Note that the MIQP problem only loosely depends on the paths geometry, but rather on the average number of conflicts per vehicle which is higher in the case of roundabout driving, thus explaining the longer times reported in fig. (b)b. Moreover, the implemented algorithm has been devised for readability over efficiency, and can be optimized by removing redundant variables to further reduce computation time. In practice, this refresh rate means that vehicles could apply a new acceleration every , which is faster than the typical reaction time of one second for a human driver, and should therefore be barely perceived. Note that for practical implementation purposes, the input of the supervisor should be predicted states at the end of the computation period instead of current states; since the acceleration of each vehicle is assumed to be known to the supervisor, these predictions can be easily performed by forward integration.

(a) Intersection scenario
(b) Roundabout scenario
Fig. 20: Distribution of computation times depending on the number of vehicles, for . Shaded areas represent the percentiles.

Vii Discussion on implementation

In the previous sections, we presented an optimization-based algorithm for the supervision of semi-autonomous vehicles; we now briefly discuss obstacles and possible solutions for actual implementation. First and foremost, not all vehicles will be equipped with the required communication capacities at the same time; therefore, the ability to deal with unequipped vehicles and other traffic participants is key to envision actual applications. Second, this work assumes perfect communication and control, and in general ignores uncertainties arising from real-world constraints.

Vii-a Dealing with unequipped vehicles

As with all innovations, the penetration rate of our system would gradually increase overtime, but remain below for years, yet the formulation proposed in Section IV requires all vehicles to be equipped with supervision capacities. Although a detailed study on the integration of unequipped vehicles in our framework is out of the scope of this paper, we present a possible technique to handle these vehicles provided that they can avoid longitudinal collisions with the leading vehicle, and have a bounded reaction time.

First, note that it is always possible to consider unequipped vehicles conservatively as proposed in [40]: at a given step , we compute the minimum and maximum curvilinear position that can be reached at time by the unequipped vehicle , denoted by and respectively. Using the same notations as in Section IV, we then define:

(20)
(21)

Therefore, the unequipped vehicle is considered as occupying the conflict region at step when there exists a control (maximum acceleration) for which it could be inside this region at step . Similarly, the vehicle is only considered as liberating the conflict region when, even by applying a maximum braking, it would exit it. The collision avoidance constraints (9) and (10) are also modified to use and , where is the minimum speed reachable by at . Other traffic participants such as cyclists (and, to a lesser extent, pedestrians) could also be taken into account in this fashion. Recently proposed “non-conservatively defensive strategies” [41] could also be applied.

A limitation of this simple approach is that it can lead equipped vehicles to often yield right-of-way to unequipped vehicles, which may be problematic and can slow the acceptance of the system. A possible method (introduced in [42]) to reduce this problem while improving the global level of safety is to use the existing equipped vehicles to force the unequipped ones to stop when required. Suppose that an unequipped vehicle (denoted by ) follows an equipped one (), both crossing the path of another equipped vehicle . By setting (thus requiring to pass before ), we effectively force the unequipped vehicle to also pass after ; the reaction time of the unequipped vehicle can be taken into account by adjusting the lower bound on the longitudinal acceleration of vehicle .

Note that this approach still guarantees that no collision can happen between an unequipped and an equipped vehicle; moreover, as the penetration rate of equipped vehicles increases, additional rules may be enforced to reduce the number of occurrences in which conflicting unequipped vehicles are simultaneously allowed in the conflict region, thus increasing safety even for the unequipped vehicles. Future work will study the impact of penetration rate on safety and efficiency for both equipped and unequipped vehicles.

Vii-B Practical implementation

We propose a centralized implementation, where a roadside computer (supervisor) with communication capacities is added to the infrastructure, and is tasked with repeatedly solving FH-SP. Note that resolution could also be performed using cloud computing, possibly providing much faster computations without necessitating fully dedicated hardware. The supervisor is also assumed to be equipped with a set of sensors (e.g., cameras), so that the arrival of new vehicles in the supervision area can be monitored (in order to account for unequipped vehicles and other traffic participants). Equipped vehicles are supposed to regularly communicate their current state, including position, velocity and driver’s control input, and receive instructions (the safe acceleration sequence solution of FH-SP) from the roadside supervisor. The vehicle’s on-board computer then uses these instructions to override the driver’s control inputs when needed. We argue that the main sources of uncertainty, i.e. communication, sensing and control errors, can be taken into account by using safety margins when computing collision regions.

Communications are assumed to have similar performance to current 802.11p specifications; we use the figures provided in [43, 44] as reference, with latency below , and packet loss probability of less than under . To account for network congestion, we use more conservative values than those reported experimentally in [44]

. Moreover, using the additional roadside sensors, we estimate that uncertainty in each vehicle’s localization could be reduced to below

longitudinally.

First, the latency corresponds to less than at highway speed. Second, since they do not require exchanging a lot of data, such messages can be sent much more frequently than the refresh rate of the supervisor. Considering messages can be sent at , the probability of a message not being received in is roughly , and after . Since they receive a whole sequence of safe accelerations, individual vehicles can keep executing this sequence until a new one is successfully received. A worst-case scenario would be having one vehicle using acceleration (maximum acceleration) where it should have used (maximum braking): after a duration , the corresponding positioning error is , which is roughly after and after for typical values of and of . More robust contingency protocols could likely be developed, and will be the subject of future work, but these values can be used as safety margins without compromising performance.

Similarly, positioning and control uncertainty can be accounted for as margins in the collision regions, provided they can be bounded. In this work, we assume that vehicle self-positioning can be improved using the roadside sensors from the supervisor (which can be precisely calibrated), which could provide relatively tight bounds on error.

Viii Conclusion

In this article, we designed a framework allowing safe semi-autonomous driving of multiple cooperative vehicles in various traffic situations. We first introduce a set of linear constraints ensuring infinite horizon safety for a group of human-driven vehicles, traveling inside predefined corridors with the help of existing lane-keeping technologies. Based on this set of constraints, a discrete-time Supervisor is allowed to override the drivers’ longitudinal control inputs if they would lead the vehicles into an inevitable collision state. In this case, the control used for overriding is chosen as close as possible to the one originally requested by the drivers. These two properties ensure that intervention only occurs when strictly necessary to maintain safety, thus facilitating the acceptation of the system by human drivers.

Theoretical considerations prove this supervisor guarantees both safety and deadlock avoidance, and can be applied without distinction to multiple situations such as traffic intersection, highway entry lanes or roundabouts. Using the realistic vehicle physics simulator PreScan, we demonstrated that our algorithm can handle complex situations over an arbitrary duration, with continuous arrivals of vehicles. Moreover, the proposed formulation can be solved in real-time on a standard desktop computer for up to ten vehicles, which makes it suitable for practical applications.

Additionally, this work opens up many perspectives for future research. First and foremost, the current framework does not deal with non-equipped vehicles or other traffic participants such as cyclists or pedestrians, nor does it take sensor and communication uncertainties into account. Before considering an actual implementation, the system should be more robust to these various sources of noise. Moreover, our formulation has been designed in a mostly centralized fashion; various approaches need to be explored to design a more realistic decentralized system, that could be implemented in actual cars.

Appendix A Demonstrations

A-a Proofs for Section Iv

Before proving theorem 3, we introduce the following lemma stemming from graph theory:

Lemma 2.

Let a directed graph with vertices set and edges set . All cycles in can be removed by reversing a set of edges, each of them contributing to at least one cycle.

Proof.

The proof is based on the existence of minimum feedback arc sets [45], i.e. a minimum set such that is acyclic. By minimality of , any belongs to at least one cycle of . Moreover, it can be seen that reversing the edges of also leads to an acyclic graph, thus proving the lemma. ∎

Proof of Theorem 3.

Note that the only constraints requiring a vehicle to stop are (8) to (10), forcing a vehicle to wait for a vehicle with . From the hypotheses and theorem 2, there exists a solution to IH-SP at time . We define a directed priority graph with and where an edge belongs to if there exists such that . Using this representation, a cycle in corresponds to a chain of conflicting vehicles for which there exists a connected component such that for all .

If is acyclic, it defines a (partial) topological order, and it is always possible to admit the vehicles one by one in that order. Therefore, there exists a feasible solution where all the vehicles of exit the supervision area in finite time.

We now assume that there exists at least one cycle in . If all the vehicles involved in the cycle can exit in finite time, the result of the theorem is proven. Otherwise, we note a set of vehicles corresponding to a cycle in : all of these vehicles are stopped at infinity, and are prevented to move further by a constraint of form (8), for a certain . Moreover, the no-stop condition (17) ensures that, for all and all