Leader-Follower Formation Control of Wheeled Mobile Robots without Attitude Measurements

: The problem of leader-follower formation of a platoon of differential-drive wheeled mobile robots without using attitude measurements is addressed in this paper. Contrary to the position-distance approaches existing in the literature, the formation and collision avoidance is achieved by introducing a state-dependent delay in the desired trajectory. The delay is obtained as the output of a dynamical system and its magnitude will decrease/increase depending on the distance between the robots. To guarantee trajectory tracking and to overcome the lack of orientation measurements, an output feedback control and attitude observer are proposed based on the kinematic model of the robots. The attitude observer is designed directly on the special orthogonal group SO ( 2 ) and it can be used in open-loop schemes. The proposed control-observer scheme ensures asymptotic convergence of the tracking and observer errors. Finally, experimental results are presented to show the performance of the proposed approach.


Introduction
In recent years, autonomous mobile vehicles have attracted interest from the scientific community, mainly due to the wide range of applications in which they can be implemented; ranging from searching, surveillance and exploration applications to cargo transportation and cooperative manipulation [1,2]. Cooperative formation control focuses more on the efficiency and fault tolerance that a single mobile robot could not provide [3]. A particular problem of multi-robot coordination that has received much attention in the last decades is formation control. The objective of formation control of multiple mobile robots is to achieve a desired formation pattern while guaranteeing that the multiple robots as a group accomplish a given task cooperatively [4].
The formation control approach has been implemented in different types of vehicles, this in order to perform the tasks, with greater ease and robustness. For example, in [5] this approach was used for underwater vehicles, where the follower tracks a reference trajectory based on the leader position and predetermined formation without the need for leader's velocity and dynamics. This is desirable in marine robotics due to weak underwater communication and low bandwidth. In order to tackle the harsh conditions of underwater environment, in [6] the authors drive unmanned underwater vehicle using a deterministic artificial intelligence approach. This technique is based on self-awareness of the robot and relies on the dynamic of the vehicle and linear regression instead of stochastic or traditional control theory methods. Another application of this technique is presented in [7], where the authors address formation control for a team of quadrotor UAVs in which the robots follow a specified group trajectory while safely changing the shape of the formation according to the specifications of the task. On the other hand, in [8] the formation control of a group of unicycle-type wheeled mobile robots at the dynamics level with a little amount of inter-robot communication is investigated. Another interesting approach arises from bio-inspired control techniques. The robotic swarm control is a new paradigm of multi-robot control system aiming to achieve task in collective way using low level interactions between the members of the swarm. For instance, in [9] the authors proposed to employ a variation of the particle swarm optimization (PSO) algorithm to achieve formation in a swarm of agents while tracking a dynamic target. This techniques is inspired on the pheromone based communication of the ant colonies.
Several formation control approaches have been proposed in the literature, and they are mainly divided in three categories: behavior-based methods, leader-follower and virtual structure methods [10,11]. The behavior-based approach is inspired by the emerging behaviors in nature such as flock of birds, random walks of ants and school of fish [12]. In this case, a group behavior (or mission) comprises some low-level actions (or sub-tasks) and is constructed to achieve the global objective, where the individual robot needs to perform low-level actions to accomplish the group behavior [13]. In the leader-follower formation [14,15], one robot is chosen as the leader which decides the whole formation group's moving trajectory, the other ones are the followers which are tasked to follow the leader, and the desired relative separations and bearings are expected to be maintained [16]. This strategy is easily implemented by using two controllers only and is suitable to describe the formation of robots, but it is hard to take into account the functioning capabilities of different robots, i.e., ability gap of a robot [17]. Finally, in the virtual structure formation, robots behave like particles embedded in a rigid virtual structure [18].
Some very interesting works where the leader-follower formation is used are, for example [19], in which the formation problem is converted to a trajectory tracking problem, where each follower robot tracks its corresponding generated reference trajectory such that the whole group forms and maintains the desired shape. In this work, some experiments were successfully conducted and reported using a group of four TURTLEBOTs. In [20], the authors tackle the leader-follower formation control problem of non-holonomic mobile robots. In this case, the trajectory tracking control for a single non-holonomic mobile robot is extended to the formation control for two non-holonomic robots in which one is the leader and the second is the follower. The controllers proposed by the authors are based on the PI control technique. Simulation results are presented to demonstrate the good performance of the proposed controller.
In this work, we addressed the leader-follower formation control problem for a group of nonholonomic wheeled mobile robots (WMRs). We do not follow the common distancebased approaches where the follower tracks the trajectory generated by the leader with an offset to avoid a collisions. In our approach, the followers track the delayed desired trajectory of the leader. The time delay is not arbitrary, on the contrary, it is obtained as the output of a dynamical system whose inputs are the position of the robots. The aforementioned dynamical system is designed in such way that, when the distance between the robots increases, the magnitude of the delay decreases and vice versa. The proposed methodology allows to achieve a convoy formation or platooning without collisions. Another advantage of the proposed approach is that the followers do not deviate from the leader's path during cornering [21] like the distance-based approaches. Moreover, the distance between robots can be modified by simple tuning the parameters of the delay's dynamical system. To track the desired trajectory, a novel control is proposed that exploits the cascade structure of the robot's kinematic model.
On the other hand, one of the most common problems during the implementation of controllers is the lack of state measurements such as velocity, acceleration, orientation, to name a few. This absence of information could be treated by using different sensors to mediate it. However, this would make the system more complex and above all more expensive. Another factor, for example, is that accurate velocity measurements can be difficult, and actually, may be contaminated by the noises in real environments, which can deteriorate the control performance [22].
One of the alternatives to solve the problem of lack of information, is the design of state observers to estimate the measures that the controller needs. There are many works that use observers to estimate information of a system. In [23], the Cartesian position and the kinematic model is employed to design nonlinear observers to estimate the orientation angle and the linear velocity of a mobile robot. On the other hand, in [24] a state-feedback controller for the non-linear error dynamics of the robot is combined with an observer that estimates the orientation error based on available trajectory information and measurement of the position coordinates. Furthermore, in [25] kinematic and dynamic models of the WMR are described, and an output feedback controller is proposed using adaptive sliding mode controller and a high gain observer is designed for velocity estimation to obtain WMR trajectory tracking.
In this paper, we assume that only the Cartesian position and its time derivative are available from measurements. To overcome the lack of attitude measurements a nonlinear observer is proposed based on the kinematic model of the robot. The orientation observer is designed directly on SO(2) and can be used in either open and closed loop schemes. The stability analysis is carried out by means of Lyapunov theory.
The rest of the paper is organized as follows: In Section 2 the kinematic model of a unicycle mobile robot is presented. The design of the attitude observer and its stability analysis is presented in Section 3, and in Section 4, the control algorithm for Leader-follower formation is described. In Section 5 the stability analysis for the complete closed-loop system is presented. Experimental results with a group of three wheeled mobile robots are presented in Section 6. Finally, conclusions and future research directions are stated in Section 6.

