An optimization-based cooperative path-following framework for multiple robotic vehicles

TL;DR

This paper introduces an optimization-based distributed control framework for multiple robotic vehicles that balances path-following accuracy and coordination, ensuring convergence and effective cooperation in a networked environment.

Contribution

It presents a novel multi-agent model predictive control scheme combining path-following and consensus objectives with convergence guarantees.

Findings

Effective cooperative path following demonstrated in simulations

Balances transient coordination and path errors optimally

Guarantees convergence to desired coordinated behavior

Abstract

This paper addresses the design of an optimization-based cooperative path-following control law for multiple robotic vehicles that optimally balances the transient trade-off between coordination and path-following errors. To this end, we formulate a more general multi-agent framework where each agent is associated with (i) a continuous-time dynamical model, which governs the evolution of its state, and (ii) an output equation that is a function of both the state of the agent and a coordination vector. According to a given network topology, each agent can access its state and coordination vector, as well as the coordination vectors of the neighboring agents. In this setup, the goal is to design a distributed control law that steers the output signals to the origin, while simultaneously driving the coordination vectors of the agents of the network to consensus. To solve this, we propose a model predictive control scheme that builds on a pre-existing auxiliary consensus control law to design a performance index that combines the output regulation objective with the consensus objective. Convergence guarantees under which one can solve this coordinated output regulation problem are provided. Numerical simulations display the effectiveness of the proposed scheme applied to a cooperative path following control problem of a network of 3D nonholonomic robotic vehicles.

Figures

Equations

$${\dot{p}}^{[i]}(t)$$

$$\varphi (t):=\sum _{(i,j)\in \mathcal{E}}({\gamma }^{[i]}(t)-{\gamma }^{[j]}(t){)}^2,$$

$$y^{[i]}(t)$$

$${\dot{x}}^{[i]}(t)$$

$${\dot{\gamma }}^{[i]}(t)$$

$$y^{[i]}(t)$$

$${\dot{\eta }}^{[i]}(t)=v_{\gamma }^{[i]}(t),$$

$$u^{[i]}(t)={\bar{u}}^{\star \lfloor t\rfloor }(t),$$

$$J_T^{\star [i]}(p)=\underset{\begin{array}{c}{\bar{\mathbf{u}}}^{[i]}\in \mathcal{P}\mathcal{C}(t,t+T)\\ {\bar{\mathbf{v}}}_{\gamma }^{[i]}\in \mathcal{P}\mathcal{C}(t,t+T)\end{array}}{\min }{J}_T(p,{\bar{\mathbf{u}}}^{[i]},{\bar{\mathbf{v}}}_{\gamma }^{[i]})$$

$${\dot{\bar{x}}}^{[i]}(\tau )=f^{[i]}(\tau ,{\bar{x}}^{[i]}(\tau ),{\bar{u}}^{[i]}(\tau )),{\bar{x}}^{[i]}(t)=x^{[i]},$$

$${\dot{\bar{\gamma }}}^{[i]}(\tau )=g(\tau )+{\bar{u}}_{\gamma }^{[i]}(\tau ),{\bar{\gamma }}^{[i]}(t)={\gamma }^{[i]},$$

$${\dot{\bar{\eta }}}^{[i]}(\tau )={\bar{v}}_{\gamma }^{[i]}(\tau ),{\bar{\eta }}^{[i]}(t)={\eta }^{[i]},$$

$${\bar{y}}^{[i]}(\tau )=h^{[i]}(\tau ,{\bar{x}}^{[i]}(\tau ),{\bar{\gamma }}^{[i]}(\tau )),$$

$${\bar{u}}_{\gamma }^{[i]}(\tau )={\bar{u}}_{\gamma ,au{x}_t}^{[i]}(\tau )+{\bar{\eta }}^{[i]}(\tau ),$$

$$({\bar{u}}^{[i]}(\tau ),{\bar{v}}_{\gamma }^{[i]}(\tau ),{\bar{y}}^{[i]}(\tau ))\in {\mathcal{U}}^{[i]}(\tau )\times {\mathcal{V}}_{\gamma }^{[i]}(\tau )\times {\mathcal{Y}}^{[i]}(\tau )$$

$$({\bar{y}}^{[i]}(t+T),{\bar{\eta }}^{[i]}(t+T))\in {\mathcal{Y}}_{aux}^{[i]}(t+T)\times \mathcal{B}(r_{\eta }^{[i]})$$

$$|{\bar{\eta }}^{[i]}(\tau )|\le a_{\eta }^{[i]}{e}^{-{\lambda }_{\eta }^{[i]}(t-t_0)}$$

$$J_T^{[i]}$$

$$+m^{[i]}(t+T,{\bar{x}}^{[i]}(t+T),{\bar{\gamma }}^{[i]}(t+T))$$

$$\displaystyle+\int_{t}^{t+T}l_{c}^{[i]}(\bar{\eta}^{[i]},\bar{v}_{\gamma}^{[i]})d\tau+\frac{1}{2}m_{\eta}^{[i]}\big{(}\bar{\eta}^{[i]}(t+T)\big{)}^{2}$$

$$\Vert x(t)\Vert$$

$$+{\sigma }_y(\Vert y{\Vert }_{[t_0,t)})+{\sigma }_x$$

$$m(\hat{t}+\delta ,x(\hat{t}+\delta ),\gamma (\hat{t}+\delta ))-m(\hat{t},x(\hat{t}),\gamma (\hat{t}))\le -{\int }_{\hat{t}}^{\hat{t}+\delta }l(t)dt$$

$$\frac{d}{dt}\frac{1}{2}{m}_{\eta }{\eta }^2=-m_{\eta }{\lambda }_{\eta }{\eta }^2\le -l_c(\eta ,-{\lambda }_{\eta }\eta )$$

$$|l(t,x,u,{\gamma }_1,u_{\gamma ,1})-l(t,x,u,{\gamma }_2,u_{\gamma ,2})|\le C_l\Vert \left[\begin{array}{c}{\gamma }_1-{\gamma }_2\\ u_{\gamma ,1}-u_{\gamma ,2}\end{array}\right]\Vert$$

$$|m(t,x,{\gamma }_1)-m(t,x,{\gamma }_2)|\le C_m\Vert \left[\begin{array}{c}{\gamma }_1-{\gamma }_2\\ u_{\gamma ,1}-u_{\gamma ,2}\end{array}\right]\Vert$$

$${\xi }^{[i]}(k+1)={\xi }^{[i]}(k)+k_{con}({\xi }^{[i]}(k),{\xi }_{{\mathcal{N}}^{[i]}}(k))+{\eta }^{[i]}(k)$$

$$\Vert {\eta }^{[i]}(k)\Vert \le a_{\eta }^{[i]}{e}^{-{\lambda }_{\eta }^{[i]}(k-k_0)}$$

$$\Vert k_{con}({\xi }^{[i]}(k),{\xi }_{{\mathcal{N}}^{[i]}}(k))\Vert \le \beta (\sum _{i\in \mathcal{I}}\Vert {\xi }_0^{[i]}\Vert ,k-k_0).$$

$$k_{con}({\xi }^{[i]},{\xi }_{{\mathcal{N}}^{[i]}})=-\overline{ϵ}\sum _{j\in {\mathcal{N}}^{[i]}}{a}_{ij}({\xi }^{[i]}-{\xi }^{[j]}),$$

$${\bar{u}}_{\gamma ,au{x}_t}^{[i]}(\tau )$$

$$\dot{y}$$

$$=-\Omega (\omega )y+\Omega (\omega )ϵ+{\left[\begin{array}{ccc}{v}_1& 0& 0\end{array}\right]}^{\prime }-R^{\prime }\dot{c_d}$$

$$=-\Omega (\omega )y-R^{\prime }{c}_{d_{\gamma }}(\gamma )\dot{\gamma }+\Delta u,\Delta :=\left[\begin{array}{ccc}1& {ϵ}_3& -{ϵ}_2\\ 0& 0& {ϵ}_1\\ 0& -{ϵ}_1& 0\end{array}\right]$$

