# Resilience in Platoons of Cooperative Heterogeneous Vehicles: Self-
organization Strategies and Provably-correct Design

Di Liu, Sebastian Mair, Kang Yang, Simone Baldi, Paolo Frasca, and Matthias
Althoff This research was partly supported by the European Union Horizon 2020
research and innovation programme under the Marie Sklodowska-Curie grant
899987, by the Natural Science Foundation of China grant 62073074, and by the
ANR (French National Science Foundation) no. ANR-18- CE40-0010 (HANDY), and by
the European Commission Project justITSELF grant 817629.
D. Liu, S. Mair and M. Althoff are with School of Computation, Information and
Technology, Technical University of Munich (TUM), Germany. D. Liu is also with
Visual Intelligence for Transportation lab (VITA), Ecole Polytechnique
Federale de Lausanne (EPFL), Switzerland (emails: {di.liu, sebi.mair,
<EMAIL_ADDRESS>
K. Yang and S. Baldi are with Frontiers Science Center for Mobile Information
Communication and Security, Southeast University, Nanjing, China. S Baldi is
also guest with Delft Center for Systems and Control, TU Delft, The
Netherlands (emails<EMAIL_ADDRESS><EMAIL_ADDRESS>
P. Frasca is with University of Grenoble Alpes, CNRS, Inria, Grenoble INP,
GIPSA-Lab, Grenoble F-38000, France (e-mail<EMAIL_ADDRESS>
###### Abstract

This work proposes provably-correct self-organizing strategies for platoons of
heterogeneous vehicles. We refer to self-organization as the capability of a
platoon to autonomously homogenize to a common group behavior. We show that
self-organization promotes resilience to _acceleration limits_ and
_communication failures_ , i.e., homogenizing to a common group behavior makes
the platoon recover from these causes of impairments. In the presence of
acceleration limits, resilience is achieved by self-organizing to a common
constrained group behavior that prevents the vehicles from hitting their
acceleration limits. In the presence of communication failures, resilience is
achieved by self-organizing to a common group observer to estimate the missing
information. Stability of the self-organization mechanism is studied
analytically, and correctness with respect to traffic actions (e.g. emergency
braking, cut-in, merging) is realized through a provably-correct safety layer.
Numerical validations via the platooning toolbox OpenCDA in CARLA and via the
CommonRoad platform confirm improved performance through self-organization and
the provably-correct safety layer.

###### Index Terms:

Platooning, cooperative adaptive cruise control, self-organization, group
dynamics, provably-correct design.

##  I Introduction

Platooning refers to automated vehicles following each other at a short
distance by automatically accelerating and braking [1]: pioneering studies
date back to the 70’s [2]. Recognized advantages are better road exploitation
and energy efficiency due to reduced air drag [3, 4]. After initially using
only on-board radar sensing as in Adaptive Cruise Control (ACC) technology, it
was shown that the combination of on-board sensing and inter-vehicle wireless
communication could achieve shorter inter-vehicle distances, leading to
Cooperative Adaptive Cruise Control (CACC) [1, 5]. Inter-vehicle communication
in CACC may involve the vehicle predecessor, the leading vehicle [6], or
k-nearest neighboring vehicles [7, 8].

Resilience of CACC-equipped platoons, i.e., their capacity to recover from
various impairments, has been the subject of several research efforts.
_Acceleration limits_ might impair the formation-keeping task [9, 10], and
approaches to improve resilience make use of cooperation mechanisms [11] or
adaptation mechanisms [12, 13]. _Communication failures_ also cause
impairments in CACC: studies have shown that cyber-attacks [14] or
communication issues [15, 16] deeply affect CACC performance [17]. Approaches
to improve resilience include robust control [18, 19, 20], delay compensation
[21], resource-efficient communication [22, 23], dynamic topology adjustments
[24], and observer design [25]. While several approaches to improve resilience
of CACC have been proposed, we are not aware of provably-correct CACC designs
with respect to given specifications. Previous methods, e.g. fail-safe
planners [26], positive invariant sets [27, 28] or control barrier functions
[29], were only applied to ACC. Specifications arising in lane changing [30],
interaction with human drivers [31], or collision avoidance [32, 33], are
still not ensured in CACC.

This work proposes _self-organization_ in CACC-equipped platoons of
cooperative vehicles, to address causes of impairments like acceleration
limits and communication failures. Our work covers the analytic study of
stability for the self-organization mechanism and its provably-correct design.
The term _self-organization_ refers to the capability of the platoon to
homogenize to a priori unknown common group dynamics, despite the vehicles in
the platoon being heterogeneous. The reason for seeking a homogeneous behavior
is that making all vehicles respond in an analogous way to impairments
promotes resilience of the platoon. Seeds of this idea can be found in
distributed model reference control [34, 35], where heterogeneous systems are
homogenized to the same reference dynamics. Notably, the common group behavior
in our approach is not imposed a priori, but generated autonomously by
vehicles composing the platoon. Our contributions are:

  * a)

We study analytically the stability of the self-organization mechanism. While
standard CACC [11, 13, 17] does not achieve convergence of the spacing errors
to zero when the platoon is composed of heterogeneous vehicles, self-
organization realizes asymptotic convergence, despite the heterogeneity of the
vehicles composing the platoon.

  * b)

We show that self-organization realizes resilience to acceleration limits.
Here, the self-organization mechanism makes all vehicles converge autonomously
to the worst-case acceleration limits in the platoon (constrained group
model). Generating such group model prevents loss of cohesiveness and
collisions, since the leading vehicle can limit its acceleration/deceleration
to prevent all follower vehicles from hitting their acceleration limits;

  * c)

We show that self-organization realizes resilience to wireless communication
failures. Here, the self-organization mechanism makes it possible to design an
observer with common dynamics (group observer). With this observer, each
vehicle reconstructs the missing signal to recover a performance close to the
ideal communication scenario;

  * d)

We combine CACC self-organization with a provably-correct safety layer for the
first time. We provide an implementation in CommonRoad, where self-
organization is integrated with a provably-correct safety layer to handle non-
nominal operation. Our tests confirm the capability of the approach to recover
the formation with convergence of the spacing errors, even in the presence of
the safety layer acting on top of the proposed self-organization mechanism.

The rest of the paper is organized as follows: our solution concept is
presented in Sec. II. Self-organization is achieved and analyzed in Sec. III.
Sec. IV proposes resilient designs based on the self-organization idea.
Effectiveness of the designs is validated in Sec. V. Conclusions are drawn in
Sec. VI.

Figure 1: CACC-equipped platoon: on-board sensing of spacing error and
relative velocity are supplemented by the wireless communication of additional
variables (e.g. acceleration).

##  II CACC system structure and solution concept

Consider a platoon composed of $M$ vehicles (Fig. 1). Let
$S_{M}=\\{i=2,3,\ldots,M\\}$ be the set of following vehicles, with $i=1$
reserved for the leading vehicle. A widely used vehicle model in the CACC
literature [11, 13, 17] is

$\begin{bmatrix}\dot{q_{i}}(t)\vspace{0.15cm}\\\
\dot{v_{i}}(t)\vspace{0.15cm}\\\
\dot{a_{i}}(t)\end{bmatrix}=\begin{bmatrix}v_{i}(t)\vspace{0.05cm}\\\
a_{i}(t)\vspace{-0.25cm}\\\
-\frac{1}{\tau_{i}}a_{i}(t)+\frac{1}{\tau_{i}}(\overbrace{u_{bl_{i}}(t)+u_{hm_{i}}(t)}^{u_{i}(t)})\end{bmatrix},$
(1)

where $q_{i}$ is the position of vehicle $i$, $v_{i}$ is its velocity, and
$a_{i}$ is its acceleration. The control input $u_{i}$ to be designed is a
desired acceleration, which differs from the acceleration $a_{i}$ due to the
engine time constant $\tau_{i}>0$, representing the time required for the
engine to reach the desired acceleration. Because each vehicle has its own
engine time constant $\tau_{i}$, we propose the input
$u_{i}=u_{bl_{i}}+u_{hm_{i}}$, where $u_{bl_{i}}$ is a _baseline_ input
designed according to the standard CACC protocol (cf. Sec. II-A), and
$u_{hm_{i}}$ is an auxiliary input designed to _homogenize_ the vehicle
behavior (cf. Sec. II-B). The aim of homogenization is to promote resilience
by letting all vehicles respond with the same dynamics to impairments.

###  II-A Baseline input

The goal of each vehicle in the platoon, except the leading vehicle, is to
maintain a desired distance with its precedessor. To this purpose, a constant
time headway spacing policy is adopted [11, 13, 17], resulting in the spacing
error:

$\displaystyle e_{i}(t)$ $\displaystyle=q_{i-1}(t)-q_{i}(t)-hv_{i}(t),$
$\displaystyle\dot{e}_{i}(t)$
$\displaystyle=v_{i-1}(t)-v_{i}(t)-ha_{i}(t),\>\>i\in S_{M},$ (2)

where $h>0$ is the time headway constant and $hv_{i}$ is the velocity-
dependent desired distance.

The control objective is to regulate $e_{i}$ to zero. After defining the
relative velocity $\nu_{i}=v_{i-1}-v_{i}$, the following dynamics are obtained
from (1) and (II-A):

