SCAN-Planner：面向路线引导长距离四足机器人导航的空间碰撞感知局部规划【文献解读】

文献：SCAN-Planner: Spatial Collision-Aware Local Planning for Route-Guided Long-Range Quadruped Navigation
作者：Han Zheng, Zhe Chen, Yiwen Fu, Ming Yang, Tong Qin
单位：Shanghai Jiao Tong University, Shanghai, China
论文地址：https://arxiv.org/abs/2606.19555
相关开源项目参考：https://github.com/wuyi2121/SCAN-Planner
关键词：Trajectory Optimization，Whole-Body Collision Checking，B-spline Planning，Quadruped Robots

本文围绕上述文献展开系统解读，重点分析其面向四足机器人长距离导航的空间碰撞感知局部规划框架、yaw-aware 双圆柱机身模型、Projected A* 引导搜索、机器人中心三维滑动地图以及仿真与实物实验结果。

图1：

---

1. 研究背景与问题定位

四足机器人具有较强的地形适应能力，能够在楼梯、狭窄通道、室内桌椅环境以及室外非结构化道路中运动，因此适合承担巡检、配送和服务等任务。但在真实环境中，四足机器人局部规划并不是简单的二维路径搜索问题，而是需要同时处理三维障碍物、机身整体碰撞、地面跟随运动、局部地图边界以及长距离路线引导等问题。

传统局部规划方法常将机器人简化为点、圆、球或使用各向同性膨胀半径进行碰撞检测。这种做法对无人机或圆形移动机器人比较有效，但对四足机器人并不充分。四足机器人机身通常呈细长结构，其碰撞范围会随着偏航角变化而变化：正向通过狭窄通道时可能安全，但转弯时机身侧面可能碰撞；若直接使用较大膨胀半径，则会把很多原本可通过的狭窄区域错误判为不可通行。

此外，四足机器人虽然可以上下楼梯或跨越低矮障碍物，但仍属于接触式地面机器人，不能像无人机一样在完整三维空间中任意上升、下降或飞越障碍物。因此，局部规划既需要利用三维占据地图识别悬空桌面、架子、墙体等空间结构，又需要保持轨迹符合地面运动规律，避免产生不可执行的垂直绕障轨迹。

---

2. 文献解决的关键科学问题与技术挑战

2.1 狭窄空间中的整体机身碰撞检测

文献首先关注的是四足机器人在狭窄通道中的整体机身碰撞问题。普通二维圆形膨胀模型无法描述四足机器人“长宽不一致、碰撞区域随 yaw 变化”的特点。若采用宽度膨胀，转弯时可能发生机身碰撞；若采用长度膨胀，又会过于保守，导致可行通道被封死。

因此，核心挑战是：如何在不显著增加优化维度的情况下，实现对四足机器人机身形状和航向变化的高效碰撞检测。

2.2 三维非结构化环境中的地面跟随规划

二维规划器难以区分墙体、台阶、桌面和桌下空间；2.5D elevation map 又难以准确表示悬空结构和多层空间。若直接采用三维无人机规划方法，又可能产生对地面机器人不可执行的垂直绕障。

因此，第二个挑战是：如何在三维占据环境中进行空间碰撞判断，同时让轨迹主要沿地形高度趋势变化，保持四足机器人可执行的地面运动特性。

2.3 长距离导航中的局部地图边界与死胡同问题

长距离导航通常依赖粗全局路线提供方向，但高分辨率碰撞检测只能在机器人附近的局部地图内完成。当局部目标位于地图边界外，或者前方路线被新观测障碍物阻挡时，局部规划器容易陷入局部极小值或直接失败。

因此，第三个挑战是：如何在有限大小的机器人中心滑动地图中，实现高分辨率实时碰撞检测、路线引导和局部死胡同恢复。

---

3. SCAN-Planner 总体技术路线

SCAN-Planner 的总体框架如图2所示。系统输入包括点云、里程计和由全局路线给出的三维局部目标。局部建图模块维护机器人中心三维滑动地图，轨迹规划模块基于该地图进行初始化、碰撞检测、A* 引导搜索和 B-spline 轨迹优化，最终将轨迹发送给四足机器人运动控制器执行。

图2

从技术链路看，SCAN-Planner 主要包含以下几个部分：

