Robot Path Planning & Navigation

Robot Path Planning & Navigation — SLAM, A*, and Motion Planning Algorithms

Every robot that moves through the world must answer three fundamental questions continuously: Where am I? What is around me? How do I get where I need to go? These questions — localization, mapping, and planning — form the core of robot path planning and navigation. The algorithms that answer them determine whether a robot navigates smoothly and efficiently or gets stuck, collides with obstacles, or takes absurdly inefficient routes. Getting navigation right is what separates a useful mobile robot from an expensive demonstration that only works in carefully controlled conditions.

At ESS ENN Associates, we implement navigation systems for robots operating in warehouses, outdoor terrain, underwater environments, and domestic spaces. This guide provides a deep technical treatment of the algorithms and architectures that make robot navigation work: SLAM using LiDAR and visual methods, graph search algorithms (A*, D*, Theta*), sampling-based planners (RRT, PRM), local planning for dynamic obstacle avoidance, costmap representation and configuration, and the architecture that connects global and local planning into a cohesive navigation system.

SLAM: Simultaneous Localization and Mapping

SLAM is the foundational capability that enables a robot to navigate in environments without pre-built maps. The algorithm simultaneously constructs a map of the environment from sensor observations and estimates the robot's position within that map. This circular dependency — needing a map to localize and needing a location to build a map — makes SLAM one of the most studied problems in robotics, with decades of algorithmic development producing solutions that work reliably in a wide range of environments.

LiDAR SLAM uses laser range measurements to build accurate geometric maps. A 2D LiDAR scanner (for ground robots) or 3D LiDAR (for more complex environments) provides distance measurements to surrounding surfaces at angular intervals. The SLAM algorithm aligns successive scans to estimate the robot's motion between scans (scan matching), detects when the robot returns to a previously visited location (loop closure), and optimizes the entire trajectory and map to minimize accumulated error (graph optimization). GMapping provides a well-tested particle filter approach for 2D LiDAR SLAM. Cartographer from Google uses a submap-based approach that scales to large environments. LOAM and its successors (LeGO-LOAM, LIO-SAM, FAST-LIO) handle 3D LiDAR SLAM with increasingly sophisticated IMU integration for improved accuracy and robustness.

Visual SLAM uses camera images instead of laser scans. Feature-based visual SLAM (ORB-SLAM3) extracts distinctive visual features (corners, edges, textures) from images, tracks them across frames to estimate camera motion, and builds a sparse 3D map of feature positions. Direct visual SLAM (LSD-SLAM, DSO) works directly with pixel intensity values rather than extracted features, producing semi-dense depth maps. Visual-Inertial SLAM combines camera data with IMU measurements, using the IMU to predict motion between camera frames and the camera to correct IMU drift. The result is more robust than either sensor alone — the IMU handles rapid motion and low-texture environments where vision struggles, while the camera prevents long-term IMU drift.

Sensor fusion for SLAM combines multiple sensor modalities to exploit their complementary strengths. A common configuration uses 3D LiDAR for accurate geometric mapping, cameras for texture-rich feature detection and object recognition, IMU for high-rate motion estimation, and wheel odometry for ground-plane motion constraints. Factor graph optimization frameworks (GTSAM, Ceres Solver) provide the mathematical foundation for fusing these heterogeneous measurements into a unified, consistent estimate of the robot's trajectory and the environment map. The factor graph represents each sensor measurement as a constraint (factor) connecting robot poses and map elements, and nonlinear optimization finds the trajectory and map that best satisfy all constraints simultaneously.

Map representations vary based on the planning algorithms that will use them. Occupancy grids divide the environment into cells classified as occupied, free, or unknown — directly usable by grid-based planners like A*. Point cloud maps preserve the full 3D geometric detail from LiDAR but require processing to extract navigable surfaces. Topological maps represent the environment as a graph of places connected by traversable paths — useful for high-level route planning in large environments. OctoMap provides an efficient 3D occupancy representation using octrees, supporting variable resolution that uses fine cells near obstacles and coarse cells in open space.

Graph Search Algorithms: A*, D*, and Variants

Graph search algorithms find optimal paths through discrete representations of the environment — typically occupancy grid maps where each cell is a node and adjacent cells are connected by edges with costs proportional to traversal difficulty. These algorithms form the backbone of global path planning for mobile robots.

A* (A-star) is the foundational algorithm for optimal graph search. Starting from the robot's current position, A* expands nodes in order of f(n) = g(n) + h(n), where g(n) is the actual cost from start to node n, and h(n) is a heuristic estimate of the remaining cost from n to the goal. With an admissible heuristic (one that never overestimates), A* guarantees finding the shortest path. The Euclidean distance heuristic is standard for grid-based robot planning. A* is computationally efficient for moderate-sized maps but becomes slow for very large environments or when replanning is needed frequently.

