Вы находитесь на странице: 1из 76

# FAB30703

## Universiti Kuala Lumpur Malaysia France Institute

Forward Kinematics - 1

## Originally prepared by: Prof Engr Dr Ishkandar Baharin

UniKL MFI
Universiti Kuala Lumpur Malaysia France Institute

Orientation Representations
P •Euler Angle I
•Euler Angle II
•Yaw-Pitch-Roll

DH Parameters
⎡ q1 ⎤ ⎡X ⎤
⎢q ⎥ Joint ⎢Y ⎥
⎢ 2⎥ variables
Position and Orientation
of the end-effector
⎢ ⎥
q = ⎢ q3 ⎥ Direct Kinematics ⎢Z ⎥
P=⎢ ⎥
⎢ ⎥
⎢M⎥ ⎢ψ ⎥
⎢⎣ qn ⎥⎦ ⎢θ ⎥
⎢ ⎥
⎣φ ⎦
Inverse Kinematics
Universiti Kuala Lumpur Malaysia France Institute

## Kinematics Frame Assignment

using Denavit-Hartenberg
Convention

## Finding the DH Parameters for our Robots

Coordinate Transformations
Universiti Kuala Lumpur Malaysia France Institute

End-
effector

Z Robot Base

Supply
Table
X
Y Goal
X
Coordinate Transformations
Universiti Kuala Lumpur Malaysia France Institute

## Robot Forward Kinematics

End-
effector
Robot Base

Supply
End - Effector
GoalT
Goal

Table
Coordinate Transformations
Universiti Kuala Lumpur Malaysia France Institute

## Proof this equation !!!

Robot forward
kinematic model
Manipulator Forward Kinematics
Universiti Kuala Lumpur Malaysia France Institute

## • Motion is composition of elementary

End-effector

Base
Universiti Kuala Lumpur Malaysia France Institute

Universiti Kuala Lumpur Malaysia France Institute

## • Frames can be chosen arbitrarily

• Denavit-Hartenberg convention is used to assign
frames
• Iterative process (start at base, assign frames
for each link from base to end-effector)
DH Frame assignment
• Frame {i} moves with link i when joint i is actuated
Universiti Kuala Lumpur Malaysia France Institute

## • Zi axis is along joint axis i+1

Joint axis i+1
• Zi is axis of actuation for joint i+1

## Imagine Link i does’nt move

Zi-1 Zi

F{i}

DH convention: Assign Z axes
Universiti Kuala Lumpur Malaysia France Institute

## • Use actuation as a guide

– Prismatic – joint slides along zi
– Revolute – joint rotates around zi
• Establish base frame {0}:
– Nearly arbitrary

## • Start at base and assign frames 1,…,N

– Pick x-axis and origin
– y-axis chosen to form a right hand system
DH Frame assignment
• Frame {1} moves with link 1 when joint 1 is actuated
Universiti Kuala Lumpur Malaysia France Institute

## • Z1 axis is along joint axis 2

Joint axis 2
• Z1 is axis of actuation for joint 2
Imagine there is a joint here to move link 2,
called joint 2
Z0 Z1

F{1}

## Lets try for first Frame, where i=1; F{1}

Robot Base
• Often base is “given” or
some fixed point on the
Universiti Kuala Lumpur Malaysia France Institute

work-table is used.
• z0 is along joint axis 1
• Origin:
– any point on z0 for origin
• Modified DH:
– {0} is defined to be
completely co-incident
with the reference system
{1}, when the variable
joint parameter, d1 or θ1 ,
is zero.
DH convention: Assign X axes
Universiti Kuala Lumpur Malaysia France Institute

## • Start at base and assign frames 1,…,N

– Pick x-axis and origin
– y-axis chosen to form a right hand system
• Consider 3 cases for zi-1 and zi:
– Not-coplanar
– Parallel
– Intersect
DH convention: x axis
• zi-1 and zi are not-coplanar
Universiti Kuala Lumpur Malaysia France Institute

