234 lines
14 KiB
Markdown
234 lines
14 KiB
Markdown
# 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` 集成/可视化入口都必须提供。
|
||
- 每个文件只承担一个明确职责；对外调用通过模块门面类完成，不让调用者拼装搜索、地图和碰撞的内部对象。
|
||
|
||
## 目录和命名空间
|
||
|
||
```text
|
||
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
|
||
│ ├── PlanningMapRequest.cs
|
||
│ ├── PlanningMapBuildResult.cs
|
||
│ └── PlanningMapFactory.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 模块
|
||
|
||
### 对外调用门面
|
||
|
||
粗路径模块不直接实例化 `EnvironmentMapBuilder`、`MapObstacleRasterizer`、`TwoLegObstacleProjector` 或 `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` 集中地图边界/分辨率、人工障碍物、可选 `TwoLegDetect` 投影输入和空旷地图声明；`PlanningMapBuildResult` 返回 `Succeeded`、失败原因、每个障碍物来源的摘要，以及成功时不可变的 `PlanningGridMap`。因此 A* 的 `PlanningRequest.Map` 始终是已完成校验、mm→m 适配、距离场生成的输入，规划器不需要了解地图构造细节。
|
||
|
||
### 环境地图与图层
|
||
|
||
`EnvironmentGridMap` 保存 mm 单位的地图边界、分辨率和仅含外部障碍物的占据单元。它不接受 `MarkVehicleFootprint` 一类接口。
|
||
|
||
`EnvironmentMapBuilder` 是地图模块的唯一对外构造入口：
|
||
|
||
```csharp
|
||
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` 是地图与规划器之间唯一的单位/数据边界：
|
||
|
||
```csharp
|
||
public sealed class PlanningMapAdapter
|
||
{
|
||
public PlanningGridMap Adapt(EnvironmentGridMap map);
|
||
}
|
||
```
|
||
|
||
适配时验证边界和分辨率、深拷贝占据数据、将长度从 mm 转为 m、将边界外固定解释为占据，并生成 `ObstacleDistanceField`。`PlanningGridMap` 是不可变的规划输入，包含 m 单位边界、分辨率、行列、占据数据、距离场、源地图版本与 `PlanningReady/PlanningBlockReason`。
|
||
|
||
`PlanningReady` 的规则：有效的人工图层即可使地图可用于测试/规划；若没有人工障碍也没有 `TwoLegDetect`，在已明确配置“空旷地图”时仍可规划；未明确空旷语义的未观测区域则以 `PlanningBlockReason` 阻止规划，而不是暗中当作空闲。
|
||
|
||
## CoarsePath 模块
|
||
|
||
### 对外门面与契约
|
||
|
||
调用方只调用 `HybridAStarPlanner`：
|
||
|
||
```csharp
|
||
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` 大文件追加规划功能。
|