# Trajectory Planning: Polynomials as universal, integrable approximation functions?

I'd like to program a trajectory planner, let's say for a robot, and I can pass acceleration commands to the robot, the robot can move in one dimension. The outcome of the planner is now a vector with accelerations, for example 10 accelerations, and since I set the accelerations with a frequency of 2, this will be sufficient to steer the robot for the next 5 seconds.

The trajectory should be planned in a way that (a) minimizes jerk and (b) is as near at the desired velocity as possible. So I know what makes up my fitness function: sum over squared jerks and squared differences v, v_desired.

So I want to minimize jerk. I thought I search for a jerk profile (that is, jerk values at 10 discrete times, like 0.5, 1, ..., 5, to be consistent with the example above), integrate this twice so that I also have the speed profile, and apply my fitness function. The jerk profile is searched with a particle swarm optimizer. The nice thing is, 0 is already a good initialization for the profile.

Then I thought about approximating the jerk profile with a polynomial, and not to search for the profile directly, but for the polynomial parameters. This way, integration would be trivial, everything would be steady and smooth, and I could sample points at whatever times I'd like. At least I thought this was a good idea.

So my question is: Are polynomials a good tool for this? I can't believe that estimating e.g. 5 parameters (and evaluating the resulting polynomial at e.g. 30 positions) will yield better results than searching for 30 points (the jerk) directly. My experiments from the last hours shows that the curves very quickly start looking like waves, and that's not what you want for a trajectory, especially not for the velocity profile ...

Is my approach with "estimate function parameters rather than sample points" bad? Is a polynomial bad? Are there better functions, easily to extend in number of parameters and trivially to integrate?

I hope my question is understandable, if not, please complain and I will try being more specific.

UPDATE

The picture below shows the result of my first approach, with "optimizing" parameters for a 4th degree polynomial for jerk. This was integrated with the correct initial values for a, v and s in my one-dimensional world. The initial v was set to 5.0, v desired is 2.0, and initial acceleration was 2.0. I would expect the velocity to first increase (caused by the initial acceleration), and then decrease slowly to 2. At point number 10, you can see the "overshoot" of v and the wiggly behaviour mentioned in the first comment.

• It might be more profitable to use piecewise polynomials instead of polynomials, since high-degree polynomials tend to wiggle rather wildly. You might want to look into using splines with the appropriate sort of continuity; maybe $C^4$ quintic splines? May 19, 2013 at 14:54
• Wiggly – exactly this is the word I should have found when formulating my question. Regarding splines: So it should be possible to say "my acceleration function consists of 5 splines" and I then search for good parameters? I know splines only for interpolation. May 19, 2013 at 14:57
• If it's not too much trouble, can you post a plot of the points you were using to build a path? I see that you do not yet have sufficient rep, so at least post a link to the picture, and someone can edit your post to embed the picture. May 19, 2013 at 15:00
• "my acceleration function consists of 5 splines" - more like "a spline with $n$ pieces", with each piece being a suitable low-degree polynomial. I suggested looking for something with $C^4$ continuity, since it seems you want the jerk to be continuous as well; quintic splines can fit the bill. (Cubic splines only have $C^2$ continuity, so not suitable for you.) May 19, 2013 at 15:02
• Since I integrate, will cubic not be sufficient? I understand your comment like you care that I cannot differentiate cubic splines often enough ... May 19, 2013 at 15:25

• "inversion of a matrix" - you shouldn't have to invert a matrix; the matrix that often turns up in constructing splines is tridiagonal (or at worst banded), and there are algorithms specially adapted for solving tridiagonal systems that don't need you to have a full $n\times n$ array. May 20, 2013 at 12:14
If you approximate a function on an interval by polynoms and you have the freedom to choose x, the approximation points, the recommend method is to use gauss quadrature points and the corresponding polynom basis. Once the approximation is obtained you are free to evaluate it of course anywhere in the interval. So use chebyshev polynoms together with chebyshev nodes. This way you won't see any oscillations and you can easily integrate / differentiate the solution on the entire domain.