Robust Distance-Based Formation Control of Multiple Rigid Bodies with Orientation Alignment

The set of positive integers is denoted as $\mathbb{N}$. The real $n$-coordinate space, with $n\in\mathbb{N}$, is denoted as $\mathbb{R}^{n}$; $\mathbb{R}^{n}_{\geq 0}$ and $\mathbb{R}^{n}_{>0}$ are the sets of real $n$-vectors with all elements nonnegative and positive, respectively. Given a set $S$, we denote as $\lvert S\lvert$ its cardinality. The notation $\|x\|$ is used for the Euclidean norm of a vector $x\in\mathbb{R}^{n}$. Given a symmetric matrix $A,\lambda_{\text{min}}(A)=\min\{|\lambda|:\lambda\in\sigma(A)\}$ denotes the minimum eigenvalue of $A$, respectively, where $\sigma(A)$ is the set of all the eigenvalues of $A$ and $rank(A)$ is its rank; $A\otimes B$ denotes the Kronecker product of matrices $A,B\in\mathbb{R}^{m\times n}$, as was introduced in (Horn and Johnson, 2012). Define by $\mathbbm{1}_{n}\in\mathbb{R}^{n},I_{n}\in\mathbb{R}^{n\times n},0_{m\times n}\in\mathbb{R}^{m\times n}$ the column vector with all entries $1$, the unit matrix and the $m\times n$ matrix with all entries zeros, respectively; $\mathcal{B}(c,r)=\{x\in\mathbb{R}^{3}:\|x-c\|\leq r\}$ is the $3$D sphere of radius $r\geq 0$ and center $c\in\mathbb{R}^{3}$. The vector connecting the origins of coordinate frames $\{A\}$ and $\{B$} expressed in frame $\{C\}$ coordinates in $3$D space is denoted as $p^{\scriptscriptstyle C}_{{\scriptscriptstyle B/A}}\in{\mathbb{R}}^{3}$. Given $a\in\mathbb{R}^{3}$, $S(a)$ is the skew-symmetric matrix defined according to $S(a)b=a\times b$. We further denote as $q_{\scriptscriptstyle B/A}\in\mathbb{T}^{3}$ the Euler angles representing the orientation of frame $\{B\}$ with respect to frame $\{A\}$, where $\mathbb{T}^{3}$ is the $3$D torus. The angular velocity of frame $\{B\}$ with respect to $\{A\}$, expressed in frame $\{C\}$ coordinates, is denoted as $\omega^{\scriptscriptstyle C}_{\scriptscriptstyle B/A}\in\mathbb{R}^{3}$. We also use the notation $\mathbb{M}=\mathbb{R}^{3}\times\mathbb{T}^{3}$. For notational brevity, when a coordinate frame corresponds to an inertial frame of reference $\{0\}$, we will omit its explicit notation (e.g., $p_{\scriptscriptstyle B}=p^{\scriptscriptstyle 0}_{\scriptscriptstyle B/0},\omega_{\scriptscriptstyle B}=\omega^{\scriptscriptstyle 0}_{\scriptscriptstyle B/0}$ etc.). All vector and matrix differentiations are derived with respect to an inertial frame $\{0\}$, unless otherwise stated.

Prescribed Performance control, originally proposed in (Bechlioulis and Rovithakis, 2008), describes the behavior where a tracking error $e(t):\mathbb{R}_{\geq 0}\to\mathbb{R}$ evolves strictly within a predefined region that is bounded by certain functions of time, achieving prescribed transient and steady state performance. The mathematical expression of prescribed performance is given by the following inequalities:

where $\rho_{L}(t),\rho_{U}(t)$ are smooth and bounded decaying functions of time, satisfying $\lim\limits_{t\to\infty}\rho_{L}(t)>0$ and $\lim\limits_{t\to\infty}\rho_{U}(t)>0$, called performance functions (see Fig. 1). Specifically, for the exponential performance functions $\rho_{i}(t)=(\rho_{i0}-\rho_{i\infty})e^{-l_{i}t}+\rho_{i\infty}$, with $\rho_{i0},\rho_{i\infty},l_{i}\in\mathbb{R}_{>0},i\in\{U,L\}$, appropriately chosen constants, $\rho_{L0}=\rho_{L}(0),\rho_{U0}=\rho_{U}(0)$ are selected such that $\rho_{U0}>e(0)>\rho_{L0}$ and the constants $\rho_{L\infty}=\lim\limits_{t\to\infty}\rho_{L}(t)<\rho_{L0},\rho_{U\infty}=\lim\limits_{t\to\infty}\rho_{U}(t)<\rho_{U0}$ represent the maximum allowable size of the tracking error $e(t)$ at steady state, which may be set arbitrarily small to a value reflecting the resolution of the measurement device, thus achieving practical convergence of $e(t)$ to zero. Moreover, the decreasing rate of $\rho_{L}(t),\rho_{U}(t)$, which is affected by the constants $l_{L},l_{U}$ in this case, introduces a lower bound on the required speed of convergence of $e(t)$. Therefore, the appropriate selection of the performance functions $\rho_{L}(t),\rho_{U}(t)$ imposes performance characteristics on the tracking error $e(t)$.

Consider the initial value problem:

with $H:\mathbb{R}_{\geq 0}\times\Omega_{\psi}\to\mathbb{R}^{n}$, where $\Omega_{\psi}\subseteq\mathbb{R}^{n}$ is a non-empty open set.

An undirected graph $\mathcal{G}$ is a pair $(\mathcal{V},\mathcal{E})$, where $\mathcal{V}$ is a finite set of nodes, representing a team of agents, and $\mathcal{E}\subseteq\{\{i,j\}:i,j\in\mathcal{V},i\neq j\}$, with $M=|\mathcal{E}|$, is the set of edges that model the communication capability between neighboring agents. For each agent, its neighbors’ set $\mathcal{N}_{i}$ is defined as $\mathcal{N}_{i}=\{j_{1},\ldots,j_{N_{i}}\}=\{j\in\mathcal{V}\text{ s.t. }\{i,j\}\in\mathcal{E}\}$, where $N_{i}=|\mathcal{N}_{i}|$.

If there is an edge $\{i,j\}\in\mathcal{E}$, then $i,j$ are called adjacent. A path of length $r$ from vertex $i$ to vertex $j$ is a sequence of $r+1$ distinct vertices, starting with $i$ and ending with $j$, such that consecutive vertices are adjacent. For $i=j$, the path is called a cycle. If there is a path between any two vertices of the graph $\mathcal{G}$, then $\mathcal{G}$ is called connected. A connected graph is called a tree if it contains no cycles.

The adjacency matrix $A(\mathcal{G})=[a_{ij}]\in\mathbb{R}^{N\times N}$ of graph $\mathcal{G}$ is defined by $a_{ij}=a_{ji}=1$, if $\{i,j\}\in\mathcal{E}$, and $a_{ij}=0$ otherwise. The degree $d(i)$ of vertex $i$ is defined as the number of its neighboring vertices, i.e. $d(i)=N_{i},i\in\mathcal{V}$. Let also $\Delta(\mathcal{G})=\text{diag}\{[d(i)]_{i\in\mathcal{V}}\}\in\mathbb{R}^{N\times N}$ be the degree matrix of the system. Consider an arbitrary orientation of $\mathcal{G}$, which assigns to each edge $\{i,j\}\in\mathcal{E}$ precisely one of the ordered pairs $(i,j)$ or $(j,i)$. When selecting the pair $(i,j)$, we say that $i$ is the tail and $j$ is the head of the edge $\{i,j\}$. By considering a numbering $k\in\mathcal{M}=\{1,...,M\}$ of the graph’s edge set, we define the $N\times M$ incidence matrix $D(G)$ as it was given in (Mesbahi and Egerstedt, 2010). The Laplacian matrix $L(\mathcal{G})\in\mathbb{R}^{N\times N}$ of the graph $\mathcal{G}$ is defined as $L(\mathcal{G})=\Delta(\mathcal{G})-A(\mathcal{G})=D(\mathcal{G})D(\mathcal{G})^{\tau}$.

