# Scheduling-Based Optimization for Motion Coordination of Autonomous Vehicles at Multilane Intersections

Authors: Mehmet Ali Guney; Ioannis A. Raptis
Journal: Journal of Robotics
 (2020)

---

## Abstract

This paper considers the motion coordination problem of autonomous vehicles in an intersection of a traffic network. The featured challenge is the design of an intersection traffic manager, in the form of a supervisory control algorithm, that regulates the motion of the autonomous vehicles in the intersection. We cast the multivehicle coordination task as an optimization problem, with a one-dimensional search-space. A model- and optimization-based heuristic method is employed to compute the control policy that results in the collision-free motion of the vehicles at the intersection and, at the same time, minimizes their delay. Our approach depends on a computation framework that makes the need for complex analytical derivations obsolete. A complete account of the computational complexity of the algorithm, parameterized by the configuration parameters of the problem, is provided. Extensive numerical simulations validate the applicability and performance of the proposed autonomous intersection traffic manager.


---

## Body

## 1. Introduction

In the last decade, significant progress has been made in integrating wireless communications, advanced sensing technologies, and computationally powerful embedded systems on-board vehicles and transportation systems. These augmenting technologies are projected to mitigate existing problems of urban traffic such as congestion, safety, reliability, and sustainability [1]. The most notable challenge in road traffic management systems is the control of traffic in junction nodes of the transportation network where roads cross or merge such as intersections, roundabouts, and onramps [2, 3]. Traffic intersections in particular, even though they constitute a small portion of the overall road transportation network, they account for a disproportionately high amount of traffic accidents (in both US [4] and EU [5]) and are predominant locations where traffic congestion aggravates place.Advances in the fields of communication and information technologies, customized for road transport, and vehicles autonomy gave rise to Intelligent Transportation Systems (ITSs) [6], which are commissioned to improve the efficiency of existing traffic management and mobility. Present-day vehicles are equipped with a variety of sophisticated sensors suitable for situational awareness and capable of measuring with sufficient accuracy the vehicle’s state, e.g., location, speed, and acceleration. At the same time, emerging technologies in vehicle-to-vehicle (V2V) and vehicle-to-infrastructure (V2I) communications (jointly referred to as V2X communications) allow the information exchange between powerful computing systems on-board the vehicles and the infrastructure, which are capable of real-time processing of the information collected by the traffic system and online decision-making.The emergent integrated networked systems of autonomous, computing, and sensing nodes are brought into play to automate the traffic management of intersections. The overarching objective of this trending research field is to improve the safety and efficiency performance of intersections, compared to traditional means of traffic control (signals, signs, and right-of-way conventions). The long-term vision is to have autonomous vehicles transporting in accident-free traffic systems. The featured module of automated intersections is the control algorithm, also referred to as Autonomous Intersection Manager (AIM) [7, 8], that is responsible for coordinating the collision-free motion of the vehicles within the intersection and also optimizes a performance metric of the traffic system. Due to the large volume of data, the scale and complexity of the problem, the multitude of factors that introduce uncertainty (wheel slippage, modeling imperfections, and packet drops to the communication system), and the processing and bandwidth limitations, the design of efficient control algorithms for autonomous intersection management is becoming a prominent challenge.The motion coordination of vehicles in autonomous intersections manifests a unique control problem. Its complexity makes the derivation of closed-form solutions cumbersome or analytically infeasible. The problem itself is cross-disciplinary: the autonomous intersection is a multiagent system, subjected to asynchronous communication broadcasts, where nonlinear equations govern the agents’ dynamics. Traditional nonlinear systems theory is required for the design of the low-level feedback control laws that steer the vehicles based on their kinematic (or dynamic) configuration; elements from computational optimization and dynamic programming are needed to seek for an optimal control policy that minimizes the performance metrics of the optimization problem; and heuristic algorithms are requisite to efficiently search for an acceptable solution in the high-dimensional problem space. All these computational and analytical components need to be amalgamated into a single framework that will be computationally tractable and applicable to a computing system.Seminal work on the design of decision-making algorithms for automated road systems can be traced all the way back to the 1960s in the work of Athans [9], and Levine and Athans [10], where the merging of streams of vehicles and the coordination of vehicles in a single string (platooning) were originally cast as optimal control problems. It should be noted that the intersection traffic management bears distinct differences from conventional collision avoidance applications, with the most distinctive being airspace control. The vehicles in intersections are expected to follow predefined paths; the AIM can only change the temporal parameterization of the vehicles’ trajectory by adjusting their acceleration. The fact that multiple vehicles need to transport over a confined shared space, with no option of readjusting their route, makes the intersection control problem NP-hard [11, 12].In the existing literature, the classification of autonomous intersection controllers is principally based on the architecture (concentration) of the controller. The intersection controllers are mainly classified into centralized and decentralized [3, 13–15]. Centralized methods employ a singleton decision-making unit that globally plans the motion of all vehicles in the intersection, and they are broadly categorized as reservation optimization-, model predictive control-, and queuing theory-based. A prominent work on reservation-based autonomous intersection controllers is reported in [7, 16] where the connected vehicles transmit reservation requests to the AIM. The AIM either approves or denies the request, by considering the availability of the intersection. In the case of disapproval, the driver agent slows down and sends a new reservation request with an updated arrival time. Model predictive control methods plan the optimal collision-free trajectory of each vehicle by evaluating the cost of the motion in a future time horizon [17–19]. The optimization methods achieve minimization of performance metrics such as travel time within the intersection [20–22], the length of overlapped trajectories of the vehicles [23, 24], and fuel consumption [25]. Polling policies, which fall under the subject of queuing theory, for autonomous intersections have been introduced in [26, 27], where the AIM is modeled as a server, which accommodates multiple queues. The server determines which motion direction to serve based on several criteria such as the vehicles’ service time and the queue length in each intersecting road.In contrast to centralized AIMs, the decision-making process in decentralized architectures takes place at the vehicle level, using local information exchange between communicating vehicles [3]. There has been a considerable research effort on the decentralized intersection control; proposed formulations include decentralized reservation, conflict resolution, and optimization-based control methods. In [28, 29], the centralized reservation scheme is revised with a decentralized architecture using a V2V communication protocol and ad hoc rules. The conflict resolution approaches introduce a fixed priority graph to define the motion precedence between vehicles [30, 31]. Based on the priority scheme, the motion planner optimizes the trajectory of the conflicting vehicles. The decentralized optimization methods essentially formulate the motion coordination as a multiobjective optimization problem [3]. The optimization objectives include but are not limited to the travel time, energy consumption, and collision avoidance terms [13, 32]. Distributed variants of the motion coordination problem in intersecting nodes are reported in [33, 34].This work presents a supervisory AIM tasked to coordinate the motion of computer-controller vehicles in a multiroad and multilane intersection. At the intersection, it is expected that the vehicles can make a turn in any road that complies with the right-hand rule of bidirectional traffic, and it is assumed that nonlinear kinematic equations govern their motion. The traffic manager calculates the trajectory of each vehicle sequentially, in a first-come first-served order, and outputs the control policy of each vehicle in the form of a linear velocity profile. The AIM is designed such that the vehicles enter and cross the shared region of the intersection with their maximum velocity, improving the intersection’s throughput.The multivehicle coordination control problem is cast as an optimization search with nonlinear constraints, which seeks each vehicle’s optimal arrival time to the shared—by all vehicles—region of the intersection. This is accomplished by uniquely mapping the arrival time to the entire control policy (velocity profile) of the vehicle, making the arrival time the only optimization variable. The constraints of the optimization problem encapsulate the safety requirements of the intersection system and the kinematic bounds of the vehicles. A constraint handling method converts the constrained optimization problem into an unconstrained one. Subsequently, the unconstrained formalism is suitably processed by a metaheuristic optimization technique that employs multiple moving candidate solutions (called particles) that herd the search-space seeking for the optimal solution to the optimization problem. To aid the optimization search process, we analytically limit the search-space to an interval where a solution is guaranteed to reside. The search process is further assisted by a preliminary procedure that handpicks candidate solutions for which the trajectory of the vehicle is unreserved in the shared region of the intersection. These candidate solutions become initial values of the optimization procedure and improve its performance by initiating the optimization search with partially feasible trajectories.The main contribution of this paper is the schedule-driven optimization formulation of the motion coordination problem of computer-controlled vehicles at a traffic intersection. To solve the problem, we introduce model-based heuristics, with well-defined computational complexity bounds, that can effectively explore the search-space of the complex monolithic optimization problem and generate control policies that satisfy the intersection system’s performance metrics and the vehicles’ safety requirements. We employ a metaheuristic optimization search formulation that conveniently captures the safety and motion constraints of the AIM problem and can be straightforwardly implemented to a computer program, in contrast to standard, analytically laborious nonlinear programming methods. In principle, metaheuristic optimization algorithms are capable of carrying out more robust searches than conventional optimization methods when the search-space is high dimensional, discontinuous, and multimodal [35]. Our approach relies mainly on computational tools that eliminate the need for rigorous, analytically derived optimal control formulations while guaranteeing performance. Towards this end, we provide a fundamental understanding of the impact that the geometric layout of the intersection and the configuration parameters of the algorithm have on the overall complexity of the AIM. Furthermore, we show how scheduling information, which pertains to the spatiotemporal availability of the intersection’s shared region, can be gracefully integrated into the computational optimization algorithm that was developed for the multivehicle coordination task. The constraint handling method retains the information of infeasible solutions instead of discarding them, facilitating the search process of the metaheuristic algorithm employed to solve the optimization problem. Finally, detailed pseudocodes outline the implementation of the proposed AIM rigorously.This paper is organized as follows. Section2 provides a formal description of the control problem that is addressed in this paper, in particular, the model of the intersection, a description of the constrained optimization problem that represents the intersection control task, and the outline of the proposed solution. The preliminary feasibility search procedure of candidate solutions, which correspond to unreserved trajectories in the shared region of the intersection, is given in Section 3.1. A detailed exposition of the metaheuristic optimization method that is applied to the autonomous intersection problem is provided in Section 3.2. Section 3.3 describes the penalty method, which is used to convert the constrained optimization problem into an unconstrained one, and is followed by Section 3.4 that gives insight on how to quantify the severity of constraint violations in the penalty method. The computational complexity of the proposed AIM is discussed in Section 4. The performance of the proposed AIM is assessed in Section 5 via numerical simulations. Concluding remarks are given in Section 6.


## 2. Problem Statement

### 2.1. Model of the Traffic System

This paper focuses on the motion coordination problem of autonomous vehicles in a traffic intersection. The traffic system is denoted by the set of vehicles:(1)Aij,k:j∈NR,k∈NL,where t is the discrete time variable; i∈Z+ is the index of the incoming vehicle (i⟶∞ as t⟶∞); j and k are indexes designating the entry road and lane of Ai to the intersection, respectively; and NR and NL are the index sets of roads and lanes in a road, respectively. The road index set is defined as NR=1,…,NR, where NR∈2,4 is the total number of parallel and mutually orthogonal intersecting roads, and the lane index set is defined as NL=1,…,NL, where NL is the total number of lanes in every road. For simplicity, it is assumed that all the intersecting roads have equal number of lanes. The region where the roads overlap is referred to as the intersection region (IR), and the segments of the roads of length L, preceding the IR, are referred to as the control regions (CRs) of the intersection. The set Nj,kt, with j∈NR and k∈NL, denotes the collection of vehicles that transport within the CR of road j and lane k at the time instant t.The planar 2D motion of vehicleAij,k is described by the pose column vector ci=xi;yi;ψi, where xi and yi denote the Cartesian coordinates of Ai’s front bumper with respect to the intersection-fixed frame ℱℐ, and ψi its heading. The heading angle is the relative angular displacement of ℱℐ with respect to a vehicle-fixed frame ℱi that is rigidly attached to Aij,k. When ψi=0, the two frames (ℱℐ and ℱi) are aligned. The two frames are illustrated in Figure 1. It is assumed that the autonomous vehicles have the kinematics of unicycle, or differential drive, robot. The kinematics of vehicle Ai are given by(2)x˙i=vicosψiy˙i=visinψiψ˙i=ωi,where vi is the linear velocity and ωi is the angular velocity of Ai. The vehicles cannot move in reverse; therefore, the velocity vit can take values in the interval 0,vmax, where vmax is the maximum allowed velocity of the traffic system. Furthermore, their acceleration is also bounded by a positive constant amax; consequently, for vehicle Ai, we have v˙i≤amax. The vehicles are considered as two-dimensional rectangular rigid bodies where their width and length are denoted by w and l, respectively. Let Gi be the set of all points in the 2D space that are enclosed by the geometric shape of Ai. The occupancy set, Gi, of Ai at the time instant t depends on its pose, ci, and is defined as follows:(3)Git=x,y∈ℝ2xit−l⋅cosψit+w⋅sinψit2≤x≤xit+w⋅sinψit2,yit−l⋅sinψit+w⋅cosψit2≤y≤yit+w⋅cosψit2.Figure 1
Illustration with the key features relevant to the formulation of the intersection control problem: the intersection and control regions, the geometry and dimensions of the vehicles, and the vehicle- and intersection-fixed frames.LetXit=xit,yit∈ℝ2 be the position coordinates of the front bumper at time t of the vehicle Ai. It is assumed that the vehicle Ai enters the CR at time t¯i with the speed v¯i=vmax. The control objective is to determine the arrival time t¯i of Ai at the IR such that it enters the region with the speed v¯i=vmax. The vehicles cross the IR with a constant linear velocity vmax. Incoming vehicles to the IR can move straight or make a turn that is compatible with the right-hand convention of bidirectional traffic. The final direction of vehicle Ai is represented by the parameter di∈0,1,2. When di=0, the vehicle retains its direction, else it will make a turn (left, di=1, or right, di=2) to one of the intersecting roads.The crossing lanes in the IR form a square grid array. Each cell of the array is uniquely identified by its row and column indexes. The grid hasc=NR/2⋅NL columns and r=NR/2⋅NL rows. Let C=1,…,c and ℛ=1,…,r denote the column and row sets, respectively. A cell entry at the intersection located at the I∈ℛ row and J∈C column is denoted by CI,J. The edge of square cells is denoted by e and is equal to the lane’s width. The enumeration order of the IR’s cells is shown in Figure 2. The decomposition of IR to cells will be used later on to determine the occupancy of the IR by the incoming vehicles.Figure 2
Grid array of the IR and its double index ordering. The enumeration of the array starts from the lower left cell of the grid array.


### 2.2. The Optimization Problem

We formulate the intersection control task, that is, the effective and collision-free multivehicle motion coordination at the intersection, as an optimization search for which its solution satisfies the specifications of the autonomous intersection in terms of performance and safety.To begin with, an optimal traffic flow, which maximizes the intersection throughput, is achieved by minimizing the sojourn time of the vehicles at the intersection [36]. As mentioned earlier, the vehicles traverse the IR with a constant linear velocity vmax; thus, the only delay occurs within the CR. The time it takes for Ai to travel the CR (of length L) is td=t¯i−t¯i, and Ai experiences a delay when td>ts, where ts=L/vmax. More precisely, ts is the minimal time duration that takes a vehicle to traverse the CR with its maximum velocity, vmax. With the following definition, we manifest the function that needs to be minimized in order to reduce the sojourn time of a vehicle Ai at the intersection.Definition 1.
The delay of vehicleAi is a function of the time difference t¯i−t¯i and is defined as follows:(4)ft¯i,t¯i=t¯i−t¯i−ts.
Obviously, from the above, if there is no delay forAi at the CR, i.e., td=ts, then ft¯i,t¯i=0. The vehicles are further expected to transport the intersection without colliding. The following definition states the safety requirement for the collision-free motion of the vehicles within the intersection.Definition 2.
The intersection system is safe at timet, if there are no collisions between any two vehicles Aij,k and Apm,n, i.e., Git∩Gpt=∅, where j,m∈NR; k,n∈NL; and p≠i.
Apart from the safety condition, the vehicles are subjected to kinematic constraints. In particular, the vehicles enter and exit the CR with their maximum speed,vmax, while their velocity and acceleration in the CR should satisfy certain bounds (vit≤vmax and v˙it≤amax for every t in t¯it¯i). We would like to have the arrival time as the only optimization variable and also simplify the design of the vehicles’ velocity profiles in the CR. Thus, we parameterize the velocity function of Ai in the CR with its arrival time, t¯i, and assign a one-to-one mapping between the entry and arrival times, t¯i and t¯i, respectively, and vit. To emphasize the dependency of the vit function in t¯i and t¯i, we write the velocity of Ai as vit,t¯i,t¯i.
Based on the above definitions—pertaining to the autonomous intersection’s performance (Definition1) and the vehicles’ safety (Definition 2) and kinematic constraints—we formulate the motion coordination optimization problem for an arbitrary vehicle Ai entering the autonomous traffic intersection as follows:(5)minimizeft¯i,t¯i,(6)subject toGit∩Gpt=∅,∀t∈t¯i,t¯i+τiandp≠i;Xit−Xpt−l≤0,∀t∈t¯i,t¯iwhereAij,k,Apj,k∈Nj,ktwithj∈NR,k∈NL,andi≠p;0<vit,t¯i,t¯i≤vmax,∀t∈t¯i,t¯iwithv¯it¯i=v¯it¯i=vmax;v˙it,t¯i,t¯i≤amax,∀t∈t¯i,t¯i.
The above optimization formulation yields a constrained minimization search with a linear objective function and nonlinear constraints. The optimization variable is the arrival timet¯i and the objective function, ft¯i,t¯i, is cast such that the solution of the optimization search will minimize the delay caused by the deceleration of the vehicles in the CR. The safety constraints incorporate the nonlinear kinematic equations of the vehicles and guarantee their collision-free motion in the intersection.


