4. 局部轨迹规划：Frenet坐标系详解、五次多项式轨迹生成、优化目标与约束条件
局部轨迹规划，说白了就是让车在眼前这段路上，怎么走得又稳又好。我刚开始接触Autoware时，觉得全局规划才是重头戏，局部规划不过是“微调”而已。直到有一次在园区测试，全局路径明明规划好了，车却在弯道里来回画龙——嗯，从那以后我再也不敢小看局部规划了。
这一节，我们重点聊三个东西：Frenet坐标系、五次多项式轨迹生成、以及优化目标和约束条件。这三者是局部轨迹规划的基石，也是你调试Autoware时绕不开的核心。
4.1 Frenet坐标系：为什么不用笛卡尔坐标系？
你想想看，在笛卡尔坐标系下，一条弯弯曲曲的道路，用(x, y)来描述车辆的轨迹，是不是很麻烦？尤其是当道路曲率变化时，横向和纵向的运动完全耦合在一起，解耦起来非常痛苦。
Frenet坐标系就是来解决这个问题的。它把车辆的轨迹分解成两个独立的方向：
- 纵向（s）：沿着参考线的方向，表示走了多远。
- 横向（d）：垂直于参考线的方向，表示偏离了多少。
这样一来，车辆的横向运动和纵向运动就可以分开处理了。我个人习惯把Frenet坐标系想象成“铁轨上的小车”——小车沿着铁轨走是纵向，左右摆动是横向，互不干扰。
核心公式：
在Frenet坐标系下，车辆状态可以表示为：
s(t) —— 纵向位置随时间变化
d(t) —— 横向位置随时间变化而笛卡尔坐标与Frenet坐标的转换，需要用到参考线的曲率κ和切线方向角θ_r。具体公式我就不展开了，你只需要记住：Frenet坐标系让轨迹规划问题从二维降为一维×2，大大简化了计算。
我的经验： 我在项目中遇到过一个问题——当参考线曲率变化剧烈时，Frenet坐标系的转换会出现数值不稳定。后来我加了一个曲率阈值判断，当曲率超过0.1时，强制加密参考线采样点。这个小技巧帮我省了不少调试时间。
4.2 五次多项式轨迹生成：为什么是五次？
有了Frenet坐标系，接下来就是怎么生成一条平滑的轨迹。常用的方法是用五次多项式来描述s(t)和d(t)。
为什么会是五次？说白了，五次多项式有6个系数，可以满足6个边界条件：
- 起始位置、速度、加速度
- 终点位置、速度、加速度
这6个条件一确定，五次多项式的系数就唯一确定了。你想想看，如果只用三次多项式，只能控制位置和速度，加速度就没办法约束了——那车开起来就会一抖一抖的。
五次多项式的形式如下：
s(t) = a0 + a1*t + a2*t^2 + a3*t^3 + a4*t^4 + a5*t^5
d(t) = b0 + b1*t + b2*t^2 + b3*t^3 + b4*t^4 + b5*t^5其中a0~a5和b0~b5就是我们要解的系数。怎么解？很简单，把起始和终点的位置、速度、加速度代入，解一个6元一次方程组就行。
代码示例（Python伪代码）：
import numpy as np
def quintic_polynomial(start, end, T):
"""
start: [s0, s_dot0, s_ddot0]
end: [s1, s_dot1, s_ddot1]
T: 时间长度
"""
# 构建系数矩阵
A = np.array([
[0, 0, 0, 0, 0, 1],
[0, 0, 0, 0, 1, 0],
[0, 0, 0, 2, 0, 0],
[T**5, T**4, T**3, T**2, T, 1],
[5*T**4, 4*T**3, 3*T**2, 2*T, 1, 0],
[20*T**3, 12*T**2, 6*T, 2, 0, 0]
])
b = np.array([start[0], start[1], start[2],
end[0], end[1], end[2]])
coeffs = np.linalg.solve(A, b)
return coeffs注意： 解五次多项式时，矩阵A的条件数可能会很大，尤其是当T很小或很大时。我曾经遇到过因为T=0.1秒导致矩阵奇异的情况——嗯，后来我加了一个最小时间阈值0.5秒，问题就解决了。
4.3 优化目标与约束条件：怎么才算“好”的轨迹？
轨迹生成了，但好不好呢？这就需要优化目标和约束条件来评判。
4.3.1 优化目标
我个人习惯把优化目标分为三类：
- 舒适性：加速度变化率（jerk）要小。说白了就是别让乘客晕车。
- 效率：尽可能接近目标速度，别太慢。
- 安全性：横向偏离参考线不要太远，别蹭到路边。
在Autoware中，常用的代价函数形式是：
Cost = w1 * ∫(jerk_s^2)dt + w2 * ∫(jerk_d^2)dt + w3 * (v_target - v_actual)^2 + w4 * d^2其中w1~w4是权重系数，需要根据实际场景调参。我记得有一次在高速场景测试，舒适性权重调得太高，结果车开得像老奶奶散步——后来我把效率权重提高了两倍，才达到预期效果。
4.3.2 约束条件
约束条件就是“不能做什么”。常见的约束有：
| 约束类型 | 具体内容 | 典型值 |
|---|---|---|
| 速度约束 | 不能超速，也不能倒车 | 0 ≤ v ≤ v_max |
| 加速度约束 | 加速/减速不能太猛 | -a_max ≤ a ≤ a_max |
| 曲率约束 | 转弯半径不能太小 | κ ≤ κ_max |
| 横向偏移约束 | 不能偏离车道太远 | |d| ≤ d_max |
避坑指南： 我曾经在约束条件里忘了加“横向偏移约束”，结果车在直道上开得好好的，一到弯道就往外飘——因为优化器觉得走捷径更“舒适”。从那以后，我每次都会检查约束条件是否完整，尤其是横向偏移约束，绝对不能漏。
4.4 知识体系总览
为了让你更直观地理解这一节的内容，我画了一张图：
这张图把这一节的核心逻辑串起来了：先用Frenet坐标系把问题简化，再用五次多项式生成候选轨迹，最后通过优化目标和约束条件选出最优的那一条。每一步都有坑，但每一步也都有对应的解决办法。
小结： 局部轨迹规划不是玄学，而是有章可循的工程问题。Frenet坐标系帮你解耦，五次多项式帮你平滑，优化和约束帮你选优。把这三点吃透了，Autoware的规划调试你就入门了。