前言

• 最近我们解读了很多关于SLAM后端的源码:
  • 第一期：【3D SLAM源码解读系列】(一)Scan Context与Scan Context++：3D点云回环检测源码解析
  • 第二期：【3D SLAM源码解读系列】(二)Small_gicp——5 个积木搭出最优点云配准
  • 第三期：【3D SLAM源码解读系列】(三)SOLiD：面向有限 FOV 的轻量回环检测
  • 第四期：【3D SLAM源码解读系列】(四) GTSAM与iSAM2——从回环约束到位姿图优化
• 同时我们也解读过SLAM前端的源码
  • 前置知识：
    • 【现代控制理论与卡尔曼滤波】从状态空间到Python仿真实现
    • 【EKF、UKF与粒子滤波】非线性系统状态估计的三种经典方法与基本python实现
  • 第一期：【FAST-LIO2 源码解读】从 Mid-360 到 ESEKF — 公式与代码逐行对照
  • 第二期：【Point-LIO 源码解读】逐点更新取代逐帧更新 — 公式与代码逐行对照
  • 第三期上：【FAST-LIVO2 源码解读（上篇）】从 LIO 到 LIVO：直接法视觉如何补全激光里程计的最后一块拼图
  • 第三期下：【FAST-LIVO2 源码解读（下篇）】从 LIO 到 LIVO：直接法视觉如何补全激光里程计的最后一块拼图
  • 第四期：【Super-LIO 源码解读】紧凑体素地图 + 启发式近邻，换掉 ikd-Tree — 公式与代码逐行对照
• 同时我们也解读过类似的规划算法
  • 【10天速通ROS2-PX4无人机】(三) EGO-Planner 自主避障飞行，飞机终于会自己找路了
  • 【FUEL 源码解读】未知环境自主探索 — 从 Frontier 到 B-spline 的公式与代码逐行对照
  • 【基于OctoMap与地面约束的三维A*路径规划方法实现】26邻域查找与八叉树离散化映射实现
• 本期我们换一个胃口，来解读一个全局规划算法 PCT_planner。
• 它的全称是 Point Cloud Tomography，直译过来就是「点云断层扫描」——一个借用了医学 CT 影像概念，用一组等间距的水平平面把三维点云「切片」成一层层 2.5 维地图，再在多层地图上做 A* 搜索与轨迹优化的全局路径规划框架。
• 仓库地址：https://github.com/byangw/PCT_planner
• 论文：Efficient Global Navigational Planning in 3-D Structures Based on Point Cloud Tomography（TMECH 2024，港科大）。
• 老规矩，我们仍然是「先讲直觉、再上公式、最后逐行对代码」，把这篇论文里最硬核的两块——点云断层扫描（Tomogram） 与 GPMP2 轨迹优化——掰开揉碎讲清楚。
  

---

---

1 传统的3维路径规划

1-1 全局路径规划器

• 在开始之前，我们先花一小节，把「路径规划 / 全局规划 / 局部规划」这三个被很多人混用的词说清楚，免得后面的讨论失去共同语境。
• 所谓路径规划（Path Planning），就是给定一个环境、一个起点、一个终点，求一条从起点到终点、同时满足某些约束（无碰撞、可通行、代价最小、平滑等）的轨迹。
• 按「信息范围」和「时间尺度」的不同，路径规划一般分成两个层次：

层次	全局路径规划（Global Planner）	局部路径规划（Local Planner）
输入	完整的、先验的地图（静态）	传感器实时感知的局部信息（动态）
输出	一条从起点到终点的「粗略」全局路径	沿着全局路径前进时的实时控制量（速度、角速度）
时间尺度	秒级到分钟级，规划一次用较久	毫秒级，高频重规划
典型代表	A*、Dijkstra、RRT、PCT	DWA、TEB、EGO-Planner（局部避障）

• 本文解读的 PCT_planner 属于全局路径规划器。
• 它的前提假设是：我们已经有一幅先验的、静态的三维点云地图（比如在建图阶段用 LIO 采集并累积出来的），规划器在这幅地图上离线地找出一条全局可通行路径。

说人话：全局规划器是「拿着整张地图，先算一条从 A 到 B 的大致路线」；局部规划器是「边走边看，实时决定下一脚油门和方向盘」。PCT 干的是前者——它默认我们已经建好图了。

1-2 常见算法一览

• 三维空间里的全局路径规划，主流做法可以大致分成「基于图搜索」和「基于采样」两大类，再加一类「基于势场/优化」的方法：

类别	代表算法	核心思想	优点	缺点
图搜索	A*、Dijkstra、D*	在离散化的栅格/体素图上做最优搜索	能保证最优（给定启发式一致时）	高维时状态爆炸，内存吃紧
采样	RRT、RRT*、PRM	随机采样 + 碰撞检测连边	高维空间也能用	概率完备而非最优，路径抖动
势场/优化	CHOMP、GPMP2、EGO	把路径当成连续变量去优化代价	平滑、可加动力学约束	依赖好的初值，可能陷入局部极值

• 而要把这些算法跑起来，关键在于用什么数据结构来表达三维环境。常见的有：
  • 体素栅格（Voxel Grid）：把空间切成一个个小立方体，标记占用/空闲。最通用，但 3D 体素数量是 2D 栅格的「分辨率」倍，内存随分辨率三次方爆炸。
  • 八叉树（OctoMap）：用树状结构对体素做自适应压缩，空的地方用大块表示。缓解了内存问题，但查询和遍历复杂了。
  • ESDF（欧氏距离场）：每个体素存「到最近障碍的距离」。对优化类方法很友好，但构建和动态更新很贵。
  • 高程图（2.5D Elevation Map）：只存「每根竖直柱子上最高的那个点」。内存小、直观，但它天然只能表达一层表面，表达不了悬挑、桥洞、多层楼板。
• 记忆点：这些经典表示，要么内存爆炸（体素），要么表达力不足（高程图只能一层），要么构建太贵（ESDF）。这正是 PCT 要解决的问题的入口。

1-3 传统方法面对的难题

• 当我们把场景从「一片开阔地」换成「多层建筑、楼梯、坡道、上下层结构」时，传统三维路径规划会遇到几个非常具体的痛点：

• 痛点一：内存爆炸。

  • 体素栅格要把整个空间都离散化。一个 100 m × 100 m × 10 m 的楼，按 0.1 m 分辨率切，就是 $10^3\times 10^3\times 10^2=10^8$ 个体素，每个还要存占用、存距离场，内存和 A* 的搜索空间都吃不消。

• 痛点二：高程图表达不了「多层」。

  • 2.5D 高程图每根柱子只能记一个高度。而一座楼里，同一根竖直柱子上可能「下面是 1 楼地面、中间是 2 楼天花板、再上面是 2 楼地面」，一层高程图根本记不住这种「楼中楼」。

• 痛点三：天花板/悬挑信息丢失。

  • 地面机器人虽然贴着地面走，但头顶上的天花板高度（净空高度） 直接决定它能不能钻过去。经典高程图只关心「脚下」，完全不管「头顶」，于是机器人会在一个净空不足的洞口里硬钻。

• 痛点四：楼梯/台阶这类「可跨越障碍」需要专门建模。

  • 对轮式机器人，一个 30 cm 的台阶是不可跨越的障碍；对足式机器人，它可能就是一个可以「踩上去」的台阶。传统二值占用栅格把台阶和墙一视同仁，丢掉了「可跨越性」这个重要信息。

• 上面这四个痛点，正好对应了 PCT_planner 的四个设计动机：

  • 用「切片」把 3D 问题降维成「多层 2.5D」，省内存；
  • 每一层分别存「地面层 + 天花板层」，表达多层与净空；
  • 用「可通行代价」而非「二值占用」来刻画地面，区分可跨越台阶与硬障碍。

说人话：传统方法要么「整层楼当一个大方块」太笨重，要么「只记一层地面」太天真。PCT 想的是——能不能像 CT 拍片子一样，把整栋楼沿高度切成几十张「平面切片」，每张切片上同时记「脚下有多高」和「头顶有多高」，然后让机器人一层一层地爬？

---

2 PCT_planner

2-1 介绍

• PCT_planner 是一个面向 3D 复杂结构（多层建筑、楼梯、坡道）的高效全局导航规划框架，论文发表于 TMECH 2024（IEEE/ASME Transactions on Mechatronics），作者来自香港科技大学，仓库地址 https://github.com/byangw/PCT_planner。
• 论文标题：Efficient Global Navigational Planning in 3-D Structures Based on Point Cloud Tomography，直译就是「基于点云断层扫描的三维结构高效全局导航规划」。
• 论文给出的实验结论很亮眼（这里先给结论，后面讲完原理我们自然就明白为什么）：
  • 场景评估（建 Tomogram）比传统 3D 方法快约三个数量级；
  • 路径规划（A* 搜索）快约 3 倍；
  • 整体导航时间快约两个数量级；
  • 而且规划的轨迹更平滑、净空更安全、且天然避开了头顶天花板。
• 整个框架可以分成离线（offline） 和在线（online） 两大阶段：

阶段	模块	作用	对应代码
离线	点云断层扫描（Tomography）	把 3D 点云切成多层 2.5D Tomogram	tomography/（CUDA 核函数）
离线	可通行性估计（Traversability）	把高程/净空/坡度算成代价图	travKernel、inflationKernel
离线	层简化（Simplification）	把几十层压成少数几层	tomogram.py 的 idx_simp
在线	多层 A* 搜索	在多层代价图上搜出一条路径	a_star_search.cc
在线	GPMP2 轨迹优化	把折线路径优化成平滑轨迹	gpmp_optimizer.cc

说人话：离线阶段像个「扫描 + 预处理」，把一栋楼的点云先变成几十张「带代价的平面地图」；在线阶段才真正做「找路 + 顺滑」。因为重活都在离线干完了，在线部分又快又省。

• 下面我们顺着这个表格，一个模块一个模块地讲，先讲清楚「为什么要这么设计」，再上公式，最后逐行对代码。

2-2 核心思想：像 CT 一样「切片」

• 论文的核心创新，一句话概括就是：用一个「水平切片」的操作，把复杂的三维结构，转换成一组二维的、可以单独做 A 的「断层图（Tomogram）」*。

• 具体做法是这样（对应论文的 Algorithm 1，也是 tomography/scripts/kernels.py 里的 CUDA 核函数）：

  • 第一步，切平面。 沿竖直的 $z$ 轴从最低高度 $z_{min}$ 开始，每隔一个固定的间隔 $d_s$（代码里的 slice_dh）放一个水平切片平面。第 $k$ 个切片平面的高度就是 $z_{min}+k\cdot d_s$。
  • 第二步，投影。 对每一个切片平面 $k$，把整个点云里落在该平面「下方」的点，竖直向上投影到平面上，取其中最高的那个点，作为这一层的「地面高程」$e^G$；把落在该平面「上方」的点，竖直向下投影到平面上，取其中最低的那个点，作为这一层的「天花板高程」$e^C$。
  • 第三步，得到一张断层图。 于是第 $k$ 层切片 $S_k$ 上，每个栅格 $(i,j)$ 都记录了三样东西：
    • $e_{i,j,k}^G$：地面高程（脚下有多高）；
    • $e_{i,j,k}^C$：天花板高程（头顶有多高）；
    • $c_{i,j,k}^T$：可通行代价（这个地方好走不好走）。

• 先用一张 3D 视角来看「水平切片」这件事：左图用几个水平半透明平面去切一栋多层点云（一楼地面 + 斜坡楼梯 + 二楼地面 + 天花板）；右图取其中 $z=1.5$ 这一层，蓝色是平面下方的点（构成地面 $e^G$），橙色是平面上方的点（构成天花板 $e^C$）：

• 再换到侧视图，把「切平面 → 投影 → 断层图」这三步看得更细：左图是切平面，中图是投影得到 $e^G$/$e^C$，右图是该层沿 $x$ 的地面/天花板曲线，绿色阴影即净空 $d^I$：

说人话：想象我们拿一把水平刀，从楼底到楼顶每隔半米切一刀。每切一刀，我们就趴在地上看这一刀的横截面——「脚下踩的是什么」「头顶顶的是什么」。每一刀截面就是一层 Tomogram。把几十刀叠起来，就是整栋楼的「断层扫描图」。

• 这套设计妙在降维：原本三维空间的搜索，被拆成了「每一层内做 2D 的 A*」+「层与层之间做 gateway 切换」。内存从「分辨率的三次方」降到了「分辨率平方 × 层数」。

2-3 整体数据流：Python 离线建图 → pickle → C++ 在线搜索

• 先给一张「全景图」，否则后面 2-4～2-6 和第 3 章一堆代码容易看丢主线。整个 Tomogram 的代码分成离线建图和在线搜索两段：

  • 离线建图（2-4～2-6）：tomogram.py 里的 Tomogram.point2map() 把点云算成多层地图（切片 → 代价 → 膨胀 → 层简化），最终产物是一个 .pickle 文件，由 ROS 节点 tomography.py 落盘到 rsc/tomogram/<场景名>.pickle。
  • 在线搜索（3-1）：planner_wrapper.py 读回这个 pickle，重建出 C++ 的多层地图，才正式开始 A*。

• 这个 pickle 里装的是一张 5 通道的多层地图（data 字段，形状 (5, 层数, 宽, 高)，float16；「层数」已经是层简化之后的层数，比如 5 层而不是 46 层）。5 个通道及各自的「消费者」对应如下：

通道	内容	消费者
layers_t	膨胀后的可通行代价	A* 的代价图 g（cost 直接取它）
trav_gx	代价沿 $x$ 的梯度	GPMP2 障碍因子的雅可比
trav_gy	代价沿 $y$ 的梯度	GPMP2 障碍因子的雅可比
layers_g	地面高程 $e^G$（无效栅格 = NaN）	gateway（层间对齐）+ 轨迹落点高度
layers_c	天花板高程 $e^C$（无效栅格 = NaN）	gateway（算净空 $e^C-e^G$）

• 除了这 5 通道，pickle 还附带 4 个元数据：resolution（栅格分辨率）、center（地图中心 $x,y$）、slice_h0（首层切片高度）、slice_dh（切片间距）——C++ 侧要靠它们把「栅格索引」换算回「世界坐标」。

• 导出与读回的两段代码，正好首尾相接：

