Frenet坐标系局部路径规划器

Frenet坐标系局部路径规划器

背景

常用的机器人局部路径规划器有DWA算法,本文主要讲解在Frenet坐标系下生产的局部路径
DWA算法主要适用于两轮机器人等，但是对于长方形机器人不太好用，主要是在两边都是障碍物的情况下DWA算法效果不太好。

DWA算法仿真路径基本图示如下
DWA算法详解链接
该仿真生成的路径，每一根路径上的加速度是定死的，比如向左面仿真出来的路径，该路径的加速度就是不会变的，也就是一旦向左规划，就不会产生向右边方向的运动趋势。

Frenet算法路径如下
参考链接　建议先看一遍
该仿真生成的路径，每一根路径上的加速度是可以改变的，所以可以出来平滑和方向变化的路径。每一时刻的点的速度和加速度是计算好的
比如向左面仿真出来的路径，该路径的加速度就是不会变的，也就是一旦向左规划，就不会产生向右边方向的运动趋势。

Frenet　坐标系

Frenet 坐标系是什么, frenet 坐标系是根据导航中间线的轨迹就是把Ｓ拉直，Ｄ拉直的坐标系。在进行空间坐标系互相变化一下就可以转化出来。因为按照这个坐标系比较容易生成很汽车使用的路径

Frenet　坐标系轨迹的生成

Frenet坐标系轨迹是在中心线左右两侧生成一系列的线，
所以要在限定中心线左右两侧的路线宽度 D内生成d，在中心线上生成路线s，然后根据采样周期 T 来生成最终的路径。最后将Frenet坐标系下生成的轨迹转化到全局坐标系下来判断障碍物什么的。

对于Ｄ坐标轴如何生成路径??
STEP1
假设机器人当前位置是D，速度是VD,加速度是AD,
经过Ｔ时间以后，我们对机器人
期望的位置是QD,期望的速度是QVD,期望的加速度是QAD。
STEP2
那么如果计算机器人在T周期内每一时刻的位置呢？
答案是五次多项式数学公式
STEP3
设机器人位置随时间周期变化的数学公式如下
d(t)=a₀+a₁t¹+a₂t²+a₃t³+a₄t⁴+a₅t⁵
那么机器人速度随时间变化的公式则如下，位置的微分及是速度
vd(t)=a₁+2a₂t+3a₃t²+4a₄t³+5a₅t⁴
那么机器人加速速度随时间变化的公式则如下，速度的微分及是加速度
ad(t)=2a₂+6a₃t+12a₄t²+20a₄t³
STEP4
对于初始位置和期望位置可以列出一下方程
初始位置，时间为0
Ｄ＝a₀
VD=a₁
AD=2*a₂
末尾期望位置,时间是Ｔ
QD=a₀+a₁T+a₂T²+a₃T³+a₄T⁴+a₅T⁵
QVD=a₁+2a₂T+3a₃T²+4a₄T³+5a₅T⁴
QAD=2a₂+6a₃T+12a₄T²+20a₄T³

STEP5
结果
a₀=D
a₁=VD
a₂=AD/2
整理变成矩阵形式则如下
a₃T³+a₄T⁴+a₅T⁵=QD-a₀-a₁T-a₂T²
3a₃T²+4a₄T³+5a₅T⁴=QVD-a₁-2a₂T
6a₃T+12a₄T²+20a₄T³=QAD-2a₂

可以得到如下矩阵，用numpy数学公式求解
$$\left[\begin{array}{ccc}{T}^3& T^4& T^5\\ 3T^2& 4T^3& 5T^4\\ 6T& 12T^2& 20T^3\end{array}\right]\text{\hspace{1em}(A)}$$
$$\left[\begin{array}{c}a3\\ a4\\ a5\end{array}\right]\text{\hspace{1em}(B)}$$
$$\left[\begin{array}{c}QD-a0-a1T-a2T^2\\ QVD-a1-2a2T\\ QAD-2a2\end{array}\right]\text{\hspace{1em}(C)}$$
A*B=C
B=np.linalg.solve(A,C)

对于Ｓ轴坐标系如果最后只有目标期望的速度与加速度则可以用四次多项式求解，步骤是一样的。

生成路径的具体步骤如下

参数意义解释

# Parameter
MAX_SPEED = 1.0  # maximum speed [m/s]
MAX_ACCEL = 2.0  # maximum acceleration [m/ss]
#1/R R越小 该值越大 最小转向旋转半径
MIN_R=0.2
MAX_CURVATURE = 1.0/MIN_R  # maximum curvature [1/m]
MAX_ROAD_WIDTH = 2.5  # maximum road width [m]
D_ROAD_W = 0.5  # road width sampling length [m]

#预测时间
DT = 0.5  # time tick [s]
#预测的最长时间
MAXT = 5.0  # max prediction time [m]
#预测的最短时间
MINT = 4.0  # min prediction time [m]

#横向速度预期
TARGET_SPEED = 1  # target speed [m/s]
D_T_S = 0.3  # target speed sampling length [m/s]
N_S_SAMPLE = 4  # sampling number of target speed

ROBOT_RADIUS = 0.5  # robot radius [m]

