RoboTrail
built·Pose-graph SLAM · ICP · Raspberry Pi 5
A 3D-printed robot on a Raspberry Pi 5, given no map and no lidar. It picks an unexplored edge of what it knows, drives there, sweeps a single laser through 180 degrees, and corrects every pose it has ever held against what it sees. Pose-graph SLAM, frontier exploration and ICP, written from scratch in numpy and scipy.
A rover that maps a room by itself
There is no lidar
The scanner is one VL53L1X time-of-flight sensor on a hobby servo, taking 80 readings across a 180 degree arc every time the rover stops. A real lidar returns thousands of points, continuously, in every direction. Eighty points across half the compass, from a sensor that gives up past 250cm, is a sparse and half-blind view of a room — and making the rest of the pipeline work on that is most of what the project is.
A pose graph, not a filter
There is no EKF or particle filter in the running system. Every scan becomes a node, stored in sensor frame so it can be replayed later at a corrected pose, and edges come from odometry, from sequential ICP and from loop closures — each weighted by an information matrix derived from its match ratio and residual. scipy's least_squares solves every pose jointly against a sparse Jacobian, and then the map is thrown away and rebuilt from the corrected trajectory. That runs every five nodes, or immediately when a loop closes.
Scan matching that knows about walls
ICP here is hybrid point-to-line. Surface normals are estimated from each map point's six nearest neighbours, and where they are reliable the solver constrains a scan point to lie somewhere on the wall itself rather than on top of one specific dot of the map. With only 80 points in a scan that distinction is the difference between converging and not. Where the normals are unreliable it falls back to point-to-point SVD, and a match ratio under 25% aborts the alignment.
The guards are the work
An ICP result that disagrees with odometry by more than 10cm or 20 degrees is rejected outright, because alongside a long blank wall ICP hits the aperture problem: a confident match, the rotation right, the translation off by half the wall. A loop closure additionally needs a five-node gap, a candidate within 70cm, a heading gap under 90 degrees and a match ratio of 0.6 — two scans facing away from each other share no visible geometry, so a confident match between them is a lie. Turns over 60 degrees trigger a mid-turn scan, because otherwise a 90 degree turn leaves consecutive scans with nothing to overlap.
It decides where to go next
Frontier cells — traversable cells that touch unknown space — are clustered by BFS flood fill, and clusters under 30 cells are dismissed as noise. Obstacles are inflated by the rover's own radius of 7.75cm plus 5cm of padding, then A* on the 8-connected grid plans a route which line-of-sight checks then simplify. It drives exactly one waypoint per iteration and re-detects frontiers after every scan, so it never keeps chasing a frontier the last scan already resolved. When nothing reachable is left unknown, the run is over.
- chassis
- 3d-printed, tank tracks
- compute
- raspberry pi 5
- scanner
- one tof laser on a servo
- slam
- pose graph + icp
- written in
- numpy + scipy, no ros
How it was worked out
The repository carries its own derivations and a stage-by-stage build log, which is where the reasoning actually lives.
- LEARNING_PATH.mdThe stage-by-stage build log, dead reckoning through visual SLAM.
- ICM_SCAN_MATCHING.mdICP derived from first principles.
- GRAPH_SLAM_GUIDE.mdPose graphs, information matrices and loop closure.
- CHASSIS.mdChassis design, printing and wiring.