$\displaystyle\begin{bmatrix}\dot{e}_{i}(t)\\\ \dot{\nu}_{i}(t)\\\
\dot{a}_{i}(t)\end{bmatrix}$ $\displaystyle=\begin{bmatrix}0&1&-h\\\ 0&0&-1\\\
0&0&-\frac{1}{\tau_{i}}\end{bmatrix}\begin{bmatrix}e_{i}(t)\\\ \nu_{i}(t)\\\
a_{i}(t)\end{bmatrix}$ (3) $\displaystyle\quad+\begin{bmatrix}0\\\ 1\\\
0\end{bmatrix}a_{i-1}(t)+\begin{bmatrix}0\\\ 0\\\
\frac{1}{\tau_{i}}\end{bmatrix}(u_{bl_{i}}(t)+u_{hm_{i}}(t)).$

The control input to regulate the spacing error in standard CACC literature
[11, 13, 17] includes a proportional action from $e_{i}$ and a derivative
action from $\dot{e}_{i}$. A similar control is adopted here as a baseline
input $u_{bl_{i}}$:

$\begin{split}h\dot{u}_{bl_{i}}(t)&=-u_{bl_{i}}(t)+\xi_{i}(t),\\\
\xi_{i}(t)&=\begin{cases}K_{p_{i}}e_{i}(t)+K_{d_{i}}\dot{e}_{i}(t)&\\\
\quad+\xi_{hm_{i}}(t)+u_{bl_{i-1}}(t),\;&i\in S_{M}\\\
u_{r}(t)\;\;\;\;\;\;&i=1\end{cases}\end{split}$ (4)

where $K_{p_{i}}$ and $K_{d_{i}}$ are the proportional and derivative gains,
$u_{bl_{i-1}}$ is the cooperative signal received with wireless communication
between vehicles $i$ and $i-1$, and $u_{r}$ is the desired acceleration of the
leading vehicle. Compared to the standard control input in CACC, a new
auxiliary term $\xi_{hm_{i}}$ is introduced in (4), whose aim is to homogenize
the vehicle behavior, which is otherwise heterogeneous due to different gains
$K_{p_{i}}$ and $K_{d_{i}}$. The homogenizing design for $u_{hm_{i}}$ and
$\xi_{hm_{i}}$ is presented hereafter.

###  II-B Auxiliary homogenizing input

To explain the homogenization idea, assume that, in place of the heterogeneous
dynamics (3)-(4), each vehicle exhibits the following dynamics (referred to as
_homogeneous dynamics_)

$\displaystyle\begin{bmatrix}\dot{e}_{i}(t)\\\ \dot{\nu}_{i}(t)\\\
\dot{a}_{i}(t)\end{bmatrix}$ $\displaystyle=\begin{bmatrix}0&1&-h\\\ 0&0&-1\\\
0&0&-\frac{1}{\tau_{0}}\end{bmatrix}\begin{bmatrix}e_{i}(t)\\\ \nu_{i}(t)\\\
a_{i}(t)\end{bmatrix}$ $\displaystyle\qquad+\begin{bmatrix}0\\\ 1\\\
0\end{bmatrix}a_{i-1}(t)+\begin{bmatrix}0\\\ 0\\\
\frac{1}{\tau_{0}}\end{bmatrix}u_{bl_{i}}(t),$ (5)
$h\dot{u}_{bl_{i}}(t)=-u_{bl_{i}}(t)+K_{p_{0}}e_{i}(t)+K_{d_{0}}\dot{e}_{i}(t)+u_{bl_{i-1}}(t).$
(6)

The difference with (3)-(4) is that $\tau_{0}$, $K_{p_{0}}$ and $K_{d_{0}}$
are homogeneous for all vehicles. Recall that homogeneous vehicle dynamics
make it easier to achieve synchronized platooning behavior [11, 13, 17]. A
natural question arises: _how to converge to the homogeneous dynamics_
(II-B)-(6) starting from the heterogeneous dynamics (3)-(4)? We answer this
question by establishing the following result:

######  Proposition 1.

The auxiliary inputs $u_{hm_{i}}$ in (3) and $\xi_{hm_{i}}$ in (4)
homogenizing any vehicle $i$ to dynamics (II-B) and (6) are

$\displaystyle u_{hm_{i}}(t)$
$\displaystyle=\frac{\tau_{0}-\tau_{i}}{\tau_{0}}(a_{i}(t)-u_{bl_{i}}(t)),$
$\displaystyle\xi_{hm_{i}}(t)$
$\displaystyle=(K_{p_{0}}-K_{p_{i}})e_{i}(t)+(K_{d_{0}}-K_{d_{i}})\dot{e}_{i}(t).$
(7)

###### Proof.

It can be verified by direct substitution of (1) into (3)-(4), which results
in the dynamics (II-B)-(6). ∎

######  Remark 1 (_A priori knowledge_).

The homogenizing inputs (1) require a common set of parameters
$(\tau_{0},K_{p_{0}},K_{d_{0}})$. The main idea of this work, presented in
Sec. III, is to avoid imposing common parameters a priori, but generate a
_homogeneous group behavior_ autonomously.

(a) Interaction between the nominal and the safety controllers.

(b) Statechart of the provably-correct control layer.

Figure 2: Concept of the provably-correct architecture.

###  II-C Provably-correct safety layer