### 2.3. Proposed Solution: Description of the Intersection Controller

The autonomous intersection utilizes a supervisory controller, which we will refer to as theintersection controller (IC), that regulates the motion of the vehicles that enter the intersection. The vehicles enter the intersection at random time instances and the IC determines their trajectories in a first-come first-serve manner.When a vehicle, e.g.,Ai, enters the intersection, the IC records its entry time t¯i. The motion control problem entails the designation of a velocity function vit that satisfies the safety and kinematic constraints of Ai and minimizes its delay at the CR. However, because we parameterize vit with t¯i and t¯i, the motion control problem is conveniently converted to determining the arrival time, t¯i, that solves the optimization problem described in (5).Our solution is structured as follows: a computational optimization method solves the minimization problem expressed in (5). Contrary to a conventional optimization search, the intersection control problem requires at each iteration of the search to project forward in time the vehicle’s movement. This forecast is needed to examine if the safety of the vehicles at the intersection is preserved. The forward projected trajectory of Ai is compared with the trajectories of the rest of the vehicles that transport at the intersection. To make the juxtaposition between trajectories possible, a central buffer is employed that stores all the trajectories of the vehicles that move in the intersection. We refer to this buffer as the intersection’s timetable. The computational optimization procedure reads, and updates, the entries of the timetable to examine if a candidate solution corresponds to an infeasible trajectory. At the end, the optimization procedure determines the arrival time of the vehicle at the IR, and subsequently, its velocity profile vit,t¯i,t¯i.Because of the forward projection process, the optimization procedure is computationally demanding, especially when the number of candidate solutions is high. To facilitate the optimization search, we introduce a preparatory procedure that bounds the domain of the optimization problem and outputs a set of candidate solutions that correspond to trajectories with unreserved paths in the IR. These candidate solutions, which serve as a pool for initial conditions to the optimization procedure, improve the performance of the latter by initiating the search with partially feasible trajectories—in terms of safety in the IR—that may lead the search faster to an optimal solution. We show that the computational complexity of this preliminary procedure is menial compared to the complexity of the optimization search. Thus, it does not add significant computational overhead to the final algorithm; instead, it improves its performance. The block diagram of the IC is illustrated in Figure3.Figure 3
Block diagram of the IC. The IC receives the entry times of the vehicles when they enter the intersection. The preliminary feasibility search procedure generates a set of initial conditions for the optimization search. The output of the optimization search is the arrival time of each vehicle to the IR, which is translated into a velocity profile.


## 2.1. Model of the Traffic System

This paper focuses on the motion coordination problem of autonomous vehicles in a traffic intersection. The traffic system is denoted by the set of vehicles:(1)Aij,k:j∈NR,k∈NL,where t is the discrete time variable; i∈Z+ is the index of the incoming vehicle (i⟶∞ as t⟶∞); j and k are indexes designating the entry road and lane of Ai to the intersection, respectively; and NR and NL are the index sets of roads and lanes in a road, respectively. The road index set is defined as NR=1,…,NR, where NR∈2,4 is the total number of parallel and mutually orthogonal intersecting roads, and the lane index set is defined as NL=1,…,NL, where NL is the total number of lanes in every road. For simplicity, it is assumed that all the intersecting roads have equal number of lanes. The region where the roads overlap is referred to as the intersection region (IR), and the segments of the roads of length L, preceding the IR, are referred to as the control regions (CRs) of the intersection. The set Nj,kt, with j∈NR and k∈NL, denotes the collection of vehicles that transport within the CR of road j and lane k at the time instant t.The planar 2D motion of vehicleAij,k is described by the pose column vector ci=xi;yi;ψi, where xi and yi denote the Cartesian coordinates of Ai’s front bumper with respect to the intersection-fixed frame ℱℐ, and ψi its heading. The heading angle is the relative angular displacement of ℱℐ with respect to a vehicle-fixed frame ℱi that is rigidly attached to Aij,k. When ψi=0, the two frames (ℱℐ and ℱi) are aligned. The two frames are illustrated in Figure 1. It is assumed that the autonomous vehicles have the kinematics of unicycle, or differential drive, robot. The kinematics of vehicle Ai are given by(2)x˙i=vicosψiy˙i=visinψiψ˙i=ωi,where vi is the linear velocity and ωi is the angular velocity of Ai. The vehicles cannot move in reverse; therefore, the velocity vit can take values in the interval 0,vmax, where vmax is the maximum allowed velocity of the traffic system. Furthermore, their acceleration is also bounded by a positive constant amax; consequently, for vehicle Ai, we have v˙i≤amax. The vehicles are considered as two-dimensional rectangular rigid bodies where their width and length are denoted by w and l, respectively. Let Gi be the set of all points in the 2D space that are enclosed by the geometric shape of Ai. The occupancy set, Gi, of Ai at the time instant t depends on its pose, ci, and is defined as follows:(3)Git=x,y∈ℝ2xit−l⋅cosψit+w⋅sinψit2≤x≤xit+w⋅sinψit2,yit−l⋅sinψit+w⋅cosψit2≤y≤yit+w⋅cosψit2.Figure 1
Illustration with the key features relevant to the formulation of the intersection control problem: the intersection and control regions, the geometry and dimensions of the vehicles, and the vehicle- and intersection-fixed frames.LetXit=xit,yit∈ℝ2 be the position coordinates of the front bumper at time t of the vehicle Ai. It is assumed that the vehicle Ai enters the CR at time t¯i with the speed v¯i=vmax. The control objective is to determine the arrival time t¯i of Ai at the IR such that it enters the region with the speed v¯i=vmax. The vehicles cross the IR with a constant linear velocity vmax. Incoming vehicles to the IR can move straight or make a turn that is compatible with the right-hand convention of bidirectional traffic. The final direction of vehicle Ai is represented by the parameter di∈0,1,2. When di=0, the vehicle retains its direction, else it will make a turn (left, di=1, or right, di=2) to one of the intersecting roads.The crossing lanes in the IR form a square grid array. Each cell of the array is uniquely identified by its row and column indexes. The grid hasc=NR/2⋅NL columns and r=NR/2⋅NL rows. Let C=1,…,c and ℛ=1,…,r denote the column and row sets, respectively. A cell entry at the intersection located at the I∈ℛ row and J∈C column is denoted by CI,J. The edge of square cells is denoted by e and is equal to the lane’s width. The enumeration order of the IR’s cells is shown in Figure 2. The decomposition of IR to cells will be used later on to determine the occupancy of the IR by the incoming vehicles.Figure 2
Grid array of the IR and its double index ordering. The enumeration of the array starts from the lower left cell of the grid array.


## 2.2. The Optimization Problem

We formulate the intersection control task, that is, the effective and collision-free multivehicle motion coordination at the intersection, as an optimization search for which its solution satisfies the specifications of the autonomous intersection in terms of performance and safety.To begin with, an optimal traffic flow, which maximizes the intersection throughput, is achieved by minimizing the sojourn time of the vehicles at the intersection [36]. As mentioned earlier, the vehicles traverse the IR with a constant linear velocity vmax; thus, the only delay occurs within the CR. The time it takes for Ai to travel the CR (of length L) is td=t¯i−t¯i, and Ai experiences a delay when td>ts, where ts=L/vmax. More precisely, ts is the minimal time duration that takes a vehicle to traverse the CR with its maximum velocity, vmax. With the following definition, we manifest the function that needs to be minimized in order to reduce the sojourn time of a vehicle Ai at the intersection.Definition 1.
The delay of vehicleAi is a function of the time difference t¯i−t¯i and is defined as follows:(4)ft¯i,t¯i=t¯i−t¯i−ts.
Obviously, from the above, if there is no delay forAi at the CR, i.e., td=ts, then ft¯i,t¯i=0. The vehicles are further expected to transport the intersection without colliding. The following definition states the safety requirement for the collision-free motion of the vehicles within the intersection.Definition 2.
The intersection system is safe at timet, if there are no collisions between any two vehicles Aij,k and Apm,n, i.e., Git∩Gpt=∅, where j,m∈NR; k,n∈NL; and p≠i.
Apart from the safety condition, the vehicles are subjected to kinematic constraints. In particular, the vehicles enter and exit the CR with their maximum speed,vmax, while their velocity and acceleration in the CR should satisfy certain bounds (vit≤vmax and v˙it≤amax for every t in t¯it¯i). We would like to have the arrival time as the only optimization variable and also simplify the design of the vehicles’ velocity profiles in the CR. Thus, we parameterize the velocity function of Ai in the CR with its arrival time, t¯i, and assign a one-to-one mapping between the entry and arrival times, t¯i and t¯i, respectively, and vit. To emphasize the dependency of the vit function in t¯i and t¯i, we write the velocity of Ai as vit,t¯i,t¯i.
Based on the above definitions—pertaining to the autonomous intersection’s performance (Definition1) and the vehicles’ safety (Definition 2) and kinematic constraints—we formulate the motion coordination optimization problem for an arbitrary vehicle Ai entering the autonomous traffic intersection as follows:(5)minimizeft¯i,t¯i,(6)subject toGit∩Gpt=∅,∀t∈t¯i,t¯i+τiandp≠i;Xit−Xpt−l≤0,∀t∈t¯i,t¯iwhereAij,k,Apj,k∈Nj,ktwithj∈NR,k∈NL,andi≠p;0<vit,t¯i,t¯i≤vmax,∀t∈t¯i,t¯iwithv¯it¯i=v¯it¯i=vmax;v˙it,t¯i,t¯i≤amax,∀t∈t¯i,t¯i.
The above optimization formulation yields a constrained minimization search with a linear objective function and nonlinear constraints. The optimization variable is the arrival timet¯i and the objective function, ft¯i,t¯i, is cast such that the solution of the optimization search will minimize the delay caused by the deceleration of the vehicles in the CR. The safety constraints incorporate the nonlinear kinematic equations of the vehicles and guarantee their collision-free motion in the intersection.


## 2.3. Proposed Solution: Description of the Intersection Controller

The autonomous intersection utilizes a supervisory controller, which we will refer to as theintersection controller (IC), that regulates the motion of the vehicles that enter the intersection. The vehicles enter the intersection at random time instances and the IC determines their trajectories in a first-come first-serve manner.When a vehicle, e.g.,Ai, enters the intersection, the IC records its entry time t¯i. The motion control problem entails the designation of a velocity function vit that satisfies the safety and kinematic constraints of Ai and minimizes its delay at the CR. However, because we parameterize vit with t¯i and t¯i, the motion control problem is conveniently converted to determining the arrival time, t¯i, that solves the optimization problem described in (5).Our solution is structured as follows: a computational optimization method solves the minimization problem expressed in (5). Contrary to a conventional optimization search, the intersection control problem requires at each iteration of the search to project forward in time the vehicle’s movement. This forecast is needed to examine if the safety of the vehicles at the intersection is preserved. The forward projected trajectory of Ai is compared with the trajectories of the rest of the vehicles that transport at the intersection. To make the juxtaposition between trajectories possible, a central buffer is employed that stores all the trajectories of the vehicles that move in the intersection. We refer to this buffer as the intersection’s timetable. The computational optimization procedure reads, and updates, the entries of the timetable to examine if a candidate solution corresponds to an infeasible trajectory. At the end, the optimization procedure determines the arrival time of the vehicle at the IR, and subsequently, its velocity profile vit,t¯i,t¯i.Because of the forward projection process, the optimization procedure is computationally demanding, especially when the number of candidate solutions is high. To facilitate the optimization search, we introduce a preparatory procedure that bounds the domain of the optimization problem and outputs a set of candidate solutions that correspond to trajectories with unreserved paths in the IR. These candidate solutions, which serve as a pool for initial conditions to the optimization procedure, improve the performance of the latter by initiating the search with partially feasible trajectories—in terms of safety in the IR—that may lead the search faster to an optimal solution. We show that the computational complexity of this preliminary procedure is menial compared to the complexity of the optimization search. Thus, it does not add significant computational overhead to the final algorithm; instead, it improves its performance. The block diagram of the IC is illustrated in Figure3.Figure 3
Block diagram of the IC. The IC receives the entry times of the vehicles when they enter the intersection. The preliminary feasibility search procedure generates a set of initial conditions for the optimization search. The output of the optimization search is the arrival time of each vehicle to the IR, which is translated into a velocity profile.


## 3. Description of the Intersection Controller

### 3.1. Preliminary Feasibility Search of Candidate Solutions

