PAPER DEEP DIVE
通过不安全踏步惩罚和稀疏LiDAR高程图实现楼梯全向人形运动
人形机器人自由度多且重心高，本质上不稳定。在楼梯上的安全全向运动需要全向地形感知和可靠的落脚点选择。现有方法通常依赖前向深度相机，产生盲区限制全向感知。本文使用稀疏LiDAR高程图实现全向楼梯感知，通过不安全踏步惩罚确保全向安全行走。
一、研究背景与动机
人形机器人拥有拟人化的外形和多自由度结构，在家庭服务、灾后救援和工业巡检等非结构化环境中具有广阔的应用前景。然而与四足机器人相比，人形机器人重心更高、脚底更大，在攀爬楼梯等复杂地形时面临严峻的平衡与稳定性挑战。安全的全向楼梯通行不仅需要感知周围地形，还需要精确的落脚点选择策略，任何一次危险的落脚都可能导致硬件损坏或跌倒。
现有方法大多依赖安装在机器人头部的深度相机来获取前方地形信息，但前向视野造成了大范围的侧向和后方盲区，严重限制了机器人的全向移动能力。此外，深度图像容易受到光照变化和运动模糊的影响，进一步削弱了策略的鲁棒性。在落脚点安全方面，传统的不安全步惩罚是稀疏的——只有在脚与台阶发生物理碰撞或踩到边缘后才施加惩罚，这种延迟反馈无法有效引导策略学习精确的落脚位置，导致训练收敛慢或策略过于保守。
本文针对上述两大瓶颈——感知盲区与稀疏安全约束——提出了一个单阶段全向感知运动框架，核心创新包括：密集不安全步惩罚函数和基于LiDAR的边缘引导高程图重建系统。
二、核心方法概述
整个框架分为仿真训练和真实部署两个阶段。在仿真中，策略网络通过强化学习学习安全的楼梯通行步态；在真实部署中，训练好的策略直接迁移到物理机器人上，配合点云建图和边缘引导高程图模块提供地形感知输入。框架的核心组件包括：策略网络与评论家网络、密集不安全步惩罚、地形课程学习、时空滚动建图与自保护区机制、以及边缘引导非对称U-Net（EGAU）重建网络。
图2：框架总览。(A)仿真训练阶段：MLP和CNN分别提取本体感知和感知特征后拼接，不安全步惩罚引导策略学习安全步态。(B)真实部署阶段：策略网络直接迁移，点云建图与边缘引导高程图模块提供稳定输入。
三、强化学习训练框架
由于机载传感器无法获取完整状态，问题被建模为部分可观测马尔可夫决策过程（POMDP），定义为元组 $\mathcal{M}=\langle\mathcal{S},\mathcal{A},\mathcal{P},\mathcal{O},r,\gamma\rangle$，其中 $\mathcal{S}$ 和 $\mathcal{A}$ 分别为连续全状态和动作空间，$\mathcal{O}$ 为部分观测空间。策略优化的目标是最大化期望折扣回报：
$$\max_{\pi_\phi} J(\mathcal{M},\pi_\phi) = \mathbb{E}\left[\sum_{t=0}^{\infty} \gamma^t r(\mathbf{s}_t, \mathbf{a}_t)\right]$$训练采用近端策略优化（PPO）算法。策略网络的输入包含两部分：本体感知观测 $\mathbf{o}_t$ 和高程图历史 $\mathbf{e}_{t-h:t}$。本体感知观测直接从传感器获取：
$$\mathbf{o}_t = \left[\boldsymbol{\omega}_t \quad \mathbf{g}_t \quad \mathbf{c}_t \quad \boldsymbol{\theta}_t \quad \dot{\boldsymbol{\theta}}_t \quad \mathbf{a}_{t-1}\right]^T$$该向量包含基座角速度 $\boldsymbol{\omega}_t$、机体系下的重力投影 $\mathbf{g}_t$、线速度指令 $\mathbf{c}_t$、关节位置 $\boldsymbol{\theta}_t$、关节速度 $\dot{\boldsymbol{\theta}}_t$ 和上一步动作 $\mathbf{a}_{t-1}$。高程图历史由 $h=5$ 帧连续的机器人中心局部高程图堆叠而成，分辨率为0.05m，覆盖范围1.4m×1.0m。本体感知通过MLP处理，高程图通过CNN处理，两组特征拼接后输入actor头输出动作。
评论家网络采用相同架构，但输入为特权观测 $\mathbf{s}_t$（额外包含真实基座线速度）和无噪声高程图。动作空间定义为关节电机的相对目标位置，策略输出 $\mathbf{a}_t$ 加上默认站立关节位置 $\boldsymbol{\theta}_{\text{def}}$ 得到期望关节位置，再通过PD控制器转换为关节力矩：
$$\boldsymbol{\tau} = \mathbf{k}_p(\boldsymbol{\theta}_{\text{des}} - \boldsymbol{\theta}) + \mathbf{k}_d(\dot{\boldsymbol{\theta}}_{\text{des}} - \dot{\boldsymbol{\theta}})$$其中 $\mathbf{k}_p$ 和 $\mathbf{k}_d$ 分别为比例和微分增益。
四、密集不安全步惩罚
本文最核心的创新之一是密集不安全步惩罚函数。传统方法仅在碰撞发生后才施加惩罚，而本文设计了一个在危险落脚发生之前就提供连续负反馈的密集惩罚。该惩罚由两部分组成：足碰撞项和边缘接触项。
图3：密集不安全步惩罚示意图。左图为足碰撞惩罚计算，右图为边缘接触惩罚计算。
足碰撞项：首先获取足部在XY平面的速度向量 $\mathbf{v}_{xy}$，然后在以该速度方向为中心的30°锥形范围内找到最近障碍物向量 $\mathbf{d}_{xy}$。碰撞惩罚基础值计算为速度在障碍物方向上的投影：
$$p_{\text{colli}} = \max\left(0, \frac{\mathbf{v}_{xy} \cdot \mathbf{d}_{xy}}{\|\mathbf{d}_{xy}\|}\right)$$当脚远离台阶时不应施加惩罚，因此引入安全距离项：
$$d_{\text{colli}} = \max\left(0, 1 - \frac{\|\mathbf{d}_{xy}\|}{d_{\text{unsafe}}}\right)$$其中 $d_{\text{unsafe}}$ 为惩罚生效的距离阈值。为区分楼梯和普通斜坡，计算 $\mathbf{d}_{xy}$ 终点处的地形坡度 $s$，当 $s$ 低于阈值 $\epsilon_{\text{slope}}$ 时判定为斜坡不施加惩罚。总的足碰撞惩罚为：
$$r_{\text{colli}} = -\mathds{1}(s > \epsilon_{\text{slope}}) \cdot p_{\text{colli}} \cdot d_{\text{colli}}$$边缘接触项：利用Sobel算子计算脚下局部高程图的梯度幅值 $\mathbf{G} = \|\nabla\mathbf{H}_{\text{fl}}\|_2$，梯度超过阈值的点标记为边缘点，计算所有边缘点的几何质心 $\mathbf{e}_c$，构建从脚心 $\mathbf{f}_c$ 到边缘质心的向量 $\mathbf{e}_{xy}$。取当前线速度指令 $\mathbf{v}_{xy}^{\text{cmd}}$，边缘惩罚基础值为：
$$p_{\text{edge}} = s_f \cdot \min\left(0, \mathbf{e}_{xy} \cdot \frac{\mathbf{v}_{xy}^{\text{cmd}}}{\|\mathbf{v}_{xy}^{\text{cmd}}\|}\right)$$其中符号修正因子 $s_f = -\text{sgn}(\mathbf{g} \cdot \mathbf{v}_{xy}^{\text{cmd}})$，$\mathbf{g}$ 为局部平均梯度向量。该设计使得上楼时边缘在脚前半部分产生负惩罚，在脚后半部分不产生惩罚。进一步引入高度权重项：
$$d_{\text{edge}} = \max\left(0, 1 - \frac{\mathbf{d}_z}{d_{\text{min}}}\right)$$最终边缘惩罚为 $r_{\text{edge}} = p_{\text{edge}} \cdot d_{\text{edge}}$。总不安全步惩罚为两项的加权和：
$$r_{\text{safe}} = w_1 \cdot r_{\text{colli}} + w_2 \cdot r_{\text{edge}}$$这种密集惩罚设计使策略在脚接近危险位置之前就能获得连续的负反馈信号，从而主动调整落脚点，而非等到碰撞发生后才被动学习。
五、奖励函数设计
总奖励是各奖励项的加权和，下表列出了训练中使用的全部奖励项：
| 奖励项 | 表达式 | 权重 |
|---|---|---|
| 线速度跟踪 | $\exp(-4\|\mathbf{v}_{xy}-\mathbf{v}_{xy}^{\text{cmd}}\|^2)$ | 5.0 |
| 角速度跟踪 | $\exp(-4\|\boldsymbol{\omega}_z-\boldsymbol{\omega}_z^{\text{cmd}}\|^2)$ | 5.0 |
| 基座高度 | $\exp(-400\|h-h_{\text{tar}}\|^2)$ | 1.0 |
| 足部空中时间 | $\mathds{1}_{\mathbf{c}_i=1}\cdot\min(t_{\text{air}}, t_c, 1.0)$ | 7.0 |
| 基座Z向线速度 | $-\|\mathbf{v}_z\|^2$ | −1.0 |
| 基座XY角速度 | $-\|\boldsymbol{\omega}_{xy}\|^2$ | −0.05 |
| 基座朝向 | $-\|\mathbf{g}\|^2$ | −6.0 |
| 关节动作速率 | $-\|\dot{\mathbf{a}}_t\|^2$ | −0.01 |
| 关节动作平滑 | $-\|\ddot{\mathbf{a}}_t\|^2$ | −0.01 |
| 关节加速度 | $-\|\ddot{\boldsymbol{\theta}}\|^2$ | −1e−6 |
| 关节力矩 | $-\|\boldsymbol{\tau}\|^2$ | −0.2 |
| 足部滑动 | $\sum_{i}\mathds{1}_{\mathbf{c}_i=1}\|\mathbf{v}_{i,xy}\|^2$ | −0.1 |
| 关节偏移 | $-\sum_j\|\theta_j-\theta_{\text{nom},j}\|^2$ | −0.5 |
| 终止惩罚 | $\mathds{1}(\text{terminate})$ | −250.0 |
| 不安全步惩罚 | 式(10) | 1.0 |
其中线速度和角速度跟踪项权重最高（5.0），鼓励策略精确跟随指令速度；足部空中时间权重为7.0，鼓励合理的步态节奏；终止惩罚高达−250.0，强烈抑制导致摔倒的行为；不安全步惩罚权重为1.0，提供连续的安全引导信号。
六、地形课程学习
为使策略能够从简单任务逐步过渡到复杂地形，训练中采用地形课程机制。当机器人在当前难度级别成功完成任务时晋升到下一级，否则降级。共定义10个难度级别，地形类型包括楼梯、斜坡和平地。楼梯地形分为上行和下行，踏面深度均匀采样自[0.25, 0.6]m，台阶高度自[0, 0.23]m。斜坡角度自[0, 0.4]rad。所有子地形的难度参数随课程级别线性增加，确保策略在渐进式挑战中稳步提升能力。
七、时空滚动建图与自保护区机制
为在全向运动中获取连续稳定的地形特征，本文从点云构建时空滚动地图。朴素地累积观测会因为环境动态和里程计漂移产生鬼影伪影，因此引入时间置信度衰减机制逐步过滤过时点云。然而传统超时衰减对人形机器人存在关键缺陷：当机器人原地踏步或执行缓慢精细动作时，身体下方物理盲区内的地形历史记忆会在超时后被错误丢弃。为解决此问题，本文提出自保护区机制——当有效点落在机器人基座正下方的圆柱形空间 $\mathcal{Z}_{\text{safe}}$ 内时，这些点的时间置信度被锁定为最大值，防止丢失身体下方关键地形记忆。
图4：时空滚动建图与自保护区机制示意图。点云颜色表示时间置信度，红色锥形区域为LiDAR物理盲区，浅蓝色圆柱为机器人基座下方的自保护区 $\mathcal{Z}_{\text{safe}}$。
地图维护采用异步解耦架构，以两种频率运行：全局点云地图以10Hz进行增量空间融合和置信度更新；状态提取模块以不低于50Hz的频率构建机器人中心局部点云地图，裁剪1.4m×1.0m局部区域并以0.05m分辨率进行2.5D栅格化，输出初始稀疏点云图和无效单元掩码，作为后续重建网络的原始输入。
八、边缘引导非对称U-Net（EGAU）
为从稀疏栅格化局部地图中精确重建高程图，本文提出EGAU架构。该网络采用特征级联设计，包含单一编码器和双解码器（高度解码流和边缘解码流）。核心思想是在逐层恢复空间分辨率的过程中显式注入几何边界先验，克服线性插值固有的过平滑效应。
在第 $i$ 层解码阶段（$i \in \{1,2,3,4\}$，$i=1$ 为最低分辨率瓶颈层），高度解码流的特征图计算为：
$$\mathbf{F}_h^{(i)} = \mathcal{D}_h^{(i)}\left(\left[\mathbf{F}_{\text{enc}}^{(i)}, \mathcal{U}(\mathbf{F}_h^{(i-1)}), \Phi(\mathbf{F}_{\text{edge}}^{(i)})\right]\right)$$其中 $\mathcal{D}_h^{(i)}$ 为第 $i$ 层高度流解码器，$\mathbf{F}_{\text{enc}}^{(i)}$ 为编码器对应层的跳跃连接特征，$\mathcal{U}(\cdot)$ 为双线性上采样，$\Phi(\cdot)$ 为特征对齐函数，将边缘解码流提取的隐藏特征 $\mathbf{F}_{\text{edge}}^{(i)}$ 显式注入高度解码流。这种级联拓扑使边缘流在逐层解码过程中持续向高度流提供强几何边界先验，有效抑制稀疏输入导致的跨边缘平滑插值。
九、区域解耦混合损失
在稀疏点云输入下，全局均匀回归损失无法精确恢复地形物理结构。本文提出细粒度的区域感知损失，总损失为：
$$\mathcal{L}_{\text{total}} = \mathcal{L}_h + \lambda_e \mathcal{L}_e + \lambda_r \mathcal{L}_r + \lambda_s \mathcal{L}_s + \lambda_g \mathcal{L}_g$$首先用Sobel算子计算真值高程图的梯度幅值 $\mathbf{M}_{\text{gt}} = \|\nabla\mathbf{H}_{\text{gt}}\|_2$，从中导出边缘掩码 $\mathbf{M}_{\text{edge}}$ 和平坦区域掩码 $\mathbf{M}_{\text{flat}}$。除标准全局高度回归损失 $\mathcal{L}_h$ 和边缘分类损失 $\mathcal{L}_e$ 外，引入三个区域特定惩罚：边缘感知回归损失 $\mathcal{L}_r$ 在边缘区域内施加额外L1惩罚以纠正台阶边缘高度塌陷；平滑损失 $\mathcal{L}_s$ 仅在平坦区域操作，通过一阶差分的L1和L2组合惩罚抑制稀疏插值产生的高频噪声；自适应梯度损失 $\mathcal{L}_g$ 定义为：
$$\mathcal{L}_g = \frac{1}{N}\sum (1 + \alpha \mathbf{M}_{\text{gt}}) \odot |\mathbf{M}_{\text{pred}} - \mathbf{M}_{\text{gt}}|$$其中 $\mathbf{M}_{\text{pred}}$ 为重建高程图的梯度幅值，$\alpha$ 为自适应放大系数，在真值梯度陡峭的区域显式约束网络拟合物理直角。
为弥合仿真与现实的感知差距，仿真数据生成管线中加入了基于物理的传感器噪声模型。由于真实固态LiDAR在大入射角（如楼梯立面）处频繁丢失回波信号，仿真中根据地形局部梯度条件施加射线丢弃机制，迫使网络适应严重不完整的点云输入，大幅增强真实部署鲁棒性。
十、运动策略实验结果
仿真实验在Isaac Lab中训练，使用单块NVIDIA RTX 4090 GPU。机器人平台为Unitree G1人形机器人（29自由度），头部安装Livox Mid-360 LiDAR（360°×59°视场角）。真实部署中RL控制器和重建模块运行在NVIDIA Jetson Orin NX边缘计算平台上，里程计由DLIO提供（100Hz），RL控制器运行频率50Hz。
图5：仿真综合性能对比。(a)各方法安全步率随台阶高度的变化，上行为上行，下行为下行。(b)地形课程级别随训练迭代的变化。(c)以0.7m/s指令速度上行15cm楼梯时的前进线速度跟踪。
运动策略对比设置了四组实验：Naive（Isaac Lab默认框架，简单MLP，无安全惩罚）、Ours w/o penalty（本文网络架构但无安全惩罚）、PIM（单阶段混合内部模型方法）、Ours（完整方法）。结果表明：Naive因简单网络结构无法从大量感知信息中学习，成功率随台阶高度增加而下降；PIM和Ours w/o penalty虽能通行超过25cm高度的楼梯，但无安全惩罚时策略倾向于采用激进步态，频繁碰撞台阶立面和踩踏边缘；Ours方法学会谨慎抬脚并选择安全落脚点，在所有可通行地形上实现近100%安全步率，显著优于所有其他方法。课程训练曲线显示Ours在约3000次迭代时最先达到最高地形级别，确认密集惩罚使策略更快习得楼梯通行技能。
十一、高程图重建消融实验
下表展示了网络架构和损失函数的消融结果：
| 网络 | $\mathcal{L}_r$ | $\mathcal{L}_s$ | $\mathcal{L}_g$ | G-MSE↓ | E-MAE↓ | F-MAE↓ | F-Rgh↓ |
|---|---|---|---|---|---|---|---|
| Baseline | 2.05 | 1.32 | 5.57 | 2.57 | |||
| EGAU | 1.99 | 1.18 | 5.89 | 2.26 | |||
| EGAU | ✓ | 1.54 | 0.93 | 5.58 | 2.36 | ||
| EGAU | ✓ | ✓ | 1.46 | 0.87 | 5.41 | 1.87 | |
| EGAU | ✓ | ✓ | ✓ | 1.23 | 0.75 | 4.24 | 1.32 |
完整模型（EGAU + 全部区域解耦损失）在所有指标上均取得最佳性能。Baseline方法（后分支U-Net）在台阶边缘处误差最高，表明标准架构在稀疏输入下倾向过平滑几何边界。EGAU的引入通过注入边缘边界先验显著降低了E-MAE。边缘感知回归损失 $\mathcal{L}_r$ 纠正了台阶边缘的绝对高度塌陷，平滑损失 $\mathcal{L}_s$ 严格抑制平坦区域高频噪声（F-Rgh显著降低），自适应梯度损失 $\mathcal{L}_g$ 显式惩罚梯度失真以保持锐利的不连续性。
十二、真实世界实验
完整感知模型在Jetson Orin NX上通过Onnxruntime-GPU运行，模型仅2.76M参数，单帧推理延迟2ms，轻松满足50Hz控制循环需求。室内实验中机器人在标准楼梯（高15cm，踏面宽20cm）上实现前向、侧向和后向通行，一致避免边缘踩踏和立面碰撞。
图6：真实世界实验结果。(A)室内楼梯通行：前向上行、前向下行、后向下行、侧向下行。(B)长距离户外行走，成功穿越下坡、平地和楼梯。
图1：Unitree G1人形机器人全向楼梯通行概览，包括前向上行、侧向上行和后向上行。
户外长距离实验中，机器人完成了超过400米的连续不间断行走（最终手动终止），依次穿越山坡下坡、平地和楼梯（含上行和下行），全程保持稳定步态和安全落脚，不受周围行人和车辆影响。该结果证明方法具有出色的长期稳定性和复杂户外环境操作能力。
十三、方法流程图
+自保护区机制"] B --> C["稀疏栅格化
局部点云地图"] C --> D["EGAU边缘引导
非对称U-Net重建"] D --> E["高程图历史
5帧堆叠"] F["本体感知观测"] --> G["MLP编码"] E --> H["CNN编码"] G --> I["特征拼接"] H --> I I --> J["Actor策略网络
输出关节目标"] J --> K["PD控制器
输出关节力矩"] K --> L["机器人运动"] L --> A L --> F M["密集不安全步惩罚
足碰撞项+边缘项"] --> J
十四、局限性与未来方向
本文坦诚指出两个主要局限：第一，全向2.5D高程图的表示能力有限，无法有效表示沟渠等下凹地形，这类地形需要更丰富的3D表示方法；第二，传感器引起的失真导致真实部署性能较仿真略有下降，尽管射线丢弃机制已大幅缓解此问题，但sim-to-real差距仍然存在。未来方向可能包括引入3D地形表示以处理沟渠等复杂地形，以及进一步优化传感器噪声模型以缩小仿真到现实的差距。
十五、总结
本文提出了一个鲁棒的全向人形机器人楼梯通行框架。密集不安全步惩罚在危险落脚发生前提供连续反馈引导落脚点选择，显著提升安全性和学习效率；基于LiDAR的滚动建图配合边缘引导重建大幅减少盲区影响，产出一致的地形几何信息。在Unitree G1上的零样本sim-to-real实验验证了室内楼梯和复杂户外地形上的可靠全向移动能力，407.9米户外连续行走测试证明了长期稳定性。该工作为人形机器人在真实环境中的安全自主通行提供了有力方案。
