Conditions for State and Control Constraint Activation in Coordination of Connected and Automated Vehicles
A M Ishtiaque Mahbub Andreas A. Malikopoulos


Observation of edge waves in a two-dimensional Su-Schrieffer-Heeger acoustic network
    Vincent Pagneux
    December 30, 2023
====================================================================================

empty

arabic



Connected and automated vehicles (CAVs) provide the most intriguing opportunity to reduce pollution, energy consumption, and travel delays. In earlier work, we addressed the optimal coordination of CAVs using Hamiltonian analysis. In this paper, we investigate the nature of the unconstrained problem and provide conditions under which the state and control constraints become active. We derive a closed-form analytical solution of the constrained optimization problem and evaluate the solution using numerical simulation.
















§ INTRODUCTION
 
The implementation of an emerging transportation system with connected automated vehicles (CAVs) enables a novel computational framework to provide real-time control actions that optimizes energy consumption and associated benefits. CAVs can alleviate congestion at major transportation segments <cit.>, reduce emission, improve fuel efficiency <cit.>, and increase passenger safety. 


Several research efforts have been reported in the literature proposing either centralized or decentralized approaches on coordinating CAVs at different traffic scenarios, e.g., intersections, merging at roadways and roundabouts, speed reduction zones, to improve traffic flow and reduce stop-and-go driving. Some papers have used a reservation scheme <cit.> to control a signal-free intersection of two roads. Other approaches have focused on coordinating vehicles to improve the traffic flow <cit.>. 



More recently, an optimal control framework was established for coordinating online CAVs. A closed-form, analytical solution without considering state and control constraints was presented in <cit.> for coordinating online CAVs at highway on-ramps, in <cit.> at intersections, and in <cit.> for traffic corridors. The solution to the state and control unconstrained optimization problem discussed above shows possible state and control constraint activation within the optimization horizon and at the boundaries. To mitigate terminal jerk, Ntousakis et al. <cit.> reformulated the optimal control problem with terminal constraint on state and control, which does not guarantee constrained state/control values within the optimization horizon. The optimal control problem considering state and control constraints was addressed in <cit.> at an urban intersection, where the resulting formulation relies on piecing the unconstrained and constrained arcs together, and leads to a recursive computation of the solution until all constraint activation cases are resolved. This procedure may end up in a complicated recursive structure, and can become computationally exhaustive due to the presence of implicit system of equations to be solved numerically, preventing possible real-time implementation. The process is similar if the rear-end safety constraint is included <cit.>.

In this paper, we investigate the formulation presented in <cit.> to provide useful insights about the state and control constraint activation to increase computational efficiency, and derive closed form analytical solution using the Hamiltonian analysis.

In particular, we provide (1) conditions under which the constraint activation space can be reduced, and (2) conditions under which specific combination of state and control constraint activation can be identified to increase computational efficiency.


The remainder of the paper is organized as follows. In Section II, we introduce the problem formulation and present the unconstrained case. In Section III, we discuss different aspects of the state and control constrained formulation in detail. In Section IV, we provide the analytical solution of the constrained optimization. In Section V, we evaluate the effectiveness of the proposed approach in a simulation environment. We draw conclusions and discuss next steps in Section VI.








§ PROBLEM FORMULATION
 


Although our theoretical framework can be applied to any
traffic scenario, e.g., merging at roadways <cit.> and roundabouts, passing through speed reduction
zones <cit.>, we use an
intersection (Fig. <ref>) as a reference to present the fundamental ideas
and results of this paper, since an intersection provides unique features
making it technically more challenging compared to
other traffic scenarios.

We define the area illustrated by the red square of dimension S in Fig. <ref> as the merging zone where possible lateral collision may occur. Upstream of the merging zone, we define a control zone of length L, inside of which CAVs can coordinate with each other before they pass the merging zone. The intersection has a coordinator that only maintains the queue of the CAVs inside the control zone, and is not involved in any decision-making process. When a CAV enters the control zone, the coordinator receives its information and assigns a unique identity i∈ℕ to it. 

The objective of each CAV is to derive its optimal control input to cross the intersection without activating any of the safety, state and control constraints.





 §.§ Vehicle Dynamics and Constraints

