# 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

**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].

This research was partly supported by the European Union Horizon 2020 research and innovation programme under the Marie Skłodowska-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, althoff}@tum.de)

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: 220194629@seu.edu.cn, s.baldi@tudelft.nl)

P. Frasca is with University of Grenoble Alpes, CNRS, Inria, Grenoble INP, GIPSA-Lab, Grenoble F-38000, France (e-mail: paolo.frasca@gipsa-lab.fr)

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-
<figure><img src="image_1.png" /><figcaption>Fig. 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).</figcaption></figure>

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.

## II. CACC SYSTEM STRUCTURE AND SOLUTION CONCEPT

Consider a platoon composed of M vehicles (Fig. 1). Let $S_M = \{i = 2, 3, \dots, 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) \\ \dot{v}_i(t) \\ \dot{a}_i(t) \end{bmatrix} = \begin{bmatrix} v_i(t) \\ a_i(t) & u_i(t) \\ -\frac{1}{\tau_i}a_i(t) + \frac{1}{\tau_i}(u_{bl_i}(t) + u_{hm_i}(t)) & 0 \end{bmatrix}, \quad (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.

### A. Baseline input

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

$$ \begin{aligned} e_i(t) &= q_{i-1}(t) - q_i(t) - h v_i(t), \\ \dot{e}_i(t) &= v_{i-1}(t) - v_i(t) - h a_i(t), \quad i \in S_M, \end{aligned} \quad (2) $$

where $h > 0$ is the time headway constant and $h v_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 (2):

$$ \begin{bmatrix} \dot{e}_i(t) \\ \dot{v}_i(t) \\ \dot{a}_i(t) \end{bmatrix} = \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} + \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)). \quad (3) $$

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{aligned} h\dot{u}_{bl_i}(t) &= -u_{bl_i}(t) + \xi_i(t), \\ \xi_i(t) &= \begin{cases} K_{pi}e_i(t) + K_{di}\dot{e}_i(t) + \xi_{hm_i}(t) + u_{bl_{i-1}}(t), & i \in S_M \\ u_r(t) & i = 1 \end{cases} \end{aligned} \quad (4) $$

where $K_{pi}$ and $K_{di}$ 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_{pi}$ and $K_{di}$. The homogenizing design for $u_{hm_i}$ and $\xi_{hm_i}$ is presented hereafter.

### 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)

$$ \begin{bmatrix} \dot{e}_i(t) \\ \dot{v}_i(t) \\ \dot{a}_i(t) \end{bmatrix} = \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} + \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), \quad (5) $$

$$ h\dot{u}_{bl_i}(t) = -u_{bl_i}(t) + K_{p0}e_i(t) + K_{d0}\dot{e}_i(t) + u_{bl_{i-1}}(t). \quad (6) $$
<figure><img src="image_2.png" /><figcaption>(a) Interaction between the nominal and the safety controllers.</figcaption></figure>

<figure><img src="image_3.png" /><figcaption>(b) Statechart of the provably-correct control layer.</figcaption></figure>

<caption>Fig. 2: Concept of the provably-correct architecture.</caption>

