Multicriteria Autonomous Vehicle Control at Non-Signalized Intersections

: The aim of the paper is to describe a multicriteria model predictive control method for autonomous vehicles at non-signalized intersections. The centralized controller aims to describe control action for each autonomous vehicle to guarantee collision free passage. At the same time, performances are deﬁned for the centralized Model Predictive Controller, namely the minimization of traveling time and energy consumption. Since these control goals are often conﬂicting, a scheduling variable is introduced to create a balance between them. Hence, the centralized controller can be tuned based on the importance of each control goal, which can be useful in urban environment where trafﬁc densities may vary heavily depending on the period of the day. The effectiveness of the proposed centralized multicriteria controller is demonstrated through a complex simulation example in CarSim simulation environment using different tpye of autonomous vehicles. and accelerations of autonomous vehicles.


Introduction
Significant part of the scientific research focuses on the challenges of control design of vehicles at non-signalized intersections. The basis of research is to determine the control design challenges related to the coordination of autonomous vehicles at intersection scenarios. In the US, there were more than 32 thousands fatal crashes on the roads in 2017 involving approximately 53 thousands vehicles and about 30 percentages of the human-driven vehicles participated in intersection accidents, see [1]. To decrease the number of fatal crashes related to human errors, it is attempted to analyze the use of autonomous vehicles in traffic systems. Traditionally the exact ordering at an intersection with traffic lights and signs is given by traffic rules followed by the driver of the vehicles. By the development of the automation in traffic systems, the coordination of autonomous vehicles at non-signalized intersection scenarios have become significant challenge of the control design.
A recent survey considering non-signalized intersection strategies is made by [2]. Multi-agent systems are developed for the control of non-signalized intersections and approached in several papers, see [3,4]. The proposed optimization algorithms are used to secure collision-free passage of vehicles through the intersection. The optimization problem for the control of autonomous vehicles crossing an intersection is reformulated as a convex program and solved by [5], while an optimal scheduling is proposed for the ordering problem as a Mixed-Integer Linear Program by [6].
For the determination of the crossing order of vehicles through the intersection Model Predictive Control (MPC) is used frequently to get an optimal solution by both centralized or decentralized controllers. An algorithm taking the expected entering time of vehicles into consideration is proposed to secure safe crossing of the intersection, see [7]. A decentralized MPC is used for the determination of time, while the negotiation between vehicles and the detection of the safety critical situations are guaranteed by vehicle-to-infrastructure (V2I) communication units. Two different decentralized MPC methods are presented to consider efficient fuel consumption, the requirement of comfort and to handle unexpected vehicle maneuvers during the passage through the intersection, see [8]. A MPC method is used for the assignment of priorities of automated vehicles to guarantee the optimal scheduling at the intersection by solving its control problem as a Mixed Integer Programming problem [9]. A bi-level, model predictive controller with coordination is presented by [10] to ensure the collision avoidance between automated vehicles at intersections. The optimization problem is solved by the use of a distributed Sequential Quadratic Programming method. In the control strategy optimal traveling speed is designed for each vehicle to secure safety constraints. An intersection crossing strategy by using a centralized controller is proposed by [11] formulating the control problem as a convex quadratic program, while a decentralized solution solving the local optimization problems of the sequence of the vehicles is developed by [12]. A centralized Optimal Control Problem (OPC) has been solved in a distributed fashion in [13] using a decomposition technique to create local OCPs with coupled constraints.
The contribution of the paper is a MPC-based multicriteria control method for autonomous vehicles considering both time duration of the intersection crossing and the total energy consumption of all vehicles. For safety and comfort reasons, constraints as speed limits, predefined minimum and maximum accelerations are built in the control strategy. As the purpose of the research is to determine the ordering of vehicles, the requirements of minimum traveling time and efficient energy consumption are incorporated in the control design, see also [10,14].
There are several benefits of the presented method compared to other strategies in the literature. Data-driven methods relying on machine learning techniques need to generate a great deal of possible scenarios defined by the autonomous vehicles position, speed, acceleration and turning intention. Moreover, little changes in the vehicles parameter space can result in very different solutions regarding the ordering and accelerations of the vehicle, which may lead to difficulties in the presence of sensor noises and uncertainties. Conventional MPC controls are superior in this sense, however, real-time implementation might be difficult due to the necessary computational power needed by the centralized controller. Presented method relies on a simplified optimization strategy, which is easy to implement and sufficiently fast for real-time applications.
The paper is organized as follows. The motivation of the research and the challenges in the control of autonomous vehicles at intersections are defined in Section 2. Section 3 shows the intersection scenario, describes the process of the MPC control design along with the constraints and performances defined. The proposed MPC intersection controller is validated in several simulation examples in Section 4. Finally, conclusions are drawn in Section 5.

