# 局部 G2 路径预平滑设计

**日期：** 2026-07-30
**状态：** 已完成方案评审，等待用户评审书面规范
**范围：** Hybrid A* 粗路径与后续 SQP 路径优化之间的局部预平滑

## 1. 背景与问题

当前 Hybrid A* 使用离散曲率等级生成运动基元。单个运动基元是直线或恒曲率圆弧，路径在位置和航向上连续，但相邻运动基元的车辆曲率可能直接跳变。

当前典型配置为：

- 运动基元长度：`0.50 m`
- 路径点间距：通常不大于 `0.025 m`
- 最大车辆曲率：约 `0.8333 1/m`
- 相邻曲率等级的典型跳变量：约 `0.4167 1/m`

因此粗路径视觉上可能已经比较平缓，但仍可能要求车轮转角瞬间改变。当前 PathSmoothing 中的算法不能稳定解决该问题：

- 局部三次 Bézier 通过相邻线段夹角检测拐点。Hybrid A* 相邻采样点的航向变化很小，算法经常检测不到需要处理的运动基元边界。
- 三次 B 样条和旧分段五次曲线可能修改整段路径、切弯或产生曲率过冲。
- 当前运行器只检查候选是否安全，没有要求候选在平滑质量上优于粗路径。
- 无变化甚至更差的候选仍可能被报告为成功。
- 粗路径和平滑路径没有始终使用同一个几何分析器计算曲率，比较基线不公平。

## 2. 目标

新增一条单一主路线：使用局部、分段、二维参数化五次 Hermite 曲线，低成本地消除容易处理的曲率跳变，并将所得完整路径作为 SQP 的更好初值。

具体目标：

1. 直接检测同一行驶方向段内部的运动基元曲率跳变。
2. 只修改突变点附近的局部区域，保留本来已经平滑的恒曲率圆弧。
3. 在局部曲线与原粗路径连接处保证位置、切线和曲率连续，即达到 G2 连续。
4. 对重叠的局部窗口进行联合处理，避免独立平滑后再次产生接缝。
5. 对每个候选同时执行碰撞、车辆几何、偏移和质量验证。
6. 允许只完成部分区域；部分改善路径可以作为 SQP 初值，但不能伪装成全部完成。
7. 输出可靠的空间路径、曲率和曲率对弧长的导数，不输出速度或时间参数。

## 3. 非目标

本次工作不包含：

- 修改 Hybrid A* 的搜索模型、几何中心参考点或运动基元生成方法。
- 实现或修改 SQP 路径优化器。
- 实现速度规划。
- 计算四个车轮的轮速或转角。
- 根据最大转向角速度直接输出每点速度上限。
- 修复或重写现有三次 B 样条、局部三次 Bézier和旧分段五次算法。
- 将预平滑输出直接定义为最终控制路径。
- 第一版约束转向角加速度或要求 `dκ/ds` 自身连续。

现有三种平滑算法可以保留用于离线比较，但不参与本主路线向 SQP 发布的路径选择。

## 4. 系统定位与数据流

主数据流为：

```text
Hybrid A* 粗路径
    ↓
输入标准化与行驶方向硬分段
    ↓
曲率跳变检测
    ↓
局部窗口规划与重叠窗口合并
    ↓
局部 G2 五次 Hermite 候选生成
    ↓
候选硬约束验证与质量比较
    ↓
成功区域替换、失败区域保留粗路径
    ↓
完整路径统一重采样与全局复验
    ↓
Complete / PartialImprovement / NotNeeded / Unchanged / Failed
    ↓
SQP 初始路径
    ↓
最终验证、速度规划和控制
```

对于通过输入复核的有效粗路径，PathSmoothing 始终向下游提供一条完整路径。局部处理失败不能造成路径缺口。若输入粗路径本身无效，则返回 `Failed` 且不发布路径。

## 5. 输入契约

### 5.1 必需输入

每个粗路径点至少包含：

- 位置 `x, y`
- 车辆航向 `heading`
- 行驶方向 `TravelDirection`
- 累计弧长 `s`
- Hybrid A* 车辆曲率 `κ_vehicle`
- 换向点标记
- 车体净空或可供重新检查净空的地图上下文

请求还应包含：

- Hybrid A* 已生成的方向段集合
- 规划栅格地图
- 车辆外形、安全边距和最大曲率
- 局部 G2 预平滑配置

### 5.2 起点曲率修正

当前 `CoarsePathAssembler` 在存在运动基元时使用第一条运动基元曲率作为起点曲率，覆盖了回溯结果中的真实起始曲率。这会隐藏车辆当前转角与第一条运动基元之间的初始曲率跳变。

