Skip to content
←Back to Papers

PAPER DEEP DIVE

HumanoidStair ClimbingPerception and motion

PolygMap: A Perceptive Locomotion Framework for Humanoid Robot Stair Climbing

Recently, biped robot walking technology has been significantly developed, mainly in the context of a bland walking scheme. To emulate human walking, robots need to step on the positions they see in unknown spaces accurately. In this paper, we present PolyMap, a perception-based locomotion planning framework for humanoid robots to climb stairs. Our core idea is to build a real-time polygonal staircase plane semantic map, followed by a footstep planar using these polygonal plane segments. These plane segmentation and visual odometry are done by multi-sensor fusion(LiDAR, RGB-D camera and IMUs). The proposed framework is deployed on a NVIDIA Orin, which performs 20-30 Hz whole-body motion planning output. Both indoor and outdoor real-scene experiments indicate that our method is efficient and robust for humanoid robot stair climbing.

1. Background and Motivation

Humanoid robots are emerging from laboratories into diverse real-world scenarios including building inspections, emergency responses, and industrial collaborations. Compared with level-ground locomotion, stairs as a standard yet high-risk man-made structure present step height differences, narrow contact surfaces, and fall-prevention constraints, imposing higher requirements on the robot's full-chain capability to traverse complex geometries. Many existing systems rely on powerful self-balancing and high-bandwidth tracking stabilizers, using rapid whole-body posture regulation to "brute-force" geometric disturbances during stair climbing. However, these approaches lack explicit perception of environmental geometry and foothold regions, often resulting in overly conservative gaits and considerable footstep uncertainty.

Perception-based approaches face challenges in stair environments: stair treads are small with sharp edges, depth measurements are affected by low texture and absorptive materials; body vibration and camera rolling-shutter effects amplify noise; odometry-mapping coupling induces local drift accumulating in multi-frame fusion; and the perception-planning-control loop must operate in real time. PolygMap addresses these challenges through a multi-sensor fusion perception-locomotion planning framework building real-time polygonal staircase plane semantic maps.

Figure 1: Perceptive locomotion for humanoid stair climbing. Visual perception acquires prior information about traversable planes, step edges, and obstacles before foot contact.

2. System Architecture

The system fuses LiDAR and RGB-D point cloud data for staircase mapping and humanoid footstep planning. Sensors include a downward-facing Realsense L515, Livox Mid360 LiDAR, and IMU with Point-LIO for odometry. Deployed on NVIDIA Orin, achieving 20-30Hz whole-body motion planning output.

Figure 2: System integrating joint recorders, depth sensing, and LIO estimator. Robot pose obtained via forward kinematics and LIO fusion; depth images provide polygonal map for foothold generation.

3. Multi-Sensor Fusion State Estimation

A linear Kalman filter fuses multi-modal data from IMU, joint encoders, and foot contact states for unified estimation of robot base position, velocity, orientation, and foot positions. The state vector is defined as:

$$\mathbf{x} = \left[{}^{W}\mathbf{p}_{\text{base}}^{\top} \quad {}^{W}\mathbf{v}_{\text{base}}^{\top} \quad {}^{W}\mathbf{p}_{c_1}^{\top} \quad \cdots \quad {}^{W}\mathbf{p}_{c_8}^{\top}\right]^{\top} \in \mathbb{R}^{30}$$

The state-space equation is $\mathbf{x}_{k+1} = \mathbf{A}\mathbf{x}_k + \mathbf{B}\mathbf{u}_k$ with control input $\mathbf{u} = {}^{W}\mathbf{a}_{\text{base}}$. LIO odometry is obtained through Point-LIO. Since the LiDAR is rigidly connected to the robot, the extrinsic ${}^{B}\mathbf{T}_{\text{lidar}}$ is known, and LIO is transformed to the CoM:

$$\mathbf{T}_{\text{base}}^{(L)} = \mathbf{T}_{\text{lidar}} \left({}^{B}\mathbf{T}_{\text{lidar}}\right)^{-1}$$

Position fusion uses complementary filtering:

$$\mathbf{p}_{\text{base}}^{\text{(fused)}} = \alpha \cdot \mathbf{p}_{\text{base}} + (1-\alpha) \cdot \mathbf{p}_{\text{base}}^{(L)}$$

where $\alpha = \tau / (\tau + \Delta t)$. Attitude fusion uses the exponential map form:

$$\Delta R = \log\left(\left({}^{W}R_{\text{base},k}\right)^{\top} {}^{W}R_{\text{base},k}^{(L)}\right)$$

The final fused attitude is ${}^{W}R_{\text{base}}^{\text{fused}} = {}^{W}R_{\text{base},k} \exp((1-\alpha)\Delta R)$.

4. Polygonal Map Construction

Per-pixel normals are computed from depth images and camera intrinsics. The 3D position of each pixel is:

$$P(u,v) = \left[\frac{(u-c_x)D(u,v)}{f_x}, \quad \frac{(v-c_y)D(u,v)}{f_y}, \quad D(u,v)\right]^T$$

Differential vectors $\vec{a}$ and $\vec{b}$ are constructed in the pixel neighborhood, and the local normal is obtained via cross product normalization:

$$\vec{n}(u,v) = \frac{\vec{a} \times \vec{b}}{\|\vec{a} \times \vec{b}\|}$$

Anisotropic diffusion filtering reduces depth noise while preserving edges:

$$I^{t+1}(x,y) = I^t(x,y) + \lambda \sum_{i \in \mathcal{N}(x,y)} c_i(x,y) \nabla I_i(x,y)$$

where $c_i(x,y)$ is adaptively adjusted based on gradient magnitude. RANSAC is then applied for plane fitting within extracted polygonal regions.

Figure 3: PolygMap footstep motion planning logic, from depth image processing to polygonal map construction to foothold generation.

5. Optimal Foothold Generation

Candidate footholds are extracted from the polygon map. 2D convex hulls are computed for each polygon's vertices:

$$H_i = \text{ConvexHull}(P_i), \quad H_i \subset \mathbb{R}^2$$

Uniform grid points are generated within bounding boxes, keeping only points inside convex hulls, with height assigned as the average of polygon vertices:

$$z = \frac{1}{N_i} \sum_{j=1}^{N_i} z_{i,j}$$

Points are filtered by range ($|x(p)| \leq g_{\text{range}}$, $|y(p)| \leq g_{\text{range}}$), and the maximum height point of each grid is extracted. Points above the foot threshold are removed:

$$P_{\text{max\_filtered}} = \{p \in P_{\text{max}} \mid z(p) \leq z_{\text{foot}} + g_z\}$$

Points are divided into height layers with erosion operations per layer to filter sharp edge points. The primary foothold candidate is selected as the point closest to the robot base satisfying height conditions:

$$p^* = \arg\min \|(x(p) - x_{\text{base}}, y(p) - y_{\text{base}})\|^2$$

The relative direction of the candidate point is:

$$\theta_{\text{rel}} = \arctan2(y(p^*), x(p^*)) - \psi_{\text{base}}$$

6. Footstep Planning

Gait trajectory generation is based on real-time robot pose estimation. The foot position is generated from torso pose and left/right foot offset via rotation matrix:

$$\mathbf{p}_f = \mathbf{p}_t + \mathbf{R}_z(\phi_t) \cdot \mathbf{b}_f$$

The Z-direction foot trajectory uses sinusoidal interpolation during lift-off and landing:

$$z_f(t) = \begin{cases} z_{\max} \sin\left(\frac{\pi t}{2t_{\text{lift}}}\right) & 0 \leq t \leq t_{\text{lift}} \\ z_{\max} \cos\left(\frac{\pi(t - t_{\text{lift}})}{2t_{\text{land}}}\right) + z_0 & t_{\text{lift}} < t \leq T \end{cases}$$

A rotating rectangle model represents foot occupied space to avoid foot overlap, checking for intersections between consecutive steps.

7. Experimental Setup

Experiments use a KUAVO humanoid robot (166cm, 55kg, 28 DOF, foot size 26x9.6cm), integrating 3D LiDAR, RGB-D camera, and IMU on NVIDIA Orin NX for 20-30Hz whole-body motion planning. Three experiment groups: sensor fusion validation, Gazebo simulation, and indoor/outdoor real-world experiments. Metrics include stair detection continuity, consecutive climbing steps, and planned-versus-executed foothold errors.

