企业官网建设流程全解析

简介：一份面向自动驾驶与辅助驾驶领域学习者的无人车动作规划实例，采用Python语言实现，重点演示基于Frenet坐标系的优化轨迹生成方法。示例针对高速场景下的换道、跟驰与巡航等典型工况，有助于理解从候选轨迹采样、代价函数设计到最优轨迹选择的完整决策流程。压缩包共8个文件，包含两个核心Python脚本（三次样条插值与Frenet最优轨迹生成），以及项目配置文件xml、模块描述iml与编译缓存pyc等，整体仅12KB，结构轻量精炼。已有5356人学习下载，具备较高参考价值。读者可结合代码注释与配套博客讲解，复现轨迹优化过程，掌握平滑曲线拟合、动态窗口约束、碰撞避免与舒适性权衡等要点，示例代码清晰、便于二次修改与扩展，适合有一定规划算法基础并希望深入工程实现的开发者。

1. 为什么无人车动作规划绕不开Frenet坐标系：一个实例带你入门

如果让一个刚接触自动驾驶规划的人直接看笛卡尔坐标系下的轨迹规划，他大概率会一头雾水：车辆明明沿着车道走，为什么横向控制要和纵向速度搅在一起？换道时既要考虑横向偏移，又要考虑前方车距，一个方程里塞满了互相耦合的变量。而Frenet坐标系把这个问题拆开了——沿着参考线方向看“走了多远”，垂直参考线方向看“偏了多少”，横向和纵向变成了两个相对独立的子问题。这个实例要讲的就是基于Frenet优化轨迹的无人车动作规划：从采样生成候选轨迹，到代价评估剔除坏轨迹，再到二次规划平滑输出。适合正在做动作规划、轨迹规划或决策规划的工程师，也适合想搞懂自动驾驶车怎么“自己打方向盘”的初学者。读完你可以照着代码跑通一个换道实例，也知道实车调试时最该盯哪些参数。

2. Frenet坐标系与动作规划的基本套路：从采样到最优轨迹选择

2.1 Frenet坐标系为什么适合无人车动作规划：横向纵向解耦

传统笛卡尔坐标系下，一条参考线（比如车道中心线）是一条任意弯曲的曲线，车辆位置用 (x, y) 表示。如果想让车辆沿着车道走，需要把“保持在哪条车道”转换成对 x 和 y 的约束，这个过程在弯道上尤其麻烦——同一个弯道，不同车速对应的前馈转角不同，横向偏差和纵向位置会互相影响。

Frenet坐标系换了一个思路：先用参考线定义一条“道路骨架”，然后车辆的任意位置用两个量描述：s 是沿参考线方向的弧长，l 是垂直于参考线方向的横向偏移。这样一来，“车辆在这条车道没有跑偏”直接对应 l 接近某个目标值，“前方多少米有障碍物”直接对应 s 区间。横向规划只关心 l 随时间怎么变化，纵向规划只关心 s 随时间怎么变化，二者通过公共时间轴和车辆运动学耦合。

这种解耦的价值在工程上非常明显：换道时，横向规划只需要生成一条从当前 l 到目标车道 l 的光滑曲线；纵向规划只需要根据前车距离决定加速还是减速。两个子问题可以分别用不同阶数的多项式拟合，最后再合成一条完整轨迹。代价函数里的舒适性、效率、安全三个指标，也分别落在横向和纵向的不同项上，权重调整变得直观可解释。

当然，解耦不是免费的。Frenet坐标系的建立依赖于参考线的质量：如果参考线本身不平滑，s 方向的弧长累积会出现抖动，投影后 l 也会失真。所以实际项目中，参考线通常经过平滑处理（比如样条拟合或局部优化），再交给规划模块使用。

2.2 动作规划的最小闭环：采样、代价评估、碰撞检测

无论基于Frenet还是笛卡尔坐标系，动作规划模块的输入输出通常是固定的：输入自车当前状态（位置、速度、加速度、航向角）、参考线、障碍物列表和语义地图信息；输出一条未来若干秒内可执行的轨迹（包含时间和速度信息）。在这个闭环里，最常见的做法是“采样-评估-选择”三步走。

