轨迹规划-贝塞尔曲线

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.specialshow_animation = Truedef 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) / offsetcontrol_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_pointsdef calc_bezier_path(control_points, n_points=100):"""Compute bezier path (trajectory) given control points.:param control_points: (numpy array):param n_points: (int) number of points in the trajectory:return: (numpy array)"""traj = []for t in np.linspace(0, 1, n_points):traj.append(bezier(t, control_points))return np.array(traj)def bernstein_poly(n, i, t):"""Bernstein polynom.:param n: (int) polynom degree:param i: (int):param t: (float):return: (float)"""return scipy.special.comb(n, i) * t ** i * (1 - t) ** (n - i)def bezier(t, control_points):"""Return one point on the bezier curve.:param t: (float) number in [0, 1]:param control_points: (numpy array):return: (numpy array) Coordinates of the point"""n = len(control_points) - 1return np.sum([bernstein_poly(n, i, t) * control_points[i] for i in range(n + 1)], axis=0)def bezier_derivatives_control_points(control_points, n_derivatives):"""Compute control points of the successive derivatives of a given bezier curve.A derivative of a bezier curve is a bezier curve.See https://pomax.github.io/bezierinfo/#derivativesfor detailed explanations:param control_points: (numpy array):param n_derivatives: (int)e.g., n_derivatives=2 -> compute control points for first and second derivatives:return: ([numpy array])"""w = {0: control_points}for i in range(n_derivatives):n = len(w[i])w[i + 1] = np.array([(n - 1) * (w[i][j + 1] - w[i][j])for j in range(n - 1)])return wdef curvature(dx, dy, ddx, ddy):"""Compute curvature at one point given first and second derivatives.:param dx: (float) First derivative along x axis:param dy: (float):param ddx: (float) Second derivative along x axis:param ddy: (float):return: (float)"""return (dx * ddy - dy * ddx) / (dx ** 2 + dy ** 2) ** (3 / 2)def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):  # pragma: no cover"""Plot arrow."""if not isinstance(x, float):for (ix, iy, iyaw) in zip(x, y, yaw):plot_arrow(ix, iy, iyaw)else:plt.arrow(x, y, length * np.cos(yaw), length * np.sin(yaw),fc=fc, ec=ec, head_width=width, head_length=width)plt.plot(x, y)def main():"""Plot an example bezier curve."""start_x = 10.0  # [m]start_y = 1.0  # [m]start_yaw = np.radians(180.0)  # [rad]end_x = -0.0  # [m]end_y = -3.0  # [m]end_yaw = np.radians(-45.0)  # [rad]offset = 3.0path, control_points = calc_4points_bezier_path(start_x, start_y, start_yaw, end_x, end_y, end_yaw, offset)# Note: alternatively, instead of specifying start and end position# you can directly define n control points and compute the path:# control_points = np.array([[5., 1.], [-2.78, 1.], [-11.5, -4.5], [-6., -8.]])# path = calc_bezier_path(control_points, n_points=100)# Display the tangent, normal and radius of cruvature at a given pointt = 0.86  # Number in [0, 1]x_target, y_target = bezier(t, control_points)derivatives_cp = bezier_derivatives_control_points(control_points, 2)point = bezier(t, control_points)dt = bezier(t, derivatives_cp[1])ddt = bezier(t, derivatives_cp[2])# Radius of curvatureradius = 1 / curvature(dt[0], dt[1], ddt[0], ddt[1])# Normalize derivativedt /= np.linalg.norm(dt, 2)tangent = np.array([point, point + dt])normal = np.array([point, point + [- dt[1], dt[0]]])curvature_center = point + np.array([- dt[1], dt[0]]) * radiuscircle = plt.Circle(tuple(curvature_center), radius,color=(0, 0.8, 0.8), fill=False, linewidth=1)assert path.T[0][0] == start_x, "path is invalid"assert path.T[1][0] == start_y, "path is invalid"assert path.T[0][-1] == end_x, "path is invalid"assert path.T[1][-1] == end_y, "path is invalid"if show_animation:  # pragma: no coverfig, ax = plt.subplots()ax.plot(path.T[0], path.T[1], label="Bezier Path")ax.plot(control_points.T[0], control_points.T[1],'--o', label="Control Points")ax.plot(x_target, y_target)ax.plot(tangent[:, 0], tangent[:, 1], label="Tangent")ax.plot(normal[:, 0], normal[:, 1], label="Normal")ax.add_artist(circle)plot_arrow(start_x, start_y, start_yaw)plot_arrow(end_x, end_y, end_yaw)ax.legend()ax.axis("equal")ax.grid(True)plt.show()def main2():"""Show the effect of the offset."""start_x = 10.0  # [m]start_y = 1.0  # [m]start_yaw = np.radians(180.0)  # [rad]end_x = -0.0  # [m]end_y = -3.0  # [m]end_yaw = np.radians(-45.0)  # [rad]for offset in np.arange(1.0, 5.0, 1.0):path, control_points = calc_4points_bezier_path(start_x, start_y, start_yaw, end_x, end_y, end_yaw, offset)assert path.T[0][0] == start_x, "path is invalid"assert path.T[1][0] == start_y, "path is invalid"assert path.T[0][-1] == end_x, "path is invalid"assert path.T[1][-1] == end_y, "path is invalid"if show_animation:  # pragma: no coverplt.plot(path.T[0], path.T[1], label="Offset=" + str(offset))if show_animation:  # pragma: no coverplot_arrow(start_x, start_y, start_yaw)plot_arrow(end_x, end_y, end_yaw)plt.legend()plt.axis("equal")plt.grid(True)plt.show()if __name__ == '__main__':main()

参考文章：
1.曲线篇-贝塞尔曲线
2.运动规划-贝塞尔Bezier曲线
3.自动驾驶-初探轨迹规划