Vous êtes sur la page 1sur 61

Ch 4: Manipulator kinematics

Task space Joint space


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

q1, q2, ..., q6 Forward


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:
• a (link length)
describe the link itself
• a (link twist)
• d (link offset) describe the link's connection to a
• 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).
Link i
Zi

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


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

• Link length ai: the ai


length of the common
normal of axis (i) and
axis (i + 1). ai
• Link twist ai: angle
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
Link i - 1 Link i

• Link offset di:


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

Link Link Link Link Joint


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

• Intermediate links in the Link i


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
linkage parameters as possible to
become zero.

 C.B. Pham 4-12


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
• Link length ai: distance from 𝐙i-1 to 𝐙i measured along 𝑿i
 C.B. Pham 4-13
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
• Link twist ai: the angle from 𝐙i-1 to 𝐙i measured about 𝑿i
 C.B. Pham 4-14
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
• Link offset di: distance from 𝐗i-1 to 𝐗i measured along 𝒁i-1
 C.B. Pham 4-15
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
• Joint angle qi: the angle from 𝐗i-1 to 𝐗i measured about 𝒁i-1
 C.B. Pham 4-16
Summary of link-frame attachment procedure

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
Example of link-frame attachment procedure

Assign link frames to an RRR mechanism and give D-H


parameters.

 C.B. Pham 4-18


Example of link-frame attachment procedure

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

 C.B. Pham 4-19


Example of link-frame attachment procedure

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


mechanism.

 C.B. Pham 4-20


Example of link-frame attachment procedure
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
Example of link-frame attachment procedure

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


manipulator.

 C.B. Pham 4-22


Example of link-frame attachment procedure

Two possible frame assignments and corresponding


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

Z0 Z0

Z1 Z1

X1 X1

 C.B. Pham 4-23


Example of link-frame attachment procedure

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


axis 2 intersect.

Z0 X1
Z1

 C.B. Pham 4-24


4.3. 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}: 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


4.3. Link transformation
{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

Link i - 1 Link i

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

From the figure:

 C.B. Pham 4-27


4.3. Link transformation

 C.B. Pham 4-28


4.4. Steps to formulate forward kinematics

The problem of forward kinematics is approached as


follows:

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


each link
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

• 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).
 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

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

 C.B. Pham 4-34


Example: a 3-DOF planar robot, RRR

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

 C.B. Pham 4-35


Example: a 3-DOF planar robot, RRR

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

 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

Place the tool frame:

 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
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-48
4.5.2. Algebraic vs. Geometric

• Algebraic solution

4-49
i ai ai di qi

1 0 l1 0 q1

2 0 l2 0 q2

3 0 l3 0 q3

 C.B. Pham 4-49


Algebraic solution

4-50

Because this is a planar manipulator, transformation from


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

 C.B. Pham 4-50


Algebraic solution

• Compute q2

 C.B. Pham 4-51


Algebraic solution

 C.B. Pham 4-52


Algebraic solution

• Compute q1

We have

Rearrange in q1 results in:

Where

 C.B. Pham 4-53


Algebraic solution

If: then:

• Compute q3

 C.B. Pham 4-54


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-55


Geometric solution

4-56
For AC of ABC:

 C.B. Pham 4-56


Geometric solution

Similarly for BC:

 C.B. Pham 4-57


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-58


4.5.3. Three consecutive axes intersect

We have

 C.B. Pham 4-59


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-60
4.5.3. Three consecutive axes intersect

The PUMA 560


can reach certain
goals with eight
different solutions.

 C.B. Pham 4-61