Let 𝒩(t)={1,..., N(t)}, where N(t)∈ℕ is the number of CAVs inside the control zone at time t∈ℝ^+, be the queue of CAVs inside the control zone.
We model each CAV i∈𝒩(t) as a double integrator

    ṗ_i  =v_i(t),  v̇_i   =u_i(t), ∀ t∈ [t_i^0, t_i^f],

where p_i(t)∈𝒫_i, v_i(t)∈𝒱_i, and u_i(t)∈𝒰_i denote the position, speed and
acceleration (control input) of each CAV i in the corridor. 
The sets 𝒫_i, 𝒱_i, and 𝒰_i, i∈𝒩(t), are complete and totally bounded subsets of ℝ.
Let x_i(t)=[p_i(t)   v_i(t)] ^T denote the state vector of each CAV i∈𝒩(t), with initial value x_i^0=[p_i^0  v_i^0]  ^T taking values in 𝒳_i
=𝒫_i×𝒱_i. The state space 𝒳_i for each CAV i∈𝒩(t) is closed with respect to the induced topology on 𝒫_i×𝒱_i and thus, it is compact. Each CAV i enters the control zone at t_i^0, enters the merging zone at t_i^m, and exits the merging zone at t_i^f.

To ensure that the control input and vehicle speed are within a
given admissible range, the following constraints are imposed

    u_min≤ u_i(t)≤ u_max,  and
     
    0  ≤ v_min≤ v_i(t)≤ v_max,  ∀ t∈ t_i
    ^0,t_i^f],

where u_min, u_max are the minimum and maximum
acceleration for each CAV i∈𝒩(t), and v_min, v_max are the minimum and maximum speed limits respectively.
 

We impose the following rear-end safety constraint 


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

where s_i(t) denotes the distance of CAV i from CAV k which is physically immediately ahead of i, and  δ_i(t) denotes minimum safe distance which is a function of speed v_i(t).




In the modeling framework described above, we impose the following assumptions:Assumption 1.
Each CAV is equipped with sensors to measure and share their local information.Assumption 2.
Communication among CAVs occurs without any delays or errors. 


2 
The final endpoint speed is not constrained for any CAV i. 



Assumption 1 restricts the problem to the case of 100% CAV penetration rate.
Assumption 2 may be strong, but it is relatively straightforward to relax as long as the noise in the measurements and/or delays is bounded. For example, 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.




 §.§ Upper-Level Vehicle Coordination

The queue of CAVs inside the control zone 𝒩(t) can be determined as an outcome of an upper-level vehicle coordination problem as described in <cit.>. 
In what follows, we assume that the time t_i^m that each CAV enters the merging zone is determined as the solution of some upper-level coordination problem, and will focus only on a low-level energy optimization problem that will yield the optimal control input u_i(t) for each CAV i∈𝒩(t) to achieve the assigned merging time t_i^m.



 §.§ Low-Level Energy Optimization Problem

For each CAV i∈𝒩(t) traveling inside the control zone, we formulate an optimal control problem to minimize the L^2-norm of the control input, i.e., 1/2u_i^2(t), so that the transient engine operation can be minimized, and thus, fuel consumption  <cit.>,


    min_u_i∈ U_iJ_i(u(t))=  ∫_t_i^0^t_i^m1/2u_i^2(t) dt,
    
    subject to
    :(<ref>),(<ref>),
    and given t_i^0, p_i(t_i^0)=0, v_i(t_i^0), t_i^m,  p_i(t_i^m).

Note that, we do not provide a desired speed v_i(t_i^m) at t_i^m. Additionally, we do not explicitly include the lateral and rear-end (<ref>) safety constraints. We enforce the lateral collision constraint by selecting the appropriate merging time t_i^m for each CAV i in the upper-level vehicle coordination problem. The activation of rear-end safety constraint can be avoided under proper initial conditions [t_i^0,v_i(t_i^0)] as it was shown in <cit.>. 
From (<ref>) and the state equations (<ref>), we formulate the state and control constraint (<ref>) adjoined Hamiltonian function H_i(t, λ(t),μ(t), x(t),u(t)) for each CAV i∈𝒩(t),

    H_i(t, λ(t),μ(t), x(t),u(t)) 
    =1/2 u^2_i
    + λ^p_i·
    v_i + λ^v_i· u_i
    
    +μ^a_i·(u_i - u_max)+ μ^b_i·(u_min - u_i)
    
    + μ^c_i·(v_i - v_max) 
    + μ^d_i·(v_min - v_i),
      ∀ t ∈[t_i^0, t_i^m],

