Leader-follower formation control of nonholonomic mobile robots subject to robots failure

Abstract

Over the years, control of autonomous vehicles in a defined formation has been the subject of much research. Albeit leader-follower approach being one of the most used in formation control, it suffers a major practical drawback of leader failure while cruising in formation. In this work, we aim to solve this problem by proposing a novel assignment algorithm that assigns a new leader from the follower robots to ensure robots complete their given task when their leader fails. This algorithm also assign role to new robots joining the group, as well as the failed robot when rescued back to the team. We drive robots towards their desired trajectories to achieve formation using a Lyapunov-based time-varying state tracking controller from the literature. Due to role switching amongst member robots, we propose a new variant of the limit-cycle obstacle avoidance algorithm to ensure smooth and collision free transition. Simulations and experiments are performed using the robot operating system framework due to its flexibility to verify the effectiveness and reliability of the proposed algorithms.

Keywords

Mobile robots trajectory tracking multi-robot coordination formation control

Introduction

Control of multi-robot systems has attracted much research in recent years owing to its potential for solving problems that are difficult or nearly impossible for a single robot. Formation control is one of the common areas where multi-robot systems are involved. In this scenario, two or more robots move to a desired posture to attain formation or move in a defined shape to a desired destination.^1–3

Depending on the sensing capabilities and the interaction topology of the robots, a variety of formation control problems have been studied in the literature. Some of these problems include flocking, estimation-based control, pure distance-based control, angle-based control, containment control, and cyclic pursuit, among other formation control strategies.⁴ Depending on whether or not the desired formation is time varying, formation control problems are generally divided into two; namely, formation generating problems and formation tracking problems. In their review, Oh et al.⁴ described how the existing literature has addressed each of these problems. Formation generating problem refers to the situation in which the collective objective of the agents is to achieve a prescribed desired formation shape. Formation tracking problem, on the other hand, refers to the case in which the agents are controlled to track some prescribed reference trajectory. Both these problems have been addressed in the literature using a variety of tools, including matrix theory based approach, Lyapunov based approach, graph rigidity approach, potential fields approach, receding horizon, among other approaches.⁵

Formation tracking problems have been used in multiple applications and have, therefore, received a great deal of attention. Beard et al.⁶ and Scharf et al.⁷ classified the formation tracking problems into leader-follower, behavioral-based, and virtual structure approaches. The leader-follower approach has gained more attention due to its simplicity, reliability, and no need for global knowledge.⁸ In this strategy, a leader robot is selected to determine the motion of the whole group and serves as a reference for the other follower robots. Each follower robot maintains a desired relative position with respect to its leader. When the motion of the leader is known, desired trajectories for the follower robots are generated by the leader relative to its posture. A tracking controller is designed for each follower robot to achieve formation. Therefore, in some sense, the leader-follower formation control problem can be regarded as a natural extension of the traditional trajectory tracking problem.⁸

The major practical drawback of leader-follower strategy is leader failure.³ Indeed, since the leader-follower consists of following the leader, leader failure results in failure of the whole group to maintain formation or move to the desired destination. To the best of our knowledge, very few works explicitly address this problem, despite the abundance of literature on the subject. According to Besseghieur et al.,⁸ leader failure is addressed by dynamically changing the leadership of the team manually form a dedicated workstation, and switching to a linear formation to avoid obstacles. However, total failure of the leader is not considered, and the possibility of collision between robots during role transition is another problem.

In this work, our main objective is to present a novel approach that will enable follower robots to overcome the problem of leader failure in a practical setting. The main contributions of this article are as follows:

We propose an algorithm that assigns a new leader from the follower robots in case of leader failure. The algorithm enables other follower robots adjust their roles in the team to follow the new leader, and assign a new role to the failed leader when we are able to launch it back to the team.

To ensure robots do not collide with one another or with a static obstacle during role transition, we proposed a new approach to the limit-cycle obstacle avoidance algorithm by Adouane,⁹ Mouad et al.¹⁰ to enable the use of this avoidance strategy in trajectory tracking applications.

We consider the case where a leader is periodically changes by adding a layer of automation on the work by Besseghieur et al.⁸

We simulated our proposed solutions and conduct a set of experiments on a group of low-cost e-puck2 robots using the ROS framework to validate the effectiveness of our approach.

The rest of this article is organized as follows: We begin by presenting an overview of the formation control problem, including the trajectory tracking problem and its conversion to a formation control problem. Next, we introduced a Lyapunov-based motion controller from the literature designed to drive robots, and an avoidance controller to guarantee collision-free motion. Following this, we present our proposed algorithms, including the avoidance algorithm as well as an assignment algorithm to address the leader failure problem. We then report on the simulations conducted using GAZEBO robotics simulator. Experimental results using a group of four e-puck2 robots are presented to verify the effectiveness of our algorithms. Finally, we draw conclusions and offer considerations for future work.

Related work

