# Local G2 五次 Hermite 路径平滑算法说明

> 文档定位：说明当前 Local G2 五次 Hermite 路径平滑任务的算法结构、输入、输出和实现边界。  
> 证据范围：截至 2026-07-31 的实现计划、代码结构和已运行专项检查。

## 1. 目标、位置与非目标

### 1.1 它要解决什么问题

Hybrid A* 粗路径由离散运动基元连接而成。它能提供可行的车辆空间路径，但相邻基元交界处可能出现车辆曲率的突变。曲率突变会使后续连续优化或控制模块得到不够平滑的初值。

Local G2 五次 Hermite 预平滑的目标，是只在这些局部曲率突变附近，用一小段五次 Hermite 曲线替换原有几何，使连接处的位置、切向趋势和曲率更连续，并且不牺牲车辆碰撞安全、净空和方向切换拓扑。

### 1.2 它在系统中的位置

    成功的 Hybrid A* 粗路径
              ↓
    Local G2 五次 Hermite 预平滑
              ↓
    后续 SQP 或时间参数化模块的空间初始路径

这里的 Local 表示只修改必要的局部窗口；G2 表示曲线连接处不仅位置相连、切向连续，还要让曲率连续；五次 Hermite 表示每一小段曲线由两端的位置、一阶导数和二阶导数确定。

### 1.3 本算法不做什么

- 不重新搜索 Hybrid A*，不改变其代价、地图模型或运动基元生成策略。
- 不跨越前进/倒车换向点做曲率计算或曲线拼接。
- 不做 SQP 求解、速度规划、加速度规划、轮速分解或动态障碍物预测。
- 不承诺当前代码已经把 Local G2 结果正式发布给下游。当前完成的是候选构造和评价层；专用发布流水线仍待接入。

## 2. 输入：进入算法前必须具备什么

Local G2 的业务入口设计基于一个路径平滑请求。它需要的输入如下。

| 输入 | 作用 | 关键要求 |
|---|---|---|
| 成功的 Hybrid A* 粗路径 | 给出原始空间轨迹与每个点的车辆曲率 | 路径点数值有限，弧长非递减 |
| 路径方向段 | 将前进段、倒车段隔离 | 段索引连续，段内方向一致，换向点不被跨越 |
| 地图 | 用于完整车体碰撞和净空复核 | 地图查询可用 |
| 车辆参数 | 提供车体外形、曲率上限和安全边界 | 曲率约束有效 |
| LocalG2QuinticOptions | 控制窗口、偏移、质量门和候选上限 | 所有参数通过有限值与范围校验 |
| 公共平滑配置 | 控制输出采样、碰撞步长、净空余量 | 输出采样和碰撞步长必须为正 |
| 取消令牌 | 允许耗时计算被安全取消 | 每个候选和区域循环均应检查 |

### 2.1 坐标和单位

- 位置、路径长度、窗口长度、偏移和净空：米 m。
- 航向：弧度 rad。
- 车辆曲率 κ：1/m。
- 曲率导数 dκ/ds：1/m²，表示沿路径走一米时曲率变化得有多快。

### 2.2 Local G2 的默认安全参数

| 参数 | 默认值 | 含义 |
|---|---:|---|
| 最小 / 偏好 / 最大窗口长度 | 0.20 / 0.50 / 0.80 m | 单个局部替换允许的长度范围 |
| 最大局部偏移 | 0.10 m | 候选曲线不得偏离原始窗口过远 |
| 曲率跳变阈值 | max(0.001，0.05 × 最大车辆曲率) | 过滤数值噪声，只处理足够明显的跳变 |
| 峰值 dκ/ds 最低改善 | 20% | 候选必须明显降低曲率变化尖峰 |
| 曲率变化代价容差 | 2% | 不允许用新的高频摆动换取表面平滑 |
| 每区域候选上限 | 12 | 保证计算量与结果可重复 |
| 输出采样 / 碰撞检查步长 | 0.025 / 0.025 m | 统一几何分析与连续车体复核的精度 |
| 最小净空余量 | 0.02 m | 候选与障碍物必须保留的安全距离 |

