Vous êtes sur la page 1sur 61

# Ch 4: Manipulator kinematics

(x, y, z, a, b, g) (q1, q2, ..., q6)

 Forward kinematics
 Inverse kinematics

##  C.B. Pham 4-1

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.

kinematics
x, y, z, a, b, g

q P, R
q1 x
y
q2

qn z

##  C.B. Pham time time 4-2

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
 C.B. Pham time time 4-3
Denavit - Hartenberg notation

## A manipulator may be thought of as a set of bodies

connected in a chain by joints.

3
3
2

2
Frame {i} is described relative to frame
{i-1} by 𝑖−1𝑖𝐓
1

0
This is an expression that depends on N joint
variables (N = 6 in most cases)
 C.B. Pham 4-4
Denavit - Hartenberg notation

## The Denavit – Hartenberg notation is introduced as a

systematic method of describing the kinematic relationship
𝑖−1
𝑖𝐓 using only four parameters:
• q (joint angle) neighboring link

## If the joint is:

• revolute: q joint variable the other three are
• prismatic: d joint variable fixed link parameters
 C.B. Pham 4-5
4.1. 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).
Zi

##  C.B. Pham Axis i Axis i + 1 4-6

4.1. Description of D-H parameters
Axis i Axis i + 1

## • Link length ai: the ai

length of the common
normal of axis (i) and
axis (i + 1). ai
between the two axes
 C.B. Pham 4-7
4.1. Description of D-H parameters

## • Link length a = 7 in.

• Link twist a = 450

##  C.B. Pham 4-8

4.1. Description of D-H parameters
Axis i Axis i + 1

distance between
the two common
a
normal lines i-1 ai
di
qi
ai
• Joint angle qi: angle between
the two common normal lines
 C.B. Pham 4-9
4.1. Description of D-H parameters

3
3
2 If there are n links, then
2

length twist offset angle
1
1 a1 a1  
1 2 a2 a2 d2 q2
0
⋮ ⋮ ⋮ ⋮ ⋮
n–1 an-1 an-1 dn-1 qn-1
n    

##  C.B. Pham ( means the parameter is not determined yet) 4-10

4.2. Convention for affixing frames to links

Zi
chain Axis i Axis i + 1
- The 𝐙 -axis of frame {i},
called 𝐙i , is coincident with
the joint axis (i + 1).
- 𝐗i points along ai in the
direction from joint axis (i) Xi
to joint axis (i + 1).
- 𝐘i is formed by the right-
hand rule to complete the Yi
ith frame, i.e.
 C.B. Pham 4-11
4.2. Convention for affixing frames to links

## • First link in the chain ( frame {0})

Frame {0} is arbitrary, so it always
simplifies matters to choose 𝐙0 along
axis 1.

## • Last link in the chain ( frame {N})

Choose an origin location of frame
{N}. 𝐙N and 𝐗N are assigned freely,
but generally so as to cause as many
become zero.

##  C.B. Pham 4-12

Zi
Zi-1
ai
qi
ai
ai-1 di
Xi
Xi-1

Axis i Axis i + 1
• Link length ai: distance from 𝐙i-1 to 𝐙i measured along 𝑿i
 C.B. Pham 4-13
Zi
Zi-1
ai
qi
ai
ai-1 di
Xi
Xi-1

Axis i Axis i + 1
• Link twist ai: the angle from 𝐙i-1 to 𝐙i measured about 𝑿i
 C.B. Pham 4-14
Zi
Zi-1
ai
qi
ai
ai-1 di
Xi
Xi-1

Axis i Axis i + 1
• Link offset di: distance from 𝐗i-1 to 𝐗i measured along 𝒁i-1
 C.B. Pham 4-15
Zi
Zi-1
ai
qi
ai
ai-1 di
Xi
Xi-1

Axis i Axis i + 1
• Joint angle qi: the angle from 𝐗i-1 to 𝐗i measured about 𝒁i-1
 C.B. Pham 4-16

q1
Z1 q2
O1
X1 Z2 Z6 q3
Y1 O2
Y3 Z Z4
O3 X 2 5 q 6 Y6
Y2
O6
q5
Z0
X 3 Y4 Y5
X5 X6
O O4 Z 35
X4
q4
 C.B. Pham 4-17