The difference with (3)-(4) is that $\tau_0$, $K_{p0}$ and $K_{d0}$ 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* (5)-(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 (5) and (6) are

$$ 
\begin{aligned}
u_{hm_i}(t) &= \frac{\tau_0 - \tau_i}{\tau_0}(a_i(t) - u_{bl_i}(t)), \\
\xi_{hm_i}(t) &= (K_{p0} - K_{p_i})e_i(t) + (K_{d0} - K_{d_i})\dot{e}_i(t).
\end{aligned} 
\quad (7) $$

*Proof.* It can be verified by direct substitution of (7) into (3)-(4), which results in the dynamics (5)-(6). □

**Remark 1** (A priori knowledge). The homogenizing inputs (7) require a common set of parameters $(\tau_0, K_{p0}, K_{d0})$. 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.

### 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. 2a, 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. 2b or to [26] for details. The safety layer is implemented in the CommonRoad platform [36], as clarified in Sec. V.

### 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_{p0}, K_{d0}$ 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.

### A. Preliminaries on consensus dynamics

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

$$ \dot{\bar{K}}_{p\tau_i}(t) = \mu_p \sum_{j \in \mathcal{N}_i} (\bar{K}_{p\tau_j}(t) - \bar{K}_{p\tau_i}(t)), $$
$$
\begin{align}
\dot{\bar{K}}_{d_i}(t) &= \mu_d \sum_{j \in \mathcal{N}_i} (\bar{K}_{d_j}(t) - \bar{K}_{d_i}(t)), \nonumber \\
\dot{\bar{\tau}}_i(t) &= \mu_\tau \sum_{j \in \mathcal{N}_i} (\bar{\tau}_j(t) - \bar{\tau}_i(t)), \tag{8}
\end{align}
$$

with initial conditions

$$
\bar{K}_{p\tau_i}(0) = K_{p\tau_i} := K_{p_i}\tau_i, \quad \bar{K}_{d_i}(0) = K_{d_i}, \quad \bar{\tau}_i(0) = \tau_i, \quad (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 (8) leads to

$$
\begin{align}
\lim_{t \to \infty} \bar{K}_{p\tau_i}(t) &= \frac{1}{M} \sum_{j=1}^{M} K_{p\tau_j} :=: K_{p\tau_0}, \nonumber \\
\lim_{t \to \infty} \bar{K}_{d_i}(t) &= \frac{1}{M} \sum_{j=1}^{M} K_{d_j} :=: K_{d_0}, \nonumber \\
\lim_{t \to \infty} \bar{\tau}_i(t) &= \frac{1}{M} \sum_{j=1}^{M} \tau_j :=: \tau_0, \tag{10}
\end{align}
$$

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 communi-
cation 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.

B. Self-organizing design and stability analysis

The parameters in (10) have been denoted as $(\tau_0, K_{p_0}, K_{d_0})$
like the common set of parameters in (5)-(6). Note, however,
that the parameters in (10) 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:

$$
\begin{align}
u_{hm_i}(t) &= \frac{\bar{\tau}_i(t) - \tau_i}{\bar{\tau}_i(t)} (a_i(t) - u_{bl_i}(t)), \nonumber \\
\xi_{hm_i}(t) &= (\bar{K}_{p_i}(t) - K_{p_i})e_i(t) + (\bar{K}_{d_i}(t) - K_{d_i})\dot{e}_i(t) \tag{11}
\end{align}
$$

where $\bar{K}_{p_i}(t) = \bar{K}_{p\tau_i}(t)/\bar{\tau}_i(t)$. The main difference between (7) and (11) 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 (11): in fact, the gains in (11) are time-varying due to (8) and result in a nonlinear closed-loop system.

The following result shows the stability properties of the
self-organizing strategy (8), (11), i.e. the capability of achiev-
ing asymptotic formation thanks to the common group behav-
ior.

**Theorem 1.** Consider the platoon composed of vehicle error dynamics (3) and input dynamics (4). Select the auxiliary inputs as in (11) with parameters $(\bar{\tau}_i, \bar{K}_{p_i}, \bar{K}_{d_i})$ arising from the consensus dynamics (8). 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), (8), and (11) is nonlinear, it can be written in the form $\dot{x}(t) = Fx(t) + \tilde{F}(t)x(t) + \tilde{F}(t)$, where $F$ is a Hurwitz state matrix, and $\tilde{F}(t)$, $\tilde{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 (11), we get the dynamics in the form

$$
\dot{x}_1(t) = F_1 x_1(t) + \tilde{F}_1(t) x_1(t) + \hat{F}_1(t), \quad (12)
$$

with the Hurwitz state matrix

$$
F_1 = \begin{bmatrix} -\frac{1}{\tau_0} & \frac{1}{\tau_0} \\ 0 & -\frac{1}{h} \end{bmatrix} \quad (13)
$$

$$
\text{and} \quad \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}, \quad \hat{F}_1(t) = \begin{bmatrix} 0 \\ \frac{u_r(t)}{h} \end{bmatrix}. \quad (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 (11), so as to obtain the form

$$
\dot{x}_i(t) = F x_i(t) + \tilde{F}_i(t) x_i(t) + \hat{F}_i(t), \quad (15)
$$

with

$$
F = \begin{bmatrix} 0 & 1 & -h & 0 \\ 0 & 0 & -1 & 0 \\ 0 & 0 & -\frac{1}{\tau_0} & \frac{1}{\tau_0} \\ \frac{K_{p0}}{h} & \frac{K_{d0}}{h} & -K_{d0} & -\frac{1}{h} \end{bmatrix}, \quad (16)
$$

$$
\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}, \quad (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}. \quad (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 $\hat{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, \dots$. We have that that $\hat{F}_i$ in (17) converges to zero exponentially because $(\hat{\tau}_i, \hat{K}_{p_i}, \hat{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

$$|\Gamma(j\omega)| \le 1, \forall \omega \ge 0. \quad (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.

### 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

$$\begin{aligned} \dot{\bar{a}}_{max_i}(t) &= \operatorname{sgn}_{-} \left( \sum_{j \in \mathcal{N}_j} (\bar{a}_{max_j}(t) - \bar{a}_{max_i}(t)) \right) \\ \dot{\bar{a}}_{min_i}(t) &= \operatorname{sgn}_{+} \left( \sum_{j \in \mathcal{N}_j} (\bar{a}_{min_j}(t) - \bar{a}_{min_i}(t)) \right), \end{aligned} \quad (20)$$

where

$$\operatorname{sgn}_{+}(x) = \begin{cases} 0 & \text{if } x \le 0 \\ 1 & \text{if } x > 0 \end{cases}, \quad \operatorname{sgn}_{-}(x) = \begin{cases} 0 & \text{if } x \ge 0 \\ 1 & \text{if } x < 0 \end{cases} \quad (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 (20) lead to

$$\begin{aligned} \lim_{t \to \infty} \bar{a}_{max_i}(t) &= \min_i [a_{max_i}] := a_{max_0} \\ \lim_{t \to \infty} \bar{a}_{min_i}(t) &= \max_i [a_{min_i}] := a_{min_0} \end{aligned} \quad (22)$$

in finite time.

**Remark 3 (No a priori knowledge of the group model).** Albeit having different dynamics, the consensus mechanisms (8) and (20) 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 (22). 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:

$$\dot{h} \ddot{u}_{bl_i} = \begin{cases} 0 & \text{if } u_i = \bar{a}_{max_i} \text{ and } -u_{bl_i} + \xi_i \ge 0 \\ -u_{bl_i} + \xi_i & \text{if } \bar{a}_{min_i} < u_i < \bar{a}_{max_i} \\ \text{or } u_i = \bar{a}_{max_i} \text{ and } -u_{bl_i} + \xi_i < 0 \\ \text{or } u_i = \bar{a}_{min_i} \text{ and } -u_{bl_i} + \xi_i > 0 \\ 0 & \text{if } u_i = \bar{a}_{min_i} \text{ and } -u_{bl_i} + \xi_i \le 0. \end{cases} \quad (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)**

**Input:** initial conditions $a_{max_i}$, $a_{min_i}$.

**Output:** the constrained control (23) with constrained acceleration $a_i \in [\bar{a}_{min_i}, \bar{a}_{max_i}]$.

1. Online, at time $t$: Run the max-min consensus dynamics (20) to get $\bar{a}_{min_i}(t)$, $\bar{a}_{max_i}(t)$;

2. 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)]$.
## 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_{p0}e_i(t) + K_{d0}\dot{e}_i(t). \quad (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 (8) when communication was active, so that the vehicle dynamics have converged to $K_{p0}$, $K_{d0}$, $\tau_0$ in (10). 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 (8), 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.

1) **Dynamics of the homogenized preceding vehicle:** As all vehicles have converged to homogeneous dynamics via (8), using (5)-(6) with index $i-1$, one obtains the homogenized dynamics of vehicle $i-1$ as:

$$\begin{bmatrix} \dot{v}_{i-1}(t) \\ \dot{a}_{i-1}(t) \\ \dot{u}_{bl_{i-1}}(t) \end{bmatrix} = \underbrace{\begin{bmatrix} 0 & 1 & 0 \\ 0 & -\frac{1}{\tau_0} & \frac{1}{\tau_0} \\ -K_{p0} - \frac{K_{d0}}{\hbar} & -K_{d0} & -\frac{1}{\hbar} \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)} + \underbrace{\begin{bmatrix} 0 \\ 0 \\ \frac{1}{\hbar} \end{bmatrix}}_{B_o} \underbrace{\left[ K_{p0}(q_{i-2}(t) - q_{i-1}(t)) + K_{d0}v_{i-2}(t) + u_{bl_{i-2}}(t) \right]}_{\nu_{o_{i-1}}(t)}, \quad (25)$$

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].

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

$$v_{i-1}(t) = H_o x_{o_{i-1}}(t), \quad (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

$$\mathrm{rank}(B_o) \neq \mathrm{rank}(B_o H_o). \quad (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

$$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}. \quad (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

$$\begin{bmatrix} \dot{v}_{i-1}(t) \\ \dot{a}_{i-1}(t) \\ \dot{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)), \quad (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}}$.

3) **Observer for missing signal $u_{bl_{i-1}}$:** The unknown input observer is in the sliding-mode form

$$\dot{x}_{o_{i-1}}(t) = A_o \dot{x}_{o_{i-1}}(t) + B_o E_o(t) + L_o (\bar{y}_{i-1}(t) - \hat{y}_{i-1}(t)), \quad (30)$$

where $\hat{y}_{i-1}(t) = C_o \dot{x}_{o_{i-1}}(t)$ are the reconstructed auxiliary variables, $L_o$ is such that $(A_o - L_o C_o)$ is Hurwitz, and

$$\hat{x}_{o_{i-1}}(t) = \begin{bmatrix} \dot{v}_{i-1}(t) \\ \dot{a}_{i-1}(t) \\ \dot{u}_{bl_{i-1}}(t) \end{bmatrix}, \quad \bar{y}_{i-1}(t) = \begin{bmatrix} v_{i-1}(t) \\ S_a \mathrm{sat}\left(\frac{\bar{a}_{i-1}(t)}{S_a}\right) \\ S_j \mathrm{sat}\left(\frac{\bar{j}_{i-1}(t)}{S_j}\right) \end{bmatrix}, \quad (31)$$

$$E_o(t) = \begin{cases} \eta \frac{F_o(\bar{y}_{i-1}(t) - \hat{y}_{i-1}(t))}{\|\bar{y}_{i-1}(t) - \hat{y}_{i-1}(t)\|} & \text{if } F_o(\bar{y}_{i-1}(t) - \hat{y}_{i-1}(t)) \neq 0 \\ 0 & \text{otherwise,} \end{cases} \quad (32)$$

with sat(·) 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

$$(A_o - L_o C_o)^T P_o + P_o (A_o - L_o C_o) = -2Q_o, \\ F_o C_o = B_o^T P_o, \quad P_o, Q_o > 0, \quad (33)$$

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 (33) that $F_o \in \mathbb{R}^{1 \times 3}$ takes the form

$$F_o = \begin{bmatrix} \frac{p_{o13}}{\hbar} & \frac{p_{o32} + p_{o33}}{\hbar} & \frac{p_{o33}}{\hbar} \\ \hbar & \hbar & \tau_0 \end{bmatrix}, \quad (34)$$

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)-(33) guarantees that there exists a constant $\epsilon^* \in (0, 1)$ such that, if $\epsilon \in (0, \epsilon^*)$ and
<figure><img src="image_4.png" /><figcaption>(a) Velocity (upper plot) and acceleration (lower plot) for all vehicles. Note the effect of noise on acceleration.</figcaption></figure>

