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

(is paper considers the motion coordination problem of autonomous vehicles in an intersection of a traffic network.(e 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 themultivehicle coordination task as an optimization problem, with a onedimensional search-space. A modeland 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.


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. ese augmenting technologies are projected to mitigate existing problems of urban traffic such as congestion, safety, reliability, and sustainability [1]. e 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 realtime processing of the information collected by the traffic system and online decision-making. e emergent integrated networked systems of autonomous, computing, and sensing nodes are brought into play to automate the traffic management of intersections. e 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). e longterm vision is to have autonomous vehicles transporting in accident-free traffic systems. e 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. e 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. e 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. e 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. e 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. e intersection controllers are mainly classified into centralized and decentralized [3,[13][14][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. e 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][18][19]. e optimization methods achieve minimization of performance metrics such as travel time within the intersection [20][21][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. e 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]. ere 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. e 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. e decentralized optimization methods essentially formulate the motion coordination as a multiobjective optimization problem [3]. e 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].
is 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. e 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. e 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.
e 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. is is accomplished by uniquely mapping the arrival time to the entire control 2 Journal of Robotics policy (velocity profile) of the vehicle, making the arrival time the only optimization variable. e 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. e 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. ese candidate solutions become initial values of the optimization procedure and improve its performance by initiating the optimization search with partially feasible trajectories. e main contribution of this paper is the scheduledriven 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. e 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. is paper is organized as follows. Section 2 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. e 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. e computational complexity of the proposed AIM is discussed in Section 4. e performance of the proposed AIM is assessed in Section 5 via numerical simulations. Concluding remarks are given in Section 6.

Model of the Traffic System.
is paper focuses on the motion coordination problem of autonomous vehicles in a traffic intersection. e traffic system is denoted by the set of vehicles: 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 A i to the intersection, respectively; and N R and N L are the index sets of roads and lanes in a road, respectively. e road index set is defined as { } is the total number of parallel and mutually orthogonal intersecting roads, and the lane index set is defined as N L � 1, . . . , N L , where N L is the total number of lanes in every road. For simplicity, it is assumed that all the intersecting roads have equal number of lanes. e 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. e set N j,k (t), with j ∈ N R and k ∈ N L , denotes the collection of vehicles that transport within the CR of road j and lane k at the time instant t. e planar 2D motion of vehicle A i (j, k) is described by the pose column vector c i � [x i ; y i ; ψ i ], where x i and y i denote the Cartesian coordinates of A i 's front bumper with respect to the intersection-fixed frame F I , and ψ i its heading. e heading angle is the relative angular displacement of F I with respect to a vehicle-fixed frame F i that is rigidly attached to A i (j, k). When ψ i � 0, the two frames (F I and F i ) are aligned. e two frames are illustrated in Figure 1. It is assumed that the autonomous vehicles have the kinematics of unicycle, or differential drive, robot. e kinematics of vehicle A i are given by where v i is the linear velocity and ω i is the angular velocity of A i . e vehicles cannot move in reverse; therefore, the Journal of Robotics where v max is the maximum allowed velocity of the traffic system. Furthermore, their acceleration is also bounded by a positive constant a max ; consequently, for vehicle A i , we have | _ v i | ≤ a max . e vehicles are considered as two-dimensional rectangular rigid bodies where their width and length are denoted by w and l, respectively. Let G i be the set of all points in the 2D space that are enclosed by the geometric shape of A i . e occupancy set, G i , of A i at the time instant t depends on its pose, c i , and is defined as follows: Let X i (t) � (x i (t), y i (t)) ∈ R 2 be the position coordinates of the front bumper at time t of the vehicle A i . It is assumed that the vehicle A i enters the CR at time t i with the speed v i � v max . e control objective is to determine the arrival time t i of A i at the IR such that it enters the region with the speed v i � v max . e vehicles cross the IR with a constant linear velocity v max . Incoming vehicles to the IR can move straight or make a turn that is compatible with the right-hand convention of bidirectional traffic. e final direction of vehicle A i is represented by the parameter d i ∈ 0, 1, 2 { }. When d i � 0, the vehicle retains its direction, else it will make a turn (left, d i � 1, or right, d i � 2) to one of the intersecting roads. e crossing lanes in the IR form a square grid array. Each cell of the array is uniquely identified by its row and column indexes. e grid has c � (N R /2) · N L columns and r � (N R /2) · N L rows. Let C � 1, . . . , c { } and R � 1, . . . , r { } denote the column and row sets, respectively. A cell entry at the intersection located at the I ∈ R row and J ∈ C column is denoted by C I,J . e edge of square cells is denoted by e and is equal to the lane's width. e enumeration order of the IR's cells is shown in Figure 2. e decomposition of IR to cells will be used later on to determine the occupancy of the IR by the incoming vehicles.

2.2.
e 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 v max ; thus, the only delay occurs within the CR. e time it takes for A i to travel the CR (of length L) is t d � t i − t i , and A i experiences a delay when t d > t s , where t s � (L/v max ). More precisely, t s is the minimal time duration that takes a vehicle to traverse the CR with its maximum velocity, v max . With the following definition, we manifest the function that needs to be minimized in order to reduce the sojourn time of a vehicle A i at the intersection.
Definition 1. e delay of vehicle A i is a function of the time difference t i − t i and is defined as follows: Obviously, from the above, if there is no delay for A i at the CR, i.e., t d � t s , then f(t i , t i ) � 0. e vehicles are further expected to transport the intersection without colliding. e following definition states the safety requirement for the collision-free motion of the vehicles within the intersection.

Definition 2.
e intersection system is safe at time t, if there are no collisions between any two vehicles A i (j, k) and 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, v max , while their velocity and acceleration in the CR should satisfy certain bounds (|v 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. us, we parameterize the velocity function of A i 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 v i (t). To emphasize the dependency of the v i (t) function in t i and t i , we write the velocity of A i as v i (t, t i , t i ).
Based on the above definitions-pertaining to the autonomous intersection's performance (Definition 1) and the vehicles' safety (Definition 2) and kinematic constraints-we formulate the motion coordination optimization problem for an arbitrary vehicle A i entering the autonomous traffic intersection as follows: with j ∈ N R , k ∈ N L , and i ≠ p; e above optimization formulation yields a constrained minimization search with a linear objective function and nonlinear constraints. e optimization variable is the arrival time t i and the objective function, f(t i , t i ), is cast such (1,1) (1, 2) (1, w) Journal of Robotics 5 that the solution of the optimization search will minimize the delay caused by the deceleration of the vehicles in the CR. e safety constraints incorporate the nonlinear kinematic equations of the vehicles and guarantee their collision-free motion in the intersection.

Proposed Solution: Description of the Intersection
Controller.
e autonomous intersection utilizes a supervisory controller, which we will refer to as the intersection controller (IC), that regulates the motion of the vehicles that enter the intersection. e 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., A i , enters the intersection, the IC records its entry time t i . e motion control problem entails the designation of a velocity function v i (t) that satisfies the safety and kinematic constraints of A i and minimizes its delay at the CR. However, because we parameterize v i (t) 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. is forecast is needed to examine if the safety of the vehicles at the intersection is preserved. e forward projected trajectory of A i 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. e 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 v i (t, 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. ese 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.
us, it does not add significant computational overhead to the final algorithm; instead, it improves its performance. e block diagram of the IC is illustrated in Figure 3.

Preliminary Feasibility Search of Candidate Solutions.
In this section, we show how the search-space of the optimization task is determined. e 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. ese candidate solutions are further refined during the optimization search. e 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, v max .
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.
e 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. Let � (e/v max ). 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 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. e 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. e turning radius e 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. e optimization and design of curvilinear paths is outside the scope of this work; we refer the interested reader to [37,38].
Let D i be the ordered set with elements the indices pairs of the cells that belong to Journal of Robotics e total service time that is required for A i to travel through the IR is given by where 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. e 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. e longest possible path is illustrated in Figure 5 for a generic intersection model. e search-space's upper bound ("latest" arrival time) is calculated by the following equation: 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). e 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 A i 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: Intersection (physical system) IC (cyber system) Preliminary feasibility search erefore, the search-space of A i is defined as A preliminary feasible arrival time signifies that all the requested cells in O i are unoccupied and available to reserve for the duration of the service times (see Figure 6). e occupancy schedule of the intersection is represented by assigning a timetable B I,J to the cell O I,J of the IR, where B I,J (t) � 0 signals that the cell O I,J is available at time t, and B I,J (t) � 1 that is occupied.
Assume that the vehicle A i is set to traverse in the IR over a path represented by the set O i . e actual search-space is comprised of the discrete execution time instances of the algorithm that lie within the time interval S i . Denote by S i � Subsequently, the IC determines the set of entrance-tocells time instances denoted as t i Similarly, the set of exit-from-cell instances, denoted by q i exit � q i D 1 , . . . , q i D |D i | , is also calculated, where q i D j marks the time instant that the vehicle exits entirely the cell O D j . Let T j be the collection of all execution instances e feasibility of a candidate arrival time in S i is determined by the following function: e above function is calculated sequentially for the elements of S i ; therefore, the search-space is revised as S ⋆ i � s ∈ S i | g(s) � 0 such that S ⋆ i ⊆ S i . e set S ⋆ i will be referred to as the arrival times set of A i .

Particle Swarm Optimization.
e 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. e elements of the arrival time set S ⋆ i correspond to the incoming vehicle's A i feasible arrival times to the IR. ese 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. e 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.
e 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. e PSO employs a group of moving particles (swarm) to seek a solution in the search-space. e motion of the particles within the search-space is determined by their position and velocity, while a temporary memory buffer stores the fittest

The intersection timetable
Search space Figure 6: Graphical illustration of the intersection's reservation timetable. e timetable is a buffer that stores for each cell of the IR its occupancy information with respect to time. For an arbitrary vehicle A i , the preliminary feasibility search identifies the elements of the set S i that correspond to an unreserved trajectory at the IR using the timetable. 8 Journal of Robotics (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. erefore, 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. e optimization problem, which the PSO is assigned to solve, involves the unconstrained minimization (or maximization) of an objective function ϕ(X), where ϕ: R n ⟶ R, X is a location in the search-space, and n is its dimension. e search-space is also called the domain of the optimization problem. e swarm of the PSO algorithm consists of N p particles, where each particle serves as a candidate solution of the optimization problem. Let X a and V a , where a ∈ P � 1, . . . , N p , be the position and velocity vectors in R n of the a th 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 X a is ϕ(X a ).
e particles move around in the search-space, according to a simple set of rules that determine each particle's velocity, to find a solution X ⋆ 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]. e initial positions of the particles are randomly assigned in the n-dimensional search-space of the optimization problem. e particle's direction of motion in the search-space is guided by two elastic forces [52]. e first force attracts the particle, with a random magnitude, towards the best (fittest) position that has been explored by the swarm, denoted by P g . e second pulls the particle, with a random magnitude, to the fittest position that has been discovered by itself, denoted by P a . Figure 7 depicts the two forces that determine the trajectory of a particle in the search-space. e velocity of the a th particle in the swarm is calculated by the following equation: where t p 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 c 1 and c 2 are the constant acceleration coefficients (the positive constants c 1 and c 2 are also referred to as the cognitive and social parameters of the swarm, respectively [42]). e acceleration coefficients encapsulate the particle's confidence to its own success and the swarm's success, and they are typically selected as c 1 � c 2 � 2 [42]. e search process is terminated when a prespecified number of iterations, t max p , has been reached. e final solution of the optimization problem is the swarm's best discovered value, P g . e term w n 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, t p , 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: where w max and w min are the prespecified maximum and minimum values of the inertia weight, respectively. e maximum inertia weight, w max , is generally selected as 1 while the minimum number, w min , is set to 0.1 (close to zero) [42].
After calculating the velocity vector V a of particle a, its position is updated based on the following expression: w n V a : inertia's influence c 2 σ2(P g − X a ): swarm's influence c 1 σ 1 (P a − X a ): particle's memory influence X a (t) Figure 7: Graphical illustration of the a th particle's subcomponents that form its updated velocity vector, V a (t + 1), at the execution instant t of the PSO. e updated velocity is a linear combination of its current velocity, V a (t), and its relative displacements from its own and the population's fittest position values, P a (t) and P g (t), respectively.
X a t p + 1 � X a t p + V a t p + 1 .

(15)
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.
us, all particles are attracted by P g (the population's current fittest position value), while keeping their own information (through P a ). e individual's and population's best visited positions P a and P g , respectively, are updated according to the fitness values of the particles. For example, in the case of a minimization problem, if ϕ(X a (t p + 1)) < P g , then the a th particle's position becomes the population's best discovered value, and P g takes the value of X a (t p + 1). e individuals' and population's best values P a and P g , respectively, drive iteratively the particles of the swarm towards a satisfactory (suboptimal) solution of the optimization problem. e pseudocode of the PSO procedure is outlined in Algorithm 1.
Returning to the intersection control problem, the PSO search is executed for every incoming vehicle. e initial candidate solutions, N p in the number, for the vehicle A i are selected randomly from the set S ⋆ i . us, the position of the a th particle, X a , represents a candidate arrival time, t a i , of A i . As a reminder, we note that-by design-the selection of the Recall that the PSO is suitable for only unconstrained optimization problems. erefore, we need to convert the original objective function-that is, the delay, f(t 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.

Constraint Handling Method.
e intersection control problem entails the selection of each vehicle's candidate arrival time at the IR that minimizes the delay function, described in Definition 1, 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. ereby, 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. e value of the penalty function is a measure of the candidate solution's constraint violations severity. e 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. e penalty function also is a means of making use of the infeasible solutions instead of discarding them (also known as the death penalty method). e disadvantage of entirely discarding the infeasible solutions is that the information that they may contain will be lost. e 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]. e 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]. is formulation utilizes the information and experience that is gained through the search process to regulate the penalty amount added to the infeasible solutions. e 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 a i , t i ), of an incoming vehicle A i is parameterized by the candidate solution that corresponds to particle a ∈ P (arrival time t a i ) of the swarm and the vehicle's entrance time to the CR, t i . e self-adaptive penalty method replaces the objective function of the constrained optimization problem with a 10 Journal of Robotics metric called distance value, ρ(t a i , 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. e distance value is blended with a penalty term, β(t a i , t i ), to form the unconstrained objective function, ϕ(t a i , t i ), and the candidate solutions are ranked based on their fitness values. erefore, the unconstrained optimization formulation of the intersection motion coordination problem is defined as follows: Starting with the formulation of the distance value, ρ(t a i , t i ), the value of the objective function of the constrained problem-that is, the delay to the CR, f(t a i , t i )-is normalized as follows: where f and f are the minimum and maximum values of the constrained objective function amongst all the candidate solutions, respectively. Let N f be the number of particles in the swarm that satisfy all the constraints of the optimization problem. e ratio of the particles that satisfy the constraints (feasible particles) is calculated by the following equation: where α(t a i , 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. e selfadaptive 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. us, the penalty term, β(t a i , t i ), is defined as follows: If the infeasible solutions comprise of the swarm's majority, the first term, c(t a i , t i ), will dominate the penalty value. erefore, 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 a i , t i ), will dominate the penalty value. e candidate Procedure: PSO Input: S ⋆ i Output: t i Required: objective function (1) Draw randomly N p elements from the set S ⋆ i . (2) Initialize the particles' positions, X a (0), and fitness values, ϕ(X a (0)), for all a ∈ P.
(3) Find P g based on the initial fitness values of the population.
V a (t p + 1) ⟵ w n · V a (t p ) + c 1 · α 1 · (P a − X a (t p )) + c 2 · α 2 · (P g − X a (t p )) ▷ Update the velocity of the a th particle. (8) w n (t p + 1) ⟵ w max − ((w max − w min )/t max p ) · t p ▷ Update the inertia weight term of the a th particle. (9) X a (t p + 1) ⟵ X a (t p ) + V a (t p + 1) ▷ Update the position of the a th particle. (10) Calculate the fitness value, ϕ(X a (t p + 1)), of the a th particle. (11) if ϕ(X a (t p + 1)) < ϕ(P a ) then (12) P a ⟵ X a (t p + 1) (13) end if (14) if ϕ(X a (t p + 1)) < ϕ(P g ) then (15) P g ⟵ X a (t p + 1) (16) end if (17) end for (18)  Journal of Robotics 11 solutions that have a high fitness value will be added more penalty than the ones with a low value.

Severity of Constraint Violations: Calculating the Infeasibility
Measure. e previous section described how the constrained optimization problem could be converted into an unconstrained one using the penalty function method.
is conversion is deemed necessary because the PSO metaheuristic is applicable only to unconstrained optimization searches. e penalty function method relies on the incorporation of an auxiliary additive term, which is referred to as the penalty 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. e distance value ρ(t a i , t i ) and the self-adaptive penalty term β(t a i , t i ) of the unconstrained minimization problem, given in (16), require the determination of the infeasibility measure α(t a i , t i ). e 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. e motion characteristic conditions limit the trajectory of the vehicles to certain acceptable velocity and acceleration profiles when the vehicle is transporting in the CR.
is 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. is process incorporates three intermediate procedures: the Trajectory Generation, the Motion Projection, and the Quantification of Constraint Violations. e subsequent steps are described for an arbitrary vehicle A i that enters the IR at a time instant t i and has a candidate arrival time t a i .

Trajectory Generation.
e 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. us, each incoming vehicle's velocity profile, in the CR, is parameterized by its arrival time.
e trajectory of each vehicle in the CR is generated based on two kinematic conditions. e first requires that the vehicles shall not exceed their maximum velocity and acceleration bounds, v max and a max , respectively. e second necessitates that the vehicles enter and exit the CR with their maximum velocity, v max . 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, v max ). 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. Figure 8 illustrates the velocity and acceleration profiles of A i and the sequence of the distinct acceleration periods. e expected time duration where A i will be conveying in the CR is defined as follows: As mentioned earlier, the projected time duration of A i in the CR, t a g , 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 t a q � t a g /3. With the trapezoidal formulation of the velocity profile, the only parameter that needs to be determined is the vehicle's constant speed v a l during its zero acceleration phase (advancement with constant speed). is value is conveniently calculated by noting that the area under the velocity curve in the interval [t i t a i ] is equal to the displacement of A i 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 e constant acceleration that is needed to bring the velocity of A i from v a l to v max in a linear manner, during the acceleration period, is calculated by the following equation: e above value also represents the slope of the velocity's straight line segment at the acceleration phase. e vehicle's constant deceleration, at the first phase of its trajectory, is equal to − _ v a . Finally, the velocity of A i in the CR, for all three periods of the trajectory's profile, is expressed as follows: where t ∈ [t i , t a i ].