第一步是采样。在Frenet坐标系下，横向采样通常取目标横向位置 l_target 的集合（比如变道到相邻车道中心线、回到当前车道中心线、保持现状），纵向采样取目标速度或相对前车的距离集合。每一个“横向末状态 + 纵向末状态 + 末端时间 T”组合，通过多项式拟合生成一条候选轨迹。多项式阶数的选择有讲究：横向常用五次多项式，因为它能同时约束位置、速度和加速度共 6 个边界条件；纵向常用四次或五次多项式，取决于是否需要约束纵向加速度变化率。

第二步是代价评估。每条候选轨迹都要算一个综合代价，包括：横向偏移量（偏离目标车道多远）、纵向速度差（与巡航速度的差距）、加速度和加加速度（舒适性）、轨迹长度（效率），以及碰撞距离裕量（安全）。代价函数通常写成加权和，权重就是调参时最常拧的旋钮。

第三步是碰撞检测。代价函数负责“选好”，碰撞检测负责“排坏”。在Frenet空间中，可以把自车表示成沿参考线的若干个圆或矩形，障碍物投影后占据一段 s 区间和 l 范围，然后检查候选轨迹上每个时刻的自车轮廓是否与障碍物轮廓相交。碰撞检测不能只看路径，还要看时间——同一位置不同时刻到达，碰撞风险完全不同。

这三步构成一个最小闭环。实际无人车系统里，这个闭环每 50ms 到 100ms 跑一次，输出的轨迹会被下游控制模块跟踪。如果采样粒度太粗，可能漏掉最优轨迹；如果太细，又会把CPU占满。所以采样策略和代价函数设计，是动作规划工程师的日常功课。

2.3 一个简单的Frenet轨迹生成实现（Python示例）

这里给一个最简实现：输入自车当前横向状态和纵向状态，输出一条Frenet坐标下的候选轨迹。横向部分用五次多项式，纵向部分用四次多项式。代码省略了坐标系转换，只展示曲线生成的骨架。

import numpy as np def quintic_poly_coeffs(x0, v0, a0, x1, v1, a1, T): # 五次多项式系数，满足起点和终点的位置、速度、加速度约束 # x0/v0/a0: 起点位置、速度、加速度 # x1/v1/a1: 终点位置、速度、加速度 # T: 末端时间 M = 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] ]) b = np.array([x0, v0, a0, x1, v1, a1]) return np.linalg.solve(M, b) def eval_poly(coeffs, t): # 根据多项式系数求时刻t的值 return np.polyval(coeffs[::-1], t) # 横向：从当前l开始，目标偏移l=1.0m，末端横向速度和加速度都为0 T = 5.0 l0, dl0, ddl0 = 0.0, 0.0, 0.0 l1, dl1, ddl1 = 1.0, 0.0, 0.0 c_l = quintic_poly_coeffs(l0, dl0, ddl0, l1, dl1, ddl1, T) # 纵向：从当前速度20m/s，目标速度20m/s（匀速巡航） s0, ds0 = 0.0, 20.0 s1, ds1 = 20.0 * T, 20.0 # 四次多项式：约束位置、速度、加速度起点和位置、速度终点 # 这里为了简化直接用线性插值，展示逻辑 t = np.linspace(0, T, 100) s_traj = s0 + ds0 * t l_traj = [eval_poly(c_l, ti) for ti in t] # 打印几个关键点 for i in range(0, 100, 20): print(f"t={t[i]:.1f}, s={s_traj[i]:.2f}, l={l_traj[i]:.2f}")

逻辑说明：代码里的quintic_poly_coeffs通过解线性方程组求出五次多项式系数，约束条件写成一个 6x6 矩阵。横向轨迹l_traj是基础，纵向部分为了简单用了匀速模型，实际项目中会换成四次多项式或速度规划器。输出的是Frenet坐标下的 (s, l) 序列，还需要通过参考线投影转换成笛卡尔坐标，才能喂给控制模块。

参数说明：T是轨迹末端时间，也是横纵向共用的时间轴，它的取值直接影响轨迹形态。T 太小，换道会很急，横向加速度大；T 太大，轨迹过于保守，可能被后车追尾。常见的取值范围是 5 到 10 秒。横向终点速度dl1和加速度ddl1通常设为 0，保证换道结束后车身稳定。纵向目标速度则来自上层决策（巡航 or 跟车），这个例子里的 20m/s 对应约 72km/h。