## 3. 模块架构：每个模块负责什么

| 模块 | 职责 | 当前状态 |
|---|---|---|
| PathSmoothingPreprocessor | 校验粗路径，将其拆成不跨换向点的方向段，并保留真实起始车辆曲率 | 已实现 |
| PathGeometryAnalyzer | 统一重采样，恢复航向，计算 κ、dκ/ds、曲率变化代价等指标 | 已实现 |
| CurvatureTransitionDetector | 在同一方向段内检测相邻粗路径点的曲率跳变 | 已实现 |
| LocalG2WindowPlanner | 为跳变规划受限局部窗口，并合并可能相互影响的事件 | 已实现 |
| QuinticHermiteCurve2D | 由两端的位置、切向导数、二阶导数构造二维五次 Hermite 曲线 | 已实现 |
| LocalG2CandidateBuilder | 生成有限数量的候选曲线，处理前进/倒车曲率符号 | 已实现 |
| LocalG2PathSplicer | 将一个局部候选替换进完整方向段，同时保持换向拓扑 | 已实现 |
| LocalG2CandidateEvaluator | 执行偏移、曲率、碰撞、净空、改善度和确定性排序检查 | 已实现 |
| SmoothedPathValidator | 以完整车体模型复核候选路径和相邻点扫掠安全 | 已实现 |
| LocalG2PreSmoothingPipeline | 顺序处理全部区域、局部回退、全局回滚并发布最终结果 | 尚未实现 |
| PathSmoothingService 的 Local G2 分派 | 将 LocalG2Quintic 请求送入专用流水线 | 尚未接入 |

## 4. 数据流：从粗路径到候选安全路径

### 步骤 1：预处理与方向隔离

粗路径先按前进和倒车拆成方向段。换向点两侧允许同一车辆位姿重复出现，但绝不被当作同一条连续曲线的一部分。这样可以避免把停车换向误判为需要平滑的几何折点。

### 步骤 2：检测曲率跳变

检测器只比较同一方向段内相邻粗路径点的车辆曲率。当差值达到阈值时，记录曲率事件：它所在的段、左右点、局部弧长、车辆航向，以及跳变前后的曲率。

### 步骤 3：规划局部窗口

窗口规划器围绕每个事件产生不同长度和左右分配的窗口方案。相邻事件若可能相互影响，会被归入同一个区域；每个区域最多输出 12 个确定性窗口/尺度组合，避免随机结果。

### 步骤 4：构造五次 Hermite 候选

对一个窗口，候选构造器在窗口两端以及必要的内部曲率事件处建立边界节点。每个节点包含位置、车辆航向、车辆曲率和导数尺度。对每对相邻节点构造一段五次 Hermite 曲线并按 0.025 m 左右的空间间距采样。

直观理解：原始路径像由短直线和圆弧拼出的折线化轨迹；五次 Hermite 曲线像在局部用一条可控的柔性曲线接住两边，使方向和转弯程度逐步过渡。

### 步骤 5：拼接候选

拼接器保留窗口之前和之后的原始点，用精确插值得到窗口端点，再插入候选点。随后重新计算该方向段的局部弧长，保持段首、段尾和换向标志不变。

### 步骤 6：统一几何分析和完整车体验证

候选路径与原始窗口都通过同一个几何分析器计算指标，再由完整车体碰撞检查器验证每个姿态和相邻姿态之间的扫掠。这样“看起来更圆滑”的曲线不会绕过实际车体安全检查。

### 步骤 7：质量门与选优

一个候选只有同时通过以下检查，才可被接受：

1. 不超过车辆曲率上限，也不超出原始局部曲率范围。
2. 到原始窗口折线的最大偏移不超过 0.10 m。
3. 完整路径碰撞检查通过，最小净空不少于 0.02 m。
4. 峰值 dκ/ds 至少降低 20%。
5. 曲率变化代价不比原始窗口增加超过 2%。

