How do you calculate a trajectory through a series of 3D points using cubic splines?

I am trying to write a program that constructs a trajectory from an intial position [x1,y1,z1] to an end position that passes through a series of intermediate points. e.g. the program accepts three arrays x=[1,6,2,5,3], y=[4,1,6,4,1,1] and z=[9,5,1,5,1,2]. From what I can gather, cubic splines could be implemented in order to achieve this, however I'm not sure if this is the most suitable approach. Does any one have any alternative options? I've managed to find examples of cubic splines in 2 dimensions (i.e. x and y directions) but none that are applied to three axes and if this is the correct approach, I am not sure how to extrapolate this to 3 dimensions. If someone could provide an explanation and example, I would be very greatful! Essentially, I am aiming to write a program that iterates through the spline incrementally so as to achieve a smooth trajectory that could be followed by a simulated robot.

[David Goodmanson]

Hi Sara,

Interpolation based on the point number works pretty well when the points are similar distance from each other along the path, but it has limitations when the spacing between points varies significantly. A more natural parameter than point number is the arc length along the path. You of course don't know the arc length along the path that is going to be produced by the spline fit. But you can calculate the lengths of straight line segments between the points that you have, and get a decent arc length estimate that way. I don’t want to just drop in a finished product, so here is the basic idea. Arc length between two points is

sqrt( (xb-xa)^2 + (yb-ya)^2 + (zb-za)^2 )

You could calculate all of those with a for loop, but you can find the (squared) x pair distances all at once using diff(x).^2. Same for y and z. Sum the contributions, take the square root and you have the set of distances between pairs of points. Since the starting point is distance 0 from the first point, you can use square brackets to append 0 to the beginning of the distances vector. Then use cumsum to produce a running arc length as you travel along the points. That vector can be interpolated.

You can set up a fine array of points between 0 and arclength(end) then interpolate the x, y, z coordinates using splines similarly to what Akira did. You can also try pchip instead of spline. Spline takes a wide path between points 1 and 2, and pchip has an awfully sharp corner at point 3, but you are the best judge of what is needed. Also, the 'makima' option is worth checking out. It is supposed to be in between pchip and spline but in this case appears to be closer to pchip than spline.

Plotting interpolated points as in plot3(xx,yy,zz,'o-') is quite useful and shows the resulting point spacing.

Answer 1

[Akira Agata]

One possible straight-forward way would be like this:

Actually, spline interpolation seems to be better than cubic spline...

But if you want to use cubic spline, please use 'pchip' option, instead of 'spline'.

% Sample point
x = [1,6,2,5,3,1];
y = [4,1,6,4,1,1];
z = [9,5,1,5,1,2];
t = [1,2,3,4,5,6]; % Assumed time stamp

% Apply interpolation for each x,y and z 
tt = linspace(t(1),t(end));
xx = interp1(t,x,tt,'spline');
yy = interp1(t,y,tt,'spline');
zz = interp1(t,z,tt,'spline');

% Visualize the result
figure
scatter3(x,y,z)
hold on
plot3(xx,yy,zz)

[David Goodmanson]

Hi Akira,Aren't interp1(... 'spline') and spline basically the same thing, both not-a-knot cubic spline? I tried some test cases and they agreed, with some very small differences down around numerical precision.

[Akira Agata]

Hi David-san,

Thank you for the clarification. Yes, as you mentioned, interp1(...'spline') and cubic spline (not-a-knot spline) is the same thing. I was just confused "piecewise cubic interpolation" and "cubic spline" when posting my previous answer.

Thank you again!