Bearing-Only Formation Control for Cascade Multirobots

A new formation control method is proposed, which is used to queue multirobots in a single-direction cascade structure. In the cascade formation, each robot is a follower for the previous robot and a leader for the next robot, and the robots in the middle act as both leader and follower. The follower robot can only observe the bearing information of the leader robot. The observability of the cascade leader-follower formation is studied, which shows that the bearing-only observationmeets the observability conditions required for the nonlinear system. Based on the bearing-only observations, the unscented Kalman filter (UKF) is employed for the state estimation of the leader and the follower robots at all levels, which enables the real-time movement control of the follower robots via the input-output feedback control. Simulation results demonstrate that the proposed approach can efficiently control the formation of multirobots as desired.


Introduction
Multirobots formation control has long been a hot points in both academic research and industrial application, particularly, in the areas such as underwater or outer space exploration, shop-floor transportation, guarding, escorting, and patrolling missions [1,2].The advantages of multirobots system over a single robot include greater flexibility, adaptability, and robustness [3][4][5].However, multirobots' 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 virtual structure approach [6,7], behavior-based approach [8], leader-follower approach [9][10][11][12][13][14], artificial potential approach [15,16], and graph theory approach [17][18][19].Among them, the leader-follower approach has been widely used owing to the simplicity, scalability, and reliability.In this case, the leader follows a predefined trajectory, while the other robots (called follower robots) are keeping the position and direction with a certain distance to the leader [9,13,[20][21][22].A leader-follower formation control is designed in [9] for the follower robot to track the leader robot in the desired separation and bearing angle.In [10], a robust control technique is developed that can effectively handle the unknown parameters and uncertainties in the system.In [11], a receding-horizon leader-follower control framework is presented to solve the formation problem of multirobots with a rapid error convergence rate.To maintain the desired leader-follower relationship, a separationbearing-orientation scheme (SBOS) for two-robot formation and a separation-separation-orientation scheme (SSOS) for three-robot formation are proposed.In [12], a framework for controlling groups of autonomous mobile robots to achieve predetermined formation based on a leader-follower approach is presented.In [13], the follower position is not rigidly fixed with respect to the leader but varies in proper circle arcs which is centered in the leader reference frame.The formation is implemented using speed measurements from optical encoders and distance measurements from image processing in [14].In the existing leader-follower approaches, most of them need at least distance-angle information or more information.
In real situations, the available observations of the robots might be only bearing observations, which will make a great challenge to the formation control.Until now, only a few special cases regarding bearing-only formation control problems have been solved.In [23], a bearing-only controller which can stabilize a group of mobile robots into a balanced 2 Mathematical Problems in Engineering circular formation is presented.In [24][25][26], bearing-only control laws that guarantee global stability are only applicable to formations of three or four robots.In [27], the bearing rigidity is employed for mobile formations, so that bearings can be used for shape control in mobile formation.In [28], a distributed control law to stabilize bearing-constrained formations and the concept of parallel rigidity are proposed.In [29], the decentralised formation control of multiple robots in the plane when each robot can only measure the local bearings of their neighbours by using bearing-only sensors is studied, and the target formation is locally finite-time stable with collision avoidance guaranteed.Distributed control of multirobot formation with angle constraints using bearingonly observations is investigated in [30].In [20], observability conditions for position tracking by using bearing-only observations are established, and the localization problem is studied using a new observability condition valid for general nonlinear systems and based on the extended output Jacobian.The bearing-only formation control in [23][24][25][26][27] has special requirements for the number of robots or the form of the formation.A distributed control law in [28][29][30] is used to stabilize formations.In order to utilize the feedback control law, an off-the-axis point in [20][21][22] has to be additionally constructed which is a handling point lying on the robot's axis of orientation, a distance  from the centre.
In this paper, we focus no special requirements for the form of the formation and apply situations of  robots.The contribution is threefold.Firstly, we do not need to construct the off-the-axis points when the input-output feedback control law is used.Secondly, the observability of the cascade leader-follower formation is studied based on the rank of the system observability matrix.This allows us to identify the robot motions that preserve the observability.Finally, based on the bearing-only observations, the unscented Kalman filter (UKF) is employed for the state estimation of the leader and the follower robots at all levels, which enables the realtime and stable movement control of the follower robots via the input-output feedback control.
The rest of the paper is organized as follows.In Section 2, observability and controllability of the nonlinear system are presented.Section 3 presents the UKF algorithm and the input-output feedback control law.Simulation results are given in Section 4. In Section 5, we offer our conclusions.