In this section, we show how the search-space of the optimization task is determined. The output of this preliminary processing operation is the assignment of a set of arrival times, to every incoming vehicle, that serve as initial candidate solutions for the optimization task. These candidate solutions are further refined during the optimization search. The IC schedules the arrival time of each incoming vehicle, and based on this value, it shapes the vehicle’s linear velocity time profile in the CR. It is expected that each vehicle enters and exits the CR with its maximum velocity value,vmax.We assume that the routes of the vehicles have already been determined when they enter the CR and this information is available to the IC. For simplicity, we further assume that the vehicles do not change lanes, whether they remain on the same road or change direction by making a turn. However, the proposed IC can be easily modified to accommodate routes within the intersection that involve both direction and lane change.The first task of the IC is to identify the intersection cells that the traversing vehicles will occupy along their motion in the IR—as mentioned earlier, the path of each vehicle in the IR is predetermined. LetOi denote the ordered set of cells that the vehicle Ai occupies while it traverses the IR. Each cell OI,J∈Oi is assigned with a unique service time TI,J, representing the occupancy duration of OI,J from Ai. The service time TI,J is determined by the motion of the vehicle within the cell. Each vehicle can move straight or execute a quarter turn. If the vehicle does not change direction, the occupancy duration of the cell OI,J is TI,Jstraight=e/vmax. When changing directions, the vehicles move straight until reaching the cell of the IR that corresponds to their turning lane. During the turn maneuver, the vehicles retain the value of their linear velocity; therefore, the occupancy duration of the “turning” cells is calculated by TI,Jturn=π⋅R/2⋅vmax, where R is the turning radius of the vehicles R=e/2. Figure 4 shows the two admissible trajectories of the vehicles (move straight and make a turn) within the IR and their respective duration.Figure 4
The two types of paths within the IR and service times of cells that correspond to straight and turning segments of paths, respectively. When a vehicle makes a turn, it maintains the same lane in its exit road with its entry road.The model of the turning trajectories within the IR, which consists of two straight paths and a quarter circle in between, is selected to illustrate, in an uncomplicated manner, the calculation of the total service time of a vehicle within the IR. The turning radiuse can be increased, by expanding the width of the lanes, to reduce the angular velocity of the vehicles in the single-cell quarter turns in order to meet the comfort levels of the passengers while the vehicle is turning. Additionally, this simplified path can be substituted effortlessly by a curved trajectory that connects the entry and exit points of the turning vehicle in the IR and produces a lower angular velocity for the turning vehicle. In this case, the corresponding total service time of the vehicle has to be redetermined accordingly. The optimization and design of curvilinear paths is outside the scope of this work; we refer the interested reader to [37, 38].LetDi be the ordered set with elements the indices pairs of the cells that belong to Oi, such that Di=D1,D2,…,DOi. The total service time that is required for Ai to travel through the IR is given by(7)τi=∑m=1DiTDm+tv,where(8)tv=lvmax,is the constant duration needed for the vehicle to exit the last cell of its route in the IR.Initially, the IC determines the time window that serves as the search-space of the optimization routine for seeking the optimal value of each vehicle’s arrival time to the intersection. The duration of this time window is selected wide enough to ensure that at least one solution exists within the search-space.A conservative value of the search window’s upper bound is selected by assuming that each incoming vehicle will convey the longest possible path in the intersection while all lanes are experiencing maximum congestion. The longest possible path is illustrated in Figure5 for a generic intersection model. The search-space’s upper bound (“latest” arrival time) is calculated by the following equation:(9)t¯il=t¯i+Ll⋅δmax⋅NR⋅NL,where ⌈⋅⌉ denotes the ceiling function, ⌈L/l⌉ is the maximum number of vehicles that can accumulate in a single lane of the CR without colliding (maximum lane capacity), and δmax is the time duration that is required for the vehicles to convey the longest possible path in the IR (maximum service time). The maximum lane capacity and the maximum service time are multiplied by the product of roads times lanes to determine the total time that is required to serve all other vehicles before Ai arrives at the intersection—assuming that all CRs are fully congested. For an arbitrary intersection layout, the service time of the longest path in the IR, δmax, is calculated by the following equation:(10)δmax=NRNL−2TI,Jstraight+TI,Jturn+lvmax=4NRNL−2e+πe+4l4vmax.Figure 5
The longest possible path that a vehicle can travel in the IR. It is traveled by the vehicles that transport at the outermost right lane of the CR and make a left turn at the IR.The “earliest” arrival time,t¯ie, occurs when a vehicle does not experience any delay in the CR and is calculated by the following equation:(11)t¯ie=t¯i+ts.Therefore, the search-space ofAi is defined as Si=s∈ℝ+t¯ie≤s≤t¯il. A preliminary feasible arrival time signifies that all the requested cells in Oi are unoccupied and available to reserve for the duration of the service times (see Figure 6). The occupancy schedule of the intersection is represented by assigning a timetable BI,J to the cell OI,J of the IR, where BI,Jt=0 signals that the cell OI,J is available at time t, and BI,Jt=1 that is occupied.Figure 6
Graphical illustration of the intersection’s reservation timetable. The timetable is a buffer that stores for each cell of the IR its occupancy information with respect to time. For an arbitrary vehicleAi, the preliminary feasibility search identifies the elements of the set Si that correspond to an unreserved trajectory at the IR using the timetable.Assume that the vehicleAi is set to traverse in the IR over a path represented by the set Oi. The actual search-space is comprised of the discrete execution time instances of the algorithm that lie within the time interval Si. Denote by Si=s1i,…,sNiisji∈Si the set of candidate arrival times of Ai, where Ni is the total number of discrete execution time instances within Si.Subsequently, the IC determines the set of entrance-to-cells time instances denoted astentrancei=tD1i,…,tDDii, where tDji stands for the time instant that Ai enters the cell ODj with j∈1,…,Di. To calculate tentrancei, we initially set tD1i=sji. Similarly, the set of exit-from-cell instances, denoted by qexiti=qD1i,…,qDDii, is also calculated, where qDji marks the time instant that the vehicle exits entirely the cell ODj. Let Tj be the collection of all execution instances that lie within the closed interval tDji,qDji, where j∈1,…,Di, and let NTj=Tj. The feasibility of a candidate arrival time in Si is determined by the following function:(12)gis=∑m∈Di∑n∈TnBDmn.The above function is calculated sequentially for the elements ofSi; therefore, the search-space is revised as Si⋆=s∈Sigs=0 such that Si⋆⊆Si. The set Si⋆ will be referred to as the arrival times set of Ai.


### 3.2. Particle Swarm Optimization

The preliminary search process of the previous section results in a queue of arrival time sets; the queue’s order follows the sequence that the vehicles enter the autonomous intersection. The elements of the arrival time setSi⋆ correspond to the incoming vehicle’s Ai feasible arrival times to the IR. These feasible arrival times depend on the availability of the IR’s timetable. Apart from the availability of the IR, a feasible arrival time solution should further satisfy the safety and kinematic constraints of the vehicle. To this end, the sets of feasible arrival times are postprocessed sequentially, in a first-come first-serve manner, by a metaheuristic optimization routine, namely, the particle swarm optimization (PSO) algorithm. The PSO seeks the suboptimal arrival time that minimizes the delay of each vehicle and, at the same time, satisfies the vehicle’s safety and kinematic constraints.The PSO algorithm was introduced in 1995 by Kennedy and Eberhart [39, 40]; it belongs to a class of metaheuristic, intelligent self-learning methods, which induce near-optimal solutions for complex optimization problems. The PSO employs a group of moving particles (swarm) to seek a solution in the search-space. The motion of the particles within the search-space is determined by their position and velocity, while a temporary memory buffer stores the fittest (in mathematical optimization, the objective function (to be minimized or maximized) is also referred to as cost function, for minimization problems, or fitness function, for maximization problems. In the context of evolutionary programming, the terms objective and fitness functions are synonymous, regardless if the problem entails minimization or maximization. In this paper, we use an evolutionary computing method to solve a minimization problem; thus, to keep the terminology consistent for readers from both disciplines, we refer to a candidate solution as “fit” if it minimizes the objective (cost) function) location that has been explored in the search-space by the swarm [41]. At every iteration, the motion of each particle is determined based on the best-explored position of its own as well as the swarm. Therefore, the candidate solutions propagate towards the optima. Compared with other evolutionary optimization algorithms, the PSO requires less computational effort to solve complex problems [39, 42] and is relatively more robust to finding an optimal solution [43].Due to its computational efficiency and robustness, the PSO algorithm has a broad spectrum of application areas such as robotics [44, 45], image and video analysis [46, 47], antenna design [48], sensor networks [49], signal processing [50], and scheduling [51]. To the best of the authors’ knowledge, the application of the PSO algorithm to plan and coordinate the motion of autonomous vehicles at an intersection of a traffic network is novel.The optimization problem, which the PSO is assigned to solve, involves the unconstrained minimization (or maximization) of an objective functionϕX, where ϕ:ℝn⟶ℝ, X is a location in the search-space, and n is its dimension. The search-space is also called the domain of the optimization problem. The swarm of the PSO algorithm consists of Np particles, where each particle serves as a candidate solution of the optimization problem. Let Xa and Va, where a∈P=1,…,Np, be the position and velocity vectors in ℝn of the ath particle in the swarm, respectively. Each particle has a fitness value; that is, the value of the objective function at the particle’s position in the search-space, e.g., the fitness value of particle a with position Xa is ϕXa.The particles move around in the search-space, according to a simple set of rules that determine each particle’s velocity, to find a solutionX⋆ that minimizes the objective function, such that ϕX⋆≤ϕX for all locations, X, that have been visited by the swarm, in the search-space. Each particle’s movement is influenced by its own experience and the experience of the most successful particle in the swarm [42].The initial positions of the particles are randomly assigned in then-dimensional search-space of the optimization problem. The particle’s direction of motion in the search-space is guided by two elastic forces [52]. The first force attracts the particle, with a random magnitude, towards the best (fittest) position that has been explored by the swarm, denoted by Pg. The second pulls the particle, with a random magnitude, to the fittest position that has been discovered by itself, denoted by Pa. Figure 7 depicts the two forces that determine the trajectory of a particle in the search-space. The velocity of the ath particle in the swarm is calculated by the following equation:(13)Vatp+1=wntp⋅Vatp+c1⋅σ1⋅Pa−Xatp+c2⋅σ2⋅Pg−Xatp,where tp is the iteration number of the PSO algorithm, σ1 and σ2 are independent random variables with a uniform distribution in the closed interval 0,1, and c1 and c2 are the constant acceleration coefficients (the positive constants c1 and c2 are also referred to as the cognitive and social parameters of the swarm, respectively [42]). The acceleration coefficients encapsulate the particle’s confidence to its own success and the swarm’s success, and they are typically selected as c1=c2=2 [42]. The search process is terminated when a prespecified number of iterations, tpmax, has been reached. The final solution of the optimization problem is the swarm’s best discovered value, Pg.Figure 7
Graphical illustration of theath particle’s subcomponents that form its updated velocity vector, Vat+1, at the execution instant t of the PSO. The updated velocity is a linear combination of its current velocity, Vat, and its relative displacements from its own and the population’s fittest position values, Pat and Pgt, respectively.The termwn is the inertia weight; it balances the global and local search performance of the PSO algorithm [53]. A high value of the inertia weight yields an explorative global search while a smaller one results in a finer local search. In [53, 54], it was shown that the gradual decrease of the inertia weight, during the execution of the algorithm, notably improves the performance of the PSO. In similar fashion, we linearly decrease the inertia weight with the iteration number, tp, to balance the search response of the swarm. With this approach, at the early iterations, the PSO tends to have a more global search ability while towards the end of the execution it tends to have a more local one. Similarly to [42], the inertia weight is calculated by the following equation:(14)wntp+1=wmax−wmax−wmin⋅tptpmax,where wmax and wmin are the prespecified maximum and minimum values of the inertia weight, respectively. The maximum inertia weight, wmax, is generally selected as 1 while the minimum number, wmin, is set to 0.1 (close to zero) [42].After calculating the velocity vectorVa of particle a, its position is updated based on the following expression:(15)Xatp+1=Xatp+Vatp+1.According to (13), at each iteration, the velocity of a particle is a linear combination of its relative distances from its individual and the population’s fittest position values. Thus, all particles are attracted by Pg (the population’s current fittest position value), while keeping their own information (through Pa).The individual’s and population’s best visited positionsPa and Pg, respectively, are updated according to the fitness values of the particles. For example, in the case of a minimization problem, if ϕXatp+1<Pg, then the ath particle’s position becomes the population’s best discovered value, and Pg takes the value of Xatp+1. The individuals’ and population’s best values Pa and Pg, respectively, drive iteratively the particles of the swarm towards a satisfactory (suboptimal) solution of the optimization problem. The pseudocode of the PSO procedure is outlined in Algorithm 1.Algorithm 1:Pseudocode of the PSO.
Procedure: PSO
Input: Si⋆
Output: t¯i
Required: objective function
(1)
Draw randomlyNp elements from the set Si⋆.
(2)
Initialize the particles’ positions,Xa0, and fitness values, ϕXa0, for all a∈P.
(3)
FindPg based on the initial fitness values of the population.
(4)
t
p
⟵
1
(5)
While tp≤tpmax do
(6)
for i=1:Np do
(7)
Vatp+1⟵wn⋅Vatp+c1⋅α1⋅Pa−Xatp+c2⋅α2⋅Pg−Xatp ▷ Update the velocity of the ath particle.
(8)
wntp+1⟵wmax−wmax−wmin/tpmax⋅tp ▷ Update the inertia weight term of the ath particle.
(9)
Xatp+1⟵Xatp+Vatp+1 ▷ Update the position of the ath particle.
(10)
Calculate the fitness value,ϕXatp+1, of the ath particle.
(11)if ϕXatp+1<ϕPa then
(12)
Pa⟵Xatp+1
(13)
end if
(14)
if ϕXatp+1<ϕPg then
(15)
Pg⟵Xatp+1
(16)
end if
(17)
end for
(18)
tp⟵tp+1
(19)
end whileReturning to the intersection control problem, the PSO search is executed for every incoming vehicle. The initial candidate solutions,Np in the number, for the vehicle Ai are selected randomly from the set Si⋆. Thus, the position of the ath particle, Xa, represents a candidate arrival time, t¯ia, of Ai. As a reminder, we note that—by design—the selection of the latest arrival time, t¯il, of vehicle Ai guarantees that there exists at least one solution of the optimization problem in the search-space t¯iet¯il (see Section 3.1).Recall that the PSO is suitable for only unconstrained optimization problems. Therefore, we need to convert the original objective function—that is, the delay,ft¯i,t¯i—and its nonlinear constraints, described in (5), into a scalar function, ϕt¯i,t¯i, that encapsulates both the cost function and the constraint violations. In the following section, we show how the original constrained problem is recast into an unconstrained one, making it compatible with the PSO algorithm.


### 3.3. Constraint Handling Method

The intersection control problem entails the selection of each vehicle’s candidate arrival time at the IR that minimizes the delay function, described in Definition1, and satisfies the vehicle’s trajectory kinematic and safety constraints. We have manifested the intersection’s multivehicle collision-free motion control preconditions into a minimization problem, expressed in (5). Because of the vehicles’ nonholonomic kinematics and the compound form of their trajectory profile, the constraints of the minimization problem are nonlinear and complex.In the literature of mathematical optimization, metaheuristics are a practical and effective option for solving complex optimization problems [55]. However, as mentioned earlier, metaheuristics, and in particular PSO, are suitable for unconstrained optimization problems. Thereby, for optimization problems that include constraints, an additional mechanism is needed to handle the infeasible candidate solutions, which fail to satisfy the given constraints.In this paper, we adopt the penalty method that methodically converts the constrained optimization problem into an unconstrained one [56, 57]. According to this formulation, an auxiliary term (penalty term) is added to the objective function of the original constrained problem. For a minimization search, the penalty term adds positive value to every infeasible solution—a solution is infeasible if it does not satisfy the constraints of the optimization problem—that signifies the level of violation of each constraint.The value of the penalty function is a measure of the candidate solution’s constraint violations severity. The number of constraint violations, which correspond to a candidate solution, determines the contribution of the penalty to the objective function of the unconstrained problem (for clarity (and brevity), the principal minimization problem of the intersection’s motion control application, expressed in (5), will be referred to as the “constrained” optimization problem and the delay function, f⋅,⋅, of Definition 1 as the “constrained” objective function, whereas its unconstrained counterpart, suitable for the PSO search and described in Section 3.2, will be referred to as the “unconstrained” optimization problem and the objective function ϕ⋅,⋅ as the “unconstrained” objective function). If for a candidate solution there is no violation of any constraint, then the penalty term is zero, and the resultant objective function becomes identical with the one of the original constrained problem. The penalty function also is a means of making use of the infeasible solutions instead of discarding them (also known as the death penalty method). The disadvantage of entirely discarding the infeasible solutions is that the information that they may contain will be lost.The penalty function can be designed as static or nonstatic [58, 59]. Typically, the static penalty function is formulated as a weighted sum of the constraint violations [60]. However, this approach is highly problem-oriented because the designer needs to determine the contribution of each constraint violation into the penalty amount [55]. The nonstatic penalty function requires no explicit parameter tuning; the penalty amount that is added to each constraint violation is adaptively controlled such that the infeasible solutions gradually converge to the feasible solution region [61].In this work, we adopt a self-adaptive penalty function that has been reported in [59]. This formulation utilizes the information and experience that is gained through the search process to regulate the penalty amount added to the infeasible solutions. The goal is to derive a new objective function that encapsulates the fitness of the original one (constrained problem) and the penalty values that quantify the constraint violations. To directly connect the constraint handling method with the PSO search formulation for the intersection control problem, the new objective function (of the unconstrained problem), ϕt¯ia,t¯i, of an incoming vehicle Ai is parameterized by the candidate solution that corresponds to particle a∈P (arrival time t¯ia) of the swarm and the vehicle’s entrance time to the CR, t¯i.The self-adaptive penalty method replaces the objective function of the constrained optimization problem with a metric called distance value,ρt¯ia,t¯i, which combines the normalized constrained objective function (normalized delay) with the constraint violations. In the absence of feasible particles in the swarm, the distance value allows to distinguish the particles with low constraint violations from the ones with high constraint violations. By doing this, the search will progressively convey to the feasible region. The distance value is blended with a penalty term, βt¯ia,t¯i, to form the unconstrained objective function, ϕt¯ia,t¯i, and the candidate solutions are ranked based on their fitness values. Therefore, the unconstrained optimization formulation of the intersection motion coordination problem is defined as follows:(16)minimizeϕt¯ia,t¯i=ρt¯ia,t¯i+βt¯ia,t¯i.Starting with the formulation of the distance value,ρt¯ia,t¯i, the value of the objective function of the constrained problem—that is, the delay to the CR, ft¯ia,t¯i—is normalized as follows:(17)f^t¯ia,t¯i=ft¯ia,t¯i−f¯f¯−f¯,where f¯ and f¯ are the minimum and maximum values of the constrained objective function amongst all the candidate solutions, respectively. Let Nf be the number of particles in the swarm that satisfy all the constraints of the optimization problem. The ratio of the particles that satisfy the constraints (feasible particles) is calculated by the following equation:(18)rf=NfNp.The distance value,ρt¯i,t¯i, is computed as follows:(19)ρt¯ia,t¯i=αt¯ia,t¯i,ifrf=0;f^t¯ia,t¯i+αt¯ia,t¯i,otherwise;where αt¯ia,t¯i is the overall measure of all constraint violations, and it will be referred to as the infeasibility measure of a solution. A detailed description of the calculation of the infeasibility measure is given in Section 3.4.3. The self-adaptive penalty utilizes the population of feasible solutions in the swarm to control the penalty amount that will be added to the infeasible solutions. When the majority of the swarm is comprised of feasible particles, a penalty is added to the particles that have a high fitness value. In the case where the minority of the swarm’s population consists of feasible solutions, the particles that have a large number of constraint violations are penalized significantly. Thus, the penalty term, βt¯ia,t¯i, is defined as follows:(20)βt¯ia,t¯i=1−rf⋅γt¯ia,t¯i+rf⋅ζt¯ia,t¯i,where(21)γt¯ia,t¯i=0,ifrf=0;αt¯ia,t¯i,otherwise;ζt¯ia,t¯i=0,ift¯iais a feasible particle;f^t¯ia,t¯i,ift¯iais an  infeasible particle.If the infeasible solutions comprise of the swarm’s majority, the first term,γt¯ia,t¯i, will dominate the penalty value. Therefore, each solution will be penalized based on its overall constraint violations. In the case of minor amount of infeasible solutions in the swarm, then the second term, ζt¯ia,t¯i, will dominate the penalty value. The candidate solutions that have a high fitness value will be added more penalty than the ones with a low value.


