robotics - mnrlab · 2020. 1. 23. · 4 1 t 0 t2 0 t 3 0 0 1 2t2 3t2 0 1 t f t2 f t 3 f 0 1 2t f...

Robotics

Islam S. M. Khalil

German University in Cairo

Islam S. M. Khalil Trajectory Planning

Trajectory for Point to point Motion

Figure: Difference between joint and task spaces.


Trajectory for Point to point Motion

The problem is to find a trajectory that connects the initial andfinal configurations while satisfying other specified constraints atthe end points such as velocity and/or acceleration constraints.Suppose that at time t0 the joint variables satisfies

q(t0) = q0 , q̇(t0) = v0

and we wish to attain the valuesat tf

q(tf ) = qf , q̇(tf ) = vf

We may also wish to specify theconstrains on the initial and finalaccelerations

q̈(tf ) = α0 , q̈(tf ) = αf

Figure: A typical joint spacetrajectory.


Cubic Polynomial Trajectories

Consider firs the case where we wish to generate a polynomial jointtrajectory between two configurations, and that we wish to specifythe start and end velocities for the trajectory. This gives 4constraints the trajectory must satisfy. Therefore, we require apolynomial with four independent coefficients that can be chosento satisfy the constraints.

Therefore, we consider a cubictrajectory of the following form:

q(t) = a0 + a1t + a2t2 + a3t

3

Then the desired velocity is givenby

q̇(t) = a1 + 2a2t + 3a3t2

Figure: A typical joint spacetrajectory.



Combining equations yields

q0 = a0 + a1t0 + a2t20 + a3t

30

v0 = a1 + 2a2t0 + 3a3t20

qf = a0 + a1tf + a2t2f + a3t

3f

vf = a1 + 2a2tf + 3a3t2f

These equations can be combined into asingle matrix equation

1 t0 t20 t300 1 2t2 3t201 tf t2f t3f0 1 2tf 3t2f

a0a1a2a3

=

q0v0qfvf

The determinant of thecoefficient matrix is(tf − t0)4, and hence wehave a unique solutionprovided a nonzero timeinterval is allowed for theexecution of the trajectory.

Figure: A typical joint spacetrajectory.Islam S. M. Khalil Trajectory Planning


As an illustrative example, we mat consider the special case thatthe initial and final velocities are zero. Suppose we take t0 = 0 andtf = 1 s, with

v0 = 0 , vf = 0

Therefore, we need to move the initial position q0 to the finalposition qf in 1 second, starting and ending with zero velocity.

1 0 0 00 1 0 01 1 1 10 1 2 3

a0a1a2a3

=

q00qf0

q0 = a0

0 = a1

qf − q0 = a2 + a3

0 = 2a2 + 3a3The latter two equations can be solved and yield

a2 = 3(qf − q0) , a3 = −2(qf − q0)



The required cubic polynomial function is therefore

q(t) = q0 + 3(qf − q0)t2 − 2(qf − q0)t3

the corresponding velocity and acceleration curves are

q̇(t) = 6(qf − q0)t − 6(qf − q0)t2

q̈(t) = 6(qf − q0)− 12(qf − q0)t

Figure: Trajectories with q0 = 10 deg and qf = −20 deg.


Quintic Polynomial Trajectories

The cubic trajectory gives continuous positions and velocities atthe start and finish points times but discontinues in theacceleration. The derivative of acceleration is called the jerk. Adiscontinuity in acceleration leads to an impulsive jerk, which mayexcite vibrational modes in the manipulator and reduce trackingaccuracy.

Figure: Trajectories with q0 = 10 deg and qf = −20 deg.

For this reason, one may wish to specify constraints on theacceleration as well as on the position and velocity.



In this case, we have six constraints. on each initial and finalconfiguration, initial and final velocity, and initial and finalacceleration. Therefore, we require a fifth order polynomial.

q(t) = a0 + a1t + a2t2 + a3t

3 + a4t4 + a5t

5

we obtain

q0 = a0 + a1t0 + a2t20 + a3t

30 + a4t

40 + a5t

