自动驾驶规划控制决策知识点扫盲

局部规划采用横纵向分开规划（路径-速度解耦）

局部规划采用横纵向分开规划在自动驾驶规划领域（Planning），这种做法通常被称为 "路径-速度解耦" (Path-Velocity Decomposition)。

简单来说，就是把"怎么走"（路径/横向）和"开多快"（速度/纵向）分成两步来算，而不是混在一起算。这是目前工业界（包括百度 Apollo、Autoware 等）最主流的规划架构。

一、 什么是"横纵向分开规划"？

想象你在开车过弯：

1. 分开规划（解耦）：先盯着路面，画出一条完美的过弯曲线（路径规划）；然后再思考在这条线上每一秒该开多少码（速度规划）。

2. 联合规划（耦合）：大脑里直接生成一个 x,y,t 的三维管道，同时决定位置和时间。

二、 优点 (Pros)

对于刚入职的你来说，你会发现大部分量产车都采用这种方案，原因如下：

1. 降维打击，计算速度快 (Computational Efficiency)

• 原理：规划问题本质上是一个高维空间的数学优化问题。

  • 如果是联合规划，我们要解一个 x,y,t (甚至包含 θ,v,a) 的高维方程，搜索空间巨大，很难在 100ms (10Hz) 内算出最优解。

  • 如果是分开规划，我们先解一个 2D 路径问题 (S-L)，再解一个 1D 速度问题 (S-T)。

• 好处 ：把一个超难的题目拆成了两个中等难度的题目，计算量呈指数级下降，能满足实时性要求。

2. 鲁棒性强，不易"死机" (Robustness)

• 场景：如果求解器在一个大坑里找最优解（非凸优化），很容易陷入局部最优或者根本算不出来（No Solution）。

• 好处 ：拆分后，每一层（路径层、速度层）通常都可以转化为 凸优化 (Convex Optimization) 或者 二次规划 (QP) 问题。这种数学形式保证了：只要有解，我就能稳定地算出来，而且算得很快。

3. 便于调试及问题定位 (Easier Debugging)

这是对工程师最友好的地方。

• 现象：车子开得像醉酒（画龙）。

• 定位：如果是解耦架构，你可以直接看路径规划出的线直不直。

  • 如果线是直的，车还在晃，那是控制层的问题。

  • 如果线本身就是弯的，那是路径规划层的问题。

• 对比：如果是联合规划，路径和速度混在一起，你很难判断是因为追求速度导致路径歪了，还是因为避障导致速度抖动。

三、 缺点 (Cons)

虽然它好用，但它不是完美的，特别是在极限场景下：

1. 局部最优，非全局最优 (Sub-optimality)

• 问题 ：因为你是"先画路，再定速"，路径规划器在画路的时候，不知道后来速度规划器能不能跑得通。

• 案例：

  • 路径层：前面有个障碍物，规划了一条非常急的变道曲线（曲率很大），觉得这样绕过去路径最短。

  • 速度层：拿到这条急弯路径傻眼了。因为离心力限制（F=mv2/r），为了不翻车，速度规划器只能把速度降到极低（比如 5km/h）才能过这个弯。

  • 结果：车子在一个高速路上突然急刹车慢慢扭过去。

  • 理想情况：如果是联合规划，可能会选择"稍微刹一点车，但是变道幅度小一点"的折中方案，整体通行效率更高。

2. 难以处理高动态交互 (Dynamic Obstacles)

• 场景：你要汇入主路，主路上车速很快。

• 问题：

  • 路径规划通常是在静态地图（或者低速假设）下生成的几何形状。

  • 当你面对高速切入的旁车时，最优的策略往往是"在这个时刻抢在它前面切入"或者"减速让它先过"。这种时间（T）和空间（S）强耦合的博弈，在分开规划里很难完美表达。

  • 解耦规划容易出现：路径层生成了切入轨迹，速度层发现会撞车，于是急刹放弃，导致车子在路口"犹豫不决"。

3. 动力学约束的丢失 (Loss of Dynamic Constraints)

• 原理：汽车的抓地力是有限的（摩擦圆）。横向占用的摩擦力多了，纵向能用的就少了。

• 缺陷 ：分开规划时，路径规划往往假设横向控制能完美执行，忽略了当时可能正在重刹（纵向占满了摩擦力），导致生成的路径在物理上根本开不出来，造成控制层跟不住轨迹。

四、 总结：

1. 大部分量产场景（高速巡航、跟车、低速避障）： 横纵向解耦（分开规划） 是绝对的王者。因为它稳、快、好调。

2. 极端场景（高速紧急避让、赛车控制）： 需要考虑 联合规划 或者基于 MPC（模型预测控制） 的规划方法，因为这时候路径和速度必须同时考虑物理极限。

团队现状： 目前主流的 L2/L2+ 辅助驾驶，90% 都是采用 EM Planner （Expectation-Maximization，百度 Apollo 的方案）或者类似的变种，这正是经典的先路径、后速度的解耦架构。

