2025-09-07 12:44:54 +08:00

7.2 KiB

Raw Blame History

Roy-T.AStar库的使用方法与经验总结

概述

Roy-T.AStar是一个高性能的C# A*寻路算法库，位于C:\Users\Tellme\apps\OpenSource\AStar-master。本文档记录了在NavisworksTransport项目中集成和使用该库的经验教训。

核心概念理解

1. 坐标系统

关键发现：Roy-T.AStar使用米坐标系统，而不是网格索引。

// 创建网格时，传入的是物理尺寸
var cellSize = new Size(
    Distance.FromMeters((float)cellSizeInMeters), 
    Distance.FromMeters((float)cellSizeInMeters)
);

// GridPosition构造函数接受的是网格索引
var gridPos = new GridPosition(x, y);  // x,y是网格索引，如(0,0), (1,0)等

// 但Node.Position返回的是米坐标！
// 例如：网格(1,0)的Node.Position可能是(2.0, 0.0)米（假设cellSize=2米）

2. 路径数据结构

Path对象结构：

Path.Edges: 边的列表（IReadOnlyList）
每条边包含：
- Start: 起始节点
- End: 终止节点
- Distance: 边的长度
- TraversalVelocity: 遍历速度

重要特性：连续边的关系

Edge[0]: Start=A, End=B
Edge[1]: Start=B, End=C  // 注意：Edge[0].End == Edge[1].Start
Edge[2]: Start=C, End=D

常见陷阱与解决方案

陷阱1：坐标转换时的重复点问题

错误做法：

// ❌ 错误：会产生重复点
var gridPath = new List<GridPoint2D>();
gridPath.Add(ConvertToGrid(edges[0].Start));  // 添加起点
foreach (var edge in edges) {
    gridPath.Add(ConvertToGrid(edge.End));    // 每条边的终点
}
// 结果：[A, B, B, C, C, D] - 转弯点重复！

正确做法：

// ✅ 正确：避免重复
var gridPath = new List<GridPoint2D>();
if (edges.Count > 0) {
    gridPath.Add(ConvertToGrid(edges[0].Start));  // 只添加第一个起点
    foreach (var edge in edges) {
        var gridPoint = ConvertToGrid(edge.End);
        // 检查是否与上一个点重复
        if (gridPath.Count == 0 || !gridPath.Last().Equals(gridPoint)) {
            gridPath.Add(gridPoint);
        }
    }
}
// 结果：[A, B, C, D] - 完美的连续路径

陷阱2：米坐标到网格坐标的转换

关键代码：

// 从A*的米坐标转换为网格索引
double cellSizeInMeters = UnitsConverter.ConvertToMeters(gridMap.CellSize);
int gridX = (int)Math.Floor(node.Position.X / cellSizeInMeters);
int gridY = (int)Math.Floor(node.Position.Y / cellSizeInMeters);

陷阱3：网格创建时的节点连接

默认创建方法的限制：

// Roy-T.AStar提供的默认方法会连接所有节点
var grid = Grid.CreateGridWithLateralConnections(gridSize, cellSize, velocity);

自定义障碍物处理：

// 1. 先断开所有连接
for (int x = 0; x < gridMap.Width; x++) {
    for (int y = 0; y < gridMap.Height; y++) {
        grid.DisconnectNode(new GridPosition(x, y));
    }
}

// 2. 只连接可通行的节点
for (int x = 0; x < gridMap.Width; x++) {
    for (int y = 0; y < gridMap.Height; y++) {
        if (IsWalkable(x, y)) {
            // 连接到右侧邻居
            if (x + 1 < width && IsWalkable(x + 1, y)) {
                grid.AddEdge(new GridPosition(x, y), 
                           new GridPosition(x + 1, y), velocity);
            }
            // 连接到下方邻居
            if (y + 1 < height && IsWalkable(x, y + 1)) {
                grid.AddEdge(new GridPosition(x, y), 
                           new GridPosition(x, y + 1), velocity);
            }
        }
    }
}

路径优化策略

1. 网格路径优化算法