本设计要求做一个最小输入契约修正：

- 粗路径首点保留请求中的真实 `StartVehicleCurvature`。
- 第一条运动基元仍保留自己的曲率。
- 如果二者不同，预平滑器将其识别为方向段起点处的单侧曲率过渡。

此修正不改变 Hybrid A* 搜索、参考点或运动基元。

## 6. 两层分段

### 6.1 第一层：行驶方向硬分段

前进段和倒车段独立处理：

```text
前进段 | 停车换向点 | 倒车段
```

规则：

- 不跨换向点生成平滑曲线。
- 换向点的位置和车辆航向保持不变。
- 换向点两侧的曲率不要求连续。
- 换向点两侧不计算跨段 `dκ/ds`。
- 倒车时按照运动方向构造几何切线；不能直接把车辆航向当成沿路径前进方向的切线。

### 6.2 第二层：方向段内部的局部过渡窗口

在同一方向段内检测相邻运动基元的曲率跳变，并围绕每个跳变建立局部过渡窗口。窗口必须跨越跳变点，不能在跳变点将路径切开后分别平滑。

窗口外的粗路径保持不变。重叠窗口合并成一个联合平滑区域。

## 7. 曲率跳变检测

检测使用 Hybrid A* 提供的相邻运动基元车辆曲率，不使用相邻采样点夹角。

有效检测阈值为：

```text
max(
    AbsoluteCurvatureJumpFloorPerMeter,
    CurvatureJumpRatioOfMaximum * MaximumVehicleCurvaturePerMeter)
```

默认值：

- `AbsoluteCurvatureJumpFloorPerMeter = 0.001 1/m`
- `CurvatureJumpRatioOfMaximum = 0.05`

当最大曲率为 `0.8333 1/m` 时，有效阈值约为 `0.0417 1/m`。当前典型的 `0.4167 1/m` 跳变会被稳定检测，浮点噪声不会生成平滑事件。

换向点两侧的曲率差不参与检测。

## 8. 自适应过渡窗口

### 8.1 默认长度

单个曲率跳变事件使用以下总窗口长度：

- 最小：`0.20 m`
- 首选：`0.50 m`
- 最大：`0.80 m`

这里的长度是突变点左右合计的总长度，不是每一侧的长度。窗口可以左右不对称。

### 8.2 可用范围

窗口不得越过：

- 当前行驶方向段的硬边界
- 换向点
- 路径全局起点和终点

窗口可根据两侧可用路径长度、相邻事件和候选验证结果自动调整。单个事件的窗口不超过 `0.80 m`；多个事件的窗口合并后，联合区域总长度可以超过 `0.80 m`。

### 8.3 优先级

窗口策略遵循：

1. 保证完整车体无碰撞和最低安全余量。
2. 保持粗路径拓扑、方向和硬锚点。
3. 达到最低平滑改善要求。
4. 在达到要求的候选中选择偏离粗路径最小的结果。
5. 空间不足时允许后续速度规划降速，不为了保持高速强行扩大窗口。

## 9. 局部 G2 五次 Hermite 技术

### 9.1 曲线表示

使用二维参数曲线：

```text
r(u) = (x(u), y(u)), u ∈ [0, 1]
```

`x(u)` 和 `y(u)` 分别为五次多项式。不能使用 `y = f(x)`，以免无法正确处理竖直、回转和倒车路径。

### 9.2 边界状态

一个 Hermite 片段的两端状态包含：

- 位置 `P`
- 沿运动方向的单位切线 `T`
- 法向 `N`
- 有符号几何曲率 `κ_geometric`
- 参数导数尺度 `λ`

端点导数按以下关系构造：

```text
r'(u_endpoint)  = λ T
r''(u_endpoint) = λ² κ_geometric N
```

车辆曲率与几何曲率的符号转换必须统一考虑前进和倒车方向。端点位置、一阶导数和二阶导数确定后，五次 Hermite 系数唯一确定。

### 9.3 单个曲率事件

窗口外端点从稳定的原始圆弧读取位置、切线和曲率。原运动基元边界的航向作为内部硬边界值；其位置默认作为内部锚点，但允许按 `2026-07-31-local-g2-soft-anchor-candidate-recovery-design.md` 在配置偏移范围内沿运动法向生成有限软位置锚点候选。跳变两侧的两个曲率值替换为一个共享的内部目标曲率。

内部目标曲率首先按照事件左右可用长度做距离加权插值：

```text
κ_shared =
    κ_left + (κ_right - κ_left)
    * left_available_length
    / (left_available_length + right_available_length)
```

