# Hybrid A* 粗路径规划设计

## 目标与范围

在现有栅格地图能力之上，实现四舵轮 AMR 的 Hybrid A* 空间粗路径：支持前进、倒车、换向、静态/投影障碍绕行、车体碰撞检查、可配置的终点位置与航向容差，以及稠密路径与方向分段输出。

本阶段**不包含**曲线平滑、B 样条、Bezier、局部 QP、SQP、Reeds-Shepp 精确终点连接、速度/加速度/时间参数化或底盘舵角控制。它们由后续独立模块处理。

## 已确认的约束

- 地图与外部人工障碍输入继续使用现有世界坐标单位：mm。
- Hybrid A* 内部统一使用 m、rad、1/m；单位转换只能经过地图适配与 `Utils`，不能散落在搜索代码中。
- 人工障碍物第一版支持以几何中心放置的轴对齐矩形和圆形。
- `TwoLegDetect` 是可选障碍物输入，不是规划地图是否可用的唯一条件；它的检测结果经投影后与人工障碍物合并。
- 环境占据地图只保存外部障碍物，绝不写入 AMR 自身足迹。AMR 尺寸、当前位姿和安全余量只用于粗路径的车体碰撞检测。
- 障碍物不做安全膨胀；安全余量仅通过碰撞检查时的扩大车辆矩形应用，防止双重膨胀。
- 第一版的终点条件为可配置的位置容差和车头航向容差。默认建议为 0.15 m 与 5°，而非精确连接到目标位姿。
- 测试是交付的一部分：纯逻辑契约测试与参照 `Map` 的 `MovementTest` 集成/可视化入口都必须提供。
- 每个文件只承担一个明确职责；对外调用通过模块门面类完成，不让调用者拼装搜索、地图和碰撞的内部对象。

## 目录和命名空间

```text
ClumsyPilot/ParkrobTrajplanner/
├── Initial_plan/                         # 已有路线与方案文档，不放运行时代码
├── Utils/                                # MultiWheelC.TrajectoryPlanning.Utils
│   ├── AngleMath.cs
│   ├── UnitConverter.cs
│   ├── CoordinateTransform.cs
│   ├── NumericGuard.cs
│   └── GridIndex.cs
├── Map/                                  # MultiWheelC.TrajectoryPlanning.Mapping
│   ├── Core/
│   │   ├── EnvironmentGridMap.cs
│   │   ├── MapBuildRequest.cs
│   │   ├── EnvironmentMapBuildResult.cs
│   │   └── EnvironmentMapBuilder.cs
│   ├── Obstacles/
│   │   ├── IMapObstacle.cs
│   │   ├── AxisAlignedRectangleObstacle.cs
│   │   ├── CircleObstacle.cs
│   │   └── MapObstacleRasterizer.cs
│   ├── Sources/
│   │   ├── ManualObstacleSource.cs
│   │   └── TwoLegObstacleProjector.cs
│   ├── Planning/
│   │   ├── PlanningGridMap.cs
│   │   ├── ObstacleDistanceField.cs
│   │   └── PlanningMapAdapter.cs
│   ├── PlanningMapRequest.cs
│   ├── PlanningMapBuildResult.cs
│   └── PlanningMapFactory.cs
│   └── Test/
│       └── MovementTest.MapTest.cs
├── CoarsePath/                           # MultiWheelC.TrajectoryPlanning.CoarsePath
│   ├── Contracts/
│   │   ├── Pose2D.cs
│   │   ├── VehicleParameters.cs
│   │   ├── PlanningRequest.cs
│   │   ├── HybridAStarConfiguration.cs
│   │   ├── PlanningResult.cs
│   │   ├── PlanningStatus.cs
│   │   ├── CoarsePathPoint.cs
│   │   └── PathSegment.cs
│   ├── Vehicle/
│   │   ├── VehicleKinematics.cs
│   │   ├── HeadingFootprintTemplate.cs
│   │   ├── HeadingFootprintTemplateCache.cs
│   │   └── FootprintCollisionChecker.cs
│   ├── Search/
│   │   ├── MotionPrimitive.cs
│   │   ├── MotionPrimitiveGenerator.cs
│   │   ├── HybridAStarNode.cs
│   │   ├── HybridAStarNodeKey.cs
│   │   ├── GoalToleranceChecker.cs
│   │   ├── GridDijkstraHeuristic.cs
│   │   └── HybridAStarSearch.cs
│   ├── Output/
│   │   ├── PathBacktracker.cs
│   │   ├── CoarsePathAssembler.cs
│   │   └── CoarsePathValidator.cs
│   ├── HybridAStarPlanner.cs
│   └── Test/
│       ├── CoarsePathScenarioFactory.cs
│       └── MovementTest.CoarsePathTest.cs
└── README.md                             # 只说明模块边界、调用入口与文档链接

ClumsyPilot/tests/
├── verify_planning_utils.ps1
├── verify_planning_map_adapter.ps1
├── verify_coarse_path_search.ps1
└── verify_coarse_path_integration.ps1
```

