## Figures

## Abstract

The observability of the leader robot system and the leader-follower formation control are studied. First, the nonlinear observability is studied for when the leader robot observes landmarks. Second, the system is shown to be completely observable when the leader robot observes two different landmarks. When the leader robot system is observable, multi-robots can rapidly form and maintain a formation based on the bearing-only information that the follower robots observe from the leader robot. Finally, simulations confirm the effectiveness of the proposed formation control.

**Citation: **Han Q, Ren S, Lang H, Zhang C (2017) Bearing-based localization for leader-follower formation control. PLoS ONE 12(4):
e0175378.
https://doi.org/10.1371/journal.pone.0175378

**Editor: **Long Wang, Peking University, CHINA

**Received: **September 5, 2016; **Accepted: **March 26, 2017; **Published: ** April 20, 2017

**Copyright: ** © 2017 Han et al. This is an open access article distributed under the terms of the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original author and source are credited.

**Data Availability: **All relevant data are within the paper and its Supporting Information files.

**Funding: **This work is sponsored by the National Natural Science Foundation of China (grant no. 51475383, 51075337) and the applied basic research project of Yunnan (grant no. 2013FD049).

**Competing interests: ** The authors have declared that no competing interests exist.

## Introduction

Multi-robot formation control has long been a topic of interest in both academic research and industrial applications. Furthermore, the advantages of multi-robot systems over a single robot include greater flexibility, adaptability and robustness [1–3]. Based on these characteristics, typical applications for multi-robot formations include underwater or outer space exploration, shop floor transportation, guarding, escorting, airport snow shoveling, surveillance and patrolling missions [4–7]. However, multi-robot formation control is challenging, especially when the observation information is poor and the system is highly nonlinear. A variety of formation control methods have been proposed, such as the virtual structure approach [8,9], the behavior-based approach [10], the leader-follower approach [11–14], the artificial potential approach [15] and the graph theory approach [16]. Among these approaches, the leader-follower approach has been the most widely used method owing to its simplicity, scalability and reliability. However, the majority of existing leader-follower approaches require the distance-angle information or more information [13].

In real environments, the available observations of multi-robot systems could be the only bearing observations, which pose significant challenges for formation control. If the leader robot can observe no less than two different landmarks while the robot and the landmarks are not collinear, then the system is completely observable [16]. In [17], the observability analysis of cooperative locations for two robots is presented with bearing-only measurements, and the robot states are not observable with respect to a global reference frame. It has also been shown that two landmarks are needed for the observability of a single vehicle [18–20]. The leader robot plays a critical role in the robot formation control. In previous studies, the trajectories of the leader robot for multi-robot formation control had to be ideal. When the leader robot system is observable, the ideal trajectories of the leader robot for formation control will be avoided, and the leader-follower formation can run along complex trajectories and maintain the desired formation. Only a few special cases regarding bearing-only formation control problems have been solved. In [21–23], bearing-only control laws that can guarantee global stability for the formation control of only three or four robots are presented. In [24], observability conditions for position tracking are established based on bearing-only observations, and the localization problem is studied using the extended output Jacobian. To our knowledge, bearing-only leader-follower formation control for multi-robots that do not require the trajectories of the leader robot to be ideal is still a relatively new research topic that has not attracted much attention; thus, this topic is the focus of the present study.

To restrict multi-robot formation errors, the system must be observable, and the estimation techniques must be used to solve the localization problem. Estimation techniques include the unscented Kalman filter (UKF) [11], the extended information filter (EIF) [16], the particle filter (PF) [25] and the extended Kalman filter (EKF) [26].

Our approach in this paper can be applied to situations with n robots. This paper offers two main contributions. First, to achieve multi-robot formation control, a PF estimation algorithm is used to solve the leader robot localization problem. Furthermore, we ensure that the leader robot system is completely observable if the leader robot can observe no less than two different landmarks. In this case, the trajectories of the leader robot for formation control are not required to be ideal. Second, control of the leader-follower formation for multi-robots is studied based on the bearing-only UKF algorithm and input-output feedback control to achieve rapid multi-robot formation that can remain consistent.