Motivation
With the development of intelligent transport systems and the expansion of the applications of highly automated vehicles, transport scenarios are emerging that require the development of special methods in terms of control planning. The introduction of autonomous vehicles in traffic calls for a review of a number of traffic rules which, in addition to established habits and the ethical behavior of drivers, currently provide a solution in most traffic situations. However, for highly automated vehicles without a driver, these sets of rules are not applicable in all traffic scenarios. The control of autonomous vehicles at intersections is a significant challenge from the point of view of control planning. Currently, traffic lights, traffic signs and rules known to drivers ensure that vehicles pass through intersections without accident. When designing an intersection, engineers make a decision appropriate to the size of the traffic flow to use one of the above methods to reduce the number and magnitude of congestion or accident-prone situations. With the proliferation of autonomous vehicles, the spread of vehicle-to-vehicle (V2V) and vehicle-to-infrastructure (V2I) communication technologies and sensor fusion procedures, it is possible to trigger traffic rule-based management of intersections with intelligent intersection control. As an expected result of these research and development, in addition to increasing traffic safety, the development of congestion and the energy consumption of individual vehicles can also be reduced. In order to increase safety and comfort of the passengers, various conditions/limits (such as speed limits, acceleration limits) need to be incorporated into the control during design. A key task in planning the crossing is to determine the purpose of the intersection management. The primary goal is the safe passage of vehicles, which can be accompanied by performances such as minimizing transit time to avoid congestion and energy-optimal control to reduce fuel consumption and emissions. A balance between these often contradictory performances can be reached with multicriteria control procedures. The first step in planning the control of an automated intersection is to determine the passing order of the autonomous vehicles. Next, the control intervention for the implementation of the given order must be designed.

Problem Statement
The coordination of autonomous vehicles at an intersection can be more complex by A non-signalized intersection scenario with several autonomous vehicles is proved to signify more complex situation in designing control for the crossing of the vehicles, see Figure 1. The safety condition of collision avoidance, the achievement of minimizing traveling time and the consideration of uncertainties of the measured data (e.g., distance of the vehicles from the origin of the intersection) affect to the design of multiple steps of control strategy. The proposed method considers most of the above-mentioned conditions increasing the complexity of the control of the intersection. The minimization of traveling time and energy of the vehicles passing through the intersection are in the focus of the coordination method.
Related to this field of control, several research studies maneuver and motion prediction of autonomous vehicles, see [15][16][17]. In connection with the driving intention of autonomous vehicles, detection of vehicle maneuvers using alert signals (e.g., brake lights, turn signals) are also of great importance, see [18,19]. A particular area of interest is the communication methods between the participants of the traffic and the infrastructures [20,21].

