Inverse Kinematics –IKS Solutions

filercaliforniaΜηχανική

14 Νοε 2013 (πριν από 3 χρόνια και 9 μήνες)

117 εμφανίσεις

Inverse Kinematics

IKS Solutions

ME 4135


Robotics and Controls

R.R. Lindeke, Ph.D., Fall 2011

FKS vs. IKS


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





In IKS we need Joint models from given End
Point
POSE Geometry
:

Joint
Space

Cartesian
Space

Joint
Space

Cartesian
Space

So this IKS is
‘Nasty’


it is a so
-
call Hard
Mathematical Problem


It a more difficult problem because:


The Equation set is “Over
-
Specified”



12 equations in 6 unknowns


Space

may be
“Under
-
Specified”



for example ‘Planer devices’
with more joints than 2


The Solution set can contain Redundancies



Thus they exhibit Multiple
solutions


The Solution Sets may be un
-
defined



Unreachable in 1 or many joints

But the IKS in Critical in Robotics


Builds
Workspace limits and controller maps



Allows “Off
-
Line Programming” solutions



Thus, compares Workspace capabilities with
Programming
desirability
to assure that execution is
feasible



Aids in Workplace design and
operational
simulations

Doing a Pure IKS solution: the

R
Manipulator

X
0
Y
0
Z
0
Z
1
X
1
Y
1
Y
2
X
2
Z
2

R Frame
Skeleton

LP Table and A
i
’s

1
1 0 1 0
1 0 1 0
0 1 0 0
0 0 0 1
S C
C S
A

 
 
 

 
 
 
Frames

Link

Var



d

l



S


C


S


C


0


1

1

R



+ 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 0
0 0 1
0 0 0 1
A
d cl
 
 
 

 

 
 
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 1
0 0 0 1 0 0 0 1
S C
C S
d cl

   
   
   
 
   

   
   
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
 
 
 

 
 
 
 
Forming The IKS:

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
 
 
 

 

 
 
 
0 0 0 1
x x x x
y y y y
z z z z
n s a d
n s a d
n s a d
 
 
 
 
 
 
Forming The IKS:


Examining
the
two
sides of this FKS equation


n
,
s,
a and d
are
given in an inverse
sense


since we desire to
put the manipulator on a particular target


But we will build the general solution only


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

:


Select (1,4
): C1*(d
2
+cl
2
) =
d
x


Select (2,4
): S1*(d
2
+cl
2
) =
d
y



Form a ratio
of Term
2,4

by Term
1,4

to
build
an
equation for Tan
(

) and then

:



S1/C1 =
d
y
/
d
x



Tan


=
d
y
/
d
x





= Atan2(
d
x
,
d
y
)


Forming The IKS:


After


is found, back substitute and
solve for d
2
:


Choose term(1,4
): C1*(d
2
+cl
2
) =
d
x


Isolating d
2
: d
2
= [
d
x
/C1]
-

cl
2

Alternative Method


doing a pure inverse
approach


Form
[A
1
]
-
1

then pre
-
multiply
both side by this
‘inverse’


Leads to: A2 = A
1
-
1
*T
0
n
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 s a d
n s a d
d cl C S n s a d

     
     
     
 
     

     
     
After Simplifying the RHS:

















1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1
0 0 0 1
x y x y x y x y
z z z z
x y x y x y x y
S n C n S s C s S a C a S d C d
n s a d
C n S n C s S s C a S a C d S d
 
       
 
 
 
   
 
 
 
2 2
1 0 0 0
0 1 0 0
0 0 1
0 0 0 1
d cl
 
 
 

 

 
 
Solving:


Selecting and Equating
terms (1,4
)



0 =
-
S1*
d
x

+ C1*
d
y



Solving: S1*
d
x

= C1*
d
y




Tan(

) = (S1/C1) = (
d
y
/
d
x
)





= Atan2(
d
x
,
d
y
)


Selecting and Equating
terms (3,4
)
--

after back
substituting


solution


and note, after the
above step,


is known as an angle



d
2

+ cl
2

= C1*
d
x

+ S1*
d
y




d
2

= C1*
d
x

+ S1*
d
y

-

cl
2


d
2
= Cos[Atan2(
d
x
,
d
y
)]*
d
x

+ Sin[Atan2(
d
x
,
d
y
)]*
d
y


cl
2


Performing IKS For Industrial Robots:


First lets consider the previously defined
Spherical Wrist simplification


All Wrist joint Z’s intersect at a point


The n Frame is offset from this intersection as a
distance d
n

along the
a

vector of the desired
solution (3
rd

column of desired orientation sub
-
matrix)


This is as expected by the DH Algorithm

Performing IKS


We can now separate the effects of the ARM joints


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


They function to position the spherical wrist at a target
POSITION related to the desired target POSE


Arm Joints are separated from the WRIST Joints


Joints 4 to 6 in a full functioning spherical wrist


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


Performing IKS: Focus on Positioning


We will define a point (the WRIST CENTER) as:


P
c

= [P
x
, P
y
, P
z
]


Here we define P
c

= d
target
-

d
n
*
a


P
x

= d
target,x

-

d
n
*
a
x


P
y

= d
target,y

-

d
n
*
a
y


P
z

= d
target,z

-

d
n
*
a
z


Focusing on the ARM Manipulators
in terms
of P
c
:


Prismatic:


q
1

= P
z

(its along Z
0
!)


cl1


q
2

= P
x

or P
y

-

cl2


q
3

= P
y

or P
x

-

cl3



Cylindrical:



1

= Atan2(P
x
, P
y
)


d
2

= P
z



cl2


d
3

= P
x
*C1


cl3 or +(P
x
2

+ P
y
2
)
.5



cl3

Focusing on the ARM Manipulators
in terms
of P
c
:


Spherical:



1 = Atan2(Px, Py)



2 = Atan2( (P
x
2

+ P
y
2
)
.5

, P
z
)


D3 = (P
x
2

+ P
y
2
+ P
z
2
)
.5



cl3

Focusing on the ARM Manipulators
in terms
of P
c
:


Articulating:



1 = Atan2(Px, Py)



3 = Atan2(D,

(1


D
2
)
.5
)



Where D =





2 =


-







is: Atan2((Px
2

+ Py
2
)
.5
, P
z
)





is:




2 2 2 2 2
2 3
2 3
2
x y z
P P P l l
l l
   








2 2 2 2 2 2
2 3 2 3
tan2(cos,sin )
tan2,2 1
x y z
A
A P P P l l l l D
 

     
One Further Complication:


This is called the d
2

offset problem


A d
2

offset is a problem that states that the n
frame has a non
-
zero offset along the Y
0

axis
as observed with all joints at home, in the
solution of the
0
T
n


This leads to two solutions for

1

the So
-
Called
Shoulder Left and Shoulder Right solutions

Defining the d
2

Offset issue

X
0
, X
1
Y
0
, Z
1
Z
0
d
2
The ARM
The
Wrist
Y
pc
X
pc
Z
pc
Here: ‘The ARM’ might be a prismatic joint as in the Stanford Arm or it might be
l
2

&
l
3

links in an Articulating Arm and rotates out of plane

A d2 offset means that there are two places where

1 can be placed to touch a
given point (and note, when

1

is at Home, the wrist center is not on the X0
axis!)

Lets look at this Device “From the Top”

Pc'(Px, Py)
X
0
Y
0
Z
1
Z
1
'
X
1
d
2
d
2
a
2
'
a
3
'
R'
X
1
'
L
2


L
3





11



X
1

Solving For

1


We will have a Choice (of two) poses for

:






1 1 1
1
.5
2 2 2
2 2
1
1 tan2(,)
tan2,

pc pc
pc pc
A X Y
A X Y d d
  

 

  






2 1 1
.5
2 2 2
2 2
1 180
180 tan2,
tan2,

pc pc
pc pc
A X Y d d
A X Y
  
    
    
In this so
-
called “Hard Arm”


We have two

1
’s


These lead to two

2
’s
(Spherical)


Or to four

2
’s
and

3
’s
in the Articulating Arm


Shoulder Right Elbow Up & Down


Shoulder Left Elbow Up & Down

The Orientation Model


Evolves from:






Separates Arm Joint and
Wrist Joint Contribution
to Target (given)
orientation



3 6
0 3
given
R R R

Focusing on Orientation Issues


Lets begin by
considering Euler
Angles (they are a
model that is almost
identical to a full
functioning Spherical
Wrist):


Form Product:


R
z1
*R
y2
*R
z3


This becomes

R
3
6

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
 
 
 
 
 
 

 
 

 
 
 
 
 

 
 

 

 
 

 
 
 
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
    
    
  
  
 
 
  
 
 

 
And this matrix is equal to a U matrix prepared by multiplying the inverse of the ARM
joint orientation matrices inverse and the Desired (given) target orientation



1
3
0
x x x
y y y
z z z
given
n s a
R n s a
n s a

 
 
 
 
 
NOTE: R
0
3

is
Manipulator
dependent!

Simplifying the RHS:
(our so
-
called U Matrix)



3
0
1
3
0
11 12 13
21 22 23
31 32 33
11 21 31
12 22 32
13 23 33
(
this is a transpose!)
R R R
R R R R
R R R
R R R
R R R R
R R R

 
 

 
 
 

 
 

 
 
 
Continuing:

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 R n R n R s R s R s R a R a R a R
n R n R n R s R s R s R a R a R a R
n R n R n R s R s R s R a R a R a R
 
 

 
 
 
 
     
 
     
 

     
 

Finally:

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
    
    
  
  
 
 
   
 
 

 
11 12 13
21 22 23
31 32 33
U U U
U U U
U U U
 
 
 
 
 
Solving for Individual Orientation Angles
(1
st


):


Selecting (3,3
)


C


= U
33




With
C


we “know”
S




