Vision-aided Dynamic Quadrupedal Locomotion on Discrete Terrain using Motion Libraries

2026/5/27

文章总结

• Takeaways：先用完整动力学和接触切换模型，离线优化出一批两步周期 trot gait；每个 gait 对应不同步长组合。在线运行时，机器人用视觉地图判断哪里能踩，再从步态库里选出/插值出合适的步态参考。这样，机器人不是只改脚的位置，而是连同质心速度、身体姿态一起调整。

这篇文章的核心问题是：让四足机器人在"离散落脚点"地形上动态行走，比如一块块踏脚石之间有缝隙，机器人必须一边用视觉建图，一边决定脚踩哪里、身体速度怎么变、姿态怎么稳。作者认为难点不只是"找落脚点"，还包括：机器人运动可能是非周期的、后腿看不到当前前方相机看到的地形、遇到大缝隙时需要加速或减速，而不是保持固定速度。

它的总体方案可以概括成一句话：离线生成一个"步态动作库"，在线根据视觉地图选择合适的步长和步态，再用几何MPC稳定身体姿态和质心运动。也就是说，它不是端到端强化学习，而是比较典型的"规划 + 控制 + 感知建图"的组合框架。

文章的第一个关键点是 motion library。作者离线优化出很多"两步周期"的 trot 步态，每个步态由不同的前后两步步长参数化。在线运行时，机器人根据当前状态和可踩区域，从库里插值出合适的质心速度、身体姿态参考和落脚目标。这里"两步周期"很重要，因为四足机器人前后腿、左右腿之间有运动学约束；如果只做"一步周期"，很多左右脚步长组合无法满足周期性。

第二个关键点是 Geometric MPC。普通四足MPC常用欧拉角、小角度近似来线性化身体姿态，但在离散地形上机器人可能有较大的俯仰、横滚和角速度，小角度假设会变差。作者改用 SO(3) 上的旋转矩阵表示姿态，并用几何变分积分思想构造离散动力学，再在线求QP输出支撑腿接触力。这样做的目的，是让姿态误差和角速度误差的建模更符合刚体旋转的几何结构。

第三个关键点是视觉建图。机器人使用前向深度相机建立局部高度图，并结合 contact-aided invariant EKF 做定位。由于相机朝前，后腿当前能踩哪里不能直接看到，所以系统需要融合历史深度图，维护一个机器人周围的局部地图，再从中提取可踩区域和不可踩区域。

实验部分用 Unitree A1 在三类离散地形上测试：aligned、staggered 和 random terrain。结果显示，纯启发式落脚方法在 aligned 和 staggered 上都是 0/3；加入 gait library 但仍用普通 Jacobian 线性化MPC后，aligned 是 1/3，staggered 是 0/3；作者的 GVMPC 在 aligned 是 3/3，staggered 是 2/3，random terrain 是 2/2。失败主要来自脚踩到边缘后打滑，而不是规划完全失效。

这篇文章的价值在于，它把"离散落脚点规划"和"四足动态控制"结合得比较清楚：不是只让机器人盲走，也不是只做视觉落脚点检测，而是让步态库决定"该用什么速度和步长"，再让几何MPC负责稳定执行。局限也很明显：它依赖高度图分割，把地形分成可踩/不可踩区域；小型机器人视角低，缝隙可能被深度图遮挡；同时定位漂移会直接影响落脚精度。

II 混合步态模型

这篇文章里的机器人是 Unitree A1，12 个关节电机，加上机身 6 个自由度，所以完整广义坐标是 18 维：机身位置、机身姿态，以及四条腿各 3 个关节。作者先用完整动力学描述机器人在不同接触状态下的运动，也就是常见的 manipulator equation：惯性项、科氏力项、重力项、关节力矩项、接触力项一起决定机器人运动。这里的关键不是公式本身，而是它说明作者的离线步态库是基于接触约束下的机器人动力学优化出来的。

然后作者把 trot 步态建模成一个 hybrid system。所谓 hybrid，是因为机器人运动不是单一连续动力学，而是在不同接触相之间切换。文章中每个 trot step 包含两类相位：DS，也就是 diagonal double support，对角两条腿支撑；QS，也就是 quadruple support，四条腿都接触地面。脚落地时会发生 impact dynamics，脚离地时又切换到下一个接触相。因此，机器人一边连续运动，一边在 DS/QS 之间离散切换。

