Optimal Conflict Resolution for Vehicles With Intersecting and Overlapping Paths

A collaborative centralized model predictive controller solving the problem of autonomous vehicles safely crossing an intersection is presented. The solution gives optimal speed trajectories for each vehicle while considering collision avoidance constraints between vehicles traveling on the same path before, after and/or within the intersection. This extends earlier results, where collision avoidance was only considered for vehicles with intersecting paths, with the possibility of vehicles on the same path and by this, the controller is not only one step closer to handling complex traffic intersections but can now be used for merging and splitting of roads, roundabouts and intersection networks. The proficiency of the extended controller is demonstrated by applying it to a four-way intersection. It is shown that the controller provides smooth, collision free trajectories in scenarios with and without vehicles traveling in the same lane. Further, it is evaluated how the solutions differ when using various cost functions and how the controller handles disturbances in the form of a sudden lane blockage. Lastly, it is discussed how the presented controller could also be extended to handle mixed-traffic scenarios and how soft constraints can be used to avoid infeasibility in the case of missing or noisy traffic data.

The set of samples in the exit lane where the paths of Vehicles n and m does not overlap.U p 0 Set of discrete control signals along the prediction horizon.

LIST OF OPERATORS (•)
Derivative with respect to distance.

I. INTRODUCTION
T HE DEVELOPMENT of Intelligent transportation systems (ITS) has been a major subject for the automotive industry as well as governmental institutions in recent years, due to its potential for safer, smarter and greener solutions [1].Autonomous driving is one of the areas undergoing extensive research within ITS.
One of the most extensively researched applications of autonomous driving is the intersection problem [2], [3], [4].The intersection problem is the problem of determining the crossing order and collision free trajectories of a number of autonomous vehicles traveling through an intersection.
Among the main motivations for studying the intersection problem are that they are prone to congestion and accidents.For example, in Europe, intersection-related accidents are responsible for 21% of traffic related deaths and 43% of nonfatal injuries [5].Similar numbers have been reported for the U.S. [6].Due to this accident risk, intersections are highly regulated by traffic lights, signs and road markings, which increases the risk for congestion.Autonomous vehicles have been suggested as one way of reducing the accident risk and congestion, simultaneously.
To solve the intersection problem using MPC, the problem is modeled as an optimization program, where constraints enforce collision avoidance, as well as state and control bounds.This optimization program, called an optimal control program, is solved over a prediction horizon to determine the future state and control signals of the vehicles.However, to account for deviations in the traffic environment from the predicted one, on which the optimization is based, it is necessary to regularly take new traffic information into account.This is done by repeatedly solving the optimization problem over a moving prediction horizon, containing updated traffic information [17].
One difficulty of solving the intersection problem is that it is a combinatorial problem when the crossing order of the vehicles is unknown.This leads to a computationally expensive MPC.To handle this, it is common to split the intersection problem into two parts; one combinatorial and one non-combinatorial.
The non-combinatorial part consists of determining the optimal trajectories, including longitudinal speed and acceleration, of each vehicle crossing the intersection, under the assumption that it is known in which order the vehicles cross and that each vehicle is traveling along a fixed path inside the intersection.There are several suggested algorithms for solving the non-combinatorial part of the intersection problem.The main difference between them is how the collision avoidance constraints are handled.In [11], [12] collision avoidance is modeled by introducing constraints that prohibit more than one vehicle to occupy the intersection at the same time.This is suboptimal since it excludes solutions where vehicles with non-intersecting paths cross simultaneously.Less conservative but more complicated collision avoidance constraints can be implemented by either splitting the physical area into quadrants [18] or by introducing local critical zones centered around the points where the predefined paths of vehicles intersect [13].Another difference between the MPC algorithms is if the sampling is done in traveling time or distance.The main advantages of sampling in distance is that all samples at which collision is possible is known a-priori and the simplicity of minimizing time by introducing traveling time as an optimization variable.The resulting optimization problems are usually quadratic problems (QP) or nonlinear QPs, both of which can be solved efficiently using existing methods.
The combinatorial part consists of finding the order in which the vehicles should cross the intersection.Since the number of possible crossing orders grows rapidly with the number of vehicles, a crossing order is often found by applying a heuristic to reduce the computational complexity.Proposed heuristics range from reservation systems, in which vehicles reserve a time at which to cross the intersection [19], [20], to more advanced heuristics where the crossing order is determined based on the solution of a mixed-integer program [18].The downside of heuristics is that there is no guarantee that the optimal crossing order is found, and the chosen one might be much worse than the optimal one.This would lead to higher emissions, higher congestion and making safety critical solutions more prevalent.An alternative to heuristics is to solve an optimization program to determine the optimal trajectories of the vehicles for all possible crossing orders.Solving for all crossing orders gives the optimum but is computationally expensive due to the vast number of possible crossing orders.It is, however, possible to somewhat reduce the number of crossing orders through removal of apparent non-optimal orderings in an ad-hoc fashion.Additionally, by assuming no overtaking and removing orders which give solutions that are identical to another solution the number of crossing orders can be reduced even further.This can greatly reduce the number of crossing orders that need to be solved for some scenarios [13].
This paper expands upon the approach previously presented in [11] and [13].Therein, a collaborative centralized MPC with an optimal control program sampled in distance is proposed for control of all autonomous vehicles in the intersection.The approach presented in [11] and [13] is limited to a single vehicle in each entry and exit lane when computing the crossing order as well as the vehicle trajectories.In [13], the collision avoidance inside the intersection is performed using local critical zones between every vehicle pair with intersecting paths to decrease the conservativeness of the algorithm and increase the throughput.In this paper, the MPC is expanded by allowing any number of vehicles in the same entry lane, the same exit lane or traveling along the same path throughout the intersection (this is the case if vehicles have the same entry and exit lane), which demands the introduction of additional collision avoidance constraints to prevent rearend collisions.It is shown how the collision avoidance for local critical zones and vehicles along the same path can be written using the same linear formulation applied at different samples.Secondly, in a case study results are presented in which it is shown how the controller behaves for different cost functions and in the presence of disturbances in the form of lane blockage; which forces the central control to redirect vehicles onto different paths.Thirdly, it is concluded that the optimal control program can be extended to a wider set of applications, including, but not limited to, roundabouts and intersection grids due to its function as a cooperative speed controller when vehicles travel along the same paths.
The paper is organized as follows.Section II discusses the features and limitations of the intersections which the controller presented within this paper handles.In Section III the intersection problem is formulated and all constraints needed for the optimal control program are presented.In Section IV the full continuous optimal control program is given.In Section V the optimization problem introduced in Section IV is discretized and formulated in a receding horizon fashion.Section VI contains a case study in which it is demonstrated that the MPC can plan collision free and smooth trajectories for vehicles traveling through a fourway intersection.It is further shown that the local zones are less conservative than using the full intersection as a critical zone, how solutions differ when applying different cost functions and that the MPC can handle a sudden lane blockage.Section VII discusses extensions of the controller, such as inclusion of non-autonomous vehicles, solving for more general intersections, roundabouts and intersection grids.Lastly, it discusses the possibility to introduce soft constraints and how to determine the crossing order, before conclusions are drawn in Section VIII.

