简介这份基于Frenet优化轨迹的无人车动作规划实例面向自动驾驶、辅助驾驶领域的算法学习者和工程师聚焦高速场景下轨迹规划与动作决策的Python实现。包内代码包含cubic_spline.py和frenet_optimal_trajectory.py两个核心脚本分别负责三次样条曲线拟合与Frenet坐标系下的最优轨迹生成另有配置与工程说明类文件辅助运行。压缩包共8个文件以py源码和xml配置为主体积仅12KB轻量易读适合快速拆解核心逻辑。目前已有5347人学习下载。通过阅读源码可掌握道路参考线平滑、横纵向轨迹采样、代价函数设计及最优轨迹选择等关键环节并直接用于自身项目验证或二次开发是理解无人车动作规划从理论到落地的良好入门素材。样例代码可直接运行便于对照理论验证输出轨迹。 做无人车决策规划这几年我越来越觉得轨迹规划是整个系统里最“实在”也最考验细节的一环。感知把环境信息给过来定位把自车位置算出来但最终车怎么走、怎么避障、怎么在车流里平稳换道全看动作规划这一层能不能给出既安全又平顺的轨迹。而在所有轨迹生成框架中基于Frenet坐标系的那套优化思路是我自己从科研到落地一直在用、也最推荐初学者先啃明白的方法。这篇文章我会拿一个实际的无人车动作规划实例出来从坐标变换、轨迹生成到代价函数调参完整过一遍把我实测踩过的坑和排查思路一并写出来。无论你是刚接触路径规划的学生还是已经上车调试的工程师都能在这里找到可以直接抄作业的细节。1. 为什么说Frenet是无人车动作规划的“通用语言”1.1 规划层要解决的核心矛盾动作规划或者说运动规划本质上是在回答一个问题给定当前状态、参考路径和周围障碍物信息未来几秒内车辆应该怎么运动才是安全、舒适、高效的。这里面有几个目标经常互相打架——安全要求离障碍物远一点效率要求离边界近一点舒适性要求加速度和急动度小一点。规划层需要做的就是在一堆可行轨迹里找出综合代价最小的那一条并且确认这条轨迹能被执行器真正跟得上。我在实际项目里习惯把动作规划拆成三个子问题路径形状、时间分配和避障约束。路径形状决定车走多弯的线时间分配决定加减速节奏避障约束决定哪些区域不能碰。如果这三件事搅在一起解状态空间会大到没法实时计算。Frenet坐标系的价值就是让横向形状和纵向速度能够分开求解最后再合并验证这也是它能成为工程主流的根本原因。1.2 笛卡尔坐标系下轨迹规划为什么别扭刚入门的时候我在笛卡尔坐标系下直接用X-Y坐标生成轨迹很快就遇到了麻烦。第一横向偏移不好表达。像“贴着车道中心线往左0.5米”这句话在直角坐标下得借助车道线函数去换算一旦车道是弯的这个换算就非常绕。第二参考线约束很难加。车辆必须沿道路方向行驶可直角坐标下的路径采样点经常落在车道外或者需要额外做投影运算每一步都很费劲。第三速度规划与路径形状耦合在一起同一段曲线的曲率会直接影响横向加速度限制直角坐标系下很难直观地分离。相比之下Frenet坐标系把问题重新表达成“沿参考线走了多远s”和“偏离参考线多远d”两个独立量。s是纵向决定车辆跑了多少距离和时间d是横向决定车辆处在车道内什么位置。这样横向避障和纵向加减速就变成两个可以并行优化的子问题规划目标一下子清晰了很多。2. Frenet坐标变换与轨迹生成的核心细节2.1 坐标变换从笛卡尔到Frenet坐标变换是Frenet方法的第一步也是最容易忽略细节的一步。参考线通常是一系列稠密离散点每个点都有位置、朝向角、曲率等属性。要把自车从笛卡尔坐标转换到Frenet坐标第一步是找参考线上距离自车最近的点这个点在自动驾驶里经常叫最近匹配点也有叫投影点的。得到投影点后纵向距离s就是这个投影点对应的累计弧长横向距离d则是自车位置到投影点的带符号距离车辆在参考线左侧为正、右侧为负。这里有一个容易踩的坑参考线采样密度不够时最近的投影点并不唯一尤其是车辆处在车道内偏一侧、又在两个采样点之间的时候。最简单的排查方法是把参考线插值加密采样间隔建议小于0.2米否则曲率变化大的地方d值会抖动直接污染后续的轨迹生成。另外如果车在弯道中d的计算不能简单地用“欧氏距离最近点”还需要考虑朝向一致性否则投影点可能落在反向车道路段上。这个细节我在高速动态场景里吃过亏后文会再提。对于速度转换需要记录投影点处的切线和法线方向。把自车速度分解到切线方向得到纵向速度法线方向得到横向速度。这里还涉及一项由参考线曲率引起的耦合项如果完全忽略高速弯道里规划出的轨迹会和实际位置差出米级。我自己的做法是保留一阶曲率修正项纵向速度超过5m/s且弯道曲率大于0.02时必须参与计算。2.2 横向/纵向轨迹生成与多项式拟合拿到自车当前的Frenet状态后下一步要对横、纵两个方向分别生成候选轨迹。横向轨迹描述d随时间的变化纵向轨迹描述s随时间的变化。我用得最顺手的是五次多项式d(t) a0 a1t a2t^2 a3t^3 a4t^4 a5*t^5。五次多项式的好处在于它能同时满足起点和终点的位置、速度、加速度六个边界条件解起来是线性方程组的闭式解不需要非线性优化实时性非常稳。对纵向轨迹我通常会根据场景降一个维度用四次或五次多项式目标点给出速度约束加速度约束按上限处理。实际生成候选轨迹时做法是这样设定一个时间视界T比如3秒横向偏移目标在可行范围内等间隔采样比如从-2.0米到2.0米每隔0.5米采一个同时横向末端速度设为0纵向速度目标则围绕当前车速等间隔采样比如从0到10m/s每隔1m/s采一组。接着对每一组横纵向目标状态分别求解多项式系数得到一整组s(t)和d(t)曲线。提示横向轨迹和纵向轨迹的时间基准必须一致否则合并出来的轨迹时间逻辑对不上。我习惯先统一离散化时间轴再用同一个t序列去分别评估s和d。很多初学者会忽略这一点结果生成的轨迹在地图上看起来S形扭动怎么调权重都压不下去最后才发现是时间采样不一致导致的。2.3 轨迹代价函数怎么搭轨迹代价函数我推荐分段累加的方式每一段候选轨迹计算五个代价分量与参考线的横向偏差代价、轨迹平滑度代价、与目标速度偏差代价、纵向加速度冲击代价、以及时间效率代价。每项乘一个权重系数再求和。权重怎么调是整个过程中最耗时间的部分我在后文会专门写。更具体一点横向偏差代价可以取目标横向距离与当前横向距离差值的平方平滑度代价使用横向急动度的积分急动度是位置的三阶导数用来衡量“方向盘动作刺激不刺激”速度偏差代价取目标速度与候选末端速度的平方差效率代价直接和规划时间挂钩时间越短代价越小。最终算法对所有候选轨迹按总代价排序选择最靠前的一条做后续验证。3. 一个完整实例从场景设定到最优轨迹输出3.1 场景参数与采样空间设计为了讲得更直观我拿一个实际跑过的仿真场景举例。场景是一条双向两车道的城市道路车道宽度3.5米参考线取本车道中心线总长约200米。自车初始速度8m/s初始横向偏差0.2米正前方40米处有一个静止障碍物占据本车道半边。规划周期20Hz也就是每50毫秒重规划一次时间视界T3秒时间步长0.2秒总共15个离散时刻。这种情况下如果把横向目标从-2.0扫到2.0步长0.5米就是9个候选纵向目标速度从0到12m/s步长1m/s就是13个候选横纵组合一下就是117条候选轨迹。这个规模用C在单核上跑每次规划耗时大约在几毫秒到十几毫秒完全能满足20Hz的实时要求。如果你用Python纯代码跑同样规模可能会到几十毫秒需要做点向量化优化或者减少采样密度。3.2 碰撞检测与可行性筛选候选轨迹生成之后并不能直接选代价最低的那条必须先过三道筛选。第一道是边界约束检查轨迹上每一个离散点对应的d值是否落在道路边界内同时检查自车矩形轮廓在目标位置是否超出车道线。第二道是碰撞约束简单做法是把自车建模成一个带膨胀半径的矩形或圆障碍物也做膨胀处理然后逐采样时刻算距离。我通常把车辆包络近似为三个圆的组合车头、车尾、车中每个圆半径1.1米左右这样既能保留转向姿态信息碰撞检测又不会太贵。第三道是运动学可行性约束包括最大加速度、最大减速度和最大曲率。比如最大减速度我按4.5m/s^2来限制超出直接剔除最大曲率要结合最小转弯半径换算阿克曼底盘的无人车通常会在轨迹生成阶段就把曲率约束考虑进去。三道筛选之后剩下的候选轨迹往往只剩三分之一左右正好进入代价排序环节。3.3 最优轨迹选择与平滑切换筛选后再用代价函数排序。以我上面那个场景为例当自车接近静态障碍物时代价函数会自然地选出两条典型轨迹一条是横向偏移较早开始变化、绕行曲率平缓的轨迹另一条是等到很近才开始猛打方向的轨迹后者虽然横向偏差更小但急动度代价很高综合排序会明显落后。最终输出给控制层的就是一条横向平滑、纵向速度也没有突变的换道绕障轨迹。这里还要提一个工程问题相邻两个规划周期选出的最优轨迹不能差异太大。如果50毫秒前选的是d0.4米50毫秒后突然变成d2.0米即使单条轨迹本身没问题控制层跟起来也会出现明显顿挫。我的处理方式是增加一条“上一周期最优轨迹”作为候选让它在代价排序里获得一个小的惯性偏置保证轨迹在时间维度上连续。这也是很多论文不会写、但工程上几乎必做的细节。4. 代码结构、参数调试与避坑记录4.1 核心代码模块怎么划分工程项目里Frenet规划器我习惯分成五个模块参考线处理、状态转换、轨迹生成、代价评估、轨迹选择与输出。参考线处理负责把高精度地图或感知输出的车道线点云平滑并插值输出一条带曲率和弧长属性的离散线。状态转换模块负责自车状态与Frenet坐标的互转。轨迹生成模块负责多项式求系数和离散化采样。代价评估负责碰撞检测和代价计算。轨迹选择模块负责排序、惯性偏置和结果输出。模块之间用简单的结构体数据流连接不要互相持有复杂引用。比如Frenet状态可以定义成这样struct FrenetState { double s; // 纵向弧长 double s_d; // 纵向速度 double s_dd; // 纵向加速度 double d; // 横向偏移 double d_d; // 横向速度 double d_dd; // 横向加速度 };候选轨迹则包含时间序列、s序列、d序列以及对应的笛卡尔坐标序列。我踩过最大的坑是早期把所有功能塞进一个类里导致最后调试权重时改一个参数要重新编译很久还容易引入状态污染。后来拆成纯函数风格的模块参数配置用单独的文件管理调试效率直线上升。4.2 参数调优过程与踩坑记录调参数这事网上很难找到完整方法论基本是经验和现场调试结合。我先把横向偏差权重设为1.0作为基准然后调平滑度权重。平滑度权重太小轨迹会出现明显的锯齿太大会导致横向变化非常犹豫明明障碍物已经很近了还不舍得变道。经过一组对比实验我发现平滑度权重与横向偏差权重的比值在0.1到1之间比较合适我最终用的是0.4。时间视界T也是关键变量。T太短比如1.5秒车辆根本看不到障碍物后面的空间避障会非常激进T太长比如6秒计算量明显增大同时远处不可见区域的假设与实际偏差变大。实测下来城市工况3秒是一个不错的起点高速工况需要放到5秒以上。纵向速度采样范围也需要注意我一开始把上限定到16m/s结果在城市场景里经常会选出偏高速的轨迹虽然单看轨迹没问题但舒适性很差。后来改成以当前车速为中心做非均匀采样近处密、远处疏效果更好。调参过程中有一点比较容易被忽略不同车速下最优权重并不完全一样。低速时横向灵活最重要高速时平滑和稳定性权重应该提高。我的做法是把权重做成车速的分段函数车速大于15m/s时自动提高平滑度权重实测下来比固定权重舒服很多。5. 常见问题与排查技巧实录5.1 轨迹抖动先看参考线再看采样和时间基准轨迹抖动是我被问得最多的问题。现象是车辆在直线段上正常行驶时前轮转角频繁小幅摆动或者规划出的轨迹横向位置忽左忽右。排查顺序我建议先看参考线。参考线如果滤波过多曲率会出现虚假突变滤波过少离散点噪点又会让投影结果抖动。我自己处理的土办法是输出d值的原始曲线如果d在直线段上出现超过1厘米的高频波动问题大概率出在参考线质量和最近点搜索上。其次是采样密度和时间基准。横纵轨迹的时间步长不一致合并后的轨迹会在地图上呈S形抖动时间步长太大则会导致碰撞检测漏检。建议横向和纵向用同一份离散时间序列并且把时间步长控制在0.1到0.2秒之间。还有一点惯性偏置权重调大了会让轨迹反应迟钝调小了又会抖动我最终用的偏置系数是0.3大家可以把它当作一个经验起点。注意排查抖动优先查参考线质量其次是采样密度和时间一致性最后再怀疑权重参数。5.2 绕障时离障碍物太近膨胀半径与速度约束还有一种常见现象轨迹看起来绕开了障碍物但实车过的时候依然紧张侧向最小距离不到10厘米。问题往往出在碰撞检测的包络模型上。只用后轴中心点做碰撞检测等于把车辆当成质点当然不靠谱。车辆实际宽度通常在1.8米以上绕障时要留至少0.3米的安全余量。我在工程里把障碍物膨胀半径设为0.5米再把车辆当三个圆处理规划出来的绕障轨迹才更接近真实路径。另外绕障轨迹的横向速度不能太大。如果横向速度超过0.8m/s配合曲率变化很容易让车辆产生侧滑感。我会在代价函数里对横向速度项加一个软约束超过阈值时代价指数上升这样可以保证绕障过程既安全又不突兀。5.3 实时性不够采样空间剪枝与缓存策略如果单次规划耗时超过20毫秒就得优化了。我建议先做采样空间剪枝也就是先生成一堆粗粒度候选快速剔除明显不合理的然后在剩余空间里加密采样。比如横向目标可以先以1米间隔采样剔除碰撞和边界越线的再对幸存的目标做0.25米细粒度扩展。这个策略能减少一半以上的无效计算实际效果非常明显。还有一个容易忽略的优化点是距离计算缓存。参考线匹配是实时规划里最耗时的部分之一尤其是参考线有几千个点的时候。我的做法是缓存上一周期的匹配索引在当前周期从它附近开始搜索而不是每次都全量遍历。另外当车辆速度很低时匹配结果变化不大可以隔一个周期再做一次全量搜索校准。性能优化看起来是“小事”但在20Hz控制周期下这几十毫秒就是能不能顺利上车的分水岭。5.4 快速排障速查表我整理了一张排障速查表基本覆盖了这类项目最常见的几种问题可以贴在工位旁边。现象常见原因处理思路直线段轨迹左右抖动参考线噪声大、最近点搜索不稳定加密参考线、缓存上一周期索引转弯处轨迹外抛曲率估计不准、横纵向时间基准不一致修正曲率项、统一离散时间序列绕障距离过近包络模型不合理、横向速度约束缺失车辆模型改多圆、加横向速度软约束频繁紧急制动时间视界过短、纵向速度采样过激进延长T、改为以当前车速为中心采样规划耗时超上限采样空间过大、匹配全量遍历粗筛加细粒度两阶段搜索、缓存匹配索引6. 一些想留给后来者的经验最后写点个人体会。Frenet轨迹优化这套方法能在无人车领域这么多年依然是主流不是因为论文漂亮而是因为它在工程上真的太顺手了横纵解耦让复杂问题变得可拆解多项式拟合让轨迹足够光滑代价函数又给了调参和注入经验的入口。但我也得说实话它并不是银弹。当场景里障碍物数量多到一个候选轨迹要同时避让七八个目标时基于采样的代价优化会变得吃力这时候往往需要跟行为决策层、甚至学习类方法配合使用。如果你正准备在自己项目里落地这个方法我给三个建议。第一先把参考线做扎实参考线质量决定整个规划的上限。第二参数不要一上来就调全局最优先让仿真跑通、记录日志再根据轨迹曲线逐项调权重。第三所有坐标变换和代价计算都加断点日志真出问题的时候回放数据和看数据曲线比在脑子里猜快得多。我自己做这个项目的经验是Frenet方法真正难的不是公式推导而是怎么在工程中把无数细节磨平这个坑只有亲手跑一遍才能体会得到。本文还有配套的精品资源点击获取