Virtual spring damper method for nonholonomic robotic swarm self-organization and leader following

In this paper, we demonstrate a method for self-organization and leader following of nonholonomic robotic swarm based on spring damper mesh. By self-organization of swarm robots we mean the emergence of order in a swarm as the result of interactions among the single robots. In other words the self-organization of swarm robots mimics some natural behavior of social animals like ants among others. The dynamics of two-wheel robot is derived, and a relation between virtual forces and robot control inputs is defined in order to establish stable swarm formation. Two cases of swarm control are analyzed. In the first case the swarm cohesion is achieved by virtual spring damper mesh connecting nearest neighboring robots without designated leader. In the second case we introduce a swarm leader interacting with nearest and second neighbors allowing the swarm to follow the leader. The paper ends with numeric simulation for performance evaluation of the proposed control method.


Introduction
Recently, swarm robotics includes the both robots design and the developments in the controls of their behavior.Here we consider a swarm consisting of two-wheel robots and propose some algorithms for their selforganization.By self-organization of swarm robots we mean the emergence of order in a robotic system as the result of interactions among the single robots constituting the swarm.In other words this self-organization mimics that one which we often observe in biology, and in particular in animal societies, not limited to social insects like bees, ants or termites, see, e.g., [1][2][3][4][5].For example, swarms of submarine robots which mimics some fish behavior were analyzed in [6,7], where in particular were described how the configuration of the swarm can be calculated from the individual element, in order to achieve the desired shape of the swarm.The objective of swarm control algorithm is to achieve desired swarm behavior by introducing the proper local interaction between swarm members.A collective behavior emerges from all combined decisions made by robots according to relatively simple rules.As a decision-making input, robots take into consideration their relative position with respect to the local neighbors and to the environment that they can sense and/or interact with.Besides, other requirements are swarm scalability and robustness that have to be ensured in designing swarm control algorithms.Available methods of swarm control that met all the listed requirements are either bioinspired, based on physics phenomena, or various optimization approaches.Similar works using virtual spring-based approach have been released; for example, Urcola et al. [8] present a formation control based on virtual structure composed of virtual spring dampers connecting following robots to the leader.Shucker and Bennett [9] propose a fully distributed robotic macrosensor in which control algorithm was influenced by spring damper mesh connectivity.Robotic swarm interconnected by virtual spring and damper connected in parallel was proposed by Chen et al. [10] where formation control of satellites under a gravitational potential field is investigated.Belkacem and Foudil [11] study a swarm aggregation model based on equilibrium emerging from robots interconnected with virtual springs and dampers.A slightly different approach can be found in the works [12,13] where a centroid-based interaction is analyzed; in particular, these works are focused on local and nonlocal interactions; in [14], a mathematical proof of the cohesion of a monodimensional swarm is given.
The study of discrete systems as those analyzed is particularly interesting not only to evaluate the behavior of robotic swarms, but can also be used for other purposes: In fact, considering the single constituent of the swarm as a material particle that can interact with neighboring particles with suitable potentials, the discrete approach can be seen as a possible numerical implementation tool for the study of deformable continua which may also show nonlocal interaction effects (see, e.g., for possible fields of application [15][16][17][18][19][20][21][22]).The presented approach to robotic swarm is rather similar to the modeling of a discrete system by a continuum and vice versa which has very long traditions in the continuum mechanics.
In our approach, the swarm control algorithm is similar in the behavior of point mass particles interconnected with spring dampers.Such swarm mimics the behavior of viscoelastic fluid which falls into the category of physics-based method.For movement control of nonholonomic robot, we define the relation between virtual net forces acting on the robots and their linear and angular velocity.The paper is organized as follows.In Sect. 2 we introduce the ordinary differential equations describing the motion of the two-wheel robots.In Sect. 3 we propose new swarm control methods based on spring damper mesh.The results of the model are discussed in Sect. 4 where we considered the motion of 21 two-wheel robots with and without swarm leader.It is shown that the technique based on the control of one robot called swarm leader with properly introduced inter-robot forces may lead to the formation of relative stable ordered structure of the swarm.Moreover controlling the behavior of the swarm leader we can almost get a priory desired configuration of the whole swarm.In addition we also consider the swarm leader technique in the case of avoiding of obstacles.