where λ^p_i, λ^v_i are the co-state components corresponding to the state vector x_i(t),
and μ^a_i, μ^b_i, μ^c_i, μ^d_i are the Lagrange multipliers with the following KKT conditions, μ^a_i· (u_i(t) - u_max) = 0, μ^b_i· (u_min - u_i(t)) = 0, μ^c_i· (v_i(t) - v_max) = 0, μ^d_i· (v_min - v_i(t)) = 0, and μ^a_i, μ^b_i, μ^c_i, μ^d_i>0 on the constrained arc. 
The Euler-Lagrange equations are

    λ̇^p_i(t) = - ∂ H_i/∂ p_i = 0,  λ̇^v_i = - ∂ H_i/∂ v_i =-λ^p_i-μ^c_i+μ^d_i,
    ∂ H_i/∂ u_i = u_i + λ
    ^v_i + μ^a_i - μ^b_i = 0.




 §.§ Unconstrained Optimization

If the inequality state and control constraints (<ref>) are not active, we have μ^a_i = μ^b_i= μ^c_i=μ^d_i=0.
From (<ref>) and (<ref>), for each CAV i∈𝒩(t) we derive the optimal control input u_i^*(t), and the optimal states p_i^*(t), v_i^*(t) as

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

where a_i, b_i, c_i, and d_i are constants of integration which can be computed by using the boundary conditions in (<ref>).




§ CONSTRAINED OPTIMIZATION

To derive a closed-form analytical solution for (<ref>), we (1) identify the conditions for constraint(s) exclusion, (2) define the condition under which they become active, and (3) derive the final constrained optimal solution without recursion. 


The optimal trajectory profile between the initial and final time t_i^0 and t_i^m remains unchanged if the time horizon is shifted to the initial and final time t_i^0 =0 and t_i^m' = t_i^m-t_i^0. As a result, without the loss of generality, we can proceed to analyze the unconstrained trajectory for step 1 and 2 at the shifted horizon [0,t_i^m'] and derive the constrained optimal trajectory for step 3 at the actual time horizon [t_i^0,t_i^m]. In the following analysis, without creating any ambiguity, we use the modified final merging time as t_i^m instead of t_i^m'.




 §.§ Condition of Constraint Exclusion

Although we have two state and two control constraints from (<ref>), there are 15 constraint combinations in total that can be activated. In this section, we show that it is only possible for a subset of the constraints to be active within an unconstrained solution. Therefore, it is not necessary to consider all the cases in the constrained problem. In what follows, we delve deeper into the nature of the uncontrolled optimal solution to derive useful information about the possible existence of constraint activation within the control zone.

Lemma 1.
If v_i(t_i^m) is not prescribed, and the terminal time t_i^m is fixed, then

    t_i^m=-b_i/a_i.






For fixed terminal time t_i^m and unspecified state v_i(t_i^m), λ_i^v(t_i^m) = 0.
Using λ_i^v(t_i^m) in (<ref>), we have u_i(t_i^m)=a_it_i^m+b_i=0,
and the result follows.

 

Corollary 1.
The constants a_i and b_i always have opposite signs. 


Lemma 2.
The unconstrained optimal control input u_i(t) is linearly decreasing if v_i(t_i^0)<(p_i(t_i^m)-p_i(t_i^0))/t_i^m.
The unconstrained optimal control input u_i(t) is linearly increasing if v_i(t_i^0)>(p_i(t_i^m)-p_i(t_i^0))/t_i^m.