<figure><img src="image_5.png" /><figcaption>(b) Spacing error (upper plot) and distance to leading vehicle (lower plot). The spacing error does not converge.</figcaption></figure>

<figure><img src="image_6.png" /><figcaption>(a) Velocity (upper plot) and acceleration (lower plot) for all vehicles. The effect of noise on acceleration reduces.</figcaption></figure>

<figure><img src="image_7.png" /><figcaption>(b) Spacing error (upper plot) and distance to leading vehicle (lower plot). The spacing error is kept closer to zero.</figcaption></figure>

<caption>Fig. 4: Platooning with self-organizing strategy.</caption>

$\eta \ge |v_{o_{i-1}}(t)|, \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) \to 0$ when $\epsilon \to 0$.

Proposition 4 suggests that the control input is finally

$$h\dot{u}_{bl_i}(t) = -u_{bl_i}(t) + K_{p0}e_i(t) + K_{d0}\dot{e}_i(t) + \hat{u}_{bl_{i-1}}(t), \quad (35)$$

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_{p0}, K_{d0}, \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)

**Input:** a priori unknown ($K_{p0}, K_{d0}, \tau_0$) from convergence of (8); 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$.

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

1. Offline: form a copy of the homogenized preceding vehicle matrices ($A_o, B_o$) as in (25);

