简介这是一份基于Frenet优化轨迹的无人车动作规划实战代码包面向自动驾驶、辅助驾驶领域的算法学习者和初中级开发者适用于高速场景下的轨迹规划与动作决策研究。资源采用Python实现涵盖三次样条插值、Frenet坐标系下轨迹生成与优化等核心模块并附有项目配置文件便于直接在IDE中复现调试。整个压缩包共8个文件包含2个Python脚本、4个IDE配置xml文件、1个项目结构文件及1个编译缓存包体仅12KB轻量精简适合快速阅读与二次开发。已有5356人学习下载资源结构清晰可对照博客讲解运行示例理解高速工况下无人车如何生成平滑、安全且满足动力学约束的优化轨迹是一份兼具理论参考与工程实践的入门素材。1. 为什么动作规划要绕道Frenet坐标系解耦是最大红利一个典型的低速园区无人车场景高精地图给出一条参考线车需要绕行前方静止的障碍车、再回到自己的车道。直接在笛卡尔坐标系里生成轨迹会发现障碍物映射、曲率约束、横向控制全搅在一起调一个参数崩一片。基于Frenet优化轨迹的无人车动作规划核心思路是把车辆运动分解成两个独立通道沿参考线的纵向位移s和偏离参考线的横向位移d然后在各通道用低阶多项式生成候选轨迹再用代价函数选优。下面这套思路适合正在搭规划模块、或者被轨迹生成过程困扰的工程师。看懂它对标定、调参和排查都有帮助。2. 把轨迹规划拆成s和d两个通道Frenet坐标与横向轨迹生成2.1 s和d到底拆开了什么从笛卡尔坐标到Frenet坐标的映射在笛卡尔坐标系里一条路径用一系列(x,y)点表示车辆位姿直接落在这些点上。问题在于障碍物是世界坐标系里的位置而车辆行为是相对道路的两套表达混在一起。Frenet坐标换了一种描述方式先对参考线通常来自高精地图的引导线或全局路径做密集采样把每一点的累积弧长记作s再取车辆当前位置在参考线上的最近点作为投影点车辆到投影点的垂直距离记作dd带符号表示在参考线的左侧还是右侧。这样一来原本的二维问题变成了两个一维问题。换道可以写成d从0变为一个车道宽度的过程跟车可以写成s上速度曲线的选择“沿着路走”这个隐含约束自动被参考线消掉。常见的转换做法是先对参考线做插值得到s值对应的坐标再用二分或KD树找最近点批量得到(s,d,Δheading)。注意d不是点到参考线的欧氏距离那么简单还要看投影点处的参考线航向d的正负要结合航向来定义。我一般会在一开始就写一个坐标转换模块把(x,y,heading)到(s,d)和反向转换都封装好后面所有模块只和Frenet状态打交道。这样预测、决策、控制都用同一套约定排查问题能省一半时间。参考线也要先处理全局路径通常是一串比较稀的点直接用会造成s分辨率不足。我先做样条插值到每0.1米一个点再做滑动平均滤波最后重采样成0.2米间隔。很多换道轨迹不平滑的问题根源不在轨迹生成而在参考线本身有毛刺。2.2 用五次多项式生成一条可落地横向轨迹的完整代码横向轨迹描述的是d(s)随s的变化通常在换道和避障场景用五次多项式。五次的好处是能同时约束初始和末端的横向位置、横向速度d′对s的导数、横向加速度d″让轨迹两端都平稳。import numpy as np def lateral_trajectory_coeffs(d0, d1, s_len, vd00.0, vd10.0, dd00.0, dd10.0): 生成横向五次多项式系数轨迹形式: d(s) a0 a1*s a2*s^2 a3*s^3 a4*s^4 a5*s^5 d0: 起点横向位移 (m)d1: 终点横向位移 (m) s_len: 该段轨迹覆盖的弧长 (m) vd0/vd1: 起点/终点 d 对 s 的一阶导dd0/dd1: 起点/终点二阶导 A np.array([ [1, 0, 0, 0, 0, 0], # d(0) d0 [0, 1, 0, 0, 0, 0], # d(0) vd0 [0, 0, 2, 0, 0, 0], # d(0) dd0 [1, s_len, s_len**2, s_len**3, s_len**4, s_len**5], # d(s_len) d1 [0, 1, 2*s_len, 3*s_len**2, 4*s_len**3, 5*s_len**4], # d(s_len) vd1 [0, 0, 2, 6*s_len, 12*s_len**2, 20*s_len**3] # d(s_len) dd1 ]) b np.array([d0, vd0, dd0, d1, vd1, dd1]) return np.linalg.solve(A, b) def sample_lateral(coeffs, s): a0, a1, a2, a3, a4, a5 coeffs d a0 a1*s a2*s**2 a3*s**3 a4*s**4 a5*s**5 d_s a1 2*a2*s 3*a3*s**2 4*a4*s**3 5*a5*s**4 d_ss 2*a2 6*a3*s 12*a4*s**2 20*a5*s**3 return d, d_s, d_ssA矩阵每一行对应一个边界条件前3行是起点状态后3行是终点状态与b向量一一对应。np.linalg.solve是线性代数求解只要A非奇异、s_len不为0就一定有唯一解。但s_len太小时会数值病态实际工程里s_len小于1米时我会直接放弃这条候选轨迹。d′在这里表示横向位移随弧长的变化率不是随时间的变化率这个区分在高速场景里非常关键后面避坑章节会细说。2.3 多项式次数怎么选三、四、五次的适用边界横向换道最常见的正确选择是五次多项式但并不是所有场景都需要它。如果只是车道内微调d的偏差很小三次也够用三次只约束位置和速度加速度在端点可能不连续但实车上影响可控。四次多项式能约束起点位置、速度、加速度和终点位置、速度适合需要控制末端速度但不太关心末端加速度的场景。多项式次数可约束的边界条件典型场景风险三次两端位置、速度车道内小幅修正端点加速度跳变四次起点位置/速度/加速度终点位置/速度停车前的横向调整终点加速度不受控五次两端位置/速度/加速度换道、避障绕行系数过小时数值敏感从我的实践看动作规划层统一用五次多项式最省心。不是五次更高级而是换道场景里末端的d″不给0轨迹几何连续但曲率连续性会出问题参考线本身又有曲率叠加起来方向盘很容易抖。所以我在换道候选里固定五次且末端d″强制为0不对外留配置开关。多项式次数也不是越高越好六次七次虽然能进一步约束jerk的端点但系数对噪声非常敏感T稍长就出现数值摆动反而不如五次可控。少一个参数就少一个坑。3. 纵向速度规划巡航、跟车、停车三招走天下3.1 纵向轨迹不是一条s(t)曲线那么简单横向轨迹解决“走哪条线”纵向轨迹解决“走多快”。纵向规划的输入是当前s、速度、加速度输出是一条s(t)曲线或速度曲线。和横向不同纵向轨迹的自变量是时间t因为车辆沿s的运动本质上是时间序列控制周期和预测周期都按时间来对齐。纵向轨迹的另一个作用是“给横向轨迹计时”横向轨迹的s是弧长但车辆何时到达哪个s由纵向决定。只有把纵向s(t)和横向d(s)拼成以时间为自变量的轨迹控制层才能直接使用。常见的纵向工况有三种。巡航前方没有障碍物或障碍物很远目标是从当前速度平稳过渡到参考速度。跟车前方有车目标是在一段距离内跟随前车速度。停车前方有停止线、行人或急停障碍目标是在规定距离内把速度降到0。三种工况的边界条件差异明显但都能统一到多项式轨迹的框架下给定初末状态求解系数然后把超限的候选丢进代价池里比较。常见误用是直接把s(t)当成笛卡尔x轴上的位移。s是沿参考线的弧长弯道里s和x差很多把s当成x来画会得到一条被“拉直”的错误轨迹。正确做法是每个采样时刻都用坐标转换模块把(s,d)转成(x,y)再交给下游模块。3.2 一个通用的纵向多项式求解函数按次数切换实际工程里我习惯写一个通用的多项式求解函数横向、纵向共用次数作为参数传入让三种工况对应三次、四次、五次可以自由切换。代码里最关键的是纵向轨迹的x是时间t而不是弧长。import numpy as np def poly_coeffs(x0, v0, a0, x1, v1, a1, T, order5): 通用一维多项式轨迹系数求解 x 可以是横向位移 d 对应的自变量 s也可以是纵向位移 s 对应的自变量 t x0: 起点位置v0: 起点速度a0: 起点加速度 x1/v1/a1: 终点对应量T: 自变量区间弧长或时间 order: 多项式次数3/4/5 if order 3: A np.array([[1, 0, 0], [0, 1, 0], [1, T, T**2], [0, 1, 2*T]], dtypefloat) b np.array([x0, v0, x1, v1], dtypefloat) elif order 4: A np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 2, 0], [1, T, T**2, T**3], [0, 1, 2*T, 3*T**2]], dtypefloat) b np.array([x0, v0, a0, x1, v1], dtypefloat) else: A np.array([[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0], [0, 0, 2, 0, 0, 0], [1, T, T**2, T**3, T**4, T**5], [0, 1, 2*T, 3*T**2, 4*T**3, 5*T**4], [0, 0, 2, 6*T, 12*T**2, 20*T**3]], dtypefloat) b np.array([x0, v0, a0, x1, v1, a1], dtypefloat) return np.linalg.solve(A, b)停车工况我一般用五次起点速度是当前车速起点加速度是当前加速度终点速度、终点加速度都设0T按“当前车速除以期望减速度”估算。比如车辆以5 m/s行驶前方10米处需要停车期望减速度按1.5 m/s²估算T大约3.3秒再在3~6秒之间按0.5步长采样生成6条不同减速风格的候选。T太短时末端加速度为零的约束很难满足解出来的多项式会有较大反向加速度过滤阶段大概率被丢掉。T太长车辆会低速蠕行很久进度惩罚项会把它压下去。这样设计候选池不需要人为指定“最优减速”而是把选择权交给代价函数。跟车工况则用四次偏多因为前车速度可以预测但前车加速度预测不准终点加速度不值得花一个自由度去约束。巡航用三次就够初末速度约束好加速度中间自己变化。如果调用时出现np.linalg.LinAlgError十有八九是T太小或者输入状态里有NaN先查这两个。3.3 纵向Jerk限制为什么车内乘客一晕车都怪它乘客的晕车感和纵向加速度变化率强相关业内通常把它称为jerk。动作规划里jerk不是事后算的而是在选轨迹阶段通过代价函数压制的。每个周期会生成一批T不同、目标速度不同的候选轨迹T给得小加减速就猛jerk大T给得大动作慢jerk小但效率差。代价函数里jerk项的量级通常远小于速度偏差项如果不对代价做归一化jerk约束等于没设这点在第5章会展开但设计纵向轨迹时就得提前留意。要看到jerk数值可以对多项式显式求三阶导。对纵向五次多项式三阶导是6a3 24a4t 60a5t²直接算稳定也可以对采样后的加速度序列做一阶差分差分会引入噪声。我在工程里两者都算评估用显式求导监控用差分两个数值长期偏差过大时说明采样间隔太粗要加密。求解阶段能保证多项式平滑但“坐着舒不舒服”最终是代价函数阶段决定的。4. 轨迹选优不再是玄学代价函数设计与权重标定4.1 候选轨迹池怎么生成采样不追求全追求差异化第2章生成了横向五次多项式系统第3章生成了纵向多项式系统。组合起来就是候选池每个横向目标比如目标车道中心d1和一组纵向方案不同的T和目标速度配对形成一条完整轨迹。采样不追求把空间铺满追求差异化的行为选择。举一个具体例子在60米长的换道场景里横向候选目标d1取当前车道中心0、目标车道中心-3.5或3.5按符号约定为了保留“绕障后回到原车道”的意图再加几个中间偏移量比如-1.2和-2.6。每组横向目标配6种纵向方案T从4秒到8秒、参考速度从80%到100%的巡航速度50~80条轨迹就出来了。采样密度不搞等距网格而是让横向候选代表行为意图、纵向候选代表加速风格这样轨迹池的行为多样性高计算量也不大。对每条候选轨迹要先做可行性过滤检查是否碰撞、是否越过道路边界、曲率和加速度是否超限再做代价排序。过滤必须在代价计算之前。某条轨迹代价再低只要碰撞就整条丢弃没有商量的余地。4.2 代价函数代码把安全、效率、舒适拧成一条绳子代价函数的设计目标是可解释每个单项都能说清它惩罚什么。下面是我在模拟项目里常用的结构以候选轨迹的采样序列为输入输出各项代价和加权总分。import numpy as np def evaluate_trajectory(traj, ref_speed, obstacles, weights): traj: 包含 time, s, d, v_s, v_d, jerk_s, jerk_d, dist_obs 的字典 ref_speed: 参考速度 (m/s) obstacles: 障碍物列表 [(s, d, r)] weights: 各代价项权重顺序与下方字典保持一致 costs {} # 横向/纵向jerk取整段轨迹的平均绝对值惩罚不平顺 costs[jerk_lat] np.mean(np.abs(traj[jerk_d])) costs[jerk_lon] np.mean(np.abs(traj[jerk_s])) # 速度偏差参考巡航效率和跟车贴合度 costs[speed_dev] np.mean(np.abs(traj[v_s] - ref_speed)) # 横向偏差惩罚长时间骑线行驶 costs[lane_dev] np.mean(np.abs(traj[d])) # 障碍物距离惩罚取整段轨迹的最小距离距离越近惩罚越大 d_min np.min(traj[dist_obs]) costs[obs] 1.0 / (d_min 1e-3) total sum(costs[k] * w for k, w in zip(costs.keys(), weights)) return costs, totaldist_obs的计算对每个采样点取该点到所有障碍物中心的距离减去障碍物半径和外扩安全距离取最小值。obs项用倒数函数而不是线性函数是因为距离在2米和3米之间的风险差别远大于在10米和11米之间倒数会放大近距离的风险差异。权重顺序必须和costs字典顺序一致我踩过顺序错位的坑结果“安全”被错误放大调参调了半天最后发现是zip配错。代价函数的常见变体是再加“航向偏差”惩罚但我不建议和横向偏差混在一起。横向偏差惩罚的是“骑线”航向偏差惩罚的是车辆航向与参考线的夹角两个量物理意义不同混在一起会让同一条轨迹在弯道处出现奇怪的权重行为。如果场景需要单独加一项heading_dev。提示代价函数里的每个单项都要有物理意义不要为了“效果好”硬加没有解释的惩罚项。后期换场景排查问题能解释的代价项才是可维护的。4.3 权重系数怎么标先归一化再谈调参新手最常见的误区是直接按经验给权重。同样一组权重在一个场景里jerk量级是0.1在另一个场景里变成10速度偏差量级一直是1左右。如果只调权重不归一化相当于在不同场景之间偷偷换了评价标准。我建议的标定流程分三步第一步跑三个典型场景巡航、换道避障、跟车停车把候选池里每个单项代价的中位数和标准差统计出来第二步做z-score归一化第三步再给权重权重只表达对某项的关注程度不再受量纲影响。代价项归一化前典型量级参考权重区间说明横向jerk0.01 ~ 0.50.5 ~ 2换道平顺量级小但很重要纵向jerk0.1 ~ 51 ~ 3加速度变化率速度偏差0 ~ 80.5 ~ 2对应限速与跟车目标横向偏差0 ~ 3.50.1 ~ 1车道中心保持障碍物距离项0.2 ~ 10100 ~ 500安全项建议最高量级归一化常数不要写死。如果某个场景下某项代价的标准差为0说明所有候选轨迹在这一项上没有区分度除零会产生NaN这种情况直接跳过该归一化把该项权重临时置0。另一个实用做法是记录每个场景的归一化统计值做成标定表换场景时对比历史值能很快发现输入数据是否有异常。这些区间只是数量级参考真正落地要以自己候选池的统计为准。5. 避坑指南Frenet轨迹规划的六个高频翻车现场这一章聊坑全是实操里能复现的现象。每条按现象、原因、解决三个层次记录配合代码或参数说明比堆理论有用。5.1 d的正负约定不一致换道方向莫名反向现象同一套坐标转换代码从一个参考线库切到另一个库之后原来向右换道变成向左换道车直接怼向对向车道。原因Frenet坐标里d的正方向在不同库里的定义不同有的以参考线前进方向左侧为正有的右侧为正。坐标转换模块没做统一接口层直接用了对方的d值。解决在整个系统里只保留一种约定比如规定“参考线前进方向左侧为正”所有涉及Frenet坐标的外接库在入口处做一次符号转换。这个约定要写进坐标转换模块的注释里并在单元测试里用一段已知的左弯道数据验证正负号。5.2 末端d″不为零曲率连续性当场翻车现象换道轨迹在s的末点几何上是连续的但车辆在这个位置会有明显转向抖动回放轨迹时发现末端曲率出现一个毛刺。原因只把五次多项式当成“位置的平滑工具”没把末端d″约束成0。横向加速度d″叠加到参考线曲率上末端总曲率突变控制层会为修正误差猛打方向。解决横向换道统一把末尾的dd1设成0。同时检查参考线本身的曲率是否连续如果参考线来自全局路径且有折点先做平滑再进规划。这两个问题往往同时存在单修一个看不出来。5.3 T采样范围太宽轨迹在“等待”和“冲出去”之间横跳现象低速园区场景里规划结果有时是一条几乎不动的轨迹下个周期又变成急加速冲出去的轨迹车体表现犹豫不决。原因纵向轨迹的T采样范围给得太宽T很大的轨迹jerk很小在代价上占便宜但这意味着车辆停在原地很久行为上不符合场景预期。解决给T设上限。我一般用剩余路径长度除以一个最小期望速度来估算T_maxT太小也不行减速度会超限。同时代价函数里加一个“轨迹进度”项惩罚末端s相比当前s没有明显进展的候选项让“原地等待”不再低成本。5.4 d′当成d/dt来用高速大曲率处横向速度算错现象高速弯道场景横向轨迹回算出来的笛卡尔速度合成模长和车体速度对不上偏差能到20%以上。原因Frenet里d′是d对s的导数常规直觉里“速度是位移对时间的导数”在这里不成立。要在笛卡尔坐标里算横向速度得先乘上纵向速度还要处理车辆航向与参考线航向的夹角、参考线曲率等耦合项。解决在轨迹从Frenet转到笛卡尔时做一次速度合成校验。先算出笛卡尔下的vx和vy再和规划速度v做模长对比偏差超过阈值比如5%就丢弃这条轨迹。不要相信转换函数注释里写的“约等于”高速场景约等于会出大事。5.5 代价项没归一化jerk约束形同虚设现象无论怎么把jerk权重调大选出来的轨迹总是速度偏差最小那几条车辆加减速依然激进。原因jerk量级远小于速度偏差未归一化时加权之后被淹没。权重只在归一化之后才有“表达关注度”的意义不归一化就是纯玄学调参。解决先统计候选池所有单项代价的median和std用z-score归一化再加权。实现时注意对同批候选轨迹做实时统计不要把归一化常数写死因为不同场景的代价分布差异很大。5.6 轨迹时间轴没对齐拼接处出现跳变现象预测模块输出的障碍物轨迹是相对时间规划输出的s(t)也是相对时间但合并到控制模块时出现整条轨迹的前段跳变车辆先顿一下再走。原因各模块时间零点不一致。有的用系统启动时间有的用当前周期时间拼接时没有对齐。解决接口层统一用相对周期时间t0表示“当前时刻”所有轨迹结构体从t0开始并且强制第一条采样点等于当前自车状态。每次重规划时把上一周期轨迹锁存与本周期轨迹的前1秒做s、d偏差检查超过阈值就沿用上一周期保证输出到控制层的轨迹时间一致性。6. 让规划结果真正可落地三个验证技巧与一个调参习惯6.1 静态可行性过滤先用稀疏点把坑筛掉轨迹池生成后不要直接进代价函数先做过滤。第一层用稀疏采样比如每0.5秒取一个点检查道路边界、障碍物外扩碰撞、纵向加加速度是否超限。稀疏过滤计算量小把明显不行的轨迹丢出去。第二层对代价排前三的轨迹做密集采样每0.1秒一个点重点检查曲率突变和横向加速度峰值。两层过滤还有个额外好处可以放心把障碍物外扩0.2米作为安全余量因为计算量被控制住了不会拖累实时性。6.2 闭环里看轨迹还得盯住时间一致性很多人验证规划器只看单帧轨迹是否平滑忽略周期之间的连续性。我现在验证时必须回放连续三帧把三个周期的轨迹画在一起看同一s位置上的d是否抖动、速度曲线是否有锯齿。出现锯齿先查时间对齐再查锁存逻辑。这个检查在仿真里看不出问题实车上会表现为一顿一顿的前行排查起来非常费劲。6.3 把代价项逐个打印出来治一治黑匣子最后分享一个习惯调参之前先把某个固定场景下所有候选轨迹的每个代价项分布打出来。比如巡航速度为10时看jerk项是0.1还是10速度偏差项是0.8还是8。分布没有概念之前任何权重都是瞎调。我到现在接新项目第一件事仍然不是调权重而是让每条候选轨迹把单项代价打印出来用表格看一眼量级分布。就这一步能省掉后面至少三天的排查时间。血泪经验是调参前必看单项分布别把规划器当成黑匣子碰运气。希望帮到你。本文还有配套的精品资源点击获取
