Real-time Trajectory Planning for Managing Demand and Safety during the Crossing of an Intersection by Connected Autonomous Vehicles

This work investigates the problem of autonomous intersection crossings facilitated by CAVs by presenting an optimization framework that also considers the vehicle's system state uncertainties and intersection demand management. Assuming linear-Gaussian systems, the presented approach, i.e., AVOID-OM, tackles the problem in two phases. First it decides the best arrival moment in time for a vehicle to enter the area surrounding the intersection, i.e., the danger zone. Then, it selects a set of controls for the vehicle that allows the safe traversal of the intersection, even in presence of vehicle location uncertainty. The overall objective is the maximization of the capacity of the intersection and the presented approach improves previous state-of-the-art solutions both in terms of average number of admitted vehicles and of the vehicles' average speed, while also ensuring smoothness in the vehicles' acceleration profiles.


I. INTRODUCTION
Intersections are among the most challenging parts of the road infrastructure and a clear bottleneck for traffic flows [1]. For this reason, recent research efforts have focused on analyzing the potential use of Connected Autonomous Vehicles (CAVs) for a safer and more efficient intersection management.
Several techniques have been exploited for coordinating CAVs at intersections that mainly focused on: (i) reservation schemes to grant priorities for intersection crossing [2]; (ii) optimizations based on Model Predictive Control (MPC) to minimize a given metric, e.g., intersection travel time [3]; and (iii) fuzzy controllers that set the vehicles controls (i.e., acceleration and braking) [4]. Interestingly, even though an extensive body of work has investigated the CAV coordination problem at intersections, vehicle location uncertainties due to, e.g., imprecise measurements or prediction models, are rarely accounted for. Among the available studies, [5] evaluates the probability of a vehicle to reach a goal location outside the region of the intersection under model uncertainties and unreliable wireless communications. Further, [6] and [7] present path planning algorithms that, under vehicle location uncertainty, guarantee respecting predetermined movement constraints with a target probability. While all of these aspects are interesting, if location uncertainty is accounted for, only our previous work in [8] The considers constraints that change over time and performance optimizations.
In this work, the novel modeling framework presented in [8] is extended to include a new dimension of the problem, i.e., the possibility of managing the demand of vehicles at the intersection in order to maximize its capacity. In order to achieve such a result, an Intersection Manager (IM) is assumed that implements the proposed approach, i.e., AVOID-DM, coordinating vehicles in an area surrounding the intersection, that in this work is divided into a pre-danger and a danger zone. When a vehicle enters the pre-danger zone, the IM selects its trajectory so as to set a specific time and speed to enter the danger zone. Such a target trajectory is obtained exploiting information collected from all CAVs in the system, so that: (i) a set of controls exists that avoids collisions in the danger zone, with a guaranteed very small (specified as required) probability; (ii) it maximizes the distance traveled by the vehicle over an optimization window, hence it maximizes the capacity of the intersection; (iii) it maximizes the smoothness of the controls used. Once in the danger zone, the IM evaluates the deviation between the actual and target system state, due to uncertainty, and updates accordingly the vehicle's future trajectory.
Specifically, in order to obtain a safe trajectory for the vehicles, the IM characterizes collision-free areas along the intersection that vehicles can safely use to move. To do so, future vehicle locations are estimated considering the propagated location uncertainty, as a function of the possible controls applied. Because of this characterization, future acceleration profiles can be selected for each CAV so that the areas describing vehicle locations, characterized as ellipses for linear-Gaussian systems, never intersect with each other.
In the following, it is shown that the resulting lowcomplexity approach outperforms state-of-the-art solutions in terms of intersection capacity, i.e., average number of admitted vehicles, and average speed kept by the vehicles in the area coordinated by the IM. Interestingly, such gain is introduced by jointly accounting both for demand management and intersection capacity maximization, even in the presence of vehicle system state uncertainty.
The rest of the paper is organized as follows. Section II presents the system model, while Sec. III showcases the optimization framework underlying AVOID-DM. Section IV unveils the potential of the introduced optimization via extensive simulation experiments, including a comparison with AVOID, the reference state-of-the-art solution presented in [8]. Finally, Sec. V concludes the paper and presents future avenues of research.