Intersection Scenario
In the proposed intersection scenario vehicles can drive straight, turn left or right, therefore there is a probability of collision between vehicles tracking the same or different trajectories [22]. All features (e.g., velocity, driving intentions, position at the intersection) of autonomous vehicles are collected by a centralized controller transmitting measured data and control input for all participants of the traffic situation. The goal of the paper is to design an algorithm which guarantees collision-free passage of autonomous vehicles at intersections, while it minimizes the total traveling time or the energy consumed by the vehicles. Therefore, congestion at the intersection can be avoided and fuel consumption can be reduced while safety of the autonomous vehicles can be enhanced. In view of the intersection scenario, the goal of the centralized controller is to determine the ordering of autonomous vehicles as well as their accelerations with the consideration of total traveling time or energy consumption, and to guarantee collision-avoidance at the same time.
The proposed Model Predictive Control (MPC) algorithm is based on some pre-assumptions. A four-directional type intersection shown in Figure 1 is examined, which is separated into different sections. Before reaching the control zone, each autonomous vehicle is self-controlled, supervised by its own cruise control system. When a vehicle enters the control zone, a velocity profile and a corresponding acceleration is calculated for it by the centralized intersection controller founded on the actual velocity and planned trajectory of the autonomous vehicle. The latter data along with the calculated accelerations are sent from the vehicle to the intersection manager and vice versa by V2I communication methods. Although the iterative calculation detailed later takes into account up to four vehicles at a time, the calculation is repeated with reconfigured initial conditions in case a new vehicle enters the intersection. Note, that in case the new vehicle enters while the preceding vehicle is still ahead, it engages adaptive cruise control with a predefined spacing until the preceding vehicle leaves the intersection conflict zone. In this manner, the proposed algorithm can be enhanced to handle intersections having bigger traffic density. Moreover, the proposed algorithm can be augmented to consider more complex intersections as well.

Constraints for the Control Design
In the case of autonomous vehicles, speed limits must also be specified for safety reasons while crossing the intersection. The safe speed of autonomous vehicles is given by simplified vehicle dynamics: v l = R l gµ for left turn, v r = R r gµ for right turn and towards the straight line v s = v lim is calculated, where R l and R r are the radius of left and right turns at the intersection, g is the gravitational constant, µ is the coefficient of adhesion between the tire and the road, which is assumed to be described by estimation methods, see e.g., [23][24][25], v lim is the speed limit. For a typical intersection with four directions and a lane width of 5 m, taking into account a tire-road adhesion factor of µ = 0.8, the safe right-turn speed is v r ≈ 16 km/h, the safe left-turn speed is v l ≈ 28 km/h. Thus, as vehicles turning left follow a trajectory defined with a much bigger cornering radius then right turning vehicles taking a sharp turn, their maximally allowed velocities in the intersection is almost the double of right turning vehicles. Note, that vehicles heading straight do not need to meet any dynamic constraint, hence the speed limit is posed as maximally allowed velocity for them. Maximum and minimum accelerations are also set for the autonomous vehicles to ensure passenger comfort, maintain traction and prevent wheel slip. Here, the thresholds a max = 5 m/s 2 and a min = −5 m/s 2 are chosen based on [26].
For collision-avoidance, some other constraints must also be built in the control design. The base for the collision-free passage of the autonomous vehicles at the intersection is to set the condition that only one vehicle can be located in the conflict zone of the intersection at the same time. Consequently, the oncoming vehicle (independently of its predefined trajectory) can enter the conflict zone of the intersection only at the exit time of the previous vehicle.

