- Email: [email protected]
Formation path following control of unicycle-type mobile robots
Robotics and Autonomous Systems 58 (2010) 727–736
Contents lists available at ScienceDirect
Robotics and Autonomous Systems journal homepage: www.elsevier.com/locate/robot
Formation path following control of unicycle-type mobile robots Jawhar Ghommam a , Hasan Mehrjerdi b , Maarouf Saad b , Faiçal Mnif a,c,∗ a
Research unit on Mechatronics and Autonomous Systems, École Nationale d’Ingénieurs de Sfax (ENIS), BP W, 3038 Sfax, Tunisia
b
Department of Electrical Engineering, École de technologie supérieure, 1100, rue Notre-Dame Ouest, Montréal, Québec, H3C 1K3, Canada
c
Department of Electrical and Computer Engineering, Sultan Qaboos University, P.O. Box 33, Muscat 123, Oman
article
info
Article history: Received 30 November 2008 Received in revised form 27 October 2009 Accepted 29 October 2009 Available online 12 November 2009 Keywords: Path following Formation control Virtual structure Nonholonomic robot Adaptive control
abstract This paper presents a control strategy for the coordination of multiple mobile robots. A combination of the virtual structure and path following approaches is used to derive the formation architecture. A formation controller is proposed for the kinematic model of two-degree-of-freedom unicycle-type mobile robots. The approach is then extended to consider the formation controller by taking into account the physical dimensions and dynamics of the robots. The controller is designed in such a way that the path derivative is left as a free input to synchronize the robot’s motion. Simulation results with three robots are included to show the performance of our control system. Finally, the theoretical results are experimentally validated on a multi-robot platform. © 2009 Elsevier B.V. All rights reserved.
1. Introduction During recent years, efforts have been made to give autonomy to single mobile robots by using different sensors, actuators and advanced control algorithms. This was mainly motivated by the necessity to develop complex tasks in an autonomous way, as demanded by service or production applications. In some applications, a valid alternative (or even the mandatory solution) is the use of multiple simple robots which, operating in a coordinated way, can develop complex tasks [1–3]. This alternative offers additional advantages, in terms of flexibility in operating the group of robots and failure tolerance due to redundancy in available mobile robots [4]. In the literature, there have been three main methods to formation control of multiple robots: leader following, behavioral and virtual structure. Each approach has its own advantages and disadvantages. In the leader following approach, some vehicles are considered as leaders, whilst the rest of robots in the group act as followers [1,5]. The leaders track predefined reference trajectories, and the followers track transformed versions of the states of their nearest neighbors according to given schemes. An advantage of the leader
∗ Corresponding author at: Department of Electrical and Computer Engineering, Sultan Qaboos University, P.O. Box 33, Muscat 123, Oman. E-mail addresses: [email protected] (J. Ghommam), [email protected] (H. Mehrjerdi), [email protected] (M. Saad), [email protected] (F. Mnif). 0921-8890/$ – see front matter © 2009 Elsevier B.V. All rights reserved. doi:10.1016/j.robot.2009.10.007
following approach is that it is easy to understand and implement. In addition, the formation can still be maintained even if the leader is perturbed by some disturbances. However, the disadvantage related to this approach is that there is no explicit feedback to the formation; that is, there is no explicit feedback from the followers to the leader in this case. The behavioral approach prescribes a set of desired behaviors for each member in the group, and weighs them such that desirable group behavior emerges without an explicit model of the subsystems or the environment. Possible behaviors include trajectory and neighbor tracking, collision and obstacle avoidance, and formation keeping. In [6], the behavioral approach for multi-robot teams is described where formation behaviors are implemented with other navigational behaviors to derive control strategies for goal seeking, collision avoidance and formation maintenance. The advantage is that it is natural to derive control strategies when vehicles have multiple competing objectives, and an explicit feedback is included through communication between neighbors. The disadvantages are that the group behavior cannot be explicitly defined, and it is difficult to analyze the approach mathematically and guarantee the group stability. In the virtual structure approach, the entire formation is treated as a single, virtual, structure and it acts as a single rigid body. The control law for a single vehicle is derived by defining the dynamics of the virtual structure and it then translates the motion of the virtual structure into the desired motion for each vehicle [7–9]. In [10], virtual structures have been achieved by having all members of the formation tracking assigned nodes which move into desired configuration. A formation feedback has been used to prevent
728
J. Ghommam et al. / Robotics and Autonomous Systems 58 (2010) 727–736
members leaving the group. Each member of the formation tracks a virtual element, while the motion of the elements is governed by a formation function that specifies the desired geometry of the formation. The main advantages of the virtual structure approach is that it is fairly easy to prescribe the coordinated behavior for the group, and the formation can be maintained very well during the maneuvers; that is, the virtual structure can evolve as a whole in a given direction with some given orientation and maintain a rigid geometric relationship among multiple vehicles. However, if the formation has to maintain the exact same virtual structure all the time, the potential applications are limited, especially when the formation shape is time varying or needs to be frequently reconfigured. In addition to the references mentioned so far, there have been many results on the mathematical analysis of formation control. In [11], the authors analyze the stability of the behavior of the first-order vehicles in a formation that relied on the Graph Laplacian associated with a given communication topology. Consensus algorithms applied to a team represented by a second-order dynamics are presented in [12] to guarantee attitude alignment and velocities in a group of vehicles using undirected communication information. New results on formation control based on second-order consensus protocols was given in [13]. In [14], new general potential functions are constructed to design formation controllers that yield semi-global asymptotic convergence of a group of mobile agents to a desired formation, and guarantee no collisions among the agents. However, most of those works model the agents as point robots and consider only the problem of position control of the robots. In numerous applications the orientation of the agents plays an important role, which means that the agents must be modeled as rigid bodies. Taking into account the position and orientation of the rigid bodies in a formation of a team of mobile robots, Breivik et al. [15] proposed a guided formation control scheme based on a modular design procedure that makes the design completely decentralized in the sense that no variables need to be communicated between the formation members. In [16], the problem of coordinated path following of multiple wheeled robots was solved by resorting to linearization and gain scheduling techniques. Even though the solution given to the problem is simple, it lacks global results; that is, convergence of the vehicles to their paths and to the desired formation pattern is only guaranteed locally. The challenge we are tackling in this paper is to design a nonlinear formation control law based on a virtual structure approach for the coordination of a group of N mobile robots [17]. This control law should force the robots’ relative positions with respect to the center of the virtual structure during the motion. The control system is derived in four stages. First, the dynamic of the virtual structure is defined. Second, the desired motion for each mobile robot is defined by translating by a given distance the motion of the virtual structure center. Third, the path following problem for each mobile robot is solved individually, by introducing a virtual target that propagates along the path. We interpret the path following errors in a triangular form, for which the backstepping technique can be applied to control the rate of progression of the virtual target along the path, and a local control law that makes the mobile robot track the virtual target. Finally, coordination is achieved by synchronizing the parametrization states that capture the positions of the virtual targets with the parametrization states of the virtual structure’s center. The formation of a simplified kinematic model of robots is first considered to clarify the design philosophy. The proposed technique is then extended to the dynamics of mobile robots with nonholonomic constraints. The experimental controller testing was performed on EtsRo vehicles, available at the GREPCI, Robotics Lab of the École de Technologie Supérieure of Montreal. The reminder of this paper is organized as follows. The problem statement is presented in Section 2. Section 3 is devoted to
Fig. 1. Formation structure.
modeling of the path following configuration for a wheeled mobile robot, while Sections 4 and 5 are the main part of this paper focussing on the study of the coordinated control for, respectively, a team of kinematic wheeled mobile robots and dynamic mobile robots. Simulation and experimental results are presented in Section 6. A conclusion is given in Section 7. 2. Problem statement 2.1. Kinematic model We consider a group of N mobile robots, each of which has the following equations of motion: x˙ i = vi cos(θi ) y˙ i = vi sin(θi )
θ˙i = ωi
(1)
where ηi = [xi , yi , θi ] denotes the position and the orientation vector of the ith robot of the group with respect to an inertial coordination frame (Fig. 1). vi and ωi stand for the linear and angular velocities, respectively. For the group to move in a prescribed formation, each member will require an individual parameterized reference path so that when all path parameters are synchronized the member i will be in formation. We can generalize the setup of a single path ξ (s) to n paths by defining the center of a virtual structure that moves along a given reference path ξ (s0 ) = [xd0 (s0 ), yd0 (s0 )], with s0 being a path parameter, and create a set of n designation vectors li (xd0 (si ), yd0 (si )) relative to the center of the structure. When the center of the virtual structure moves along the path ξ0 (s0 ), mobile robot i will then follow the individual desired path given by >
ξi (si ) = ξ0 (s0 ) + R(θd0 (si ))li (xd0 (si ), yd0 (si ))
(2)
where R(θd0 (si )) is the rotation matrix from a frame attached to the center of the virtual structure and the earth fixed frame, which is given as cos(θ0 (si )) R(θd0 (si )) = sin(θ0 (si ))
− sin(θ0 (si )) cos(θ0 (si ))
θd0 (si ) = arctan
s
i yd0
(3)
s
i xd0
where the partial derivative notations in [17,18] are used; i.e., s
i yd0 =
∂ yd0 (si ) , ∂ si
s
i and xd0 =
∂ xd0 (si ) . ∂ si
J. Ghommam et al. / Robotics and Autonomous Systems 58 (2010) 727–736
2.2. Control objective
where we have defined
Before going on with the design, we first give the following assumption.
q
s
s
xdii (si )2 + ydii (si )2 s˙i := v¯ di (si )˙si
vdi =
Assumption 1. 1. We assume that each mobile robot i of the formation broadcasts its state and reference trajectory to the rest of the robots in the group. Moreover, it can receive states and reference trajectories from the other robots of the group. 2. The desired geometric path is regularly parameterized; i.e., there exist strictly positive constants ε1i and ε2i , 1 ≤ i ≤ N such that s
(xdii )2 (s) + (ydii )2 (s) ≥ ε1i ,
s˙0 ≥ ε2i .
(4)
Remark 1. The second inequality of Eq. (4) means that the center of the virtual structure moves forward. The overall control objective is to solve the formation problem for N mobile robots which consists of two parts [17]. The geometric task ensures that an individual mobile robot converges to and stays at its designation in the formation. The dynamic task ensures that the formation maintains a speed along the path according to the given speed assignment. The formation control objective boils down to design controller vi and ωi such that
ωdi =
s2i
s2i
s
s
729
s
xdii (si )ydi (si ) − xdi (si )ydii (si ) s
s
(9)
xdii (si )2 + ydii (si )2
s˙i := ω ¯ di (si )˙si .
4. Control design In the tracking model (8), yei could not be directly controlled, and to overcome this difficulty, we employ the backstepping approach [19]. Define the following variable: s˙˜i = s˙i + $i (t , xe , ye , θe )
(11)
where xe = [xe1 , xe2 , . . . , xen ]> , ye = [ye1 , ye2 , . . . , yen ]> and θe = [θe1 , θe2 , . . . , θen ]> ; $i (t , xe , ye , θe ) is a strictly positive function that specifies how fast the ith mobile robot should move to maintain the formation since s˙i is related to the desired forward speed vdi . Step 1. Design a controller to stabilize the xei dynamic. The variable s˙˜i should be left as an extra controller in order to synchronize all the path parameters of each mobile robot in the formation. We therefore choose the following control for xei :
vi = −k1i xei + v¯ di $ cos(θei ).
lim kηi (t ) − ηdi (t )k = 0
t →∞
lim |si (t ) − s0 (t )| = 0
(5)
t →∞
where ηid denotes the ith reference path to be tracked by robot i. The following lemma [19] will be useful later. Lemma 1. Consider a scalar system x˙ = −kx + f (t ) where k > 0 and f (t ) is a bounded and uniformly continuous function. If, for any initial t0 ≥ 0 and any initial condition x(t0 ), the solution x(t ) is bounded and converges to 0 as t → ∞, then lim f (t ) = 0.
t →∞
Proof. The result follows using the Bellman–Grown inequality lemma and the triangular inequality.
(10)
(12)
In a closed loop, the xei dynamic can be rewritten as x˙ ei = −k1i xei − v¯ di s˙˜i cos(θei ) + yei ωi .
(13)
From (13), it is seen that, if s˙˜i and yei are zero, then xei globally exponentially converges to zero. The next step of the design will focus on designing a controller to stabilize θei at the origin. Step 2. Design a controller for ωi . Consider the following Lyapunov function: n 1X
(x2ei + y2ei + θei2 + γi (si − s0 )2 ) (14) 2 i=1 where γi is a positive constant. The time derivative of (14) along the solutions of (13) and (8) yields V =
V˙ =
n X
xei (−k1i xei − v¯ di s˙˜i cos(θei ) + yei ωi )
i=1
3. Path following error dynamic
+
xei yei
ψei
#
" =
cos(θi ) − sin(θi ) 0
sin(θi ) cos(θi ) 0
0 0 1
xi − xdi yi − ydi
#"
θi − θdi
#
θdi = arctan
si
ydi s xdii
.
θei (ωi − ω¯ di (s˙˜ + $i ))
i=1
+
n X
γi (si − s0 )(s˙˜i + $i − s˙0 ).
(15)
i=1
$i = s˙0 ,
V˙ = (7)
(16)
n X
" −
k1i x2ei
+ θei yei v¯ di $i
1
Z
cos(λθei )dλ 0
i=1
# + ωi − ω¯ di $i +
" n X
−¯vdi cos(θei )xei
i =1
x˙ ei = vi − vdi cos(θei ) + yei ωi
θ˙ei = ωi − ωdi
n X
the derivative of the Lyapunov function V becomes
The error dynamics are then y˙ ei = vdi sin(θei ) − xei ωi
+
If we select (6)
where θdi is defined as
yei (¯vdi (s˙˜i + $i ) sin(θei ) − xei ωi )
i=1
The problem we consider here is the path following for each mobile robot in the formation; that is, we wish to find the control law vi and ωi such that the robot follows a reference point in the path with position ηdi = [xdi , ydi , θdi ]> and inputs vdi and ωdi . The path error is therefore interpreted in a frame attached to the reference path ξ (si ). Following [20], we define the error coordinates
"
n X
(8)
# + yei v¯ di sin(θei ) − θei ω¯ di + γi (si − s0 ) s˙˜i .
(17)
730
J. Ghommam et al. / Robotics and Autonomous Systems 58 (2010) 727–736
To make the derivative of the Lyapunov function V negative, we choose the following controllers:
ωi = −k2i θei − yei v¯ di $i
1
Z
cos(λθei )dλ + ω ¯ di $i 0
s˙˜i = −εω tanh −¯vdi cos(θei )xei + yei v¯ di sin(θei )
− θei ω¯ di + γi (si − s0 )
(18)
where εω is a positive constant to be selected later. Substituting the equations of (18) into (17) yields V˙ =
n X
−k1i x2ei − k2i θei2 − εω φi tanh(φi )
(19)
i =1
From the definition of V , the right-hand side of (24) is bounded by a positive constant depending on the initial conditions. The boundedness of the left-hand side of (24) also implies those of xei , yei , θei and si − s0 for all t ≥ t0 ≥ 0. The assumed boundedness of ω0 (t ) implies that of the right-hand side of the last equation of (23) depends continuously on t through bounded functions. It follows that si and therefore s0 are bounded on the maximal interval of definition [0, T ); this excludes finite escape time, so T = +∞. This in turn implies by construction that xi (t ), yi (t ), θi (t ), si and s0 do not go to infinity in finite time. (Stability of (xei , yei , θei )) From the above argument on the boundedness of (xei , yei , θei , si − s0 ), applying Barbalt’s lemma [22] to (19) results in lim (xei , θei ) = 0
t →∞
where φi = −¯vdi cos(θei )xei + yei v¯ di sin(θei ) − θei ω ¯ di + γi (si − s0 ). We are able now to choose an appropriate function for s˙0 such that, when the tracking errors are large, the center of the virtual structure will wait for the rest of the group; however, when the errors converge to zero, we impose s˙0 to approach a given positive bounded function ω0 (t ), which means that the center of the virtual structure will move at a desired speed. s˙0 can be chosen as, among many others, [21] s˙0 = ω0 (t )(1 − κ1 e−κ2 (t −t0 ) )(1 − κ3 tanh(x> e Γx xe > + y> e Γy ye + θe Γθ θe ))
lim φi tanh(φi ) = 0.
(25)
t →∞
On the other hand, from the second equation of (25), we conclude that lim φi = 0
t →∞
lim s˙˜i = 0.
(26)
t →∞
From this fact, we also conclude that (20)
lim (si − s0 ) = 0,
(27)
t →∞
where ω0 (t ) is a strictly positive and bounded function. This function specifies how fast the whole group of robots should move. Γx , Γy and Γθ are weighting positive matrices. All the constants κ1 , κ2 and κ3 are nonnegative, and κ1 and κ3 should be less than 1. Now, if we choose the constant εω such that
which satisfies the second objective of (5). To satisfy the first objective of (5), we have to show that yei also converges to zero as t goes to infinity. In a closed loop the θei dynamic can be rewritten as follows:
εω < ω0 (t )(1 − κ1 )(1 − κ3 )
θ˙ei = −k2i θei − yei v¯ di $i
(21)
cos(λθei )dλ − ω ¯ di s˙˜i .
(28)
0
then we have Let f (t ) = −yei v¯ di $i it follows that
s˙i = −εω tanh(φi ) + ω0 (t )(1 − κ1 e−κ2 (t −t0 ) ) > > × (1 − κ3 tanh(x> e Γx xe + ye Γy ye + θe Γθ θe ))
≥ −εω + ω0 (t )(1 − κ1 )(1 − κ3 ) > 0.
(22)
From the above control design, we have the closed loop system x˙ ei = −k1i xei − v¯ di s˙˜i cos(θei ) + yei ωi y˙ ei = vdi sin(θei ) + k2i θei xei + yei xei v¯ di
× $i
1
Z
1
Z
cos(λθei )dλ − xei ω ¯ di $i
(23)
lim yei v¯ di $i
t →∞
Z
R1 0
cos(λθei )dλ− ω ¯ di s˙˜i ; according to Lemma 1,
1
cos(λθei )dλ = 0
0
R1
since limt →∞ 0 cos(λθei )dλ = 1, then limt →∞ yei = 0, which concludes the first objective of (5). 5. Extension of formation control to the dynamic model of the mobile robot
0
θ˙ei = −k2i θei − yei v¯ di $i
1
Z
In this section, we study the augmented system (8) appended with a dynamic model of a nonholonomic mobile robot [23].
cos(λθei )dλ − ω ¯ di s˙˜i 0
s˙i = −εω tanh(φi ) + ω0 (t )(1 − κ1 e−κ2 (t −t0 ) )
x˙ ei = vi − vdi cos(θei ) + yei ωi
× (1 − κ3 tanh(xe Γx xe + ye Γy ye + θe Γθ θe )). >
>
>
y˙ ei = vdi sin(θei ) − xei ωi
We now state the main result of the formation control for the kinematic model of unicycle-type mobile robots.
(29)
θ˙ei = ωi − ωdi M i ν˙ i = −C i (ωi )νi − Di νi + Bi τi
Theorem 2. Under Assumption 1, the control inputs vi and ωi given in (12) and (18) and the time derivative of each individual path s˙i given in (22) for the mobile robot i solve the formation control objective. In particular, the closed loop system is forward complete, and the position and orientation of the robots track their path reference asymptotically in the sense of (5).
where νi = [vi , ωi ]> , τi = [τ1i , τ2i ]> are the control vector torques applied to the wheels of the robot i. The modified mass matrix, Coriolis and Damping matrices are given by
Proof (Forward Completness). From (19), we have that V˙ ≤ 0, which implies that
where Bi is an invertible matrix given by Bi =
V (t ) ≤ V (t0 ),
bi , m11i , m22i , d11i , d22i and ci are the dynamic parameters of each
∀t ≥ t 0 .
(24)
1 M i = B− i Mi Bi ,
1 C i (ωi ) = B− i Ci (ωi )Bi
1 Di = B− i Di Bi
M =
h
m11i m12i
m12i m11i
i
, Ci (ωi ) =
h
0 −ci ωi
ci ωi 0
i
, Di =
h
h
1 1
d11i 0
bi −bi 0 d22i
i
i
and with
J. Ghommam et al. / Robotics and Autonomous Systems 58 (2010) 727–736
731
Fig. 3. The three EtsRo mobile robots in the experiments. Fig. 2. Interaction between the host computer and the robots.
group, which are not necessarily know. Definitions of these parameters are given in [23]. In this section we intend to show that the formation control developed for the kinematic model can also be obtained for the complete dynamic of unicycle-type mobile robots. The control objective is thereby similar to that of (5) and consists of finding the controller τi that satisfies all the conditions of (5). We introduce the following new variables:
νei = νi − ανi ,
vei = vi − αvi ,
⇒
ωei = ωi − αωi
(30)
where αvi and αωi are defined as in (12) and (18), respectively. Following the notation in Section 4, in the new coordinates (xei , yei , θei , vei , ωei ) the system (29) is transformed into x˙ ei = −k1i xei − v¯ di s˙˜i cos(θei ) + yei ωi + vei
× $i
proj(π , µ) ˆ = π,
if, Ξ (µ) ˆ ≤0
proj(π , µ) ˆ = π,
if, Ξ (µ) ˆ ≥ 0 and Ξµˆ (µ) ˆ ≤0
proj(π , µ) ˆ = (1 − Ξ (µ))π ˆ ,
if, Ξ (µ) ˆ > 0 and Ξµˆ (µ) ˆ >0
where Ξ (µ) ˆ = (µ ˆ 2 − µ2M )/(k2 + 2kµM ), Ξµˆ (µ) ˆ =
∂Ξ
ˆ µ ˆ (µ) ∂µ ˆ
,k
is an arbitrarily small positive constant, ω ˆ is an estimate of µ and ˙ˆ = proj(π , µ) |µ| ≤ µM . The projection algorithm is such that if µ and µ( ˆ t0 ) ≤ µM then (a) µ( ˆ t ) ≤ µM + k, ∀0 ≤ t0 ≤ t ≤ ∞ (b) proj(π , µ) ˆ is Lipschitz continuous
y˙ ei = vdi sin(θei ) + k2i θei xei + yei xei v¯ di
(c) |proj(π , µ)| ˆ ≤ |π |
1
Z
where the operator, proj, is the Lipschitz continuous projection [24] algorithm applied in our case as follows:
cos(λθei )dλ − xei ω ¯ di $i − xei ωei
(d) µ ˜ proj(π , µ) ˆ ≥ µπ ˜ with µ ˜ =µ−µ ˆ
0
θ˙ei = −k2i θei − yei v¯ di $i
Z
1
cos(λθei )dλ − ω ¯ di s˙˜i + ωei
(31)
and K1i is a symmetric positive definite matrix. Using Property (d) of the projection algorithm results in
0
M νei = −C i (ωi )νi − Di νi − M i [α˙ vi , α˙ ωi ]> + Bi τi
V˙ 2 ≤ −
= −Di νei + Φi Θi + Bi τi
Φi =
0
Θi = bi ci
−αvi
−α˙ vi
0
0
d11i
0
0
−ωi vi
m11i + m12i
−αωi ci bi
0
+ νei> (Di + K1i )νei .
−α˙ ωi >
d22i
m11i − m12i
. (32)
N
V2 = V +
2
νei M i νei + Θi Γi Θi >
e>
−1 e
(33)
i
ei = Θi − Θ bi , with Θ bi being an estimate of Θi and Γi a symwhere Θ metric positive definite matrix. Differentiating both sides of (33) along the solutions of (31) and (17) yields a non-negative function. This suggests that we choose for the control input τi , the path bi the following parameter si and the updated law Θ bi − xei τi = B−1 −K1i νei − Φi Θ
θei
>
x˙ ei = −k1i xei − v¯ di s˙˜i cos(θei ) + yei ωi + vei y˙ ei = vdi sin(θei ) + k2i θei xei + yei xei v¯ di
× $i
1
Z
cos(λθei )dλ − xei ω ¯ di $i − xei ωei 0
θ˙ei = −k2i θei − yei v¯ di $i
1
Z
cos(λθei )dλ − ω ¯ di s˙˜i + ωei 0
ei − xei M νei = −(Di + K1i )νei + Φi Θ
θei
>
s˙i = −εω tanh(φi ) + ω0 (t )(1 − κ1 e−κ2 (t −t0 ) )
> > × (1 − κ3 tanh(x> e Γx xe + ye Γy ye + θe Γθ θe ))
˙ = −Γ proj(Φ > ν , Θ e bi ). Θ i i ei i
(36)
Theorem 3. Under Assumption 1, the control inputs τi and the
˙ = Γ proj(Φ > ν , Θ b bi ) Θ i i ei i s˙i = −εω tanh(φi ) + ω0 (t )(1 − κ1 e−κ2 (t −t0 ) )
> > × (1 − κ3 tanh(x> e Γx xe + ye Γy ye + θe Γθ θe ))
(35)
From the above control design, we have the closed loop system
At this stage, we design the actual control input vector τi and updated laws for the unknown system parameter vector Θi for each robot i. To do so, we consider the following Lyapunov function: 1X
k1i x2ei + k2i θei2 + εω φi tanh(φi )
i=1
where
2 ωi
n X
(34)
˙ given in (34) for the mobile robot i solve the formation b update law Θ i control objective. In particular, the closed loop system (36) is forward complete and all signals in the closed loop system are Uniformly Ultimately Bounded (UUB).
732
J. Ghommam et al. / Robotics and Autonomous Systems 58 (2010) 727–736
(a) Desired and actual vehicle’ trajectories.
(b) Path following errors xei .
(d) Heading errors θei .
(c) Path following errors yei .
(e) Path parameter error in the form
qP 3
i=1
(si − s0 )2 .
(f) Time evolution of the forward velocity of each robot.
Fig. 4. Coordination of three wheeled robots along a sinusoidal path.
bi> , si ]> . = [xei , yei , θei ]> and Zi = [Xei> , νei> , Θ P N e > −1 Θ ei to Considering (35), by subtracting and adding 21 i=1 Θi Γi
where p qi is a constant that depends on the elements of Γi , and
its right-hand side, we get
guaranteed to be UUB. The assumed boundedness of ω0 (t ) implies that the right-hand side of (36) depends continuously on time t through a bounded function. With Zi being bounded, it follows that (36) is bounded on the maximal interval of definition. This excludes finite escape times.
Proof. Let Xei
V˙ 2 ≤ δ V2 + ρ
(37)
where δ = min(1, 2µ1 , . . . , µN ), µi = min(k1i , k2i , εω ) and ρ = PN 1 e > −1 Θ ei . From (37), it is straightforward to show that i=1 Θi Γi 2 V2 (t ) ≤ V2 (t0 )e−δ(t −t0 ) +
ρ . δ
(38)
This implies that, for all t in the maximal interval of definition [0, T ),
kZi k ≤ pi kZi (t0 )ke−0.5δ(t −t0 ) + ρi
(39)
ρi =
ρ . δ
Consequently, all the signals in the closed-loop are
6. Simulation and experimental tests In this section, we provide simulation and experimental results to validate the theoretical results of Sections 4 and 5. For the simulations and experiments, the number of robots in the formation group is chosen for simplicity of implementation to be N = 3, and
J. Ghommam et al. / Robotics and Autonomous Systems 58 (2010) 727–736
(a) Desired and actual vehicle trajectories.
733
(b) Time varying distance between the center of the virtual structure and the robots.
Fig. 5. Maneuvering coordination of three wheeled robots along different paths.
the interactions between the host computer and the robots are assumed to be bidirectional, as in Fig. 2. The experimental platform, implementation of the algorithm, and experimental results will be described in detail. 6.1. Numerical simulations To illustrate the dynamic performance of the proposed coordination control scheme in Section 4, numerical simulations were carried out. The three mobile robots assemble and maintain an in-line formation; that is, the three robots are requested to follow some regular paths with respect to the path parameter si by having them aligned along a common vertical line. In the case when the paths are not regular, one can break them into different regular paths and consider each path separately. Hereafter, we propose two different scenarios for coordination. In the first one, we simulate the coordination control to execute an in-line formation along three sinusoidal paths by having them aligned along a common vertical line. Next, a different kind of formation maneuver of three mobile robots is presented, in which one robot is required to follow the x-axis, while the two others must follow a sinusoidal path, while the three agents keep an in-line formation along the y-axis. For both scenarios, the control gains, the initial conditions, and the parameters involved the update of the path parameters are k1 = 150, k2 = 70, εω = 0.12, ω0 = 5, κ1 = 0.5, κ2 = 2, κ3 = 0.5, si (0) = 2, [x1 (t0 ), y1 (t0 ), θ1 (t0 )]> = [−4, 2, π4 ]> , [x2 (t0 ), y2 (t0 ), θ2 (t0 )]> = [−3.5, −1, π4 ]> , [x3 (t0 ), y3 (t0 ), θ3 (t0 )]> = [−3.5, 0.5, π4 ]> , and Γx = Γy = Γθ = diag(1, 1, 1). In the first scenario, the reference path for the center of the virtual structure is taken as xd0 = 0.1s0 − 3.5 and yd0 = 0.5 cos(0.1s0 ). The distances from the vehicles to the center of the virtual structure are chosen as l1 (xd0 (s1 ), yd0 (s1 )) = (0, 0), l2 (xd0 (s2 ), yd0 (s2 )) = (0.1s2 − 3.5, 0.5 cos(0.1s2 ) − 1) and l3 (xd0 (s3 ), yd0 (s3 )) = (0.1s1 − 3.5, 0.5 cos(0.1s1 ) + 1). Clearly the choice for the distance l1 means that the first robot coincides with the center of the virtual structure. In Fig. 4(a), the vehicles are progressing successfully towards their paths and the vehicles ultimately attain their desired formation configuration. The path tracking and path parameter errors are plotted in Fig. 4(b), (c), (d) and (e), respectively. Notice in Fig. 4(f) how the vehicles adjust their speed along the path so as to achieve coordinations. This could be explained as follows. During the first few seconds, the vehicles quickly move to reach their trajectories; once they keep moving on their paths, each vehicle slows down to wait for its neighbors in order to synchronize as long as they propagate. Fig. 5(a) depicts a typical formation maneuver involving three robots along different paths. This example captures the situation where a vehicle (moving in straight line) directs two other robots
Fig. 6. Path references and real robot trajectories.
to inspect two distinct symmetric areas along a fixed direction. This particular formation shows the novelty of the method adopted using the virtual structure approach, since we allow the distances between robots to change (see Fig. 5(b)). 6.2. Experimental validation In this section, we provide experimental results to validate the theoretical results of Section 5 on a team of three mobile robots. 6.2.1. Robot platform and program implementation To show the performance of the proposed controllers, several experiments were executed. The proposed controllers were implemented on a team of EtsRo mobile robot platforms. The EtsRo vehicle is available at the Robotics Lab of the École de Technologie Supèrieure in Montreal, supplied with an automatic navigation system. A picture of a team of three EtsRo robots is given in Fig. 3. EtsRo is a four-wheel car-like mobile robot, equipped with two driving front wheels and DC motors. The EtsRo sensor equipment consists of high precision wheel encoders giving the relative localization of the vehicle, mounted on the motors counting (6000 pulses/turn). Within each robot there is a 32-bit microcontroller ATmega32 that maintains support for all sensor and actuation features of the robot and manages the navigation, guidance and control of the vehicle. A control program is sent remotely by a Zigbee USB-Modem to the EtsRos. The range of the modem is about 100 m, which makes it a high quality modem for indoor applications. The host computer executes programs which transmit the desired posture and the control commands to the EtsRos
734
J. Ghommam et al. / Robotics and Autonomous Systems 58 (2010) 727–736
Fig. 7. Path following errors xei .
Fig. 10. Evolution of the forward velocities in the first scenario.
Fig. 8. Path following errors yei .
Fig. 11. Evolution of the angular velocities in the first scenario.
Fig. 9. Heading errors θei .
through an on-board micro-controller. The communication delay between the host computer and each robot is evaluated as 100 ms. Two level controller architectures are designed to navigate and steer the EtsRo robots. The lower one is a PID controller, responsible for adjusting the velocity of right and left wheels. To filter sparks and unwanted noises on the velocities, a second-order filter was designed and implemented. The higher level one is a nonlinear controller, and it is designed to navigate and steer the mobile robots with regard to the desired coordination. 6.2.2. Experimental results The simulations presented in Section 5 do not take into account the dynamics of the robots. EtsRo robots are actuated with DC motors that are characterized by a relatively poor transient responses. Moreover, the left and right motors may not have the same response due to the nonhomogeneous mass distribution over the robot body. For this reasons, the coordinated controller in Section 5
Fig. 12. Path references and real robot trajectories.
was implemented on the testbed program. The robots’ configuration was used as before, but the controller gains were set with smaller values since the physical robots are very sensitive to gains in actuation. Therefore the control gains k1i = 0.04, k2i = 2 and Ki = diag(25, 35) were implemented to prevent the control inputs from saturating the motors. The environment in which the robots can evolve is a rectangular arena, 4 m × 7 m. Assuming that the three robots are identical, the nominal robot parameters (which were obtained via identification) are given in Table 1. The expressions for each element of the mass matrix are given as 2 2 2 m11i = 0.25b− i ri (mi bi + Ii ) + Iw i , 2 2 2 m12i = 0.25b− i ri (mi bi − Ii )
mi = mci + 2mwi ,
Ii = mci a2i + 2mwi b2i + Ici + 2Imi .
J. Ghommam et al. / Robotics and Autonomous Systems 58 (2010) 727–736
Fig. 13. Evolution of the forward velocities in the second scenario. Table 1 Parameters of the EtsRo mobile robot. Parameters
Values
Unit
ri bi ai mci mw i Ic i Iw i Imi
0.04 0.1 0.02 2.3 0.28 0.01 0.0056 0.002
(m) (m) (m) (kg) (kg) (kg m2 ) (kg m2 ) (kg m2 )
In the experiment, the trajectories to follow by the robots are as described in simulations section. The initial positions and orientations for the robots remained at [x1 (t0 ), y1 (t0 ), θ1 (t0 )]> = [−4.5 m, 1.5 m, 0 rad]> , [x2 (t0 ), y2 (t0 ), θ2 (t0 )]> = [−3.5 m, −1.5 m, 0 rad]> , [x3 (t0 ), y3 (t0 ), θ3 (t0 )]> = [−4.5 m, 1.5 m, 0 rad]> . We ran two tests for different scenarios. In the first, the robots are required to follow three parallel sinusoidal paths, while in the second, one of the robot coincides with the center of the virtual structure which moves along a straight line and the other two robots move on two sinusoidal paths. Fig. 6 presents the reference and the actual robot trajectories. The path tracking errors xi − xdi , yi − ydi are shown in Figs. 7 and 8. Indeed, these errors exhibit a steady-state value of 0.2 m; this is due to the dependence of the steady errors upon the loop gain, and because we introduced small values for the control gains (to avoid saturating the motors), the errors drifted away from zero. Fig. 9 reports the orientation error θi − θdi . Linear and angular velocities are plotted in Figs. 10 and 11 respectively; these show that the robots travel along their path with a common velocity, and the formation is experimentally successful. In the second experimental scenario, the first mobile robot is forced to move on a straight line which coincides with the center of the virtual structure, and the other two robots move on two opposite sinusoidal paths (see Fig. 12). The experimental linear and angular velocities in this scenario are reported in Figs. 13 and 14, respectively. Note from Fig. 13 how the first robot moves with a constant velocity while the other two robots move with varying velocity to preserve the formation. 7. Conclusion This paper has proposed a methodology for formation control of a group of unicycle-type mobile robots represented at a kinematic level and a dynamic level. The approach that has been developed is mainly based on a combination of the virtual structure and path following approaches. The controller is designed in such a way that the derivative of the path parameter is left as an additional control input to synchronize the formation motion. Real-time experiments on a multi-robot platform showed the effectiveness of the theoretical results. Future work is to design an observer for both kinematic
735
Fig. 14. Evolution of the angular velocities in the second scenario.
and dynamic models to estimate unavailable signals such as velocities or orientation error tracking. Moreover, the impact of sensor noise on the system performance can be further alleviated by using state filter estimators. References [1] P.K.C. Wang, Navigation strategies for multiple autonomous robots moving in formation, Journal of Robotic Systems 8 (2) (1992) 177–195. [2] T.D. Barfoot, C.M. Clark, Motion planning for formations of mobile robots, Journal of Robotics and Autonomous Systems 46 (2) (2004) 65–78. [3] Q. Chen, J.Y.S. Luh, Coordination and control of a group of small mobile robots, in: Proc. IEEE Int. Conf. Robotics and Automation, San Diego, CA, USA, 1994, pp. 2315–2320. [4] Y. Ishida, Functional complement by cooperation of multiple autonomous robots, in: Proc. IEEE Int. Conf. Robotics and Automation, San Diego, CA, USA, 1994, pp. 2476–2481. [5] S. Sheikholeslam, C. Desoer, Control of interconnected nonlinear dynamical systems: The platoon problem, IEEE Transactions on Automatic Control 37 (1992) 806–810. [6] T. Balch, R.C. Arkin, Behavior-based formation control for multirobot teams, IEEE Transactions on Robotics and Automation 14 (6) (1998) 926–939. [7] K.D. Do, Formation tracking control of unicycle-type mobile robots, in: Proc. of Robotics and Automation Conf., Roma, 2007. [8] X. Li, J. Xiao, Z. Cai, Backstepping based multiple mobile robots formation control, in: Proc. IEEE Int. Conf. Intelligent Robots and Systems, Roma, 2005. [9] T. Dierks, S. Jagannathan, Control of nonholonomic mobile robot formations: Backstepping kinematics into dynamics, in: Proc. of the Int. Conference on Control Application, Singapore, 2007. [10] R. Beard, J. Lawton, Hadaegh, A coordination architecture for spacecraft formation control, IEEE Transactions on Control Systems Technology 9 (1999) 777–790. [11] A. Fax, R.M. Murray, Information flow and cooperative control of vehicle formations, IEEE Transactions on Automatic Control 50 (2004) 1465–1476. [12] J. Lawton, W. Beard, Synchronized multiple spacecraft rotations, Journal of Automatica 8 (38) (2002) 1359–1364. [13] W. Ren, On consensus algorithms for double-integrator dynamics, IEEE Transactions on Automatic Control 6 (53) (2008) 1503–1509. [14] K. Do, Bounded controllers for formation stabilization of mobile agents with limited sensing ranges, IEEE Transactions on Automatic Control 3 (52) (2007) 569–576. [15] M. Breivik, T. Fossen, Guided formation control for wheeled mobile robots, in: Proc. of 9th IEEE Conference on Control, Automation, Robotics and Vision, Singapore, 2006, pp. 1–7. [16] R. Ghabcheloo, A. Pascoal, A. Silvestre, C. Kaminer, Coordinated path following control of multiple wheeled robots using linearization techniques, International Journal of Systems Science 37 (2006). [17] R. Skjetne, S. Moi, T. Fossen, Nonlinear formation control of marine craft, in: Proc. of the 41st Conf. on Decision and Contr., Las Vegas, NV, 2002. [18] R. Skjetne, T. Fossen, P.V. Kokotovic, Robust output maneuvering for a class of nonlinear systems, Journal of Automatica 40 (2004) 373–383. [19] Z.P. Jiang, H. Nijmeijer, Tracking control of mobile robots: A case study in backstepping, Journal of Automatica 33 (1997) 1393–1399. [20] Y.K. Kanayama, F. Miyazaki, T. Noguchi, A stable tracking control method for an autonomous mobile robot, in: Proc. of Robotics and Automation Conf., Las Vegas, NV, 1990. [21] K.D. Do, J. Pan, Nonlinear formation control of unicycle-type mobile robots, Journal of Robotics and Autonomous Systems 55 (2007) 191–204. [22] H.K. Khalil, Nonlinear Systems, Macmillan, New York, 1992. [23] T. Fukao, H. Nakagawa, N. Adachi, Adaptive tracking control of nonholonomic mobile robot, IEEE Transactions on Robotics And Automation 16 (2000) 609–615. [24] J. Pomet, L. Praly, Adaptive nonlinear regulation: Estimation from the Lyapunov equation, IEEE Transactions on Robotics And Automation 37 (1992) 729–740.
736
J. Ghommam et al. / Robotics and Autonomous Systems 58 (2010) 727–736
Jawhar Ghommam was born in Tunis, Tunisia, in 1979. He received his B.Sc. degree from Institut Nationale des Sciences Appliquées et de Technologies (INSAT), Tunisia, in 2003, his M.Sc. degree in Control Engineering from the Laboratoire d’Informatique, de Robotique et de Microelectronique (LIRMM), Montpellier, France, in 2004, and his Ph.D. in Control Engineering and Industrial Computing in 2008, jointly from the Université of Orléans, France, and Ecole Nationale d’Ingénieurs de Sfax, Tunisia. He is an Assistant Professor of Control Engineering at the Institut Nationale des Sciences Appliquées et de Technologies (INSAT), Tunisia. He is a Member of the research unit on MEChatronics and Autonomous systems (MECA). His research interests include nonlinear control of underactuated mechanical systems, adaptive control, guidance and control of underactuated ships and cooperative motion of nonholonomic vehicles. Hasan Mehrjerdi received his B.Sc. and M.Sc. in Electrical Engineering from Ferdowsi University of Mashhad and Tarbiat Modares University of Tehran, respectively. He is currently pursuing a Ph.D. in Electrical and Robotics Engineering as a member of the GREPCI lab at the Université du Québec (Ecole de technologie supérieure). His research interests include mobile robotics, artificial intelligence, mobile sensor networks and mobile robot coordination in known and unknown environments.
Maarouf Saad received his Bachelor and Master degrees in Electrical Engineering from Ecole Polytechnique of Montreal in 1982 and 1984, respectively. In 1988, he received his Ph.D. in Electrical Engineering from McGill University. He joined Ecole de technologie supérieure in 1987, where he is teaching control theory and robotics courses. His research is mainly in nonlinear control and optimization applied to robotics and flight control system.
Faïçal Mnif received his M.Sc. and Ph.D. degrees in Control and Robotics from the Ecole Polytechnique de Montreal, in 1991 and 1996, respectively. Dr. Mnif is an Associate Professor of Control Engineering and Robotics at the National Institute of Applied Sciences and Technology, Tunis, Tunisia. He is currently on leave to Sultan Qaboos University, Oman, where he is holding a faculty position with the Department of Electrical and Computer Engineering. He is also a Member of the Research Unit on Mechatronics and Autonomous Systems, Tunisia. His main research interests include robot modeling and control, control of autonomous vehicles, modeling and control of holonomic and nonholonomic mechanical systems, and robust and nonlinear control.
Contents lists available at ScienceDirect
Robotics and Autonomous Systems journal homepage: www.elsevier.com/locate/robot
Formation path following control of unicycle-type mobile robots Jawhar Ghommam a , Hasan Mehrjerdi b , Maarouf Saad b , Faiçal Mnif a,c,∗ a
Research unit on Mechatronics and Autonomous Systems, École Nationale d’Ingénieurs de Sfax (ENIS), BP W, 3038 Sfax, Tunisia
b
Department of Electrical Engineering, École de technologie supérieure, 1100, rue Notre-Dame Ouest, Montréal, Québec, H3C 1K3, Canada
c
Department of Electrical and Computer Engineering, Sultan Qaboos University, P.O. Box 33, Muscat 123, Oman
article
info
Article history: Received 30 November 2008 Received in revised form 27 October 2009 Accepted 29 October 2009 Available online 12 November 2009 Keywords: Path following Formation control Virtual structure Nonholonomic robot Adaptive control
abstract This paper presents a control strategy for the coordination of multiple mobile robots. A combination of the virtual structure and path following approaches is used to derive the formation architecture. A formation controller is proposed for the kinematic model of two-degree-of-freedom unicycle-type mobile robots. The approach is then extended to consider the formation controller by taking into account the physical dimensions and dynamics of the robots. The controller is designed in such a way that the path derivative is left as a free input to synchronize the robot’s motion. Simulation results with three robots are included to show the performance of our control system. Finally, the theoretical results are experimentally validated on a multi-robot platform. © 2009 Elsevier B.V. All rights reserved.
1. Introduction During recent years, efforts have been made to give autonomy to single mobile robots by using different sensors, actuators and advanced control algorithms. This was mainly motivated by the necessity to develop complex tasks in an autonomous way, as demanded by service or production applications. In some applications, a valid alternative (or even the mandatory solution) is the use of multiple simple robots which, operating in a coordinated way, can develop complex tasks [1–3]. This alternative offers additional advantages, in terms of flexibility in operating the group of robots and failure tolerance due to redundancy in available mobile robots [4]. In the literature, there have been three main methods to formation control of multiple robots: leader following, behavioral and virtual structure. Each approach has its own advantages and disadvantages. In the leader following approach, some vehicles are considered as leaders, whilst the rest of robots in the group act as followers [1,5]. The leaders track predefined reference trajectories, and the followers track transformed versions of the states of their nearest neighbors according to given schemes. An advantage of the leader
∗ Corresponding author at: Department of Electrical and Computer Engineering, Sultan Qaboos University, P.O. Box 33, Muscat 123, Oman. E-mail addresses: [email protected] (J. Ghommam), [email protected] (H. Mehrjerdi), [email protected] (M. Saad), [email protected] (F. Mnif). 0921-8890/$ – see front matter © 2009 Elsevier B.V. All rights reserved. doi:10.1016/j.robot.2009.10.007
following approach is that it is easy to understand and implement. In addition, the formation can still be maintained even if the leader is perturbed by some disturbances. However, the disadvantage related to this approach is that there is no explicit feedback to the formation; that is, there is no explicit feedback from the followers to the leader in this case. The behavioral approach prescribes a set of desired behaviors for each member in the group, and weighs them such that desirable group behavior emerges without an explicit model of the subsystems or the environment. Possible behaviors include trajectory and neighbor tracking, collision and obstacle avoidance, and formation keeping. In [6], the behavioral approach for multi-robot teams is described where formation behaviors are implemented with other navigational behaviors to derive control strategies for goal seeking, collision avoidance and formation maintenance. The advantage is that it is natural to derive control strategies when vehicles have multiple competing objectives, and an explicit feedback is included through communication between neighbors. The disadvantages are that the group behavior cannot be explicitly defined, and it is difficult to analyze the approach mathematically and guarantee the group stability. In the virtual structure approach, the entire formation is treated as a single, virtual, structure and it acts as a single rigid body. The control law for a single vehicle is derived by defining the dynamics of the virtual structure and it then translates the motion of the virtual structure into the desired motion for each vehicle [7–9]. In [10], virtual structures have been achieved by having all members of the formation tracking assigned nodes which move into desired configuration. A formation feedback has been used to prevent
728
J. Ghommam et al. / Robotics and Autonomous Systems 58 (2010) 727–736
members leaving the group. Each member of the formation tracks a virtual element, while the motion of the elements is governed by a formation function that specifies the desired geometry of the formation. The main advantages of the virtual structure approach is that it is fairly easy to prescribe the coordinated behavior for the group, and the formation can be maintained very well during the maneuvers; that is, the virtual structure can evolve as a whole in a given direction with some given orientation and maintain a rigid geometric relationship among multiple vehicles. However, if the formation has to maintain the exact same virtual structure all the time, the potential applications are limited, especially when the formation shape is time varying or needs to be frequently reconfigured. In addition to the references mentioned so far, there have been many results on the mathematical analysis of formation control. In [11], the authors analyze the stability of the behavior of the first-order vehicles in a formation that relied on the Graph Laplacian associated with a given communication topology. Consensus algorithms applied to a team represented by a second-order dynamics are presented in [12] to guarantee attitude alignment and velocities in a group of vehicles using undirected communication information. New results on formation control based on second-order consensus protocols was given in [13]. In [14], new general potential functions are constructed to design formation controllers that yield semi-global asymptotic convergence of a group of mobile agents to a desired formation, and guarantee no collisions among the agents. However, most of those works model the agents as point robots and consider only the problem of position control of the robots. In numerous applications the orientation of the agents plays an important role, which means that the agents must be modeled as rigid bodies. Taking into account the position and orientation of the rigid bodies in a formation of a team of mobile robots, Breivik et al. [15] proposed a guided formation control scheme based on a modular design procedure that makes the design completely decentralized in the sense that no variables need to be communicated between the formation members. In [16], the problem of coordinated path following of multiple wheeled robots was solved by resorting to linearization and gain scheduling techniques. Even though the solution given to the problem is simple, it lacks global results; that is, convergence of the vehicles to their paths and to the desired formation pattern is only guaranteed locally. The challenge we are tackling in this paper is to design a nonlinear formation control law based on a virtual structure approach for the coordination of a group of N mobile robots [17]. This control law should force the robots’ relative positions with respect to the center of the virtual structure during the motion. The control system is derived in four stages. First, the dynamic of the virtual structure is defined. Second, the desired motion for each mobile robot is defined by translating by a given distance the motion of the virtual structure center. Third, the path following problem for each mobile robot is solved individually, by introducing a virtual target that propagates along the path. We interpret the path following errors in a triangular form, for which the backstepping technique can be applied to control the rate of progression of the virtual target along the path, and a local control law that makes the mobile robot track the virtual target. Finally, coordination is achieved by synchronizing the parametrization states that capture the positions of the virtual targets with the parametrization states of the virtual structure’s center. The formation of a simplified kinematic model of robots is first considered to clarify the design philosophy. The proposed technique is then extended to the dynamics of mobile robots with nonholonomic constraints. The experimental controller testing was performed on EtsRo vehicles, available at the GREPCI, Robotics Lab of the École de Technologie Supérieure of Montreal. The reminder of this paper is organized as follows. The problem statement is presented in Section 2. Section 3 is devoted to
Fig. 1. Formation structure.
modeling of the path following configuration for a wheeled mobile robot, while Sections 4 and 5 are the main part of this paper focussing on the study of the coordinated control for, respectively, a team of kinematic wheeled mobile robots and dynamic mobile robots. Simulation and experimental results are presented in Section 6. A conclusion is given in Section 7. 2. Problem statement 2.1. Kinematic model We consider a group of N mobile robots, each of which has the following equations of motion: x˙ i = vi cos(θi ) y˙ i = vi sin(θi )
θ˙i = ωi
(1)
where ηi = [xi , yi , θi ] denotes the position and the orientation vector of the ith robot of the group with respect to an inertial coordination frame (Fig. 1). vi and ωi stand for the linear and angular velocities, respectively. For the group to move in a prescribed formation, each member will require an individual parameterized reference path so that when all path parameters are synchronized the member i will be in formation. We can generalize the setup of a single path ξ (s) to n paths by defining the center of a virtual structure that moves along a given reference path ξ (s0 ) = [xd0 (s0 ), yd0 (s0 )], with s0 being a path parameter, and create a set of n designation vectors li (xd0 (si ), yd0 (si )) relative to the center of the structure. When the center of the virtual structure moves along the path ξ0 (s0 ), mobile robot i will then follow the individual desired path given by >
ξi (si ) = ξ0 (s0 ) + R(θd0 (si ))li (xd0 (si ), yd0 (si ))
(2)
where R(θd0 (si )) is the rotation matrix from a frame attached to the center of the virtual structure and the earth fixed frame, which is given as cos(θ0 (si )) R(θd0 (si )) = sin(θ0 (si ))
− sin(θ0 (si )) cos(θ0 (si ))
θd0 (si ) = arctan
s
i yd0
(3)
s
i xd0
where the partial derivative notations in [17,18] are used; i.e., s
i yd0 =
∂ yd0 (si ) , ∂ si
s
i and xd0 =
∂ xd0 (si ) . ∂ si
J. Ghommam et al. / Robotics and Autonomous Systems 58 (2010) 727–736
2.2. Control objective
where we have defined
Before going on with the design, we first give the following assumption.
q
s
s
xdii (si )2 + ydii (si )2 s˙i := v¯ di (si )˙si
vdi =
Assumption 1. 1. We assume that each mobile robot i of the formation broadcasts its state and reference trajectory to the rest of the robots in the group. Moreover, it can receive states and reference trajectories from the other robots of the group. 2. The desired geometric path is regularly parameterized; i.e., there exist strictly positive constants ε1i and ε2i , 1 ≤ i ≤ N such that s
(xdii )2 (s) + (ydii )2 (s) ≥ ε1i ,
s˙0 ≥ ε2i .
(4)
Remark 1. The second inequality of Eq. (4) means that the center of the virtual structure moves forward. The overall control objective is to solve the formation problem for N mobile robots which consists of two parts [17]. The geometric task ensures that an individual mobile robot converges to and stays at its designation in the formation. The dynamic task ensures that the formation maintains a speed along the path according to the given speed assignment. The formation control objective boils down to design controller vi and ωi such that
ωdi =
s2i
s2i
s
s
729
s
xdii (si )ydi (si ) − xdi (si )ydii (si ) s
s
(9)
xdii (si )2 + ydii (si )2
s˙i := ω ¯ di (si )˙si .
4. Control design In the tracking model (8), yei could not be directly controlled, and to overcome this difficulty, we employ the backstepping approach [19]. Define the following variable: s˙˜i = s˙i + $i (t , xe , ye , θe )
(11)
where xe = [xe1 , xe2 , . . . , xen ]> , ye = [ye1 , ye2 , . . . , yen ]> and θe = [θe1 , θe2 , . . . , θen ]> ; $i (t , xe , ye , θe ) is a strictly positive function that specifies how fast the ith mobile robot should move to maintain the formation since s˙i is related to the desired forward speed vdi . Step 1. Design a controller to stabilize the xei dynamic. The variable s˙˜i should be left as an extra controller in order to synchronize all the path parameters of each mobile robot in the formation. We therefore choose the following control for xei :
vi = −k1i xei + v¯ di $ cos(θei ).
lim kηi (t ) − ηdi (t )k = 0
t →∞
lim |si (t ) − s0 (t )| = 0
(5)
t →∞
where ηid denotes the ith reference path to be tracked by robot i. The following lemma [19] will be useful later. Lemma 1. Consider a scalar system x˙ = −kx + f (t ) where k > 0 and f (t ) is a bounded and uniformly continuous function. If, for any initial t0 ≥ 0 and any initial condition x(t0 ), the solution x(t ) is bounded and converges to 0 as t → ∞, then lim f (t ) = 0.
t →∞
Proof. The result follows using the Bellman–Grown inequality lemma and the triangular inequality.
(10)
(12)
In a closed loop, the xei dynamic can be rewritten as x˙ ei = −k1i xei − v¯ di s˙˜i cos(θei ) + yei ωi .
(13)
From (13), it is seen that, if s˙˜i and yei are zero, then xei globally exponentially converges to zero. The next step of the design will focus on designing a controller to stabilize θei at the origin. Step 2. Design a controller for ωi . Consider the following Lyapunov function: n 1X
(x2ei + y2ei + θei2 + γi (si − s0 )2 ) (14) 2 i=1 where γi is a positive constant. The time derivative of (14) along the solutions of (13) and (8) yields V =
V˙ =
n X
xei (−k1i xei − v¯ di s˙˜i cos(θei ) + yei ωi )
i=1
3. Path following error dynamic
+
xei yei
ψei
#
" =
cos(θi ) − sin(θi ) 0
sin(θi ) cos(θi ) 0
0 0 1
xi − xdi yi − ydi
#"
θi − θdi
#
θdi = arctan
si
ydi s xdii
.
θei (ωi − ω¯ di (s˙˜ + $i ))
i=1
+
n X
γi (si − s0 )(s˙˜i + $i − s˙0 ).
(15)
i=1
$i = s˙0 ,
V˙ = (7)
(16)
n X
" −
k1i x2ei
+ θei yei v¯ di $i
1
Z
cos(λθei )dλ 0
i=1
# + ωi − ω¯ di $i +
" n X
−¯vdi cos(θei )xei
i =1
x˙ ei = vi − vdi cos(θei ) + yei ωi
θ˙ei = ωi − ωdi
n X
the derivative of the Lyapunov function V becomes
The error dynamics are then y˙ ei = vdi sin(θei ) − xei ωi
+
If we select (6)
where θdi is defined as
yei (¯vdi (s˙˜i + $i ) sin(θei ) − xei ωi )
i=1
The problem we consider here is the path following for each mobile robot in the formation; that is, we wish to find the control law vi and ωi such that the robot follows a reference point in the path with position ηdi = [xdi , ydi , θdi ]> and inputs vdi and ωdi . The path error is therefore interpreted in a frame attached to the reference path ξ (si ). Following [20], we define the error coordinates
"
n X
(8)
# + yei v¯ di sin(θei ) − θei ω¯ di + γi (si − s0 ) s˙˜i .
(17)
730
J. Ghommam et al. / Robotics and Autonomous Systems 58 (2010) 727–736
To make the derivative of the Lyapunov function V negative, we choose the following controllers:
ωi = −k2i θei − yei v¯ di $i
1
Z
cos(λθei )dλ + ω ¯ di $i 0
s˙˜i = −εω tanh −¯vdi cos(θei )xei + yei v¯ di sin(θei )
− θei ω¯ di + γi (si − s0 )
(18)
where εω is a positive constant to be selected later. Substituting the equations of (18) into (17) yields V˙ =
n X
−k1i x2ei − k2i θei2 − εω φi tanh(φi )
(19)
i =1
From the definition of V , the right-hand side of (24) is bounded by a positive constant depending on the initial conditions. The boundedness of the left-hand side of (24) also implies those of xei , yei , θei and si − s0 for all t ≥ t0 ≥ 0. The assumed boundedness of ω0 (t ) implies that of the right-hand side of the last equation of (23) depends continuously on t through bounded functions. It follows that si and therefore s0 are bounded on the maximal interval of definition [0, T ); this excludes finite escape time, so T = +∞. This in turn implies by construction that xi (t ), yi (t ), θi (t ), si and s0 do not go to infinity in finite time. (Stability of (xei , yei , θei )) From the above argument on the boundedness of (xei , yei , θei , si − s0 ), applying Barbalt’s lemma [22] to (19) results in lim (xei , θei ) = 0
t →∞
where φi = −¯vdi cos(θei )xei + yei v¯ di sin(θei ) − θei ω ¯ di + γi (si − s0 ). We are able now to choose an appropriate function for s˙0 such that, when the tracking errors are large, the center of the virtual structure will wait for the rest of the group; however, when the errors converge to zero, we impose s˙0 to approach a given positive bounded function ω0 (t ), which means that the center of the virtual structure will move at a desired speed. s˙0 can be chosen as, among many others, [21] s˙0 = ω0 (t )(1 − κ1 e−κ2 (t −t0 ) )(1 − κ3 tanh(x> e Γx xe > + y> e Γy ye + θe Γθ θe ))
lim φi tanh(φi ) = 0.
(25)
t →∞
On the other hand, from the second equation of (25), we conclude that lim φi = 0
t →∞
lim s˙˜i = 0.
(26)
t →∞
From this fact, we also conclude that (20)
lim (si − s0 ) = 0,
(27)
t →∞
where ω0 (t ) is a strictly positive and bounded function. This function specifies how fast the whole group of robots should move. Γx , Γy and Γθ are weighting positive matrices. All the constants κ1 , κ2 and κ3 are nonnegative, and κ1 and κ3 should be less than 1. Now, if we choose the constant εω such that
which satisfies the second objective of (5). To satisfy the first objective of (5), we have to show that yei also converges to zero as t goes to infinity. In a closed loop the θei dynamic can be rewritten as follows:
εω < ω0 (t )(1 − κ1 )(1 − κ3 )
θ˙ei = −k2i θei − yei v¯ di $i
(21)
cos(λθei )dλ − ω ¯ di s˙˜i .
(28)
0
then we have Let f (t ) = −yei v¯ di $i it follows that
s˙i = −εω tanh(φi ) + ω0 (t )(1 − κ1 e−κ2 (t −t0 ) ) > > × (1 − κ3 tanh(x> e Γx xe + ye Γy ye + θe Γθ θe ))
≥ −εω + ω0 (t )(1 − κ1 )(1 − κ3 ) > 0.
(22)
From the above control design, we have the closed loop system x˙ ei = −k1i xei − v¯ di s˙˜i cos(θei ) + yei ωi y˙ ei = vdi sin(θei ) + k2i θei xei + yei xei v¯ di
× $i
1
Z
1
Z
cos(λθei )dλ − xei ω ¯ di $i
(23)
lim yei v¯ di $i
t →∞
Z
R1 0
cos(λθei )dλ− ω ¯ di s˙˜i ; according to Lemma 1,
1
cos(λθei )dλ = 0
0
R1
since limt →∞ 0 cos(λθei )dλ = 1, then limt →∞ yei = 0, which concludes the first objective of (5). 5. Extension of formation control to the dynamic model of the mobile robot
0
θ˙ei = −k2i θei − yei v¯ di $i
1
Z
In this section, we study the augmented system (8) appended with a dynamic model of a nonholonomic mobile robot [23].
cos(λθei )dλ − ω ¯ di s˙˜i 0
s˙i = −εω tanh(φi ) + ω0 (t )(1 − κ1 e−κ2 (t −t0 ) )
x˙ ei = vi − vdi cos(θei ) + yei ωi
× (1 − κ3 tanh(xe Γx xe + ye Γy ye + θe Γθ θe )). >
>
>
y˙ ei = vdi sin(θei ) − xei ωi
We now state the main result of the formation control for the kinematic model of unicycle-type mobile robots.
(29)
θ˙ei = ωi − ωdi M i ν˙ i = −C i (ωi )νi − Di νi + Bi τi
Theorem 2. Under Assumption 1, the control inputs vi and ωi given in (12) and (18) and the time derivative of each individual path s˙i given in (22) for the mobile robot i solve the formation control objective. In particular, the closed loop system is forward complete, and the position and orientation of the robots track their path reference asymptotically in the sense of (5).
where νi = [vi , ωi ]> , τi = [τ1i , τ2i ]> are the control vector torques applied to the wheels of the robot i. The modified mass matrix, Coriolis and Damping matrices are given by
Proof (Forward Completness). From (19), we have that V˙ ≤ 0, which implies that
where Bi is an invertible matrix given by Bi =
V (t ) ≤ V (t0 ),
bi , m11i , m22i , d11i , d22i and ci are the dynamic parameters of each
∀t ≥ t 0 .
(24)
1 M i = B− i Mi Bi ,
1 C i (ωi ) = B− i Ci (ωi )Bi
1 Di = B− i Di Bi
M =
h
m11i m12i
m12i m11i
i
, Ci (ωi ) =
h
0 −ci ωi
ci ωi 0
i
, Di =
h
h
1 1
d11i 0
bi −bi 0 d22i
i
i
and with
J. Ghommam et al. / Robotics and Autonomous Systems 58 (2010) 727–736
731
Fig. 3. The three EtsRo mobile robots in the experiments. Fig. 2. Interaction between the host computer and the robots.
group, which are not necessarily know. Definitions of these parameters are given in [23]. In this section we intend to show that the formation control developed for the kinematic model can also be obtained for the complete dynamic of unicycle-type mobile robots. The control objective is thereby similar to that of (5) and consists of finding the controller τi that satisfies all the conditions of (5). We introduce the following new variables:
νei = νi − ανi ,
vei = vi − αvi ,
⇒
ωei = ωi − αωi
(30)
where αvi and αωi are defined as in (12) and (18), respectively. Following the notation in Section 4, in the new coordinates (xei , yei , θei , vei , ωei ) the system (29) is transformed into x˙ ei = −k1i xei − v¯ di s˙˜i cos(θei ) + yei ωi + vei
× $i
proj(π , µ) ˆ = π,
if, Ξ (µ) ˆ ≤0
proj(π , µ) ˆ = π,
if, Ξ (µ) ˆ ≥ 0 and Ξµˆ (µ) ˆ ≤0
proj(π , µ) ˆ = (1 − Ξ (µ))π ˆ ,
if, Ξ (µ) ˆ > 0 and Ξµˆ (µ) ˆ >0
where Ξ (µ) ˆ = (µ ˆ 2 − µ2M )/(k2 + 2kµM ), Ξµˆ (µ) ˆ =
∂Ξ
ˆ µ ˆ (µ) ∂µ ˆ
,k
is an arbitrarily small positive constant, ω ˆ is an estimate of µ and ˙ˆ = proj(π , µ) |µ| ≤ µM . The projection algorithm is such that if µ and µ( ˆ t0 ) ≤ µM then (a) µ( ˆ t ) ≤ µM + k, ∀0 ≤ t0 ≤ t ≤ ∞ (b) proj(π , µ) ˆ is Lipschitz continuous
y˙ ei = vdi sin(θei ) + k2i θei xei + yei xei v¯ di
(c) |proj(π , µ)| ˆ ≤ |π |
1
Z
where the operator, proj, is the Lipschitz continuous projection [24] algorithm applied in our case as follows:
cos(λθei )dλ − xei ω ¯ di $i − xei ωei
(d) µ ˜ proj(π , µ) ˆ ≥ µπ ˜ with µ ˜ =µ−µ ˆ
0
θ˙ei = −k2i θei − yei v¯ di $i
Z
1
cos(λθei )dλ − ω ¯ di s˙˜i + ωei
(31)
and K1i is a symmetric positive definite matrix. Using Property (d) of the projection algorithm results in
0
M νei = −C i (ωi )νi − Di νi − M i [α˙ vi , α˙ ωi ]> + Bi τi
V˙ 2 ≤ −
= −Di νei + Φi Θi + Bi τi
Φi =
0
Θi = bi ci
−αvi
−α˙ vi
0
0
d11i
0
0
−ωi vi
m11i + m12i
−αωi ci bi
0
+ νei> (Di + K1i )νei .
−α˙ ωi >
d22i
m11i − m12i
. (32)
N
V2 = V +
2
νei M i νei + Θi Γi Θi >
e>
−1 e
(33)
i
ei = Θi − Θ bi , with Θ bi being an estimate of Θi and Γi a symwhere Θ metric positive definite matrix. Differentiating both sides of (33) along the solutions of (31) and (17) yields a non-negative function. This suggests that we choose for the control input τi , the path bi the following parameter si and the updated law Θ bi − xei τi = B−1 −K1i νei − Φi Θ
θei
>
x˙ ei = −k1i xei − v¯ di s˙˜i cos(θei ) + yei ωi + vei y˙ ei = vdi sin(θei ) + k2i θei xei + yei xei v¯ di
× $i
1
Z
cos(λθei )dλ − xei ω ¯ di $i − xei ωei 0
θ˙ei = −k2i θei − yei v¯ di $i
1
Z
cos(λθei )dλ − ω ¯ di s˙˜i + ωei 0
ei − xei M νei = −(Di + K1i )νei + Φi Θ
θei
>
s˙i = −εω tanh(φi ) + ω0 (t )(1 − κ1 e−κ2 (t −t0 ) )
> > × (1 − κ3 tanh(x> e Γx xe + ye Γy ye + θe Γθ θe ))
˙ = −Γ proj(Φ > ν , Θ e bi ). Θ i i ei i
(36)
Theorem 3. Under Assumption 1, the control inputs τi and the
˙ = Γ proj(Φ > ν , Θ b bi ) Θ i i ei i s˙i = −εω tanh(φi ) + ω0 (t )(1 − κ1 e−κ2 (t −t0 ) )
> > × (1 − κ3 tanh(x> e Γx xe + ye Γy ye + θe Γθ θe ))
(35)
From the above control design, we have the closed loop system
At this stage, we design the actual control input vector τi and updated laws for the unknown system parameter vector Θi for each robot i. To do so, we consider the following Lyapunov function: 1X
k1i x2ei + k2i θei2 + εω φi tanh(φi )
i=1
where
2 ωi
n X
(34)
˙ given in (34) for the mobile robot i solve the formation b update law Θ i control objective. In particular, the closed loop system (36) is forward complete and all signals in the closed loop system are Uniformly Ultimately Bounded (UUB).
732
J. Ghommam et al. / Robotics and Autonomous Systems 58 (2010) 727–736
(a) Desired and actual vehicle’ trajectories.
(b) Path following errors xei .
(d) Heading errors θei .
(c) Path following errors yei .
(e) Path parameter error in the form
qP 3
i=1
(si − s0 )2 .
(f) Time evolution of the forward velocity of each robot.
Fig. 4. Coordination of three wheeled robots along a sinusoidal path.
bi> , si ]> . = [xei , yei , θei ]> and Zi = [Xei> , νei> , Θ P N e > −1 Θ ei to Considering (35), by subtracting and adding 21 i=1 Θi Γi
where p qi is a constant that depends on the elements of Γi , and
its right-hand side, we get
guaranteed to be UUB. The assumed boundedness of ω0 (t ) implies that the right-hand side of (36) depends continuously on time t through a bounded function. With Zi being bounded, it follows that (36) is bounded on the maximal interval of definition. This excludes finite escape times.
Proof. Let Xei
V˙ 2 ≤ δ V2 + ρ
(37)
where δ = min(1, 2µ1 , . . . , µN ), µi = min(k1i , k2i , εω ) and ρ = PN 1 e > −1 Θ ei . From (37), it is straightforward to show that i=1 Θi Γi 2 V2 (t ) ≤ V2 (t0 )e−δ(t −t0 ) +
ρ . δ
(38)
This implies that, for all t in the maximal interval of definition [0, T ),
kZi k ≤ pi kZi (t0 )ke−0.5δ(t −t0 ) + ρi
(39)
ρi =
ρ . δ
Consequently, all the signals in the closed-loop are
6. Simulation and experimental tests In this section, we provide simulation and experimental results to validate the theoretical results of Sections 4 and 5. For the simulations and experiments, the number of robots in the formation group is chosen for simplicity of implementation to be N = 3, and
J. Ghommam et al. / Robotics and Autonomous Systems 58 (2010) 727–736
(a) Desired and actual vehicle trajectories.
733
(b) Time varying distance between the center of the virtual structure and the robots.
Fig. 5. Maneuvering coordination of three wheeled robots along different paths.
the interactions between the host computer and the robots are assumed to be bidirectional, as in Fig. 2. The experimental platform, implementation of the algorithm, and experimental results will be described in detail. 6.1. Numerical simulations To illustrate the dynamic performance of the proposed coordination control scheme in Section 4, numerical simulations were carried out. The three mobile robots assemble and maintain an in-line formation; that is, the three robots are requested to follow some regular paths with respect to the path parameter si by having them aligned along a common vertical line. In the case when the paths are not regular, one can break them into different regular paths and consider each path separately. Hereafter, we propose two different scenarios for coordination. In the first one, we simulate the coordination control to execute an in-line formation along three sinusoidal paths by having them aligned along a common vertical line. Next, a different kind of formation maneuver of three mobile robots is presented, in which one robot is required to follow the x-axis, while the two others must follow a sinusoidal path, while the three agents keep an in-line formation along the y-axis. For both scenarios, the control gains, the initial conditions, and the parameters involved the update of the path parameters are k1 = 150, k2 = 70, εω = 0.12, ω0 = 5, κ1 = 0.5, κ2 = 2, κ3 = 0.5, si (0) = 2, [x1 (t0 ), y1 (t0 ), θ1 (t0 )]> = [−4, 2, π4 ]> , [x2 (t0 ), y2 (t0 ), θ2 (t0 )]> = [−3.5, −1, π4 ]> , [x3 (t0 ), y3 (t0 ), θ3 (t0 )]> = [−3.5, 0.5, π4 ]> , and Γx = Γy = Γθ = diag(1, 1, 1). In the first scenario, the reference path for the center of the virtual structure is taken as xd0 = 0.1s0 − 3.5 and yd0 = 0.5 cos(0.1s0 ). The distances from the vehicles to the center of the virtual structure are chosen as l1 (xd0 (s1 ), yd0 (s1 )) = (0, 0), l2 (xd0 (s2 ), yd0 (s2 )) = (0.1s2 − 3.5, 0.5 cos(0.1s2 ) − 1) and l3 (xd0 (s3 ), yd0 (s3 )) = (0.1s1 − 3.5, 0.5 cos(0.1s1 ) + 1). Clearly the choice for the distance l1 means that the first robot coincides with the center of the virtual structure. In Fig. 4(a), the vehicles are progressing successfully towards their paths and the vehicles ultimately attain their desired formation configuration. The path tracking and path parameter errors are plotted in Fig. 4(b), (c), (d) and (e), respectively. Notice in Fig. 4(f) how the vehicles adjust their speed along the path so as to achieve coordinations. This could be explained as follows. During the first few seconds, the vehicles quickly move to reach their trajectories; once they keep moving on their paths, each vehicle slows down to wait for its neighbors in order to synchronize as long as they propagate. Fig. 5(a) depicts a typical formation maneuver involving three robots along different paths. This example captures the situation where a vehicle (moving in straight line) directs two other robots
Fig. 6. Path references and real robot trajectories.
to inspect two distinct symmetric areas along a fixed direction. This particular formation shows the novelty of the method adopted using the virtual structure approach, since we allow the distances between robots to change (see Fig. 5(b)). 6.2. Experimental validation In this section, we provide experimental results to validate the theoretical results of Section 5 on a team of three mobile robots. 6.2.1. Robot platform and program implementation To show the performance of the proposed controllers, several experiments were executed. The proposed controllers were implemented on a team of EtsRo mobile robot platforms. The EtsRo vehicle is available at the Robotics Lab of the École de Technologie Supèrieure in Montreal, supplied with an automatic navigation system. A picture of a team of three EtsRo robots is given in Fig. 3. EtsRo is a four-wheel car-like mobile robot, equipped with two driving front wheels and DC motors. The EtsRo sensor equipment consists of high precision wheel encoders giving the relative localization of the vehicle, mounted on the motors counting (6000 pulses/turn). Within each robot there is a 32-bit microcontroller ATmega32 that maintains support for all sensor and actuation features of the robot and manages the navigation, guidance and control of the vehicle. A control program is sent remotely by a Zigbee USB-Modem to the EtsRos. The range of the modem is about 100 m, which makes it a high quality modem for indoor applications. The host computer executes programs which transmit the desired posture and the control commands to the EtsRos
734
J. Ghommam et al. / Robotics and Autonomous Systems 58 (2010) 727–736
Fig. 7. Path following errors xei .
Fig. 10. Evolution of the forward velocities in the first scenario.
Fig. 8. Path following errors yei .
Fig. 11. Evolution of the angular velocities in the first scenario.
Fig. 9. Heading errors θei .
through an on-board micro-controller. The communication delay between the host computer and each robot is evaluated as 100 ms. Two level controller architectures are designed to navigate and steer the EtsRo robots. The lower one is a PID controller, responsible for adjusting the velocity of right and left wheels. To filter sparks and unwanted noises on the velocities, a second-order filter was designed and implemented. The higher level one is a nonlinear controller, and it is designed to navigate and steer the mobile robots with regard to the desired coordination. 6.2.2. Experimental results The simulations presented in Section 5 do not take into account the dynamics of the robots. EtsRo robots are actuated with DC motors that are characterized by a relatively poor transient responses. Moreover, the left and right motors may not have the same response due to the nonhomogeneous mass distribution over the robot body. For this reasons, the coordinated controller in Section 5
Fig. 12. Path references and real robot trajectories.
was implemented on the testbed program. The robots’ configuration was used as before, but the controller gains were set with smaller values since the physical robots are very sensitive to gains in actuation. Therefore the control gains k1i = 0.04, k2i = 2 and Ki = diag(25, 35) were implemented to prevent the control inputs from saturating the motors. The environment in which the robots can evolve is a rectangular arena, 4 m × 7 m. Assuming that the three robots are identical, the nominal robot parameters (which were obtained via identification) are given in Table 1. The expressions for each element of the mass matrix are given as 2 2 2 m11i = 0.25b− i ri (mi bi + Ii ) + Iw i , 2 2 2 m12i = 0.25b− i ri (mi bi − Ii )
mi = mci + 2mwi ,
Ii = mci a2i + 2mwi b2i + Ici + 2Imi .
J. Ghommam et al. / Robotics and Autonomous Systems 58 (2010) 727–736
Fig. 13. Evolution of the forward velocities in the second scenario. Table 1 Parameters of the EtsRo mobile robot. Parameters
Values
Unit
ri bi ai mci mw i Ic i Iw i Imi
0.04 0.1 0.02 2.3 0.28 0.01 0.0056 0.002
(m) (m) (m) (kg) (kg) (kg m2 ) (kg m2 ) (kg m2 )
In the experiment, the trajectories to follow by the robots are as described in simulations section. The initial positions and orientations for the robots remained at [x1 (t0 ), y1 (t0 ), θ1 (t0 )]> = [−4.5 m, 1.5 m, 0 rad]> , [x2 (t0 ), y2 (t0 ), θ2 (t0 )]> = [−3.5 m, −1.5 m, 0 rad]> , [x3 (t0 ), y3 (t0 ), θ3 (t0 )]> = [−4.5 m, 1.5 m, 0 rad]> . We ran two tests for different scenarios. In the first, the robots are required to follow three parallel sinusoidal paths, while in the second, one of the robot coincides with the center of the virtual structure which moves along a straight line and the other two robots move on two sinusoidal paths. Fig. 6 presents the reference and the actual robot trajectories. The path tracking errors xi − xdi , yi − ydi are shown in Figs. 7 and 8. Indeed, these errors exhibit a steady-state value of 0.2 m; this is due to the dependence of the steady errors upon the loop gain, and because we introduced small values for the control gains (to avoid saturating the motors), the errors drifted away from zero. Fig. 9 reports the orientation error θi − θdi . Linear and angular velocities are plotted in Figs. 10 and 11 respectively; these show that the robots travel along their path with a common velocity, and the formation is experimentally successful. In the second experimental scenario, the first mobile robot is forced to move on a straight line which coincides with the center of the virtual structure, and the other two robots move on two opposite sinusoidal paths (see Fig. 12). The experimental linear and angular velocities in this scenario are reported in Figs. 13 and 14, respectively. Note from Fig. 13 how the first robot moves with a constant velocity while the other two robots move with varying velocity to preserve the formation. 7. Conclusion This paper has proposed a methodology for formation control of a group of unicycle-type mobile robots represented at a kinematic level and a dynamic level. The approach that has been developed is mainly based on a combination of the virtual structure and path following approaches. The controller is designed in such a way that the derivative of the path parameter is left as an additional control input to synchronize the formation motion. Real-time experiments on a multi-robot platform showed the effectiveness of the theoretical results. Future work is to design an observer for both kinematic
735
Fig. 14. Evolution of the angular velocities in the second scenario.
and dynamic models to estimate unavailable signals such as velocities or orientation error tracking. Moreover, the impact of sensor noise on the system performance can be further alleviated by using state filter estimators. References [1] P.K.C. Wang, Navigation strategies for multiple autonomous robots moving in formation, Journal of Robotic Systems 8 (2) (1992) 177–195. [2] T.D. Barfoot, C.M. Clark, Motion planning for formations of mobile robots, Journal of Robotics and Autonomous Systems 46 (2) (2004) 65–78. [3] Q. Chen, J.Y.S. Luh, Coordination and control of a group of small mobile robots, in: Proc. IEEE Int. Conf. Robotics and Automation, San Diego, CA, USA, 1994, pp. 2315–2320. [4] Y. Ishida, Functional complement by cooperation of multiple autonomous robots, in: Proc. IEEE Int. Conf. Robotics and Automation, San Diego, CA, USA, 1994, pp. 2476–2481. [5] S. Sheikholeslam, C. Desoer, Control of interconnected nonlinear dynamical systems: The platoon problem, IEEE Transactions on Automatic Control 37 (1992) 806–810. [6] T. Balch, R.C. Arkin, Behavior-based formation control for multirobot teams, IEEE Transactions on Robotics and Automation 14 (6) (1998) 926–939. [7] K.D. Do, Formation tracking control of unicycle-type mobile robots, in: Proc. of Robotics and Automation Conf., Roma, 2007. [8] X. Li, J. Xiao, Z. Cai, Backstepping based multiple mobile robots formation control, in: Proc. IEEE Int. Conf. Intelligent Robots and Systems, Roma, 2005. [9] T. Dierks, S. Jagannathan, Control of nonholonomic mobile robot formations: Backstepping kinematics into dynamics, in: Proc. of the Int. Conference on Control Application, Singapore, 2007. [10] R. Beard, J. Lawton, Hadaegh, A coordination architecture for spacecraft formation control, IEEE Transactions on Control Systems Technology 9 (1999) 777–790. [11] A. Fax, R.M. Murray, Information flow and cooperative control of vehicle formations, IEEE Transactions on Automatic Control 50 (2004) 1465–1476. [12] J. Lawton, W. Beard, Synchronized multiple spacecraft rotations, Journal of Automatica 8 (38) (2002) 1359–1364. [13] W. Ren, On consensus algorithms for double-integrator dynamics, IEEE Transactions on Automatic Control 6 (53) (2008) 1503–1509. [14] K. Do, Bounded controllers for formation stabilization of mobile agents with limited sensing ranges, IEEE Transactions on Automatic Control 3 (52) (2007) 569–576. [15] M. Breivik, T. Fossen, Guided formation control for wheeled mobile robots, in: Proc. of 9th IEEE Conference on Control, Automation, Robotics and Vision, Singapore, 2006, pp. 1–7. [16] R. Ghabcheloo, A. Pascoal, A. Silvestre, C. Kaminer, Coordinated path following control of multiple wheeled robots using linearization techniques, International Journal of Systems Science 37 (2006). [17] R. Skjetne, S. Moi, T. Fossen, Nonlinear formation control of marine craft, in: Proc. of the 41st Conf. on Decision and Contr., Las Vegas, NV, 2002. [18] R. Skjetne, T. Fossen, P.V. Kokotovic, Robust output maneuvering for a class of nonlinear systems, Journal of Automatica 40 (2004) 373–383. [19] Z.P. Jiang, H. Nijmeijer, Tracking control of mobile robots: A case study in backstepping, Journal of Automatica 33 (1997) 1393–1399. [20] Y.K. Kanayama, F. Miyazaki, T. Noguchi, A stable tracking control method for an autonomous mobile robot, in: Proc. of Robotics and Automation Conf., Las Vegas, NV, 1990. [21] K.D. Do, J. Pan, Nonlinear formation control of unicycle-type mobile robots, Journal of Robotics and Autonomous Systems 55 (2007) 191–204. [22] H.K. Khalil, Nonlinear Systems, Macmillan, New York, 1992. [23] T. Fukao, H. Nakagawa, N. Adachi, Adaptive tracking control of nonholonomic mobile robot, IEEE Transactions on Robotics And Automation 16 (2000) 609–615. [24] J. Pomet, L. Praly, Adaptive nonlinear regulation: Estimation from the Lyapunov equation, IEEE Transactions on Robotics And Automation 37 (1992) 729–740.
736
J. Ghommam et al. / Robotics and Autonomous Systems 58 (2010) 727–736
Jawhar Ghommam was born in Tunis, Tunisia, in 1979. He received his B.Sc. degree from Institut Nationale des Sciences Appliquées et de Technologies (INSAT), Tunisia, in 2003, his M.Sc. degree in Control Engineering from the Laboratoire d’Informatique, de Robotique et de Microelectronique (LIRMM), Montpellier, France, in 2004, and his Ph.D. in Control Engineering and Industrial Computing in 2008, jointly from the Université of Orléans, France, and Ecole Nationale d’Ingénieurs de Sfax, Tunisia. He is an Assistant Professor of Control Engineering at the Institut Nationale des Sciences Appliquées et de Technologies (INSAT), Tunisia. He is a Member of the research unit on MEChatronics and Autonomous systems (MECA). His research interests include nonlinear control of underactuated mechanical systems, adaptive control, guidance and control of underactuated ships and cooperative motion of nonholonomic vehicles. Hasan Mehrjerdi received his B.Sc. and M.Sc. in Electrical Engineering from Ferdowsi University of Mashhad and Tarbiat Modares University of Tehran, respectively. He is currently pursuing a Ph.D. in Electrical and Robotics Engineering as a member of the GREPCI lab at the Université du Québec (Ecole de technologie supérieure). His research interests include mobile robotics, artificial intelligence, mobile sensor networks and mobile robot coordination in known and unknown environments.
Maarouf Saad received his Bachelor and Master degrees in Electrical Engineering from Ecole Polytechnique of Montreal in 1982 and 1984, respectively. In 1988, he received his Ph.D. in Electrical Engineering from McGill University. He joined Ecole de technologie supérieure in 1987, where he is teaching control theory and robotics courses. His research is mainly in nonlinear control and optimization applied to robotics and flight control system.
Faïçal Mnif received his M.Sc. and Ph.D. degrees in Control and Robotics from the Ecole Polytechnique de Montreal, in 1991 and 1996, respectively. Dr. Mnif is an Associate Professor of Control Engineering and Robotics at the National Institute of Applied Sciences and Technology, Tunis, Tunisia. He is currently on leave to Sultan Qaboos University, Oman, where he is holding a faculty position with the Department of Electrical and Computer Engineering. He is also a Member of the Research Unit on Mechatronics and Autonomous Systems, Tunisia. His main research interests include robot modeling and control, control of autonomous vehicles, modeling and control of holonomic and nonholonomic mechanical systems, and robust and nonlinear control.