From (<ref>) and (<ref>), we can write v_i(t_i^0)=1/2a_i·(t_i^0)^2+b_i· t_i^0+c_i and p_i(t_i^0) = 1/6  a_i· (t_i^0)^3 +1/2 b_i· (t_i^0)^2 + c_i· t_i^0 + d_i.
With t_i^0=0, we have

    c_i=v_i(t_i^0),  d_i = p_i(t_i^0).

From (<ref>), we have p_i(t_i^m)=1/6a_i·(t_i^m)^3+1/2b_i·(t_i^m)^2+c_i· t_i^m+d_i.
Using the results from (<ref>), and (<ref>) in the above equation and solving for a_i, we have

    a_i=3(v_i(t_i^0)t_i^m-(p_i(t_i^m)-p_i(t_i^0)))/(t_i^m)^3.

Since t_i^m>0, a_i is non positive if v_i(t_i^0)t_i^m-(p_i(t_i^m)-p_i(t_i^0))<0,
which in turn implies negative slope of the linear control input u_i^*(t), i.e., linearly decreasing acceleration profile. The second part of Lemma 2 can be proved following similar steps.
 

Lemma 3.
For a given unconstrained optimal profile, only a subset of state-control constraints can remain active. Furthermore, if either v_i(t)-v_max≤0 or u_i(t)-u_max≤0 is activated, none of the constraint pairs v_min-v_i(t)≤0 and u_min-u_i(t)≤0 can be activated. The reverse also holds.


 
The optimal solution derived from the unconstrained case yields a linear control profile increasing or decreasing to the terminal value zero (Lemma 2). If the acceleration profile is linearly decreasing, we have u_i(t)=a_it+b_i≥ 0>u_min, ∀ t ∈ [t_i^0,t_i^m], implying that the constraint u_min-u_i(t)≤0 will never be activated. Now, applying the necessary condition of optimality in (<ref>), we have ∂ v_i(t)/∂ t = a_i t + b_i=0, which yields the inflection point at time t=-b_i/a_i corresponding to the vertex of the concave parabola of (<ref>), and equal to t_i^m according to Lemma 1. As the inflection point of the concave parabola is located at t_i^m and v_min<v_i^0<v_max, we have v_i(t)>v_min, ∀ t ∈ [t_i^0,t_i^m]. This concludes the proof of the first part of Lemma 3. The second part of Lemma 3 can be proved following similar steps.




1
The sign of the constant a_i dictates the activation of the state and control constraints.


From (<ref>), we see that a_i is the slope of the acceleration profile. Based on the result from (<ref>), it is evident that the sign of a_i determines the shape of the acceleration profile which in turn determines activation criteria as stated in Lemma 3.



Remark 1.
The sign of the constant a_i provides insight for the possible state and control constraint activation.


The following result provides the condition under which certain state and control constraints never become active.

Theorem 1.
(i) The state constraint v_min-v_i(t)≤ 0 and the control constraint u_min-u_i(t)≤ 0 are not activated if v_i(t_i^0)<(p_i(t_i^m)-p_i(t_i^0))/t_i^m. (ii) The state constraint v_i(t)-v_max≤ 0 and the control constraint u_i(t)-u_max≤ 0 are not activated, if v_i(t_i^0)>(p_i(t_i^m)-p_i(t_i^0))/t_i^m. 



If v_i(t_i^0)<(p_i(t_i^m)-p_i(t_i^0))/t_i^m holds, then from Lemma 2, a_i is negative and the optimal control input u_i(t) is linearly decreasing. From Lemma 3, a decreasing optimal control input u_i(t) indicates that the state constraint v_min-v_i(t)≤ 0 and the control constraint u_min-u_i(t)≤ 0 will never be activated, which concludes the proof of the first part. The second part of Theorem 1 can be proved following similar procedure.




Remark: For negative a_i, as the speed profile is monotonically increasing, any activation of the state constraint v_i(t)-v_max≤ 0 will occur near the end of the CAV trajectory. Following the same logic, this also holds true for the state constraint v_i(t)-v_min≥ 0 activation.




 §.§ Conditions of Constraint Activation

The following results provide the condition for which specific state and control constraints are activated.