8. Sensor Fusion Experiment

Figure 4: Position estimation comparison: proprioceptive, LIO, and fusion method. The fused trajectory effectively suppresses LIO high-frequency noise and significantly reduces cumulative drift.

Proprioceptive estimation (odom_base) shows significant cumulative drift on x and z axes. LIO (odom_lidar) provides higher absolute accuracy but with pronounced high-frequency oscillations on the y-axis. The fused result (odom_fused) combines both: retaining LIO global accuracy while using proprioceptive observations to constrain and smooth the estimate.

9. Simulation Results

TrialT(s)StepsFPS(Hz)$e_m$(mm)
134102111.2
235102010.3
334102512.0
41442912.4
51542811.8

Maximum climbing time 35s, average 3.4-3.7s per step. Maximum footstep error 12.4mm, far smaller than available tread width, satisfying safety margin requirements.

10. Real-World Experiment Results

SceneGaitT(s)Stepsf(Hz)$e_m$(mm)
IndoorDS9.642112.1
IndoorDS10.242011.4
IndoorSS7.742324.7
OutdoorSS11.262033.4
OutdoorSS9.852122.2

DS (Double Step) gait waits for stable stance before each step, yielding smaller errors (11.4-12.1mm). SS (Single Step) gait is faster but cannot always obtain stable base observations during online planning, resulting in larger errors (max 33.4mm, approaching safety margin). Outdoor plane detection frequency maintains 20-23Hz despite ground unevenness and illumination variations.

Figure 6: Real-world stair climbing experiment. Top: experimental scene snapshots. Bottom: RViz visualization with green rectangles for detected stair planes and blue rectangles for feasible foothold regions.

Figure 7: Experimental results. Left: 3D trajectories of robot base and feet. Right: foot trajectory tracking performance along x and z axes with error curves.

11. Method Pipeline

