1. Robotics Chapter 5 – Path and Trajectory Planning
Dr. Amit Goradia

2. Topics Introduction – 2 hrs Coordinate transformations – 6 hrs
Forward Kinematics hrs
Inverse Kinematics hrs
Velocity Kinematics hrs
Trajectory Planning hrs
Robot Dynamics (Introduction) hrs
Force Control (Introduction) - 1 hrs
Task Planning hrs

3. Robot Motion Planning Path planning Trajectory planning,
Geometric path
Issues: obstacle avoidance, shortest path
Trajectory planning,
“interpolate” or “approximate” the desired path by a class of polynomial functions
Generate a sequence of time-based “control set points” for the control of manipulator from the initial configuration to its destination.

4. The World is Comprised of…
Obstacles
Already occupied spaces of the world
In other words, robots can’t go there
Free Space
Unoccupied space within the world
Robots “might” be able to go here
To determine where a robot can go, we need to discuss what a Configuration Space is

5. Configuration Space
Notation:
A: single rigid object –(the robot)
W: Euclidean space where A moves;
B1,…Bm: fixed rigid obstacles distributed in W
• FW – world frame (fixed frame)
• FA – robot frame (moving frame rigidly associated with the robot)
Configuration q of A is a specification of the physical state (position and orientation) of A w.r.t. a fixed environmental frame FW.
Configuration Space is the space of all possible robot configurations.

6. Definitions
Configuration: Specification of all the variables that define the system completely
Example: Configuration of a dof robot is
Configuration space (C-space): Set of all configurations
Free configuration: A configuration that does not collide with obstacles
Free space ( F ) : Set of all free configurations
It is a subset of C

7. Configuration Space of a 2D Planer Robot
Configuration Space of A is the space (C )of all possible configurations of A.
Point robot (free-flying, no constraints)
qslug
qrobot C
Cfree
Cobs
For a point robot moving in 2-D plane, C-space is

8. Configuration Space of a Robot Moving in 3DZ x y
qstart
qgoal C
Cfree
Cobs
For a point robot moving in 3-D, the C-space is
What is the difference between Euclidean space and C-space?

9. Configuration Space of a 2R Articulated Robot
2R manipulator
Configuration space
topology

10. Two points in the robot’s workspace
Configuration Space
360
qrobot
270 b
180 b a 90
qgoal a 45 90
135
180
Two points in the robot’s workspace
Torus
(wraps horizontally and vertically)

11. Configuration Space qrobot qslug
If the robot configuration is within the blue area, it will hit the obstacle
360
qrobot
270 b
180 b a 90
qslug a 45 90
135
180
An obstacle in the robot’s workspace
a “C-space” representation
What is dimension of the C-space of puma robot (6R)?
Visualization of high dimension C-space is difficult

12. Motion Planning
Find a collision free path from an initial configuration to goal configuration while taking into account the constrains (geometric, physical, temporal)
C-space concept provide a generalized framework to study the motion planning problem
A separate problem for each robot?

13. Robot as a Point Object Expand obstacle(s) Reduce robot
not quite right ...

14. Growing Obstacles
C-obstacle
Point robot

15. Minkowski Sums
This expansion of one planar shape by another is called the Minkowski sum 
Rectangular robot which can translate only
P  R R P
P  R = { p + r | p  P and r  R }
(Dilation operation)
Used in robotics to ensure that there are free paths available.

16. Additional Dimensions
What would the C-obstacle be if the rectangular robot (red) can translate and rotate in the plane.
(The blue rectangle is an obstacle.) y
Rectangular robot which can translate and rotate x

17. C-obstacle in 3-D
What would the configuration space of a 3DOF rectangular robot (red) in this world look like?
(The obstacle is blue.)
3-D y
180º 0º x
can we stay in 2d ?

18. One Slice of C-obstacle
Taking one slice of the C-obstacle in which the robot is rotated 45 degrees...
P  R R y
45 degrees P x

19. 2-D Projection y x
why not keep it this simple?

20. Projection problems
qinit
qgoal
too conservative!

21. Motion Planning Methods
The motion planning problem consists of the following:
Input
Output
geometric descriptions of a robot and its environment (obstacles)
initial and goal configurations
a path from start to finish (or the recognition that none exists)
qrobot
qgoal
Problem Statement
Compute a collision-free path for a rigid or articulated moving object among static obstacles
What to do?

22. Motion Planning Methods
(1) Roadmap approaches
(2) Cell decomposition
(3) Potential Fields
(4) Bug algorithms
Goal reduce the N-dimensional configuration space to a set of one-D paths to search.
Goal account for all of the free space
Goal Create local control strategies that will be more flexible than those above
Limited knowledge path planning

23. Roadmap: Visibility Graphs
Visibility graphs: In a polygonal (or polyhedral) configuration space, construct all of the line segments that connect vertices to one another (and that do not intersect the obstacles themselves).
Formed by connecting all “visible” vertices, the start point and the end point, to each other.
For two points to be “visible” no obstacle can exist between them
Paths exist on the perimeter of obstacles
From Cfree, a graph is defined
Converts the problem into graph search.
Dijkstra’s algorithm
Order(N^2)
N = the number of vertices in C-space

24. Visibility Graph in Action
First, draw lines of sight from the start and goal to all “visible” vertices and corners of the world.
goal
start

25. Visibility Graph in Action
Second, draw lines of sight from every vertex of every obstacle like before. Remember lines along edges are also lines of sight.
goal
start

26. The Visibility Graph Repeat until you’re done. goal start
Since the map was in C-space, each line potentially represents part of a path from the start to the goal.