3. 基于Frenet优化轨迹的换道实例：从采样到最优动作输出

3.1 场景定义与输入：自车状态、参考线、障碍物

我们来看一个具体的换道实例。假设自车在双车道高速公路的右侧车道行驶，车速 20m/s，前方 60m 处有一辆慢车以 10m/s 速度行驶。左侧车道前方 100m 内没有障碍物。自车决定向左换道超越慢车。这个场景在动作规划里非常典型——包含纵向跟车决策和横向换道动作。

输入分三部分。自车状态包括当前位置、航向角、速度、加速度。参考线从高精地图获得，是一系列离散点，经过平滑后生成一条连续曲线。障碍物列表包括前车的位置、速度、尺寸以及预测轨迹（短期内认为它保持匀速直线运动）。在Frenet空间里，自车状态被投影成 (s, v_s, a_s, l, v_l, a_l)，其中 v_s 和 v_l 分别是沿参考线和垂直参考线的速度分量。

代码层面，输入通常包装成一个结构体。这里用一个简化的Python类来描述：

class DrivingState: def __init__(self, s, s_dot, s_ddot, l, l_dot, l_ddot): self.s = s # 纵向弧长位置 self.s_dot = s_dot # 纵向速度 self.s_ddot = s_ddot # 纵向加速度 self.l = l # 横向偏移 self.l_dot = l_dot # 横向速度 self.l_ddot = l_ddot # 横向加速度 # 当前状态：右侧车道中心线，l=0，匀速20m/s ego = DrivingState(s=0.0, s_dot=20.0, s_ddot=0.0, l=0.0, l_dot=0.0, l_ddot=0.0) # 障碍物：前车在s=60m处，速度为10m/s，车宽1.8m obs = DrivingState(s=60.0, s_dot=10.0, s_ddot=0.0, l=-1.75, l_dot=0.0, l_ddot=0.0)

这里把障碍物也表达成Frenet状态，是为了方便后续在SL空间做碰撞检测。注意前车的 l 坐标等于其所在车道中心线的横向偏移，而不是它的车身宽度。实际系统中，障碍物轮廓会被离散成多个点投影到参考线上。

参数说明：l_dot和l_ddot是横向速度和横向加速度，在稳态跟随中通常为 0。s_ddot是纵向加速度，会进入代价函数。这个状态定义是后面所有轨迹生成的输入接口，实际项目里还会加上时间戳和置信度。

3.2 横向运动规划：采样末状态并生成五次多项式轨迹

横向运动的目标是让车辆从右侧车道中心线（l=0）平顺地移到左侧车道中心线（l=3.75m，标准车道宽度）。对于换道这类动作，横向末速度、末加速度都应为 0。时间 T 是一个采样变量——不同的 T 会产生不同的换道激进程度。

常见做法是设置一组 T 的候选值，例如 4s、5s、6s、7s，分别生成候选轨迹。五次多项式能保证整个换道过程中横向位置、速度、加速度连续，不会出现折点。代码如下：

# 横向采样：目标横向偏移为左侧车道中心线（假设车道宽3.75m） def generate_lateral_trajectories(T_list, l_target, current_state): trajectories = [] for T in T_list: coeffs = quintic_poly_coeffs( current_state.l, current_state.l_dot, current_state.l_ddot, l_target, 0.0, 0.0, T ) t = np.linspace(0, T, 50) l_curve = [eval_poly(coeffs, ti) for ti in t] dl_curve = [eval_poly(np.polyder(coeffs[::-1], 1), ti) for ti in t] trajectories.append((T, l_curve, dl_curve)) return trajectories candidates = generate_lateral_trajectories([4.0, 5.0, 6.0, 7.0], 3.75, ego)

逻辑说明：这里使用了上一节定义的quintic_poly_coeffs。每个 T 生成一条独立的横向偏移曲线和横向速度曲线。注意np.polyder是对多项式系数求导，eval_poly需要把系数反转成np.polyval需要的顺序，实际使用时要封装好。