1. 三维滑动占据地图：以机器人为中心维护局部高分辨率 3D occupancy grid，并使用循环缓冲区实现地图窗口平移。
2. Yaw-aware 双圆柱机身模型：使用两个沿机身纵向布置的竖直圆柱近似四足机器人整体机身，用于高效整体碰撞检测。
3. 高度正则化初始化：对初始多项式轨迹的高度进行平滑插值，避免轨迹在垂直方向产生不必要振荡。
4. Projected A* 搜索：在入口点和出口点确定的地面跟随投影曲面上搜索无碰撞引导路径。
5. B-spline 轨迹优化：综合碰撞惩罚、平滑性惩罚和动力学可行性惩罚，生成可执行轨迹。
6. 边界回退机制：当局部目标不可达时，将搜索路径引导至滑动地图边界，实现有限恢复运动。

---

4. 轨迹表示与偏航角诱导

4.1 B-spline 位置轨迹

SCAN-Planner 只优化机器人位置轨迹：

$$p(t)\in {\mathbb{R}}^3$$

位置轨迹采用均匀 B-spline 表示。设 B-spline 阶数为 $p_b$，控制点数量为 $N_c$，节点间隔为 $\Delta t$，控制点序列为：

{Qi}i=0Nc−1 \{Q_i\}_{i=0}^{N_c-1} {Qi​}i=0Nc​−1​

文中使用三次 B-spline，即 $p_b=3$。轨迹总时长为：

$$T=(N_c-p_b)\Delta t$$

速度、加速度和 jerk 控制点分别由相邻控制点差分得到：

$$V_i=\frac{Q_{i+1}-Q_i}{\Delta t}$$

$$A_i=\frac{V_{i+1}-V_i}{\Delta t}$$

$$J_i=\frac{A_{i+1}-A_i}{\Delta t}$$

B-spline 的优点在于轨迹天然平滑，且通过移动少量控制点即可改变整段轨迹形状，适合实时局部重规划。

4.2 由轨迹切向方向诱导 yaw

文献没有将偏航角 $\psi$ 作为独立优化变量，而是根据局部轨迹切向方向估计 yaw：

$${\psi }_i=atan2\left(e_y^{\top }(Q_{i+1}-Q_{i-1}),e_x^{\top }(Q_{i+1}-Q_{i-1})\right)$$

其中：

• $Q_i$ 为第 $i$ 个控制点；
• $e_x=[1,0,0{]}^{\top }$；
• $e_y=[0,1,0{]}^{\top }$；
• $Q_{i+1}-Q_{i-1}$ 近似表示轨迹局部切向方向。

这种设计的意义在于：优化变量仍然只有位置控制点，不增加 yaw 控制点，但碰撞检测和执行方向仍能与机器人实际运动方向保持一致。

---

5. Yaw-aware 双圆柱整体机身碰撞检测

SCAN-Planner 的核心创新之一是使用 yaw-aware twin-cylinder footprint 表示四足机器人机身。该模型用两个竖直圆柱覆盖机器人前后机身区域，并随机器人 yaw 旋转，从而表达机身碰撞范围随航向变化的特点。

图3

在机器人机体系下，两个圆柱中心定义为：

$$s_{b,1}=[d_{off},0,0{]}^{\top }$$

$$s_{b,2}=[-d_{off},0,0{]}^{\top }$$

其中 $d_{off}$ 表示圆柱中心相对于机器人中心的纵向偏移。给定第 $k$ 个检查点 $Q_k$ 和诱导偏航角 ${\psi }_k$，圆柱中心在世界坐标系下为：

sk,j=Rz(ψk)sb,j+Qk,j∈{1,2} s_{k,j}=R_z(\psi_k)s_{b,j}+Q_k,\quad j\in\{1,2\} sk,j​=Rz​(ψk​)sb,j​+Qk​,j∈{1,2}

其中 $R_z({\psi }_k)$ 表示绕 $z$ 轴旋转 ${\psi }_k$ 的旋转矩阵。

文中将三维占据地图按照圆柱半径和上下间隙进行膨胀，因此整体机身碰撞检测可以简化为对两个变换后圆柱中心点的占据查询：

Hk(Qk,ψk)=max⁡j∈{1,2}χ(sk,j) H_k(Q_k,\psi_k)=\max_{j\in\{1,2\}}\chi(s_{k,j}) Hk​(Qk​,ψk​)=j∈{1,2}max​χ(sk,j​)

其中 $\chi (\cdot )$ 表示膨胀占据地图中的占据值。若 $H_k=0$，则认为当前检查点处无碰撞；否则认为存在碰撞风险。

