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 bestknown 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 semiautonomous 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 semiautonomous 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. Learningbased 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 vehicletovehicle or vehicletoinfrastructure communication for cooperative semiautonomous 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 semiautonomous 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 socalled 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 semiautonomous 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 realworld 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 socalled 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 nonoverlapping 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 collisionfree 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 semiautonomous driving assistance, possibly because the presence of human drivers brings a lot of additional complexity. The goal of a semiautonomous 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 semiautonomous 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 semiautonomy one step further, to allow cooperative driving between semiautonomous vehicles in more complex conflict situations.
Some of these more complex problems have already been studied in the literature. In [22], the authors consider semiautonomous 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 humanfriendly 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 NPhard [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 socalled 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 jobshop scheduling to develop a supervisor that considers several possible conflict points inside the intersection; however, vehicle dynamics are only modeled as firstorder integrators, which is not realistic in a realworld setting. Campos et al. [8] proposed a Paretooptimal 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 firstcome, firstserved
[11, 27] or rely on exhaustive search [28, 8]. A possible method to handle the combinatorial explosion is to use pruning techniques such as branchandbound, which avoid exploring branches of the decision tree that would provably yield suboptimal results. These methods are commonly used in mixedinteger 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, branchandbound methods have never been applied to semiautonomous 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 multilane 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 highlyoptimized solvers [35], allowing realtime 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 semiautonomous vehicles and adjust the modeling of the problem to handle bounded control errors. We provide a detailed discussion on how complex road geometries with multiplyintersecting 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 semiautonomous vehicles on the road, in order to prevent collisions and deadlock situations where no vehicle is able to move forward. Since vehicles are humandriven, a form of outside supervision is necessary to prevent undesirable situations. This section presents our formulation of a socalled 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.
Iiia Modeling
IiiA1 Supervision area
We consider an isolated portion of a road infrastructure used by semiautonomous 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 realworld 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.
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 .
IiiA2 Semiautonomous vehicles
We consider semiautonomous 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 onboard 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 environmentsensing 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 onroad obstacles or loss of GNSS signal. Moreover, lateral collisions or deadlock situations can happen due to human error, justifying the need for supervision.
IiiA3 Parametrization
In the remainder of this article, we only consider the twodimensional 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.
IiiA4 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 secondorder 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 nonnegative 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 .
IiiA5 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 .



IiiA6 Nostop 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 nostop 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 nostop 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 offline 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 nostop region, and we assume that for all vehicles.
For a nostop 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 nostop region, leaving them unable to proceed forward due to the minimum speed requirement. Figure 9 illustrates an example of the nostop regions and the corresponding acceleration regions.
IiiA7 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 piecewiseconstant 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 piecewiseconstant admissible controls for the vehicles of . By definition, for all and all , .
IiiB Problem statement
Before presenting the socalled 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 piecewiseconstant 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 piecewiseconstant controls for the vehicles in ; by definition, a control is a piecewiseconstant 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 IVC that these hypotheses allow discretetime 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 failsafety 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 mixedinteger quadratic programming (MIQP) in Section IVA. As the supervisor works in discrete time steps of duration , we consider the beginning of a step , corresponding to a time and formulate an infinitehorizon MIQP problem. Assuming the initial state is safe, we will show in Section IVB that this formulation can be used to find a minimally deviating safe control for the vehicles in . We will show in Section IVC 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.
Iva 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 .
IvA1 Vehicle dynamics
When they evolve inside the supervision area, vehicles use a piecewiseconstant 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) 
IvA2 Logical constraints
In [38], we showed that it is possible to enforce logical constraints on continuous and integer variables with linear inequalities using a “bigM” 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 semiinfinite 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.IvA3 Collision avoidance
As presented in Section IIIA3, the collision region between two vehicles , , can be computed offline. 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.
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) 
IvA4 Deadlock avoidance
As described in Section IIIA6, we require all vehicles to maintain a minimum speed inside their nostop 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.
IvA5 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) 
IvB Objective function
Any piecewiseconstant 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:
(IHSP)  
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 highoccupancy vehicles. More complex cost functions can also be used, for instance to penalize a forced acceleration more than a forced braking.
IvC Receding horizon properties
We now assume that there exists a solution to IHSP 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).
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 IHSP thus proving the theorem. ∎
We now state that the IHSP formulation effectively prevents the apparition of deadlocksThe proof of this theorem can be found in Section AA.
Theorem 3 (Deadlock avoidance).
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.
IvD 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 semiautonomous 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 IVC, 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 FHSP the restriction of IHSP 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 FHSP 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 FHSP only has a subset of the constraints of IHSP, we can formulate the following proposition:
Proposition 1.
Let and let be a solution of IHSP at step . The restriction of to the first time steps is a feasible solution to FHSP.
Using the global bounds , , and defined in section IIIA4, we will now prove a reciprocal implication to proposition 1: if is chosen large enough, any solution of FHSP can be used to construct a solution of IHSP.
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 rearend 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 rearend collisions. Finally, in proposition 3 we give a bound on ensuring the recursive feasibility of FHSP; this allows formulating theorem 4, stating the equivalence of FHSP and IHSP. In this section, we only present sketches of proofs for each result; detailed demonstrations can be found in Section AB.
Lemma 1.
At a time , consider a horizon with . Let be a vehicle for which there exists a piecewiseconstant 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 rearend 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 rearend 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 frontmost vehicle. The term arises from the piecewiseconstant 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 FHSP 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 FHSP is recursively feasible under the hypotheses of theorem 2, i.e. if there exists a solution to FHSP 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 rearend collisions), or has already planned to exit its nostop 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 IHSP and FHSP:
Theorem 4.
Problems IHSP and FHSP 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 IHSP is a feasible solution of FHSP. Proposition 3 shows that a solution to FHSP (with ) can be recursively extended to a solution of IHSP; therefore, the optimal solution of FHSP is feasible for IHSP. 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 FHSP with large enough is also a solution to the minimally deviating supervision problem, and ensures deadlock avoidance as well. Contrary to IHSP, FHSP is relatively easy to solve with dedicated mixedinteger quadratic programming solvers, as will be demonstrated in the following section.
Vi Simulation Results
Via 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 opensourced 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 optimal^{1}^{1}1Among the set of piecewiseconstant 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 offline 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 offline.
In the second phase, we run the simulation by coupling the highfidelity 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 FHSP 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 lowlevel 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 roadtire 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.
ViB 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 online^{2}^{2}2Available at https://youtu.be/JJZKfHMUeCI.
ViB1 Highway merging
We first consider a very simple highway merging scenario, where an entry lane merges into a singlelane 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.


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.ViB2 Intersection crossing


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 symmetrybreaking 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 online^{3}^{3}3https://youtu.be/cl32nbceZvw.
ViB3 Roundabout driving
Finally, the third scenario consists of vehicles driving inside a twolanes 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 online^{4}^{4}4https://youtu.be/pLoG32wFnkE.
ViB4 Computation time
Due to the relatively short time horizon needed to ascertain infinite horizon safety, computation time remains reasonable despite the NPhardness 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 i76700K 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 realtime 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.


Vii Discussion on implementation
In the previous sections, we presented an optimizationbased algorithm for the supervision of semiautonomous 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 realworld constraints.
Viia 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 “nonconservatively defensive strategies” [41] could also be applied.
A limitation of this simple approach is that it can lead equipped vehicles to often yield rightofway 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.
ViiB 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 FHSP. 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 FHSP) from the roadside supervisor. The vehicle’s onboard 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 worstcase 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 selfpositioning 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 semiautonomous 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 humandriven vehicles, traveling inside predefined corridors with the help of existing lanekeeping technologies. Based on this set of constraints, a discretetime 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 realtime 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 nonequipped 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
Aa 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 IHSP 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 nostop condition (17) ensures that, for all and all
Comments
There are no comments yet.