Optimal Time Trajectory and Coordination for Connected and Automated Vehicles
Andreas A. Malikopoulos Logan E. Beaver Ioannis Vasileios Chremos
math.OC






 

[footnoteinfo]This research was supported in part by ARPAE's NEXTCAR program under the award number DE-AR0000796 and by the Delaware Energy Institute (DEI).
Paestum]Andreas A. Malikopoulosandreas@udel.edu,    
Paestum]Logan Beaverlebeaver@udel.edu,               
Paestum]Ioannis Vasileios Chremosichremos@udel.edu  
[Paestum]Department of Mechanical Engineering, University of Delaware, 126 Spencer Lab, 130 Academy Street, Newark, DE, 19716, USA  



                           
Connected and automated vehicles, cyber-physical systems, emerging mobility, decentralized optimal control, autonomous intersections, path planning.
                             


                          
In this paper, we provide a decentralized theoretical framework for coordination of connected and automated vehicles (CAVs) at different traffic scenarios. The framework includes: (1) an upper-level optimization that yields for each CAV its optimal time trajectory and lane to pass through a given traffic scenario while alleviating congestion; and (2) a low-level optimization that yields for each CAV its optimal control input (acceleration/deceleration). We provide a complete, analytical solution of the low-level optimization problem that includes the rear-end, speed-dependent safety constraint. 
Furthermore, we provide a problem formulation for the upper-level optimization in which there is no duality gap. The latter implies that the optimal time trajectory for each CAV does not activate any of the state, control, and safety constraints of the low-level optimization, thus allowing for online implementation. Finally, we present a geometric duality framework with hyperplanes to derive the condition under which the optimal solution of the upper-level optimization always exists. We validate the effectiveness of the proposed theoretical framework through simulation.





§ INTRODUCTION



Emerging mobility systems, e.g., connected and automated vehicles (CAVs), shared mobility, provide the most intriguing opportunity for enabling users to better monitor transportation network conditions and make better operating decisions to improve safety and reduce pollution, energy consumption, and travel delays; see <cit.>. Emerging mobility systems are typical cyber-physical systems where the cyber component (e.g., data and shared information through vehicle-to-vehicle and vehicle-to-infrastructure communication) can aim at optimally controlling the physical entities (e.g., CAVs, non-CAVs); see <cit.>. The cyber-physical nature of such systems is associated with significant control challenges and gives rise to a new level of complexity in modeling and control; see <cit.>. As we move to increasingly complex emerging mobility systems, new control approaches are needed to optimize the impact on system behavior of the interplay between vehicles at different traffic scenarios.  

<cit.> provided the key features of an automated mobility system along with a control system architecture. An automated mobility system can alleviate congestion, reduce energy use and emissions, and improve safety by increasing significantly traffic flow as a result of closer packing of automatically controlled vehicles in platoons. Forming platoons of vehicles traveling at high speed was a popular system-level approach to address traffic congestion that gained momentum in the 1980s and 1990s; see <cit.>. Addressing string stability of platoons, see <cit.>, has been a technical challenge before demonstrating their significant benefit; see <cit.>. Ramp metering has been another common approach used to regulate the flow of vehicles merging into freeways to decrease traffic congestion; see <cit.>. One of the very early efforts in this direction was proposed by <cit.> for safe and efficient coordination of merging maneuvers with the intention of avoiding congestion. Assuming a given merging sequence, Athans formulated the merging problem as a linear optimal regulator, proposed by <cit.>, to control a single string of vehicles, with the aim of minimizing the speed errors that will affect the desired headway between each consecutive pair of vehicles.  



 §.§ Related Work

In a typical commute, we encounter traffic scenarios that include merging at roadways and roundabouts, crossing intersections, cruising in congested traffic, passing through speed reduction zones, and lane-merging or passing maneuvers. These scenarios, along with the driver responses to various disturbances, contribute to traffic congestion. Several research efforts have been reported in the literature towards developing control algorithms for coordinating CAVs at  such  traffic scenarios to alleviate congestion. <cit.> proposed the use of the reservation scheme to control a single intersection of two roads with vehicles traveling with similar speed on a single direction on each road, i.e., no turns are allowed. In their approach, each vehicle is treated as a driver agent who requests the reservation of the space-time cells to cross the intersection at a particular time interval defined from the estimated arrival time to the intersection. 
Since then, numerous approaches have been reported in the literature to achieve safe and efficient control of traffic through intersections; see <cit.>. Some efforts have proposed model predictive control that allows each vehicle to optimize its movement locally in a distributed manner; see <cit.>. Other research efforts have employed scheduling theory based on which the vehicles can make a decision about the appropriate schedule of crossing an intersection; see <cit.>. <cit.> constructed the invariant set for the control inputs that ensure lateral collision avoidance. There has been also some work focusing on multi-objective optimization problems for intersection coordination, mostly solved as a receding horizon control problem; see <cit.>. More recently, a study by <cit.> indicated that transitioning from intersections with traffic lights to autonomous intersections, where vehicles can coordinate and cross the intersection without the use of traffic lights, has the potential of doubling capacity and reducing delays.

In prior work, we presented a decentralized optimal control framework for coordinating online CAVs in different traffic scenarios, e.g., at merging roadways, intersections, adjacent intersections, speed reduction zones, roundabouts, and corridors; see <cit.>. 

The framework provides a closed-form analytical solution that exists under certain conditions, see <cit.>, and which, based on Hamiltonian analysis, yields for each CAV the optimal acceleration/deceleration at any time in the sense of minimizing fuel consumption. The solution allows the CAVs to coordinate and pass through these traffic scenarios without creating congestion and under the hard safety constraint of collision avoidance. Similar control approaches have considered passengers' comfort in addition to alleviating congestion; see <cit.>.
A detailed discussion of the research efforts that have been reported in the literature to date in this area can be found in <cit.> and <cit.>. 



 §.§ Contributions of This Paper

In this paper, we provide a decentralized theoretical framework for coordination of CAVs in different traffic scenarios that  include merging at roadways and roundabouts, crossing intersections, cruising in congested traffic, passing through speed reduction zones, and lane-merging or passing maneuvers. The framework includes a two-level joint optimization: (I) an upper-level optimization that yields for each CAV its optimal time trajectory and appropriate lane, to pass through a traffic scenario while alleviating congestion, and (II) a low-level optimization that yields for each CAV its optimal control input (acceleration/deceleration) subject to the state, control, and safety constraints.

The contributions of this paper are: (1) a complete, analytical solution of the low-level optimization problem that includes the rear-end safety constraint, where the safe distance is a function of speed; (2) a problem formulation for the upper-level optimization in which there is no duality gap, implying that the optimal time trajectory for  each CAV does not activate any of the state, control, and safety constraints of the low-level optimization, thus allowing for online implementation; (3) a geometric duality framework with hyperplanes to derive the condition under which the solution of the upper-level optimization always exists.
A limited-scope analysis of the low-level optimization was presented in <cit.>, where we formulated the problem with the rear-end, speed-dependent safety constraint without providing the complete analysis and technical details related to the different possible activation of the constrained arcs though. A preliminary formulation of the upper-level optimization was discussed in <cit.>, where we introduced the idea of deriving the optimal time trajectories of CAVs.


The proposed framework advances the state of the art in the following ways. First, in contrast to other efforts reported in the literature, see <cit.>, where either the safety constraint was not considered,

or considered using a constant safety distance, see <cit.>, in our framework, the low-level analytical solution considers the safety distance between the CAVs to be a function of speed leading to a complicated, yet very interesting, analysis. Moreover, we augment the double integrator model representing a CAV with an additional state corresponding to the distance from its preceding CAV, thus we are able to address the lateral collision constraint in the low-level optimization. Second, in several efforts reported in the literature to date, the upper-level optimization either (a) was implemented with centralized approaches; see <cit.>; or (b) was considered known through a given protocol <cit.>; or (c) was implemented using a strict first-in-first-out queueing structure; see <cit.>. In our proposed framework, the upper-level optimization yields, in a decentralized fashion, the optimal time for each CAV to pass a given traffic scenario along with the appropriate lane that needs to occupy. Finally, in contrast to the research efforts reported in the literature to date, the solution of the upper-level optimization allows CAVs to change lanes.



 §.§ Organization of This Paper

The structure of the paper is organized as follows. In Section 2, we provide the modeling framework and our assumptions. In Section 3, we formulate the low-level optimization problem and derive the analytical solution. In Section 4, we formulate the upper-level optimization problem and prove that it imposes no duality gap. In Section 5, we validate the effectiveness of the proposed theoretical framework through simulation. Finally, we provide concluding remarks and discuss potential directions for future research in Section 6.



§ MODELING FRAMEWORK

Although the theoretical framework presented in this paper can be applied to any  traffic scenario, e.g.,  merging at roadways and roundabouts, cruising in congested traffic, passing through speed reduction zones, and lane-merging or passing maneuvers, we use an intersection as a reference to provide the fundamental ideas and results. This is because an intersection has  unique features which makes it technically more challenging compared to other traffic scenarios. However, our analysis and results can be applied to other traffic scenarios too.

We consider CAVs at a 100% penetration rate crossing a signal-free intersection (Fig. <ref>). The region at the center of the intersection, called merging zone, is the area of potential lateral collision of  CAVs. The intersection has a control zone inside of which the CAVs can communicate with each other and with a crossing protocol. The  crossing protocol, defined formally in the next subsection, stores the CAVs' time trajectories from the time they enter until the time they exit the control zone. The distance from the entry of  the control zone until the entry of the merging zone is S_c and, although it is not restrictive, we consider to be the same for all entry points of the control zone. We also consider the merging zone to be a square of side S_m (Fig. <ref>). Note that  S_c could be in the order of hundreds of meters depending on the CAVs' communication range capability, while S_m is the length of a typical intersection. The CAVs crossing the intersection can also make a right turn of radius R_r, or a left turn of radius R_l (Fig. <ref>). The aforementioned values of the intersection's geometry are not restrictive in our modeling framework, and are used only to determine the total distance traveled by each CAV inside the control zone.



