Bearing-based localization for leader-follower formation control

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.


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][2][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][5][6][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][12][13][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 distanceangle 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][19][20]. The leader robot plays a critical role in the robot formation control. In previous studies, the trajectories of the a1111111111 a1111111111 a1111111111 a1111111111 a1111111111 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][22][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.

Modeling
The setup considered throughout this paper is for n robots. The ith robot kinematics can be abstracted as where s i = [x i y i θ i ] T 2 < 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 ith robot to the jth robot or landmark can be written as 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: The zeroth-order Lie derivative of any scalar function is the function itself, i.e., The first-order Lie derivative of the function h k (s, z) with respect to f n i is defined as Here, r represents the gradient operation and Á denotes the vector inner product.
Because L 1 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 f n j , given its first Bearing-based formation control derivative with respect to f n i , is Based on the preceding expressions for the Lie derivatives, the observability matrix is defined as where i, j = (1,Á Á Á,n), k = (1,Á Á Á,m) and p 2 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 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 cy i ▷cosy i and sy i ▷siny i . In Eq (3), The zeroth-order Lie derivative is and its gradient scaled by R 2 ik 1 is given by The first-order Lie derivatives are and their gradients are given by 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 g ik 1 using the gradients of the Lie derivatives up to the first order as follows: Lemma 1: The rank of the observability matrix, M ik 1 , given by Eq (17) is 2 if 2. 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 M ik 1 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 ðM ik 1 Þ ¼ 2. 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.
f v i and f o i are the same as in Eqs (9) and (10), respectively. Because a robot observes two different landmarks, h k (s, z) is defined as The zeroth-order Lie derivative is and its gradient scaled by R 2 where, The first-order Lie derivatives are and their gradients are given by We can write the observability matrix with the rows corresponding to g ik 1 g ik 2 " # using the gradients of the Lie derivatives up to the first order as shown in Eq (26). 3. the robot and two landmarks are not collinear (i.e., g ik 1 . Proof: To meet the three prerequisites above, the M ik 12 matrix in Eq (26) can be transformed into the simplified form shown in Eq (27) The results show that the simplified form of the M ik 12 matrix has three linearly independent rows, i.e., rank ðM ik 12 Þ ¼ 3. 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.

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 hWi, respectively, and α i is the relative orientation between the leader robot and the follower robot, i.e.,a i Dy i À y j .
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). S n : where the state vector is sD s T 1 Â Ã T , sD r 1 φ 1 a 1 ½ T , δ 1 = φ 1 + α 1 , the input vector is S n :

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 where 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 s(0) and that 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.
where GðsðkÞ; uðkÞÞ ¼ T c FðsÞU þ sðkÞ and k 2 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 statespace kinematic model of the multi-robot formation is equivalent to Eqs (33) and (34) is obtained by taking the time derivative of a 1 Dy 1 À y 2 , where s 1r D r 1 φ 1 ½ T is the reduced state-space vector and the matrices Lðs 1 Þ 2Â2 ¼ 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 inputoutput state feedback control for multi-robot formation control. According to Eq (33), we consider the following control input for R 2 : where C ¼ À Kðs 1r À s ide 1r Þ; K ¼ diag k 1 k 2 ½ , 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 The initial configuration vectors of the leader robot and the follower robots are 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.

Simulation analysis
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.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 [16]; however, Bearing-based formation control 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.   Project administration: QH SR.