1D polynomial trajectory

We want the position and orientation of our robot to vary smoothly with time as it’s following the trajectory. We want to do this for a number of reasons. Firstly, we want to reduce the maximum acceleration on the robot. If we can reduce the peak acceleration of the robot, then we can reduce the size of the motors that are required. If the trajectory is smooth, then it’s not going to induce vibrations into the structure of the robot. And, this is an important issue with very, very large robots. But this raises the question of what is smooth? Now, firstly, we want the position of the robot X as a function of time to be continuous, to have no discontinuities. We also want the velocity to be continuous and the acceleration to be continuous. And perhaps, we want the third derivative, which is referred to as jerk, to be continuous as well and perhaps the fourth derivative and so on. There are very many smooth mathematical functions, sinusoids, Gaussians and polynomials that we could perhaps press in to service.

In robotics, it’s most common to use polynomials. And, the quintic polynomial, that’s a fifth order polynomial, is particularly useful. The quintic polynomial has got six coefficients, A, B, C, D, E and F. We’re going to consider the case S as a function of T. So, S is the position that we’re interested in over a trajectory which starts at time 0 and ends at time T. The advantage of the quintic polynomial is it got six coefficients and that means that it got six boundary conditions. It means that we can independently set the position, the velocity and the acceleration at the start time and at the end time. This table here then shows the names of the symbols that we used to denote these boundary conditions, initial and final position, initial and final acceleration, initial and final acceleration. Here is our quintic polynomial again and I can compute its derivative with respect to time. So, this is the velocity along the path and the acceleration along the path is obtained by yet another derivative.

I can substitute in the boundary conditions. When time equals 0, I have these relationships here. And, when time equals T, I have these relationships here. Now, I can write these six constraint equations in matrix form. It looks something like this. The matrix is 6 by 6. It’s square, which means that I can invert it. And now, what I have is a relationship that gives me the polynomial coefficients A through F in terms of the boundary conditions and a matrix which is essentially a function of the trajectory time and some constants. Let’s create a polynomial trajectory and to do that I’m going to use the Toolbox function tpoly, short hand for trajectory polynomial. The polynomial is going to have an initial value of 0, a final value of 1 and there are going to be 50 time steps.

And here, we can see the trajectory. We can see that S starts off at 0 and rises smoothly and settles out at a value of 1. We can see that the velocity starts at 0 and ends at 0 and the acceleration also starts and ends at the value of 0. And so, change the final condition. So, instead of going from 0 to 1, it goes from 0 to -1 and we can see the polynomial now does exactly that.

I can get the value of the variable S from this function if I provide an output argument and instead of producing a plot it will return the vector S. And now, we have in our workspace S which is a vector with 50 rows and one column, represents the value of S at each of the 50 time steps. I can also ask for the velocity and the acceleration and I’m going put those in to the variables SD and STD for S dot and S double dot. And again, I have created in my work space some additional vector variables, each of them has got 50 rows. The rows correspond to the time steps.

You'll notice in all the plots that the initial and final velocities have been equal to 0. Well, I can override that by providing explicit values for the initial and I’m going to ask the initial velocity to be 0.5 and I’m going to ask for the final velocity to be equal to 0. What you can see is the initial velocity now is equal to 0.5, final velocity is equal to 0. But, what you'll notice is if you look at the plot of S, you’ll see that it has gone from 0 to 1, but it has massively overshot. It has gone up as high as 5 and then reduced to the desired final value. By changing the boundary conditions, we have dramatically changed the shape of the trajectory.

So, we have seen that the quintic polynomial makes nice, smooth curves moving from 0 to 1 over a period of 50 time units and we can see that the velocity is smooth and the acceleration is smooth. But, there’s a bit of a problem here. That is that the peak velocity that the robot undertakes has got a value of 0.038 units, but the average velocity over the path is much, much less. It’s only .02. On average, we’re only going at 52% of the peak velocity for this path. So, that means that for most of the path, the motor isn't working as hard as it could be. It means that we have to install a motor which can go at this peak speed but mostly we’re not using that capacity. We’re wasting performance. It also means that it’s going to take us longer to get where we want to go.