INTRODUCTION
PATH PLANNING
ALGORITHMS
Nguyễn Khắc Hiếu
Mechatronics Division
Contents:
• Introduction to motion planning
• Configuration space
• Search-based planning
• Sample-based planning
A. Introduction to motion planning
Navigation system
Difference between local planner and global planner
The global planner (path planning) is used to construct safe
and collision free paths of the robot from initial pose to the
destination pose with a given map.
The local planner (obstacle avoidance) focuses on changing
the robot’s trajectory as informed by its sensor during robot
motion.
Particularly, the robots should predict obstacle’s future states
including position and orientation as well as future trajectory of
the obstacles in their vicinity.
Difference between local planner and global planner
Configuration Space
Free space and obstacle region
With W = being the work space , O the set of obstacles,
A(q) the robot in configuration q C.
: start configuration
: goal configuration
Configuration Space
Finding a continues path.
With this setting we can do
planning with the robot being a
point in C-space!
C-space’s discretization
Continuous terrain needs to be discretized for path planning
There are two general approaches to discrete C-spaces:
Combinational planning (Search-based planning)
Characterizes explicitly by capturing the connectivity of into a
graph and finds solutions using search.
Sampling-based planning
Uses collision-detection to probe and search the C-space for a
solution.
Cell Decomposition
Fixed cell decomposition
Exact cell decomposition
B. Search-based planning
• Breadth-first search (BFS)
• Dijkstra
• A*
Search
The performance of a search algorithm is measured in four ways:
Completeness: does the algorithm find the solution when there is
one?
Optimally: is the solution the best one of all possible solution in
terms of path cost?
Time complexity: How long does it take to find the solution ?
Space complexity: How much memory is needed to perform the
search ?
Objective
Input required:
Geometry of the robot and of obstacles
Kinematics of mobile robot
Initial and goal state
Expected result:
Collision-free trajectory
Robot should reach the goal location as fast as possible
Tree structure
• Nodes
• Edges
• Parent node
• Current node
• f(n) total cost from the initial state to
current node.
• g(n) actual cost from the initial state to
current node.
• c(n,n’) edge traversal cost.
• h(n) heuristic cost.
f(n) = g(n) + Ɛ.h(n)
BFS Start node
Step 1: Define start node and goal node and
visited
mark start node is “active”.
Neighbor nodes Step 2: Explore all neighbors node of the
of start node current node and mark them “open”.
Step 3: Choose one of these “open” nodes,
mark it “active” then mark the previous
node is visited and “parent”.
Step 4: Loops until get the goal node.
The path is constructed by following the
parent node of the current node.
Goal node 7 -> 6 -> 8 -> 1
Dijkstra
• Dijkstra algorithm is similar to BFS but it choosing node that have the lowest total
cost f(n).
f(n) = g(n)
• Ex:
A*
A* is another path-finding that extends Dijkstra’s algorithm by adding a
heuristic function to stop unnecessary nodes from being search.
Heuristic function in A* is Euclid distance from current node to goal node.
Cost function:
f(n) = g(n) + h(n)
Example
A* Dijkstra
Conclusion
Problem
Moving on the shortest path often guides the robot along a trajectory close to
the obstacle.
How far the path is isolated to the obstacle based on the mobile robot ‘s
geometry.
Trajectory aligned to the grid structure
Convolution of the Grid Map
Obstacles are assumed to be bigger than in reality.
Robot increases distance to obstacles and moves on a short path!
Example
A*
B. Sample-based planning
• Rapidly-exploring random tree (RRT)
• RRT*
Idea
• RRT is a random data structure and is
designed efficiently for multi dimension
space.
• Idea of RRT is sampling random node in
the configuration space until we find a
path from initial state to goal state.
• Reducing process time.
• The trajectory is not optimal and not
suitable for mobile robot in real life. RRT algorithm
RRT
• Step 1: Sample a node in the space, check if
𝛿 the node is in . If the node in return step 1.
𝑞𝑛𝑒𝑤 𝑞𝑟𝑎𝑛𝑑
𝑞𝑛𝑒𝑎𝑟𝑠𝑡 • Step2 : Find the nearest node in T . If the
line from to is collision free, add a line
segment with a length is to and define a
new node in the end of the line segment
T
then add it to the tree T.
• Step3 : Loop this process until it reach the
goal node.
𝑞𝑟𝑎𝑛𝑑 Follow the parent node to get the path
𝑞𝑛𝑒𝑎𝑟𝑠𝑡
T
RRT* 5
7
𝑞𝑛𝑒𝑤
S
𝑞𝑛𝑒𝑎𝑟𝑠𝑡
7 7
5 4
1 2 1 2
3 𝑞𝑛𝑒𝑤 3 𝑞𝑛𝑒𝑤
S 3 S
𝑞𝑛𝑒𝑎𝑟𝑠𝑡 3
6
𝑞𝑛𝑒𝑎𝑟𝑠𝑡 3
2 2 6
4 4
Step 1: Sample a node from free space, find the nearest node to that node the Tree
Step 2: Define is a line segment from to with a fixed length .
Step 3: check if line(, ) is collision-free ? else return step 1
Step 4: Define a circle with radius r around .
Step 5: Check all cost from to the neighbors node in the circle, if it higher exist one then remove the existing path
connecting to that node and generate a path from to that node then update the cost.
Step 6: Return step 1 until get termination.
RRT*
Conclusion
Trajectory generated by RRT* is optimal than RRT but
process time is higher.
Sampling algorithm are faster and have a trade-off between
optimally and speed
Summary
Grid-based search
A*
A* bidirectional
A* variant
Sample-based search
RRT*
Informed RRT*
BIT*
ABIT*