自动驾驶轨迹规划算法——Apollo OpenSpace Planner

自动驾驶轨迹规划算法——Apollo OpenSpace Planner

一、Apollo OpenSpace Planner 是什么？

1.1 定义与核心思想

Apollo OpenSpace Planner 是百度 Apollo 自动驾驶系统中专为 非结构化道路与开放空间 设计的 实时轨迹规划器。
在这些场景中，车辆无法依赖标准车道线或明确的车道几何结构（如高速公路、城市主干道），而是需要在 二维自由空间 中根据障碍物分布、可行驶区域以及目标位置，直接规划可行轨迹。

核心思想是：

• 直接在笛卡尔坐标系下进行空间搜索与轨迹优化，而非在 Frenet 坐标系中依赖车道参考线；
• 在搜索过程中引入 车辆运动学约束（如最小转弯半径、最大曲率变化率等），保证生成的路径可以直接被执行；
• 支持 前进、倒车、多段换向 等复杂机动，适应狭窄空间泊车、调头、绕障等任务；
• 结合搜索与平滑优化，既能快速找到可行解，又能保证轨迹的连续性和舒适性。

在自动驾驶系统中，OpenSpace Planner 的职责是：

• 基于障碍物地图和目标位置，搜索一条无碰撞、可执行的轨迹；
• 在低速场景下同时考虑空间避障与机动动作（前进/倒车切换）；
• 对粗轨迹进行优化，使其满足车辆动力学约束与舒适性要求；
• 以滚动规划方式实时更新轨迹，确保对环境变化做出响应。

1.2 与其他规划器的区别

总结：
OpenSpace Planner 最大的不同点在于，它完全摆脱了对车道几何的依赖，而是直接在障碍物地图构成的自由空间内进行规划，生成满足车辆运动学约束的可执行轨迹。这使它成为停车、调头、堆场作业等低速复杂场景的核心规划器。

1.3 典型应用场景

1. 自动泊车（Autonomous Parking）
   • 自动驶入平行、垂直、斜列等多种类型的停车位；
   • 支持前进或倒车入位，多段机动操作。
2. 狭窄空间调头（Tight-space U-turn）
   • 在仅略宽于车辆长度的道路上完成三点或多点调头；
   • 通过前进/倒车切换实现安全掉头。
3. 港口、仓储调度（Logistics Yard Operations）
   • 在无明显道路标识的大型开放区域中，规划避障路径；
   • 绕过货物堆放、行人和临时障碍。
4. 临时施工区绕行（Construction Zone Maneuvering）
   • 在封闭或无标线路段中重新寻找可行驶路径；
   • 避开施工设备和障碍物。
5. 特种作业车辆机动（Special Vehicle Maneuvers）
   • 在机场、矿区、港口等特殊作业环境中完成精确轨迹规划；
   • 满足低速安全和路径可控性要求。

二、Apollo OpenSpace Planner 的原理

OpenSpace Planner 的核心是 自由空间搜索 + 轨迹平滑优化。在没有车道约束的开放区域（停车、调头、堆场）里，先用 Hybrid A* 找到一条可执行的“粗轨迹骨架”，再用 二次型/凸优化 平滑并加入动力学约束，最后做 时间参数化 生成可跟踪的时序轨迹。关于****Hybrid A*详细可以参考我这篇文章，QPSpline路径平滑可以参考这篇文章QPSpline

2.1 总体架构与数据流

输入：

• 可行驶区域与静态障碍物：多边形/栅格/距离场（SDF）。
• 动态障碍（低速场景常简化为准静态，必要时线性外推）。
• 车辆状态 $z=(x,y,\theta ,v)$、目标位姿 $z_g=(x_g,y_g,{\theta }_g)$ 或目标盒（车位）。
• 车辆几何：长 $L_{\text{veh}}$、宽 $W_{\text{veh}}$、轴距 $L$、转角上限 ${\delta }_{\max }$。