D* Lite extends A* to handle dynamic environments efficiently. When the robot discovers a new obstacle (the map changes), D* Lite repairs the existing path locally rather than replanning from scratch. It maintains a priority queue of nodes affected by the map change and re-expands only those nodes whose costs have changed. For robots navigating with incomplete information — discovering obstacles as they approach them — D* Lite provides significantly faster replanning than running A* from scratch each time the map updates. This makes it the preferred global planner for robots operating in partially known or changing environments.

Theta* and other any-angle planners produce smoother, shorter paths than A* by allowing paths to cross cell boundaries at arbitrary angles rather than restricting movement to grid-aligned directions. A* on an 8-connected grid produces paths with visible zigzag artifacts. Theta* adds line-of-sight checks during the search — if a straight line from the current node's parent to a neighbor is unobstructed, the path goes directly rather than through the intermediate node. The resulting paths are closer to optimal straight-line paths while maintaining the efficiency of grid-based search.

NavFn and Smac planners in the ROS 2 Nav2 stack implement these algorithms for production robot navigation. NavFn provides a navigation function planner based on Dijkstra's algorithm with A* optimization. Smac planners (2D, Hybrid-A*, and Lattice) provide more advanced planning that respects vehicle kinematics — the Hybrid-A* variant generates paths that are dynamically feasible for car-like robots with minimum turning radius constraints, while the Lattice planner uses precomputed motion primitives for arbitrary vehicle geometries.

Sampling-Based Planners: RRT, PRM, and Beyond

Sampling-based planners handle high-dimensional planning problems where grid-based methods are computationally infeasible. A robot arm with 6 joints operates in a 6-dimensional configuration space — discretizing this space into a grid with even modest resolution creates billions of cells. Sampling-based planners avoid this curse of dimensionality by exploring the configuration space through random sampling rather than exhaustive discretization.

RRT (Rapidly-exploring Random Tree) grows a tree of collision-free configurations by repeatedly sampling a random configuration, finding the nearest node in the existing tree, and extending the tree toward the sample (if the extension is collision-free). The tree rapidly explores the configuration space, tending to grow into unexplored regions. RRT-Connect grows two trees — one from start and one from goal — and attempts to connect them, typically finding solutions faster than single-tree RRT. The paths produced by basic RRT are suboptimal and jagged, requiring post-processing (shortcutting, smoothing) to produce practically usable trajectories.

RRT* adds asymptotic optimality to RRT. As the algorithm samples more configurations, it rewires the tree to reduce path costs, converging toward the optimal path given enough time. The rewiring operation checks whether newly sampled nodes provide cheaper connections for existing tree nodes and updates the tree accordingly. RRT* with informed sampling (Informed RRT*) focuses sampling in the region that could improve the current best solution, dramatically improving convergence speed in practice. For production use, RRT* with a time budget (stop after N milliseconds and return the best path found) provides a good balance between solution quality and planning speed.

PRM (Probabilistic Roadmap) is a multi-query planner suited for environments where many planning queries will be made in the same static map. The roadmap is built once by sampling collision-free configurations and connecting nearby samples with collision-free edges. Subsequent planning queries find paths on the roadmap using graph search. PRM amortizes the expensive map-building phase across many queries, making each individual query very fast. For robot arms that perform repetitive tasks in a fixed workspace, PRM provides near-instant path planning after the initial roadmap construction.

Local Planning and Dynamic Obstacle Avoidance

Global planners compute paths assuming the map is complete and static. Reality disagrees — people walk through the robot's path, doors close, objects get moved. Local planners provide the reactive layer that handles these discrepancies, generating short-horizon trajectories that follow the global path while avoiding real-time obstacles and respecting the robot's kinematic constraints.

Dynamic Window Approach (DWA) operates in the robot's velocity space. Given the robot's current velocity and acceleration limits, DWA defines a window of achievable velocities for the next time step. Within this window, it simulates trajectories for a set of candidate velocities, scores each trajectory based on proximity to obstacles, progress toward the goal, and adherence to the global path, and selects the highest-scoring trajectory. DWA runs at 10 to 20 Hz, producing smooth velocity commands that naturally account for the robot's dynamic capabilities. The DWB (Dynamic Window B) controller in Nav2 extends DWA with pluggable scoring functions for customized navigation behavior.

Timed Elastic Band (TEB) represents the local path as a sequence of timed poses connected by elastic constraints. Optimization adjusts the poses to minimize a multi-objective cost function that includes obstacle clearance, kinematic feasibility, time optimality, and deviation from the global path. TEB produces smoother trajectories than DWA, particularly around tight obstacles and in narrow passages. It can also plan multiple distinct homotopy classes (different ways to go around an obstacle) and select the best one, avoiding the local minima that trap single-path planners. The Regulated Pure Pursuit controller provides a simpler alternative for robots following well-defined paths where aggressive obstacle avoidance is less critical.

