Duvedi R. K., Robotics-Trejectory Planing

7/28/2019 Duvedi R. K., Robotics-Trejectory Planing

1/38

1

Robotics:Trajectory Planning

Ravinder Kumar DuvediAssistant Professor

Mechanical Engineering DepartmentThapar University, Patiala

18.03.2013 RKD, TU


2/38

2

Outline

Review

Kinematics Model

Inverse Kinematics Example

Jacobian Matrix

Singularity

Trajectory Planning


3/38

Definitions & Planning

The user inputs a list ofparameters andconstraints describing thedesired trajectory

The trajectory plannergenerates a time

sequence of intermediateconfigurations expressedin either joint orCartesian space

Trajectory Planning Problem

3


4/38

Terminology

Path: A path is the locus of points (either in the joint space or in the Cartesian space)

to be traversed by the manipulator to execute the specified task A path is a purely geometric (spatial) description of the motion

Trajectory: A trajectory is a path with specified qualities of motion, that is, a path on which a

time law is specified in terms of velocities and / or accelerations at each point

Thus, a trajectory is the time sequence (or time history) of position, velocity andacceleration for each joint or end-effector of the manipulator. A trajectory is botha spatial and temporal representation of motion. It can be specified either in jointspace or in Cartesian space

Cartesian Space Trajectory Planning: In Cartesian space trajectory planning, the path is explicitly specified in the

Cartesian space The path constraints (velocity, acceleration, etc.) are specified in Cartesian

coordinates and the joint actuators are served in joint coordinates to the specifiedtrajectory

Path Update Rate: The rate at which the trajectory points are computed at run time is called path

update rate 4


5/38

Terminology

Knot Points or Via points: Knot points or via points or interpolation points are the set of intermediate

locations between the start and goal points on the trajectory through which themanipulator must pass enroute to the destination

Joint Space Trajectory Planning: In joint space trajectory planning each path point is specified in terms of a

desired position and orientation of the end-effector frame relative to the baseframe

Each of these points is converted into a set of desired joint positions by

application of inverse kinematics A smooth function is then found for each of the joints, which passes throughthese points

5


6/38

6

Review Steps to derive kinematics model:

Assign D-H coordinates frames

Find link parameters

Transformation matrices of adjacent joints

Calculate kinematics matrix

When necessary, Euler angle representation


7/387

Denavit-Hartenberg Convention Number the joints from 1 to n starting with the base and ending with the

end-effector.

Establish the base coordinate system. Establish a right-handed orthonormalcoordinate system at the supporting base with axis lying along

the axis of motion of joint 1. Establish joint axis. Align the Zi with the axis of motion (rotary or sliding) of

joint i+1.

Establish the origin of the ith coordinate system. Locate the origin of the ithcoordinate at the intersection of the Zi & Zi-1 or at the intersection of

common normal between the Zi & Zi-1 axes and the Zi axis. Establish Xi axis. Establish or along the common

normal between the Zi-1 & Zi axes when they are parallel.

Establish Yi axis. Assign to complete the right-handed coordinate system.

