# Learning-to-Rank at the Speed of Sampling: Plackett-Luce Gradient Estimation
With Minimal Computational Complexity

Harrie Oosterhuis  Radboud UniversityNijmegenThe Netherlands
<EMAIL_ADDRESS>
(2022)

###### Abstract.

Plackett-Luce gradient estimation enables the optimization of stochastic
ranking models within feasible time constraints through sampling techniques.
Unfortunately, the computational complexity of existing methods does not scale
well with the length of the rankings, i.e. the ranking cutoff, nor with the
item collection size.

In this paper, we introduce the novel PL-Rank-3 algorithm that performs
unbiased gradient estimation with a computational complexity comparable to the
best sorting algorithms. As a result, our novel learning-to-rank method is
applicable in any scenario where standard sorting is feasible in reasonable
time. Our experimental results indicate large gains in the time required for
optimization, without any loss in performance. For the field, our contribution
could potentially allow state-of-the-art learning-to-rank methods to be
applied to much larger scales than previously feasible.

Learning to Rank; Policy Gradients; Computational Complexity

††journalyear: 2022††copyright: rightsretained††conference: Proceedings of the
45th International ACM SIGIR Conference on Research and Development in
Information Retrieval; July 11–15, 2022; Madrid, Spain.††booktitle:
Proceedings of the 45th Int’l ACM SIGIR Conference on Research and Development
in Information Retrieval (SIGIR ’22), July 11–15, 2022, Madrid, Spain††doi:
10.1145/3477495.3531842††isbn: 978-1-4503-8732-3/22/07††ccs: Information
systems Learning to rank††submissionid: 1493

##  1\. Introduction

Learning-to-Rank (LTR) methods optimize ranking systems for search and
recommendation purposes (Liu, 2009). In the field of Information Retrieval
(IR), they are generally deployed to maximize well-known ranking metrics such
as: Discounted Cumulative Gain (DCG) or precision (Järvelin and Kekäläinen,
2002). The main difficulty with the LTR task is that ranking metrics are non-
differentiable, discrete and very non-smooth due to the underlying sorting
process (Burges, 2010). In broad terms, the existing solutions to this problem
can be divided into two categories: heuristic bounds or approximations of
ranking metrics or their gradients (Fuhr, 1989; Joachims, 2002; Taylor et al.,
2008; Bruch et al., 2019; Wang et al., 2018; Burges, 2010); and optimizing a
probabilistic ranking model instead of a deterministic model (Xia et al.,
2008; Cao et al., 2007; Oosterhuis, 2021; Ustimenko and Prokhorenkova, 2020).
The latter category does not have issues with differentiability or smoothness
because their methods optimize a probabilistic distribution over rankings.
Recently, probabilistic ranking models have also received additional attention
for their applicability to ranking fairness tasks (Diaz et al., 2020). Since
probabilistic models are able to divide exposure over items more fairly than
deterministic models.

Oosterhuis (2021) introduced the PL-Rank method to efficiently optimize
Plackett-Luce (PL) ranking models: a specific type of ranking model based on
decision theory (Plackett, 1975; Luce, 2012). They use Gumbel sampling
(Gumbel, 1954; Bruch et al., 2020) to quickly sample many rankings from a PL
ranking model, and subsequently, apply the PL-Rank-1 or PL-Rank-2 algorithm to
these samples to unbiasedly approximate the gradient of a ranking metric
w.r.t. the model. While PL-Rank provides a significant contribution to the LTR
field, we recognize two shortcomings in the work of Oosterhuis (2021): neither
PL-Rank-1 or PL-Rank-2 scale well with long rankings; and they do not compare
PL-Rank to the earlier and comparable StochasticRank algorithm (Ustimenko and
Prokhorenkova, 2020).

This paper addresses both of these shortcomings: our main contribution is the
novel PL-Rank-3 algorithm that computes the same approximation as PL-Rank-2
while minimizing its computational complexity. PL-Rank-3 has computional costs
in the same order as the best sorting algorithms; we posit that this is the
lowest order possible for a metric-based LTR method. Our experimental
comparison includes both the previous PL-Rank-2 and StochasticRank as
baselines, providing the first comparison between these two methods. The
introduction of PL-Rank-3 is exciting for the LTR field, as it pushes the
limit of the minimal computational complexity that LTR methods can have.
Potentially, it may enable future LTR to be applied to much larger scales than
currently feasible.

Table 1.  Overview of LTR methods in terms of their relevant theoretical properties: (i) Reliance on an iteration over item pairs; (ii) Directed by the model’s full-ranking behavior; (iii) Directed by metric to be optimized; (iv) Usage of sample-based approximation; (v) Applicability to general rank-based exposure metrics (e.g. for ranking fairness); (vi) Computational complexity w.r.t. number of items $D$ and length of ranking $K$. The properties of a method are indicated by ✓or ? when it is open to interpretation and $D^{2*}$ indicates the number of item-pairs with unequal relevance labels.  method name | pairwise | ranking- | metric- | sample | rank-based | computational | notes
---|---|---|---|---|---|---|---
based | based | approximation | exposure | complexity
Pointwise (Fuhr, 1989) |  |  |  |  |  | $D$ | not an LTR loss
SoftMax Cross-Entropy (Qin et al., 2021) |  |  |  |  |  | $D$ |
Pairwise (Joachims, 2002) | ✓ |  |  |  |  |  $D^{2}$* | memory efficient
Listwise/ListMLE (Xia et al., 2008; Cao et al., 2007) |  | ✓ |  |  |  | $DK$ |
SoftRank (Taylor et al., 2008) | ✓ | ? | ✓ |  |  | $D^{3}$ |
ApproxNDCG (Bruch et al., 2019) | ✓ | ? | ✓ |  |  | $D^{2}$ | proven bound
LambdaRank/Loss (Wang et al., 2018; Burges, 2010) | ✓ | ✓ | ✓ |  |  | $D^{2*}+D\log(D)$ | proven bound
StochasticRank (Ustimenko and Prokhorenkova, 2020) | ? | ✓ | ✓ | ✓ | ✓ | $DK$ | policy-gradient
PL-Rank-1/2 (Oosterhuis, 2021) |  | ✓ | ✓ | ✓ | ✓ | $DK$ | policy-gradient
PL-Rank-3 (ours) |  | ✓ | ✓ | ✓ | ✓ | $D+K\log(D)$ | policy-gradient

##  2\. Background: PL-Rank-2

Let $\pi$ indicate a PL ranking model based on a scoring function $f$ with
$f(d)$ indicating the score for item $d$; the probability of sampling ranking
$y$ from item set $D$ from $\pi$ is then:

$\pi(y)=\prod\pi(d|\,y_{1:k-1},D),\,\pi(d|\,y_{1:k-1},D)=\frac{e^{f(d)}\mathds{1}[d\not\in
y_{1:k-1}]}{\sum_{d^{\prime}\in D\setminus y_{1:k-1}}e^{f(d^{\prime})}},$

where $y_{1:k-1}$ indicates the ranking from rank $1$ up to $k-1$. In other
words, the probability of placing $d$ at rank $k$ is $e^{f(d)}$ divided by the
$e^{f(d^{\prime})}$ of all unplaced items (similar to a SoftMax activation
function), unless $d$ was already placed at an earlier rank then it has a zero
probability. The probability of the entire ranking $y$ is simply the product
of all its individual item placements.

PL-Rank optimizes $f$ so that the metric value of sampled rankings are
maximized in expectation (Oosterhuis, 2021). PL-Rank assumes ranking metrics
can be decomposed into weights per rank $\theta_{k}$ and relevance of an item
$\rho_{q,d}$ (Moffat et al., 2017). Its objective is thus to maximize the
expected metric value over its sampling procedure, for a single query $q$:

$\mathcal{R}(q)=\sum_{y}\pi(y)\sum_{d\in
D_{q}}\theta_{\text{rank(f,q,d)}}\rho_{q,d}=\mathbb{E}_{y}\bigg{[}\sum_{d\in
D_{q}}\theta_{\text{rank(f,q,d)}}\rho_{q,d}\bigg{]}.$

The rank weights can be chosen to match well-known ranking metrics, e.g. DCG:
$\theta_{k}^{\text{DCG@K}}=\mathds{1}[k\leq K]/\log_{2}(1+k)$ (Järvelin and
Kekäläinen, 2002); or precision: $\theta_{k}^{\text{prec}}=\mathds{1}[k\leq
K]$. To keep our notation brief, we will omit $q$ when denoting the relevances
$\rho_{q,d}=\rho_{d}$. Oosterhuis (2021) based the PL-Rank-2 method on the
following formulation of the policy gradient:

(1) $\begin{split}\frac{\delta\mathcal{R}(q)}{\delta
f(d)}&=\mathbb{E}_{y}\Bigg{[}\Bigg{(}\sum_{k=\text{rank}(d,y)+1}^{K}\theta_{k}\rho_{y_{k}}\Bigg{)}\\\
&\quad\,\,\,\,\,\,\;\;+\sum_{k=1}^{\text{rank}(d,y)}\pi(d\mid
y_{1:k-1})\left(\theta_{k}\rho_{d}-\sum_{x=k}^{K}\theta_{x}\rho_{y_{x}}\right)\Bigg{]}.\end{split}$

The PL-Rank-2 algorithm (Oosterhuis, 2021) can approximate the gradient based
on $N$ sampled rankings for $D$ items and a ranking length of $K$ with a
computational complexity of: $\mathcal{O}(NDK)$. Table 1 compares the
theoretical properties of PL-Rank with other LTR methods. Importantly, PL-Rank
is the only method that is not a pairwise method, while also being based on
the actual metric that is optimized.111We define a pairwise method as any
method that uses an iteration over item pairs in their algorithm. Thus under
our definition methods like ApproxNDCG – that approximates the ranks of items
– and SoftRank – that approximates ranking via a distribution over ranks per
item – are considered pairwise methods because they iterate over all possible
item pairs to compute their approximations and gradients. Furthermore, it
shares most properties with StochasticRank: an alternative method for
approximating the policy gradient.

Algorithm 1 PL-Rank-3 Gradient Estimation

1: Input: items: $\mathcal{D}$; Relevances: $\rho$; Metric weights: $\theta$;
Input Score function: $f$; Number of samples: $N$.

2: $\\{y^{(1)},y^{(2)},\dots,y^{(N)}\\}\leftarrow\text{Gumbel\\_Sample}(N,m)$

3: $\text{Grad}\leftarrow\mathbf{0}$ // initialize zero gradient per item

4: $S\leftarrow\sum_{d\in\mathcal{D}}e^{f(d)}$ // initialize PL denominator

5: for $i\in[1,2,\ldots,N]$ do

6: $S^{\prime}\leftarrow S$ // copy initial PL denominator

7: $PR_{K+1}\leftarrow 0$ // zero value for easy computation

8: for $k\in[K,K-1,\ldots,1]$ do

9: $PR_{k}\leftarrow PR_{k+1}+\theta_{k}\rho_{y_{k}^{(i)}}$ // pre-compute all
placement rewards

10: $(RI_{0},DR_{0})\leftarrow(0,0)$ // zero values for easy computation

11: for $k\in[1,2,\ldots,K]$ do

12: $RI_{k}\leftarrow RI_{k-1}+PR_{k}/S$ // pre-compute all risks

13: $DR_{k}\leftarrow DR_{k-1}+\theta_{k}/S$ // pre-compute all direct rewards

14: $S^{\prime}\leftarrow S^{\prime}-e^{f(y_{k}^{(i)})}$ // renormalize
denominator

15: for $d\in\mathcal{D}$ do

16: $r\leftarrow\min(\text{rank}(d,y),K)$ // index of the pre-computed values

17:
$\text{Grad}(d)\leftarrow\text{Grad}(d)+\frac{1}{N}(PR_{r+1}+e^{f(d)}(\rho_{d}DR_{r}-RI_{r}))$

