Introduction - Electrical Engineering
Download
Report
Transcript Introduction - Electrical Engineering
Introduction to ROBOTICS
Robot Kinematics II
Jizhong Xiao
Department of Electrical Engineering
City College of New York
[email protected]
The City College of New York
1
Outline
• Review
– Manipulator Specifications
• Precision, Repeatability
– Homogeneous Matrix
• Denavit-Hartenberg (D-H)
Representation
• Kinematics Equations
• Inverse Kinematics
The City College of New York
2
Review
• Manipulator, Robot arms, Industrial robot
– A chain of rigid bodies (links) connected by
joints (revolute or prismatic)
• Manipulator Specification
– DOF, Redundant Robot
– Workspace, Payload
How accurately a specified point can be reached
– Precision
accurately the same position can be reached
– Repeatability How
if the motion is repeated many times
The City College of New York
3
Review
• Manipulators:
Cartesian: PPP
Cylindrical: RPP
Spherical: RRP
Hand coordinate:
Articulated: RRR
SCARA: RRP
n: normal vector; s: sliding vector;
(Selective Compliance
Assembly Robot Arm)
a: approach vector, normal to the
tool mounting plate
The City College of New York
4
Review
• Basic Rotation Matrix
px i x i u
Pxyz p y jy i u
pz k z i u
Pxyz RPuvw
i x jv
jy jv
k z jv
i x k w pu
jy k w pv RPuvw
k z k w pw
z
w
P
Puvw QPxyz
Q R 1 RT
u
v
y
x
The City College of New York
5
Basic Rotation Matrices
– Rotation about x-axis with
1 0
Rot ( x, ) 0 C
0 S
0
S
C
– Rotation about y-axis with
C
Rot ( y, ) 0
S
– Rotation about z-axis with
Pxyz RPuvw
C
Rot ( z, ) S
0
The City College of New York
S
1 0
0 C
0
S
C
0
0
0
1
6
Review
• Coordinate transformation from {B} to {A}
r ARB B r P Ar o '
A P
A r P A RB
1 013
B P
r
r
1 1
A o'
• Homogeneous transformation matrix
RB
TB
013
A
A
r R33
1 0
A o'
P31
1
Scaling
The City College of New York
Rotation
matrix
Position
vector
7
Review
• Homogeneous Transformation
– Special cases
1. Translation
I 33
A
TB
013
A o'
r
1
A
RB
A
TB
013
031
1
2. Rotation
The City College of New York
8
Review
• Composite Homogeneous Transformation
Matrix
• Rules:
– Transformation (rotation/translation) w.r.t. (X,Y,Z)
(OLD FRAME), using pre-multiplication
– Transformation (rotation/translation) w.r.t.
(U,V,W) (NEW FRAME), using postmultiplication
The City College of New York
9
Review
• Homogeneous Representation
– A point in R3 space
px
p
P y Homogeneous coordinate of P w.r.t. OXYZ
pz
P
(
p
,
z
x
1
a
– A frame in R space
3
nx
n s a P n y
F
0 0 0 1 nz
0
sx
ax
sy
ay
sz
az
0
0
px
p y
pz
1
The City College of New York
p y , pz )
s
n
y
x
10
Review
• Orientation Representation
(Euler Angles)
– Description of Yaw, Pitch, Roll
• A rotation of about the OX
axis ( Rx , ) -- yaw
• A rotation of about the OY
axis ( R y , ) -- pitch
• A rotation of about the OZ
axis ( Rz , ) -- roll
Z
roll
yaw
Y
pitch
X
The City College of New York
11
Quiz 1
• How to get the resultant rotation matrix for YPR?
T Rz , R y , Rx ,
C
S
0
0
S
C
0
0
0 0 C
0 0 0
1 0 S
0 1 0
Z
X
0
S
1
0
0
0
0
1
0 C
0
0
1 0
0 C
0 S
0 0
0
S
C
0
0
0
0
1
The City College of New York
Y
12
Quiz 2
• Geometric Interpretation?
R33
T
0
P31
1
Orientation of OUVW coordinate
frame w.r.t. OXYZ frame
Position of the origin of OUVW
coordinate frame w.r.t. OXYZ frame
• Inverse Homogeneous Matrix?
T
T
Inverse of the rotation submatrix
R
R
P
1
is equivalent to its transpose
T
Position of the origin of OXYZ
1 reference frame w.r.t. OUVW frame
0
T
R
1
T T
0
RT P R P RT R 0
I 44
1 0 1 0 1
The City College of New York
13
Kinematics Model
• Forward (direct) Kinematics
q (q1 , q2 , qn )
Joint
variables
Direct Kinematics
z
Position and Orientation
of the end-effector
y
x
Inverse Kinematics
Y ( x, y, z, , , )
• Inverse Kinematics
The City College of New York
14
Robot Links and Joints
The City College of New York
15
Denavit-Hartenberg Convention
• Number the joints from 1 to n starting with the base and ending with
the end-effector.
• Establish the base coordinate system. Establish a right-handed
orthonormal coordinate system ( X 0 , Y0 , Z 0 ) at the supporting base
with Z 0 axis lying along the axis of motion of joint 1.
• Establish joint axis. Align the Zi with the axis of motion (rotary or
sliding) of joint i+1.
• Establish the origin of the ith coordinate system. Locate the origin of
the ith coordinate at the intersection of the Zi & Zi-1 or at the
intersection of common normal between the Zi & Zi-1 axes and the Zi
axis.
• Establish Xi axis. Establish X i (Zi 1 Zi ) / Zi 1 Zi or along the
common normal between the Zi-1 & Zi axes when they are parallel.
• Establish Yi axis. Assign Yi (Zi X i ) / Zi X i to complete the righthanded coordinate system.
• Find the link and joint parameters
The City College of New York
16
Example I
• 3 Revolute Joints
Z3
Z1
Z0
Y0
O3
Y1
Link 1
Joint 1
Joint 3
O0 X0
Joint 2
Link 2
X3
d2
O1 X1 O2 X2
Y2
a0
a1
The City College of New York
17
Link Coordinate Frames
• Assign Link Coordinate Frames:
– To describe the geometry of robot motion, we assign a Cartesian
coordinate frame (Oi, Xi,Yi,Zi) to each link, as follows:
• establish a right-handed orthonormal coordinate frame O0 at
the supporting base with Z0 lying along joint 1 motion axis.
• the Zi axis is directed along the axis of motion of joint (i + 1),
that is, link (i + 1) rotates about or translates along Zi;
Z3
Z1
Z0
Y0
O3
Y1
Link 1
Joint 1
Joint 3
O0 X0
Joint 2
Link 2
X3
d2
O1 X1 O2 X2
Y2
a0
a1
The City College of New York
18
Link Coordinate Frames
– Locate the origin of the ith coordinate at the intersection
of the Zi & Zi-1 or at the intersection of common normal
between the Zi & Zi-1 axes and the Zi axis.
– the Xi axis lies along the common normal from the Zi-1
axis to the Zi axis X i (Zi 1 Zi ) / Zi 1 Zi , (if Zi-1 is
parallel to Zi, then Xi is specified arbitrarily, subject only
Z3
to Xi being perpendicular to Zi);
Z1
Z0
Y0
Joint 3
O3
Y1
X3
d2
Joint 1
O0 X0
Joint 2
O1 X1 O2 X2
Y2
a0
a1
The City College of New York
19
Link Coordinate Frames
– Assign Yi (Zi X i ) / Zi X i to complete the righthanded coordinate system.
• The hand coordinate frame is specified by the geometry
On
of the end-effector. Normally, establish Zn along the
direction of Zn-1 axis and pointing away from the robot;
establish Xn such that it is normal to both Zn-1 and Zn
axes. Assign Yn to complete the right-handed coordinate
system.
Z3
Z1
Z0
Y0
Joint 3
O3
Y1
X3
d2
Joint 1
O0 X0
Joint 2
O1 X1 O2 X2
Y2
a0
a1
The City College of New York
20
Link and Joint Parameters
• Joint angle i : the angle of rotation from the Xi-1 axis to
the Xi axis about the Zi-1 axis. It is the joint variable if joint i
is rotary.
• Joint distance d i : the distance from the origin of the (i-1)
coordinate system to the intersection of the Zi-1 axis and
the Xi axis along the Zi-1 axis. It is the joint variable if joint i
is prismatic.
• Link length ai : the distance from the intersection of the Zi-1
axis and the Xi axis to the origin of the ith coordinate
system along the Xi axis.
• Link twist angle i : the angle of rotation from the Zi-1 axis
to the Zi axis about the Xi axis.
The City College of New York
21
Example I
Z3
Z1
Z0
Y0
Joint 3
O3
Y1
X3
d2
Joint 1
O0 X0
Joint 2
O1 X1 O2 X2
Y2
a0
a1
i : rotation angle from Zi-1 to Zi about Xi
ai : distance from intersection of Zi-1 & Xi
D-H Link Parameter Table
Joint i
i
ai
di
i
1
0
a0
0
0
2
-90
a1
0
1
3
0
0
d2
2
to origin of i coordinate along Xi
di
: distance from origin of (i-1) coordinate to intersection of Zi-1 & Xi along Zi-1
i
: rotation angle from Xi-1 to Xi about Zi-1
The City College of New York
22
Example II: PUMA 260
1
2
Z1
3
O1
1.
Number the joints
2.
Establish base frame
3.
Establish joint axis Zi
4.
Locate origin, (intersect.
of Zi & Zi-1) OR (intersect
of common normal & Zi )
X1
Z2 Z6
5.
Y1
O
Y3 2
Z5
Z4
Y6
O3
X
6
Y2 2
5
O
6
Z0
Y5
Y
X3 4
t
O5
X5 X6
O4 Z 3
X4
Establish Xi,Yi
X i (Zi 1 Zi ) / Zi 1 Zi
Yi (Zi X i ) / Zi X i
4
PUMA 260
The City College of New York
23
Link Parameters
1
J
2
1
Z1
2
3
O1
3
i
1
2
3
4
5
6
i
ai d i
-90 0
13
0
8
0
90
0
-l
8
4
-90 0
X1
Z2 Z6
Y1
5
90 0 0
Y3O2
Z5
Z4 6
Y6
0
0 t
O3
X
6
2
Y2
5 : angle from X to X
O
i-1
i
6
Z0
i
Y5
about Zi-1
X 3 Y4
O5
X5 X6
i : angle from Zi-1 to Zi
O4 Z 3
about X
i
X4
Joint distance
4 ai : distance from intersection
of Zi-1 & Xi to Oi along Xi
di : distance from Oi-1 to intersection of Zi-1 & Xi along Zi-1
The City College of New York
24
Transformation between i-1 and i
• Four successive elementary transformations
are required to relate the i-th coordinate frame
to the (i-1)-th coordinate frame:
– Rotate about the Z i-1 axis an angle of i to align the
X i-1 axis with the X i axis.
– Translate along the Z i-1 axis a distance of di, to bring
Xi-1 and Xi axes into coincidence.
– Translate along the Xi axis a distance of ai to bring
the two origins Oi-1 and Oi as well as the X axis into
coincidence.
– Rotate about the Xi axis an angle of αi ( in the righthanded sense), to bring the two coordinates into
coincidence.
The City College of New York
25
Transformation between i-1 and i
• D-H transformation matrix for adjacent coordinate
frames, i and i-1.
– The position and orientation of the i-th frame coordinate
can be expressed in the (i-1)th frame by the following
homogeneous transformation matrix:
Source coordinate
Ti i 1 T ( zi 1 , d i ) R ( zi 1 , i )T ( xi , ai ) R ( xi , i )
Reference
Coordinate
C i
S
i
0
0
C i S i
S i S i
C i C i
S i C i
S i
C i
0
0
The City College of New York
ai C i
ai S i
di
1
26
Kinematic Equations
q (q1 , q2 , qn )
• Forward Kinematics
– Given joint variables
– End-effector position & orientation Y ( x, y, z, , , )
n
• Homogeneous matrix T0
– specifies the location of the ith coordinate frame w.r.t.
the base coordinate system
– chain product of successive coordinate transformation
matrices of Ti i 1
T T T T
n
0
Orientation
matrix
R0n
0
1 2
0 1
n
n 1
Position
vector
P0n n s a P0n
1 0 0 0 1
The City College of New York
27
Kinematics Equations
• Other representations
– reference from, tool frame
0
Treftool Bref
T0n H ntool
– Yaw-Pitch-Roll representation for orientation
T Rz , R y , Rx ,
C
S
0
0
S
C
0
0
0 0 C
0 0 0
1 0 S
0 1 0
0
S
1
0
0 C
0
0
The City College of New York
0
0
0
1
1 0
0 C
0 S
0 0
0
S
C
0
0
0
0
1
28
Representing forward kinematics
• Forward kinematics
1
2
3
4
5
6
px
p
y
pz
• Transformation Matrix
nx
n
T y
nz
0
The City College of New York
sx
ax
sy
ay
sz
az
0
0
px
p y
pz
1
29
Representing forward kinematics
• Yaw-Pitch-Roll representation for orientation
CC CSS SC
SC SSS CC
T0n
S
CS
0
0
nx
n
T0n y
nz
0
Problem?
sx
ax
sy
ay
sz
az
0
0
px
p y
pz
1
CSC SS
SSC CS
CC
0
sin 1 (nz )
px
p y
pz
1
az
cos (
)
cos
nx
1
cos (
)
cos
1
Solution is inconsistent and ill-conditioned!!
The City College of New York
30
atan2(y,x)
y
x
0 90
for x and y
for x and y
90 180
a tan 2( y, x)
for x and y
180 90
90 0
for x and y
The City College of New York
31
Yaw-Pitch-Roll Representation
T Rz , R y , Rx ,
C
S
0
0
S
nx
n
y
nz
0
sx
ax
sy
ay
sz
az
0
0
C
0
0
0 0
0 0
1 0
0 1
C
0
S
0
0
S
1
0
0 C
0
0
0
0
0
1
1 0
0 C
0 S
0 0
0
S
C
0
0
0
0
1
0
0
0
1
The City College of New York
32
Yaw-Pitch-Roll Representation
1
z ,
R T Ry , Rx,
C
S
0
0
C
0
S
0
S
C
0
0
0 0 n x
0 0 n y
1 0 n z
0 1 0
0
S
1
0
0 C
0
0
0
0
0
1
sx
ax
sy
ay
sz
az
0
0
1 0
0 C
0 S
0 0
0
S
C
0
The City College of New York
0
0
0
1
(Equation A)
0
0
0
1
33
Yaw-Pitch-Roll Representation
• Compare LHS and RHS of Equation A, we have:
sin nx cos n y 0
a tan 2(n y , nx )
cos nx sin n y cos
nz sin
a tan 2(nz , cos nz sin n y )
sin sx cos s y cos
sin ax cos a y sin
a tan 2(sin a x cos a y , sin s x cos s y )
The City College of New York
34
Kinematic Model
• Steps to derive kinematics model:
– Assign D-H coordinates frames
– Find link parameters
– Transformation matrices of adjacent joints
– Calculate Kinematics Matrix
– When necessary, Euler angle representation
The City College of New York
35
Example
Z3
Z1
Z0
Y0
Joint 3
O3
Y1
X3
d2
Joint 1
O0 X0
Joint 2
O1 X1 O2 X2
Y2
a0
a1
Joint i
i
ai
di
i
1
0
a0
0
0
2
-90
a1
0
1
3
0
0
d2
2
The City College of New York
36
Example
Joint i
i
ai
di
i
1
0
a0
0
0
i
i 1
T
2
-90
a1
0
1
3
0
0
d2
2
C i
S
i
0
0
C i S i
S i S i
C i C i
S i C i
S i
C i
0
0
T03 (T 01)(T 21)(T 23)
ai C i
ai S i
di
1
cosθ 0
1 sinθ 0
T 0
0
0
cosθ1
2 sinθ1
T 1
0
0
cosθ 2
3 sin 2
T 2
0
0
The City College of New York
0 a0 cos 0
0 a0 sin 0
1
0
0
1
sinθ 0
cosθ 0
0
0
0
sin 1
0
cos 1
1
0
0
0
sinθ 2
cos 2
0
0
a1 cos 1
a1 sin 1
0
1
0
0 0
1 d2
0 1
0
37
Example: Puma 560
The City College of New York
38
Example: Puma 560
The City College of New York
39
Link Coordinate Parameters
PUMA 560 robot arm link coordinate parameters
Joint i
i
i
1
1
-90
2
2
0
431.8 149.09
3
3
90
-20.32
0
4
5
4
5
-90
90
0
0
433.07
0
6
6
0
0
56.25
ai(mm) di(mm)
0
The City College of New York
0
40
Example: Puma 560
The City College of New York
41
Example: Puma 560
The City College of New York
42
Inverse Kinematics
• Given a desired position (P)
& orientation (R) of the endeffector
z
q (q1 , q2 , qn )
• Find the joint variables
which can bring the robot
the desired configuration
The City College of New York
y
x
43
Inverse Kinematics
• More difficult
– Systematic closed-form
solution in general is not
available
– Solution not unique
(x , y)
• Redundant robot
• Elbow-up/elbow-down
configuration
– Robot dependent
The City College of New York
44
Inverse Kinematics
• Transformation Matrix
nx
n
T y
nz
0
sx
ax
sy
ay
sz
az
0
0
px
p y
T01T12T23T34T45T56
pz
1
1
2
3
4
5
6
Special cases make the closed-form arm solution possible:
1.
Three adjacent joint axes intersecting (PUMA, Stanford)
2.
Three adjacent joint axes parallel to one another (MINIMOVER)
The City College of New York
45
Thank you!
Homework 2 posted on the web.
Due: Sept. 23, 2008
Next class: Inverse Kinematics, Jocobian
Matrix, Trajectory planningz
z
z
y
y
x
z
x
y
x
y
x
The City College of New York
46