若多个候选合格，按最大偏移、峰值 dκ/ds、曲率变化代价、路径长度变化和候选索引依次排序，确保结果可重复。

## 5. 输出：路径、段、指标、区域报告与状态

### 5.1 候选层已经能够产生的结果

候选评价层可以得到一条经过完整车体复核的候选安全路径，以及对应的方向段和局部指标。每个输出路径点包含：

- 位置、航向、展开航向和全局弧长；
- 前进或倒车方向；
- 几何曲率、车辆曲率、车辆曲率导数 dκ/ds；
- 重新计算的车体净空；
- 是否为换向点，以及该点来自原始锚点、插值点或 Local G2 过渡点。

每个方向段则记录其段号、方向、路径点的起止索引，以及换向拓扑标志。

### 5.2 计划中的正式发布结果

公共契约已为 Local G2 预留以下最终状态：

| 状态 | 计划含义 |
|---|---|
| NotNeeded | 未检测到需要处理的曲率跳变，发布已验证的原始路径 |
| Complete | 所有区域都得到有效改善 |
| PartialImprovement | 只有部分区域得到有效改善，其余区域保留原样 |
| Unchanged | 检测到事件，但没有候选能安全通过质量门 |

正式结果还应包含每个区域的报告：窗口范围、候选数、被选候选、原始/结果峰值 dκ/ds、曲率变化代价、最大偏移、最小净空和保留原路径的原因。

### 5.3 当前输出边界

上述最终状态和区域报告的契约已经存在，但负责顺序处理多个区域、必要时全局回滚、并由服务发布结果的 LocalG2PreSmoothingPipeline 尚未实现。因此，当前代码的准确描述是“可生成和评价局部 G2 候选”，而不是“已经对外发布完整 Local G2 平滑路径”。

## 6. 安全与质量门

Local G2 的核心原则不是尽可能把曲线画圆，而是在严格约束下只接受确实更好的局部替换。

- **方向安全**：不跨换向点，前进/倒车的曲率符号转换只做一次。
- **几何安全**：候选端点必须与原始窗口精确对应，避免拼接留下缝隙或重复点。
- **车辆安全**：逐点与逐段扫掠检查完整车体，而不是只检查轨迹中心线。
- **净空安全**：以本次检查的净空为准，不信任候选输入自带的净空值。
- **平滑质量**：以 dκ/ds 的峰值和曲率变化代价衡量改善，避免引入新的摆动。
- **确定性**：固定窗口、固定导数尺度顺序和固定排序规则，便于复现与诊断。

## 7. 当前实现进度与边界

实施计划共 9 个任务。任务 1 至任务 7 已完成公共契约、真实起始曲率保留、统一 dκ/ds 分析、事件检测、窗口规划、五次 Hermite 几何、候选拼接和质量评价。

已重新执行并通过的专项检查包括：

- 路径平滑契约检查；
- 粗路径集成与搜索检查；
- 路径平滑几何、验证器、服务和比较检查；
- Local G2 跳变检测、五次曲线和候选质量检查。

任务 8 的专用流水线、服务分派和 Local G2 端到端夹具检查尚未实现；任务 9 的完整回归与主路线文档也尚未完成。另有一个共享原始基线回归会在第二份报告中单独说明：RectangleDetour 场景在进入 G2 前的基线复验失败。

## 8. 证据索引

- Local G2 实施计划：docs/superpowers/plans/2026-07-30-local-g2-path-presmoothing.md
- Local G2 设计说明：docs/superpowers/specs/2026-07-30-local-g2-path-presmoothing-design.md
- Local G2 实现目录：ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/
- 统一几何分析：ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalyzer.cs
- 完整车体验证：ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Validation/SmoothedPathValidator.cs
- 专项检查：ClumsyPilot/tests/verify_path_smoothing_local_g2_detection.ps1、verify_path_smoothing_local_g2_curve.ps1、verify_path_smoothing_local_g2_candidates.ps1