A. Vehicle's System State Characterization
In this work, it is assumed that CAVs approaching an intersection interact with an Intersection Manager (IM) to safely traverse the intersection. Specifically, the IM coordinates the vehicles in proximity to the intersection, selecting their controls. The IM runs as a software instance in close proximity to the area of interest, in order to respect tight latency constraints. The actual implementation of such a solution is out of the scope of this work, but cellular or 802.11p architectures, as described in [9] and [10], respectively, can be employed to realize such an implementation.
The motion model of CAV j traversing the intersection follows the discrete-time linear dynamics below: x , a j y ] t ∈ R 2 which denotes the applied acceleration vector at time t, while w j t = [w j ,ẇ j ] t ∈ R 4 ∼ N (0, Σ j w ) denotes the zero mean Gaussian disturbance acting on the system with covariance matrix Σ j w , due to uncontrolled forces acting on the CAV.
In order to exemplify the proposed solution, it is assumed that vehicles do not turn, i.e., their intended path is either from north to south (or vice-versa) or from west to east (or vice-versa). Hence, in case vehicle j travels west-to-east (or west-to-east) in the intersection, Φ = Φ H and Γ = Γ H are: where δt denotes the sampling interval. Similarly, Φ = Φ V and Γ = Γ V can be obtained for vehicles traveling north-tosouth (or south-to-north) in the intersection. As a result of the selected dynamics, overtakes are not possible, and vehicles keep their lane while traversing the intersection. As shown in Eq. (1), the CAV dynamics obey the Markov property i.e., the state of a CAV at the next time step depends only upon its current state and control input. That said, given a known initial state x 0 and a sequence of control inputs u 0:T −1 over the planning horizon of T time-steps, the state x t , t ∈ [1, .., T ] of the CAV can be computed by recursive application of Eq. (1) as: where the CAV index j is omitted for notational clarity. Subsequently, the CAV trajectory X T = {x t }, t ∈ [1, ..T ] over the planning horizon is a stochastic process, where each future state x t is distributed according to with µ t = [µ,μ] t and Ξ t given by: where Σ 0 is the uncertainty associated with the initial system state of vehicle j. Interestingly, the covariance matrix Ξ t does not depend on the applied controls u 0:T −1 , and can be easily pre-computed. As a consequence, selecting the controls for a CAV only modifies the average µ t of the predicted system state, not its distribution. Furthermore, computing recursively µ t , the average of the predicted system state only depends linearly on the applied controls.
Finally, vehicle j computes a valid estimate of its own system state periodically, i.e., each δ t , and it communicates it promptly to the IM using Cooperative Awareness Messages (CAMs) via a wireless link. Such updated system state estimation is obtained by vehicles exploiting classic filtering techniques, such as Kalman Filtering, with linear motion model predictions fused with GPS location measurements. An example of such system state estimation update is reported in [8]. With Gaussian measurement errors associated with the on-board sensors and GPS measurements, vehicle j obtains at any time an estimate of its own system state which follows a multivariate Gaussian distribution with mean µ t and covariance matrix Ξ t . At any time, such estimate, transmitted to the IM, can be used as a starting point of the control decisions, i.e., as x 0 and Σ 0 in Eq. (3).

B. Uncertainty Characterization
In order to account for possible collisions during the CAV's trajectory planning, the 2D-area containing the barycenter of a CAV with probability 1− , with arbitrarily small, is modeled as an ellipse based on the CAV's location distribution. The underlying idea is that, if, at any time t, the controls selected by the IM are such that the ellipses containing the barycenters of two CAVs never intersect, then the probability of collision between the two CAVs is bounded. Specifically, the two CAVs may collide only if at least one of them is actually outside of the ellipse. Hence, the maximum probability of collision P c between i and j is: As mentioned in Sec. II-A, Ξ t can be pre-computed, for all t. Therefore, the associated ellipse is obtained exploiting well-known statistical results which take into account the numerical integration of the distribution of the CAV's location [11]. Specifically, the 2 × 2 sub-matrix of Ξ t describing the multi-variate Gaussian distribution µ t of the CAV's barycenter location is considered. Then, the two semi-axes of the ellipse containing the location of the CAV barycenter with probability 1 − are equal to: where λ + t,j and λ − t,j are the largest and the smallest eigenvalues of the Ξ t 's location sub-matrix, and K is the inverse of the cumulative density function (CDF) of the chi-squared distribution having two degrees of freedom computed at 1− .

