inverse kinematics-geometric+algebraic

8/18/2019 Inverse Kinematics-Geometric+Algebraic

1/22

Inverse Kinematics

Geometric and Algebraic Approaches

Mechanism and Robot Kinematics2015


2/22

A Simple Example

θ1

X

Y

S

Revolute and

Prismatic

Joints

Combined

(x , y)

Finding θ:

)x

yarctan(θ =

More Specifically:

)x

y(2arctanθ =

arctan2() specifies that it’s in the

first quadrant

Finding S:

)y(xS 22+=


3/22

θ2

θ1

(x , y)

l2

l1

Inverse Kinematics of a Two Link

ManipulatorGiven: l1, l2 , x , y

Find: θ1, θ2

Redundancy:

A unique solution to this

problem does not exist. Notice, thatusing the “givens” two solutions are

possible.

Sometimes no solution is possible.

(x , y)


4/22

The Geometric

Solution

l1

l2 2

1

(x , y) Using the Law of Cosines:

−−+=

−−+=

−=−

−−+=+

−+=

21

2

2

2

1

22

21

2

2

2

1

22

21

2

2

2

1

22

222

2arccosθ

2)cos(θ

)cos(θ)θ180cos(

)θ180cos(2)(

cos2

llll y x

ll

ll y x

llll y x

C abbac

2

2

22

2

Using the Law of Cosines:

=

+=

+=

+

−=

=

x

y2arctanα

αθθ

yx

)sin(θ

yx

)θsin(180θsin

sinsin

11

22

2

22

2

2

1

l

c

C

b

B

+

+=

x

y2arctan

yx

)sin(θarcsinθ

22

221

l

Redundant since θ2 could be in the

first or fourth quadrant.

Redundancy caused since θ2 has two

possible values


5/22

( ) ( )( )

−−+=∴

++=

+++=

+++++=

=+=+

++

++++

21

2

2

2

1

22

2

221

2

2

2

1

21121121

2

2

2

1

21121

2

21

2

2

2

1

2

121121

2

21

2

2

2

1

2

1

2222

2

yxarccosθ

c2

)(sins)(cc2

)(sins2)(sins)(cc2)(cc

yx)2((1)

ll

ll

llll

llll

llllllll

The Algebraic Solution

l1

l2 θ2

θ1

(x , y)

21

21211

21211

1221

11

θθθ(3)

sinsy(2)

ccx(1)

)θcos(θc

cosθc

+=

+=

+=

+=

=

+

+

+

ll

ll

Only

Unknown

))(sin(cos))(sin(cos)sin(

))(sin(sin))(cos(cos)cos(

:

abbaba

bababa

Note

+

−

+

−

−+

+−

=

=


6/22

))(sin(cos))(sin(cos)sin(

))(sin(sin))(cos(cos)cos(

:

abbaba

bababa

Note

+−

+−

−+

+−

=

=

)c(s)s(c

cscss

sinsy

)()c(c

ccc

ccx

2211221

12221211

21211

2212211

21221211

21211

lll

lll

ll

slsll

sslll

ll

++=

++=

+=

−+=

−+=

+=

+

+

We know what θ2 is from the

previous slide. We need to solve

for θ1 . Now we have two

equations and two unknowns (sin

θ1 and cos θ1 )

( )

22

222211

2212

22

1122221

221122221

221

221

2211

yx

x)c(ys

)c2(sx)c(

1

)c(s)s()c(

)(xy

)c(

)(xc

+

−+=

++++

=

+++

+=

+

+=

slll

llllslll

lllll

sls

ll

sls

Substituting for c1 and

simplifying many times

Notice this is the law ofcosines and can be replaced

by x2+ y2

+

−+=

22

222211

yx

x)c(yarcsinθ

slll


7/22

Inverse Kinematics

7

• given the pose of the end effector, find the joint variables that produce the end effectorpose

• for a 6-joint robot, given

find

=

1

0

6

0

60

60

o RT

654321 ,,,,, qqqqqq


8/22

RPP + Spherical Wrist

8


9/22

RPP + Spherical Wrist

9

• solving for the joint variables directly is hard

==

1000333231

232221

131211

3

6

0

3

0

6

z

y

x

d r r r

d r r r

d r r r

T T T

21654

651641654111

d d d ssd

csssscccccr

z ++−=

+−=


10/22

Kinematic Decoupling

10

• for 6-joint robots where the last 3 jointsintersecting at a point (e.g., last 3 joints arespherical wrist) there is a simpler way to solve

the inverse kinematics problem1. use the intersection point (wrist center) to solve

for the first 3 joint variables

• inverse position kinematics

2. use the end-effector pose to solve for the last 3 joint variables

• inverse orientation kinematics


11/22

Spherical Wrist

11

d 6

pc

p


12/22

RPP Cylindrical Manipulator

3/10/2015 12

xc

yc

pc

d 3*

d 2*

d 1


13/22

RRP Spherical Manipulator

3/10/2015 13


14/22

Spherical Wrist

14

Link ai α i d i θ i

4 0 -90 0 θ 4*

5 0 90 0 θ 5*

6 0 0 d 6θ

6** joint variable


15/22

Spherical Wrist

15

−

+−+

−−−

==

1000

6556565

654546465464654

654546465464654

5

6

4

5

3

4

3

6

d ccsscs

d ssssccscsscccs

d scsccssccssccc

T T T T


16/22

Spherical Wrist

16

d 6

pc

p

−=1

0

0

066

06

0 Rd p pc


17/22

Inverse Kinematics Recap

17

1. Solve for the first 3 joint variables q1, q2, q3

such that the wrist center pc has coordinates

2. Using the results from Step 1, compute

3. Solve for the wrist joint variables q4, q5, q6

corresponding to the rotation matrix

0

3 R

( ) 060336 R R R T

=

−=10

00

66

0

6

0

Rd p pc


18/22

Spherical Wrist

18

• for the spherical wrist

−

+−+

−−−

==

1000

6556565

654546465464654

654546465464654

5

6

4

5

3

4

3

6 d ccsscs

d ssssccscsscccs

d scsccssccssccc

T T T T

( )( )33233neg5

33

2

33

pos

5

5

,1atan2

,1atan2

0if

r r

r r

s

−−=

−=

≠

θ

θ


19/22

Spherical Wrist

19

−

+−+−−−

==

1000

6556565

654546465464654

654546465464654

5

6

4

5

3

4

3

6d ccsscs

d ssssccscsscccs

d scsccssccssccc

T T T T

( )( )31326

13234

5

pos

5

,atan2

,atan2

0,for

r r

r r

s

−=

=

>

θ

θ

θ


20/22

Spherical Wrist

20

−

+−+−−−

==

1000

6556565

654546465464654

654546465464654

5

6

4

5

3

4

3

6d ccsscs

d ssssccscsscccs

d scsccssccssccc

T T T T

( )( )31326

13234

5

neg

5

,atan2

,atan2

0,for

r r

r r

s

−=

−−=

<

θ

θ

θ


21/22

Spherical Wrist

21

• if θ 5 = 0

−

+−+−−−

==

1000

6556565

654546465464654

654546465464654

5

6

4

5

3

4

3

6d ccsscs

d ssssccscsscccs

d scsccssccssccc

T T T T

+−+

−−−

=

1000

100

00

00

6

64646464

64646464

d

ccsssccs

csscsscc


22/22

Spherical Wrist

22

+−+−−−

=

1000

100

00

00

6

64646464

64646464

d

ccsssccs

csscsscc

−

=

++

++

1000

100

00

00

6

6464

6464

d

cs

sc

only the sum θ 4+θ 6

can be determined

inverse kinematics-geometric+algebraic

Documents