"""
Path Planner with B-Spline
author: Atsushi Sakai (@Atsushi_twi)
"""
import sys
import pathlib
sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
import numpy as np
import matplotlib.pyplot as plt
import scipy.interpolate as interpolate
from utils.plot import plot_curvature
[docs]
def approximate_b_spline_path(x: list,
y: list,
n_path_points: int,
degree: int = 3,
s=None,
) -> tuple:
"""
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
"""
distances = _calc_distance_vector(x, y)
spl_i_x = interpolate.UnivariateSpline(distances, x, k=degree, s=s)
spl_i_y = interpolate.UnivariateSpline(distances, y, k=degree, s=s)
sampled = np.linspace(0.0, distances[-1], n_path_points)
return _evaluate_spline(sampled, spl_i_x, spl_i_y)
[docs]
def interpolate_b_spline_path(x, y,
n_path_points: int,
degree: int = 3) -> tuple:
"""
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
"""
return approximate_b_spline_path(x, y, n_path_points, degree, s=0.0)
def _calc_distance_vector(x, y):
dx, dy = np.diff(x), np.diff(y)
distances = np.cumsum([np.hypot(idx, idy) for idx, idy in zip(dx, dy)])
distances = np.concatenate(([0.0], distances))
distances /= distances[-1]
return distances
def _evaluate_spline(sampled, spl_i_x, spl_i_y):
x = spl_i_x(sampled)
y = spl_i_y(sampled)
dx = spl_i_x.derivative(1)(sampled)
dy = spl_i_y.derivative(1)(sampled)
heading = np.arctan2(dy, dx)
ddx = spl_i_x.derivative(2)(sampled)
ddy = spl_i_y.derivative(2)(sampled)
curvature = (ddy * dx - ddx * dy) / np.power(dx * dx + dy * dy, 2.0 / 3.0)
return np.array(x), y, heading, curvature,
def main():
print(__file__ + " start!!")
# way points
way_point_x = [-1.0, 3.0, 4.0, 2.0, 1.0]
way_point_y = [0.0, -3.0, 1.0, 1.0, 3.0]
n_course_point = 50 # sampling number
plt.subplots()
rax, ray, heading, curvature = approximate_b_spline_path(
way_point_x, way_point_y, n_course_point, s=0.5)
plt.plot(rax, ray, '-r', label="Approximated B-Spline path")
plot_curvature(rax, ray, heading, curvature)
plt.title("B-Spline approximation")
plt.plot(way_point_x, way_point_y, '-og', label="way points")
plt.grid(True)
plt.legend()
plt.axis("equal")
plt.subplots()
rix, riy, heading, curvature = interpolate_b_spline_path(
way_point_x, way_point_y, n_course_point)
plt.plot(rix, riy, '-b', label="Interpolated B-Spline path")
plot_curvature(rix, riy, heading, curvature)
plt.title("B-Spline interpolation")
plt.plot(way_point_x, way_point_y, '-og', label="way points")
plt.grid(True)
plt.legend()
plt.axis("equal")
plt.show()
if __name__ == '__main__':
main()