FAST Planner 是一款面向无人机/地面无人车的高实时性、非结构化环境路径规划算法框架，核心定位是“Fast and Safe Trajectory Planner”——兼顾速度（Fast） 和安全（Safe），特别适配矿区、越野等无结构化道路场景，也是无人矿卡局部规划的重要参考方案。

下面从核心定位、算法架构、关键模块、矿卡适配四个维度，详细拆解FAST Planner的路径规划逻辑，兼顾原理理解和工程落地。

---

一、FAST Planner 核心定位

1. 设计目标

• 实时性：单帧规划耗时 < 50ms（适配无人车/无人机的硬实时要求）
• 安全性：严格避障，满足车辆动力学约束
• 平滑性：轨迹连续可微分（jerk连续），适配车辆/无人机执行
• 通用性：支持2D（地面车）/3D（无人机）、结构化/非结构化环境

2. 适用场景

• 无人机：室内/室外避障、自主飞行
• 地面无人车：矿区/园区/越野场景、无车道线避障、窄路会车
• 核心优势：对“未知/动态障碍、无先验地图”场景鲁棒性极强（完美匹配矿区矿卡需求）

3. 与传统算法的差异

算法/框架	核心特点	实时性	非结构化适配	矿卡友好度
FAST Planner	采样+优化融合、实时避障	高	极高	高（需改造）
Lattice Planner	Frenet采样、结构化道路	高	中	中高
Hybrid A*	网格搜索、非完整约束	中	高	高
DWA/TEB	反应式、低速轻量	极高	低	低

---

二、FAST Planner 核心算法架构

FAST Planner采用分层规划架构，从“全局粗路径”到“局部平滑轨迹”逐层优化，核心分为4个模块：

模块1：前端——采样式路径搜索（快速找可行路径）

核心算法：改进版 RRT* / BIT*（Batch Informed Trees）

• 传统RRT*问题：随机采样效率低，收敛慢，实时性差；
• FAST Planner改进：
  1. 目标偏向采样：优先向目标点/全局路径方向采样，减少无效搜索；
  2. 增量式重规划：仅在局部环境变化区域重新采样，而非全局重搜；
  3. BIT*批量采样：一次性生成一批采样点，并行计算，提升搜索速度；
  4. 障碍剪枝：提前过滤碰撞采样点，减少无效节点扩展。

输出

• 一条无碰撞、满足基本动力学约束的粗路径（离散点序列）；
• 核心作用：快速找到“能走的路径”，保证可行性（矿卡核心需求）。

模块2：中端——轨迹初生成（粗路径→连续轨迹）

核心方法：五次/七次多项式插值

• 输入：前端输出的粗路径离散点；
• 处理逻辑：
  1. 将粗路径按“时间/距离”分段；
  2. 用五次多项式拟合每段路径（保证位置、速度、加速度连续）；
  3. 用七次多项式优化（可选，保证jerk连续，提升平滑性）；
• 约束：
  • 矿车关键：最小转弯半径、最大转向角、横向加速度（防侧翻）；
  • 无人机关键：最大航迹角、爬升率。

输出

• 一条连续、无碰撞，但可能存在局部抖动的初始轨迹；
• 核心作用：将离散路径转为连续轨迹，为后端优化打基础。

模块3：后端——轨迹优化（平滑+约束收紧）

这是FAST Planner的核心创新点，分为两步优化：

第一步：几何优化（避障+平滑）

• 建模方式：将轨迹优化转为带约束的优化问题：
  • 优化目标：轨迹长度最短 + 曲率最小 + 偏离全局路径最小；
  • 硬约束：无碰撞（障碍距离>安全阈值）、道路边界、最小转弯半径；
  • 软约束：曲率连续、jerk最小（提升平顺性）。
• 求解算法：QP（二次规划）/ SQP（序列二次规划）；
  • QP：适合线性约束（如避障、速度限制），求解速度快；
  • SQP：适合非线性约束（如矿车侧翻约束、坡道力约束），精度高。

第二步：动力学优化（适配车辆执行）

• 核心逻辑：将几何轨迹与车辆动力学绑定，生成“可执行”的速度/加速度曲线；
• 矿车关键约束：
  1. 最大/最小速度（矿区干线30km/h，窄路10km/h）；
  2. 最大加速度/减速度（重载矿卡≤0.5m/s²）；
  3. 加加速度（jerk）限制（≤0.2m/s³，避免货料撒落）；
  4. 坡道补偿：上坡降速、下坡限速（防溜车）。
• 求解算法：ST图（s-t graph）速度规划（与Apollo/Lattice逻辑一致）。

模块4：实时重规划触发机制

FAST Planner的“Fast”核心在于增量式重规划，而非全局重规划：

