Tracking Control of Directed Acyclic Formation via Target Point Localization

Abstract

This paper studies a leader-follower formation tracking problem where the leaders are moving with an unknown bounded velocity in the $d$-dimensional space. Based on leaders’ positions, each agent determines and tracks the desired moving position, which is called the target point. The characteristics of the proposed control laws are then examined in details. Simulations are also provided to support the theoretical analysis.

1 Introduction

Recently, formation control has attracted a lot of research attention due to its applications in complex missions such as satellite formation or search and rescue. Most existing studies on distance-based formation control focused on designing distributed control laws for agents to acquire the desired formation shape. A more challenging problem is the formation tracking, where the agents must keep a formation shape and follow some moving leaders simultaneously. A control law was presented in [1] to solve a formation tracking problem, in which the network topology is undirected, and the leaders’ velocity is a constant vector. In [2], based on a velocity estimator, the acyclic leader-follower formation tracking problem was studied, in which each follower agent can determine the leaders’ constant velocity, and can follow the leaders’ motions.

This paper studies the directed acyclic distance-based formation tracking problem in ${\mathbb{R}}^{d} \left( {d = 2,3} \right)$. It is assumed that there are $d$ leaders moving with the same reference velocity that is an unknown bounded continuous function and the remaining agents are referred to as followers. Each follower can measure the local displacements of $d$ neighboring agents in its local coordinate system and know an upper bound of the leaders’ velocity. After determining a target point, a distributed fixed-time control law is proposed for the first follower to move to the target point and track the leaders’ velocity. Each control law consists of two components: the first component eliminates the effect of the leaders’ velocity and the second one steers the agent to the localized target point in a fixed time. Note that a signum component in the control law can also be used to eliminate disturbances as shown in [3] and a target point formation control problem in 2D was introduced in [4].

The remainder of this paper is organized as follows. Section 2 formulates the problem studied in this paper. Formation control laws and stability analysis are provided in Sect. 3. Section 4 presents simulation results and Sect. 5 concludes the paper.

2 Problem Formulation

Consider an $n$-agent system in ${\mathbb{R}}^{d}$ characterized by a directed graph $G = \left( {V,E} \right)$, where $V = \left\{ {1, \ldots ,n} \right\}$ is the node set and $E \subset V \times V$ is the edge set. Let $N_{i} = \{ j \in V|\left( {i,j} \right) \in E\}$ be the neighbor set of an agent $i$. If $j \in N_{i}$, agent $i$ senses the relative position of agent $j$ with respect to agent $i$‘s local reference frame ${}^{i}\varSigma$ and controls the distance $d_{ji}$. between them. The positions of agents are written in a global frame ${}^{g}\varSigma$ by $\varvec{p}_{i} , \forall i \in V$. In the system, there are $d$ leaders moving at the same velocity, and $n - d$ followers. The sets of leaders and followers are denoted by $L = \left\{ {1, \ldots ,d} \right\}$ and $F = \left\{ {d + 1, \ldots ,n} \right\}$. Assume that the velocity $\varvec{f}\left( t \right) \in {\mathbb{R}}^{d}$ of the leaders is a continuously bounded function satisfying $\varvec{f}\left( t \right) \le \kappa$, where $\kappa > 0$ is a known constant. The motion of the leaders can be expressed in the global reference frame as $\dot{\varvec{p}}_{i} = \varvec{f}\left( t \right), i \in L.$ The motion of each follower $i$ is written in the local frame ${}^{i}\varSigma$ as $\dot{\varvec{p}}_{i}^{i} = \varvec{u}_{i}^{i} , i \in F,$ where $\varvec{p}_{i}^{i}$, $\varvec{u}_{i}^{i} \in {\mathbb{R}}^{d}$ represent agent $i$’s position and control input written in ${}^{i}\varSigma$, respectively. The local and global displacements between $i$ and $j$ are defined as $\varvec{p}_{ji}^{i} \, : { = }\,\varvec{p}_{j}^{i} - \varvec{p}_{i}^{i}$ and $\varvec{p}_{ji} \, : { = }\,\varvec{p}_{j} - \varvec{p}_{i}$. The distance $d_{ji}$ between $i$ and $j$ is computed as $d_{ji} = \left\| {\varvec{p}_{ji} } \right\|$.