Several control strategies have been used to solve leader-follower formation control problem. Chen et al.¹¹ employed a receding-horizon control scheme with a leader-follower formation control strategy to solve the formation problem of multiple nonholonomic mobile robots with a rapid error convergence rate. The authors address formation problems when robots move backwards by explicitly controlling the orientation deviation between leaders and followers. In their approach, however, the orientation of the leader is not taken care of as the leader is assumed to move in a straight line while generating the reference trajectory. As a consequence, formation may be lost when the angular velocity of the leader becomes non-zero.

The work by Hoshikawa et al.¹² uses a receding-horizon leader-follower formation control strategy to automatically determine an optimal formation among predefined formation according to the environment. Haga et al.² later extended this work by proposing an automatic formation control method with temporal logic constraints. The approach leads to an optimal formation switching in a cluttered or sufficiently small environment among predetermined formations in real time using receding-horizon.

A Lyapunov-based approach is used by Besseghieur et al.⁸ in a leader-follower strategy to solve the formation control problem by converting it to a trajectory tracking problem. The authors tackle the problem by feeding trajectory information to the leader, and the leader has the task of defining a desired pose for each follower robot at some point relative to its pose. Each robot in the group tracks its desired trajectory using a Lyapunov-based backstepping controller.

Chen et al.¹¹ considered the same problem, however in this reference, the nonholonomic constraint of the robot is respected, and the generated trajectories for the follower robots are feasible. The work by Stipanovič et al.¹³ also uses a Lyapunov-based approach in a leader-follower strategy to design a distributed formation control law that guarantees collision avoidance between agents with a bounded tracking error outside of the avoidance region.

According to Rout and Subudhi,¹⁴ multiple autonomous underwater vehicles (AUVs) are controlled using a Lyapunov-based controller to achieve formation by utilizing the leader-follower formation control strategy in the presence of discrete data transmission between the leader AUV and the follower AUVs. A continuous-discrete extended Kalman filter (CD-EKF) is designed for follower agents to estimate the position and velocity of the leader when communication constraints occur.

Despite the wealth of research on control strategies for leader-follower formation control, none directly address the challenge of leader failure. These studies predominantly focus on optimizing formations and ensuring collision avoidance. However, they do not explicitly tackle the scenario where the leader becomes incapacitated. This gap underscores the need for future research to develop robust control strategies capable of seamlessly transitioning and adapting in the event of leader failure.

Problem definition

In this section, we present the kinematics of a two-wheeled robot, an overview of trajectory tracking problem by JIANGdagger and Nijmeijer¹⁵ and how to use trajectory tracking to tackle the formation control problem as outlined by Besseghieur et al.⁸ We then highlight the obstacle avoidance strategy and how Adouane,⁹ Mouad et al.,¹⁰ Benzerrouk et al.¹⁶ and Kim and Kim¹⁷ used the limit-cycle approach to ensure robots avoid collision with static obstacles or other robots.

Robot kinematics

Consider a nonholonomic constrained mobile robot in Figure 1 whose kinematics are modeled by the nonlinear ordinary differential equation (1).

[\begin{matrix} \dot{x} \\ \dot{y} \\ \dot{θ} \end{matrix}] = [\begin{matrix} \cos θ & 0 \\ \sin θ & 0 \\ 0 & 1 \end{matrix}] [\begin{matrix} v \\ ω \end{matrix}]

Figure 1.

Robot schematic diagram.

$x \in R$ and $y \in R$ are the Cartesian coordinates of the center of mass of the vehicle, $θ \in [0, 2 π)$ is the heading of the robot with respect to the coordinate axis, $v$ and $ω$ are the control inputs for linear velocity and angular velocity, respectively.

Trajectory tracking

Consider a tracking control problem where a robot is required to track a reference robot with a posture $P_{r} = (x_{r}, y_{r}, θ_{r})$ and reference velocities $v_{r}$ and $ω_{r}$ . Given robot’s posture $(x, y, θ)$ , the error coordinates can be described by the following equation:¹⁸

[\begin{matrix} e_{x} \\ e_{y} \\ e_{θ} \end{matrix}] = [\begin{matrix} \cos θ & \sin θ & 0 \\ - \sin θ & \cos θ & 0 \\ 0 & 0 & 1 \end{matrix}] [\begin{matrix} x_{r} - x \\ y_{r} - y \\ θ_{r} - θ \end{matrix}]

The error dynamics is defined by the following equation¹⁸:

[\begin{matrix} {\dot{e}}_{x} \\ {\dot{e}}_{y} \\ {\dot{e}}_{θ} \end{matrix}] = [\begin{matrix} v_{r} \cos e_{θ} + ω e_{y} - v \\ v_{r} \sin e_{θ} - ω e_{x} \\ ω_{r} - ω \end{matrix}]

A Lyapunov stable time-varying state-tracking control law is developed by JIANGdagger and Nijmeijer¹⁵ based on the backstepping technique to control the robot to track a reference trajectory. According to JIANGdagger and Nijmeijer,¹⁵ if

{\dot{v}}_{r}, {\dot{ω}}_{r}, v_{r}

, and

ω_{r}

are bounded with the assumption that either

v_{r}

ω_{r}

does not converge to zero, the global asymptotic stability of the error dynamics in equation (3) is guaranteed, and thus the error converges to zero using the control inputs defined by equation (4) (see JIANGdagger and Nijmeijer¹⁵ for proof of convergence).