18: return Grad

Yahoo! Webscope-Set1 |  MSLR-Web30k |  Istella
---|---|---
Minutes per Epoch |  |
Cutoff $K$ |  Cutoff $K$ |  Cutoff $K$
Figure 1. Mean number of minutes required to complete one training epoch as $K$ varies on three datasets. Yahoo! Webscope-Set1 |  MSLR-Web30k |  Istella
---|---|---
_$K=5$_ NDCG@5 |  |
_$K=10$_ NDCG@10 |  |
_$K=25$_ NDCG@25 |  |
_$K=50$_ NDCG@50 |  |
_$K=100$_ NDCG@100 |  |
Minutes Trained |  Minutes Trained |  Minutes Trained
Figure 2. NDCG@K performance of PL-Rank-2, PL-Rank-3 and StochasticRank with $N=100$ and $N=1000$ when trained up to 200 minutes and for various $K$ values evaluated on the test-set of three datasets. All displayed results are averages over 30 independent runs. NDCG was normalized on dataset-level instead of query-level (Ferrante et al., 2021).  Table 2. Mean number of minutes taken to complete one epoch with various methods, ranking length/cutoff $K$ and number of samples $N$ on three datasets. Bold numbers indicate the minimal time per $N$ and $K$ values on each dataset.  | method | $N$ |  $K=5$ |  $K=10$ |  $K=25$ |  $K=50$ |  $K=100$
---|---|---|---|---|---|---|---
Yahoo! |  Stoc.Rank |  1100 | 0.52 | 0.80 | 1.64 | 2.58 | 3.01
1000 | 3.38 | 6.18 | 15.41 | 26.17 | 31.03
PL-Rank-2 |  1100 | 0.43 | 0.58 | 0.96 | 1.35 | 1.50
1000 | 2.41 | 4.13 | 8.96 | 13.30 | 15.19
PL-Rank-3 |  1100 | 0.33 | 0.34 | 0.38 | 0.40 | 0.40
1000 | 1.33 | 1.53 | 1.92 | 2.18 | 2.19
MSLR |  Stoc.Rank |  1100 | 1.55 | 2.53 | 6.01 | 13.23 | 31.12
1000 | 14.15 | 25.46 | 65.25 | 150.00 | 353.01
PL-Rank-2 |  1100 | 1.19 | 1.75 | 4.04 | 8.36 | 15.71
1000 | 10.34 | 18.91 | 46.27 | 94.81 | 185.42
PL-Rank-3 |  1100 | 0.74 | 0.77 | 0.86 | 1.00 | 1.20
1000 | 4.77 | 5.09 | 5.93 | 7.31 | 9.37
Istella |  Stoc.Rank |  1100 | 2.79 | 4.61 | 10.75 | 22.25 | 51.94
1000 | 27.34 | 49.07 | 116.73 | 240.40 | 531.09
PL-Rank-2 |  1100 | 1.96 | 2.87 | 6.72 | 13.25 | 27.07
1000 | 18.49 | 30.37 | 74.09 | 142.55 | 278.01
PL-Rank-3 |  1100 | 1.24 | 1.26 | 1.34 | 1.46 | 1.72
1000 | 9.93 | 10.14 | 10.87 | 12.20 | 14.75

##  3\. Method: A Faster PL-Rank Algorithm

We will now introduce PL-Rank-3: an algorithm for computing the same
approximation as PL-Rank-2 but with a significantly better computational
complexity. Algorithm 1 displays PL-Rank-3 in pseudo-code, we will now show
that it produces the same approximation as PL-Rank-2 (cf. Algorithm 1 in
(Oosterhuis, 2021)).

To start, we define $PR_{y,i}$ as the placement reward, the reward following
the item placed at rank $i$ in ranking $y$:

(2) $PR_{y,i}=\sum_{k=i}^{\min(i,K)}\theta_{k}\rho_{y_{k}},\quad
PR_{y,d}=PR_{y,rank(d,y)+1}.$

Importantly, all $PR_{y,i}$ values for a ranking can be computed in $K$ steps
(Line 9). Next, we point out that a summation over item placement
probabilities similar to Eq. 1 can be formulated as:

(3) $\sum_{k=1}^{\text{rank}(d,y)}\pi(d\mid
y_{1:k-1})=e^{f(d)}\left(\sum_{k=1}^{\text{rank}(d,y)}\frac{1}{\sum_{d^{\prime}\in
D\setminus y_{1:k-1}}e^{f(d^{\prime})}}\right).$

Using this insight, we define $RI_{y,i}$ to later compute the _risk_ imposed
by items with a non-zero placement probability at rank $i$ in $y$:

(4) $RI_{y,i}=\sum_{k=1}^{\min(i,K)}\frac{PR_{y,k}}{\sum_{d^{\prime}\in
D\setminus y_{1:k-1}}e^{f(d^{\prime})}},\quad
RI_{y,d}=RI_{y,\text{rank}(d,y)}.$

Similarly, we define $DR_{y,i}$ to compute the expected direct reward:

(5) $DR_{y,i}=\sum_{k=1}^{\min(i,K)}\frac{\theta_{k}}{\sum_{d^{\prime}\in
D\setminus y_{1:k-1}}e^{f(d^{\prime})}},\;DR_{y,d}=DR_{y,\text{rank}(d,y)}.$

Crucial is that all $RI_{y,i}$ and $DR_{y,i}$ values can also be computed in
$K$ steps (Line 12 & 13). With these newly defined variables, we can
reformulate Eq. 1 without any summation over $K$:

(6) $\frac{\delta\mathcal{R}(q)}{\delta
f(d)}=\mathbb{E}_{y}\left[PR_{y,d}+e^{f(d)}\big{(}\rho_{d}DR_{y,d}-RI_{y,d}\big{)}\right].$

Accordingly, Algorithm 1 approximates the gradient using:

(7) $\begin{split}\frac{\delta\mathcal{R}(q)}{\delta
f(d)}\approx&\frac{1}{N}\sum_{i=1}^{N}\left(PR_{y^{(i)},d}+e^{f(d)}\left(\rho_{d}DR_{y^{(i)},d}-RI_{y^{(i)},d}\right)\right).\end{split}$

