文献：SE(2) Navigation Mesh
作者：Shuyang Shi, Kaixian Qu, Changan Chen, Ines Kast, Yuntao Ma, Marco Hutter
单位：Robotic Systems Lab, ETH Zurich
论文地址：https://arxiv.org/abs/2607.01454
项目主页：https://se2-navmesh.github.io/
关键词：SE(2) NavMesh、Yaw-dependent Traversability、Navigation Mesh、Footprint Mask、ASA、A*、String Pulling、Voxblox、Multi-level Navigation、Legged Robot

---

0. 摘要

这篇工作的核心不是再设计一个新的局部避障器，而是重新定义全局导航地图应该如何表达“可通行”。传统 NavMesh 通常把机器人近似为圆柱，因此同一位置是否可通行与 yaw 无关；对长宽差异明显的四足机器人、移动机械臂或矩形底盘而言，这会在窄门、狭窄走廊、楼梯、悬垂结构下产生明显保守性：某个位置可能“正着能过、横着不能过”，而经典 NavMesh 会因为圆形外接包络过大直接把整块区域删掉。SE(2) NavMesh 将二维表面位置与机器人朝向联合起来，以离散 yaw channel、连续 yaw footprint mask 和分层连通图显式编码 yaw-dependent traversability；在此基础上，作者提出 A* -String Pulling- A* （ASA）三阶段规划，通过第一次 A* 保证位置—朝向可行性，String Pulling 缩短几何路径，再用第二次 A* 重新优化朝向。论文同时给出基于 Voxblox 和 slab 局部更新的在线构建方法。实验表明，在 6 个 HM3D 场景中，其可通行面积相对经典 NavMesh 均增加超过 50%，受限环境中的 ASA 规划性能优于 RRT/RRT*/PRM 基线，真实四足机器人能够完成多楼层、窄走廊、门洞、0.8 m 窄通道以及悬垂障碍下的导航。

---

1. 这篇论文到底解决什么问题

1.1 关键科学问题：三维环境中的“可通行”并不是位置的二值函数

经典地面导航通常把可通行性写成

p ↦ { 0 , 1 } , \mathbf{p}\mapsto \{0,1\}, p↦{0,1},

即某个空间位置要么可通过，要么不可通过。但对具有非圆形 footprint 的机器人而言，更准确的关系应是

( p , ψ ) ↦ { 0 , 1 } , (\mathbf{p},\psi)\mapsto \{0,1\}, (p,ψ)↦{0,1},

其中 $\psi$ 为机器人 yaw。也就是说，同一位置在不同朝向下可能有完全不同的碰撞状态。

一个典型例子是窄门或窄楼梯平台：机器人沿机身长轴对准通道时能够通过，但横转 90° 后 footprint 会碰撞墙体。经典 NavMesh 为了消除 yaw 变量，通常采用机器人矩形 footprint 的外接圆作为碰撞包络；这样虽然查询简单，却会把只在部分朝向可通过的区域全部删除。

论文真正要回答的是：

1. 如何在复杂、多层三维表面上显式表示 $\mathbf{p}$ 与 $\psi$ 的联合可行域；
2. 如何在不退化为高密度连续 SE(2) 采样的前提下，保留 NavMesh 的高效图搜索优势；
3. 如何让该表示从在线点云流中增量更新，而不是只能离线从完整地图一次性构建；
4. 如何利用这种 yaw-aware 表示规划既短、又满足实际转身空间约束的路径。

论文将问题形式化为：机器人在三维表面网格 $\mathcal{M}\subset {\mathbb{R}}^3$ 上运动，状态为

$$\mathbf{x}=(\mathbf{p},\psi ),$$

其中 $\mathbf{p}\in \mathcal{M}$，$\psi \in S^1$，配置空间为

$$\mathcal{X}=\mathcal{M}\times S^1.$$

考虑机器人几何尺寸与地形通过能力后，自由可通行状态空间写为

X f r e e = { ( p , ψ ) ∈ X ∣ p ∈ P f r e e ( ψ ) } . \mathcal{X}_{\mathrm{free}}= \left\{(\mathbf{p},\psi)\in\mathcal{X}\mid \mathbf{p}\in\mathcal{P}_{\mathrm{free}}(\psi)\right\}. Xfree​={(p,ψ)∈X∣p∈Pfree​(ψ)}.

这里最重要的是 ${\mathcal{P}}_{\mathrm{free}}$ 显式依赖 $\psi$。论文的 SE(2) NavMesh，本质上就是对这个 yaw-dependent free space 的结构化离散近似。

图 1　SE(2) NavMesh 的真实机器人导航示例。来源：论文 Figure 1。窄通道、悬垂障碍以及跨楼层楼梯均由同一 yaw-aware NavMesh 表达；黄色为规划路径，蓝色为可通行多边形。

---

2. 为什么已有表示不够用

论文将现有全局导航表示的局限概括为一个“表达能力—规划效率”的矛盾。

2.1 点云

点云能忠实保存复杂三维几何，但缺少显式表面结构与邻接关系。若直接在点云上估计 traversability，需要做邻域搜索、法向/粗糙度分析、机器人 footprint 检查并构图；地图一大，计算量和数据结构复杂度迅速上升。

2.2 Occupancy / TSDF / ESDF

体素地图适合三维碰撞查询，但它关注空间占据，而不是“机器人应该踩在哪个表面以及表面之间如何连通”。地面机器人最终沿表面运动，因此仅靠自由空间体素仍需额外恢复地表结构与地形可通行属性。

2.3 2D 栅格与 2.5D elevation map

2D 栅格无法表达楼上楼下；单值 elevation map 在同一 $(x,y)$ 处只能保存一个高度，也无法天然表示桥下/楼板上下等多层重叠表面。

2.4 三角网格

Triangle mesh 有表面和邻接结构，理论上很适合地面机器人，但高精度场景需要大量三角形，直接在稠密 mesh 上进行全局图搜索开销较大。

2.5 经典 NavMesh

NavMesh 将连续可行表面压缩成相互连接的凸多边形区域，因此搜索效率很高；问题在于 Recast 一类经典方法通常用圆柱近似机器人，使 traversability 与 yaw 无关。对于长方形机器人，外接圆半径为

