{"id":1877,"date":"2024-10-15T11:33:20","date_gmt":"2024-10-15T11:33:20","guid":{"rendered":"https:\/\/algocademy.com\/blog\/algorithms-in-robotics-pathfinding-and-navigation\/"},"modified":"2026-10-15T10:34:49","modified_gmt":"2026-10-15T10:34:49","slug":"algorithms-in-robotics-pathfinding-and-navigation","status":"publish","type":"post","link":"https:\/\/algocademy.com\/blog\/algorithms-in-robotics-pathfinding-and-navigation\/","title":{"rendered":"Algorithms in Robotics: Pathfinding and Navigation Explained"},"content":{"rendered":"<p>A mobile robot&#8217;s navigation software splits into a few layers: a map and a position estimate (localization or SLAM), a <strong>global planner<\/strong> that finds a route on the map (A*, Dijkstra, RRT), and a <strong>local planner or controller<\/strong> that follows the route while dodging obstacles it sees right now. Most robots you can read about use grid-based A* globally and a fast sampling or optimization controller locally. This post shows how those pieces work, with Python that was run, and where the usual mistakes are.<\/p>\n<h2>The layers of robot navigation<\/h2>\n<ol>\n<li><strong>Perception and mapping.<\/strong> Sensors (lidar, cameras, wheel odometry, an IMU) are turned into a map, often an occupancy grid where each cell is free, occupied or unknown.<\/li>\n<li><strong>Localization.<\/strong> Where am I on that map? Methods include particle filters, Kalman filters and scan matching.<\/li>\n<li><strong>Global planning.<\/strong> Given the map, start and goal, find a collision-free path.<\/li>\n<li><strong>Local planning and control.<\/strong> Turn the path into velocity commands each fraction of a second, reacting to sensor data and to the robot&#8217;s own limits on speed, acceleration and turning.<\/li>\n<li><strong>Recovery.<\/strong> If the robot is stuck, back up, spin, clear the local map, or replan.<\/li>\n<\/ol>\n<p>Separating the layers matters. The global planner can be slow and thorough because it runs rarely; the controller must be fast and runs many times per second.<\/p>\n<h2>A* on a grid<\/h2>\n<p>A* is Dijkstra&#8217;s algorithm with a guide. It expands the node with the lowest <code>f = g + h<\/code>, where <code>g<\/code> is the cost so far and <code>h<\/code> is a heuristic estimate of the cost remaining. If <code>h<\/code> never overestimates (it is <em>admissible<\/em>), the first time you reach the goal the path is optimal.<\/p>\n<p>The heuristic must match the moves. If the robot can move diagonally, Manhattan distance overestimates: from (0,0) to (5,5) it says 10, but eight diagonal steps cost only 5 &times; 1.414 = 7.07. Use the octile distance, which accounts for diagonals. The code below also forbids cutting the corner of an obstacle, since a real robot has width:<\/p>\n<pre><code class=\"language-python\">import heapq, math\n\nMOVES = [(-1,0,1), (1,0,1), (0,-1,1), (0,1,1),\n         (-1,-1,math.sqrt(2)), (-1,1,math.sqrt(2)),\n         (1,-1,math.sqrt(2)),  (1,1,math.sqrt(2))]\n\ndef octile(a, b):\n    dx, dy = abs(a[0]-b[0]), abs(a[1]-b[1])\n    return (dx + dy) + (math.sqrt(2) - 2) * min(dx, dy)\n\ndef free(grid, r, c):\n    return 0 &lt;= r &lt; len(grid) and 0 &lt;= c &lt; len(grid[0]) and grid[r][c] == 0\n\ndef a_star(grid, start, goal, h=octile):\n    open_heap = [(h(start, goal), 0.0, start)]\n    g = {start: 0.0}\n    parent, closed = {}, set()\n    while open_heap:\n        f, gc, cur = heapq.heappop(open_heap)\n        if cur in closed:\n            continue                      # stale heap entry\n        if cur == goal:\n            path = [cur]\n            while cur in parent:\n                cur = parent[cur]\n                path.append(cur)\n            return path[::-1], gc\n        closed.add(cur)\n        for dr, dc, cost in MOVES:\n            nxt = (cur[0] + dr, cur[1] + dc)\n            if not free(grid, *nxt):\n                continue\n            if dr and dc and not (free(grid, cur[0] + dr, cur[1]) and\n                                  free(grid, cur[0], cur[1] + dc)):\n                continue                  # don't cut corners\n            ng = gc + cost\n            if ng &lt; g.get(nxt, float(\"inf\")):\n                g[nxt] = ng\n                parent[nxt] = cur\n                heapq.heappush(open_heap, (ng + h(nxt, goal), ng, nxt))\n    return None, float(\"inf\")\n\ngrid = [\n [0,0,0,0,0,0,0],\n [0,0,0,1,0,0,0],\n [0,0,0,1,0,0,0],\n [0,0,0,1,0,0,0],\n [0,0,0,0,0,0,0],\n [0,0,0,0,0,0,0],\n]\npath, cost = a_star(grid, (0, 0), (3, 6))\nprint(round(cost, 3))   # 7.828\n# path: (0,0) (0,1) (0,2) (0,3) (0,4) (1,4) (2,5) (3,6)\n# *****..\n# ...#*..\n# ...#.*.\n# ...#..*<\/code><\/pre>\n<p>Details that cause real bugs: use a heap with &#8220;lazy deletion&#8221; (skip entries already closed) instead of searching the open list, which the common copy-pasted version does in O(n) per step; and store a node&#8217;s best <code>g<\/code> so you only push a node when you find a cheaper route to it.<\/p>\n<p>How much does the heuristic help? On an empty 50&times;50 grid, going from (0,0) to (49,30), A* with the octile heuristic expanded 233 nodes. The same search with <code>h = 0<\/code>, which is Dijkstra, expanded 2,376. On cluttered maps the gap is smaller, and with a poor heuristic it can vanish, but the heuristic never makes the answer worse as long as it is admissible.<\/p>\n<h3>Dijkstra and BFS<\/h3>\n<p>Dijkstra is A* with <code>h = 0<\/code>. Use it when you need distances to <em>all<\/em> cells (for example, a cost field that every robot position can follow downhill), or when there is no good heuristic. Breadth-first search is Dijkstra when every step costs the same, and it is the right tool for unweighted problems. If you want practice on exactly these, AlgoCademy has <a href=\"https:\/\/algocademy.com\/link\/?lang=py&amp;problem=bfs-min-distance-to-every-vertex&amp;solution=1\">BFS Min Distance to Every Vertex<\/a> and <a href=\"https:\/\/algocademy.com\/link\/?lang=py&amp;problem=dijkstra&amp;solution=1\">Dijkstra&#8217;s Algorithm<\/a>; our post on <a href=\"https:\/\/algocademy.com\/blog\/a-search-algorithm-the-pathfinding-powerhouse-in-computer-science\/\">the A* search algorithm<\/a> covers the algorithm itself in more depth.<\/p>\n<h2>Why grid paths are not enough<\/h2>\n<p>Grid A* gives paths that follow 8 directions, which look jagged and ignore the robot&#8217;s shape. Four common fixes:<\/p>\n<ul>\n<li><strong>Inflate obstacles<\/strong> by the robot&#8217;s radius in the cost map, so the robot can be treated as a point.<\/li>\n<li><strong>Add a cost gradient near obstacles<\/strong> so paths keep a safety margin instead of hugging walls.<\/li>\n<li><strong>Smooth the path<\/strong>, or use any-angle planners such as Theta* that check line of sight between cells.<\/li>\n<li><strong>Plan in the robot&#8217;s state space<\/strong> (x, y, heading) for car-like robots that cannot turn on the spot. Hybrid-A* does this using motion primitives.<\/li>\n<\/ul>\n<h2>Sampling-based planning: RRT<\/h2>\n<p>Grids get too large when the robot has many degrees of freedom. A robot arm with 6 joints lives in a 6-dimensional space; a grid with 100 cells per axis would have 10<sup>12<\/sup> cells. Sampling-based planners such as RRT (Rapidly-exploring Random Tree) avoid building that grid. They grow a tree from the start: sample a random point, find the nearest tree node, take a small step toward the sample, and keep the new node only if the segment is collision-free. Sampling the goal itself occasionally (a goal bias) speeds up convergence.<\/p>\n<pre><code class=\"language-python\">import math, random\n\ncircles = [(0.5, 0.5, 0.2), (0.25, 0.75, 0.1)]     # obstacles: (x, y, radius)\n\ndef collides(p, q):\n    for cx, cy, r in circles:\n        for i in range(11):                         # check points along the segment\n            t = i \/ 10\n            x = p[0] + (q[0] - p[0]) * t\n            y = p[1] + (q[1] - p[1]) * t\n            if (x - cx) ** 2 + (y - cy) ** 2 &lt;= r * r:\n                return True\n    return False\n\ndef rrt(start, goal, step=0.05, iters=5000, goal_bias=0.1):\n    nodes, parent = [start], {start: None}\n    for i in range(iters):\n        s = goal if random.random() &lt; goal_bias else (random.random(), random.random())\n        near = min(nodes, key=lambda n: (n[0]-s[0])**2 + (n[1]-s[1])**2)\n        d = math.dist(near, s)\n        if d == 0:\n            continue\n        k = min(step, d) \/ d\n        new = (near[0] + (s[0]-near[0]) * k, near[1] + (s[1]-near[1]) * k)\n        if collides(near, new):\n            continue\n        nodes.append(new)\n        parent[new] = near\n        if math.dist(new, goal) &lt; step:\n            path, n = [goal], new\n            while n is not None:\n                path.append(n)\n                n = parent[n]\n            return path[::-1]\n    return None\n\nrandom.seed(3)\npath = rrt((0.05, 0.05), (0.95, 0.95))\nprint(path is not None, len(path))   # True 38<\/code><\/pre>\n<p>With this seed it found a route in 129 iterations using a 38-node path. The length was 1.8 against a straight-line distance of 1.27, which shows the main property of plain RRT: it finds <em>a<\/em> path quickly, but the path is wobbly and not optimal. RRT* rewires the tree to improve paths as sampling continues, and a smoothing pass usually follows either one. Sampling-based planners are also only probabilistically complete: if no path exists, they never say so, so always cap the iterations.<\/p>\n<h2>Local planning and obstacle avoidance<\/h2>\n<p>The global path is only a guide. Once the robot moves, the controller samples candidate velocity commands, simulates each one forward for a short time, scores them (progress along the path, distance to obstacles, smoothness, speed), and picks the best. The Dynamic Window Approach (DWA) is the classic form; model-predictive controllers such as MPPI (model predictive path integral control) are a newer, optimization-based alternative. Potential fields are simpler still, pulling the robot toward the goal and pushing it away from obstacles, but they can trap it in local minima such as a U-shaped wall.<\/p>\n<h2>SLAM: map and position at the same time<\/h2>\n<p>If the robot has no map, it has to build one while localizing in it, which is the SLAM problem (simultaneous localization and mapping). The difficulty is that errors compound: a small wrong turn estimate makes every later map update wrong. Practical systems use filters or graph optimization, and rely on <em>loop closure<\/em>: recognizing a place already visited and correcting the accumulated drift. Writing a real SLAM system is a big project. Start by learning the occupancy grid and the idea of updating it from sensor readings, then use an existing implementation.<\/p>\n<h2>How this appears in real stacks<\/h2>\n<p>You rarely write these from scratch on a real robot. The ROS 2 navigation stack, Nav2, is built from plugins in exactly the layers above. As of October 2026 its documentation lists planners including NavFn (a grid planner that uses A* or Dijkstra expansion and assumes a point-sized, holonomic robot), the Smac planner family (2D grid A*, Hybrid-A* for car-like robots, and a state lattice planner) and Theta*, plus controllers including DWB (a modified Dynamic Window Approach) and MPPI. See the <a href=\"https:\/\/docs.nav2.org\/setup_guides\/algorithm\/select_algorithm.html\">Nav2 plugin selection guide<\/a> for the current list and recommendations, which change between releases.<\/p>\n<h2>Common mistakes<\/h2>\n<ul>\n<li>Using a heuristic that overestimates the true cost, which silently returns non-optimal paths.<\/li>\n<li>Planning for a point robot, then driving a robot with width into narrow gaps. Inflate obstacles.<\/li>\n<li>Ignoring the robot&#8217;s dynamics: a path made of sharp corners may be impossible to follow.<\/li>\n<li>Replanning from scratch every cycle on a large map. Plan globally at a low rate and track locally at a high rate.<\/li>\n<li>Trusting a stale map in dynamic environments. Keep a separate, short-lived local cost map.<\/li>\n<\/ul>\n<h2>Where to go from here<\/h2>\n<p>Implement grid A* and BFS with different move sets and check the path costs by hand, as above. Then add weighted cells (terrain costs), then try RRT with your own obstacles. If you want guided practice with graph search before moving to robotics code, start with the graph lessons on <a href=\"https:\/\/app.algocademy.com\/\">AlgoCademy<\/a>, then look at ROS 2 and Nav2 tutorials to see the same ideas inside a real framework.<\/p>\n","protected":false},"excerpt":{"rendered":"<p>How robots find paths and navigate: grid A*, Dijkstra, RRT, local planners, and SLAM, with tested Python code and notes on how ROS 2 Nav2 uses them.<\/p>\n","protected":false},"author":1,"featured_media":1876,"comment_status":"closed","ping_status":"open","sticky":false,"template":"","format":"standard","meta":{"footnotes":""},"categories":[23],"tags":[],"class_list":["post-1877","post","type-post","status-publish","format-standard","has-post-thumbnail","hentry","category-problem-solving"],"_links":{"self":[{"href":"https:\/\/algocademy.com\/blog\/wp-json\/wp\/v2\/posts\/1877"}],"collection":[{"href":"https:\/\/algocademy.com\/blog\/wp-json\/wp\/v2\/posts"}],"about":[{"href":"https:\/\/algocademy.com\/blog\/wp-json\/wp\/v2\/types\/post"}],"author":[{"embeddable":true,"href":"https:\/\/algocademy.com\/blog\/wp-json\/wp\/v2\/users\/1"}],"replies":[{"embeddable":true,"href":"https:\/\/algocademy.com\/blog\/wp-json\/wp\/v2\/comments?post=1877"}],"version-history":[{"count":1,"href":"https:\/\/algocademy.com\/blog\/wp-json\/wp\/v2\/posts\/1877\/revisions"}],"predecessor-version":[{"id":8691,"href":"https:\/\/algocademy.com\/blog\/wp-json\/wp\/v2\/posts\/1877\/revisions\/8691"}],"wp:featuredmedia":[{"embeddable":true,"href":"https:\/\/algocademy.com\/blog\/wp-json\/wp\/v2\/media\/1876"}],"wp:attachment":[{"href":"https:\/\/algocademy.com\/blog\/wp-json\/wp\/v2\/media?parent=1877"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"https:\/\/algocademy.com\/blog\/wp-json\/wp\/v2\/categories?post=1877"},{"taxonomy":"post_tag","embeddable":true,"href":"https:\/\/algocademy.com\/blog\/wp-json\/wp\/v2\/tags?post=1877"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}