# cost weights
#惩罚jerk
KJ = 0.01
#制动时间
KT = 0.1
#偏离中心线
KD = 2.0
KLAT = 1.0
KLON = 1.0

class FrenetPath:

    def __init__(self):
        self.t = []
        self.d = []
        self.d_d = []
        self.d_dd = []
        self.d_ddd = []
        self.s = []
        self.s_d = []
        self.s_dd = []
        self.s_ddd = []
        self.cd = 0.0
        self.cv = 0.0
        self.cf = 0.0

        self.x = []
        self.y = []
        self.yaw = []
        self.ds = []
        self.c = []


def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0):

	#初始化frenet路径数组
    frenet_paths = []

    # generate path to each offset goal 根据路宽进行采样
    for di in np.arange(-MAX_ROAD_WIDTH, MAX_ROAD_WIDTH, D_ROAD_W):
		
        #第一层 预期到达的 di
        # Lateral motion planning 根据采样时间进行规划
        for Ti in np.arange(MINT, MAXT, DT):
            fp = FrenetPath()
            
            ##根据上文的计算原理求取d(t)的系数
            lat_qp = QuinticPolynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti)

			# Ti时间内　以3.5s举例时刻 得到每个点时刻位置D 速度　加速度
            fp.t = [t for t in np.arange(0.0, Ti, DT)]
            #获取采样时间内每一刻d的位置
            fp.d = [lat_qp.calc_point(t) for t in fp.t]
            #获取采样时间内每一刻d的速度
            fp.d_d = [lat_qp.calc_first_derivative(t) for t in fp.t]
            #获取采样时间内每一刻d的加速度
            fp.d_dd = [lat_qp.calc_second_derivative(t) for t in fp.t]
            #获取采样时间内每一刻加速度的变化量
            fp.d_ddd = [lat_qp.calc_third_derivative(t) for t in fp.t]

            # Loongitudinal motion planning (Velocity keeping)　设置S坐标系最后的跟随速度
            for tv in np.arange(TARGET_SPEED - D_T_S * N_S_SAMPLE, TARGET_SPEED + D_T_S * N_S_SAMPLE, D_T_S):
                #首先复制Ｄ轴方向的采样出来的轨迹
                tfp = copy.deepcopy(fp)
                # 求取该期望速度下四次多项式的系数
                lon_qp = QuarticPolynomial(s0, c_speed, 0.0, tv, 0.0, Ti)
				#获取采样时间内每一刻s的位置
                tfp.s = [lon_qp.calc_point(t) for t in fp.t]
                #获取采样时间内每一刻s的速度
                tfp.s_d = [lon_qp.calc_first_derivative(t) for t in fp.t]
                #获取采样时间内每一刻s的加速度
                tfp.s_dd = [lon_qp.calc_second_derivative(t) for t in fp.t]
                #获取采样时间内每一刻s的加速度变化量
                tfp.s_ddd = [lon_qp.calc_third_derivative(t) for t in fp.t]

                Jp = sum(np.power(tfp.d_ddd, 2))  # d方向加速度的变化量的总和 总和越小　人越舒服　机器人速度越平滑
                Js = sum(np.power(tfp.s_ddd, 2))  # s方向加速度的变化量的总和　总和越小　人越舒服　机器人速度越平滑

                # square of diff from target speed　tfp.s_d[-1]　数组内的最后一个速度　从后面往前面数
                ds = (TARGET_SPEED - tfp.s_d[-1])**2
                
                #真对于无人驾驶的打分函数
                #横向D轴的损失函数原则　Jp　横向加速度变化量越小越好　Ti　速度变化时间越小越好　tfp.d[-1]　终点距离中心线越近越好
                tfp.cd = KJ * Jp + KT * Ti + KD * tfp.d[-1]**2
                #S轴的损失函数原则　Js　纵向加速度变化量越小越好　Ti　速度变化时间越小越好　ds　速度差越小越好
                tfp.cv = KJ * Js + KT * Ti + KD * ds
                #损失函数汇总成一个函数
                tfp.cf = KLAT * tfp.cd + KLON * tfp.cv
                
                frenet_paths.append(tfp)
    return frenet_paths

直观感受一下原始frenet 路径

附加到全局路径后的frenet 路径

用来显示的代码

    plt.figure('origin frenet path')
    for fp in fplist:
        plt.plot(fp.s,fp.d)
        print('frenet size',len(fp.s),len(fp.d))
    plt.figure('global frenet path')
    for fp in fplist:
        plt.plot(fp.x,fp.y)
    plt.figure('best frenet path')
    plt.plot(bestpath.x,bestpath.y)
    plt.show()

打分路径

#
def check_collision(fp, ob):
    for i in range(len(ob[:, 0])):
        d = [((ix - ob[i, 0])**2 + (iy - ob[i, 1])**2)
             for (ix, iy) in zip(fp.x, fp.y)]
        collision = any([di <= ROBOT_RADIUS**2 for di in d]) #检测规定距离内是否有障碍物
        if collision:
            return False
    return True