2. 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);

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

## 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
<figure><img src="image_8.png" /><figcaption>(a) Velocity (upper plot) and acceleration (lower plot) for all vehicles. The platoon cannot follow the leader.</figcaption></figure>

<figure><img src="image_9.png" /><figcaption>(b) Spacing error (upper plot) and distance to leading vehicle (lower plot). A collision happens in the platoon.</figcaption></figure>

<figure><img src="image_10.png" /><figcaption>(a) Velocity (upper plot) and acceleration (lower plot) for all vehicles. The platoon follows the leader.</figcaption></figure>

<figure><img src="image_11.png" /><figcaption>(b) Spacing error (upper plot) and distance to leading vehicle (lower plot). The spacing error is regulated despite the acceleration limits.</figcaption></figure>

<caption>Fig. 6: Acceleration limits, proposed resilient strategy.</caption>

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 m²/s²), tachometer measurement noise (variance 0.25 m²/s²) and accelerometer measurement noise (variance 0.1 m⁴/s⁴). 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. Validation of self-organization

Let us consider a CACC-equipped platoon of one leading and five following vehicles, with headway $h = 0.7$ and hetero-

geneous parameters in as Tab. I. First, let us keep the proposed self-organizing strategy (8), (11) 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. 3a and 3b: 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 (2).