Let 𝒩(t)={1,…,N(t)}, N(t)∈ℕ, be the set of CAVs inside the control zone at time t∈ℝ^+. Let t_i^f be the time for CAV i to exit the control zone.
There is a number of ways to determine t_i^f for each CAV i. For example, we may
impose a strict first-in-first-out queuing structure, see <cit.>, where each CAV must
enter the merging zone in the same order it entered the control zone. The policy, which  determines the time t_i^f that each CAV i exits the control zone,  is the
result of an upper-level optimization problem which can aim at maximizing the throughput at the intersection. On the other hand, deriving the optimal control input (minimum acceleration/deceleration) for each CAV i∈𝒩(t) from the time t_i^0 it enters the control zone until the target t_i^f is the result of a low-level optimization problem that can aim at minimizing the energy of each individual CAV.

In what follows, we present a two-level, joint optimization framework: (1) an upper-level optimization that yields for each CAV i∈𝒩(t), with a given origin (entry of the control zone) and desired destination (exit of the control zone), (a) the minimum time t_i^f to exit the control zone and (b) optimal path including the lanes that each CAV should be occupying while traveling inside the control zone; and (2) a low-level optimization that yields, for CAV i∈𝒩(t), its optimal control input (acceleration/deceleration) to achieve the optimal path and t_i^f derived in (1) subject to the state, control, and safety constraints.
The two-level optimization framework is executed by each CAV i∈𝒩(t) as follows. When a CAV i enters the control zone at t_i^0, it accesses the crossing protocol that includes the time trajectories, defined formally in the next subsection, of each CAV cruising inside the control zone. If two, or more, CAVs enter the control zone simultaneously, then the crossing protocol decides arbitrarily the sequence that each of these CAVs will get access to the information.  Then, the CAV solves the upper-level optimization problem and derives the minimum time t_i^f to exit the control zone along with the appropriate lanes that should occupy. The minimum time t_i^f of the upper-level optimization problem is the input of the low-level optimization problem. 

The implications of the proposed optimization framework are that CAVs do not have to come to a full stop at the intersection, thereby conserving momentum and fuel while also improving travel time. Moreover, by optimizing each CAV's acceleration/deceleration, we minimize transient engine operation, and thus we have additional benefits in fuel consumption. In our analysis, we consider that each CAV i∈𝒩(t) is governed by the following dynamics

    ṗ_i(t)      =v_i(t), 
    v̇_i(t)      =u_i(t), 
    ṡ_i(t)      = ξ_i · (v_k(t)-v_i(t)),    t∈[t_i^0, t_i^f],

where t_i^0 and t_i^f correspond to the times that CAV i enters and exits the control zone, respectively; p_i(t)∈𝒫_i is the position of each CAV i from the entry until the exit of the control zone at t; v_i(t)∈𝒱_i and u_i(t)∈𝒰_i are the speed and
acceleration/deceleration (control input) of each CAV i inside the control zone at t;  s_i(t)∈𝒮_i denotes the distance of CAV i from CAV k which is physically located  ahead of i (e.g., k either cruising on the same lane as i, or crossing the merging zone and can cause lateral collision with i – in the latter we have ṡ_i(t) = -ξ_i · v_i(t)), and ξ_i is a reaction constant of CAV i. The sets 𝒫_i, 𝒱_i,  𝒰_i, and 𝒮_i, i∈𝒩(t), are complete and totally bounded subsets of ℝ. Let x_i(t)=[p_i(t)   v_i(t)   s_i(t)]  ^T denote the state of each CAV i taking values in 𝒳_i
=𝒫_i×𝒱_i×𝒮_i, with initial value
x_i(t_i^0)=x_i^0=[p_i^0  v_i^0 s_i^0]  ^T, where p_i^0= p_i(t_i^0)=0, v_i^0= v_i(t_i^0), and s_i^0= s_i(t_i^0) at the entry of the control zone.  The state space 
𝒳_i for each CAV i is
closed with respect to the induced topology on 𝒫_i×𝒱_i×𝒮_i and thus, it is compact.

To ensure that the control input and CAV speed are within a
given admissible range,  we impose the following constraints

    u_i,min≤ u_i(t)≤ u_i,max,   and
    
    0 < v_min≤ v_i(t)≤ v_max,  ∀ t∈ t_i
    ^0,t_i^f],

where u_i,min, u_i,max are the minimum  and maximum control input for each CAV i∈𝒩(t), and v_min, v_max are the minimum and maximum speed limits, respectively.
To ensure the absence of rear-end collision of two CAVs traveling on the same lane,  the position of the preceding CAV should be greater than, or equal to the position of the following CAV plus a minimum safe distance δ_i(t), which is a function of speed v_i(t), i.e., δ_i(t)=δ̅ + ρ_i · v_i(t), where δ̅ is the standstill distance and ρ_i is the minimum headway that CAV i  maintains while following the preceding CAV. 
The rear-end safety constraint is

    s_i(t)=ξ_i · (p_k(t)-p_i(t)) ≥δ_i(t) ,  t∈ [t_i^0, t_i^f],

where k∈𝒩(t) is some CAV which is physically ahead of i. 
Similarly, a lateral collision inside the merging zone can occur between CAV i and a CAV k∈𝒩(t) which crosses the zone from a different direction than i. In this case, (<ref>) becomes 

    s_i(t)=ξ_i · (p_k,i-p_i(t)) ≥δ_i(t),   t∈ [t_i^0, t_i^e],

where p_k,i is the (constant) distance of CAV k from the entry point that CAV i entered the control zone, and t_i^e is the time that  CAV i exits the merging zone.





	
	The set of all lanes within the control zone is 
	 ℒ:={1,…,M}, M∈ℕ.


Note that the length of each lane θ∈ℒ is 2S_c+S_m  (Fig. <ref>).




 
	For each CAV i∈𝒩(t), we define the function l_i(t): [t_i^0, t_i^f]→ℒ which yields the lane θ∈ℒ that i occupies at time t.







 
	The cardinal points that each CAV i∈𝒩(t) enters and exits the control zone is denoted by o_i.

For instance, based on Definition <ref>, for a CAV i that enters the control zone from the West entry  (Fig. <ref>) and exits the control zone from the South, we have o_i=(W,S).




 
	For each CAV i∈𝒩(t), the set C_o_i includes the lanes in ℒ that can be used on a given o_i, i.e.,
		
    C_o_i:={θ∈ℒ |  o_i  is feasible, ∀ i∈𝒩(t) }.








	The occupancy set, O_θ, of each lane θ∈ℒ includes the time intervals that θ is occupied, i.e.,
	
    O_θ:={[t_i^n_1, t_i^n_2]⊂ℝ_≥ 0, ∀ i∈𝒩(t),   t_i^n_1, t_i^n_2∈ [t_i^0, t_i^f], 
     n_1, n_2∈ℕ, n_2>n_1 |  l_i(t)=θ, ∀ t∈ [t_i^n_1, t_i^n_2]}.








	For each CAV i∈𝒩(t), the function t_p_i(p_i):𝒫_i→[t_i^0, t_i^f], is the time trajectory of i, which yields the time that i is at the position p_i inside the control zone. 








    The crossing protocol is denoted by ℐ(t) and includes the following information
	
    ℐ(t):={t_p_i(p_i),o_i, C_o_i, O_θ, t_i^0, t_i^f}, 
    ∀ i∈𝒩(t),  ∀ l∈ℒ,  t∈[t_i^0, t_i^f].







 
	 The CAVs traveling inside the control zone can change lanes either (1) in the lateral direction (e.g., move to a neighbor lane), or (2) when making a right (or a left) turn inside the merging zone. In the former case, when a CAV changes lane, it travels along the hypotenuse dy of a triangle created by the width of the lane and the longitudinal displacement dp if it had not changed lane. Thus, in this case, the CAV travels an additional distance which is equal to the difference between the hypotenuse dy and the longitudinal displacement dp, i.e., dy-dp.






 
	 When a CAV is about to make a right turn it must occupy the right lane of the road before it enters the merging zone. Similarly, when a CAV is about to make a left turn it must occupy the left lane before it enters the merging zone.


In the modeling framework presented above, we impose the following assumptions:





  
	The CAV's additional distance traveled when it changes neighbor lanes is neglected. 






 
	 Each CAV i∈𝒩(t) has proximity sensors and can communicate with other CAVs and the crossing protocol without any errors or delays.






 
	For each CAV i, none of the constraints  (<ref>)-(<ref>) is active at t_i^0.


The first assumption can be justified by the general observation that the additional distance traveled by a CAV when it changes neighbor lanes is very small compared to the total distance traveled within the control zone. However, by including a two-dimensional vehicle model in our analysis, this additional distance could be taken into account, and thus we believe that this assumption does not provide any restrictions in our exposition. The second assumption may be strong, but it is relatively straightforward to relax as long as the noise in the communication, measurements, and/or delays is bounded. In this case, we can determine upper bounds on the state uncertainties as a result of sensing or communication errors and delays, and incorporate these into more conservative safety constraints. Finally, the last assumption  ensures that the initial state is feasible. This is a reasonable assumption since  CAVs are automated, and so there is no compelling reason for them to activate any of the constraints by the time they enter the control zone.

When each CAV i, with a given o_i, i.e., a cardinal entry of the control zone and a desired cardinal destination (exit of the control zone), enters the control zone, it accesses the  crossing protocol and solves two optimization problems: (1) an upper-level optimization problem, the solution of which yields its time trajectory t_p_i(p_i) and the minimum time t_i^f to exit the control zone; and (2) a low-level optimization problem the solution of which yields its optimal control input (acceleration/deceleration) to achieve the optimal path and t_i^f derived in (1) subject to the state, control, and safety constraints.

We start our exposition with the low-level optimization problem and then we discuss the upper-level problem.








§ LOW-LEVEL OPTIMIZATION

In this section, we consider that the solution of the upper-level optimization problem is given, and thus, the minimum time t_i^f for each CAV i∈𝒩(t) is known. We focus on a low-level optimization problem that yields for each CAV i the optimal control input (acceleration/deceleration) to achieve the minimum time t_i^f subject to the state, control, and safety constraints.

 
If t_i^f is determined, the low-level optimal control problem for each CAV i∈𝒩(t) is to minimize the cost functional J_i(u(t)), which is the L^2-norm of the control input in [t_i^0, t_i^f], i.e.,

    min_u_i(t)∈𝒰_i J_i(u_i(t)) = 1/2∫_t^0_i^t^f_i u^2_i(t) dt,
    subject to: (<ref>),(<ref>),(<ref>), (<ref>), (<ref>),
    and given t_i^0, v_i^0,  p_i(t_i^0), t_i^f,
    p_i(t_i^f),

