How to get closest point on cubic spline for Robotics project?

Question

I've got a robot and I want to make this robot follow a predefined path in the form of an eight figure on a field outside. The robot is not easy to steer and there are some external factors such as wind which make it very likely that the robot will not follow the exact path so it will often be next to the path.

The path I want to make it follow was formed by defining five points and create a line over those points using a cubic spline (the code I used to define this path is below this message):

I always want to be able to supply the robot a point to steer to which is on the cubic spline line. To be able to do this I thought the easiest way is to:

1. Calculate the nearest point on the cubic spline from the current location of the robot
2. Advance 0.2 units along the cubic spline line to determine the new waypoint for the robot to aim at.

For example, if the location of the robot in the grid above is x=0.4, y=-0.5, the nearest point is approximately x=0.4, y=-0.28 and the new waypoint would be approximately x=0.22, y=-0.18:

Now I've got three questions:

1. How do I find the nearest point on the cubic spline?
2. How do I "advance" 0.2 units from the found point on the cubic spline?
3. How do I stay on the given path, even when the path crosses itself in the middle?

All tips are welcome!

import numpy as np
import matplotlib.pyplot as plt
from scipy import interpolate

x = [-0.5, 0, 0.5, 0.5,  0, -0.5,  -0.5, 0, 0.5]
y = [0.25, 0, -0.25, 0.25, 0, -0.25, 0.25, 0, -0.25]

tck, u = interpolate.splprep([x, y], s=0)
unew = np.arange(0, 1.01, 0.01)
out = interpolate.splev(unew, tck)

plt.figure()
plt.plot(x, y, 'o', out[0], out[1])
plt.legend(['Predefined points', 'Cubic Spline'])
plt.axis([-0.75, 0.75, -0.75, 0.75])
plt.show()

Answer 1

Now I've got three questions:

How do I find the nearest point on the cubic spline? How do I "advance" 0.2 units from the found point on the cubic spline? How do I stay on the given path, even when the path crosses itself in the middle?

I believe a help would be to take advantage of the spline array index as a navigaion dimension

with no initial info, a crude but not that slow startup step would be to simply find the min distance to the whole spline x, y trace, out - many SE ans for finding closest

then use that point's index as a state variable, and along with direction (increasing or decreasing out index) find the next index you want to head for

if you have started up, are close, and know your last index position (or estimated target point's index), then you could just search for "closest" in a slice near (in whatever is the current "forward" direction) your internal state index, updating your internal out index as you step...

with state involved I would consider a OOP Robot Class with XY position, heading, move magnitude, out spline index estimate, index direction

and some fun to be had with actual programming

[edits:]
1st pass at "closest"

# out is a list of arrays, covert to 2d array
aout=np.array(out)

tpoint = np.array([[0.5],[-0.7]])

diff = aout-tpoint

sd = diff[0]*diff[0] + diff[1]*diff[1]  # squared distance

np.min(sd)
cpi=np.where(sd<=np.min(sd)+0.00001)[0]
plt.plot((aout[0, cpi],tpoint[0]),( aout[1, cpi], tpoint[1]), linewidth=2)

just checking if out index is srictly montonic/cyclic - and it is, but the linspace gives more than one exact cycle, I eyeballed ~ 75 points for a full cycle: # animate plotting of spline points by index

#import numpy as np
#import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation

fig1, ax = plt.subplots()
xdata, ydata = [], []
ln, = plt.plot([], [], 'ro', animated=True)

def update(frame):
    xdata.append(aout[0][frame])
    ydata.append(aout[1][frame])
    ln.set_data(xdata, ydata)
    return ln,


def init():
    ax.set_xlim(-1, 1)
    ax.set_ylim(-1, 1)
    return ln,

data = np.array(out) #np.random.rand(2, 25)
l, = plt.plot([], [], 'r-')
plt.xlim(0, 1)
plt.ylim(0, 1)
ani = FuncAnimation(fig, update,
                              frames=np.array(range(74)),
                               init_func=init, blit=True)
plt.show()