<table><caption>TABLE I: Heterogeneous parameters of the platoon.</caption><thead><tr><th></th><th>1</th><th>2</th><th>3</th><th>4</th><th>5</th><th>6</th></tr></thead><tbody><tr><th>τ<sub>i</sub></th><td>0.10</td><td>0.20</td><td>0.05</td><td>0.30</td><td>0.15</td><td>0.075</td></tr><tr><th>K<sub>p<sub>i</sub></sub></th><td>0.20</td><td>0.10</td><td>0.40</td><td>0.067</td><td>0.133</td><td>0.267</td></tr><tr><th>K<sub>d<sub>i</sub></sub></th><td>0.70</td><td>0.35</td><td>1.40</td><td>0.23</td><td>0.467</td><td>0.933</td></tr></tbody></table>

For the same leader acceleration/deceleration scenario, let
<figure><img src="image_12.png" /><figcaption>(a) Velocity (upper plot) and acceleration (lower plot) for all vehicles. Note the large velocity and acceleration transients.</figcaption></figure>

<figure><img src="image_13.png" /><figcaption>(b) Spacing error (upper plot) and distance to leading vehicle (lower plot). The spacing error is not regulated close to zero.</figcaption></figure>

<figure><img src="image_14.png" /><figcaption>(a) Velocity (upper plot) and acceleration (lower plot) for all vehicles, with observer estimates in red in the small box.</figcaption></figure>