def check_paths(fplist, ob):
    """
    check path above max speed, max a, does collision or not
    """
    okind = []
    for i in range(len(fplist)):
        if any([v > MAX_SPEED for v in fplist[i].s_d]):  # 检查最大速度　
            continue
        elif any([abs(a) > MAX_ACCEL for a in fplist[i].s_dd]):  # 检查最大加速度
            continue
        elif any([abs(c) > MAX_CURVATURE for c in fplist[i].c]):  # 检查最大曲率
            continue
        elif not check_collision(fplist[i], ob):
            continue
        okind.append(i)
    return [fplist[i] for i in okind]

cubic spline 带注释版本

#coding = utf8
"""
cubic spline planner

Author: Atsushi Sakai

"""
import math
import numpy as np
import bisect


class Spline:
    u"""
    Cubic Spline class 三次方曲线
    f(x)=a+b*x+c*x^2+d*x^3
    """

    def __init__(self, x, y):
        """
            @breaf:传入累积距离和 传入坐标值 计算三次方函数的参数
            @param x:传入累积距离和
            @param 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 a
        #获得坐标值
        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.c)

        # 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)

        print(self.a)
        print(self.b)
        print(self.c)
        print(self.d)

    def calc(self, t):
        u"""
        pram t:参数从起始点到目前这个点的距离
        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
        一阶导数 为 d(a+b*x+c*x^2+d*x^3)=b+2*c*x+3*d*x^2
        """

        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
        二阶导数 为 dd(a+b*x+c*x^2+d*x^3)=2*c+6*d*x
        """

        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
        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]
        return B


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

    """

    def __init__(self, x, y):
        """
            传入实际曲线的坐标
            param x：传入x坐标的集合
            param y: 传入y坐标的集合
        """
        #计算各个点之间的累积距离和
        self.s = self.__calc_s(x, y)
        #获得和x 相关的 spline 类
        self.sx = Spline(self.s, x)
        #获得和x 相关的 spline 类
        self.sy = Spline(self.s, y)

    def __calc_s(self, x, y):
        dx = np.diff(x)
        dy = np.diff(y)
        #两个点之间的距离
        self.ds = [math.sqrt(idx ** 2 + idy ** 2)
                   for (idx, idy) in zip(dx, dy)]
        s = [0]
        #到第几个点的距离
        s.extend(np.cumsum(self.ds))
        
        return s

    
    def calc_position(self, s):
        u"""
        传入参数 到起始点的距离 得出那个点的x 和 y 的坐标
        calc position
        """
        x = self.sx.calc(s)
        y = self.sy.calc(s)

        return x, y

    def calc_curvature(self, s):
        u"""
        calc curvature
        """
        dx = self.sx.calcd(s)
        ddx = self.sx.calcdd(s)
        dy = self.sy.calcd(s)
        ddy = self.sy.calcdd(s)
        k = (ddy * dx - ddx * dy) / (dx ** 2 + dy ** 2)
        return k

    def calc_yaw(self, s):
        u"""
        calc yaw
        """
        dx = self.sx.calcd(s)
        dy = self.sy.calcd(s)
        yaw = math.atan2(dy, dx)
        return yaw


def calc_spline_course(x, y, ds=0.1):
    sp = Spline2D(x, y)
    s = list(np.arange(0, sp.s[-1], ds))

    rx, ry, ryaw, rk = [], [], [], []
    for i_s in s:
        ix, iy = sp.calc_position(i_s)
        rx.append(ix)
        ry.append(iy)
        ryaw.append(sp.calc_yaw(i_s))
        rk.append(sp.calc_curvature(i_s))

    return rx, ry, ryaw, rk, s


def main():
    print("Spline 2D test")
    import matplotlib.pyplot as plt
    x = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0]
    y = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0]

    sp = Spline2D(x, y)
    s = np.arange(0, sp.s[-1], 0.1)

    rx, ry, ryaw, rk = [], [], [], []
    for i_s in s:
        ix, iy = sp.calc_position(i_s)
        rx.append(ix)
        ry.append(iy)
        ryaw.append(sp.calc_yaw(i_s))
        rk.append(sp.calc_curvature(i_s))

    flg, ax = plt.subplots(1)
    plt.plot(x, y, "xb", label="input")
    plt.plot(rx, ry, "-r", label="spline")
    plt.grid(True)
    plt.axis("equal")
    plt.xlabel("x[m]")
    plt.ylabel("y[m]")
    plt.legend()

    flg, ax = plt.subplots(1)
    plt.plot(s, [math.degrees(iyaw) for iyaw in ryaw], "-r", label="yaw")
    plt.grid(True)
    plt.legend()
    plt.xlabel("line length[m]")
    plt.ylabel("yaw angle[deg]")

    flg, ax = plt.subplots(1)
    plt.plot(s, rk, "-r", label="curvature")
    plt.grid(True)
    plt.legend()
    plt.xlabel("line length[m]")
    plt.ylabel("curvature [1/m]")

    plt.show()


if __name__ == '__main__':
    main()

具体代码下载　python robotics
https://github.com/AGV-IIT-KGP/PythonRobotics#optimal-trajectory-in-a-frenet-frame

python robotics 的CPP版本
https://github.com/onlytailei/CppRobotics