Transcript Trajectory
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)
qt0 q0
q t0 v0
qt 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
qt a0 a1t a2t 2 a3t 3
– The desired velocity:
qt a1 2a2t 3a3t 2
Point to point motion
• Evaluation of the ai coeff to satify the constaints
qt0 q0
q t0 v0
qt0 a0 a1t0 a2t0 a3t0 q0
2
3
qt0 a1 2a2t0 3a3t02 v0
qt f q f qt f v f
qt 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
qt 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
qt 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
qt 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.
qt 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)
qt0 q0
q t0 v0
qt0 0
q t f q f
q t f v f
qt 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
qt a0 a1t a2t 2 a3t 3 a4t 4 a5t 5
– The desired velocity:
qt a1 2a2t 3a3t 2 4a4t 3 5a5t 4
– The desired acceleration:
qt 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