95 lines
7.0 KiB
Markdown
95 lines
7.0 KiB
Markdown
|
# Hybrid A* P0 规划核心实施设计
|
||
|
|
|
||
|
|
## 目标
|
||
|
|
|
||
|
|
在已完成的 `PlanningGridMap` 静态地图能力之上，交付可复用、确定性且可验证的 Hybrid A* 粗路径规划核心。常规调用方通过 `CoarsePathPlanningService.Plan(job)` 一次完成建图和规划；纯算法测试可直接使用 `HybridAStarPlanner.Plan(request)`。
|
||
|
|
|
||
|
|
本设计只覆盖 P0-PLAN。P1 的 Clumsy UI 后台任务、Painter 绘制、Release 性能基准和旧 TrapMap 运行时入口退役不在本次实现范围内。
|
||
|
|
|
||
|
|
## 既有边界
|
||
|
|
|
||
|
|
- `Map` 模块只保存外部障碍物；不得写入 AMR 自身足迹，也不得对障碍物做车辆安全膨胀。
|
||
|
|
- `PlanningGridMap` 是规划器唯一接受的地图类型，世界查询坐标为 m；越界位置视为占据且净空为 0。
|
||
|
|
- 规划核心不读取传感器、定位、UI 或系统时间。取消令牌是唯一允许的外部控制输入。
|
||
|
|
- 所有公共契约和核心逻辑兼容 `netstandard2.0`：不使用 `PriorityQueue`、`Math.Clamp`、`double.IsFinite`、`record` 或 `init`。
|
||
|
|
- 注释延续已有模块风格：公开类型和成员写中文 XML 文档，说明单位、边界、返回/失败语义；内部复杂几何或搜索不变量保留简短中文行注释。
|
||
|
|
|
||
|
|
## 方案选择
|
||
|
|
|
||
|
|
采用“碰撞核心先行、搜索核心随后接入”的两段实现。
|
||
|
|
|
||
|
|
先建立公开数据契约、车辆扩大矩形和连续碰撞检查，使安全语义可以在不依赖搜索器的情况下通过自动化测试固定下来。随后在这些稳定边界上实现恒曲率运动原语、启发式、确定性 Open List、Hybrid A*、路径装配/复核和一次调用服务门面。此顺序避免 UI 或搜索状态掩盖车辆擦边、扫掠和地图边界错误。
|
||
|
|
|
||
|
|
## 架构与数据流
|
||
|
|
|
||
|
|
```text
|
||
|
|
CoarsePathPlanningJob
|
||
|
|
-> PlanningMapFactory.Create(MapRequest)
|
||
|
|
-> PlanningMapBuildResult / PlanningGridMap
|
||
|
|
-> HybridAStarPlanner.Plan(PlanningRequest)
|
||
|
|
-> FootprintCollisionChecker
|
||
|
|
-> MotionPrimitiveGenerator
|
||
|
|
-> GridDijkstraHeuristic + HybridAStarSearch
|
||
|
|
-> PathBacktracker + CoarsePathAssembler
|
||
|
|
-> CoarsePathValidator
|
||
|
|
-> CoarsePathPlanningJobResult
|
||
|
|
```
|
||
|
|
|
||
|
|
`Facade` 只编排地图与规划，不接触栅格化、车辆几何或搜索节点。`HybridAStarPlanner` 在搜索前完成请求、地图、车辆、起点和终点的有效性检查；搜索成功后必须经过 `CoarsePathValidator` 才能发布路径。
|
||
|
|
|
||
|
|
### 公共契约
|
||
|
|
|
||
|
|
`Contracts` 定义 m/rad/1/m 单位的值对象、车辆参数、配置、请求、状态、诊断、路径点和分段。默认配置固定为：0.50 m 原语最大长度、0.05 m 积分步长、5° 航向分辨率、0.15 m 位置容差、5° 航向容差、200,000 节点和 5 s 搜索上限。
|
||
|
|
|
||
|
|
`PlanningResult` 对所有结果提供明确 `PlanningStatus` 和可读诊断。输入错误、地图未就绪、碰撞、无解、取消、超时、节点上限、回溯失败和最终校验失败都返回空路径；不会通过异常发布部分路径。
|
||
|
|
|
||
|
|
### 车辆碰撞
|
||
|
|
|
||
|
|
`VehicleFootprint` 将以车辆几何中心为参考的长宽与安全余量转换为扩大矩形、AABB 和外接圆。`VehicleKinematics` 从最大曲率和最小转弯半径推导更保守的最大曲率。
|
||
|
|
|
||
|
|
`FootprintCollisionChecker` 固定按以下顺序检查连续位姿：
|
||
|
|
|
||
|
|
1. 扩大矩形是否完整位于地图边界内；
|
||
|
|
2. 使用保守距离场与外接圆进行严格大于关系的快速放行；
|
||
|
|
3. 对包围盒内的每个占据格，使用 SAT 检查旋转矩形与格矩形是否相交或擦边；
|
||
|
|
4. 对相邻采样位姿，以中心平移与航向变化构造扫掠附加余量，检查中间区间。
|
||
|
|
|
||
|
|
因此距离场只能加速安全放行，不能替代精确碰撞判定；安全余量只作用于车辆扩大矩形，绝不回写地图。
|
||
|
|
|
||
|
|
### 搜索与路径输出
|
||
|
|
|
||
|
|
第一版只生成前进、倒车和原语边界换向的恒曲率原语。运动积分使用直线/圆弧解析公式，积分点间距不超过配置步长；碰撞采样的中心位移不超过 `min(0.025 m, Map.ResolutionMeters / 2)`。
|
||
|
|
|
||
|
|
搜索键由位置格、航向格、方向和曲率等级组成。确定性二叉最小堆按 `F`、`H`、较大 `G` 和插入序号排序；更小 `G` 的状态允许重新打开，旧堆条目延迟丢弃。二维八邻域 Dijkstra 启发式禁止穿越障碍的对角夹角。
|
||
|
|
|
||
|
|
原语的每个内部积分点依次进行数值、碰撞和终点容差检查。首次满足终点条件时截断原语并将候选压入 Open List；只有该候选作为当前最优有效节点出队时才宣布成功。成功后由回溯器重建稠密积分点，由装配器生成累计弧长和包含式方向分段；换向处保留一对位置、航向和弧长相同但方向不同的相邻点。
|
||
|
|
|
||
|
|
### 文档
|
||
|
|
|
||
|
|
新增 `ClumsyPilot/ParkrobTrajplanner/CoarsePath/README.md`，作为 P0 调用者文档。它说明模块边界、一次调用示例、输入单位、来源版本与地图缓存关系、状态处理方式、路径输出含义和第一版非目标。现有 `Map/README.md` 不复制粗路径内容，继续保留地图构建细节。
|
||
|
|
|
||
|
|
## 错误与取消语义
|
||
|
|
|
||
|
|
- `PlanningGridMap` 为空、不可规划或起终点不在地图内时，在搜索前返回对应状态。
|
||
|
|
- 起点/终点与扩大车辆矩形碰撞时，返回 `StartInCollision` 或 `GoalInCollision`。
|
||
|
|
- 每次扩展节点前检查取消、超时和最大扩展数；触发后立即返回空路径和累计诊断。
|
||
|
|
- 任意内部不变量异常被收敛为 `InternalError`，不向调用方泄露部分路径。
|
||
|
|
- 地图构建失败时，服务直接包装 `PlanningMapBuildResult`，不启动 `HybridAStarPlanner`。
|
||
|
|
|
||
|
|
## 测试策略
|
||
|
|
|
||
|
|
测试继续使用真实 `netstandard2.0` 程序集的 PowerShell 反射脚本，所有 P0 生产代码均遵循 Red-Green-Refactor：先在脚本中写可反射调用的失败断言并确认其因类型或行为缺失失败，再写最小实现，最后运行同一脚本确认通过。
|
||
|
|
|
||
|
|
1. `verify_coarse_path_collision.ps1`：参数边界、亚栅格位姿、任意航向、擦边、薄障碍、地图边界、距离场放行和扫掠碰撞。
|
||
|
|
2. `verify_coarse_path_search.ps1`：原语解析积分、非整倍数终点截断、方向约束、换向、启发式、堆确定性、重开、取消、超时、节点上限和无解。
|
||
|
|
3. `verify_coarse_path_integration.ps1`：从多来源 `PlanningMapRequest` 到最终路径的服务编排、地图缓存复用、最终复核和调试开关不改变规划结果。
|
||
|
|
|
||
|
|
每个脚本在测试前构建 `ClumsyPilot/ClumsyPilot.csproj`，随后只加载 `bin/Debug/netstandard2.0/ClumsyPilot.dll`。完成 P0 前必须重新运行构建与所有既有 Map 验证脚本，确保新规划代码没有破坏地图模块。
|
||
|
|
|
||
|
|
## P0 验收
|
||
|
|
|
||
|
|
- 固定静态场景可以经 `CoarsePathPlanningService.Plan` 得到连续、无碰撞、满足终点容差的稠密粗路径。
|
||
|
|
- 最终路径通过同一套扩大车体和扫掠规则复核；失败不发布部分路径。
|
||
|
|
- 路径点、方向分段、状态和诊断可由不依赖 Clumsy 的调用方直接消费。
|
||
|
|
- 使用说明可独立解释 Map 与 CoarsePath 边界、单位、调用方式及版本限制。
|