Cooperative Formation Planning and Control of Multiple Mobile Robots

rectangular workspace


Introduction
Application of intelligent Wheeled Mobile Robots (WMR) for material handling in the manufacturing environment has been the topic of research in the past decade.Even though researchers have succeeded in applying mobile robots for material handling purpose in the shop floor environment, transporting heavy objects in the assembly line is still a challenge.The dynamical characteristics of a manufacturing environment impose particular abilities a mobile robot should have if it is to operate on the shop floor efficiently, accurately and successfully.Consequently, a WMR needs to adapt itself to everlasting changes.Under such conditions, the use of multiple WMR in closed defined geometric spatial pattern/ formation can be a solution for such applications.One of the essential problems in guiding multiple mobile robots in such dynamically changing environments is to plan, navigate and coordinate the motion of robots, avoiding obstacles as well as each other while transporting the materials/objects towards the goal.Further it requires the robots to control their relative position and orientation between them on the fly.Hence the control of group of mobile robots performing such tasks requires coordination at different levels starting from navigation to formation (Sugar et al., 2001).A variety of strategies and control approaches for formation control of group of coordinated robots, have been adopted in the literature such as Graph Theory (Desai, 2002), Vector Potential Field (Yamaguchi et al., 2001), Virtual Structure (Belta & Kumar, 2004), Leader Follower (Fierro et al., 2002) and Behaviour Based Approaches (Arkin, 1998;Goldberg & Matarić, 2002).Further, a comprehensive review of robotic formation, control approaches and algorithms, applications and their advantages and disadvantages have also been addressed (Chen & Wang, 2005).Among all the approaches mentioned in the literature, behaviour -based and leader -follower approach has been widely adopted and well recognized by the researchers because of their simplicity and scalability.Even though robots are able to move in a closed defined formation when controlled using the various methods reported in the literature, the major limitation has been the difficulty to achieve a stable formation between the robots in the group in dynamically changing, unknown environments filled with obstacles.Under such circumstances, as the number of robots increases, the control methods such as the virtual structure, leader follower and graph theory fail due to their centralization and requirement of higher communication bandwidth.Further it is difficult to design and model the system in a traditional manner and necessitates the implementation of a distributed control strategy with wide communication capabilities between the group members to have knowledge about their states and actions of their teammates.Hence, the control of group of mobile robots performing such tasks requires coordination at different levels starting from navigation to formation (Sugar et al., 2001).Further, in most of the studies found in the literature, the researchers have dealt the Formation planning and Navigation problems separately, in spite of considering them as combined entity.However, as in the case of guiding the robot group in an unknown environment, in addition to formation planning, robots also need other navigational capabilities to plan their paths to reach their particular goal by avoiding collision between themselves and obstacles in the environment of interest, which have not been addressed in the literature.Another important challenge for formation control is active obstacle avoidance on follower robots, which are not studied in detail in the literature (Chaimowicz et al., 2004;Shi-cai et al., 2007).Therefore the more challenging and important problem is to combine the formation planning and active obstacle avoidance on the follower path, because the follower robot not only needs to perform obstacle avoidance but also has to control itself to remain in the desired formation in relation to the other robots in the group.In view of the limitations summarized above, three important issues related to distributed formation planning and control of multiple mobile robots namely i) distributed layered formation control framework ii) dynamic role switching algorithm and iii) real-time implementations are addressed in this chapter.Towards acheiving these goals, a new distributed methodology for the multi robot formation platoons of unicycle robots is presented in this chapter.The presented methodology combines the formation planning and navigation based on the hierarchical architecture composed of layers, whose components are the fundamental behaviours/motion states of the robots.Apart from combining formation planning and navigation, one of the most important problems and the major challenge is the avoidance of obstacles in the path of the robots designated other than the leader while guiding the robot group in an unknown environment.To address this problem, a dynamic switching of roles, based on the exchange of leadership between the robots, is incorporated in the control methodology.This is an peculiar feature that distinguishes the presented methodology from the others that exist in the literature.In the first part of the chapter, the detailed description and methodology regarding the layered approach developed in this work, for solving the multi robot formation control problem is addressed.The control problem combines together formation planning, navigation and active obstacle avoidance, when operating in an unknown environment.In the subsequent parts, the detail about the method of selection of the individual layers and behaviours of the presented approach is presented.Detailed description about how the individual task achieving layers and behaviours are formulated is also provided.The theoretical formulation of formation behaviour based on the leader referenced model and the development of the tracking controller, which is used to minimize the tracking error asymptotically zero and makes the robot in the desired tight formation, is also addressed.In addition to that, the dynamic role switching methodology through the exchange of leadership between the robots and its behaviours, adopted by the robots other than leader in the group, to actively avoid obstacles in their path is presented.Finally the contributions on the development of the presented formation control methodology and its advantages over the other methods found in the literature are summarized and concluded.

