Swarm Herding: A Leader-Follower Framework For Multi-Robot Navigation

In this paper, a team of leader robots is modeled as a mass-spring-damper network subject to external forces which are generated by multiple constraints. The leaders are considered as masses that are interconnected by virtual springs and dampers as shown in the Fig. 1.

Let $x_{k}\in\mathbb{R},\,k=1,\cdots,M$ ($M$ is an even number) denote the position of the leaders. Based on Newton’s second law, the equation of motion is given by

where $\mathcal{N}_{k}$ denotes a set which contains masses connected to mass $k$ (i.e., neighbor set of mass $k$), the spring force and the damping force on mass $i$ are $g_{kl}=-c(\dot{x}_{k}-\dot{x}_{l})$ and $f_{kl}=-\kappa(\|x_{k}-x_{l}\|-l_{kl}^{o})\frac{x_{k}-x_{l}}{\|x_{k}-x_{l}\|}$ respectively, and the $F_{k}$ is the external forces applied on mass $k$. Moreover, $c$ denotes the damping constant, $\kappa$ denotes the stiffness, $l_{kl}^{o}$ represents the rest length of the spring between mass $k$ and $l$.

For such a system, the stability property can be described as the following proposition.

From the proof above, without the external input force $F_{k}$, it can be noticed that given an initial condition the MSD system will asymptotically converge to the configuration with springs at their rest lengths, and all the masses have the same velocity. Now we can design the external forces such that the domain defined by leaders will move along the reference path, and the vertices of the domain (leader robots) will avoid the obstacles.

To satisfy the constraint that the domain defined by the leaders always sustains an area for the follower robots to occupy, we introduce the following lemma.

Lemma 1 imposes a restriction on the deformation of the mass-spring-damper network such that each mesh defined by 4 masses will always be a convex quadrilateral, and this property will be used in next section.

Several constraints are imposed on the masses of the MSD network, the “head” of the MSD network (i.e., the $m_{1}$ and $m_{2}$ shown in Fig. 1) will take the responsibility of tracking the reference path and determining the orientation of the system, and all the masses (leader robots) will avoid the obstacles based on the sensing information.

The external force applied to a mass is defined as

where $f_{\text{friction}}^{K}$ is a virtual friction force imposed on masses to dissipate kinetic energy stored in the system. $f_{\text{tracking}}^{k}$ is the force which drives the team of leaders to move along the reference path, the force $f_{\text{orienting}}^{k}$ adjusts the orientation of the system, and $f_{\text{sensing}}^{k}$ is the force generated based on sensing information that drives the $k$th leader robot to move away from the obstacles.

The sensing information about the environment obtained by the sensors installed on the leaders can be presented as $[o_{k},\,d_{k}]$, where $o_{k}$ denotes the position of the nearest obstacle, and $d_{k}$ denotes the distance from the nearest obstacle detected in the $k$th leader’s reference frame, then the external force which drives the leader robot away from the obstacle is defined as

where $\kappa_{1}>0$ is a tuning parameter, and $\delta_{\text{sensing}}$ is the prescribed safe distance between a leader robot and the obstacles. Perceivably, $\|f_{\text{sensing}}^{k}\|\to\infty$ as $d_{k}\to\delta_{\text{sensing}}$.

Moreover, we assume that the first layer of our control scheme returns the information of global reference path in the form of $[\Gamma,\,\dot{\Gamma}]$, then $f_{\text{tracking}}^{k}$ is defined as

which has a feedforward term $\dot{\Gamma}$ and a feedback term $\kappa_{2}(\Gamma-c_{x})$ to drive the bisecting point of the positions of “head” of the system (i.e., $c_{x}=\frac{1}{2}(x_{1}+x_{2})$) to track the reference path, where $\kappa_{2}>0$ is a tuning parameter.

As we shown in Proposition 1, even if the external force becomes zero, the system will keep moving at certain speed, thus friction is necessary to exist for dissipation of the kinetic energy stored in the system and can be designed as

where $\kappa_{3}>0$ is a control parameter.

Besides, the “head” of the MSD network (i.e., $m_{1}$ and $m_{2}$) also takes the responsibility to determine the orientation of the system, i.e., the line $\overline{x_{1}x_{2}}$ maintains orthogonal to the path $\Gamma$. Thus the force $f_{\text{orienting}}^{k}$ can be designed as

where $\kappa_{4}>0$. The inner product of vector $x_{1}-x_{2}$ and the tangent of path $\dot{\Gamma}$ is greater than zero indicates the angle between the two vectors is less than $90^{\circ}$, then the the force $f_{\text{orienting}}^{1}$ will drive $m_{1}$ opposite to the direction of $\dot{\Gamma}$ until the inner product becomes zero.

It can be seen that by applying the designed external forces to the MSD network, the “head” of network will steer and lead entire system, and the orientation information will be propagated through the network until it finally reaches the “tail” of the network; the robots will also avoid the obstacles based on the sensing information.