The rest of this paper is organized as follows. In Section II, the leader robot localization problem based on the bearing-only observation is presented. Section III presents leader-follower formation control for multi-robots based on the bearing-only UKF algorithm and input-output feedback control. Simulation results are given in Section IV, and in Section V, we offer our conclusions.

## Leader robot localization based on bearing-only observations

### Modeling

The setup considered throughout this paper is for *n* robots. The *i*th robot kinematics can be abstracted as
(1)
where *s*_{i} = [*x*_{i} *y*_{i} *θ*_{i}]^{T} ∈ ℜ^{3} is the robot state, including the robot position [*x*_{i} *y*_{i}] and the robot orientation *θ*_{i}, and *U*_{i} = [*v*_{i} *ω*_{i}]^{T} is the control input. Without loss of generality, we assume that robots can only move forward (*v*_{i} ≥ 0, *i* = 1,⋯,*n*). Each robot uses the exteroceptive sensors to measure its bearings relative to the other robots and the known landmarks that are in the field of view of the sensors. As shown in Fig 1, the leader robot can obtain bearing measurements by measuring two different landmarks, and relative bearings from the *i*th robot to the *j*th robot or landmark can be written as
(2)

### Observability analysis of the leader robot system

The PF method is used to determine the location of the leader robot based on bearing-only information and to approximate the state of the leader robot; the position estimation of the leader robot is critical to improving the localization accuracy. The leader robot plays a crucial role in the leader-follower multi-robot formation control. When the leader robot system is observable, the formation errors are bounded, and the leader robot’s trajectories are very close to the true trajectories. Based on the nonlinear observability rank criteria [27], we derive the linearly independent rows in the observability matrix for the leader robot that observes a certain number of landmarks. To compute the Lie derivatives, the nonlinear kinematic equation in Eq (1) is changed into the following convenient form: (3)

The zeroth-order Lie derivative of any scalar function is the function itself, i.e., (4)

The first-order Lie derivative of the function *h*_{k} (*s*, *z*) with respect to is defined as
(5)

Here, ∇ represents the gradient operation and · denotes the vector inner product.

Because is a scalar function, the second-order Lie derivative of *h*_{k} (*s*, *z*) with respect to is
(6)

Higher order Lie derivatives are computed similarly. Additionally, mixed Lie derivatives are defined. The second-order Lie derivative of *h*_{k} (*s*, *z*) with respect to , given its first derivative with respect to , is
(7)

Based on the preceding expressions for the Lie derivatives, the observability matrix is defined as
(8)
where *i*, *j* = (1,⋯,*n*), *k* = (1,⋯,*m*) and *p* ∈ N. The important role of this matrix in the observability analysis of a nonlinear system is demonstrated by Theorem 1.

**Theorem 1:** A system is locally weakly observable if its observability matrix, *M*, has full rank, e.g., rank (*M*) = 3*n* in our case.

The necessary Lie derivatives of *h*_{k} (*s*, *z*) and the corresponding gradients are computed, and the observability matrix, *M*, is obtained. In the first case, the observability condition that a robot observes a landmark is analyzed.

For simplicity, we assume that and . In Eq (3), (9) (10)

The zeroth-order Lie derivative is (11) and its gradient scaled by is given by (12) where , , and .

The first-order Lie derivatives are (13) (14) and their gradients are given by (15) (16)

Thus, the gradients of the second-order and higher order Lie derivatives are linearly dependent on the rows of the observability matrix corresponding to the gradients of the first-order and zeroth-order Lie derivatives. Therefore, we can write the observability matrix with the rows corresponding to using the gradients of the Lie derivatives up to the first order as follows: (17)

**Lemma 1:** The rank of the observability matrix, , given by Eq (17) is 2 if

*V*> 0,_{i}*i*= 1,⋯,*n*; and- the robot does not move along the line that joins the robot and the landmark.

