A Semi-Distributed Interior Point Algorithm for Optimal Coordination of Automated Vehicles at Intersections

Abstract

In this paper, we consider the optimal coordination of automated vehicles at intersections under fixed crossing orders. We formulate the problem using direct optimal control and exploit the structure to construct a semi-distributed primal-dual interior-point algorithm to solve it by parallelizing most of the computations. Differently from standard distributed optimization algorithms, where the optimization problem is split, in our approach we split the linear algebra steps, such that the algorithm takes the same steps as a fully centralized one, while still performing computations in a distributed fashion. We analyze the communication requirements of the algorithm, and propose an approximation scheme which can significantly reduce the data exchange. We demonstrate the effectiveness of the algorithm in hard but realistic scenarios, which show that the approximation leads to reductions in communicated data of almost 99% of the exact formulation, at the expense of less than 1% suboptimality.

Assumption

Assumption 1

G8BI

There are no non-cooperative entities.

Assumption

Assumption 2

GPHW

The vehicles do not change lanes.

Remark

Remark 1

AM4C

We present Problem (?) without turning vehicles for simplicity, but the same formalism can be deployed also in that case, as shown in [Hult2019a_kappa]. Additionally, both (?) and (?) can also be defined with state-dependent safety margins. Such details are omitted for the sake of simplicity.

Remark

Remark 2

W502

In order to solve the nonconvex NLP using derivative-based approaches, we assume that all functions in (?) are continuously differentiable. Twice continuous differentiability is usually assumed for simplicity but is not strictly necessary.

Proposition

Proposition 1

1P2D

The KKT system (?) can be solved as the following sequence of equations

\begin{align} &\hspace{-2pt}\left (\hspace{-1pt}\bar M^\mathrm{C}-\bar M^\mathrm{LC}\hspace{-2pt} \left (\bar M^\mathrm{L}\right )^{-1} \hspace{-2pt} \bar M^\mathrm{CL}\hspace{-1pt}\right )\hspace{-1pt} \Delta z^\mathrm{C} = -\bar r^\mathrm{C}+\bar M^\mathrm{LC}\hspace{-2pt}\left (\bar M^\mathrm{L}\right )^{-1} \hspace{-2pt}\bar r^\mathrm{L}, \hspace{-10em}&&\hspace{10em} \\ &\bar M^\mathrm{L}_l \Delta z^\mathrm{L}_l = -\bar r^\mathrm{L}_l - \bar M^\mathrm{LC}_l\Delta z^\mathrm{C}, && \forall \, j \in \mathcal{I}^\mathrm{L} , \\ &M^\mathrm{v}_{i} \Delta z^\mathrm{v}_{i} = -r^\mathrm{v}_{i} - \begin{bmatrix} M^\mathrm{vL}_i & M^\mathrm{vC}_i \end{bmatrix} \begin{bmatrix} \Delta z^\mathrm{L} \\ \Delta z^\mathrm{C} \end{bmatrix},&& \forall i \,\in \mathcal{I}^\mathrm{v}, \end{align}

where we define

\begin{align} \bar M^\mathrm{L} &:= \mathrm{blockdiag}\left (\bar M^\mathrm{L}_{1}, \hdots, \bar M^\mathrm{L}_{L}\right ), \\ \bar M^\mathrm{L}_l &:= M^\mathrm{L}_l - \sum_{i\in\mathcal{I}^\mathrm{v}_l}M^\mathrm{Lv}_{l,i} \left (M^\mathrm{v}_i\right )^{-1} M^\mathrm{vL}_{i,l}, \\ \bar M^\mathrm{LC} &:= \begin{bmatrix} \bar M^\mathrm{LC}_1,\ldots,\bar M^\mathrm{LC}_L \end{bmatrix},\\ \bar M^\mathrm{LC}_l &:= - \sum_{i\in\mathcal{I}^\mathrm{v}_l} M^\mathrm{Lv}_{l,i} \left (M^\mathrm{v}_i\right )^{-1}M^\mathrm{vC}_{i},\\ \bar M^\mathrm{C} &:= M^\mathrm{C} - \sum_{i=1}^N M^\mathrm{Cv}_i \left (M^\mathrm{v}_i\right )^{-1}M^\mathrm{vC}_i, \\ \bar r^\mathrm{C} &:= r^\mathrm{C} - \sum_{i=1}^N M^\mathrm{Cv}_{i} \left (M^\mathrm{v}_i\right )^{-1} r^\mathrm{v}_i, \\ \bar r^\mathrm{L} &:= (\bar r^\mathrm{L}_1,\ldots,\bar r^\mathrm{L}_L), \\ \bar r^\mathrm{L}_l &:= r^\mathrm{L}_l - \sum_{i\in\mathcal{I}^\mathrm{v}_l} M^\mathrm{Lv}_{l,i}\left (M^\mathrm{v}_i\right )^{-1} r^\mathrm{v}_i, \end{align} and \begin{align} \bar M^\mathrm{LC} \left (\bar M^\mathrm{L}\right )^{-1} \bar M^\mathrm{CL} &:= \sum_{l=1}^L \bar M^\mathrm{LC}_l \left (\bar M^\mathrm{L}_l\right )^{-1} \bar M^\mathrm{CL}_l, \\ \bar M^\mathrm{LC}\left (\bar M^\mathrm{L}\right )^{-1}\bar r^\mathrm{L} &:= \sum_{l=1}^L \bar M^\mathrm{LC} \left (\bar M^\mathrm{L}_l\right )^{-1}\bar r^\mathrm{L}_l. \end{align}