flowchart TD A["RGB-D Depth Image"] --> B["Anisotropic Diffusion Filter"] B --> C["Normal Computation
Sobel + Cross Product"] C --> D["RANSAC Plane Fitting"] D --> E["Polygonal Semantic Map"] F["LiDAR + IMU"] --> G["Point-LIO Odometry"] H["Joint Encoders"] --> I["Forward Kinematics"] G --> J["Kalman Filter Fusion"] I --> J J --> K["Fused Pose Estimation"] K --> E E --> L["Convex Hull + Grid + Erosion
Foothold Candidate Generation"] L --> M["Optimal Foothold Selection"] M --> N["Gait Trajectory Planning
Sinusoidal Interpolation + Collision Check"] N --> O["Whole-Body Motion Execution"]

12. Limitations and Future Directions

The SS (Single Step) gait still has a failure rate during outdoor long staircase climbing, mainly due to inaccurate state estimation, outdoor lighting conditions affecting depth perception, and actuator issues under single-step mode. The maximum footstep error of 33.4mm approaches the safety margin between foot size and stair tread, indicating that SS gait reliability on protruding-edge stairs needs improvement. Future directions include optimizing outdoor state estimation robustness, enhancing depth perception on challenging materials, and improving SS gait online planning stability.

13. Conclusion

PolygMap presents a vision-guided stair climbing framework for full-size humanoid robots. The perception module fuses LiDAR odometry with proprioceptive observations and point clouds, extracting planes and outputting stable, low-drift state estimation. The planning module selects foothold regions and generates foot trajectories satisfying stability and safety constraints. Simulation and real-world experiments demonstrate strong robustness with 20-30Hz real-time performance, enabling stable continuous stair climbing with high consistency between planned and executed footholds. The framework successfully validated DS gait strategy for 4-step climbing on standard-dimension stairs (13cm height, 28cm tread width, challenging for a robot with 26cm long feet) and demonstrated good robustness in outdoor 5-6 step climbing.

Related Papers

HarnessVLN: Unifying Training-Free Embodied Navigation through an Agent Harness

HarnessVLN: Unifying Training-Free Embodied Navigation through an Agent Harness

Embodied navigation requires an agent to ground instructions or object goals in spatial observations and turn plans into execution. HarnessVLN is a zero-shot, training-free agent harness that coordinates perception, hierarchical event memory, a persistent spatiotemporal graph, and navigation tools through a shared interface. Before dispatch, the harness checks evidential support, geometric feasibility, and subgoal consistency; structured execution feedback then updates task progress, spatial evidence, and failure records for subsequent planning, recovery, and stopping. HarnessVLN reaches success rates of 60.8% on R2R, 53.9% on RxR, 76.0% on HM3D-v2, and 59.3% on HM3D-OVON, outperforming the training-free methods compared in the paper. Three real-world scenarios on an AgiBot A3U humanoid demonstrate deployment feasibility across both navigation families. Project page: https://agibot-harnessvln.netlify.app/.

Yang Chen, Lirong Che, Zhenyu Huang · 98 views · Sep 14, 2026
Vision-language navigationEmbodied AI
Sep 14, 2026
Chunk-wise Model-based Policy Optimization

Chunk-wise Model-based Policy Optimization

Model-based reinforcement learning has shown superior sample efficiency by leveraging the learned latent dynamics either in the form of derivative-free planning/imagined rollout training or dynamics gradient-based policy updates. While these approaches achieve comparatively fast learning, they all suffer, to varying degrees, from issues of recurrent compounding errors, high inference-time latency, and instability in backward propagation through time (BPTT). In this work, we propose a model-based RL approach that uses the chunk world model as a differentiable training-time evaluator to optimize the chunk actor without recurrent model invocation. Through the collaborative chunk-wise prediction of the world model and policy, we can generate H-step hypothetical rollouts in a single forward pass and directly optimize value expansion, avoiding the costly and unstable BPTT. We evaluate our method on 19 challenging high-dimensional humanoid tasks from HumanoidBench and deploy our method to the real-world for quadruped bipedal walking. Project Website: https://song-wx.github.io/cmpo.github.io/

Weixi Song, Donglin Wang, Yong-Lu Li · 136 views · Oct 10, 2026
HumanoidModel-Based RL
Oct 10, 2026
Movens-Zero: Scaling Humanoid Motion Foundation Models with Large-Scale Human Videos

Movens-Zero: Scaling Humanoid Motion Foundation Models with Large-Scale Human Videos

Movens-Zero builds five million executable humanoid motions from Internet human videos, unifies text, video, speech, and music conditioning, and drives Unitree G1 in real time through chunked closed-loop control.

Wendong Bu, Tinghong Ye, Qizhou Wang · 150 views · Oct 30, 2026
HumanoidHumanoid Motion Foundation Model
Oct 30, 2026
METATRON: A Promptable Whole-Body Execution Layer for Humanoids

METATRON: A Promptable Whole-Body Execution Layer for Humanoids

Humanoid controllers often require task-specific training despite sharing the need for balance and whole-body coordination. We present Metatron, a promptable whole-body execution layer combining a generative base policy with a programmable skill layer. The base policy receives partial goals, collectively called a prompt, that can combine timed body-part targets, velocity commands, and style cues. It produces diverse whole-body motions consistent with these goals. Training combines motion capture, synthetic recovery trajectories, and retargeted scene-interaction motions. After training, the policy is frozen. Stateful programs called skills express task intent as prompts at each control step, drawing rough geometric sketches that the policy turns into coordinated movement. Skills are ordinary code, refined by coding agents that analyze motion data and fit compact rules without task-specific policy training. On the physical Unitree G1, one frozen checkpoint supports walking, recovery, teleoperated object pickup, and sitting on furniture. Comparative evaluations show higher reaching success and push robustness than the tested reference-tracking pipelines. The same recipe produces climbing and acrobatics on a simulated character. Metatron enables coding agents to turn learned motor capabilities into new skills through code, without further policy training.

Chen Tessler, Yifeng Jiang, Alec Otto · 435 views · Oct 27, 2026
HumanoidWhole-Body Control
Oct 27, 2026

As an Amazon Associate, we earn from qualifying purchases.