---

横纵向控制与避障

一、 纵向控制 (Longitudinal Control)

纵向控制管理车辆在前进方向上的运动。

简单说就是控制 速度（Speed） 和 位置（Distance）。

它决定了车辆是加速、匀速巡航，还是刹车。

速度跟踪：大脑说"开 30km/h"，脚就要把速度稳在 30km/h，不能超速也不能太慢。

位置跟踪：大脑说"停在前面停车线"，脚就要精准地在停车线前刹停，不能冲出去。

2. 常用算法

• PID 控制：最经典、最常用。

  • P (比例)：离目标速度差得越远，油门踩得越深。

  • I (积分)：用来消除稳态误差（比如上坡时，只靠P油门不够，速度上不去，I会慢慢把油门加上去）。

  • D (微分)：预测趋势，防止超调（比如快到目标速度了，提前松油门，防止冲过头）。

• MPC (模型预测控制)：进阶版。它会往后"多看几步"，考虑到车辆的物理限制（比如发动机最大扭矩、舒适度要求），计算出未来一串最优的油门指令。

---

二、 横向控制 (Lateral Control)

横向控制管理车辆的转向。它负责控制 方向盘角度（Steering Angle），让车沿着规划好的线走。

1. 核心目标

• 减小横向误差 (Cross-track Error, CTE)：车离中心线的垂直距离越小越好。

• 减小航向误差 (Heading Error)：车头的朝向要和道路走向一致。

2. 常用算法

• 纯追踪 (Pure Pursuit)：

  • 原理：像人眼看路一样。在规划好的路径上，向前看一段距离（预瞄距离 Ld），找一个目标点，然后把方向盘打死对准那个点开过去。

  • 特点：简单、鲁棒性好。但预瞄距离选不好容易画龙（震荡）或切弯（内切）。

  • 适用：低速场景、AGV小车。

• Stanley 算法 (前轮反馈控制)：

  • 原理：基于前轮中心的几何关系。它同时考虑了"我离线有多远（横向误差）"和"我车头歪了多少（航向误差）"。

  • 特点：收敛速度快，适合倒车入库等场景。

• LQR (线性二次调节器)：

  • 原理：基于车辆动力学模型。它通过解一个优化方程（Riccati方程），找到一组最优的控制参数，让系统的误差能量最小。

  • 特点：高速行驶非常稳，是百度 Apollo 等开源框架中的主流横向控制算法。

• MPC (横向)：

  • 同样是预测未来，能处理车辆的非线性约束（比如方向盘转角不能超过 450度）。算力消耗大，但在复杂工况下表现最好。

---

三、 纵向避障 (Longitudinal Obstacle Avoidance)

纵向避障通常发生在跟车 或遇到静止障碍物 时。这通常是规划层（Planning）**先做决策，然后**控制层执行。

1. 场景

• 前车急刹。

• 前方有红灯或斑马线行人。

• 路上有个大石头挡住了路，但旁边不能变道。

2. 策略逻辑

• ACC (自适应巡航)：保持安全时距（Time Headway）。如果你跟前车距离缩短，规划模块会生成一个减速的速度曲线。

• AEB (自动紧急制动) ：这是最后一道防线。如果计算出 TTC (Time To Collision, 碰撞时间) 小于某个阈值（比如 2秒），不经过规划层，直接底层锁死刹车。

• 虚拟墙 (Virtual Wall)：在规划层面，会在障碍物后面立一道虚拟的墙，要求车辆的速度在到达墙之前降为 0。

---

四、 横向避障 (Lateral Obstacle Avoidance)

横向避障是指绕行（Nudging）**或**变道（Lane Change）。

1. 场景

• 路边违停了一辆车，占了一半车道。

• 邻车道有大货车压线行驶。

2. 核心技术：Frenet 坐标系 (SL 坐标系)

这是规控工程师必须掌握的概念。为了方便计算，我们会把弯弯曲曲的道路"拉直"，建立一个坐标系：

• S轴 (纵向)：沿着道路中心线的距离。

• L轴 (横向)：偏离道路中心线的距离（左正右负）。

3. 避障策略

• 微避 (Nudging)：

  • 规划器发现前方 S=50m,L=−1m 处有个障碍物。

  • 原路径是 L=0（沿中心线走）。

  • 规划器生成一条新曲线，在 S=50m 时让 L 变成 1.5m（向左偏移），绕过障碍物后再回到 L=0。

  • 控制层接收到这条弯曲的轨迹，通过 LQR 或 MPC 算法控制方向盘去跟踪它。

---

总结：数据流是怎么走的？

为了让你更清楚工作的上下文，整个闭环是这样的：

1. 感知 (Perception)：看到前面 30米 有个石头。

2. 决策 (Behavior)：决定"我要绕过去"（不是刹停）。

