Industrial Robot - Nguyen, Quoc Chi - C4

Mechatronics Dept.
, Dynamics & Control Group, 207701-Industrial Robot
Manipulator kinematics
Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-1

Mechatronics Dept., Dynamics & Control Group, 207701-Industrial Robot
Task space Joint space

(x, y, z, a, b, g) (q1, q2, ..., q6)
- Forward kinematics
- Inverse kinematics

Forward kinematics
Forward kinematics is the static geometrical problem of
computing the position and orientation of the end-effector of
the manipulator. Specifically, given a set of joint angles, the
forward kinematic problem is to compute the position and
orientation of the tool frame relative to the base frame.
q1, q2, ..., q6 Forward
kinematics
x, y, z, a, b, g
q P, R
q1 x
y
q2
qn z
time time
Inverse kinematics
Given the position and orientation of the end-effector of the
manipulator, calculate all possible sets of joint angles that could
be used to attain this given position and orientation. This is a
fundamental problem in the practical use of manipulators.
x, y, z, a, b, g Inverse
kinematics
q1, q2, ..., q6
P, R q
x q1
y
q2
z qn
time time
Denavit - Hartenberg notation

A manipulator may be thought of as a set of bodies connected in
a chain by joints.
3
3
2
1 This is an expression that depends on N joint variables (N = 6

0
in most cases)

Denavit - Hartenberg notation
describe the link itself
describe the link's connection to a

neighboring link
If the joint is:

• revolute: q joint variable the other three are fixed
• prismatic: d joint variable link parameters

Description of D-H parameters
For a link (i), there are some convention:
• Joint axis (i): on the side of the fixed link.
• Joint axis (i + 1): on the side of the end-effector.
• Frame {i} is put on the link {i} so that axis Zi is coincident
with the joint axis (i + 1).
Link i
Zi
Axis i Axis i + 1
Link i
Axis i Axis i + 1
• Link length ai: the

length of the common
normal of axis (i) and
ai
axis (i + 1).
twist ai: angle
• Link
between the two axes ai

• Link length a = 7 in.

• Link twist a = 450

Axis i Axis i + 1
Link i - 1 Link i
• Linkoffset di:
distance between the
two common normal
lines ai-1
ai
di
qi
ai
• Joint angle qi: angle between the

two common normal lines

3
3
2 If there are n links, then
2
Link Link Link Link Joint

length twist offset angle
1
1 a1 a1 d1 q1
1 2 a2 a2 d2 q2
0
n–1 an-1 an-1 dn-1 qn-1
n an an dn qn
( means the parameter is not determined yet)
Convention for affixing frames to links
Zi
Link i
Axis i Axis i + 1
Xi
Yi

Convention for affixing frames to links

Link parameters in terms of the link frames
Zi
Zi-1
ai
qi
ai
ai-1 di
Xi
Xi-1
Link i - 1 Link i
Axis i Axis i + 1

Zi
Zi-1
ai
qi
ai
ai-1 di
Xi
Xi-1
Link i - 1 Link i
Axis i Axis i + 1

Zi
Zi-1
ai
qi
ai
ai-1 di
Xi
Xi-1
Link i - 1 Link i
Axis i Axis i + 1

Zi
Zi-1
ai
qi
ai
ai-1 di
Xi
Xi-1
Link i - 1 Link i
Axis i Axis i + 1

Example of link-frame attachment procedure
Assign link frames to an RRR mechanism and give D-H
parameters.

Note that three joint axes are parallel.
D-H parameters
i ai ai di qi
1 0 L1 0 q1
2 0 L2 0 q2
3 0 L3 0 q3

Assign link frames and give D-H parameters to an RPR

mechanism.

Y1 X1 X2 X3
Z1
Z0 Z2 Z3
X0
Y0
i ai ai di qi
1 900 0 L1 q1
D-H parameters
2 0 0 d2 0
3 0 0 L3 q3

Determine D-H parameters to a three link, non-planar

manipulator.

Z0 Z0
Z1 Z1
X1 X1

Z0 X1
Z1

Summary of link-frame attachment procedure
q1
Z1 q2
O1
X1 Z2 Z6 q3
Y1 O2
Y3 Z4
O3 XZ 5 q Y6
Y2 2 6
q5
Z0 O6
X 3 Y4 Y5
X5 X6
O5 O4 Z 3
X4
q4

