Warehouse Robot Path Planning (3D)

What the 3D scene shows

Racks are the walls of the grid, dark oil slicks are mud (cost 3). Coloured tiles are expanded cells (hue = cost so far, optionally raised by that cost), orange gems are the frontier, the green tube is the best path and the robot drives it with its wheels and lidar turning. Everything is driven by the same planner code as the 2D page.

The search loop

All grid planners keep a priority queue (the frontier) and repeatedly expand the cheapest cell. They differ only in the key: BFS uses discovery order, Dijkstra uses the cost so far g(n), A* uses f(n) = g(n) + h(n), weighted A* uses f = g + w h, and greedy best-first uses h(n) alone.

Optimality

If h(n) never over-estimates the true remaining cost (admissible) and satisfies h(n) ≤ c(n,m) + h(m) (consistent), A* expands each cell at most once and its path is optimal. Weighted A* with w ≥ 1 returns a path of cost at most w times the optimum while expanding far fewer cells. Greedy search has no bound and can be trapped in a cup.

Heuristics

For 8-connected moves of cost 1 and 1.414 the exact open-field distance is the octile distance dx + dy + (√2 − 2) min(dx, dy). Manhattan distance dx + dy is admissible only for 4-connected moves; on an 8-connected grid it over-estimates and A* may return a longer path. Terrain costs are at least 1, so these heuristics stay admissible on mud.

Sampling planners

RRT grows a tree towards random samples in steps of length η; it finds a path quickly but not a short one. RRT* also picks the cheapest parent among nodes within radius r and rewires neighbours through the new node, so the path cost converges towards the shortest any-angle path, which is shorter than the best grid path because it is not limited to 45 degree moves.

Honest simplifications

Costs are in cell lengths; mud multiplies the cost by 3. RRT and RRT* ignore mud (length only) and check collisions by sampling every 0.2 cells along each edge. The rewiring radius is set by hand instead of the theoretical (log n / n)^(1/d) schedule. Compute times are real JavaScript timings on your device.

Model checks: A* with a consistent heuristic equals Dijkstra on 160 random maps and both move sets; BFS length equals Dijkstra on unit-cost 4-connected maps; the chosen heuristic is tested against exact distances-to-goal on the current map; weighted A* respects its factor-w bound.