3. 规划 (Planning)：

   • 在 Frenet 坐标系下，画出一条向左绕行的平滑曲线（S-L图）。

   • 同时规划好在这个曲线上的速度（S-T图）。

4. 控制 (Control)：

   • 横向：用 PP 算方向盘转角，紧贴那条绕行曲线。

   • 纵向：用 PID 算油门/刹车，保持规划要求的速度。

---

Frenet 坐标系

一、 为什么要发明 Frenet 坐标系？

在自动驾驶中，我们最容易获得的定位信息是 笛卡尔坐标系 (Cartesian / Global Frame)，也就是 (x, y) 坐标

但如果直接用 (x, y) 来做路径规划，会非常痛苦：

1. 道路是弯的：如果你在一个弯道上行驶，虽然你在"顺着路走"，但你的 x 和 y 坐标都在剧烈变化，很难用数学公式描述"我在直行"。

2. 很难描述"左/右"：在弯道里，判断一个障碍物是在你左边还是右边，通过 (x, y) 计算非常复杂。

Frenet 坐标系的诞生，就是为了把"弯曲的道路"在数学上"拉直"。

---

二、 Frenet 坐标系是怎么定义的？

它基于一条参考线 (Reference Line)。这条线通常是地图中心线或者上一帧规划好的轨迹。在这个坐标系里，只有两个轴：

1. S 轴 (Station / Longitudinal) ：纵向距离。

   • 表示你沿着这条路（参考线）走了多远。

   • S=0 是起点，S=50 就是沿路走了 50 米。

2. L 轴 (Lateral) ：横向偏差。

   • 表示你离参考线垂直距离有多远。

   • 通常规定：参考线左侧为正 (+)，右侧为负 (-)。

   • L=0 表示正好压在中心线上。

---

三、 案例：S-L 图解避障流程

现在我们要处理在 S=50m, L=-1m 的障碍物。

1. 感知输入与坐标转换

感知模块告诉你障碍物在世界坐标 (x_obs,y_obs)。 规划模块首先把它投影到参考线上，计算出它的 Frenet 坐标：

• 障碍物位置：(S=50,L=−1)。

• 含义 ：在前方 50 米处，道路中心线右侧 1 米的地方有个东西。

2. 规划决策 (Path Planning)

规划器思考：

• 当前状态：我在 (S=0,L=0)。

• 前方情况：S=50 处右边有障碍。

• 策略：为了安全，我要往左躲。目标设定为在 S=50 时，到达 L=1.5（中心线左侧 1.5 米）。

3. 生成曲线 (S-L Graph)

规划器不会画折线（直接瞬移），而是会画一条平滑的 五次多项式曲线 (Quintic Polynomial)。

在 S-L 坐标系 图上，这条轨迹长这样：

• S=0 ~ 20m：L 保持在 0（直行）。

• S=20 ~ 50m：L 逐渐从 0 增加到 1.5（平滑向左变道）。

• S=50m：L 达到 1.5（通过障碍物旁边，此时横向安全距离为 1.5−(−1)=2.5m，非常安全）。

• S=50 ~ 80m：L 逐渐从 1.5 减小回 0（绕过障碍物后，平滑回归车道中心）。

这一步的产出：是一系列的 (s,l) 点。例如：

• (s=10,l=0)

• (s=30,l=0.5)

• (s=40,l=1.2)

• (s=50,l=1.5)

---

四、 从规划到控制 (Planning to Control)

现在规划器生成了这条漂亮的曲线，控制层（Control）怎么去执行呢？这里有一个关键的逆转换步骤。

1. 坐标回转 (Frenet -> Cartesian)

方向盘和油门是不认识 S 和 L 的。 在发给控制层之前，规划器会将上面生成的 (s,l) 轨迹点，结合参考线的几何形状，转换回世界坐标 (x,y) 以及对应的航向角 θ 和曲率 κ。

2. 控制器跟踪 (Tracking)

现在，控制层拿到了包含 (x,y,v,θ) 的轨迹点列表。

• MPC / LQR 的工作：

  • 控制器会看："我现在在哪（实际位置）？" vs "我应该在哪（规划位置）？"

  • 横向误差 (CTE)：即使规划让我往左偏 (L=1.5)，如果我实际只偏了 1.0，那么误差就是 0.5，我要把方向盘往左打。

  • 航向误差 (Heading Error)：规划的轨迹是斜着出去的，如果我的车头是正的，我就要打方向盘对准那个斜的轨迹。

总结这个过程：

1. Frenet (S-L) 帮规划器把复杂的"弯道避障"简化成了简单的"左右移动数学题"。

2. 规划器算出一道完美的数学曲线。

3. 再把曲线转回 世界坐标。

4. 控制算法 疯狂操作方向盘，死死咬住这条虚拟的曲线，最终车子就在现实世界中画出了一条漂亮的绕行轨迹。

---

轨迹边界：

