# Hybrid A* 粗路径规划实施计划 > For agentic workers: implement task-by-task with checkbox tracking. **目标：** 完成可独立调用的规划地图入口与 Hybrid A* 粗路径输出。所有核心功能和自动化测试通过后，最后编写 MovementTest。 **架构：** Map 的唯一公共调用类是 PlanningMapFactory；CoarsePath 的唯一公共调用类是 HybridAStarPlanner。Map 输出只读 PlanningGridMap，Planner 只接收该输入。 **技术栈：** C# 10、.NET Standard 2.0、现有 Clumsy 引用、PowerShell 反射测试、Clumsy MovementTest。 ## 全局约束 - 外部地图与障碍物使用现有世界坐标 mm；规划内部使用 m、rad、1/m。 - 环境占据图只保存外部障碍物；车体尺寸与安全余量只属于碰撞检查。 - 支持中心放置的轴对齐矩形、圆形和可选 TwoLegDetect 投影；障碍物不膨胀。 - 第一版终点使用可配置位置/航向容差；不含 Reeds-Shepp、平滑、SQP、时间轨迹与控制。 - 每个公共接口必须有成功、边界、失败测试；MovementTest 是最后一项任务。 --- ## 最终目录与职责 ~~~text ClumsyPilot/ParkrobTrajplanner/ ├── Initial_plan/ │ ├── AMR_轨迹规划技术路线_Agent版.md │ └── AMR_HybridAStar_粗路径实施计划.md ├── Utils/ # MultiWheelC.TrajectoryPlanning.Utils │ ├── AngleMath.cs # 角度归一化、角差、航向索引 │ ├── UnitConverter.cs # mm/m、deg/rad、半径/曲率 │ ├── CoordinateTransform.cs # 世界系与车体系二维变换 │ ├── NumericGuard.cs # 数值合法性校验 │ └── GridIndex.cs # 行列索引值对象 ├── Map/ # MultiWheelC.TrajectoryPlanning.Mapping │ ├── Core/ │ │ ├── EnvironmentGridMap.cs # 仅外部障碍物的 mm 栅格 │ │ ├── MapBuildRequest.cs # 环境图构造参数 │ │ ├── EnvironmentMapBuildResult.cs # 环境图构造结果 │ │ └── EnvironmentMapBuilder.cs # 内部构造器 │ ├── Obstacles/ │ │ ├── IMapObstacle.cs # 障碍物几何契约 │ │ ├── AxisAlignedRectangleObstacle.cs # 中心、宽、高矩形 │ │ ├── CircleObstacle.cs # 中心、半径圆形 │ │ └── MapObstacleRasterizer.cs # 唯一占据写入器 │ ├── Sources/ │ │ ├── ManualObstacleSource.cs # 人工障碍物集合 │ │ └── TwoLegObstacleProjector.cs # TwoLegDetect 投影 │ ├── Planning/ │ │ ├── PlanningGridMap.cs # 只读 m 单位规划地图 │ │ ├── ObstacleDistanceField.cs # 最近障碍距离 │ │ └── PlanningMapAdapter.cs # mm 地图适配 │ ├── PlanningMapRequest.cs # Map 统一输入 │ ├── PlanningMapBuildResult.cs # Map 统一输出 │ ├── PlanningMapFactory.cs # Map 唯一公共调用类 │ └── Test/MovementTest.MapTest.cs # 最后阶段的地图测试入口 ├── CoarsePath/ # MultiWheelC.TrajectoryPlanning.CoarsePath │ ├── Contracts/ │ │ ├── Pose2D.cs # m/rad 位姿 │ │ ├── VehicleParameters.cs # 尺寸、曲率、安全余量 │ │ ├── PlanningRequest.cs # Planner 输入 │ │ ├── HybridAStarConfiguration.cs # 可调参数 │ │ ├── PlanningResult.cs # Planner 输出 │ │ ├── PlanningStatus.cs # 成功/失败状态 │ │ ├── PlanningDiagnostics.cs # 诊断统计 │ │ ├── CoarsePathPoint.cs # 稠密空间路径点 │ │ └── PathSegment.cs # 前进/倒车分段 │ ├── Vehicle/ │ │ ├── VehicleKinematics.cs # 最大曲率 │ │ ├── HeadingFootprintTemplate.cs # 单航向车体模板 │ │ ├── HeadingFootprintTemplateCache.cs # 模板预计算 │ │ └── FootprintCollisionChecker.cs # 碰撞检查 │ ├── Search/ │ │ ├── MotionPrimitive.cs # 恒曲率原语 │ │ ├── MotionPrimitiveGenerator.cs # 原语积分 │ │ ├── HybridAStarNode.cs # 搜索节点 │ │ ├── HybridAStarNodeKey.cs # Closed Set 键 │ │ ├── GoalToleranceChecker.cs # 容差终点 │ │ ├── GridDijkstraHeuristic.cs # 2D 启发 │ │ └── HybridAStarSearch.cs # 搜索管理 │ ├── Output/ │ │ ├── PathBacktracker.cs # 恢复内部积分点 │ │ ├── CoarsePathAssembler.cs # 弧长、换向、分段 │ │ └── CoarsePathValidator.cs # 最终复核 │ ├── HybridAStarPlanner.cs # CoarsePath 唯一公共调用类 │ └── Test/ │ ├── CoarsePathScenarioFactory.cs # 可复现场景 │ └── MovementTest.CoarsePathTest.cs # 最后阶段的规划测试入口 └── README.md # 两个门面类的调用说明 ClumsyPilot/tests/ ├── verify_planning_utils.ps1 ├── verify_planning_map_factory.ps1 ├── verify_planning_map_adapter.ps1 ├── verify_coarse_path_search.ps1 └── verify_coarse_path_integration.ps1 ~~~ ## 唯一调用方式 ~~~csharp var mapResult = new PlanningMapFactory().Create(new PlanningMapRequest { ResolutionMm = 50f, ManualObstacles = manualObstacles, TwoLegInput = optionalTwoLegInput, AllowExplicitEmptyMap = true, }); if (!mapResult.Succeeded) return PlanningResult.FromMapFailure(mapResult); return new HybridAStarPlanner().Plan(new PlanningRequest { Map = mapResult.Map, Start = startPose, Goal = goalPose, Vehicle = vehicle, Configuration = configuration, }); ~~~ 调用方不得直接创建 EnvironmentMapBuilder、MapObstacleRasterizer、PlanningMapAdapter、碰撞检查器、原语生成器或搜索节点。 --- ### Task 1：建立 Utils 与工具测试 **文件：** - Create：Utils 下的 AngleMath.cs、UnitConverter.cs、CoordinateTransform.cs、NumericGuard.cs、GridIndex.cs - Test：ClumsyPilot/tests/verify_planning_utils.ps1 **输出：** NormalizeRadians、ShortestSignedAngleDifference、MillimetersToMeters、DegreesToRadians、TransformLocalToWorld 和 GridIndex。 - [ ] 写失败测试： ~~~powershell Assert-Equal 1.25 ([UnitConverter]::MillimetersToMeters([single]1250)) Assert-Near 0.0 ([AngleMath]::NormalizeRadians(6.283185307179586)) 1e-12 ~~~ - [ ] 运行 verify_planning_utils.ps1；预期 FAIL，类型不存在。 - [ ] 实现 UnitConverter： ~~~csharp public static double MillimetersToMeters(float millimeters) => millimeters / 1000d; public static double DegreesToRadians(float degrees) => degrees * Math.PI / 180d; ~~~ - [ ] 构建并重跑脚本；预期 PASS。 - [ ] 提交：git add Utils 与 verify_planning_utils.ps1；git commit -m "feat: add planning utilities"。 ### Task 2：定义 CoarsePath 契约与可调容差 **文件：** - Create：CoarsePath/Contracts 下的 Pose2D、VehicleParameters、PlanningRequest、HybridAStarConfiguration、PlanningResult、PlanningStatus、PlanningDiagnostics、CoarsePathPoint、PathSegment。 - Test：verify_planning_utils.ps1 **输出：** 默认原语 0.50 m、积分 0.05 m、航向 5°、位置容差 0.15 m、航向容差 5°。 - [ ] 写失败测试，断言上述配置默认值和 Success、InvalidMap、StartInCollision、GoalInCollision、NoFeasiblePath 状态。 - [ ] 运行脚本；预期 FAIL，配置类型不存在。 - [ ] 实现不可变请求/结果契约；结果不得包含速度、加速度或时间。 - [ ] 构建并重跑脚本；预期 PASS。 - [ ] 提交：git add CoarsePath/Contracts 与 verify_planning_utils.ps1；git commit -m "feat: add coarse path contracts"。 ### Task 3：实现外部障碍环境地图 **文件：** - Create：Map/Core/EnvironmentGridMap.cs - Create：Map/Obstacles 下的 IMapObstacle、AxisAlignedRectangleObstacle、CircleObstacle、MapObstacleRasterizer。 - Create：Map/Sources/ManualObstacleSource.cs - Test：verify_planning_map_factory.ps1 **输出：** 矩形按中心/宽/高，圆按中心/半径栅格化；环境地图没有车辆足迹接口。 - [ ] 写失败测试，分别验证矩形中心、圆心、边界外占据和不存在 MarkVehicleFootprint。 - [ ] 运行脚本；预期 FAIL，地图和障碍物类型不存在。 - [ ] 实现轴对齐矩形与圆-格矩形相交栅格化。 - [ ] 构建并重跑脚本；预期 PASS。 - [ ] 提交：git add Map/Core Map/Obstacles Map/Sources/ManualObstacleSource.cs 和测试；git commit -m "feat: add external obstacle map"。 ### Task 4：实现 TwoLeg 图层和 PlanningMapFactory **文件：** - Create：Map/Sources/TwoLegObstacleProjector.cs - Create：Map/Core 下的 MapBuildRequest、EnvironmentMapBuildResult、EnvironmentMapBuilder。 - Create：Map 下的 PlanningMapRequest、PlanningMapBuildResult、PlanningMapFactory。 - Test：verify_planning_map_factory.ps1 **输出：** new PlanningMapFactory().Create(request) 是 Map 唯一公共入口；TwoLeg 失败不能删除人工地图。 - [ ] 写失败测试，验证人工地图成功时 TwoLegApplied 为 false 仍然成功。 - [ ] 运行脚本；预期 FAIL，PlanningMapFactory 不存在。 - [ ] 实现工厂：合并人工障碍与可选投影；TwoLegObstacleProjector 只产出 CircleObstacle。 - [ ] 构建并重跑脚本；预期 PASS。 - [ ] 提交：git add Map 与 verify_planning_map_factory.ps1；git commit -m "feat: add planning map factory"。 ### Task 5：适配 PlanningGridMap 并生成距离场 **文件：** - Create：Map/Planning 下的 PlanningGridMap、ObstacleDistanceField、PlanningMapAdapter。 - Modify：Map/PlanningMapFactory.cs - Test：verify_planning_map_adapter.ps1 **输出：** 深拷贝占据格，mm 转 m，越界占据，生成最近障碍距离场。 - [ ] 写失败测试，断言 ResolutionMeters=0.05、障碍坐标转换、越界和障碍格距离为 0。 - [ ] 运行脚本；预期 FAIL，适配类型不存在。 - [ ] 实现 PlanningMapAdapter.Adapt(EnvironmentGridMap) 和多源距离场；显式空旷图使用正无穷距离。 - [ ] 构建并重跑脚本；预期 PASS。 - [ ] 提交：git add Map/Planning Map/PlanningMapFactory.cs 和测试；git commit -m "feat: adapt planning map"。 ### Task 6：实现车辆足迹与碰撞检查 **文件：** - Create：CoarsePath/Vehicle 下的 VehicleKinematics、HeadingFootprintTemplate、HeadingFootprintTemplateCache、FootprintCollisionChecker。 - Test：verify_coarse_path_search.ps1 **输出：** IsCollisionFree(Pose2D, PlanningGridMap, VehicleParameters, out clearanceMeters)。 - [ ] 写失败测试，覆盖占据碰撞、45° 安全、越界碰撞和不同航向。 - [ ] 运行脚本；预期 FAIL，碰撞类型不存在。 - [ ] 实现扩大车体模板；距离场仅在大于外接圆时放行，否则查询航向模板。 - [ ] 构建并重跑脚本；预期 PASS。 - [ ] 提交：git add CoarsePath/Vehicle 和测试；git commit -m "feat: add footprint collision checking"。 ### Task 7：生成恒曲率运动原语 **文件：** - Create：CoarsePath/Search/MotionPrimitive.cs、MotionPrimitiveGenerator.cs - Test：verify_coarse_path_search.ps1 **输出：** 五级曲率、前进/倒车、最多 0.50 m、每步不超过 0.05 m、保留内部点。 - [ ] 写失败测试，覆盖直行 10 点、曲线航向变化、倒车和曲率跳变拒绝。 - [ ] 运行脚本；预期 FAIL，原语生成器不存在。 - [ ] 实现积分： ~~~csharp x += directionSign * Math.Cos(theta) * stepMeters; y += directionSign * Math.Sin(theta) * stepMeters; theta = AngleMath.NormalizeRadians(theta + directionSign * curvature * stepMeters); ~~~ - [ ] 构建并重跑脚本；预期 PASS，所有内部点经过碰撞检查。 - [ ] 提交：git add 两个原语文件和测试；git commit -m "feat: add motion primitives"。 ### Task 8：实现目标容差、启发和 Hybrid A* 搜索 **文件：** - Create：CoarsePath/Search 下的 HybridAStarNode、HybridAStarNodeKey、GoalToleranceChecker、GridDijkstraHeuristic、HybridAStarSearch。 - Test：verify_coarse_path_search.ps1 **输出：** Closed Set 键为位置格、航向格、方向、曲率索引；终点读取可调容差。 - [ ] 写失败测试，无障碍前进、单障碍绕行、一次倒车、容差成功、无解和节点上限。 - [ ] 运行脚本；预期 FAIL，搜索类型不存在。 - [ ] 实现搜索循环： ~~~csharp while (openList.Count > 0 && diagnostics.ExpandedNodeCount < configuration.MaxExpandedNodes) { var current = PopBestValidNode(); if (_goalChecker.IsReached(current.Pose, request.Goal, configuration, current.Direction)) return SearchResult.Succeeded(current.NodeIndex); ExpandValidPrimitives(current); } ~~~ - [ ] 构建并重跑脚本；预期 PASS。 - [ ] 提交：git add CoarsePath/Search 和测试；git commit -m "feat: add hybrid astar search"。 ### Task 9：回溯、输出和 HybridAStarPlanner 门面 **文件：** - Create：CoarsePath/Output 下的 PathBacktracker、CoarsePathAssembler、CoarsePathValidator。 - Create：CoarsePath/HybridAStarPlanner.cs - Test：verify_coarse_path_integration.ps1 **输出：** new HybridAStarPlanner().Plan(request) 返回稠密路径、换向标记、分段、诊断和状态。 - [ ] 写失败测试，验证首点弧长为 0、内部点连续、换向标记、终点容差和最终复核。 - [ ] 运行脚本；预期 FAIL，门面不存在。 - [ ] 实现编排： ~~~csharp var search = _search.Run(request); if (!search.Succeeded) return PlanningResult.Failed(search.Status, search.Diagnostics); var points = _assembler.Assemble(_backtracker.Backtrack(search)); return _validator.Validate(points, request) ? PlanningResult.Succeeded(points, search.Diagnostics) : PlanningResult.Failed(PlanningStatus.FinalValidationFailed, search.Diagnostics); ~~~ - [ ] 构建并重跑脚本；预期 PASS。 - [ ] 提交：git add CoarsePath/Output CoarsePath/HybridAStarPlanner.cs 和测试；git commit -m "feat: expose coarse path planner"。 ### Task 10：完成非 UI 回归与调用说明 **文件：** - Create：ParkrobTrajplanner/README.md - Modify：全部 verify_planning 脚本。 - [ ] 写失败测试，断言 PlanningMapFactory 和 HybridAStarPlanner 是唯一由外部测试实例化的模块门面。 - [ ] 运行全部脚本；预期在 README、状态或门面断言未齐全时 FAIL。 - [ ] 在 README 写入 Map 到 Planner 的调用代码、mm/m-rad 边界和非目标。 - [ ] 构建、运行全部规划脚本并运行 git diff --check；预期全部 PASS。 - [ ] 提交：git add README 和 tests；git commit -m "test: complete coarse path regression coverage"。 ### Task 11：最后编写 MovementTest 集成模块 **文件：** - Create：Map/Test/MovementTest.MapTest.cs - Create：CoarsePath/Test/CoarsePathScenarioFactory.cs - Create：CoarsePath/Test/MovementTest.CoarsePathTest.cs - Test：Clumsy UI 手动运行两个 MovementTest，然后重跑 Task 10 自动化回归。 **输出：** MapTest 只调用 PlanningMapFactory；CoarsePathTest 只调用两个门面；两者绝不发送底盘命令。 - [ ] 写失败测试，场景工厂生成的地图请求可由 PlanningMapFactory 构建，规划请求可由 HybridAStarPlanner 成功规划。 - [ ] 运行 verify_coarse_path_integration.ps1；预期 FAIL，场景工厂不存在。 - [ ] 实现入口： ~~~csharp [MovementTest(name = "Hybrid A* 粗路径测试")] public sealed class CoarsePathTest : MovementTest { public override void Test() { var map = new PlanningMapFactory().Create(CoarsePathScenarioFactory.CreateSingleObstacleMapRequest()); var result = map.Succeeded ? new HybridAStarPlanner().Plan(CoarsePathScenarioFactory.CreatePlanningRequest(map.Map)) : PlanningResult.FromMapFailure(map); DrawMapAndPath(map, result); } } ~~~ - [ ] MapTest 绘制人工矩形、人工圆、TwoLeg 投影和边界；CoarsePathTest 绘制起点、目标、路径、换向点和失败状态。TestStop 只清理 Painter/任务。 - [ ] 构建、运行全部 PowerShell 脚本，并在 Clumsy UI 手动运行“规划地图测试”和“Hybrid A* 粗路径测试”；预期只显示调试结果，不发送运动命令。 - [ ] 提交：git add Map/Test CoarsePath/Test 和 tests；git commit -m "feat: add coarse path movement tests"。 ## 实施前检查 - PlanningMapFactory 与 HybridAStarPlanner 是未来调用方和测试入口唯一允许直接实例化的模块类。 - MovementTest 位于最后一个任务；它之前所有 Map 与搜索逻辑必须已完成非 UI 自动化验证。