(
1
-
C
2

)
.
5



Hence:


㴠=t慮㈨a
33
,

(1
-
U
33
2
)
.5




NOTE: 2 solution for

!


Re
-
examining the Matrices:


To solve for

㨠卥汥捴:t敲浳㨠⠱ⰳ⤠☠⠲ⰳ)



C

S


= U
13



S

S


= U
23



Dividing
the 2
nd

by the 1
st
: S


/C


= U
23
/U
13



so Tan
(

) = U
23
/U
13



leading to:


= Atan2(U
13
, U
23
)

Continuing our Solution:


To solve for

㨠卥汥捴:t敲浳㨠⠳ⰱ⤠☠⠳ⰲ(



-
S

C


= U
31



S

S


=
U
32



so Tan
(

) = U
32
/
-
U
31

(note the ‘minus’ sign track
the appropriate term!)



And Thus


= Atan2(
-
U
31
, U
32
)



Summarizing:




= Atan2(U
33
,

(1
-
U
33
2
)
.5





= Atan2(U
13
, U
23
)





= Atan2(
-
U
31
, U
32
)


Let’s examine
a Spherical Wrist:

Z
3
X
3
Y
3
Z
4
X
4
Y
4
Z
5
X
5
Y
5
Z
6
X
6
Y
6
IKSing the Spherical Wrist

Frames

Link

Var



d

l



3


4

4

R


4

0

0

-
90

4


5

5

R


5

0

0

+90

5


6

6

R


6

d
6

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
 
     
     
   
     
     

     
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 U
U U U
 
     
     
   
     
     

     
 
 
 
 
 
Lets See
(By
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

   
   
  
   
   
   
 
 
 
 

 
 
 
 

 
 
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

 
 
  
 
 
 
  
 
 
  
 
 
  
 
Solving:


After Examination
here
let’s
select (3,3) both
sides:


0 = C4U
23



S4U
13


S4U
13

= C4U
23


Tan(

4
) = S4/C4 = U
23
/U
13




4

= Atan2(U
13
, U
23
)


With the given and back
-
substituted values
(from the arm joints) we have a value for

4
the RHS is completely known

Solving for

5

&

6


For

5
: Select (1,3) & (2,3) terms



S5 = C4U
13

+ S4U
23



C5 = U
33



Tan(

5
) = S5/C5 = (C4U
13

+ S4U
23
)/U
33




5

= Atan2(U
33
, C4U
13

+ S4U
23
)



For

6
: Select (3,1) & (3, 2)


S6 = C4U
21



S4U
11



C6 = C4U
22



S4U
12


Tan(

6
) = S6/C6 = ([C4U
21



S4U
11
],[C4U
22



S4U
12
])




6
= Atan2 ([C4U
21



S4U
11
], [C4U
22



S4U
12
])

Summarizing:



4

= Atan2(U
13
, U
23
)




5

= Atan2(U
33
, C4U
13

+ S4U
23
)




6
= Atan2 ([C4U
21



S4U
11
], [C4U
22



S4U
12
])

Lets Try One:


Cylindrical Robot w/ Spherical Wrist


Given a Target matrix (it’s an IKS after all!)


The d
3

“constant” is 400mm; the d
6

offset (call
it the ‘Hand Span’) is 150 mm.



1 = Atan2((d
x



a
x
*150),(d
y
-
a
y
*150))


d2 = (dz


a
z
*150)


d3 = ((d
x



a
x
*150)
2
,(d
y
-
a
y
*150)
2
)
.5

-

400


The Frame Skeleton:

X
Z
F
0
F
1
Z
X
X
F
2
Z
Z
X
F
6
F
2.5
Z
X
F
4
X
F
3
Z
F
5
Z
X
X
Z
Note “Dummy” Frame to
account for Orientation
problem with Spherical
Wrist

Solving for U:

1 1 0 0 0 1 0 0 1 0 1 0
1 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 a
U S C n o a
n o a
 
         
         
       
         
         
 
         
NOTE: We needed a “Dummy Frame” to
account for the Orientation issue at the
end of the Arm

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
x z x z x z
y y y
U U U C n S n C s S s C a S a
U U U S n C n S s C s S a C a
U U U n o a
 
  
 
 
 
   
 
 
 
 
 
 
Subbing U
ij
’s Into Spherical Wrist Joint Models:



4

= Atan2(U
13
, U
23
)

= Atan2((C1a
x

+ S1a
z
), (S1a
x
-
C1a
z
))




5

= Atan2(U
33
, C4U
13

+ S4U
23
)

= Atan2{a
y
, [C4(C1a
x
+S1a
z
) + S4 (S1a
x
-
C1a
z
)]}





6
= Atan2 ([C4U
21

-

S4U
11
], [C4U
22

-

S4U
12
])

= Atan2{[C4(S1n
x
-
C1n
z
)
-

S4(C1n
x
+S1n
z
)],



[
C4(S1s
x
-
C1s
z
)
-

S4(C1s
x
+S1s
z
)]}