Time-Optimal Intersection Control Design
One of the design goal is to minimize the crossing time of autonomous vehicles T total , therefore reduce the risk of a congestion emerging. This goal is achieved by ordering and accelerating vehicles to cross the intersection at the highest possible speed.
Hence, the control algorithm defines constant acceleration a i i ∈ [1...n] for the vehicles entering the intersection, by which the maximum speeds v i,max on the given track available can be reached. First, the following computation is processed for vehicles entering the intersection: where v i,0 is the initial speed, s i,ent is the initial distance of all vehicles from the intersection origin.
Corresponding to the specified conditions for acceleration, a i = {a max ; a min } replaces the calculated accelerations when the thresholds are reached. In this case, the maximum entry speed in the conflict zone shall be changed as follows: The essence of the coordination is to compare the travel time of autonomous vehicles at the intersection to prevent collisions during their crossing. In the process of minimizing travel time, conflict situations between autonomous vehicles are analyzed to control accelerations. Hence, altering the acceleration followed by the vehicles before entering the conflict zone of the intersection is integrated into the proposed algorithm. In case vehicles are in a conflict situation, the priority for passing through the intersection should be given to the vehicle with the least travel time. The rest of the vehicles reduce their acceleration to give priority for the first vehicle, then give priority to the vehicle leaving second in the same way. In the event of multiple vehicle conflicts, the algorithm takes into account the stopping of vehicles at the end of the entry zone and determines the waiting time for entering the conflict zone with a predetermined acceleration.
For the proposed method, the entry and exit times of each vehicle in the conflict zone must be calculated. At the intersection, the entry time of vehicles can be determined by solving the following second-order equation: where t i,ent ≥ 0 i ∈ [1...n] is the entrance time. Assuming constant accelerations in the entering zone, (3) is simplified as: The time spent by the vehicles at the intersection shall be determined by assuming that the vehicle is traveling at a constant velocity when it gets to the conflict zone. Selecting a i = 0 in the conflict zone the time spent is given as follows: where s i,int is the trajectory length corresponding with the turning intention.
If the autonomous vehicle has to stop to give priority to other vehicle (or vehicles), a waiting time t i,wait is also computed. In this case, the value v i,max = 0 is set in order for the vehicle to stop in front of the conflict zone. The waiting time is defined as the difference between the final time of the previous vehicle and the entry time of the subject vehicle. After the conflict zone is free, i.e., t > t i,ent + t i,wait , the autonomous vehicle enters the conflict zone with the predetermined acceleration a max , hence the travel time in the intersection is given as follows: Finally, the travel time of each autonomous vehicles is the aggregate of the entry and exit times: Founded on the on-board measurements sent by the autonomous vehicles to the intersection coordinator, the computation is evaluated as follows iteratively: • First, the maximum vehicle speed v i,max i ∈ [1...n] is defined for each vehicle, calculated based on the planned vehicle trajectory. Given the initial speed and the distance measured from the origin of the intersection, a constant acceleration of a i i ∈ [1...n] is determined based on (1). In addition, if the maximum speed cannot be reached with predefined acceleration thresholds, the maximum speed can be modified by using (2).

•
The time of entry into the conflict zone t i,ent and the time of exit t i, f in ∈ [1...n] is defined for all vehicles using (4) and (7). Potential conflicts are then determined by analyzing the time overlaps of each vehicle in the conflict zone. If no collision danger is detected, the autonomous vehicles use the previously calculated accelerations to cross the intersection.

•
In the event of a conflict between two or more vehicles, the algorithm is evaluated as follows: vehicle with the minimum exit time t i, f in i ∈ [1...n] is given priority and does not change its formerly calculated acceleration. The algorithm follows by iteratively reducing the acceleration of other vehicles and recalculating their corresponding entry time t i,ent using (4) until the entry time exceeds the exit time of the previous vehicle. This iterative calculation process is followed for every conflicting vehicle. The derived accelerations a i i ∈ [1...n] are then used in the entry zone. In case the waiting time t i,wait is greater than zero, the subject vehicle waits for the calculated time at the beginning of the conflict zone and then applies a i = a max . • Lastly, when a new vehicle enters the intersection, the algorithm follows the vehicle tracking mode until the previous vehicle leaves the intersection conflict zone. If this occurs, the above procedure shall be repeated with the new starting conditions for each vehicle in the intersection control zone.