PL-Rank-3 computes the $PR$, $RI$ and $DR$ values in $K$ steps and then reuses
them for each of the $D$ items, resulting in a computational complexity of
$\mathcal{O}(N(D+K))$ given $N$ sampled rankings. However, when we consider
that sampling a ranking relies on (partial) sorting, the full complexity of
applying PL-Rank-3 becomes $\mathcal{O}(N(D+K\log(D))$.

Table 1 reveals that - to the best of our knowledge - PL-Rank-3 has the best
computational complexity of all metric-based LTR methods. Moreover, because
its computational complexity is limited by the underlying sorting procedure,
we posit that PL-Rank-3 has reached the minimum order of computational
complexity that is possible for a LTR method that is based on the full-ranking
behavior of the model it optimizes.

##  4\. Experimental Setup

We experimentally evaluate how the improvements in computational complexity
translate to improvements in practical costs. Our experimental runs optimize
the $DCG@K$ of neural ranking models on the Yahoo! Webscope-Set1 (Chapelle and
Chang, 2011), MSLR-Web30k (Qin and Liu, 2013) and Istella (Dato et al., 2016)
datasets. The neural models have two-hidden layers of 32 sigmoid activation
nodes, backpropagation via standard gradient descent with a learning rate of
$0.01$ was applied using _Tensorflow_ (Abadi et al., 2016). We compare our PL-
Rank-3 algorithm, with PL-Rank-2 (Oosterhuis, 2021) and StochasticRank
(Ustimenko and Prokhorenkova, 2020). StochasticRank was chosen as a baseline
because it shares many properties with PL-Rank (see Table 1) yet was not
compared with PL-Rank in previous work (Oosterhuis, 2021). Our StochasticRank
implementation uses the Gumbel distribution as stochastic noise instead of the
normal distribution of the original algorithm (Ustimenko and Prokhorenkova,
2020), this makes the method applicable and effective to optimize a PL model.
We reimplemented the PL-Rank and StochasticRank algorithms in _Numpy_ (Harris
et al., 2020) and performed all our experiments on AMD EPYC™ 7H12
CPUs.222https://www.amd.com/en/products/cpu/amd-epyc-7H12 The ranking
length/metric cutoff was varied: $K\in\\{5,10,25,50,100\\}$ and number of
sampled rankings: $N\in\\{100,1000\\}$, to measure their impact on
computational costs. Performance was measured over 200 minutes of training
time, in addition to the average time required to complete one training epoch.
All reported results are averages over 30 independent runs performed under
identical circumstances. We report Normalized $DCG@K$ (NDCG@K) as our ranking
performance metric computed on the held-out test-set of each dataset;
following the advice of Ferrante et al. (2021), we do not use query-
normalization but dataset-normalization: we divide the $DCG@K$ of a ranking
model by the maximum possible $DCG@K$ value on the entire test-set of the
dataset.

##  5\. Results

Figure 1 and Table 2 shows the effect of ranking length $K$ on the
computational costs of the LTR methods. As expected, Figure 1 reveals that PL-
Rank-2 and StochasticRank are heavily affected by increases in $K$: there
appears to be a clear linear trend on the MSLR and Istella datasets. We note
that many queries in the Yahoo! dataset have less than 100 documents ($D\leq
100$) which could explain why the effect is sub-linear on that dataset. In
contrast, PL-Rank-3 appears barely affected by $K$ on all of the datasets.

Table 2 allows us to also compare the computational costs in more detail.
Regardless of whether $N=100$ or $N=1000$, the required times of PL-Rank-2 and
StochasticRank scale close to linearly with $K$, but those for PL-Rank-3 do
not. For instance, on Istella with $N=100$ and $K=5$, PL-Rank-2 needs 1.96
minutes, StochasticRank needs 2.79 and PL-Rank-3 needs 1.24 minutes, when
compared to $K=100$, PL-Rank-2 needs an additional 25 minutes, StochasticRank
49 minutes more but PL-Rank-3 only requires an increase of 28 seconds.
Moreover, PL-Rank-3 has the lowest computational costs when compared to the
other methods with the same $N$ and $K$ values, across all three datasets. We
thus conclude that in terms of time required to complete a single epoch, PL-
Rank-3 is a clear improvement over PL-Rank-2 and StochasticRank. Additionally,
in terms of scalability of computational costs w.r.t. $K$, PL-Rank-3 is the
best choice by a considerable margin.

Now that we have established that PL-Rank-3 completes training epochs
considerably faster than the other methods, we consider its effect on how
quickly certain performance can be reached. Figure 2 show the performance of
the LTR methods when optimizing $DCG@K$ over 200 minutes for various $K$
values. In all but two scenarios, PL-Rank-3 provides the highest performance
at all times, where it seems to mostly depend on $K$ whether $N=100$ or
$N=1000$ is a better choice. In particular, when $K\in\\{5,10\\}$ PL-Rank-3
with $N=100$ has the highest performance on MSLR and Istella and is only
outperformed by PL-Rank-3 with $N=1000$ on Yahoo! Conversely, when
$K\in\\{50,100\\}$, $N=1000$ is the better choice for PL-Rank-3 on all
datasets. This suggests that gradient estimation for larger values of $K$ is
more prone to variance and thus requires more samples for stable optimization.
Overall, PL-Rank-2 appears very affected by variance across datasets and $K$
values, we mostly attribute this to the small number of epochs it can complete
in 200 minutes. For example, on Istella with $K=100$, PL-Rank-2 completes less
than eight epochs whereas PL-Rank-3 can complete 116 epochs. Interestingly,
StochasticRank with $N=100$ has stable performance that is sometimes
comparable with PL-Rank-3 with $N=1000$ on the Yahoo! dataset. This indicates
that the sample-efficiency of StochasticRank is actually better than PL-
Rank-3, however, due to its low computational costs, PL-Rank-3 still
outperforms it on MSLR and Istella and when $K\in\\{5,10,20\\}$ on Yahoo! We
conclude that PL-Rank-3 provides a clear and substantial improvement over PL-
Rank-2, and in most scenarios also outperforms StochasticRank.

##  6\. Conclusion

We have introduced PL-Rank-3 an LTR algorithm to estimate the gradient of a PL
ranking model with the same computational complexity as the best sorting
algorithms. PL-Rank-3 could enable future metric-based LTR to be applicable to
ranking lengths and item collection sizes of much larger scales than
previously feasible.

### Code and data

To facilitate reproducibility, this work only made use of publicly available
data and our experimental implementation is publicly available at
https://github.com/HarrieO/2022-SIGIR-plackett-luce.

###### Acknowledgements.

This work was partially supported by the Google Research Scholar Program and
made use of the Dutch national e-infrastructure with the support of the SURF
Cooperative using grant no. EINF-1748. All content represents the opinion of
the author, which is not necessarily shared or endorsed by their respective
employers and/or sponsors.

## References

  * (1)
  * Abadi et al. (2016) Martín Abadi, Paul Barham, Jianmin Chen, Zhifeng Chen, Andy Davis, Jeffrey Dean, Matthieu Devin, Sanjay Ghemawat, Geoffrey Irving, Michael Isard, et al. 2016\.  Tensorflow: A System for Large-Scale Machine Learning. In _12th USENIX symposium on operating systems design and implementation OSDI’16)_. 265–283.
  * Bruch et al. (2020) Sebastian Bruch, Shuguang Han, Michael Bendersky, and Marc Najork. 2020.  A Stochastic Treatment of Learning to Rank Scoring Functions. In _Proceedings of the 13th International Conference on Web Search and Data Mining_. 61–69.
  * Bruch et al. (2019) Sebastian Bruch, Masrour Zoghi, Michael Bendersky, and Marc Najork. 2019.  Revisiting Approximate Metric Optimization in the Age of Deep Neural Networks. In _Proceedings of the 42nd International ACM SIGIR Conference on Research and Development in Information Retrieval_. 1241–1244.
  * Burges (2010) Christopher J.C. Burges. 2010\.  _From RankNet to LambdaRank to LambdaMART: An Overview_.  Technical Report MSR-TR-2010-82. Microsoft.
  * Cao et al. (2007) Zhe Cao, Tao Qin, Tie-Yan Liu, Ming-Feng Tsai, and Hang Li. 2007.  Learning to Rank: From Pairwise Approach to Listwise Approach. In _Proceedings of the 24th international conference on Machine learning_. 129–136.
  * Chapelle and Chang (2011) Olivier Chapelle and Yi Chang. 2011.  Yahoo! Learning to Rank Challenge Overview.  _Journal of Machine Learning Research_ 14 (2011), 1–24.
  * Dato et al. (2016) Domenico Dato, Claudio Lucchese, Franco Maria Nardini, Salvatore Orlando, Raffaele Perego, Nicola Tonellotto, and Rossano Venturini. 2016\.  Fast Ranking with Additive Ensembles of Oblivious and Non-Oblivious Regression Trees.  _ACM Transactions on Information Systems (TOIS)_ 35, 2 (2016), Article 15.
  * Diaz et al. (2020) Fernando Diaz, Bhaskar Mitra, Michael D. Ekstrand, Asia J. Biega, and Ben Carterette. 2020\.  Evaluating Stochastic Rankings with Expected Exposure. In _Proceedings of the 29th ACM International Conference on Information & Knowledge Management_. Association for Computing Machinery, New York, NY, USA, 275–284.
  * Ferrante et al. (2021) Marco Ferrante, Nicola Ferro, and Norbert Fuhr. 2021\.  Towards Meaningful Statements in IR Evaluation: Mapping Evaluation Measures to Interval Scales.  _IEEE Access_ 9 (2021), 136182–136216.
  * Fuhr (1989) Norbert Fuhr. 1989\.  Optimum Polynomial Retrieval Functions Based on the Probability Ranking Principle.  _ACM Transactions on Information Systems (TOIS)_ 7, 3 (1989), 183–204.
  * Gumbel (1954) Emil Julius Gumbel. 1954\.  _Statistical Theory of Extreme Values and Some Practical Applications: A Series of Lectures_. Vol. 33.  US Government Printing Office.
  * Harris et al. (2020) Charles R. Harris, K. Jarrod Millman, St’efan J. van der Walt, Ralf Gommers, Pauli Virtanen, David Cournapeau, Eric Wieser, Julian Taylor, Sebastian Berg, Nathaniel J. Smith, Robert Kern, Matti Picus, Stephan Hoyer, Marten H. van Kerkwijk, Matthew Brett, Allan Haldane, Jaime Fern’andez del R’ıo, Mark Wiebe, Pearu Peterson, Pierre G’erard-Marchant, Kevin Sheppard, Tyler Reddy, Warren Weckesser, Hameer Abbasi, Christoph Gohlke, and Travis E. Oliphant. 2020.  Array Programming with NumPy.  _Nature_ 585, 7825 (2020), 357–362.
  * Järvelin and Kekäläinen (2002) Kalervo Järvelin and Jaana Kekäläinen. 2002.  Cumulated Gain-Based Evaluation of IR Techniques.  _ACM Transactions on Information Systems (TOIS)_ 20, 4 (2002), 422–446.
  * Joachims (2002) Thorsten Joachims. 2002\.  Optimizing Search Engines Using Clickthrough Data. In _Proceedings of the eighth ACM SIGKDD international conference on Knowledge discovery and data mining_. ACM, 133–142.
  * Liu (2009) Tie-Yan Liu. 2009\.  Learning to Rank for Information Retrieval.  _Foundations and Trends in Information Retrieval_ 3, 3 (2009), 225–331.
  * Luce (2012) R Duncan Luce. 2012\.  _Individual Choice Behavior: A Theoretical Analysis_.  Courier Corporation.
  * Moffat et al. (2017) Alistair Moffat, Peter Bailey, Falk Scholer, and Paul Thomas. 2017.  Incorporating User Expectations and Behavior into the Measurement of Search Effectiveness.  _ACM Transactions on Information Systems (TOIS)_ 35, 3 (2017), 1–38.
  * Oosterhuis (2021) Harrie Oosterhuis. 2021\.  Computationally Efficient Optimization of Plackett-Luce Ranking Models for Relevance and Fairness. In _Proceedings of the 44th International ACM SIGIR Conference on Research and Development in Information Retrieval_ (Virtual Event, Canada) _(SIGIR ’21)_. ACM, 1023–1032.
  * Plackett (1975) Robin L Plackett. 1975\.  The Analysis of Permutations.  _Journal of the Royal Statistical Society: Series C (Applied Statistics)_ 24, 2 (1975), 193–202.
  * Qin and Liu (2013) Tao Qin and Tie-Yan Liu. 2013.  Introducing LETOR 4.0 datasets.  _arXiv preprint arXiv:1306.2597_ (2013).
  * Qin et al. (2021) Zhen Qin, Le Yan, Honglei Zhuang, Yi Tay, Rama Kumar Pasumarthi, Xuanhui Wang, Mike Bendersky, and Marc Najork. 2021\.  Are Neural Rankers still Outperformed by Gradient Boosted Decision Trees?. In _9th International Conference on Learning Representations, ICLR 2021, Virtual Event, Austria, May 3-7, 2021_. OpenReview.net.  https://openreview.net/forum?id=Ut1vF_q_vC
  * Taylor et al. (2008) Michael Taylor, John Guiver, Stephen Robertson, and Tom Minka. 2008.  Softrank: Optimizing Non-Smooth Rank Metrics. In _Proceedings of the 2008 International Conference on Web Search and Data Mining_. 77–86.
  * Ustimenko and Prokhorenkova (2020) Aleksei Ustimenko and Liudmila Prokhorenkova. 2020.  StochasticRank: Global Optimization of Scale-Free Discrete Functions. In _International Conference on Machine Learning_. PMLR, 9669–9679.
  * Wang et al. (2018) Xuanhui Wang, Cheng Li, Nadav Golbandi, Michael Bendersky, and Marc Najork. 2018.  The LambdaLoss Framework for Ranking Metric Optimization. In _Proceedings of the 27th ACM International Conference on Information and Knowledge Management_. ACM, 1313–1322.
  * Xia et al. (2008) Fen Xia, Tie-Yan Liu, Jue Wang, Wensheng Zhang, and Hang Li. 2008.  Listwise Approach to Learning to Rank: Theory and Algorithm. In _Proceedings of the 25th international conference on Machine learning_. 1192–1199.

  *[LTR]: Learning-to-Rank
  *[IR]: Information Retrieval
  *[DCG]: Discounted Cumulative Gain
  *[PL]: Plackett-Luce


#  Nonconvex Consensus ADMM for Cooperative Lane Change Maneuvers of Connected
Automated Vehicles

Alexander Katriniok  Ford Research & Innovation Center (RIC), Süsterfeldstr.
200,
52072 Aachen, Germany (e-mail: de.alexander.katriniok@ieee.org).

###### Abstract

Connected and automated vehicles (CAVs) offer huge potential to improve the
performance of automated vehicles (AVs) without communication capabilities,
especially in situations when the vehicles (or agents) need to be cooperative
to accomplish their maneuver. Lane change maneuvers in dense traffic, e.g.,
are very challenging for non-connected AVs. To alleviate this problem, we
propose a holistic distributed lane change control scheme for CAVs which
relies on vehicle-to-vehicle communication. The originally centralized optimal
control problem is embedded into a consensus-based Alternating Direction
Method of Multipliers framework to solve it in a distributed receding horizon
fashion. Although agent dynamics render the underlying optimal control problem
nonconvex, we propose a problem reformulation that allows to derive
convergence guarantees. In the distributed setting, every agent needs to solve
a nonlinear program (NLP) locally. To obtain a real-time solution of the local
NLPs, we utilize the optimization engine OpEn which implements the proximal
averaged Newton method for optimal control (PANOC). Simulation results prove
the efficacy and real-time capability of our approach.

###### keywords:

Distributed control and estimation; Model predictive and optimization-based
control; Real time optimization and control; Autonomous Vehicles; Multi-
vehicle systems.

##  1 Introduction

Automated vehicles (AVs) usually take independent decisions which are based
upon sensor measurements and motion predictions of surrounding vehicles.
However, these predictions are often highly uncertain as they rely on
simplified assumptions. This uncertainty may be crucial, especially in
situations when the vehicles (hereafter referred to as agents) need to rely on
these predicted trajectories to accomplish their maneuver. For instance, an AV
might fail to perform a fully automated lane change or lane merging maneuver
when traffic is dense and the target lane is already occupied. When exploiting
vehicle-to-vehicle (V2V) communication, we can alleviate this issue by
transmitting future control actions or state trajectories to other agents, or
even by cooperatively negotiating control actions.

In this paper, we focus on fully automated lane change maneuvers in situations
when the target lane is already occupied. During the last two decades, the
problem of automating lane change maneuvers has intensively been discussed in
literature, see Bevly et al. (2016) for a comprehensive survey. In the recent
years, cooperative control strategies have gained significant attention.
Besides those based on consensus (Wang et al. (2017)) or lane change protocols
(An and Jung (2018)), optimization based concepts are often a favorable choice
as they allow to impose constraints and treat the control problem more
holistically. Centralized schemes, which involve a central node that optimizes
the agents’ control actions, are discussed in Wang et al. (2016) and Hu and
Sun (2019). Decentralized or distributed optimal control schemes, though, may
be preferable as they are more resilient and scalable. Existing distributed
schemes, such as Liu et al. (2017); Blasi et al. (2018), however, introduce
conservatism to decouple the agents or require the other agents’ state space
models to be known.

###  1.1 Main Contribution

