ParkingRobot/docs/superpowers/specs/2026-07-26-hybrid-astar-coarse-path-design.md

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 集成/可视化入口都必须提供。
• 每个文件只承担一个明确职责；对外调用通过模块门面类完成，不让调用者拼装搜索、地图和碰撞的内部对象。

目录和命名空间

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 模块唯一的公共创建入口：

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

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

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 是地图模块的唯一对外构造入口：

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 是地图与规划器之间唯一的单位/数据边界：

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

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

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

CoarsePath 模块

对外门面与契约

调用方只调用 HybridAStarPlanner：

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 大文件追加规划功能。