II. INTERSECTIONS
A traffic intersection may include crosswalks, bicycle paths, lanes with limited travel options and divisional islands [21].While the methods in this paper can be applied to such a general intersection, the example used is a four-way intersection containing motorized vehicles only.
The physical area of an intersection is the area in which the roads merge [22].Each road that connects to the physical area is referred to as an intersection leg or leg for short.In this paper, the legs are numbered 1, 2, . . ., L.Besides its leg number, a leg is identified via its leg angle.The leg angle is the angle measured counterclockwise from the horizontal axis to the leg.Each leg consists of a number of lanes; the lanes where vehicles are traveling towards the physical area are called entry lanes and the lanes in which vehicles travel away from the physical area are called exit lanes.When vehicles enter the physical area via one of the entry lanes, they can travel through the physical area to any or a subset of the exit lanes, depending on the traveling limitations of the intersection.Lastly, a closed curve containing the physical area within its interior is introduced.Any vehicle inside this closed curve is assumed to be part of the intersection.This closed curve is called the control boundary.
As an example, consider the four-way intersection depicted in Fig. 1, where vehicles are allowed to turn right, drive straight or turn left inside the physical area, depicted in gray.There are four legs labeled Leg 1, Leg 2, Leg 3 and Leg 4 with leg angles 0 • , 90 • , 180 • and 270 • .Each leg consists of one entry lane and one exit lane, as shown by the arrows implying the direction of travel.There are a total of 12 predefined paths, three from each entry lane leading to each exit lane which does not belong to the same leg as the entry lane, see Fig. 1.It is assumed that all vehicles are traveling along one of these paths.Thus, a vehicle can change its predefined path to another path, only before reaching the physical area.Doing so inside the physical area would necessitate the introduction of additional paths.This is possible, but outside the scope of this paper.Lastly, the red circle in Fig. 1 is the control boundary.

III. PROBLEM FORMULATION
Mathematically, any intersection is represented by the tuple x i (s), y i (s), κ i (s), ψ i (s), v sl (s), I, s , where x i (s), y i (s) are the global path coordinates for the predefined path i ∈ I = {1, 2, . . ., M}, κ i (s) is the path curvature and ψ i (s) is the path angle defined by the tangent angle at the sample s, which is the distance traveled along the arch length of the path.In general, the speed limit of the roads can vary between legs, but for simplicity it is in this paper assumed that all legs have the same speed limit v sl (s).Now, consider N > 0 autonomous vehicles traveling through the intersection.For each Vehicle n within the set of vehicle indices N = {1, . . ., N}, it is assumed that 1) the vehicle travels along one of the M predefined paths; 2) the assigned path is perfectly followed; 3) the speed v n ∈ R and acceleration a n ∈ R of the vehicle along the path can be varied; 4) the vehicle is not overtaking within the control boundary; 5) the vehicle clock is synchronized with that of the other vehicles.

A. VEHICLE PATHS
Each Vehicle n ∈ N is traveling along a path for all n ∈ N and all p ∈ [0, p f ].Thus, p is a sampling variable representing the arc length of any vehicle along its predefined path relative to its starting position p n0 .For the sake of brevity, x n (p + p n0 ) will be written as x n (p) in the rest of the paper.

B. LONGITUDINAL DYNAMICS
Since sampling is done in distance via the sampling variable p, the traveling time of a vehicle at any sample is unknown.Therefore, the traveling time of each vehicle, t n (p), is chosen as a state and fulfills t n (p) = 1/v n (p), where (•) denotes the derivative with respect to distance, i.e., d / d p.
To get a linear state space model, n additional states are introduced, representing the inverse speed z n (p) = 1/v n (p) of each vehicle, see [11] for details.The state vector for Vehicle n reads resulting in a total of 2N states, two for each vehicle.Each Vehicle n is represented by a linear system where the control signal u n (p) is the derivative of the inverse speed of Vehicle n, i.e., u n (p) = z n (p), and

