Introduction to ROBOTICS
Trajectory Planning
University of Bridgeport
1
Trajectory planning
• A trajectory is a function of time q(t) s.t. q(t0)=qs
And q(tf)=qf .
• tf-t0 : time taken to execute the trajectory.
• Point to point motion: plan a trajectory from the
initial configuration q(t0) to the final q(tf). In some
cases, there may be constraints (for example: if
the robot must begin and end with zero velocity)
Point to point motion
• Choose the trajectory of polynomial of degree
n , if you have n+1 constraints.
Ex (1):Given the 4 constraints: (n=3)
qt0   q0
q t0   v0
qt f   q f
q t f   v f
t0 is thestart time
t f is theend time
q0 and v0 are theinitialpositionand velocity
q f and v f are thefinal positionand velocity
Point to point motion
• Cubic Trajectories
– 4 coefficients (4 constraints)
– Define the trajectory q(t) to be a polynomial of
degree n
qt   a0  a1t  a2t 2  a3t 3
– The desired velocity:
qt   a1  2a2t  3a3t 2
Point to point motion
• Evaluation of the ai coeff to satify the constaints
qt0   q0
q t0   v0
qt0   a0  a1t0  a2t0  a3t0  q0
2
3
qt0   a1  2a2t0  3a3t02  v0
qt f   q f qt f   v f
qt f   a0  a1t f  a2t f  a3t f  q f
2
q t f   a1  2a2t f  3a3t 2f  v f
3
Point to point motion
• Combined the four equations into a single matrix
equation.
1 t0

0 1
1 t f

0 1
t02
2t0
2
tf
2t f
t03  a0   q0 
v 

2 
3t0   a1   0 

3 
t f  a2   q f 
  
2 
3t f   a3   v f 
Point to point motion
Example
q0  10o , q f  20o , v0  v f  0 deg/sec, t0  0, t f  1
1
0

1

0
0 0 0 a0   10 
1 0 0  a1   0 

1 1 1 a2   20
  

1 2 3  a3   0 
1
0

1

0
0
1
1
1
 a0  10, a1  0, a2  90, a3  60
qt   10  90t 2  60t 3
0
0
1
2
0
0
1

3
1
 10  a0 
 0  a 

   1
 20 a2 

  
 0   a3 
Point to point motion
• Cubic polynomial trajectory
qt   10  90t 2  60t 3
• Matlab code:
•
•
•
•
•
•
syms t;
q=10-90*t^2+60*t^3;
t=[0:0.01:1];
plot(t,subs(q,t))
xlabel('Time sec')
ylabel('Angle(deg)')
Point to point motion
• Velocity profile for cubic polynomial trajectory
q t   180t  180t 2
• Matlab code:
•
•
•
•
•
•
syms t;
qdot=-180*t+180*t^2;
t=[0:0.01:1];
plot(t,subs(qdot,t))
xlabel('Time sec')
ylabel(’velocity(deg/s)')
Point to point motion
• Acceleration profile for cubic polynomial
trajectory
qt   180 360t
• Matlab code:
•
•
•
•
•
•
syms t;
qddot=-180+360*t;
t=[0:0.01:1];
plot(t,subs(qddot,t))
xlabel('Time sec')
ylabel(’Acceleration(deg/s2)')
HW 1
• A single link robot with a rotary joint is at
Ө=15ْ degrees. It is desired to move the
joint in a smooth manner to Ө=75ْ in 3
sec. Find the coefficeints of a cubic to
bring the manipulator to rest at the goal.
qt   a0  a1t  a2t 2  a3t 3
q0  15o , q f  75o , v0  v f  0 deg/sec, t0  0, t f  3
HW2
• The task is to take the end point of the RR
robot from (0.5, 0.0, 0.0) to (0.5, 0.3, 0.0) in
the X0Y0Z0 frame in a period of 5 seconds.
Assume the robot is at rest at the starting
point and should come to come to a
complete stop at the final point.
Example 2
• Given the 6 constraints: (n=5)
qt0   q0
q t0   v0
qt0    0
q t f   q f
q t f   v f
qt f    f
t0 is thestart time
t f is theend time
q0 , v0 and  0 are theinitialposition, velocityand acceleration
q f , v f and  f are thefinal position, velocityand acceleration
Point to point motion
• Quintic Trajectories
– 6 coefficients (6 constraints)
– Define the trajectory q(t) to be a polynomial of
degree n
qt   a0  a1t  a2t 2  a3t 3  a4t 4  a5t 5
– The desired velocity:
qt   a1  2a2t  3a3t 2  4a4t 3  5a5t 4
– The desired acceleration:
qt   2a2  6a3t  12a4t 2  20a5t 3
Point to point motion
• Evalautation of the ai coeff to satify the
constaints
q0  a0  a1t0  a2t02  a3t03  a4t04  a5t05
v0  a1  2a2t0  3a3t02  4a4t03  5a5t04
 0  2a2  6a3t0  12a4t02  20a5t03
q f  a0  a1t f  a2t 2f  a3t 3f  a4t 4f  a5t 5f
v f  a1  2a2t f  3a3t 2f  4a4t 3f  5a5t 4f
 f  2a2  6a3t f  12a4t 2f  20a5t 3f
Point to point motion
• Combined the six equations into a single matrix
equation.
1 t0

0 1
0 0

1 t f
0 1

0 0
t02
2t0
t03
3t02
t04
4t03
2
t 2f
2t f
2
6t0 12t02
t 3f
t 4f
3t 2f 4t 3f
2
6t f 12t f
 a0   q0 
   
  a1   v0 
20t03  a2   0 
 
5 
t f   a3   q f 
5t 4f  a4   v f 
   
3 
20t f  a5   f 
t05
5t04
Point to point motion
• Combined the six equations into a single matrix
equation.
1
2
3
4
5
t0
t0
t 0   q0 
a0  1 t0 t0

 a  0 1 2t 3t 2 4t 3
4  
v0 
5
t
1
0
0
0
0



 
 a 2  0 0
2 6t0 12t02 20t03   0 

 
2
3
4
5  
tf
tf
t f  q f 
 a3  1 t f t f
a4  0 1 2t f 3t 2f 4t 3f
5t 4f   v f 
  
  
2
3
2 6t f 12t f 20t f   f 
 a5  0 0
Linear segments with parabolic bends
• We want the middle part of the trajectory
to have a constant velocity V
– Ramp up to V
– Linear segment
– Ramp down