[\begin{matrix} v \\ ω \end{matrix}] = [\begin{matrix} v_{r} \cos e_{θ} + k_{x} e_{x} \\ ω_{r} + \frac{k_{y} e_{y} \sin e_{θ}}{e_{θ}} + k_{x} e_{θ} \end{matrix}]

with

a = \sqrt{v_{r}^{2} + b ω_{r}^{2}}

k_{x} = 2 ϵ a

k_{y} = b | v_{r} |

ϵ > 0

and

b > 0

Formation control

Leader-follower formation control problem can be viewed as a natural extension of trajectory tracking problem. Besseghieur et al.⁸ and Mastellone et al.¹⁹ formulated the formation control problem as a trajectory tracking problem by defining a desired posture $P_{d} = (x_{d}, y_{d}, θ_{d})$ using parameters $l$ and $ϕ$ as desired distance and bearing, respectively, relative to the posture of a leader robot or center of mass of the desired formation. This formulation is defined by equation (5) given below (see Besseghieur et al.⁸ and Mastellone et al.¹⁹):

\begin{aligned} x_{d} & = x_{l} + l \cos (ϕ + θ_{l}) \\ y_{d} & = y_{l} + l \sin (ϕ + θ_{l}) \\ θ_{d} & = atan2 ({\dot{y}}_{d}, {\dot{x}}_{d}) + k π, k = 0, 1 \end{aligned}

where

k = 0, 1

defines the desired drive direction (0 for forward and 1 for reverse motion) and atan2 is the four-quadrant inverse tangent function. As outlined by Besseghieur et al.,⁸ it is possible for a nonholonomic constrained mobile robot to track a reference trajectory if equation (5) respects the nonholonomic constraint of the robot, meaning it should be consistent with the following form:

[\begin{matrix} {\dot{x}}_{d} \\ {\dot{y}}_{d} \\ {\dot{θ}}_{d} \end{matrix}] = [\begin{matrix} \cos θ_{d} & 0 \\ \sin θ_{d} & 0 \\ 0 & 1 \end{matrix}] [\begin{matrix} v_{d} \\ ω_{d} \end{matrix}]

Therefore, Besseghieur et al.⁸ choosed the desired orientation as in equation (5) for the desired trajectory to satisfy (6).

Given the posture of the follower robot $P_{f} = (x_{f}, y_{f}, θ_{f})$ and that of the generated desired posture $P_{d} = (x_{d}, y_{d}, θ_{d})$ , formation is achieved if each follower robot tracks its corresponding desired trajectory generated by the leader robot. Mathematically, formation is achieved if $lim_{t \to \infty} (x_{d} - x_{f}) = 0$ and $lim_{t \to \infty} (y_{d} - y_{f}) = 0$ , the condition $lim_{t \to \infty} (θ_{d} - θ_{f}) = 0$ is desirable but not necessary, however, must be bounded. The desired velocities $v_{d}$ and $ω_{d}$ are derived from equation (6):

\begin{aligned} v_{d} & = \pm \sqrt{{\dot{x}}_{d}^{2} + {\dot{y}}_{d}^{2}} \\ ω_{d} & = {\dot{θ}}_{d} = \frac{{\dot{x}}_{d} {\ddot{y}}_{d} + {\dot{y}}_{d} {\ddot{x}}_{d}}{{\dot{x}}_{d}^{2} + {\dot{y}}_{d}^{2}} \end{aligned}

The sign

\pm

depends on whether the motion is forward or backward. Note that the necessary conditions for reference path design according to JIANGdagger and Nijmeijer¹⁵ is a nonzero desired linear velocity

(v_{d} \neq 0)

and a smooth twice differentiable path. Similar to equation (4), the control inputs in equation (8) will drive the follower robots to the desired position

P_{d}

to achieve desired formation.

[\begin{matrix} v_{f} \\ ω_{f} \end{matrix}] = [\begin{matrix} v_{d} \cos e_{θ} + k_{x} e_{x} \\ ω_{d} + \frac{k_{y} e_{y} \sin e_{θ}}{e_{θ}} + k_{x} e_{θ} \end{matrix}]

with

k_{x} = 2 ϵ a

k_{y} = b | v_{d} |

a = \sqrt{ω_{d}^{2} + b v_{d}^{2}}

ϵ > 0

and

b > 0

Obstacle avoidance

We want robots to avoid static obstacles along their respective trajectories and avoid collision with other robots when switching roles in case of failure of leader failure. Several avoidance strategies can be found in the literature. An interesting overview of some of these strategies is given by Minguez et al.²⁰ Mastellone et al.¹⁹ and Barogh et al.¹ addressed the avoidance control using an avoidance function defined by Stipanovič et al.,¹³ and Skowronski and Leitmann²¹ which is active only in the bounded sensing regions of each individual robot and do not interfere with the robot’s individual optimal control law outside of this region.

In limit-cycle avoidance strategy, a robot needs to follow accurately the limit-cycle vector fields defined by the following set of differential equations:

\begin{aligned} {\dot{x}}_{s} & = (s i g n) y_{s} + x_{s} (R_{c}^{2} - x_{s}^{2} - y_{s}^{2}) \\ {\dot{y}}_{s} & = - (s i g n) x_{s} + y_{s} (R_{c}^{2} - x_{s}^{2} - y_{s}^{2}) \end{aligned}

Here

(x_{s}, y_{s})

is the position of the robot according to the center of convergence of the limit-cycle.

R_{c}

defines the radius of the limit-cycle, and “

s i g n = \pm 1

” defines the direction of rotation for clockwise and counter-clockwise avoidance, respectively.

The control law used by Adouane⁹ to follow these trajectories is orientation control. The robot’s desired orientation is given by equation (10) below:

θ_{d} = atan 2 ({\dot{y}}_{s}, {\dot{x}}_{s})

The orientation error is given by (11) below:

θ_{e} = θ_{d} - θ

It is proved by Adouane⁹ that with

k > 0

, the input

ω = {\dot{θ}}_{d} + k θ_{e}

will move a robot to the desired orientation, and the nominal speed of the robot

v

is constant when this controller is active.

Control architecture

In this section, two algorithms are presented: our proposed limit-cycle obstacle avoidance algorithm that determines which obstacles are to be avoided and whether an obstacle is completely avoided or not, and an assignment algorithm that tackles the problem of robot failure by assigning an appropriate role to each robot in the team.

Obstacle avoidance algorithm

We use a reactive avoidance approach when an obstacle that can obstruct a robot’s motion is detected. While the reactive approach by Huang et al.²² and Zhang et al.²³ activates the avoidance controller only when the robot is close to an obstacle, the approach introduced by Adouane,⁹ Mouad et al.,¹⁰ Benzerrouk et al.,¹⁶ and Kim and Kim¹⁷ activates the avoidance controller as soon existence of an obstacle that can obstruct the motion of a robot is detected, thus reducing the amount of time needed to reach the target. In contrast to previous approaches, where the disturbing obstacle is assumed to be on a path between a robot and the target position, our work is on a trajectory tracking problem where a near zero tracking error is expected. Hence, robot and goal positions are approximately the same.

Our proposed approach to obstacle avoidance is different from the avoidance algorithm by Adouane,⁹ and Benzerrouk et al.¹⁶ in order to suit our trajectory tracking problem. Inspired by Adouane,⁹ we define a region of influence of radius $R_{i}$ defined by equation (13) for each obstacle encountered, and activate the avoidance controller as soon as a robot enters such region.

To avoid an obstacle it is necessary to:

Detect obstacle to avoid.

Define an escape criterion which determines if an obstacle is completely avoided or not.

We define

D_{j}

, the Euclidean distance between robot and obstacle

j

. Choose obstacle with the least distance to the robot, this obstacle is characterized by its position

(x_{o}, y_{o})

and radius

R_{o}

. Given position

(x, y)

of the robot, the obstacle’s position

(x_{o}, y_{o})

is the center of a limit-cycle. Expressing the robot’s position

(x_{s}, y_{s})

with respect to the center of convergence of the limit-cycle, we have

x_{s} = (x - x_{o})

and

y_{s} = (y - y_{o})

D_{j}

is defined by equation (12), the radius of the region of influence

R_{i}

and that of the limit-cycle

R_{c}

are defined by equations (13) and (14), respectively:

D_{j} = \sqrt{x_{s}^{2} + y_{s}^{2}}

R_{i} = R_{o} + R_{r} + δ

R_{c} = R_{i} - ε, ε < δ

R_{r}

is the radius of the robot,

δ

is a safe margin for collision avoidance, and

ε

a relatively small positive number. Because we are dealing with dynamic obstacles (namely, other robots), the sign in equation (9) is set to

- 1

(counterclockwise avoidance). A robot approaching an obstacle from any direction has coordinate positions

(x_{s}, y_{s})

that can have either positive or negative values (see Figures 2 and 3). We use this framework to define an escape criterion by keeping track of the signs of

x_{s}

and

y_{s}

just after the robot enters region of influence of an obstacle and during avoidance.

Figure 2.

Phase portrait of the limit-cycle defined by equation (9).

Figure 3.

Definition of robot’s position relative to an obstacle.

We define $s g n_{x s}$ and $s g n_{y s}$ as the signs of $x_{s}$ and $y_{s}$ , respectively, just after the robot enters the circle of influence of the obstacle, and let ${n_s g n}_{x s}$ and ${n_s g n}_{y s}$ be the signs of $x_{s}$ and $y_{s}$ , respectively, when the robot is avoiding the obstacle. We compare $(s g n_{x s}, s g n_{y s})$ and $({n_s g n}_{x s}, {n_s g n}_{y s})$ and say that an obstacle is completely avoided and switch to tracking controller when the conditions $s g n_{x s} \neq {n_s g n}_{x s}$ and $s g n_{y s} \neq {n_s g n}_{y s}$ are satisfied. The pseudo code for our obstacle avoidance algorithm is as follows:

Algorithm 1: Obstacle avoidance.

Remark 1