In this paper, we consider formation with directed acyclic leader-follower graph such that for each $i \in F$, $N_{i} = \left\{ {i - d, \ldots ,i - 1} \right\}$ (see Fig. 1). We use $\left( {\overline{G} ,\varvec{p}} \right)$ to describe the formation, where $\overline{G} = \left( {V,\overline{E} } \right)$, $\overline{E} = E \cup \left\{ {\left( {i,j} \right): i,j \in L} \right\}$, and $\varvec{p} = \left[ {\varvec{p}_{1}^{T} , \ldots ,\varvec{p}_{n}^{T} } \right]^{T}$ is a realization of $\overline{G}$ in ${\mathbb{R}}^{d}$. Each follower knows and needs to control several desired distance constraints $d_{ji}^{*}$ with regard to its neighboring agents. It is assumed that the set of desired distance constraints $\varGamma = \left\{ {d_{ji}^{*} :\left( {i,j} \right) \in \overline{E} } \right\}$ is realizable in ${\mathbb{R}}^{d}$, i.e., there exists a realization $\varvec{p}^{*}$ such that $d_{ji}^{*} = \left\| {\varvec{p}_{j}^{*} - \varvec{p}_{i}^{*} } \right\|, \forall \left( {i,j} \right) \in \overline{E}$. The leaders’ positions satisfy $\left\| {\varvec{p}_{ji} \left( 0 \right)} \right\| = d_{ji}^{*}$, $\forall i, j \in L$, and the velocity $\varvec{f}\left( t \right)$ is unknown to the followers. The objective is to design control laws for the followers so that: (i) $\left\| {\varvec{p}_{j} \left( t \right) - \varvec{p}_{i} \left( t \right)} \right\| \to d_{ji}^{*} , \forall \left( {i, j} \right) \in E$, (ii) $\left\| {\dot{\varvec{p}}_{i} \left( t \right) - \dot{\varvec{p}}_{j} \left( t \right)} \right\| \to 0, \forall i, j \in V$, (iii) $\left\| {\varvec{u}_{i}^{i} \left( t \right)} \right\|$ is bounded, $\forall i \in V$.

Fig. 1.

Examples of directed acyclic leader-follower graphs in 2D and 3D

3 Proposed Control Laws and Stability Analysis

3.1 Formation in Two-Dimensional Space ($\varvec{d} = 2$)

Due to the acyclic directed structure of the formation, we firstly consider the first follower $i = d + 1 = 3$. Control design for other followers will follow similarly.

Target Point Localization:

The target point $i^{ * }$ of agent $i$ can be determined based on $\varvec{p}_{1i}^{i} , \varvec{p}_{2i}^{i}$ $d_{1i}^{ * }$, and $d_{2i}^{ * }$ as follows [4]:

$$ \begin{aligned} d_{12} = \left\| {\varvec{p}_{2i}^{i} - \varvec{p}_{1i}^{i} } \right\|, \phi_{i} = \arccos \left( {\frac{{d_{1i}^{*2} - d_{2i}^{*2} + d_{12}^{2} }}{{2d_{1i}^{*} d_{12} }}} \right) \hfill \\ \varvec{e}_{i} = \varvec{p}_{1i}^{i} + \varvec{R}\left( { \pm \phi_{i} } \right)\left( {\varvec{p}_{2i}^{i} - \varvec{p}_{1i}^{i} } \right)\frac{{d_{1i}^{*} }}{{d_{12} }} \hfill \\ \end{aligned} $$

(1)

Here, $\varvec{e}_{i}$ represents the relative vector $\varvec{p}_{{i^{*} i}}^{i}$, so that $i \to i^{*}$ is equivalent to $\varvec{e}_{i} \to 0$, and $\varvec{R}\left( { \pm \phi_{i} } \right) = \left[ {\begin{array}{*{20}c} {\cos \phi_{i} } & { \mp \sin \phi_{i} } \\ { \pm \sin \phi_{i} } & {\cos \phi_{i} } \\ \end{array} } \right]$ denotes rotation matrices in 2D. Since there are two desired locations $i^{*}$ which can be found, we use $\varvec{R}\left( { + \phi_{i} } \right)$ if we want $\Delta 12i^{*}$ to be anti-clockwise; and $\varvec{R}\left( { - \phi_{i} } \right)$ to make $\Delta 12i^{*}$ clockwise (see Fig. 2). Let $\varvec{R}_{i}$ be the rotation matrix that aligns the orientation of ${}^{g}\varSigma$ with ${}^{i}\varSigma$, so that $\varvec{f}^{i} \left( t \right) = \varvec{R}_{i} \varvec{f}\left( t \right)$. The target point moves at the same velocity with two leaders 1 and 2, and thus $\varvec{e}_{i} = \varvec{p}_{{i^{*} }}^{i} - \varvec{p}_{i}^{i}$ and $\dot{\varvec{e}}_{i} = \varvec{f}^{\varvec{i}} \left( t \right) - \varvec{u}_{i}^{i} \left( t \right)$.