Kinematic Model
Consider a group of differential-drive mobile robots as it is shown in the Figure 1. According to the figure, the Cartesian position and orientation of each robot are denoted by x i = col(x i , y i ) ∈ 2 and θ i ∈ , respectively. The posture of each robot can be described by the unicycle kinematic model as followṡ where Θ i col(cos(θ i ), sin(θ i )) ∈ 2 represents the direction of the Cartesian velocity and n is the number of slave robots. Finally, ν i and ω i are the linear and angular velocities, respectively. The orientation angle and the Cartesian velocity satisfy the nonholonomic constraintẏ or equivalently, The nonholonomic constraint (3) implies that the velocity in the direction of wheel axis is zero, i.e., the robot cannot move in the lateral directions.
x y

Attitude Observer
In order to develop the attitude observer, first notice that an equivalent representation of the kinematic model (1) is the followinġ is the rotation matrix and S ∈ 2×2 is a skew-symmetric matrix given by For the case of SO(2) the rotation matrix R i and skew-symmetric matrix S commute, i.e., SR i = R i S. On the other hand, from (1a) the rotation matrix R i can be reconstructed in an algebraic way as follows as long as ẋ i (t) 0 for all t ≥ 0. Based on the foregoing equation, the attitude observation error is defined as where (2) and Θ i are estimates of R i and Θ i , respectively. With the previous definition the objective is to design an attitude observer such that R i (t) → I as t → ∞ where I ∈ 2×2 is the identity matrix. Motivated by the work reported in [26] the following attitude observer is proposeḋ where k ai ∈ is a positive constant and v i ∈ 2 is given in (6). Notice that the proposed attitude observer can be used in open-loop schemes. Now we can establish the first result of the paper.

Theorem 1.
Assume that the angular velocity ω i and the Cartesian velocityẋ i are available. Moreover, assume that the robot's motion satisfies ẋ i (t) 0 ∀ t ≥ 0. Then, the attitude observer given in (8) guarantees that R i → I as t → ∞.
Proof. By taking into account (4b) and (8) the time derivative of the attitude observation error (7) is given by˙ where R i S = SR i has been used. Now consider the positive scalar function whose time derivative along (9) is given bẏ By taking into account (6), the elements of R i can be expressed as Therefore, the trace of the matrix R i S is given by Substituting the previous result in (11) yieldṡ Therefore V Ri converges asymptotically to zero, this in turn implies that R i → I as t → ∞. This completes the proof.

