Optimal Path Planning for Connected and Automated Vehicles at Urban Intersections
OOptimal Path Planning for Connected and Automated Vehicles atUrban Intersections
Andreas A. Malikopoulos,
Senior Member, IEEE , and Liuhui Zhao,
Member, IEEE
Abstract — In earlier work, a decentralized optimal controlframework was established for coordinating online connectedand automated vehicles (CAVs) at urban intersections. Thepolicy designating the sequence that each CAV crosses theintersection, however, was based on a first-in-first-out queue,imposing limitations on the optimal solution. Moreover, no lanechanging, or left and right turns were considered. In this paper,we formulate an upper-level optimization problem, the solutionof which yields, for each CAV, the optimal sequence and laneto cross the intersection. The effectiveness of the proposedapproach is illustrated through simulation.
I. I
NTRODUCTION
We are currently witnessing an increasing integrationof our energy, transportation, and cyber networks, which,coupled with the human interactions, is giving rise to a newlevel of complexity in the transportation network. As wemove to increasingly complex emerging mobility systems,new control approaches are needed to optimize the impacton system behavior of the interplay between vehicles atdifferent transportation scenarios, e.g., intersections, mergingroadways, roundabouts, speed reduction zones. These scenar-ios along with the driver responses to various disturbances[1] are the primary sources of bottlenecks that contribute totraffic congestion [2].An automated transportation system [3] can alleviate con-gestion, reduce energy use and emissions, and improve safetyby increasing significantly traffic flow as a result of closerpacking of automatically controlled vehicles in platoons. Oneof the very early efforts in this direction was proposed in1969 by Athans [4] for safe and efficient coordination ofmerging maneuvers with the intention to avoid congestion.Varaiya [5] has discussed extensively the key features of anautomated intelligent vehicle-highway system and proposeda related control system architecture.Connected and automated vehicles (CAVs) provide themost intriguing opportunity for enabling decision makers tobetter monitor transportation network conditions and makebetter operating decisions to improve safety and reducepollution, energy consumption, and travel delays. Severalresearch efforts have been reported in the literature oncoordinating CAVs at at different transportation scenarios,e.g., intersections, merging roadways, roundabouts, speed
This research was supported in part by ARPAE’s NEXTCAR programunder the award number de-ar0000796 and by the Delaware Energy Institute(DEI).The authors are with the Department of Mechanical Engineering,University of Delaware, Newark, DE 19716 USA (email: [email protected]; [email protected]. reduction zones. In 2004, Dresner and Stone [6] proposed theuse of the reservation scheme to control a single intersectionof two roads with vehicles traveling with similar speed on asingle direction on each road. Since then, several approacheshave been proposed [7], [8] to maximize the throughputof signalized-free intersections including extensions of thereservation scheme in [6]. Some approaches have focusedon coordinating vehicles at intersections to improve traveltime [9]. Other approaches have considered minimizing theoverlap in the position of vehicles inside the intersection,rather than arrival time [10]. Kim and Kumar [11] proposedan approach based on model predictive control that allowseach vehicle to optimize its movement locally in a distributedmanner with respect to any objective of interest. A detaileddiscussion of the research efforts in this area that have beenreported in the literature to date can be found in [12].In earlier work, a decentralized optimal control frameworkwas established for coordinating online CAVs in differenttransportation scenarios, e.g., merging roadways, urban in-tersections, speed reduction zones, and roundabouts. Theanalytical solution without considering state and control con-straints was presented in [13], [14], and [15] for coordinatingonline CAVs at highway on-ramps, in [16] at two adjacentintersections, and in [17] at roundabouts. The solution of theunconstrained problem was also validated experimentally atthe University of Delaware’s Scaled Smart City using 10CAV robotic cars [18] in a merging roadway scenario. Thesolution of the optimal control problem considering stateand control constraints was presented in [19] at an urbanintersection.However, the policy designating the sequence that eachCAV crosses the intersection in the aforementioned ap-proaches, was based on a first-in-first-out queue, imposinglimitations on the optimal solution. Moreover, no lane chang-ing, or left and right turns were considered. In this paper, weformulate an upper-level optimization problem, the solutionof which yields, for each CAV, the optimal sequence and laneto cross the intersection. The effectiveness of the solution isillustrated through simulation.The structure of the paper is organized as follows. InSection II, we formulate the problem of vehicle coordinationat an urban intersection and provide the modeling framework.In Section III, we briefly present the analytical, closed formsolution for the low-level optimization problem. In SectionIV, we present the upper-level optimization problem thesolution of yields, for each CAV, the optimal sequence andlane to cross the intersection. Finally in Section V, wevalidate the effectiveness of the solution through simulation. a r X i v : . [ m a t h . O C ] S e p erging ZoneControl Zone R l R r S m S m S c S c S c S c W e s t E n t r y Ex i t Ea s t E n t r y Ex i t North
Entry Exit
South
Exit Entry
Fig. 1. A signalized-free intersection.
We offer concluding remarks in Section VI.II. P
ROBLEM F ORMULATION
A. Modeling Framework
We consider CAVs at a 100% penetration rate crossinga signalized-free intersection (Fig. 1). The region at thecenter of the intersection, called merging zone , is the area ofpotential lateral collision of the vehicles. The intersection hasa control zone inside of which the CAVs can communicatewith each other and with the intersection’s crossing protocol .The crossing protocol , defined formally in the next subsec-tion, stores the vehicles’ path trajectories from the time theyenter until the time they exit the control zone. The distancefrom the entry of the control zone until the entry of themerging zone is S c and, although it is not restrictive, weconsider to be the same for all entry points of the controlzone. We also consider the merging zone to be a squareof side S m (Fig. 1). Note that the length S c could be in theorder of hundreds of m depending on the crossing protocol’scommunication range capability, while S m is the length of atypical intersection. The CAVs crossing the intersection canalso make a right turn of radius R r , or a left turn of radius R l (Fig. 1). The intersection’s geometry is not restrictive inour modeling framework, and is used only to determine thetotal distance travelled by each CAV inside the control zone.Let N ( t ) = { , . . . , N ( t ) } , N ( t ) ∈ N , be the set of CAVsinside the control zone at time t ∈ R + . Let t fi be the assignedtime for vehicle i to exit the control zone. There is a numberof ways to assign t fi for each vehicle i . For example, wemay impose a strict first-in-first-out queuing structure [19],where each CAV must exit the control zone in the same orderit entered the control zone. The policy, which determinesthe time t fi that each vehicle i exits the control zone, is the result of an upper-level optimization problem and canaim at maximizing the throughput of the intersection. Onthe other hand, deriving the optimal control input (minimumacceleration/deceleration) for each vehicle i from the time t i it enters the control zone to achieve the target t fi can aim atminimizing its energy [20].In what follows, we present a two-level, joint optimizationframework: (1) an upper level optimization that yields foreach CAV i ∈ N ( t ) with a given origin (entry of thecontrol zone) and desired destination (exit of the controlzone) the sequence that will be exiting the control zone,namely, (a) minimum time t fi to exit the control zone and(b) optimal path including the lanes that each CAV shouldbe occupying while traveling inside the control zone; and (2)a low-level optimization that yields, for CAV i ∈ N ( t ) , itsoptimal control input (acceleration/deceleration) to achievethe optimal path and t fi derived in (1) subject to the state,control, and safety constraints.The two-level optimization framework is used by eachCAV i ∈ N ( t ) as follows. When vehicle i enters the controlzone at t i , it accesses the intersection’s crossing protocol that includes the path trajectories, defined formally in thenext subsection, of all CAVs inside the control zone. Then,vehicle i solves the upper-level optimization problem andderives the minimum time t fi to exit the control zone alongwith its optimal path including the appropriate lanes that itshould occupy. The outcome of the upper-level optimizationproblem becomes the input of the low-level optimizationproblem. In particular, once the CAV derives the minimumtime t fi , it derives its minimum acceleration/decelerationprofile, in terms of energy, to achieve the exit time t fi .The implications of the proposed optimization frameworkare that CAVs do not have to come to a full stop at the in-tersection, thereby conserving momentum and energy whilealso improving travel time. Moreover, by optimizing eachvehicle’s acceleration/deceleration, we minimize transientengine operation [21], and thus we have additional benefitsin fuel consumption. B. Vehicle Model, Constraints, and Assumptions
In our analysis, we consider that each CAV i ∈ N ( t ) isgoverned by the following dynamics ˙ p i = v i ( t )˙ v i = u i ( t )˙ s i = ξ i · ( v k ( t ) − v i ( t )) (1)where p i ( t ) ∈ P i , v i ( t ) ∈ V i , and u i ( t ) ∈ U i denote theposition, speed and acceleration/deceleration (control input)of each vehicle i inside the control zone at time t ∈ [ t i , t fi ] ,where t i and t fi are the times that vehicle i enters and exitsthe control zone respectively; s i ( t ) ∈ S i , with s i ( t ) = p k ( t ) − p i ( t ) , denotes the distance of vehicle i from theCAV k ∈ N ( t ) which is physically immediately ahead of i in the same lane, and ξ i is a reaction constant of vehicle i . The sets P i , V i , U i , and S i , i ∈ N ( t ) , are complete andtotally bounded subsets of R .et x i ( t ) = [ p i ( t ) v i ( t ) s i ( t )] T denote the state of eachvehicle i taking values in X i = P i × V i × S i , with initialvalue x i ( t i ) = x i = (cid:2) p i v i s i (cid:3) T , where p i = p i ( t i ) = 0 , v i = v i ( t i ) , and s i = s i ( t i ) at the entry of the control zone.The state space X i for each vehicle i is closed with respect tothe induced topology on P i ×V i ×S i and thus, it is compact.We need to ensure that for any initial state ( t i , x i ) and everyadmissible control u ( t ) , the system (1) has a unique solution x ( t ) on some interval [ t i , t fi ] . The following observationsfrom (1) satisfy some regularity conditions required both onthe state equations and admissible controls u ( t ) to guaranteelocal existence and uniqueness of solutions for (1): a) thestate equations are continuous in u and continuously differ-entiable in the state x , b) the first derivative of the stateequations in x , is continuous in u , and c) the admissiblecontrol u ( t ) is continuous with respect to t .To ensure that the control input and vehicle speed arewithin a given admissible range, the following constraintsare imposed. u i,min ≤ u i ( t ) ≤ u i,max , and (2) < v min ≤ v i ( t ) ≤ v max , ∀ t ∈ [ t i , t fi ] , (3)where u i,min , u i,max are the minimum deceleration andmaximum acceleration for each vehicle i ∈ N ( t ) , and v min , v max are the minimum and maximum speed limitsrespectively.To ensure the absence of rear-end collision of two con-secutive vehicles traveling on the same lane, the positionof the preceding vehicle should be greater than or equal tothe position of the following vehicle plus a predefined safedistance δ i ( t ) . Thus we impose the rear-end safety constraint s i ( t ) = ξ i · ( p k ( t ) − p i ( t )) ≥ δ i ( t ) , ∀ t ∈ [ t i , t fi ] . (4)We consider constant time headway instead of constantdistance that each vehicle should keep when following theother vehicles, thus, the minimum safe distance δ i ( t ) isexpressed as a function of speed v i ( t ) and minimum timeheadway between vehicle i and its preceding vehicle k ,denoted as ρ i . δ i ( t ) = γ i + ρ i · v i ( t ) , ∀ t ∈ [ t i , t fi ] , (5)where γ i is the standstill distance (i.e., the distance betweentwo vehicles when they both stop).A lateral collision can occur if a vehicle j ∈ N ( t ) cruisingon a different road from i inside the merging zone. In thiscase, the lateral safety constraint between i and j is s i ( t ) = ξ i · ( p j,i ( t ) − p i ( t )) ≥ δ i ( t ) , ∀ t ∈ [ t i , t fi ] , (6)where p j,i ( t ) is the distance of vehicle j from the entry pointthat vehicle i entered the control zone. Definition 1.
The set of all lanes at the roads of theintersection is denoted by L := { , . . . , M } , M ∈ N . Definition 2.
For each vehicle i ∈ N ( t ) , the function l i ( t ) :[ t i , t fi ] → L yields the lane the vehicle i occupies inside thecontrol zone at time t . Definition 3.
For each vehicle i ∈ N ( t ) , the pair of thecardinal point that the vehicle enters the control zone andthe cardinal point that the vehicle exits the control zone isdenoted by o i .For example, based on Definition 3, for a vehicle i thatenters the control zone from the West entry (Fig. 1) and exitsthe control zone from the South exit, o i = ( W, S ) . Definition 4.
For each vehicle i ∈ N ( t ) , the function t p i ,l i (cid:0) p i ( t ) , l i ( t ) (cid:1) : P i × L → [ t i , t fi ] , is called the pathtrajectory of vehicle i , and it yields the time when vehicle i is at the position p i ( t ) inside the control zone and occupieslane l i ( t ) . Definition 5.
The intersection’s crossing protocol is denotedby Π( t ) and includes the following information Π( t ) := { t p i ,l i (cid:0) p i ( t ) , l i ( t ) (cid:1) , l i ( t ) , o i , t i , t fi } , (7) ∀ i ∈ N ( t ) , t ∈ R + . Remark 1.
The vehicles traveling inside the control zonecan change lanes either (1) in the lateral direction (e.g.,move to a neighbor lane), or (2) when making a right (or aleft) turn inside the merging zone. In the former case, whenthe vehicle changes lane it travels along the hypotenuse dy of the triangle created by the width of the lane and thelongitudinal displacement dp if it had not changed lane.Thus, in this case, the vehicle travels an additional distancewhich is equal to the difference between the hypotenuse dy and the longitudinal displacement dp , i.e., dy − dp . Remark 2.
When a vehicle is about to make a right turn itmust occupy the right lane of the road before it enters themerging zone. Similarly, when a vehicle is about to makea left turn it must occupy the left lane before it enters themerging zone.
In the modeling framework presented above, we imposethe following assumptions:
Assumption 1.
The vehicle’s additional distance dy − dp traveled when it changes lanes in the lateral direction can beneglected. Assumption 2.
Each CAV i ∈ N ( t ) has proximity sensorsand can communicate with other CAVs and the crossingprotocol without any errors or delays.The first assumption can be justified since we consider anintersection and the speed limit inside the control zone isrelatively low, hence dy ≈ dp . The second assumption maybe strong, but it is relatively straightforward to relax it aslong as the noise in the communication, measurements anddelays are bounded. In this case, we can determine upperbounds on the state uncertainties as a result of sensing orcommunication errors and delays, and incorporate these intomore conservative safety constraints.When each vehicle i with a given o i enters the controlzone, it accesses the intersection’s crossing protocol andsolves two optimization problems: (1) an upper-level op-timization problem, the solution of which yields its pathrajectory t p i ,l i (cid:0) p i ( t ) , l i ( t ) (cid:1) and the minimum time t fi toexit the control zone; and (2) a low-level optimizationproblem, the solution of which yields its optimal controlinput (acceleration/deceleration) to achieve the optimal pathand t fi derived in (1) subject to the state, control, and safetyconstraints.We start our exposition with the low-level optimizationproblem, and then we discuss the upper-level problem.III. L OW - LEVEL OPTIMIZATION
In this section, we consider that the solution of the upper-level optimization problem is given, and thus, the minimumtime t fi for each vehicle i ∈ N ( t ) is known, and we focuson a low-level optimization problem that yields for eachvehicle i the optimal control input (acceleration/deceleration)to achieve the assigned t fi subject to the state, control, andsafety constraints. Problem 1.
Once t fi is determined, the low-level problemfor each vehicle i ∈ N ( t ) is to minimize the cost functional J i ( u ( t )) , which is the L -norm of the control input in [ t i , t fi ]min u ( t ) ∈ U i J i ( u ( t )) = 12 (cid:90) t fi t i u i ( t ) dt, (8) subject to : (1) , (2) , (3) , (4) , and given t i , v i , t fi , p i ( t i ) , p i ( t fi ) , where p i ( t i ) = 0 , while the value of p i ( t fi ) for each i ∈ N ( t ) depends on o i and, based on Assumption 1, can take thefollowing values (Fig. 1): (1) p i ( t fi ) = 2 S c + S m , if theCAV crosses the merging zone, (2) p i ( t fi ) = 2 S c + πR r , ifthe CAV makes a right turn at the merging zone, and (3) p i ( t fi ) = 2 S c + πR l , if the CAV makes a left turn at themerging zone. For the analytical solution of (8), we formulate the Hamil-tonian H i (cid:0) t, p i ( t ) , v i ( t ) , s i ( t ) , u i ( t ) (cid:1) = 12 u i ( t ) i + λ pi · v i ( t ) + λ vi · u i ( t ) + λ si · ξ i · ( v k ( t ) − v i ( t ))+ µ ai · ( u i ( t ) − u max ) + µ bi · ( u min − u i ( t ))+ µ ci · u i ( t ) − µ di · u i ( t )+ µ si · ( ρ i · u i ( t ) − ξ i (cid:0) v k ( t ) − v i ( t ) (cid:1) ) , (9)where λ pi , λ vi , and λ si are the influence functions [22], and µ T is the vector of the Lagrange multipliers. To addressthis problem, the constrained and unconstrained arcs will bepieced together to satisfy the Euler-Lagrange equations andnecessary condition of optimality.For the case that none of the state and control constraintsbecome active, the optimal control is [23] u ∗ i ( t ) = ( a i − b i · ξ i ) · t + c i , t ∈ [ t i , t fi ] . (10)Substituting the last equation into (1) we find the optimal speed and position for each vehicle, namely v ∗ i ( t ) = 12 ( a i − b i · ξ i ) · t + c i · t + d i , t ∈ [ t i , t fi ] , (11) p ∗ i ( t ) = 16 ( a i − b i · ξ i ) · t + 12 c i · t + d i · t + e i , (12) t ∈ [ t i , t fi ] , where a i , b i , c i , d i and e i are constants of integration that canbe computed by the initial, final, and transversality conditions[23]. IV. U PPER - LEVEL OPTIMIZATION
When a vehicle i ∈ N ( t ) , with a given o i , enters the con-trol zone, it accesses the intersection’s crossing protocol andsolves an upper-level optimization problem. The solution ofthis problem yields for i the path trajectory t p i ,l i (cid:0) p i ( t ) , l i ( t ) (cid:1) and the minimum time t fi to exit the control zone. Inour exposition, we seek to derive the minimum t fi withoutactivating any of the state and control constraints of thelow-level optimization Problem 1. Therefore, the upper-leveloptimization problem should yield a t fi such that the solutionof the low-level optimization problem will result in theunconstrained case (10) - (12).There is an apparent trade off between the two problems.The lower the value of t fi in the upper-level problem, thehigher the value of the control input in [ t i , t fi ] in the low-level problem. The low-level problem is directly related tominimizing energy for each vehicle (individually optimalsolution). On the other hand, the upper-level problem isrelated to maximizing the throughput of the intersection,thus eliminating stop-and-go driving (social optimal solu-tion). Therefore, by seeking a solution for the upper-levelproblem which guarantees that none of the state and controlconstraints become active may be considered an appropriatecompromise between the two.For simplicity of notation, for each vehicle i ∈ N ( t ) wewrite the optimal position (12) of the unconstrained case inthe following form p ∗ i ( t ) = φ i, · t + φ i, · t + φ i, · t + φ i, , t ∈ [ t i , t fi ] , (13)where φ i, , φ i, , φ i, , φ i, ∈ R are the constants of integra-tion derived in the Hamiltonian analysis, in Section III, forthe unconstrained case. Remark 3.
For each i ∈ N ( t ) , the optimal position (13) isa continuous and differentiable function. Based on (3) , it isalso an increasing function with respect to t ∈ R + . Next, we investigate some properties of (13).
Lemma 1.
For each i ∈ N ( t ) , the optimal position p ∗ i givenby (13) is an one-one function.Proof. Since, for each i ∈ N ( t ) , p ∗ i ( t ) is an increasingfunction with respect to t ∈ R + and from (3), for any t , t ∈ [ t i , t fi ] , p ∗ i ( t ) (cid:54) = p ∗ i ( t ) . orollary 1. Since, for each i ∈ N ( t ) , (13) is an one-onefunction, there exist an inverse function p ∗ i ( t ) − such that p ∗ i ( t ) − = ω i, · p + ω i, · p + ω i, · p + ω i, , (14) where ω i, , ω i, , ω i, , ω i, ∈ R are constants that are afunction of φ i, , φ i, , φ i, , φ i, . Remark 4.
For each i ∈ N ( t ) , t ∈ [ t i , t fi ] , we rewrite (13) as follows p ∗ i ( t ) = φ i, · t i + φ i, · t i + φ i, · t i + φ i, . (15) Lemma 2.
Let p ∗ i ( t ) − be the inverse function of (13) for each vehicle i ∈ N ( t ) . Then the con-stants φ i, , φ i, , φ i, , φ i, ∈ R can be derived by ω i, , ω i, , ω i, , ω i, ∈ R . Proof.
Due to space limitation the proof is omitted. However,the result is trivial.
Remark 5.
The inverse function p ∗ i ( t ) − = t i ( p ∗ ( t )) , where t i ( p ∗ ( t )) ∈ [ t i , t fi ] , yields the time that vehicle i ∈ N ( t ) isat the position p ∗ i ( t ) inside the control zone. Lemma 3.
For each i ∈ N ( t ) , the domain of t i ( p ∗ ( t )) isthe closed interval [ p i ( t i ) , p i ( t fi )] .Proof. Since, for each i ∈ N ( t ) , p ∗ i ( t ) is an increasingfunction in [ t i , t fi ] , then by the Intermediate Value Theorem, p ∗ i ( t ) takes values on the closed interval [ p i ( t i ) , p i ( t fi )] . Corollary 2.
Since p ∗ i ( t ) is a continuous and one-onefunction in [ t i , t fi ] for each i ∈ N ( t ) , then t i ( p ∗ ( t )) is alsocontinuous. Corollary 3.
For each i ∈ N ( t ) , p (cid:48) (cid:0) t i ( p ( t )) (cid:1) (cid:54) = 0 forall p ∈ [ p i ( t i ) , p i ( t fi )] . Hence, t i ( p ∗ ( t )) is differentiable in [ p i ( t i ) , p i ( t fi )] . Lemma 4.
For each i ∈ N ( t ) , t i ( p ∗ ( t )) is an increasingfunction in [ p i ( t i ) , p i ( t fi )] .Proof. From Lemma 3, for each i ∈ N ( t ) the domain of t i ( p ∗ ( t )) is [ p i ( t i ) , p i ( t fi )] . Let p i ( t i ) < α < α < p i ( t fi ) with t i ( p i ( t i ) < t i ( α ) . If we had t i ( p i ( t i )) > t i ( α ) , thenby applying the Intermediate Value Theorem to the interval [ α , p i ( t fi )] would give an α with α < α < p i ( t fi ) and t i ( p i ( t i )) = t i ( α ) contradicting the fact that t i ( p ∗ ( t )) isone-one on [ p i ( t i ) , p i ( t fi )] .Since each vehicle i ∈ N ( t ) can change lanes insidethe control zone, its position should be associated with thefunction l i ( t ) (Definition 2) that yields the lane vehicle i occupies inside the control zone at t . Definition 6.
The position of each vehicle i ∈ N ( t ) usinglane l i ( t ) , m ∈ L , is denoted by p i,l ( t, l ) .Based on Definition 6, we augment the optimal positionof i ∈ N ( t ) given by (13) to capture the lane that vehicle i as follows p ∗ i ( t, l ) = p ∗ ( t ) · I ( l ) + p ∗ ( t ) · I ( l ) + · · · + p ∗ ( t ) · I M ( l ) ,t ∈ [ t i , t fi ] , (16) where I m ( l ) , m ∈ L , is the indicator function with I m ( l = m ) = 1 , if i occupies lane m ∈ L and I m ( l (cid:54) = m ) = 0 otherwise. For each vehicle i ∈ N ( t ) , the inverse functionof (13) enhanced with the lane that vehicle i occupies is thepath trajectory (Definition 4) and can be written as follows t p i ,l i ( p i ( t ) , l i ( t ))) = ω i, · p i ( t, l ) + ω i, · p i ( t, l )+ ω i, · p i ( t, l ) + ω i, . (17)The path trajectory t p i ,l i ( p i ( t ) , l i ( t ))) yields the time thatvehicle i is at the position p i ( t ) inside the control zone andoccupies lane l i ( t ) and is used as the cost function for theupper-level optimization problem.In the upper-level optimization problem, each vehicle i ∈N ( t ) derives its optimal path trajectory which yields theminimum time t fi that vehicle i exits the control zone alongwith the lane l ∗ ∈ L that should occupy at each p ∗ i . Toformulate this problem, we need to minimize (17), evaluatedat p i ( t fi ) , with respect to ω i, , ω i, , ω i, , ω i, that determinethe shape of the path trajectory of the vehicle in [ p i , p fi ] . Notethat the value of p i ( t fi ) for each i ∈ N ( t ) depends on o i and,based on the Assumption 1, it can be equal to (see Fig. 1): (1) p i ( t fi ) = 2 S c + S m , if the vehicle crosses the merging zone,(2) p i ( t fi ) = 2 S c + πR r , if the vehicle makes a right turn atthe merging zone, and (3) p i ( t fi ) = 2 S c + πR l , if the vehiclemakes a left turn at the merging zone. For simplicity ofnotation, we denote the total distance travelled by the vehicle i ∈ N ( t ) in [ t i , t fi ] with S i,total , thus p i ( t fi ) = S i,total .Hence, the upper-level optimization problem is formulatedas follows. Problem 2. min ω i, ,ω i, ,ω i, ,ω i, t p i ,l i (cid:0) S i,total , l i ( t ) (cid:1) (18) subject to : (2) , (3) , (4) , and given t i , v i , t fi , p i ( t i ) , p i ( t fi ) . From Lemma 2, the constants φ i, , φ i, , φ i, , φ i, ∈ R corresponding to the constraints imposed through (13) canbe derived by ω i, , ω i, , ω i, , ω i, ∈ R . This is a nonlinearprogramming problem that each vehicle can solve usingLagrange multiplier theory.V. S IMULATION R ESULTS
A. Validation of Upper-Level Optimization
To evaluate the effectiveness of the solution of the pro-posed upper-level optimization problem, we conduct a sim-ulation in MATLAB. The simulation setting is as follows.The intersection contains two roads, each of which has onelane per direction. The length of each direction is 300 m ,the merging zone of the intersection is 25 m by 25 m , andthe entry of merging zone is located at 125 m from theentry point for both directions. The maximum and minimumspeed are 18 m/s and 2 m/s , respectively. The maximumand minimum acceleration are 3.0 m/s and -3.0 m/s .The safety (minimum allowed) headway is 1.0 s , and thestandstill distance is 1.5 m . Six vehicles are entering intothe intersection from three directions at different time steps.ince vehicle 1 is the first vehicle in the network, it cruisesthrough the intersection without any constraints imposed.The trajectories of all vehicles along with the safety distanceare shown in Fig. 2. Negative values of the safety distancemeans violation of the rear-end constraint. We see fromFig. 2 that both rear-end and lateral collision constraints aresatisfied. The control input (acceleration) and speed profilesfor the vehicles in the network is shown in Fig. 3. We notethat for all vehicles driving through the intersection, none ofthe acceleration and speed constraints are activated. Fig. 2. Trajectories and safety distances of vehicles.Fig. 3. Speed and control profiles of vehicles.
VI. C
ONCLUDING R EMARKS
In this paper, we formulated an upper-level optimizationproblem, the solution of which yields, for each CAV, theoptimal sequence and lane to cross the intersection. Theeffectiveness of the solution was illustrated through simula-tion. We showed, through numerical results, that vehicles aresuccessfully crossing an intersection without any rear-end orlateral collision. In addition, the state and control constraintsdid not become active for the entire trajectory for eachvehicle. While the potential benefits of full penetration ofCAVs to alleviate traffic congestion and reduce energy havebecome apparent, different penetrations of CAVs can altersignificantly the efficiency of the entire system. Therefore,future research should look into this direction.R
EFERENCES[1] A. A. Malikopoulos and J. P. Aguilar, “An Optimization Frameworkfor Driver Feedback Systems,”
IEEE Transactions on IntelligentTransportation Systems , vol. 14, no. 2, pp. 955–964, 2013. [2] R. Margiotta and D. Snyder, “An agency guide on how to establishlocalized congestion mitigation programs,” U.S. Department of Trans-portation. Federal Highway Administration, Tech. Rep., 2011.[3] L. Zhao and A. A. Malikopoulos, “Enhanced mobility with connectiv-ity and automation: A review of shared autonomous vehicle systems,”
IEEE Intelligent Transportation Systems Magazine , 2020.[4] M. Athans, “A unified approach to the vehicle-merging problem,”
Transportation Research , vol. 3, no. 1, pp. 123–133, 1969.[5] P. Varaiya, “Smart cars on smart roads: problems of control,”
IEEETransactions on Automatic Control , vol. 38, no. 2, pp. 195–207, 1993.[6] K. Dresner and P. Stone, “Multiagent traffic management: areservation-based intersection control mechanism,” in
Proceedings ofthe Third International Joint Conference on Autonomous Agents andMultiagents Systems , 2004, pp. 530–537.[7] ——, “A Multiagent Approach to Autonomous Intersection Manage-ment,”
Journal of Artificial Intelligence Research , vol. 31, pp. 591–653, 2008.[8] A. de La Fortelle, “Analysis of reservation algorithms for cooperativeplanning at intersections,” , pp. 445–449, Sept. 2010.[9] F. Yan, M. Dridi, and A. El Moudni, “Autonomous vehicle sequencingalgorithm at isolated intersections,” , pp. 1–6, 2009.[10] J. Lee and B. Park, “Development and Evaluation of a CooperativeVehicle Intersection Control Algorithm Under the Connected Vehi-cles Environment,”
IEEE Transactions on Intelligent TransportationSystems , vol. 13, no. 1, pp. 81–90, 2012.[11] K.-D. Kim and P. Kumar, “An MPC-Based Approach to ProvableSystem-Wide Safety and Liveness of Autonomous Ground Traffic,”
IEEE Transactions on Automatic Control , vol. 59, no. 12, pp. 3341–3356, 2014.[12] J. Rios-Torres and A. A. Malikopoulos, “A Survey on Coordinationof Connected and Automated Vehicles at Intersections and Merging atHighway On-Ramps,”
IEEE Transactions on Intelligent TransportationSystems , vol. 18, no. 5, pp. 1066–1077, 2017.[13] J. Rios-Torres, A. A. Malikopoulos, and P. Pisu, “Online OptimalControl of Connected Vehicles for Efficient Traffic Flow at MergingRoads,” in , 2015, pp. 2432–2437.[14] J. Rios-Torres and A. A. Malikopoulos, “Automated and CooperativeVehicle Merging at Highway On-Ramps,”
IEEE Transactions onIntelligent Transportation Systems , vol. 18, no. 4, pp. 780–789, 2017.[15] I. A. Ntousakis, I. K. Nikolos, and M. Papageorgiou, “Optimalvehicle trajectory planning in the context of cooperative merging onhighways,”
Transportation Research Part C: Emerging Technologies ,vol. 71, pp. 464–488, 2016.[16] Y. Zhang, A. A. Malikopoulos, and C. G. Cassandras, “Optimal controland coordination of connected and automated vehicles at urban trafficintersections,” in
Proceedings of the American Control Conference ,2016, pp. 6227–6232.[17] L. Zhao, A. A. Malikopoulos, and J. Rios-Torres, “Optimal control ofconnected and automated vehicles at roundabouts: An investigation ina mixed-traffic environment,” in , 2018, pp. 73–78.[18] A. Stager, L. Bhan, A. A. Malikopoulos, and L. Zhao, “A scaled smartcity for experimental validation of connected and automated vehicles,”in , 2018,pp. 120–135.[19] A. A. Malikopoulos, C. G. Cassandras, and Y. Zhang, “A decentralizedenergy-optimal control framework for connected automated vehiclesat signal-free intersections,”
Automatica , vol. 93, pp. 244 – 256, 2018.[20] A. A. Malikopoulos, P. Y. Papalambros, and D. N. Assanis, “Onlineidentification and stochastic control for autonomous internal com-bustion engines,”
Journal of Dynamic Systems, Measurement, andControl , vol. 132, no. 2, pp. 024 504–024 504, 2010.[21] A. A. Malikopoulos, P. Papalambros, and D. Assanis, “Optimal enginecalibration for individual driving styles,” in
SAE Congress , 2008.[22] A. E. Bryson, Jr., W. F. Denham, and S. E. Dreyfus, “Optimalprogramming problems with inequality constraints i: Necessary con-ditions for extremal solutions,”
American Institute of Aeronautics andAstronautics Journal , vol. 1, no. 11, pp. 2544 – 2550, 1963.[23] A. A. Malikopoulos and L. Zhao, “A closed-form analytical solutionfor optimal coordination of connected and automated vehicles,” in