Robotic Kinematics_Inverse Kinematic Solution

8/8/2019 Robotic Kinematics_Inverse Kinematic Solution

1/54

11/24/2010ME 3230Page 1

Robotic Kinematics the InverseKinematic Solution

ME 3230

Kinematics and MechatronicsDr. R. Lindeke


2/54

11/24/2010ME 3230Page 2

FKS vs. IKS

In FKS we built a tool for finding end frame geometryfrom Given Joint data:

In IKS we need Joint models from given End PointGeometry:

Joint

Space

CartesianSpace

Joint

SpaceCartesianSpace


3/54

11/24/2010ME 3230Page 3

So this IKS Problem is Nasty(in Mathematics this isconsidered a Hard Modeling Issue)

It a more difficult problem because:

The Equation set is Over-Specified:

12 equations in 6 unknowns

Space can be Under-Specified: Planer devices with more joints than 2

The Solution set can contain Redundancies:

Multiple solutions The Solution Sets may be un-defined:

Unreachable in 1 or many joints


4/54

11/24/2010ME 3230Page 4

But the IKS is VERY Useful some Usesof IKS include:

Building Workspace Maps

Allow Off-Line Programming solutions

The IKS allows the engineer to equate Workspacecapabilities with Programming realities to assure thatexecution is feasible(as done in ROBCAD & IGRIP)

The IKS Aids in Workplace Design and OperationalSimulations


5/54

11/24/2010ME 3230Page 5

Doing a Pure IKS solution: the UR Manipulator

X0

Y0

Z0

Z1

X1

Y1 Y2

X2

Z2

X0

Y0

Z0

Z1

X1

Y1 Y2

X2

Z2

Same Origin

Point!

UR Frame Skeleton (as DH Suggest!)


6/54

11/24/2010ME 3230Page 6

LP Table and Ais

1

1 0 1 0

1 0 1 00 1 0 0

0 0 0 1

S C

C SA

! -

Frames Link Var U d a E SE CE S U CU

0 1 1 R U + 90 0 0 90 1 0 C1 -S1

1 2 2 P 0 d2 + cl2 0 0 1 0 1 0

2

2 2

1 0 0 0

0 1 0 00 0 1

0 0 0 1

Ad cl

! -


7/54

11/24/2010ME 3230Page 7

FKS is A1*A2:

2 2

1 0 1 0 1 0 0 0

1 0 1 0 0 1 0 0

0 1 0 0 0 0 10 0 0 1 0 0 0 1

S

S

d cl

y

- -

2 2

2 2

1 0 1 1 ( )

1 0 1 1 ( )0 1 0 0

0 0 0 1

S C C d cl

C S S d cl

-

g

g


8/54

11/24/2010ME 3230Page 8

Forming The IKS:

2 2

2 2

1 0 1 1 ( )

1 0 1 1 ( )

0 1 0 0

0 0 0 1

S d cl

S S d cl

-

0 0 0 1

x x x x

y y y y

z z z z

n o a d

n o a d

n o a d

-

In the Inverse Problem, The RHS MATRIX is completely known(perhaps from a robot mapped solution)! And we use these values tofind a solution to the joint equations that populate the LHS MATRIX


9/54

11/24/2010ME 3230Page 9

Forming The IKS:

Examining these two matrices n, o, a and d are givens in the inverse sense!!!

(But typically we want to build generalmodels leaving these terms

unspecified) Term (1, 4) & (2,4) on both sides allow us to find an equation for

U:

(1,4): C1*(d2+cl2) = dx (2,4): S1*(d

2+cl

2) = d

y

Form a ratio to build Tan(U): S1/C1 = dy/ dx Tan U = dy/dx U = Atan2(dx, dy)

If 2 Matrices areEqual then EACHand EVERY term isalso Uniquely equalas well!


10/54

11/24/2010ME 3230Page 10

Forming The IKS:

After U is found, back substituteand solve for d

2

: (1,4): C1*(d2+cl2) = dx Isolating d2: d2 = [dx/CosU1] - cl2


11/54

11/24/2010ME 3230Page 11

Alternative Method doing a pure inverse approach