• 触发条件：
  1. 感知到新障碍（如矿区突然出现的落石/工程车）；
  2. 车辆偏离当前轨迹超过阈值；
  3. 全局路径更新（如作业点切换）；
• 重规划范围：仅在“当前位置 + 前瞻30-50m”的局部区域重规划（矿卡前瞻长度）；
• 优势：耗时<20ms，满足矿卡10-20Hz的规划频率要求。

---

三、FAST Planner 核心创新点（区别于其他规划器）

1. 速度优化：BIT* + 增量重规划

• 比传统RRT*快5-10倍，单帧搜索耗时<10ms；
• 适配矿区“动态障碍多、环境变化快”的特点。

2. 安全保障：多层碰撞检测

• 粗路径阶段：离散点碰撞检测（快速）；
• 初始轨迹阶段：连续轨迹碰撞检测（精确）；
• 优化阶段：障碍膨胀检测（矿卡大包围盒，含货箱/悬臂）。

3. 平滑性：jerk连续的多项式优化

• 轨迹的位置、速度、加速度、加加速度均连续；
• 完全适配矿卡“重载、低速、忌冲击”的执行特性。

4. 鲁棒性：无地图/弱地图适配

• 无需高精地图，仅需激光/视觉生成的局部代价地图；
• 完美匹配矿区“无车道线、临时路面、软基”的非结构化环境。

---

四、FAST Planner 适配无人矿卡的关键改造点

FAST Planner原生面向无人机/小型无人车，适配矿卡需修改以下核心点：

1. 动力学约束替换（最核心）

原生参数（无人机/小车）	矿卡适配参数
最大转弯半径：1-2m	最大转弯半径：10-20m（矿卡轴距长）
最大加速度：2-5m/s²	最大加速度：0.3-0.5m/s²（重载）
碰撞包围盒：圆形/小矩形	碰撞包围盒：大矩形+外摆差（矿卡长×宽×高）
前瞻长度：5-10m	前瞻长度：30-50m（矿卡制动距离长）

2. 采样策略调整

• 减少垂直于行驶方向的采样（矿卡横向机动少）；
• 增加“坡道启发采样”：上坡路段优先采样低速轨迹，下坡优先采样制动轨迹；
• 采样步长：0.5-1m（干线）/ 0.2-0.5m（装卸对位）。

3. 代价函数定制（矿卡优先级）

FAST Planner原生代价函数：平滑性 > 效率 > 安全；
矿卡适配后：安全 > 可行 > 平滑 > 效率；
具体权重：

• 碰撞代价：权重100（最高）；
• 侧翻风险代价：权重80；
• 偏离压实路面代价：权重50；
• 平滑性代价：权重20；
• 效率代价：权重10。

4. 安全兜底机制

• 增加“规划失败→平滑停车”逻辑：当无可行轨迹时，立即生成减速停车轨迹；
• 多轨迹备份：同时生成3条候选轨迹，主轨迹失效时毫秒级切换。

---

五、FAST Planner 矿卡落地工程要点

1. 实时性优化

• 采样点数：控制在500-1000个（过多耗时，过少不可行）；
• 碰撞检测：用栅格地图+包围盒快速检测（替代逐点检测）；
• 算力要求：嵌入式平台（如NVIDIA Xavier）即可满足，无需高端GPU。

2. 与局部规划的集成

• FAST Planner作为“局部避障规划器”，承接全局路径；
• 输出轨迹后，可叠加QP精修（与Apollo/Autoware逻辑一致）；
• 最终输出给MPC跟踪控制器，实现轨迹跟踪。

3. 测试验证重点

• 场景1：矿区干线避障（会车/绕落石）；
• 场景2：装卸点对位（Hybrid A替代RRT，提升对位精度）；
• 场景3：坡道紧急避障（验证减速/停车轨迹的平顺性）。

---

总结

核心要点回顾

1. FAST Planner核心：以“改进版RRT*/BIT*采样”找可行路径，以“多项式+QP优化”做平滑，兼顾速度与安全；
2. 矿卡适配关键：替换动力学约束、调整采样策略、重构代价函数（安全优先）；
3. 落地价值：适配矿区非结构化环境，实时性满足矿卡硬要求，是除Lattice/Hybrid A*外的优质选择。

矿卡工程选型建议

• 干线行驶：Lattice Planner（稳定）为主，FAST Planner为避障备份；
• 窄路/会车/避障：FAST Planner为主（实时性强）；
• 装卸对位：Hybrid A*（精度高）+ FAST Planner平滑。

1. FAST Planner适配矿卡的核心参数配置表；
2. FAST Planner与Lattice Planner的矿卡场景切换逻辑；
3. FAST Planner矿卡版的核心代码改造片段（C++）。