First_Dev_Dept/ParkingRobot

Fork 0

Files

T

梁薄云 a0d21eea57 docs: plan hybrid astar coarse path modules

2026-07-26 23:20:10 +08:00

17 KiB

Raw Blame History

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 是最后一项任务。

最终目录与职责

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

唯一调用方式

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。

写失败测试：

Assert-Equal 1.25 ([UnitConverter]::MillimetersToMeters([single]1250))
Assert-Near 0.0 ([AngleMath]::NormalizeRadians(6.283185307179586)) 1e-12

运行 verify_planning_utils.ps1；预期 FAIL，类型不存在。
实现 UnitConverter：

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，原语生成器不存在。
实现积分：

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，搜索类型不存在。
实现搜索循环：

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，门面不存在。
实现编排：

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，场景工厂不存在。
实现入口：

[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 自动化验证。

17 KiB Raw Blame History Unescape Escape

Hybrid A* 粗路径规划实施计划

全局约束

最终目录与职责

唯一调用方式

Task 1：建立 Utils 与工具测试

Task 2：定义 CoarsePath 契约与可调容差

Task 3：实现外部障碍环境地图

Task 4：实现 TwoLeg 图层和 PlanningMapFactory

Task 5：适配 PlanningGridMap 并生成距离场

Task 6：实现车辆足迹与碰撞检查

Task 7：生成恒曲率运动原语

Task 8：实现目标容差、启发和 Hybrid A* 搜索

Task 9：回溯、输出和 HybridAStarPlanner 门面

Task 10：完成非 UI 回归与调用说明

Task 11：最后编写 MovementTest 集成模块

实施前检查

17 KiB

Raw Blame History