这里可以把它理解成：作者不是直接让机器人"随便走"，而是先规定了一个可控的 trot 接触时序。这样做的好处是优化和控制问题会简单很多；坏处是它的行为空间被限制在 trot 类步态附近，不能自然地产生非常自由的跳跃、爬行、侧向跨步等动作。

公式详解

• 公式1 - 连续动力学：
  

  这段话是在说：机器人在某一个接触阶段里的运动，可以用一条"动力学方程"描述。这里的某一个阶段，指的是比如"两条对角腿着地、另外两条腿摆动"的阶段，或者"四条腿都着地"的阶段。 核心公式是： \(D(q)q̈ + C(q,q̇)q̇ + G(q) = Bτ + J_cᵀλ_c\) 你可以把它理解成机器人版的"牛顿第二定律"：左边是"机器人自己运动时需要克服的东西"，右边是"外部给机器人施加的力"。

  • $D(q)q̈$ 是惯性项。意思是：机器人各个部件有质量和惯量，要让它产生加速度$q̈$，需要付出力。$D(q)$ 可以理解成"机器人当前姿态下的质量分布矩阵"。腿伸开、收起来，惯性都会变，所以它依赖 $q$。

  • $C(q,q̇)q̇$ 是科氏力/离心力相关项。简单说，它描述"机器人已经在运动时，由运动本身带来的额外动力学影响"。比如腿快速摆动、身体旋转时，会出现一些耦合效应，不是静止状态下能看出来的。

  • $G(q)$ 是重力项。机器人要站着、抬腿、保持身体高度，都要对抗重力。不同姿态下，重力对各关节的影响也不同，所以它也依赖 $q$。

  • 右边的 $Bτ$ 是电机力矩项。$τ$ 是 12 个电机输出的关节力矩；$B$ 是 selection matrix，因为完整状态 $q$ 包括机身位置、机身姿态和关节角，但电机只能直接驱动关节，不能直接"驱动机身在空中平移"。所以 $B$ 的作用是把 12 维电机力矩放到完整动力学方程里对应的位置。

  • $J_cᵀλ_c$ 是地面对脚的接触力项。$λ_c$ 是脚底受到的地面反作用力，$J_c$ 是接触雅可比矩阵，用来把"脚底接触力"转换成对整个机器人广义坐标的影响。比如脚踩地时，地面对脚的力最终会推动机身、影响关节。

  下面这个约束：

  \[J_c q̈ + J̇_c q̇ = 0\]

  意思是：如果某只脚正在接触地面，那么这只脚不能随便动。更准确地说，接触点的加速度要满足"不穿透地面、不在接触约束方向上乱动"的条件。它是由"接触脚的位置约束"对时间求导得到的。

  所以这段话整体表达的是：

  在每一个步态阶段里，作者用标准机器人动力学方程描述 A1 机器狗的运动。机器人受到三类主要影响：自身惯性、运动耦合、重力；同时又受到两类输入：电机力矩和地面接触力。由于不同阶段着地的腿不同，接触雅可比 $J_c$ 和接触力 $λ_c$ 的维度也会变。比如两条腿着地时，接触力少一些；四条腿着地时，接触力更多。

• 公式2 - 冲击动力学：
  

  这是在讲"脚落地的一瞬间"怎么建模。前一段 Continuous Dynamics 描述的是：脚已经接触地面、或者正在摆动时，机器人连续运动的动力学。这一段 Impact Dynamics 描述的是：摆动腿刚碰到地面的瞬间，速度会突然变化。比如你跑步时脚落地，脚底和地面碰撞，身体速度、腿的速度都会被瞬间"重置"一下。

  这里的 $q̇⁻$ 和 $q̇⁺$ 很重要：$q̇⁻$ 表示碰撞前一瞬间的速度；$q̇⁺$ 表示碰撞后一瞬间的速度。

  作者假设这个碰撞是 instantaneous rigid impact，也就是"瞬时刚性碰撞"。意思是：碰撞时间极短，脚和地面都近似刚体，不考虑软接触的缓慢压缩过程。

  这个矩阵方程的作用是：已知碰撞前速度 $q̇⁻$，求碰撞后的速度 $q̇⁺$，以及碰撞过程中产生的冲量/接触作用 $λ_c$。

  你可以把它理解成两个条件拼在一起：第一行大概对应"动量变化"。碰撞前机器人有动量 $D(q)q̇⁻$，碰撞后变成 $D(q)q̇⁺$，中间差异由地面对脚的冲量造成。第二行是"新接触脚不能继续往地里穿"。也就是碰撞后，接触点在约束方向上的速度要变成 0，所以有 $J_c(q) q̇⁺ = 0$。

  所以这段话整体意思是：当摆动脚撞到地面时，机器人速度会发生瞬时跳变。作者用刚性碰撞模型，根据碰撞前速度和接触约束，计算碰撞后的速度。这个过程不是普通连续微分方程能描述的，所以要单独写成 impact dynamics。

  在整篇文章里，它的作用主要是服务于离线步态优化：作者要生成 physically feasible 的 trot gait，就必须同时考虑连续运动阶段和脚落地瞬间的速度重置。

