A robot may know:

where it is,
where the goal is,
where the obstacles are,

and still have one enormous problem left:

How do I get from here to there safely, efficiently, and physically?

That is the problem of motion planning and navigation.

A useful mental model is:

Perception tells the robot what the world looks like.
Localization tells the robot where it is.
Planning decides where it should go.
Control makes it actually go there.

If you permanently remember that pipeline, most of robotics navigation becomes easier to organize.

1. The Big Picture

Imagine a delivery robot standing in a hospital corridor.

Its current position is:

START
  |
  v
+-----------------------+
|                       |
|       WALL            |
|   ###########         |
|                       |
|                 GOAL  |
+-----------------------+

The robot cannot simply command:

drive_to(goal)

It must solve several problems.

First:

Is there a collision-free route?

Then:

Which collision-free route is best?

Then:

Can the robot physically follow that route?

Then:

What happens if someone suddenly walks in front of it?

And finally:

What motor commands should we send right now?

This gives us the basic hierarchy:

World / Map
    ↓
Configuration Space
    ↓
Collision Checking
    ↓
Global Planning
    ↓
Path
    ↓
Path Smoothing
    ↓
Trajectory Generation
    ↓
Velocity Planning
    ↓
Local Planning / Obstacle Avoidance
    ↓
Controller
    ↓
Motors

Navigation is not one algorithm.

It is a stack of algorithms cooperating at different time scales.

2. Path vs Trajectory — Never Confuse These

This distinction is extremely important.

A path tells you:

Where should the robot go?

For example:

(x1, y1)
(x2, y2)
(x3, y3)
...
(xgoal, ygoal)

There is no concept of time.

A trajectory tells you:

Where should the robot be, and when?

For example:

t = 0.0 s → (x1, y1)
t = 0.5 s → (x2, y2)
t = 1.0 s → (x3, y3)
...

It may also contain:

position
velocity
acceleration
orientation
angular velocity

So:

Path = geometry.

Trajectory = geometry + time + dynamics.

A path may be perfectly collision-free but physically impossible to follow.

For example:

------+
      |
      |

A path containing a perfect 90° corner might be acceptable geometrically.

But a car moving at 80 km/h cannot instantly change direction by 90°.

The actual robot needs something like:

------)
       )

with appropriate slowing before the turn.

That is where trajectory generation and velocity planning enter.

3. Configuration Space

Configuration space, usually written C-space, is one of the most important ideas in robotics.

The key question is:

What information completely describes the robot's pose?

For a point robot moving on a flat floor:

q = (x, y)

For a mobile robot that can also rotate:

q = (x, y, θ)

For a 6-joint robot arm:

q = (θ1, θ2, θ3, θ4, θ5, θ6)

Each complete configuration corresponds to one point in configuration space.

The powerful trick

Suppose your robot has width.

Checking whether the entire body collides with every wall becomes inconvenient.

Instead, transform the problem.

Imagine enlarging every obstacle by the robot's radius.

Then shrink the robot conceptually into a point.

Original world:

Robot O

        wall
       ######
   O   ######
       ######

Configuration-space world:

Robot becomes:

.

Obstacle becomes larger:

     ##########
     ##########
  .  ##########
     ##########
     ##########

Now collision checking becomes much simpler:

Is the robot's configuration point inside an obstacle region?

This process is often called configuration-space obstacle inflation.

The obstacle region is commonly written:

C_obstacle

and the collision-free region:

C_free

Planning becomes:

Find a continuous path through C_free from the start configuration to the goal configuration.

That single sentence captures much of classical motion planning.

4. Why Configuration Space Gets Hard

For a mobile robot:

q = (x, y, θ)

only three variables exist.

But suppose a humanoid robot has 30 joints.

Its configuration might be:

q = (q1, q2, q3, ... q30)

That is a 30-dimensional configuration space.

You cannot draw it.

You cannot simply divide it into millions of grid cells.

This is one reason algorithms such as:

PRM
RRT
RRT*
trajectory optimization

become important.

They avoid exhaustively searching enormous spaces.

5. Collision Checking

Almost every motion planner repeatedly asks:

Is this configuration safe?

or:

Is this movement between two configurations safe?

That is collision checking.

For a mobile robot, collision checking might mean:

Does the robot footprint overlap an occupied map cell?

For a robot arm:

Does any arm link collide with the environment?

Does one robot link collide with another robot link?

The second problem is called self-collision checking.

6. Collision Checking Along a Path

Suppose a planner proposes:

A ---------------- B

Checking only A and B is not enough.

A and B could both be safe while an obstacle lies between them.

A ------- █ ------- B

Therefore planners usually perform collision checking at intermediate configurations.

Conceptually:

for q along segment(A, B):
    if collision(q):
        reject segment

Collision checking can become one of the most computationally expensive parts of motion planning.

That matters especially for:

robot manipulators,
humanoids,
complex meshes,
sampling-based planners.

A planner might generate thousands or millions of candidate motions, and every candidate may require collision checking.

7. From Geometry to Graph Search

Suppose the floor is represented as a grid:

S . . # .
. # . # .
. # . . .
. . . # G

Where:

S = start
G = goal
# = obstacle
. = free cell

We can reinterpret this world as a graph.

Each free cell becomes a node.

Possible movements become edges.

For example:

A -- B -- C
     |
     D

Motion planning can now become:

Find a path through a graph from the start node to the goal node.

This is where classical graph-search algorithms enter.

8. BFS — Breadth-First Search

BFS explores the graph layer by layer.

Imagine dropping a stone into water.

Waves expand outward:

BFS asks:

What can I reach in 1 step?

What can I reach in 2 steps?

What can I reach in 3 steps?

If every edge has the same cost, BFS finds the shortest path in number of edges.

BFS intuition

Imagine navigating a maze where every movement costs exactly one unit.

BFS systematically explores all places one step away before considering places two steps away.

So when it reaches the goal, no shorter route could have existed.

BFS strength

Simple and optimal for unweighted graphs.

BFS weakness

It explores huge amounts of irrelevant space.

If the goal is northeast, BFS still explores:

north
south
east
west
northwest
southwest
...

It has no idea which direction looks promising.

9. DFS — Depth-First Search

DFS behaves differently.

Instead of expanding everywhere, it picks one path and keeps going.

Start
  |
  A
  |
  B
  |
  C
  |
  D

If it hits a dead end, it backtracks.

Think:

Walk down one maze corridor until you cannot continue. Then return and try another corridor.

