做运动规划这些年真正让我反复返工的往往不是算法公式而是坐标系。至少在动态街道场景里是这样早期的版本我直接在笛卡尔坐标系下采样 (x, y) 点结果在直道上跑得好好的车一进弯道就开始切内弯、贴着路缘石走横向加速度抖得像心电图。后来把整个规划器搬到 Frenet 坐标系下横向一条五次多项式、纵向一条四次多项式配上 S-T 图做动态障碍物检查代码量少了一半行为反而更像人开车了。这篇内容就把 Frenet坐标系、动态街道场景、最优轨迹生成这条链路从头拆一遍坐标系怎么定义、参考线怎么建、投影怎么算、动态障碍物怎么塞进时空图、代价函数怎么配权重、参数怎么调、哪些坑一定会踩。适合已经会写基础规划器、但总觉得输出不够顺的同学也适合刚入门想搞清楚工业界常见做法的人。1. 先把坐标系这件事讲透Frenet到底在解决什么1.1 笛卡尔坐标系下直接规划为什么总会贴着障碍物走在笛卡尔坐标系里一条轨迹的表示方式是 (x(t), y(t)) 或者更完整的 (x, y, θ, κ, v, a) 序列。这个表示法本身没问题问题出在约束的表达上。道路是弯的车道边界是曲线保持在车道内这条最基本的约束就得写成车道边界曲线与车辆位置之间的不等式而这条曲线通常只能用离散点或样条表示落到优化问题里就是一堆非线性、非凸的约束。求解器在这种约束下要么收敛慢要么干脆给你一个局部最优的怪解。采样法的处境同样尴尬。在笛卡尔平面上撒点弯道区域的样本会大量落到车道外面落到车道外的候选直接被丢弃等于白算而为了让保留率够高你又不得不把采样密度调大算力消耗成倍增长。更隐蔽的问题是横向控制精度笛卡尔系里横向偏移是个随位置变化的量规划器很难直接约束横向加速度只能通过曲率间接推推出来的值在弯道入口和出口往往已经超了舒适区间。举个具体的数城市快速路设计半径常在 250 米上下车速 25 m/s 通过时横向加速度是 v²/R 625/250 2.5 m/s²。这个值已经贴近乘客舒适度的上限稍微快一点就会让人明显感到被甩。而在笛卡尔系里做横向加速度约束你得先把轨迹拟合成曲率序列再反推中间任何一步滤波没做好约束就失真了。1.2 把曲线拉直Frenet坐标的三要素Frenet坐标系的思路特别朴素既然道路是弯的那就用一条参考线把道路拉直然后在一个顺着路走、一个垂直于路走的两个方向上描述世界。顺路方向叫纵向记作 s单位是米物理含义是沿着参考线走了多长的弧长垂直方向叫横向记作 l单位也是米通常在参考线左侧为正、右侧为负。这里有一个特别有用的类比把参考线想成一条河道中心线你的车就是一艘船漂在河里。s 是你顺流漂了多远l 是你偏离河道中心线多少米。船在河里的位置只要这两个数就够了河道是弯是直完全不影响你对位置的描述。落到轨迹规划上保持在车道内这条约束就直接变成了 l 落在 [-w/2, w/2]w 是车道宽度一个与 s 完全无关的常数区间。这一下子就把一个随位置变化的曲线约束变成了一个矩形约束。除了 s 和 l实际工程中还会用到它们对时间的一阶、二阶导数s_dot 是纵向速度s_ddot 是纵向加速度l_dot 是横向速度l_ddot 是横向加速度。这四个量加上 s、l 本身构成了规划器真正操作的状态向量。横向加速度在这里还有一个非常好用的解析表达a_lat 约等于 l_ddot 加上参考线曲率带来的离心项。只要把 l_ddot 约束住乘客体感就能被直接控制住不用再去反推曲率。我把两套坐标系在规划场景下的差别整理成一张表对比更直观对比维度笛卡尔坐标系Frenet坐标系位置表示(x, y)(s, l)车道保持约束曲线/非线性随位置变化矩形区间与 s 无关横向加速度表达需由曲率间接推导近似等于 l_ddot直接可控采样效率弯道样本大量落在车道外样本天然贴合道路走向参考线依赖无强依赖参考线变化即失效适用场景泊车、越野、无结构化区域结构化道路、高速、城区街道1.3 参考线与Frenet坐标系的关系到底是谁决定谁这个问题被问得最多答案其实很干脆参考线决定 Frenet坐标系而不是反过来。Frenet坐标系的原点位置、s 轴走向、l 轴正方向、曲率定义全部由参考线唯一确定。也就是说Frenet坐标系不是一个全局坐标系它是一个贴着某条参考线长出来的局部坐标系。同一辆车在同一个物理位置上如果参考线从本车道中心线换成了隔壁车道中心线它的 (s, l) 就会完全不一样。这个性质带来两个工程上必须遵守的规则。第一条规划和坐标系绑定参考线必须和轨迹一起往控制模块传。你不可能只传一条 (s, l) 轨迹而不告诉控制模块它相对哪条参考线否则控制模块根本还原不出笛卡尔轨迹。第二条参考线切换的那一帧要特别小心。变道决策生效后规划器参考线从 A 车道切到 B 车道上一帧所有历史状态在新坐标系下的值都变了如果直接把上一帧终点作为本帧初值轨迹会出现一个突兀的跳变车会横着抽一下。我处理这个问题的做法是统一走一次重投影把上一帧的车辆状态笛卡尔下的位置、速度、加速度重新往新参考线上投一遍拿到新的 (s, l) 和导数作为初值再开始规划。这一步只多几毫秒但能消掉绝大部分变道瞬间的抽搐。参考线变化场景常见做法容易出的问题直道巡航参考线不变沿用上一帧投影结果做初值需加时间一致性检查防止投影跳点变道参考线切换车道重新投影重置横向状态为0直接沿用旧初值会导致横向速度突变路口转弯参考线重定向拼接新旧参考线保证切点曲率连续拼接处曲率不连续加速度跳变参考线整体更新地图版本切换全局重投影清空历史缓存缓存了旧坐标系下的障碍物位置2. 参考线构建与坐标投影整套方案的隐形地基2.1 参考线从哪来以及为什么它比算法更重要参考线的来源通常有三个高精地图里直接给出的车道中心线、搜索模块算出来的全局路径、决策模块选定车道序列后拼接出来的局部参考线。工业界最常见的做法是第三种由决策给出未来要走哪几条车道规划模块按车道序列从地图取中心线拼接成一条长度在 100 到 300 米之间的局部参考线。拼接和取点有两个硬性要求。第一是采样间距要均匀我一般用 0.5 到 2 米太密了计算量浪费太稀了曲率差分噪声大。第二是曲率必须做平滑地图给的离散点直接做二阶差分求曲率噪声会非常吓人通常会先做一次样条拟合再求曲率或者在曲率序列上跑一个滑动平均。这件事的重要性被严重低估参考线曲率如果不连续规划出来的轨迹横向加速度就会在同一个位置反复跳控制模块跟着抖最后背锅的是控制参数其实是参考线的锅。还有一个容易被忽略的细节是参考线长度和起点位置。参考线的起点一般放在车辆当前位置往后一点保证投影时车辆不会跑到参考线范围之外终点则要覆盖整个规划时域内的最大行驶距离。以 25 m/s 车速、8 秒规划时域估算需要覆盖 200 米再留一点余量参考线长度取 250 米是比较稳的。如果只给 100 米高速场景下轨迹还没规划完参考线就没了投影直接失败。2.2 笛卡尔到Frenet的投影从粗定位到Newton精修投影是整套方案里最核心的一段代码它的任务是给定车辆在笛卡尔下的状态 (x, y, θ, v, a)求出它在参考线上的 (s, l) 以及对应的 s_dot、s_ddot、l_dot、l_ddot。第一步是粗定位用 KD-tree 在参考线离散点上找距离车辆最近的点作为 s 的初值。第二步是精修这一步必须做因为最近点不等于投影点。投影的严格定义是参考线上的点 R(s) 满足向量 P - R(s) 与参考线在该点的切向垂直也就是正交投影条件。写成函数就是 f(s) (P - R(s)) · t_r(s) 0其中 t_r 是参考线切向量。这个方程用 Newton 法迭代解导数也很干净f(s) l(s)·κ_r(s) - 1κ_r 是参考线曲率l 是横向偏移。迭代式就是 s ← s - f(s) / (l·κ_r - 1)。一般迭代三到五次就能收敛到毫米级比直接取最近点精确得多尤其是在曲率大的弯道上最近点和正交投影点能差几十厘米。拿到 s 和 l 之后速度加速度的转换用下面这组公式这也是这个领域里最经典的一组关系式。设 Δθ θ - θ_r 是车辆航向与参考线切向的夹角v 是车辆速度κ_r 是参考线曲率κ_r 是曲率对 s 的导数s_dot v·cos(Δθ) / (1 - κ_r·l) l_dot v·sin(Δθ) s_ddot [a_x - (s_dot²·(1 - κ_r·l)·κ_r l_ddot)·sin(Δθ)] / ((1 - κ_r·l)·cos(Δθ)) (2·s_dot·l_dot·κ_r s_dot²·l·κ_r) / (1 - κ_r·l) l_ddot [a_y (s_ddot·(1 - κ_r·l) - 2·s_dot·l_dot·κ_r - s_dot²·l·κ_r)·sin(Δθ)] / cos(Δθ) - s_dot²·(1 - κ_r·l)·κ_r这两条公式里 s_ddot 和 l_ddot 互相耦合实际代码里通常先按小角度近似取一遍再用一次迭代修正误差就够小了。下面是我常用的实现骨架import numpy as np from scipy.spatial import cKDTree class ReferenceLine: def __init__(self, xs, ys, ds1.0): self.xs np.asarray(xs, dtypefloat) self.ys np.asarray(ys, dtypefloat) self.n len(xs) # 弧长参数化 dxy np.hypot(np.diff(self.xs), np.diff(self.ys)) self.s np.concatenate([[0.0], np.cumsum(dxy)]) # 切向、法向、曲率 tx np.gradient(self.xs, self.s) ty np.gradient(self.ys, self.s) norm np.hypot(tx, ty) self.tx, self.ty tx / norm, ty / norm self.nx, self.ny -self.ty, self.tx # 左法向l 左正右负 # 曲率用切向角变化率先平滑再求导 theta_r np.unwrap(np.arctan2(self.ty, self.tx)) kappa np.gradient(theta_r, self.s) self.kappa self._smooth(kappa, win9) self.tree cKDTree(np.column_stack([self.xs, self.ys])) staticmethod def _smooth(a, win9): k np.ones(win) / win pad win // 2 return np.convolve(np.pad(a, pad, modeedge), k, modevalid) def _interp(self, s_query): s_query np.clip(s_query, self.s[0], self.s[-1]) return ( np.interp(s_query, self.s, self.xs), np.interp(s_query, self.s, self.ys), np.interp(s_query, self.s, self.tx), np.interp(s_query, self.s, self.ty), np.interp(s_query, self.s, self.kappa), ) def project(self, x, y, s_initNone, iters5): if s_init is None: _, idx self.tree.query([x, y]) s_init self.s[idx] s float(s_init) for _ in range(iters): rx, ry, tx, ty, kappa self._interp(s) dx, dy x - rx, y - ry l dx * (-ty) dy * tx # 投影到法向 f dx * tx dy * ty # 正交条件残差 dfds l * kappa - 1.0 s s - f / dfds rx, ry, tx, ty, kappa self._interp(s) l (x - rx) * (-ty) (y - ry) * tx return s, l, kappa提示投影函数的输入一定带上上一帧的 s 作为初值 s_init不要每次都从 KD-tree 全局搜索开始。全局搜索在参考线自交的场景下会跳到错误的分支上而且浪费计算。加上时间一致性检查本帧 s 与上帧 s 的差值不应超过 v·dt 加一个小余量超出就说明投影跳点了。2.3 投影的三个经典坑分支、自交与符号第一个坑是分支问题。参考线是弯曲的当车辆偏离参考线较远比如两三米以上且正好在弯道内侧时KD-tree 的最近点可能落在参考线的另一段上投影结果会莫名其妙地跳到前面几十米的 s。这个现象在上坡下坡的高架匝道、环形立交附近特别常见。解法就是前面说的用上一帧的 s 做初值同时限制每帧的 s 增量。第二个坑是参考线自交。掉头、环形匝道、广场环岛这类几何参考线在物理空间上会和自己交叉同一辆车的投影点不唯一。严格来说这时候 Frenet 表示本身就退化了因为 (s, l) 到 (x, y) 的映射不再是一一对应。工程上的做法是避免在这种区域使用单条参考线改成按行驶方向切分成多段参考线或者直接退化回笛卡尔坐标系下的规划。第三个坑是 l 的符号约定。左正右负还是左负右正本身没有对错但整个项目必须完全统一从投影、轨迹生成、代价函数到控制接口任何一处反了都会导致车往反方向打方向盘。我见过的最隐蔽的一次事故就是地图参考线的法向定义和规划器的法向定义差了个负号直道上完全看不出来一进弯道车就往弯外偏。注意参考线的曲率一定要做平滑且在投影时对 κ_r 做上下限截断。κ_r 数值上出现尖峰会让 s_dot 的分母 (1 - κ_r·l) 趋于零或变负进而算出莫名其妙的纵向速度轨迹直接失控。3. 动态街道场景怎么建模障碍物、预测与时空图3.1 静态和动态障碍物必须分开处理Frenet 坐标系下处理障碍物有一个天然便利静态障碍物投影之后就是一串 (s, l) 点加上尺寸直接膨胀成一个矩形区域就完事了。这里要注意膨胀量要包含车身宽度的一半加上安全裕度而且最好按 s 方向再留一点制动距离的余量。静态障碍物的区域在 (s, l) 平面上是常驻的不随时间变化所以可以一次性生成反复复用。动态障碍物就没这么简单了因为它带着时间维度。街道场景和高速场景最大的区别也在这里高速上大部分时间只有前后几辆车结构简单街道上会出现横穿的行人、从路边突然探头出来的自行车、路边临时停靠的网约车开门、对向右转车辆抢占你的车道。这些目标的运动方向和你的行驶方向往往不同甚至垂直笛卡尔系里处理这种异向运动会非常别扭Frenet 系里则统一变成它在我的 s 轴上往前推了多少、在我的 l 轴上横移了多少。我的处理流程是分三步。第一步把每个动态障碍物的预测轨迹通常是一串带时间戳的 (x, y, θ, v)投影到同一条参考线上得到它在每个时刻的 (s, l)。第二步用车辆矩形在 Frenet 下做近似膨胀s 方向膨胀约等于车长的一半加安全距离l 方向膨胀约等于车宽的一半加安全距离。第三步把所有时刻的占据区间连起来就得到了一个在时间轴上滑动的占据条形。障碍物类型预测时域典型膨胀策略备注前向同向车辆3 到 5 秒s 方向加 0.5 倍车距l 方向加半车宽速度较准时可适当收紧横穿行人2 到 3 秒各向同性膨胀按 1.0 米/秒保守扩散预测误差大宁可多让路边静止车辆全程按静态障碍物处理l 方向加开门余量街道场景建议至少留 1 米对向来车1 到 2 秒s、l 两个方向都加大膨胀城市窄路必须留足侧向余量大型车辆3 到 5 秒s 方向膨胀按实际车长盲区大避免长时间并排3.2 S-T图与L-T图把动态障碍物变成约束真正好用的表达方式是 S-T 图和 L-T 图。S-T 图横轴是时间纵轴是纵向弧长 s动态障碍物在上面画出一块块占据矩形矩形的位置随时间推进而前移。你的规划轨迹在 S-T 图上是一条曲线只要这条曲线和任何一块矩形不相交纵向就安全了。L-T 图同理横轴时间纵轴横向偏移 l用来判断横扫类冲突。为什么要在两个图上分开检查因为直接做时空联合的碰撞检测虽然严格但计算量大而且很难给规划器提供清晰的该往哪调的梯度。分开检查的好处是S-T 图冲突说明纵向速度要调加速或减速就能解决L-T 图冲突说明横向要保持或者换道两侧都冲突说明得让行。这直接对应到实际驾驶行为上非常符合直觉。实际实现中我不会真的去画图而是把它写成区间重叠判断。对每个候选轨迹在同一组时间戳上和每个障碍物的预测做双重检测纵向重叠且横向重叠才算碰撞。只在纵向重叠就判碰撞会过度保守导致车无谓地减速只在横向重叠就判碰撞会把合理的并线也封死。def check_dynamic_conflicts(s_traj, l_traj, t_grid, predictions, ego_len4.8, ego_wid1.9, margin_s1.5, margin_l0.5): s_traj, l_traj: 候选轨迹在 t_grid 上的采样值 predictions: list of dict{t: array, s: array, l: array, len: float, wid: float} 返回 True 表示安全False 表示存在冲突 for pred in predictions: t_o pred[t] # 只比较时间上重叠的部分 mask (t_grid t_o[0]) (t_grid t_o[-1]) if not mask.any(): continue t_cmp t_grid[mask] s_o np.interp(t_cmp, t_o, pred[s]) l_o np.interp(t_cmp, t_o, pred[l]) half_s pred[len] / 2.0 ego_len / 2.0 margin_s half_l pred[wid] / 2.0 ego_wid / 2.0 margin_l overlap_s np.abs(s_traj[mask] - s_o) half_s overlap_l np.abs(l_traj[mask] - l_o) half_l if np.any(overlap_s overlap_l): return False return True提示S-T 图检查的时间戳一定要和预测轨迹的时间戳对齐不要各用各的网格再去插值凑。插值本身会引入误差在高速场景下几米的位置误差就足够把一次安全通过判成碰撞或者反过来。我一般让规划的时间网格直接取 0.1 秒固定步长预测模块也按这个步长输出。3.3 不确定性怎么办把预测误差随时域膨胀进去预测轨迹永远有误差而且误差随时间增长。刚出预测的那一刻误差可能只有几十厘米三秒之后就可能是好几米。所以静态的膨胀量是不够用的膨胀量应该随时域变化。我的经验公式是横向膨胀量在 0.5 米基础上每预测一秒再加 0.3 到 0.5 米纵向膨胀量在 1.5 米基础上每预测一秒再加 0.3 米左右。具体值要看你的预测模块实际精度最好用实际数据的误差分位数来标定比如取预测误差的 90 分位数作为膨胀量。还有一个更稳妥的做法是做多模态预测的并集。如果预测模块输出的是三条概率不同的可能轨迹不要只取概率最高的那条而是把三条都拿来做检查或者把三条轨迹包络成一个更宽的占据区域。这样会保守一些但在街道场景下保守一点不亏因为街道上最贵的成本是事故不是通行效率。交互也是绕不开的。目标车辆会因为你而改变行为最典型的就是你加速逼近时前车突然变道或者你在路口减速时侧向车辆加速抢行。完整的做法是预测和规划联合迭代但工程上更常见的是在预测里加一个交互项或者把预测出来的轨迹按最不利原则取包络。我做街道场景的时候会额外加一条规则如果目标车的预测轨迹和我当前规划的轨迹在时空上有交叉趋势就把它的不确定性膨胀量直接翻倍让它自己先长胖一圈再判断。4. 最优轨迹的生成与求解4.1 横向五次、纵向四次阶数怎么选轨迹生成的第一步是选函数形式。Frenet 系下最经典的做法是横向用五次多项式、纵向用四次多项式原因在于边界条件的数量刚好匹配。横向五次多项式 l(t) c0 c1·t c2·t² c3·t³ c4·t⁴ c5·t⁵六个待定系数对应六个边界条件起点 l0、l0、l0终点 l1、l1、l1。为什么横向要约束到二阶导数因为 l 对应横向速度l 对应横向加速度这两个直接决定乘客体感。终点横向速度、横向加速度都设为零意味着轨迹到达目标横向偏移时已经摆正了不会斜着扎进目标车道。纵向四次多项式 s(t) a0 a1·t a2·t² a3·t³ a4·t⁴五个系数配五个条件起点 s0、v0、a0终点速度 v1、终点加速度 a1 0而终点位置 s1 不约束。这个设计非常巧妙它让纵向轨迹天然具有速度保持的行为给定一个目标速度 v1 和一个时间 T就能解出一条平滑过渡到目标速度的轨迹跑多远由时间和速度自己决定。横向五次的封闭解可以直接写出来设 Δ l1 - l0T 是走完的时间c0 l0 c1 l0 c2 l0 / 2 c3 (20·Δ - (12·l0 3·l0·T)·T) / (2·T³) c4 (-30·Δ (16·l0 3·l0·T)·T) / (2·T⁴) c5 (12·Δ - (6·l0 l0·T)·T) / (2·T⁵)纵向四次同理设起点加速度 a0a0_c s0 a1_c v0 a2_c a0 / 2 a3_c (v1 - v0 - (2.0 / 3.0)·a0·T) / T² a4_c (v0 a0·T / 2 - v1) / (2·T³)这套公式我用了几年实测比直接调线性求解器快而且数值更稳。用一个小例子验算一下横向从 0 走到 1 米起点速度和加速度都是零T 取 1 秒代入得 c3 10、c4 -15、c5 6回代 l(1) 10 - 15 6 1l(1) 30 - 60 30 0l(1) 60 - 180 120 0三个边界条件全部满足可以放心用。如果场景需要精确停到某个位置比如跟车刹停到前车后方 5 米处纵向就要升级成五次多项式把终点位置也固定住代价是终点速度和加速度都要设为零灵活性下降。什么时候用四次、什么时候用五次我的判断标准很简单末状态是速度给定用四次末状态是位置给定用五次。4.2 代价函数怎么设计jerk、时间、偏离和速度偏差解出一堆候选之后得有个标准来挑。Frenet 系下最常用的代价函数由几项组成横向和纵向分开算再加权求和。横向代价通常包含三项。第一项是 jerk 的平方在时间上的积分也就是 ∫ l²dt它衡量这条轨迹颠不颠。jerk 是加速度的变化率人对它非常敏感一个恒定加速度的乘客可能没感觉但一个来回变化的加速度会让人立刻晕车。第二项是总时间 T越短越激进权重调大就会让车更果断。第三项是终点横向偏移的平方用来表达尽量回到参考线附近的偏好这个权重决定了车是倾向于压在车道中间跑还是倾向于自由游走。纵向代价结构类似把第三项换成 (s_end - s_target)²s_target 是期望的纵向终值。这里有个细节值得说在速度保持场景下s_target 其实不该硬性规定因为四次多项式本身终点位置自由。这时候更合理的做法是用终点速度偏差 (v_end - v_target)² 作为代价项让规划器自己决定跑多远。def quintic_coeffs(l0, l0d, l0dd, l1, T): d l1 - l0 c3 (20 * d - (12 * l0d 3 * l0dd * T) * T) / (2 * T ** 3) c4 (-30 * d (16 * l0d 3 * l0dd * T) * T) / (2 * T ** 4) c5 (12 * d - (6 * l0d l0dd * T) * T) / (2 * T ** 5) return l0, l0d, l0dd / 2.0, c3, c4, c5 def jerk_integral(coeffs, T, n41): ∫ l² dt用 Gauss-Legendre 之外的简单 Simpson 即可够准 t np.linspace(0, T, n) # l 6c3 24c4 t 60c5 t² j 6 * coeffs[3] 24 * coeffs[4] * t 60 * coeffs[5] * t ** 2 return float(np.trapezoid(j ** 2, t)) def lateral_cost(coeffs, T, l1, k_jerk1.0, k_time1.0, k_offset0.5): J jerk_integral(coeffs, T) return k_jerk * J k_time * T k_offset * l1 ** 2权重的整定我没有万能公式但有一个比较稳的起步组合横向 k_jerk 1.0、k_time 1.0、k_offset 0.5纵向 k_jerk 1.0、k_time 1.0、k_v 2.0。先按这个跑然后看你抱怨什么就调什么抱怨轨迹蛇形微调加 k_jerk抱怨反应迟钝、该超不超降 k_time抱怨老是不在车道中间加 k_offset。注意这几项的量纲差别很大jerk 积分的数值可能有几十上百T 只有几秒所以实际上 k_jerk 通常要设得比 k_time 小一到两个数量级别被都是 1.0误导一定要打印每一项的实际数值看一眼。4.3 采样法和数值优化两条路线各自的脾气采样法的做法是把参数空间离散化逐组求解。横向的采样维度是走完时间 T和目标横向偏移 l1纵向的采样维度是走完时间 T和目标速度 v1。取横向 T 在 2 到 5 秒之间采 10 档l1 按目标车道中心线的位置采 3 到 5 档纵向 T 在 2 到 6 秒采 10 档v1 在 0 到限速之间采 20 档。组合起来几千条候选全部用向量化的 numpy 算单帧耗时可以压到十毫秒以内。采样法的优势是三件事鲁棒因为每条候选都是解析解不会有求解失败可解释出问题的时候能直接看是哪条候选赢的、为什么赢天然处理非凸约束因为硬约束是过滤而不是约束不满足就扔掉。缺点也明确分辨率受限于算力参数空间的缝隙里可能藏着更优的解另外离散化会在参数边界产生锯齿表现为轨迹在某一档突然变激或变怂。数值优化是另一条路。把代价函数和约束直接写成一个优化问题横向纵向解耦之后横向是个二次规划可以秒解一旦把运动学约束、障碍物约束耦合进来就变成非凸问题得用 iLQR、SQP 或者序列凸化来解。它的优势是精细代价能连续优化约束能写得非常丰富。劣势是对初值敏感收敛性没保证而且最要命的是求解时间不确定——你没法保证它在某些极端场景下不会突然卡 200 毫秒。我自己的选择是混合方案采样生成候选并用硬约束过滤、软代价排序取最优那条作为初值再看时间预算决定要不要跑一轮优化做微调。在算力紧张的嵌入式平台上我干脆只用采样把 T 和 l1 的采样密度调高一点效果已经足够。真正需要优化的场景通常是低速窄路会车、极窄的空间通过这类几何约束很紧的情况。4.4 完整流程过滤、排序、平滑输出整个求解流程我总结成四步。第一步生成候选横向纵向各自采样求解做笛卡尔积。第二步硬约束过滤把不满足以下条件的全部丢掉横向加速度的绝对值不超过 3 m/s²舒适上限紧急情况可放宽到 4、纵向加速度在 -4 到 2.5 m/s² 之间、jerk 不超过 5 m/s³、纵向速度非负、S-T 图和 L-T 图检查通过、轨迹不越出道路边界。第三步软代价排序对剩下的候选算横向代价加纵向代价的加权和取最小的。第四步输出前的连续性检查把本帧最优解和上一帧最优解在重叠时间段上比一下如果差异超过阈值说明决策发生了跳变这时候宁可维持上一帧轨迹一小段时间也不要立刻切过去。def select_best(candidates, dynamics_ok, coll_free, cost_fn): best, best_cost None, float(inf) for cand in candidates: if not dynamics_ok(cand): continue if not coll_free(cand): continue c cost_fn(cand) if c best_cost: best, best_cost cand, c return best, best_cost注意硬约束过滤的顺序很影响效率。先过滤计算最便宜的加速度、速度、jerk 这些只看多项式系数的再做 S-T 图检查。我见过有人反过来写结果几千条候选全都跑了碰撞检测单帧耗时从 8 毫秒涨到 60 毫秒白烧算力。5. 工程落地模块划分与参数整定5.1 模块划分和数据流一个能跑起来的 Frenet 规划器我一般拆成五个模块。参考线管理模块负责参考线的生成、拼接、平滑和缓存对外提供按 s 取点和按位置投影两个接口。状态投影模块负责把车辆和障碍物从笛卡尔搬进 Frenet它是无状态的纯函数但调用方要负责把上一帧的 s 传进来做初值。轨迹生成模块负责采样和解析求解。轨迹评估模块负责硬约束和软代价。输出模块负责把最优轨迹从 Frenet 还原回笛卡尔交给控制。数据流是单向的参考线 → 投影 → 生成 → 评估 → 输出。不要在这一串里面搞双向依赖比如让投影模块去问评估模块要结果那会带来非常恶心的初始化顺序问题。我在一个项目里见过投影模块内部去读上一帧的最优轨迹来做时序滤波结果是第一帧永远初始化失败因为那时候还没有最优轨迹。这种耦合要坚决避免。还原回笛卡尔的公式要注意别写错给定 (s, l, θ_r)位置是参考线上 s 处的点加上 l 倍法向量方向角是 θ_r 加上 atan2(l_dot, s_dot·(1 - κ_r·l))速度是 sqrt((s_dot·(1 - κ_r·l))² l_dot²)。最后这条速度公式很容易漏掉 (1 - κ_r·l) 这个因子漏了之后在弯道上速度会偏小表现为车过弯莫名其妙变慢。5.2 关键参数整定表调参这件事我踩过多年的坑最后总结出来的原则是先调时间再调密度最后调权重。时间范围决定行为的上限采样密度决定能不能找到它权重只是在合格解里做偏好选择。参数典型取值影响调整方向横向 T 范围2.0 到 5.0 秒换道快慢嫌慢就下探到 1.5 秒横向 T 采样数8 到 12 档平顺度轨迹抖就加密横向终值 l1 采样目标车道中心 上下各一档车道内位置偏好多车道场景要覆盖全部纵向 T 范围2.0 到 6.0 秒速度响应快慢城区偏短高速偏长纵向终值 v1 采样0 到限速20 档加减速的细腻度跟车场景加密低速段规划时间步长0.1 秒与预测对齐别乱改会影响碰撞判定最大横向加速度3.0 m/s²舒适度硬边界紧急避让临时放宽到 4.0最大 jerk5.0 m/s³平顺度硬边界乘客抱怨晕车就降到 3.0安全裕度横向0.5 米侧向安全街道场景建议 0.8 米起安全裕度纵向1.5 米跟车距离高车速要按 v·0.5 增加5.3 数值稳定性几处必须做的防护第一处是分母保护。(1 - κ_r·l) 这一项在曲率大、横向偏移大的时候会趋近于零甚至为负。物理上它代表参考线在横向偏移 l 后的弧长伸缩率接近零意味着偏移后的曲线已经缩到一点了这在正常的道路几何里不该出现。所以代码里必须对它做下限截断我一般要求 κ_r·l 的绝对值小于 0.8超了就报警并把投影结果标为不可用。第二处是曲率截断。地图给的参考线曲率偶尔会出现异常大的尖峰直接把 κ_r 截断在 [-0.2, 0.2] 以内更安全因为这对应的最小转弯半径是 5 米比一般的乘用车最小转弯半径还小真实道路不可能更急。第三处是长度量纲混用。Frenet 里的 s 是弧长l 是垂直距离两者在参考线曲率不为零的地方并不等价于笛卡尔距离。做碰撞检测的时候如果直接把 l 方向的间隔当垂直距离用在弯道上会有误差误差量级大约是 l²·κ/2。当 l 是 3.5 米、曲率是 0.02 时误差约 12 厘米还能接受但当 l 到了 7 米多车道变道、曲率是 0.05 时误差就到了 1.2 米绝对不能忽略。我的做法是S-T/L-T 图只用于快速筛选通过筛选的候选一定要再做一次笛卡尔域的矩形相交检测做最终确认。6. 常见问题与排查实录6.1 问题速查表现象最可能的原因排查方法处理方式直道正常弯道贴内圈l 符号约定反了或投影点跳分支打印弯道处的 l 值看符号统一法向定义加投影初值约束轨迹一帧一跳、控制抖参考线切换未做重投影看两帧的参考线是不是同一条切换时重投影并重置横向状态过弯时速度莫名变慢笛卡尔还原时漏了 (1-κl) 因子对比 s_dot 与车辆实际速度补上因子S-T 图频繁误报冲突时间网格和预测网格不对齐打印重叠时刻的 s、l 对比统一时间步长轨迹在最后时刻急打方向终点横向速度/加速度没设为零检查五次多项式终点条件硬性设 l1 l1 0车老是压不住车道线道路边界约束没做或用错参考检查 l 的允许区间边界按 l 上下限写进硬约束单帧耗时突然飙高过滤顺序错碰撞检测跑太多打点统计各阶段耗时便宜的约束先过滤变道时车横摆横向初值没重置继承了大 l_dot打印变道瞬间的 l_dot重投影后把横向状态清零跟车距离忽远忽近纵向代价里 s_target 设置不合理看 v1 采样范围是否覆盖前车速度改用速度偏差作为代价项停车场景轨迹不平滑用了四次多项式做刹停看末位置是否精确收敛停车场景切成五次多项式6.2 我自己踩过的坑和一些不写在文档里的心得先说最惨的一次。项目里车在环岛附近会莫名其妙往环岛里面偏偏移量能到一米多。查了三天最后发现是参考线在环岛处自交投影点跳到了另一段分支上算出来的 l 直接翻了符号。这件事之后我给自己定了一条铁律任何使用 Frenet 表示的模块入口处都要做一次投影合法性检查具体做法是拿投影结果反算笛卡尔位置和原始位置比较误差超过 10 厘米就判定投影失败这一帧退回上一帧的参考线状态或者切回笛卡尔规划。这个检查只花几微秒但能挡掉一大类隐蔽 bug。第二件事是关于投影初值的。我早期版本的投影每次都从 KD-tree 全局搜索开始好处是鲁棒坏处是在参考线密集折返的场景下会跳。后来改成上一帧 s 做初值 全局搜索做兜底具体逻辑是先用上一帧 s 做 Newton 迭代如果收敛残差小于 1 毫米就采用如果不收敛或者算出来的 |l| 大于 8 米说明初值不对再退回全局搜索。这个策略我在三个项目上都用了稳定性提升非常明显。第三个心得是关于 jerk 的。很多团队调参只盯着加速度觉得加速度在舒适区间里就万事大吉实际上乘客对 jerk 的敏感度远高于加速度。我做过一次简单的实车对比两条轨迹横向加速度峰值都是 2.5 m/s²一条是平滑变化的一条是快速到达峰值再快速回落后者车上三个同事有两个明确表示不舒服。所以我的建议是jerk 权重在开发阶段就调到一个偏保守的值别等到实车反馈再说那时候改代价函数牵动的模块会多得多。第四个心得是关于采样密度的错配。横向 T 采样 10 档、纵向 T 采样 10 档、l1 采样 5 档、v1 采样 20 档笛卡尔积是 10000 条候选。这时如果你把某个维度加密到 20 档总候选数会翻倍但换来的收益可能只有一点点。我的经验是优先加密最影响体验的那个维度城区跟车场景加密 v1 的低速段高速变道场景加密横向 T。不要每个维度都无脑加密。第五个心得可能有点反直觉很多时候问题不在规划器在参考线。我曾经花了两周优化代价函数想让过弯更平顺最后发现是地图参考线在弯道处曲率不连续采样点间距在 5 米到 8 米之间跳来跳去。把参考线重采样成等间距、曲率做一次高斯平滑之后同样的代价函数过弯平顺度立刻就上来了。所以遇到任何调参数怎么都调不动的情况先去把参考线的 s 序列、曲率序列打印出来看一眼八成问题在那里。最后分享一个我在做的扩展方向把动态障碍物的交互预测和规划放在一个滚动时域里联合求解。具体做法是预测模块输出多条带概率的轨迹规划不只用最高概率那条而是把代价里加一项交互风险用预测轨迹的概率和势场加权。这样车在有人加塞的时候会表现得更像人——不是等对方完全切进来才刹车而是稍微收一点速度、往旁边让一点点。这套东西还在打磨效果不稳定但方向我感觉是对的。