参数说明：l_target如果设为 3.75m，就是标准换道；如果设为中间值，比如 1.0m，表示压线或借道，通常不会用于正常换道。T 的下限受车辆横向加速度极限约束，一般要求最大横向加速度不超过 3m/s²，所以 T 不能太小。T 的上限受路径规划时效性约束，如果换道时间超过 8s，驾驶员会明显感觉犹豫不决。

3.3 纵向运动规划：速度保持与跟车速度规划

换道场景中，纵向运动需要同时满足两个目标：换道过程中尽量保持巡航速度，但又要避免撞上前方慢车。传统做法是先判断“能不能安全换道”，如果左侧车道有足够的空间，就采用速度保持策略；如果没有，则需要先减速跟随前车，再伺机换道。这个实例假设左侧车道空余，所以纵向规划的任务是在换道期间保持一个稳定的纵向速度。

常见做法是生成一个梯形速度曲线：从当前速度开始，加速到目标巡航速度或减速到目标速度，然后保持匀速。为了与横向轨迹严格同步，纵向轨迹的时间也必须取相同的 T。我们用四次多项式规划纵向位移 s(t)，同时约束起点的 s、v、a 和终点的 s、v，这样能保证纵向加加速度有界。

def quartic_poly_coeffs(x0, v0, a0, x1, v1, T): # 四次多项式，满足起点位置、速度、加速度和终点位置、速度 M = np.array([ [1, 0, 0, 0, 0], [0, 1, 0, 0, 0], [0, 0, 2, 0, 0], [1, T, T**2, T**3, T**4], [0, 1, 2*T, 3*T**2, 4*T**3] ]) b = np.array([x0, v0, a0, x1, v1]) return np.linalg.solve(M, b) # 纵向：起点速度20m/s，终点速度20m/s，位移为20*T c_s = quartic_poly_coeffs(0.0, 20.0, 0.0, 20.0*5.0, 20.0, 5.0)

逻辑说明：这个四次多项式把纵向运动限定在“起点和终点速度一致”的约束下，实际上就是先匀加速再匀减速的平滑版本。如果前方有慢车，终点速度要改成慢车速度，终点位移则根据安全距离计算。纵向和横向的多项式系数都算好后，每个时间戳 t 对应一对 (s(t), l(t))。

参数说明：v1是纵向目标速度。如果换道是为了超车，通常会把 v1 设成比前车高 5m/s 以上。a0是当前纵向加速度，如果自车正在匀速行驶则为 0。终点位移x1不能随便给，它必须满足 x1 = ∫v(t)dt，否则多项式会产生不必要的速度波动。更稳妥的做法是只约束速度不约束终点位移，这属于后续二次规划优化的内容。

3.4 轨迹合并与代价函数设计：舒适性、效率、安全

横纵向轨迹合成笛卡尔坐标轨迹时，最关键是时间轴对齐。横向轨迹有每个时刻的 l，纵向轨迹有每个时刻的 s，但二者必须采用同一个 T 和同一组采样时刻。然后通过参考线的切线方向，把 (s, l) 映射回 (x, y)。

合并代码简要如下：

def frenet_to_cartesian(ref_line, s, l): # ref_line: 从参考线平滑采样得到的离散点，包含x,y,s # 这里简化为线性插值，实际项目用样条插值 i = np.searchsorted(ref_line['s'], s) if i >= len(ref_line['s']) - 1: i = len(ref_line['s']) - 2 s0, s1 = ref_line['s'][i], ref_line['s'][i+1] ratio = (s - s0) / (s1 - s0) x = ref_line['x'][i] + ratio * (ref_line['x'][i+1] - ref_line['x'][i]) y = ref_line['y'][i] + ratio * (ref_line['y'][i+1] - ref_line['y'][i]) theta = ref_line['theta'][i] + ratio * (ref_line['theta'][i+1] - ref_line['theta'][i]) x_l = x - l * np.sin(theta) y_l = y + l * np.cos(theta) return x_l, y_l, theta

合成之后，每条候选轨迹都带有完整的笛卡尔坐标序列和时间戳。接下来就要给它们打分。代价函数一般包含以下几项：