Formation Control Algorithm
The kinematic model (1) is an underactuated nonlinear system. To overcome this problem, consider the auxiliary control input where R di = Θ di SΘ di ∈ SO(2) represents the desired orientation with Θ di = col(cos(θ di ), sin(θ di )). Therefore, the kinematic model can be written aṡ In this case, the translational subsystem given by (16a) can be analyzed as a completely actuated system perturbed by the coupling term ν i (Θ i − Θ di ) which relates the translational subsystem with the attitude subsystem (16b). On the other hand, given the control input u i the desired vector Θ di and ν i can be computed as The proposed formation control strategy is based on the leader-follower approach and delayed reference signals. Contrary to the conventional distance-based leader-follower approach, where the follower robot follows the trajectory generated by the leader, in our proposed approach, the followers track the delayed desired trajectory of the leader robot. To avoid collisions, the time delay depends on the distance between the robots. The time delay becomes larger when the follower is closed to the leader and vice versa. The control objective can be stated as follows: design the control inputs u i and ω i such that the position and attitude tracking errors defined as converges asymptotically to zero without using attitude measurements. In (18), the subscripts and f denote the leader and follower robots and x d (t) ∈ 2 and θ di (t) are the desired Cartesian trajectory and desired orientation, respectively. Finally, τ i denotes the time delay (i = 1, . . . , n) which is obtained as the output of the systeṁ where a i , b i and γ i are positive parameters and s 0 = 0. For the first leader we have ρ 1 = x − x f , and for i > 1 we have ρ i = x fi−1 − x fi . The second term in (19a) increases or decreases the magnitude of the time delay depending on the distance between the robots.

Leader Robot Controller
Before presenting the leader's controller, let us introduce the following auxiliary error variable η = x + β (20) where β ∈ 2 is the state of the following auxiliary linear systeṁ where Based on (20) and (21) the proposed leader's controller is given by where k o ∈ is a positive gain, Θ is extracted from R and Θ d is computed according to (17). Regarding ω d can be computed as

Follower Robot Controller
The next step is to design the tracking controller for the follower. The proposed position and attitude control laws for the followers have a similar structure to the leader's controllers and are given by where K βfi = K βfi > O, k ofi > 0 are the control gains and β fi ∈ 2 is obtained as the solution ofβ with K ηfj = K ηfj > O and η fj = x fj + β fj .
To avoid complex calculations, the time-derivative of Θ di can be approximated by a low-pass filter,Θ di = s λs + 1 Θ di with λ > 0 is the cutoff frequency. It is important to point out that the attitude control laws (22b) and (24) does not explicitly use the orientation error θ i .

Stability Analysis
Once the leader and follower control laws and attitude observers have been presented we can state the following theorem which summarizes the main result of the paper. (22) and (24) in combination with the attitude observer (8). Furthermore, assume that | θ i (0)| < π and ẋ i (t) 0 for all t ≥ 0. Then, the leader-follower formation is achieved, i.e.,  24) and (25), the position error dynamics is given byβ

Theorem 2. Consider a group of nonholonomic mobile robots described by (1) in closed loop with the control laws
By using trigonometric identities, the term ν i (Θ i − Θ di ) can be written as On the other hand, by taking into account the attitude observer (8) and attitude control laws (22b) and (24b), the attitude error dynamics is given bẏ where the equalities Θ di SΘ i = sin( θ i ) and Θ i − Θ i = R i (I − R i )e 1 have been used.
Finally, by defining z i = β i η i ∈ 4 the whole closed-loop dynamics can be written as follows Clearly, the complete closed-loop system presents a cascade structure between the position and attitude error dynamics. If R i = I, then, R i = R i which implies that Θ i = Θ i . In this case, the term ω i − ω i becomes The result follows from the nonholonomic constrain (2). The previous result shows that the equilibrium point of (31) is (z i , θ i , R i ) = (0, kπ, I) with k = 0, 1, 2, . . .. Now let us analyze the subsystem Σ 2 which is independent of the state z i and has a cascade structure with k oi Θ di SR i (I − R i )e 1 as an interconnection term. This term is bounded and according to Theorem 1 vanishes since the attitude observer error R i → I as t → ∞. Moreover, the equilibrium point θ i = 0 of the unperturbed systeṁ is locally asymptotically stable with Lyapunov function V θi = 1 − cos( θ i ) ∀ θ i ∈ (−π, π). Furthermore, θ i = 0 is locally exponentially stable since the linear approximation of (32) is given by˙ θ i = −k oi θ i . Therefore, according with Lemma A1 (see Appendix A) the Clearly, the attitude subsystem Σ 2 satisfies the condition of Theorem A1 given in Appendix A, thus, it is concluded that the equilibrium point ( θ i , R i ) = (0, I) is uniformly asymptotically stable. The position subsystem Σ 1 can be analyzed using similar arguments. It is straightforward to show that the matrix A i is Hurwitz. Therefore, the equilibrium point z i = 0 of the unforced subsystemż i = A i z i (with θ i = 0) is exponentially stable. This implies that the subsystem Σ 1 is ISS with input θ i . The closed loop system Σ 1 and Σ 2 satisfy the conditions of Theorem A1. As a result, it is concluded that (z i , θ i , R i ) → (0, 0, I) as t → ∞. The convergence of z i to zero implies that η i and β i also converge asymptotically to zero. Then, it follows that the position tracking error x i = η i + β i → 0 as t → ∞. This completes the proof.

