Centralized Coordination of Connected Vehicles at Intersections using Graphical Mixed Integer Optimization*

—This paper proposes a centralized multi-vehicle coordination scheme serving unsignalized intersections. The whole process consists of three stages: a) target velocity optimization: formulate the collision-free vehicle coordination as a Mixed Integer Linear Programming (MILP) problem, with each incoming lane representing an independent variable; b) dynamic vehicle selection: build a directed graph with result of the optimization, and reserve only some of the vehicle nodes to coordinate by applying a subset extraction algorithm; c) synchronous velocity proﬁle planning: bridge the gap between current speed and optimal velocity in a synchronous manner. The problem size is essentially bounded by number of lanes instead of vehicles. Thus the optimization process is real-time with guaranteed solution quality. Simulation has veriﬁed efﬁciency and real-time performance of the scheme.


I. INTRODUCTION
Intersection capacity is a limit to transportation efficiency.A jammed crossing will deteriorate safety, efficiency, gas emission, as well as passengers experience (because of frequently stop-and-go operations).Much effort has been contributed to this field due to fore-mentioned reasons.
Researches on vehicles at signalized intersections mostly focus on the energy consumption [1] [2], while there are different concerns about the coordination of vehicles at intersections without traffic signalthe balance between safety and efficiency, the real-time performance and quality of solution, as well as the dynamic change of variables to be optimized.
The classic principle of First Come, First Serve (FCFS) was proposed in [3], where vehicles dynamically apply for right of way to the intersection agent, and the latter responds according to current reservations by previous vehicles.However, no much optimization is done with regard to passing sequence in such reservation-based methods.
Optimization-based intersection management is an active field, where the numerical solution has been an issue.Li et al [4] propose a concept of safety driving patterns to represent the collision-free movements of vehicles at crossings.As the authors point out, the complexity of this cooperative driving planning will increase quickly with the number of vehicles.Kamal et al [5] adopt a risk expression based on a 2-D Gaussian function, of which the variables are vehicles distance to the intersection.The proposed approach requires initial values or guess of the solution.Joyoung Lee et al [6] *This work is supported by National Key R&D Program in China with 2018YFB1600600.
Q. Ge, Q. Sun, Z. Wang, S. Li, Z. Gu and S. Zheng are with the State Key Laboratory of Automotive Safety and Energy, Tsinghua University, Beijing, 100084, China.All correspondence should be sent to S. Zheng (Email: zsf@tsinghua.edu.cn)develop a Cooperative Vehicle Intersection Control (CVIC) system that enables cooperation between fully automated vehicles and infrastructure.By eliminating the potential overlaps of vehicular trajectories, the CVIC algorithm seeks a safe maneuver for every vehicle.The system performs well in simulation, but there may exist infeasible solutions, for which the authors have designed recovery modes.
Among these optimization methods, mixed integer linear programming is relatively popular for intersection management [7][8] [9].By introducing binary variables as a mathematical expression for different passing sequences, arrangement of sequence can be transformed into a MILP formulation.The real-time requirement may not be satisfied because of computation delay [7].Additionally, velocity of vehicles are not optimized globally.
Since the objective function consists of items representing each vehicle, it may dynamically change because of the entering/leaving of vehicles, probably before the solution is done.Obviously, to guarantee the real-time performance as well as quality of the solution, and adapt to the objective functions dynamic changing feature, decomposition is key to the intersection vehicle coordination problem.One way to decompose is to control vehicles in a distributed manner, which will decrease the communication demand and reduce the size of optimal problem, and no central agent is needed.Researches using virtual platoon [10], model predictive distributed control [11], multi-agent reinforcement learning [12], and the alternating direction method of multipliers (ADMM) [13] have been introduced.These works have preconditions or limits, such as centralized problem construction at certain steps, rule-based relative priority, or the lack of interpretability.
Oriented Graph is usually adopted to describe relative priorities between vehicles [10][14] [15].Specially, Yu Chao et al [14] used Coordination Graph (CG) to decompose the global payoff function into linear combination of local payoff functions.To realize a multiagent coordinated learning effect, they apply the Variable Elimination (VE) algorithm [16].The VE algorithm sequentially eliminates agent nodes till only one remains, and then the last agent selects the action that maximizes the payoff function.Similarly, in this paper we solve the optimal velocity of vehicles after nodes in a graph is eliminated to minimum.
In this paper, we propose a centralized coordination scheme, where the MILP formulation deals with the safety and efficiency simultaneously.Scale of the optimization problem is limited, by introducing a subset extraction method based on graph theory.Thus, the real-time performance of solution is improved, and the scheme can be easily applied into practice since it does not require the group of vehicles to be fixed.
The remnant of this paper is arranged as follows.In Section II, a multi-vehicle passing problem is built with all due assumptions and simplifications.Section III presents the methodology of centralized vehicle coordination.The simulation and results are illustrated in Section IV.Section V gives some concluding remarks.

