5 Inverse Kinematics

Robotics 5.
01 Lecture 5
05. Inverse Kinematics
HCM City Univ. of Technology, Faculty of Mechanical Engineering Phung Tri Cong
Robotics 5.02 Lecture 5
CONTENTS
1. BASIC TECHNIQUE
2. DECOUPLING TECHNIQUE
3. INVERSE TRANSFORMATION TECHNIQUE
4. ITERATIVE TECHNIQUE
5. COMPARISON OF THE INVERSE KINEMATICS TECHNIQUES
6. SINGULAR CONFIGURATION
• What are the joint variables for a given configuration of a robot?

 This is the inverse kinematic problem
• The determination of the joint variables reduces to solving a set of
nonlinear coupled algebraic equations
• There are a few analytic and numerical methods to solve (no standard)
• The main difficult of inverse kinematic is the multiple solutions
I. BASIC TECHNIQUE
• Computer controlled robots are usually actuated in the joint variable
space, however objects to be manipulated are usually expressed in the
global Cartesian coordinate frame
• To control the configuration of the end-effector to reach an object, we

need to know what the required values of joint variables are, to reach a
desired point in a desired orientation
I. BASIC TECHNIQUE
• Determination of joint variables in terms of the end-effector position and
orientation is called inverse kinematics
• Mathematically, inverse kinematics is searching for the elements of 𝒒

𝒒 = 𝑞1 𝑞2 𝑞3 ⋯ 𝑞𝑛 𝑇
When a transformation 0𝑻𝑛 is given as a function of the joint variables
0
𝑻𝑛 = 0𝑻1 𝑞1 1𝑻2 𝑞2 2𝑻3 𝑞3 3𝑻4 𝑞4 ⋯ 𝑛−1𝑻𝑛 𝑞𝑛
I. BASIC TECHNIQUE
• Example 187 (Solution of trigonometric equation: 𝑎 𝑐𝑜𝑠 𝜃 + 𝑏 𝑠𝑖𝑛 𝜃 = 𝑐)
The first type of trigonometric equation can be solved by introducing two
new variables 𝑟 and 𝜙 such that
𝑎 = 𝑟 𝑠𝑖𝑛 𝜙
𝑏 = 𝑟 cos 𝜙
So 𝑟 and 𝜙 can be
𝑟 = 𝑎2 + 𝑏 2
𝜙 = 𝑎𝑡𝑎𝑛2 𝑎, 𝑏
Substituting the new variables show that
𝑐
sin 𝜙 + 𝜃 =
𝑟
𝑐2
cos 𝜙 + 𝜃 = ± 1 − 2
𝑟
I. BASIC TECHNIQUE
Hence, the solutions of the problem are
𝑐 𝑐2
𝜽 = 𝑎𝑡𝑎𝑛2 , ± 1 − 2 − 𝑎𝑡𝑎𝑛2 𝑎, 𝑏
𝑟 𝑟
Or
𝜽 = 𝑎𝑡𝑎𝑛2 𝑐, ± 𝑟 2 − 𝑐 2 − 𝑎𝑡𝑎𝑛2 𝑎, 𝑏
I. BASIC TECHNIQUE
• Example 184 (Inverse kinematics for a 2R planar manipulator)
Consider a 2R manipulator with two 𝑅 ∥ 𝑅 links with forward kinematics
𝑐 𝜃1 + 𝜃2 −𝑠 𝜃1 + 𝜃2 0 𝑙1 𝑐𝜃1 + 𝑙2 𝑐 𝜃1 + 𝜃2
0
𝑻2 = 𝑠 𝜃1 + 𝜃2 𝑐 𝜃1 + 𝜃2 0 𝑙1 𝑠𝜃1 + 𝑙2 𝑠 𝜃1 + 𝜃2
0 0 1 0
0 0 0 1
The global position of the tip point of the manipulator is at
𝑋 𝑙 𝑐𝜃 + 𝑙2 𝑐 𝜃1 + 𝜃2
= 1 1
𝑌 𝑙1 𝑠𝜃1 + 𝑙2 𝑠 𝜃1 + 𝜃2
Therefore
𝑋 2 + 𝑌 2 = 𝑙12 + 𝑙22 + 2𝑙1 𝑙2 cos 𝜃2
And
𝑋 2 + 𝑌 2 − 𝑙12 − 𝑙22
cos 𝜃2 =
2𝑙1 𝑙2
I. BASIC TECHNIQUE
We employ the half angle formula
𝜃 1 − cos 𝜃
𝑡𝑎𝑛2 =
2 1 + cos 𝜃
To find 𝜃2 using an atan function
𝑙1 + 𝑙2 2 − 𝑋 2 + 𝑌 2
𝜽𝟐 = ±2 𝑎𝑡𝑎𝑛
𝑋 2 + 𝑌 2 − 𝑙1 − 𝑙2 2
I. BASIC TECHNIQUE
The first joint variable 𝜃1 of an elbow up configuration can be
𝜽𝟏 = 𝑎𝑡𝑎𝑛2(𝑌, 𝑋) + 𝑎𝑡𝑎𝑛2(𝑙2 sin 𝜃2 , 𝑙1 + 𝑙2 cos 𝜃2 )
And for an elbow down configuration is
𝜽𝟏 = 𝑎𝑡𝑎𝑛2 𝑌, 𝑋 − 𝑎𝑡𝑎𝑛2(𝑙2 sin 𝜃2 , 𝑙1 + 𝑙2 cos 𝜃2 )
I. BASIC TECHNIQUE
• Example 182 (An articulated manipulator)
Consider an articulated manipulator, links of the manipulator are 𝑅 ⊢
𝑅 90 , 𝑅 ∥ 𝑅 0 , 𝑅 ⊢ 𝑅 90
The forward kinematics of the manipulator is
0
𝑻3 = 0𝑻1 1𝑻2 2𝑻3
𝑐𝜃1 𝑐 𝜃2 + 𝜃3 𝑠𝜃1 𝑐𝜃1 𝑠 𝜃2 + 𝜃3 𝑙2 𝑐𝜃1 𝑐𝜃2