在自动驾驶规划（Planning）中，"轨迹边界"（Trajectory Boundary） ，也被称为**"可行行驶走廊"（Drivable Corridor）** 或**"凸空间"（Convex Space）**。它是一个"安全活动范围"**。规划器的核心任务就是在这个"安全活动范围"里画出一条最优的线。

1. 形象理解：

想象你在玩赛车游戏：

• 地图边界（Lanelet2） ：是赛道的护栏（静态边界）。

• 感知障碍物 ：是赛道中间的香蕉皮或大石头（动态边界）。

• 轨迹边界 ：就是扣除掉 香蕉皮和大石头周围的危险区域后，你可以安全行驶的沥青路面范围。

2. 轨迹边界是如何生成的？

1. 第一刀：切出静态范围 (Static Bounds)

   • 利用 Lanelet2 地图，获取当前车道的左边界（Left Bound）和右边界（Right Bound）。

   • 例子：车道宽 3.5米，以中心线为基准，L 的范围是 [-1.75m, +1.75m]。

2. 第二刀：挖掉障碍物 (Dynamic Bounds)

   • 利用 感知障碍物 信息。如果前方 S=50m 处，右侧 L=-1m 有个障碍物。

   • 为了避障，我们不能在这个位置继续使用 [-1.75, +1.75] 的范围。

   • 我们需要把障碍物及其周边的**安全缓冲（Buffer）**挖掉。

   • 修改后：在 S=50m 处，边界可能变成了 [0.5m, 1.75m]（强迫车辆往左走）。

3. 数学表达：S-L 走廊

在算法层面（通常在 Frenet 坐标系下），轨迹边界是一系列基于 S（纵向距离）的 L（横向）约束对。

它通常长这样：

纵向距离 S (米)	右边界 Lmin	左边界 Lmax	备注
0	-1.75	1.75	起点，车道正常
...	...	...	...
40	-1.75	1.75	接近障碍物
50	+0.5	1.75	遇到障碍物，右边界被迫收缩（变道/绕行）
60	-1.75	1.75	越过障碍物，恢复正常

4. 轨迹边界有何用？

这就是路径规划（Path Planning） 的核心技巧------将避障问题转化为约束问题。

• 输入前：规划算法（如 QP 二次规划或 MPC）原本想画一条直线（参考轨迹）。

• 输入后 ：你给了它这个"修改后的边界"。你告诉算法："你要画什么样的线我不管，但你必须保证线上的每一个点，都落在 L*{min} 和 L*{max} 之间。"

• 结果 ：算法为了满足这个新的边界约束，被迫计算出一条弯曲的线，从而自动实现了避障 或换道的功能。

---

横向规划：

基于优化的方法：

在基于优化的规划方法中，我们将"如何开车"这个问题转化为了一个"解数学题"的问题。

• 题目 ：在给定的轨迹边界（管道）内，画出一条最平滑、离参考线最近的曲线。

• 规则：必须遵守物理规律（运动学限制）。

• 解题工具：OSQP（求解器）。

---

一、什么是 运动学限制 (Kinematic Constraints)

运动学限制就是这辆车的物理极限 。否则，求解器可能会算出一条数学上完美，但物理上方向盘根本打不到的轨迹（比如原地直角转弯）。

在 Frenet (SL) 坐标系 下，这些物理限制通常被转化为对曲线的导数的限制：

1. 曲率约束 (Curvature Constraint) ------ 对应方向盘转角

• 物理含义：前轮转角（Steering Angle）是有最大值的（比如 ±470∘ 对应轮胎转角 ±35∘）。车不能转得太急。

• 数学表达：在 SL 坐标系中，曲线的二阶导数 l′′ 近似代表曲率 κ。

• 公式关系：κ≈l′′。由于 κ=Ltan(δ) （δ为前轮转角，L为轴距），限制 δ 就是限制 l′′。

• 约束：∣l′′∣≤κmax

2. 航向角偏差约束 (Heading Error Constraint)

• 物理含义：车身相对于道路中心线的夹角不能太大。你不能横着车在路上走。

• 数学表达：曲线的一阶导数 l′ 代表航向偏差。

• 约束：∣l′∣≤tan(θmax)

3. 侧向加速度约束 (Lateral Acceleration) ------ 对应舒适性/防侧翻

• 物理含义：车速越快，过弯就必须越缓，否则离心力会让乘客不舒服，甚至翻车。

• 公式：alat=v2⋅κ。

• 约束：由于 v（纵向速度）通常是给定的或分步规划的，限制 alat 本质上也是在动态地限制曲率 κ（即 l′′）。

总结： 运动学限制就是给求解器加上的"手铐"，要求输出的轨迹 l(s) 必须满足：

"不要太偏 (l), 不要太斜 (l′), 不要太弯 (l′′)。"

---

二、 OSQP 算法 (Operator Splitting Quadratic Program)

OSQP 是目前自动驾驶领域（特别是百度 Apollo）最常用的二次规划 (Quadratic Programming, QP) 求解器。

