文献：Efficient Global Navigational Planning in 3-D Structures Based on Point Cloud Tomography
作者：Bowen Yang, Jie Cheng, Bohuan Xue, Jianhao Jiao, Ming Liu
期刊：IEEE/ASME Transactions on Mechatronics（TMECH）
DOI：10.1109/TMECH.2024.3396001
arXiv：https://arxiv.org/abs/2403.07631
论文项目主页：https://byangw.github.io/projects/tmech2024/
开源代码：https://github.com/byangw/PCT_planner
IEEE Xplore：https://ieeexplore.ieee.org/document/10531813
关键词：Point Cloud Tomography、3D Navigation、Traversability Estimation、Multi-layer Environment、A、Trajectory Optimization*

---

1. 文献概述

这篇论文讨论的是一个很典型、但长期存在矛盾的问题：

地面机器人如何在具有楼层、楼梯、坡道、桥梁、悬空结构和狭窄顶部空间的复杂三维环境中，同时获得足够完整的三维环境表达和足够高的全局规划效率？

对于无人机，使用三维体素描述自由空间较为自然；但对于轮式机器人和四足机器人，真正决定其能否通过某一区域的往往不是三维自由空间本身，而是地面形状、坡度、台阶、机器人尺寸、上方净空以及机器人自身的运动能力。

传统二维或 2.5D 高程地图计算效率很高，但一张高程图在同一个平面坐标 $(x,y)$ 上通常只能表达一个主要地面高度，因此难以同时表示：

• 楼上与楼下；
• 桥面与桥下；
• 隧道；
• 悬空平台；
• 上方障碍物；
• 多层螺旋结构。

如果改用三维 voxel、ESDF、mesh 或直接在点云上进行计算，则能够保留完整三维结构，却会带来明显增加的地图构建、环境评估和图搜索开销。

论文提出 Point Cloud Tomography，点云层析表示。其核心思想不是把整个三维空间离散成稠密体素，而是沿高度方向设置一系列水平切片面，将点云投影到这些平面上，从而生成若干张 2.5D tomogram slices。每一张切片同时保存：

1. 地面高程；
2. 顶棚高程；
3. 通行代价。

随后进一步删除冗余切片，在保留下来的少量 2.5D 图层之间使用改进 A* 搜索，再通过轨迹优化得到带速度信息的连续三维轨迹。

整套流程可以概括为：

3D Point Cloud
      │
      ▼
Point Cloud Tomography
      │
      ├── Ground Elevation
      ├── Ceiling Elevation
      └── Travel Cost
      │
      ▼
Traversability Estimation
      │
      ▼
Tomogram Simplification
      │
      ▼
Multi-slice A* Search
      │
      ▼
Initial 3D Path
      │
      ▼
Polynomial Trajectory Optimization
      │
      ▼
Smooth 3D Trajectory
      +
Active Body Height Adaptation

Fig. 1　PCT 全局导航框架的总体能力展示：多层结构规划、轮式/四足机器人适配以及主动机身高度调整

论文报告，在测试场景中，该方法相较现有三维导航方法将场景评估时间降低约 3 个数量级，将路径规划速度提高约 3 倍；集成后的完整导航流程整体速度可提高约 2 个数量级。

---

2. 论文解决的关键科学问题

2.1 三维表达能力与计算效率之间的矛盾

三维导航中的环境表达大致可以分为四类：

表达方式	优点	主要问题
Point Cloud	保留原始三维几何	数据无序，邻域和搜索开销大
Mesh	表面连续，适合几何分析	重建和更新成本较高
3D Voxel / ESDF	数据结构规则，三维关系明确	大场景内存和搜索节点数量大
Elevation Map	地面机器人效率很高	难以表达多层和悬空结构

论文试图保留 elevation map 的高效率，同时获得接近三维地图的多层表达能力。

因此，核心问题不是简单地“做一个新的 A*”，而是：

能否设计一种更适合地面机器人的三维环境表示，使三维导航问题尽可能退化为若干个二维/2.5D 问题？

PCT 的答案是：可以通过“层析切片”实现。

---

2.2 同一个位置不仅要知道地面，还要知道上方空间

对于轮式机器人，判断某位置能否通过通常需要关注：

$$\text{ground condition}.$$

但是对于四足机器人或能够主动改变机身高度的平台，还要同时考虑：

$$\text{ground condition}+\text{ceiling clearance}.$$

例如某处地面完全平坦，但顶棚离地只有 $0.4\text{m}$，对于正常机身高度 $0.65\text{m}$ 的机器人仍不可直接通过。

因此论文不是简单增加一个障碍高度，而是对每个栅格同时保存：

$$e^G\text{和}{e}^C,$$

分别表示 Ground Elevation 和 Ceiling Elevation。

---

2.3 不同机器人具有不同的“可通行”定义

同一个台阶：

• 对普通轮式机器人可能不可通行；
• 对四足机器人可能能够跨越；
• 对具有高度调整能力的机器人，上方净空不足时也可能仍然能够通过。

所以 traversability 不能完全由几何环境决定，还应该与机器人运动能力有关。

论文将这一思想概括为：

Motion Capabilities Awareness

即地图中的 travel cost 与机器人本体能力相关，而不是纯粹的几何障碍地图。

---

2.4 三维全局搜索不能访问过多无意义节点

若采用分辨率为 $r$ 的稠密三维 voxel，理论节点规模近似为：

$$N_{\mathrm{voxel}}=\frac{L_x{L}_y{L}_z}{r^3}.$$

对于地面机器人，大量体素实际上根本不会成为可行运动表面。

PCT 希望将搜索规模降低为：

$$N_{\mathrm{PCT}}\approx \sum _{k=1}^K{N}_{xy}^{(k)},$$

其中 $K$ 是简化后真正需要保留的 tomogram slice 数量。

这也是整篇论文效率提升的根本来源。

---

3. Point Cloud Tomography：点云层析表示

3.1 Tomogram Slice 的定义

论文定义一组 tomogram slices：

S={Sk∣k=0,1,…,N}. \mathcal{S} =\left\{ S_k \mid k=0,1,\ldots,N \right\}. S={Sk​∣k=0,1,…,N}.

每张切片为：

$$S_k=\left\{e_k^G,e_k^C,c_k^T\right\}.$$

其中：

• $e_k^G$：Ground Elevation Map；
• $e_k^C$：Ceiling Elevation Map；
• $c_k^T$：Travel Cost Map。

进一步写到每个栅格：

$$e_{i,j,k}^G,e_{i,j,k}^C,c_{i,j,k}^T.$$

其中 $(i,j)$ 表示二维平面上的栅格索引，$k$ 表示切片编号。

因此，PCT 并不是把不同高度简单做成 occupancy slice，而是每个 slice 自身仍然是一张包含连续地面高度和连续顶棚高度的 2.5D 地图。

---

3.2 水平切片平面的建立

设点云最低高度为：

$$z_{\min }.$$

第 $k$ 个水平平面的高度定义为：

$$z_k=z_{\min }+k{d}_s,$$

其中 $d_s$ 为切片间距。

为了保证所有机器人能够实际通过的空间至少被一个切片捕获，论文要求：

$$d_s\le d_{\min },$$

其中 $d_{\min }$ 是机器人能够通过空间所要求的最小地面—顶棚间距。

实验中采用：

$$d_s=d_{\min }=0.50\text{m}.$$

---

3.3 点云投影

对于三维点：

$${\mathbf{p}}_u=[x_u,y_u,z_u{]}^T,$$

先根据：

$$(x_u,y_u)$$

进行二维 rasterization，得到栅格：

$$(i,j).$$

对于第 $k$ 个切片平面，如果：

$$z_u\ge z_{\min }+k{d}_s,$$

该点位于切片上方对应关系中的“lower group”边界贡献，论文算法实际以最大高度更新地面：

$$e_{i,j,k}^G=\max \left(z_u,e_{i,j,k}^G\right).$$

如果：

$$z_u<z_{\min }+k{d}_s,$$

则用于更新 ceiling：

$$e_{i,j,k}^C=\min \left(z_u,e_{i,j,k}^C\right).$$

从几何意义上理解，就是从切片平面分别向下和向上“观察”点云，找到该平面对应位置最近的地面和顶棚表面。

Fig. 2　点云层析切片构造、冗余切片判定及 Gateway 跨层连接示意

Fig. 2 实际上已经把 PCT 的三个核心概念全部说明：

1. 每个 slice 同时具有 ground 与 ceiling；
2. 中间冗余 slice 可以删除；
3. 不同 slice 通过 gateway 发生跨层搜索。

---

4. 通行性评估

4.1 地面—顶棚间距

定义：

$$d^I=e^C-e^G.$$

机器人机身高度可以在：

$$[d_{\min },d_{\mathrm{ref}}]$$

范围内调节。

其中：

• $d_{\min }$：机器人压低机身后所需最小空间；
• $d_{\mathrm{ref}}$：正常工作高度。

如果：

$$d^I<d_{\min },$$

则无论怎样降低机身都无法通过。

论文定义 interval cost：