• 横向舒适性：对 l 的加速度和加加速度平方求和，权重较高，因为横向抖动是乘客晕车的主要来源。
• 纵向舒适性：对 s 的加速度和加加速度平方求和。
• 效率：实际平均速度与目标速度的差，差值越大代价越高。
• 安全：轨迹与最近障碍物的距离平方倒数，距离越近代价越高。
• 偏移惩罚：终点 l 与目标车道中心线的偏差。

每个候选 T 对应的轨迹会得到一个总代价，最后选代价最小的作为输出。注意 T 本身也可以作为效率项的一部分——T 越小换道越快，但舒适性可能下降。实车调试时，这几个权重是最常调的。比如某公司在实车测试中发现乘客投诉“换道太猛”，工程师第一反应就是把横向加加速度权重调大，代价是换道时间变长，超车成功率下降。所以权重标定本身就是一场平衡游戏。

4. 轨迹优化与选优：从采样到二次规划平滑

4.1 为什么需要优化：采样轨迹的痛点

采样生成的多项式轨迹看着很平滑，但放到车辆动力学模型里去检查，往往不能直接执行。主要原因有三个：一是多项式轨迹没有考虑车辆最小转弯半径，某些高速换道轨迹的曲率可能超过车辆转向能力；二是横向和纵向分别生成后再合成，合成后的轨迹在重力方向上可能产生不自然的加速度突变；三是采样轨迹末状态只约束了位置、速度、加速度，没有约束加加速度（jerk），导致实际跟踪时方向盘抖动剧烈。

所以真正输出给控制模块之前，轨迹通常要做一次优化。优化的对象可以是采样出来的最优轨迹附近的一个“走廊”，也可以直接是SL空间里的一个连续函数。工程上最常用的做法是把轨迹优化转成一个二次规划问题：目标函数是二次型（比如最小化加加速度平方积分），约束是线性等式和不等式（比如位置边界、速度边界、加速度边界）。二次规划求解快、有全局最优解，适合嵌入式平台。

二次规划并不神秘，它本质上是在采样轨迹周围画一个允许的“管道”，然后在这条管道里面找一条最平滑的曲线。采样轨迹提供的是初始解，QP负责把它修到平滑且满足约束。

4.2 二次规划在Frenet轨迹优化中的应用

在Frenet坐标系下，二次规划的决策变量通常是离散时间点上的横向偏移 l_0, l_1, ..., l_n 和纵向位置 s_0, s_1, ..., s_n，或者更常见的是关于 s 的横向偏移函数 l(s) 的离散点。目标函数包含两部分：一是让 l(s) 尽量靠近参考线（即 l 接近目标车道中心线），二是让相邻点之间的二阶导和三阶导尽量小。

一个简化的QP问题如下：

import numpy as np from scipy.optimize import minimize def smooth_lateral(l_ref, l_init, w_ref=1.0, w_2nd=10.0, w_3rd=5.0): n = len(l_init) def objective(x): # 三项代价：偏离参考线距离、二阶导（加速度）、三阶导（jerk） cost = 0.0 for i in range(n): cost += w_ref * (x[i] - l_ref[i])**2 for i in range(1, n): cost += w_2nd * (x[i] - 2*x[i-1] + x[i-2])**2 if i >= 2 else 0 for i in range(2, n): cost += w_3rd * (x[i] - 3*x[i-1] + 3*x[i-2] - x[i-3])**2 if i >= 3 else 0 return cost res = minimize(objective, l_init, method='SLSQP') return res.x

逻辑说明：这个例子使用scipy.optimize.minimize做一个简化求解，实际项目中会改用基于OSQP或自定义求解器的专用QP库。l_ref是从采样最优轨迹提取出的参考序列，l_init是采样轨迹原始的横向偏移序列。目标函数第一项让轨迹贴合参考线，第二项惩罚相邻点之间的“突变”（近似横向加速度），第三项惩罚三阶差分（近似加加速度）。

参数说明：w_ref是轨迹跟踪权重，调大它会让最终轨尽量贴近采样轨迹；w_2nd是平滑权重，调大后轨迹更平滑，但会偏离采样轨迹；w_3rd是jerk抑制权重，它决定了方向盘抖动的程度。这三个权重的平衡需要结合车辆动力学和乘客舒适度标准来标定。注意这里用差分代替导数，离散时间间隔越小，近似越准，但决策变量越多，求解越慢。