DFS is useful for:

graph traversal,
connectivity,
recursive search,
certain exploration problems.

But for robot shortest-path planning, DFS is usually not what we want.

Why?

Because DFS may discover:

S → enormous detour → G

before discovering:

S → G

It does not naturally produce the shortest path.

10. BFS vs DFS

The easiest permanent memory:

BFS explores outward.

DFS explores deep.

BFS:

Start
 ├─ A
 ├─ B
 └─ C

then children of A/B/C

DFS:

Start
 └─ A
     └─ D
         └─ H

For navigation:

BFS matters historically and conceptually.

DFS is fundamental computer science but usually not a practical shortest-path robot planner.

11. Dijkstra's Algorithm

Now imagine different terrain costs.

normal floor     cost = 1
carpet           cost = 3
mud              cost = 8
stairs           impossible

The shortest path in distance might not be the cheapest path.

Example:

Route A:
10 meters through mud

Route B:
15 meters through clean floor

If mud is expensive, Route B may be preferable.

This is where Dijkstra's algorithm becomes important.

Dijkstra finds:

The minimum-total-cost path from the start to every reachable node.

Instead of asking:

How many edges have I crossed?

it asks:

What total cost have I accumulated?

We usually call that cost:

g(n)

Meaning:

Actual cost from the start to node n.

Dijkstra repeatedly expands the currently known node with the smallest g(n).

12. Why Dijkstra Is Better Than BFS

BFS assumes:

every edge cost = 1

Dijkstra allows:

edge 1 = 1
edge 2 = 5
edge 3 = 0.7
edge 4 = 10

So Dijkstra handles weighted worlds.

In robotics, weights can represent much more than physical distance.

For example:

distance
energy consumption
terrain difficulty
collision risk
distance from obstacles
slope
turning cost

A robot may intentionally take a slightly longer route because it is safer.

This is the beginning of a crucial engineering idea:

Planning means minimizing a cost function, not necessarily minimizing distance.

13. The Main Problem with Dijkstra

Suppose the goal is obviously east.

Dijkstra still expands outward in every direction because it does not know where the goal is.

Conceptually:

        ↑
      ↖ ↑ ↗
    ← ← S → → → G
      ↙ ↓ ↘
        ↓

A huge amount of work can be wasted.

What if we could tell the planner:

The goal is over there. Prefer searching in that direction.

That produces one of the most famous algorithms in robotics and computer science.

14. A* Search

A* combines:

cost already paid
+
estimated cost remaining

Its famous equation is:

f(n) = g(n) + h(n)

where:

g(n) = known cost from start → n

h(n) = estimated cost from n → goal

f(n) = estimated total path cost through n

Think of driving.

Suppose two possible roads have been explored.

Road A:

already traveled: 4 km
estimated remaining: 20 km
total estimate: 24 km

Road B:

already traveled: 7 km
estimated remaining: 5 km
total estimate: 12 km

A* prefers B.

15. What Is a Heuristic?

The h(n) in A* is called a heuristic.

A heuristic is simply:

An intelligent estimate of how far the goal still is.

On a 2D map, one common heuristic is straight-line distance:

h = sqrt((xgoal - x)^2 + (ygoal - y)^2)

Why?

Because regardless of the walls, you cannot reach the goal using less distance than the straight-line distance.

Another common heuristic on a four-connected grid is Manhattan distance:

h = |xgoal - x| + |ygoal - y|

16. Why A* Can Be Much Faster

Dijkstra behaves roughly like:

Search everywhere until goal encountered.

A* behaves more like:

Search everywhere that looks promising toward the goal.

Visually:

Dijkstra:

        .......
      ...........
    .......G.......
      ...........
        ..S....


A*:

        ...
       ..G
      ...
     ..
    S

This is simplified, but the intuition is right.

17. A* and Optimality

A* can still find the true optimal path if the heuristic obeys certain conditions.

The most important one to remember is:

Do not overestimate the true remaining cost.

Such a heuristic is called admissible.

For example, straight-line distance is usually admissible when actual movement cannot be shorter than a straight line.

Why does overestimation matter?

Because a wildly optimistic or pessimistic heuristic can cause A* to ignore the route that is actually optimal.

Another useful property is consistency, which essentially ensures heuristic estimates behave sensibly from node to neighboring node.

You do not need to memorize the formal inequality first.

Remember:

Good heuristic = informative enough to guide search, but trustworthy enough not to trick it.

18. Heuristic Search

A* belongs to the broader category called heuristic search.

The idea is simple:

Use knowledge about the problem to avoid searching blindly.

Without a heuristic:

"Try everything."

With a heuristic:

"Try promising things first."

This pattern appears throughout AI.

The heuristic might consider:

distance to goal,
orientation,
terrain,
obstacle density,
expected energy,
expected travel time.

A more aggressive heuristic can produce much faster planning but may sacrifice optimality.

For real robots, that can be a perfectly acceptable engineering tradeoff.

Because:

A theoretically optimal path that takes 20 seconds to compute may be worse than a 2% longer path computed in 20 milliseconds.

19. D*

Suppose A* finds a path:

S ---------------- G

Then halfway through driving, the robot discovers:

S -------- █ ------ G

Maybe:

construction appeared,
a door closed,
a map was wrong,
debris blocked the road.

One solution is:

Run A* completely again.

That works.

But much of the previous search may still be valid.

Why throw everything away?

This motivates D*.

20. D* Intuition

D* belongs to a family of incremental replanning algorithms.

Instead of solving the entire planning problem from scratch after a map change, it tries to reuse previous search information.

Think of Google Maps.

You already calculated a route.

Then a road closes.

You would prefer:

repair affected part of route

rather than:

forget the whole world
recalculate everything from zero

That is roughly the intuition behind D*.

Variants include:

D*
Focused D*
D* Lite

D Lite* became particularly popular because it provides similar incremental replanning behavior with a cleaner algorithmic formulation.

The important engineering idea is not memorizing every D* equation.

Remember:

A is excellent for planning on a known graph.*

D-style algorithms are excellent when graph costs change while the robot is navigating.*

21. Grid Search Has a Problem

Algorithms like:

BFS,
Dijkstra,
A*,
D*

work beautifully on graphs.

But imagine a 7-joint robot arm.

Its configuration is:

q = (q1, q2, q3, q4, q5, q6, q7)

Suppose we discretize each joint into only 100 positions.

Possible combinations:

100^7

That is:

100,000,000,000,000

configurations.

Searching them exhaustively is absurd.