III. THE OPTIMIZATION FRAMEWORK
This section presents the optimization framework used in this work to maximize the achievable capacity of a specific intersection. Specifically, the presented optimization framework exploits two important aspects: (i) the management of the demand into the intersection, i.e., the so-called danger zone, and (ii) the coordination of vehicles within the danger zone. Therefore, first the formal definition of the pre-danger and danger zones is introduced in Sec. III-A. Then, the optimization regulating the access of the vehicles to the danger zone, i.e., AVOID-DM, is showcased in Sec. III-B. Finally, the algorithm that allows vehicles to traverse safely the danger zone even in the presence of vehicles' location uncertainty, i.e., AVOID 2.0, is outlined in Sec. III-D.

A. Overview of the IM and of the System Scenario
As previously mentioned, in this work, an IM coordinates vehicles within an area around the intersection. Specifically, it is assumed that vehicles interact with the IM as soon as they are l p m from the center of the intersection. Such area is called, hereinafter, the pre-danger zone of the intersection and could correspond to the communication coverage area of the IM. A smaller area around the intersection is also defined, i.e., the danger zone. A vehicle enters the danger zone as soon as it is located l d m, with l d <l p , from the center of the intersection. A representation of the system scenario is reported in Fig. 1 below. The objective of the IM is to maximize the achievable capacity of a specific intersection, i.e., to maximize the average number of vehicles admitted safely into the intersection. The IM achieves such an objective by maximizing the distance traveled by vehicles, after entering the pre-danger zone, over a specific optimization horizon T . Such an objective function is equivalent to minimizing the vehicles' intersection traversing time, hence maximizing the intersection capacity.
In order to obtain the aforementioned objective, for each vehicle, the IM divides the problem into two main phases, the pre-danger and the danger zone optimizations. In the pre-danger zone, the IM selects the controls for the entire optimization horizon, hence accounting also for the center of the intersection, with the intent to individuate the best moment in time and speed for each vehicle to enter the danger zone. After receiving the selected time and speed for entering the danger zone, the vehicle keeps track of the associated trajectory, avoiding the only possible collision in the predanger zone, i.e., with the preceding vehicle. It should be noted that the time for the vehicle to enter the danger zone and the required speed are only met approximately, due to the uncertainty associated with the vehicle's system state. Thus, at the entrance to the danger zone, the IM updates the vehicle's controls to account for the new approximated system state. The three mentioned optimizations, two used in the pre-danger zone and one in the danger zone, are named respectively hereinafter: AVOID-DM, Car-Follow, and AVOID 2.0.