Energy-Optimal Intersection Control Design
Another priority of the intersection management may be to minimize the total energy consumption of the autonomous vehicles. Here, the conditions listed in Section 3.2 along with collision avoidance and dynamic constraints must also be considered. However, when ordering vehicles and defining accelerations, energy optimality is a fundamental design element. Hence, the main difference from the time-optimizing algorithm is that the vehicles at the intersection are sorted not by travel time but by their kinetic energy . This calculation assumes that each autonomous vehicle, in addition to its initial position and speed, sends information to the coordinator about its mass m i i ∈ [1...n].
The iterative computation of the vehicle ordering by the intersection coordinator are as follows: • The maximum speed v i,max i ∈ [1...n] is calculated and an additional limit on the maximum speed is defined as v i,max ≤ v i,0 . Here, the difference compared to the time-optimized algorithm is that only the negative acceleration a i can be assigned for vehicles.  (4) and (7). If no conflict is detected, vehicles adapt the previously calculated accelerations to cross the intersection.

•
In the event of a conflict between two or more vehicles, the energy optimization algorithm is evaluated as follows: vehicle with the maximum kinetic energy max(E i,kin ) i ∈ [1...n] gets priority and does not change its acceleration. The algorithm then reduces the acceleration of the other vehicles in an iterative manner similarly as listed in the time-optimized solution, with the difference that the priority is always chosen based on kinetic energy.

Multicriteria Intersection Control Design
For the multicriteria control, the energy-optimal solution is tuned by selecting the maximal velocity for the autonomous vehicles in a different manner. The goal of the design, is to scale the velocity by which the autonomous vehicles arrive at the intersection. Hence, the result of the control algorithm will consider energy optimality in the ordering of the vehicles, but time optimality will also be considered due to the larger velocities the vehicles are allowed to enter the intersection conflict zone.
Remark: First, the maximal velocities v i,max are calculated, serving as the constraints for the vehicles. Next, a scheduling parameter ρ i i ∈ [0...1] is introduced, which scales the gap between the initial velocities v i,0 and maximal velocities v i,max given by the speed limits as follows: v . Thus, selecting ρ = 1 stands for a solution where traveling time is also considered greatly, while choosing ρ = 0 gives back the energy optimal solution, and between a mixed performance is given. The rest of the multicriteria algorithm is similar to that detailed in Section 3.4, with the difference of substituting v ρ i,max in place of v i,max .

Operation of the MPC Controller
The operation of the intersection MPC control is shown in Figure 2. The coordinator of the intersection collects the position and velocity data s i,ent (k), v i,ent (k) i ∈ [1...n] of each autonomous vehicle entering the intersection at a discrete time step k using a sampling time T s , along with their turning intention d i . For the newly entered vehicle, a decision is made based on the position of the preceding vehicle, whether to add the vehicle to the MPC optimization process, or switch to adaptive cruise control mode. Note, that for the MPC optimization method the control performance can be set by the scheduling variable ρ. At each discrete time step k, based on the predefined ρ value the solution of the time-optimal, energy-optimal or multicriteria vehicle ordering is calculated based on the analytical method detailed in Sections 3.3-3.5 for a time horizon T = max(t i, f in ) i ∈ [1...n]. Note, that the time horizon depends on the vehicle states and the intersection geometry, and the optimization problem is solved in a single control problem for all vehicles. Moreover, newly entering vehicles only join the MPC optimization process when the preceding vehicle leaves the intersection, thus the optimization proceeds with maximum four vehicles at the same time. The solution of the optimization gives the control variables a i (k + 1) i ∈ [1...n], which the autonomous vehicles in the intersection follow until the subsequent time step, where the optimization is repeated with the forward shifted horizon. Since the vehicle ordering and prescribed accelerations are recalculated at every time step for vehicles in the entering zone, a sampling time T s = 0.1 s is selected to avoid chattering of the control signals. Note that present MPC method is inherently robust against bounded initial errors in the measurement signals, since as the accuracy of the signals improve as the vehicles get closer to the intersection origin, the online recalculation of the MPC controller adjusts the vehicle order and control inputs to the punctual data.

Autonomous Vehicle Entering Intersection
Preceding Vehicle Exited Intersection?

No
Adaptive Cruise Control Mode

Model Predictive
Control Yes