Find the link and joint parameters

),,( 000 ZYX

iiiii ZZZZX = 11 /)(

iiiii XZXZY += /)(

0Z


8/388

Link and Joint Parameters Joint angle : the angle of rotation from the Xi-1 axis to the Xi axis

about the Zi-1

axis. It is the joint variable if joint i is rotary.

Joint distance : the distance from the origin of the (i-1) coordinatesystem to the intersection of the Zi-1 axis and the Xi axis along theZi-1 axis. It is the joint variable if joint i is prismatic.

Link length : the distance from the intersection of the Zi-1 axis

and the Xi axis to the origin of the ith

coordinate system along theXi axis.

Link twist angle : the angle of rotation from the Zi-1 axis to the Ziaxis about the Xi axis.

i

id

ia

i

Review


9/389

REVIEW

D-H transformation matrix for adjacent coordinate

frames, i and i-1.

The position and orientation of the i-th frame coordinatecan be expressed in the (i-1)th frame by the following 4

successive elementary transformations:

=

=

1000

0

),(),(),(),( 111

iii

iiiiiii

iiiiiii

iiiiiiii

i

i

dCS

SaCSCCS

CaSSSCC

xRaxTzRdzTT

Source coordinate

Reference

Coordinate


10/3810

REVIEW

Kinematics Equations

chain product of successive coordinate transformationmatrices of

specifies the location of the n-th coordinate frame

w.r.t. the base coordinate system

=

=

=

100010

000

1

2

1

1

00

nnn

n

n

n

PasnPR

TTTT

i

iT 1nT0

Orientation

matrix

Positionvector


11/3811

REVIEW

Forward Kinematics

z

y

x

p

p

p

6

5

4

3

2

1

=

1000

zzzz

yyyy

xxxx

pasn

pasn

pasn

T

Kinematics Transformation

Matrix

+

+

++

==

yandxfor

yandxfor

yandxfor

yandxfor

xya

090

90180

18090

900

),(2tan

Why use Euler angle representation?

),(2tan xyaWhat is ?


12/38

12

REVIEW

=

1000

0

00

0

CCSCS

SC

CSSSC

,,, xyz RRRT = ,,1, xyz RRTR =

1000

0100

00

00

CS

SC

=

1000

00

0010

00

CS

SC

1000

00

00

0001

CS

SC

1000

0

0

0

zzz

yyy

xxx

asn

asn

asn

(Equation A)

Yaw-Pitch-Roll Representation

+++

+

1000

0

0

0

XXXXn

aCaSsCsSnCnS

XXXXnSnC

z

yxyxyx

yx


13/38

13

REVIEW

0cossin =+ yx nn

==+

sincossincos

z

yx

nnn

=+

=+

sincossin

coscossin

yx

yx

aa

ss

Compare LHS and RHS of Equation A, we have:

),(2tan xy nna=

)sincos,(2tan yzz nnna +=

)cossin,cos(sin2tan yxyx ssaaa +=


14/38

14

INVERSE KINEMATICS

6

5

4

3

2

1

6

5

5

4

4

3

3

2

2

1

1

0

6

0

1000

TTTTTTpasn

pasn

pasn

Tzzzz

yyyy

xxxx

=

=

Transformation Matrix

Special cases make the closed-form arm solution possible:

1. Three adjacent joint axes intersecting (PUMA, Stanford)

2. Three adjacent joint axes parallel to one another (MINIMOVER)

Robot dependent, Solutions not unique

Systematic closed-form solution in general is not available


15/38

15

EXAMPLE

Solving the inverse kinematics of Stanford arm

6

5

5

4

4

3

3

2

2

1

1

0

6

0

1000

TTTTTTpasn

pasn

pasn

Tzzzz

yyyy

xxxx

=

=

6

1

6

5

5

4

4

3

3

2

2

1

6

0

11

0 )( TTTTTTTT ==

+

+

=

1000

11

11

6

1

yx

z

yx

pCpSXXX

pXXXpSpCXXX

T

=

1000

1.0

32

32

XXX

dCXXXdSXXX


16/38

16

EXAMPLE


3211 sinsincos dpp yx =+

1.0cos 11 =+ yx ppSin

32cos dpz =

Equation (1)

Equation (2)

Equation (3)

In Equ. (1), let

)(2tan,,sin,cos 22

x

y

yxyxp

papprrprp =+===

=

==

21

1

11

)/1.0(1)cos(

1.0)sin(1.0cossincossin

r

r

r

)1.0

1.0(2tan)(2tan

221

=

ra

p

pa

x

y

)sincos

