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

FAB30703

Universiti Kuala Lumpur Malaysia France Institute

Forward Kinematics - 1

Originally prepared by: Prof Engr Dr Ishkandar Baharin


Head of Campus & Dean
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


motions for each link

End-effector

Base
Universiti Kuala Lumpur Malaysia France Institute

Link i-1
Link i
Relative Pose between 2 links
Relative Pose between 2 links
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
Link i Link i+1 moves

Link i+1

F{i}

Link i-1
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
Imagine Link 1 doesn’t move
Z0 Z1
Link 1 Link 2 moves

Link 2

F{1}

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


Link 0
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}
Link 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}

link 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

link axis

Joint Axis

zi
Origin of F{i}

Oi Link Axis

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}

link 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
link i
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
F{e} Xn axis (Link n
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

Link i

ai
αi
Zi

ai+1
Z’i
Link Parameters
αi+1

Zi+1
Universiti Kuala Lumpur Malaysia France Institute

θi
Link i

θi
di
θi+1
Joint Parameters

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

Frame is placed at
distal end of link

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

Robot Revolute Joint DH

Class Test – You Try !!!


Universiti Kuala Lumpur Malaysia France Institute

Class Test – You Try !!!


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

– αi : twist
– ai : link length
– 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
Link Transformations
Universiti Kuala Lumpur Malaysia France Institute

• Described by 4 parameters:
– αi : twist
– ai : link length
– di : joint offset
– θi : joint angle
• Joint variable is di or θi
• Link Transformation is

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
distal end of link

zi-1 screw motion xi screw motion


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

Link i
Link i+1

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


DH Example: “academic manipulator”
•3 revolute joints
Universiti Kuala Lumpur Malaysia France Institute

•Shown in home position

joint 1
Follow Right-Hand-Rule !!!
R
Link 2 Link 3

Link 1
joint 2 joint 3

L1 L2
DH Example: “academic manipulator”
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
DH Example: “academic manipulator”
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
DH Example: “academic manipulator”
Z0 and Z1 are not co-planar:
Universiti Kuala Lumpur Malaysia France Institute

Use x0 is the common normal


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
of the link
DH Example: “academic manipulator”
Z0 and Z1 are not co-planar:
Universiti Kuala Lumpur Malaysia France Institute

x0 is the common normal


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
the link
DH Example: “academic manipulator”
Universiti Kuala Lumpur Malaysia France Institute

Shown with joints in non-zero positions


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

Observe that frame i moves with link i


DH Example: “academic manipulator”
Link lengths given
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
DH Example: “academic manipulator”
Z0 R
Universiti Kuala Lumpur Malaysia France Institute

L1 L2
θ1
Link 1 x1 Link 2 x2 x3
Link 3
α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
DH Example: “academic manipulator”
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”
DH Example: “academic
DH Example: “academic 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}
DH Example: “academic manipulator”
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}
DH Example: “academic manipulator”
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
DH Example: “academic manipulator”
DH Example: “academic manipulator” –
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
DH Example: “academic manipulator” –
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
about y2
here!
DH Example: “academic manipulator” –
alternate end-effector frame
Universiti Kuala Lumpur Malaysia France Institute

Z0
x’2
θ1
x1 x2
α1 x0
Z3
θ2 θ3
Z1
Solution: Add
“offset” to
rotation about z2
(θ3+90o )
DH Example: “academic manipulator” –
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
about x’ to
align z2 and z3
DH Example: “academic manipulator” –
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
DH Example: “academic manipulator” –
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
DH Example: “academic manipulator” –
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
DH Example: “academic manipulator” –
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
DH Example: “academic manipulator” –
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

Combining the two equation gives:

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 ⎥⎦