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

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 for p in points])
yPoints = np.array([p 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 for p in pointsYellow]
yPointsYellow = [p for p in pointsYellow]
xPointsBlue = [p for p in pointsBlue]
yPointsBlue = [p 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.

• Take a look at cubic splines Apr 11, 2022 at 18:53