(2tan11

2

z

yx

p

ppa

+=

2

3

cos

zpd =


17/38

17

EXAMPLE


))(

(2tan

2112

11

4

zyx

yx

aSaSaCC

aCaSa

+

+=

==

1000

00

0

0

)()()()(5

5

6

5

5

4

6

0

11

0

12

1

13

2

14

3XX

CXX

SXX

TTTTTTT

0)(])([ 11421124 =+++ yxzyx aCaSCaSaSaCCS

++=

+++=

))(

)(])((

21125

114211245

zyx

yxzyx

aCaSaCSC

aCaSSaSaSaCCCS

From term (3,3)

From term (1,3), (2,3)

)(2tan5

55

C

Sa=


18/38

18

EXAMPLE


+++=

++++++=

)(])([

])([)}(])([{

114211246

211251142112456

yxzyx

zyxyxzyx

sCsSCsSsSsCCSC

sCsSsCSSsCsSSsSsSsCCCCS

==

1000

0100

00

00

)()()()()(66

66

6

5

6

0

11

0

12

1

13

2

14

3

15

4

CS

SC

TTTTTTT

)(2tan6

66

C

Sa=


19/38

19

JACOBIAN MATRIX

6

5

4

3

2

1

6

5

4

3

2

1

z

y

x

J oint Space Task Space

Forward

Inverse

Kinematics

J acobian Matrix: Relationship between jointspace velocity with task space velocity

z

y

x

z

y

x

J acobian

Matrix


20/38

20

JACOBIAN MATRIX

z

y

x

16

6

5

4

3

2

1

)(

=

q

q

q

q

q

q

h

166216

6215

6214

6213

6212

6211

),,,(

),,,(

),,,(

),,,(

),,,(

),,,(

=

qqqh

qqqh

qqqh

qqqh

qqqh

qqqh

Forward kinematics

)( 116 = nqhY

qdq

qdh

dt

dq

dq

qdhqh

dt

dY n )()()( 116 ===

z

y

x

z

y

x

1

2

1

6

)(

=

nn

n

q

q

q

dq

qdh

1616 = nnqJY

dq

qdhJ

)(=


21/3821

JACOBIAN MATRIX

z

y

x

z

y

x

1

2

1

6

)(

=

nn

n

q

q

q

dq

qdh

nn

n

n

n

q

h

q

h

q

h

q

h

q

h

q

h

q

h

q

h

q

h

dqqdhJ

=

=

6

6

2

6

1

6

2

2

2

1

2

1

2

1

1

1

6

)(

J acobian is a function ofq, it is not a constant!


22/38

22

JACOBIAN MATRIX

44

6

01000

=

pasnT

=

=

)(

)(

)(

3

2

1

qh

qh

qh

z

y

x

p

=

)(

)(

)(

)(

)(

)(

},,{

6

5

4

qh

qh

qh

q

q

q

asn

=

=Vz

y

x

Y

z

y

x

Forward Kinematics

=

z

y

x

V

=

==

)(

)(

)(

)(

6

2

1

16

qh

qh

qh

qhY

1616 = nnqJY

Linear velocity Angular velocity


23/38

23

EXAMPLE

2-DOF planar robot arm

Given l1, l2,Find: Jacobian 2

1

(x , y)

l2

l1

=

++

++=

),(

),(

)sin(sin

)cos(cos

212

211

21211

21211

h

h

ll

ll

y

x

+++

++=

=)cos()cos(cos

)sin()sin(sin

21221211

21221211

2

2

1

2

2

1

1

1

lll

lll

hh

hh

J

=

=

2

1

J

y

xY


24/38

24

JACOBIAN MATRIX

+++

+++

++++++

+++

+++

=

666262161

656252151

646242141

636232131

626222121

616212111

qJqJqJ

qJqJqJ

qJqJqJqJqJqJ

qJqJqJ

qJqJqJ

z

y

x

==

666261

262221

161211

JJJ

JJJ

JJJ

qJY

6

5

4

3

2

1

qq

q

q

q

q

Physical Interpretation

How each individual jointspace velocity contributeto task space velocity.


25/3825

JACOBIAN MATRIX

Inverse Jacobian

Singularity

rank(J)


26/38

26

EXAMPLE

Find the singularity configuration of the 2-DOF

planar robot arm

+++++=

)cos()cos(cos

)sin()sin(sin

21221211

21221211

lll

lllJ

=

=

2

1

J

y

xY

2

1

(x , y)

l2

l1

x

Y

=0

V

determinant(J )=0 Not full rank

02 =

Det(J )=0


27/38

27

JACOBIAN MATRIX

Pseudo-inverse

Let A be an mxn matrix, and let be the pseudo-inverseof A. If A is of full rank, then can be computed as:

Example:

+A

+A

=

=

+

nmAAAnmA

nmAAA

ATT

TT

1

1

1

][

][

=

2

3

011

201

x

=

==

+

24

51

41

9

1

21

15

02

10

11

][

1

1TT AAAA

TbAx ]16,13,5[9/1 == +


28/38

28

ROBOT MOTION PLANNING

Path planning

Geometric path

Issues: obstacle avoidance, shortest path

Trajectory planning,

interpolate or approximate the desiredpath by a class of polynomial functionsand generates a sequence of time-basedcontrol set points for the control ofmanipulator from the initial configurationto its destination.

Task Plan

Action Plan

Path Plan

TrajectoryPlan

Controller

Sensor

Robot

Tasks


29/38

THREE STEPS IN TRAJECTORY PLANNING1. Task Description: Identify the kind of motion required, which can be grouped into

following three categories

I. Point-to-Point (PTP) motion: The task is specified as initial and final end effectorlocation such as pick and place operation

No particular specification about the intermediate locations of the endeffector is givenand the planner is free to formulate any convenient path.

II. Continuous path (CP) motion: In addition to start and finish points, a specific path

between them is required to be traced by the endeffector in Cartesian space such asarc welding and plotting

In such cases, user specifies the type and parameters of the path to be tracked

In a third type of task description,Where more than two points of the path are specifiedsuch as in pick-and-place operation where obstacles are present in the path. Here, the

first point on the path is called the initial point, while the last point is the goalpoint. The intermediate locations are the via points. Together, these are referred to aspath points.

29


30/38

THREE STEPS IN TRAJECTORY PLANNING2. Selecting and Employing a Trajectory Planning Technique

The various trajectory-planning techniques fall into two categories:

Joint space techniques or

Cartesian space techniques

In the case of point-to-point motion (with or without via points), joint spacetechniques are employed in which motion planning is done at the joint level

The joint-space planning schemes generate time-dependent functions (time

history) of all joint variables and their first two derivatives to describe the desiredmotion of the manipulator

For applications requiring continuous path motion, Cartesian space techniques areused

The Cartesian space-planning techniques provide time history of the location,velocity and acceleration of the end-effector with respect to the base. Thecorresponding joint variables and their derivatives are computed, using inversekinematics.

30


31/38

THREE STEPS IN TRAJECTORY PLANNING

3. Computing Trajectory

The final step is to compute the time sequence of values attained by thefunctions generated from the trajectory planning technique. These valuesare computed at a particular path update or sampling rate.

The path update rate in real time, lies between 20 Hz and 200 Hz in typical

industrial manipulator systems

31


32/38

32

TRAJECTORY PLANNING

sequence of control set pointsalong desired trajectory

(continuity,smoothness)

TrajectoryPlanner

Pathconstraints

Pathspecification

)}(),(),({ tqtqtq

)}(),(),({ tatvtp

joint space

cartesian space

or


33/38

33

TRAJECTORY PLANNING

Path Profile

Velocity Profile

Acceleration Profile

t0 t1 t2 tf Time

q(t0)

q(t1)

q(t2)q(tf)

Initial

Lift-off

Set down

Final

J oint i

t0 t1 t2 tf Time

Speed

t0 t1 t2 tf Time

Acceleration


34/38

34

THE BOUNDARY CONDITIONS

1) Initial position

2) Initial velocity3) Initial acceleration