The transition time depends on the predefined value of the constant linear velocity in the avoidance controller, as well as the radius $R_{c}$ in equation (14). It is interesting to notice that based on our defined escape criterion, the trajectory length $T_{l}$ of the robot along the limit-cycle is bounded by the following:

\frac{1}{2} π R_{c} < T_{l} < π R_{c}

This is true because coordinate sign changes are only possible when a robot moves from one quadrant to another (see Figure 3). This implies that for the escape criterion to be met, the robot must travel at least one-fourth of the circumference of the limit-cycle and no more than half of the circumference.

Assignment algorithm

Leader failure is the major drawback practical of the leader-follower formation control strategy. According to Besseghieur et al.,⁸ this problem is addressed by dynamically changing the leader, choosing a new leader from the follower robots through a dedicated workstation. Formation parameters are also (re)defined through this workstation. This seems like a good idea; however, it does not explicitly solve the problem because the newly selected leader could fail as well. The approach also adds the burden of having a dedicated supervisor at the workstation to manually select a new leader. In this work, we propose a new approach to solve this problem. Our approach consists of an algorithm that assigns a role to each robot in the team. We rank roles serially from $0$ to $(n - 1)$ where “ $n$ ” is the number of robots in the team. Rank “0” is the highest, defined as the leadership-role, while all other roles are follower-role with $(n - 1)$ having the least rank. Figure 4 shows the role positions in a square-shaped formation with robots joining the team at different time instances. It also shows how we ranked the roles serially from $0$ to $(n - 1)$ . Roles are occupied sequentially by robots joining the team. Each robot joining the team with its default role set to “None” takes the role with the highest rank from the vacant roles. This means the first robot to join will take the leadership role, and the last to join takes the role with the least rank. In case of a leader failure, the algorithm let active robots in the team change their roles to that with a rank one step higher, meaning that a robot with a previous role of “one” will take the leadership role “zero,” and all other robots adjust their roles one step higher to follow the new leader, leaving the least role vacant for another robot joining the team to occupy. With this, the problem of leader failure is eliminated, as the algorithm will let one of the followers to take leadership automatically, as well, let other followers adjust their roles to follow the new leader. Additionally, any robot in the team could fail without affecting the formation, the active robots only need to autonomously adjust their roles if a role ranked higher than their current role become vacant. This also mean that if we can recover the failed robot, we can easily launch it back to the team to occupy the vacant role.

Figure 4.

Role positions in a square-shaped formation with robots occupying positions sequentially.

The pseudocode for this algorithm is as follows:

Algorithm 2: Assignment algorithm.

Remark 2

The leading vacant role is the role with the highest rank from the list of vacant roles. The algorithm assumes that robots can easily know the state of their team members, that is, the role a robot member is occupying and whether it is active or not. Its implementation is possible because of the available communication resources.

Implementation

Robots need to communicate data with other robots in the group to achieve formation, avoid obstacles, or perform any given task as a group. As such, a reliable system is required for robots to interact with one another. One of such system is the robot operating system (ROS), a framework for writing robot software. ROS is a collection of tools, libraries, and conventions that aim to simplify the task of creating complex and robust robot behavior across a wide variety of robotic platforms.²⁴ ROS provides functionality for hardware abstraction, communication between processes over multiple machines, and great tools for visualization. It also provides the flexibility to work with heterogeneous devices in a shared environment. This makes ROS the ideal framework to implement our algorithms. We carefully select packages that meet our requirements for implementation.

Simulations

In this section, simulations are presented to verify the effectiveness of our algorithms. To run our simulations, we developed a simulation platform using GAZEBO, an open-source 3D robotics simulator on ROS Melodic, running on Ubuntu 18.04. What follows describes the simulation results for our proposed obstacle avoidance algorithm, periodic leadership switching, and assignment algorithm.

Simulation 1: Obstacle avoidance

Consider a nonholonomic constrained mobile robot on a ground plane whose initial position is chosen at random relatively close to the trajectory. The goal is to track a given reference trajectory and simultaneously avoid obstacles that might be present along. We set the control parameters for the tracking controller to be $ϵ = 0.9$ and $b = 2$ , while the parameter for the avoidance controller is set for $k = 1.8$ . To test our avoidance algorithm, we run two sets of experiments: In the first simulation, we used three obstacles at different random positions exactly along the referenced trajectory. The second simulation has three obstacles, the first of which is exactly along the trajectory, while the other two close enough to the trajectory such that the trajectory passes through their region of influence.

Simulation results in Figure 5(a) show our robot leaving its referenced trajectory each time a disturbing obstacle is detected by switching to avoidance controller, and then switching back to tracking controller when escape criterion is satisfied. Figure 5(b) shows how our proposed approach responds to obstacles that are not exactly along the trajectory. It can be observed from the plot how the robot is taking a slightly longer time for tracking errors to converge to zero for disturbing obstacles off the trajectory.

Figure 5.

Simulation results for the proposed limit-cycle-based obstacle avoidance algorithm. (a) Trajectory of the robot avoiding three different obstacles exactly on the reference trajectory. (b) Trajectory of the robot avoiding the first obstacle exactly on the reference trajectory, with the second and third obstacles near the reference trajectory close enough to obstruct the motion of the robot.

