Robust Tracking Control of Mobile Robot Formation with Obstacle Avoidance

We consider the formation control problem of multiple wheeled mobile robots with parametric uncertainties and actuator saturations in the environment with obstacles. First, a nonconvex optimization problem is introduced to generate the collision-free trajectory. If the robots tracking along the reference trajectory find themselves moving close to the obstacles, a new collision-free trajectory is generated automatically by solving the optimization problem. Then, a distributed control scheme is proposed to keep the robots tracking the reference trajectory. For each interacting robot, optimal control problem is generated. And in the framework of LMI optimization, a distributed moving horizon control scheme is formulated as online solving each optimal control problem at each sampling time. Moreover, closed-loop properties inclusive of stability and H∞ performance are discussed. Finally, simulation is performed to highlight the effectiveness of the proposed control law.


INTRODUCTION
Over the past decade, multirobot system has been intensively studied, because the system can accomplish tasks that are too difficult for a single robot to perform alone.For example, in applications like exploration, surveillance, and tracking, we want to control a team of robots to achieve better overall performance.Formation control is a particularly active area of multirobot system.This problem has been receiving much attention from researchers working on cooperative robotics.The recent review of multirobot formation control is summarized in [1,2].
Leader-follower strategy is the most studied formation control strategy, which uses a hierarchical arrangement of individual controllers.So, the problem of formation control is reduced to individual tracking problems [3][4][5].A way of introducing feedback from followers to a virtual leader is presented in [6].It discusses how to move a group of mobile robots in formation via a virtual leader-follower scheme.Similar to leader-follower strategy, a cyclic architecture is formed by connecting individual robot controllers [7].The difference is that the cyclic controller connections are not hierarchical.In [8][9][10], a behavior-based approach is presented.In this approach, the basic behaviors have been assigned to the independent systems to form a guidance al-gorithm.Then the controllers for achieving different objectives are combined.A model independent coordination strategy for multi-agent formation control is proposed in [11].In [12], an adaptive formation control for distributed autonomous mobile robot groups is presented.In [13], model predictive control is applied to cooperative control of multiple robots.
Obstacle avoidance and path planning are also fundamental problems in mobile robotics [14][15][16][17][18][19].Planning multirobot motion with obstacle avoidance has a number of peculiarities and difficulties that are related to necessity of taking into account not only possible obstacles in a working space, but also movement of other robots.Mixed-integer linear programming (MILP) is a powerful tool for planning and controlling problems because of its modeling capability and the availability of good solvers.In [15], an iterative mixedinteger linear programming algorithm is presented to solve the trajectory generation problem with obstacle-avoidance requirement and minimum-time trajectory generation problem.Another well-studied obstacle-avoidance algorithm is potential field method, where an artificial potential function depending on the obstacles is constructed [18,19].In [19], an artificial potential function called navigation function is proposed when the obstacles are represented by star-shaped sets.This potential function has no local minima except the global minimum at the goal.Furthermore, it is continuously differentiable and attains its maximum value at all the obstacle boundaries.
Although formation control and obstacle avoidance are particularly active areas, few approaches involve them simultaneously.In this paper, we suggest a robust formation tracking scheme for a team of constrained wheeled mobile robots (WMRs) in the environment with obstacles.Our first main result is that the problem of finding the collision-free trajectory for the robots can be formulated as a nonconvex optimization problem under general convex obstacle assumption and solved using MILP method.Our second main result concerns moving horizon formation control of multiple mobile robots with parametric uncertainties and actuator saturations.Under the proposed control scheme, the whole optimization problem is decomposed into a family of simple optimization problems according to the number of cooperative robots.Thus the computational complexity can be reduced.In addition, closed-loop properties inclusive of stability and robustness are also guaranteed.
The rest of the paper is organized as follows.In Section 2, we establish the model to describe the dynamics of the WMR.In Section 3, a nonconvex optimization problem is formulated for computation of collision-free movement of multiple mobile robots.In Section 4, we first generate distributed optimal control problems for each interacting robot whose dynamics and constraints are uncoupled.Then in the framework of LMI optimization, a distributed moving horizon H ∞ control scheme is consequently formulated as online solving each constrained optimization problem synchronously at each sampling time.Closed-loop properties inclusive of stability and robustness are discussed.Some significant simulation results are presented to validate the theoretical analysis in Section 5.