围绕该初值生成有限候选。窗口左片段和右片段在内部锚点共享位置、切线、导数尺度和曲率，使内部连接达到 G2；在参数尺度一致时达到 C2。

方向段起点处的真实起始曲率跳变只有单侧可用空间，直接以真实起始状态为左边界、稳定运动基元状态为右边界生成候选。

### 9.4 多事件联合区域

当窗口重叠时：

- 合并为一个联合区域。
- 保留各运动基元边界的航向；位置按软锚点恢复设计生成有限、同配置的内部位置候选。
- 每个内部锚点只允许一个共享曲率值。
- 相邻五次 Hermite 片段共享位置、一阶导数和二阶导数。
- 内部共享曲率和导数尺度通过有限、确定性的候选组合选取。

联合处理避免先分别生成曲线再拼接所产生的新曲率接缝。

### 9.5 第一版连续性范围

第一版要求：

- 位置连续
- 航向连续
- 曲率连续
- `|dκ/ds|` 有限并得到量化改善

第一版不要求 `dκ/ds` 连续，因为当前车辆配置只提供最大转向角速度，没有提供最大转向角加速度。

## 10. 候选生成

每个独立或联合区域只进行有限、确定性的候选搜索，不运行新的通用优化器。

候选维度包括：

- 窗口总长度
- 左右窗口长度分配
- 端点和内部锚点的导数尺度
- 内部共享曲率在距离加权初值附近的有限变化

约束：

- 每个区域最多评估 `12` 个候选。
- 候选顺序固定。
- 重复运行同一输入必须生成相同候选和相同结果。
- 达到候选上限仍无合格结果时，该区域保留粗路径并记录失败原因。

通用 `SmoothingStrength` 和旧 `RetryStrengthScales` 不控制新的局部 G2 算法。新算法使用语义明确的 `LocalG2QuinticOptions`。

## 11. 候选硬约束验证

任何一项失败都直接拒绝候选：

1. 所有数值有限，无 NaN 或无穷大。
2. 曲线无尖点、回环或错误的局部运动方向。
3. 行驶方向和换向拓扑不变。
4. 起点、终点和换向硬锚点不移动。
5. 窗口外端点的位置、切线和曲率连接满足数值容差。
6. 完整车体扫掠无碰撞。
7. 碰撞检查步长不超过 `0.025 m`。
8. 保留现有默认额外净空 `0.02 m`。
9. 车辆曲率绝对值不超过车辆最大曲率加数值容差。
10. 候选曲率不超出该区域原始最小曲率和最大曲率范围加数值容差。
11. 任意候选点到原粗路径局部折线的最近距离不超过 `0.10 m`。

最大偏移使用候选点到对应粗路径窗口折线的单向最大最近距离计算。完整车体碰撞检查仍是安全性的最终依据。

## 12. 统一几何分析与质量门槛

### 12.1 公平基线

粗路径和所有候选必须：

- 按同一个目标弧长间距重采样，默认 `0.025 m`
- 使用同一个几何分析器重新计算航向、几何曲率和车辆曲率
- 不跨换向点计算曲率变化

Hybrid A* 保存的曲率用于检测运动基元事件和提供边界条件，但不能单独作为粗路径质量统计，而候选使用几何重算结果。

### 12.2 主要指标

区域峰值曲率变化率：

```text
PeakCurvatureGradient =
    max |Δκ_vehicle / Δs|
```

成功处理的区域要求：

```text
candidatePeak
    <= rawPeak * (1 - MinimumPeakGradientImprovementRatio)
```

默认：

- `MinimumPeakGradientImprovementRatio = 0.20`

即峰值至少降低 `20%`。

### 12.3 次要指标

“曲率变化代价”定义为：

```text
CurvatureVariationCost =
    Σ (Δκ_vehicle / Δs)² Δs
```

它不是车辆能耗，用于惩罚整段路径中的快速变化和反复振荡。

成功处理的区域以及最终完整路径要求：

```text
candidateCost
    <= rawCost * (1 + MaximumVariationCostRegressionRatio)
```

默认：

- `MaximumVariationCostRegressionRatio = 0.02`

即只允许 `2%` 的数值容差，不允许实质恶化。

### 12.4 局部与全局评价

`PartialImprovement` 可能保留未处理的原始突变，因此不能要求整条路径的峰值一定下降。评价规则为：

- 每个被替换区域独立达到峰值改善和变化代价门槛。
- 未处理区域保留粗路径并报告原因。
- 完整路径的曲率变化代价不得实质恶化。
- 至少一个区域真实改善，才允许返回 `PartialImprovement`。

## 13. 候选选择

