V
vishwa vimukthi gammuduwaththage
Guest
I build navigation software for drones that can't rely on GPS. Whenever I explain that work, I end up explaining the same thing first: where path planning sits in an autonomous system, and why it's harder than "find the shortest route." This is that explanation, written once, properly.
The examples lean on self-driving cars because that's where the literature is deepest. Everything here carries over to drones, ground robots, and boats, with a few differences I'll cover near the end.
Autonomous driving is an old idea. Experiments with radio-controlled "driverless" cars date back to the 1920s. What changed in the last decade is that sensors became cheap, compute moved to the edge, and deep learning made perception actually work.
The safety argument is the one you hear most. You'll often see the claim that around 94% of crashes are caused by human error. That number comes from NHTSA, and NHTSA itself warns that it describes the last failure in the chain before a crash, not blame. The direction is still right: most crashes involve a human misjudgement somewhere, and a machine that never gets tired or distracted has room to do better. "Room to do better" is the honest version.
Most autonomous driving systems are described as six layers:
The planner sits at layer five, which means it inherits every mistake made above it. A good planner is therefore as much about handling uncertain inputs as it is about clever search. So before getting to planning, here's what it's being fed.
Radar and LiDAR share a basic idea: emit a signal and measure what comes back. FMCW radar sweeps the frequency of radio waves and computes range from the frequency difference between the transmitted and returned signal. LiDAR uses infrared laser light, either scanned mechanically or, increasingly, with solid-state and flash designs that have no spinning parts.
No sensor is good at everything, which is why production vehicles fuse several. That fusion is also the first place uncertainty enters the planner's world.
Perception converts sensor data into things a planner can reason about:
The planner needs to know where the vehicle is to within centimetres. Three approaches dominate:
This layer is personal for me. My company builds a vision-only navigation SDK for drones in GPS-denied environments, and the core lesson from that work is simple: a planner is only as good as the pose estimate it's given. If localisation is off by a metre, a "collision-free" path isn't.
The assessment layer asks two questions: how dangerous is the current scene, and what are the other agents about to do?
That covers detecting unsafe lane changes, predicting cut-ins, and classifying driving styles (an aggressive driver behind you behaves differently from a cautious one). Older systems used clustering methods such as PCA and k-means, or SVMs and small neural networks. Modern stacks increasingly use learned trajectory-prediction models that output several possible futures for each agent, with probabilities.
Those multiple futures are the planner's most important input, and its biggest headache.
Here's the core of the article. "Path planning" is really three nested problems running at different speeds.
Which roads take me from A to B? This runs on a road graph built from an offline map, recalculates rarely, and is basically solved. Dijkstra's algorithm and A* handle it, with tricks such as contraction hierarchies making continent-scale routing fast.
One correction to a claim I made in the original version of this article: finding the shortest path on a graph is not NP-hard. Dijkstra solves it in polynomial time. What's genuinely hard is general motion planning in continuous space, which has been known since the late 1970s to be PSPACE-hard. That's where the difficulty lives, in the next two layers.
Given the route and the scene, what should the vehicle do? Keep the lane, change lanes, yield, overtake, stop, creep forward at an occluded junction. This used to be done with finite state machines and rule trees. It's increasingly learned or handled by search over candidate manoeuvres, because rules explode in number once you hit real traffic.
Given the chosen behaviour, what exact trajectory should the vehicle follow for the next few seconds? It must be:
This is where most of the engineering effort goes.
Graph search on a discretised world. Chop the space into a grid or lattice and search it. A* is the workhorse: it explores nodes in order of cost so far + estimated cost to go, and with an admissible heuristic it's guaranteed to find the optimal path. Here's a minimal version on a 4-connected grid:
python
Grid A* ignores vehicle dynamics, so cars use Hybrid A*, which searches over continuous position and heading using motion primitives a real vehicle can drive. It was made famous by Stanford's entry in the 2007 DARPA Urban Challenge and is still common in parking and low-speed manoeuvring.
Sampling-based planners. Instead of discretising everything, randomly sample the space and connect feasible samples. RRT grows a tree quickly toward unexplored space; RRT* adds rewiring so the path converges toward optimal as samples increase; PRM builds a reusable roadmap. These shine in high-dimensional or cluttered spaces, which is why they're popular for drones and robot arms. The trade-off is jittery paths that need smoothing.
Lattice and Frenét-frame planners. On structured roads, express motion relative to the lane centreline (along-road distance and lateral offset), generate a fan of candidate trajectories, score each one for safety, comfort, and progress, and pick the best. Simple, fast, interpretable, and widely used in highway driving.
Optimisation-based planning. Treat the trajectory as an optimisation problem: minimise a cost (deviation, jerk, time) subject to constraints (dynamics, collision avoidance). Model Predictive Control (MPC) solves this repeatedly over a short receding horizon, so it naturally blurs the line between planning and control. The catch is that collision constraints are non-convex, so solvers need good initial guesses, often supplied by one of the methods above.
Learned planners. Train a network to output trajectories directly, usually by imitating expert driving and increasingly refined with reinforcement learning. Since 2024, the most active line of research has been diffusion-based planners, which generate trajectories through iterative denoising. They're appealing because driving is multimodal: at a junction, "go left" and "go right" are both valid, and regression-based models tend to average the two into "go straight into the wall." DiffusionDrive (CVPR 2025) and Diffusion Planner (ICLR 2025) are good entry points.
In practice, production systems mix these: search or sampling for a coarse answer, optimisation to make it smooth and feasible, and learned components to predict others and score options.
The biggest change since I first wrote about this topic in mid-2025 is how far the industry moved toward end-to-end systems, where perception, prediction, and planning are trained together, from sensor input to trajectory output.
Tesla replaced most of its hand-written planning code with neural networks in FSD v12. Waymo published EMMA in 2024, a research model built on Gemini that maps camera input directly to planner trajectories. Then in late 2025 Waymo described its production driver as a foundation model trained end-to-end. Waymo's framing is the useful one: end-to-end is a training strategy, not an architecture. You can keep distinct modules and still propagate gradients through all of them.
So the six-layer diagram above isn't wrong. It just describes the roles in the system, not necessarily separate pieces of code anymore.
Most of my own work is in the air, so a few differences worth knowing:
When I reverse-engineered a cheap RC drone to make it autonomous <!-- LINK: RC drone reverse-engineering article -->, the planner was the easy part to write and the hard part to trust. Every planning failure I hit traced back to state estimation, not the search algorithm.
It would be easy to end with "end-to-end won, learn everything." The evidence is more mixed than that.
The steelman for modular, rule-based planning: In the 2023 nuPlan planning challenge, a team showed that a simple rule-based planner (PDM-Closed, essentially lane-following with a carefully tuned speed and safety model) matched or beat much more complex learned planners in closed-loop evaluation. Separately, researchers showed that common open-loop benchmarks on nuScenes could be "solved" surprisingly well using little more than the vehicle's own past motion. Learned planners often look better on paper than on the road. Modular systems also have something end-to-end systems struggle with: you can inspect each stage, test it in isolation, and prove certain safety properties. Regulators and safety engineers care about that.
The steelman for end-to-end: Hand-written rules don't scale to the long tail. Every edge case adds rules, and the rules start conflicting. A learned system improves with data, and the companies with the most driving data have bet heavily on it. Diffusion planners and RL fine-tuning are also directly attacking the weaknesses that rule-based baselines exposed.
Where I land: hybrid. Use learning where the world is too complex to write down, such as prediction, scoring, and generating candidate trajectories. Keep a verifiable layer underneath that can veto unsafe plans: collision checks, kinematic limits, hard safety rules. For safety-critical vehicles, and especially for anything airborne, I want a planner that can explain why it didn't crash.
Path planning is the moment an autonomous system commits to an action. Everything upstream, including sensors, perception, localisation, and prediction, exists to make that commitment a good one. The algorithms are well understood. The open problems are about uncertainty, interaction with other agents, and trust.
If you're starting out, implement A* on a grid, then Hybrid A*, then RRT* in a simulator. You'll learn more about why planning is hard from watching your own planner fail than from any survey, including this one.
The examples lean on self-driving cars because that's where the literature is deepest. Everything here carries over to drones, ground robots, and boats, with a few differences I'll cover near the end.
Why autonomy, and why now
Autonomous driving is an old idea. Experiments with radio-controlled "driverless" cars date back to the 1920s. What changed in the last decade is that sensors became cheap, compute moved to the edge, and deep learning made perception actually work.
The safety argument is the one you hear most. You'll often see the claim that around 94% of crashes are caused by human error. That number comes from NHTSA, and NHTSA itself warns that it describes the last failure in the chain before a crash, not blame. The direction is still right: most crashes involve a human misjudgement somewhere, and a machine that never gets tired or distracted has room to do better. "Room to do better" is the honest version.
The stack in sixty seconds
Most autonomous driving systems are described as six layers:
- Sensors / hardware: cameras, LiDAR, radar, IMU, GNSS collect raw data.
- Perception: turns raw data into objects, lanes, and free space.
- Localisation & mapping: works out where the vehicle is in the world.
- Risk assessment: estimates danger and predicts what other road users will do.
- Path planning & decision-making: chooses where to go and how.
- Control: turns the plan into steering, throttle, and braking.
The planner sits at layer five, which means it inherits every mistake made above it. A good planner is therefore as much about handling uncertain inputs as it is about clever search. So before getting to planning, here's what it's being fed.
What the planner sees: sensors
| Sensor | What it gives you | Main weakness |
|---|---|---|
| Monocular camera | Colour, texture, semantics; cheap | No direct depth; struggles at night and in harsh shadows |
| Stereo camera | Depth from two cameras a known baseline apart | Fails on low-texture surfaces; accuracy drops with distance |
| Structured light / ToF camera | Depth via projected IR pattern or light round-trip time | Short range; sunlight interference outdoors |
| Event camera | Per-pixel brightness changes, asynchronously; microsecond latency, ~120 dB dynamic range | Unusual data format; tooling still maturing |
| Omnidirectional camera | 360° view from fisheye arrays | Needs careful multi-camera calibration |
| Radar (FMCW) | Range and relative velocity, robust in rain and fog | Low angular resolution |
| LiDAR | Dense 3D point clouds from laser pulses | Cost; degraded by heavy rain, fog, dust |
Radar and LiDAR share a basic idea: emit a signal and measure what comes back. FMCW radar sweeps the frequency of radio waves and computes range from the frequency difference between the transmitted and returned signal. LiDAR uses infrared laser light, either scanned mechanically or, increasingly, with solid-state and flash designs that have no spinning parts.
No sensor is good at everything, which is why production vehicles fuse several. That fusion is also the first place uncertainty enters the planner's world.
What the planner sees: perception
Perception converts sensor data into things a planner can reason about:
- 2D object detection finds cars, pedestrians, signs, and lights in images and draws bounding boxes around them, usually with CNN- or transformer-based detectors.
- Segmentation comes in two flavours. Semantic segmentation labels every pixel with a class ("road", "car"). Instance segmentation goes further and separates individual objects, giving each car its own pixel mask.
- 3D object detection adds depth from stereo, LiDAR, or radar. Grouping sparse 3D points into clean objects is still hard.
- Tracking follows moving objects over time to estimate their velocity and heading. This is what lets the system predict where things will be, not just where they are. Fusing multiple sensors here, often into an occupancy grid, is standard.
- Road and lane detection is its own problem because lanes are continuous surfaces, not objects. The usual pipeline is: preprocess the raw data, extract lane features, fit continuous curves (splines and similar models), then smooth the result over time using the vehicle's motion.
What the planner sees: localisation
The planner needs to know where the vehicle is to within centimetres. Three approaches dominate:
- GNSS + IMU fusion. The IMU (gyroscope, accelerometer, magnetometer) tracks motion but drifts over time. GNSS corrects the drift but is only accurate to a few metres without augmentation, and it vanishes in tunnels and urban canyons. Good as an initial estimate, not enough on its own.
- SLAM (Simultaneous Localisation and Mapping) builds a map and localises within it at the same time. Visual SLAM uses cameras and is cheap and rich in landmarks but sensitive to lighting and texture. LiDAR SLAM produces accurate point-cloud maps at higher cost. I've written separately about building a VSLAM pipeline
- Prior-map localisation matches live sensor data against a pre-built high-definition map, either by point-cloud matching or by recognising known landmarks such as signs and road markings.
This layer is personal for me. My company builds a vision-only navigation SDK for drones in GPS-denied environments, and the core lesson from that work is simple: a planner is only as good as the pose estimate it's given. If localisation is off by a metre, a "collision-free" path isn't.
What the planner sees: risk and intent
The assessment layer asks two questions: how dangerous is the current scene, and what are the other agents about to do?
That covers detecting unsafe lane changes, predicting cut-ins, and classifying driving styles (an aggressive driver behind you behaves differently from a cautious one). Older systems used clustering methods such as PCA and k-means, or SVMs and small neural networks. Modern stacks increasingly use learned trajectory-prediction models that output several possible futures for each agent, with probabilities.
Those multiple futures are the planner's most important input, and its biggest headache.
Path planning, properly
Here's the core of the article. "Path planning" is really three nested problems running at different speeds.
1. Global (route) planning
Which roads take me from A to B? This runs on a road graph built from an offline map, recalculates rarely, and is basically solved. Dijkstra's algorithm and A* handle it, with tricks such as contraction hierarchies making continent-scale routing fast.
One correction to a claim I made in the original version of this article: finding the shortest path on a graph is not NP-hard. Dijkstra solves it in polynomial time. What's genuinely hard is general motion planning in continuous space, which has been known since the late 1970s to be PSPACE-hard. That's where the difficulty lives, in the next two layers.
2. Behavioural planning
Given the route and the scene, what should the vehicle do? Keep the lane, change lanes, yield, overtake, stop, creep forward at an occluded junction. This used to be done with finite state machines and rule trees. It's increasingly learned or handled by search over candidate manoeuvres, because rules explode in number once you hit real traffic.
3. Local (motion) planning
Given the chosen behaviour, what exact trajectory should the vehicle follow for the next few seconds? It must be:
- collision-free, against obstacles that move and react;
- dynamically feasible, meaning a car can't turn on the spot, and curvature and acceleration have limits;
- comfortable, with low jerk and no wobbling;
- fast, typically replanned every 50–100 ms.
This is where most of the engineering effort goes.
The algorithm families
Graph search on a discretised world. Chop the space into a grid or lattice and search it. A* is the workhorse: it explores nodes in order of cost so far + estimated cost to go, and with an admissible heuristic it's guaranteed to find the optimal path. Here's a minimal version on a 4-connected grid:
python
Code:
import heapq
def astar(grid, start, goal):
"""4-connected grid A*. grid[r][c] == 1 means blocked."""
def h(p): # Manhattan distance: admissible on a 4-connected grid
return abs(p[0] - goal[0]) + abs(p[1] - goal[1])
open_heap = [(h(start), 0, start)]
came_from = {start: None}
g = {start: 0}
while open_heap:
_, cost, node = heapq.heappop(open_heap)
if node == goal:
path = []
while node is not None:
path.append(node)
node = came_from[node]
return path[::-1]
if cost > g[node]:
continue # stale heap entry
r, c = node
for nr, nc in ((r + 1, c), (r - 1, c), (r, c + 1), (r, c - 1)):
if 0 <= nr < len(grid) and 0 <= nc < len(grid[0]) and grid[nr][nc] == 0:
new_cost = cost + 1
if new_cost < g.get((nr, nc), float("inf")):
g[(nr, nc)] = new_cost
came_from[(nr, nc)] = node
heapq.heappush(open_heap, (new_cost + h((nr, nc)), new_cost, (nr, nc)))
return NoneGrid A* ignores vehicle dynamics, so cars use Hybrid A*, which searches over continuous position and heading using motion primitives a real vehicle can drive. It was made famous by Stanford's entry in the 2007 DARPA Urban Challenge and is still common in parking and low-speed manoeuvring.
Sampling-based planners. Instead of discretising everything, randomly sample the space and connect feasible samples. RRT grows a tree quickly toward unexplored space; RRT* adds rewiring so the path converges toward optimal as samples increase; PRM builds a reusable roadmap. These shine in high-dimensional or cluttered spaces, which is why they're popular for drones and robot arms. The trade-off is jittery paths that need smoothing.
Lattice and Frenét-frame planners. On structured roads, express motion relative to the lane centreline (along-road distance and lateral offset), generate a fan of candidate trajectories, score each one for safety, comfort, and progress, and pick the best. Simple, fast, interpretable, and widely used in highway driving.
Optimisation-based planning. Treat the trajectory as an optimisation problem: minimise a cost (deviation, jerk, time) subject to constraints (dynamics, collision avoidance). Model Predictive Control (MPC) solves this repeatedly over a short receding horizon, so it naturally blurs the line between planning and control. The catch is that collision constraints are non-convex, so solvers need good initial guesses, often supplied by one of the methods above.
Learned planners. Train a network to output trajectories directly, usually by imitating expert driving and increasingly refined with reinforcement learning. Since 2024, the most active line of research has been diffusion-based planners, which generate trajectories through iterative denoising. They're appealing because driving is multimodal: at a junction, "go left" and "go right" are both valid, and regression-based models tend to average the two into "go straight into the wall." DiffusionDrive (CVPR 2025) and Diffusion Planner (ICLR 2025) are good entry points.
In practice, production systems mix these: search or sampling for a coarse answer, optimisation to make it smooth and feasible, and learned components to predict others and score options.
The 2025–26 shift: end-to-end
The biggest change since I first wrote about this topic in mid-2025 is how far the industry moved toward end-to-end systems, where perception, prediction, and planning are trained together, from sensor input to trajectory output.
Tesla replaced most of its hand-written planning code with neural networks in FSD v12. Waymo published EMMA in 2024, a research model built on Gemini that maps camera input directly to planner trajectories. Then in late 2025 Waymo described its production driver as a foundation model trained end-to-end. Waymo's framing is the useful one: end-to-end is a training strategy, not an architecture. You can keep distinct modules and still propagate gradients through all of them.
So the six-layer diagram above isn't wrong. It just describes the roles in the system, not necessarily separate pieces of code anymore.
Drones and other vehicles: what changes
Most of my own work is in the air, so a few differences worth knowing:
- The space is 3D, and often unstructured, with no lanes and no road graph. Sampling planners (RRT*) and polynomial trajectory optimisation (minimum-snap) are more common than lattice planners.
- Dynamics are fast and unforgiving. A quadrotor can't stop instantly, and a planning delay of 100 ms at speed is a real distance.
- Compute and weight budgets are tight. You can't carry a car's GPU rack on a 250 g drone, so efficiency matters more than benchmark scores.
- Localisation is fragile. GNSS jamming and spoofing are now a routine problem in contested airspace. That's exactly why we build vision-only navigation at FromSky.
When I reverse-engineered a cheap RC drone to make it autonomous <!-- LINK: RC drone reverse-engineering article -->, the planner was the easy part to write and the hard part to trust. Every planning failure I hit traced back to state estimation, not the search algorithm.
The case against everything I just said
It would be easy to end with "end-to-end won, learn everything." The evidence is more mixed than that.
The steelman for modular, rule-based planning: In the 2023 nuPlan planning challenge, a team showed that a simple rule-based planner (PDM-Closed, essentially lane-following with a carefully tuned speed and safety model) matched or beat much more complex learned planners in closed-loop evaluation. Separately, researchers showed that common open-loop benchmarks on nuScenes could be "solved" surprisingly well using little more than the vehicle's own past motion. Learned planners often look better on paper than on the road. Modular systems also have something end-to-end systems struggle with: you can inspect each stage, test it in isolation, and prove certain safety properties. Regulators and safety engineers care about that.
The steelman for end-to-end: Hand-written rules don't scale to the long tail. Every edge case adds rules, and the rules start conflicting. A learned system improves with data, and the companies with the most driving data have bet heavily on it. Diffusion planners and RL fine-tuning are also directly attacking the weaknesses that rule-based baselines exposed.
Where I land: hybrid. Use learning where the world is too complex to write down, such as prediction, scoring, and generating candidate trajectories. Keep a verifiable layer underneath that can veto unsafe plans: collision checks, kinematic limits, hard safety rules. For safety-critical vehicles, and especially for anything airborne, I want a planner that can explain why it didn't crash.
Closing
Path planning is the moment an autonomous system commits to an action. Everything upstream, including sensors, perception, localisation, and prediction, exists to make that commitment a good one. The algorithms are well understood. The open problems are about uncertainty, interaction with other agents, and trust.
If you're starting out, implement A* on a grid, then Hybrid A*, then RRT* in a simulator. You'll learn more about why planning is hard from watching your own planner fail than from any survey, including this one.