Lecture 5: Kinematics: Inverse Kinematics - umu.se · Problem Formulation Given a 4× 4matrix of homogeneous transformation H= R o 0 1 ∈ SE(3) cAnton Shiriaev. 5EL158: Lecture 5–

Lecture 5: Kinematics: Inverse Kinematics

• Problem Formulation

c©Anton Shiriaev. 5EL158: Lecture 5 – p. 1/15



• Kinematic Decoupling


Problem Formulation

Given a 4 × 4 matrix of homogeneous transformation

H =

R o

0 1

∈ SE(3)


Problem Formulation


H =

R o

0 1

∈ SE(3)

The task is to find a solution (possibly one of many) of theequation

T 0

n

(

q1, . . . , qn)

= A1(q1)A2(q2) · · ·An(qn) = H


Problem Formulation


H =

R o

0 1

∈ SE(3)

The task is to find a solution (possibly one of many) of theequation

T 0

n

(

q1, . . . , qn)

= A1(q1)A2(q2) · · ·An(qn) = H

It is 12 equations with respect to n variables q1, . . . , qn


Requirements on a Solution

It is often advantageous to find a solution of

T 0

n

(

q1, . . . , qn)

= H =

h11 h12 h13 h14

h21 h22 h23 h24

h31 h32 h33 h34

0 0 0 1

in analytical form

qk = fk(

h11, h12, . . . , h34

)

, k = 1, . . . , n




T 0

n

(

q1, . . . , qn)

= H =

h11 h12 h13 h14

h21 h22 h23 h24

h31 h32 h33 h34

0 0 0 1

in analytical form

qk = fk(

h11, h12, . . . , h34

)

, k = 1, . . . , n

• For a closed loop system qk(t) are references to follow,they should be often computed as fast as possible;




T 0

n

(

q1, . . . , qn)

= H =

h11 h12 h13 h14

h21 h22 h23 h24

h31 h32 h33 h34

0 0 0 1

in analytical form

qk = fk(

h11, h12, . . . , h34

)

, k = 1, . . . , n

• For a closed loop system qk(t) are references to follow,they should be often computed as fast as possible;

• If there are several solutions, then on-line numericalprocedure can find another one and references will have ajump!




• Kinematic Decoupling


Kinematic Decoupling

Given R ∈ SO(3) and o ∈ R3

• Problem of inverse kinematics is quite difficult



Given R ∈ SO(3) and o ∈ R3

• Problem of inverse kinematics is quite difficult

• If manipulator has

◦ at least 6 joints

◦ the last 3 joint axes intersecting in one point

then the problem is decoupled into sub-problems

◦ inverse position kinematics

◦ inverse orientation kinematics




Kinematic Decoupling: How to compute o0

c and R0

3, R3

6?


Kinematic Decoupling:

If the last three joint axes intersect in one point then

o = o0

c + d6 ·R

0

0

1




o = o0

c + d6 ·R

0

0

1

Therefore,

o0

c =

xc

yc

zc

o− d6 ·R

0

0

1




o = o0

c + d6 ·R

0

0

1

Therefore,

o0

c =

xc

yc

zc

o− d6 ·R

0

0

1

We know that

R = R0

3·R3

6⇒ R3

6=

[

R0

3

]

−1R =

[

R0

3

]

T

R


Inverse Position Problem: What are θ1, θ2 and θ3?


The first angle is given by

θ1 = Atan2(xc, yc)


Singular Configuration for defining θ1


Inverse Position Problem: What are θ2 and θ3?


To find θ2, θ3, consider the plane formed by 2nd and 3rd links


cos θ3 =r2 + s2 − a2

2− a2

3

2a2a3


cos θ3 =r2 + s2 − a2

2− a2

3

2a2a3

=r2 + (zc − d1)

2 − a2

2− a2

3

2a2a3


cos θ3 =r2 + (zc − d1)

2 − a2

2− a2

3

2a2a3

=x2

c + y2

c + (zc − d1)2 − a2

2− a2

3

2a2a3


θ2 = Atan2(r, s) − Atan2(b, c)


θ2 = Atan2(r, s) − Atan2(a2 + a3 cos θ3, a3 sin θ3)



Link 1: a = 0, α = π2

, d = d1, θ = θ1 ⇒ H0

1


Link 1: a = 0, α = π2

, d = d1, θ = θ1 ⇒ H0

1

Link 2: a = a2, α = 0, d = 0, θ = θ2 ⇒ H1

2


Link 1: a = 0, α = π2

, d = d1, θ = θ1 ⇒ H0

1

Link 2: a = a2, α = 0, d = 0, θ = θ2 ⇒ H1

2

Link 3: a = a3, α = 0, d = 0, θ = θ3 ⇒ H2

3


Inverse Orientation Kinematics

Given R ∈ SO(3) and o ∈ R3,

We have computed H0

3= H0

1H1

2H2

3⇒ R0

3, o0

3= o0

c




We have computed H0

3= H0

1H1

2H2

3⇒ R0

3, o0

3= o0

c

Therefore, we can compute the rotation matrix

R3

6=

[

R0

3

]

−1R =

r11 r12 r13

r21 r22 r23

r31 r32 r33




We have computed H0

3= H0

1H1

2H2

3⇒ R0

3, o0

3= o0

c

Therefore, we can compute the rotation matrix

R3

6=

[

R0

3

]

−1R =

r11 r12 r13

r21 r22 r23

r31 r32 r33

We need to compute, for example, Euler angles (φ, θ, ψ) suchthat

R3

6= Rz,φ ·Ry,θ ·Rz,ψ


Euler Angles:

Euler angles are angles of 3 rotations about current axes

RZYZ := Rz,φ ·Ry,θ ·Rz,ψ


Euler Angles:


RZYZ :=

cφ −sφ 0

sφ cφ 0

0 0 1

·Ry,θ ·Rz,ψ


Euler Angles:


RZYZ :=

cφ −sφ 0

sφ cφ 0

0 0 1

·

cθ 0 sθ

0 1 0

−sθ 0 cθ

·Rz,ψ


Euler Angles:


RZYZ :=

cφ −sφ 0

sφ cφ 0

0 0 1

·

cθ 0 sθ

0 1 0

−sθ 0 cθ

·

cψ −sψ 0

sψ cψ 0

0 0 1


Euler Angles:


RZYZ :=

(cφcθcψ − sφsψ) −(cφcθsψ + sφcψ) cφsθ

(sφcθcψ + cφsψ) (cφcψ − sφcθsψ) sφsθ

−sθcψ sθsψ cθ


Euler Angles:


(cφcθcψ − sφsψ) −(cφcθsψ + sφcψ) cφsθ

(sφcθcψ + cφsψ) (cφcψ − sφcθsψ) sφsθ

−sθcψ sθsψ cθ

=

r11 r12 r13

r21 r22 r23

r31 r32 r33


Documents

Lecture 5: Kinematics: Inverse Kinematics - umu.se · Problem Formulation Given a 4× 4matrix of homogeneous transformation H= R o 0 1 ∈ SE(3) cAnton Shiriaev. 5EL158: Lecture 5–