自动驾驶规划轨迹 — 贝塞尔曲线

大家好，欢迎来到IT知识分享网。

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

在自动驾驶中，路径规划包含全局路径规划和局部路径规划，全局路径规划的结果可以是一系列首尾相连的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) * ti * (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) / (dx2 + dy2)(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 改变一个控制点整体轨迹改变，牵一发动全身