不能接受第一个通过安全检查的候选。所有有限候选评估完成后，按以下规则选择：

1. 仅保留通过全部硬约束的候选。
2. 仅保留达到最低质量改善门槛的候选。
3. 优先选择最大路径偏移最小的候选。
4. 偏移在数值容差内相同时，选择峰值 `|dκ/ds|` 更小的候选。
5. 峰值仍相同时，选择曲率变化代价更小的候选。
6. 仍相同时，选择路径长度变化更小的候选。
7. 完全相同时，按固定候选生成顺序选择。

该排序表达“达到足够平滑后，优先最小修改粗路径”。

## 14. 拼接、重采样与全局复验

区域处理完成后：

1. 成功区域替换为局部 G2 曲线。
2. 无合格候选的区域保留原粗路径。
3. 合并为完整路径，去除接缝重复点。
4. 按方向段分别以 `0.025 m` 目标间距重采样。
5. 统一重算航向、几何曲率、车辆曲率和 `dκ/ds`。
6. 对完整路径重新执行数值、方向、曲率、偏移、碰撞和净空验证。
7. 如果某个替换区域导致最终全局复验失败，回退该区域并重新复验。
8. 如果无法得到通过全局复验的完整路径，返回经过复核的原粗路径并报告 `Failed`。

## 15. 输出契约

### 15.1 完整空间参考路径

输出路径点至少包含：

- `X`
- `Y`
- `Heading`
- `TravelDirection`
- `AccumulatedArcLengthMeters`
- `GeometricCurvaturePerMeter`
- `VehicleCurvaturePerMeter`
- `VehicleCurvatureDerivativePerSquareMeter`
- 换向点或方向段边界标记

`dκ/ds` 只在单个方向段内部计算。换向点两侧分别使用单侧值或显式标记为段边界，绝不跨换向点做差分。

输出不包含：

- 每点最大速度
- 时间戳
- 线速度或加速度
- 轮速
- 车轮转角

速度规划根据最终 SQP 路径、实际最高速度、轴距和最大转向角速度重新计算速度约束。当前约定的最大转向角速度 `60°/s` 和当前最大线速度 `1 m/s` 不写入 PathSmoothing 几何算法。

### 15.2 总体状态

状态定义：

- `Complete`：所有检测到的同方向移动中曲率跳变均已处理。
- `PartialImprovement`：至少一个区域有效改善，至少一个区域保留粗路径；完整输出安全，可作为 SQP 初值。
- `NotNeeded`：不存在达到检测门槛的同方向曲率跳变。
- `Unchanged`：存在需要处理的曲率跳变，但没有任何区域获得安全且有效的改善；SQP 使用原粗路径。
- `Failed`：输入、数值计算或最终完整路径复验失败。原粗路径通过复核时回退到原粗路径；原粗路径无效时不发布路径。

`Complete` 和 `PartialImprovement` 都不能由无变化路径产生。

### 15.3 区域报告

每个检测事件或联合区域输出：

- 方向段编号
- 原始路径索引和弧长范围
- 原始曲率跳变列表
- 计划和实际窗口长度
- 左右窗口长度
- 生成候选数量
- 选中候选编号
- 区域状态
- 未处理或失败原因
- 原始与结果的峰值 `|dκ/ds|`
- 原始与结果的曲率变化代价
- 最大路径偏移
- 最小车体净空
- 最大车辆曲率

区域报告用于诊断和测试，不要求 SQP 依赖其内部结构。

## 16. 配置

保留现有通用安全配置：

- `OutputSpacingMeters = 0.025`
- `MaximumCollisionCheckStepMeters = 0.025`
- `MinimumClearanceReserveMeters = 0.02`
- `AllowFallbackToCoarsePath = true`

SQP 主链路要求 `AllowFallbackToCoarsePath = true`，以保证有效粗路径在局部预平滑没有改善时仍可继续作为 SQP 初值。该配置保留是为了兼容现有公共 API。

新增 `LocalG2QuinticOptions`：

| 配置 | 默认值 | 含义 |
|---|---:|---|
| `MinimumWindowLengthMeters` | `0.20` | 单事件最小总窗口长度 |
| `PreferredWindowLengthMeters` | `0.50` | 单事件首选总窗口长度 |
| `MaximumWindowLengthMeters` | `0.80` | 单事件最大总窗口长度 |
| `MaximumDeviationMeters` | `0.10` | 候选相对粗路径的最大偏移 |
| `AbsoluteCurvatureJumpFloorPerMeter` | `0.001` | 曲率跳变检测绝对下限 |
| `CurvatureJumpRatioOfMaximum` | `0.05` | 相对车辆最大曲率的检测比例 |
| `MinimumPeakGradientImprovementRatio` | `0.20` | 区域峰值最小改善比例 |
| `MaximumVariationCostRegressionRatio` | `0.02` | 曲率变化代价数值容差 |
| `MaximumCandidatesPerRegion` | `12` | 单区域候选数量上限 |