• 公式3 - 混合模型：
  

  这一段是在把前面两块东西合起来：连续动力学 + 碰撞动力学 = 混合动力学模型。所谓 hybrid model，就是机器人有两种运动模式：一种是 DS，double-support，对角两条腿支撑。比如左前腿 + 右后腿着地，另外两条腿摆动。另一种是 QS，quadruple-support，四条腿都着地。它通常发生在脚刚落地、准备切换支撑腿的短暂阶段。

  所以一个 trot step 不是单纯连续运动，而是：DS：对角两腿支撑，另外两腿摆动；摆动腿落地；QS：四腿短暂同时接触；原来的支撑腿离地； 进入下一个 DS。

  公式里的 $Σ_{ds}$ 和 $Σ_{qs}$ 就是在分别描述 DS 阶段和 QS 阶段。在 DS 里，如果还没触发切换条件，机器人就按连续动力学走：

  \[ẋ = f_{ds}(x) + g_{ds}(x)τ\]

  这里 $x$ 是机器人状态，包括 $q$ 和 $q̇$，也就是位置/姿态/关节角，以及它们的速度。$f_{ds}(x)$ 可以理解成系统自身动力学，$g_{ds}(x)τ$ 是电机力矩带来的影响。

  如果触发了 DS → QS 的条件，就执行：

  \[x⁺ = Δ_{ds→qs}(x⁻)\]

  意思是：摆动脚落地了，发生碰撞，状态从碰撞前 $x⁻$ 瞬间跳到碰撞后 $x⁺$。这个 $Δ$ 就是前面 impact dynamics 给出的速度重置规则。DS → QS 的切换条件是：摆动脚的高度为 0，并且竖直速度小于 0。也就是说，脚碰到地面，而且是向下碰到地面，不是从地面往上离开。

  在 QS 里，如果还没触发切换条件，机器人按 QS 阶段的连续动力学走：

  \[ẋ = f_{qs}(x) + g_{qs}(x)τ\]

  但如果触发 QS → DS 的条件，就进入下一个双腿支撑阶段。这个切换条件是：某些支撑脚的竖直地面反作用力变成 0。这表示这只脚已经不再压地了，可以认为它离地了。这里的 reset map 是恒等映射，也就是：

  \[Δ_{qs→ds} = I\]

  意思是脚离地时没有碰撞，不需要像落地那样重置速度，状态可以连续继承过去。

  所以这一段整体是在说：

  作者把 trot 步态建模成一个在 DS 和 QS 两种接触模式之间来回切换的系统。DS 阶段主要是对角腿支撑和摆动腿前摆；当摆动脚落地，就通过碰撞模型切到 QS；QS 阶段四脚都接触；当某些脚的接触力降为 0，就切回新的 DS 阶段。这个模型是后面离线生成 motion library 的动力学基础。

III 本文方法

A 轨迹优化

接下来是 motion library。作者离线生成一个步态库，每个步态由两个连续步长参数决定，记作 $l_0$ 和 $l_1$。它们不是单个脚的步长，而是左右脚对之间的相对步长组合。作者把可选步长设为 ${-0.2, -0.1, 0, 0.1, 0.2}$ 米，因此组合起来一共有 $5^4$ 个 gait。每个 gait 里面保存的是优化得到的机身线速度、角速度、身体高度、姿态等参考轨迹，后面会交给 MPC 跟踪。

为什么一定要"两步周期"？这是这篇文章比较重要的细节。对双足机器人来说，一步周期或者两步周期比较容易理解；但四足机器人有前后腿、左右腿之间的运动学约束。如果只要求"一步周期"，那么左右两侧脚的净位移必须匹配，否则走完一步之后四只脚的相对构型就变了，机器人身体可能被"拉长"或"挤短"，无法回到周期初始构型。所以作者说，一步周期会额外要求 $l_0(0)+l_1(1)=l_0(1)+l_1(0)$，这会限制可选步长组合。两步周期则多给了一个调整空间，使 l0 和 l1 可以更自由地选择。