This is called the curse of dimensionality.

We need another strategy.

Instead of representing the entire space...

What if we sample only a small portion of it?

This leads to sampling-based motion planning.

22. Sampling-Based Planning

Instead of building every possible configuration, randomly sample configurations from C_free.

For example:

      .       .
  .       .
       █████
 .     █████       .
       █████
    .          .
           .

Those dots are candidate robot configurations.

The planner connects useful samples and tries to discover a route between the start and goal.

This becomes extremely powerful in high-dimensional spaces.

Sampling-based planning is especially common for:

robotic arms,
manipulators,
drones,
humanoids,
complex geometric planning.

Two algorithms dominate the introductory picture:

PRM

and

RRT

23. PRM — Probabilistic Roadmap

PRM means Probabilistic Roadmap.

Imagine you want to understand all the possible highways through a complicated environment.

Step 1:

Randomly sample collision-free configurations.

.      .        .
    █████
.   █████    .
    █████
       .       .
  .          .

Step 2:

Connect nearby samples when a collision-free movement exists.

.------.        .
 \            /
  \  █████   /
.    █████--.
     █████
        \------.

Now you have a roadmap.

Step 3:

When a start and goal arrive, connect them to the roadmap and search the graph.

24. PRM's Superpower

The roadmap can be reused.

Imagine a robot arm in the same factory every day.

The environment barely changes.

It receives thousands of queries:

Move from A → B

Move from C → D

Move from E → F

PRM can spend significant time building a useful roadmap once.

Then many future queries become fast graph searches.

This makes PRM a multi-query planner.

Mental shortcut:

PRM builds a reusable road network.

25. RRT — Rapidly-exploring Random Tree

RRT takes a different approach.

Instead of building a reusable roadmap everywhere, it starts from one position and grows a tree.

Suppose:

S = start
G = goal

Start with:

Randomly sample some point:

x_rand

Find the existing tree node nearest to it:

x_near

Move a small step from x_near toward x_rand.

If the movement is collision-free, add the new node.

Repeat.

The structure might grow like:

Eventually one branch reaches near the goal.

26. Why RRT Is Clever

RRT naturally prefers unexplored regions.

Why?

Large unexplored areas are more likely to contain random samples.

Therefore branches quickly stretch into empty space.

That is why it is called:

Rapidly-exploring Random Tree.

RRT can be extremely useful in complicated configuration spaces.

For example, getting a robotic arm through a narrow geometric arrangement can be difficult for grid-based planning but tractable with sampling methods.

27. RRT Does Not Usually Produce Beautiful Paths

A basic RRT path may look like:

It can be:

jagged,
unnecessarily long,
awkward,
far from optimal.

RRT's original objective is largely:

Find a feasible path.

Not necessarily:

Find the best possible path.

This brings us to RRT*.

28. RRT*

RRT* extends RRT with an important idea:

Whenever we add a new node, check whether nearby parts of the tree can be connected more cheaply.

This process is called rewiring.

Suppose RRT initially builds:

Later it discovers a better connection:

S -------- B
            \
             C

The tree gets improved.

Again and again.

Over time, RRT* tends toward the optimal path.

Formally, it is asymptotically optimal.

Meaning:

As the number of samples approaches infinity, its solution approaches the optimal solution.

Memory:

RRT = quickly find a way.

RRT = find a way and keep improving it.*

29. PRM vs RRT vs RRT*

A useful engineering intuition:

Algorithm	Mental Model	Best Fit
PRM	Build reusable roads	Many planning queries in mostly static environment
RRT	Grow a tree toward unexplored space	Quickly finding feasible motion
RRT*	RRT + repeatedly improve connections	Higher-quality / near-optimal paths

None is universally best.

Robot motion planning is about selecting algorithms based on the structure of the problem.

30. Graph Search vs Sampling-Based Planning

This distinction is worth permanently remembering.

Graph/grid planning

You already have a structured search space.

grid → graph → search

Typical algorithms:

BFS
Dijkstra
A*
D*

Excellent for things like:

mobile robot navigation on 2D maps

Sampling-based planning

The state space is too large to enumerate.

continuous/high-dimensional space
        ↓
random samples
        ↓
connect samples
        ↓
search

Typical algorithms:

PRM
RRT
RRT*

Excellent for things like:

robot arms
high-dimensional motion planning

31. A Path Is Still Not Enough

Suppose A* produces:

****************
               *
               *
               ********

The robot would need instantaneous turns.

Or RRT produces:

*---*
     \
      *
       \
        *---*

The path is valid geometrically.

But real robots have:

mass,
inertia,
maximum velocity,
maximum acceleration,
turning radius,
actuator limits.

So planning must eventually respect robot dynamics.

There are several ways to address this.

One major family is trajectory optimization.

32. Trajectory Optimization

Trajectory optimization starts with the question:

Instead of merely finding a valid path, can I directly optimize the entire movement?

Suppose the trajectory is represented by:

q0, q1, q2, ... qN

We define a cost.

For example:

total_cost =
    path_length
  + obstacle_cost
  + smoothness_cost
  + acceleration_cost

Then optimize the trajectory to reduce total cost.

Conceptually:

ugly trajectory
       ↓
optimization
       ↓
shorter, smoother, safer trajectory

33. Cost Functions Are Everywhere

Modern planning becomes easier to understand once you start seeing everything as a cost function.

Maybe we define:

J =
    w1 × distance
  + w2 × collision_risk
  + w3 × curvature
  + w4 × energy
  + w5 × time

The w values specify priorities.

For example:

A warehouse robot may care strongly about:

collision safety

and somewhat about:

travel time

A racing robot might assign much greater importance to:

time

Planning becomes:

Find the motion that minimizes J, while respecting constraints.

34. Hard Constraints vs Soft Costs

Suppose a wall exists.

We could treat collision as a hard constraint:

collision = forbidden

But distance from the wall can be a soft cost.

10 cm from wall → huge cost
50 cm from wall → medium cost
2 m from wall → small cost

This creates natural robot behavior.

Instead of:

"Anything not colliding is equally acceptable."

we get:

"Stay comfortably away from obstacles when possible."

That is a much better navigation philosophy.

35. Trajectory Optimization Algorithms

You may encounter names such as:

CHOMP
STOMP
TrajOpt
MPC-based optimization

You do not need to initially memorize their internals.

Their shared philosophy is more important:

Represent motion as something optimizable and continuously improve it according to a cost function and constraints.

Optimization-based planning is especially useful when we care about:

smoothness,
dynamics,
energy,
control feasibility,
complex constraints.

36. Global Planner vs Local Planner

This is one of the most important navigation-stack concepts.

A robot usually uses two levels of planning.

Global Planner

The global planner asks:

How should I travel from my current general location to the destination?

It uses the larger map.

For example:

Start
  |
  | hallway
  |
  +------+
         |
         | corridor
         |
         Goal

Typical global algorithms include:

A*
Dijkstra
D*
Hybrid A*

Global planners usually reason over relatively large distances.

Local Planner

The local planner asks:

Given where I am right now and what I currently see around me, what should I do during the next few seconds?

Suppose the global planner says:

Go straight down corridor.

Then suddenly:

      person
        O
        |
--------|---------- global path
      robot

The local planner might temporarily steer around the person.

The global route remains broadly correct.

37. The Ship Analogy

Imagine sailing across the ocean.

The global planner says:

Sail from Nepal—if Nepal had an ocean—to Japan using this general route.

The local planner says:

There is a small boat 100 meters ahead. Turn slightly right.

Global planning handles:

strategy

Local planning handles:

immediate execution

Robots need both.

38. Why Not Recalculate the Entire Global Path Every Millisecond?

Because the global map might contain millions of cells.

Running expensive global search at 20–100 Hz would be wasteful.

Instead:

Global planner:
    slower
    large world
    long horizon

Local planner:
    fast
    nearby world
    short horizon

This division is an important systems-engineering pattern.

39. Obstacle Avoidance

Obstacle avoidance answers:

How should I move without hitting nearby obstacles?

There are two important categories.

Static obstacles

Examples:

walls
tables
shelves
pillars

They are often represented in the map.

Dynamic obstacles

Examples:

humans
cars
other robots
animals

These can move unpredictably.

Dynamic obstacle avoidance is significantly harder because the robot must reason about the future, not merely present geometry.

40. Reactive Obstacle Avoidance

The simplest philosophy is:

Obstacle left → turn right

Obstacle right → turn left

Obstacle ahead → slow/stop

This can work for simple robots.

But purely reactive systems can behave badly.

For example:

robot gets trapped inside U-shaped obstacle

because locally every action seems reasonable while globally the robot has no plan.

This is why robust navigation combines:

global reasoning
+
local reaction

41. Dynamic Window Approach

A classic local navigation technique is DWA — Dynamic Window Approach.

Instead of directly searching positions, DWA considers possible velocity commands.

For example:

linear velocity v
angular velocity ω

Try candidate commands:

(v=0.3, ω=0.0)
(v=0.3, ω=0.2)
(v=0.3, ω=-0.2)
(v=0.1, ω=0.6)
...

Predict where each would take the robot over a short time horizon.

Score them based on things like:

goal progress
distance from obstacles
speed
alignment with path

Then choose the best safe command.

This produces an important mental transition:

Global planners often search where to go.

Local planners often search what motion command to execute next.

42. Cost Maps

A navigation system needs a mathematical representation of:

How desirable is each location?

This is often a cost map.

Imagine a grid:

0   0   1   5   X
0   0   2   8   X
0   0   1   5   X

Where:

0 = very safe
1 = low cost
5 = uncomfortable
8 = very close to obstacle
X = occupied / impossible

The planner prefers low-cost areas.

43. Obstacle Inflation

Suppose a wall occupies this region:

########

If we only mark those exact cells as dangerous, the planner may generate:

robot path
********
########

technically avoiding collision by millimeters.

Real robots should not scrape against walls.

So we inflate cost around obstacles:

....555555....
...5888885...
...58####85...
...58####85...
...5888885...
....555555....

The closer to the obstacle:

higher cost

This encourages the planner to keep clearance.

44. Robot Footprint Matters

A common beginner mistake is thinking of a robot as a point.

Suppose a robot is 70 cm wide.

A 50 cm opening may appear free in the map.

But the robot cannot pass through it.

Therefore navigation uses a robot footprint.

Examples:

circle
rectangle
polygon

Collision checking and cost-map inflation depend heavily on robot size.

A configuration safe for a tiny robot may be impossible for a large robot.

45. Global Cost Map and Local Cost Map

Navigation systems commonly maintain two cost maps.

Global cost map

Covers a large region.

Based primarily on:

known map
static obstacles
large-scale navigation

Used mainly for global planning.

Local cost map

Covers the region around the robot.

Often continuously updated from sensors such as:

LiDAR
depth camera
stereo camera
radar

Used for:

local planning
dynamic obstacle handling
immediate collision avoidance

Think:

Global cost map = what I know about the world.

Local cost map = what is happening around me right now.

46. Path Smoothing

Graph planners often produce paths like:

___
   |
   |____
        |
        |____

Why?

Because grid movement is discrete.

The robot would prefer:

____
    \
     \
      \____

Path smoothing removes unnecessary corners and produces easier motion.

A simple smoothing method might ask:

Can I directly connect waypoint 1 to waypoint 5 without collision?

If yes:

delete waypoints 2, 3, 4

More sophisticated smoothers optimize:

curvature
clearance
path length
smoothness

47. Why Smoothing Is Not Cosmetic

A smoother path:

reduces steering changes,
reduces acceleration changes,
decreases actuator stress,
may use less energy,
can be faster,
is easier for controllers to track,
creates more natural robot motion.

For autonomous vehicles, curvature is especially important.

A car cannot instantaneously rotate like a differential-drive robot.

48. Kinematic Constraints

A robot's geometry limits how it can move.

Consider a car.

It cannot normally move:

sideways

A planner that generates:

← car →

sideways motion has produced a mathematically valid path but a physically impossible one.

This is a kinematic constraint.

Different robot types have different constraints.

A differential-drive robot can approximately:

move forward/backward
rotate

A car-like robot has:

minimum turning radius
steering limits

A drone has much more freedom in 3D.

A robotic arm has joint limits.

A good planner must respect the actual robot.

49. Kinodynamic Planning

Sometimes we must reason about both:

kinematics
+
dynamics

Dynamics includes quantities such as:

mass
force
torque
velocity
acceleration
momentum

This produces kinodynamic planning.

Instead of asking:

Can the robot geometrically move here?

we ask:

Can the robot physically execute this movement given its dynamics?

This distinction becomes critical for:

drones,
autonomous cars,
legged robots,
high-speed robots.

50. Trajectory Generation

Suppose the final geometric path is:

P0 → P1 → P2 → P3