Modeling and Observability Analysis
2.1.Modeling.Consider the leader-follower setup in Figure 1, where R 1 is the leader of R 2 and R 2 is the leader of R 3 .The control inputs of R 1 , R 2 , and R 3 are the linear and angular and [] 3  3 ], respectively.  is the distance from the centroid of the leader robot to the centroid of the follower robot.  is the view-angle from the axis of the follower robot to the centroid of the leader robot.  and   are the orientations of the leader robot and the follower robot with respect to the world frame ⟨W⟩, respectively, while   is the relative orientation between the leader robot and the follower robot; that is,   ≜   −   .
With reference to Figure 1, the kinematic model of a cascade leader-follower robots' formation can be expressed as where state vector s , and , and where the detailed derivation of () is given in Appendix A.

Nonlinear Observability of Cascade Multirobots.
The function  can be separated into a summation of independent functions in the special case, and each one is excited by a different component of the control input vector; (1) can be restated as Accordingly, (3) can be also restated as ( In order to analyse the observability of the entire leaderfollower system, the nonlinear observability rank criterion which is developed in [31] is used.The observability matrix of ( 5) is defined as the matrix with rows, where ,  = 1, . . ., ,  = 2, . . .,  + 1,  ∈ N, and ∇ represents the gradient operator.
Lemma 1. System   is locally weakly observable if its observability matrix  which is constructed by the row vector has full rank; for example, in our case rank(M) = 3,  refers to the number of follower robots.
In (4), The necessary Lie derivatives of ℎ() and their gradients are computed; then observability matrix  is obtained.
Zeroth-order Lie derivative is and its gradient is The first-order Lie derivatives of the function ℎ() with respect to  ] and   are defined as where denotes the vector inner product, with their gradients given by According to the form of ( 6), row vector matrix  of the cascade leader-follower formation described in Figure 1 can be expressed using gradients of Lie derivatives up to the first order as (12).Theorem 2. The rank of  given by ( 12) is six if (1) ]  > 0, ]  > 0, where  = 1, . . ., ,  = 2, . . .,  + 1; (2)   ̸ = 0,  = (1, . . ., ); that is, the follower robot at all levels, which is measuring the bearing, does not move along the line joining the leader robot and the follower robot; (3) the leader robot and follower robot at all levels can not do parallel linear motion.
Proof.Meeting the three prerequisite requirements above,  matrix in (12), by means of a finite sequence of elementary row operations, can be transformed and simplified as (13) (the detailed derivation of ( 13) is given in Appendix B).From the results, we can see that the simplified form of  matrix has six linearly independent rows; therefore, rank(M) = 6.Consider We can see that their gradients of the necessary Lie derivatives of ℎ() are equal to the same order gradients of the time derivatives of ℎ() from proposition 2 in [21].
Proof.Suppose that   =  1 ,   =  2 , and  1 ,  2 are constants; time differentiation of the functions   and   is zero, and thus we can get ( 14)-( 16); further we can get simplified form of  matrix as (17), From the results of ( 17), we can see that  matrix has four linearly independent rows; therefore, rank () = 4.
Clearly, the gradients of the second-order and higher order Lie derivatives are linearly dependent on the row of the observability matrix corresponding to the gradients of firstorder and zeroth-order Lie derivatives.Therefore, we have the observability matrix for the cascade formation, using the gradients of the first-order and zeroth-order Lie derivatives, as (12).The rank of the observability matrix does not change with the increasing order of the Lie derivatives; that is, rank () = 6 in Theorem 2 and rank () = 4 in Theorem 3.
When the robots move along the curvilinear trajectory, rank () = 6 (see Theorem 2); namely, matrix  has full rank and system   is locally weakly observable (as defined in Lemma 1).When the robots move along the rectilinear trajectory, rank () = 4 (see Theorem 3); namely,  matrix has no full rank and system   is not locally weakly observable.
For the formation control problem to be solvable, the system must be observable.If system is locally weakly observable, the system output can convey an information rich enough to allow the observer to provide a correct estimate of the state, thus improving the formation control effectively.For a system not being observable means that the output does not convey an information rich enough to allow the observer to provide a correct estimate of the state, thus affecting the formation control negatively.

