robot motion analysis - kinematicsczhou/motion.pdf · robot motion analysis - kinematics kinematics...

Robot Motion Analysis - Kinematics

Kinematics AnalysisWe are made of bones, muscles and senses. We control using muscles and measure

with senses: touch, vision, etc. Robots are built with links and joints in variousconfigurations. Robot without intelligence can only control and measure the jointsdirectly, such as rotate joint 1 for 300 pulses. We call this joint coordinates (you can alsoconsider angles).

To accomplish a task in an application, we need to control the position andorientation in various coordinate systems such as world, work piece to tool. The primitiverobot does not know the relationships between joint coordinates and other coordinatesystems. It is very difficult to be used in applications. That separates a toy robot from anindustrial robot.

In order for a robot to go to certain place at certain orientation conveniently, it isnecessary to know the relationship between the joint coordinate system and some othersystems, such as base or tool systems.

Coordinate systems (mostly based on ANSI R15-1)Joint coordinate system: defined in each joint (θ1, θ2, … θN). For rotational joint, it canbe angles. The direct joint parameters can also be pulses or encoder counts.World coordinate system: A Cartesian coordinate system (X, Y, Z, A, B, Z) witharbitrary location.Base coordinate system: A Cartesian coordinate system (X0, Y0, Z0, A0, B0, Z0), with itsorigin at the base of the robot mounting plate. For robot with rotational first joint, the z-axis is vertical coincide with first joint (waist rotation). For robot with linear first joint, theorigin is at the intersection of midpoint of travel of the fist axis and mounting surface. Inaddition,

X0 : from origin to projection of center point of the work envelop.Z0 : from origin away from base plate.

joint values

θ1, θ2… , θN,(x, y, z, y, p, r, ..)

direct kinematics

inverse kinematics

joint typeslink dimentions

Chen Zhou, motion.doc November 3, 1999

3

The solution to inverse kinematics may not be unique. For example, a SCARA robot canreach most positions within the working envelope in two possible ways:

When you solve the inverse, there will be two solutions. When there are possible multiplesolutions, industrial robots are often designed with default and can be modified.

For a robot that has more than 6 dof, the solution to the inverse kinematics can bea continuous function. Theoretically, there are infinite solutions.

Inverse kinematics is performed by an industrial robot often also. For example, youinstruct the robot to move to location P1 defined in base coordinate system using jointinterpolation. In order to perform joint interpolation, a robot must know the increment foreach joint in joint coordinate. Therefore, the robot must first find the joint valuescorrespond to P1.

(θ11, θ21, … θN1) = f--1(x1, y1, z1, y1, p1, r1)

It then find the difference between the current point P0 and P1 to perform jointinterpolation.

f and f-1 is not too complicated for a Cartesian robot because the translation can bedecomposed from orientation. This is the major reason why the control for Cartesianrobot is simpler, But f is rather complex for other configurations. The inverse can be evenmore complex. An arbitrary design may not even have a closed form solution.


4

Background

Right hand coordinate systemxyz: model

Matrix

Square matrix: number of rows = no. of col.

Column matrix: one col.

Row matrix: one row.

Identity (unit vector)

Orientation and unit vector

Orientation of a vector can be expressed with a unit vector. A unit vector is a vector withmagnitude of “1”. It is used to indicate directions. In 3-D,

x y z2 2 2 1+ + =e.g.

(1, 0, 0), along x(-.5, 0, .5) is not a unit vector(.707, 0, -.707) is between x and -z in xz plane.

13

13

13

, ,

is between x, y and z.

TransposeA’: columns and rows are exchanged.

EqualityEach element is the same.

Addition and Substraction

Association: (A + B) + C = A + (B + C)

MultiplicationNo of col in the 1st = no of rows in the 2nd.A(B + C) = AB + ACABC = A(BC)AB <> BA


5

Inverse

Dot product

The dot product of two vectors is a scalar u * v, u = (ux, uy, uz), v = (vx, vy, vz)w = u * v = ux vx + uy vy + uz vz

Cross product

The cross product of two vectors is a vectorTwo non-parallel vectors u and v can be moved such that their initial points

coincide, w = u × v is also a vector. w is in the normal defined by the plane by u and v.The magnitude is |u| |v| sin θ.

