Capstone: a SLAM system for a warehouse robot

This capstone brings the course together in one design: a mapping and localization system for a fleet of autonomous mobile robots in a distribution warehouse. The scenario is fictional but typical. Northfield Logistics is commissioning 40 robots that tow carts between receiving, storage aisles and outbound docks in a 220 m by 140 m single-story building. The robots must localize to within 5 cm and 1° near pick-up and drop-off stations, run 20 hours a day, coexist with people and forklifts, and keep working as racking and stock change. The task is to choose sensors, a map representation, a SLAM and localization approach, a mapping procedure and a loop-closure strategy; to estimate whether the design meets its requirements; to plan its evaluation; and to analyze how it can fail. Planning and control (ROB-350) are out of scope except where they depend on localization.

Requirements

Good designs start from explicit, testable requirements. The table below lists those agreed with the operations team. Each one is measurable, so the evaluation plan can show whether it is met.

Sensors

Each robot already carries two safety-rated 2-D lidars at opposite corners, giving a 360° field of view at 25 cm height, as required for collision safety. Using the same scanners for localization avoids extra cost, though their 25 cm mounting height sees pallet loads and cart wheels as well as rack uprights and walls. The design adds a gyroscope-equipped IMU to the wheel odometry, and a forward-facing RGB-D camera that serves obstacle detection above scanner height and could later support visual features. A 3-D lidar was considered: it would see the ceiling and upper racking, which change less than floor-level stock, but at roughly ten times the cost per robot it was deferred as an option for problem areas.

Map representation and approach

The 2-D lidars and the flat floor point to a 2-D occupancy grid at 5 cm for navigation, produced from a pose graph by graph-based SLAM (lesson 8). The pose graph itself is the master map, because it can be re-optimized, edited and extended, while the grid is a derived product. Commissioning uses slam_toolbox in mapping mode (lesson 9) with correlative scan matching and loop closure, and the robots then run slam_toolbox in localization mode against the serialized graph, with AMCL kept as a fallback, which gives precise scan-to-map matching with global relocalization. At the 1,750 stations, the robots switch to a dedicated docking mode that matches a local scan of the station's reflective markers, giving millimeter-level repeatability where it matters most.

Aisles are the hard part. Long rows of identical racking are both degenerate (little constraint along the aisle) and perceptually aliased (aisle 12 looks like aisle 13). The design addresses this three ways: wheel odometry and the gyroscope carry the estimate along aisles, where lidar constraint is weak; retroreflective tags at each aisle end, visible as high-intensity returns, break the aliasing; and the localizer reports degeneracy from the scan-matching Hessian (lesson 9) so the fusion filter down-weights lidar along the aisle.

Mapping procedure and loop-closure strategy

The mapping run is planned rather than improvised. The site is mapped at night when it is quiet, with a robot driven at 0.5 m/s. The route first drives the full perimeter (about 720 m), returning to the start to close one large loop. It then covers the aisles in pairs, down one aisle and back up the next, crossing the main cross-aisle each time, so every pair creates a loop closure within a few hundred meters. Loop closures are accepted by slam_toolbox only above a conservative match score, and every accepted closure is reviewed afterwards in RViz, where the graph can be edited. After optimization, the grid is regenerated, people and parked equipment captured during mapping are erased, and keep-out zones and stations are annotated.

import numpy as np
rng = np.random.default_rng(12)
def worst_error(route_len, spacing, gyro=0.0002, noise=0.01, trials=200):
    worst = []
    for _ in range(trials):
        s = np.arange(0, route_len, 1.0)
        heading = np.cumsum(rng.normal(0, gyro, len(s)))      # heading random walk (rad)
        err = np.cumsum(np.sin(heading)) + rng.normal(0, noise, len(s))   # lateral error (m)
        for start in np.arange(0, route_len, spacing):
            m = (s >= start) & (s < start + spacing)
            if m.sum() > 1:
                seg = err[m]; err[m] = seg - np.linspace(seg[0], seg[-1], m.sum())
        worst.append(np.percentile(np.abs(err), 95))
    return np.mean(worst)
for spacing in (50, 120, 300, 600):
    print("loop closure every %4d m -> 95th-percentile map error %.3f m" % (spacing, worst_error(1200, spacing)))

loop closure every   50 m -> 95th-percentile map error 0.029 m
loop closure every  120 m -> 95th-percentile map error 0.062 m
loop closure every  300 m -> 95th-percentile map error 0.212 m
loop closure every  600 m -> 95th-percentile map error 0.522 m

With these illustrative parameters, closing loops about every 120 m keeps the 95th-percentile map error near 6 cm, inside the 10 cm target, while closures 300 m apart let it grow to about 20 cm. Because heading drift makes position error grow faster than linearly with distance, doubling the spacing more than doubles the error.

Error budget

The localization error at a station combines several independent sources, which add as a root sum of squares. The budget shows whether R1 is achievable and which term to improve first.

Evaluation plan

Verification follows the requirements table. Ground truth comes from a total station (a surveying instrument accurate to a few millimeters) that tracks a prism on the robot, plus surveyed reference points at a sample of stations. Test runs cover each zone (docks, aisles, cross-aisles) at normal speed and load, at different times of day to include people and forklift traffic. The team computes ATE after rigid alignment for map consistency, RPE over 10 m segments for drift, and station accuracy directly against surveyed poses (lesson 11). Kidnapping tests power-cycle robots at 50 random poses and measure time to relocalize (R5). A two-week shadow period runs the fleet with operators watching, logging localization status, recovery events and scan-to-map agreement per aisle (R4, R6). Acceptance requires every requirement to be met with margin on the measured data, not only in the budget.

Failure modes and mitigations

A failure mode and effects analysis lists how localization can fail, how each failure would be detected, and what the system does about it. The most important point is that every failure must be detectable: a robot that knows it is lost can stop safely and ask for help, while a robot that is confidently wrong is dangerous.

What this capstone shows

The design uses nearly every idea in the course. Calibrated odometry (lesson 1) carries the robot through degenerate aisles. The map is a pose graph that generates an occupancy grid (lessons 2, 4 and 8). Localization combines scan matching with particle-filter fallback and recovery (lesson 3). The mapping run is planned around the realities of SLAM, data association and loop closure (lesson 5), with lidar-specific front-end choices and degeneracy detection (lesson 9). Evaluation uses ATE, RPE and station surveys, and map maintenance relies on lifelong mapping (lesson 11). Filter-based SLAM (lessons 6 and 7) and visual SLAM (lesson 10) were considered and rejected for this site, with reasons recorded, which is itself part of good engineering. A design review would also ask what happens on a new site with different geometry, which is why the requirements, budgets and tests, not just the software configuration, are the real deliverables.