Proposed Control Law:

The following control law is proposed for the follower $i$:

$$ \varvec{u}_{i}^{i} = k{\text{sgn}}^{a} \left( {\varvec{e}_{i} } \right) + k{\text{sgn}}^{b} \left( {\varvec{e}_{i} } \right) + \kappa {\text{sgn}}\left( {\varvec{e}_{i} } \right) , $$

(2)

where $a = 1 - \eta^{ - 1}$, $b = 1 + \eta^{ - 1}$, $k \ge \frac{\pi \eta }{2\tau }$, and $\eta > 1$ and $\tau > 0$ are two arbitrary numbers. We prove the following theorem:

Theorem 1:

Under the control law (2), $\left\| {\varvec{p}_{1i}^{i} } \right\| = d_{1i}^{*}$ and $\left\| {\varvec{p}_{2i}^{i} } \right\| = d_{2i}^{*}$, $\forall t \ge \tau$. In addition, $\left\| {\dot{\varvec{p}}_{i} \left( t \right) - \dot{\varvec{p}}_{1} \left( t \right)} \right\| = 0$, $\forall t \ge \tau$ and $\varvec{u}_{i}^{i} \left( t \right)$ is bounded $\forall t$.

Proof:

Consider the Lyapunov function $V = \frac{1}{2}\left\| {\varvec{e}_{i} } \right\|^{2}$. Based on [5], along a trajectory of (2), we have

$$ \dot{V} = \nabla V^{T} K\left[ {\dot{\varvec{e}}_{i} } \right] = \varvec{e}_{i}^{T} \varvec{f}^{i} - k\left( {\left\| {\varvec{e}_{i} } \right\|^{a + 1} + \left\| \varvec{e} \right\|_{i}^{b + 1} } \right) - \kappa \varvec{e}_{i}^{T} K\left[ {{\text{sgn}}\left( {\varvec{e}_{i} } \right)} \right]. $$

(3)

It follows from $xK\left[ {{\text{sgn}}\left( x \right)} \right] = \left| x \right|$, $\forall x \in {\mathbb{R}}$, that

$$ \dot{V} = \varvec{e}_{i}^{T} \varvec{f}^{i} - k\left( {\left\| {\varvec{e}_{i} } \right\|^{a + 1} + \left\| {\varvec{e}_{i} } \right\|^{b + 1} } \right) - \kappa \left\| {\varvec{e}_{i} } \right\|_{1} . $$

(4)

Since $\left\| {\varvec{e}_{i}^{T} \varvec{f}^{i} } \right\| \le \left\| {\varvec{e}_{i} } \right\|_{1} \left\| {\varvec{f}^{i} } \right\|_{\infty } \le \left\| {\varvec{e}_{i} } \right\|_{1} \left\| {\varvec{f}^{i} } \right\| \le \kappa \left\| {\varvec{e}_{i} } \right\|_{1}$, we have $\dot{V} \le - k\left( {\left\| {\varvec{e}_{i} } \right\|^{a + 1} + \left\| {\varvec{e}_{i} } \right\|^{b + 1} } \right)$, which is equivalent to

$$ \dot{V} \le - k2^{{\frac{1 + a}{2}}} V^{{\frac{1 + a}{2}}} - k2^{{\frac{1 + b}{2}}} V^{{\frac{1 + b}{2}}} . $$

(5)

Thus, using [6] yields $V = 0$, $\forall t \ge \frac{\pi \gamma }{2k}$. Note that $V = 0$ if and only if $\left\| {\varvec{p}_{1i}^{i} } \right\| = d_{1i}^{*}$ and $\left\| {\varvec{p}_{2i}^{i} } \right\| = d_{2i}^{*}$. Thus, the first conclusion of this theorem is as follows. When $t \ge \tau$, agent $i$ reached the desired position $i^{*}$, the velocity of $i$ is equal to the leaders’ velocity. Therefore, the second result holds. Finally, the boundedness of $\varvec{e}_{i}$ and thus of the controller follows from the fact that $V$ is nonnegative and nonincreasing. ■

After $t \ge \tau$, the agent $i$ satisfies all desired distances and has the same velocity as the leaders. It is then possible to consider agent $i$ as a leader to control the next follower. Thus, the control of subsequent followers can be treated similarly and the stability of the $n$-agent formation follows from mathematical induction.

Fig. 2.

Calculating the target point in 2D