Autonomous Vehicles
Inside Intersection

Vehicle Control Model
The acceleration of the autonomous vehicles a i i ∈ [1...n] calculated by the MPC controller is realized by applying a longitudinal force through the drive system. For this purpose, the following equation is implemented to derive the corresponding forces for the autonomous vehicles: where m i i ∈ [1...n] is the mass, F i,l is the control input, F i,d is the disturbance acting on the vehicle [27] including aerodynamic drag, rolling resistance and disturbance from the road slope.
The autonomous vehicles lateral trajectory tracking in the intersection is founded on the two-wheeled bicycle model, see [27,28]. The differential equations of the yaw motion is given as: where v is the velocity, ψ is the yaw angle, X and Y are the coordinates of the vehicle in a global coordinate system. Here, a simplified steering model is used with δ representing the steer angle, while L is the wheelbase of the vehicle and R is the radius of the road curvature. Next, by using the relationψ = v R , the state-space equations of the vehicle is given as: Assuming small velocities and changes in the reference signals (ẏ re f ∼ = 0;ψ re f ∼ = 0), Equation (10) can be organized as follows:ẋ where x = e Y e ψ T includes the error signals of the lateral position and yaw angle, u = δ is the steering input. The performance of the steering control is to minimize the error signals given in the state vector x. Hence, a Linear Quadratic (LQ) controller is applied using the cost function: where Q and r are design parameters. Solving the Ricatti equation the control gain K is derived, generating the steering input δ for the autonomous vehicles based on the measured error signals.

Verification of the MPC Method
In order to analyze the effectiveness and time-optimality of the proposed MPC method, it has been compared to the result of a simulation-based optimization algorithm in CarSim environment. Note, that latter optimization technique can only be used for offline analysis and validation due to its big computational time.
For the sake of comparison, a four-directional double lane intersection has been chosen with four vehicles having initial conditions and turning intentions shown in Figure 3. As illustrated, Vehicle 1 and Vehicle are heading straight, while Vehicle 2 and Vehicle 4 are turning left with a 28 km/h speed constraint for entering the intersection.  The simulation-based optimization algorithm is searching for the optimal acceleration values for each vehicles, by which they can safely pass the intersection without the danger of collision or skidding, while the total traveling time is minimized. The optimization algorithm running iteratively in MATLAB environment works as follows: The constrained optimization algorithm runs the CarSim simulation with initial conditions and turning intentions depicted in Figure 3. The scheme of the CarSim multi-vehicle simulation is shown in Figure 4. Note, that the acceleration values for which the optimization algorithm is searching for are noted with a i ∈ [1...4], and are used as inputs for the simulated vehicles. The minimization function for the optimization algorithm is the total traveling time, which is measured by the CarSim simulation. The simulation ends when the last vehicle leaves the intersection conflict zone.

•
To guarantee collision free passing of the vehicles, the positions of each vehicles are measured, and a inter-vehicle distant constraint is built in the optimization procedure for avoiding them to get closer to each other more than 3 m. In this case, a large number is added to the traveling time measured in CarSim, thus the optimization algorithm discards the corresponding acceleration values.

