Trajectory optimization for smoothness

Question

I want to achieve the following in 2D (and without obstacles):

Given start position A and end position B, generate the path between the two points that optimizes a cost function that depends on total length and overall smoothness. The one constraint is a tangential constraint (ie. velocity constraint) at point A.

For example, the cost function could be

$$U = \int\limits_{t_\mathrm{A}}^{t_\mathrm{B}} a^2(t) dt + L$$

with $a$ the acceleration and $L$ the path length.

Do you know of a simple way I could solve this optimization problem, preferably using Python?

Answer 1

You will have to transcribe the control problem in some manner so that you can feed it to a non-linear programming solver. You may want to look at Matthew Kelly's paper for a good introduction to transcription methods for trajectory optimization problems. It basically summarizes transcription methods as follows:

There are various classifications of transcription methods. The most intuitive is to discretize the problem first, then find an optimal solution to the discrete problem. This is referred to as a Direct Method. One could do the reverse, which is to optimize before discretizing. This is referred to as the Indirect Method. According to the references in the paper, the Direct Method is easier to pose and solve than the Indirect Method, but also leads to a less accurate solution than the indirect method.

While I'm not particularly familiar with indirect methods, I can certainly speak about direct methods. In particular, there are two main classes of discretization for a direct method: Shooting and Collocation. Shooting works essentially by trial and error, adjusting control parameters until the trajectory becomes optimal. Shooting can even be broken up over multiple discrete intervals in the time domain (also called Multiple Shooting).

Alternatively, collocation works by replacing the ordinary differential equation with a system of algebraic ones (say, using finite difference approximation) and by representing the continuous control in terms of interpolants whose values at fixed timesteps (also known as knots) are resolved by the non-linear programming solver. It is common to use spline interpolants for this purpose so that . In the end, one obtains a non-linear objective function with algebraic constraints over a finite number of knots. One systematically obtains higher accuracy by either increasing the number of timesteps (h-refinement) or by increasing the order of the interpolants (p-refinement).

I highly recommend reading the aforementioned paper, along with watching a lecture the author gave about this very same topic: Introduction to Trajectory Optimization . The video includes a worked out example involving control of an inverted pendulum.

The control function between the knots will be smooth by virtue of the interpolant function. I'm not sure how to impose smoothness at the knots themselves, and I'm not sure if techniques to impose this constraint exist for this kind of problem.