Simulation 2: Formation control with periodic leader switch

As discussed in the previous sections, leader-follower formation control strategy is easy to implement, but suffers a major drawback of leader failure, resulting in followers’ inability to complete the given task. One way to solve this problem is by periodically changing the leader, this is different from what Besseghieur et al.⁸ presented in the sense that new leader selection is automatic, all robots are controlled using a single workstation, and as well, our leader robot is controlled by tracking and avoidance controllers. At each transition, the new leader breaks out of the formation to continue with the trajectory tracking, and of course, avoids collision with the former leader present along the trajectory. Other followers and the former leader then take new positions with respect to the new leader and move in a formation. To illustrate this approach, we run two simulations with four and three robots respectively tracking first a linear and second a circular trajectory.

Figure 6(a) shows the result of our simulation using four robots tracking a linear trajectory. The plot shows the robots’ trajectories while in a formation and during leadership transition. The plot of the formation shape confirms the convergence of the robots to the desired formation anytime a new leader takes charge of the team. The case is similar in the second simulation as shown in Figure 6(b) with robots moving in a triangular-shaped formation.

Figure 6.

Simulation results for the leader-follower formation control with periodic leader switch. (a) Trajectories of four robots in a rhombus shaped formation tracking a referenced linear trajectory and the formation shape when robots 0, 1, 2, and 3 are leading the team, respectively, in the first, second, third, and fourth periods. (b) Trajectories of three robots in a triangular shaped formation tracking a circular trajectory showing leadership transition stages and the formation shape after complete leadership transition.

Simulation 3: Assignment algorithm

Consider a group of four nonholonomic constrained mobile robots on a ground plane. For each of the robots in the group, we write a distinct control script which runs on the robot. When a robot is leading the group, its control script gets trajectory information and control the robot to track it, and as well, publish formation information. Follower robots have no access to the reference trajectory, rather, they get formation information from the leader. During the simulation, each robot knows its pose and that of other robots in the group with respect to the origin ( $0, 0$ ) of the ground plane, through the odometry readings of each of the robot. To demonstrate the effectiveness of our proposed algorithm, we make the robots fail by forcefully killing their node since robots in a simulation won’t fail naturally. Other robots in the group will know a robot has failed when its node is not in the list of active nodes, thus, adjust their roles to maintain formation and keep with the trajectory tracking.

We launch the robots sequentially to track the reference trajectory in a formation. Robot 0 being the first to be launched takes the leadership of the group while other robots take positions in the formation according to their respective roles. Figure 7(a) depicts the robots’ trajectories tracking the circle in a rhombus shape formation. In this case, none of the robots failed, and we observed the robots’ trajectories, their positions in the formation, and the formation shape for a complete cycle.

Figure 7.

Simulation results for a group of four robots in a rhombus shaped formation subjected to robots failure. Active and failed robots are represented by circular-blue and diamond-green markers respectively. (a) Trajectories and formation shape of robots without robots failure with robot 0 leading the group. (b) Robots’ trajectories and formation shape when robots 2, 1, and 0, respectively, fail at different time instances. (c) Robots’ trajectories and formation shape when robot 1 fails and then launched back to occupy the vacant role.

We launch the robots again in the same sequence and consider a worst case scenario of failure of multiple robots within the group. We kill robots 2, 1, and 0, respectively, at different time intervals. As shown in Figure 7(b), robots 0 and 1 maintained their roles when robot 2 died, while robot 3 leaves its role “3” to occupy the vacant role “2.” Similarly, robot 3 moves to occupy role 1 when robot 1 died, and finally become the leader when we kill robot 0. This shows that unless if each of the robots in the group have died, there is always at least one robot that will take the leadership of the team to complete the task.

We killed robot 1 as shown in Figure 7(c) to demonstrate the case of robot recovery. Robots 2 and 3 adjust their roles to follow the leader (observe the formation shape with just three robots) after robot 1 had died (diamond-green marker). After a relaunch of the failed robot, we can observe that it is now back in the team occupying the least ranked role “3.”

Experiment

Being that uncertainties are marginally handled in simulations, we run experiments using low-cost robots to demonstrate our avoidance algorithm, periodic leader switch, and assignment algorithm working on real robots.

Experimental setup

In our experiments, we use four small, low-cost, differential drive, ROS-enabled robots called e-puck2. Since the robot has a short laser reading range of just about 8cm, it cannot be used for simultaneous localization and mapping (SLAM) in a large environment. Therefore we use a ZED Stereo camera to provide the pose estimate of each of the four robots. We mount a unique AR Tag on each of the robots to be detected by the camera using the $a r_t r a c k_a l v a r$ package. We use $z e d_a r_t r a c k_a l v a r$ driver to get pose estimates of the AR tags from the camera. We select the C++ Wi-Fi version of the ROS e-puck2 driver to get sensor readings from the robots as well as rostopics for the control script to publish commands on actuators. Figure 8 depicts the whole setup consisting of a Dell G7 workstation with Ubuntu 18.04 and ROS Melodic installed on it, a ZED Stereo camera mounted at about 2 m above the ground, four e-puck2 robots each with a unique AR tag mounted on top, and a Dlink router that connects the robots and the workstation via wireless communication. Throughout the experiment, the parameters for the tracking controller are set for $ϵ = 0.9$ and $b = 10$ , while the parameter for the avoidance controller is set for $k = 2.2$ . We use circular trajectory throughout the experiment due to the limited view of the camera in the setup.

