diff --git a/.claude/settings.local.json b/.claude/settings.local.json
index 551525e..9263755 100644
--- a/.claude/settings.local.json
+++ b/.claude/settings.local.json
@@ -158,7 +158,8 @@
       "Bash(git restore:*)",
       "Read(//c/Program Files/Autodesk/Navisworks Manage 2026/**)",
       "Bash(where:*)",
-      "Read(//c/Users/Tellme/Pictures/Screenshots/**)"
+      "Read(//c/Users/Tellme/Pictures/Screenshots/**)",
+      "Bash(./deploy-plugin.bat)"
     ],
     "deny": [],
     "additionalDirectories": [
diff --git a/src/PathPlanning/AutoPathFinder.cs b/src/PathPlanning/AutoPathFinder.cs
index e8395d4..ed9a3e3 100644
--- a/src/PathPlanning/AutoPathFinder.cs
+++ b/src/PathPlanning/AutoPathFinder.cs
@@ -43,21 +43,464 @@ namespace NavisworksTransport.PathPlanning
     /// <summary>
     /// 自动路径查找器
     /// 基于A*算法的路径规划核心类
-    /// 支持传统模式和2.5D高度约束的路径规划
+    /// 支持传统模式和真3D图路径规划
     /// </summary>
     public class AutoPathFinder
     {
         /// <summary>
-        /// 2.5D路径规划（使用通道覆盖数据、高度约束和路径策略）
+        /// 水平边连接允许的最大高度差（米）
+        /// 足以支持台阶连接（约0.15米）和地面到楼梯底部的连接（约0.3米）
+        /// 使用时需转换为模型单位
+        /// </summary>
+        private const double MAX_HEIGHT_DIFF_METERS = 0.35;
+
+        /// <summary>
+        /// 3D节点信息（存储每个A*节点对应的3D坐标和类型）
+        /// </summary>
+        private class Node3DInfo
+        {
+            public int X { get; set; }
+            public int Y { get; set; }
+            public int LayerIndex { get; set; }
+            public double Z { get; set; }
+            public CategoryAttributeManager.LogisticsElementType CellType { get; set; }
+        }
+
+        /// <summary>
+        /// 3D图构建结果
+        /// </summary>
+        private class Graph3DResult
+        {
+            /// <summary>
+            /// 节点字典：(x, y, layerIndex) -> A*节点
+            /// </summary>
+            public Dictionary<(int x, int y, int layerIndex), Roy_T.AStar.Graphs.Node> Nodes { get; set; }
+
+            /// <summary>
+            /// 节点信息字典：A*节点 -> 3D信息
+            /// </summary>
+            public Dictionary<Roy_T.AStar.Graphs.Node, Node3DInfo> NodeInfo { get; set; }
+
+            /// <summary>
+            /// 所有节点列表（用于查找最近节点）
+            /// </summary>
+            public List<Roy_T.AStar.Graphs.Node> AllNodes { get; set; }
+
+            /// <summary>
+            /// 最大速度
+            /// </summary>
+            public Velocity MaxVelocity { get; set; }
+        }
+
+        /// <summary>
+        /// 真3D路径规划（使用通道覆盖数据、高度约束和路径策略）
         /// </summary>
-        /// <param name="start">起点（世界坐标）</param>
-        /// <param name="end">终点（世界坐标）</param>
-        /// <param name="gridMap">网格地图</param>
-        /// <param name="channelCoverage">通道覆盖数据</param>
-        /// <param name="vehicleHeight">车辆高度（模型单位）</param>
-        /// <param name="strategy">路径规划策略</param>
-        /// <returns>路径查找结果</returns>
         public PathFindingResult FindPath(Point3D start, Point3D end, GridMap gridMap, ChannelCoverage channelCoverage, double vehicleHeight, PathStrategy strategy = PathStrategy.Shortest)
+        {
+            try
+            {
+                LogManager.Info("[3D路径规划] 开始执行");
+
+                // 构建3D图
+                var graph3D = BuildGraphWithStrategy(gridMap, channelCoverage, vehicleHeight, start, end, strategy);
+
+                // 执行A*搜索
+                var pathResult = ExecuteAStarOnGraph(start, end, gridMap, graph3D);
+
+                // 优化路径
+                pathResult = OptimizePath(pathResult, gridMap);
+
+                return pathResult;
+            }
+            catch (Exception ex)
+            {
+                LogManager.Error($"[3D路径规划] 发生异常: {ex.Message}\n{ex.StackTrace}");
+                return new PathFindingResult
+                {
+                    PathPoints = new List<Point3D> { start, end },
+                    IsComplete = false,
+                    OriginalEndPoint = end,
+                    ActualEndPoint = start,
+                    CompletionPercentage = 0
+                };
+            }
+        }
+
+        /// <summary>
+        /// 构建3D图（每个(x,y,layerIndex)作为独立节点）
+        /// </summary>
+        private Graph3DResult BuildGraph3D(GridMap gridMap, ChannelCoverage channelCoverage, double vehicleHeight, Point3D startPos, Point3D endPos)
+        {
+            LogManager.Info("[3D图构建] 开始构建真3D图");
+
+            var nodes = new Dictionary<(int x, int y, int layerIndex), Roy_T.AStar.Graphs.Node>();
+            var nodeInfo = new Dictionary<Roy_T.AStar.Graphs.Node, Node3DInfo>();
+            var allNodes = new List<Roy_T.AStar.Graphs.Node>();
+
+            double baseSpeed = 5.0; // km/h
+            float maxSpeed = (float)baseSpeed;
+
+            // 将米转换为模型单位
+            double maxHeightDiffInModelUnits = MAX_HEIGHT_DIFF_METERS * UnitsConverter.GetMetersToUnitsConversionFactor();
+            LogManager.Info($"[3D图构建] 高度差阈值: {MAX_HEIGHT_DIFF_METERS}米 = {maxHeightDiffInModelUnits:F2}模型单位");
+
+            // === 阶段1：创建所有3D节点 ===
+            int totalNodesCreated = 0;
+            for (int x = 0; x < gridMap.Width; x++)
+            {
+                for (int y = 0; y < gridMap.Height; y++)
+                {
+                    var cell = gridMap.Cells[x, y];
+                    if (!cell.IsWalkable || cell.HeightLayers.Count == 0)
+                        continue;
+
+                    // 为每个高度层创建独立节点
+                    for (int li = 0; li < cell.HeightLayers.Count; li++)
+                    {
+                        var layer = cell.HeightLayers[li];
+
+                        // 楼梯/电梯区域跳过层0（下方楼板，不是可行走表面）
+                        if (li == 0 && (layer.Type == CategoryAttributeManager.LogisticsElementType.楼梯 ||
+                                         layer.Type == CategoryAttributeManager.LogisticsElementType.电梯))
+                        {
+                            continue;
+                        }
+
+                        if (layer.PassableHeight.GetSpan() < vehicleHeight)
+                            continue;
+
+                        // 使用2D位置（用于A*启发式）
+                        var position = new Position(x, y);
+                        var node = new Roy_T.AStar.Graphs.Node(position);
+
+                        nodes[(x, y, li)] = node;
+                        nodeInfo[node] = new Node3DInfo
+                        {
+                            X = x,
+                            Y = y,
+                            LayerIndex = li,
+                            Z = layer.Z,
+                            CellType = layer.Type
+                        };
+                        allNodes.Add(node);
+                        totalNodesCreated++;
+                    }
+                }
+            }
+
+            LogManager.Info($"[3D图构建] 阶段1完成：创建了 {totalNodesCreated} 个3D节点");
+
+            // === 阶段2：创建水平边（同层的相邻节点，高度差在阈值内）===
+            var directions = new[] { (1, 0), (-1, 0), (0, 1), (0, -1) };
+            int horizontalEdges = 0;
+
+            for (int x = 0; x < gridMap.Width; x++)
+            {
+                for (int y = 0; y < gridMap.Height; y++)
+                {
+                    var cell = gridMap.Cells[x, y];
+                    if (!cell.IsWalkable || cell.HeightLayers.Count == 0)
+                        continue;
+
+                    for (int li = 0; li < cell.HeightLayers.Count; li++)
+                    {
+                        var currentLayer = cell.HeightLayers[li];
+                        if (currentLayer.PassableHeight.GetSpan() < vehicleHeight)
+                            continue;
+
+                        if (!nodes.TryGetValue((x, y, li), out var currentNode))
+                            continue;
+
+                        double currentZ = currentLayer.Z;
+
+                        foreach (var (dx, dy) in directions)
+                        {
+                            int nx = x + dx;
+                            int ny = y + dy;
+
+                            if (nx < 0 || nx >= gridMap.Width || ny < 0 || ny >= gridMap.Height)
+                                continue;
+
+                            var neighborCell = gridMap.Cells[nx, ny];
+                            if (!neighborCell.IsWalkable)
+                                continue;
+
+                            // 查找邻居中所有满足高度约束的层
+                            for (int nli = 0; nli < neighborCell.HeightLayers.Count; nli++)
+                            {
+                                var neighborLayer = neighborCell.HeightLayers[nli];
+                                if (neighborLayer.PassableHeight.GetSpan() < vehicleHeight)
+                                    continue;
+
+                                double heightDiff = Math.Abs(neighborLayer.Z - currentZ);
+                                if (heightDiff > maxHeightDiffInModelUnits)
+                                    continue;
+
+                                if (!nodes.TryGetValue((nx, ny, nli), out var neighborNode))
+                                    continue;
+
+                                float speed = CalculateSpeedByHeightDiff(heightDiff, baseSpeed, maxHeightDiffInModelUnits);
+                                maxSpeed = Math.Max(maxSpeed, speed);
+                                var velocity = Velocity.FromKilometersPerHour(speed);
+
+                                currentNode.Connect(neighborNode, velocity);
+                                horizontalEdges++;
+                            }
+                        }
+                    }
+                }
+            }
+
+            LogManager.Info($"[3D图构建] 阶段2完成：创建了 {horizontalEdges} 条水平边");
+
+            // === 阶段3：创建垂直边（仅在楼梯/电梯内部的相邻层）===
+            int verticalEdges = 0;
+
+            for (int x2 = 0; x2 < gridMap.Width; x2++)
+            {
+                for (int y2 = 0; y2 < gridMap.Height; y2++)
+                {
+                    var cell = gridMap.Cells[x2, y2];
+
+                    // 检查该网格是否有任何楼梯或电梯层（允许垂直移动）
+                    bool hasStairsOrElevator = cell.HeightLayers.Any(layer =>
+                        layer.Type == CategoryAttributeManager.LogisticsElementType.楼梯 ||
+                        layer.Type == CategoryAttributeManager.LogisticsElementType.电梯);
+
+                    if (!hasStairsOrElevator || cell.HeightLayers.Count < 2)
+                        continue;
+
+                    // 连接同一(x,y)位置的不同高度层（相邻层之间）
+                    for (int li = 0; li < cell.HeightLayers.Count - 1; li++)
+                    {
+                        var layer1 = cell.HeightLayers[li];
+                        var layer2 = cell.HeightLayers[li + 1];
+
+                        // 垂直边只连接相同类型的层（楼梯到楼梯，电梯到电梯）
+                        // 不允许从通道跳到楼梯，必须通过水平边进入楼梯
+                        if (layer1.Type != layer2.Type)
+                            continue;
+
+                        // 垂直边只在楼梯/电梯类型之间创建
+                        if (layer1.Type != CategoryAttributeManager.LogisticsElementType.楼梯 &&
+                            layer1.Type != CategoryAttributeManager.LogisticsElementType.电梯)
+                            continue;
+
+                        if (layer1.PassableHeight.GetSpan() < vehicleHeight ||
+                            layer2.PassableHeight.GetSpan() < vehicleHeight)
+                            continue;
+
+                        double heightDiff = Math.Abs(layer2.Z - layer1.Z);
+                        if (heightDiff > maxHeightDiffInModelUnits)
+                            continue;
+
+                        if (!nodes.TryGetValue((x2, y2, li), out var node1) ||
+                            !nodes.TryGetValue((x2, y2, li + 1), out var node2))
+                            continue;
+
+                        // 垂直移动速度：电梯较快，楼梯较慢
+                        float verticalSpeed = layer1.Type == CategoryAttributeManager.LogisticsElementType.电梯
+                            ? (float)(baseSpeed * 0.5)
+                            : (float)(baseSpeed * 0.3);
+                        maxSpeed = Math.Max(maxSpeed, verticalSpeed);
+                        var velocity = Velocity.FromKilometersPerHour(verticalSpeed);
+
+                        // 双向连接
+                        node1.Connect(node2, velocity);
+                        node2.Connect(node1, velocity);
+                        verticalEdges += 2;
+                    }
+                }
+            }
+
+            LogManager.Info($"[3D图构建] 阶段3完成：创建了 {verticalEdges} 条垂直边（双向）");
+            LogManager.Info($"[3D图构建] 总结：{totalNodesCreated}个节点，{horizontalEdges + verticalEdges}条边");
+
+            return new Graph3DResult
+            {
+                Nodes = nodes,
+                NodeInfo = nodeInfo,
+                AllNodes = allNodes,
+                MaxVelocity = Velocity.FromKilometersPerHour(maxSpeed)
+            };
+        }
+
+        /// <summary>
+        /// 根据高度差计算速度（高度差单位：模型单位）
+        /// </summary>
+        /// <param name="heightDiff">高度差（模型单位）</param>
+        /// <param name="baseSpeed">基础速度（km/h）</param>
+        /// <param name="maxHeightDiff">最大允许高度差（模型单位，对应0.5米）</param>
+        private float CalculateSpeedByHeightDiff(double heightDiff, double baseSpeed, double maxHeightDiff)
+        {
+            // 计算相对高度差比例
+            double heightRatio = heightDiff / maxHeightDiff;
+
+            if (heightRatio < 0.06) return (float)baseSpeed;           // 平地 (<3cm，即<0.06*0.5m)
+            if (heightRatio <= 0.5) return (float)(baseSpeed * 0.8);   // 缓坡 (≤0.25m)
+            if (heightRatio <= 1.0) return (float)(baseSpeed * 0.5);   // 楼梯 (≤0.5m)
+            return (float)(baseSpeed * 0.1);                           // 过陡 (>0.5m)
+        }
+
+        /// <summary>
+        /// 根据策略构建图
+        /// </summary>
+        private Graph3DResult BuildGraphWithStrategy(GridMap gridMap, ChannelCoverage channelCoverage, double vehicleHeight, Point3D startPos, Point3D endPos, PathStrategy strategy)
+        {
+            return BuildGraph3D(gridMap, channelCoverage, vehicleHeight, startPos, endPos);
+        }
+
+        /// <summary>
+        /// 在3D图上执行A*搜索
+        /// </summary>
+        private PathFindingResult ExecuteAStarOnGraph(Point3D start, Point3D end, GridMap gridMap, Graph3DResult graph3D)
+        {
+            var startGridPos = gridMap.WorldToGrid(start);
+            var endGridPos = gridMap.WorldToGrid(end);
+
+            // 找到起点和终点最近的3D节点
+            var startNode = FindClosest3DNode(graph3D, startGridPos, start.Z);
+            var endNode = FindClosest3DNode(graph3D, endGridPos, end.Z);
+
+            if (startNode == null || endNode == null)
+            {
+                LogManager.Error($"[A*执行-3D] 无法找到起点或终点节点");
+                return new PathFindingResult
+                {
+                    PathPoints = new List<Point3D> { start, end },
+                    IsComplete = false,
+                    OriginalEndPoint = end,
+                    ActualEndPoint = start,
+                    CompletionPercentage = 0
+                };
+            }
+
+            var startInfo = graph3D.NodeInfo[startNode];
+            var endInfo = graph3D.NodeInfo[endNode];
+            LogManager.Info($"[A*执行-3D] 起点映射: ({startGridPos.X},{startGridPos.Y},Z={start.Z:F2}) -> 节点({startInfo.X},{startInfo.Y},层{startInfo.LayerIndex},Z={startInfo.Z:F2})");
+            LogManager.Info($"[A*执行-3D] 终点映射: ({endGridPos.X},{endGridPos.Y},Z={end.Z:F2}) -> 节点({endInfo.X},{endInfo.Y},层{endInfo.LayerIndex},Z={endInfo.Z:F2})");
+
+            // 执行A*
+            var pathfinder = new Roy_T.AStar.Paths.PathFinder();
+            var astarPath = pathfinder.FindPath(startNode, endNode, graph3D.MaxVelocity);
+
+            if (astarPath.Edges.Count == 0)
+            {
+                LogManager.Warning($"[A*执行-3D] 未找到路径");
+                return new PathFindingResult
+                {
+                    PathPoints = new List<Point3D> { start, end },
+                    IsComplete = false,
+                    OriginalEndPoint = end,
+                    ActualEndPoint = start,
+                    CompletionPercentage = 0
+                };
+            }
+
+            // 转换为世界坐标路径
+            var path3D = ConvertGraphPathToWorld(astarPath, graph3D, gridMap, start, end);
+
+            double completionPercentage = 100.0;
+            bool isComplete = true;
+            Point3D actualEnd = path3D.Count > 0 ? path3D[path3D.Count - 1] : start;
+
+            LogManager.Info($"[3D路径规划] 路径生成完成，包含 {path3D.Count} 个路径点，完成度: {completionPercentage:F1}%");
+
+            return new PathFindingResult
+            {
+                PathPoints = path3D,
+                IsComplete = isComplete,
+                OriginalEndPoint = end,
+                ActualEndPoint = actualEnd,
+                CompletionPercentage = completionPercentage
+            };
+        }
+
+        /// <summary>
+        /// 找到最接近指定位置和高度的3D节点
+        /// </summary>
+        private Roy_T.AStar.Graphs.Node FindClosest3DNode(Graph3DResult graph3D, GridPoint2D gridPos, double targetZ)
+        {
+            Roy_T.AStar.Graphs.Node closestNode = null;
+            double minDist = double.MaxValue;
+
+            foreach (var node in graph3D.AllNodes)
+            {
+                var info = graph3D.NodeInfo[node];
+                if (info.X == gridPos.X && info.Y == gridPos.Y)
+                {
+                    double heightDiff = Math.Abs(info.Z - targetZ);
+                    if (heightDiff < minDist)
+                    {
+                        minDist = heightDiff;
+                        closestNode = node;
+                    }
+                }
+            }
+
+            return closestNode;
+        }
+
+        /// <summary>
+        /// 将图路径转换为世界坐标路径
+        /// </summary>
+        private List<Point3D> ConvertGraphPathToWorld(Roy_T.AStar.Paths.Path astarPath, Graph3DResult graph3D, GridMap gridMap, Point3D originalStart, Point3D originalEnd)
+        {
+            var path3D = new List<Point3D>();
+            path3D.Add(originalStart);
+
+            int pointIndex = 0;
+            LogManager.Info($"[路径点{pointIndex}] 原始起点: ({originalStart.X:F2}, {originalStart.Y:F2}, Z={originalStart.Z:F2})");
+            pointIndex++;
+
+            // 添加第一条边的起点（如果与originalStart不同）
+            if (astarPath.Edges.Count > 0)
+            {
+                var firstEdge = astarPath.Edges[0];
+                var firstNode = firstEdge.Start as Roy_T.AStar.Graphs.Node;
+                if (firstNode != null && graph3D.NodeInfo.ContainsKey(firstNode))
+                {
+                    var firstInfo = graph3D.NodeInfo[firstNode];
+                    var firstPos = gridMap.GridToWorld2D(new GridPoint2D(firstInfo.X, firstInfo.Y));
+                    var firstPoint = new Point3D(firstPos.X, firstPos.Y, firstInfo.Z);
+
+                    // 如果与起点高度不同，添加这个节点（避免直接跳跃）
+                    if (Math.Abs(firstPoint.Z - originalStart.Z) > 0.01)
+                    {
+                        path3D.Add(firstPoint);
+                        LogManager.Info($"[路径点{pointIndex}] A*起点: 网格({firstInfo.X},{firstInfo.Y}), 层{firstInfo.LayerIndex}, Z={firstInfo.Z:F2}, 类型={firstInfo.CellType}");
+                        pointIndex++;
+                    }
+                }
+            }
+
+            // 添加所有A*路径节点
+            foreach (var edge in astarPath.Edges)
+            {
+                var endNode = edge.End as Roy_T.AStar.Graphs.Node;
+                if (endNode == null || !graph3D.NodeInfo.ContainsKey(endNode))
+                    continue;
+
+                var nodeInfo = graph3D.NodeInfo[endNode];
+                var worldPos = gridMap.GridToWorld2D(new GridPoint2D(nodeInfo.X, nodeInfo.Y));
+                path3D.Add(new Point3D(worldPos.X, worldPos.Y, nodeInfo.Z));
+
+                LogManager.Info($"[路径点{pointIndex}] 网格({nodeInfo.X},{nodeInfo.Y}), 层{nodeInfo.LayerIndex}, Z={nodeInfo.Z:F2}, 类型={nodeInfo.CellType}");
+                pointIndex++;
+            }
+
+            path3D.Add(originalEnd);
+            LogManager.Info($"[路径点{pointIndex}] 原始终点: ({originalEnd.X:F2}, {originalEnd.Y:F2}, Z={originalEnd.Z:F2})");
+            LogManager.Info($"[路径转换完成] 总共 {path3D.Count} 个路径点");
+
+            return path3D;
+        }
+
+
+        /// <summary>
+        /// 2.5D路径规划（使用通道覆盖数据、高度约束和路径策略）[已废弃，保留作为备份]
+        /// </summary>
+        private PathFindingResult FindPath_Legacy_2_5D(Point3D start, Point3D end, GridMap gridMap, ChannelCoverage channelCoverage, double vehicleHeight, PathStrategy strategy = PathStrategy.Shortest)
         {
             try
             {
@@ -944,21 +1387,13 @@ namespace NavisworksTransport.PathPlanning
                         var distance = Math.Sqrt(Math.Pow(endPoint.X - startPoint.X, 2) +
                                                 Math.Pow(endPoint.Y - startPoint.Y, 2));
 
-                        // 🔥 新增：在尝试连接远点之前，先检查是否会跳过高度变化点
-                        bool canSkip = true;
-                        const double heightTolerance = 1e-6; // 与PathOptimizer保持一致
+                        // 🔥 关键修复：检查起点和终点的高度差，如果超过阈值就不能跳过
+                        // 这样可以防止从地面直接跳到楼梯上层
+                        double totalHeightDiff = Math.Abs(endPoint.Z - startPoint.Z);
+                        const double maxAllowedHeightDiffMeters = 0.3; // 0.3米，允许小台阶但不允许跨越整个楼梯
+                        double maxAllowedHeightDiffInModelUnits = maxAllowedHeightDiffMeters * UnitsConverter.GetMetersToUnitsConversionFactor();
 
-                        for (int checkIdx = currentIndex + 1; checkIdx < testIndex; checkIdx++)
-                        {
-                            // 检查被跳过的点是否有高度变化
-                            if (Math.Abs(optimizedPath[checkIdx].Z - optimizedPath[checkIdx-1].Z) > heightTolerance ||
-                                (checkIdx + 1 < optimizedPath.Count &&
-                                 Math.Abs(optimizedPath[checkIdx+1].Z - optimizedPath[checkIdx].Z) > heightTolerance))
-                            {
-                                canSkip = false;
-                                break;
-                            }
-                        }
+                        bool canSkip = totalHeightDiff <= maxAllowedHeightDiffInModelUnits;
 
                         if (canSkip && IsDirectPathClear(startPoint, endPoint, gridMap))
                         {
@@ -997,6 +1432,14 @@ namespace NavisworksTransport.PathPlanning
                 int reducedPoints = optimizedPath.Count - diagonalOptimizedPath.Count;
                 LogManager.Info($"[斜线优化] 斜线优化完成，点数变化: {optimizedPath.Count} -> {diagonalOptimizedPath.Count} (减少{reducedPoints}个点)");
 
+                // 打印斜线优化后的路径点详情
+                LogManager.Info($"[斜线优化后路径详情] 共 {diagonalOptimizedPath.Count} 个点：");
+                for (int i = 0; i < diagonalOptimizedPath.Count; i++)
+                {
+                    var pt = diagonalOptimizedPath[i];
+                    LogManager.Info($"  [斜线优化后点{i}] 位置=({pt.X:F2}, {pt.Y:F2}, Z={pt.Z:F2})");
+                }
+
                 return diagonalOptimizedPath;
             }
             catch (Exception ex)
@@ -1851,6 +2294,7 @@ namespace NavisworksTransport.PathPlanning
                     tempRoute.Points.Add(pathPoint);
                 }
 
+                // 使用基于网格的路径优化算法
                 // 使用基于网格的路径优化算法
                 if (gridMap != null)
                 {
@@ -1874,6 +2318,8 @@ namespace NavisworksTransport.PathPlanning
                         pathResult.PathPoints = optimizedPoints;
 
                         // 🔥 步骤2：在现有优化基础上应用专门的斜线优化
+                        // 2D场景：高度差为0，不影响原有逻辑
+                        // 3D场景：通过高度差检查保护楼梯路径
                         var diagonalOptimizedPoints = ApplyDiagonalOptimization(optimizedPoints, gridMap);
 
                         if (diagonalOptimizedPoints.Count != optimizedPoints.Count)
@@ -1892,6 +2338,14 @@ namespace NavisworksTransport.PathPlanning
                     LogManager.Info($"[路径生成] 未提供网格地图，跳过路径优化，点数: {pathResult.PathPoints.Count}");
                 }
 
+                // 打印最终返回给用户的路径点详情
+                LogManager.Info($"[最终路径详情] 共 {pathResult.PathPoints.Count} 个点：");
+                for (int i = 0; i < pathResult.PathPoints.Count; i++)
+                {
+                    var pt = pathResult.PathPoints[i];
+                    LogManager.Info($"  [最终点{i}] 位置=({pt.X:F2}, {pt.Y:F2}, Z={pt.Z:F2})");
+                }
+
                 return pathResult;
             }
             catch (Exception ex)
diff --git a/src/PathPlanning/ChannelBasedGridBuilder.cs b/src/PathPlanning/ChannelBasedGridBuilder.cs
index d5a0420..baaac6f 100644
--- a/src/PathPlanning/ChannelBasedGridBuilder.cs
+++ b/src/PathPlanning/ChannelBasedGridBuilder.cs
@@ -60,17 +60,31 @@ namespace NavisworksTransport.PathPlanning
         {
             LogManager.Info("[通道网格构建器] 开始构建通道覆盖网格");
 
-            // 1. 获取所有通道物品
-            var channelItems = CategoryAttributeManager.GetLogisticsItemsByType(
+            // 1. 获取所有通行相关的物品（通道 + 楼梯 + 电梯）
+            var channelItems = new List<ModelItem>();
+
+            // 获取通道
+            var channels = CategoryAttributeManager.GetLogisticsItemsByType(
                 CategoryAttributeManager.LogisticsElementType.通道);
-            
+            channelItems.AddRange(channels);
+
+            // 获取楼梯
+            var stairs = CategoryAttributeManager.GetLogisticsItemsByType(
+                CategoryAttributeManager.LogisticsElementType.楼梯);
+            channelItems.AddRange(stairs);
+
+            // 获取电梯
+            var elevators = CategoryAttributeManager.GetLogisticsItemsByType(
+                CategoryAttributeManager.LogisticsElementType.电梯);
+            channelItems.AddRange(elevators);
+
             if (!channelItems.Any())
             {
-                LogManager.Warning("[通道网格构建器] 未找到任何通道物品");
+                LogManager.Warning("[通道网格构建器] 未找到任何通行物品（通道/楼梯/电梯）");
                 return CreateEmptyChannelCoverage();
             }
 
-            LogManager.Info($"[通道网格构建器] 找到 {channelItems.Count} 个通道物品");
+            LogManager.Info($"[通道网格构建器] 找到通行物品: 通道{channels.Count}个, 楼梯{stairs.Count}个, 电梯{elevators.Count}个, 总计{channelItems.Count}个");
 
             // 2. 计算通道总边界
             var totalBounds = BoundingBoxGeometryUtils.CalculateTotalBounds(channelItems);
diff --git a/src/PathPlanning/PathOptimizer.cs b/src/PathPlanning/PathOptimizer.cs
index 645dc0f..0257364 100644
--- a/src/PathPlanning/PathOptimizer.cs
+++ b/src/PathPlanning/PathOptimizer.cs
@@ -151,13 +151,19 @@ namespace NavisworksTransport.PathPlanning
             var worldPath = points.Select(p => p.Position).ToList();
             var gridPath = worldPath.Select(p => gridMap.WorldToGrid(p)).ToList();
 
-            // 步骤1：先去除重复点，但保护限速边界
+            // 步骤1：先去除重复点，但保护限速边界和多层高度
             var dedupedGridPath = new List<GridPoint2D>();
             var dedupedPoints = new List<PathPoint>();
             for (int i = 0; i < gridPath.Count; i++)
             {
-                // 🔥 关键修复：即使位置相同，如果限速不同也不能去重
-                bool shouldKeep = i == 0 || !gridPath[i].Equals(gridPath[i-1]) || HasSpeedLimitChange(points[i-1], points[i]);
+                // 🔥 关键修复：即使网格坐标相同，如果限速不同或高度不同也不能去重
+                // 2D场景：hasHeightChange为false，不影响原有逻辑
+                // 3D场景：保留同网格不同高度的点
+                bool hasGridChange = i == 0 || !gridPath[i].Equals(gridPath[i-1]);
+                bool hasSpeedChange = i > 0 && HasSpeedLimitChange(points[i-1], points[i]);
+                bool hasHeightChange = i > 0 && Math.Abs(points[i].Position.Z - points[i-1].Position.Z) > 1e-6;
+
+                bool shouldKeep = hasGridChange || hasSpeedChange || hasHeightChange;
 
                 if (shouldKeep)
                 {
@@ -165,9 +171,12 @@ namespace NavisworksTransport.PathPlanning
                     dedupedPoints.Add(points[i]);
                 }
             }
+            LogManager.Info($"[路径优化] 去重完成：{gridPath.Count} -> {dedupedPoints.Count} 个点");
 
-            // 步骤2：基于去重后的网格路径进行方向优化
-            var simplified = new List<GridPoint2D> { dedupedGridPath[0] };
+            // 步骤2：基于去重后的路径进行方向优化
+            // 🔥 关键修复：不使用网格坐标，直接使用索引来跟踪保留的点
+            // 这样可以避免同一网格坐标的不同高度点被混淆
+            var simplifiedIndices = new List<int> { 0 }; // 保留起点索引
 
             // 获取初始方向（归一化）
             int initialDx = dedupedGridPath[1].X - dedupedGridPath[0].X;
@@ -177,7 +186,8 @@ namespace NavisworksTransport.PathPlanning
                 initialDy == 0 ? 0 : Math.Sign(initialDy)
             );
 
-            // 简化路径：只保留转弯点、高度变化点和限速变化点（使用归一化方向）
+            // 简化路径：只保留转弯点、高度变化点和限速变化点
+            bool forceKeepNext = false;
             for (int i = 2; i < dedupedGridPath.Count; i++)
             {
                 // 计算当前线段的位移
@@ -190,14 +200,14 @@ namespace NavisworksTransport.PathPlanning
                     dy == 0 ? 0 : Math.Sign(dy)
                 );
 
-                // 检查高度变化
+                // 检查高度变化（2D场景下始终为false，不影响原有逻辑）
                 bool hasHeightChange = HasHeightChange(
                     dedupedPoints[i-2],
                     dedupedPoints[i-1],
                     dedupedPoints[i]
                 );
 
-                // 🔥 关键修复：检查限速变化
+                // 检查限速变化
                 bool hasSpeedLimitChange = HasSpeedLimitChange(
                     dedupedPoints[i-2],
                     dedupedPoints[i-1],
@@ -205,37 +215,36 @@ namespace NavisworksTransport.PathPlanning
                 );
 
                 // 保留转折点、高度变化点或限速变化点
-                if (!currentDirection.Equals(prevDirection) || hasHeightChange || hasSpeedLimitChange)
-                {
-                    simplified.Add(dedupedGridPath[i - 1]);
-                    prevDirection = currentDirection;
+                bool shouldKeep = !currentDirection.Equals(prevDirection) || hasHeightChange || hasSpeedLimitChange || forceKeepNext;
 
-                    if (hasHeightChange)
-                    {
-                        LogManager.Debug($"[网格路径优化] 保留高度变化点 {i-1}");
-                    }
-                    if (hasSpeedLimitChange)
-                    {
-                        LogManager.Debug($"[网格路径优化] 保留限速变化点 {i-1}");
-                    }
+                if (shouldKeep)
+                {
+                    simplifiedIndices.Add(i - 1); // 保留索引而不是网格坐标
+                    prevDirection = currentDirection;
                 }
+
+                // 🔥 如果当前检测到高度变化，下一个点也必须保留
+                forceKeepNext = hasHeightChange;
             }
 
-            // 添加终点
-            simplified.Add(dedupedGridPath[dedupedGridPath.Count - 1]);
+            // 添加终点索引
+            simplifiedIndices.Add(dedupedGridPath.Count - 1);
 
-            LogManager.Info($"[网格路径优化] 最终优化完成：{gridPath.Count} -> {dedupedGridPath.Count}(去重) -> {simplified.Count}(简化) 个点");
+            LogManager.Info($"[网格路径优化] 简化完成：{dedupedGridPath.Count} -> {simplifiedIndices.Count} 个点");
 
-            // 步骤3：转换回PathPoint格式
+            // 步骤3：根据索引提取PathPoint
             var simplifiedPoints = new List<PathPoint>();
-            foreach (var gridPoint in simplified)
+            foreach (int index in simplifiedIndices)
             {
-                // 在去重后的路径中找到对应的点
-                int originalIndex = dedupedGridPath.FindIndex(g => g.Equals(gridPoint));
-                if (originalIndex >= 0)
-                {
-                    simplifiedPoints.Add(dedupedPoints[originalIndex]);
-                }
+                simplifiedPoints.Add(dedupedPoints[index]);
+            }
+
+            // 打印优化后的路径点详情
+            LogManager.Info($"[优化后路径详情] 共 {simplifiedPoints.Count} 个点：");
+            for (int i = 0; i < simplifiedPoints.Count; i++)
+            {
+                var pt = simplifiedPoints[i];
+                LogManager.Info($"  [优化后点{i}] 位置=({pt.Position.X:F2}, {pt.Position.Y:F2}, Z={pt.Position.Z:F2}), 限速={pt.SpeedLimit:F1}");
             }
 
             return simplifiedPoints;