MTS 418 Applied Robotics

Kanwal Naveed
Associate HoD
D* Algorithm
Why Do We Need D*?
Lab : 25
• Unlike A*, which computes a full path once, D* recalculates
paths dynamically when obstacles or costs change.
• It reuses previous search results, making it faster than
running A* from scratch every time an update is needed.
• The algorithm propagates cost changes backward from
the affected areas, ensuring the robot always follows the
optimal path.
• Variants:
-D* -Lite
-Field D* 2
 D* Algorithm
1. Initialize:
- Set `goal` node's cost to 0.
- Insert `goal` into the priority queue `OPEN` with
priority `0`. Lab : 25
3. If an obstacle is detected in the known path:
- Mark all other nodes with infinite cost.
a. Increase the cost of affected edges or mark them
as obstacles.
2. While `OPEN` is not empty:
b. Propagate changes:
a. Extract node `current` with the lowest cost from
i. Backtrack to affected nodes.
`OPEN`.
ii. Update costs and re-insert nodes into `OPEN` if
b. If `current` is the `start` node, break (optimal path
necessary.
found).
iii. Recompute path dynamically.
c. For each `neighbor` of `current`:
i. Compute new cost `g(neighbor) = g(current) +
4. Continue navigation based on the updated path.
cost(current, neighbor)`.
ii. If `g(neighbor)` is lower than previous cost:
- Update `g(neighbor)`.
- Set `parent(neighbor) = current`.
- Insert or update `neighbor` in `OPEN` with
priority `g(neighbor) + h(neighbor, start)`. 3
RRT Algorithm

Lab : 25

Rapidly Exploring Random Trees is a sampling-based motion planning algorithm that efficiently
searches large, high-dimensional spaces
-Can be used for both manipulators and mobile robots

4
Difference between Dijkstra and RRT Algorithm

Lab : 25

5
RRT Algorithm

Lab : 25

6
RRT Algorithm

Lab : 25

Random Tree??
Start from a starting node.
Which is a random place. The red dot.

7
RRT Algorithm

Lab : 25

Random Tree??
Start from a starting node.
Which is a random place. The red dot.

Select a random node. And

try to move towards it but only by a
set distance called dstep

8
 RRT Algorithm

Lab : 25

So just a random tree is not helping much…

May be we need a direction in our exploration hence the word Rapidly…
9
RRT Algorithm
Similar to random tree.
Lab : 25
The first step is exactly the same.
Select a random node from start
and move a set distance in that
direction

10
RRT Algorithm
Now each time we try to expand a node we select the nearest node. This
way each node will have a parent node. Lab : 25

11
RRT Algorithm

Lab : 25

When to Stop?
• When the nearest node is within a set radius
of goal
• When max iterations are reached
• When Tree covers enough free space

• Time limit exceeds

12
RRT Algorithm

Lab : 25
Finding the path?
• Use backtracking method.

13
RRT Algorithm

Lab : 25
Finding the path?
• What if the path is jagged and not very
optimal.

• Adding more nodes certainly don’t help

anymore.

• Moving to RRT*

14
RRT* Algorithm

Lab : 25

15
RRT* Algorithm

Lab : 25

16
RRT* Algorithm

Lab : 25

17
RRT* Algorithm

Lab : 25

Dump some
more nodes

More nodes
means more
optimal path
Computational
Overhead

18
RRT Algorithm and Obstacle Avoidance

Lab : 25

If an obstacle is hit, the node is discarded

and the algorithm tries again from the same
parent node

19
RRT Algorithm- Applying to Robots

Lab : 25

For a mobile robot, each node is represented by the x,y position and distance between a
parent and current node is Euclidean distance

20
 RRT Algorithm- Applying to Robots

For a manipulator, xyz representation is not feasible Lab : 25

The nodes are represented in terms of joint angles.
The start location is the starting angle

21