From 7604d12a447dcb42cd1f612f9e2a67b7faec2b38 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Sun, 26 Jul 2026 23:02:14 +0800 Subject: [PATCH] docs: define hybrid astar coarse path architecture --- ...6-07-26-hybrid-astar-coarse-path-design.md | 200 ++++++++++++++++++ 1 file changed, 200 insertions(+) create mode 100644 docs/superpowers/specs/2026-07-26-hybrid-astar-coarse-path-design.md diff --git a/docs/superpowers/specs/2026-07-26-hybrid-astar-coarse-path-design.md b/docs/superpowers/specs/2026-07-26-hybrid-astar-coarse-path-design.md new file mode 100644 index 0000000..cc2b4ca --- /dev/null +++ b/docs/superpowers/specs/2026-07-26-hybrid-astar-coarse-path-design.md @@ -0,0 +1,200 @@ +# 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 +│ └── 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 模块 + +### 环境地图与图层 + +`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` 和 `IReadOnlyList`。粗路径不包含时间、速度、加速度、舵轮角或轮速。 + +### 车辆、碰撞和搜索 + +- `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` 大文件追加规划功能。