`Occupancygird_Map/Map_test` 是当前原型位置。实施时会把其中仍然需要的地图能力按以上职责迁移到 `Map`，避免继续向两个现有大文件叠加功能；图片导出可以保留为独立地图测试辅助，不成为规划器依赖。

## Utils

`Utils` 只放无状态、确定性的通用计算，不依赖 Clumsy UI、地图、传感器或搜索器。

| 文件 | 唯一职责 |
| --- | --- |
| `AngleMath` | 角度归一化、最小有符号角差、度/弧度边界规则、航向离散索引。 |
| `UnitConverter` | mm↔m、deg↔rad、曲率/半径转换；不包含业务配置。 |
| `CoordinateTransform` | 车体系与世界系的二维刚体变换。 |
| `NumericGuard` | 有限值、正值和范围校验的可复用方法。 |
| `GridIndex` | 行列索引值对象与比较，不存储地图状态。 |

## Map 模块

### 对外调用门面

粗路径模块不直接实例化 `EnvironmentMapBuilder`、`MapObstacleRasterizer`、`TwoLegObstacleProjector` 或 `PlanningMapAdapter`。`PlanningMapFactory` 是 `Map` 模块唯一的公共创建入口：

```csharp
public sealed class PlanningMapFactory
{
    public PlanningMapBuildResult Create(PlanningMapRequest request);
}
```

粗路径调用只依赖其稳定输出：

```csharp
var mapResult = new PlanningMapFactory().Create(mapRequest);
if (!mapResult.Succeeded)
    return PlanningResult.FromMapFailure(mapResult);

var result = new HybridAStarPlanner().Plan(new PlanningRequest
{
    Map = mapResult.Map,
    Start = startPose,
    Goal = goalPose,
    Vehicle = vehicle,
    Configuration = configuration,
});
```

`PlanningMapRequest` 集中地图边界/分辨率、人工障碍物、可选 `TwoLegDetect` 投影输入和空旷地图声明；`PlanningMapBuildResult` 返回 `Succeeded`、失败原因、每个障碍物来源的摘要，以及成功时不可变的 `PlanningGridMap`。因此 A* 的 `PlanningRequest.Map` 始终是已完成校验、mm→m 适配、距离场生成的输入，规划器不需要了解地图构造细节。

### 环境地图与图层

`EnvironmentGridMap` 保存 mm 单位的地图边界、分辨率和仅含外部障碍物的占据单元。它不接受 `MarkVehicleFootprint` 一类接口。

`EnvironmentMapBuilder` 是地图模块的唯一对外构造入口：

```csharp
public sealed class EnvironmentMapBuilder
{
    public EnvironmentMapBuildResult Build(MapBuildRequest request);
}
```

它依次校验地图参数、栅格化人工障碍物、投影可用的 `TwoLegDetect` 结果、合并占据单元，并报告每个来源是否生效。任何可选传感器输入失败都不会删除已成功构建的人工地图。

人工障碍物实现 `IMapObstacle`。`AxisAlignedRectangleObstacle` 使用 `(CenterXmm, CenterYmm, WidthMm, HeightMm)`；`CircleObstacle` 使用 `(CenterXmm, CenterYmm, RadiusMm)`。两者的尺寸必须为有限正数，矩形轴与世界 X/Y 轴对齐。`MapObstacleRasterizer` 是唯一直接写入环境栅格的类。

`TwoLegObstacleProjector` 仅负责将检测坐标通过现有车体到世界系变换变成 `CircleObstacle`，不负责地图边界、栅格化或规划可用性判断。

### 规划适配

`PlanningMapAdapter` 是地图与规划器之间唯一的单位/数据边界：

```csharp
public sealed class PlanningMapAdapter
{
    public PlanningGridMap Adapt(EnvironmentGridMap map);
}
```

适配时验证边界和分辨率、深拷贝占据数据、将长度从 mm 转为 m、将边界外固定解释为占据，并生成 `ObstacleDistanceField`。`PlanningGridMap` 是不可变的规划输入，包含 m 单位边界、分辨率、行列、占据数据、距离场、源地图版本与 `PlanningReady/PlanningBlockReason`。

`PlanningReady` 的规则：有效的人工图层即可使地图可用于测试/规划；若没有人工障碍也没有 `TwoLegDetect`，在已明确配置“空旷地图”时仍可规划；未明确空旷语义的未观测区域则以 `PlanningBlockReason` 阻止规划，而不是暗中当作空闲。

## CoarsePath 模块

### 对外门面与契约

调用方只调用 `HybridAStarPlanner`：

```csharp
public sealed class HybridAStarPlanner
{
    public PlanningResult Plan(PlanningRequest request);
}
```

`PlanningRequest` 组合 `PlanningGridMap`、起点 `Pose2D`、目标 `Pose2D`、`VehicleParameters` 与 `HybridAStarConfiguration`。它不接受地图构建器、UI 对象或传感器对象。

