The main idea of the method can be clearly illustrated for the nonholonomic integrator,

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

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

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

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

To maintain the constraint that , must be chosen as for some integer . Integration of yields

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.

Steven M LaValle 2012-04-20