Multi robot formation control
The detailed description and method of the formation control approach is presented in this section.The main purpose of this approach is to achieve a formation control which guides a group of mobile robots in a closed defined formation relative to each other, while navigating in an unknown unstructured environment.To perform this collective task, layered distributed control architecture whose components are the fundamental behaviours/motion states of the robots, similar to (modified form of) the pack and homogeneous controller (Harry Chia et al., 2005), is developed and presented as shown in Fig . 1 (Kuppan Chetty et al., 2010, 2011).In this layered architecture, to achieve the desired objective of the formation planning and navigation in a distributed manner, the total functionality of the multi-robot system is decomposed into functional behaviours.The behaviours such as Navigation and Formation are obtained, based on the motion states of the robots, utilizing the methodology of the behavior based reactive approach, as given in (Arkin, 1998;Xiaoming Hu et al., 2003;Goldberg & Matarić, 2001).The fundamental behaviours/motion states of the robots are derived based on the advantages of the behaviour based approach and the objective of the entire system.The states are arranged into two levels, a lower level navigation and a higher supervisor level formation, which works on individual goals concurrently and asynchronously.These two levels yield the collective task upon integration.Both these layers and behaviours are related using the priority based arbitration technique, where the entire sets of behaviours are swapped in and out of execution for achieving the goals such as navigation and formation.Therefore, the robots select a particular behaviour/motion state, based on the sensory information perceived by the robot sensors from environment during the fly.A reactive controller made up of simple prepositional representation comprising of if -then -else model with task specific sensing, reasoning and planning is used to formulate the behaviours at the navigational level.This controller provides the necessary navigational capabilities and deals with the dynamic control of robots while guiding them in the environment of interest.Further, to have the theoretical formalization and the convergence of the robots into the desired formation, a closed-loop tracking controller at the supervisor level formation is realized using the kinematic model of robots employed in the group in the leader follower model.This controller handles the higher-level objective of multi robot formation.Hence, the proposed approach conquers the deficiencies of the leader follower approach and the behavior based approach, by wrapping up the former with the later, and has the advantages of both the approaches.Thus, the proposed controllers have the capability to address the combined problem of formation planning and navigation through obstacle avoidance.Referring to Fig. 1 the rectangles in the architecture represent the robot sensors, with the sensor values being transmitted to the behaviors along the long dashed lines.The behaviors themselves are drawn as rectangles with rounded corners arranged in three levels of hierarchical layers namely avoid obstacle layer, explore layer and supervisor layer.The dotted lines represent the command signals sent by the behaviors to the actuators and the inter behavior control signals.The arrowheads in the command lines indicate the priority of the behaviors, which constitutes the pathway to the actuator.The subsumption style priority based arbitration scheme is represented by '' with the actuator command coming from the upper level layer taking the precedence.The next section gives the details of the layers, their corresponding behaviors, and the functionality of each behavior in the architecture.

Layers and behaviors
This section briefs about the selection of layers and their corresponding task achieving behaviors and how these behaviors are formulated and coordinated to achieve the objective of formation maintenance and navigation tasks.

Selection of layers and behaviours
In order to have the distributed control nature, the collective task of formation planning and navigation, is divided into three primitive tasks such as Formation, Navigation and Obstacle avoidance, based on the motion states of the robot.These primitive motion states are considered as the basic fundamental behaviors of the robot and are placed in two separate hierarchical levels called the control levels, termed as the supervisor level and the navigational level in the control architecture.The navigational level is the controller's low level.This is responsible for the robot designated as the leader to safely guide the team members towards the goal, without colliding with obstacles or with other team members in the environment of interest, using the behaviors/ motion states present in this level.The supervisor level is the controller's top level, which helps the robots designated as followers to remain in the closed defined formation with their leader using the formation behavior present in this level.The selection of control levels by the robots are based on the priority number assigned to them.While considering the specific problem of navigation, the leader robot has to perform numerous motion states such as estimating the destination from current location, avoiding collisions with obstacles and other robots, and guiding the follower robots to move in the absence of obstacles to cover large areas.To do this the navigational behaviour is further decomposed into lower level behaviours such as Proceed, Avoid obstacles, Safe-wander and Pit-sensing.All these behaviours are placed in the avoid obstacle and explore layers in the navigational level as shown in Fig. 1.Further, it is necessary to retain the robots in a closed defined formation and to minimize the separation error between the robots.Hence, the formation behaviour is chosen to provide the necessary tracking control for the robots and placed in the supervisor layer in the controller top level.In order to retain the robot formation, it is necessary for the robots to have the postures, orientation and behaviour/state information of the other robots in the group.This helps the robots designated as follower to position themselves relative to the leader with the desired relative separation and orientation, while the leader maneuvers independently.Therefore, the behaviour of message passing is used to provide the explicit communication between the robots using the wireless TCP/IP protocol and placed in the supervisor level.Initially by default, safe wander behaviour is activated in the controller's leader level and others are suppressed.If one of the other behaviours becomes active, transition from the safe wander to the avoid obstacle occurs accordingly.If the robot is in the follower mode, it executes the formation behaviour which is responsible to keep the robot in a desired formation by minimizing the separation error to zero.Therefore, there are totally five behaviours which are derived based on the objective of the collective task and motion states of the robots, arranged in three layers and in two levels as shown in the Fig 1 .Even though the task of maintenance of formation has the highest priority in the approach, the obstacle avoidance priority also finds the higher order of priority based on the role of the robots in the group.The formation behavior has the highest priority in the follower robot and the obstacle avoidance is considered as the critical behavior, while in the leader robot the obstacle avoidance has the higher priority and the Pit sensing behavior is considered as the critical behavior.The next section presents the details on how these layers and the behaviours on the control architecture are formulated.