where p_i(t_i^0)=0, while p_i(t_i^f)=p_i^f, depends on o_i and, based on Assumption <ref>, can take the following values (Fig. <ref>): (1) p_i^f=2 S_c + S_m, if CAV i crosses the merging zone, (2) p_i^f=2 S_c + π R_r/2, if  CAV i makes a right turn at the merging zone, and (3) p_i^f=2 S_c + π R_l/2, if  CAV i makes a left turn at the merging zone. By minimizing the L^2-norm of the control input (acceleration/deceleration), essentially we minimize transient engine, if the CAV is a conventional vehicle, and thus we have direct benefits in fuel consumption; see <cit.>.



Let S_i(t,x_i(t)) be the vector of the constraints in Problem 1 which does not explicitly depend on u_i(t), see <cit.>, i.e.,

    S_i(t,x_i(t)) = 
    [                          v_i(t) - v_max;                          v_min - v_i(t); δ̅ +ρ_i· v_i(t) - ξ_i·(p_k(t) - p_i(t)) ].

We take successive total time derivatives of (<ref>) until we obtain an expression that is explicitly dependent on u_i(t). If n∈ℕ time derivatives are required, then the nth total time derivative of S_i(t,x_i(t)) becomes the arc constraint in our analysis in t∈ [t_i^0, t_i^f], while the remaining n-1 components of S_i(t,x_i(t)) constitute a boundary condition  at the entry (or exit) of the constrained arc. Since Ṡ_i(t,x_i(t)) is an explicit function of u_i(t),  the Hamiltonian for Problem <ref> is


    H_i(t, p_i(t), v_i(t), s_i(t), u_i(t))  
    
    =1/2 u_i(t)^2 + λ^p_i· v_i(t) + λ^v_i· u_i(t) +λ^s_i·ξ_i · (v_k(t) - v_i(t)) 
    
    + μ^a_i·(u_i(t) - u_max)+ μ^b_i·(u_min 
    - u_i(t)) + μ^c_i·  u_i(t) 
     - μ^d_i· u_i(t)  
    + μ^s_i·(ρ_i · u_i(t) - ξ_i(v_k(t) - v_i(t))) ,

