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.
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 , the leader-follower approach [11–14], the artificial potential approach  and the graph theory approach . 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 .
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 . In , 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 , 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) , the extended information filter (EIF) , the particle filter (PF)  and the extended Kalman filter (EKF) .
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
The setup considered throughout this paper is for n robots. The ith robot kinematics can be abstracted as (1) where si = [xi yi θi]T ∈ ℜ3 is the robot state, including the robot position [xi yi] and the robot orientation θi, and Ui = [vi ωi]T is the control input. Without loss of generality, we assume that robots can only move forward (vi ≥ 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 ith robot to the jth 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 , 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)
Here, ∇ represents the gradient operation and · denotes the vector inner product.
Higher order Lie derivatives are computed similarly. Additionally, mixed Lie derivatives are defined. The second-order Lie derivative of hk (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) = 3n in our case.
The necessary Lie derivatives of hk (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)
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
- Vi > 0, 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.(18)
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.
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
- Vi > 0, 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., ).
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) = 3n). 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
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, R1 represents the leader robot, while R2, R3 and R4 are the follower robots. The control inputs for the robots are the linear and angular velocities [vi ω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 s given the input, U, and the output, y. Assuming that both the state and the observation equations are affected by additive noises, we obtain (30) (31) where C is the output transition matrix and O and N are white Gaussian noises with zero mean and zero covariance matrices PO and PN, respectively. For simplicity, we assume that s(0) and that O and N are uncorrelated, and then we apply the Euler forward method with a sampling time Tc, to discretize the state dynamics in Eq (30), so Eq (32) is obtained. (32) where and k ∈ N.
In , 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 R1 and R2, 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 Fr(s1), respectively.
Using the standard techniques of input-output linearization in , we propose an input-output state feedback control for multi-robot formation control. According to Eq (33), we consider the following control input for R2: (35) where , with k1, k2 > 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).(36)
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 (R2, R3, R4) follow the leader (R1). 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.
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 Tc = 0.01s; the sample particles are Nt = 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 k1 = 0.85 and k2 = 0.15. The other parameters of the UKF are Tc = 0.01s, PL = diag([h h]), PN = diag([h h]) and P = ([1.13 1.13]), where h = 3.0 × 10−2 rad2.
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.
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 Tc = 0.2s and t = 14s 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 ; 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.
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.
- 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.
- 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.