$$k_{aux}(\cdot )=\{\begin{array}{ll}{\Delta }^{-1}\left(R^{\prime }\frac{\partial }{\partial \gamma }{c}_d(\gamma )(v_d+u_{\gamma })-K\frac{y}{\Vert y\Vert }\right),& \Vert y\Vert \ne 0\\ {\Delta }^{-1}{R}^{\prime }\frac{\partial }{\partial \gamma }{c}_d(\gamma )(v_d+u_{\gamma }),& \Vert y\Vert =0\end{array}$$

$$\dot{W}=\{\begin{array}{ll}\frac{y^{\prime }\dot{y}}{\Vert y\Vert }=-\frac{y^{\prime }Ky}{\Vert y{\Vert }^2}\le -{\lambda }_{min}(K),& \Vert y\Vert \ne 0\\ 0,& \Vert y\Vert =0,\end{array}$$

$$\Vert y(\tau )\Vert \le \{\begin{array}{ll}\Vert y(t)\Vert -{\lambda }_{min}(K)(\tau -t),& \tau \in [t,\bar{t}]\\ 0,& \tau >\bar{t}\end{array}$$

$$l(t,x,u,\gamma ,u_{\gamma })=\Vert y{\Vert }_Q^2+\Vert u-k_{aux}(t,x,\gamma ,u_{\gamma }){\Vert }_U^2$$

Full text

I Introduction

Important applications using multiple autonomous robotic vehicles that arise in a number of scenarios including searching, surveying, and exploration, motivate the formulation of a particular class of motion control tasks that can be described as cooperative path-following (CPF) problems.

In simple terms, given a finite set of vehicles and conveniently desired geometric paths (one per vehicle), and an underlying communication topology that establishes the communication between them, the CPF problem consists of designing the motion control algorithms for each vehicle to drive and maintain them in their respective desired paths with a common speed profile and holding a specified formation pattern. Different solutions to this problem and similar problems can be found in the literature, see e.g., [1, 2, 3, 4, 5, 6, 7, 8, 9, 10], and the references therein.

A particular interesting strategy to solve the problem consists of decoupling it into (i) a path following goal, where the objective is to derive control laws to drive each vehicle to its path at the reference speed profile, and (ii) a multiple vehicle coordination task, where the objective is to adjust the speed of each vehicle so as to achieve the desired formation pattern by running a consensus like algorithm that should take explicitly into account the topology of the inter-vehicle communications network. The works in [1, 7] offer a theoretical overview of the subject and introduce techniques to solve this problem. In those works, a key idea is to parameterize by a single variable each desired geometric path with the restriction that when all the vehicles are following the respective desired paths in the required formation pattern, the parameterization variables have the same value, that is, the path variables are all synchronized. Then, control laws are independently designed for path-following and consensus.

In this paper, contrary to the works mentioned above, we propose an optimization-based CPF design methodology that optimally balances in an integrated form the transient trade-off between coordination and path-following errors. To this end, we consider a more general framework that captures the case of a multi-agent system where each agent is associated with a continuous dynamical model that includes an output equation that is a function of the state of the agent and a coordination vector. For a given network topology, each agent can access its state and coordination vector, as well as the coordination vectors of the neighboring agents. The goal in this setup is to design a distributed control law that steers the output signals to the origin, while simultaneously driving the coordination vectors of the agents of the network to consensus. To solve this, we propose a model predictive control scheme that builds on a pre-existing auxiliary consensus control law to design a performance index that combines the output regulation objective with the consensus objective. It is important to stress that this coordinated output regulation problem, which is formally introduced in Section II, stands at the intersection of the consensus problem in multi-agent systems (MAS) and the output stabilization problem.

The consensus problem addresses the design of distributed control laws for the agents of a MAS to be able to steer the state of each agent to a common value. The control law is distributed, in the sense that for a given network topology each agent can only use measurements of its state and the state of the neighboring agents. The high relevance of such multi-agent system coordination problem is well motivated by the existence of an extensive literature on the topic, and we refer to, e.g., [11, 12, 13, 14] for an introduction to it.

The Model Predictive Control (MPC) solves the stabilization problem by selecting among the future feasible input signals, the one that minimizes a given (and suitable) performance index defined in a finite horizon, applying the first part of the optimal input signal, and iterating the process. For MPC solutions to the trajectory tracking and path-following problem, we refer the works in [15, 16, 17]. For a general survey on MPC, the works [18, 19, 20], and [21] for a classification of different MPC architectures in MAS.

One of the main challenges in the design of MPC schemes for MAS stems from the fact that every agent can only predict its own future state and input trajectory without the knowledge of the future behavior of the neighboring systems. To address this problem, in [22, 23, 24], the controllers employ a specific constraint, often called consistency constraint, devoted to guaranteeing that the optimal prediction of each agent stays close to the one obtained at the previous time instant, which was exchanged among neighboring agents. This information is then used in the analysis to obtain closed-loop guarantees. For the case of systems affected by noise, the work in [23] provides guarantees of convergence to the minimal disturbance invariant set. For the case of single-/double-integrator systems, the method proposed in [25] uses a performance index that prompts a contraction of the states of the network and therefore, building on [26], guarantees convergence to consensus.

In this work, for a specific class of systems, we propose a different MPC framework that does not require sharing the prediction of the state with the other neighboring systems and thus does not employ consistency constraints, but it only shares the coordination vectors that indirectly define when the systems are in coordination. Because of this, the knowledge of the dynamical model of the neighboring agents, their state, or prediction of the state, are not required, and only the value of their coordination vectors is shared. In addition, building on a pre-existing auxiliary consensus control law, only the sharing of the current value of the coordination vector, and not its prediction, is required.

The propose optimization-based control law is able to drive the output signals of the agents in a MAS to the origin, where such output is a function of a coordination vector that we wish to drive to consensus. In this approach, one can consider heterogeneous agents, where each agent may have a possible different dynamical model, and we provide convergence guarantees under which it is possible to solve the proposed MAS coordinated output regulation problem. We focus on distributed non-iterative MPC schemes, i.e., the information is only exchanged among neighboring agents, and the communication is only performed once per every solution of the MPC optimization problem. This is in contrast to iterative methods, where the MPC controllers need to communicate multiple times to solve the local MPC problem. Furthermore, the proposed scheme falls in the category of non-cooperative MPC schemes, where each agent minimizes an independent performance index. In the single-agent case, the proposed scheme also provides a solution to the trajectory tracking and path following problem.

A subset of the results reported here was presented in [27]. In this paper, we focus more on the CPF framework problem, relax the assumptions on the model formulation and design procedure, and provide an extended proof of the results.

The remainder of the paper is organized as follows. Section II starts with the description of the motivated CPF example for a class of 3D nonholonomic robotic vehicles and ends with the formulation of the more general coordinated output regulation problem for multi-agent systems. Next, Section III proposes the optimization-based control framework with convergence guarantees. For the sake of clarity, all the proofs are reported in the Appendix. In Section IV, we particularize the framework for the motivating example and present numerical simulations that show the effectiveness and performance of the proposed scheme. Conclusions are presented in Section V.

Notation and definitions: The term $\mathcal{C}(a,b)$ and $\mathcal{PC}(a,b)$ denotes the space of continuous and piecewise continuous trajectories, respectively, defined over $[a,b]$ or $[a,+\infty)$ for the case where $b=+\infty$. For a generic time-dependent trajectory, we use the bold notation to refer to the whole trajectory and the normal notation to refer to the trajectory evaluated at a specific point in time, e.g., for a continuous-time trajectory $x:\mathbb{R}_{\geq t_{0}}\to\mathbb{R}^{n}$ we write $\boldsymbol{x}\in\mathcal{C}(t_{0},+\infty)$ and $x(t)\in\mathbb{R}^{n}$, with $t\geq t_{0}$. For a given $n\in\mathbb{N}$, $SE(n)$ denotes the Cartesian product of $\mathbb{R}^{n}$ with the group $SO(n)$ of $n\times n$ rotation matrices and $se(n)$ denotes the Cartesian product of $\mathbb{R}^{n}$ with the space $so(n)$ of $n\times n$ skew-symmetric matrices. For a generic time $t\geq t_{0}$, the superscript ⋆t is used to denote all the trajectories of a given signal associated with the optimal predictions of $\mathcal{P}(t)$. The terms $I_{n}$ and $\mathbf{1}_{n}$ denote the $n$-by-$n$ identity matrix and the $n$-by-$1$ column vector with all components equal to $1$. For a generic set $\mathcal{S}$ the term $\operatorname{int}(\mathcal{S})$ denotes the interior of $\mathcal{S}$. The term $\mathcal{B}(r)$ denotes the closed ball set of radius $r\geq 0$, i.e., $\mathcal{B}(r):=\{x:\|x\|\leq r\}$, where for a generic vector $v$ and matrix $M$ the terms $\|v\|=\sqrt{v^{\prime}v}$ and $\|M\|=\max_{v\neq 0}(\|Av\|/\|v\|)$ denote the vector norm and the associated induced matrix norm, respectively. In addition, for a square matrix $A$, we define $\|v\|^{2}_{A}:=v^{\prime}Av$. For a generic matrix $A$, $[A]_{i}$ denotes its $i$-th row vector. Given a set $\mathcal{T}:=~{}\{t_{0},t_{1},\dots\}\subset\mathbb{R}$ of time instants, we denote with $\lfloor t\rfloor$ the maximum sampling instant $t_{k}\in\mathcal{T}$ smaller than or equal to $t$, i.e., $\lfloor t\rfloor=\max_{k\in\mathbb{N}_{\geq 0}}\{t_{k}~{}\in~{}\mathcal{T}:t_{k}~{}\leq~{}t\}.$ We also use the simplified notation of omitting the time dependence and in particular, for the $\tau$ dependent signals $\boldsymbol{\bar{x}}^{b}_{a}$, $\boldsymbol{\bar{u}}^{b}_{a}$, $\boldsymbol{\bar{\gamma}}^{b}_{a}$ $\boldsymbol{\bar{v}}^{b}_{a}$, a function $l(\cdot)$ evaluated as $l(\tau,\bar{x}^{b}_{a}(\tau),\bar{u}^{b}_{a}(\tau),\bar{\gamma}^{b}_{a}(\tau),\bar{v}^{b}_{a}(\tau))$ can be denoted by $l(\tau,\bar{x}^{b}_{a},\bar{u}^{b}_{a},\bar{\gamma}^{b}_{a},\bar{v}^{b}_{a})$ or $\bar{l}^{b}_{a}(\tau)$, whenever clear from the context.

II Motivating Example and Problem Statement

In this section, as a motivating example, we formulate a cooperative path-following control problem for a class of 3D nonholonomic robotic vehicles that fits the more general problem that will be described afterwards.

II-A Cooperative path-following example

Let $\{I\}$ be an inertial coordinate frame and $\{B\}^{[i]}$ a body coordinate frame attached to the generic vehicle $i$. The pair $(p^{[i]}(t),R^{[i]}(t))\in SE(3)$ denotes the configuration of the vehicle, position and orientation, where $R^{[i]}(t)$ is the rotation matrix from body to inertial coordinates. Now, let $(v^{[i]}(t),\Omega(\omega^{[i]}(t)))~{}\in~{}se(3)$ be the twist that defines the velocity of the vehicle, linear and angular, where the matrix $\Omega(\omega^{[i]}(t))$ is the skew-symmetric matrix associated with the angular velocity $\omega^{[i]}(t):=\begin{bmatrix}\omega_{1}^{[i]}(t)&\omega_{2}^{[i]}(t)&\omega_{3}^{[i]}(t)\end{bmatrix}^{\prime}$, defined as $\Omega(\omega^{[i]}):={\footnotesize\begin{pmatrix}0&-\omega_{3}^{[i]}&\omega_{2}^{[i]}\\ \omega_{3}^{[i]}&0&-\omega_{1}^{[i]}\\ -\omega_{2}^{[i]}&\omega_{1}^{[i]}&0\end{pmatrix}.}$ The kinematic model of the body frame satisfies

[TABLE]

In this example, we consider the nonholonomic case with the only actuation in the forward linear velocity, that is, $v^{[i]}(t)~{}=~{}\begin{bmatrix}~{}v^{[i]}_{1}(t)&0&0\end{bmatrix}^{\prime}$, and in the two components of the angular velocity associated with the pitch and yaw motion, that is, $\omega_{1}^{[i]}(t)=0$. Therefore, the control input is given as $u^{[i]}(t):=\begin{bmatrix}v^{[i]}_{1}(t)&\omega_{2}^{[i]}(t)&\omega_{3}^{[i]}(t)\end{bmatrix}^{\prime}.$ Let $c^{[i]}(t)\in\mathbb{R}^{3}$ be a constant point in the body frame placed at a nonzero distance from the center of rotation of $\{B\}^{[i]}$ such that $c^{[i]}(t):=p^{[i]}(t)+R^{[i]}(t)\epsilon^{[i]}$, with $\epsilon^{[i]}\in\mathbb{R}^{3}$.

Each vehicle is associated with a differentiable desired path $c_{d}^{[i]}:\mathbb{R}\to\mathbb{R}^{3}$ parametrized by $\gamma^{[i]}(t)\in\mathbb{R}$ and it is assumed that it can communicate (transmit and receive) a coordination parameter with the other vehicles according to a given graph $\mathcal{G}=(\mathcal{V},\mathcal{E})$ that defines the underlying communication topology.

The cooperative path-following (CPF) problem consists in designing motion control laws for the vehicles such that, as $t$ goes to infinity, the following holds:

• •

Convergence to the paths: The vectors $c^{[i]}(t)$ of the vehicles converge to $c_{d}^{[i]}(\gamma^{[i]}(t))$;

• •

Coordination: The network disagreement function

[TABLE]

converges to the origin;

• •

Desired velocity along the path: The rate of the path parameters $\dot{\gamma}^{[i]}(t)$ converge to a desired constant value $v_{d}\in\mathbb{R}$.

One important difference with respect to the other works in the literature is that here we are looking for an optimization-based framework that integrates the path-following problem and the consensus problem with a combined cost function. Additional, we also consider input and output restrictions, where the output is the error that we would like to regulate to zero. More precisely, we define the output

[TABLE]

with $t\geq t_{0}$. Notice that $y^{[i]}(t)=0$ if and only if $c^{[i]}(t)~{}=~{}c_{d}^{[i]}(\gamma^{[i]}(t))$. The rotation matrix is introduced to obtain an output with time derivative that directly depends on the input, as is made explicit in Section IV-A2.

For the numerical results described in Section IV-B, we present an illustrative scenario where three vehicles are tasked to perform CPF along the geometric paths denoted in black in Fig. 3. In the desired configuration, the vehicles should move parallel to each other. Therefore, the path of each vehicle is parametrized such that if $\gamma^{[1]}=\gamma^{[2]}=\gamma^{[3]}$ then the desired positions $c_{d}^{[1]}(\gamma^{[1]})$, $c_{d}^{[2]}(\gamma^{[2]})$, and $c_{d}^{[3]}(\gamma^{[3]})$ are aligned as specified. The desired velocity of the parameter is chosen as $v_{d}=2$. The communication topology is set to be such that Vehicle 2, which is assigned to the path in the middle, communicates bidirectionally with Vehicles 1 and 3, but Vehicles 1 and 3 cannot communicate with each other.

II-B Problem definition

The motivated CPF problem can be generalized as follows: Consider a set of $n_{\mathcal{I}}\in\mathbb{Z}_{\geq 1}$ agents (that is, dynamical systems, e.g., robotic vehicles) where the generic $i$-th agent, $i\in{\mathcal{I}}:=\{1,2,\ldots,n_{\mathcal{I}}\}$ is described by the continuous-time state equation

[TABLE]

The output of each system is defined as

[TABLE]

that is constrained within the output constrain set denoted by $\mathcal{Y}^{[i]}~{}:~{}\mathbb{R}_{\geq t_{0}}~{}\rightrightarrows~{}\mathbb{R}^{p^{[i]}}$.

Given the described setup, we can now formulate the following problem:

Problem 1** (Coordinated output regulation)**

Design a control law for the input signals $\boldsymbol{u}^{[i]}\in\mathcal{PC}(t_{0},\infty)$ and $\boldsymbol{u}^{[i]}_{\gamma}\in\mathcal{PC}(t_{0},\infty)$, $i\in\mathcal{I}$, using only the information $x^{[i]}(t_{k})$ and $\gamma_{\mathcal{N}^{[i]}}(t_{k})$ available at the time instants $t_{k}\in\mathcal{T}:=~{}\{t_{0},t_{1},\dots\}$, such that for every $i\in{\mathcal{I}}$ the state vectors $x^{[i]}(t)$ and $y^{[i]}(t)$ are bounded for all $t\geq t_{0}$, and as time approaches infinity the following holds:

The output vector $y^{[i]}(t)\in\mathbb{R}^{p^{[i]}}$ converges to the origin; 2. 2.

*The network disagreement function defined in (2) converges to the origin; * 3. 3.

The vector $\dot{\gamma}^{[i]}(t)$ converges to the predefined value ${g}(t)\in\mathbb{R}^{n_{c}}$, that is, $u^{[i]}_{\gamma}(t)$ converges to zero. $\square$

The problem addressed captures applications where it is important to solve in a unified way the output regulation problem and the consensus problem. In Problem 1, the output is regulated to the origin without loss of generality. In fact, if a different output trajectory is desired, it is always possible to make a coordinate transformation with a new virtual output given by the difference between the original output and the desired one. Notice that for the particular case of ${g}(t)=0$, the design of a distributed control law for $u_{\gamma}^{[i]}$, with $i\in\mathcal{I}$ corresponds to solving the standard consensus problem for single-integrator systems. Adding a term ${g}(t)\neq 0$ in (4b), common to all the vehicles, does not affect the value of the disagreement function, which only regards to the relative values, and allows the coordination parameters to converge to a (possibly time-varying) desired signal that is important in many applications, see, e.g., the motivating example, where $g(t)$ corresponds to the desired formation velocity $v_{d}$.

III MPC Design and Convergence Analysis

This section proposes a sampled-data MPC approach with convergence guarantees that solves Problem 1. The key idea behind the proposed scheme is to define a performance index composed by a term for the output tracking MPC and another term that rewards the consensus among the coordination vectors.

Fig. 1 shows a block diagram of the proposed control architecture, where the blue block corresponds to the algorithm that runs in each agent, which includes two internal states. The first one is the coordination vector $\gamma^{[i]}$ that evolves in time according to (4b) with $u^{[i]}_{\gamma}$ given by a continuous-time auxiliary consensus input signal $u^{[i]}_{\gamma,aux_{t}}$, derived in Section III-B, plus an extra signal $\eta^{[i]}$, that is, $u^{[i]}_{\gamma}(\tau)=u^{[i]}_{\gamma,aux_{t}}(\tau)+\eta^{[i]}(\tau).$ The second internal state $\eta^{[i]}\in\mathbb{R}^{n_{c}}$ evolves according to

[TABLE]

with initial condition $\eta_{0}^{[i]}\in\mathbb{R}^{n_{c}}$ and can be viewed as an error signal that denotes the difference between the auxiliary consensus control law computed in $u^{[i]}_{\gamma,aux_{t}}$ and the actual signal $u_{\gamma}^{[i]}$. This error signal is actuated through $v^{[i]}_{\gamma}$ that is one of the outputs of the MPC controller. The other output is $u^{[i]}$ that directly commands the evolution of the state vector $x^{[i]}$ according to (4a).

The MPC controller is a sampled-data scheme that solves at every time sample $t_{k}\in\mathcal{T}$ an optimization problem. The obtained optimal inputs are applied open-loop to the system within the generic interval $t\in[t_{k},t_{k+1})$, with $k\in\mathbb{Z}_{\geq 0}$, i.e.,

[TABLE]

The optimization is defined as follows.

[TABLE]

Definition 1** (Open-loop MPC problem)**

Given the tuple of parameters $p=(t,x^{[i]},\gamma^{[i]},\gamma_{\mathcal{N}^{[i]}},\eta^{[i]})\in\mathbb{R}_{\geq t_{0}}\times\mathbb{R}^{n^{[i]}}\times\mathbb{R}^{n_{c}}\times\mathbb{R}^{|\mathcal{N}^{[i]}|n_{c}}\times\mathbb{R}^{n_{c}}$, an horizon length $T\in\mathbb{R}_{>0}$, and the auxiliary signal $\boldsymbol{\bar{u}}^{[i]}_{\gamma,aux_{t}}\in\mathcal{C}(t,+\infty)$ that is introduced in (LABEL:eq:ubaric), the open-loop MPC optimization problem $\mathcal{P}(p)$ consists in finding the optimal control signals $\boldsymbol{\bar{u}}^{\star[i]}\in\mathcal{PC}(t,t+T)$ and $\boldsymbol{\bar{v}}_{\gamma}^{\star[i]}\in\mathcal{PC}(t,t+T)$ that solve

[TABLE]

where $\tau\in[t,t+T]$ and

[TABLE]

and $\lambda_{\eta}^{[i]}>0$, $a_{\eta}^{[i]}\geq 0$, $m_{\eta}^{[i]}\geq 0$, $r_{\eta}^{[i]}\geq 0$. $\square$

From (7e), one can see that the finite horizon cost $J^{[i]}_{T}(\cdot)$, which corresponds to the performance index of the MPC controller, is composed of two stage costs, i.e., the regulation stage cost $l^{[i]}~{}:~{}\mathbb{R}_{\geq t_{0}}~{}\times~{}\mathbb{R}^{n^{[i]}}~{}\times~{}\mathbb{R}^{m^{[i]}}\times\mathbb{R}^{n_{c}}\times\mathbb{R}^{n_{c}}~{}\rightarrow~{}\mathbb{R}_{\geq 0}$ and the consensus stage cost $l^{[i]}_{c}~{}:~{}\mathbb{R}^{n_{c}}\times\mathbb{R}^{n_{c}}\rightarrow~{}\mathbb{R}_{\geq 0}$, and two terminal costs, i.e., the regulation terminal cost $m^{[i]}~{}:~{}\mathbb{R}_{\geq t_{0}}\times\mathbb{R}^{n_{c}}\times\mathbb{R}^{n_{c}}~{}\rightarrow~{}\mathbb{R}_{\geq 0}$, and the consensus terminal cost {1\over 2}m_{\eta}^{[i]}\big{(}\bar{\eta}^{[i]}(t+T)\big{)}^{2}. The key idea to combine these two stage costs is to optimally balance the output regulation objective ($\bar{y}^{[i]}(\tau)=0$) and the consensus objective ($\bar{\eta}^{[i]}(\tau)=0$) according to the weights specified in the performance index. To ensure that this difference given by $\eta$ converges to zero, we include the constraint (7d), which also implies a specific exponentially bound. Although, in practice, taking high values of $a_{\eta}^{[i]}>0$ and small values of $\lambda_{\eta}^{[i]}>0$, such constraint is never active and the convergence rate of $\bar{\eta}^{[i]}$ is fully determined by the performance index. Furthermore, constraint (7d) can be omitted if the vehicle is disconnected from the network ($\mathcal{N}^{[i]}=\emptyset$ and $i\notin\mathcal{N}^{[j]}$ with $j=\mathcal{I}$, $i\neq j$), in which case the scheme results in a pure output regulation controller. The regulation terminal cost is defined over the set of $(t,x^{[i]},\gamma^{[i]})$ such that the associated $y^{[i]}(t)\in\mathcal{Y}_{aux}^{[i]}(t)$ belongs to the regulation terminal set $\mathcal{Y}_{aux}^{[i]}:\mathbb{R}_{\geq t_{0}}\rightrightarrows\mathbb{R}^{p^{[i]}}$. Similarly, the consensus terminal cost is evaluated with $\bar{\eta}^{[i]}(t+T)$ constrained in the consensus terminal set $\mathcal{B}(r_{\eta}^{[i]})$.

The main result of this section follows next. For the sake of clarity, the assumptions and the associated design methods are reported in Sections III-A and III-B.

Theorem 1

Consider the set of constrained dynamical systems (4) that communicates according to a communication network $\mathcal{G}=(\mathcal{V},\mathcal{E})$ as described in Section II. If Assumptions 1-7 hold, then the proposed sampled-data MPC control law (6) solves Problem 1. The region of attraction of the proposed controller corresponds to the set of initial conditions of the system such that the open-loop MPC problem is feasible.

Remark 1

The knowledge of the state of the neighboring systems of agent $i$ is indirectly captured by their coordination vectors. For instance, in the cooperative path-following example, the coordination vector $\gamma^{[i]}$ defines where the desired position is within the desired path, i.e., $c_{d}^{[i]}(\gamma^{[i]})$, and formation is achieved when all the coordination vectors are in consensus (and the associated path-following errors are zero). Because of this, the knowledge of the dynamical model of the neighboring agents, their state, or prediction of the state, is not required, and only the value of their coordination vectors is shared. In addition, it is worth noticing that the controller only shares the current value of the coordination vector, and not its prediction, because the proposed scheme builds on a pre-existing auxiliary consensus control law that only requires the current values. The MPC controller modifies such auxiliary control law with an additive vanishing term that is used to optimally balance the coordination objective with the path-following objective.

III-A Main assumptions

Whenever it is clear from the context, we omit the explicit superscript $[i]$ to improve the clarity of the paper. The first set of conditions is obtained by adapting the standard MPC sufficient conditions for state convergence to the origin in the state space, to the convergence of the output to the origin in the output space (see, e.g., [28] with Remark 18 for the generic case of time-varying system considered in this paper).

Assumption 1

The input constraint set $\;\mathcal{U}(t)$ is compact for all $t\geq t_{0}$ and uniformly bounded over time. Moreover, the function $f(\cdot)$ in (4a) is locally Lipschitz in $x$, piecewise continuous in $t$ and $u$, and bounded for bounded $x$ in the region of interest, i.e., the set $\{\|f(t,x,u)\|:t\geq t_{0},x\in\bar{\mathcal{X}},u\in\mathcal{U}(t)\}$ is bounded for any bounded $\bar{\mathcal{X}}\subset\mathbb{R}^{n}$. $\square$

Assumption 2

The function $g:\mathbb{R}_{\geq t_{0}}\to\mathbb{R}^{n_{c}}$ in (4b) is uniformly bounded over time. $\square$

To guarantee that the state trajectories will be uniformly bounded over time provided that the output and input trajectories are bounded, the following assumption is considered.

Assumption 3

Consider the output defined in (4c).

The system (4) satisfies for all $t\geq t_{0}$ the condition

[TABLE]

for a class-$\mathcal{KL}$* function $\beta_{x}~{}:\mathbb{R}_{\geq 0}~{}\times~{}\mathbb{R}_{\geq 0}~{}\to~{}\mathbb{R}_{\geq 0}$, class-$\mathcal{K}$ functions $\sigma_{u},\sigma_{y}~{}:~{}\mathbb{R}_{\geq 0}~{}\to~{}\mathbb{R}_{\geq 0}$, and a constant scalar $\sigma_{x}\geq 0$ that could be dependent on the initial condition $x_{0}$;* 2. 2.

The gradient of the right-hand side of the output equation (4c) $\nabla h(t,x,\gamma)$ is uniformly bounded over time for bounded values of $x$, i.e., for all $x$ with $\|x\|\leq B$, there exists a scalar $b_{B}>0$, possibly dependent on $B\geq 0$ such that $\|\nabla h(t,x,\gamma)\|\leq b_{B}$ for all $t\geq t_{0}$ and $\gamma\in\mathbb{R}^{n_{c}}$. $\square$

Condition (8) is used to avoid cases where the system has, e.g., non-observable unstable states. More specifically, for the case $\sigma_{x}=0$, inequality (8) corresponds to the input-output-to-state stability (IOSS) condition (see, e.g., [29, 30]) that represents a detectability condition of the state from the output (see also [31] and Proposition 2.6 of [30] for the case of linear systems). It is worth noticing that this condition does not imply a-priori that system (4a) needs to be stable (it can be indeed open loop unstable). It is however possible to conclude stability of the closed-loop (or more precisely internal stability) provided that one can show that the closed-loop system is external stable (see [32] where it is shown the equivalence between the IOSS and the input-to-output stability (IOS) with the input-to-state stability (ISS) property). Thus, only after properly regulating the output it is possible to guarantee the boundedness of the state trajectory. In this paper, we further relax this assumption allowing $\sigma_{x}\geq 0$. Broadly speaking, (8) requires the state of the system to be uniformly bounded for uniformly bounded output and input signals. Similarly, also the item 2 of the latter assumption is rather general, and we refer to the illustrative example for more insight on when such conditions hold.

In a classic MPC scheme, to guarantee closed-loop convergence of the state trajectory to the origin, the performance index and the constraint sets of the open-loop MPC problem are required to satisfy a set of sufficient conditions (see, e.g., [33, 34, 35]). In the following two assumptions, similar sets of conditions are required to guarantee that $y$ and $\eta$, respectively, converge to the origin.

Assumption 4

The following hold:

The output constraint set $\mathcal{Y}(t)$ and the terminal set $0~{}\in~{}\mathcal{Y}_{aux}(t)~{}\subseteq~{}\mathcal{Y}(t)$ are closed, connected, and contain the origin for all $t\geq t_{0}$; 2. 2.

There is a class-$\mathcal{K}_{\infty}$* function $\alpha_{s}:\mathbb{R}_{\geq 0}~{}\rightarrow~{}\mathbb{R}_{\geq 0}$ such that $l(\tau,\bar{x},\bar{u},\bar{\gamma},\bar{u}_{\gamma})~{}\geq~{}\alpha_{s}(\|y\|)$ for all $(\tau,\bar{x},\bar{u},\bar{\gamma},\bar{u}_{\gamma})\in\mathbb{R}_{\geq t_{0}}\times\mathbb{R}^{n}\times\mathcal{U}(\tau)\times\mathbb{R}^{n_{c}}\times\mathbb{R}^{n_{c}}$;* 3. 3.

For any given values of $(x,u,\gamma,u_{\gamma})\in\mathbb{R}^{n}\times\mathbb{R}^{m}\times\mathbb{R}^{n_{c}}\times\mathbb{R}^{n_{c}}$ the functions $l(t,x,u,\gamma,u_{\gamma})$ and $m(t,x,\gamma)$ are uniformly bounded over time $t\geq t_{0}$; 4. 4.

There exists a auxiliary regulation control law $k_{aux}~{}:~{}\mathbb{R}_{\geq t_{0}}~{}\times~{}\mathbb{R}^{n}~{}\times~{}\mathbb{R}^{n_{c}}~{}\times~{}\mathbb{R}^{n_{c}}~{}\rightarrow~{}\mathbb{R}^{m}$ such that, for the associated closed-loop system (4a) with $u(t)=k_{aux}(t,x,\gamma,u_{\gamma})$, (4b) with $u_{\gamma}(t)=\eta(t)$, and (5) with $v_{\gamma}(t)=-\lambda_{\eta}\eta(t)$, $\lambda_{\eta}>0$, with initial time and states $(\hat{t},\hat{x},\hat{\gamma},\hat{\eta})\in\mathbb{R}_{\geq t_{0}+T}\times\mathbb{R}^{n}\times\mathbb{R}^{n_{c}}\times\mathbb{R}^{n_{c}}$, for all $t\geq t_{0}+T$ the input and output vectors satisfy $u(t)~{}\in~{}\mathcal{U}(t)$ and $y(t)~{}\in~{}\mathcal{Y}_{aux}(t)$ and the condition

[TABLE]

holds for any $\delta>0$. $\square$

Assumption 4.4 requires the existence of an auxiliary control law able to steer the system to track the output trajectory obtained when $\gamma(t)$ is driven by the (dynamic) controller $u_{\gamma}(t)=\eta(t)$, $\dot{\eta}(t)=v_{\gamma}(t)=-\lambda_{\eta}\eta(t)$. The design of a suitable terminal set and terminal cost follows analogously to classic MPC, where the auxiliary controller drives the system to the origin, instead of driving the output to a trajectory. Specifically, if $k_{aux}(\cdot)$ is given with a global Lyapunov function $V_{aux}:\mathbb{R}_{\geq t_{0}+T}\times\mathbb{R}^{p}\times\mathbb{R}^{n_{c}}\to\mathbb{R}_{\geq 0}$ such that $\alpha_{aux,1}(\|y\|)\leq V_{aux}(t,x,\gamma)\leq\alpha_{aux,2}(\|y\|)$, $\alpha_{aux,3}(\|y\|)\geq l(\tau,x,k_{aux}(\cdot),\gamma,u_{\gamma})$, and $\dot{V}_{aux}(t,x,\gamma)\leq-\alpha_{aux,3}(\|y\|)$ for three class-$\mathcal{K}_{\infty}$ functions $\alpha_{aux,1}(\cdot)$, $\alpha_{aux,2}(\cdot)$, and $\alpha_{aux,3}(\cdot)$, then $m(t,x,\gamma)=V_{aux}(t,x,\gamma)$ and $\mathcal{Y}_{aux}=\mathbb{R}^{p}$ is a valid choice of terminal cost and terminal set. Moreover, if the conditions on $V_{aux}$ only hold in a neighborhood $\mathcal{N}$ around $y=0\in\operatorname{int}(\mathcal{N})$, Assumption 4.4 is satisfied by choosing the set $\mathcal{Y}_{aux}(\cdot)$ to be a level set of $m(\cdot)$ (therefore positively-invariant) contained in $\mathcal{N}$. Notice that, although for the case of differentiable terminal costs (9) can be rewritten as $\dot{m}(t,x,\gamma)\leq-l(t,\bar{x},k_{aux}(\cdot),\bar{\gamma},\bar{u}_{\gamma})$, we adopt the integral representation to allow non-differentiable terminal cost, as it is the case of the illustrative example in this paper.

Assumption 5

The following hold:

The constraint set $\mathcal{V}_{\gamma}(t)$ is compact, uniformly bounded over time and such that $\mathcal{B}(r_{\eta}\lambda_{\eta})\subseteq\mathcal{V}_{\gamma}(t)$, for all $t\geq t_{0}$; 2. 2.

*The consensus cost function $l_{c}(\cdot)$ is zero with $\eta~{}=~{}0$ and the function $\alpha_{c}:\mathbb{R}_{\geq 0}\rightarrow\mathbb{R}_{\geq 0}$ is such that $l_{c}(\eta,v_{\gamma})~{}\geq\alpha_{c}(\|\eta\|)$ for all $(\eta,v_{\gamma})~{}\in~{}\mathbb{R}^{n_{c}}\times\mathbb{R}^{n_{c}}$, and $m_{\eta}\lambda_{\eta}\eta^{2}\geq l_{c}(\eta,-\lambda_{\eta}\eta)$. *$\square$

Similarly to (4) in Assumption 4, Assumption 5 implies that, for the closed-loop (5) with $v_{\gamma}=-\lambda_{\eta}\eta$, the consensus terminal cost ${1\over 2}m_{\eta}\eta^{2}$ satisfies

[TABLE]

that is used in the convergence analysis of the proposed controller.

The proposed MPC controller balances the regulation objective (Problem 1, item 1) with the consensus objective (Problem 1 items 2 and 3). Since such objectives are generally conflicting in a transient phase, the performance index of the MPC controller can be tuned to favorite either output regulation or consensus. The existence of the auxiliary control law in Assumption 4.4 implicitly implies that, asymptotically, both objectives can be satisfied (i.e., $\phi(t)=0$ and $y^{[i]}(t)=0$, $i\in\mathcal{I}$, is a feasible solution for system (4) for all $t\geq t_{0}$).

Assumption 6

Consider the open-loop MPC optimization problem from Definition 1. The regulation stage cost $l(\cdot)$ and the regulation terminal cost $m(\cdot)$ are Lipschitz continuous on $(\gamma,u_{\gamma})$, i.e., there exists a pair of constants $C_{l}\geq 0$ and $C_{m}\geq 0$ such that

[TABLE]

holds for any given $(t,x,u)\in\mathbb{R}_{\geq t_{0}}\times\mathbb{R}^{n}\times\mathbb{R}^{m}$, $(\gamma_{1},u_{\gamma,1})\in\mathbb{R}^{n_{c}}\times\mathbb{R}^{n_{c}}$, and $(\gamma_{2},u_{\gamma,2})\in\mathbb{R}^{n_{c}}\times\mathbb{R}^{n_{c}}$. $\square$

In Assumption 6, the functions $m(\cdot)$ and $l(\cdot)$ are required to be Lipschitz only on the variables $(\gamma,u_{\gamma})$, which makes the assumption rather general. Moreover, since the Lipschitz constants are not used in the design phase, only their existence is required, and not their computation.

III-B Continuous-time auxiliary consensus input signal

To design the continuous-time auxiliary consensus input signal, we rely on a class of consensus control laws for discrete-time systems. In particular, we assume the following:

Assumption 7** (Auxiliary consensus control law)**

Consider a set of discrete-time systems that communicate according to the same communication graph $\mathcal{G}~{}=~{}(\mathcal{V},\mathcal{E})$ introduced in Section II, and satisfies for each $i\in{\mathcal{I}}$ the dynamical model

[TABLE]

where $\xi^{[i]}(k)\in\mathbb{R}^{n_{c}}$, $\eta^{[i]}(k)\in\mathbb{R}^{n_{c}}$, and $\xi_{\mathcal{N}^{[i]}}(k)$ denote the $i$-th coordination vector, an external vector, and the coordination vectors from the neighborhood $\mathcal{N}^{[i]}$, respectively, at step $k~{}\in~{}\mathbb{Z}_{\geq k_{0}}$, with $\xi^{[i]}(k_{0})=\xi_{0}^{[i]}\in\mathbb{R}^{n_{c}}$ denoting the initial condition of the system $i$ at the initial time step $k_{0}\in\mathbb{Z}$. We assume that there exists an auxiliary consensus control law $k_{con}(\cdot)$ that satisfies the property that if

[TABLE]

for some constants $\lambda_{\eta}>0$ and $a_{\eta}\geq 0$ then:

As $k\to\infty$, the discrete disagreement function $\phi(k)~{}=~{}\sum_{(i,j)\in\mathcal{E}}(\xi^{[i]}(k)-\xi^{[j]}(k))^{2}$ converges asymptotically to zero; 2. 2.

The consensus control law $k_{con}:\mathbb{R}\times\mathbb{R}^{|\mathcal{N}^{[i]}|}\to\mathcal{U}_{c}$, is bounded, i.e., $\mathcal{U}_{c}\subset\mathbb{R}^{n_{c}}$ is bounded; 3. 3.

There exists an integrable class-$\mathcal{KL}$* function $\beta:\mathbb{R}_{\geq 0}\times\mathbb{R}_{\geq 0}\to\mathbb{R}_{\geq 0}$ such that*

[TABLE]

Assumption 7 requires a distributed consensus law that robustly solves (in the sense of the required properties mentioned above) the consensus problem for multi-agent systems, where each agent is given by a simple discrete integrator. Moreover, it implicitly defines the class of network topologies that can be addressed by the proposed scheme. Therefore, in the design phase, an auxiliary consensus control law should be chosen accordingly to the specific application/network topology to satisfy Assumption 7. See Appendix B for a brief background on the analysis of consensus control laws.

In particular, for the case of time-invariant balanced strongly connected graphs, we show that the consensus control law

[TABLE]

for a suitable choice of the constants $a_{ij}>0$ and $\bar{\epsilon}$, satisfies Assumption 7.

Proposition 1

Consider the network of discrete-time systems in Assumption 7 with $n_{c}=1$ (scalar state) and where $\mathcal{G}$ is a balanced strongly connected graph. Then, the consensus control law (13) with $\bar{\epsilon}\in(0,1/\Delta)$ where $\Delta=\max_{i}(\sum_{j\neq i}a_{ij})$, satisfies Assumption 7. $\square$

Next, the discrete auxiliary consensus control law $k_{con}(\cdot)$ required by Assumption 7 is used to design a continuous-time sample-data consensus control signal for (4b) that only uses state observations taken at the discrete time instants $\mathcal{T}~{}=~{}\{t_{0},t_{1},\dots\}$ where it is assumed that the difference between time samples $\delta_{k}:=t_{k+1}-t_{k}$ is upper bounded by the horizon length $T$ of the MPC controller, and lower bounded by a fixed value $\delta_{lb}>0$.

Proposition 2

Consider the discrete-time auxiliary consensus control law in Assumption 7, the continuous-time system (4b), and the continuous-time auxiliary consensus input signal $\boldsymbol{\bar{u}}^{[i]}_{\gamma,aux_{t}}\in\mathcal{C}(t,+\infty)$ defined as

[TABLE]

with $t\in[t_{k},t_{k+1})$ and $\delta_{k}=t_{k+1}-t_{k}$, $k\in\mathbb{N}_{\geq 0}$. Then, for the closed-loop system (4b) with $u^{[i]}_{\gamma}(t)=\bar{u}^{[i]}_{\gamma,aux_{\lfloor t\rfloor}}(t)$ , the disagreement function (2) converges asymptotically to zero. $\square$

IV Cooperative path following for multiple 3D nonholonomic vehicles

In this section, we apply the proposed optimization-based control framework to solve the CPF problem presented in Section II-A. Numerical simulations are then provided to illustrate the effectiveness of the proposed approach.

IV-A Control Design

In this section, we show how to satisfy all the assumptions presented in Section III for the CPF problem for nonholonomic robotic vehicles of Section II-A. Indeed, the latter can be formulated in the form of Problem 1 by choosing $x^{[i]}(t)=\begin{pmatrix}p^{[i]}(t)&r(R^{[i]}(t))^{\prime}\end{pmatrix}^{\prime}$, with associated dynamical model from (1), where $r(R^{[i]}(t))$ denotes a vectorial parameterization of the matrix $R^{[i]}(t)$, e.g., the components of the rotation matrix, and with output $y^{[i]}(t)$ given in (3). It is worth noticing that despite this example uses homogeneous systems, Theorem 1 applies to systems with possibly different dynamical models. The reference paths $c_{d}^{[i]}$ are designed to be bounded since we would like to guarantee boundedness of the state trajectories of the vehicles. The control design consists of six steps:

IV-A1 Auxiliary consensus control law

We propose the consensus control law (13) with $a_{ij}=1$ for $(i,j)\in\{(1,2),(2,1),(2,3),(3,2)\}$ and zero otherwise, and $\bar{\epsilon}=0.0125\!<\!{1\over\Delta_{d}}$. Thus, Assumption 7 is satisfied by Proposition 1.

IV-A2 Auxiliary regulation control law

We consider a saturated version of the auxiliary control law mentioned in [17]. From (1) and defining $c_{d_{\gamma}}(\gamma)={\partial\over\partial\gamma}c_{d}(\gamma)$, the time derivative of the output (3) is

[TABLE]

where we used the fact that $\Omega(\omega)\epsilon=-\Omega(\epsilon)\omega$. Then, for any choice of $\epsilon$ with $\epsilon_{1}\neq 0$ the input $u=k_{aux}(t,x,\gamma,u_{\gamma})$ with

[TABLE]

results in $\dot{y}=-\Omega y-K{y\over\|y\|}$, for $\|y\|\neq 0$, and $\dot{y}=0$, for $\|y\|=0$, where the matrix $K\succ 0$ is a tuning parameter. Therefore, the Lyapunov-like function $W=\|y\|$ satisfies

[TABLE]

where we used the fact that $\Omega$ is skew-symmetric and therefore $y^{\prime}\Omega y=0$ for all $y\in\mathbb{R}^{3}$. As a consequence, the vector $y$ converges in finite time to the origin as follows

[TABLE]

with $\bar{t}:=t+{\|y(t)\|\over\lambda_{min}(K)}$. This controller is used in the following sections for the computation of the stage cost, terminal cost, and terminal constraint set. The values used in the numerical simulation are $\epsilon^{[i]}\!=\![-0.5,0,0]^{\prime}$ for all $i\!\in\!\mathcal{I}$ and $K\!=\!0.2I_{3\times 3}$.

IV-A3 Regulation stage cost

Assumption 4.2 is satisfied by the regulation stage cost

[TABLE]

for any $Q\succ 0$ and $U\succeq 0$ of suitable dimensions. In the simulations, we use $U=I_{3\times 3}$. To highlight the behavior of the proposed scheme, $Q$ is first selected as $Q=0.1I_{3\times 3}$ and then updated to $Q=100I_{3\times 3}$ as described in Section IV-B.

IV-A4 Regulation terminal cost

Let $(\boldsymbol{y}_{aux},\boldsymbol{u}_{aux})$ be the pair of output and input trajectories of the system in closed-loop with the auxiliary regulation control law (15), starting at the time and output pair $(\hat{t},\hat{y})$. Then, using (16) in (17), it follows that the associated regulation stage cost is upper bounded as $l(\tau,y_{aux},u_{aux})\leq\hat{l}(\tau;\hat{t},\hat{y})$, with

[TABLE]

where $\hat{l}(\cdot)$ satisfies $\hat{l}(\tau;\hat{t}+\delta,y_{aux}(\hat{t}+\delta))\leq\hat{l}(\tau;\hat{t},\hat{y})$ for all $\delta\geq 0$ and $\lim_{\tau\to\infty}\hat{l}(\tau;\hat{t},\hat{y})=0.$ We can now conclude by Lemma 24 of [28] that the regulation terminal cost $m(\hat{t},\hat{y})=\int_{\hat{t}}^{+\infty}\hat{l}(\tau;\hat{t},\hat{y})d\tau=\left.-\tfrac{\lambda_{max}(Q)}{3\lambda_{min}(K)}{(\|\hat{y}\|-\lambda_{min}(K)\tau)^{3}}\right|_{\tau=0}^{\|\hat{y}\|\over\lambda_{min}(K)}=\tfrac{\lambda_{max}(Q)}{3\lambda_{min}(K)}\|\hat{y}\|^{3}$ satisfies the terminal cost decrease of Assumption 4.4.

IV-A5 Regulation terminal set

From (15) we have

[TABLE]

where $\bar{n}:=(\|v_{d}\|+r_{\eta})\sup_{\gamma}\left\|c_{d_{\gamma}}(\gamma)\right\|$. Using now the fact that the derivative of the desired path is required to be bounded, then from (18), by selecting an uniformly bounded $\mathcal{U}(t)$ with

[TABLE]

one conclude that the auxiliary regulation controller is always feasible, and therefore the regulation terminal set can be omitted, i.e., choosing $\mathcal{Y}_{aux}(t)=\mathbb{R}^{3}$ for all $t\geq t_{0}$. The selected regulation terminal set, regulation terminal cost, and constraint sets satisfy Assumption 4, items 1 and 4.

The terminal constraint on $\bar{\eta}(t+T)$ is chosen with $r_{\eta}=1$.

IV-A6 Consensus stage cost

We simple select as consensus stage cost $l_{c}(\eta,v_{\eta})=\eta^{\prime}Q_{c}\eta+v_{\eta}^{\prime}U_{c}v_{\eta}$, with $Q_{c}\succ 0$ and $\lambda_{max}(Q_{c}+\lambda_{\eta}^{2}U_{c})\leq m_{\eta}\lambda_{\eta}$, which clearly satisfies Assumption 4.2. In this example we use $\lambda_{\eta}=Q_{c}=U_{c}=1$ and $m_{\eta}=\lambda_{max}(Q_{c}+\lambda_{\eta}^{2}U_{c})/\lambda_{\eta}=2$.

IV-B Numerical simulations

The MPC controller is designed with horizon length $T\!\!=\!0.4$ $s$. The constraint (7d) is never active. At time $t_{0}$ the internal state of the controller is initialized with $\eta_{0}^{[i]}=0$ and $\gamma_{0}^{[i]}=15$ for all $i\in\mathcal{I}$, i.e., at consensus. Two simulation scenarios are considered. First, selecting a strong weight on the tracking error, i.e., choosing $Q=100I_{3\times 3}$, and then reducing such weight to $Q=0.1I_{3\times 3}$ and therefore favoring the consensus. Fig. 3 displays the associated closed-loop state trajectories and Fig. 3 the time evolution of the path parameter, disagreement function, and output signals.

The simulations show that the proposed scheme, depending on the choice of the parameter $Q$, optimally exploits the coordination parameters in order to increase the convergence rate of the output signals. For the case with $Q=100I_{3\times 3}$ the controller initially drives the coordination parameters out of consensus to favor the reduction of the path error. Then, once the output is decreased, the controller drives the coordination vectors back to consensus. In the case $Q=0.1I_{3\times 3}$, we notice the same behavior, but where the value of the disagreement function is kept almost zero. This latter case resembles a decoupled, and undesired, design where the consensus of the coordination parameters is carried out independently from the output tracking problem.

IV-B1 Decoupled approach

In contrast to the proposed procedure, we now compare the numerical results with a simpler decoupled strategy that can be found in the literature (e.g., [10]). This decoupled approach consists in designing a first controller that drives the vehicle to its path, for any velocity of the coordination vector, and a second controller that drives the coordination vectors to consensus and to a common velocity. Connecting these two controllers we obtain a solution to Problem 1. For instance, we could achieve this scheme using the path following control law (15) and the consensus law in Proposition 1, respectively. Although this strategy is appealing due to its simplicity, it presents some strong limitations. Specifically, since there is no feedback from the position of the vehicle to the consensus law, the vehicle will never be able to adapt their speed to help each others by retarding or accelerating their motion to favorite the decrease of the output signal (tracking error) at the expenses of coordination. Indeed, one can see in Fig. 4 the simulations results with the simplified version, where it is clear by comparing with Fig. 3 (solid line) that the evolution of the coordination vector evolves independently from the tracking error and follows a standard exponential convergence to consensus. This is due to the absence of feedback with the real position of the vehicle. In contrast, in Fig. 3 and Fig. 3 (solid line, i.e., when the tracking error is highly penalized in the performance index) the coordination vectors adjust and move out of consensus to favorite the decreased of the tracking error. Specifically, in Fig. 3 (solid line) the vehicles first move to their paths and then are driven to the formation. This is in contrast to the decoupled approach where the vehicles are forced to follow the point associated with the coordination vectors that move independently from their position, therefore delaying the convergence to zero of the tracking error. Moreover, it is important to stress that the decoupled approach suffers from the fact that the satisfaction of the input constraints can be challenging to verify/satisfy and might lead to inadequate performances (e.g., by implying extremely low gains of the controllers in order to have input signals with small magnitude). Adopting the proposed framework, we are able to optimally balance the output regulation objective with the consensus goal while enforcing hard constraints.

V Conclusions

The paper presents a distributed MPC scheme for coordinated output regulation with application to the CPF problem of multiple robotic vehicles. The proposed solution is non-iterative and distributed, in the sense that the exchange of information is only performed among neighboring vehicles and only take place once every $t\in\mathcal{T}$. In contrast to similar approaches described in the literature, the proposed control framework optimally balances the output regulation goal with the objective of driving the coordination parameters to consensus, achieving the coordination of the vehicles without using of consistency-like constraints. The simulation results show that being able to optimally drive the coordination parameter can lead to a significant increase of convergence rate of the output signals, which is desired in many practical applications. Note however that since the presented scheme is optimization based, the computation burden required to compute the input might become a limitation in case of highly nonlinear systems or long prediction horizon. As a future direction, we wish to validate the proposed approach on the field with practical experiments.

Appendix A Proof of Theorem 1

The proof is structured as follows. First, the value function used for the analysis is introduced. Then, we define and construct the shifted trajectories that are used (i) to prove the recursive feasibility of the open-loop MPC problem and (ii) to analyze the evolution of the value function, where in the latter step we also consider the effect of the interaction with the other vehicles in the network. The proof is concluded with the convergence result obtained using Barbalat’s lemma.

Value function definition. Consider the value function $V(t):=J_{T}^{*}(p(t))$ which is obtained by solving the problem $\mathcal{P}(p(t))$ with $p(t)=(t,x(t),\gamma(t),\gamma_{\mathcal{N}}(t),\eta(t)).$ Notice that, although the implementation of (6) only requires to solve $\mathcal{P}(p(t))$ at the time instants $t\in\mathcal{T}$, the value function considers its optimal value at every time $t\geq t_{0}$, and therefore is well defined for all time instants.

Shifted trajectories design. We recall that, for a generic time $t\geq t_{0}$, we use the superscript ⋆t to denote all the trajectories of a given signal associated with the optimal predictions of $\mathcal{P}(t)$. Next, the optimizers of the problem $\mathcal{P}(t_{k})$ are used to build the shifted inputs $\boldsymbol{\bar{u}}^{s_{t_{k}+\delta}}\in\mathcal{PC}(t_{k}+\delta,t_{k}+\delta+T)$ and $\boldsymbol{\bar{v}}_{\gamma}^{s_{t_{k}+\delta}}\in\mathcal{PC}(t_{k}+\delta,t_{k}+\delta+T)$, with $\delta\in(0,T)$, as follows

[TABLE]

with $\bar{u}_{\gamma,aux_{t_{k}+\delta}}(\cdot)$ from (LABEL:eq:ubaric), and where the superscript ${}^{s_{t_{k}+\delta}}$ is used to refer a shifted trajectory, meaning an open-loop trajectory associated to $J_{T}(p(t_{k}+\delta),\boldsymbol{\bar{u}}^{s_{t_{k}+\delta}},\boldsymbol{\bar{v}}_{\gamma}^{s_{t_{k}+\delta}})$.

Recursive feasibility. Next, we show that the shifted trajectories are feasible for $J_{T}(p(t_{k}+\delta),\boldsymbol{\bar{u}}^{s_{t_{k}+\delta}},\boldsymbol{\bar{v}}_{\gamma}^{s_{t_{k}+\delta}})$.

Analysis of $(\boldsymbol{\bar{u}},\boldsymbol{\bar{x}},\boldsymbol{\bar{v}}_{\gamma},\boldsymbol{\bar{\eta}})$: For all $\tau\in[t_{k}+\delta,t_{k}+T)$ the shifted trajectories $(\boldsymbol{\bar{u}}^{s_{t_{k}+\delta}},\boldsymbol{\bar{x}}^{s_{t_{k}+\delta}},\boldsymbol{\bar{v}}_{\gamma}^{s_{t_{k}+\delta}},\boldsymbol{\bar{\eta}}^{s_{t_{k}+\delta}})$ are feasible since, in that interval, they corresponds to trajectories $(\boldsymbol{\bar{u}}^{\star t_{k}},\boldsymbol{\bar{x}}^{\star t_{k}},\boldsymbol{\bar{v}}_{\gamma}^{\star t_{k}},\boldsymbol{\bar{\eta}}^{\star t_{k}})$, which were feasible at time $t_{k}$. For all $\tau\in[t_{k}+T,t_{k}+T+\delta]$ we have $\bar{v}_{\gamma}^{s_{t_{k}+\delta}}(\tau)=-\lambda_{\eta}\bar{\eta}(\tau)\implies\bar{\eta}(\tau)=\bar{\eta}(t_{k}+T)e^{-\lambda_{\eta}(\tau-t_{k}-T)}\implies\|\bar{\eta}(\tau)\|\leq\|\bar{\eta}(t_{k}+T)\|\leq r_{\eta}\implies\|\bar{v}_{\gamma}^{s_{t_{k}+\delta}}(\tau)\|\leq\lambda_{\eta}r_{\eta}.$ Combining this result with Assumption 5.1, it follows that the input constraint $\bar{v}_{\gamma}(\tau)\in\mathcal{V}_{\gamma}(\tau)$ and the state constraint (7d) are satisfied. Moreover, $(\boldsymbol{\bar{u}}^{s_{t_{k}+\delta}},\boldsymbol{\bar{x}}^{s_{t_{k}+\delta}})$ are kept feasible by the auxiliary regulation controller from Assumption 4.4.

Analysis of $(\boldsymbol{\bar{\gamma}},\boldsymbol{\bar{u}}_{\gamma})$: The variables $(\boldsymbol{\bar{\gamma}}^{s_{t_{k}+\delta}},\boldsymbol{\bar{u}}_{\gamma}^{s_{t_{k}+\delta}})$ are not constrained, and therefore are trivially feasible.

Unknown future behavior of the neighboring systems. It is important to notice that, in contrast to $(\boldsymbol{\bar{u}},\boldsymbol{\bar{x}},\boldsymbol{\bar{v}}_{\gamma},\boldsymbol{\bar{\eta}})$, the variables $(\boldsymbol{\bar{\gamma}}^{s_{t_{k}+\delta}},\boldsymbol{\bar{u}}_{\gamma}^{s_{t_{k}+\delta}})$ do not match $(\boldsymbol{\bar{\gamma}}^{\star t_{k}},\boldsymbol{\bar{u}}_{\gamma}^{\star t_{k}})$ in the common time interval $[t_{k}+\delta,t_{k}+T)$. Specifically, from the definition in (LABEL:eq:ubaric), $\bar{u}_{\gamma,aux_{t_{k}+\delta}}(\tau)$ differs from $\bar{u}_{\gamma,aux_{t_{k}}}(\tau)$ in the interval $\tau\geq t_{k}+\delta_{k}$. For the case $\delta=\delta_{k}$ this difference captures the discrepancy between the predictions obtained using the network measurements at time $t_{k}$ and the one obtained using the measurements at time $t_{k+1}$. The effect of this distinction on the convergence analysis is considered in Lemma 1, which will be called in the next developments.

Value function decrease. Before analyzing the whole value function, notice that along the shifted trajectories with $\tau\in[t_{k}+T,t_{k}+T+\delta]$, i.e., when $\bar{v}_{\gamma}^{s_{t_{k}+\delta}}(\tau)=-\lambda_{\eta}\bar{\eta}^{s_{t_{k}+\delta}}(\tau)$, integrating (10) from $t_{k}+T$ to $t_{k}+T+\delta$ results in

[TABLE]

where to simplify the notation we use $\bar{l}_{c}(\tau)$ to denote $l_{c}(\cdot)$ evaluated at the predicted signals as function of $\tau$. Similarly, evaluating (9) along the shifted trajectories $(\boldsymbol{\bar{u}}^{s_{t_{k}+\delta}},\boldsymbol{\bar{x}}^{s_{t_{k}+\delta}},\boldsymbol{\bar{v}}_{\gamma}^{s_{t_{k}+\delta}},\boldsymbol{\bar{\eta}}^{s_{t_{k}+\delta}})$ we have

[TABLE]

The shifted trajectories are now used in combination with the optimality of the MPC controller to analyze the evolution of the value function. Consider first $\delta\in[0,t_{k+1}-t_{k}]$, for the generic $t_{k}\in\mathcal{T}$, we have

[TABLE]

for an integrable function $\bar{\beta}_{\delta}(\cdot)$, where the first inequality stems from the sub-optimality of the shifted variables $(\boldsymbol{\bar{u}}^{s_{t_{k}+\delta}},\boldsymbol{\bar{v}}_{\gamma}^{s_{t_{k}+\delta}})$, the second equality uses the fact that $\bar{l}^{s_{t_{k}+\delta}}(\tau)-\bar{l}_{c}^{\star t_{k}}(\tau)=0$ for $\tau\in[t_{k}+\delta,t_{k}+T)$, the second inequality is due to (20) and (21), and the last one is obtained from Lemma 1 in the Appendix A-A. Since the latter inequity holds on any interval $[t_{k},t_{k}+\delta]$ with $\delta\!\in\![0,t_{k+1}\!\!-\!t_{k}]$, we can analyze the generic case $\delta\!>\!0$ as follows

[TABLE]

where we used the fact that the summation $\bar{\bar{\beta}}:=\bar{\beta}_{t_{k}+\delta-\lfloor t_{k}+\delta\rfloor}(\lfloor t_{k}+\delta\rfloor)+\sum_{\begin{subarray}{c}t_{j}\in\mathcal{T}\\ t_{k}\leq t_{j}<\lfloor t_{k}+\delta\rfloor\end{subarray}}\bar{\beta}_{\delta}(t_{j})$ converges from Lemma 1, and the bound $\bar{l}(\tau)+\bar{l}_{c}(\tau)\geq\alpha(\|z(\tau)\|)$ from Assumption 4.2 and Assumption 5.2 where $z(t):=[y(t)^{\prime},\eta(t)^{\prime}]^{\prime}$ and $\alpha:\mathbb{R}_{\geq 0}\to\mathbb{R}_{\geq 0}$ is a class-$\mathcal{K}_{\infty}$ function such that $\alpha(\|z\|)\leq\alpha_{s}(\|y\|)+\alpha_{c}(\|\eta\|)$. By Lemma 2 in the Appendix A-A, the derivative of $\boldsymbol{y}$ is bounded for bounded values of $\boldsymbol{y}$, and the same apply to the signal $\boldsymbol{\eta}$, from $\mathcal{V}_{\gamma}(\cdot)$ being uniformly bounded over time by Assumption 5.1, and therefore also the derivative of the signal $\boldsymbol{z}$ is bounded for bounded values of $z$. Then, applying Lemma 37 of [28] on $\boldsymbol{z}$ we have that for any $t\geq t_{0}$ and a given $\epsilon>0$ there exists a class-$\mathcal{K}_{\infty}$ functions $\alpha_{\epsilon}:\mathbb{R}_{\geq 0}\to\mathbb{R}_{\geq 0}$, possibly dependent on $\epsilon$, such that $\int_{t}^{t+\epsilon}\alpha(\|z(\tau)\|)d\tau\geq\alpha_{\epsilon}(\|z(t)\|).$ As a consequence, using the definition (7e) of $J_{T}(\cdot)$, for any $\delta_{1}\in(0,T]$ we have that $V(t)\geq\alpha_{\delta_{1}}(\|z(t)\|)$ that is obtained choosing $\epsilon=\delta_{1}$. Combining the latter with the fact that, from (22), $V(t_{0}+\delta)\leq V(t_{0})+\bar{\bar{\beta}}$ for all $\delta\geq 0$, implies that the trajectory $\boldsymbol{z}$ is bounded within the set

[TABLE]

for all $t\geq t_{0}$, where the set is compact because it is a level set of a class-$\mathcal{K}_{\infty}$ function, and therefore also the state $x(t)$ of the system is uniformly bounded over time by (8) and the boundedness of the input in Assumption 4.1.

At this point, in order to prove convergence of $z(t)$ to the origin as $t\to\infty$, we can use Barbalat’s lemma (see, e.g., Lemma 8.2 in [36]). Toward that, next we show that $\alpha(\|z(\tau)\|)$ is a uniformly continuous function of $\tau$.

First notice that $y(\tau)$ is uniformly continuous on $\tau$ since, by Lemma 2, possesses a uniformly bounded time derivative for bounded values of $y$, where $y$ is uniformly bounded from $z$ being uniformly bounded in (23). Similarly, $\eta(\tau)$ is uniformly continuous on $\tau$, from $\mathcal{V}_{\gamma}(\cdot)$ being uniformly bounded over time. As a consequence, the combined vector $z(\tau)$ is uniformly continuous on $\tau$. Then, we notice that the function $\alpha(\|z(\tau)\|)$ is a uniformly continuous function of $\tau$ because (i) the function $\alpha(\|z(\tau)\|)$ is continuous in $z$ with $z$ bounded within (23) and (ii) $z$ is uniformly continuous in $\tau$.

At this point, the inequality (22) implies $\lim_{\delta\rightarrow\infty}\int_{t_{0}}^{t_{0}+\delta}\alpha(\|z(\tau)\|)d\tau~{}\leq V(t_{0})+\bar{\bar{\beta}}<\infty$ where the limit exists since the function $\int_{t_{0}}^{t_{0}+\delta}\alpha(\|z(\tau)\|)d\tau$ is non decreasing in $\delta$, from $\alpha(\cdot)$ being non negative, and it is upper bounded by $V(t_{0})+\bar{\bar{\beta}}$. Thus, by Barbalat’s lemma, $\alpha(\|z(t)\|)\to 0$ as $t\to\infty$ and, by the continuity and positive-definitiveness of $\alpha(\cdot)$, the vector $z(t)$, and therefore the vectors $y(t)$ and $\eta(t)$, converges to the origin as $t\to\infty$. The proof is concluded by noticing that, by Assumption 7, as $\eta(t)\to 0$ also the disagreement function is driven to zero. $\blacksquare$

A-A Lemmas

Lemma 1

Consider the open-loop MPC problem from Definition 1 and let Assumption 7 hold. Let the superscripts ${}^{s_{t_{k}+\delta}}$ and ${}^{\star t_{k}}$ denote all the trajectories associated with the shifted inputs (19) and the optimal solutions of $\mathcal{P}(t_{k})$, respectively. Then, for any $t_{k}\!\in\!\mathcal{T}$ and $\delta\!\in\![0,t_{k+1}\!-\!t_{k}]$ the following holds $\int_{t_{k}+\delta}^{t_{k}+T}\!\bar{l}^{s_{t_{k}+\delta}}(\tau)\!-\!\bar{l}^{\star t_{k}}(\tau)d\tau\!+\!\bar{m}^{s_{t_{k}+\delta}}(t_{k}+T)\!-\!\bar{m}^{\star t_{k}}(t_{k}\!+\!T)\!\leq\!\bar{\beta}_{\delta}(t_{k})$ for an integrable non-increasing scalar function $\bar{\beta}:\mathbb{R}_{\geq t_{0}}\to\mathbb{R}_{\geq 0}$ with $\int_{t_{0}}^{+\infty}\bar{\beta}_{\delta}(\tau)d\tau\leq b_{\bar{\beta}}<\infty$ and constant $b_{\bar{\beta}}\in\mathbb{R}_{\geq 0}$.

Proof:

From the definitions of $\bar{u}_{\gamma}(\cdot)$ in (7c) and $\bar{u}_{\gamma,aux_{t}}(\cdot)$ in (LABEL:eq:ubaric), for all $\tau\in[t_{k}+\delta,t_{k}+T)$ we have that

[TABLE]

The constraint (7d) enforces (12), and therefore by Assumption 7 we have $\|\bar{u}^{\star t_{k}}_{\gamma}(\tau)-\bar{u}^{s_{t_{k}+\delta}}_{\gamma}(\tau)\|=\|\bar{u}_{\gamma,aux_{t_{k}}}(\tau)-\bar{u}_{\gamma,aux_{t_{k}+\delta}}(\tau)\|\leq 2\left\|{1\over\delta_{k}}k_{con}(\gamma^{[i]}(t_{k}),\gamma_{\mathcal{N}^{[i]}}(t_{k}))\right\|\leq{2\over\delta_{lb}}\beta(\sum_{i\in\mathcal{I}}\|\gamma_{0}^{[i]}\|,k)$ where $k$ is the subscript of $t_{k}$. Combining the latter with (7a) we also obtain that $\|\bar{\gamma}^{\star t_{k}}(\tau)-\bar{\gamma}_{aux_{t_{k}}}(\tau)\|\leq{2T\over\delta_{lb}}\beta(\sum_{i\in\mathcal{I}}\|\gamma_{0}^{[i]}\|,k)$ where $\tau\in[t_{k},t_{k}+T)$ and where the last inequality comes from $\tau$ being less than $t_{k}+T$. At this point, combining the latter inequalities with the Lipschitz properties of Assumption 6 we can write $\int_{t_{k}+\delta}^{t_{k}+T}l(\tau,\bar{x}^{\star t_{k}},\bar{u}^{\star t_{k}},\bar{\gamma}_{aux_{t_{k}}},\bar{u}_{\gamma,aux_{t_{k}}})-\bar{l}^{\star t_{k}}(\tau)d\tau\leq{\footnotesize\int_{t_{k}+\delta}^{t_{k}+T}{C_{l}}\left\|\begin{bmatrix}\bar{\gamma}_{aux_{t_{k}}}-\bar{\gamma}^{\star t_{k}}\\ \bar{u}_{\gamma,aux_{t_{k}}}-\bar{u}^{\star t_{k}}_{\gamma}\end{bmatrix}\right\|}\leq\tfrac{2C_{l}T(1+T)}{\delta_{lb}}\beta(\sum_{i\in\mathcal{I}}\|\gamma_{0}^{[i]}\|,k)$ and, similarly, $m(t_{k}+\delta+T,\bar{x}^{s_{t_{k}}},\bar{\gamma}_{aux_{t_{k}}})-m(t_{k}+T,\bar{x}^{\star t_{k}},\bar{\gamma}^{\star t_{k}}_{i})\leq C_{m}\left\|\begin{bmatrix}\bar{\gamma}_{aux_{t_{k}}}-\bar{\gamma}^{\star t_{k}}\\ \bar{u}_{\gamma,aux_{t_{k}}}-\bar{u}^{\star t_{k}}_{\gamma}\end{bmatrix}\right\|\leq\tfrac{2C_{m}(1+T)}{\delta_{lb}}\beta(\sum_{i\in\mathcal{I}}\|\gamma_{0}^{[i]}\|,k).$ Therefore, the lemma holds with $\bar{\beta}_{\delta}(t)\!=\!{2(C_{l}T+C_{m})(1+T)\over\delta_{lb}}\beta(\sum_{i\in\mathcal{I}}\|\gamma_{0}^{[i]}\|,k)$, $t\in[t_{k},t_{k+1})$, which is an integrable non-increasing function because $\beta(\cdot)$ is assumed to be integrable and non-increasing. ∎

Lemma 2

Consider the system (4), let Assumption 1-3 and 7 hold and let $\mathcal{V}_{\gamma}(\cdot)$ and $\mathcal{U}(\cdot)$ be uniformly bounded over time. Then, the time derivative of $y(t)$ is uniformly bounded over time for any trajectory of $y(t)$ uniformly bounded over time.

Proof:

From the assumptions stated in this lemma, one can conclude that there exist scalars $b_{y}\geq 0$, $b_{u}\geq 0$, and $b_{g}\geq 0$ such that $\|y(t)\|\leq b_{y}$, $\|u(t)\|\leq b_{u}$, and $\|g(t)\|\leq b_{g}$ uniformly over time. Then, from (8), the evolution of the state vector is uniformly bounded over time as $\|x(t)\|\leq B:=\beta_{x}(\|x_{0}\|,0)+\sigma_{y}(b_{y})+\sigma_{u}(b_{u})+\sigma_{x}$ and therefore, by Assumption 1, also its derivative is bounded as $\|f(\cdot)\|\leq b_{f}$ for some scalar $b_{f}\geq 0$. Moreover, we recall that in Assumption 7, the consensus controller provides bounded inputs, i.e.,$\|k_{con}(\cdot)\|\leq b_{c}$ for some constants $b_{c}\geq 0$. Combining the latter facts with Assumption 3.2, it follows that the time derivative of $y(t)$, given by $\dot{y}(t)\!=\!\nabla h(t,x,\gamma)\begin{bmatrix}1&f(t,x,u)&{g}(t)+u_{\gamma}\end{bmatrix}^{\prime}$ is bounded as $\|\dot{y}(t)\|\!\leq\!b_{B}(1\!+\!\|f(t,x,u)\|\!+\!b_{g}\!+\!\|u_{\gamma}\|)\!\leq\!b_{B}(1\!+\!b_{f}\!+\!b_{g}\!+\!b_{c}\!+\!a_{\eta}e^{-\lambda_{\eta}t_{0}})$ where we used the fact that $u_{\gamma}$ is chosen as in (7c) where $\bar{\eta}$ is constrained by (7d). ∎

Appendix B Analysis of the consensus control law

Consider a network of discrete-time systems as stated in Assumption 7. This section shows that the closed-loop system (11) with (13) satisfies Assumption 7. For the sake of simplicity, we consider the scalar case, i.e., $n_{c}=1$, which is the case used in the Section IV for the numerical results.

B-A Background

We start by recalling some results from [13, 12] that are used in the analysis of the consensus control law (13).

The matrix $A$ denotes the adjacency matrix of the graph $\mathcal{G}$, where the generic component at row $i$ and column $j$ is defined as $[A]_{i,j}=a_{ij}$, for positive scalars $a_{ij}>0$, if $(i,j)\in\mathcal{E}$ and $[A]_{i,j}=0$, otherwise. The associated Laplacian is defined as $L=D-A$ where $D=\operatorname{diag}(d_{1},\dots,d_{|\mathcal{V}|})$ denotes the degree matrix with generic diagonal component $d_{i}=\sum_{j\neq i}a_{ij}$. The matrix $P=I_{|\mathcal{V}|}-\epsilon L$ denotes the corresponding Perron matrix with parameter $\epsilon$. A nonnegative matrix is called row (column) stochastic if its row-sums (column-sums) are one. A doubly stochastic matrix is a matrix that is row stochastic and column stochastic. A matrix is called primitive if it has only one eigenvalue with maximum modulus, and it is called irreducible if its associated graph is strongly connected.

Lemma 3** (Lemma 3 of [13])**

Let $\mathcal{G}$ be a digraph with maximum degree $\Delta=\max_{i}(\sum_{j\neq i}a_{ij})$. Then, the Perron matrix P with parameter $\epsilon\in(0,1/\Delta]$ satisfies the following properties: 1) P is a row stochastic nonnegative matrix with a trivial eigenvalue of $1$; 2) All eigenvalues of P are in the unit circle; 3) If $\mathcal{G}$ is a balanced graph, then P is a doubly stochastic matrix; 4) If $\mathcal{G}$ is strongly connected and $\epsilon\in(0,1/\Delta)$, then P is a primitive matrix. $\square$