We propose a distributed optimal control approach for collaborative, fully
automated lane change maneuvers which adopts the consensus Alternating
Direction Method of Multipliers (ADMM) (Bertsekas and Tsitsiklis (1989); Boyd
et al. (2011)) as methodology to solve the lane change problem in a
distributed receding horizon fashion. To exchange information, the agents rely
on V2V communication. In the considered scenario, the subject agent (SA),
i.e., the agent who intends to change lanes determines two consecutive agents
in the target lane to eventually merge into the gap between them. The maneuver
is then carried out in two steps: 1) the agents increase their headway
distance to allow the SA to change lanes; 2) the SA changes lanes.

Compared to the literature, we aim to solve the originally centralized lane
change problem in a distributed way without introducing additional
conservatism. Particularly, every agent optimizes its local control actions
while consensus with other agents is achieved through the use of a
coordinator, which is run on the SA. As an advantage of our formulation, the
agents’ parameters and their state space models remain private and do not need
to be known by the other agents. This way, we can also reduce load on the
communication channel. Moreover, we holistically account for longitudinal and
lateral vehicle motion instead of assuming the agents to change
instantaneously from one lane to another. Essentially, every agent has to
solve a nonconvex nonlinear program (NLP) while the coordinator problem is a
standard quadratic program (QP). Although the local NLPs are nonconvex, we
propose a problem reformulation that allows to guarantee convergence of the
nonconvex consensus ADMM problem. We run the algorithm in real-time by
adopting OpEn (Optimization Engine) (Sopasakis et al. (2020)) which relies on
the proximal averaged Newton method for optimal control (PANOC) (Stella et al.
(2017)) to solve the local NLPs fast.

The remainder of the paper is organized as follows. First, we outline the lane
change problem in Section 2. Then, we present a centralized formulation in
Section 3 before we distribute the problem in Section 4. In Section 5, we
discuss simulation results. Finally, we conclude and give an outlook for
future work in Section 6.

###  1.2 Notation

With $x_{k+j\mid k}$, we refer to the prediction of variable $x$ at the future
time step $k+j$ given information up to time $k$ while $x_{\cdot\mid k}$
denotes the trajectory of $x$ along the entire prediction horizon of length
$N\in\mathbb{N}_{>0}$. For $x\in\mathbb{R}^{n}$ and $i\in\\{1,\ldots,n\\}$,
$x_{i}$ is the $i$-th entry of $x$, and the interval $[a,b]\subset\mathbb{N}$
with $a<b$ is denoted as $\mathbb{N}_{[a,b]}$. Finally,
$[x]_{+}\triangleq\max\\{0,x\\}$ for $x\in\mathbb{R}$ is referred to as the
plus operator.

##  2 Cooperative Lane Change Problem

###  2.1 Problem Description

Fig. 1 illustrates a sketch of the problem we intend to solve. To reduce
complexity, we restrict ourselves to two lane scenarios, in which the SA wants
to change lanes while the target lane is already occupied by other agents.
This scheme, though, can easily be extended to scenarios with more than two
lanes. In our use case, the SA sends a cooperation request to $N_{A}-1$ agents
in order to eventually merge in between two of these agents in the target
lane. These are the preceding agent (PAT) and the following agent (FAT) in the
target lane in accordance to Fig. 1. The preceding and following agent in the
subject lane are referred to as PAS and FAS respectively.

PASSAsubjectlaneFATFASPATtargetlane Figure 1: Sketch of the lane change
scenario with $N_{A}=5$ cooperative agents. The SA (blue) aims to merge in
between the PAT (red) and FAT (green).

Generally, the fully automated lane change maneuver can as such be subdivided
into the following steps:

  1. 1.

Initialize cooperative group: SA sets up a cooperative group of $N_{A}$
agents, in which every agent can communicate with every other agent of the
group.

  2. 2.

Negotiate agent order: Determine PAT and FAT
of SA in the target lane, i.e., the agents that will be in front and behind
the SA after changing lanes.

  3. 3.

Establish headway: There must be sufficient headway distance between PAT & FAT
to let the SA in.

  4. 4.

Conduct lane change: SA changes lanes.

Some of these steps may also be combined instead of solving them separately.
To reduce complexity, we assume that Step 1 and Step 2 have already been
accomplished, i.e., the PAT and FAT are known. These steps will further be
investigated as part of future work. In this work, we propose a distributed
algorithm to solve Step 3 and Step 4 while relying on the following
fundamental assumptions.

###### Assumption 1

A1. All agents are equipped with V2V communication; A2. No communication
failures or package dropouts occur; A3. Agent clocks are synchronized; A4.
Every agent has access to a digital map to have knowledge about the road
geometry ahead; A5. Every agent in the scenario belongs to the cooperative
group.

Assumption A5 reduces complexity in the problem description but does not limit
the applicability of our approach. Conversely, it can easily be extended in
that direction.

###  2.2 Modeling

$s$$\delta$$\Delta\psi$$t_{p}$$\Delta
y$$v$$\beta$$\dot{\psi}$$l_{f}$$l_{r}$$X_{I}$$Y_{I}$intertial frameroad
centerlineFrenet frame Figure 2: Agent motion in the Frenet frame.

To derive a mathematical model of the lane change maneuver, a kinematic
bicycle model (Rajamani (2012)) is adopted to describe the agents’ motion in a
curvilinear reference frame, see Fig. 2. For such kind of use case, it is a
common approach in literature to apply the Frenet frame (Qian et al. (2016)),
in which the agent’s position is given in terms of its path coordinate $s$ and
the perpendicular displacement $\Delta y$ from the road centerline. With this
definition, the center of the subject lane in Fig. 1 is given by $(s,\Delta
y)=(s,-w_{\text{lane}}/2)$ for any $s$ where $w_{\text{lane}}$ is the lane
width. For every agent, we devise a state space model of the form

$\displaystyle\dot{s}$
$\displaystyle=v\cos(\Delta\psi+\beta)\left(\frac{1}{1-\Delta
y\,\kappa(s)}\right)$ (1a) $\displaystyle\Delta\dot{y}$
$\displaystyle=v\sin(\Delta\psi+\beta)$ (1b) $\displaystyle\Delta\dot{\psi}$
$\displaystyle=\frac{v}{l_{r}}\sin(\beta)-v\cos(\Delta\psi)\left(\frac{\kappa(s)}{1-\Delta
y\,\kappa(s)}\right)$ (1c) $\displaystyle\dot{v}$ $\displaystyle=a_{x}$ (1d)

where

$\displaystyle\beta=\arctan\bigl{(}\tan(\delta)\,l_{r}/(l_{f}+l_{r})\bigr{)}$

denotes the vehicle sideslip angle, $\delta$ the wheel steering angle, $v$ the
vehicle speed, $a_{x}$ the longitudinal vehicle acceleration, $\Delta\psi$ the
heading error between the path tangent (along $t_{p}$) and the longitudinal
vehicle axis. Moreover, $l_{f}$, $l_{r}$ refer to the distance between the
front respectively rear axle and the center of gravity. The path curvature
$\kappa(s)$ of the road centerline is assumed to be a known parameterized
curve (see assumption A4) in the path coordinate $s$.

The resulting nonlinear state space model $\dot{x}=f(x,u)$ features the state
vector $x=[s,\,\Delta
y,\,\Delta\psi,\,v]^{\scriptscriptstyle\top}\in\mathbb{R}^{n_{x}}$ and the
input vector
$u=[a_{x},\,\delta]^{\scriptscriptstyle\top}\in\mathbb{R}^{n_{u}}$. It can be
recognized that model (1) exhibits a singularity for $\Delta
y=1/\kappa(s)=r(s)$ where $r(s)$ is the path radius. In our case, though, even
for a radius of $10\,\mathrm{m}$, a path deviation of $10\,\mathrm{m}$ is very
unlikely to happen. To subsequently differ between individual agents, we
define the set of collaborative agents
$\mathcal{A}\triangleq\\{1,\ldots,N_{A}\\}$ where $N_{A}$ is a positive
integer. This way, we refer to the state vector $x$ of agent $i\in\mathcal{A}$
as $x^{[i]}$.

##  3 Centralized Formulation

After generally introducing the cooperative lane change problem in Section
2.1, hereafter, we aim to formalize the problem in terms of an optimal control
problem (OCP) which is solved in a receding horizon fashion. We start with a
centralized formulation and derive its distributed variant in Section 4.
Following Section 2.1, we subdivide the lane change in two maneuver steps.

  1. (M-Step 1)

SA, PAT & FAT establish required headway.

  2. (M-Step 2)

The SA changes lanes.

We integrate both steps in a single OCP as subsequently outlined.

####  3.0.1 Control Objectives

To perform a proper fully automated cooperative lane change maneuver, the
agents have to accommodate certain control objectives. First, every agent
should track its reference speed $v^{\text{ref}}$ (usually provided by a
higher level planning algorithm). Second, the deviation from the lane center,
defined through the desired lateral displacement $\Delta y^{\text{ref}}$ from
the road centerline, should be minimized. Additionally, we aim to minimize the
heading error $\Delta\psi$ (thus choosing $\Delta\psi^{\text{ref}}=0$) to
reduce overshooting during a lane change. For reasons of comfort, we penalize
the control input magnitude, that is, the applied longitudinal acceleration
$a_{x}$ and the wheel steering angle $\delta$.

Along a horizon of $N$ steps, we formalize these objectives for every agent
$i\in\mathcal{A}$ as the stage cost at time $k+j$ for
$j\in\mathbb{N}_{[0,N-1]}$

$\displaystyle\ell_{j}^{[i]}({x}_{k+j\mid k}^{[i]},{u}_{k+j\mid
k}^{[i]}){}\triangleq{}{u}_{k+j\mid
k}^{[i],\scriptscriptstyle\top}\,R^{[i]}\,{u}_{k+j\mid k}^{[i]}$ (2)
$\displaystyle~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}+\bigl{(}{x}_{k+j\mid
k}^{[i]}-{x}_{k+j\mid
k}^{{[i]},\mathrm{ref}}\bigr{)}^{\scriptscriptstyle\top}Q^{[i]}\,\bigl{(}{x}_{k+j\mid
k}^{[i]}-{x}_{k+j\mid k}^{{[i]},\mathrm{ref}}\bigr{)}$

and the terminal cost

$\displaystyle\ell_{N}^{[i]}({x}_{k+N\mid
k}^{[i]}){}\triangleq{}\bigl{(}{x}_{k+N\mid k}^{[i]}-{x}_{k+N\mid
k}^{{[i]},\mathrm{ref}}\bigr{)}^{\scriptscriptstyle\top}Q_{N}^{[i]}\,\bigl{(}{x}_{k+N\mid
k}^{[i]}-{x}_{k+N\mid k}^{{[i]},\mathrm{ref}}\bigr{)}$ (3)

where ${x}_{k+j\mid k}^{[i],\text{ref}}\triangleq[s_{k+j\mid
k}^{[i],\text{ref}},\Delta y_{k+j\mid k}^{[i],\text{ref}},\Delta\psi_{k+j\mid
k}^{[i],\text{ref}},v_{k+j\mid k}^{[i],\text{ref}}]^{\scriptscriptstyle\top}$
denotes the reference state while
$Q^{[i]}\triangleq\mathrm{diag}(q_{s}^{[i]},q_{\Delta
y}^{[i]},q_{\Delta\psi}^{[i]},q_{v}^{[i]})$ $\succeq 0$,
$Q_{N}^{[i]}\triangleq\mathrm{diag}(q_{N,s}^{[i]},q_{N,\Delta
y}^{[i]},q_{N,\Delta\psi}^{[i]},q_{N,v}^{[i]})\succeq 0$ and
$R^{[i]}\triangleq\mathrm{diag}(r_{a_{x}}^{[i]},r_{\delta}^{[i]})\succ 0$ are
positive (semi)definite weighting matrices.

