CMP-IRRT*: A Perception-Assisted Height-Adaptive Planner for Quadruped Robots

1. Introduction

• Binary occupancy maps treat obstacles as free or blocked, which can cause unnecessary detours when a quadruped can safely cross a low obstacle.The planning question is how to incorporate visual perception and robot-specific traversability constraints so low obstacles need not always be bypassed.Source: §1 · pp. 1–2
• The paper presents a practical perception-conditioned planning formulation and implementation, not a new optimality theory for RRT-based planning.Source: §1 · p. 2
• The framework estimates obstacle geometry from calibrated top-view RGB and uses height-conditioned collision checks to choose whether to bypass or traverse obstacles.Obstacle handling depends on perceived height and the quadruped’s crossing capability.Source: §1 · p. 2
• CMP-IRRT* uses a Channel Mamba PointNet guide to predict promising search regions and improve finite-budget sampling efficiency.The guide is integrated with Informed RRT*.Source: §1 · p. 2
• Up to 16.3% path-length reduction is reported when low obstacles are traversable, alongside fewer explored nodes and iterations for CMP-IRRT* on 2D benchmarks.The evaluation also includes constructed traversability-aware scenarios and a Unitree Go2 demonstration.Source: §1 · p. 2

2.1. Sampling-Based and Learning-Guided Planning

• Informed RRT* restricts sampling after an initial solution, while Neural A* and NIRRT* use learned guidance; these methods generally retain binary obstacle treatment.The paper positions traversability-aware collision handling as complementary to learned sampling guidance.Source: §2.1 · p. 2

2.2. Traversability-Aware Planning for Legged Robots

• Legged-robot traversability depends on morphology and control constraints, and this work compares perceived obstacle height with a robot-specific crossing threshold.The method targets approximately planar-topped obstacles in static scenes and does not directly compare against elevation-map planners.Source: §2.2 · p. 3

2.3. Vision-Based Obstacle Perception

• The pipeline uses existing obstacle-detection and monocular-depth modules to estimate obstacle position and height, rather than proposing a new depth model.It converts calibrated top-view RGB into an image-aligned ground-relative height map without an online terrain-map fusion pipeline; the setting remains sensitive to depth error, calibration, occlusion, and non-planar objects.Source: §2.3 · p. 3, §2.2 · p. 3

3.1. Problem Setup and Assumptions

• The setup assumes a static or slowly changing mapped environment, known image-to-map scale, and obstacle estimates that are used by the planner rather than jointly optimized.The current implementation plans before execution and does not perform closed-loop online replanning during robot motion.Source: §3.1 · pp. 3–4

3.2. Perception-Assisted Planning Pipeline

• The pipeline estimates obstacle shape, location, and height from a calibrated top-view RGB image, plans with the robot’s height threshold, and decodes the path into velocity commands.Its planning output includes bypass-or-traverse mode decisions, not only a geometric path.Source: §3.2 · p. 4

3.3. Traversability-Aware Obstacle Handling

• Estimated depth is converted using calibrated top-view geometry into a ground-relative height map, which supplies approximate obstacle-height estimates.The planner uses these estimates to select a traversal mode, not to guarantee precise 3D reconstruction.Source: §3.3 · p. 5
• Obstacle height is sampled at each detected bounding box’s center, a choice suited to the controlled planar-topped obstacles but not robust to irregular, non-planar, or occluded objects.Region-based estimates and uncertainty margins are described as possible extensions but are not used in the reported experiments.Source: §3.3 · p. 5
• Obstacles above the robot-specific Hα threshold are blocked, while lower obstacles may be crossed by selecting a traversal mode.The threshold depends on foot clearance and feasible thigh-joint angle; the height-conditioned decision is applied during collision checking.Source: §3.3 · pp. 4–5

3.4. Channel Mamba PointNet-Guided Informed RRT*

• The CMP guide predicts a compact search region, while the sampling-based planner still performs collision checking, tree expansion, and rewiring.With probability pg, sampling uses the CMP-predicted guide set; otherwise it falls back to free-space or informed sampling.Source: §3.4 · pp. 5–7

3.5. Path and Mode Decoder

• After planning, the path is decoded into velocity commands and traversal modes, including whether to bypass or traverse an obstacle.The decoder uses path coordinates, command interval, robot heading, and mode.Source: §3.2 · p. 4, §3.5 · p. 7