问题：A*输出的路径包含大量中间点，需要优化。

解决方案：基于方向变化的路径简化

private List<GridPoint2D> SimplifyPath(List<GridPoint2D> path) {
    if (path.Count < 3) return path;
    
    var simplified = new List<GridPoint2D> { path[0] };
    
    // 计算初始方向（归一化）
    int dx = path[1].X - path[0].X;
    int dy = path[1].Y - path[0].Y;
    var prevDirection = new GridPoint2D(
        dx == 0 ? 0 : Math.Sign(dx),
        dy == 0 ? 0 : Math.Sign(dy)
    );
    
    // 检测方向变化
    for (int i = 2; i < path.Count; i++) {
        dx = path[i].X - path[i-1].X;
        dy = path[i].Y - path[i-1].Y;
        var currentDirection = new GridPoint2D(
            dx == 0 ? 0 : Math.Sign(dx),
            dy == 0 ? 0 : Math.Sign(dy)
        );
        
        // 方向改变时保留转弯点
        if (!currentDirection.Equals(prevDirection)) {
            simplified.Add(path[i - 1]);
            prevDirection = currentDirection;
        }
    }
    
    simplified.Add(path.Last());  // 添加终点
    return simplified;
}

2. 方向归一化的重要性

问题：不同步长的移动被误判为转弯

(-1, 0) → (-2, 0)：都是向左，但步长不同
(0, -1) → (0, -3)：都是向上，但步长不同

解决：使用Math.Sign()归一化方向向量

// 归一化为单位方向向量 (-1, 0, 1)
int dirX = dx == 0 ? 0 : Math.Sign(dx);
int dirY = dy == 0 ? 0 : Math.Sign(dy);

性能优化建议

1. 路径缓存

对于频繁查询的路径，考虑缓存结果：

private Dictionary<(Point3D, Point3D), Path> _pathCache;

2. 分层寻路

对于大型地图，可以使用分层A*算法：

高层：粗略网格，快速找到大致路径
低层：精细网格，优化局部路径

3. 动态障碍物

Roy-T.AStar支持动态修改网格连接：

// 添加障碍物
grid.DisconnectNode(position);

// 移除障碍物
grid.AddEdge(from, to, velocity);

2.5D路径规划扩展

高度约束处理

// 检查节点是否满足高度约束
if (cell.PassableHeights != null && cell.PassableHeights.Any()) {
    bool heightOk = cell.PassableHeights.Any(
        interval => interval.GetSpan() >= vehicleHeight
    );
    if (!heightOk) {
        grid.DisconnectNode(position);  // 不满足高度要求，断开连接
    }
}

调试技巧

1. 日志输出

LogManager.Info($"[A*执行] 找到路径，包含 {path.Edges.Count} 条边");
LogManager.Debug($"[路径优化] 方向从({prev.X},{prev.Y})变为({curr.X},{curr.Y})");

2. 路径验证

// 验证路径连续性
for (int i = 1; i < path.Count; i++) {
    var dist = Math.Abs(path[i].X - path[i-1].X) + 
               Math.Abs(path[i].Y - path[i-1].Y);
    if (dist > 1) {
        LogManager.Warning($"路径不连续：从{path[i-1]}到{path[i]}");
    }
}

实际优化效果

在NavisworksTransport项目中的实测结果：

原始A*输出：101个路径点
优化后：19个关键转弯点
优化率：81.2%
处理时间：约8ms

总结

使用Roy-T.AStar库的关键要点：

理解米坐标系统，正确进行坐标转换
注意Path.Edges的连续性，避免重复点
使用方向归一化进行路径优化
灵活运用DisconnectNode和AddEdge处理障碍物
对于2.5D场景，在网格连接阶段处理高度约束

参考资源

Roy-T.AStar源码：C:\Users\Tellme\apps\OpenSource\AStar-master
NavisworksTransport集成代码：
- src\PathPlanning\AutoPathFinder.cs
- src\PathPlanning\PathOptimizer.cs

7.2 KiB Raw Blame History Unescape Escape