这种方法的优点是同时兼顾了几何准确性和计算效率：相比点或圆形模型更加符合四足机器人细长机身结构，相比完整 mesh 碰撞检测又显著降低了实时计算复杂度。

---

6. 高度正则化初始化与 Rebound 轨迹优化

为了避免三维规划中出现不必要的垂直振荡，SCAN-Planner 在初始多项式轨迹上进行高度正则化。设初始采样点为：

R={r0,r1,…,rNp} R=\{r_0,r_1,\ldots,r_{N_p}\} R={r0​,r1​,…,rNp​​}

首先计算水平累计弧长：

$${\ell }_i=\sum _{m=1}^i\Vert r_{m,xy}-r_{m-1,xy}\Vert$$

总长度为：

$$L={\ell }_{N_p}$$

然后根据水平路径进度插值高度：

$$z_i=z_s+{\alpha }_i(z_g-z_s)$$

其中：

$${\alpha }_i=\{\begin{array}{ll}{\ell }_i/L,& L>0\\ i/N_p,& L=0\end{array}$$

该初始化方法保留 $xy$ 平面的路径形状，同时让高度沿水平路径平滑变化。图4展示了在楼梯场景中，初始多项式轨迹提供基本高度趋势，而优化后的 B-spline 主要通过水平方向绕开障碍物。

图4

SCAN-Planner 的优化目标函数为：

$$\underset{Q}{\min }J={\lambda }_c{J}_c+{\lambda }_s{J}_s+{\lambda }_f{J}_f$$

其中：

• $J_c$：碰撞惩罚项；
• $J_s$：平滑性惩罚项；
• $J_f$：动力学可行性惩罚项；
• ${\lambda }_c,{\lambda }_s,{\lambda }_f$：对应权重。

碰撞惩罚只在由 yaw-aware footprint 检测出的碰撞段上构造。对于受影响控制点 $Q_i$，假设其对应锚点为 $a_{ij}$，rebound 单位方向为 $v_{ij}$，则障碍物距离定义为：

$$d_{ij}=(Q_i-a_{ij}{)}^{\top }{v}_{ij}$$

碰撞代价为：

$$J_c=\sum _{i,j}\rho (c_{ij})$$

其中：

$$c_{ij}=s_f-d_{ij}$$

$s_f$ 为安全距离，$\rho (\cdot )$ 为单边连续可微惩罚函数。碰撞代价的梯度为：

$$\frac{\partial J_c}{\partial Q_i}=-\sum _j{\rho }^{\prime }(c_{ij})v_{ij}$$

该梯度会推动控制点沿远离障碍物的方向移动，实现局部轨迹反弹。

平滑性惩罚定义为：

$$J_s=\sum _k\Vert J_k{\Vert }^2$$

动力学可行性惩罚用于限制速度和加速度：

$$J_f=\sum _{k}F(\Vert V_k\Vert ,v_m)+\sum _{k}F(\Vert A_k\Vert ,a_m)$$

其中：

$$F(c,c_m)=\max (c-c_m,0{)}^2$$

这表示只有当速度或加速度超过上限时才产生惩罚。

---

7. 机器人中心三维滑动地图

为了支持长距离导航，SCAN-Planner 使用固定大小的 robot-centric 3D sliding map。局部地图随着机器人移动而平移，重叠区域保留，离开窗口的区域释放，新进入窗口的区域重新初始化。

图5

该地图结构的关键作用包括：

• 维持局部高分辨率三维占据地图；
• 避免为整个大尺度环境维护全局高分辨率地图；
• 通过循环缓冲区复用内存，降低地图平移成本；
• 支持实时障碍物更新和局部碰撞检测；
• 为长距离路线引导下的局部避障提供地图基础。

文中还使用 log-odds 占据更新机制，通过概率上下界限制避免占据值过度自信，使地图能够适应动态或临时障碍物。

---

8. Projected A* 搜索与边界回退机制

8.1 Projected A* 引导搜索

当某段轨迹被检测为碰撞时，系统需要搜索一条局部无碰撞路径，用于构造 rebound 方向。SCAN-Planner 没有在完整三维栅格中搜索，而是在由入口点 $x_{in}$ 和出口点 $x_{out}$ 确定的投影曲面上进行 A* 搜索。

对于候选水平网格点 $x_{xy}$，首先计算其沿入口点到出口点方向的投影比例：

$$\beta (x_{xy})={\Pi }_{[0,1]}\left(\frac{(x_{xy}-x_{in,xy}{)}^{\top }(x_{out,xy}-x_{in,xy})}{\Vert x_{out,xy}-x_{in,xy}{\Vert }^2}\right)$$

其中：

Π[0,1](η)=min⁡{1,max⁡{0,η}} \Pi_{[0,1]}(\eta)=\min\{1,\max\{0,\eta\}\} Π[0,1]​(η)=min{1,max{0,η}}

然后通过线性插值得到该节点高度：

$$z(x_{xy})=z_{in}+\beta (x_{xy})(z_{out}-z_{in})$$

因此，A* 的搜索主要发生在二维水平网格上，高度则由地面跟随曲面给出。这种方式能够抑制四足机器人不合理的垂直绕障。

8.2 边界回退机制

当局部目标在滑动地图内部不可达时，SCAN-Planner 会临时在地图边界外添加一层虚拟自由栅格。该虚拟层只用于假设路径搜索，不作为真实可执行空间。如果 A* 路径从真实地图离开进入虚拟层，则离开位置对应的边界栅格会被选为 fallback voxel，作为新的局部目标。

图6

这一设计能够把“局部目标不可达”转化为“朝地图边界进行有限恢复运动”，增强长距离导航中的鲁棒性。

---

9. 实验设计与结果分析

9.1 仿真实验设置

论文在 MARSIM 仿真环境中对 SCAN-Planner 进行评估，并与以下方法进行对比：

• EGO-Planner-2D；
• EGO-Planner-3D；
• CMU-Planner；
• ART-Planner。

测试场景包括密集随机障碍物、杂乱桌椅三维场景和楼梯场景。每种方法在每个场景中进行 50 次独立测试，最大速度为 $1.5m/s$，最大加速度为 $1.0m/{\mathrm{s}}^2$，超时时间为 120 s。优化求解器采用 L-BFGS。

评价指标包括：

• SR（Success Rate）：成功率；
• CR（Collision Rate）：碰撞率；
• SPL（Success weighted by Path Length）：路径长度加权成功率。

9.2 密集障碍物实验

图7展示了不同障碍物密度下各规划器的路径表现。随着障碍物从 100 个增加到 500 个，传统方法更容易出现绕行、局部不稳定或碰撞风险，而 SCAN-Planner 能够保持相对直接、平滑且安全的路径。

图7

定量结果如下：

方法	SR ↑	CR ↓	SPL ↑
EGO-Planner-2D	0.96	0.04	0.88
EGO-Planner-3D	0.76	0.24	0.75
CMU-Planner	0.92	0.08	0.86
ART-Planner	0.44	0.56	0.31
SCAN-Planner	1.00	0.00	0.95

从结果可以看出，SCAN-Planner 在密集障碍物场景中取得最高成功率和最低碰撞率。其原因在于 yaw-aware 双圆柱模型能够更准确地利用狭窄通道，同时避免单一圆形膨胀模型的过保守或不安全问题。

9.3 桌椅与悬空结构场景

图8展示了杂乱桌椅场景。该场景中存在桌面、椅子和桌下空间。对于依赖 2.5D 地形表达的方法，桌面可能被错误当作不可通行地形；而 SCAN-Planner 使用完整三维占据地图，可以显式判断桌下空间是否具有足够净空。

图8

该实验说明，SCAN-Planner 不仅能避开地面障碍物，还能处理悬空结构和局部净空约束，适合真实室内三维环境。

9.4 楼梯场景

图9展示了楼梯场景对比。EGO-Planner-3D 可能生成向上绕过障碍物的轨迹，虽然在几何三维空间中更短，但对四足地面机器人而言不可执行；EGO-Planner-2D 则容易把楼梯投影为障碍物而失败。SCAN-Planner 能保持楼梯跟随趋势，并通过水平方向绕开障碍物。

图9

图10进一步比较了桌椅场景和楼梯场景中的路径长度。SCAN-Planner 在桌椅场景中路径更短；在楼梯场景中，虽然 EGO-Planner-3D 的路径可能略短，但其轨迹并不符合接触式地面机器人运动约束。

图10

---

10. 真实机器人实验

论文在 Unitree Go2 四足机器人上进行了真实实验。平台使用 Livox Mid-360 LiDAR 获取点云，使用改进版 FAST-LIO2 进行高频状态估计，并在 NVIDIA Jetson Orin NX 上运行感知、建图、规划和控制模块。

图11展示了室内真实实验，包括杂乱房间、S 形弯道、三维非结构化场景和动态障碍物避障。

图11

真实实验验证了 SCAN-Planner 的以下能力：

1. 在桌椅形成的狭窄通道中生成安全局部轨迹；
2. 在连续急转弯的 S 形通道中保持轨迹平滑；
3. 在桌子、椅子、低矮障碍物混合的三维环境中区分可通行区域和阻挡区域；
4. 在存在行人等动态障碍物时，依靠在线占据地图更新实现避障；
5. 在多楼层办公楼和校园级环境中完成长距离导航任务。

---

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

11.1 提出 yaw-aware 双圆柱 footprint

该方法用两个圆柱近似四足机器人机身，通过轨迹切向方向诱导 yaw，并在膨胀三维占据地图中进行稀疏点查询。它在不增加 yaw 优化变量的情况下，实现了对细长机身和航向相关碰撞区域的建模。

11.2 提出适合地面机器人的 projected A* 搜索

相比完整三维 A*，projected A* 将搜索限制在由入口和出口高度插值得到的地面跟随曲面上，使局部绕障主要发生在水平方向，避免生成对四足机器人不可执行的“飞行式”轨迹。

11.3 将 B-spline 轨迹优化与 rebound 碰撞代价结合

SCAN-Planner 继承了梯度式轨迹优化的高效率，通过碰撞段检测、A* 引导路径和 rebound 方向构造，将离散碰撞信息转化为可优化的连续惩罚项，从而生成平滑、安全的局部轨迹。

11.4 构建机器人中心三维滑动地图

文献通过固定大小的滑动地图和循环缓冲区，在长距离导航中保持局部高分辨率地图，同时控制内存开销。该机制使方法能够扩展到大型室内外环境。

11.5 引入边界回退机制提升长距离导航鲁棒性

当局部目标不可达时，边界回退机制能够将死胡同规划失败转化为有界恢复动作，使局部规划器更适合与粗全局路线结合使用。

---

12. 方法局限性与改进方向

SCAN-Planner 已经较好解决了四足机器人在三维复杂环境中的局部规划问题，但仍有一些进一步研究方向：

1. 地形可通行性建模仍可增强：当前方法更侧重空间碰撞，未来可加入坡度、摩擦、软硬地面、湿滑地面等地形代价。
2. 动态障碍物预测能力有限：当前主要依靠占据地图更新和局部重规划，对高速移动目标的预测和交互仍可加强。
3. 机身模型仍是几何近似：双圆柱模型效率高，但对复杂外形、负载变化或大姿态变化的适应性有限。
4. 仍依赖全局粗路线或局部目标：未来可以结合自主探索、语义导航或拓扑地图，实现无预定义目标点的场景探索。

---

13. 总结

SCAN-Planner 是一篇面向四足机器人实际部署的局部规划研究。它针对狭窄空间机身碰撞、三维非结构化环境地面跟随规划、长距离导航中的局部地图边界与死胡同恢复等问题，提出了一个完整的空间碰撞感知局部规划框架。

从算法上看，SCAN-Planner 的关键思想是：用 yaw-aware 双圆柱模型提高整体碰撞检测准确性，用 projected A* 和 z 方向抑制保证轨迹符合地面机器人运动约束，用 B-spline 优化生成平滑可执行轨迹，并用机器人中心滑动地图支持大尺度环境中的实时局部规划。

从实验上看，该方法在密集障碍物、桌椅悬空结构、楼梯和真实长距离导航任务中均表现出较好的安全性、平滑性和实用性。因此，该文献对于四足机器人局部规划、三维避障、长距离自主导航和实物系统部署具有较高参考价值。

---

14. 图片索引

图号	内容	对应章节
图1	真实长距离导航演示	研究背景
图2	SCAN-Planner 系统框架	技术路线
图3	Yaw-aware 双圆柱机身模型	整体碰撞检测
图4	楼梯场景 rebound 轨迹变形	轨迹优化
图5	三维滑动地图更新	滑动地图
图6	Projected A* 与边界回退	死胡同恢复
图7	密集障碍物仿真对比	仿真实验
图8	桌椅悬空结构场景	三维非结构化环境
图9	楼梯场景对比	楼梯实验
图10	路径长度对比	定量分析
图11	室内真实机器人实验	实物实验