4.3 平滑性与曲率约束处理

优化后的轨迹虽然在数值上平滑了，但还需要显式检查曲率。曲率 κ 的定义是航向角对弧长的导数，在Frenet轨迹合成笛卡尔坐标后，可以近似用相邻点的航向角差除以弧长差来算。车辆的最小转弯半径对应最大曲率，比如某车型的最大转向角对应的曲率上限是 0.1 1/m（即最小转弯半径 10m），如果轨迹上任一点的曲率超过这个值，车辆就无法精确跟踪。

解决曲率超限的方法有三种：第一种是直接检查并丢弃超限轨迹，简单粗暴，但可能让候选轨迹稀疏；第二种是在QP中加入曲率约束不等式，但这会让问题变非线性，增加求解难度；第三种是在代价函数里加入曲率惩罚项，让优化器自己避开大曲率区域。第三种工程上用得多，因为实现最简单。

曲率惩罚项通常这样设计：先计算每个离散点的曲率 κ_i，如果 |κ_i| > κ_max，则惩罚 (|κ_i| - κ_max)^2，否则惩罚为 0。这样优化器只有在不得不突破曲率极限时才会付出代价，从而在大多数情况下自动生成满足转向能力的轨迹。不过，加了曲率惩罚后，QP目标函数不再是纯二次型，需要用迭代重加权或非线性优化器，这就是另一个坑了。

5. 避坑指南：Frenet轨迹规划常见的5个坑

5.1 参考线s坐标突变导致轨迹跳变

现象：车辆在匝道或连续弯道中行驶时，规划出的轨迹在某个帧突然跳变，横纵向控制同时剧烈摆动，就像司机突然猛打了一把方向。排查时发现，参考线的 s 坐标在上一帧和下一帧之间发生了不连续。

原因：参考线是由高精地图离散点拟合的，当车辆位置投影到参考线上时，如果前后两帧使用了不同长度的参考线（比如地图更新、参考线截断），或者参考线在弯道处有折点，s 的累积弧长就会突变。另外，当车辆跨越参考线的投影点时，最近点搜索可能跳到另一段，导致 s 跳变。

解决：一是对参考线做样条平滑，保证一阶、二阶连续；二是使用累积弧长并缓存上一帧的投影点，新一帧以缓存点附近搜索，避免全局最近点跳变；三是在输出轨迹前加一个中值滤波或变化率限制，s 突变超过阈值时沿用上一帧合理值。

5.2 障碍物在Frenet与笛卡尔坐标转换中的失真

现象：某次仿真中，一个静止在人行道边缘的障碍物，投影到Frenet坐标后 l 值忽大忽小，导致自车误判“障碍物侵入车道”而频繁刹车。

原因：障碍物在笛卡尔坐标下静止，但参考线是弯曲的，同一个障碍物在不同 s 处投影得到的 l 不同。当障碍物离参考线较远时，投影点可能跨越曲线的拐点，l 的符号和大小都不可靠。这在曲率大的地方尤其明显。

解决：不要把所有障碍物都强行投影到Frenet空间做判断。对离参考线较远（比如超过一个车道宽度）的障碍物，保留在笛卡尔坐标做碰撞检测；只有靠近参考线的障碍物才投影到SL空间。另外，投影时采用“最近点”而非“垂足点”，对每个障碍物取最小距离的点，并检查距离是否在可信范围内。

5.3 五次多项式轨迹超出曲率极限

现象：高速换道时，采样轨迹看起来l曲线非常平滑，但实车测试时转向系统报“跟踪超限”，方向盘达到饱和，车辆实际轨迹比规划轨迹更晚到达目标车道。

原因：五次多项式约束的是 l(t) 的光滑性，没有约束曲率。换道时间 T 太小或车速太高时，横向速度变化剧烈，合成后的轨迹曲率远超车辆物理极限。这是用位置多项式做规划时最容易踩的坑——位置平滑不等于可执行。

解决：给每条候选轨迹增加一个“动力学可执行检查”：按0.2秒间隔计算曲率，并换算成前轮转角，如果超过转向极限则直接剔除；或者用上文提到的曲率惩罚项。另外，在横向采样时增加一个约束：最大横向加速度 a_l_max = v_s² / R_min，据此反推最小 T。