50

v0 = a1 + 2a2t0 + 3a3t20 + 4a4t

30 + 5a5t

40

α0 = 2a2 + 6a3t0 + 12a4t20 + 20a5t

50

qf = a0 + a1tf + a2t2f + a3t

3f + a4t

4f + a5t

5f

vf = a1 + 2a2tf + 3a3t2f + 4a4t

3f + 5a5t

4f

αf = 2a2 + 6a3tf + 12a4t2f + 20a5t

5f



which can be written as

1 t0 t20 t30 t40 t500 1 2t0 3t20 4t30 5t400 0 2 6t0 12t20 20t301 tf t2f t3f t4f t5f0 1 2tf 3t2f 4t3f 5t4f0 0 2 6tf 12t2f 20t3f

a0a1a2a3a4a5

=

q0v0α0

qfvfαf

Figure: Trajectories with q0(0) = 10 deg and qf (2) = 40 deg.



We have to observe that the this method does not provideconstant velocity profile along the path.

Figure: Velocity profile with q0(0) = 10 deg and qf (2) = 40 deg.


Linear Segments with Parabolic Blends (LSPB)

Another way to generate suitable joint space trajectories is byusing so-called Linear Segments with Parabolic Blends(LSPB). This type of trajectory has a Trapezoidal VelocityProfile and is appropriate when a constant velocity is desired alonga portion of the path. It consists of 3 parts:

t0 → tb: Quadratic polynomial andthis results in a linear rampvelocity.

At tb: At the blend time, switchto linear function and thiscorresponds to constant velocity.

tb → tf : The trajectory switch toquadratic polynomial so that thevelocity is linear.

Figure: Blend times for LSPBtrajectory.



we choose the blend time so that the positron curve is symmetric.Suppose that t0 = 0 and q̇(tf ) = 0. Then between times 0 and tbwe have

q(t) = a0 + a1t + a2t2

so that the velocity is

q̇(t) = a1t + 2a2t

The constraints q0 = 0 and q̇0 = 0 imply that

a0 = q0

a1 = 0

A time tb we want the velocity to equal agiven constant, say V . Thus, we have

q̇(tb) = 2a2tb = V → a2 = V /2tb

Figure: Blend timesfor LSPB trajectory.



Therefore the required trajectorybetween 0 and tb is give by

q(t) = q0 +V

2tbt2 = q0 +

α

2t2

q̇(t) =V

tbt = αt

q̈(t) =V

tb= α

where α denotes the acceleration. Figure: Blend times for LSPBtrajectory.



Now, between time tb and tf − tb, the trajectory is a linearsegment with velocity V

q(t) = q(tb) + V (t − tb)

Since, by symmetry,

q(tf2

) =q0 + qf

2we have

q0 + qf2

= q(tb) + V (tf2− tb)

which implies that

q(tb) =q0 + qf

2− V (

tf2− tb)

Since the two segments must blend at time tb we require

q0 +V

2tb =

q0 − qf + Vtf2

+ Vtb



which, upon solving for the blend time gives

tb =q0 − qf + Vtf

V

Note that we have the constraint 0 < tb <tf2 . This leads to the

inequalityqf − q0

V≤ tf ≤

2(qf − q0)

V

to put it another way we have the inequality

qf − q0tf

≤ V ≤ 2(qf − q0)

tf

Thus, the specified velocity must be between these limits or themotion is not possible.



The complete LSPN trajectory is given by

q(t) =

q0 + α

2 t2, 0 ≤ t ≤ tb;

q0−qf +Vtf2 + Vt, tb ≤ t ≤ tf − tb;

qf −αt2f2 −

α2 t

2, tf − tb < t ≤ tf .

Figure: Blend times for LSPB trajectory for V=60 and tb=1/3.



Figure: Cubic, quintic, and LSPB trajectory planning.


Thanks

Questions please


robotics - mnrlab · 2020. 1. 23. · 4 1 t 0 t2 0 t 3 0 0 1 2t2 3t2 0 1 t f t2 f t 3 f 0 1 2t f...

Documents