自动驾驶规划轨迹的表达形式：

在自动驾驶中，路径规划包含全局路径规划和局部路径规划，全局路径规划的结果可以是一系列首尾相连的lane组成或者比较粗糙的路径点集。局部路径规划则通常是符合运动学约束的连续曲线构成的离散点，那这条曲线应该如何表达呢？通常我们表达二维平面曲线的时候有两种方式，单值函数和参数方程。

单值函数：

这种方式y是x的单值函数，耦合性强，自动驾驶中一般不会使用这种方式。

参数方程：

--1 距离：

每个参数p代表了曲线上的某一个点，在某一点也就是某个参数p的位置无穷小的一段ds可以近似为一条直线，那也就是这个p附近的dx dy三角形组成的斜边：

这里我们求出了距离求解的积分形式，但是通常我们是无法得到积分的原函数，也无法得到解析解。所以在离散环境中，我们还是会使用数值积分的方式，将曲线离散成n个离散点，累计求和多个离散点之间的欧式距离来计算距离。

--2 切向角：

--3 曲率：

这里用到了链式法则、反正切函数求导、商求导。

以上我们对一条参数曲线进行了描述，p是控制参数，每个p对应了一个轨迹点，并且可以通过数值解求得对应的距离，通过解析解求得对应的切向角和曲率。

贝塞尔曲线：

贝塞尔曲线也被称为贝塞尔多项式，是一种由一系列控制点所定义的平滑曲线。看了很多文档，有说次有说阶，大部分描述还是阶，n阶贝塞尔曲线就是n次贝塞尔曲线，方程中参数t最高也是n次，控制点是n+1个。

三阶贝塞尔曲线代码实现：

import numpy as np
import matplotlib.pyplot as plt
from scipy.special import comb  # 导入组合数函数

# 定义贝塞尔曲线函数
def bezier_curve(control_points, t):
    n = len(control_points) - 1
    curve_point = np.zeros(2)
    for i in range(n + 1):
        curve_point += control_points[i] * bernstein_poly(n, i, t)
    return curve_point

# 定义Bernstein基函数
def bernstein_poly(n, i, t):
    return comb(n, i) * t**i * (1 - t)**(n - i)

# 定义贝塞尔曲线的一阶导数
def bezier_first_derivative(control_points, t):
    n = len(control_points) - 1
    derivative_point = np.zeros(2)
    for i in range(n):
        derivative_point += (control_points[i + 1] - control_points[i]) * bernstein_poly(n - 1, i, t)
    return n * derivative_point

# 定义贝塞尔曲线的二阶导数
def bezier_second_derivative(control_points, t):
    n = len(control_points) - 1
    second_derivative_point = np.zeros(2)
    for i in range(n - 1):
        second_derivative_point += (control_points[i + 2] - 2 * control_points[i + 1] + control_points[i]) * bernstein_poly(n - 2, i, t)
    return n * (n - 1) * second_derivative_point

# 计算航向角
def heading_angle(dx, dy):
    return np.arctan2(dy, dx)

# 计算曲率
def curvature(dx, dy, ddx, ddy):
    return (dx * ddy - dy * ddx) / (dx**2 + dy**2)**(3/2)

# 计算距离（弧长）
def calculate_distance(points):
    distances = np.zeros(len(points))
    for i in range(1, len(points)):
        distances[i] = distances[i - 1] + np.linalg.norm(points[i] - points[i - 1])
    return distances

# 主程序
if __name__ == "__main__":
    # 定义三阶贝塞尔曲线的控制点
    control_points = np.array([
        [0, 0],
        [1, 3],
        [4, 3],
        [5, 0]
    ])

    # 离散点取50个
    t_values = np.linspace(0, 1, 50)

    # 计算轨迹点
    curve_points = np.array([bezier_curve(control_points, t) for t in t_values])

    # 计算一阶导数（切向量）
    first_derivatives = np.array([bezier_first_derivative(control_points, t) for t in t_values])

    # 计算二阶导数
    second_derivatives = np.array([bezier_second_derivative(control_points, t) for t in t_values])

    # 计算航向角
    heading_angles = np.array([heading_angle(dx, dy) for dx, dy in first_derivatives])

    # 计算曲率
    curvatures = np.array([curvature(dx, dy, ddx, ddy) for (dx, dy), (ddx, ddy) in zip(first_derivatives, second_derivatives)])

    # 计算距离（弧长）
    distances = calculate_distance(curve_points)

    # 输出结果
    print("轨迹点：", curve_points)
    print("距离：", distances)
    print("航向角：", heading_angles)
    print("曲率：", curvatures)

    # 可视化
    plt.figure(figsize=(10, 6))
    plt.plot(curve_points[:, 0], curve_points[:, 1], 'b-', label="贝塞尔曲线")
    plt.plot(control_points[:, 0], control_points[:, 1], 'ro--', label="控制点")
    plt.legend()
    plt.title("三阶贝塞尔曲线")
    plt.xlabel("X")
    plt.ylabel("Y")
    plt.grid(True)
    plt.axis("equal")
    plt.show()

总结：

--1 贝塞尔曲线平滑、连续、基于控制点控制形状

--2 轨迹只能确定起点和终点，中间点无法确定

--3 改变一个控制点整体轨迹改变，牵一发动全身