545 lines
42 KiB
Markdown
545 lines
42 KiB
Markdown
# Hybrid A* 粗路径规划设计
|
||
|
||
## 目标与范围
|
||
|
||
在现有栅格地图能力之上，实现四舵轮 AMR 的 Hybrid A* 空间粗路径：支持前进、倒车、换向、静态/投影障碍绕行、车体碰撞检查、可配置的终点位置与航向容差，以及稠密路径与方向分段输出。
|
||
|
||
本阶段**不包含**曲线平滑、B 样条、Bezier、局部 QP、SQP、Reeds-Shepp 精确终点连接、速度/加速度/时间参数化或底盘舵角控制。第一版采用汽车式恒曲率模型，不支持蟹行、纯横移和原地旋转。它们由后续独立模块处理。
|
||
|
||
## 已确认的约束
|
||
|
||
- 地图与外部人工障碍输入继续使用现有世界坐标单位：mm。
|
||
- Hybrid A* 内部统一使用 m、rad、1/m；单位转换只能经过地图适配与 `Utils`，不能散落在搜索代码中。
|
||
- 人工障碍物第一版支持以几何中心放置的轴对齐矩形和圆形。
|
||
- `TwoLegProjectionInput` 是可选障碍物输入，不是规划地图是否可用的唯一条件；上层采集到的 `TwoLegDetect` 结果经 DTO 投影后与人工障碍物合并。
|
||
- 环境占据地图只保存外部障碍物，绝不写入 AMR 自身足迹。AMR 尺寸、当前位姿和安全余量只用于粗路径的车体碰撞检测。
|
||
- 障碍物不做安全膨胀；安全余量仅通过碰撞检查时的扩大车辆矩形应用，防止双重膨胀。
|
||
- 第一版的终点条件为可配置的位置容差和车头航向容差。默认建议为 0.15 m 与 5°，而非精确连接到目标位姿。
|
||
- `PrimitiveLength=0.50 m` 是单条原语的最大长度，不是终点只能出现的离散间隔。每条原语必须在内部积分点检查目标条件；该原语内首次满足条件时立即截断并建立终点候选节点。终点候选仍须进入 Open List，只有当它作为当前最优有效节点出队时，整个搜索才返回成功。
|
||
- 碰撞检测必须保守：不得因航向离散、车辆中心的亚栅格偏移、距离场误差或原语离散采样而发布可能碰撞的路径。
|
||
- `ObstacleDistanceField` 只能作为保守快速放行和代价估计，任何可能高估真实净空的近似都不得用于跳过精确车体碰撞检查。
|
||
- 测试是交付的一部分：纯逻辑契约测试与参照 `Map` 的 `MovementTest` 集成/可视化入口都必须提供。
|
||
- 每个文件只承担一个明确职责；对外调用通过模块门面类完成，不让调用者拼装搜索、地图和碰撞的内部对象。
|
||
- 推荐调用方只使用 `CoarsePathPlanningService` 一次完成建图、粗路径搜索和可选调试发布；`PlanningMapFactory` 与 `HybridAStarPlanner` 是可独立测试、复用的下层模块门面。请求、结果、枚举、障碍物 DTO 和值对象仍然是可公开构造的数据契约。
|
||
|
||
## 目录和命名空间
|
||
|
||
```text
|
||
ClumsyPilot/ParkrobTrajplanner/
|
||
├── Initial_plan/ ------ 已有路线与方案文档，不放运行时代码
|
||
├── 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 单位占据栅格
|
||
│ │ ├── MapBoundsMm.cs ------ 有限、非退化的 mm 地图边界值对象
|
||
│ │ ├── MapBuildRequest.cs ------ 环境地图边界、分辨率和障碍物图层输入
|
||
│ │ ├── EnvironmentMapBuildResult.cs ------ 环境地图构建状态、来源摘要和失败原因
|
||
│ │ └── EnvironmentMapBuilder.cs ------ 校验并合并人工与投影障碍物图层
|
||
│ ├── Obstacles/
|
||
│ │ ├── IMapObstacle.cs ------ 人工和投影障碍物的公共几何契约
|
||
│ │ ├── AxisAlignedRectangleObstacle.cs ------ 世界轴对齐矩形障碍物 DTO
|
||
│ │ ├── CircleObstacle.cs ------ 世界坐标圆形障碍物 DTO
|
||
│ │ └── MapObstacleRasterizer.cs ------ 通过形状与格子相交测试写入占据栅格
|
||
│ ├── Sources/
|
||
│ │ ├── IMapObstacleSource.cs ------ 纯快照障碍来源统一投影接口
|
||
│ │ ├── ObstacleSourceStatus.cs ------ Applied、Empty、Unavailable、Invalid 来源状态
|
||
│ │ ├── ObstacleProjectionResult.cs ------ 来源版本、状态、诊断和世界障碍物集合
|
||
│ │ ├── ManualObstacleSource.cs ------ 把人工圆和矩形作为世界障碍物输出
|
||
│ │ ├── TwoLegProjectionInput.cs ------ 已验证两腿端点、检测状态和检测时车辆位姿 DTO
|
||
│ │ ├── TwoLegObstacleSource.cs ------ 把 TwoLeg 快照接入统一障碍来源接口
|
||
│ │ └── TwoLegObstacleProjector.cs ------ 把车体系两腿端点投影为世界坐标圆障碍
|
||
│ ├── Planning/
|
||
│ │ ├── PlanningGridMap.cs ------ 只读 m 单位占据图、距离场和规划可用状态
|
||
│ │ ├── EuclideanDistanceTransform.cs ------ 线性时间生成栅格中心精确欧氏距离
|
||
│ │ ├── ObstacleDistanceField.cs ------ 生成不高估障碍净空的保守欧氏距离下界
|
||
│ │ ├── PlanningMapCache.cs ------ 线程安全的输入指纹与占据哈希快照缓存
|
||
│ │ └── PlanningMapAdapter.cs ------ 深拷贝占据数据并完成 mm 到 m 的边界适配
|
||
│ ├── PlanningMapRequest.cs ------ 地图门面的统一输入契约
|
||
│ ├── PlanningMapBuildResult.cs ------ 地图门面的统一输出契约
|
||
│ ├── PlanningMapFactory.cs ------ Map 模块唯一公共行为入口
|
||
│ └── Test/
|
||
│ ├── MovementTest.MapTest.cs ------ Clumsy UI 地图构建与可视化测试入口
|
||
│ └── Visualization/
|
||
│ ├── PlanningMapImageExportRequest.cs ------ 规划快照、叠加层和输出选项 DTO
|
||
│ ├── PlanningMapImageExportResult.cs ------ PNG 导出状态、路径、大小和诊断
|
||
│ ├── PlanningMapImageExporter.cs ------ 校验请求、编排渲染并原子发布 PNG
|
||
│ ├── PlanningMapImageRenderer.cs ------ 把地图、起终点、车体和路径绘制到 RGBA
|
||
│ └── ValidatedPngWriter.cs ------ Stb PNG 编码、结构和 CRC 完整性校验
|
||
├── CoarsePath/ ------ 命名空间 MultiWheelC.TrajectoryPlanning.CoarsePath
|
||
│ ├── Contracts/
|
||
│ │ ├── Pose2D.cs ------ m/rad 单位的不可变二维位姿
|
||
│ │ ├── TravelDirection.cs ------ Forward 与 Reverse 运动方向枚举
|
||
│ │ ├── GoalDirectionConstraint.cs ------ Any、Forward、Reverse 目标进入方向约束
|
||
│ │ ├── VehicleParameters.cs ------ 车体尺寸、安全余量和最大曲率参数
|
||
│ │ ├── PlanningRequest.cs ------ 地图、起终点、起始曲率和方向约束
|
||
│ │ ├── HybridAStarConfiguration.cs ------ 原语、离散、代价、限额和容差配置
|
||
│ │ ├── PlanningResult.cs ------ 状态、诊断、稠密路径和方向分段
|
||
│ │ ├── PlanningStatus.cs ------ 输入、碰撞、搜索和验证结果枚举
|
||
│ │ ├── PlanningDiagnostics.cs ------ 节点、堆、耗时、路径和终止统计
|
||
│ │ ├── CoarsePathPoint.cs ------ 位姿、弧长、方向、曲率和保守净空
|
||
│ │ ├── CoarsePathPointSource.cs ------ 起点、普通原语和终点截断来源枚举
|
||
│ │ └── PathSegment.cs ------ 前进/倒车分段及其包含式索引范围
|
||
│ ├── Vehicle/
|
||
│ │ ├── VehicleKinematics.cs ------ 解析并校验车辆保守最大曲率
|
||
│ │ ├── VehicleFootprint.cs ------ 计算扩大车体矩形、包围盒和外接圆
|
||
│ │ ├── OrientedRectangleCellIntersection.cs ------ 精确判断旋转车体矩形与栅格矩形相交
|
||
│ │ └── FootprintCollisionChecker.cs ------ 边界、距离场、精确和扫掠碰撞检查
|
||
│ ├── Search/
|
||
│ │ ├── MotionPrimitive.cs ------ 单条恒曲率原语及其实际截断长度描述
|
||
│ │ ├── MotionPrimitiveGenerator.cs ------ 解析积分前进/倒车原语并保留内部点
|
||
│ │ ├── BinaryMinHeap.cs ------ netstandard2.0 兼容且确定性排序的 Open List
|
||
│ │ ├── SearchCostCalculator.cs ------ 统一计算长度、倒车、换向、曲率和净空代价
|
||
│ │ ├── HybridAStarNode.cs ------ 连续位姿、代价、父索引和原语描述
|
||
│ │ ├── HybridAStarNodeKey.cs ------ 位置格、航向格、方向和曲率离散键
|
||
│ │ ├── GoalToleranceChecker.cs ------ 位置、航向和目标进入方向容差判断
|
||
│ │ ├── GridDijkstraHeuristic.cs ------ 八邻域二维绕障距离启发
|
||
│ │ └── HybridAStarSearch.cs ------ 节点扩展、重开、限额和终点候选管理
|
||
│ ├── Output/
|
||
│ │ ├── PathBacktracker.cs ------ 按父索引确定性重建原语内部点
|
||
│ │ ├── CoarsePathAssembler.cs ------ 生成弧长、换向点和包含式方向分段
|
||
│ │ └── CoarsePathValidator.cs ------ 复核数值、碰撞、曲率、终点和分段
|
||
│ ├── HybridAStarPlanner.cs ------ 只消费 PlanningGridMap 的纯搜索下层门面
|
||
│ ├── Facade/
|
||
│ │ ├── CoarsePathPlanningJob.cs ------ 一次调用所需地图、起终点、车辆和调试选项
|
||
│ │ ├── CoarsePathPlanningJobResult.cs ------ 同时返回地图构建结果和粗路径结果
|
||
│ │ ├── PlanningDebugOptions.cs ------ 地图、路径和碰撞调试发布开关
|
||
│ │ ├── IPlanningDebugSink.cs ------ 不影响规划状态的调试结果消费接口
|
||
│ │ └── CoarsePathPlanningService.cs ------ 建图、缓存、搜索和调试编排的一次调用入口
|
||
│ └── Test/
|
||
│ ├── CoarsePathScenarioFactory.cs ------ 生成固定、可复现的地图和规划场景
|
||
│ └── MovementTest.CoarsePathTest.cs ------ 后台运行规划并在 Clumsy UI 绘制结果
|
||
│ └── README.md ------ 粗规划模块边界、公共调用入口和文档链接
|
||
|
||
ClumsyPilot/tests/
|
||
├── verify_planning_utils.ps1 ------ 单位、角度、坐标和数值守卫测试
|
||
├── verify_planning_map_factory.ps1 ------ 地图输入、图层事务、快照和版本测试
|
||
├── verify_planning_map_adapter.ps1 ------ 地图栅格化、图层、适配和距离场测试
|
||
├── verify_planning_map_image.ps1 ------ 只读快照 PNG 渲染、限制和原子发布测试
|
||
├── verify_coarse_path_collision.ps1 ------ 亚栅格、擦边和扫掠碰撞测试
|
||
├── verify_coarse_path_search.ps1 ------ 原语截断、搜索、方向、限额和重开测试
|
||
├── verify_coarse_path_integration.ps1 ------ Map 到最终路径验证的端到端测试
|
||
└── benchmark_coarse_path.ps1 ------ 参考与压力场景性能资源验收
|
||
```
|
||
|
||
`Occupancygird_Map/Map_test` 是当前原型位置。实施时会把其中仍然需要的地图能力按以上职责迁移到 `Map`，避免继续向两个现有大文件叠加功能；图片导出可以保留为独立地图测试辅助，不成为规划器依赖。
|
||
|
||
## Utils
|
||
|
||
`Utils` 只放无状态、确定性的通用计算，不依赖 Clumsy UI、地图、传感器或搜索器。
|
||
|
||
| 文件 | 唯一职责 |
|
||
| --- | --- |
|
||
| `AngleMath` | 角度归一化、最小有符号角差、度/弧度边界规则、航向离散索引。 |
|
||
| `UnitConverter` | mm↔m、deg↔rad、曲率/半径转换；不包含业务配置。 |
|
||
| `CoordinateTransform` | 车体系与世界系的二维刚体变换。 |
|
||
| `NumericGuard` | 有限值、正值和范围校验的可复用方法。 |
|
||
| `GridIndex` | 行列索引值对象与比较，不存储地图状态。 |
|
||
|
||
## Map 模块
|
||
|
||
### 对外调用门面
|
||
|
||
`HybridAStarPlanner` 不直接实例化任何地图对象；`CoarsePathPlanningService` 只持有 `PlanningMapFactory`，不接触 `EnvironmentMapBuilder`、`MapObstacleRasterizer`、来源投影器或 `PlanningMapAdapter`。`PlanningMapFactory` 是 `Map` 模块唯一的公共创建入口：
|
||
|
||
```csharp
|
||
public sealed class PlanningMapFactory
|
||
{
|
||
public PlanningMapBuildResult Create(PlanningMapRequest request);
|
||
}
|
||
```
|
||
|
||
下层模块独立调用时只依赖其稳定输出：
|
||
|
||
```csharp
|
||
var mapResult = new PlanningMapFactory().Create(mapRequest);
|
||
if (!mapResult.Succeeded)
|
||
return PlanningResult.FromMapFailure(mapResult);
|
||
|
||
var result = new HybridAStarPlanner().Plan(new PlanningRequest
|
||
{
|
||
Map = mapResult.Map,
|
||
Start = startPose,
|
||
Goal = goalPose,
|
||
Vehicle = vehicle,
|
||
Configuration = configuration,
|
||
});
|
||
```
|
||
|
||
`PlanningMapRequest` 集中地图边界/分辨率、`IReadOnlyList<IMapObstacleSource>` 和空旷地图声明；`PlanningMapBuildResult` 返回 `Succeeded`、失败原因、每个障碍物来源的摘要、缓存命中类型，以及成功时不可变的 `PlanningGridMap`。因此 A* 的 `PlanningRequest.Map` 始终是已完成校验、投影、栅格化、mm→m 适配和距离场生成的输入，规划器不需要了解地图构造细节。
|
||
|
||
### 统一障碍物来源与投影
|
||
|
||
所有人工、TwoLeg 和后续障碍物输入统一实现：
|
||
|
||
```csharp
|
||
public interface IMapObstacleSource
|
||
{
|
||
string SourceId { get; }
|
||
long SourceVersion { get; }
|
||
bool IsRequired { get; }
|
||
ObstacleProjectionResult ProjectToWorld();
|
||
}
|
||
```
|
||
|
||
`ProjectToWorld` 只能消费构造来源对象时已经取得的不可变快照，不得在内部读取传感器、定位、UI 或系统时间。它统一返回世界坐标 mm 几何体：
|
||
|
||
```text
|
||
ObstacleProjectionResult:
|
||
SourceId
|
||
SourceVersion
|
||
Status Applied/Empty/Unavailable/Invalid
|
||
Message
|
||
IReadOnlyList<IMapObstacle> Obstacles
|
||
```
|
||
|
||
必需来源返回 `Unavailable/Invalid` 时地图构建失败；可选来源返回上述状态时记录诊断并继续处理其他来源。多个来源产生重叠障碍物是合法的，占据写入具有幂等语义。
|
||
|
||
`ManualObstacleSource` 直接输出已经位于世界坐标系的圆和轴对齐矩形。`TwoLegProjectionInput` 是纯数据 DTO，明确包含检测状态、车体坐标系中的两个端点、端点半径、检测时刻的 AMR 世界位姿，以及 mm/deg 单位声明。`MovementTest` 或上层采集适配器负责调用现有 `TwoLegDetect`，随后把结果封装为快照；`TwoLegObstacleSource` 委托 `TwoLegObstacleProjector` 执行确定性车体到世界变换并输出零个或两个 `CircleObstacle`。
|
||
|
||
新增障碍物来源只需投影为现有 `IMapObstacle` 几何体；如果需要新增多边形等几何类型，必须同时为 `MapObstacleRasterizer` 添加保守的形状-栅格相交实现和成功/边界/失败测试。任何来源都不得应用车辆安全余量。
|
||
|
||
### 环境地图与图层
|
||
|
||
`EnvironmentGridMap` 保存 mm 单位的地图边界、分辨率和仅含外部障碍物的占据单元。它不接受 `MarkVehicleFootprint` 一类接口。
|
||
|
||
`EnvironmentMapBuilder` 是由 `PlanningMapFactory` 使用的内部环境图构造器：
|
||
|
||
```csharp
|
||
public sealed class EnvironmentMapBuilder
|
||
{
|
||
public EnvironmentMapBuildResult Build(MapBuildRequest request);
|
||
}
|
||
```
|
||
|
||
它依次校验地图参数、按 `SourceId` 确定性排序来源、收集 `ObstacleProjectionResult`、合并所有成功投影的 `IMapObstacle` 并调用唯一栅格化器。任何可选来源失败都不会删除其他来源已成功构建的占据内容。
|
||
|
||
人工障碍物实现 `IMapObstacle`。`AxisAlignedRectangleObstacle` 使用 `(CenterXmm, CenterYmm, WidthMm, HeightMm)`；`CircleObstacle` 使用 `(CenterXmm, CenterYmm, RadiusMm)`。两者的尺寸必须为有限正数，矩形轴与世界 X/Y 轴对齐。`MapObstacleRasterizer` 是唯一直接写入环境栅格的类。
|
||
|
||
`MapObstacleRasterizer` 是唯一直接写入 `EnvironmentGridMap` 的类型；各来源和投影器都不能取得地图写入接口。`TwoLegObstacleProjector` 不负责地图边界、栅格化、缓存或规划可用性判断。
|
||
|
||
### 规划适配
|
||
|
||
`PlanningMapAdapter` 是地图与规划器之间唯一的单位/数据边界：
|
||
|
||
```csharp
|
||
public sealed class PlanningMapAdapter
|
||
{
|
||
public PlanningGridMap Adapt(EnvironmentGridMap map);
|
||
}
|
||
```
|
||
|
||
适配时验证边界和分辨率、深拷贝占据数据、将长度从 mm 转为 m、将边界外固定解释为占据，并生成 `ObstacleDistanceField`。`PlanningGridMap` 是不可变的规划输入，包含 m 单位边界、分辨率、行列、占据数据、距离场、来源版本摘要、快照标识与 `PlanningReady/PlanningBlockReason`。
|
||
|
||
距离场使用精确的二维欧氏距离变换计算栅格中心到最近占据栅格中心的距离，再减去一个完整栅格对角线 `sqrt(2) * ResolutionMeters` 并截断到零，得到当前自由栅格内任意点到任意占据栅格矩形的保守下界。查询不得进行会抬高结果的插值；查询点使用其所在栅格的保守值。显式空旷地图的障碍物距离可以是正无穷，但车体边界检查仍必须先执行，地图外始终按占据处理。
|
||
|
||
距离场的用途受以下规则约束：
|
||
|
||
- 当保守距离严格大于扩大车体外接圆半径时，碰撞检查器可以快速放行。
|
||
- 当保守距离小于或等于外接圆半径时，必须执行精确的扩大车体矩形与占据栅格矩形相交检查。
|
||
- `BodyClearance` 发布 `max(0, ConservativeCenterClearance - ExpandedFootprintCircumscribedRadius)`，作为车体净空的保守下界，不得声称为精确几何净空。
|
||
- 多源距离场实现、空图语义、地图边界和最大栅格数都必须有自动化测试。
|
||
|
||
`PlanningReady` 的规则：至少一个成功来源提供有效障碍语义即可使地图用于测试/规划；所有来源均为空时，只有已明确配置 `AllowExplicitEmptyMap=true` 才可规划。必需来源失败或未明确空旷语义时，以 `PlanningBlockReason` 阻止规划，而不是暗中把未观测区域当作空闲。
|
||
|
||
### 现有地图优化与规划适配
|
||
|
||
本阶段不在旧 `GridMapData` 外再包一层长期兼容适配器，而是把其中经过测试的几何规则迁移为纯逻辑、静态快照式地图管线。迁移目标是消除 UI、传感器、车辆自身足迹和规划查询之间的职责耦合，同时降低 Hybrid A* 高频占据查询与距离查询的开销。
|
||
|
||
#### 现有职责拆分
|
||
|
||
| 现有类型/函数 | 处理方式 | 新职责位置 |
|
||
| --- | --- | --- |
|
||
| `TrapMapBounds.TryCreate` | 保留有限值、退化边界、分辨率和最大栅格数校验；移除“车辆到工作站”业务假设 | `MapBoundsMm`、`MapBuildRequest` |
|
||
| `GridMapData.WorldToGrid/GridToWorld` | 保留 X→列、Y→行和 XMax/YMax 排他规则；统一处理最后一个非完整栅格 | `EnvironmentGridMap`、`PlanningGridMap` |
|
||
| `GridMapData.Cells byte[,]` | 改为私有行优先 `byte[]`，索引固定为 `row * Cols + col`；不暴露可写数组 | 两类 GridMap 的内部存储 |
|
||
| `GridMapData.MarkObstacle/MarkObstacles` | 移除默认安全距离；保留候选包围盒裁剪和形状-格矩形相交 | `MapObstacleRasterizer` |
|
||
| `GridMapData.MarkVehicleFootprint` | 从地图模块删除，不提供兼容开关 | `FootprintCollisionChecker` 查询时处理 |
|
||
| `GridMapData.Copy` | 不再暴露可变副本；适配时只进行一次占据缓冲区深拷贝 | `PlanningMapAdapter` |
|
||
| `TrapMapLayerComposer.Compose` | 泛化为“可选来源失败不破坏其他成功来源”的事务语义 | `IMapObstacleSource`、`EnvironmentMapBuilder` |
|
||
| `TrapMapBuilder.Get` | 拆除 `MovementDefinition`、定位读取、TwoLeg 调用、Toast、Painter 和协程依赖 | 上层采集适配器 + `CoarsePathPlanningService` |
|
||
| `TrapMapImageExporter.cs` | 保留经过验证的纯托管 RGBA/PNG 能力，拆分请求、结果、渲染、发布和 PNG 校验职责；输入改为只读规划快照 | `Map/Test/Visualization` |
|
||
|
||
#### 统一地图输入与静态快照
|
||
|
||
`PlanningMapRequest` 必须显式包含：
|
||
|
||
```text
|
||
MapBoundsMm Bounds
|
||
float ResolutionMm
|
||
IReadOnlyList<IMapObstacleSource> ObstacleSources
|
||
bool AllowExplicitEmptyMap
|
||
```
|
||
|
||
`Bounds` 使用 `[XMin, XMax) × [YMin, YMax)`；`ResolutionMm` 必须是 20～200 mm 的有限正数。来源 `SourceId` 必须非空且在一次请求内唯一，`SourceVersion` 必须非负，并在对应来源快照内容变化时递增。
|
||
|
||
`PlanningMapFactory.Create` 返回与来源输入隔离的不可变静态快照，不实现增量栅格更新或距离场局部修补。`PlanningGridMap` 保存各来源版本摘要、`InputFingerprint`、`OccupancyHash` 和通过 `Interlocked.Increment` 生成的进程内单调 `SnapshotId`。该计数器只标识快照，不保存地图内容。规划开始后只读取同一快照；上层即使收到新障碍物，也不得修改正在使用的占据缓冲区。
|
||
|
||
#### 两级指纹与快照复用
|
||
|
||
`PlanningMapCache` 是 `PlanningMapFactory` 的线程安全、容量为 4 的最近使用缓存；长期存在的 `CoarsePathPlanningService` 持有同一个工厂实例，因此多次规划可以复用快照。
|
||
|
||
每次创建按以下顺序判断：
|
||
|
||
1. 调用纯快照来源的 `ProjectToWorld`，按 `SourceId` 排序，并对边界、分辨率、空图策略、来源状态/版本和规范化世界几何体计算 `InputFingerprint`。
|
||
2. 若缓存中存在相同 `InputFingerprint`，直接返回同一不可变 `PlanningGridMap`；不重新栅格化或生成距离场。
|
||
3. 输入指纹不同时重新栅格化，并对最终连续占据 `byte[]` 计算 `OccupancyHash`。
|
||
4. 若地图几何参数和 `OccupancyHash` 与缓存快照相同，复用占据缓冲区与距离场，只生成包含新来源摘要和新 `SnapshotId` 的轻量快照。
|
||
5. `OccupancyHash` 不同时才重新执行距离场变换并缓存完整新快照。
|
||
|
||
浮点几何按其 IEEE 位模式和固定字段顺序计算确定性指纹，不通过简单的“坐标除以分辨率取整”判断变化，避免圆或矩形在格边附近发生漏失效。缓存项同时保留规范化输入描述；`InputFingerprint` 命中后仍执行结构相等比较。`OccupancyHash` 命中后仍比较地图几何参数和连续占据缓冲区长度/内容，不能只依赖哈希值判等。
|
||
|
||
起点、终点、车辆尺寸、安全余量、Hybrid A* 参数和可视化开关不属于地图指纹。TwoLeg 的检测状态从有效变为无检测/不可用/过期时，其来源结果必须改变；上层采集适配器负责根据检测有效期构造正确的 `TwoLegProjectionInput`，地图来源接口本身不读取系统时间。
|
||
|
||
#### 存储、坐标和查询优化
|
||
|
||
- 占据数据使用私有连续 `byte[]`，避免公开 `byte[,]` 带来的可变性和多维数组索引开销。
|
||
- `IsOccupied(row,col)`、`IsOccupiedWorld(x,y)` 和保守距离查询保持无分配、常数复杂度；地图外直接返回占据或零净空。
|
||
- 世界坐标到格索引使用 `floor((value - min) / resolution)`；`XMax`、`YMax` 排他。最后一个格子的几何上界必须裁剪到实际地图上界。
|
||
- 构造阶段使用 `checked` 计算 `Rows * Cols`，继续采用 4,000,000 格绝对上限；任何溢出或超限在分配前返回失败结果。
|
||
- 圆形与矩形栅格化只遍历其裁剪后的格索引包围盒，不扫描全图。与地图完全不相交的合法障碍物被忽略并记录在来源摘要中，而不是使地图构建失败。
|
||
- `EnvironmentGridMap` 只有程序集内部的占据写入入口；`PlanningGridMap` 不提供任何写入入口，也不返回内部缓冲区引用。
|
||
|
||
#### 距离场优化
|
||
|
||
`EuclideanDistanceTransform` 使用两次一维平方距离变换完成精确二维栅格中心距离计算，时间复杂度为 `O(Rows × Cols)`，不得为每个自由格遍历全部障碍格。中间数组按行列最大长度复用，最终距离使用连续 `double[]` 保存。
|
||
|
||
`ObstacleDistanceField` 在精确中心距离上执行前述保守修正并封装查询，不允许调用方直接取得未经修正的中心距离用于碰撞放行。显式空图不运行无意义的变换，直接构造正无穷障碍距离场；地图边界仍由规划地图和车体碰撞检查独立约束。
|
||
|
||
#### 地图迁移顺序
|
||
|
||
1. 先建立 `MapBoundsMm`、新占据存储和纯栅格化测试，不修改旧 UI 入口。
|
||
2. 建立 `PlanningMapFactory`、静态快照、距离场和规划查询测试。
|
||
3. 将人工障碍、TwoLeg DTO 与现有 Ghost/固定场景迁移为统一 `IMapObstacleSource`。
|
||
4. 建立两级指纹缓存与 `CoarsePathPlanningService` 一次调用入口。
|
||
5. 将旧 `TrapMapImageExporter.cs` 拆分为 `Map/Test/Visualization` 下的五个文件，并将 PNG 导出与 `MovementTest.MapTest` 改为只消费新快照。
|
||
6. 新旧地图回归结果一致后，退役旧地图构建、车体写入和图层合成入口，并更新或删除对应旧反射测试。
|
||
|
||
迁移期间不得让 Hybrid A* 同时支持新旧两种地图类型；规划器从第一天起只接受 `PlanningGridMap`。
|
||
|
||
## CoarsePath 模块
|
||
|
||
### 一次调用编排门面
|
||
|
||
常规调用方只调用：
|
||
|
||
```csharp
|
||
public sealed class CoarsePathPlanningService
|
||
{
|
||
public CoarsePathPlanningJobResult Plan(
|
||
CoarsePathPlanningJob job,
|
||
CancellationToken cancellationToken = default);
|
||
}
|
||
```
|
||
|
||
`CoarsePathPlanningJob` 集中以下输入：
|
||
|
||
```text
|
||
PlanningMapRequest MapRequest
|
||
Pose2D Start
|
||
Pose2D Goal
|
||
VehicleParameters Vehicle
|
||
HybridAStarConfiguration Configuration
|
||
double StartVehicleCurvature
|
||
TravelDirection? StartDirection
|
||
GoalDirectionConstraint GoalDirection
|
||
PlanningDebugOptions Debug
|
||
```
|
||
|
||
推荐调用形式：
|
||
|
||
```csharp
|
||
var result = planningService.Plan(new CoarsePathPlanningJob
|
||
{
|
||
MapRequest = new PlanningMapRequest
|
||
{
|
||
Bounds = bounds,
|
||
ResolutionMm = 50f,
|
||
ObstacleSources = new IMapObstacleSource[]
|
||
{
|
||
new ManualObstacleSource(manualObstacles),
|
||
new TwoLegObstacleSource(twoLegSnapshot),
|
||
},
|
||
AllowExplicitEmptyMap = true,
|
||
},
|
||
Start = startPose,
|
||
Goal = goalPose,
|
||
Vehicle = vehicle,
|
||
Configuration = configuration,
|
||
Debug = new PlanningDebugOptions
|
||
{
|
||
VisualizeMap = true,
|
||
VisualizePath = true,
|
||
},
|
||
}, cancellationToken);
|
||
```
|
||
|
||
一次调用内部固定执行：
|
||
|
||
```text
|
||
PlanningMapFactory.Create(MapRequest)
|
||
→ 地图失败则生成 FromMapFailure 结果
|
||
→ HybridAStarPlanner.Plan(PlanningRequest, cancellationToken)
|
||
→ IPlanningDebugSink 按 Debug 开关发布地图、路径和诊断
|
||
```
|
||
|
||
`CoarsePathPlanningJobResult` 同时保留 `PlanningMapBuildResult MapResult` 与 `PlanningResult PlanningResult`，使调用方能够取得实际使用的 `PlanningGridMap` 快照、缓存命中情况和粗路径状态。地图失败时不启动搜索；调试发布失败只写入调试诊断，不改变地图或路径规划状态。
|
||
|
||
`PlanningDebugOptions` 只包含 `VisualizeMap`、`VisualizePath`、`VisualizeCollisionChecks` 等旁路开关。`IPlanningDebugSink` 由 Clumsy `MovementTest` 适配实现；核心服务默认使用空实现，因此无 UI 环境与自动化测试不加载 Painter。
|
||
|
||
### 纯搜索下层门面
|
||
|
||
需要复用已有地图快照或单独测试搜索时调用 `HybridAStarPlanner`：
|
||
|
||
```csharp
|
||
public sealed class HybridAStarPlanner
|
||
{
|
||
public PlanningResult Plan(
|
||
PlanningRequest request,
|
||
CancellationToken cancellationToken = default);
|
||
}
|
||
```
|
||
|
||
`PlanningRequest` 组合以下不可变输入：
|
||
|
||
- `PlanningGridMap Map`
|
||
- `Pose2D Start` 与 `Pose2D Goal`
|
||
- `VehicleParameters Vehicle`
|
||
- `HybridAStarConfiguration Configuration`
|
||
- `double StartVehicleCurvature`，未提供时显式使用零曲率
|
||
- `TravelDirection? StartDirection`，`null` 表示起步方向不受约束
|
||
- `GoalDirectionConstraint GoalDirection`，取值为 `Any`、`Forward` 或 `Reverse`
|
||
|
||
它不接受地图构建器、UI 对象或传感器对象。起点曲率必须在车辆最大曲率内，并离散到最近的合法曲率等级；该索引作为起始搜索状态的一部分。
|
||
|
||
`VehicleParameters` 明确使用车辆几何中心为 `Pose2D` 参考点，并包含 `LengthMeters`、`WidthMeters`、`SafetyMarginMeters`、可选 `MaximumCurvaturePerMeter` 与可选 `MinimumTurningRadiusMeters`。最大曲率和最小转弯半径同时存在时使用更保守的限制；两者都未提供时请求无效。
|
||
|
||
`HybridAStarConfiguration` 集中所有可调参数，包括最大节点数、超时、航向分辨率、原语最大长度、积分步长、碰撞采样步长、曲率等级、代价权重、是否允许倒车、位置容差和航向容差。默认值为：
|
||
|
||
| 参数 | 默认值 |
|
||
| --- | --- |
|
||
| `PrimitiveLengthMeters` | 0.50 m，表示最大长度 |
|
||
| `IntegrationStepMeters` | 0.05 m |
|
||
| `MaximumCollisionCheckStepMeters` | 0.025 m，且运行时不得大于 `Map.ResolutionMeters / 2` |
|
||
| `HeadingResolutionRadians` | 5° |
|
||
| `CurvatureLevelCount` | 5 |
|
||
| `GoalPositionToleranceMeters` | 0.15 m |
|
||
| `GoalHeadingToleranceRadians` | 5° |
|
||
| `MaximumExpandedNodes` | 200,000 |
|
||
| `SearchTimeout` | 5 s |
|
||
| `HeuristicWeight` | 1.0 |
|
||
| `ReverseCostMultiplier` | 1.5 |
|
||
| `GearSwitchPenaltyMeters` | 1.0 |
|
||
| `CurvatureMagnitudeWeight` | 0.10 |
|
||
| `CurvatureChangePenaltyMetersPerLevel` | 0.05 |
|
||
| `ClearanceCostWeight` | 0.20 |
|
||
| `ClearanceCostDistanceMeters` | 0.50 m |
|
||
|
||
搜索代价全部以“等效米”为单位：
|
||
|
||
```text
|
||
primitiveCost =
|
||
lengthMeters
|
||
× directionMultiplier
|
||
× (1
|
||
+ CurvatureMagnitudeWeight × abs(curvature / maximumCurvature)
|
||
+ ClearanceCostWeight × max(0, 1 - clearance / ClearanceCostDistanceMeters))
|
||
+ gearSwitchPenalty
|
||
+ CurvatureChangePenaltyMetersPerLevel × abs(curvatureLevelDelta)
|
||
```
|
||
|
||
其中前进的 `directionMultiplier=1`，倒车使用 `ReverseCostMultiplier`；没有换向时 `gearSwitchPenalty=0`。所有权重必须为有限非负值。默认 `HeuristicWeight=1.0`；若调用方调大该值，只承诺更快地寻找可行解，不承诺离散图上的最低代价。
|
||
|
||
`PlanningResult` 始终返回明确 `PlanningStatus`、诊断信息、零或一条 `IReadOnlyList<CoarsePathPoint>` 和 `IReadOnlyList<PathSegment>`。成功结果中的点契约固定为：
|
||
|
||
```text
|
||
CoarsePathPoint:
|
||
X、Y m
|
||
Heading、UnwrappedHeading rad
|
||
ArcLength m，非负且不递减
|
||
Direction Forward/Reverse
|
||
VehicleCurvature 1/m
|
||
BodyClearance m，保守下界
|
||
IsGearSwitchPoint bool
|
||
Source Start/MotionPrimitive/GoalTruncation
|
||
```
|
||
|
||
`PathSegment` 固定包含 `SegmentIndex`、`Direction`、`StartIndex`、`EndIndex`、`StartsAtGearSwitch` 与 `EndsAtGearSwitch`。`StartIndex` 与 `EndIndex` 都是包含端点的索引，所有分段按索引顺序完整覆盖整条路径。换向时保留两个坐标和航向相同、弧长相同但方向不同的相邻点：前一点结束旧分段，后一点开始新分段并设置 `IsGearSwitchPoint=true`。除这种换向对外，装配器删除相邻重复点。粗路径不包含时间、速度、加速度、舵轮角或轮速。
|
||
|
||
### 车辆、碰撞和搜索
|
||
|
||
- `VehicleKinematics` 根据车辆参数提供最大曲率；直接最大曲率和最小转弯半径同时存在时采用更保守的值。
|
||
- `VehicleFootprint` 以连续位姿计算扩大车辆矩形的四角、轴对齐包围盒和外接圆。安全余量只在这里同时加到长度和宽度两侧，不写入地图。
|
||
- `OrientedRectangleCellIntersection` 使用分离轴定理判断连续位姿下的扩大车辆矩形是否与占据栅格矩形相交，不使用仅按离散航向和整数格偏移的模板，因此车辆中心的亚栅格偏移不会漏检。
|
||
- `FootprintCollisionChecker` 依次执行扩大车体边界检查、保守距离场快速放行和包围盒内占据栅格的精确相交检查；它不执行搜索，也不改变地图。
|
||
- `MotionPrimitiveGenerator` 仅生成恒曲率前进/倒车原语，以不大于 0.05 m 的步长保留输出积分点，并使用直线/圆弧解析公式更新位姿，不使用累计误差更大的显式欧拉积分。
|
||
- 相邻碰撞检查位姿的中心位移不得超过 `min(MaximumCollisionCheckStepMeters, Map.ResolutionMeters / 2)`。同时用 `0.5 × (中心位移 + 外接圆半径 × 航向变化绝对值)` 作为扫掠附加余量检查相邻区间端点，保守覆盖两个采样位姿之间的车体运动；该附加余量只用于区间碰撞验证，不写入输出车体尺寸。
|
||
- `GoalToleranceChecker` 只判断位置、航向和最后一段方向约束；位置和航向阈值来自 `HybridAStarConfiguration`。
|
||
- 每条原语按积分点顺序执行数值合法性、碰撞和目标检查。如果某个内部积分点满足目标条件，当前原语在该点截断并生成终点候选；候选加入 Open List，只有当它作为最佳有效节点出队时才成功终止搜索。
|
||
- `GridDijkstraHeuristic` 从目标在占据图上生成八邻域二维绕障距离启发，禁止穿过两个对角相邻障碍物的夹角；它不处理车辆运动学。若目标在二维图上不可达，规划返回 `NoFeasiblePath`。
|
||
- `SearchCostCalculator` 只实现本节定义的等效米代价公式，集中处理倒车、换向、曲率、曲率变化与保守净空代价，不管理节点或 Open List。
|
||
- `BinaryMinHeap` 是兼容 `netstandard2.0` 的内部最小堆，不依赖较新运行时的 `PriorityQueue`。排序依次使用 `F`、`H`、较大的 `G` 和单调递增插入序号，保证相同输入的搜索顺序可复现。
|
||
- `HybridAStarSearch` 使用 `Dictionary<HybridAStarNodeKey,double>` 保存每个离散键当前最佳 `G`。发现更小 `G` 时允许重新打开节点；Open List 中的旧条目通过比较最佳 `G` 惰性丢弃。Closed Set 键为位置格、航向格、方向和曲率等级。
|
||
- 搜索循环在扩展节点前检查取消、超时和节点上限。终点候选只有作为当前最佳有效节点出队时才返回成功。
|
||
- `PathBacktracker` 只存父节点索引和原语描述，在成功后确定性地重新生成内部积分点，避免为所有搜索节点长期保存稠密点。`CoarsePathAssembler` 按既定换向规则累计弧长并构造 `PathSegment`。
|
||
- `CoarsePathValidator` 使用相同的连续位姿、扫掠余量和碰撞规则复核最终稠密输出，同时检查有限数值、曲率上限、目标容差、方向约束、弧长单调性、换向对和分段索引完整覆盖。
|
||
|
||
第一版允许前进、倒车和换向。换向只可发生在原语边界；相邻原语曲率等级最多变化一级。目标达到容差即成功，不尝试 Reeds-Shepp 精确连接。
|
||
|
||
## 状态与失败处理
|
||
|
||
`PlanningStatus` 至少区分：`Success`、`Cancelled`、`InvalidRequest`、`InvalidMap`、`MapNotReady`、`InvalidVehicleParameters`、`InvalidCurvatureConfiguration`、`StartOutsideMap`、`StartInCollision`、`GoalOutsideMap`、`GoalInCollision`、`SearchTimeout`、`SearchNodeLimitExceeded`、`NoFeasiblePath`、`BacktrackingFailed`、`FinalValidationFailed` 与 `InternalError`。
|
||
|
||
所有输入错误在开始搜索前返回状态与可读原因；取消、超时和节点上限均返回空路径，不发布部分结果。除参数为空这类编程错误外，搜索或地图对象不能通过异常把部分路径发布给调用方。`PlanningDiagnostics` 记录扩展节点数、生成节点数、重新打开节点数、丢弃的陈旧堆条目数、Open List 峰值、总路径长度、最小保守净空、耗时和终止原因，供之后的性能优化使用。
|
||
|
||
## 测试设计
|
||
|
||
纯逻辑测试沿用当前 PowerShell 反射契约测试方式，确保在项目的 `netstandard2.0` 与 Clumsy 引用环境中验证真实程序集。
|
||
|
||
| 测试文件/入口 | 覆盖内容 |
|
||
| --- | --- |
|
||
| `verify_planning_utils.ps1` | mm/m、deg/rad、角度环绕、车体/世界坐标变换和非法数值。 |
|
||
| `verify_planning_map_factory.ps1` | 统一来源投影、必需/可选失败策略、来源确定性顺序、空图声明、输入指纹、占据哈希、完整/缓冲区缓存命中、来源版本摘要、静态快照隔离与并发访问。 |
|
||
| `verify_planning_map_adapter.ps1` | 圆与轴对齐矩形的包围盒裁剪和格矩形相交、AMR 自身不占据环境图、连续行优先存储、坐标边界、非完整末格、越界保守占据、mm→m 深拷贝、距离场不高估与 `PlanningReady`。 |
|
||
| `verify_planning_map_image.ps1` | 从 `PlanningGridMap` 渲染占据格、边界、起终点、车辆和路径叠加；覆盖关闭导出、非法尺寸、像素/文件上限、唯一命名、临时文件清理、PNG 结构与 CRC。 |
|
||
| `verify_coarse_path_collision.ps1` | 车体中心位于栅格中心和亚栅格位置时的正交/45°/任意航向，边角擦碰、薄障碍、地图边界、距离场快速放行与原语区间扫掠碰撞。 |
|
||
| `verify_coarse_path_search.ps1` | 无障碍前进、0.30 m 非整倍数终点截断、单障碍绕行、允许倒车的狭窄场景、起始曲率、目标进入方向、换向对、±π 航向容差、起点已满足目标、无解、取消、超时、节点上限、节点重新打开与确定性顺序。 |
|
||
| `verify_coarse_path_integration.ps1` | `CoarsePathPlanningService` 一次调用完成多来源建图、缓存复用、规划、回溯和最终验证；复核调试开关不改变地图指纹或规划结果。 |
|
||
| `benchmark_coarse_path.ps1` | Release 构建下的参考场景耗时、扩展节点数、Open List 峰值与托管内存增量。 |
|
||
| `MovementTest.MapTest` | 在 Clumsy UI 中显示人工与 TwoLeg 投影后的环境栅格；可选 PNG 导出只用于调试证据。 |
|
||
| `MovementTest.CoarsePathTest` | 使用固定可复现实例调用 `CoarsePathPlanningService`，绘制地图、起终点、扩大车体检查点和粗路径；`TestStop` 取消规划并清理 Painter，不向底盘发送运动命令。 |
|
||
|
||
每个新增公共契约均需有成功、边界和失败三类测试。测试场景由 `CoarsePathScenarioFactory` 统一生成，不在 `MovementTest` 中手写地图、原语或搜索细节。
|
||
|
||
`MovementTest.CoarsePathTest` 在后台任务中调用同步的 `CoarsePathPlanningService.Plan`，持有专用 `CancellationTokenSource`；`TestStop` 先取消规划，再清理任务引用和 Painter。UI 入口不得在界面线程上执行最长 5 s 的搜索，也不得调用任何底盘运动接口。
|
||
|
||
PowerShell 测试统一使用以下形式执行，绕过本机脚本执行策略差异，并在首个错误处停止：
|
||
|
||
```powershell
|
||
powershell -NoProfile -ExecutionPolicy Bypass -File .\tests\<script>.ps1
|
||
```
|
||
|
||
每个脚本首行设置 `$ErrorActionPreference = 'Stop'`。测试先执行一次项目构建，之后加载同一个 `bin/Debug/netstandard2.0/ClumsyPilot.dll`，不得混用 `obj` 与 `bin` 中的程序集。`netstandard2.0` 实现不得直接使用 `PriorityQueue`、`Math.Clamp`、`double.IsFinite` 或缺少兼容类型时的 `record/init`。
|
||
|
||
### 性能与资源验收
|
||
|
||
- 地图参考场景：20 m × 20 m、0.05 m 分辨率、160,000 格和 100 个圆/矩形障碍。Release 构建预热后连续构建 20 次，首次完整构建 P95 不超过 200 ms；完整快照缓存命中 P95 不超过 5 ms。
|
||
- 地图极限场景：4,000,000 格、100 个障碍。完整构建必须在 3 s 内成功或以明确状态失败；成功时单次托管内存增量不超过 160 MB，不得出现整数溢出或部分发布快照。
|
||
- 默认硬限制：`MaximumExpandedNodes=200000`、`SearchTimeout=5 s`；任一限制触发后必须在下一次循环检查点终止。
|
||
- 参考场景：12 m × 8 m、0.05 m 分辨率、一个阻断直线路径的矩形障碍、起终点距离至少 8 m。Release 构建预热后连续运行 20 次，P95 规划耗时不超过 2 s，单次托管内存增量不超过 256 MB。
|
||
- 压力场景：20 m × 20 m、0.05 m 分辨率、160,000 栅格。无论成功或无解，都必须在 5 s 与 200,000 扩展节点内返回，托管内存增量不超过 512 MB。
|
||
- 性能脚本输出地图规模、状态、耗时、扩展/生成/重开节点数、Open List 峰值和内存增量；超过阈值返回非零退出码。
|
||
|
||
## 非目标与迁移边界
|
||
|
||
- 不修改 `TrajPlanner` 下的 Python 原型，也不把它作为运行时依赖。
|
||
- 不在本阶段实现平滑、SQP、时间轨迹或控制接口；后续模块只消费 `PlanningResult` 中稳定的粗路径与方向分段。
|
||
- 不保留将车辆自身写入规划占据图的兼容开关；若调试可视化需要车辆图形，应作为渲染叠加层。
|
||
- 现有 `Occupancygird_Map/Map_test` 原型中的通用栅格化、TwoLeg 投影和 PNG 调试能力按以上职责迁移。迁移完成后，旧 `GridMapData`、`TrapMapBuilder`、`TrapMapLayerComposer` 与旧 `TrapMapTest` 不再作为公共运行时入口；旧反射测试必须更新到新命名空间和门面，或在等价覆盖后删除。
|
||
- PNG 导出器若保留，只能作为内部测试/可视化适配器消费只读 `PlanningGridMap`，不得重新拥有地图构建、障碍膨胀或车辆足迹写入逻辑。
|
||
- 迁移验收必须证明 `CoarsePathPlanningService` 是推荐的一次调用入口，`PlanningMapFactory` 与 `HybridAStarPlanner` 只作为下层模块门面；旧地图构建器、投影器、栅格化器和搜索内部类型不得成为额外公共服务。
|