Motion
Projection. e 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. e 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. is 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. e total duration at the intersection π a i , of vehicle A i and corresponds to the candidate arrival time t a i , is calculated by the following equation: 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 π a i . Figure 9 illustrates how the Motion Projection procedure maps a vehicle's motion in the control and intersection regions. After determining the total duration π a i , a structure with all the discrete execution instances in the interval [t i , t i + π a i ] is formed; let T a i be the set of all these instances. e Motion Projection process finalizes by determining the set of postures, denoted by χ a i , that A i will have at all instances of T a i .

Quantification of Constraint
Violations. e final step of the constraint handling process is dedicated to quantifying the level of violation of each constraint by a candidate solution. e objective is to capture the safety conflicts, in both the control and intersection regions, and also record violations of the vehicles' kinematic limitations. e identification of safety conflicts in the IR is performed similarly to the reservation paradigm that was introduced in [7]. e 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 + . e refined grid array has c r � μ · (N R /2) · N L columns, r r � μ · (N R /2) · N L rows, and the cells' length is e r � e/μ. Each cell is identified by its row and column indexes as E K,L , where K ∈ R r � 1, . . . , r r and L ∈ C r � 1, . . . , c r . A timetable entry B r K,L (t) is assigned to the refined cell E K,L . If the latter is occupied at the execution instant t, then we set B r K,L (t) � 1; otherwise, B r K,L � 0. Before mapping the projected motion of A i to the refined cells of the intersection, a space-buffer is applied to add a safety distance between the vehicles in the IR. e dimensions of the vehicle are artificially expanded by a factor κ > 1 such that its new dimensions become l ′ � l · κ and w ′ � w · κ.
e refined grid array of the IR is shown in Figure 10. e occupancy mapping process, for A i , entails the determination of the set of occupied cells P i (t) in the refined IR that are encapsulated by the expanded dimensions of A i at time t. Let I i (t) � I 1 , I 2 , . . . , I |P i | be the set of indices that correspond to the elements (cells) of P i (t). e number of violations that the candidate solution t a i attains, when A i moves in the IR, is calculated by the following equation: where τ n denotes the collection of all discrete execution instances that belong to the closed interval [t

Journal of Robotics 13
CR, the safety conflict between two same-lane vehicles is detected by inspecting the distance between them. e number of violations of A i in the CR is given by where τ c denotes the set of execution instances in the closed interval [t i , t a i ], which corresponds to the duration that A i conveys in the CR, and b c is a binary decision variable that signals whether or not there is a safety conflict between A i and another vehicle in the lane. e decision variable b c is defined as follows: e term X p represents the coordinates of the same lane, succeeding vehicles to A i 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. e 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 In a similar fashion, the (binary) velocity violation value of the vehicle's trajectory is expressed as follows: e overarching objective of the constraint handling method is to quantify all the constraint violations for each candidate solution. erefore, the net value of the constraint violations of a candidate solution t a i is expressed as the weighted mean of all four constraint violation functions, given by where the term η max

Computational Complexity of the IC
is section provides a detailed account of the computational complexity (or worst-case running time (we use the term computational complexity interchangeably with worstcase running time or time complexity. Formally, these terms are not equivalent [63]. e 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 A i . is 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 C 1 be the computational complexity of the preliminary feasibility search and C 2 the complexity of the optimization search. e total complexity, C, of the procedure for calculating a vehicle's arrival time is C � C 1 + C 2 . 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. e properties of the big O notation can be 14 Journal of Robotics found in standard textbooks on the theory of computation and discrete mathematics such as [64] and [65].

Complexity of the Preliminary Feasibility
Search. e preliminary search for feasible solutions first identifies the fixed domain [t e t 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. e terms of the double summation in (12) are equal to the number of execution instances at which A i transports within the IR. We have already established that the duration of the longest path in the IR is δ max , described in (10). e 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 /T s , where T s is the sampling interval of the IC. Hence, the complexity of the preliminary feasibility search is given by From the above expression, we observe that the complexity of the preliminary feasibility search is cubic with respect to the product N R · N L (total number of lanes), which in part encapsulates the layout of the intersection. e complexity of the procedure is linear with respect to the floor term (⌈L/l⌉). e floor term (⌈L/l⌉) captures the impact of the control region's capacity to the complexity of the procedure. e higher the ratio (L/l) is, the more vehicles may simultaneously utilize the intersection, and the larger the searchspace, [t e t l ], becomes. Finally, the term (e + l)/T s v max expresses the impact to the computational complexity of the relation between the vehicles motion characteristics (maximum velocity v max ), 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.

Complexity of the Optimization Process.
e 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. e complexity, C 2 , of the PSO is O(t max p · N p · C o ), where C 0 is the complexity required to compute the fitness value of the unconstrained objective function, ϕ(·). e calculation of the objective function's fitness value, for a candidate solution, is outlined in Algorithm 2.
e reader is reminded that t max p is the number of iterations and N p is the number of particles in the swarm. e product term t max p · N p in C 2 appears because of the iterative double forloop 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, t max p is incremented until the swarm finds one that satisfies the constraints of the problem. e time complexity, C 0 -associated with the calculation of the unconstrained objective function's fitness value, ϕ(t a i , t i ), for a candidate solution t a i -grows with the maximal duration of the time horizon for which the motion of the vehicle is projected at the intersection. e maximum span of this time horizon is τ h � t l i − 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 /T s discrete execution time instances. e calculation of the objective function's fitness value is partitioned to three consecutive executable procedures (see Section 3.4).
e Trajectory Generation is a static map; hence, its procedure does not contribute to the asymptotic time complexity of the algorithm. e Motion Projection calculates the posture of A i for all execution instances in the time horizon τ h ; thus, its complexity is given by e final procedure, for determining the fitness value ϕ(t a i , t i ), is the Quantification of Constraint Violations, which is responsible for calculating the infeasibility measure, α(t a i , t i ), of a candidate solution t a i . e 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). e 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 /T s . 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 e second constraint violation function, η 2 , takes place for all execution instances for which the vehicle moves within the CR. e maximum duration of this interval, for a vehicle A i , is t l i − 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 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, O(1), 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 big O; see [65]: if g 1 ∈ O(f 1 ) and g 2 ∈ O(f 2 ), then g 1 + g 2 ∈ O(max f 1 , f 2 )) (namely, Trajectory Generation, Motion Projection, and Quantification of Constraint Violations), the complexity C 0 associated with the calculation of a single fitness value of the objective function is given by e 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, C 0 is multiplied by t max p · N p , rendering the complexity of the optimization step to (we have used the transitivity property Procedure: unconstrained objective function ALGORITHM 2: Pseudocode of the unconstrained objective function procedure. e procedure computes the fitness value of the objective function that corresponds to a candidate solution of the unconstrained optimization problem. for big O; see [64,65]: if h ∈ O(g) and g ∈ O(f), then h ∈ O(f)): In practice, due to the product t max p · N p , 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 N R · N L . e quadratic term of the ratio e + l/T s v max that appears in C 1 does not have significant impact when the distance covered by the vehicles in a single step (distance T s · v max ) is comparable to their dimensions (length l) and the size of the intersection's cells (width e).

Total Complexity: Single Vehicle.
e time complexity, C, of the comprehensive procedure that computes the arrival time of an incoming vehicle to the IR is given by e 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. e complexity of the algorithm depends on the physical dimensions and configuration of the intersection, encapsulated by the parameters N R , N L , and e; the geometry and motion characteristics of the vehicles, captured by the parameters l, w, and v max ; and parameters of the algorithm, namely, t max p , N p , μ, and κ.

Simulation Results
In this section, we evaluate the performance and effectiveness of the proposed IC via numerical simulations. e 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-toinfrastructure (centralized) architecture. us, 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. e parameters of the numerical simulations are outlined in Table 1. e length, l, and width, w, of the autonomous vehicles are set to 6 m and 3 m, respectively. e maximum velocity of the vehicles, v max , was set to 10 m/s, and their accelerate/decelerate bound, to ±2 m/s 2 . e 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 e r � 0.3 m. Finally, the sampling interval of the IC, T s , was selected as 0.01 s. e 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. e configuration of the first intersection involves two roads with a single-lane each-the vehicles can only move straight, and there are no turns. e two single-lane road intersection configuration (N R � 2 and N L � 1) has a constant service time τ i � ((e + l)/v max ) s for every incoming vehicle. e entry times of the incoming vehicles, at each lane of the CR, are randomly generated by a Poisson process. e 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. Figure 12 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.8 s. 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, λ. e difference in the traffic flow reflects the impedance (delay) of the vehicles in the CR. e two traffic flows, at the entrance and exit of the CR, are nearly equivalent when λ is less and equal to 30 vpm. A reduction of roughly 10% in the exit flow, compared to the entry, appears at the maximum traffic load, when the arrival intensity is 50 vpm. Figure 14 depicts the utilization of the IR by the incoming vehicles. For the two single-lane road intersection configuration (N R � 2 and N L � 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. e 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. e results show that the IC controller accomplishes almost full utilization when the arrival intensity of the vehicles is greater and equal to 40 vpm. e second simulation case study accounts an intersection configuration where four roads with two lanes each are intersecting (N R � 4 and N L � 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. e 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 30 vpm. e 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 probability p t � 0.3. In the case of a turn, the selection of the exit road is also random. is probability depends on the lane of the vehicle. For a left lane vehicle, the probabilities for turning left and right are p l f � 0.7 and p r f � 0.3, respectively. For a right lane vehicle, the probabilities p l f and p r f are reversed. e performance of the IC is compared with a conventional traffic lights intersection control system. e 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. en, the vehicles that are going straight are allowed to move, but the left turn is prohibited during this period. e final phase is the amber light, and it provides a safe transition between the red and green lights. e amber light enables vehicles to finalize their motion in the IR before the next nonintersecting roads are mobilized. Figure 15 depicts the average delay that the vehicles experience along the CR. e 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. e delay of the vehicles in the intersection reaches its maximum value, of approximately 14 s, when the vehicle arrival rate is 30 vpm per lane. e 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. e incoming and outgoing traffic flows are almost the same (negligible delay), and their difference is at most 16% when λ � 30 vpm. The proposed IC Traffic light g.c. 5s Traffic light g.c. 10s Traffic light g.c. 15s Arrival intensity (veh/min) Figure 16: Second case study: average traffic flow at the entry and exit of the CR, respectively, for varying values of the arrival intensity, λ.

Conclusion
We have presented a scheduling-based approach for the formulation of an IC for autonomous vehicles that cross a multilane intersection. e 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. e IC computes the trajectory of each incoming vehicle in a first-come first-served order. e performance metric of the optimization problem is the delay of the vehicle at the CR. e penalty method is used to convert the constrained optimization problem into an unconstrained one, making it compatible with metaheuristics. e fitness value of the unconstrained problem's objective function captures the severity of the violations that correspond to a candidate solution. e PSO is employed to harvest the search-space for an optimal solution to the optimization problem.
e 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.
Data Availability e data used to support the findings of this study are included within the article.

Disclosure
is article is the complete and detailed version of the work by the same authors reported in the Proceedings of 2018 IEEE Annual American Control Conference (ACC) [67].

Conflicts of Interest
e authors declare that they have no conflicts of interest.