Lemma 4.
Activation of the control constraint u_i(t)-u_max≤ 0 or u_min-u_i(t)≤ 0 occurs at time t=t_i^0.



For a_i<0, there is a possibility that either the state constraint v_i(t)-v_max≤ 0, or the control constraint u_i(t)-u_max≤ 0, or both to be activated (Lemma 3). In this case, the control input linearly decreases to zero at time t=t_i^m. Therefore, any activation of the control constraint u_i(t)-u_max≤ 0 occurs at time t=t_i^0. Following similar reasoning, it can be proved that the activation of u_min-u_i(t)≤ 0 occurs at time t=t_i^0


Theorem 2.
(i) If a_i<0, the state constraint  v_i(t)-v_max≤ 0 is activated if t_i^m<3(p_i(t_i^m)-p_i(t_i^0))/v_i^0+2v_max.
(ii) If a_i>0, the state constraint v_min-v_i(t)≤ 0 is activated if t_i^m>3(p_i(t_i^m)-p_i(t_i^0))/v_i^0+2v_min.


Suppose that for a_i<0, there exists a time t_s ∈ (t_i^0,t_i^m] at which the state constraint v_i(t)-v_max≤ 0 is activated. Using (<ref>), from (<ref>) we have 1/2a_i t_s^2+b_i t_s+v_i^0=v_max.
Solving for t_s, we have t_s = -2b_i±√(4b_i^2-8a_i·(v_i^0-v_max))/2a_i, yielding two possible solutions, either t_s,1=t_i^m+ √(4b_i^2-8a_i·(v_i^0-v_max)/4a_i^2) or t_s,2=t_i^m-√(4b_i^2-8a_i·(v_i^0-v_max)/4a_i^2).
Since t_s,1 is not feasible as √(4b_i^2-8a_i·(v_i^0-v_max)) cannot be negative or equal to zero to satisfy t_s<t_i^m, t_s,2 is the only acceptable solution. To satisfy t_s<t_i^m, we have √(4b_i^2-8a_i·(v_i^0-v_max))>0 resulting in a_i<2(v_i^0-v_max)/(t_i^m)^2.
Using (<ref>), the first part of Theorem 2 follows. The second part of Theorem 2 can be proved for a_i>0 following similar arguments.
 

Theorem 3.
(i) For a_i<0, the control constraint u_i(t)-u_max≤ 0 is activated if t_i^m<-3v_i^0+√(9(v_i^0)^2+12u_max·(p_i(t_i^m)-p_i(t_i^0)))/2u_max.
(ii) For a_i>0, control constraint u_min-u_i(t)≤ 0 is activated if t_i^m<3v_i^0+√(9(v_i^0)^2+12  u_min(p_i(t_i^m)-p_i(t_i^0)))/2  u_min.


Based on Lemma 4, the control constraint activation occurs at t=t_i^0. For a_i<0, we have u_i(t_i^0)=a_it_i^0+b_i>u_max.
Without loss of generality, for t_i^0=0, we have b_i=-a_it_i^m>u_max. Substituting a_i from (<ref>), we obtain u_max·(t_i^m)^2+3v_i^0t_i^m-3(p_i(t_i^m)-p_i(t_i^0))<0.
Solving the above quadratic term for t_i^m, we obtain

    (t_i^m+3v_i^0+√(9(v_i^0)^2+12u_max·(p_i(t_i^m)-p_i(t_i^0)))/2u_max) 
      ·(t_i^m+3v_i^0-√(9(v_i^0)^2+12u_max·(p_i(t_i^m)-p_i(t_i^0)))/2u_max)<0.

The first term of (<ref>) is always positive since √(9(v_i^0)^2+12u_max·(p_i(t_i^m)-p_i(t_i^0)))>0. Therefore, the second part has to be negative, and the result follows. Following similar steps, the second part of Theorem 3 can be proved for a_i>0.


We have discussed so far the conditions under which the state and control constraints are activated individually. With this information, we can solve the constrained optimization problem and derive corresponding analytical solutions, which we will present in the following section.
However, there is a possibility that the constrained optimal solution may result in additional activation of constraint arcs, i.e., a state constrained optimal solution might cause a control constraint activation. Similarly, a control constrained optimal solution might cause a state constraint activation. In such cases, the optimization problem has to be resolved by piecing all constrained arcs together to derive the corresponding solution. To overcome this recursive procedure, we can identify a priori under which conditions whether other constraint arcs might be activated.

