{'fraction_non_alphanumeric': 0.04415316229743446, 'fraction_numerical': 0.015137727065905989, 'mean_word_length': 4.005957074025405, 'pattern_counts': {'":': 0, '<': 17, '<?xml version=': 0, '>': 7, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 23, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
Decentralized Optimal Control of Connected Automated Vehicles at Signal-Free Intersections Including Comfort-Constrained Turns and Safety Guarantees

We extend earlier work for optimally controlling Connected Automated Vehicles (CAVs) crossing a signal-free intersection by including all possible turns taken so as to optimize a passenger comfort metric along with energy and travel time minimization. We show that it is possible to achieve this goal in a decentralized manner with each CAV solving an optimal control problem, and derive explicit solutions that guarantee collision avoidance and safe distance constraints within a control zone. We investigate the associated tradeoffs between minimizing energy and vehicle travel time, as well as the passenger comfort metric and include extensive simulations to illustrate this framework.


I. INTRODUCTION
Intersections in transportation systems are one of the major traffic control challenges as they account for a large part of accidents and of overall road congestion. As documented in [1], in 2014, traffic congestion caused vehicles in urban areas to spend 6.9 billion extra hours on the road at a cost of an extra 3.1 billion gallons of fuel, resulting in a total cost estimated at $160 billion. From a control and optimization standpoint, the goal is to develop efficient traffic management methods so as to reduce congestion and increase safety with minimal impact on the existing infrastructure. This is typically accomplished through tighter spacing of vehicles [2], [3] which can alleviate congestion, reduce energy use and emissions, and improve safety under proper control. Forming "platoons" of vehicles is a popular system-level approach that gained momentum in the 1990s [4], [5]. More recently, a study in [6] indicated that transitioning from intersections with traffic lights to autonomous ones has the potential of doubling capacity and reducing delays.
To date, traffic light control is the prevailing method for coordinating conflicting traffic flows and ensure road safety in urban areas. Recent technological developments include designing adaptive traffic light control systems that can dynamically adjust the signal timing to various context, e.g., [7] and references therein. However, in addition to the obvious infrastructure cost of traffic lights, the efficiency and safety offered by such signaling methods is limited, thus motivating research efforts to explore new approaches capable of enabling smoother traffic flow while ensuring safety. Connected Automated Vehicles (CAVs), also referred to as autonomous or self-driving vehicles, provide the most intriguing opportunity for better traffic conditions in a transportation network and for improving traffic flow. CAVs can assist drivers in making better operating decisions or they can do so in a fully automated way so as to improve safety and reduce pollution, energy consumption, and travel delays. One of the very early efforts in this direction was proposed in [8] and [9] where the merging problem was formulated as a linear optimal regulator to control a single string of vehicles. The key features of an automated intelligent vehicle-highway system (IVHS) and a related control system architecture are discussed in [10]. More recently, several research efforts have been reported in the literature for CAV coordination at intersections. Dresner and Stone [11] proposed a reservation-based scheme for centralized automated vehicle intersection management while no turns are allowed. Since then, numerous research efforts have explored safe and efficient control strategies, e.g., [12]- [14]. Several efforts have focused on minimizing vehicle travel time under collision-avoidance constraints [15]- [20]. Zohdy et al. [18] presented an approach based on Cooperative Adaptive Cruise Control (CACC) for minimizing intersection delay and hence maximizing the throughput. Lee and Park [19] considered minimizing the overlap between vehicle positions. Miculescu and Karaman [20] have studied intersections as polling systems and determined a sequence of times assigned to vehicles on each road which provides provable guarantees on safety and expected wait time. Reducing energy consumption is another desired objective which has been considered in recent literature [21]- [24]. Hellstrom [23] proposed an energy-optimal control algorithm for heavy diesel trucks by utilizing road topography information. Based on Vehicle-to-Vehicle (V2V) communication, a minimum energy control strategy is investigated in car-following scenarios in [24]. A detailed discussion of recent advances in this area can be found in [25].
Our earlier work [26] and [27] established a decentralized optimal control framework for coordinating on line a continuous flow of CAVs crossing an urban intersection without using explicit traffic signaling, assuming no left and right turns are allowed. For each CAV, an energy minimization optimal control problem is formulated where the time to cross the intersection is first determined through a throughput maximization problem. We solved this problem for each CAV entering a specified Control Zone (CZ) so that the acceleration/deceleration of the CAV is controlled until it reaches a Merging Zone (MZ) where the potential of lateral collisions exists. To ensure safety, we required CAVs to have a constant speed through the MZ while also maintaining a safe distance between them to avoid rear-end collisions. The presence of hard safety constraints makes it challenging to ensure the existence of a feasible solution to each such decentralized problem. Therefore, we also established in [26] the conditions under which such feasible solutions exist and showed that they can be enforced through an appropriately designed Feasibility Enforcement Zone (FEZ) that precedes the CZ in [28].
The inclusion of left and right turns in this framework creates significant complications. Aside from the added complexity of ensuring collision avoidance, we can no longer expect each CAV to maintain a constant speed in the MZ. Instead, we must allow a CAV to vary its speed depending on the turn. In addition, passenger comfort in taking such turns becomes an essential component of an automated trajectory design. The problem of coordinating CAVs at intersections including left and right turns has been addressed by Kim and Kumar [29] based on an approach using Model Predictive Control (MPC) to achieve system-wide safety and liveness of intersection-crossing traffic. Dresner and Stone in [30] considered scenarios that allowed left and right turns using a reservation-based scheme together with a communication protocol which may involve a stop sign and traffic lights.
The contribution of this paper consists of extending the optimal control framework in [26]. First, unlike the approach in [26] where we first solved a throughput maximization problem to determine a CAV's travel time and then an energy minimization problem for each CAV, here we formulate a problem in which a CAV seeks to jointly minimize both its travel time through the CZ and its energy consumption. This allows us to readily quantify the tradeoff between these two criteria. Second, we formulate and solve a subsequent optimal control problem for each CAV crossing the MZ in which we allow it to vary its speed inside the MZ and include a passenger comfort metric when a turn is taken. The ability to decentralize the optimal control in [26] rests on showing that each CAV needs only information from the CAV which is physically ahead of it and the one immediately preceding it in entering the CZ. The presence of turns complicates this simple coordination structure; however, we are still able to show that each CAV needs only information from a small set of well-defined CAVs among those preceding it (but not necessarily immediately preceding it) in entering the CZ. We also derive explicit solutions for the two optimal control problems, through the CZ and then the MZ, including the possibility of safety, state and control constraints becoming active. Our analysis includes the derivation of properties characterizing an optimal control solution, such as the continuity of the optimal control when an unconstrained optimal trajectory arc is followed by one with an active constraint, and it allows us to determine whether an optimal control solution for each CAV is feasible at the time it enters the CZ. The paper is structured as follows. In Section II, we review the model in [26] and extend it to include left and right turns. In Section III, we derive the conditions that guarantee safety for each CAV in terms of its time to reach the MZ constrained by those of CAVs preceding it in the CZ. In Section IV, we formulate a decentralized optimal control problem for each CAV that jointly minimizes its travel time and energy consumption, prove structural properties of optimal trajectories, and derive an explicit solution for it. In Section V, we formulate and solve another optimization problem with the objective of jointly minimizing a passenger comfort metric inside the MZ and its energy consumption. Simulation results are given in Section VI where the timeenergy tradeoff is illustrated, as well as the comfort-energy tradeoff.

II. THE INTERSECTION MODEL
We begin with a brief review of the model introduced in [26] and fully developed in [27]. We consider an intersection ( Fig. 1) where the region at its center, assumed to be a square of side S, is called Merging Zone (MZ) and defines the area of potential lateral CAV collisions. The intersection has a Control Zone (CZ) and the road segment from the CZ entry to the CZ exit (i.e., the MZ entry) is referred to as a CZ segment. The length of each CZ segment is L > S and it is assumed to be the same for all entry points to a given CZ. Extensions to asymmetric CZ segments are possible and considered in [31].
We assume the existence of a "coordinator" whose task is to handle the information exchanges between CAVs, while each CAV maintains its own control autonomy. Let N (t) ∈ N be the cumulative number of CAVs which have entered the CZ by time t and formed a queue that designates the crossing sequence in which these CAVs will enter the MZ. There is a number of ways to manage such a queue. In [26] a strict First-In-First-Out (FIFO) crossing sequence is assumed, that is, when a CAV reaches the CZ, the coordinator assigns it an integer value i = N (t) + 1. This can be relaxed as in [31] to allow for dynamically resequencing CAVs as each new one arrives, hence maximizing throughput. If two or more CAVs enter a CZ at the same time, then the corresponding coordinator selects randomly the first one to be assigned the value N (t) + 1.
For simplicity, we assume that each CAV is governed by second order dynamics: where p i (t) ∈ P i , v i (t) ∈ V i , and u i (t) ∈ U i denote the position, i.e., travel distance since the entry of the CZ, speed and acceleration/deceleration (control input) of each CAV i. The sets P i , V i and U i are complete and totally bounded subsets of R. These dynamics are in force over an interval where t 0 i and t f i are the times that the vehicle i enters the CZ and exits the MZ respectively. To ensure that the control input and vehicle speed are within a given admissible range, the following constraints are imposed: (2) To ensure the absence of any rear-end collision throughout the CZ, we impose the rear-end safety constraint where k is the CAV physically ahead of i and δ is the minimal safe following distance allowable. An alternative safety constraint often used is to maintain a safe headway [5], i.e., a time gap that is a function of speed. However, since we consider urban intersections, the average speed does not exhibit significant variations and we can translate the allowable headway to a safe inter-vehicle distance (replacing (3) by a headway constraint is still possible at the expense of complicating the explicit solutions to the optimal control problems we will consider in the sequel).
In this modeling framework, we assume that (i) CAVs follow the crossing sequence established by the coordinator and that no overtaking, reversing directions, or lane-changing are allowed, (ii) each vehicle has proximity sensors and can observe and/or estimate local information that can be shared with other vehicles, (iii) the decision of each CAV on whether a turn needs to be made at the MZ is known upon its entry in the CZ and remains unchanged, and (iv) for each CAV, the speed constraints in (2) and the rear-end safety constraint in (3) are not active at t 0 i . If this last assumption is violated, any optimal control solution is obviously infeasible and we must resort to control actions that simply attempt to satisfy these constraints as promptly as possible; alternatively, we may impose a Feasibility Enforcement Zone (FEZ) that precedes the CZ as described in [28].
Finally, we make the following simplifying assumption: Assumption 1. For each CAV i exiting the MZ, its speed remains constant for at least a distance of length δ.
This assumption is reasonable since there is no compelling reason for vehicles to accelerate or decelerate right after they exit the MZ unless an imminent safety issue is involved.

A. Modeling Turns
The inclusion of left and right turns needs special attention in the context of safety as well in ensuring passenger comfort. As a vehicle turns, it is affected simultaneously by two forces: the braking force and the centripetal force (see Fig.2). Note that the centripetal force is provided by the friction force, which depends on the speed of the vehicle. Hence, to ensure safety, vehicles need to slow down while making turns. The smaller the turning radius is, the lower the speed needs to be. On the other hand, to minimize passenger discomfort, vehicles need to maintain a steady movement, i.e., avoid any abrupt change in the braking force. In other words, we need to minimize the change in acceleration (i.e., the jerk).
Let d i denote the decision of vehicle i on whether a turn is to be made at the MZ, where d i = 0 indicates a left turn, d i = 1 indicates going straight and d i = 2 indicates a right turn. Letting S L denote the length of the left turn trajectory and S R the length of the right turn trajectory, we assume that S R < S < S L holds. The speed for which an intersection curve is designed depends on an assigned speed limit, the type of intersection, and the traffic volume [32]. Generally, the "desirable time" ∆ i that a vehicle needs to make a turn at an intersection [32] is given by where R i is the centerline turning radius (see Fig. 2); E is the super-elevation, which is zero in urban conditions; F is the side friction factor; and ∆ 1 i is the time for CAV i going straight. Therefore, the time t m i when CAV i enters the MZ is directly related to the time t f i that the vehicle exits the MZ through ∆ i : Note that ∆ i is different for left and right turns since the associated turning radii R i are different.

B. Relative Location Sets
When a CAV enters the CZ and is assigned a unique index i = N (t) + 1 by the coordinator, it is also assigned to one and only one of four subsets which capture its position relative to CAVs j < i. These subsets are considerably different than those in [26] due to the presence of turns and are defined as follows: (1) E i (t) contains all vehicles j < i that can cause a rearend collision with i at the end of the MZ. For example, in Fig. 3(a), E 3 (t) contains vehicle #2 as it may cause a rearend collision with vehicle #3 at the end of the MZ, and E 4 (t) contains vehicle #3 as it may cause a rear-end collision with vehicle #4 at the end of the MZ.
(2) S i (t) contains all vehicles j < i traveling on the same lane that can cause rear-end collision with i at the beginning of the MZ. For example, in Fig. 3(b), S 3 (t) contains vehicle #2 as it may cause a rear-end collision with vehicle #3 at the beginning of the MZ, and S 4 (t) contains vehicle #3 as it may cause a rear-end collision with vehicle #4 at the beginning of the MZ. Note that if CAVs j and i, j < i travel on the same lane and in the same direction, we have j ∈ E i (t); since j belongs to one and only one of these subsets, we cannot have j ∈ S i (t). (

C. Merging Zone Safety Constraints
As in [26], we consider a First-In-First-Out (FIFO) queue by imposing the following condition: However, as already mentioned, this FIFO structure may be relaxed by dynamically resequencing CAVs as each new one arrives [31] aiming to maximize throughput and subsequently re-solving each decentralized CAV problem. As we will see, this is made possible by the relatively modest computational cost of solving such problems. A lateral collision involving CAV i may occur only if some CAV j = i belongs to L i (t). This leads to the following definition: Definition 1. For each CAV i ∈ N (t), the set Γ i that includes all time instants when a lateral collision involving CAV i is possible: Consequently, to avoid a lateral collision for any two vehicles i, j ∈ N (t) on different roads, the following constraint should hold: In earlier work [26], the speed in the MZ was considered to be constant, a condition made possible by the fact that, in the absence of turns, all CAVs have the same trajectory within the MZ. Thus, t m i > t m i−1 could be implicitly ensured by t f i > t f i−1 . Since this is no longer the case, i.e., speeds and position trajectories in the MZ may be different for CAVs with different driving directions (e.g., CAV i may turn left This becomes an issue only when i and i−1 are traveling on the same lane. We shall henceforth reserve the symbol k (k < i) to denote the index of the CAV physically located ahead of i. Then, for CAVs i and k we impose the following constraint: to avoid any rear-end collision at the beginning of the MZ.

D. Terminal Conditions
We now turn our attention to the terminal conditions that must be enforced on each CAV i at times t m i and t f i when it enters and exits the MZ respectively. The safety requirements discussed above imply constraints on these times that depend on the relative location sets of i as explained next. ( In this case, CAV e is the vehicle immediately ahead of CAV i in the FIFO queue that may cause a rear-end collision with i at the end of the MZ. To avoid such a rear-end collision, e and i should maintain the minimal safe distance δ defined in (3) by the time i exits the MZ. Since, by Assumption 1, each CAV maintains a constant speed after the MZ exit for at least a distance δ, we set where t f e and t f i are the times that CAVs e and i exits the MZ respectively, and v f e is the speed of CAV e at the exit of the MZ. It is easy to see that if the only objective for i is to minimize its travel time, then it should set t f i = t f e +δ/v f e . In general, however, as we will see when setting up each CAV's decentralized optimal control problem in Section IV, (9) in conjunction with (5) provides only a lower bound constraint on t m i for this problem: Unlike our earlier work [26] where we considered a constant speed for all CAVs inside the MZ, the inclusion of left and right turns forces us to vary the speed , are determined by the solution of the optimal control problem we will define in Section V so as to take a passenger comfort metric into consideration. For this problem, t m i is the initial time which is obtained from the decentralized optimal control problem over In this case, CAV s is the vehicle immediately ahead of CAV i in the FIFO queue such that it may cause a rear-end collision at the beginning of the MZ. To guarantee the rear-end collision constraint does not become active we set where, in view of (4), is the time vehicle s needs to travel a distance δ inside the MZ, where ∆ 1δ s is the time for CAV s traveling a distance δ if it goes straight. The time t f i when CAV i will be exiting the MZ is given by (5) and, using (11), we get t f i ≥ t m s +∆ δ s +∆ i . However, if s makes a left turn and i makes a right turn, since S L > S R , we may have t f i < t f s , which violates (6). In that case, we must set t f i ≥ t f s . This situation arises whenever the time to cover the trajectory of s through the MZ is longer that that of i. Combining these two cases, it follows that and, from (5), t m i is constrained so that In this case, CAV l is the vehicle immediately ahead of CAV i in the FIFO queue such that it may cause a lateral collision inside the MZ. We constrain the MZ to contain only either CAV l or i so as to avoid such a collision; this is intended to enhance safety awareness, but it could be modified appropriately, if necessary. Therefore, CAV i will enter the MZ only after l has exited it, that is, and t f i is obtained through (5).
In this case, CAV o is the vehicle immediately ahead of CAV i in the FIFO queue such that it will not generate any collision with i in the MZ, so we only require that and, from (5), We are now in a position to establish a lower bound on t m i , the time for CAV i to enter the MZ, which will serve as a terminal time constraint in the decentralized optimal control problem considered in Section IV. In so doing, we need to also take into account the lower bound t L i imposed on t m i due to the control and state constraints (2). To derive this lower bound of t m i , we have to consider two cases, which depend on whether CAV i can reach v max upon arriving at the MZ: (i) If CAV i enters the CZ at t 0 i , accelerates with u i,max until it reaches v max and then cruises at this speed until it leaves the MZ at time t 1 i , it was shown in [26] that The following provides a lower bound for t m i ensuring that is feasible. Theorem 1. The lower bound on the time when CAV i can enter the MZ and satisfy all MZ safety constraints is given by is feasible since it depends only on the control and state constraints (2). Let j < i and j = e, s, l, o. There are four cases to consider as follows.
(1) j ∈ E i (t). Since e, j ∈ E i (t), CAVs e and j are driving towards the same lane. As e = max r {r ∈ E i (t)} and j = e, we have j < e and j ∈ E e (t). If t m which ensures the absence of a rearend collision at the end of the MZ and (9) holds for all j ∈ E i (t).
(2) j ∈ S i (t). Similarly, since we have s, j ∈ S i (t), CAVs s and j are traveling on the same lane. As s = max r {r ∈ S i (t)} and j = s, we have j < s and j ∈ S s (t). In this case, a rear-end collision at the beginning of the MZ is possible. If In this case, a lateral collision is possible between i and j. Given t m i ≥ t f l , unlike cases (1) and (2), we cannot determine which subset j belongs to with respect to CAV l. To explore the relationship between CAV l and j explicitly, there are four subcases to consider as follows. (i) j ∈ E l (t). According to (9), According to (13), According to the definition of O i (t), j cannot collide with i. To summarize, as long as (18) holds, we can guarantee that all MZ constraints are satisfied.
Corresponding to the lower bound of terminal time t L i , there also exists the upper bound t U i , that is, where are derived in a similar way as t 1 i and t 2 i in (17) respectively. Based on (19), the following upper bound constraint applies: It follows directly from (18) that if each CAV seeks to minimize its travel time through the intersection it should select a time t m i given by the lower bound on the righthand-side. The CAV can then formulate a fixed terminal time optimal control problem with a given objective function and solve this problem in a decentralized way since the value of t m i depends only on information associated with four CAVs (with indices given by e, s, l, o < i) whose optimal trajectories have already been determined prior to the arrival of CAV i at the CZ. This approach was followed in earlier work [26] where no turns were considered. In what follows, we formulate instead a problem that jointly aims to minimize the travel time of CAV i along with a measure of its energy consumption. Therefore, the optimal value of t m i is obtained as part of this problem's solution and the value of Theorem 1, i.e., (18), is to provide a constraint ensuring that this value is feasible.

III. OPTIMAL CONTROL OF CAVS IN THE CZ
The objective of each CAV inside the CZ, i.e., over , is to derive the optimal acceleration/deceleration which minimizes a convex combination of its travel time and energy consumption. For each CAV i, we define its where p i (t), v i (t) are the traveling distance and speed of CAV i and d i indicates whether i is making left or right turn or going straight at the MZ. The fourth element in , the inter-vehicle distance between CAV i and CAV k which is physically ahead of i in the same lane (the index k is made available to i by the coordinator). Based on this information the coordinator can also evaluate consisting of CAV indices as previously defined for the corresponding sets where with β ∈ [0, 1] a weight associated with the importance of travel time relative to energy, andū = max{u i,max , |u i,min |}. In this manner, we have ∈ [0, 1] and the overall objective is a welldefined convex combination of the normalized travel time and normalized energy. Note that the objective function in (21) can be rewritten as Remark 1. Unlike the problem considered in [26] where t m i was obtained a priori to optimize travel times, here the optimal travel time is determined by the solution of the problem itself. Similarly, the terminal speed v m i is also obtained from the optimal control problem. An alternative formulation is to pre-specify a "target" v m i or to include a penalty term on the deviation of the actual terminal speed from v m i .

A. Problem Decomposition
In order to simplify notation, we rewrite (21) as (18), (20), where γ = γ1 2γ2 . Note that (18) and (20) are constraints applied to the terminal time t m i . In order to efficiently obtain analytical solutions on line, we proceed with a two-step approach. The first step is to consider the following problem by relaxing the terminal time constraints in (22), that is, We denote the problem above as P 0 . After solving P 0 , the terminal time obtained is denoted as t m0 i . The second step is to check t m0 i and (i) If t m0 i satisfies both (18) and (20), then t m0 i is the optimal terminal time, (ii) If t m0 i violates (18), then solve problem P 1 by adding to P 0 the terminal time lower bound constraint from Theorem 1, i.e., t m (20) , then solve problem P 2 by adding to P 0 the upper bound terminal time constraint (20) , the lower bound on t m i is higher than its upper bound, the problem is obviously infeasible.

B. Analytical Solution
Given the objective function of problem P 0 , the Hamiltonian is and the Lagrangian with constraints directly adjoined is where (omitting time arguments for simplicity) λ i = [λ p i , λ v i ] T ∈ R 2 is the costate vector, g i (u i , t) ≤ 0 and h i (p i , v i , t) ≤ 0 represents the control constraints and state constraints respectively, and where k is the CAV physically ahead of i in the same lane and its position p k (t) is known to i through the coordinator (or through on-board sensors).
The Euler-Lagrange equations becomė anḋ Note that if we need to solve P 1 and P 2 , then the second equation in (33) no longer holds; instead, t m i is fixed to its upper or lower bound as described above. In addition, there also exist the state boundary conditions p i (t 0 A complete solution of this problem requires that constrained and unconstrained arcs of an optimal trajectory are pieced together to satisfy all conditions (26) through (34). This includes the five constraints (three pure-state constraints, two control constraints) in (26) through (30). While there are many different cases that can occur, the nature of the optimal solution rules out the possibility of several cases. In what follows, we provide a complete analysis of the case where no constraints are active and of the case where the safety constraint p i (t) + δ − p k (t) ≤ 0 is the only active one. A discussion of the remaining cases can be found in Appendix A.

C. Unconstrained Optimal Control Analysis
For problem P 0 , the terminal time is free whereas for P 1 and P 2 the terminal time is fixed. Thus, we provide the analysis for each of these two cases.
1) Free Terminal Time: When the state and control con- Since ν s i = 0, (31) becomesλ p i (t) = − ∂Li ∂pi = 0 which leads to where a i is a constant. Since where b i is a constant. We can now obtain a complete analytical soloution of P 0 as follows.
The next two results establish a basic property of the optimal control, i.e., it is non-negative and non-increasing, and the fact that two of the constraints in (2) cannot be active.

Lemma 1.
For the unconstrained problem with free terminal time, the optimal control is non-negative, i.e., u * i (t) ≥ 0, and monotonically non-increasing Proof. To prove that u * i (t) ≥ 0, let us assume that the optimal control includes an interval that is negative, i.e., u * Next, we construct another control u c i (t) which is the same as u * i (t) except that u c i (t) = 0 for t ∈ [t 1 , t 2 ]. Let us first prove that u c i (t) is feasible. Note that . Moreover, for t ∈ [t 1 , t 2 ], u c i (t) = 0 does not violate any control constraints. In addition, since we . Hence, the speed constraints are satisfied as well. By assumption, the safety constraint (3) is inactive over [t 0 i , t 1 ]. If, under u c i (t), the safety constraint remains inactive over [t 1 , t 2 ], then u c i (t) is feasible. Otherwise, since (3) is inactive under u * i (t), there exists some > 0 such that it remains inactive over [t 1 , t 1 + ], in which case we modify If that happens, then we modify u c i , which indicates that the optimal control u * i (t) leads to a longer travel time compared to u c i (t). Denoting the energy consumption in (22) as J u (u i (t)) and allowing for the possibility that there exists where we have used the fact that u c i (t) = 0 for at least t ∈ [t 1 , t 1 + ] and u c . In view of the objective function (21), since we have shown that , we can see that the optimal control u * i (t) incurs a larger cost compared to u c i (t), which contradicts the assumption that u * i (t) is the optimal control. Therefore, u * i (t) cannot be negative at any t ∈ [t 0 i , t m i ] and we have u * i (t) ≥ 0 as long as the optimal trajectory is unconstrained.
To prove that u * i (t) is monotonically non-increasing, observe that u * i (t m i ) = 0 from (41d). Since the optimal control is a linear function of time as indicated by (38) and non-negative as established above, it follows that u * i (t) is monotonically non-increasing.

Lemma 2.
For the unconstrained problem with free terminal time, it is not possible for constraints v min −v i (t) ≤ 0 and/or u min − u i (t) ≤ 0 to become active.
Proof. By Lemma 1, u * i (t) ≥ 0. Hence, u min − u i (t) ≤ 0 cannot become active. Since, by assumption, v min < v 0 i < v max , v * i (t) cannot reach v min and the constraint v min − v i (t) ≤ 0 cannot become active.
2) Fixed Terminal Time: If the terminal time t m * i obtained from solving P 0 turns out to violate (18) or (20), then, as described in the two-step approach earlier, we need to solve either P 1 or P 2 by setting t m i to a fixed value which is either the lower bound in (18) or the upper bound in (20). Therefore, the transversality condition H i (t m i ) = 0, i.e., the fifth equation in (41), no longer holds and the solution of this problem reduces to which yields the four parameters a i , b i , c i , d i from a simple system of linear equations. A typical simulation example of this case when (18) is violated can be found in Section V. With the terminal time fixed, Lemma 1 needs to be modified as follows.
Lemma 3. For the unconstrained problem with fixed terminal time, the optimal control must be either monotonically non-increasing and u * i (t) ≥ 0, or monotonically nondecreasing and u * i (t) ≤ 0. Proof. Due to the linearity of the unconstrained optimal control in (38), u * i (t) must be either non-increasing or nondecreasing over all t ∈ [t 0 i , t m i ]. With the terminal time fixed, the last equation in (42)

D. Constrained Optimal Control Analysis
Checking whether the optimal solution of the unconstrained problem P 0 , P 1 or P 2 violates any of the constraints (26) through (30) is easily accomplished since the unconstrained optimal control in (38) is a linear function of time and the optimal speed is a quadratic function of time. When this happens, we must check whether there exists a nonempty feasible control set. One approach followed in earlier work [28] is to identify the set of all initial conditions (t 0 i , v 0 i ) such that no constraint is violated over [t 0 i , t m i ] or at least some of the constraints are not violated while the rest are explicitly dealt with through the Lagrangian in (25). As shown in [28], it is possible to define a Feasibility Enforcement Zone (FEZ) which precedes the CZ such that each CAV is controlled over the FEZ so as to reach a feasible initial condition when reaching the CZ. Here, however, we proceed differently by following a direct approach through which we derive explicit solutions for any feasible optimal constrained trajectory. In so doing, we can also explicitly identify when an optimal solution is infeasible under initial conditions (t 0 i , v 0 i ). When the optimal solution of the unconstrained problem violates a constraint, we need to re-solve the problem by identifying an optimal trajectory that includes unconstrained arcs pieced together with one or more constrained arcs such that all necessary conditions for optimality are satisfied. For a control constraint of the form g i (u i , t) ≤ 0 as in (26)- (27), the optimal control on a constrained arc can be simply obtained by solving g i (u i , t) = 0. The remaining constraints (28)- (30) in our problem are pure state constraints of the form h i (x i , t) ≤ 0. In this case (see [33]), we define the tangency constraints i (x i , t) is the kth time derivative and q derivatives are taken until we obtain an expression that explicitly depends on the control u i so that At the junction points of constrained and unconstrained arcs, the costate and Hamiltonian trajectories may have discontinuities. This can be determined using the following jump conditions [33], where τ denotes a junction point and τ − ,τ + denote the left-hand side and the right-hand side limits, respectively: where N i (x i , t) is the q-dimensional vector in (43) and π i is a q-dimensional vector of constant Lagrange multipliers satisfying π T i N i (x i , t) = 0 and π i ≥ 0, i = 1, . . . , q. Consequently, the optimal control u * i (t) may or may not be continuous at junction points.
In what follows, we concentrate on the safety constraint (3) which is the most challenging to deal with. In this case, The remaining constraints are discussed in the Appendix. Thus, we set h i (p i , t) = p i + δ − p * k (t) where we observe that p * k (t) is a known explicit function of time given by the optimal position trajectory of CAV k specified in (41) or (42) since, upon arrival of CAV i at the CZ, the optimal solution of the problem associated with k < i has already been fully determined. Moreover, h is also an explicit function of time in (41) or (42) and h , hence the optimal control on the constrained arc is given by u * i (t) = u * k (t). The following result establishes the continuity property of the optimal control when the trajectory enters a constrained arc where p i (t) + δ − p * k (t) = 0. Theorem 3. The optimal control u * i (t) is continuous at the junction τ of the unconstrained and safety-constrained arcs, i.e., u * i (τ − ) = u * i (τ + ). Proof. By assumption, the rear-end safety constraint is not active at t 0 i . Hence, when the safety constraint p i (t) + δ − p * k (t) ≤ 0 becomes active, τ is the entry time of the constrained arc, and the jump conditions in (45) become ∂v * k (t) ∂t = u * k (t) are explicit functions of t specified through (41) or (42). We assume that u * k (t), k < i, is continuous in t so that, if we can establish that u * k (t) is continuous, then a simple iterative argument completes the proof. The equations above become For t ≥ τ + , the tangency conditions (43)-(44) with q = 2 hold: In addition, note that the position p i (t) and speed v i (t) are continuous functions of t. Combining the equations above and recalling from (24) Assuming that u i (τ − ) − u i (τ + ) = 0, recall that at τ − the trajectory arc is unconstrained so that (35) holds: u i (τ − ) = −λ v i (τ − ) and it follows that u i (τ − ) − u i (τ + ) = 0. We conclude that u i (t) is continuous at τ and the proof is complete.
Once an optimal trajectory for CAV i enters the constrained arc p i (t) + δ − p * k (t) = 0, it may remain on this arc through the terminal time t m i or exit it at some point τ > τ and follow an unconstrained arc over [τ , t m i ]. This depends on whether such an exit point τ is feasible on an optimal trajectory. More generally, it is possible that an optimal trajectory consists of a sequence of alternating unconstrained and constrained arcs whose feasibility needs to be checked. Thus, once we establish that an optimal trajectory contains a constrained arc, there are two cases to consider.
Case 1: No exit point from the constrained arc. In this case, CAV i remains on the constrained arc until it reaches the MZ and we have The constants a i , b i , c i and d i along with τ are determined through The first two equations above are the same as in (41) and follow from the initial conditions., while (47c) follows from (46) and Theorem 3. In addition, (47d) follows from (47a) when the safety constraint becomes active, i.e., p * i (τ ) + δ − p * k (τ ) = 0, and (47e) follows from v * i (τ ) = v * k (τ ). Note that in this case the terminal time t m i is fixed and determined by CAV s in (18). A typical simulation example of this case is given in Section V (Fig. 5).

Remark 2.
As noted in Section II, an alternative to the distance-based safety constraint p k (t) − p i (t) ≥ δ is the speed-based safety constraint [5] While the expression of the analytical solutions become more complicated, the approach for deriving all necessary conditions is the same as described above.
Case 2: There exists an exit point from the constrained arc. In this case, letting τ 1 denote the entry point to the constrained arc and τ 2 the exit point, there are two subcases to consider: (i) when the terminal time t m i is free, and (ii) when the terminal time is fixed. When the terminal time is free, the transversality condition (33) holds, and a solution is obtained through the system of five equations in (47) with τ 1 replacing τ along with the following equations: The first equation above follows from the fact that [τ 2 , t m i ] is an unconstrained arc so that (38) applies but with new constans e i , r i and from u * i (τ − 2 ) = u * i (τ + 2 ) = u * k (τ 2 ); (48b) follows from the constraint p * i (τ 2 )+δ−p * k (τ 2 ) = 0 and (48c) from v * i (τ 2 ) = v * k (τ 2 ). Next, (48d) is the boundary constraint p * i (t m i ) = L, while (48e) and (48f) are the transversality conditions similar to the last two equations in (41). A simulation example of this case is given in Section V (Fig.  6). In the case where the terminal time t m i is fixed (as in problems P 1 and P 2 ) we simply remove the transversality condition in the last equation above.
Remark 3. Note that a i , b i , c i , d i and τ 1 can be determined from the five equations in (47) independently from e i , r i , q i , m i and τ 2 in (48). Thus, the construction of an optimal trajectory is obtained by solving two sub-problems and piecing the solutions together. This is an important property because it also allows us to easily check for the existence of a feasible solution: if τ 2 < τ 1 then no feasible optimal trajectory exists in this case.

IV. OPTIMAL CONTROL OF CAVS IN THE MZ
As mentioned in Section II-A, the inclusion of left and right turns requires consideration of passenger comfort which is commonly quantified through the jerk J i (t) =u i (t) [34] defined as the time derivative of acceleration. Thus, one approach to minimize passenger discomfort in the MZ is to keep the magnitude of the resultant force, which consists of the centripetal force and the braking force, unchanged. Note that both the magnitude of the centripetal force and the angle between the centripetal and braking forces do not change while the vehicle makes a turn. Hence, the following optimization problem is formulated with the objective of minimizing the L 2 -norm of jerk for each CAV i: (49) and, following the definitions in Section II-A, we have The analytical solution of problem (49) was obtained in [35] using Hamiltonian analysis and considering the jerk as the control input.

A. Joint Minimization of Passenger Discomfort and Energy Consumption
In dealing with turning CAVs in the MZ, we formulate a joint objective expressed as a convex combination of acceleration/deceleration and jerk as follows: given where ρ 1 = w · q 1 , ρ 2 = (1 − w) · q 2 with q 1 , q 2 being the normalization factors which are selected so that q 1 ·u 2 i ∈ [0, 1] and q 2 ·J 2 i ∈ [0, 1], and w ∈ [0, 1] is a weight associated with the importance of energy consumption relative to passenger discomfort. Note that t m i has already been determined by solving the CZ optimal control problem in the previous section, hence also u i (t m i ) and v m i are known. Finally, v f i is also set to a constant v f i = v f for all CAVs, where v f is a predetermined desired exit speed (e.g., v max if we wish to maximize traffic throughput after the intersection). The reason for selecting a common speed is to prevent the chance of collisions that might result when CAVs exit the MZ at different speeds.
Given the objective function in (51), the Hamiltonian is where λ p i , λ v i , λ u i are the costate variables. Note that the collision avoidance inside the MZ has been ensured through (18). The Euler-Lagrange equations associated with the position and speed respectively arė is associated with the acceleration. Since the terminal acceleration/deceleration u i (t f i ) is not pre-specified, we also have the transversality condition The necessary condition for optimality is Note that (53) leads to where a i and b i are constants of integration. Given (54), we have where c i is a constant. We can now obtain a complete analytical solution of (51) as follows.
Theorem 4. The optimal trajectory for (51) is given by where A 1 = ρ1 ρ2 , A 2 = − ρ1 ρ2 , and a i , b i , c i , d i , e i and f i are constants of integration determined through 1 Proof. Combining (58) with (56), we have the ordinary differential equation: whose solution yields the optimal speed (61). Using (61) in the system dynamics, we can then derive (59) using (59). Note that since 0 ≤ w ≤ 1, the optimal solution is only valid when w = 1 and w = 0. When w = 0, the problem becomes (49) with the objective of minimizing jerk only. When w = 1, the problem minimizes energy consumption only. Although the state and control constraints are not incorporated in (51), it is possible that the minimum speed v min and/or maximum deceleration u min constraints become active. The approach to analyze such cases is similar to the analysis in Appendix A.

V. SIMULATION EXAMPLES
We begin with several numerical examples illustrating the different cases discussed in Section III. In terms of computational complexity, we should point out that except for the case where the complete solution is given by the simple system of linear equations (42), solving a system of nonlinear equations with six parameters involved as in (48) is certainly nontrivial. A good initial 'guess' of the parameter values is extremely useful in the convergence of the root-finding algorithm for numerical solvers. To do so, our approach is to start with the unconstrained optimal solution and add the constraints step by step. At each step, when a constraint is added to the problem, we use the solution from the previous step as the initial estimate and obtain the new solution, which is then used as the initial estimate for the problem formulated in the next step; if the current solution satisfies all the constraints, then we have obtained the optimal solution.
Unconstrained optimal control with free terminal time. The parameters used are: L = 400m, γ = 0.1, v 0 i = 10m/s, t 0 i = 0s. The optimal terminal time is obtained as t m i = 32.03s as shown by the blue curves in Fig. 4.
Unconstrained optimal control with fixed terminal time. Assuming t m i = 32.03s violates (18), and we need to formulate problem P 1 by adding t m i = 33s to P 0 . The resulting optimal control, speed, and position trajectories are shown by the red curves in Fig. 4.
Safety-constrained optimal control without exit. Assuming CAV k = 1 enters the CZ at t 0 k = 0 with an initial speed v 0 k = 10m/s and exits at t m k = 32.03s, the optimal control is u * k (t) = −0.0073t + 0.23. Then, we assume that CAV i = 2 enters the CZ at t 0 i = 2s with an initial speed v 0 i = 13m/s.
76s where the minimal safe following distance is δ = 10m. The optimal control for CAV i is as shown in Fig. 5. Note that in this case, CAV i needs to start out by decelerating before entering the constrained arc. Safety-constrained optimal control with exit. Assuming  CAV k = 1 enters the CZ at t 0 k = 0 with an initial speed v 0 k = 10 and exits at t m k = 41s with a terminal speed v m k = 10m/s, the optimal control is u * k (t) = 0.0017t − 0.0357. Then, we assume that CAV i = 2 enters the CZ at t 0 i = 1.5s with an initial speed v 0 i = 12m/s, and the terminal time of CAV i is t m i = 42.5s satisfying t m i > t m k + δ/v m k where the minimal safe following distance is δ = 10m. The optimal control for CAV i is as shown in Fig. 6.

A. Optimal Trajectories in the CZ
The proposed decentralized optimal control framework inside the CZ is illustrated through simulation in MATLAB.  We assumed a single lane for each traffic direction and the parameters used are: L = 400m, S = 30m; the speed constraints are v max = 15m/s and v min = 5m/s; the control constraints are u max = 0.5m/s 2 and u min = −0.5m/s 2 ; S L = 3 8 πS, S R = 1 8 πS, δ = 10 m, and ∆ i = 5, 3, 3s for a left turn, going straight, and a right turn respectively. CAVs arrive at the CZ based on a random arrival process which we assumed to be a Poisson process with rate λ = 1 and the initial speeds are uniformly distributed over [8,12]m/s.
The optimal speed and control trajectories in the CZ are shown in Figs. 7 and 8, with labels indicating the position of the CAV in the FIFO queue and the driving direction. To illustrate cases where the acceleration and speed constraints become active, note that for CAV #4, the optimal trajectory consists of three arcs: (i) a constrained arc starting at t 0 4 = 8.03s where the CAV accelerates at u max until τ 1 = 16.02s, (ii) an unconstrained arc where the CAV accelerates to v max at τ 2 = 27.57s, (iii) a constrained arc where the CAV cruises at v max (u i (t) = 0) until it exits the CZ at t m 4 = 38.09s. For CAV #7, the optimal trajectory consists of two arcs: (i) an unconstrained arc starting at t 0 7 = 14.96s where the CAV keeps accelerating until it reaches v max at τ = 42.38s, (ii) a constrained arc where the CAV cruises at v max (u i (t) = 0) until t m 7 = 44.96s. To illustrate a case where the safety constraint is included, observe that CAV #10 is traveling on the same lane as #9. At τ = 37.85s, #10 enters the constrained arc where p 10 (t)+10−p 9 (t) = 0. It then follows the optimal trajectory of #9, which can be reflected by the slope change in Fig.  8. The inter-vehicle distance between #9 and #10 is shown as a subfigure in Fig. 8. Observe that after #10 enters the constrained arc, it stays constrained until reaching the MZ without exiting. The lower bound of t m 10 happens to be constrained by #9, hence, once #10 enters the constrained arc, there is no incentive for it to exit.
The optimal solution to (22) varies as the weight β changes. To investigate the tradeoff between energy consumption and traffic throughput, we examine a range of cases with different β values and generate the associated Pareto sets illustrating the fact that no objective can be made better off without making at least one other objective worse. In (22), we use u 2 i (t) as a rough approximation of energy consumption, since it adequately captures its monotonic depen- dence on acceleration, while also allowing us to derive the analytical solution. However, to more accurately assess the impact of our controller, we use the polynomial metamodel proposed in [36] which yields vehicle energy consumption as a function of speed and acceleration:ḟ =ḟ cruise +ḟ accel whereḟ cruise = w 0 +w 1 v i (t)+w 2 v 2 i (t)+w 3 v 3 i (t) estimates the energy consumed by a vehicle cruising at a constant ] estimates the additional energy consumed due to acceleration with u i (t). The polynomial coefficients w = [w 0 , w 1 , w 2 , w 3 ] and r = [r 0 , r 1 , r 2 ] are calculated from experimental data. In addition, we use the average travel time inside the CZ, i.e., t m i − t 0 i as a measurement of the traffic throughput. By obtaining all of the optimal solutions to (22) while varying the weight β, we can derive the Pareto sets and the Pareto frontier corresponding to different combinations of fuel consumption and average travel time as shown in Fig.  9. Observe that there exists a tradeoff between minimizing energy consumption and maximizing traffic throughput.
To evaluate the effectiveness of the proposed solution, we carried out a comparison with the baseline scenario using VISSIM, where all the vehicles are assumed to be non-CAVs under the control of traffic lights with fixed switching times. The comparison is shown in Table I, where the weight β in (21), used for trading off energy and throughput, is set to 0.75, 0.5, and 0.25 respectively. When β = 0.5 where energy and throughput are equally weighted, the energy consumption improvement is 13.46%, while the average travel time is improved by 29.84% compared to the baseline scenario. As β varies (see Table I) the resulting tradeoff between travel time and energy changes as expected. Since our objective is to jointly minimize energy consumption and maximize traffic throughput, we also compute the value of the objective function in (21). As shown in Table I, the optimized non-signalized performance is significantly better than the signalized baseline regardless of β values.

B. Optimal Trajectories in the MZ
The proposed decentralized optimal control framework inside the MZ incorporating turns is illustrated through simulation in MATLAB with the same model parameters as in Section V-A. For simulation purposes, we assume the speeds at the entry of the MZ are set to 8m/s for CAVs turning left, 6m/s for CAVs turning right, and 10m/s for CAVs going straight, respectively. The speed at the exit of the MZ is set to v f = 10m/s.  Pareto efficiency sets and frontier corresponding to different combinations of energy consumption and passenger discomfort in the MZ.
The position trajectories of the first 20 CAVs inside the MZ are shown in Fig. 10. CAVs are separated into two groups: those shown above zero are driving from east or west, and those below zero are driving from north or south, with labels indicating the position of the vehicles in the FIFO queue and the driving direction. Observe that CAV #11 belongs to O 12 (t) and no collision would occur between #11 and #12. Hence, they can be traveling inside the MZ at the same time, i.e., t f 12 = t f 11 . Similarly, since CAV #12 belongs to O 13 (t), no collision would occur between #12 and #13 as well. However, recalling the dependence of the terminal conditions on e, s, l, o in (18), #13 is constrained by #10 which may collide with it at the end of the MZ. As a result, #13 has to wait until #10 leaves the MZ for a distance δ, which leads to t f 13 = t f 10 + δ v f

10
. The optimal solution to (51) varies as the weight w changes. Similarly, to illustrate the tradeoff between passenger discomfort and energy consumption, we can examine a range of cases with different weights and generate the associated Pareto sets. In Fig. 11, we use u 2 i (t) and J 2 i (t) as rough approximations of energy consumption and passenger discomfort respectively. Clearly, there is a tradeoff between these two metrics as the vehicle turns within the intersection.

VI. CONCLUSIONS AND FUTURE WORK
We have extended earlier work in [26] and [27] where a decentralized optimal control framework was established for optimally controlling CAVs crossing an urban intersection without considering turns. In this paper, we have included left and right turns and considered a new optimal control problem formulation where the tradeoff between energy and travel time is explicitly quantified for CAVs in the CZ and all safety constraints are incorporated. Despite the added complexity of turns, we have shown that the optimal solution can still be obtained in decentralized fashion, with each CAV requiring information from a subset of other CAVs. This enables the on-line solution to be obtained by on-board computation resources for each individual CAV. In addition, we have formulated another optimization problem with the objective of minimizing passenger discomfort while the vehicle turns at the MZ of the intersection, and investigated the tradeoff between minimizing energy consumption and passenger discomfort. Ongoing research is exploring the effect of partial CAV penetration in mixed traffic situations where both CAVs and human-driven vehicles share the the road [37]. Future work will investigate the effect of errors and/or communication delays to the system behavior as well as the coupling between multiple intersections, and the possibility of extending the resequencing approach in [31] to potentially improve overall traffic throughput.

APPENDIX A CONSTRAINED OPTIMAL CONTROL ANALYSIS
In this Appendix, we discuss the effect of the control and state constraints not included in our analysis of Section III-B. Clearly, there are a number of possible situations that may arise, including the possibility of both the state constraint v i (t)−v max ≤ 0 and the control constraint u i (t)−u max ≤ 0 becoming active (e.g., CAV #4 in Figs. 7 and 8). The analysis provided here is limited to the state constraint v i (t)−v max ≤ 0 or the control constraint u i (t) − u max ≤ 0 each becoming active on its own. These basic cases serve as "building blocks" to handle situations of multiple constraints becoming active when this is feasible.
A. State constraint v i (t) − v max ≤ 0 active only When the state constraint v i (t)−v max ≤ 0 becomes active at τ , the jump conditions in (45) become Thus, we have H i (τ − ) = H(τ + ) and λ p i (τ − ) = λ p i (τ + ). Based on this fact, we have the following result whose proof is similar to that of Theorem 3.
Theorem 5. The optimal control u i (t) is continuous at the junction τ of the unconstrained and v max -constrained arcs, i.e., u * i (τ − ) = u * i (τ + ).
For this case, CAV i remains on the constrained arc until it reaches the MZ and we have where τ is the entry point of the constrained arc v i (t) − v max = 0, and, due to Theorem 5, we also have a i τ +b i = 0. Combined with the boundary conditions and the transversality conditions (33), we have the following conditions where a i , b i , c i , d i , τ and t m i are obtained by solving these six equations. The first two immediately follow from the initial conditions, while (66c) follows from p i (τ ) + v max · (t m i − τ ) = L, (66d) follows from v i (τ ) = v max , and (66e) follows from the fact that u i (τ − ) = u i (τ + ) = 0. The last equation (66f) follows from the continuity of the Hamiltonian and the transversality condition H i (t m i ) = 0. Note that in this case we assumed a free terminal time t m i . If t m i is fixed, then we simply remove the transversality condition (66f). Simulation example of this case arises in Figs. 7 and 8, e.g., CAV #7.
B. Control constraint u i (t) − u max ≤ 0 active only When the control constraint u i (t) − u max ≤ 0 is active at τ , both the Hamiltonian and the costates are continuous according to (45). For this case, we have Combined with the boundary conditions and the transversality conditions (33), we have the following conditions where a i , b i , c i , d i , τ and t m i are obtained by solving these equations. The first two equations (68a) and (68b) follow from the fact that p i (τ − ) = p i (τ + ) and v i (τ − ) = v i (τ + ), (68c) follows from the fact that u i (τ − ) = u i (τ + ) = u max , (68d) follows from the terminal condition, and the last two equations follow from the transversality conditions. In this case, we assume a free terminal time t m i . If t m i is fixed, then we simply remove the last transversality condition (68f).