DYNAMIC MODEL AND PROBLEM FORMULATION
In a team of N a WMRs, the dynamics of the ith robot is given by [21] ξi where , and I i are the radius of the wheels, the length of the rear wheel axis, the mass, and the moment of inertia of the platform, respectively; (x i , y i , φ i ) are the position and orientation angle of the mobile robot; (v i , ω i ) are the linear velocity and angular velocity.Due to the actuator saturations, the control inputs are constrained as In this paper, it is assumed that the parameters β 1i , β 2i are unknown but bounded, that is, Then the error dynamics in the continuous-time domain is given in [21].In order to design the controller in the discrete-time domain, the dynamics is discretized by replacing the time derivative with the incremental ratio and using a zeroth order hold for the control input with a sampling period T. The resulting discrete linear parameter varying (LPV) system is as follows: where in which θ i = [v d , φ d , e i3 ] are the time-varying parameters, and w i is a description of uncertainties, that is, the parametric uncertainties are described as additive disturbance [21].
It is assumed that the LPV system matrix varies inside a corresponding polytope Ω i whose vertices consist of L local system matrices, that is, Partitioning the state vector in terms of e i = [η T i , σ T i ] T will be useful for notational reasons, in which In this paper, we consider the robust formation tracking control problem for multiple constrained mobile robots in the environment with obstacles.There are two problems addressed as follows.
Problem 1 (obstacle-avoidance problem).If some obstacles are inside the collision region, the system regenerates a new reference trajectory to avoid collisions with static or moving obstacles.
Problem 2 (formation tracking problem).The formation tracking problem considered is typical, that is, (i) the formation of the robots asymptotically converges to the desired formation; (ii) the leader robot asymptotically tracks the reference trajectory.

OBSTACLE AVOIDANCE
When a team of mobile robots move in the environment in which there are some obstacles or threats, the robots have to consider the problems of formation control and obstacle avoidance simultaneously.Only when the robots are at safe distance from the obstacles, they can take care of maintaining the formation.
In this paper, it is assumed that a reference trajectory for the leader robot is predefined in the absence of obstacles, and the trajectory is exact and feasible, that is, it is an exact solution to the dynamic model ( 1) and satisfies the control constraints (2).If there are no obstacles or threats, the mobile robots will move along the desired trajectory.Once the robots find themselves moving close to the obstacles, they will replan the motion with modification of the reference trajectory to avoid the obstacles.A new collision-free trajectory is generated.Then they will move along the new reference trajectory until they find another obstacle.Since one of the goals is to keep the robots in a given formation, from the planning point of view, we can treat the entire team as a whole object.In the remainder of the section, we propose an optimization problem to generate a collision-free trajectory for the robots in the presence of obstacles.
Here, the obstacle is represented by a convex set, denoted by O, and defined by a set of inequalities.
where R is the number of linear constraints needed to define an obstacle.A moving threat is described by the same kind of inequality (7) with r p being a time-dependent vector.As discussed in [15], in order to ensure that the robot stays outside the obstacle, at least one of the constraints that describe the obstacle is violated, that is, where In (9), π p ∈ {0, 1} is a logical variable such that if a statement is true (or false), π p = 1 (or 0).So ( 9) is a nonconvex constraint and generally hard to solve.MILP allows the encoding of logical rules, decisions, and constraints into the optimization problem [15].This capability of MILP makes it possible to incorporate nonconvex constraints (9) into the problem formulation.Now, we can formulate the optimization problem as follows to regenerate the reference trajectory in the presence of obstacles.Without loss of generality, robot 1 is taken to be the leader robot associated with the tracking cost.
Problem 3. In a team of N a robots, given the nominal (Δβ j1 = 0, j = 1, 2) dynamic model (1) of the leader robot 1, the actual and desired states ξ d,start and ξ d,end find the control vector u d which takes the leader robot from ξ d,start to ξ d,end through the same step as moving along original trajectory while satisfying some constraints, that is, with where the vector z contains the values of the state and the control vector at discrete time points: , and x id , y id are the positions of the other robots that satisfy the desired formation in the team. Moreover, with 1 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 Remark 1. Equations (11b) and (11c) are nonconvex constraints, which makes the optimization problem hard to solve using general optimization techniques.Fortunately, current MILP solvers (e.g., CPLEX, GLPK) can handle nonconvex constraints reasonably well.
Remark 2. In Problem 3, minimization of the objective function which is a quadratic function of u d indicates to minimize the control effort or energy while moving.The equality constraint (11a) contains the state equation ( 1) for robot 1 after discretization.The inequality constraint (11b) ensures that the reference trajectory stays outside the obstacle, and (11c) ensures that other robots that satisfy the desired formation in the team avoid the obstacle safely.
Remark 3. In Problem 3, the equilibrium of the formation is known a prior.So, the individual robots in the team have predetermined geometric relationships with respect to the leader and each other.We can define the equilibrium of the formation in several ways.For instance, we can take a robot to be the leader (real or virtual) and the equilibrium is given in terms of distance of each robot from the leader.It is also possible to formulate the equilibrium by using relative distance between robots.
By solving the optimization problem above, the system can find a collision-free path through the environment with obstacles.After we get the discrete path sequences, the smooth reference trajectory of the team can be produced.For the sake of simplicity, the problem is formulated with the presence of one static obstacle; however, it could be easily extended to multiple static or moving obstacles.