Form A1-1 then pre-multiply both side by this

inverse

Leads to: A2 = A1-1

*T0n

given

2 2

1 0 0 0 1 1 0 0

0 1 0 0 0 0 1 0

0 0 1 1 1 0 0

0 0 0 1 0 0 0 1 0 0 0 1

x x x x

y y y y

z z z z

S C n o a d

n o a d

d cl C S n o a d

! y - - -


12/54

11/24/2010ME 3230Page 12

Simplifying & Solving:

Selecting and Equating (1,4) 0 = -S1*dx +C1*dy Solving: S1*d

x= C1*d

y Tan(U) = (S1/C1) = (dy/dx) U = Atan2(dx, dy) the same as before

Selecting and Equating (3,4) -- after backsubstituting U solution

d2 + cl2 = C1*dx + S1*dy d2 = C1*dx + S1*dy - cl2


13/54

11/24/2010ME 3230Page 13

Performing IKS For Industrial Robots: (a moreinvolved problem)

First lets consider the concept of the Spherical WristSimplification:

All Wrist joint Zs intersect at a point The n Frame is offset from this Zs intersection point at a

distance dn (the hand span) along the a vector of thedesired solution (3rd column of desired orientation sub-matrix!) which is the z direction of the 3rd wrist joint

This follows the DH Algorithm design tools as we havelearned them!


14/54

11/24/2010ME 3230Page 14

Performing IKS with a Spherical Wrist

We can now separate the POSE effects:

ARM joints

Joints 1 to 3 in a full function manipulator (without redundantjoints)

They function to maneuver the spherical wrist to a targetPOSITION related to the desired target POSE

WRIST Joints Joints 4 to 6 in a full functioning spherical wrist

Wrist Joints function as a primary tool to ORIENT the end frameas required by the desired target POSE


15/54

11/24/2010ME 3230Page 15

Performing IKS: Focus on Positioning

We will define a point(called the WRISTCENTER) where all 3 zs intersect as:

Pc = [Px, Py, Pz] We find that this position is exactly:

Pc = dtarget- dn*a

Px

= dtarget,x

- dn*a

x Py = dtarget,y - dn*ay Pz = dtarget,z - dn*az

Note: dn is the so calledHandspan (a CONSTANT)


16/54

11/24/2010ME 3230Page 16

Treating the Arm Types (as separaterobot entities):

Cartesian (2 types) Cantilevered

Gantry

Cylindrical

Spherical (w/o d2 offset)

2 Link Articulating (w/o d2 offset)


17/54

11/24/2010ME 3230Page 17

Cantilevered Cartesian Robot

P-P-P

Configuration

J1

J3

J2

X0

Y0

Z0


18/54

11/24/2010ME 3230Page 18

Gantry Cartesian Robot

P-P-P

ConfigurationZ0X0

Y0

J1

J2

J3


19/54

11/24/2010ME 3230Page 19

Cylindrical Robot

Doing IKS given a

value for: X, Y and Z of End

Compute U, Z and R

R

X0Y0

Z0


20/54

11/24/2010ME 3230Page 20

Spherical Robot

Developing a IKS

(model): Given Xe, Ye, & Ze Compute U, J & R

R

J

X0

Y0

Z0


21/54

11/24/2010ME 3230Page 21

2-Link Articulating Arm Manipulator

U3

U1

U2

L2

L1

Z0

X0Y0


22/54

11/24/2010ME 3230Page 22

Focusing on the ARM Manipulatorsin terms of P

c

:

Prismatic: q1 = d1= Pz (its along Z0!) cl1 q

2= d2 = P

xor P

y- cl

2 q3 = d3= Py or Px - cl3

Cylindrical: U1 = Atan2(Px, Py) d2 = Pz cl2 d3 = Px/C1 cl3 {or +(Px

2 + Py2).5 cl3}


23/54

11/24/2010ME 3230Page 23


c

:

Spherical: U1 = Atan2(Px, Py)

U2 = Atan2((Px2 + Py2).5 , Pz) D3 = (Px

2 + Py2 + Pz

2).5 cl3


24/54

11/24/2010ME 3230Page 24


c

:

Articulating: U1 = Atan2(Px, Py)

U3 = Atan2(D, s(1 D2).5)

Where D =

U2 = J - E J is: Atan2((Px2 + Py2).5, Pz)

E is:

2 2 2 2 22 32 32

x y z P P P a a

a a

2

2 3

2 2 2 2 2

2 3

sintan2( )cos

2 1tan2

x y z

A

a a DA

P P P a a

E E !

s


25/54

11/24/2010ME 3230Page 25


c

:

U2 =

Where D =

2

2 3

2 2 2 2 22 3

2 1

tan2 x y z

a a D

A P P P a a

s

2 2 2 2 22 32 32

x y z P P P a a

a a

Atan2((Px2 + Py

2).5, Pz) -


26/54

11/24/2010ME 3230Page 26

One Further Complication MustBeConsidered:

This is called the d2 offset problem

A d2 offset is a problem that states that the nth frame

has a non-zero offset along the Y0 axis as observed in

the solution of the T0n with all joints at home (like the 5 dof articulating robots in our lab!)

This leads to two solutions for U1 theSo-Called Shoulder Left and Shoulder Rightsolutions


27/54

11/24/2010ME 3230Page 27

Defining the d2

Offset issue

X0, X1

Y0, Z1

Z0 d2The ARM

The

Wrist

Ypc

Xpc

Zpc

Here: The ARM might contain a prismatic joint (as in the Stanford Arm discussed

in text) or it might be the a2 & a3 links in an Articulating Arm as it rotates out of plane

A d2 offset means that there are two places where U1 can be placed to touch a givenpoint (and note, when U1 is at Home, the wrist centeris not on the X0 axis!)


28/54

11/24/2010ME 3230Page 28

Lets look at this Device From the Top a planview of the structure projected to the X0Y0 plane

Pc'(Px, Py)

X0

Y0

Z1

Z1'

X1

d2

d2

a2'

a3'

R'

X1'

E

U11

F1


29/54

11/24/2010ME 3230Page 29

Solving For U1:

We will have a Choice of two poses for U1:

1 1 1

1

.52 2 2

2 2

1

1 tan2( , )

tan2 ,pc pc

pc pc

A X Y

A X Y d d

U J E

U

!

!

2 1 1

.52 2 2

2 2

1 180

180 tan2 ,

tan2 ,pc pc

pc pc

A X Y d d

A X Y

U E J! r !

r


30/54

11/24/2010ME 3230Page 30

This device (like the S110 robots in lab) is called aHard Arm Solution

We have two U1s These lead to two U2s (Spherical)

One for Shoulder Right & one for Shoulder Left

Or four U2s and U3s in the Articulating Arm: Shoulder Right Elbow Up & Down

Shoulder Left Elbow Up & Down


31/54

11/24/2010ME 3230Page 31

Now, lets look at Orientation IKSs

Orientation IKS again relies on separation ofjoint effects

We (now) know the first 3 joints controlpositions they would have been solved by using the appropriate set of

equations developed above

The last three (wrist joints) will control theachievement of our desired Orientation


32/54

11/24/2010ME 3230Page 32

These Ideas Lead to an Orientation Model for aDevice that is Given by:

This model separates Arm Joint and WristJointContribution to the desired Target

Orientation Note: target orientation is a given for the

IKS model!

3 6

0 3 given R R R!g


33/54

11/24/2010ME 3230Page 33

Focusing on Orientation Issues

Lets begin by considering EulerAngles(they are a model that isalmost identical to a fullfunctioning Spherical Wrist as

defined using the D-H algorithm!):

Step 1, Form a Product:

Rz1*Ry2*Rz3

This product becomes R36 inthe model on the previous slide

1

2

3

cos sin 0

sin cos 0

0 0 1

cos 0 sin

0 1 0

sin 0 cos

cos sin 0

sin cos 0

0 0 1

z

y

z

R

R

R

J J

J J

U U

U U

] ]

] ]

! - ! -

! -


34/54

11/24/2010ME 3230Page 34

Euler Wrist Simplified:

C C C S S C C S S C C S

S C C C S S C S C C S S

S C S S C

J U ] J ] J U ] J ] J U