# tomography/scripts/tomography.py  exportTomogram() （官方源码，已加中文注释）
# 注意：参数 tomogram 在调用处 process() 里是
#   np.stack((layers_t, trav_grad_x, trav_grad_y, layers_g, layers_c))
# 即「可通行代价 + x/y 代价梯度 + 地面高程 + 天花板高程」5 个通道。
def exportTomogram(self, tomogram, map_file):
    data_dict = {
        'data': tomogram.astype(np.float16),   # 5 通道多层地图，半精度省内存
        'resolution': self.resolution,          # 栅格分辨率
        'center': self.center,                  # 地图中心 (x, y)
        'slice_h0': self.slice_h0,              # 首层切片高度
        'slice_dh': self.slice_dh,              # 切片间距
    }
    file_name = map_file + '.pickle'
    with open(self.export_dir + file_name, 'wb') as handle:
        pickle.dump(data_dict, handle, protocol=pickle.HIGHEST_PROTOCOL)
    rospy.loginfo("Tomogram exported: %s", file_name)

# planner/scripts/planner_wrapper.py  loadTomogram() （官方源码，已加中文注释）
def loadTomogram(self, tomo_file):
    with open(self.tomo_dir + tomo_file + '.pickle', 'rb') as handle:
        data_dict = pickle.load(handle)

        tomogram = np.asarray(data_dict['data'], dtype=np.float32)

        self.resolution = float(data_dict['resolution'])           # 栅格分辨率
        self.center = np.asarray(data_dict['center'], dtype=np.double)  # 地图中心
        self.n_slice = tomogram.shape[1]                          # 层数 L
        self.slice_h0 = float(data_dict['slice_h0'])              # 首层切片高度
        self.slice_dh = float(data_dict['slice_dh'])              # 切片间距
        self.map_dim = [tomogram.shape[2], tomogram.shape[3]]     # (W, H)
        self.offset = np.array([int(self.map_dim[0] / 2), int(self.map_dim[1] / 2)],
                               dtype=np.int32)

    trav = tomogram[0]      # 可通行代价 c^T
    trav_gx = tomogram[1]   # 代价沿 x 的梯度
    trav_gy = tomogram[2]   # 代价沿 y 的梯度
    elev_g = tomogram[3]    # 地面高程 e^G
    elev_g = np.nan_to_num(elev_g, nan=-100)   # 无地面 -> -100
    elev_c = tomogram[4]    # 天花板高程 e^C
    elev_c = np.nan_to_num(elev_c, nan=1e6)    # 露天 -> 1e6

    self.initPlanner(trav, trav_gx, trav_gy, elev_g, elev_c)

• 记住这个 5 通道结构——下面 2-4 讲切片得到 $e^G$/$e^C$、2-5 讲代价、2-6 讲层简化，实际上就是在逐个解释这 5 个通道是怎么算出来的；等到了 3-1，planner_wrapper.py 会把它读回、反推出 gateway 并交棒给 C++ 的 A*。

2-4 Tomogram 构建：地面层与天花板层

• 这一节对应论文 Algorithm 1，代码落在 tomography/scripts/kernels.py（CUDA 核函数）和 tomography/scripts/tomogram.py（把核函数的结果组装成地图）。
  

• 先看数学定义。设点 $p_u=[x_u,y_u,z_u{]}^T$ 是点云里的一个点，它投影到平面上的栅格索引为 $(i,j)$。对第 $k$ 层切片，我们做以下两个「累加」：

$$e_{i,j,k}^G\leftarrow \max (z_u,e_{i,j,k}^G),\text{当}{z}_u\le z_{min}+k{d}_s$$

$$e_{i,j,k}^C\leftarrow \min (z_u,e_{i,j,k}^C),\text{当}{z}_u>z_{min}+k{d}_s$$

• 解释一下这两个式子的物理含义：

  • $e^G$ 取「切片平面以下所有点里 $z$ 最大的那个」——也就是离平面最近的、支撑在脚下的表面（地面）；
  • $e^C$ 取「切片平面以上所有点里 $z$ 最小的那个」——也就是悬在头顶上、离平面最近的表面（天花板/悬挑）。

• 这两个值一减，就是该栅格处的净空高度（Interval / Clearance）：

$$d_{i,j,k}^I=e_{i,j,k}^C-e_{i,j,k}^G$$

• $d^I$ 是整个算法里最重要的一个量：它刻画了「这个栅格头顶上到底有没有空间、够不够机器人通过」。

• 再看代码。核心核函数在 kernels.py 里，是一个用 CUDA 写的 tomographyKernel。为了省内存，论文用 float16 存高程，用 CUDA 的原子操作 atomicMax/atomicMin 来并发更新（因为多个点可能同时落在同一个栅格上）：

// tomography/scripts/kernels.py  tomographyKernel 的 operation（官方源码，已加中文注释）
U px = points[i * 3];
U py = points[i * 3 + 1];
U pz = points[i * 3 + 2];

int idx = getIndexMap_1d(px, py, center[0], center[1]);  // 点 (px,py) 落到哪个栅格
if ( idx < 0 ) 
    return;                                              // 栅格越界 -> 跳过
for ( int s_idx = 0; s_idx < ${n_slice}; s_idx ++ )
{
    U slice = ${slice_h0} + s_idx * ${slice_dh};         // 本切片平面的高度
    if ( pz <= slice )
        // 点在平面下方 -> 属于「地面层」，取最高点（原子 max）
        atomicMaxFloat(&layers_g[getIndexBlock_1d(idx, s_idx)], pz);
    else
        // 点在平面上方 -> 属于「天花板层」，取最低点（原子 min）
        atomicMinFloat(&layers_c[getIndexBlock_1d(idx, s_idx)], pz);
}

• 这里有两个工程细节值得注意：
  • 原子操作的必要性：同一个栅格 $(i,j)$ 可能被成千上万个点命中，如果不用 atomicMax/atomicMin，并发写就会产生数据竞争，得到错误的高程。
  • float16 半精度：高程用半精度存，内存直接减半。对于「地面高程 / 天花板高程」这种量级在几十米以内、精度要求到厘米级的场景，半精度足够，而且换来了可观的省内存收益。
• 核函数跑完之后，tomogram.py 里的 point2map 负责把结果组装起来——先算净空，再算代价、膨胀、层简化（后三步见 2-5、2-6），最后返回 6 个量：

# tomography/scripts/tomogram.py  point2map() （官方源码节选，已加中文注释）
def point2map(self, points):
    points = cp.asarray(points)
    points = points[~cp.isnan(points).any(axis=1)]   # 去掉含 NaN 的点
    self.clearMap()

    # ① 逐点跑 tomographyKernel，得到地面层 layers_g / 天花板层 layers_c
    self.tomography_kernel(
        points, self.center,
        self.layers_g, self.layers_c,
        size=(points.shape[0])
    )

    # ② 地面高程沿 x/y 的相邻栅格差（前、后向取较大者再平方），见 2-5 的 Eq. (2)
    diff_x_sq = cp.maximum(
        (self.layers_g[:, 1:-1, :] - self.layers_g[:, :-2, :]) ** 2,
        (self.layers_g[:, 1:-1, :] - self.layers_g[:,  2:, :]) ** 2
    )
    diff_y_sq = cp.maximum(
        (self.layers_g[:, :, 1:-1] - self.layers_g[:, :, :-2]) ** 2,
        (self.layers_g[:, :, 1:-1] - self.layers_g[:, :,  2:]) ** 2
    )
    self.grad_mag_sq[:, 1:-1, 1:-1] = diff_x_sq[:, :, 1:-1] + diff_y_sq[:, 1:-1, :]
    self.grad_mag_max[:, 1:-1, 1:-1] = cp.maximum(diff_x_sq[:, :, 1:-1], diff_y_sq[:, 1:-1, :])

    # ③ 净空 = 天花板 - 地面，即论文里的 d^I = e^C - e^G
    interval = (self.layers_c - self.layers_g)

    # ……中间省略 travKernel（2-5 代价）、inflationKernel（2-5-5 膨胀）、
    #    idx_simp（2-6 层简化）三部分……

    # ④ 层简化后：代价梯度 / 无效栅格转 NaN，最后返回 6 个量
    trav_grad_x = (self.inflated_cost[idx_simp][:, 2:, :] - self.inflated_cost[idx_simp][:, :-2, :])
    trav_grad_y = (self.inflated_cost[idx_simp][:, :, 2:] - self.inflated_cost[idx_simp][:, :, :-2])

    layers_t = self.inflated_cost[idx_simp].get()
    layers_g = cp.where(self.layers_g[idx_simp] > -1e6, self.layers_g[idx_simp], cp.nan).get()
    layers_c = cp.where(self.layers_c[idx_simp] < 1e6, self.layers_c[idx_simp], cp.nan).get()
    trav_gx = np.zeros_like(layers_g)
    trav_gx[:, 1:-1, :] = trav_grad_x.get()
    trav_gy = np.zeros_like(layers_g)
    trav_gy[:, :, 1:-1] = trav_grad_y.get()

    return layers_t, trav_gx, trav_gy, layers_g, layers_c, t_gpu

• 这 6 个返回值里，layers_g / layers_c 就是本节的 $e^G$ / $e^C$；它们和 layers_t（代价）、trav_gx / trav_gy（代价梯度）合起来，正好是 2-3 节 pickle 里的 5 通道；最后的 t_gpu 只是 benchmark 计时，不参与建图。

• 这里有一个 layers_g 初始化的细节：没有点落在某栅格「下方」时，layers_g 会保持初值（一个极小的数，比如 $-10^6$），表示「这个栅格脚下没有地面」，后续会被标记为无效（NaN），A* 搜索时直接跳过。同样，layers_c 初值是一个极大的数，表示「头顶没有天花板（露天）」。

举个例子：假设一楼地面高 0 m，天花板高 3 m。在切片平面高 1.5 m 的地方，某栅格 $(i,j)$ 下方有点在 $z=0$（地面）、上方有点在 $z=3$（天花板），那么 $e^G=0,e^C=3$，净空 $d^I=3$ m，机器人可以自由通过。如果这个栅格上方吊下来一根横梁到 $z=1.8$ m，那么 $e^C=1.8$，净空瞬间缩到 $1.8$ m，机器人就得低头或者绕行。

2-5 可通行性估计：从高程到代价

• 有了地面高程 $e^G$、天花板高程 $e^C$ 和净空 $d^I$，下一步要把它们翻译成一个标量「可通行代价」$c^T$。这个代价越小越好走，越大越难走，超过某个阈值就是「不可通行」。

• 论文把这部分拆成两个子代价的叠加：净空代价 $c^I$ 和地形代价 $c^G$，再合成、膨胀，最终得到送入 A* 的代价图。下面分五个小节展开。

2-5-1 净空代价（Interval Cost）

• 论文 Eq. (1)：

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

• 含义：

  • 净空 $d^I$ 小于最小通过高度 $d_{min}$（代码里 interval_min）时，直接判为障碍，代价取上限 $c^B$（cost_barrier）。
  • 否则，净空越小，代价线性上升：${\alpha }_d(d_{ref}-d^I)$，其中 $d_{ref}$ 是「正常操作高度」（interval_free），净空达到 $d_{ref}$ 以上就零代价。
  • 这个式子的作用是：逼着规划器挑净空高的路走，避免机器人贴着头顶的天花板/横梁钻。

• 净空代价的曲线如下。注意 $d^I=d_{min}$ 处是跳变：刚低于 0.50 直接判为障碍 $c^B=50$，刚高于 0.50 的代价只有 $20\times 0.15=3$；过了 $d_{ref}=0.65$ 就归零。

• 对应代码在 kernels.py 的 travKernel 里（净空部分，已加中文注释）：

// tomography/scripts/kernels.py  travKernel （官方源码，已加中文注释）
// ---- 净空代价（对应 Eq. (1)）----
if ( interval[i] < ${interval_min} )            // d^I < d_min
{
    trav_cost[i] = ${cost_barrier};             // c^I = c^B（障碍）
    return;
}
else
    trav_cost[i] += max(0.0, 20 * (${interval_free} - interval[i]));
    // c^I = max(0, alpha_d * (d_ref - d^I))，alpha_d = 20

2-5-2 地形代价（Ground Cost）

• 先看地面坡度。论文用相邻栅格的高程差（前、后向取较大者）近似地面沿 $x$、$y$ 的梯度 $g^x,g^y$，再定义两种「坡度范数」（Eq. (2)）：

$$m_{xy}=\max (|g^x|,|g^y|),m_{grad}=\sqrt{(g^x{)}^2+(g^y{)}^2}$$

• 其中 $m_{xy}$ 是「最大方向坡度」（最陡的那个方向的坡），$m_{grad}$ 是「总坡度范数」。然后地形代价按坡度大小分档（Eq. (3)）：

