基于Frenet优化轨迹的无人车动作规划

基于Frenet优化轨迹的无人车动作规划实例，使用Python实现，主要为高速场景，具体参考博客：https://blog.csdn.net/AdamShan/article/details/80779615

import math
import numpy as np
import bisect


class Spline:
    u“““
    Cubic Spline class
    “““

    def __init__（self x y）:
        self.b self.c self.d self.w = [] [] [] []

        self.x = x
        self.y = y

        self.nx = len（x）  # dimension of x
        h = np.diff（x）

        # calc coefficient c
        self.a = [iy for iy in y]

        # calc coefficient c
        A = self.__calc_A（h）
        B = self.__calc_B（h）
        self.c = np.linalg.solve（A B）
        #  print（self.c1）

        # calc spline coefficient b and d
        for i in range（self.nx - 1）:
            self.d.append（（self.c[i + 1] - self.c[i]） / （3.0 * h[i]））
            tb = （self.a[i + 1] - self.a[i]） / h[i] - h[i] * \
                （self.c[i + 1] + 2.0 * self.c[i]） / 3.0
            self.b.append（tb）

    def calc（self t）:
        u“““
        Calc position
        if t is outside of the input x return None
        “““

        if t < self.x[0]:
            return None
        elif t > self.x[-1]:
            return None

        i = self.__search_index（t）
        dx = t - self.x[i]
        result = self.a[i] + self.b[i] * dx + \
            self.c[i] * dx ** 2.0 + self.d[i] * dx ** 3.0

        return result

    def calcd（self t）:
        u“““
        Calc first derivative
        if t is outside of the input x return None
        “““

        if t < self.x[0]:
            return None
        elif t > self.x[-1]:
            return None

        i = self.__search_index（t）
        dx = t - self.x[i]
        result = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx ** 2.0
        return result

    def calcdd（self t）:
        u“““
        Calc second derivative
        “““

        if t < self.x[0]:
            return None
        elif t > self.x[-1]:
            return None

        i = self.__search_index（t）
        dx = t - self.x[i]
        result = 2.0 * self.c[i] + 6.0 * self.d[i] * dx
        return result

    def __search_index（self x）:
        u“““
        search data segment index
        “““
        return bisect.bisect（self.x x） - 1

    def __calc_A（self h）:
        u“““
        calc matrix A for spline coefficient c
        “““
        A = np.zeros（（self.nx self.nx））
        A[0 0] = 1.0
        for i in range（self.nx - 1）:
            if i != （self.nx - 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.nx - 1 self.nx - 2] = 0.0
        A[self.nx - 1 self.nx - 1] = 1.0
        #  print（A）
        return A

    def __calc_B（self h）:
        u“““
        calc matrix B for spline coefficient c
        “““
        B = np.zeros（self.nx）
        for i in range（self.nx - 2）:
            B[i + 1] = 3.0 * （self.a[i + 2] - self.a[i + 1]） / \
                h[i + 1] - 3.0 * （self.a[i + 1] - self.a[i]） / h[i]
        #  print（B）
        return B


class Spline2D:
    u“““
    2D Cubic Spline class
    “““

    def 

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2018-06-22 22:58  frenet_optimal\
     文件        6446  2018-06-22 22:58  frenet_optimal\cubic_spline.pyc
     文件        9773  2018-06-22 22:57  frenet_optimal\frenet_optimal_trajectory.py
     文件        4354  2018-06-22 22:58  frenet_optimal\cubic_spline.py
     目录           0  2018-06-22 22:59  frenet_optimal\.idea\
     文件         398  2018-06-17 18:54  frenet_optimal\.idea\frenet_optimal.iml
     目录           0  2018-06-17 18:53  frenet_optimal\.idea\inspectionProfiles\
     文件         228  2018-06-17 18:53  frenet_optimal\.idea\inspectionProfiles\profiles_settings.xml
     文件       21875  2018-06-22 22:59  frenet_optimal\.idea\workspace.xml
     文件         280  2018-06-17 18:53  frenet_optimal\.idea\modules.xml
     文件         206  2018-06-17 18:54  frenet_optimal\.idea\misc.xml