J U ] J ] J U ] J ] J U

U ] U ] U

- this matrix, which contains the joint control angles, is then set equal to a U

matrix prepared by multiplying the inverse of the ARM joint orientation sub-

matrices and the Desired (given) target orientation sub-matrix:

130 x x x

y y y

z z z given

n o a

U n o a

n o a

! -

g

NOTE: R03 is

Manipulatordependent!

(Inverse required a

Transpose)


35/54

11/24/2010ME 3230Page 35

Continuing: to define the U-Matrix

11 12 13

21 22 23

31 32 33

11 21 31 11 21 31 11 21 31

12 22 32 12 22 32 12 22 32

13 23 33 13 23 33 13 23 33

x y z x y z x y z

x y z x y z x y z

x y z x y z x y z

U U U

U U U

U U U

n n n o o o a a a

n n n o o o a a a

n n n o o o a a a

! -

-

Rij are terms from the product of the first 3 Ais (rotational sub-matrix)

ni, oi & ai are from given target orientation

we develop our models in a general way to allow computation ofspecific angles for specific cases


36/54

11/24/2010ME 3230Page 36

Now Set:LHS (euler angles) = RHS (U-Matrix)

C C C S S C C S S C C S

S C C C S S C S C C S S

S C S S C

J U ] J ] J U ] J ] J U

J U ] J ] J U ] J ] J UU ] U ] U

! -

11 12 13

21 22 23

31 32 33

U U U

U U U

U U U

-

U as defined on theprevious slide! (a

function of arm-joint

POSE and Desired End-

Orientation)


37/54

11/24/2010ME 3230Page 37

Solving for Individual Orientation Angles (1stsolve forUthemiddle one):

Selecting (3,3) CU = U33

With CU we know SU = s(1 - C2U).5

Hence: U = Atan2(U33, s(1-U332).5)

NOTE: leads to 2 solutions for U!


38/54

11/24/2010ME 3230Page 38

Re-examining the Matrices:

To solve for J: Select terms: (1,3) & (2,3) CJSU = U13

SJSU = U23 Dividing the 2nd by the 1st: SJ/CJ = U23/U13 Tan(J) = U23/U13 J = Atan2(U13, U23)


39/54

11/24/2010ME 3230Page 39

Continuing our Solution:

To solve for ]: Select terms: (3,1) &(3,2)

-SUC] = U31 SUS] = U32 (and dividing this by previous) Tan(]) = U32/-U31 (note sign migrates with

term!)] = Atan2(-U31, U32)


40/54

11/24/2010ME 3230Page 40

Summarizing:

U = Atan2(U33, s(1-U332).5)

J = Atan2(U13, U23)

] = Atan2(-U31, U32)

Uij

s as defined on the earlier slide!

and

U is manipulator and desired orientation

dependent


41/54

11/24/2010ME 3230Page 41

Let consider a Spherical Wrist:

Z3

X3

Y3

Z4

X4

Y4

Z5

X5

Y5

Z6

X6

Y6 Here drawn in

Good Kinematic

Home for

attachment to anArticulating Arm

Same OriginPoint


42/54

11/24/2010ME 3230Page 42

IKSing the Spherical Wrist

Frames Link Var 5 d a E

3 4 4 R U4 0 0 -90

4 5 5 R U5 0 0 +90

5 6 6 R U6 d6 0 0

4

4 0 4 5 0 5 6 6 0

4 0 4 ; 5 5 0 5 ; 6 6 6 0

0 1 0 0 1 0 0 0 1

C S C S C S

R S C R S C R S C

! ! ! - - -


43/54

11/24/2010ME 3230Page 43

Writing The Solution:

11 12 13

21 22 23

31 32 33

4 0 4 5 0 5 6 6 0

4 0 4 5 0 5 6 6 0

0 1 0 0 1 0 0 0 1

C S C S C S

S C S C S C

U U U

U U UU U U

y y !

- - -

-

Uijs as defined on the earlier slide!

and

U is manipulator and desired orientation

dependent


44/54

11/24/2010ME 3230Page 44

Lets Solve (here I try the Pure Inverse Technique):

11 12 13

