1 Introduction

Obstacle avoidance allows robots to navigate around unexpected obstacles during movement, rather than following a predetermined static path like in the path planning process. Controlling a swarm of robots adds complexity to the problem, as the swarm must not only avoid obstacles, but also maintain cohesion and perform the assigned task, such as reaching a specific point. This involves robots maintaining a set distance from each other and moving in a coordinated way along a given trajectory or toward a designated point. Some early methods for obstacle avoidance are based on nature and involve moving directly toward a goal until an obstacle is encountered, at which point the obstacle is circumnavigated until the goal can be pursued again. One of these methods is known as bug algorithm [1]. It can result in long trajectories and close proximity to obstacles. Other methods for swarm control are drawn from classical techniques used for single robot control, including graph theory [2, 3], Voronoi diagrams [4], and most commonly used artificial potential fields [5,6,7]. Advanced methods for obstacle avoidance include fuzzy logic techniques [8, 9], genetic algorithms [10, 11], neural networks [12, 13], and other artificial intelligence approaches [14, 15]. In practical applications, such as with mobile robots, obstacle avoidance must be considered if an obstacle is detected on the trajectory of the vehicle. Fuzzy logic can be used to define user-friendly robot control rules, which greatly simplifies the mathematical description of the robot control. The genetic algorithms can automate the selection of the right parameters in the swarm control law. Most genetic algorithms are often offline methods, which is caused by high computational complexity. Algorithms are often limited to adapting the control to a specific environment, which can affect the robot’s ability to avoid obstacles in a non-simulated environment. Neural networks can be utilized to recognize obstacles in the environment and control the speed of the robot to guide it to its destination.

In the field of swarm robotics, control methods for coordinating the movement of robots are commonly based on the concept of Virtual Physics [16], which is related to the method of virtual potential fields. The aim is to ensure that the robots maintain a specific distance between each other, while moving in a coordinated manner along a given trajectory or toward a desired point. To achieve this, in case of the method of artificial potential fields, the virtual forces acting on the robot are calculated from the resultant virtual potential field in which the robot is located. The environment of the robots is modeled as a sum of virtual potential fields from obstacles, other robots, and the goals of individual robots and the swarm. In this article, a different approach to calculate virtual forces is presented. The virtual forces are the result of virtual spring-damper connections between robots, obstacles, and a reference point. The presented method solves the problem of swarm trajectory tracking in an environment with obstacles, after the swarm has self-organized into the shape of a regular polygon. Tracking control of a swarm is understood as following a given trajectory of the reference point by the swarm’s geometric center. The reference point’s path is a straight line between two obstacles with velocity profile as a trapezoidal curve, with acceleration and deceleration stages approximated by sigmoidal functions. The robots have to avoid collisions with two polygons spaced apart at a distance of 0.7 times the swarm diameter after self-organization. Due to sensor limitations, each robot can only determine the location of its neighboring robots, while the position of the reference point is known to all robots.

The article is structured in 5 sections. In Sect. 1, a comprehensive review of the relevant literature and a clear explanation of the problem under study are presented. The description of the kinematic and dynamic equations for two-wheeled robots in a swarm, along with a thorough explanation of the swarm control method, is presented in Sect. 2. In Sect. 3, the details of the simulation and experimental setup, including the assumptions and parameters adopted for the swarm are shown. Section 4 presents and compares the results of the research, which is followed by a discussion. Finally, Sect. 5 provides a summary of the research and the main findings of the article.

2 Methods

This section provides a full description of swarm control. The section begins with a description of the kinematics and dynamics of a two-wheeled robot. Next, the details of the swarm trajectory algorithm and the relationship between the virtual forces and robots kinematic parameters are given. The description consists of definitions of virtual forces and deformations of virtual springs. The equations of the control signals of the i-th robot were derived, and the concept of the “geometric center of the swarm” was introduced. The section ends with a detailed swarm control scheme with its description.

2.1 Single robot model

The swarm consists of 5 two-wheeled robots with two spherical support wheels. As shown in Fig. 1, each robot has two drive modules marked as 1 and 2 with wheels whose geometric centers marked as points B and C. The center of gravity of the robot is at point \( S_i \) on the robot’s frame 3. The point \( S_i \) lies on the axis of symmetry of the driving wheels at a distance d from the center of the robot’s driving axis. The angles of rotation of the wheels are marked as \( \alpha _{1i}\) and \(\alpha _{2i} \). Point D is the instantaneous frame rotation center around which the robot’s instantaneous frame rotation angle \( \beta _i \) is determined. The point \(A_i\), shown in Fig. 1, is the geometric center of the i-th robot. It moves with velocity \(v_{Ai}\) and has coordinates \((x_{Ai},y_{Ai})\) in the fixed x, y coordinate system. It is assumed that the robots move on a flat surface without skidding.

Fig. 1
figure 1

The schematics of the i-th wheeled robot in the swarm of robots

Full size image

The i-th robot follows a given trajectory expressed as a function of the angular velocities of the driving wheels \({\dot{\alpha }}_{1i}\) and \({\dot{\alpha }}_{2i}\). The angular velocities of the robot’s driving wheels are the robot configuration coordinates which are determined from the following equations:

$$\begin{aligned} {\dot{\alpha }}_{1i} = \dfrac{v_{Ai}}{r}+\dot{\beta _{i}}\dfrac{L}{r}, \quad {\dot{\alpha }}_{2i} = \dfrac{v_{Ai}}{r}-\dot{\beta _{i}}\dfrac{L}{r}, \end{aligned}$$
(1)

as well as equations:

$$\begin{aligned} \begin{aligned} \dot{\beta _i}&= \dfrac{r}{2L}({\dot{\alpha }}_{1i} - {\dot{\alpha }}_{2i}), \quad {\dot{x}}_{Ai} = \dfrac{r}{2}({\dot{\alpha }}_{1i} + {\dot{\alpha }}_{2i})\cos (\beta _{i}), \\ {\dot{y}}_{Ai}&= \dfrac{r}{2}({\dot{\alpha }}_{1i} + {\dot{\alpha }}_{2i})\sin (\beta _{i}). \end{aligned} \end{aligned}$$
(2)

