企业官网建设流程全解析

开车的朋友大概都有过这种体验：明明前面路是空的，方向盘却总在微调，一会儿往左偏一点，一会儿往右偏一点，坐车的人被晃得直皱眉。把这个场景搬到自动驾驶的轨迹规划模块里，问题会更尖锐——车不仅要走得稳，还得在几秒钟之内算出一条既不撞人、又不压线、还得让乘客舒服的路线。我做了几年路径规划相关的工作，踩过的坑基本都集中在坐标系选择和动态障碍处理这两块。Frenet 坐标系下动态街道场景的最优轨迹生成，说直白一点，就是把“我该往哪走”这件事，翻译成“沿着道路往前多少米、横向偏出多少米”这两个数字，然后在这个二维空间里搜索一条代价最小的曲线，同时把周围会动的车、人、自行车都当成随时间变化的空间占用区域，躲开它们。这个思路适用的场景非常具体：城市道路、有明确车道线或可提取中心线的结构化路段、中低速行驶（大致 0~60km/h）、周围存在动态交通参与者。如果你是做自动驾驶规控的工程师、机器人移动底盘方向的学生，或者正在把一套扫地机式的规划器往室外场景迁移，下面这些内容应该能直接拿去用。我会把数学推导里最容易翻车的地方、代码里最耗时的部分、以及那些只有真上过车才知道的调参经验都摊开讲。

1. 为什么偏偏是 Frenet 坐标系：从笛卡尔坐标的三大尴尬说起

1.1 笛卡尔坐标下的轨迹规划到底卡在哪

在笛卡尔坐标系里描述车辆，我们习惯写成 (x, y, θ, v, a)。这套表达最直观，但如果直接拿它做优化变量，问题马上就来了。第一条尴尬是参考线约束没法自然表达。车辆必须沿着道路走，这是一个沿路方向的强约束，而在笛卡尔坐标里，“沿着道路”只能用一系列位置点或者复杂的曲线不等式来近似，优化器处理起来非常别扭。第二条尴尬是横向和纵向的运动耦合在一起：你在 x 方向上调整一个值，y 方向的可行性也跟着变，曲率约束、道路边界约束全都拧在一块，求解复杂度直接上一个台阶。第三条尴尬是障碍物的表达。一个行驶中的前车，在笛卡尔坐标里是一个随时间移动的多边形加上一段预测轨迹，你很难用一个简洁的代数形式把它写成约束。

我做第一个版本规划器的时候，就是用笛卡尔坐标硬做，结果发现每加一个约束，求解时间就往上涨，最后在一个十字路口场景里直接超时。后来换成 Frenet 坐标，同一台机器上求解时间掉了将近一半，而且代码可读性好了非常多。这不是说 Frenet 万能，而是它把问题放到了“自然的坐标系”里。

1.2 Frenet 坐标系和参考线的关系，一句话讲透

网上关于“frenet 坐标系和参考线的关系”的讨论很多，我自己的理解是：参考线提供了一把尺子，Frenet 坐标系是这把尺子上的刻度。

具体来说，参考线是一串按行驶方向排好序的离散点，每个点包含 (x, y, θ_r, κ_r, s)，其中 s 是从起点算起的弧长，θ_r 是切向角，κ_r 是曲率。有了这条线，空间中任意一点都可以用两个量描述：s，也就是这一点在参考线上投影位置的弧长；l，也就是这一点到参考线的横向偏移，向左为正、向右为负（正负约定要统一，否则后面全是 bug）。也就是说，Frenet 坐标把二维平面“拉直”成了一条带子：纵向是沿着路往前延伸，横向是垂直于路的方向。

这个变换带来的最大好处是解耦。道路边界通常可以写成 l ≤ l_max 和 l ≥ l_min 的简单不等式，而且 l_max、l_min 只随 s 变化，不随其他东西变。车道保持就是让 l 尽量接近 0 或某个车道中心；变道就是让 l 从当前值平滑过渡到目标车道中心。这些在 Frenet 空间里都是最自然的表达。同理，纵向的速度控制、跟车距离控制，全都在 s 方向上单独处理。

