轨迹规划-贝塞尔曲线

1. 简介

贝塞尔曲线于 1962 年，由法国工程师皮埃尔·贝济埃（Pierre Bézier）所广泛发表，他运用贝塞尔曲线来为汽车的主体进行设计,贝塞尔曲线最初由保尔·德·卡斯特里奥于1959年运用德卡斯特里奥算法开发，以稳定数值的方法求出贝塞尔曲线.

贝塞尔曲线有着很多特殊的性质, 在图形设计和路径规划中应用都非常广泛, 贝塞尔曲线完全由其控制点决定其形状,　ｎ个控制点对应着ｎ－１阶的贝塞尔曲线，并且可以通过递归的方式来绘制.

先感受一下贝塞尔曲线：
贝塞尔曲线

2. 公式及原理

一阶贝塞尔曲线

中间点运动的轨迹为：

一阶曲线就是很好理解, 就是根据t来的线性插值. P0表示的是一个向量 [x ,y], 其中x和y是分别按照这个公式来计算的.

二阶贝塞尔曲线

既然重点是递归, 那么二阶贝塞尔必然和一阶有关系.

在平面内任选 3 个不共线的点，依次用线段连接。在第一条线段上任选一个点 D。计算该点到线段起点的距离 AD，与该线段总长 AB 的比例。
根据上一步得到的比例，从第二条线段上找出对应的点 E，使得 AD:AB = BE:BC。

这时候DE又是一条直线了, 就可以按照一阶的贝塞尔方程来进行线性插值了, t= AD:AB

这时候就可以推出公式了.

三阶贝塞尔曲线

二阶的贝塞尔通过在控制点之间再采点的方式实现降阶, 每一次选点都是一次的降阶.

四个点对应是三次的贝塞尔曲线. 分别在 AB BC CD 之间采EFG点, EFG三个点对应着二阶贝塞尔, 在EF FG之间采集HI点来降阶为一阶贝塞尔曲线.

贝塞尔曲线公式

可以通过递归的方式来理解贝塞尔曲线, 但是还是给出公式才方便计算的.
仔细看可以发现, 贝塞尔的参数B是二项式(t+(1-t))^n = (1)^n的展开公式. 划重点了: 系数是二项式的展开. 后面的很多的贝塞尔曲线的性质都可以用这个来解释

3. 贝塞尔曲线的性质

贝塞尔曲线具有众多特性，例如：递归性、凸包性、对称性、几何不变性、仿射不变性、拟局部性，保证了生成曲线的平滑性、连续性和可控性。

3.1 各项系数之和为1.
这个很好理解,因为是系数是二项式的展开(t+(1-t))^n = (1)^n非负性. 好理解, 二项式的展开啊

3.2 递归性
递归性指其系数满足下式：
3.3一阶导数性质
假设上图中贝塞尔的ｔ是由左到右从０到１增加的，那么贝塞尔曲线在ｔ＝０时的导数是和Ｐ０Ｐ１的斜率（导数）是相同，ｔ＝１时的导数是和Ｐ2 P3的斜率（导数）是相同

这一点的性质可以用在贝塞尔曲线的拼接，只要保证三点一线中的中间点是两段贝塞尔曲线的连接点，就可以保证两端贝塞尔曲线的导数连续连续．

4. 如何确定贝塞尔曲线的控制点

假设道路上已经有(n+1)个采样点Pi(i=0，1，…，n)，需要在相邻的每两个点P(i)和P(i+1)之间，用一条3次Bezier曲线连接。即由4个点确定， P0为起点、P3为终点，另外中间2个控制点P1和P2。

由四个控制点定义的平面三阶贝塞尔曲线可以形式化地表示为：

如图所示，在轨迹生成过程中，将目标车辆的中心点坐标作为起点P0，以目标车辆的驶意图为引导推理得到的目标点作为P3，其他两个控制点由车辆的约束条件计算得到。

由于车辆在运动过程中无法进行原地滑动移动，所以生成的曲线需要满足起点方向约束。另外,在执行换道和转弯动作时，在终点处行驶轨迹要遵从道路结构化特征的约束，行驶轨迹的切线方向和道路的走势要相同，所以生成轨迹的终点需要满足终点方向约束。为了满足上述方向约束，P1和P2分别通过以起点坐标为起点沿起点航向向前移动距离d，以终点P3为起点沿终点航向的反方向向后移动距离d得到。

简单来说，考虑到车辆起止点速度方向的连续性，即要求曲线函数的一阶导数连续。

5. 代码实现

import matplotlib.pyplot as plt
import numpy as np
import scipy.special

show_animation = True


def calc_4points_bezier_path(sx, sy, syaw, ex, ey, eyaw, offset):
    """
    Compute control points and path given start and end position.

    :param sx: (float) x-coordinate of the starting point
    :param sy: (float) y-coordinate of the starting point
    :param syaw: (float) yaw angle at start
    :param ex: (float) x-coordinate of the ending point
    :param ey: (float) y-coordinate of the ending point
    :param eyaw: (float) yaw angle at the end
    :param offset: (float)
    :return: (numpy array, numpy array)
    """
    dist = np.hypot(sx - ex, sy - ey) / offset
    control_points = np.array(
        [[sx, sy],
         [sx + dist * np.cos(syaw), sy + dist * np.sin(syaw)],
         [ex - dist * np.cos(eyaw), ey - dist * np.sin(eyaw)],
         [ex, ey]])

    path = calc_bezier_path(control_points, n_points=100)

    return path, control_points


def calc_bezier_path(control_points, n_points=100)