Trajectory generation converts that into something executable over time.

For example:

t=0.0 → P0
t=1.2 → P1
t=2.8 → P2
t=4.0 → P3

But we also want smooth velocity.

Bad trajectory:

0 m/s
instantly 3 m/s
instantly -1 m/s

Impossible.

Good trajectory:

accelerate
cruise
decelerate
stop

51. Velocity Planning

Velocity planning asks:

How fast should I travel along the path?

This sounds simple until you consider:

maximum motor speed
acceleration limits
braking distance
turning curvature
obstacles
humans
road conditions
stability
energy

Imagine:

straight road → 2.0 m/s

sharp corner → 0.4 m/s

person nearby → 0.2 m/s

goal approaching → decelerate

The geometric path may remain identical.

Only the velocity profile changes.

52. Velocity Profile

A simple motion might look like:

velocity
   ^
   |       ________
   |      /        \
   |     /          \
   |____/            \____
   +----------------------→ time

Three phases:

accelerate
cruise
decelerate

A smoother profile may also constrain jerk.

Jerk is:

rate of change of acceleration

Why care?

Because suddenly changing acceleration makes robot motion feel violent.

Elevators are a familiar example.

A good elevator controller minimizes unpleasant jerk.

53. Planner vs Controller

Another distinction to permanently remember:

Planner decides what motion should happen.

Controller tries to make the physical robot perform it.

Planner:

follow this trajectory

Controller:

motor left = ...
motor right = ...
steering = ...
torque = ...

Controllers may include:

PID
Pure Pursuit
Stanley controller
MPC
LQR

Planning and control strongly interact, but they are different problems.

54. Local Planner vs Controller

These are also often confused.

Suppose an obstacle appears.

The local planner may decide:

temporarily steer around obstacle

The controller then determines the commands required to follow that selected local trajectory.

So:

Global Planner
      ↓
Global Path
      ↓
Local Planner
      ↓
Local Trajectory
      ↓
Controller
      ↓
Actuators

That hierarchy is worth memorizing.

A navigation stack integrates everything required to autonomously reach a destination.

Conceptually:

                  ┌───────────────┐
                  │     Goal      │
                  └───────┬───────┘
                          ↓
                  ┌───────────────┐
                  │ Global Planner│
                  └───────┬───────┘
                          ↓
                     Global Path
                          ↓
Sensors → Costmaps → Local Planner
                          ↓
                   Local Trajectory
                          ↓
                     Controller
                          ↓
                       Motors
                          ↓
                        Robot
                          ↓
                  Odometry/Sensors
                          └──── feedback

But this stack depends on several other systems.

A real navigation stack usually needs:

Localization
Mapping
TF / coordinate transforms
Sensor processing
Global planner
Local planner
Costmaps
Controller
Recovery behaviors
Goal management

Suppose the planner calculates:

from (2, 3) → (20, 17)

But the robot does not know whether it is currently at:

(2, 3)

or:

(7, 11)

The path is useless.

Therefore:

Navigation without localization is impossible.

Localization technologies may include:

wheel odometry
IMU
GPS
LiDAR localization
visual localization
AMCL
SLAM
sensor fusion

A robot may simultaneously reason in several coordinate systems.

For example in ROS:

map
 ↓
odom
 ↓
base_link
 ↓
laser

An obstacle measured by the LiDAR may initially be represented relative to:

laser

But the global planner needs to understand its location in:

map

Coordinate transforms connect these representations.

This is why broken TF transforms can make an otherwise correct navigation system completely fail.

Planning is only meaningful when everyone agrees on:

Where is everything?

Real navigation frequently fails temporarily.

Examples:

path blocked
robot stuck
local planner oscillating
localization uncertain
cost map polluted
goal unreachable

A robust system should not simply crash.

It can perform recovery actions such as:

stop

wait

rotate to observe environment

clear/rebuild local obstacle map

request new global plan

back away

choose another route

Modern navigation stacks often coordinate these behaviors using state machines or behavior trees.

Imagine:

Try FollowPath
      |
      ├── Success → Done
      |
      └── Failure
            ↓
       ClearCostmap
            ↓
         Replan
            ↓
       FollowPath again

This logic becomes complicated quickly.

Behavior trees provide a structured way to express:

try this
if it fails, try that
retry
recover
fallback

ROS 2 Nav2 uses behavior-tree concepts extensively for navigation orchestration.

The planner itself may be mathematically sophisticated.

But the robot also needs decision logic around the planner.

That is systems engineering.

60. Exploration

So far we assumed the robot knows where it wants to go.

Exploration asks something different:

What if the world is unknown?

Suppose a robot begins with:

???
??R??
?????

As sensors observe the environment:

########
#......?
#..R...?
#......?
########

Some regions are known.

Others remain unknown.

The robot must decide where to go next so it can learn the environment.

61. Frontier-Based Exploration

One famous idea is frontier exploration.

A frontier is:

The boundary between known free space and unknown space.

Example:

############
#..........?
#..........?
#....R.....?
#..........?
############

The boundary near the ? region is a frontier.

The robot selects a useful frontier and navigates toward it.

When it gets there, its sensors reveal additional space.

Repeat:

detect frontiers
      ↓
choose frontier
      ↓
plan path
      ↓
navigate there
      ↓
observe new space
      ↓
update map
      ↓
detect new frontiers

Eventually much of the environment becomes mapped.

62. Exploration Is an Optimization Problem Too

If several frontiers exist:

Frontier A = 3 meters away, small unexplored area

Frontier B = 10 meters away, enormous unexplored area

Frontier C = 5 meters away, risky corridor

Which should we choose?

We can define something like:

utility =
    information_gain
  - travel_cost
  - risk

Then choose the frontier with the highest utility.

This reveals a larger principle:

Robotics repeatedly becomes an optimization between progress, cost, uncertainty, and risk.

Navigation:

I know where I want to go.
Find a safe way there.

Exploration:

I do not yet know enough about the world.
Decide where I should go to learn more.

Exploration therefore often contains navigation inside it.

Exploration system
      ↓
chooses next goal
      ↓
Navigation system
      ↓
moves robot there

64. One Complete Example

Suppose an autonomous warehouse robot receives:

Deliver package to Shelf B42.

Here is what may happen.

Step 1 — Localization

The robot estimates:

current pose:
x = 12.3
y = 8.4
θ = 32°

Step 2 — Goal

Shelf B42 corresponds to:

x = 63.0
y = 17.5

Step 3 — Global cost map

The navigation stack considers:

walls
shelves
restricted zones
inflated obstacle regions

Step 4 — Global planning

A* may search the cost map.

It minimizes approximately:

distance + obstacle costs

and generates:

global path

Step 5 — Smoothing

Jagged grid corners are softened.

Step 6 — Local sensing

LiDAR detects:

worker standing in corridor

This obstacle may not exist in the static map.

Step 7 — Local cost map

The worker is inserted as a nearby high-cost obstacle.

Step 8 — Local planner

The robot evaluates short-term motions:

left
right
slow
stop

and chooses a safe trajectory.

Step 9 — Controller

The controller translates that trajectory into:

linear velocity
angular velocity
motor commands

Step 10 — Worker moves

The local map updates.

The robot returns toward the global path.

Step 11 — Corridor becomes completely blocked

Now a small local detour may be impossible.

The system requests global replanning.

A* or an incremental planner finds another corridor.

Step 12 — Goal approached

Velocity planner gradually reduces speed.

Step 13 — Arrival

Robot verifies that goal tolerance has been satisfied.

position error < threshold
orientation error < threshold

Mission complete.

That single example contains almost the entire navigation stack.

65. The Planning Spectrum

You can now place planning algorithms on a conceptual spectrum.

LOW-DIMENSIONAL / STRUCTURED SPACE

BFS
 ↓
Dijkstra
 ↓
A*
 ↓
D*


CONTINUOUS / HIGH-DIMENSIONAL SPACE

PRM
RRT
RRT*
 ↓
trajectory optimization

This isn't a strict hierarchy.

It is a mental map of when different ideas become useful.

66. Another Useful Classification

Planning methods can also be classified by what they optimize.

Search-based

A*
Dijkstra
D*

Search through a structured graph.

Sampling-based

PRM
RRT
RRT*

Sample a large continuous space.

Optimization-based

CHOMP
STOMP
TrajOpt
MPC-style methods

Continuously improve a candidate trajectory.

Modern robotic systems frequently combine ideas from multiple categories.

67. Hybrid A*

Ordinary A* on a 2D grid may create motions impossible for cars.

Therefore autonomous-vehicle systems may use techniques such as Hybrid A*.

Instead of treating states only as:

(x, y)

Hybrid A* considers orientation:

(x, y, θ)

and generates motions compatible with car steering constraints.

This is another important lesson:

The correct planning representation depends on the robot's motion model.

A warehouse robot, car, drone, manipulator, and humanoid should not automatically use identical planners.

68. Planning Under Uncertainty

Everything we discussed so far may appear deterministic.

But reality is uncertain.

The robot may not know exactly:

where it is
where obstacles are
where humans will move
whether wheels will slip
whether sensors are wrong

A sophisticated system may therefore consider uncertainty.

Instead of:

obstacle is at x = 4.000

we might have:

obstacle likely around x = 4.0 ± uncertainty

Planning may need to prefer routes that remain safe despite imperfect knowledge.

This leads into deeper fields such as:

belief-space planning
POMDPs
chance-constrained planning
risk-aware planning

Those are beyond the basic navigation stack, but the idea is important:

A real autonomous robot does not plan through reality.

It plans through its belief about reality.

69. Static Planning vs Dynamic Planning

Another useful distinction:

Static planning

Assume environment stays constant while planning.

Example:

robot arm inside fixed factory cell

Dynamic planning

The environment evolves.

Example:

autonomous car among traffic

Dynamic planning must consider:

where obstacle is now
+
where obstacle may be later

At that point, time effectively becomes another planning dimension.

Instead of obstacle:

(x, y)

we care about:

(x, y, t)

because a point may be safe now but occupied three seconds later.

70. Why Dynamic Obstacles Are Hard

Imagine two robots moving toward an intersection.

Robot A plans:

intersection clear

Robot B independently plans:

intersection clear

Both paths are geometrically valid.

But both arrive simultaneously.

Collision.

Therefore dynamic navigation requires predicting:

future occupancy

not merely current occupancy.

For humans, this becomes even harder because their movement is not perfectly predictable.

Modern robotics increasingly combines planning with learned motion prediction.

71. Where AI Enters Motion Planning

Classical planning remains extremely powerful.

But AI can help with:

predicting pedestrian motion
learning cost functions
learning heuristics
semantic navigation
terrain classification
goal selection
planning from vision
world models
policy learning
navigation language understanding

For example:

"Take this package to the desk beside the red sofa."

A classical planner cannot directly understand that instruction.

An AI system may first infer the semantic goal.

Then classical planning can still perform safe navigation.

That hybrid architecture is often stronger than assuming an LLM or neural network should directly control motors.

72. Classical Planning vs Learned Policies

Classical planner:

map
+
robot model
+
goal
+
cost function
→
explicit path

Learned policy:

observation
→ neural network →
action

Learned policies can be powerful.

But classical planners offer major engineering advantages:

interpretability
constraints
predictability
debuggability
safety reasoning

Many advanced systems therefore combine both.

For autonomous systems, the important question is rarely:

Classical or AI?

It is:

Which parts should be learned, and which parts should remain explicit and constrained?

A real robot does not:

plan once
execute blindly

It continuously loops.

Sense
  ↓
Estimate state
  ↓
Update world
  ↓
Plan
  ↓
Act
  ↓
Sense again

More specifically:

sensor data
    ↓
localization + perception
    ↓
map / cost map update
    ↓
global/local planning
    ↓
trajectory
    ↓
control
    ↓
robot moves
    ↓
new sensor data

This might happen dozens of times per second.

Autonomy is fundamentally a closed-loop process.

74. Open Loop vs Closed Loop

Open-loop navigation:

Plan trajectory.

Execute it.

Hope reality matches prediction.

Closed-loop navigation:

Plan.

Move a little.

Measure what actually happened.

Correct.

Move.

Measure.

Correct.

Real autonomous systems must be strongly closed-loop.

Because reality never exactly follows the mathematical model.

75. A Very Important Engineer's Principle

Do not judge a navigation algorithm only by:

Does it find a path?

A production engineer asks many more questions.

For example:

How long does planning take?

How much memory does it use?

How often does it fail?

Does it behave deterministically?

How close does it drive to humans?

Can it recover from blocked paths?

Does it oscillate?

Does it produce smooth commands?

Can the controller actually track its trajectory?

What happens when localization jumps?

What happens when sensors temporarily fail?

Can it meet real-time deadlines?

This is the difference between:

knowing a planning algorithm

and