DISTRIBUTED MOVING HORIZON TRACKING CONTROL SCHEME
In this section, we assume that the reference trajectory of the mobile robots can be computed using the algorithm in Section 3. Now, corrective strategies are required to keep the robots tracking the reference trajectory in formation.

Formation control objective
Before giving our results, we review some notions in graph theory.In a team of N a robots, we associate the ith robot to the ith node of the graph.For a graph Ψ = (τ, ), τ = {1, 2, . . ., N a } is the set of robots and ⊂ τ × τ is the set of relative vectors between robots.Each edge is denoted (τ i , τ j ) ∈ where i, j ∈ τ.Two mobile robots are called neighbors if (τ i , τ j ) ∈ .The set of neighbors of robot i is denoted by N i ∈ τ.Without loss of generality, robot 1 is taken to be the leader associated with the tracking cost.Given an admissible formation graph Ψ = (τ, 0 ) and a reference trajectory ξ d , the formation vector F = ( f 1 , . . ., f m+1 ) T ∈ R 2(m+1) has components f i ∈ R 2 defined as follows [13]: where i, j are the nodes of lth element of 0 and d i j is the desired relative vector between any two neighbors i and j.Note that when F ≡ 0, the robots will be keeping formation.By concatenating the states into vectors as η = (η T 1 , . . ., η T Na ) T , we can write the mapping from η to F as F = Dη + d, where D is a matrix that is related with the geometric configuration of the formation and d = (. . ., d i j , . . ., 0).From the definition of the formation vector, we know that Dη c = − d, in which η c is the desired equilibrium state.Then, the integrated cost function relevant for multirobot formation tracking is defined as where e = (e T 1 , . . ., e T Na ) T , σ = (σ T 1 , . . ., σ T Na ) T , u e = (u T e1 , . . ., u T eNa ) T , w = (w T 1 , . . ., w T Na ) T .We can write (16) in the following form where To reduce (17) to the standard form, we define a shift state e i (k) = e i (k) − e c i and a shift input ei , in which e c i = A i (θ i )e c i + B 2i u c ei .Then the dynamic model becomes Component-wise peak bounds on the control signal u ei (k) can be translated to constraints on u ei (k) as follows Then the cost function ( 17) can be rewritten as where e = ( e T 1 , . . ., e T Na ) T , u e = ( u T e1 , . . ., u T eNa ) T , w = w = (w T 1 , . . ., w T Na ) T .And the system for which the stability is to be guaranteed is as follows: where