$$r_{\mathrm{circ}}=\frac{\sqrt{l_{\mathrm{robot}}^2+w_{\mathrm{robot}}^2}}{2},$$

它会显著大于机器人短边半宽，在狭窄区域产生不必要的保守膨胀。

论文指出，与现有方法相比，SE(2) NavMesh 同时支持多层环境、楼梯、yaw-dependent traversability、可配置 footprint、可配置通过能力和在线生成，这是其地图表示层面的完整目标。

---

3. 总体技术路线

论文系统由三条主线组成：

• **离线建图：**已有 triangle mesh → 局部几何提取 → voxelization → walkable voxel annotation → yaw feasibility evaluation → region polygon generation → SE(2) NavMesh；
• **在线建图：**点云流 → Voxblox 增量几何重建 → 受影响局部 slab 提取 → 复用离线生成流程局部更新；
• **路径规划：在生成的 yaw-layered NavMesh 上执行 ASA，即 Initial A → String Pulling → Yaw Refinement A。

图 2　SE(2) NavMesh 系统总览。来源：论文 Figure 2。离线与在线分支最终进入同一 Mesh Construction，再使用 ASA 完成位置与朝向联合规划。

从系统设计上看，这篇论文不是“先生成普通 NavMesh，再在路径上做一次碰撞检查”，而是把 yaw 可行性前置到地图生成阶段。因此规划器查询到的不是单一二维多边形，而是“这个多边形在哪些 yaw channel 下存在”。

---

4. SE(2) NavMesh 的核心表示

4.1 从圆柱近似改为 cuboid / footprint 近似

作者以四足机器人为例，用长度 $l_{\mathrm{robot}}$、宽度 $w_{\mathrm{robot}}$、高度 $h_{\mathrm{robot}}$ 描述机器人近似包围盒，盒体高度方向与重力方向对齐。相比外接圆柱，cuboid 在窄通道中能够保留机器人长宽不一致带来的方向性。

(a) 圆柱近似	(b) 长方体近似

图 3　机器人几何近似与 footprint mask。来源：论文 Figure 3。论文由圆柱近似转向 cuboid，并用离散/连续 yaw footprint mask 表达不同朝向下的平面占据区域。

需要强调：论文框架本身并不要求 footprint 一定是矩形。只要能够为给定 yaw 生成二维 footprint mask，理论上可以换成其他形状。

4.2 Yaw Channel：把连续朝向离散成层

连续 yaw 区间 $[0,2\pi )$ 被均匀离散为 $N_{\Psi }$ 个角度：

Ψ = { ψ 1 , ψ 2 , … , ψ N Ψ } , \Psi=\{\psi_1,\psi_2,\ldots,\psi_{N_\Psi}\}, Ψ={ψ1​,ψ2​,…,ψNΨ​​},

$${\psi }_i=i\frac{2\pi }{N_{\Psi }},$$

对应 channel index 集合

I Ψ = { 1 , 2 , … , N Ψ } . \mathcal{I}_\Psi=\{1,2,\ldots,N_\Psi\}. IΨ​={1,2,…,NΨ​}.

对表面位置 $\mathbf{p}$，第 $i$ 个 yaw channel 的可行性定义为

