Académique Documents
Professionnel Documents
Culture Documents
,
( )
' ' ' ' ' y rly rry ey cy
F m u u F F F F = + = + + +
,
( ) ( )
' ' ' '
2
z z rrx rlx rly rry
d
M I F F b F F = = + +
( ) ( )
' ' ey cy e
e b F c b F + + ; (1)
where m is the robot mass; and I
z
is the robot moment of
inertia about la vertical axis located in G. The kinematics of
point h is:
( ) cos sin sin x u u a b = ,
( ) sin cos cos y u u a b = + + . (2)
According to [18], velocities u, and u , including the slip
speeds, are:
( ) ( )
1
2
s s
r l r l
u r u u
(
= + + +
,
( ) ( )
1
s s
r l r l
r u u
d
(
= +
,
( ) ( )
s s s
r l r l
b
u r u u u
d
(
= + +
, (3)
where r is the right and left wheel radius;
r
and
l
are the
angular velocities of the right and left wheels; u
r
s
and u
l
s
are
the longitudinal slip speeds of the right and left wheel, and
s
u is the lateral slip speed of the wheels.
The motor models attained by neglecting the voltage on
the inductances are:
( ) /
r a r b r a
k v k R =
,
( ) /
l a l b l a
k v k R = . (4)
where v
r
and v
l
are the input voltages applied to the right and
left motors; k
b
is equal to the voltage constant multiplied by
the gear ratio; R
a
is the electric resistance constant;
r
and
l
are the right and left motor torques multiplied by the gear
ratio; and k
a
is the torque constant multiplied by the gear
ratio. The dynamic equations of the motor-wheels are:
' '
,
e r e r r rrx t e l e l l rlx t
I B F R I B F R + = + = , (5)
Fig. 1. Parameters of the unicycle-like mobile robot
where I
e
and B
e
are the moment of inertia and the viscous
friction coefficient of the combined motor rotor, gearbox,
and wheel, and R
t
is the nominal radius of the tire.
In general, most market-available robots have low level
PID velocity controllers to track input reference velocities
and do not allow the motor voltage to be driven directly.
Therefore, it is useful to express the mobile robot model in a
suitable way by considering rotational and translational
reference velocities as control signals. For this purpose, the
velocity controllers are included into the model. To simplify
the model, a PD velocity controller has been considered
which is described by the following equations:
( )
( )
PT ref DT
u
PR ref DR
k u u k u
v
v
k k
(
(
(
=
(
(
. (6)
Variables and
ref ref
u are neglected in (6) to further
simplify the model.
From (1), (2), (3), (4), (5) and (6) the following dynamic
model of the mobile robot is obtained:
0 0
2 3 4
0 0 0
1 1 1
0 0
5 6
0
0 0
2
2 2
cos sin
0 0
sin cos
0 0
0 0
0 1
0
1
0
x
y
ref
ref
u
u a
u a
x
y
u
u
u
u
(
(
(
(
+ ( (
(
(
( (
(
(
( (
( (
(
( ( = + +
( (
(
( (
(
(
( (
(
(
( (
(
(
(
(
. (7)
The parameters of the dynamic model are:
( ) ( )
0
1
2 2 / 2 ,
a
t e DT PT
a
R
mR r I rk rk
k
| |
= + +
|
\ .
( ) ( )
( )
0 2 2
2
2 2 / 2 ,
a
e t z DR PR
a
R
I d R r I mb rdk rdk
k
| |
= + + +
|
\ .
(8)
E
F
ex
F
ey
C
F
cx
F
cy
x u
y u
F
rlx
F
rrx
F
rry
F
rly
d
y
x
b
G
h
a e
c
B
e
3881
( ) ( )
0 0
3 4
/ 2 , / 1,
a a a b
t PT e PT
a a a
R R k k
mbR k B rk
k k R
| |
= = + +
|
\ .
( ) ( )
0 0
5 6
/ , / 2 1.
a a a b
t PR e PR
a a a
R R k k
mbR dk B d rk
k k R
| |
= = + +
|
\ .
(9)
The elements of the uncertainty vector
0
T
x y u
(
are:
sin
s
x
u = ,
cos
s
y
u = ,
' '
,
2 2
s s s s s
ex cx a b r l r l
e
e a t e t
u
DT a
e t a t e
m u F F k k u u u u
B
I R R rI R r
k k m
I R r R R I
| | + + + +
+ + +
|
\ .
=
+ +
' '
2
2 2
.
2
s s s s s
ey cy e
a b r l r l
e
a t e t e e
DR a z
t e t a e
eF cF
k k u u u u mbu
B
R R rI R r I d I d
k k I mb d
R r I d R R I
+ + | |
+ + +
|
\ .
=
+
+ +
The uncertainty vector in (7) will not be considered, if the
slip speed of the wheels, the forces and moments exerted by
the tool, and the forces exerted by the castor are of no
significant value.
Accelerations u and do not depend on the states , x
and y; then u and can be expressed as follows:
0 0
2 3 4
0 0 0
1 1 1
0 0
5 6
0 0 0
2 2 2
1
1
ref
u
ref
u u
u
u
(
+
(
( (
(
= +
( (
(
+ (
(
.
By re-arranging -and disregarding the uncertainty vector- the
linear parameterization of (10) is attained:
2
0
0 0 0
0 0 0
ref
ref
u
u u
u
( (
=
( (
, (10)
where
0 0 0 0 0 0 0
1 2 3 4 5 6
.
T
( =
With an
identification method,
0
can be easily identified.
III. INPUT-OUTPUT FEEDBACK LINEARIZATION OF
THE MODEL
In order to perform the input-output feedback linearization
of model (7), the output vector is defined as:
.
x
h
y
(
=
(
(11)
Considering model (7), the first and second time derivatives
of (11) can be expressed as:
cos sin
sin cos
x
y
x u a
h
y u a
( ( (
= = +
( ( (
+
, (12)
2
2
cos sin sin cos
sin cos cos sin
a u a
h
a u a
( (
= +
( (
0 0
2 3 4
0 0 0
1 1 1
0 0
5 6
0
0 0
2
2 2
1
0
,
1
0
ref
ref
u
u
u
| | ( (
|
(
(
(
|
(
(
+ +
(
|
( (
|
(
(
|
(
\ .
(13)
where:
cos sin
sin cos
x u
y
a
a
( ( (
= +
( ( (
(
.
From (12) and (13), it can be noted that the system has a total
relative degree equal to 4. Considering (13) the following
control input is obtained:
0
1
0
2
cos sin
0
1 1
sin cos 0
ref
ref
u
a a
| (
( (
(
=
( (
(
(
\
0 0
2 3 4
0 0 2
1 1
2 0 0
5 6
0 0
2 2
sin cos
cos sin
u
u a
u a
u
| (
|
(
| | (
|
(
|
(
| |
(
\ .
| (
|
(
.
, (14)
where is the new input, which is defined in the formation
control design. Equation (14) is constrained to a nonzero
value of a. Substituting (14) into (13), the input-output
feedback linearized system can be expressed as
h = +
. (15)
The following inequality is obtained from (12):
1/
h
a C
, where
( ) ( )
max max
s
h
C h u
= +
.
Let
max
be the saturation value of . Then, to avoid the
saturation of , the following condition must be met:
max
/
h
a C
. (16)
In order to find the internal dynamics, the following map is
considered [11]:
( ) cos sin
sin cos
x
y
X u a
u a
(
(
(
( = =
(
+
(
(
, (17)
where
[ ] .
T
X x y u = This map defines a
diffeomorphism whose inverse is:
( )
1
2
1
5
3 5 4 5
3 5 4 5
.
cos sin
1 1
sin cos
X
a a
(
(
(
(
= =
(
+
(
(
+
(
(18)
where
i
is the i-th element of vector .
3882
The input-output feedback linearized model expressed in
the new coordinates is:
3
4
1 1
2 2
3 5 4 5
.
1 1
sin cos
x
y
x
y
a a
+ (
(
+
(
( +
=
(
+
(
(
+
(
(19)
where
i
is the i-th element of vector ; and
i
is the i-th
element of vector . From (19) the internal dynamics is
obtained:
5 3 5 4 5
1 1
sin cos
a a
= +
, (20)
which is unobservable and uncontrollable. From (17),
5
= is
attained. Analyzing in (7) and in (14), it can be concluded
that it is not necessary to bound this angle. However,
5
= = needs to be bounded. The following analysis
proves the boundedness of
5
:
( )
5 3 5 4 5 3 4
1 1 1
sin cos
a a a
= + +
. (21)
The formation control of section IV guarantees that
d
x x
or at least x gets into a neighborhood of
d
x , then:
d xpe
x x C , (22)
where C
xpe
is a bound of
d
x x . From (12) and (17),
3 x
x = is obtained. Then, a bound of
3
is calculated as
follows:
3 x d d d d x
x x x x x x = + + + ,
3 3 xpe xpd x
C C C C
+ + = , (23)
where C
xpd
is a bound of
d
x ; and C
x
is a bound of
x
.
Similarly, a bound of
4
can be calculated:
4 4 ype ypd y
C C C C
+ + = , (24)
where C
ype
is a bound of
d
y y ; C
ypd
is a bound of
d
y ; and
C
y
is a bound of
y
.
Then, the model of the multi-robot system with n mobile
robots is:
= +
, (26)
where
( ) ( )
1
...
T
T
T
n
h h
(
=
(
,
( ) ( )
1
...
T
T T
n
(
=
(
,
and
( ) ( )
1
...
T
T T
n
(
=
(
. The elements of and
are
considered the system states.
A variable z
1
is defined as the vector that contains the
formation parameters: relative positions between robots,
centroid position, formation heading, etc. A variable z
2
is
defined as the time derivative of z
1
, thus
2 1
z z = . A
coordinate transformation is performed in order to express
the multi-robot system model (26) in terms of the formation
state vector z=[(z
1
)
T
(z
2
)
T
]
T
. To this aim, a smooth map
1
is
defined that associates with z
1
, such that it has a smooth
inverse
1
-1
. Now, a map is defined in terms of
1
that
associates the state vector [ ]
T T T
(27)
This map defines a diffeomorphism which inverse is:
( )
( )
( ) ( )
( )
1 1 1 1
1 1
1 1
1
1 2
, where .
z z
z J z
z
J z z
(
(
(
= = =
(
(
(28)
From (28), the time derivative of
is:
( ) ( )
( ) ( )
1 2
2 1 2
.
d J z z
J z z J z z
dt
= = +
(29)
By substituting (26) into (29) the new representation of the
multi-robot system is obtained:
( ) ( ) ( ) ( )
1 1 1
2 1 2 1 1
z J z J z z J z J z
= + +
. (30)
The following inverse dynamics formation control law is
proposed:
( )( ) ( )
1 1 2 2 2
1 2 d
J z K z K z z J z z = + + +
, (31)
where
1 1 1
d
z z z = ;
2 2 2
d
z z z = ;
2 1
d d
z z = ;
z
d
=[(z
1
d
)
T
(z
2
d
)
T
]
T
is the desired formation state vector; K
1
>0;
and K
2
>0. Then, the closed loop system obtained by
substituting the controller of (31) into the system model of
(30) is:
( )
1
1 1 2 2
1 2
. J z K z K z z
= + +
(32)
which can be expressed as:
0
T
T
z
z z ( =
, (33)
where
( )
1
1
J z
=
;
( ) ( )
1 2
T
T T
d
z z z z z
(
= =
(
; and:
2 2
1 2
0
n n
z
I
K K
(
=
(
. (34)
3883
In (34), I
2n2n
represents the 2n2n identity matrix. Let
consider K
1
and K
2
as diagonal matrices with positive
elements different from zero. From these considerations, A
z
is Hurwitz.
The following Lyapunov candidate and its time derivative
are now considered:
( ) 0; 0
T T
V z z Pz P P = > = >
( ) 2 0
T
T T T
V z z Qz z P ( =
(35)
where:
T
z z
Q P P =
(36)
Matrix Q is chosen as Q=I
4n4n
, where I
4n4n
is the 4n4n
identity matrix. Matrix P is calculated from (36). Since A
z
is
Hurwitz, P=P
T
>0. A sufficient condition for the second of
(35) to be negative definite is:
2 0
T
T
z P ( >
(37)
If vector is bounded, the formation error vector z will
also be bounded after a finite time. If the uncertainty vectors
of the robots are null, then =0 and
d
z z with t .
Remark 1. The proposed controller is able to track a moving
or continuously varying desired formation.
Remark 2. The controller needs either the sensorial
information from the direct measurement of formation state
vector z or the calculation of these states by measuring the
state vector [ ]
T T T
. (38)
Remark 3.
( ) ( )
1 1
d
z z
with
d
z z because
( )
1
.
is
a continuous function. Therefore, from (28)
d
and
d
with ,
d
z z where
( )
1
T
T
T
d d d
z
(
=
.
V. EXPERIMENT
An experiment to test the control of a two-robot formation
was performed using two Pioneer mobile robots, one of
which had an omnidirectional camera system and an on-
board computer to monitor and control the other robot (see
Fig. 2).
The identification of the robot model was performed by
using least squares estimation [2] applied to a filtered
regression model [13] obtained from (10). The identified
parameters of the robot with the omnidirectional camera
system are:
0 0 0
1 2 3
0 0 0
4 5 6
0.26038, 0.25095, 0.00049969,
0.99646, 0.002629, 1.0768.
= = =
= = =
In (16), C
h
=0.2m/s and
max
=1.745rad/s are considered, thus
a0.1146m. Then, a constant a=0.12m was chosen.
The formation states were defined as: d
12
, x
1
, y
1
,
c
(see
Fig. 3) and its derivatives; and desired values were defined
as: d
12d
, x
1d
, y
1d
and
cd
. Map
1
is defined as follows:
( )
( ) ( )
( )
2 2
12 2 1 2 1
1 1
1
1
1
1
1 2 1 2
.
, c
d x x y y
x
x
z
y
y
f x x y y
(
( +
(
(
(
(
= = =
(
(
(
(
(
(
(39)
In (39), x
i
, y
i
are elements of the vector h
i
; function f
(x,y)
allows calculating the angle of the vector (x,y). This angle
lies in the range [,2+). For instance f
(x,y)=atan2(y,x)
when =.
The inverse map
1
-1
is calculated as follows:
( )
( )
( )
1 1
1 1 1 1
1
1 12 2
1 12 2
cos
sin
c
c
x x
y y
z
x d x
y d y
( (
( (
( (
= = =
( (
( (
( (
. (40)
Map
1
defines the diffeomorphism of (27) in the region
8
= .
From (28), the Jacobian
( ) z J
is calculated as:
( )
1
12
12
0 1 0 0
0 0 1 0
cos 1 0 sin
sin 0 1 cos
c c
c c
J z
d
d
(
(
(
=
(
(
(
. (41)
The following control parameters are used in the
experiment:
( )
( )
1
2
0.36, 0.25, 0.25, 0.25 ,
1.2, 1, 1, 1 , 0,
K diag
K diag
=
= =
Fig. 2. Mobile robots and omnidirectional camera
Fig. 3. Formation variables
C
2
1
d
12
(x
1
,y
1
)
3884
where diag(.) represents a diagonal matrix. The initial
velocities of the robots are set to zero. The desired formation
is chosen as:
[ ]
1
1.2 0.5 0.05 0
T
d
z t = +
[ ]
2
0 0.05 0 0
T
d
z =
.
In z
d
, the positions and distances are expressed in meters and
the angles are in radians.
Figures 4 and 5 present the experimental results of the
formation control, showing a good tracking performance of
the desired formation. The formation errors are smaller than
0.05m (distance errors) and smaller than 0.03rad (angle error)
after 17.5 seconds. These results validate the theoretical
controller design presented in the previous sections.
VI. CONCLUSION
A complete dynamic model of a unicycle-like mobile robot
and its linear parameterization were presented. An input-
output feedback linearization of the model was performed.
Then, after defining a formation state vector, a formation
control of the multi-robot system was designed using inverse
dynamics. The controller allows the tracking of desirable
formations evolving on the plane. The use of the formation
states, both in the model and in the formation control design
of the multi-robot system, allows achieving a better design of
the time evolution of the formation.
Future work will delve into the issues of obstacle
avoidance and decentralized formation control.
REFERENCES
[1] G. Antonelli and S. Chiaverini, Kinematic Control of a Platoon of
Autonomous Vehicles, IEEE International Conference on Robotics
and Automation, Taipei, Taiwan, September 2003, pp. 1464-1469.
[2] K. J. Astrm and B. Wittenmark, Adaptive control, Addison-Wesley,
1995.
[3] T. Balch and R. C. Arkin, Behavior-based formation control for
multirobot teams, IEEE Transactions on Robotics and Automation,
vol. 14, no. 6, pp. 926-939, December 1998.
[4] C. Belta and V. Kumar, Trajectory design for formations of robots by
kinetic energy shaping, IEEE International Conference on Robotics
and Automation, Washington, DC, May 2002, pp. 2593-2598.
[5] F. D. Boyden and S. A. Velinsky, Dynamic Modeling of Wheeled
Mobile Robots for High Load Applications, IEEE International
Conference on Robotics and Automation, vol. 4, pp. 3071-3078, 1994.
[6] A. K. Das, et al., A vision-based formation control framework, IEEE
Transactions on Robotics and Automation, vol. 18, no. 5, pp. 813-825,
October 2002.
[7] J. P. Desai, J. Ostrowski, and V. Kumar, Controlling formations of
multiple mobile robots, in Proc. IEEE Int. Conf. Robotics and
Automation, Leuven, Belgium, May 1998, pp. 2864-2869.
[8] R. Fierro, A. K. Das, V. Kumar, and J. P. Ostrowski, Hibrid control of
formations of robots, in Proc. IEEE Int. Conf. Robotics and
Automation, Seoul, Korea, May 2001, pp. 3672-3677.
[9] J. Fredslund and M. J. Mataric, Robot formations using only local
sensing and control, IEEE International Symposium on
Computational Intelligence in Robotics and Automation, Banff,
Alberta, Canada, July 2001, pp. 308-313.
[10] R. Kelly, R. Carelli, J. M. Ibarra Zannatha, and C. Monroy, Control
de una pandilla de robots mviles para el seguimiento de una
constelacin de puntos objetivo, VI Congreso Mexicano de Robtica,
COMRob, Torren, Coahuila, Mexico, October 2004.
[11] J. R. T. Lawton, R. W. Beard, and B. J. Young, A Decentralized
Approach to Formation Maneuvers, IEEE Transactions on Robotics
and Automation, vol. 19, no. 6, pp. 933-941, December 2003.
[12] S. Monteiro, M. Vaz, and E. Bicho, Attractor dynamics generates
robot formations: from theory to implementation, IEEE International
Conference on Robotics and Automation, New Orleans, LA, April
2004, pp. 2582-2587.
[13] F. Reyes and R. Kelly, On Parameter Identification of Robot
Manipulator, IEEE International Conference on Robotics and
Automation, Albuquerque, New Mexico, April 1997, pp. 1910-1915.
[14] J. J. Slotine and W. Li, Applied nonlinear control, Prentice Hall Int.,
1991.
[15] D. J. Stilwell and B. E. Bishop, A strategy for controlling autonomous
robot platoons, IEEE Conference on Decision and Control, Sydney,
Australia, December 2000, pp. 3483-3488.
[16] K. H. Tan and M. A. Lewis, Virtual Structures for High-Precision
Cooperative Mobile Robotic Control, International Conference on
Intelligent Robots and Systems, vol. 1, pp. 132-139, November 1996.
[17] H. Yamaguchi, T. Arai, and G. Beni, A distributed control scheme for
multiple robotic vehicles to make group formations, Robotics and
Autonomous Systems, vol. 36, pp. 125-147, 2001.
[18] Y. Zhang, D. Hong, J. H. Chung, and S. A. Velinsky, Dynamic Model
Based Robust Tracking Control of a Differentially Steered Wheeled
Mobile Robot, Proceedings of the American Control Conference,
Philadelphia, Pennsylvania, June 1998, pp. 850-855.
Fig. 4. Formation trajectory, the little circles represent the positions of the
robots.
Fig. 5. Formation error through time.
3885