## • Common normal to axes is the “link” axis

• Intersection with zi is origin

## Origin of Frame {i}

zi-1 zi
Xi Usually, xi
F{i-1} points from
“Link Axis” F{i} frame i-1 to i
DH convention: x axis
• zi and zi-1 are parallel
Universiti Kuala Lumpur Malaysia France Institute

## • Infinitely many common normals

• Pick one to be the “link” axis
• Choose normal that passes through origin of frame
{i-1} pointing toward zi
• Origin is intersection of xi with zi
Origin of Frame {i}
Xi
F{i-1} F{i}

zi-1 zi
DH convention: x axis
If joint axes zi-1 and zi zi
Universiti Kuala Lumpur Malaysia France Institute

## intersect, xi is normal to the

plane containing the axes

xi = ±(zi-1 × zi )

## Origin of Frame {i}

zi-1

xi = ±(zi-1 × zi ) Xi
DH convention: Origin non-coplanar Z
Origin of frame {i} is placed at intersection of joint axis and
Universiti Kuala Lumpur Malaysia France Institute

Joint Axis

zi
Origin of F{i}

F{i} xi
DH convention: y axis
• Yi is chosen to make a right hand frame
Universiti Kuala Lumpur Malaysia France Institute

Zi
Yi
xi points from F{i-1} F{i}
frame i-1 to i xi
DH convention: Origin parallel Z
• zi and zi-1 are parallel
Universiti Kuala Lumpur Malaysia France Institute

## • Origin is intersection of xi with zi

zi-1 zi
F{i-1} F{i}
xi
DH convention: x axis - parallel Z
• zi and zi-1 are parallel
Universiti Kuala Lumpur Malaysia France Institute

## • Origin is intersection of xi with zi

• Yi is chosen to make a right hand frame

zi-1 yi
zi

xi
DH convention: origin
Universiti Kuala Lumpur Malaysia France Institute

## If joint axes intersect, the

origin of frame {i} is
usually placed at
intersection of the joint
axes

zi
Oi
zi-1 F{i}

xi
DH convention: y axis
Universiti Kuala Lumpur Malaysia France Institute

Yi is chosen to make a
right hand frame

zi
Oi
zi-1 F{i}
yi
xi
End-Effector Frame
Universiti Kuala Lumpur Malaysia France Institute

• Frame to which
the gripper is
attached
– Sometimes {n} is
used
Zn – denoted by {e} (or
{n+1} in many
texts)
Xn – Often simple
Ze translation along
Xe axis) or the Last
Axis !!!
End-Effector Frame
Universiti Kuala Lumpur Malaysia France Institute

## • Frame to which the

gripper is attached –
– denoted by {e} (or
{n+1} in many texts)
– Often simple
translation along Xn
axis
Z4 • Often:
– Origin between
grippers
– Z points outward
(approach)
ye F{e}
– Y points along pinch
direction (sliding)
– X points normal
xe ze
Universiti Kuala Lumpur Malaysia France Institute

Zi-1

ai
αi
Zi

ai+1
Z’i
αi+1

Zi+1
Universiti Kuala Lumpur Malaysia France Institute

θi

θi
di
θi+1
Joint Parameters

di+1
Original DH
-1
Universiti Kuala Lumpur Malaysia France Institute

Frame is placed at

## zi-1 screw motion xi screw motion

Universiti Kuala Lumpur Malaysia France Institute

## Class Test – You Try !!!

DH Frames and Parameters
Universiti Kuala Lumpur Malaysia France Institute

## Class Test – You Try !!!

Universiti Kuala Lumpur Malaysia France Institute

## Class Test – You Try !!!

Prismatic Joint DH
• Described by 4 parameters:
Universiti Kuala Lumpur Malaysia France Institute

– αi : twist
– di : joint offset
– θi : joint angle
• Joint variable is di or θi
• Build Table with values for each link:

Link Var θ d α a
1 θ1 θ1 0 90o L1
2 d2 0 d2 0 0
M M M M M M
Universiti Kuala Lumpur Malaysia France Institute

• Described by 4 parameters:
– αi : twist
– di : joint offset
– θi : joint angle
• Joint variable is di or θi

## zi-1 screw motion xiscrew motion

Universiti Kuala Lumpur Malaysia France Institute

Ai =
A-matrices

or
contains only one variable
Original DH
-1
Universiti Kuala Lumpur Malaysia France Institute

Frame is placed at

## zi-1 screw motion xi screw motion

Modified DH
Zi Zi+1 Zi+2
Universiti Kuala Lumpur Malaysia France Institute

zi yi
Frame is placed at
xi proximal end of link i

## xi-1 screw motion zi screw motion

Universiti Kuala Lumpur Malaysia France Institute

## Modified DH – text figure

•3 revolute joints
Universiti Kuala Lumpur Malaysia France Institute

## •Shown in home position

joint 1
R

joint 2 joint 3

L1 L2
Zi is axis of actuation for joint i+1
Universiti Kuala Lumpur Malaysia France Institute

## (The Distal Frame !!!)

Start from Base, where i=0
Z0 Z0 and Z1 are not co-planar
θ1 Z1 and Z2 are parallel
Joint 1

Joint 2 Joint 3

θ2 θ3
Z1 Z2
Universiti Kuala Lumpur Malaysia France Institute

## Z0 and Z1 are not co-planar:

Use x0 is the common normal
Z0

θ1

x0

θ2 θ3
Z1 Z2 Z3
Z0 and Z1 are not co-planar:
Universiti Kuala Lumpur Malaysia France Institute

Z0

θ1
x1
x0

θ2 θ3
Z1 Z2 Z3

## Z1 and Z2 are parallel :

x1 is selected as the common
normal that lies along the center
Z0 and Z1 are not co-planar:
Universiti Kuala Lumpur Malaysia France Institute

Z0

θ1
x1 x2 x3
x0

θ2 θ3
Z1 Z2 Z3

## Z2 and Z3 are parallel :

x2 is selected as the common
normal that lies along the center of
Universiti Kuala Lumpur Malaysia France Institute

F{3}
Z0 x3
k3
Lin z3
k 2
θ2 F{2} θ3
L i n
F{1}
x1
θ1 Z2
x0
Z1

## Observe that frame i moves with link i

Universiti Kuala Lumpur Malaysia France Institute

## α1 = 90o (rotate by 90o around x0 to align Z0 and Z1)

R
Z0
L1 L2
x1 x2 x3
α1 x0

Z1 Z2 Z3
Z0 R
Universiti Kuala Lumpur Malaysia France Institute

L1 L2
θ1
α1 x0

θ2 θ3
Z1 Z2 Z3

Build table
Link Var θ d α a
1 θ1 θ1 0 90o R
2 θ2 θ2 0 0 L1
3 θ3 θ3 0 0 L2
Universiti Kuala Lumpur Malaysia France Institute

Link Var θ d α a
1 θ1 θ1 0 90o R
2 θ2 θ2 0 0 L1
3 θ3 θ3 0 0 L2
Universiti Kuala Lumpur Malaysia France Institute

manipulator”
z0 x3
Universiti Kuala Lumpur Malaysia France Institute

z3
θ2 θ3
x2
x1
θ1 z2
x0
z1
x1 axis expressed y1 axis expressed z1 axis expressed
wrt {0} wrt {0} wrt {0}

Origin of {1}
w.r.t. {0}
z0 x3
Universiti Kuala Lumpur Malaysia France Institute

z3
θ2 θ3
x2
x1
θ1 z2
x0
z1
x2 axis expressed y2 axis expressed z2 axis expressed
wrt {1} wrt {1} wrt {1}

Origin of {2}
w.r.t. {1}
z0 x3
Universiti Kuala Lumpur Malaysia France Institute