Description of layers and behaviors
The lower level navigational controller of layered formation control architecture consists of two layers such as explore and avoid obstacle layer, which are necessary to provide the navigation capability to the robots.The higher-level formation control consists of supervisor layer, necessary for high-level missions such as formation and communication.The details of the behaviours in the above mentioned layers are given below.a. Proceed: This behaviour processes the positioning data and provides approximate proceed values for the safe wandering and the obstacle avoidance behaviours.This provides the robot current position and orientation information at every instant in the two-dimensional workspace using dead reckoning principle and the kinematic configurations of the robots and makes the robot to head towards the goal.Hence this behaviour is placed under avoid obstacle layer in the architecture.b.Avoid Obstacle: This behaviour makes the robot to avoid obstacles / objects without colliding and to manoeuvre within the workspace based on the information received from its sensors as shown in where, v and  correspond to the translational and rotational velocities in mm/s and rad/s respectively.v avoid is the avoid velocity in mm/s, d obs is the distance of the obstacle with the robot, where the maximum distance is less than 5 m , d is the desired threshold distance necessary to avoid the obstacle, h is the distance between the sensor assembly from the axis of rotation of the robot. t is the angle of turn in radians and  is the angle of the obstacle with respect to the robot frame.This behaviour is placed in the avoid obstacle layer, whose functionality is to prevent the collision of robot with obstacle or with other robots and to ensure the safety of the robot.c.Pit sensing: Existence of pit in the environment is a very critical issue, which has to be avoided by robots manoeuvring the environment of interest.This behaviour makes the robot to avoid pits by controlling the motion of the robot in the backward direction to a predetermined distance, based on the sensor information.Hence this critical behaviour is placed with the highest priority in the explore layer.d.Safe-wandering: This behaviour guides the robot through the workspace/environment with piece wise constant velocity by turning left or right at regular intervals with predetermined angles as shown in Fig. 2(b).This makes the robot to wander through the environment thoroughly and to look for goals if specified.Hence this behaviour is placed under the explore layer in the architecture.e. Message passing: Message passing behaviour provides the necessary interaction between the robots allowing them to exchange their motion states, position, orientation and velocity information, using the explicit socket communication capability through wireless links (Hu et al., 1998;Crowley & Reignier, 1993).A 5 -10 Hz updation rate is provided making the inter-robot communication feasible.This helps other robots in the group to know the current task or behaviour of their teammates.This in turn helps the individual robot in the group to make suitable decisions.Hence this behaviour is assigned with highest priority and is placed on top of all the layers.f.Formation: When the followers know the trajectory or plan of the leader, this behaviour manipulates the necessary wheel velocities of the follower to maintain its position relative to the leader with desired separation and orientation through the tracking controller.This behaviour is placed in the supervisor level, since our main objective is to maintain the desired formation between the leader and the follower with the robots manoeuvring the workspace/environment.Mathematical modelling of this behavioural function is given in detail in the next section.

Mathematical modeling of formation behavior
Formation behavior is made up of mathematical formulation of tracking controller based on the kinematics of the wheeled mobile robot.This section details the formulation of such model to keep the multiple mobile robots in a defined geometric spatial pattern.In formation control problem of multiple wheeled mobile robots, the objective of the robots is to remain in the closed defined geometrical pattern with their teammates.There are several approaches discussed in the literature for formation control.One of the prominent approaches is the leader follower approach where one or more robots acts as the leader and other robots designated as followers follow them.Therefore, the major critical task is to derive a control methodology for the followers to maintain their desired linear and angular separation with their leader to remain in the defined formation topology.This section details about the methodology adopted in deriving a control strategy for the formation behavior, which plays a major role for the robots.To derive a suitable control methodology the following assumptions are made.The relative information between the robots is utilized rather than the global information systems.-Leader Referenced model is used, in which other robots maintain the desired position to the leader.

Leader referenced multi robot system
Let us consider a simple system consisting of two robots in a leader-follower framework and assume that the pose vectors of both the robots are given in the global reference frame of the cartesian coordinate representation as given by Eqn. 3. Fig. 3 shows the kinematic model of non-holonomic differential drive wheeled mobile robots arranged in such a configuration.
Here R L represents the leader robot, R F represents the follower robot and R r represent the desired position to be reached by the follower robot to remain in closed formation with the leader by keeping the leader as its reference.In the formation control of pair or robots as shown in figure 3, there are two critical parameters l and  that determine the geometric shape of the two-vehicle sub system as given by the following representation Where, i = 1, 2, 3…n denotes the robots identification number and SF i denotes the shape of the formation.
Fig. 3. Kinematic model of the robots in Leader -Follower Formation Let l and l d be the relative and desired linear separation and  and  d be the relative and the desired angular separation between the robots respectively.To achieve the desired formation the control has to make l  l d and    d and to bring the separation and orientation errors asymptotically to zero.In this case, the control problem reduces to a trajectory tracking control problem rather than the regulation problem of the follower, where it plans its path to efficiently position itself relative to its leader by observing the leader's information.Hence, a tracking controller is to be designed for the follower robots to remain in the closed formation.Therefore, the objective of the tracking controller is to find the velocities of the follower robots.

Tracking controller
In formation control, the objective of the tracking controller is to find the values of the translational and rotational wheel velocities v F and  F of the follower robots in such a way that the formation/separation errors (linear and angular) decay asymptotically to zero, and position the follower in the desired geometric pattern with its leader.
In order to formulate the tracking control algorithm to find out the wheel velocities of the follower, let the position of the leader and the follower robot in a unit time as in Fig. 3; be given by X L , Y L and X F , Y F respectively in the fixed ground coordinate system.Let X r and Y r be the position of the reference robot, which is the desired position to be reached by the follower to remain in a formation with the desired linear and angular separation with the leader.The orientation of leader and follower robots is given by  L and  F respectively, and the orientation of the reference robot  r is same as the orientation of the leader, which is the basic requirement for the formation platoons.