B. Demand Management in the Pre-Danger Zone
In this section, the proposed AVOID-DM optimization is described. AVOID-DM extends the capabilities of a stateof-the-art intersection management algorithm, i.e., AVOID, that we previously presented in [8], by jointly tackling the demand management and the intersection capacity maximization problems.
The underlying idea for AVOID-DM is that vehicles' trajectories are computed sequentially, at the entrance to the pre-danger zone, and that the information related to the expected trajectories of the vehicles can be exploited to obtain a safe trajectory also for new incoming vehicles. As previously mentioned, given the dynamics discussed in Sec. II, vehicles keep their lane while traversing the intersection. For this reason, two categories of collisions can be considered: lateral, between vehicles crossing the intersection while traveling in perpendicular directions, and frontal, between vehicles following one another. For all lateral collisions, for each vehicle j, a precise collision area can be individuated for each lane crossing its way. As an example, Fig. 1 presents the possible collision areas for vehicle j, entering the predanger zone traveling east-to-west in a 4-way intersection. If only one of the ellipses is in the collision area at any time, then it is ensured that ellipses never cross each other and that the probability of collision respects the bound in Eq. (4). In the same way, the prediction on the location of the vehicle preceding j, i.e., vehicle v in Fig. 1, is known, and thus a constraint can be identified prior to the optimization, such that, at any time t, vehicle j's ellipse keeps at least a distance of d min = f v + f j + s from the preceding ellipse, where f j is the distance between the barycenter of vehicle j and its front bumper, f v is the distance between the barycenter of vehicle v and its back bumper, and s is the safety distance. In order to obtain a safe trajectory for vehicle j, avoiding frontal and lateral collisions, the IM decides vehicle j's acceleration profile, in order for µ t in Eq. (1) to respect the aforementioned constraints. To do so, the IM accounts for the size of the ellipse at any future time t.
In this work, the uncertainty is accounted differently by the IM in the pre-danger and in the danger zone. In the predanger zone, the vehicle follows the trajectory selected by the IM on its own, exploiting for this task all the on-board sensors (e.g., proximity sensors, GPS sensors, etc.). It has to be noted that the only possible collision is with the preceding vehicle and several car-following approaches can be used in order to traverse safely this area [12]. In practice, the predanger area is similar to any other road section that precedes the intersection. For this reason, the IM considers in this area the location of the vehicle distributed as a multi-variate Gaussian distribution, with a constant covariance matrix equal to Σ 0 + Σ j w . In order to approximate the uncertainty associated with the vehicle's location as an output of the on-board Kalman Filter, i.e., Σ 0 , the IM considers a worstcase approximation of Σ 0 over the area of the intersection. On the other hand, in the danger zone, on-board sensors are not always sufficient to avoid collisions, since also lateral, hence sudden, collisions are possible. Thus, the IM decides in advance the controls for the vehicle in the danger zone and propagates the errors as in Eq. (3). The ellipse therefore grows with time and the vehicles only apply the received controls, without tracking any pre-computed trajectory.
It has to be noted that the moment in which vehicle j enters the danger zone is unknown prior to the optimization. For this reason, AVOID-DM embeds the computation of the optimal time for vehicle j to enter the danger zone, accounting for the difference in uncertainty between predanger and danger zones. In order to achieve such a result, auxiliary binary variables are introduced. Let us assume to have a binary variable for each of the slots in the optimization window of vehicle j and that such auxiliary binary variables are equal to 0 if vehicle j is in the pre-danger zone, 1 otherwise. Then, the following equation is sufficient to compute the correct ellipse's semi-major axes at any time t: In Eq. (5), the different terms cancel each other out and the only remaining term is the one that corresponds to the number of time-slots spend by the vehicle in the danger zone at time t. Because of this uncertainty characterization, and considering again, without loss of generality, that vehicle j traverses the intersection from west-to-east, the AVOID-DM optimization is as follows: subject to: where the index for the x direction has been omitted for notational clarity. The maximization of the traveled distance by vehicle j is obtained in Eq. (6a) as the maximization of the distance of the vehicle's barycenter from the center of the intersection at the end of the optimization window T , i.e., µ j T . If vehicle j is constrained by the movement of another crossing vehicle i, it means that even if vehicle j accelerates to the maximum possible speed, it will not be able to pass the corresponding collision area before vehicle i. In most cases, vehicle j could reach the intersection at full speed, but a possible collision forbids choosing such "best" acceleration profile. For this reason, vehicle j is able to maximize the traveled distance with all acceleration profiles that lead to crossing the collision area just after vehicle i. In order to chose among all those optimal acceleration profiles, a multi-objective function is used, that includes two other terms (multiplied by two very small factors γ and β, respectively). The first term sums over the entire optimization window the distance traveled by the vehicle at each time slot. As a result, the vehicle is pushed closer to the danger zone, as much as possible, starting from the moment it enters the pre-danger zone, clearing out the way for new incoming vehicles. The second term minimizes the difference between subsequent accelerations applied by the vehicle, improving the smoothness of the applied controls. Such a term includes the summation over a series of absolute values. In our implementation, absolute values are transformed into a series of additional linear constraints [13], without negatively affecting the computational complexity of the proposed approach.
System state predictions respect the estimation as in Eq. In order for the auxiliary binary variables to assume the desired values, Eq. (6f) is used. Specifically, if vehicle j is in the danger zone at time t, the binary variable b t multiplied by a large constant M turns to 1, so that the constraint can be satisfied. If vehicle j is not yet in the danger zone, Eq. (6f) is always satisfied and b t can assume any value. Nevertheless, a larger number of binary variables equal to 1 represents a larger than necessary ellipse for vehicle j, hence placing its barycenter further away from the preceding vehicle or from the area where a crossing vehicle is placed before the collision. As a consequence, the maximization of the traveled distance forces such binary variables to 0, allowing the automatic determination of the correct value for the binary variables. It should be noted that the possible combinations of values assumed by the binary variables are very limited, i.e., 0 in the pre-danger zone, 1 in the danger zone. Therefore, AVOID-DM is only required to choose the optimal slot when the vehicle enters the danger zone to obtain the values of all the auxiliary binary variables. If the optimization window is reasonably small, the solution can be found in real-time.
Finally, the coordination between vehicles is ensured by the last two constraints, where the values of α i and α v are known at the moment of the optimization. Equation (6g) ensures that the distance between the barycenters of the ellipses of vehicle j and its preceding vehicle v is such that it is larger than d M IN . Further, Eq. (6h) forces vehicle j to traverse B i,j , before or after vehicle i. It is thus imposed that, during the traversal time of vehicle i, the predicted location of vehicle j plus (minus) the larger axes of its elliptical uncertainty description is out of B i,j . A possible implementation of the last constraint in AVOID-DM requires the use of a binary variable multiplied by a big constant M, so that only one of the two possible inequalities is valid at each time slot. Hence, the number of required binary variables is equal to the number of vehicles crossing the way of vehicle j, i.e., very few for realistic intersection scenarios.

