Abstract
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 observation meets 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.
1. 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–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–14], artificial potential approach [15, 16], and graph theory approach [17–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–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 separation-bearing-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 circular formation is presented. In [24–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 bearing-only 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–27] has special requirements for the number of robots or the form of the formation. A distributed control law in [28–30] is used to stabilize formations. In order to utilize the feedback control law, an off-the-axis point in [20–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 real-time 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.
2. Modeling and Observability Analysis
2.1. Modeling
Consider the leader-follower setup in Figure 1, where R1 is the leader of R2 and R2 is the leader of R3. The control inputs of R1, R2, and R3 are the linear and angular velocities , , and , 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 , 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 , , , input vector , and output vector , , and where ,the detailed derivation of is given in Appendix A.
The kinematic model of multistage cascade leader-follower robots’ formation can be readily retrieved as an extension of (1):where is the state vector, is the control input vector, , and .
2.2. 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 leader-follower 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 , , , 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 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 isand 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), where , ;(2), ; 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, = 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 in [21].
Theorem 3. The rank of given by (12) is four if(1), where , ;(2), , where , and , are constants; that is, the leader robot and the follower robot at all levels do parallel linear motion.
Proof. Suppose that , , and , 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 first-order 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.
2.3. 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–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.
In order analyze controllability of the cascade robot formation system, the state equation of (5) is restated as (18). According to the concept of nonlinear controllability as defined in [31], system (18) is said to be locally controllable if the vector fields , and their consecutive Lie brackets , span the whole state space. ConsiderSuppose . We can easily obtain (19) according to the state equation of (5) and (18); that is, , and the controllability rank condition is satisfied, so the the cascade robot formation system is said to be controllable. Consider
3. 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.
3.1. UKF Algorithm
In order to control the formation, an estimate of the true state is required. The UKF is designed to estimate angle information, that is, of the state . The UKF uses the input vector and the output vector . 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 , , and are uncorrelated. We apply the Euler forward method with sampling time to discretize (20) and we can obtain where and .
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.
3.2. Input-Output State Feedback Control
The control law is designed for R1 and R2, 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 is the upper-left submatrix of , and matrices and are the two upper-left and right submatrices of , 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 R2:where , , and , with , . 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 closed-loop dynamics become
4. 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 (R2 following R1 and R3 following R2). 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.
(a) Trajectory 1 of the robots
(b) Time history of observation angle error
(c) Time history of direction angle error
(d) Time history of velocities of R2 and R3
(a) Trajectory 2 of the robots
(b) Time history of observation angle error
(c) Time history of direction angle error
(d) Time history of velocities of R2 and R3
(a) Trajectory 3 of the robots
(b) Time history of observation angle error
(c) Time history of direction angle error
(d) Time history of distance error
(e) Time history of velocities of R2 and R3
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.
4.1. Initial Conditions
The following velocity inputs have been assigned to the leader robot when the cascade robot formation follows trajectory 1. Consider
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.
The formation considered here consists of three robots. We set and , where distances are in meters and angles in radians. The gains of the controller are . The other parameters of the UKF are , , , and , where . 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.
4.2. Simulation Analysis
Figure 3(a) shows the trajectories of the leader and the follower robots. It shows that R1, R2, and R3 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 R1, R2, and R3 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.
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 R1, R2, and R3 maintain the desired formation. Figure 5(b) shows observation angle error is almost zero when the robot formation moves along trajectory 3. Figure 5(c) shows that direction angle error almost converged to zero after when the robot formation moves along trajectory 3. Figure 5(d) shows that distance error is also almost zero when the robot formation moves along trajectory 3. Figure 5(e) shows that the velocities of the follower robots have a trend of increase when the robot formation moves along trajectory 3.
5. 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 input-output 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.
Appendix
A. The Detailed Derivation of
Consider
B. The Transition Process from (12) to (13)
denotes the row of (12), where . is added to and is added to . Similarly, is added to ; is added to ; is added to ; is added to ; is added to and is multiplied by ; is added to ; and is multiplied by ; (12) is restated to
Based on the results above, row operation is continued. interchange each other; interchange each other; interchange each other; interchange each other; interchange each other; thus, the simplest form of (12) is expressed as (13). Consider
C. Unscented Kalman Filter
The UKF is carried out as follows.
Step 1 (choosing sigma points). For , a set of points and weights , called sigma points, are chosen in a deterministic way so that their sample mean and covariance are and , respectively, and the following is obtained: where .
Step 2 (prediction). () Fix and compute in (C.1) using . Transform according to (20), thus obtaining() The predicted mean and covariance are computed as () Compute in (25) using and define the predicted points as() The predicted observation is () The innovation covariance and the cross correlation matrices are defined as (C.6) and (C.7), respectively.
Step 3 (prediction). Considerwhere .
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 cascade formation is equivalent to (23) in our paper, which is derived as where , , and .
Competing Interests
The authors declare that there is no conflict of interests regarding the publication of this paper.
Acknowledgments
The authors gratefully acknowledge the financial support of the 973 Program of China (Grant no. 2010CB328000) and the National Natural Science Foundation of China (Grant no. 51475383).