The robot’s angular velocities of driving wheels are denoted by \( {\dot{\alpha }}_{1i} \),\( {\dot{\alpha }}_{2i} \). The robot’s driving wheels are spaced 2L apart and have radius r. Robot kinematic parameters are \(\alpha _{1i},\alpha _{2i},{\dot{\alpha }}_{1i},{\dot{\alpha }}_{2i}\).

The dynamic equations for the motion of a two-wheeled robot are described in [17]. The presented model of the i-th robot is extended with the dynamics of DC motors with a gearbox [18]. Dynamic equations for a mobile robot will have the form:

$$\begin{aligned} {\overline{{\textbf {M}}}\ddot{{\textbf {q}}}_{{\textbf {Mi}}}}+ {\overline{{\textbf {C}}}({\dot{{\textbf {q}}}}_{Mi}){\dot{{\textbf {q}}}}_ {{\textbf {Mi}}}} +{\overline{{\textbf {F}}}({\dot{{\textbf {q}}}}_{{\textbf {Mi}}})}={\textbf {u}}_{\textbf {i}}. \end{aligned}$$
(3)

The vectors and matrices in Eq. (3) are as follows: vector of generalized coordinates \(\mathbf {q_{Mi}}=[\alpha _{1i},\alpha _{2i}]^T\), inertia matrix \({\overline{\textbf{M}}}\), vector of centrifugal and Coriolis forces \( {\overline{\textbf{C}}}({\dot{\textbf{q}}}_{\textbf{Mi}}){\dot{\textbf{q}}}_{\textbf{Mi}} \), vector of friction forces \({\overline{\textbf{F}}}({\dot{\textbf{q}}}_{\textbf{Mi}})\) and control inputs of the i-th robot (driving torques of wheels in function of voltage) \(\mathbf {u_i}=[u_{1i},u_{2i}]^T\).

Proper grouping of the matrix elements \({\overline{\textbf{M}}}, {\overline{\textbf{C}}}({\dot{\textbf{q}}}_{\textbf{Mi}}) \) and vector \({\overline{\textbf{F}}}({\dot{\textbf{q}}}_{\textbf{Mi}})\) will result in:

$$\begin{aligned} \begin{aligned} \overline{{\textbf{M}}}&= \begin{bmatrix} (a_1+a_2+a_3)r_k^2 + J_{k1} &{} (a_1 - a_2)r_k^2 \\ (a_1 - a_2)r_k^2 &{} (a_1+a_2+a_3)r_k^2 + J_{k2} \end{bmatrix}, \\ {\overline{\textbf{C}}}({\dot{\textbf{q}}}_{\textbf{Mi}})&= \begin{bmatrix} 0 &{} 2a_4r_k^2({\dot{\alpha }}_{2i} - {\dot{\alpha }}_{1i}) \\ -2a_4r_k^2({\dot{\alpha }}_{2i} - {\dot{\alpha }}_{1i}) &{} 0 \end{bmatrix},\\ {\overline{\textbf{F}}}({\dot{\textbf{q}}}_{\textbf{Mi}})&= \begin{bmatrix} a_5r_k^2sgn({\dot{\alpha }}_{1i}) +B_1{\dot{\alpha }}_{1i} \\ a_6r_k^2sgn({\dot{\alpha }}_{2i}) +B_2{\dot{\alpha }}_{2i} \end{bmatrix}, \mathbf {u_i} = \begin{bmatrix} u_{1i} \\ u_{2i} \end{bmatrix}u_{\max }, \end{aligned}\nonumber \\ \end{aligned}$$
(4)

\(u_{\max }=\dfrac{K_Mr_k}{R}V_{\max } \quad dla \quad k=1,2, \quad u_1,u_2 \in <-1,1>.\)

The variables \(u_{1i}\) and \(u_{2i}\) are the normalized control signals, \(K_M\) is the torque constant, \(r_k\) is the gear ratio for k-th drive module, and \(J_{k1}\),\(J_{k2}\) are the mass moments of inertia of the k-th module.

The vector of the parameters \( {\textbf{a}} = [a_1, a_2, a_3, a_4, a_5, a_6] ^ T \) results from the expansion of the dynamics equations. The process of parametric identification of the robot model was carried out using the nonlinear least squares method. The obtained values of unknown parameters were:

\({\textbf{a}}= [7.3125\times 10^{-5},8.97\times 10^{-9},5.543 \times 10^{-6},0.0126,0.917,0.917]\) and \(B_1=3.01\times 10^{-5},B_2=3.37\times 10^{-5}\)

The value of the gear ratio \( r_k \) is known from the documentation [19], while \( V_{\max } \) is the maximum voltage that can be provided by the robot’s power supply. Based on the geometry of the robot and the motor documentation, other parameters can be defined as: \(r= 0.03~\text {m}, L=0.069~\text {m},K_m=0.048[-],R=2.17[\Omega ], r_k = 1/120[-], V_{\max } = 7.4[V]\)

The presented model is used in the simulation.

2.2 Swarm trajectory tracking algorithm

The main goal of robots is to follow a reference point at a certain distance, maintain a given distance to the nearest neighboring robots and avoid collisions with obstacles by moving away from them further than the set minimum distance. A given robots trajectory is defined for the virtual reference point, the initial position of which is selected by the swarm operator. The robots that are closest to their neighboring robots are considered the j-th robots within the sensory range of the i-th robot. Robots know predefined coordinates of a moving reference point.

The behavior of one single robot under the influence of virtual forces can be described in the following way.