### 3.4. Severity of Constraint Violations: Calculating the Infeasibility Measure

The previous section described how the constrained optimization problem could be converted into an unconstrained one using the penalty function method. This conversion is deemed necessary because the PSO metaheuristic is applicable only to unconstrained optimization searches. The penalty function method relies on the incorporation of an auxiliary additive term, which is referred to as thepenalty term, to the objective function. Its purpose is to retain the information collected by the infeasible solutions, instead of discarding it, such that they gradually converge to the feasible space.The distance valueρt¯ia,t¯i and the self-adaptive penalty term βt¯ia,t¯i of the unconstrained minimization problem, given in (16), require the determination of the infeasibility measure αt¯ia,t¯i. The latter should represent both the number of constraints that are activated and the severity to which each constraint is violated [62]. In this work, we adopt the self-adaptive penalty method presented in [55, 59, 62].Contrary to a typical optimization search, the intersection control problem requires a sequence of intermediate steps before calculating the infeasibility measure of the candidate solutions. For a given candidate solution, the motion of the vehicle is projected to both the control and intersection regions to determine if the safety and kinematic constraints are violated.A candidate arrival time constitutes a viable solution to the optimization problem if the incoming vehicle’s trajectory satisfies specific requirements regarding its safety and motion characteristics. In summary, the safety requirements necessitate that two or more vehicles do not overlap (collide) while conveying in the control and intersection regions. The motion characteristic conditions limit the trajectory of the vehicles to certain acceptable velocity and acceleration profiles when the vehicle is transporting in the CR.This section presents in detail the process that is used to define the infeasibility measure, of the self-adaptive penalty method, by quantifying the constraint violations of the candidate solutions. This process incorporates three intermediate procedures: the Trajectory Generation, the Motion Projection, and the Quantification of Constraint Violations. The subsequent steps are described for an arbitrary vehicleAi that enters the IR at a time instant t¯i and has a candidate arrival time t¯ia.


#### 3.4.1. Trajectory Generation

The first step towards the calculation of each solution’s (vehicle’s candidate arrival time) infeasibility measure is to generate the incoming vehicle’s trajectory in the CR based on its candidate arrival time. Thus, each incoming vehicle’s velocity profile, in the CR, is parameterized by its arrival time.The trajectory of each vehicle in the CR is generated based on two kinematic conditions. The first requires that the vehicles shall not exceed their maximum velocity and acceleration bounds,vmax and amax, respectively. The second necessitates that the vehicles enter and exit the CR with their maximum velocity, vmax. To minimize the number of configuration parameters of the trajectories, we limit the vehicle’s velocity profile to have a trapezoidal form consisting of three distinct periods: constant deceleration, advancement with constant speed (zero acceleration), and constant acceleration (the deceleration period of the vehicle’s trapezoidal velocity profile, within the CR, must proceed the acceleration period such that the vehicle’s speed, at any instant, does not exceed its maximum admissible value, vmax). Furthermore, all three periods have equal duration, and the absolute values of the deceleration and acceleration are also equal. Hence, the acceleration profile is comprised of two opposite pulses of same area.Figure8 illustrates the velocity and acceleration profiles of Ai and the sequence of the distinct acceleration periods. The expected time duration where Ai will be conveying in the CR is defined as follows:(22)tga=t¯ia−t¯i.Figure 8
Velocity and acceleration profiles of vehicleAi. The motion of the vehicles is broken down to three distinct periods: constant deceleration, advancement with constant speed, and constant acceleration.As mentioned earlier, the projected time duration ofAi in the CR, tga, is further subdivided into three equal time intervals. Each interval corresponds to one of the vehicle’s trajectory acceleration periods, and their respective duration is denoted by tqa=tga/3. With the trapezoidal formulation of the velocity profile, the only parameter that needs to be determined is the vehicle’s constant speed vla during its zero acceleration phase (advancement with constant speed). This value is conveniently calculated by noting that the area under the velocity curve in the interval t¯it¯ia is equal to the displacement of Ai during that time interval, i.e., the length of the CR, L. Using elementary calculations, the velocity of the vehicle at its zero acceleration period is given by(23)vla=L−vmax⋅tqa2⋅tqa.The constant acceleration that is needed to bring the velocity ofAi from vla to vmax in a linear manner, during the acceleration period, is calculated by the following equation:(24)v˙a=vmax−vlatqa.The above value also represents the slope of the velocity’s straight line segment at the acceleration phase. The vehicle’s constant deceleration, at the first phase of its trajectory, is equal to−v˙a. Finally, the velocity of Ai in the CR, for all three periods of the trajectory’s profile, is expressed as follows:(25)vit,t¯i,t¯ia=vmax−v˙a⋅t−t¯i,t¯i≤t≤t¯i+tqa;vla,t¯i+tqa<t≤t¯i+2⋅tqa;vla+v˙a⋅t−t¯i+2⋅tqa,t¯i+2⋅tqa<t≤t¯i+3⋅tqa;where t∈t¯i,t¯ia.


#### 3.4.2. Motion Projection

The second step involves the projection of the motion of each vehicle, in the control and intersection regions, according to its generated velocity profile. It should be reminded that the vehicles transport through the IR with constant velocity. The projected motion, which corresponds to a candidate arrival time of the vehicle, is utilized to evaluate the part of the feasibility measure that pertains to its safety requirements.To begin with, we calculate the total duration that each vehicle spends at the autonomous intersection. This duration is the sum of the time spans that the vehicle conveys in the control and intersection regions, respectively. Clearly, the vehicle’s total duration at the intersection depends on its candidate arrival time. The total duration at the intersectionπia, of vehicle Ai and corresponds to the candidate arrival time t¯ia, is calculated by the following equation:(26)πia=t¯ia−t¯i+τi.Using the kinematic expressions given in (2) and the generated velocity profile of (25), this procedure projects the motion of the vehicle in the time horizon πia. Figure 9 illustrates how the Motion Projection procedure maps a vehicle’s motion in the control and intersection regions. After determining the total duration πia, a structure with all the discrete execution instances in the interval t¯i,t¯i+πia is formed; let Tia be the set of all these instances. The Motion Projection process finalizes by determining the set of postures, denoted by χia, that Ai will have at all instances of Tia.Figure 9
Illustration of the projected motion of vehicleAi during its movement, with duration πia, across the control and intersection regions, respectively, for the time instances of the set Tia.


#### 3.4.3. Quantification of Constraint Violations

The final step of the constraint handling process is dedicated to quantifying the level of violation of each constraint by a candidate solution. The objective is to capture the safety conflicts, in both the control and intersection regions, and also record violations of the vehicles’ kinematic limitations. The identification of safety conflicts in the IR is performed similarly to the reservation paradigm that was introduced in [7].The first constraint captures conflicts between vehicles in the IR that have not been identified at the preliminary search, e.g., the case where a vehicle overlays multiple cells. To capture these conflicts, initially, the resolution of the grid array is refined by a factor ofμ∈Z+. The refined grid array has cr=μ⋅NR/2⋅NL columns, rr=μ⋅NR/2⋅NL rows, and the cells’ length is er=e/μ. Each cell is identified by its row and column indexes as EK,L, where K∈ℛr=1,…,rr and L∈Cr=1,…,cr. A timetable entry BK,Lrt is assigned to the refined cell EK,L. If the latter is occupied at the execution instant t, then we set BK,Lrt=1; otherwise, BK,Lr=0. Before mapping the projected motion of Ai to the refined cells of the intersection, a space-buffer is applied to add a safety distance between the vehicles in the IR. The dimensions of the vehicle are artificially expanded by a factor κ>1 such that its new dimensions become l′=l⋅κ and w′=w⋅κ. The refined grid array of the IR is shown in Figure 10.Figure 10
Illustration of the refined grid array at the IR. Inside the IR, the dimensions of the vehicles are artificially expanded as a means to introduce a space-buffer for their collision-free motion. The colored elements represent the occupied cells of the IR.The occupancy mapping process, forAi, entails the determination of the set of occupied cells Pit in the refined IR that are encapsulated by the expanded dimensions of Ai at time t. Let ℐit=I1,I2,…,IPi be the set of indices that correspond to the elements (cells) of Pit. The number of violations that the candidate solution t¯ia attains, when Ai moves in the IR, is calculated by the following equation:(27)η1t¯ia,t¯i=∑m∈τn∑n=1ℐimBInrm,where τn denotes the collection of all discrete execution instances that belong to the closed interval t¯ia,t¯ia+τi. In the CR, the safety conflict between two same-lane vehicles is detected by inspecting the distance between them. The number of violations of Ai in the CR is given by(28)η2t¯ia,t¯i=∑m∈τcbcm,where τc denotes the set of execution instances in the closed interval t¯i,t¯ia, which corresponds to the duration that Ai conveys in the CR, and bc is a binary decision variable that signals whether or not there is a safety conflict between Ai and another vehicle in the lane. The decision variable bc is defined as follows:(29)bcm=0,ifl′−Xiam−Xpm≤0;1,otherwise.The termXp represents the coordinates of the same lane, succeeding vehicles to Ai that convey in the CR at the time instant m, and l′ (see definition of η1) is the artificially expanded vehicles’ length that adds a safe distance between vehicles in the CR. The remaining violations are assessed based on the kinematic constraints of the vehicles. Firstly, the (binary) acceleration violation value of the trapezoidal velocity’s profile is given by(30)η3t¯ia,t¯i=0,ifv˙a∈−amax,amax;1,otherwise.In a similar fashion, the (binary) velocity violation value of the vehicle’s trajectory is expressed as follows:(31)η4t¯ia,t¯i=0,ifvla∈0,vmax;1,otherwise.The overarching objective of the constraint handling method is to quantify all the constraint violations for each candidate solution. Therefore, the net value of the constraint violations of a candidate solutiont¯ia is expressed as the weighted mean of all four constraint violation functions, given by(32)αt¯ia,t¯i=14∑m=14ηmt¯ia,t¯iηmmax,where the term ηmmax is the maximum possible number of violations of the function ηm. The values of η1max of η2max are τn⋅ℐim and τc, respectively. These two functions obtain their maximum value when the vehicle constantly overlaps with another one, when moving in the control and intersection regions, respectively. For the kinematic constraints, we have η3max=η4max=1. A synoptic illustration in block diagrammatic form of the search process for the determination of the arrival time t¯i of vehicle Ai, based on its entry time t¯i, is shown in Figure 11.Figure 11
Block diagram of the procedures, and their respective interconnections, associated with the determination of the arrival timet¯i for vehicle Ai based on the entry time t¯i.


## 3.1. Preliminary Feasibility Search of Candidate Solutions