5.4 采样密度与实时性冲突

现象：为了让轨迹更优，把横向末状态从3个增加到10个，纵向末状态从3个增加到6个，时间T从4个增加到8个，结果单帧规划耗时从30ms涨到180ms，超出100ms控制周期，车辆输出滞后。

原因：采样组合是乘法关系，候选轨迹数量 = 横向目标数 × 纵向目标数 × 时间目标数，盲目加密导致组合爆炸。很多新手忽略了一个事实：相邻采样点生成的轨迹可能非常相似，多数候选在代价评估后排不到前面，纯属浪费算力。

解决：分层采样、逐级筛选。先横向采样，横向选出2到3条有代表性的轨迹；再纵向采样，把横向候选作为基础，每组纵向配置只生成轨迹并做快速碰撞检测；最后只对通过检测的少量候选做高精度代价评估。还可以用上一帧的最优轨迹附近局部采样，而不是全局重采样。

5.5 轨迹合并后横纵向时间轴不同步

现象：换道过程中，纵向速度出现一个奇怪的“点头”现象：明明横向规划很平滑，但车速突然下降又恢复，驾驶员能感觉到车辆在顿挫。

原因：横向轨迹生成时用 T=5s，纵向轨迹生成时用了另一个函数，内部默认了 T=6s，或者纵向用了独立的时间变量。合成到笛卡尔坐标时，横向已经完成了换道，纵向还在加速，两个动作错位，导致加速度矢量方向突变。

解决：从架构上保证横纵向规划共用同一个时间数组t = np.linspace(0.0, T_common, N)，横向和纵向多项式都必须定义在[0, T_common]上。合并前做一个“时间轴一致性断言”：检查横向轨迹和纵向轨迹点数一致、时间戳一致、末端时间偏差小于1ms。这个坑在早期原型代码里极常见，因为横向和纵向往往是两个不同的小组各自维护的模块。

6. 落地验证与进阶：从仿真到实车的动作规划调试技巧

6.1 用仿真环境验证轨迹的快速方法

拿到一条新轨迹，先别急着上车。我会先用一个轻量级仿真环境做闭环验证：给车辆模型输入轨迹跟踪控制器（比如纯跟踪或MPC），看跟踪误差和轨迹本身是否在合理范围内。重点观察三个量：横向跟踪偏差是否超过0.3m，纵向速度是否呈现高频振荡，方向盘转角是否频繁打到饱和。仿真时故意把参考线加一个小的随机扰动，模仿地图误差，能提前暴露Frenet投影的脆弱性。

6.2 实车调试中必看的几个指标

实车调试时，我习惯同时记录四路信号：轨迹s坐标的时间序列，看是否连续平滑；横向偏移 l 的跟踪误差，看稳态偏差；曲率与转向角的对应关系，看是否达到转向极限；以及乘客座椅下方的加速度计数据，那是评估舒适性最直接的方式。如果发现 l 误差在弯道处单调增大，基本可以断定是参考线投影或曲率适配出了问题，而不是控制参数问题。

6.3 一个让你少踩坑的验证习惯

我自己的硬性习惯是：调整任何代价函数权重或采样参数后，必须用同一组回放数据重新规划，把新旧轨迹画在同一个坐标系里对比。不要只看输出的代价数值，因为不同权重之间的代价不能直接比较。某次我调大横向舒适性权重后，代价从 0.35 降到 0.29，以为优化有效，结果对比轨迹图才发现，车辆换道时间被拖长了 2 秒，几乎是在路上“爬行”。从那以后，我只看轨迹形态和物理量指标，不迷信代价分数。换道时长、最大横向加速度、最小安全距离，这三个指标直接写进测试用例，比任何参数调优都有说服力。

这个习惯让我少走了很多弯路。如果你打算上手Frenet轨迹规划，建议也从“先能画轨迹、再能跑仿真、最后才谈实车”开始。动作规划是个系统工程，采样、优化、碰撞检测每一环都会互相影响，希望这些经验能帮你避开我踩过的坑。

本文还有配套的精品资源，点击获取