这也是这篇文章和普通"固定步态控制"的区别：它不是只有一个 trot gait，而是有一堆不同步长组合的 trot gait。在线遇到不同大小的缝隙时，机器人可以切换到不同 motion primitive，相当于在"慢走、小步、正常步、大步"之间连续调整。

• 公式4 - 轨迹优化
  

  这里的 trajectory optimization 指的是：离线求一条"最优的机器人运动轨迹"。也就是说，作者不是手工规定机器人每个时刻身体怎么动、腿怎么摆、关节力矩是多少，而是把它写成一个优化问题，让优化器自动找出一条满足动力学和约束的步态轨迹。在这篇文章里，它要优化的东西主要是：机器人状态轨迹 ($x(t)$)：比如机身位置、姿态、关节角、速度等；控制输入轨迹 ($\tau(t)$)：也就是 12 个电机在每个时刻输出多大力矩。

  优化目标是：

  \[\min \int_0^T ||\tau(t)||_2^2 dt\]

  意思是：在整段步态中，让电机力矩平方和尽量小。可以粗略理解为"尽量省力/省能量/动作平滑"。 但是它不能随便优化，必须满足一些约束：

  • 第一，必须满足机器人动力学。也就是轨迹不能是物理上不可能的，必须符合前面讲的连续动力学和接触动力学。
  • 第二，必须满足状态和输入限制。比如关节角不能超过机械范围，电机力矩不能超过上限。
  • 第三，必须满足摩擦约束。脚踩地时，接触力不能超出摩擦锥，否则脚会打滑。
  • 第四，必须满足周期性约束。比如 two-step periodic gait 要求走完两步后，机器人状态回到初始状态的平移等价版本。
  • 第五，必须满足步长约束。也就是这个 gait 对应的 $(l_0, l_1)$ 要按指定步长落脚。

  Direct Collocation 是求解这个优化问题的一种方法。它的意思是：不要连续地优化整个时间函数，而是把一段运动时间切成很多离散节点，比如第 0、1、2、…、N 个时刻。优化器只需要决定这些节点上的状态和力矩，然后用约束保证相邻节点之间大致符合动力学。

  所以这段话的整体意思是：

  作者离线为不同的步长组合 $(l_0, l_1)$ 分别求解轨迹优化问题。每求一次，就得到一条满足动力学、接触、摩擦、周期性和步长约束的 trot gait。最后把这些 gait 存成 motion library。在线运行时就不用再解这么复杂的轨迹优化，只需要从库里查找或插值。

B 落足点选择与步态规划

继续往下，这篇文章接下来进入在线部分：Footstep Planning and Gait Selection。

前面离线生成了很多 two-step periodic gait，但机器人在线走的时候，不能只是机械地播放某一个 gait。因为真实地形上哪里能踩、哪里是缝隙，是由视觉地图决定的。所以在线时要做两件事：先决定"下一脚踩哪里"，再决定"用哪个 gait primitive 来配合这个落脚点"。

Footstep Planning 的逻辑比较朴素：先根据当前机器人状态、当前质心速度、期望质心速度，从 gait library 里得到一个 nominal foothold，也就是理想落脚点；然后在视觉地图中找一个离 nominal foothold 最近、同时又属于可踩区域的位置，作为真正的落脚点。这样它不是纯视觉贪心，也不是纯动力学规划，而是把"动力学上合理的位置"和"地形上可踩的位置"折中起来。

Gait Selection 则是：已知当前步长 ($l_0$) 和目标步长 ($l_1$)，从 gait library 中取出对应的 gait。如果刚好库里有这个组合，就直接取；如果落在两个库中步态之间，就用双线性插值。取出来的不是完整关节轨迹直接播放，而主要是 reduced-order rigid-body model 需要的参考状态，比如质心速度、身体高度、身体姿态等，后面交给 MPC 跟踪。

这一步的核心意义是：机器人不是只改脚的位置，而是连身体运动也一起改。比如前面有大缝隙，如果只把脚往远处放，但身体速度还保持原来的 0.25m/s，机器人可能够不到、或者落地后身体被拉崩。motion library 会告诉控制器：为了迈这个步长，身体速度、姿态、角速度应该怎么变化。

