How to do path planning using only a fixed number of points?

Question

I have a vehicle with cameras to detect different colored cones. I want to generate a drivable path based on the location of the detected cones.The number of detected cones varies, as it depends on the detection accuracy: for example I might detect 4 blue cones on the left and only 3 yellow cones on the right. I was thinking of using Bezier's curve to perform this task, however it seems like this method will not result in feasible driving paths. Here is what I used to visualise a simple 2D path using the Bezier code that @reptilicus posted in this question:

import numpy as np
from scipy.special import comb

def bernstein_poly(i, n, t):

    return comb(n, i) * ( t**(n-i) ) * (1 - t)**i

def bezier_curve(points,nPoints, nTimes=100):


    nPoints = len(points)
    xPoints = np.array([p[0] for p in points])
    yPoints = np.array([p[1] for p in points])

    t = np.linspace(0.0, 1.0, nTimes)

    polynomial_array = np.array([ bernstein_poly(i, nPoints-1, t) for i in range(0, nPoints)   ])

    xvals = np.dot(xPoints, polynomial_array)
    yvals = np.dot(yPoints, polynomial_array)

    return xvals, yvals


if __name__ == "__main__":
    from matplotlib import pyplot as plt
    nYellow = 3
    nBlue = 3
    pointsYellow = [[0,0],[10,10], [5,20]]
    pointsBlue = [[10,0],[20,10],[15,20]]
    xPointsYellow = [p[0] for p in pointsYellow]
    yPointsYellow = [p[1] for p in pointsYellow]
    xPointsBlue = [p[0] for p in pointsBlue]
    yPointsBlue = [p[1] for p in pointsBlue]
    xvalsY, yvalsY = bezier_curve(pointsYellow,nYellow, nTimes=100)
    xvalsB, yvalsB = bezier_curve(pointsBlue,nBlue, nTimes=100)
    
    xPath = (xvalsY+xvalsB)/2
    yPath = (yvalsY+ yvalsB)/2
    
    plt.plot(xvalsY, yvalsY, 'y')
    plt.plot(xPointsYellow, yPointsYellow, "yo")
    plt.plot(xvalsB, yvalsB, 'b')
    plt.plot(xPointsBlue, yPointsBlue, "bo")
    plt.plot(xPath, yPath, 'r')

    plt.show()

I used the midpoint between the two boundaries as my drivable path (in red). The result is as follows:

It is evident that bezier curve will not be appropriate as the vehicle will knock the cone if it follows this path.

Is there another method I should look into to constrain my path? I haven't come across many examples that use cones as the boundaries.

Answer 1

If I remember correctly, Bezier curves have the unfortunate side-effect that they do not necessarily form trajectories that move through all of your control points. Two techniques that move through their given control points are Catmull-Rom splines and Hermite Interpolation. Both of these methods have other negative side-effects - I'm not terribly familiar with them, but regardless they will still probably outperform Bezier curves given your problem.

The basic idea of both concepts is that we create polynomials related to our control points, where the trajectory must move through these points (plus some other constraints). Cubic hermite interpolation (the most popular form of Hermite interpolation) puts constraints on "hitting" control points and user-defined velocities at these points. Catmull-Rom splines uses surrounding control points to develop velocity constraints at each control point - thus, we lose control of determining our speed at points of interest (there's a downside right there!).

The actual formulations can be a bit tedious, so perhaps it's best to focus on what constraints are being satisfied when applying one spline technique versus another.

Also - I would say that the technique of forming splines from points like this is a good idea! Personally, I think using the midpoint of your boundaries as control points is pretty clever, so I wouldn't worry about changing this part of your solution.

Here are some resources - I tried to find application-oriented sites for both of the interpolation techniques:

• Splines: https://people.cs.clemson.edu/~dhouse/courses/405/notes/splines.pdf
• Catmull-Rom: https://qroph.github.io/2018/07/30/smooth-paths-using-catmull-rom-splines.html
• Hermite: https://blog.demofox.org/2015/08/08/cubic-hermite-interpolation/