Dynamics of robot swarms
The most commonly used robots in swarm research are nonholonomic two-wheel robots.The robot is shown in Fig. 1.
To describe the dynamics of a two-wheel robot, we could use Euler-Lagrange's equations with multipliers.Unfortunately, one of the drawbacks is the necessity of decoupling the multipliers from robot torques.To avoid having to decouple multipliers we will use different formalism using Maggie's equations [23,24], derived from the principle of virtual work.The equations are given by where s is the number of independent parameters in the generalized coordinates q j equal to the numbers of the system degrees of freedom.The generalized velocities and the right-hand sides of Eq. ( 1) can be expressed explicitly by where ėi , i = 1, . ..s, are called kinetic parameters and in this case they are equal to α1 , α2 , i.e., the rotation angles of the two wheels.The generalized coordinates and generalized velocities for two-wheel robot are where β is the angle between the line perpendicular to the segment joining the wheel centers and the x-axis.
For the described robot, we have two degrees of freedom and the independent generalized coordinates are the wheels rotation angles α 1 , α 2 .Using kinematics analysis, we determine the velocities relationships as follows where r is the radius of the wheels and l half the distance between the wheel centers.The total kinetic energy E is equal to the sum of the kinetic energies of both wheels and robot chassis: Explicitly, we have: where m w , m r , I zw , I xw , I zr are, respectively, the masses and the moments of inertia of wheels and robot chassis with respect to the x-and z-axes.Using Eqs. ( 2) and ( 5) we can calculate the coefficients C i j , G j , and with Eqs. ( 1) and (3) we determine the generalized forces Q j in the following form The generalized forces and coefficients C i j , G j in (8) and Eq. ( 1) yield the following the Maggie equations If we write Eq. ( 9) in matrix form, we get where M is the inertia matrix, F (q) is the friction function, and U is the wheels torque vector.
The parameters a 1 , . . ., a 5 are calculated using the relations where N 1 , N 2 are the loads applied to wheels 1 and 2, while f 1 , f 2 are the coefficients of rolling friction of both wheels.Robot control inputs are wheels angular acceleration q = [ä 1 , ä2 ] T and wheels angular velocities q = [ȧ 1 , ȧ2 ] T given by the equations 3 Swarm control method Swarm members are linked together with springs and dampers connected in parallel, see Fig. 2. We assume the robots have mass m and are connected via springs with stiffness k and dampers with damping coefficient c.The length of the springs at rest is equal to d 0 .
Virtual springs and dampers are connected in the centers of mass of each robot, depicted in what follows as point A.
Every link between the robots exerts a force F i j equal to the sum of elastic spring and viscous damper forces such as discussed in [8][9][10][11].We assume that the spring and damper are linear and their forces are proportional, respectively, to the displacement and relative velocity between spring ends.
Fig. 2 Spring dampers mesh connecting mobile robots where k stands for the spring stiffness coefficient, c is the damping coefficient, and d i j is the current length of the spring connecting the robots iand j.
Each robot has limited sensing range S R r which determines how many spring damper links are present; the number of links is, in this way, equal to the number of visible neighboring robots.To establish links with only the nearest neighbors, the sensing range has to meet the inequality For example, setting the sensing range at the value 1.2d 0 results in creating links between only nearest neighbors forming the swarm in the equilateral triangle formation shown in Fig. 3. Let us note that the sensing range is quite similar to the notion of horizon used in the peridynamics, see [25], or the interaction range in the lattice dynamics [26].The nearest neighbors do not remain the same all the time, in general.The total force exerted on a robot i is equal to the sum of all link forces between robot i and its visible neighbors N r From Eq. ( 13) we conclude that we require values of linear velocity v A and angular velocity β, to specify robot control inputs, having the net force F i as known value.To determine the values of v A , β we first have to find the corresponding forces causing the robot to move forward or backward (force F y ) and rotate around the point A (force F x ).These forces are shown in Fig. 4. The origin of the vectors F y , F x is in the point D. If we know the values of link forces and their orientation, we can find the x and y components of the total force F i given by The vectors F y , F x are the components of the vector F i j in a rotating frame of reference around the point A by an angle ϕ i = β i − π 2 .The addition of -π 2 brings F y (instead of vector F x ) in the direction of the velocity vector v A .The values F y , F x are calculated using the following equation Using the above equations the values of the linear velocity v A and the angular velocity β are determined by   where λ is the gain constant.The gain constant λ allows one to control the robots' rotation speed around the point A, aligning the robot frame with the direction of the total force F i acting on the robot.Moreover, the damping coefficient c and stiffness coefficient k can be set by the designer so that the values of v A and β are adequate for the task at hand.

Simulation experiments
In order to evaluate the performance of the presented control method, two simulations were performed, that is without and with a leader.The robots were initially randomly distributed with random headings.In both simulations 21 robots were used.The parameters and the control algorithm constants for both simulations are listed in Table 1.