Distributed optimal control problems
In this subsection, N a distributed optimal control problems and a corresponding moving horizon control algorithm are stated.
Problem 4. For each robot i = 1, . . ., N a and at each update time k, given the current state e i (k), e j (k), solve the following optimization problem: where H i e i , e j , u ei , w i = H i e i , e j + q 3 u ei 2 − γ 2 r w i 2 , subject to the LPV model (19), and P i = P T i > 0 satisfies with Note that Na i=1 H i ( e i , e j , u ei , w i ) = H( e, u e , w).If an edge (τ i , τ j ) connecting the ith and jth node is present, the cost of the optimization problem will have a component which is a function of both e i and e j .In the optimization problem above, the constraints and objectives are explicit, making it possible to incorporate important features directly into the online computation.That is the reason that we apply moving horizon control to formation control of multiple mobile robots.

Lemma 1. Problem 4 can be solved by the following semidefinite programming:
where Proof.First, we will find the worst disturbance that maximizes the cost function From (28), we obtain the worst disturbance that maximizes J i (k), The corresponding worst value of the cost function Thus min/maximization of J i (k) can be converted to minimization of the worst-case value of the cost function J wst i , which is equivalent to subject to By using Schur complements and defining Z i = α 2i P −1 i , we can convert (31a), (31b), and (31c) to the LMI forms (27a), (27b), and (27c).And inequality (25) can be expressed as LMI form (27d) by substituting In the optimization problem, θ i and e i (k) are the measured parameter and state, respectively.Optimization determines the first move u ei (k | k) that gets implemented.
Then, the distributed control law is Remark 4. In the optimization problem above, the worstcase disturbance is computed as a function of current state and control input, and substituted in the min-max formulation.So the optimization problem can be easily solved using LMI technique.In the framework of LMI optimization, the suggested formation control law is consequently formulated as online solving the constrained optimization problems at each sampling time, updated with the actual state and model parameters.Although the proposed scheme cannot consider more than one horizon, however, it can guarantee closedloop stability directly.
Remark 5.The control inputs can be split into two parts To improve conservatism, we relax the future constraints and bound only the implemented control input.

