{'fraction_non_alphanumeric': 0.05546740925428229, 'fraction_numerical': 0.022277628468736845, 'mean_word_length': 3.868221941992434, 'pattern_counts': {'":': 0, '<': 11, '<?xml version=': 0, '>': 7, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
Time and Energy-Optimal Lane Change Maneuvers for Cooperating Connected Automated Vehicles

We derive optimal control policies for a Connected and Automated Vehicle (CAV) cooperating with neighboring CAVs to implement a highway lane change maneuver. We optimize the maneuver time and subsequently minimize the associated energy consumption of all cooperating vehicles in this maneuver. We prove structural properties of the optimal policies which simplify the solution derivations and lead to analytical optimal control expressions. The solutions, when they exist, are guaranteed to satisfy safety constraints for all vehicles involved in the maneuver. Simulation results show the effectiveness of the proposed solution and significant performance improvements compared to maneuvers performed by human-driven vehicles.


I. INTRODUCTION
Advances in next generation transportation system technologies and the emergence of Connected and Automated Vehicles (CAVs), also known as "autonomous vehicles", have the potential to drastically improve a transportation network's performance in terms of safety, comfort, congestion reduction and energy efficiency. In highway driving, an overview of automated intelligent vehicle-highway systems was provided in [1] with more recent developments mostly focusing on autonomous car-following control [2], [3], [4]. Automating a lane change maneuver remains a challenging problem which has attracted increasing attention in recent years [5], [6], [7], [8].
The basic architecture of an automated lane-change maneuver can be divided into the strategy level and the control level [9]. The strategy level generates a feasible (possibly optimal in some sense) trajectory for a lane-change maneuver. The control level is responsible for determining how vehicles track the aforementioned trajectory. For example, [7] adopts such an architecture for an automated lane-change maneuver, but does not provide an analytical solution and assumes that there are no other vehicles in the left lane (the lane in which the controllable vehicle ends up after completing the maneuver). In [10], background vehicles are included in the left lane and the goal is to check whether there exists a lane-change trajectory or not; if one exists, the controllable vehicle will then track this trajectory. A similar approach is taken in [11] with the trajectory being updated during the maneuver based on the latest surrounding information. In  these papers, only one vehicle can be controlled during the maneuver and no analytical solutions are provided.
The emergence of CAVs brings up the opportunity for cooperation among vehicles traveling in both left and right lanes in carrying out an automated lane-change maneuver [9], [12], [13]. Such cooperation presents several advantages relative to the two-level architecture mentioned above. In particular, when controlling a single vehicle and checking on the feasibility of a maneuver depending on the state of the surrounding traffic, as in [14], [15], the maneuver may be infeasible without the cooperation of other vehicles, especially under heavier traffic conditions. In contrast, a cooperative architecture can allow multiple interacting vehicles to implement controllers enabling a larger set of maneuvers. This cooperative behavior can also improve the throughput, hence reducing the chance of congestion. Feasible, but not necessarily optimal, vehicle trajectories for cooperative multi-agent lane-changing maneuvers are derived in [16]. The case of multiple cooperating vehicles simultaneously changing lanes is considered in [17] with the requirement that all vehicles are controllable and their velocities prior to the lane change are all the same. First, vehicles with a lower priority must adjust their positions in their current lane and give way to those with a higher priority so as to avoid collisions. Then, a lane changing optimal control problem is solved for each vehicle without considering the usual safe distance constraints between vehicles. This "progressively constrained dynamic optimization" method facilitates a numerical solution to the underlying optimal control problem at the expense of some loss in performance.
Our goal is to provide an optimal solution for the maneuver in Fig. 1, in which the controlled vehicle C attempts to overtake an uncontrollable vehicle U by using the left lane to pass. In this case, the initial velocities of all vehicles can be different and arbitrary. The overall lane changing and passing maneuver consists of three steps: (i) The target vehicle C moves to the left lane, (ii) C moves faster than U (and possibly other vehicles ahead of it) while on the left lane, (iii) C moves back to the right lane. The first step is further subdivided into two parts. First, vehicle C adjusts its position in the current lane to prepare for a lane shift, while vehicles 1 and 2 in Fig. 1 cooperate to create space for C in the left lane. Next, the latitudinal lane shift of C takes place. In this paper, we limit ourselves to the first part of step (i). Our objective is to minimize both the maneuver time and the energy consumption of vehicles C, 1 and 2 which are all assumed to share their state information. We also impose a hard safe distance constraint between all adjacent vehicles located in the same lane, as well as constraints due to speed and acceleration limits imposed on all vehicles. We first determine a minimum feasible time for the maneuver (if one exists) and associated terminal positions for vehicles C, 1 and 2. We then solve a fixed terminal time decentralized optimal control problem for each of the three vehicles. We derive several properties of the optimal solution which facilitate obtaining explicit analytical solutions, hence leading to realtime implementability. Our approach applies to a wider range of scenarios relative to those in [10], [11], [14], [15] and incorporates the safety distance constraint not included in [16] and [17].
The rest of this paper is organized as follows. Section II formulates the lane-change maneuver problem. In Section III, a complete optimal control solution is obtained. Section IV provides simulation results for several representative examples and we conclude with Section V.

II. PROBLEM FORMULATION
We define x i (t) to be the longitudinal position of vehicle i along its current lane measured with respect to a given origin, where we use i = 1, 2,C,U. Similarly, v i (t) and u i (t) are vehicle i's velocity and (controllable) acceleration. The dynamics of vehicle i arė The maneuvers carried out by vehicles 1, 2,C are initiated at time t 0 and end at time t f . We define d i (v i (t)) to be the minimal safe distance between vehicle i and the one that precedes it in its lane, which in general depends on the vehicle's current speed. The control input and speed are constrained as follows for all t ∈ [t 0 ,t f ]: where u imax , u imin , v imax , v imin are the maximal and minimal acceleration (respectively speed) limits. In Fig. 1, we control vehicles 1, 2 and C to complete a lane change maneuver while minimizing the maneuver time and the corresponding energy consumption. For each vehicle i = 1, 2,C we formulate the following optimization problem assuming that x i (0) and v i (0) are given: s.t. (1), (2) and where w t , w u are weights associated with the maneuver time t f and with a measure of the total energy expended. The two terms in the previous function need to be properly normalized and we set w t = ρ T max and w i,u = 1−ρ max{u 2 imax ,u 2 imin } , where ρ ∈ [0, 1] and T max is a prespecified upper bound on the maneuver time (e.g., T max = l/ min{v imin }, i = 1, 2,C,U, where l is the distance to the next highway exit). Clearly, if ρ = 0 this problem reduces to an energy minimization problem and if ρ = 1 it reduces to minimizing the maneuver time. The safe distance is defined as d i (v i (t)) = φ v i (t) + δ where φ is the headway time (the general rule φ = 1.8 is usually adopted as in [18]). As stated, the problem allows for a free terminal time t f and terminal state constraints We will next specify the terminal time t f as the solution of a minimization problem which allows each vehicle to specify a desired "aggressiveness level" relative to the shortest possible maneuver time subject to (2). Then, we will also specify x i (t f ), i = 1, 2,C.

III. OPTIMAL CONTROL SOLUTION
Terminal time specification. We begin by formulating the following minimization problem based on which the maneuver terminal time t f is specified: where α i ∈ [0, 1), i = 1, 2,C is an "aggressiveness coefficient" for vehicle i which can be preset by the driver. Observe that To minimize t f , vehicle 1 should accelerate and vehicle 2 decelerate so as to increase the gap between them in Fig. 1. If C accelerates, then (4a) ensures the safety constraint is still satisfied. If C has to decelerate because it is constrained by U, then (4b) ensures that the safety constraint between U and C is satisfied and (4c) ensures that the safety constraint between 2 and C is also satisfied. As we will subsequently show, the optimal control of C is either always non-positive or always nonnegative throughout [0,t f ] so that either the first or the last two constraints are relevant to it. Naturally, a solution to (4) may not exist, in which case we must iterate on the values of α i until one is possibly identified. If that is not possible, then the maneuver is clearly aborted. If t f exists, we will specify terminal position x i (t f ) next and check the feasibility of (x i, f ,t f ) later in this section.
Terminal position specifications. Assuming a solution t f is determined, we next seek to specify terminal vehicle positions x i (t f ), i = 1, 2,C, to be associated with problem (3). To do so, we define which is the difference between the actual terminal position of i and its ideal terminal position under constant speed v i (t 0 ); this is ideal from the energy point of view in (5), since the energy component is minimized when u i (t) = 0. Thus, the energy-optimal value is ∆x i (t f ) = 0. We then seek terminal positions that minimize a measure of deviating form these energy-optimal values over all three vehicles: The max values in (5) are assumed to be given by a prespecified maximum inter-vehicle safe distance. However, as subsequently shown in Theorem 1, they actually turn out to be the known initial or terminal values of d 2 (v 2 (t)) and 1 (t f ) < 0, then ∆x 1 (t f ) = 0 is a better solution since it is feasible (the distance between vehicles 1, 2 under ∆x 1 (t f ) = 0 is larger than under ∆x * 1 (t f ) < 0) and it is obvious that it yields a lower cost in (5) than the one with ∆x * With the terminal time t f and longitudinal position x i (t f ), i = 1, 2, set through (4) and (5) respectively, the optimal control problems of vehicles i = 1, 2 in (3) become: where x 1, f and x 2, f are given above. In (7), we use an inequality x 2 (t f ) ≤ x 2, f to describe the terminal position constraint instead of the equality since it suffices for the distance between the two vehicles to accommodate vehicle C while at the same time allowing for the cost under a control with x 2 (t f ) < x 2, f to be smaller than under a control with (6), there is no need to consider the case that x 1 (t f ) > x 1, f since it is clear that the optimal cost when The next result establishes the fact that the solution of these two problems involves vehicle 1 never decelerating and vehicle 2 never accelerating. Theorem 1 The optimal control in (6) is u * 1 (t) ≥ 0 and the optimal control in (7) is not feasible. The same applies to u 2 (t) ≤ 0 being a feasible solution of (7).
Starting with vehicle 1, suppose that there exists some [t 1 ,t 2 ) ⊂ [0,t f ] in which the optimal solution satisfies u * 1 (t) < 0. We will show that there exists another control which would lead to a smaller cost than u * 1 (t). Consider a control u 1 . It is obvious that the cost of the control , thus violating the terminal condition in (6). Therefore, we construct another control u 2 (8). Thus, the terminal position constraint is not violated under u 2 1 (t). Based on the definitions of u 2 1 (t) and u 1 1 (t), it is obvious that u 2 1 (t) does not violate the acceleration constraints in (2). Next, we show that the velocity constraints in (2) are also not violated. Assume that for some t n , v 1 1 (t n ) = v 1max initiating an arc where the velocity is v 1 1 (t) = v 1max . There are two cases: Based on the definition of u 2 1 (t), the maximal speed under the control u 2 1 (t) is v 1 1 (t m ) and the velocity constraint is, therefore, inactive.
where the equality follows from the definition of t m above. Then, let us construct a new control u 3 (9). We conclude that t n < t m is not possible. In summary, we have shown that the velocity constraint is inactive for control u 2 1 (t). Therefore, u 2 1 (t) is feasible and results in a lower cost in (6) than u * 1 (t) since it includes a trajectory arc over which u 2 1 (t) = 0. This contradicts the optimality of u * 1 (t) and we conclude that the optimal control cannot contain any interval over which u * 1 (t) < 0. Next, consider vehicle 2 and suppose that there exists some . It is clear that the cost under u 1 2 (t) is lower than that of u * 2 (t) and that the acceleration constraint in (2) Therefore, the terminal position inequality in (7) is not violated. Based on the definition of the safety distance constraint, Finally, we consider the speed constraint in (2) which may be active under u 1 2 (t). There are two cases: is a feasible solution which results in a lower cost in (7) than u * 2 (t) since it includes a trajectory arc over which u 2 , that is, the terminal position inequality is satisfied. Also, it is obvious that the acceleration and the speed constraints are not violated over [0,t f ]. Finally, we have shown that u 1 2 (t) does not violate the safety constraint. Based on the same argument, it is straightforward to show that u 2 2 (t) will not violate this constraint, since (7) and the corresponding cost is lower than that of u * 2 (t) because the trajectory segment with u 2 2 (t) = 0 contributes to zero cost. We conclude that the optimal control u * 2 (t) cannot contain any time interval with u * 2 (t) > 0. Based on Theorem 1, in addition to showing that vehicle 1 never decelerates and vehicle 2 never accelerates, we also eliminate the safe distance constraint in (7) since the distance between the vehicles will increase in the course of the maneuver and the last two safety constraints in (3) ensure that this distance is eventually large enough to accommodate the length of vehicle C. Thus, (7) becomes min u 2 (t) Feasible terminal state set. The constraints in (2) limit the sets of feasible terminal conditions (x i, f ,t f ), i = 1, 2,C as shown in Fig. 2 where the feasible set is the unshaded area defined as follows for each i = 1, 2,C: 2u imin . In addition, vehicle C must also satisfy a safety distance constraint with respect to vehicle U, hence if Note that if an optimal t f is determined in (4) and the solution of (5) guarantees that x i (t f ), i = 1, 2,C, do not violate the safety constraints, (x i, f ,t f ) is expected to be feasible. However, if (x i, f ,t f ) is infeasible for vehicle i, then the following algorithm is used to find a feasible such pair: Algorithm 1: (1) t f is updated using t f = βt f , β > 1.
(2) With updated t f , (5) is re-solved to obtain new x i, f . Fig. 2, stop; else return to step (1) with a higher value of β .
In the above, the coefficient β is used to relax the maneuver time t f so as to accommodate one or more of the constraints in Fig. 2 Solution of problem (6). We can now proceed to derive an explicit solution for (6) taking advantage of Theorem 1. We begin by writing the Hamiltionian and associated Lagrangian functions for (6): , λ x (t)] T and η = [η 1 (t), ..., η 4 (t)] T . In view of Theorem 1, i.e., u * 1 (t) ≥ 0, (12) reduces to The explicit solution of (6) is given next.
be a solution of (6). Then, Proof : Problem (6) is of the same form as the fixed terminal time optimal control Problem 3 in [19] whose solution when u * 1 (t) ≥ 0 is given in Theorem 2 of [19] and is therefore omitted. By Pontryagin's principle applied to (11), u * 1 (t) =min{u 1 max , −λ v (t)} and the key parts of the proof in [19] are showing that η 3 (t) = 0 and that λ v (t) is continuous for all t ∈ [0,t f ].
Furthermore, following a derivation similar to that in [19] we can obtain the optimal cost J * 1 (t f ) in (6) based on several cases depending on the initial acceleration u * 1,0 and the terminal velocity v * 1 (t f ) which can be explicitly evaluated as in [19]. The final optimal cost is the minimal among all possible values obtained.
Case I: u * 1,0 = u 1max andu * Otherwise, when v 1 (t) = v 1max , the control switches to u * 1 (t) = 0. Therefore, Case II: u * 1,0 = u 1max and v * 1 (t f ) = v 1max . We define t 1 as the time that u * 1 (t) begins to decrease and τ as the first time that u * 1 (τ) = 0. Thus, u * 1 (t) is a piecewise linear function of time t and (following calculations similar to those in [19]): Using similar calculations, we summarize below the remaining three cases: Solution of problem (10). Similar to the solution of (6), we can derive an explicit solution for (10) taking advantage of Theorem 1 and obtain the following result.
Proof : Problem (7) is also of the same form as the fixed terminal time optimal control Problem 3 in [19] whose solution when u * 2 (t) ≤ 0 is given in Theorem 3 of [19] and is therefore omitted.
We can also obtain the optimal cost J * 2 (t f ) in (7) based on several cases depending on the initial acceleration u * 2,0 and the terminal velocity v * 2 (t f ) which can be explicitly evaluated as in [19]. In what follows, we define t 1 as the time that u * 2 (t) begins to increase and τ as the first time that u * 2 (τ) = 0.

B. Optimal Control of Vehicle C
Unlike (6) and (10), deriving the optimal control of vehicle C as in Fig. 1 is more challenging. First, since we need to keep a safe distance between vehicles C and U, a constraint The resulting problem formulation is: in which d C (v C (t)) is time-varying. To simplify (18), we use , which is a more conservative constraint still ensuring that the original one is not violated (the problem with d C (v C (t)) = φ v C (t) + δ can still be solved at the expense of added complexity and is the subject of ongoing research). The Hamiltonian for (18) with the constraints adjoined yields the Lagrangian (20) when none of the constraints is active along an optimal trajectory. In order to account for the constraints becoming active, we identify several cases depending on the terminal states of vehicles U and C. Let us definex C (t f ) to be the terminal position of C if u C (t) = 0 for all t ∈ [0,t f ].
The relationship betweenx C (t f ) and x C (t f ) is critical. In particular, ifx C (t f ) < x C (t f ), vehicle C must accelerate in order satisfy the terminal position constraint. Otherwise, C must decelerate. Also critical is the value of x U (t f ) − d C , i.e., the upper bound of the safe terminal position of C. In addition, during the entire maneuver process, we require that We begin with the 3! cases for ordering x C (t f ),x C (t f ) and x U (t f ) − d C . Fortunately, we can exclude several cases as infeasible because x C (t f ) ≤ x U (t f ) − d C is a necessary condition to have feasible solutions. This leaves three remaining cases as follows.
These are visualized in Fig. 3. The following results provide structural properties of the optimal solution (20) depending on which case applies.
Proof : Assume that at time t k , we have therefore, the safety constraint is violated at t + k . We conclude that v C (t k ) = v U which completes the proof.
Theorem 4 [Case 1 in Fig. 3]: in which the optimal solution satisfies u * C (t) < 0. We will show that there exists another control which would lead to a lower cost than u * C (t). First, we construct a control u 1 . It is clear that u 1 C (t) will not violate the acceleration constraint (2). However, the terminal position constraint is violated. Therefore, we will construct u 2 C (t), a variant of u 1 C (t) as follows, and will show that u 2 C (t) is feasible. First, define and note that g C (t) is continuous in t since x 1 C (t) and v 1 Moreover, based on its definition, it is obvious that it will not violate the acceleration constraint. Next, we show that the velocity constraint is also not violated. Suppose there exists some time t n such that v 1 C (t n ) = v Cmax so that the trajectory may include an arc over which v 1 C (t) =v Cmax . There are two cases: , the maximal speed is v 1 C (t m ) and the velocity constraint is not violated.
We then construct a control u 3 . We conclude that t n < t m is not possible. In summary, we have proved that the speed constraint will not be violated under the control u 2 C (t). Next, we show that u 2 C (t) will also not violate the safety constraint. Suppose that at time t σ ∈ [0,t f ], the safety constraint is active under control u 2 C (t), i.e., x 2 C (t σ ) = x U (t σ ) − d C . Because u 2 C (t) ≥ 0, based on Lemma 2, it is straightforward to show that Recall that, based on the definition of u 2 C (t) and the condition that contradicts with (23). Therefore, the safety constraint will never be activated.
We conclude that u 2 C (t) is a feasible solution. Moreover, under u 2 C (t) the cost is lower than that of u * C (t) because u 2 C (t) contains a segment with u 2 C (t) = 0 that contributes zero cost in (18) relative to u * C (t). Therefore, the optimal control u * C (t) cannot contain any time interval with u * C (t) < 0. Finally, we use a similar argument as above to show that the safety constraint will be inactive under the optimal control u * C (t), that is, η * 5 (t) = 0. Assume that at time t η ∈ (0,t f ], η * 5 (t η ) > 0. Because the safety constraint is active at t η and is not violated at t f , vehicle C must have decelerated to relax the safety constraint. However, this violates the fact that u * C (t) ≥ 0 as shown above. Therefore, we conclude that η * 5 (t) = 0 for al t ∈ [0,t f ]. This completes the proof. Theorem 5 [Case 2 in Fig. 3]: The proof is similar to that of Theorem 4 and is omitted.
Theorem 6 [Case 3 in Fig. 3] The proof is similar to Theorem 4. The only difference is in the way we prove that the constructed control u 2 C (t) will not violate the safety constraint. Suppose that there exists some [t 1 ,t 2 ) ⊂ [0,t f ] in which the optimal solution satisfies u * C (t) > 0. First, we construct a control u 1 Considering the safety constraint in (18), note that if u * C (t) does not violate the safety constraint, then neither does u 1 C (t). Using g C (t) defined in (21), note that g C (0) = x 1 will not violate the safety constraint when t ≤ t m . For t > t m , we have u 2 will not violate the safety constraint because the upper bound of vehicle C's safe position, x U (t) − d C , is also linear in t. Based on the definition of u 2 C (t), it is obvious that it will not violate the acceleration constraint. We can then use the same argument as in the proof of Theorem 4 to show that v C (t m ) ≥ v C min . Therefore, u 2 C (t) is a feasible solution. It is also obvious that the cost of u 2 C (t) is lower than that of u * C (t) because u 2 C (t) contains a segment with u 2 C (t) = 0. Therefore, the optimal control u * C (t) cannot contain any time interval with u * C (t) > 0. This completes the proof. Based on Theorems 4,5, Cases 1,2 in Fig. 3 can be solved without the safety constraint in (18) since we have shown that η * 5 (t) = 0. Therefore, the optimal control is the same as that derived for vehicles 1 and 2 in Theorems 2,3. This leaves only Case 3 to analyze. We proceed by first solving (18) without the safety constraint, so it reduces to the solution in Theorem 3, since we know that u * C (t) ≤ 0. If a feasible optimal solution exists, then the problem is solved. Otherwise, we need to re-solve the problem in order to determine an optimal trajectory that includes at least one arc in which it is easy to see that there is at most one such constrained arc, since v C (t) = v U (0) as soon as this arc is entered.) We then split problem (18) into two subproblems as follows: where (24) has a fixed terminal time τ 1 (to be determined), position a, and speed v U (0), while (25) has a fixed terminal time t f and position x C, f with given x C (τ 1 ) = a.
Let us first solve (24). Since u * C (t) ≤ 0 and the terminal speed is v U (0), only the acceleration constraint u C min − u C ≤ 0 can be active in [0, τ 1 ]. Suppose that this constraint becomes active at time τ 2 < τ 1 . Since u C min −u C is independent of t, x C (t), and v C (t), it follows (see [20]) that there are no discontinuities in the Hamiltonian or the costates, i.e., (20). Either condition used in the above equation leads to the conclusion that u * C (τ − 2 ) = u * C (τ + 2 ), i.e., u * C (t) is continuous at τ 2 .
Let us now evaluate the objective function in (24) as a function of τ 1 and a, denoting it by J 1 (τ 1 , a), under optimal control. In view of (20), there are two cases.
(a) u * C (t) = u Cmin for t ∈ [0, τ 2 ), u * C (t) = −λ v (t) for t ∈ [τ 2 , τ 1 ]. As in the proof of Theorem 2, the costate equations areλ v (t) = −λ x (t) andλ x (t) = 0. Therefore, λ v (t) = ct − b where b, c are to be determined. It follows that and the following boundary conditions hold: Using (26) and (27) to eliminate c and τ 2 and then evaluate J 1 (τ 1 , a) in (24) after some algebra yields: and, after some calculations, we obtain J 1 (τ 1 , a) in (24): Proceeding to the second subproblem (25), note that the control at the entry point of the constrained arc at time τ 1 is no longer guaranteed to be continuous. This problem is of the same form as the optimal control problem for vehicle 2 in (10) whose solution is given in Theorem 3, except that initial conditions now apply at time τ 1 as given in (25). Proceeding exactly as before, we can obtain the cost J 2 (τ 1 , a) under optimal control. Adding the two costs, we obtain J C (τ 1 , a) = J 1 (τ 1 , a) + J 2 (τ 1 , a) in (18). This results in a simple nonlinear programming problem whose solution (τ * 1 , a * ) results from setting 0. Finally, the optimal control is the one corresponding to (τ * 1 , a * ).
Based on our analysis, we find that Case 3 is the only one where the safety constraint may become active. This provides an option to the vehicle C controller: if Case 3 applies, the maneuver may either be implemented or it may be delayed until the conditions change to either one of Cases 1,2 so as avoid the more complex situation that arises through (24),(25).

IV. SIMULATION RESULTS
We provide simulation results illustrating the time and energy-optimal optimal maneuver controller we have derived and compare its performance to a baseline of human-driven vehicles. In what follows, we set the minimal and maximal vehicle speeds to 1m/s and 33m/s respectively and the maximal acceleration and deceleration to 3.3m/s 2 and −7m/s 2 respectively. The aggressiveness coefficients α i , i = 1, 2,C in (4) are all set to α i = 0.5.
Case 1 in Fig. 3. We set (4), we get t f = 28.14s and after solving (5), we obtain x 1 (t f ) = 455.8m, x C (t f ) = 303.24m and x 2 (t f ) = 273.24m. Figs. 4-5 show the optimal trajectories of all controllable vehicles. In Fig.  4, vehicle 1 is cruising with a constant velocity which contributes a zero value to the cost in (6), while the velocity of vehicle 2 decreases to create space for vehicle C to change lanes. The optimal trajectory of vehicle C in Fig. 5 is obtained without considering the safety constraint because of Theorem 4. Vehicle C keeps on accelerating and the safety distance constraint is never violated.   Fig. 3. We set x 1 (0) = 70m, v 1 (0) = 13m/s, x 2 (0) = 30m, v 2 (0) = 18m/s, x C (0) = 13m, v C (0) = 12m/s, x U (0) = 80m, v U (0) = 10m/s. Solving (4) and (5), we get t f = 21.4s and x 1 (t f ) = 348.37m, x 2 (t f ) = 214.13m, x C (t f ) = 244.13m. Figure 6 shows the optimal trajectories of vehicles 1,2 in which 1 is cruising with a constant speed and the associated energy cost is zero, while the velocity of vehicle 2 decreases. Figure 7 shows the optimal trajectory of vehicle C which, once again, is obtained without considering the safety constraint based on Theorem 5. Vehicle C decelerates to ensure it satisfies its terminal position while the safety constraint is never violated.   Fig. 3. We set x 1 (0) = 40m, v 1 (0) = 11m/s, x U (0) = 40m, v U (0) = 8m/s x 2 (0) = 10m, v 2 (0) = 23m/s, x C (0) = 13m, v C (0) = 19m/s. Solving (4) and (5), we get t f = 14.49s and x 1 (t f ) = 199.37m, x 2 (t f ) = 75m, x C (t f ) = 105.9m. The optimal trajectories of vehicles 1,2 are shown in Fig. 8. In this case, vehicle 1 accelerates and vehicle 2 decelerates in order to create space for vehicle C. For vehicle C, we first solve the optimal control problem (18) without considering the safety constraint and find that it actually becomes active. Therefore, we proceed with the two subproblems (24) and (25) to derive the true optimal trajectories. We obtained a * = 43m and τ * 9 shows the optimal trajectory of vehicle C. Observe that  C decelerates over the maneuver and the safety distance constraint is active at τ * 1 = 3.2s when there is a jump in the acceleration trajectory. Following that, vehicle C continues decelerating until it reaches its terminal position.

Case 3 in
Comparison of optimal maneuver control and humandriven vehicles. We use standard car-following models in the commercial SUMO simulator to simulate a lane change maneuver implemented by human-driven vehicles with the requirement that vehicle C changes lanes between vehicles 1 and 2. We considered all cases in Fig. 3 with both CAVs and human-driven vehicles sharing the same initial states as shown in Table I. The associated energy consumption is shown in Table II and provides evidence of savings in the range 43 − 59% over all three cases.

V. CONCLUSION AND FUTURE WORK
We used an optimal control framework to derive time and energy-optimal policies for a CAV cooperating with neighboring CAVs to implement a highway lane change maneuver. We optimize the maneuver time and subsequently minimize the associated energy consumption of all cooperating vehicles in this maneuver. Our solution is limited to the first step of the complete maneuver, i.e., all three cooperating vehicles adjust their positions before the lane-changing vehicle makes the lane shift. Our ongoing work aims to complete this step. In addition, we plan to incorporate a "comfort" factor in the problem by minimizing any resulting jerk and adopt a more general velocity-varying safety distance constraint.