Omnidirectional Humanoid Locomotion on Stairs via Unsafe Stepping Penalty and Sparse LiDAR Elevation Mapping

1. Background and Motivation

Humanoid robots, with their anthropomorphic morphology and versatile motion capabilities, hold significant potential for deployment in unstructured environments such as domestic service, post-disaster rescue, and industrial inspection. However, compared with quadrupedal robots, humanoids exhibit a higher center of gravity and larger foot soles, presenting serious balance and stability challenges when traversing rough terrain such as stairs. Safe omnidirectional stair traversal requires both omnidirectional terrain perception and reliable foothold selection, as any single hazardous footstep can cause hardware damage or a fall.

Existing methods predominantly rely on forward-facing depth cameras mounted on the robot head, which create substantial lateral and rear blind zones that severely restrict omnidirectional mobility. Moreover, depth images are susceptible to illumination changes and motion blur, further undermining policy robustness. Regarding foothold safety, conventional unsafe-stepping penalties are sparse—punishment is applied only after a physical collision or edge contact has already occurred. Such delayed feedback fails to steer the policy toward precise foot placements, resulting in slow convergence or overly conservative suboptimal strategies.

This paper addresses both bottlenecks—perception blind zones and sparse safety constraints—with a single-stage omnidirectional perceptive locomotion framework, featuring a dense unsafe stepping penalty and a LiDAR-based edge-guided elevation map reconstruction system.

2. Framework Overview

The framework consists of a simulation training stage and a real-world deployment stage. In simulation, the policy network learns safe stair-traversal gaits via reinforcement learning. In deployment, the trained policy is directly transferred to the physical robot, supported by point-cloud mapping and edge-guided elevation map modules for terrain perception. Core components include the policy and critic networks, the dense unsafe stepping penalty, terrain curriculum learning, spatiotemporal rolling mapping with a self-protection zone, and the Edge-Guided Asymmetric U-Net (EGAU) reconstruction network.

Figure 2: Framework overview. (A) Simulation training: MLP and CNN extract proprioceptive and perceptive features respectively, concatenated and guided by the unsafe stepping penalty. (B) Real-world deployment: policy directly transferred with point-cloud mapping and edge-guided elevation map providing stable inputs.

3. Reinforcement Learning Formulation

Since onboard sensors cannot access the complete state, the problem is formulated as a Partially Observable Markov Decision Process (POMDP), defined as $\mathcal{M}=\langle\mathcal{S},\mathcal{A},\mathcal{P},\mathcal{O},r,\gamma\rangle$, where $\mathcal{S}$ and $\mathcal{A}$ are the continuous full state and action spaces, and $\mathcal{O}$ is the partial observation space. The policy optimization objective is to maximize the expected discounted return:

Training uses Proximal Policy Optimization (PPO). The policy network input comprises proprioceptive observation $\mathbf{o}_t$ and elevation map history $\mathbf{e}_{t-h:t}$. The proprioceptive observation is obtained directly from sensors:

This vector includes the base angular velocity $\boldsymbol{\omega}_t$, gravity projection in the body frame $\mathbf{g}_t$, linear velocity command $\mathbf{c}_t$, joint positions $\boldsymbol{\theta}_t$, joint velocities $\dot{\boldsymbol{\theta}}_t$, and previous action $\mathbf{a}_{t-1}$. The elevation map history stacks $h=5$ consecutive robot-centric local elevation maps at 0.05m resolution covering 1.4m x 1.0m. Proprioceptive data is processed by an MLP and elevation maps by a CNN, with features concatenated and fed to the actor head.

The critic network uses the same architecture but takes privileged observation $\mathbf{s}_t$ (additionally including ground-truth base linear velocity) and noise-free elevation maps. The action space defines relative target positions for joint motors; the policy output $\mathbf{a}_t$ is added to the default standing joint position $\boldsymbol{\theta}_{\text{def}}$ to form desired positions, then converted to joint torques via a PD controller:

where $\mathbf{k}_p$ and $\mathbf{k}_d$ are the proportional and derivative gains.

4. Dense Unsafe Stepping Penalty

The core innovation is the dense unsafe stepping penalty. Unlike conventional methods that apply punishment only after collision, this penalty provides continuous negative feedback before a hazardous step occurs. It consists of a foot-collision term and an edge-contact term.

Figure 3: Dense unsafe stepping penalty illustration. Left: foot-collision penalty computation. Right: edge-stepping penalty computation.

Foot-collision term: The foot velocity vector $\mathbf{v}_{xy}$ in the XY plane is obtained, and the nearest obstacle vector $\mathbf{d}_{xy}$ is found within a 30-degree cone centered on the velocity direction. The collision penalty base value is the velocity projection onto the obstacle direction:

No penalty should apply when the foot is far from a step, so a safety distance term is introduced:

where $d_{\text{unsafe}}$ is the distance threshold. To distinguish stairs from slopes, the terrain slope $s$ at the endpoint of $\mathbf{d}_{xy}$ is computed; when $s$ falls below threshold $\epsilon_{\text{slope}}$, the surface is classified as a slope and no penalty applies. The total foot-collision penalty is:

Edge-contact term: A Sobel operator computes the gradient magnitude $\mathbf{G} = \|\nabla\mathbf{H}_{\text{fl}}\|_2$ of the local elevation map beneath the foot. Points exceeding a threshold are marked as edge points, whose geometric centroid $\mathbf{e}_c$ is computed. The vector $\mathbf{e}_{xy}$ from foot center $\mathbf{f}_c$ to edge centroid is constructed. Taking the linear velocity command $\mathbf{v}_{xy}^{\text{cmd}}$, the edge penalty base value is:

where the sign correction factor $s_f = -\text{sgn}(\mathbf{g} \cdot \mathbf{v}_{xy}^{\text{cmd}})$ with $\mathbf{g}$ being the local mean gradient vector. This design ensures that during stair ascent, a negative penalty arises when the edge lies under the front half of the foot, while no penalty applies when it lies under the rear half. A height weighting term is further introduced:

The final edge penalty is $r_{\text{edge}} = p_{\text{edge}} \cdot d_{\text{edge}}$. The total unsafe stepping penalty is the weighted sum:

This dense design enables the policy to receive continuous negative feedback as the foot approaches a hazardous position, proactively adjusting foot placement rather than passively learning after collision.

5. Reward Function Design

The total reward is a weighted sum of individual terms. The table below lists all reward terms used in training:

Linear and angular velocity tracking carry the highest weights (5.0), encouraging precise velocity following. Feet air time at 7.0 promotes proper gait rhythm. The termination penalty of -250.0 strongly suppresses fall-inducing behaviors, while the unsafe stepping penalty at 1.0 provides continuous safety guidance.

6. Terrain Curriculum Learning

To enable progressive skill acquisition from simple to complex terrains, a terrain curriculum is employed. When the robot successfully completes the task at a given difficulty level, it is promoted; otherwise demoted. Ten difficulty levels are defined, with terrain types including stairs, slopes, and flat ground. Stair terrain includes ascending and descending variants with tread depths sampled from [0.25, 0.6]m and step heights from [0, 0.23]m. Slope angles range from [0, 0.4]rad. All sub-terrain difficulty parameters increase linearly with curriculum level.

7. Spatiotemporal Rolling Mapping and Self-Protection Zone

To acquire continuous and stable terrain features during omnidirectional motion, a spatiotemporal rolling map is constructed from point clouds. Naive accumulation produces ghosting artifacts due to environmental dynamics and odometry drift, so a temporal confidence decay mechanism progressively filters aging point clouds. However, conventional timeout-based decay has a critical flaw for humanoid robots: when stepping in place or performing slow maneuvers, historical terrain memory within the physical blind zone beneath the body is erroneously discarded upon timeout expiration. To address this, a self-protection zone mechanism is proposed—when valid points fall within the cylindrical space $\mathcal{Z}_{\text{safe}}$ directly beneath the robot base, their temporal confidence is locked to the maximum value, preventing loss of critical terrain memory.

