diff --git a/src/PathPlanning/AutoPathFinder.cs b/src/PathPlanning/AutoPathFinder.cs
index 33d4a96..1768a80 100644
--- a/src/PathPlanning/AutoPathFinder.cs
+++ b/src/PathPlanning/AutoPathFinder.cs
@@ -100,11 +100,16 @@ namespace NavisworksTransport.PathPlanning
 
                 LogManager.Info($"[2.5D路径规划] A*算法找到原始路径，包含 {pathResult.PathPoints.Count} 个点");
 
-                // 5. 根据网格点通行高度区间对路径点Z坐标调整
-                var enhancedPath = ApplyGridHeightConstraints(pathResult.PathPoints, gridMap, vehicleHeight);
+                // 5. 预先生成路径点和网格坐标的对应关系，避免重复转换
+                var pathWithGridCoords = pathResult.PathPoints
+                    .Select(point => (point, gridMap.WorldToGrid(point)))
+                    .ToList();
 
-                // 6. 验证路径中所有点的高度约束（特别是门网格）
-                ValidatePathHeightConstraints(enhancedPath, gridMap, vehicleHeight);
+                // 6. 根据网格点通行高度区间对路径点Z坐标调整
+                var enhancedPath = ApplyGridHeightConstraints(pathWithGridCoords, gridMap, vehicleHeight);
+
+                // 7. 验证路径中所有点的高度约束（直接使用原始网格坐标，因为网格位置未变）
+                ValidatePathHeightConstraints(pathWithGridCoords, gridMap, vehicleHeight);
 
                 // 更新PathFindingResult
                 pathResult.PathPoints = enhancedPath;
@@ -1234,21 +1239,19 @@ namespace NavisworksTransport.PathPlanning
         /// <summary>
         /// 根据网格高度区间调整路径点Z坐标
         /// </summary>
-        /// <param name="path">原始路径</param>
+        /// <param name="pathWithGridCoords">路径点和对应的网格坐标</param>
         /// <param name="gridMap">网格地图</param>
         /// <param name="vehicleHeight">车辆高度（模型单位）</param>
         /// <returns>调整后的路径</returns>
-        private List<Point3D> ApplyGridHeightConstraints(List<Point3D> path, GridMap gridMap, double vehicleHeight)
+        private List<Point3D> ApplyGridHeightConstraints(List<(Point3D point, GridPoint2D gridPos)> pathWithGridCoords, GridMap gridMap, double vehicleHeight)
         {
-            LogManager.Info($"[应用高度区间] 开始调整，路径点数: {path.Count}");
+            LogManager.Info($"[应用高度区间] 开始调整，路径点数: {pathWithGridCoords.Count}");
 
             var adjustedPath = new List<Point3D>();
             int adjustedCount = 0;
 
-            foreach (var point in path)
+            foreach (var (point, gridPos) in pathWithGridCoords)
             {
-                var gridPos = gridMap.WorldToGrid(point);
-
                 // 检查网格位置有效性
                 if (!gridMap.IsValidGridPosition(gridPos))
                 {
@@ -1273,7 +1276,7 @@ namespace NavisworksTransport.PathPlanning
                 adjustedCount++;
             }
 
-            LogManager.Info($"[应用高度区间] 完成，调整了 {adjustedCount}/{path.Count} 个点");
+            LogManager.Info($"[应用高度区间] 完成，调整了 {adjustedCount}/{pathWithGridCoords.Count} 个点");
             return adjustedPath;
         }
 
@@ -1285,14 +1288,10 @@ namespace NavisworksTransport.PathPlanning
         /// <returns>网格坐标</returns>
         private GridPoint2D ConvertAStarPositionToGrid(Position astarPosition, GridMap gridMap)
         {
-            LogManager.Info($"[A*原始坐标] A*返回: ({astarPosition.X:F3}, {astarPosition.Y:F3})");
-
             double cellSizeInMeters = UnitsConverter.ConvertToMeters(gridMap.CellSize);
             int gridX = (int)Math.Round(astarPosition.X / cellSizeInMeters);
             int gridY = (int)Math.Round(astarPosition.Y / cellSizeInMeters);
 
-            LogManager.Info($"[A*转换坐标] 转换结果: ({gridX}, {gridY})");
-
             return new GridPoint2D(gridX, gridY);
         }
 
@@ -1397,22 +1396,21 @@ namespace NavisworksTransport.PathPlanning
         /// <summary>
         /// 验证路径中所有点的高度约束（特别检查门网格）
         /// </summary>
-        /// <param name="path">路径点列表</param>
+        /// <param name="pathWithGridCoords">路径点和对应的网格坐标</param>
         /// <param name="gridMap">网格地图</param>
         /// <param name="vehicleHeight">车辆高度（模型单位）</param>
-        private void ValidatePathHeightConstraints(List<Point3D> path, GridMap gridMap, double vehicleHeight)
+        private void ValidatePathHeightConstraints(List<(Point3D point, GridPoint2D gridPos)> pathWithGridCoords, GridMap gridMap, double vehicleHeight)
         {
-            LogManager.Info($"[路径高度验证] 开始验证 {path.Count} 个路径点的高度约束");
+            LogManager.Info($"[路径高度验证] 开始验证 {pathWithGridCoords.Count} 个路径点的高度约束");
 
             int checkedPoints = 0;
             int passedPoints = 0;
             int failedPoints = 0;
             int doorPoints = 0;
 
-            for (int i = 0; i < path.Count; i++)
+            for (int i = 0; i < pathWithGridCoords.Count; i++)
             {
-                var point = path[i];
-                var gridPos = gridMap.WorldToGrid(point);
+                var (point, gridPos) = pathWithGridCoords[i];
 
                 if (gridMap.IsValidGridPosition(gridPos))
                 {