<figure><img src="image_15.png" /><figcaption>(b) Spacing error (upper plot) and distance to leading vehicle (lower plot). Close-to-ideal performance is recovered.</figcaption></figure>

<caption>Fig. 8: Communication failures, proposed resilient strategy.</caption>

us now switch the proposed self-organizing strategy (8), (11) on, with $\mu_p = \mu_d = \mu_\tau = 0.2$. Fig. 4a shows that all vehicles manage to follow the velocity and acceleration of the leader, the effect of noise on acceleration being smaller than Fig. 3a. Meanwhile, Fig. 4b shows that the spacing error is kept closer to zero as compared to Fig. 3b.

## 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/s²] due to the leader acceleration limits.

Fig. 5a 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. 5b 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><caption>TABLE II: Heterogeneous limits of the platoon.</caption><thead><tr><th></th><th>1</th><th>2</th><th>3</th><th>4</th><th>5</th><th>6</th></tr></thead><tbody><tr><th>a<sub>max</sub><sub>i</sub></th><td>0.425</td><td>0.35</td><td>0.375</td><td>0.40</td><td>0.325</td><td>0.45</td></tr><tr><th>a<sub>min</sub><sub>i</sub></th><td>-0.425</td><td>-0.35</td><td>-0.375</td><td>-0.40</td><td>-0.325</td><td>-0.45</td></tr></tbody></table>

With the same acceleration limits of Tab. II, Fig. 6a 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. 6a compared to 32m/s in Fig. 4a), but this is not a surprise: it was already shown in
<figure><img src="image_16.png" /><figcaption>(a) Spacing error and distance to leader without self-organization.</figcaption></figure>

<figure><img src="image_17.png" /><figcaption>(b) Spacing error and distance to leader with self-organization.</figcaption></figure>

<figure><img src="image_18.png" /><figcaption>(c) Snapshots from CommonRoad without self-organization.</figcaption></figure>

<figure><img src="image_19.png" /><figcaption>(d) Snapshots from CommonRoad with self-organization.</figcaption></figure>

<caption>Fig. 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.</caption>

[9], [10] that reducing performance is necessary to keep the formation, since it avoids that the vehicles hit their acceleration limits. Fig. 6b shows that the spacing error is regulated in a way similar to Fig. 4b, 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. 7a 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. 7b).

To demonstrate the benefits of the proposed resilient strategy (29)-(34), we apply the proposed observer. The observer design parameters are $S_a = S_j = 1$, $\epsilon = 0.01$, $\eta = 1.5$, $Q_o = \text{diag}\{0.1, 0.2, 0.01\}$, $\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. 8a 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. 8a with the noise in previous scenarios, e.g. Figs. 4b and 6b, it can be noticed that the observer does not amplify the measurement noises in the vehicle behavior. Remarkably, Fig. 8b 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. 8b are much smaller than those in Fig. 7b, making the performance of the proposed resilient design in Fig. 8 clearly superior to that of Fig. 7.

### 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. 9a show that the absence
<figure><img src="image_20.png" /></figure>

<figure><img src="image_21.png" /><figcaption>(a) Emergency braking without safety layer.</figcaption></figure>

<figure><img src="image_22.png" /></figure>

<figure><img src="image_23.png" /><figcaption>(b) Emergency braking with safety layer.</figcaption></figure>