4) Lift-off position

5) Continuity in position at t16) Continuity in velocity at t

17) Continuity in acceleration at t18) Set-down position

9) Continuity in position at t210) Continuity in velocity at t2

11) Continuity in acceleration at t212) Final position

13) Final velocity

14) Final acceleration


35/38

35

REQUIREMENTS

Initial Position

Position (given)

Velocity (given, normally zero)

Acceleration (given, normally zero)Final Position

Position (given)

Velocity (given, normally zero)

Acceleration (given, normally zero)


36/38

36

REQUIREMENTS

Intermediate positions

set-down position (given)set-down position (continuous with previous trajectory

segment)Velocity (continuous with previous trajectory segment)Acceleration (continuous with previous trajectory segment)

Lift-off position (given)Lift-off position (continuous with previous trajectory

segment)

Velocity (continuous with previous trajectory segment)Acceleration (continuous with previous trajectory segment)


37/38

37

TRAJECTORY PLANNING

n-th order polynomial, must satisfy n+1 conditions,

13-th order polynomial

need 14 conditions, for solution.

4-3-4 trajectory

3-5-3 trajectory

0012

2

13

13 =++++ atatata

02

2

2

3

3

4

4

2021

2

22

3

232

1012

2

12

3

13

4

141

)(

)(

)(

nnnnnn atatatatath

atatatath

atatatatath

++++=

+++=

++++= t0t1, 5 unknown

t1t2, 4 unknown

t2tf, 5 unknown


38/38

ThankYou

Documents

Duvedi R. K., Robotics-Trejectory Planing