•
The constrained optimization runs the CarSim simulation iteratively until it founds acceleration values for the autonomous vehicles by which a local minimum for the traveling time is reached. Next, the proposed time-optimal MPC intersection control has been evaluated with the same initial conditions depicted in Figure 3. The scheme of the real-time MPC control implemented in CarSim environment is depicted in Figure 5. In the proposed time-optimal intersection control, Vehicle 3 crosses the intersection first, followed by Vehicle 1 and Vehicle 2, while Vehicle 4 finally leaves the intersection at 10.45 s.
With the solution of the simulation-based optimization algorithm, the ordering of the vehicles are the same to that suggested by the proposed MPC controller. Here, the total traveling time of the vehicles are slightly smaller, with the last vehicle leaving the intersection at 10.31 s.
Although the ordering of the vehicles are the same and the total traveling time is similar, the velocity profiles are different for the proposed MPC and the simulation-based optimization method, as depicted in Figure 7a,b. Note, that while the velocity of Vehicle 4 is similar for both cases, the other vehicles choose different acceleration values due to the nature of the underlying algorithm. With the solution of the simulation-based optimization, Vehicle 1, Vehicle 2 and Vehicle 3 selects very small acceleration values compared to the proposed MPC method. The latter method has the benefit mitigating the chance of a congestion to be formed in the intersection, as all of the entering vehicle tends to leave the intersection as soon as possible without holding the oncoming vehicles back.  Hence, the comparison of the proposed MPC time-optimal method with the solution of the simulation-based constrained optimization showed that the total traveling time with the proposed method is very close to a global optimum given by the simulation-based optimization. Although in some cases, choosing different initial conditions a suboptimal solution might be given by the proposed MPC method, the online applicability due to small computational time is a very desirable feature.

Complex Simulation Example
The proposed method is also validated with a more complex example in CarSim environment. Here, four different type of autonomous vehicles arrive at the intersection, while a fifth vehicle also approaches the intersection entering zone and joins the proposed optimization as the preceding vehicle leaves the intersection. The initial positions and velocities of the autonomous vehicles along with their masses are shown in Figure 8. As illustrated, the intention of Vehicle 1 and Vehicle 3 is to continue heading straight, while Vehicle 2, Vehicle 4 and Vehicle 5 are intending to turn left.  Simulations with the above detailed initial conditions were run with differently calibrated centralized controllers: first optimizing traveling time, secondly energy consumption.
The velocity and acceleration profiles of the vehicles running with the two different performance criteria are shown in Figure 9. In the case of the time optimal control law, Vehicle 4 crosses the intersection first, followed by Vehicle 3, Vehicle 1 and Vehicle 5, while Vehicle 2 finally leaves the intersection at 10.2 s. As shown in Figure 9a the velocities of each vehicles increase at some point and Vehicle 1 and Vehicle 3 uses the maximal possible acceleration for a short period of time, see Figure 9b. Note, that Vehicle 4 having a big initial velocity decelerates heavily before entering the intersection conflict zone, and as all other vehicles, keeps an even velocity after entering.
On the other hand, with the energy optimal intersection control the vehicle ordering is changed significantly. Similarly to the time-optimal case, Vehicle 4 having big initial velocity and mass as well, leaves the intersection first. Next, Vehicle 1 also having bigger mass enters the intersection, followed by Vehicle 3 and Vehicle 2, while Vehicle 5 leaves the intersection finally at 12 s. Thus, in case of the energy optimal intersection control, the total traveling time of the vehicles increase by almost 2 s. Note, that in order to save energy, all vehicles only decelerate as shown in Figure 9d, decreasing their velocity before entering the conflict zone, see Figure 9c.
In Figure 10, the control energies (both driving and braking) of each vehicles are depicted along with the total control energy of all vehicles. It is well demonstrated, that the time-optimal solution shown in Figure 10a requires more than 2500 kJ of total control energy in contrast with the approximately 1700 kJ required by the energy-optimal centralized controller. Thus, for the given scenario, more than 800 kJ of total control energy can be saved in the intersection for the price of approximately 2 s in total traveling time.
Note, that above simulation scenarios where repeated with several different initial conditions. The proposed algorithm proved to be reliable, computationally efficient and fast enough for the simulation environment, hence it can be tested in real-time implementations.

Conclusions
The paper proposed a tunable intersection control algorithm for autonomous vehicles, with the aim to minimize both traveling time and energy consumption while guaranteeing collision free passage of the vehicles. For this purpose, a MPC has been designed with tunable performances. The proposed algorithm has been validated in CarSim environment by comparing its solution with a simulation-based optimization. Also, a complex example has been tested with a newly entering vehicle joining the optimization process. The results demonstrated the effectiveness of the proposed centralized controller.