Experimental Results
In this section, experimental results are presented to validate the performance of the attitude observer and control laws developed in Sections 3 and 4. The testbed is composed of three Khepera III mobile robots from K-Team and six infrared Optitrack cameras which measure the Cartesian position of the robots (see Figure 2). Although the infrared cameras can also measure the orientation of the robot, This measurement is used only for comparison purposes and do not influence the behavior of the controller. The control laws and the observer were programmed in Matlab with a sample time of 20 [ms]. The control signals were sent to the robots via WIFI communication channel. Table 1 summarizes the parameter values of the control law and the orientation observer employed in the experiments. It is worth to notice, that the initial postures of the robots are selected arbitrarily in every case.
The control algorithms together with the attitude observer were tested using two desired trajectories, a circular path and a lemniscate curve. The parametric equations of both desired trajectories are shown below The Cartesian velocityẋ i can be computed by means of numerical differentiation. However, we obtained better results with the following velocity observer [27] where κ i , ζ i ∈ 2×2 are positive definite matrices and x i ∈ 2 is an estimate of the Cartesian position x i .
The observer, controller gains and the parameters of the delay dynamic equation are shown in Table 1. Regarding the parameter a j , for the first trajectory was set to a 1 = 3 and for the second one we have a 2 = 5. All other parameters were the same for both trajectories.
The trajectories of the robots obtained during the two experiments with the desired Cartesian trajectories are shown in Figures 3 and 4  In order to assess the performance of the attitude observer and control laws we compute the orientation errors θ i = θ i − θ di andθ i = θ i − θ i where θ i is the angle measured by the cameras and the estimated angle θ i is extracted from Θ i as follows where atan2(·, ·) is the two argument arctangent function and Θ i = col( Θ 1i , Θ 2i ). The time evolution of the position and attitude tracking errors obtained in each experiment are shown in Figures 5 and 6. It is observed in the Figures that despite the unmodeled dynamics and discretization of the control laws, a good tracking was achieved. The time evolution of the time delays are shown in Figures 7 and 8. Notice that for the circular path the delays converge to a constant value while for the second trajectory the time delays change slowly while their magnitude increases at the points of the curve with greater curvature (see Figure 4b). This behavior was expected since at this points the robots come closer to each other.
In order to assess the performance of the proposed algorithm, the RMS error is computed for the distance and orientation of each robot to its desired trajectory, additionally to the RMS error of the estimation of the orientation observer is presented. Table 2 collects  the results for the circular desired trajectory while Table 3 shows the results corresponding to the lemniscate curve desired trajectory experiment. On both cases it is observed that the RMS distance error is bellow 0.04 m, while RMS orientation error is under 0.155 radians.        Figures 9 and 10 show the control inputs. Notice that ν i (t) 0 for all t ≥ 0 this implies that the assumption ẋ i (t) 0 is satisfied in both experiments.

Conclusions
In this paper, we proposed kinematic control laws in combination with an attitude observer that solve the problem of convoy formation for a group of nonholonomic mobile robots without using attitude measurements. The proposed control approach is based on the leader-follower scheme but contrary to other works, we used delayed reference signals for the follower robots. The time delays depend on the distance between the robots and are obtained as the outputs of a dynamical system that couples the leader and follower dynamics. As result, collisions between the members of the group are avoided. The kinematic control laws were designed by exploiting the cascade structure of the robots' kinematic model. The proposed controllers are decentralized since only require its own position and the position of the nearest leader. On the other hand, the attitude observer was designed directly on SO (2)