Theorem 4.
(i) The constrained optimal solution corresponding to the state constraint v_i(t)-v_max≤0 with a junction point at t=τ_s may result in activating the control constraint u_i(t)-u_max≤0,  if τ_s < -3v_i^0+√(9(v_i^0)^2+12u_max·(p_i(τ_s)-p_i(t_i^0)))/2u_max.
(ii) The constrained optimal solution corresponding to state constraint v_min-v_i(t)≤0 with a junction point at t=τ_s may result in activating the control constraint u_min-u_i(t)≤0, if τ_s<3v_i^0+√(9(v_i^0)^2+12  u_min(p_i(τ_s)-p_i(t_i^0)))/2  u_min.



Suppose that the state constrained (v_i(t)-v_max≤0) solution has a junction point between the constrained and unconstrained arc at t=τ_s, where u_i(τ_s)=0. Therefore, we focus on the unconstrained arc of the constrained solution with the time horizon [t_i^0,τ_s] instead of [t_i^0,t_i^m]. Following similar procedure as in the proof of Theorem 3, we can construct the condition of the first part of Theorem 4 that indicates whether the control constraint u_i(t)-u_max≤0 is being activated within the time horizon t∈ [t_i^0,τ_s]. The second part of Theorem 4 can be proved following similar arguments.


Theorem 5.
(i) The constrained optimal solution corresponding to the control constraint u_i(t)-u_max≤0 with a junction point at t=τ_c may result in activating the state constraint v_i(t)-v_max≤0, if t_i^m<τ_c + 3(p_i(t_i^m)-p_i(τ_c))/v_i^0+u_maxτ_c+2v_max.
(ii) The constrained optimal solution corresponding to the control constraint u_min-u_i(t)≤0 with a junction point at t=τ_c may result in activating the state constraint v_min-v_i(t)≤0, if t_i^m>τ_c + 3(p_i(t_i^m)-p_i(τ_c))/v_i^0+u_minτ_c+2v_min.


Suppose that the control constrained (u_i(t)-u_max≤0) solution has a junction point between the constrained and unconstrained arc at t=τ_c>t_i^0. Therefore, we focus on the unconstrained arc of the control constrained solution with the time horizon [τ_c,t_i^m] instead of [t_i^0,t_i^m]. Following similar procedure to the proof of Theorem 2, we can construct the condition stated in the first part of Theorem 5 which indicates whether the state constraint v_i(t)-v_max≤0 is being activated within the time horizon t∈ [τ_c,t_i^m]. Similar approach can be employed to prove the second part of Theorem 5.






 §.§ Existence of Constrained Solution

7
 If only the control constraint is not imposed, then a state constrained optimal solution always exists.


Suppose, the control constraint is not imposed and the state constraint is activated at time t=τ_1. If v_i^0∈[v_min,v_max], then τ_1 is bounded within [t_i^0, t_i^m]. Given the fact that control constraint is not imposed, this implies that the unconstrained control arc take any shape to enter the constrained arc at τ_1 with u_i(τ_1)-0 to ensure the realization of the state constraint.


8
 If only the state constraint is not imposed, then a control constrained optimal solution exists if,
 
    v_i^0 t_i^m + 1/2u_max(t_i^m)^2 ≤ p_i(t_i^m)-p_i(t_i^0).    a_i<0 
    
          v_i^0 t_i^m + 1/2u_min(t_i^m)^2 ≤ p_i(t_i^m)-p_i(t_i^0).    a_i>0




Let us consider the first case where the state constraint is not imposed and the control constraint with negative a_i is activated at t=t_i^0 with according to lemma 3. The maximum distance, L_max corresponding to a constrained control trajectory (i.e., u_i(t)=u_max,   for    t∈[t_i^0,t_i^m]) is,

    L_max = v_i^0 t_i^m + 1/2u_max(t_i^m)^2.