II. PROBLEM FORMULATION
This paper chooses a multi-lane intersection scenario as a general example shown in Fig. 1, and we suppose there are enough lanes so that vehicles will not merge.Thus the form of conflict between all paths is only crossing.Vehicles turning right can accelerate/deaccelerate freely.Denote number of all the incoming traffic flows to be coordinated as N, and number of all crossing points as P. Another assumption holds that vehicles are already on their target lanes and their trajectories are all predefined, thus only longitudinal advisory speed is considered.Communication condition is ideal without any latency or package loss.Vehicles approach the intersection with different but numerically close initial velocity.Without losing generality, their departure time can be stochastic, as long as keeping reasonable headway.
For simplicity of demonstration, take 4-leg single-lane intersection as an example, where vehicles only go straightly.This example differs only in scale from Fig. 1 essentially, and the latter is the scenario applied in simulation.

A. Concepts and Notations
For the ith vehicle, denote its distance to intersection center O as L i .In following section, an approach is introduced to dynamically coordinate different sets of vehicle, each time vehicles constituting the set are chosen from the N lanes.Denote set of their distances as L 1×N .Define matrix C N×N , if i ∈ N, j ∈ N have their paths crossing, then c i j = 1, otherwise the element is set to be 0. distance between vehicle i and intersection center conflicting matrix, c i j = 1 if movements of i and j cross, otherwise c i j = 0 S N×N priority matrix, optimal velocity vector of the subset initial velocity vector of the subset G 1×N directed graph, with V 1×N stored in vertexes, and all s i j = 0 indicating directed edges At each crossing point, there exists a relative priority between two vehicles.Define S N×N , where , vehicle i has priority over vehicle j −1, vehicle j has priority over vehicle i 0, paths of vehicle i and vehicle j do not cross (1) According to definition in last section we know that there are P positive values and P negative values (i.e. 1 and -1) in S N×N .Table I gives important expressions in this paper and their meanings.For ∀ (i, j) with c i j = 1, build a Cartesian coordinate system as shown in Fig. 2. If one or both of the paths are curves, map their trajectories onto the axes.Since two vehicles share a potential conflict point O on their intended trajectories, to guarantee safety, never should they appear at the point simultaneously.This constraint can be expressed in geometry by defining a joint trajectory (green dotted line in Fig. 2), of which transverse coordinate is x j , and longitudinal coordinate is x i .The joint trajectory should either go under (−L sa f e , L enter ), or beyond (L enter , −L sa f e ).The two points represent vehicle j leaves before/after vehicle i respectively.Here L enter refers to distance from centroid of vehicle to origin O when the vehicle head just touches the overlapped region.Similarly, L sa f e refers to distance limit from centroid of the other vehicle to origin O at the same time.Both values can be adjusted to change safety margin.Notice that the origin O is biased with regard to intersection center O, we define L i and L j , based on coordination transformation.Obviously the feasible region has deterministic linear boundary if joint trajectory is a straight line.Therefore, we suppose that Assumption 1: Both vehicles drive at constant speed from current position to the intersection area, without acceleration or deceleration.

B. 2-vehicle collision-free constraint
Given initial locations L i and L j in local coordinate system, we have Thus following equation is obtained: The basic 2-vehicle collision constraint can be converted into inequality expressions: From ( 4), it is apparent that the feasible region of joint trajectory line is determined by three points on the coordinate plane, i.e. (−L sa f e , L enter ), (L enter , −L sa f e ), and start point (L j , L i ), which is in accordance with Fig. 2. Combining (3) and (4), we get Expression ( 5) cannot be used in an optimal problem directly, except that the inequalities have a logical relation of AND, ∧.Hence, the big-M method is applied here [8] [17].Introduce a binary variable b i j and a relatively big constant value M, and (5) now equals So far, under Assumption 1 the 2-vehicle collision avoidance is formulated as inequality constraints, with variables v i , v j and b i j to be optimized.

C. Basic MILP Problem
From each approaching direction pick one vehicle and build an linear programming problem: max The feasible region is further constrained by (6).Note that b i j is either 1 or 0, making it a MILP problem.If there is no vehicle in incoming lane i currently, L i can be set infinity.
Then v opt,i will definitely be v max , and other variables are totally not affected.
Remark 1: By now, based on Assumption 1, given current position of certain vehicles (i.e. one from each approaching lane), solution to the MILP problem is optimal passing velocity {v opt,i } and their priorities {b i j }.Here {v opt,i } is denoted as V 1×N , and b i j determines S N×N .