C. Tracking the Optimal Trajectory in the Pre-Danger Zone
The output of AVOID-DM allows the determination of the last time q vehicle j is in the pre-danger zone, as well as the expected trajectory and speed, denoted respectively with m j t andṁ j t , for each t ∈ [0, ..., q]. Because of this result, vehicle j can implement any trajectory tracking algorithm. In the following, one of the possible implementations is presented, but any other alternative approach could also be used. Specifically, exploiting a receding horizon approach and exploiting updated system state estimation via Kalman Filtering, at any t ∈ [0, ..., q], vehicle j minimizes the error between all future expected system states and the target system states that are computed by AVOID-DM, as follows: subject to: The minimization of the error of the target trajectory is obtained by again modifying the absolute values in the objective function in Eq. (7a) into linear constraints. The future vehicle's system state prediction is obtained by exploiting the motion dynamics of the vehicle, as in Eq. (3). Constraints include limits on the values assumed by the controls and by the speed of the vehicle (Eqs. (7c)-(7d)). Furthermore, vehicle j is forced to respect a minimum distance to the preceding vehicle v, in order avoid frontal collisions (Eq. (7e)). It has to be noted that the future trajectory of vehicle v (and the associated uncertainty) is known at the moment of the optimization.

D. Traversing the Danger Zone
Just before entering the danger zone, i.e., at time q, vehicle j receives from the IM an updated set of controls to traverse the intersection, that are calculated using AVOID 2.0 as follows: subject to: The optimization used at the ingress of the danger zone optimizes a multi-objective function that is exactly the same as the one used in AVOID-DM. As a result, the controls obtained by AVOID 2.0 only change, compared to the ones obtained by AVOID-DM, to account for the deviation from the expected initial system state. Also, the constraints are the same in order to obtain only an update of the controls decided at the entrance to the pre-danger zone. Equations (8f)-(8g) take into account the fact that all vehicles considered for frontal and lateral collisions are now in the danger zone, hence, the binary variables introduced in AVOID-DM are not required. Furthermore, for all vehicles apart from vehicle j, the most updated system state estimate is used, hence having an associated covariance matrix at time q equal to Σ 0 + Σ w .

IV. PERFORMANCE EVALUATION
In this section, the performance evaluation of AVOID-DM is presented. First, the reference scenario is illustrated (Sec. IV-A). Then, AVOID-DM is compared to the state-of-the-art solutions in Sec. IV-B. Herein, the impact of the different components of the multi-objective function used is also evaluated.

