Outline
Path and Trajectory Planning
EE 451 - Path and Trajectory Planning
H.I. Bozma
Electric Electronic Engineering
Bogazici University
November 12, 2014
H.I. Bozma
EE 451 - Path and Trajectory Planning
Outline
Path and Trajectory Planning
Path and Trajectory Planning
Introduction
Path Planning Algorithms
Probabilistic Roadmap Methods (PRM)
Potential Fields
Trajectory Planning
H.I. Bozma
EE 451 - Path and Trajectory Planning
Outline
Path and Trajectory Planning
Introduction
Path Planning Algorithms
Probabilistic Roadmap Methods (PRM)
Potential Fields
Trajectory Planning
Getting robot to move
I
So far - Kinematics: The map btw configuration space DOF
and workspace position
I
Velocity kinematics: The map btw configuration space
velocities and workspace velocities
I
How to get the robot move and do what we want it to do?
I
Assume: Initial and final configurations of the robot is given.
H.I. Bozma
EE 451 - Path and Trajectory Planning
Outline
Path and Trajectory Planning
Introduction
Path Planning Algorithms
Probabilistic Roadmap Methods (PRM)
Potential Fields
Trajectory Planning
Getting robot to move - Motion planning
H.I. Bozma
EE 451 - Path and Trajectory Planning
Outline
Path and Trajectory Planning
Introduction
Path Planning Algorithms
Probabilistic Roadmap Methods (PRM)
Potential Fields
Trajectory Planning
Path vs Trajectory Planning
I
Path: A sequence of points (either in configuration or
workspace)
I
Trajectory: A sequence of points with timing
H.I. Bozma
EE 451 - Path and Trajectory Planning
Outline
Path and Trajectory Planning
Introduction
Path Planning Algorithms
Probabilistic Roadmap Methods (PRM)
Potential Fields
Trajectory Planning
Planning Considerations - Configuration (Joint) Space
H.I. Bozma
EE 451 - Path and Trajectory Planning
Outline
Path and Trajectory Planning
Introduction
Path Planning Algorithms
Probabilistic Roadmap Methods (PRM)
Potential Fields
Trajectory Planning
Planning Considerations - Workspace
H.I. Bozma
EE 451 - Path and Trajectory Planning
Outline
Path and Trajectory Planning
Introduction
Path Planning Algorithms
Probabilistic Roadmap Methods (PRM)
Potential Fields
Trajectory Planning
Obstacles
I
No consideration of restrictions due to the workspace
(Obstacles!)
I
Path and Trajectory Planning: Collision free paths or
trajectories
I
Computationally complex!
H.I. Bozma
EE 451 - Path and Trajectory Planning
Outline
Path and Trajectory Planning
Introduction
Path Planning Algorithms
Probabilistic Roadmap Methods (PRM)
Potential Fields
Trajectory Planning
Notation
I
Configuration space Q
I
Workspace W
I
Obstacles in workspace Oi ⊂ W
I
All obstacles O = ∪Oi
I
Robot with q ∈ Q → A(q) ⊂ W
I
Configuration space obstacles QO = {q ∈ Q | A(q) ∩ O =
6 ∅}
I
Free configuration space Qfree = Q \ QO
I
Initial configuration qs ∈ Qfree , Final configuration qf ∈ Qfree
H.I. Bozma
EE 451 - Path and Trajectory Planning
Outline
Path and Trajectory Planning
Introduction
Path Planning Algorithms
Probabilistic Roadmap Methods (PRM)
Potential Fields
Trajectory Planning
Problem Statement:
Given qs , qf ∈ Qfree , find a collision-free path γ : [0, 1] → Qfree
such that γ(0) = qs and γ(1) = qf
H.I. Bozma
EE 451 - Path and Trajectory Planning
Outline
Path and Trajectory Planning
Introduction
Path Planning Algorithms
Probabilistic Roadmap Methods (PRM)
Potential Fields
Trajectory Planning
Approaches
I
Not feasible to generate Qfree completely!
I
Search algorithm – Incremental generation of Qfree
I
I
Roadmap methods
Potential fields
H.I. Bozma
EE 451 - Path and Trajectory Planning
Outline
Path and Trajectory Planning
Introduction
Path Planning Algorithms
Probabilistic Roadmap Methods (PRM)
Potential Fields
Trajectory Planning
RM - General Approach
I
I
Generate a representation for Qfree
Generate 1-D network of curves (time as varying parameter)
where each arc → Collision-free path btw two points
I
I
I
Generate a path in Qfree from qs to qa
Generate a path in Qfree from qf to qb
Generate a path in Qfree from qa to qb
H.I. Bozma
EE 451 - Path and Trajectory Planning
Outline
Path and Trajectory Planning
Introduction
Path Planning Algorithms
Probabilistic Roadmap Methods (PRM)
Potential Fields
Trajectory Planning
Probabilistic Roadmap Methods (PRM)
I
Sampling the configuration space: A set of random
configurations are generated → Nodes
I
Connect pairs of configurations: Plan trajectories between
nodes
I
Enhancement: If trajectories are disconnected, generate new
paths to ensure connectivity
H.I. Bozma
EE 451 - Path and Trajectory Planning
Outline
Path and Trajectory Planning
Introduction
Path Planning Algorithms
Probabilistic Roadmap Methods (PRM)
Potential Fields
Trajectory Planning
Example - PRM Approach
H.I. Bozma
EE 451 - Path and Trajectory Planning
Outline
Path and Trajectory Planning
I
Advantages
I
I
Introduction
Path Planning Algorithms
Probabilistic Roadmap Methods (PRM)
Potential Fields
Trajectory Planning
No problems of local minima!
Problems
I
I
Computational
Not reactive
H.I. Bozma
EE 451 - Path and Trajectory Planning
Outline
Path and Trajectory Planning
Introduction
Path Planning Algorithms
Probabilistic Roadmap Methods (PRM)
Potential Fields
Trajectory Planning
Potential Field
I
Imagine a surface U defined on workspace on which the robot
is visualized to move down the gradient
I
Final configuration - Attractive (minimal point)
I
Obstacle boundaries - Repelling
I
Goal: One minimum (attractive) only with every other critical
point being nondegenerate!
I
U(q) = Ua (q) + Ur (q) → F = Do U(q)
H.I. Bozma
EE 451 - Path and Trajectory Planning
Outline
Path and Trajectory Planning
Introduction
Path Planning Algorithms
Probabilistic Roadmap Methods (PRM)
Potential Fields
Trajectory Planning
Attractive Field
Attractive force Ua,i =
1
T
2 ζi (oi (q) − oi (qf )) (oi (q) − oi (qf ))
1
1
T
2
2 ζi (oi (q) − oi (qf )) (oi (q) − oi (qf )) − 2 ζi d
H.I. Bozma
koi (q) − oi (qf )k ≤ d
koi (q) − oi (qf )k > d
EE 451 - Path and Trajectory Planning
Outline
Path and Trajectory Planning
Introduction
Path Planning Algorithms
Probabilistic Roadmap Methods (PRM)
Potential Fields
Trajectory Planning
Example – RR Planar Robot
H.I. Bozma
EE 451 - Path and Trajectory Planning
Outline
Path and Trajectory Planning
Introduction
Path Planning Algorithms
Probabilistic Roadmap Methods (PRM)
Potential Fields
Trajectory Planning
Repulsive Field
Ur ,i =
(
1
2 ηi
0
1
ρ(oi (q))
−
1
ρ0
2
H.I. Bozma
: ρ(oi (q)) ≤ ρ0
: ρ(oi (q)) > ρ0
EE 451 - Path and Trajectory Planning
Outline
Path and Trajectory Planning
Introduction
Path Planning Algorithms
Probabilistic Roadmap Methods (PRM)
Potential Fields
Trajectory Planning
Example – RR Planar Robot, Repulsive Force
H.I. Bozma
EE 451 - Path and Trajectory Planning
Introduction
Path Planning Algorithms
Probabilistic Roadmap Methods (PRM)
Potential Fields
Trajectory Planning
Outline
Path and Trajectory Planning
Repulsive Field
Ur ,2 =
1
2 η2
1
0.5
−1
2
1
0.52
0
−1
H.I. Bozma
EE 451 - Path and Trajectory Planning
Outline
Path and Trajectory Planning
Introduction
Path Planning Algorithms
Probabilistic Roadmap Methods (PRM)
Potential Fields
Trajectory Planning
Mapping Workspace Forces to Joint Torques
I
I
I
τ = JvT F
For each oi , construct the respective Jacobian Joi
P
P
Total torque τ = i JoTi Fai (q) + i JoTi Fri (q)
H.I. Bozma
EE 451 - Path and Trajectory Planning
Outline
Path and Trajectory Planning
Introduction
Path Planning Algorithms
Probabilistic Roadmap Methods (PRM)
Potential Fields
Trajectory Planning
Gradient Descent Planning
1. q(0) = qs , k = 0
2. If kq(k) − qf k > (q(k))
q(k + 1) = q(k) + αi kττ (q(k))k
k =k +1
else return q
3. Go to (2)
H.I. Bozma
EE 451 - Path and Trajectory Planning
Outline
Path and Trajectory Planning
Introduction
Path Planning Algorithms
Probabilistic Roadmap Methods (PRM)
Potential Fields
Trajectory Planning
General Performance
I
Advantages
I
I
I
Generate the torque trajectory to reach the goal
Reactive - As obstacles changes, so does the trajectory
Problems
I
I
Stuck in local minima
May be difficult to compute attactive & repulsive forces
H.I. Bozma
EE 451 - Path and Trajectory Planning
Outline
Path and Trajectory Planning
Introduction
Path Planning Algorithms
Probabilistic Roadmap Methods (PRM)
Potential Fields
Trajectory Planning
Revised Gradient Descent Planning
1. q(0) = qs , k = 0
2. If kq(k) − qf k > (q(k))
q(k + 1) = q(k) + αi kττ (q(k))k
k =k +1
else return q
3. If stuck in local minimum, move randomly to q 0
4. q(0) = q 0
5. Go to (2)
H.I. Bozma
EE 451 - Path and Trajectory Planning
Outline
Path and Trajectory Planning
Introduction
Path Planning Algorithms
Probabilistic Roadmap Methods (PRM)
Potential Fields
Trajectory Planning
Why Trajectory Planning?
I
In PRM’s, additional step of trajectory planning since we have
a sequence of points along the path!
I
I
I
I
Discrete time computation:
γ : [0, 1] → Qfree s.t. γ(0) = qs and γ(1) = qf
Map this to time taken to do the task tf − t0
Convert to a sequence of joint configurations
I
I
Inverse kinematics
Teach and playback
H.I. Bozma
EE 451 - Path and Trajectory Planning
Outline
Path and Trajectory Planning
Introduction
Path Planning Algorithms
Probabilistic Roadmap Methods (PRM)
Potential Fields
Trajectory Planning
Planning a Trajectory
I
Point to point – From q(0) → q(tf )
I
Velocity constraints q(0)
˙
= κ0 q(t
˙ f ) = κtf (Usually both
κ0 = κtf = 0
I
Acceleration constraints q¨(0) = α0 q¨(tf ) = αtf (Usually both
α0 = αt f = 0
H.I. Bozma
EE 451 - Path and Trajectory Planning
Outline
Path and Trajectory Planning
Introduction
Path Planning Algorithms
Probabilistic Roadmap Methods (PRM)
Potential Fields
Trajectory Planning
Point to Point Trajectories
I
I
I
I
P
Cubic polynomial trajectories q(t) = 4i=0 ai t i Discontinuties in jerk (Derivative of acceleration)
P
Quintic polynomial trajectories q(t) = 5i=0 ai t i
Linear segments with parabolic blends (LSPB)
Minimum time trajectory - A special case of LSPB
H.I. Bozma
EE 451 - Path and Trajectory Planning
Outline
Path and Trajectory Planning
Introduction
Path Planning Algorithms
Probabilistic Roadmap Methods (PRM)
Potential Fields
Trajectory Planning
Cubic Polynomial Trajectory - Example
H.I. Bozma
EE 451 - Path and Trajectory Planning
Outline
Path and Trajectory Planning
Introduction
Path Planning Algorithms
Probabilistic Roadmap Methods (PRM)
Potential Fields
Trajectory Planning
Quintic Polynomial Trajectory - Example
H.I. Bozma
EE 451 - Path and Trajectory Planning
Outline
Path and Trajectory Planning
Introduction
Path Planning Algorithms
Probabilistic Roadmap Methods (PRM)
Potential Fields
Trajectory Planning
LSPB Trajectory - Example
H.I. Bozma
EE 451 - Path and Trajectory Planning
Outline
Path and Trajectory Planning
Introduction
Path Planning Algorithms
Probabilistic Roadmap Methods (PRM)
Potential Fields
Trajectory Planning
Minimum Time Trajectory - Example
H.I. Bozma
EE 451 - Path and Trajectory Planning
Outline
Path and Trajectory Planning
Introduction
Path Planning Algorithms
Probabilistic Roadmap Methods (PRM)
Potential Fields
Trajectory Planning
Multi-Point Trajectories
I
A sequence of points – Endpoints are taken appropriately!
H.I. Bozma
EE 451 - Path and Trajectory Planning