`HybridAStarConfiguration` 集中所有可调参数，包括最大节点数、超时、航向分辨率、原语长度、积分步长、曲率等级、代价权重、是否允许倒车、位置容差和航向容差。初始值遵循已有技术方案：0.50 m 原语、0.05 m 积分、5° 航向离散、五级曲率、0.15 m 位置容差、5° 航向容差。

`PlanningResult` 始终返回明确 `PlanningStatus`、诊断信息、零或一条 `IReadOnlyList<CoarsePathPoint>` 和 `IReadOnlyList<PathSegment>`。粗路径不包含时间、速度、加速度、舵轮角或轮速。

### 车辆、碰撞和搜索

- `VehicleKinematics` 根据车辆参数提供最大曲率；直接最大曲率和最小转弯半径同时存在时采用更保守的值。
- `HeadingFootprintTemplateCache` 为每一个离散航向预计算扩大车辆矩形覆盖的相对栅格偏移；扩大尺寸只在这里应用安全余量。
- `FootprintCollisionChecker` 依次检查地图边界、距离场快速安全放行和精确矩形模板；它不执行搜索，也不改变地图。
- `MotionPrimitiveGenerator` 仅生成恒曲率前进/倒车原语，并以不大于 0.05 m 的步长积分，保留内部积分点。
- `GoalToleranceChecker` 只判断位置、航向和最后一段方向约束；位置和航向阈值来自 `HybridAStarConfiguration`。
- `GridDijkstraHeuristic` 仅从目标在占据图上生成二维绕障距离启发；它不处理车辆运动学。
- `HybridAStarSearch` 管理 Open List、Closed Set、节点扩展、代价、父索引与终点选择。Closed Set 键为位置格、航向格、方向和曲率等级。它不拼装最终路径。
- `PathBacktracker` 从成功节点恢复原语的内部积分点；`CoarsePathAssembler` 去除相邻重复点、累计弧长、标记换向点并构造 `PathSegment`；`CoarsePathValidator` 用同一碰撞规则复核最终稠密输出。

第一版允许前进、倒车和换向。换向只可发生在原语边界；相邻原语曲率等级最多变化一级。目标达到容差即成功，不尝试 Reeds-Shepp 精确连接。

## 状态与失败处理

`PlanningStatus` 至少区分：`Success`、`InvalidRequest`、`InvalidMap`、`MapNotReady`、`InvalidVehicleParameters`、`InvalidCurvatureConfiguration`、`StartOutsideMap`、`StartInCollision`、`GoalOutsideMap`、`GoalInCollision`、`SearchTimeout`、`SearchNodeLimitExceeded`、`NoFeasiblePath`、`BacktrackingFailed`、`FinalValidationFailed` 与 `InternalError`。

所有输入错误在开始搜索前返回状态与可读原因；搜索或地图对象不能通过异常把部分路径发布给调用方。`PlanningDiagnostics` 记录扩展节点数、生成节点数、总路径长度、最小净空、耗时和终止原因，供之后的性能优化使用。

## 测试设计

纯逻辑测试沿用当前 PowerShell 反射契约测试方式，确保在项目的 `netstandard2.0` 与 Clumsy 引用环境中验证真实程序集。

| 测试文件/入口 | 覆盖内容 |
| --- | --- |
| `verify_planning_utils.ps1` | mm/m、deg/rad、角度环绕、车体/世界坐标变换和非法数值。 |
| `verify_planning_map_adapter.ps1` | 圆与轴对齐矩形的中心放置和栅格化、人工与 TwoLeg 图层合并、AMR 自身不占据环境图、边界外保守占据、mm→m 深拷贝、距离场与 `PlanningReady`。 |
| `verify_coarse_path_search.ps1` | 无障碍前进、单障碍绕行、允许倒车的狭窄场景、换向标记、位置/航向容差、越界/起终点碰撞/无解、曲率与 0.05 m 稠密点复核。 |
| `verify_coarse_path_integration.ps1` | 由人工障碍地图构建、适配、规划、最终验证的端到端结果与诊断。 |
| `MovementTest.MapTest` | 在 Clumsy UI 中显示人工与 TwoLeg 投影后的环境栅格；可选 PNG 导出只用于调试证据。 |
| `MovementTest.CoarsePathTest` | 使用固定可复现实例调用 `HybridAStarPlanner`，绘制地图、起终点、扩大车体检查点和粗路径；不向底盘发送运动命令。 |

每个新增公共契约均需有成功、边界和失败三类测试。测试场景由 `CoarsePathScenarioFactory` 统一生成，不在 `MovementTest` 中手写地图、原语或搜索细节。

## 非目标与迁移边界

- 不修改 `TrajPlanner` 下的 Python 原型，也不把它作为运行时依赖。
- 不在本阶段实现平滑、SQP、时间轨迹或控制接口；后续模块只消费 `PlanningResult` 中稳定的粗路径与方向分段。
- 不保留将车辆自身写入规划占据图的兼容开关；若调试可视化需要车辆图形，应作为渲染叠加层。
- 当前地图文件中的职责会按以上边界迁移；不会在迁移后继续向原 `Map_test` 大文件追加规划功能。