The disagreement vector of the network is defined as

[TABLE]

with $\xi_{\mathcal{G}}:=(\xi^{[1]},\xi^{[2]},\dots,\xi^{[|\mathcal{V}|]})^{\prime}$ and $n=|\mathcal{V}|$.

Theorem 2** (Theorem 3 of [13])**

Let $\mathcal{G}$ be a balanced digraph (or undirected graph) with Laplacian L with a symmetric part $L_{s}=(L+L^{\prime})/2$ and Perron matrix P. Then, 1) $\lambda_{2}=\min_{\mathbf{1}^{\prime}\delta=0}(\delta^{\prime}L\delta/\delta^{\prime}\delta)$ with $\lambda_{2}=\lambda_{2}(L_{s})$, i.e., $\delta^{\prime}L\delta\geq\lambda_{2}\|\delta\|^{2}$ for all disagreement vectors $\delta$; 2) $\mu_{2}=\max_{\mathbf{1}^{\prime}\delta=0}(\delta^{\prime}P\delta/\delta^{\prime}\delta)$ with $\mu_{2}=1-\epsilon\lambda_{2}$, i.e., $\delta^{\prime}P\delta\leq\mu_{2}\|\delta\|^{2}$ for all $\delta$. $\square$

B-B Robustness analysis

