diff --git a/src/PathPlanning/AutoPathFinder.cs b/src/PathPlanning/AutoPathFinder.cs
index cc2cb92..33d4a96 100644
--- a/src/PathPlanning/AutoPathFinder.cs
+++ b/src/PathPlanning/AutoPathFinder.cs
@@ -68,12 +68,14 @@ namespace NavisworksTransport.PathPlanning
                 var originalEnd = end;
 
                 // 1. 检查起点和终点的高度约束
-                if (!IsPointPassableAtHeight(start, gridMap, vehicleHeight))
+                var startGridPos = gridMap.WorldToGrid(start);
+                if (!IsPointPassableAtHeight(startGridPos, start, gridMap, vehicleHeight))
                 {
                     LogManager.Warning("[2.5D路径规划] 起点位置不满足高度约束");
                 }
 
-                if (!IsPointPassableAtHeight(end, gridMap, vehicleHeight))
+                var endGridPos = gridMap.WorldToGrid(end);
+                if (!IsPointPassableAtHeight(endGridPos, end, gridMap, vehicleHeight))
                 {
                     LogManager.Warning("[2.5D路径规划] 终点位置不满足高度约束");
                 }
@@ -1175,23 +1177,16 @@ namespace NavisworksTransport.PathPlanning
         /// <summary>
         /// 检查指定点在给定高度约束下是否可通行
         /// </summary>
-        /// <param name="point">检查点</param>
+        /// <param name="gridPos">网格坐标</param>
+        /// <param name="point">检查点（世界坐标）</param>
         /// <param name="gridMap">网格地图</param>
         /// <param name="vehicleHeight">车辆高度（模型单位）</param>
         /// <returns>是否可通行</returns>
-        /// <summary>
-        /// 检查指定点在给定高度约束下是否可通行
-        /// </summary>
-        /// <param name="point">检查点</param>
-        /// <param name="gridMap">网格地图</param>
-        /// <param name="vehicleHeight">车辆高度（模型单位）</param>
-        /// <returns>是否可通行</returns>
-        private bool IsPointPassableAtHeight(Point3D point, GridMap gridMap, double vehicleHeight)
+        private bool IsPointPassableAtHeight(GridPoint2D gridPos, Point3D point, GridMap gridMap, double vehicleHeight)
         {
             try
             {
-                // 将世界坐标转换为网格坐标
-                var gridPos = gridMap.WorldToGrid(point);
+                // 使用传入的网格坐标，避免重复转换
                 int gridX = gridPos.X;
                 int gridY = gridPos.Y;
 
@@ -1432,7 +1427,7 @@ namespace NavisworksTransport.PathPlanning
                     }
 
                     // 进行高度约束检查
-                    bool isPassable = IsPointPassableAtHeight(point, gridMap, vehicleHeight);
+                    bool isPassable = IsPointPassableAtHeight(gridPos, point, gridMap, vehicleHeight);
                     if (isPassable)
                     {
                         passedPoints++;
diff --git a/src/PathPlanning/GridMap.cs b/src/PathPlanning/GridMap.cs
index 52ea4a5..5ec512b 100644
--- a/src/PathPlanning/GridMap.cs
+++ b/src/PathPlanning/GridMap.cs
@@ -183,7 +183,7 @@ namespace NavisworksTransport.PathPlanning
             double gridX = (worldPosition.X - Origin.X) / CellSize;
             double gridY = (worldPosition.Y - Origin.Y) / CellSize;
 
-            return new GridPoint2D((int)Math.Floor(gridX), (int)Math.Floor(gridY));
+            return new GridPoint2D((int)Math.Round(gridX), (int)Math.Round(gridY));
         }
 
         /// <summary>