Link transformation
{R} & {Q}: RZ(qi) {P} & {i}: RX(ai)
ZR Zi
Zi-1
ZQ ai
XR
qi ZP
ai
ai-1 di
XQ XP Xi
Xi-1
Link i - 1 Link i
Axis i + 1
Axis i
{i - 1} & {R}: DZ(di) {Q} & {P}: DX(ai)
Link transformation
To describe frame {i} relative to the frame {i-1}, three
intermediate frames {P}, {Q}, and {R} are used so that their
transformation is a function of one link parameter only.
{i - 1} and {R}: di {R} and {Q}: q

{Q} and {P}: ai {P} and {i}: ai
The transformation that transforms vectors defined in {i} to their

description in {i-1} can be written as:

Link transformation
From the figure:

Link transformation

Steps to formulate forward kinematics
The problem of forward kinematics is approached as follows:
• Step 1: Assign frame for each i ai ai di qi

link
1 a0 a0 d1 q1
• Step 2: Determine D-H
2 a1 a1 d2 q2
parameters for each link and
put them in the table on the |
right. 6 a5 a5 d6 q6
(Note d and q, which one is variable for each link)

Step 3: Using D-H parameters above to compute the individual

transformations for each link.
It a good practice to check them against common sense (for

example, the elements of the fourth column of each transform
should give the coordinates of the origin of the next higher
frame).

Step 4: the link transformations are then multiplied together to

find the single transformation that relates frame {N} to frame
{0}:

Example: a 3-DOF planar robot, RRR
D-H parameters
4-33 i ai ai di qi
1 0 l1 0 q1
2 0 l2 0 q2
3 0 l3 0 q3


In case: q1 = q2 = q3 = 00; calculate 0P?

In case: q1 = 1800, q2 = q3 = 900; calculate 0P?

In case: q1 = 900, q2 = -900, q3 = -450; calculate 0P?

Example: 6-DOF Puma robot


D-H parameters
i ai ai di qi i ai ai di qi
1 -900 0 0 q1 4 900 0 d4 q4
2 00 a2 d2 q2 5 -900 0 0 q5
3 -900 a3 0 q3 6 00 0 0 q6


Where

and

Note: notation of D-H parameters used in [2]
P q3
In [2], axis Zi is coincident with the joint axis i.
L3
i ai-1 ai-1 di qi L2
1 0 0 0 q1
q2
2 0 L1 0 q2 L1
3 0 L2 0 q3
q1
When:

Inverse kinematics
Place the tool frame:

Inverse Kinematics – the general approach

The general problem of inverse kinematics can be stated as follows.
R O
H 
 0 1 
find (one or all) solutions of the equation
0
n T q1 ,...,q n   H
where
0
n T  01T 12T... n1nT
Here, H represents the desired position and orientation of the end-effector, and
our task is to find the values for the joint variables q1,…, qn

Solvability
This is a nonlinear equation system:

- 6 variables:
• 3 equations from the position-vector portion

• 3 equations are independent (among the 9 equations
arising from the rotation-matrix portion)
- These equations are nonlinear, transcendental equations
Inverse kinematics is quite difficult to solve

Existence of solutions
Existing of any solution raises the question of the manipulator's

workspace, which is the volume of space that the end-effector of
the manipulator can reach.
 Dexterous workspace:
 Reachable workspace:

Multiple solutions
Another possible problem encountered in solving kinematic equations

is that of multiple solutions.
It often comes up with an optimization problem to choose a

solution.

Multiple solutions
The PUMA 560 can

reach certain goals
with eight different
solutions.

Inverse Kinematics – Closed form solution

In solving the inverse kinematics problem we are most interested in finding
a closed form solution of the equations rather than a numerical solution.
Finding a closed form solution means finding an explicit relationship:
q k  f (h11 ,..., h34 )
Closed form solutions are preferable for two reasons.

- First, in certain applications, such as tracking a welding seam whose
location is provided by a vision system, the inverse kinematic equations
must be solved at a rapid rate, say every 20 milliseconds, and having
closed form expressions rather than an iterative search is a practical
necessity.
- Second, the kinematic equations in general have multiple solutions.
Having closed form solutions allows one to develop rules for choosing a
particular solution among several.