engineering an autonomous system.

Understanding failure modes often teaches more than memorizing algorithms.

Robot hugs walls

Likely issues:

inflation radius too small
obstacle cost too weak
footprint incorrect

Robot refuses to enter narrow doorway

Possible causes:

inflation radius too large
robot footprint too large
map inaccurate
localization error

Robot oscillates left-right

Possible causes:

local planner scoring
control instability
competing trajectories
poor lookahead

Robot repeatedly replans

Possible causes:

noisy cost map
unstable localization
global path invalidated repeatedly

Robot follows path but cuts corners

Possible causes:

controller tuning
lookahead
velocity too high
kinematic mismatch

Robot stops for phantom obstacle

Possible causes:

sensor noise
stale cost-map data
incorrect clearing
bad transform

Planner says no path exists

Possible causes:

goal inside obstacle

start inside inflated region

disconnected free space

incorrect map

wrong footprint

cost-map corruption

Notice that most real failures are not:

"A* is broken."

They are interactions between multiple subsystems.

77. Planner Quality Metrics

Several measurements help evaluate planning systems.

Path length

How far must the robot travel?

Planning time

How long did finding the path take?

Success rate

How often can a feasible path be found?

Clearance

How far does the path remain from obstacles?

Smoothness

How aggressively does curvature change?

Execution time

How long does the physical robot require?

Energy

How much energy does execution consume?

Replanning frequency

How often does the robot need a new plan?

There is rarely one universally best path.

The "best" path depends on the objective.

78. Planning Is Usually Multi-Objective

Suppose two routes exist.

Route A:

10 m long
5 cm from wall
many sharp turns

Route B:

12 m long
1 m from walls
smooth

Which is better?

If the objective is only distance:

If we value:

safety
smoothness
robustness

then B may be much better.

A real cost function might be:

Cost =
    distance
  + 10 × obstacle_risk
  + 3 × curvature
  + 2 × energy

Engineering the cost function can matter as much as choosing the search algorithm.

79. Planning Frequency Matters

Different parts of the stack operate at different rates.

Rough conceptual example:

Global planner:
occasionally / when needed

Local planner:
several to tens of Hz

Controller:
tens to hundreds of Hz

Motor control:
potentially hundreds or thousands of Hz

Why?

The closer you get to the physical hardware, the faster feedback generally needs to happen.

Another important systems insight:

Robotics is a hierarchy of loops operating at different timescales.

80. Motion Planning for Robot Arms

Everything we've discussed extends beyond mobile robots.

Suppose the arm begins:

q_start

and must reach:

q_goal

while avoiding:

table
box
itself
human
joint limits

Planning takes place primarily in joint configuration space.

A path might look like:

q_start
→ q1
→ q2
→ q3
→ q_goal

Each q specifies all joint angles.

Sampling-based planners such as:

RRT
RRTConnect
RRT*
PRM

are therefore common in manipulation frameworks such as MoveIt.

81. Why Planning in Cartesian Space Can Mislead You

Suppose the robot hand must move:

A -------- B

A straight Cartesian line looks perfect.

But the elbow may collide with a wall.

The end effector alone does not represent the entire robot.

That is exactly why configuration-space reasoning is so important.

A safe end-effector location does not guarantee a safe robot configuration.

For a drone:

q ≈ (x, y, z, roll, pitch, yaw, ...)

Now planning may occur in 3D space.

Additional constraints appear:

flight dynamics
velocity
acceleration
thrust
battery
wind
no-fly zones

A path may be collision-free but dynamically impossible.

Therefore drones often require stronger trajectory optimization and kinodynamic planning than slow indoor wheeled robots.

Cars introduce constraints such as:

road topology
lanes
traffic rules
minimum turning radius
other vehicles
pedestrians
speed limits
vehicle dynamics

The stack may resemble:

Route planning
      ↓
Behavior planning
      ↓
Motion planning
      ↓
Trajectory generation
      ↓
Control

Behavior planning decides things like:

follow lane
change lane
yield
stop
overtake

Motion planning then determines exactly how the vehicle should execute that behavior.

Again:

Higher-level intent and lower-level motion planning should not be confused.

84. The Deep Unifying Idea

All these algorithms may initially seem unrelated:

A*
D*
PRM
RRT
RRT*
trajectory optimization
costmaps
local planning

But underneath, they are all solving variations of one problem:

Search through possible futures and find a feasible, low-cost way to move from the current state toward a desired state.

The differences come from:

How is the world represented?

What counts as a state?

How are candidate motions generated?

How is collision checked?

What cost are we minimizing?

How much computation is available?

Is the world changing?

How uncertain is the world?

What dynamics must be respected?

If you understand those questions, you can understand unfamiliar planners even before mastering their equations.

85. The Algorithm Family Tree

Keep this mental map:

                  MOTION PLANNING
                         |
        +----------------+----------------+
        |                |                |
   Graph Search     Sampling-Based   Optimization-Based
        |                |                |
      BFS              PRM              CHOMP
      DFS              RRT              STOMP
   Dijkstra            RRT*            TrajOpt
      A*                                MPC
      D*

And around all of them:

collision checking
cost functions
robot constraints
maps
sensors
control

86. The Fast Memory Version

When you forget everything, reconstruct it like this.

Configuration space

Represent every possible robot pose/configuration as a point.

Collision checking

Ask whether a configuration or motion intersects obstacles.

BFS

Search outward equally.

DFS

Search one branch deeply.

Dijkstra

Expand lowest accumulated-cost path.

A*

Dijkstra + intelligence about where the goal is.

f = g + h

Heuristic

Estimate remaining cost.

D*

Repair plans efficiently when map costs change.

Sampling-based planning

Sample huge continuous spaces instead of enumerating everything.

PRM

Build a reusable network of collision-free configurations.

RRT

Grow a tree rapidly into unexplored space.

RRT*

RRT that keeps rewiring itself toward better solutions.

Trajectory optimization

Directly improve an entire motion according to costs and constraints.

Global planner

Long-distance strategic route.

Local planner

Short-distance immediate motion.

Obstacle avoidance

Stay safe as obstacles appear nearby.

Cost map

Encode how undesirable each region is.

Path smoothing

Remove ugly unnecessary geometric turns.

Trajectory generation

Add time and dynamic feasibility to the path.

Velocity planning

Decide how fast to move along it.

Integrate localization, maps, planners, controllers, recovery, and sensors.

Exploration

Choose where to move when the environment itself is still unknown.