**Proof:** If the robot does not move along the line that joins the robot and the landmark, the matrix in Eq (17) can be transformed into the simplified form shown in Eq (18) using a finite sequence of elementary row operations.

The two nonzero rows in Eq (18) are linearly independent. Therefore, the rank of Eq (18) is 2, i.e., rank . Therefore, from Theorem 1, the single robot states are not locally weakly observable.

Similarly, the observabilities of a robot that observes two different landmarks is also analyzed. The rank of the observability matrix can be easily obtained using the same method as that in Lemma 1.

and are the same as in Eqs (9) and (10), respectively. Because a robot observes two different landmarks, *h*_{k} (*s*, *z*) is defined as
(19)

The zeroth-order Lie derivative is (20) and its gradient scaled by is given by (21) where, , , , , and .

The first-order Lie derivatives are (22) (23) and their gradients are given by (24) (25)

We can write the observability matrix with the rows corresponding to using the gradients of the Lie derivatives up to the first order as shown in Eq (26).

(26)**Lemma 2:** The rank of the observability matrix given by Eq (26) is 3 if

*V*> 0,_{i}*i*= 1,⋯,*n*;- the robot does not move along the line that joins the robot and the landmark; and
- the robot and two landmarks are not collinear (i.e., ).

**Proof:** To meet the three prerequisites above, the matrix in Eq (26) can be transformed into the simplified form shown in Eq (27) using a finite sequence of elementary row operations.

The results show that the simplified form of the matrix has three linearly independent rows, i.e., rank . Therefore, from Theorem 1, the single robot states are completely observable.

From Lemma 2, we know that if all of the *n* robots in the group can directly observe two different landmarks, then the system is completely observable (i.e., rank (*M*) = 3*n*). If all of the *n* robots in the group can directly observe more than two different landmarks, the system can obtain more observation information and is also more observable. If a system is locally weakly observable, the system output can convey sufficient information to allow the observer to provide a correct estimate of the state, thus effectively improving the localization of the leader robot. In a non-observable system, the output does not convey sufficient information to allow the observer to provide a correct estimate of the state, thus negatively affecting the localization of the leader robot.

## Leader-follower formation control for multi-robots based on the bearing-only UKF algorithm and input-output feedback control

### Problem statement

Based on the observabilities of a leader robot that observes two landmarks, the bearing-only leader-follower formation control is studied in this section. We formulate the leader-follower formation model and define the notations used. As shown in Fig 2, *R*_{1} represents the leader robot, while *R*_{2}, *R*_{3} and *R*_{4} are the follower robots. The control inputs for the robots are the linear and angular velocities [*v*_{i} *ω*_{i}]^{T}, where (*i* = 1,⋯,*n*). *ρ*_{i} is the distance from the centroid of the leader to the centroid of the follower, and *φ*_{i} is the view angle from the y-axis of the follower robot to the centroid of the leader robot. *θ*_{i} and *θ*_{j} are the orientations of the leader robot and the follower robot with respect to the world frame 〈W〉, respectively, and *α*_{i} is the relative orientation between the leader robot and the follower robot, i.e.,.

With reference to Fig 2, the kinematic model of a formation with one leader and one follower in a polar coordinate system can be readily defined as shown in Eq (28). The kinematic model of a formation with one leader and *n* followers can be defined as shown in Eq (29).
(28)
where the state vector is , , *δ*_{1} = *φ*_{1} + *α*_{1}, the input vector is , the output vector is , and .
(29)

### Input-output feedback control based on bearing-only UKF

To achieve and maintain the desired leader-follower formation, the follower robots require information regarding the relative position of the leader robot to adjust their positions in real time. In our approach, the leader robot position is estimated using UKF, and the follower positions are calculated using the classical input-output feedback control law; this system is shown Fig 3. In the following, we explain how to achieve input-output feedback control based on the bearing-only UKF.

The variable is the state estimation of the true state ** s**. The UKF is designed to estimate the angle information, i.e., [

*φ*

_{i}

*α*

_{i}]