27. Visibility Graph Drawbacks
Visibility graphs do not preserve their optimality in higher dimensions:
shortest path
shortest path within the visibility graph
In addition, the paths they find are “semi-free,” i.e. in contact with obstacles.
No clearance

28. Roadmap: Voronoi diagrams
“official” Voronoi diagram
(line segments make up the Voronoi diagram isolates a set of points)
Generalized Voronoi Graph (GVG): locus of points equidistant from the closest two or more obstacle boundaries, including the workspace boundary.
Property: maximizing the clearance between the points and obstacles.

29. Roadmap: Voronoi diagrams
GVG is formed by paths equidistant from the two closest objects
maximizing the clearance between the obstacles.
This generates a very safe roadmap which avoids obstacles as much as possible

30. Voronoi Diagram: Metrics
Many ways to measure distance; two are:
L1 metric
(x,y) : |x| + |y| = const
L2 metric
(x,y) : x2 +y2 = const

31. Note the lack of curved edges
Voronoi Diagram (L1)
Note the lack of curved edges

32. Voronoi Diagram (L2)
Note the curved edges

33. Motion Planning Methods
Roadmap approaches
Visibility Graph
Voronoi Diagram
Cell decomposition
Exact Cell Decomposition (Trapezoidal)
Approximate Cell Decomposition (Quadtree)
Potential Fields
Hybrid local/global

34. Exact Cell Decomposition
Trapezoidal Decomposition:
Decomposition of the free space into trapezoidal & triangular cells
Connectivity graph representing the adjacency relation between the cells
(Sweepline algorithm)

35. Exact Cell Decomposition
Trapezoidal Decomposition:
Search the graph for a path (sequence of consecutive cells)

36. Exact Cell Decomposition
Trapezoidal Decomposition:
Transform the sequence of cells into a free path (e.g., connecting the mid-points of the intersection of two consecutive cells)

37. Optimality Trapezoidal Decomposition: 15 cells 9 cells
Obtaining the minimum number of convex cells is NP-complete.
Trapezoidal decomposition is exact and complete, but not optimal
there may be more details in the world than the task needs to worry about...

38. Approximate Cell Decomposition
Quadtree Decomposition:
recursively subdivides each mixed obstacle/free (sub)region into four quarters...
Quadtree:

39. Further Decomposition
Quadtree Decomposition:
recursively subdivides each mixed obstacle/free (sub)region into four quarters...
Quadtree:

40. Even Further Decomposition
Quadtree Decomposition:
• The rectangle cell is recursively decomposed into smaller rectangles
• At a certain level of resolution, only the cells whose interiors lie entirely in the free space are used
• A search in this graph yields a collision free path
Again, use a graph-search algorithm to find a path from the start to goal
is this a complete path-planning algorithm?
i.e., does it find a path when one exists ?
Quadtree

41. Probablistic Roadmap Methods
What is a PRM (Probablistic Roadmap Method)
A probabilistic road map is a discrete representation of a continuous configuration space generated by randomly sampling the free configurations of the C-space and connecting those points into a graph
Complete path planning in high dimensional C- spaces is very complex
PRM methods boost performance by trading completeness for probabilistic completeness
Two phase approach: Learning phase, Query phase

42. Probabilistic Roadmap Methods
Probabilistic techniques to incrementally build a roadmap in free space of robot
Efficiency-driven
Robots with many dofs (high-dim C-spaces)
Static environments s g s ~ g

43. Learning Phase Learning phase: Construction:
randomly sample free space and create a list of nodes in free space.
Connect all the nearest neighbors using a fast local planner.
Store the graph whose nodes are configurations and edges are paths computed by local planner
Expansion step: Find “Difficult” nodes and expand the graph around them using random walk techniques

44. Query Phase
Find a path from the start and goal positions to two nodes of the roadmap
Search the graph to find a sequence of edges connecting those nodes in the roadmap
Concatenating successive segments gives a feasible path for robot.

45. Motion Planning Methods
Roadmap approaches
Cell decomposition
Exact Cell Decomposition (Trapezoidal)
Approximate Cell Decomposition (Quadtree)
Potential Fields
Hybrid local/global

46. Potential Field Method
Potential Field (Working Principle)
– The goal location generates an attractive potential – pulling the robot towards the goal
– The obstacles generate a repulsive potential – pushing the robot far away from the obstacles
– The negative gradient of the total potential is treated as an artificial force applied to the robot
-- Let the sum of the forces control the robot
C-obstacles

47. Potential Field Method
Compute an attractive force toward the goal
C-obstacles
Attractive potential

48. Potential Field Method
Compute a repulsive force away from obstacles
Repulsive Potential
Create a potential barrier around the C-obstacle region that cannot be traversed by the robot’s configuration
It is usually desirable that the repulsive potential does not affect the motion of the robot when it is sufficiently far away from C-obstacles

49. Potential Field Method
Compute a repulsive force away from obstacles
Repulsive Potential

50. Potential Field Method
Sum of Potential
Attractive potential
Repulsive potential
C-obstacle
Sum of potentials

51. Potential Field Method
After get total potential, generate force field (negative gradient)
Let the sum of the forces control the robot
Negative gradient
Equipotential contours
Total potential
To a large extent, this is computable from sensor readings

52. Potential Field Method
Pros:
Spatial paths are not preplanned and can be generated in real time
Planning and control are merged into one function
Smooth paths are generated
Planning can be coupled directly to a control algorithm
Cons:
Trapped in local minima in the potential field
Because of this limitation, commonly used for local path planning
Use random walk, backtracking, etc to escape the local minima