If L_max>(p_i(t_i^m)-p_i(t_i^0)), then the control constrained arc is not contained within the time horizon [t_i^0,t_i^m] and no corresponding optimal solution exists.


9
 If both state and control constraints are imposed, then there exists an optimal solution if,
 
    v_i^0+1/2u_maxτ_1^2-1/6(τ_2^3-τ_1^3)/(τ_2-τ_1)+
    1/2u_max(τ_2^2-τ_1^2)/(τ_2-τ_1)+v_i^0(τ_2-τ_1)+
    
         p_i(t_i^0)+v_i(τ_2)(τ_2-τ_1) ≤  p_i(t_i^m)-p_i(t_i^0),

 wrong, check
 where, τ_1 and τ_2 are the time where control trajectory leaves and enters the constrained arc.


If both state and control constraints are imposed.


check for ai signs





§ ANALYTICAL SOLUTION OF CONSTRAINED OPTIMIZATION
 
To derive the analytical solution of (<ref>) using Hamiltonian analysis, we adopt the standard procedure used in optimal control problems with control and state constraints <cit.>, and present a simplified framework. We first start with the result of Theorem 1 to reduce the set of possible constraint activations. Then we evaluate the conditions in Theorem 2 and 3 to identify possible constraint activations. If no activation is identified, we simply derive the unconstrained solution. If any contraint activation is detected by applying the conditions in Theorem 2 and 3, we then use the conditions in Theorem 4 and 5 to identify any additional constraint activations that might rise from the previous case. Once the nature of the current and additional constraint activations is specified, we then piece together the relevant unconstrained and constrained arcs that yield a set of algebraic equations which are solved simultaneously using the boundary conditions of and interior conditions between the arcs.





Namely, we first start with the unconstrained arc and derive the unconstrained. If the unconstrained solution violates any of the state or control constraints, then the unconstrained arc is pieced together with the arc corresponding to the activated constraint, and we re-solve the problem with the two arcs pieced together. The two arcs yield a set of algebraic equations which are solved simultaneously using the boundary conditions of and interior conditions 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 activated constraint, and we re-solve the problem with the three arcs pieced together. The process is repeated until the solution does not violate any other constraints.




Case 1:
If only the state constraint v_i(t)-v_max≤ 0 is activated, we have μ^a
_i = μ^b_i=μ^d_i=0. The corresponding necessary condition for optimality, and the Euler-Lagrangian equations (<ref>) for the costates  become u_i+λ_i^v =0, 
     λ̇_i^p=0, and
     λ̇_i^v=-λ_i^p-μ_i^c.
Let us assume that the time t=τ_s such that t_i^0<τ_s<t_i^m, the unconstrained arc enters the constrained arc. We denote τ_s^- and τ_s^+ as the immediate left and the right side of τ_s. We have an tangency condition N(t,x_i(t))=v_i(t)-v_max=0, which yields the optimal state v_i^*(t) = v_max and control u_i^*(t) = 0 on the constraint arc. Considering the jump conditions of the costates and Hamiltonian at the entry point as follows,

    λ_i(τ_s^-)-λ_i(τ_s^+)=η_i∂N(t,x_i(t))/∂x_i|_t=τ_s, 
    
         H_i(τ_s^+)-H_i(τ_s^-)=η_i∂N(t,x_i(t))/∂ t|_t=τ_s,

where, η_i is a constant Lagrange multiplier.

Note that, the state  variables remain continuous, i.e., x_i(τ_s^-)=x_i(τ_s^+). From the above equations, both the position costate and the Hamiltonian are continuous at t=τ_s. With u_i(τ_s^+)=0, (<ref>) yields u_i(τ_s^-)=u_i(τ_s^+), which implies continuity of control.
Using (<ref>), (<ref>), state and control continuity, the initial and final conditions (<ref>), and the terminal condition of co-states at t=t_i^m, we can formulate a closed form analytical solution piecing the unconstrained and constrained arcs together at t=τ_s.

