B-Spline planning
This is a B-Spline path planning routines.
If you input waypoints, it generates a smooth path with B-Spline curve.
This codes provide two types of B-Spline curve generations:
Interpolation: generate a curve that passes through all waypoints.
Approximation: generate a curve that approximates the waypoints. (Not passing through all waypoints)
Bspline basics
BSpline (Basis-Spline) is a piecewise polynomial spline curve.
It is expressed by the following equation.
\(\mathbf{S}(x)=\sum_{i=k-p}^k \mathbf{c}_i B_{i, p}(x)\)
here:
\(S(x)\) is the curve point on the spline at x.
\(c_i\) is the representative point generating the spline, called the control point.
\(p+1\) is the dimension of the BSpline.
\(k\) is the number of knots.
\(B_{i,p}(x)\) is a function called Basis Function.
The the basis function can be calculated by the following De Boor recursion formula:
\(B_{i, 0}(x):= \begin{cases}1 & \text { if } \quad t_i \leq x<t_{i+1} \\ 0 & \text { otherwise }\end{cases}\)
\(B_{i, p}(x):=\frac{x-t_i}{t_{i+p}-t_i} B_{i, p-1}(x)+\frac{t_{i+p+1}-x}{t_{i+p+1}-t_{i+1}} B_{i+1, p-1}(x)\)
here:
\(t_i\) is each element of the knot vector.
This figure shows the BSpline basis functions for each of \(i\):
Note that when all the basis functions are added together, summation is 1.0 for any x value.
This means that the result curve is smooth when each control point is weighted addition by this basis function,
This code is for generating the upper basis function graph using scipy.
from scipy.interpolate import BSpline
def B_orig(x, k, i, t):
if k == 0:
return 1.0 if t[i] <= x < t[i + 1] else 0.0
if t[i + k] == t[i]:
c1 = 0.0
else:
c1 = (x - t[i]) / (t[i + k] - t[i]) * B(x, k - 1, i, t)
if t[i + k + 1] == t[i + 1]:
c2 = 0.0
else:
c2 = (t[i + k + 1] - x) / (t[i + k + 1] - t[i + 1]) * B(x, k - 1, i + 1, t)
return c1 + c2
def B(x, k, i, t):
c = np.zeros_like(t)
c[i] = 1
return BSpline(t, c, k)(x)
def main():
k = 3 # degree of the spline
t = [0, 1, 2, 3, 4, 5] # knots vector
x = np.linspace(0, 5, 1000, endpoint=False)
t = np.r_[[np.min(t)]*k, t, [np.max(t)]*k]
n = len(t) - k - 1
for i in range(n):
y = np.array([B(ix, k, i, t) for ix in x])
plt.plot(x, y, label=f'i = {i}')
plt.title(f'Basis functions (k = {k}, knots = {t})')
plt.show()
Bspline interpolation planning
PathPlanning.BSplinePath.bspline_path.interpolate_b_spline_path()
generates a curve that passes through all waypoints.
This is a simple example of the interpolation planning:
This figure also shows curvatures of each path point using utils.plot.plot_curvature.
The default spline degree is 3, so curvature changes smoothly.
API
- PathPlanning.BSplinePath.bspline_path.interpolate_b_spline_path(x, y, n_path_points: int, degree: int = 3) tuple [source]
Interpolate x-y points with a B-Spline path
- Parameters
x (array_like) – x positions of interpolated points
y (array_like) – y positions of interpolated points
n_path_points (int) – number of path points
degree (int, optional) – B-Spline degree. Must be 2<= k <= 5. Default: 3
- Returns
x (array) – x positions of the result path
y (array) – y positions of the result path
heading (array) – heading of the result path
curvature (array) – curvature of the result path
Bspline approximation planning
PathPlanning.BSplinePath.bspline_path.approximate_b_spline_path()
generates a curve that approximates the waypoints, which means that
the curve might not pass through waypoints.
Users can adjust path smoothness by the smoothing parameter s. If this value is bigger, the path will be smoother, but it will be less accurate. If this value is smaller, the path will be more accurate, but it will be less smooth.
This is a simple example of the approximation planning:
This figure also shows curvatures of each path point using utils.plot.plot_curvature.
The default spline degree is 3, so curvature changes smoothly.
API
- PathPlanning.BSplinePath.bspline_path.approximate_b_spline_path(x: list, y: list, n_path_points: int, degree: int = 3, s=None) tuple [source]
Approximate points with a B-Spline path
- Parameters
x (array_like) – x position list of approximated points
y (array_like) – y position list of approximated points
n_path_points (int) – number of path points
degree (int, optional) – B Spline curve degree. Must be 2<= k <= 5. Default: 3.
s (int, optional) – smoothing parameter. If this value is bigger, the path will be smoother, but it will be less accurate. If this value is smaller, the path will be more accurate, but it will be less smooth. When s is 0, it is equivalent to the interpolation. Default is None, in this case s will be len(x).
- Returns
x (array) – x positions of the result path
y (array) – y positions of the result path
heading (array) – heading of the result path
curvature (array) – curvature of the result path