From d3feaa7fc0d7f06c9d99bb7a21ff795e20469a4f Mon Sep 17 00:00:00 2001
From: tian <11429339@qq.com>
Date: Wed, 10 Sep 2025 02:55:47 +0800
Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E5=AE=89=E5=85=A8=E4=BC=98?=
=?UTF-8?q?=E5=85=88=E8=B7=AF=E5=BE=84=E7=AE=97=E6=B3=95=EF=BC=88=E5=9F=BA?=
=?UTF-8?q?=E7=A1=80=E7=89=88=EF=BC=8C=E5=8F=AA=E8=AE=A1=E7=AE=97=E4=B8=AD?=
=?UTF-8?q?=E5=BF=83=E8=B7=9D=E7=A6=BB=EF=BC=89?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
doc/requirement/todo_features.md | 10 +
src/Core/PathPlanningModels.cs | 7 +-
src/PathPlanning/AutoPathFinder.cs | 258 +++++++++---------
src/UI/WPF/ViewModels/PathEditingViewModel.cs | 6 +
4 files changed, 148 insertions(+), 133 deletions(-)
diff --git a/doc/requirement/todo_features.md b/doc/requirement/todo_features.md
index e6a0d89..8a06cb9 100644
--- a/doc/requirement/todo_features.md
+++ b/doc/requirement/todo_features.md
@@ -2,6 +2,16 @@
## 功能点
+### [2025/09/09]
+
+1. [ ] （功能）增加安全优先路径策略
+
+### [2025/09/08]
+
+1. [x] （功能）增加局部直线优先路径策略
+2. [x] (性能优化) 提高网格地图创建性能（优化API的使用）
+3. [x] (提高稳定性) 通过idle改进UI事件管理，把反射改成事件通知
+
### [2025/09/07]
1. [ ] （功能）增加插件系统参数配置和管理
diff --git a/src/Core/PathPlanningModels.cs b/src/Core/PathPlanningModels.cs
index 602ae09..f97949b 100644
--- a/src/Core/PathPlanningModels.cs
+++ b/src/Core/PathPlanningModels.cs
@@ -140,7 +140,12 @@ namespace NavisworksTransport
///
/// 直线优先路径 - 优先选择主方向的路径，减少转弯次数
///
- Straightest = 1
+ Straightest = 1,
+
+ ///
+ /// 安全优先路径 - 基于障碍物距离选择安全路径，适合大型车辆居中行驶
+ ///
+ SafestCenter = 2
}
///
diff --git a/src/PathPlanning/AutoPathFinder.cs b/src/PathPlanning/AutoPathFinder.cs
index a2f4679..91b133b 100644
--- a/src/PathPlanning/AutoPathFinder.cs
+++ b/src/PathPlanning/AutoPathFinder.cs
@@ -1364,31 +1364,141 @@ namespace NavisworksTransport.PathPlanning
///
/// 安全优先的A*网格转换方法
- /// 先创建标准网格，再根据障碍物距离调整速度权重
+ /// 🔥 重写：参照局部直线优先算法的成功模式，重新构建网格连接
///
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();
- 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;
}
- ///
- /// 根据安全距离调整A*网格边的速度
- /// 🔥 修复：确保每条边只被设置一次，且双向速度完全一致
- ///
- private void ApplySafetyVelocityWeights(Grid grid, GridMap gridMap, int[,] safetyDistanceMap)
- {
- int adjustedEdges = 0;
- var velocityStats = new Dictionary();
- var processedEdges = new HashSet(); // 防止重复处理同一条边
-
- 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 _loggedDistances = new HashSet();
@@ -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))
diff --git a/src/UI/WPF/ViewModels/PathEditingViewModel.cs b/src/UI/WPF/ViewModels/PathEditingViewModel.cs
index ebf8763..1ce6eea 100644
--- a/src/UI/WPF/ViewModels/PathEditingViewModel.cs
+++ b/src/UI/WPF/ViewModels/PathEditingViewModel.cs
@@ -520,6 +520,12 @@ namespace NavisworksTransport.UI.WPF.ViewModels
Value = PathStrategy.Straightest,
DisplayName = "直线优先",
Description = "优先选择主方向直线路径，减少转弯次数"
+ },
+ new PathStrategyOption
+ {
+ Value = PathStrategy.SafestCenter,
+ DisplayName = "安全优先",
+ Description = "基于障碍物距离选择安全路径，适合大型车辆居中行驶"
}
};