####  3.0.2 Constraints

Besides control objectives, we need to ensure that agents only move in their
designated lanes. Thus, we constrain the lateral displacement $\Delta y$.
Moreover, the agents should not exceed the maximum speed
$\overline{v}_{k+j\mid k}^{[i]}$ at time $k+j$ and not drive backwards. That
said, we are able to derive the admissible state set

$\displaystyle\mathcal{X}_{k+j\mid k}^{[i]}\triangleq$
$\displaystyle\left\\{x\in\mathbb{R}^{n_{x}}~{}\lvert~{}\Delta\underline{y}_{k+j\mid
k}^{[i]}\leq x_{2}\leq\Delta\overline{y}_{k+j\mid k}^{[i]}\right.$ (4)
$\displaystyle~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}\,\land~{}\left.0\leq
x_{4}\leq\overline{v}^{[i]}_{k+j\mid k}\right\\}$

at time $k+j$ for $j\in\mathbb{N}_{[1,N]}$ where $\Delta\underline{y}_{k+j\mid
k}^{[i]}$ and $\Delta\overline{y}_{k+j\mid k}^{[i]}$ are reasonable bounds
which assure that agents in neighboring lanes have a minimum lateral distance
to each other. Due to actuator limitations and for reasons of ride comfort and
vehicle stability, we constrain the control actions by

$\displaystyle\mathcal{U}^{[i]}\triangleq\left\\{u\in\mathbb{R}^{n_{u}}~{}\lvert~{}\underline{a}_{x}\leq
u_{1}\leq\overline{a}_{x}~{}\land~{}\underline{\delta}\leq
u_{2}\leq\overline{\delta}\right\\}$ (5)

where $\underline{a}_{x}$, $\overline{a}_{x}$ and $\underline{\delta}$,
$\overline{\delta}$ are appropriately chosen lower and upper bounds
respectively. Moreover, for the same reason, we bound the lateral acceleration
(i.e., the product of longitudinal velocity and yaw rate)

$\displaystyle
a_{y}^{[i]}=(v^{[i]}\cos(\beta^{[i]}))\cdot(v^{[i]}\sin(\beta^{[i]}))/l_{r}^{[i]}$

and the total acceleration
$a_{\text{tot}}^{[i]}=((a_{x}^{[i]})^{2}+(a_{y}^{[i]})^{2})^{\nicefrac{{1}}{{2}}}$
through the constraints

$\displaystyle-\overline{a}_{y}^{[i]}{}\leq{}a_{y,k+j\mid
k}^{[i]}{}\leq{}\overline{a}_{y}^{[i]}~{}~{}\land~{}~{}(a_{\text{tot},k+j\mid
k}^{[i]})^{2}{}\leq{}(\overline{a}_{\text{tot}}^{[i]})^{2},$ (6)

as a function of $(u_{k+j\mid k}^{[i]},x_{k+j\mid k}^{[i]})$, for
$j\in\mathbb{N}_{[0,N-1]}$ with appropriate upper bounds
$\overline{a}_{y}^{[i]}>0$ and $\overline{a}_{\text{tot}}^{[i]}>0$.

Let $\text{Pd}(i)\subset\mathcal{A}$ denote the set of preceding vehicles of
Agent $i$. In case Agent $i$ is the SA, $\text{Pd}(i)$ contains the PAT and,
until M-Step 2 is completed, the PAS. If Agent $i$ is the FAT, $\text{Pd}(i)$
contains the SA and, until M-Step 2 is completed, the PAT. For all other
agents, $\text{Pd}(i)$ is the agent that is physically ahead. To establish the
headway distance between SA, PAT and FAT (M-Step 1) and to avoid collisions
between agents, Agent $i$ bounds the headway distance from below, i.e.,

$\displaystyle{s_{k+j\mid k}^{[l]}-s_{k+j\mid k}^{[i]}}\geq
d_{\text{hw}},~{}\forall l\in\text{Pd}(i)$ (7)

where the lower bound $d_{\text{hw}}>0$ also encodes the vehicles’ geometry.
As the agents’ path coordinate $s$ refers to the road centerline (see Section
2.2), $d_{\text{hw}}$ has additionally to be increased on highly curved road
sections in dependence of the (maximum) road curvature
$\lvert\kappa(s)\lvert$.

####  3.0.3 Optimization Problem

For notational convenience, hereafter, we stack the states of all agents
$i\in\mathcal{A}$ at time $k+j$ in a single vector, i.e., $\bar{x}_{k+j\mid
k}\triangleq(x_{k+j\mid k}^{[i]})_{i=1}^{N_{A}}$. Likewise, for the control
inputs, we define $\bar{u}_{k+j\mid k}\triangleq(u_{k+j\mid
k}^{[i]})_{i=1}^{N_{A}}$ . Additionally, when referring to the entire state or
input trajectory, we write $\bar{x}_{\cdot\mid k}\triangleq(\bar{x}_{k+j\mid
k})_{j=1}^{N}$ and $\bar{u}_{\cdot\mid k}\triangleq(\bar{u}_{k+j\mid
k})_{j=0}^{N-1}$. This way, we introduce the aggregated stage costs

$\displaystyle\ell_{N}(\bar{x}_{k+N\mid k})$
$\displaystyle\triangleq\sum_{i=1}^{N_{A}}\ell_{N}^{[i]}({x}_{k+N\mid
k}^{[i]}),$ $\displaystyle\ell_{j}(\bar{x}_{k+j\mid k},\bar{u}_{k+j\mid k})$
$\displaystyle\triangleq\sum_{i=1}^{N_{A}}\ell_{j}^{[i]}({x}_{k+j\mid
k}^{[i]},{u}_{k+j\mid k}^{[i]}).$

By adopting a direct multiple shooting formulation (Bock and Plitt (1984)),
that is, including the discrete-time system dynamics as equality constraints
in the OCP, we phrase the resulting centralized lane change NLP as

$\displaystyle\operatorname*{minimize}_{\bar{u}_{\cdot\mid
k},\,\bar{x}_{\cdot\mid k}}\ \ell_{N}(\bar{x}_{k+N\mid
k}){}+{}\sum_{j=0}^{N-1}\ell_{j}(\bar{x}_{k+j\mid k},\bar{u}_{k+j\mid k})$
(8a) s.t. $\displaystyle~{}\text{for every agent~{}}i\in\mathcal{A}:$
$\displaystyle~{}u_{k+j\mid
k}^{[i]}{}\in{}\mathcal{U}^{[i]},~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}\,~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}j\in\mathbb{N}_{[0,N-1]}$
(8b) $\displaystyle~{}x_{k+j\mid k}^{[i]}{}\in{}\mathcal{X}_{k+j\mid
k}^{[i]},~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}\,~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}j\in\mathbb{N}_{[1,N]}$
(8c) $\displaystyle~{}x_{k+j+1\mid k}^{[i]}{}={}f_{d}^{[i]}(x_{k+j\mid
k}^{[i]},u_{k+j\mid k}^{[i]}),~{}~{}~{}~{}\,j\in\mathbb{N}_{[0,N-1]}$ (8d)
$\displaystyle~{}-\overline{a}_{y}^{[i]}{}\leq{}a_{y,k+j\mid
k}^{[i]}{}\leq{}\overline{a}_{y}^{[i]},~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}\,j\in\mathbb{N}_{[0,N-1]}$
(8e) $\displaystyle~{}(a_{\text{tot},k+j\mid
k}^{[i]})^{2}{}\leq{}(\overline{a}_{\text{tot}}^{[i]})^{2},~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}j\in\mathbb{N}_{[0,N-1]}$
(8f) $\displaystyle~{}{s_{k+j\mid k}^{[l]}-s_{k+j\mid k}^{[i]}}\geq
d_{\text{hw}},\,\forall l\in\mathrm{Pd}(i);\,j\in\mathbb{N}_{[1,N]}$ (8g)
$\displaystyle~{}x_{k\mid k}^{[i]}{}={}x_{k}^{[i]},$ (8h)

where

$\displaystyle f_{d}^{[i]}(x_{k+j\mid k}^{[i]},u_{k+j\mid k}^{[i]})\triangleq
x_{k+j\mid k}^{[i]}+\int_{t_{k+j}}^{t_{k+j+1}}f(x^{[i]}(\tau),u_{k+j\mid
k}^{[i]})\,d\tau$ (9)

represents the discretized system dynamics using zero order hold. We
approximate the integral in (9) using a 4th order Runge-Kutta method (Nocedal
and Wright (2006)).

###### Remark 1

During M-Step 1, the SA’s reference value $\Delta y^{\text{ref}}$ is set to
the center of the subject lane while the bounds on $\Delta y$ prevent the SA
from leaving its lane. When the required headway distance between the SA, PAT
and FAT has been established (for time $t_{k}$ and the entire prediction
horizon), the bounds on $\Delta y$ are modified in M-Step 2 to allow the SA to
drive in both lanes. Simultaneously, the reference $\Delta y^{\text{ref}}$ is
set to the center of the target lane to initiate the lane change. After M-Step
2, the bounds on $\Delta y$ are finally adapted to the target lane.

##  4 Distributed Solution using ADMM

Compared to a centralized formulation, as described in Section 3, distributing
computations among the agents appears to be more scalable and resilient. For a
distributed control scheme that could eventually be implemented in a test
vehicle, we impose the following requirements.

###### Requirements 1

R1. The agents’ state space models and their parameters should be private; R2.
Instead of the entire state vector, only position information should be
exchanged to reduce communication load; R3. The distributed OCP must be solved
within the sampling time, including the time for data exchange via V2V.

To solve Problem (8) in a distributed fashion, we apply the consensus
Alternating Direction Method of Multipliers (ADMM) (Bertsekas and Tsitsiklis
(1989)). As shown in the remainder of this section, we decompose Problem (8)
such that objectives and constraints, belonging to the individual agent, are
incorporated in a local OCP while the joint satisfaction of the minimum
headway distance is tackled by a coordinator which is run on the SA.

###  4.1 Problem Reformulation and Decomposition

To come up with a distributed OCP that is compliant with a consensus ADMM
formulation, in a first step, we need to reformulate Problem (8). For
notational convenience, we define an augmented optimization vector that
contains control input and state trajectories over the prediction horizon,
that is, $\xi_{\cdot\mid k}^{[i]}=(x_{k+j\mid k}^{[i]},u_{k+j-1\mid
k}^{[i]})_{j=1}^{N}$. Moreover, the aggregated vector $\bar{\xi}_{\cdot\mid
k}\triangleq({\xi}_{\cdot\mid k}^{[i]})_{i=1}^{N_{A}}$ stacks the variables
$\xi_{\cdot\mid k}^{[i]}$ for every agent $i\in\mathcal{A}$ in a single
vector. This way, we can summarize equality constraints (8d) in a compact form
as $h^{[i]}({\xi}_{\cdot\mid k}^{[i]})=0$. Likewise, inequality constraints
(8e) and (8f) can concisely be stated as $g^{[i]}({\xi}_{\cdot\mid
k}^{[i]})\leq 0$. That said, we introduce the associated indicator functions

$\displaystyle\mathds{1}_{h^{[i]}}({\xi}_{\cdot\mid k}^{[i]})$
$\displaystyle\triangleq\begin{cases}0,&h^{[i]}({\xi}_{\cdot\mid
k}^{[i]})=0\\\ \infty,&\text{otherwise},\end{cases}$ (10)
$\displaystyle\mathds{1}_{g^{[i]}}({\xi}_{\cdot\mid k}^{[i]})$
$\displaystyle\triangleq\begin{cases}0,&g^{[i]}({\xi}_{\cdot\mid k}^{[i]})\leq
0\\\ \infty,&\text{otherwise}.\end{cases}$ (11)

To accommodate equality and inequality constraints, we define the augment cost
for every agent $i$ as