Proof of Proposition 1

We proceed by first solving the KKT system with respect to \Delta z^\mathrm{v}, which yields

\begin{aligned} M^\mathrm{v} \Delta z^\mathrm{v} = -r^\mathrm{v} - \begin{bmatrix} M^\mathrm{vL} & M^\mathrm{vC} \end{bmatrix} \begin{bmatrix} \Delta z^\mathrm{L} \\ \Delta z^\mathrm{C} \end{bmatrix}. \end{aligned}

Since M^\mathrm{v} is block-diagonal, we further split this equation per vehicle to obtain (1). Since these equations require knowledge of \Delta z^\mathrm{L}, \Delta z^\mathrm{C}, they will have to be solved last.

We now use (1) to replace

\begin{aligned} \Delta z_i^\mathrm{v} = -\hbox{$M_i^\mathrm{v}$}^{^{-1}}r^\mathrm{v}_{i} - \hbox{$M_i^\mathrm{v}$}^{^{-1}} \begin{bmatrix} M^\mathrm{vL}_i & M^\mathrm{vC}_i \end{bmatrix} \begin{bmatrix} \Delta z^\mathrm{L} \\ \Delta z^\mathrm{C} \end{bmatrix} \end{aligned}

in the remaining equations. The first term in the right hand side of (?) yields (1)-(1). The second term in the right hand side of (?) yields (1)-(1). In equations (1) and (1) the sum is restricted to i\in\mathcal{I}_l^\mathrm{v} since M_{i,l}^\mathrm{vL}=0 for all i\notin\mathcal{I}_l^\mathrm{v}.

We can now write (?) as

\begin{aligned} \begin{bmatrix} \bar M^\mathrm{L\phantom{C}} & \bar M^\mathrm{LC} \\ \bar M^\mathrm{CL} & \bar M^\mathrm{C\phantom{L}} \end{bmatrix} \begin{bmatrix} \Delta z^\mathrm{L} \\ \Delta z^\mathrm{C} \end{bmatrix} = - \begin{bmatrix} \bar r^\mathrm{L} \\ \bar r^\mathrm{C} \end{bmatrix}. \end{aligned}

We can then obtain (1) by eliminating \Delta z^\mathrm{L} to obtain

\begin{aligned} \bar M^\mathrm{L} \Delta z^\mathrm{L} = -\bar r^\mathrm{L} - \bar M^\mathrm{LC}\Delta z^\mathrm{C}. \end{aligned}

Since by (1) \bar M^\mathrm{L} is block diagonal, we can solve this equation separately for each lane, therefore obtaining (1).

Finally, Equation (1) is obtained by replacing

\begin{aligned} \Delta z^\mathrm{L} = -\hbox{$\bar{M}^\mathrm{L}$}^{^{-1}}\bar r^\mathrm{L} - \hbox{$\bar{M}^\mathrm{L}$}^{^{-1}} \bar M^\mathrm{LC}\Delta z^\mathrm{C} \end{aligned}

in the second row of (?). Note that also in (1)-(1) we exploit the block-diagonal structure of \bar M^\mathrm{L}.