1. 为什么要用"二次规划" (QP)？

我们在横向规划中想要的东西，写成数学公式刚好是一个二次函数

2. 什么是 OSQP？

• 全称：Operator Splitting Quadratic Program（算子分裂二次规划）。

• 作用：它是一个数值优化引擎。你把上面的 Cost Function（目标）和前面提到的边界、运动学限制（约束 l≤Ax≤u）扔给它，它能在几毫秒内算出最优的 l,l′,l′′ 序列。

3. 为什么 OSQP 比采样算法（Sampling）更高效？

这是你问题中的关键对比。

维度	采样算法 (Sampling / Lattice)	优化算法 (Optimization / QP / OSQP)
原理	"猜"：在前方撒一大把线（比如撒50条），然后一条条检查哪条最好。	"算"：直接通过数学导数梯度下降，一步到位算出那条最好的线。
解的质量	离散的：最优解可能在你没撒线的空隙里，你选出来的只是"矮子里拔将军"。	连续的：算出来的是理论上的全局最优解（在凸空间内）。
效率	撒线少了找不到解，撒线多了计算量爆炸。	非常快 ：OSQP 利用了矩阵的稀疏性（Sparse Matrix），对于几百个点的规划，计算时间通常在 ms 级别。
平滑性	选出来的线可能不那么平滑，需要后处理。	天然保证平滑（因为 Cost 里包含了 l′′）。

三、 总结：整个工作流

结合你之前的上下文，这个模块的工作流如下：

1. 输入：

   • 参考线（大致方向）。

   • 轨迹边界（由感知和地图生成的硬约束，比如 s=50 时 l∈[0.5,1.75]）。

   • 运动学限制（由车辆物理属性决定的硬约束，比如 ∣l′′∣≤0.15）。

2. 建模：

   • 将上述信息转化为矩阵形式：P (权重), A (约束矩阵), l,u (上下界)。

3. 求解 (OSQP)：

   • OSQP 启动，通过 ADMM（交替方向乘子法）迭代求解。

4. 输出：

   • 一条既躲开了障碍物，又满足车辆物理极限，且开起来最舒服的最优轨迹。

这就是为什么现在的量产自动驾驶（尤其是 L2+ 级别）在规控底层大量依赖 OSQP 这类求解器的原因。

---

ST图

ST 图 (S-T Graph)

全称 Space-Time Graph (空间-时间图)

是自动驾驶纵向规划 (Speed Planning) 中最核心的工具。

如果说 SL 图 (Frenet) 解决了"怎么打方向盘"的问题，那么 ST 图 就解决了"怎么踩油门和刹车"的问题。

---

一、 坐标轴的物理含义

首先，你要把 ST 图看作是一个二维平面，但它的两个维度比较特殊：

• 横轴 T (Time) ：未来时间。原点 t=0 是当前时刻，向右延伸表示预测的未来（例如未来 8 秒）。

• 纵轴 S (Station) ：纵向位移 。这通常对应 Frenet 坐标系下的 S 轴。原点 s=0 是车辆当前位置，向上延伸表示沿参考线向前行驶的距离。

ST 图上的任何一个点 (t,s)，意思是："在 t 时刻，车子应该到达 s 这个位置"。

---

二、 "线"代表什么？（速度与加速度）

在 ST 图上画一条线，这条线就是车辆的纵向轨迹。这条线的几何特征对应着物理运动学参数：

1. 切线斜率 (Slope) = 速度 (v)

   • v=dtds。

   • 线越陡，斜率越大 → 车速越快。

   • 线越平，斜率越小 → 车速越慢。

   • 水平线 (斜率为0) → 车停在那里不动。

2. 曲线弯曲程度 (Curvature) = 加速度 (a)

   • a=dtdv=dt2d2s。

   • 向下弯曲 (凸) → 斜率变小 → 减刹车。

   • 向上弯曲 (凹) → 斜率变大 → 加速。

---

三、 障碍物在 ST 图长什么样？（禁区）

这是规控中最有意思的部分。现实世界中形状各异的障碍物，投射到 ST 图上，都会变成一个个**"几何禁区"**。

1. 静态障碍物 (Static Obstacle)

   • 例子：前方 50 米处有一辆抛锚的车，停在那不动。

   • ST 图表现：因为它一直占据 S=50 的位置，无论时间 T 怎么变，它都在那。

   • 形状 ：一条水平的长条矩形 (Wall)。

2. 动态障碍物 (Dynamic Obstacle)

   • 例子：一辆旁车正在切入，或者你在跟车。

   • ST 图表现：随着时间 T 推移，它的位置 S 也在变（往前跑）。

   • 形状 ：一个倾斜的平行四边形或多边形。

   • 斜率：这个四边形的斜率大约就是前车的车速。

3. 虚拟墙 (Virtual Wall)

   • 例子：红灯。

   • ST 图表现：在停车线位置 S 画一条横线，要求你的轨迹线不能超过这条横线。