C. STATE AND CONTROL CONSTRAINTS
Each Vehicle n ∈ N is subject to state and control constraints where each inequality is imposed for all p ∈ [0, p f ] and Here, 0 < v min n (p) ≤ v max n (p) denote the minimum and maximum speed of Vehicle n, respectively.
An expression for the minimum and maximum bounds u min n (p, z n ) and u max n (p, z n ) on the control signal is found by using the lower and upper limit of the acceleration denoted a min n (p) < 0 and a max n (p) > 0, respectively.Observe that From this, the bounds on the control signal in (1) are given by For details on the choice of bounds, see [11] and [13].

1) MOTION ON CURVED PATHS
When vehicles turn right or left they move on paths with significant curvature, see Fig. 2. Thus, they are subjected to non-negligible lateral forces, which should be constrained to lie within a range that is realizable by actual vehicles.Since, the largest part of the lateral acceleration in a curve stems from the centripetal force, a maximum bound on the lateral acceleration is implicitly applied through the choice of the upper bound on the longitudinal speed.If the maximum lateral acceleration of Vehicle n is given by a max n lat , the upper bound on the longitudinal speed is equal to a max n lat /κ n (p).Adopting the convention that division by a curvature of zero yields infinity, the maximum speed for Vehicle n at each sample is given by . This means that when traveling along a straight part of the path the maximum speed is v sl (p) while in a curve the maximum speed is the smallest of a max n lat /κ n (p) and v sl (p).

D. COLLISION AVOIDANCE CONSTRAINTS
To find collision free solutions, the collision avoidance constraints need to guarantee that there are no collisions along overlapping or intersecting paths.Here, it is understood that two vehicles have intersecting paths if their paths have the same global coordinates at exactly one point, while they have overlapping paths if their paths have the same global coordinates at more than one point.As an example, the paths of Vehicles 1 and 3 in Fig. 2 are said to intersect, while the paths of Vehicles 3 and 4 (and 1 and 2) are said to overlap.
A vehicle traveling along a path which does not intersect or overlap with the path of any other vehicle is assumed to not be at risk of collision.A point where two vehicle paths have the same global coordinates is referred to as a collision point.
For intersecting paths, the collision avoidance constraints can be interpreted as a critical zone who's entry and exit samples guarantee that only one vehicle resides within the critical zone at any given time, see the yellow rectangle in Fig. 2.
For overlapping paths, a safety distance between the vehicles needs to be maintained to prevent rear-end collisions.This is done by imposing collision avoidance constraints at every sample along the overlapping path.
Let O ∈ R N represent a given crossing order, i.e., O is a vector containing a permutation of the vehicle indices in N. Also, let O l be the l:th entry of O.Then, a Vehicle n with the predefined path (x n (p), y n (p), κ n (p), ψ n (p), p) crossing before a Vehicle m with the predefined path (x m (p), y m (p), κ m (p), ψ m (p), p) has collision points with Vehicle m at the samples where (x n (p), y n (p)) = (x m (p 1 ), y m (p 1 )) indicates that the paths of the two vehicles pass through the same point and n = O l , m = O l, l < l mean that Vehicle n crosses before Vehicle m.For vehicle pairs with overlapping paths, the set P nm is an interval, for intersecting paths it is a point and for vehicle pairs which have neither overlapping nor intersecting paths, it is empty.However, imposing collision avoidance constraints directly at the entries of P nm is not enough since it does not take into account the physical dimensions of the vehicles, the reference points of the vehicles nor the fact that the samples in P nm are samples along the path of Vehicle n, which are not necessarily equal to the samples of the path of Vehicle m due to the shape of the path and/or different initial positions, see Fig. 3. To account for all of this, the scalars p nm , p mn are introduced to shift the samples for Vehicle n and m, respectively.
The collision avoidance constraint between Vehicle n and Vehicle m is a linear constraint on the traveling time nm , ∀p ∈ P nm , (3) where t hw nm is a non-negative, typically positive, headway time between Vehicle n and Vehicle m.
As an example, consider the scenario depicted in Fig. 2 with the crossing order [1 2 3 4].To form the collision avoidance constraints we first find the collision points, i.e., all nonempty sets P nm .The nonempty sets are: P 13 which contains the sample at which the path of Vehicle 1 intersects with the path of Vehicle 3, P 34 which contains the samples at which the path of Vehicle 3 overlaps with the path of Vehicle 4 and P 12 at which the path of Vehicle 1 overlaps with the path of Vehicle 2. For Vehicles 1 and 3 the shifts, p 13 > 0 and p 31 < 0, are chosen such that P 13 + p 13 is the sample at which Vehicle 1 exits the critical zone, and P 13 + p 31 is the sample at which Vehicle 3 enters it, see Fig. 2. The collision avoidance constraint then reads 13 , and guarantees that Vehicle 1 exits the critical zone before Vehicle 3 enters it.
For the collision avoidance between Vehicles 1 and 2 the shift p 21 < 0 is chosen such that a safety distance between the vehicles is introduced and the shift p 12 > 0 is chosen to avoid collision at samples shortly after the paths split, see Fig. 3 which depicts a zoom in of the paths of Vehicle 1 and 2 along with samples (horizontal markings) and the sets P

