3、速度规划坐标系：Frenet坐标系与笛卡尔坐标系的转换、SL与ST图的含义、为什么ST图是速度规划的核心
做速度规划，第一个绕不开的问题就是：在哪个坐标系下干活？
我刚开始接触这个领域时，也踩过坑。直接在笛卡尔坐标系下做速度规划，结果车辆横摆角一变化，规划出来的速度曲线根本没法用。后来才明白，Frenet坐标系才是速度规划的主场。
3.1 为什么需要Frenet坐标系？
说白了，笛卡尔坐标系（x, y）描述的是绝对位置。而车辆是沿着道路走的，道路是弯曲的。你想想看，在弯道上用笛卡尔坐标去描述“车辆离车道中心多远”、“沿着道路走了多远”，是不是很别扭？
Frenet坐标系就解决了这个问题。它把问题拆成两个独立维度：
- s：沿着参考线的弧长（纵向）
- l：偏离参考线的横向距离（横向）
这样一来，速度规划就变成了在s-t图上做决策，横向控制交给另一条线去处理。解耦，清晰，好调。
核心思想：Frenet坐标系将车辆运动分解为“沿着路走”和“偏离路多少”两个独立问题，速度规划只关心前者。
3.2 Frenet ↔ 笛卡尔坐标转换
实际工程中，传感器给的是笛卡尔坐标，规划器需要Frenet坐标，控制又得转回笛卡尔。所以正反转换是基本功。
3.2.1 笛卡尔 → Frenet（正向转换）
给定车辆位置 (x, y)，参考线上最近点 (x_r, y_r) 及其切线方向 θ_r：
// 伪代码：笛卡尔 → Frenet
s = s_r // 参考点弧长
dx = x - x_r
dy = y - y_r
// 横向偏移（带符号）
l = sign(dy * cos(θ_r) - dx * sin(θ_r)) * sqrt(dx² + dy²)
// 纵向速度
s_dot = v * cos(θ - θ_r) / (1 - κ_r * l)
// 横向速度
l_dot = v * sin(θ - θ_r)这里有个坑：参考线最近点的搜索。我曾在项目中直接用暴力搜索，结果每帧要算几百个点，CPU直接拉满。后来改用KD树，效率提升了一个数量级。
避坑指南：我曾经在高速场景下忽略曲率κ_r的影响，导致s_dot计算偏差，速度规划在弯道入口突然急刹。记住：曲率项不能丢，尤其是大曲率弯道。
3.2.2 Frenet → 笛卡尔（反向转换）
给定 (s, l)，找到参考线上对应点：
// 伪代码：Frenet → 笛卡尔
// 找到s对应的参考点 (x_r, y_r, θ_r, κ_r)
// 法向量 n_r = (-sin(θ_r), cos(θ_r))
x = x_r + l * (-sin(θ_r))
y = y_r + l * cos(θ_r)
// 航向角
θ = atan2(s_dot * sin(θ_r) + l_dot * cos(θ_r),
s_dot * cos(θ_r) - l_dot * sin(θ_r))反向转换在轨迹平滑时特别有用。我习惯在Frenet坐标系下做优化，最后再转回笛卡尔给控制模块。
3.3 SL图与ST图：两个核心工具
这两个图是速度规划的“眼睛”。不理解它们，就没法做决策。
3.3.1 SL图：横向规划的地图
SL图的横轴是s（纵向弧长），纵轴是l（横向偏移）。它描述的是：车辆在道路上的横向位置随纵向位置的变化。
| 维度 | 含义 | 典型值 |
|---|---|---|
| s (横轴) | 沿参考线的弧长 | 0 ~ 200m |
| l (纵轴) | 偏离参考线的横向距离 | -3.5 ~ +3.5m（车道宽） |
SL图主要用于横向避障和车道选择。比如前方有静止障碍物，你需要在SL图上找到一条绕过它的l(s)曲线。
个人经验：SL图上的障碍物通常用“凸多边形”表示。我习惯先做s方向的一维投影，再在l方向做区间合并，这样能快速判断哪些s区间是“被占用的”。
3.3.2 ST图：速度规划的核心
好了，重点来了。ST图的横轴是t（时间），纵轴是s（纵向弧长）。它描述的是：车辆在什么时间到达什么纵向位置。
为什么说ST图是速度规划的核心？因为：
- 速度就是s-t曲线的斜率：v = ds/dt
- 加速度就是s-t曲线的二阶导：a = d²s/dt²
- 障碍物在ST图上表现为“不可通行区域”
你想想看，速度规划的本质不就是找一条s-t曲线，让它避开所有障碍物，同时满足速度、加速度、舒适性约束吗？
一句话总结：ST图把“速度规划”这个动态问题，转化成了“在二维平面上找一条避开障碍物的路径”这个几何问题。这是整个速度规划的理论基石。
3.4 为什么ST图是速度规划的核心？
我结合项目经验，给你拆解三个原因：
- 解耦了时间和空间：ST图把“什么时候到哪”和“怎么到”分开。你只需要关心s-t曲线，横向偏差由SL图处理。
- 障碍物建模直观：动态障碍物在ST图上是一个个“平行四边形”。比如前车以10m/s匀速行驶，它在ST图上就是一个斜率为10的矩形区域。你一眼就能看出“什么时候超车”、“什么时候跟车”。
- 优化问题好求解：ST图上的速度规划可以建模为二次规划（QP）或动态规划（DP）问题。我常用的做法是：先用DP在ST图上粗搜索出一条可行路径，再用QP做平滑优化。
避坑指南：我曾经在ST图上只考虑障碍物的“当前位置”，忽略了它们的“未来轨迹预测”。结果规划出的速度曲线在3秒后直接撞上了一个变道车辆。记住：ST图上的障碍物必须是时变的，要用预测模块的输出。
3.5 坐标系与ST图的关系：一张图看懂
下面这张图展示了Frenet坐标系、SL图、ST图之间的逻辑关系：
3.6 实战中的坐标系选择建议
说了这么多理论，最后给点实际建议：
| 场景 | 推荐坐标系 | 原因 |
|---|---|---|
| 高速公路巡航 | Frenet + ST图 | 道路曲率变化小，s-l解耦效果好 |
| 城区复杂路口 | 笛卡尔 + Frenet混合 | 路口内参考线不稳定，需要笛卡尔做局部修正 |
| 泊车场景 | 笛卡尔为主 | 速度低、路径短，Frenet的曲率假设不成立 |
| 动态避障 | ST图 + SL图联合 | 需要同时考虑横向和纵向的避障空间 |
我的习惯：在Frenet坐标系下做速度规划时，我会把ST图的分辨率设为0.1s（时间）× 0.5m（纵向）。这个分辨率在大多数场景下够用，计算量也适中。如果遇到高动态场景（比如城市快速路），我会把时间分辨率提高到0.05s。
嗯，坐标系和ST图这部分就讲到这里。记住一句话：ST图是速度规划的灵魂，Frenet坐标系是ST图的骨架。搞懂了这两者的关系，后面的速度规划算法才能落地。