z3
θ2 θ3
x2
x1
θ1 z2
x0
z1
x3 axis expressed y3 axis expressed z3 axis expressed
wrt {2} wrt {2} wrt {2}

Origin of {3}
w.r.t. {2}
Universiti Kuala Lumpur Malaysia France Institute

where
alternate end-effector frame
Universiti Kuala Lumpur Malaysia France Institute

## Zi is axis of actuation for joint i+1

Z0 Z0 and Z1 are not co-planar
θ1 Z1 and Z2 are parallel

θ3 Pick this z3
Z1 θ2 Z2
alternate end-effector frame
Universiti Kuala Lumpur Malaysia France Institute

Z0
y2
θ1
x1 x2
α1 x0
Z3
θ2 θ3
Z1 Z2
Would need
to rotate
here!
alternate end-effector frame
Universiti Kuala Lumpur Malaysia France Institute

Z0
x’2
θ1
x1 x2
α1 x0
Z3
θ2 θ3
Z1
“offset” to
(θ3+90o )
alternate end-effector frame
Universiti Kuala Lumpur Malaysia France Institute

Z0
x’2
L2 x3
θ1
x1 x2
α1 x0
Z3
θ2 θ3
Z1 Z2
Now can rotate
align z2 and z3
alternate end-effector frame
Universiti Kuala Lumpur Malaysia France Institute

Link Var θ d α a
1 θ1 θ1 0 90o R
2 θ2 θ2 0 0 L1
3 θ3 θ3 +90o 0 90o 0
e - L2
alternate end-effector frame
x3
Universiti Kuala Lumpur Malaysia France Institute

Z0 R
x’2
L1 L2 Z3
θ1
x1 x2
α1 x0

θ2 θ3
Z1 Z2

Link Var θ d α a
1 θ1 θ1 0 90o R
2 θ2 θ2 0 0 L1
3 θ3 θ3 +90o 0 90o 0
alternate end-effector frame
Universiti Kuala Lumpur Malaysia France Institute

x3

Z0 R
x’2
L1 L2 Z3
θ1
x1 x2
α1 x0
Z3
θ2 θ3
Z1 Z2
alternate end-effector frame
x3
Universiti Kuala Lumpur Malaysia France Institute

Z0 R
x’2
L1 L2 Z3
θ1
x1 x2
α1 x0
Z3
θ2 θ3
Z1 Z2
alternate end-effector frame
x3
Universiti Kuala Lumpur Malaysia France Institute

Z0 R
x’2
L1 L2 Z3
θ1
x1 x2
α1 x0
Z3
θ2 θ3
Z1 Z2
Universiti Kuala Lumpur Malaysia France Institute

## Class Test – Your Turn !!!!

The Situation:
You have a robotic arm that starts
out aligned with the xo-axis.
You tell the first link to move by θ1
Universiti Kuala Lumpur Malaysia France Institute

## and the second link to move by θ 2

The Question:
What is the position of the end of the
robotic arm?

Solution:
1. Geometric Approach
This might be the easiest solution for the simple situation. However, notice
that the angles are measured relative to the direction of the previous link. (The
first link is the exception. The angle is measured relative to it’s initial
position.) For robots with more links and whose arm extends into 3 dimensions
the geometry gets much more tedious.

2. Algebraic Approach
Involves coordinate transformations.
Example Problem:
You have a three link arm that starts out aligned in the x-axis. Each link
has lengths l1, l2, l3, respectively. You tell the first one to move by θ1 , and
Universiti Kuala Lumpur Malaysia France Institute

## so on as the diagram suggests. Find the Homogeneous matrix to get the

position of the yellow dot in the X 0 Y0 frame.
Y3

θ3
Y2 θ2 X2 X3

l3
l2
Y0
X1 l1
Y1 θ1
X0
Y3

θ3
⎡l3 ⎤
θ2
P33 = ⎢⎢ 0 ⎥⎥
Y2 X2 X3
Universiti Kuala Lumpur Malaysia France Institute