A. Simulation Scenario
In order to showcase the performance of the proposed approaches, a 4-way intersection as in Fig. 1 is considered and the performance of AVOID-DM is compared to the approach presented in [8], i.e., AVOID. Specifically, two versions of AVOID are considered. The first one as reported in [8], while the second one considers the same multi-objective function as AVOID-DM, i.e., the optimization AVOID 2.0 presented in Sec. III-D, so as to evaluate the impact of managing the demand in the danger-zone.
Vehicles enter the pre-danger zone when their location is at 300 m from the center of the intersection and the entrance to the danger zone is at 150 m from the center of the intersection. In AVOID, vehicles are controlled by the IM only in the danger zone, hence, at 150 m from the intersection. In order to evaluate fairly the two approaches, the optimization window used is chosen so that a vehicle traveling with an average speed of 8 m/s can traverse the entire danger zone. Hence, T = 56 s in the case of AVOID-DM and T = 37.5 s in the case of AVOID.
The distribution of the time interval between vehicles entering the danger zone is exponential, with an average equal to 2 s. This traffic rate is comparable to the average arrival rate at a central intersection in a medium size city around peak hour [14]. As suggested by all common cruise control algorithms, vehicles are allowed into the danger zone if it exist at least one feasible acceleration profile that avoids a collision with the preceding vehicle. Vehicles select, with uniform distribution, their initial speed (between v M IN = 0 and v M AX = 14 m/s) and their lane. Apart from the location of the arrival (pre-danger vs. danger zone), the same set of arrivals is used for both AVOID-DM and AVOID.   The uncertainty of the initial state, as well as of acceleration/GPS measurements used in the Kalman Filter by the vehicles and in the system state prediction by the IM, follows typical sensor sensitivity [15]. Hence: (i) the worst case approximation of the Kalman Filter's co-variance is Σ 0 = [0.6m 2 , 0.2(m/s) 2 ; 0.2m 2 , 0.06(m/s) 2 ], while (ii) the covariance of the dynamics error is Σ w = [0.0125δt 4 0.025δt 3 ; 0.025δt 3 0.5δt 2 ]. In both cases, the covariance matrix holds for location and speed in the direction of the vehicle's movement, and it is zero otherwise. Finally, it is assumed that all vehicles have the same size and that vehicles' barycenters have to be at d min = 8 m from each other to ensure at least s = 4 m of safe distance. The absolute value of the acceleration is also constrained to values within 0 and 3 m/s 2 , while ∆a = 1 m/s 2 . Finally, δt, i.e., the slot duration, and are fixed to δt = 1 s and = 10 −5 , respectively. Each simulation involves a total of 10000 vehicles.
Four different metrics are used to assess the performance of the proposed approach: (i)-(ii) the CDF of the distance traveled and, for a fair comparison, of the average speed achieved by vehicles in the optimization window T ; (iii) the average time between vehicles admitted to the intersection; (iv) the minimum distance between vehicles' barycenters sharing a potential collision area.