Nonlinear Controllability of Cascade Multirobots.
The "duality" between controllability and observability (which is, mathematically, just the duality between vector fields and differential forms) is presented in [31].The design of the input-output feedback controller is a feedback linearizing process.According to [22,[32][33][34], we know that complete state-controllability and complete observability are equivalent, so the controllability of the robot formation system is simply presented as follows.

Input-Output Feedback Control
In order to achieve the feedback control, state estimation is necessary.We choose the UKF algorithm to estimate the state of the robots in this paper.Combining the UKF algorithm with the input-output state feedback control, the formation control can make full use of bearing-only observations of the cascade robot formation to realize motion control, so the desired formation control can be achieved.

UKF Algorithm.
In order to control the formation, an estimate ŝ of the true state s is required.The UKF is designed to estimate angle information, that is, [    ]  of the state s.The UKF uses the input vector U and the output vector y = ℎ() = [ℎ  1 (), . . ., ℎ   ()]  .Combining the Euler forward method with the UKF algorithm, we can obtain the more accurate information.We assume additive noise in both the process equation ( 20) and the measurement equation ( 21): where  is the output transition matrix and  and  are white Gaussian noises with zero mean and covariance matrices   and   , respectively.We assume that s(0), , and  are uncorrelated.We apply the Euler forward method with sampling time   to discretize (20) and we can obtain where Γ((), ()) =   () + () and  ∈ N.
In [35,36], the UKF is based on the Unscented Transformation, which includes a prediction and correction steps.In order to improve the robot localization, the process equation ( 20) and the measurement equation ( 21) have to be sampled, which are detailed in Appendix C.

Input-Output State Feedback Control.
The control law is designed for R 1 and R 2 , which is also similarly designed to other robots of cascade formation.Changing the form of the state equation in ( 1), the first level kinematic system model of cascade formation is equivalent to ( 23) and ( 24) is obtained by taking the derivative of where is the reduced state-space vector; more details can be found in Appendix D. Matrix () 2×4 is the upper-left submatrix of (), and matrices () 2×2 and () 2×2 are the two upper-left and right submatrices of () 2×4 , respectively.Using standard techniques of I/O linearization in [21,37,38], we propose an input-output state feedback control for the robot formation control.According to (23), let us consider the following control input of R 2 : where with  1 ,  2 > 0. The superscript "ide" refers to the desired state, and  is auxiliary control parameter.Equation (25) acts in (23) as a feedback linearizing control, combining with (24), so that the closedloop dynamics become

Simulation Validation
The effectiveness of the cascade robot formation control approach proposed in this paper is verified by three Pioneer-3at robots with laser radar and panoramic camera, and the angle information is easily obtained by these sensors.In order to validate the proposed formation control approach in a real scenario, we set up 3D simulation platform using the powerful Webots 7, by which the real environment can be simulated and simulation datum can also be imported into Matlab and analysed there.The cascade formation of three robots involves one leader following controller at all levels (R 2 following R 1 and R 3 following R 2 ).Simulation scenarios are shown as in Figure 2.
The cascade robot formation undergoes the piecewise rectilinear-curvilinear combination trajectory 1, trajectory 2 looking like 8, and spiral trajectory 3 that are particularly suited for checking the observability conditions discussed in Section 2.2.Simulation results are shown as in Figures 3, 4, and 5, respectively.
In the simulation, the follower robot at all levels of cascade formation can adjust its movement according to movement of the leader robot.UKF is employed to estimate the state of robots.The input-output state feedback control is used to stabilize the cascade robot formation.

Initial Conditions.
The following velocity inputs have been assigned to the leader robot when the cascade robot formation follows trajectory 1.Consider Trajectory of the robots The following velocity inputs have been assigned to the leader robot when the cascade robot formation follows trajectory 2. Consider The linear and angular velocities of leader robot are the functions of time  when the cascade robot formation follows trajectory 3.
The initial configuration vectors of leader robot and follower robots are Parameters of UKF and the input-output state feedback law are as follows., where ℎ = 3.0 × 10 −2 rad 2 .White Gaussian noise has been injected into the observations.In the initial moment, the robots need to adjust the original postures to the ideal postures, and the original position error is relatively large, which has no effect on the robot formation after the robots enter the normal movement, so we do not consider position error at the initial moment when all kinds of simulations are analysed.3(a) shows the trajectories of the leader and the follower robots.It shows that R 1 , R 2 , and R 3 can maintain the desired formation when three robots undergo trajectory 1. Figure 3(b) shows the observation angle error is almost zero when the robot moves along trajectory 1.The maximum error is −0.0003 even when the trajectory changes suddenly.Figure 3(c) shows the direction angle error is also very small when the robot moves along trajectory 1, and the maximum error is −0.008 when the trajectory changes suddenly.Figure 3(d) shows that the velocities of the follower robots change slightly when the follower robots move along rectilinear trajectory (or curvilinear trajectory).When the trajectories of robot formation change, the velocities of robot formation change significantly.The velocities of the follower robots are almost the same as the initial velocity of the leader robot when the robot formation moves along rectilinear trajectory.Figure 4(a) shows the trajectories of the three robots following trajectory 2. According to observable theory described in Section 2.2, the cascade leader-follower formation system is observable when three robots undergo trajectory 2, so it is evident that R 1 , R 2 , and R 3 maintain the desired formation.Figure 4(b) shows that observation angle error is almost zero when the robot formation moves along trajectory 2. The maximum error is −0.0005 even when the trajectory changes suddenly.Figure 4(c) shows that direction angle error is also almost zero when the robot formation moves along trajectory 2, and the maximum error is −0.02107 even when the direction changes suddenly.Figure 4(d) shows that the velocities of the follower robots have a big change when direction of the robot formation motion has great change.The velocity of the follower robot outside of the leader robot is greater than the initial velocity of the leader robot, and the velocity of the follower robot inside the leader robot is less than the initial velocity of the leader robot.

Simulation Analysis. Figure
Figure 5(a) shows the trajectories of the three robots following trajectory 3.According to observable theory described in Section 2.2, the cascade leader-follower formation system is observable when three robots undergo trajectory 3. We can see that R 1 , R 2 , and R 3 maintain the desired formation.

Conclusion
A new formation control method is proposed for multirobots which are queued in a single-direction cascade structure.The observability of the cascade leader-follower formation is studied, which shows that the bearing-only observation meets the observability conditions required for the nonlinear system.Based on the cascade bearing-only observations, the UKF is employed for the state estimation of the leader and the follower robots at all levels, which enables the movement control of the follower robots via the inputoutput feedback control.Simulation results show that the multirobots can rapidly form a cascade formation, run along complex trajectory with very small error, and maintain the desired formation.Further research will include formation control of multirobots in the environment of obstacles.

C. Unscented Kalman Filter
The UKF is carried out as follows.

D. The First Level Kinematic System Model of Cascade Formation
In order to design control law, changing the form of the state equation in ( 1), the first level kinematic system model of Mathematical Problems in Engineering cascade formation is equivalent to (23) in our paper, which is derived as

Figure 2 :
Figure 2: The simulation scene of mobile robots.
Time history of direction angle error

Figure 5 (
b) shows observation angle error is almost zero when the robot formation moves along trajectory 3. Figure5(c) shows that direction angle error almost converged to zero after  = 2 s when the robot formation moves along trajectory 3. Figure5(d)shows that distance error is also almost zero when the robot formation moves along trajectory 3. Figure5(e) shows that the velocities of the follower robots have a trend of increase when the robot formation moves along trajectory 3.