PAPER DEEP DIVE
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
| Trial | T(s) | Steps | FPS(Hz) | $e_m$(mm) |
|---|---|---|---|---|
| 1 | 34 | 10 | 21 | 11.2 |
| 2 | 35 | 10 | 20 | 10.3 |
| 3 | 34 | 10 | 25 | 12.0 |
| 4 | 14 | 4 | 29 | 12.4 |
| 5 | 15 | 4 | 28 | 11.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
| Scene | Gait | T(s) | Steps | f(Hz) | $e_m$(mm) |
|---|---|---|---|---|---|
| Indoor | DS | 9.6 | 4 | 21 | 12.1 |
| Indoor | DS | 10.2 | 4 | 20 | 11.4 |
| Indoor | SS | 7.7 | 4 | 23 | 24.7 |
| Outdoor | SS | 11.2 | 6 | 20 | 33.4 |
| Outdoor | SS | 9.8 | 5 | 21 | 22.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
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.
SOURCE LINKS
