1. 五次多项式换道轨迹规划原理剖析
在自动驾驶领域,五次多项式因其良好的数学特性成为轨迹规划的热门选择。这种方法的本质是通过多项式函数来描述车辆横向位移随时间的变化规律。具体来说,横向位移y(t)可以表示为:
python复制def quintic_poly(t, coeffs):
return coeffs[0] + coeffs[1]*t + coeffs[2]*t**2 + coeffs[3]*t**3 + coeffs[4]*t**4 + coeffs[5]*t**5
为什么选择五次而非更低阶的多项式?这源于我们对轨迹的三大核心要求:
- 位置连续性:确保轨迹起点和终点位置准确匹配预期
- 速度连续性:避免速度突变导致乘坐不适
- 加速度连续性:保证控制指令的平滑过渡
要满足这六个边界条件(起点和终点各有位置、速度、加速度三个条件),五次多项式是最低阶能满足所有约束的选择。四次多项式只能满足五个条件,会导致某些边界条件无法满足。
需要模型API调用? 免费领10W Token,多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. 系数求解与轨迹生成
2.1 线性方程组构建
多项式系数的求解本质上是一个线性代数问题。我们需要构建如下形式的矩阵方程:
python复制def calc_coeffs(start, end, T):
A = np.array([
[1, 0, 0, 0, 0, 0], # 起点位置约束
[1, T, T**2, T**3, T**4, T**5], # 终点位置约束
[0, 1, 0, 0, 0, 0], # 起点速度约束
[0, 1, 2*T, 3*T**2, 4*T**3, 5*T**4], # 终点速度约束
[0, 0, 2, 0, 0, 0], # 起点加速度约束
[0, 0, 2, 6*T, 12*T**2, 20*T**3] # 终点加速度约束
])
b = np.array([start[0], end[0], start[1], end[1], start[2], end[2]])
return np.linalg.solve(A, b)