In order to show that (13) satisfies Assumption 7, we start by proving that the dynamical model of the disagreement vector (25) associated to the closed-loop system (11) with (13) is Input-to-State-Stable (ISS) with respect to $\delta_{\eta}(k)$ where

[TABLE]

with $\eta_{\mathcal{G}}:=(\eta^{[1]},\eta^{[2]},\dots,\eta^{[|\mathcal{V}|]})^{\prime}$ and $n=|\mathcal{V}|$. That is, we will show that there exists a class-$\mathcal{KL}$ function $\beta:\mathbb{R}_{\geq 0}\times\mathbb{R}_{\geq 0}\to\mathbb{R}_{\geq 0}$ and a class-$\mathcal{K}$ function $\gamma:\mathbb{R}_{\geq 0}\to\mathbb{R}_{\geq 0}$ such that, for all $k\geq k_{0}$, the inequality

[TABLE]

holds, where $\boldsymbol{\delta}_{\eta}([k_{0},k])$ denotes the trajectory $\boldsymbol{\delta}_{\eta}(\cdot)$ considered over the time steps $k_{0},k_{0}+1,\dots,k$. At this point, we refer to [37, Definition 3.2] for a definition of ISS-Lyapunov function. The following result provides an ISS-Lyapunov function that by [37, Lemma 3.5] implies (27).

