Академический Документы
Профессиональный Документы
Культура Документы
&
A NALYSIS
M ODULE 3 K INEMATICS OF SERIAL ROBOTS
Ashitava Ghosal1
1 Department
of Mechanical Engineering
&
Centre for Product Design and Manufacture
Indian Institute of Science
Bangalore 560 012, India
Email: asitava@mecheng.iisc.ernet.in
NPTEL, 2010
.
NPTEL, 2010
1 / 96
.
. .1 C ONTENTS
.
. .2 L ECTURE 1
Introduction
Direct Kinematics of Serial Robots
.
. .3 L ECTURE 2
Inverse Kinematics of Serial Robots
.
. .4 L ECTURE 3
Inverse Kinematics of Serial Robots with n < 6
Inverse Kinematics of Serial Robots with n > 6
.
. .5 L ECTURE 4
Elimination Theory & Solution of Non-linear Equations
Inverse Kinematics of a General 6R Robot
.
. .6 A DDITIONAL M ATERIAL
Problems, References and Suggested Reading
.
NPTEL, 2010
2 / 96
O UTLINE
.
. .1 C ONTENTS
.
. .2 L ECTURE 1
Introduction
Direct Kinematics of Serial Robots
.
. .3 L ECTURE 2
Inverse Kinematics of Serial Robots
.
. .4 L ECTURE 3
Inverse Kinematics of Serial Robots with n < 6
Inverse Kinematics of Serial Robots with n > 6
.
. .5 L ECTURE 4
Elimination Theory & Solution of Non-linear Equations
Inverse Kinematics of a General 6R Robot
.
. .6 A DDITIONAL M ATERIAL
Problems, References and Suggested Reading
.
NPTEL, 2010
3 / 96
I NTRODUCTION
NPTEL, 2010
4 / 96
2
X
Link 3
l3
O3
{3}
3
Y
0
Y
1
Y
2
Y
Link 2
l2
{2}
1
X
{0}
O2
{1}
Link 1
l1
1
O1
0
X
Z 2
Z 1
2
{ 2}
{ 1}
X 2
X 1
d3
{ 3}
O 0, O1
O2
Z 3
{ 4}
O3, O4
(d) 4 DOF
SCARA Robot
.
NPTEL, 2010
5 / 96
DOF = (N J 1) + Fi
(1)
i=1
NPTEL, 2010
6 / 96
...
...
...
1
NPTEL, 2010
7 / 96
S OME D EFINITIONS
J joint variables i s or di s form the joint space.
Position and orientation variables form the task space
For planar motion, = 3 Task space (x, y , ).
For spatial motion, = 6 Task space (x, y , z; [R]).
NPTEL, 2010
8 / 96
T WO P ROBLEMS IN K INEMATICS OF
S ERIAL ROBOTS
Direct Kinematics Problem:
Given the constant D-H link parameters and the joint
variable, ai1 , i1 , di , and i i = 1, 2, ..n, find the position
and orientation of the last link in a fixed or reference
coordinate system.
Most basic problem in serial manipulator kinematics.
Required to be solved for computer visualization of motion
and in off-line programming systems.
Used in advanced control schemes.
NPTEL, 2010
9 / 96
O UTLINE
.
. .1 C ONTENTS
.
. .2 L ECTURE 1
Introduction
Direct Kinematics of Serial Robots
.
. .3 L ECTURE 2
Inverse Kinematics of Serial Robots
.
. .4 L ECTURE 3
Inverse Kinematics of Serial Robots with n < 6
Inverse Kinematics of Serial Robots with n > 6
.
. .5 L ECTURE 4
Elimination Theory & Solution of Non-linear Equations
Inverse Kinematics of a General 6R Robot
.
. .6 A DDITIONAL M ATERIAL
Problems, References and Suggested Reading
.
NPTEL, 2010
10 / 96
NPTEL, 2010
11 / 96
represents orientation of
the tool.
From 0Tool [T ]
3, X
T ool
X
{T ool}
T ool
Y
2
X
Link 3
l3
O3
{3}
3
Y
0
Y
1
Y
2
Y
Link 2
l2
1
X
{2}
{0}
x
y
= l1 c1 + l2 c12 + l3 c123
= l1 s1 + l2 s12 + l3 s123
= 1 + 2 + 3
(2)
O2
{1}
Link 1
l1
1
0
X
O1
NPTEL, 2010
12 / 96
D IRECT
SCARA
K INEMATICS
P ROBLEM :
Z 2
Z 1
represents orientation of
the {4}.
The position, (x, y , z), and
orientation of {4} is
2
{ 2}
{ 1}
X 2
X 1
d3
{ 3}
O 0, O1
O2
Z 3
x
y
z
{ 4}
O3, O4
=
=
=
=
a1 c1 + a2 c12
a1 s1 + a2 s12
d3
1 + 2 + 4
(3)
NPTEL, 2010
13 / 96
=
=
=
=
=
=
=
=
=
=
=
=
S UMMARY
NPTEL, 2010
15 / 96
O UTLINE
.
. .1 C ONTENTS
.
. .2 L ECTURE 1
Introduction
Direct Kinematics of Serial Robots
.
. .3 L ECTURE 2
Inverse Kinematics of Serial Robots
.
. .4 L ECTURE 3
Inverse Kinematics of Serial Robots with n < 6
Inverse Kinematics of Serial Robots with n > 6
.
. .5 L ECTURE 4
Elimination Theory & Solution of Non-linear Equations
Inverse Kinematics of a General 6R Robot
.
. .6 A DDITIONAL M ATERIAL
Problems, References and Suggested Reading
.
NPTEL, 2010
16 / 96
I NTRODUCTION
Inverse Kinematics Problem (restated): Given the constant
D-H link parameters and 0n [T ], find the joint variables
i , i = 1, .., n.
For 3D motion, 6 task space variables 3 position + 3
orientation in 0n [T ].
For planar motion, 3 task space variables 2 position + 1
orientation in 0n [T ].
...
...
NPTEL, 2010
17 / 96
P LANAR 3R M ANIPULATOR
Direct kinematics
equations
x
y
= l1 c1 + l2 c12 + l3 c123
= l1 s1 + l2 s12 + l3 s123
= 1 + 2 + 3
3, X
T ool
X
{T ool}
2
X
Link 3
l3
T ool
Y
O3
{3}
3
Y
0
Y
1
Y
2
Y
Link 2
l2
1
X
{2}
{0}
O2
{1}
Link 1
l1
1
0
X
O1
NPTEL, 2010
18 / 96
2 = cos
X 2 + Y 2 l12 l22
2l1 l2
(5)
)
(6)
Once 2 is known
1 = atan2(Y , X ) atan2(k2 , k1 )
(7)
3 = 1 2
(8)
.
NPTEL, 2010
19 / 96
3R M ANIPULATOR W ORKSPACE
Workspace: All (x, y , )
such that inverse
kinematics solution exists.
From equation (6)
X 2 +Y 2 l12 l22
1 (
) +1
2l1 l2
or
6
phi axis
0
10
10
5
5
0
0
5
5
Y axis
10
X axis
10
NPTEL, 2010
20 / 96
3R M ANIPULATOR :
(C ONTD .)
W ORKSPACE
l1 + l2 + l3
l1 + l2 l3
0
Y
0
X
Configuration 2
Cofiguration 1
l1 l2 + l3
l1 l2 l3
0 Y0 plane for
Projection of the workspace on X
l1 > l2 > l3 Four circles of radii l1 + l2 + l3 , l1 + l2 l3 ,
l1 l2 + l3 and l1 l2 l3 .
.
.
.
.
A SHITAVA G HOSAL (IIS C )
NPTEL, 2010
21 / 96
3R M ANIPULATOR :
(C ONTD .)
W ORKSPACE
NPTEL, 2010
22 / 96
3R M ANIPULATOR : U NIQUENESS OF
IK S OLUTIONS
Equation (6) revisited:
2 = cos1
X 2 + Y 2 l12 l22
2l1 l2
NPTEL, 2010
23 / 96
2
X
2
Y
1
Z
{1}
3
Z
1
Y
{3}
O3
1
X
a2
3
X
d4
4
X
3
Y
d3
{4}
3
X
O1 , O2
a3
O4
O3
3
Y
6
X
5
X
{5}
{6}
4
X
4
Y
4
Z
{3}
6
Z
{4}
4
Z
5
Y
O4 , O5 , O6
NPTEL, 2010
24 / 96
= a2 s2 a3 s23 d4 c23
(10)
NPTEL, 2010
25 / 96
1
,
2
c1 =
1 x12
,
1 + x12
s1 =
2x1
1 + x12
(11)
in s1 x + c1 y = d3
gives x12 (d3 + y ) + (2x)x1 + (d3 y ) = 0
Solve quadratic in x1 and take tan1 to get
x x 2 + y 2 d32
1 = 2tan1
y + d3
(12)
NPTEL, 2010
26 / 96
d4 d42 + a32 K 2
3 = 2tan1
K + a3
(13)
2 = 2tan
a2 a3 c3 + d4 s3
z (a3 s3 + d4 c3 )
(14)
NPTEL, 2010
27 / 96
c4 c5 c6 s4 s6 c4 c5 s6 s4 c6 c4 s5
3
s 5 c6
s5 s6
c5
6 [R] =
s4 c5 c6 c4 s6 s4 c5 s6 c4 c6
s4 s5
(15)
Since
3
6 [R]
= 03 [R]T 06 [R]
(16)
NPTEL, 2010
28 / 96
NPTEL, 2010
29 / 96
PUMA 560:
S OLUTIONS
U NIQUENESS OF IK
NPTEL, 2010
30 / 96
= a2 s2 a3 s23 d4 c23
(17)
NPTEL, 2010
31 / 96
R2
=0
3
which gives K2 s3 + K3 c3 = 0.
Eliminating 3 and denoting a32 + d42 by l 2 , gives
[x 2 +y 2 +z 2 ((a2 +l )2 +d32 )][x 2 +y 2 +z 2 ((a2 l )2 +d32 )] = 0
(18)
which implies that the bounding surfaces are spheres.
At every point in the solid all possible orientations, except
two special singular configurations when r23 = 1, are
possible.
.
NPTEL, 2010
32 / 96
i1
degrees
0
-90
0
-90
90
-90
ai1
m
0
0
0.4318
0.019
0
0
i
degrees
45
60
135
30
-45
120
di
m
0
0
0.125
0.432
0
0
6 [T ] =
0.1502 0.5190
0.8415 0.0482
0
0
0
1
.
NPTEL, 2010
33 / 96
1
-91
-91
45
45
-91
-91
45
45
2
120
120
-77.73
-77.73
-102.27
-102.27
60
60
3
50.04
50.04
50.04
50.04
135
135
135
135
4
177.51
-2.49
85.25
-94.75
92.28
-87.72
30
210
5
-42.65
42.65
-159.22
159.22
-178.31
178.31
-45
45
6
105.34
-74.66
-132.87
47.13
15.79
-164.21
120
300
NPTEL, 2010
34 / 96
Z axis
0.2
0
0.2
0.4
0.6
0.8
0.8
0.6
0.4
0.2
0
0.2
0.4
0.6
0.8
Y axis
1
0.8
0.6
0.4
0.2
0.2
0.4
0.6
0.8
X axis
NPTEL, 2010
35 / 96
R EVIEW OF IK
Transcendental equations Polynomial equations using
tangent half angle substitution.
Polynomial equation of higher degree Linear in sin( ) or
cos( ) Quadratic in x 2 with x = tan( 2 ).
For analytical solutions to IK Eliminate joint variable(s)
from set of non-linear equations in several joint variables
A single equation in one joint variable.
Planar 3R example Three equations in three joint
variables Two equations in 1 and 2 One equation,
equation (5), in 2 alone.
Single equation solved for 2 and then solve for 1 and 3 .
PUMA 560 3 equations in first 3 joint variables.
Solve for the first 3 joint joint variables and then for last 3
joint variables using orientation information.
Decoupling of the position and orientation was first noticed
by Pieper (Pieper, 1968) for manipulators with intersecting
wrist. Later generalised to any six DOF serial
wrist-decoupled manipulators where three consecutive
.
.
.
.
.
joint axes intersect.
A SHITAVA G HOSAL (IIS C )
NPTEL, 2010
36 / 96
R EVIEW OF IK (C ONTD .)
For all six DOF serial manipulators, with three joint axes
intersecting At most a fourth-order polynomial in the
tangent of a joint angle need to be solved.
The manipulator wrist point can reach any position in the
workspace in at most four possible ways.
Fourth-degree polynomials can be solved in closed-form
(Korn and Korn, 1968) IK of all six- degree-of-freedom
serial manipulators with three intersecting axes can be
solved in closed-form.
PUMA 560 Workspace of the wrist point is bounded by
two spheres and require solution of only a quadratic due to
special geometry.
General geometry robot with intersecting wrist, boundaries
of the solid region traced by wrist point form a torus, a
fourth-degree surface (Tsai and Soni, 1984).
.
NPTEL, 2010
37 / 96
N ON - INTERSECTING WRISTS
Difficult to design/manufacture three intersecting axis
wrist.
Much easier if wrist has two intersecting axis.
2
D-H parameters
6
5
4
3
5
R Joint
R Joint
R Joint
i
1
2
3
4
5
6
i1
0
/2
0
/2
/2
/2
ai1
0
0
a2
a3
0
0
di
0
0
d3
d4
d5
0
i
1
2
3
4
5
6
NPTEL, 2010
38 / 96
N ON - INTERSECTING WRISTS
Figure 8 shows a six- degree-of-freedom robot First 3
joint axis are similar to PUMA 560.
Last three axes do not intersect and there is an offset d5 .
From D-H table compute 01 [T ], ..., 56 [T ] and then 06 [T ].
Last column of 06 [T ] is
x
(19)
NPTEL, 2010
39 / 96
N ON - INTERSECTING WRISTS
T0
6 [R],
c4 c5 c6 s4 s6
c4 c5 s6 s4 c6 c4 s5
s5 c6
s5 s6
c5
s4 c5 c6 c4 s6
s4 c5 s6 c4 c6
s4 s5
c1 c23
s1 c23
s23
r11 r12
c1 s23 s1 s23 c23 r21 r22
s1
c1
0
r31 r32
r13
r23 (20)
r33
Divide the (1, 3) and the (3, 3) terms of the above matrix
equation, to get (for 5 = 0, ),
s4 (r13 c1 c23 + r23 s1 c23 + r33 s23 ) = c4 (r13 s1 r23 c1 )
(21)
NPTEL, 2010
40 / 96
N ON - INTERSECTING WRISTS
Numerical values of D-H parameters same as PUMA 560.
Offset d5 is chosen to be 20 mm.
0 [T ] same as used for the PUMA 560 example
6
6 [T ] =
0.1502 0.5190
0.8415 0.0482
0
0
0
1
Solve 4 non-linear equations numerically Function fsolve
R
used here.
in Matlab
NPTEL, 2010
41 / 96
S UMMARY
Inverse kinematics: Given end-effector position and
orientation and constant D-H parameters, obtain joint
variables.
Number of joint variables must be 3 (for planar motion)
and 6 (for 3D motion).
Involve solution of a set of non-linear (transcendental)
equations No general approach IK of serial manipulators.
Existence of solution Workspace of serial manipulator.
Order of polynomial Number of configurations possible
for given end-effector position and orientation.
Planar 3R manipulator IK can be solved easily,
reachable and dexterous workspace.
PUMA 560 has 8 configurations, general 6R has 16
possible configurations.
.
NPTEL, 2010
42 / 96
O UTLINE
.
. .1 C ONTENTS
.
. .2 L ECTURE 1
Introduction
Direct Kinematics of Serial Robots
.
. .3 L ECTURE 2
Inverse Kinematics of Serial Robots
.
. .4 L ECTURE 3
Inverse Kinematics of Serial Robots with n < 6
Inverse Kinematics of Serial Robots with n > 6
.
. .5 L ECTURE 4
Elimination Theory & Solution of Non-linear Equations
Inverse Kinematics of a General 6R Robot
.
. .6 A DDITIONAL M ATERIAL
Problems, References and Suggested Reading
.
NPTEL, 2010
43 / 96
I NTRODUCTION
0 [T ]
n
{0}.
0 [T ],
n
NPTEL, 2010
44 / 96
SCARA ROBOT
0 [T ]
4
Z 2
Z 1
2
{ 2}
{ 1}
X 2
X 1
d3
{ 3}
O 0, O1
O2
Z 3
{ 4}
O3, O4
a1 c1 + a2 c12
a1 s1 + a2 s12
d3
1 + 2 + 4
=
=
=
=
.
(22)
NPTEL, 2010
45 / 96
2
1
d3
4
=
=
=
=
x 2 +y 2 l 2 l 2
1
2
)
cos1 (
2l1 l2
atan2(y , x) atan2(l2 s2 , l1 + l2 c2 )
z
1 2
(23)
NPTEL, 2010
46 / 96
O UTLINE
.
. .1 C ONTENTS
.
. .2 L ECTURE 1
Introduction
Direct Kinematics of Serial Robots
.
. .3 L ECTURE 2
Inverse Kinematics of Serial Robots
.
. .4 L ECTURE 3
Inverse Kinematics of Serial Robots with n < 6
Inverse Kinematics of Serial Robots with n > 6
.
. .5 L ECTURE 4
Elimination Theory & Solution of Non-linear Equations
Inverse Kinematics of a General 6R Robot
.
. .6 A DDITIONAL M ATERIAL
Problems, References and Suggested Reading
.
NPTEL, 2010
47 / 96
R EDUNDANT M ANIPULATORS
If n > 6 for 3D motion or n > 3 for planar motion More
unknowns than equations and hence infinite number of
solutions Redundant manipulators.
A simple example of a planar 3R robot but not interested
in orientation of the last link.
Direct kinematics equations are
x
y
= l1 c1 + l2 c12 + l3 c123
= l1 s1 + l2 s12 + l3 s123
(24)
NPTEL, 2010
48 / 96
R EDUNDANT M ANIPULATORS
Need to use additional equation so that unique
i , i = 1, 2, 3 can be obtained.
Optimisation of a function of joint variables (Nakamura,
1991)
Minimisation of joint rotations, velocities and acceleration.
Avoiding obstacles and singularities.
Minimisation of actuator torques.
...
...
1
NPTEL, 2010
49 / 96
(25)
f
g1
g2
= 1
+ 2
g1 ( ) = g2 ( ) = 0
.
(26)
.
NPTEL, 2010
50 / 96
1
2
1
1
1
1
f g1 g2
(27)
2 2 2 1 = 0
g1
g2
f
2
3
(28)
NPTEL, 2010
51 / 96
1
1.5
2
2.5
6
5
The trajectory
choosen
25
20
15
10
10
15
20
25
NPTEL, 2010
52 / 96
1
1.5
2
2.5
6
25
20
15
10
10
15
20
25
Figure 11: Plot of joint variables for redundant planar 3R robot with joint limit
.
NPTEL, 2010
53 / 96
10
O
j0
Y-axis
8
dy
6
dr
O
dx
tractrix
4
2
j1
0
S
0
P
5
P'
dp
X-axis 10
15
NPTEL, 2010
54 / 96
T RACTRIX EQUATION
Since the velocity vector at j0 is always aligned with the
link, the tractrix equation is
dy
y
=
dx
L2 y 2
(29)
x = L log
L2 y 2
L L2 y 2
p
p
x(p) = p L tanh( ), y (p) = L sech( )
L
L
(30)
NPTEL, 2010
55 / 96
10
8
6
4
2
X'
1
m1 < 0
Modified differential
equation of tractrix
dy
y ye
dx = xxe
(x ,y)
points
on
tractrix
Y'
Y - axis
12
m1 > 0
Solution is
B 2 4AC
xe = B 2A
where
2
A = 1+m ,B =
2my +2x, C = x 2 +y 2 L2 .
ye = m x e
2
0
2
X - axis
0
10
15
NPTEL, 2010
56 / 96
.
.
.
.
.
.
.
.
NPTEL, 2010
57 / 96
R ESOLUTION OF
USING T RACTRIX
R EDUNDANCY
NPTEL, 2010
58 / 96
.
.
...
3
NPTEL, 2010
59 / 96
P ROPERTIES
OF
A LGORITHM
RESOLUTION-TRACTRIX
...
1
...
2
...
...
3
ji1 ji (k) is the unit vector from the tail to the head of the
i-th link at k-th instant.
Resolution of redundancy in Cartesian space and then the
joint angles are computed.
Head of link ln moves by drn , displacements obey the
inequality dr0 dr1 ... drn1 drn .
The motion of the links appears to die out as we move
toward the first link.
Joints near to base see large inertia and a desirable
strategy would be to move them the least.
...
5
NPTEL, 2010
60 / 96
E XPERIMENTAL H ARDWARE
Experimental 8-link planar manipulator Each link is 70
mm long.
Joint driven by Futaba S3003 RC hobby servos.
NPTEL, 2010
61 / 96
S IMULATION R ESULTS
Path traced by endeffector
500
500
Endeffector trajectory
Tractrix solution
Pseudoinverse solution
Modal solution
400
Endeffector trajectory
Tractrix solution
Pseudoinverse solution
Modal solution
400
300
300
2
200
100
200
100
6
100
200
300
200
100
200
300
400
100
500
300
100
200
300
400
500
NPTEL, 2010
62 / 96
S IMULATION R ESULTS
Joint angles for tractrix solution
100
100
80
80
60
40
Joint 1
Joint 2
Joint 3
Joint 4
Joint 65
20
0
20
40
60
Joint 7
20
Joint 4
Joint 5
Joint 3
Joint 2
Joint 1
40
20
Joint 6
Joint 8
40
60
80
100
0
60
Joint 7
Joint 8
200
400
600
800
Point in trajectory
1000
1200
80
0
200
400
600
Point in trajectory
800
1000
(c) Plot of joint variables for straight (d) Plot of joint variables for circular
line motions
motion
NPTEL, 2010
63 / 96
E XPERIMENTAL R ESULTS
NPTEL, 2010
64 / 96
2 See
NPTEL, 2010
65 / 96
NPTEL, 2010
66 / 96
S UMMARY
Constrained motion: number of actuated joints less than 6
(in 3D) and less than 3 (in plane).
Redundant serial manipulators Number of actuated joints
greater than 6 (in 3D) and greater than 3 (in a plane)
Inverse kinematics solutions are infinitely many.
Require additional constraints Resolution of redundancy.
Optimisation (minimisation) of joint motion.
Optimsation (minimisation) of Cartesian motion.
Tractrix based approach gives more natural motion.
Simulation and experimental verification of properties of
tractrix based resolution scheme.
NPTEL, 2010
67 / 96
O UTLINE
.
. .1 C ONTENTS
.
. .2 L ECTURE 1
Introduction
Direct Kinematics of Serial Robots
.
. .3 L ECTURE 2
Inverse Kinematics of Serial Robots
.
. .4 L ECTURE 3
Inverse Kinematics of Serial Robots with n < 6
Inverse Kinematics of Serial Robots with n > 6
.
. .5 L ECTURE 4
Elimination Theory & Solution of Non-linear Equations
Inverse Kinematics of a General 6R Robot
.
. .6 A DDITIONAL M ATERIAL
Problems, References and Suggested Reading
.
NPTEL, 2010
68 / 96
NPTEL, 2010
69 / 96
E LIMINATION T HEORY
Polynomial equations f (x, y ) = 0 and g (x, y ) = 0 of degree
m and n
Degree of a polynomial is sum of exponents of the highest
degree term.
Example: In polynomial,
f (x, y ) = xy 2 + x 2 y + x 2 + y 2 + 1 = 0, the degree is 3 since
sum of exponent of x and y in first (and second) term is 3.
NPTEL, 2010
70 / 96
NPTEL, 2010
71 / 96
NPTEL, 2010
72 / 96
S YLVESTER S M ETHOD
i
Two polynomials P(x) = m
i=0 ai x = 0 and
n
i
Q(x) = i=0 bi x = 0, ai and bi are co-efficients.
Construct the Sylvesters matrix of P(x) and Q(x)
am am1
...
a1 a0
am am1 ... a1 a0
am ... ... a0
[SM] =
b
b
...
b
b
n
n1
1
0
bn
bn1 ... b1 b0
bn
...
(32)
... b0
NPTEL, 2010
73 / 96
(33)
NPTEL, 2010
74 / 96
n
i
i
P(x) = m
i=0 ai (y )x = 0 and Q(x) = i=0 bi (y )x = 0.
Note: all coefficients are function of y or constant.
...
...
...
...
NPTEL, 2010
75 / 96
(34)
a2
0
[SM] =
b2
0
a1
a2
b1
b2
a0 0
a1 a0
b0 0
b1 b0
(35)
det[SM](y ) = 0 reduces to
(a2 b1 b2 a1 )(a1 b0 b1 a0 ) (a2 b0 b2 a0 )2 = 0.
.
NPTEL, 2010
76 / 96
a2
0
b2
0
as
x = x1 =
3
a0 0
x
x2
a1 a0
b0 0 x 1
b1 b0
x0
=0
(36)
a1 b0 b1 a0 a2 b0 b2 a0
=
a2 b0 b2 a0 a1 b2 a2 b1
(37)
a2 b0 b2 a0
a1 b2 a2 b1
is same as det[SM](y ) = 0!
.
NPTEL, 2010
77 / 96
B ZOUTS M ATRIX
det[SM] is also called the resultant of P(x) and Q(x) and
is denoted by res(P, Q).
[SM] is (m + n) (m + n) and res(P, Q) can become
computationally expensive.
Bzout in the 18th century proposed a method where
res(P, Q) can be computed as a determinant of order
max(m, n).
The key idea is to divide instead of multiplying to get
required number of independent equations and a square
matrix.
Although dimension of matrix is less, each element of the
matrix is more complex.
NPTEL, 2010
78 / 96
am
am1 x m1 + .... + a0
=
bn
bn1 x m1 + ... + b0 x mn
to get
to get
(am2 bn bn2 am )x m1 +
[(am3 bn bn3 am ) + (am2 bn1 bn2 am1 )]x m2
+... + a0 bn1 = 0
(39)
.
NPTEL, 2010
79 / 96
+.... + a0 b1 = 0
Construct m n equations
x mn1 Q(x) = bn x m1 + bn1 x m2 + ... + b0 x mn1
x
mn2
mn2
.....
0(41)
Q(x) = bn x + ... + b0
Q(x) = bn x
m2
+ ... + b0 x
n
NPTEL, 2010
80 / 96
am1 bn am bn1
...
amn bn am b0
bn
am2 bn am bn2
...
amn1 bn + amn bn1 am1 b0
bn1
bn
...
..
..
..
b0
..
..
..
a 0 bn
..
a0 b1
..
..
..
b0
..
...
..
b0
(42)
NPTEL, 2010
81 / 96
(43)
b3 a2 a3 b2
[BM] = b3 a1 a3 b1
b3 a0 a0 b3
b3 a 0 a 3 b0
b2 a0 a2 b0
b1 a0 a1 b0
(44)
b3 a 1 a 3 b1
(b3 a0 a3 b0 ) + (b2 a1 a2 b1 )
b2 a 0 a 2 b0
NPTEL, 2010
82 / 96
a3 a2 a1 a0 0
0 a3 a2 a1 a0
(45)
[SM] =
b2 b1 b0 0 0
0 b2 b1 b0 0
0 0 b2 b1 b0
The Bzout matrix is given as
a1 b2 a3 b0 a0 b2 + a1 b1 a2 b0 a0 b1
a1 b2 a3 b0
a0 b2 (46)
[BM] = a2 b2 a3 b1
b2
b1
b0
.
NPTEL, 2010
83 / 96
1 0
0
0
0 1
0
0
b
b
a
a
[A] =
2
1
3
2
0 b2
0
a3
0 0
0
0
to get
[B] =
a3
0
0
0
0
a2
a3
0
0
0
a1
a2
a1 b2 a3 b0
a2 b2 a3 b1
b2
0
0
0
0
1
a0
a1
a0 b2 + a1 b1 a2 b0
a 1 b2 a 3 b0
b1
(47)
0
a0
a0 b1
a0 b2
b0
(48)
NPTEL, 2010
84 / 96
O UTLINE
.
. .1 C ONTENTS
.
. .2 L ECTURE 1
Introduction
Direct Kinematics of Serial Robots
.
. .3 L ECTURE 2
Inverse Kinematics of Serial Robots
.
. .4 L ECTURE 3
Inverse Kinematics of Serial Robots with n < 6
Inverse Kinematics of Serial Robots with n > 6
.
. .5 L ECTURE 4
Elimination Theory & Solution of Non-linear Equations
Inverse Kinematics of a General 6R Robot
.
. .6 A DDITIONAL M ATERIAL
Problems, References and Suggested Reading
.
NPTEL, 2010
85 / 96
IK OF G ENERAL 6R ROBOT
General 6R robot: no constant D-H link parameters have
special values, such as 0, /2, or .
Special D-H values (PUMA 560) result in easier
elimination.
If prismatic joint is present Elimination is easier.
Inverse kinematics of general 6R unsolved for a long time.
Several researchers worked on problem Duffy and Crane
(1980) first derived a 32nd order polynomial in one joint
angle.
Eventually Raghavan and Roth (1993) derived a 16th
degree polynomial in one joint angle.
(49)
.
NPTEL, 2010
86 / 96
IK OF G ENERAL 6R ROBOT
Recall with respect to equation (49)
i 1
[T ]
i
Step 1: write i1
i [T ] as product of two matrices
(ii1 [T ])st (i1
[T
])jt .
i
i1
[T ] = (ii 1 [T ])st (i1
[T ])jt
i
i
1
0
0
0
0
ci 1
si 1
0
0
si 1
ci 1
0
ai 1
ci
s
0
i
0 0
1
0
si
ci
0
0
0
0
1
0
0
0
(50)
di
1
Matrix (i1
i [T ])st is constant.
Matrix (i1
i [T ])jt is a function of the joint variable i (for a
rotary joint) or di (for a prismatic joint).
.
NPTEL, 2010
87 / 96
IK OF G ENERAL 6R ROBOT
Step 2: Reorganize equation of direct kinematics
Rewrite equation (49) as
(23 [T ])jt 34 [T ]45 [T ](56 [T ])st =
1
1 0
1 0
5
1
(23 [T ])1
(51)
st (2 [T ]) (1 [T ])
6 [T ](6 [T ])jt
(52)
NPTEL, 2010
88 / 96
IK OF G ENERAL 6R ROBOT
Step 3: Eliminate four of five i , i = 1, .., 5 in equation (52).
Key step is to obtain minimal set of equations.
Minimal set of equations is 14 (Raghavan & Roth, 1993)
Three equations from p,
Three equations from l,
One scalar equation from the scalar dot product p p,
One scalar equation from the scalar dot product p l,
Three equations from the vector cross product p l, and
Three scalar equations from (p p)l (2p l)p.
(53)
NPTEL, 2010
89 / 96
IK OF G ENERAL 6R ROBOT
Step 3: Elimination of four i (Contd.)
First use any eight of the 14 equations in equation (53) and
solve for the eight variables s1 s2 , s1 c2 , c1 s2 , c1 c2 , s1 , c1 , s2 , c2 .
Always possible to solve eight linear equations in eight
unknowns.
Substitute the eight variables in the rest of the six
equations to get
[R] (s4 s5 s4 c5 c4 s5 c4 c5 s4 c4 s5 c5 1)T = 0
(54)
NPTEL, 2010
90 / 96
IK OF G ENERAL 6R ROBOT
Step 4: Elimination of 4 and 5
Use tangent half-angle formulas for s3 , c3 , s4 , c4 , s5 , and
c5 .
On simplifying get
(
)T
[S] x42 x52 x42 x5 x42 x4 x52 x4 x5 x4 x52 x5 1 = 0
(55)
NPTEL, 2010
91 / 96
IK OF G ENERAL 6R ROBOT
Step 4: Elimination of 4 and 5 (Contd.)
The 12 equations can be
written as
x43 x52
x 3x
4 5
x3
4
x 2x 2
4 5
x 2x
5
)
(
4
S 0 x42
=0
2
0 S x4 x5
x4 x5
x4
2
x5
x5
1
(56)
A SHITAVA G HOSAL (IIS C )
Following Sylvesters
method,
set )
(
S 0
=0
det
0 S
On simplification, a
16th-degree polynomial in
x3 is obtained.
Solve for roots of this
polynomial and find
3 = 2 tan1 (x3 ).
NPTEL, 2010
92 / 96
IK OF G ENERAL 6R ROBOT
Step 5: Obtain other joint angles
Once 3 is known, find x4 and x5 from equation (56) using
standard linear algebra.
From x4 and x5 find 4 and 5 .
Once 3 , 4 , and 5 are known, solve s1 s2 , s1 c2 , ..., s2 , c2
from eight linearly independent equations (53).
Obtain unique 1 and 2 .
To obtain 6 , rewrite equation (49) as
5
6 [T ]
= 45 [T ]
1 3
1 2
1 1
1 0
1 0
4 [T ] 3 [T ] 2 [T ] 1 [T ] 6 [T ]
(57)
NPTEL, 2010
93 / 96
IK OF 6R ROBOT: S UMMARY
A sixteenth degree polynomial in x3 is obtained in Step 4
General 6R serial manipulator has 16 possible solutions.
A 6R manipulator with special geometry Polynomial in
x3 can be of lower order.
If one or more joints are prismatic IK is simpler since
prismatic joint variable is not in terms of sines or cosines.
No general expression for workspace boundary
Closed-form solution for 16th-degree polynomial not
possible!
Check: If all the roots of the 16th-degree polynomial are
complex, then 06 [T ] is not in the workspace of the
manipulator.
All the inverse kinematics solutions & entire workspace may
not be available due to the presence of joint limits and
limitations of hardware (see, Rastegar and Deravi, 1987 &
Dwarakanath et al. 1992).
.
NPTEL, 2010
94 / 96
O UTLINE
.
. .1 C ONTENTS
.
. .2 L ECTURE 1
Introduction
Direct Kinematics of Serial Robots
.
. .3 L ECTURE 2
Inverse Kinematics of Serial Robots
.
. .4 L ECTURE 3
Inverse Kinematics of Serial Robots with n < 6
Inverse Kinematics of Serial Robots with n > 6
.
. .5 L ECTURE 4
Elimination Theory & Solution of Non-linear Equations
Inverse Kinematics of a General 6R Robot
.
. .6 A DDITIONAL M ATERIAL
Problems, References and Suggested Reading
.
NPTEL, 2010
95 / 96
A DDITIONAL M ATERIAL
Exercise Problems
References & Suggested Reading
NPTEL, 2010
96 / 96