^{T}of the state

**given the input,**

*s**U*, and the output,

**. Assuming that both the state and the observation equations are affected by additive noises, we obtain (30) (31) where**

*y**C*is the output transition matrix and

*O*and

*N*are white Gaussian noises with zero mean and zero covariance matrices

*P*

_{O}and

*P*

_{N}, respectively. For simplicity, we assume that

**(0) and that**

*s**O*and

*N*are uncorrelated, and then we apply the Euler forward method with a sampling time

*T*

_{c}, to discretize the state dynamics in Eq (30), so Eq (32) is obtained. (32) where and

*k*∈ N.

In [11], the UKF is based on the unscented transformation, which includes a prediction and a correction step. To improve the robot localization, the process equation in Eq (30) and the observation equation in Eq (31) must be sampled.

The control law is designed for *R*_{1} and *R*_{2}, similar to the design for the other robots of a multi-robot formation. Changing the form of the state equation in Eq (28), the reduced state-space kinematic model of the multi-robot formation is equivalent to Eqs (33) and (34) is obtained by taking the time derivative of ,
(33)
(34)
where is the reduced state-space vector and the matrices and are the two upper-left and right submatrices of *F*_{r}(*s*_{1}), respectively.

Using the standard techniques of input-output linearization in [28], we propose an input-output state feedback control for multi-robot formation control. According to Eq (33), we consider the following control input for *R*_{2}:
(35)
where , with *k*_{1}, *k*_{2} > 0. The superscript “ide” refers to the desired state, and *C* is the auxiliary control parameter. Eqs (34) and (35) act as a feedback linearizing control in Eq (33), so that the closed-loop dynamics are shown in Eq (36).

## Simulation study

This section presents the results of the simulation conducted to validate the observability conditions of the leader robot system and the leader-follower formation control discussed in the previous section. For the leader robot localization system, the simulation environment consists of one leader robot and two landmarks. For the leader-follower formation system, the simulation environment consists of one leader robot and three follower robots, where the followers (*R*_{2}, *R*_{3}, *R*_{4}) follow the leader (*R*_{1}). To demonstrate the validity of the proposed formation control approach, simulations are designed using both Webots 7 and Matlab. The simulation scenario given in Webots 7 is shown in Fig 4. The leader robot system performance and the leader-follower formation system performance are given in Figs 5 and 6, respectively. In Tables 1 and 2, the algorithmic (EIF versus PF) performances are given.

### Initial conditions

In this study, we assume that the noise is white Gaussian. The simulation parameters that are used in this study are given as follows:

For PF, the sampling time is *T*_{c} = 0.01s; the sample particles are *N*_{t} = 500; the standard deviations of the robot state noise are ; and the standard deviation of the measured noise is .

The parameters for the UKF and the input-output state feedback law are as follows:

We set ** s**(0) = [0.4474 2.4223 0 0.4360 2.4469 0 0.4126 1.816 0]

^{T}and , where the distances have units of meters and the angles have units of radians. The gains of the controller are

*k*

_{1}= 0.85 and

*k*

_{2}= 0.15. The other parameters of the UKF are

*T*

_{c}= 0.01s,

*P*

_{L}=

*diag*([

*h h*]),

*P*

_{N}=

*diag*([

*h h*]) and

*P*= ([1.13 1.13]), where

*h*= 3.0 × 10

^{−2}rad

^{2}.

The following velocity inputs are assigned to the leader robot: (37)

The initial configuration vectors of the leader robot and the follower robots are (38)

In the initial moment, the robots must adjust their original positions to the ideal positions. The original position errors are relatively large, which has no effect on the normal movement of the robot formation. As a result, we do not consider the initial position errors when analyzing the simulations.

### Simulation analysis

