diff --git a/src/PathPlanning/AutoPathFinder.cs b/src/PathPlanning/AutoPathFinder.cs
index ed9a3e3..2a70a1d 100644
--- a/src/PathPlanning/AutoPathFinder.cs
+++ b/src/PathPlanning/AutoPathFinder.cs
@@ -126,10 +126,19 @@ namespace NavisworksTransport.PathPlanning
             }
         }
 
+        /// <summary>
+        /// 速度计算委托：根据当前节点、邻居节点和其他上下文信息计算边的速度
+        /// </summary>
+        private delegate float SpeedCalculator(
+            int currentX, int currentY, int currentLayerIndex, double currentZ,
+            int neighborX, int neighborY, int neighborLayerIndex, double neighborZ,
+            double heightDiff, double baseSpeed, double maxHeightDiffInModelUnits,
+            Point3D startPos, Point3D endPos);
+
         /// <summary>
         /// 构建3D图（每个(x,y,layerIndex)作为独立节点）
         /// </summary>
-        private Graph3DResult BuildGraph3D(GridMap gridMap, ChannelCoverage channelCoverage, double vehicleHeight, Point3D startPos, Point3D endPos)
+        private Graph3DResult BuildGraph3D(GridMap gridMap, ChannelCoverage channelCoverage, double vehicleHeight, Point3D startPos, Point3D endPos, SpeedCalculator speedCalculator = null)
         {
             LogManager.Info("[3D图构建] 开始构建真3D图");
 
@@ -239,7 +248,18 @@ namespace NavisworksTransport.PathPlanning
                                 if (!nodes.TryGetValue((nx, ny, nli), out var neighborNode))
                                     continue;
 
-                                float speed = CalculateSpeedByHeightDiff(heightDiff, baseSpeed, maxHeightDiffInModelUnits);
+                                // 使用策略特定的速度计算器，如果未提供则使用默认的高度差计算
+                                float speed;
+                                if (speedCalculator != null)
+                                {
+                                    speed = speedCalculator(x, y, li, currentZ, nx, ny, nli, neighborLayer.Z,
+                                        heightDiff, baseSpeed, maxHeightDiffInModelUnits, startPos, endPos);
+                                }
+                                else
+                                {
+                                    speed = CalculateSpeedByHeightDiff(heightDiff, baseSpeed, maxHeightDiffInModelUnits);
+                                }
+
                                 maxSpeed = Math.Max(maxSpeed, speed);
                                 var velocity = Velocity.FromKilometersPerHour(speed);
 
@@ -347,7 +367,73 @@ namespace NavisworksTransport.PathPlanning
         /// </summary>
         private Graph3DResult BuildGraphWithStrategy(GridMap gridMap, ChannelCoverage channelCoverage, double vehicleHeight, Point3D startPos, Point3D endPos, PathStrategy strategy)
         {
-            return BuildGraph3D(gridMap, channelCoverage, vehicleHeight, startPos, endPos);
+            SpeedCalculator speedCalc = null;
+
+            switch (strategy)
+            {
+                case PathStrategy.Shortest:
+                    LogManager.Info($"[3D图构建] 使用最短路径策略");
+                    // 默认策略，speedCalc = null，使用高度差计算
+                    break;
+
+                case PathStrategy.Straightest:
+                    LogManager.Info($"[3D图构建] 使用直线优先策略");
+                    // 直线优先：计算沿边方向能够直走的距离，距离越长速度越高
+                    {
+                        speedCalc = (cx, cy, cli, cz, nx, ny, nli, nz, heightDiff, baseSpeed, maxHeightDiff, start, end) =>
+                        {
+                            // 基础速度（根据高度差）
+                            float baseSpeedValue = CalculateSpeedByHeightDiff(heightDiff, baseSpeed, maxHeightDiff);
+
+                            // 计算边的方向
+                            var direction = new GridPoint2D(nx - cx, ny - cy);
+                            var currentPos = new GridPoint2D(cx, cy);
+
+                            // 计算沿该方向能够直走的距离
+                            int straightDistance = CalculateStraightDistance(currentPos, direction, gridMap, vehicleHeight);
+
+                            // 直走距离越长，速度加成越高（最多+30 km/h）
+                            float bonusSpeed = Math.Min(straightDistance * 0.5f, 30.0f);
+                            float finalSpeed = baseSpeedValue + bonusSpeed;
+
+                            return finalSpeed;
+                        };
+                    }
+                    break;
+
+                case PathStrategy.SafestCenter:
+                    LogManager.Info($"[3D图构建] 使用安全优先策略");
+                    // 安全优先：计算安全距离图，基于到障碍物的距离调整速度
+                    {
+                        var safetyDistanceMap = CalculateSafetyDistanceFromGridMap(gridMap);
+                        LogManager.Info($"[3D图构建] 安全距离图计算完成");
+
+                        speedCalc = (cx, cy, cli, cz, nx, ny, nli, nz, heightDiff, baseSpeed, maxHeightDiff, start, end) =>
+                        {
+                            // 基础速度（根据高度差）
+                            float baseSpeedValue = CalculateSpeedByHeightDiff(heightDiff, baseSpeed, maxHeightDiff);
+
+                            // 获取两个节点的安全距离，取较大值（更安全的距离）
+                            int currentSafety = safetyDistanceMap[cx, cy];
+                            int neighborSafety = safetyDistanceMap[nx, ny];
+                            int maxSafety = Math.Max(currentSafety, neighborSafety);
+
+                            // 根据安全距离计算速度系数
+                            float safetySpeed = CalculateSpeedBySafetyDistance(maxSafety);
+
+                            // 返回基础速度和安全速度的加权组合
+                            // 高度差影响基础速度，安全距离影响额外速度
+                            return baseSpeedValue * 0.3f + safetySpeed * 0.7f;
+                        };
+                    }
+                    break;
+
+                default:
+                    LogManager.Warning($"[3D图构建] 未知策略 {strategy}，使用默认最短路径");
+                    break;
+            }
+
+            return BuildGraph3D(gridMap, channelCoverage, vehicleHeight, startPos, endPos, speedCalc);
         }
 
         /// <summary>
@@ -1329,11 +1415,15 @@ namespace NavisworksTransport.PathPlanning
                     if (endCell.HasValue)
                     {
                         // 调试：检查门网格的Z坐标
+                        double endZ = endCell.Value.HeightLayers != null && endCell.Value.HeightLayers.Count > 0
+                            ? endCell.Value.HeightLayers[0].Z
+                            : 0;
+
                         if (endCell.Value.CellType == CategoryAttributeManager.LogisticsElementType.门)
                         {
-                            LogManager.Info($"[路径转换-门] 门网格({endGridPos.X},{endGridPos.Y}) WorldPosition.Z={endCell.Value.WorldPosition.Z:F3}, 位置({endWorldPos.X:F2},{endWorldPos.Y:F2})");
+                            LogManager.Info($"[路径转换-门] 门网格({endGridPos.X},{endGridPos.Y}) HeightLayer[0].Z={endZ:F3}, 位置({endWorldPos.X:F2},{endWorldPos.Y:F2})");
                         }
-                        endWorldPos = new Point3D(endWorldPos.X, endWorldPos.Y, endCell.Value.WorldPosition.Z);
+                        endWorldPos = new Point3D(endWorldPos.X, endWorldPos.Y, endZ);
                     }
                     else
                     {
diff --git a/src/PathPlanning/ChannelBasedGridBuilder.cs b/src/PathPlanning/ChannelBasedGridBuilder.cs
index baaac6f..f77ceee 100644
--- a/src/PathPlanning/ChannelBasedGridBuilder.cs
+++ b/src/PathPlanning/ChannelBasedGridBuilder.cs
@@ -148,7 +148,9 @@ namespace NavisworksTransport.PathPlanning
                     if (cell.IsWalkable)
                     {
                         walkableCount++;
-                        double cellZ = cell.WorldPosition.Z;
+                        double cellZ = cell.HeightLayers != null && cell.HeightLayers.Count > 0
+                            ? cell.HeightLayers[0].Z
+                            : 0;
                         
                         if (cellZ < minZ)
                             minZ = cellZ;
diff --git a/src/PathPlanning/GridCellBuilder.cs b/src/PathPlanning/GridCellBuilder.cs
index c1d19dc..284e964 100644
--- a/src/PathPlanning/GridCellBuilder.cs
+++ b/src/PathPlanning/GridCellBuilder.cs
@@ -25,7 +25,6 @@ namespace NavisworksTransport.PathPlanning
                 CellType = CategoryAttributeManager.LogisticsElementType.通道,
                 IsInChannel = true,
                 ChannelType = ChannelType.Corridor,
-                WorldPosition = worldPosition,
                 RelatedModelItem = channel,
                 SpeedLimit = speedLimit,
                 HeightLayers = new System.Collections.Generic.List<HeightLayer>(),
@@ -49,7 +48,6 @@ namespace NavisworksTransport.PathPlanning
                 CellType = CategoryAttributeManager.LogisticsElementType.门,
                 IsInChannel = false,
                 ChannelType = ChannelType.Other,
-                WorldPosition = worldPosition,
                 RelatedModelItem = door,
                 SpeedLimit = speedLimit,
                 HeightLayers = new System.Collections.Generic.List<HeightLayer>(),
@@ -71,7 +69,6 @@ namespace NavisworksTransport.PathPlanning
                 CellType = CategoryAttributeManager.LogisticsElementType.障碍物,
                 IsInChannel = false,
                 ChannelType = ChannelType.Other,
-                WorldPosition = worldPosition,
                 RelatedModelItem = obstacle,
                 SpeedLimit = 0,
                 HeightLayers = new System.Collections.Generic.List<HeightLayer>(),
@@ -95,7 +92,6 @@ namespace NavisworksTransport.PathPlanning
                 CellType = CategoryAttributeManager.LogisticsElementType.电梯,
                 IsInChannel = false,
                 ChannelType = ChannelType.Other,
-                WorldPosition = worldPosition,
                 RelatedModelItem = elevator,
                 SpeedLimit = speedLimit,
                 HeightLayers = new System.Collections.Generic.List<HeightLayer>(),
@@ -119,7 +115,6 @@ namespace NavisworksTransport.PathPlanning
                 CellType = CategoryAttributeManager.LogisticsElementType.楼梯,
                 IsInChannel = false,
                 ChannelType = ChannelType.Other,
-                WorldPosition = worldPosition,
                 RelatedModelItem = stairs,
                 SpeedLimit = speedLimit,
                 HeightLayers = new System.Collections.Generic.List<HeightLayer>(),
@@ -140,7 +135,6 @@ namespace NavisworksTransport.PathPlanning
                 CellType = CategoryAttributeManager.LogisticsElementType.Unknown,
                 IsInChannel = false,
                 ChannelType = ChannelType.Other,
-                WorldPosition = worldPosition,
                 RelatedModelItem = null,
                 SpeedLimit = 0,
                 HeightLayers = new System.Collections.Generic.List<HeightLayer>(),
diff --git a/src/PathPlanning/GridMap.cs b/src/PathPlanning/GridMap.cs
index 828be01..6c8cc23 100644
--- a/src/PathPlanning/GridMap.cs
+++ b/src/PathPlanning/GridMap.cs
@@ -164,8 +164,7 @@ namespace NavisworksTransport.PathPlanning
                         CellType = CategoryAttributeManager.LogisticsElementType.Unknown, // 修改：默认为未知/空洞类型
                         IsInChannel = false,
                         HeightLayers = new List<HeightLayer>(),
-                        ChannelType = ChannelType.Other,
-                        WorldPosition = worldPos
+                        ChannelType = ChannelType.Other
                     };
                     cell.Cost = cell.GetCost(); // 使用GetCost方法计算成本
                     Cells[x, y] = cell;
@@ -200,16 +199,23 @@ namespace NavisworksTransport.PathPlanning
         }
 
         /// <summary>
-        /// 将网格坐标转换为世界3D坐标（需要显式提供Z坐标）
+        /// 将网格坐标转换为世界3D坐标（使用最低层HeightLayer的Z坐标）
         /// </summary>
         /// <param name="gridPosition">网格坐标</param>
-        /// <param name="z">Z坐标值</param>
         /// <returns>世界3D坐标点</returns>
         public Point3D GridToWorld3D(GridPoint2D gridPosition)
         {
             var world2D = GridToWorld2D(gridPosition);
             var cell = Cells[gridPosition.X, gridPosition.Y];
-            return new Point3D(world2D.X, world2D.Y, cell.WorldPosition.Z);
+
+            // 使用最低层的Z坐标（HeightLayers[0]），如果没有层则用0
+            double z = 0;
+            if (cell.HeightLayers != null && cell.HeightLayers.Count > 0)
+            {
+                z = cell.HeightLayers[0].Z;
+            }
+
+            return new Point3D(world2D.X, world2D.Y, z);
         }
 
 
@@ -259,7 +265,6 @@ namespace NavisworksTransport.PathPlanning
                 IsInChannel = cellType == CategoryAttributeManager.LogisticsElementType.通道,
                 HeightLayers = new List<HeightLayer>(),
                 ChannelType = cellType == CategoryAttributeManager.LogisticsElementType.通道 ? ChannelType.Corridor : ChannelType.Other,
-                WorldPosition = worldPos,
                 SpeedLimit = speedLimit
             };
 
