PATH PLANNING USING DISCRETIZED EQUILIBRIUM PATHS
A Robotics Example
Cornel Sultan
Aerospace and Ocean Engineering Department, Virginia Tech, 215 Randolph Hall, Blacksburg, VA, U.S.A.
Keywords: Nonlinear control, equilibrium path, robot path planning.
Abstract: A collision avoidance path planning problem is considered and a simple solution which uses piecewise
constant controls generated by discretizing a feasible equilibrium path is presented and investigated.
1 INTRODUCTION
A new methodology has been recently proposed
(Sultan, 2007) for the control of nonlinear ODEs,
.,,
),,(
RTtRUuRXx
uxf
dt
dx
x
mn
⊂∈⊂∈⊂∈
==

(1)
Here f is a function of class C
k
in
UX ×
(k > 0), x,
u, and t are the state, control vectors, and time,
whereas X, U, and T are open sets in the n, m, and
one dimensional real spaces.
The key idea is to control (1) such that its state
space trajectory is close to an equilibrium path
obtained by solving
),(0 uxf= .
(2)
If
),(
ii
ux is a solution of (2) and
),(
iii
ux
x
f
J
∂
∂
=
is
not singular, there exist an open set U
e
and a unique
function g of class C
k
such that
.:g ,0)),((
),(),(
ee
ii
XUuugf
ugxugx
→=
==
(3)
Here U
e
is the largest domain in U in which (2) can
be solved uniquely for x as in (3) and
)),(( uug
x
f
∂
∂
is
not singular. If
),(
ff
ux
,
ef
Uu ∈
is a different
solution of (3),
i
u and
f
u
can be connected by a
curve
)(su
e
in
e
U , parameterized by
],0[
τ
∈s
,
feie
uuuu =
=
)(,)0(
τ
,
(4)
which is g-mapped onto an equilibrium path,
))(()( sugsx
ee
=
, ,)0(
ie
xx =
fe
xx =)(
τ
.
The control problem is to develop control laws
which guarantee that the state space trajectory of the
system is close to the equilibrium path, as illustrated
in Figure 1. In order to achieve this goal, the strategy
described next was proposed in (Sultan, 2007). The
controls are initially fixed at u
i
and when the
transition begins, at t=0, they start to vary along
e
u ,
)()( tutu
e
=
,
Tt ⊂
∈
],0[
τ
. When t reaches
τ
the
controls are frozen at the final, desired value:
⎪
⎩
⎪
⎨
⎧
>
≤≤
<
=
τ
τ
tu
ttu
tu
tu
f
e
i
,
0 ),(
0,
)(
.
(5)
The corresponding state space trajectory,
)(tx
d
,
called the deployment path, is the solution of
iddd
xxtuxfx =
=
)0()),(,(

.
(6)
If
)(
τ
d
x
belongs to the basin of attraction of
f
x
then the system’s trajectory will settle down,
asymptotically in time, to the desired final value,
f
x . Asymptotical stability of
f
x is crucial for the
application of this methodology.
236
Sultan C. (2008).
PATH PLANNING USING DISCRETIZED EQUILIBRIUM PATHS - A Robotics Example.
In Proceedings of the Fifth International Conference on Informatics in Control, Automation and Robotics - SPSMC, pages 236-239
DOI: 10.5220/0001480602360239
Copyright
c
 SciTePress

Figure 1: Deployment and equilibrium paths.
In this paper an example of a collision avoidance
path planning problem is considered. An equilibrium
path which satisfies the constraints is found and
discretized to generate piecewise constant controls
which are used to drive the system. It is important
to remark that this strategy is different from the one
proposed in (Sultan, 2007), where continuous
controls are used. Here, the parameterization of the
equilibrium path, originally continuous, is
discretized. One justification for this approach is the
easiness of discrete controls implementation.
2 THEORETICAL RESULTS
In the following two important results are given (the
proofs are omitted for brevity).
Theorem 1. If the equilibrium path is composed
only of asymptotically stable equilibria then, for
0>∀
ε
there exists a piecewise constant control
)(tu , obtained by discretizing the equilibrium path,
such that the distance between the corresponding
segments of the deployment and equilibrium paths is
less than
ε
(i.e. the deployment and equilibrium
paths are arbitrarily close).
Theorem 2. If the equilibrium path is composed
only of asymptotically stable equilibria and for any
u,
),( uxf is Taylor series expandable in x, for
0>∀
η
there exists a piecewise constant control
)(tu , obtained by discretizing the equilibrium path
such that
],0[,)( τη ∈∀< ttx
d

.
3 A PATH PLANNING PROBLEM
Consider a two link robotic manipulator in the
vertical plane (Figure 2). The links are rigid, the
system is placed in a constant gravitational field,
control torques and damping torques proportional to
the relative angular velocity between the moving
parts act at the joints. The equations of motion are:
111211
2
221212
1122121211
2
12
2
11
)sin()()sin(
)cos()(
uglmcmclm
dclmIlmcm
=++−
++−+++
θθθθ
θθθθθ


(7)
2222
2
121212
12222
2
22121212
)sin()sin(
)()()cos(
ugcmclm
dIcmclm
=+−
−−+++−
θθθθ
θθθθθθ


(8)
where angles
21
,
θ
θ
describe the motion, m
i
, l
i
, c
i
, I
i
,
are the mass, length, center of mass (CM) position,
transversal moment of inertia of the i-th link, d
i
and
u
i
are the damping coefficient and control torque at
joint i, respectively, g is the gravitational constant.
These equations can be easily cast into the first order
form (1). The numerical values (SI units) used are:
.81.9,5.0,12/
,5.0,3/1,5,10
21
212121
====
======
gddlmI
ccllmm
iii
(9)
The system must transition between two
equilibria,
0,70
21
==
ii
θθ
,
0,70
21
=−=
ff
θθ
.
Collision with a circular sector obstacle, of radius
R=1, described below, must be avoided:
.6030,60
300,0
3
1
)sin(
)30sin(
060,60
112
1
21
2
112
<≤−>
<<<−
−
−
≤<−+>
θθθ
θ
θθ
θ
θθθ
if
if
if
(10)
1
θ
2
θ
C
1
C
2
g
ˆ
R
l
1
l
2
Obstacle
CM
1
Figure 2: Two link robotic manipulator.
Equilibrium Path
Deployment Path
ε
x
f
x
i
PATH PLANNING USING DISCRETIZED EQUILIBRIUM PATHS - A Robotics Example
237

An equilibrium path which satisfies (10) is
].,[,cos62
111
11
1
2 fie
if
e
e
θθθ
θθ
θ
θ
∈
⎟
⎟
⎠
⎞
⎜
⎜
⎝
⎛
−
=
(11)
and the equilibrium controls are easily found,
).sin(
),sin()(
2222
112111
ee
ee
gcmu
lmcmgu
θ
θ
=
+=
(12)
The equilibrium path is parameterized using the
following class C
2
function
,0 ,)(
3
)(
630
)(
30
)(
2
345
11
5
11
τττ
θθ
τ
θθ
≤≤
⎟
⎟
⎠
⎞
⎜
⎜
⎝
⎛
−+−−
−+=
tt
t
t
tt
t
ifie
(13)
which is further discretized to obtain piecewise
constant controls using (11) and (12).
Consider
10=
τ
(“fast deployment”). Piecewise
constant controls are generated using N equal time
intervals. Figure 3 shows the deployment and
equilibrium paths.
Figure 3: Deployment paths for “fast” deployment.
Figures 4 and 5 give the time histories of the
controls and angles for N=17 and N=40. The
deployment error cannot be made small enough to
avoid the obstacle regardless of how large N is
(higher values of N were considered). Thus
τ
should be increased and the controls refined for the
deployment error to be sufficiently small.
Figure 4: Controls variation for “fast” deployment.
Figure 5: Generalized coordinates variation for “fast”
deployment.
In the second scenario, called “slow” deployment,
the deployment time is
20=
τ
and piecewise
constant controls are generated by discretizing (11-
13) with N=34 and N=80. Figures 8-10 show that
collision is avoided. The deployment error is smaller
because the deployment time is longer and finer
controls are used. It is important to mention that if
only the deployment time is increased the desired
result is not obtained; if N=17 or N=40 are used in
conjunction with
20
=
τ
, the deployment error is
still big and collision with the obstacle occurs.
ICINCO 2008 - International Conference on Informatics in Control, Automation and Robotics
238

Figure 6: Deployment paths for “slow” deployment.
Figure 7: Controls variation for “slow” deployment.
Figure 8: Generalized coordinates variation for “slow”
deployment.
4 CONCLUSIONS
An example of a path planning problem is used to
illustrate the control of nonlinear systems using
equilibrium paths. The idea is to find an equilibrium
path which satisfies the collision avoidance
constraints, which is a much easier problem than
finding a dynamic path which satisfies the
constraints. Then the equilibrium path is discretized
to build piecewise constant controls which are used
to drive the system. Simulations indicate that for the
deployment and equilibrium paths to be close the
deployment time should be sufficiently long and the
controls sufficiently refined.
It is important to remark that the solution
investigated here uses discretizations of an
equilibrium path which satisfies the collision
avoidance constraints as opposed to continuous
parameterizations and hence continuous controls.
One justification for this approach is the easiness of
practical implementation of discrete controls.
REFERENCES
Sultan, C., 2007. Nonlinear systems control using
equilibrium paths. In Proceedings of the Conference
on Decision and Control, New Orleans, LA, USA.
PATH PLANNING USING DISCRETIZED EQUILIBRIUM PATHS - A Robotics Example
239