$$c^I=\{\begin{array}{ll}{c}^B,& d^I<d_{\min }\\ \max \left(0,{\alpha }_d\left(d_{\mathrm{ref}}-d^I\right)\right),& \text{otherwise}\end{array}$$

其中：

• $c^B$：不可通行障碍的 barrier cost；
• ${\alpha }_d$：高度调整代价缩放系数。

这意味着当：

$$d_{\min }\le d^I<d_{\mathrm{ref}},$$

机器人虽然能够通过，但必须降低机身，因此会增加 travel cost。

这不是硬约束，而是：

“可以通过，但不优先走”的软代价。

---

5. 地形坡度与台阶判断

5.1 地面梯度

对 ground elevation map 使用有限差分得到：

$$g^x,g^y.$$

论文定义：

$$m^{xy}=\max \left(|g^x|,|g^y|\right),$$

以及：

$$m^{\mathrm{grad}}=\sqrt{(g^x{)}^2+(g^y{)}^2}.$$

显然：

$$m^{xy}\le m^{\mathrm{grad}}.$$

三个重要阈值为：

$${\theta }_b,{\theta }_s,{\theta }_p.$$

其中：

• ${\theta }_b$：barrier threshold；
• ${\theta }_s$：smooth terrain threshold；
• ${\theta }_p$：周围安全地面比例阈值。

---

5.2 普通坡面

当：

$$m^{xy}>{\theta }_b,$$

认为该位置属于明显 barrier boundary：

$$c^G=c^B.$$

当：

$$m^{\mathrm{grad}}<{\theta }_s,$$

认为属于连续平缓地形：

$$c^G={\alpha }_s{\left(\frac{m^{\mathrm{grad}}}{{\theta }_s}\right)}^2.$$

坡度越大，代价越高。

---

5.3 四足机器人为什么能够规划楼梯

这里是论文相较普通 elevation-map traversability 的一个重要区别。

如果某个位置既不是普通平缓地面，也没有直接超过 barrier threshold，它可能是：

• 台阶边缘；
• 楼梯竖直面附近；
• 可被四足机器人跨越的不连续地形。

如果直接按照局部梯度判障，楼梯竖直面几乎一定会被认为不可通行。

论文进一步统计该点邻域中平缓栅格所占比例：

$$p_s.$$

若：

$$p_s>{\theta }_p,$$

说明虽然当前点具有较大高度突变，但周围仍然存在足够的稳定落脚区域，因此四足机器人可以跨过该位置。

此时代价为：

$$c^G={\alpha }_b{\left(\frac{m^{xy}}{{\theta }_b}\right)}^2.$$

否则：

$$c^G=c^B.$$

所以 PCT Planner 并不是“把楼梯当平面”，而是：

允许具有足够安全支撑区域的局部高度不连续结构被四足机器人跨越。

对于轮式机器人，只需设置：

$${\theta }_p=1.0,$$

即可基本禁用这种 stair/step traversability。

---

6. 初始通行代价与膨胀核

地面和顶棚代价融合：

$$c^{\mathrm{init}}=\min \left(c^B,c^I+c^G\right).$$

随后考虑机器人碰撞半径 $r_c$。

论文设置障碍膨胀距离：

$$d_{\inf }\ge r_c,$$

并进一步设置 safe margin：

$$d_{\mathrm{sm}}.$$

对于 inflation kernel 中位置 $(m,n)$，距离 kernel center 为：

$$d_{mn}.$$

核函数为：

$$K(m,n)=\max \left(0,\min \left(1-\frac{d_{mn}-d_{\inf }}{d_{\mathrm{sm}}-r_g},1\right)\right).$$

其中 $r_g$ 是地图栅格分辨率。

Fig. 3　障碍膨胀核、膨胀前通行代价和最终 Travel Cost Map

Fig. 3 中：

• orange：不可通行区域；
• blue：可通行区域；
• 中心螺旋楼梯由于宽度不足被判为不可通行；
• inflation 后 barrier 周围形成连续变化的安全代价梯度。

最终 travel cost 通过 kernel 和局部 cost patch 做 Hadamard product，并取结果最大值作为中心栅格代价。

这一步的意义类似于二维导航中的 obstacle inflation，但 PCT 的 cost 本身已经综合了：

$$\text{terrain}+\text{ceiling}+\text{robot capability}.$$

---

7. Tomogram Simplification：为什么 46 层最后只剩 5 层

如果始终按照：

$$d_s=d_{\min }=0.5\text{m}$$

构造切片，在高大的场景中仍然可能产生大量 slice。

论文进一步提出切片简化。

设第 $k$ 层全部可通行栅格组成：

$$M_k.$$

如果：

$$M_k\subset \left(M_{k-1}\cup M_{k+1}\right),$$

意味着第 $k$ 层能够表达的搜索空间已经完全包含在相邻两层中，因此：

$$S_k$$

可以删除。

---

7.1 Unique Grid 判断

对满足：

$$c_{i,j,k}^T<c^B$$

的可通行栅格，如果相对于下层满足：

$$\left(e_{i,j,k}^G-e_{i,j,k-1}^G>0\right)\vee \left(c_{i,j,k}^T<c_{i,j,k-1}^T\right),$$

同时相对于上层满足：

$$\left(e_{i,j,k+1}^G-e_{i,j,k}^G>0\right)\vee \left(c_{i,j,k}^T<c_{i,j,k+1}^T\right),$$

则该栅格具有独特的：

• 空间位置；
• 或更合理的通行代价。

若某个 slice 中不存在任何 unique grid，则整个 slice 被删除。

论文的 spiral 场景中：

$$46⟶5$$

即原始 46 张层析切片最终只保留 5 张。

这是路径搜索效率明显提升的重要原因。

Fig. 4　螺旋多层场景的 PCT 表达：五张简化 Tomogram Slice、Travel Cost Map 以及跨 Slice 的路径搜索过程

Fig. 4 的三行含义为：

• row a：原始点云及 5 张简化后的 ground/ceiling tomogram；
• row b：对应 travel cost maps；
• row c：规划器通过红色/蓝色 gateway 在不同 slice 之间切换并生成三维路径。

---

8. 跨 Tomogram Slice 的改进 A*

8.1 同一层内部搜索

每个二维栅格作为图节点。

在同一个 cost map 中使用 8-connected neighborhood：

Nxy={N, NE, E, SE, S, SW, W, NW}. \mathcal{N}_{xy} =\{ N,\, NE,\, E,\, SE,\, S,\, SW,\, W,\, NW \}. Nxy​={N,NE,E,SE,S,SW,W,NW}.

因此单层内仍然可以使用非常成熟且高效的二维 A*。

---

8.2 相邻层之间的连接

查询：

$$(i,j,k)$$

时，同时检查：

$$(i,j,k-1)$$

和：

$$(i,j,k+1).$$

如果两个切片在相同 $(i,j)$ 上具有相同 ground elevation：

$$e_{i,j,k}^G=e_{i,j,k+1}^G,$$

它们在真实三维空间中实际上对应同一个地面位置。

节点代价可取：

$$c^N=\min \left(c_{i,j,k}^T,c_{i,j,k+1}^T\right).$$

---

8.3 Gateway

如果相邻 slice 在同一个三维地面位置重叠，但上层 slice 的 cost 更低，更能反映该区域真实通行性，那么当前栅格成为一个 upward gateway。

简化理解为：

$$e_{i,j,k}^G=e_{i,j,k+1}^G$$

且：

$$c_{i,j,k+1}^T<c_{i,j,k}^T.$$

此时：

$$S_k\to S_{k+1}.$$

向下搜索采用同样逻辑。

因此搜索实际发生在：

2.5D Slice 1
      │
   Gateway
      ▼
2.5D Slice 2
      │
   Gateway
      ▼
2.5D Slice 3

而不是：

Dense 3D Voxel Grid

这就是论文所谓：

searching through multiple tomogram slices

的本质。

---

8.4 A* 代价

从节点 $n_i$ 移动到 $n_j$，边代价由：

$$\text{Euclidean distance}+\text{target node travel cost}$$

共同构成。

可以写成：

$$g(n_j)=g(n_i)+{\Vert {\mathbf{p}}_j-{\mathbf{p}}_i\Vert }_2+c^N(n_j).$$

当：

$$c^N=c^B,$$

连接直接断开。

heuristic 使用 queried node 到 goal 的 diagonal distance。

因此 PCT 的 A* 不只是最短距离搜索，而是：

$$\text{距离}+\text{地形风险}+\text{顶棚净空}+\text{机身高度调整成本}.$$

---

9. 为什么它仍然能得到三维路径

虽然搜索主要发生在多张 2.5D 图上，但每个节点的真实三维位置为：

$${\mathbf{p}}_{i,j,k}=\left[\begin{array}{c}{x}_i\\ y_j\\ e_{i,j,k}^G\end{array}\right].$$

因此搜索结果自然包含：

$$x,y,z.$$

不同 slice 只是用于表达不同的空间层和不同的 ground-ceiling 关系，并不意味着最终路径被限制在二维平面。

所以：

PCT 是一种“三维问题的分层 2.5D 表达”，而不是二维规划器。

---

10. 三维轨迹优化

A* 输出的是离散路径，不能直接作为机器人最终执行轨迹。

论文进一步使用多项式优化生成平滑三维轨迹。

Fig. 5　Factory、Building、Forest 和 Overpass 四个仿真场景中的模型、点云、Travel Cost Map 与规划轨迹

---

10.1 五阶分段多项式

轨迹由 $M$ 段组成，第 $i$ 段：

$${\mathbf{q}}_i(t)={\mathbf{\sigma }}_i^T\mathbf{\beta }(t),t\in [0,T_i].$$

论文采用：

$$N=5.$$

自然基：

$$\mathbf{\beta }(t)={\left[\begin{array}{ccccc}1& t& t^2& \cdots & t^N\end{array}\right]}^T.$$

---

10.2 优化目标

论文优化问题为：

$$\underset{\mathbf{\sigma },\mathbf{T}}{\mathrm{minimize}}{J}_c+w_z{\Vert q_z(t)-Z_{\mathrm{ref}}\left(\mathbf{q}(t)\right)\Vert }_2+w_{T}T.$$

其中：

• $J_c$：jerk control effort；
• $Z_{\mathrm{ref}}$：机器人期望工作高度对应的参考 $z$；
• $w_z$：高度偏离权重；
• $T$：总轨迹时间；
• $w_T$：时间权重。

参考高度：

$$Z_{\mathrm{ref}}\left(\mathbf{q}(t)\right)=e^G\left(\mathbf{q}(t)\right)+d_{\mathrm{ref}}.$$

---

10.3 起终点约束

$${\mathbf{q}}_1(0)={\bar{\mathbf{q}}}_0,$$

$${\mathbf{q}}_M(T)={\bar{\mathbf{q}}}_f.$$

---

10.4 分段连续性

相邻轨迹段满足：

$${\mathbf{q}}_i^{[3]}(T_i)={\mathbf{q}}_{i+1}^{[3]}(0),i=1,\dots ,M-1.$$

这里原文采用 ${\mathbf{q}}^{[3]}$ 表示直到三阶导数相关的连续状态，因此轨迹能够保持位置、速度、加速度及 jerk 层面的平滑衔接。

---

10.5 安全约束

$$C\left(\mathbf{q}(t)\right)\ge C_{\mathrm{safe}},t\in [0,T].$$

这里 $C_{\mathrm{safe}}$ 表示轨迹安全裕度相关约束。

---

10.6 运动学约束

$$\mathcal{G}\left(\mathbf{q}(t),\dots ,{\mathbf{q}}^{(2)}(t)\right)\le 0.$$

其中包含：

• 最大速度；
• 最大加速度；
• 航向变化率。

所以最终结果不是只有几何 path，而是包含时间和速度信息的 trajectory。

---

10.7 主动高度调整

对于能够主动改变机身高度的机器人：

$${\mathcal{H}}_g\left(\mathbf{q}(t)\right){\le }_z(t)\le {\mathcal{H}}_c\left(\mathbf{q}(t)\right).$$

下界：

$${\mathcal{H}}_g\left(\mathbf{q}(t)\right)=e^G\left(\mathbf{q}(t)\right)+d_{\min }.$$

上界 ${\mathcal{H}}_c$ 来自经过 inflation 后查询得到的 ceiling elevation。

因此机器人通过低矮拱门时可以主动降低机身高度。

---

11. 整体算法流程

可以把 PCT Planner 总结为以下步骤。

Step 1：输入全局点云

P={pu}. P =\{ \mathbf{p}_u \}. P={pu​}.

Step 2：沿高度方向建立水平切片

$$z_k=z_{\min }+k{d}_s.$$

Step 3：生成每个 slice 的 ground / ceiling map

Sk={ekG,ekC}. S_k =\{ e_k^G,e_k^C \}. Sk​={ekG​,ekC​}.

Step 4：评估 ground traversability

计算：

$$g^x,g^y,m^{xy},m^{\mathrm{grad}}.$$

Step 5：评估 ceiling clearance

$$d^I=e^C-e^G.$$

Step 6：生成 initial cost

$$c^{\mathrm{init}}=\min \left(c^B,c^I+c^G\right).$$

Step 7：Inflation

生成：

$$c_k^T.$$

Step 8：Tomogram Simplification

删除没有 unique grid 的 slice。

Step 9：跨 Slice A*

通过 gateway：

$$S_k\leftrightarrow S_{k\pm 1}.$$

Step 10：得到离散三维 path

Step 11：五阶多项式优化

加入：

• jerk；
• 时间；
• traversability；
• 速度/加速度；
• ground；
• ceiling；

得到最终 trajectory。

---

12. 实验设计

12.1 仿真场景

论文使用四个主要场景。

场景	尺寸	主要特征
Factory	$84\times 68\times 12\text{m}$	城市结构、坡道、悬空物
Building	$22\times 20\times 16\text{m}$	多楼层、楼梯、不同坡度
Forest	$40\times 40\times 7\text{m}$	不规则地形、隧道、细障碍
Overpass	$155\times 95\times 30\text{m}$	大尺度多层螺旋结构

地图分辨率：

Factory / Overpass：

$$r=0.2\text{m}.$$

Building / Forest：

$$r=0.1\text{m}.$$

Factory、Forest 和 Overpass 的轨迹进一步在 CoppeliaSim 中使用 Pioneer 3-DX 验证。

---

12.2 轨迹质量实验

另外设置：

Plaza

$$56\times 56\times 5\text{m}$$

城市复杂结构。

Hills

$$60\times 60\times 3\text{m}$$

不规则障碍和崎岖地形。

每个场景从地图中心出发，在距离起点：

$$26.5\text{m}$$

处均匀采样：

$$50$$

个 goal。

Fig. 6　Plaza 与 Hills 场景及轨迹质量评估示例

**原文核对说明：**Fig. 6 原始图注最后写成“(b1), (b2): example trajectories generated by our approach”。从图面内容可以看出轨迹分别位于 (a2) 和 (b2)，原文很可能存在一个子图标注笔误。本文保留 Fig. 6 的原始编号，不自行修改论文子图编号。

---

13. 实机实验

论文使用 Jueying Mini 四足机器人。

两个实机场景为：

Stairs

$$14\times 14\times 7\text{m}.$$

使用：

Livox Mid-70

扫描。

机器人需要从底层沿绕行楼梯到达上层。

Boxes

$$22\times 22\times 4\text{m}.$$

机器人搭载：

RS-Helios-32

场景包含：

• 狭窄拱门；
• 楼梯；
• 坡道；
• 上层平台。

点云地图使用：

A-LOAM

构建，作者在实验中人工删除噪声点和动态目标。

---

14. 参数设置

Table I　Algorithm Parameters

项目	记号	数值
Height Adaptation	$[d_s,d_{\min },d_{\mathrm{ref}}]$	$[0.50,0.50,0.65]$
Thresholds	$[{\theta }_b,{\theta }_s,{\theta }_p]$	$[1.70,0.36,0.20]$
Costs and Scaling Factors	$[c^B,{\alpha }_d,{\alpha }_b,{\alpha }_s]$	$[50,20,20,15]$

对于轮式机器人：

$${\theta }_p=1.0$$

即可禁用对楼梯/台阶的跨越式通行判断。

---

15. 对比算法

论文选择了四种具有代表性的环境表达方式。

15.1 Yang：Elevation Map

使用传统 elevation map 在 GPU 上评估地形，并通过 A* 导航。

优点：

• 高效；
• 能考虑四足运动能力；
• 能规划楼梯。

缺点：

• 一次只能表示一个主要地面层；
• 无法完成真正的全局 multi-layer planning。

Fig. 7　传统单层 Elevation Map 与 PCT 多层三维规划能力对比

---

15.2 Liu：Point Cloud

直接在 point cloud 上使用 GPU tensor voting 计算环境几何属性。

论文为了提高对比公平性，将其原始 path search 替换成 A*。

优点：

• 不需要显式重建 surface；
• 支持三维结构。

缺点：

• 点云数据无序；
• scene evaluation 仍然非常耗时；
• 缺少 robot motion capability awareness。

---

15.3 Pütz：Mesh

使用 Ball-Pivoting 从点云生成 mesh，再生成 layered navigation mesh。

优点：

• 三维表面表示较自然；
• 具有运动能力相关的 traversability。

缺点：

• mesh 构建和 scene evaluation 较慢；
• 论文测试方法不能处理四足楼梯；
• 不输出带速度信息的 trajectory。

---

15.4 Wang：3D2M Planner / Voxel + ESDF

对应：

https://github.com/ZJU-FAST-Lab/3D2M-planner

流程为：

Point Cloud
   ↓
Valid Ground
   ↓
3D Voxel
   ↓
ESDF
   ↓
A*
   ↓
Trajectory Optimization

这是与 PCT 最直接的对比对象。

主要问题是搜索仍发生在稠密 3D grid 中。

---

16. 导航能力对比

Table II　Scene Evaluation and Trajectory Generation Capabilities

方法	3D 多层规划	机身高度调整	楼梯规划	运动能力感知	带速度轨迹
Yang	✗	✗	✓	✓	✗
Liu	✓	✗	✓	✗	✗
Pütz	✓	✗	✗	✓	✗
Wang	✓	✓	✗	✓	✓
PCT / Ours	✓	✓	✓	✓	✓

这张表实际上非常直接地体现了论文的目标：

PCT 希望同时保留 elevation map 的 terrain-awareness 和 voxel map 的 multi-layer capability。

---

17. 计算效率评价指标

论文统计：

• $T_p$：地图预处理 / representation construction；
• Size：地图内存占用；
• $T_e$：scene evaluation；
• $T_s$：initial path search；
• $N_s$：搜索访问节点数量；
• $T_o$：trajectory optimization；
• $T_{\mathrm{all}}$：完整导航问题总耗时。

硬件：

Intel i9-12900KF @ 3.2 GHz
NVIDIA RTX 3080 Ti

同时在：

NVIDIA Jetson AGX Orin, MAXN

上测试。

---

18. 主要效率结果

由于原 Table III 数据量很大，下面按照原表给出关键场景中的完整 PCT 行，并保留主要 baseline 作为对照。时间单位均按论文原表，为 ms。

18.1 Factory

方法	$T_p$	Size	$T_e$	$T_s$	$N_s$	$T_o$	$T_{\mathrm{all}}$
Liu	-	12.31	$178.28\times 10^3$	189.50	135290	-	$178.75\times 10^3$
Pütz	$3.44\times 10^3$	28.84	$13.36\times 10^3$	100.94	80333	-	$16.91\times 10^3$
Wang	$2.22\times 10^3$	34.27	$24.54\times 10^3$	$1.37\times 10^3$	3892807	953.46	$29.08\times 10^3$
PCT CPU	630.76	4.57	785.68	25.66	133648	475.64	$1.92\times 10^3$
PCT GPU	2.39	4.57	2.41	25.66	133648	475.64	542.55
PCT Orin	20.06	4.57	12.53	75.56	133648	$1.36\times 10^3$	$1.56\times 10^3$

PCT 在 RTX 3080 Ti 上 scene evaluation：

$$T_e=2.41\text{ms}.$$

而 Wang：

$$24.54\times 10^3\text{ms}.$$

差距约达到 4 个数量级量级；综合各种场景后论文给出的结论是 scene evaluation 整体实现约 3 orders of magnitude 的提升。

---

18.2 Building

方法	Size	$T_e$	$T_s$	$N_s$	$T_o$	$T_{\mathrm{all}}$
Liu	7.40	$116.47\times 10^3$	765.54	208973	-	$117.51\times 10^3$
Pütz	17.10	$11.36\times 10^3$	Fail	-	-	Fail
Wang	28.16	$11.53\times 10^3$	Fail	-	-	Fail
PCT CPU	3.17	234.95	7.33	39760	359.39	892.65
PCT GPU	3.17	3.42	7.33	39760	359.39	398.14
PCT Orin	3.17	17.69	17.89	39760	989.66	$1.11\times 10^3$

这里最关键的并不是速度，而是：

Pütz 与 Wang 无法正确识别可通行楼梯，因此直接规划失败。

---

18.3 Forest

方法	Size	$T_e$	$T_s$	$N_s$	$T_o$	$T_{\mathrm{all}}$
Liu	6.94	$93.86\times 10^3$	66.24	29234	-	$94.18\times 10^3$
Pütz	17.94	$12.88\times 10^3$	68.27	57162	-	$17.47\times 10^3$
Wang	44.80	$24.67\times 10^3$	917.61	3014581	42.95	$27.42\times 10^3$
PCT GPU	6.40	3.02	19.56	85450	65.43	109.99
PCT Orin	6.40	19.88	60.50	85450	175.75	317.40

---

18.4 Overpass

大型场景尺寸：

$$155\times 95\times 30\text{m}.$$

Wang 的稠密 voxel A* 访问：

$$55489651$$

个节点。

PCT 只访问：

$$224651$$

个节点。

两者约相差：

$$\frac{55489651}{224651}\approx 247.$$

这非常直观地展示了 tomogram simplification 对搜索空间的压缩。

PCT GPU 总耗时：

$$3.17\times 10^3\text{ms},$$

而 Wang：

$$127.26\times 10^3\text{ms}.$$

---

19. 路径结果对比

Fig. 8　Factory 与 Forest 中不同方法的路径结果：绿色 PCT、蓝色 Liu、黄色 Pütz、红色 Wang

论文指出：

Liu

没有 robot shape / motion capability awareness。

结果包括：

• Factory 中没有充分考虑机器人尺寸；
• Forest 中由于过度偏向 geodesic direction，轨迹与柱状障碍冲突。

Pütz

路径可行，但没有速度信息。

Wang

可以生成平滑 trajectory，但 voxel 对细节地形表达较粗。

Factory 中红色轨迹在坡道区域出现从坡侧提前下降的行为，几何上虽然可能满足 voxel free-space，却不符合合理地面运动逻辑。

PCT

依靠连续 ground elevation 和 travel cost 能够更明确地理解：

“机器人应该沿地面表面怎样走”，

而不是简单判断：

“三维空间中哪部分没有被占据”。

---

20. 轨迹质量

Table IV　Trajectory Generation Performance

Plaza

方法	$T_s$ / ms	$T_o$ / ms	$L_t$ / m	$C_t$	$S_t$
Liu	$358.17\pm 25.78$	-	$28.91\pm 0.71$	$0.353\pm 0.645$	0.12
Pütz	$44.16\pm 4.43$	-	$30.69\pm 1.53$	$0.164\pm 0.018$	0.92
Wang	$1.19\times 10^3\pm 335.25$	$33.83\pm 19.19$	$47.13\pm 9.59$	$0.037\pm 0.010$	0.94
PCT	$13.93\pm 3.93$	$52.57\pm 25.99$	$28.32\pm 1.55$	$0.018\pm 0.007$	1.00

Hills

方法	$T_s$ / ms	$T_o$ / ms	$L_t$ / m	$C_t$	$S_t$
Liu	$219.31\pm 9.19$	-	$29.65\pm 0.57$	$0.149\pm 0.015$	0.28
Pütz	$62.42\pm 3.36$	-	$28.79\pm 0.30$	$0.130\pm 0.031$	0.80
Wang	$1.05\times 10^3\pm 528.27$	$33.58\pm 30.33$	$42.53\pm 13.73$	$0.011\pm 0.010$	0.86
PCT	$14.23\pm 4.39$	$32.89\pm 22.87$	$27.79\pm 0.44$	$0.011\pm 0.006$	0.92

其中：

• $L_t$：平均轨迹长度；
• $C_t$：平均轨迹曲率；
• $S_t$：50 次目标采样中找到 feasible trajectory 的成功率。

值得注意的是：

PCT 的优势不只是运行时间。

在 Plaza：

$$S_t=\mathrm{1.00.}$$

同时平均轨迹长度和曲率都较小。

---

21. 实机结果

Fig. 9　Stairs 与 Boxes 两个实机场景中的四足机器人、点云地图、Travel Cost Map 和规划结果

论文验证了两个比较关键的能力。

21.1 多层楼梯

在 Stairs 场景中，机器人：

Ground Floor
    ↓
Winding Stairs
    ↓
Upper Floor

通过不同 tomogram slice 之间的 gateway 完成多层路径连接。

这正是传统单层 elevation map 无法完成的情况。

---

21.2 狭窄拱门下主动降低机身

Boxes 中存在低矮顶部空间。

由于：

$$d_{\min }\le d^I<d_{\mathrm{ref}},$$

该区域不会被直接判为 barrier，而是产生 body-height-adjustment cost。

机器人最终能够：

1. 自动降低机身；
2. 穿过狭窄 arch；
3. 通过后恢复正常高度。

---

21.3 在楼梯与缓坡之间选择缓坡

Boxes 同时存在：

• steep stairs；
• gentle slope。

两条路线都可能几何可行，但陡楼梯 travel cost 更高。

因此规划器选择缓坡。

这里体现的是：

$$\text{feasible}\ne \text{optimal}.$$

PCT 不只是寻找“能够到达”的路线，还通过 $c^T$ 选择更符合机器人运动能力的路线。

---

22. 为什么 PCT 比 3D Voxel 快

PCT 的效率来自三个层次。

22.1 表达层降维

Voxel：

$$x-y-z$$

全体空间都需要表示。

PCT：

$$\text{several 2.5D surfaces}.$$

地面机器人实际上只需要关心潜在接触面及其上方净空，因此没有必要搜索所有自由空间体素。

---

22.2 Slice Simplification

从：

$$46$$

层减少到：

$$5$$

层的 spiral 案例说明，固定高度采样产生的大量 slice 并没有新增搜索信息。

PCT 通过 unique grid 判断将其删除。

---

22.3 GPU 并行

Tomogram construction 和 scene evaluation 天然适合栅格并行。

论文使用 GPU 加速后，scene evaluation 在部分场景从：

$$10^4\sim 10^5\text{ms}$$

降低到：

$$10^0\sim 10^1\text{ms}.$$

---

23. PCT 与传统高程图真正的区别

容易产生一个误解：

PCT 是不是简单维护了多张 elevation map？

从数据结构上看确实接近，但核心区别在于它并不是简单的 multi-map stack。

每一张 slice 都包含：

$$\boxed{\text{Ground}+\text{Ceiling}+\text{Travel Cost}}$$

同时 slice 之间具有：

$$\text{Gateway Connectivity}.$$

所以整体上形成一个隐式三维可通行图。

传统 elevation map：

$$(x,y)\to z.$$

PCT：

$$(x,y,k)\to \left(e^G,e^C,c^T\right).$$

而 $k$ 不是普通 voxel 的高度索引，而是经过简化后具有实际导航意义的空间层索引。

---

24. PCT 为什么适合四足机器人

PCT 并没有做：

• foothold planning；
• contact sequence planning；
• MPC；
• WBC；
• joint torque control。

它依然属于：

body-level global navigation planner

但它比普通轮式导航地图更适合四足的原因在于，地图构建阶段已经考虑：

1. 台阶跨越能力

通过：

$${\theta }_p$$

区分 wheeled / legged traversability。

2. 机身高度变化

通过：

$$[d_{\min },d_{\mathrm{ref}}]$$

表达 body height adjustment。

3. 连续地面

通过：

$$e^G$$

避免三维自由空间路径脱离真实地面。

4. Ceiling

通过：

$$e^C$$

判断低矮通道。

因此它所输出的 global trajectory 已经具备一定程度的：

locomotion capability awareness

但最终是否真的能踩稳台阶，仍然要由下游 locomotion controller 保证。

---

25. 与 FAR Planner 的区别

两者都可以作为四足机器人的全局规划器，但思想差异很大。

对比	PCT Planner	FAR Planner
环境表达	Tomogram / 2.5D Grid	Visibility Graph
是否保留地面连续高程	是	主要依赖可见图节点
多层表达	Slice + Gateway	3D Visibility Graph
楼梯运动能力显式建模	是	不以此为核心
Ceiling / Height Adaptation	是	非核心
输入	全局 Point Cloud Map	在线局部 Point Cloud / Terrain
典型定位	地形感知全局规划	在线未知环境长距离路由
主要优势	多层 + 地形 + 高度约束	在线图增长和未知空间探索

如果目标是：

已有较完整点云地图，希望在楼梯、多层建筑、坡道中做机器人能力感知的全局规划

PCT 非常合适。

如果目标是：

完全未知环境中边走边建拓扑图，持续向远距离目标推进

FAR 的在线 visibility graph 更自然。

---

26. PCT 的主要创新点

26.1 Point Cloud Tomography 表示

核心贡献不是新的搜索算法，而是：

$$\text{3D Point Cloud}\to \text{Multi-layer 2.5D Tomogram}$$

这种表示把三维结构表达与地面机器人导航需求进行了匹配。

---

26.2 Ground + Ceiling 双层几何描述

传统 elevation map 只关心 ground。

PCT 同时引入：

$$e^G,e^C,$$

因此能够表达：

• overhang；
• bridge；
• tunnel；
• multi-floor；
• narrow arch。

---

26.3 Motion-capability-aware Traversability

cost 不是纯几何 cost，而是和机器人有关：

$$c^T=f\left(\text{terrain},\text{clearance},\text{robot capability}\right).$$

同一张地图可以通过参数切换适配：

• wheel；
• quadruped。

---

26.4 Tomogram Simplification

不是保留所有固定高度切片，而是通过：

$$M_k\subset M_{k-1}\cup M_{k+1}$$

主动消除冗余层。

这是降低内存和 path search burden 的关键。

---

26.5 Gateway 跨层 A*

没有构建稠密 3D graph，而是：

$$2.5D{A}^{\ast }+Gateway.$$

以较低搜索开销完成 multi-layer 3D navigation。

---

26.6 地面运动与机身高度运动解耦

平面路径主要在 tomogram ground 上搜索。

机身 $z$ 方向则在轨迹优化中进一步根据：

$$e^G,e^C$$

调整。

这避免把所有 body-height 状态直接加入高维搜索。

---

26.7 完整轨迹而不是只有 Path

最终输出不仅包含位置路径，还考虑：

• velocity；
• acceleration；
• heading-rate；
• jerk；
• trajectory time。

因此更容易直接连接下游 tracking controller。

---

27. 学术贡献与工程价值

论文真正有价值的地方可以归纳为一句话：

它不是试图让一个三维搜索算法变得更快，而是重新设计环境表示，让大量三维搜索本身不再必要。

这种思路属于典型的：

Representation-driven Planning

很多规划问题的计算瓶颈，并不一定应该通过更复杂的 search algorithm 解决。

如果地图表达本身能够更贴合机器人运动空间，搜索问题的规模可能直接下降几个数量级。

这也是 PCT 相较单纯改进 A*、Dijkstra、RRT 的更重要意义。

---

28. 局限性

论文在 Discussion 中明确指出几个限制。

28.1 依赖较稠密、质量较好的点云

当前方法需要：

$$\text{dense and high-quality point cloud}.$$

如果点云：

• 很稀疏；
• 噪声严重；
• 含大量动态目标；

可能产生：

• sub-optimal trajectory；
• planning failure。

---

28.2 未观测区域存在风险

如果某处完全没有点云，就无法可靠获得：

$$e^G$$

和：

$$e^C.$$

简单插值可能在真正的坑、悬崖或遮挡区域中引入错误。

---

28.3 实验点云进行了人工预处理

实机场景的点云中，作者人工删除：

• noise points；
• dynamic objects。

这意味着当前版本并不是完整的：

raw lidar
→ online mapping
→ fully autonomous PCT planning

pipeline。

---

28.4 不是足端级可通行性模型

它知道“四足可以跨阶”，但并没有计算：

$$\text{foothold feasibility}.$$

对于极端楼梯、碎石、窄梁等环境，单纯 ground cost 不能替代真正的接触规划。

---

29. 论文后续可以怎样扩展

作者提出 tomogram 可以继续增加 semantic channels。

例如：

Sk={ekG, ekC, ckT, sk}. S_k =\{ e_k^G,\, e_k^C,\, c_k^T,\, s_k \}. Sk​={ekG​,ekC​,ckT​,sk​}.

其中：

$$s_k$$

可以表示语义信息。

进一步可以加入：

• grass；
• mud；
• water；
• stairs；
• glass；
• moving obstacle；
• terrain uncertainty。

对于四足机器人，还可以进一步加入：

$$c_{\mathrm{slip}},c_{\mathrm{foothold}},c_{\mathrm{energy}}.$$

从而把 PCT 扩展为真正的 locomotion-aware global map。

---

30. 开源项目 PCT_planner

官方代码：

https://github.com/byangw/PCT_planner

仓库把代码分成两个主要部分：

PCT_planner/
├── tomography/
│   └── 点云层析与 Tomogram 构建
│
└── planner/
    └── Path Search + Trajectory Optimization

---

30.1 官方依赖

README 要求：

Ubuntu >= 20.04
ROS >= Noetic
CUDA >= 11.7
Python >= 3.8
CuPy
Open3D

其中 CuPy 用于 GPU 计算。

---

30.2 编译 Planner

cd planner
./build_thirdparty.sh
./build.sh

---

30.3 官方示例

仓库提供：

• Spiral；
• Building；
• Plaza。

---

30.4 构建 Tomogram

先启动：

roscore

使用：

rsc/rviz/pct_ros.rviz

启动 RViz。

然后：

cd tomography/scripts
python3 tomography.py --scene Spiral

生成结果保存到：

rsc/tomogram/

同时以 ROS PointCloud2 可视化。

---

30.5 轨迹生成

配置 GTSAM 动态库：

export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/YOUR/DIRECTORY/TO/PCT_planner/planner/lib/3rdparty/gtsam-4.1.1/install/lib

运行：

cd planner/scripts
python3 plan.py --scene Spiral

输出 trajectory 以 ROS Path 显示在 RViz。

---

31. 对整篇论文的理解

PCT Planner 可以用下面这组关系理解：

$$\boxed{P\to \mathcal{S}\to c^T\to {\mathcal{S}}_{\mathrm{simple}}\to P_{{\mathrm{A}}^{\ast }}\to \mathbf{q}(t)}$$

分别对应：

$$\boxed{\text{Point Cloud}\to \text{Tomogram}\to \text{Traversability}\to \text{Slice Reduction}\to \text{3D Path}\to \text{Smooth Trajectory}}$$

PCT 最大的特点并不是“能规划楼梯”。

真正核心的是：

用多张带 ground-ceiling 关系的 2.5D 地图表达地面机器人的三维可通行空间。

楼梯、多楼层、桥下、低矮通道、四足运动能力感知以及更高的路径搜索效率，都是这一表示方式带来的结果。

---

32. 总结

Efficient Global Navigational Planning in 3-D Structures Based on Point Cloud Tomography 的主要贡献可以归结为：

$$\boxed{\text{用适合 Ground Robot 的数据表示， 替代对完整 3D 空间的暴力表达与搜索}}$$

传统 elevation map：

$$\text{快，但无法完整表达多层}.$$

传统 3D voxel：

$$\text{表达完整，但计算和搜索成本高}.$$

PCT 则选择：

$$\text{Multi-layer 2.5D Tomogram}.$$

因此同时获得：

• multi-layer 3D navigation；
• stair planning；
• terrain traversability；
• ceiling awareness；
• active body-height adaptation；
• motion capability awareness；
• smooth trajectory with velocity；
• 高效 GPU scene evaluation；
• 显著减少的搜索节点数。

对于四足机器人而言，PCT 很适合作为：

三维点云地图之上的全局地形感知规划器。

它输出的仍然是 body-level trajectory，后续可以继续交给：

• SCAN-Planner；
• MPC；
• WBC；
• locomotion controller；

完成局部避障和四足运动控制。

---

参考链接

• 论文 arXiv：
  https://arxiv.org/abs/2403.07631

• IEEE/ASME TMECH DOI：
  https://doi.org/10.1109/TMECH.2024.3396001

• PCT Planner：
  https://github.com/byangw/PCT_planner

• 作者项目主页：
  https://byangw.github.io/projects/tmech2024/

• 对比项目 3D2M Planner：
  https://github.com/ZJU-FAST-Lab/3D2M-planner

• A-LOAM：
  https://github.com/HKUST-Aerial-Robotics/A-LOAM