$\displaystyle\phi^{[i]}({\xi}_{\cdot\mid
k}^{[i]}){}\triangleq{}\ell_{N}^{[i]}(x_{k+N\mid k}^{[i]}){}$
$\displaystyle+{}\sum_{j=0}^{N-1}\ell_{j}^{[i]}(x_{k+j\mid
k}^{[i]},{u}_{k+j\mid k}^{[i]})$ (12)
$\displaystyle+{}\mathds{1}_{h^{[i]}}({\xi}_{\cdot\mid
k}^{[i]})+\mathds{1}_{g^{[i]}}({\xi}_{\cdot\mid k}^{[i]}).$

Moreover, we define a cost associated with the minimum headway distance
constraints (8g), i.e.,

$\displaystyle\gamma(\bar{\xi}_{\cdot\mid
k})\triangleq\sum_{j=1}^{N}\sum_{i=1}^{N_{A}}\sum_{l\in\mathrm{Pd}(i)}\mathds{1}_{\text{hw}}(\xi_{k+j\mid
k}^{[i]},\xi_{k+j\mid k}^{[l]})$ (13)

with the indicator function

$\displaystyle\mathds{1}_{\text{hw}}(\xi_{k+j\mid k}^{[i]},\xi_{k+j\mid
k}^{[l]})$ $\displaystyle\triangleq\begin{cases}0,&{s_{k+j\mid
k}^{[l]}-s_{k+j\mid k}^{[i]}}\geq d_{\text{hw}}\\\
\infty,&\text{otherwise}.\end{cases}$

As the minimum headway distance constraints are convex, so is the associated
indicator function $\mathds{1}_{\text{hw}}$ and as such the cost $\gamma$.
With (12) and (13), we can rewrite the centralized NLP (8) as a box
constrained NLP

$\displaystyle\operatorname*{minimize}_{\bar{\xi}_{\cdot\mid k}}\ $
$\displaystyle\gamma(\bar{\xi}_{\cdot\mid
k})+\sum_{i=1}^{N_{A}}\phi^{[i]}(\xi_{\cdot\mid k}^{[i]})$ (14a) s.t.
$\displaystyle~{}\xi_{k+j\mid k}^{[i]}{}\in{}{\Xi}_{k+j\mid
k}^{[i]},~{}~{}~{}~{}~{}~{}~{}~{}i\in\mathcal{A},~{}j\in\mathbb{N}_{[1,N]}$
(14b)

where the original input and state constraints (8b) and (8c) are represented
by the close and convex feasible set

$\displaystyle{\Xi}_{k+j\mid k}^{[i]}\triangleq\Bigl{\\{}(x_{k+j\mid
k}^{[i]},u_{k+j-1\mid k}^{[i]})\in(\mathcal{X}_{k+j\mid
k}^{[i]}\times\mathcal{U}^{[i]})\subseteq\mathbb{R}^{n_{x}}\times\mathbb{R}^{n_{u}}\Bigr{\\}}.$

To decouple the agents, we introduce new auxiliary variables $z_{\cdot\mid
k}^{[i]}\triangleq(z_{k+j\mid k}^{[i]})_{j=1}^{N}$ respectively
$\bar{z}_{\cdot\mid k}\triangleq(z_{\cdot\mid k}^{[i]})_{i=1}^{N_{A}}$ which
correspond to Agent $i$’s path coordinate $s^{[i]}$ over the prediction
horizon. By imposing the consensus constraint
$c_{\text{cc}}^{\scriptscriptstyle\top}\,\xi_{k+j\mid k}^{[i]}=z_{k+j\mid
k}^{[i]}$ with
$c_{\text{cc}}\triangleq[1~{}0~{}0~{}0~{}0~{}0]^{\scriptscriptstyle\top}$ for
every $j\in\mathbb{N}_{[1,N]}$ and every agent $i\in\mathcal{A}$, we can
rewrite (14) as an equivalent problem with auxiliary variables

$\displaystyle\operatorname*{minimize}_{\bar{\xi}_{\cdot\mid
k},\,\bar{z}_{\cdot\mid k}}\ $ $\displaystyle\gamma(\bar{z}_{\cdot\mid
k})+\sum_{i=1}^{N_{A}}\phi^{[i]}(\xi_{\cdot\mid k}^{[i]})$ (15a) s.t.
$\displaystyle~{}c_{\text{cc}}^{\scriptscriptstyle\top}\,\xi_{k+j\mid
k}^{[i]}=z_{k+j\mid
k}^{[i]},~{}~{}~{}~{}i\in\mathcal{A},~{}j\in\mathbb{N}_{[1,N]}$ (15b)
$\displaystyle~{}\xi_{k+j\mid k}^{[i]}{}\in{}{\Xi}_{k+j\mid
k}^{[i]},~{}~{}~{}~{}~{}~{}~{}~{}i\in\mathcal{A},~{}j\in\mathbb{N}_{[1,N]}$
(15c)

By virtue of Problem (15), it can be recognized that the agents’ cost
$\phi^{[i]}$ solely depends on the local optimization variable
$\xi^{[i]}_{\cdot\mid k}$. Conversely, the cost $\gamma$, which accommodates
the minimum headway distance constraints, relies on the auxiliary variables
$z_{\cdot\mid k}^{[i]}$ or copies of the agents’ path coordinate $s^{[i]}$
over the prediction horizon. Such formulation motivates a distributed solution
of Problem (15), that is, every agents optimizes its local OCP while the
minimum headway distance constraints are accommodated by a coordinator. For
reasons of brevity, hereafter, we abbreviate constraint (15b) as
$C_{\text{cc}}\xi_{\cdot\mid k}^{[i]}=z_{\cdot\mid k}^{[i]}$ where
$C_{\text{cc}}$ is a matrix of appropriate dimension.

###### Remark 2

Our choice of $c_{\text{cc}}$ in (15b) is crucial to satisfy requirements R1
and R2. This way, only the agents’ path coordinates
$s^{[i]}=c_{\text{cc}}^{\scriptscriptstyle\top}\,\xi^{[i]}$ need to be
exchanged amongst each other via V2V.

###  4.2 Consensus ADMM Framework

To embed the reformulated Problem (15) in the ADMM framework, we dualize
consensus constraint (15b) and obtain the Augmented Lagrangian function
(Bertsekas and Tsitsiklis (1989))

$\displaystyle\mathcal{L}_{\rho}(\bar{\xi}_{\cdot\mid k},\bar{z}_{\cdot\mid
k},\bar{\lambda})=\gamma(\bar{z}_{\cdot\mid
k})+\sum_{i=1}^{N_{A}}\phi^{[i]}(\xi_{\cdot\mid k}^{[i]})$ (16)
$\displaystyle~{}~{}~{}~{}+\sum_{i=1}^{N_{A}}\Bigl{[}(\lambda^{[i]})^{\scriptscriptstyle\top}({C}_{\text{cc}}\xi_{\cdot\mid
k}^{[i]}-z_{\cdot\mid k}^{[i]})+\frac{\rho}{2}\|{C}_{\text{cc}}\xi_{\cdot\mid
k}^{[i]}-z_{\cdot\mid k}^{[i]}\|^{2}\Bigr{]}$

where $\rho>0$ is a constant penalty parameter, $\lambda^{[i]}$ is the vector
of Lagrangian multipliers associated with consensus constraint (15b), and
$\bar{\lambda}\triangleq(\lambda^{[i]})_{i=1}^{N_{A}}$ is the aggregated
vector of multipliers. The consensus ADMM algorithm, applied to minimize (16)
subject to $\xi_{k+j\mid k}^{[i]}{}\in{}{\Xi}_{k+j\mid k}^{[i]}$ for
$i\in\mathcal{A}$ and $j\in\mathbb{N}_{[1,N]}$, is summarized in Algorithm 1.
In a receding horizon fashion, we run the following steps at time $k$.

Step 1: Starting with an initial guess $(\xi_{\cdot\mid
k}^{[i]},{z}_{\cdot\mid k}^{[i]},\lambda^{[i]})$, every agent solves the box
constrained NLP (18) and transmits the optimized path coordinate trajectory
$s_{\cdot\mid k}^{[i]}={C}_{cc}\,\xi_{\cdot\mid k}^{[i]}$ to the coordinator.
As $\phi^{[i]}$ in (16) is nonsmooth and nonconvex, NLP (18) adopts a smooth
but still nonconvex reformulation $\Phi^{[i]}$ to guarantee convergence of the
ADMM scheme, see Section 4.3 and Section 4.4.

Step 2: The coordinator (residing on the SA) solves the constrained
coordination QP (19) which imposes the minimum headway distance (encoded in
$\gamma$) as linear constraints (cf. (8g)). For notational convenience, we
abbreviate this constraint as $\bar{C}_{\text{hw}}\bar{z}_{\cdot\mid
k}\leq\bar{d}_{\text{hw}}$ where $\bar{C}_{\text{hw}}$ and
$\bar{d}_{\text{hw}}$ are a matrix and a vector of appropriate dimension.
Mostly, the initial condition of the lane change maneuver may look like in
Fig. 1. Then, the original constraint (8g) may not be satisfied. As a
consequence, we may not be able to establish consensus without either
violating agent dynamics (as we would need to shift agents’ positions
instantaneously to satisfy (8g)) or the minimum headway distance constraints.
For this reason, we reformulate the minimum headway distance constraint as a
soft constraint

$\displaystyle\bar{C}_{\text{hw}}\bar{z}_{\cdot\mid
k}\leq\bar{d}_{\text{hw}}+\bar{E}_{\text{hw}}\epsilon~{}~{}\text{with}~{}~{}\epsilon\triangleq(\epsilon_{i,l})_{i\in\mathcal{A},l\in\mathrm{Pd}(i)}$
(17)

where $\epsilon$ is a vector of slack variables $\epsilon_{i,l}\geq 0$ for
every $(i,l)$ with $i\in\mathcal{A}$ and $l\in\mathrm{Pd}(i)$ and
$\bar{E}_{\text{hw}}$ is a matrix of appropriate dimension. That way,
consensus can be established and $\epsilon_{i,l}\rightarrow 0$ will hold after
M-Step 1 is completed. Moreover, we augment the cost function in QP (19) with
the additional linear cost term
$q_{\epsilon}^{\scriptscriptstyle\top}\epsilon\geq 0$ that penalizes
$\epsilon$ where $q_{\epsilon}$ is a weighting vector of appropriate dimension
with all weights larger than zero. The resulting Problem (19) is a standard QP
that optimizes over $(\bar{z}_{\cdot\mid k},\epsilon)$. Its solution can be
obtained fast using mature QP solvers.

Algorithm 1 Nonconvex ADMM Problem at time $k$

Initial guess: $\bar{\xi}_{\cdot\mid k},\bar{z}_{\cdot\mid k},\bar{\lambda}$
(every agent & coordinator)

repeat

1) Every agent $i\in\mathcal{A}$: Solve NLP in parallel
$\displaystyle~{}~{}{\xi}_{\cdot\mid
k}^{[i]}\leftarrow\text{arg\,}\underset{\xi_{\cdot\mid
k}^{[i]}}{\text{min}}~{}\Phi^{[i]}(\xi_{\cdot\mid k}^{[i]};\alpha,\mu)$
$\displaystyle+(\lambda^{[i]})^{\scriptscriptstyle\top}{C}_{\text{cc}}\,\xi_{\cdot\mid
k}^{[i]}$ (18) $\displaystyle+\frac{\rho}{2}\|{C}_{\text{cc}}\,\xi_{\cdot\mid
k}^{[i]}-z_{\cdot\mid k}^{[i]}\|^{2}$

s.t. ${\xi}_{\cdot\mid k}^{[i]}\in{\Xi}_{\cdot\mid k}^{[i]}$

and transmit ${C}_{\text{cc}}\,\xi_{\cdot\mid k}^{[i]}$ to coordinator.