Self-organization without swarm leader
In the first case the swarm self-organizes with no leader, forming equilateral triangle formation, the results are shown in Fig. 5.
In Fig. 6 it is seen some oscillations during first seconds in the linear and angular velocities of the robots which vanish in finite time due to the presence of friction and limited time response of robot actuators.In the first simulation, the formation reached by the swarm is imperfect (disordered), and this may suggest a change of control gains to different values.
Forces from virtual springs and dampers acting on the robots also oscillated around zero or in some cases reached zero.For the second simulation in case of robot no. 5 marked in Fig. 5 the velocities and forces acting on the robot oscillate around zero, whereas heading has nonzero value (Fig. 6).Oscillations at the first 2 s are present due to the termination of links with robots going out of range.This process creates sudden change of acting force value as well as its heading, which results in change of angular and linear velocity of the robot.With correctly chosen values of k, c, λ the swarm reaches the desired formation.As we mention before there are some similarities between swarm robots and other discrete models, such as lattices [26].Let us also underline the similarities with dynamic fracture of discrete lattices, where the breakage of a link leads to the local oscillations or a wave propagation for infinite systems, see, e.g., Slepyan et al. [27][28][29][30].

Self-organization with swarm leader
In the second case one of the robots is designated to be the swarm leader, interacting with nearest and second neighbors.Swarm cohesion is achieved due to two types of interactions.Inter-agent interaction as in previous example and additional virtual forces acted by the leader on its first and second neighbors.In calculating leader-follower forces the same values of k and c are used as given in Table 1.The length of the unstressed spring between leader and its nearest neighbors is equal to d 0 , while the one between the leader and its second neighbors is equal to 1.5d 0 .
Followers position themselves around the leader and are shown in Fig. 7.The diameter of the swarm is between 2d 0 and d 0 .In this case it is equal to 1.57 m.
With robots organized around the leader the swarm will follow the leader trajectory.Example of leader following is shown in Fig. 8.
During the swarm movement, the swarm maintains its cohesion.The swarm moves in the same heading as the leader.The individual robots are rotating around the leader to the left, which is the same direction as the first turn of the swarm.The followers continue to rotate with constant speed until the next turn of the swarm to the right which slows the rotation of the robots.If friction is present, the system of robots would eventually stop rotating around its leader.
The swarm trajectory can be represented as trajectory of the swarm center of mass.The swarm trajectory is calculated only for the followers and compared to the trajectory of the leader.
The difference between position of the swarm leader and center of mass is smaller than 4 [cm] in X and Y directions.The calculated error could be used as a design parameter for choosing correct values of k, c, λ and d 0 for swarm leader following.

Obstacle and collision avoidance
Obstacle and collision avoidance can be achieved using the same approach as for maintaining the cohesion of the swarm.If a robot moves near its neighbor closer than allowed minimal distance d min , a spring damper link is established between the robots creating a repulsive force, moving the robots apart.The same happens in Fig. 6 Simulation results for robot no. 5 case robots driving near obstacles.One of the simplest forms of such forces for i-th robot the force is given by where k = k ob , d min = d minob and for obstacles and k = k co , d min = d minco to prevent collisions, and d is the distance between i-th robot and the nearest neighbor or the obstacle.The link robot-obstacle is perpendicular to the obstacle and created between the robot and the closest point on the obstacle.Simulations for swarm obstacle avoidance were performed using the parameters given in Table 2.
Here k r , k l , k ob , k co are the spring constants, respectively, for neighboring robots, leader, obstacles and robots collision avoidance, d 0 is the resting spring length for springs between robots, and d minob , d minco are the minimal allowed distances from obstacles and between robots.In this paper, we have presented a method for self-organization of nonholonomic robotic swarm using virtual spring damper mesh.Choosing appropriate values of spring stiffness, damping coefficient and angular velocity constant, the swarm is able to achieve equilateral triangle formation.Moreover, if we introduce a swarm leader, with correct design parameters, the swarm follows the leader almost keeping the established formation.Numerical simulations show that for the detailed nonholonomic vehicle dynamics it is possible to design a self-organizing swarm control algorithm based on restoring force of spring with damping.It is of interest in future works to address to problem of selecting the optimal values of control gains to achieve better algorithm performance including the determination of the level of nonlocality that is determination of the sensing range.Moreover, future works should include the development of an extended version of the presented method with obstacle avoidance and swarm shape formation control.

Fig. 3
Fig. 3 Sensing range in nearest neighbors link connection

Fig. 5
Fig. 5 Swarm self-organization for first set of parameters a robots trajectories, b final robot positions