In this section, we show how the search-space of the optimization task is determined. The output of this preliminary processing operation is the assignment of a set of arrival times, to every incoming vehicle, that serve as initial candidate solutions for the optimization task. These candidate solutions are further refined during the optimization search. The IC schedules the arrival time of each incoming vehicle, and based on this value, it shapes the vehicle’s linear velocity time profile in the CR. It is expected that each vehicle enters and exits the CR with its maximum velocity value,vmax.We assume that the routes of the vehicles have already been determined when they enter the CR and this information is available to the IC. For simplicity, we further assume that the vehicles do not change lanes, whether they remain on the same road or change direction by making a turn. However, the proposed IC can be easily modified to accommodate routes within the intersection that involve both direction and lane change.The first task of the IC is to identify the intersection cells that the traversing vehicles will occupy along their motion in the IR—as mentioned earlier, the path of each vehicle in the IR is predetermined. LetOi denote the ordered set of cells that the vehicle Ai occupies while it traverses the IR. Each cell OI,J∈Oi is assigned with a unique service time TI,J, representing the occupancy duration of OI,J from Ai. The service time TI,J is determined by the motion of the vehicle within the cell. Each vehicle can move straight or execute a quarter turn. If the vehicle does not change direction, the occupancy duration of the cell OI,J is TI,Jstraight=e/vmax. When changing directions, the vehicles move straight until reaching the cell of the IR that corresponds to their turning lane. During the turn maneuver, the vehicles retain the value of their linear velocity; therefore, the occupancy duration of the “turning” cells is calculated by TI,Jturn=π⋅R/2⋅vmax, where R is the turning radius of the vehicles R=e/2. Figure 4 shows the two admissible trajectories of the vehicles (move straight and make a turn) within the IR and their respective duration.Figure 4
The two types of paths within the IR and service times of cells that correspond to straight and turning segments of paths, respectively. When a vehicle makes a turn, it maintains the same lane in its exit road with its entry road.The model of the turning trajectories within the IR, which consists of two straight paths and a quarter circle in between, is selected to illustrate, in an uncomplicated manner, the calculation of the total service time of a vehicle within the IR. The turning radiuse can be increased, by expanding the width of the lanes, to reduce the angular velocity of the vehicles in the single-cell quarter turns in order to meet the comfort levels of the passengers while the vehicle is turning. Additionally, this simplified path can be substituted effortlessly by a curved trajectory that connects the entry and exit points of the turning vehicle in the IR and produces a lower angular velocity for the turning vehicle. In this case, the corresponding total service time of the vehicle has to be redetermined accordingly. The optimization and design of curvilinear paths is outside the scope of this work; we refer the interested reader to [37, 38].LetDi be the ordered set with elements the indices pairs of the cells that belong to Oi, such that Di=D1,D2,…,DOi. The total service time that is required for Ai to travel through the IR is given by(7)τi=∑m=1DiTDm+tv,where(8)tv=lvmax,is the constant duration needed for the vehicle to exit the last cell of its route in the IR.Initially, the IC determines the time window that serves as the search-space of the optimization routine for seeking the optimal value of each vehicle’s arrival time to the intersection. The duration of this time window is selected wide enough to ensure that at least one solution exists within the search-space.A conservative value of the search window’s upper bound is selected by assuming that each incoming vehicle will convey the longest possible path in the intersection while all lanes are experiencing maximum congestion. The longest possible path is illustrated in Figure5 for a generic intersection model. The search-space’s upper bound (“latest” arrival time) is calculated by the following equation:(9)t¯il=t¯i+Ll⋅δmax⋅NR⋅NL,where ⌈⋅⌉ denotes the ceiling function, ⌈L/l⌉ is the maximum number of vehicles that can accumulate in a single lane of the CR without colliding (maximum lane capacity), and δmax is the time duration that is required for the vehicles to convey the longest possible path in the IR (maximum service time). The maximum lane capacity and the maximum service time are multiplied by the product of roads times lanes to determine the total time that is required to serve all other vehicles before Ai arrives at the intersection—assuming that all CRs are fully congested. For an arbitrary intersection layout, the service time of the longest path in the IR, δmax, is calculated by the following equation:(10)δmax=NRNL−2TI,Jstraight+TI,Jturn+lvmax=4NRNL−2e+πe+4l4vmax.Figure 5
The longest possible path that a vehicle can travel in the IR. It is traveled by the vehicles that transport at the outermost right lane of the CR and make a left turn at the IR.The “earliest” arrival time,t¯ie, occurs when a vehicle does not experience any delay in the CR and is calculated by the following equation:(11)t¯ie=t¯i+ts.Therefore, the search-space ofAi is defined as Si=s∈ℝ+t¯ie≤s≤t¯il. A preliminary feasible arrival time signifies that all the requested cells in Oi are unoccupied and available to reserve for the duration of the service times (see Figure 6). The occupancy schedule of the intersection is represented by assigning a timetable BI,J to the cell OI,J of the IR, where BI,Jt=0 signals that the cell OI,J is available at time t, and BI,Jt=1 that is occupied.Figure 6
Graphical illustration of the intersection’s reservation timetable. The timetable is a buffer that stores for each cell of the IR its occupancy information with respect to time. For an arbitrary vehicleAi, the preliminary feasibility search identifies the elements of the set Si that correspond to an unreserved trajectory at the IR using the timetable.Assume that the vehicleAi is set to traverse in the IR over a path represented by the set Oi. The actual search-space is comprised of the discrete execution time instances of the algorithm that lie within the time interval Si. Denote by Si=s1i,…,sNiisji∈Si the set of candidate arrival times of Ai, where Ni is the total number of discrete execution time instances within Si.Subsequently, the IC determines the set of entrance-to-cells time instances denoted astentrancei=tD1i,…,tDDii, where tDji stands for the time instant that Ai enters the cell ODj with j∈1,…,Di. To calculate tentrancei, we initially set tD1i=sji. Similarly, the set of exit-from-cell instances, denoted by qexiti=qD1i,…,qDDii, is also calculated, where qDji marks the time instant that the vehicle exits entirely the cell ODj. Let Tj be the collection of all execution instances that lie within the closed interval tDji,qDji, where j∈1,…,Di, and let NTj=Tj. The feasibility of a candidate arrival time in Si is determined by the following function:(12)gis=∑m∈Di∑n∈TnBDmn.The above function is calculated sequentially for the elements ofSi; therefore, the search-space is revised as Si⋆=s∈Sigs=0 such that Si⋆⊆Si. The set Si⋆ will be referred to as the arrival times set of Ai.


## 3.2. Particle Swarm Optimization

The preliminary search process of the previous section results in a queue of arrival time sets; the queue’s order follows the sequence that the vehicles enter the autonomous intersection. The elements of the arrival time setSi⋆ correspond to the incoming vehicle’s Ai feasible arrival times to the IR. These feasible arrival times depend on the availability of the IR’s timetable. Apart from the availability of the IR, a feasible arrival time solution should further satisfy the safety and kinematic constraints of the vehicle. To this end, the sets of feasible arrival times are postprocessed sequentially, in a first-come first-serve manner, by a metaheuristic optimization routine, namely, the particle swarm optimization (PSO) algorithm. The PSO seeks the suboptimal arrival time that minimizes the delay of each vehicle and, at the same time, satisfies the vehicle’s safety and kinematic constraints.The PSO algorithm was introduced in 1995 by Kennedy and Eberhart [39, 40]; it belongs to a class of metaheuristic, intelligent self-learning methods, which induce near-optimal solutions for complex optimization problems. The PSO employs a group of moving particles (swarm) to seek a solution in the search-space. The motion of the particles within the search-space is determined by their position and velocity, while a temporary memory buffer stores the fittest (in mathematical optimization, the objective function (to be minimized or maximized) is also referred to as cost function, for minimization problems, or fitness function, for maximization problems. In the context of evolutionary programming, the terms objective and fitness functions are synonymous, regardless if the problem entails minimization or maximization. In this paper, we use an evolutionary computing method to solve a minimization problem; thus, to keep the terminology consistent for readers from both disciplines, we refer to a candidate solution as “fit” if it minimizes the objective (cost) function) location that has been explored in the search-space by the swarm [41]. At every iteration, the motion of each particle is determined based on the best-explored position of its own as well as the swarm. Therefore, the candidate solutions propagate towards the optima. Compared with other evolutionary optimization algorithms, the PSO requires less computational effort to solve complex problems [39, 42] and is relatively more robust to finding an optimal solution [43].Due to its computational efficiency and robustness, the PSO algorithm has a broad spectrum of application areas such as robotics [44, 45], image and video analysis [46, 47], antenna design [48], sensor networks [49], signal processing [50], and scheduling [51]. To the best of the authors’ knowledge, the application of the PSO algorithm to plan and coordinate the motion of autonomous vehicles at an intersection of a traffic network is novel.The optimization problem, which the PSO is assigned to solve, involves the unconstrained minimization (or maximization) of an objective functionϕX, where ϕ:ℝn⟶ℝ, X is a location in the search-space, and n is its dimension. The search-space is also called the domain of the optimization problem. The swarm of the PSO algorithm consists of Np particles, where each particle serves as a candidate solution of the optimization problem. Let Xa and Va, where a∈P=1,…,Np, be the position and velocity vectors in ℝn of the ath particle in the swarm, respectively. Each particle has a fitness value; that is, the value of the objective function at the particle’s position in the search-space, e.g., the fitness value of particle a with position Xa is ϕXa.The particles move around in the search-space, according to a simple set of rules that determine each particle’s velocity, to find a solutionX⋆ that minimizes the objective function, such that ϕX⋆≤ϕX for all locations, X, that have been visited by the swarm, in the search-space. Each particle’s movement is influenced by its own experience and the experience of the most successful particle in the swarm [42].The initial positions of the particles are randomly assigned in then-dimensional search-space of the optimization problem. The particle’s direction of motion in the search-space is guided by two elastic forces [52]. The first force attracts the particle, with a random magnitude, towards the best (fittest) position that has been explored by the swarm, denoted by Pg. The second pulls the particle, with a random magnitude, to the fittest position that has been discovered by itself, denoted by Pa. Figure 7 depicts the two forces that determine the trajectory of a particle in the search-space. The velocity of the ath particle in the swarm is calculated by the following equation:(13)Vatp+1=wntp⋅Vatp+c1⋅σ1⋅Pa−Xatp+c2⋅σ2⋅Pg−Xatp,where tp is the iteration number of the PSO algorithm, σ1 and σ2 are independent random variables with a uniform distribution in the closed interval 0,1, and c1 and c2 are the constant acceleration coefficients (the positive constants c1 and c2 are also referred to as the cognitive and social parameters of the swarm, respectively [42]). The acceleration coefficients encapsulate the particle’s confidence to its own success and the swarm’s success, and they are typically selected as c1=c2=2 [42]. The search process is terminated when a prespecified number of iterations, tpmax, has been reached. The final solution of the optimization problem is the swarm’s best discovered value, Pg.Figure 7
Graphical illustration of theath particle’s subcomponents that form its updated velocity vector, Vat+1, at the execution instant t of the PSO. The updated velocity is a linear combination of its current velocity, Vat, and its relative displacements from its own and the population’s fittest position values, Pat and Pgt, respectively.The termwn is the inertia weight; it balances the global and local search performance of the PSO algorithm [53]. A high value of the inertia weight yields an explorative global search while a smaller one results in a finer local search. In [53, 54], it was shown that the gradual decrease of the inertia weight, during the execution of the algorithm, notably improves the performance of the PSO. In similar fashion, we linearly decrease the inertia weight with the iteration number, tp, to balance the search response of the swarm. With this approach, at the early iterations, the PSO tends to have a more global search ability while towards the end of the execution it tends to have a more local one. Similarly to [42], the inertia weight is calculated by the following equation:(14)wntp+1=wmax−wmax−wmin⋅tptpmax,where wmax and wmin are the prespecified maximum and minimum values of the inertia weight, respectively. The maximum inertia weight, wmax, is generally selected as 1 while the minimum number, wmin, is set to 0.1 (close to zero) [42].After calculating the velocity vectorVa of particle a, its position is updated based on the following expression:(15)Xatp+1=Xatp+Vatp+1.According to (13), at each iteration, the velocity of a particle is a linear combination of its relative distances from its individual and the population’s fittest position values. Thus, all particles are attracted by Pg (the population’s current fittest position value), while keeping their own information (through Pa).The individual’s and population’s best visited positionsPa and Pg, respectively, are updated according to the fitness values of the particles. For example, in the case of a minimization problem, if ϕXatp+1<Pg, then the ath particle’s position becomes the population’s best discovered value, and Pg takes the value of Xatp+1. The individuals’ and population’s best values Pa and Pg, respectively, drive iteratively the particles of the swarm towards a satisfactory (suboptimal) solution of the optimization problem. The pseudocode of the PSO procedure is outlined in Algorithm 1.Algorithm 1:Pseudocode of the PSO.
Procedure: PSO
Input: Si⋆
Output: t¯i
Required: objective function
(1)
Draw randomlyNp elements from the set Si⋆.
(2)
Initialize the particles’ positions,Xa0, and fitness values, ϕXa0, for all a∈P.
(3)
FindPg based on the initial fitness values of the population.
(4)
t
p
⟵
1
(5)
While tp≤tpmax do
(6)
for i=1:Np do
(7)
Vatp+1⟵wn⋅Vatp+c1⋅α1⋅Pa−Xatp+c2⋅α2⋅Pg−Xatp ▷ Update the velocity of the ath particle.
(8)
wntp+1⟵wmax−wmax−wmin/tpmax⋅tp ▷ Update the inertia weight term of the ath particle.
(9)
Xatp+1⟵Xatp+Vatp+1 ▷ Update the position of the ath particle.
(10)
Calculate the fitness value,ϕXatp+1, of the ath particle.
(11)if ϕXatp+1<ϕPa then
(12)
Pa⟵Xatp+1
(13)
end if
(14)
if ϕXatp+1<ϕPg then
(15)
Pg⟵Xatp+1
(16)
end if
(17)
end for
(18)
tp⟵tp+1
(19)
end whileReturning to the intersection control problem, the PSO search is executed for every incoming vehicle. The initial candidate solutions,Np in the number, for the vehicle Ai are selected randomly from the set Si⋆. Thus, the position of the ath particle, Xa, represents a candidate arrival time, t¯ia, of Ai. As a reminder, we note that—by design—the selection of the latest arrival time, t¯il, of vehicle Ai guarantees that there exists at least one solution of the optimization problem in the search-space t¯iet¯il (see Section 3.1).Recall that the PSO is suitable for only unconstrained optimization problems. Therefore, we need to convert the original objective function—that is, the delay,ft¯i,t¯i—and its nonlinear constraints, described in (5), into a scalar function, ϕt¯i,t¯i, that encapsulates both the cost function and the constraint violations. In the following section, we show how the original constrained problem is recast into an unconstrained one, making it compatible with the PSO algorithm.


## 3.3. Constraint Handling Method

The intersection control problem entails the selection of each vehicle’s candidate arrival time at the IR that minimizes the delay function, described in Definition1, and satisfies the vehicle’s trajectory kinematic and safety constraints. We have manifested the intersection’s multivehicle collision-free motion control preconditions into a minimization problem, expressed in (5). Because of the vehicles’ nonholonomic kinematics and the compound form of their trajectory profile, the constraints of the minimization problem are nonlinear and complex.In the literature of mathematical optimization, metaheuristics are a practical and effective option for solving complex optimization problems [55]. However, as mentioned earlier, metaheuristics, and in particular PSO, are suitable for unconstrained optimization problems. Thereby, for optimization problems that include constraints, an additional mechanism is needed to handle the infeasible candidate solutions, which fail to satisfy the given constraints.In this paper, we adopt the penalty method that methodically converts the constrained optimization problem into an unconstrained one [56, 57]. According to this formulation, an auxiliary term (penalty term) is added to the objective function of the original constrained problem. For a minimization search, the penalty term adds positive value to every infeasible solution—a solution is infeasible if it does not satisfy the constraints of the optimization problem—that signifies the level of violation of each constraint.The value of the penalty function is a measure of the candidate solution’s constraint violations severity. The number of constraint violations, which correspond to a candidate solution, determines the contribution of the penalty to the objective function of the unconstrained problem (for clarity (and brevity), the principal minimization problem of the intersection’s motion control application, expressed in (5), will be referred to as the “constrained” optimization problem and the delay function, f⋅,⋅, of Definition 1 as the “constrained” objective function, whereas its unconstrained counterpart, suitable for the PSO search and described in Section 3.2, will be referred to as the “unconstrained” optimization problem and the objective function ϕ⋅,⋅ as the “unconstrained” objective function). If for a candidate solution there is no violation of any constraint, then the penalty term is zero, and the resultant objective function becomes identical with the one of the original constrained problem. The penalty function also is a means of making use of the infeasible solutions instead of discarding them (also known as the death penalty method). The disadvantage of entirely discarding the infeasible solutions is that the information that they may contain will be lost.The penalty function can be designed as static or nonstatic [58, 59]. Typically, the static penalty function is formulated as a weighted sum of the constraint violations [60]. However, this approach is highly problem-oriented because the designer needs to determine the contribution of each constraint violation into the penalty amount [55]. The nonstatic penalty function requires no explicit parameter tuning; the penalty amount that is added to each constraint violation is adaptively controlled such that the infeasible solutions gradually converge to the feasible solution region [61].In this work, we adopt a self-adaptive penalty function that has been reported in [59]. This formulation utilizes the information and experience that is gained through the search process to regulate the penalty amount added to the infeasible solutions. The goal is to derive a new objective function that encapsulates the fitness of the original one (constrained problem) and the penalty values that quantify the constraint violations. To directly connect the constraint handling method with the PSO search formulation for the intersection control problem, the new objective function (of the unconstrained problem), ϕt¯ia,t¯i, of an incoming vehicle Ai is parameterized by the candidate solution that corresponds to particle a∈P (arrival time t¯ia) of the swarm and the vehicle’s entrance time to the CR, t¯i.The self-adaptive penalty method replaces the objective function of the constrained optimization problem with a metric called distance value,ρt¯ia,t¯i, which combines the normalized constrained objective function (normalized delay) with the constraint violations. In the absence of feasible particles in the swarm, the distance value allows to distinguish the particles with low constraint violations from the ones with high constraint violations. By doing this, the search will progressively convey to the feasible region. The distance value is blended with a penalty term, βt¯ia,t¯i, to form the unconstrained objective function, ϕt¯ia,t¯i, and the candidate solutions are ranked based on their fitness values. Therefore, the unconstrained optimization formulation of the intersection motion coordination problem is defined as follows:(16)minimizeϕt¯ia,t¯i=ρt¯ia,t¯i+βt¯ia,t¯i.Starting with the formulation of the distance value,ρt¯ia,t¯i, the value of the objective function of the constrained problem—that is, the delay to the CR, ft¯ia,t¯i—is normalized as follows:(17)f^t¯ia,t¯i=ft¯ia,t¯i−f¯f¯−f¯,where f¯ and f¯ are the minimum and maximum values of the constrained objective function amongst all the candidate solutions, respectively. Let Nf be the number of particles in the swarm that satisfy all the constraints of the optimization problem. The ratio of the particles that satisfy the constraints (feasible particles) is calculated by the following equation:(18)rf=NfNp.The distance value,ρt¯i,t¯i, is computed as follows:(19)ρt¯ia,t¯i=αt¯ia,t¯i,ifrf=0;f^t¯ia,t¯i+αt¯ia,t¯i,otherwise;where αt¯ia,t¯i is the overall measure of all constraint violations, and it will be referred to as the infeasibility measure of a solution. A detailed description of the calculation of the infeasibility measure is given in Section 3.4.3. The self-adaptive penalty utilizes the population of feasible solutions in the swarm to control the penalty amount that will be added to the infeasible solutions. When the majority of the swarm is comprised of feasible particles, a penalty is added to the particles that have a high fitness value. In the case where the minority of the swarm’s population consists of feasible solutions, the particles that have a large number of constraint violations are penalized significantly. Thus, the penalty term, βt¯ia,t¯i, is defined as follows:(20)βt¯ia,t¯i=1−rf⋅γt¯ia,t¯i+rf⋅ζt¯ia,t¯i,where(21)γt¯ia,t¯i=0,ifrf=0;αt¯ia,t¯i,otherwise;ζt¯ia,t¯i=0,ift¯iais a feasible particle;f^t¯ia,t¯i,ift¯iais an  infeasible particle.If the infeasible solutions comprise of the swarm’s majority, the first term,γt¯ia,t¯i, will dominate the penalty value. Therefore, each solution will be penalized based on its overall constraint violations. In the case of minor amount of infeasible solutions in the swarm, then the second term, ζt¯ia,t¯i, will dominate the penalty value. The candidate solutions that have a high fitness value will be added more penalty than the ones with a low value.


