简介面向自动驾驶与辅助驾驶领域的算法学习者和开发者这份Python项目演示了高速场景下基于Frenet优化轨迹的无人车动作规划思路。通过完整可运行的代码读者可以学习如何将道路坐标转换到Frenet系理解轨迹生成、平滑与可行性验证的整体流程适合具有一定Python基础、希望深入局部路径规划的读者。压缩包共8个文件核心为2个py脚本——三次样条插值工具与Frenet最优轨迹生成主程序配套xml工程配置、iml模块文件及pyc编译缓存整体仅12KB轻量且易于直接运行调试。目前已有5347人次学习浏览对正在学习路径规划、动作规划或准备入门自动驾驶项目的开发者有一定参考价值。借助代码读者可直观看到不同采样参数对轨迹形态的影响并结合作者博客梳理代价函数设计、障碍物约束处理和横向/纵向运动解耦等关键点修改车道间距、速度等参数后还能快速验证多种场景下的规划效果是理解无人车动作规划从理论到实现的实用样例。 作为一名在自动驾驶行业摸爬滚打了多年的工程师我深知「Frenet」这三个字在无人车动作规划中的分量。我们常说车要开得「像老司机」本质上就是要在任意弯曲的道路上既能把控好方向盘、保持车道又能安全平稳地跟车、变道、避障。而这一切的起点就是坐标系的选择。坦白说早期我用笛卡尔坐标系做轨迹规划时被弯道上的目标点约束问题折腾得够呛直到彻底吃透基于Frenet优化轨迹的动作规划框架才真正体会到什么叫「豁然开朗」。这篇文章我不打算聊教科书上那种枯燥的推导而是以我实际跑过的一个项目为例把「为什么用Frenet」「轨迹怎么生成」「代价函数怎么设计」以及「实车调试中的那些坑」拆开讲透。无论你是刚接触动作规划的学生还是已经在工程里被各种轨迹抖动折磨的同行这篇都值得收藏慢慢看。1. 为什么无人车的动作规划绕不开Frenet坐标系1.1 笛卡尔坐标系下的「约束困境」先从我踩过的一个坑说起。项目初期我直接在全局坐标系笛卡尔坐标系下规划轨迹车辆的位姿是 (x, y, yaw)规划的每一步都要把车的位置、朝向、曲率放在统一的XY平面上计算。当时遇到一个典型的场景车辆需要在一个曲率较大的弯道内侧变换车道目标点不仅要满足横向位移还要时刻保持在道路边界内。麻烦就出在这里道路边界在X-Y平面上是一个不规则的弯曲区域如果你想约束轨迹不压线就得把道路边界离散成几百个点然后做大量点在多边形内部的判断计算量陡增。更要命的是横向偏移和纵向行驶距离在笛卡尔坐标系下是高度耦合的。你想让车在弯道里平缓地横向偏移但同样的横向偏移量在弯道外侧和内侧对应的实际道路空间完全不同。我花了大量时间去调整约束条件结果轨迹仍然会出现局部抖动就是因为坐标系本身的耦合特性把问题复杂化了。1.2 Frenet框架真正表达的是什么后来我切换到了Frenet坐标系一句话概括它的核心思想以参考线通常是道路中心线或车道中心线为基准把车辆的任意运动拆解成两个独立的分量——纵向位移 s沿着参考线走了多远和横向偏移 d偏离参考线多少。这个拆解带来的好处是革命性的。道路的弯曲形状被「拉直」成了一个弧长轴 s原本在笛卡尔坐标系下的弯道约束变成了对横向偏移 d 的简单区间约束。比如「车辆不能压线」可以直接写成一个不等式d_min d d_max。而「车辆需要保持车道中心」则意味着目标横向偏移 d_target ≈ 0。这就很像我们平时开车时脑子里的导航逻辑我不会去想「我现在在世界坐标系里的绝对坐标是多少」而是想「我离车道中心偏了多远」「前方弯道还有多少米」。Frenet坐标系本质上就是把这种人类驾驶直觉数学化。所以做无人车动作规划时Frenet 几乎是绕不开的基础设施。2. 从行车状态到Frenet轨迹完整的数学建模与状态转换2.1 Frenet坐标系下车辆状态的六个关键量要在代码里实现这个框架第一步是把车辆的实时状态映射到Frenet坐标系中。这里需要用到六个关键量它们共同描述了一辆车在当前参考线上的运动状态s纵向弧长表示车辆在参考线上的投影位置。s_dot纵向速度即沿参考线方向的速度分量。s_ddot纵向加速度。d横向偏移表示车辆相对参考线的侧向距离左负右正符号约定因工程而异。d_dot横向速度即横向偏移的变化率。d_ddot横向加速度。严格来说完整的Frenet公式还会涉及 d 对 s 的各阶导数d、d、d用来关联轨迹几何与车辆运动学。但在实际的采样规划中我们通常会保留时间域上的导数因为后续生成轨迹多项式时时间域积分更直观。2.2 从笛卡尔坐标到Frenet坐标的转换实践工程上做坐标转换最核心的工作是找到车辆当前位置在参考线上的最近投影点。这个点对应的弧长就是 s而从车辆位置到投影点的有向向量长度就是 d。我们项目里的做法是把参考线预采样成密集的点列每0.5米一个点然后用KD-Tree加速查找最近点。这里有一个容易被忽略的细节找到最近点之后还需要对s坐标做线性插值修正。因为最近点所在的线段其方向可能和车辆前进方向有夹角直接取离散点索引做s会导致纵向速度产生微小跳变。我试过忽略这个修正结果在弯道出口处轨迹曲率出现了周期性抖动。坐标转换的伪代码大致长这样def cartesian_to_frenet(x, y, ref_line): # 1. KD-Tree 查找最近点索引 nearest_idx kd_tree.query([x, y])[1] # 2. 在最近线段上做投影插值 segment_start ref_line[nearest_idx] segment_end ref_line[nearest_idx 1] dx segment_end.x - segment_start.x dy segment_end.y - segment_start.y seg_len sqrt(dx * dx dy * dy) # 3. 计算横向偏移带符号 cross dx * (y - segment_start.y) - dy * (x - segment_start.x) d cross / seg_len # 4. 计算纵向弧长考虑插值比例 proj_ratio ((x - segment_start.x) * dx (y - segment_start.y) * dy) / (seg_len * seg_len) s ref_line.cumulative_s[nearest_idx] proj_ratio * seg_len return s, d这一步如果做得扎实后面轨迹生成的所有输入都是干净、连续的能省下无数排查时间。3. 候选轨迹生成横向与纵向解耦后的采样与多项式拟合3.1 横向轨迹五次多项式连接起终点状态有了当前状态下一件大事就是「生成候选轨迹」。Frenet框架的核心魅力在于横向运动 d(t) 和纵向运动 s(t) 可以被分别规划然后在时间轴上重新组合。先看横向。我在项目中生成横向轨迹时使用的是五次多项式d(t) a0 a1·t a2·t² a3·t³ a4·t⁴ a5·t⁵为什么是五次而不是三次因为横向轨迹需要满足的边界条件有六个起点位置的横向偏移、横向速度、横向加速度以及终点位置的横向偏移、横向速度、横向加速度。六个未知数a0~a5六个方程恰好能解出唯一解。如果需要终点加速度可控三次多项式是无论如何也做不到的。实际操作中我会先把当前车辆的 d、d_dot、d_ddot 读出来作为起点状态然后遍历一系列目标横向偏移值 d_target。比如在标准双车道场景下我会采样 {-3.5, -3.0, -2.5, ..., 0, ..., 3.5} 米同时设定到达时间 T 的采样集合常见的取值是 {1秒, 2秒, 3秒, 4秒, 5秒}。这样横向规划就生成了一批「从当前位置平滑挪到不同横向位置」的轨迹簇。3.2 纵向轨迹四次多项式让速度变化更平滑纵向轨迹 s(t) 采用了四次多项式s(t) b0 b1·t b2·t² b3·t³ b4·t⁴这里只需要五个边界条件因为纵向规划的终点通常不需要约束加速度只需要约束位置和速度即可。起点纵向位置 s、纵向速度 s_dot、纵向加速度 s_ddot终点纵向位置 s_target、终点纵向速度 s_dot_target。我特别强调一个技巧终点速度的选择决定了车辆的驾驶风格。如果你想让车在跟车时保持舒适那么 s_dot_target 应该朝向目标车辆的当前速度收敛。比如我在高速巡航场景下会设置一个「巡航速度」参数比如 25 m/s约90 km/h然后所有纵向采样的终点速度都向这个值靠拢。而在路口或拥堵场景则会根据前车速度动态调整。纵向采样的目标位置 s_target 不是随意定的它由想要到达的纵向距离范围决定。比如前方10米、30米、60米处分别采样一个目标位置加上目标速度就生成了一组全速域可选的纵向轨迹。3.3 采样组合与轨迹合并成完整运动轨迹横向轨迹和纵向轨迹生成好之后就要把它们「合体」。具体的做法是在每一个时间步 t 上把横向多项式给出的 d(t) 和纵向多项式给出的 s(t) 合并成一个Frenet坐标点 (s(t), d(t))然后转换回笛卡尔坐标系。这个合体过程在代码里特别容易出错因为它要求横向和纵向的采样时间集合必须完全对齐。我见过不少新手直接把两个独立时间轴的结果套在一起导致轨迹出现「时间扭曲」。稳妥的做法是先定义一个统一的时间轴比如 t 0, 0.1, 0.2, ..., T_max再分别对横向和纵向多项式做求值最后配对组合。组合之后的轨迹还要做一次几何转换。可以把参考线的每个点看成是一个局部坐标系原点把 (s(t), d(t)) 映射到全局坐标系。这个环节里s 方向决定参考线切向d 方向决定参考线法向转换公式是def frenet_to_cartesian(s, d, ref_line): # 找到参考线上 s 对应的区间插值得到位置和切线方向 x_ref, y_ref, yaw_ref interpolate_ref_line(ref_line, s) # 法向方向偏移 x x_ref - d * sin(yaw_ref) y y_ref d * cos(yaw_ref) return x, y到这里一条完整的候选轨迹就生成了。批量遍历横向目标偏移和纵向目标位置的笛卡尔积就能得到几十上百条候选轨迹下一步就是从中选出最优。4. 最优轨迹筛选代价函数设计、碰撞检测与权重调参4.1 代价函数的架构与各项的物理意义候选轨迹生成后不能一股脑全交给控制模块必须通过「淘汰评分」两道关卡。淘汰靠碰撞检测和物理约束检查评分靠代价函数。我项目的代价函数长这样J_total w_safe · J_collision w_jerk · J_jerk w_lat · J_lateral_offset w_eff · J_efficiency w_consistency · J_consistency每一项的物理意义很直接J_collision安全代价轨迹与障碍物距离的惩罚项。如果距离小于安全阈值这一项直接给极大值距离越大惩罚越小。更精细的做法是引入「时间维度」的碰撞检查即考虑障碍物未来一段时间的预测位置与轨迹在对应时刻的位置比较。J_jerk舒适性代价评价轨迹的加加速度即横向jerk和纵向jerk的加权和。这个值越低车辆加减速和方向盘动作越柔和乘客感受越舒适。J_lateral_offset偏移代价评价轨迹与参考线的偏差程度。它反映了「保持车道中心」的倾向避免车辆在无必要的情况下频繁变道或压线。J_efficiency效率代价评价轨迹与期望车速的偏差。如果前方畅通效率代价会鼓励车辆加速到巡航速度如果前方堵车效率代价会引导车辆跟随前车速度。J_consistency一致性代价比较当前候选轨迹与上一周期规划结果的差异。这是我在实车调试后补充的非常重要。没有这一项车辆在快速切换规划周期时轨迹容易左右摇摆乘感极差。4.2 碰撞检测与物理约束检查的前置把关代价函数之前必须先做硬约束检查。我把它分成三层静态障碍物碰撞把栅格地图上被占用的区域和轨迹点做距离检查。这里我使用「圆-矩形」粗检测速度快然后对候选轨迹上曲率较大的点做精确检测避免轨迹弧线中部侵入障碍物。动态障碍物碰撞一辆无人车不可能只面对静止的锥桶。对于动态车辆我会用简单的线性速度模型预测它未来3~5秒的包围盒位置然后和轨迹逐时刻检查。这里有个经验预测时间窗不要设太长超过5秒的预测模型误差极大反而会误杀很多本来安全的轨迹。车辆动力学约束生成轨迹时可以天马行空但最终能落地的轨迹必须满足车辆本身的转向和加速度极限。比如最大转向角约束会转化为轨迹最大曲率限制最大加速度/减速度约束会对应纵向加速度限制。如果一个候选轨迹在以上任意一层被卡住就直接淘汰不再参与代价评分。这样既保证了安全性又大幅减少了评分阶段的计算量。4.3 权重调参的几个实用方向代价函数写完之后真正的工程难点才出现权重怎么设。我常用的方法是「分层调参」第一层先固定安全相关权重把J_collision调成一个「一刀切」的硬门限不是靠权重压而是靠淘汰机制兜底。第二层调舒适性和一致性。先设定一组默认权重在仿真环境里跑标准弯道和变道场景观察横向加速度曲线。如果峰值加速度超过0.3g说明J_jerk权重偏低或采样时间窗太短我会把采样时间窗拉长1~2秒而不是一味加权重这样更容易根治抖动。第三层调效率和偏移。城市道路场景中效率和偏移经常打架——想开得快就难免偏离车道中心。我的建议是在限速较高的场景让效率权重占优在拥堵场景让偏移和舒适权重占优。一个我常用的初始权重方案相对值代价项权重比例说明J_collision1000本质替代淘汰碰到即死J_jerk10保证基本舒适度J_lateral_offset5趋向车道中心J_efficiency8追求合理速度J_consistency15抑制轨迹抖动这套参数在我的项目里表现稳定但具体数值必须在你的场景里重新标定。不要迷信任何一套固定权重。5. 实车落地中被反复教育的几个细节5.1 Frenet坐标与笛卡尔坐标转换的精度陷阱仿真里跑得好好的轨迹一上实车就可能出现「车辆画龙」。我排查了整整两周最后定位到问题根源参考线投影的精度不够。当车辆实际位置与参考线的最近点落在两个预采样点之间时线性插值会引入一个微小的切向误差这个误差在弯道曲率变化剧烈的地方会被放大导致转换出的笛卡尔轨迹出现高频抖动。解决办法是在投影点附近做一次局部二阶插值或者把参考线采样间隔从0.5米加密到0.2米。加密之后轨迹抖动问题基本消失。代价是内存和查找时间略微增加但换来的是实车稳定性这笔账非常划算。5.2 高速场景下横向采样的范围收缩很多从仿真转到实车的朋友会忽略一个问题横向采样范围应该随车速变化。在低速泊车场景横向采样范围可以拉到 ±3.5 米甚至更大让车有足够的机动空间。但在高速公路上车速到 80 km/h 以上时横向采样范围如果还是 ±3.5 米生成的轨迹往往因为横向加速度过大而被动力学约束淘汰有效轨迹稀疏甚至可能导致规划无解。我实际的做法是做一个简单的线性收缩max_d clamp(0.5 0.04 * v, 1.0, 3.5)车速越高横向可探索范围越窄。这样既能保证高速下轨迹都在车辆物理极限附近又能让低速时的机动灵活性不丢失。5.3 轨迹连续性防止上一帧到下一帧的跳变最后聊一个所有实车工程师都会遇到的痛点轨迹跳变。原因是每个规划周期都是独立采样的上一帧选出的最优轨迹是T1下一帧可能因为障碍物位置轻微变化或权重微小波动选出一条与前帧差异很大的轨迹T2。如果直接把T2交给控制模块车辆会猛地打一把方向。解决这个问题我在项目中引入了「上一周期的轨迹作为基准」策略。具体来说把上一帧的最优轨迹作为一组固定的候选轨迹加入本次的候选集中。在代价函数里提高 J_consistency 的权重凡是与上帧轨迹偏离过大的候选直接扣分。这套策略实车效果非常显著。我见过一个真实案例去掉轨迹连续性约束后车辆在自动驾驶变道时平均每15秒出现一次明显的横向抖动加上之后连续跑了30分钟都没有一次明显跳变。另外还有一个细节在做轨迹平滑时我会对最终输出的路径点做一个轻量级的曲率平滑比如滑动窗口平均或道格拉斯-普克抽稀后再插值。这个步骤不改变轨迹的整体形态但能显著减少方向盘修正频率对乘坐体验的改善非常直接。最后再分享一个我个人的调试心得每次改完代价函数或采样参数不要只在单一场景下验证。我会把同一套参数丢到三组数据里——高速弯道、城市拥堵、紧急避障——去跑回归测试。只有三个场景都不出问题才敢提交。因为Frenet框架的解耦特性虽然强大但横纵向轨迹组合之后小概率会出现「横向没问题、纵向也没问题、合在一起就是不舒服」的耦合效应。这种时候检查一下是不是采样组合里漏掉了「动态障碍物预测轨迹与候选轨迹重叠」的情况通常能找到答案。本文还有配套的精品资源点击获取 Frenet坐标系在无人车动作规划中的核心原理与工程实践
简介面向自动驾驶与辅助驾驶领域的算法学习者和开发者这份Python项目演示了高速场景下基于Frenet优化轨迹的无人车动作规划思路。通过完整可运行的代码读者可以学习如何将道路坐标转换到Frenet系理解轨迹生成、平滑与可行性验证的整体流程适合具有一定Python基础、希望深入局部路径规划的读者。压缩包共8个文件核心为2个py脚本——三次样条插值工具与Frenet最优轨迹生成主程序配套xml工程配置、iml模块文件及pyc编译缓存整体仅12KB轻量且易于直接运行调试。目前已有5347人次学习浏览对正在学习路径规划、动作规划或准备入门自动驾驶项目的开发者有一定参考价值。借助代码读者可直观看到不同采样参数对轨迹形态的影响并结合作者博客梳理代价函数设计、障碍物约束处理和横向/纵向运动解耦等关键点修改车道间距、速度等参数后还能快速验证多种场景下的规划效果是理解无人车动作规划从理论到实现的实用样例。 作为一名在自动驾驶行业摸爬滚打了多年的工程师我深知「Frenet」这三个字在无人车动作规划中的分量。我们常说车要开得「像老司机」本质上就是要在任意弯曲的道路上既能把控好方向盘、保持车道又能安全平稳地跟车、变道、避障。而这一切的起点就是坐标系的选择。坦白说早期我用笛卡尔坐标系做轨迹规划时被弯道上的目标点约束问题折腾得够呛直到彻底吃透基于Frenet优化轨迹的动作规划框架才真正体会到什么叫「豁然开朗」。这篇文章我不打算聊教科书上那种枯燥的推导而是以我实际跑过的一个项目为例把「为什么用Frenet」「轨迹怎么生成」「代价函数怎么设计」以及「实车调试中的那些坑」拆开讲透。无论你是刚接触动作规划的学生还是已经在工程里被各种轨迹抖动折磨的同行这篇都值得收藏慢慢看。1. 为什么无人车的动作规划绕不开Frenet坐标系1.1 笛卡尔坐标系下的「约束困境」先从我踩过的一个坑说起。项目初期我直接在全局坐标系笛卡尔坐标系下规划轨迹车辆的位姿是 (x, y, yaw)规划的每一步都要把车的位置、朝向、曲率放在统一的XY平面上计算。当时遇到一个典型的场景车辆需要在一个曲率较大的弯道内侧变换车道目标点不仅要满足横向位移还要时刻保持在道路边界内。麻烦就出在这里道路边界在X-Y平面上是一个不规则的弯曲区域如果你想约束轨迹不压线就得把道路边界离散成几百个点然后做大量点在多边形内部的判断计算量陡增。更要命的是横向偏移和纵向行驶距离在笛卡尔坐标系下是高度耦合的。你想让车在弯道里平缓地横向偏移但同样的横向偏移量在弯道外侧和内侧对应的实际道路空间完全不同。我花了大量时间去调整约束条件结果轨迹仍然会出现局部抖动就是因为坐标系本身的耦合特性把问题复杂化了。1.2 Frenet框架真正表达的是什么后来我切换到了Frenet坐标系一句话概括它的核心思想以参考线通常是道路中心线或车道中心线为基准把车辆的任意运动拆解成两个独立的分量——纵向位移 s沿着参考线走了多远和横向偏移 d偏离参考线多少。这个拆解带来的好处是革命性的。道路的弯曲形状被「拉直」成了一个弧长轴 s原本在笛卡尔坐标系下的弯道约束变成了对横向偏移 d 的简单区间约束。比如「车辆不能压线」可以直接写成一个不等式d_min d d_max。而「车辆需要保持车道中心」则意味着目标横向偏移 d_target ≈ 0。这就很像我们平时开车时脑子里的导航逻辑我不会去想「我现在在世界坐标系里的绝对坐标是多少」而是想「我离车道中心偏了多远」「前方弯道还有多少米」。Frenet坐标系本质上就是把这种人类驾驶直觉数学化。所以做无人车动作规划时Frenet 几乎是绕不开的基础设施。2. 从行车状态到Frenet轨迹完整的数学建模与状态转换2.1 Frenet坐标系下车辆状态的六个关键量要在代码里实现这个框架第一步是把车辆的实时状态映射到Frenet坐标系中。这里需要用到六个关键量它们共同描述了一辆车在当前参考线上的运动状态s纵向弧长表示车辆在参考线上的投影位置。s_dot纵向速度即沿参考线方向的速度分量。s_ddot纵向加速度。d横向偏移表示车辆相对参考线的侧向距离左负右正符号约定因工程而异。d_dot横向速度即横向偏移的变化率。d_ddot横向加速度。严格来说完整的Frenet公式还会涉及 d 对 s 的各阶导数d、d、d用来关联轨迹几何与车辆运动学。但在实际的采样规划中我们通常会保留时间域上的导数因为后续生成轨迹多项式时时间域积分更直观。2.2 从笛卡尔坐标到Frenet坐标的转换实践工程上做坐标转换最核心的工作是找到车辆当前位置在参考线上的最近投影点。这个点对应的弧长就是 s而从车辆位置到投影点的有向向量长度就是 d。我们项目里的做法是把参考线预采样成密集的点列每0.5米一个点然后用KD-Tree加速查找最近点。这里有一个容易被忽略的细节找到最近点之后还需要对s坐标做线性插值修正。因为最近点所在的线段其方向可能和车辆前进方向有夹角直接取离散点索引做s会导致纵向速度产生微小跳变。我试过忽略这个修正结果在弯道出口处轨迹曲率出现了周期性抖动。坐标转换的伪代码大致长这样def cartesian_to_frenet(x, y, ref_line): # 1. KD-Tree 查找最近点索引 nearest_idx kd_tree.query([x, y])[1] # 2. 在最近线段上做投影插值 segment_start ref_line[nearest_idx] segment_end ref_line[nearest_idx 1] dx segment_end.x - segment_start.x dy segment_end.y - segment_start.y seg_len sqrt(dx * dx dy * dy) # 3. 计算横向偏移带符号 cross dx * (y - segment_start.y) - dy * (x - segment_start.x) d cross / seg_len # 4. 计算纵向弧长考虑插值比例 proj_ratio ((x - segment_start.x) * dx (y - segment_start.y) * dy) / (seg_len * seg_len) s ref_line.cumulative_s[nearest_idx] proj_ratio * seg_len return s, d这一步如果做得扎实后面轨迹生成的所有输入都是干净、连续的能省下无数排查时间。3. 候选轨迹生成横向与纵向解耦后的采样与多项式拟合3.1 横向轨迹五次多项式连接起终点状态有了当前状态下一件大事就是「生成候选轨迹」。Frenet框架的核心魅力在于横向运动 d(t) 和纵向运动 s(t) 可以被分别规划然后在时间轴上重新组合。先看横向。我在项目中生成横向轨迹时使用的是五次多项式d(t) a0 a1·t a2·t² a3·t³ a4·t⁴ a5·t⁵为什么是五次而不是三次因为横向轨迹需要满足的边界条件有六个起点位置的横向偏移、横向速度、横向加速度以及终点位置的横向偏移、横向速度、横向加速度。六个未知数a0~a5六个方程恰好能解出唯一解。如果需要终点加速度可控三次多项式是无论如何也做不到的。实际操作中我会先把当前车辆的 d、d_dot、d_ddot 读出来作为起点状态然后遍历一系列目标横向偏移值 d_target。比如在标准双车道场景下我会采样 {-3.5, -3.0, -2.5, ..., 0, ..., 3.5} 米同时设定到达时间 T 的采样集合常见的取值是 {1秒, 2秒, 3秒, 4秒, 5秒}。这样横向规划就生成了一批「从当前位置平滑挪到不同横向位置」的轨迹簇。3.2 纵向轨迹四次多项式让速度变化更平滑纵向轨迹 s(t) 采用了四次多项式s(t) b0 b1·t b2·t² b3·t³ b4·t⁴这里只需要五个边界条件因为纵向规划的终点通常不需要约束加速度只需要约束位置和速度即可。起点纵向位置 s、纵向速度 s_dot、纵向加速度 s_ddot终点纵向位置 s_target、终点纵向速度 s_dot_target。我特别强调一个技巧终点速度的选择决定了车辆的驾驶风格。如果你想让车在跟车时保持舒适那么 s_dot_target 应该朝向目标车辆的当前速度收敛。比如我在高速巡航场景下会设置一个「巡航速度」参数比如 25 m/s约90 km/h然后所有纵向采样的终点速度都向这个值靠拢。而在路口或拥堵场景则会根据前车速度动态调整。纵向采样的目标位置 s_target 不是随意定的它由想要到达的纵向距离范围决定。比如前方10米、30米、60米处分别采样一个目标位置加上目标速度就生成了一组全速域可选的纵向轨迹。3.3 采样组合与轨迹合并成完整运动轨迹横向轨迹和纵向轨迹生成好之后就要把它们「合体」。具体的做法是在每一个时间步 t 上把横向多项式给出的 d(t) 和纵向多项式给出的 s(t) 合并成一个Frenet坐标点 (s(t), d(t))然后转换回笛卡尔坐标系。这个合体过程在代码里特别容易出错因为它要求横向和纵向的采样时间集合必须完全对齐。我见过不少新手直接把两个独立时间轴的结果套在一起导致轨迹出现「时间扭曲」。稳妥的做法是先定义一个统一的时间轴比如 t 0, 0.1, 0.2, ..., T_max再分别对横向和纵向多项式做求值最后配对组合。组合之后的轨迹还要做一次几何转换。可以把参考线的每个点看成是一个局部坐标系原点把 (s(t), d(t)) 映射到全局坐标系。这个环节里s 方向决定参考线切向d 方向决定参考线法向转换公式是def frenet_to_cartesian(s, d, ref_line): # 找到参考线上 s 对应的区间插值得到位置和切线方向 x_ref, y_ref, yaw_ref interpolate_ref_line(ref_line, s) # 法向方向偏移 x x_ref - d * sin(yaw_ref) y y_ref d * cos(yaw_ref) return x, y到这里一条完整的候选轨迹就生成了。批量遍历横向目标偏移和纵向目标位置的笛卡尔积就能得到几十上百条候选轨迹下一步就是从中选出最优。4. 最优轨迹筛选代价函数设计、碰撞检测与权重调参4.1 代价函数的架构与各项的物理意义候选轨迹生成后不能一股脑全交给控制模块必须通过「淘汰评分」两道关卡。淘汰靠碰撞检测和物理约束检查评分靠代价函数。我项目的代价函数长这样J_total w_safe · J_collision w_jerk · J_jerk w_lat · J_lateral_offset w_eff · J_efficiency w_consistency · J_consistency每一项的物理意义很直接J_collision安全代价轨迹与障碍物距离的惩罚项。如果距离小于安全阈值这一项直接给极大值距离越大惩罚越小。更精细的做法是引入「时间维度」的碰撞检查即考虑障碍物未来一段时间的预测位置与轨迹在对应时刻的位置比较。J_jerk舒适性代价评价轨迹的加加速度即横向jerk和纵向jerk的加权和。这个值越低车辆加减速和方向盘动作越柔和乘客感受越舒适。J_lateral_offset偏移代价评价轨迹与参考线的偏差程度。它反映了「保持车道中心」的倾向避免车辆在无必要的情况下频繁变道或压线。J_efficiency效率代价评价轨迹与期望车速的偏差。如果前方畅通效率代价会鼓励车辆加速到巡航速度如果前方堵车效率代价会引导车辆跟随前车速度。J_consistency一致性代价比较当前候选轨迹与上一周期规划结果的差异。这是我在实车调试后补充的非常重要。没有这一项车辆在快速切换规划周期时轨迹容易左右摇摆乘感极差。4.2 碰撞检测与物理约束检查的前置把关代价函数之前必须先做硬约束检查。我把它分成三层静态障碍物碰撞把栅格地图上被占用的区域和轨迹点做距离检查。这里我使用「圆-矩形」粗检测速度快然后对候选轨迹上曲率较大的点做精确检测避免轨迹弧线中部侵入障碍物。动态障碍物碰撞一辆无人车不可能只面对静止的锥桶。对于动态车辆我会用简单的线性速度模型预测它未来3~5秒的包围盒位置然后和轨迹逐时刻检查。这里有个经验预测时间窗不要设太长超过5秒的预测模型误差极大反而会误杀很多本来安全的轨迹。车辆动力学约束生成轨迹时可以天马行空但最终能落地的轨迹必须满足车辆本身的转向和加速度极限。比如最大转向角约束会转化为轨迹最大曲率限制最大加速度/减速度约束会对应纵向加速度限制。如果一个候选轨迹在以上任意一层被卡住就直接淘汰不再参与代价评分。这样既保证了安全性又大幅减少了评分阶段的计算量。4.3 权重调参的几个实用方向代价函数写完之后真正的工程难点才出现权重怎么设。我常用的方法是「分层调参」第一层先固定安全相关权重把J_collision调成一个「一刀切」的硬门限不是靠权重压而是靠淘汰机制兜底。第二层调舒适性和一致性。先设定一组默认权重在仿真环境里跑标准弯道和变道场景观察横向加速度曲线。如果峰值加速度超过0.3g说明J_jerk权重偏低或采样时间窗太短我会把采样时间窗拉长1~2秒而不是一味加权重这样更容易根治抖动。第三层调效率和偏移。城市道路场景中效率和偏移经常打架——想开得快就难免偏离车道中心。我的建议是在限速较高的场景让效率权重占优在拥堵场景让偏移和舒适权重占优。一个我常用的初始权重方案相对值代价项权重比例说明J_collision1000本质替代淘汰碰到即死J_jerk10保证基本舒适度J_lateral_offset5趋向车道中心J_efficiency8追求合理速度J_consistency15抑制轨迹抖动这套参数在我的项目里表现稳定但具体数值必须在你的场景里重新标定。不要迷信任何一套固定权重。5. 实车落地中被反复教育的几个细节5.1 Frenet坐标与笛卡尔坐标转换的精度陷阱仿真里跑得好好的轨迹一上实车就可能出现「车辆画龙」。我排查了整整两周最后定位到问题根源参考线投影的精度不够。当车辆实际位置与参考线的最近点落在两个预采样点之间时线性插值会引入一个微小的切向误差这个误差在弯道曲率变化剧烈的地方会被放大导致转换出的笛卡尔轨迹出现高频抖动。解决办法是在投影点附近做一次局部二阶插值或者把参考线采样间隔从0.5米加密到0.2米。加密之后轨迹抖动问题基本消失。代价是内存和查找时间略微增加但换来的是实车稳定性这笔账非常划算。5.2 高速场景下横向采样的范围收缩很多从仿真转到实车的朋友会忽略一个问题横向采样范围应该随车速变化。在低速泊车场景横向采样范围可以拉到 ±3.5 米甚至更大让车有足够的机动空间。但在高速公路上车速到 80 km/h 以上时横向采样范围如果还是 ±3.5 米生成的轨迹往往因为横向加速度过大而被动力学约束淘汰有效轨迹稀疏甚至可能导致规划无解。我实际的做法是做一个简单的线性收缩max_d clamp(0.5 0.04 * v, 1.0, 3.5)车速越高横向可探索范围越窄。这样既能保证高速下轨迹都在车辆物理极限附近又能让低速时的机动灵活性不丢失。5.3 轨迹连续性防止上一帧到下一帧的跳变最后聊一个所有实车工程师都会遇到的痛点轨迹跳变。原因是每个规划周期都是独立采样的上一帧选出的最优轨迹是T1下一帧可能因为障碍物位置轻微变化或权重微小波动选出一条与前帧差异很大的轨迹T2。如果直接把T2交给控制模块车辆会猛地打一把方向。解决这个问题我在项目中引入了「上一周期的轨迹作为基准」策略。具体来说把上一帧的最优轨迹作为一组固定的候选轨迹加入本次的候选集中。在代价函数里提高 J_consistency 的权重凡是与上帧轨迹偏离过大的候选直接扣分。这套策略实车效果非常显著。我见过一个真实案例去掉轨迹连续性约束后车辆在自动驾驶变道时平均每15秒出现一次明显的横向抖动加上之后连续跑了30分钟都没有一次明显跳变。另外还有一个细节在做轨迹平滑时我会对最终输出的路径点做一个轻量级的曲率平滑比如滑动窗口平均或道格拉斯-普克抽稀后再插值。这个步骤不改变轨迹的整体形态但能显著减少方向盘修正频率对乘坐体验的改善非常直接。最后再分享一个我个人的调试心得每次改完代价函数或采样参数不要只在单一场景下验证。我会把同一套参数丢到三组数据里——高速弯道、城市拥堵、紧急避障——去跑回归测试。只有三个场景都不出问题才敢提交。因为Frenet框架的解耦特性虽然强大但横纵向轨迹组合之后小概率会出现「横向没问题、纵向也没问题、合在一起就是不舒服」的耦合效应。这种时候检查一下是不是采样组合里漏掉了「动态障碍物预测轨迹与候选轨迹重叠」的情况通常能找到答案。本文还有配套的精品资源点击获取