Figure 8.

A snapshot of our workspace.

Experiment 1: Trajectory tracking with obstacle avoidance

To demonstrate obstacle avoidance, we use two e-puck2 robots 0 and 1, with robot 0 tracking a circular trajectory and robot 1 static at a random position along the trajectory. As shown in Figure 9, robot 0 was able to detect and avoid robot 1 located at approximately $(- 0.18, - 0.36)$ along the trajectory, then switched to tracking after avoidance is complete (see Table 1 for experiment videos). Figure 10(a) shows robot 0 deviating from the referenced trajectory to avoid robot 1, and then coming back to the referenced trajectory after the avoidance is complete. Figure 10(b) represents the tracking errors and input velocities from the controllers.

Figure 9.

Snapshot of the obstacle avoidance experiment showing: (a) robot 0 just before the region of influence of robot 1, (b) Robot 0 inside the region of influence of robot 1 and avoiding it, (c) robot 0 back to trajectory tracking after avoiding robot 1 completely.

Figure 10.

Experimental results for our proposed obstacle avoidance algorithm showing: (a) reference trajectory and motion trajectory for robot 0 and (b) robot 0 tracking errors and controller input velocities.

Table 1.

Links to experiment videos.

S/N	Experiment	Video link
1	Obstacle avoidance	https://drive.google.com/file/d/1f0KRSVM-HIKb9D1kjLxLPo8IjKhoXlfj/view?usp=sharingLink 1
2	Periodic leader switch	https://drive.google.com/file/d/1-x6r5Apx0n_B_cIFQtNFQf2y0vZHo_n4/view?usp=sharingLink 2
3	Robots failure	https://drive.google.com/file/d/1f795M6AXa9SXY9mS4xDIgz84EEdxNQ0G/view?usp=sharingLink 3

Experiment 2: Formation control with periodic leader switch

For this experiment, we task four e-puck2 robots to track a circular trajectory of radius 0.4 m with three of the robots following the leader, and then automatically switch leader each time a circle is complete. We implement it such that robots move in a square formation of size 0.15 m, with follower robots following the leader at bearings of 180 $\circ$ , 225 $\circ$ , and 270 $\circ$ . After completing a circle, the robot at a bearing of 270 $\circ$ from the leader becomes the new leader, follower robots at bearings 180 $\circ$ and 225 $\circ$ follow the new leader at bearings 225 $\circ$ , and 270 $\circ$ , respectively, while the former leader follow at 180 $\circ$ bearing. The video of this experiment (link in Table 1) shows how the robots track a circular trajectory four times, with each of the robots leading the team once for a complete circle.

Experiment 3: Assignment algorithm

We run this experiment to show how our algorithm works using real robots. The case presented here is the case of leader failure in a formation. Consider four e-puck2 robots each with a unique AR tag tracking a circular trajectory in a square formation. Figure 11(a) shows the group tracking a circular trajectory in the counter-clockwise direction with the encircled robot leading the team. We forced a leader failure in Figure 11(b) by killing its control node. After the leader failure, the robot on role 1 takes the vacant role 0, while robots on roles 2 and 3 now following the new leader on roles 1 and 2, respectively. We assume to have recovered the failed robot and launched it back to the group. Figure 11(c) shows the robot back in the group right behind the new leader occupying role 3.

Figure 11.

Experimental results showing four robots in a formation with: (a) encircled robot leading the team before failure; (b) failed leader robot out of the formation with two followers forming a triangular shape with the new leader; and (c) failed robot recovered and joined the team as a follower occupying the least ranked role to complete the formation.

Similar to the results in our simulation, the algorithm enables follower robots to overcome leader failure, and the failed robot launched back to the team takes highest ranked available role not its former role as the leader. We have considered other cases of failure, that is, failure of follower robots in the team. The video for our experiments can be found in the link provided in Table 1.

Conclusions

In this article, we study the leader-follower formation control strategy to address its practical drawback of leader failure. We proposed a novel assignment algorithm that ensure follower robots overcome the practical problem of leader failure. To ensure our robots do not collide with one another during leader switch, we propose a new limit-cycle obstacle avoidance algorithm that can be applied in trajectory tracking problems. We use trajectory tracking controller and avoidance controller from the literature, to drive robots to track a reference trajectory in a defined formation shape. A set of simulations and experiments were carried out using ROS framework and low-cost e-puck2 robots. We have shown results that verify the effectiveness of our proposed algorithms. To address the limitations of our proposed avoidance algorithm, future studies will be conducted that consider avoiding not only failed robots, but also cooperatively avoiding unexpected obstacles while cruising in a formation. Another promising direction is improving our assignment algorithm to consider more parameters in selecting new leader. These parameters can include robot’s position in a formation and sensing capabilities when dealing with heterogeneous robots.