2) Coordinator: Solve coordination QP  $\displaystyle\bar{z}_{\cdot\mid
k}\leftarrow\text{arg\,}\underset{\bar{z}_{\cdot\mid
k},\epsilon}{\text{min}}~{}\sum_{i=1}^{N_{A}}\Bigl{[}$
$\displaystyle-(\lambda^{[i]})^{\scriptscriptstyle\top}z_{\cdot\mid k}^{[i]}$
(19) $\displaystyle+\frac{\rho}{2}\|{C}_{\text{cc}}\,\xi_{\cdot\mid
k}^{[i]}-z_{\cdot\mid
k}^{[i]}\|^{2}\Bigr{]}+q_{\epsilon}^{\scriptscriptstyle\top}\epsilon$

s.t. $\bar{C}_{\text{hw}}\bar{z}_{\cdot\mid
k}\leq\bar{d}_{\text{hw}}+\bar{E}_{\text{hw}}\epsilon,~{}\epsilon_{i,l}\geq
0.$

3) Coordinator: Perform dual gradient step
$\displaystyle\lambda^{[i]}\leftarrow\lambda^{[i]}+\rho\,({C}_{\text{cc}}\,\xi_{\cdot\mid
k}^{[i]}-z_{\cdot\mid k}^{[i]}),~{}\forall i\in\mathcal{A}.~{}\,$ (20)

4) Coordinator: Broadcast $(\bar{\lambda},\bar{z}_{\cdot\mid k})$ to every
agent.

until stopping criterion satisfied

Step 3 & 4: The coordinator updates the dual variables $\lambda^{[i]}$ and
transmits the vectors $(\bar{\lambda},\bar{z}_{\cdot\mid k})$ to the agents.

This scheme is iterated until the stopping criteria, adopted from (Boyd et
al., 2011, Sec. 3.1.1), is satisfied, that is, until the norm of the primal
and dual residuals are below their thresholds $\epsilon^{\text{prim}}\geq 0$
and $\epsilon^{\text{dual}}\geq 0$ respectively

$\displaystyle\|\xi_{\cdot\mid k}-\bar{z}_{\cdot\mid
k}\|\leq\epsilon^{\text{prim}},\,\|(\rho\,[{C}_{\text{cc}}\,{\xi}_{\cdot\mid
k}^{[i]}-{z}_{\cdot\mid k}^{[i]}])_{i=1}^{N_{A}}\|\leq\epsilon^{\text{dual}}.$
(21)

After convergence, every agent $i$ applies the control action $u_{k\mid
k}^{[i]\,\star}$ locally. At the next time step $k+1$, Algorithm 1 is warm-
started by exploiting the solution $(\bar{\xi}_{\cdot\mid
k}^{\star},\bar{z}_{\cdot\mid k}^{\star},\bar{\lambda}^{\star})$ from time
step $k$ as initial guess.

###  4.3 Agent NLPs: Smooth Reformulation and Solution

To ensure convergence of Algorithm 1, $\phi^{[i]}$ in (16) needs to be convex
or at least a smooth nonconvex function, see Hong et al. (2016). Convexity,
though, is not satisfied due to nonconvex system dynamics
$h^{[i]}(\xi_{\cdot\mid k+j}^{[i]})=0$. At the same time, indicator functions
in (12) are nonsmooth. Therefore, we come up with a smooth reformulation of
the indicator functions $\mathds{1}_{h^{[i]}}$ and $\mathds{1}_{g^{[i]}}$.

By applying the Augmented Lagrangian method (ALM) (Nocedal and Wright, 2006,
Chap. 17), we replace the indicator function
$\mathds{1}_{h^{[i]}}({\xi}_{\cdot\mid k}^{[i]})$ of the equality constraints,
related to system dynamics, with the cost

$\displaystyle\phi_{\text{ALM}}^{[i]}({\xi}_{\cdot\mid
k}^{[i]};\alpha,\mu){}\triangleq{}\mu^{\scriptscriptstyle\top}h^{[i]}({\xi}_{\cdot\mid
k}^{[i]})+\frac{\alpha}{2}\,\|h^{[i]}({\xi}_{\cdot\mid k}^{[i]})\|^{2}$ (22)

where $\alpha>0$ is a penalty parameter and $\mu$ the vector of Lagrangian
multipliers related to the equality constraints. Moreover, we rephrase
inequality constraints $g^{[i]}({\xi}_{\cdot\mid k}^{[i]})\leq 0$ as equality
constraints $G({\xi}_{\cdot\mid k}^{[i]})=0$ (Sopasakis et al. (2020)) where
for each constraint $\iota=1,\ldots,n_{\text{ineq}}$ holds

$\displaystyle G_{\iota}({\xi}_{\cdot\mid
k}^{[i]})\triangleq[g_{\iota}^{[i]}({\xi}_{\cdot\mid k}^{[i]})]_{+}.$ (23)

As (23) is a nonsmooth function, we can not formulate $G({\xi}_{\cdot\mid
k}^{[i]})=0$ as an ALM-type constraint (Sopasakis et al. (2020)). Instead, we
apply the quadratic penalty method (PM) (Nocedal and Wright, 2006, Chap. 17)
to replace the indicator function (11) of the inequality constraints with the
cost

$\displaystyle\phi_{\text{PM}}^{[i]}({\xi}_{\cdot\mid
k}^{[i]};\alpha){}\triangleq{}\frac{\alpha}{2}\,\|G({\xi}_{\cdot\mid
k}^{[i]})\|^{2}.$ (24)

With (22) and (24), we gain the Augmented Lagrangian function of the local OCP

$\displaystyle\Phi^{[i]}({\xi}_{\cdot\mid
k}^{[i]};\alpha,\mu){}\triangleq{}\phi^{[i]}({\xi}_{\cdot\mid
k}^{[i]})+\phi_{\text{ALM}}^{[i]}({\xi}_{\cdot\mid
k}^{[i]};\alpha,\mu)+\phi_{\text{PM}}^{[i]}({\xi}_{\cdot\mid k}^{[i]};\alpha)$

which is a nonconvex $\mathcal{C}^{1}$ continuous differentiable function and
as such a smooth reformulation of $\phi^{[i]}$. To compute a local solution of
NLP (18), we apply the open source code framework OpEn v0.6.2 (Sopasakis et
al. (2020)), available on github.com/alphaville/optimization-engine. In an
inner loop, OpEn utilizes the proximal averaged Newton method for optimal
control (PANOC) to solve NLP (18) for a fixed penalty $\alpha$ and fixed
Lagrangian multipliers $\mu$. In an outer loop, $\alpha$ and $\mu$ are updated
to achieve constraint satisfaction as described in Sopasakis et al. (2020). In
every iteration of Algorithm 1 at time step $k$, the solver is warm-started
with the solution of the previous iteration. Likewise, OpEn is warm-started at
time step $k+1$ by exploiting the solution from time step $k$. For such kind
of problems, PANOC has already shown superior performance (Stella et al.
(2017); Katriniok et al. (2019)).

###  4.4 Convergence

For convex consensus ADMM problems, that is, if every $\phi^{[i]}$ and
$\gamma$ in (15) are convex, convergence has been proven in literature (Boyd
et al. (2011)). For nonconvex functions $\phi^{[i]}$ respectively
$\Phi^{[i]}$, as in our case, we can show that the nonconvex consensus ADMM
problem as well as the subproblems (18) converge to a set of stationary points
under some mild conditions (Hong et al. (2016); Wang et al. (2019); Sopasakis
et al. (2020)). Without any assumptions on the iterates, Algorithm 1 is
guaranteed to converge to a set of stationary points (i.e., to a local
solution) if Problem (15) meets certain regularity conditions and the step
size $\rho$ is chosen large enough. By virtue of Hong et al. (2016), our
problem satisfies all assumptions which are a prerequisite to convergence,
such as: the feasible set $({\Xi}_{k+j\mid
k}^{[i]})_{i\in\mathcal{A},\,j\in\mathbb{N}_{[1,N]}}$ is closed and convex,
$\gamma$ is convex and $\Phi^{[i]}$ has a Lipschitz-continuous gradient if the
steering angle $\delta$ is constrained on the interval
$(-\nicefrac{{\pi}}{{2}},\nicefrac{{\pi}}{{2}})$ — technically, even tighter
bounds are required, see Section 5.1.

##  5 Simulation Results

###  5.1 Simulation Setup

For a proof of concept, we evaluate the proposed consensus ADMM-based
framework in a realistic lane change scenario with $N_{A}=3$ cooperative
connected agents. According to Fig. 5a, the SA (Agent 2, blue) is driving on
the right (subject) lane while the PAT (Agent 1, red) and FAT (Agent 3, green)
on the left (target) lane are initially blocking the road for a lane change of
the SA.

In the simulation study, we adopt the control-oriented model (1) as validation
model. Every agent has dimensions $L^{[i]}=5\,\mathrm{m}$,
$W^{[i]}=2\,\mathrm{m}$, $l_{f}^{[i]}=1.4\,\mathrm{m}$ and
$l_{r}^{[i]}=1.4\,\mathrm{m}$. The distance between the center of gravity and
the front respectively rear bumper is $2.5\,\mathrm{m}$. In our scenario, we
want the agents to keep a bumper-to-bumper distance of $10\,\mathrm{m}$.
Taking vehicle dimensions and road curvature into account (the road radius is
always larger or equal to $200\,\mathrm{m}$), we set $d_{\text{hw}}$ to
$15\,\mathrm{m}$. This way, the minimum bumper-to-bumper distance is
approximately $10\,\mathrm{m}$. For every agent, the initial and reference
velocity is set to $14\,\nicefrac{\mathrm{m}}{\mathrm{s}}$. With a lane width
of $w_{\text{lane}}=4\,\mathrm{m}$, the initial Frenet coordinates
$(s^{[i]},\Delta y^{[i]})$ are $(12\,\mathrm{m},2\,\mathrm{m})$ and
$(0\,\mathrm{m},2\,\mathrm{m})$ for the PAT (red) and FAT (green) in the left
lane, respectively, while the initial coordinates of the SA (blue) in the
right lane are $(6\,\mathrm{m},-2\,\mathrm{m})$, see Fig. 5a.

In the consensus ADMM framework, we have selected the same weights for every
agent: $q_{s}^{[i]}=0$, $q_{\Delta y}^{[i]}=1$, $q_{\Delta\psi}^{[i]}=100$,
$q_{v}^{[i]}=1$, $r_{a_{x}}^{[i]}=1$, $r_{\delta}^{[i]}=600$ and
$Q_{N}^{[i]}=Q^{[i]}$. The sample period between two consecutive runs of
Algorithm 1 is set to $T_{s}=0.1\,\mathrm{s}$, the horizon length to $N=15$
and the penalty parameter to $\rho=100$. To keep the agents in their
designated lanes, the bounds
$(\Delta\underline{y}^{[i]},\Delta\overline{y}^{[i]})$ on the lateral
displacement are chosen as $(1.25\,\mathrm{m},2.75\,\mathrm{m})$ for the left
lane and $(-1.25\,\mathrm{m},-2.75\,\mathrm{m})$ for the right lane. The
absolute longitudinal acceleration should always be less or equal to
$4\,\nicefrac{\mathrm{m}}{\mathrm{s^{2}}}$, the absolute lateral acceleration
should not exceed $3.5\,\nicefrac{\mathrm{m}}{\mathrm{s^{2}}}$ and the total
acceleration is bounded from above by
$4\,\nicefrac{\mathrm{m}}{\mathrm{s^{2}}}$. Finally, the steering angle should
be within $\pm 5\,\mathrm{deg}$ while the maximum velocity is set to
$17\,\nicefrac{\mathrm{m}}{\mathrm{s}}$. Simulations are run on an Intel i7
machine at $2.9\,\mathrm{GHz}$ with Matlab R2018b.

###  5.2 Discussion of Results