parameters.

D-H parameters

i ai ai di qi

1 0 L1 0 q1

2 0 L2 0 q2

3 0 L3 0 q3

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
 C.B. Pham 4-21

manipulator.

## Two possible frame assignments and corresponding

parameters for the two possible choices of direction of 𝐙1.

Z0 Z0

Z1 Z1

X1 X1

## There are 2 more choices of 𝐗1 when joint axis 1 and joint

axis 2 intersect.

Z0 X1
Z1

## 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}: qi

{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:

##  C.B. Pham 4-25

{R} & {Q}: RZ(qi) ZR Zi {P} & {i}: RX(ai)
Zi-1

ZQ ai
XR
qi ZP
ai
ai-1 di
XQ XP Xi
Xi-1

Axis i + 1
Axis i
{i - 1} & {R}: DZ(di) {Q} & {P}: DX(ai)
 C.B. Pham 4-26

##  C.B. Pham 4-28

4.4. Steps to formulate forward kinematics

follows:

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

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

##  C.B. Pham 4-29

4.4. Steps to formulate forward kinematics

## 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).
 C.B. Pham 4-30
4.4. Steps to formulate forward kinematics

## • Step 4: the link transformations are then multiplied

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

## This transformation 𝑁0𝐓 is a function of all N joint variables.

If the robot's joint-position sensors are queried, the
Cartesian position and orientation of the last link can be
computed by 𝑁0𝐓.

##  C.B. Pham 4-31

Example: a 3-DOF planar robot, RRR

D-H parameters

4-32 i ai ai di qi

1 0 l1 0 q1

2 0 l2 0 q2

3 0 l3 0 q3

##  C.B. Pham 4-32

Example: a 3-DOF planar robot, RRR

##  C.B. Pham 4-33

Example: a 3-DOF planar robot, RRR

##  C.B. Pham 4-34

Example: a 3-DOF planar robot, RRR

##  C.B. Pham 4-35

Example: a 3-DOF planar robot, RRR

##  C.B. Pham 4-36

Example: 6-DOF Puma robot

##  C.B. Pham 4-37

Example: 6-DOF Puma robot

##  C.B. Pham 4-38

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
 C.B. Pham 4-39
Example: 6-DOF Puma robot

##  C.B. Pham 4-40

Example: 6-DOF Puma robot

Where

##  C.B. Pham 4-41

Example: 6-DOF Puma robot

and

##  C.B. Pham 4-42

Note: notation of D-H parameters used in [2]

## • In [2], axis Zi is coincident with the joint axis i. P q3

L3
i ai-1 ai-1 di qi
L2
1 0 0 0 q1
2 0 L1 0 q2 q2
L1
3 0 L2 0 q3
q1
When:

##  C.B. Pham 4-43

4.5. Inverse kinematics

##  C.B. Pham 4-44

4.5.1. 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

##  C.B. Pham 4-45

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:

##  C.B. Pham 4-46

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.
 C.B. Pham 4-47
Multiple solutions

## The PUMA 560

can reach certain
goals with eight
different solutions.

##  C.B. Pham 4-48

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.
 C.B. Pham 4-49
4.5.2. Algebraic vs. Geometric

• Algebraic solution

4-50
i ai ai di qi

1 0 l1 0 q1

2 0 l2 0 q2

3 0 l3 0 q3

##  C.B. Pham 4-50

Algebraic solution

4-51

## Because this is a planar manipulator, transformation from

{3} to {0} can be expressed in form:

##  C.B. Pham 4-51

Algebraic solution

• Compute q2

##  C.B. Pham 4-52

Algebraic solution

##  C.B. Pham 4-53

Algebraic solution

• Compute q1

We have

Where

##  C.B. Pham 4-54

Algebraic solution

If: then:

• Compute q3

##  C.B. Pham 4-55

4.5.2. 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

##  C.B. Pham 4-56

Geometric solution

4-57
For AC of ABC:

##  C.B. Pham 4-57

Geometric solution

##  C.B. Pham 4-58

4.5.3. 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

##  C.B. Pham 4-59

4.5.3. Three consecutive axes intersect

We have

##  C.B. Pham 4-60

4.5.3. Three consecutive axes intersect

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.
 C.B. Pham 4-61