with S_i(t_1,x_i(t_1))=0, at the entry t_1 ∈ [t_i^0, t_i^f] of the constrained arc; λ^p_i, λ^v_i, and λ^s_i are the influence functions, see <cit.>, and μ_i=[μ^a_i μ^b_i μ^c_i μ^d_i μ^s_i] ^ T is the vector of the Lagrange multipliers with


    μ^a_i = {[  >0, if ,;  =0, if , ].


    μ^b_i = {[  >0, if ,;  =0, if , ].


    μ^c_i = {[  >0, if ,;  =0, if , ].


    μ^d_i = {[  >0, if ,;  =0, if , ].


    μ^s_i = {[  >0, if ,;  =0, if . ].

For each i∈𝒩(t), the Euler-Lagrange equations are

    λ̇^p_i(t) = - ∂ H_i/∂ p_i = 0, 
    λ̇^v_i(t) = - ∂ H_i/∂ v_i = - (λ^p_i - λ^s_i·ξ_i + μ^s_i ·ξ_i),  
    λ̇^s_i(t) = - ∂ H_i/∂ s_i = 0,


    ∂ H_i/∂ u_i = u_i(t) + λ
    ^v_i + μ^a_i - μ^b_i + μ^c_i - μ^d_i + μ^s_i ρ_i = 0,

with boundary conditions 

    p_i(t_i^0) = p_i^0,   v_i(t_i^0) = v_i^0,   s_i(t_i^0) = s_i^0,
    
    p_i(t_i^f) = p_i^f,  λ^v_i(t_i^f) = 0,  λ^s_i(t_i^f) = 0,

where λ^v_i(t_i^f)=λ^s_i(t_i^f) = 0 since the states v_i(t_i^f) and s_i(t_i^f) are not prescribed at t_i^f, see <cit.>.
From (<ref>) and (<ref>), we have λ^p_i(t) = α_i and λ^s_i(t) = β_i, where α_i and β_i are constants of integration. 

To address this problem, constrained and unconstrained arcs are pieced together to satisfy the Euler-Lagrange equations. The optimal solution is the result of different combinations of the following possible arcs.







 §.§ State and Control Constraints are not Active


In this case, we have 
μ^a_i = μ^b_i= μ^c_i=μ^d_i=μ^s_i=0.
From (<ref>), the optimal control is 

    u_i^*(t) + λ^v_i= 0,    i ∈𝒩(t).

Since λ^p_i(t) = α_i,   
λ^s_i(t)= β_i, setting a_i=α_i - β_iξ_i, from (<ref>) we have

    λ^v_i(t) = -(a_i· t + c_i),
 
where c_i is a constant of integration for each i∈𝒩(t). Thus, from (<ref>)
the optimal control input (acceleration/deceleration) is given by

    u^*_i(t) = a_i· t + c_i,   t ∈ [t^0_i, t_i^f].

Substituting the last equation into (<ref>), we derive the optimal speed and position for each i∈𝒩(t),
namely

    v^*_i(t) = 1/2 a_i · t^2 + c_i· t +d_i,   t ∈ [t^0_i, t_i^f], 
    
    p^*_i(t) = 1/6 a_i · t^3 +1/2 c_i· t^2 + d_i· t +e_i,   t ∈ [t^0_i, t_i^f],

where d_i and e_i are constants of integration. The constants of integration a_i, c_i, d_i, and e_i can be computed using the boundary conditions (<ref>).








 §.§ The State s_i(t) Constraint  Becomes Active


Suppose CAV i∈𝒩(t) starts from a feasible state and control at t=t_i^0, and at some time t=t_1≤ t_i^f, s_i(t_1)=δ(t_1), while v_min < v_i(t_1) < v_max and u_i,min < u_i(t_1) < u_i,max. In this case, μ_i^s≠ 0. 
Let N_i(t,x_i(t))= δ̅ + ρ_i v_i^*(t) - ξ_ip_k^*(t)+ξ_ip_i^*(t). Then, we have

    N_i(t_1,x_i(t_1))=  δ̅ + ρ_i v_i^*(t_1) - ξ_i p_k^*(t_1) + ξ_i p_i^*(t_1) = 0,

which represents a tangency constraint for the state s_i(t) in t∈ [t_1, t_2], where t_2 is the exit point of the constrained arc s_i(t)-δ(t)≤ 0.
Since N_i(t_1,x_i(t_1))=0, then Ṅ_̇i̇(t_1,x_i(t_1))=0,



hence, the value of the optimal control at t=t_1^+ is given by

    u_i^*(t_1^+) = ξ_i (v_k^*(t_1^+) - v_i^*(t_1^+))/ρ_i.


From (<ref>), we note that the optimal control input may not be continuous at t_1, hence the junction point at t_1 may be a corner; see <cit.>. The interior boundary conditions at t_1 for the influence functions are







    λ_i^p(t_1^-)    = λ_i^p(t_1^+) + π_i ∂N_i(t_1,x_i(t_1))/∂ p_i = λ_i^p(t_1^+) + π_i  ξ_i, 
    λ_i^v(t_1^-)    = λ_i^v(t_1^+) + π_i ∂N_i(t_1,x_i(t_1))/∂ v_i = λ_i^v(t_1^+) + π_i  ρ_i, 
    λ_i^s(t_1^-)    = λ_i^s(t_1^+) + π_i  ∂N_i(t_1,x_i(t_1))/∂ s_i  = λ_i^s(t_1^+)-π_i.

The Hamiltonian at t_1 is

    H_i(t_1^-)=H_i(t_1^+)- π_i ∂N_i(t_1,x_i(t_1))/∂ t_1,

or 

    1/2u_i^2(t_1^-)+ λ_i^p(t_1^-) v_i(t_1^-) + λ_i^v(t_1^-) u_i(t_1^-) 
    
    + λ_i^s(t_1^-) ξ_i (v_k(t_1^-)-v_i(t_1^-)) 
    = 1/2u_i^2(t_1^+)+ λ_i^p(t_1^+) v_i(t_1^+) 
    
    + λ_i^v(t_1^+) u_i(t_1^+) + λ_i^s(t_1^+) ξ_i (v_k(t_1^+)-v_i(t_1^+)) 
    + π_i ξ_i v_k(t_1),

where π_i is a Lagrange multiplier constant. The influence functions, λ_i^T(t_1^+)=[λ_i^p(t_1^+)  λ_i^v(t_1^+)  λ_i^s(t_1^+)]^T at t_1^+, the time  t_1, and the Lagrange multiplier π_i constitute 3+1+1 quantities that are determined so as to satisfy (<ref>), (<ref>) - (<ref>) and (<ref>). The unconstrained and constrained arcs are pieced together to determine the 3+1+1 quantities above along with the constants of integration in (<ref>)-(<ref>).

Since (<ref>) holds for all t∈[t_1,t_2], where t_2≤ t_i^f is the exit point of the  constrained arc δ_i(t)-s_i(t) ≤ 0, the optimal control of CAV i∈𝒩(t) is

    u_i^*(t^+) = ξ_i (v_k^*(t^-) - v_i^*(t^-))/ρ_i,  t∈[t_1,t_2].

 
	The exit point t_2 of the constrained arc, δ_i(t)-s_i(t) ≤ 0, can either lead to the unconstrained arc or to other constrained arcs.

If the exit point t_2 leads to the unconstrained arc, then for all  t∈[t_2, t_i^f], we have a set of equations as in (<ref>) - (<ref>) for the optimal control, speed, and position of CAV i, i.e.,  u^*_i(t) = a_i'· t + c_i', v^*_i(t) = 1/2 a_i' · t^2 + c_i' · t +d_i', and p^*_i(t) = 1/6 a_i' · t^3 +1/2 c_i' · t^2 + d_i'· t +e_i', where a_i', c_i', d_i', and e_i', are constants of integration that can be computed along with t_2 from the boundary conditions (<ref>) and the following interior constraints: v_i^*(t_2^-) = v_i^*(t_2^+), p_i^*(t_2^-) = p_i^*(t_2^+), λ_i^p(t_2^-)=λ_i^p(t_2^+), λ_i^v(t_2^-)=λ_i^v(t_2^+), λ_i^s(t_2^-)=λ_i^s(t_2^+), and H_i(t_2^-) = H_i(t_2^+).

If the exit point t_2 does not lead to the unconstrained arc, then we have the following three potential cases to consider: (1)  the speed, v_k(t), of the preceding CAV k is decreasing, (2)  the speed, v_k(t), of the preceding CAV k is either increasing or constant, and (3)  CAV k is  cruising on a different road inside the merging zone and can cause lateral collision with CAV i.


Case 1: The speed, v_k(t), of the preceding CAV k is decreasing.

Let CAV i be in the constrained arc δ_i(t)-s_i(t) ≤ 0 while the speed, v_k(t), of the preceding CAV k is decreasing. Then the following subcases can occur:	(a) u_i^*(t)=u_i,min, for all t∈[t_2,t_i^f], (b) u_i^*(t)=u_i,min, for all t∈[t_2,t_3], and v_i^*(t)= v_min for all t∈[t_3,t_i^f], where t_3 is another junction point, and (c) v_i^*(t)= v_min for all t∈[t_2,t_i^f].

Subcase (a): 
u_i^*(t)=u_i,min, for all t∈[t_2, t_i^f].
By integrating u_i^*(t)=u_i,min, we have v_i^*(t) = u_i,min· t + h_i and p_i^*(t) = u_i,min·t^2/2 + h_i t + q_i, for all t∈[t_2,t_i^f],
where h_i and q_i are constants of integration. To compute t_2 and the constant of integration h_i and q_i, we piece together this  arc with the prior unconstrained and constrained arcs with the following additional interior constraints and boundary conditions: p_i^*(t_2^-) = p_i^*(t_2^+), v_i^*(t_2^-) = v_i^*(t_2^+), s_i^*(t_2^-) = s_i^*(t_2^+),  and p_i^*(t_i^f) = p_i^f,
from which we can compute t_2 and the constants of integration h_i and q_i.

Subcase (b): u_i(t)=u_i,min, for all t∈[t_2,t_3], and v_i(t)= v_min for all t∈[t_3,t_i^f].

In this subcase, at the junction point t_3, CAV i exits the constrained arc, u_i,min-u_i(t) ≤ 0, and enters the arc v_min - v_i(t) ≤ 0, then it follows that u_i^*(t) = 0, for all t∈[t_3, t_i^f], and the optimal speed and position of i are v^*_i(t) =  v_min and p^*_i(t) = v_min t + r_i respectively,
where r_i is a constant of integration. To compute t_3 and the constant of integration r_i, we piece together this  arc with the prior unconstrained and constrained arcs with the following additional interior constraints and boundary conditions:
v_i^*(t_3^-) = v_i^*(t_3^+), p_i^*(t_3^-) = p_i^*(t_3^+), and p_i^*(t_i^f) = p_i^f.


Subcase (c): v_i(t)= v_min for all t∈[t_2,t_i^f].
It follows that u_i^*(t) = 0, for all t∈[t_2, t_i^f], and the optimal speed and position of CAV i are as in subcase (b). The junction point t_2 along with the constants of integration can be computed by the interior constraints and boundary condition as presented in subcase (b).


Case 2: The speed, v_k(t), of the preceding CAV k is either increasing or constant. Since N_i(t_1,x_i(t_1))=0, and hence, Ṅ_̇i̇(t_1,x(t_1))=0, at the corner t_1, this implies that v_i(t)>v_k(t), for t≥ t_1. Therefore, CAV i remains in the constrained arc for as long as k is ahead of it, and its optimal control input is given by (<ref>).


Case 3: CAV k cruises on a different road from i and in a direction that might cause lateral collision with  i inside the merging zone. In this case, from (<ref>), p_k,i is the constant distance of CAV k from the entry point that CAV i entered the control zone to its position inside the merging zone. Hence, v_k,i=0, and thus the analysis is similar to the subcases (a) and (b) in Case 1.













 §.§ State, v_i(t), and Control, u_i(t), Constraints Become Active

 
	For each CAV i∈𝒩(t), the optimal control input u_i^*(t) in the unconstrained arc can be either increasing or decreasing  for all t∈[t_i^0, t_i^f].


	Since λ^v_i(t_i^f) = 0, from (<ref>) u_i^*(t_i^f)=0. Given that a u_i^*(t) is a linear function of t for all t∈[t_i^0, t_i^f], the result follows.

 
	The optimal control input u_i^*(t) in the unconstrained arc can be either negative and increasing, or positive and decreasing, or u_i^*(t)=0 for all t∈[t_i^0, t_i^f].

 
	 For each CAV i∈𝒩(t), the optimal control input u_i^*(t) never becomes active in t∈[t_i^0, t_i^f], given that it is not active at t_i^0 (Assumption <ref>), unless the safety constraints (<ref>) or (<ref>) become active.

 
	For each CAV i∈𝒩(t), if any of  the constraints (<ref>) becomes active, then the exit of the constrained arc can be only at t_i^f, unless the safety constraint s_i(t)-δ(t)≥ 0 becomes active.


 	From Assumption <ref>, for each i∈𝒩(t) none of the constraints (<ref>) is active at t_i^0. Suppose that  either v_i(t)-v_max≤ 0 or v_i(t)-v_min≥ 0 becomes active at  a junction point t_1, such that t_i^0< t_1≤ t_i^f. Then from (<ref>), it follows that u_i^*(t)=0 for t≥ t_1. Note that u_i(t) is continuous at t_1 (see Theorems <ref> and <ref>).
 	Hence, we have either v_i^*(t)=v_max, or v_i^*(t)=v_min respectively for all t∈ [t_1,t_i^f].












  §.§.§ The State Constraint, v_i(t)-v_min≥ 0, Becomes Active

Suppose the CAV starts from a feasible state and control at t=t_i^0 and at time t=t_1, (<ref>) becomes equal to v_min while u_min< u_i(t_1) <u_max and s_i(t_1)>δ(t). It follows that u_i^*(t_1)=0 for all t∈ [t_1 , t_i^f].
Hence, v^*_i(t) =   v_min and p^*_i(t) = v_min t + r_i for all t∈ [t_1 , t_i^f], where r_i is a constant of integration.
Let N_i(t,x_i(t))= v_min-v_i^*(t). Then, we have

    N_i(t_1,x_i(t_1))=  v_min-v^*_i(t_1) = 0,

which represents a tangency constraint for the state v_i^*(t) in t∈ [ t_1, t_i^f]. 
Since N_i(t_1,x_i(t_1))=0, Ṅ_̇i̇(t_1,x_i(t_1))=  -u_i^*(t_1) = 0.
The boundary conditions at t_1 for the influence functions are

    λ_i^T(t_1^-)=λ_i^T(t_1^+)+π_i ∂N_i(t_1,x_i(t_1))/∂ x_i(t_1),

which yield

    λ_i^p(t_1^-)    = λ_i^p(t_1^+) + π_i ∂N_i(t_1,x_i(t_1))/∂ p_i = λ_i^p(t_1^+), 
    λ_i^v(t_1^-)    = λ_i^v(t_1^+) + π_i ∂N_i(t_1,x_i(t_1))/∂ v_i = λ_i^v(t_1^+) - π_i , 
    λ_i^s(t_1^-)    = λ_i^s(t_1^+) + π_i  ∂N_i(t_1,x_i(t_1))/∂ s_i  = λ_i^s(t_1^+).

The Hamiltonian at the corner is

    H_i(t_1^-)=H_i(t_1^+)- π_i ∂N_i(t_1,x_i(t_1))/∂ t_1,

or 

    1/2u_i^2(t_1^-)+λ_i^p(t_1^-) v_i(t_1^-) + λ_i^v(t_1^-) u_i(t_1^-) 
    
    + λ_i^s(t_1^-) ξ_i (v_k(t_1^-)-v_i(t_1^-)) = 1/2u_i^2(t_1^+)+λ_i^p(t_1^+) v_i(t_1^+) 
    
    + λ_i^v(t_1^+) u_i(t_1^+) + λ_i^s(t_1^+) ξ_i (v_k(t_1^+)-v_i(t_1^+),

where π_i is a  Lagrange multiplier constant. The influence functions, λ_i^T(t_1^+), at t_1^+, the entry time  t_1, and the Lagrange multiplier π_i constitute 3+1+1 quantities that are determined so as to satisfy (<ref>), (<ref>) - (<ref>) and (<ref>).  Note, the state variables are continuous at the junction point, t_1, i.e., p_i(t_1^-) = p_i(t_1^+), v_i(t_1^-) = v_i(t_1^+), s_i(t_1^-) = s_i(t_1^+). The unconstrained and constrained arcs are pieced together to determine the 3+1+1 quantities above along with the constants of integration in (<ref>) - (<ref>) and (<ref>).
 
	For each CAV i∈𝒩(t), if the speed constraint v_i(t)-v_min≥ 0 becomes active at the junction point t_1, then the optimal control input is continuous at t_1. 


	From (<ref>)-(<ref>), the Hamiltonian at the corner t_1, given by (<ref>), becomes
	
    1/2(u_i^2(t_1^-) - u_i^2(t_1^+) )+ λ_i^v(t_1^+) (u_i(t_1^-) -  u_i(t_1^+)) +
    π_i (u_i(t_1^-) - u_i(t_1^+))	=0.

	Since u_i(t_1^+)=0, we have
	
    1/2 u_i^2(t_1^-) + λ_i^v(t_1^+) u_i(t_1^-) +
    	π_i u_i(t_1^-) =0,

	implying that either u_i(t_1^-)=0 or u_i(t_1^-)=-2(λ_i^v(t_1^+) + π_i ). However, the latter cannot be true since from (<ref>),  u_i(t_1^-)= -λ_i^v(t_1^-)= -( λ_i^v(t_1^+) -π_i). Hence, u_i(t_1^-)=0, and thus u_i(t_1^-)=u_i(t_1^+)=0.

 
	For each CAV i∈𝒩(t), the speed constraint v_i(t)-v_min≥ 0 becomes active at the junction point t_1 only if u_i^*(t) is negative and increasing in [t_i^0, t_i^f].


	v^*_i(t), given by (<ref>), has a minimum at t_1∈ (t_i^0, t_i^f] if ∇ v^*_i(t_1) (t-t_1)≥ 0. Since u_i^*(t)<0, for all t∈ [t_i^0, t_i^f], and (t-t_1)≤0, for all t∈[t_i^0, t_1], and given Theorem <ref>,  the result follows.











  §.§.§ The State Constraint, v_i(t)-v_max≤ 0, Becomes Active

The analysis when the state constraint, v_i(t)-v_max≤ 0, becomes active is similar to the analysis for the arc v_i(t)-v_min≥ 0, thus due to space limitation we do not repeat it here.
The proofs of the following theorems are similar to Theorems <ref> and <ref>, and thus we just provide the statements.
 
	For each CAV i∈𝒩(t), if the speed constraint v_i(t)-v_max≤ 0 becomes active at the junction point t_1, then the optimal control input is continuous at t_1. 

 
	For each CAV i∈𝒩(t), the speed constraint v_i(t)-v_max≤ 0 becomes active at the junction point t_1 only if u_i^*(t) is positive and decreasing in [t_i^0, t_i^f].







 §.§ Interior Constraints for Left and Right Turns

For any CAV i∈𝒩(t) that makes a left, or right turn, we need to impose interior speed constraints at the entry of the merging zone. These constraints will ensure that the CAV enters the merging zone with the corresponding allowable speed, v_entry, that guarantees comfort  for the passengers, hence v_i(t_i^m)≤ v_entry,
where t_i^m is the time that  CAV i enters the merging zone. The analysis is the same as in the constrained arc v_i(t)-v_max≤ 0.


 
	For the implementation of the analytical solution corresponding to the combination of the above cases, we first start with the unconstrained arc and derive the solution using (<ref>) - (<ref>). If the solution violates any of the state or control constraints, then the unconstrained arc is pieced together with the arc corresponding to the violated constraint. The two arcs yield a set of algebraic equations which are solved simultaneously using the boundary conditions of (<ref>) and interior constraints between the arcs. If the resulting solution, which includes the determination of the optimal switching time from one arc to the next one, violates another constraint, then the last two arcs are pieced together with the arc corresponding to the new violated constraint, and we re-solve the problem with the three arcs pieced together. The three arcs will yield a new set of algebraic equations that need to be solved simultaneously using the boundary conditions of (<ref>) and interior constraints between the arcs. The resulting solution includes the optimal switching time from one arc to the next one. The process is repeated until the solution does not violate any other constraints.


The process of piecing the arcs together to derive the optimal solution of the low-level problem can be computational intensive and might prevent real-time implementation. Next, we discuss the upper-level optimization problem in which we seek the minimum time t_i^f that guarantees an optimal solution for the low-level problem without activating any of the constraint arcs.








§ UPPER-LEVEL OPTIMIZATION

When a CAV i∈𝒩(t) with a given o_i, enters the control zone, it accesses the crossing protocol and solves an upper-level optimization problem. The solution of this problem yields for CAV i the  time trajectory t_p_i(p_i). 
In our exposition, we seek to derive the minimum time t_i^f that CAV i exits the control zone without activating any of the state and control constraints of the low-level optimization Problem <ref>. Therefore, the upper-level optimization problem  should yield a t_i^f such that the solution of the low-level optimization problem will result in the unconstrained case (<ref>) - (<ref>). 

There is an obvious trade-off between the two problems. The lower the value of t_i^f in the upper-level problem the higher the value of the control input in [t_i^0, t_i^f] in the low-level problem. 
The low-level problem is directly related to minimizing energy for each CAV (individually optimal solution). On the other hand, the upper-level problem is related to maximizing the throughput of the intersection, thus eliminating stop-and-go driving and travel time (system optimal solution). Therefore, by seeking a solution for the upper-level problem which guarantees  that none of the state and control constraints becomes active may be considered an appropriate compromise between the two. 




 §.§ The Time Trajectory


For simplicity of notation, for each CAV i∈𝒩(t) we write the optimal position (<ref>) of the unconstrained arc in the following form

    p^*_i(t) = ϕ_i,3· t^3 +ϕ_i,2· t^2 + ϕ_i,1· t +ϕ_i,0 ,   t∈ [t_i^0, t_i^f],

where ϕ_i,3≠ 0, ϕ_i,2, ϕ_i,1, ϕ_i,0∈ℝ are the constants of integration derived in the Hamiltonian analysis in Section <ref>. 
 
	For each i∈𝒩(t), the optimal position (<ref>) is a real-valued continuous and differentiable function ℝ_≥ 0↦ℝ_≥ 0. Based on (<ref>), it is also a strictly increasing function with respect to t∈ℝ_≥ 0.

The optimal speed and control are given by

	v^*_i(t)     = 3ϕ_i,3 ·t^2 +2ϕ_i,2 ·t + ϕ_i,1,          t ∈[t_i^0, t_i^f],         

	u^*_i(t)     = 6ϕ_i,3 ·t + 2ϕ_i,2,        t ∈[t_i^0, t_i^f].        

Next, we investigate some properties of (<ref>).
 
For each i∈𝒩(t), the optimal position p_i^*(t) given by (<ref>) is a one-to-one function for all t∈[t_i^0, t_i^f].


It follows from (<ref>) that, for each i∈𝒩(t), p_i^*(t) is a strictly increasing function with respect to t∈ℝ_≥ 0. Thus, it follows from the mean value theorem that for all t_1, t_2 ∈ [t_i^0, t_i^f] with t_1 ≠ t_2, we have p_i^*(t_1) ≠ p_i^*(t_2).

Clearly (<ref>) is a surjective function as any cubic polynomial function always has at least one real root. Therefore, (<ref>) is a bijective function and its inverse exists.
We rewrite the cubic polynomial function (<ref>) as

    t^3 + ϕ_i,2/ϕ_i,3 t^2 + ϕ_i,1/ϕ_i,3 t + ( ϕ_i,0/ϕ_i,3 - p_i/ϕ_i,3) = 0 ,   t∈ [t_i^0, t_i^f],


which then can be reduced by the substitution t = τ - ϕ_i,2/3ϕ_i,3 to the normal form

    τ^3 + ω_i,0τ + ( ω_i,1 + ω_i,2 p_i ) = 0,

where

    ω_i,0 = ϕ_i,1/ϕ_i,3 - 1/3(ϕ_i,2/ϕ_i,3)^2, 
    ω_i,1 = 1/27[2(ϕ_i,2/ϕ_i,3)^3 - 9 ϕ_i,2·ϕ_i,1/(ϕ_i,3) ^ 2] + ϕ_i,0/ϕ_i,3, 
    ω_i,2 = - 1/ϕ_i,3.

We are interested in deriving the expression for the inverse function of (<ref>) which can be accomplished by finding the root of (<ref>).

	Since, for each i ∈𝒩(t), (<ref>) is a bijective function, there exists an inverse function p_i^*(t)^-1. From Definition <ref>, the inverse function, p_i^*(t)^-1 is the time trajectory t_p_i(p_i) that yields the time that CAV i is at the position p_i inside the control zone, i.e.,
	
    t_p_i ^ * (p_i)= 
    √( - 1/2(ω_i,1 + ω_i,2  p_i ) + √(1/4(ω_i,1 + ω_i,2  p_i ) ^ 2 + 1/27ω_i,0 ^ 3)) +
    √( - 1/2(ω_i,1 + ω_i,2  p_i ) - √(1/4(ω_i,1 + ω_i,2  p_i ) ^ 2 + 1/27ω_i,0 ^ 3))
    
    + ω_i,3,
	
	where ω_i,3, ω_i,2, ω_i,1, and ω_i,0∈ℝ
	such that we have ω_i,3 = - ϕ_i,2/3 ϕ_i,3 and 1/4(ω_i,1 + ω_i,2  p_i) ^ 2 + 1/27ω_i,0 ^ 3 > 0.


	Using the Cardano method for cubic polynomials, we can derive the algebraic solution of the cubic equation. This yields the inverse function for bijective cubic polynomial function for each i∈𝒩(t) defined in the closed interval [t_i^0, t_i^f]. The algebra is tedious but standard, and thus, we omit the derivation.

 
	Let t_p_i(p_i^*) = p^*_i(t)^-1 be the time trajectory for each i∈𝒩(t). Then the constants ϕ_i,3, ϕ_i,2, ϕ_i,1, ϕ_i,0∈Φ_i, Φ_i⊂ℝ, with ϕ_i,3≠ 0, can be derived by ω_i,3, ω_i,2, ω_i,1, ω_i,0∈Ω_i, Ω_i⊂ℝ, from the following equations:	ϕ_i,0  = -ω_i,1+ω_i,0 ω_i,3-ω_i,3^3/ω_i,2 ^2,	ϕ_i,1  = - ω_i,0 + ω_i,3 ^ 2/ω_i,2, ϕ_i,2 = 3ω_i,3/ω_i,2, and	ϕ_i,3  = - 1/ω_i,2.
 

	We have
	
    ω_i,0 = ϕ_i,1/ϕ_i,3 - 1/3(ϕ_i,2/ϕ_i,3)^2, 
    ω_i,1 = 1/27[2(ϕ_i,2/ϕ_i,3)^3 - 9 ϕ_i,2·ϕ_i,1/(ϕ_i,3) ^ 2] + ϕ_i,0/ϕ_i,3, 
    ω_i,2 = - 1/ϕ_i,3, 
    ω_i,3 =  - ϕ_i,2/3 ϕ_i,3.

	After some algebraic manipulations and rearrangements, the result follows. The algebra is tedious but quite straightforward, and thus, we omit the derivation.

 
	For each i∈𝒩(t), the time trajectory t_p_i(p_i^*) is a function of  ϕ_i,3, ϕ_i,2, ϕ_i,1, ϕ_i,0∈Φ_i, Φ_i⊂ℝ, with ϕ_i,3≠ 0.

 
	The time trajectory t_p_i(p_i^*) ∈[t_i^0, t_i^f], yields the time that CAV i∈𝒩(t) is at the position p^*_i(t) inside the control zone. 

 
	For each i∈𝒩(t), the domain of t_p_i(p_i^*) is the closed interval [p_i(t_i^0),p_i(t_i^f)].
 

	Since, for each i∈𝒩(t), p_i^*(t) is a strictly increasing function in [t_i^0, t_i^f], then by the Intermediate Value Theorem, p_i^*(t) takes values on the closed interval [p_i(t_i^0),p_i(t_i^f)]. 

 
	For each i∈𝒩(t), ṗ(p^*_i(t)^-1)≠ 0 for all p∈ [p_i(t_i^0),p_i(t_i^f)]. Hence, t_p_i(p_i^*) is differentiable in [p_i(t_i^0),p_i(t_i^f)].


 
	For each i∈𝒩(t), t_p_i(p_i^*) is a strictly increasing function in [p_i(t_i^0),p_i(t_i^f)].	
 







 §.§ Optimization Framework






In what follows, for each CAV i∈𝒩(t),  we formulate a constrained optimization problem to yield its optimal path in [t_i^0, t_i^f]. We start our exposition with the introduction of the cost function and proceed with the equality and inequality constraints.





  
Cost Function.


We seek to derive the minimum time t_i^f^* that a CAV i∈𝒩(t) exits the control zone without activating any of the state and control constraints of the low-level optimization Problem <ref>, i.e., t_i^f^* should yield (<ref>) - (<ref>).
For each CAV i, the minimum time t_i^f^* can be derived by minimizing the time trajectory t_p_i(p_i^*), given by (<ref>) and evaluated at p_i^f.  

For any fixed p_i∈[p_i^0, p_i^f] of i∈𝒩(t), since the time trajectory t_p_i(p_i^*) is a function of ϕ_i (Corollary <ref>), if we vary the constants  ϕ_i the time that  i is at the position p_i changes.
Hence, in our analysis, we construct the function f_i:Φ_i→[t_i^0, t_i^f], which evaluates the time trajectory at p_i^f and yields that time that each CAV i is located at p_i^f with respect to the variables ϕ_i, i.e.,
	
    f_i(ϕ_i) = t_p_i(p_i^f).

Therefore, to derive the minimum time t_i^f^* for a CAV i, we seek to minimize f_i(ϕ_i), with respect to ϕ_i= (ϕ_i,3, ϕ_i,2, ϕ_i,1, ϕ_i,0).


	The function f_i(ϕ_i) is convex.



    If we fix the time in (<ref>) and vary ϕ_i= (ϕ_i,3, ϕ_i,2, ϕ_i,1, ϕ_i,0), then (<ref>) is an affine function denoted as p_i(ϕ_i). The variables ϕ_i take values from a closed subset Φ_i of ℝ^4,
	Similarly, the image of p_i(ϕ_i) is a closed subset of ℝ.
	For any κ∈[0,1], for a fixed time τ∈ [t_i^0, t_i^f], and for any ϕ_i, ϕ_i'∈Φ_i, we have
	
    κ(ϕ_i,3·τ^3 +ϕ_i,2·τ^2 + ϕ_i,1·τ +ϕ_i,0) +(1-κ) (ϕ_i,3' · t^3 
       +ϕ_i,2' · t^2 + ϕ_i,1' · t +ϕ_i,0') =
    	κϕ_i,3τ^3 +(1-κ) ϕ_i,3' τ^3 
       +κϕ_i,2τ^2 +(1-κ) ϕ_i,2' τ^2 + κϕ_i,1τ +(1-κ) ϕ_i,1' τ + κϕ_i,0
       +(1-κ) ϕ_i,0'.

	Hence, p_i(ϕ_i) is a convex function. Since f_i(ϕ_i) is the inverse of p_i(ϕ_i) (Lemma <ref>), the result follows.






  
Equality Constraints.


The initial and final conditions (<ref>) at the entry and exit of the control zone respectively along with the interior constraint p_(t_i ^ m) = p_i^m, at the time t_i^m that CAV i enters the merging zone (in case of left or right turns), designate the equality constraints. Thus,

    h_i^(1)(ϕ_i)    = ϕ_i,3· (t_i^0)^3 +ϕ_i,2·(t_i^0)^2 + ϕ_i,1· t_i^0+ϕ_i,0 =0, 
    
    h_i^(2)(ϕ_i)    = ϕ_i,3· (t_i^f)^3 +ϕ_i,2· (t_i^f)^2 + ϕ_i,1· t^f_i +ϕ_i,0 -p_i^f 
        = 0,
    
    h_i^(3)(ϕ_i)    =  3·ϕ_i,3· (t_i^0)^2 +2·ϕ_i,2· t_i^0 + ϕ_i,1  -v_i^0=0, 
    
    h_i^(4)(ϕ_i)    = 6·ϕ_i,3· t_i^f +2·ϕ_i,2=0, 
    
    h_i^(5)(ϕ_i)    = ϕ_i,3· (t_i^m)^3 +ϕ_i,2· (t_i^m)^2 + ϕ_i,1· t_i^m +ϕ_i,0 -p_i^m
       =0.


 
	The functions h_i^(r)(ϕ_i), r=1,…, 5 are convex.



	If we fix the time in (<ref>) and vary ϕ_i= (ϕ_i,3, ϕ_i,2, ϕ_i,1, ϕ_i,0), then h_i^(r)(ϕ_i),  r = 1, …, 5, are affine functions. The variables ϕ_i take values from a closed subset Φ_i of ℝ^4, so Φ_i is a convex set. Similarly, the image of p_i(ϕ_i) is a closed subset of ℝ. For any κ∈[0,1], for a fixed time τ∈ [t_i^0, t_i^f], and for any ϕ_i, ϕ_i'∈Φ_i, 
	h_i^(r)(δϕ_i+(1-κ)ϕ_i') = κ h_i^(r)(ϕ_i) + (1-κ) h_i^(r)(ϕ_i'), r=1,…, 5.






  
Inequality Constraints.


To avoid the speed v_i(t) constraints (<ref>) becoming active, for each i∈𝒩(t), and for all t∈[t_i^0, t_i^f], 

    v_min≤ 3·ϕ_i,3· t^2 +2·ϕ_i,2· t + ϕ_i,1≤ v_max.
	
It suffices to check the last equation at its extremum. The first derivative of (<ref>) yields the time τ_v∈[t_i^0, t_i^f] that such extremum exist.

To avoid the control input u_i(t) constraint (<ref>) becoming active, for each i∈𝒩(t), and for all t∈[t_i^0, t_i^f], 

    u_i,min≤ 6·ϕ_i,3· t +2·ϕ_i,2≤ u_i,max.

From Corollary <ref>, given that  none of the safety constraints (<ref>) and (<ref>) are activated, as discussed next, the extremum of u(t) is at t_i^0.
 Hence

    u_i,min≤ 6·ϕ_i,3· t_i^0 +2·ϕ_i,2≤ u_i,max.


Next, we impose a condition to avoid the state constraint (<ref>) becoming active within the control zone. This implies that the distance between the path trajectories of CAV i and the preceding CAV k∈𝒩(t), 𝒞_o_i∩𝒞_o_k≠ 0,  on lane θ∈ℒ at each p_i(t) should be  greater than δ_i(t), hence

    ξ_i · (p_k(t)-p_i(t)) > δ̅ + ρ_i · v_i(t),  t∈[t_i^0, t_i^f].

By substituting p_k(t), p_i(t), and v_i(t), from (<ref>) and (<ref>), we have

    t^3 (ϕ_i,3 - ϕ_k,3) + t^2 (ϕ_i,2 -ϕ_k,2 + 3 ρ_i ·ϕ_i,3/ξ_i ) 
    
        + t (ϕ_i,1 - ϕ_k,1 + 2 ρ_i ·ϕ_i,2/ξ_i) + ρ_i ϕ_i,1/ξ_i 
    
        +ϕ_i,0 -ϕ_k,0 + δ̅/ξ_i <0.

It suffices to check the last equation at its extremum. The first derivative of (<ref>) yields the time τ_s∈[t_i^0, t_i^f] that such extremum exist.

Similarly, the constraint (<ref>) may become active when the path trajectories of i∈𝒩(t) and a CAV j∈𝒩(t), 𝒞_o_i∩𝒞_o_j≠∅, cruising on another road, are crossed inside the merging zone which could lead to a lateral collision. Thus, we impose the following condition

    - p_k,i(t) + [ ϕ_i,3 t^3 + t^2 (ϕ_i,2 + 3 ρ_i ϕ_i,3/ξ_i ) 
     
        + t (ϕ_i,1 + 2 ρ_i ϕ_i,2/ξ_i )]+ρ_i ϕ_i,1 /ξ_i +ϕ_i,0 + δ̅ /ξ_i <0,

where p_k,i(t) is the constant distance of CAV k from the entry point that CAV i entered the control zone.
It suffices to check the last equation at its extremum. The first derivative of (<ref>) yields the time τ_l∈[t_i^0, t_i^e], where t_i^e is the time that CAV i exits the merging zone, that such extremum exist.

Finally, when a CAV i needs to make either a  left or right turn, the speed at the entry of the merging zone needs to be less than or equal to the corresponding allowable speed, v_entry, that guarantees comfort for the passengers.
Hence

    3·ϕ_i,3· (t_i^m)^2 +2·ϕ_i,2· t_i^m+ ϕ_i,1≤ v_entry.


Since we seek to derive the minimum time without activating any of the state, control, and safety constraints, we add a very small ε > 0, in each inequality constraint that will prevent any of these to become active. Without loss of generality, to simplify the exposition, we also consider ξ_i=1.
Therefore, the set of inequality constraints in the upper-level optimization is

    g_i^(1)(ϕ_i)    = 3·ϕ_i,3·τ_v^2 +2·ϕ_i,2·τ_v + ϕ_i,1  - v_max +ε≤ 0, 
    
    
    g_i^(2)(ϕ_i)    = v_min - 3·ϕ_i,3·τ_v^2 - 2·ϕ_i,2·τ_v - ϕ_i,1  +ε≤ 0, 
    
    
    g_i^(3)(ϕ_i)    = 6·ϕ_i,3· t_i^0 +2·ϕ_i,2 - u_i,max+ε≤ 0, 
    
    
    g_i^(4)(ϕ_i)    = u_i,min - 6·ϕ_i,3· t_i^0 - 2·ϕ_i,2 +ε≤ 0, 
    
    
    g_i^(5)(ϕ_i)    =τ_s^3 (ϕ_i,3 - ϕ_k,3) + τ_s^2 (ϕ_i,2 -ϕ_k,2 + 3 ρ_i ϕ_i,3) 
       + τ_s (ϕ_i,1 - ϕ_k,1 + 2 ρ_i ·ϕ_i,2) +ρ_i ϕ_i,1 +ϕ_i,0 -ϕ_k,0
       + δ̅ +ε≤ 0,
    
    g_i^(6)(ϕ_i)    = - p_k,i(t) + [ ϕ_i,3τ_l^3 + τ_l^2 (ϕ_i,2 + 3 ρ_i ·ϕ_i,3 ) 
       + τ_l (ϕ_i,1 + 2 ρ_i ·ϕ_i,2)] 
    +ρ_i ϕ_i,1 +ϕ_i,0 +δ̅ +ε≤ 0,
    
    g_i^(7)(ϕ_i)    =  3·ϕ_i,3· (t_i^m)^2 +2·ϕ_i,2· t_i^m+ ϕ_i,1  -v_entry≤ 0.



	The functions g_i^(m)(ϕ_i), m=1,…, 7, are convex.



The proof is similar to the proof of Proposition <ref>.








  
Problem Formulation.


For each CAV i∈𝒩(t), we consider the following problem






 
	
    min_ϕ_i     f_i (ϕ_i) 
    subject to  ϕ_i∈Φ_i,       h_i^(r)(ϕ_i)=0,  r=1,…, 5,
        g_i^(m)(ϕ_i)≤ 0,  m=1,…, 7.


Note that the set Φ_i is determined by the occupancy sets of the lanes, i.e.,

	 
    Φ_i={ϕ_i |  f_i(ϕ_i) ∉⋃_θ∈ C_o_i O_θ},
 
and can be formed by each i∈𝒩(t) at t_i^0 by accessing the crossing protocol ℐ(t).

The cost function, f_i (ϕ_i), of Problem <ref> is bounded below (Remark <ref>). The Lagrangian function, L_i(ϕ_i, γ_i, ν_i): ℝ^r+m+1→ℝ, is


    L_i(ϕ_i, γ_i, ν_i) = f_i(ϕ_i) + γ_i^T h_i(ϕ_i) + ν_i^T g_i(ϕ_i),

where h_i(ϕ_i)=[h_i^(1)(ϕ_i) …  h_i^(5)(ϕ_i)]^T, g_i(ϕ_i)=[g_i^(1)(ϕ_i) …  g_i^(7)(ϕ_i)]^T, γ_i=[γ_i^(1)…γ_i^(5)]^T, γ_i∈ℝ^5, and  ν_i=[ν_i^(1)…ν_i^(7)]^T,
ν_i∈ℝ^7_≥ 0.

Next, we investigate some properties of the optimal solution in Problem <ref> using a geometric duality framework.







 §.§ Geometric Duality Framework







A geometric duality framework can admit insightful visualization through the use of hyperplanes along with their set support and separation properties. Before we proceed, and for easy reference, we provide some standard definitions that we use in our exposition.






	Let Λ be a subset of ℝ^n, n∈ℕ. The affine hull of Λ, denoted aff(Λ), is the intersection of all affine sets containing Λ.



	Let Λ be a subset of ℝ^n, n∈ℕ. We say that z is a relative interior point of the set Λ, if z∈Λ and there exists an open sphere R centered at z such that R  ∩aff(Λ)⊂Λ. The set of all relative interior points of Λ is called the relative interior of Λ, and is denoted by ri(Λ). 



	Let Λ be a subset of ℝ^n, n∈ℕ. We say that z is a closure point of the set Λ, if there exists a sequence {z_k}⊂Λ that convergences to z. The closure of Λ, denoted cl(Λ), is the set of all closure points of Λ.


Given a nonempty set Λ⊂ℝ^n, n∈ℕ, let Λ_L be the set of all limit points of Λ. The closure of Λ is cl(Λ)=Λ∪Λ_L.


	 Given a nonempty set Λ⊂ℝ^n, n∈ℕ, we say that a vector z' is a direction of recession of Λ if z+κ z'∈Λ for all z∈Λ and κ≥ 0.


Thus, z' is a direction of recession of Λ if starting at any z∈Λ and going indefinitely along z', we never cross the relative boundary of Λ to points outside Λ. The set of all directions of recession is a cone containing the origin, and it is called the recession cone of Λ. 

The proofs of the following three lemmas can be found in <cit.>.

  
	Let Λ be a nonempty closed convex set of ℝ^m+r+1, m, r∈ℕ. Then the recession cones of Λ and ri(Λ) are equal.


  
	Let Λ be a nonempty closed convex set of ℝ^m+r+1, m, r∈ℕ. Then cl(Λ)= cl(ri(Λ)).


  
	Let Λ be a nonempty closed convex set of ℝ^m+r+1, m, r∈ℕ, that contains no vertical lines. Let (z, y, w) be a vector in Λ, where z∈ℝ^m, y∈ℝ^r, and w∈ℝ. Then, Λ is contained in a closed halfspace corresponding to a nonvertical hyperplane, i.e., there exist a vector ν∈ℝ^m_≥ 0, γ∈ℝ^r, δ≠ 0, and a scalar η such that
	
    ν^T z + γ^T y +δ w ≥η,   ∀ (z,y,w)∈Λ.

Furthermore, if (z', y', w')∉Λ, then there exist a nonvertical hyperplane strictly separating (z', y', w') and Λ.


In our analysis, we consider hyperplanes in the space of constraint-cost pairs (h_i(ϕ_i)^(r), g_i(ϕ_i)^(m), f_i(ϕ_i)) of  Problem <ref> viewed as vectors in ℝ^m+r+1, where m=7,  r=5, in our case. A hyperplane P_H of this type is specified by a linear  equation involving a nonzero normal vector (ν, γ, δ), where ν∈ℝ^m_≥ 0, γ∈ℝ^r, δ≠ 0, and a scalar η
	
    P_H = { (z,y,w)| z∈ℝ^m, y∈ℝ^r, w∈ℝ, 
    ν^T z + γ^T y +δ w = η}.

A hyperplane with normal (ν, γ, δ), ν∈ℝ^m_≥ 0, γ∈ℝ^r, δ≠ 0, is referred to as nonvertical. By dividing the normal vector of such a hyperplane by δ, we can restrict attention to the case where δ=1.








	The subset Λ of ℝ^m+r+1, where m=7,  r=5, given by the space of constraint-cost pairs (h_i(ϕ_i)^(r), g_i(ϕ_i)^(m), f_i(ϕ_i)) of Problem <ref>, i.e.,
		
    Λ = { h_i(ϕ_i)^(r), g_i(ϕ_i)^(m), f_i(ϕ_i)  |  ϕ_i∈ℝ^4 },

	is convex.


	Let (z, y, w) and (z', y', w') be two elements in Λ. For any κ∈[0,1], κ (ν^T z + γ^T y + w ) + (1-κ) (ν^T z' + γ^T y' + w')  = κν^T z + (1-κ) ν^T z' +	
	 κγ^T y + (1-κ) γ^T y' + κ w +(1-κ) w'. Since Λ is defined in the space of constraint-cost pairs (h_i(ϕ_i)^(r), g_i(ϕ_i)^(m), f_i(ϕ_i)), which are convex (by Propositions <ref>, <ref>, and <ref>), the result follows.



	The set
	
    Λ_E = { (z,y,w)  |  ∃ w'≤ w and  (z,y,w')∈Λ},

	is convex.



  
	If for every sequence {(z_k, y_k, w_k)⊂Λ} with (z_k, y_k)→ (0,0), we have  w^*≤lim inf_k→∞ w_k, where w^*=inf_(0, 0, w)∈Λ w, then the set Λ_E does not contain any vertical lines.




	Suppose that Λ_E contains a vertical line. Then, since Λ_E is convex, the direction (0, 0, -1) would be a direction of recession of cl(Λ_E), and hence from Lemma <ref>, a direction of recession of ri(Λ_E). Since (0, 0, w^*) is a closure point of Λ_E, it is also a closure point of ri(Λ_E) (Lemma <ref>), and therefore, there exists a sequence {(z_k, y_k, w_k)⊂ ri(Λ_E)} converging to (0, 0, w^*). Since (0, 0, -1)  is a direction of recession of ri(Λ_E), {(z_k, y_k, w_k-1)⊂ ri(Λ_E)}, and consequently, {(z_k, y_k, w_k-1)⊂ Λ_E}. Hence, in view of the definition of Λ_E, there is a sequence {(z_k, y_k, w_k')∈Λ}, with w_k'≤ w_k-1, for all k, such that
	lim inf_k→∞ w_k'≤ w^*-1.
	However, this contradicts that w^*≤lim inf_k→∞ w_k for every sequence {(z_k, y_k, w_k)∈Λ} with (z_k, y_k)→ (0,0). 




	The hyperplane in P_H with normal (ν, γ, 1) that passes through a vector (h_i(ϕ_i)^(r), g_i(ϕ_i)^(m), f_i(ϕ_i)) in Λ intercepts the vertical axis {(0, 0, w) |  w∈ℝ} at the level of the Lagrangian function, L_i(ϕ_i, γ_i, ν_i), in (<ref>). 



	A hyperplane in P_H with normal (ν, γ, 1)  crosses the (m+r+1)-st axis at (0, 0, η), η≥ 0. Furthermore, it contains the set Λ in its upper closed halfplane if and only if, for all (z, y, w)∈Λ, 
		
    ν^T z + γ^T y + w ≥η.




	Among all hyperplanes in P_H with a normal (ν, γ, 1) that contain in their positive, closed halfspace set Λ, the highest attained level of interception of the vertical axis is
	
    inf_ϕ_i∈Φ_i L_i(ϕ_i, γ_i, ν_i).









	There is no duality gap in Problem <ref>.



Since f_i(·), h_i^(r)(·), g_i^(m)(·); r = 1,…, 5; m = 1, …, 7, are convex functions, and h_i^(r)(·), g_i^(m)(·) are affine, the result immediately follows by applying the Slater's weaker condition; see <cit.>.




	If there is no duality gap in Problem <ref>, then
		
    maxinf_(z, y)∈Λ{ν^T z + γ^T y + w}= w^*,

	 where w^*=inf_(0, 0, w)∈Λ w.



Next, we provide the condition under which an optimal solution in Problem <ref> exists.









    An optimal solution of Problem <ref> exists if and only if for every sequence {(z_k, y_k, w_k)⊂Λ} with (z_k, y_k)→ (0,0), there holds  w^*≤lim inf_k→∞ w_k, where w^*=inf_(0, 0, w)∈Λ w.




    We show sufficiency first. Let {(z_k, y_k, w_k)⊂Λ} such that (z_k, y_k)→ (0,0). Taking the limit as k→∞, we have
	inf_(z, y)∈Λ{ν^T z + γ^T y + w}≤lim inf_k→∞ w_k,
	which implies that
	maxinf_(z, y)∈Λ{ν^T z + γ^T y + w}= w^*≤lim inf_k→∞ w_k.
    
    To show necessity, first we note that (0, 0, w^*) is a closure point of Λ_E, since by the definition of w^*, there exist a sequence {0, 0, w_k} that belongs to Λ, and hence also to Λ_E, and is such that w_k→ w^*. Next, we show by contradiction that (0, 0, w^*-ε)∉cl(Λ_E), for any ε>0. Suppose that (0, 0, w^*-ε)∈cl(Λ_E) for some ε>0. Hence, there exists a sequence {(z_k, y_k, w_k)⊂Λ_E} such that  (z_k, y_k, w_k)→ (0, 0, w^*-ε). In view of the definition of Λ_E, this implies the existence of another sequence {(z_k, y_k, w_k')⊂Λ} with (z_k, y_k)→ (0,0) and w_k'≤ w_k for all k, such that 
	lim inf_k→∞ w_k'≤ w^*-ε,
	which contradicts the hypothesis w^*≤lim inf_k→∞ w_k. Since Λ_E does not contain any vertical lines (Lemma <ref>) and (0, 0, w^*-ε)∉cl(Λ_E) for any ε>0, it follows (Lemma <ref>) that there exists a nonvertical hyperplane in P_H that separates strictly (0, 0, w^*-ε) and Λ_E. This hyperplane crosses the (m+r+1)-st axis at a unique vector (0,0, η), which must lie between (0, 0, w^*-ε) and (0, 0, w^*), i.e., w^*-ε≤η≤ w^*. Furthermore, η cannot exceed the value 
	maxinf_(z, y)∈Λ{ν^T z + γ^T y + w},
	for all (z, y, w)∈Λ, which implies 
	w^*-ε≤maxinf_(z, y)∈Λ{ν^T z + γ^T y + w}≤ w^*.
	Since ε can be arbitrarily small, it follows that 
	maxinf_(z, y)∈Λ{ν^T z + γ^T y + w}= w^*.





	The solution of Problem <ref> yields the optimal ϕ_i= (ϕ_i,3, ϕ_i,2, ϕ_i,1, ϕ_i,0)∈Φ_i that minimizes (<ref>) evaluated at p_i^f, and hence t_i^f. Recall that the constants ω_i,3, ω_i,2, ω_i,1, ω_i,0∈Ω_i in (<ref>) are derived directly from ϕ_i (Lemma <ref>). However, the inverse of (<ref>) defined in [p_i^0, p_i^f] yields the optimal position (<ref>) for the unconstrained arc (Corollary <ref>) in [t_i^0, t_i^f], for all i ∈𝒩(t). Hence, for each CAV i ∈𝒩(t), the optimal control input of the low-level optimization can be derived directly by taking the second derivative of the inverse of (<ref>) evaluated at p_i^f using the optimal ϕ_i= (ϕ_i,3, ϕ_i,2, ϕ_i,1, ϕ_i,0)∈Φ_i.




	If the condition w^*≤lim inf_k→∞ w_k holds, it guarantees that the set Λ_E does not contain any vertical lines (Lemma <ref>). The physical interpretation of the condition is that there exists a time trajectory for a CAV to exit the control zone without activating any of the equality and inequality constraints.





	The decentralized framework presented here is implemented as follows. Every time a CAV i∈𝒩(t) enters the control zone, it formulates and solves Problem <ref> by accessing the crossing protocol ℐ(t).	The solution yields the optimal time trajectory t_p_i(p_i) of CAV i, and as a result, the minimum time t_i^f to exit the control zone. Problem <ref> is solved sequentially by each CAV that enters the control zone. Once the time trajectory of a CAV inside the control zone is derived, then it does not change. By inversing t_p_i(p_i) and taking the second derivative, CAV i obtains the optimal control input that corresponds to the unconstrained arc. Therefore, the solution of Problem <ref>, if it exists, guarantees that none of the state and control constraints becomes active in the low-level optimization. If, however, the solution of Problem <ref> does not exist, then CAV i selects a feasible t_i^f from the crossing protocol ℐ(t), and follows the analysis of the low-level optimization, which includes piecing together the constrained and unconstrained arcs, to derive the optimal control input from t_i^0 to t_i^f.













§ SIMULATION RESULTS


In this section, we present simulation results to evaluate the analysis in the low-level and upper-level optimization.
First, we demonstrate the analysis of the low-level optimization with three case studies using two CAVs. We consider cases where the state and control constraints become active.
Second, we demonstrate the upper-level optimization analysis on a set of 10 and 24 CAVs at a four-way intersection. These cases include intersection-crossing, left, turns right turns, and lane changes.



 §.§ Low-Level Optimization
 
We simulate two CAVs that share a single lane within the control zone. The initial conditions of the CAVs are designed such that the state and control constraints become active on the lead CAV 1, while the rear-end safety constraint activates on the following CAV 2.
Initially, CAV 1 generates all possible trajectories which satisfy its boundary conditions.
Then, it selects the feasible trajectory which minimizes its total energy consumption.
CAV 2 applies the same process and verifies whether the resulting trajectory is feasible concerning the rear-end safety constraint. If this constraint is not satisfied, then CAV 2 must solve a boundary-value problem that satisfies the boundary, continuity, and optimality conditions. 
We provide three scenarios for this simulation case study. In the first scenario (Fig. <ref>), CAV 1 follows an unconstrained trajectory. In the second scenario, the constraint u_max becomes active for CAV 1. In the third scenario, the constraint v_max is activated for CAV 1. In each case, CAV 1 starts with a much lower speed than CAV 2 to ensure the rear-end safety constraint is activated.



For the first scenario (Fig. <ref>), CAV 1 follows an unconstrained control input, which is positive and decreasing. To prevent a rear-end collision, CAV 2 follows a small positive acceleration profile until the safety constraint becomes active. This constraint activation causes CAV 2 to jump into a new arc, which corresponds to a linear deceleration to zero (Fig. <ref>). The jump in the control input of CAV 2 corresponds to a corner in the CAV's speed, as well as an instantaneous activation of the rear-end safety constraint.



In the second scenario (Fig. <ref>), CAV 1 begins with the u_max constraint active (we relax Assumption <ref>) for the first 1.3 s. CAV 2 uses this time to increase its acceleration until the rear-end safety constraint becomes instantaneously active around t = 3.2 s. Then, CAV 2 slowly decelerates until it reaches the intersection at its prescribed time. 
In the absence of the rear-end safety constraint, CAV 2 would have followed a small linear acceleration profile, as opposed to the initial breaking behavior observed in Fig. <ref>.





Finally, in  the third scenario (Fig. <ref>), CAV 1 activates the v_max constraint arc from t = 6 s until the terminal time. In this case, CAV 2 starts with some positive acceleration before jumping to a negative unconstrained arc. The jump occurs instantaneously when the rear-end safety constraint is activated. 
In each scenario, CAV 1 follows a trajectory with a piecewise-linear control input. The rear-end safety constraint determines the trajectory of CAV 2, which must follow two unconstrained arcs with an instantaneous jump where the safety constraint becomes active. The magnitude of this jump depends on the jump conditions of the influence functions.





 §.§ Upper-Level Optimization
 
To demonstrate the efficacy of the upper-level optimization, a simulation was run for 𝒩(t)= 24 CAVs on an intersection shown in Fig. <ref>. This scenario consists of 6 paths with 9 locations for potential lateral collisions. It also includes turning speed, state and control, and rear-end safety constraints.




Each CAV enters at p_i(t_i^0) = 0 and follows a monotonically increasing trajectory to the final distance p_i(t_i^f) = S_1. The infeasible region caused by the rear-end safety constraint is shaded with a dashed line, while the vertical lines represent the lateral time headway constraint at each point along path 1.



















A discussion about the implications of real-time implementation of the upper-level control algorithm in an experimental setting is discussed in <cit.>.
Supplementary videos of the simulation and experimental results of the proposed framework as well as the parameters used for the simulation results can be found at: https://sites.google.com/view/ud-ids-lab/oppc.
















§ CONCLUDING REMARKS AND DISCUSSION


In this paper, we presented a decentralized theoretical framework, consisting of an upper-level and a low-level optimization, that aims at coordinating CAVs at different traffic scenarios. We provided a complete, analytical solution of the low-level optimization problem that includes the rear-end safety constraint, where the safe distance is a function of speed. We also provided a problem formulation for the upper-level optimization in which there is no duality gap, implying that the optimal time trajectory for each CAV does not activate any of the state, control, and safety constraints of the low-level optimization,  thus allowing for online implementation. Finally, we presented a geometric duality framework with hyperplanes to derive the condition under which the solution of the upper-level optimization always exists.

In our framework, we considered 100% penetration rate of CAVs having access to perfect information (no errors or delays) which both impose limitations for real-world applications. It is expected that CAVs will gradually penetrate the market, interact with non-CAVs and contend with vehicle-to-vehicle and vehicle-to-infrastructure communication limitations, e.g., bandwidth, dropouts, errors and/or delays. Although some recent studies have explored the implications of partial CAV penetration rates, see <cit.>, no system approaches to date have reported in the literature to optimally coordinate CAVs at different penetration rates. Ongoing research focusing on addressing partial penetration rates of CAVs relying upon on-board sensing and overcoming real-world communication limitations. A direction for future research should extend the proposed framework to consider passengers' comfort in addition to energy efficiency and safety.






abbrvnat




