Theorem 3** (ISS property of consensus)**

Consider the network of discrete-time systems in Assumption 7 with $n_{c}=1$ (scalar state) and (13), and the disagreement vector of the network defined as (25). If $\mathcal{G}$ is a balanced strongly connected graph and $\epsilon\in(0,1/\Delta)$ where $\Delta=\max_{i}(\sum_{j\neq i}a_{ij})$, then $\Phi(k):=\delta(k)^{\prime}\delta(k)$ is an ISS-Lyapunov function that satisfies

[TABLE]

with $\delta_{\eta}(k)$ from (26), for constants $c>0$ and $0<\lambda_{\Phi}<1$.

Proof:

First notice that since $\mathcal{G}$ is a balanced strongly connected graph with $\epsilon\in(0,1/\Delta)$, by Lemma 3 the Perron matrix $P$ is double stochastic and, therefore, satisfies $\mathbf{1}^{\prime}P=\mathbf{1}^{\prime}$ and $P\mathbf{1}~{}=~{}\mathbf{1}$. Recalling the definitions (25) and (26), the closed-loop (11) with (13) can be rewritten in the compact form $\xi_{\mathcal{G}}(k+1)=P\xi_{\mathcal{G}}(k)+\eta_{\mathcal{G}}(k)$, $\alpha(k+1)={1\over n}\mathbf{1}^{\prime}(P\xi_{\mathcal{G}}(k)+\eta_{\mathcal{G}}(k))=\alpha(k)+\alpha_{\eta}(k)$ where we used the fact that $\mathbf{1}^{\prime}P=\mathbf{1}^{\prime}$. Moreover, combining the latter with the definition (25) of $\delta(k+1)$ and the fact that $P\mathbf{1}~{}=~{}\mathbf{1}$, we have $\delta(k+1)=P(\delta(k)+\alpha(k)\mathbf{1})+\eta_{\mathcal{G}}(k)-\alpha(k)\mathbf{1}-\alpha_{\eta}(k)\mathbf{1}=P\delta(k)+\delta_{\eta}(k)$ and therefore$\Phi(k+1)=\|P\delta(k)\|^{2}+\|\delta_{\eta}(k)\|^{2}+2\delta_{\eta}(k)^{\prime}P\delta(k)\leq\mu_{2}^{2}\Phi(k)+\|\delta_{\eta}(k)\|^{2}+2\|\delta_{\eta}(k)\|\|\delta(k)\|$ where the inequality comes from Theorem 2 and the fact that $\|P\|\leq\mu_{2}\leq 1$. Lastly, we can write $\Phi(k+1)-\Phi(k)\leq-0.5(1-\mu_{2}^{2})\Phi(k)\quad\quad-0.5(1-\mu_{2}^{2})\Phi(k)+\|\delta_{\eta}(k)\|^{2}+2\|\delta_{\eta}(k)\|\|\delta(k)\|=-0.5(1-\mu_{2}^{2})\Phi(k)-(a\|\delta(k)\|+b\|\delta_{\eta}(k)\|)^{2}+c\|\delta_{\eta}(k)\|^{2}\leq-\lambda_{\Phi}\Phi(k)+c\|\delta_{\eta}(k)\|^{2}$ with $\lambda_{\Phi}=0.5(1-\mu_{2}^{2})<1$, $a=\sqrt{(1-\mu_{2}^{2})/2}$, $b=-\mu_{2}/a$, $c=b^{2}+1$, that concludes the proof. ∎

