Up till now
§ Lecture 1
Path and trajectory planning
Ÿ Rigid body motion
Ÿ Representation of rotation
Ÿ Homogenous transformation
§ Lecture 2
Lecture 5
Mikael Norrlöf
Ÿ Kinematics
• Position
• Velocity via Jacobian
• DH parameterization
§ Lecture 4
Ÿ Robot Motion Control
Overview
Ÿ Current and Torque Control
Ÿ Control Methods for Rigid
and Flexible Robots
Ÿ Interaction with the
environment
§ Lecture 3
Ÿ Lagrange’s equation
Ÿ (Newton Euler)
Ÿ Parameter identification
• Experiment design
• Model structure
PhDCourse Robot modeling and control
Lecture 4
Outline
§ Path vs trajectory
§ Standard path planning techniques in industrial
robots
§ Trajectory generation – an introduction to the
problem
§ More general path planning algorithms
Ÿ Potential field approach
Ÿ An introduction to Probabilistic Road Maps (PRMs)
Path and trajectory planning
What is the difference between path and trajectory?
Path: Only geometric
considerations. The way to
go from config a to b.
Compare: kinematics versus dynamics.
§ Lab session: date/time to be decided
PhDCourse Robot modeling and control
Lecture 4
Trajectory: Include time,
i.e., consider the dynamics.
PhDCourse Robot modeling and control
Lecture 4
Path planning
§ In industrial robot applications two path planning
modes can be identified
Path planning
§ In industrial robot applications two path planning
modes can be identified
Ÿ Change configuration
Ÿ Change configuration
In RAPID (ABB’s programming language)
MoveJ p10, v1000, z10, tool;
Ÿ Perform an operation
Ÿ Perform an operation
MoveL p20, v50, z1, tool;
MoveC p30, p40, v25, fine, tool;
PhDCourse Robot modeling and control
Lecture 4
Rapid, robot motion instructions
§ Linear in
Ÿ joint space: MoveJ
Ÿ Cartesian space: MoveL
PhDCourse Robot modeling and control
Lecture 4
Via points
§ Point-to-point (fine points in Rapid). Robot has to
stop.
p20
p40
Ÿ MoveC
p30
p10
§ Circle segment
§ Via points (zones in Rapid). Robot does not reach the
programmed position.
p20
p40
p10
PhDCourse Robot modeling and control
Lecture 4
PhDCourse Robot modeling and control
Lecture 4
p30
Geometric description
Orientation interpolation
0.7
4
2
4
0.6
0.5
0.4
q(lc)
0.3
0.2
0.1
Inverse
kinematics
0
-0.1
-0.2
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
lc
One possible
parameterization is
cubic splines.
1.45
1.4
1.35
z
1.3
1.25
1.2
0
0.1
1.15
0.2
1
0.3
0.9
0.4
0.8
0.7
See MSc theses, “Path planning for
industrial robots” by Maria Nyström and
“Path generation in 6-DOF for industrial
robots” by Daniel Forssman.
§ The orientation along the path is interpolated to get a
smooth change of orientation.
§ Given initial orientation (as a quaternion), q0 and final
orientation q1 compute
q01 = q1q0-1
and interpolate the angle a from 0 to q01 in
qip(a) = <cos(a/2), sin(a/2) s01>
where (q01,s01) is the angle-axis representation of q01
§ The interpolation scheme is often referred to as slerp
interpolation
0.5
y
x
Chap 5 in “Modeling and Control of Robot
Manipulators”, Sciavicco and Siciliano
PhDCourse Robot modeling and control
Lecture 4
PhDCourse Robot modeling and control
Lecture 4
The trajectory generation problem
The trajectory generation problem
Optimal control!
Optimal control!
qf
The path is parameterized in
some index s Þ qr(s) which
introduces additional
constraints.
q0
PhDCourse Robot modeling and control
Lecture 4
qf
q0
PhDCourse Robot modeling and control
Lecture 4
The trajectory generation problem
Trajectory generation problem
Resulting optimization problem
Complexity:
• 6 joints/actuators
• A robot can have > 100
constraints
vd = 500 mm/s
1.15
• Multiple robots possible
1.1
1.05
z
vd = 1000 mm/s
xx
1
0.95
0.4
0.9
1
0.3
0.8
0.2
0.6
0.1
0.4
y
PhDCourse Robot modeling and control
Lecture 4
0.2
0
x
PhDCourse Robot modeling and control
Lecture 4
Trajectory generation problem
Dynamisk optimering
1.15
1.1
1
0.95
0.4
0.9
1
0.3
0.8
0.2
0.6
0.1
0.4
y
0.2
0
x
vd = 500 mm/s
1.15
1.1
1.05
z
vd = 1000 mm/s
xx
1
0.95
0.4
0.9
1
0.2
0.6
0.2
0
1.1
1.05
v d = 1000 mm/s
xx
1
0.95
q(t)
0.4
0.9
1
0.3
0.8
0.2
0.6
0.1
0.4
0.2
y
0
x
1.5
1
0.5
0
0.1
0.4
y
v d = 500 mm/s
1.15
q(t)
0.3
0.8
O
P
T
I
M
I
Z
A
T
I
O
N
z
z
1.05
x
-0.5
-1
0
0.5
1
1.5
Time [s ec]
PhDCourse Robot modeling and control
Lecture 4
PhDCourse Robot modeling and control
Lecture 4
2
2.5
Solution
Dynamic scaling of trajectories
§ The dynamic model can be rewritten in the form
D( q )q&& + C( q , q& )q& + g( q ) = t
1
424
3
G( q )[ q&q& ]
§ Let r = ct (time scaling). The original speed/acc dep
torque is
t s = D( q )q&& + G( q )[ q&q& ]
If time scaling is applied
t s = r& 2t s + r&&D( q( r ))q¢( r )
and with linear time scaling
t s = c 2t s
PhDCourse Robot modeling and control
Lecture 4
Application to a robot system
PhDCourse Robot modeling and control
Lecture 4
A more general path planning scheme
Use more than one robot to
increase the flexibility in an
application.
Here with application arc
welding.
PhDCourse Robot modeling and control
Lecture 4
PhDCourse Robot modeling and control
Lecture 4
Configuration space
Obstacles
A complete specification of the location of every point
on the robot is a configuration (Q).
The workspace of the robot is W. The subset of W
occupied by the robot is A(q).
The set of all configurations is called the configuration
space.
The configuration space obstacle is defined as
q + kinematics give configuration.
with O = ÈOi
QO = {q Î Q A( q ) Ç O ¹ Æ}
The collision free configurations are
Qfree = Q \ QO
PhDCourse Robot modeling and control
Lecture 4
Collision detection
§ A number of packages exists on the internet:
Ÿ One example is from the group “team gamma” at Berkley
http://gamma.cs.unc.edu/research/collision/packages.html
Ÿ Another at University of Oxford
http://web.comlab.ox.ac.uk/people/Stephen.Cameron/distances/
Ÿ See also wikipedia
http://en.wikipedia.org/wiki/Collision_detection
§ An application where collision detection is used can
be found in MSc thesis
http://www.control.isy.liu.se/student/exjobb/xfiles/2050.pdf
PhDCourse Robot modeling and control
Lecture 4
PhDCourse Robot modeling and control
Lecture 4
General formulation of the path planning problem
Find a collision free path from an initial configuration qs
to a final configuration qf.
More formally g : [ 0,1] ® Qfree
with g (0) = qs and g (1) = qf.
Examples of methods:
§ Path planning using potential fields
§ Probabilistic road maps (PRMs)
PhDCourse Robot modeling and control
Lecture 4
Artificial potential field
§ Idea: Treat the robot as a point particle in the
configuration space, influenced by an artificial
potential field. Construct the field U such that the
robot is attracted to the final configuration qf while
being repelled from the boundaries of QO.
§ In general it is difficult/impossible to construct a field
without having local minima.
Slide from: http://ai.stanford.edu/~latombe/
PhDCourse Robot modeling and control
Lecture 4
Construction of the field U
§ U can be constructed as an addition of an attractive
field and a second component that repels the robot
from the boundary of QO
U(q) = Uatt(q) + Urep(q)
§ Path planning can be treated as an optimization
problem, finding the global minimum of U(q). One
simple approach is to use a gradient descent
algorithm.
Let t ( q ) = -ÑU ( q ) = -ÑU att ( q ) - ÑU rep ( q )
PhDCourse Robot modeling and control
Lecture 4
Construction of the field U
Comments
§ In general difficult to construct the potential field in
configuration space (the field is often based on the
norm of the min length to the obstacles)
§ Easier to define the field in the robot workspace
§ For an n-link manipulator a potential field is
constructed for each DH-frame
§ The link between workspace and configuration space
is the Jacobian
PhDCourse Robot modeling and control
Lecture 4
PhDCourse Robot modeling and control
Lecture 4
The attractive field
The repulsive field
§ Conic well potential (Uatt,i(q) = ||oi(q) - oi(qf)||)
§ Parabolic well potential (Uatt,i(q) = ½ xi ||oi(q) - oi(qf)||2)
§ Parabolic well potential with upper bound
2
ì
1
z o ( q ) - oi ( q f ) ,
2 i i
ï
U att ,i ( q ) = í
2
1 2
ïdz i oi ( q ) - oi ( q f ) - d z i ,
2
î
and
(
)
ì - z i oi ( q ) - oi ( q f ) ,
ï
oi ( q ) - oi ( q f )
Fatt ,i ( q ) = í
ï dz i o ( q ) - o ( q ) ,
i
i
f
î
oi ( q ) - oi ( q f ) £ d
Criteria for the repulsive field to satisfy,
§ Repel the robot from obstacles (never allow the robot
to collide)
§ Obstacles should not affect the robot when being far
away
oi ( q ) - oi ( q f ) > d
oi ( q ) - oi ( q f ) £ d
oi ( q ) - oi ( q f ) > d
PhDCourse Robot modeling and control
Lecture 4
Path planning – obstacle free path
Notice:
Including only the origin of the DH-frames does not guarantee
collision free path. (Additional points can however be added.)
PhDCourse Robot modeling and control
Lecture 4
The repulsive field
One possible choice
2
ì æ
1
1 ö
ï 12 h i ç
÷
U rep ,i ( q ) = í ç r ( o ( q )) r ÷ , r ( oi ( q )) £ r 0
0 ø
i
è
ï
0,
r ( oi ( q )) > r 0
î
with
æ
1
1 ö
1
÷÷ 2
Ñr ( oi ( q ))
Frep ,i ( q ) = h i çç
è r ( oi ( q )) r 0 ø r ( oi ( q ))
If the obstacle region is convex and b is the closest point to oi
r ( oi ( q )) = oi ( q ) - b ,
Ñr ( x ) x = o ( q ) =
i
PhDCourse Robot modeling and control
Lecture 4
PhDCourse Robot modeling and control
Lecture 4
oi ( q ) - b
oi ( q ) - b
Comment, the “convex assumption”
Mapping the workspace forces into joint torques
§ The force vector has a
discontinuity
§ Distance function not
differentiable everywhere
If a force is exerted at the end-effector
Can be avoided if repulsive
fields of distinct obstacles
do not overlap.
Notice that the full Jacobian can be used when mapping
forces and torques from workspace to torques in
configuration space.
PhDCourse Robot modeling and control
Lecture 4
Path construction in configuration space
§ Build the path using the resulting configuration space torques
and an optimization algorithm
J vT F = t
The Jacobian can be derived in all the points oi.
PhDCourse Robot modeling and control
Lecture 4
Escape local minima using randomization
When stuck in a local minimum execute a random walk
Gradient descent algorithm
Design parameters, ai, zi, hi, r0.
Typical problem: Can get stuck in local minima.
PhDCourse Robot modeling and control
Lecture 4
New problems:
§ Detect when a local minimum is reached
§ Define how the random walk should behave (how many steps,
define the random terms, variance, distribution, …)
PhDCourse Robot modeling and control
Lecture 4
A more systematic way to build collision free paths
Probabilistic Roadmap (PRM)
§ But very fast algorithms exist that can check if an articulated
object at a given configuration collides with obstacles.
à Basic idea of Probabilistic Roadmaps (PRMs):
Compute a very simplified representation of the free
space by sampling configurations at random.
free space
PhDCourse Robot modeling and control
Lecture 4
Slide from: http://ai.stanford.edu/~latombe/
§ The cost of computing an exact representation of the
configuration space of a multi-joint articulated object is often
prohibitive.
forbidden space
Slide from: http://ai.stanford.edu/~latombe/
Space Ân
PhDCourse Robot modeling and control
Lecture 4
Configurations are sampled by picking coordinates at random
Configurations are sampled by picking coordinates at random
PhDCourse Robot modeling and control
Lecture 4
Slide from: http://ai.stanford.edu/~latombe/
Probabilistic Roadmap (PRM)
Slide from: http://ai.stanford.edu/~latombe/
Probabilistic Roadmap (PRM)
PhDCourse Robot modeling and control
Lecture 4
Sampled configurations are tested for collision (in workspace!)
The collision-free configurations are retained as “milestones”
PhDCourse Robot modeling and control
Lecture 4
Slide from: http://ai.stanford.edu/~latombe/
Probabilistic Roadmap (PRM)
Slide from: http://ai.stanford.edu/~latombe/
Probabilistic Roadmap (PRM)
PhDCourse Robot modeling and control
Lecture 4
Each milestone is linked by straight paths to its k-nearest neighbors
Each milestone is linked by straight paths to its k-nearest neighbors
PhDCourse Robot modeling and control
Lecture 4
Slide from: http://ai.stanford.edu/~latombe/
Probabilistic Roadmap (PRM)
Slide from: http://ai.stanford.edu/~latombe/
Probabilistic Roadmap (PRM)
PhDCourse Robot modeling and control
Lecture 4
The collision-free links are retained to form the PRM
The start and goal configurations are included as milestones
PhDCourse Robot modeling and control
Lecture 4
Comments
§ In industrial robotic applications the path planning
problem is very much left to the user
§ New ideas from mobile robotics (potential field
algorithms, PRMs, etc. could be applied)
§ Automatic planning algorithms highly complex
g
Slide from: http://ai.stanford.edu/~latombe/
The PRM is searched for a path from s to g
PhDCourse Robot modeling and control
Lecture 4
s
PhDCourse Robot modeling and control
Lecture 4
Probabilistic Roadmap (PRM)
s
g
§ The trajectory generation problem can be solved by
applying optimal control techniques
§ Conceptually easy to solve the offline problem
§ Difficult to implement online
PhDCourse Robot modeling and control
Lecture 4
Slide from: http://ai.stanford.edu/~latombe/
Probabilistic Roadmap (PRM)
Slide from: http://ai.stanford.edu/~latombe/
Probabilistic Roadmap (PRM)
Comments
§ Other planning requirements in many applications
Lab session
§ Suggestion
Ÿ Jan 31 – Time 9-15 (?)
Ÿ Explore the possibilities in Rapid/RobotStudio
Ÿ Exercise based on the previous lab but including multiple
frames and also including moving frames (additional
mechanical units)
PhDCourse Robot modeling and control
Lecture 4
Projects
§
§
§
§
Kinematic redundancy
Estimation and control
Modeling and Identification
Path planning and trajectory generation
Ÿ Energy optimal
Ÿ Sensor control (conveyor tracking)
Ÿ …
§ Daignosis
§ ROS – Robot operating system
§ Further develop the robot from the exercises – modeling
and control
§ Further explore the DH parameterization and the
possibility to use it in RobotStudio
§ …
PhDCourse Robot modeling and control
Lecture 4
PhDCourse Robot modeling and control
Lecture 4