The center of the driving axis \( A_i \) of the i-th robot in the swarm is influenced by a virtual force derived from the starting point of the resultant force W, moving at the velocity \( \overline{v_w} \). The \( W_i \) point is the point from which the virtual force \( \overline{F_{iW}} \) acts on the i-th robot. The robot is placed in the fixed x, y coordinate system at a distance \(r_i\) from the point (0, 0). At point A, the local movable coordinate system p, q is attached, where the p axis is directed to the front of the robot and the q axis coincides with the drive axis of the robot (Fig. 2).

Fig. 2
figure 2

The resultant virtual force acting on the i-th robot in the virtual force field

Full size image

Virtual force \( \overline{F_{iW}} \) is the vector sum of virtual forces from neighboring robots and the reference point. The robots are to reach the point \( W_i \) that is at the distance \(r_{iW}\) from the point \( A_i \). The closer the distance from the point \( A_i \) to the point \( W_i \), the closer to zero is the vector sum of the virtual forces acting on the i-th robot. The angle of inclination of the robot frame to the x axis is \( \beta _i \), and the center of the drive axis of the i-th robot moves at the velocity \( \overline{v_{Ai}} \).

The value of the vector of the resultant force acting on the i-th robot is defined as:

$$\begin{aligned} F_{iW} = k_{iW}e_{iW} + c_{iW}{\dot{e}}_{iW}. \end{aligned}$$
(5)

The virtual force \(F_{iW}\) has two parameters, the spring constant of the virtual resultant spring \(k_{iW}>0\) and the damping constant of the virtual resultant damper \(c_{iW}>0\). The virtual spring has a virtual deformation denoted by \(e_{iW}\).

In order for the robot to move in the direction of the resultant force to its source, the velocity vector of the point \( A_i \) must have the direction of the resultant vector of virtual forces, therefore the following relationship must be fulfilled:

$$\begin{aligned} \overline{F_{iW}} \times \overline{dV_{Ai}} = 0. \end{aligned}$$
(6)

The angle \( \psi _{iW} \) between the vectors \( \overline{F_{iW}} \) and \( \overline{dV_{i}} \) will approach zero if the vector of the instantaneous velocity of the robot’s frame is proportional and opposite to the vector product (6) [20]:

$$\begin{aligned} \overline{\dot{\beta _{i}}}=-\lambda _1(\overline{F_{iW}} \times \overline{dV_{Ai}}){\overline{k}},\quad \lambda _1 > 0. \end{aligned}$$
(7)

By eliminating the vector product, the value of the desired angular velocity \(\dot{\beta _{id}}\) in relation to the x and y projections \(F_{iWx}\) and \(F_{iWy}\) of the force \(F_{iW}\) will take the form:

$$\begin{aligned} \overline{\dot{\beta _{id}}}=-\lambda _1(F_{iWx}d{\dot{y}}_{Ai}-F_{iWy}d{\dot{x}}_{Ai}){\overline{k}}. \end{aligned}$$
(8)

The values of the projections of virtual forces on the x and y axes are determined from the rotation equation of the p and q coordinate system:

$$\begin{aligned} \begin{bmatrix} F_{iWx} \\ F_{iWy} \end{bmatrix}= & {} Rot\begin{bmatrix} F_{iWp} \\ F_{iWq} \end{bmatrix}= \begin{bmatrix} \cos \beta _i -\sin \beta _i\\ sin\beta _i \cos \beta _i \end{bmatrix} \begin{bmatrix} F_{iWp} \\ F_{iWq} \end{bmatrix} \nonumber \\= & {} \begin{bmatrix} F_{iWp}\cos \beta _i - F_{iWq}\sin \beta _i\\ F_{iWp}\sin \beta _i + F_{iWq}\cos \beta _i \end{bmatrix}. \end{aligned}$$
(9)

The projections of virtual force \(F_{iW}\) on the p and q axes \((F_{iWp},F_{iWq})\) can be derived from Fig. 2 as follows:

$$\begin{aligned} F_{iWp}= & {} F_{iW}\cos \psi _{iW}, \end{aligned}$$
(10)
$$\begin{aligned} F_{iWq}= & {} F_{iW}\sin \psi _{iW}. \end{aligned}$$
(11)

Assuming that:

$$\begin{aligned} d{\dot{x}}_{Ai} = dV_{Ai}\cos \beta _i, \quad d{\dot{y}}_{Ai} = dV_{Ai}\sin \beta _i, \quad dV_{Ai} = 1. \nonumber \\ \end{aligned}$$
(12)

By inserting the relationships from Eq. (12) into Eq. (8), the general form of the desired angular velocity of the robot frame will take the form:

$$\begin{aligned} \overline{\dot{\beta _{id}}}=\lambda _1F_{iWq}{\overline{k}}=\lambda _1F_{iW}\sin \psi _{iW}{\overline{k}}. \end{aligned}$$
(13)

Using Eq. (11), the value of the desired angular velocity of the robot frame will be:

$$\begin{aligned} \dot{\beta _{id}}=\lambda _1F_{iW}\sin \psi _{iW}, \quad \lambda _1 > 0, \lambda _1~\text {rad}/\text {Ns}. \end{aligned}$$
(14)

Assuming that the acceleration of the robot is proportional to the projection of the vector \(F_{iW}\) on the p axis (10), the desired acceleration of the i-th robot \(a_{id}\) can be expressed as:

$$\begin{aligned} a_{id}=\lambda _2F_{iWp}=\lambda _2F_{iW}\cos \psi _{iW}, \quad \lambda _2 >0, \lambda _2~1/\text {kg}. \end{aligned}$$
(15)

By integrating Eq. (15), the desired velocity of the point \(A_i\), \((v_{id})\), will be:

$$\begin{aligned} v_{id} = \lambda _2\int \left( F_{iW}\cos \psi _{iW}\right) dt. \end{aligned}$$
(16)

Knowing that \(v_{id}=u_{vi}\) and \(\dot{\beta _{id}} = u_{\beta i}\), after writing out the equation of virtual resultant force (5) in Eqs. (16, 14), we will get:

$$\begin{aligned} u_{vi}= & {} \lambda _2\int \left( \left( k_{iW}e_{iW} + c_{iW}{\dot{e}}_{iW} \right) \cos \psi _{iW}\right) dt, \end{aligned}$$
(17)
$$\begin{aligned} u_{\beta i}= & {} \lambda _1 \left( k_{iW}e_{iW} + c_{iW}{\dot{e}}_{iW} \right) sin\psi _{iW}. \end{aligned}$$
(18)

The robots determine their own \( u_{vi} \) and \( u_{\beta i} \) controls based on their knowledge of the positions and velocities of neighboring robots and a reference point. The obtained values of the control signals are used to determine the desired trajectory to be followed by the i-th robot. The position of the i-th robot in relation to the j-th robot and to the moving reference point in the presence of an obstacle is shown in Fig. 3. The i-th robot is described in relation to the same coordinate systems as in Fig. 2. The j-th robot, point \(O_{io}\) and point P are a distance \(r_{ij}\),\(r_{io}\),\(r_{ip}\) from the point \(A_i\), respectively.

Fig. 3
figure 3

Virtual forces acting on the point \(A_i\) of the i-th robot from the neighboring j-robots and the moving reference point P in the presence of an obstacle

Full size image

It is difficult to calculate the value of the resultant spring deformation \(e_{iW}\), therefore it is necessary to decompose the resultant force vector \(F_{iW}\) into its components. Those components are the virtual forces from neighboring robots, the reference point and obstacles.

The virtual forces and the distances between the i-th robot and adjacent j-th robots and the reference point and the obstacle O are related to each other and their relation is described as follows: the i-th robot is affected by virtual forces from virtual spring-damping connections between the nearest robots \(\overline{F_{ij}}\) (between the centers of the robot’s drive axes i and j), obstacles \( \overline{F_{io}} \) (between the center of the drive axis of the i-th robot and the nearest point \( O_{io} \) on the edge of the obstacle O) and the reference point \(\overline{F_{ip}} \).

The vectors of virtual forces are described by the equations:

$$\begin{aligned} \overline{F_{ij}}= & {} (k_{ij}e_{ij}+c_{ij}{\dot{e}}_{ij})\overline{\delta _{ij}}, \end{aligned}$$
(19)
$$\begin{aligned} \overline{F_{ip}}= & {} (k_{ip}e_{ip}+c_{ip}{\dot{e}}_{ip})\overline{\delta _{ip}}. \end{aligned}$$
(20)

The vectors \(\overline{F_{ij}}\) and \(\overline{F_{ip}}\) point in the direction of the unit vectors \(\overline{\delta _{ij}}\),\(\overline{\delta _{ip}}\) that come from the center of the drive axis of the i-th robot \( A_i \) to the center of the drive axis of the nearest adjacent j-th robot \( A_j \) and to the reference point P, respectively (Fig. 3). The virtual forces \(\overline{F_{ij}}\) and \(\overline{F_{ip}}\) are proportional to the virtual spring deformation \(e_{ij},e_{ip}\) and their derivatives. The virtual deformations \(e_{ij}\) and \(e_{ip}\) are calculated between the i-th and j-th robots and between the i-th robot and point A, respectively.

The values of the virtual component forces are:

$$\begin{aligned} F_{ij}= & {} k_{ij}e_{ij} + c_{ij}{\dot{e}}_{ij}, \end{aligned}$$
(21)
$$\begin{aligned} F_{ip}= & {} k_{ip}e_{ip} + c_{ip}{\dot{e}}_{ip}. \end{aligned}$$
(22)

Equations (21) and (22) describe the virtual forces from the spring and dampers. There are two types of constants, the spring constant of the virtual spring \(k_{ij}\) and the damping constant \(c_{ij}\) of the virtual damper between the i-th robot and the closest j-th robot. Similar constants are for the reference point P, i.e., \(k_{ip}\) and \(c_{ip}\).

The values of the virtual deformation \(e_{ij}\) and \(e_{ip}\) are expressed by the following equations:

$$\begin{aligned} e_{ij}= & {} r_{ij}-l_{ij}=\sqrt{(x_{Aj}-x_{Ai})^2+(y_{Aj}-y_{Ai})^2}-l_{ij}, \end{aligned}$$
(23)
$$\begin{aligned} e_{ip}= & {} r_{ip}-l_{ip}=\sqrt{(x_p-x_{Ai})^2+(y_p-y_{Ai})^2}-l_{ip}. \end{aligned}$$
(24)

The virtual deformations are calculated knowing the x and y coordinates of the centers of the robots i, j \( (x_{Ai}, y_{Ai})\) and \((x_{Aj}, y_{Aj})\) and the coordinates of the reference point \((x_p\) and \(y_p)\). The resting lengths of virtual springs are denoted as \( l_{ij}> 0\) for the spring between the i-th robot and the j-th robot and \( l_{ip}> 0 \) for the spring between the i-th robot and the reference point P.

The virtual force vector \(\overline{F_{io}} \) is described by the equation:

$$\begin{aligned} \overline{F_{io}}=(k_{io}e_{io}+c_{io}{\dot{e}}_{io})\overline{\delta _{io}}. \end{aligned}$$
(25)

The vector \(e_{io}\) is the virtual spring deformation between the i-th robot and point \(O_{io}\). The unit vector \(\overline{\delta _{io}}\) is directed from the center of the drive axis of the i-th robot \( A_i \) to the nearest point \( O_{io} \) on the o-th obstacle (Fig. 3). The value of \(\overline{F_{io}} \) virtual force is given by the formula:

$$\begin{aligned} F_{io} = k_{io}e_{io} + c_{io}{\dot{e}}_{io}. \end{aligned}$$
(26)

Similarly to Eqs. (21, 22) the virtual force \(\overline{F_{io}} \) has two constants, virtual spring constant \(k_{io}\) and virtual damper constant \(c_{io}\) between the i-th robot and the obstacle O.

The value of the virtual deformation \(e_{io} \) expresses the relationship:

$$\begin{aligned} e_{io}=r_{io}-l_{io}=\sqrt{(x_{io}-x_{Ai})^2+(y_{io}-y_{Ai})^2}-l_{io}. \end{aligned}$$
(27)

The coordinates \((x_{Ai},y_{Ai})\) and \( (x_{io}, y_{io})\) are x and y coordinates of the robot’s driving axis center and coordinates of the nearest point on the o-th obstacle closest to i-th robot, respectively. The constant \( l_{io} \) is the resting length of the virtual spring between the point \( A_i \) of the i-th robot and the point \( O_ {io} \) on the obstacle (so called limit distance).

For robots to be pushed away from obstacles, the force between the robots and the obstacles \( F_{io} \) must satisfy the inequality:

$$\begin{aligned} F_{io}= {\left\{ \begin{array}{ll}k_{io}e_{io} + c_{io}{\dot{e}}_{io} &{} e_{io}\le l_{io} \\ 0 &{} e_{io}>l_{io} \end{array}\right. }. \end{aligned}$$
(28)

The i-th robot is influenced by virtual forces from the closest neighboring robots in the swarm, from the nearest obstacles in the sensory range and from the reference point. Moreover, it is assumed that the obstacles do not limit the sensory range of the robot, i.e., the robots’ sensors are not obstructed by obstacles. Given that the virtual force \(\overline{F_{iW}}\) is the vector sum of \( \overline{F_{ij}}\),\(\overline{F_{ip}}\) and \(\overline{F_{io}}\) we can replace the projections of \(\overline{F_{iW}}\) in Eqs. (14, 16) with the corresponding projections of vectors \( \overline{F_{ij}}\),\(\overline{F_{ip}}\) and \(\overline{F_{io}}\). Therefore, the velocity values of the given robots will be calculated from the following equations:

$$\begin{aligned} v_{id}= & {} \lambda _2\int \left( F_{ip}\cos \psi _{ip}+F_{ij}\cos \psi _{ij}+F_{io}\cos \psi _{io}\right) dt, \end{aligned}$$
(29)
$$\begin{aligned} {\dot{\beta }}_{id}= & {} \lambda _1\left( F_{ip}sin\psi _{ip}+F_{ij}sin\psi _{ij}+F_{io}sin\psi _{io}\right) . \end{aligned}$$
(30)

The velocity profile of the P point is a modified trapezoidal profile expressed by the formula:

$$\begin{aligned} v_{p} = v_{pmax} \left( \dfrac{1}{1+e^{-c(t-b)}} - \dfrac{1}{1+e^{-c(t-b1)}} \right) . \end{aligned}$$
(31)

The \(v_{pmax}\) is the maximum velocity of point P (velocity in steady motion), c 1/s is the coefficient affecting the velocity change during acceleration and deceleration of point P, b s and b1 s are the coefficients describing the appropriate mean time of accelerating and braking. Time t s is the current value of the simulation/experiment time.

In order to be able to define the shape of the swarm by specifying the given inter-robot distances, it was assumed that the i-th robot is influenced by virtual forces only from the closest neighboring robots in the swarm and from the reference point.

Fig. 4
figure 4

Robot sensor range

Full size image

The range of sensors (SR) is limited to the following inequalities:

$$\begin{aligned} l_{ij}\sqrt{3}< e_{ij} < l_{ij}. \end{aligned}$$
(32)

The inequality (32) can be illustrated in the case of a swarm without obstacles and without a point of reference. The forces acting on the robots will be in equilibrium when the robots are be spaced at the given distance \( l_{ij} \) creating a mesh made of equilateral triangles. This is the case when the robot is influenced by virtual forces coming only from the closest neighboring robots (Fig. 4).

The geometric center of the swarm \(T(x_t,y_t)\) is introduced as a quality indicator of the swarm’s trajectory tracking and is expressed by the equation:

$$\begin{aligned} x_t = \frac{\sum _{i=1}^{n}x_{Ai}}{n}, \quad y_t = \frac{\sum _{i=1}^{n}y_{Ai}}{n}, \end{aligned}$$
(33)

the velocity of the swarm’s geometric center is:

$$\begin{aligned} {\dot{x}}_t = \frac{\sum _{i=1}^{n}{\dot{x}}_{Ai}}{n}, \quad {\dot{y}}_t = \frac{\sum _{i=1}^{n}{\dot{y}}_{Ai}}{n}, \end{aligned}$$
(34)

where i is the robot number and n is the total number of robots.

If the geometric center of the swarm coincides with the reference point during its movement, the trajectory tracking can be considered successful. Any deviation from this would be considered as a trajectory tracking error.

To summarize, the swarm control presented in this article is shown in Fig. 5 and can be described in the following way.

The desired trajectory of the i-th robot is realized by the tracking control algorithm. Due to the simple construction of the robot and the high computational complexity of the algorithm for generating the desired trajectory, the PD controller ("PD" block) was used as the trajectory tracking controller of the robot (Fig. 5). The controller gains were selected experimentally. The values of the proportional (\(k_p\)) and derivative (\(k_d\)) gain constants of the PD controller were selected as \( k_p = 0.113 \) and \( k_d = 0.046 \). Integrating the dynamic equation of motion (3) of a mobile wheeled robot (block "WMR"), the kinematic parameters were determined: \( \alpha _{1i}, \alpha _{2i}, {\dot{\alpha }}_{1i}\) and \( {\dot{\alpha }}_{2i} \) for the i-th robot, which on the basis of the Eq. (2) allows for calculation the velocity projections of the point \( A_i \) \({\dot{x}}_{Ai}\) and \({\dot{y}}_{Ai} \) and angular velocity \( \dot{\beta _i} \) ("WMR position determination" block). The knowledge of these kinematic parameters and initial conditions enables the determination of the coordinates of the \( A_i \) points and the orientation angles of the \( \beta _i \) frames of all robots. The values of the position of the point P, i.e., \( x_p\) and \( y_p \) are determined on the basis of the values of the velocity projections of the point P, i.e., \( {\dot{x}}_p\) and \( {\dot{y}}_p \). With the kinematic parameters of the robots and knowledge of the coordinates of the reference point, based on Eqs. (23, 24) the distances between the robots \( e_{ij} \), the robots and the reference point \( e_{ip} \) were determined. The robots determine the position \( O_{io}\) of the nearest point on obstacles within the range of their sensors by determining the end point of the shortest distance to the detected part of the obstacle. The angles between the virtual force vectors \( {\overline{F}}_{ij}\) and \( {\overline{F}}_{ip} \) and the p axis, i.e.. \( \psi _{ij}\) and \( \psi _{ip} \) and their derivatives are calculated ("distance and orientation determination" block). With knowledge of the distances, angles, and their derivatives, the controls \(u_{vi}\) and \(u_{\beta i}\) can be determined using Eqs. (29, 30) ("controls determination" block). Based on Eqs. (1) ("kinematics equation" block) and (2) ("set trajectory" block), the set trajectory for the i-th robot is determined.

Fig. 5
figure 5

Schematics of the swarm tracking control

Full size image

The used notations have the following meaning: subscript \((\star )_d \) means the set (desired) values, whereas \( \mathbf {\epsilon _i} = [\epsilon _{1i}, \epsilon _{2i}] \) is an error vector determined from the dependence \( \epsilon _{1i, 2i} = \alpha _{1id, 2id} - \alpha _{1i, 2i} \). The control signals \( u_{1i}\) and \( u_{2i} \) come from the PD controller and are determined from the relationship: \( u_{1i, 2i} = k_p \epsilon _{1i, 2i} + k_d {\dot{\epsilon }}_{1i, 2i} \). The swarm consists of n robots, where \( n \in {\mathbb {N}}_{\ne 0}. \)

The PD controller gains were selected during a test where the robot had to follow the desired trajectory. The path to follow was a straight-loop-straight curve with a trapezoidal velocity profile of the robot’s geometric center. The goal was to find the PD gains that would make the robot position difference less than 1 mm. In case of the chosen gains, the difference was 0.15 mm. The difference was calculated based on encoder measurements. It is possible to find the initial values of \(k_p\) and \(k_d\) for the linearized i-th model of the robot and its step response using the PID tuner available in Simulink. These initial values were the basis for finding the correct regulator gains during the experiment. The linearized model was assumed as a differential equation of the second-order with constant coefficients. The coefficients were chosen using Matlab System Identification. First, \(k_p\) was lowered to limit robot response oscillation and overshoot. Second, \(k_d\) was increased to further limit response oscillations if necessary. The correction had to be small to avoid numerical instability. Then, if oscillation is no longer a problem, \(k_p\) can be increased for faster robot response. This procedure can be repeated, or it can be used for different starting values of the controller gains.

3 Simulation and experiment setup

The presented swarm control method was tested numerically and experimentally. The simulation was performed using the Matlab/Simulink software, and it was verified experimentally using Motion Capture System, taking into account the behavior of the swarm of two-wheeled robots.

The simulation and experiment concern a swarm of 5 robots which, after the process of self-organization around the reference point P = 0.7,0 m, follows the moving reference point along a given trajectory in the environment with obstacles. The reference point moves along the straight line with the length of 1.4 m at the velocity \( v_{pmax} = 0.02 \) m/s with a trapezoidal profile reaching the position [\(-\) 0.7,0] m. The velocity profile coefficients are: b = 8.3 s, b1 = 78.2 s, c = 5 1/s. Obstacles are rectangles diagonally spaced apart at a distance equal to 0.7 of the diameter of the circle described on the shape of a swarm after self-organization. The robots’ task is to achieve and maintain the set distance from each other (\( l_{ij} \)) and the reference point (\( l_{ip} \)) and to avoid collisions with obstacles, knowing the limit distance to the obstacles (\( l_{io} \)).

The Motion Capture System was used to verify the simulation tests. The arena used for experimental research and testing algorithms for controlling a swarm or a group of wheeled robots consists of two systems: a vision system and a wireless communication system with robots.

Motion Capture [21] is a technology used for determination of the position and tracking the movement of markers that reflect or generate infrared light. The technology is based on a vision system consisting of more than three infrared cameras.

The experimental research presented in the paper was carried out on a research arena thanks to participation in the TerriNet (The European Robotics Research Infrastructure Network) project organized by the Federal University of Technology in Lausanne.

Fig. 6
figure 6

a Research arena, b robot with markers

Full size image

The arena (Fig. 6a) was equipped with infrared cameras enabling the determination of the position of passive markers placed on the robot (Fig. 6b) with an accuracy of 0.15 mm while maintaining 350 frames per second.

The simulation and experiment parameters are presented in Table 1, while the initial positions of points \( A_i \) and the frame orientation angles are presented in Table 2 and in Fig. 7. The coordinates of the obstacle corners are shown in Table 3.

Table 1 Assumed parameter values for the simulation and the experiment
Full size table
Table 2 Initial values of the coordinates of the \( A_i \) points and the orientation angles of the robot frames
Full size table
Table 3 The coordinates values of the obstacles corners
Full size table
Fig. 7
figure 7

The initial positions of \( A_i \) points and the orientations of the robot frames 1–5 and the position of obstacles 1 and 2

Full size image

4 Results

The task of the swarm tracking control in the environment with obstacles is aimed at the movement of the swarm in the shape of a polygon along a given trajectory, keeping its shape and avoiding the obstacles. The reference point moves in accordance with a given trajectory, e.g., along a straight line, along a loop, etc. The task of the swarm is to move in such a way that during the movement the geometric center of the swarm coincides with the moving reference point. The reference point can be understood as a common known point, relative to which the robots maintain a given distance. The values of elasticity coefficients and damping of the virtual spring-damper connections as well as the values of the \( \lambda _1\) and \(\lambda _2 \) parameters are selected experimentally.

If it is assumed that the deformations of the virtual spring are similar in nature to the error of a PD controller, an example of a method for selecting appropriate swarm control parameters can be taken from PD controller tuning methods. The procedure for selecting these parameters firstly requires setting the value of \(\lambda _2\) to 1. This parameter can be used to increase or decrease the velocity of the robots if the velocity of the swarm movement is not satisfactory. The parameter \(\lambda _1\) should be set to a value that is an order of magnitude higher than \(\lambda _2\) to prioritize direction changes toward the point \(W_i\) over the movement toward it. The spring-damper coefficient values should then be made equal, with small values below 1, to ensure stability of control. Next, in a series of simulations, the damping coefficient values should be increased to limit oscillating motion in the robots, and the spring coefficient values should be adjusted so that the deformation value is close to zero. The most challenging aspect of the tuning process is choosing the correct values of the spring-damper coefficients to prevent collisions with obstacles. This can be done by increasing the values of the obstacle virtual force \(F_{io}\) parameters or by decreasing the velocity of the robots (lower value of \(\lambda _2\)). The article does not provide a formal stability proof for the swarm control, so an experimental tuning method must be used to determine the appropriate parameters.

4.1 Simulation

In this subsection, the simulation results in form of plots are presented. First of all, the robot paths with four positions in time were presented to illustrate the movement of the swarm between obstacles in Fig. 8. In addition, the plots of the distances between the robots each other, and between the robots and obstacles are presented in Figs. 9, 10, 11 and 12.

Then, a comparison of the coordinates and paths of the geometric center of the swarm and the reference point is presented in Figs. 13a and 13b and 14. The last Fig. 15a and Fig. 15b shows the x,y projections of the swarm’s geometric center and the reference point. Those are the tracking quality indicators of the desired trajectory followed by the swarm.

Due to the introduced sensory disturbances, all plots except for Fig. 8 show the plots every twentieth sample. For the sake of clarity of the graphs, the data from 3 out of 5 robots are presented. The other two robots behaved similarly to the presented robots. Their responses were similar in magnitude and shape; therefore, they do not add value to the following analysis.

Fig. 8
figure 8

Paths of the robots with marked robots position in time

Full size image
Fig. 9
figure 9

Distances obtained between robot 1 and the other robots in the swarm

Full size image

The paths of the robots (Fig. 8) indicate that the robots are correctly avoiding obstacles. The robots did not collide with each other, even if their paths overlapped. The robots were in the same place, but at different times. On the basis of the shape of the robot paths and the plots of the distances between the robots (Fig. 9), it can be concluded that the greatest deformation of the swarm occurred between the obstacles. Despite the deformations of the swarm’s shape, the final shape took the form of a regular pentagon. The plot of the robot paths shows the change in the positions of the robots in the final shape of the swarm compared to the initial shape. The swarm’s rotation results from the action of virtual forces tangent to the swarm’s shape on the robots.

Fig. 10
figure 10

Distances obtained between robot 1 and the obstacles

Full size image

The distances between robots and obstacles show when the obstacle was in the robot’s sensor range. In the case of robot 1 (Fig. 10), obstacle 1 was in the sensor range in 28 s and left the sensor range in 40 s. This is a consequence of the robot’s position change due to virtual forces other than the virtual force from obstacle. In the case of obstacle 2, the robot crossed the limit distance, this time by 0.05 m, while from 68 s the distance from the obstacle was more than 0.2 m.

Similar graphs of the distances between the other robots and obstacles are shown in Fig. 11 for obstacle 1 and Fig. 12 for obstacle 2.

Fig. 11
figure 11

Distances obtained between the robots and obstacle 1

Full size image
Fig. 12
figure 12

Distances obtained between the robots and obstacle 2

Full size image

Figures 13a, 13b, 15a and 15b illustrate the geometrical parameters of the swarm movement.

Fig. 13
figure 13

a Comparison of the values of the x coordinates of the swarm’s geometric center and the reference point, b comparison of the values of the y coordinates of the swarm’s geometric center and the reference point

Full size image
Fig. 14
figure 14

Paths of swarm’s geometric center and the reference point

Full size image
Fig. 15
figure 15

a value comparison of the projection on the x-axis of the velocity of the swarm’s geometric center and the reference point, b value comparison of the projection on the y-axis of the velocity of the swarm’s geometric center and the reference point

Full size image

The position coordinates (Figs. 13a and b) and the geometric center velocities (Figs. 15a and b) oscillate around the set values. Due to the presence of obstacles in the swarm’s path, the swarm is deformed, which is visible on the plots of the coordinates of the geometric center. The swarm has to alter its path to avoid collision with obstacles (Fig. 14). The differences in the values of the coordinates on the x- and y-axis are close to each other and do not exceed 0.08 m. The largest differences in the values of the coordinates and the velocity of the swarm’s geometric center and the reference point are visible in [20–45 s] and [55–80 s]. During these time intervals, the greatest number of robots was subjected to repulsive forces from obstacles. This resulted in a correction of the position of the robots in relation to the reference point, and thus a change in the velocity and position of the geometric center of the swarm.

4.2 Experiment

In this subsection, the experiment results in form of plots are presented. The plots are for the same variables as in the simulation. The presented results are for robot paths (Fig. 16), the distances between robots and between robots and the obstacles (Figs. 17, 18, 19 and 20) and the comparison of the swarm geometric center to the reference point (Figs. 21a, b, 23a and 23b).

The experiment was carried out for the same values of swarm parameters as in the simulation. Due to the introduced sensory disturbances, all plots except for Fig. 16 show the plots every twentieth sample. For the sake of clarity of the graphs, the plots of 3 out of 5 robots are presented in the simulations.

Fig. 16
figure 16

Paths of robots with marked robots position in time

Full size image
Fig. 17
figure 17

Distances obtained between robot 1 and the other robots in the swarm

Full size image

Similarly to the simulation (Fig. 8), the paths of the robots in Fig. 16 show that the robots are avoiding collisions with obstacles. As expected, the biggest deformation of the swarm shape was between the obstacles. After the robots moved away from the obstacles, the swarm took shape of a regular pentagon. Due to the presence of obstacles the distances between robots (Fig. 17) differ from the set values. Only after the swarm moves out of the influence of virtual forces from obstacles, the inter-robot distances as well as the distance robot-reference point come close to the desired values. The obtained final shape of the swarm differs from the initial shape in positioning of the robots in the swarm. During the movement of the swarm in the simulation and experiment, the action of virtual forces tangent to the swarm’s shape on the robots causes the swarm to rotate. Figure 16 shows that the virtual forces from obstacle 1 changed the swarm shape from the polygon in 20th s into an arc in 30th s. After passing around obstacle 1, the swarm shape returned into the polygon shape in 60th s. Avoiding obstacle 1, robot 1 had to stop in the 20th s, causing it to lag behind the other robots. Slowing down robot 2 caused the other robots to move faster around obstacle 1 to keep up with the moving reference point. This caused the final shape of the swarm to rotate in relation to its initial shape. The robots had to change their position relative to each other to avoid colliding with obstacles.

Fig. 18
figure 18

Distances obtained between robot 1 and the obstacles

Full size image
Fig. 19
figure 19

Distances obtained between the robots and obstacle 1

Full size image
Fig. 20
figure 20

Distances obtained between the robots and obstacle 2

Full size image

The distances between robot 1 and the obstacles (Fig. 18) show that the robot moved beyond the limit distance to obstacle 2. Same behavior can be observed in case of the other robots as shown in Fig. 19 and Fig. 20. When the robot crosses the limit distance, the repulsive force from the obstacle starts acting on the robot (see Eq. 28). The robots did not collide with obstacles, which means that the virtual force \(F_{io}\) parameters are appropriate for this example.

Figures 21a, b, 23a and b illustrate the geometrical parameters of the swarm movement.

Fig. 21
figure 21

a comparison of the values of the x coordinates of the swarm’s geometric center and the reference point, b comparison of the values of the y coordinates of the swarm’s geometric center and the reference point

Full size image
Fig. 22
figure 22

Paths of the swarm’s geometric center and the reference point

Full size image
Fig. 23
figure 23

a value comparison of the projection on the x-axis of the velocity of the swarm’s geometric center and the reference point, b value comparison of the projection on the y axis of the velocity of the swarm’s geometric center and the reference point

Full size image

Similarly to the simulation, the position coordinates (Figs. 21a and b) and the geometric center velocities (Figs. 23a and b) oscillate around the set values. The plots of the coordinates of the geometric center of the swarm show that the shape of the swarm is deformed. Similarly to the simulation the swarm changes its path (Fig. 22) to avoid collision with obstacles. The difference in the values of the coordinates on the x axis does not exceed 0.015 m, while on the y axis it is less than 0.1 m. Likewise in the simulation, the greatest divergences appear when the swarm comes near the obstacles and is subjected to the virtual forces from them. This affects the value of the velocity and the geometric position of the swarm center.

The results of the simulation and the experiment show that as a result of the virtual forces coming from the obstacles, the distance differences increase to 0.08 m for the inter-robot distances. The shortest distance recorded was the distance between point \(A_i\) of robot 4 and obstacle 1 which was 0.11 m. Despite the resulting deformations of the swarm’s shape, the swarm moved between the obstacles, reaching its final position, forming a shape close to the desired one. The projections of velocity and coordinates of the geometric center and the reference point differ in shape from each other, which is caused by the deformation of the swarm’s shape.

There are differences in the shapes of the graphs from the simulation and the experiment. These differences result from slight variations in the deformations of the virtual springs that accumulate over time due to small inaccuracies in the model parameters of the swarm. When comparing the graphs of inter-robot distances from the simulation (Fig. 9) to those from the experiment (Fig. 17), it can be seen that the distances start to deviate from 5 s. These small changes in the positions of the robots lead to different values of virtual forces, which in turn cause larger differences in inter-robot distances between the simulation and the experiment. However, it is worth noting that the overall behavior of the swarm remains the same and the differences are of a similar magnitude. The swarm maintains its shape while moving between obstacles without collisions.

More experimental tests were made with the same parameters and obstacles positions in order to prove the swarm control efficiency. A slight change in the orientation or position of the robots at the start resulted in a noticeable change in the path of the robots, but without colliding with obstacles. The experiments were also carried out with different positions of obstacles. It can be noted that with the appropriate selection of parameters, collisions with obstacles can be avoided. The paper presents one example to demonstrate control algorithm at work.

5 Conclusions

The paper presents a method of virtual forces enabling simultaneous tracking control and the control of the shape of a swarm of wheeled robots. The research concerned the swarm’s travel, i.e., following a given trajectory by the swarm’s geometric center with stationary obstacles. The analysis and synthesis of the swarm trajectory tracking algorithm were presented. The work included considerations on the design and tests of the algorithm to generate a given swarm trajectory in real conditions in an environment with stationary obstacles. The proposed algorithm was tested numerically and experimentally. The obtained research results were compared and discussed.

Numerical tests and experimental studies showed that the swarm follows a given trajectory, avoiding obstacles without colliding, while deforming its shape. Based on the research, it can be said that improper selection of the values of virtual force coefficients coming from obstacles may lead to a collision of the robot with the obstacle. In addition, it was assumed that the obstacles do not obscure the robot’s field of vision, which would be the case in reality. During the action of virtual forces from obstacles on robots, the shape of the swarm is significantly deformed. This resulted in large differences in distances between the closest robots and between the robots and the reference point. Despite the deformation of the swarm shape, the shape of the swarm’s path was similar to that of the reference point, which is particularly visible when comparing the coordinates of the swarm’s geometric center and the reference point. The greatest impact of swarm shape deformation was visible when comparing the y projection of the swarm’s geometric center and the reference point.

The results of the conducted verification coincided with the simulation results sufficiently enough to conclude that the proposed swarm control algorithm enables the swarm to follow a moving reference point in an environment with stationary obstacles.