optymalizacja parametryczna1.pdf

(149 KB) Pobierz
Optimal path planning for nonholonomic
robotic systems via parametric optimisation
James Biggs
Advanced Space Concepts Laboratory, Department of Mechanical Engineering, University of
Strathclyde.james.biggs@strath.ac.uk
Abstract.
Motivated by the path planning problem for robotic systems this paper
considers nonholonomic path planning on the Euclidean group of motions
SE(n)
which describes a rigid bodies path in
n-dimensional
Euclidean space. The prob-
lem is formulated as a constrained optimal kinematic control problem where the
cost function to be minimised is a quadratic function of translational and angu-
lar velocity inputs. An application of the Maximum Principle of optimal control
leads to a set of Hamiltonian vector field that define the necessary conditions for
optimality and consequently the optimal velocity history of the trajectory. It is
illustrated that the systems are always integrable when
n
=
2 and in some cases
when
n
=
3. However, if they are not integrable in the most general form of the
cost function they can be rendered integrable by considering special cases. This
implies that it is possible to reduce the kinematic system to a class of curves de-
fined analytically. If the optimal motions can be expressed analytically in closed
form then the path planning problem is reduced to one of parameter optimisation
where the parameters are optimised to match prescribed boundary conditions.
This reduction procedure is illustrated for a simple wheeled robot with a sliding
constraint and a conventional slender underwater vehicle whose velocity in the
lateral directions are constrained due to viscous damping.
1 Introduction
In recent years there has been a great deal of research that involves motion planning
in the presence of nonholonomic constraints. Several books give a general overview of
the nonholonomic motion planning problem (MPP) [1, 2] for nonholonomic mechan-
ical systems, and [3–5] in the context of robotics. Nonholonomic motion planning is
challenging because nonlinear control theory does not provide an explicit procedure
for constructing controls. In addition linearisation techniques, effective for nonlinear
systems, fail to be useful, as highlighted in [6] that linearisation renders such systems
uncontrollable. The computation of feasible trajectories for nonholonomic systems is
a complex task and generally treated using numerical methods. However, numerical
methods have the drawback that they are inherently local and not guaranteed to find a
feasible solution. Additionally, such methods as dynamic programming [7] may provide
global optimal solutions, but are often computationally expensive. However, when the
configuration space can be represented by a Lie group the motion planning algorithms
can be designed to exploit the underlying structure of the system.
Sophus Lie introduced and developed his ideas on continuous transformation groups
that leave certain mathematical structures invariant. This comprehensive theory of in-
finitesimal transformations is now known as the study of Lie groups and their accom-
panying Lie algebras. Lie focussed on using these transformation groups to solve dif-
ferential equations, see [8]. However, since then Lie groups have made a significant
contribution to many diverse areas of mathematics and theoretical physics and have
more recently made an appearance in the control literature. In particular the seminal
paper [9], puts the theory of Lie groups and Lie algebras into the context of nonlinear
control theory to express notions of controllability and observability for invariant sys-
tems evolving on matrix Lie groups. One of the most important consequences of this
work was the recognition that questions about these kind of systems on Lie groups can
be reduced to questions about their associated Lie algebra. Lie algebras are simpler than
matrix Lie groups, because the Lie algebra is a linear space. Thus, much can be under-
stood about a Lie algebra by performing Linear algebra. For nonholonomic systems
defined on Lie groups, the MPP methodologies are naturally based on Lie-algebraic
techniques. The general idea is to use a family of control functions i.e. piecewise con-
stant controls, periodic controls or polynomial controls to generate motions in the di-
rections of iterated Lie brackets, that is, steering the system in directions that are not
directly controlled. These types of methods were first initiated using piecewise con-
stant inputs, see for example [10], where the authors propose a general motion planning
algorithm for kinematic models of nonholonomic systems. The algorithm is based on
expressing the flow, resulting from constant control inputs, as an exponential product
expansion involving iterated Lie brackets. For nilpotent systems i.e. systems for which
all iterated Lie brackets of high order are zero, the algorithm provides exact steering.
The use of different families of control functions other than that of piecewise constant
controls are used in [11], [12], [13] where open loop control laws are derived using a
family of periodic controls at integrally related frequencies. The problem then amounts
to finding appropriate frequencies and amplitudes for the periodic controls.
These methodologies provide a constructive procedure for motion planning based
on the iterated Lie bracket in that they provide a feasible path between two configura-
tions (at least approximately). However, in general there will be more than one feasible
path between two configurations with some more practically desirable than others. As
in dynamic programming a specific path can be selected by optimising the manoeuvre
with respect to some pre-specified cost function. Despite the numerous optimisation
tools available, the use of optimal control theory to tackle the MPP has had little im-
pact on practical applications, presumably because the delicate numerical treatment of
optimal control problems is often less suited to practical implementation than other
methods. However, since the development of geometric control theory, new approaches
have arisen, distinguished from numerical methods, in that they exploit the systems
underlying analytic structure. The use of optimal control theory to tackle the MPP for
nonholonomic systems on Lie groups is studied in [14–18, 2, 9]. In Brockett’s seminal
papers [15, 16] it is shown that a number of these optimal control problems are com-
pletely solvable in closed form where the optimal angular velocities can be expressed
as analytic functions such as trigonometric or Jacobi elliptic functions. In this paper
the focus is placed on the group
SE(n)
where
n
=
2, 3 which have direct applications
to robotic systems. The cost function used is an integral function of angular velocities.
The function yields smooth curves where the velocity along the curve is minimised and
therefore the forces required to track are theoretically small. The emphasis in this pa-
per is placed on integrable cases that not only lead to closed form expressions for the
optimal velocity inputs but also closed form expression for the corresponding motions.
Contrary to integrable systems in mechanics, which are few and far between, this set-
ting allows us to render the closed-loop system integrable by appropriate manipulation
of the cost function. This can be achieved by setting weights of the cost function to
be equal which essentially introduces enough symmetry into the resulting Hamiltonian
system for it to be integrable. However, setting weights to be equal reduces the gener-
ality of the cost function and therefore the class of solution. Reducing the MPP to an
analytic closed form solution essentially reduces the MPP to a problem of optimising
the available parameters of these analytic functions to match prescribed boundary con-
ditions. In addition an iterative approach can be used to select the parameters to avoid
stationary and known obstacles.
1.1 Nonholonomic Systems on the Euclidean Groups
SE(n)
From a practical point of view the kinematics of robotic systems can often be framed
as nonholonomic control systems on the Euclidean Lie groups
SE(2)
(position and
orientation in the plane) and
SE(3)
(position and orientation in space). Examples of
these are the “vertical” rolling disk or Unicycle [18], hopping robots [19] on
SE(2)
and
Autonomous Underwater Vehicles [6], Unmanned Air Vehicles [20, 21] and robotic
manipulators [22] on
SE(3).
The nonholonomic kinematics of these systems can be
expressed as:
s
dg(t)
=
∑
u
i
(t)X
i
(1)
dt
i=1
where the curve
g(t)
∈
SE(n)
describes the motions of the system in the configuration
manifold
SE(n). X
1
, ...,
X
n
are arbitrary vector fields in the tangent space
T SE(n)
at
g(t),
denoted
T
g(t)
SE(n)
and
u
1
, ...,
u
s
are the control functions. It follows that
X
1
, ...,
X
s
∈
T
g(t)
SE(n)
are the controlled vector fields on the manifold
SE(n)
and
X
0
=
∑
X
i
∈
T
g(t)
SE(n)
is the drift vector on
SE(n).
For nonholonomic systems
n
>
s.
The cases
considered are when
X
i
are left (respectively right) invariant vector fields on
SE(n),
see
[17], the vector fields can be expressed as
X
1
=
g(t)A
1
, ...,
X
n
=
g(t)A
n
∈
T
g(t)
SE(n),
where
A
1
, ...,
A
n
∈
T
I
SE(n)
are basis elements of the tangent space at the identity
I
∈
SE(n).
The tangent space at the identity is called the Lie algebra denoted
se(n).
It
follows that the differential equation (1) can be expressed as:
s
dg(t)
=
g(t)(
∑
u
i
(t)A
i
)
dt
i=1
i=1
m
(2)
where
A
1
, ...,
A
n
∈
se(n)
where the Lie algebra
se(n),
is a vector space together with the
matrix commutator, the Lie bracket:
[X,Y ] =
XY
−Y
X
(3)
where
X,Y
∈
se(n)
where for
n
=
2 the basis of the Lie algebra is
001
000
0
−1
0
A
1
=
0 0 0
,
A
2
=
0 0 1
,
A
3
=
1 0 0
000
000
0 0 0
satisfying the following relations
[A
1
,
A
3
] =
−A
2
,
[A
2
,
A
3
] =
A
1
,
[A
1
,
A
2
] =
0.
and when
n
=
3 the basis for the six-dimensional Lie algebra is:
00 0 0
0 0 00
000 0
0 0 0 0
,
A
2
=
0 0 0 1
,
A
3
=
0 0
−1
0
A
1
=
0 1 0 0
0 0 0 0
0 0 0
−1
00 0 0
0
−1
0 0
001 0
0000
0000
0000
1 0 0 0
,
A
5
=
0 0 0 0
,
A
6
=
0 0 0 0
A
4
=
0 0 0 0
1 0 0 0
0 0 0 0
0000
0000
1000
and satisfies the commutative table:
[, ]
A
1
A
2
A
3
A
4
A
5
A
6
A
1
0
-A
3
A
2
0
-A
6
A
5
A
2
A
3
0
-A
1
A
6
0
-A
4
A
3
-A
2
A
1
0
-A
5
A
4
0
A
4
0
-A
6
A
5
0
0
0
A
5
A
6
0
-A
4
0
0
0
A
6
-A
5
A
4
0
0
0
0
(4)
(5)
Note that the driftless system in (2) can be augmented to include systems with drift by
setting one of the controls
u
i
to a constant a priori. The method described in this paper is
also applicable to these cases. Controllability for systems of the form (2) can be assessed
through computations performed at the level of the Lie algebra involving the Lie bracket
(3), see [17, 2]. For example if
[X,Y ] =
Z
for some
Z
∈
se(n),
then it is possible to move
in the direction of the vector field
Z
by controlling only the vector fields
X
and
Y
. From
a control theory viewpoint this is particularly useful if
Z
∈
span{X,Y
}.
Indeed if it is
/
possible to directly control the vector fields
X
1
, ...,
X
s
then motions can be generated in
the direction of its iterated Lie brackets:
X
i
,
[X
i
,
X
j
], [X
i
,
[X
j
,
X
k
]],
...,
(6)
1
i, j, k,
...
s.
In other words, although not all directions are controlled, it may be
possible to obtain motions in all of them, by taking sufficiently many Lie brackets. If
the system is controllable then it is possible to construct a well defined optimal control
problem. This is formalised in the following subsection.
1.2 Problem Statement
Subject to the kinematic nonholonomic constraint given by (2) and given that the system
is controllable the problem is then to find a trajectory
g(t)
∈
SE(n)
from an initial
position and orientation
g(0)
∈
SE(n)
to a final position and orientation
g(T
)
∈
SE(n)
where
T
is some fixed final time that minimises the functional
J
=
1
2
T
0
c
i
u
2
dt
i
(7)
where
i
=
1,
...,
s
and
c
i
are constant weights. This cost function is not conventional but
it is meaningful as it ensures smooth motions which, given a reasonably long enough
fixed final time, will not require large torques and forces to track them. In addition
it enables the MPP to be formulated in the context of geometric optimal control and
this enables us to ask questions of integrability and in some cases solve the system in
closed form. Furthermore, obtaining a closed form solution essentially reduces the MPP
to a problem of optimising the available parameters to match the prescribed boundary
conditions.
2 Methodology
The methodology for MPP comprises of the following phases:
1. Lifting the optimal control problem on
SE(n)
to a Hamiltonian setting via the max-
imum principle of optimal control and Poisson calculus.
2. Solving integrable cases of the Hamiltonian vector fields analytically in the most
general form of the cost function (7).
3. Given the optimal velocities derive the corresponding motions in
SE(n)
analytically
reducing the MPP to a parameter optimisation problem.
4. As the boundary conditions are not contained in the cost function it is necessary
to numerically optimise the available parameters of the analytic solutions to match
the prescribed boundary conditions (This stage is not covered in this paper.)
2.1 General Hamiltonian lift on SE(n)
The application of the coordinate free Maximum Principle to left-invariant optimal con-
trol problems are well known, see [23], [17]. As the Hamiltonian is left-invariant the
cotangent bundle
T
∗
SE(n)
can be realised as the direct product
SE(3)
×
se(n)
∗
where
se(n)
∗
is the dual of the Lie algebra
se(n)
of
SE(n).
Therefore, the original Hamilto-
nian defined on
T
∗
SE(n)
can be expressed as a reduced Hamiltonian on the dual of the
Lie algebra
se(n)
∗
as
T
∗
SE(n)/SE(n)
∼
se(n)
∗
. The appropriate Hamiltonian for the
=
constraint (2) with respect to minimizing the cost function (7) is given by (see [17] for
details):
s
1
s
H(p, u, g)
=
∑
u
i
p(g(t)A
i
)
−
ρ
0
∑
c
i
u
2
(8)
2
i=1 i
i=1
where
p
∈
T
∗
SE(n)
(where
n
=
1 or
n
=
2) and
ρ
0
=
1 for regular extremals and
ρ
0
=
0
for abnormal extremals. In this paper we consider only the regular extremals, therefore

Plik z chomika:

Inne pliki z tego folderu:

Inne foldery tego chomika:

Zgłoś jeśli naruszono regulamin