然后文章进入 Geometric MPC。这里作者要解决的是"支撑腿怎么发力，才能让身体跟上参考轨迹"。MPC 的输出不是直接的关节角，而是支撑脚的接触力 ($\lambda_c$)。得到接触力后，再通过 ($\tau_{st}=-J_c^T\lambda_c$) 转成支撑腿关节力矩。

为什么要用 Geometric MPC？因为普通四足 MPC 常常用欧拉角表示姿态，并且对 roll/pitch 做小角度近似。但在离散地形上，机器人可能突然俯仰、横滚，角速度也比较大，小角度近似就不可靠。而且欧拉角本身有奇异性问题。作者改用旋转矩阵 ($R\in SO(3)$) 表示机身姿态，在 $SO(3)$ 流形上做线性化，这就是"geometric"的含义。

后面一堆 SO(3) 公式的直观含义是：不要把机身姿态粗暴地当成三个普通角度来减，而是用旋转矩阵之间的差异来定义姿态误差。这样在机器人姿态变化较大时，误差定义和动力学离散化更合理。

C 支撑腿的几何MPC

Geometric MPC 这一节的核心问题其实很简单：机器人已经从 motion library 得到了"身体应该怎么走"的参考轨迹，那么控制器怎么让真实机器人跟上这条轨迹？

作者把四足机器人简化成一个 single rigid body，也就是一个刚体机身。四条腿不再完整建模，而是把支撑腿对地面的作用简化成接触力。MPC 要做的事情就是：在未来一小段时间内，计算每只支撑脚应该给地面多大的力，使机身位置、速度、姿态和角速度尽量接近期望轨迹。

普通 MPC 常用欧拉角表示姿态，比如 roll、pitch、yaw。但作者认为这在离散地形上不够好，因为机器人踩踏脚石时可能产生较大的俯仰、横滚和角速度，小角度近似容易失效；而且欧拉角有奇异性。于是他们用旋转矩阵 ($R\in SO(3)$) 表示姿态，并在 $SO(3)$ 上定义误差。

接下来这些公式是在建立离散时间刚体动力学。比如：

\[p_{k+1}=p_k+\dot p_k\Delta t\]

意思是：下一时刻的位置 = 当前的位置 + 当前速度 × 时间步长。

\[\dot p_{k+1}=\dot p_k+\Delta t g+\frac{f_{k+1}}{m}\Delta t\]

意思是：下一时刻速度由当前速度、重力、以及外力共同决定。

姿态更新写成：

\[R_{k+1}=R_k\Delta R_k\]

这和普通位置加法不一样。位置可以相加，但旋转不能简单相加，所以它用一个增量旋转矩阵 \(\Delta R_k\) 右乘当前姿态。

这里的 $\Delta R_k=\exp(\Delta t\hat\omega_k)$ 可以理解成：角速度 $\omega_k$ 在一个小时间步 $\Delta t$ 内产生了一个小旋转，这个小旋转通过指数映射变成旋转矩阵。你现在不需要深入掌握指数映射，只要知道它是把"角速度向量对应的小旋转"转换成合法旋转矩阵的方法。

然后作者做 linearization，也就是线性化。MPC 为了实时求解，通常要把非线性动力学近似成线性形式：

\[\delta \xi_{k+1}=A_k\delta \xi_k+B_k\delta F_k\]

这里的 $\delta \xi_k$ 是"真实状态相对参考状态的误差"，$\delta F_k$ 是"真实输入相对参考输入的误差"。$A_k,B_k$ 描述误差如何随时间传播。

这一步的重点是：位置误差、速度误差可以直接相减，但姿态误差不能直接用 $R-R_d$。所以作者用：

\[\frac{1}{2}(R_d^TR-R^TR_d)^\vee\]

来表示姿态误差。它本质上是在问：从期望姿态 $R_d$ 转到当前姿态 $R$，需要一个怎样的小旋转？这个小旋转再被表示成三维向量，放进 MPC 的误差状态里。

最后，MPC 优化问题就是：在未来 $N$ 个时间步里，让状态误差 $\delta\xi$ 尽量小，让输入修正 $\delta F$ 不要太大，同时满足动力学、摩擦锥、接触力方向、接触状态等约束。求出来的结果是每只脚的最优接触力 $\lambda_c^*$。

约束里比较重要的有几个：

• 摩擦锥约束：脚底接触力不能太"横"，否则会打滑。

• 竖直力约束：脚只能推地，不能把地面"拉起来"；摆动腿的接触力必须为 0。