Fig 5A shows the true and estimated trajectories of the leader robot for all three cases. The enlarged insets in Fig 5A show that the estimated trajectories with two different landmarks are those closest to the true trajectories. This is because with two different landmarks, the leader robot system is observable according to the observability condition described in Section 2.2. Fig 5B shows the root mean square errors with no landmark, one landmark and two landmarks. The root mean square errors with no landmark and one landmark are higher than those with two landmarks because the leader robot system is observable in the case with two landmarks. Fig 5C shows that the errors of *θ* for all three cases are very small. In addition, the errors for two landmarks are smaller than the errors for no landmark or one landmark because according to the observability condition described in Section 2.2, the leader robot system is observable with more than one landmark. Fig 5D shows the relative bearing errors for all three cases. Although the relative bearing errors for all three cases are small, the errors for two landmarks are smaller than the errors for one landmark.

Fig 6A shows the trajectories of the leader and the follower robots and shows that the desired formation is properly maintained. This is because the estimated trajectories of the leader robot are closest to the true trajectories when the leader robot system is observable. Fig 6B shows that the observation angle estimation errors are very small. Fig 6C shows that the direction angle estimation errors are also very small during the input-output feedback control process except when the leader suddenly adjusts the movement direction from right to left or from left to right; the maximum direction angle error occurs at approximately -0.015.

All of the simulation results indicate stable performance of the proposed formation control solution and a quick response to changes as long as the leader robot system is observable.

To conveniently compare the localization performance of the algorithms (EIF versus PF), we set *T*_{c} = 0.2*s* and *t* = 14*s* with the other initial conditions set as described above. Tables 1 and 2 show that the localization accuracy is higher for the PF than the EIF in [16]; however, the PF requires slightly more time to run than the EIF. In Table 2, the maximum errors of *γ* and *θ* greatly differ between the PF and the EIF, whereas the maximum errors of X and Y differ only slightly. The minimum errors of *γ*, *θ*, X and Y are approximately the same because all of the minimum errors are bounded.

## Conclusion

Based on the bearing-only observations, the nonlinear observability properties of the leader robot system and the leader-follower formation control are studied. When the leader robot system is observable, the leader-follower formation can be formed rapidly and then maintained. Simulation results are presented to demonstrate that the proposed approach can efficiently control the desired formation of multi-robots. Future research for multi-robot formation control should consider dynamic obstacles and formation transformation.

## Author Contributions

**Conceptualization:**QH.**Data curation:**HL.**Formal analysis:**CZ.**Funding acquisition:**QH.**Investigation:**SR.**Methodology:**QH.**Project administration:**QH SR.**Resources:**QH.**Software:**HL.**Supervision:**QH.**Validation:**CZ.**Visualization:**HL.**Writing – original draft:**QH.**Writing – review & editing:**CZ.

## References