D. Intersection coordination decomposed into sub-problems
We propose a coordination scheme consisting of 3 stages, which are introduced below.
1) Target Velocity Optimization: Given vehicles to be coordinated at current stage, the MILP problem is solved with their positions as input.Consider following question: where Input variables are L 1×N and C N×N .L 1×N determines values in (9)

after simple coordination transformation (O to O ). And indexes of non-zero values in
To make the algorithm concise, it does not involve vehicle size explicitly.Actually, vehicle lengths and widths are used as additional input parameters of the function, and they finally influence the result by changing L sa f e and L enter in ( 6) and (9).
Output of the MILP optimization is V 1×N and the P values of b i j .
2) Vehicle Dynamic Selection: The basic idea is to choose one vehicle from each lane of N to form a subset.In Fig.Here we make use of the directed graph G 1×N to find excluded nodes.Calculate the optimal velocity and sequence of N vehicles by constructing a basic MILP problem described in 1).The optimal velocity is then denoted as V 1×N , and the relative passing priority which comes from b i j in (11) is stored in S N×N .With N vehicles nodes and their relative priorities it is easy to build a directed graph G 1×N .Vehicle node k is excluded if and only if one of the items below is satisfied: 1) optimal velocity assigned to the kth node is v max , indegree of node k is not zero, and node k is not an element in a loop 2) there exist two nodes i and j, both assigned v max and having an indegree of zero, while node k is their common child node 3) node k is child node of any node satisfying 1) or 2) In the directed graph, the edge pointing from jth node to ith node means vehicle j is prior to vehicle i, and vice versa.Let's call a vehicle assigned v max free node, and others constrained node.The reason behind above three items is intuitive.
In a directed acyclic graph (DAG), optimized velocity equalling upper limit implicitly means the ego vehicle is either the first to pass the intersection, or following some other vehicles much closer than it.Otherwise it should drive slower than speed limit.The latter case, reflected in the graph, will be at least one directed edge pointing to the node, namely its indegree is ≥ 1, while the former case keeps its indegree zero.In this sense, a free node with indegree ≥ 1 should be expelled because it can drag down the overall speed.As is shown in Fig. 3(a), node W2 has to follow node S1, even it is closer to stop line, which is not allowed.The only exception of this rule rises when the graph is not DAG and the free node is on a circuit.In this case, the none-zero indegree value does not stand for a much higher distance to the intersection, so the node should be reserved in the subset.So far, the rule 1) is concluded.
For constrained nodes, the constrained optimal velocity reflects that they need to drive slower to avoid crash.These nodes should be reserved, because they are closely coupled with predecessors instead of following them with a long headway.But there still can be an exception: suppose there are two free nodes in parallel phases, indegree of both are zeros, thus both not excluded according to rule 1).In Fig. 3(b), they are N1 and S1.However, remaining distances of the two free nodes can be distinct, making the child node of the farther one arriving even later, like E1 following S1 in Fig. 3(b).In this case, N2 would have to wait until E1 leaves, which is not reasonable enough.Therefore, E1 should be excluded and optimized again in next round.
Rule 3) is easy to understand: once a node is excluded to enhance overall efficiency, nodes forming its spanning tree should be expelled for the same reason.
Remark 2: Excluding nodes avoids unnecessary deacceleration, especially when traffic flow density is unbalanced in different phases.By this means, our decomposition of intersection coordination problem is essentially adaptive.
The algorithm is as following: The spanningtree(), neighbor(), predecessors() are graph theory involving functions, which return a set of nodes.indegree() is also a function which returns number of edges pointing to its input node.
The result of Algorithm 1 is FLAG 1×N , and FLAG(i) = 0 means the ith node should be excluded.We denote the size of the extracted subset as N .
3) Synchronous Velocity Profile Planning: Actually, Assumption 1 is a relaxation to practical constraint, because velocity must be continuous with regard to time due to vehicle dynamics.Therefore, we must ensure that, the process of vehicles accelerating/deaccelerating, is equivalent to a period they drive at their respective optimal velocity (Fig. 4).By this means, Assumption 1 is violated only in transient process (t 0 ∼ t acc,i in Fig. 4), optimal solution of N vehicles' MILP problem at t 0 is consistent with new solution after max(t acc,i ).t acc,i means the time when the ith vehicle's velocity converges to its target.The conditions to be met are concluded as follows.
Synchronous Condition: By all vehicles' velocity convergence, distances they have travelled since MILP solution should be proportional to their optimal velocity.
Transfer Condition: Vehicles should never collide with members in last coordination subset.
To realize the 2 conditions, a velocity profile planning algorithm is proposed.Here the input contains the subset index FLAG, as well as the reduced optimal velocity, initial velocity et al.A max is the maximum acceleration of the vehicles, and K is a constant greater than 1.Expressions like S OA i C i D i refer to area illustrated in Fig. 4. The t lastleave,1×N comes from last run of this algorithm, namely the output t leave,1×N generated last time.The Synchronous Condition is guaranteed by a distanceequivalent method, which can also be realized by optimal control.As for the Transfer Condition, latest leaving time of current subset is transferred to the next subset as a constraint of acceleration planning.If vehicles are entering the intersection too early, their velocity profile is rescheduled.
To be emphasized, the pseudo code does not embody coordinate transformation, neither the size of vehicle body, which should be considered during implementation.By 3 steps above, a minimum sub-problem is defined and solved.Each sub-problem will deal with collision-free  constraint among a subset of vehicles at time T clock , as well as safe time interval between subsets.The whole process is chain-like, with computation divided in time domain.