𝑠𝜃1 𝑐 𝜃2 + 𝜃3 −𝑐𝜃1 𝑠𝜃1 𝑠 𝜃2 + 𝜃3 𝑙2 𝑐𝜃2 𝑠𝜃1
=
𝑠 𝜃2 + 𝜃3 0 −𝑐 𝜃2 + 𝜃3 𝑙1 + 𝑙2 𝑠𝜃2
0 0 0 1
Therefore, the tip point 𝑃 is at
𝑑𝑥 0
0𝐝 = 𝑑 0
𝑃 𝑦 = 𝑻3 0
𝑑𝑧 𝑙3
I. BASIC TECHNIQUE
𝑑𝑥 𝑙3 sin 𝜃2 + 𝜃3 cos 𝜃1 + 𝑙2 cos 𝜃1 cos 𝜃2
𝑑𝑦 = 𝑙3 sin 𝜃2 + 𝜃3 sin 𝜃1 + 𝑙2 sin 𝜃1 cos 𝜃2
𝑑𝑧 𝑙1 − 𝑙3 cos 𝜃2 + 𝜃3 + 𝑙2 sin 𝜃2
The first angle can be found from
𝑑𝑥 sin 𝜃1 − 𝑑𝑦 cos 𝜃1 = 0
So we have
𝜽𝟏 = 𝑎𝑡𝑎𝑛2 𝑑𝑦 , 𝑑𝑥
We combine the first and second elements of 0𝐝𝑃 to find
𝑑𝑥 cos 𝜃1 + 𝑑𝑦 sin 𝜃1 = 𝑙3 sin 𝜃2 + 𝜃3 + 𝑙2 cos 𝜃2
Now, combining this equation and the third element of 0𝐝𝑃 provides
2
𝑑𝑧 − 𝑙1 − 𝑙2 sin 𝜃2 2
+ 𝑑𝑥 cos 𝜃1 + 𝑑𝑦 sin 𝜃1 − 𝑙2 cos 𝜃2 = 𝑙32
I. BASIC TECHNIQUE
We receive a trigonometric equation
𝑎 cos 𝜃2 + 𝑏 sin 𝜃2 = 𝑐
𝑎 = −2𝑙2 𝑑𝑥 cos 𝜃1 + 𝑑𝑦 sin 𝜃1
𝑏 = 2𝑙2 𝑙1 − 𝑑𝑧
2
𝑐= 𝑙32 − 𝑑𝑥 cos 𝜃1 + 𝑑𝑦 sin 𝜃1 + 𝑙12 − 2𝑙1 𝑑𝑧 + 𝑙22 + 𝑑𝑧2
We solve this equation for 𝜽𝟐
From the third element of 0𝐝𝑃 , we also have
𝑑𝑥 cos 𝜃1 + 𝑑𝑦 sin 𝜃1 − 𝑙2 cos 𝜃2
tan 𝜃2 + 𝜃3 =
𝑙1 + 𝑙2 sin 𝜃2 − 𝑑𝑧
𝑑𝑥 cos 𝜃1 + 𝑑𝑦 sin 𝜃1 − 𝑙2 cos 𝜃2
𝜽𝟑 = 𝑎𝑡𝑎𝑛 − 𝜃2
𝑙1 + 𝑙2 sin 𝜃2 − 𝑑𝑧
II. DECOUPLING TECHNIQUE

• The result of forward kinematics of a 6 DOF robot is
𝑟11 𝑟12 𝑟13 𝑟14
0 𝑟21 𝑟22 𝑟23 𝑟24
𝑻6 =
𝑟31 𝑟32 𝑟33 𝑟34
0 0 0 1
• It is possible to decouple the inverse kinematics problem into two sub-

problems, known as inverse position and inverse orientation kinematics

• Following the decoupling principle, the overall transformation matrix of a
robot can be decomposed to a translation and a rotation
0 0 0 0
0
𝑻6 = 𝑹 6 𝐝 6 = 𝑫 0 0
𝑹 = 𝐈 𝐝6 𝑹6 0
6 6
0 1 0 1 0 1
• The translation matrix 0𝑫6 indicates the position of the end-effector in

𝑩0 and involved only three joint variables of the manipulator
• The rotation matrix 0𝑹6 indicates the orientation of the end-effector and
involves only three joint variables of the wrist

• Example 186 (Inverse kinematics of an articulated robot)
Consider an articulated robot where the overall transformation matrix of
end-effector as found based on wrist and arm transformation matrices
0𝑻 = 𝑻 0 3
7 𝑎𝑟𝑚 𝑻𝑤𝑟𝑖𝑠𝑡 = 𝑻3 𝑻7
No. Joint
1 𝑅 ⊢ 𝑅 90
2 𝑅∥𝑅 0
3 𝑅 ⊢ 𝑅 90
4 𝑅 ⊢ 𝑅 −90
5 𝑅 ⊢ 𝑅 90
6 𝑅∥𝑅 0

The tool transformation matrix in the base coordinate frame is
𝑡11 𝑡12 𝑡13 𝑡14
0 𝑡 𝑡22 𝑡23 𝑡24
𝑻7 = 0𝑻3 3𝑻6 6𝑻7 = 21
𝑡31 𝑡32 𝑡33 𝑡34
0 0 0 1
Solution of the inverse kinematics problem starts with the wrist position
vector d, which is 𝑡14 𝑡24 𝑡34 𝑇 of 0𝑻7 for 𝑑7 = 0
𝑐𝜃1 𝑙3 𝑠 𝜃2 + 𝜃3 + 𝑙2 𝑐𝜃2 + 𝑑2 𝑠𝜃1 𝑑𝑥
𝐝 = 𝑠𝜃1 𝑙3 𝑠 𝜃2 + 𝜃3 + 𝑙2 𝑐𝜃2 − 𝑑2 𝑐𝜃1 = 𝑑𝑦
𝑙3 𝑐 𝜃2 + 𝜃3 + 𝑙2 𝑠𝜃2 𝑑𝑧
Theoretically, we must be able to solve for the three joint variables 𝜃1 , 𝜃2 ,
and 𝜃3 . It can be seen that
𝑑𝑥 sin 𝜃1 − 𝑑𝑦 cos 𝜃1 = 𝑑2
 𝜽𝟏 = 𝑎𝑡𝑎𝑛2 𝑑2 , ± 𝑑𝑥 2 + 𝑑𝑦 2 − 𝑑2 2 − 𝑎𝑡𝑎𝑛2 −𝑑𝑦 , 𝑑𝑥


Combining the first two elements of d gives
𝑙3 sin 𝜃2 + 𝜃3 = ± 𝑑𝑥2 + 𝑑𝑦2 − 𝑑22 − 𝑙2 cos 𝜃2
Then, the third element of d may be utilized to find

2
𝑙32 = ± 𝑑𝑥2 + 𝑑𝑦2 − 𝑑22 − 𝑙2 cos 𝜃2 + 𝑑𝑧 − 𝑙2 sin 𝜃2 2
Which can be rearranged to the following form

𝑎 cos 𝜃2 + 𝑏 sin 𝜃2 = 𝑐
𝑎 = 2𝑙2 𝑑𝑥2 + 𝑑𝑦2 − 𝑑22 , 𝑏 = 2𝑙2 𝑑𝑧 , 𝑐 = 𝑑𝑥2 + 𝑑𝑦2 + 𝑑𝑧2 − 𝑑22 + 𝑙22 − 𝑙32
We have two solutions

𝑐 𝑐2
𝜽𝟐 = 𝑎𝑡𝑎𝑛2 ,± 1− − 𝑎𝑡𝑎𝑛2 𝑎, 𝑏 𝑟 2 = 𝑎2 + 𝑏 2
𝑟 𝑟2

Summing the squares of the elements of d gives
𝑑𝑥2 + 𝑑𝑦2 + 𝑑𝑧2 = 𝑑22 + 𝑙22 + 𝑙32 + 2𝑙2 𝑙3 sin 2𝜃2 + 𝜃3
That provides
𝑑𝑥2 + 𝑑𝑦2 + 𝑑𝑧2 − 𝑑22 − 𝑙22 − 𝑙32
𝜽𝟑 = arcsin − 2𝜃2
2𝑙2 𝑙3

Because the joint variables in 0𝑇3 and in 3𝑇6 are independent, we should
find the orientation of end-effector by solving 3𝑅6 for 𝜃4 , 𝜃5 and 𝜃6
𝑐𝜃4 𝑐𝜃5 𝑐𝜃6 − 𝑠𝜃4 𝑠𝜃6 −𝑐𝜃6 𝑠𝜃4 − 𝑐𝜃4 𝑐𝜃5 𝑠𝜃6 𝑐𝜃4 𝑠𝜃5
3
𝑅6 = 𝑐𝜃5 𝑐𝜃6 𝑠𝜃4 + 𝑐𝜃4 𝑠𝜃6 𝑐𝜃4 𝑐𝜃6 − 𝑐𝜃5 𝑠𝜃4 𝑠𝜃6 𝑠𝜃4 𝑠𝜃5
−𝑐𝜃6 𝑠𝜃5 𝑠𝜃5 𝑠𝜃6 𝑐𝜃5
𝑠11 𝑠12 𝑠13
= 𝑠21 𝑠22 𝑠23
𝑠31 𝑠32 𝑠33
Thus, we have
𝜽𝟒 = 𝑎𝑡𝑎𝑛2 𝑠23 , 𝑠13
2 2
𝜽𝟓 = 𝑎𝑡𝑎𝑛2 𝑠13 + 𝑠23 , 𝑠33
𝜽𝟔 = 𝑎𝑡𝑎𝑛2 𝑠32 , −𝑠31
III. INVERSE TRANSFORMATION TECHNIQUE

• Assume we have transformation matrix 0𝑻6 indicating the global
position and the orientation of end-effector of a 6 DOF robot in base
frame 𝑩0
• Assume the geometry and individual transformation matrices 0𝑻1 𝑞1 ,
1
𝑻2 𝑞2 , 2𝑻3 𝑞3 , 3𝑻4 𝑞4 , 4𝑻5 𝑞5 , and 5𝑻6 𝑞6 are given as
functions of joint variables
• According to forward kinematics
0
𝑻6 = 0𝑻1 1𝑻2 2𝑻3 3𝑻4 4𝑻5 5𝑻6
𝑟11 𝑟12 𝑟13 𝑟14
𝑟21 𝑟22 𝑟23 𝑟24
=
𝑟31 𝑟32 𝑟33 𝑟34
0 0 0 1

• We can solve the inverse kinematics problem by solving the following
equations for the unknown joint variables
𝑻6 = 0𝑻1−1 0𝑻6
1
2 0 −1 0
𝑻6 = 1𝑻−12 𝑻1 𝑻6
3
𝑻6 = 2𝑻3−1 1𝑻2−1 0𝑻1−1 0𝑻6
4
𝑻6 = 3𝑻−14
2 −1 1 −1 0 −1 0
𝑻3 𝑻2 𝑻1 𝑻6
5 3 −1 2 −1 1 −1 0 −1 0
𝑻6 = 4𝑻−1
5 𝑻4 𝑻3 𝑻2 𝑻1 𝑻6
𝑰 = 5𝑻6−1 4𝑻5−1 3𝑻−1
4
2 −1 1 −1 0 −1 0
𝑻3 𝑻2 𝑻1 𝑻6

• Example 191 (Articulated manipulator and numerical case)
Consider an articulated manipulator. Point 𝑷 is supposed to be the point at
which we attach a spherical wrist. We attach a coordinate frame 𝑩4 at 𝑷 that
is at a constant distance 𝑙3 from 𝑩3
1 0 0 0
3 0 1 0 0
𝑻4 =
0 0 1 𝑙3
0 0 0 1
The overall forward kinematics is
𝑐1 𝑐23 𝑠1 𝑐1 𝑠23 𝑙3 𝑠23 𝑐1 + 𝑙2 𝑐1 𝑐2

0 𝑠 𝑐 −𝑐1 𝑠1 𝑠23 𝑙3 𝑠23 𝑠1 + 𝑙2 𝑐2 𝑠1
𝑻4 = 1 23
𝑠23 0 −𝑐23 𝑙1 − 𝑙3 𝑐23 + 𝑙2 𝑠2
0 0 0 1

Using the following dimensions and its tip point
𝑙1 = 1𝑚, 𝑙2 = 1.05𝑚, 𝑙3 = 0.89𝑚, 0𝐝𝑃 = 1 1.1 1.2 𝑇
The forward kinematics reduces to
cos 𝜃2 + 𝜃3 cos 𝜃1 sin 𝜃1 sin 𝜃2 + 𝜃3 cos 𝜃1 1
0 cos 𝜃2 + 𝜃3 sin 𝜃1 − cos 𝜃1 sin 𝜃2 + 𝜃3 sin 𝜃1 1.1
𝑻4 =
sin 𝜃2 + 𝜃3 0 − cos 𝜃2 + 𝜃3 1.2
0 0 0 1
Let us multiply both side by 0𝑻1−1 to have
0 −1 0
𝑻1 𝑻4 = 0𝑻1−1 0𝑻1 1𝑻2 2𝑻3 3𝑻4 = 1𝑻4
The left side is
cos 𝜃2 + 𝜃3 0 sin 𝜃2 + 𝜃3 cos 𝜃1 + 1.1 sin 𝜃1
0 −1
𝑻1 0
𝑻4 = sin 𝜃2 + 𝜃3 0 − cos 𝜃2 + 𝜃3 0.2
0 1 0 sin 𝜃1 − 1.1 cos 𝜃1
0 0 0 1

The right side is
c 𝜃2 + 𝜃3 0 s 𝜃2 + 𝜃3 0.89𝑠 𝜃2 + 𝜃3 + 1.05𝑐𝜃2
1
𝑻4 = 1𝑻2 2𝑻3 3𝑻4 = s 𝜃2 + 𝜃3 0 − c 𝜃2 + 𝜃3 1.05𝑠𝜃2 − 0.89𝑐 𝜃2 + 𝜃3
0 1 0 0
0 0 0 1
Equating the element 𝑟34 of both sides provide an equation to determine 𝜃1
sin 𝜃1 − 1.1 cos 𝜃1 = 0
𝜽𝟏 = 𝑎𝑡𝑎𝑛2 1.1, 1 ≈ 47.72 𝑑𝑒𝑔
We multiply both sides by 1𝑻−1
2 to have
1 −1 1 3
𝑻2 𝑻4 = 1𝑻−1 2
1
𝑻 2
2
𝑻3 𝑻 4 = 2
𝑻4
cos 𝜃3 0 sin 𝜃3 1.4866 cos 𝜃2 + 0.2 sin 𝜃2 − 1.05
− cos 𝜃3 0.2 cos 𝜃2 − 1.4866 sin 𝜃2
1 −1
𝑻2 1
𝑻4 = sin 𝜃3 0
0 1 0 0
0 0 0 1

The right side can be
cos 𝜃3 0 sin 𝜃3 0.89 sin 𝜃3
− cos 𝜃3 −0.89 cos 𝜃3
2
𝑻4 = 2𝑻3 3𝑻4 = sin 𝜃3 0
0 1 0 0
0 0 0 1
Squaring the elements 𝑟14 and 𝑟24 provides an equation to determine 𝜃2
1.4866 cos 𝜃2 + 0.2 sin 𝜃2 − 1.05 2 + 0.2 cos 𝜃2 − 1.4866 sin 𝜃2 2
= 0.89 sin 𝜃3 2 + −0.89 cos 𝜃3 2 = 0.892
This equation has the following solutions
𝜽𝟐 ≈ 43.28 𝑑𝑒𝑔 or 𝜽𝟐 = −27.96 𝑑𝑒𝑔
Having 𝜃2 , we can calculate 𝜃3 from
1.4866 cos 𝜃2 + 0.2 sin 𝜃2 − 1.05
𝜽𝟑 = 𝑎𝑡𝑎𝑛2
0.2 cos 𝜃2 − 1.4866 sin 𝜃2

• Example 192 (Inverse kinematics for a spherical robot)
The position and orientation of end-effector for a set of joint variables can
be found by matrix multiplication
0
𝑻6 = 0𝑻1 1𝑻2 2𝑻3 3𝑻4 4𝑻5 5𝑻6
𝑟11 𝑟12 𝑟13 𝑟14
𝑟21 𝑟22 𝑟23 𝑟24
= (6.165)
𝑟31 𝑟32 𝑟33 𝑟34
0 0 0 1
Multiplying both sides of (6.165) by
0 −1
𝑻1 provides
𝑓11 𝑓12 𝑓13 𝑓14
0 −1 0 𝑓21 𝑓22 𝑓23 𝑓24
𝑻1 𝑻6 =
𝑓31 𝑓32 𝑓33 𝑓34
0 0 0 1

Where
𝑓3𝑖 = 𝑟2𝑖 cos 𝜃1 − 𝑟1𝑖 sin 𝜃1 , 𝑖 = 1,2,3,4
The right side of (6.165) after multiplying is
𝑓11 𝑓12 𝑓13 𝑑3 𝑠𝜃2
𝑓 𝑓 𝑓 −𝑑3 𝑐𝜃2
1
𝑻6 = 1𝑻2 2𝑻3 3𝑻4 4𝑻5 5𝑻6 = 21 22 23
𝑓31 𝑓32 𝑓33 𝑙2
0 0 0 1
Equating element 𝑎34 in both sides we have
𝑟24 cos 𝜃1 − 𝑟14 sin 𝜃1 = 𝑙2
Hence, the solution for 𝜃1 is
𝑟
−1 24 −1
𝑙2
𝜽𝟏 = 𝑡𝑎𝑛 − 𝑡𝑎𝑛
𝑟14 ± 𝑟 2 + 𝑟 2 − 𝑙2 14 24 2

Equating elements 𝑎14 and 𝑎24 in both sides we have
𝑓14 = 𝑑3 sin 𝜃2 = 𝑟14 cos 𝜃1 + 𝑟24 sin 𝜃1
𝑓24 = −𝑑3 cos 𝜃2 = −𝑟34
Hence, it is possible to use them and find 𝜃2
𝑟 cos 𝜃1 + 𝑟24 sin 𝜃1
−1 14
𝜽𝟐 = 𝑡𝑎𝑛
𝑟34
In the next step, we find the third joint variable 𝑑3 from
1 −1 0 −1 0
𝑻2 𝑻1 𝑻6 = 2𝑻6 (6.194)
Employing the elements of the matrices on both sides of (6.194) shows that
the element (3,4) can be utilized to find 𝑑3
𝒅𝟑 = 𝑟34 cos 𝜃2 + 𝑟14 cos 𝜃1 sin 𝜃2 + 𝑟24 sin 𝜃1 sin 𝜃2
We move to the next step and evaluate 𝜃4 from
3 −1 2 −1 1 −1 0 −1 0
𝑻4 𝑻3 𝑻2 𝑻1 𝑻6 = 4𝑻6