• 接触状态约束：当前哪只脚着地，哪只脚摆动，是由步态相位决定的。

• 质心 wrench 约束：四只脚的接触力合起来，要产生期望的机身合力和合力矩。

MPC 输出接触力之后，支撑腿关节力矩通过：

\[\tau_{st}=-J_c^T\lambda_c^*\]

得到。也就是说，控制器先决定"脚应该对地面施加什么效果"，再换算成"关节电机应该出多大力"。

这一节可以概括成一句话：作者用 motion library 给出身体参考轨迹，用 Geometric MPC 在 $SO(3)$ 上更合理地计算姿态误差和动力学误差，再求出支撑腿接触力，让机器人在踏脚石这种不平稳地形上保持稳定。

D 摆动腿控制（Swing Leg Control）

前面讲的 Geometric MPC 主要控制支撑腿，也就是脚已经踩在地上的腿。它计算这些支撑脚应该产生多大的接触力，进而稳定身体。但摆动腿在空中，没有接触力，所以它需要单独控制。

作者的做法比较常规：给摆动脚生成一条 Bezier 曲线轨迹。起点是摆动开始时脚的真实位置，终点是 footstep planner 选出来的目标落脚点。中间轨迹一般会抬起来再落下，避免拖地或撞到边缘。然后用 PD 控制器让脚的位置和速度跟踪这条期望轨迹。

公式里的意思是：

\[\tau_{sw}=J_{sw}^T[-K_p(p_{sw}-p_{sw}^d)-K_d(\dot p_{sw}-\dot p_{sw}^d)]\]

括号里是脚端空间的 PD 控制力：脚的位置偏离目标，就用 $K_p$ 拉回来；脚的速度偏离目标，就用 $K_d$ 阻尼修正。外面的 $J_{sw}^T$ 是把脚端力转换成关节力矩。也就是说，控制器先在"脚的位置空间"里想清楚脚该怎么动，再通过雅可比矩阵转成电机力矩。

到这里，整套在线控制就完整了：视觉建图告诉系统哪里能踩；footstep planner 选择目标落脚点；gait library 给出对应的身体参考速度和姿态；Geometric MPC 控制支撑腿接触力，稳定身体；Swing leg PD 控制摆动腿，把脚送到目标落脚点。

这也是这篇文章的系统性所在：它不是单独提出一个控制器，而是把感知、落脚规划、步态库、支撑腿控制、摆动腿控制串成了一个完整的实机系统。

E 定位与映射

这个部分解决的是：机器人怎么知道哪里能踩。

这篇文章的机器人只有一个前向深度相机，所以它看到的是身体前方的地形。问题是，四足机器人不只是前腿要落脚，后腿也要落脚；而后腿附近的地形往往已经不在当前相机视野里。因此作者不能只看当前一帧深度图，而是要把过去看到过的深度图融合起来，形成一个围绕机器人的局部高度地图。这样，后腿虽然"当前看不见"，但系统还能从历史地图里知道后腿附近哪里是石块、哪里是缝隙。

这里需要两个模块配合。第一个是 localization，也就是估计机器人在世界中的位置和姿态。作者用的是 contact-aided invariant EKF，它会利用 IMU、运动学信息和脚底接触信息来估计机器人位姿。脚底接触信息很重要，因为当某只脚稳定踩在地面上时，它可以作为一个短暂的"参考点"，帮助约束机器人身体的位置估计。

第二个是 mapping，也就是建高度图。深度相机给出前方点云/深度信息，EKF 给出机器人自身位姿，二者融合后就能把不同时间看到的地形拼到同一个局部地图里。作者用的是 robot-centric elevation mapping，也就是以机器人为中心维护一个局部高度图，而不是建一个无限大的全局地图。这样更适合实时控制。

建好高度图后，系统还要判断哪些地方可踩、哪些地方不可踩。作者用高度和法向量方向做阈值判断：比较平、比较像石块表面的地方是 stepable；高度突变、边缘、缝隙附近是 un-stepable。为了避免脚踩到石块边缘，他们还在可踩/不可踩边界附近加了 5cm 安全边界。这个 5cm 很实用，因为真实机器人存在落脚误差、状态估计误差和控制误差。

所以 Mapping 这一节可以概括为：前向深度相机负责看地形，EKF 负责估计机器人在哪里，局部高度图负责保存过去看到的地形，最后从地图里提取可踩区域，供 footstep planner 选择落脚点。