Kos, Greece, July 25, 2007
TuD08.1
Decentralized robust control for multivehicle navigation
Michael Defoort, Thierry Floquet, Annemarie Kokosy and Wilfrid Perruquetti
Abstract— This paper presents a decentralized architecture
for the navigation of a formation of autonomous mobile robots
evolving in an uncertain environment with obstacles. The
motion planning scheme consists in decentralized receding
horizon controllers that reside on each vehicle to achieve coordination among formation agents. The advantage of the proposed
algorithm is that each vehicle only requires local knowledge of
its neighboring vehicles. The main requirement for designing a
conﬂict free trajectory that satisfy the coupling constraints, in a
decentralized way, is that each robot do not deviate too far from
its assumed trajectory designed without taking into account the
coupling constraints. Having established an open loop control
strategy for motion planning, an effective saturated closedloop
controller based on integral sliding mode for trajectory tracking
is presented. Finally, some simulation results demonstrate the
effectiveness, realtime and high robustness properties of the
proposed architecture.
Index Terms— Cooperative motion planning, Decentralized
control, Integral sliding mode, Nonholonomic mobile robot.
I. I NTRODUCTION
The research effort in multiagent system relies on the fact
that multiple agents have the possibility to solve problems
more efﬁciently than a single agent. Cooperative control
of multiple vehicles gives rise to signiﬁcant theoretical and
technical challenges and has various engineering applications
including cooperative transportation of a large payload, formation ﬂying of unmanned aerial vehicles [12], and multi
agent gaming such as robot soccer or robot rescue [2], [17],
[23].
In this paper, the problem of interest is to control a formation of autonomous mobile robots through a cluttered environment. Here, the vehicles are dynamically decoupled but
have common constraints that make them interact between
each other. Indeed, each robot has to avoid collision with the
other ones. Moreover, some communication links between
some vehicles must be maintained during the movement.
Since the available power onboard the vehicle is limited,
the distance between two vehicles which may exchange information will naturally be constrained. Besides maintaining
the communication, the feasibility of the trajectories implies
the respect of the dynamic constraints, as well as avoiding
obstacles and collisions. In this paper, we will focus solely
on these dynamic and geometric aspects and ignore mobile
networking factors, such as fading, cross talk, and delay,
This work was partially supported by programs Interreg ACOS, TAT T31
and ARCIR Robocoop.
M. Defoort, T. Floquet and W. Perruquetti are with LAGIS UMR CNRS
8146, Ecole Centrale de Lille, BP 48, Cite Scientiﬁque, 59 651 Villeneuved’Ascq, France
A. Kokosy is with ISEN, 41 bvd Vauban, 59 046 Lille Cedex, France
ISBN 9783952417386
which can also affect the quality of communication between
the vehicles.
The planning problem consists of computing conﬂict free
trajectories for all vehicles from start to the ﬁnal destination.
Depending on the distance that the robots have to travel,
the computation of complete trajectories from start until
ﬁnish may be computationally expensive. Moreover, the
environment is usually partially known and further explored
in real time. Therefore, the trajectories have to be computed
gradually over time while the mission unfolds. It can be
accomplished using an online receding horizon controller
[19], in which partial trajectories from the current states
toward the goal are computed by solving an optimal control
problem over a limited horizon.
Although centralized approaches have been used in different studies (see [7] for instance), its computation time scales
exponentially with the number of vehicles. To reduce the
computational complexity and communication requirements,
it is therefore more sensible to use a distributed strategy
which results in a formation behavior similar to what is
obtained with a centralized approach.
Recently, some decentralized receding horizon controllers
have been proposed in [8], [15], [16]. In [8], a solution is
provided for unconstrained subsystems (decoupled input constraints only) but it cannot be applied when there are coupling
constraints between robots (i.e constraints on the distances
between robots). In [15], a distributed model predictive
scheme is formulated where each subsystem optimizes locally for itself and every neighbor at each update, resulting in
the increase of the computational time. Furthermore, in order
to ensure collision avoidance, some emergency controllers
must be deﬁned. In [16], the decentralized scheme is based
on a leaderfollower architecture. Indeed, the vehicles update
sequentially their trajectory and feasibility is guaranteeing in
spite of the presence of coupling constraints between subsystems. One advantage of the leaderfollower approach is that it
is easy to implement. However, there is no explicit feedback
from the follower to the leader. Another disadvantage is that
the leader is a single point of failure. Other decentralized
strategies based on potential ﬁelds are given in [5], [6] but
they do not enable to satisfy the nonholonomic constraint
imposed by the rolling wheels
In this paper, a distributed implementation, without assigning any leader, of receding horizon control is presented. Each
robot optimizes only for its own control at each update and
exchanges information only with neighboring subsystems. It
requires the exchange of the assumed trajectory designed
without taking into account the coupling constraints between
2150
TuD08.1
II. P ROBLEM SETUP
The control objective is to stabilize each robot of a
formation of N vehicles toward an equilibrium point in a
cooperative way. Each mobile robot Rn , n ∈ {1, . . . , N} (see
Fig. 1), is of unicycle type, with two driving wheels mounted
on the same axis. The geometrical shape of the robot is
inscribed in a circle of radius ρ . The two driving wheels
are independently controlled by two actuators. The nth agent
is fully described by a three dimensional vector qn (t) constituted by coordinates [xn (t), yn (t)]T of the midpoint between
the two driving wheels and by the orientation angle θn (t)
with respect to a ﬁxed frame:
qn (t) = [xn (t), yn (t), θn (t)]T .
In this paper, the kinematic equations of the wheeled
mobile robots are derived under the assumption that the
nonholonomic constraints (see [3] for details) are satisﬁed.
The ideal kinematic equations are:
q˙ n (t) = f (qn (t), un (t))
Z
YQ Q
[Q\Q
<$;,6
neighboring subsystems prior to each update. Providing that
robot does not deviate too far from the assumed trajectory,
a conﬂictfree trajectory satisfying all the constraints can
be found. Contrary to [8], [15], the distributed planning
algorithm guarantee feasibility of the vehicle path, which
is crucial for realtime implementation.
Then, each robot must follow accurately the optimal
trajectory. Obstacles to the tracking of nonholonomic systems
are the uncontrollability of their linear approximation and the
fact that the Brockett’s necessary condition to the existence
of a smooth timeinvariant state feedback is not satisﬁed
[1]. To overcome these difﬁculties, various methods have
been investigated: homogeneous and timevarying feedbacks
[22], sinusoidal and polynomial controls [21], piecewise
controls [11], backstepping approaches [13] or discontinuous
controls [10]. However, most of these methods do not provide
good robustness properties or do not take into account the
important saturation constraints on control inputs.
This objective will be achieved by using integral sliding
mode control [24]. A drawback of the classical sliding mode
control is that the trajectory of the designed solution is
not robust on a time interval preceding the sliding motion.
Here, the design of an integral sliding mode controller which
takes into account the saturation constraints on velocities is
presented.
First, in Section II, the problem setup is described. In
Section III, our decentralized algorithm is presented using
only local information in order to generate conﬂictfree trajectories. Then, the use of the integral sliding mode in section
IV enables that the robots track the planned trajectory in spite
of perturbations. Finally, in section V, a numerical example
illustrates the advantages of the proposed architecture.
ș Q
;$;,6
Fig. 1.
The car like robot
where the vector ﬁeld f : R3 × R2 → R3 and the control
inputs un are deﬁned as:
⎤
⎡
cos θn (t) 0
f (qn (t), un (t)) = ⎣ sin θn (t) 0 ⎦ un (t)
0
1
un (t) = [vn (t), wn (t)]T .
vn (t) and wn (t) are respectively the linear and angular
velocities. The dynamic model (1) is constrained to take into
account the practical limitations on the vehicle velocities, i.e:
vn (t) ≤ vn,max
.
wn (t) ≤ wn,max
(2)
Furthermore, the feasibility for the trajectory planning can
be affected by the presence of obstacles in the environment.
For all n ∈ {1, . . . , N}, the subset O n (t)⊂ {O1 , O2 , . . . , }
of Mn relevant obstacles for Rn is deﬁned. Note that the
obstacle set is time dependent and evolves as long as the
robot moves and discovers new obstacles. Each obstacle,
supposed to be circular, is deﬁned by its center omn =
[Xmn ,Ymn ]T and its radius rmn (1 ≤ mn ≤ Mn ). In order to
ensure the collision avoidance with obstacles, the distance
between the robot and the detected obstacles d(qn (t), omn ) =
(xn (t) − Xmn )2 + (yn (t) −Ymn )2 must satisfy:
d(qn (t), omn ) ≥ ρ + rmn ,
∀mn ∈ {1, . . . , Mn }.
(3)
Here, it is considered a cooperative control problem where
some constraints couple the dynamic behavior of the robots.
Indeed, the trajectory feasibility implies that the topology of
the wireless network is kept for all time, i.e some communication links between some vehicles must be maintained
during the movement. These coupling constraints can be
expressed by the communication graph deﬁned as follows.
Deﬁnition: A communication graph (R, E , S ) is a labeled
graph consisting of:
•
(1)
2151
a ﬁnite set R = {R1 , . . . , RN } of N nodes representing
the robots.
TuD08.1
•
•
a set of edges E ⊂ R × R encoding the communication
links. The pair Rn − R p belongs to E if there is a
communication link between the robots Rn and R p .
a set of edge constraints. Since the available power
onboard the vehicle is limited, the distance between two vehicles which may exchange information is constrained. Let us denote the broadcasting range of each agent Rn as dn,com (> 2ρ ). For
each pair Rn − R p ∈ E , the distance d(qn (t), q p (t)) =
(xn (t) − x p (t))2 + (yn (t) − y p (t))2 has to be smaller
than the shortest range of the two robots, i.e:
d(qn (t), q p (t)) ≤ min(dn,com , d p,com ).
(4)
Lastly, the agents should remain at a safe distance 2ρ from
each other to avoid collisions. This coupling constraint can
be expressed as follows: ∀n ∈ {1, . . . , N −1}, ∀p ∈ {n+1, N}:
d(qn (t), q p (t)) ≥ 2ρ .
(5)
III. D ECENTRALIZED MOTION PLANNING ALGORITHM
The motion planning problem consists of computing the
optimal trajectory for each vehicle from the starting point
qn (t0 ) (t0 is the initial time instant) to the ﬁnal point qn, f in
which satisﬁes the requirements (1)(5) outlined above. The
proposed algorithm relaxes the constraint that the ﬁnal point
is reached in the planning horizon allowing the use of
receding horizon control. Let us denote the planning horizon
as T ∈ R+ . The cost function for multivehicle formation
navigation is deﬁned as:
L(q∗ , u∗ ) =
N
∑
α q∗n − qn, f in 2 + β u∗n 2
•
d(qn (t), q p (t)) ≥ min(dn,com , d p,com )−(vn,max +v p,max )T
(8)
Note that the conﬂict set is time dependent and is expressed
as:
C n (t) = C n,collision (t)
Here, it is proposed that each vehicle Rn only plans its own
trajectory using a receding horizon strategy that only takes
into account the predicted trajectory of all other vehicles
belonging to the conﬂict set C n (t). In order to deﬁne C n (t),
two conﬂict subsets of robots characterizing conﬂicts that
may occur are deﬁned:
• the interrobot collision conﬂict set C n,collision (t) ⊂ R is
the subset of all vehicles R p
=n for which the separation
distance veriﬁes:
d(qn (t), q p (t)) ≤ 2ρ + (vn,max + v p,max )T
C n,com (t).
B. Decentralized motion planning algorithm
In each receding horizon control problem, the same planning horizon T , obstacle detection horizon Td (Td ≥ T ) and
update period γ ∈ (0, T ] are used. At each update tk =
t0 + γ k (k ∈ N), each robot optimizes only its own trajectory
satisfying constraints (1)(5) using only local information.
For each robot Rn , let us denote:
qn (t,tk ) :
the assumed trajectory designed without taking
into account the coupling constraints (4)(5)
where t ∈ [tk ,tk + Td ]
q∗n (t,tk ) : the optimal predicted trajectory satisfying
constraints (1)(5) where t ∈ [tk ,tk + T ]
qn (t,tk ) : the real trajectory where t ∈ [tk ,tk+1 ]
(6)
A. Conﬂict sets
Remark 1: For all (Rn , R p ) ∈R 2 with R p
∈C n (t), the
coupling constraints (4)(5) between robots Rn and R p are
satisﬁed during the interval [t,t + T ]. This fact implies that
the robot Rn must only know the predicted trajectories of
card(C n (t)) robots. It implies local information exchange
between neighbors.
n=1
∗
where
α ,
β ∈ R+ are
T constants, q =
∗T
∗T weighting
T
∗T
∗
∗T
q1 , . . . , qN
and u = u1 , . . . , uN
are the concatened
optimal trajectory and optimal control input. Note that
L(q∗ , u∗ ) = 0 if and only if q∗n = qn, f in and u∗n = 0.
In the following, in order to reduce the computational
complexity and communication requirements, a distributed
strategy is stated. One idea is to only include, for each
receding horizon controller, the robots that could have direct
conﬂicts (i.e. may produce collision or may lost the communication). It enables a distributed optimization based on
local information.
the communication conﬂict set C n,com (t) ⊂ R is the
subset of all vehicles R p
=n such that Rn − R p ∈E and
for which the separation distance veriﬁes:
n (t,tk ), u∗n (t,tk ) and
The associated control inputs are u
un (t,tk ).
Since the coupling constraints (4)(5) depend on the
neighboring trajectories, each vehicle Rn must know
a prediction of these trajectories over each planning
horizon. To that end, before each update tk , the vehicle Rn
receives an assumed trajectory
q p (t,tk ), from each neighbor
R p ∈C n (tk ), satisfying constraints (1)(3). Likewise, it
transmits an assumed trajectory
qn (t,tk ) to all neighbors.
n (t,tk ), for any vehicle
Therefore, the assumed trajectory q
is the same in every distributed optimal control problem
in which it occurs. Then, using its assumed trajectory
n (t,tk ) and the ones of its neighbors, the optimal predicted
q
trajectory q∗n (t,tk ), which satisﬁes all the constraints (1)(5),
is computed.
Let the following optimal control problem Pn (tk−1 ) associated with the nth robot which consists of determining the
n (t,tk ) and the assumed trajectory
assumed control input u
n (t,tk ) which only satisﬁes constraints (1)(3):
q
Problem Pn (tk−1 ):
(7)
2152
min
tk +Td
un (t,tk ) tk
α
qn (t,tk ) − qn, f in 2 + β
un (t,tk ) 2 dt
TuD08.1
subject to
˙ n (t,tk ) =
q
f (
qn (t,tk ),
un (t,tk ))

vn (t,tk ) ≤ vn,max − εvn
n (t,tk ) ≤ wn,max − εwn
w
d(
qn (t,tk ), omn ) ≥ ρ + rmn , ∀omn ∈ O n (tk−1 )
n (tk ,tk ) = q∗n (tk ,tk−1 ),
for all t ∈ [tk ,tk + Td ] and where q
+
+
εvn ∈ R , εwn ∈ R .
Remark 2: The inclusion of the constants εvn and εwn in
the constraints of the motion planning generator guarantees
that there is sufﬁcient control authority to track the optimal
planned trajectory (see Section IV).
Given the conﬂict sets C n,collision (tk−1 ), C n,com (tk−1 ), the
n (t,tk ) and the ones of its neighbors, let
assumed trajectory q
us deﬁne the optimal control problem Pn∗ (tk−1 ) associated
with the nth robot which consists of determining the optimal
control input u∗n (t,tk ) and the optimal predicted trajectory
q∗n (t,tk ) = [xn∗ (t,tk ), y∗n (t,tk ), θn∗ (t,tk )]T which satisﬁes all the
constraints (1)(5):
Problem Pn∗ (tk−1 ):
min
tk +T
u∗n (t,tk ) tk
α q∗n (t,tk ) − qn, f in 2 + β u∗n (t,tk ) 2 dt
subject to ∀t ∈ [tk ,tk + T ]
q˙∗ n (t,tk ) = f (q∗n (t,tk ), u∗n (t,tk ))
v∗n (t,tk ) ≤ vn,max − εvn
w∗n (t,tk ) ≤ wn,max − εwn
∀omn ∈ O n (tk−1 )
d(q∗n (t,tk ), omn )
∀R p ∈ C n,collision (tk−1 )
q p (t,tk ))
d(q∗n (t,tk ),
∗
qq (t,tk ))
d(qn (t,tk ),
∗
qn (t,tk ))
d(qn (t,tk ),
≥
,
≥
≤
≤
(9)
(10)
(11)
ρ + rmn ,
(12)
∀Rq ∈ C n,com (tk−1 )
2ρ + ξ γ
(13)
min(dn,com , dq,com ) − ξ γ(14)
ξγ
(15)
where ξ ∈ R+ is a constant. Here, it is assumed that the
optimal predicted trajectory q∗n (t,tk ) is constrained to be
at most at a distance of ξ γ from the assumed trajectory
n (t,tk ) (see eq. (15)). This constraint enforces the degree
q
of correspondence between the optimal trajectory and the
assumed trajectory known by the neighbors. Therefore, in
order to guarantee the collision avoidance between vehicles
and the preservation of the communication links, the
uncertainty ξ γ due to the mismatch between the assumed
and the optimal predicted neighboring trajectories is added
(see equations (13)(14)).
For each vehicle Rn , the feasible decentralized receding
horizon control algorithm is as follows:
Algorithm:
1) Initialization (before robot moves):
a) Measure its actual state qn (t0 )
b) Compute the obstacle set O n (t0 )
c) Compute the conﬂict sets C n,collision (t0 ) and
C n,com (t0 ) using the equations (7)(8)
d) Gather by communication the neighboring obstacle sets O p (t0 ), for each neighboring robot R p
belonging to the conﬂict set C n (t0 )
e) Set O n (t0 )←O n (t0 ) ( p O p (t0 ))
n (t0 ,t0 ) = qn (t0 ), compute the asf) Knowing that q
sumed trajectory
qn (t,t0 ) and the assumed control
n (t,t0 ), ∀t ∈ [t0 ,t0 + Td ]
input u
g) Gather by communication the assumed vehicles
trajectory
q p (t,t0 ) for each neighboring robot R p
belonging to the conﬂict sets C n (t0 )
h) Compute the optimal predicted trajectory q∗n (t,t0 )
satisfying all the constraints (1)(5) and the optimal control input u∗n (t,t0 ), ∀t ∈ [t0 ,t0 + T ].
2) Robot moves
a) Over any interval [tk ,tk+1 ),
i) Measure its actual state qn (tk ) and apply the
robust closed loop control un (t,tk ) obtained
from the optimal trajectory q∗n (t,tk ) and the
optimal control input u∗n (t,tk ) (see Section IV)
ii) Update the obstacle set O n (tk )
iii) Knowing the optimal predicted state
q∗n (tk+1 ,tk ), compute the assumed trajectory
n (t,tk+1 ) and the assumed control input
q
n (t,tk+1 ), ∀t ∈ [tk+1 ,tk+1 + Td ] by solving
u
the problem Pn (tk )
iv) Update the conﬂict sets C n,collision (tk ) and
C n,com (tk ) using the equations (7)(8)
v) Gather by communication the neighboring
∈ C n (tk )
obstacle sets O p (tk ),∀R
p
vi) Set O n (tk )←O n (tk ) ( p O p (tk ))
vii) Gather by communication the assumed vehi p (t,tk+1 ) for each neighboring
cles trajectory q
robot R p belonging to the conﬂict sets C n (tk )
viii) Compute the optimal predicted trajectory
q∗n (t,tk+1 ) satisfying all the constraints (1)(5) and the optimal control input u∗n (t,tk+1 ),
∀t ∈ [tk+1 ,tk+1 + T ] by solving the problem
Pn∗ (tk ).
b) At time tk+1 go to 2.a
Remark 3: It is assumed that the resolution of problems
Pn (tk ), Pn∗ (tk ), updates and communication delays (steps
2.a.ii2.a.viii) are less than the update period γ .
Remark 4: The proposed algorithm only requires local
information about the vehicles for which conﬂicts may occur,
resulting in a more scalable approach. Furthermore, since
each vehicle only needs the information from its neighbors,
the algorithm requires much less communication bandwidth.
Remark 5: Note that only discrete time trajectories must
be exchanged pairwise. Indeed, the time domain is truncated
into smaller intervals by quadratic laws (see next part for
details).
Remark 6: The control is also decentralized because that
each agent has no knowledge of the desired destinations of
2153
TuD08.1
the others.
C. Technique for solving receding horizon control problem
In this part, the optimal control problem Pn∗ (tk ) is solved
using ﬂatness properties, B spline parametrization and sequential quadratic programming.
Remark 7: One can easily apply the same technique to
solve problem Pn (tk ).
Using the ﬂatness property [9] of system (9), all system
variables can be differentially parameterized by xn∗ , y∗n and
a ﬁnite number of their time derivatives. Indeed, θn∗ , v∗n and
w∗n can be expressed by xn∗ , y∗n and their ﬁrst and second time
derivatives, i.e.
∗
θn∗
v∗n
= arctan xy˙˙n∗
n
=
x˙n∗2 + y˙∗2
n
.
y¨∗n x˙n∗ − x¨n∗ y˙∗n
=
x˙n∗2 + y˙∗2
n
w∗n
(16)
Once the cost and the constraints are mapped into the ﬂat
output space, the optimal trajectory is planned in this space.
Indeed, the optimal control problem Pn∗ (tk ) becomes:
⎛ tk +T ∗
⎞
α tk qn − qn, f in 2 dt
2
⎜
⎟
min
tk +T
⎝
⎠
y¨∗n x˙n∗ − x¨n∗ y˙∗n T
∗
∗
∗2
∗2
xn ,yn
x˙n + y˙n , ∗2
dt
+β tk
∗2
x˙n + y˙n
subject to ∀t ∈ [tk ,tk + T ]
x˙n∗2 + y˙∗2
n
∗ ∗
y¨n x˙n − x¨n∗ y˙∗n
x˙∗2 + y˙∗2
n
≤ vn,max − εvn
≤ wn,max − εwn
n
∀omn ∈ O n (tk−1 )
d(q∗n , omn )
∀R p ∈ C n,collision (tk−1 )
p)
d(q∗n , q
∗
d(qn , qq )
n )
d(q∗n , q
≥ ρ + rmn ,
, ∀Rq ∈ C n,com (tk−1 )
≥ 2ρ + ξ γ
≤ min(dn,com , dq,com ) − ξ γ
≤ ξγ
For each ﬂat outputs, i.e xn∗ and y∗n , an initial guess trajec∗ (t,t ) and y∗ (t,t )) which respects the terminal
tory (xn,ini
k
k
n,ini
constraints is computed.
Then, in order to transform the optimal trajectory generation problem into a parameter optimization one, a piecewise
polynomial function, Bspline, is adopted to approximate
the trajectory. The Bspline functions are chosen as basis
functions due to their ﬂexibility and ease of enforcing
continuity across breakpoints.
The trajectories of the ﬂat outputs are deﬁned as:
xn∗ (t,tk ) =
y∗n (t,tk ) =
∗ (t,t ) + ln a∗ B (t,t ),
xn,ini
∑ j=1 j,n j,3 k
k
∗
n
yn,ini (t,tk ) + ∑lj=1
b∗j,n B j,3 (t,tk )
(17)
where a∗j,n , b∗j,n ∈ R, P ∗n = {a∗1,n , . . . , a∗ln ,n , b∗1,n , . . . , b∗ln ,n } is
the parameter vector, ln is the number of Bspline function.
It depends on the number of polynomial segments and on
the number of smoothness conditions at the knots. B j,3 is
the third order Bspline basis function.
Finally, the time domain is truncated into smaller intervals
by quadratic laws. The optimal coefﬁcients P ∗n are computed
using the constrained feasible sequential quadratic optimization algorithm [18]. See [20] for a detailed analysis of the
efﬁciency of this approach. To ﬁnish, the open loop control
inputs are deduced using equation (16).
IV. T RACKING MODULE
In this section, the subscript n for all variables is omitted
for notation simplicity. For each robot, an integral sliding
mode controller is designed as follows.
A. Problem setup
For each robot, the task is to construct the closed loop
control inputs u(t,tk ) such that the vehicle tracks the optimal
predicted trajectory q∗ (t,tk ) = [x∗ (t,tk ), y∗ (t,tk ), θ ∗ (t,tk )]T ,
generated by the decentralized receding horizon controller.
For all t ∈ [tk ,tk+1 ], the optimal trajectory fulﬁlls the
differential equation:
⎤ ⎡
⎤
⎡ ∗
x˙ (t,tk )
cos θ ∗ (t,tk ) 0 ∗
⎣ y˙∗ (t,tk ) ⎦ = ⎣ sin θ ∗ (t,tk ) 0 ⎦ v ∗(t,tk )
(18)
w (t,tk )
0
1
θ˙ ∗ (t,tk )
where the optimal predicted velocities v∗ (t,tk ) and w∗ (t,tk )
satisfy:
v∗ (t,tk ) ≤ vmax − εv
.
w∗ (t,tk ) ≤ wmax − εw
By directly applying v∗ (t,tk ) and w∗ (t,tk ) obtained in section
III.C, the robot does not follow the reference trajectory
with a good accuracy. It is obvious that the real control
v(t,tk ) and w(t,tk ) rely on the state measurements q(t,tk ).
Furthermore, one can note that there are tracking errors at
time tk , i.e q(tk ,tk )
= q∗ (tk ,tk ). Due to measurement noise
and modeling uncertainties, there are input uncertainties for
v(t,tk ) and w(t,tk ). That is to say, the real equation of the
robot trajectory actually fulﬁlls the following differential
equation: ∀t ∈ [tk ,tk+1 ],
⎤ ⎡
⎤
⎡
x(t,t
˙ k)
cos θ (t,tk ) 0
v(t,tk ) + ϖv
⎣ y(t,t
˙ k ) ⎦ = ⎣ sin θ (t,tk ) 0 ⎦
(19)
w(t,tk ) + ϖw
0
1
θ˙ (t,tk )
where ϖ v and ϖ w represent the uncertainties. Due to saturation constraints, it is assumed that:
ϖ v < εv
.
ϖ w < εw
The controls v(t,tk ) and w(t,tk ) must be designed such
that system (19) follows the reference (18) in spite of the
perturbations. In fact, the goal is to asymptotically stabilize
the tracking errors ex (t,tk ) = x∗ (t,tk ) − x(t,tk ), ey (t,tk ) =
y∗ (t,tk ) − y(t,tk ) and eθ (t,tk ) = θ ∗ (t,tk ) − θ (t,tk ) to zero
while respecting the following constraints:
2154
 v  ≤ vmax
.
 w  ≤ wmax
(20)
TuD08.1
Transforming the tracking errors expressed in the inertial
frame to the robot frame, the error coordinates can be
denoted as:
⎤
⎤ ⎡
⎤⎡
⎡
cos θ
ex
sin θ 0
e1
⎣ e2 ⎦ = ⎣ − sin θ cos θ 0 ⎦ ⎣ ey ⎦ .
e3
eθ
0
0
1
Accordingly, the trackingerror model is represented by:
⎤
⎤ ⎡ ∗
⎡
⎤ ⎡
v cos e3
e˙1
−1 e2
⎣ e˙2 ⎦ = ⎣ v∗ sin e3 ⎦ + ⎣ 0 −e1 ⎦ v + ϖv .
w + ϖw
e˙3
w∗
0
−1
The above condition holds if:
−M1 sign(s1 )
uism =
−M2 sign(−e2 s1 + s2 )
So, the tracking dynamics can be described as
e˙ = g1 (e) + g2 (e)(u + ϖ )
(21)
with e = [e1 , e2 , e3 ]T , u = [v, w]T and ϖ = [ϖv , ϖw ]T .
B. Integral sliding mode controller
For system (21), the control law is deﬁned as follows:
u = uid + uism .
(22)
uid is the ideal control and uism represents the ISM part
which is designed to be discontinuous in order to reject the
perturbation.
The ﬁrst part of the control design is to ﬁnd a saturated
control law uid such that the nominal system e˙ = g1 (e) +
g2 (e)uid is globally asymptotically stable in spite of initial
tracking errors (i.e. q(tk ,tk )
= q∗ (tk ,tk )). The chosen control
law uid has been developed by Jiang in [14]. The motivation
for such a choice is that this design takes into account the
actuator bounds. It is described by:
∗
v cos e3 + λ3 tanh e1
∗
(23)
uid = w∗ + λ1 v e2 sin e3 + λ tanh e
2
3
1+e2 +e2 e3
1
Based on the following Lyapunov function candidate,
V = 21 sT s, the discontinuous control term can be determined
such that V˙ < 0, guaranteeing the attractivity of the sliding
manifold.
∂ s0
∂ s0
e˙ −
(g1 (e) + g2 (e)uid )
V˙ = sT
∂e
∂e
T T
∂ s0
=
g2 (e) s (uism + ϖ ) < 0
∂e
2
Note that the positive parameters λ1 , λ2 and λ3 can be
designed such that the bounds of the controls are met for
our controllers.
Remark 8: However, through experiments, one can conclude that using only controller (23), the robustness performance is not good enough [4]. Therefore, in order to improve
the robustness properties, a discontinuous term based on integral sliding mode control is added to this existing controller.
Let deﬁne the sliding variable s as:
s = [s1 , s2 ]T = s0 (e) + z.
(24)
[−e1 , −e3 ]T ,
s0 (e) =
z˙
= − ∂∂se0 (g1 (e) + g2 (e)uid ),
−s0 (e(tk ))
z(tk ) =
(25)
where
Here, z induces the integral term and provides one more
degree of freedom in the sliding variable design. Initial
condition z(tk ) is determined such that the sliding variable
always satisﬁes s(tk ) = 0. Hence, the controlled system slides
on the sliding surface {s = 0} from tk without any reaching
phase.
(26)
with M1 > max(ϖv ) and M2 > max(ϖw ).
Remark 9: The trajectory evolves on the manifold {s = 0}
from tk and remains there in spite of the perturbations. The
time derivative of the sliding variable is s˙ = ∂∂se0 (e˙ − g1 (e) −
g2 (e)uid ). Therefore, writing that s˙ = 0, the motion equation
in sliding mode is given by e˙ = g1 (e) + g2 (e)uid which is
globally asymptotically stable.
Remark 10: The control gains can be designed such that
the bounds on the control inputs are satisﬁed. In order to
design these constants, a compromise must be found between
the optimality, the performance and the robustness with
respect to perturbations.
V. S IMULATION RESULTS
This section demonstrates the effectiveness of the proposed method. The following simulation presents ﬁve robots
represented by the uncertain nonlinear model (19). The
linear and angular velocities are bounded, i.e. for each
robot Rn , vn,max  ≤ 0.5m.s−1 and wn,max  ≤ 1rad.s−1 . The
disturbances ϖvn and ϖwn enters through the input velocities
and is bounded as follows: ϖvn  ≤ 0.05m.s−1 and ϖwn  ≤
0.1rad.s−1 . The radius of the robots is ρ = 0.15m.
The scenario involves ﬁve vehicles that have to reach a
speciﬁed location marked with together with the corresponding vehicle indices. All along the way, they need
to maintain the connectivity, i.e there are communication
links between the pairs R1 − R2 , R2 − R3 , R1 − R3 , R1 − R4
and R3 − R4 (see Fig. 2). The ﬁve robots make decisions
independently in order to avoid collision with other robots
and obstacles and to maintain the communication links. The
broadcast range of each agent Rn is dn,com = 1.2m.
The proposed decentralized scheme is applied to the
problem using an obstacle detection horizon Td = 2s, a
planning horizon T = 1.3s, an update period γ = 1s and
ξ = 0.2. The robots reach their ﬁnal objectives without
collision while maintaining the communication links in spite
of the uncertainties. Figure 3 depicts the resulting trajectories
(the locations of each robot at time 2k, k ∈ N are marked
with +). Figure 4 shows that the distances between robots
are respected, i.e (Fig. 4a)
2155
∀i = {1, . . . , 4}, ∀ j = {i + 1, . . . , 5},
0.3m ≤ d(qi , q j )
TuD08.1
3
and (Fig. 4b)
d(q1 , q2 )
d(q2 , q3 )
d(q1 , q3 )
d(q1 , q4 )
d(q4 , q5 )
≤
≤
≤
≤
≤
1.2m
1.2m
1.2m .
1.2m
1.2m
2.5
2
1.5
1
0.5
d
safe
Fig. 2.
0
0
Communication graph
5
A5
A
4
A2
A
1
5
(a)
10
15
1.2
A3
1
4
0.8
3
0.6
dq
,q
dq
,q
dq
,q
dq
,q
1
2
2
0.4
1
0.2
1
d
4
2
3
4
5
q ,q
1
0
0
0
5
10
3
15
(b)
−2
−1
0
1
2
3
4
Fig. 4.
Fig. 3.
Respect of the coupling constraints (4)(5)
Trajectory for each robot
TABLE I
C OMPARISON OF THE PROPOSED DECENTRALIZED ALGORITHM WITH
Finally, in order to show the advantages of the proposed
decentralized algorithm (“DEC2”), the same ﬁve vehicle
scenario is tested using a centralized algorithm (“CEN”), a
leaderfollower algorithm [16] (“LF”) and another decentralized algorithm [15] (“DEC1”) applying the resolution
technique given in Section III.C (ﬂatness, Bspline parametrization and CFSQP algorithm). The comparison in term
of computation time for the design of the optimal trajectory
during one planning horizon, of information ﬂow, robustness
properties and feasibility (i.e. guarantee of collision avoidance and maintenance of communication links) using these
algorithms are summarized in Table I.
SOME OTHER CENTRALIZED AND DECENTRALIZED ALGORITHMS FOR
THE FORMATION OF FIVE MOBILE ROBOTS .
Algo.
Comput.
time
Comm.
links
Practise
Feasibility
Guarantee
CEN
LF[16]
DEC1[15]
DEC2
10 sec
with all
vehicles
infeasible
if
N1
YES
0.5 sec
with
neighbors
dependance
on the
leader
YES
5 sec
with
neighbors
high
robustness
properties
NO
0.3 sec
with
neighbors
high
robustness
properties
YES
VI. C ONCLUSIONS
The proposed algorithm, based on receding horizon control, provides a solution to the stabilization problem for a
formation of robots which is subject to constraints (control
bounds, obstacle avoidance and bounded distances between
robots) and uncertainties (measurement uncertainties,. . . ).
The main contribution of this work is the design of a robust
decentralized receding horizon controller guaranteeing the
collision avoidance and the maintenance of communication
links using only local information. Simulations on a formation of ﬁve mobile robots highlighted the good results in term
of computational time to generate a conﬂict free trajectory.
2156
R EFERENCES
[1] R. Brockett, “Asymptotic stability and feedback stabilization”, in R.
Brockett, R. Millman, and H. Sussmann (eds.), Differential geometric
control theory (Boston, MA: Birkhauser), pp. 181195, 1983.
TuD08.1
[2] C. Clark, “A coordination platform for multirobot systems”, PhD
Thesis, Standford University, 2004.
[3] C. Canudas de Wit and O. Sordalen, “Exponential stabilization of mobile robots with nonholonomic constraints”, IEEE Trans. on Automatic
Control, 37, No 11, pp. 17911797, 1992.
[4] M. Defoort, J. Palos, A. Kokosy, T. Floquet, W. Perruquetti and
D. Boulinguez, “Experimental Motion Planning and Control for an
Autonomous Nonholonomic Mobile Robot”, Int. Conf. on Robotics
and Automation, Roma, Italy, 2007.
[5] M. C De Gennaro and A. Jadbabaie, “Formation control for a cooperative multiagent system using decentralized navigation functions”,
American Control Conference, pp. 1416, 2006.
[6] D. V. Dimarogonas and K. J. Kyriakopoulos, “Formation Control
and Collision Avoidance for MultiAgent Systems and a Connection
between Formation Infeasibility and Flocking Behavior”, IEEE Conf
on Decision and Control, pp. 8489, 2005.
[7] W. Dunbar and R. M. Murray, “Model predictive control of coordinated multivehicle formation”, IEEE Conf. on Decision and Control,
2002.
[8] W. Dunbar and R. M. Murray, “Distributed receding for multivehicle
formation stabilization”, Automatica, pp. 549558, 2006.
[9] M. Fliess, J. L´evine, Ph. Martin and P. Rouchon, “Flatness and defect
of nonlinear systems: introductory theory and examples”, Int. J. of
Control, 61, No 6, pp 13271361, 1995.
[10] T. Floquet, J. P. Barbot and W. Perruquetti, “Higherorder sliding
mode stabilization for a class of nonholonomic perturbed systems”,
Automatica, 39, No 6, pp. 10771083, 2003.
[11] J. P. Hespanha and A. S. Morse, “Stabilization of nonholonomic
integrators via logicbased switching”, Automatica, 35, No 3, pp 385393, 1999.
[12] G. Inalhan, D. Stipanovic and C. Tomlin, “Decentralized optimization,
with application to multiple aircraft coordination”, IEEE Conf. on
Decision and Control, 2001.
[13] Z. P. Jiang and H. Nijmeijer, “Tracking control of mobile robots: a
case study in backsteeping”, Automatica, 33, No 7, pp. 13931399,
1997.
[14] Z. P. Jiang, E. Lefeber and H. Nijmeijer, “Saturated stabilization and
track control of a nonholonomic mobile robot”, Syst. and Cont. Letters,
42, No 5, pp. 327332, 2001.
[15] T. Keviczky, K. Fregene, F. Borelli, G. Balas and D. Godbole,
“Coordinated autonomous vehicle formations: decentralization, control
synthesis and optimization”, IEEE American Control Conference, pp.
20222027, 2006.
[16] Y. Kuwata, A. Richards, T. Schouwenaraars and J. P. How, “Decentralized robust receding horizon control”, IEEE American Control
Conference, pp. 20472052, 2006.
[17] J. Lawton, R. Beard and B. Young, “A decentralized approach to
formation maneuvers”, IEEE Trans. on Robotics and Automation, 19,
No 6, pp. 933941, 2003.
[18] C. Lawrance, J. Zhou and A. Tits, “User’s guide for CFSQP Version
2.5”, Institute for Systems Research, University of Maryland, College
Park.
[19] D. Mayne, J. Rawlings, C. Rao and P. Scokaert, “Constrained model
predicitive control: Stability and Optimality”, Automatica, 36, No 6,
pp. 789814, 2000.
[20] M. B. Milam, “Real time optimal trajectory generation for constrained
dynamical systems”, PhD Thesis, California Institute of Technology,
2003.
[21] R. Murray and S. Sastry, “Nonholonomic Motion Planning: Steering
Using Sinusoids”, IEEE Trans. on Automatic Control, 38, No 5, pp.
700716, 1993.
[22] C. Samson, “Control of chained systems: Application to path following
and timevarying pointstabilization of mobile robots”, IEEE Trans. on
Automatic Control, 40, pp. 6477, 1995.
[23] T. Sugar, J. Desai, J. Ostrowski and V. Kumar, “Coordination of multiple mobile manipulators”, IEEE Conf. on Robotics and Automation,
pp. 30223027, 2001.
[24] V. Utkin and J. Shi, “Integral sliding mode in systems operating under
uncertainty conditions”, IEEE Conf. on Decision and Control, pp.
45914596, 1996.
2157