B-C Proof of Proposition 2

For the case ${g}(t)=0$, the closed-loop (4b) with $u^{[i]}_{\gamma}(t)=\bar{u}^{[i]}_{\gamma,aux_{\lfloor t\rfloor}}(t)$ results in $\dot{\gamma}^{[i]}(t)={1\over\delta_{k}}k_{con}(\gamma^{[i]}(t_{k}),\gamma_{\mathcal{N}^{[i]}}(t_{k}))$ for $t\in[t_{k},t_{k+1})$, with $k\in\mathbb{Z}_{\geq 0}$. Integrating it, $\gamma^{[i]}(t_{k+1})=\gamma^{[i]}(t_{k})+k_{con}(\gamma^{[i]}(t_{k}),\gamma_{\mathcal{N}^{[i]}}(t_{k})).$ This last fact shows that the continuous time evolution of $\gamma(t)$ evaluated at $t=t_{k}\in\mathcal{T}$ exactly recovers the discrete time evolution of $\xi(k)$ in (11), with $\eta(k)=0$, and therefore the disagreement function evaluated at $t_{k}\in\mathcal{T}$ is asymptotically driven to the origin. Moreover, within the generic interval $t\in[t_{k},t_{k+1}]$ the state is a linear interpolation of $\gamma^{[i]}(t_{k})$ and $\gamma^{[i]}(t_{k+1})$, i.e., $\gamma^{[i]}(t)\!\!=\!\!\gamma^{[i]}(t_{k})\!+\!(t\!\!-\!\!t_{k})(\gamma^{[i]}(t_{k+1})\!-\!\gamma^{[i]}(t_{k}))/(t_{k+1}\!\!-\!\!t_{k})$ that implies the consensus to be asymptotically reached for the whole continuous-time trajectory. Lastly, since ${g}(t)$ is common to all the vehicles, choosing ${g}(t)\!\neq\!0$ would not affect the value of the disagreement function and, therefore, would still guarantee consensus. $\blacksquare$