$$c^G=\{\begin{array}{ll}{c}^B,& m_{xy}>{\theta }_b\\ {\alpha }_s{\left(\frac{m_{grad}}{{\theta }_s}\right)}^2,& m_{grad}<{\theta }_s\end{array}$$

• 含义：坡度太陡（$m_{xy}>{\theta }_b$，代码里 step_cross 对应）就判为硬障碍；坡度平缓（$m_{grad}<{\theta }_s$，step_stand 对应）就按坡度的平方给一个「爬坡代价」。坡度越大越难爬，代价越高。

• 地形代价的曲线如下。平缓坡按 $m_{grad}$ 的平方增长，超过 ${\theta }_b$ 直接封顶 $c^B$；中间的「台阶区域」交给下一小节。

• 对应代码（平缓坡分支，已加中文注释）：

// tomography/scripts/kernels.py  travKernel （官方源码，已加中文注释）
// ---- 地形代价·平缓坡（对应 Eq. (3)）----
if ( grad_mag_sq[i] <= ${step_stand_sq} )
{
    // 平缓坡：m_grad < theta_s，代价 = alpha_s * (m_grad/theta_s)^2
    trav_cost[i] += 15 * grad_mag_sq[i] / ${step_stand_sq};
    return;
}

• 坡度（梯度）的平方是在 tomogram.py 里用**相邻栅格的高程差（前、后向取较大者，再平方）**算的，对应 Eq. (2)：

# tomography/scripts/tomogram.py  point2map （官方源码，已加中文注释）
# 地面层沿 x 方向的相邻栅格高程差：前、后向差值的平方，取较大者
diff_x_sq = cp.maximum(
    (self.layers_g[:, 1:-1, :] - self.layers_g[:, :-2, :]) ** 2,   # 与左边邻居
    (self.layers_g[:, 1:-1, :] - self.layers_g[:,  2:, :]) ** 2    # 与右边邻居
)
# 沿 y 方向同理
diff_y_sq = cp.maximum(
    (self.layers_g[:, :, 1:-1] - self.layers_g[:, :, :-2]) ** 2,
    (self.layers_g[:, :, 1:-1] - self.layers_g[:, :,  2:]) ** 2
)
# grad_mag_sq = (g^x)^2+(g^y)^2，grad_mag_max = max((g^x)^2,(g^y)^2) —— 对应 Eq. (2)
self.grad_mag_sq[:, 1:-1, 1:-1] = diff_x_sq[:, :, 1:-1] + diff_y_sq[:, 1:-1, :]
self.grad_mag_max[:, 1:-1, 1:-1] = cp.maximum(diff_x_sq[:, :, 1:-1], diff_y_sq[:, 1:-1, :])

• 这里注意，代码里存的全是平方值（为了少开一次根号），这也是为什么核函数里比较的时候用的是 step_stand_sq（$={\theta }_s^2$）而不是 ${\theta }_s$。

2-5-3 台阶/楼梯可跨越性

• 这是面向足式机器人专门加的（Eq. (4)）：

$$c^G=\{\begin{array}{ll}{\alpha }_b{\left(\frac{m_{xy}}{{\theta }_b}\right)}^2,& p_s>{\theta }_p\\ c^B,& \text{otherwise}\end{array}$$

• 含义：如果某个陡坡/台阶的 $7\times 7$ 邻域（kernel_size=7）里「可站立栅格的比例」$p_s$ 超过阈值 ${\theta }_p$（standable_ratio），说明这是个「可以一级一级踩上去」的楼梯，而不是一面墙，就给它一个有限的跨越代价 ${\alpha }_b(m_{xy}/{\theta }_b{)}^2$；否则判为障碍 $c^B$。

• 这一步让足式机器人能把楼梯当成「可跨越的台阶」而不是「墙」，是 PCT 面向复杂结构的关键一环。

• 下图左半是台阶的侧视剖面（踏板平缓＝绿色可站立，立板陡峭＝红色不可站立，橙色虚线框是 7×7 邻域窗口）；右半对比了「墙」和「台阶」两种邻域里可站立栅格的比例 $p_s$。

• 注意红/绿只描述单个栅格的属性：绿色＝坡度平缓、可站立（$m_{grad}<{\theta }_s$），红色＝坡度陡峭、不可站立（台阶的立板/竖直面）。红色本身不代表「障碍」——是否判为障碍，看的是整个邻域里绿色（可站立）栅格的比例 $p_s$：台阶图里那列红色立板虽然陡峭，但两侧全是绿色踏板，$p_s\approx 0.86>0.20$，所以整体判为「可跨越台阶」而非障碍；墙图里全是红色、$p_s=0<0.20$，才判为障碍 $c^B$。

• 对应代码（台阶/陡坡分支，承接上一小节的 if，已加中文注释）：

// tomography/scripts/kernels.py  travKernel （官方源码，已加中文注释）
// ---- 地形代价·台阶/陡坡（对应 Eq. (4)）----
else
{
    if ( grad_mag_max[i] <= ${step_cross_sq} )
    {
        // 陡坡/台阶：统计邻域内「可站立栅格」比例 p_s
        int standable_grids = 0;
        for ( int dy = -${half_kernel_size}; dy <= ${half_kernel_size}; dy++ )
        {
            for ( int dx = -${half_kernel_size}; dx <= ${half_kernel_size}; dx++ )
            {
                int idx = getIdxRelative(i, dx, dy);
                if ( idx < 0 )
                    continue;
                if ( grad_mag_sq[idx] < ${step_stand_sq} )
                    standable_grids += 1;
            }
        }
        if ( standable_grids < ${standable_th} )   // p_s <= theta_p
        {
            trav_cost[i] = ${cost_barrier};        // c^G = c^B（墙）
            return;
        }
        else
            trav_cost[i] += 20 * grad_mag_max[i] / ${step_cross_sq};
            // 可跨越台阶：c^G = alpha_b * (m_xy/theta_b)^2，alpha_b = 20
    }
    else
    {
        trav_cost[i] = ${cost_barrier};            // m_xy > theta_b，硬障碍
        return;
    }
}

2-5-4 代价合成

• 初始代价合成（Eq. (5)）：

$$c_{i,j,k}^{init}=\min (c^B,c^I+c^G)$$

• 即净空代价和地形代价相加，再用 $c^B$ 封顶。

• 合成的流程就是「净空 + 地形 → 相加 → $c^B$ 封顶」，用流程图表示如下：

• 这里要特别注意一点：上面几段代码里的 += 就是「相加」的实现——净空代价先累加进 trav_cost，地形代价再累加进去，最后凑成 $c^I+c^G$。而「封顶」不是显式写 min(c^B, ...)，而是靠那几处 trav_cost[i] = ${cost_barrier} 的直接赋值——一旦命中障碍分支，就把整个值覆盖成 $c^B$，自然不可能超过 $c^B$。

• 到这里，把上面几节涉及的参数汇总成一张表（取值以 scene_building.py 默认参数为例），方便对照代码：

符号	代码变量	取值	含义
$d_{min}$	interval_min	0.50	最小净空通过高度，$d^I$ 低于它直接判障碍
$d_{ref}$	interval_free	0.65	正常操作净空，$d^I$ 达到它净空代价归零
${\alpha }_d$	硬编码 20	20	净空代价的线性斜率
${\theta }_s$	step_stand	≈0.05	可站立坡度上限（$=1.2r_g\tan {\theta }_{slope}$）
${\theta }_b$	step_cross/step_max	0.17	可跨越台阶高度上限
${\theta }_p$	standable_ratio	0.20	邻域可站立比例阈值，$p_s$ 超过它才判「台阶」
${\alpha }_s$	硬编码 15	15	平缓坡代价系数
${\alpha }_b$	硬编码 20	20	台阶跨越代价系数
$c^B$	cost_barrier	50	代价上限，任何障碍分支都赋这个值
$m_{grad}^2$	grad_mag_sq	—	总坡度范数平方 $=(g^x{)}^2+(g^y{)}^2$
$m_{xy}^2$	grad_mag_max	—	最大方向坡度平方 $=\max ((g^x{)}^2,(g^y{)}^2)$

• 最后两行值得强调：代码里存的全是平方值（省一次开根号），所以核函数里比较阈值时用的是 step_stand_sq（$={\theta }_s^2$）、step_cross_sq（$={\theta }_b^2$），而不是 ${\theta }_s$、${\theta }_b$ 本身。

2-5-5 安全膨胀

• 光看当前栅格自己的代价还不够，为了给机器人留出安全余量（机身尺寸），论文把每个栅格的代价「向外膨胀」，用一个小核函数在邻域上做「带权取最大」（Eq. (6)）：

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

• 其中 $d_{mn}$ 是邻域栅格 $(m,n)$ 到中心的距离，$d_{inf}$ 是膨胀起始半径（inflation），$d_{sm}$ 是安全余量（safe_margin），$r_g$ 是栅格分辨率。这个核函数的意思是：离中心越近权越接近 1，越远越接近 0，最终在邻域内做「带权最大值」：

// tomography/scripts/kernels.py  inflationKernel （官方源码，已加中文注释）
int counter = 0;
for ( int dy = -${half_kernel_size}; dy <= ${half_kernel_size}; dy++ )
{
    for ( int dx = -${half_kernel_size}; dx <= ${half_kernel_size}; dx++ )
    {
        int idx = getIdxRelative(i, dx, dy);
        if ( idx >= 0 )
            // score_table 就是按 Eq. (6) 预先算好的核权重，逐栅格做「带权取最大」
            inflated_cost[i] = max(inflated_cost[i], trav_cost[idx] * score_table[counter]);
        counter += 1;
    }
}

• 膨胀之后，机器人就会「自觉地」和障碍、陡坡保持一段安全距离，而不是贴着边硬挤。这正是论文能规划出「平滑、安全」轨迹的原因之一。

• 下图左半是膨胀核权重 $K$ 随距离 $d_{mn}$ 的衰减（$d_{inf}$ 内全额继承，$d_{sm}+r_g$ 处衰减到 0）；右半是一个障碍点膨胀成安全区的热图（颜色越亮代价越高）。

2-6 层简化：把几十层压成几层

• 上一节算出来的 Tomogram 有几十层（例如 spiral 场景 $d_s=0.5$ m、楼高约 10 m，就有 20+ 层）。如果每层都拿去 A* 搜索，层与层之间的 gateway 组合会爆炸。所以论文加了一步「层简化（Tomogram Simplification）」。

• 简化准则（Eq. (7)）很朴素：

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

• 意思是：如果第 $k$ 层的所有「可通行栅格」集合 $M_k$，完全被上下两层（$k-1$ 和 $k+1$）的可通行栅格并集所覆盖，那么第 $k$ 层就是「冗余」的，可以删掉——因为任何从这层能走的路，在上下层也都能走。

• 更精细一点，论文用 Eq. (8)(9) 定义了「独特栅格（unique grid）」：只有当某栅格的地面高程或代价，在相邻层之间发生了变化（比如地面升高了、或者代价变低了），它才值得保留。只保留含有独特栅格的层。

• 对应代码在 tomogram.py 的 point2map 里，用 idx_simp 记下「被保留下来的层的索引」：

# tomography/scripts/tomogram.py  （官方源码，已加中文注释）
idx_simp = [0]                                   # 第 0 层一定保留
if self.layers_g.shape[0] > 1:
    l_idx, m_idx = 0, 1
    diff_h = self.layers_g[1:] - self.layers_g[:-1]   # 相邻层地面高程差
    while m_idx < self.n_slice_init - 2:
        # 与「上一个保留层」比较：地面是否升高
        mask_l_g = self.layers_g[m_idx] - self.layers_g[l_idx] > 0
        # 代价是否下降，即更上面一层更好走
        mask_l_t = self.inflated_cost[l_idx] > self.inflated_cost[m_idx]
        # 上一层地面更高（向上是「新的层」而不是同一层）
        mask_u_g = diff_h[m_idx] > 0
        # 且该栅格本身可通行
        mask_t = self.inflated_cost[m_idx] < self.cost_barrier
        # 四个条件同时满足 -> 这一层存在「独特栅格」，值得保留
        unique = (mask_l_g | mask_l_t) & mask_u_g & mask_t
        if cp.any(unique):
            idx_simp.append(m_idx)               # 保留第 m_idx 层
            l_idx = m_idx                        # 更新基准层
        m_idx += 1
    idx_simp.append(m_idx)                       # 顶层一定保留

• 下面用一个夸张的小例子直观演示这条准则：左图沿高度切成 9 层，中间的 7 层都是「重复的楼梯中段」——只有楼梯通道这一窄条，而且每一层都被上、下层的并集覆盖（$M_k\subset M_{k-1}\cup M_{k+1}$），判为冗余（红色）；简化后合并成 3 层——1 楼、1 层楼梯中段、2 楼（右图）。这正是论文里 spiral 场景从 46 层压到 5 层的来源。

• 经过这一步，原本几十层的 Tomogram 被压成了少数几层（论文里 spiral 场景从 46 层压到 5 层），A* 的搜索空间大大缩小。这就是论文「路径规划快 3 倍」的主要来源。

举个例子：一栋楼里，1 楼到 2 楼之间是 20 级台阶。每级台阶高 0.1 m，用 0.5 m 的切片间距去切，会得到好几层「只差一点点高度」的切片。简化之后，这些几乎重复的层被合并成「1 楼」「楼梯中段」「2 楼」这样几层关键的层，A* 只需要在这几层之间做 gateway 切换，而不是在几十层里瞎找。

2-7 离线建图的使用与接口：一个 pickle 就是全部契约

• 前面 2-4～2-6 讲「怎么算」，这一节讲「怎么用」。离线阶段你只做一件事：喂点云、跑 tomography.py，拿回一个 <场景名>.pickle；中间那套「切片 → 代价 → 膨胀 → 层简化」全在 Python 内部完成，你不用碰。

• 这个 pickle 就是离线端和在线端唯一的对接点，格式如下：

{
    'data':       np.float16, shape (5, L, W, H)   # 5 通道多层地图（L = 简化后的层数）
                  #  [0] layers_t      膨胀后可通行代价（A* 的 cost 直接取它）
                  #  [1] trav_grad_x   代价沿 x 梯度（GPMP2 障碍因子雅可比）
                  #  [2] trav_grad_y   代价沿 y 梯度
                  #  [3] layers_g      地面高程 eG（无效栅格 = NaN）
                  #  [4] layers_c      天花板高程 eC（无效栅格 = NaN）
    'resolution': float    # 栅格分辨率
    'center':     (2,)     # 地图中心 (x, y)
    'slice_h0':   float    # 首层切片高度
    'slice_dh':   float    # 切片间距
}

• 在线端 planner_wrapper.py 读回它、反推 gateway，塞进 C++ 的 OfflineElePlanner.init_map(...) 开始 A*。C++ 只认这些字段名和形状，不知道、也不关心数据是 CUDA 算的还是 Python 算的。

• 所以这个 Python 可以自己实现——这正是「离线 / 在线解耦」的红利：只要你按上面的字段名和形状 dump 出 pickle，在线部分就原封不动地跑。那套吃显存的 CUDA 切片只是离线建图的一种实现，换成纯 Python、PCL 都行；离线慢一点、重一点无所谓，在线保持轻快即可。

---

3 在线路径规划：多层 A* 搜索与 GPMP2 轨迹优化

3-1 多层 A*：gateway 跨层搜索

3-1-1 总览

• Tomogram 简化完，就进入在线的路径搜索阶段。核心是 planner/lib/src/a_star/a_star_search.cc，一个「多层 A*」。
• 按 2-3 那张全景图所说，planner_wrapper.py 会读回 .pickle，先由相邻层代价 / 高程的差分反推出「gateway」（层与层之间可上下楼的通道），再把代价、高程、梯度、gateway 一起塞进 C++ 的 OfflineElePlanner.init_map(...)——离线地图在这里正式「交棒」给 C++，A* 随之开始。
• 相比经典 A*，PCT 的多层 A* 加了两个东西：
  • 同层 8 邻域扩展（3-1-3，同一层内向 8 个方向扩展）
  • 跨层 gateway 切换（3-1-4，在 gateway 处切到正上方 / 正下方相邻层）。

3-1-2 A* 回顾

• A* 我们都很熟，核心是评价函数：
  $$f(n)=g(n)+h(n)$$
• 其中 $g(n)$ 是从起点到当前节点 $n$ 的实际代价，$h(n)$ 是当前节点到终点的启发式估计（PCT 里默认用八方向 octile 距离，见 3-1-5 的 GetHeuristic），$f(n)$ 越小优先级越高——每次从优先队列里弹出 $f$ 最小的节点扩展。多层 A* 复用了这套骨架，只是把「邻居」的定义扩大了（同层 8 邻域 + 跨层 gateway）。

3-1-3 同层 8 邻域扩展

• 同一层内，当前栅格向 8 个方向（上下左右 + 四个对角）扩展。扩展时新节点的 $g$ 由「几何距离」和「地形代价」两部分累加：
  $$g_{\text{new}}=g+\Vert \Delta \mathbf{p}\Vert +w\cdot c^T$$
• 其中 $\Vert \Delta \mathbf{p}\Vert =\sqrt{\Delta x^2+\Delta y^2+\Delta z^2}$ 是到邻居的欧氏距离（含高度差），$c^T$ 是邻居栅格的可通行代价（2-5 算出来的），$w$ 是权重。对应代码：

// planner/lib/src/a_star/a_star_search.cc  （官方源码，已加中文注释）
// diff 是邻居节点与当前节点的索引差（含整数化的 z 层高差）
auto diff = neighbor_node->idx - current_node->idx;
// 邻居栅格本身的可通行代价，乘一个权重
double step_cost = step_cost_weight_ * neighbor_node->cost;
if (step_cost < 5) step_cost = 0.0;   // 代价很小就当它是「免费」的路
// 实际代价 = 距离 + 地形代价
tentative_g =
    current_node->g +
    std::sqrt(diff[0] * diff[0] + diff[1] * diff[1] + diff[2] * diff[2]) +
    step_cost;

• step_cost_weight_ 在这里被 planner_wrapper.py 的 init_map(..., 0.2, ...) 设为 0.2（C++ 头文件里的默认值是 1.0，但实际总是被覆盖成 0.2）。注意 if (step_cost < 5) step_cost = 0 这个技巧：代价低于阈值的地面（比如平坦地面）不额外加价，只有「明显有代价」的地面（坡、台阶）才计入，让路径更倾向于走平坦路。

• 下图是「同层 8 邻域」的九宫格示意（俯视同一层平面）：中心红格是当前节点，周围 8 个蓝格是它的同层邻居；绿箭头是正交方向（距离 $=1$）、橙箭头是对角方向（距离 $=\sqrt{2}$）。

3-1-4 gateway

• 跨层切换靠「gateway」——层与层之间可以上下楼的通道。论文的定义是：同一位置，地面高程不变、但相邻层代价更低（这里的「高程不变」更准确说是上下两层地面高程几乎一致，否则一点点高程噪声也会被误判成「能上楼」；下面代码会把它落成两个阈值）：
  $$e_{i,j,k}^G=e_{i,j,k+1}^G\wedge c_{i,j,k+1}^T<c_{i,j,k}^T$$

• 下图把 gateway 的来源讲透——左图是真实点云台阶，中图是它被切成多层图（每一层只记录一段地面），右图是俯视的代价图：台阶各级交界处（立板处）被标记成 gateway（可上下楼）。

• 这个 gateway 是离线阶段在 planner/scripts/planner_wrapper.py 的 initPlanner 里预先算好、存进 gateway 掩码里的（即 C++ 侧的 ele，向上记为 2、向下记为 -2）：

# planner/scripts/planner_wrapper.py  initPlanner() （官方源码，已加中文注释）
def initPlanner(self, trav, trav_gx, trav_gy, elev_g, elev_c):
    diff_t = trav[1:] - trav[:-1]                 # 相邻两层的可通行代价之差
    diff_g = np.abs(elev_g[1:] - elev_g[:-1])     # 相邻两层地面高程之差的绝对值

    # gateway_up：上层代价明显更低（<-8.0）+ 地面连续 -> 可以「上一层」
    gateway_up = np.zeros_like(trav, dtype=bool)
    mask_t = diff_t < -8.0
    mask_g = (diff_g < 0.1) & (~np.isnan(elev_g[1:]))
    gateway_up[:-1] = np.logical_and(mask_t, mask_g)

    # gateway_dn：下层代价明显更低（>8.0）+ 地面连续 -> 可以「下一层」
    gateway_dn = np.zeros_like(trav, dtype=bool)
    mask_t = diff_t > 8.0
    mask_g = (diff_g < 0.1) & (~np.isnan(elev_g[:-1]))
    gateway_dn[1:] = np.logical_and(mask_t, mask_g)

    gateway = np.zeros_like(trav, dtype=np.int32)
    gateway[gateway_up] = 2     # 标记为「向上层切换」的 gateway
    gateway[gateway_dn] = -2    # 标记为「向下层切换」的 gateway

• 代码把论文里的「地面不变」「代价更低」具体化成了两个阈值，写成式子：
  $$\text{gateway\_up}=(c_{k+1}^T-c_k^T<-8.0)\wedge (|e_{k+1}^G-e_k^G|<0.1)$$
  $$\text{gateway\_dn}=(c_{k+1}^T-c_k^T>8.0)\wedge (|e_{k+1}^G-e_k^G|<0.1)$$

• 用 8.0 和 0.1 这两个阈值，是为了避免数值抖动造成误判（否则一点点高程噪声就可能被误当成「可以上楼」）。

• 到了 A* 搜索时，DecideLayer 函数根据当前节点的 ele 值，决定应该切到「哪一层」去扩展邻居：

// planner/lib/src/a_star/a_star_search.cc  （官方源码，已加中文注释）
int Astar::DecideLayer(const Node* cur_node) const {
  int layer = cur_node->layer;
  int i = cur_node->idx[1];
  int j = cur_node->idx[2];
  double cur_height = cur_node->height;

  int true_layer = layer;

  // 依次检查当前层、下一层、上一层（search_layers_offset_ = {0,-1,+1}）
  for (const auto offset : search_layers_offset_) {
    int cur_layer = layer + offset;

    if (cur_layer < 0 || cur_layer >= max_layers_) {
      continue;
    }

    const Node& search_node = grid_map_[cur_layer][i][j];

    // 高度与当前节点接近，才可能是同一个「物理位置」的 gateway
    if (abs(search_node.height - cur_height) > 0.2) {
      continue;
    }

    if (search_node.ele > 0.5) {          // 标记为向上 -> 切到上一层
      true_layer = std::min(cur_layer + 1, max_layers_ - 1);
      break;
    } else if (search_node.ele < -0.5) {  // 标记为向下 -> 切到下一层
      true_layer = std::max(cur_layer - 1, 0);
      break;
    }
  }

  return true_layer;
}

3-1-5 完整流程

• 于是，一条完整的路径可能「在 1 楼走一段 → 通过楼梯 gateway 切到 2 楼 → 在 2 楼走一段」。这正是 PCT 能规划出「跨层导航」的原因。搜索结果最后会被整理成一个个 PathPoint（layer, x, y, height），交给轨迹优化器（3-2）。

• 把 3-1-2 的 $f=g+h$、3-1-3 的同层 8 邻域、3-1-4 的 gateway 切换串起来，下面直接给出 a_star_search.cc（含 Node 结构体）的官方完整实现，每个环节用中文注释标注了对应小节：

// planner/lib/src/a_star/a_star_search.h / .cc  —— 官方完整实现（3-1-2 ~ 3-1-4）

// —— 搜索节点：一个栅格在「哪一层 layer」的「哪个索引 idx」——
class Node {
 public:
  Node() = default;
  Node(Eigen::Vector3i idx, Node* parent) : idx(idx), parent(parent) {}
  ~Node() = default;

  bool operator==(const Node& other) const { return idx == other.idx; }

  void Reset() {
    f = 0.0;
    g = 1e9;
    parent = nullptr;
  }

  double f = 1e9;
  double g = 1e9;
  double height = 0.0;                       // 地面高程 eG
  double ele = 0;                            // gateway 掩码：2 向上 / -2 向下 / 0 无
  double cost = 0.0;                         // 可通行代价 cT
  int layer = 0;                             // 所在层
  Eigen::Vector3i idx = Eigen::Vector3i(0, 0, 0);  // (layer, row, col)
  Node* parent = nullptr;
};

struct NodeCompare {                          // 小顶堆比较器
  bool operator()(const Node* a, const Node* b) const { return a->f > b->f; }
};

// 同层 8 邻域的 8 个偏移：上下左右 + 四个对角
static std::vector<Eigen::Vector2i> kNeighbors = std::vector<Eigen::Vector2i>{
    Eigen::Vector2i(-1, -1), Eigen::Vector2i(-1, 0), Eigen::Vector2i(-1, 1),
    Eigen::Vector2i(0, -1),  Eigen::Vector2i(0, 1),  Eigen::Vector2i(1, -1),
    Eigen::Vector2i(1, 0),   Eigen::Vector2i(1, 1),
};

bool Astar::Search(const Eigen::Vector3i& start, const Eigen::Vector3i& goal) {
  auto t0 = std::chrono::high_resolution_clock::now();

  if (!search_result_.empty()) {
    Reset();
    search_result_.clear();
  }

  auto start_node = &grid_map_[start[0]][start[2]][start[1]];
  auto goal_node = &grid_map_[goal[0]][goal[2]][goal[1]];
  start_node->g = 0.0;                       // ① 起点 g=0（3-1-2：f = g + h）

  if (goal_node->cost > cost_threshold_) {   // 终点本身不可通行
    printf("goal node is not reachable, cost: %f", goal_node->cost);
    return false;
  }

  std::priority_queue<Node*, std::vector<Node*>, NodeCompare> open_set;
  std::unordered_map<int, Node*> closed_set;

  open_set.push(start_node);

  printf("start searching\n");

  while (!open_set.empty()) {
    Node* current_node = open_set.top();
    open_set.pop();                          // ② 弹出 f 最小的节点

    if (current_node->idx == goal_node->idx) {
      while (current_node->parent != nullptr) {   // ⑤ 到达终点，回溯路径
        search_result_.emplace_back(current_node);
        current_node = current_node->parent;
      }
      std::reverse(search_result_.begin(), search_result_.end());
      if (debug_) ConvertClosedSetToMatrix(closed_set);
      auto duration = std::chrono::duration_cast<std::chrono::microseconds>(
          std::chrono::high_resolution_clock::now() - t0);
      printf("path found, time elapsed: %f ms\n",
             duration.count() / 1000.0);
      return true;
    }

    closed_set[GetHash(current_node->idx)] = current_node;

    int layer = DecideLayer(current_node);   // ④ 跨层 gateway 切换（3-1-4）

    int i, j = 0;
    double tentative_g = 0.0;
    for (const auto& neighbor : kNeighbors) {   // ③ 同层 8 邻域扩展（3-1-3）
      i = current_node->idx[1] + neighbor[0];
      j = current_node->idx[2] + neighbor[1];

      if (i < 0 || i >= max_y_ || j < 0 || j >= max_x_) {
        continue;
      }

      auto neighbor_node = &grid_map_[layer][i][j];

      if (neighbor_node->cost > cost_threshold_) {
        if (abs(neighbor_node->ele) < 0.5) {
          continue;
        } else {
          if (std::abs(neighbor_node->height - current_node->height) > 0.3) {
            continue;
          }
        }
      }

      auto diff = neighbor_node->idx - current_node->idx;
      double step_cost = step_cost_weight_ * neighbor_node->cost;
      if (step_cost < 5) step_cost = 0.0;
      tentative_g =
          current_node->g +
          std::sqrt(diff[0] * diff[0] + diff[1] * diff[1] + diff[2] * diff[2]) +
          step_cost;

      auto p_neighbor = closed_set.find(GetHash(neighbor_node->idx));
      if (p_neighbor != closed_set.end()) {
        if (tentative_g >= p_neighbor->second->g) {
          continue;
        }
      }

      if (tentative_g < neighbor_node->g) {
        neighbor_node->g = tentative_g;
        neighbor_node->f = tentative_g + GetHeuristic(neighbor_node, goal_node);
        neighbor_node->parent = current_node;
        open_set.push(neighbor_node);
      }
    }
  }

  auto duration = std::chrono::duration_cast<std::chrono::microseconds>(
      std::chrono::high_resolution_clock::now() - t0);
  printf("path not found\n, time elapsed: %f ms\n",
         duration.count() / 1000.0);
  if (debug_) {
    ConvertClosedSetToMatrix(closed_set);
  }
  return false;
}

// 启发式 h(n)：默认 kDiagonal（八方向 octile 距离），另有 kEuclidean / kManhattan
double Astar::GetHeuristic(const Node* node1, const Node* node2) const {
  double cost = 0.0;

  if (h_type_ == kEuclidean) {
    cost = (node1->idx - node2->idx).norm();
  } else if (h_type_ == kDiagonal) {
    Eigen::Vector3i d = node1->idx - node2->idx;
    int dx = abs(d(0)), dy = abs(d(1)), dz = abs(d(2));
    int dmin = std::min(dx, std::min(dy, dz));
    int dmax = std::max(dx, std::max(dy, dz));
    int dmid = dx + dy + dz - dmin - dmax;
    double h =
        std::sqrt(3) * dmin + std::sqrt(2) * (dmid - dmin) + (dmax - dmid);
    cost = h;
  } else if (h_type_ == kManhattan) {
    cost = (node1->idx - node2->idx).lpNorm<1>();
  } else {
    assert(false && "not implemented");
  }

  return cost;
}

说人话：普通 A* 在一张平铺的地图上找路；PCT 的 A* 在一摞叠起来的地图上找路。它除了在「同一层里前后左右走」，还能在「同一个位置往上爬或往下降」。而这个「能不能往上/往下爬」，就是 gateway——离线时已经算好了，只有地面连续、且那一层更好走的地方，才允许上下楼。

3-2 GPMP2 轨迹优化

3-2-1 总览

• A* 搜出来的是一条「折线」路径——由一串离散的 PathPoint 拼成，直接给机器人走会一顿一顿的，还可能贴着障碍边沿擦。所以最后一步要用轨迹优化把它变成一条平滑、连续、满足动力学约束的轨迹。

• 论文 Eq. (10)-(11) 描述的是「分段五阶多项式 + 最小 jerk 控制代价」的框架：第 $i$ 段轨迹用一个 5 次多项式表示，优化目标是「控制代价（jerk）最小 + 高度贴合参考 + 时间最短」，并带上一堆约束（起点终点、可通行、净空、转角速度上限等）：

$$\underset{\sigma ,T}{\min }{J}_c+w_z\Vert q_z(t)-Z_{ref}(q(t)){\Vert }^2+w_{T}T$$

• 而开源代码实际落地用的是 GPMP2（Gaussian Process Motion Planner 2，基于 GTSAM 因子图的高斯过程轨迹优化）。两者本质是同构的：GPMP2 里 use_quintic=True 用的是 WNOJ（White Noise On Jerk，白噪声加加速度） 先验，它的 MAP 解恰好就是分段五次多项式，和论文 Eq. (10) 的「五阶多项式」一一对应。下面我们按代码实际实现来逐行解读，并点明这层对应关系。

关联阅读：GPMP2 的底层是 GTSAM 因子图 + Levenberg-Marquardt 迭代，和位姿图优化/回环约束是同一套东西。想看 GTSAM 与 iSAM2 怎么从回环约束做到位姿图优化，可参考第四期：【3D SLAM源码解读系列】(四) GTSAM与iSAM2——从回环约束到位姿图优化。

3-2-2 状态定义与初值

• 先看状态定义。WNOJ 的 6 维状态是「位置、速度、加速度」在 $x$、$y$ 两轴各一份：

$$x=[x,\dot{x},\ddot{x},y,\dot{y},\ddot{y}{]}^{\top }$$

• 对照 planner/lib/src/trajectory_optimization/gpmp_optimizer/gpmp_optimizer.cc 里的 PathPointToNode：

// planner/lib/src/trajectory_optimization/gpmp_optimizer/gpmp_optimizer.cc  （官方源码，已加中文注释）
// 把 A* 输出的离散 PathPoint 转成 6 维状态 [x, vx, ax, y, vy, ay]
void GPMPOptimizer::PathPointToNode(const PathPoint& path_point, Vector6& x) {
  double v = std::max(path_point.ref_v, 1.0);   // 参考速度，clamp 到至少 1.0
  x(0, 0) = path_point.x;
  x(1, 0) = std::cos(path_point.heading) * v;   // vx = cos(航向)*v
  x(2, 0) = 0.0;                                // ax = 0（初值设加速度为 0）
  x(3, 0) = path_point.y;
  x(4, 0) = std::sin(path_point.heading) * v;   // vy = sin(航向)*v
  x(5, 0) = 0.0;                                // ay = 0
}

• 可以看到，A* 输出的离散 PathPoint 被当成了初值，其中速度用「航向 × max(ref_v, 1.0)」来初始化（ref_v 在首尾两点被设为 1、中间为 0，所以实际初速度都是 1.0），加速度先设 0，然后交给因子图优化去「平滑」。

3-2-3 GP 先验因子：平滑的灵魂

• 因子图里有三类因子（factor），分别对应论文里的不同约束。

• （1）GP 先验因子（GP Prior Factor）：这是 GPMP2 的灵魂，编码了「轨迹要平滑（jerk 小）」的先验。代码在 factors/gp_prior_factor.cc，误差函数一句话：

// planner/lib/src/trajectory_optimization/gpmp_optimizer/factors/gp_prior_factor.cc  （官方源码）
gtsam::Vector GPPriorFactor::evaluateError(
    const gtsam::Vector6& x1, const gtsam::Vector6& x2,
    boost::optional<gtsam::Matrix&> H1,
    boost::optional<gtsam::Matrix&> H2) const {
  if (H1) *H1 = phi_;
  if (H2) *H2 = -gtsam::Matrix66::Identity();
  return phi_ * x1 - x2;    // 误差 = Φ(Δt)·x1 - x2
}

• 这里的 $\Phi (\Delta t)$ 是状态转移矩阵。对 WNOJ 的 1 维 [x, ẋ, ẍ]，它就是：

$$\Phi (\tau )=\left[\begin{array}{ccc}1& \tau & \frac{1}{2}{\tau }^2\\ 0& 1& \tau \\ 0& 0& 1\end{array}\right]$$

• 这个因子把相邻两个状态「绑」在一起：要求 $x_{k+1}\approx \Phi (\Delta t)x_k$，也就是「下一时刻的状态，要符合加速度恒定的运动学外推」。而 Q 矩阵（wnoj.hpp 里 WhiteNoiseOnJerkModel2D::Q）是「白噪声加加速度」的协方差，正是它让 MAP 解成为五次多项式——这就是论文 Eq. (10)「五阶多项式」的代码实现。

3-2-4 障碍因子与航向角速率因子

• （2）障碍因子（Obstacle Factor）：把轨迹点约束到「可通行、代价低」的地方。代码在 factors/gp_obstacle_factor.cc：

// planner/lib/src/trajectory_optimization/gpmp_optimizer/factors/gp_obstacle_factor.cc  （官方源码）
gtsam::Vector GPObstacleFactor::evaluateError(
    const gtsam::Vector6& x1, boost::optional<gtsam::Matrix&> H1) const {
  double cost;
  Eigen::Vector2d grad;

  // 在可通行代价图上，双线性插值出 (x, y) 处的代价 cost 和梯度 grad
  cost = map_->GetValueBilinearSafe(current_layer_, x1(0, 0), x1(3, 0),
                                    height_hint_, &grad);
  current_layer_ =
      map_->UpdateLayerSafe(current_layer_, x1(0, 0), x1(3, 0), height_hint_);
  height_hint_ =
      map_->GetHeightSafe(current_layer_, x1(0, 0), x1(3, 0), height_hint_);

  double error = 0.0;

  if (cost <= cost_threshold_) {                       // 代价低于阈值 -> 无误差
    if (H1) {
      *H1 = Eigen::MatrixXd::Zero(1, 6);
    }
  } else {
    error = (cost - cost_threshold_) * (cost - cost_threshold_);  // 超出的平方
    if (H1) {
      *H1 = Eigen::MatrixXd::Zero(1, 6);
      (*H1)(0, 0) = 2 * (cost - cost_threshold_) * grad(0);
      (*H1)(0, 3) = 2 * (cost - cost_threshold_) * grad(1);
    }
  }

  return gtsam::Vector1(error);
}

• 这里 cost_threshold_ 就是 safe_cost_margin（默认 15），误差的雅可比是 $H=2(c-c_{thr})\nabla c$，对应 H1(0,0) 和 H1(0,3) 两项。

• 把「A* 阈值」和「障碍因子阈值」放在一起对比，能看到一个两段递进的安全设计：

环节	阈值	含义
A* 搜索	cost_threshold = 20	栅格 cost > 20 判「不可走」，直接挡死
GPMP2 障碍因子	cost_threshold = safe_cost_margin = 15	cost > 15 就开始惩罚

• A* 先以 20 为界找出一条可行路径，GPMP2 再以 15 为界把轨迹进一步推离障碍——优化阶段比搜索阶段更保守，轨迹优化器会尽量把路径「拉回」代价更低、更安全的地方。这正是 safe_cost_margin 这个名字的由来。

• （3）航向角速率因子（Heading Rate Factor）：限制轨迹转弯不能太急。代码在 factors/gp_heading_rate_factor.cc：

// planner/lib/src/trajectory_optimization/gpmp_optimizer/factors/gp_heading_rate_factor.cc  （官方源码）
gtsam::Vector GPHeadingRateFactor::evaluateError(
    const gtsam::Vector6& x1, boost::optional<gtsam::Matrix&> H1) const {
  double dx = x1(1);
  double ddx = x1(2);
  double dy = x1(4);
  double ddy = x1(5);
  double sqr_dx_dy = (dx * dx + dy * dy + 1e-6);
  double ddy_dx_dy_ddx = ddy * dx - dy * ddx;
  double dot_theta = ddy_dx_dy_ddx / sqr_dx_dy;   // 航向角变化率

  double error = 0.0;
  Eigen::MatrixXd J_dot_theta = Eigen::MatrixXd::Zero(1, 6);

  if (abs(dot_theta) > max_heading_rate_) {
    if (H1) {
      J_dot_theta(0, 1) = (ddy * dy * dy - ddy * dx * dx + 2 * dx * dy * ddx) /
                          (sqr_dx_dy * sqr_dx_dy);
      J_dot_theta(0, 2) = -dy / sqr_dx_dy;
      J_dot_theta(0, 4) = (-ddx * dx * dx + ddx * dy * dy - 2 * dx * dy * ddy) /
                          (sqr_dx_dy * sqr_dx_dy);
      J_dot_theta(0, 5) = dx / sqr_dx_dy;
      *H1 = 2 * (dot_theta - max_heading_rate_) * J_dot_theta;
    }
  }

  // hinge 误差：|dot_theta| 超上限才罚，且只在超出的方向罚
  if (dot_theta > max_heading_rate_) {
    error = dot_theta - max_heading_rate_;
    if (H1) {
      *H1 = J_dot_theta;
    }
  } else if (dot_theta < -max_heading_rate_) {
    error = -max_heading_rate_ - dot_theta;
    if (H1) {
      *H1 = -J_dot_theta;
    }
  } else {
    if (H1) {
      *H1 = Eigen::MatrixXd::Zero(1, 6);
    }
  }

  return gtsam::Vector1(error);
}

• 这个因子对应论文 Eq. (11e) 的「转角速度上限」约束，物理意义是「机器人转弯的角速度不能超过设定值，否则罚分」。注意官方代码里上面第一个 if (abs(dot_theta) > max_heading_rate_) 块算出的 *H1 会被下面 hinge 分支覆盖（残留代码），真正生效的是 hinge 误差。

3-2-5 插值、高度平滑与小结

• 除了这些「点上」的因子，GPMP2 还有插值因子（Interpolate Factor），在相邻两个状态之间做插值，用 $t\in (0,\Delta t)$ 中间时刻的点来做障碍/航向角速率约束。插值公式来自 GP 的插值理论（LambdaAndPsi）：

$$\Psi =Q(\tau )\Phi (\Delta t-\tau {)}^{\top }{Q}^{-1}(\Delta t),\Lambda =\Phi (\tau )-\Psi \Phi (\Delta t)$$

• 中间状态 $x(\tau )=\Lambda x_k+\Psi x_{k+1}$，这样就保证了「相邻两点之间的整段轨迹」都被障碍和航向角速率约束覆盖，而不只是孤立地检查两个端点。

• 注意：这里的「插值」不是普通的几何插值（线性插值、三次样条之类），而是由 GP 先验推导、有物理含义的插值。LambdaAndPsi 里的 $\Psi$、$\Lambda$ 是 GP 后验均值的解析解，而 GP 先验选的是 WNOJ（白噪声加加速度）——所以插出来的整段轨迹天然就是五次多项式，等价于「最小化 jerk（加加速度）」的最平滑曲线。换句话说，LambdaAndPsi 不是随手挑的插值核，而是「让加加速度最小」这个物理目标在数学上的必然结果。

• 优化用 GTSAM 的 LevenbergMarquardt 迭代求解。求解完 $x$、$y$ 两轴之后，$z$ 轴（高度）是单独用一条 5 次样条去平滑的（height_smoother_，基于 OSQP），并且把天花板高程当作上界，保证高度轨迹不顶头：

// planner/lib/src/trajectory_optimization/gpmp_optimizer/gpmp_optimizer.cc  （官方源码，已加中文注释）
// 先优化 x-y 平面轨迹，再用高度平滑器平滑 z 轴，并受天花板约束
for (int i = 0; i < opt_layers_.size(); ++i) {
    opt_height_(i) =
        map_->GetHeight(opt_layers_(i), trajectory_(i, 0), trajectory_(i, 3)) +
        reference_height_;
    opt_ceiling_(i) =
        map_->GetCeiling(opt_layers_(i), trajectory_(i, 0), trajectory_(i, 3));
}
opt_height_ = height_smoother_.Smooth(opt_height_, opt_ceiling_, tau, N, dt);

• 小结一下这段的对应关系：

论文 Eq. (10)(11)	代码实现（GPMP2）	作用
五阶多项式 / 最小 jerk	WNOJ 先验 + GP 先验因子	轨迹平滑
可通行约束	GPObstacleFactor	避开高代价/障碍
转角速度上限	GPHeadingRateFactor	限制转弯速率
高度贴合参考	OSQP 5 次样条平滑 $z$	高度平滑 + 不顶头

说人话：A* 给我们一串「折线拐点」，GPMP2 把这些拐点当成橡皮筋上的钉子，然后松手——橡皮筋会在「平滑（jerk 小）」「别撞障碍」「别急转弯」这三股力量的拉扯下，自动滑成一条又顺又不撞墙的曲线。那股「平滑」的力量，就是「白噪声加加速度」的高斯过程先验，它天然就是五次多项式。

3-3 完整流程串讲

• 最后我们把整条链路串起来，从「喂入点云」到「输出轨迹」，看一遍数据是怎么流动的。

• 顶层入口是 planner/scripts/plan.py。注意「加载 Tomogram」和「初始化规划器」其实是同一件事——loadTomogram 内部就读 pickle、拆通道、算 gateway、并调用 initPlanner 建好规划器；随后 plan() 一次完成「A* 搜索 + 轨迹优化」：

# planner/scripts/plan.py  （官方源码，已加中文注释）
planner = TomogramPlanner(cfg)

def pct_plan():
    planner.loadTomogram(tomo_file)                 # 1. 加载 Tomogram（内部已算 gateway + 建规划器）

    traj_3d = planner.plan(start_pos, end_pos)      # 2. A* 搜索 + 轨迹优化
    if traj_3d is not None:
        path_pub.publish(traj2ros(traj_3d))         # 3. 发布轨迹
        print("Trajectory published")


if __name__ == '__main__':
    rospy.init_node("pct_planner", anonymous=True)

    pct_plan()

    rospy.spin()

• loadTomogram 把 pickle 里的 Tomogram 拆成 5 个通道（还记得 point2map 返回的 6 个量里前 5 个吗）：

# planner/scripts/planner_wrapper.py  loadTomogram() 尾部（官方源码，已加中文注释）
trav = tomogram[0]      # 可通行代价 c^T
trav_gx = tomogram[1]   # 代价沿 x 的梯度（给障碍因子算雅可比用）
trav_gy = tomogram[2]   # 代价沿 y 的梯度
elev_g = tomogram[3]    # 地面高程 e^G
elev_g = np.nan_to_num(elev_g, nan=-100)   # 无地面 -> -100
elev_c = tomogram[4]    # 天花板高程 e^C
elev_c = np.nan_to_num(elev_c, nan=1e6)    # 露天 -> 1e6

• initPlanner 接着算 gateway（前面 3-1 已经讲过），并构造出 OfflineElePlanner。规划的核心在 planner/lib/src/ele_planner/offline_ele_planner.cc 的 Plan()：

// planner/lib/src/ele_planner/offline_ele_planner.cc  （官方源码，已加中文注释）
bool OfflineElePlanner::Plan(const Eigen::Vector3i& start,
                             const Eigen::Vector3i& goal, const bool optimize) {
  if (!path_finder_.Search(start, goal)) {        // 1. 多层 A* 搜索
    printf("A star Failed!\n");
    return false;
  }

  if (optimize) {
    path_ = path_finder_.GetPathPoints();         // 2. 把搜索结果整理成 PathPoint
    path_.front().ref_v = 1;
    path_.back().ref_v = 1;

    bool success = false;
    if (use_quintic_) {                           // use_quintic=True 用 WNOJ（6 维五次多项式）
      success = trajectory_optimizer_wnoj_.GenerateTrajectory(path_, 200);
    } else {
      success = trajectory_optimizer_.GenerateTrajectory(path_, 200);
    }

    return success;
  }

  return true;
}

• 两步走，清晰对应「图搜索 + 优化」的两段式结构。GenerateTrajectory 内部再把优化的轨迹点换算回世界坐标（transTrajGrid2Map），输出最终的 3D 轨迹。

• 到这里，一条完整的数据链路就闭环了：

---

附录

附录一：3D 切片示意图生成代码

• 下面是 2-2 节那张 3D 图的生成代码：左图用几个水平半透明平面去切一栋多层点云（一楼地面 + 斜坡楼梯 + 二楼地面 + 天花板），右图取其中 $z=1.5$ 这一层，把平面下方的点标成蓝色（地面）、上方的点标成橙色（天花板）。
• 代码依赖 mpl_toolkits.mplot3d（matplotlib 自带）。
  

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D  # noqa: F401   # 注册 3D 投影

# 中文字体（若你的系统没有 Noto Sans CJK，可换成 SimHei / PingFang SC 等本机中文字体）
plt.rcParams['font.sans-serif'] = ['Noto Sans CJK JP', 'Noto Sans CJK SC', 'SimHei']
plt.rcParams['axes.unicode_minus'] = False

rng = np.random.default_rng(3)

def surface_points(xmin, xmax, ymin, ymax, z_fn, n=18, jitter=0.03):
    xs = np.linspace(xmin, xmax, n)
    ys = np.linspace(ymin, ymax, int(n * (ymax - ymin) / (xmax - xmin)))
    xx, yy = np.meshgrid(xs, ys)
    zz = np.vectorize(z_fn)(xx, yy)
    xx = xx + rng.uniform(-0.08, 0.08, xx.shape)
    yy = yy + rng.uniform(-0.08, 0.08, yy.shape)
    zz = zz + rng.uniform(-jitter, jitter, zz.shape)
    return xx.ravel(), yy.ravel(), zz.ravel()

# 一楼地面 (z=0)
x1, y1, z1 = surface_points(0, 10, 0, 8, lambda x, y: 0.0)
# 二楼地面 (z=1，只在 x>6 半区)
x2, y2, z2 = surface_points(6, 10, 0, 8, lambda x, y: 1.0)
# 斜坡楼梯 (x 4->6, z 0->1, 只在 y<4)
def ramp(x, y):
    return np.where(x <= 4, 0.0, np.where(x >= 6, 1.0, (x - 4) * 0.5))
x3, y3, z3 = surface_points(4, 6, 0, 4, ramp)
# 天花板 (z=3)
x4, y4, z4 = surface_points(0, 10, 0, 8, lambda x, y: 3.0)

X = np.concatenate([x1, x2, x3, x4])
Y = np.concatenate([y1, y2, y3, y4])
Z = np.concatenate([z1, z2, z3, z4])

slice_zs = [0.5, 1.5, 2.5]
slice_colors = ['#d62728', '#2ca02c', '#9467bd']

fig = plt.figure(figsize=(12.5, 5.2))

# ---------- 左图：第一步「水平切片平面切 3D 点云」 ----------
ax = fig.add_subplot(121, projection='3d')
ax.scatter(X, Y, Z, s=1.5, c='#9aa0a6', depthshade=False, label='点云')
gx, gy = np.meshgrid(np.linspace(0, 10, 2), np.linspace(0, 8, 2))
for c, sz in zip(slice_colors, slice_zs):
    ax.plot_surface(gx, gy, np.full_like(gx, sz), color=c, alpha=0.28, shade=False)
ax.set_xlabel('x (m)'); ax.set_ylabel('y (m)'); ax.set_zlabel('z (m)')
ax.set_title('第一步：水平切片平面（间隔 $d_s$）')
ax.set_xlim(0, 10); ax.set_ylim(0, 8); ax.set_zlim(-0.3, 3.3)
ax.view_init(elev=24, azim=-58)

# ---------- 右图：第二步「某层切片的地面 / 天花板」 ----------
ax = fig.add_subplot(122, projection='3d')
s = 1.5
below = Z <= s
above = Z > s
ax.scatter(X[below], Y[below], Z[below], s=2.0, c='#1f77b4', depthshade=False, label='平面下方点 → 地面')
ax.scatter(X[above], Y[above], Z[above], s=2.0, c='#ff7f0e', depthshade=False, label='平面上方点 → 天花板')
ax.plot_surface(gx, gy, np.full_like(gx, s), color='#d62728', alpha=0.35, shade=False)
ax.set_xlabel('x (m)'); ax.set_ylabel('y (m)'); ax.set_zlabel('z (m)')
ax.set_title('第二步：以 $z=1.5$ 这一层为例（蓝=地面，橙=天花板）')
ax.set_xlim(0, 10); ax.set_ylim(0, 8); ax.set_zlim(-0.3, 3.3)
ax.legend(loc='upper left', fontsize=8, markerscale=4)
ax.view_init(elev=24, azim=-58)

plt.tight_layout()
plt.show()

• 从右图可以直接看出「水平切片」的含义：同一个切片平面把点云分成了「脚下（地面）」和「头顶（天花板）」两部分，这一刀切出来的横截面，就是一层 Tomogram。

附录二：侧视图切片示意图生成代码

• 为了把 2-2 节的「切平面 → 投影 → 断层图」三步用侧视图看得更细，这里给一段 matplotlib 代码，画一个三连图：
  • 左图是第一步「切平面」：多层结构的侧视点云（一楼地面 + 斜坡楼梯 + 二楼地面 + 天花板 + 一处悬挑横梁），红虚线是间隔 $d_s$ 的切片平面；
  • 中图是第二步「投影」：以 $z=1.5$ 这一层为例，平面下方的点（蓝）竖直向上投影得到地面 $e^G$（取最高），平面上方的点（橙）竖直向下投影得到天花板 $e^C$（取最低）；
  • 右图是第三步「断层图」：该层沿 $x$ 的 $e^G$、$e^C$ 曲线，绿色阴影就是净空 $d^I=e^C-e^G$，横梁下探处净空明显收窄。

import numpy as np
import matplotlib.pyplot as plt

# 中文字体（若你的系统没有 Noto Sans CJK，可换成 SimHei / PingFang SC 等本机中文字体）
plt.rcParams['font.sans-serif'] = ['Noto Sans CJK JP', 'Noto Sans CJK SC', 'SimHei']
plt.rcParams['axes.unicode_minus'] = False

# ---------- 1. 构造多层结构点云（侧视图，x-z 平面） ----------
rng = np.random.default_rng(3)
xs, zs = [], []

def add_surface(x_range, z_fn, n_pt=6, jitter=0.02):
    for x in x_range:
        z = z_fn(x)
        for _ in range(n_pt):
            xs.append(x + rng.uniform(-0.06, 0.06))
            zs.append(z + rng.uniform(-jitter, jitter))

add_surface(np.linspace(0, 4, 50), lambda x: 0.0)           # 一楼地面
add_surface(np.linspace(4, 6, 25), lambda x: (x - 4) * 0.5) # 斜坡（楼梯）
add_surface(np.linspace(6, 10, 50), lambda x: 1.0)          # 二楼地面
add_surface(np.linspace(0, 10, 120), lambda x: 3.0)         # 天花板
add_surface(np.linspace(5.2, 6.8, 20), lambda x: 2.2)       # 悬挑横梁（局部下探）

X = np.array(xs)
Z = np.array(zs)

# 切片平面：z_min + k * d_s
z_min, d_s, n_slice = 0.5, 0.5, 5
slices = z_min + np.arange(n_slice) * d_s

fig, axes = plt.subplots(1, 3, figsize=(15, 4.6))

# ---------- 左图：第一步「切平面」 ----------
ax = axes[0]
ax.scatter(X, Z, s=4, c='#9aa0a6', label='点云')
for s in slices:
    ax.axhline(s, ls='--', lw=1.0, color='#d62728', alpha=0.7)
ax.set_xlabel('x (m)'); ax.set_ylabel('z (m)')
ax.set_title('第一步：切平面（间隔 $d_s$）')
ax.set_ylim(-0.4, 3.6); ax.legend(loc='lower right', fontsize=9); ax.grid(alpha=0.25)

# ---------- 中图：第二步「投影」（以 z=1.5 这一层为例） ----------
s = 1.5
ax = axes[1]
below = Z <= s          # 切片平面下方的点 -> 地面
above = Z > s           # 切片平面上方的点 -> 天花板
ax.scatter(X[below], Z[below], s=4, c='#1f77b4', label='平面下方点 → 地面')
ax.scatter(X[above], Z[above], s=4, c='#ff7f0e', label='平面上方点 → 天花板')
ax.axhline(s, ls='-', lw=1.2, color='#d62728')

# 分 bin 求每个柱子的 eG / eC，并画竖直投影线（投影点画在真实取到 max/min 的点上）
bins = np.linspace(0, 10, 21)
for xl, xr in zip(bins[::2], bins[2::2]):    # 隔一个 bin 画一条，避免太密
    m = (X >= xl) & (X < xr)
    mb = m & below
    ma = m & above
    if mb.any():                             # 地面：该 bin 内平面下方最高点
        j = int(np.argmax(Z[mb]))
        gx, gz = X[mb][j], Z[mb][j]          # 该点的真实 (x, z)，落在点云上
        ax.plot([gx, gx], [gz, s], ls=':', lw=0.8, color='#1f77b4')
        ax.plot(gx, gz, 'o', ms=4, color='#1f77b4')
    if ma.any():                             # 天花板：该 bin 内平面上方最低点
        j = int(np.argmin(Z[ma]))
        cx, cz = X[ma][j], Z[ma][j]          # 该点的真实 (x, z)，落在点云上
        ax.plot([cx, cx], [cz, s], ls=':', lw=0.8, color='#ff7f0e')
        ax.plot(cx, cz, 'o', ms=4, color='#ff7f0e')

ax.annotate('$e^G$ = 下方最高点', xy=(1.5, 0.0), xytext=(0.2, -0.42),
            fontsize=9, color='#1f77b4')
ax.annotate('$e^C$ = 上方最低点', xy=(9.0, 3.0), xytext=(6.8, 3.38),
            fontsize=9, color='#ff7f0e')
ax.set_xlabel('x (m)'); ax.set_ylabel('z (m)')
ax.set_title('第二步：投影得到 $e^G$ / $e^C$（$z=1.5$ 这一层）')
ax.set_ylim(-0.4, 3.6); ax.legend(loc='lower right', fontsize=9); ax.grid(alpha=0.25)

# ---------- 右图：第三步「断层图」：该层的 eG / eC 曲线 ----------
ax = axes[2]
eG, eC = [], []
for xl, xr in zip(bins[:-1], bins[1:]):
    m = (X >= xl) & (X < xr)
    eG.append(Z[m & below].max() if (m & below).any() else np.nan)
    eC.append(Z[m & above].min() if (m & above).any() else np.nan)
xc = (bins[:-1] + bins[1:]) / 2
eG = np.array(eG); eC = np.array(eC)
ax.fill_between(xc, eG, eC, color='#2ca02c', alpha=0.25, label='净空 $d^I=e^C-e^G$')
ax.plot(xc, eG, '-o', ms=3, lw=1.5, color='#1f77b4', label='地面 $e^G$')
ax.plot(xc, eC, '-o', ms=3, lw=1.5, color='#ff7f0e', label='天花板 $e^C$')
ax.axhline(s, ls='--', lw=0.8, color='#d62728', alpha=0.6)
ax.set_xlabel('x (m)'); ax.set_ylabel('z (m)')
ax.set_title('第三步：断层图（该层的 $e^G$ / $e^C$）')
ax.set_ylim(-0.4, 3.6); ax.legend(loc='lower right', fontsize=9); ax.grid(alpha=0.25)

plt.tight_layout()
plt.show()

附录三：2-5 节四张代价图的生成代码

净空代价（2-5-1）：

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""图1：净空代价 c^I vs d^I"""
import numpy as np
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt
import time

plt.rcParams['font.sans-serif'] = ['Noto Sans CJK JP', 'Noto Sans CJK SC', 'SimHei', 'DejaVu Sans']
plt.rcParams['axes.unicode_minus'] = False

OUT = "/home/lzh/postgraduate0/"
cB = 50.0
d_min = 0.50      # interval_min
d_ref = 0.65      # interval_free
alpha_d = 20

t = time.strftime("%Y%m%d%H%M%S", time.localtime(time.time()))
fname = OUT + "Pasted image " + t + ".png"

fig, ax = plt.subplots(figsize=(6.4, 3.8))
d = np.linspace(0.35, 0.80, 900)
cI = np.where(d < d_min, cB, np.maximum(0.0, alpha_d * (d_ref - d)))

ax.plot(d, cI, color='#1f77b4', lw=2.5)
ax.axvline(d_min, color='#d62728', ls='--', lw=1.2)
ax.axvline(d_ref, color='#2ca02c', ls='--', lw=1.2)
ax.axhline(cB, color='#d62728', ls=':', lw=1.0)

ax.annotate('$d_{min}=0.50$\n净空不足→障碍 $c^B$=50',
            xy=(0.49, 30), xytext=(0.365, 36),
            fontsize=9, color='#d62728',
            arrowprops=dict(arrowstyle='->', color='#d62728'))
ax.annotate('$d_{ref}=0.65$\n净空充足→代价归零',
            xy=(0.66, 1.5), xytext=(0.66, 16),
            fontsize=9, color='#2ca02c',
            arrowprops=dict(arrowstyle='->', color='#2ca02c'))

# 断崖标注
ax.annotate('净空代价 = $c^B$', xy=(0.43, cB), xytext=(0.355, 44),
            fontsize=9, color='#d62728')
ax.annotate('$c^I=\\alpha_d(d_{ref}-d^I)$', xy=(0.57, 2.0), xytext=(0.52, 8),
            fontsize=10, color='#1f77b4')

ax.set_xlabel('净空 $d^I$ (m)')
ax.set_ylabel('代价 $c^I$')
ax.set_title('净空代价（Interval Cost）', fontsize=12)
ax.set_xlim(0.35, 0.80)
ax.set_ylim(-2, 54)
ax.grid(alpha=0.3)
fig.tight_layout()
fig.savefig(fname, dpi=130)
plt.close(fig)

print(fname)

地形代价（2-5-2）：

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""图2：地形代价——平地(缓坡)二次代价 + 陡坡障碍封顶，删掉中间空白"""
import numpy as np
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt
import time

plt.rcParams['font.sans-serif'] = ['Noto Sans CJK JP', 'Noto Sans CJK SC', 'SimHei', 'DejaVu Sans']
plt.rcParams['axes.unicode_minus'] = False

OUT = "/home/lzh/postgraduate0/"
cB = 50.0
alpha_s = 15
theta_s = 0.0507   # step_stand

t = time.strftime("%Y%m%d%H%M%S", time.localtime(time.time()))
fname = OUT + "Pasted image " + t + ".png"

fig, ax = plt.subplots(figsize=(6.4, 4.0))

# 平地/缓坡（m_grad < theta_s）：蓝色实线，二次增长
m1 = np.linspace(0, theta_s, 200)
c1 = alpha_s * (m1 / theta_s) ** 2
ax.plot(m1, c1, color='#1f77b4', lw=2.5, label='平地/缓坡：$c^G=\\alpha_s(m_{grad}/\\theta_s)^2$')

# 陡坡障碍：封顶线
ax.axhline(cB, color='#d62728', ls=':', lw=1.4, label='$c^B=50$（障碍）')

# 可站立坡度上限竖线
ax.axvline(theta_s, color='#2ca02c', ls='--', lw=1.2)

# 从 theta_s 起向右，坡度太陡 → 障碍区（不再留空白，紧接平缓坡）
ax.fill_betweenx([0, 56], theta_s, 0.20, color='#d62728', alpha=0.10)

# 在 theta_s 处画一条竖直虚线，表示从「平地」跳到「障碍」
ax.plot([theta_s, theta_s], [c1[-1], cB], color='#ff7f0e', ls='--', lw=1.5)

ax.annotate('$\\theta_s\\approx0.05$\n可站立坡度上限',
            xy=(theta_s, 15), xytext=(0.075, 34),
            fontsize=9, color='#2ca02c',
            arrowprops=dict(arrowstyle='->', color='#2ca02c'))
ax.annotate('坡度太陡（$m_{xy}>\\theta_b$）\n→ 障碍 $c^B$',
            xy=(0.13, 50), xytext=(0.135, 40),
            fontsize=9, color='#d62728', ha='center',
            arrowprops=dict(arrowstyle='->', color='#d62728'))

ax.set_xlabel('坡度 $m$（高程梯度）')
ax.set_ylabel('代价 $c^G$')
ax.set_title('地形代价（Ground Cost）', fontsize=12)
ax.set_xlim(0, 0.20)
ax.set_ylim(-2, 56)
ax.grid(alpha=0.3)
ax.legend(fontsize=8, loc='upper left')
fig.tight_layout()
fig.savefig(fname, dpi=130)
plt.close(fig)

print(fname)

台阶可跨越性（2-5-3）：

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""图3：台阶剖面（左）+ 墙/台阶邻域对比（右上/右下）——修正版"""
import numpy as np
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt
from matplotlib.patches import Rectangle
import time

plt.rcParams['font.sans-serif'] = ['Noto Sans CJK JP', 'Noto Sans CJK SC', 'SimHei', 'DejaVu Sans']
plt.rcParams['axes.unicode_minus'] = False

OUT = "/home/lzh/postgraduate0/"
step_h = 0.17
step_w = 0.35
n_step = 4

t = time.strftime("%Y%m%d%H%M%S", time.localtime(time.time()))
fname = OUT + "Pasted image " + t + ".png"

fig = plt.figure(figsize=(9.4, 4.8))
gs = fig.add_gridspec(2, 2, width_ratios=[1.4, 1.0], height_ratios=[1, 1],
                      hspace=0.55, wspace=0.32)
ax1 = fig.add_subplot(gs[:, 0])
ax_wall = fig.add_subplot(gs[0, 1])
ax_stair = fig.add_subplot(gs[1, 1])

# ---- 左：台阶侧视剖面 ----
ax1.plot([0, step_w], [0, 0], color='#2ca02c', lw=3, label='可站立（平缓）')
for i in range(n_step):
    x_rise = (i + 1) * step_w
    z_top = (i + 1) * step_h
    z_bot = i * step_h
    ax1.plot([x_rise, x_rise], [z_bot, z_top], color='#d62728', lw=3)
    ax1.plot([x_rise, x_rise + step_w], [z_top, z_top], color='#2ca02c', lw=3)
    if i == n_step - 1:
        ax1.plot([x_rise + step_w, x_rise + step_w + 1.0],
                 [z_top, z_top], color='#2ca02c', lw=3)
ax1.plot([], [], color='#d62728', lw=3, label='陡峭（不可站立）')

# 邻域窗口（7×7 = 0.7m 见方），中心在第二级立板
cx, cz = 0.70, 0.255
win = 0.35
ax1.add_patch(Rectangle((cx - win, cz - win), 2 * win, 2 * win,
                        fill=False, edgecolor='#ff7f0e', lw=2, ls='--'))
# 标注放在右上角空白处（顶层平台上方），避开台阶与图例
ax1.annotate('邻域窗口 7×7\n统计可站立比例 $p_s$',
             xy=(cx + win, cz + win), xytext=(2.05, 0.92),
             fontsize=8.5, color='#ff7f0e', ha='center',
             arrowprops=dict(arrowstyle='->', color='#ff7f0e',
                             connectionstyle='arc3,rad=-0.25'))

ax1.set_xlabel('水平距离 (m)')
ax1.set_ylabel('高程 (m)')
ax1.set_title('台阶剖面：踏板可站立、立板陡峭', fontsize=10.5)
ax1.legend(fontsize=8, loc='upper left')
ax1.set_xlim(-0.3, 3.2)
ax1.set_ylim(-0.3, 1.15)
ax1.set_aspect('equal')
ax1.grid(alpha=0.2)

# ---- 右上：墙（全部陡峭，p_s = 0）----
# ---- 右下：台阶（中间一列立板，两侧踏板，p_s 高）----
N = 7
def draw_grid(ax, flat, title, sub):
    for i in range(N):
        for j in range(N):
            c = '#2ca02c' if flat[i, j] else '#d62728'
            ax.add_patch(Rectangle((j, N - 1 - i), 1, 1, facecolor=c,
                                   edgecolor='white', lw=0.6))
    ax.set_xlim(0, N); ax.set_ylim(0, N)
    ax.set_aspect('equal'); ax.set_xticks([]); ax.set_yticks([])
    ax.set_title(title, fontsize=9.5)
    ax.text(N / 2, -1.0, sub, ha='center', va='top', fontsize=8.5)

# 墙：全红（竖直墙面，每个栅格都陡峭）→ p_s = 0
flat_wall = np.zeros((N, N), bool)
draw_grid(ax_wall, flat_wall, '墙：$p_s=0<0.20$ → 障碍 $c^B$',
          '邻域内全是陡峭格（竖直墙面）')

# 台阶：中间一列是立板（陡峭红），两侧是踏板（平缓绿）→ p_s = 42/49 ≈ 0.86
flat_stair = np.ones((N, N), bool)
flat_stair[:, N // 2] = False
draw_grid(ax_stair, flat_stair, '台阶：$p_s\\approx0.86>0.20$ → 可跨越',
          '立板两侧是踏板，可站立比例高')

fig.tight_layout()
fig.savefig(fname, dpi=130)
plt.close(fig)

print(fname)

安全膨胀（2-5-5）：

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""图5：安全膨胀——左：核函数曲线；右：障碍膨胀热图（hot 正序，越亮代价越高）"""
import numpy as np
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt
import time

plt.rcParams['font.sans-serif'] = ['Noto Sans CJK JP', 'Noto Sans CJK SC', 'SimHei', 'DejaVu Sans']
plt.rcParams['axes.unicode_minus'] = False

OUT = "/home/lzh/postgraduate0/"
cB = 50.0
d_inf = 0.2
d_sm = 0.4
r_g = 0.10

t = time.strftime("%Y%m%d%H%M%S", time.localtime(time.time()))
fname = OUT + "Pasted image " + t + ".png"

fig, axes = plt.subplots(1, 2, figsize=(8.6, 3.8))
ax1, ax2 = axes

# 左：核函数 K(d)
d = np.linspace(0, 0.8, 900)
K = np.clip(1 - (d - d_inf) / (d_sm + r_g), 0.0, 1.0)
ax1.plot(d, K, color='#1f77b4', lw=2.5)
ax1.axvline(d_inf, color='#2ca02c', ls='--', lw=1.2)
ax1.axvline(d_sm + r_g, color='#d62728', ls='--', lw=1.2)
ax1.annotate('$d_{inf}=0.2$\n全额继承代价', xy=(d_inf, 1.0),
             xytext=(0.02, 0.78), fontsize=9, color='#2ca02c',
             arrowprops=dict(arrowstyle='->', color='#2ca02c'))
ax1.annotate('$d_{sm}+r_g=0.5$\n衰减到 0', xy=(0.5, 0.0),
             xytext=(0.54, 0.22), fontsize=9, color='#d62728',
             arrowprops=dict(arrowstyle='->', color='#d62728'))
ax1.set_xlabel('距障碍栅格距离 $d_{mn}$ (m)')
ax1.set_ylabel('核权重 $K(m,n)$')
ax1.set_title('膨胀核函数 $K(m,n)$', fontsize=11)
ax1.set_xlim(0, 0.8); ax1.set_ylim(-0.08, 1.12)
ax1.grid(alpha=0.3)

# 右：障碍点膨胀成安全区（hot 正序：亮=代价高）
n = 200
yy, xx = np.mgrid[-1:1:n * 1j, -1:1:n * 1j]
dist = np.sqrt(xx ** 2 + yy ** 2)
cost = np.zeros_like(dist)
cost[dist < 0.06] = cB                      # 中心障碍
K2 = np.clip(1 - (dist - d_inf) / (d_sm + r_g), 0.0, 1.0)
infl = cB * K2
cost = np.maximum(cost, infl)               # 膨胀：越靠近中心代价越高

im = ax2.imshow(cost, extent=[-1, 1, -1, 1], origin='lower',
                cmap='hot', vmin=0, vmax=cB)
ax2.set_title('障碍膨胀成安全区（越亮代价越高）', fontsize=11)
ax2.set_xlabel('x (m)'); ax2.set_ylabel('y (m)')
cb = fig.colorbar(im, ax=ax2, fraction=0.046, pad=0.04)
cb.set_label('代价', fontsize=9)
ax2.add_patch(plt.Circle((0, 0), 0.06, fill=False, ec='#2ca02c', lw=1.5))
ax2.add_patch(plt.Circle((0, 0), d_sm + r_g, fill=False, ec='#d62728', lw=1.5, ls='--'))
ax2.annotate('障碍', xy=(0.0, 0.0), xytext=(-0.9, 0.7), fontsize=9, color='#2ca02c',
             arrowprops=dict(arrowstyle='->', color='#2ca02c'))
ax2.annotate('安全边界 $d_{sm}+r_g$', xy=(0.5 / 1.414, 0.5 / 1.414), xytext=(0.25, 0.55),
             fontsize=9, color='#d62728',
             arrowprops=dict(arrowstyle='->', color='#d62728'))

fig.tight_layout()
fig.savefig(fname, dpi=130)
plt.close(fig)

print(fname)

附录四：2-6 节层简化 3D 图的生成代码

• 2-6 节「层简化」那张 3D 图的生成代码（左图 9 层含冗余层，右图合并成 3 层）。依赖 mpl_toolkits.mplot3d。
  

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""图6：层简化（Tomogram Simplification）——3D 演示大量冗余层被合并"""
import numpy as np
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D  # noqa: F401
from matplotlib.patches import Patch
import time

plt.rcParams['font.sans-serif'] = ['Noto Sans CJK JP', 'Noto Sans CJK SC', 'SimHei', 'DejaVu Sans']
plt.rcParams['axes.unicode_minus'] = False

OUT = "/home/lzh/postgraduate0/"
t = time.strftime("%Y%m%d%H%M%S", time.localtime(time.time()))
fname = OUT + "Pasted image " + t + ".png"

nx, ny, nz = 6, 6, 9
dz, gap = 0.9, 0.45
zmax = nz * (dz + gap)

def draw(ax, f, redundant, title, zlabels=None):
    for k in range(nz):
        for i in range(nx):
            for j in range(ny):
                if f[i, j, k]:
                    z = k * (dz + gap)
                    if redundant and k in redundant:
                        c, ec = '#d62728', '#7f0000'
                    else:
                        c, ec = '#1f77b4', '#0b3d61'
                    ax.bar3d(j, i, z, 0.85, 0.85, dz,
                             color=c, edgecolor=ec, linewidth=0.4, shade=False)
    ax.set_xlabel('x（栅格）')
    ax.set_ylabel('y（栅格）')
    ax.set_zlabel('层 k（高度）')
    ax.set_title(title, fontsize=11, pad=2)
    ax.set_xlim(0, nx); ax.set_ylim(0, ny); ax.set_zlim(0, zmax)
    ax.view_init(elev=15, azim=-58)
    ticks = [k * (dz + gap) + dz / 2 for k in range(nz)]
    ax.set_zticks(ticks)
    if zlabels is None:
        zlabels = [f'k={k}' for k in range(nz)]
    ax.set_zticklabels(zlabels, fontsize=7)
    ax.set_xticks(range(nx)); ax.set_yticks(range(ny))
    ax.tick_params(axis='both', labelsize=7)
    ax.dist = 10.5

fig = plt.figure(figsize=(11.5, 6.4))
ax1 = fig.add_subplot(121, projection='3d')
ax2 = fig.add_subplot(122, projection='3d')

# ---- 左图：简化前 9 层，层 1~7 都是重复的楼梯中段（冗余）----
f1 = np.zeros((nx, ny, nz), bool)
for k in range(nz):
    f1[:, 3, k] = True          # 楼梯通道（中间列），每层都有
f1[:3, :3, 0] = True            # 1 楼大厅（左下），只有层 0
f1[3:, 3:, 8] = True            # 2 楼（右上），只有层 8
draw(ax1, f1, redundant=set(range(1, 8)),
     title='简化前：9 层（中间 7 层是重复的楼梯中段）')

# ---- 右图：简化后，7 层重复层合并成 1 层中段 ----
f2 = np.zeros((nx, ny, nz), bool)
f2[:, 3, 0] = True              # 1 楼楼梯口
f2[:, 3, 4] = True              # 保留 1 层楼梯中段（代表）
f2[:, 3, 8] = True              # 2 楼楼梯口
f2[:3, :3, 0] = True
f2[3:, 3:, 8] = True
zlabels2 = [f'k={k}' for k in range(nz)]
for k in (1, 2, 3, 5, 6, 7):
    zlabels2[k] = '已删'
zlabels2[4] = 'k=4\n中段'
draw(ax2, f2, redundant=set(), title='简化后：3 层（重复层合并成 1 层中段）',
     zlabels=zlabels2)

# 标注冗余层
ax1.text3D(6.0, 6.0, 4 * (dz + gap) + dz / 2,
           '冗余层：重复的楼梯中段\n$M_k\\subset M_{k-1}\\cup M_{k+1}$',
           color='#d62728', fontsize=9, fontweight='bold')

# 图例放底部，避开顶部标题
fig.legend(handles=[
    Patch(facecolor='#1f77b4', edgecolor='#0b3d61', label='保留层（可通行栅格）'),
    Patch(facecolor='#d62728', edgecolor='#7f0000', label='冗余层（将被删除）'),
], loc='lower center', bbox_to_anchor=(0.5, -0.02), ncol=2,
    fontsize=9, frameon=False)

fig.text(0.5, 0.5, '层简化\n9 层 → 3 层\n(论文 46 层 → 5 层)',
         ha='center', va='center', fontsize=11, color='#555555',
         bbox=dict(fc='white', ec='#cccccc', boxstyle='round,pad=0.5'))

fig.suptitle('层简化（Tomogram Simplification）：大量重复层被上下层并集覆盖 → 合并删除',
             fontsize=13, y=0.99)
fig.subplots_adjust(left=0.02, right=0.98, top=0.88, bottom=0.12, wspace=0.05)
fig.savefig(fname, dpi=130)
plt.close(fig)

print(fname)

附录五：3-1-3 节 8 邻域九宫格图的生成代码

• 3-1-3 节「同层 8 邻域扩展」那张九宫格图的生成代码（中心红格 = 当前节点，周围 8 蓝格 = 同层邻居，绿箭头 = 正交方向距离 1、橙箭头 = 对角方向距离 $\sqrt{2}$）。

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""图7：同层 8 邻域扩展——九宫格（同一层平面）演示当前节点向周围 8 个方向搜索"""
import numpy as np
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt
from matplotlib.patches import Rectangle, Patch
import time

plt.rcParams['font.sans-serif'] = ['Noto Sans CJK JP', 'Noto Sans CJK SC', 'SimHei', 'DejaVu Sans']
plt.rcParams['axes.unicode_minus'] = False

OUT = "/home/lzh/postgraduate0/"
t = time.strftime("%Y%m%d%H%M%S", time.localtime(time.time()))
fname = OUT + "Pasted image " + t + ".png"

fig, ax = plt.subplots(figsize=(6.8, 7.0))

# 3x3 九宫格：中心红、8 邻居蓝
for i in range(3):
    for j in range(3):
        fc = '#d62728' if (i == 1 and j == 1) else '#1f77b4'
        ax.add_patch(Rectangle((j - 0.5, i - 0.5), 1, 1,
                               facecolor=fc, edgecolor='white', linewidth=2.5, zorder=2))

# 箭头：中心 -> 8 个邻居（正交绿/距离1，对角橙/距离√2）
for di in (-1, 0, 1):
    for dj in (-1, 0, 1):
        if di == 0 and dj == 0:
            continue
        c = '#2ca02c' if (abs(di) + abs(dj) == 1) else '#ff7f0e'
        ax.annotate('', xy=(1 + dj, 1 + di), xytext=(1, 1),
                    arrowprops=dict(arrowstyle='-|>', color=c, lw=2.6,
                                    mutation_scale=20, shrinkA=11, shrinkB=11),
                    zorder=3)

# 中心标注
ax.text(1, 1, '当前节点', ha='center', va='center', color='white',
        fontsize=10.5, fontweight='bold', zorder=4)

ax.set_xlim(-0.75, 2.75); ax.set_ylim(-0.75, 2.75)
ax.set_xticks([0, 1, 2]); ax.set_yticks([0, 1, 2])
ax.set_xlabel('x（栅格）', fontsize=10)
ax.set_ylabel('y（栅格）', fontsize=10)
ax.set_title('同层 8 邻域扩展：同一层平面内，当前节点向周围 8 个方向搜索', fontsize=12.5, pad=10)
ax.set_aspect('equal')
ax.tick_params(labelsize=9)

# 图例（底部）
fig.legend(handles=[
    Patch(facecolor='#d62728', label='当前节点（正在扩展）'),
    Patch(facecolor='#1f77b4', label='8 邻域（同一层）'),
    Patch(facecolor='#2ca02c', label='正交方向 · 距离 = 1'),
    Patch(facecolor='#ff7f0e', label='对角方向 · 距离 = √2'),
], loc='lower center', bbox_to_anchor=(0.5, -0.02), ncol=2,
    fontsize=9, frameon=False)

fig.subplots_adjust(left=0.08, right=0.96, top=0.90, bottom=0.14)
fig.savefig(fname, dpi=130)
plt.close(fig)
print(fname)

附录六：3-1-4 节 gateway 跨层 3D 图的生成代码

• 3-1-4 节「gateway」那张三联图的生成代码（左：真实 3D 点云台阶；中：多层图切片；右：代价俯视图 + gateway 标记）。依赖 mpl_toolkits.mplot3d。

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""图8：gateway 跨层——真实 3D 点云台阶 → 多层图切片 → 可上下楼区域的代价俯视图"""
import numpy as np
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D  # noqa: F401
from matplotlib.patches import Patch
import time

plt.rcParams['font.sans-serif'] = ['Noto Sans CJK JP', 'Noto Sans CJK SC', 'SimHei', 'DejaVu Sans']
plt.rcParams['axes.unicode_minus'] = False

OUT = "/home/lzh/postgraduate0/"
t = time.strftime("%Y%m%d%H%M%S", time.localtime(time.time()))
fname = OUT + "Pasted image " + t + ".png"

fig = plt.figure(figsize=(16.5, 5.8))
rng = np.random.default_rng(7)

n_levels = 3
tread_w = 1.5      # 每级踏板沿 x 的宽度
tread_h = 1.0      # 每级踏板的高度
tread_y = 1.0      # y 方向半宽
layer_colors = ['#1f77b4', '#2ca02c', '#ff7f0e']

# ============ 左图：真实 3D 点云台阶 ============
ax1 = fig.add_subplot(131, projection='3d')
pts = []
for L in range(n_levels):
    z = L * tread_h
    x0 = L * tread_w
    for _ in range(600):                       # 踏板（水平面）
        x = rng.uniform(x0, x0 + tread_w)
        y = rng.uniform(-tread_y, tread_y)
        pts.append([x, y, z + 0.01])
    if L < n_levels - 1:                        # 立板（垂直面，连接下一级）
        for _ in range(200):
            x = x0 + tread_w
            y = rng.uniform(-tread_y, tread_y)
            zz = rng.uniform(z, z + tread_h)
            pts.append([x, y, zz])
pts = np.array(pts)
ax1.scatter(pts[:, 0], pts[:, 1], pts[:, 2], c=pts[:, 2], cmap='viridis',
            s=2.5, alpha=0.8, linewidths=0)
ax1.set_xlabel('x'); ax1.set_ylabel('y'); ax1.set_zlabel('z（高度）')
ax1.set_title('真实 3D 点云台阶', fontsize=11.5)
ax1.view_init(elev=24, azim=-60)
ax1.set_xlim(-0.5, 4.8); ax1.set_ylim(-1.5, 1.5); ax1.set_zlim(0, 3.3)
ax1.tick_params(labelsize=8)

# ============ 中图：多层图（Tomogram）切片 ============
ax2 = fig.add_subplot(132, projection='3d')
for L in range(n_levels):
    z = L * tread_h + 0.5
    x0 = L * tread_w
    ax2.bar3d(x0, -tread_y, z - 0.06, tread_w, 2 * tread_y, 0.12,
              color=layer_colors[L], edgecolor='k', linewidth=0.4,
              alpha=0.9, shade=False)
    ax2.text(x0 + tread_w / 2, 0, z + 0.35, f'k={L}', ha='center',
             color=layer_colors[L], fontsize=10, fontweight='bold')
ax2.set_xlabel('x'); ax2.set_ylabel('y'); ax2.set_zlabel('z（层高度）')
ax2.set_title('多层图（Tomogram）：台阶被切成 3 个水平层', fontsize=11.5)
ax2.view_init(elev=24, azim=-60)
ax2.set_xlim(-0.5, 4.8); ax2.set_ylim(-1.5, 1.5); ax2.set_zlim(0, 3.3)
ax2.tick_params(labelsize=8)

# ============ 右图：代价俯视图 + gateway ============
ax3 = fig.add_subplot(133)
nx, ny = 80, 40
cost = np.full((ny, nx), 1.0)                 # 周围：高代价（不可通行）
x = np.linspace(-0.5, 5.0, nx)
y = np.linspace(-1.6, 1.6, ny)
X, Y = np.meshgrid(x, y)
stair = (X >= 0) & (X <= 4.5) & (Y >= -1) & (Y <= 1)
cost[stair] = 0.1                              # 台阶投影：低代价（可通行）
ax3.imshow(cost, origin='lower', extent=[-0.5, 5.0, -1.6, 1.6],
           cmap='Greens', vmin=0, vmax=1, aspect='auto')
# gateway 竖条（台阶各级交界处 = 可上下楼）
for gx in (1.5, 3.0):
    ax3.axvspan(gx - 0.13, gx + 0.13, color='#d62728', alpha=0.92)
    ax3.text(gx, 0.0, 'gateway\n可上下楼', ha='center', va='center',
             color='white', fontsize=8, fontweight='bold', rotation=90)
# 各级踏板标注
for L in range(n_levels):
    x0 = L * tread_w
    ax3.text(x0 + tread_w / 2, -1.22, f'k={L}\n低代价', ha='center', va='top',
             fontsize=8, color=layer_colors[L], fontweight='bold')
ax3.set_xlabel('x（俯视）'); ax3.set_ylabel('y（俯视）')
ax3.set_title('代价俯视图：gateway 标记可上下楼区域', fontsize=11.5)
ax3.set_yticks([-1, 0, 1])
ax3.tick_params(labelsize=8)
ax3.legend(handles=[
    Patch(facecolor='#d62728', label='gateway（可上下楼）'),
    Patch(facecolor='#74c476', label='可通行（低代价）'),
    Patch(facecolor='#0b3d1f', label='不可通行（高代价）'),
], loc='lower center', bbox_to_anchor=(0.5, -0.30), ncol=3, fontsize=8, frameon=False)

fig.suptitle('gateway 跨层：真实台阶 → 多层图切片 → 可上下楼区域的代价俯视图',
             fontsize=13.5, y=0.99)
fig.subplots_adjust(left=0.02, right=0.98, top=0.86, bottom=0.12, wspace=0.12)
fig.savefig(fname, dpi=130)
plt.close(fig)
print(fname)

---

总结

• 本文解读了 PCT_planner——一个把「点云断层扫描」引入全局路径规划的高效框架。核心思想一句话：用水平切片把复杂 3D 结构降维成多层 2.5D 地图，每层同时记录地面高程与天花板高程，再在多层地图上做 A 搜索和 GPMP2 轨迹优化。*

• 我们逐行梳理了它的完整链路：

  • Tomogram 构建：用 CUDA 原子操作 atomicMax/atomicMin，把点云切成多层「地面层 + 天花板层」；
  • 可通行性估计：净空代价 + 地形代价（坡度 / 台阶可跨越性）合成可通行代价，再做安全膨胀；
  • 层简化：删掉被上下层覆盖的冗余切片，把几十层压成几层；
  • 多层 A*：同层 8 邻居 + gateway 跨层切换，搜出折线路径；
  • GPMP2 轨迹优化：用 WNOJ 高斯过程先验（等价于五次多项式）平滑路径，叠加障碍因子与航向角速率因子，再单独平滑高度。

• 它的价值在于把「多层建筑、楼梯、坡道」这类传统方法很难处理的场景，用一套优雅的「切片 + 多层搜索」统一解决，并且离线/在线解耦，让在线部分又快又省。

• 限于篇幅，tomography 里还有不少 CUDA 内存布局、float16 量化、transTrajGrid2Map 坐标变换等工程细节没有展开，感兴趣的读者可以顺着本文的文件路径去仓库里继续深挖。

• 如有错误，欢迎指出！感谢观看！