Footnotes

Media links

Declaration of conflicting interests

The author(s) declared the following potential conflicts of interest with respect to the research, authorship, and/or publication of this article: The authors declare the following financial interests/personal relationships which may be considered as potential competing interests: Horacio J Marquez reports financial support was provided by Natural Science and Engineering Research Council of Canada.

Funding

The author(s) disclosed receipt of the following financial support for the research, authorship, and/or publication of this article: This work was funded by the Natural Science and Engineering Research Council of Canada.

ORCID iD

Yusuf Abdul-razaq

References

Barogh

Rosero

Werner

. Formation control of non-holonomic agents with collision avoidance. In: 2015 American control conference (ACC) 1 July 2015, pp.757–762. IEEE.

Haga

Zanma

Hoshikawa

et al. Optimal automatic formation control for two-wheeled vehicles using model predictive control with temporal logic constraints. In: 2018 12th France-Japan and 10th Europe-Asia Congress on Mechatronics 10 September 2018, pp.161–164. IEEE.

Guanghua

Deyi

Wenyan

et al. Study on formation control of multi-robot systems. In: 2013 Third international conference on intelligent system design and engineering applications 16 January 2013, pp.1335–1339. IEEE.

Park

Ahn

. A survey of multi-agent formation control. Automatica 2015; 53: 424–440.

Ren

Cao

. Distributed coordination of multi-agent networks: emergent problems, models, and issues. Springer Science & Business Media, 2010.

Beard

Lawton

Hadaegh

. A coordination architecture for spacecraft formation control. IEEE Trans Control Syst Technol 2000; 9: 777–790.

Scharf

Hadaegh

Ploen

. A survey of spacecraft formation flying guidance and control. Part II: control. In: Proceedings of the 2004 American control conference 30 June 2004, pp.2976–2985. IEEE.

Besseghieur

Trebinski

Kaczmarek

et al. Leader-follower formation control for a group of ROS-enabled mobile robots. In: 2019 6th international conference on control, decision and information technologies (CoDIT) 23 April 2019, pp.1556–1561. IEEE.

Adouane

. Orbital obstacle avoidance algorithm for reliable and on-line mobile robot navigation. In: 9th Conference on autonomous robot systems and competitions 2009, Castelo-Branco, Portugal.

10.

Mouad

Adouane

Khadraoui

et al. Mobile robot navigation and obstacles avoidance based on planning and re-planning algorithm. IFAC Proc Vol 2012; 45: 622–628.

11.

Chen

Sun

Yang

et al. Leader-follower formation control of multiple non-holonomic mobile robots incorporating a receding-horizon scheme. Int J Robot Res 2010; 29: 727–747.

12.

Hoshikawa

Zanma

Ogura

et al. Optimal formation control of two-wheeled vehicles using model predictive control. In: 2015 10th Asian control conference (ASCC) 31 May 2015, pp.1–6. IEEE.

13.

Stipanovič

Hokayem

Spong

et al. Cooperative avoidance control for multiagent systems. J Dyn Sys Meas Control 2007; 129: 699–707.

14.

Rout

Subudhi

. A backstepping approach for the formation control of multiple autonomous underwater vehicles using a leader–follower strategy. J Marine Engg Technol 2016; 15: 38–46.

15.

JIANGdagger

Nijmeijer

. Tracking control of mobile robots: A case study in backstepping. Automatica 1997; 33: 1393–1399.

16.

Benzerrouk

Adouane

Martinet

. Dynamic obstacle avoidance strategies using limit cycle for the navigation of multi-robot system. In: 2012 IEEE/RSJ IROS’12, 4th workshop on planning, perception and navigation for intelligent vehicles 2012, Vilamoura, Algarve, Portugal.

17.

Kim

. A real-time limit-cycle navigation method for fast mobile robots and its application to robot soccer. Robot Auton Syst 2003; 42: 17–30.

18.

Kanayama

Kimura

Miyazaki

et al. A stable tracking control method for an autonomous mobile robot. In: Proceedings under IEEE international conference on Robotics and automation 13 May 1990, pp.384–389. IEEE.

19.

Mastellone

Stipanovič

Graunke

et al. Formation control and collision avoidance for multi-agent non-holonomic systems: Theory and experiments. Int J Robot Res 2008; 27: 107–126.

20.

Minguez

Lamiraux

Laumond

. Motion planning and obstacle avoidance. Springer Handbook Robot 2016; 1177–1202.

21.

Skowronski

Leitmann

. Avoidance control. J Optim Theory Appl 1977; 23: 581–591.

22.

Huang

Fajen

Fink

et al. Visual navigation and obstacle avoidance using a steering potential function. Robot Auton Syst 2006; 54: 288–299.

23.

Zhang

Liu

Yang

. A hybrid robot navigation approach based on partial planning and emotion-based behavior coordination. In: 2006 IEEE/RSJ international conference on intelligent Robots and systems 9 October 2006, pp.1183–1188. IEEE.

24.

Quigley

Conley

Gerkey

et al. ROS: an open-source Robot Operating System. In: ICRA workshop on open source software 2009.