Kinematic Decoupling
- For manipulators having six joints, with the last three joints
intersecting at a point (such as the Stanford Manipulator above), it is
possible to decouple the inverse kinematics problem into two simpler
problems, known respectively, as inverse position kinematics, and
inverse orientation kinematics.
- For a six-DOF manipulator with a spherical wrist, the inverse
kinematics problem may be separated into two simpler problems,
namely first finding the position of the intersection of the wrist axes,
hereafter called the wrist center, and then finding the orientation of
the wrist.
- We can establish two sets of equations representing the rotational
and positional equations
0
6 R (q1 ,...,q6 )  R
0
6 O(q1 ,...,q6 )  Oc

Method of solution
 Numerical solution Robots for which an analytic (or

 Closed-form solution closed-form) solution exists are
characterized either by
• having several intersecting joint
axes (ai = 0)
• having joint axes parallel or
perpendicular (ai = 00 / 900)
A sufficient condition that a
manipulator with six revolute joints
have a closed-form solution is that
three neighboring joint axes
intersect at a point.

Algorithm to solve
For this class of manipulators the determination of the inverse
kinematics can be summarized by the following algorithm.
Step 1: Find q1, q2, q3 such that the wrist center O has coordinates
given by

Algorithm to solve
For this class of manipulators the determination of the inverse
kinematics can be summarized by the following algorithm.
3
6 R  ( 03 R)1 R

Algebraic vs. Geometric
Algebraic solution
4-56
i ai ai di qi
1 0 l1 0 q1
2 0 l2 0 q2
3 0 l3 0 q3

Algebraic solution
4-57
Because this is a planar manipulator, transformation from {3} to

{0} can be expressed in form:

Algebraic solution
Compute q2

Algebraic solution

Algebraic solution
Compute q1
We have
Rearrange in q1 results in:
Where

Algebraic solution
If: then:
Compute q3

Algebraic vs. Geometric
Geometric solution
- Decompose the spatial geometry of the arm into several plane-
geometry problems
- Done quite easily particularly when the ai = 00 / 900
- Apply the "law of cosines“ in plane geometry

Geometric solution
4-63
For AC of ABC:

Geometric solution
Similarly for BC:

Three consecutive axes intersect
Pieper studied manipulators with six degrees of freedom in

which three consecutive axes intersect at a point and showed
that these special cases can be solved.
Consider the case of all six joints revolute, with the last three
axes intersecting, when the origins of link frames (4), {5}, and {6}
are all located at this point of intersection. This point is given in
base coordinates {0} as

We have

Key concept:
- Using substitution to result in an equation of one variable, it
generally appears as sinqi and cosqi.
- Making the following substitutions yields an expression in
terms of a single variable, u:
The first one-variable equation is a 4th polynomial equation of u3.

Having solved for u3, we can solve for u2 and then u1.

Industrial Robot - Nguyen, Quoc Chi - C4

Uploaded by

Document Information

Original Title

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

Industrial Robot - Nguyen, Quoc Chi - C4

Uploaded by

Copyright:

Available Formats

Mechatronics Dept.

, Dynamics & Control Group, 207701-Industrial Robot

Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-1

Task space Joint space

Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-2

Denavit - Hartenberg notation

1 This is an expression that depends on N joint variables (N = 6

Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-5

describe the link itself

describe the link's connection to a

If the joint is:

Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-6

• Link length ai: the

Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-8

• Link length a = 7 in.

Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-9

• Joint angle qi: angle between the

Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-10

Link Link Link Link Joint

Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-12

Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-13

Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-14

Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-15

Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-16

Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-17

Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-18

Note that three joint axes are parallel.

Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-19

Assign link frames and give D-H parameters to an RPR

Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-20

Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-21

Determine D-H parameters to a three link, non-planar

Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-22

Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-23

Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-24

Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-25

{i - 1} and {R}: di {R} and {Q}: q

The transformation that transforms vectors defined in {i} to their

Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-27

Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-28

Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-29

The problem of forward kinematics is approached as follows:

• Step 1: Assign frame for each i ai ai di qi

(Note d and q, which one is variable for each link)

Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-30

Step 3: Using D-H parameters above to compute the individual

It a good practice to check them against common sense (for

Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-31

Step 4: the link transformations are then multiplied together to

Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-32

Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-33

Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-34

In case: q1 = q2 = q3 = 00; calculate 0P?

Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-35

In case: q1 = 1800, q2 = q3 = 900; calculate 0P?

Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-36

In case: q1 = 900, q2 = -900, q3 = -450; calculate 0P?

Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-37

Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-38

Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-39

Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-40

Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-41

Prepared by Q. C. Nguyen (PhD) & C.B. Pham (PhD) 4-42