Remember this picture above everything else:

                         GOAL
                           |
                           v
                   +---------------+
                   | Global Planner|
                   |  A*, D*, ...  |
                   +-------+-------+
                           |
                       Global Path
                           |
                           v
Sensors ------------> Cost Maps
 LiDAR                   |
 Camera                  |
 Radar                    v
                  +---------------+
                  | Local Planner |
                  | DWA/MPC/etc.  |
                  +-------+-------+
                          |
                    Local Trajectory
                          |
                          v
                  +---------------+
                  |  Controller   |
                  +-------+-------+
                          |
                       Commands
                          |
                          v
                        ROBOT
                          |
                    motion/sensors
                          |
                          +---------- feedback

Supporting everything:

Localization
Mapping
Coordinate transforms
Collision checking
Robot model
Recovery behavior

88. The Three Questions That Organize Everything

When looking at any planning algorithm, ask:

Question 1 — What space are we searching?

Examples:

2D grid
(x, y, θ)
joint space
3D flight space
state + velocity

This tells you the state representation.

Question 2 — How do we search it?

Examples:

BFS
Dijkstra
A*
D*
PRM
RRT
optimization

This tells you the planning algorithm.

Question 3 — What counts as good?

Examples:

shortest
fastest
safest
smoothest
lowest energy
maximum clearance
dynamically feasible

This tells you the objective / cost function.

Those three questions can decode a surprising amount of motion-planning research.

89. The Superior Engineer's Mental Model

Do not memorize:

A* = this algorithm
RRT = that algorithm
PRM = another algorithm

Think in layers.

Layer 1 — Representation

What does a state mean?

configuration space
map
cost map

Layer 2 — Feasibility

What is allowed?

collision checking
joint limits
kinematics
dynamics

Layer 3 — Search

How do we discover possible motions?

A*
D*
PRM
RRT

Layer 4 — Optimization

Which possible motion is preferable?

cost
length
clearance
smoothness
time
energy

Layer 5 — Execution

Can the physical robot actually perform it?

trajectory generation
velocity planning
control

Layer 6 — Adaptation

What happens when reality changes?

local planning
obstacle avoidance
replanning
recovery

Layer 7 — Autonomy

What if the robot does not even know where it should go next?

exploration
semantic goals
task planning

That is the complete architecture.

90. One Sentence to Remember the Entire Chapter

If you remember only one sentence, remember this:

Motion planning searches the robot's possible configurations for a collision-free, low-cost path; navigation continuously turns that plan into a safe, dynamically feasible trajectory while sensing, replanning, and controlling the robot as the real world changes.

And if you remember one pipeline:

WHERE AM I?
    ↓
WHERE DO I WANT TO GO?
    ↓
WHAT SPACE CAN I MOVE THROUGH?
    ↓
WHAT IS COLLISION-FREE?
    ↓
WHAT GLOBAL ROUTE IS BEST?
    ↓
WHAT SHOULD I DO RIGHT NOW?
    ↓
HOW FAST SHOULD I MOVE?
    ↓
WHAT MOTOR COMMANDS PRODUCE THAT MOTION?
    ↓
DID REALITY MATCH MY EXPECTATION?
    ↓
UPDATE AND REPEAT

That loop is navigation.

Everything else in this chapter is machinery for making one part of that loop better.

Final Mental Compression

When you see a robot autonomously moving through the world, imagine seven invisible systems working behind it:

1. REPRESENT
   Configuration space + maps

2. CHECK
   Collision checking

3. SEARCH
   BFS / Dijkstra / A* / D*
   PRM / RRT / RRT*

4. OPTIMIZE
   Costs + smoothing + trajectory optimization

5. TIME
   Trajectory generation + velocity planning

6. REACT
   Local planning + obstacle avoidance

7. REPEAT
   Sense → plan → act → observe → replan

And above all of them sits the autonomy question:

What should I do next?

When the goal is already known, that is navigation.

When even the next useful goal must be discovered, that becomes exploration.

That is Motion Planning and Navigation.

Command Palette

1. The Big Picture

2. Path vs Trajectory — Never Confuse These

3. Configuration Space

The powerful trick

4. Why Configuration Space Gets Hard

5. Collision Checking

6. Collision Checking Along a Path

7. From Geometry to Graph Search

8. BFS — Breadth-First Search

BFS intuition

BFS strength

BFS weakness

9. DFS — Depth-First Search

10. BFS vs DFS

11. Dijkstra's Algorithm

12. Why Dijkstra Is Better Than BFS

13. The Main Problem with Dijkstra

14. A* Search

15. What Is a Heuristic?

16. Why A* Can Be Much Faster

17. A* and Optimality

18. Heuristic Search

19. D*

20. D* Intuition

21. Grid Search Has a Problem

22. Sampling-Based Planning

23. PRM — Probabilistic Roadmap

24. PRM's Superpower

25. RRT — Rapidly-exploring Random Tree

26. Why RRT Is Clever

27. RRT Does Not Usually Produce Beautiful Paths

28. RRT*

29. PRM vs RRT vs RRT*

30. Graph Search vs Sampling-Based Planning

Graph/grid planning

Sampling-based planning

31. A Path Is Still Not Enough

32. Trajectory Optimization

33. Cost Functions Are Everywhere

34. Hard Constraints vs Soft Costs

35. Trajectory Optimization Algorithms

36. Global Planner vs Local Planner

Global Planner

Local Planner

37. The Ship Analogy

38. Why Not Recalculate the Entire Global Path Every Millisecond?

39. Obstacle Avoidance

Static obstacles

Dynamic obstacles

40. Reactive Obstacle Avoidance

41. Dynamic Window Approach

42. Cost Maps

43. Obstacle Inflation

44. Robot Footprint Matters

45. Global Cost Map and Local Cost Map

Global cost map

Local cost map

46. Path Smoothing

47. Why Smoothing Is Not Cosmetic

48. Kinematic Constraints

49. Kinodynamic Planning

50. Trajectory Generation

51. Velocity Planning

52. Velocity Profile

53. Planner vs Controller

54. Local Planner vs Controller

55. Navigation Stack

56. Navigation Requires Localization

57. Navigation Requires Coordinate Frames

58. Navigation Requires Recovery Behavior

59. Behavior Trees in Navigation

60. Exploration

61. Frontier-Based Exploration

62. Exploration Is an Optimization Problem Too

63. Exploration vs Navigation

64. One Complete Example

Step 1 — Localization

Step 2 — Goal

Step 3 — Global cost map