B-D Proof of Proposition 1

The proposition is proved by combining the bound (12) with the ISS property of the consensus law (13) from Theorem 3. We start by noticing that from (12), and recalling the definition of $\delta_{\eta}(k)$ in (26), for all $k\geq k_{0}$ we have $\|\delta_{\eta}(k)\|^{2}=\|\eta_{\mathcal{G}}(k)\|^{2}-\|\eta_{\mathcal{G}}(k)^{\prime}\mathbf{1}\|^{2}/|\mathcal{V}|\leq\|\eta_{\mathcal{G}}(k)\|^{2}\leq\sum_{i\in\mathcal{E}}(a^{[i]}_{\eta})^{2}e^{-2\lambda^{[i]}_{\eta}(k-k_{0})}\leq|\mathcal{V}|(a^{\max}_{\eta})^{2}e^{-2\lambda^{\min}_{\eta}(k-k_{0})}$ with $a^{\max}_{\eta}=\max_{i\in\{1,\dots|\mathcal{V}|\}}a^{[i]}_{\eta},\quad\lambda^{\min}_{\eta}=\min_{i\in\{1,\dots|\mathcal{V}|\}}\lambda^{[i]}_{\eta}.$ Applying recursively (28) and combining it with the latter inequality results that for a generic $n\geq 0$ we have $\Phi(k_{0}+n)\leq(1-\lambda_{\Phi})^{n}\Phi(k_{0})+\sum_{i=0}^{n-1}(1-\lambda_{\Phi})^{i}c|\mathcal{V}|(a^{\max}_{\eta})^{2}e^{-2\lambda^{\min}_{\eta}(n-1-i)}\leq(1-\lambda_{\Phi})^{n}\Phi(k_{0})+d(e^{-2\lambda^{\min}_{\eta}n}-(1-\lambda_{\Phi})^{n})$ with $d={c|\mathcal{V}|(a^{\max}_{\eta})^{2}e^{2\lambda^{\min}_{\eta}}\over 1-(1-\lambda_{\Phi})e^{2\lambda^{\min}_{\eta}}}$. Since, $\lambda_{\Phi}<1$ and $\lambda_{\eta}^{\min}>0$, as $n\to\infty$ the disagreement vector $\|\delta(k_{0}+n)\|\to 0$ and therefore Assumption 7.1 is satisfied.