---

四、 怎么在 ST 图上做规划？（决策与优化）

规划器的任务，就是在这个图上画一条连续、光滑的曲线，要求：

1. 不碰到任何障碍物方块（碰撞检测）。

2. 满足动力学限制（斜率不能无限大，不能超速）。

3. 最舒适（曲率变化最小，急刹急起最少）。

这里涉及两个核心步骤：

1. 决策 (Speed Decision)：选"上"还是选"下"？

面对一个障碍物方块，你的曲线有两种绕法：

• Overtake (超车/抢先) ：曲线从方块的上方穿过。

  • 含义：在障碍物到达该位置之前，我已经冲过去了（S 比它大）。

  • 风险：需要加速，风险较高。

• Yield (让行/跟车) ：曲线从方块的下方穿过。

  • 含义：等障碍物走远了，我才到达该位置（S 始终小于它）。

  • 常态：这是最常见的跟车或减速避让策略。

2. 优化 (Speed Optimization)：画出最完美的线

确定了从下方走（Yield）之后，我们会用 QP (二次规划) 或 样条插值 (Spline) 算法，在这个狭窄的"通道"里拉出一条最顺滑的线。这条线对应的就是最终下发给控制器的目标速度剖面。

---

总结：ST 图的作用

对于规划算法工程师，ST 图将复杂的交通博弈问题 （要不要让车、什么时候减速、红绿灯怎么停），转化为了一个纯粹的数学几何问题（在二维坐标系里找一条不穿过矩形的线）。

一句话口诀：

横看时间竖看路，斜率代表车速度。 方块挡路是障碍，上下绕行定前途。

---

Decider（决策） 和 Optimizer（规划）

先有决策，后有规划。

它们的关系可以概括为：决策负责"可以运动的区域"，规划负责"规划路线"。

下面我用通俗的语言结合你提供的输入输出，为你深度拆解这两者的关系：

---

一、 核心关系：老板与执行者的关系

• 先决策 (Decision)：先定大方向。

• 后规划 (Planning)：再看看细节。

2. 它们在干什么？

• 纵向决策 (Decision) ------ 它是"老板"

  • 任务 ：面对障碍物（比如前车），它要拍板做一个定性的决定：是跟车 (Follow) 、超车 (Overtake) 、还是停车 (Stop)？

  • 过程 ：现实世界中，前车在 ST 图上是一个"方块"。"跟车"意味着我要选方块下面 的路；"超车"意味着我要选方块上面的路。

  • 输出 (你的输入) ：一旦老板拍板说"跟车"，它就会把方块上面的空间全部切掉，只保留方块下方的空间。这就形成了你提到的 "唯一的速度边界" （也就是由上界和下界围成的 ST 走廊 / ST Corridor）。

  • 本质 ：将非凸问题转化为凸问题（Convexification）。

• 纵向规划 (Planning) ------ 它是"老司机"

  • 任务：老板已经划定好了安全区（速度边界），老司机现在的任务是在这个框框里，开出一条最稳、最舒服的线。

  • 过程 ：利用 OSQP 数学求解器，在满足边界限制（不撞墙）的前提下，让急加减速最少（平滑）。

  • 输出 (你的输出) ：一条具体的、带时间戳的 平滑速度曲线（告诉控制层：第1秒开10m/s，加速度0.5；第2秒开11m/s...）。

---

二、 为什么要如此划分？（技术原理）

你可能会问："为什么不能直接算一条线出来，非要中间搞个决策？"

这就涉及到了数学优化的核心难点：非凸性 (Non-convexity)。

1. 原始问题是"多解"的（非凸）

想象前面有辆慢车。在数学上，你有两个选择：

• 解A：一脚油门绕过去（超车）。

• 解B：一脚刹车跟在后面（跟车）。

求解器（OSQP）很笨，如果你同时给它两个选择（中间有个障碍物挡着），它会"精神分裂"，算不出来，或者算出一条直接撞上障碍物的线（因为它试图取两个解的平均值）。

2. 决策的作用：剪枝

决策 的核心作用，就是从 A 和 B 里面强制选一个。

• 决策层说："现在的路况太挤，不准超车，选 B！"

• 于是，它把 A 的可能性全部删掉（在 ST 图上把障碍物上方的空间封死）。

• 此时，剩下的空间就是一个单连通的、凸的管道。

3. 规划的作用：在单行道里狂奔

一旦空间变成了"凸"的（没有分叉路），OSQP 这种二次规划求解器就是无敌的。它能在几毫秒内算出全局最优解。

---

三、 结合例子梳理

让我们把流程串起来：

1. 感知输入：

   • 前方 50米 有辆车（障碍物）。

2. 纵向决策 (Speed Decider)：

   • 思考：我是超过去还是跟在后面？（根据变道意图、当前车速等判断）。

   • 决定：跟车 (Yield)。

   • 动作：在 ST 图上，以前车轨迹为天花板，画出一根底线。

   • Output ：唯一的速度边界（比如：t=0 \sim 5s 内，你的位置 s 必须小于前车位置 s_{lead}）。