Consider a set of $N$ rigid bodies, with $\mathcal{V}=\{1,2,\ldots,N\}$, $N\geq 2$, operating in a workspace $W\subseteq\mathbb{R}^{3}$, with coordinate frames $\{i\},i\in\mathcal{V}$, attached to their centers of mass. We consider that each agent occupies a sphere $\mathcal{B}_{r_{i}}(p_{i}(t))$, where $p_{i}:\mathbb{R}_{\geq 0}\to\mathbb{R}^{3}$ is the position of the agent’s center of mass and $r_{i}$ is the agent’s radius (see Fig. 2). We also denote as $q_{i}:\mathbb{R}_{\geq 0}\to\mathbb{T}^{3},i\in\mathcal{V}$, the Euler angles representing the agents’ orientation with respect to an inertial frame $\{0\}$, with $q_{i}=[\phi_{i},\theta_{i},\psi_{i}]^{\tau}$. By defining $x_{i}:\mathbb{R}_{\geq 0}\to\mathbb{M},v_{i}:\mathbb{R}_{\geq 0}\to\mathbb{R}^{6},$ with $x_{i}=[p^{\tau}_{i},q^{\tau}_{i}]^{\tau},v_{i}=[\dot{p}^{\tau}_{i},\omega^{\tau}_{i}]^{\tau}$, we model each agent’s motion with the $2$nd order dynamics:

where $J_{i}:\mathbb{M}\to\mathbb{R}^{6\times 6}$ is a Jacobian matrix that maps the Euler angle rates to $v_{i}$, given by

for which we make the following assumption:

The aforementioned assumption guarantees that $J_{i}$ is always well-defined and invertible, since $\det(J_{i})=\tfrac{1}{\cos\theta_{i}}$. Furthermore, $M_{i}:\mathbb{M}\to\mathbb{R}^{6\times 6}$ is the positive definite inertia matrix, $C_{i}:\mathbb{M}\times\mathbb{R}^{6}\to\mathbb{R}^{6\times 6}$ is the Coriolis matrix, $g_{i}:\mathbb{M}\to\mathbb{R}^{6}$ is the gravity vector, and $w_{i}:\mathbb{M}\times\mathbb{R}^{6}\times\mathbb{R}_{\geq 0}\to\mathbb{R}^{6}$ is a bounded vector representing model uncertainties and external disturbances. We consider that the aforementioned vector fields are unknown and continuous. Finally, $u_{i}\in\mathbb{R}^{6}$ is the control input vector representing the $6$D generalized force acting on the agent.

The dynamics (2) can be written in vector form as:

where $x=[x_{1}^{\tau},\dots,x_{N}^{\tau}]^{\tau}:\mathbb{R}_{\geq 0}\to\mathbb{M}^{N},v=[v_{1}^{\tau},\dots,v_{N}^{\tau}]^{\tau}:\mathbb{R}_{\geq 0}\to\mathbb{R}^{6N},u=[u_{1}^{\tau},\dots,u_{N}^{\tau}]^{\tau}\in\mathbb{R}^{6N}$, and

It is also further assumed that each agent can measure its own $p_{i},q_{i},\dot{p}_{i},v_{i},i\in\mathcal{V}$, and has a limited sensing range of $s_{i}>\max\{r_{i}+r_{j}:i,j\in\mathcal{V}\}$. Therefore, by defining the neighboring set $\mathcal{N}_{i}(t)=\{j\in\mathcal{V}:p_{j}(t)\in\mathcal{B}_{s_{i}}(p_{i}(t))\}$, agent $i$ also knows at each time instant $t$ all $p^{i}_{j/i}(t),q_{j/i}(t)$ and, since it knows its own $p_{i}(t),q_{i}(t)$, it can compute all $p_{j}(t),q_{j}(t),\forall j\in\mathcal{N}_{i}(t),t\in\mathbb{R}_{\geq 0}$.