l3 ⎢⎣ 0 ⎥⎦
l2
Vector P33 is in the X3-Y3 frame.
Y0
X1 l1 3
Find Vector P3 in the X0-Y0 frame ?.
Y1 θ1 Solution:
X0

## Rotating by θ1 will put you in the X 1 Y1 frame.

Translating along the X 1 axis by l1.
Rotating by θ 2 will put you in the X 2 Y2 frame, and so on until you are in the
X 3 Y3 frame. The position of the yellow dot relative to the X 3 Y3 frame is
(l3, 0). Multiplying H by that position vector will give you the
coordinates of the yellow point relative the the X 0 Y0 frame.
H 03 = RZ (θ1 ) ⋅ TX 1 (l1 ) ⋅ RZ (θ 2 ) ⋅ TX 2 (l2 ) ⋅ RZ (θ3 )
P03 = H 03 ⋅ P33
Slight variation on the last solution:
Make the yellow dot the origin of a new coordinate X 4 Y4 frame
Y3
Universiti Kuala Lumpur Malaysia France Institute

Y4
θ3
Y2 θ2 X2 X3
X4
l3
l2
Y0
l1
X1
H = RZ (θ1 ) ⋅ TX 1 (l1 ) ⋅ RZ (θ 2 ) ⋅ TX 2 (l2 ) ⋅ RZ (θ 3 ) ⋅ TX 3 (l3 )
Y1 θ1
X0 This takes you from the X 4 Y4 frame to the X 0 Y0 frame.
The position of the yellow dot relative to theX 4 Y4
frame is (0,0).
⎡X ⎤ ⎡0 ⎤
⎢Y ⎥ ⎢0 ⎥
⎢ ⎥ = H ⎢ ⎥ Notice that multiplying by the (0,0,0,1) vector will equal
⎢Z ⎥ ⎢0 ⎥ the last column of the H matrix.
⎢ ⎥ ⎢ ⎥
⎣ 1 ⎦ ⎣1 ⎦
Forward KinematicTransformation
LL Robot:
Let us consider a Cartesian LL robot

## Joints J1 and J2 are linear joints

with links of variable lengths L1 and
J1(x1, y1) L
y 2 L2. Let joint J1 be denoted by (x1
J2(x2,y2) y1) and joint J2 by (x2, y2).

L3
From geometry, we can easily get
the following:
(x, y)
L1
x2=x1+L2 y2 = y1

x
Forward KinematicTransformation
These relations can be represented in homogeneous matrix
form:

⎡ x2 ⎤ ⎡ 1 0 L2 ⎤ ⎡ x1⎤
⎢ y2 ⎥ = ⎢0 1 0 ⎥ ⋅ ⎢ y1⎥
⎣ 1 ⎦ ⎣0 0 1 ⎦ ⎣ 1 ⎦

or X2=T1 X1
Forward KinematicTransformation
where

⎡ x2 ⎤ ⎡ 1 0 L2 ⎤ ⎡x1⎤
X2 = ⎢ y 2 ⎥ T1 = ⎢0 1 0 ⎥ X1⎢y1⎥
⎣ 1⎦ ⎣0 0 1 ⎦ ⎣ 1⎦
If the end-effector point is denoted by (x, y), then:

x = x2
y = y2 - L3
Forward KinematicTransformation
therefore:
⎡ x ⎤ ⎡ 1 0 0 ⎤ ⎡ x2 ⎤
⎢ y ⎥ = ⎢0 1 −L2 ⎥ ⋅ ⎢ y2 ⎥
⎢⎣ 1 ⎥⎦ ⎣0 0 1 ⎦ ⎣ 1 ⎦

X = T2 X2
or TLL = T2 T1

and ⎡ 1 0 L2 ⎤
TLL = ⎢0 1 −L ⎥
⎢⎣0 0 1 ⎥⎦
Forward KinematicTransformation
RR Robot:
Let θ and α be the rotations at joints J1 and J2
respectively. Let J1 and J2 have the coordinates of (x1,
y1) and (x2, y2), respectively.
J
2
y (x 2 y2 )

L2 -
One can write the following
L
3 from the geometry:
J1 (x y)
(x 1 y1)

L
1
x2 = x1+L2 cos(θ)
y2 = y1 +L2 sin(θ)
x
Forward KinematicTransformation
In matrix form:
⎡x2 ⎤ ⎡ 1 0 L2 cos(θ)⎤ ⎡x1⎤
⎢y2 ⎥ = ⎢0 1 L2 sin(θ) ⎥ ⋅ ⎢y1⎥
⎣ 1 ⎦ ⎣0 0 1 ⎦ ⎣ 1⎦

or
X2 = T1 X1
On the other end:

x = x2 +L3 cos(α-θ)
y = y2 - L3 sin(α-θ)
Forward KinematicTransformation
In matrix form: ⎡x⎤ ⎡ 1 0 L2 cos(α − θ) ⎤ ⎡x2 ⎤
⎢y⎥ = ⎢0 1 −L2 sin(α − θ)⎥ ⋅ ⎢y2 ⎥
⎣1⎦ ⎣0 0 1 ⎦ ⎣ 1⎦

or
X = T2 X2

## X = T2 (T1 X1) = TRR X1

Forward KinematicTransformation
where
TRR = T2 T1

## ⎡ 1 0 L2 cos(θ) + L2 cos(α − θ)⎤

TRR = ⎢0 1 L2 sin(θ) − L2 sin(α − θ) ⎥
⎣0 0 1 ⎦
Forward KinematicTransformation
TL Robot:
Let α be the rotation at twisting joint J1 and L2 be the
variable link length at linear joint J2.

z
J2 ( x 2 y 2 ) y One can write that:
(x y )
L2
x = x2 + L2 cos(α)
J1 ( x 1 y 1 )
y = y2 + L2 sin(α)
x
Forward KinematicTransformation
In matrix form:

## ⎡x⎤ ⎡ 1 0 L2 cos(α)⎤ ⎡x2 ⎤

⎢y⎥ = ⎢0 1 L2 sin(α) ⎥ ⋅ ⎢y2 ⎥
⎣1⎦ ⎣0 0 1 ⎦ ⎣ 1⎦

or
X = TTL X2
Link Var θ d α a
1 θ1 θ1 0 90o L1
2 d2 0 d2 0 0
Universiti Kuala Lumpur Malaysia France Institute

M M M M M M
⎡X ⎤
⎡ q1 ⎤ ⎢Y ⎥
⎢q ⎥ ⎢ ⎥
⎢ 2⎥ ⎢Z ⎥
q = ⎢ q3 ⎥ T = T0 = T0 ⋅ T1 ⋅⋅Tn −1 P = Tq P = ⎢ ⎥
n 1 2 n
⎢ ⎥ ⎢ψ ⎥
⎢M⎥ P = Tq ⎢θ ⎥
⎢⎣ qn ⎥⎦ ⎢ ⎥
⎡ X ⎤ ⎡ T1,1 T1,2 T1,3 L T1,n −1 T1,n ⎤ ⎡ q1 ⎤ ⎣ φ ⎦
⎢ Y ⎥ ⎢T M M ⎥⎢
q ⎥
⎢ ⎥ ⎢ 2,1 ⎥⎢ 2 ⎥
⎢ Z ⎥ ⎢T3,1 M M ⎥ ⎢ q3 ⎥
⎢ ⎥=⎢ ⎥⎢ ⎥
⎢ψ ⎥ ⎢T4,1 M M ⎥⎢ M ⎥
⎢ θ ⎥ ⎢T5,1 L L L T5,n −1 T5,n ⎥ ⎢ qn −1 ⎥
⎢ ⎥ ⎢ ⎥⎢ ⎥
⎣ φ ⎦ ⎢⎣T6,1 L L L T6,n −1 T6,n ⎥⎦ ⎢⎣ qn ⎥⎦