Generalized tracking controller in global frame
The motion for a non-holonomic differential drive wheeled mobile robot is governed by Eqn. 3, cos 0 sin 0 0 1 Where, J is the Jacobian matrix which defines the kinematics of the WMR.Here, the robot pose vectors are assumed in the global (inertial) reference frame of the cartesian coordinate representation.
Based on the above kinematic model, the velocity equations of the leader robot in the ground frame is given by cos sin Similarly, for the robot designated as follower, the velocity equations are cos sin In order to minimize the separation errors, the follower robot adjusts its position relative to its leader by estimating the leader's current posture and velocity information.However, as in the real case, the estimation of information in the global coordinates in the real time requires complex estimation methods.There exists a reference position, which is obtained based on the desired linear and angular separation relative with the leader, since the reference position cannot be estimated directly in the real world.This reference position is used as the desired position for the follower robot to be reached to remain in the formation and to minimize the separation error.Therefore the pose vector of the reference/desired position to be reached by the follower from its position relative with the leader is given by cos sin sin cos Fig. 4 shows the block diagram of the tracking controller assuming that the robots are in the global coordinate frames.

Fig. 4. Block Diagram of the Tracking Controller
The next step is to find the tracking error vector between the reference and actual position for the follower robot.This is given by the Eqn. 7. ( Separation values Tracking Controller

Actuator Control
Leader Velocity

Robot Motion
By substituting Eqn. 5 and Eqn.6 in the above Eqn.7, the tracking error becomes ( cos cos sin( )) ( sin sin cos( )) The above equations are nonlinear.In order to derive a controller, the non linear nature of the above equations are linearized by Input-Output linearization technique and by choosing

Tracking controller with mapping of coordinate frames
As in the first case, the state of the robot is described in relation to the global coordinate system.But in the real case of the robots, their coordinate system is different from the global coordinate system.In this case, X-axis represents the forward motion of the robot.Hence, to describe the robot motion in terms of component motion, it is necessary to map the motion along the axes of the global reference frame to motion along the axes of the robot local reference frame as shown in Fig. 3.The following orthogonal rotational transformation matrix is used to accomplish the mapping.
Further in the case of the Multi Robot Systems (MRS), different robots in the group as members must be able to compare measurements and coordinate their actions with in a common frame of reference.Hence, it is necessary to match the coordinate frame of robots designated as the followers relative to the leader coordinate frames, using the relationship between the inertial and relative coordinate representation of the robots.When the leader is subjected to only rotation by an angle, it is reflected in the reference robot (desired position to be reached by the follower) as a combination of translation and rotation, following a circular path with linear separation as the radius.Hence, the velocity equations of the reference robot in the ground frame is given by cos sin( ) sin cos( ) Similarly, the velocity equations of the follower robot is given by cos sin sin cos The next step in deriving the tracking controller is to obtain the error dynamics of the system in the robot frame by choosing the error coordinates x e in the direction of v and y e perpendicular to this direction as depicted in Fig. 3.4.Therefore, to obtain the error in the common local reference/ robot frame, where the coordinate system of the follower is related with leaders system, the orthogonal rotation matrix given above takes the form as where, cos sin 0 is the orthogonal rotation matrix.
Fig. 5 shows the block diagram of tracking controller in which the tracking error is represented in the new coordinate system of robot frame.
sin( ) cos( ) The next step is to find a control law for the velocity input and to position the follower in the desired position w.r.t the leader and to minimize the tracking error to zero.An obvious complexity of the above equations of the error dynamics is that the presence of the terms x  indirectly relating the output x and y with the inputs v F and  F through the state variable.Thus, in turn makes the system model to be nonlinear in nature.
In order to find the control law for the velocity inputs of the tracking controller, the Eqn.18 and 19 are linearized using suitable linearization technique.Here the idea is to algebraically transform the nonlinear system dynamics in to a fully or partly one, so that the linear control theory can be applied.Hence, Feedback linearization technique finds the best solution to linearize the above nonlinear system model especially for tracking controllers (Khalil, 1996;Shankar Shastry, 1999) where the approach involves coming up with a transformation of the nonlinear system into an equivalent linear system through a change of variables and a suitable control input.The difficulty of the tracking controller design is decreased by finding a simple and direct relation between the system output 'y' and the control input 'u = [v ] T ' by applying where, stands for the position error between the desired position i.e. position of the reference robot, and the position of the follower robot in the new coordinate system and k 1 and k 2 are controller gains that are constant positive integers greater than zero, which guarantee the system stability.

Obstacle avoidance on follower
One of the most important problems and the major challenge is the avoidance of obstacles in the path of the robots designated other than the leader while guiding the robots group in an unknown environment.To avoid this problem, a dynamic role switching methodology based on the exchange of leadership between the robots is incorporated in the developed formation control methodology.The principle behind the switching strategy is that the robot designated as the leader in the real time takes the responsibility of guiding the group through the environment by executing the navigational part of the controller and the robot designated as the follower follows the leader by executing the formation controller.The Follower only leads the group during short time periods in the fly when it has to avoid the obstacle present on its path.Therefore, at any moment during the coordination motion, the robot performing the leading role can become a follower, and any follower can take over the leadership of the team and makes the robot controller to exchange their control modes from navigation to formation and formation to navigation, based on their previous roles and sensory information through explicit inter-robot communication.
As the follower perceives the obstacle on its path based on the sensory information received from its sensors, it sends a request packet to the leader to release the leadership.When the current leader receives the request of releasing the leadership, it immediately releases the leadership to the follower, through an explicit inter-robot socket communication mechanism.Once the follower attains the leadership, it switches its role from follower to leader.Hence, the follower switches from the controllers formation mode to the navigational mode and starts navigating the environment as a temporary leader.In the other side leader robot switches its control to the formation mode and plans its path to track the temporary leader in the defined spatial pattern until the obstacle has been avoided.Under such conditions, the desired linear separation of the follower remains the same and the angular separation changes based on the geometric relationship between the robots as given by Eqn.24 and shown in Fig. 6. ; ; Fig. 7 shows three robots R 1 , R 2 and R 3 in a wedge shaped formation topology, with R 1 designated as leader and R 2 and R 3 designated as followers.When any one of the follower robots R 2 and R 3 perceives the obstacle on its path, the analogous robot attains the leadership temporarily.
Let us consider that the follower robot R 2 perceives the obstacle in it paths.Under such conditions, the Robot R 2 attains the leadership, the desired formation parameter between R 1 and R 2 is obtained as given by the Eqn. 25, and the other robot R 3 tracks the robot R 1 as its reference, without changing the formation parameters between them, which in turn tracks the temporary leader R2 using the simple geometrical relationship between the robots.Similarly, when robot R 3 attains the leadership temporarily, desired formation parameter between R 3 and R 1 is obtained as given by the Eqn.26, and the other robot R 2 tracks the robot R 1 as its reference.This methodology reduces the computational and inter-robot communication complexity, and helps to minimize the transitory errors between the robots, due to the abrupt change in the formation parameters and the switching of leadership between the robots.; ; Another important problem in the three-robot formation is, when the follower robots R 2 and R 3 perceive the obstacle in their path at the same time.In these circumstances, Robot R 1 retains the leadership by itself and commands the follower robots to change the type of formation to inline formation from their initial formation.Therefore, the required change in the desired formation parameters is obtained based on the simple geometric relationship between the robots, and are given by Eqn.27 and 28.For Robot R 2 ; ; This kind of change in formation topology is preferred to avoid the deadlock situation in the release of leadership, when more than one robot requests the leadership simultaneously.
After avoiding the obstacle, temporary leader releases the leadership back to the previous leader, starts executing the formation behaviour, and plans its path according to the leader.Hence, the dynamic switching roles/behaviours in the control architecture allow the robots to trade their roles between them and to actively avoid obstacles on the robots designated as follower's path while maintaining the desired formation.

Simulation description
The main objective of the simulation studies is to evaluate the different aspects of the control architecture, i.e. to measure the emergence of all possible reactive behaviours/AFSM of the layered approach.The response of the active behaviour yield the motor control output to the robot actuator, which determines the motion of the robot.Simulation studies are carried out in a phased manner.Before going into the study of multi Robot formation systems, simulation studies to measure the response of the reactive task achieving behaviours of the navigational controller are carried out.The relationship between the formation parameters such as the Instantaneous Centre of Radius/Curvature (ICR/ICC), linear separation (l d ) and the angular separation ( d ), and their effects on the tracking controller are investigated.Then the error dynamics of the tracking controller which are responsible for positioning the follower robots in the desired separation and orientation with the leader is tested for various formation topologies.In this simulation study, the leader robot is made to move in a predefined trajectory such as 'straight line', 'arc', 'S-shaped' and 'eight shaped' trajectories, with the generalized parameters obtained from the simulation studies.Formation strategies such as in-line, parallel and wedge shaped have been simulated for both two and three robots in the group.Finally, the dynamic switching of roles for active obstacle avoidance in the follower is incorporated along with the formation planning and navigation.Matlab -SIMULINK/ Stateflow environment as the simulation tool to evaluate the different aspects of the proposed formation control architecture, since it is an interactive graphical design and development tool that works with Simulink to model and simulate complex systems modeled as finite state machines, also called reactive event driven systems (Stormont & Chen, 2005;Dougherty et al., 2004).In this simulation model, simulation is carried out for 160 units in the time frame with two robots R1 and R2 performing the Leader -Follower formation.Leader is made to navigate the environment using the lower level navigational behaviour, with a piecewise constant wheel velocity of 100 mm/s and the Instantaneous Centre of Rotation (ICR) of 5732mm.Follower is made to track the leader with the desired separation and orientation using the supervisor level formation behaviour.Wheel velocities, wheel direction of rotation and the position and orientation information of the robots are taken as the behavioural outputs and are logged into the data loggers.Simulation parameters and the threshold values for the behavioural activation are provided in the simulation environment taking the kinematics of the robots into account.The wheel velocities obtained as behavioural outputs are constrained and bounded by the conditions v < v max < 300 mm/s and  <  max < 50°/s It is observed from Fig. 8, that for a particular type of formation topology either for a parallel, inline or collateral spatial pattern, as the value of the linear separation increases the value of the ICC/ICR also increases making it to be directly proportional in nature.It is also observed that the maximum ICR/ICC is at the collateral formation topology with the desired angular separation of (( d ) = 157° and 202°, in which the follower robots are placed in the II and IV quadrant of the cartesian coordinates w.r.t the robot frame as shown in Fig. 9.For the further investigations, the line representing the collateral formation of  d = 157° and 202° is taken as the reference based on the consideration of the length and the diameter of the real robots, which are 420mm and 345mm respectively, used in the experimental setup.With this information, in order to have the closed defined stable formation between the robots in all formation topologies, the value of the linear separation and ICC/ICR of 1000mm and 2.3m is chosen as generalized values, by considering the wheel base of the robot.

Formation control combined with dynamic switching of roles
From the several simulation studies carried out, the results obtained from the state based simulation studies, in which the dynamic switching control strategy is incorporated along with formation planning and navigation between the robots is presented.The input parameters for simulation studies are given in Table 1 Fig. 10 shows the trajectory of robots R 1 and R 2 , where the leader robot navigates the environment by switching between safe wandering, avoid obstacle, and pit sensing behaviours marked as 'S', 'O' and 'P' respectively.The dynamic switching of roles and exchange of leadership between the robots are indicated by the dotted circles in the figures.At these places R 2 encounters obstacles/pit.Hence robot R 1 acts as the follower and adjusts its wheel velocities to track its temporary leader 'R 2 ' maintaining the linear and angular separations as required.Fig. 11 shows the orientation profile of both the robots, where it is observed that both robots remain in the same orientation throughout the workspace.This  type of formation is best suited for the application of platoon of vehicles in search and surveillance and industrial transport systems.Fig. 12 (a) and (b) provides the individual behavioural/state output of the robots R 1 and R 2 for the given set of sensory information, in which 'logical 1' represents the behaviours that are active at that time instant to have control over the robot actuator.Fig. 13 shows the relative linear and angular separation between the robots being traded to meet the desired one, where  d = 270° and 90 shows the desired angular separation of the robots when R 1 and R 2 acts as the leader respectively.The dynamic motions of the follower robots to remain in the tight spatial pattern with its leader were observed from the translational and rotational velocity profiles plots obtained during simulation as shown in Fig. 14.A positive spike in the follower's wheel is due to the transitory error that persisted in the controller when a sudden reversal of the wheel velocity are involved in the non-holonomic robot systems, when pit sensing behaviour is executed to overcome a pit in both the robots.The dynamic switching of roles and exchange of leadership between the robots are marked by a dotted circle in the above figures, which can also be observed from the encircled portions of the Fig. 14, where robot R 1 acts as the follower and adjusts its wheel velocities to track its temporary leader 'R 2 ' by keeping the linear separation of 1000mm and angular separation as given by Eqn.25; to avoid the obstacle found in the path of 'R 2 '.Similarly, the formation errors in both linear and angular separation for several formation topologies are tabulated in Table 2. From the simulation studies, it is seen that the tracking error between the robots is around 1.8% and 0.44% in the linear and angular separation respectively, even though the roles and behaviours of the robots are interchanged dynamically shows the results for the simulation studies carried out to measure the error dynamics and formation convergence of the controller when the formation between the robots are switched from one topology to another.This study has been carried out to investigate the application of switching of formation topologies between the robots to avoid the deadlock situation that occurs when more than two follower robots requests for leadership.In this case the follower robots are made to change their initial formation into an inline formation with the leader, to avoid the deadlock situation encountered while taking up the leadership.Two different configurations are simulated which can be used to solve the deadlock when two or more than two robots are used in the formation framework.
In the first configuration, the formation topology between the robots is varied by changing the angular separation at a defined rate w.r.t time and the linear separation between the robots is kept constant.This is used when only two robots are in the group.When more than two robots are involved in the formation framework, the linear separation between the robots are also varied at a defined rate along with the angular separation, inorder to avoid the positioning of the robots to overlap each other.Fig. 15 (a) shows the results of the simulation study, where the leader is made to move in a straight line trajectory and the follower is made to switch from parallel line to inline formation by varying its angular separation at a continuous rate.In this simulation study, initial positions of the leader and follower robots are taken as (0,0) and (-200, -1000) and the desired linear separation between the robots is set as 800 mm.The change in the formation topology is initiated as a continuous change in the angular separation at various rates to find the optimum value that can be used in the experimental studies.It is also clearly observed that the design of the tracking controller makes the follower robot to switches its formation topology from parallel line to inline spatial pattern with the leader.Several experiments have been carried out to measure the performance of the proposed formation control approach for formation convergence and as a supplement to the investigation of the same in the simulation studies reported in the previous section.
First experiment is carried out to investigate the performance of the tracking controller and the formation errors when applied to multiple WMRs moving as a whole in a tightly coupled coordination in all formation topologies.In this experiment, the robot R 1 designated as leader is made to move in the environment in predefined trajectories such as 'arc', 'S-shaped' and 'eight shaped trajectories, with the generalized parameters obtained from the simulation studies.These trajectories are preferred, since they provide both translational and rotational profiles to the robot and the translational and rotational changes can be considered as step changes in the input to the controller as considered in the simulation studies.
Second experiment is carried out to measure the performance of the proposed formation control architecture combined with lower level navigation and supervisor level formation.In this experiment, the initial and desired separation values are taken as 1414 mm, 262 and 1000 mm, 270 respectively and both the robots are employed with the layered control architecture.The leader is made to navigate an environment of 12m by 10m rectangular workspace filled with rectangular obstacles of size 200 x 180 mm and 550 x 400 mm.Leader velocities are fixed at 160 mm/s and 4/s, obstacle distance of 800 mm and safe wander time of 6 s, and the follower is made to follow the leader with the desired separation and orientation as mentioned earlier.
Experimental study on dynamic switching of roles is carried out as the third experiment, to test the performance of the approach on combined formation and navigation capability of multi-robot systems, to have active obstacle avoidance in the follower path.Experiment is conducted in a similar environment as used in the second experiment with the robots moving in a parallel line formation.The initial and desired separation values are taken as 1019 mm, 258 and 800mm, 270 between the robots.In this experiment, both the robots are employed with the same control configuration, having the capability to perform both navigation and formation and they are allowed to trade their roles between themselves using the leadership exchange method.These experiments have been carried out in a workspace of 12 m by 10 m with three obstacles of size 200 x 180 mm, 200 x 180 mm and 550 x 400 mm as shown in Fig. 17.Finally, experiments on switching of formation topology between the robots are carried out to measure the capability of the proposed approach to avoid from the deadlock situation when three robots are used in the formation study.The idea is to measure the adaptability of the proposed approach and convergence of the tracking controller, when the robots designated as followers encounter obstacle on their path simultaneously.In this case the follower robots are made to change their initial formation into an inline formation with the leader, to avoid the deadlock situation encountered while taking up the leadership.This experiment is conducted with two robots instead of three robots due to the limited availability of research platforms, and the robots designated as leader is made to navigate a straight line in the workspace and the follower is made to switch from the parallel line to the in-line formation to measure the formation convergence.In this experiment, the initial and desired separation between the robots are taken as 0, 0 and -1044 mm, 264 and 800 mm, 180 respectively and are given by the following relationship.The change in formation is initiated with constant linear separation and varying the angular separation at the rate of 3/s.The results of all the above experiments are reported and discussed in detail in the following sub section.

Experimental results
Fig. 18 to Fig. 21 shows the results of the experiments illustrating the good performance of the control algorithm (Kuppan Chetty et al., 2010).For the first experiment, Fig. 18 (a) shows the trajectory of the robots in the parallel line formation, where the leader robot navigates the environment by switching between safe wandering, avoid obstacle behaviours marked as 'S', 'O' respectively whenever it perceives the obstacle information from the environment.Moreover the performance of the formation controller is also observed from figure 18, where follower tracks the behaviour of the leader form its initial linear and angular separation and remains in the desired formation relative to the leader throughout the workspace.It also shows that, the formation controller minimizes the separation error to zero, keeping the robots in the tight coupled formation in the environment filled with obstacles.Fig. 18 (b) shows the orientation plot of the above experiments, where it can be observed that the tracking controller makes the follower to remain in the same orientation with the leader throughout the fly, making the system suitable for transporting objects from one location to the goal in the dynamic industrial environments.Fig. 19 and 20 shows the experimental results on dynamic switching of roles, carried out to test the obstacle avoidance in the follower path.Fig. 19(a) shows the trajectory of leader and follower robots performing the combined task of navigation, formation and obstacle avoidance where 'O' represents the obstacles in the path of the robots and the dotted rectangles represents the switching of roles and leadership between the robots to reach the desired goal.Fig. 19 (b) shows the behavioural / state output of the robots R1 and R 2 involved in the experiments.Initially, robot R 1 is designated as the leader and leads R 2 which follows the motion of the leader in the parallel line formation with the desired separation of 800 mm and 270.At t = 8.4 and 26s, follower finds the obstacle on its path, request for exchange of leadership with the leader, acquires the leadership and performs obstacle avoidance on its path.After avoiding the obstacle, robot R 2 relinquishes the leadership back to the leader at t=14 and 30s.This can be clearly observed from Fig. 19 (b), where the role switching behaviour takes precedence which intern activates obstacle avoidance behaviour present in the control architecture of robot R 2 .This clearly indicates that at any moment during the coordinated motion, the follower robot, which experiences obstacle on its path, can take over the leadership from the leader and the robot performing the lead role can become a follower by switching their leadership between themselves.Fig. 20 shows the variation in the linear and angular separation errors, where the robot R 2 performs obstacle avoidance on its path by switching its roles with R 1 and preserves the formation by adjusting its angular separation as given by Eqn.25.It is also observed; that the formation controller takes 9s to settle in the desired formation with the leader and the formation errors is estimated to be less than 1.4% and 0.5% in both linear and angular separation between the robots.It is observed from the results, that the tracking controller takes less than 12 s to settle in the desired linear separation of 800mm between the robots from their initial position and it takes another 40s to switch the formation topology between the robots which includes the 30s time taken for the rate of change of desired angular separation.This method of changing the formation topology is one of the remedy to avoid the deadlock caused when one or more robots in the group request for leadership simultaneously, to avoid the obstacle on their path and the group leader is free to move in the environment.

Conclusion
A combined layered formation control approach to conquer the deficiencies of the leader follower approach and the behavior based approach, by wrapping up the former with the later is developed.The developed approach has the advantages of both the approaches, which have the capability to address the combined problem of formation planning and navigation through obstacle avoidance.In order to have the distributed control nature collective task of formation planning and navigation is divided into three major (primitive) tasks such as Formation, Navigation and Obstacle avoidance based on the motion states of the robot.These primitive motion states are considered as the basic fundamental behaviors of the robot according to the reactive behavior based approach and are placed in three separate layers, which yields the collective task upon integration.Further, to have the theoretical formalization and the convergence of the robots into the desired formation, and the closed loop tracking controller based on the leader follower model and the kinematics of the unicycle mobile robots is formulated to keep the robots in the team in a closed defined spatial pattern, where the separation errors are ,minimized asymptotically to zero.A unique feature of the developed formation control approach is the incorporation of dynamic role switching methodology through the exchange of leadership between the robots and their behaviours, to actively avoid the obstacle in the path of the robots designated other than the leader in the group.The results show the capability to avoid the obstacle in the follower's path.This clearly indicates the capability of the approach to make the robots to remain in a closed defined stable formation between them even though the roles are switched dynamically during the fly.Under these circumstances, the tracking controller takes less than 12s to make the follower to remain in the formation with the leader and the formation errors are less than 2.81% and 0.82% in both linear and angular separation between the robots respectively.As a whole the formation error for the projected formation control approach combined with formation planning, navigation and obstacle avoidance capability between the robots, the formation errors are estimated at less than ±3% and ±0.81% in linear and angular separation between the robots; when compared to ±5.5% and ±3.6% reported in the literature (Fierro et. al., 2002 andLIU Shi-Cai et. al., 2007).
In future, we plan to test the capability of the developed control methodology by conducting the experiments carrying a real object in the real dynamic industrial environment filled with obstacles.The switching control strategy between the behaviours and robots arises many interesting questions such as deadlocks between the robots when they exchange the leadership.There are many cases to be solved to answer this deadlock phenomenon.We also plan to extend into the issues to overcome the deadlock situation in the multi robot systems through simulation and experimentation studies.

Fig. 1 .
Fig. 1.Layered formation control architecture with task achieving behaviours classified into supervisor level formation and lower level navigation Fig 2 (a).When the robot sensors finds an obstacle/object within the workspace enclosed by the semicircle 'd', the avoid obstacle behaviour is activated and it manipulates the wheel velocities of the robot, as given by Eqn. 1 (a) and (b) necessary to avoid the obstacle.
Fig. 2. (a) Emergence of Obstacle Avoidance behaviour -A top view look of robot and obstacle, (b) Safewander Behavior

Fig. 5 .
Fig. 5. Block diagram of tracking controller in the robot frame after the feedback linearization the control law for v F and  F is obtained as

Fig. 6 .
Fig. 6.Role of angular separation while role switching when (a) R1 as leader and R2 as follower, (b) R2 as leader and R1 as follower

Fig. 8
shows the simulation results that are obtained to address the generalized relationship between the formation parameters such as the linear separation (l d ), angular separation ( d ) and the Instantaneous center of curvature ICC/ICR.The critical value of the ICC/ICR needed for the robots to remain in the closed defined formation is obtained for various values of linear separation (l d ) starting from 500mm to 3000 mm, in several formation topologies starting from parallel line to inline formation, using the eqn.21.The various values of angular separation ( d ) represent the type of formation topology employed between the leader and follower robots as shown in Fig.9.The desired angular separation values of 90° and 270° represents the parallel line, 180° represents the inline and the values between 91° and 179° & 181° and 269° represents the collateral line spatial pattern between the robots.

Fig. 10 .
Fig. 10.Trajectory of two robots in parallel line formation.

Fig. 11 .
Fig. 11.Orientation profiles of robots in parallel line formation

Fig. 14 .
Fig. 14.Velocity profiles of robots with role switching

Fig. 15 .
Fig. 15.Switching of Formation topology with variations in angular separation (a) trajectory of robots switching from Parallel line (b) Separation plot of robots

Fig. 16 .
Fig. 16.Experimental Robot Research PlatformsThe individual task achieving behaviours are programmed as parallel motion functions, and they are integrated as the functional classes/library files in the application interfaces of the mobile robots.An explicit wireless socket communication mechanism is developed by using the 'Net packet' functional library classes of the mobile robots, to provide the necessary inter-robot communication and to initiate the exchange of leadership between the robots.A trapezoidal acceleration function with the acceleration and deceleration value of 50% is used as the velocity function.During startup, the robots are considered to be in the origin position of 0, 0, 0 in X, Y and .The initial velocities of the robots are considered to be zero.Further, the Leader robot is made to initiate its motion, only if the connection between the follower robots is established by the wireless socket communication protocol.Hence, when two robots are employed in the formation framework, leader robot is powered first and then the followers are started up.Several experiments have been carried out to measure the performance of the proposed formation control approach for formation convergence and as a supplement to the investigation of the same in the simulation studies reported in the previous section.First experiment is carried out to investigate the performance of the tracking controller and the formation errors when applied to multiple WMRs moving as a whole in a tightly coupled coordination in all formation topologies.In this experiment, the robot R 1 designated as leader is made to move in the environment in predefined trajectories such as 'arc', 'S-shaped' and 'eight shaped trajectories, with the generalized parameters obtained from the simulation studies.These trajectories are preferred, since they provide both translational and rotational profiles to the robot and the translational and rotational changes can be considered as step changes in the input to the controller as considered in the simulation studies.Second experiment is carried out to measure the performance of the proposed formation control architecture combined with lower level navigation and supervisor level formation.In this experiment, the initial and desired separation values are taken as 1414 mm, 262 and 1000 mm, 270 respectively and both the robots are employed with the layered control architecture.The leader is made to navigate an environment of 12m by 10m rectangular workspace filled with rectangular obstacles of size 200 x 180 mm and 550 x 400 mm.Leader velocities are fixed at 160 mm/s and 4/s, obstacle distance of 800 mm and safe wander time of 6 s, and the follower is made to follow the leader with the desired separation and orientation as mentioned earlier.

Fig. 17 .
Fig. 17.Arrangement of robots and obstacles in the 2D workspace Figs. 21 (a)  and (b) show a comparison between the simulation and experimental results indicating the behaviour of the tracking controller to position the follower robots even after switching of formation topology from parallel line to inline spatial pattern.The angular separation ( d ) is changed at a rate of 3°/s, which is the critical parameter to initiate the formation change.

Fig. 18 .
Fig. 18.Performance of Formation Controller combined with Navigation and Formation behaviors (a) Trajectory of robots in parallel line formation (b) Orientation profile between the robots Fig. 21.(a) Trajectory of robots switching from parallel to straight line formation (b) Formation plot of robots switching from parallel to straight line.
-Robots employed in the group are identical in their kinematic model, and have same set of sensors, actuators and control strategies.-Tire differences, model uncertainties, odometry errors by slide and skid are ignored.Robots employed are assumed as perfect velocity controlled robot without considering the dynamics.Hence, the kinematic model is used for development of multi robot leader follower formation framework.-Robots are aware of each other through explicit inter robot communication with onboard sensing, computation and communication capability.- Li et al., 2004dinate representation as mentioned in (XiaohaiLi et al., 2004) is utilized to develop the control algorithm.- By substituting Eqn. 14 and Eqn. 15 in the above Eqn.16, the tracking error in the new coordinate system is obtained as,