3.2 Formation in Three-Dimensional Space ($\varvec{d} = 3$)

Following the same process as in 3.1, the most important part of the control strategy is to determine the target point from agent $i$’s locally-measured displacements. The control law can be adopted as in (2). Consider the first follower $i = d + 1 = 4$.

Target Point Localization:

As in 2D case, we determine the relative position of the desired position $i^{*}$ with regard to the current position of the agent $i$ in its local frame. Figure 3 illustrates the steps for calculating $\varvec{p}_{{i^{*} i}}^{i}$. The available information of agent $i$ includes $d_{1i}^{*}$, $d_{2i}^{*}$, $d_{3i}^{*}$, $\varvec{p}_{1i}^{i}$, $\varvec{p}_{2i}^{i}$, and $\varvec{p}_{3i}^{i}$. Based on this, the vector $\varvec{p}_{{i^{*} i}}^{i}$ can be computed as follows. First, the distances between the leaders are constants $d_{jk} = \left\| {\varvec{p}_{ji}^{i} - \varvec{p}_{ki}^{i} } \right\|,j,k \in \left\{ {1,2,3} \right\}$. Next, we calculate the volume $V_{i}$ of the tetrahedron $123i^{*}$ as follows:

Fig. 3.

Target point localization in 3D

$\alpha = \angle 2i^{*} 3 = \arccos \left( {\frac{{d_{2i}^{*2} + d_{3i}^{*2} - d_{23}^{2} }}{{2d_{2i}^{*} d_{3i}^{*} }}} \right)$, $\beta = \angle 1i^{*} 3 = \arccos \left( {\frac{{d_{1i}^{*2} + d_{3i}^{*2} - d_{13}^{2} }}{{2d_{1i}^{*} d_{3i}^{*} }}} \right)$, $\gamma = \angle 1i^{*} 2 = { \arccos }\left( {\frac{{d_{1i}^{*2} + d_{2i}^{*2} - d_{12}^{2} }}{{2d_{1i}^{*} d_{2i}^{*} }}} \right)$, and $V_{i} = \frac{{d_{1i}^{*} d_{2i}^{*} d_{3i}^{*} }}{6}\sqrt {1 + 2\cos \alpha \cos \beta \cos \gamma - \cos^{2} \alpha - \cos^{2} \beta - \cos^{2} \gamma }$.

Thus, the height $h_{i}$ from $i^{*}$ to the plane containing $123$ can be calculated from $V_{i}$ as $h_{i} = \frac{{6V_{i} }}{{\left\| {\left( {\varvec{p}_{1i}^{i} - \varvec{p}_{2i}^{i} } \right) \times \left( {\varvec{p}_{1i}^{i} - \varvec{p}_{3i}^{i} } \right)} \right\|}}$. The distances between the projection H of $i^{*}$ to the plane containing tree leaders and the leaders are $q_{k} = \sqrt {d_{ki}^{*2} - h_{i}^{2} } ,\forall k = 1, 2, 3$. Therefore, it holds $\phi = \angle 213 = \arccos \left( {\frac{{d_{12}^{2} + d_{13}^{2} - d_{23}^{2} }}{{2d_{12} d_{13} }}} \right)$, $\phi_{1} = \angle 21H = \arccos \left( {\frac{{d_{12}^{2} + q_{1}^{2} - q_{2}^{2} }}{{2d_{12} q_{1} }}} \right)$, and $\phi_{2} = \angle H13 = \arccos \left( {\frac{{d_{13}^{2} + q_{1}^{2} - q_{3}^{2} }}{{2d_{13} q_{1} }}} \right)$. In Fig. 3, 1 M and 1 N are projections of vector 1H along two lines 12 and 13, respectively. Depending on the position of H, vectors 1 M and 1 N are in the same or opposite direction to vectors 12 and 13, respectively. Figure 4 illustrates different cases corresponding to the position of H. Determining which cases occur is based on angles $\phi$, $\phi_{1}$, and $\phi_{2}$. Note that 1 M $\varvec{ } \uparrow \uparrow$ 12 if and only if $\phi_{1} = \left| {\phi - \phi_{2} } \right|$, and 1 N $\varvec{ } \uparrow \uparrow$ 13 if and only if $\phi_{2} = \left| {\phi - \phi_{1} } \right|$. Thus, by defining $\overline{{\phi_{1} }} = \phi_{1} \left( {1 - 2{\text{sgn}}\left( {\left| {\phi_{2} - \left| {\phi - \phi_{1} } \right|} \right|} \right)} \right)$ and $\overline{{\phi_{2} }} = \phi_{2} \left( {1 - 2{\text{sgn}}\left( {\left| {\phi_{1} - \left| {\phi - \phi_{2} } \right|} \right|} \right)} \right)$, the position of H is given as