B. Comparison with AVOID
To evaluate the performance of the presented approaches, an ad-hoc MatLab simulator has been developed, following the framework in Sec. II and modeling the predicted, the estimated, and the real position of vehicles in the danger zone at each time slot. The GUROBI optimization solver is used to solve, for each vehicle arrival, the above-described two versions of AVOID, and the AVOID-DM with {γ = 10 −6 , β = 10 −5 } and with {γ = 10 −6 , β = 0}, i.e., with and without considering the smoothness in the objective function. Figure 2 shows the results obtained in terms of the CDF of the total traveled distance and of the average speed achieved by vehicles in the optimization window. To fully evaluate the achieved performance, the average time between vehicles admitted into the intersection is also considered (Fig. 3). It should be noted that, even though arrivals are generated every 2 s, on average, not all of them are valid. This is the case every time a vehicle entering the pre-danger zone (or the danger zone in case of AVOID) has an initial speed or an arrival time that would not allow avoiding a collision with the 2538 preceding vehicle in the same lane. Figure 3 shows that the probability for a randomly generated arrival to be admitted to the intersection increases dramatically when the pre-danger zone is considered. AVOID-DM, independently from the use of smooth accelerations, admits on average 20% more vehicles than AVOID (for both cases of AVOID considered). Further, even though a larger number of vehicles is present in the intersection, AVOID-DM obtains a better performance than AVOID, especially at the tail of the distributions. Specifically, while only 0.13% of the vehicles do not reach the center of the intersection with AVOID-DM (positioned after 300 m), at least 2.37% of the vehicles do not reach the center of the intersection (positioned after 150 m) with AVOID. The difference in performance is even more evident when the average speed over proportional optimization windows is considered (Fig. 2b). Indeed, the 1-percentile and the 10percentile of the distributions present an average speed gain of at least 114% and 20.2%, respectively, with AVOID-DM, even though a larger number of vehicles is in the intersection. Figure 2b also reports the average speed achieved by vehicles with AVOID-DM considering only the danger zone. Interestingly, for 99% of the vehicles, the average speed in the danger zone is larger than 12 m/s. This means that vehicles traverse the danger zone almost at maximum speed, reducing the time spend in the intersection. As previously mentioned, this is due to the fact that, in the danger zone, the uncertainty grows with time. If vehicle j shares a collision area with a crossing vehicle, reducing the time spend by vehicle j in the danger zone prior to that collision area, also reduces the size of the vehicle's ellipse just before vehicle j can cross the corresponding collision area. This means that the barycenter of vehicle j is closer to the collision area at that time, originating a trajectory that overall allows traveling a larger distance for vehicle j.
In addition, as expected, the performance of AVOID-DM in terms of distance traveled by the vehicles does not depend on the (small) values assumed by γ and β. The multi-objective function used only affects how AVOID-DM achieves the maximization of the distance traveled, but not its maximum value.  that share a collision area, in case of crossing vehicles, or vehicles in the same lane that are on the same side of the intersection for at least one time slot. Interestingly, all vehicles respect the minimum distance of 8 m among them, hence avoiding collisions and ensuring safe intersection traversing. In general, the minimum distance between vehicles following each other is smaller than the distance between vehicles crossing each other's trajectory. This is due to the fact that AVOID-DM exploits a conservative approach for vehicles at the center of the intersection, imposing the presence of at most one ellipse in each of the collision areas. Furthermore, exploiting the fact that vehicles reduce their time in the danger zone prior to the collision areas, hence the uncertainty associated to their locations, the first 25-percentile of the CDF of the distance between crossing vehicles presents smaller values when AVOID-DM is used. Finally, the impact of the multi-objective function on the trajectory used by the vehicles is evaluated. First, the difference between the expected system state and the actual system state of vehicles at the entrance to the danger zone is reported in Fig. 5. It is shown that Car-Follow is able to carefully track in the pre-danger zone the trajectory chosen by AVOID-DM, irrespective of whether smoothness is considered or not. Indeed, at the entrance to the danger zone, less than 4.15% of the vehicles have an error on the expected location and less than 0.22% of the vehicles have an error on the expected speed, that is larger than 2 m and 2 m/s, respectively.
Further, Fig. 6 presents the statistics on the difference between consecutive accelerations applied by the vehicles. Clearly, including the smoothness function in the objective allows, especially in the pre-danger zone, to reduce dramatically the variability of the controls applied by the vehicles. With the use of the smoothness function, 60% of the applied accelerations only differ by ±0.25 m/s 2 if compared to the previous value. Without the smoothness function, this is true for only 26% of the cases. Maximizing the smoothness of the applied acceleration is not only fundamental from the perspective of the comfort of the vehicles' passengers, but it also reduces the vehicle's gas consumption, if associated with a smaller absolute value of the average acceleration  used [16]. This is indeed the case with AVOID-DM. When the smoothness function is considered, the average absolute value of the acceleration applied by a vehicle is 0.501 m/s 2 , while it is 0.626 m/s 2 otherwise. Once again, it has to be noted that the additional advantage brought by also considering smoothness in the objective function does not affect the performance achieved by AVOID-DM (in terms of both average speed and average number of vehicles admitted to the intersection). Finally, Fig. 6b also confirms that the vehicles traverse the danger zone mostly at constant speeds, since the applied accelerations do not change for more than 92% of the cases.

V. CONCLUSIONS
This work presented a novel framework for autonomous intersection management that considers uncertainty in the location of CAVs. The use of a low-complexity algorithm, AVOID-DM, is envisioned, that coordinates vehicles within a danger zone surrounding the intersection. AVOID-DM is a centralized approach that exploits a joint optimization of demand management, intersection capacity maximization, and periodic transmissions from vehicles to compute and distribute acceleration profiles, so that connected autonomous vehicles can safely cross an intersection. Performance results indicate that the proposed technique allows the safe crossing of vehicles, while at the same time maximizing the average number of vehicles admitted to the intersection and the average speed of the vehicles, ensuring also smoothness in the vehicles' acceleration profiles.
Future research directions involve an extension of AVOID-DM to cases where vehicles take turns or change lanes. As a consequence, envisioned extensions will also consider motion models where vehicle controls affect nonlinearly the uncertainty of vehicle location.