$$C_i(\mathbf{p})=\{\begin{array}{ll}1,& \text{机器人能以 }{\psi }_i\text{ 占据 }\mathbf{p},\\ 0,& \text{否则}.\end{array}$$

这一步把“朝向”从规划阶段的临时碰撞变量变成地图本身的属性。

4.3 关键细节：Continuous-Yaw Footprint Mask

如果只检查离散 yaw 的瞬时 footprint，会出现一个隐患：${\psi }_i$ 可行、${\psi }_{i+1}$ 也可行，不代表机器人从前者连续旋转到后者的过程中不会碰撞。

作者因此不只使用 isolated-yaw mask，而为每个 channel 构造覆盖一个 yaw bin 的continuous-yaw footprint mask。第 $i$ 个 mask 覆盖

$$\left[{\psi }_i-\frac{{\psi }_{\mathrm{interval}}}{2},{\psi }_i+\frac{{\psi }_{\mathrm{interval}}}{2}\right),{\psi }_{\mathrm{interval}}=\frac{2\pi }{N_{\Psi }}.$$

它不是单一角度下的矩形，而是机器人 footprint 在这段角度区间内旋转扫掠区域的并集。这样，如果同一位置对两个相邻 yaw channel 的 continuous-yaw mask 都可行，就能够保证相邻 channel 之间的原地旋转具有安全空间。

这是论文十分关键、又容易被忽略的一点：yaw layer 不只是离散朝向标签，还通过 swept footprint 为层间旋转提供了几何安全保证。

4.4 Safe / Restricted / Inaccessible 三类区域

对于一个可通行区域 ${\mathcal{R}}_j$，定义其可行 yaw channel 集合

$$\mathcal{I}({\mathcal{R}}_j)\subseteq {\mathcal{I}}_{\Psi }.$$

论文把区域分成：

• Safe region：所有 yaw 都可行，$\mathcal{I}({\mathcal{R}}_j)={\mathcal{I}}_{\Psi }$；
• Restricted region：只在部分 yaw 下可行，$\varnothing ⊊\mathcal{I}({\mathcal{R}}_j)⊊{\mathcal{I}}_{\Psi }$；
• Inaccessible：没有任何可行 yaw。

因此

$${\mathcal{R}}_{\mathrm{trav}}={\mathcal{R}}_{\mathrm{safe}}\cup {\mathcal{R}}_{\mathrm{restricted}}.$$

经典 NavMesh 通常只保留近似于这里的 safe 区域；SE(2) NavMesh 的增益恰恰来自把 restricted region 保留下来，并附带“在哪些 yaw 下可通过”的信息。

4.5 Yaw-Specific Traversability Layer

对每个 channel $i$，定义一层

L i = { R j ∈ R t r a v ∣ i ∈ I ( R j ) } . \mathcal{L}_i= \left\{ \mathcal{R}_j\in\mathcal{R}_{\mathrm{trav}} \mid i\in\mathcal{I}(\mathcal{R}_j) \right\}. Li​={Rj​∈Rtrav​∣i∈I(Rj​)}.

于是：

$${\mathcal{R}}_{\mathrm{trav}}=\bigcup _i{\mathcal{L}}_i,$$

$${\mathcal{R}}_{\mathrm{safe}}=\bigcap _i{\mathcal{L}}_i.$$

同一个物理多边形 ${\mathcal{R}}_j$ 可以在多个 yaw layer 中存在，记为 ${\mathcal{R}}_j^{(i)}$。如果这个区域只允许某几个朝向，那么它只会出现在对应层。

4.6 两种连通性：平移边与旋转边

论文把 NavMesh 的单一“区域邻接”扩展成两类边：

平移连通（Translational Connectivity）：同一个 yaw layer 内，相邻两个多边形共享非零长度边界时可连接。对应机器人保持 yaw 不变，从一个区域平移到另一个区域。

$${\mathrm{Conn}}_{\mathrm{trans}}\left({\mathcal{R}}_a^{(i)},{\mathcal{R}}_b^{(i)}\right)=1$$

当且仅当 ${\mathcal{R}}_a$ 与 ${\mathcal{R}}_b$ 的多边形边界存在有效重叠。

旋转连通（Rotational Connectivity）：同一个物理区域在相邻 yaw layer 中的副本相连接：

$${\mathrm{Conn}}_{\mathrm{rot}}\left({\mathcal{R}}_k^{(i)},{\mathcal{R}}_k^{(j)}\right)=1,j\equiv i\pm 1(\mathrm{mod}{N}_{\Psi }).$$

它表示机器人保持位置基本不变，在该区域内从一个 yaw channel 安全转到相邻 yaw channel。

图 4　Yaw-specific region 与导航图生成。来源：论文 Figure 5。先根据每个区域的可行 yaw channel 生成 layer-specific region，再在层内建立平移边、在相邻层之间建立旋转边。

这种图结构可以直观理解为：经典 NavMesh 是二维多边形拓扑图；SE(2) NavMesh 则把每个多边形沿 yaw 方向“复制成若干层”，再通过旋转边把这些层连接起来。

---

5. 离线 SE(2) NavMesh 如何生成

离线输入是环境 triangle mesh，输出为带 yaw feasibility 的多边形区域和导航图。作者复用了 Recast 的 TileMesh 思路，但把机器人几何检查改造成 yaw-aware 版本。

5.1 参数

机器人参数包括：

$$l_{\mathrm{robot}},w_{\mathrm{robot}},h_{\mathrm{robot}},h_{\mathrm{step}},{\theta }_{\mathrm{climb}}.$$

地图参数包括 voxel resolution、tile size 和 yaw layer 数 $N_{\Psi }$。

5.2 Step 1：预计算 Continuous-Yaw Footprint Masks

每个 yaw channel 对应一个 footprint mask，mask 的网格分辨率与地图 voxel 在水平面的分辨率一致。由于 mask 可预先计算，后续大量 voxel 判断不需要实时旋转几何模型。

5.3 Step 2：Tile Partitioning

地图在水平面被切分为 tile。每个 tile 只需要自身及一个带 border 的邻域几何就能独立生成局部 NavMesh，因此：

• 可以并行构建；
• 在线更新时可以只重建局部 tile / slab；
• 全局地图增长后不必每次全部重算。

border 的尺寸至少覆盖机器人 footprint 外接圆半径以及额外安全裕度，避免 tile 边界处因为缺少邻域几何而错误判定可行。

5.4 Step 3：Voxelization 与 Walkable Voxel Annotation

输入三角网格先体素化。沿竖直方向，相连的 occupied voxel 合并成 solid spans，solid span 之间的空隙形成 free space spans。再依据：

• 表面几何；
• 机器人高度 $h_{\mathrm{robot}}$；
• 最大跨越台阶 $h_{\mathrm{step}}$；
• 最大可爬坡角 ${\theta }_{\mathrm{climb}}$；

将 free-space span 判定为 walkable / non-walkable。每个可行 span 的底部 voxel 代表地表，形成 terrain voxel map。

这一处理让算法能够处理楼梯和多层表面，而不是只在平面 $(x,y)$ 栅格中进行障碍膨胀。

5.5 Step 4：Yaw Feasibility Evaluation

这是离线生成最核心的一步。

作者先构造每个 terrain voxel 到最近 locally-invalid voxel 的距离图 $D(v)$。locally-invalid 不只包括 non-walkable voxel，还包括邻接不可行/未知区域，以及与四邻域高度差超过通过约束的 voxel。

对矩形 footprint 定义：

$$r_{\mathrm{in}}=\frac{\min (l_{\mathrm{robot}},w_{\mathrm{robot}})}{2},$$

$$r_{\mathrm{circ}}=\frac{\sqrt{l_{\mathrm{robot}}^2+w_{\mathrm{robot}}^2}}{2}.$$

然后做三级快速判断：

1. 若 $D(v)<r_{\mathrm{in}}$：无论怎么转都放不下，直接 Inaccessible；
2. 若 $D(v)\ge r_{\mathrm{circ}}$：外接圆都放得下，因此任意 yaw 都安全，直接 Safe；
3. 只有满足

$$r_{\mathrm{in}}\le D(v)<r_{\mathrm{circ}}$$

的中间带区域，才需要逐个 yaw channel 进行 footprint mask 检查。

这种“内切圆—外接圆”两级筛选非常重要。它避免了对所有 voxel、所有 yaw 都进行完整 footprint 卷积，把计算集中在真正的狭窄边界区。

对中间区域 voxel $v$，将第 $i$ 个 continuous-yaw mask 的 reference cell 与 $v$ 对齐。只有 mask 内所有占据 cell 都对应 walkable voxel 时，$i$ 才加入 $\mathcal{I}(v)$。若最终 $\mathcal{I}(v)$ 为空则 inaccessible，否则 restricted。

5.6 Step 5：从 voxel 变成凸多边形 region

论文不直接在 voxel graph 上规划，而是进一步压缩表示：

1. 对 safe / restricted voxel 应用 watershed 分区；
2. 只有可行 yaw channel 集完全相同的 voxel 才允许归到同一个 region；
3. 不规则 region 通过 ear clipping 三角剖分；
4. 再按 Recast 流程合并为凸多边形；
5. 各 tile 局部结果合并成全局 SE(2) NavMesh，并建立平移/旋转连通。

这一步产生了一个很有价值的“自适应空间离散”性质：开阔区域的 yaw feasibility 几乎处处相同，可以合并成大多边形；狭窄区域 yaw feasibility 变化快，会自动产生更多、更小的多边形。因此计算资源被自然集中到真正需要高分辨率规划的区域。

---

6. ASA：A* –String Pulling– A* 路径规划

仅有 SE(2) NavMesh 还不够。如果直接在 edge-midpoint graph 上做一次 A* ，路径会沿多边形边中点产生明显折线；如果只做 String Pulling，又可能改变路径方向，使原本 A* 分配的 yaw 不再合理。ASA 用三阶段解决这一矛盾。

图 5　ASA 三阶段路径规划。来源：论文 Figure 7。第一次 A* 找到可行 layer-specific region sequence，String Pulling 缩短位置路径，第二次 A* 在固定/收缩后的位置走廊上重新优化 yaw。

6.1 第一阶段：Initial A*

首先根据 start/goal 的位置找到对应 traversable region，再依据 start/goal yaw 找到相应 yaw layer。图节点设置在 layer-specific polygon 的边界代表点（论文采用多边形边上的代表点，如 midpoint），并为查询临时加入 start/goal 节点。

平移代价

对第 $i$ 个 yaw layer，机器人局部纵向、横向单位向量分别为

$${\mathbf{n}}_{\mathrm{long}}=\left[\begin{array}{c}\cos {\psi }_i\\ \sin {\psi }_i\end{array}\right],{\mathbf{n}}_{\mathrm{lat}}=\left[\begin{array}{c}-\sin {\psi }_i\\ \cos {\psi }_i\end{array}\right].$$

将水平位移 $\Delta {\mathbf{p}}_{xy}$ 投影到机器人纵向和横向，平移边成本定义为估计运动时间：

$$\mathrm{Cost}({\mathbf{x}}_a,{\mathbf{x}}_b)=\frac{|\Delta {\mathbf{p}}_{xy}\cdot {\mathbf{n}}_{\mathrm{long}}|}{v_{\mathrm{long}}}+\frac{|\Delta {\mathbf{p}}_{xy}\cdot {\mathbf{n}}_{\mathrm{lat}}|}{v_{\mathrm{lat}}}.$$

这比简单欧氏距离更适合四足机器人，因为前后运动与侧移速度可以不同。实验中作者设置 $v_{\mathrm{long}}>v_{\mathrm{lat}}$，因此规划器会自然偏好少侧移的姿态安排。

旋转代价

相邻 yaw layer 的原地旋转成本为

$${\mathrm{Cost}}_{\mathrm{rot}}=\frac{{\psi }_{\mathrm{interval}}}{\omega }=\frac{2\pi }{N_{\Psi }\omega },$$

其中 $\omega$ 是 yaw rate。

因此 ASA 的“cost”不是纯几何长度，而更接近一个简化执行时间模型：前后走、横移、转身分别具有不同代价。

A* 启发函数

论文采用

$$\mathrm{Heur}({\mathbf{x}}_j)=\frac{\Vert {\mathbf{p}}_{\mathrm{goal}}-{\mathbf{p}}_j{\Vert }_2}{\max (v_{\mathrm{long}},v_{\mathrm{lat}})}+\frac{\min \left(|{\psi }_{\mathrm{goal}}-{\psi }_j|,2\pi -|{\psi }_{\mathrm{goal}}-{\psi }_j|\right)}{\omega }.$$

第一项给出平移时间下界，第二项给出最短 yaw 调整时间下界。搜索输出完整状态序列

$${\tau }_{\mathrm{init}}=({\mathbf{x}}_{\mathrm{start}},{\mathbf{x}}_1,\dots ,{\mathbf{x}}_{\mathrm{goal}}),$$

并可映射成 layer-specific region sequence ${\Pi }_{\mathrm{init}}$。

6.2 第二阶段：String Pulling

Initial A* 受制于图节点位置，几何路径通常存在不必要的 zig-zag。作者先丢弃 region sequence 中的 yaw layer 标记，并合并连续重复的同一物理区域，例如

$$({\mathcal{R}}_a^{(2)},{\mathcal{R}}_b^{(2)},{\mathcal{R}}_b^{(3)},{\mathcal{R}}_b^{(4)},{\mathcal{R}}_c^{(4)})$$

压缩为

$${\Pi }_{\mathrm{compact}}=({\mathcal{R}}_a,{\mathcal{R}}_b,{\mathcal{R}}_c).$$

这些 region 构成连续 polygon corridor；随后执行 String Pulling，在 corridor 内寻找更接近欧氏最短的路径：

$$\gamma =({\mathbf{p}}_{\mathrm{start}},{\mathbf{p}}_{\gamma ,1},\dots ,{\mathbf{p}}_{\mathrm{goal}}).$$

注意：这一步只优化位置，不重新优化 yaw。

6.3 第三阶段：Yaw Refinement A*

String Pulling 改变了中间 waypoint 和运动方向，原来 Initial A* 给出的 yaw 可能不再合适。作者在 $\gamma$ 上重新注册各位置可行的 yaw channel，根据当前位置所属区域的 $\mathcal{I}(\mathcal{R})$ 构建一个新的小型 yaw graph，再执行一次 A*。

因此 ASA 的逻辑可以概括为：

第一遍 A* 先找“能过的拓扑与朝向通道”；String Pulling 再把位置路径拉直；第二遍 A* 最后把朝向重新配准到已经拉直的位置路径。

这种分解比直接在高维连续 $SE(2)$ 中做一次昂贵优化更容易利用 NavMesh 的拓扑结构。

---

7. 在线 SE(2) NavMesh 生成

7.1 点云 → Voxblox → Triangle Mesh

在线输入是机载传感器产生的 point cloud。作者用 Voxblox 增量构建 TSDF，并通过 Marching Cubes 从 TSDF block 中生成 triangle mesh。

这意味着 SE(2) NavMesh 不是直接从每帧点云做邻域 PCA，而是先获得结构化、可局部更新的表面 mesh，再复用离线 NavMesh 构建流程。

7.2 为什么需要 Slab

如果每次新增点云都重建整个 SE(2) NavMesh，地图越大，更新耗时越长。作者进一步把每个 tile 沿 z 方向切成等高 slab。

一旦某个 slab 内的 mesh 几何发生变化，只更新其附近一个 3D window 内的 slab。水平和竖直影响范围分别为

$$u_{xy}=1+2\lceil \frac{d_{\mathrm{border}}}{d_{\mathrm{tile}}}\rceil ,$$

$$u_z=1+2\lceil \frac{d_{\mathrm{zborder}}}{d_{\mathrm{slab}}}\rceil .$$

对受影响 slab，重新执行 voxelization、walkability、yaw feasibility、polygon generation，随后只重连局部与相邻 slab/tile 的连接关系。

这一在线设计的本质是地图表示层的局部增量维护，而不是让规划器不断面对一张从零重建的全局图。

---

8. 实验设计

论文实验分三类：

1. 表示能力实验：SE(2) NavMesh vs classical NavMesh；
2. 路径规划实验：ASA vs RRT / RRT* / PRM，并比较 voxel validity checker 与 SE(2) NavMesh validity checker；
3. 真实机器人实验：验证在线地图更新频率与复杂场景导航。

8.1 仿真机器人与地图参数

表 1　论文仿真中的机器人与 SE(2) NavMesh 主要参数（对应论文 Table II / III）

类别	参数	数值
机器人尺寸	$l_{\mathrm{robot}}$	0.93 m
机器人尺寸	$w_{\mathrm{robot}}$	0.53 m
机器人尺寸	$h_{\mathrm{robot}}$	0.89 m
通过能力	$h_{\mathrm{step}}$	0.25 m
通过能力	${\theta }_{\mathrm{climb}}$	30°
速度模型	$v_{\mathrm{long}}$	0.5（论文作为纵向速度参数）
速度模型	$v_{\mathrm{lat}}$	0.1（论文作为横向速度参数）
速度模型	$\omega$	0.5（论文作为 yaw rate 参数）
NavMesh voxel	$s_{\mathrm{voxel}}$	0.1 m
Tile	$d_{\mathrm{tile}}\times d_{\mathrm{tile}}$	$16\times 16$ voxels
Yaw 离散	$N_{\Psi }$	40
对应 yaw 分辨率	$2\pi /N_{\Psi }$	9°

仿真场景来自 HM3D，共 6 个：Garage、Store、Gym、Forum、Studio、Apartment；覆盖双层环境、楼梯、货架窄通道、门洞、走廊和房间等结构。实验 CPU 为 Intel i7-11800H @ 2.30 GHz。

图 6　六个 HM3D 场景中的离线 NavMesh 生成对比。来源：论文 Figure 9。第一行是场景，第二行为 SE(2) NavMesh，第三行为 classical NavMesh。

---

9. 表示能力实验结果

9.1 可通行面积：每个场景都增加超过 50%

论文最直接的结果是：在六个场景中，SE(2) NavMesh 相对 classical NavMesh 均获得超过 50% 的额外 traversable area。

原因不是它“降低了安全距离”，而是它不再要求一个位置对所有 yaw 都可行。原本会被外接圆删掉的窄边界、货架通道、门洞、楼梯等区域，被重新表示为 restricted region，同时保留允许通过的 yaw channel。

更关键的是，restricted region 还能把原本分裂的 safe components 接起来，因此不仅总面积增加，largest connected traversable component 也显著增大。对全局规划来说，这比单纯多出一些孤立可行面积更重要，因为它直接改变了 start 与 goal 是否拓扑连通。

9.2 多边形数量明显增加，但构建时间没有同比增长

表 2　NavMesh 与 SE(2) NavMesh 的离线构建开销（对应论文 Table IV）

场景	NavMesh 总时间 / ms	SE(2) NavMesh 总时间 / ms	NavMesh 多边形数	SE(2) NavMesh 多边形数
Garage	37.80	88.69	278	2859
Store	48.15	94.44	374	4007
Gym	43.53	91.52	478	4362
Forum	42.02	109.41	628	5055
Studio	42.87	104.54	603	4521
Apartment	25.83	62.75	193	1729

SE(2) NavMesh 产生的 region polygon 大约是经典 NavMesh 的 8～10 倍，但总构建时间大约只增加到 2～3 倍。continuous-yaw footprint mask 的生成本身平均约为 2.35 ms，主要成本仍是 mesh construction。

这里可以看出作者的 representation strategy：不是简单把整张地图复制 40 份做全密集 yaw grid，而是先以 region polygon 做空间压缩，只有 yaw feasibility 变化剧烈的地方才细分出更多 polygon。

---

10. 路径规划 Benchmark

10.1 对比方法

论文使用 OMPL 实现：

• RRT；
• RRT*；
• PRM；
• RRT-SE2NM；
• RRT*-SE2NM；
• PRM-SE2NM；
• ASA。

其中前 3 个使用 voxel-based state validity checker，后缀 SE2NM 的方法直接在 SE(2) NavMesh 上查询状态可行性。

Sampling-based planner 的状态仍为

$$\mathbf{s}=({\mathbf{p}}_s,{\psi }_s)\in {\mathbb{R}}^3\times S^1.$$

Voxel checker 需要：找到对应地形 voxel → 检查 z 偏差 → 根据 ${\psi }_s$ 生成 isolated-yaw footprint mask → 检查 mask 覆盖的所有 voxel。

SE2NM checker 则只需：找到对应 traversable region → 检查 z 偏差 → 查询该 yaw 是否属于 region 的可行 yaw 集。

因此 SE(2) NavMesh 不只服务 ASA，也可以作为其他 SE(2) sampling planner 的高效 validity-query acceleration structure。

10.2 指标：SR 与 SPC

成功率为 SR。论文另外定义 Success weighted by inverse Path Cost（SPC）：

$$\mathrm{SPC}=\frac{1}{N}\sum _{i=1}^N{S}_i\frac{c_i^{\ast }}{\max (c_i,c_i^{\ast })},$$

其中 $S_i$ 表示任务是否成功，$c_i$ 是该方法路径代价，$c_i^{\ast }$ 是所有方法、所有试验中该问题获得的最低代价。

SPC 同时惩罚“找不到路径”和“虽然找到但路径很差”两种情况，因此比单独看 SR 更能体现全局规划质量。

图 7　受限环境中的 Pathfinding Tasks 1–4 与示例结果。来源：论文 Figure 12。任务包含楼梯、窄通道、窄门等，ASA 的路径整体更直，姿态旋转也更连续。

10.3 受限场景结果

论文为 Tasks 1–4 设计了楼梯、窄通道和门洞等强约束环境，每个方法运行 100 次；RRT 最大 100 s，RRT*/PRM 等还分别考察 1 s、10 s、100 s 时间预算。

结论是：

• Tasks 1–4 中 ASA 持续取得最短规划时间和最高 SPC；
• sampling-based 方法中，PRM-SE2NM 通常具有最高 SPC，但为了建立足够稠密的 roadmap 仍需明显更多时间；
• 在开放场景 Task 5 中，所有方法都能达到 100% SR，RRT-SE2NM 的规划时间可以略短于 ASA；
• 即使在开放环境中，ASA 仍能得到更低的路径 cost。

这说明 ASA 的优势主要出现在拓扑狭窄、姿态可行域受限的场景，而不是声称在所有开放空间随机规划问题上都比 RRT 更快。

10.4 SE(2) NavMesh 作为 validity checker 的加速效果

论文报告单状态可行性检查时间：

• SE(2) NavMesh checker：约 $0.1\mu s$ 量级；
• voxel footprint checker：约 $10\mu s$ 量级。

约有两个数量级差异。原因是前者将昂贵的 footprint 几何检查前移到了 NavMesh 构建阶段，规划时变成 region lookup + yaw-set query。

10.5 随机 start-goal 测试

Gym 与 Studio 各随机采样 100 对 start-goal，start 和 goal 不保证位于同一 connected component，因此 SR 同时反映地图连通结构与规划能力。

表 3　随机 start-goal 任务中的 SR / SPC（对应论文 Table V）

Planner	Gym SR ↑	Gym SPC ↑	Studio SR ↑	Studio SPC ↑
ASA	41%	0.41	98%	0.98
RRT-SE2NM	40%	0.13	97%	0.29
RRT*-SE2NM	32%	0.18	82%	0.26
PRM-SE2NM	41%	0.27	96%	0.63
RRT	41%	0.13	97%	0.29
RRT*	36%	0.16	80%	0.25
PRM	36%	0.15	84%	0.42

Gym 本身存在多个 traversable components，因此所有方法平均 SR 都较低；Studio 大部分区域连通，因此 SR 高。ASA 在两种场景中都取得最高或并列最高 SR，并明显取得最高 SPC。

值得注意的是：这里 Gym 的 41% SR 并不意味着 ASA “只有 41% 成功率”，因为随机 start-goal 可能根本不处于同一连通分量。该实验不是单纯的 planner failure rate 测试。

---

11. ASA 三阶段消融：为什么需要第二次 A*

论文对 ASA 的三阶段分别比较路径长度与 path cost：

1. Initial A*：保证 SE(2) 可行，但路径受图节点位置影响；
2. String Pulling：平均使几何路径长度减少 6.2%；
3. 但 String Pulling 改变了 waypoint 与运动方向，若保留旧 yaw，path cost 反而平均增加约 10%；
4. Yaw Refinement A* 再次优化朝向后，最终 path cost 平均降到 Initial A* 的 87%。

因此第二次 A* 不是可有可无的后处理。它解决的是“几何最短路径”和“朝向/横移/转身代价”之间的耦合：单纯拉直位置不保证运动代价更低。

从算法设计上，这一消融也解释了 ASA 的名字为什么不是 A* + Funnel，而是 A–String Pulling–A**。

---

12. 真实机器人与在线更新实验

真实平台为四足机器人 + 6-DoF manipulator，末端安装 ZED X Mini，相机 RGB-D 转为彩色点云；图像处理、Voxblox 和 SE(2) NavMesh 均在机载 NVIDIA Jetson Orin 上执行。

由于机械臂增加了整体高度，真实实验中设置

$$h_{\mathrm{robot}}=1.00\mathrm{m},$$

并将最大坡度设置为 40°。

表 4　真实在线生成主要参数（对应论文 Table VI）

参数	数值
Voxblox voxel	$0.05\times 0.05\times 0.05$ m
Voxblox block	$16\times 16\times 16$ voxels
Slab	$16\times 16\times 8$ SE(2)-NavMesh voxels
目标在线更新率	4 Hz
Local update 平均耗时	83 ms
Local update 峰值耗时	237 ms

图 8　花园环境中的在线 SE(2) NavMesh 增量生成。来源：论文 Figure 16。绿色表示本轮因几何变化而重新生成的局部 NavMesh，蓝色为无需更新的既有部分。

12.1 Local update vs Global update

随着点云持续输入，全局重建的 update time 近似随地图规模增长；实验大约到 100 s 后便无法继续维持 4 Hz。Local slab update 的平均 83 ms、峰值 237 ms，则能在整段数据中持续满足 4 Hz 目标。

这说明在线版本的实时性并不是来自“SE(2) NavMesh 本身很小”，而是来自只维护局部变化区域。

12.2 多楼层楼梯导航

图 9　室外多层真实环境导航。来源：论文 Figure 18。机器人从下层出发，经多段楼梯到达上层平台，并在平台绕过桌椅。

机器人先由人工遥操作遍历环境以在线构建地图，随后在生成的 SE(2) NavMesh 上定义 start/goal，并按规划 state sequence 作为导航 waypoint 执行。真实实验展示了跨多个楼梯段返回上层平台的过程。

12.3 长窄走廊、门洞与杂乱区域

图 10　室内单层真实环境导航。来源：论文 Figure 19。测试包含长窄走廊、门洞、转角与杂物密集区域。

论文报告：

• 一条约 41 m 的长窄走廊导航任务成功完成；
• 连续执行进入/离开房间的两条任务，路径约 19 m 与 18 m，均通过窄门洞且无失败；
• Figure 1(a) 中的通道宽度只有 0.8 m，比机器人宽度 0.53 m 仅多 0.27 m；
• Figure 1(b) 还验证了悬垂结构下的高度 clearance 判定。

这些场景直接对应论文最核心的主张：同一位置能否通过不仅由障碍距离决定，还取决于机器人 yaw。

---

13. 主要创新点与学术贡献

13.1 创新一：把 NavMesh 从 yaw-invariant representation 提升为 yaw-dependent representation

论文最核心的贡献不是“把 A* 状态多加一个 $\psi$”，而是把可通行地图本身从

$${\mathcal{P}}_{\mathrm{trav}}$$

提升为对

$$\mathcal{M}\times S^1$$

自由空间的结构化表示。restricted region 不再被删除，而是带着可行 yaw set 保留下来。

这使得机器人可以在“只有特定姿态才能通过”的位置仍然进行全局拓扑连接。

13.2 创新二：Continuous-yaw footprint mask 为离散层间旋转提供安全保证

单纯离散 yaw 很容易出现采样点可行、采样间旋转碰撞的问题。作者用每个 yaw bin 的 footprint sweep union 进行检查，使相邻 channel 的旋转连通关系具有明确的几何含义。

这一点让 layer graph 不只是经验离散，而是与真实 footprint 连续旋转安全性对应。

13.3 创新三：Safe / Restricted 的自适应 polygonal abstraction

方法没有使用统一 $(x,y,\psi )$ 三维网格，而是：

• 开阔区合并成大 safe polygon；
• 只在 yaw feasibility 快速变化的狭窄区域细分；
• yaw 作为 region layer 属性存储。

因此同时保留了 NavMesh 的低维多边形压缩优势和 SE(2) 的方向可行性。

13.4 创新四：ASA 分层优化位置与朝向

ASA 不是一次性在连续高维空间进行全局最优求解，而是：

1. 用 yaw-aware A* 找可行拓扑；
2. 用 String Pulling 优化几何位置；
3. 再在缩小后的路径位置集合上优化 yaw。

这种 hierarchical optimization 把难问题拆成两个图搜索和一个几何短路过程，充分利用 NavMesh corridor 的结构。

13.5 创新五：在线局部 slab 更新

作者把 yaw-aware representation 做成可以在线运行的系统：Voxblox 负责增量几何，slab 负责限定局部 NavMesh 重建范围，使更新成本与本轮几何变化范围相关，而不是与已探索全局地图规模直接绑定。

---

14. 这篇论文的几个关键算法理解

14.1 它不是完整的连续 SE(2) trajectory optimization

虽然名字叫 SE(2) NavMesh，但输出本质上仍是全局 path / state sequence。它处理的是全局几何可行性和朝向选择，并不直接求连续时间动力学轨迹，也没有在图搜索中加入四足机器人完整 centroidal dynamics、足端接触、关节极限等约束。

因此它更适合位于：

全局几何规划层 / traversability representation 层，而不是低层 locomotion control 层。

14.2 “旋转边”不是说机器人必须真的停下来原地旋转

图结构把平移和 yaw 改变分解成 translational edge 与 rotational edge，是一种离散搜索建模。实际机器人跟踪最终 state sequence 时可以由下层控制器产生更连续的运动。论文的 cost model 将它们分开，是为了让图上朝向改变具有明确代价并便于 A* 搜索。

14.3 String Pulling 后仍需要重新检查 yaw

位置路径留在原 polygon corridor 内，只能说明空间路径仍位于已有可通行区域；它不能保证原先沿 edge-midpoint path 分配的 yaw 在新 waypoint 上仍然代价最优。因此第二次 A* 专门修复这一问题。

14.4 SE(2) NavMesh 同时是一种 planner acceleration structure

对 RRT/PRM 这类方法，最耗时的操作之一是大量 state validity query。SE(2) NavMesh 把 footprint 检查结果缓存为 region 的 yaw feasibility，后续查询只需定位 region 并查集合，因此论文观察到约 $0.1\mu s$ 对 $10\mu s$ 的量级差异。

---

15. 与经典 NavMesh、PCT 类方法和 Sampling-based Planner 的本质区别

表 5　方法层面的对比总结

方法	地图核心表示	是否显式考虑 yaw-dependent traversability	多层环境	规划特点	典型短板
Classical NavMesh	凸多边形表面	否	是	A* + polygon corridor	非圆 footprint 下过度保守
Point-cloud tomography / PCT 类	多层/层析点云或栅格结构	通常按圆形或 yaw-invariant clearance 处理	是	层间图搜索	狭窄转角处机器人朝向约束表达不足
RRT / RRT* / PRM	连续状态采样	可以	可以	通用、灵活	受限空间采样效率低，大量 collision query
SE(2) NavMesh + ASA	yaw-layered 凸多边形	是	是	两次 A* + String Pulling	yaw 离散、固定高度/footprint，非动态可行轨迹

论文在 Related Work 中也专门讨论了 point cloud tomography 的多层表示：它能处理三维多楼层，但如果仍采用外接圆 footprint，就会在窄通道中产生与 classical NavMesh 类似的保守性。SE(2) NavMesh 的切入点正是把这一“圆形机器人假设”显式解除。

---

16. 对狭窄楼梯拐弯问题的启发（工程延伸解读，非论文原文）

如果目标是不替换现有的 PCT 全局规划和 SCAN 局部规划，这篇论文最值得借鉴的并不是把整个导航栈改成 NavMesh，而是引入一个SE(2) yaw-feasibility layer。

可以把现有系统理解为：

$$\text{PCT 全局路径}\to \text{局部规划 / SCAN}\to \text{四足运动控制}.$$

在中间加入：

$$\boxed{\text{PCT path corridor}\to (x,y,\psi )\text{ 可行性标注}\to \text{yaw-constrained waypoint / corridor}}$$

具体可借鉴论文做法：

1. 保留 PCT 的多楼层 global topology，不改变其跨楼层搜索框架；
2. 只在 PCT 路径附近生成一个局部 corridor，而不是为全地图构建完整 SE(2) NavMesh；
3. 根据四足机身长宽生成 $N_{\Psi }$ 个 continuous-yaw footprint masks；
4. 对楼梯平台、转角、门洞等低 clearance 区域计算 $\mathcal{I}(\mathbf{p})$；
5. 如果某个拐角位置只允许“机身沿楼梯方向”或“侧身”通过，就在 global waypoint 中显式给出 $\psi$ 约束，而不是只给 $(x,y,z)$；
6. SCAN 仍负责实时局部避障与轨迹生成，但其局部目标由纯 position target 改成 pose / yaw-corridor target；
7. 若当前 yaw 不允许进入下一狭窄区域，则提前在宽平台处完成转身，而不是进入拐角后才发现 footprint 转不过去。

因此，对窄楼梯拐角而言，这篇论文真正有价值的思路可以压缩成一句话：

把传统只考虑位置 $(x,y)$ / $(x,y,z)$ 的“通不通”，升级为同时考虑机身朝向的 $(x,y,\psi )$ 可通行性，并把“在哪里可以转身、以什么朝向进入窄区”提前编码进全局/中间层规划。

这与单纯给障碍物做更大的 inflation 不同：inflation 只会越来越保守，而 yaw-aware feasibility 能保留“虽然窄，但某些姿态仍可通过”的空间。

---

17. 局限性与尚未解决的问题

17.1 论文明确指出的局限

固定机器人高度。 当前方法假设 agent height 固定。如果机器人可以下蹲、降低机身或主动改变 body height，那么原本可通过的低矮区域可能被错误删除。

仅依赖几何。 低矮物体可能被几何重建误认为地表，从而错误标记为 traversable。作者提出未来可融合 semantic segmentation 以区分地面、障碍物和不同 terrain type。

在线建图尚未与自主探索闭环。 真实实验中先遥操作获取环境，再用生成的 NavMesh 执行导航。作者将 autonomous exploration + online mesh generation 作为未来工作。

17.2 从方法假设可以进一步看到的工程限制

以下属于基于论文模型的工程分析，不是作者原文结论：

• Yaw 离散误差：$N_{\Psi }=40$ 时为 9° 一层。分辨率越高，restricted 区域表达越精细，但生成和图规模也会增大；
• Fixed planar footprint：四足机器人在急转、抬腿、身体 roll/pitch 时真实 swept volume 会变化，cuboid + gravity-aligned footprint 仍是近似；
• Holonomic 假设：论文面向典型 legged robot 的 omnidirectional translation。对 Ackermann/nonholonomic 机器人，图边和 cost model 需要进一步加入曲率、转向半径等运动学约束；
• 不包含完整动力学可行性：能在 footprint 几何上通过，不等于具体 gait、足端接触或摩擦条件下一定能稳定通过；
• 动态障碍物不是其核心问题：在线更新主要针对环境几何重建，不等同于高频动态障碍预测与局部避碰。

因此最合理的系统分工仍然是：SE(2) NavMesh 解决全局几何—朝向可行性，局部 planner 和 locomotion controller 解决短时动态、轨迹平滑和机器人动力学执行。

---

18. 论文的研究价值

这篇工作的价值在于，它指出了一个在很多机器人导航系统中长期被“圆形膨胀”掩盖的问题：对非圆机器人，traversability 本身就是 orientation-dependent 的。

把 yaw 放入全局导航并不新鲜，Hybrid A*、State Lattice、SE(2) RRT 等都在搜索状态中包含 yaw；SE(2) NavMesh 的区别在于，它把 yaw-dependent collision feasibility 编译进地图表示，并仍然保持 polygonal abstraction。这使得同一张地图既可以支持 ASA，也可以作为采样规划器的高速 validity checker。

从四足机器人场景看，它特别适合以下几类问题：

• 窄楼梯及楼梯平台转角；
• 窄门、货架通道；
• 长宽比明显的四足机器人或移动机械臂；
• 上下多层重叠结构；
• 顶部有悬垂障碍的低 clearance 区域；
• 希望全局层提前决定“以什么姿态进入狭窄区”的导航系统。

其最有代表性的学术贡献不是单独某个搜索算子，而是形成了完整链路：

$$\boxed{\text{Yaw-aware Traversability Representation}\to \text{Layered Navigation Graph}\to \text{ASA Pathfinding}\to \text{Incremental Online Update}}$$

也正因为这四部分是闭环的，论文才能从仿真表示对比一直验证到机载在线建图和真实四足机器人导航。

---

19. 最终结论

SE(2) Navigation Mesh 可以理解为对 classical NavMesh 的一个关键扩展：传统 NavMesh 问“这个位置能不能站”，SE(2) NavMesh 问“这个位置以哪些 yaw 能站、能否保持该 yaw 平移、能否在这里安全转到另一个 yaw”。

它用 continuous-yaw footprint mask 将机器人真实 footprint 的方向性编码到 voxel traversability，再把结果压缩成 yaw-specific polygon layers；通过 translational / rotational connectivity 构建图；ASA 先解决拓扑与 yaw 可行性，再缩短位置路径并重新分配 yaw；在线版本则利用 Voxblox 和 slab 局部重建保证更新效率。

实验结果完整支撑了这一设计：六个 HM3D 场景可通行面积均比 classical NavMesh 高 50% 以上，受限场景中 ASA 相比 RRT/RRT*/PRM 更快且 path cost 更低，String Pulling 使几何路径平均缩短 6.2%，最终 yaw refinement 将 cost 降至初始路径的 87%，局部在线更新平均 83 ms 并维持 4 Hz；真实四足机器人完成了楼梯、多楼层、门洞、长窄走廊、0.8 m 窄通道和悬垂障碍下的导航。

对实际狭窄楼梯转弯问题而言，它提供的最重要思路不是“换掉当前 planner”，而是：让全局/中间层路径携带 yaw feasibility，让机器人在进入窄区之前就知道应该在哪个宽敞区域完成转身，以及以什么朝向穿过受限区域。

---

参考资料

1. Shuyang Shi, Kaixian Qu, Changan Chen, Ines Kast, Yuntao Ma, Marco Hutter. SE(2) Navigation Mesh. arXiv:2607.01454v1, 2026.
   https://arxiv.org/abs/2607.01454
2. SE(2) Navigation Mesh Project Page.
   https://se2-navmesh.github.io/
3. arXiv HTML version used for Figure cross-checking and formulas.
   https://arxiv.org/html/2607.01454v1
4. DBLP entry: SE(2) Navigation Mesh, CoRR abs/2607.01454 (2026).