1.3 什么场景该用、什么场景别硬上

Frenet 坐标系有一个前提：存在一条合理的参考线。城市结构化道路、高速公路、封闭园区道路，这些场景车道线清晰，参考线提取很稳定，用起来非常舒服。但如果是在无标线的停车场、野外越野、完全开放的空地，参考线本身就很难提取，或者提取出来的线频繁跳变，这时候硬用 Frenet 反而会引入噪声。我的经验是，参考线的横向抖动超过 0.3 米、或者曲率符号频繁翻转的路段，要先做参考线平滑和重采样，再进规划，否则规划器会一直输出左右摆动的轨迹。

还有一个容易忽略的点：参考线是“静态”的，但我们的规划是分周期滚动的。每个规划周期（通常 100ms）都要重新提取一次参考线，如果两次提取结果差异很大，轨迹就会跳。所以工程上一般会把参考线做一次缓存和平滑，只在偏离超过阈值时才更新，这个细节后面还会展开讲。

2. 核心数学细节：坐标转换、参数化与曲线构造

2.1 参考线的离散化与弧长参数化

参考线的质量决定了后面一切。原始输入可能是高精地图的车道中心线，也可能是感知出来的车道线拟合结果，点间隔可能不均匀。第一步必须做重采样，按固定间隔（我常用 0.5 米）插值出新的点列，并保证 s 单调递增，这样后面做最近点查找和插值才稳。

重采样之后要算每点的三个量：切向角 θ_r、曲率 κ_r、弧长 s。

• θ_r 用相邻点差分：θ_r[i] = atan2(y[i+1] - y[i-1], x[i+1] - x[i-1])，用中心差分比前向差分稳定得多。
• κ_r 用切向角的变化率除以弧长：κ_r[i] = (θ_r[i+1] - θ_r[i-1]) / (s[i+1] - s[i-1])。这里要注意角度要解卷绕（unwrap），否则在 ±π 附近会出现巨大的假曲率，我第一版就栽在这，导致车辆在接近直线段时突然“发抖”。
• s 就是逐点累加欧氏距离。

注意：曲率一定要做一次低通滤波或滑动平均。原始差分出来的曲率噪声很大，直接拿去算横向加速度会导致轨迹抖动。

2.2 笛卡尔与 Frenet 互转：公式、近似和真实工程做法

从笛卡尔转到 Frenet，核心是找最近点。常见做法是先按粗略的 s 定位（比如用 GPS 或者上一周期的结果做初值），然后在附近几个点上找距离最小的点，再在相邻两点之间做投影。投影的方式是把待转换点到线段两端点的连线，算垂足位置，判断垂足是否落在线段内，落在里面就取垂足，否则取端点。

找到投影点之后，位置分量很简单：

• l = (x - x_r) * (-sin θ_r) + (y - y_r) * cos θ_r，也就是点到参考线的向量在法向上的投影。
• s = s_r + 投影长度。

麻烦的是速度和加速度的转换。设车辆速度为 v、航向角为 θ，参考线切向角为 θ_r，横向偏移为 l，曲率为 κ，定义航向偏差 θ_e = θ - θ_r，那么有关系：

• ṡ = v * cos θ_e / (1 - κ * l)
• l̇ = v * sin θ_e

这两个式子很重要，因为它解释了为什么高速过弯时横向偏移会“放大”纵向速度。当 κl 接近 1 的时候，分母趋近 0，ṡ 会爆炸。物理含义是：当你的横向偏移等于曲率半径时，实际上你已经绕到了曲率中心，坐标系退化了。工程上必须给 1 - κl 设一个下限，比如 0.2，低于这个值直接判定该点无效，丢弃这个候选。我见过不止一个项目在这里没做保护，结果在急弯处规划器输出了一个 NaN，整条轨迹作废。