In Fig. 3, we illustrate the optimized state and input trajectories of the
three agents while the steering angle plot (bottom plot, Fig. 3) is augmented
with the SA’s lateral acceleration and its upper bound. Moreover, Fig. 5
highlights three snapshots of the lane change maneuver.

$5$$10$$15$$20$$25$M-Step 1M-Step 2min. headwayHeadway [m]SA to PATFAT to
SA$-4$$-2$$0$$2$$4$lane markings (right)road centerlinelane markings
(left)$\Delta y$ bounds$\Delta y$ referenceLat. Displ. [m]PAT SA
FAT$0$$5$$10$Heading Err. [deg]$12$$14$$16$referenceVelocity
[m/s]$-4$$-2$$0$$2$$4$Lon. Accel.
[$\nicefrac{{\mathrm{m}}}{{\mathrm{s}^{2}}}$]$0$$2$$4$$6$$8$$10$$12$$0$$2$$4$upper
boundupper boundlat. accel. (SA) Time [s]Steering [deg] Lat. Acc.
[$\nicefrac{{\mathrm{m}}}{{\mathrm{s}^{2}}}$] Figure 3: Optimized state and
input trajectories of the lane change maneuver. M-Step 1 is accomplished at
$t=4.7\,\mathrm{s}$ and M-Step 2 at $t=8.0\,\mathrm{s}$.
$10^{0}$$10^{1}$$10^{2}$$\mathbf{\downarrow}$ Coord. QP: $\leq
0.2\,\mathrm{ms}$Exec. Time
[ms]PATSAFATCoord.$0$$2$$4$$6$$8$$10$$12$$0$$2$$4$$6$$8$$10$M-Step 1M-Step
2Time [s]ADMM Iter. [-] Figure 4: Execution times (top) for solving the OCPs
(solid lines) as a sum over all ADMM iterations (bottom) along with
communication overhead (shaded area). The solving time of the coordination QP
has been added to the SA communication overhead (blue shaded area).
$0$$10$$20$$30$$-20$$-10$$0$$10$(a) $t=0.0\,\mathrm{s}$optimized
trajectory$80$$90$$100$$110$$-50$$-40$$-30$$-20$(b)
$t=5.8\,\mathrm{s}$(simplified) illustration ofmin. headway
distance$110$$120$$130$$140$$-60$$-50$$-40$$-30$(c) $t=8.0\,\mathrm{s}$ Figure
5: Snapshots: (Left) Initial configuration: SA (blue) intends to change lanes;
(Middle) SA (blue) changes lanes after required headway has been established
(M-Step 2); (Right) SA (blue) has accomplished lane change maneuver.

During M-Step 1 of the maneuver, that is, until $t=4.7\,\mathrm{s}$ (light
orange patch in Fig. 3, 1 plot) the agents establish the required headway
distance to allow the SA (blue) to change lanes safely. As a minimum cost
maneuver, the PAT (red) accelerates and the FAT (green) decelerates to
increase the headway distance whereas the SA (blue) keeps its speed almost
constant. As the SA (blue) is located in the middle of the PAT (red) and FAT
(green), the acceleration and speed trajectories of PAT and FAT are symmetric
to each other — which appears to be the most reasonable cooperative maneuver.
The maximum absolute longitudinal acceleration during M-Step 1 is
$3.3\,\nicefrac{\mathrm{m}}{\mathrm{s^{2}}}$.

At the beginning of M-Step 2 (light purple patch in Fig. 3, 1 plot), the SA
(blue) modifies its reference value $\Delta y^{\text{ref}}$ (dashed blue line
in Fig. 3, 2 plot) from $-2\,\mathrm{m}$ (right lane) to $2\,\mathrm{m}$ (left
lane). At the same time, the constraint bounds on the lateral displacement are
adjusted to
$(\Delta\underline{y}^{[i]},\Delta\overline{y}^{[i]})=(-2.75\,\mathrm{m},2.75\,\mathrm{m})$,
see blue patch in Fig. 3, 2 plot. This enables the SA (blue) to drive in both
lanes and to initiate the lane change, see Fig. 5b. The lane change maneuver
is accomplished after $3.3\,\mathrm{s}$, that is, at $t=8\,\mathrm{s}$, see
also Fig. 5c. As we are rather focusing on lane change maneuvers than on
platooning, there is no need to bound the headway distance from above to
obtain a high vehicle density in the target lane. Consequently, the headway
distance between the SA (blue) and the PAT (red) is always larger than the
lower bound of $15\,\mathrm{m}$.

Right after the end of M-Step 2, the agents’ speed trajectory converge to the
common set point of $14\,\nicefrac{\mathrm{m}}{\mathrm{s}}$. As the maneuver
is performed on a curved road, the wheel steering angle is almost always
different from zero. During the entire maneuver, the agents’ state and input
trajectories are always smooth and within their designated bounds — thus,
satisfying all our requirements.

Finally, the real-time capability of proposed control scheme, according to
requirement R3 (see Section 4), needs to be assessed. Fig. 4 provides an
overview of the agents’ and the coordinator’s execution times (top) and the
required ADMM iterations (bottom). The execution times include the time needed
to solve the local NLPs (solid lines in top plot) as well as the communication
overhead related to the ADMM iterations (shaded area in same color, top plot).
For the SA (blue), the shaded area additionally incorporates the solving time
of the coordination QP. As communication overhead, we assume a maximum end-to-
end latency of 3 ms as it has been specified by the 3rd Generation Partnership
Project (3GPP) for the new 5G communication standard in order to support
advanced driving functions (3GPP.org (2019)).

Fig. 4 provides evidence that our control scheme is real-time capable as the
maximum execution time (including communication overhead) is always below the
sampling time of $100\,\mathrm{ms}$ (black dashed line). Only in the first
time step, i.e., when the distributed problem is initialized, the computation
of a first (initial) solution takes longer than the sampling time, that is,
$131\,\mathrm{ms}$ for the PAT (red). Initially, 9 ADMM iterations are
required for convergence. However, the first time step can be viewed as an
initialization phase which provides a viable initial guess for the second time
step (in a receding horizon manner). During the rest of M-Step 1, we require 1
to 6 ADMM iterations for convergence while execution times for the PAT (red)
and FAT (green) are mostly in a range of 50 ms to 70 ms — with a maximum of
87.5 ms at $t=4.2\,\mathrm{s}$ for the FAT (green). The SA (blue) shows lower
execution times of 15 ms to 50 ms. During M-Step 2, consensus can be
accomplished in a single ADMM step as the original minimum headway distance
constraint (8g) is always satisfied. Thus, computation times are much lower,
that is, these are in a range of 4 ms to 7 ms. Only the SA (blue) temporarily
requires 49 ms to solve its local OCP when the lateral acceleration
constraints become active.

##  6 Conclusion and Future Work

We have introduced a distributed control concept for fully automated lane
change maneuvers of CAVs which is able to accommodate situations when the
traffic is dense and the target lane is already occupied. To this end, we have
embedded the nonconvex problem formulation in a consensus ADMM framework which
has shown convincing performance and real-time capability in our simulation
study. As part of future work, we intend to remove the central coordinator
from the control scheme, to validate the scheme in experiments and to embed it
into a broader motion planning framework.

## References

  * 3GPP.org (2019) 3GPP.org (2019).  Enhancement of 3GPP support for V2X scenarios. 3GPP TS 22.186, V16.2.0.  URL https://portal.3gpp.org /desktopmodules/ Specifications/SpecificationDetails.aspx ?specificationId=3180.
  * An and Jung (2018) An, H. and Jung, J. (2018).  Design of a cooperative lane change protocol for a connected and automated vehicle based on an estimation of the communication delay.  _Sensors_ , 18(10).
  * Bertsekas and Tsitsiklis (1989) Bertsekas, D. and Tsitsiklis, J. (1989).  _Parallel and Distributed Computation: Numerical Methods_.  Prentice Hall.
  * Bevly et al. (2016) Bevly, D., Cao, X., Gordon, M., Ozbilgin, G., Kari, D., Nelson, B., Woodruff, J., Barth, M., Murray, C., Kurt, A., Redmill, K., and Ozguner, U. (2016).  Lane Change and Merge Maneuvers for Connected and Automated Vehicles: A Survey.  _IEEE Transactions on Intelligent Vehicles_ , 1(1), 105–120.
  * Blasi et al. (2018) Blasi, S., Kögel, M., and Findeisen, R. (2018).  Distributed Model Predictive Control Using Cooperative Contract Options.  _IFAC Conference on Nonlinear Model Predictive Control_ , 51(20), 448–454.
  * Bock and Plitt (1984) Bock, H. and Plitt, K. (1984).  A Multiple Shooting Algorithm for Direct Solution of Optimal Control Problems.  _IFAC World Congress_ , 17(2), 1603–1608.
  * Boyd et al. (2011) Boyd, S., Parikh, N., Chu, E., Peleato, B., and Eckstein, J. (2011).  Distributed Optimization and Statistical Learning via the Alternating Direction Method of Multipliers.  _Found. and Trends in Machine Learning_ , 3(1), 1–122.
  * Hong et al. (2016) Hong, M., Luo, Z.Q., and Razaviyayn, M. (2016).  Convergence Analysis of Alternating Direction Method of Multipliers for a Family of Nonconvex Problems.  In _SIAM Journal on Optimization_ , volume 26, 337–364.
  * Hu and Sun (2019) Hu, X. and Sun, J. (2019).  Trajectory optimization of connected and autonomous vehicles at a multilane freeway merging area.  _Transportation Research Part C: Emerging Technologies_ , 101, 111–125.
  * Katriniok et al. (2019) Katriniok, A., Sopasakis, P., Schuurmans, M., and Patrinos, P. (2019).  Nonlinear Model Predictive Control for Distributed Motion Planning in Road Intersections Using PANOC.  In _IEEE Conference on Decision and Control_ , 5272–5278.
  * Liu et al. (2017) Liu, P., Ozguner, U., and Zhang, Y. (2017).  Distributed MPC for cooperative highway driving and energy-economy validation via microscopic simulations.  _Transportation Research Part C: Emerging Techn._ , 77, 80–95.
  * Nocedal and Wright (2006) Nocedal, J. and Wright, S.J. (2006).  _Numerical Optimization_.  Springer, 2nd edition.
  * Qian et al. (2016) Qian, X., de La Fortelle, A., and Moutarde, F. (2016).  A hierarchical Model Predictive Control framework for on-road formation control of autonomous vehicles.  In _IEEE Intelligent Vehicles Symposium_ , 376–381.
  * Rajamani (2012) Rajamani, R. (2012).  _Vehicle Dynamics and Control_ , volume 2.  Springer.
  * Sopasakis et al. (2020) Sopasakis, P., Fresk, E., and Patrinos, P. (2020).  OpEn: Code Generation for Embedded Nonconvex Optimization.  In _IFAC World Congress_.
  * Stella et al. (2017) Stella, L., Themelis, A., Sopasakis, P., and Patrinos, P. (2017).  A simple and efficient algorithm for nonlinear model predictive control.  In _IEEE Conference on Decision and Control_ , 1939–1944.
  * Wang et al. (2016) Wang, D., Hu, M., Wang, Y., Wang, J., Qin, H., and Bian, Y. (2016).  Model predictive control-based cooperative lane change strategy for improving traffic flow.  _Advances in Mechanical Engineering_ , 8(2), 1–17.
  * Wang et al. (2019) Wang, Y., Yin, W., and Zeng, J. (2019).  Global Convergence of ADMM in Nonconvex Nonsmooth Optimization.  _Journal of Scientific Computing_ , 78(1), 29–63.
  * Wang et al. (2017) Wang, Z., Wu, G., and Barth, M. (2017).  Developing a Distributed Consensus-Based Cooperative Adaptive Cruise Control System for Heterogeneous Vehicles with Predecessor Following Topology.  _Journal of Advanced Transportation_.