E. FEASIBILITY BEYOND THE HORIZON
The collision avoidance constraints (3) guarantee that no collisions occur along overlapping and intersecting paths defined within the prediction horizon [0, p f ].However, since vehicles with the same exit lane do not necessarily have the same (absolute) position at the final sample p f , there is still a risk of collision in the exit lane.As an example, consider again the scenario in Fig. 2. As illustrated by the yellow and purple lines, Vehicles 3 and 4 have the same exit lane and same path length but different final positions due to different starting positions and entry lanes.The collision avoidance constraints (3) only guarantee that Vehicles 3 and 4 do not collide within the prediction horizon, but not that collision is avoidable after the path of Vehicle 3 ends.
To guarantee collision avoidance in the exit lane after a vehicle reaches the end of their predefined path, additional constraints are introduced on the velocity and final acceleration of the vehicles.The constraints on the acceleration at the last point of the horizon read where 1 and 2 are small positive scalar numbers.This constraint guarantees that the acceleration of all vehicles is close to zero at the last sample and it is assumed that after the horizon the acceleration goes directly down to zero.Thus, adding a constraint which enforces that a vehicle traveling behind another vehicle in an exit lane has a velocity that is not higher than the vehicle in front, when the front vehicle reaches its end position within the prediction horizon, would guarantee collision avoidance beyond the horizon.To formulate such a speed constraint, it is necessary to find the set of all vehicle pairs that have the same exit lane.Further, we need to know which vehicle travels the shorter distance in the exit lane.This is found by studying the shifts p nm , p mn in the collision avoidance constraints (3).If the vehicles n and m have the same exit lane then p nm = 0 if vehicle m is crossing first and p mn = 0 if Vehicle n is crossing first.Hence, exactly one of the shifts is always zero for overlapping paths and identifies which vehicle has the least number of samples in the exit lane.Hence, the set denotes all vehicle pairs with the same exit lane when the vehicle in front, n, has the shorter path, while , where are the sets of samples between the endpoints of the paths of Vehicle n and m.

IV. OPTIMAL CONTROL PROGRAM
Let x n0 = [0, 1/v n0 ] T be the initial state vector of Vehicle n where v n0 is the initial velocity of Vehicle n.Then, the optimal control program, for a given crossing order, is formulated as where the constraints (5b)-(5d) hold for all p ∈ [0, p f ], (5e) holds for all p ∈ P nm , (5f) holds for all p ∈ P f 1 nm and (5g) holds for all p ∈ P f 2 nm .All constraints in (5) are linear except for the nonlinear and non-convex input constraints (5d).The program can be iteratively solved using linearized convex subproblems.This solution method is commonly known as sequential convex programming (SCP), or, if the subproblem is a quadratic program, sequential quadratic programming (SQP) [23].The choice of objective function (5e) is discussed in detail after discretization of the optimal control program, in Section V. Next, the linearization of the constraints (5d) is discussed.

A. LINEARIZATION OF INPUT CONSTRAINTS
To see that the constraints (5d) are concave, recall that the bounds are given by ( 2) and rewrite the bounds to ≤ 0, and calculate the Hessian of both functions The Hessians are negative definite since a min n < 0, a max n > 0 and z n (p) > 0 for all p, which implies that the constraints are concave [24].The linearization of a concave constraint is always an inner approximation; thus the linearization does not introduce non-realizable solutions.
To find the linearized bounds ũmin n and ũmax n , z 3 n (p) is linearized around the inverse speed z n lin (p)

Using this together with (2) gives
Thus, the subproblem to be solved is (5), where (5d) is replaced by (6).This gives a QP that can be solved efficiently.

V. MODEL PREDICTIVE CONTROL
In this section, the optimal control program ( 5) is discretized and written in a receding horizon fashion.Let k ∈ {p 0 , p 0 + p, . . ., p 0 + p f } = P be the discretized sampling variable, P the set of discrete distance samples corresponding to the continuous sampling interval [0, p f ], p 0 the first sample and p the length of the discrete sampling interval.To simplify the formulation of the discretized optimal control program, the state and control variables for all vehicles at sample k are stacked into the vectors where x 1,k,p 0 is the state vector of the first vehicle at iteration k when the sampling starts at p 0 and u 1,k,p 0 is the corresponding control signal.Discretizing the dynamical model (5b) using zero-order hold yields where ξ 0 is the initial state vector stacked in the same order as the states in (7).Further, the state and input matrix is given by , and Here Letting the full input sequence be U p 0 = [u p 0 ,p 0 , u p 0 + p ,p 0 , ... ,u p 0 +p f ,p 0 ], the discretized optimal control minimize U p 0 subject to ξ k+ p,p 0 = A d ξ k,p 0 + B d u k,p 0 , ∀k ∈ P \ P f , (8b) where ξ min k,p 0 , ξ max k,p 0 , ũmin k,p 0 and ũmax k,p 0 are vectors containing the stacked bounds and initial states, respectively, in the same way as the states and inputs in (7).Lastly, the cost function J d is the discretized version of (5e).
After solving the discretized problem (8), the first optimal control signal, u * p 0 ,p 0 , is used to calculate the states of all vehicles at sample p 0 + p.This corresponds to all vehicles moving p meters on their paths.Then N, the shifts p nm , p mn , and the set P nm are updated along with the state bounds, control bounds, the initial and final states before the optimal control program is resolved over a shifted horizon.

A. CONVEX COST FUNCTIONS
In this section it is shown how the commonly used cost functions: minimum time, trajectory tracking [25] and minimum discomfort are formulated for the optimal control program (8).For an explanation on how the time formulated costs correlate with the spatial costs presented in this paper, see [11].