Model Predictive Path Integral (MPPI) is a modern local planner that uses sampling-based optimization in a model predictive control framework. MPPI generates thousands of random trajectory perturbations, evaluates each against a cost function using the robot's dynamics model, and computes a weighted average of the best trajectories to produce the control command. GPU acceleration enables MPPI to evaluate thousands of trajectories in real time, providing robust obstacle avoidance even in complex, dynamic environments. MPPI is available in Nav2 and is gaining adoption for applications requiring aggressive, high-performance navigation.

Costmaps: The Bridge Between Perception and Planning

The costmap is the data structure that connects perception (what the sensors see) to planning (where the robot can go). It translates raw sensor data into a planning-ready representation that encodes obstacle locations, safety margins, terrain preferences, and navigation rules in a format that path planning algorithms consume directly.

Layered costmap architecture in Nav2 composes the final costmap from multiple independent layers. The static layer loads the pre-built map (from SLAM or provided maps). The obstacle layer integrates real-time sensor data — LiDAR points, depth camera readings, sonar measurements — marking detected obstacles in the costmap. The inflation layer adds cost gradients around obstacles, creating a buffer zone that keeps the robot at a safe distance. The footprint of the robot determines the inflation radius — larger robots need wider inflation. Custom layers can encode application-specific costs: speed limit zones, preferred traffic lanes, restricted areas, or terrain difficulty derived from elevation maps.

Costmap configuration is one of the most impactful tuning exercises in robot navigation. The obstacle detection parameters (sensor range, obstacle height thresholds, clearing behavior) determine what appears in the costmap. The inflation radius and cost scaling function control how aggressively the planner avoids obstacles — too aggressive and the robot cannot navigate narrow spaces, too timid and the robot clips obstacles during turns. The update frequency must balance between responsiveness (detecting new obstacles quickly) and computational load (processing sensor data and updating the costmap consumes CPU cycles). For each robot and environment combination, these parameters must be tuned through systematic testing to achieve reliable navigation.

3D costmaps and voxel layers extend the costmap concept to three dimensions for robots that need to navigate under overhanging obstacles, through multi-level spaces, or in environments where obstacles exist at heights above or below the robot's primary sensing plane. Voxel grid layers maintain a three-dimensional occupancy representation and project it into the 2D costmap used by planners, ensuring that overhanging obstacles and low obstacles both appear in the planning representation. The computational cost of 3D costmaps is significantly higher than 2D, requiring careful configuration of voxel resolution and update regions to maintain real-time performance.

Navigation Architecture: Putting It All Together

A complete robot navigation system combines SLAM, global planning, local planning, costmaps, and behavior management into an integrated architecture. The ROS 2 Nav2 stack provides the reference architecture that most production mobile robot systems use or adapt.

The navigation lifecycle begins with SLAM building the initial map (or loading a pre-built map). The localization module (AMCL for 2D, or SLAM running in localization mode) estimates the robot's position within the map. When a navigation goal is received, the global planner computes a path on the global costmap. The local planner follows this path while avoiding obstacles detected in the local costmap. The behavior tree (BT) managing the navigation task handles recovery behaviors when the robot gets stuck — clearing the costmap and replanning, spinning in place to reacquire localization, or backing up from tight spaces.

Recovery behaviors are essential for robust production navigation. Even well-tuned navigation systems encounter situations where the robot cannot make progress — dead ends that were not in the map, obstacles that the sensors cannot detect, or localization failures that place the robot in an impossible position on the map. The behavior tree defines a sequence of escalating recovery actions: wait briefly (the obstacle might move), clear the costmap (sensor artifacts might be blocking the path), replan globally (find an alternative route), spin in place (improve localization), and finally report failure to the operator. Without effective recovery behaviors, robots in production environments require constant human supervision to unstick them — defeating the purpose of automation.

Navigation performance tuning requires balancing competing objectives. Faster travel reduces task completion time but increases collision risk. Wider obstacle margins improve safety but make the robot unable to navigate narrow passages. More frequent replanning improves responsiveness but consumes computational resources needed for other tasks. The tuning process involves systematic testing across representative scenarios — open corridors, narrow doorways, crowded spaces, dynamic obstacles — measuring both navigation success rate and efficiency. Simulation testing (Gazebo, Isaac Sim) enables rapid iteration over parameter combinations before deploying to physical hardware.

Frequently Asked Questions

For navigation applied to specific domains, see our warehouse robotics automation guide, land drone software development guide, and underwater robotics guide. For testing navigation systems in simulation, explore our robot testing and simulation QA guide.

At ESS ENN Associates, our robotics navigation team implements SLAM, path planning, and obstacle avoidance systems that work reliably in production environments. Whether you need indoor, outdoor, or underwater navigation, contact us for a free technical consultation.