Optimal Path Planning Using the Minimum-Time Criterion

by James Bobrow

Guha JayachandranApril 29, 2002

Background

Automatic generation of large motions for robots is a problem for many applications (automated factories, for example)

Often break problem into subproblems such as collision free path planning, time optimal control along specified paths, feedback control along specified path using known velocity profile, and vision based planning

Our Focus

Here, attack problem of finding collision free path with minimum time motion

Assume that have an initial collision free path, that manipulator equations of motion are known, and that a geometric description of the workspace is available

We will optimize the path for time

Criterion

Why “minimum time?”

Usually infinitely many collision free paths possible so must choose criterion

Usually, choose minimum distance traversed as criterion, but this has key drawbacks

Diagrams from Frederic Mazzella’s 2001 presentation.

Definitions

Following work will be in relation to a three degree of freedom, elbow type robot (but procedure can be used generally)

3D Cartesian path of end effector represented with uniform cubic B spline polynomial

Parameterization for motion in x dimension, for example, given by where vi are specified

path vertices and bi(s) are piecewise-cubic B spline basis functions

More on the Function

For any s, only 4 of bi(s) are nonzero! This means summation only 4 terms and gives local control of path shape as any position is influenced by only its 4 closes neighboring vertices

Velocity Profile for s

Path of robot end effector totally defined by s.

From path definition, time optimal velocity profile for s can be determined using an algorithm not in this paper (in “Time-optimal control of robotic manipulators

along specified paths” by Bobrow, Dubowsky, and Gibson)

Idea of that algorithm is that since for any s, there is a s’max above which no combination of admissible joint torques will keep manipulator moving on the path, can find optimum s’(t) by integrating equations of motion using maximum or minimum of available joint torques that will keep robot on path and keep v below s’max at every s

Equation to Optimize

Having all these definitions, can say time is given by

We want to minimize this.

Constraints

- Robot equations of motion must be satisfied

- Torques must be within bounds

- Initial and final joint positions must be reached

- Avoid collisions!

Formal Statement of Problem

“Find the B spline vertices vi, i = 1, 2, …, n-1 which, along with the specified values of v0, vn, and the computed values of v-1 and vn+1, define a path that minimizes”

subject to constraints on previous slide.

Gradients and Derivatives

To do the optimization, need gradients of tf and of equations for distance from obstacle and joint position

These are hard and “extremely tedious” to calculate

Points where distance from obstacle function not differentiable, but in practice, didn’t seem to be a problem

Example

Find optimal path between Start and Goal.

Assume no obstacles.

Initial path was straight line.

First optimization: path broken into 2 cubic B spline intervals with x and y positions of center vertex being parameters to vary

30 Iterations

After 30 iterations, nice curved path obtained.

0.428 seconds versus 0.470 seconds for the straight line

Optimal Phase Plane Trajectory

Convergence Properties

Path broken into more intervals (1, 3, 5, and 8 intermediate B spline vertices tried)

Difference in optimum traversal time between when using 1 intermediate vertex and using 8 intermediate vertices is just 2.5%

Computation time increases linearly with number of vertices.

Another Example

3 dimensional path with obstacle in workspace

Premature Termination

As increase number of B spline intervals, premature termination of optimization program observed.

Due to high local path curvature requiring robot to slow down to stay on path, causing local minima for the optimization

Regularization

Premature termination problem partially solved by regularization.

Penalizes highly curved or irregularly shaped functions.

To regularize minimization, add a measure of total path curvature to the objective function.

True Optimum Reached?

Question “very difficult—if not impossible—to answer”!

In experiments, seems to converge, but no proof.

Optimality conditions for unconstrained path, fixed endpoint, minimum time motion problem can be derived using “Maximum Principle,” which offers some insight

Conclusion

Nice, simple algorithm which takes care automatically of things like obstacles

Proof of convergence would be nice

Computationally expensive.