Now we can define a time-varying domain $\mathcal{S}(t)$ with vertices $\{x_{k},\,k=1,\cdots,M\}$. The time-varying domain will evolve subject to the external force defined by (2). In the next section, we can design the control scheme for a team of followers.

To find the best configuration formed by the robotic team over a domain, we partition the domain into non-overlapping regions and let each region be taken care of by a robot. Formally, let $p_{i}\in\mathcal{S}(t),i\in\{1,\ldots,N\}$ be the positions of a group of follower robots in a convex domain $\mathcal{S}(t)$ defined by leaders, the coverage problem is dictated as finding the best configuration of robots $p(t)=\left[p_{1}^{T}(t),\ldots,p_{N}^{T}(t)\right]^{T}$ to minimize the locational cost [21] which evaluates the coverage performance of $p$ over the domain $\mathcal{S}$:

where $\phi(q,t):\mathcal{S}(t)\times[0,\infty)\to\mathbb{R}_{>0}$ is a density function that represents the relative importance of each point in the domain at time $t$, and $V_{i}(p,t)=\left\{q\in\mathcal{S}(t)~{}\middle|~{}\|p_{i}-q\|\leq\|p_{j}-q\|~{}\forall j\right\}$ is a Voronoi tessellation.

It is known that a necessary condition to minimize the locational cost defined in (6) is the centroidal Voronoi tessellation (CVT) configuration, i.e., $p_{i}(t)=C_{i}(p,t)\,\,\,\forall i$, where $C_{i}(p,t)$ is the cener of mass (centroid) of $i$th Voronoi cell. A control law is proposed for time-varying densities and time-varying domains to exponentially converge to a centroidal Voronoi configuration in [20, 16],

where the exponential convergence rate is tuned by the parameter $K>0$.

The control law given in (7) can be approximated as

by truncating the Neumann series for the matrix inverse to make the control law decentralized.

The expressions of the centroids $C_{i}(p,t)$, and matrices $\frac{\partial C}{\partial p}$ and $\frac{\partial C}{\partial t}$ in (7) and (8) can be found in [16]. The matrix $\frac{\partial C}{\partial p}$ is a block matrix with sparsity structure encoding the adjacency information in the Voronoi tessellation [20], and the matrix $\frac{\partial C}{\partial t}$ captures the evolution of time-varying densities and time-varying domains.

The coverage control law in (7) and (8) can only treat a time-varying convex domain. However, for a domain induced by a group of leader robots moving in a cluttered environment, it is likely that the domain will become non-convex. In previous work [22], we dealt with a non-convex domain by using a diffeomorphism [23] to map points between the non-convex domain and its convex hull. In this paper, we will treat the problem in a different way that leverages the quadrilateral structure of the mesh formed by the leader agents.

As a consequence of Lemma 1, a mesh in the mass-spring-damper network will always be a quadrilateral by restricting the elongation and compression of the springs, thus the time-varying domain defined by a team of leaders will be a composition of quadrilaterals. The strategy for performing coverage control of a group of followers over the composition of quadrilaterals will be to transform the successive quadrilaterals one by one such that a long rectangular region with prescribed height and length is obtained, as illustrated in Fig. 2. In this way, the coverage control law (7) and (8) can be directly utilized, and the resulting control signals can be transformed back into the actual domain.

Without loss of generality, let the $M$ leaders ($M$ being an even number) be labeled as illustrated in Fig. 2(ii), then there are $\frac{M}{2}-1$ meshes in the network, and the $h$th mesh (denoted as mesh$(h)$) will be defined by vertices $x_{2h-1},\,x_{2h},\,x_{2h+2}$ and $x_{2h+1},\,h=1,\cdots,\frac{M}{2}-1$ in counterclockwise order. Given these vertices and by using a perspective projection as in [24], the mesh$(h)$ can be transformed to an arbitrary convex quadrilateral mesh$(\widetilde{h})$ of vertices with prescribed positions $\widetilde{x}_{2h-1},\,\widetilde{x}_{2h},\,\widetilde{x}_{2h+2}$ and $\widetilde{x}_{2h+1}$, i.e., $T_{h}:\text{mesh}(h)\to\text{mesh}(\widetilde{h})$.

The position of $i$th follower robot $p_{i}$ in the actual domain will be transformed by $T_{h}$ if $p_{i}\in\text{mesh}(h)$, i.e., $\widetilde{p}_{i}=T_{h}(p_{i})$. The transformed followers $\{\widetilde{p}_{i}\}_{i=1}^{N}$ will be performing coverage over the resulting long rectangular domain $\widetilde{\mathcal{S}}(t)$ defined by vertices $\{\widetilde{x}_{k}\}_{k=1}^{M}$.

For $\widetilde{p}_{i}=T_{h}(p_{i})$, by the chain rule we can obtain that

which leads to

where from (7) and (8),

we can find that $\frac{\partial\widetilde{C_{i}}}{\partial t}$ is zero in this case since the time-dependency of the domain is captured by the defined transformation $T_{h}$ when the result virtual rectangular domain and the density are static.