21 22 23

31 32 33

5 0 5 6 6 0

5 0 5 6 6 0

0 1 0 0 0 1

4 4 0

0 0 1

4 4 0

C S C S

S C S C

C S U U U

U U U

S C U U U

y !

- -

- -

Note:

R4s

Inverse


45/54

11/24/2010ME 3230Page 45

Simplifying

11 21 12 22 13 23

31 32 33

21 11 22 12 23 13

5 6 5 6 5

5 6 5 6 5

6 6 0

4 4 4 4 4 4

4 4 4 4 4 4

C C C S S

S C S S C

S C

C U S U C U S U C U S U

U U U

C U S U C U S U C U S U

! -

-


46/54


47/54

11/24/2010ME 3230Page 47

Solving for U5 & U6 For U5: Select(1,3) & (2,3) terms

S5 = C4U13 + S4U23 C5 = U33 Tan(

U5) = S

5/C

5= (C

4U

13+ S

4U

23)/U

33 U5 = Atan2(U33, C4U13 + S4U23)

For U6: Select(3,1) & (3, 2) terms S6 = C4U21 S4U11 C6 = C4U22 S4U12 Tan(U6) = S6/C6 = ([C4U21 S4U11]/[C4U22

S4U12])

U6 = Atan2 ([C4U22 S4U12], [C4U21 S4U11])


48/54

11/24/2010ME 3230Page 48

Using the Pure Inversing:

We removed ambiguity from the solution

We were able to solve for joints In Order

Without noting we see the obviousrelationship between Spherical wristandEuler Orientation!


49/54

11/24/2010ME 3230Page 49

Summarizing:

U4 = Atan2(U13, U23)

U5 = Atan2(U33, C4U13 + S4U23)

U6 = Atan2 ([C4U22 S4U12], [C4U21 S4U11])


50/54

11/24/2010ME 3230Page 50

Lets Try One:

Cylindrical Robot w/ Spherical Wrist

Given would be a Target matrix from Robot Mapping!(its an IKS after all!)

The d3constant is 400mm; the d6 offset(the HandSpan) is 150 mm.

U1 = Atan2((dx ax*150),(dy-ay*150)) d2 = (dz az*150)

d3 = ((dx ax*150)2+p(dy-ay*150)

2).5 - 400


51/54

11/24/2010ME 3230Page 51

The Frame Skeleton:

X

ZF0

F1 Z

X

X

F2 Z

Z

X

F6

F2.5

Z

X

F4

XF3

Z

F5

Z

X

X

Z

Note Dummy Frame to account forOrientation problem

with Spherical Wrist might not be needed if we had set

wrist kinematic home for CylindricalMachine!


52/54

11/24/2010ME 3230Page 52

Solving for U:

1

1 1 0 0 0 1 0 0 1 0 1 01 1 0 1 0 0 1 0 0 1 0 0

0 0 1 0 1 0 0 1 0 0 0 1

x x x

y y y

z z z

C S n o aU S C n o a

n o a

! y y y y - - - - - -

NOTE: We needed a Dummy Frame to account for the

Orientation issue at the end of the Arm (as drawn) this

becomes a part of the arm space not the wrist space!


53/54

11/24/2010ME 3230Page 53

Simplifying:

11 12 13

21 22 23

31 32 33

1 1 1 1 1 1

1 1 1 1 1 1

x z x z x z

y y y

x z x z x z

U U U C n

Sn

Co

So

Ca

Sa

U U U n o a

U U U S n C n S o C n S a C a

! - -


54/54

11/24/2010ME 3230Page 54

Subbing Uijs Into Spherical Wrist JointModels:

U4 = Atan2(U13, U23)= Atan2((C1ax + S1az), ay)

U5 = Atan2(U33, C4U13 + S4U23)= Atan2{ (S1ax-C1az) , [C4(C1ax+S1az) + S4*ay]}

U6 = Atan2 ([C4U21 - S4U11], [C4U22 - S4U12])= Atan2{[C4*ny - S4(C1nx+S1nz)],

[C4*oy - S4(C1ox+S1oz)]}

Documents

Robotic Kinematics_Inverse Kinematic Solution