1) TRAJECTORY TRACKING
States and inputs are tracked using the cost function Here, Q, R and S are positive definite weighting matrices while ξ r,k,p 0 and u r,k,p 0 are the reference trajectories for the states and inputs, respectively.Further, D u k,p 0 refers to the discrete derivative of u k,p 0 and is included to mitigate high spikes in jerk which may result from using pure tracking costs.Typically, the input reference u r,k,p 0 is zero.

2) MINIMUM TIME
A common goal for traffic control of intersections is to maximize the throughput.Mathematically, this is formulated as a cost function that minimizes the p-norm of the final traveling time of each vehicle, i.e., where Q = diag(q 1 , 0, q 2 , 0, . . ., q N , 0) ∈ R 2N×2N is the weighting matrix for Vehicle n ∈ N. The most intuitive choice of norm is p = 1 which corresponds to minimizing the weighted sum of all final times and p = ∞ which corresponds to minimizing the largest final time.
Minimizing only the time can lead to sudden shifts in acceleration and jerk resulting in an uncomfortable ride for the occupants of the vehicle.This is mitigated by adding penalties for the input signal u k,p 0 and its derivative D u k,p 0 , i.e., (10) where R and S are positive definite weighting matrices.

VI. CASE STUDY
In this section, the proposed MPC is validated on the scenarios in Fig. 4.Besides the solver metrics such as solution time and closeness to the optimal solution there are many metrics that are interesting when studying the effectiveness of an intersection algorithm, including: completion time, i.e., how long it takes before the last vehicle leaves the intersection; how well vehicles track their reference speed (if there is one), average time it takes for a vehicle to travel through the intersection, level of congestion and energy consumption.
Not all these metrics can be optimal simultaneously.For instance, tracking a reference speed and minimizing completion time are competing objectives unless the reference speed is equal to the upper limit of the speed.Which metrics are most important for the current application should factor into the choice of cost function, see Section V-A.
The scenarios in Fig. 4 take place in a four-way intersection where all leg angles are 90 • and each leg consists of two lanes with a width of 5 m.The physical area is 30 m across and the control boundary is a circle with a radius of 90 m whose center coincides with the center of the physical area.For both scenarios, it is assumed that v min n (p) = 1 km/h and where the curvature κ n (p) = 0 corresponds to a straight path, κ n (p) = 0.08 corresponds to a right turn and κ n (p) = 0.0571 corresponds to a left turn.The speed values in (11) correspond to a maximum lateral acceleration of a max n lat = 2 m/s 2 for all n ∈ N and a constant road speed limit v sl = 50 km/h for all intersection legs.The longitudinal acceleration limits are a min n = −3.5 m/s 2 and a max n = 2 m/s 2 for all n ∈ N. The sampling interval is chosen to be p = 1 m.Two cost functions are used in the case study.The first cost function is a combination of speed tracking and minimization of discomfort written on the form (9), with the weights 1) , where and q n v = 1, q n a = 1 and q n j = 0.5 for all n ∈ N are the weights corresponding to a time formulated problem, see [11].Further, v n m is the mean velocity of the linearization velocity, i.e., v n m = mean(1/z n lin ), and the reference vectors are ξ r,k,p 0 = 0, v r,k,1 , . . ., 0, v r,k,N , u r,k,p 0 = 0 where v r,k,n is the reference velocity of Vehicle n.This cost function is referred to as the tracking cost in this case study.The second cost function is a combination of minimizing the sum of the final vehicle traveling times and discomfort.It is written on the form (10) with R and S chosen as for the tracking cost, p = 1 and This is referred to as the minimum time cost in this case study.In the first MPC iteration, when no previous solution is available, the inverse linearization speed which the input constraints (8d) depend on is chosen to be the inverse reference speed of each vehicle n ∈ N for the tracking cost and the upper speed limit for the minimum time cost.Thereafter, for both cost functions, the linearization is done about the previous solution, shifted by one example.
The proposed controller is implemented in MATLAB and the optimal control program (8) is solved using the built-in solver quadprog.All simulations are performed on a laptop with Intel Core i7-5600 CPU at 2.60GHz with 16 GB RAM.
The case study is divided into three parts: comparison between local and global critical zones, a comparison between cost functions and an investigation of how well the algorithm handles a sudden lane blockage.In addition to the figures and analysis presented within this paper there is also an animation showing the optimal solution of each case solved. 1