At this point, notice that the consensus control law can be bounded as follows $\|k_{con}(\xi^{[i]}(k_{0}+n),\xi_{\mathcal{N}^{[i]}}(k_{0}+n))\|\leq\|L\xi_{\mathcal{G}}\|=\|L\delta\|\leq\|L\|\|\delta\|=\|L\|\sqrt{\Phi(k_{0}+n)}\leq\|L\|((1-\lambda_{\Phi})^{n/2}\sqrt{\Phi(k_{0})}+\sqrt{d}(e^{-\lambda^{\min}_{\eta}n}+(1-\lambda_{\Phi})^{n\over 2}))\leq\|L\|((1-\lambda_{\Phi})^{n/2}\sum_{i\in\mathcal{I}}\|\xi_{0}^{[i]}\|+\sqrt{d}(e^{-\lambda^{\min}_{\eta}n}+(1-\lambda_{\Phi})^{n\over 2}))=\beta(\sum_{i\in\mathcal{I}}\|\xi_{0}^{[i]}\|,n)$ where the first equality comes from $L\mathbf{1}=0$, the third inequality is obtained by applying twice the fact that $\sqrt{a+b}\leq\sqrt{a}+\sqrt{b}$ for any positive scalars $a,b>0$, and the fourth inequality stems from $\|\delta(k_{0})\|\leq\|\xi(k_{0})\|\leq\sum_{i\in\mathcal{I}}\|\xi_{0}^{[i]}\|$. The function $\beta:\mathbb{R}_{\geq 0}\times\mathbb{R}_{\geq 0}\to\mathbb{R}_{\geq 0}$ is defined as $\beta(r,s):=\|L\|(1-\lambda_{\Phi})^{(\lfloor s\rfloor+k_{0})/2}r+\|L\|\sqrt{d}(e^{-\lambda^{\min}_{\eta}(\lfloor s\rfloor+k_{0})}+(1-\lambda_{\Phi})^{\lfloor s\rfloor+k_{0}\over 2})$, $\forall s\in[\lfloor s\rfloor,\lfloor s\rfloor+1)$ where $\lfloor s\rfloor$ denotes the smaller integer less than or equal to $s$, i.e., $\lfloor s\rfloor=\max_{k\in\mathbb{Z},\;k\leq s}k$. Since $\beta(r,s)$ is non increasing with $s$, the consensus control law is bounded as $\|k_{con}(\xi^{[i]}(k_{0}+n),\xi_{\mathcal{N}^{[i]}}(k_{0}+n))\|\leq\beta(\sum_{i\in\mathcal{I}}\|\xi_{0}^{[i]}\|,n)$ uniformly over time, and therefore Assumption 7.2 is satisfied. Moreover, by definition of $\beta(\cdot)$ we have $\int_{0}^{\infty}\beta(r,s)ds=\sum_{k=0}^{\infty}\beta(r,k)=\sum_{k=k_{0}}^{\infty}\|L\|\left[(1-\lambda_{\Phi})^{k/2}r+\sqrt{d}(e^{-\lambda^{\min}_{\eta}k}+(1-\lambda_{\Phi})^{k\over 2})\right]<\infty$ where the last inequality holds from the fact that the summation converges since $|1-\lambda_{\Phi}|<1$ and $\lambda_{\eta}^{\min}>0$. Then, Assumption 7.3 holds and this concludes the proof. $\blacksquare$