12 KiB
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
│ └── 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 是地图模块的唯一对外构造入口：
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大文件追加规划功能。