Python sample program of Path generation with Cubic Spline Interpolation

Introduction to Cubic Spline Interpolation with Examples in Python (English Edition)

Introduction

Cubic spline is a piecewise cubic function that interpolates a set of data points and guarantees smoothness at the data points. So, cubic spline can be used to generate a smooth reference path for autonomous driving. In this entry, I'm introducing a summary of cubic spline algorithm and Python sample program of a path generation with cubic spline interpolation.

Table of contents

Source code at GitHub

All source codes are located at the following GitHub repository.

Cubic spline module
github.com

Path generation simulation
github.com

Spline

Spline is a function defined piecewise by polynomials. In computer graphics, spline can be used to interpolate a set of data points as a smooth curve which passes on or near the points. This curve guarantees that the position, the heading and the curvature of the points.

en.wikipedia.org

Cubic Spline Interpolation

Cubic spline interpolation is a method to interpolate a given set of data points with cubic polynomials smoothly. The spaces between each points are divided and those segments are interpolated with different cubic polynomials.

The cubic polynomial is defined as follow. The parameters of a, b, c and d are different at each segments.

Conditions for Parameters decision

The following conditions need to be satisfied for deciding the parameters, a, b, c and d.

1. Constraint condition at edge points of segments

2. Continuity condition at boundaries of segments

3. Continuity condition of first derivative at boundaries of segments

4. Continuity condition of second derivative at boundaries of segments

5. Boundary condition of second derivative between start point and end point

1D Cubic Spline

A class for 1D Cubic spline interpolation is implemented as follow.

"""
cubic_spline.py

Author: Shisato Yano
"""

import numpy as np
import bisect


class CubicSpline:
    """
    1D Cubic Spline class
    Each section between two points are approximated as the following equation
    y_i = a_i + b_i * (x - x_i) + c_i * (x - x_i)^2 + d_i * (x - x_i)^3
    """

    def __init__(self, x_points, y_points):
        """
        Constructor
        x_points: List of x coordinate points. This must be stored in ascending order.
        y_points: List of y coordinate points.
        """
        
        h = np.diff(x_points)
        if np.any(h < 0):
            raise ValueError("X coordinate points must be stored in ascending order")
        
        self.a, self.b, self.c, self.d = [], [], [], []
        self.x_points = x_points
        self.y_points = y_points
        self.size_x_points = len(self.x_points)

        self._calculate_coefficient_a()
        self._calculate_coefficient_c(h)
        self._calculate_coefficient_b_d(h)
    
    def calculate_position(self, x):
        if x < self.x_points[0]: return None
        elif x > self.x_points[-1]: return None

        i_x = self._search_segment_index(x)
        dx = x - self.x_points[i_x]
        y = self.a[i_x] + self.b[i_x] * dx + \
            self.c[i_x] * dx ** 2.0 + self.d[i_x] * dx ** 3.0
        
        return y

    def _search_segment_index(self, x):
        return bisect.bisect(self.x_points, x) - 1
    
    def _calculate_coefficient_a(self):
        self.a = [y_point for y_point in self.y_points]
    
    def _calculate_coefficient_c(self, h):
        A = self._calculate_matrix_A(h)
        B = self._calculate_matrix_B(h, self.a)
        self.c = np.linalg.solve(A, B)

    def _calculate_coefficient_b_d(self, h):
        for i in range(self.size_x_points - 1):
            d = (self.c[i + 1] - self.c[i]) / (3.0 * h[i])
            b = 1.0 / h[i] * (self.a[i + 1] - self.a[i]) \
                - h[i] / 3.0 * (2.0 * self.c[i] + self.c[i + 1])
            self.d.append(d)
            self.b.append(b)

    def _calculate_matrix_A(self, h):
        A = np.zeros((self.size_x_points, self.size_x_points))
        A[0, 0] = 1.0
        for i in range(self.size_x_points - 1):
            if i != (self.size_x_points - 2):
                A[i + 1, i + 1] = 2.0 * (h[i] + h[i + 1])
            A[i + 1, i] = h[i]
            A[i, i + 1] = h[i]
        A[0, 1] = 0.0
        A[self.size_x_points - 1, self.size_x_points - 2] = 0.0
        A[self.size_x_points - 1, self.size_x_points - 1] = 1.0
        return A
    
    def _calculate_matrix_B(self, h, a):
        B = np.zeros(self.size_x_points)
        for i in range(self.size_x_points - 2):
            B[i + 1] = 3.0 * (a[i + 2] - a[i + 1]) / h[i + 1] \
                        - 3.0 * (a[i + 1] - a[i]) / h[i]
        return B

The public member function, calculate_position(self, x) is used to compute an interpolated value, y toward an parametric variable, x. The parametric variable, x is used to handle multiple dimensional interpolation and changeable size of input data. An interpolated 1D cubic spline curve can be drawn with this class refer to the following sample program.

"""
cubic_spline_plot.py

Author: Shisato Yano
"""

# import path setting
import numpy as np
import sys
import matplotlib.pyplot as plt
from pathlib import Path

abs_dir_path = str(Path(__file__).absolute().parent)
relative_path = "/../../../components/"

sys.path.append(abs_dir_path + relative_path + "course/cubic_spline_course")

# import component module
from cubic_spline import CubicSpline


# flag to show plot figure
# when executed as unit test, this flag is set as false
show_plot = True