加速度转换会更长，完整解析式里包含曲率导数 κ' 的修正项。我的实际做法是：如果曲率变化平缓（相邻点曲率差很小），可以用忽略 κ' 的简化式；如果要精细，我更倾向于不手推解析式，而是在 Frenet 空间里直接对 s 和 l 的序列做数值差分得到 ṡ、l̇、s̈、l̈，然后做一次滑动平均或卡尔曼滤波去噪。这样做的好处是代码短、不容易出错，代价是差分会放大噪声。实测下来，在规划周期 100ms、参考线间隔 0.5 米的条件下，数值差分加五点平滑的效果足够用，横向加速度的误差大概在 0.1 m/s² 量级，比手推公式写错了要好得多。

反方向转换（Frenet 转笛卡尔）相对简单：

x = x_r - l * sin θ_r，y = y_r + l * cos θ_r

做轨迹输出的时候，一定要用这个式子把 (s, l) 序列还原成 (x, y) 序列，然后再做碰撞检测和可视化。我建议在这里加一个自检：把还原后的点再转回 Frenet，看 s、l 的误差是否在 1e-3 以内，超过就报警，这能帮你快速发现 θ_r 符号或索引错位的 bug。

2.3 横向纵向解耦后，为什么用多项式而不是样条

解耦之后，横向轨迹通常写成 l(s) 的函数，纵向写成 s(t) 的函数。选多项式而不是贝塞尔或 B 样条，主要原因是边界条件可以精确满足。五次多项式有六个系数，正好可以同时约束起点和终点的位置、一阶导、二阶导。对于横向运动，起点是当前 l、l̇、l̈，终点是目标 l_target、0、0，五点或六点边界条件刚好凑齐。

横向五次多项式的一般形式：

l(s) = a0 + a1 s + a2 s² + a3 s³ + a4 s⁴ + a5 s⁵