Case 2:
If only the control constraint u_i(t)-u_max≤ 0 is activated, we have μ^b_i= μ^c_i=μ^d_i=0. The corresponding necessary condition for optimality, and the Euler-Lagrangian equations (<ref>) for the costates  become u_i+λ_i^v+μ_i^a =0, 
     λ̇_i^p=0, and
     λ̇_i^v=-λ_i^p.
Let us assume that at a time τ_c>t_i^0, the unconstrained arc leaves the constrained arc. We denote τ_c^- and τ_c^+ as the immediate left and the right side of τ_c. For pure control constraint case, we do not have any discontinuity of the costates and Hamiltonian at t=τ_c, i.e., λ_i(τ_c^-)=λ_i(τ_c^+) and H(τ_c^+)=H(τ_c^-). Solving these jump conditions, we have u_i(τ_c^+)=u_i(τ_c^-)=u_max implying the continuity of control input at t=τ_c.
Using (<ref>), (<ref>), state and control continuity, the initial and final conditions (<ref>), and the terminal condition of co-states at t=t_i^m, we can formulate a similar closed form solution as mentioned in case 1.

Case 3:
If both the control constraint u_i(t)-u_max≤ 0 and state constraint v_i(t)-v_max≤ 0 are activated, we can derive the analytical solution following the procedure described in the previous two cases.

The remaining constraint activation cases for only v_min-v_i(t)≤ 0, only u_min-u_i(t)≤ 0 and their combination can be derived following similar procedure as described above.





§ SIMULATION RESULTS

We validate the analytical solution of the constrained optimization problem through numerical analysis in MATLAB. We select the initial and final position as p_i(t_i^0)=0 m and p_i(t_i^m)=200 m, and the initial speed as v_i(t_i^0)=13.4 m/s. We enforce maximum speed v_max=21 m/s and maximum acceleration u_max=1.4 m/s^2. We present only the cases with the state constraint v_i(t)-v_max≤ 0 and control constraint u_i(t)-u_max≤ 0 activation.

The unconstrained optimal trajectory for two different merging time t_i^m at 10s and 20s is shown in Fig. <ref>. As stated in Lemma 2, we observe two different types of optimal trajectories. This implies that based on the terminal conditions, a CAV may speed up, or slow down optimally to satisfy the boundary conditions. We also observed that both the predefined state and control constraints (for a_i<0) are activated in Fig. <ref> (top-left and right).

We can save  computational effort for producing the unconstrained results in Fig. <ref>, if we know that any of the constraints might be activated. Once we employ Theorem 1 to reduce possible constraint activation set, we use Theorems 2 and 3 to pinpoint the specific constraint activation case. We illustrate the optimal state and control trajectories for only state constrained in Fig. <ref> (top) and only control constrained in Fig. <ref> (bottom) cases. However, the results in Fig. <ref> have to be recalculated if additional constraint activation is encountered. We can avoid the unnecessary computational effort to produce the intermediate result in Fig. <ref> based on the following observation. 

Note that, in the state constrained solution, the control constraint is activated (Fig. <ref>, top-right) which was non-existent before, as discussed in theorem 4. Similarly, we observe in Fig. <ref> (bottom-right) that the control constrained optimal trajectory creates a possibility of additional state constraint activation (Fig. <ref>, bottom-left) due to the increased speed, as discussed in Theorem 5. Hence, we need to take the additional constraint activation into account from Theorems 4 and 5, and enforce both state and control constraints if needed. The unconstrained and fully constrained state and control constrained trajectories are shown in Fig. <ref>. The four-point boundary value problem is solved in this case, and two constrained and one unconstrained arcs are pieced together to provide the optimal solution.






§ CONCLUDING REMARKS

In this paper, we presented a theoretical analysis of the decentralized optimal control framework for coordinating CAVs that was presented earlier <cit.>. We
provided the conditions under which the state and control constraints are activated along with the corresponding solutions. These conditions can provide insight about when any of the state and control constrained arc is activated, and thus can be useful for real-time implementation.
Ongoing work includes the analysis of other cases, including safety constraints, along with the conditions that a solution always exists. A potential direction of future research includes the enhancement of this analysis by considering different penetration rates of CAVs and the implications of error or delays in the communication.



IEEEtran














