Lecture 5: Kinematics: Inverse Kinematics - umu.se · Problem Formulation Given a 4× 4matrix of...
Transcript of Lecture 5: Kinematics: Inverse Kinematics - umu.se · Problem Formulation Given a 4× 4matrix of...
Lecture 5: Kinematics: Inverse Kinematics
• Problem Formulation
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 1/15
Lecture 5: Kinematics: Inverse Kinematics
• Problem Formulation
• Kinematic Decoupling
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 1/15
Problem Formulation
Given a 4 × 4 matrix of homogeneous transformation
H =
R o
0 1
∈ SE(3)
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 2/15
Problem Formulation
Given a 4 × 4 matrix of homogeneous transformation
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
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 2/15
Problem Formulation
Given a 4 × 4 matrix of homogeneous transformation
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
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 2/15
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
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 3/15
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
• For a closed loop system qk(t) are references to follow,they should be often computed as fast as possible;
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 3/15
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
• 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!
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 3/15
Lecture 5: Kinematics: Inverse Kinematics
• Problem Formulation
• Kinematic Decoupling
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 4/15
Kinematic Decoupling
Given R ∈ SO(3) and o ∈ R3
• Problem of inverse kinematics is quite difficult
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 5/15
Kinematic Decoupling
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
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 5/15
Kinematic Decoupling
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 6/15
Kinematic Decoupling: How to compute o0
c and R0
3, R3
6?
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 6/15
Kinematic Decoupling:
If the last three joint axes intersect in one point then
o = o0
c + d6 ·R
0
0
1
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 7/15
Kinematic Decoupling:
If the last three joint axes intersect in one point then
o = o0
c + d6 ·R
0
0
1
Therefore,
o0
c =
xc
yc
zc
o− d6 ·R
0
0
1
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 7/15
Kinematic Decoupling:
If the last three joint axes intersect in one point then
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
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 7/15
Inverse Position Problem: What are θ1, θ2 and θ3?
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 8/15
The first angle is given by
θ1 = Atan2(xc, yc)
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 9/15
Singular Configuration for defining θ1
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 10/15
Inverse Position Problem: What are θ2 and θ3?
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 11/15
To find θ2, θ3, consider the plane formed by 2nd and 3rd links
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 12/15
cos θ3 =r2 + s2 − a2
2− a2
3
2a2a3
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 12/15
cos θ3 =r2 + s2 − a2
2− a2
3
2a2a3
=r2 + (zc − d1)
2 − a2
2− a2
3
2a2a3
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 12/15
cos θ3 =r2 + (zc − d1)
2 − a2
2− a2
3
2a2a3
=x2
c + y2
c + (zc − d1)2 − a2
2− a2
3
2a2a3
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 12/15
θ2 = Atan2(r, s) − Atan2(b, c)
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 12/15
θ2 = Atan2(r, s) − Atan2(a2 + a3 cos θ3, a3 sin θ3)
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 12/15
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 13/15
Link 1: a = 0, α = π2
, d = d1, θ = θ1 ⇒ H0
1
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 13/15
Link 1: a = 0, α = π2
, d = d1, θ = θ1 ⇒ H0
1
Link 2: a = a2, α = 0, d = 0, θ = θ2 ⇒ H1
2
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 13/15
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
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 13/15
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
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 14/15
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
Therefore, we can compute the rotation matrix
R3
6=
[
R0
3
]
−1R =
r11 r12 r13
r21 r22 r23
r31 r32 r33
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 14/15
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
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,ψ
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 14/15
Euler Angles:
Euler angles are angles of 3 rotations about current axes
RZYZ := Rz,φ ·Ry,θ ·Rz,ψ
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 15/15
Euler Angles:
Euler angles are angles of 3 rotations about current axes
RZYZ :=
cφ −sφ 0
sφ cφ 0
0 0 1
·Ry,θ ·Rz,ψ
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 15/15
Euler Angles:
Euler angles are angles of 3 rotations about current axes
RZYZ :=
cφ −sφ 0
sφ cφ 0
0 0 1
·
cθ 0 sθ
0 1 0
−sθ 0 cθ
·Rz,ψ
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 15/15
Euler Angles:
Euler angles are angles of 3 rotations about current axes
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
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 15/15
Euler Angles:
Euler angles are angles of 3 rotations about current axes
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θ
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 15/15
Euler Angles:
Euler angles are angles of 3 rotations about current axes
(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
c©Anton Shiriaev. 5EL158: Lecture 5 – p. 15/15