@@ -693,10 +698,6 @@ namespace NavisworksTransport.PathPlanning
         /// </summary>
         public ChannelType ChannelType { get; set; }
 
-        /// <summary>
-        /// 世界坐标位置 - 用于高度计算和空间查询
-        /// </summary>
-        public Point3D WorldPosition { get; set; }
         /// <summary>
         /// 限速（米/秒），0表示未设置限速
         /// </summary>
diff --git a/src/PathPlanning/GridMapGenerator.cs b/src/PathPlanning/GridMapGenerator.cs
index 379b43d..825023b 100644
--- a/src/PathPlanning/GridMapGenerator.cs
+++ b/src/PathPlanning/GridMapGenerator.cs
@@ -520,7 +520,6 @@ namespace NavisworksTransport.PathPlanning
                                 CellType = CategoryAttributeManager.LogisticsElementType.通道,
                                 IsInChannel = true,
                                 ChannelType = ChannelType.Corridor,
-                                WorldPosition = point,
                                 RelatedModelItem = cell.RelatedModelItem, // 保持原有关联
                                 SpeedLimit = cell.SpeedLimit, // 保持原有限速
                                 HeightLayers = cell.HeightLayers, // 保持高度层
@@ -542,7 +541,6 @@ namespace NavisworksTransport.PathPlanning
                                 CellType = CategoryAttributeManager.LogisticsElementType.通道,
                                 IsInChannel = true,
                                 ChannelType = ChannelType.Corridor,
-                                WorldPosition = point,
                                 RelatedModelItem = cell.RelatedModelItem, // 保持原有关联
                                 SpeedLimit = cell.SpeedLimit, // 保持原有限速
                                 HeightLayers = cell.HeightLayers, // 保持高度层
@@ -560,9 +558,7 @@ namespace NavisworksTransport.PathPlanning
                         // 非通道单元格：不应该通过垂直扫描改变其状态
                         // 这种情况理论上不应该发生，因为只对通道单元格进行扫描
                         LogManager.Warning($"[通道2.5D模式] 警告：非通道单元格 ({gridX},{gridY}) 收到高度数据，类型：{cell.CellType}, IsInChannel：{cell.IsInChannel}");
-                        
-                        // 仍然更新世界位置，但保持原有状态
-                        cell.WorldPosition = point;
+
                         // 多层架构：高度区间在HeightLayer中设置
                         // cell.PassableHeight = interval;
                         gridMap.Cells[gridX, gridY] = cell;