## 3.4. Severity of Constraint Violations: Calculating the Infeasibility Measure

The previous section described how the constrained optimization problem could be converted into an unconstrained one using the penalty function method. This conversion is deemed necessary because the PSO metaheuristic is applicable only to unconstrained optimization searches. The penalty function method relies on the incorporation of an auxiliary additive term, which is referred to as thepenalty term, to the objective function. Its purpose is to retain the information collected by the infeasible solutions, instead of discarding it, such that they gradually converge to the feasible space.The distance valueρt¯ia,t¯i and the self-adaptive penalty term βt¯ia,t¯i of the unconstrained minimization problem, given in (16), require the determination of the infeasibility measure αt¯ia,t¯i. The latter should represent both the number of constraints that are activated and the severity to which each constraint is violated [62]. In this work, we adopt the self-adaptive penalty method presented in [55, 59, 62].Contrary to a typical optimization search, the intersection control problem requires a sequence of intermediate steps before calculating the infeasibility measure of the candidate solutions. For a given candidate solution, the motion of the vehicle is projected to both the control and intersection regions to determine if the safety and kinematic constraints are violated.A candidate arrival time constitutes a viable solution to the optimization problem if the incoming vehicle’s trajectory satisfies specific requirements regarding its safety and motion characteristics. In summary, the safety requirements necessitate that two or more vehicles do not overlap (collide) while conveying in the control and intersection regions. The motion characteristic conditions limit the trajectory of the vehicles to certain acceptable velocity and acceleration profiles when the vehicle is transporting in the CR.This section presents in detail the process that is used to define the infeasibility measure, of the self-adaptive penalty method, by quantifying the constraint violations of the candidate solutions. This process incorporates three intermediate procedures: the Trajectory Generation, the Motion Projection, and the Quantification of Constraint Violations. The subsequent steps are described for an arbitrary vehicleAi that enters the IR at a time instant t¯i and has a candidate arrival time t¯ia.


### 3.4.1. Trajectory Generation

The first step towards the calculation of each solution’s (vehicle’s candidate arrival time) infeasibility measure is to generate the incoming vehicle’s trajectory in the CR based on its candidate arrival time. Thus, each incoming vehicle’s velocity profile, in the CR, is parameterized by its arrival time.The trajectory of each vehicle in the CR is generated based on two kinematic conditions. The first requires that the vehicles shall not exceed their maximum velocity and acceleration bounds,vmax and amax, respectively. The second necessitates that the vehicles enter and exit the CR with their maximum velocity, vmax. To minimize the number of configuration parameters of the trajectories, we limit the vehicle’s velocity profile to have a trapezoidal form consisting of three distinct periods: constant deceleration, advancement with constant speed (zero acceleration), and constant acceleration (the deceleration period of the vehicle’s trapezoidal velocity profile, within the CR, must proceed the acceleration period such that the vehicle’s speed, at any instant, does not exceed its maximum admissible value, vmax). Furthermore, all three periods have equal duration, and the absolute values of the deceleration and acceleration are also equal. Hence, the acceleration profile is comprised of two opposite pulses of same area.Figure8 illustrates the velocity and acceleration profiles of Ai and the sequence of the distinct acceleration periods. The expected time duration where Ai will be conveying in the CR is defined as follows:(22)tga=t¯ia−t¯i.Figure 8
Velocity and acceleration profiles of vehicleAi. The motion of the vehicles is broken down to three distinct periods: constant deceleration, advancement with constant speed, and constant acceleration.As mentioned earlier, the projected time duration ofAi in the CR, tga, is further subdivided into three equal time intervals. Each interval corresponds to one of the vehicle’s trajectory acceleration periods, and their respective duration is denoted by tqa=tga/3. With the trapezoidal formulation of the velocity profile, the only parameter that needs to be determined is the vehicle’s constant speed vla during its zero acceleration phase (advancement with constant speed). This value is conveniently calculated by noting that the area under the velocity curve in the interval t¯it¯ia is equal to the displacement of Ai during that time interval, i.e., the length of the CR, L. Using elementary calculations, the velocity of the vehicle at its zero acceleration period is given by(23)vla=L−vmax⋅tqa2⋅tqa.The constant acceleration that is needed to bring the velocity ofAi from vla to vmax in a linear manner, during the acceleration period, is calculated by the following equation:(24)v˙a=vmax−vlatqa.The above value also represents the slope of the velocity’s straight line segment at the acceleration phase. The vehicle’s constant deceleration, at the first phase of its trajectory, is equal to−v˙a. Finally, the velocity of Ai in the CR, for all three periods of the trajectory’s profile, is expressed as follows:(25)vit,t¯i,t¯ia=vmax−v˙a⋅t−t¯i,t¯i≤t≤t¯i+tqa;vla,t¯i+tqa<t≤t¯i+2⋅tqa;vla+v˙a⋅t−t¯i+2⋅tqa,t¯i+2⋅tqa<t≤t¯i+3⋅tqa;where t∈t¯i,t¯ia.


### 3.4.2. Motion Projection

The second step involves the projection of the motion of each vehicle, in the control and intersection regions, according to its generated velocity profile. It should be reminded that the vehicles transport through the IR with constant velocity. The projected motion, which corresponds to a candidate arrival time of the vehicle, is utilized to evaluate the part of the feasibility measure that pertains to its safety requirements.To begin with, we calculate the total duration that each vehicle spends at the autonomous intersection. This duration is the sum of the time spans that the vehicle conveys in the control and intersection regions, respectively. Clearly, the vehicle’s total duration at the intersection depends on its candidate arrival time. The total duration at the intersectionπia, of vehicle Ai and corresponds to the candidate arrival time t¯ia, is calculated by the following equation:(26)πia=t¯ia−t¯i+τi.Using the kinematic expressions given in (2) and the generated velocity profile of (25), this procedure projects the motion of the vehicle in the time horizon πia. Figure 9 illustrates how the Motion Projection procedure maps a vehicle’s motion in the control and intersection regions. After determining the total duration πia, a structure with all the discrete execution instances in the interval t¯i,t¯i+πia is formed; let Tia be the set of all these instances. The Motion Projection process finalizes by determining the set of postures, denoted by χia, that Ai will have at all instances of Tia.Figure 9
Illustration of the projected motion of vehicleAi during its movement, with duration πia, across the control and intersection regions, respectively, for the time instances of the set Tia.


### 3.4.3. Quantification of Constraint Violations

The final step of the constraint handling process is dedicated to quantifying the level of violation of each constraint by a candidate solution. The objective is to capture the safety conflicts, in both the control and intersection regions, and also record violations of the vehicles’ kinematic limitations. The identification of safety conflicts in the IR is performed similarly to the reservation paradigm that was introduced in [7].The first constraint captures conflicts between vehicles in the IR that have not been identified at the preliminary search, e.g., the case where a vehicle overlays multiple cells. To capture these conflicts, initially, the resolution of the grid array is refined by a factor ofμ∈Z+. The refined grid array has cr=μ⋅NR/2⋅NL columns, rr=μ⋅NR/2⋅NL rows, and the cells’ length is er=e/μ. Each cell is identified by its row and column indexes as EK,L, where K∈ℛr=1,…,rr and L∈Cr=1,…,cr. A timetable entry BK,Lrt is assigned to the refined cell EK,L. If the latter is occupied at the execution instant t, then we set BK,Lrt=1; otherwise, BK,Lr=0. Before mapping the projected motion of Ai to the refined cells of the intersection, a space-buffer is applied to add a safety distance between the vehicles in the IR. The dimensions of the vehicle are artificially expanded by a factor κ>1 such that its new dimensions become l′=l⋅κ and w′=w⋅κ. The refined grid array of the IR is shown in Figure 10.Figure 10
Illustration of the refined grid array at the IR. Inside the IR, the dimensions of the vehicles are artificially expanded as a means to introduce a space-buffer for their collision-free motion. The colored elements represent the occupied cells of the IR.The occupancy mapping process, forAi, entails the determination of the set of occupied cells Pit in the refined IR that are encapsulated by the expanded dimensions of Ai at time t. Let ℐit=I1,I2,…,IPi be the set of indices that correspond to the elements (cells) of Pit. The number of violations that the candidate solution t¯ia attains, when Ai moves in the IR, is calculated by the following equation:(27)η1t¯ia,t¯i=∑m∈τn∑n=1ℐimBInrm,where τn denotes the collection of all discrete execution instances that belong to the closed interval t¯ia,t¯ia+τi. In the CR, the safety conflict between two same-lane vehicles is detected by inspecting the distance between them. The number of violations of Ai in the CR is given by(28)η2t¯ia,t¯i=∑m∈τcbcm,where τc denotes the set of execution instances in the closed interval t¯i,t¯ia, which corresponds to the duration that Ai conveys in the CR, and bc is a binary decision variable that signals whether or not there is a safety conflict between Ai and another vehicle in the lane. The decision variable bc is defined as follows:(29)bcm=0,ifl′−Xiam−Xpm≤0;1,otherwise.The termXp represents the coordinates of the same lane, succeeding vehicles to Ai that convey in the CR at the time instant m, and l′ (see definition of η1) is the artificially expanded vehicles’ length that adds a safe distance between vehicles in the CR. The remaining violations are assessed based on the kinematic constraints of the vehicles. Firstly, the (binary) acceleration violation value of the trapezoidal velocity’s profile is given by(30)η3t¯ia,t¯i=0,ifv˙a∈−amax,amax;1,otherwise.In a similar fashion, the (binary) velocity violation value of the vehicle’s trajectory is expressed as follows:(31)η4t¯ia,t¯i=0,ifvla∈0,vmax;1,otherwise.The overarching objective of the constraint handling method is to quantify all the constraint violations for each candidate solution. Therefore, the net value of the constraint violations of a candidate solutiont¯ia is expressed as the weighted mean of all four constraint violation functions, given by(32)αt¯ia,t¯i=14∑m=14ηmt¯ia,t¯iηmmax,where the term ηmmax is the maximum possible number of violations of the function ηm. The values of η1max of η2max are τn⋅ℐim and τc, respectively. These two functions obtain their maximum value when the vehicle constantly overlaps with another one, when moving in the control and intersection regions, respectively. For the kinematic constraints, we have η3max=η4max=1. A synoptic illustration in block diagrammatic form of the search process for the determination of the arrival time t¯i of vehicle Ai, based on its entry time t¯i, is shown in Figure 11.Figure 11
Block diagram of the procedures, and their respective interconnections, associated with the determination of the arrival timet¯i for vehicle Ai based on the entry time t¯i.


## 3.4.1. Trajectory Generation

The first step towards the calculation of each solution’s (vehicle’s candidate arrival time) infeasibility measure is to generate the incoming vehicle’s trajectory in the CR based on its candidate arrival time. Thus, each incoming vehicle’s velocity profile, in the CR, is parameterized by its arrival time.The trajectory of each vehicle in the CR is generated based on two kinematic conditions. The first requires that the vehicles shall not exceed their maximum velocity and acceleration bounds,vmax and amax, respectively. The second necessitates that the vehicles enter and exit the CR with their maximum velocity, vmax. To minimize the number of configuration parameters of the trajectories, we limit the vehicle’s velocity profile to have a trapezoidal form consisting of three distinct periods: constant deceleration, advancement with constant speed (zero acceleration), and constant acceleration (the deceleration period of the vehicle’s trapezoidal velocity profile, within the CR, must proceed the acceleration period such that the vehicle’s speed, at any instant, does not exceed its maximum admissible value, vmax). Furthermore, all three periods have equal duration, and the absolute values of the deceleration and acceleration are also equal. Hence, the acceleration profile is comprised of two opposite pulses of same area.Figure8 illustrates the velocity and acceleration profiles of Ai and the sequence of the distinct acceleration periods. The expected time duration where Ai will be conveying in the CR is defined as follows:(22)tga=t¯ia−t¯i.Figure 8
Velocity and acceleration profiles of vehicleAi. The motion of the vehicles is broken down to three distinct periods: constant deceleration, advancement with constant speed, and constant acceleration.As mentioned earlier, the projected time duration ofAi in the CR, tga, is further subdivided into three equal time intervals. Each interval corresponds to one of the vehicle’s trajectory acceleration periods, and their respective duration is denoted by tqa=tga/3. With the trapezoidal formulation of the velocity profile, the only parameter that needs to be determined is the vehicle’s constant speed vla during its zero acceleration phase (advancement with constant speed). This value is conveniently calculated by noting that the area under the velocity curve in the interval t¯it¯ia is equal to the displacement of Ai during that time interval, i.e., the length of the CR, L. Using elementary calculations, the velocity of the vehicle at its zero acceleration period is given by(23)vla=L−vmax⋅tqa2⋅tqa.The constant acceleration that is needed to bring the velocity ofAi from vla to vmax in a linear manner, during the acceleration period, is calculated by the following equation:(24)v˙a=vmax−vlatqa.The above value also represents the slope of the velocity’s straight line segment at the acceleration phase. The vehicle’s constant deceleration, at the first phase of its trajectory, is equal to−v˙a. Finally, the velocity of Ai in the CR, for all three periods of the trajectory’s profile, is expressed as follows:(25)vit,t¯i,t¯ia=vmax−v˙a⋅t−t¯i,t¯i≤t≤t¯i+tqa;vla,t¯i+tqa<t≤t¯i+2⋅tqa;vla+v˙a⋅t−t¯i+2⋅tqa,t¯i+2⋅tqa<t≤t¯i+3⋅tqa;where t∈t¯i,t¯ia.


## 3.4.2. Motion Projection

The second step involves the projection of the motion of each vehicle, in the control and intersection regions, according to its generated velocity profile. It should be reminded that the vehicles transport through the IR with constant velocity. The projected motion, which corresponds to a candidate arrival time of the vehicle, is utilized to evaluate the part of the feasibility measure that pertains to its safety requirements.To begin with, we calculate the total duration that each vehicle spends at the autonomous intersection. This duration is the sum of the time spans that the vehicle conveys in the control and intersection regions, respectively. Clearly, the vehicle’s total duration at the intersection depends on its candidate arrival time. The total duration at the intersectionπia, of vehicle Ai and corresponds to the candidate arrival time t¯ia, is calculated by the following equation:(26)πia=t¯ia−t¯i+τi.Using the kinematic expressions given in (2) and the generated velocity profile of (25), this procedure projects the motion of the vehicle in the time horizon πia. Figure 9 illustrates how the Motion Projection procedure maps a vehicle’s motion in the control and intersection regions. After determining the total duration πia, a structure with all the discrete execution instances in the interval t¯i,t¯i+πia is formed; let Tia be the set of all these instances. The Motion Projection process finalizes by determining the set of postures, denoted by χia, that Ai will have at all instances of Tia.Figure 9
Illustration of the projected motion of vehicleAi during its movement, with duration πia, across the control and intersection regions, respectively, for the time instances of the set Tia.