Two sides can be
𝑔11 𝑔12 𝑔13 𝑔14 𝑐𝜃5 𝑐𝜃6 −𝑐𝜃5 𝑠𝜃6 𝑠𝜃5 0
𝑔21 𝑔22 𝑔23 𝑔24 𝑐𝜃6 𝑠𝜃5 −𝑠𝜃5 𝑠𝜃6 −𝑐𝜃5 0
=
𝑔31 𝑔32 𝑔33 𝑔34 𝑠𝜃6 𝑐𝜃6 0 0
0 0 0 1 0 0 0 1
We can find 𝜃4 by equating element (3,3)
𝑟13 −𝑐𝜃4 𝑠𝜃1 − 𝑐𝜃1 𝑐𝜃2 𝑠𝜃4 + 𝑟23 𝑐𝜃1 𝑐𝜃4 − 𝑐𝜃2 𝑠𝜃1 𝑠𝜃4 + 𝑟33 𝑠𝜃2 𝑠𝜃4 = 0
So 𝜃4 can be
−1
−𝑟13 𝑠𝜃1 + 𝑟23 𝑐𝜃1
𝜽𝟒 = 𝑡𝑎𝑛
𝑐𝜃2 𝑟13 𝑐𝜃1 + 𝑟23 𝑠𝜃1 − 𝑟33 𝑠𝜃2
Now we use elements (1,3) and (2,3)
sin 𝜃5 = 𝑟23 𝑐𝜃1 𝑠𝜃4 + 𝑐𝜃2 𝑐𝜃4 𝑠𝜃1 − 𝑟33 𝑐𝜃4 𝑠𝜃2 + 𝑟13 𝑐𝜃1 𝑐𝜃2 𝑐𝜃4 − 𝑠𝜃1 𝑠𝜃4
− cos 𝜃5 = −𝑟33 𝑐𝜃2 − 𝑟13 𝑐𝜃1 𝑠𝜃2 − 𝑟23 𝑠𝜃1 𝑠𝜃2