4. Experiments

• The experiments assess finite-budget search efficiency and practical feasibility rather than establishing a new theoretical optimality result.They comprise standard 2D planning tests, constructed traversability-aware environments, and a Unitree Go2 demonstration.Source: §4 · p. 8, §4.1.2 · p. 9

4.1.1. Experimental Settings

• The planner evaluation uses Center Block and Random World tasks; the latter varies obstacle layouts, while Center Block tests sampling focus in large free space.The guide was trained using 1,000 random worlds and tested on 150; A* paths in pixel space supplied offline guide-region supervision.Source: §4.1.1 · p. 8

4.1.2. Metrics and Results

• On Random World, CMP-IRRT* had lower observed mean initial-solution planning time (1.37 s vs. 1.81 s) and lower mean final cost than NIRRT*.Traditional RRT* and IRRT* found initial paths quickly without neural inference; final cost was measured after 18 seconds of post-initial optimization.Source: Table 1 · p. 9, §4.1.2 · p. 9

4.1.3. Ablation on Guide Architecture

• The architecture ablation reported normalized node and iteration ratios of 0.919 and 0.920 for CMP, about 8.1% and 8.0% fewer than the PointNet++ guide.Adding Mamba alone yielded ratios of 0.984 and 0.980; lower ratios indicate better initial-solution search efficiency.Source: Table 2 · p. 10, §4.1.3 · p. 10

4.1.4. Paired Repeated Evaluation

• A paired repeated evaluation found lower observed mean first-solution iterations and terminal path cost for CMP-IRRT* under equal-expansion settings, while Table 1 showed equal observed success rates.The repeated results characterize variation across maps and planner seeds for one training pair, not across training seeds.Source: Table 3 · p. 10, §4.1.4 · pp. 10–11

4.2. Traversability-Aware Planning

• In constructed maps, height-conditioned traversal reduced mean path length by 7.7% to 16.3% with at most two traversable obstacles per map.The comparison used the same CMP-guided planner with and without height-conditioned obstacle handling, 1,500 iterations, and averages over 10 runs; Figure 4 shows the maps and path shortening.Source: §4.2 · p. 11, Figure 4 · p. 12

4.3. Controlled Real-Robot Evaluation

• In 20 Full predicted-height CMP tasks, 18 were completed; on ten matched trials, Oracle-height and Full predicted-height CMP shortened executed paths by 15.3% and 9.9% versus Binary-CMP.In separate threshold tests, traversal succeeded in 10/10 trials at 0.7Hα and 9/10 at 0.9Hα; height decisions were correct in 18/20 traversable-height and 20/20 blocked-height cases, with no false-traversable decisions. Figure 5 shows Go2 bypassing obstacles and traversing low ones.Source: §4.3 · p. 11, Figure 5 · p. 12, Table 4 · p. 13

• The pipeline assumes calibrated top-view observations and static or slowly changing scenes, so it does not directly address onboard first-person perception or dynamic-scene replanning.Source: §5 · p. 13
• Noisy monocular RGB height estimates can produce incorrect traversal decisions.Source: §5 · p. 13
• The CMP guide is evaluated as a finite-budget sampling aid, and the results do not establish statistical significance across a large benchmark suite.Source: §5 · p. 13
• The method uses a modular perception-planning-control pipeline rather than an end-to-end learned controller.Source: §5 · p. 13

6. Conclusion

• Experiments show improved finite-budget search behavior in evaluated 2D planning settings and path shortening when low obstacles are treated as traversable.The Unitree Go2 demonstration also illustrates conversion of planned paths and traversal modes into executable robot commands.Source: §6 · p. 13
• CMP-IRRT* combines calibrated top-view perception, ground-relative height estimation, height-conditioned collision checking, and CMP-guided informed sampling.The planner can choose to traverse low obstacles or bypass high ones while biasing search toward promising regions.Source: §6 · p. 13
• A Unitree Go2 demonstration illustrates that planned paths and traversal modes can be converted into executable robot commands.Source: §6 · p. 13
• The results suggest coupling learned sampling guidance with height-adaptive obstacle handling as a practical direction for reducing unnecessary quadruped-planning detours.Source: §6 · p. 13