robotics chapter 5 – path and trajectory planning
DESCRIPTION
Robotics Chapter 5 – Path and Trajectory Planning. Dr. Amit Goradia. Topics. Introduction – 2 hrs Coordinate transformations – 6 hrs Forward Kinematics - 6 hrs Inverse Kinematics -6 hrs Velocity Kinematics - 2 hrs Trajectory Planning - 6 hrs - PowerPoint PPT PresentationTRANSCRIPT
![Page 1: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/1.jpg)
RoboticsChapter 5 – Path and Trajectory
Planning
Dr. Amit Goradia
![Page 2: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/2.jpg)
Topics
• Introduction – 2 hrs• Coordinate transformations – 6 hrs• Forward Kinematics - 6 hrs• Inverse Kinematics - 6 hrs• Velocity Kinematics - 2 hrs• Trajectory Planning - 6 hrs• Robot Dynamics (Introduction) - 2 hrs• Force Control (Introduction) - 1 hrs• Task Planning - 6 hrs
![Page 3: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/3.jpg)
Robot Motion Planning
• Path 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.
Task P lan
Action P lan
Path P lan
TrajectoryP lan
C ontro ller
Sensor
R obot
Tasks
![Page 4: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/4.jpg)
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
![Page 5: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/5.jpg)
• Notation:– A: single rigid object –(the robot)– W: Euclidean space where A moves;– B1,…Bm: fixed rigid obstacles distributed in W
Configuration Space
Configuration Space is the space of all possible robot configurations.
32 RorRW
• 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.
![Page 6: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/6.jpg)
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
110 ,, dqqqq d
q
![Page 7: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/7.jpg)
Configuration Space of a 2D Planer Robot
For a point robot moving in 2-D plane, C-space is
Configuration Space of A is the space (C )of all possible configurations of A.
qslug
qrobot
CCfree
Cobs
2R
Point robot (free-flying, no constraints)
![Page 8: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/8.jpg)
Configuration Space of a Robot Moving in 3D
For a point robot moving in 3-D, the C-space is
x
y
qstart
qgoal
C
Cfree
Cobs
3R
What is the difference between Euclidean space and C-space?
![Page 9: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/9.jpg)
Configuration Space of a 2R Articulated Robot
2R manipulator
topology
Configuration space
![Page 10: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/10.jpg)
Configuration Space
Two points in the robot’s workspace
270
360
180
90
090 18013545
Torus(wraps horizontally and vertically)
qrobot
qgoal
![Page 11: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/11.jpg)
Configuration Space
An obstacle in the robot’s workspace
270
360
180
90
090 18013545
qslug
qrobot
a “C-space” representation
If the robot configuration is within the blue area, it will hit the obstacle
What is dimension of the C-space of puma robot (6R)?
Visualization of high dimension C-space is difficult
![Page 12: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/12.jpg)
Motion PlanningFind a collision free path from an initial configuration to goal configuration while taking into account the constrains (geometric, physical, temporal)
A separate problem for each robot?
C-space concept provide a generalized framework to study the motion planning problem
![Page 13: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/13.jpg)
Expand obstacle(s)
Reduce robot
not quite right ...
Robot as a Point Object
![Page 14: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/14.jpg)
C-obstacle
Point robot
Growing Obstacles
![Page 15: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/15.jpg)
This expansion of one planar shape by another is called the Minkowski sum
P
RP R
Used in robotics to ensure that there are free paths available.
P R = { p + r | p P and r R }
Rectangular robot which can translate only
(Dilation operation)
Minkowski Sums
![Page 16: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/16.jpg)
What would the C-obstacle be if the rectangular robot (red) can translate and rotate in the plane.
(The blue rectangle is an obstacle.)
x
yRectangular robot which can translate and rotate
Additional Dimensions
![Page 17: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/17.jpg)
C-obstacle in 3-DWhat would the configuration space of a 3DOF rectangular robot (red) in this world look like?
(The obstacle is blue.)
x
y
0º
180º
can we stay in 2d ?
3-D
![Page 18: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/18.jpg)
One Slice of C-obstacle Taking one slice of the C-obstacle in which the robot is rotated 45 degrees...
x
y45 degrees
P
RP R
![Page 19: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/19.jpg)
2-D Projection
why not keep it this simple?
x
y
![Page 20: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/20.jpg)
Projection problems
qinit
qgoal too conservative!
![Page 21: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/21.jpg)
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)
qgoalqrobot
What to do?
Problem Statement
Compute a collision-free path for a rigid or articulated moving object among static obstacles
![Page 22: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/22.jpg)
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
![Page 23: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/23.jpg)
Roadmap: Visibility GraphsVisibility 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).
From Cfree, a graph is defined
Converts the problem into graph search.
Dijkstra’s algorithmOrder(N^2)
N = the number of vertices in C-space
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
![Page 24: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/24.jpg)
Visibility Graph in Action
• First, draw lines of sight from the start and goal to all “visible” vertices and corners of the world.
start
goal
![Page 25: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/25.jpg)
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.
start
goal
![Page 26: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/26.jpg)
The Visibility Graph • Repeat until you’re done.
start
goal
Since the map was in C-space, each line potentially represents part of a path from the start to the goal.
![Page 27: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/27.jpg)
Visibility Graph Drawbacks
Visibility graphs do not preserve their optimality in higher dimensions:
In addition, the paths they find are “semi-free,” i.e. in contact with obstacles.
shortest path
shortest path within the visibility graph
No clearance
![Page 28: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/28.jpg)
“official” Voronoi diagram
(line segments make up the Voronoi diagram isolates a set of points)
Roadmap: Voronoi diagrams
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.
![Page 29: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/29.jpg)
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
![Page 30: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/30.jpg)
Voronoi Diagram: Metrics• Many ways to measure distance; two are:
– L1 metric• (x,y) : |x| + |y| = const
– L2 metric• (x,y) : x2 +y2 = const
![Page 31: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/31.jpg)
Voronoi Diagram (L1)
Note the lack of curved edges
![Page 32: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/32.jpg)
Voronoi Diagram (L2)
Note the curved edges
![Page 33: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/33.jpg)
Motion Planning Methods
Roadmap approaches• Visibility Graph
• Voronoi Diagram
Cell decomposition
• Exact Cell Decomposition (Trapezoidal)
• Approximate Cell Decomposition (Quadtree)
Potential Fields
Hybrid local/global
![Page 34: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/34.jpg)
Exact Cell Decomposition
Decomposition of the free space into trapezoidal & triangular cells
Connectivity graph representing the adjacency relation between the cells
(Sweepline algorithm)
Trapezoidal Decomposition:
![Page 35: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/35.jpg)
Exact Cell Decomposition
Search the graph for a path (sequence of consecutive cells)
Trapezoidal Decomposition:
![Page 36: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/36.jpg)
Exact Cell Decomposition
Transform the sequence of cells into a free path (e.g., connecting the mid-points of the intersection of two consecutive cells)
Trapezoidal Decomposition:
![Page 37: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/37.jpg)
Obtaining the minimum number of convex cells is NP-complete.
Optimality
there may be more details in the world than the task needs to worry about...
15 cells 9 cells
Trapezoidal decomposition is exact and complete, but not optimal
Trapezoidal Decomposition:
![Page 38: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/38.jpg)
Quadtree Decomposition:
Approximate Cell Decomposition
recursively subdivides each mixed obstacle/free (sub)region into four quarters...
Quadtree:
![Page 39: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/39.jpg)
Further Decomposition
recursively subdivides each mixed obstacle/free (sub)region into four quarters...
Quadtree:
Quadtree Decomposition:
![Page 40: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/40.jpg)
Even Further Decomposition
Again, use a graph-search algorithm to find a path from the start to goal
Quadtreeis this a complete path-planning algorithm?
i.e., does it find a path when one exists ?
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
![Page 41: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/41.jpg)
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
![Page 42: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/42.jpg)
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~
![Page 43: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/43.jpg)
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
![Page 44: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/44.jpg)
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.
![Page 45: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/45.jpg)
Motion Planning Methods
Roadmap approaches
Cell decomposition
• Exact Cell Decomposition (Trapezoidal)
• Approximate Cell Decomposition (Quadtree)
Potential Fields
Hybrid local/global
![Page 46: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/46.jpg)
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
![Page 47: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/47.jpg)
• Compute an attractive force toward the goal
Potential Field Method
C-obstacles
Attractive potential
![Page 48: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/48.jpg)
Potential Field Method
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
• Compute a repulsive force away from obstacles
![Page 49: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/49.jpg)
• Compute a repulsive force away from obstacles
Potential Field Method
• Repulsive Potential
![Page 50: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/50.jpg)
• Sum of Potential
Potential Field Method
C-obstacle
Attractive potential Repulsive potential
Sum of potentials
![Page 51: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/51.jpg)
• After get total potential, generate force field (negative gradient)
• Let the sum of the forces control the robot
To a large extent, this is computable from sensor readings
Equipotential contours
Negative gradient
Total potential
Potential Field Method
![Page 52: Robotics Chapter 5 – Path and Trajectory Planning](https://reader036.vdocuments.us/reader036/viewer/2022081501/56813608550346895d9d7e64/html5/thumbnails/52.jpg)
Potential Field Method
• 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
Pros:
• 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
Cons: