Professional Documents
Culture Documents
L5 - Inverse Kinematics V1
L5 - Inverse Kinematics V1
L5 - Inverse Kinematics V1
Given World Space Position (x, y), find the Joint Space Solution
Finding θ1
Revolute and
Prismatic Joints ⎛ y⎞
Combined θ1 = arctan ⎜ ⎟
⎝x⎠
More Specifically:
(x , y)
⎛ y⎞
θ1 = arctan 2 ⎜ ⎟ arctan2( ) specifies that
Y
⎝x⎠ it’s in the first quadrant
r Finding r:
θ1 X
r= ( x 2
+ y 2
)
Inverse Kinematics of a Two Link Manipulator
l2 Redundancy:
A unique solution to this problem
does not exist. Notice, that using the
“givens” two solutions are possible.
θ1 l1
Sometimes no solution is possible.
l2 (x , y)
l2
l1 l1
The Geometric Solution
(x , y) Using the Law of Cosines:
θ2 l2
c 2 = a 2 + b 2 − 2ab cos C
2 2
( x 2 + y 2 ) = l1 + l2 − 2l1l2 cos(180 − θ 2 )
Universiti Kuala Lumpur Malaysia France Institute
cos(180 − θ 2 ) = − cos(θ 2 )
2 2
l1 θ1 x 2 + y 2 − l1 − l2
α cos(θ 2 ) =
2l1l2
θ1
⎛ x 2 + y 2 − l12 − l2 2 ⎞
θ 2 = arccos⎜⎜ ⎟
⎟
Using the Law of Cosines: ⎝ 2l1l2 ⎠
sin B sin C Redundant since θ2 could be in the
=
b c first or fourth quadrant.
sinθ1 sin(180 − θ 2 ) sin(θ 2 )
= =
l2 x 2 + y2 x 2 + y2 Redundancy caused since θ2 has two
θ1 = θ1 − α possible values
θ1 = θ1 + α ⎛ l sin(θ ) ⎞ ⎛ y⎞
θ1 = arcsin⎜ 2 2 ⎟
+ arctan 2⎜ ⎟
⎛y⎞ ⎜ x 2 + y2 ⎟ ⎝x⎠
α = arctan 2 ⎜ ⎟ ⎝ ⎠
⎝x⎠
The Algebraic Solution
(x , y) Υ
θ2 l2
c 1 = c o sθ 1
Universiti Kuala Lumpur Malaysia France Institute
c 1 + 2 = c o s(θ 1 + θ 2 )
l1 (1 ) x = l1 c 1 + l 2 c 1 + 2
θ1 (2 ) y = l1 s 1 + l 2 sin 1 + 2
(3 ) θ = θ 1 + θ 2
(1)2 + (2) 2 = x 2 + y 2 =
( 2 2 2
) ( 2 2 2
= l1 c1 + l2 (c1+ 2 ) 2 + 2l1l2 c1 (c1+ 2 ) + l1 s1 + l2 (sin1+ 2 ) 2 + 2l1l2 s1 (sin1+ 2 ) )
= l1 + l2 + 2l1l2 (c1 (c1+ 2 ) + s1 (sin1+ 2 ) )
2 2
2 2
= l1 + l2 + 2l1l2 c 2 Only Unknown
Note:
⎛ x 2 + y 2 − l12 − l2 2 ⎞ cos(a−+b) = (cosa)(cosb)−+ (sina)(sinb)
∴ θ 2 = arccos⎜⎜ ⎟
⎟
⎝ 2 l l
1 2 ⎠ sin(a−+b) = (cosa)(sinb)+− (cosb)(sina)
x = l1 c 1 + l 2 c 1 + 2 Note:
= l1 c 1 + l 2 c 1 c 2 − l 2 s 1 s 2 cos(a−+b) = (cosa)(cosb)−+ (sina)(sinb)
sin(a−+b) = (cosa)(sinb)+− (cosb)(sina)
Universiti Kuala Lumpur Malaysia France Institute
= c 1 ( l1 + l 2 c 2 ) − s 1 ( l 2 s 2 )
=
1
( l1 + l 2 c 2 )
(
x l 2 s 2 + s 1 ( l1 2 + l 2 2 + 2 l1 l 2 c 2 ) ) Notice this is the law of
cosines and can be
replaced by x2+ y2
y ( l1 + l 2 c 2 ) − x l 2 s 2 ⎛ y ( l1 + l 2 c 2 ) − x l 2 s 2 ⎞
s1 = θ 1 = arcsin ⎜⎜ ⎟
x2+ y2 x2+ y2 ⎟
⎝ ⎠
Doing a Pure IKS solution: the θR
Manipulator
Universiti Kuala Lumpur Malaysia France Institute
X1 X2
Same Point!
Y0 Z1 Z2
Y1 Y2
X0
Z0
0ª1 1 R θ+ 0 0 90 1 0 C1 -S1
90
1ª2 2 P 0 d2 + 0 0 1 0 1 0
cl2
⎡ − S1 0 C1 0⎤ ⎡1 0 0 0 ⎤
⎢ C1 0 S1 0⎥ ⎢0 1 0 0 ⎥
A1 = ⎢ ⎥ A2 = ⎢ ⎥
⎢ 0 1 0 0⎥ ⎢0 0 1 d 2 + cl2 ⎥
⎢ ⎥ ⎢ ⎥
⎣ 0 0 0 1⎦ ⎣0 0 0 1 ⎦
FKS is A1*A2:
Universiti Kuala Lumpur Malaysia France Institute
⎡ − S1 0 C1 0 ⎤ ⎡1 0 0 0 ⎤
⎢ C1 0 S1 0 ⎥ ⎢ 0 1 0 0 ⎥
⎢ ⎥•⎢ ⎥=
⎢ 0 1 0 0⎥ ⎢0 0 1 d 2 + cl2 ⎥
⎢ ⎥ ⎢ ⎥
⎣ 0 0 0 1 ⎦ ⎣0 0 0 1 ⎦
⎡ − S1 0 C1 C1 (d 2 + cl2 ) ⎤
⎢ C1 0 S1 S1 (d 2 + cl2 ) ⎥
⎢ ⎥
⎢ 0 1 0 0 ⎥
⎢ ⎥
⎣ 0 0 0 1 ⎦
Forming The IKS:
o
F ⎡On ,o ,a Dx , y , z ⎤
Universiti Kuala Lumpur Malaysia France Institute
d
F =⎢ ⎥
z0 a ⎣ 0 1 ⎦
n KNOWN
y0
x0
⎡ − S1 0 C1 C1 ( d 2 + cl2 ) ⎤ ⎡ nx ox ax dx ⎤
⎢ C1 0 S1 S1 ( d 2 + cl2 ) ⎥ ⎢ n y oy ay dy ⎥
⎢ ⎥ =⎢ ⎥
⎢ 0 1 0 0 ⎥ ⎢ nz oz az dz ⎥
⎢ ⎥ ⎢ ⎥
⎣ 0 0 0 1 ⎦ ⎣0 0 0 1⎦
In the Inverse Problem, The RHS MATRIX is
completely known (perhaps from a robot mapped
solution)! And we use these values to find a solution
to the joint equation that populate the LHS MATRIX
Forming The IKS:
Universiti Kuala Lumpur Malaysia France Institute
S1/C1 = dy/ dx
Tan θ = dy/dx
θ = Atan2(dx, dy)
Forming The IKS:
Universiti Kuala Lumpur Malaysia France Institute
⎡1 0 0 0 ⎤ ⎡ − S1 C1 0 0 ⎤ ⎡ nx ox ax dx ⎤
⎢0 1 0 0 ⎥ ⎢ 0 0 1 0⎥ ⎢ ny oy ay dy ⎥
⎢ ⎥=⎢ ⎥•⎢ ⎥
⎢0 0 1 d 2 + cl2 ⎥ ⎢ C1 S1 0 0 ⎥ ⎢ nz oz az dz ⎥
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎣0 0 0 1 ⎦ ⎣ 0 0 0 1⎦ ⎣ 0 0 0 1⎦
Solving:
Universiti Kuala Lumpur Malaysia France Institute
Prismatic:
q1 = d1= Pz (its along Z0!) – cl1
q2 = d2 = Px or Py - cl2
q3 = d3= Py or Px - cl3
Cylindrical:
θ1 = Atan2(Px, Py)
d2 = Pz – cl2
d3 = Px/C1 – cl3 {or +(Px2 + Py2).5 – cl3}
Focusing on the ARM Manipulators
in terms of Pc:
Universiti Kuala Lumpur Malaysia France Institute
Spherical:
θ1 = Atan2(Px, Py)
θ2 = Atan2( (Px2 + Py2).5 , Pz)
D3 = (Px2 + Py2 + Pz2).5 – cl3
Focusing on the ARM Manipulators
in terms of Pc:
Universiti Kuala Lumpur Malaysia France Institute
Articulating:
θ1 = Atan2(Px, Py)
θ3 = Atan2(D, ±(1 – D2).5)
Where D = (P x
2
+ Py2 + Pz2 − a22 − a32 )
2 a2 a3
θ2 = φ-α
φ is: Atan2((Px2 + Py2).5, Pz)
A tan 2(sin α )=
cos α
α is:
( )
⎜ 2 3 (
⎛ 2a a ± 1 − D 2
) ⎞
⎟
A tan 2 ⎜
⎜ ( Px + Py + Pz + a2 − a3 ) ⎟
2 2 2 2 2 ⎟
⎝ ⎠
Focusing on the ARM Manipulators
in terms of Pc:
Universiti Kuala Lumpur Malaysia France Institute
(
⎜
A tan 2 ⎜
2 3 (
⎛ 2a a ± 1 − D 2
) ) ⎞
⎟
⎜ ( Px + Py + Pz + a2 − a3 ) ⎟
2 2 2 2 2 ⎟
⎝ ⎠
Where D = ( P x
2
+ Py2 + Pz2 − a22 − a32 )
2 a 2 a3
One Further Complication Must Be
Considered:
Universiti Kuala Lumpur Malaysia France Institute
The Zpc
The ARM Wrist
Z0 d 2 Xpc
Y0, Z1
Ypc
X0, X1
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 θ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” – a plan
view of the structure projected to the X0 Y0 plane
Universiti Kuala Lumpur Malaysia France Institute
Pc'(Px, Py)
Y0
a3'
d2
Z1 a 2'
R' φ1
α1 X1
X 1 = R′
d2
θ1
X0
Z1'
X1'
Solving For θ1:
Universiti Kuala Lumpur Malaysia France Institute
(
− A tan 2 ( X pc
2
+ Y pc2 − d 22 ) , d 2
.5
)
θ 12 = 180° + α1 + φ1 =
180° + A tan 2 ( )
( X pc2 + Y pc2 − d 22 ) , d 2 +
.5
A tan 2 ( X pc , Y pc )
It is called a “Hard Arm” Solution
Universiti Kuala Lumpur Malaysia France Institute
R R = Rgiven
3
0
6
3
⎡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θ ⎥⎦
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:
⎡ nx ox ax ⎤
( R0 ) ⎢ny
3 −1 ⎢ ⎥
NOTE: R03 is
oy ay
⎥
Manipulator
dependent!
Inverse requires
its Transpose
⎢⎣ nz oz az ⎥⎦ given
Simplifying the U Matrix: (a RHS
matrix relative to the R36 Euler Product)
Universiti Kuala Lumpur Malaysia France Institute
⎡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θ ⎥⎦
⎡U 11 U 12 U 13 ⎤
⎢U 21 U 22 U 23⎥
⎢ ⎥ U as defined on the
previous slide! (a
⎢⎣U 31 U 32 U 33 ⎥⎦ function of arm-joint
POSE and Desired End-
Orientation)
Solving for Individual Orientation Angles
(1st we will solve for ‘θ’ the middle one):
Universiti Kuala Lumpur Malaysia France Institute
Euler Angle
θ = Atan2(U33, ±(1-U33 2).5)
Rotations
φ = Atan2(U13, U23) Rz1*Ry2*Rz3
or
Rz ,φ , Ry ,θ , Rz ,ϕ
ψ = Atan2(-U31, U32)
Same Point
Z4
Here drawn in
Y4 Y5
‘Good Kinematic
Home’ – for
attachment to X4
an Articulating Y3 X5
Arm
Z5
X3 Y6
Z3
X6
Z6
IKSing the Spherical Wrist
Universiti Kuala Lumpur Malaysia France Institute
3ª4 4 R θ4 0 0 -90
4ª5 5 R θ5 0 0 +90
5ª6 6 R θ6 d6 0 0
⎡C 4 0 − S 4 ⎤ ⎡C 5 0 S 5 ⎤
R 4 = ⎢ S 4 0 C 4 ⎥ ; R 5 = ⎢ S 5 0 − C 5 ⎥ ;
⎢ ⎥ ⎢ ⎥
⎢⎣ 0 − 1 0 ⎥⎦ ⎢⎣ 0 1 0 ⎥⎦
⎡C 6 − S 6 0 ⎤
R 6 = ⎢ S 6 C 6 0 ⎥
⎢ ⎥
⎢⎣ 0 0 1 ⎥⎦
Writing The Solution:
Universiti Kuala Lumpur Malaysia France Institute
⎡C 4 0 −S 4⎤ ⎡C5 0 S 5 ⎤ ⎡C 6 −S 6 0⎤
⎢ S 4 0 C 4 ⎥ • ⎢ S 5 0 −C5⎥ • ⎢ S 6 C 6 0⎥ =
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎢⎣ 0 −1 0 ⎥⎦ ⎢⎣ 0 1 0 ⎥⎦ ⎢⎣ 0 0 1⎥⎦
⎡U11 U12 U13 ⎤
⎢U U U ⎥
⎢ 21 22 23 ⎥ U ’s as defined on the earlier slide!
ij
⎢⎣U 31 U 32 U 33 ⎥⎦ – and –
U is manipulator and desired orientation
dependent
Let’s Solve (and try the ‘Pure Inverse’ Technique):
Universiti Kuala Lumpur Malaysia France Institute
⎡C 5 0 S 5 ⎤ ⎡C 6 − S 6 0 ⎤
⎢ S5 ⎥ ⎢
0 −C 5 • S 6 C 6 0 = ⎥
⎢ ⎥ ⎢ ⎥
⎢⎣ 0 1 0 ⎥⎦ ⎢⎣ 0 0 1 ⎥⎦
⎡ C4 S 4 0 ⎤ ⎡U11 U12 U13 ⎤
Note:
R4’s
Inverse
⎢ 0 ⎥ ⎢
0 −1 U 21 U 22 U 23 ⎥
⎢ ⎥⎢ ⎥
⎢⎣ − S 4 C 4 0 ⎥⎦ ⎢⎣U 31 U 32 U 33 ⎥⎦
Simplifying
Universiti Kuala Lumpur Malaysia France Institute
⎡C5C 6 −C5S 6 S 5 ⎤
⎢ S 5C 6 −S 5S 6 −C5⎥ =
⎢ ⎥
⎢⎣ S 6 C6 0 ⎥⎦
⎡C 4U11 + S 4U 21 C 4U12 + S 4U 22 C 4U13 + S 4U 23 ⎤
⎢ −U31 −U 32 −U 33 ⎥
⎢ ⎥
⎢⎣C 4U 21 − S 4U11 C 4U 22 − S 4U12 C 4U 23 − S 4U13 ⎥⎦
Solving:
Universiti Kuala Lumpur Malaysia France Institute
θ4 = Atan2(U13, U23)
F4
Z
X
F2.5 X F3 X
F5
F2 Z
X
X Z
Z Z
F6
F1 Z
Z
F0 Z
X
Note “Dummy” Frame to account for Orientation
X
problem with Spherical Wrist – might not be needed if
we had set wrist kinematic home for Cylindrical
Machine!
Solving for U:
Given from F Frame
(R ) 3 −1
Universiti Kuala Lumpur Malaysia France Institute
−1
⎡⎡C1 −S1 0⎤ ⎡ 0 0 −1⎤ ⎡ 0 0 1⎤ ⎡ 0 1 0⎤ ⎤ ⎡ nx ox ax ⎤
⎢ ⎥
U = ⎢⎢ S1 C1 0⎥ • ⎢−1 0 0 ⎥ • ⎢−1 0 0⎥ • ⎢−1 0 0⎥ ⎥ • ⎢ny oy ay ⎥
⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎢⎣⎢⎣ 0 0 1⎥⎦ ⎢⎣ 0 −1 0 ⎥⎦ ⎢⎣ 0 −1 0⎥⎦ ⎢⎣ 0 0 1⎥⎦ ⎥⎦ ⎢⎣ nz oz az ⎥⎦
θ4 = Atan2(U13, U23)
= Atan2((C1ax + S1az), ay)