Trajectory Generation
Trajectory Generation
Trajectory refer to a time history of position,
velocity and acceleration for each degree of
freedom.
Trajectory Generation – Problem Definition
Problem
Given: Manipulator geometry, End Effector
via point or Trajectory
Compute: The trajectory of each joint such that
the end effector move in space
from point A to Point B
Solution (Domains)
• Joint space / Cartesian Space
Definitions
• Trajectory (Definition) - Time history of
position, velocity, and acceleration for
each DOF.
• Trajectory Generation – Methods of
computing a trajectory that describes the
desired motion of a manipulator in a
multidimensional space
Trajectory Generation – Problem Definition
• Human Machine interface (requirements)
– Human - Specifying trajectories with simple description of the desired motion
• Example – start / end points position and orientation of the end effectors
– System - Designing the details of the trajectory
• Example – Design the exact shape of the path, duration, joint velocity,
etc.
• Trajectory Representation
– Representation of trajectory in the computer after they were planned
• Trajectory Generation
– Generation occurs at runtime (real time) where positions, velocities, and
accelerations are computed.
– Path update rate 60-2000 Hz
General Consideration
• General approach for the motion of
the manipulator
– Specify the path as a motion of the
tool frame {T} relative to the station
frame {S}. Frame {S} may change it
position in time (e.g. conveyer belt)
• Advantages
– Decouple the motion description
from any particular robot, end
effector, or workspace.
– Modularity – Use the same path with:
• Different robot
• Different tool size
General Consideration
• Basic Problem – Move the tool frame {T}
from its initial position / orientation {T_initial}
to the final position / orientation {T_final}.
• Specific Description
– Via Point – Intermediate points between
the initial and the final end- effector
locations that the end-effector mast go
through and match it position and
orientation along the trajectory.
– Each via point is defined by a frame
defining the position/orintataion of the
tool with respect to the station frame
– Path Points – includes all the via points
along with the initial and final points
– Point (Frame) – Every point on the
trajectory is define by a frame (spatial
description)
General Consideration
• “Smooth” Path or Function
– Continuous path / function with first and second derivatives.
– Add constrains on the spatial and temporal qualities of the path between the
via-points
– Great verity of ways in which paths might be specified or planned.
• Implications of non-smooth path
– Increase wear in the mechanism (rough jerky movement)
– Vibration – exciting resonances.
Joint Space Schemes
• Joint space Schemes – Path shapes (in space
and in time) are described in terms of functions in
the joint space.
• General process (Steps) given initial and target
P/O
1. Select a path point or via point (desired
position and orientation of the tool frame {T}
with respect to the base frame {s})
2. Convert each of the “via point” into a set of
joint angles using the invers kinematics
3. Find a smooth function for each of the n joints
that pass trough the via points, and end the
goal point.
Note 1: The time required to complete each
segment is the same for each joint such
that the all the joints will reach the via point at
the same time. Thus resulting in the position
and orientation of the frame {T} at the via
point.
Note 2: The joints move independently with
only one time restriction (Note 1)
Joint Space Schemes – Cubic Polynomials
• Define a function for each joint such
that value at t 0 is the initial position
of the joint and whose value at t f is
the desire goal position of the joint
• There are many smooth functions (t )
that may be used to interpolate the
joint value.
Joint Space Schemes – Cubic Polynomials
• Problem - Define a function for each joint such that it value at
– t 0 is the initial position of the joint and at
– t f is the desired goal position of the joint
• Given - Constrains on (t )
( 0) 0
(t f ) f
(0) 0
(t f ) 0
• What should be the order of the polynomial function to meet these constrains?
Joint Space Schemes – Cubic Polynomials
• Solution - The four constraints can be satisfied by a polynomial of at least third
degree
(t ) a0 a1t a2t 2 a3t 3
• The joint velocity and acceleration
(t ) a1 2a2t 3a3t 2
(t ) 2a2 6a3t
• Combined with the four desired constraints yields four equations in four
unknowns
( 0) 0 0 a0
(t f ) f f a0 a1t f a2t 2f a3t 3f
(0) 0 0 a1
(t f ) 0 0 a1 2a2t f 3a3t 2f
Joint Space Schemes – Cubic Polynomials
• Solving these equations for the ai we obtain
a0 0
a1 0
3
a2 2
( f 0 )
tf
2
a3 3
( f 0 )
tf
Joint Space Schemes – Cubic Polynomials
• Example – A single-link robot with a rotary joint is motionless at 0 15
degrees. It is desired to move the joint in a smooth manner to f 75 degrees
in 3 seconds. Find the coefficient of the cubic polynomial that accomplish this
motion and brings the manipulator to rest at the goal
(0) 15 a0 0 15
(t f ) 75 a1 0
3 3
(0) 0 a2 2
( f 0 ) (75 15) 20
tf 9
(t f ) 0
2 2
a3 3
( f 0 ) (75 15) 4.44
tf 27
(t ) 15 20t 2 4.44t 3
(t ) 40t 13.33t 2
(t ) 40 26.66t
Joint Space Schemes – Cubic Polynomials
• The velocity profile of any cubic function is a
parabola
• The acceleration profile of any cubic function is
linear
Joint Space Schemes – Cubic Polynomials – Via Points
• Previous Method - The manipulator comes to rest at each via point
• General Requirement - Pass through a point without stopping
• Problem - Define a function for each joint such that it value at
– t 0 is the initial position of the joint and at
– t is the desire goal position of the joint
f
• Given - Constrains on (t ) such that the velocities at the via points are not zero
but rather some known velocities
( 0) 0
(t f ) f
(0) 0
(t f ) f
Joint Space Schemes – Cubic Polynomials – Via Points
• Solution - The four constraints can be satisfied by a polynomial
(t ) a0 a1t a2t 2 a3t 3
(t ) a1 2a2t 3a3t 2
(t ) 2a2 6a3t
• Combined with the four desired constraints yields four equations in four
unknowns
( 0) 0 0 a0
(t f ) f f a0 a1t f a2t 2f a3t 3f
(0) 0 0 a1
(t f ) f f a1t f 2a2t f 3a3t 2f
Joint Space Schemes – Cubic Polynomials – Via Points
• Solving these equations for the ai we obtain
a0 0
a
1 0
3 2 1
a2 ( f 0 ) 0 f
t 2f tf tf
2 2
a3 3
( f 0 ) 2
( f 0 )
tf tf
• Given - velocities at each via point are
• Solution - Apply these equations for each segment of the trajectory.
Joint Space Schemes – Cubic Polynomials – Via Points
• Specify the desired velocities at each segment:
1. User Definition – Desired Cartesian linear and angular velocity of the tool
frame at each via point.
2. System Definition – The system automatically chooses the velocities
(Cartesian or angular) automatically using a suitable heuristic method.
3. System Definition – The system automatically chooses the velocities
(Cartesian or angular) to cause the acceleration at the via point to be
continuous.
Joint Space Schemes – Cubic Polynomials – Via Points
1. User Definition – Desired Cartesian linear and angular velocity of the tool
frame at each via point.
– Mapping (Cartesian Space to Joint Space) - Cartesian velocities at the via
point are “mapped” to desired joint rates by using the inverse Jacobian
f J 1 X f
– Singularity - If the manipulator is at a singular point at a particular via point
then the user is not free to choose an arbitrarily velocity at this point.
– Difficult
Joint Space Schemes – Cubic Polynomials – Via Points
2. System Definition – The system automatically chooses the velocities
(Cartesian or angular) using a suitable heuristic method given a trajectory .
Heuristic method
– Consider a path defined by via points
– Connect the via points with straight
lines
– If the slope change sign
– Set the velocity at the via point to
be zero
– If the slope have the same sign
– Calculate the average between the
to velocities at the via point.
Joint Space Schemes – Cubic Polynomials – Via Points
3. System Definition – The system automatically chooses the velocities
(Cartesian or angular) to cause the acceleration at the via point to be
continuous.
– Spline - Enforcing the velocity and the acceleration to be continuous at the
via point
Cubic Polynomials – Via Points – Example
• Solve for the coefficients of two cubic functions that are connected in a two
segment spline with a continuous acceleration at the intermediate via point.
(t ) a10 a11t a12t 2 a13t 3
(t ) a20 a21t a22t 2 a23t 3
(t ) a11 2a12t 3a13t 2
(t ) a21 2a22t 3a23t 2
(t ) 2a12 6a13t
(t ) 2a22 6a23t
Cubic Polynomials – Via Points – Example
• The joint angle velocity and acceleration for each segment (8 unknowns )
(t ) a10 a11t a12t 2 a13t 3
(t ) a20 a21t a22t 2 a23t 3
(t ) a11 2a12t 3a13t 2
(t ) a21 2a22t 3a23t 2
(t ) 2a12 6a13t
(t ) 2a22 6a23t
Cubic Polynomials – Via Points – Example
• Position at the beginning and end of each segment
• Segment 1
0 (t 0) a10
via (t t f ) a10 a11t f a12t f 2 a13t f 3
1 1 1 1
• Segment 2
via (t 0) a20
g (t t f ) a20 a21t f a22t f 2 a23t f
2 2 2 2
3
Cubic Polynomials – Via Points – Example
• Velocity at the beginning of the interval
(t 0) a11
• Velocity at the end of the interval
(t t f ) a21 2a22t f 3a23t f
2 2 2
2
• Velocity at the mid point between the intervals
Function1(t t f ) Function 2(t 0)
1
a11 2a12t f1 3a13t f1 a21
2
Cubic Polynomials – Via Points – Example
• Acceleration at the mid point between the intervals
Function1(t t f ) Function 2(t 0)
1
2a12 6a13t f1 2a22
• Solve 8 equations with 8 unknown
Cubic Polynomials – Via Points – Example
• Solution for the 8 equations
a10 0
a11 0
12 v 3 g 9 0
a12
4t 2f
8 v 3 g 5 0
a13
4t 3f
a10 v
3 g 3 0
a21
4t f
12 v 6 g 6 0
a22
4t 2f
8 v 5 g 3 0
a23
4t 3f
Joint Space Schemes – High Order Polynomials
• Problem - Define a function for each joint such that it value at
– t 0 is the time at the initial position
– t f is the time at the desired goal position
• Given - Constrains on the position velocity and acceleration at the beginning and
the end of the path segment
( 0) 0
(t f ) f
(0) 0
(t f ) f
(0) 0
(t f ) f
• What should be the order of the polynomial function to meet these constrains?
Joint Space Schemes – High Order Polynomials
• Solution - The six constraints can be satisfied by a polynomial of at least fifth
order
(t ) a0 a1t a2t 2 a3t 3 a4t 4 a5t 5
• Combined with the six desired constraints yields six equations with six unknowns
( 0) 0 0 a0
(t f ) f f a0 a1t f a2t 2f a3t 3f a4t 4f a5t 5f
(0) 0 0 a1
(t f ) f f a1 2a2t f 3a3t 2f 4a4t 3f 5a5t 4f
(0) 0 0 2a1
(t f ) f f 2a2 6a3t f 12a4t 2f 20a5t 3f
Joint Space Schemes – Cubic Polynomials
• Solving these equations for the ai we obtain
a0 0
a
1 0
0
a2
2
20 f 20 0 (8 f 12 0 )t f (3 0 f )t 2f
a3
2t 3f
30 0 30 f (14 f 16 0 )t f (3 0 2 f )t 2f
a4
2t 4f
12 f 12 0 (6 f 6 0 )t f ( 0 f )t 2f
a5
2t 5f
Cartesian Space Schemes – Introduction
• Joint Space Schemes
– Advantages –
• Path go through all the via and goal points
• Points can be specified by Cartesian frames.
– Disadvantages -
• End effector moves along a curved line (not a straight line - shortest
distance).
• Path depends on the particular joint kinematics of the manipulator
•
Cartesian Space Schemes – Introduction
• Cartesian Space Scheme
– Advantage
• Most common path is straight line (shortest). Other shapes can also be
used.
– Disadvantage
• Computationally expansive to execute – At run time the inverse
kinematics needs to be solved at path update rate (60-2000 Hz)
Control of Manipulators
Introduction – Problem Definition
Problem
Given: Joint angles (sensor readings) links
geometry, mass, inertia, friction, Direct
/inverse kinematics & dynamics
Compute: Joint torques to achieve an end
effector position / trajectory
Solution
Control Algorithm (PID - Feedback loop, Feed
forward dynamic control)
Robot Manipulator Control
D (q)q H (q, q ) C (q)
• Robot System:
Y h( q )
e qd q
q q
• Joint Level Controller Trajectory q d q d e e tor
Controller Robot
Planner qd _
Find a control input , q qd as t
• Task Level Controller
e Yd Y
Task level Yd Yd e e tor Robot q q Forward Y Y
Controller
Planner _
Dynamics Kinematics
Find a control input Y Yd as t e Yd Y 0
Robot Manipulator Control
• Control Methods
– Conventional Joint PID Control
• Widely used in industry
– Advanced Control Approaches
• Computed torque approach
• Nonlinear feedback
• Adaptive control
• ….