给定 s 从 0 到 Δs，起点 (l0, l0', l0'')，终点 (l1, l1', l1'')，可以解出唯一的系数向量。这个解在数学上就是横向最小 jerk 问题的最优解，代价函数 ∫ (l''')² ds 最小，所以采出来的轨迹天然平滑。这也是为什么这个框架被叫做“最优轨迹生成”——在给定边界条件下，它确实是最优的，只是最优性建立在横向和纵向可以分开处理这个假设上。

纵向通常用四次多项式 s(t)，因为速度的边界条件比横向少一个（一般不需要约束 s 的二阶导），四次就够。但如果你希望加加速度连续（jerk 连续），就用五次。巡航、跟车、超车、停车这几种模式，本质上就是给终点状态赋不同的值，然后用同一套多项式求解器算出来。

提示：采样时不要只采终点位置，要采终点状态的组合。只采位置会得到一堆形状相似、但速度不匹配的轨迹，最后评价函数只能靠硬凑权重来区分，很难调。

3. 动态街道场景建模：把会动的东西变成可计算的约束

3.1 动静态障碍物在 Frenet 空间里的投影

静态障碍物好办，把它投影到 (s, l) 平面就是一个矩形或一组离散点，规划时候直接检查候选轨迹是否进入这个区域就行。真正麻烦的是动态障碍物，因为它们的位置随时间变化。

主流做法是把动态障碍物的预测轨迹投影到 s-t 平面，形成“时空占用矩形”。具体来说，对某个障碍物，它的预测轨迹是一系列 (t, x, y, v, θ) 的点，把每个点转成 (t, s, l)，然后在 s-t 平面上，对于每个时刻 t，障碍物在 s 方向占据的区间是 [s_center - L/2 - margin, s_center + L/2 + margin]，其中 L 是障碍物沿参考线方向的长度。把所有时刻的区间连起来，就得到一个随时间变化的 s 区间带。规划出来的轨迹是一条 s(t) 曲线，只要它在任意时刻都不落在某个障碍物的 s 区间内，就说明纵向没有冲突。

横向同理，可以在 l-s 平面上投影。这样做的好处是把三维问题（x, y, t）拆成两个二维问题分别检查，计算量大幅下降。代价是丢了圆形障碍物在斜向的相对关系，所以最后一定要用真正的多边形碰撞检测做一次复核，二维投影只用于快速剪枝。

3.2 动态障碍物的不确定性怎么处理

预测永远不准。一个行人可能站着不动，也可能突然迈步；前车可能匀速，也可能急刹。工程上常见的有三种处理方式：

第一种是膨胀法。直接把障碍物的尺寸在预测基础上膨胀一圈，比如横向加 0.5 米、纵向加 1.0 米，把不确定性吃掉。简单粗暴，但会损失通行空间，窄路会车的时候经常导致规划器找不到可行解。

第二种是多模态假设。给每个障碍物生成多条可能的预测轨迹（直行、左偏、右偏、减速、加速），然后对每一种组合做一次规划，取最好的。这样通行能力强，但组合数会爆炸。实践中一般只对最危险的 1~2 个障碍物做多模态，其余还是单模态膨胀。

第三种是概率约束。用高斯过程或者卡尔曼滤波给出预测的协方差，把碰撞概率控制在某个阈值以内。这个理论上漂亮，但工程落地时协方差很难标定得准，最后往往还是会退化成定值膨胀。

我自己的做法是混合：对行人、自行车这类高机动性目标，用较大的横向膨胀加上多模态；对四轮车，用较小的膨胀加单模态，但把跟车距离调大一些。实测下来这个组合在城区道路上比较平衡，既不会频繁卡死，也不会太激进。

3.3 约束怎么写成优化问题的一部分

把上面这些东西组织成约束，大致是这样的一个结构：

• 道路边界约束：l_min(s) ≤ l(s) ≤ l_max(s)，逐点检查。
• 曲率约束：|κ| ≤ κ_max，由横向加速度反推，κ_max = a_lat_max / v²。注意 v 在变，所以曲率约束其实是速度相关的。
• 碰撞约束：候选轨迹的时空占用与障碍物时空占用不相交。
• 动力学约束：横向加速度、纵向加速度、jerk 都在车辆能力范围内。
• 运动学约束：对于非完整约束车辆，轨迹的曲率要与航向变化一致，这个在横向五次多项式里天然满足，因为 l(s) 的二阶导和曲率有解析关系。

其中曲率到横向加速度的换算要特别注意符号。横向加速度 a_lat = v² * κ，其中 κ 是轨迹曲率，不是参考线曲率。轨迹曲率 κ_traj 与 l(s) 的关系在参考线曲率较小时可以近似为 κ_traj ≈ l''，但当参考线本身弯的时候，需要加上参考线曲率项。我一般用 κ_traj ≈ κ_r + l'' / (1 + l'²)^1.5 这个形式，l' 很小时第二项就退化成 l''。

注意：横向加速度上限别直接取 3.0 m/s²。乘客舒适性阈值通常在 1.5~2.0 m/s²，超过 2.5 m/s² 大部分人就会明显感到被甩。紧急避障可以放宽，但要用单独的代价惩罚项标记出来，方便后续复盘。

4. 最优轨迹生成实操流程：从采样到落地

4.1 采样-评价框架怎么搭

这套框架的骨架其实很朴素：生成一批候选轨迹，逐条算代价，取最小代价的那条，再做一次平滑输出。难点全在“生成”和“评价”两个环节的细节上。

纵向采样我一般这样设计。先根据当前状态和场景决定一个目标速度 v_target，比如跟车时取前车速度、巡航时取限速、接近红绿灯时取减速曲线。然后围绕 v_target 上下各采 2~3 个值，形成速度候选集。同时采几个不同的到达时间 t_arrival，因为同样的终点位置配不同时间，得到的速度曲线形状完全不同。我的经验是速度采 ±20% 分三档、时间采 ±15% 分三档比较合适，再细就只是增加计算量，收益很小。

横向采样用终点状态的方式更高效。不是把 l 的每个值都试一遍，而是采一组 (l_target, l_target', l_target'')。常见的几类：

这样每类意图下只要采 1~3 个 Δs，总共也就十几条横向轨迹。纵向十几条乘以横向十几条，两百条左右的候选项，在一般的车载算力上做碰撞检测和评价，100ms 周期内足够跑完。

4.2 代价函数怎么设计才不会互相打架

代价函数是这套框架里最玄学的部分。我的建议是分层设计，避免所有项平铺在一起然后互相拉扯。

第一层是硬约束，不满足就直接淘汰，比如越界、碰撞、超过加速度极限。这一层用布尔判断，不进入代价。

第二层是安全性代价，包括与障碍物的距离、与前车的时距。距离项用反比例或者指数衰减，保证离得越近代价涨得越快，形成一个软墙。我常用 J_obs = w_obs * exp(-d / d0)，d0 取 2~3 米。

第三层是舒适性代价，主要是横向加速度平方积分、纵向加速度平方积分、jerk 平方积分三项。这三项的量纲不一样，需要归一化。我的做法是用各自的典型最大值做分母，把每一项压到 0~1 之间，再加权。

第四层是效率代价，包括速度偏差和时间。J_v = w_v * (v - v_target)²，J_t = w_t * t_arrival。

权重标定我推荐从一个大到小的顺序调：先只留硬约束和碰撞代价，确认能安全跑；再加舒适性，把横向加速度压下来；最后加效率。一上来就把所有项全开，你根本分不清是哪一项导致的行为异常。我给一组起步值参考：w_obs = 10, w_lat_acc = 1.0, w_lon_acc = 0.5, w_jerk = 0.8, w_l = 0.3, w_v = 0.5, w_t = 0.1。这是量级参考，实际要按你的归一化方式缩放。

提示：代价函数里保留一个“调试项”，比如把每条候选轨迹的各项代价都记录到日志里。上线前分析行为异常时，能直接把日志拉出来看是哪一项在主导，比在车上盲调强太多。

4.3 碰撞检测与轨迹平滑的最后一道关

候选轨迹做碰撞检测，一定要考虑时间维度。把自车按时间步长（比如 0.1 秒）展开成一系列位姿，每个位姿用一个矩形或者三个圆来近似，然后检查每一帧是否与障碍物的预测位姿重叠。

用三个圆近似车体的做法很常见：前圆、中圆、后圆，半径取车宽的一半加安全裕度。这样做的好处是碰撞检测退化成圆与圆的距离比较，速度极快。缺点是车头车尾的角落会有“切角”误差，所以安全裕度要给足一点，一般 0.2~0.3 米。

矩形的话可以用分离轴定理（SAT），精度高但慢一些。我的经验是先用圆做粗筛，对通过粗筛的轨迹再用 SAT 精算，能省下大量时间。

碰撞检测通过之后，还有一步平滑。多项式采样的轨迹虽然平滑，但不同周期之间选中的轨迹可能在连接处有跳变，表现为方向盘的小幅抖动。常见的处理是在输出前做一次时间上的插值平滑，比如用上一周期的轨迹和新轨迹做加权融合，融合系数取 0.2~0.3。但要注意，平滑不能破坏碰撞检测的结论，所以融合之后的轨迹必须重新做一次碰撞检测，这一步绝对不能省。我见过为了省时间跳过后检导致的擦碰事故，教训很直接。

5. 常见问题与排查技巧实录

5.1 典型 Bug 速查表

这套框架实现下来，出问题的地方高度集中在几个位置。我把踩过的典型问题整理成表，出问题时可以对着查。

这张表里的每一条我基本都真实遇到过。最坑的是第一条和最后一条，一个导致行为诡异但难复现，一个导致偶发超时，都很难查。

5.2 曲率与横向偏移的那几个数值陷阱

回到前面提到的分母问题，这里展开讲。1 - κ*l 这个因子在整个框架里出现频率极高，横向速度、纵向速度、加速度转换都要用。它出问题的方式有两种：一种是直接等于零或负数，导致方向反转；另一种是接近零但不为零，导致数值巨大。

我的处理方式是在参考线构建阶段就算出每个点的 κ 和 s 方向上的最大允许横向偏移，取 l_limit = 0.9 / max(|κ|, 1e-3)。规划时，候选轨迹的 l 值如果超过这个限制，直接淘汰。这个限制比道路边界往往更紧，所以能提前发现坐标系退化区域。

还有一个陷阱是曲率的符号约定。参考线曲率如果定义成“左转为正”，那么横向偏移也必须是“左正右负”，否则轨迹曲率算出来符号全反。我在两个项目里见过不同的约定，混用一次，结果是车辆在弯道里往外冲。所以接手任何一套代码，第一件事就是找这两个约定的定义，写一行注释钉死在文件头上。

5.3 参数标定与验证该怎么下手

参数标定别一上来就车调，先在离线回放里做。把一段真实的路测数据（包含感知结果、定位结果）录下来，然后让规划器离线跑，把输出轨迹和实际行驶轨迹叠在一起看。这样能快速筛掉大部分明显不合理的参数组合。

离线验证我一般看几个指标：横向加速度的 95 分位值、纵向加速度的 95 分位值、jerk 的最大值、最小障碍物距离、规划失败率、单周期计算耗时。前三个反映舒适性，第四个反映安全性，后两个反映可用性。这几个指标一起看，基本能判断一组参数能不能用。

上车之后先做低速场景（20km/h 以内），确认基本行为正确，再逐步提速。城区的挑战主要来自两点：一是路口内没有车道线，参考线需要靠推理生成；二是行人和非机动车的预测非常不准。我的做法是在路口区域把横向膨胀临时放大 30%，同时把速度上限压低，宁可慢一点也别冒险。

注意：每次改参数，一定要记录改之前之后的几个关键指标。凭感觉调参最后会陷入“改了一个问题冒出三个问题”的循环，有数据对照才走得出来。

6. 从跑通到好用：几个个人体会

第一点体会是关于代码结构的。这套框架里，坐标转换、参考线管理、采样器、评价器、碰撞检测，这五个模块一定要彻底分开。我早期版本把它们耦合在一起，结果每次调参都要翻遍整个文件。拆开之后，每个模块单独写单元测试，坐标转换用几组手工算好的数据验证，碰撞检测用构造的矩形对验证，出问题时定位快得多。

第二点体会是关于“最优”这个词的。数学意义上的最优轨迹，在真实场景里往往不是最舒服的那条。因为评价函数里的权重是我们主观设的，而乘客的感受又很难量化。我的做法是保留一个 Pareto 前沿的概念：不追求单条最优，而是在安全性代价接近的候选里，选舒适性最好的那条。这样做出来的车，行为风格会更接近人类司机，不会有那种“精确但生硬”的感觉。

第三点体会是关于失败兜底的。再好的规划器都会有找不到可行解的时候，这时候必须有一条确定安全的兜底轨迹，通常是“保持当前横向位置、以可控减速度减速到停”。这条轨迹不参与评价，直接由规划器外部注入。千万不要让规划器在无解时输出上一周期的轨迹继续跑，因为上一周期的轨迹是针对上一时刻的障碍物算的，可能已经不安全了。

最后聊一个后续可以做扩展的方向：把这条采样-评价的流水线换成基于优化的方法，比如把采样得到的次优解作为二次规划的热启动，用 QP 在连续空间里精修。这样能得到更平滑的结果，同时因为有采样解兜底，求解失败的频率也会低很多。我自己在小范围试过，横向加速度的波动能再降一档，代价是计算时间增加，需要看具体平台的算力预算。