A. COMPARISON OF LOCAL AND GLOBAL CRITICAL ZONE
In this part of the case study, the proposed controller is applied to Scenario 1; a scenario where four vehicles are moving straight through an intersection, as depicted in Fig. 4(a).This scenario is similar to the one we solved in [13] and is resolved here using the new MPC formulation for repeatability and for its simplicity, before the more complicated Scenario 2 is solved.In addition to the parameters introduced in the previous section, the time headway is chosen to be t hw nm = 1.1 s for all n, m ∈ N and the initial conditions are as shown in the column labeled Scenario 1 of Table 1.The tracking cost is chosen as the cost function.
Scenario 1 is solved for all crossing orders and the crossing order with the lowest cost is chosen, i.e., the crossing order 1. https://chalmersuniversity.app.box.com/folder/140303688929 and vehicle trajectories are optimized.A solution is found for both the case of local critical zones and the case where the full physical area is considered a global critical zone within which only one vehicle is allowed at the time.
The result for the global critical zone is presented in Fig. 5.The Fig. 5(a) shows the traveling time of each vehicle plotted against its position.The gray area represents the physical area, which is also the global critical zone.Since no vehicles have overlapping paths the solution is collision free if there is at most one vehicle inside the physical area at the time.It can be seen that this is the case for the solution in Fig. 5(a) since the time at which a vehicle enters the physical area, represented by a red circle, is always lower than the exit time of the preceding vehicles, represented by a blue square.
The results for the local critical zones are presented in Fig. 6.Here, the traveling times plotted against the vehicle positions are shown in Fig. 6(a).However, since the physical area is no longer the critical zone it is not enough to study this plot to determine if the collision avoidance constraints are fulfilled or not.Indeed, as expected, it is seen in this figure that several vehicles occupy the physical area at the same time.Instead, collision avoidance is verified when there is only one vehicle within each local critical zone at the time.That this is the case in this Scenario is seen in the second column of Table 2, which lists the time difference between the exit of the first vehicle and entry of the second vehicle into each local critical zone, and since all of them are equal to or less than −1.1 s, the solution fulfills the collision avoidance constraints.
From Fig. 5(a) it is seen that the optimal crossing order is [3,4,1,2] when the critical zone is the full physical area.When the problem is solved with local critical zones the optimal crossing order is instead [3,1,4,2] (this is not easy to discern from Fig. 6 and Table 2).Thus, when using local critical zones, compared to a global critical zone, Vehicle 1 swaps places with Vehicle 4 in the crossing order.This together with the less conservative collision avoidance constraints that allow for several vehicles inside the physical area at the same time makes it possible for the vehicles to accelerate/decelerate less and keep a speed closer to their reference throughout the simulation, compare Fig. 5(b) and Fig. 6(b).Especially vehicles 1 and 2 have to decelerate significantly more in the case of the global critical zone and due to this, it takes the last vehicle, Vehicle 2, 14.34 s to leave the physical area for the global critical zone while only 8.87 s to leave the physical area in case of the local critical zones.

B. COMPARISON OF COST FUNCTIONS
In this section, a scenario is solved with two vehicles in each of the four lanes, see Fig. 4(b).This scenario is referred to as Scenario 2 throughout the rest of the paper.Scenario 2 is here solved using the tracking cost and the minimum time cost, see Section V-A.
In addition to the parameters introduced in Section VI, the time headway is chosen to be t hw nm = 0.7 s for all vehicles n, m ∈ N with overlapping paths and t hw nm = 1.1 s for all vehicles n, m ∈ N with intersecting paths.Finally, the initial conditions are as shown in the third column of Table 1.
The solution obtained when applying the tracking cost is presented in Fig. 7, while the corresponding solution for using the minimum time cost is shown in Fig. 8.
To determine that the tracking cost solution is collision free, we study Fig. 7(a) and the third column of Table 2.The Table 2 lists the headway difference for all vehicles with intersecting paths, in the same way as for Scenario 1. Fig. 7(a), depicts the headway difference between all vehicles with overlapping paths against the position.The thicker part of the lines corresponds to the positions at which  collision avoidance constraints are active.For example, the red line labeled t 1 − t 5 is thick for all the plotted samples since Vehicles 1 and 5 have the same path, while the brown line, labeled t 5 − t 7 , is thick from 110 m onward since the Vehicles 5 and 7 have overlapping paths after the physical area.Here, a circle indicates the start of the overlapping path and a square represents its end.Since the headway difference is below −0.7 s for all vehicle pairs with overlapping paths at all samples where there is a risk of a rear-end collision and the headway difference is below −1.1 s for all vehicle pairs with intersecting paths, the solution obtained by applying the tracking cost is collision free.The corresponding plot and table column for the minimum time objective, Fig. 8(a) and the fourth column of Table 2, show that the minimum time solution is also collision free.
The headway difference plots for the tracking cost, Fig. 7(a), and the minimum time cost, Fig. 8(a), are similar and so are the headway differences for the intersecting paths, see Table 2.However, if one studies the velocities and accelerations plotted in Fig. 7(b) and Fig. 8(b) there are some differences.In the case of the minimum time cost Vehicle 1 applies maximum acceleration at the start of the scenario and almost reaches the speed limit of 50 km/h.However, for the tracking cost, where 50 km/h is the speed limit and the speed tracked, Vehicle 1 stays far below that value until after the physical area has been passed.A similar pattern is seen for most other vehicles, where the speed for the earlier samples are kept higher in the minimum time solution than in the tracking solution.Further, the velocities of Vehicles 7 and 8 have smoother trajectories in the minimum time cost.The price to pay for using the minimum time cost is increased acceleration of Vehicle 1 at the start of the simulation.It is also seen that the vehicles accelerate faster, up to 50 km/h, after the physical area for the minimum time case than in the tracking case.As expected, the sum of travel times until all vehicles reach their final position is shorter for the minimum time cost, 191.4 s, than for the tracking cost, 197.7 s, while the last vehicle (Vehicle 8) reaches the last sample at 30.1 s and 29.5 s, respectively.Hence, the minimum time solution pushes the vehicles through the intersection faster than the tracking cost.