u

v

wθ


6

Motion TransformationTransformation matrix: To transform a position and orientation from one coordinate toanother, including rotation and translation:

=

1000zzzz

yyyy

xxxx

paonpaonpaon

T

4 regions.P translationHere, n, o, a orientation change6 unit vectors

n, o, a(nx, ox, ax)

Translation

=

1000100010001

cba

T ntranslatio

Rotation is identifyP = (a, b, c)E.g. A point in the tool coordinate system is (xN, yN, zN). The origin of the tool coordinateis at (a, b, c) in the base coordinate system. What is (x0, y0, z0), this point in the basecoordinate system?

Using simple analytical geometryV3 = (x3 + a, y3 + b, z3 + c)’

J1

J2

J3

TCP

x0

y0

z0

x3

y3

z3


7

We can also use the translation transformation.

xyz

abc

xyz

x ay bz c

0

0

0

3

3

3

3

3

3

1

1 0 00 1 00 0 10 0 0 1 1 1

=

=

+++

The advantage is not obvious, other than a fancy representation

Rotation transformation

Simple rotation about z axis. Consider the waist rotation about z axis in Rhino. It is easyto know a point in the first joint (x1, y1). What is its corresponding (x0, y0)?

x x yy x yz z

0 1 1 1 1

0 1 1 1 1

0 1

= −= +=

cos sinsin cos

θ θθ θ

In matrix form.

Rot z( , )

cos sinsin cos

θ

θ θθ θ

=

−

0 00 0

0 0 1 00 0 0 1

Characteristics• first 3 * 3 is for rotation• z does not change, the row and column for z is 1.• 6 unit vectors• P = 0• If 1 → -1, z change sign, 2nd col change sign. Show with model.

Rotation for other axes

x0

x1

y0y1

θ1


8

Rot x

Rot y

( , )cos sinsin cos

( , )

cos sin

sin cos

θθ θθ θ

θ

θ θ

θ θ

=−

= −

1 0 0 00 00 00 0 0 1

0 00 1 0 0

0 00 0 0 1

Characteristics

• Translation vector.• Rotational unit vectors. This is because rotation should not change the scale.• “1” can appear in any position. That mean a variable rotation plus a change of axis.

The signs can also change due to the orientation of the rotating axis.• When two consecutive joints do not intersect, the transformation may also include

translation. The rotation may also involve constant rotation of an axis. Then, “1” mayappear in other parts of the matrix.

Example

CRS F3 is holding a plate. The plate is at location (0, 0, 200) in the tool coordinatesystem. At the ready position, the tool coordinate system is at (500, 0, 600).• What is the location of the plate in the base coordinate system?• If the base is rotated 300 about z0, what is the location of the plate?

=

−==

16000

700

120000

10006000010010