3. 纵向规划 (Speed Optimizer)：

   • 思考：现在天花板定死了，我怎么踩油门最省油、乘客最不晕车？

   • 动作：启动 OSQP。

     • 目标函数 J：最小化加速度的变化率 (Jerk)。

     • 约束条件 A x \le b：位置 s 不能超过刚才给的边界。

   • Output ：平滑的速度曲线。

总结

• 关系 ：决策指导规划，规划实现决策。

• 顺序 ：先决策，后规划。

• 核心逻辑：决策负责把复杂的"多选一"问题（非凸），变成简单的"填空题"（凸）；规划负责用数学工具把这个"填空题"做得最漂亮。

---

凸问题，非凸问题（Non-Convex）

我用最直观的图解和自动驾驶案例来给你解释。

---

一、 直观理解：碗 vs 山脉

1. 凸问题 (Convex Problem) ------ 只有一个坑

想象一个光滑的碗（或者一口大锅）。

• 目标：你要把一颗玻璃珠滚到碗底（寻找最低点/最优解）。

• 特点 ：无论你把珠子放在碗壁的哪里，它最终一定会滚到同一个最低点。

• 结论 ：在凸问题里，局部最优解 = 全局最优解。只要你找到了一个低点，它就是全宇宙最低的点。

• 求解器心情：开心。无脑往下走就行，不用担心走错路，而且计算极快。

2. 非凸问题 (Non-Convex Problem) ------ 连绵起伏的山脉

想象一个鸡蛋托（或者连绵的山峰和山谷）。

• 目标 ：你要找到整个鸡蛋托里最深的那个坑（全局最优）。

• 特点：

  • 如果你把珠子放在位置 A，它可能会滚进一个小坑（局部最优）。

  • 如果你把珠子放在位置 B，它可能会滚进另一个小坑。

  • 当你掉进一个小坑时，你环顾四周，发现周围都比你高，你以为到底了。但其实在隔壁山头后面，有一个更深的坑（全局最优）。

• 结论 ：局部最优解不等于全局最优解。求解器很容易被困在一个并不完美的坑里出不来。

• 求解器心情：崩溃。我怎么知道山那边是不是还有更深的山谷？我是不是要跳出来重找？

---

二、 几何定义：连线测试 (The Line Test)

这是区分两者最严谨的方法，在做路径规划时非常有用。

1. 凸集 (Convex Set) ------ 空间是"实心"的

定义 ：如果你在集合里随便取两个点 A 和 B，把它们连成一条直线。如果这条直线上的每一个点都在集合内部，这就是凸集。

• 例子 ：圆形、正方形、三角形、没有障碍物的空旷道路。

2. 非凸集 (Non-Convex Set) ------ 空间有"洞"或"凹陷"

定义 ：如果你取两个点 A 和 B，连线后发现线段的一部分跑到了集合外面，这就是非凸集。

• 自动驾驶经典案例 ：路中间有个障碍物。

  • 点 A：障碍物左边的路。

  • 点 B：障碍物右边的路。

  • 连线 AB：直接穿过了障碍物（不可行区域）。

  • 所以，带障碍物的避障规划，天然是一个非凸问题。

---

三、 为什么这对规控工程师很重要？

这就解释了为什么我们需要**"决策（Decision）"** 和**"规划（Planning）"**分家。

1. 规划器（OSQP）喜欢凸问题

我们在做轨迹优化（用 OSQP 算线）时，数学求解器非常依赖"凸性"。如果是凸问题，它能在 10ms 内算出结果；如果是非凸问题，它可能算 10s 还没算出来，或者算出一个撞墙的轨迹。

2. 障碍物让问题变成了非凸

只要路上有车（障碍物），可行空间就是"中间被挖掉一块"的非凸形状（如上文提到的 ST 图中的矩形禁区）。 直接在这个空间里做优化，不仅慢，而且容易出错（求解器可能会在"左绕"和"右绕"之间犹豫不决，导致最后直接撞上去）。

3. 决策的作用：把非凸切成凸 (Convexification)

这就是**决策层（Speed Decider / Path Decider）**存在的意义！

• 原始问题（非凸）：我既可以从左边绕，也可以从右边绕。

• 决策介入 ：拍板决定 ------ "选左边！"

• 结果（凸）：决策层把"右边"的空间切掉，只保留"左边"的空间。现在的空间是一个单连通的管子（凸集）。

• 规划介入：在这个"凸"的管子里，用 OSQP 快速算出一条最优曲线。

总结

• 凸问题 ：碗状，实心，只有一个最优解，好算。

• 非凸问题 ：蛋托状，有洞，有无数个陷阱，难算。

• 工作流 ：决策 负责把非凸 的复杂路况切成凸 的简单通道，然后交由规划去算最优解。