Stability analysis
Now we will proceed with analyzing the distributed moving horizon control laws.
Theorem 1. Suppose that optimization problem (26) subject to (27) admits an (almost) optimal solution; then the system (22) with the moving horizon control law (32) is asymptotically stable for vanishing disturbance.Moreover, where Proof.At any time k, the sum of the distributed optimal value functions is denoted as Applying the optimal control (32), we are now at time k + 1. Suppose that the feasible control for the optimization problem of each robot is denoted by u ei (•), and the state trajectory corresponding to the feasible control is denoted by e i (•).Then, we have for any robot i = 1, . . ., N a , Summing over i, we can upper bound J * Σ (k+1), and comparing to J * Σ (k), we have H i e i (k + 1), e j (k + 1), By using H i e i (k + 1), e j (k + 1), Note that e i (k At time k+1, a feasible control for the optimization problem is equal to the optimal values computed at time k, that is, and by (27d), we observe that (39) From ( 21), we have Na i=1 H i e i (k + 1), e j (k + 1), (40) Now from Q > Q f , we can write (41) Then, it is straightforward that the moving horizon control scheme guarantees the H ∞ norm bound (33) and the asymptotical stability.
For the implementation of the proposed trajectory generation strategy and formation control method, we give an algorithm as follows.
Step 1.For a team of N a robots, define an orientation of set Ψ and a desired reference trajectory in the absence of obstacles.
Step 2. At time k = 0, get the initial state e i (0), the states of all its neighbors e j (0), j ∈ N i , and the polytopic model Ω i (0).For each robot i, solve (26) subject to (27), and find the optimal solutions u * ei (0).By exploiting (32), obtain u * e (0) and apply it to the system.
Step 3. At any time k > 0, update the optimization problem with current states e i (k), e j (k), and the actual polytopic model Ω i (k).
Step 4. If the team is in the safe zone, go to the next step.Otherwise, generate a new reference trajectory online as proposed in Section 3. Solve problem (10) subject to (11) and obtain the new trajectory.Then, go to the next step.
Step 5. Solve optimization problem (26) subject to (27) for each robot i, yielding u * ei (k).Then by exploiting (32), obtain u * e (k) and apply it to the system.
Step 6. Set k = k + 1 and return Step 3 at the next sampling time.

SIMULATION RESULTS
This section presents the results of numerical simulations demonstrating the applications of the obstacle-avoidance technique by using Matlab and CPLEX.The interface available in [22] is used to run CPLEX from Matlab.The dynamics of mobile robots is described in Section 2 and the physical parameters in the model are given as m = 80 kg, Example 1.In this example, a simulation of three-mobilerobot formation with obstacle avoidance of a static obstacle is presented.We consider a simple case involving a team of three mobile robots.The dimension of the position vector for all mobile robots is two.The predesired reference trajectory is ξ d = (t − 50, 0, 0, 1, 0), t ≥ 0. The vector formation graph is defined by τ = 1, 2, 3 and = (1, 2), (1, 3).The relative vector is defined as d 12 = (−2, 1), d 13 = (−2, −1).In the distributed moving horizon implementations, the sampling time is chosen as T = 0.5 s, which implies that the optimization problem is solved at each 0.5 second, updated with the actual state and the actual model.The following weighting parameter values are consistent in the implementation: q 1 = 2, q 2 = 1, q 3 = 1, r = 1, and γ = 1.2.And the matrix D for this example is Correspondingly, 0 0 0 4 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 1 0 0 0 0 0 1 0 0 0 0 0 1 0 0 0 0 0 1 Regarding the condition in Problem 4, we choose And it is assumed that there is an obstacle described as follows moving in formation are shown in Figures 1 and 2. The symbols along each trajectory mark the points at which the moving horizon updates occur.Figure 2 is the position trajectories when the formation objective has been met.It is observed that the mobile robots can avoid the obstacle successfully.We can also see that the mobile robots achieve tracking the reference trajectory, in addition to staying in desired formation.Moreover, the control constraints are guaranteed, which is confirmed in Figure 3. parameter values consistent in the implementation are given as d 12 = (0, 1), d 13 = (0, −1), T = 0.5 s, q 1 = 2, q 2 = 1, The moving obstacle is described as follows: O(t) = x(t), y(t) : x(t) ≤ 0.5, −x(t) ≤ −0.5, y(t) ≤ −0.1(t − 26) + 0.9, −y(t) ≤ 0.1(t − 26) + 0.1 . (45) By applying the implementation algorithm described in Section 4, the simulation result in which the mobile robots encounter a moving obstacle with velocity v obs = 0.1 m/s is drawn in Figure 4. Sequential plots of the robots movement are shown as the obstacle moves.The robots depart from the initial positions and move along the predesired reference trajectory ξ d = (t − 30, 0, 0, 1, 0), t ≥ 0. At time t = 26 s, the robots encountered the moving obstacle.In this situation, the robots behave to avoid the obstacle by regenerating the reference trajectory.As can be seen from the figure, within the interval t ∈ [26, 32], the robots move forward and backward to avoid the obstacle.It can be also observed that the obstacle-avoidance technique is a success for the mobile robot with moving obstacles.

CONCLUSIONS
In this paper, we have discussed robust formation control problem of multiple mobile robots with parametric uncertainties and actuator saturations in the environment with obstacles.There are two main results in our work.The first result concerns the problem of obstacle avoidance.When the robots tracking along the predefined trajectory find themselves moving close to the obstacles, a new collision-free reference trajectory is generated by solving a nonconvex optimization problem under general convex obstacle assumption.The second result concerns formation control of multiple mobile robots with parametric uncertainties and actuator saturations.A distributed moving horizon H ∞ control scheme is proposed.Under the control scheme, the whole optimization problem is decomposed into a family of simple optimization problems according to the number of cooperative robots.Thus, the computational complexity can be reduced.In addition, closed-loop properties inclusive of stability and robustness are also guaranteed.Simulation results validate the proposed approach.
where (β 1i , β 2i ) are nominal values of the uncertain parameters, Δβ 1i,max and Δβ 2i,max are bounds of the additive uncertainties.Letξ d = [x d , y d , φ d , v d , ω d ]T denote the desired trajectory and let u d = [u 1d , u 2d ] T be the corresponding control inputs.Define the error vector as e