所有配置必须有限且满足顺序和范围约束。无效配置在开始处理前返回明确错误，不能静默修正。

## 17. 错误处理与回退

- 单个候选失败：继续评估该区域其他候选。
- 单个区域无合格候选：保留该区域粗路径，记录原因。
- 部分区域成功：全局复验通过后返回 `PartialImprovement`。
- 所有区域均无改善：返回 `Unchanged` 和原粗路径。
- 无需处理：返回 `NotNeeded` 和原粗路径。
- 输入或配置无效：返回 `Failed`，不运行候选生成。
- 完整路径复验失败：逐区域回退；仍失败则返回经过复核的原粗路径和 `Failed`。
- 如果原粗路径本身未通过输入安全复核，不允许把它作为“安全回退”发布；返回 `Failed` 并说明原始路径无效。

失败原因使用稳定枚举，并可附带人类可读说明，便于测试和日志统计。

## 18. 测试与验收

### 18.1 单元测试

必须覆盖：

- 曲率跳变检测和浮点噪声过滤
- 不跨换向点检测
- 起点真实曲率与第一运动基元曲率之间的单侧过渡
- 对称和不对称窗口
- 窗口长度裁剪
- 重叠窗口合并
- 前进与倒车的切线和曲率符号
- 五次 Hermite 端点位置、切线和曲率
- 多片段内部 G2 连接
- 尖点、回环、过冲和错误方向拒绝
- 最大偏移计算
- 统一几何分析基线
- 峰值改善和变化代价门槛
- 候选确定性排序
- 局部失败与局部回退
- 拼接去重和完整路径段覆盖
- 换向点不跨段计算 `dκ/ds`
- NaN、无穷和无效配置

### 18.2 现有场景验收

- `Straight`：`NotNeeded`，输出几何与粗路径一致。
- `SingleTurn`：`Complete`，曲率突变全部处理。
- `LargeHeadingChange`：`Complete`，无曲率过冲。
- `SBend`：`Complete` 或 `PartialImprovement`，无新增曲率振荡。
- `RectangleDetour`：`Complete` 或 `PartialImprovement`，不切入障碍物。
- `MultiObstacleDetour`：`Complete` 或 `PartialImprovement`，满足净空要求。
- `ReverseGearSwitch`：不跨换向点平滑，换向位置和航向保持不变。

### 18.3 所有输出的共同断言

- 完整车体扫掠无碰撞。
- 最小净空满足配置。
- 最大车辆曲率不超限。
- 最大路径偏移不超过 `0.10 m`。
- 每个成功区域的峰值 `|dκ/ds|` 至少降低 `20%`。
- 曲率变化代价不实质恶化。
- 起点、终点、换向点和方向拓扑保持不变。
- 输出路径完整，没有重复接缝、NaN 或无穷值。
- 相同输入重复运行的状态、路径和报告一致。
- `Complete` 和 `PartialImprovement` 至少包含一个真实改善区域。
- 无变化或更差的候选不能报告成功。

### 18.4 构建与回归

实施完成后至少运行：

- PathSmoothing 新增单元验证脚本
- 现有 PathSmoothing 合同、安全、可视化和集成验证
- CoarsePath 起点曲率与输出合同验证
- `dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore`

旧对比算法允许继续保持原有结果；本任务不以提高旧算法成功率作为验收条件。

## 19. 设计决策摘要

1. Hybrid A* 粗路径保留几何中心模型。
2. 路径按前进/倒车硬分段，换向点允许停车调转角。
3. 曲率跳变是同一方向段内部的运动基元曲率变化，不是换向边界。
4. 主算法是局部、二维、分段五次 Hermite G2 预平滑。
5. 单事件窗口默认 `0.20–0.80 m`，首选 `0.50 m`，允许不对称。
6. 重叠窗口联合处理。
7. 优先安全和最小偏移，达到最低改善后不继续追求过度平滑。
8. 最大偏移默认 `0.10 m`。
9. 区域峰值 `|dκ/ds|` 至少改善 `20%`，变化代价不得实质恶化。
10. 部分改善是合法的 SQP 初值结果，不是最终控制路径。
11. PathSmoothing 输出空间几何和 `κ、dκ/ds`，速度规划拥有全部速度约束。
12. 现有三种算法暂时保留但不参与本主路线。