195 lines
5.7 KiB
Markdown
195 lines
5.7 KiB
Markdown
# 多高度层GridMap解决方案
|
||
|
||
**日期**: 2025-01-08
|
||
**问题**: 楼梯与楼板在同一(x,y)坐标重叠时，2D网格投影导致互相覆盖
|
||
**目标**: 支持多高度层，同时保留RoyT.AStar高性能算法
|
||
|
||
---
|
||
|
||
## 问题分析
|
||
|
||
### 场景描述
|
||
- **楼梯**: 倾斜通道，在(x,y)位置有不同Z高度
|
||
- **楼梯下方楼板**: 水平通道，在相同(x,y)位置也可通行
|
||
- **当前问题**: 两者投影到同一2D网格坐标(x,y)，后处理的覆盖前处理的
|
||
|
||
### 核心矛盾
|
||
- 2D网格投影系统无法处理垂直空间层叠
|
||
- 起点在楼面(Z=3.0m)，终点在楼梯(Z=5.0m)的路径无法正确规划
|
||
|
||
---
|
||
|
||
## 解决方案
|
||
|
||
### 核心思路
|
||
1. **保留RoyT.AStar**: 2D A*算法不变，继续产生最优2D路径
|
||
2. **扩展数据结构**: GridCell支持多个HeightLayer
|
||
3. **投影改为追加**: 通道投影不覆盖，而是添加新层
|
||
4. **路径后处理选层**: A*产生2D路径后，为每个路径点智能选择高度层
|
||
|
||
### 关键洞察
|
||
- **2D路径已经最优**: RoyT.AStar产生的2D网格序列`[(x1,y1), (x2,y2), ..., (xn,yn)]`不需要改变
|
||
- **只需选Z坐标**: 为每个2D点选择正确的高度层，形成3D路径
|
||
- **基于高度趋势**: 根据当前位置到终点的剩余高度变化，选择最合适的层
|
||
|
||
---
|
||
|
||
## 技术设计
|
||
|
||
### 1. 数据结构扩展
|
||
|
||
#### HeightLayer结构
|
||
```csharp
|
||
public struct HeightLayer
|
||
{
|
||
public double Z; // 该层的Z坐标
|
||
public HeightInterval PassableHeight; // 可通行高度范围（用于物体检查）
|
||
public ModelItem SourceItem; // 来源模型项
|
||
public double SpeedLimit; // 限速
|
||
public CategoryAttributeManager.LogisticsElementType Type; // 层类型
|
||
}
|
||
```
|
||
|
||
#### GridCell扩展
|
||
```csharp
|
||
public struct GridCell
|
||
{
|
||
// ... 保留所有现有字段 ...
|
||
|
||
// 新增：多高度层支持
|
||
public List<HeightLayer> HeightLayers { get; set; }
|
||
}
|
||
```
|
||
|
||
### 2. 通道投影改为追加模式
|
||
|
||
**文件**: `src/PathPlanning/ChannelBasedGridBuilder.cs`
|
||
**方法**: `RasterizeTriangleToGrid()`
|
||
|
||
**修改前**:
|
||
```csharp
|
||
var cell = GridCellBuilder.Channel(preciseWorldPosition, channel, channelSpeedLimit);
|
||
gridMap.PlaceCell(gridPos, cell); // 覆盖
|
||
```
|
||
|
||
**修改后**:
|
||
```csharp
|
||
var layer = new HeightLayer
|
||
{
|
||
Z = gridCenterHeight,
|
||
PassableHeight = new HeightInterval(0, 0), // 初始化，后续设置
|
||
SourceItem = channel,
|
||
SpeedLimit = channelSpeedLimit,
|
||
Type = CategoryAttributeManager.GetLogisticsElementType(channel)
|
||
};
|
||
gridMap.AddHeightLayer(gridPos, layer); // 追加
|
||
```
|
||
|
||
### 3. PassableHeight计算适配
|
||
|
||
**保留现有逻辑**: `SetChannelPassableHeights()` 已经很完善
|
||
**修改点**: 从设置单层GridCell改为设置多个HeightLayer
|
||
|
||
```csharp
|
||
// 为每个高度层设置PassableHeight
|
||
if (cell.HeightLayers != null && cell.HeightLayers.Any())
|
||
{
|
||
for (int i = 0; i < cell.HeightLayers.Count; i++)
|
||
{
|
||
var layer = cell.HeightLayers[i];
|
||
layer.PassableHeight = new HeightInterval(0, scanHeightInModelUnits);
|
||
cell.HeightLayers[i] = layer;
|
||
}
|
||
}
|
||
```
|
||
|
||
### 4. 路径后处理智能选层
|
||
|
||
**核心算法**: `AssignLayerBased3DCoordinates()`
|
||
|
||
**选层策略**:
|
||
```
|
||
1. 起点锁定: 选择包含start.Z的层
|
||
2. 终点锁定: 选择包含end.Z的层
|
||
3. 中间点选层:
|
||
- 计算剩余高度变化: remainingHeight = end.Z - current.Z
|
||
- 计算期望每步变化: desiredChange = remainingHeight / remainingSteps
|
||
- 目标Z: targetZ = prevZ + desiredChange
|
||
- 选择最接近targetZ且净空足够的层
|
||
```
|
||
|
||
**示例执行**:
|
||
```
|
||
场景: 起点楼面(3.0m) → 终点楼梯(5.0m)
|
||
2D路径: [(0,0), (1,1), (2,2), (3,3)]
|
||
|
||
赋值过程:
|
||
1. (0,0): Z=3.0 (起点锁定楼面层)
|
||
2. (1,1): remainingHeight=2.0, steps=2, desiredChange=1.0
|
||
targetZ=4.0, 选择楼梯层3.5m (更接近4.0)
|
||
3. (2,2): remainingHeight=1.5, steps=1, desiredChange=1.5
|
||
targetZ=5.0, 选择楼梯层4.5m
|
||
4. (3,3): Z=5.0 (终点锁定楼梯层)
|
||
|
||
最终路径: [(0,0,3.0), (1,1,3.5), (2,2,4.5), (3,3,5.0)]
|
||
✅ 平滑爬升！
|
||
```
|
||
|
||
---
|
||
|
||
## 实施计划
|
||
|
||
### 步骤1: 扩展GridCell支持多层
|
||
- **文件**: `src/PathPlanning/GridMap.cs`
|
||
- **内容**: 新增HeightLayer结构、扩展GridCell、添加AddHeightLayer()和FindLayerContainingZ()方法
|
||
|
||
### 步骤2: 通道投影改为追加模式
|
||
- **文件**: `src/PathPlanning/ChannelBasedGridBuilder.cs`
|
||
- **内容**: 修改RasterizeTriangleToGrid()，从PlaceCell改为AddHeightLayer
|
||
|
||
### 步骤3: 适配PassableHeight计算
|
||
- **文件**: `src/PathPlanning/GridMapGenerator.cs`
|
||
- **内容**: 修改SetChannelPassableHeights()支持多层
|
||
|
||
### 步骤4: 实现智能选层算法
|
||
- **文件**: `src/PathPlanning/AutoPathFinder.cs`
|
||
- **内容**: 新增AssignLayerBased3DCoordinates()和SelectBestLayer()方法，替换现有路径后处理
|
||
|
||
### 步骤5: 测试验证
|
||
- 起点楼面 → 终点楼梯（验证爬升）
|
||
- 起点楼梯 → 终点楼面（验证下降）
|
||
- 楼梯下方通行（验证多层并存）
|
||
|
||
---
|
||
|
||
## 关键约束
|
||
|
||
1. **保留RoyT.AStar**: 不修改A*算法，保持高性能
|
||
2. **不考虑向后兼容**: 直接改为多层架构
|
||
3. **保留现有PassableHeight逻辑**: 只适配到多层结构
|
||
4. **最小化改动**: 只修改必要的数据结构和投影逻辑
|
||
|
||
---
|
||
|
||
## 预期效果
|
||
|
||
- ✅ 解决楼梯-楼板重叠问题
|
||
- ✅ 自动选择最优高度层
|
||
- ✅ 平滑的高度过渡
|
||
- ✅ 保持RoyT.AStar高性能
|
||
- ✅ 代码改动最小化
|
||
|
||
---
|
||
|
||
## 文件修改清单
|
||
|
||
1. `src/PathPlanning/GridMap.cs` - 数据结构扩展
|
||
2. `src/PathPlanning/ChannelBasedGridBuilder.cs` - 投影逻辑修改
|
||
3. `src/PathPlanning/GridMapGenerator.cs` - PassableHeight计算适配
|
||
4. `src/PathPlanning/AutoPathFinder.cs` - 路径后处理智能选层
|
||
|
||
---
|
||
|
||
**状态**: 设计完成，待实施
|
||
**预计工作量**: 4个文件，约200行代码修改
|