To ensure safety of the CACC, we embed it into a provably-correct safety layer
in line with [26]. The goal is to keep a certain distance to the preceding
vehicle such that safe emergency braking is always possible, even if a
preceding vehicle fully brakes. The functioning of the provably-correct layer
is illustrated in Fig. 2(a), which we explain subsequently. For that, we
assume that the CACC protocol computes inputs $u_{i}$ for discrete,
equidistant points in time $t_{k}=kdt$, where $k\in\mathbb{N}$ is the time
step and $dt$ is the sampling time. In the time interval $[t_{k-1},t_{k}[$,
the CACC computes an input to be applied from time $t_{k}$ on. This input is
simulated for the ego vehicle until time $t_{k+1}$, followed by a user-defined
emergency deceleration profile until standstill. At the same time, a forward
simulation of the preceding vehicle is executed, assuming it applies the
largest possible deceleration. If no collision between the ego vehicle and the
preceding vehicle occurs, the input computed by the CACC is applied during the
interval $[t_{k},t_{k+1}[$. Otherwise, the emergency deceleration profile
starts to be applied from $t_{k}$ on, the safety of which was verified during
the interval $[t_{k-2},t_{k-1}[$.

The architecture also includes a safety-recapturing controller, designed for
the case of cut-ins by other vehicles violating the safe distance between the
ego vehicle and the cut-in vehicle: the recapturing controller ensures that
the safe distance is re-established within a specified time period, assuming
that the cut-in vehicle does not exceed a certain deceleration. We would like
to refer the reader to the concept in Fig. 2(b) or to [26] for details. The
safety layer is implemented in the CommonRoad platform [36], as clarified in
Sec. V.

###  II-D Problem formulation

The goal of this work is to achieve resilience via self-organization. Let us
formulate the objectives as follows:

_Self-organization Problem_ (group behavior): Consider the dynamics (3), (4)
in the presence of heterogeneous engine constants $\tau_{i}$ and heterogeneous
proportional-derivative gains $K_{p_{i}}$, $K_{d_{i}}$. Design the auxiliary
inputs $u_{hm_{i}}$ and $\xi_{hm_{i}}$ in (3), (4) where, differently from
Proposition 1, homogenization to common dynamics $\tau_{0}$, $K_{p_{0}}$,
$K_{d_{0}}$ is achieved autonomously without such dynamics being known a
priori.

_Resilience Problem 1_ (acceleration limits): Consider the dynamics (3) in the
presence of possibly heterogeneous acceleration limits,
$a_{i}(t)\in[a_{min_{i}},a_{max_{i}}]$. Design a suitable group behavior where
each vehicle in the platoon can still maintain the formation without hitting
the acceleration limits.

_Resilience Problem 2_ (communication failures): Consider the control action
(4) with missing $u_{bl_{i-1}}$ due to communication failures. Design a
suitable group behavior where each vehicle in the platoon can still maintain
the formation by reconstructing the missing $u_{bl_{i-1}}$.

##  III Converging to a group behavior

This section contains the main result about convergence of the platoon
dynamics to a group behavior, and stability properties of the resulting
system. Before this, a few preliminaries on consensus dynamics are introduced.

###  III-A Preliminaries on consensus dynamics

To explain how to converge to a common group model, let us define the
following consensus dynamics

$\displaystyle\dot{\bar{K}}_{p\tau_{i}}(t)$
$\displaystyle=\mu_{p}\sum_{j\in\mathcal{N}_{i}}(\bar{K}_{p\tau_{j}}(t)-\bar{K}_{p\tau_{i}}(t)),$
$\displaystyle\dot{\bar{K}}_{d_{i}}(t)$
$\displaystyle=\mu_{d}\sum_{j\in\mathcal{N}_{i}}(\bar{K}_{d_{j}}(t)-\bar{K}_{d_{i}}(t)),$
$\displaystyle\dot{\bar{\tau}}_{i}(t)$
$\displaystyle=\mu_{\tau}\sum_{j\in\mathcal{N}_{i}}(\bar{\tau}_{j}(t)-\bar{\tau}_{i}(t)),$
(8)

with initial conditions

$\displaystyle\bar{K}_{p\tau_{i}}(0)=K_{p\tau_{i}}:=K_{p_{i}}\tau_{i},\
\bar{K}_{d_{i}}(0)=K_{d_{i}},\ \bar{\tau}_{i}(0)=\tau_{i},$ (9)

where $\mu_{p}$, $\mu_{d}$, $\mu_{\tau}>0$ are consensus gains, and
$\mathcal{N}_{i}$ indicates the set of vehicles that vehicle $i$ can
communicate with. The variables $\bar{K}_{p\tau_{i}}$, $\bar{K}_{d_{i}}$,
$\bar{\tau}_{i}$ are _consensus variables_ , whose aim is to converge to
common values for all vehicles. It is well known that convergence depends on
the graph connectivity [37], in line with the following result.

######  Proposition 2 (Average consensus [37]).

For an undirected connected communication graph, the consensus dynamics
(III-A) leads to

$\displaystyle\lim_{t\rightarrow\infty}\bar{K}_{p\tau_{i}}(t)$
$\displaystyle=\frac{1}{M}\sum_{j=1}^{M}K_{p\tau_{j}}=:K_{{p\tau}_{0}},$
$\displaystyle\lim_{t\rightarrow\infty}\bar{K}_{d_{i}}(t)$
$\displaystyle=\frac{1}{M}\sum_{j=1}^{M}K_{d_{j}}=:K_{d_{0}},$
$\displaystyle\lim_{t\rightarrow\infty}\bar{\tau}_{i}(t)$
$\displaystyle=\frac{1}{M}\sum_{j=1}^{M}\tau_{j}=:\tau_{0},$ (10)

exponentially.

The communication graph in Proposition 2 should not be confused with the
inter-vehicle look-ahead interaction graph due to on-board sensing. For
instance, an undirected communication graph arises from bidirectional wireless
communication often considered in CACC [13, 21, 38] where $\mathcal{N}_{i}$
comprises the predecessor and the follower of each vehicle.

###  III-B Self-organizing design and stability analysis

The parameters in (2) have been denoted as $(\tau_{0},K_{p_{0}},K_{d_{0}})$
like the common set of parameters in (II-B)-(6). Note, however, that the
parameters in (2) are a priori unknown to all vehicles in the platoon. In line
with [34], such parameters define an autonomously-generated common group
behavior. To let all vehicles in the platoon homogenize to such common
behavior, the following design for the auxiliary inputs is proposed:

$\displaystyle u_{hm_{i}}(t)$
$\displaystyle=\frac{\bar{\tau}_{i}(t)-\tau_{i}}{\bar{\tau}_{i}(t)}(a_{i}(t)-u_{bl_{i}}(t)),$
$\displaystyle\xi_{hm_{i}}(t)$
$\displaystyle=(\bar{K}_{p_{i}}(t)-K_{p_{i}})e_{i}(t)+(\bar{K}_{d_{i}}(t)-K_{d_{i}})\dot{e}_{i}(t)$
(11)

where $\bar{K}_{p_{i}}(t)=\bar{K}_{p\tau_{i}}(t)/\bar{\tau}_{i}(t)$. The main
difference between (1) and (III-B) is replacing the constants $K_{p_{0}}$,
$K_{d_{0}}$, $\tau_{0}$ with the consensus variables $\bar{K}_{p_{i}}(t)$,
$\bar{K}_{d_{i}}(t)$, $\bar{\tau}_{i}(t)$. A non-trivial question is to
analyze the stability properties resulting from (III-B): in fact, the gains in
(III-B) are time-varying due to (III-A) and result in a nonlinear closed-loop
system.

The following result shows the stability properties of the self-organizing
strategy (III-A), (III-B), i.e. the capability of achieving asymptotic
formation thanks to the common group behavior.

######  Theorem 1.

Consider the platoon composed of vehicle error dynamics (3) and input dynamics
(4). Select the auxiliary inputs as in (III-B) with parameters
$(\bar{\tau}_{i},\bar{K}_{p_{i}},\bar{K}_{d_{i}})$ arising from the consensus
dynamics (III-A). Then, the resulting spacing error $e_{i}$, $i\in S_{M}$
converge to zero as time goes to infinity.

###### Proof.

The proof relies on showing that, although the closed-loop system formed by
(3), (4), (III-A), and (III-B) is nonlinear, it can be written in the form
$\dot{x}(t)=Fx(t)+\tilde{F}(t)x(t)+\hat{F}(t)$, where $F$ is a Hurwitz state
matrix, and $\tilde{F}(t)$, $\hat{F}(t)$ are exponentially vanishing nonlinear
terms acting as disturbances (the state $x$ will be defined later on). For
this type of dynamics, one can use the result in [34, Lemma 1] stating that
the exponentially vanishing terms still allow exponential stability of state
$x$.

First, consider vehicle 1: because vehicle 1 has no preceding vehicle, its
dynamics do not contain any spacing error $e_{1}$ and relative velocity
$\nu_{1}$. Only the dynamics of $x_{1}=[a_{1}\ u_{bl_{1}}]^{T}$ must be
calculated. By using the last equations in (3), (4), and the first equation in
(III-B), we get the dynamics in the form

$\displaystyle\dot{x}_{1}(t)=F_{1}x_{1}(t)+\tilde{F}_{1}(t)x_{1}(t)+\hat{F}_{1}(t),$
(12)

with the Hurwitz state matrix

$\displaystyle F_{1}=\begin{bmatrix}-\frac{1}{\tau_{0}}&\frac{1}{\tau_{0}}\\\
0&-\frac{1}{h}\end{bmatrix}$ (13)

and

$\displaystyle\tilde{F}_{1}(t)=\begin{bmatrix}-\frac{\tilde{\tau}_{1}(t)}{\tau_{0}(\tau_{0}-\tilde{\tau}_{1}(t))}&\frac{\tilde{\tau}_{1}(t)}{\tau_{0}(\tau_{0}-\tilde{\tau}_{1}(t))}\\\
0&0\end{bmatrix},\ \hat{F}_{1}(t)=\begin{bmatrix}0\\\
\frac{u_{r}(t)}{h}\end{bmatrix}.$ (14)

For the other vehicles, we define $x_{i}=[e_{i}\ \nu_{i}\ a_{i}\
u_{bl_{i}}]^{T}$ and proceed by combining the platooning system (3)-(4) with
the auxiliary inputs (III-B), so as to obtain the form

$\displaystyle\dot{x}_{i}(t)=Fx_{i}(t)+\tilde{F}_{i}(t)x_{i}(t)+\hat{F}_{i}(t),$
(15)

with

$\displaystyle F=\begin{bmatrix}0&1&-h&0\\\ 0&0&-1&0\\\
0&0&-\frac{1}{\tau_{0}}&\frac{1}{\tau_{0}}\\\
\frac{K_{p_{0}}}{h}&\frac{K_{d_{0}}}{h}&-K_{d_{0}}&-\frac{1}{h}\end{bmatrix},$
(16) $\displaystyle\tilde{F}_{i}(t)=\begin{bmatrix}0&0&0&0\\\ 0&0&0&0\\\
0&0&\frac{-\tilde{\tau}_{i}(t)}{\tau_{0}(\tau_{0}-\tilde{\tau}_{i}(t))}&\frac{\tilde{\tau}_{i}(t)}{\tau_{0}(\tau_{0}-\tilde{\tau}_{i}(t))}\\\
\frac{\tilde{K}_{p_{i}}(t)}{h}&\frac{\tilde{K}_{d_{i}}(t)}{h}&-\tilde{K}_{d_{i}}(t)&0\end{bmatrix},\
$ (17)

and $\hat{F}_{i}(t)=[0\ a_{i-1}(t)\ 0\ \frac{u_{bl_{i-1}}(t)}{h}]^{T}$. Note
that $F$ is homogeneous for all vehicles and is Hurwitz as a result of the
Routh-Hurwitz conditions [39, Sec. 5.5.3]

$h>0,\quad K_{p_{0}},K_{d_{0}}>0,\quad K_{d_{0}}>\tau_{0}K_{p_{0}}.$ (18)

We refer the reader to Remark 2 for details on why (18) holds for the group
parameters, given that the controller of each vehicle is stable, i.e. given
that the Routh-Hurwitz conditions $K_{p_{i}},K_{d_{i}}>0$,
$K_{d_{i}}>\tau_{i}K_{p_{i}}$ hold individually.

To derive exponential stability from [34, Lemma 1], let us start with (15) in
the absence of exogenous input $u_{r}$, i.e. $\hat{F}_{1}=0$. Noticing that
the term $\tilde{\tau}_{1}=\tau_{0}-\tau_{1}$ converges to zero exponentially
due to Proposition 2, we have that $\tilde{F}_{i}$ in (14) is exponentially
vanishing. Thus, exponential stability of $x_{1}$ is concluded. At this point,
one can proceed iteratively from (15) for all vehicles $i=2,3,\ldots$. We have
that that $\tilde{F}_{i}$ in (17) converges to zero exponentially because
$(\tilde{\tau}_{i},\tilde{K}_{p_{i}},\tilde{K}_{d_{i}})$ converge to zero
exponentially due to Proposition 2. Meanwhile, $\hat{F}_{i}$ also converges to
zero exponentially because exponential convergence of $a_{i-1}$,
$u_{bl_{i-1}}$ has been previously shown.

In other words, we obtain that the preceding vehicle $i-1$ affects vehicle $i$
with an exponentially vanishing disturbance, leading to exponential stability
of $x_{i}$ for any vehicle in the platoon, which concludes the proof. ∎

######  Remark 2 (_Stability and string stability_).

Note that, when each vehicle satisfies the Routh-Hurwitz condition
$K_{d_{i}}>\tau_{i}K_{p_{i}}=K_{p\tau_{i}}$, we have
$\sum_{i}K_{d_{i}}>\sum_{i}K_{p\tau_{i}}$, i.e. the Routh-Hurwitz condition is
satisfied on average. A common stabilizing gain for the group model is
obtained via $\bar{K}_{p_{i}}(t)=\bar{K}_{p\tau_{i}}(t)/\bar{\tau}_{i}(t)$. In
addition, as the transfer function from $a_{i-1}$ to $a_{i}$ for homogeneous
vehicle dynamics is $\Gamma(s)=1/(hs+1)$ [24], we achieve the standard notion
of string stability

$\left|\Gamma(j\omega)\right|\leq 1,\forall\omega\geq 0.$ (19)

In other words, convergence to a homogeneous group model leads to a stable and
string stable behavior.

##  IV Self-organization for resilient design

Having shown the properties of self-organization, we are in a position to use
group behavior ideas to solve the two resilience problems in Sec. II-D.
Specifically, we will design a _constrained group model_ to face acceleration
limits, and a _group observer model_ to face communication failures.

###  IV-A Resilient design to acceleration limits

We refer the reader to Sec. V-B for an example of the problems that
acceleration limits can create. Denote with $a_{max_{i}}$ the maximum
acceleration of vehicle $i$ and with $a_{min_{i}}$ its minimum acceleration
(maximum deceleration). We will design a resilient mechanism based on a simple
idea: limiting the behavior of the leading vehicle (i.e. limit its
acceleration/deceleration) avoids that the follower vehicles hit their limits.
Stemming from this idea, we need to find the worst-case acceleration limits in
the platoon, i.e. the minimum among the maximum accelerations, and the maximum
among the minimum accelerations. To this purpose, we use the max-min consensus
[40], aiming to find the minimum and the maximum value in a network in a
distributed way

$\displaystyle\dot{\bar{a}}_{max_{i}}(t)=\operatorname{sgn}_{-}\left(\sum_{j\in\mathcal{N}_{j}}\left(\bar{a}_{max_{j}}(t)-\bar{a}_{max_{j}}(t)\right)\right)$
$\displaystyle\dot{\bar{a}}_{min_{i}}(t)=\operatorname{sgn}_{+}\left(\sum_{j\in\mathcal{N}_{j}}\left(\bar{a}_{min_{j}}(t)-\bar{a}_{min_{i}}(t)\right)\right),$
(20)

where

$\displaystyle\operatorname{sgn}_{+}(x)=\begin{cases}0&{\rm if}\;x\leq 0\\\
1&{\rm if}\;x>0\end{cases},\ \operatorname{sgn}_{-}(x)=\begin{cases}0&{\rm
if}\;x\geq 0\\\ 1&{\rm if}\;x<0\end{cases}$ (21)

and with initial conditions

$\bar{a}_{max_{i}}(0)=a_{max_{i}},\quad\bar{a}_{min_{i}}(0)=a_{min_{i}}.$

In other words, $\bar{a}_{max_{i}}$ and $\bar{a}_{min_{i}}$ represent the
estimates of the maximum and minimum acceleration in the platoon according to
vehicle $i$. Clearly, we want such estimates to converge consistently along
the platoon, i.e. $\bar{a}_{max_{i}}$ should converge to a common value for
all vehicles, and analogously for $\bar{a}_{min_{i}}$. The following
convergence result holds.

######  Proposition 3 (Max-min consensus [40]).

For an undirected connected communication graph, the dynamics (IV-A) lead to

$\displaystyle\lim_{t\rightarrow\infty}\bar{a}_{max_{i}}(t)$
$\displaystyle=\min_{i}\left[a_{max_{i}}\right]=:a_{max_{0}}$
$\displaystyle\lim_{t\rightarrow\infty}\bar{a}_{min_{i}}(t)$
$\displaystyle=\max_{i}\left[a_{min_{i}}\right]=:a_{min_{0}}$ (22)

in finite time.

######  Remark 3 (No a priori knowledge of the group model).

Albeit having different dynamics, the consensus mechanisms (III-A) and (IV-A)
obey a similar philosophy: the variables
$(\bar{a}_{min_{i}},\bar{a}_{max_{i}})$ are initialized according to the
vehicle parameters and shared in a distributed way among neighboring vehicles,
until convergence to common values $a_{max_{0}}$, $a_{min_{0}}$ in (3). These
common values, which are a priori unknown, can be regarded as an autonomously
generated _constrained common group model_.

Once the worst-case acceleration limits in the platoon are available, the
desired acceleration can be limited within the same acceleration constraints
arising from the constrained common group model. Specifically, one can modify
the control input (4) as:

$h\dot{u}_{bl_{i}}=\begin{cases}0&{\rm if}\ u_{i}=\bar{a}_{max_{i}}\ {\rm
and}\ -u_{bl_{i}}+\xi_{i}\geq 0\vspace{-0.3cm}\\\ \\\
-u_{bl_{i}}+\xi_{i}&{{\rm if}\ \bar{a}_{min_{i}}<u_{i}<\bar{a}_{max_{i}}}\\\
&\ {\rm\textbf{or}}\ u_{i}=\bar{a}_{max_{i}}\ {\rm and}\
-u_{bl_{i}}+\xi_{i}<0\\\ &\ {\rm\textbf{or}}\ u_{i}=\bar{a}_{min_{i}}\ {\rm
and}\ -u_{bl_{i}}+\xi_{i}>0\vspace{-0.3cm}\\\ \\\ 0&{\rm if}\
u_{i}=\bar{a}_{min_{i}}\ {\rm and}\ -u_{bl_{i}}+\xi_{i}\leq 0.\end{cases}$
(23)

Analogously, since the worst-case bounds $\bar{a}_{max_{i}}$,
$\bar{a}_{min_{i}}$ can be applied also to the acceleration, one can constrain
the acceleration within the same limits, that is
$a_{i}\in[\bar{a}_{min_{i}},\bar{a}_{max_{i}}]$. The proposed resilient
approach can be sketched  via the following procedure:

Resilient Strategy 1 (against acceleration limits)

1:initial conditions $a_{max_{i}}$, $a_{min_{i}}$.

2:the constrained control (23) with constrained acceleration
$a_{i}\in[\bar{a}_{min_{i}},\bar{a}_{max_{i}}]$.

3:Online, at time $t$: Run the max-min consensus dynamics (IV-A) to get
$\bar{a}_{min_{i}}(t)$, $\bar{a}_{max_{i}}(t)$;

4:Online, at time $t$: use $\bar{a}_{min_{i}}(t)$, $\bar{a}_{max_{i}}(t)$ to
realize the constrained input (23) and the constrained acceleration
$a_{i}\in[\bar{a}_{min_{i}}(t),\bar{a}_{max_{i}}(t)]$.

###  IV-B Resilient design to communication failures

Without communication, the CACC law in (4) cannot make use of $u_{bl_{i-1}}$,
i.e., the control input degenerates to a standard ACC [1, 5] where only on-
board radar sensing is used:

$h\dot{u}_{bl_{i}}(t)=-u_{bl_{i}}(t)+K_{p_{0}}e_{i}(t)+K_{d_{0}}\dot{e}_{i}(t).$
(24)

We refer the reader to Sec. V-B for an example showing the adverse effects of
losing communication. To obtain resilience, we propose an observer to estimate
the missing $u_{bl_{i-1}}$. As communication is inactive, let us assume that
all vehicles already homogenized by means of (III-A) when communication was
active, so that the vehicle dynamics have converged to $K_{p_{0}}$,
$K_{d_{0}}$, $\tau_{0}$ in (2). The observer design follows three steps:

  * 1)

Forming a model for the homogenized dynamics of vehicle $i-1$, i.e. the
preceding vehicle;

  * 2)

Designing a high-gain observer for some auxiliary variables of the homogenized
preceding vehicle;

  * 3)

Designing a sliding-mode observer for the missing $u_{bl_{i-1}}$.

Thanks to self-organization (III-A), all preceding vehicles $i-1$ exhibit the
same the homogenized dynamics: as a result, all vehicles design the same
observer based on the same preceding vehicle dynamics. This concept would not
be applicable in ACC with heterogeneous vehicles, unless assuming the platoon
to be homogeneous or proceeding at constant speed [25, 20]. The observer
design below does not require these assumptions.

####  IV-B1 Dynamics of the homogenized preceding vehicle

As all vehicles have converged to homogeneous dynamics via (III-A), using
(II-B)-(6) with index $i-1$, one obtains the homogenized dynamics of vehicle
$i-1$ as:

$\displaystyle\begin{bmatrix}\dot{v}_{i-1}(t)\\\ \dot{a}_{i-1}(t)\\\
\dot{u}_{bl_{i-1}}(t)\end{bmatrix}$
$\displaystyle=\underbrace{\begin{bmatrix}0&1&0\\\
0&-\frac{1}{\tau_{0}}&\frac{1}{\tau_{0}}\\\
-K_{p_{0}}-\frac{K_{d_{0}}}{h}&-K_{d_{0}}&-\frac{1}{h}\end{bmatrix}}_{A_{o}}\underbrace{\begin{bmatrix}v_{i-1}(t)\\\
a_{i-1}(t)\\\ u_{bl_{i-1}}(t)\end{bmatrix}}_{x_{o_{i-1}}(t)}\vspace{-0.8cm}$
(25) $\displaystyle+\underbrace{\begin{bmatrix}0\\\ 0\\\
\frac{1}{h}\end{bmatrix}}_{B_{o}}\underbrace{\begin{bmatrix}K_{p_{0}}(q_{i-2}(t)-q_{i-1}(t))+K_{d_{0}}v_{i-2}(t)+u_{bl_{i-2}}(t)\end{bmatrix}}_{\nu_{o_{i-1}}(t)},$

where subscript $o$ refers to the _observer_. Among the components of
$x_{o_{i-1}}$, only $v_{i-1}$ is measurable to vehicle $i$ from on-board
sensors (by adding the tachometer measurement $v_{i}$ to the radar measurement
$v_{i-1}-v_{i}$): $a_{i-1}$ and $u_{bl_{i-1}}$ are not available during
failures. The quantity $\nu_{o_{i-1}}$ in (25) must be regarded as an _unknown
input_ as it contains signals not available to vehicle $i$. Therefore, vehicle
$i$ should observe (i.e. reconstruct) the state in (25) using available
measurements. Due to the unknown input term $\nu_{o_{i-1}}$ in (25), the type
of observer we seek is an _unknown input observer_. Specifically, we design a
sliding-mode observer, in line with [41].

####  IV-B2 Auxiliary variables

In the state $x_{o_{i-1}}$ in (25), the only measurement available to vehicle
$i$ is

$\displaystyle v_{i-1}(t)=H_{o}x_{o_{i-1}}(t),$ (26)

where $H_{o}=[1\ \ 0\ \ 0]$. However, the dynamics (25) do not satisfy the
observer matching condition needed for standard unknown input observer design
[41], i.e. we have

$\displaystyle\operatorname{rank}(B_{o})\neq\operatorname{rank}(B_{o}H_{o}).$
(27)

This implies that the measurement $v_{i-1}$ in (27) alone is not enough to
reconstruct the missing signal $u_{bl_{i-1}}$. To this purpose, let us combine
$H_{o}$ in (27) with the auxiliary matrix

$\displaystyle C_{o}=\begin{bmatrix}H_{o}\\\ H_{o}A_{o}\\\
H_{o}A_{o}^{2}\end{bmatrix}=\begin{bmatrix}1&0&0\\\ 0&1&0\\\
0&-\frac{1}{\tau_{0}}&\frac{1}{\tau_{0}}\end{bmatrix}.$ (28)

Note that the variables $C_{o}x_{o_{i-1}}$ defined by (28) are $v_{i-1}$,
$a_{i-1}$, $j_{i-1}$, i.e. the velocity, acceleration and jerk of vehicle
$i-1$. The physics interpretation of these variables allows a more direct
design than [41], Namely, the chain-of-integrators structure of these
variables allows to design the high-gain observer

$\displaystyle\begin{bmatrix}\dot{\bar{v}}_{i-1}(t)\\\
\dot{\bar{a}}_{i-1}(t)\\\
\dot{\bar{j}}_{i-1}(t)\end{bmatrix}=\begin{bmatrix}0&1&0\\\ 0&0&1\\\
0&0&0\end{bmatrix}\begin{bmatrix}\bar{v}_{i-1}(t)\\\ \bar{a}_{i-1}(t)\\\
\bar{j}_{i-1}(t)\end{bmatrix}+\begin{bmatrix}\frac{\alpha_{0}}{\epsilon}\\\
\frac{\alpha_{1}}{\epsilon^{2}}\\\
\frac{\alpha_{2}}{\epsilon^{3}}\end{bmatrix}(v_{i-1}(t)-\bar{v}_{i-1}(t)),$
(29)

where the state comprises the reconstructed $v_{i-1}$, $a_{i-1}$, $j_{i-1}$;
$\epsilon>0$ is a small design constant; $\alpha_{0}$, $\alpha_{1}$,
$\alpha_{2}$ are selected to make the observer error dynamics stable, i.e. the
polynomial $\lambda^{3}+\alpha_{0}\lambda^{2}+\alpha_{1}\lambda+\alpha_{2}$ is
Hurwitz. The state of the high-gain observer (29) is now used to reconstruct
the missing $u_{bl_{i-1}}$.

####  IV-B3 Observer for missing signal $u_{bl_{i-1}}$

The unknown input observer is in the sliding-mode form

$\displaystyle\dot{\hat{x}}_{o_{i-1}}(t)$
$\displaystyle=A_{o}\hat{x}_{o_{i-1}}(t)+B_{o}E_{o}(t)$
$\displaystyle\qquad+L_{o}(\bar{y}_{{i-1}}(t)-\hat{y}_{i-1}(t)),$ (30)

where $\hat{y}_{i-1}(t)=C_{o}\hat{x}_{o_{i-1}}(t)$ are the reconstructed
auxiliary variables, $L_{o}$ is such that $(A_{o}-L_{o}C_{o})$ is Hurwitz, and

$\displaystyle\hat{x}_{o_{i-1}}(t)=\begin{bmatrix}\hat{v}_{i-1}(t)\\\
\hat{a}_{i-1}(t)\\\ \hat{u}_{bl_{i-1}}(t)\end{bmatrix},\
\bar{y}_{{i-1}}(t)=\begin{bmatrix}v_{i-1}(t)\\\
S_{a}\operatorname{sat}(\frac{\bar{a}_{i-1}(t)}{S_{a}})\\\
S_{j}\operatorname{sat}(\frac{\bar{j}_{i-1}(t)}{S_{j}})\end{bmatrix},$ (31)
$\displaystyle E_{o}(t)=$ (32) $\displaystyle\ \
\left\\{\begin{array}[]{ll}\eta\frac{F_{o}(\bar{y}_{{i-1}}(t)-\hat{y}_{i-1}(t))}{\left\|\bar{y}_{{i-1}}(t)-\hat{y}_{i-1}(t)\right\|}&{\rm
if}\;F_{o}(\bar{y}_{{i-1}}(t)-\hat{y}_{i-1}(t))\neq 0\\\ 0&{\rm
otherwise,}\end{array}\right.$ (35)

with $\operatorname{sat}(\cdot)$ being a standard saturation function in the
range $\pm 1$. In (31)-(32), $\eta>0$ is a sliding mode gain, $S_{a}$,
$S_{j}>0$ are bounds for maximum acceleration and jerk in the platoon, and
$F_{o}$ is designed such that

$\displaystyle(A_{o}-L_{o}C_{o})^{T}P_{o}+P_{o}(A_{o}-L_{o}C_{o})=-2Q_{o},$
$\displaystyle F_{o}C_{o}=B_{o}^{T}P_{o},\quad P_{o},Q_{o}>0,$ (36)

for some $Q_{o}>0$. Using the notation $p_{o_{ij}}$ for the entry $(i,j)$ of
$P_{o}\in\mathbb{R}^{3\times 3}$, it is obtained from direct calculation of
$F_{o}C_{o}=B_{o}^{T}P_{o}$ in (IV-B3) that $F_{o}\in\mathbb{R}^{1\times 3}$
takes the form

$\displaystyle
F_{o}=\begin{bmatrix}\frac{p_{o_{13}}}{h}&\frac{p_{o_{32}}+p_{o_{33}}}{h}&\frac{p_{o_{33}}}{h}\tau_{0}\end{bmatrix},$
(37)

to be used in the observer (32). The following result holds.

######  Proposition 4 (Unknown input observer [41]).

For the vehicle dynamics (25), the observer (29)-(IV-B3) guarantees that there
exists a constant $\epsilon^{*}\in(0,1)$ such that, if
$\epsilon\in(0,\epsilon^{*})$ and $\eta\geq\left|\nu_{o_{i-1}}(t)\right|$,
$\forall t$, then the state estimation error
$x_{o_{i-1}}(t)-\hat{x}_{o_{i-1}}(t)$ is uniformly ultimately bounded by a
function $\kappa(\epsilon)$ that satisfies $\kappa(\epsilon)\rightarrow 0$
when $\epsilon\rightarrow 0$.

(a) Velocity (upper plot) and acceleration (lower plot) for all
vehicles. Note the effect of noise on acceleration.

(b) Spacing error (upper plot) and distance to leading vehicle
(lower plot). The spacing error does not converge.

Figure 3: Platoon without self-organizing strategy.

(a) Velocity (upper plot) and acceleration (lower plot) for all
vehicles. The effect of noise on acceleration reduces.

(b) Spacing error (upper plot) and distance to leading vehicle
(lower plot). The spacing error is kept closer to zero.

Figure 4: Platooning with self-organizing strategy.

Proposition 4 suggests that the control input is finally

$h\dot{u}_{bl_{i}}(t)=-u_{bl_{i}}(t)+K_{p_{0}}e_{i}(t)+K_{d_{0}}\dot{e}_{i}(t)+\hat{u}_{bl_{i-1}}(t),$
(38)

where the missing $u_{bl_{i-1}}(t)$ has been replaced by
$\hat{u}_{bl_{i-1}}(t)$.

######  Remark 4 (_Homogeneous a priori unknown observer_).

Each vehicle $i$ does not use the heterogeneous parameters $\tau_{i-1}$,
$K_{p_{i-1}}$, $K_{d_{i-1}}$ of the preceding vehicle $i-1$ to design the
observer, but homogenized preceding vehicle dynamics defined by
$(A_{o},B_{o})$ in (25). As $(A_{o},B_{o})$ stems from the common group
behavior $(K_{p_{0}},K_{d_{0}},\tau_{0})$, each vehicle ends up designing the
same observer, regarded as a _common group observer_.

The proposed resilient approach can be sketched along the aforementioned three
steps as:

Resilient Strategy 2 (against communication failures)

1:a priori unknown ($K_{p_{0}}$, $K_{d_{0}}$, $\tau_{0}$) from convergence of
(III-A); Lyapunov equation matrix $Q_{o}$ and observer gain $L_{o}$;
sliding gain $\eta$ and high-gain observer gains $\epsilon$, $\alpha_{0}$,
$\alpha_{1}$, $\alpha_{2}$.

2:control (38), with the reconstructed $\hat{u}_{bl_{i-1}}(t)$.

3:Offline: form a copy of the homogenized preceding vehicle matrices
$(A_{o},B_{o})$ as in (25);

4:Online, at time $t$: reconstruct $v_{i-1}(t)$, $a_{i-1}(t)$, $j_{i-1}(t)$ as
$\bar{v}_{i-1}(t)$, $\bar{a}_{i-1}(t)$, $\bar{j}_{i-1}(t)$ via the high-gain
observer (29);

5:Online, at time $t$: reconstruct the missing $u_{bl_{i-1}}(t)$ as
$\hat{u}_{bl_{i-1}}(t)$ via the observer (IV-B3), and use
$\hat{u}_{bl_{i-1}}(t)$ in (38).

##  V Numerical validations

The numerical validations are organized as follows. Sec. V-A validates the
benefits of self-organization, while Sec. V-B validates the proposed resilient
designs based on self-organization. Secs. V-C and V-D provide implementations
of the proposed ideas in CommonRoad [36] and CARLA [42]: the former tests
full-range operation by means of the safety architecture in Sec. II-C; the
latter tests the proposed methods with the vehicle/sensor models available in
CARLA. Sec. V-E considers cut-in and merging scenarios, to validate the
effectiveness in more complex traffic scenarios. Numerical validations are
carried out in presence of zero-mean Gaussian noise to simulate radar
measurement noise (variance 0.025 m2/s2), tachometer measurement noise
(variance 0.25 m2/s2) and accelerometer measurement noise (variance 0.1
m4/s4). These noises prevent ideal asymptotic convergence of the errors, but
allow to validate the proposed strategies in a more realistic setting and to
test sensitivity of controllers and observers to noise.

(a) Velocity (upper plot) and acceleration (lower plot) for all
vehicles. The platoon cannot follow the leader.

(b) Spacing error (upper plot) and distance to leading vehicle
(lower plot). A collision happens in the platoon.

Figure 5: Acceleration limits, without resilient strategy.

(a) Velocity (upper plot) and acceleration (lower plot) for all
vehicles. The platoon follows the leader.

(b) Spacing error (upper plot) and distance to leading vehicle (lower plot).
The spacing error is regulated despite the acceleration limits.

Figure 6: Acceleration limits, proposed resilient strategy.

(a) Velocity (upper plot) and acceleration (lower plot) for all
vehicles. Note the large velocity and acceleration transients.

(b) Spacing error (upper plot) and distance to leading vehicle
(lower plot). The spacing error is not regulated close to zero.

Figure 7: Communication failures, without resilient strategy.

(a) Velocity (upper plot) and acceleration (lower plot) for all
vehicles, with observer estimates in red in the small box.

(b) Spacing error (upper plot) and distance to leading vehicle
(lower plot). Close-to-ideal performance is recovered.

Figure 8: Communication failures, proposed resilient strategy.

###  V-A Validation of self-organization

Let us consider a CACC-equipped platoon of one leading and five following
vehicles, with headway $h=0.7$ and heterogeneous parameters in as Tab. I.
First, let us keep the proposed self-organizing strategy (III-A), (III-B) off
($u_{hm_{i}}=\xi_{hm_{i}}=0$), i.e. every vehicle keeps its heterogeneous
dynamics. We let the leader first accelerate, then decelerate. The results are
in Figs. 3(a) and 3(b): although all vehicles manage to follow the velocity
and acceleration of the leader, the absence of self-organization results in
non-converging spacing errors due to heterogeneity. Recall that the distance
to the leading vehicle is velocity-dependent, as a result of having $hv_{i}$
in the constant time headway policy (II-A).

TABLE I: Heterogeneous parameters of the platoon. | 1 | 2 | 3 | 4 | 5 | 6
---|---|---|---|---|---|---
$\tau_{i}$ | 0.10 | 0.20 | 0.05 | 0.30 | 0.15 | 0.075
$K_{p_{i}}$ | 0.20 | 0.10 | 0.40 | 0.067 | 0.133 | 0.267
$K_{d_{i}}$ | 0.70 | 0.35 | 1.40 | 0.23 | 0.467 | 0.933

For the same leader acceleration/deceleration scenario, let us now switch the
proposed self-organizing strategy (III-A), (III-B) on, with
$\mu_{p}=\mu_{d}=\mu_{\tau}=0.2$. Fig. 4(a) shows that all vehicles manage to
follow the velocity and acceleration of the leader, the effect of noise on
acceleration being smaller than Fig. 3(a). Meanwhile, Fig. 4(b) shows that the
spacing error is kept closer to zero as compared to Fig. 3(b).

###  V-B Resilience to acceleration limits and communication failures

To clarify the adverse effect of acceleration limits towards formation
keeping, consider the heterogeneous acceleration limits in Tab. II. As in Sec.
V-A, the leader first accelerates, then decelerates, but this time we chop the
acceleration within $\pm 0.425$ [m/s2] due to the leader acceleration limits.

Fig. 5(a) shows that several followers with even tighter acceleration limits
than the leader are not able to follow the leader velocity and acceleration.
As a result, Fig. 5(b) shows that the platoon first loses cohesiveness during
the acceleration phase, and then a collision happens during the deceleration
phase. This can be seen from the relative distance crossing zero at $t\approx
110$s. The collision is caused by the fact that most vehicles cannot follow
the deceleration of the leader due to their deceleration limits.

TABLE II: Heterogeneous limits of the platoon. | 1 | 2 | 3 | 4 | 5 | 6
---|---|---|---|---|---|---
$a_{max_{i}}$ | 0.425 | 0.35 | 0.375 | 0.40 | 0.325 | 0.45
$a_{min_{i}}$ | -0.425 | -0.35 | -0.375 | -0.40 | -0.325 | -0.45

With the same acceleration limits of Tab. II, Fig. 6(a) demonstrates the
effectiveness of the proposed resilient strategy against acceleration limits:
all vehicles manage to follow the velocity and acceleration of the leader. Of
course, this is done at the expense of reduced performance. Indeed, the
platoon reaches lower velocity (25m/s in Fig. 6(a) compared to 32m/s in Fig.
4(a)), but this is not a surprise: it was already shown in [9, 10] that
reducing performance is necessary to keep the formation, since it avoids that
the vehicles hit their acceleration limits. Fig. 6(b) shows that the spacing
error is regulated in a way similar to Fig. 4(b), despite no acceleration
limits were present in the latter. Therefore, it is confirmed that self-
organization to a constrained group model promotes resilience to acceleration
limits, since even the vehicle with the worst acceleration limits can follow
the behavior of the leader.

To illustrate the adverse effects of losing communication, let us remove
acceleration limits for simplicity. Also, to make the effect of losing
$u_{bl_{i-1}}$ as in (24) more evident, we make the leader
accelerate/decelerate with sharp piecewise constant phases. Fig. 7(a) shows
that communication failures result in poor vehicle-following capabilities,
with transients in acceleration and velocity, and with spacing errors that
cannot converge and grow in the order of 5 meters (Fig. 7(b)).

(a) Spacing error and distance to leader without self-organization.

(b) Spacing error and distance to leader with self-organization.

(c) Snapshots from CommonRoad without self-organization.

(d) Snapshots from CommonRoad with self-organization.

Figure 9: Spacing error, distance to leading vehicle, and snapshots from
CommonRoad, with and without self-organizing strategy, in the presence of
heterogeneous acceleration limits. Our provably-correct safety layer is
included. Self-organization allows the spacing errors to converge to zero.

(a) Emergency braking without safety layer.

(b) Emergency braking with safety layer.

(c) Snapshots from CommonRoad without safety layer.

(d) Snapshots from CommonRoad with safety layer.

Figure 10: Spacing error, distance to leading vehicle, and snapshots from
CommonRoad, with and without safety layer, for an emergency braking scenario.
When present, the provably-correct safety layer allows to avoid the crash. The
spacing policy is set to be small on purpose, to test safety in challenging
scenarios.

To demonstrate the benefits of the proposed resilient strategy (29)-(37), we
apply the proposed observer. The observer design parameters are
$S_{a}=S_{j}=1$, $\epsilon=0.01$, $\eta=1.5$,
$Q_{o}=\operatorname{diag}\left\\{0.1,0.2,0.01\right\\}$, $\alpha_{0}=3$,
$\alpha_{1}=0.2$, $\alpha_{2}=0.01$, and $L_{o}$ selected to place the poles
of $A_{o}-L_{o}C_{o}$ in $-5$ and $-1.5\pm j0.5$. For the leader behavior as
before, Fig. 8(a) shows that all vehicles manage to follow the velocity and
acceleration of the leader (the red lines represent the observed variables
from the observer). Notably, comparing the acceleration noise in Fig. 8(a)
with the noise in previous scenarios, e.g. Figs. 4(b) and 6(b), it can be
noticed that the observer does not amplify the measurement noises in the
vehicle behavior. Remarkably, Fig. 8(b) shows that the spacing errors are kept
close to zero, except for a small observer transient when the leader switches
from acceleration to deceleration. Note that the transients in Fig. 8(b) are
much smaller than those in Fig. 7(b), making the performance of the proposed
resilient design in Fig. 8 clearly superior to that of Fig. 7.

###  V-C Full-range safe-CACC validation with CommonRoad

We validate the provably-safe control layer mentioned in Sec. II-B via
CommonRoad [36]. CommonRoad is a collection of composable benchmarks for
motion planning on roads, which provides a means of evaluating and comparing
different control strategies for automated vehicles. To evaluate the impact of
self-organization, we perform tests with and without the self-organization
mechanism: in these tests, the safety layer is wrapped around the nominal
platooning strategy, as discussed in Sec. II-B. We test the presence of
heterogeneous acceleration limits for a leader behavior resembling a stop-and-
go behavior: the spacing errors in Fig. 9(a) show that the absence of self-
organization causes spacing errors in the order of 10 meters, indicating poor
platooning performance. Upon activating the proposed self-organization
strategy, Fig. 9(b) shows that the spacing errors are small and converge close
to zero. Snapshots from CommonRoad are shown in Fig. 9(c) (without self-
organization) and Fig. 9(d) (with self-organization): note that the proposed
self-organization strategy promotes a homogeneous behavior for all vehicles
during the whole platooning operation, despite the heterogeneity in the
vehicle dynamics.

To test the impact of the safety layer we have created on purpose a
challenging emergency-braking scenario: the scenario is challenging because
the desired inter-vehicle spacing is set to be small ($h=0.3$). By doing this,
strong braking of the leading vehicle would activate the safety layer. Fig.
10(a) shows that the absence of a safety layer can indeed lead to a crash, cf.
the snapshots from CommonRoad in Fig. 10(c). However, when the safety layer is
active, it promptly activates emergency braking to avoid collision, as shown
in Fig. 10(d).

TABLE III: Results of numerical experiments in CARLA. The time gap error is evaluated between adjacent vehicles. |  | Scenario 1 |  | Scenario 2 |  | Scenario 3
---|---|---|---|---|---|---
|  | no group model | with group model |  | no group model | with group model |  | no group model | with group model
time gap error 1 |  | 0.0946 |  0.0578 (-63.7%) |  | 0.1060 |  0.0726 (-46.0%) |  | 0.1365 |  0.1076 (-26.9%)
time gap error 2 |  | 0.0723 |  0.0597 (-21.1%) |  | 0.0851 |  0.0739 (-15.2%) |  | 0.1155 |  0.1097 (-5.3%)
time gap error 3 |  | 0.0613 |  0.0587 (-4.4%) |  | 0.0753 |  0.0672 (-12.1%) |  | 0.1136 |  0.1005 (-13.0%)
time gap error 4 |  | 0.0664 |  0.0539 (-23.2%) |  | 0.0734 |  0.0650 (-12.9%) |  | 0.1032 |  0.0900 (-14.7%)

###  V-D Validations with CARLA

To further validate the work, we make use of the toolbox OpenCDA [42] in CARLA
simulator (cf. Fig. 12). We consider a platoon of one leading vehicle and four
following vehicles, where the type of vehicles are: Lincoln MKZ2017, Dodge
Charger2020, BMW Grand Tourer, Ford Mustang, Nissan Patrol. Upon estimating
approximately the engine time constant of each vehicle, we implemented the
homogenizing strategy in Sec. III, so as to compare the difference between the
absence and the presence of a common group model. In order to test the
strategy in different conditions, we considered three different behaviors for
the leading vehicle:

(a) Time gap (upper plot) and spacing error (lower plot)
for all vehicles, without group model.

(b) Time gap (upper plot) and spacing error (lower plot)
for all vehicles, with group model.

Figure 11: Results with CARLA simulator for Scenario 1. Figure 12: Snapshots
from OpenCDA in CARLA.

  * •

Scenario 1: the velocity of the leading vehicle oscillates around 15m/s with
period 70s;

  * •

Scenario 2: the velocity of the leading vehicle oscillates around 15m/s with
period 50s;

  * •

Scenario 3: the velocity of the leading vehicle oscillates around 15m/s with
period 30s.

For all scenarios, the amplitude of the oscillation was 6m/s. The results are
reported in Tab. III in terms of time gap error between adjacent vehicles (the
difference between the desired time gap and the actual time gap). The results
are also reported in Fig. 11 in terms of time gap and spacing error (for
compactness, only the results for Scenario 1 are reported). From the plots it
is possible to see that the main difference between Fig. 11(a) (without common
group model) and Fig. 11(b) (with common group model) is that the presence of
a common group model attenuates the oscillations around the desired spacing.
Such oscillations are induced by the oscillating behavior of the leading
vehicle: in the absence of a group model, the oscillations are more pronounced
and different for different vehicles due to the different vehicle types; when
reaching a common group model thanks to the homogenizing strategy, the
oscillations are smaller and close to each other. This validates the
effectiveness of enforcing a common behavior in order to have better
platooning performance.

###  V-E Validations in complex traffic scenarios

Finally, we go back to the CommonRoad platform so as to test complex traffic
scenarios beyond the standard platooning operation. In particular, we test:

  * •

a merging scenario with three different platoons merging into one;

  * •

a cut-in scenario with one vehicle suddenly cutting in the middle of the
platoon.

These scenarios are tested for the proposed self-organization mechanism and
the provably-correct safety layer wrapped around it. The merging scenario has
two phases, as shown in Fig. 13(a) and Fig. 13(b): first the two platoons on
the left will merge, and then the vehicles will all accelerate to merge with
the platoon on the right. The maneuver is accomplished successfully. In the
cut-in scenario, the velocity and spacing errors are in Fig. 14(a): the red
vehicle shown in Fig. 14(b) suddenly cuts in the middle of the platoon
violating the safety distance. As a result, the safety layer is activated, in
particular the safety recapturing controller that aims to recover the safety
distance in finite time: this explains the large spacing error in Fig. 14(a),
which is needed to create the gap for the cut-in vehicle to safely merge.
Summarizing, the validation tests in Figs. 13 and 14 suggest that the proposed
self-organization mechanism can be embedded into a full-range CACC protocol
with safety characteristics.

(a) Spacing error and distance to leading vehicle.

(b) Snapshots from CommonRoad (vehicles appear with different size due to the
different length of the road in each snapshot).

Figure 13: Spacing error, distance to leading vehicle, and snapshots from
CommonRoad, for a merging scenario. The proposed self-organizing strategy and
the safety layer are both included. The scenario is accomplished successfully.

(a) Spacing error and distance to leading vehicle.

(b) Snapshots from CommonRoad.

Figure 14: Spacing error, distance to leading vehicle, and snapshots from
CommonRoad, for a cut-in scenario. The proposed self-organizing strategy and
the safety layer are both included. The scenario is accomplished successfully.

##  VI Conclusions and future work

This work studied self-organization strategies in formations of cooperative
vehicles and their provably-correct safety implementation. Self-organization
allows all vehicles to homogenize to a common (group) behavior, which has been
shown to promote resilience against acceleration limits and wireless
communication failures. Stability of the self-organization mechanism has been
studied analytically in terms of convergence of the spacing errors to zero and
convergence to a common homogeneous behavior, despite the vehicles comprising
the platoon being heterogeneous. The theoretical results have been supported
by numerical implementations in the platooning toolbox OpenCDA in CARLA and in
the CommonRoad platform. The proposed self-organization approach was
integrated in a provably-correct layer to ensure safety.

In future work, self-organization can be used to determine metrics for
vehicles leaving or joining a platoon. These metrics stem from the observation
that platoons require some homogeneity to operate effectively [43] (e.g. it is
not effective for a sport car to join a platoon of trucks). Motivated by the
proposed validations with a provably-correct safety layer, another interesting
work is to study the benefits of CACC as compared to ACC in terms of safety.

## References

  * [1] R. Kianfar, P. Falcone, and J. Fredriksson, “Reachability analysis of cooperative adaptive cruise controller,” _Proc. of the 2012 15th IEEE International Conference on Intelligent Transportation Systems_ , pp. 1537-1542.
  * [2] L. Peppard, “String stability of relative-motion PID vehicle control systems,” _IEEE Transactions on Automatic Control_ , vol. 19, no. 5, pp. 579–581, 1974.
  * [3] E. van Nunen, J. Reinders, E. Semsar-Kazerooni, and N. van de Wouw, “String stable model predictive cooperative adaptive cruise control for heterogeneous platoons,” _IEEE Transactions on Intelligent Vehicles_ , vol. 4, no. 2, pp. 186–196, 2019.
  * [4] B. Besselink, V. Turri, S. H. van de Hoef, K.-Y. Liang, A. Alam, J. Mårtensson, and K. H. Johansson, “Cyber–physical control of road freight transport,” _Proc. of the IEEE_ , vol. 104, no. 5, pp. 1128–1141, 2016.
  * [5] F. Morbidi, P. Colaneri, and T. Stanger, “Decentralized optimal control of a car platoon with guaranteed string stability,” _Proc. of the 2013 European Control Conference_ , pp. 3494-3499.
  * [6] M. Pirani, E. M. Shahrivar, B. Fidan, and S. Sundaram, “Robustness of leader–follower networked dynamical systems,” _IEEE Transactions on Control of Network Systems_ , vol. 5, no. 4, pp. 1752–1763, 2018.
  * [7] M. Pirani, E. Hashemi, J. W. Simpson-Porco, B. Fidan, and A. Khajepour, “Graph theoretic approach to the robustness of $k$ -nearest neighbor vehicle platoons,” _IEEE Transactions on Intelligent Transportation Systems_ , vol. 18, no. 11, pp. 3218–3224, 2017.
  * [8] E. Abolfazli, B. Besselink, and T. Charalambous, “On time headway selection in platoons under the MPF topology in the presence of communication delays,” _IEEE Transactions on Intelligent Transportation Systems_ , vol. 23, no. 7, pp. 8881–8894, 2022.
  * [9] S. C. Warnick and A. A. Rodriguez, “Longitudinal control of a platoon of vehicles with multiple saturating nonlinearities,” _Proc. of the 1994 American Control Conference_ , pp.403-407.
  * [10] M. R. Jovanovic, J. M. Fowler, B. Bamieh, and R. D’Andrea, “On avoiding saturation in the control of vehicular platoons,” _Proc. of the 2004 American Control Conference_ , pp. 2257-2262.
  * [11] J. C. Zegers, E. Semsar-Kazerooni, J. Ploeg, N. van de Wouw, and H. Nijmeijer, “Consensus control for vehicular platooning with velocity constraints,” _IEEE Transactions on Control Systems Technology_ , vol. 26, no. 5, pp. 1592–1605, 2018.
  * [12] T. Tao, V. Jain, and S. Baldi, “An adaptive approach to longitudinal platooning with heterogeneous vehicle saturations,” _Proc. of the 2019 15th IFAC Symposium on Large Scale Complex Systems_ , pp. 7-12.
  * [13] S. Baldi, D. Liu, V. Jain, and W. Yu, “Establishing platoons of bidirectional cooperative vehicles with engine limits and uncertain dynamics,” _IEEE Transactions on Intelligent Transportation Systems_ , vol. 22, no. 5, pp. 2679–2691, 2021.
  * [14] M. Sun, A. Al-Hashimi, M. Li, and R. Gerdes, “Impacts of constrained sensing and communication based attacks on vehicular platoons,” _IEEE Transactions on Vehicular Technology_ , vol. 69, no. 5, pp. 4773–4787, 2020.
  * [15] X. Jin, W. M. Haddad, Z.-P. Jiang, A. Kanellopoulos, and K. G. Vamvoudakis, “An adaptive learning and control architecture for mitigating sensor and actuator attacks in connected autonomous vehicle platoons,” _International Journal of Adaptive Control and Signal Processing_ , vol. 33, no. 12, pp. 1788–1802, 2019.
  * [16] M. Pirani, E. Hashemi, A. Khajepour, B. Fidan, B. Litkouhi, S. Chen, and S. Sundaram, “Cooperative vehicle speed fault diagnosis and correction,” _IEEE Transactions on Intelligent Transportation Systems_ , vol. 20, no. 2, pp. 783–789, 2019.
  * [17] J. Ploeg, E. Semsar-Kazerooni, G. Lijster, N. van de Wouw, and H. Nijmeijer, “Graceful degradation of cooperative adaptive cruise control,” _IEEE Transactions on Intelligent Transportation Systems_ , vol. 16, no. 1, pp. 488–497, 2015.
  * [18] J. Sawant, U. Chaskar, and D. Ginoya, “Robust control of cooperative adaptive cruise control in the absence of information about preceding vehicle acceleration,” _IEEE Transactions on Intelligent Transportation Systems_ , vol. 22, no. 9, pp. 5589–5598, 2021.
  * [19] F. Acciani, P. Frasca, G. Heijenk, and A. A. Stoorvogel, “Achieving robust average consensus over lossy wireless networks,” _IEEE Transactions on Control of Network Systems_ , vol. 6, no. 1, pp. 127–137, 2019\.
  * [20] F. Acciani, P. Frasca, G. Heijenk, and A. A. Stoorvogel, “Stochastic string stability of vehicle platoons via cooperative adaptive cruise control with lossy communication,” _IEEE Transactions on Intelligent Transportation Systems_ , vol. 23, no. 8, pp. 10 912–10 922, 2022.
  * [21] H. Xing, J. Ploeg, and H. Nijmeijer, “Compensation of communication delays in a cooperative ACC system,” _IEEE Transactions on Vehicular Technology_ , vol. 69, no. 2, pp. 1177–1189, 2020.
  * [22] Z. Li, B. Hu, M. Li, and G. Luo, “String stability analysis for vehicle platooning under unreliable communication links with event-triggered strategy,” _IEEE Transactions on Vehicular Technology_ , vol. 68, no. 3, pp. 2152–2164, 2019.
  * [23] F. Ma, J. Wang, S. Zhu, S. Y. Gelbal, Y. Yang, B. Aksun-Guvenc, and L. Guvenc, “Distributed control of cooperative vehicular platoon with nonideal communication condition,” _IEEE Transactions on Vehicular Technology_ , vol. 69, no. 8, pp. 8207–8220, 2020.
  * [24] Y. A. Harfouch, S. Yuan, and S. Baldi, “An adaptive switched control approach to heterogeneous platooning with intervehicle communication losses,” _IEEE Transactions on Control of Network Systems_ , vol. 5, no. 3, pp. 1434–1444, 2018.
  * [25] F. Acciani, P. Frasca, A. Stoorvogel, E. Semsar-Kazerooni, and G. Heijenk, “Cooperative adaptive cruise control over unreliable networks: an observer-based approach to increase robustness to packet loss,” _Proc. of the 2018 European Control Conference_ , pp. 1399-1404.
  * [26] M. Althoff, S. Maierhofer, and C. Pek, “Provably-correct and comfortable adaptive cruise control,” _IEEE Transactions on Intelligent Vehicles_ , vol. 6, no. 1, pp. 159–174, 2021.
  * [27] M. Jalalmaab, B. Fidan, S. Jeon, and P. Falcone, “Guaranteeing persistent feasibility of model predictive motion planning for autonomous vehicles,” _Proc. of the 2017 IEEE Intelligent Vehicles Symposium_ , pp. 843-848.
  * [28] S. Sadraddini, S. Sivaranjani, V. Gupta, and C. Belta, “Provably safe cruise control of vehicular platoons,” _IEEE Control Systems Letters_ , vol. 1, no. 2, pp. 262–267, 2017.
  * [29] X. Xu, J. W. Grizzle, P. Tabuada, and A. D. Ames, “Correctness guarantees for the composition of lane keeping and adaptive cruise control,” _IEEE Transactions on Automation Science and Engineering_ , vol. 15, no. 3, pp. 1216–1229, 2018.
  * [30] D. Bevly, X. Cao, M. Gordon, G. Ozbilgin, D. Kari, B. Nelson, J. Woodruff, M. Barth, C. Murray, A. Kurt, K. Redmill, and U. Ozguner, “Lane change and merge maneuvers for connected and automated vehicles: A survey,” _IEEE Transactions on Intelligent Vehicles_ , vol. 1, no. 1, pp. 105–120, 2016.
  * [31] V. Giammarino, S. Baldi, P. Frasca, and M. L. D. Monache, “Traffic flow on a ring with a single autonomous vehicle: An interconnected stability perspective,” _IEEE Transactions on Intelligent Transportation Systems_ , vol. 22, no. 8, pp. 4998–5008, 2021.
  * [32] Y. Zhu, D. Zhao, and H. He, “Synthesis of cooperative adaptive cruise control with feedforward strategies,” _IEEE Transactions on Vehicular Technology_ , vol. 69, no. 4, pp. 3615–3627, 2020.
  * [33] S. Thormann, A. Schirrer, and S. Jakubek, “Safe and efficient cooperative platooning,” _IEEE Transactions on Intelligent Transportation Systems_ , vol. 23, no. 2, pp. 1368–1380, 2022.
  * [34] S. Baldi and P. Frasca, “Leaderless synchronization of heterogeneous oscillators by adaptively learning the group model,” _IEEE Transactions on Automatic Control_ , vol. 65, no. 1, pp. 412–418, 2020.
  * [35] S. Baldi, S. Yuan, and P. Frasca, “Output synchronization of unknown heterogeneous agents via distributed model reference adaptation,” _IEEE Transactions on Control of Network Systems_ , vol. 6, no. 2, pp. 515–525, 2019\.
  * [36] M. Althoff, M. Koschi, and S. Manzinger, “Commonroad: Composable benchmarks for motion planning on roads,” _Proc. of the 2017 IEEE Intelligent Vehicles Symposium_ , pp. 719-726.
  * [37] F. Fagnani and P. Frasca, _Introduction to Averaging Dynamics over Networks_. Springer, 2018.
  * [38] J. C. Zegers, E. Semsar-Kazerooni, J. Ploeg, N. van de Wouw, and H. Nijmeijer, “Consensus-based bi-directional CACC for vehicular platooning,” _Proc. of the 2016 American Control Conference_ , pp. 2578-2584.
  * [39] G. C. Goodwin, S. F. Graebe, and M. E. Salgado, _Control Systems Design_. Prentice Hall, 2001.
  * [40] J. Cortés, “Distributed algorithms for reaching consensus on general functions,” _Automatica_ , vol. 44, no. 3, pp. 726–737, 2008.
  * [41] K. Kalsi, J. Lian, S. Hui, and S. H. Żak, “Sliding-mode observers for systems with unknown inputs: A high-gain approach,” _Automatica_ , vol. 46, no. 2, pp. 347 – 353, 2010.
  * [42] R. Xu, Y. Guo, X. Han, X. Xia, H. Xiang, and J. Ma, “OpenCDA: an open cooperative driving automation framework integrated with co-simulation,” _Proc. of the 2021 IEEE International Intelligent Transportation Systems Conference_ , pp. 1155–1162.
  * [43] H. Hu, R. Lu, Z. Zhang, and J. Shao, “REPLACE: A reliable trust-based platoon service recommendation scheme in VANET,” _IEEE Transactions on Vehicular Technology_ , vol. 66, no. 2, pp. 1786–1797, 2017.