Figure 4: Spatiotemporal rolling mapping and self-protection zone. Point cloud colors indicate temporal confidence. The red conical region is the LiDAR physical blind zone; the light blue cylinder denotes the self-protection zone $\mathcal{Z}_{\text{safe}}$.

Map maintenance uses an asynchronous decoupled architecture at two frequencies: the global point cloud map undergoes incremental spatial fusion and confidence updates at 10Hz; the state extraction module constructs a robot-centric local point cloud map at no less than 50Hz, cropping a 1.4m x 1.0m region rasterized at 0.05m resolution into an initial sparse map and invalid cell mask as input to the reconstruction network.

8. Edge-Guided Asymmetric U-Net (EGAU)

To accurately reconstruct elevation maps from sparse rasterized local maps, the EGAU architecture is proposed. It employs a feature-cascaded design with a single encoder and dual decoders (height and edge streams). The core idea is to explicitly inject geometric boundary priors during progressive spatial resolution recovery, overcoming the over-smoothing inherent in linear interpolation.

At decoding level $i$ ($i \in \{1,2,3,4\}$, $i=1$ being the bottleneck), the height decoding stream feature map is computed as:

where $\mathcal{D}_h^{(i)}$ is the height-stream decoder at level $i$, $\mathbf{F}_{\text{enc}}^{(i)}$ is the skip-connection feature from the encoder, $\mathcal{U}(\cdot)$ is bilinear upsampling, and $\Phi(\cdot)$ is a feature alignment function injecting edge stream features $\mathbf{F}_{\text{edge}}^{(i)}$ into the height stream. This cascaded topology enables the edge stream to continuously supply strong geometric boundary priors to the height stream during layer-by-layer decoding, effectively suppressing cross-edge smooth interpolation caused by sparse inputs.

9. Region-Decoupled Hybrid Loss

Under sparse point cloud inputs, a globally uniform regression loss is insufficient for accurate terrain reconstruction. A fine-grained Region-Aware Loss is proposed, with total loss:

A Sobel operator computes the ground-truth gradient magnitude $\mathbf{M}_{\text{gt}} = \|\nabla\mathbf{H}_{\text{gt}}\|_2$, from which edge mask $\mathbf{M}_{\text{edge}}$ and flat-region mask $\mathbf{M}_{\text{flat}}$ are derived. Beyond the standard global height regression loss $\mathcal{L}_h$ and edge classification loss $\mathcal{L}_e$, three region-specific penalties are introduced: the Edge-aware Regression Loss $\mathcal{L}_r$ imposes additional L1 penalty within $\mathbf{M}_{\text{edge}}$ to correct height collapse at step edges; the Smoothness Loss $\mathcal{L}_s$ operates exclusively in flat regions, suppressing high-frequency noise via combined L1 and L2 penalties on first-order differences; the Adaptive Gradient Loss $\mathcal{L}_g$ is defined as:

where $\mathbf{M}_{\text{pred}}$ is the reconstructed elevation map gradient magnitude and $\alpha$ is an adaptive amplification coefficient constraining the network to fit physical right angles in steep-gradient regions.

To bridge the sim-to-real perception gap, a physics-based sensor noise model is incorporated into simulation data generation. Since real solid-state LiDAR frequently loses return signals at large incidence angles (e.g., stair risers), a ray-drop mechanism conditioned on local terrain gradient is applied in simulation, forcing the network to accommodate severely incomplete point cloud inputs and substantially enhancing real-world robustness.

10. Locomotion Policy Experiments

Simulation training uses Isaac Lab on a single NVIDIA RTX 4090 GPU. The platform is the Unitree G1 humanoid robot (29 DoF) with a Livox Mid-360 LiDAR (360x59 degree FoV) on the head. In real deployment, the RL controller and reconstruction module run on NVIDIA Jetson Orin NX; odometry is provided by DLIO at 100Hz, with the RL controller at 50Hz.