C. LANE BLOCKAGE
In this last part of the case study, Scenario 2 is solved with an added disturbance.The disturbance is the blockage of the exit lane of Leg 2. The blockage forces the controller to redirect all vehicles, in this case only Vehicle 3, from the exit lane of Leg 2 to another exit lane.In this scenario it is assumed that Vehicle 3 is redirected to the exit lane of Leg 4, i.e., after the lane blockage all vehicles travel along the paths defined in Fig. 4(b) except Vehicle 3, which is now traveling along the same path as Vehicle 7. The Scenario is solved for 3 cases: when the blockage occurs after 10, 30 and 50 samples.Note that in all these three cases the blockage occurs prior to Vehicle 3 reaching the physical area to guarantee that it can choose one of the other predefined paths.Lane blockages occurring after Vehicle 3 has entered the physical area would require a method to construct a new path for the vehicle to follow, which is not a part of this paper.
The result for the three cases is presented in Table 3 and Figs.9(a)-9(c).The table lists the minimum time headway along overlapping paths and the headway difference for intersecting paths.The time headways are 1.1 s for all intersecting paths and 0.7 s for all overlapping paths.Therefore, Table 3 shows that the solver yields collision free solutions for all three cases.
To see the effect of the blockage, the three Figs.9(a)-9(c) are compared.The direct consequence of the blockage is that Vehicle 3 takes a right turn instead of a left turn, which forces it to lower its speed when getting close to the physical area.As is seen in Fig. 9(a)-9(c) this deceleration starts at the moment the blockage occurs, and is more severe the later the blockage occurs.Further, the change of the path of Vehicle 3, makes it possible for Vehicle 2 to get through the physical area faster due to it no longer having to wait for Vehicle 3.This is why the speed and acceleration of Vehicle 2 jumps in Figs.9(a)-9(b) at the sample where the blockage occurs.

TABLE 3. Minimum headway difference between vehicle pairs at risk of collision.
Values lower than −0.7 s for overlapping paths and lower than −1.1 s for intersecting paths imply there is no violation of the collision avoidance constraints.This jump is not as prominent in Fig. 9(c), since Vehicle 2 is close to the physical area when the blocking occurs and has a speed already close to its allowed maximum inside the physical area, 21.3 km/s.Similar behavior is observed for Vehicle 6.This is due to the fact that Vehicle 6 travels behind Vehicle 2 before the physical area, which allows it to accelerate when Vehicle 2 does.
Lastly, the speed and acceleration of Vehicle 1 is affected.This is because it has the same exit lane as Vehicle 3 after the blockage occurs.Since, the crossing order is [1,3,2,5,6,7,4,8], Vehicle 1 has to accelerate ones the controller becomes aware of the blockage to reach the exit lane before Vehicle 3.This is illustrated by the jump in acceleration at the occurrence of the blockage in Figs.9(a)-9(c).

VII. WIDER APPLICATIONS OF THE CONTROLLER
In this paper, it has been shown how the algorithm for cooperative coordination of vehicles driving in traffic intersections is extended to include vehicles traveling in the same lane.This enables the algorithm to be used as a cooperative speed controller before and after the intersection.Further, this enables other extensions, e.g., cooperative control in roundabouts and intersection grids.Other useful extensions include the possibility of handling mixed traffic, optimization of the crossing order and avoidance of feasibility problems in the presence of noisy or non-existing data using soft constraints.

A. MIXED TRAFFIC
Since it is not realistic that all of today's vehicles are replaced by autonomous vehicles at the same time, it is beneficial for intersection algorithms to handle mixed traffic, i.e., traffic with both autonomous and non-autonomous vehicles.
It is possible to include non-autonomous vehicles into the control program (8) given that the centralized controller has access to a predefined path and planned/predicted longitudinal speed along the path for all non-autonomous vehicles.Then, collision avoidance constraints similar to (8e)  can be formulated between non-autonomous and autonomous vehicles.However, it is not possible to prevent nonautonomous vehicles from colliding with each other.

B. A GRID OF INTERSECTIONS
Since, the optimization program (8) models collision avoidance for vehicles with intersecting and overlapping paths, it is straightforward to generalize the MPC to a grid of intersections, as the one in Fig. 10.For each intersection, a program identical to (8) is solved, and in between intersections, i.e., outside of the control boundaries, the same program but with only overlapping collision avoidance constraints is solved.If the distance between intersections is small, it is possible to use a large control boundary encompassing multiple intersections and regarding them as a single large intersection while if the distance between the intersections is large, one might consider using a controller between the intersections that permit overtaking, e.g., the one proposed in [26].

C. GENERALIZED INTERSECTIONS
The algorithm can be applied to other types of intersections besides the four-way intersection.As long as the predefined paths are well defined for all vehicles, the intersection can have any number of legs and lanes.It is also possible to apply the algorithm to merging and splitting of roads.One application that is enabled due to the extended possibility of avoiding rear-end collisions is cooperative driving in roundabouts, Fig. 11.However, including road users with a different movement pattern, such as pedestrians and/or bicyclists is more complex, since it would require predicting their path and speed along that path.Such predictions are often based on a probabilistic model and would require computation of robust bounds to be implemented in the proposed algorithm.

D. DETERMINING THE CROSSING ORDER
One straightforward way of determining the crossing order is to solve the optimization program (8) for all possible crossing orders and choose the solution with the lowest cost (or one of the solutions with the lowest cost).However, the number of possible crossing orders increases rapidly with the number of vehicles.If every vehicle is traveling along its own lane, the number of possible crossing orders is equal to the number of ways to permute the vehicle indices.This means that for N vehicles there are N! possible crossing orders.As an example, if eight vehicles are crossing the intersection, there are 8! = 40320 possible crossing orders.However, typically there are more vehicles than intersection legs, which lowers the number of possible crossing orders, due to the assumption of no overtaking.Considering the scenario depicted in Fig. 4(b), which contains eight vehicles but only four intersection legs, the number of combinations that need to be considered when removing the possibility of overtaking is 2520.Further, some crossing orders might be identical in the sense that they provide the same cost when (8) is solved.This is due to the fact that interchanging two adjacent vehicles in the crossing order, which do not have intersecting or overlapping paths, does not affect the formulation of the optimal control program (8).
By only considering the crossing orders in the Scenario in Fig. 4(b) which provides a unique solution to (8), the number of orderings are reduced further to 14.While it was possible to reduce the number of crossing orders significantly in this particular case, the reduction is heavily dependent on the predefined paths of the vehicles.For more details on how to find the unique orderings see the paper [13].