500100

)90,()600,0,500(0 NvyRotTranv


9

++

=

−

−

=

=

1600

100250173433

120000

1000000100100100

10006001000010

500001

1000010000866.5.005.866.

)90,()600,0,500()30,(0 NvyRotTranzRotv

If you swap the order of z rotation and translation, you will haveTran(433, 250,600)Rot(z, 30)Rot(y,90). Please verify.

Example

The power lies when you deal with multiple links. Consider the first two joints in aSCARA robot. Top view.

We know the length of L1 and L2 are fixed at a1 and a2. (x0, y0) are functions of θ1 and θ2.x a ay a a

0 1 1 2 1 2

0 1 1 2 1 2

= + += + +

cos cos( )sin sin( )

θ θ θθ θ θ

This is rather different from a Cartesian robot. In reality we will also have anotherrotational, vertical and a twist joint. It could also have revolving joint. We can see that thetransformations depend on the configuration.We can use the transformation method we used earlier. Let's consider a very simple robotWe can consider the link 2 being translated to point P then rotated for θ1.

θ1

θ2

x0

y0(x0, y0)(x1 , y1)

J1

a1

a2

x1

y1

P

θ1

θ2

x0

y0(x0, y0)(x1, y1)

J1

a1

a2


10

The translation to P along x0 and y0 are: a1 cos θ1, a1 sin θ1

The rotation is about z for θ1.The vector in the translated coordinate is v1 = (a2 cos θ2, a2 sin θ2, z, 1)’ Let’s use thestandard transformation to calculate TCP with respect to (x0, y0, z0).

v A A v

aa

aa

z

a a aa a a

z

tran rot0 2 1

1 1

1 1

1 1

1 1

2 2

2 2

2 1 2 2 1 2 1 1

2 1 2 2 1 2 1 12

1 0 00 1 00 0 1 00 0 0 1

0 00 0

0 0 1 00 0 0 1 1

1

= =

−

=

− ++ +

cossin

cos sinsin cos

cossin

cos cos sin sin cossin cos cos sin sin

θθ

θ θθ θ

θθ

θ θ θ θ θθ θ θ θ θ

Identical, with trigonomitry identities.If we find the transformation,

−

==

10000100sin0cossincos0sincos

111

1111

θθθθθθ

aa

AAT rottran

• you can combine two matrix together if translation is before rotation.• matrix multiplication is not commutative, or, A1A2 ≠ A2A1.

This means, that the z axis does not change in orientation or positionx and y changes orientation, and position.In the first method, if we add a gripper pointing down, it might become difficult. In the 2nd

method, we only need to add another transformation and multiply.

Robot kinematic analysisAn industrial robot has 4, 5, 6 or even more dof. The transformation from the base to TCPcan be complex. With the transformation introduced earlier, the transformation of theentire robot can be decomposed to successive transformations between connected links.Let joint n has transformation Ai. The transformation of a robot with N dof has is

TN = A1A2 ... An.


11

TN defines the location and orientation of the tool coordinate system (TCP). In addition,Then, a point in the base coordinate v0 = 0Tn vn.

In order to be consistent, people in the field follow certain convention in naming links andjoints and define the angles. This is called Denavit-Hartenberg convention andhomogeneous transformation.

Physically, what does this mean? Let’s look at the example. If rotation proceeds translation, the translation should be along x axis for a1.Let’s see if they will be equal.

v A A v

a aa

z

a a aa a a

z

rot tran0 1

1 1

1 1

1 2 2

2 2

2 1 2 2 1 2 1 1

2 1 2 2 1 2 1 12

0 00 0

0 0 1 00 0 0 1

1 0 00 1 0 00 0 1 00 0 0 1 1

1

= =

−

=

− ++ +

cos sinsin cos

cossin

cos cos sin sin cossin cos cos sin sin

θ θθ θ

θθ

θ θ θ θ θθ θ θ θ θ

Identical. Also check the unit vectors.

v A A vn tran rot n− = =

−

=

2

1 01 6

1 01

1 0 0 00 30 30 00 30 30 00 0 0 1

3751

383487 83

1

cos sinsin cos

.,

Most robots have 4, 5, 6 or more joints. The location and orientation of the end effector becomes cumbersome if found directly. People inthe field developed conventions and step-by-step method to cope with the complexity.Steps:1. Finding transformation between link n-1 and n, An,2. Finding the transformation of the robot, 0TN.

3. v T vN N00= , where

01 2T A A AN N= ... , For TCP, vN = (0, 0, 0, 1), v0 is called Cartesian coordinate in Rhino.

What is the purpose?Robots often have to find the relationships between its tool coordinate system and its base coordinate system.


12

Homogeneous transformation and Denavit-Hartenberg conventionAn n dof robot has N mobile links linked by N joints. A link may be translated,

rotated with respect to its previous link through their joint. Two joint coordinate systemfor 2 consecutive joints can be setup (xn-1, yn-1, zn-1) and (xn, yn, zn). We allow 4 out of 6independent motions,

1. Rotate θ about zn-1

2. Translate d along zn-1

3. Translate a along xn

4. Rotate t about xn

Then, the transformation from (xn-1, yn-1, zn-1) to (xn, yn, zn) will be

A = Rot(zn-1, θ) Tran(zn-1, d) Tran(xn, a) Rot(xn, t)

After multiplication, we have

−

−

=

1000cossin0

sinsincoscoscossincossinsincotsincos

dttattatt

Aθθθθθθθθ

In case that a mechanism can not be modeled with this method because the conventionallows only 4 motions, one can add an intermediate coordinate system to handle.

CONSTRUCTION PROCEDURE (transparency).1. Number the links and joints, starting from the base. The stationary base is link 0. Link

n rotates and/or translates with respect to link n-1 through joint n. The last joint is N.2. Definition of coordinate systems for each link.

• zn-1: axis of motion of joint n. If joint n is rotational, link n rotates about zn-1. Ifjoint n is linear, link n translates along zn-1.

• xn: along common perpendicular (zn-1 → zn) perpendicular and passing both.3. Define the joint parameters.

• θn is the rotation from xn-1 to xn about zn-1.• dn is the translation along zn-1.• an: the translation along xn.• tn is the rotation from zn-1 to zn about xn.

4. Form the homogeneous displacement matrix An.


14

Example:The first 3 axes of a spherical robot,

`

Step 1. Number the links and jointsStep 2. Set up joint coordinate systemsStep 3. Find link parameters.

Link a t d θ1 0 90 0 θ1

2 0 90 0 θ2

3 0* 0* d3 0 (const)

t1 differ in sign from book. The only effects are different x orientation and different θvalues. Otherwise, you get the same result. This way, θ is more intuitive.Step 4. Find As. Short hands: cn is cosθn, sn is sinθn,

−

=

−

−

=

100000100000

1000)()(0

)()()(

11

11

111

111111

1111111

1

cssc

dtctssatsccscatsstcsc

A

y1 becomes z0, it makes sense that they are both vertically up.

A

c s c t s s t a cs c c s t a s

s t c t d

c ss c

2

2 2 2 2 2 2 2

2 2 2 2 2

2 2 2

2 2

2 2

00 0 0 1

0 00 0

0 1 0 00 0 0 1

=

−−

=

−

( ) ( )( )

( ) ( )\2

z0

,

z1L0

J1

L1

L2

J3d3J2 x0

x2

x3 z3

x1

z2


15

=

1000100

00100001

33 d

A

E.g. Adept 1 robot. SCARA, 4 axis. Please find the transformation. x1 and x2 are at the same height of1000 mm. The shoulder link is 800 mm long while the elbow link is 600 mm long.

Solution:1. label the links2. 2. Coordinate system as indicated3. parameters

A T D θ1 800 0 1000 θ1

2 600 0 0 θ2

3 0 180 d3 04 0 0 0 θ4

4. The matrices.

x0

x1

x2

x3

z0

x4z3, z4

z2z1

d3

600800

1000


16

E.g. Jointed arm robot with pitch (Let's simplify by not cosiderting first roll and last roll). The height ofthe shoulder is 400 mm. The length of the shoulder link is 1200mm. The length of the elbow link is1000mm.

5. label the links6. 2. Coordinate system as indicated7. parameters

A T D θ1 0 90 300 θ1

2 1200 0 0 θ2

3 1000 0 0 θ4

4 0 90 0 θ4

8. The matrices.

x0

x1

x2

x3

z2

z1

z0

z3

z4


17

Transformation for the robot, forward kinematics

The beauty of this method for a robot that has N joints, the location and orientation of tool(TCP) in base coordinate system is defined by

0TN = A1A2 ... AN

In the polar robot example, we have

−−−

=

−

−

=

10000

10000010

00

100000100000

2322

21321121

21321121

2322

2322

11

11

32130

cdcsssdssccsscdscscc

cdcssdsc

cssc

AAAT


18

Forward (direct) kinematic analysisFor each robot, you can find TN with joint parameters as constants and variablesembedded in the matrix. For any robot stop, all joint variables are known to the robot.Substitute in the variables in the transformation, you can find tool (TCP)• Location• Orientation

The meaning of A and T

An defines the location and orientation of the (xn, yn, zn) in terms of (xn-1, yn-1, zn-1).If we write An in form

=

1000zzzz

yyyy

xxxx

n paonpaonpaon

A

Wherep vector defines the location of the origin in nth coordinate systema is a unit vector defines direction of zn,o is a unit vector defines direction of yn

n is a unit vector defines xn

E.g. Please fill in the missing parameters in the following matrix and indicate TCP.

A = B = C = E = … = 0. J = 1xn = (1, 0, 0)yn = (0, .707, -.707)zn = (0, .707, .707)

E.g. When θ1 = 0, we have

−

=

−

=

1000001001000001

100000100000

11

11

1

cssc

A

The location of the second coordinate system in the base coordinate system is (0, 0, 0)

−−

JIHGFEDCBA

0707.3707.

51


19

Orientationz1 → -y0

y1 → z0

x1 → x0

Let’s see if this is correct.

This is right and both are right hand coordinate system. Please draw when θ1 = 90.

E.g. For the simple polar robot, let θ1 = 0, θ2 = 90, d3 = 10.

−

=

−−−

=

100000010010

10100

10000 2322

21321121

21321121

30


T

This matrix says that the tool is at (10, 0, 0), pointing (z) to x0, Its x is up.

x0

y0

z0

z3

y3

x3

x0

y0z0

o (yn)

a (zn)n (xn)

TCP (px, py, pz)

x0

y0

z0

x1

z1

y1


20

E.g. For the polar robot, what is TCP if θ1 = 900, θ2 = 300 and and d3 = 5”?

−−=

−−−

=

100033.4866.05.5.25.0866.

0010

10000 2322

21321121

21321121

30


T

That means, the TCP is at (0, 2.5, -4.33), The gripper is pointing down&front. Thegripper opens along y, so it is along x axis.


21

Forward kinematics can also be used to relate positions in tool coordinate and basecoordinate.

v0 = 0TN vN

For Rhino:v0 are called cartesian coordinates.vN is called tool coordinates (0, 0, 0) is TCP.Joint coordinates are B (roll), C (pitch), ...

E.g. If a point in the scope link coordinate is (0, 0, 1), where is TCP and what is itsorientation if θ1 = 900, θ2 = 300 and d3 = 5”?

−=

−−=

−−−==

1196.530

1100

1000866.*5866.05.5.25.0866.

0010

1100

10000 2322

21321121

21321121

330

0 cdcsssdssccsscdscscc

vTv

This means, a point (0, 0, 1) in the gripper coordinate is at (0, 3, -5.196) in basecoordinate.


22

Inverse KinematicsGiven: position and orientation of the toolFind: joint valuesUseful in: offset, Cartesian incremental movement, linear interpolation, etc.

=

1000

)(var0

zzzz

yyyy

xxxx

N paonpaonpaon

iablesT

If we know the location and orientation, then we know the values of of p, a, o and n. thismatrix is known. We can setup up to 12 simultaneous equations.

Example

For the polar robot, If we want the robot TCP to go to (0, 5, 3)

−−

=

−−−

=

10003050

10000 22

21121

21121

2322

21321121

21321121

30

csssccsscscc


T

ps are fixed. The orientation is fixed at any given location. Effectively, we can only solvefor 3 joint values.

3cos5sinsin0sincos

23

213

213

=−==

θθθθθ

ddd

3 equations and 3 unknowns.In 3, d3 can not be zero.In 1, θ1 = 900 or θ2 = 00,In 2, θ2 ≠ 00. Therefore, θ1 = 900.In 2 and 3,

d3 sin θ2 = 5- d3 cos θ2 = 3θ2 = tan-1 (5 / (-3) = - 590 using calculatorIt should be 180 - 59 = 1210

d3 = 5 / sin 121 = 5.833


23

Check right hand rule: Check.

This is a trivial case with 3 dof. See handout. In this case, the solution will be unique.Often, the solution is not unique.For a 6 dof robot, the forward requires 10 transcendental calls, 30 multiplies, and 12additions. The inverse 13 transcendental calls, 27 multiplies, and 20 additions aftermanipulations.

Inverse of transformation matrixIn kinematics analysis, you often needs to find an inverse. If

=

1000zzzz

yyyy

xxxx

paonpaonpaon

T , then its inverse is

⋅−⋅−⋅−

=−

1000

1

apaaaopooonpnnn

Tzyx

zyx

zyx

Where the last column are dot products of vectors.

e.g.

A

c ss c

l1

1 1

1 1

1

0 00 0

0 1 00 0 0 1

=−

, and A

c sl

s c11

1 1

1

1 1

0 00 0 1

0 00 0 0 1

− =−

−

Robot Motion and TransformationWhen we discuss the motion types, I "claimed" that joint interpolation is simple withsmooth path while continuous path control is complex.E.g. The polar robot is taught two points: P1 and P2. The positions in joint coordinates are(30, 90, 5) and (210, 120, 10). In a section of an application, the robot is at P1. Weinstruct the robot to go to P2 in• Joint interplation• Linear interpolation.

−

=

−−

=

10003515.0857.05857.0515.0010

10003050

22

21121

21121

3

csssccsscscc

T


24

Please explain the robot controller function if you use move. The default speeds of therobot in joint coordinates are (180, 120, 10).In Joint interpolation, the changes in joint coordinates are (180, 30, 5). Using the analysisfrom before, we find:


25

Transformation graphYou can express the transformation of a robot in two ways from the base. A transform graph is a directed graph to connect consecutivelinks of a robot. For the example spherical robot.

We can write the transformation from the base to TCP as 0T3 or A1A2 A3, and we have

0T3 = A1A2 A3,As a matter of fact, we can write this equality between any two points.• in the direction of the arrow, use the transformation,• against the direction, use the inverse.Let’s choose between L1 and TCP.

A2 A3 = A1-1 0T3

This is useful in setting different equations. Solving inverse kinematics is often difficult. Some may be better than others. Matrix inversion.

Base

L3 atTCP

0 T3A3A2A1

L1 L2


26

Comprehensive exampleThe 3 dof polar example is not very useful. Let’s add a pitch to it. This can be used for vertical insertion of round pins. Or moving columnmaterials.Look at the modified axis in blue. The link parameters will be now

Link a t D θ1 0 90 0 θ1

2 0 90 0 θ2

0 90 d3 − 1803 0 90 0 θ4

−+−−−−−−+−−−+−

=

10000 2342424242

2134214211421421

2134214211421421

40

cdccsssccsssdcssscscsssccsscdcscsscssscccc

T

The location portion does not change. Because pitch does not affect location the way we assign the coordinates.Rotational portion becomes more complex. You can verify that the vectors in rotational portion are unit vectors.Forward kinematics.If θ1 = 90, θ2 = 90, d3 = 10, and θ4 = 90. Location and orientation = ?

−=

10000100

100010010

40T

The gripper is at (0, 10, 0), on y0 axis. Gripper is pointing down. Opens about x axis.InverseWe thought that this robot can perform vertical insertion of round pins. What does that mean in this matrix ????????????The tool coordinate must be able to go down. The approach vector must be (0, 0, -1).That is we must find solutionWhich has solution anywhere in the envelop.

We know that any position can be reached within envelop by varying θ1, θ2, and d3. By simply choosing θ4 = 180 − θ2, We can maintainvertically down.

How about insertion in x-z plane????We need approach vector to be in y, or (0, 1, 0). Or,

In order for this to be true, we must have first and second term be 1 or –1.That can only be achieved at certain positions.

Other kinematics issues1. Euclidean speed. For the popular articulated robot, there is no direct control of the

speed of the end effector, which is important in almost all applications. Speed is thefirst derivative of position w.r.t the time. Jacobian.

2. Euclidean acceleration. 2nd derivative.3. Singularity.4. Trajectory control.5. Solution uniqueness. Define posture.

14242 −=+− θθθθ ccss

180or ,1)(

42

42

=+−=+

θθθθc

1)( 42421 =+− csscs


27

Summary of kinematics analysisCommercial robots are not necessary built with application coordinates.It is necessary to find relationships between the joint coordinates and applicationcoordinates.Forward: knownInverse:Robots has 4 - 7 dof. Analysis can be difficult.Homogeneous transformation make it simpleFinding robot transformationForward application: Given joint, find TCP, v0

Inverse:Other issuesUsefulness


28

E.g. in an RR robot,

Example 2: The SCORBOT or Rhino robot.

z’s

x’s

Parameters

Summary of link parameters.

Link a t d θ

1 0 90 l1 θ1

2 L2 0 0 θ2

3 L3 0 0 θ3

4 0 90 0 θ4

5 0 0 0 θ5

L0

z0 z1

z2 z3x1

x2

z4, z5

x0

robot motion analysis - kinematicsczhou/motion.pdf · robot motion analysis - kinematics kinematics...

Documents