Experimental validation of a semi‐distributed sequential quadratic programming method for optimal coordination of automated vehicles at intersections

In this article, we study the optimal coordination of automated vehicles at intersections. The problem can be stated as an optimal control problem (OCP), which can be decomposed as a bi‐level scheme composed by one nonlinear program (NLP) which schedules the access to the intersection and one OCP per vehicle which computes the appropriate vehicle commands. We discuss a practical implementation of the bi‐level controller where the NLP is solved with a tailored semi‐distributed sequential quadratic programming (SQP) algorithm that enables distribution of most computation to the vehicles. Results from an extensive experimental campaign are presented, where the bi‐level controller and the semi‐distributed SQP are implemented on a test setup consisting of three automated vehicles. In particular, we show that the vehicle‐level controller can enforce the scheduled intersection access beyond the accuracy admitted by the sensor system, and that the bi‐level controller can handle large perturbations and large communication delays, which makes the scheme applicable in practical scenarios. Finally, the use of wireless communication introduces delays in the outer control loop. To allow faster feedback, we introduce a real‐time iteration (RTI) like variation of the bi‐level controller. Experimental and simulated results indicate that the RTI‐like variation offers comparable performance using less computation and communication.


INTRODUCTION
The current trend toward automation of road vehicles can be expected to continue, and eventually most vehicles will be fully automated and communicating. This technology can be leveraged to obtain synergistic effects through cooperation between the automated vehicles, and thereby enable drastic improvements to the traffic system. In this article, we discuss an algorithm necessary for one such improvement: the automation of intersection crossings. With all vehicles automated, communicating and cooperative, the traffic-lights, signs and rules used today could be removed and the vehicles could instead rely on automated coordination controllers. As discussed in Reference 1, the potential benefits include increased safety, increased energy efficiency and higher traffic throughput.
However, there are several challenges that must be addressed before coordination algorithms can be applied in practice. Most importantly, such controllers must be able to guarantee that no collisions occur, and in particular, the guarantees must be applicable to scenarios with uncertainty. This includes handling unexpected events and the online recoordination of vehicles in the presence of new information. Furthermore, a useful coordination algorithm must be scalable to be relevant for more than small scenarios. However, since finding the optimal collision free motion profiles for vehicles crossing an intersection is a combinatorial problem, there are computational scalability issues. In fact, determining the existence of even one collision free solution has been shown to be an NP-hard problem in the general case. 2 Moreover, it is a known problem that vehicle-to-vehicle (V2V) communication systems have capacity limits. 3 A practically useful coordination algorithm must therefore also scale well in terms of both how often communication is required and the data volumes involved.
Although the application of intersection coordination algorithms lies in the future, a number of contributions have been made during the last decade, many of which are surveyed in References 4 and 5. In the literature, various heuristics are used to address the challenges of the problem: the motion profile is typically the result of a rule-based controller which switches between discrete behavioral modes, [6][7][8][9] or is obtained from a restricted space, for example, trapezoidal 10 or linear 11 velocity profiles. On the other hand, a number of approaches based on optimal control (OC) formulations of the coordination problem can be found in the literature, for example, . In most cases, the selection of a crossing order is separated from the computation of the optimal state and control trajectories to avoid the combinatorial complexity of the solution space. For instance, in References 14,16,19 variations of "First-Come-First-Served" policies are first used to produce a crossing order and OC problems constrained to satisfy this order are then solved. Using a similar strategy, 15 leverages results from polling-systems to compute the crossing order, while in References 24 and 23 mixed-integer quadratic programming (QP) is used to compute an approximately optimal crossing order. A different approach is taken in Reference 12, where a heuristic gives a decision order rather than a crossing order. The vehicles thereafter sequentially solve optimal control problems (OCPs), where each vehicle is constrained to avoid collisions with the vehicles that precedes it in the decision order. The application of OC formulations to closed-loop control is considered in References 13,21 and 25.
As discussed in Reference 1, the benefits of OC approaches in general include the ability to consider a wider range of applicable motion profiles and include constraints. Given the severity of collisions, closed-loop control, that is, the recalculation of control commands based on measurements of the system state, is a necessity to handle the uncertainty that is present in real scenarios. In many cases, OC schemes can leverage well established theory to derive properties of closed-loop control schemes, account for various forms of uncertainty and construct efficient solution algorithms.
In this article, we use the OC formulation of the coordination problem first presented in Reference 24 but focus on finding the optimal solution for a given crossing order. With this formulation, the problem is given a hierarchical structure, where optimal, collision free intersection occupancy time-slots are obtained as the solution to a nonlinear program (NLP), and the optimal state and control trajectories as the solution to OCPs that are separable between the vehicles. This structure enables a bi-level model predictive control (MPC) architecture where coordination is separated from vehicle control. In particular, the outer, intersection-level, controller computes and updates optimal, nonoverlapping time-slots based on the current vehicle states, and the lower, vehicle-level, controllers compute the control commands for the vehicles, given a time-slot and the current state.
In earlier work, we proposed a semi-distributed sequential quadratic programming (SQP) approach for the solution of the time-slot NLP. 26 The algorithm was extended in Reference 27 where a convergence proof also was given. We established the persistent feasibility of the bi-level MPC scheme and discussed robustness aspects in Reference 25, and presented experimental results. Extensions to economic nonlinear MPC were presented in Reference 22, and a comparison of References 26 and 27 was given in Reference 28, supported by experimental data.
The SQP procedure of References 26 and 27 has the property that most computations can be performed on-board the vehicles and the algorithm's internal message passing can be performed using V2V communication. Consequently, the SQP procedure can be used to close the intersection-level control loop (repeatedly solve the time-slot NLP online) in a semi-distributed manner. While such a scheme has several desirable properties, it is necessary to evaluate its usefulness in a practical setting. First, the algorithmic performance needs to be assessed for real scenarios where, in particular, the effects of delays inherent to the use of real communication systems must be studied. Second, the effects of both algorithmic performance and real-world perturbations on the performance of the bi-level controller must be investigated and possible issues addressed. To this end, we describe a practical implementation of the bi-level controller in this F I G U R E 1 A, Contains a schematic illustration of the scenarios considered in this article. B, Illustrates how the intersection is modeled: the arrows show the fixed paths of the vehicles, and the red square illustrates the zone inside the intersection where collisions can occur [Colour figure can be viewed at wileyonlinelibrary.com] article, where the intersection-level control loop is closed using the SQP presented in References 26,27. In particular, we detail the application of the bi-level controller on a test setup consisting of three automated vehicles, where the SQP is solved in a semi-distributed fashion using V2V communication, and the vehicle-level control loop is closed using the high-performance QP solver HPMPC. 29 Furthermore, we introduce modifications to the bi-level controller that increase the practical applicability of the scheme. First, a relaxation of the vehicle-level MPC problem is presented, which resolves infeasibility issues inherent to the formulation in References 26,27. Second, two modifications to the intersection-level controller are introduced to handle the large computational delays that can arise due to the execution of the SQPs over a wireless network which introduces delays. In particular, we propose a scheme where the intersection-level control loop is closed in a real-time iteration (RTI) like fashion 30 to allow faster feedback. That is, instead of solving the NLP to convergence, the intersection-level control loop consists of the time-slot updates resulting from one SQP iteration. Moreover, we present results from an extensive experimental campaign where the implementation was evaluated. We discuss the algorithmic performance and provide a detailed study of the SQP execution times, where the experimental data is compared with ideal cases. Comparative data is provided for both experimental and simulated cases, where the system is subject to both large and small perturbations.
The remainder of the article is organized as follows: The modeling and OC formulation of the problem are introduced in Section 2, while the semi-distributed SQP and practical implementation are discussed in Sections 3 and 4, respectively. The experimental results are presented in Section 5 and the article is concluded with a discussion in Section 6.

PROBLEM FORMULATION
In this section, we introduce the modeling and OC formulation of the intersection problem and discuss how the formulation can be decomposed which enables a bi-level structure for closed-loop control. The computational and other practical aspects, for example, what computation is performed where, is discussed in Section 3.

Modeling
We consider problems such as that shown in Figure 1A, where N a vehicles approach an intersection equipped with a central coordinating unit. We assume that all involved vehicles are automated, cooperative and participate in the coordination procedure and that no noncooperative entities (eg, pedestrians and bicyclists) are present. For simplicity, we also assume that no vehicles makes turns or change lanes, but note that such vehicles could be included using the methods discussed in Reference 31. We assume that the vehicles move along predefined paths and that the vehicle dynamics along the paths can be described bẏx where x i (t) ∈ R n , u i (t) ∈ R m and A c i ∈ R n×n , B c i ∈ R n×m . Specifically, the state vector is such that x i (t) = (p i (t), y i (t)), where p i (t) ∈ R is the position of the center of the vehicle on its path and y i (t) ∈ R n−1 collects all non position states (eg, velocity and acceleration). Moreover, the vehicle state and control trajectories are subject to constraints of the form capturing, for example, actuation limitations and passenger comfort restrictions. We consider only D i , G i , A c i , B c i such thatṗ(t) ≥ 0, that is, the dynamics and constraints are such that no vehicle can reverse.
As shown in Figure 1, we define the intersection as an interval [p in i , p out i ] on the path of each vehicle such that collisions hold, for all vehicles i ≠ j * . Furthermore, we define the time-of-entry, t in i , and time-of-clearance, t out i , of the intersection through, respectively. Collision avoidance is thereby ensured if for all vehicle pairs (i, j) such that vehicle i crosses the intersection before vehicle j. In the remainder of the article, we

OC formulation
With the objective where V i,f (x i (t f )) and i (x i (t), u i (t)) are convex and quadratic, and t f is fixed, we state problem of optimally coordinating the vehicles through the intersection as where the vehicles are ordered such that vehicle i crosses before vehicle i + 1. Here, I [a,b] = {a, … , b} for integers a < b, x(t) = (x 1 (t), … , x N a (t)), u(t) = (u 1 (t), … , u N a (t)), T = (T 1 , … , T N a ) andx i,0 is the initial state of vehicle i.
*We want to emphasize that the definition of the intersection easily can be subdivided into several mutual-exclusion zones, each with its own start and stop position, as done in Reference 23. However, for simplicity of presentation, the article is developed with the single zone shown in Figure 1.

Decomposition
It was shown in Reference 24 that the coordination problem can be decomposed in a hierarchical fashion, where the time-slot schedule T is the solution of a NLP, and the vehicle state and control trajectories x i (t), u i (t) are the solution to separable vehicle OCPs. The following NLP computes the optimal time-slot schedule T for a given order S where V i (x i,0 , T i ) is defined as the optimal value function of the OCP of vehicle i For the optimal T i , (8) gives the optimal state and control trajectories x i (t), u i (t).