The topology of the multi-agent network is modeled through the graph $\mathcal{G}=(\mathcal{V},\mathcal{E})$, with $\mathcal{V}=\{1,\dots,N\}$ and $\mathcal{E}=\{\{i,j\}\in\mathcal{V}\times\mathcal{V}\text{ s.t. }j\in\mathcal{N}_{i}(0)\text{ and }i\in\mathcal{N}_{j}(0)\}$. The latter implies that at $t=0$ the graph is undirected, i.e.,

with $d_{k,\text{con}}=\min\{s_{\ell_{k}},s_{m_{k}}\},\ell_{k},m_{k}\in\mathcal{V},\forall k\in\mathcal{M}$. We also consider that $\mathcal{G}$ is static in the sense that no edges are added to the graph. We do not exclude, however, edge removal through connectivity loss between initially neighboring agents, which we guarantee to avoid, as presented in the sequel. It is also assumed that at $t=0$ the neighboring agents are at a collision-free configuration, i.e., $d_{k,\text{col}}<\lVert p_{\ell_{k}}(0)-p_{m_{k}}(0)\rVert,\forall\{\ell_{k},m_{k}\}\in\mathcal{E}$, with $d_{k,\text{col}}=r_{\ell_{k}}+r_{m_{k}}$. Hence, we conclude that

Moreover, given the desired formation constants $d_{k,\text{des}}$, $q_{k,\text{des}}$ for the edge $k\in\mathcal{M}$, the formation configuration is called feasible if the set ${\Phi}=\{x\in\mathbb{M}^{N}:\lVert p_{\ell_{k}}-p_{m_{k}}\rVert=d_{k,\text{des}},q_{\ell_{k}}-q_{m_{k}}=q_{k,\text{des}},\forall\{\ell_{k},m_{k}\}\in\mathcal{E}\}$, with $\ell_{k},m_{k}\in\mathcal{V},\forall k\in\mathcal{M}$, is nonempty.

Due to the fact that the agents are not dimensionless and their communication capabilities are limited, the control protocol, except from achieving a desired inter-agent formation, should also guarantee for all $t\in\mathbb{R}_{\geq 0}$ that (i) the neighboring agents avoid collision with each other and (iii) all the initial edges are maintained, i.e., connectivity maintenance. Therefore, all pairs $\{\ell_{k},m_{k}\}\in\mathcal{V}\times\mathcal{V}$ of agents that initially form an edge must remain within distance greater than $d_{k,\text{col}}$ and less than $d_{k,\text{con}}$. We also make the following assumptions that are required on the graph topology:

Formally, the robust formation control problem under the aforementioned constraints is formulated as follows:

Let $p=[p_{1}^{\tau},\dots,p_{N}^{\tau}]^{\tau}:\mathbb{R}_{\geq 0}\to\mathbb{R}^{3N},q=[q_{1}^{\tau},\dots,q_{N}^{\tau}]^{\tau}:\mathbb{R}_{\geq 0}\to\mathbb{T}^{3N}$ be the stacked vectors of all the agent positions and Euler angles. We denote by $\tilde{p},\tilde{q}:\mathbb{R}_{\geq 0}\to\mathbb{R}^{3M}$ the stack column vector of $p_{\ell_{k},m_{k}}(t)=p_{\ell_{k}}(t)-p_{m_{k}}(t)$ and $q_{\ell_{k},m_{k}}(t)=q_{\ell_{k}}(t)-q_{m_{k}}(t)$, respectively, $\forall\{\ell_{k},m_{k}\}\in\mathcal{E}$, with the edges ordered as in the case of the incidence matrix $D(\mathcal{G})$. Thus, the following holds:

Next, let us introduce the errors $e^{p}_{k}:\mathbb{R}_{\geq 0}\to\mathbb{R},e^{q}_{k}=[e^{q}_{k_{1}},e^{q}_{k_{2}},e^{q}_{k_{3}}]^{\tau}:\mathbb{R}_{\geq 0}\to\mathbb{T}^{3}$:

for all distinct edges $\{\ell_{k},m_{k}\}\in\mathcal{E},k\in\mathcal{M}$, in the numbered order they appear in the edge set $\mathcal{E}$.

By taking the time derivative of the aforementioned errors, the following is obtained:

Also, by defining the vectors $e^{p}(t)=[e^{p}_{1}(t),\dots,e^{p}_{M}(t)]^{\tau}\in\mathbb{R}^{M},e^{q}(t)=[(e^{q}_{1}(t))^{\tau},\dots,(e^{q}_{M}(t))^{\tau}]^{\tau}\in\mathbb{T}^{3M}$ and employing (6), (7a) and (7b) can be written in vector form as:

where $\mathbb{F}_{p}:\mathbb{M}^{N}\to\mathbb{R}^{M\times 3M}$, with

By introducing the stack error vector $e(t)=[(e^{p}(t))^{\tau},\\ (e^{q}(t))^{\tau}]^{\tau}\in\mathbb{R}^{4M}$, (8) can be written as:

where

Finally, we obtain from (3a):

and thus, (9) can be written as:

The concepts and techniques of prescribed performance control (see Section 2.2) are adapted in this work in order to: a) achieve predefined transient and steady state response for the distance and orientation errors $e^{p}_{k},e^{q}_{k},\forall k\in\mathcal{M}$ as well as ii) avoid the violation of the collision and connectivity constraints between neighboring agents, as presented in Section 3. The mathematical expressions of prescribed performance are given by the inequality objectives:

$\forall k\in\mathcal{M},n\in\{1,2,3\}$, where

are designer-specified, smooth, bounded, and decreasing functions of time, where $l^{p}_{k},l^{q}_{k},\rho^{p}_{k,\infty},\rho^{q}_{k,\infty}\in\mathbb{R}_{>0},\forall k\in\mathcal{M}$, incorporate the desired transient and steady state performance specifications respectively, as presented in Section 2.2, and $C_{k,\text{col}}$, $C_{k,\text{con}}\in\mathbb{R}_{>0},\forall k\in\mathcal{M}$, are associated with the collision and connectivity constraints. In particular, we select

$\forall k\in\mathcal{M}$, which, since the desired formation is compatible with the collision and connectivity constraints (i.e., $d_{k,\text{col}}<d_{k,\text{des}}<d_{k,\text{con}},\forall k\in\mathcal{M}$), ensures that $C_{k,\text{col}},C_{k,\text{con}}\in\mathbb{R}_{>0},\forall k\in\mathcal{M}$ and consequently, in view of (5), that:

$\forall k\in\mathcal{M},n\in\{1,2,3\}$. Hence, if we guarantee prescribed performance via (20), by employing the decreasing property of $\rho^{p}_{k}(t),\rho^{q}_{k}(t),\forall k\in\mathcal{M}$, we obtain:

and, consequently, owing to (21):

$\forall k\in\mathcal{M},t\in\mathbb{R}_{\geq 0}$, providing, therefore, a solution to problem 3.1.

In the sequel, we propose a decentralized control protocol that does not incorporate any information on the agents’ dynamic model and guarantees (20) for all $t\in\mathbb{R}_{\geq 0}$.

Given the errors $e^{p}(t),e^{q}(t)$ defined in Section 4.1:

Step I-a: Select the corresponding functions $\rho^{p}_{k}(t),\rho^{q}_{k}(t)$ and positive parameters $C_{k,\text{con}},C_{k,\text{col}},k\in\mathcal{M}$, following (20), (22b), and (21), respectively, in order to incorporate the desired transient and steady state performance specifications as well as the collision and connectivity constraints, and define the normalized errors $\xi_{k}^{p}:\mathbb{R}_{\geq 0}\to\mathbb{R},\xi^{q}_{k}=[\xi_{k_{1}}^{q},\xi^{q}_{k_{2}},\xi_{k_{3}}^{q}]^{\tau}:\mathbb{R}_{\geq 0}\to\mathbb{R}^{3}$:

$\forall k\in\mathcal{M}$, as well as the stack vector forms