流程：

1. ROI 生成 & 膨胀：裁剪规划区域，障碍物用 Minkowski 和（或 buffer）做安全膨胀 $b_{\text{safe}}$。
2. Hybrid A*：在 $(x,y,\theta ,\text{gear})$ 上搜索粗轨迹。
3. 轨迹平滑（优化）：将粗轨迹离散为锚点，最小化二差分/曲率并施加避障/曲率/边界等约束。
4. 时间参数化：基于曲率与动态约束生成 $v(s)$ 或 $v(t)$，得到 $(x(t),y(t),\theta (t),v(t))$。
5. 滚动重规划：每周期（~100 ms）用当前状态重启；拼接/继承上一周期解作为 warm start。

2.2 自由空间搜索（Hybrid A*）

2.2.1 状态空间与运动学

• 状态：$\mathbf{s}=(x,y,\theta ,\text{gear})$，gear∈{F,R}（前进/倒车）。

• 控制原语（motion primitives）：$\delta \in -{\delta }_{\max },-{\delta }_m,0,{\delta }_m,{\delta }_{\max }$，步长 $d_s$，档位切换允许但有惩罚。

• 单轨模型（常速近似积分一小步）：
  设 $R=\frac{L}{\tan \delta }$。对一步长 $d_s$，若 $\delta \ne 0$：

  $\begin{array}{rl}\Delta \theta & =\sigma \frac{d_s}{R},\sigma =\{\begin{array}{ll}+1,& \text{F}\\ -1,& \text{R}\end{array}\\ x^{\prime }& =x+R\left(\sin (\theta +\Delta \theta )-\sin \theta \right)\\ y^{\prime }& =y-R\left(\cos (\theta +\Delta \theta )-\cos \theta \right)\\ {\theta }^{\prime }& =\theta +\Delta \theta \end{array}$

  若 $\delta =0$（直线），$x^{\prime }=x+\sigma ,d_s\cos \theta ,;y^{\prime }=y+\sigma ,d_s\sin \theta ,;{\theta }^{\prime }=\theta$。

注：也可用小步欧拉积分；上式为常曲率弧闭式更新，数值更稳。

2.2.2 离散分辨率与网格索引

• 平面分辨率：$\Delta x,\Delta y$（常取 0.1–0.2 m）。
• 航向离散：$N_{\theta }$ 等分（常取 60–72，约每 5–6°）。
• 节点索引：$(\lfloor x/\Delta x\rfloor ,;\lfloor y/\Delta y\rfloor ,;\lfloor \theta /(2\pi /N_{\theta })\rfloor ,;\text{gear})$。
• 闭集判重：同一索引下若新 $g$ 更大则丢弃，避免膨胀搜索。

2.2.3 代价函数与启发

• A* 目标：$\min f(n)=g(n)+h(n)$。
• 路径代价 $g$（建议项）：
  • 距离：$w_d\cdot d_{\text{arc}}$
  • 曲率惩罚：$w_{\kappa }\cdot {\kappa }^2$，$\kappa =\tan \delta /L$
  • 变向惩罚：$w_{\text{gear}}\cdot 1[\text{gear change}]$
  • 近障惩罚：$w_o\cdot \psi (\text{SDF}(x,y))$（SDF 小则代价大；可用 $\psi (d)=\max (0,d_{\text{th}}-d{)}^2$）
  • 方向一致性：$w_{\theta }\cdot |\text{wrap}(\theta -{\theta }_g)|$（靠近目标朝向）
• 启发 $h$（可采一致/可采纳）：
  • 欧氏距离 $w\cdot |(x,y)-(x_g,y_g)|$；
  • 更强：Reeds–Shepp 距离 $d_{RS}(\mathbf{s},{\mathbf{s}}_g)$（允许前后倒），或 Dubins（仅前进）。
  • 实践中取 $h=\max d_E,;\alpha \cdot d_{RS}$ 保守可采纳。

2.2.4 分析连接（Analytic Expansion）

每扩展若干步尝试用 Reeds–Shepp 曲线直接连接至目标：

• 生成若干 $RS$ 模式序列（左/右转+直线拼接）。
• 对候选 $RS$ 轨段做 连续碰撞检测（见 2.2.6）。
• 若存在无碰撞连接，提前收敛并回溯路径。

2.2.5 档位与步长策略

• 步长 $d_s$：0.5–1.0 m（窄场景建议 0.4–0.6 m）。
• 扩展集合中同时包含 F/R 两类动作，但 gear change 计惩罚，避免无意义抖动。
• 当接近目标且朝向误差小于阈值时缩小 $d_s$，便于精细停靠。

2.2.6 碰撞检测（高效而稳健）

• 车辆模型：以车辆几何 + 安全边距 $b_{\text{safe}}$ 的 有向包围盒（OBB）。
• 障碍物：多边形（已做 buffer 膨胀）。
• 粗检：圆覆盖/多圆模型快速排除（少量半径覆盖角点）。
• 细检：分离轴定理（SAT）检测 OBB–多边形相交。
• 连续性：对一段弧插值 $M$ 个中间姿态（例如 $M=5$）逐一检测，避免“跨越穿模”。

2.2.7 搜索停止与路径回溯

• 触达目标域（位置/朝向容差）或 Analytic Expansion 成功即止。
• 保存父指针回溯，得到折线/弧段混合的粗轨迹 $\mathbf{s}\ast i\ast {i=0}^N$。
• 可做一次 道格拉斯-普客（RDP） 稀疏化，减少冗余点便于后续优化。

2.3 轨迹平滑优化（连续可控的可执行轨迹）

2.3.1 变量与参数化

• 以等弧长采样得到锚点 ${\mathbf{p}}_i=(x_i,y_i)$，$i=\mathrm{0..}N$；
• 航向 ${\theta }_i$ 与曲率 ${\kappa }_i$ 可用差分近似（或显式变量）。
• 可固定档位序列（来自 Hybrid A*），避免优化阶段改变拓扑。

2.3.2 目标函数（典型二次型）

min ⁡ { p i } w s ∑ i = 1 N − 1 ∥ p i + 1 − 2 p i + p i − 1 ∥ 2 ⏟ 二阶平滑 + w κ ∑ i κ i 2 ⏟ 曲率 + w o ∑ i ψ ( d i ) ⏟ 避障 + w a ∑ i ∥ p i − p ˉ i ∥ 2 ⏟ 锚点/惯性 p i \min_{\{\mathbf{p}_i\}} \underbrace{w_s \sum_{i=1}^{N-1}\|\mathbf{p}_{i+1}-2\mathbf{p}_i+\mathbf{p}_{i-1}\|^2}_{\text{二阶平滑}} + \underbrace{w_\kappa \sum_{i} \kappa_i^2}_{\text{曲率}} + \underbrace{w_o \sum_i \psi\big(d_i\big)}_{\text{避障}} + \underbrace{w_a \sum_i \|\mathbf{p}_i-\bar{\mathbf{p}}_i\|^2}_{\text{锚点/惯性}}{pi} min{pi​}​二阶平滑 ws​i=1∑N−1​∥pi+1​−2pi​+pi−1​∥2​​+曲率 wκ​i∑​κi2​​​+避障 wo​i∑​ψ(di​)​​+锚点/惯性 wa​i∑​∥pi​−pˉ​i​∥2​​pi

• $d_i$ 为 $\mathbf{p}\ast i$ 至最近障碍物的有符号距离（SDF）；$\psi$ 常用折线或铲形函数（如 $\psi (d)=\max (0,d\ast \text{th}-d{)}^2$）。
• ${\bar{\mathbf{p}}}_i$ 为上一周期解或 Hybrid A* 粗解（warm start），有助于滚动稳定。

曲率近似（离散 Frenet 公式）：

${\kappa }_i\approx \frac{2\text{Area}({\mathbf{p}}_{i-1},{\mathbf{p}}_i,{\mathbf{p}}_{i+1})}{\Vert {\mathbf{p}}_i-{\mathbf{p}}_{i-1}\Vert \Vert {\mathbf{p}}_{i+1}-{\mathbf{p}}_i\Vert \Vert {\mathbf{p}}_{i+1}-{\mathbf{p}}_{i-1}\Vert }$

2.3.3 约束（硬约束与线性化）

• 边界/障碍约束：用 SDF 的切向线性化构造半平面：

  ${\mathbf{n}}_i^{\top }{\mathbf{p}}_i\le b_i-d_{\text{safe}}$

  其中 ${\mathbf{n}}_i$ 是最近障碍面的外法向，$b_i$ 为该面支持函数常数；在每轮迭代更新（逐次凸化）。

• 曲率上界：$|{\kappa }_i|\le {\kappa }_{\max }$（可线性化成二范数锥近似或转为软约束加入罚项）。

• 起终点约束：$\mathbf{p}\ast 0=\mathbf{p}\ast \text{start}$，${\mathbf{p}}_N\in \text{目标盒}$，并约束 ${\theta }_0,{\theta }_N$（用邻点方向或直接角度变量）。

• 几何走廊（可选）：由 ROI 内缩得到左右边界，限制 ${\mathbf{p}}_i$ 落在带状区域内。

实践中常采用 “迭代锚定 + 线性化避障 + 二次目标” 的 逐次凸优化（SCP）：每轮解一个稀疏 QP，更新法向与锚点，直至收敛。

2.3.4 求解与收敛

• 求解器：稀疏 QP（OSQP/qpOASES/自研），带温启动。
• 终止：最大迭代数 $K_{\max }$ 或 $|\Delta \mathbf{p}{|}_{\infty }<ϵ$。
• 数值技巧：权重标准化（位置单位 m、SDF 单位 m、曲率 1/m），避免病态；分层权重（先避障后平滑）。

2.4 时间参数化（速度曲线与动力学约束）

对平滑后路径按弧长 $s$ 计算曲率 $\kappa (s)$ 与其导数 $d\kappa /ds$，用 前向–后向两遍 生成速度剖面（piecewise-jerk 思路也可）。

2.4.1 速度上限由几何与执行器决定

• 侧向加速度约束：$v_{\max ,1}(s)=\sqrt{\frac{a_{y,\max }}{|\kappa (s)|+\epsilon }}$
• 转角速率约束（$\delta \simeq L\kappa$）：$v_{\max ,2}(s)=\frac{{\dot{\delta }}_{\max }}{L,|d\kappa /ds|+\epsilon }$
• 场景限速：$v_{\max ,3}(s)$（停车/调头通常 $\le 3$–$7$ m/s）
• 综合：$v_{\max }(s)=\min v_{\max ,1},v_{\max ,2},v_{\max ,3}$

2.4.2 前向/后向扫描（加速度与减速度边界）

• 前向：$v_i=\min (v_{\max }(s_i),;\sqrt{v_{i-1}^2+2a_{\max }\Delta s})$
• 后向：$v_i=\min (v_i,;\sqrt{v_{i+1}^2+2a_{\text{brake}}\Delta s})$
• 可加入 jerk 平滑：对离散加速度 $a_i$ 加二次罚并做一次 QP 微调。
• 档位切换点附近将 $v$ 逼近 0，预留换挡缓冲。

2.4.3 生成时序轨迹

• 通过 $t_i=t_{i-1}+\Delta s/\frac{v_i+v_{i-1}}{2}$ 得到 $t(s)$；
• 插值输出均匀时间步长的 $(x(t),y(t),\theta (t),v(t),a(t))$ 序列。

2.5 滚动重规划与稳健性

• 拼接与温启动：将已执行段截去，保留后缀作为 warm start；若环境变化大，退回 Hybrid A* 重新 warm start。
• 档位迟滞：gear 切换设置最小持续时长与切换惩罚，避免抖动。
• 时间预算：如优化未收敛，使用最近一次可行解下发（带轨迹有效性检查）。
• 安全回退：连接至低速安全停靠轨迹或直线退出 ROI。

2.6 推荐默认参数（可作为起始配置）

权重建议在 单位归一化 后微调：位置（m）、曲率（1/m）、SDF（m）。

三、Apollo OpenSpace Planner 的关键技术细节

3.1 障碍物建模与地图表示

在 OpenSpace Planner 中，障碍物建模与地图表示是搜索和优化的基础，直接影响可行性判断与避障精度。

3.1.1 ROI（Region of Interest）生成

• 目的：限制规划计算在局部区域内，降低计算量。
• 生成方式：
  1. 以车辆当前位置和目标位置为中心，取一定外扩范围 $d_{\text{ROI}}$（如 20–50 m）；
  2. 裁剪高精地图中的可行驶区域；
  3. 合并感知模块输出的障碍物包络，做坐标变换至地图坐标系。

3.1.2 障碍物安全膨胀

• 原因：补偿感知误差、定位误差以及车辆外形与碰撞模型的差距。

• 方法：

  • 对多边形障碍物做 Minkowski 膨胀：

    ${\text{Obs}}_{\text{exp}}=\text{Obs}\oplus B(b_{\text{safe}})$

    其中 $B$ 为圆形缓冲区。

  • $b_{\text{safe}}$ 可分解为定位误差、感知误差和安全裕度：

    $b_{\text{safe}}=e_{\text{loc}}+e_{\text{perc}}+m_{\text{margin}}$

3.1.3 地图表示形式

• 多边形表示：直接使用障碍物轮廓进行几何碰撞检测（SAT / OBB 检测）。
• 栅格表示：用于 Hybrid A* 中快速可行性判断，常用 0.1–0.2 m 分辨率。
• SDF（Signed Distance Field）：
  • 用于优化阶段快速计算点到障碍物的有符号距离和梯度；
  • 可由栅格地图用 BFS / Jump Flooding 算法预计算。

3.2 全局搜索与局部优化的结合

OpenSpace Planner 的规划流程是 “全局搜索定拓扑 + 局部优化控几何”。

3.2.1 全局搜索（Hybrid A*）的作用

• 在自由空间内搜索一条可行的粗轨迹骨架；
• 决定路径的拓扑结构（前进/倒车顺序、绕障侧向、换向次数）；
• 保证满足车辆最小转弯半径与运动学可行性。

3.2.2 局部优化（SCP / QP）的作用

• 在 Hybrid A* 粗轨迹的基础上优化曲率连续性与平滑度；
• 精细避障，保持一定安全边距；
• 对接控制模块对曲率、加速度等动态约束的要求。

3.2.3 交互机制

• Warm Start：优化器以 Hybrid A* 轨迹为初值，减少迭代次数；
• 失败回退：若优化器无法生成可行解（如走廊被堵死），则重新调用 Hybrid A* 进行全局搜索；
• 滚动更新：每次重规划时继承上周期末段轨迹作为起始参考，保证平滑衔接。

3.3 轨迹下发与滚动规划机制

3.3.1 轨迹离散与插值

• 优化器输出的轨迹为非均匀弧长采样点；

• 对轨迹进行等时间间隔插值（如 0.1 s 一点），得到控制模块易于跟踪的离散点列：

  $\mathcal{T}=\{(x_i,y_i,{\theta }_i,v_i){\}}_{i=0}^M$

3.3.2 滚动规划周期

• OpenSpace Planner 常以 100 ms 为滚动规划周期；
• 每周期更新：
  • 车辆最新状态（定位）
  • 环境障碍物信息（感知/预测）
  • 目标位置或目标盒信息
• 使用上一周期未执行完的轨迹末段作为温启动参考，减少轨迹跳变。

3.3.3 轨迹有效性检查

• 在轨迹下发前，检查全程是否：
  1. 满足曲率、加速度等动力学限制；
  2. 不与障碍物碰撞；
  3. 起终点状态与预期相符。
• 若检查不通过，执行降级策略（低速停靠轨迹 / 紧急刹停）。

3.4 工程实现要点

3.4.1 速度与换挡控制

• 档位切换设置最小持续时间（如 $\ge 1.0$ s）；
• 在换挡点将速度剖面过渡至 0，留出执行器反应时间。

3.4.2 避障稳定性

• 搜索阶段采用连续碰撞检测，防止跳点穿越障碍物；
• 优化阶段避障约束用 SDF 线性化，每次迭代更新法向，防止贴边振荡。

3.4.3 参数标定流程

1. 固定几何约束（车辆宽、长、轴距）→ 推算最小转弯半径 → 设置曲率上限 ${\kappa }_{\max }$；
2. 调试搜索权重（$w_d,w_{\kappa },w_o,w_{\text{gear}}$）→ 平衡距离、平滑度、避障与换挡频率；
3. 优化权重调节（$w_s,w_{\kappa },w_o,w_a$）→ 先调避障权重确保安全，再调平滑与曲率权重；
4. 动态约束匹配控制器：曲率变化率、加速度/减速度、jerk 限制与车辆控制模型一致。

3.5 模块间数据交互

四、Apollo OpenSpace Planner 的优势与局限

4.1 优势

1. 场景适应性强
   • 不依赖车道线或规则道路几何，能在停车场、港口、堆场、临时施工区等非结构化场景中工作；
   • 可处理无中心参考线的复杂环境，灵活性远高于车道约束型规划器（如 Lattice、EM Planner）。
2. 支持复杂机动
   • 原生支持前进/倒车切换、多段换向、S 型泊车等机动动作；
   • 能在狭窄空间完成三点调头、多点入位等低速操作。
3. 全局–局部结合
   • Hybrid A* 确保全局可行性（运动学约束 + 拓扑选择）；
   • 优化器 提升局部平滑性与可控性，两者结合兼顾可行性与舒适性。
4. 安全性与可控性高
   • 障碍物膨胀与连续碰撞检测保证了规划轨迹的碰撞安全；
   • 优化器引入曲率、加速度、jerk 限制，使轨迹更易于控制器执行。
5. 可滚动规划，实时响应环境变化
   • 支持 100 ms 级滚动规划周期；
   • 在突发障碍或目标更新时可快速生成新轨迹，保障执行安全性。

4.2 局限

1. 计算开销相对较大
   • Hybrid A* 搜索 + 轨迹优化的组合在复杂障碍环境下计算量较高；
   • 对 CPU 资源和实时性优化要求更高，尤其在高分辨率 ROI 和密集障碍场景下。
2. 动态障碍物处理能力有限
   • 核心设计针对低速、障碍物相对静态的环境；
   • 虽然可做简单动态预测，但不适合高速场景中频繁交互的动态避障任务。
3. 依赖高精度定位与障碍物检测
   • 定位误差、感知延迟会直接影响障碍物建模与碰撞检测精度；
   • 在定位/感知精度不佳的场景中，需要更大的安全边距，可能导致可行解减少。
4. 对路径平滑参数敏感
   • 优化阶段的权重配置（平滑 vs. 避障）直接影响轨迹质量；
   • 不同车辆、不同场景需要重新标定，通用性有限。
5. 对执行器响应有依赖
   • 档位切换、转向速率等需要与底盘控制特性匹配，否则可能出现轨迹可行但执行不稳的情况。