STATERA Library · Series 08 · Volume 15 · Chapter 02 · EN
Source: https://github.com/Statera-Library/08_Vol_15_Motion_Planning_and_Navigation/blob/main/Chapter_02_Graph_Based_Planning/Chapter_02_Graph_Based_Planning_EN.md
Volume 15. Motion Planning and Navigation
Chapter 02. Graph Based Planning
02.01. Graph Search Theory BFS DFS Dijkstra A Star [w/Code]
Graph-based planning represents a navigation problem as a collection of discrete states connected by feasible transitions. A graph is commonly written as G = (V, E), where V contains vertices representing positions, configurations, or waypoints and E contains edges describing valid movements between them. In robot navigation, occupancy grids, roadmaps, and topological maps can all be transformed into graphs, allowing path planning to become a systematic search from a start vertex to a goal vertex.
Each edge may be assigned a cost describing distance, traversal time, energy consumption, terrain difficulty, risk, or another navigation objective. A planner then searches for a sequence of connected vertices that reaches the goal while satisfying the desired cost criterion. This abstraction separates the fundamental search problem from the physical representation of the environment and makes the same search principles applicable to indoor AMRs, outdoor mobile robots, manipulators, and other autonomous systems.
Breadth-First Search (BFS) explores a graph layer by layer. Starting from the initial vertex, it first visits all directly connected vertices, then vertices two edges away, and continues outward until the goal is discovered. A queue implements this first-in, first-out exploration order. For an unweighted graph, BFS is complete and returns a path containing the minimum number of edges, making it a useful baseline for understanding systematic graph exploration.
The principal limitation of BFS is that it does not distinguish between promising and unpromising search directions. In a large occupancy grid, the algorithm can expand a broad region around the start even when the goal lies in a clearly identifiable direction. Its memory demand can also become substantial because the frontier may contain many vertices simultaneously. BFS is therefore conceptually important but is rarely the most computationally efficient choice for large-scale robot navigation.
Depth-First Search (DFS) follows one branch of a graph as deeply as possible before backtracking. It is naturally implemented with a stack or recursion and generally requires less frontier memory than BFS. DFS is useful for graph traversal, connectivity analysis, exhaustive exploration, and several supporting algorithms, but it is poorly suited to shortest-path navigation because the first path reaching the goal may be unnecessarily long or costly.
The difference between BFS and DFS illustrates a fundamental planning tradeoff. BFS expands states according to search depth and can guarantee shortest paths in unweighted graphs, whereas DFS prioritizes depth and reduces frontier storage without preserving path optimality. Neither method considers arbitrary edge costs. Real robotic environments frequently contain movements with different distances or traversal penalties, motivating cost-aware search algorithms such as Dijkstra\'s algorithm.
Dijkstra\'s algorithm associates each vertex with the lowest accumulated path cost discovered from the start. The start receives cost zero, while other vertices initially have infinite cost. At each step, the algorithm selects the frontier vertex with the smallest accumulated cost and relaxes its outgoing edges. If reaching a neighbor through the current vertex produces a lower cost, the neighbor\'s cost and predecessor information are updated accordingly.
For graphs with nonnegative edge costs, Dijkstra\'s algorithm provides a complete and optimal shortest-path solution. In a robot costmap, this means that traversal penalties can influence planning rather than treating every movement identically. A planner may therefore prefer a slightly longer geometric route that avoids expensive cells near obstacles or difficult terrain. The resulting path reflects the cost model encoded in the graph instead of merely minimizing the number of transitions.
A priority queue is normally used to efficiently retrieve the vertex with the lowest current path cost. With appropriate data structures, Dijkstra\'s algorithm scales much better than a naive implementation, yet it still explores vertices in every direction according to accumulated cost. Because it has no explicit knowledge of the goal\'s direction, a considerable number of states may be processed even when only a relatively narrow region is relevant to the final route.
A* search introduces goal-directed reasoning by combining the accumulated path cost g(n) with a heuristic estimate h(n) of the remaining cost to the goal. The evaluation function is expressed as f(n) = g(n) + h(n). The planner preferentially expands vertices with low estimated total path cost, allowing it to concentrate computation toward regions that appear likely to produce an efficient route while retaining the cost-sensitive behavior of Dijkstra\'s algorithm.
The heuristic is central to A* performance. On a four-connected grid, Manhattan distance is often appropriate, while Euclidean distance naturally represents straight-line separation. Eight-connected grids can use heuristic formulations that reflect diagonal motion. A heuristic should correspond to the motion model and cost structure of the planning graph; otherwise, the search may perform unnecessary expansions or lose theoretical guarantees expected from the algorithm.
An admissible heuristic never overestimates the true minimum remaining cost. Under appropriate graph-search conditions, this property allows A* to retain optimality while reducing exploration relative to uninformed cost search. A consistent heuristic additionally satisfies a triangle-like relationship between neighboring states, simplifying efficient graph-search implementations. When h(n) is zero everywhere, A* effectively reduces to Dijkstra\'s algorithm and loses its goal-directed advantage.
The relationship among the four algorithms can therefore be understood through the information used to select the next state. DFS primarily uses search depth, BFS uses increasing graph depth, Dijkstra uses accumulated path cost g(n), and A* combines accumulated cost with estimated future cost. This progression demonstrates how increasingly informative search policies can improve navigation efficiency while maintaining different combinations of completeness, optimality, memory usage, and computational cost.
A practical implementation maintains a frontier containing candidate vertices, a record of explored or finalized states, cost information, and predecessor links. When the goal is reached under the algorithm\'s termination conditions, predecessor links are followed backward from the goal to reconstruct the path. Careful handling of repeated states, stale priority-queue entries, unreachable goals, map boundaries, and invalid transitions is essential because implementation details can significantly affect planner correctness.
For grid-based robot navigation, each traversable cell can become a vertex and neighboring cells define edges. Obstacles are excluded or assigned prohibitive costs, while inflation regions and terrain properties can increase traversal costs. The connectivity model determines allowable movements: four-connectivity permits orthogonal motion, whereas eight-connectivity additionally permits diagonals. Edge costs should remain consistent with these motions so that the graph represents physical movement meaningfully.
Graph resolution also affects planning behavior. A fine grid captures narrow passages and obstacle geometry with greater precision but creates more vertices and therefore increases search workload. A coarse representation reduces computation but may remove feasible corridors or produce paths that inadequately represent the robot footprint. Graph construction, cost assignment, and search algorithm selection must consequently be treated as coupled design decisions rather than independent software choices.
The path produced by graph search is usually a geometric or topological reference rather than a directly executable control command. Mobile robots still require collision checking, path smoothing, velocity generation, local obstacle avoidance, and feedback control. This distinction is reflected in a broader navigation architecture in which graph-based methods primarily serve global planning, while local planners and controllers handle short-horizon motion and continuously changing conditions.
Static graph search also has limitations when the environment changes after planning. Newly detected obstacles can invalidate edges or increase their costs, forcing the planner to search again or employ incremental methods designed for dynamic environments. This motivates algorithms such as D* Lite, while techniques such as Jump Point Search and Theta* address other limitations of conventional grid search. The volume structure treats these methods as subsequent developments of the graph-planning foundation.
In production robotics, algorithm selection should therefore depend on graph size, edge-cost structure, available memory, required planning latency, and the quality of heuristic information. BFS and DFS remain valuable for understanding traversal behavior and specialized graph operations, Dijkstra provides a rigorous foundation for weighted shortest paths, and A* forms one of the most important practical bridges between classical graph theory and efficient goal-directed robot navigation.
Graph search ultimately converts navigation from an informal geometric question into a structured computational problem. States define where the robot may be, edges define how it may move, costs encode operational preferences, and the search policy determines how computational effort is allocated. Understanding BFS, DFS, Dijkstra, and A* therefore provides the theoretical foundation for the more specialized graph planners, dynamic replanning methods, and navigation software architectures developed later in motion planning and navigation.
02.02. A Star and Weighted A Star Implementation [w/Code]
A* is a goal-directed graph search algorithm that combines the exact cost already accumulated from the start with an estimate of the cost remaining to the goal. For a state n, the evaluation function is f(n) = g(n) + h(n), where g(n) represents the known path cost and h(n) is the heuristic estimate. This combination allows A* to preserve cost-aware planning while concentrating exploration toward promising regions of the graph.
In robot navigation, the graph may originate from an occupancy grid, costmap, topological network, or discretized configuration space. Each traversable state becomes a node, while feasible movements define edges. Edge costs can represent geometric distance, cell traversal penalties, terrain difficulty, energy demand, or combinations of these quantities. A* therefore operates on an abstract graph while remaining closely connected to physical navigation requirements.
A practical A* implementation maintains an open set containing candidate nodes that may still be expanded and a record of nodes whose relevant search information has already been processed. The open set is normally organized as a priority queue ordered by f(n). Each node additionally requires its best-known g-cost and predecessor information so that the final route can be reconstructed after the goal is reached.
Search begins by assigning the start node a g-cost of zero and inserting it into the priority queue with its corresponding heuristic value. The algorithm repeatedly extracts the node with the smallest f-value and examines its neighbors. For every valid transition, a tentative cost is calculated by adding the edge cost to the current node\'s g-value. If this value improves the recorded cost of the neighbor, its cost and predecessor are updated.
The heuristic function strongly influences search efficiency. Euclidean distance is appropriate when motion approximates continuous straight-line travel, Manhattan distance naturally corresponds to four-connected grids, and diagonal-aware metrics can better represent eight-connected motion. The heuristic should reflect the graph\'s actual transition model because a poorly matched heuristic can cause unnecessary expansions even when the implementation itself is computationally efficient.
For standard optimal A*, the heuristic is commonly required to be admissible, meaning that it never overestimates the true minimum cost from a state to the goal. A consistent heuristic additionally satisfies h(n) ≤ c(n,n\') + h(n\') for neighboring states. Consistency helps ensure that f-values behave predictably along paths and simplifies efficient graph-search implementations by reducing complications associated with reopening previously processed nodes.
The relationship between A* and Dijkstra\'s algorithm becomes clear through the heuristic term. If h(n) = 0 for every node, then f(n) = g(n), and A* behaves as Dijkstra\'s algorithm. As heuristic information becomes more informative while remaining admissible, exploration can become increasingly focused toward the goal. The heuristic therefore provides a mechanism for trading uninformed graph expansion for structured knowledge about the remaining search distance.
Weighted A* modifies the evaluation function to f(n) = g(n) + w·h(n), where the heuristic weight w is typically greater than one. Increasing the contribution of h(n) makes the search more strongly goal-directed. The planner may consequently expand substantially fewer nodes and return a solution faster, but the selected path can become more expensive than the optimal path because estimated future cost is deliberately emphasized over accumulated cost.
The weight provides a direct mechanism for controlling the tradeoff between path quality and search effort. At w = 1, Weighted A* becomes ordinary A*. As w increases, behavior becomes progressively more heuristic-driven and can approach greedy best-first characteristics. This property is useful in robotics when planning latency matters more than obtaining the mathematically shortest possible route, particularly when global paths will subsequently be refined or locally controlled.
Under appropriate assumptions, Weighted A* offers bounded-suboptimal behavior rather than abandoning solution quality without restriction. When an admissible heuristic is used with a fixed weight, the resulting solution cost can be related to the optimal cost through the heuristic inflation factor. This makes Weighted A* attractive for systems that require predictable compromises between computational speed and route quality rather than purely heuristic acceleration without a meaningful performance bound.
Efficient priority-queue management is a critical implementation issue. Many standard heap structures do not directly support decreasing the priority of an existing entry. A common solution is to insert a new entry whenever a better g-cost is discovered and ignore obsolete entries when they are later removed from the queue. This lazy-update strategy simplifies implementation while preserving correct search behavior when stale entries are identified reliably.
Tie breaking also influences practical performance even when it does not change the fundamental algorithm. Multiple nodes may have identical or nearly identical f-values, particularly on regular grids. Secondary ordering based on h-values, g-values, insertion order, or another deterministic criterion can change the number and shape of expanded states. Deterministic tie handling is especially valuable for repeatable testing and debugging of production navigation software.
Neighbor generation must accurately reflect the robot\'s permitted motion. A four-connected grid allows horizontal and vertical transitions, whereas an eight-connected grid also introduces diagonal moves. Orthogonal edges may use unit cost and diagonal edges a cost proportional to √2 when geometric distance is represented. Invalid cells, obstacles, map boundaries, and transitions violating collision constraints must be rejected before they enter the normal relaxation process.
Diagonal motion introduces additional collision considerations. A planner should not automatically permit a diagonal transition merely because the destination cell is free. Moving diagonally between two occupied orthogonal neighbors may cause the robot to cut through an obstacle corner. Implementations therefore commonly apply corner-cutting rules or explicit edge collision checks so that graph connectivity remains consistent with the physical free space available to the robot.
Costmaps extend A* beyond pure shortest-distance planning. A transition can combine geometric movement cost with penalties derived from obstacle inflation, semantic restrictions, terrain properties, or operational preferences. The resulting g-value represents accumulated navigation cost rather than distance alone. Care is required to scale the heuristic consistently with these costs so that its theoretical properties and practical influence remain appropriate for the graph being searched.
Path reconstruction begins after the goal satisfies the algorithm\'s termination condition. Each improved node stores a parent or predecessor indicating the state from which its current best path originated. Starting from the goal, these links are followed backward until the start is reached, and the resulting sequence is reversed. The reconstructed node sequence forms the discrete global path that can subsequently be converted into poses, waypoints, or trajectory references.
Robust implementations must also handle failure explicitly. The priority queue may become empty before the goal is reached, indicating that no path exists within the currently represented graph. The planner should distinguish this condition from invalid start or goal states, blocked endpoints, map errors, resource limits, and search timeouts. Clear failure semantics allow higher-level navigation software to select recovery actions instead of treating every planning failure identically.
Memory usage becomes important on large maps because g-costs, parent information, state flags, and queue entries may be maintained for many nodes. Array-based structures are efficient for dense regular grids, while hash-based representations can be advantageous for sparse or irregular graphs. Preallocated buffers, compact node indexing, and reuse of planning memory can reduce allocation overhead and improve timing consistency in repeatedly executed robot navigation tasks.
Implementation performance should be evaluated using more than final path length. Useful measurements include planning latency, number of expanded nodes, peak frontier size, total path cost, geometric path length, and success rate. Comparing A* and Weighted A* across identical maps reveals the practical effect of heuristic inflation. Such measurements also help determine whether increased weight produces meaningful latency improvement or merely sacrifices path quality without sufficient computational benefit.
For production AMRs, a fixed heuristic weight is not always the only useful strategy. A system may use conservative A* when sufficient computation time is available and a more aggressive Weighted A* when rapid replanning is required. Weight selection can also reflect map scale, mission urgency, computational load, or operational context. The important requirement is that changes remain predictable, testable, and compatible with the navigation system\'s safety constraints.
A* and Weighted A* ultimately share the same implementation foundation: graph construction, neighbor generation, accumulated cost tracking, heuristic evaluation, priority-based expansion, predecessor storage, and path reconstruction. Their essential difference lies in how strongly future estimated cost influences node selection. Understanding this single parameterized relationship provides a practical foundation for implementing efficient global planners and for studying later graph-search accelerations and dynamic replanning methods.
02.03. D Star Lite for Dynamic Environment [w/Code]
:::
D* Lite is an incremental graph-search algorithm designed for navigation in environments where traversal costs can change after planning has begun. Instead of discarding previous search results whenever newly sensed obstacles modify the map, it repairs only the affected portions of the solution. This property makes D* Lite particularly useful for mobile robots that repeatedly discover changes while moving toward a goal.
The algorithm addresses a fundamental limitation of conventional static planners such as A*. A* can efficiently compute a path on a known graph, but when an obstacle appears or an edge cost changes, a straightforward implementation may perform a new search from the robot\'s current position. D* Lite preserves information from earlier searches and reuses it, reducing repeated computation when environmental changes affect only part of the graph.
D* Lite is closely related to Lifelong Planning A* (LPA*) and inherits its incremental-search principles. Rather than maintaining only a single best-known path cost for each state, D* Lite maintains two important values, conventionally written as g(s) and rhs(s). Their relationship represents whether the current information associated with a state remains consistent after changes in the graph.
The value g(s) represents the planner\'s current estimate of the cost associated with a state, while rhs(s) is a one-step lookahead value computed from neighboring states. For ordinary states, rhs(s) can be expressed using the minimum over successor transitions of c(s,s\') + g(s\'). The goal is initialized with rhs(goal) = 0, establishing the destination as the anchor from which cost information propagates through the graph.
A state is locally consistent when g(s) = rhs(s). If these values differ, the state is inconsistent and may require processing because some graph information affecting its best route has changed. This consistency mechanism is central to incremental replanning. Rather than searching the entire map again, D* Lite identifies inconsistent states and propagates only the corrections required to restore a valid shortest-path structure.
D* Lite performs its search conceptually backward from the goal toward the robot\'s current state. This direction is advantageous because the goal often remains fixed while the robot moves continuously. Cost-to-go information can therefore be retained as the robot changes position. When new obstacles are detected, the planner updates affected edges and repairs the previously computed information instead of reconstructing the entire search from the new start.
A priority queue called the open list stores inconsistent states that may influence the current solution. Unlike basic A*, D* Lite uses a two-component priority key. The key incorporates the minimum of g(s) and rhs(s), a heuristic estimate between the robot\'s current state and the candidate state, and an additional correction term commonly denoted km. Lexicographic ordering of these key components determines which inconsistent state is processed next.
The km term compensates for movement of the robot\'s start state between replanning episodes. As the robot advances, km is increased according to the heuristic distance between the previous and current start positions. This mechanism allows priorities to be adjusted without rebuilding the complete queue whenever the robot moves, contributing to the efficiency of repeated planning during navigation.
The UpdateVertex operation is one of the core procedures in D* Lite. When the cost of an edge changes, rhs values associated with affected states are recalculated from their successors. States for which g and rhs become unequal are inserted into or updated in the priority queue. States that regain consistency no longer require pending repair, allowing computation to remain concentrated around graph regions influenced by the change.
The ComputeShortestPath procedure repeatedly processes prioritized inconsistent states until the current start state has sufficient information to define a valid shortest path. Depending on the relationship between g and rhs, processing may lower an overestimated state value or invalidate an outdated value and propagate that change to neighboring states. Through repeated local updates, global path consistency is restored without restarting the search.
Consider an AMR traveling through a warehouse aisle using a path computed from the current costmap. If a pallet is unexpectedly placed across the planned corridor, perception updates the affected cells from free space to occupied or high-cost space. D* Lite changes the corresponding graph costs, marks relevant states inconsistent, repairs the cost structure, and derives an alternative route around the newly discovered obstruction.
The same mechanism can handle decreases in traversal cost. An obstacle previously blocking a corridor may disappear, a temporary restricted region may reopen, or updated sensing may reveal that previously uncertain space is traversable. D* Lite can propagate the improved costs and discover a shorter route. Incremental planning therefore supports both deterioration and improvement of the represented environment.
Graph construction remains as important for D* Lite as for A*. An occupancy grid may be represented using four-connected or eight-connected neighborhoods, while a topological navigation system may use intersections, rooms, doors, or waypoints as graph nodes. Edge costs must represent the intended motion and costmap semantics accurately because incremental search can only optimize the graph that the navigation system provides.
Collision validity also requires careful treatment when edge costs change. A newly occupied cell can invalidate multiple transitions, and diagonal motion may require corner-collision checks involving neighboring cells. For a robot with a nonzero footprint, obstacle inflation or footprint-aware collision checking should be reflected in the graph before replanning. Otherwise, a mathematically valid repaired path may still be physically unsafe.
D* Lite should not be confused with dynamic obstacle avoidance at the controller level. The algorithm replans a graph when the represented traversal costs change, but rapidly moving pedestrians, vehicles, or other robots may require prediction and short-horizon collision avoidance. In a complete navigation architecture, D* Lite can provide an updated global path while a local planner or controller reacts continuously to immediate dynamic obstacles.
The computational advantage of D* Lite is strongest when map changes are localized relative to the entire planning problem. If only a small region changes, previously computed values for most states remain useful and the repair can be much cheaper than complete replanning. If large portions of the map change simultaneously, however, many states can become inconsistent and the advantage over a fresh search may decrease substantially.
Memory requirements are also important because incremental planning intentionally preserves search information. Implementations maintain g and rhs values, priority information, map costs, and graph connectivity across replanning cycles. Dense grid maps can use indexed arrays for efficient access, while sparse graphs may use associative structures. Priority-queue handling must also correctly manage updated or obsolete entries to avoid unnecessary processing.
Heuristic design follows principles similar to A*. The heuristic should reflect the underlying motion model and normally satisfy consistency requirements needed by the algorithm. Manhattan, Euclidean, or diagonal-aware distance measures may be appropriate depending on graph connectivity. Because the heuristic participates in priority calculation during repeated replanning, inappropriate scaling can degrade search efficiency even if graph updates themselves are handled correctly.
A practical implementation usually operates as a continuous sense-update-replan-execute cycle. The robot follows the current path while sensors update the local representation of the environment. Detected cost changes are converted into graph updates, affected vertices are revised, and ComputeShortestPath repairs the solution. The robot then extracts a new path from its current state and continues moving while additional observations arrive.
Path extraction typically selects successive states according to transition cost plus the repaired g-value of neighboring states. This produces a route descending through the maintained cost-to-go structure toward the goal. The resulting discrete path may subsequently require smoothing, waypoint conversion, velocity generation, and local collision checking before execution, just as paths generated by other graph-based global planners do.
Evaluation of D* Lite should therefore measure more than final path length. Important metrics include initial planning time, replanning latency after map changes, number of updated or expanded states, path cost, memory consumption, and success under repeated environmental changes. Comparing D* Lite with repeated A* on identical change sequences reveals whether incremental reuse provides meaningful computational benefits for the intended robot and map scale.
D* Lite extends graph-based planning from one-time shortest-path computation toward continuous path maintenance. Its key ideas are backward cost propagation, paired g and rhs values, local consistency, prioritized processing of inconsistent states, and reuse of previous search information. Together, these mechanisms allow an autonomous robot to adapt its global route efficiently as its knowledge of the environment evolves during navigation.
02.04. Jump Point Search JPS Acceleration [w/Code]
Jump Point Search (JPS) is an acceleration technique for shortest-path search on uniform-cost grid maps. It preserves the fundamental optimality of A* while reducing the number of grid cells that must be explicitly expanded. Instead of evaluating every intermediate cell along a straight or diagonal direction, JPS identifies strategically important locations called jump points and allows the search to move directly between them.
The motivation for JPS comes from symmetry in regular grids. Consider a robot moving through a large open area using an eight-connected grid. Many different sequences of horizontal, vertical, and diagonal steps represent paths with identical cost. Conventional A* may examine numerous equivalent alternatives even though most do not provide useful routing decisions. JPS removes these symmetric possibilities without changing the underlying shortest-path solution.
JPS can therefore be understood as a search-space pruning method rather than a fundamentally different shortest-path objective. It typically uses the same accumulated path cost g(n), heuristic h(n), and evaluation value f(n) = g(n) + h(n) associated with A*. The major difference lies in successor generation: ordinary A* generates nearby cells, whereas JPS selectively generates distant jump points reached by repeatedly stepping in particular directions.
The algorithm distinguishes between natural neighbors and forced neighbors. Natural neighbors are successors that remain relevant when movement continues in the direction from which the current node was reached. Other symmetric alternatives can be pruned because an equivalent or cheaper route can reach them without passing through the current node. This directional pruning dramatically reduces branching in open and regularly structured regions.
Forced neighbors arise when obstacles break the symmetry of otherwise equivalent paths. An obstacle adjacent to the current direction of travel can make a particular alternative direction necessary for preserving shortest-path possibilities. Such a neighbor cannot safely be pruned because the obstacle prevents an equivalent symmetric route. Detecting forced neighbors is therefore one of the central mechanisms that enables JPS to reduce search while maintaining optimality.
The jump operation repeatedly advances from a node in a selected direction rather than immediately returning the next adjacent grid cell. The process continues until it reaches the goal, encounters an obstacle or map boundary, or discovers a location satisfying the jump-point condition. Intermediate cells are traversed conceptually but are not inserted into the main search frontier as ordinary expanded nodes.
For horizontal or vertical motion, a cell becomes significant when a forced neighbor is detected. If no such condition exists, the jump procedure continues along the same direction. In a large open corridor, this means that many consecutive cells can effectively be skipped. The search graph explored by the planner becomes much smaller even though the geometric grid itself remains unchanged.
Diagonal movement requires additional logic because a diagonal route interacts with both horizontal and vertical directions. During a diagonal jump, JPS also examines whether useful jump points exist along the corresponding straight directions. If either component direction reveals a forced condition, the current diagonal position can become a jump point. This recursive structure allows JPS to preserve shortest paths while aggressively pruning symmetric grid states.
Obstacle geometry therefore determines where jump points emerge. In completely open space, relatively few nodes may need to be considered between the start and goal. Near walls, corners, narrow passages, and obstacle boundaries, additional jump points appear because movement choices become constrained. JPS naturally concentrates search effort around locations where the environment creates meaningful routing decisions.
The effectiveness of JPS is particularly noticeable on large uniform grids with substantial open regions. Standard A* may expand many cells whose only purpose is to continue motion in the same direction. JPS compresses these repeated decisions into longer jumps, reducing node expansions and priority-queue operations. The resulting improvement can significantly decrease planning time without requiring a coarser map representation.
JPS does not simply shorten the output path by removing waypoints after planning. Its pruning occurs during graph search itself. This distinction is important because path smoothing and JPS solve different problems. Smoothing modifies the geometry or representation of an already generated path, whereas JPS reduces the computational effort required to discover an optimal grid path in the first place.
A typical implementation combines JPS with an A*-style open list. The start state is inserted into a priority queue, and the state with the smallest evaluation value is repeatedly selected. Instead of expanding all conventional neighbors, the planner determines allowable search directions from the parent relationship and invokes the jump procedure. Valid jump points are assigned costs, parent information, and priorities before entering the open list.
The accumulated cost between two jump points must represent the complete distance traveled through all skipped cells. A horizontal or vertical jump spanning several cells therefore contributes the corresponding straight-line grid distance, while diagonal movement must use the appropriate diagonal cost. Skipping intermediate expansions does not mean ignoring their traversal cost; JPS changes search representation while preserving path-cost semantics.
The heuristic used with JPS should also correspond to the movement model. For an eight-connected grid with standard straight and diagonal movement costs, an octile-style distance is commonly appropriate because it represents the minimum unobstructed combination of diagonal and orthogonal steps. A heuristic that accurately reflects the connectivity helps retain the goal-directed efficiency of A* in addition to the symmetry reduction provided by JPS.
Collision rules require careful implementation, especially for diagonal motion. Whether the robot may move diagonally past obstacle corners depends on the connectivity and corner-cutting policy of the grid. The jump procedure, forced-neighbor tests, and ordinary movement-validity checks must all use the same rule. Inconsistent collision assumptions can invalidate the pruning logic and potentially generate routes that are not physically traversable.
In robotic navigation, grid cells often carry nonuniform costs produced by obstacle inflation, terrain information, semantic restrictions, or safety margins. Classical JPS is most naturally suited to uniform-cost grids because its symmetry arguments depend on equivalent movement costs. When arbitrary cell penalties are introduced, straightforward JPS pruning may no longer preserve the assumptions used to eliminate alternative paths, requiring extensions or different planning strategies.
This limitation is important for practical AMR costmaps. A binary free-space map can provide conditions well suited to JPS, whereas a richly graded navigation costmap may assign different costs to cells near obstacles. In such cases, reducing search time must be balanced against preserving the intended cost semantics. The planner should not apply classical pruning rules blindly when traversing nominally free cells carries substantially different penalties.
Map resolution also affects the value of JPS. High-resolution occupancy grids contain many cells and therefore create more opportunities for conventional A* to perform repetitive expansions. JPS can remove much of this redundancy when the map structure supports long unobstructed jumps. On small maps or highly cluttered environments, however, jumps may terminate frequently and the relative acceleration can become less significant.
Memory efficiency can improve because fewer meaningful states may enter the open and closed structures, although implementation details remain important. The planner still requires grid access, accumulated costs, parent information, and priority-queue management. Recursive jump implementations should also be designed carefully because very long unobstructed regions can create deep call chains; iterative implementations may provide more predictable behavior in production software.
Path reconstruction follows the parent relationships between jump points after the goal is reached. Because consecutive parents can be separated by multiple grid cells, the resulting representation may initially contain only the important turning or jump locations. If downstream software requires every traversed grid cell, the segments can be interpolated. If a controller accepts sparse waypoints, additional processing may instead convert the jump-point route directly into a suitable reference path.
JPS performance should be evaluated against ordinary A* under identical map, connectivity, heuristic, and collision assumptions. Useful measurements include planning latency, number of expanded states, priority-queue operations, final path cost, memory usage, and success rate. Open maps, corridor networks, cluttered maps, and narrow passages should all be tested because the degree of symmetry strongly influences the acceleration that JPS can achieve.
JPS is best viewed as an example of exploiting structure in a planning problem rather than changing the optimization objective. A* treats the grid as a general graph and searches many locally available transitions, while JPS recognizes that uniform grids contain large numbers of symmetric paths that need not be explored separately. By pruning natural redundancies and retaining obstacle-induced forced choices, it constructs a much smaller effective search space.
For robot navigation systems that use large, mostly uniform grid maps, this reduction can provide a valuable acceleration mechanism for global planning. Its benefits are strongest when movement costs and connectivity match the assumptions underlying the pruning rules. Understanding JPS therefore demonstrates a broader planning principle: substantial speed improvements can sometimes be obtained not by accepting poorer paths, but by identifying and eliminating redundant search states while preserving the optimal solution.
02.05. Theta Star Any Angle Path Planning [w/Code]
Theta* is an any-angle graph-search algorithm designed to overcome the directional restrictions imposed by conventional grid-based path planning. Standard A* on a four- or eight-connected grid produces paths constrained to predefined grid edges, often creating staircase-like or unnecessarily segmented routes. Theta* retains an A*-like search structure while allowing parent connections across multiple cells whenever direct line of sight exists.
The central objective of Theta* is to approximate continuous-space shortest paths while still exploiting the computational convenience of a discrete grid. Rather than requiring every path segment to follow horizontal, vertical, or diagonal neighbor connections, the algorithm permits straight-line connections between nonadjacent vertices. This produces paths whose headings are not restricted to the small set of directions defined by grid connectivity.
Theta* maintains familiar A* components including an open list, accumulated path cost g(s), heuristic estimate h(s), evaluation value f(s) = g(s) + h(s), and a parent pointer for each state. The major difference appears during vertex expansion. When evaluating a neighbor, Theta* tests whether that neighbor can connect directly to the parent of the current state instead of automatically using the current state as its parent.
Suppose the search expands state s and examines neighboring state s\'. Conventional A* normally evaluates the path through s using g(s) + c(s,s\'). Theta* first checks whether parent(s) has line of sight to s\'. If this direct connection is collision-free, it evaluates the alternative cost g(parent(s)) + c(parent(s),s\'). If that cost is better, s\' inherits parent(s), effectively skipping an unnecessary intermediate vertex.
This parent-skipping mechanism is the essential operation behind any-angle behavior. A chain of neighboring grid states may be explored during search, but the final parent relationships can connect states separated by several cells. Consequently, the reconstructed route contains longer straight segments and fewer artificial turns. The graph is therefore searched using discrete states while the resulting path is not strictly constrained to the original grid edges.
Line-of-sight checking is the key geometric operation required by Theta*. Given two grid vertices, the planner determines whether the straight segment connecting them passes entirely through traversable space. Grid traversal methods similar to Bresenham-style line algorithms or digital differential techniques can efficiently examine the cells intersected by the segment. If any required cell is occupied or violates collision constraints, the direct connection is rejected.
Collision semantics must be defined carefully because line of sight for a mathematical point is not automatically safe for a physical robot. An AMR has a finite footprint, and a segment passing close to an obstacle may cause collision even when its centerline crosses only free cells. Practical implementations therefore combine inflated obstacles, footprint-aware collision checking, or safety margins with the line-of-sight test to represent the actual traversable configuration space.
The distinction between Theta* and simple path smoothing is important. A conventional planner can first compute an A* path and then attempt to remove unnecessary waypoints using line-of-sight checks. Theta*, however, incorporates visibility-based shortcuts directly into the search process. Parent selection and path cost evolve according to these shortcuts, allowing the search itself to reason about any-angle alternatives rather than modifying only the final path afterward.
The heuristic used by Theta* normally estimates geometric distance to the goal. Euclidean distance is particularly natural because the algorithm permits straight-line path segments with arbitrary headings. The heuristic guides expansion toward the destination while g-values represent the accumulated lengths or costs of accepted connections. As with A*, appropriate heuristic properties are important for predictable search behavior and efficient exploration.
Theta* typically begins by assigning the start vertex a zero g-value and setting its parent to itself. The start is inserted into a priority queue ordered by f. At each iteration, the lowest-priority state is removed and its neighboring states are examined. For each neighbor, the algorithm evaluates whether a direct connection through the current state\'s parent is possible; otherwise, it falls back to a conventional connection through the current state.
If line of sight exists, the planner can potentially reduce both path length and the number of turns by replacing two or more grid-constrained segments with one direct segment. If visibility is blocked by an obstacle, the normal neighbor relationship remains available. Theta* therefore combines ordinary local graph connectivity with opportunistic long-range geometric connections, allowing it to operate robustly around obstacles while exploiting open space whenever possible.
Obstacle corners play an important role in Theta* paths. In open regions, parent links can span long distances and create nearly direct routes. Near obstacles, visibility becomes restricted, causing useful parent states to appear around corners and boundaries. The resulting path often resembles a sequence of visibility-driven segments connecting geometrically significant locations rather than a dense sequence of adjacent grid cells.
Any-angle planning is particularly valuable when grid discretization produces unrealistic heading changes. A mobile robot following an eight-connected A* path may repeatedly alternate between straight and diagonal segments even when a smooth diagonal trajectory exists through free space. Theta* can connect distant visible states directly, reducing these grid artifacts and generating a global path that is usually more suitable for downstream trajectory generation and control.
Theta* does not eliminate the need for path smoothing or trajectory optimization. Although the geometric path generally contains fewer unnecessary turns than a conventional grid path, sharp direction changes may still occur near obstacles. Robot kinematics, minimum turning radius, acceleration limits, steering constraints, and dynamic feasibility are not automatically satisfied. A downstream planner or controller may therefore further refine the Theta* path.
Computational cost depends strongly on line-of-sight operations. Standard A* evaluates local graph transitions, whereas Theta* may repeatedly inspect multiple grid cells between candidate endpoints. Each individual expansion can therefore require more geometric work. This additional cost is exchanged for improved path geometry, and efficient visibility checking, caching, map representation, and collision routines become important for high-performance implementations.
Basic Theta* and related variants explore different compromises between search effort and path quality. Lazy approaches can postpone some expensive line-of-sight checks until they are actually required, while other variants refine parent selection or visibility handling. These extensions preserve the central idea that grid vertices provide the search states but should not necessarily dictate every geometric segment of the final route.
Nonuniform costmaps introduce additional considerations. A straight visible connection may geometrically cross free space while traversing cells with different penalties caused by obstacle inflation, terrain difficulty, or semantic preferences. If path cost includes these penalties, the cost of a long parent connection must account for the cells or regions crossed by the segment. Treating every visible shortcut as pure Euclidean distance can otherwise violate the intended cost model.
Map resolution remains an important design parameter. Fine grids provide more accurate obstacle boundaries and more candidate vertices for visibility connections but increase the number of states and the cost of line-of-sight evaluation. Coarser grids reduce computation but may distort narrow passages or obstacle geometry. Theta* reduces directional artifacts caused by grid connectivity, but it cannot recover environmental detail that was lost during map discretization.
Path reconstruction follows parent pointers from the goal back to the start, as in A*, but consecutive parent states may be far apart. Reversing this sequence produces an any-angle polyline consisting of relatively long collision-free segments. These segments can be converted into navigation waypoints, sampled more densely if required, or passed to a local planner that generates dynamically feasible motion while responding to newly observed obstacles.
For production navigation, Theta* should be benchmarked against grid A*, JPS, and post-smoothed A* under identical maps and collision assumptions. Useful measurements include planning latency, expanded nodes, number and cost of line-of-sight tests, path length, number of heading changes, obstacle clearance, memory usage, and success rate. Such evaluation reveals whether improved geometric path quality justifies the additional visibility computations.
Theta* occupies an important position between purely discrete graph planning and continuous geometric planning. It retains the predictable data structures and search framework of A* while relaxing the requirement that paths follow grid edges. This makes it especially attractive when occupancy grids are already available but the robot requires routes that better approximate the geometry of continuous free space.
The broader lesson of Theta* is that discretization need not completely determine the geometry of a planned path. Grid vertices can provide a convenient computational structure while visibility relationships recover movement freedom between those vertices. By combining A*-style cost search with line-of-sight parent selection, Theta* provides an effective foundation for generating shorter, less grid-constrained, and more natural global paths for robotic navigation.
02.06. Topological Map Based Planning for AMR [w/Code]
Topological map-based planning represents an AMR environment as a network of meaningful places and connectivity relationships rather than as a dense collection of geometric grid cells. Important locations such as intersections, doors, workstations, elevators, charging stations, docking points, and corridor junctions become nodes, while feasible travel connections between these locations become edges. This abstraction supports efficient planning across large industrial facilities.
A topological map answers a different question from a metric occupancy map. A metric map describes where free space and obstacles exist using coordinates and geometric measurements, whereas a topological map emphasizes which operational locations are connected and how the robot can travel between them. For AMRs, both representations are valuable: topology supports high-level route decisions, while metric information supports precise local navigation and collision avoidance.
Each topological node normally contains more information than a simple graph identifier. A node may include a metric pose, orientation, semantic label, operational type, allowable robot classes, docking parameters, or references to local maps. A charging station node, for example, can represent both a logical destination in the route network and the precise pose from which a local navigation or docking procedure begins.
Edges represent feasible transitions between nodes and can also carry rich attributes. An edge may store geometric distance, expected travel time, speed limit, directionality, corridor width, maximum robot size, floor type, safety restrictions, or energy cost. Industrial systems can therefore calculate routes according to operational objectives rather than treating every connection as an identical movement opportunity.
Directed edges are particularly important in structured facilities. A warehouse may define one-way aisles to prevent conflicts, separate inbound and outbound traffic, or constrain robot movement around production equipment. Representing these rules directly in the topological graph allows the global planner to exclude prohibited directions before geometric motion planning begins, simplifying downstream navigation and improving operational consistency.
Route planning on a topological map becomes a graph-search problem. Algorithms such as Dijkstra or A* can search from the node associated with the robot\'s current region to the destination node. The resulting route is a sequence of nodes and edges rather than a dense cell-by-cell path. Because the topological graph can contain dramatically fewer states than a high-resolution occupancy grid, long-distance route planning can be computationally efficient.
A* is especially useful when nodes have metric coordinates or another meaningful distance relationship. The accumulated cost g(n) represents the cost of traversed topological edges, while h(n) estimates the remaining cost to the destination. Euclidean distance, estimated travel time, or another lower-bound metric can guide the search. The final route can optimize distance, time, energy, risk, or a weighted combination of operational criteria.
For an AMR, the topological route must ultimately be converted into executable navigation commands. A route such as Start Zone → Junction A → Corridor B → Workstation C identifies the intended high-level movement sequence. The navigation system can then generate metric goals or local paths for each edge, execute them using a local planner and controller, and advance to the next topological segment after confirming successful arrival.
This hierarchical relationship between topological and metric planning is highly useful in large factories and warehouses. A facility-wide planner does not need to repeatedly search millions of occupancy-grid cells to decide which building corridor should be used. It can first select a compact topological route and then invoke detailed geometric planning only within the relevant corridor, room, or local map region.
Topological maps can also divide a large environment into manageable navigation zones. Individual floors, production cells, warehouse sectors, or buildings may maintain separate metric maps while a higher-level graph describes connections among them. Doors, elevators, ramps, transfer stations, and gateways become transition nodes. This organization supports scalable navigation without requiring one monolithic geometric map for the entire facility.
Semantic information strengthens this representation further. Nodes can be labeled as loading stations, inspection points, restricted zones, waiting positions, charging areas, or human-interaction locations. Edges can represent normal aisles, narrow passages, safety corridors, outdoor links, or controlled crossings. Planning can then incorporate the operational meaning of the environment instead of relying exclusively on geometric distance.
A route cost can combine multiple operational factors. One edge may be physically shorter but pass through a congested aisle, while another is longer but offers higher speed and lower interference. The planner can assign costs using expected traversal time, congestion, energy demand, safety margin, or mission priority. Consequently, the shortest geometric route does not necessarily have to be the preferred operational route.
Dynamic edge costs make the topology useful for changing facility conditions. If an aisle becomes congested, a door closes, maintenance blocks a corridor, or another fleet reserves a shared resource, the corresponding edge can receive a higher cost or become temporarily unavailable. The planner can then search the same graph again and select an alternative route without reconstructing the complete spatial representation of the facility.
Resource nodes require additional coordination beyond ordinary shortest-path planning. Elevators, automatic doors, narrow corridors, loading stations, and shared docking areas may allow only limited simultaneous access. An AMR may therefore need to reserve a resource before traversing an associated edge or entering a node. The topological graph provides a natural structure for linking route planning with fleet-level resource management.
Multi-robot operation further increases the value of topological planning. Instead of reasoning about conflicts over every metric cell, a fleet management system can reason about occupancy and reservations at meaningful graph segments. Robots may reserve corridors, intersections, or critical sections over time. This does not eliminate local collision avoidance, but it can prevent many predictable traffic conflicts before robots physically approach one another.
Localization connects the robot\'s continuous pose to the discrete topology. The AMR must determine which node, edge, or region corresponds to its current metric position before high-level route planning can begin. This process is often called topological localization or graph association. Robust association is important near intersections or overlapping regions because selecting the wrong graph element can produce an inappropriate route or transition command.
Transitions between topological segments require explicit completion criteria. Reaching a node should not always mean merely crossing a coordinate threshold. A docking node may require precise pose convergence, a doorway transition may require confirmation that the robot has completely entered the next region, and an elevator node may require successful communication with external infrastructure. Node semantics can define these specialized arrival conditions.
Failure handling is therefore an essential part of a practical topological planner. An AMR may fail to traverse an edge because of an unexpected obstacle, localization uncertainty, closed infrastructure, or controller failure. The system should distinguish temporary local blockage from permanent route invalidity. It can retry local navigation, increase an edge cost, disable the edge temporarily, or request a new topological route depending on the failure condition.
Topological graphs also simplify map maintenance when industrial layouts change. If a workstation moves or a corridor becomes unavailable, only affected nodes, edges, and associated local geometry may need modification. This can be easier to manage than rebuilding a facility-wide planning representation. Versioning and validation remain important because incorrect connectivity can create routes that are logically valid in the graph but impossible in the physical environment.
The level of abstraction must be selected carefully. A graph with too few nodes may fail to represent important route alternatives, narrow passages, or operational restrictions. A graph with excessive nodes begins to reproduce the complexity of a dense geometric representation and loses some scalability advantages. Useful nodes generally correspond to locations where navigation choices, semantic transitions, resource interactions, or operational decisions occur.
Topological planning does not replace obstacle perception or local planning. The graph may indicate that a corridor is available even when a pallet, person, or another robot temporarily blocks it. Sensors and local costmaps must continue to represent immediate conditions, while local planners generate collision-free motion. If a blockage persists, information can propagate upward and cause the topological planner to select another edge or route.
A robust AMR architecture can therefore operate hierarchically: mission management selects a destination, topological planning chooses the facility-level route, metric global or local planning determines geometric motion through each segment, and the controller executes the resulting trajectory. Feedback flows in the opposite direction so that local failures, blocked edges, completed transitions, and localization changes can modify high-level planning decisions.
Performance evaluation should include more than graph-search execution time. Relevant measures include end-to-end travel time, route cost, number of replanning events, congestion avoidance, edge traversal success, resource waiting time, localization-to-topology association reliability, and fleet throughput. Large-scale tests should also examine how planning latency changes as the number of nodes, edges, robots, and dynamic restrictions increases.
Topological map-based planning ultimately provides AMRs with an operational view of space. Instead of treating a factory or warehouse only as millions of geometric cells, it represents the environment as meaningful places connected by permitted movements. Combined with metric navigation, semantic information, dynamic costs, and fleet coordination, this approach provides a scalable foundation for facility-wide autonomous navigation and multi-robot operation.
02.07. Roadmap Planning Visibility Graph Voronoi [w/Code]
Roadmap planning transforms continuous free space into a graph that captures strategically useful connectivity for robot motion. Instead of searching every position in the environment, the planner constructs nodes at geometrically meaningful locations and connects them with collision-free edges. A graph-search algorithm can then determine a route from start to goal, after which the selected graph path is converted back into geometric motion through the workspace.
The roadmap concept separates geometric representation from graph search. Geometry determines which configurations or locations should become roadmap vertices and which pairs can be connected safely, while algorithms such as Dijkstra or A* determine the best sequence of roadmap edges. This decomposition allows different roadmap construction methods to emphasize different navigation objectives without fundamentally changing the shortest-path search mechanism.
A roadmap is especially useful when obstacles can be represented geometrically as polygons or boundaries. The planner analyzes free-space structure and extracts a smaller connectivity graph that preserves important route alternatives. The start and goal are normally inserted into this graph and connected to visible or reachable roadmap vertices. Planning then occurs on a compact representation rather than directly over the full continuous environment.
A visibility graph constructs roadmap vertices primarily from obstacle vertices, together with the start and goal. Two vertices are connected when the straight segment between them lies entirely in collision-free space. The resulting graph captures direct geometric visibility relationships among important points. Because shortest paths around polygonal obstacles tend to pass near obstacle vertices, visibility graphs provide a powerful representation for Euclidean shortest-path planning.
Construction begins by identifying candidate vertices from polygonal obstacle boundaries. For every relevant pair of vertices, the planner performs a line-of-sight or segment-intersection test. If the connecting segment does not penetrate an obstacle, an edge is created with a weight usually equal to its Euclidean length. Start and goal visibility connections are added using the same collision-validity rule before graph search begins.
The principal advantage of a visibility graph is path efficiency. Since edges directly connect mutually visible vertices, Dijkstra or A* can select routes composed of long straight segments. For point robots among polygonal obstacles, the resulting solution can correspond closely to the geometric shortest path. The representation therefore avoids the directional artifacts that can appear when paths are restricted to a regular four- or eight-connected grid.
This shortest-path behavior also produces an important practical limitation. Visibility-graph routes frequently pass very close to obstacle corners because touching or approaching those vertices minimizes geometric distance. A physical robot requires clearance for its footprint, localization uncertainty, tracking error, and safety margin. Obstacles must therefore be expanded in configuration space or visibility edges must be checked using footprint-aware collision constraints before execution.
Visibility graph construction can also become computationally expensive when the environment contains many obstacle vertices. A naive implementation tests visibility between many pairs of candidates, creating substantial geometric intersection work and potentially a dense graph. Efficient visibility algorithms, spatial indexing, obstacle preprocessing, and selective edge generation can reduce this cost, particularly when roadmaps are built repeatedly for large environments.
A Voronoi roadmap follows a substantially different geometric principle. Instead of favoring paths close to obstacle vertices, it constructs routes along locations that are approximately equidistant from nearby obstacles. These locations form the Voronoi diagram or generalized Voronoi structure of free space. Following this structure tends to maximize obstacle clearance, making the roadmap attractive when safety margin and navigation robustness are more important than minimum geometric distance.
In two-dimensional free space, Voronoi edges describe loci where points have equal distance to two nearby obstacle features, while Voronoi vertices occur where relationships among multiple features meet. The resulting network often runs through the medial regions of corridors and open spaces. Once this network is extracted, it can be treated as a graph whose vertices and edges provide candidate routes through the environment.
The start and goal usually do not lie exactly on the Voronoi structure. They must therefore be connected to suitable roadmap locations through collision-free access edges. After these connections are established, graph search determines a route through the Voronoi network. The final path consists of an entry segment, one or more high-clearance roadmap segments, and an exit segment connecting the network to the destination.
Voronoi planning naturally favors clearance because the roadmap is positioned away from surrounding obstacles. In a corridor, the path tends to follow a centerline rather than hugging one wall. This can provide additional tolerance for localization errors, controller tracking deviations, uncertain obstacle boundaries, and finite robot dimensions. Such properties can be valuable for AMRs operating in industrial environments where reliable traversal may be preferred over absolute minimum path length.
The primary tradeoff is that a high-clearance Voronoi route may be longer than the Euclidean shortest path. A visibility graph can cut directly around obstacle corners, whereas a Voronoi graph may guide the robot toward central free-space regions before approaching the goal. The two representations therefore embody different geometric priorities: visibility graphs emphasize path length, while Voronoi roadmaps emphasize clearance and robustness.
These priorities need not be treated as an absolute choice. Edge costs can combine geometric length with clearance penalties, risk measures, energy consumption, or operational preferences. A roadmap derived from one geometric structure can also be augmented with additional connections. Hybrid planning can therefore seek paths that are reasonably short while avoiding unnecessarily small obstacle clearances, producing behavior better suited to practical robot navigation.
Configuration-space reasoning remains essential for both methods. The environment seen by a robot\'s reference point must account for the robot\'s physical footprint. Polygonal obstacles can be expanded according to robot dimensions and safety margins before constructing a visibility graph or Voronoi roadmap. For noncircular robots, orientation-dependent collision geometry can make this transformation more complex because free space is no longer represented only by planar position.
Roadmap validity must also reflect environmental changes. If a new pallet blocks a corridor or a temporary obstacle intersects an existing edge, that edge can no longer be assumed traversable. Collision checking can invalidate affected connections and trigger graph replanning. When changes are substantial, portions of the roadmap may need to be reconstructed because the underlying visibility or Voronoi relationships themselves have changed.
Roadmaps are therefore well suited to relatively structured or slowly changing environments where useful geometric connectivity can be reused across multiple planning queries. A factory layout, warehouse aisle network, or known outdoor site may allow a roadmap to be generated once and queried repeatedly for different start and goal positions. Precomputation shifts part of the geometric planning cost away from real-time route requests.
The representation can also support hierarchical planning. A coarse roadmap can determine movement through major corridors or free-space regions, while a local planner handles detailed obstacle avoidance and trajectory generation. The roadmap supplies strategic connectivity, but the local navigation layer remains responsible for short-horizon sensing, dynamic obstacles, kinematic constraints, and precise execution of the selected route.
Visibility graphs and Voronoi diagrams should therefore not be interpreted as complete motion-control solutions. They generate geometric route structures, but a mobile robot still requires localization, perception, collision monitoring, path tracking, velocity control, and potentially path smoothing. A route that is geometrically valid may also require modification to satisfy minimum turning radius, steering limits, acceleration constraints, or other vehicle-specific requirements.
Performance evaluation should reflect the different objectives of the two roadmap types. Relevant measures include roadmap construction time, number of vertices and edges, graph-search latency, path length, minimum obstacle clearance, average clearance, memory consumption, and replanning cost after environmental changes. Comparing only path length would systematically undervalue the safety-oriented characteristics that motivate Voronoi-based planning.
For AMR applications, visibility graphs are attractive when accurate obstacle geometry is available and short geometric routes are important. Voronoi roadmaps are attractive when maintaining separation from obstacles improves robustness and safety. The appropriate choice depends on map representation, robot footprint, localization accuracy, available computation, required clearance, and whether the environment changes frequently during operation.
Roadmap planning ultimately reduces a continuous geometric navigation problem to a strategically constructed graph-search problem. Visibility graphs preserve direct line-of-sight opportunities and tend toward short paths, while Voronoi structures capture the medial geometry of free space and tend toward high-clearance paths. Understanding both approaches reveals how graph construction itself can encode different navigation priorities before Dijkstra, A*, or another search algorithm selects the final route.
02.08. Multi Level Graph Planning Floor Zone Room [w/Code]
Multi-level graph planning organizes a large navigation environment into several interconnected layers of abstraction, such as floors, zones, rooms, corridors, and local navigation regions. Instead of representing an entire building as one enormous graph or occupancy grid, the planner separates strategic connectivity from detailed movement. This hierarchy allows an AMR to reason efficiently about long-distance routes while preserving precise navigation where it is required.
At the highest level, floors can be represented as major graph regions connected through elevators, ramps, lifts, or other vertical transportation resources. A robot traveling from a workstation on one floor to a destination on another does not initially need to consider every corridor and doorway. It first determines which floor transitions are feasible and which inter-floor connection provides the most appropriate route for the mission.
Within each floor, the environment can be divided into operational zones. A zone may represent a warehouse sector, production area, office region, loading area, clean room, or restricted section. Zone-level planning captures major horizontal connectivity and operational constraints. This abstraction reduces the number of alternatives considered during facility-wide planning while preserving the meaningful structure needed to select appropriate routes.
Rooms and corridors provide a finer graph level below zones. Rooms may correspond to work cells, storage spaces, inspection areas, charging rooms, or service locations, while corridors provide connections among them. Doorways, gates, intersections, and narrow passages naturally become transition nodes. At this level, the graph describes how the robot can move between specific operational spaces without yet specifying every geometric motion along the route.
The resulting hierarchy can be interpreted as Floor → Zone → Room → Local Navigation. Each level answers a different planning question. The floor level determines large-scale vertical movement, the zone level selects major operational regions, the room level determines local semantic connectivity, and the metric navigation level generates collision-free geometric motion. Together, these layers form a structured decomposition of a complex facility.
A hierarchical graph can be implemented using parent-child relationships between levels. A floor node contains multiple zone nodes, each zone contains rooms or corridor segments, and each lower-level region references its corresponding metric map. Cross-level links describe transitions between these structures. This organization enables the planner to move between abstract route reasoning and detailed navigation without losing the relationship between the two representations.
Planning normally begins at the highest level required by the mission. If the start and goal are on different floors, the planner first identifies a valid sequence of floors and vertical transitions. Once the appropriate destination floor is selected, planning is refined through the relevant zones and rooms. If the start and goal are already within the same room or local region, higher levels may be bypassed and detailed planning can begin immediately.
This selective refinement is one of the major computational advantages of multi-level planning. A conventional global planner might search a very large number of states representing regions that are irrelevant to the current mission. Hierarchical planning restricts detailed search to the portions of the environment selected by higher-level decisions. Large facilities can therefore remain computationally manageable even when local maps contain high-resolution geometric information.
Graph-search algorithms such as Dijkstra and A* can operate independently at each level. A floor graph may use elevator travel time as an edge cost, while a zone graph may use expected traversal time or congestion. A room graph may include distance, doorway restrictions, or robot compatibility. The cost model can therefore reflect the operational meaning of each abstraction level instead of forcing all planning decisions into one uniform metric.
Cross-level cost consistency is nevertheless important. A high-level route that appears inexpensive should not repeatedly expand into extremely costly lower-level paths. High-level edge weights can therefore approximate the expected cost of their underlying detailed routes. Historical traversal time, geometric distance, congestion statistics, energy demand, or resource waiting time can be used to maintain realistic estimates and improve hierarchical route selection.
Vertical transportation illustrates the importance of semantic graph edges. An elevator connection is not simply a geometric line between two floor nodes. It may require calling the elevator, waiting for availability, reserving access, entering, confirming the robot position, selecting a destination floor, traveling vertically, and exiting safely. The graph edge can represent this complete operational transition while lower-level behaviors execute its individual steps.
Similar reasoning applies to controlled doors, airlocks, security gates, automatic shutters, and transfer stations. These elements connect navigation regions but require actions beyond ordinary path following. Multi-level graph planning can represent them as semantic transitions with preconditions, execution procedures, completion conditions, and failure states. Navigation therefore becomes coordinated with facility infrastructure rather than being treated purely as geometric movement.
Zone-level restrictions can encode operational policies directly into planning. A hazardous area may permit only certified robots, a clean zone may require specific entry procedures, and a production area may become unavailable during certain operations. Instead of allowing a local planner to discover these restrictions after approaching the region, the hierarchical graph can remove or penalize prohibited transitions before a facility-level route is selected.
Robot capabilities can also influence graph accessibility. A small AMR may traverse narrow corridors that are unavailable to a larger platform, while only certain robots may use particular elevators or ramps. Nodes and edges can therefore contain capability requirements such as footprint limits, payload restrictions, maximum slope, communication requirements, or localization modes. Route search then operates on a graph filtered according to the robot executing the mission.
Dynamic conditions can modify the hierarchy during operation. A blocked corridor may invalidate a room-level edge, congestion may increase a zone-level traversal cost, and an elevator failure may disable an inter-floor connection. Changes can propagate upward when they affect higher-level reachability or route cost. The planner can then reconsider only the necessary hierarchy levels rather than rebuilding and searching the complete facility representation.
Local failures should not automatically trigger complete facility-wide replanning. If an obstacle blocks one geometric path inside a room, the local planner may first attempt another route within that room. If the entire doorway becomes inaccessible, room-level replanning may be required. Only when the affected transition changes zone or floor connectivity does the failure need to propagate to higher levels. This containment reduces unnecessary planning computation.
Multi-level graphs are also valuable for large AMR fleets because reservations can be associated with different spatial scales. A robot may reserve an elevator at the floor level, a narrow corridor at the zone level, or a docking station at the room level. Fleet management can coordinate these shared resources without representing every interaction as occupancy of individual map cells, improving scalability for facility-wide traffic management.
Deadlock prevention can similarly exploit hierarchical structure. Two robots approaching a narrow inter-zone passage can be coordinated before both enter the constrained region. Elevator queues can be managed at transition nodes, while room entry can be controlled according to capacity. High-level resource awareness prevents conflicts that would be difficult or inefficient to resolve using only reactive local collision avoidance.
Localization must maintain correspondence between the robot\'s metric pose and the hierarchy. The navigation system should know not only the robot\'s coordinates but also its current room, zone, and floor. Crossing a doorway or entering an elevator can trigger a topological state transition. Reliable hierarchical localization enables mission management and fleet coordination to reason about robot position using operationally meaningful spatial concepts.
Map management becomes more scalable when metric maps are associated with individual regions. Instead of loading a complete high-resolution map of a large multi-floor facility, the robot can activate the maps required for its current and upcoming route segments. Floor or zone transitions can trigger map switching, localization initialization, or coordinate-frame transformation. This reduces unnecessary memory and computation while supporting geographically large installations.
Coordinate relationships between levels must be explicitly managed. Individual rooms or floors may use local coordinate frames, while facility management may require a common global reference. Transformation information connects these frames so that transition poses, elevator entrances, docking positions, and route handoffs remain consistent. Incorrect transformations can make a logically valid hierarchical route impossible to execute geometrically.
Path execution proceeds by progressively translating abstract route elements into concrete navigation goals. A high-level route may specify Floor 1 → Elevator A → Floor 3 → Zone C → Room C4. The execution manager expands each transition into metric waypoints, navigation actions, infrastructure commands, and confirmation conditions. Successful completion advances the robot through the hierarchical route until the mission destination is reached.
Recovery behavior should operate at the lowest level capable of resolving a problem. A controller tracking error should normally be handled locally, a blocked room transition may require room-level replanning, a closed zone connection may require zone-level rerouting, and an unavailable elevator may require floor-level replanning. This escalation principle avoids unnecessary global changes while still allowing the system to recover from major connectivity failures.
The hierarchy must be designed carefully because excessive abstraction can hide important constraints. If a zone is represented as a single node even though internal connectivity is complex, the planner may incorrectly assume that any entrance can reach any exit. Conversely, an excessively detailed hierarchy can eliminate computational advantages. Each level should preserve the connectivity and constraints necessary for correct decisions while hiding details that are irrelevant at that level.
Performance evaluation should therefore examine both planning efficiency and execution quality. Relevant measures include high-level search latency, detailed planning time, number of expanded graph states, route cost, transition success rate, resource waiting time, replanning frequency, map-switching latency, and end-to-end mission time. Scalability should be tested as floors, zones, rooms, robots, and dynamic restrictions increase.
Multi-level graph planning ultimately provides a structured bridge between facility-scale reasoning and local robot motion. Floors, zones, and rooms encode progressively finer spatial and semantic relationships, while metric planners handle detailed collision-free movement. By searching only the levels and regions relevant to each mission, an AMR can navigate complex multi-floor facilities efficiently while coordinating infrastructure, resources, operational restrictions, and local motion within one hierarchical planning architecture.
02.09. Graph Planning in Nav2 Planner Plugin [w/Code]
Graph planning in the Navigation2 (Nav2) framework is implemented through a modular global-planning architecture in which planner algorithms are loaded as plugins rather than embedded permanently in the navigation stack. This design allows an AMR to use different graph-search strategies while preserving a common navigation interface. The planner operates between a navigation goal and downstream path-following components that execute the generated global path.
Within a complete robot planning architecture, global graph planning belongs near the beginning of the motion-generation pipeline. A mission or navigation goal is converted into a global route, which is subsequently interpreted by behavior and local motion-planning functions before reaching the controller. This separation allows graph search to focus on strategic collision-free connectivity while lower layers address short-horizon motion, trajectory generation, and vehicle control.
Nav2 separates global planning functionality through the Planner Server. Rather than requiring one fixed planning algorithm, the server manages planner plugins that implement a standardized planning interface. A plugin receives information such as the robot start pose and requested goal pose and returns a path. This architecture makes the planner algorithm replaceable while the surrounding behavior-tree, costmap, controller, and lifecycle infrastructure can remain largely unchanged.
The global costmap provides the principal spatial representation used by many Nav2 graph planners. It discretizes the environment into cells and assigns costs representing free space, obstacles, inflated obstacle regions, unknown areas, and other navigation constraints. A graph planner can interpret traversable cells as states and feasible transitions between neighboring cells as edges, transforming costmap-based navigation into a weighted graph-search problem.
The graph representation is usually implicit rather than constructed as a separate graph containing every node and edge. Grid coordinates identify states, while neighborhood rules generate successors when a state is expanded. Four-connected or eight-connected transitions can be used depending on planner design. Edge costs can combine geometric movement distance with costmap penalties so that the planner considers both route length and proximity to undesirable regions.
A* and Dijkstra-style search provide natural foundations for this type of planning. The accumulated value g(n) represents the cost already incurred from the start, while A* additionally uses a heuristic h(n) estimating the remaining cost to the goal. Dijkstra can be interpreted as the case in which no goal-directed heuristic contributes to node ordering. Both approaches can generate globally consistent paths through the costmap when their assumptions and cost functions are properly configured.
Nav2\'s plugin architecture allows different global planners to embody different graph representations and search assumptions. A grid-based planner can emphasize conventional shortest-path search, while another plugin may use orientation-aware states, motion primitives, or vehicle-specific constraints. The common plugin boundary means that the navigation system can select a planner according to robot kinematics and operating environment without redesigning the complete Nav2 architecture.
This distinction is important because not every AMR should be modeled as a point moving freely between neighboring cells. Differential-drive robots, omnidirectional platforms, Ackermann-steered vehicles, and large industrial robots have different motion constraints. A graph planner intended for a particular vehicle should construct states and transitions that reflect its feasible motion sufficiently well, especially when turning radius, orientation, reversing, or large footprint constraints materially affect reachability.
Planner selection therefore becomes an architectural decision rather than merely an algorithmic preference. A simple grid search may be appropriate for an indoor differential-drive AMR operating in broad corridors, whereas a kinematically constrained platform may require a lattice or hybrid search representation. More generally, global planning may use A*, D*, Hybrid A*, or other graph-search approaches combined with traversability information according to the vehicle and environment.
A planner plugin must translate the start and goal poses into valid planning states. These poses may fall in free cells, inflated-cost regions, unknown areas, or invalid locations depending on costmap configuration. Robust implementations therefore validate endpoints before beginning graph search. Goal tolerances may also allow the planner to identify a nearby reachable state when the exact requested pose cannot be connected safely.
During search, the planner repeatedly selects promising states from an open structure, evaluates valid neighbors, updates accumulated costs, and records predecessor relationships. Obstacle cells and prohibited transitions are rejected, while high-cost regions can be penalized. When the goal is reached, predecessor information is traced backward to reconstruct the path, which is then expressed as a sequence of poses suitable for the rest of the Nav2 navigation pipeline.
Cost interpretation strongly influences the resulting route. If geometric distance dominates, the planner tends to seek short paths. If inflated obstacle costs contribute strongly, it can prefer routes with greater clearance even when they are longer. The cost model therefore connects graph-search mathematics to practical AMR behavior. Appropriate scaling is essential because excessively weak or strong costmap penalties can produce paths that conflict with the intended balance between efficiency and safety.
Obstacle inflation is particularly important because a raw occupancy map does not by itself represent the complete navigation requirements of a physical robot. Costmap layers can enlarge or grade the region surrounding obstacles, encouraging the graph planner to maintain clearance. The planner consequently searches not only for geometric connectivity but through a navigation cost field shaped by robot footprint, safety policy, map information, and configured costmap behavior.
The planner plugin does not operate independently of localization. Start poses are obtained from the robot\'s estimated state relative to the map, and localization errors can therefore influence graph planning. If the estimated pose is displaced into an obstacle or disconnected region, planning may fail even when the physical robot has sufficient space. Reliable localization and consistent coordinate transformations are prerequisites for meaningful global path generation.
Dynamic environmental information can modify the costmap between planning requests. Newly detected obstacles may block cells that belonged to the previous route, while disappearing obstacles can reopen alternative connections. The Planner Server can then invoke the configured planner again using the updated representation. Whether previous search information is reused depends on the particular planning algorithm and plugin implementation rather than on graph planning as a general concept.
Global replanning should be distinguished from local dynamic obstacle avoidance. A planner plugin can generate a new strategic path when persistent environmental changes invalidate the current route, but rapidly moving obstacles are normally handled by lower-level motion and control components. The broader planning architecture therefore combines global graph search with local motion planning rather than expecting one graph planner to solve every temporal collision problem.
The generated global path becomes an input to downstream navigation components. A controller or local planning function uses this reference while considering the robot\'s immediate state and local environment. The control layer ultimately converts validated trajectory information into steering, velocity, acceleration, or braking commands. Safety monitoring and collision validation remain necessary between planning and physical actuation.
The plugin boundary also supports experimentation and system evolution. Developers can implement a new graph-search method behind the planner interface and compare it with existing alternatives without replacing mission management or controller logic. This modularity is valuable for research and industrial AMRs because planner algorithms can evolve independently while maintaining stable interfaces with localization, costmaps, behavior coordination, and path execution.
Configuration is therefore an important part of planner deployment. Parameters governing planner choice, search tolerance, cost interpretation, unknown-space handling, motion model, and related behavior must match the robot and map. A theoretically efficient graph algorithm can still perform poorly if its configuration does not correspond to costmap resolution, footprint dimensions, operating clearance, or the actual mobility constraints of the platform.
Lifecycle management contributes to operational robustness. Nav2 components are designed to move through managed states so that configuration, activation, deactivation, and recovery can occur systematically. Planner plugins participate within this managed navigation environment rather than behaving as isolated algorithms. This is important in production robots where map availability, localization readiness, costmap initialization, and other dependencies must be established before autonomous planning begins.
Failure semantics should also be explicit. Planning can fail because the start or goal is invalid, no connected free-space route exists, the costmap is unavailable, the search exceeds resource limits, or relevant transformations cannot be obtained. Higher-level navigation logic should distinguish recoverable planning problems from conditions requiring mission-level intervention, allowing retries, costmap recovery, alternative goals, or safe termination to be selected appropriately.
Performance evaluation should include planning latency, expanded states, path length, accumulated path cost, obstacle clearance, memory usage, replanning frequency, and success rate. For industrial robots, worst-case and high-percentile planning times can be as important as average performance because delayed planning can affect mission timing. Different planner plugins should therefore be compared using identical maps, costmaps, robot footprints, and navigation scenarios.
Graph planning in a Nav2 planner plugin ultimately connects classical search algorithms with a modular robotic software architecture. The costmap supplies a structured navigation space, the planner plugin converts that representation into a global path, and downstream behavior, motion, control, and safety functions transform the path into physical movement. This separation enables AMRs to adopt different graph-search strategies while retaining a reusable and extensible navigation framework.
02.10. Large Scale Warehouse Graph Planning Case
Large-scale warehouse graph planning converts a facility containing thousands of storage locations, long aisles, cross-aisles, workstations, charging areas, and transfer points into a structured navigation graph. Instead of repeatedly searching every cell of a high-resolution occupancy map, the system represents strategically important locations as nodes and feasible travel corridors as edges. This abstraction makes facility-wide route planning practical for large AMR fleets.
The warehouse graph normally reflects the physical organization of the facility. Nodes can represent aisle entrances, intersections, picking stations, packing stations, buffer zones, charging points, elevators, and waiting positions. Edges represent traversable aisle segments or controlled transitions between these locations. Each edge can store distance, nominal travel time, direction, speed limit, width, robot restrictions, and other operational attributes required for planning.
A typical warehouse contains strong structural regularity, making graph representation particularly effective. Long parallel aisles are connected through a smaller number of cross-aisles, creating repeated patterns of intersections and constrained passages. Rather than treating this environment as undifferentiated free space, the navigation graph explicitly captures these structural relationships. Search can therefore concentrate on meaningful route alternatives instead of exploring large numbers of irrelevant geometric states.
One-way traffic policies are frequently introduced when robot density increases. Selected aisles can be represented using directed edges so that robots enter and leave through predefined directions. This reduces head-on encounters and simplifies traffic coordination. Directional restrictions can also vary according to robot class, operational period, or warehouse mode, allowing the graph to encode traffic rules before individual AMRs begin local navigation.
When an order requires an AMR to travel from a storage area to a picking or transfer station, the planner first associates the robot and destination with graph nodes or edges. Dijkstra, A*, or another shortest-path algorithm then searches the warehouse graph. Because the graph contains substantially fewer states than a detailed occupancy grid, long-distance route queries can be completed efficiently even when the physical facility covers a very large area.
Distance alone is rarely sufficient as the routing cost in a large warehouse. The shortest route may pass through heavily congested intersections or narrow aisles where waiting time dominates actual driving time. Edge costs can therefore combine geometric distance with estimated traversal time, congestion, queue length, speed restrictions, energy consumption, or operational risk. Planning then seeks the lowest operational cost rather than merely the shortest geometric path.
Dynamic edge costs allow the route network to respond to current warehouse conditions. When many robots occupy a particular corridor, its expected traversal cost can be increased. If congestion later decreases, the cost can return toward its nominal value. This mechanism distributes traffic across alternative routes and can prevent every AMR from independently selecting the same geometrically shortest corridor, which would otherwise create additional congestion.
Critical intersections require particular attention because they can become major throughput bottlenecks. The graph can identify these locations as shared resources and coordinate access using reservations, priorities, or time windows. Instead of allowing several robots to approach a narrow intersection simultaneously and relying entirely on reactive collision avoidance, fleet-level planning can sequence their arrival before physical conflict occurs.
Narrow aisles create an even stronger coordination problem. If two robots enter from opposite directions and cannot pass each other, local collision avoidance alone may produce a deadlock. The corresponding graph edge can therefore be modeled as a mutually exclusive resource. A robot obtains permission before entering, traverses the constrained segment, and releases the reservation after leaving, preventing incompatible traffic from occupying the aisle simultaneously.
Large fleets also require temporal reasoning because two routes that are geometrically independent at one moment may conflict later. A route can be represented not only as a sequence of graph edges but also as expected occupancy intervals over those edges. Space-time reservation mechanisms can then determine when robots may enter intersections, corridors, or stations. This extends ordinary shortest-path graph planning toward coordinated multi-robot traffic scheduling.
Route planning and task allocation are closely related at warehouse scale. Assigning the nearest robot to every task may appear efficient, but the selected robot could be located behind congestion or require access to heavily reserved corridors. Fleet management can use graph-derived travel estimates when assigning missions. Estimated route cost, resource waiting time, battery state, and task priority can therefore contribute jointly to robot-task selection.
Charging behavior also becomes part of graph planning. Charging stations are represented as destination nodes with limited capacity and potentially significant waiting times. When battery energy falls below an operational threshold, the fleet system can estimate not only the distance to available chargers but also route congestion and charger occupancy. The selected charging route can therefore minimize mission disruption rather than simply selecting the physically nearest charger.
Different robot classes may share the same warehouse while having different graph accessibility. A small AMR may travel through narrow aisles that a larger transport robot cannot enter, while heavy robots may require wider intersections or lower speed limits. Nodes and edges can store capability constraints, allowing the planner to filter the graph according to footprint, payload, turning requirements, safety classification, or other vehicle properties.
Graph planning does not replace the metric navigation system installed on each robot. Once the fleet planner selects a sequence of graph edges, the AMR still requires localization, obstacle perception, local costmaps, path generation, and motion control. The graph determines strategic movement through the warehouse, while onboard navigation determines the precise collision-free trajectory needed to traverse each selected corridor and intersection.
Temporary obstacles illustrate the relationship between these two planning levels. If a worker or another robot briefly blocks an aisle, the local navigation system may wait or maneuver around the obstruction without changing the facility-level route. If a pallet blocks the aisle for an extended period, the fleet system can mark the corresponding graph edge as unavailable or expensive and calculate a new route through another corridor.
This distinction prevents unnecessary global replanning. Short-lived local disturbances should normally remain local, whereas persistent connectivity changes should propagate to the warehouse graph. A practical system can use blockage duration, local planner failure, queue growth, or repeated traversal failures as evidence that an edge-level condition has changed. The route planner can then respond at the appropriate spatial and temporal scale.
Graph partitioning can improve scalability in exceptionally large facilities. The warehouse may be divided into zones, sectors, floors, or operational regions, with a higher-level graph connecting these partitions. Planning first determines a coarse sequence of regions and then searches detailed graphs only within relevant areas. Hierarchical planning reduces search complexity and makes it easier to distribute computation across fleet-management services.
Precomputation can further reduce planning latency. Static distances between important nodes, heuristic tables, connected components, or preferred corridor relationships can be calculated before operation. Real-time planning then combines this static structure with dynamic information such as congestion, blocked edges, reservations, and mission priorities. The planner avoids repeatedly solving unchanged geometric relationships while remaining responsive to operational conditions.
Graph updates must be carefully controlled because inconsistent topology can create unsafe or impossible routes. When warehouse layouts change, new racks, stations, gates, or restricted areas may require nodes and edges to be added, removed, or modified. Changes should be validated against the physical map and navigation system before deployment. Version management is especially important when hundreds of robots depend on the same facility graph.
Failure recovery should distinguish between robot-specific and infrastructure-level problems. If one AMR cannot traverse an edge because of localization or controller failure, the edge may still be valid for other robots. If multiple robots report the same obstruction, the fleet system has stronger evidence that the graph connection itself is unavailable. Aggregating fleet observations can therefore improve the reliability of dynamic graph-state estimation.
High-density operation requires route planning to consider global throughput rather than optimizing every robot independently. Giving each AMR its individually shortest path can overload central corridors and reduce overall productivity. A fleet-aware planner may intentionally assign slightly longer routes to some robots to balance traffic, reduce queues, and improve total mission completion rate. Local optimality and fleet-level optimality are therefore not necessarily equivalent.
Deadlock management becomes increasingly important as fleet density rises. Cyclic resource dependencies can occur when several robots reserve or occupy connected narrow segments while waiting for one another. The graph provides a natural representation for detecting such dependencies. Reservation ordering, controlled entry, route diversification, and designated waiting nodes can reduce the probability that traffic coordination enters an unrecoverable state.
Waiting locations should be designed explicitly rather than allowing robots to stop anywhere when congestion occurs. Graph nodes representing pull-over areas, queue positions, or staging zones provide safe places where robots can wait without blocking major traffic. The planner can incorporate these locations when resource access is delayed, separating stationary waiting behavior from the high-throughput corridors needed by moving robots.
Performance evaluation should focus on both individual navigation quality and warehouse productivity. Useful measures include route-planning latency, average travel time, P95 and P99 mission time, distance traveled, intersection waiting time, congestion level, deadlock frequency, replanning rate, energy consumption, and successful missions per hour. Fleet throughput and worst-case delays become increasingly important as the number of simultaneously operating robots grows.
Scalability testing should progressively increase graph size, task arrival rate, fleet population, and the number of dynamic restrictions. A planning system that performs well with ten robots may behave very differently with hundreds because congestion and resource interactions become dominant. Evaluation should therefore include realistic traffic distributions, peak operating periods, blocked corridors, charger contention, and temporary infrastructure failures.
A robust large-scale warehouse architecture ultimately combines graph search, dynamic cost estimation, resource reservation, fleet coordination, and onboard metric navigation. The warehouse graph supplies the strategic representation of movement possibilities, while real-time operational information continuously modifies how those possibilities are valued. This enables AMRs to navigate not merely through free space, but through a shared transportation network whose capacity must be managed.
The central lesson of large-scale warehouse graph planning is that route quality cannot be defined by geometric distance alone. As facility size and fleet density increase, congestion, shared resources, waiting time, robot capability, energy, and system-wide throughput become fundamental planning variables. A well-designed graph transforms warehouse geometry into an operational traffic network, providing a scalable foundation for coordinated AMR navigation.
Author: Richard Minsoo Go
LinkedIn: Richard Minsoo Go








Top comments (0)