def main():
    """
    Main process function
    """

    x_points = np.arange(5)
    y_points = [1.7, -6, 5, 6.5, 0.0]

    cs = CubicSpline(x_points, y_points)

    xi = np.linspace(0.0, 5.0)
    yi = [cs.calculate_position(x) for x in xi]
    
    plt.plot(x_points, y_points, "xb", label="Input points")
    plt.plot(xi, yi, "r", label="Cubic spline interpolation")
    plt.grid(True)
    plt.legend()

    if show_plot: plt.show()


if __name__ == "__main__":
    main()

2D X-Y Path generation with Cubic Spline Interpolation

2D X-Y path interpolated with cubic splines guarantees that the position, the heading angle and the curvature of each data points are continuous. For the continuity, the following two functions to compute first derivative and second derivative at each intervals need to be implemented in the above class additionally.

    def calculate_first_derivative(self, x):
        if x < self.x_points[0]: return None
        elif x > self.x_points[-1]: return None

        i_x = self._search_segment_index(x)
        dx = x - self.x_points[i_x]
        dy = self.b[i_x] + 2.0 * self.c[i_x] * dx + 3.0 * self.d[i_x] * dx ** 2.0

        return dy

    def calculate_second_derivative(self, x):
        if x < self.x_points[0]: return None
        elif x > self.x_points[-1]: return None

        i_x = self._search_segment_index(x)
        dx = x - self.x_points[i_x]
        ddy = 2.0 * self.c[i_x] + 6.0 * self.d[i_x] * dx

        return ddy

And then, a new class of 2D cubic spline is implemented by importing two 1D cubic spline class as follow.

"""
cubic_spline_2d.py

Author: Shisato Yano
"""

import numpy as np
import math

from cubic_spline import CubicSpline


class CubicSpline2D:
    """
    2D Cubic Spline class
    Each section between two points are approximated as the following equation
    y_i = a_i + b_i * (x - x_i) + c_i * (x - x_i)^2 + d_i * (x - x_i)^3
    """

    def __init__(self, x_points, y_points):
        self.s = self._calc_base_points(x_points, y_points)
        self.sx = CubicSpline(self.s, x_points)
        self.sy = CubicSpline(self.s, y_points)
    
    def calc_interpolated_xy(self, s):
        interpolated_x = self.sx.calculate_position(s)
        interpolated_y = self.sy.calculate_position(s)

        return interpolated_x, interpolated_y
    
    def calc_yaw_angle(self, s):
        dx = self.sx.calculate_first_derivative(s)
        dy = self.sy.calculate_first_derivative(s)
        yaw_angle = math.atan2(dy, dx)
        return yaw_angle
    
    def calc_curvature(self, s):
        dx = self.sx.calculate_first_derivative(s)
        ddx = self.sx.calculate_second_derivative(s)

        dy = self.sy.calculate_first_derivative(s)
        ddy = self.sy.calculate_second_derivative(s)

        curvature = (ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2) ** (3/2))
        return curvature

    def _calc_base_points(self, x_points, y_points):
        dx = np.diff(x_points)
        dy = np.diff(y_points)
        self.ds = np.hypot(dx, dy)
        s = [0]
        s.extend(np.cumsum(self.ds))
        return s

A smooth X-Y path can be generated with the above 2D cubic spline class refer to the following sample program.

"""
cubic_spline_2d_plot.py

Author: Shisato Yano
"""

# import path setting
import numpy as np
import sys
import matplotlib.pyplot as plt
from pathlib import Path

abs_dir_path = str(Path(__file__).absolute().parent)
relative_path = "/../../../components/"

sys.path.append(abs_dir_path + relative_path + "course/cubic_spline_course")

# import component module
from cubic_spline_2d import CubicSpline2D


# flag to show plot figure
# when executed as unit test, this flag is set as false
show_plot = True


def main():
    """
    Main process function
    """

    x_points = [0.0, 10.0, 25, 40, 50]
    y_points = [0.0, 4, -12, 20, -13]

    ds = 0.1 # distance between 2 interpolated points

    cs = CubicSpline2D(x_points, y_points)
    s = np.arange(0, cs.s[-1], ds)

    xs, ys, yaws, curvs = [], [], [], []
    for i_s in s:
        i_x, i_y = cs.calc_interpolated_xy(i_s)
        xs.append(i_x)
        ys.append(i_y)
        yaws.append(cs.calc_yaw_angle(i_s))
        curvs.append(cs.calc_curvature(i_s))
    
    plt.subplots(1)
    plt.plot(x_points, y_points, "xb", label="Input points")
    plt.plot(xs, ys, "-r", label="Cubic spline path")
    plt.grid(True)
    plt.axis("equal")
    plt.xlabel("X[m]")
    plt.ylabel("Y[m]")
    plt.legend()

    plt.subplots(1)
    plt.plot(s, [np.rad2deg(yaw) for yaw in yaws], "-r", label="Yaw angle")
    plt.grid(True)
    plt.xlabel("Line length[m]")
    plt.ylabel("Yaw angle[deg]")
    plt.legend()

    plt.subplots(1)
    plt.plot(s, curvs, "-r", label="Curvature")
    plt.grid(True)
    plt.xlabel("Line length[m]")
    plt.ylabel("Curvature[1/m]")
    plt.legend()

    if show_plot: plt.show()


if __name__ == "__main__":
    main()

The heading(yaw) angle and the curvature of the generated path can also be drawn by executing the above sample program. These graphs indicates that the continuity is guaranteed.