Figure 5: Comprehensive simulation performance comparison. (a) Safe stepping rate vs. stair height for each method. (b) Terrain curriculum level progression over training. (c) Forward linear velocity tracking on 15cm stairs at 0.7m/s command.

Four experimental groups are compared: Naive (Isaac Lab default, simple MLP, no safety penalty), Ours w/o penalty (proposed architecture without safety penalty), PIM (single-stage hybrid internal model), and Ours (full method). Results show Naive fails to learn from perceptive information with success rate dropping as stair height increases. PIM and Ours w/o penalty can traverse stairs exceeding 25cm but adopt aggressive gaits with frequent collisions and edge stepping. Ours learns cautious foot lifting and safe foothold selection, achieving near-100% safe stepping rate across all passable terrains, significantly outperforming all others. Curriculum curves show Ours reaches the highest terrain level first at around 3000 iterations, confirming that the dense penalty enables faster skill acquisition.

11. Elevation Map Reconstruction Ablation

The table below presents ablation results for network architecture and loss functions:

The full model (EGAU + all region-decoupled losses) achieves the best performance across all metrics. The Baseline (late-branching U-Net) yields the highest edge errors, indicating standard architectures over-smooth geometric boundaries under sparse inputs. EGAU injection significantly reduces E-MAE through edge boundary priors. The edge-aware regression loss $\mathcal{L}_r$ corrects absolute height collapse at step edges; the smoothness loss $\mathcal{L}_s$ strictly suppresses high-frequency noise in flat areas (significant F-Rgh reduction); the adaptive gradient loss $\mathcal{L}_g$ explicitly penalizes gradient distortion to preserve sharp discontinuities.

12. Real-World Experiments

The full perception model runs on Jetson Orin NX via Onnxruntime-GPU with only 2.76M parameters and 2ms single-frame inference latency, easily satisfying the 50Hz control loop. Indoor experiments demonstrate traversal of standard stairs (15cm height, 20cm tread) in forward, lateral, and backward directions, consistently avoiding edge stepping and riser collisions.

Figure 6: Real-world results. (A) Indoor stair traversal: forward ascent, forward descent, backward descent, lateral descent. (B) Long-distance outdoor walking traversing downhill slopes, flat ground, and stairs.

Figure 1: Omnidirectional stair traversal overview on Unitree G1, including forward ascent, lateral ascent, and backward ascent.

In the outdoor long-distance experiment, the robot completed over 400 meters of continuous uninterrupted walking (terminated manually), traversing downhill slopes, flat ground, and stairs (both ascending and descending) while maintaining stable gait and safe foot placement, unaffected by surrounding pedestrians and vehicles. This demonstrates excellent long-term stability and capability in complex outdoor environments.

13. Method Pipeline

14. Limitations and Future Directions

The authors acknowledge two main limitations: first, the omnidirectional 2.5D elevation map representation cannot effectively represent ditches and similar concave terrains, which require richer 3D representations; second, sensor-induced distortions cause slight real-world performance degradation compared to simulation, though the ray-drop mechanism substantially mitigates this. Future directions may include 3D terrain representation for ditch-like terrains and further sensor noise model optimization to close the sim-to-real gap.

15. Conclusion

This paper presents a robust framework for safe omnidirectional humanoid stair traversal. The dense unsafe stepping penalty provides continuous feedback before hazardous foot placement, significantly enhancing safety and learning efficiency. LiDAR-based rolling mapping with edge-guided reconstruction substantially reduces blind zone impact, yielding consistent terrain geometry. Zero-shot sim-to-real experiments on Unitree G1 validate reliable omnidirectional mobility on indoor stairs and complex outdoor terrains, with a 407.9-meter continuous outdoor walk demonstrating long-term stability. This work provides a compelling solution for safe autonomous humanoid locomotion in real-world environments.