L4 - Forward Kinematics 1 V1

FAB30703
Universiti Kuala Lumpur Malaysia France Institute
Forward Kinematics - 1
Originally prepared by: Prof Engr Dr Ishkandar Baharin

Head of Campus & Dean
UniKL MFI
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
Kinematics Frame Assignment

using Denavit-Hartenberg
Convention
Finding the DH Parameters for our Robots

Coordinate Transformations
End-
effector
Z Robot Base
Supply
Table
X
Y Goal
X
Robot Forward Kinematics
End-
effector
Robot Base
Supply
End - Effector
GoalT
Goal
Table
Proof this equation !!!
Robot forward
kinematic model
Manipulator Forward Kinematics
• Motion is composition of elementary

motions for each link
End-effector
Base
Link i-1
Link i
Relative Pose between 2 links
Relative Pose between 2 links
• 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
• 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
• 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
• 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
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
• 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
• 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
• zi and zi-1 are parallel
• 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
If joint axes zi-1 and zi zi
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
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
Zi
Yi
xi points from F{i-1} F{i}
frame i-1 to i xi
DH convention: Origin parallel Z
zi-1 zi
F{i-1} F{i}
xi
DH convention: x axis - parallel Z

• Yi is chosen to make a right hand frame
zi-1 yi
zi
xi
DH convention: origin
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
Yi is chosen to make a
right hand frame
zi
Oi
zi-1 F{i}
yi
link i
xi
End-Effector Frame
• 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
• 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
Zi-1
Link i
ai
αi
Zi
ai+1
Z’i
Link Parameters
αi+1
Zi+1
θi
Link i
θi
di
θi+1
Joint Parameters
di+1
Original DH
-1
Frame is placed at
distal end of link
zi-1 screw motion xi screw motion

Class Test – You Try !!!

DH Frames and Parameters
Robot Revolute Joint DH


Prismatic Joint DH
Link Transformations
• Described by 4 parameters:
– α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
• 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

Ai =
A-matrices
or
contains only one variable
Original DH
-1
Frame is placed at
distal end of link
zi-1 screw motion xi screw motion

Modified DH
Zi Zi+1 Zi+2
Link i
Link i+1
zi yi
Frame is placed at
xi proximal end of link i
xi-1 screw motion zi screw motion

Modified DH – text figure

DH Example: “academic manipulator”
•3 revolute joints
•Shown in home position
joint 1
Follow Right-Hand-Rule !!!
R
Link 2 Link 3
Link 1
joint 2 joint 3
L1 L2
Zi is axis of actuation for joint i+1
(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
Z0 and Z1 are not co-planar:

Use x0 is the common normal
Z0
θ1
x0
θ2 θ3
Z1 Z2 Z3
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
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
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

Link lengths given
α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
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
Link Var θ d α a
1 θ1 θ1 0 90o R
2 θ2 θ2 0 0 L1
3 θ3 θ3 0 0 L2
manipulator”
DH Example: “academic
z0 x3
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
z3
θ2 θ3
x2
x1
θ1 z2
x0
z1
wrt {1} wrt {1} wrt {1}
Origin of {2}
w.r.t. {1}
z0 x3
z3
θ2 θ3
x2
x1
θ1 z2
x0
z1
wrt {2} wrt {2} wrt {2}
Origin of {3}
w.r.t. {2}
where
DH Example: “academic manipulator” –
alternate end-effector frame
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
Z0
y2
θ1
x1 x2
α1 x0
Z3
θ2 θ3
Z1 Z2
Would need
to rotate
about y2
here!
Z0
x’2
θ1
x1 x2
α1 x0
Z3
θ2 θ3
Z1
Solution: Add
“offset” to
rotation about z2
(θ3+90o )
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
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
x3
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
x3
Z0 R
x’2
L1 L2 Z3
θ1
x1 x2
α1 x0
Z3
θ2 θ3
Z1 Z2
x3
Z0 R
x’2
L1 L2 Z3
θ1
x1 x2
α1 x0
Z3
θ2 θ3
Z1 Z2
x3
Z0 R
x’2
L1 L2 Z3
θ1
x1 x2
α1 x0
Z3
θ2 θ3
Z1 Z2
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
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
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
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
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
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
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
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 ⎥⎦
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
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(α-θ)
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

where
TRR = T2 T1
⎡ 1 0 L2 cos(θ) + L2 cos(α − θ)⎤

TRR = ⎢0 1 L2 sin(θ) − L2 sin(α − θ) ⎥
⎣0 0 1 ⎦
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
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
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 ⎥⎦

L4 - Forward Kinematics 1 V1

Diunggah oleh

Informasi Dokumen

Hak Cipta

Format Tersedia

Bagikan dokumen Ini

Bagikan atau Tanam Dokumen

Opsi Berbagi

Apakah menurut Anda dokumen ini bermanfaat?

Apakah konten ini tidak pantas?

Hak Cipta:

Format Tersedia

L4 - Forward Kinematics 1 V1

Diunggah oleh

Hak Cipta:

Format Tersedia

FAB30703

Universiti Kuala Lumpur Malaysia France Institute

Originally prepared by: Prof Engr Dr Ishkandar Baharin

Kinematics Frame Assignment

Finding the DH Parameters for our Robots

Robot Forward Kinematics

Proof this equation !!!

• Motion is composition of elementary

• Frames can be chosen arbitrarily

• Zi axis is along joint axis i+1

Imagine Link i does’nt move

• Use actuation as a guide

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

• Z1 axis is along joint axis 2

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

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

• Common normal to axes is the “link” axis

Origin of Frame {i}

• Infinitely many common normals

intersect, xi is normal to the

Origin of Frame {i}

• Origin is intersection of xi with zi

• Origin is intersection of xi with zi

If joint axes intersect, the

• Frame to which the

zi-1 screw motion xi screw motion

Class Test – You Try !!!

Robot Revolute Joint DH

Class Test – You Try !!!

Class Test – You Try !!!

zi-1 screw motion xiscrew motion

zi-1 screw motion xi screw motion

xi-1 screw motion zi screw motion

Modified DH – text figure

•Shown in home position

(The Distal Frame !!!)

Z0 and Z1 are not co-planar:

Use x0 is the common normal

Z1 and Z2 are parallel :

x0 is the common normal

Z2 and Z3 are parallel :

Shown with joints in non-zero positions

Observe that frame i moves with link i

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

Zi is axis of actuation for joint i+1

Class Test – Your Turn !!!!

and the second link to move by θ 2

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

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

Joints J1 and J2 are linear joints

Combining the two equation gives:

X = T2 (T1 X1) = TRR X1

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

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

Anda mungkin juga menyukai