---

Lanelet地图

Lanelet2 是目前自动驾驶领域最主流的 高精地图（HD Map） 格式之一。

• 普通地图（ 比如百度地图**）**：把路看作**"一根线"**。它只关心从 A 到 B 怎么走，不关心路有多宽，也不关心你在哪条车道。

• Lanelet2 地图（高精地图） ：把路看作**"一根管道"（或者一条走廊）。它不仅知道路在哪里，还精确地规定了左边线在哪、右边线在哪、限速多少、红绿灯管哪条道**。

---

1. 核心单元：Lanelet (车道小段)

Lanelet2 的名字来源就是 Lane (车道) + let (小元素)。

• 定义 ：一段可行驶的、有方向的区域。

• 组成 ：它由 左边界 (Left Bound) 和 右边界 (Right Bound) 组成。

• 作用：你的自动驾驶车，本质上就是在无数个连接起来的 Lanelet 里"游泳"。

  • 全局规划：就是在找"我要经过哪些 Lanelet"（比如 ID 101 -> ID 102 -> ID 105）。

  • 局部规划：就是在当前所在的 Lanelet 管道里，画一条不撞墙的线。

2. 五大基本图元 (Primitives)

Lanelet2 的数据结构非常像搭积木，由底层向上层构建：

1. Node (点)：

   • 最基础的单位。

   • 包含 GPS 坐标（经度、纬度、高度）和一个唯一的 ID。

   • 例子：路边的一个油漆点。

2. LineString (线串)：

   • 由一串有序的 Node 连起来的线。

   • 它可以是实线、虚线、路沿石，也可以是虚拟的线（如路口的停止线）。

   • 例子：车道左边的那条白实线。

3. Lanelet (车道)：

   • 由一条左 LineString 和 一条右 LineString 围成的区域。

   • 它有方向性（只能顺着边界线的方向开）。

   • 项目关联： "沿参考线行驶"，参考线就是这两个边界的中心线。

4. Area (区域)：

   • 由多条 LineString 围成的封闭多边形。

   • 例子： 停车场、建筑物、或者路中间的草坪。

5. Regulatory Element (交通规则)：

   • 这是 Lanelet2 聪明的地方。它把"规则"和"车道"绑在一起。

   • 例子： 一个红绿灯（物理对象）是一个 Node，但这个红绿灯管辖 哪个车道？这就需要一个 Regulatory Element 来建立连接：Lanelet A 受 Traffic Light B 控制。

---

3. Lanelet2 解决了什么问题？

A. 拓扑关系 (Topology) ------ 给全局规划用的

它明确了车道之间的连接关系：

• Predecessor / Successor：这个车道的上一个是哪个？下一个是哪个？

• Adjacent Left / Right：左边能不能变道？右边能不能变道？

• 实战场景： 你的文档里提到的"非掉头路段掉头"方案，其实就是手动修改了 Lanelet 的拓扑关系。硬生生把一个向北的 Lanelet 和旁边向南的 Lanelet 连了起来，骗全局规划器说"这里可以走"。

B. 几何形状 (Geometry) ------ 给局部规划用的

它提供了精确的物理边界。

• 实战场景： 你的文档里提到的 "轨迹边界" ，直接来源就是 Lanelet 的 Left Bound 和 Right Bound。局部规划器必须保证生成的轨迹不超出这两条线（除非变道）。

---

4. 物理存储格式 (.osm)

Lanelet2 通常直接借用了 OpenStreetMap (.osm) 的 XML 格式来存储。 这意味着你可以直接用文本编辑器打开它，会看到类似这样的代码：

<node id="1" lat="49.0" lon="8.4"> ... </node>  <way id="10" >  <nd ref="1"/>
    <nd ref="2"/>
    <tag k="type" v="line_thin"/>
    <tag k="subtype" v="solid"/>
</way>
<relation id="100"> <member type="way" ref="10" role="left"/>  <member type="way" ref="11" role="right"/> <tag k="type" v="lanelet"/>
</relation>

5. 工作流建议

1. 可视化查看：

   • 使用工具 JOSM (Java OpenStreetMap Editor) 。这是编辑 Lanelet2 地图的神器。你需要装一个 Lanelet2 插件。

   • 或者使用 Autoware 自带的 MapTool。

   • 任务 ：找导师要一下你们园区的 .osm 地图文件，用 JOSM 打开，看看里面的线是怎么连的，属性（比如 turn_direction, speed_limit）是怎么填的。

2. 代码调用：

   • 在 C++ 代码里，你们会用 lanelet2_core 这个库。

   • 你最常用的函数可能是：

     • geometry::distance(point, lanelet)：算车离车道有多远。

     • routingGraph->shortestPath(startLanelet, endLanelet)：搜全局路径。

总结

Lanelet2 就是把地面上的白线（几何）和交通规则（逻辑）打包在一起的数据格式。 它是你的自动驾驶车理解世界的"底图"。