- 1. Gustavi T, Hu X. Observer-based leader-following formation control using onboard sensor information. IEEE Trans Robot. 2008;24: 1457–1462.
- 2. Tanner HG, Piovesan JL. Randomized receding horizon navigation. IEEE Trans Automat Contr. 2010;55: 2640–2644.
- 3.
Peng Z, Wen G, Rahmani A. Leader-follower formation control of multiple nonholonomic robots based on backstepping. In: Proceedings of the 28th Annual ACM Symposium on Applied Computing. ACM: Coimbra, Portugal; 2013. pp. 211–216.
- 4. Reif JH, Wang H. Social potential fields: a distributed behavioral control for autonomous robots. Rob Auton Syst. 1999;27: 171–194.
- 5. Barnes LE, Fields MA, Valavanis KP. Swarm formation control utilizing elliptical surfaces and limiting functions. IEEE Trans Syst Man Cybern B Cybern. 2009;39: 1434–1445. pmid:19447722
- 6. Saska M, Vonásek V, Přeučil L. Trajectory planning and control for airport snow sweeping by autonomous formations of ploughs. Journal of Intelligent & Robotic Systems. 2013;72: 239–261.
- 7. Acevedo JJ, Arrue BC, Maza I, Ollero A. Cooperative large area surveillance with a team of aerial mobile robots for long endurance missions. Journal of Intelligent & Robotic Systems. 2013;70: 329–345.
- 8. Lewis MA, Tan KH. High precision formation control of mobile robots using virtual structures. Auton Robots. 1997;4: 387–403.
- 9. Ren W, Beard RW. Formation feedback control for multiple spacecraft via virtual structures. IEE Proceedings—Control Theory and Applications. 2004;151: 357–368.
- 10. Balch T, Arkin RC. Behavior-based formation control for multirobot teams. IEEE Trans Rob Autom. 1998;14: 926–939.
- 11.
Han Q, Li T, Sun S, Villarrubia G, de la Prieta F. “1-N” leader-follower formation control of multiple agents based on bearing-only observation. In: Demazeau Y, Decker KS, Bajo Pérez J, de la Prieta F, editors. Advances in Practical Applications of Agents, Multi-Agent Systems, and Sustainability: The PAAMS Collection, 13th International Conference, PAAMS 2015, Salamanca, Spain, June 3–4, 2015, Proceedings. Cham: Springer International Publishing; 2015. pp. 120–130.
- 12. Morbidi F, Mariottini GL, Prattichizzo D. Observer design via immersion and invariance for vision-based leader—follower formation control. Automatica. 2010;46: 148–154.
- 13. Chen J, Sun D, Yang J, Chen H. A leader-follower formation control of multiple nonholonomic mobile robots incorporating receding-horizon scheme. Int J Rob Res. 2009;28: 727–747.
- 14. Shao J, Xie G, Wang L. Leader-following formation control of multiple mobile vehicles. IET Control Theory and Applications. 2007;1: 545–552.
- 15. Kwon JW, Chwa D. Hierarchical formation control based on a vector field method for wheeled mobile robots. IEEE Trans Robot. 2012;28: 1335–1345.
- 16. Sharma R, Quebe S, Beard RW, Taylor CN. Bearing-only cooperative localization. Journal of Intelligent & Robotic Systems. 2013;72: 429–440.
- 17. Martinelli A, Siegwart R. Observability analysis for mobile robot localization. International Conference on Intelligent Robots and Systems. 2005;2: 1471–1476.
- 18. Betke M, Gurvits L. Mobile robot localization using landmarks. IEEE Trans Rob Autom. 1997;13: 251–263.
- 19.
Perera LDL, Melkumyan A, Nettleton E. On the linear and nonlinear observability analysis of the SLAM problem. International Conference on Mechatronics; 2009. pp. 1–6.
- 20.
Lee KW, Wijesoma WS, Javier IG. On the observability and observability analysis of SLAM. IEEE/RSJ International Conference on Intelligent Robots and Systems; 2006. pp. 3569–3574.
- 21. Basiri M, Bishop AN, Jensfelt P. Distributed control of triangular formations with angle-only constraints. Syst Control Lett. 2010;59: 147–154.
- 22.
Bishop AN. A very relaxed control law for bearing-only triangular formation control. In: Proceedings of the 18th IFAC World Congress; Milano, Italy. 2011a; pp. 5991–5998.
- 23.
Bishop AN. Distributed bearing-only quadrilateral formation control. In: Proceedings of the 18th IFAC World Congress; Milano, Italy. 2011b; pp. 4507–4512.
- 24. Mariottini GL, Morbidi F, Prattichizzo D, Valk NV, Michael N, Pappas G, et al. Vision-Based localization for leader-follower formation control. IEEE Trans Robot. 2009;25: 1431–1438.
- 25. Li T, Bolic M, Djuric PM. Resampling methods for particle filtering: classification, implementation, and strategies. IEEE Signal Process Mag. 2015;32: 70–86.
- 26. Roumeliotis SI, Bekey GA. Distributed multirobot localization. IEEE Trans Rob Autom. 2002;18: 781–795.
- 27. Hermann R, Krener A. Nonlinear controllability and observability. IEEE Trans Automat Contr. 1977;22: 728–740.
- 28.
Slotine JJE, Li W. Applied nonlinear control. Englewood Cliffs, NJ: Prentice Hall; 1991.