## 3.4.3. Quantification of Constraint Violations

The final step of the constraint handling process is dedicated to quantifying the level of violation of each constraint by a candidate solution. The objective is to capture the safety conflicts, in both the control and intersection regions, and also record violations of the vehicles’ kinematic limitations. The identification of safety conflicts in the IR is performed similarly to the reservation paradigm that was introduced in [7].The first constraint captures conflicts between vehicles in the IR that have not been identified at the preliminary search, e.g., the case where a vehicle overlays multiple cells. To capture these conflicts, initially, the resolution of the grid array is refined by a factor ofμ∈Z+. The refined grid array has cr=μ⋅NR/2⋅NL columns, rr=μ⋅NR/2⋅NL rows, and the cells’ length is er=e/μ. Each cell is identified by its row and column indexes as EK,L, where K∈ℛr=1,…,rr and L∈Cr=1,…,cr. A timetable entry BK,Lrt is assigned to the refined cell EK,L. If the latter is occupied at the execution instant t, then we set BK,Lrt=1; otherwise, BK,Lr=0. Before mapping the projected motion of Ai to the refined cells of the intersection, a space-buffer is applied to add a safety distance between the vehicles in the IR. The dimensions of the vehicle are artificially expanded by a factor κ>1 such that its new dimensions become l′=l⋅κ and w′=w⋅κ. The refined grid array of the IR is shown in Figure 10.Figure 10
Illustration of the refined grid array at the IR. Inside the IR, the dimensions of the vehicles are artificially expanded as a means to introduce a space-buffer for their collision-free motion. The colored elements represent the occupied cells of the IR.The occupancy mapping process, forAi, entails the determination of the set of occupied cells Pit in the refined IR that are encapsulated by the expanded dimensions of Ai at time t. Let ℐit=I1,I2,…,IPi be the set of indices that correspond to the elements (cells) of Pit. The number of violations that the candidate solution t¯ia attains, when Ai moves in the IR, is calculated by the following equation:(27)η1t¯ia,t¯i=∑m∈τn∑n=1ℐimBInrm,where τn denotes the collection of all discrete execution instances that belong to the closed interval t¯ia,t¯ia+τi. In the CR, the safety conflict between two same-lane vehicles is detected by inspecting the distance between them. The number of violations of Ai in the CR is given by(28)η2t¯ia,t¯i=∑m∈τcbcm,where τc denotes the set of execution instances in the closed interval t¯i,t¯ia, which corresponds to the duration that Ai conveys in the CR, and bc is a binary decision variable that signals whether or not there is a safety conflict between Ai and another vehicle in the lane. The decision variable bc is defined as follows:(29)bcm=0,ifl′−Xiam−Xpm≤0;1,otherwise.The termXp represents the coordinates of the same lane, succeeding vehicles to Ai that convey in the CR at the time instant m, and l′ (see definition of η1) is the artificially expanded vehicles’ length that adds a safe distance between vehicles in the CR. The remaining violations are assessed based on the kinematic constraints of the vehicles. Firstly, the (binary) acceleration violation value of the trapezoidal velocity’s profile is given by(30)η3t¯ia,t¯i=0,ifv˙a∈−amax,amax;1,otherwise.In a similar fashion, the (binary) velocity violation value of the vehicle’s trajectory is expressed as follows:(31)η4t¯ia,t¯i=0,ifvla∈0,vmax;1,otherwise.The overarching objective of the constraint handling method is to quantify all the constraint violations for each candidate solution. Therefore, the net value of the constraint violations of a candidate solutiont¯ia is expressed as the weighted mean of all four constraint violation functions, given by(32)αt¯ia,t¯i=14∑m=14ηmt¯ia,t¯iηmmax,where the term ηmmax is the maximum possible number of violations of the function ηm. The values of η1max of η2max are τn⋅ℐim and τc, respectively. These two functions obtain their maximum value when the vehicle constantly overlaps with another one, when moving in the control and intersection regions, respectively. For the kinematic constraints, we have η3max=η4max=1. A synoptic illustration in block diagrammatic form of the search process for the determination of the arrival time t¯i of vehicle Ai, based on its entry time t¯i, is shown in Figure 11.Figure 11
Block diagram of the procedures, and their respective interconnections, associated with the determination of the arrival timet¯i for vehicle Ai based on the entry time t¯i.


## 4. Computational Complexity of the IC

This section provides a detailed account of the computational complexity (or worst-case running time (we use the term computational complexity interchangeably with worst-case running time or time complexity. Formally, these terms are not equivalent [63]. The computational complexity of an algorithm refers to all resources required for executing it, which include both the size of memory (space complexity) and the time (time complexity) needed for running the algorithm. When the available resources are not specified, because the contrary would yield the analysis too complicated and not particularly insightful, the asymptotic running time of the algorithm is used to express its computational complexity)) of the IC with respect to the configuration parameters of the intersection, the geometry and motion characteristics of the vehicles, and the execution period of the algorithm (or else, its search resolution). More precisely, we delimit the time complexity needed by the IC to calculate the arrival time to the IR, t¯i, of an arbitrary vehicle Ai. This process is partitioned into two parts: the preliminary feasibility search, described in Section 3.1, and the search for an optimal arrival time, described in Sections 3.2–3.4. For convenience, let C1 be the computational complexity of the preliminary feasibility search and C2 the complexity of the optimization search. The total complexity, C, of the procedure for calculating a vehicle’s arrival time is C=C1+C2. In the subsequent analysis, we use standard properties of the big O notation and detail only the eventual complexity of the individual procedures involved in the calculation of the optimal arrival time. We intentionally omit the intermediate steps and the overjustification for the sake of brevity. The properties of the big O notation can be found in standard textbooks on the theory of computation and discrete mathematics such as [64] and [65].


### 4.1. Complexity of the Preliminary Feasibility Search

The preliminary search for feasible solutions first identifies the fixed domaint¯et¯l of candidate arrival times to the IR; its duration is calculated by subtracting the value of the earliest arrival time, t¯e, given in (11), from the latest arrival time, t¯l, given in (9). Subsequently, for every discrete execution instant within this interval, we perform the feasibility test, expressed in (12), that singles out feasible arrival times based on the timetable of the intersection. The terms of the double summation in (12) are equal to the number of execution instances at which Ai transports within the IR. We have already established that the duration of the longest path in the IR is δmax, described in (10). The longest possible path is traveled by the vehicles that transport at the outermost right lane of the CR and make a left turn at the IR. To this end, the maximum possible number of execution instances in the IR is δmax/Ts, where Ts is the sampling interval of the IC. Hence, the complexity of the preliminary feasibility search is given by(33)C1=Ot¯l−t¯eTs⋅δmaxTs=Oe+lTsvmax2LlNRNL3.From the above expression, we observe that the complexity of the preliminary feasibility search is cubic with respect to the productNR⋅NL (total number of lanes), which in part encapsulates the layout of the intersection. The complexity of the procedure is linear with respect to the floor term ⌈L/l⌉. The floor term ⌈L/l⌉ captures the impact of the control region’s capacity to the complexity of the procedure. The higher the ratio L/l is, the more vehicles may simultaneously utilize the intersection, and the larger the search-space, t¯et¯l, becomes. Finally, the term e+l/Tsvmax expresses the impact to the computational complexity of the relation between the vehicles motion characteristics (maximum velocity vmax), the size of the intersection (width of lanes w), and the size of the vehicles (length l). Slow moving vehicles that transport over a sizable intersection result in more discrete execution instances that need to be processed for determining their feasibility as candidate solutions.


### 4.2. Complexity of the Optimization Process

The preliminary feasibility search procedure is followed by the execution of the PSO that forages the search-space seeking an optimal solution to the motion coordination problem. The complexity,C2, of the PSO is Otpmax⋅Np⋅Co, where C0 is the complexity required to compute the fitness value of the unconstrained objective function, ϕ⋅. The calculation of the objective function’s fitness value, for a candidate solution, is outlined in Algorithm 2. The reader is reminded that tpmax is the number of iterations and Np is the number of particles in the swarm. The product term tpmax⋅Np in C2 appears because of the iterative double for-loop responsible for the motion of the particles (see the pseudocode of the PSO algorithm given in Algorithm 1). We note that if a solution is not discovered throughout the prespecified number of iterations, tpmax is incremented until the swarm finds one that satisfies the constraints of the problem.Algorithm 2: Pseudocode of the unconstrained objective function procedure. The procedure computes the fitness value of the objective function that corresponds to a candidate solution of the unconstrained optimization problem.
Procedure: unconstrained objective function
Inputs: t¯i,t¯ia,L,vmax,Oi,B,Br,ηmmax,rf,f¯,f¯
Outputs: ϕt¯ia,t¯i
(1)
v
a
,
v
˙
a
=
Trajectory Generation
t
¯
i
,
t
¯
i
a
,
L
,
v
max
(2)
χ
i
a
=
Motion  Projection
v
a
,
t
¯
i
,
t
¯
i
a
(3)
η
1
,
η
2
,
η
3
,
η
4
=
Analysis  of Constraint Violations
B
,
B
r
,
x
i
a
,
y
i
a
,
v
a
,
v
˙
a
(4)
α
t
¯
i
a
,
t
¯
i
⟵
1
/
4
∑
m
=
1
4
η
m
t
¯
i
a
,
t
¯
i
/
η
m
max
(5)
f
t
¯
i
a
,
t
¯
i
⟵
t
¯
i
a
−
t
¯
i
−
t
s
(6)
f
^
t
¯
i
a
,
t
¯
i
⟵
f
t
¯
i
a
,
t
¯
i
−
f
¯
/
f
¯
−
f
¯
(7)
if rf==0 then
(8)
ρt¯ia,t¯i⟵αt¯ia,t¯i
(9)
γt¯ia,t¯i⟵0
(10)
else
(11)
ρt¯ia,t¯i⟵f^t¯ia,t¯i+αt¯ia,t¯i
(12)
γt¯ia,t¯i⟵αt¯ia,t¯i
(13)
end if
(14)
if αt¯ia,t¯i>0 then
(15)
ζt¯ia,t¯i⟵f^t¯ia,t¯i
(16)
else
(17)
ζt¯ia,t¯i⟵0
(18)
end if
(19)
β
t
¯
i
a
,
t
¯
i
⟵
1
−
r
f
⋅
γ
t
¯
i
a
,
t
¯
i
+
r
f
⋅
ζ
t
¯
i
a
,
t
¯
i
(20)
ϕ
t
¯
i
a
,
t
¯
i
⟵
ρ
t
¯
i
a
,
t
¯
i
+
β
t
¯
i
a
,
t
¯
iThe time complexity,C0—associated with the calculation of the unconstrained objective function’s fitness value, ϕt¯ia,t¯i, for a candidate solution t¯ia—grows with the maximal duration of the time horizon for which the motion of the vehicle is projected at the intersection. The maximum span of this time horizon is τh=t¯il−t¯i+δmax, which is the sum of the longest possible duration needed for the vehicles to exit the CR plus the duration it takes them to convey the longest path in the IR. Within this time interval, the procedure that calculates the fitness value has to process τh/Ts discrete execution time instances.The calculation of the objective function’s fitness value is partitioned to three consecutive executable procedures (seeSection 3.4). The Trajectory Generation is a static map; hence, its procedure does not contribute to the asymptotic time complexity of the algorithm. The Motion Projection calculates the posture of Ai for all execution instances in the time horizon τh; thus, its complexity is given by(34)OτhTs=Oe+lTsvmaxLlNRNL2.The final procedure, for determining the fitness valueϕt¯ia,t¯i, is the Quantification of Constraint Violations, which is responsible for calculating the infeasibility measure, αt¯ia,t¯i, of a candidate solution t¯ia. The complexity of this step is dictated by the asymptotic upper bounds on the computations that take place by the quantifying constraint violation functions ηj, where j∈1,2,3,4 (see Section 3.4.3).The constraint violation functionη1 processes all the execution instances for which the vehicle conveys in the IR. We have already shown that the number of these instances is upper bounded by δmax/Ts. For each of these instances, the violation function η1 goes over all points of the refined intersection array that are enclosed within the expanded dimensions of the vehicle (length l′ and width w′). It is reminded that the length of each square “pixel” of the refined grid is e/μ; hence, the complexity for calculating η1 is given by(35)OδmaxTs⋅κwe/μ⋅κle/μ=Oe+lTsvmaxNRNLμ2κ2wle2.The second constraint violation function,η2, takes place for all execution instances for which the vehicle moves within the CR. The maximum duration of this interval, for a vehicle Ai, is t¯il−t¯i. By virtue of (9), which expresses the value of the latest arrival time to the IR, the complexity of this step is given by(36)Ot¯il−t¯iTs=Oe+lTsvmaxLlNRNL2.Finally, the violations of the kinematic constraints (velocity and acceleration) are calculated from the binary functionsη3 and η4, respectively. Because these functions are static—that is, their computation time does not vary—they have constant time complexity, O1, and do not add asymptotic time complexity of the procedure.Combining the complexities of the three intermediate procedures (we have used the rule-of-sums property for bigO; see [65]: if g1∈Of1 and g2∈Of2, then g1+g2∈Omaxf1,f2) (namely, Trajectory Generation, Motion Projection, and Quantification of Constraint Violations), the complexity C0 associated with the calculation of a single fitness value of the objective function is given by(37)C0=Oe+lTsvmaxNRNL2Ll+wle2μκ2.The above expression validates the anticipated expectation that the computational complexity increases—in a quadratic fashion—with the granularity of the collision search in the IR, encapsulated by the productμ⋅κ. To calculate the complexity of the entire optimization step, C0 is multiplied by tpmax⋅Np, rendering the complexity of the optimization step to (we have used the transitivity property for big O; see [64, 65]: if h∈Og and g∈Of, then h∈Of):(38)C2=OtpmaxNpe+lTsvmaxNRNL2Ll+wle2μκ2.In practice, due to the producttpmax⋅Np, the optimization process is significantly more computationally demanding compared to the preliminary feasibility search, despite that the complexity of the latter is cubic with respect to NR⋅NL. The quadratic term of the ratio e+l/Tsvmax that appears in C1 does not have significant impact when the distance covered by the vehicles in a single step (distance Ts⋅vmax) is comparable to their dimensions (length l) and the size of the intersection’s cells (width e).


### 4.3. Total Complexity: Single Vehicle

The time complexity,C, of the comprehensive procedure that computes the arrival time of an incoming vehicle to the IR is given by(39)C=OtpmaxNpe+lTsvmax2NRNL3Ll+wle2μκ2.The above asymptotic upper bound incorporates all the dominant configuration parameters that pertain to the intersection control problem, where a change to their value will impact the running time of the algorithm. The complexity of the algorithm depends on the physical dimensions and configuration of the intersection, encapsulated by the parametersNR, NL, and e; the geometry and motion characteristics of the vehicles, captured by the parameters l, w, and vmax; and parameters of the algorithm, namely, tpmax, Np, μ, and κ.


## 4.1. Complexity of the Preliminary Feasibility Search

The preliminary search for feasible solutions first identifies the fixed domaint¯et¯l of candidate arrival times to the IR; its duration is calculated by subtracting the value of the earliest arrival time, t¯e, given in (11), from the latest arrival time, t¯l, given in (9). Subsequently, for every discrete execution instant within this interval, we perform the feasibility test, expressed in (12), that singles out feasible arrival times based on the timetable of the intersection. The terms of the double summation in (12) are equal to the number of execution instances at which Ai transports within the IR. We have already established that the duration of the longest path in the IR is δmax, described in (10). The longest possible path is traveled by the vehicles that transport at the outermost right lane of the CR and make a left turn at the IR. To this end, the maximum possible number of execution instances in the IR is δmax/Ts, where Ts is the sampling interval of the IC. Hence, the complexity of the preliminary feasibility search is given by(33)C1=Ot¯l−t¯eTs⋅δmaxTs=Oe+lTsvmax2LlNRNL3.From the above expression, we observe that the complexity of the preliminary feasibility search is cubic with respect to the productNR⋅NL (total number of lanes), which in part encapsulates the layout of the intersection. The complexity of the procedure is linear with respect to the floor term ⌈L/l⌉. The floor term ⌈L/l⌉ captures the impact of the control region’s capacity to the complexity of the procedure. The higher the ratio L/l is, the more vehicles may simultaneously utilize the intersection, and the larger the search-space, t¯et¯l, becomes. Finally, the term e+l/Tsvmax expresses the impact to the computational complexity of the relation between the vehicles motion characteristics (maximum velocity vmax), the size of the intersection (width of lanes w), and the size of the vehicles (length l). Slow moving vehicles that transport over a sizable intersection result in more discrete execution instances that need to be processed for determining their feasibility as candidate solutions.


