Professional Documents
Culture Documents
The International Journal of Robotics Research-2007-Lapierre-361-75
The International Journal of Robotics Research-2007-Lapierre-361-75
Path-following and
Obstacle Avoidance
Control of a Wheeled
Robot
Lionel Lapierre
Rene Zapata
Pascal Lepinay
Universit Montpellier II
161, rue Ada 34392 Montpellier Cedex 5
{lapierre, zapata, lepinay}@lirmm.fr
Abstract
This paper proposes an algorithm that drives a unicycle type robot
to a desired path, including obstacle avoidance capabilities. The
path-following control design relies on Lyapunov theory, backstepping techniques and deals explicitly with vehicle dynamics. Furthermore, it overcomes the initial condition constraints present in a number of path-following control strategies described in the literature.
This is done by controlling explicitly the rate of progression of a virtual target to be tracked along the path1 thus bypassing the problems
that arise when the position of the path target point is simply defined
as the closest point on the path. The obstacle avoidance part uses
the Deformable Virtual Zone (DVZ) principle. This principle defines
a safety zone around the vehicle in which the presence of an obstacle
induces an intrusion of information that drives the vehicle reaction. The overall algorithm is combined with a guidance solution that
embeds the path-following requirements in a desired intrusion information function, which steers the vehicle to the desired path while the
DVZ ensures minimal contact with the obstacle, implicitly bypassing
it. Simulation and experimental results illustrate the performance of
the control system proposed.
1. Introduction
Real-time obstacle avoidance coupled with accurate path- following control is one of the major issues in the field of mobile
robotics (Ogren 1989, 2003). The underlying problem to be
solved can be divided into three areas:
The International Journal of Robotics Research
Vol. 26, No. 4, April 2007, pp. 361375
DOI: 10.1177/0278364907076790
c 2007 SAGE Publications
1
Figures 47 appear in color online: http://ijr.sagepub.com
362
the centre of the path curvature where the virtual target is located. This is in contrast with the results described by Micaeli
and Samson (1993) for example, where only local convergence
is proven. To the best of the authors knowledge, the idea of
exploring the extra degree of freedom that comes from controlling the motion of a virtual target along a path appeared for the
first time in the work of Aicardi et al. (1995) for the control
of wheeled robots. This idea was later extended to the control
of marine craft in the work of Aicardi et al. (2001). However,
none of these addresses the issues of vehicle dynamics and parameter uncertainty. Furthermore, the methodologies adopted
by Aicardi et al. (1995, 2001) for control system design build
on entirely different techniques that required the introduction
of a nonsingular transformation in the original error space.
iii) It combines path-following control and obstacle avoidance.
In this paper, controller design builds on the work reported
in Micaeli and Samson (1993) on path-following control and
relies heavily on Lyapunov-based and backstepping techniques
(Krstic et al. 19951 Fierro and Lewis 1997) in order to extend
kinematic control laws to a dynamic setting. In Soetanto et
al. (2003) parameter uncertainty is considered within a framework by augmenting Lyapunov function candidates with terms
that are quadratic in the parameter error. For the sake of clarity, in this paper, we do not extend the dynamic control with
the parameter uncertainty robust scheme.
2 obstacle detection,
2 path replanning,
Lapierre, Zapata, and Lepinay / Combined Path-following and Obstacle Avoidance Control of a Wheeled Robot
363
2. Path-following Algorithm
2.1. Path Following. Problem Formulation
This section reviews a solution to the problem of steering a
unicycle type vehicle along a desired path.
The following assumptions are made regarding the robot
(see Figure 1). The vehicle, of width 2L, has two identical parallel, non-deformable rear wheels of radius R, which are controlled by two independent motors, and a passive front wheel.
It is assumed that the plane of each wheel is perpendicular to
the ground and that the contact between the wheels and the
ground is pure rolling and non-slipping, i.e. the velocity of the
center of mass of the robot is orthogonal to the rear wheels
axis. It is further assumed that the masses and inertias of the
wheels are negligible and that the center of mass of the mobile
robot is located in the middle of the axis connecting the rear
364
sin 4 c
cos 4 c
2
2
R 5 3 6 sin 4 c
dp
dt
s7
0
It is also straightforward to compute the velocity of Q in 3I4 as
dq
dt
5
I
dp
dt
8 R61
I
dr
dt
8 R61 11c 9 r3
F
where r is the vector from P to Q. Multiplying the above equation on the left by R gives the velocity of Q in 3I4 expressed in
3F4 as
dq
dt
5
I
dp
dt
8
F
dr
dt
8 1c 9 r
F
dq
dt
dr
dt
2
5
2
5
5 2 Y7 5 2
3
6
0
1
X7
s71
5
2
5
5 2
3 y71 6 2
0
X7
2
5 2
2
5 2
R 2 Y7 5 5 2
3
6 3
0
7
s71 5 cos 4 c
8
5 2
5 2 y 5
593 1 5
6
6
0
74 5 cc 1s37s
0
6cc 1s37s y1
5
5
52
6
s7 11 6 cc 1s3y1 3 8 s71
sin 4 c
5
y
7
6 sin 4 c
1
9
2 5
5
52
3 0 65
s1
y71 8 cc 1s37s s1
4
5
5
55
6
c 1s3
where cc 1s3 and gc 1s3 5 dcds
denote the path curvature and
its derivative, respectively. The velocity of P with respect to
3I4 can be expressed in 3F4 to yield
(1)
2
2
5 2 cc 1s37s s1
3
0
7
8 1c 5 47 c 5 cc 1s37s
2
2
1c 9 r 5 2
3
5
0 5
6
cos 4 c
cos 4 c
1
3
X7
Y7
1
3
4
6 6 s7 11 6 cc y1 3
X7
Y7
(2)
6 6 cc s7 s1 5
At this point it is important to notice that in Micaelli and Samson (1993), s1 5 0 for all t, since the location of point P is
defined by the projection of Q on the path, assuming the projection is well defined. One is then forced to solve for s7 in the
equation above. However, by doing so 1 6 cc y1 appears in the
denominator, thus creating a singularity at y1 5 c1c . As a result,
the control law derived in Micaelli and Samson (1993) requires
that the initial position of Q be restricted to a tube around
1
the path, the radius of which must be less than cc2max
, where
cc2max denotes the maximum curvature of the path. Clearly,
this constraint is very conservative since the occurrence of a
large cc2max in only a very small section of the path will impose a rather strict constraint on the initial vehicle position, no
matter where it starts with respect to that path.
By making s1 not necessarily equal to zero, a virtual target
that is not coincident with the projection of the vehicle on the
path is created, thus introducing an extra degree of freedom for
controller design. By specifying how fast the newly defined
target moves, the occurrence of a singularity at y1 5 c1c is
removed. The velocity of the unicycle in the 3I4 frame satisfies
the equation
1
3
X7
Y7
and
65u3
cos 4 m
sin 4 m
4
6
(3)
Lapierre, Zapata, and Lepinay / Combined Path-following and Obstacle Avoidance Control of a Wheeled Robot
where 4 m and u denote the yaw angle of the vehicle and its
body-axis speed, respectively. Substituting (3) in (2) and introducing the variable 4 5 4 m 6 4 c gives the kinematic model
of the unicycle in 1s2 y3 coordinates as
7
s71 5 67s 11 6 cc y1 3 8 u cos 4
8
y71 5 6cc s7 s1 8 u sin 4
(4)
9 47 5 6 6 c s7
m
c
where 1m = 47 m .
2.1.2. Dynamics. Problem Formulation
The dynamical model of the unicycle is obtained by augmenting (4) with the equations
7
8 u7 5 F
1
(5)
9 67 5 67 6 c s
6 g s7 2
m
c
c
where 67 m 5 N2 and 1 and 2 are the mass and the moment
of inertia of the unicycle, respectively. Let FP F and N P F be
the path-following control inputs, the forward force and the
angular torque, respectively.
With the above notation, the problem under study can be
formulated as follows:
Given a desired speed profile u d 1t3 7 u min 7 0 for the vehicle
speed u, derive a feedback control law for FP F and N P F to
drive y1 , 4 , and u 6 u d asymptotically to zero.
V71
5 s1 s71 8 y1 y71 8
1 2
1
1s1 8 y12 3 8
14 6 91y1 2 u332
2
28
(6)
1
7
14 6 93147 6 93
8
5 s1 1u cos 4 6 s7 11 6 cc y1 3 6 s7 cc y1 3
8
365
y1 u sin 4 8
1
14 6 93147 6 97 3
8
5 s1 1u cos 4 6 s7 3 8 y1 u sin 9 8
1
8
sin 4 6 sin 9
7
7
5
14 6 93 4 6 9 8 8 y1 u
4 69
Let the ideal (also called virtual) kinematic control laws for
s and 4 be defined as
7
s7 5 u cos 4 8 k1 s1
8
6sin 9
(7)
47 5 97 6 8 y1 u sin 44 69
9
6k2 14 6 935
where k1 and k2 are positive gains. Then,
14 6 932
05
V71 5 6k1 s12 8 y1 u sin 9 6 k2
8
(8)
Note the presence of the term y1 u sin 9 in the previous equation and how assumption A525 is justified.
366
5 97 6 8 y1 u
sin 4 6 sin 9
6 k2 14 6 93
4 69
(9)
and let 5 47 6
be the difference between actual and desired
values of 47 . Replacing 47 by 8
in the computation of V71
gives
V71
14 6 932
8
14 6 93
8
(10)
1 2
8 1u 6 u d 32
2
(11)
with derivative
V72
1
14 6 93 8 7
8
1
14 6 932
8
9 u7
5 u7 d 6 k4 1u 6 u d 32
6k1 s12
(12)
1
8 y1 u sin 9 6 14 6 932
8
6 k3 2 6 k4 1u 6 u d 32 05
9 f 13 5 u7 5
2
d
8 1u 6 u d 31u7 6 u7 d 35
7
8 f 1 13 5
7 6 1 14 6 93 8 cc s
8 gc s7 2
8
(13)
(15)
Lapierre, Zapata, and Lepinay / Combined Path-following and Obstacle Avoidance Control of a Wheeled Robot
where the f 1 153 and f 2 153 are defines in equation (14) and k1 ,
k2 , k3 , k4 and k9 are arbitrary positive gains, drives y1 , s1 and
4 asymptotically to zero.
Proof: The key steps in the proof can be briefly described as
follows: let V2 be the Lyapunov function candidate expressed
in equation (11). The kinematic control expression (16) yields
V72 0. Since V72 is negative semi-definite and bounded below, V2 is bounded and has a well defined limit. Therefore,
s1 , y1 , 4 and u are bounded, since 9 and u d are assumed to be
bounded. Considering the previous equations, it is straightforward to show that s71 , y71 , 47 and u7 are bounded as well. Direct
derivation allows one to compute the second derivative of V2 ,
and prove it is bounded. Therefore, V72 is uniformly continuous
and application of Barbalats lemma implies that V72 tends to 0
as t tends to . Consider now the expression for V72 in (13).
Since V72 is a sum of negative terms and tends to 0 as t goes to
, we conclude that s1 , y1 , 14 6 93, , and 1u 6 u d 3 tend to
zero as well. That is, the robot asymptotically converges to the
path.
Remarks: The proposed solution consists in a foxrabbit
problem where the rabbit is really cooperative since it adjusts
its own velocity to help the convergence, through the expression:
s7 5 u cos 4 8 k1 s1 5
The first term u cos 4 is the projection of the vehicle (fox) onto
the path where the virtual target (rabbit) is located. The second term is necessary to ensure the convergence of s1 to 0,
making the virtual target asymptotically converge to the closest point with respect to the vehicle current position. In the
problem statement, the condition u d u min 7 0 has been imposed (implicitly covering the assumption A.3). Then the vehicle will not stay at rest and will effectively converge to the
path. The consequence of this is that the virtual target also
will not stay at rest, since 4 and s1 are guaranteed to converge
to zero, and u 7 0, for all t. Then, the path-following problem, as described here can never degenerate to a pose regulation problem, for which Brocketts limitations are inevitable.
The assumption that the variables 4 , s1 , y1 , cc and gc are well
defined implies that there is a path parameterization that allows
one to compute the path curvature cc 1s3 and its spatial derivative gc 1s3, for all s denoting the current curvilinear abscissa
of the virtual target. Moreover, this implies that the system is
equipped with a sensor suite that allows computation of the robot situation in the SerretFrenet frame, i.e. the variables 4 , s1
and y1 .
367
(17)
(18)
We assume that the robot can perceive distances in all directions of space. We also assume that the set of maximum
distances that can be perceived by R and the set of actually
perceived distances are also two DVZ surrounding R respectively named Sensor boundary and Information boundary (and
respectively denoted and ). The deformation I of with
respect to is given by I 5 6 .
Let I , be a DVZ which depends on the Sensor boundary
deformation I :
I 5 1I 35
(19)
The deformation of the DVZ h with respect to I can
be written:
5 I 6 h 5 1I 3 6 1U 35
(20)
368
(21)
(22)
y 8 ay 2
x 8 ax 2
8
5 15
(24)
cx
cy
The coefficients ax , a y , cx and c y are chosen heuristically
and depend on the first component of the control vector, i.e.
the general momentum u, the translational velocity of the robot. We have chosen:
cx
cy
5 cx u 2 8 cmin
x
5
cx
5
3
ax
5 61233cx
ay
5 05
(25)
In the direction with respect to the main axis of the ellipse, let dh 13 be the length of the controlled DVZ h , i.e.
6
the norm of the vector R P. We can write:
41
4
1
cos14 m 3 sin14 m 3
X
x
6
63
6 (26)
RP 5
53
y
Y
6 sin14 m 3 cos14 m 3
where [X Y ]T are the coordinates of point P in the absolute
frame.
Lapierre, Zapata, and Lepinay / Combined Path-following and Obstacle Avoidance Control of a Wheeled Robot
369
Using the definition of dh 13 5 x 2 8 y 2 , we can write:
x
1
(27)
5 dh 13
2
y
where [ 1 2 ]T 5 [cos13 sin13]T is the unit vector in the direction , expressed in the robot frame. Substituting equation
(27) and equation (26) in equation (24) leads to the quadratic
equation:
Adh 132 8 Bdh 13 8 C 5 0
(28)
with:
A
c13 dh 2
5 dh 13 elsehere5
(32)
5
6
5
2
0
1
d13
1
2A
6B8
2
0
B 2 64AC
2A2
61
d13
1
2A
6J Bu 8
(35)
B 2 64AC
J Au d
B J 4 62C J A4
8
6J B 8 B 2
B 64AC
2
6 6B8 2AB2 64AC J A4 d
2
cos d
F Rob5el 5 0 ddh2 13
13
2
F Obst5el 5 0 ddh2 13
16[x7 E cos 14 m 8 3
13
9
8 y7 E sin 14 m 8 3]3 d
(36)
370
where
7
8 J Au
c
21c y uy
x
cos2 13 8 cx c
sin2 133
u
ax
c y
8 ax
5 2 ax c y c y
u
u
a y
cx
8 ay
8 a y cx cx
u
u
c y
cx
8 cy
6 cx c y cx
u
u
(39)
9 r 5 6K J 4
r
and x7 E 2 y7 E define the obstacle absolute velocity. The assumption of static obstacles yields F Obst5el 5 0. For the sake of
simplicity, we consider only static obstacles.
I2
.
2
6 d t
7
J4 1cc s7 8 93
4
2
K r 1J 6 d 3 8 K u 1Ju 32
(44)
(40)
where d 5
(41)
2K d d
.
18d 146932
2
Lapierre, Zapata, and Lepinay / Combined Path-following and Obstacle Avoidance Control of a Wheeled Robot
371
5 14
5 045 (46)
5 6K u u
rC O R N E R
5 rC sign1d 3
(47)
N O A 8 f 13N P F F 5 FO A 8 f 13FP F 4
else
3N
where the selection function is f 13 5 18K1 , with K I a positive gain. Note that this algorithm causes the robot to travel
372
5. Results
The previous solution implicitly requires an estimation of 7
in the computation of FO A . The numerical derivation of the
sensor information induces noise amplification and will not
7 Therefore, we degrade
provide an accurate estimation of .
the previous solution at a kinematic level, that does not require
7 Moreover, the considered robot (cf. Figthe estimation of .
ure 5) accepts as control inputs the desired velocities u and r,
and its rapid actuator response (combined with its light mass
and inertia) allows implementation of a kinematic controller
without degrading the overall performance.
7
8 r 5 r O A 8 f 13r p f
(49)
9 u 5 u O A 8 f 13u p f
where
7
r 5 97 6 K rP F 14 6 93 8 cc s7
PF
t
u P F 5 0 1u7 d 6 K uP F 1u 6 u d 33dt
8
s7 5 u cos 4 8 ks s1
r O A 5 6K rO A 1 6 d 31Jr 6 d 3 8 cc s7 8 97
9 u 5 t 16K O A J 1 6 3 6 F r ob5el u3dt5
OA
d
u
u
0
J
(50)
K uP F 5 1
K rO A 5 0501
K uO A 5 051
K d 5 10
d 5 0501
K I 5 100
ud 5 1
cx 5 100
cmin
x
5 10
K uC O R N E R
5 051
rc 5 1
Lapierre, Zapata, and Lepinay / Combined Path-following and Obstacle Avoidance Control of a Wheeled Robot
373
b) Corner Situation
d) Final Trajectory
Acknowledgements
6. Conclusions
We have designed a combined path following and obstacle
avoidance control law for a unicycle type robot based on the
use of the DVZ concept and on a Lyapunov and backstepping
design. Implementation of this solution on the Wany robot
Pekee illustrates an interesting performance, avoiding unnecessary hot switches when the vehicle travels with an im-
374
Lapierre, Zapata, and Lepinay / Combined Path-following and Obstacle Avoidance Control of a Wheeled Robot
References
Aicardi, M., Casalino, G., Bicchi, A., and Balestrino, A.
(1995). A closed loop steering of unicycle-like vehicles
via Lyapunov techniques. IEEE Robotics and Automation
Magazine, March, 2735.
Aicardi, M., Casalino, G., Indiveri, G., Aguiar, P., Encarnacao,
P. and Pascoal, A. (201). A planar path following controller for underactuated marine vehicles. Proceedings of
MED2001, Dubrovnik, Croatia, June.
Althaus, P. and Christensen, H. (2002). Behavior coordination
for navigation in office environment, IEEE/RSJ International Conference on Intelligent Robots and Systems, Lausanne, Switzerland, October.
Arkin, R. C. (1998). Behavior Based Robotics. MIT Press,
Cambridge, MA.
Canudas de Wit, C., Khennouf, H., Samson, C., and Sordalen,
O. J. (1993). Nonlinear control design for mobile robots.
In Recent Trends in Mobile Robotics (eds Y. F. Zheng), Vol
11, pp.121156, World Scientific Series in Robotics and
Automated Systems. 1993
Elnagar, A. and Hussein, A. (2002). Motion planning using
Maxwells equations. IEEE/RSJ International Conference
on Intelligent Robots and Systems, Lausanne, Switzerland,
October.
Encarnao, P., Pascoal, A., and Arcak, M. (2000). Path following for marine vehicles in the presence of unknown currents. Proceedings of SYROCO2000, 6th IFAC Symposium
on Robot Control, Vienna, Austria.
Encarnao, P. and Pascoal, A. (2000). 3D path following for autonomous underwater vehicle. Proceedings of
CDC2000, 39th IEEE Conference on Decision and Control, Sydney, Australia.
Fierro, R. and Lewis, F. (1997). Control of a nonholonomic mobile robot: backstepping kinematics into dynamics. Journal of Robotic Systems, 14(3): 149163.
Ge, S. and Cui, Y. (2000). Path planning for mobile robots
using new potential functions. Proceedings of the 3rd Asian
Control Conference, Shangai, China, July.
Iniguez, P. and Rossel, J. (2002). A hierarchical and dynamic
method to compute harmonic functions for constrained motion planning. IEEE/RSJ International Conference on Intelligent Robots and Systems, Lausanne,Switzerland, October.
375