<figure><img src="image_24.png" /><figcaption>(c) Snapshots from CommonRoad without safety layer.</figcaption></figure>

<figure><img src="image_25.png" /><figcaption>(d) Snapshots from CommonRoad with safety layer.</figcaption></figure>

<caption>Fig. 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.</caption>

of self-organization causes spacing errors in the order of 10 meters, indicating poor platooning performance. Upon activating the proposed self-organization strategy, Fig. 9b shows that the spacing errors are small and converge close to zero. Snapshots from CommonRoad are shown in Fig. 9c (without self-organization) and Fig. 9d (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. 10a shows that the absence of a safety layer can indeed lead to a crash, cf. the snapshots from CommonRoad in Fig. 10c. However, when the safety layer is active, it promptly activates emergency braking to avoid collision, as shown in Fig. 10d.

## 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:

* 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
<figure><img src="image_26.png" /><figcaption>(a) Time gap (upper plot) and spacing error (lower plot) for all vehicles, without group model.</figcaption></figure>

<figure><img src="image_27.png" /><figcaption>(b) Time gap (upper plot) and spacing error (lower plot) for all vehicles, with group model.</figcaption></figure>

<figure><img src="image_28.png" /><figcaption>Fig. 12: Snapshots from OpenCDA in CARLA.</figcaption></figure>

the plots it is possible to see that the main difference between Fig. 11a (without common group model) and Fig. 11b (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.

## 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. 13a and Fig. 13b: 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. 14a: the red vehicle shown in Fig. 14b 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. 14a, 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.

# 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

<table><caption>TABLE III: Results of numerical experiments in CARLA. The time gap error is evaluated between adjacent vehicles.</caption><thead><tr><th rowspan="2"></th><th colspan="2">Scenario 1</th><th colspan="2">Scenario 2</th><th colspan="2">Scenario 3</th></tr><tr><th>no group model</th><th>with group model</th><th>no group model</th><th>with group model</th><th>no group model</th><th>with group model</th></tr></thead><tbody><tr><td>time gap error 1</td><td>0.0946</td><td><strong>0.0578</strong> (-63.7%)</td><td>0.1060</td><td><strong>0.0726</strong> (-46.0%)</td><td>0.1365</td><td><strong>0.1076</strong> (-26.9%)</td></tr><tr><td>time gap error 2</td><td>0.0723</td><td><strong>0.0597</strong> (-21.1%)</td><td>0.0851</td><td><strong>0.0739</strong> (-15.2%)</td><td>0.1155</td><td><strong>0.1097</strong> (-5.3%)</td></tr><tr><td>time gap error 3</td><td>0.0613</td><td><strong>0.0587</strong> (-4.4%)</td><td>0.0753</td><td><strong>0.0672</strong> (-12.1%)</td><td>0.1136</td><td><strong>0.1005</strong> (-13.0%)</td></tr><tr><td>time gap error 4</td><td>0.0664</td><td><strong>0.0539</strong> (-23.2%)</td><td>0.0734</td><td><strong>0.0650</strong> (-12.9%)</td><td>0.1032</td><td><strong>0.0900</strong> (-14.7%)</td></tr></tbody></table>
<figure><img src="image_29.png" /><figcaption>(a) Spacing error and distance to leading vehicle.</figcaption></figure>

<figure><img src="image_30.png" /><figcaption>(b) Snapshots from CommonRoad (vehicles appear with different size due to the different length of the road in each snapshot).</figcaption></figure>

<figure><img src="image_31.png" /><figcaption>(a) Spacing error and distance to leading vehicle.</figcaption></figure>

<figure><img src="image_32.png" /><figcaption>(b) Snapshots from CommonRoad.</figcaption></figure>

<caption>Fig. 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.</caption>

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. Khaje-pour, "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. 10912-10922, 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. Delta, "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. Zak, "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 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.