## 4.2. Complexity of the Optimization Process

The preliminary feasibility search procedure is followed by the execution of the PSO that forages the search-space seeking an optimal solution to the motion coordination problem. The complexity,C2, of the PSO is Otpmax⋅Np⋅Co, where C0 is the complexity required to compute the fitness value of the unconstrained objective function, ϕ⋅. The calculation of the objective function’s fitness value, for a candidate solution, is outlined in Algorithm 2. The reader is reminded that tpmax is the number of iterations and Np is the number of particles in the swarm. The product term tpmax⋅Np in C2 appears because of the iterative double for-loop responsible for the motion of the particles (see the pseudocode of the PSO algorithm given in Algorithm 1). We note that if a solution is not discovered throughout the prespecified number of iterations, tpmax is incremented until the swarm finds one that satisfies the constraints of the problem.Algorithm 2: Pseudocode of the unconstrained objective function procedure. The procedure computes the fitness value of the objective function that corresponds to a candidate solution of the unconstrained optimization problem.
Procedure: unconstrained objective function
Inputs: t¯i,t¯ia,L,vmax,Oi,B,Br,ηmmax,rf,f¯,f¯
Outputs: ϕt¯ia,t¯i
(1)
v
a
,
v
˙
a
=
Trajectory Generation
t
¯
i
,
t
¯
i
a
,
L
,
v
max
(2)
χ
i
a
=
Motion  Projection
v
a
,
t
¯
i
,
t
¯
i
a
(3)
η
1
,
η
2
,
η
3
,
η
4
=
Analysis  of Constraint Violations
B
,
B
r
,
x
i
a
,
y
i
a
,
v
a
,
v
˙
a
(4)
α
t
¯
i
a
,
t
¯
i
⟵
1
/
4
∑
m
=
1
4
η
m
t
¯
i
a
,
t
¯
i
/
η
m
max
(5)
f
t
¯
i
a
,
t
¯
i
⟵
t
¯
i
a
−
t
¯
i
−
t
s
(6)
f
^
t
¯
i
a
,
t
¯
i
⟵
f
t
¯
i
a
,
t
¯
i
−
f
¯
/
f
¯
−
f
¯
(7)
if rf==0 then
(8)
ρt¯ia,t¯i⟵αt¯ia,t¯i
(9)
γt¯ia,t¯i⟵0
(10)
else
(11)
ρt¯ia,t¯i⟵f^t¯ia,t¯i+αt¯ia,t¯i
(12)
γt¯ia,t¯i⟵αt¯ia,t¯i
(13)
end if
(14)
if αt¯ia,t¯i>0 then
(15)
ζt¯ia,t¯i⟵f^t¯ia,t¯i
(16)
else
(17)
ζt¯ia,t¯i⟵0
(18)
end if
(19)
β
t
¯
i
a
,
t
¯
i
⟵
1
−
r
f
⋅
γ
t
¯
i
a
,
t
¯
i
+
r
f
⋅
ζ
t
¯
i
a
,
t
¯
i
(20)
ϕ
t
¯
i
a
,
t
¯
i
⟵
ρ
t
¯
i
a
,
t
¯
i
+
β
t
¯
i
a
,
t
¯
iThe time complexity,C0—associated with the calculation of the unconstrained objective function’s fitness value, ϕt¯ia,t¯i, for a candidate solution t¯ia—grows with the maximal duration of the time horizon for which the motion of the vehicle is projected at the intersection. The maximum span of this time horizon is τh=t¯il−t¯i+δmax, which is the sum of the longest possible duration needed for the vehicles to exit the CR plus the duration it takes them to convey the longest path in the IR. Within this time interval, the procedure that calculates the fitness value has to process τh/Ts discrete execution time instances.The calculation of the objective function’s fitness value is partitioned to three consecutive executable procedures (seeSection 3.4). The Trajectory Generation is a static map; hence, its procedure does not contribute to the asymptotic time complexity of the algorithm. The Motion Projection calculates the posture of Ai for all execution instances in the time horizon τh; thus, its complexity is given by(34)OτhTs=Oe+lTsvmaxLlNRNL2.The final procedure, for determining the fitness valueϕt¯ia,t¯i, is the Quantification of Constraint Violations, which is responsible for calculating the infeasibility measure, αt¯ia,t¯i, of a candidate solution t¯ia. The complexity of this step is dictated by the asymptotic upper bounds on the computations that take place by the quantifying constraint violation functions ηj, where j∈1,2,3,4 (see Section 3.4.3).The constraint violation functionη1 processes all the execution instances for which the vehicle conveys in the IR. We have already shown that the number of these instances is upper bounded by δmax/Ts. For each of these instances, the violation function η1 goes over all points of the refined intersection array that are enclosed within the expanded dimensions of the vehicle (length l′ and width w′). It is reminded that the length of each square “pixel” of the refined grid is e/μ; hence, the complexity for calculating η1 is given by(35)OδmaxTs⋅κwe/μ⋅κle/μ=Oe+lTsvmaxNRNLμ2κ2wle2.The second constraint violation function,η2, takes place for all execution instances for which the vehicle moves within the CR. The maximum duration of this interval, for a vehicle Ai, is t¯il−t¯i. By virtue of (9), which expresses the value of the latest arrival time to the IR, the complexity of this step is given by(36)Ot¯il−t¯iTs=Oe+lTsvmaxLlNRNL2.Finally, the violations of the kinematic constraints (velocity and acceleration) are calculated from the binary functionsη3 and η4, respectively. Because these functions are static—that is, their computation time does not vary—they have constant time complexity, O1, and do not add asymptotic time complexity of the procedure.Combining the complexities of the three intermediate procedures (we have used the rule-of-sums property for bigO; see [65]: if g1∈Of1 and g2∈Of2, then g1+g2∈Omaxf1,f2) (namely, Trajectory Generation, Motion Projection, and Quantification of Constraint Violations), the complexity C0 associated with the calculation of a single fitness value of the objective function is given by(37)C0=Oe+lTsvmaxNRNL2Ll+wle2μκ2.The above expression validates the anticipated expectation that the computational complexity increases—in a quadratic fashion—with the granularity of the collision search in the IR, encapsulated by the productμ⋅κ. To calculate the complexity of the entire optimization step, C0 is multiplied by tpmax⋅Np, rendering the complexity of the optimization step to (we have used the transitivity property for big O; see [64, 65]: if h∈Og and g∈Of, then h∈Of):(38)C2=OtpmaxNpe+lTsvmaxNRNL2Ll+wle2μκ2.In practice, due to the producttpmax⋅Np, the optimization process is significantly more computationally demanding compared to the preliminary feasibility search, despite that the complexity of the latter is cubic with respect to NR⋅NL. The quadratic term of the ratio e+l/Tsvmax that appears in C1 does not have significant impact when the distance covered by the vehicles in a single step (distance Ts⋅vmax) is comparable to their dimensions (length l) and the size of the intersection’s cells (width e).


## 4.3. Total Complexity: Single Vehicle

The time complexity,C, of the comprehensive procedure that computes the arrival time of an incoming vehicle to the IR is given by(39)C=OtpmaxNpe+lTsvmax2NRNL3Ll+wle2μκ2.The above asymptotic upper bound incorporates all the dominant configuration parameters that pertain to the intersection control problem, where a change to their value will impact the running time of the algorithm. The complexity of the algorithm depends on the physical dimensions and configuration of the intersection, encapsulated by the parametersNR, NL, and e; the geometry and motion characteristics of the vehicles, captured by the parameters l, w, and vmax; and parameters of the algorithm, namely, tpmax, Np, μ, and κ.


## 5. Simulation Results

In this section, we evaluate the performance and effectiveness of the proposed IC via numerical simulations. The simulations were carried out using the MATLAB computing environment. A comparison of the proposed IC with the existing state of the art, as it appears in the literature (e.g., [7, 26, 66]), was deemed impractical. In the existing literature, the intersection control problem is staged in various dissimilar ways, making the direct comparison between methodologies unfeasible. Some of its variants are limiting the problem to single-lane roads, vehicles that have their motion governed by linear kinematics, or are adopting a vehicle-to-vehicle (decentralized) instead of a vehicle-to-infrastructure (centralized) architecture. Thus, we compare the performance of the proposed IC with a conventional traffic lights system and validate its efficacy for varying intersection configurations and traffic loads.The parameters of the numerical simulations are outlined in Table1. The length, l, and width, w, of the autonomous vehicles are set to 6m and 3m, respectively. The maximum velocity of the vehicles, vmax, was set to 10 m/s, and their accelerate/decelerate bound, to ±2 m/s2. The width of the lanes, e, was selected equal to the vehicles width, w, and the resolution factor for detecting collisions, μ, was set to 10 such that er=0.3m. Finally, the sampling interval of the IC, Ts, was selected as 0.01s.Table 1
Numerical simulation parameters.
Simulation parameters
L
6 m
w
3 m
v
max
10
m
/
s
a
max
2
m
/
s
2
e
0.3 m
μ
10
T
s
0.01 s
c
1
2.05
c
2
2.05The performance of the IC is analyzed for two different intersection configurations. For each configuration, we examine the delay of the vehicles in the CR for varying traffic loads in the roads of the intersection. The configuration of the first intersection involves two roads with a single-lane each—the vehicles can only move straight, and there are no turns. The two single-lane road intersection configuration (NR=2 and NL=1) has a constant service time τi=e+l/vmaxs for every incoming vehicle. The entry times of the incoming vehicles, at each lane of the CR, are randomly generated by a Poisson process. The rate of the Poisson process (or arrival intensity), λ, is gradually increased to simulate different traffic loads at the intersection. Light, medium, and heavy traffic conditions were reproduced by implementing an arrival intensity, λ, of 10, 30, and 50 vehicles per minute vpm, respectively.Figure12 depicts the trajectory of the vehicles while they are moving in the CR. At the low and medium traffic loads, vehicles traverse the CR with almost zero delay. However, the heavy traffic load causes the vehicles to experience a minor delay with an average value of approximately 9.8s. Figure 13 compares the traffic flow of the vehicles at the entrance versus the exit of the CR, for different values of the arrival intensity, λ. The difference in the traffic flow reflects the impedance (delay) of the vehicles in the CR. The two traffic flows, at the entrance and exit of the CR, are nearly equivalent when λ is less and equal to 30vpm. A reduction of roughly 10% in the exit flow, compared to the entry, appears at the maximum traffic load, when the arrival intensity is 50vpm.Figure 12
First case study: trajectories of the vehicles inside the CR with respect to time, for different traffic intensities. The top, middle, and bottom graphic plots represent the low, medium, and heavy traffic loads with arrival intensities 10, 30, and 50vpm, respectively. The upper half of the plot corresponds to vehicles of Road 1, while the lower half to vehicles of Road 2.
(a)
(b)
(c)Figure 13
First case study: average traffic flows at the entry and exit of the CR, respectively, for varying values of the arrival intensity,λ.Figure14 depicts the utilization of the IR by the incoming vehicles. For the two single-lane road intersection configuration (NR=2 and NL=1), the IR is comprised of a single cell. Its utilization is defined as the percentage of time that the IR is occupied by a vehicle over the system’s entire duration of the operation. The overarching goal is not only to minimize the delay of the vehicles but also to schedule the intersection’s resources efficiently for mitigating idle time—that is, attain full utilization (100%) when the traffic is heavy. The results show that the IC controller accomplishes almost full utilization when the arrival intensity of the vehicles is greater and equal to 40vpm.Figure 14
First case study: intersection’s utilization for varying values of the arrival intensity,λ.The second simulation case study accounts an intersection configuration where four roads with two lanes each are intersecting (NR=4 and NL=2), and the vehicles can make a turn in either direction. In this configuration, the number of roads is doubled, compared to the first simulation case study, resulting in a substantial increase in the vehicles traveling time. The arrival intensity, in each lane, is set to 10, 20, and 30 vpm. For this case study, the intensity values are lower compared to the first scenario. However, in this configuration, the number of lanes has been quadrupled compared to the first case; thus, the congestion is considerable, especially when the arrival intensity is 30vpm.The route of each vehicle at the intersection was determined by two consecutive Bernoulli processes. Initially, the vehicle determines if a turn takes place with a probabilitypt=0.3. In the case of a turn, the selection of the exit road is also random. This probability depends on the lane of the vehicle. For a left lane vehicle, the probabilities for turning left and right are pfl=0.7 and pfr=0.3, respectively. For a right lane vehicle, the probabilities pfl and pfr are reversed.The performance of the IC is compared with a conventional traffic lights intersection control system. The traffic lights signaling introduces stop-and-go waves, and it is implemented in three consecutive phases. In the first phase, the green light is on for only the vehicles that are on parallel roads and going to turn left. Then, the vehicles that are going straight are allowed to move, but the left turn is prohibited during this period. The final phase is the amber light, and it provides a safe transition between the red and green lights. The amber light enables vehicles to finalize their motion in the IR before the next nonintersecting roads are mobilized.Figure15 depicts the average delay that the vehicles experience along the CR. The IC allows the vehicles to traverse the CR with an average delay that is less than 10 s, until the traffic load takes its highest value. The delay of the vehicles in the intersection reaches its maximum value, of approximately 14 s, when the vehicle arrival rate is 30vpm per lane. The simulation results show that the proposed IC outperforms the traffic lights control by reducing the average delay by approximately in half. Figure 16 depicts the average traffic flow at the entry and exit of the intersection CR. The incoming and outgoing traffic flows are almost the same (negligible delay), and their difference is at most 16% when λ=30vpm.Figure 15
Second case study: delay at the traffic intersection under the proposed IC and conventional traffic signal control with green cycles (g.c.s) of5s, 10s, and 15s, respectively.Figure 16
Second case study: average traffic flow at the entry and exit of the CR, respectively, for varying values of the arrival intensity,λ.


## 6. Conclusion

We have presented a scheduling-based approach for the formulation of an IC for autonomous vehicles that cross a multilane intersection. The multivehicle motion coordination task at the intersection is cast as an optimization problem with nonlinear constraints, where the optimization variable is the vehicle’s arrival time at the CR. The IC computes the trajectory of each incoming vehicle in a first-come first-served order. The performance metric of the optimization problem is the delay of the vehicle at the CR. The penalty method is used to convert the constrained optimization problem into an unconstrained one, making it compatible with metaheuristics. The fitness value of the unconstrained problem’s objective function captures the severity of the violations that correspond to a candidate solution. The PSO is employed to harvest the search-space for an optimal solution to the optimization problem.The IC relies on model-based heuristic computation that merges effectively combinatorics, optimization, and dynamics equations into a concrete framework that makes analytically derived optimal policies redundant. We provide a detailed account of the computational complexity of the IC as a function of its configuration parameters and the intersection’s layout. It is shown that the IC is cubic to the total number of lanes in the intersection and linear to the number of particles of the PSO. Simulation results demonstrate that the IC outperforms conventional traffic signals control, reducing significantly the average delay that the vehicles experience at the intersection, especially when the traffic load is medium to heavy.Contrary to a conventional optimization task, the intersection control requires the forward projection of the vehicle’s trajectory, which corresponds to a candidate solution. Because the PSO simulates multiple candidate solutions at each iteration, the IC is subjected to considerable computational strain. Future work involves modifications to the existing algorithm that will reduce the computational complexity of the IC, for example, a better selection of the initial candidate solutions that will accelerate the convergence of the PSO to an optimal solution.



---

*Source: 6217409-2020-03-31.xml*