Refactor auto path planning coordinate semantics

2026-04-13 10:31:14 +08:00 · 2026-04-13 10:31:14 +08:00 · fdb857af17
commit fdb857af17
parent 6f037a4571
10 changed files with 797 additions and 107 deletions
--- a/NavisworksTransport.UnitTests.csproj
+++ b/NavisworksTransport.UnitTests.csproj
@ -60,6 +60,7 @@
    <Compile Include="UnitTests\Core\PathPlanningManagerHoistingCompletionTests.cs" />
    <Compile Include="UnitTests\Core\PathPersistenceTests.cs" />
    <Compile Include="UnitTests\Core\PathRouteCloneTests.cs" />
+    <Compile Include="UnitTests\CoordinateSystem\AutoPathPlanningCoordinateSemanticsTests.cs" />
    <Compile Include="UnitTests\Integration\NavisworksTestAutomationClient.cs" />
    <Compile Include="UnitTests\Integration\VirtualCollisionAutomationTests.cs" />
    <Compile Include="UnitTests\CoordinateSystem\HostCoordinateAdapterTests.cs" />
--- a/TransportPlugin.csproj
+++ b/TransportPlugin.csproj
@ -359,6 +359,7 @@
    <Compile Include="src\Utils\CoordinateSystem\ObjectSpaceOrientationHelper.cs" />
    <Compile Include="src\Utils\CoordinateSystem\RotatedObjectExtentHelper.cs" />
    <Compile Include="src\Utils\CoordinateSystem\HostCoordinateAdapter.cs" />
+    <Compile Include="src\Utils\CoordinateSystem\HostPlanarGridHelper.cs" />
    <Compile Include="src\Utils\CoordinateSystem\HoistingCoordinateHelper.cs" />
    <Compile Include="src\Utils\CoordinateSystem\LocalAxisDirection.cs" />
    <Compile Include="src\Utils\CoordinateSystem\ModelAxisConvention.cs" />
--- a/UnitTests/CoordinateSystem/AutoPathPlanningCoordinateSemanticsTests.cs
+++ b/UnitTests/CoordinateSystem/AutoPathPlanningCoordinateSemanticsTests.cs
@ -0,0 +1,114 @@
+using Microsoft.VisualStudio.TestTools.UnitTesting;
+using NavisworksTransport.Utils.CoordinateSystem;
+using System.Numerics;
+
+namespace NavisworksTransport.UnitTests.CoordinateSystem
+{
+    [TestClass]
+    public class AutoPathPlanningCoordinateSemanticsTests
+    {
+        [TestMethod]
+        public void YUp_HostPlanarGridHelper_CreateHostPoint_ShouldApplyElevationOnHostY()
+        {
+            var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
+
+            Vector3 point = HostPlanarGridHelper.CreateHostPoint3(2.0, 3.0, 14.83, adapter);
+
+            AssertPoint(point, 2.0, 14.83, 3.0);
+        }
+
+        [TestMethod]
+        public void ZUp_HostPlanarGridHelper_CreateHostPoint_ShouldApplyElevationOnHostZ()
+        {
+            var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
+
+            Vector3 point = HostPlanarGridHelper.CreateHostPoint3(2.0, 3.0, 6.25, adapter);
+
+            AssertPoint(point, 2.0, 3.0, 6.25);
+        }
+
+        [TestMethod]
+        public void YUp_HostPlanarGridHelper_GetAndSetElevation_ShouldUseHostY()
+        {
+            var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
+            var hostPoint = new Vector3(4.0f, 15.0f, 6.0f);
+
+            double elevation = HostPlanarGridHelper.GetElevation3(hostPoint, adapter);
+            Vector3 adjusted = HostPlanarGridHelper.SetElevation3(hostPoint, 20.0, adapter);
+
+            Assert.AreEqual(15.0, elevation, 1e-6);
+            AssertPoint(adjusted, 4.0, 20.0, 6.0);
+        }
+
+        [TestMethod]
+        public void YUp_HostPlanarGridHelper_GetHorizontalCoords_ShouldProjectToHostXZPlane()
+        {
+            var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
+            var hostPoint = new Vector3(8.0f, 14.83f, -5.35f);
+
+            (double h1, double h2) = HostPlanarGridHelper.GetHorizontalCoords3(hostPoint, adapter);
+
+            Assert.AreEqual(8.0, h1, 1e-6);
+            Assert.AreEqual(-5.35, h2, 1e-6);
+        }
+
+        [TestMethod]
+        public void YUp_HostPlanarGridHelper_HorizontalRange_ShouldMatchHostXZPlane()
+        {
+            var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
+            var hostMin = new Vector3(-10.0f, 2.0f, -30.0f);
+            var hostMax = new Vector3(40.0f, 12.0f, 5.0f);
+
+            (double min1, double max1, double min2, double max2) =
+                HostPlanarGridHelper.GetHorizontalRange3(hostMin, hostMax, adapter);
+
+            Assert.AreEqual(-10.0, min1, 1e-6);
+            Assert.AreEqual(40.0, max1, 1e-6);
+            Assert.AreEqual(-30.0, min2, 1e-6);
+            Assert.AreEqual(5.0, max2, 1e-6);
+        }
+
+        [TestMethod]
+        public void YUp_GridLikeWorldPointConstruction_ShouldWriteElevationToHostY()
+        {
+            var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
+            double originH1 = -210.0;
+            double originH2 = -10.0;
+            double cellSize = 1.0;
+
+            Vector3 worldPoint = HostPlanarGridHelper.CreateHostPoint3(
+                originH1 + 4 * cellSize,
+                originH2 + 6 * cellSize,
+                14.83,
+                adapter);
+
+            Assert.AreEqual(-206.0, worldPoint.X, 1e-6);
+            Assert.AreEqual(14.83, worldPoint.Y, 1e-6);
+            Assert.AreEqual(-4.0, worldPoint.Z, 1e-6);
+        }
+
+        [TestMethod]
+        public void YUp_GridLikeWorldToGrid_ShouldReadHostXZAsPlanarAxes()
+        {
+            var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
+            var origin = new Vector3(-210.0f, 14.83f, -10.0f);
+            var endPoint = new Vector3(-169.20f, 14.83f, -5.35f);
+            const double cellSize = 1.0;
+
+            (double pointH1, double pointH2) = HostPlanarGridHelper.GetHorizontalCoords3(endPoint, adapter);
+            (double originH1, double originH2) = HostPlanarGridHelper.GetHorizontalCoords3(origin, adapter);
+            int gridX = (int)System.Math.Round((pointH1 - originH1) / cellSize);
+            int gridY = (int)System.Math.Round((pointH2 - originH2) / cellSize);
+
+            Assert.AreEqual(41, gridX);
+            Assert.AreEqual(5, gridY);
+        }
+
+        private static void AssertPoint(Vector3 actual, double x, double y, double z)
+        {
+            Assert.AreEqual(x, actual.X, 1e-6);
+            Assert.AreEqual(y, actual.Y, 1e-6);
+            Assert.AreEqual(z, actual.Z, 1e-6);
+        }
+    }
+}
--- a/doc/working/2026-04-13-auto-path-planning-coordinate-refactor-plan.md
+++ b/doc/working/2026-04-13-auto-path-planning-coordinate-refactor-plan.md
@ -0,0 +1,338 @@
+# 自动路径规划坐标系重构实施方案
+
+更新时间：2026-04-13  
+状态：实施中  
+目标：将 `Ground` 自动路径规划整条链路并入当前稳定的“宿主坐标系 -> Canonical Space -> 宿主坐标系”架构，消除旧 `XY` 平面 / `Z` 高度硬编码，不再针对单一 `YUp` 现象打补丁。
+
+---
+
+## 1. 背景与问题定义
+
+当前项目的坐标系架构已经明确：
+
+- 宿主坐标系（Host Space）
+  - Navisworks 文档坐标系
+  - UI 输入输出、鼠标拾取、日志、渲染交互都属于这一层
+- 内部坐标系（Canonical Space）
+  - 项目内部统一固定为 `ZUp`
+  - 几何、姿态、法向、路径计算优先在这一层完成
+- 业务语义层
+  - 在 canonical 之上表达地面接触点、吊点、轨道法向、动画跟踪点等业务含义
+
+但自动路径规划链路目前仍大量保留旧实现习惯：
+
+- 默认 `XY` 是水平平面
+- 默认 `Z` 是高度轴
+- 默认世界 `Z` 可以直接代表“向上”
+- 路径起终点修正仍然是“保留 `XY`，替换 `Z`”
+
+这说明当前问题不是“某个 `YUp` 细节没处理”，而是：
+
+- 自动路径规划整条链没有接入当前稳定的坐标系架构
+
+因此本方案的目标不是修一个 `YUp` bug，而是把自动路径规划整体收编到现有坐标系体系中。
+
+---
+
+## 2. 现象与直接证据
+
+以 `YUp` 文档中的地面自动路径为例，日志显示：
+
+- 起点：`(-209.94, 14.83, -1.68)`
+- 终点：`(-169.20, 14.83, -5.35)`
+
+对当前文档来说，宿主 `up` 是 `Y`，所以：
+
+- 地面高度应主要体现在 `Y`
+- `Z` 应属于宿主水平面的一部分
+
+但自动路径规划链中仍有如下旧假设：
+
+1. `PathPlanningManager` 按 `X/Y` 计算边界与网格尺寸
+- `src/Core/PathPlanningManager.cs`
+- `GetModelBounds()`
+- `CalculateOptimalGridSize(...)`
+
+2. `AutoPathFinder` 把图节点还原为：
+- `new Point3D(worldPos.X, worldPos.Y, nodeInfo.Z)`
+
+3. `AutoPathFinder` 的起终点修正逻辑仍是：
+- 保留原始 `X/Y`
+- 只替换 `Z`
+
+这套实现天然绑定旧 `ZUp` 语义，无法正确支持当前多坐标系架构。
+
+---
+
+## 3. 重构目标
+
+本次重构要达成的不是“YUp 也能跑”，而是以下更高层目标：
+
+1. 自动路径规划输入输出继续保持宿主坐标语义
+- 用户点击点、UI 日志、路径显示、数据库路径点仍然按宿主坐标解释
+
+2. 自动路径规划内部计算统一切到 canonical
+- 网格范围
+- 栅格映射
+- A* 路径求解
+- 高度层处理
+- 曲线化前的路径点
+
+3. Ground 自动路径与通行空间、路径渲染、动画主链共享同一套 up / 高度语义
+
+4. 禁止继续保留旧 `XY/Z` 写死逻辑作为 fallback
+
+---
+
+## 4. 范围界定
+
+### 4.1 本次重构范围
+
+- `PathEditingViewModel`
+  - 自动路径起点/终点输入边界
+- `PathPlanningManager`
+  - 自动路径入口、边界、网格参数、路径落库前收口
+- `GridMapGenerator`
+  - BIM 到网格的平面/高度语义
+- `AutoPathFinder`
+  - 路径搜索、节点转世界坐标、首尾点修正
+- 与自动路径规划直接耦合的测试
+
+### 4.2 暂不纳入本次范围
+
+- 手动路径编辑主链
+- 吊装路径自动生成链
+- Rail 自动生成链
+- 动画姿态链
+- 碰撞恢复链
+
+说明：
+
+- 本次先聚焦 `Ground` 自动路径规划
+- 但设计必须是通用的宿主/内部坐标边界，不允许做成“只为 `YUp` 单独补丁”
+
+---
+
+## 5. 关键设计原则
+
+### 5.1 边界清晰
+
+- 宿主坐标只在输入输出边界存在
+- 内部计算统一使用 canonical
+- 不允许在算法中间层直接假设世界 `Z` 是高度
+
+### 5.2 不新增 fallback
+
+不允许：
+
+- canonical 转换失败时回退旧 `XY/Z` 逻辑
+- `YUp` 失败时偷偷走旧分支
+- 用“如果是 YUp 就特判”掩盖整体架构问题
+
+### 5.3 优先复用现有坐标工具
+
+优先复用：
+
+- `HostCoordinateAdapter`
+- `CanonicalTrackedPositionResolver`
+- 坐标系统一设计文档中已经稳定的术语和边界
+
+### 5.4 先锁测试，再动实现
+
+本次重构必须先补针对多坐标系的自动路径测试，再改实现。
+
+---
+
+## 6. 目标架构
+
+目标链路应收敛为：
+
+```text
+宿主点击点 / 宿主通道边界 / 宿主模型几何
+    ↓ HostCoordinateAdapter
+Canonical 起点 / 终点 / 边界 / 网格 / 高度层
+    ↓ 自动路径规划算法（统一在 canonical 内）
+Canonical 路径点
+    ↓ HostCoordinateAdapter
+宿主路径点 / 渲染 / UI / 数据库存储
+```
+
+关键语义：
+
+- canonical 内部固定：
+  - `XY` 为水平平面
+  - `Z` 为高度
+- 宿主坐标系的 `YUp/ZUp` 差异只由 `HostCoordinateAdapter` 负责吸收
+
+---
+
+## 7. 分阶段实施计划
+
+### Phase 0：现状锁定与测试补齐
+
+目标：
+
+- 先把当前正确/错误行为锁定，避免重构中语义漂移
+
+任务：
+
+- [ ] 新增 `Ground` 自动路径在 `ZUp` 文档下的首尾点测试
+- [ ] 新增 `Ground` 自动路径在 `YUp` 文档下的首尾点测试
+- [ ] 新增“宿主点 -> 自动路径 -> 路径点输出”边界测试
+- [ ] 新增“路径点 / 路径线 / 通行空间”首尾地面一致性测试
+
+验收标准：
+
+- 在没有改实现前，测试至少能明确暴露 `YUp` 下当前错误
+- `ZUp` 当前稳定行为被锁住，不允许回归
+
+### Phase 1：输入边界 canonical 化
+
+目标：
+
+- 自动路径规划入口不再把宿主点直接扔进旧 `XY/Z` 规划逻辑
+
+任务：
+
+- [ ] 梳理 `PathEditingViewModel` 起点/终点点击数据的宿主语义
+- [ ] 在自动路径规划入口显式引入 `HostCoordinateAdapter`
+- [ ] 将 `AutoPlanPath(...)` 的内部输入切换为 canonical 起点/终点
+- [ ] 明确日志里同时打印宿主输入点与 canonical 输入点
+
+验收标准：
+
+- 自动路径规划入口不再直接依赖宿主 `YUp/ZUp`
+- 起点终点进入算法前，坐标语义清晰可见
+
+### Phase 2：网格与边界 canonical 化
+
+目标：
+
+- 网格边界、障碍物、通道数据全部按 canonical 平面/高度工作
+
+任务：
+
+- [ ] 重构 `GetModelBounds()` / `CalculateChannelsBounds(...)`
+- [ ] 重构 `CalculateOptimalGridSize(...)`，不再把 `X/Y` 固定当平面
+- [ ] 重构 `GridMapGenerator.GenerateFromBIM(...)` 的输入语义
+- [ ] 明确网格二维平面坐标与 canonical 三维坐标的边界
+
+验收标准：
+
+- 网格生成不再依赖宿主世界 `XY`
+- `YUp` / `ZUp` 两类文档进入网格阶段后语义完全一致
+
+### Phase 3：路径求解与输出 canonical 化
+
+目标：
+
+- `AutoPathFinder` 内部只处理 canonical
+- 路径输出统一经适配器返回宿主
+
+任务：
+
+- [ ] 重构图节点到三维点的转换逻辑
+- [ ] 删除或重写“保留 XY、替换 Z”的旧首尾点修正
+- [ ] 重构路径输出与落库前的宿主还原逻辑
+- [ ] 日志里同时打印 canonical 路径首尾点与宿主路径首尾点
+
+验收标准：
+
+- 路径首尾点在 `YUp/ZUp` 下都能正确贴回宿主地面
+- 不再出现“终点被压到地面下方”的旧 `ZUp` 语义残留
+
+### Phase 4：联动校验与收尾
+
+目标：
+
+- 自动路径规划与可视化、通行空间、动画链的基础语义重新对齐
+
+任务：
+
+- [ ] 检查路径点显示是否仍使用宿主坐标
+- [ ] 检查路径线和通行空间首尾点是否与路径点一致
+- [ ] 清理旧 `XY/Z` 注释、旧日志、旧辅助方法
+- [ ] 更新 `current-engineering-state.md` 中自动路径规划状态说明
+
+验收标准：
+
+- 自动路径规划的起点、终点、路径线、通行空间在多坐标系文档下语义一致
+- 旧 `XY/Z` 硬编码实现不再残留为正式执行链
+
+---
+
+## 8. 关键改动点清单
+
+高风险文件：
+
+- `src/UI/WPF/ViewModels/PathEditingViewModel.cs`
+- `src/Core/PathPlanningManager.cs`
+- `src/PathPlanning/GridMapGenerator.cs`
+- `src/PathPlanning/AutoPathFinder.cs`
+
+重点清理的旧逻辑：
+
+- [ ] “默认 `XY` 为地面平面”的边界计算
+- [ ] “默认 `Z` 为高度”的路径节点组装
+- [ ] “保留 `XY`，替换 `Z`”的起终点修正
+- [ ] 任何直接写死世界 `Z` 为 up 的自动路径规划实现
+
+---
+
+## 9. 测试与验证策略
+
+### 9.1 单元/集成测试
+
+至少补齐：
+
+- [ ] `ZUp Ground` 自动路径首尾点测试
+- [ ] `YUp Ground` 自动路径首尾点测试
+- [ ] 起点/终点保持宿主地面接触语义测试
+- [ ] 路径输出后首尾点与通行空间一致性测试
+
+### 9.2 手工验证
+
+在真实 Navisworks 场景中验证：
+
+- [ ] `ZUp` 模型中自动地面路径仍正常显示
+- [ ] `YUp` 模型中自动地面路径终点不再掉到地面下
+- [ ] 默认显示模式下能看到起点、终点和路径线
+- [ ] 打开通行空间后，首尾位置与路径点一致
+
+---
+
+## 10. 风险与注意事项
+
+1. 自动路径规划链路历史较老，旧实现可能分散在多个辅助方法中
+- 不能只改主入口，必须顺着网格、A*、输出一路清理
+
+2. 数据库存储仍然是宿主坐标语义
+- 不能把 canonical 点直接落库
+
+3. 路径渲染和自动路径规划不能各自维护一套“高度轴”解释
+- 否则会再次出现“路径点对、通行空间错”或反过来的问题
+
+4. 本次重构不是 `Ground` 的最后一步
+- 后续 `Hoisting/Rail` 若要做自动路径，也应直接复用这次收好的边界层
+
+---
+
+## 11. 进度记录
+
+### 2026-04-13
+
+- [x] 确认当前自动路径规划主链仍大量保留旧 `XY/Z` 假设
+- [x] 明确本次方向应为“并入现有坐标系架构”，而不是追加 `YUp` 特判
+- [x] 建立本实施方案文档
+- [x] 引入 `HostPlanarGridHelper`，明确“旧 ICoordinateSystem 水平/高程契约”与 `HostCoordinateAdapter` 的桥接边界
+- [x] `YUpCoordinateSystem` / `ZUpCoordinateSystem` 改为复用统一桥接 helper，而不是各自散落实现
+- [x] 为宿主平面/高程桥接补充纯数学单测，锁定：
+  - `YUp` 高程走宿主 `Y`
+  - `YUp` 水平面走宿主 `XZ`
+  - `ZUp` 仍保持宿主 `XY + Z`
+- [x] `AutoPathFinder` 第一轮收口完成：
+  - 起终点映射改为通过 `gridMap.GetWorldElevation(...)`
+  - 图节点 / 路径点还原改为通过 `gridMap.CreateWorldPoint(...)`
+  - 高度层匹配与路径恢复改为通过 `gridMap.GetWorldElevation(...) / SetWorldElevation(...)`
+- [ ] Phase 0 剩余：补“自动路径首尾点”更贴近业务的回归测试
+- [ ] Phase 2 尚未开始：`GridMapGenerator` 仍存在较多旧 `XY/Z` 硬编码
--- a/src/PathPlanning/AutoPathFinder.cs
+++ b/src/PathPlanning/AutoPathFinder.cs
@ -495,8 +495,11 @@ namespace NavisworksTransport.PathPlanning
            var endGridPos = gridMap.WorldToGrid(end);

            // 找到起点和终点最近的3D节点
-            var startNode = FindClosest3DNode(graph3D, startGridPos, start.Z);
-            var endNode = FindClosest3DNode(graph3D, endGridPos, end.Z);
+            double startElevation = gridMap.GetWorldElevation(start);
+            double endElevation = gridMap.GetWorldElevation(end);
+
+            var startNode = FindClosest3DNode(graph3D, startGridPos, startElevation);
+            var endNode = FindClosest3DNode(graph3D, endGridPos, endElevation);

            // 起点和终点必须检查可达性
            bool startOnObstacle = (startNode == null);
@ -524,8 +527,7 @@ namespace NavisworksTransport.PathPlanning
                foreach (var node in graph3D.AllNodes)
                {
                    var info = graph3D.NodeInfo[node];
-                    var nodeWorldPos = gridMap.GridToWorld3D(new GridPoint2D(info.X, info.Y));
-                    nodeWorldPos = new Point3D(nodeWorldPos.X, nodeWorldPos.Y, info.Z);
+                    var nodeWorldPos = gridMap.CreateWorldPoint(new GridPoint2D(info.X, info.Y), info.Z);

                    double distance = Math.Sqrt(
                        Math.Pow(nodeWorldPos.X - end.X, 2) +
@ -554,8 +556,8 @@ namespace NavisworksTransport.PathPlanning

            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})");
+            LogManager.Info($"[A*执行-3D] 起点映射: ({startGridPos.X},{startGridPos.Y},高程={startElevation:F2}) -> 节点({startInfo.X},{startInfo.Y},层{startInfo.LayerIndex},高程={startInfo.Z:F2})");
+            LogManager.Info($"[A*执行-3D] 终点映射: ({endGridPos.X},{endGridPos.Y},高程={endElevation:F2}) -> 节点({endInfo.X},{endInfo.Y},层{endInfo.LayerIndex},高程={endInfo.Z:F2})");

            // 执行A*
            var pathfinder = new Roy_T.AStar.Paths.PathFinder();
@ -699,7 +701,7 @@ namespace NavisworksTransport.PathPlanning
            path3D.Add(originalStart);

            int pointIndex = 0;
-            LogManager.Debug($"[路径点{pointIndex}] 原始起点: ({originalStart.X:F2}, {originalStart.Y:F2}, Z={originalStart.Z:F2})");
+            LogManager.Debug($"[路径点{pointIndex}] 原始起点: ({originalStart.X:F2}, {originalStart.Y:F2}, {originalStart.Z:F2}), 高程={gridMap.GetWorldElevation(originalStart):F2}");
            pointIndex++;

            // 添加第一条边的起点（如果与originalStart不同）
@ -710,14 +712,13 @@ namespace NavisworksTransport.PathPlanning
                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);
+                    var firstPoint = gridMap.CreateWorldPoint(new GridPoint2D(firstInfo.X, firstInfo.Y), firstInfo.Z);

                    // 如果与起点高度不同，添加这个节点（避免直接跳跃）
-                    if (Math.Abs(firstPoint.Z - originalStart.Z) > 0.01)
+                    if (Math.Abs(gridMap.GetWorldElevation(firstPoint) - gridMap.GetWorldElevation(originalStart)) > 0.01)
                    {
                        path3D.Add(firstPoint);
-                        LogManager.Debug($"[路径点{pointIndex}] A*起点: 网格({firstInfo.X},{firstInfo.Y}), 层{firstInfo.LayerIndex}, Z={firstInfo.Z:F2}, 类型={firstInfo.CellType}");
+                        LogManager.Debug($"[路径点{pointIndex}] A*起点: 网格({firstInfo.X},{firstInfo.Y}), 层{firstInfo.LayerIndex}, 高程={firstInfo.Z:F2}, 类型={firstInfo.CellType}");
                        pointIndex++;
                    }
                }
@ -731,10 +732,9 @@ namespace NavisworksTransport.PathPlanning
                    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));
+                path3D.Add(gridMap.CreateWorldPoint(new GridPoint2D(nodeInfo.X, nodeInfo.Y), nodeInfo.Z));

-                LogManager.Debug($"[路径点{pointIndex}] 网格({nodeInfo.X},{nodeInfo.Y}), 层{nodeInfo.LayerIndex}, Z={nodeInfo.Z:F2}, 类型={nodeInfo.CellType}");
+                LogManager.Debug($"[路径点{pointIndex}] 网格({nodeInfo.X},{nodeInfo.Y}), 层{nodeInfo.LayerIndex}, 高程={nodeInfo.Z:F2}, 类型={nodeInfo.CellType}");
                pointIndex++;
            }

@ -829,10 +829,10 @@ namespace NavisworksTransport.PathPlanning

                // 初始化激活层字典
                var activeLayerZ = new Dictionary<GridPoint2D, double>();
-                double targetZ = endPos.Z;  // 使用终点Z作为目标高度
+                double targetZ = gridMap.GetWorldElevation(endPos);  // 使用宿主坐标语义下的终点高程作为目标高度
                double baseSpeed = 5.0; // km/h

-                LogManager.Info($"[A*转换-2.5D] 目标高度: {targetZ:F2}m，基础速度: {baseSpeed}km/h");
+                LogManager.Info($"[A*转换-2.5D] 目标高程: {targetZ:F2}m，基础速度: {baseSpeed}km/h");

                // 输出详细的网格统计信息
                LogManager.Info($"[A*转换-2.5D] 输入网格统计信息:\n{gridMap.GetStatistics()}");
@ -1083,7 +1083,7 @@ namespace NavisworksTransport.PathPlanning

                // 初始化激活高度层字典和目标高度
                var activeLayerZ = new Dictionary<GridPoint2D, double>();
-                double targetZ = endPos.Z;
+                double targetZ = gridMap.GetWorldElevation(endPos);

                // 1. 创建网格基础结构
                double cellSizeInMeters = UnitsConverter.ConvertToMeters(gridMap.CellSize);
@ -1564,19 +1564,19 @@ namespace NavisworksTransport.PathPlanning
                    if (endCell.HasValue)
                    {
                        // 调试：检查门网格的Z坐标
-                        double endZ = endCell.Value.HeightLayers != null && endCell.Value.HeightLayers.Count > 0
+                        double endElevation = endCell.Value.HeightLayers != null && endCell.Value.HeightLayers.Count > 0
                            ? endCell.Value.HeightLayers[0].Z
                            : 0;

                        if (endCell.Value.CellType == "门")
                        {
-                            LogManager.Info($"[路径转换-门] 门网格({endGridPos.X},{endGridPos.Y}) HeightLayer[0].Z={endZ:F3}, 位置({endWorldPos.X:F2},{endWorldPos.Y:F2})");
+                            LogManager.Info($"[路径转换-门] 门网格({endGridPos.X},{endGridPos.Y}) HeightLayer[0].高程={endElevation:F3}, 位置({endWorldPos.X:F2},{endWorldPos.Y:F2})");
                        }
-                        endWorldPos = new Point3D(endWorldPos.X, endWorldPos.Y, endZ);
+                        endWorldPos = gridMap.SetWorldElevation(endWorldPos, endElevation);
                    }
                    else
                    {
-                        LogManager.Warning($"[路径转换] 网格({endGridPos.X},{endGridPos.Y})获取失败，位置({endWorldPos.X:F2},{endWorldPos.Y:F2})，Z坐标保持为0");
+                        LogManager.Warning($"[路径转换] 网格({endGridPos.X},{endGridPos.Y})获取失败，位置({endWorldPos.X:F2},{endWorldPos.Y:F2})，高程保持为默认值");
                    }

                    // 检查是否与上一个点重复（A*可能在转弯点有重复）
@ -1632,8 +1632,8 @@ namespace NavisworksTransport.PathPlanning
                        bool canSkip = false;
                        double maxAllowedHeightDiff = ConfigManager.Instance.Current.PathEditing.MaxHeightDiff; // 从配置文件读取高度差阈值（模型单位）

-                        double startZ = startPoint.Z;
-                        double endZ = endPoint.Z;
+                        double startZ = gridMap.GetWorldElevation(startPoint);
+                        double endZ = gridMap.GetWorldElevation(endPoint);
                        double totalHeightDiff = Math.Abs(endZ - startZ);

                        // 检查1：起终点高度差必须在阈值内
@ -1645,7 +1645,7 @@ namespace NavisworksTransport.PathPlanning

                            for (int midIndex = currentIndex + 1; midIndex < testIndex; midIndex++)
                            {
-                                double midZ = optimizedPath[midIndex].Z;
+                                double midZ = gridMap.GetWorldElevation(optimizedPath[midIndex]);
                                // 如果中间点高度与起点或终点不同，说明有高度变化，不能优化掉
                                if (Math.Abs(midZ - startZ) > heightTolerance ||
                                    Math.Abs(midZ - endZ) > heightTolerance)
@ -1746,18 +1746,23 @@ namespace NavisworksTransport.PathPlanning
                {
                    // 线性插值计算采样点
                    double t = samples > 0 ? (double)i / samples : 0;
-                    var samplePoint = new Point3D(
+                    double interpolatedElevation =
+                        gridMap.GetWorldElevation(start) +
+                        t * (gridMap.GetWorldElevation(end) - gridMap.GetWorldElevation(start));
+
+                    var samplePoint = gridMap.SetWorldElevation(
+                        new Point3D(
                            start.X + t * (end.X - start.X),
                            start.Y + t * (end.Y - start.Y),
-                        start.Z + t * (end.Z - start.Z)
-                    );
+                            start.Z + t * (end.Z - start.Z)),
+                        interpolatedElevation);

                    // 🔥 修复：检查采样点的具体Z高度是否在可通行层范围内
                    // 使用IsPassableAt3DPoint而不是IsWalkable，确保不穿过障碍物层
                    var gridPos = gridMap.WorldToGrid(samplePoint);
                    double tolerance = 0.1; // Z坐标容差（模型单位）

-                    if (!gridMap.IsValidGridPosition(gridPos) || !gridMap.IsPassableAt3DPoint(gridPos, samplePoint.Z, tolerance))
+                    if (!gridMap.IsValidGridPosition(gridPos) || !gridMap.IsPassableAtElevation(gridPos, gridMap.GetWorldElevation(samplePoint), tolerance))
                    {
                        // 🔥 修复：使用Origin计算网格左下角，而不是Bounds.Min
                        var gridMinX = gridMap.Origin.X + gridPos.X * gridMap.CellSize;
@ -1766,7 +1771,7 @@ namespace NavisworksTransport.PathPlanning
                        // 计算网格中心点世界坐标（基于左下角）
                        var gridCenterX = gridMinX + 0.5 * gridMap.CellSize;
                        var gridCenterY = gridMinY + 0.5 * gridMap.CellSize;
-                        var gridCenterZ = samplePoint.Z; // Z保持不变
+                        var gridCenterZ = gridMap.GetWorldElevation(samplePoint);

                        // 计算偏差
                        var deltaX = samplePoint.X - gridCenterX;
@ -2015,7 +2020,7 @@ namespace NavisworksTransport.PathPlanning
        /// <param name="originalStart">原始起点坐标</param>
        /// <param name="originalEnd">原始终点坐标</param>
        /// <returns>修正后的路径</returns>
-        private List<Point3D> CorrectStartEndPoints(List<Point3D> path, Point3D originalStart, Point3D originalEnd)
+        private List<Point3D> CorrectStartEndPoints(List<Point3D> path, Point3D originalStart, Point3D originalEnd, GridMap gridMap)
        {
            if (path == null || path.Count == 0)
                return path;
@ -2025,21 +2030,23 @@ namespace NavisworksTransport.PathPlanning
                LogManager.Info($"[坐标修正] 开始修正路径起终点坐标");
                var correctedPath = new List<Point3D>(path);

-                // 修正起点：保持原始XY坐标，使用路径的Z坐标
+                // 修正起点：保持原始宿主水平位置，复用路径求得的高程
                if (correctedPath.Count > 0)
                {
-                    var originalStartWithZ = new Point3D(originalStart.X, originalStart.Y, correctedPath[0].Z);
-                    LogManager.Info($"[坐标修正] 起点: ({correctedPath[0].X:F3}, {correctedPath[0].Y:F3}, {correctedPath[0].Z:F3}) -> ({originalStartWithZ.X:F3}, {originalStartWithZ.Y:F3}, {originalStartWithZ.Z:F3})");
-                    correctedPath[0] = originalStartWithZ;
+                    double startElevation = gridMap.GetWorldElevation(correctedPath[0]);
+                    var correctedStart = gridMap.SetWorldElevation(originalStart, startElevation);
+                    LogManager.Info($"[坐标修正] 起点: ({correctedPath[0].X:F3}, {correctedPath[0].Y:F3}, {correctedPath[0].Z:F3}) -> ({correctedStart.X:F3}, {correctedStart.Y:F3}, {correctedStart.Z:F3}), 高程={startElevation:F3}");
+                    correctedPath[0] = correctedStart;
                }

-                // 修正终点：保持原始XY坐标，使用路径的Z坐标
+                // 修正终点：保持原始宿主水平位置，复用路径求得的高程
                if (correctedPath.Count > 1)
                {
                    var lastIndex = correctedPath.Count - 1;
-                    var originalEndWithZ = new Point3D(originalEnd.X, originalEnd.Y, correctedPath[lastIndex].Z);
-                    LogManager.Info($"[坐标修正] 终点: ({correctedPath[lastIndex].X:F3}, {correctedPath[lastIndex].Y:F3}, {correctedPath[lastIndex].Z:F3}) -> ({originalEndWithZ.X:F3}, {originalEndWithZ.Y:F3}, {originalEndWithZ.Z:F3})");
-                    correctedPath[lastIndex] = originalEndWithZ;
+                    double endElevation = gridMap.GetWorldElevation(correctedPath[lastIndex]);
+                    var correctedEnd = gridMap.SetWorldElevation(originalEnd, endElevation);
+                    LogManager.Info($"[坐标修正] 终点: ({correctedPath[lastIndex].X:F3}, {correctedPath[lastIndex].Y:F3}, {correctedPath[lastIndex].Z:F3}) -> ({correctedEnd.X:F3}, {correctedEnd.Y:F3}, {correctedEnd.Z:F3}), 高程={endElevation:F3}");
+                    correctedPath[lastIndex] = correctedEnd;
                }

                LogManager.Info($"[坐标修正] 路径起终点坐标修正完成");
@ -2087,7 +2094,7 @@ namespace NavisworksTransport.PathPlanning
                if (cell.HeightLayers != null && cell.HeightLayers.Count > 0)
                {
                    // 查找包含指定Z坐标且满足物体高度的层
-                    var matchingLayer = gridMap.FindLayerContainingZ(gridPos, point.Z, tolerance: 0.5);
+                    var matchingLayer = gridMap.FindLayerContainingElevation(gridPos, gridMap.GetWorldElevation(point), tolerance: 0.5);

                    if (matchingLayer.HasValue)
                    {
@ -2098,12 +2105,12 @@ namespace NavisworksTransport.PathPlanning
                        }
                        else
                        {
-                            LogManager.Warning($"[高度检查] ❌ 找到匹配Z={point.Z:F2}的层Z={matchingLayer.Value.Z:F2}，但高度不足：物体高度{objectHeight:F2}，层高度跨度{matchingLayer.Value.PassableHeight.GetSpan():F2}");
+                            LogManager.Warning($"[高度检查] ❌ 找到匹配高程={gridMap.GetWorldElevation(point):F2}的层高程={matchingLayer.Value.Z:F2}，但高度不足：物体高度{objectHeight:F2}，层高度跨度{matchingLayer.Value.PassableHeight.GetSpan():F2}");
                        }
                    }
                    else
                    {
-                        LogManager.Warning($"[高度检查] ❌ 单元格({gridX}, {gridY})有{cell.HeightLayers.Count}个高度层，但无法找到包含Z={point.Z:F2}的层");
+                        LogManager.Warning($"[高度检查] ❌ 单元格({gridX}, {gridY})有{cell.HeightLayers.Count}个高度层，但无法找到包含高程={gridMap.GetWorldElevation(point):F2}的层");
                    }
                    return false;
                }
@ -2136,17 +2143,17 @@ namespace NavisworksTransport.PathPlanning

            var adjustedPath = new List<Point3D>();

-            // 1. 使用用户指定的原始起点和终点Z坐标（而不是从转换后的路径中提取）
-            double startZ = originalStart.Z;
-            double endZ = originalEnd.Z;
+            // 1. 使用用户指定的原始起点和终点高程（而不是从转换后的路径中提取）
+            double startZ = gridMap.GetWorldElevation(originalStart);
+            double endZ = gridMap.GetWorldElevation(originalEnd);

            // 对比日志：显示原始坐标与路径坐标的差异
-            var pathStartZ = pathWithGridCoords[0].point.Z;
-            var pathEndZ = pathWithGridCoords[pathWithGridCoords.Count - 1].point.Z;
+            var pathStartZ = gridMap.GetWorldElevation(pathWithGridCoords[0].point);
+            var pathEndZ = gridMap.GetWorldElevation(pathWithGridCoords[pathWithGridCoords.Count - 1].point);

-            LogManager.Info($"[智能选层] 原始起点Z={startZ:F3}, 原始终点Z={endZ:F3}");
-            LogManager.Info($"[智能选层] 路径起点Z={pathStartZ:F3}, 路径终点Z={pathEndZ:F3}");
-            LogManager.Info($"[智能选层] 使用原始Z坐标进行智能选层，高度变化={endZ - startZ:F3}");
+            LogManager.Info($"[智能选层] 原始起点高程={startZ:F3}, 原始终点高程={endZ:F3}");
+            LogManager.Info($"[智能选层] 路径起点高程={pathStartZ:F3}, 路径终点高程={pathEndZ:F3}");
+            LogManager.Info($"[智能选层] 使用原始高程进行智能选层，高程变化={endZ - startZ:F3}");

            // 2. 为每个路径点选择高度层
            for (int i = 0; i < pathWithGridCoords.Count; i++)
@ -2175,8 +2182,8 @@ namespace NavisworksTransport.PathPlanning
                    var selectedLayer = SelectBestLayer(cell.HeightLayers, startZ, objectHeight, startZ, endZ, i, pathWithGridCoords.Count, true);
                    if (selectedLayer.HasValue)
                    {
-                        adjustedPath.Add(new Point3D(point.X, point.Y, selectedLayer.Value.Z));
-                        LogManager.Debug($"[智能选层] 起点({gridPos.X},{gridPos.Y}) Z={selectedLayer.Value.Z:F3}");
+                        adjustedPath.Add(gridMap.SetWorldElevation(point, selectedLayer.Value.Z));
+                        LogManager.Debug($"[智能选层] 起点({gridPos.X},{gridPos.Y}) 高程={selectedLayer.Value.Z:F3}");
                    }
                    else
                    {
@ -2188,8 +2195,8 @@ namespace NavisworksTransport.PathPlanning
                    var selectedLayer = SelectBestLayer(cell.HeightLayers, endZ, objectHeight, startZ, endZ, i, pathWithGridCoords.Count, true);
                    if (selectedLayer.HasValue)
                    {
-                        adjustedPath.Add(new Point3D(point.X, point.Y, selectedLayer.Value.Z));
-                        LogManager.Debug($"[智能选层] 终点({gridPos.X},{gridPos.Y}) Z={selectedLayer.Value.Z:F3}");
+                        adjustedPath.Add(gridMap.SetWorldElevation(point, selectedLayer.Value.Z));
+                        LogManager.Debug($"[智能选层] 终点({gridPos.X},{gridPos.Y}) 高程={selectedLayer.Value.Z:F3}");
                    }
                    else
                    {
@ -2199,12 +2206,12 @@ namespace NavisworksTransport.PathPlanning
                else
                {
                    // 中间点：根据高度趋势选择最佳层
-                    var prevZ = adjustedPath[i - 1].Z;
+                    var prevZ = gridMap.GetWorldElevation(adjustedPath[i - 1]);
                    var selectedLayer = SelectBestLayer(cell.HeightLayers, prevZ, objectHeight, startZ, endZ, i, pathWithGridCoords.Count, false);
                    
                    if (selectedLayer.HasValue)
                    {
-                        adjustedPath.Add(new Point3D(point.X, point.Y, selectedLayer.Value.Z));
+                        adjustedPath.Add(gridMap.SetWorldElevation(point, selectedLayer.Value.Z));
                    }
                    else
                    {
@ -2243,36 +2250,34 @@ namespace NavisworksTransport.PathPlanning
            for (int i = 0; i < gridPath.Count; i++)
            {
                var gridPos = gridPath[i];
-                var worldXY = gridMap.GridToWorld3D(gridPos);
-
-                double z;
+                double elevation;

                // 起点：使用原始起点Z
                if (i == 0)
                {
-                    z = originalStart.Z;
-                    LogManager.Info($"[3D还原] 起点 ({gridPos.X},{gridPos.Y}) Z={z:F2}m (原始起点)");
+                    elevation = gridMap.GetWorldElevation(originalStart);
+                    LogManager.Info($"[3D还原] 起点 ({gridPos.X},{gridPos.Y}) 高程={elevation:F2}m (原始起点)");
                }
                // 终点：使用原始终点Z
                else if (i == gridPath.Count - 1)
                {
-                    z = originalEnd.Z;
-                    LogManager.Info($"[3D还原] 终点 ({gridPos.X},{gridPos.Y}) Z={z:F2}m (原始终点)");
+                    elevation = gridMap.GetWorldElevation(originalEnd);
+                    LogManager.Info($"[3D还原] 终点 ({gridPos.X},{gridPos.Y}) 高程={elevation:F2}m (原始终点)");
                }
                // 中间点：使用A*阶段记录的激活层
                else if (activeLayerZ.TryGetValue(gridPos, out double activeZ))
                {
-                    z = activeZ;
-                    LogManager.Debug($"[3D还原] 点{i} ({gridPos.X},{gridPos.Y}) Z={z:F2}m (激活层)");
+                    elevation = activeZ;
+                    LogManager.Debug($"[3D还原] 点{i} ({gridPos.X},{gridPos.Y}) 高程={elevation:F2}m (激活层)");
                }
                else
                {
                    // 降级：无激活层记录，使用前一点高度
-                    z = path3D[i - 1].Z;
-                    LogManager.Warning($"[3D还原] 点{i} ({gridPos.X},{gridPos.Y}) 无激活层记录，使用前点Z={z:F2}m");
+                    elevation = gridMap.GetWorldElevation(path3D[i - 1]);
+                    LogManager.Warning($"[3D还原] 点{i} ({gridPos.X},{gridPos.Y}) 无激活层记录，使用前点高程={elevation:F2}m");
                }

-                path3D.Add(new Point3D(worldXY.X, worldXY.Y, z));
+                path3D.Add(gridMap.CreateWorldPoint(gridPos, elevation));
            }

            LogManager.Info($"[3D还原] 完成，生成了 {path3D.Count} 个3D路径点");
@ -2627,7 +2632,7 @@ namespace NavisworksTransport.PathPlanning

                // 初始化激活高度层字典和目标高度
                var activeLayerZ = new Dictionary<GridPoint2D, double>();
-                double targetZ = endPos.Z;
+                double targetZ = gridMap.GetWorldElevation(endPos);

                // 清空日志记录集合，确保每次执行都能看到映射关系
                _loggedDistances.Clear();
--- a/src/PathPlanning/GridMap.cs
+++ b/src/PathPlanning/GridMap.cs
@ -137,6 +137,36 @@ namespace NavisworksTransport.PathPlanning
            LogManager.Info($"[网格地图] 创建完成: {Width}x{Height}, 坐标系: {_coordinateSystem.Type}");
        }

+        /// <summary>
+        /// 轻量构造函数，主要用于单元测试与脱离 Navisworks 原生边界句柄的纯坐标语义验证。
+        /// </summary>
+        public GridMap(Point3D origin, int width, int height, double cellSize, ICoordinateSystem coordinateSystem)
+        {
+            if (origin == null)
+                throw new ArgumentNullException(nameof(origin));
+            if (width <= 0)
+                throw new ArgumentException("网格宽度必须大于0", nameof(width));
+            if (height <= 0)
+                throw new ArgumentException("网格高度必须大于0", nameof(height));
+            if (cellSize <= 0)
+                throw new ArgumentException("网格单元格大小必须大于0", nameof(cellSize));
+            if (coordinateSystem == null)
+                throw new ArgumentNullException(nameof(coordinateSystem));
+
+            Origin = origin;
+            Width = width;
+            Height = height;
+            CellSize = cellSize;
+            _coordinateSystem = coordinateSystem;
+            Bounds = null;
+
+            Cells = new GridCell[Width, Height];
+            InitializeCells();
+            InitializeHeightCalculation();
+
+            LogManager.Info($"[网格地图] 轻量创建完成: {Width}x{Height}, 坐标系: {_coordinateSystem.Type}");
+        }
+
        /// <summary>
        /// 初始化高度计算组件
        /// </summary>
@ -230,6 +260,44 @@ namespace NavisworksTransport.PathPlanning
            return _coordinateSystem.SetElevation(world2D, elevation);
        }

+        /// <summary>
+        /// 获取宿主坐标点在当前网格坐标语义下的高程值。
+        /// 注意：
+        /// - 对 ZUp 文档，高程是 point.Z
+        /// - 对 YUp 文档，高程是 point.Y
+        /// </summary>
+        public double GetWorldElevation(Point3D worldPoint)
+        {
+            if (worldPoint == null)
+            {
+                throw new ArgumentNullException(nameof(worldPoint));
+            }
+
+            return _coordinateSystem.GetElevation(worldPoint);
+        }
+
+        /// <summary>
+        /// 在当前网格坐标语义下，为一个宿主点设置高程。
+        /// </summary>
+        public Point3D SetWorldElevation(Point3D worldPoint, double elevation)
+        {
+            if (worldPoint == null)
+            {
+                throw new ArgumentNullException(nameof(worldPoint));
+            }
+
+            return _coordinateSystem.SetElevation(worldPoint, elevation);
+        }
+
+        /// <summary>
+        /// 根据网格坐标和指定高程创建宿主坐标点。
+        /// </summary>
+        public Point3D CreateWorldPoint(GridPoint2D gridPosition, double elevation)
+        {
+            Point3D world2D = GridToWorld2D(gridPosition);
+            return _coordinateSystem.SetElevation(world2D, elevation);
+        }
+


        /// <summary>
@ -344,7 +412,7 @@ namespace NavisworksTransport.PathPlanning
        /// <param name="z">Z坐标（米）</param>
        /// <param name="tolerance">容差（米），默认0.1m</param>
        /// <returns>符合条件的高度层，如果没有则返回null</returns>
-        public HeightLayer? FindLayerContainingZ(GridPoint2D gridPosition, double z, double tolerance = DEFAULT_LAYER_TOLERANCE_METERS)
+        public HeightLayer? FindLayerContainingElevation(GridPoint2D gridPosition, double elevation, double tolerance = DEFAULT_LAYER_TOLERANCE_METERS)
        {
            if (!IsValidGridPosition(gridPosition))
                return null;
@ -359,7 +427,7 @@ namespace NavisworksTransport.PathPlanning

            foreach (var layer in cell.HeightLayers)
            {
-                double distance = Math.Abs(layer.Z - z);
+                double distance = Math.Abs(layer.Z - elevation);
                if (distance < minDistance && distance <= tolerance)
                {
                    minDistance = distance;
@ -370,6 +438,11 @@ namespace NavisworksTransport.PathPlanning
            return bestLayer;
        }

+        public HeightLayer? FindLayerContainingZ(GridPoint2D gridPosition, double z, double tolerance = DEFAULT_LAYER_TOLERANCE_METERS)
+        {
+            return FindLayerContainingElevation(gridPosition, z, tolerance);
+        }
+
        /// <summary>
        /// 获取指定网格位置的所有高度层
        /// </summary>
@ -441,7 +514,7 @@ namespace NavisworksTransport.PathPlanning
        /// <param name="zCoord">Z坐标（模型单位）</param>
        /// <param name="tolerance">Z坐标容差（模型单位），默认0.1</param>
        /// <returns>是否可通行</returns>
-        public bool IsPassableAt3DPoint(GridPoint2D gridPosition, double zCoord, double tolerance = DEFAULT_LAYER_TOLERANCE_METERS)
+        public bool IsPassableAtElevation(GridPoint2D gridPosition, double elevation, double tolerance = DEFAULT_LAYER_TOLERANCE_METERS)
        {
            if (!IsValidGridPosition(gridPosition))
                return false;
@ -463,7 +536,7 @@ namespace NavisworksTransport.PathPlanning
                    double layerMinZ = layer.Z;
                    double layerMaxZ = layer.Z + layer.PassableHeight.GetSpan();

-                    if (zCoord >= layerMinZ - tolerance && zCoord <= layerMaxZ + tolerance)
+                    if (elevation >= layerMinZ - tolerance && elevation <= layerMaxZ + tolerance)
                    {
                        return true;
                    }
@ -473,6 +546,11 @@ namespace NavisworksTransport.PathPlanning
            return false;
        }

+        public bool IsPassableAt3DPoint(GridPoint2D gridPosition, double zCoord, double tolerance = DEFAULT_LAYER_TOLERANCE_METERS)
+        {
+            return IsPassableAtElevation(gridPosition, zCoord, tolerance);
+        }
+
        /// <summary>
        /// 获取所有通道类型的网格单元
        /// </summary>
--- a/src/PathPlanning/PathOptimizer.cs
+++ b/src/PathPlanning/PathOptimizer.cs
@ -118,7 +118,7 @@ namespace NavisworksTransport.PathPlanning
                    }
                    else
                    {
-                        optimizedPoints = SimplifyCollinearPoints(optimizedPoints);
+                        optimizedPoints = SimplifyCollinearPoints(optimizedPoints, gridMap);
                        LogManager.Info($"[路径优化] 简化完成，点数：{originalCount} -> {optimizedPoints.Count}");
                    }
                }
@ -170,7 +170,7 @@ namespace NavisworksTransport.PathPlanning
                // 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 hasHeightChange = i > 0 && HasHeightChange(points[i], points[i - 1], gridMap);

                bool shouldKeep = hasGridChange || hasSpeedChange || hasHeightChange;

@ -213,7 +213,8 @@ namespace NavisworksTransport.PathPlanning
                bool hasHeightChange = HasHeightChange(
                    dedupedPoints[i-2],
                    dedupedPoints[i-1],
-                    dedupedPoints[i]
+                    dedupedPoints[i],
+                    gridMap
                );

                // 检查限速变化
@ -264,7 +265,7 @@ namespace NavisworksTransport.PathPlanning
        /// </summary>
        /// <param name="points">原始路径点列表</param>
        /// <returns>简化后的路径点列表</returns>
-        private List<PathPoint> SimplifyCollinearPoints(List<PathPoint> points)
+        private List<PathPoint> SimplifyCollinearPoints(List<PathPoint> points, GridMap gridMap = null)
        {
            if (points.Count <= 2) return points;

@ -281,7 +282,7 @@ namespace NavisworksTransport.PathPlanning
                var nextPoint = points[i + 1];

                // 检查是否在同一直线上
-                bool isCollinear = IsCollinear(prevPoint.Position, currPoint.Position, nextPoint.Position);
+                bool isCollinear = IsCollinear(prevPoint.Position, currPoint.Position, nextPoint.Position, gridMap);

                // 🔥 关键修复：检查限速变化，即使共线也不能移除限速边界点
                bool hasSpeedLimitChange = HasSpeedLimitChange(prevPoint, currPoint, nextPoint);
@ -316,10 +317,12 @@ namespace NavisworksTransport.PathPlanning
        /// <param name="point1">第一个路径点</param>
        /// <param name="point2">第二个路径点</param>
        /// <returns>如果有高度变化返回true，需要保留中间点</returns>
-        private bool HasHeightChange(PathPoint point1, PathPoint point2)
+        private bool HasHeightChange(PathPoint point1, PathPoint point2, GridMap gridMap = null)
        {
            const double heightTolerance = 1e-6; // 仅覆盖浮点数精度误差
-            return Math.Abs(point1.Position.Z - point2.Position.Z) > heightTolerance;
+            double elevation1 = GetElevation(point1.Position, gridMap);
+            double elevation2 = GetElevation(point2.Position, gridMap);
+            return Math.Abs(elevation1 - elevation2) > heightTolerance;
        }

        /// <summary>
@ -329,13 +332,17 @@ namespace NavisworksTransport.PathPlanning
        /// <param name="p2">中间点的3D坐标</param>
        /// <param name="p3">第三个点的3D坐标</param>
        /// <returns>如果有高度变化返回true，不能跳过中间点</returns>
-        private bool HasHeightChange(Point3D p1, Point3D p2, Point3D p3)
+        private bool HasHeightChange(Point3D p1, Point3D p2, Point3D p3, GridMap gridMap = null)
        {
            const double heightTolerance = 1e-6; // 仅覆盖浮点数精度误差

            // 检查任意两点间是否有高度变化
-            if (Math.Abs(p1.Z - p2.Z) > heightTolerance ||
-                Math.Abs(p2.Z - p3.Z) > heightTolerance)
+            double elevation1 = GetElevation(p1, gridMap);
+            double elevation2 = GetElevation(p2, gridMap);
+            double elevation3 = GetElevation(p3, gridMap);
+
+            if (Math.Abs(elevation1 - elevation2) > heightTolerance ||
+                Math.Abs(elevation2 - elevation3) > heightTolerance)
            {
                return true;
            }
@ -349,9 +356,9 @@ namespace NavisworksTransport.PathPlanning
        /// <param name="point2">中间路径点</param>
        /// <param name="point3">第三个路径点</param>
        /// <returns>如果有高度变化返回true，不能跳过中间点</returns>
-        private bool HasHeightChange(PathPoint point1, PathPoint point2, PathPoint point3)
+        private bool HasHeightChange(PathPoint point1, PathPoint point2, PathPoint point3, GridMap gridMap = null)
        {
-            return HasHeightChange(point1.Position, point2.Position, point3.Position);
+            return HasHeightChange(point1.Position, point2.Position, point3.Position, gridMap);
        }

        /// <summary>
@ -395,25 +402,41 @@ namespace NavisworksTransport.PathPlanning
        /// <param name="p2">第二个点（中间点）</param>
        /// <param name="p3">第三个点</param>
        /// <returns>如果三点在同一直线上且方向一致返回true</returns>
-        private bool IsCollinear(Point3D p1, Point3D p2, Point3D p3)
+        private bool IsCollinear(Point3D p1, Point3D p2, Point3D p3, GridMap gridMap = null)
        {
            const double tolerance = COLLINEAR_TOLERANCE_METERS; // 容差值，单位：模型单位

            // 🔥 修复：使用统一的高度检查函数
-            if (HasHeightChange(p1, p2, p3))
+            if (HasHeightChange(p1, p2, p3, gridMap))
            {
                LogManager.Debug($"[共线检测] ❌ 拒绝Z方向变化：({p1.X:F3},{p1.Y:F3},{p1.Z:F3}) -> ({p2.X:F3},{p2.Y:F3},{p2.Z:F3}) -> ({p3.X:F3},{p3.Y:F3},{p3.Z:F3})");
                return false;
            }

-            // 🔥 关键修复：分别检查两个线段的方向
-            // 线段1: p1 -> p2
-            double dx12 = p2.X - p1.X;
-            double dy12 = p2.Y - p1.Y;
+            double dx12;
+            double dy12;
+            double dx23;
+            double dy23;

-            // 线段2: p2 -> p3  
-            double dx23 = p3.X - p2.X;
-            double dy23 = p3.Y - p2.Y;
+            if (gridMap != null)
+            {
+                var g1 = gridMap.WorldToGrid(p1);
+                var g2 = gridMap.WorldToGrid(p2);
+                var g3 = gridMap.WorldToGrid(p3);
+
+                dx12 = g2.X - g1.X;
+                dy12 = g2.Y - g1.Y;
+                dx23 = g3.X - g2.X;
+                dy23 = g3.Y - g2.Y;
+            }
+            else
+            {
+                // 回退到历史 ZUp 语义，仅用于未提供网格地图的旧调用链。
+                dx12 = p2.X - p1.X;
+                dy12 = p2.Y - p1.Y;
+                dx23 = p3.X - p2.X;
+                dy23 = p3.Y - p2.Y;
+            }

            // 🔥 修复：使用更严格的容差检查真正的重复点（坐标完全相同）
            const double duplicatePointTolerance = DUPLICATE_POINT_TOLERANCE_METERS; // 用很小的值检查真正的重复点
@ -453,6 +476,11 @@ namespace NavisworksTransport.PathPlanning
            }
        }

+        private static double GetElevation(Point3D point, GridMap gridMap)
+        {
+            return gridMap?.GetWorldElevation(point) ?? point.Z;
+        }
+
        /// <summary>
        /// 创建优化后的路径对象
        /// </summary>
--- a/src/Utils/CoordinateSystem/HostPlanarGridHelper.cs
+++ b/src/Utils/CoordinateSystem/HostPlanarGridHelper.cs
@ -0,0 +1,99 @@
+using System;
+using System.Numerics;
+
+namespace NavisworksTransport.Utils.CoordinateSystem
+{
+    /// <summary>
+    /// 自动路径规划 / 网格系统使用的宿主平面与高程语义辅助。
+    /// 
+    /// 关键约束：
+    /// - 对外继续维持 ICoordinateSystem 的历史契约：
+    ///   - ZUp: 水平面 = Host XY，高程 = Host Z
+    ///   - YUp: 水平面 = Host XZ，高程 = Host Y
+    /// - 对内通过 HostCoordinateAdapter 统一解释“哪根轴是宿主 up”。
+    /// 
+    /// 这样自动路径规划既不会继续把世界 Z 硬编码成高度轴，
+    /// 也不会把旧的水平坐标 h1/h2 语义误替换成 raw canonical XY。
+    /// </summary>
+    public static class HostPlanarGridHelper
+    {
+        public static double GetElevation3(Vector3 hostPoint, HostCoordinateAdapter adapter)
+        {
+            if (adapter == null)
+            {
+                throw new ArgumentNullException(nameof(adapter));
+            }
+
+            return adapter.ToCanonicalPoint3(hostPoint).Z;
+        }
+
+        public static Vector3 SetElevation3(Vector3 hostPoint, double elevation, HostCoordinateAdapter adapter)
+        {
+            if (adapter == null)
+            {
+                throw new ArgumentNullException(nameof(adapter));
+            }
+
+            Vector3 canonicalPoint = adapter.ToCanonicalPoint3(hostPoint);
+            Vector3 canonicalResult = new Vector3(canonicalPoint.X, canonicalPoint.Y, (float)elevation);
+            return adapter.FromCanonicalPoint3(canonicalResult);
+        }
+
+        public static (double h1, double h2) GetHorizontalCoords3(Vector3 hostPoint, HostCoordinateAdapter adapter)
+        {
+            if (adapter == null)
+            {
+                throw new ArgumentNullException(nameof(adapter));
+            }
+
+            switch (adapter.HostType)
+            {
+                case CoordinateSystemType.ZUp:
+                    return (hostPoint.X, hostPoint.Y);
+                case CoordinateSystemType.YUp:
+                    return (hostPoint.X, hostPoint.Z);
+                default:
+                    throw new InvalidOperationException($"不支持的宿主坐标系: {adapter.HostType}");
+            }
+        }
+
+        public static Vector3 CreateHostPoint3(double h1, double h2, double elevation, HostCoordinateAdapter adapter)
+        {
+            if (adapter == null)
+            {
+                throw new ArgumentNullException(nameof(adapter));
+            }
+
+            switch (adapter.HostType)
+            {
+                case CoordinateSystemType.ZUp:
+                    return new Vector3((float)h1, (float)h2, (float)elevation);
+                case CoordinateSystemType.YUp:
+                    return new Vector3((float)h1, (float)elevation, (float)h2);
+                default:
+                    throw new InvalidOperationException($"不支持的宿主坐标系: {adapter.HostType}");
+            }
+        }
+
+        public static (double min1, double max1, double min2, double max2) GetHorizontalRange3(
+            Vector3 hostMin,
+            Vector3 hostMax,
+            HostCoordinateAdapter adapter)
+        {
+            if (adapter == null)
+            {
+                throw new ArgumentNullException(nameof(adapter));
+            }
+
+            switch (adapter.HostType)
+            {
+                case CoordinateSystemType.ZUp:
+                    return (hostMin.X, hostMax.X, hostMin.Y, hostMax.Y);
+                case CoordinateSystemType.YUp:
+                    return (hostMin.X, hostMax.X, hostMin.Z, hostMax.Z);
+                default:
+                    throw new InvalidOperationException($"不支持的宿主坐标系: {adapter.HostType}");
+            }
+        }
+    }
+}
--- a/src/Utils/CoordinateSystem/YUpCoordinateSystem.cs
+++ b/src/Utils/CoordinateSystem/YUpCoordinateSystem.cs
@ -1,4 +1,5 @@
 using Autodesk.Navisworks.Api;
+using System.Numerics;

 namespace NavisworksTransport.Utils.CoordinateSystem
 {
@ -8,6 +9,8 @@ namespace NavisworksTransport.Utils.CoordinateSystem
    /// </summary>
    public class YUpCoordinateSystem : ICoordinateSystem
    {
+        private static readonly HostCoordinateAdapter Adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
+
        public CoordinateSystemType Type => CoordinateSystemType.YUp;
        
        public Vector3D UpVector => new Vector3D(0, 1, 0);
@ -18,22 +21,22 @@ namespace NavisworksTransport.Utils.CoordinateSystem
        
        public double GetElevation(Point3D point)
        {
-            return point.Y;
+            return HostPlanarGridHelper.GetElevation3(ToVector3(point), Adapter);
        }
        
        public Point3D SetElevation(Point3D point, double elevation)
        {
-            return new Point3D(point.X, elevation, point.Z);
+            return ToPoint3D(HostPlanarGridHelper.SetElevation3(ToVector3(point), elevation, Adapter));
        }
        
        public (double h1, double h2) GetHorizontalCoords(Point3D point)
        {
-            return (point.X, point.Z);
+            return HostPlanarGridHelper.GetHorizontalCoords3(ToVector3(point), Adapter);
        }
        
        public Point3D CreatePoint(double h1, double h2, double elevation)
        {
-            return new Point3D(h1, elevation, h2);
+            return ToPoint3D(HostPlanarGridHelper.CreateHostPoint3(h1, h2, elevation, Adapter));
        }
        
        public (double min, double max) GetHeightRange(BoundingBox3D bounds)
@ -43,12 +46,22 @@ namespace NavisworksTransport.Utils.CoordinateSystem
        
        public (double min1, double max1, double min2, double max2) GetHorizontalRange(BoundingBox3D bounds)
        {
-            return (bounds.Min.X, bounds.Max.X, bounds.Min.Z, bounds.Max.Z);
+            return HostPlanarGridHelper.GetHorizontalRange3(ToVector3(bounds.Min), ToVector3(bounds.Max), Adapter);
        }
        
        public override string ToString()
        {
            return "Y-Up 坐标系 (Y轴向上)";
        }
+
+        private static Vector3 ToVector3(Point3D point)
+        {
+            return new Vector3((float)point.X, (float)point.Y, (float)point.Z);
+        }
+
+        private static Point3D ToPoint3D(Vector3 point)
+        {
+            return new Point3D(point.X, point.Y, point.Z);
+        }
    }
 }
--- a/src/Utils/CoordinateSystem/ZUpCoordinateSystem.cs
+++ b/src/Utils/CoordinateSystem/ZUpCoordinateSystem.cs
@ -1,4 +1,5 @@
 using Autodesk.Navisworks.Api;
+using System.Numerics;

 namespace NavisworksTransport.Utils.CoordinateSystem
 {
@ -8,6 +9,8 @@ namespace NavisworksTransport.Utils.CoordinateSystem
    /// </summary>
    public class ZUpCoordinateSystem : ICoordinateSystem
    {
+        private static readonly HostCoordinateAdapter Adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
+
        public CoordinateSystemType Type => CoordinateSystemType.ZUp;
        
        public Vector3D UpVector => new Vector3D(0, 0, 1);
@ -18,22 +21,22 @@ namespace NavisworksTransport.Utils.CoordinateSystem
        
        public double GetElevation(Point3D point)
        {
-            return point.Z;
+            return HostPlanarGridHelper.GetElevation3(ToVector3(point), Adapter);
        }
        
        public Point3D SetElevation(Point3D point, double elevation)
        {
-            return new Point3D(point.X, point.Y, elevation);
+            return ToPoint3D(HostPlanarGridHelper.SetElevation3(ToVector3(point), elevation, Adapter));
        }
        
        public (double h1, double h2) GetHorizontalCoords(Point3D point)
        {
-            return (point.X, point.Y);
+            return HostPlanarGridHelper.GetHorizontalCoords3(ToVector3(point), Adapter);
        }
        
        public Point3D CreatePoint(double h1, double h2, double elevation)
        {
-            return new Point3D(h1, h2, elevation);
+            return ToPoint3D(HostPlanarGridHelper.CreateHostPoint3(h1, h2, elevation, Adapter));
        }
        
        public (double min, double max) GetHeightRange(BoundingBox3D bounds)
@ -43,12 +46,22 @@ namespace NavisworksTransport.Utils.CoordinateSystem
        
        public (double min1, double max1, double min2, double max2) GetHorizontalRange(BoundingBox3D bounds)
        {
-            return (bounds.Min.X, bounds.Max.X, bounds.Min.Y, bounds.Max.Y);
+            return HostPlanarGridHelper.GetHorizontalRange3(ToVector3(bounds.Min), ToVector3(bounds.Max), Adapter);
        }
        
        public override string ToString()
        {
            return "Z-Up 坐标系 (Z轴向上)";
        }
+
+        private static Vector3 ToVector3(Point3D point)
+        {
+            return new Vector3((float)point.X, (float)point.Y, (float)point.Z);
+        }
+
+        private static Point3D ToPoint3D(Vector3 point)
+        {
+            return new Point3D(point.X, point.Y, point.Z);
+        }
    }
 }