增加安全优先路径算法（基础版，只计算中心距离）
This commit is contained in:
parent
95a4c444a6
commit
d3feaa7fc0
@ -2,6 +2,16 @@
|
||||
|
||||
## 功能点
|
||||
|
||||
### [2025/09/09]
|
||||
|
||||
1. [ ] （功能）增加安全优先路径策略
|
||||
|
||||
### [2025/09/08]
|
||||
|
||||
1. [x] （功能）增加局部直线优先路径策略
|
||||
2. [x] (性能优化) 提高网格地图创建性能（优化API的使用）
|
||||
3. [x] (提高稳定性) 通过idle改进UI事件管理，把反射改成事件通知
|
||||
|
||||
### [2025/09/07]
|
||||
|
||||
1. [ ] （功能）增加插件系统参数配置和管理
|
||||
|
||||
@ -140,7 +140,12 @@ namespace NavisworksTransport
|
||||
/// <summary>
|
||||
/// 直线优先路径 - 优先选择主方向的路径，减少转弯次数
|
||||
/// </summary>
|
||||
Straightest = 1
|
||||
Straightest = 1,
|
||||
|
||||
/// <summary>
|
||||
/// 安全优先路径 - 基于障碍物距离选择安全路径，适合大型车辆居中行驶
|
||||
/// </summary>
|
||||
SafestCenter = 2
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
|
||||
@ -1364,31 +1364,141 @@ namespace NavisworksTransport.PathPlanning
|
||||
|
||||
/// <summary>
|
||||
/// 安全优先的A*网格转换方法
|
||||
/// 先创建标准网格，再根据障碍物距离调整速度权重
|
||||
/// 🔥 重写：参照局部直线优先算法的成功模式，重新构建网格连接
|
||||
/// </summary>
|
||||
private Grid ConvertToAStarGridSafestCenter(GridMap gridMap, ChannelCoverage channelCoverage, double vehicleHeight)
|
||||
{
|
||||
try
|
||||
{
|
||||
LogManager.Info($"[安全优先] 开始实施安全优先算法");
|
||||
LogManager.Info($"[安全优先] 开始实施安全优先算法，参照成功的直线优先模式");
|
||||
|
||||
// 清空日志记录集合，确保每次执行都能看到映射关系
|
||||
_loggedDistances.Clear();
|
||||
|
||||
// 1. 使用现有方法创建标准A*网格（完全不变）
|
||||
var grid = ConvertToAStarGridWith2_5D(gridMap, channelCoverage, vehicleHeight);
|
||||
// 1. 创建网格基础结构（参照局部直线优先）
|
||||
double cellSizeInMeters = UnitsConverter.ConvertToMeters(gridMap.CellSize);
|
||||
var gridSize = new GridSize(gridMap.Width, gridMap.Height);
|
||||
var cellSize = new Size(Distance.FromMeters((float)cellSizeInMeters), Distance.FromMeters((float)cellSizeInMeters));
|
||||
|
||||
LogManager.Info($"[安全优先] 标准A*网格创建完成");
|
||||
// 使用基础速度创建网格，后续会动态调整
|
||||
var baseVelocity = Velocity.FromKilometersPerHour(5); // 基础速度
|
||||
var grid = Grid.CreateGridWithLateralConnections(gridSize, cellSize, baseVelocity);
|
||||
|
||||
// 2. 计算安全距离图（复用GridMapGenerator的距离变换算法）
|
||||
LogManager.Info($"[安全优先] A*网格创建完成，尺寸: {gridMap.Width}x{gridMap.Height}");
|
||||
|
||||
// 2. 🔥 关键步骤：断开所有连接，准备重新按安全距离连接
|
||||
for (int x = 0; x < gridMap.Width; x++)
|
||||
{
|
||||
for (int y = 0; y < gridMap.Height; y++)
|
||||
{
|
||||
grid.DisconnectNode(new GridPosition(x, y));
|
||||
}
|
||||
}
|
||||
LogManager.Info($"[安全优先] 所有节点连接已断开，准备重新构建");
|
||||
|
||||
// 3. 计算安全距离图
|
||||
var safetyDistanceMap = CalculateSafetyDistanceFromGridMap(gridMap);
|
||||
|
||||
LogManager.Info($"[安全优先] 安全距离计算完成");
|
||||
|
||||
// 3. 根据安全距离调整边的速度权重
|
||||
ApplySafetyVelocityWeights(grid, gridMap, safetyDistanceMap);
|
||||
// 4. 智能重连：基于安全距离重新构建连接
|
||||
int connectedCells = 0;
|
||||
int heightConstrainedCells = 0;
|
||||
int safetyConnections = 0;
|
||||
var velocityStats = new Dictionary<float, int>();
|
||||
|
||||
LogManager.Info($"[安全优先] 速度权重调整完成");
|
||||
for (int x = 0; x < gridMap.Width; x++)
|
||||
{
|
||||
for (int y = 0; y < gridMap.Height; y++)
|
||||
{
|
||||
var cell = gridMap.Cells[x, y];
|
||||
|
||||
// 检查基本可通行性和高度约束
|
||||
bool isPassable = cell.IsWalkable;
|
||||
if (isPassable && cell.PassableHeights != null && cell.PassableHeights.Any())
|
||||
{
|
||||
bool heightOk = cell.PassableHeights.Any(interval => interval.GetSpan() >= vehicleHeight);
|
||||
if (!heightOk)
|
||||
{
|
||||
heightConstrainedCells++;
|
||||
isPassable = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (isPassable)
|
||||
{
|
||||
var pos = new GridPosition(x, y);
|
||||
var currentSafetyDistance = safetyDistanceMap[x, y];
|
||||
connectedCells++;
|
||||
|
||||
// 🔥 核心改动：右方向连接（基于安全距离）
|
||||
if (x + 1 < gridMap.Width)
|
||||
{
|
||||
var rightCell = gridMap.Cells[x + 1, y];
|
||||
if (IsPassableWithHeight(rightCell, vehicleHeight))
|
||||
{
|
||||
var rightPos = new GridPosition(x + 1, y);
|
||||
var rightSafetyDistance = safetyDistanceMap[x + 1, y];
|
||||
|
||||
// 使用两个位置中更安全的距离
|
||||
var maxSafetyDistance = Math.Max(currentSafetyDistance, rightSafetyDistance);
|
||||
var speedValue = CalculateSpeedBySafetyDistance(maxSafetyDistance);
|
||||
var rightVelocity = Velocity.FromKilometersPerHour(speedValue);
|
||||
|
||||
// 统计速度分布
|
||||
if (velocityStats.ContainsKey(speedValue))
|
||||
velocityStats[speedValue]++;
|
||||
else
|
||||
velocityStats[speedValue] = 1;
|
||||
|
||||
grid.AddEdge(pos, rightPos, rightVelocity);
|
||||
grid.AddEdge(rightPos, pos, rightVelocity);
|
||||
safetyConnections++;
|
||||
}
|
||||
}
|
||||
|
||||
// 🔥 核心改动：下方向连接（基于安全距离）
|
||||
if (y + 1 < gridMap.Height)
|
||||
{
|
||||
var bottomCell = gridMap.Cells[x, y + 1];
|
||||
if (IsPassableWithHeight(bottomCell, vehicleHeight))
|
||||
{
|
||||
var bottomPos = new GridPosition(x, y + 1);
|
||||
var bottomSafetyDistance = safetyDistanceMap[x, y + 1];
|
||||
|
||||
// 使用两个位置中更安全的距离
|
||||
var maxSafetyDistance = Math.Max(currentSafetyDistance, bottomSafetyDistance);
|
||||
var speedValue = CalculateSpeedBySafetyDistance(maxSafetyDistance);
|
||||
var bottomVelocity = Velocity.FromKilometersPerHour(speedValue);
|
||||
|
||||
// 统计速度分布
|
||||
if (velocityStats.ContainsKey(speedValue))
|
||||
velocityStats[speedValue]++;
|
||||
else
|
||||
velocityStats[speedValue] = 1;
|
||||
|
||||
grid.AddEdge(pos, bottomPos, bottomVelocity);
|
||||
grid.AddEdge(bottomPos, pos, bottomVelocity);
|
||||
safetyConnections++;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 输出统计信息
|
||||
LogManager.Info($"[安全优先] 网格重建完成，连接统计:");
|
||||
LogManager.Info($"[安全优先] - 连接的节点: {connectedCells}");
|
||||
LogManager.Info($"[安全优先] - 被高度约束排除: {heightConstrainedCells}");
|
||||
LogManager.Info($"[安全优先] - 安全连接数: {safetyConnections}");
|
||||
LogManager.Info($"[安全优先] - A*可用率: {(double)connectedCells / (gridMap.Width * gridMap.Height) * 100:F1}%");
|
||||
|
||||
// 输出速度统计信息
|
||||
if (velocityStats.Count > 0)
|
||||
{
|
||||
var sortedVelocityStats = velocityStats.OrderBy(kv => kv.Key);
|
||||
var velocityStatsText = string.Join(", ", sortedVelocityStats.Select(kv => $"{kv.Key:F1}km/h:{kv.Value}条边"));
|
||||
LogManager.Info($"[安全优先速度统计] {velocityStatsText}");
|
||||
}
|
||||
|
||||
return grid;
|
||||
}
|
||||
@ -1546,124 +1656,6 @@ namespace NavisworksTransport.PathPlanning
|
||||
return distanceMap;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据安全距离调整A*网格边的速度
|
||||
/// 🔥 修复：确保每条边只被设置一次，且双向速度完全一致
|
||||
/// </summary>
|
||||
private void ApplySafetyVelocityWeights(Grid grid, GridMap gridMap, int[,] safetyDistanceMap)
|
||||
{
|
||||
int adjustedEdges = 0;
|
||||
var velocityStats = new Dictionary<float, int>();
|
||||
var processedEdges = new HashSet<string>(); // 防止重复处理同一条边
|
||||
|
||||
LogManager.Info($"[安全优先] 开始调整边的速度权重，网格尺寸: {gridMap.Width}x{gridMap.Height}");
|
||||
|
||||
// 🔥 核心修复：单向扫描策略，只处理向右和向下的边
|
||||
for (int x = 0; x < gridMap.Width; x++)
|
||||
{
|
||||
for (int y = 0; y < gridMap.Height; y++)
|
||||
{
|
||||
if (!gridMap.Cells[x, y].IsWalkable) continue;
|
||||
|
||||
var currentPos = new GridPosition(x, y);
|
||||
var currentSafetyDistance = safetyDistanceMap[x, y];
|
||||
|
||||
// 处理向右的边（水平边）
|
||||
if (x + 1 < gridMap.Width && gridMap.Cells[x + 1, y].IsWalkable)
|
||||
{
|
||||
var rightPos = new GridPosition(x + 1, y);
|
||||
var rightSafetyDistance = safetyDistanceMap[x + 1, y];
|
||||
|
||||
// 使用两个位置中更安全的距离（取最大值）
|
||||
var maxSafetyDistance = Math.Max(currentSafetyDistance, rightSafetyDistance);
|
||||
var speedValue = CalculateSpeedBySafetyDistance(maxSafetyDistance);
|
||||
var velocity = Velocity.FromKilometersPerHour(speedValue);
|
||||
|
||||
// 统计速度分布（只统计一次）
|
||||
if (velocityStats.ContainsKey(speedValue))
|
||||
velocityStats[speedValue]++;
|
||||
else
|
||||
velocityStats[speedValue] = 1;
|
||||
|
||||
// 生成边的唯一标识符
|
||||
var edgeKey = $"{x},{y}-{x + 1},{y}";
|
||||
|
||||
if (!processedEdges.Contains(edgeKey))
|
||||
{
|
||||
processedEdges.Add(edgeKey);
|
||||
|
||||
try
|
||||
{
|
||||
// 🔥 核心修复：删除旧边，添加新边，确保双向速度一致
|
||||
grid.RemoveEdge(currentPos, rightPos);
|
||||
grid.RemoveEdge(rightPos, currentPos);
|
||||
grid.AddEdge(currentPos, rightPos, velocity);
|
||||
grid.AddEdge(rightPos, currentPos, velocity);
|
||||
adjustedEdges++;
|
||||
|
||||
LogManager.Debug($"[安全优先] 水平边({x},{y})↔({x + 1},{y}): 安全距离{currentSafetyDistance}vs{rightSafetyDistance} -> 最大值{maxSafetyDistance} -> 速度{speedValue:F1}km/h");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
LogManager.Warning($"[安全优先] 水平边更新失败: ({x},{y})↔({x + 1},{y}): {ex.Message}");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 处理向下的边（垂直边）
|
||||
if (y + 1 < gridMap.Height && gridMap.Cells[x, y + 1].IsWalkable)
|
||||
{
|
||||
var downPos = new GridPosition(x, y + 1);
|
||||
var downSafetyDistance = safetyDistanceMap[x, y + 1];
|
||||
|
||||
// 使用两个位置中更安全的距离（取最大值）
|
||||
var maxSafetyDistance = Math.Max(currentSafetyDistance, downSafetyDistance);
|
||||
var speedValue = CalculateSpeedBySafetyDistance(maxSafetyDistance);
|
||||
var velocity = Velocity.FromKilometersPerHour(speedValue);
|
||||
|
||||
// 统计速度分布（只统计一次）
|
||||
if (velocityStats.ContainsKey(speedValue))
|
||||
velocityStats[speedValue]++;
|
||||
else
|
||||
velocityStats[speedValue] = 1;
|
||||
|
||||
// 生成边的唯一标识符
|
||||
var edgeKey = $"{x},{y}-{x},{y + 1}";
|
||||
|
||||
if (!processedEdges.Contains(edgeKey))
|
||||
{
|
||||
processedEdges.Add(edgeKey);
|
||||
|
||||
try
|
||||
{
|
||||
// 🔥 核心修复：删除旧边，添加新边，确保双向速度一致
|
||||
grid.RemoveEdge(currentPos, downPos);
|
||||
grid.RemoveEdge(downPos, currentPos);
|
||||
grid.AddEdge(currentPos, downPos, velocity);
|
||||
grid.AddEdge(downPos, currentPos, velocity);
|
||||
adjustedEdges++;
|
||||
|
||||
LogManager.Debug($"[安全优先] 垂直边({x},{y})↔({x},{y + 1}): 安全距离{currentSafetyDistance}vs{downSafetyDistance} -> 最大值{maxSafetyDistance} -> 速度{speedValue:F1}km/h");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
LogManager.Warning($"[安全优先] 垂直边更新失败: ({x},{y})↔({x},{y + 1}): {ex.Message}");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 输出速度统计信息
|
||||
if (velocityStats.Count > 0)
|
||||
{
|
||||
var sortedVelocityStats = velocityStats.OrderBy(kv => kv.Key);
|
||||
var velocityStatsText = string.Join(", ", sortedVelocityStats.Select(kv => $"{kv.Key:F1}km/h:{kv.Value}条边"));
|
||||
LogManager.Info($"[速度权重统计] {velocityStatsText}");
|
||||
}
|
||||
|
||||
LogManager.Info($"[安全优先] 速度权重调整完成，共处理了 {adjustedEdges} 条边，已处理边集合大小: {processedEdges.Count}");
|
||||
}
|
||||
|
||||
// 静态集合用于记录已打印的速度映射，避免重复日志
|
||||
private static readonly HashSet<int> _loggedDistances = new HashSet<int>();
|
||||
@ -1675,15 +1667,17 @@ namespace NavisworksTransport.PathPlanning
|
||||
{
|
||||
float finalSpeed;
|
||||
|
||||
// 🔥 激进映射：极大的速度差异，强制A*选择安全路径
|
||||
// 🔧 温和映射：参考局部直线优先算法，使用5-35km/h范围，减少网格敏感性
|
||||
if (safetyDistance >= 5)
|
||||
finalSpeed = 100.0f; // 100 km/h（极高速度，强制选择安全区域）
|
||||
finalSpeed = 35.0f; // 35 km/h（最高速度，安全区域）
|
||||
else if (safetyDistance >= 3)
|
||||
finalSpeed = 50.0f; // 50 km/h
|
||||
finalSpeed = 25.0f; // 25 km/h
|
||||
else if (safetyDistance >= 2)
|
||||
finalSpeed = 15.0f; // 15 km/h
|
||||
else if (safetyDistance >= 1)
|
||||
finalSpeed = 10.0f; // 10 km/h
|
||||
else
|
||||
finalSpeed = 1.0f; // 1 km/h（极低速度，避免危险区域）
|
||||
finalSpeed = 5.0f; // 5 km/h（最低速度，但不是极端值）
|
||||
|
||||
// 只在每种距离第一次出现时记录速度映射
|
||||
if (!_loggedDistances.Contains(safetyDistance))
|
||||
|
||||
@ -520,6 +520,12 @@ namespace NavisworksTransport.UI.WPF.ViewModels
|
||||
Value = PathStrategy.Straightest,
|
||||
DisplayName = "直线优先",
|
||||
Description = "优先选择主方向直线路径，减少转弯次数"
|
||||
},
|
||||
new PathStrategyOption
|
||||
{
|
||||
Value = PathStrategy.SafestCenter,
|
||||
DisplayName = "安全优先",
|
||||
Description = "基于障碍物距离选择安全路径，适合大型车辆居中行驶"
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user