Fig. 4.

Possible configuration of H

$$ \varvec{p}_{H1}^{i} = q_{1} \frac{{\sin \overline{{\phi_{1} }} }}{\sin \phi }\frac{{\varvec{p}_{1i}^{i} - \varvec{p}_{2i}^{i} }}{{\left\| {\varvec{p}_{1i}^{i} - \varvec{p}_{2i}^{i} } \right\|}} + q_{1} \frac{{\sin \overline{{\phi_{2} }} }}{\sin \phi }\frac{{\varvec{p}_{1i}^{i} - \varvec{p}_{3i}^{i} }}{{\left\| {\varvec{p}_{1i}^{i} - \varvec{p}_{3i}^{i} } \right\|}}. $$

(6)

Next, there are two points $i^{*}$ satisfying three distance constraints and they are symmetric with respect to the plane 123. By choosing a specific direction of the vector H$i^{*}$, the target point can be chosen from the following two points:

$$ \varvec{p}_{{i^{ *} H}}^{i} = \pm h_{i} \frac{{\left( {\varvec{p}_{1i}^{i} - \varvec{p}_{2i}^{i} } \right) \times \left( {\varvec{p}_{1i}^{i} - \varvec{p}_{3i}^{i} } \right)}}{{\left\| {\left( {\varvec{p}_{1i}^{i} - \varvec{p}_{2i}^{i} } \right) \times \left( {\varvec{p}_{1i}^{i} - \varvec{p}_{3i}^{i} } \right)} \right\|}}. $$

(7)

From these derivations, we have

$$ \varvec{e}_{i} = \varvec{p}_{{i^{*} i}}^{i} = \varvec{p}_{{i^{*} H}}^{i} + \varvec{p}_{H1}^{i} + \varvec{p}_{1i}^{i} . $$

(8)

Because $\varvec{p}_{{i^{*} H}}^{i} + \varvec{p}_{H1}^{i}$ is invariant with respect to time t, agent $i$ only needs to compute this term one time. The calculation must be performed again if the desired formation is time-varying ($d_{1i}^{*}$, $d_{2i}^{*}$, or $d_{3i}^{*}$ are changing with time). The formation tracking control law in this case has the same form as (2).

4 Simulation Results

In this section, we consider two eight-agent systems in 2D and 3D. The graphs and the desired formations are depicted as in Figs. 1 and 5. The orientations and initial positions of the followers are randomly selected. In both simulations, the control law (2) is adopted with parameters $\tau = 0.25$ and $\kappa = 2$.

Fig. 5.

The desired configurations in Simulations 1 and 2

Simulation 1:

The initial positions of the leaders are chosen as $\varvec{p}_{1} \left( 0 \right) = \left[ {0, 2} \right]^{T}$ and $\varvec{p}_{2} \left( 0 \right) = \left[ {0, 0} \right]^{T}$. The leaders’ velocity is $\varvec{f}\left( t \right) = \sqrt 2 \left[ {\sin 0.2t,\cos 0.3t} \right]^{T}$. As can be seen in Fig. 6a, after a finite time, the desired formation is achieved, and the followers move at the same velocity as the leaders. Thus, the simulation results are consistent with our analysis.

Fig. 6.

Trajectories of the agents in Simulations 1 (a) and 2(b)

Simulation 2:

The leaders’ initial positions are $\varvec{p}_{1} \left( 0 \right) = \left[ {0, 0, 0} \right]^{T}$, $\varvec{p}_{2} \left( 0 \right) = \left[ {0, 2, 0} \right]^{T}$, $\varvec{p}_{3} \left( 0 \right) = \left[ {2, 2, 0} \right]^{T}$ and the reference velocity is $\varvec{f} = \frac{2}{\sqrt 3 }\left[ {\sin 0.2t,\cos 0.3t,\sin 0.4t} \right]^{T}$. Trajectories of the agents are depicted in Fig. 6b. It is also observed from the simulations that the followers track the leaders and render the desired formation in a finite time.

5 Conclusion

This paper has proposed distance-based tracking control laws in 2D and 3D for formation with directed acyclic graphs. The main idea of the control laws is simultaneously localizing and tracking a target point, which moves with the same leaders’ velocity and satisfies all the desired distances. Future works will consider the formation tracking problem with more practical requirements such as disturbance attenuation, obstacle avoidance, or realistic agents’ models.