# Incremental (on-the-fly) smooth trajectory interpolation algorithm from waypoints?

short version (TL;DR):

I'm receiving x(t) waypoints "online", which I must travel through at the specified times. I need an algorithm that would plan a smooth trajectory in-between them without knowing the whole trajectory in advance.

Longer version:

I have a 1D motion control problem. I control a DOF of a robot, and a primary design goal is for the motion to be very smooth.

A high-level controller (computer) sends my low-level motion controller a waypoint, expressed as an (xi, ti) pair, every N msec. I am running my control loop at a much higher rate, say every 1 msec.

Because of various constraints, I am not able to get the whole trajectory in advance - I am getting new waypoints as the robot is moving through previous waypoints. I am, however, able to receive a few waypoints in advance (say 3 or 4) so I have a short buffer (look-ahead) and not limited to working strictly with the next waypoint only.

I need an algorithm that would build a smooth trajectory in between these waypoints. 'Smooth' here means continuous velocity and continuous acceleration (which also implies bounded jerk).

I have tried to implement a cubic spline interpolation scheme, since it appears to be exactly what my problem requires: a piecewise function of 3rd degree polynomials, which are guaranteed to have continuous first and second derivatives everywhere, including at the 'stitches'. However, all the implementations I saw generate the spline based on all the data points, which unlike my case, are usually known in advance. My attempts to implement incremental spline generation have failed, and I'm starting to figure this is not a technical error but a mathematical impossibility. If you ask, I'll edit and try to formally explain why I think so, but this question is already quite long as it is.

So, for concrete questions:

• Do you know if it's possible to build a cubic spline this way, incrementally, and keep its 1st and 2nd derivatives smooth?

• If, as I suspect, it's impossible: What other algorithm would you suggest, that will build the trajectory incrementally, pass through all the waypoints at the specified times, while keeping everything smooth up to the 2nd derivative? Alternatively, have I defined a problem with no solution and I must relax some of the requirements?

Thank you!

• What if you assume 3 or 4 more waypoints past the known waypoints? This won't work in all scenarios. However, if you imagine a straight line trajectory as a baseline scenario, it is easy to project the line of travel several way points ahead. Curves and zig-zag motions wouldn't be as safe to project out very far, but you could build in shorter distances for them. The catch would be illogical waypoints which were in a totally different direction than the device was traveling. Those situations seem to fit your assumption of mathematical impossibility. – takintoolong Nov 26 '18 at 1:17
• you named the buffer correctly look-ahead. Have you looked at CNC Look-Ahead algorithms? – 50k4 Nov 27 '18 at 9:11
• Is there something which prohibits you to start planning from the next waypoint you will reach and iterate? I mean not keeping the past but only making cubic spline from the current state to the waypoints you have at the moment, and re-compute cubic spline when you get new waypoint, either from current state or next waypoint? – N. Staub Nov 28 '18 at 12:24
• @takintoolong: I do have available a few waypoints into the future. The question was, essentially, how to use them in order to interpolate in between those waypoints, while getting continuous position, velocity and acceleration. If I understand your comment correctly, you discuss extrapolation rather than interpolation. – Tinkerer Nov 28 '18 at 22:20
• @50k4: I did not, I'll see what I can find, thanks. – Tinkerer Nov 28 '18 at 22:21