The main idea of the method can be clearly illustrated for the
nonholonomic integrator,
|
(15.146) |
which was considered throughout Section 15.5.1. This case
will be explained in detail, and the methods obtained by generalizing
the principles will subsequently be stated. The presentation given
here is based on [727,846].
As was previously indicated, growing independent vector fields as
quickly as possible is important. For the nonholonomic integrator,
, is linearly independent of and , as observed
in Example 15.12; thus, it satisfies this property.
Consider steering the system from some
to some
while optimizing the cost functional
|
(15.147) |
The problem can be solved by using the constrained Lagrangian
formulation, which was given in Section 13.4.3. The first
step is to eliminate the variables. From (15.146),
the cost can be expressed in terms of
and
by using
and
. The third equation in
(15.146) can be written as
|
(15.148) |
and will be interpreted as a constraint on the Lagrangian, which is
combined using a (scalar) Lagrange multiplier as explained in Section
13.4.3. Define the Lagrangian as
|
(15.149) |
in which the first term comes from the integrand of
(15.147), and the second term comes from
(15.148).
The Euler-Lagrange equation (13.118) yields
|
(15.150) |
Note that
implies that
is constant for
all time. To obtain a differential equation that characterizes the
optimal action trajectory, use the fact that for ,
and
. This yields the equations
and
. These can be
represented as second-order linear differential equations. Based on
its roots, the solution is
|
(15.151) |
Given initial and goal states, the optimal action trajectory is found
by determining , , and . Suppose that
and
for some
. Other cases can be obtained by applying transformations in
to the solution.
The state trajectories for and can be obtained by
integration of (15.151) because
for
and . Starting from
, this yields
|
(15.152) |
To maintain the constraint that
, must
be chosen as
for some integer . Integration of
yields
|
(15.153) |
The cost is
|
(15.154) |
The minimum cost is therefore achieved for , which yields
and
. This fixes the magnitude of
, but any direction may be chosen.
The steering problem can be solved in two phases:
- Apply any action trajectory to steer and to their
desired values while neglecting to consider .
- Apply the solution just developed to steer to the goal
while and return to their values obtained in the first
phase.
This idea can be generalized to other systems.
Steven M LaValle
2020-08-14