To find 𝜃5
sin 𝜃5 −1
𝜽𝟓 = 𝑡𝑎𝑛
cos 𝜃5
Finally, 𝜃6 can be found from the elements (3,1) and (3,2)
sin 𝜃6 = 𝑟31 𝑠𝜃2 𝑠𝜃4 + 𝑟21 𝑐𝜃1 𝑐𝜃4 − 𝑐𝜃2 𝑠𝜃1 𝑠𝜃4 + 𝑟11 (−𝑐𝜃4 𝑠𝜃1
IV. ITERATIVE TECHNIQUE

• The inverse kinematics problem can be interpreted as searching for the
solution 𝒒𝒌 of a set of nonlinear algebraic equations
0
𝑻𝑛 = 𝑻 𝐪
= 0𝑻1 𝑞1 1𝑻2 𝑞2 2𝑻3 𝑞3 3𝑻4 𝑞4 ⋯ 𝑛−1𝑻𝑛 𝑞𝑛
𝑟11 𝑟12 𝑟13 𝑟14
𝑟21 𝑟22 𝑟23 𝑟24
=
𝑟31 𝑟32 𝑟33 𝑟34
0 0 0 1
𝑛 is the number of DOF
However, maximum 𝑛 = 𝟔 out of 12 equations are independent and can be
utilized to solve for joint variables 𝑞𝑘
Numerous methods are available to find the solutions. In general, the
methods are iterative. The most common method is Newton-Raphson
method

• In the iterative technique, to solve the kinematic equations
𝑻 𝐪 =0
for variables 𝐪, we start with an initial guess
𝐪∗ = 𝐪 + 𝛅𝐪
for the joint variables. Using the forward kinematics, we can determine the
configuration of end-effector frame for guessed joint variables
𝐓 ∗ = 𝐓 𝐪∗
The difference between configuration calculate and desired configuration is
δ𝐓 = 𝐓 − 𝐓 ∗
A first order Taylor expansion of the set of equation is
∗ ∗
𝜕𝐓
𝐓 = 𝐓 𝐪 + δ𝐪 = 𝐓 𝐪 + δ𝐪 + O δ𝐪2
𝜕𝐪
Assuming δ𝐪 ≪ 𝐈 then δ𝐓 = 𝐉δ𝐪

Where 𝐉 is the Jacobian matrix of the set of equations
𝜕𝑇𝑖
𝐉 𝐪 =
𝜕𝑞𝑗
That implies
δ𝐪 = 𝐉 −𝟏 δ𝐓
Therefore, the unknown variable 𝑞 are
𝐪 = 𝐪∗ + 𝐉 −1 𝛅𝐓
We may use this value as a new approximation to repeat the calculation and
find newer values
Repeating the methods can be summarized in following iterative equation
𝐪 i+1 = 𝐪 i + 𝐉 −𝟏 𝐪 i δ𝐓 𝐪 i
Inverse kinematics iteration technique

1. Set the initial counter 𝑖 = 0
0
2. Find or guess an initial estimate 𝐪
𝑖 𝑖 𝑖
3. Calculate the residue 𝛿𝐓 𝐪 =𝐉 𝐪 𝛿𝐪
If every element of 𝐓 𝐪 𝑖 or its norm 𝐓 𝐪 𝑖 is less than a tolerance,
𝐓 𝐪𝑖 < 𝜀 then terminate the iteration. The 𝐪 𝑖
is the desired solution
4. Calculate 𝒒 𝑖+1 = 𝒒 𝑖 + 𝑱−𝟏 𝒒 𝑖
𝛿𝑻 𝒒 𝑖
5. Set 𝑖 = 𝑖 + 1 and return to step 3

• Example 196 (Inverse kinematics for a 2R planar manipulator)
The tip point of a 2R planar manipulator can be described by
𝑋 𝑙1 𝑐𝜃1 + 𝑙2 𝑐 𝜃1 + 𝜃2
=
𝑌 𝑙1 𝑠𝜃1 + 𝑙2 𝑠 𝜃1 + 𝜃2
To solve the inverse kinematics of the manipulator and find the joint
coordinates for a known position of the tip point, we define
𝜃1 𝑋
𝐪= 𝐓=
𝜃2 𝑌
Therefore, the Jacobian of the equation is
𝜕𝑋 𝜕𝑋
𝜕𝑇𝑖 𝜕𝜃1 𝜃2 −𝑙1 𝑠𝜃1 − 𝑙2 𝑠 𝜃1 + 𝜃2 −𝑙2 𝑠 𝜃1 + 𝜃2
𝐉 𝐪 = = =
𝜕𝑞𝑗 𝜕𝑌 𝜕𝑌 𝑙1 𝑐𝜃1 + 𝑙2 𝑐 𝜃1 + 𝜃2 𝑙2 𝑐 𝜃1 + 𝜃2
𝜃1 𝜕𝜃2

The inverse of the Jacobian is
−𝟏
−1 −𝑙2 𝑐 𝜃1 + 𝜃2 −𝑙2 𝑠 𝜃1 + 𝜃2
𝐉 =
𝑙1 𝑙2 𝑠𝜃2 𝑙1 𝑐𝜃1 + 𝑙2 𝑐 𝜃1 + 𝜃2 𝑙1 𝑠𝜃1 + 𝑙2 𝑠 𝜃1 + 𝜃2
Therefore, the iterative formula is set up as
𝑖+1 𝑖 𝑖
𝜃1 𝜃 𝑋 𝑋
= 1 +𝐉 −1
−
𝜃2 𝜃2 𝑌 𝑌
Let‘s assume
𝑋 1
𝑙1 = 𝑙2 = 1 𝐓= =
𝑌 1
And start from a guess value
0
𝜃 𝜋/3
𝐪 0
= 1 =
𝜃2 −𝜋/3
1
1 𝑐𝜋/3 + 𝑐 𝜋/3 + −𝜋/3 −
2
For which 𝛿𝐓 = − = 1
1 𝑠𝜋/3 + 𝑠 𝜋/3 + −𝜋/3 − 3+1
2

The Jacobian and its inverse for these values are
1 2
− 3 0 −3 3 0
2 −1
𝐉= 3 𝐉 =
2
1 3 1
And therefore
𝜃1 1 𝜃1 0 −1 1.6245
= + 𝐉 𝛿𝐓 =
𝜃2 𝜃2 −1.7792
Based on the iterative technique, we can find the following values
1 1.6245 2 1.583 3 1.57079
𝐪 = 𝐪 = 𝐪 =
−1.7792 −1.582 −1.57086
1.570796329
𝐪4 =
−1.570796329
The result of 𝐪 4 is close enough to the exact value 𝐪 = 𝜋/2 −𝜋/2 𝑇
V. COMPARISON OF THE INVERSE KINEMATICS TECHNIQUES

1. Existence and Uniqueness of Solution
- When desired tool frame position 0𝑻7 is outside working space of
robot, there can not be any real solution for the joint variables of robot
- Even when tool frame position is within working space, there maybe
some tool orientation that are not achievable without breaking joint
constraints and violating one or more joint variable limits
- Therefore, existing solution for inverse kinematics problem generally
depends on the geometric configuration of the robot
- The normal case when number of joint is six, the inverse kinematics
solution exists in finite numbers
- Multiple solutions appear because a robot can reach to a point within
the working space in different configurations

1. Existence and Uniqueness of Solution
- When number of joints is less than six, no solution exists unless freedom
is reduced in the same time in the task space
- When number of joints exceeds six, structure becomes redundant and

an infinite number of solution exists to reach the same end-effector
configuration within the robot workspace

2. Inverse Kinematics Techniques
- The inverse kinematics problem of robot can be solved by several
methods: decoupling, inverse transformation, iterative, screw algebra,
dual matrices, dual quaternions, and geometric techniques
- The decoupling and inverse transformation technique using 4x4
homogeneous transformation matrices, rely on the skills and intuition of
the engineer
- The iterative solution method often requires a vast amount of
computation, doesn’t guarantee convergence to correct solution. It is
weak when robot is close to the singular and degenerate configurations
- The sufficient condition of solvability is when the 6 DOF robot has

three consecutive revolute joints with axes intersecting in one point
VI. SINGULAR CONFIGURATION

• Singular configuration is: when number of DOF of end-effector is
inferior to the dimension in which it generally operates
• Singular configuration happens when

+ Two axes of prismatic joints become parallel
+ Two axes of revolute joints become identical
• At singular positions, end-effector loses one or more DOF, since the

kinematic equations become linearly dependent or certain solutions
become undefined
• Singular positions must be avoided as the velocities required to move the

end-effector become theoretically infinite

• Singular configurations can be determined from the Jacobian matrix. The
Jacobian matrix relates the infinitesimal displacement of end-effector to
infinitesimal joint variables
𝛿𝐗 = 𝐉 δ𝐪
Jacobian matrix also determines the relationship between end-effector
velocities 𝐗 and joint velocities 𝐪
𝐗=𝐉𝐪
When 𝐉 is degenerate or Jacobian no longer has full rank or 𝑑𝑒𝑡 𝐉 = 0,
configuration corresponds to singularities of the robot
+ Workspace boundary singularities: end-effector near at the
workspace boundary
+ Workspace interior singularities: occurring away from boundary,
generally two or more axes line up

• Identification and avoidance of singularity configurations are very
important in robotics. Some of the main reasons are
+ Certain directions of motion may be unattainable
+ Some of the joint velocities are infinite
+ Some of the joint torques are infinite
+ There will not exist a unique solution to the inverse kinematics
problem

5 Inverse Kinematics

Uploaded by

Document Information

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

5 Inverse Kinematics

Uploaded by

Copyright:

Available Formats

Robotics 5.

05. Inverse Kinematics

3. INVERSE TRANSFORMATION TECHNIQUE

5. COMPARISON OF THE INVERSE KINEMATICS TECHNIQUES

• What are the joint variables for a given configuration of a robot?

• To control the configuration of the end-effector to reach an object, we

• Mathematically, inverse kinematics is searching for the elements of 𝒒

𝑐𝜃1 𝑐 𝜃2 + 𝜃3 𝑠𝜃1 𝑐𝜃1 𝑠 𝜃2 + 𝜃3 𝑙2 𝑐𝜃1 𝑐𝜃2

II. DECOUPLING TECHNIQUE

• It is possible to decouple the inverse kinematics problem into two sub-

II. DECOUPLING TECHNIQUE

• The translation matrix 0𝑫6 indicates the position of the end-effector in

II. DECOUPLING TECHNIQUE

II. DECOUPLING TECHNIQUE

 𝜽𝟏 = 𝑎𝑡𝑎𝑛2 𝑑2 , ± 𝑑𝑥 2 + 𝑑𝑦 2 − 𝑑2 2 − 𝑎𝑡𝑎𝑛2 −𝑑𝑦 , 𝑑𝑥

II. DECOUPLING TECHNIQUE

𝑙3 sin 𝜃2 + 𝜃3 = ± 𝑑𝑥2 + 𝑑𝑦2 − 𝑑22 − 𝑙2 cos 𝜃2

Then, the third element of d may be utilized to find

Which can be rearranged to the following form

We have two solutions

II. DECOUPLING TECHNIQUE

II. DECOUPLING TECHNIQUE

𝜽𝟔 = 𝑎𝑡𝑎𝑛2 𝑠32 , −𝑠31

III. INVERSE TRANSFORMATION TECHNIQUE

III. INVERSE TRANSFORMATION TECHNIQUE

III. INVERSE TRANSFORMATION TECHNIQUE

𝑐1 𝑐23 𝑠1 𝑐1 𝑠23 𝑙3 𝑠23 𝑐1 + 𝑙2 𝑐1 𝑐2

III. INVERSE TRANSFORMATION TECHNIQUE

III. INVERSE TRANSFORMATION TECHNIQUE

III. INVERSE TRANSFORMATION TECHNIQUE

III. INVERSE TRANSFORMATION TECHNIQUE

III. INVERSE TRANSFORMATION TECHNIQUE

III. INVERSE TRANSFORMATION TECHNIQUE

III. INVERSE TRANSFORMATION TECHNIQUE

− cos 𝜃5 = −𝑟33 𝑐𝜃2 − 𝑟13 𝑐𝜃1 𝑠𝜃2 − 𝑟23 𝑠𝜃1 𝑠𝜃2

III. INVERSE TRANSFORMATION TECHNIQUE

IV. ITERATIVE TECHNIQUE

IV. ITERATIVE TECHNIQUE

IV. ITERATIVE TECHNIQUE

IV. ITERATIVE TECHNIQUE

Inverse kinematics iteration technique

IV. ITERATIVE TECHNIQUE

IV. ITERATIVE TECHNIQUE

IV. ITERATIVE TECHNIQUE

V. COMPARISON OF THE INVERSE KINEMATICS TECHNIQUES

V. COMPARISON OF THE INVERSE KINEMATICS TECHNIQUES

- When number of joints exceeds six, structure becomes redundant and

V. COMPARISON OF THE INVERSE KINEMATICS TECHNIQUES

- The sufficient condition of solvability is when the 6 DOF robot has

VI. SINGULAR CONFIGURATION

• Singular configuration happens when

• At singular positions, end-effector loses one or more DOF, since the

• Singular positions must be avoided as the velocities required to move the

VI. SINGULAR CONFIGURATION

VI. SINGULAR CONFIGURATION

You might also like