SMUB is my 4×4 skid-steer rover: a Livox Mid-360 and a forward Mid-70, a Jetson Xavier AGX, ROS 2 Jazzy. I want it to drive to a goal a few hundred metres away over rough ground without rolling over, and to handle structures with more than one level: under a bridge, past an overhang, up the ramps of an underground car park. Since late September I’ve built and benchmarked a navigation stack for that in simulation. The latest version reaches 69 of 80 held-out off-road goals and all 12 goals 120–200 m away, with no falls in 112 benchmark runs, and it found its way round a hill in a world built from a real place.
It has not driven the real robot yet. Every number on this page comes from Gazebo with perfect odometry. Most of the page is about the bugs I found on the way, because each one was found in a recorded run, not by tuning.
Four things I wasn't willing to give up:
And one rule on top: it should get stuck before it falls over.
I started from the CMU autonomy stack: a local planner that picks among precomputed paths, FAR (a route planner on a visibility graph) and TARE (an exploration planner). The local planner and path follower stayed. The rest changed:
The CMU autonomy stack (local planner, path follower, FAR, TARE), elevation_mapping_cupy as an alternative terrain backend, and Gazebo Harmonic. Written for this project: the lgrid terrain map, the traversability rules, the grid planner and its unbounded version, the goal relay that switches planners, the stall and turn guards, dead-end memory, the benchmark harness and the world generator.
I built a harness before changing anything, because without one every fix is a guess.
All simulation, ground-truth odometry. "Held-out" is 40 episodes × 2 repeats unless noted. A run that falls is not a success; falls are also counted separately.
| Stack | Off-road held-out | Long range (12) | Garage / bridges (10 × 2) |
|---|---|---|---|
| Starting point: CMU planner + FAR on an elevation map, before fixes | 23/40, 2 falls (1 repeat) | not run | not run |
| lgrid + grid planner | 74/80, 0 falls | 10/12 | can't: single-layer |
| lgrid + FAR + TARE | 64/80, 0 falls | 12/12 | 16/20 |
| Hybrid, Sep 29 | 71/80, 0 falls | 11/12, 1 fall | 14/20 |
| Hybrid with slope-aware planning, Oct 8 | 69/80, 0 falls | 12/12, 0 falls | 16/20 |
Off-road, the grid planner beats FAR: of the episodes where they disagreed, the grid planner won 12 and FAR won 2 (p = 0.013). The hybrid is the only stack that covers all three columns. With the slope-aware planner (Oct 8), it matches the grid stack off-road within noise, reaches every long-range goal with no falls, and does better in the garage. Across the 112 regression runs, nothing fell.
I wrote the world generator and the harness first. The starting point, the CMU local planner and FAR on elevation_mapping_cupy terrain, reached 23 of 40 held-out goals and fell twice. The failures had names: spinning in place, driving off a ridge, and FAR silently dropping goals it had no path to.
FAR plans on a visibility graph and kept routing across trench floors it had never seen. A Dijkstra planner on the traversability map, with a stall guard, took the elevation-mapping stack from 23 to 37 of 40 overnight (one repeat).
On one held-out episode the route flipped behind the robot, and it turned in place at a trench edge. A rear wheel went over and it rolled to 37°. I added a guard: before turning in place next to a flagged edge, back off; with no room behind, hold. The first version held forever and stranded the robot, so the hold now ends after 5 s, and the turn is stopped if the robot tilts past 12°. The same episode afterwards: no falls in 16 runs. It's the clip at the top of this page.
My own terrain map trailed elevation_mapping_cupy by about 17 points: 59 of 80 runs against 72 of 79. Recorded runs showed why. Under the robot, where the lidars can't see, the map wrote a flat disc at the robot's height. On a 16° slope that disc ends in a 0.24 m step all round the robot, over the 0.18 m limit. The robot was boxed in by its own footprint. Tilting the disc to the robot's attitude fixed two episodes that had failed every time, 0 of 4 to 4 of 4.
A second recorded fall showed the disc was also overwriting real ground the lidars had seen on the way in. The map under the robot was its own tilt echoed back. Now the disc only fills cells that were never observed. Held-out went from 30 to 37 of 40.
The map under the robot was its own attitude echoed back.
I built a planner that judges the robot's body instead of map cells: at every cell and 16 headings it places the four wheels on the terrain and computes roll, pitch, a hanging wheel and belly clearance. It found both map bugs above. As a driver it was worse: 31 of 40 with 3 falls, against 37 of 40 with none. It stays in the code, switched off.
The grid planner was a 120 m square centred on the start, so any goal past about 60 m was clamped to its edge. A dense grid also gets slow fast: 0.1 s per plan at 120 m, 1.7 s at 400 m on a desktop. I split it into tiles that exist only where something was seen, an 80 m fine window that follows the robot, and a coarse cost-to-go from the goal. A plan now takes about 50 ms for a goal 1 km away.
Most failed long-range runs of the grid stack stopped in open ground about 57 m from the start. 57.6 m is the edge of the planner's first window. My route hysteresis kept the old route as long as it was "about as good", and continuing from its end at the old window edge always looked exactly as good. It got trimmed down to the robot's own position, and the robot stopped. Now an old route is kept only while it still reaches the current window edge. Long range for the grid stack went from 6 to 10 of 12.
FAR and TARE reached 0 of 12 long-range goals. The goal relay counted any height difference over 1 m as a different floor, at any distance, and handed the run to the explorer. On a real hillside a goal 150 m away and 3 m higher would do the same. The tolerance now grows with horizontal distance: 1 m close by, 18 m at 100 m. FAR and TARE went to 11 of 12, then 12 of 12.
Two long runs rolled over at the same kind of place: a small, steep mound ending at a 0.28 m plateau edge. The map had flagged the edge from a distance, then lost the flag as the robot got close. The cause was a timer. The map merges hits that arrive within 0.5 s, keeping the highest point, but the timer reset on every hit, and both lidars hit a visible cell every 50–100 ms. The merge never ended, and stray hits from the top of the step settled in the cell below for good. A 0.24 m step became 0.07 m plus 0.13 m, both under the limit. Now the window runs from its first hit.
A cell seen every 0.1 s never restarted: its top became the running max since it came into view.
On the off-road set the grid planner beat FAR, 74 to 64 of 80, but it can't do floors. The hybrid lets it drive and hands over to FAR only when the goal is on another floor, and only if the map doesn't already show ground at the goal's height. After FAR and TARE handed the robot back and forth 60 times in one run, mode changes are capped at 8 per goal. When the robot makes no progress for 15 s, the next stretch of route is marked as a dead end. Result: 71 of 80 off-road, 11 of 12 long range, 14 of 20 garage.
The remaining falls, about 1% of runs, cluster on one feature: a steep mound meeting a plateau edge in one long-range world. I made three fixes aimed at it, and each fixed the case I diagnosed. It still falls occasionally nearby. At this rate, telling 1% from 2% apart takes several hundred runs per config.
Procedural worlds only test what I thought to put in them, so I built a world from a real place: a hillside park near Yangsan, with buildings, roads and footpaths from OpenStreetMap, real elevation from SRTM (114 m of relief), and 5,300 tree trunks. OpenStreetMap maps no trees there, so the trunks are my assumption. The goal was 320 m away, past a forested hill. The start point turned out to be inside a 31-storey apartment tower in the map, so I moved it 5 m to the open ground outside. Nine runs failed before one worked.
The terrain map already stored real heights, but both planners reduced each cell to “obstacle or not” and planned on a flat plane. A 19° slope taken head-on and the same slope taken sideways looked the same, though only one of them rolls the robot. The route went straight over the hill and boxed itself in on a side slope. Now every planned move gets a pitch along it and a roll across it from the height gradient. Moves past 20° of roll or 25° of pitch are forbidden, steeper moves cost more, and uphill costs extra.
The slope costs only applied where the robot had already seen the ground, so the planner preferred ground it hadn’t seen, which here was the steepest part of the hill. I gave it a prior elevation map, so it knows the slopes before seeing them. In the simulator that prior is the true terrain, so this is the optimistic case.
On top of the hill the robot spun in place for minutes; at one point its heading went through a full 360° in 16 s. Two routes cost almost the same: down the far side, or back the way it came and around. Each small movement flipped the choice. I tried four fixes in turn: a time-based hold, a dead band on the planning window, committing to a target cell, and committing to a direction. Each fixed the case in front of me, and the next run found the next gap.
I stopped watching runs and asked a simpler question offline: does any route to the goal exist under the slope limits? One did, 432 m long, east along the contour and round the shoulder of the hill. The robot had never tried it. The planner’s 80 m window around the robot applied the limits, but its long-range layer only added a general steepness penalty. That layer kept pointing the robot at a descent the limits forbid, and the window only found out on the hilltop. With the same direction-aware limits in both layers, run 10 went round the hill on the roads and reached the goal with no stalls or rollovers.
I re-ran the whole benchmark. My first limits, 15° of roll and 22° of pitch, were stricter than the benchmark’s 25° notion of drivable, and two held-out episodes timed out on long detours. At 20° and 25° one of them recovered. The other had its goal on top of a steep mound, and the robot circled the mound looking for the strictly nearest reachable spot; it now takes the cheapest of the nearly-nearest spots. Final regression: 69 of 80 off-road (71 before, within noise), 16 of 20 in the garage (14 before), 12 of 12 long range (11 and a fall before), and no falls in 112 runs.
As with the A1 work, the code was written in Claude Code sessions I directed. I set the goals and the bar for success, chose what to test, and read the failures.