IV. SIMULATION AND RESULTS
Simulation of 32 vehicles, four in each movement was carried out.Depart time at a distance of 200m is shown in table II.Here ES stands for from east, into south, and others are similarly defined.
The simulation was carried out on a computer with an intel i7 CPU, programmed and solved with matlab 2017b.

A. Efficiency
Velocity trajectories are shown in Fig. 5, where each curve converges to a vicinity of maximum limit, which is set to be 20 m/s here.
During the simulation the scheme worked for 16 times, and the subset size is 1, 2, 1, 1, 2, 1, 1, 1, 2, 6, 4, 3, 1, 2, 2, 2 respectively.From the analysis we know, the scheme resembles the FCFS principle when our subset consists of only 1 vehicle.Thus it was 6 that best reveals the essential difference.The directed graph of the topology is given by Fig. 6.According to node-exclusion rules, only node SW and node ES are excluded, and the left 6 vehicles can accelerate to their optimal velocities and drive through.It is worth mentioning that the four vehicles EN, SN, WE, NS forms a cycle in the topology (Fig. 6), thus they passes the intersection end to end, as if there exists an invisible roundabout in the intersection at that time.The time by all the vehicles leave intersection is 31.9s.We have not compared data strictly with other methods yet, but the above coordinating behavior reveals its potential in improving intersection capacity.

B. Real-time Performance
The mixed integer linear programming accounts for a major part of the computation.The time consumed for the MILP operation during simulation is 0.065, 0.060, 0.057, 0.059, 0.043, 0.041, 0.044, 0.041, 0.046, 0.047, 0.049, 0.053, 0.043, 0.052, 0.045, 0.048 s respectively.The mean value is 0.0495 s and the squared difference is 5.5×10 −5 s 2 .Since the simulation step is chosen to be 0.1 s, the proposed scheme is real-time and feasible.

V. CONCLUSION AND DISCUSSION
In this paper, we propose a centralized scheme serving multi-lane intersections.The scheme consists of three steps-target velocity optimization, vehicle dynamic selection and synchronous velocity profile planning.On the premise of safety, the traffic efficiency is improved by optimization of velocity and sequence at the same time.More importantly, real-time solution is satisfied with guaranteed optimality, as the MILP has limited number of variables.
These aspects should be addressed in future research: design control experiments to test the statistical performance of this scheme, especially compared to other methods; humandriven vehicles should be modelled and considered in the scheme; how to distribute the computation and communication to get rid of the dependence on a central agent.

Fig. 2 .
Fig. 2. Collision-free joint velocity model 3 vehicles are represented by nodes.Grey nodes mean the vehicle is either not coordinated yet or already coordinated.The colored nodes represent vehicles chosen into current coordination subset, with different traffic phases expressed by red or blue.No matter how many vehicles are driving into the intersection, each running time the MILP problem only need to deal with N nodes, namely {E1,N1,W1,S1} in Fig. 3.

TABLE I NOTATIONS
AND MEANINGSSymbolDescription N number of incoming lanes coordinated N number of elements in the extracted subset P number of crossing points between N paths L i ,N end for update t arrive according to FLAG while exist (i, j) s.t.t arrive (i) ≤ t lastleave ( j), c i, j = 1 do t acc ⇐ t acc × K update t arrive according to FLAG, t acc end while update t leave , according to FLAG, t arrive

TABLE II DEPART
TIME FROM 8 APPROACHING DIRECTIONS