Professional Documents
Culture Documents
Lecture 5 Inverse Kinematics
Lecture 5 Inverse Kinematics
01 Lecture 5
HCM City Univ. of Technology, Faculty of Mechanical Engineering Phung Tri Cong
Robotics 5.02 Lecture 5
CONTENTS
1. BASIC TECHNIQUE
2. DECOUPLING TECHNIQUE
4. ITERATIVE TECHNIQUE
6. SINGULAR CONFIGURATION
HCM City Univ. of Technology, Faculty of Mechanical Engineering Phung Tri Cong
Robotics 5.03 Lecture 5
HCM City Univ. of Technology, Faculty of Mechanical Engineering Phung Tri Cong
Robotics 5.04 Lecture 5
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
HCM City Univ. of Technology, Faculty of Mechanical Engineering Phung Tri Cong
Robotics 5.05 Lecture 5
I. BASIC TECHNIQUE
• Determination of joint variables in terms of the end-effector position and
orientation is called inverse kinematics
HCM City Univ. of Technology, Faculty of Mechanical Engineering Phung Tri Cong
Robotics 5.06 Lecture 5
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
𝑟
HCM City Univ. of Technology, Faculty of Mechanical Engineering Phung Tri Cong
Robotics 5.07 Lecture 5
I. BASIC TECHNIQUE
Hence, the solutions of the problem are
𝑐 𝑐2
𝜽 = 𝑎𝑡𝑎𝑛2 , ± 1 − 2 − 𝑎𝑡𝑎𝑛2 𝑎, 𝑏
𝑟 𝑟
Or
𝜽 = 𝑎𝑡𝑎𝑛2 𝑐, ± 𝑟 2 − 𝑐 2 − 𝑎𝑡𝑎𝑛2 𝑎, 𝑏
HCM City Univ. of Technology, Faculty of Mechanical Engineering Phung Tri Cong
Robotics 5.08 Lecture 5
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
HCM City Univ. of Technology, Faculty of Mechanical Engineering Phung Tri Cong
Robotics 5.09 Lecture 5
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
HCM City Univ. of Technology, Faculty of Mechanical Engineering Phung Tri Cong
Robotics 5.10 Lecture 5
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 )
HCM City Univ. of Technology, Faculty of Mechanical Engineering Phung Tri Cong
Robotics 5.11 Lecture 5
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
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
HCM City Univ. of Technology, Faculty of Mechanical Engineering Phung Tri Cong
Robotics 5.13 Lecture 5
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 − 𝑑𝑧
HCM City Univ. of Technology, Faculty of Mechanical Engineering Phung Tri Cong
Robotics 5.14 Lecture 5
HCM City Univ. of Technology, Faculty of Mechanical Engineering Phung Tri Cong
Robotics 5.15 Lecture 5
• The rotation matrix 0𝑹6 indicates the orientation of the end-effector and
involves only three joint variables of the wrist
HCM City Univ. of Technology, Faculty of Mechanical Engineering Phung Tri Cong
Robotics 5.16 Lecture 5
No. Joint
1 𝑅 ⊢ 𝑅 90
2 𝑅∥𝑅 0
3 𝑅 ⊢ 𝑅 90
4 𝑅 ⊢ 𝑅 −90
5 𝑅 ⊢ 𝑅 90
6 𝑅∥𝑅 0
HCM City Univ. of Technology, Faculty of Mechanical Engineering Phung Tri Cong
Robotics 5.17 Lecture 5
𝑎 = 2𝑙2 𝑑𝑥2 + 𝑑𝑦2 − 𝑑22 , 𝑏 = 2𝑙2 𝑑𝑧 , 𝑐 = 𝑑𝑥2 + 𝑑𝑦2 + 𝑑𝑧2 − 𝑑22 + 𝑙22 − 𝑙32
HCM City Univ. of Technology, Faculty of Mechanical Engineering Phung Tri Cong
Robotics 5.19 Lecture 5
HCM City Univ. of Technology, Faculty of Mechanical Engineering Phung Tri Cong
Robotics 5.20 Lecture 5
HCM City Univ. of Technology, Faculty of Mechanical Engineering Phung Tri Cong
Robotics 5.21 Lecture 5
HCM City Univ. of Technology, Faculty of Mechanical Engineering Phung Tri Cong
Robotics 5.22 Lecture 5
𝑻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
HCM City Univ. of Technology, Faculty of Mechanical Engineering Phung Tri Cong
Robotics 5.23 Lecture 5
HCM City Univ. of Technology, Faculty of Mechanical Engineering Phung Tri Cong
Robotics 5.27 Lecture 5
HCM City Univ. of Technology, Faculty of Mechanical Engineering Phung Tri Cong
Robotics 5.29 Lecture 5
HCM City Univ. of Technology, Faculty of Mechanical Engineering Phung Tri Cong
Robotics 5.32 Lecture 5
HCM City Univ. of Technology, Faculty of Mechanical Engineering Phung Tri Cong
Robotics 5.35 Lecture 5
HCM City Univ. of Technology, Faculty of Mechanical Engineering Phung Tri Cong
Robotics 5.36 Lecture 5
HCM City Univ. of Technology, Faculty of Mechanical Engineering Phung Tri Cong
Robotics 5.37 Lecture 5
HCM City Univ. of Technology, Faculty of Mechanical Engineering Phung Tri Cong
Robotics 5.39 Lecture 5
HCM City Univ. of Technology, Faculty of Mechanical Engineering Phung Tri Cong
Robotics 5.40 Lecture 5
HCM City Univ. of Technology, Faculty of Mechanical Engineering Phung Tri Cong
Robotics 5.41 Lecture 5
HCM City Univ. of Technology, Faculty of Mechanical Engineering Phung Tri Cong
Robotics 5.42 Lecture 5
HCM City Univ. of Technology, Faculty of Mechanical Engineering Phung Tri Cong
Robotics 5.44 Lecture 5
HCM City Univ. of Technology, Faculty of Mechanical Engineering Phung Tri Cong