E. SOFT CONSTRAINTS
In the case study, Section VI, the measurement of states and inputs are assumed to be perfect and instantaneous.In reality, noisy and/or delayed measurements might lead to small violations of constraints and infeasibility.A common solution to this is to use soft constraints [24].This introduces slack variables for the state bounds (8c) and collision avoidance constraints (8e), which allows for violation of these constraints.The square of these slack variables are then highly penalized in the cost to discourage constraint violation.

VIII. CONCLUSION
In this paper, a centralized model predictive controller for optimal control of autonomous vehicles at intersections is presented.The intersection is defined mathematically as a number of known paths along which the vehicles are allowed to travel.The optimal control program is based on the sequential quadratic modeling first introduced in [11], but extended with collision avoidance constraints for vehicles with the same entry lane, the same exit lane and traveling along the same path throughout the intersection.The optimal control program is formulated in the spatial domain where the vehicle speed is replaced by its inverse.
The suggested model predictive controller can handle any type of intersection consisting of intersection legs and a physical area where the vehicles can go straight, turn left and/or turn right.This includes merging and splitting of roads.It can further handle multiple vehicles traveling in the same lane, sudden lane blockages.
The controller is shown to give smooth and collision free solutions in a number of scenarios and with different objective functions.It is also shown that the formulation of the optimal control program only contains collision avoidance constraint between vehicles that are at risk of collision, instead of between all vehicles entering the physical area.It is shown that since the optimal control program only implies collision avoidance constraints between vehicles that are at risk of collision, instead of between all vehicles entering the physical area, the throughput is increased.

FIGURE 1 .
FIGURE 1.An intersection with four legs, each with one entry and exit lane.There are 12 predefined paths, 3 starting from each entry lane.The paths starting from the first leg are depicted in blue.The red circle represents the control boundary.The gray area is the physical area.

FIGURE 2 .
FIGURE 2. Autonomous vehicles in an intersection.Each Vehicle n has a predefined path pn depicted by a line.Vehicle 2 and Vehicle 4 are traveling along paths with significant curvature.Vehicles 1 and 3 have intersecting paths which give rise to the yellow rectangular critical zone.Collision avoidance constraints are imposed such that only one vehicle resides within the critical zone at the time.Vehicles 1 and 2 have overlapping paths prior to the physical area while vehicles 3 and 4 have overlapping paths after the physical area.

FIGURE 3 .
FIGURE 3. A zoom in on the paths of Vehicle 1 and 2 in Fig. 2. The horizontal markings are samples, where red markings denote samples along the red path, the blue markings denote samples along the blue path and green markings denote samples which belong to both paths, i.e., the samples in P12.The blue and red cross define the starting position of the Vehicle 1 and Vehicle 2, respectively.The numbers represent the distance, in samples, from the initial position.Rear-end collisions are avoided by restricting the time difference between all samples in the shifted intervals P12 + p12 and P12 + p21.
p mn > 0 , denotes all vehicle pairs with the same exit lane when the vehicle in front, n, has the longer path.Here, ψ m (p f ) = ψ n (p f ) guarantee vehicles have the same exit lane and n = O l , m = O l, l < l guarantee Vehicle n crosses before Vehicle m.The speed constraints for the last few samples are then written as

FIGURE 4 .
FIGURE 4. Scenarios investigated in the case study.

FIGURE 5 .FIGURE 6 .
FIGURE 5. Results of solving the optimal control program for Scenario 1 with a tracking cost function and using the physical area as a global critical zone.

FIGURE 7 .
FIGURE 7. The solution for Scenario 2 using the tracking cost.Fig. a) The thick part of the coloured lines represent the positions at which the paths of the vehicle pairs overlap.Overlapping paths are collision free if the thick part of the lines are below the negative headway −0.7 s, depicted by the black dotted line.b) The acceleration bounds are fulfilled since the acceleration of all vehicles is between the dotted limits.

FIGURE 8 .
FIGURE 8.The solution for Scenario 2 using the minimum time cost.Fig. a) The thick part of the coloured lines represent the positions at which the paths of the vehicle pairs overlap.Overlapping paths are collision free if the thick part of the lines are below the negative headway −0.7 s, depicted by the black dotted line.b) The acceleration bounds are fulfilled since the acceleration of all vehicles is between the dotted limits.

FIGURE 9 .
FIGURE 9.The speed and acceleration for three cases where Vehicle 3 in Scenario 2 is forced to change its exit lane from Leg 2 to Leg 4 due to a blockage in the exit lane of Leg 2 after 10, 30 and 50 samples.

FIGURE 10 .
FIGURE 10.A grid of four intersections.Each blue circle represents the control boundary of the respective intersection.

FIGURE 11 .
FIGURE 11.A roundabout with four legs, each with an entry and exit lane.Paths starting in the first leg shown in blue.