Discretization
For practical reasons, we consider piecewise constant inputs and discretize the vehicle dynamics using the sampling time t s when we solve the vehicle problem (8). More precisely, we define x i,k = x i (t k ) and u(t) = u i,k , ∀t ∈ [t k , t k+1 [, with t k = kt s , and the state update function Since the discrete position is defined only at t k , (3) defines values of t in i and t out i that are integer multiples of t s in the discrete time case. To allow t in i and t out i to assume continuous values, we define a continuous time representation of the position using the discrete time state and control sequences as where and where t f = Nt s . The discrete time statement of the objective function is whereby we have the discrete time formulation of the OCP (8) of each vehicle i as which can be solved for real valued t in i and t out i in [0, Nt s ]. We note here that for w i which satisfies (11c), p d i (t, w i ) is K-times continuously differentiable, where K is the relative degree of (1) with the position p i (t) as the output. 26

Problem properties
The constraint set domain(V i (x i,0 , T i )) in (7b) is implicitly defined as the set of T i for which the optimization problem (11) is feasible given the initial statex i,0 . However, it was shown in Reference 24 that domain(V i (x i,0 , T i )) can be written as where are defined as the solutions to the NLPs That is, t in i must lie between the earliest and latest time-of-entry that the vehicle can perform. Similarly, for a specified time-of-entry t in i , t out i must lie between the earliest and latest time-of-clearance the vehicle can perform. Moreover, it was shown in Reference 26 that if a mild technical assumption holds, the optimal solutions to the linear programs (LPs) are also solutions to (15) and (16), respectively. Consequently, (15) and (16) can be evaluated by first solving the LPs (17), (18) and thereafter solving for U(x i,0 , t in i ) and L(x i,0 , t in i ). The bounds L in (x i,0 ) and U in (x i,0 ) can be obtained similarly. For notational convenience, in the remainder of the article we will only include the explicit dependence onx i,0 in h i (x i,0 , T i ) and V i (x i,0 , T i ) when necessary.

Receding horizon implementation
In order to reject perturbations and compensate for model inaccuracies, the solution to the optimal coordination problem can be applied in a receding horizon fashion in a MPC. In particular, the decomposed formulation offers a natural separation between coordination and vehicle control. This enables a bi-level control structure: an outer, intersection-level, control loop computes collision free time-slots by solving (7) at the current state, while inner, vehicle-level, control loops compute the vehicle control command u i,0 for a given time-slot T i at the current state through (11). Feedback is thereby due to estimates of the current state in the inner control loops, and the cost function V i (x i,0 , T i ) and the constraint set h i (x i,0 , T i ) in the outer control loop. This scheme has the benefit that perturbations acting on one vehicle are counteracted by all vehicles, that is, the optimal time-slot of vehicle i at time k, T i (X k ), is a function of the state of all vehicles X k = (x 1,k , … , x N a ,k ) through (7). In principle, the time-slot schedule could be computed once and the rejection of possible perturbations handled by the inner control loops. However, by closing the outer control loop the system can (a) reject larger perturbations by adjusting the time-slot schedule T and thereby provide collision avoidance in more demanding scenarios and (b) continuously improve the solution. The bi-level controller is discussed further in Reference 25, where the closed-loop system is shown to be persistently feasible and stable.
The computations required to close the inner, vehicle-level control loop (solution of QP (11)) can be performed independently on each vehicle. However, closing the outer, intersection-level control loop involves the solution of NLP (7), which requires information from all vehicles. In the following section, we discuss an algorithm which solves NLP (7), where most computations are distributed and performed on-board the vehicles.

A SEMI-DISTRIBUTED SQP METHOD
Considering the intended application, an algorithm where much of the computations required to solve NLP (7) can be performed on board the vehicles is desirable as it improves scalability. However, if computations are performed on board the vehicles, the algorithm requires information exchange over the V2V network. As is reported in Reference 3, there are scalability issues with the current V2V technology and frequent and large data exchange should be avoided. Consequently, second-order optimization methods are preferable to first-order ones, as the former in general need fewer iterations to find a solution to the problem. For this reason, a semi-distributed SQP algorithm was proposed in References 26,27. We recall the details of the SQP algorithm in Section 3.1, discuss its application in a practical setting with semi-distributed computation in Section 3.2, and present two different approaches to its application for closed-loop control Section 3.3.

Sequential quadratic programming
Using the developments of Section 2.2.3, we rewrite NLP (7) as where V(T) = ∑ N a i=1 V i (T i ) and we lumped constraints (7b)-(7c) in the function h(T). The associated Lagrangian function is defined as Here, i are the Lagrange multipliers of the constraint h i (T i ) ≥ 0 and s the multipliers of the precedence constraints (7c), which we write as h s (T) ≥ 0 Starting from an initial guess z (0) = (T (0) , (0) ), SQP iteratively updates the primal-dual solution candidate z (c) using with (c) ∈ (0, 1] and Δz (c) = (ΔT (c) ,̃( c) − (c) ). Here, (ΔT (c) ,̃( c) ) is the primal-dual solution of the QP subproblem where H (c) is a positive-definite approximation of the Lagrange function Hessian ∇ 2 T (T (c) , (c) ). Variants of SQP differ primarily in the computations of the step size (k) and the Lagrangian Hessian approximation H (k) . We describe next the methods employed to solve (7). For more details on SQP see, for example, Reference 32.

Hessian approximation
To ensure that the QP subproblems (23) are convex, it is required that the reduced Hessian is positive-definite. While there are several ways of enforcing positive-definiteness of the reduced Hessian, we adopt the strategy of adding enough curvature in all negative-curvature directions to ensure that the full Hessian is positive-definite. In particular, we note that NLP (21) is such that the Hessian has the block-diagonal form where i are the normalized eigenvectors corresponding to the eigenvalues in D i . The required eigenvalue decomposition is cheap due to the small size of the blocks

Step size selection
In order to guarantee convergence of SQP algorithms, the step-size (c) must be selected such that progress toward a solution to the problem is made. In this article, we employ a line search on the so-called 1 merit function, which is defined as where h − (T (c) ) = min(h(T (c) ), 0) and (c) is a parameter chosen so that (c) > || (c) || 1 . Progress toward a solution is ensured when (c) is selected such that the Armijo condition is satisfied: where ∈ (0, 0.5] and D ΔT (c) M(T (c) ) is the derivative of M(T) in the direction of ΔT (c) , evaluated at T (c) . Provided that ΔT (c) is a descent direction on (25), (c) which satisfies (26) exists and can be found by so-called backtracking, that is, by successively decreasing (c) from 1 until (26) is satisfied. 32 Since the constraint h i (T i ) ≥ 0 defines the set of feasible parameters for the parametric QP (11), and V i (T i ) is the optimal value function for the same QP, we note that V i (T i ), and thereby M(T), is undefined when h i (T i ) ≱ 0. We resolve this issue by using the projection-based method of Reference 27, where the merit function is evaluated at It was shown in Reference 27 that if ΔT (c) is a descent direction on (25), a small enough (c) exists which satisfies (26), such that one can perform backtracking based on M((T (c) + (c) ΔT (c) )). Note that with this modification, the 1 merit function reads M((T)) = V((T)) + ||h − s ((T))|| and the primal-dual update is An alternative solution to the issue of nondefined M(T) is to soften the position constraints (11e) and (11f) with an 1 penalty as suggested in Reference 26. However, in doing so the objective in the quadratic subproblem (42) will be dominated by the penalty term whenever it is evaluated at h i (T i ) ≱ 0 for one vehicle, and a sharp nonsmoothness appears at points where the problem becomes feasible. The algorithmic performance of this method has been found to be worse than the method based on (27). 28

Calculation of derivatives
The first and second-order derivatives of the objective function components V i (T (c) i ) and constraint components h i (T (c) i ) are required to form the QP-subproblem (23).
i ) is the optimal value function of the QP (11) and h i (T (c) i ) is evaluated by solving the LPs (18), (17), the derivatives are obtained using results from parametric sensitivity analysis. In particular, we have that where is the Lagrangian Function of the QP (11). 33 Here, w i (T i ) is the primal solution of (11) for T i , is the dual solution corresponding to constraints (11e), (11f). The second-order derivatives can then be obtained using the chain rule, for example, The derivatives of the constraints are obtained similarly, as is exemplified for U(t in i ) below. We have by definition that where w ub The second-order derivative is obtained by applying the chain rule to (32), but the resulting expression is rather large and is omitted here for brevity, and the interested reader can find it in Reference 26. It should be noted that it includes the term d 2 w i ∕dt in i 2 . The computation of the first and second-order derivatives of L(t in ) is identical.

Parametric sensitivity analysis
The expressions (29), (30), and (32) rely on the first and second-order sensitivity of the primal-dual solution with respect to T i , which acts as a problem parameter in the QP (11) and LPs (18), (17). Note that for the general parametric optimization problem with free variable x and parameter p the KKT conditions are satisfied at the solution, given that some constraint qualification hold. Denoting the primal-dual solution to (33) z * = (x * , * ), the KKT conditions are where (z) = q(x, p) − ⊤ a(x, p) and * denotes element-wise multiplication. In particular, denoting the set of active constraints at the solution a A (x * , p) and the corresponding multipliers * A , we have that and note that the solution map z * ∶= z(p) must be such that dr(z(p),p) dp = 0. Evaluating the total derivative of r(z(p), p) w.r.t. p, one obtains and, using the chain rule, Both (36) and (37) exist if ( r z ) −1 exists, and the latter is guaranteed if linear independence constraint qualification and the second-order sufficient conditions hold at the primal-dual solution of (33). The sensitivities required to evaluate (29), (30), and (32) can thereby be obtained by solving (36) for the KKT conditions of the QP (11) using the parameters T i , and by solving (36) and (37) for the KKT conditions of the LPs (17), (18), using the parameter t in i . Note that if the program (33) is solved using a second-order method, the solver performs iterations similar to This means that the solver will have factorized the matrix r z at the solution z, whereby the evaluation of (36) can be performed at little additional computational cost. Consequently, if a second-order method is used to evaluate V i (T i ) and h i (T i ) in the SQP, the derivatives are very cheap to compute.

Schematic algorithm
In a practical setting, the SQP procedure is performed as follows 1. In the central node, Initialize the primal-dual variables z (0) = (T (0) , (0) ).
The computations in steps 2 and 44a consist of the solution of QP (11) and LPs (17), (18) and the associated derivative computations detailed in Section 3.1.3. We emphasize that most computations are separable and can be performed in parallel on board the vehicles. However, while the LPs (17)- (18) can be solved in parallel they must be solved before the QP (11) due to the use of  i (T i ). The nonparallelizable part of the algorithm is the formation and solution of the QP-subproblem (23), which thereby necessitates a central network node. In the scenarios considered in this article, the Note: The central node needs to send two floats to each vehicle: the current primal solution candidate T (c) i .

TA B L E 1 The information
which needs to be sent from a vehicle to the central node each iterate Coordinator shown in Figure 1A takes this role. Note that the SQP sub problem (23) has 2N a variables and 5N a − 1 constraints, and will be significantly smaller than the vehicle-level QP (11) in moderately sized scenarios and horizons N. In such cases, the computational bottleneck will be the evaluation of V i (T (c) i ) and h i (t in i (c) ) and the associated derivatives.
A convergence proof for the SQP applied to (7) is given in Reference 27.

Communication aspects
The information that needs to be communicated from the central node to the vehicles is only the currently held primal solution candidate, consisting of 2N a floats. The data a vehicle is required to communicate to the central node is listed in Table 1. Consequently, each iterate will involve the communication of at least 14 floats per vehicle, and additional 14 for each reduction of the step size . This will increase the time per iterate, and can, depending on communication protocol and implementation, constitute the bulk of the time required to solve the problem. We note that it is only necessary to resend (T i ) and V i ((T i )) to evaluate the merit function M and check the Armijo condition (26). The remaining information in Table 1 could then be sent after the primal-dual update. However, while the amount of data transmitted would be less, two additional rounds of communication would be needed: one from the central node, notifying the vehicles of step acceptance, and one from the vehicles containing the remaining information. As practical communication systems include a significant overhead, the total time required by communication would be notably higher. While this requires the vehicles to compute the derivatives of V i (T i ) and h i (T i ) when not strictly needed, the calculations can be made highly efficient and will have a small impact on the total solution time (c.f. Section 3.

1.3).
Besides what is mentioned here, the algorithm will require the communication of a number of logical variables, for example, commands for algorithm start and stop, step acceptance, and so on. However, the amount of such data per iterate consists of a few bytes and, since it can be sent together with the other data, the additional time required is negligible.

Real-time implementation of the intersection-level controller
When the intersection-level control loop is closed using the SQP introduced in this section, it includes distributed computation and wireless communication. In the ideal case, algorithmic overhead, computation and communication require a negligible amount of time compared with the time scale of the system dynamics, and the current vehicle state x i,k (and therefore V i (x i,k , T i ) and h i (x i,k , T i ) can be considered constant during the time it takes to solve the SQP. However, nonnegligible delays can be expected in a real application, and the vehicle state might change significantly between the SQP iterates. The use of wireless communication in particular is a source of comparatively large delays, as packet drops are likely to occur and subsequent retransmissions of data often are necessary. This raises what is known as the real-time dilemma: 34 Should the NLP be solved to convergence when the resulting T will be outdated w.r.t. the system state, or should an approximate solution T be sought using the most up-to-date information? This consideration implies that the resulting control law T * (x i,k ) will be a suboptimal approximation of the truly optimal solution feedback, regardless of how the dilemma is handled. In this article, we consider two different solutions to the dilemma, which we present in the following.

Alternative 1: Solving the intersection-level NLP to convergence from a predicted state
In the first solution, which we denote the Converged controller, we use a scheme similar to that of Reference 35 or 36, where V i (T i ), h i (T i ) are computed from a predicted future state x i,K rather than the current state x i,k , K > k, and the resulting control law T * (X i,k ) is not applied until time t K . The predicted state is obtained from the open-loop predictions of QP (11) in the vehicle-level control loops, which are computed using a previous time-slot schedule T. A block diagram of the scheme is shown in Figure 2A.

F I G U R E 2 Schematic illustration of the two intersection-level controllers and locality of computation
If the evolution of the real system stays close to the predicted trajectories, that is, x i,K predicted at some k < K is close to the actual state x i,K at K, the resulting intersection-level control-law will provide a good approximation T * (X i,k ) to T * (X i,k ). However, the scheme will introduce a significant delay in the feedback of the intersection-level scheme. In fact, denoting the update frequency of the intersection-level controller t O s , we note that the reaction to a perturbation that occurs between kt O s and (k + 1)t O s will not be applied to the system until (k + 2)t O s .

Alternative 2: Approximate the intersection-level NLP in an RTI-fashion
The second solution, which we denote the 1-step controller, does not solve the SQP to convergence, and thereby avoids long solution times. Instead, we adopt a strategy where the current state measurements are used to compute V i (T i ), h i (T i ) and their derivatives, but where only one full SQP step is taken in the solution of (7). The resulting control law T (1) (X k ) = T (0) + ΔT (0) (X k ) is thereafter applied directly to the vehicles. While the control law is approximate, it enables rapid feedback and reaction to perturbations to the vehicles. The procedure is shown in Figure 2B. This can be seen as an application of the RTI scheme for dynamic optimization, 30 applied to the NLP (7). However, we note that the dynamic optimization problems (11) that compute the vehicle control commands are solved to convergence at all times.
The physical location of the solution of the optimization problems involved in the inner, vehicle-level control loop as well as the two variations of the outer, intersection-level control loop is provided in Figure 2C.

EXPERIMENTAL VALIDATION
In this section, we describe an experimental setup which was used to validate the bi-level controller described in Section 2.3. In particular, we detail an implementation of the semi-distributed SQP described in Section 3.1 in which most computations are performed on board the vehicles and communicated to a central coordinating unit using V2V communication. We also provide details on the hardware platform used.

Practical implementation of the vehicle-level control-loop
The vehicle-level control loop consists of solving problem (11) every time instant based on the current state x i,k , using the time-slot T i and applying the resulting optimal u i,k to the vehicle. However, problem (11) differs from standard MPC formulations in that the position constraints (11e) and (11f) force the vehicle to be at a specific position at a given time. As the vehicle gets closer to the intersection, the ability to affect when the intersection is entered and departed diminishes. Moreover, in a real scenario, the closed-loop system is constantly exposed to perturbations in the form of plant-prediction model mismatches, measurement noise and other external disturbances. It is therefore increasingly likely that problem (11) is infeasible for (x i,k , T i ) as the vehicle gets closer to the intersection. To address this issue we first relax the equality constraints (11e) and (11f) to the inequalities With this relaxation, the vehicle is allowed to occupy the intersection within T i rather than using the intersection throughout all of T i . While this ensures that the controller, for example, does not slow down the vehicle to stay longer in the intersection in response to a perturbation, infeasibilities are still possible. We therefore introduce a softening of the constraints (39) as Here, we differentiate the state and control of the vehicle x i,k , u i,k from the open-loop predictions x i,k ,ū i,k . The control command applied at time t k is the optimal open-loop control command u i,k =ū * i,k . The softening of the constraints ensures that there will be no feasibility issues due to the position constraints (39). In fact, T i no longer affects the feasibility of the optimization problem (41). Note that if i is chosen large enough, P i ( i ) † is a so-called exact penalty function. 32 A well-known property of exact penalty functions is that the problem with softened constraints will return a solution with i = 0 whenever such a solution exists (see eg, 37, Theorem 14.3.1). Moreover, when i is chosen large enough and no solution exists where i = 0, the solution minimizes || i || ∞ and thereby the violations of † The quadratic term in P i ( i ) is added for numerical reasons, and the parameter q is typically small. the constraint (39). 25 The quadratic term with parameter q i does not jeopardize the exact penalty property and is added to ensure the numerical stability of the algorithm.

Efficient solution of the QPs and LPs
General purpose solvers are too slow to give real-time feasible solutions to the vehicle-level QP (41) and the LPs (18), (17), and solvers tailored to the special structure of these optimization problems have to be considered. As discussed in Section 3.1.3, in case second-order optimization methods are used to solve these QPs/LPs, the sensitivities of the cost function and constraints can be easily and cheaply computed from the local optimization problems by reusing the KKT matrix factorization available from the QPs/LPs solver. In this work, we used a version of the interior-point method (IPM) called HPMPC 29 which is tailored to allow the efficient computation of the tangential predictor at the solution. HPMPC provides an implementation of a Mehrotra's predictor-corrector IPM tailored for the solution of QPs in the form of OCPs. The IPM employs a backward Riccati recursion for the efficient computation of the search direction. As its linear algebra framework, HPMPC makes use of BLASFEO, 38 which provides a set of linear algebra routines tailored to provide high computational performance for rather small matrix sizes, as typical in embedded optimization applications.
On the algorithmic side, the IPM in HPMPC is coupled with a partial condensing algorithm. Partial condensing 39 is a technique that allows one to control the level of sparsity of an OCP problem by trading off horizon length with input vector size, by condensing block-wise the original OCP. It is possible to compute the theoretical optimal horizon length based on the analysis of the flop count of the algorithm. In practice, however, other factors affect the optimal choice of the horizon length, such as the performance of linear algebra routines. 40 The QP (41) is a perfect example of that. Since the state and input vector sizes are very small and the horizon length is long, partial condensing gives a QP reformulation that HPMPC can solve much faster, since many operations on small matrices (where the linear algebra performs poorly) are replaced with few operations on large matrices (where the linear algebra gives higher computational performance).
In this work, HPMPC has been modified to allow the efficient computation of sensitivities. Namely, the solver now allows the reuse of the last KKT matrix factorization (where Lagrange multipliers and slack variables of inequality constraints are fixed at their value close to the solution) to cheaply compute the solution of other systems of linear equations with different right-hand side. If there are no changes in the active set, this allows the efficient computation of the tangential predictor around the current solution. 41 Therefore, the sensitivities in (36) can be cheaply computed by performing the partial condensing of the right-hand side and the solution of the KKT system reusing the cached KKT matrix factorization. The computational cost of these operations is negligible with respect to the QP/LP solution, which comprises a complete partial condensing preprocessing step, plus a KKT matrix factorization and two KKT system solutions per IPM iteration (which are typically in the range of 6-15 per QP/LP solution).

Efficient solution of the intersection-level problems
As noted in Section 3.2, the computational bottleneck of the SQP algorithm will commonly be the solution of the vehicle-level QP (41) and the LPs (18), (17). This is due to the comparatively small size of the QP subproblem (23) in such cases. General purpose QP solvers can therefore be fast enough and used to solve (23) in real-time. Due to this, MATLABs QP-solver quadprog was used in the experimental validation. However, with an increasing number of vehicles, the time required to solve the QP subproblem (23) with a general purpose solver will approach that required by HPMPC for the solution of (41) and the LPs (18), (17). For large scenarios, solving (23) could therefore become a computational bottleneck of the SQP. It is therefore desirable to use efficient, structure exploiting solvers also for the QP subproblem (23). For this reason, we propose the following reformulation of (23): That is, the time-slot increments ΔT i are formulated as states in the dynamical system (42b), where the variable ) is introduced as a fictitious control. The precedence constraint (7c) is formulated as the path constraint (42c). The problem is thereby written on a stage-wise form for which efficient solvers such as HPMPC can be deployed, and significant performance gains can be made. For instance, the typical time required to solve (23) for a three vehicle scenario with quadprog is around 2 ms using a standard laptop. The time required by HPMPC for the same problem using the same hardware lies around 40 s. Moreover, the time-complexity of HPMPC is linear in the number of stages, and approximately 100 s are required for a 30-vehicle scenario, where quadprog requires 7ms. For a 300-vehicle scenario HPMPC requires approximately 850 s to converge, while quadprog requires approximately 1second.

Algorithm
The procedures executed by the coordinator and the vehicles when the converged intersection controller is used are summarized in Algorithms 1 and 2. Before the SQP is solved the first time, the central node requests the vehicles' noncoordinated optimal time-slot T i which serves as the primal initial guess. In subsequent solutions of the SQP, the previous optimal solution is used instead. The solution is considered found when either ||(∇(z), h − (T))|| ∞ < or ||(ΔT, h − (T))|| ∞ < . On the vehicle side, the MPC problem (41), is solved every t s using the most recently commanded time-slot T * i . When a request from the central node is registered, the vehicles evaluate and send the functions and derivatives needed to solve the SQP.
where the dependence on the initial statex i,0 has been dropped for notational simplicity 1: Send coordination start state x start i and start time t start .  Estimate current state x i,0 , Get synchronized time t 3: if Central node sends new time-slot to apply then 4: Receive T * i 5: Apply optimal u i,0 to vehicle 9: if Central node request  i (T i ) then 10: Receive T i , t K , r if r == 0 then 13: Store x i,K from prediction at t K − t computed on Line˜7 14: Solve LPs for L in (x i,K ) and U in (x i,K )

Implementation restrictions and practical considerations
Due to implementation related details, the algorithm was executed at a sampling time of t s = 0.1 s. This restricted the communication of new information from both the central node and the vehicles to occur at a maximum of 10 Hz. With an ideal communication system, one iteration of the SQP (with (c) = 1) therefore requires t s s to broadcast T (line 11 of Algorithm 1) and t s s for the vehicles to respond with  i ( i (T i )) (line 12 of Algorithm 1), that is, the lowest time required per SQP iterate is 2t s s. Moreover, for the converged intersection controller, each execution of the SQP commences t O s s before the resulting time-slots should be applied to the vehicles, which gives the algorithm t O s s to converge and notify the vehicles of the results. For simplicity, the period of the intersection-level control loop is set to t O s s, which, due to the long expected solve times, is set to t O s = 3 s. Finally, to ensure that all vehicles are predicted to be before the intersection when the new time-slots are applied, the SQP is only solved when no vehicle is close to the intersection. In particular, for scenarios where the desired speed is v ref = 50km∕h, the SQP is suspended when the first vehicle is 50 m away from the intersection. We emphasize that this modification is done for simplicity of implementation and that the problem formulation allows the SQP to be solved with a vehicle to be inside the intersection.

Test platform
The coordination controller was tested at the Asta Zero proving ground outside Gothenburg, Sweden. The test platform consisted of the three different Volvo vehicles shown in Figure 3A: One Volvo S60 T5 Petrol Turbo sedan, one Volvo S60 D5 Turbo Diesel sedan and one Volvo XC90 T6 Petrol Turbo SUV. All cars were equipped with automatic gearboxes and an interface for external control of the longitudinal motion. In particular, all vehicles were commanded by supplying a desired longitudinal acceleration to a controller, which thereafter sent the appropriate commands to the engine, gear-box, and friction brakes. The vehicles had an on-board sensor suite consisting of wheel encoders, inertial measurement units and real-time kinematic GPS receivers. The latter was capable of providing positioning estimates with measurement error Moreover, each vehicle was equipped with ITS G5 compliant V2V communication equipment from RENDITS. 43 On each vehicle, the experiment software ran on two computational units that communicated with each other using UDP over Ethernet: one MicroAutoBox II (MABx) real-time prototyping platform, interfacing with the vehicle, sensors, and communication equipment which ran the algorithm logic and state estimation, and one MacBook Pro with an Intel i7 4770HQ CPU and 16 GB of RAM, on which the QPs (11), (41) and LPs (17), (18) were solved using HPMPC. The hardware setup in one of the vehicles is shown in Figure 3B.

Prediction model, objective, and parameters
The prediction model used during the experiments was a simple double integrator,ṗ i (t) = v i (t),̇v i (t) = u i (t), where the acceleration is the input and x i (t) = (p i (t), v i (t)). For this dynamical system p d where the desired speed v ref i , and the objective function weights Q i > 0 and R i > 0 were varied between different instances of the experiment. The state and control were constrained to v i,k ≥ 0 and u i,k ∈ [−4, 1.6]m∕s 2 , where the latter was due to limitations in the vehicle actuation interfaces. The vehicle-level control loops were closed with t s = 0.1 s and the horizon length was set to N = 200. The objective and prediction model were chosen due to their simplicity. In particular, the dynamics do not include any parameters to identify, and the objective enables an intuitive understanding of how the solution will change with variations to the penalty weights. However, we want to emphasize that these choices are not restrictive and that other linear-quadratic models are possible. For instance, in Reference 22 a prediction model based on a linearization of a nonlinear vehicle model is used together with a quadratic approximation of an economic objective function obtained through the method presented in Reference 44.

RESULTS
In this section, we present and discuss the results from the experimental campaign, which demonstrate the performance of the semi-distributed SQP and both the Converged and 1-step intersection-level controllers. In total, more than 80 experiments were performed, where the initial conditions, objective function weights, and other parameters were varied. In all experiment instances, the vehicles were first controlled to a predefined starting state, typically one where a collision would occur if no action was taken, before the bi-level controller was initialized. The experiments were performed in two different modes: in an actual intersection, as shown in Figure 4A and in a parallel configuration where the approaching roads were laid out next to each other and the intersection was represented by a segment on the road, as shown in Figure 4B. The latter was used to enable evaluation of the controller without risk of collision, and is the primary source of the data reported in this section. However, the interested reader can find video material from experiments performed in the crossing configuration at Reference 45.
For comparison, some simulation results of the closed-loop system are also provided in this section. In these cases, the vehicles were simulated with the nominal model.

Evaluation of the semi-distributed SQP
In this subsection, we present and analyze data from the implementation of the semi-distributed SQP used in the experiments. We focus on an experimental scenario with the objective function weights Q i = 1, R i = 10, the reference speed  Figure 5. In particular, Figure 5A,B show the solutions to the vehicle-level problems corresponding to the iterates T (c) of the SQP. The two horizontal lines in Figure 5A represent the beginning and end of the intersection, that is, collisions are avoided when at most one trajectory is between the lines at all times, and the primal iterates T (c) are shown as vertical lines. The solver is initialized at the uncoordinated solution, where all vehicles keep the constant velocity v ref i and therefore occupy the intersection simultaneously. As can be seen in Figure 5A, the time-slots T satisfy the order constraints h i (T) ≥ 0 already after the first iterate, whereby a collision free solution is available. The subsequent two iterations retain feasibility and improve the solution. Full steps ( = 1) are taken in all iterates.
The algorithm progress measures shown in Figure 5C further illustrate this fact: feasibility is reached after the first iterate (||h − s (T)|| drops below the set tolerance). Note the relatively loose convergence threshold = 10 −3 , which is selected in relation to the properties of the physical system. In particular, the GPS provides measurements with a positioning error standard deviation around GPS = 0.05 m. The standard deviation of the error between the commanded time-slot T i and that resulting from the closed-loop application of (11) using T i will therefore be above one millisecond ‡ for speeds around 50 km∕h. Enforcing constraint satisfaction or changes to the primal variables below 10 −3 [s] will consequently have no noticeable effect on the physical system.
The small impact of small changes in T i on the vehicle trajectories is further shown in Figure 5A,B, where the solutions of (11) corresponding to iteration 2 and 3 are indistinguishable. In fact, the difference inū i,0 between iteration 2 and 3 is below the resolution of the actuation interface, that is, the applied control commandū i,0 after iteration 2 and 3 would be interpreted as identical by the vehicle.

Solution time analysis
The mean computation time was 1.664 ms with ≈ 1.263 ms (n = 52736) for the LPs, and 1.607 ms with ≈ 1.107 ms (n = 246132) for the vehicle-level QPs. Since the SQP normally only required a few iterations to converge to a relevant threshold, a solution to (7) should have been found within a few hundredths of a second. However, due to a rudimentary implementation and hardware limitations this was not the case in during the experiments. Instead, the average time required to solve the SQP was 1.740 s with ≈ 0.406 s (n = 130). An example of how this time is spent is provided in Figure 6 which shows a timeline from the SQP instance shown in Figure 5. In Figure 6, the width of the bars represents the time spent solving the LPs (18), (17) and the QP (11) for the vehicles, corresponding to Lines 16 and 18 of Algorithm 2, and the time spent solving the QP-subproblem 23 in the central node, corresponding to Line 9 of Algorithm 1. As the figure illustrates, only 55 ms, corresponding to 1.6% of the total time 2.723 s, is spent in computations. The time required for the other operations relevant for the SQP in Algorithms 1 and 2 is negligible, and the remaining 98.4%, that is, the white gaps of Figure 6A, is primarily spent in the waiting states of Lines 7 and 12 in Algorithm 1. Possible explanations for the long delays are inefficient buffer handling in the communication modules of the MABx and packet drops in the wireless links. To a smaller extent, the delays are due to the low communication frequency used and the lack of synchronization between the vehicles and the central node, discussed in Section 4.3.1. The impact of the slow update rate can be seen in a comparison between the computations of vehicle 1 and 2: in iteration 1, occurring around t = −1.5 s in Figure 6A, vehicle 1 and 2 perform their computations simultaneously. In iterations 0, 2, and 3, on the other hand, there is delay of t s = 0.1s between the vehicles. The explanation is that a message is processed by Algorithm 2 at time (k + 1)t s or kt s depending on when it is received relative to the ticks of the local clock. Small variations in the reception time can therefore cause a variation of t s = 0.1 s in the relative time between the processing of a message in two cars. Moreover, the lack of synchronization can be observed when vehicle 2 and 3 are compared: the computations of vehicle 3 consistently occur around 0.07 s before those of vehicle 2. We want to highlight that the long time required to solve the SQP is almost entirely related to our specific implementation, and performance would improve dramatically with a few modifications of the implementation. For comparison, we provide an example of the timeline for a more efficient implementation of the SQP in Figure 6B. In this case, the long and unnecessary waits have been removed and the algorithm is synchronized between the vehicles, but everything else is kept unchanged. The time marked as required by communication is taken from Reference 42, where an empirical study is presented on the time performance of the communication system used during the experiment. With such an implementation, the same problem instance would be solved in 0.074 s, where 48% would be spent on communication, 38.5% on the solution of the vehicle-level LPs and QP, and 13.5% on solving the QP-subproblem (23).
Finally, it is reasonable to expect even lower solution times with improvements to both the algorithmic implementation and the equipment. For instance, the use of the reformulation of the QP-subproblem and its solution with HPMPC would, as discussed in 4.2, significantly reduce the time required for the central computations. Furthermore, solving the two LPs (18), (17) in parallel on each vehicle would shorten the time required for the vehicle-side computations. Finally, while the general purpose V2V equipment requires around 4 ms for each transmission, a tailored communication protocol could be made significantly faster. For instance, it is reported in Reference 46 that the time to transmit data using the 802.11 p physical layer could be as low as t com = 40 + ceil ((n data bits + 22)∕48) 8 μs. Since all vehicles could use different channels and transmit their data to the central node simultaneously, sending 12 floats per vehicle in double precision would thereby take 176 s. The time-line for a solution of the same SQP instance in this hypothetical setting is shown in Figure 6C, where the solution would be found in 0.0256 s.

Consistency
The algorithm consistently exhibited the same behavior as in Figure 5, with almost immediate feasibility followed by a few optimality-improving iterations. The algorithm progress measures of a selection of the SQP instances are provided in  Figure 7 for illustration. In particular, the convergence to was achieved in two iterations in 7% of the instances, in three iterations in 50%, in four iterations in 28%, and in five iterations in 15%. Finally, we have noticed that reduced steps ( < 1) are required only in "hard" scenarios. Examples of "hard" scenarios include those where the coordination is initiated very close to the intersection for the initial vehicle velocity, and those where a large number of vehicles need to cross the intersection simultaneously. For practical reasons we were not able to perform sufficiently hard scenarios during the experimental campaign, due to which full steps were taken in all experimental SQP instances. For illustration purposes, the progress measures of a three-vehicle scenario where reduced steps were taken is given in Figure 7C. This scenario was particularly hard, since p 1,0 = −45 m, v i,0 = 45 km∕h while p 2,0 = p 3,0 = −40 m, v 2,0 = v 3,0 = 50 km∕h, such that the first and last vehicles were forced to perform very aggressive maneuvers to avoid collision.

A larger example
To further demonstrate the behavior of the semi-distributed SQP, we present simulated results from a larger problem instance in Figure 8. In this scenario, 12 vehicles are randomly generated at distances between 50 and 200 m from the intersection at 50 km∕h. As Figure 8B illustrates, the algorithm exhibits the same behavior as in the smaller scenarios: feasibility, and thereby collision avoidance is reached rapidly, in this case after the second iterate, and thereafter small adjustments toward optimality are performed. While the implementation of the SQP used in the experiments would require prohibitively long time to solve the problem, it would be solved in 0.224 s with the improved implementation of Figure 6B, and attain feasibility in 0.037 s. Moreover, the problem would be solved in 0.089 s with the ideal implementation discussed in Section 5.1.1, where a feasible solution would be available in 0.015 s.

Evaluation and comparison of controllers
In this subsection, we present and analyze both simulated and experimental data on the performance of the bi-level closed-loop controller. In particular, we provide comparisons between the converged and 1-step formulations of the intersection-level controller and study their ability to reject large perturbations.

Vehicle-level control loop
Regardless of how the time-slots T are computed, the accuracy with which the vehicles conform to the time-slots determines whether or not the closed-loop system is collision free. Even though a simple dynamic model was used in the vehicle-level MPC during the experiment and several nonmodeled nonlinearities were present, the ability of the closed-loop system to satisfy the position constraints (41e) and (41f) was remarkable. In particular, the difference between the commanded (T i ) and actual (T Actual  Figure 9A, and we emphasize that for more than 90% of the potentially dangerous constraint violations, the errors were below 0.03 s. To illustrate how this small error translates to collision risks, the corresponding trajectories in the time-position space are given in Figure 9B,C for t in i and t out i , respectively. As shown in Figure 9C, the most dangerous constraint violations occur for the out time constraint (41f), that is, such that the vehicles leaves the intersection too late. Note, however, that the magnitude of violations only are such that a few decimeters of the leaving vehicle remain inside the intersection when the following vehicle enters. That violations are larger for the out time constraints is likely due to the (unmodeled) actuator dynamics being faster in deceleration than in acceleration (c.f. friction brakes and internal combustion engine). Successful compensation of errors due to measurement noise and prediction model inaccuracies can therefore be made closer to the intersection when these cause the vehicle to enter early rather than leave late. It is expected that all constraint violations could be decreased by using a more sophisticated prediction model in (41), more accurate sensors and a higher update frequency t s . Finally, we note that a constraint tightening approach was proposed in Reference 25, with which collision avoidance can be guaranteed even with potentially dangerous constraint violations.

Intersection-level control loop
To enable comparison, the same experiments were performed using both the converged and 1-step controller. Data from the application of both controllers to one experimental scenario is shown in Figure 10, where the scenario parameters were as in Section 5.1. As the figure illustrates, the difference between the two controllers is very small: the acceleration and velocity profiles in Figure 10B show a high degree of similarity, and the position trajectories in Figure 10A are almost indistinguishable. The differences are most clearly seen in the lower plot of Figure 10C, which shows the changes in t in i compared with the first coordinated solution at t = 0. Here, the smaller but more frequent changes to T by the 1-step controller are clearly differentiated from the less frequent but larger adjustments performed by the converged controller. Note that, while updates are more frequent for the 1-step controller, they are still significantly slower than the vehicle-level update frequency of 10 Hz. The reason is that each adjustment first requires all vehicles to send the relevant information to the central node, which thereafter can solve the QP subproblem (23) and send the updated time-slot back to the vehicles. The process thereby involves the same type of waiting and delays as discussed in Section 5.1.1. Note also that for both controllers, the initial time-slot schedule T is continuously pushed to later times. This is likely caused by inaccuracies in the prediction model which cause the real system to lag slightly behind the predictions. This explanation is consistent with the nature of the constraint violations discussed in Section 5.2.1, in particular those shown in Figure 9C. The use of a more accurate prediction model is expected to positively affect the behavior.
We want to highlight that the magnitude of the input commands and acceleration as well as the resulting changes in velocity are all small. For comparison, it has been shown that human drivers decelerate with down to −1.9 m∕s 2 during intersection approaches without stops (light switching from red to green) and down to −4.5 m∕s 2 for solid red lights. 47 To illustrate the effects of using a simple implementation and deploying the controller on a real system, results from a simulation of both controllers in the same scenario as the experiments are provided in Figure 11. Since one iteration of the SQP requires less than 20 ms in the ideal case, including two-way communication, the 1-step updates are computed with the same frequency as the vehicle-level controllers (0.1 s) in simulation. The differences between the controllers are even smaller in the simulated case, and are again most noticeable in the control output plot of Figure 11C.

Rejection of perturbations
To investigate the ability to counteract large perturbations, experimental scenarios were performed where the drivers of the vehicles overrode vehicle-level controllers for short periods of time by pressing the brake or accelerator pedals. Data from two such experiments where the driver of the first vehicle presses the brake pedal is given in Figure 12. For comparison, Figure 12 also shows the open-loop predictions made on the onset of the perturbation, which gives an indication of what the unperturbed trajectories would look like. Rejection of the perturbations is best handled by both feedback loops, that is, both the action of the vehicles and the time-slot schedule should be adjusted. In particular, when the velocity of the first vehicle is reduced due to the perturbation, the time-slot schedule should be adjusted so that the intersection entry of all vehicles is postponed. Indeed, this is also what occurs in both the Converged and 1-step cases, shown in Figure 12A,B, respectively, where the size of the adjustments in t in i is shown in the lower plots and the perturbation is represented by the gray slab. Note that since the perturbations are introduced manually by the driver, they differ in length and magnitude between Figure 12A,B.
The benefit of the bi-level control structure is made visible in the middle plots of Figure 12A. Here it is clearly shown that the application of a recomputed time-slot at t ≈ 7 reduces the magnitude of the control command of vehicle 1 and increases that of vehicle 2, which effectively distributes the effort required to counter the perturbation among the two vehicles.
A similar behavior, however, smaller in magnitude, can be observed for the 1-step controller in Figure 12B, which causes the jagged behavior in the middle plot. Note that as predicted in Section 3.3, the reaction of the intersection-level control loop is delayed for the converged controller, and the large adjustment to T is not performed until 2.5 s after the perturbation. This is due to the relatively long cycle time of the intersection-level control loop, t O s = 3s, and the use of predicted future states as basis for the SQP, as discussed in Section 3.3.1, which prevents faster reactions.
A perturbed scenario was also simulated to further highlight the benefits of the bi-level control structure and to enable better comparison between the different intersection-level controllers. The result is shown in Figure 13, where we also include the case where the time-slots are not adjusted and a highly idealized, unrealistic controller in which the SQP is solved to convergence every t s = 0.1 s at the current state to serve as benchmarks. Note in particular the trajectories corresponding to the case in which the time-slot schedule is kept constant. In this case, the entire effort of perturbation rejection is placed on the first vehicle, with higher transient accelerations and velocities as a consequence. A similar behavior is observed between the time-slot updates when the SQP is solved to convergence every t O s = 3s. While the effort of rejecting the disturbance is redistributed among the vehicles with recomputed time-slots, large acceleration levels are observed in Vehicle 1 between t = 3 and t = 6.
With the 1-step controller, the transient velocities and accelerations are lower as the effort to counter the perturbation is continuously distributed between the vehicles. As can be seen in Figure 13B, the size of the time-slot adjustments in the 1-step case is very small, and their application results in no rapid changes in the control command. This is due to the higher update frequency, which allows a gradual adjustment of the intersection-level controller to the disturbance. Finally, the difference between the positions resulting from the 1-step controller and the idealized case where the SQP solved to convergence every 0.1 s is shown in the lower plot of Figure 13A. As can be seen, the difference is at most in the order of the accuracy of the positioning system. For most of the time, the difference would not be distinguishable from measurement noise. This is a strong indication that there is no major benefit of solving the SQP to a higher accuracy this fast, further motivating the use of the 1-step controller. We note that the corresponding accumulated difference in t in i is around 50 ms at most, and settles around 1 ms.
Note that the trajectories for the case where the SQP is solved to convergence every t = 0.1 s are drawn in all plots of Figure 13, but are indistinguishable from the trajectories corresponding to the 1-step controller.

DISCUSSION AND CONCLUSION
This article addressed the development and experimental validation of a semi-distributed algorithm for optimal coordination of automated vehicles at intersections. In particular, we described a bi-level MPC, where an outer control layer allocates collision free intersection occupancy time-slots by solving a NLP, and a lower control layer computes the OC commands by solving QPs. We detailed a semi-distributed SQP method used to solve the NLP, and described a practical implementation of the controller, including the integration of the state-of-the art QP solver HPMPC. We demonstrated the applicability of the controller and computational scheme through an extensive experimental campaign. In particular, even though there were substantial imperfections in the implementation, the method was shown to reject large perturbations efficiently and satisfy the collision avoidance constraints to a relevant accuracy. We want to highlight that the issues observed during the experimental campaign were entirely due to implementation deficiencies and design choices and not inherent to the control formulation or algorithm. On the contrary, the performance observed despite the issues is an indication of the scheme's applicability to real scenarios. For instance, while the ITS-G5 standard has six service channels, 48 and therefore has the potential to let six vehicles communicate simultaneously, larger scenarios would require sequential communication with the current technology. The actual communication time would thereby increase and a delay would be induced: in a scenario with, for example, 42 cars, at least seven rounds where six vehicles communicate in parallel would be required. With the equipment used during the experiment, the wait on Line 12 of Algorithm 1 would increase with at least 24 ms. Even though a larger scenario likely requires more SQP iterations to converge, as indicated by the results of Section 5.1.3, the time required to solve the problem would still be much smaller than that observed in the experiments, and equal or better performance could be expected. It should also be noted that, even in a nonideal communication environment where some vehicles possibly need to resend their data due to packet drops, the added delay is small compared with the one observed during the experiments and would likely not affect the controller performance significantly.
We also want to emphasize the complete parallelizability of the vehicle-level QPs and LPs. Due to this, the time-per-iterate in the SQP will practically be independent of the number of vehicles, and the computational time decided solely by the number of iterates, use of reduced steps, and the time required to solve the QP subproblems. In terms of computation, the algorithm therefore scales well with an increased number of vehicles.
We should also point out that the central part of the algorithm could be performed at a physically remote location, for example, in the "cloud." In this case, the coordinating unit would only be required to provide an access point to the V2V network. Moreover, the ability to function with rather large delays could also motivate the use of cellular communication rather than direct radio-links. With a cellular communication solution, no dedicated intersection infrastructure would be needed at all.
As evidenced by both experimental and simulated results, the bi-level controller successfully managed to handle both large and small perturbations. In particular, by closing the outer control loop, we showed that the controller distributed the effort of rejecting perturbations among all involved vehicles. With such a system, the actuation capacities of all involved vehicles can be used to prevent a collision, should it be necessary, which is an important safety feature. Moreover, the 1-step variation of the intersection-level controller was shown to have comparable performance. Indeed, as indicated by the simulation results, the difference between solving the SQP to full convergence and applying the 1-step scheme are on a scale which makes it irrelevant to the application. As the 1-step scheme requires significantly less communication, faster feedback is thereby enabled in the intersection control loop. While this indicates that the 1-step controller is superior to the Converged controller in the tested cases, further studies are required before general conclusions can be drawn.

Future Work
We aim to generalize the method to include scenarios with more than one oncoming lane per road and explicitly account for rear-end collisions between vehicles on the same lane. While rear-end collision avoidance constraints are easy to formulate and include in the centralized setting of Problem (6), they create additional couplings between the vehicles and complicates the application of the decomposition method used in this article. We are currently working on schemes which allows both rear-end collision avoidance constraints and distributed computation.