city college of new york 1 jizhong xiao department of electrical engineering city college of new...
Post on 21-Dec-2015
216 views
TRANSCRIPT
1City College of New York
Jizhong Xiao
Department of Electrical Engineering
City College of New York
Robot Kinematics II
Introduction to ROBOTICS
2City College of New York
Outline
• Review– Manipulator Specifications
• Precision, Repeatability
– Homogeneous Matrix
• Denavit-Hartenberg (D-H) Representation
• Kinematics Equations• Inverse Kinematics
3City College of New York
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 – Precision– Repeatability
How accurately a specified point can be reached
How accurately the same position can be reached if the motion is repeated many times
4City College of New York
Review• Manipulators:
Cartesian: PPP Cylindrical: RPP Spherical: RRP
SCARA: RRP
(Selective Compliance Assembly Robot Arm)
Articulated: RRR
Hand coordinate:
n: normal vector; s: sliding vector;
a: approach vector, normal to the
tool mounting plate
5City College of New York
Review• Basic Rotation Matrix
uvwxyz RPP
xyzuvw QPP
TRRQ 1
uvw
w
v
u
z
y
x
xyz RP
p
p
p
p
p
p
P
wzvzuz
wyvyuy
wxvxux
kkjkik
kjjjij
kijiii
x
z
y
v
wP
u
6City College of New York
Basic Rotation Matrices– Rotation about x-axis with
– Rotation about y-axis with
– Rotation about z-axis with
uvwxyz RPP
CS
SCxRot
0
0
001
),(
0
010
0
),(
CS
SC
yRot
100
0
0
),(
CS
SC
zRot
7City College of New York
Review• Coordinate transformation from {B} to {A}
• Homogeneous transformation matrix
'oAPBB
APA rrRr
1101 31
' PBoAB
APA rrRr
10101333
31
' PRrRT
oAB
A
BA
Position vector
Rotation matrix
Scaling
8City College of New York
Review• Homogeneous Transformation
– Special cases
1. Translation
2. Rotation
10
0
31
13BA
BA RT
10 31
'33
oA
BA rIT
9City College of New York
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 post-multiplication
10City College of New York
Review• Homogeneous Representation
– A point in space
– A frame in space
3R
1000
1000 zzzz
yyyy
xxxx
pasn
pasn
pasn
PasnF
x
y
z ),,( zyx pppP
n
sa
1z
y
x
p
p
p
P Homogeneous coordinate of P w.r.t. OXYZ
3R
11City College of New York
Review
• Orientation Representation (Euler Angles) – Description of Roll-Pitch-Yaw
• A rotation of about the OX axis ( ) -- yaw
• A rotation of about the OY axis ( ) -- pitch
• A rotation of about the OZ axis ( ) -- roll
X
Y
Z
,xR
,yR
,zR
12City College of New York
Quiz 1• How to get the resultant rotation matrix for YPR?
X
Y
Z
,,, xyz RRRT
1000
0100
00
00
CS
SC
1000
00
0010
00
CS
SC
1000
00
00
0001
CS
SC
13City College of New York
Quiz 2• Geometric Interpretation?
•
101333 PR
T
101 PRR
TTT
441
10
0
1010
I
RRPRPRRTT
TTT
Position of the origin of OUVW coordinate frame w.r.t. OXYZ frame
Inverse of the rotation submatrix is equivalent to its transpose
Position of the origin of OXYZ reference frame w.r.t. OUVW frame
Orientation of OUVW coordinate frame w.r.t. OXYZ frame
Inverse Homogeneous Matrix?
14City College of New York
Kinematics Model• Forward (direct) Kinematics
• Inverse Kinematics
),,( 21 nqqqq
),,,,,( zyxY
x
y
z
Direct K inem atics
Inverse K inem atics
Position and O rientationof the end-effector
Jointvariables
16City College of New York
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 at the supporting base with 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 or along the common normal between the Zi-1 & Zi axes when they are parallel.
• Establish Yi axis. Assign to complete the right-handed coordinate system.
• Find the link and joint parameters
),,( 000 ZYX
iiiii ZZZZX 11 /)(
iiiii XZXZY /)(
0Z
17City College of New York
Example• 3 Revolute Joints
a0 a1
Z0
X0
Y0
Z3
X2
Y1
X1
Y2
d2
Z1
X33O
2O1O0O
Z2
Joint 1
Joint 2
Joint 3
Link 1 Link 2
18City College of New York
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;
Link 1 Link 2
a0 a1
Z0
X0
Y0
Z3
X2
Y1
X1
Y2
d2
Z1
X33O
2O1O0O
Z2
Joint 1
Joint 2
Joint 3
19City College of New York
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 , (if Zi-1 is parallel to Zi, then Xi is specified arbitrarily, subject only to Xi being perpendicular to Zi);
iiiii ZZZZX 11 /)(
a0 a1
Z0
X0
Y0
Z3
X2
Y1
X1
Y2
d2
Z1
X33O
2O1O0O
Z2
Joint 1
Joint 2
Joint 3
20City College of New York
Link Coordinate Frames– Assign to complete the right-
handed coordinate system.• The hand coordinate frame is specified by the geometry
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.
iiiii XZXZY /)(
nO
a0 a1
Z0
X0
Y0
Z3
X2
Y1
X1
Y2
d2
Z1
X33O
2O1O0O
Z2
Joint 1
Joint 2
Joint 3
21City College of New York
Link and Joint Parameters• Joint angle : 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 : 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 : 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 : the angle of rotation from the Zi-1 axis to the Zi axis about the Xi axis.
i
id
ia
i
22City College of New York
Example
Joint i i ai di
i
1 0 a0 0 0
2 -90 a1 0 1
3 0 0 d2 2
D-H Link Parameter Table
: rotation angle from Xi-1 to Xi about Zi-1 i : distance from origin of (i-1) coordinate to intersection of Zi-1 & Xi along Zi-1
: distance from intersection of Zi-1 & Xi to origin of i coordinate along Xi
id
: rotation angle from Zi-1 to Zi about Xi
iai
a0 a1
Z0
X0
Y0
Z3
X2
Y1
X1
Y2
d2
Z1
X33O
2O1O0O
Z2
Joint 1
Joint 2
Joint 3
23City College of New York
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 right-handed sense), to bring the two coordinates into coincidence.
24City College of New York
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:
1000
0
),(),(),(),( 111
iii
iiiiiii
iiiiiii
iiiiiiiii
i
dCS
SaCSCCS
CaSSSCC
xRaxTzRdzTT
Source coordinate
ReferenceCoordinate
25City College of New York
Kinematic Equations • Forward Kinematics
– Given joint variables– End-effector position & orientation
• Homogeneous matrix – specifies the location of the ith coordinate frame w.r.t.
the base coordinate system– chain product of successive coordinate transformation
matrices of
100010000
12
11
00
nnn
nn
n
PasnPR
TTTT
),,( 21 nqqqq
),,,,,( zyxY
iiT 1
nT0
Orientation matrix
Position vector
26City College of New York
Kinematics Equations
• Other representations– reference from, tool frame
– Roll-Pitch-Yaw representation for orientation
tooln
nref
toolref HTBT 0
0
,,, xyz RRRT
1000
0100
00
00
CS
SC
1000
00
0010
00
CS
SC
1000
00
00
0001
CS
SC
27City College of New York
Solving forward kinematics
• Forward kinematics
z
y
x
p
p
p
6
5
4
3
2
1
1000zzzz
yyyy
xxxx
pasn
pasn
pasn
T
• Transformation Matrix
28City College of New York
Solving forward kinematics• Roll-Pitch-Yaw representation for orientation
1000
0z
y
x
n
pCCSCS
pSCCSSCCSSSCS
pSSCSCCSSSCCC
T
1000
0zzzz
yyyy
xxxx
n
pasn
pasn
pasn
T
)(sin 1zn
)cos
(cos 1
za
)cos
(cos 1
xn
Problem? Solution is inconsistent and ill-conditioned!!
29City College of New York
atan2(y,x)
x
y
yandxfor
yandxfor
yandxfor
yandxfor
xya
090
90180
18090
900
),(2tan
30City College of New York
Roll-Pitch-Yaw Representation ,,, xyz RRRT
1000
0100
00
00
CS
SC
1000
00
0010
00
CS
SC
1000
00
00
0001
CS
SC
1000
0
0
0
zzz
yyy
xxx
asn
asn
asn
31City College of New York
Roll-Pitch-Yaw Representation
,,1, xyz RRTR
1000
0100
00
00
CS
SC
1000
00
0010
00
CS
SC
1000
00
00
0001
CS
SC
1000
0
0
0
zzz
yyy
xxx
asn
asn
asn
(Equation A)
32City College of New York
Roll-Pitch-Yaw Representation
0cossin yx nn
sin
cossincos
z
yx
n
nn
sincossin
coscossin
yx
yx
aa
ss
• Compare LHS and RHS of Equation A, we have:
),(2tan xy nna
)sincos,(2tan yzz nnna
)cossin,cos(sin2tan yxyx ssaaa
33City College of New York
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
34City College of New York
Example
Joint i i ai di
i
1 0 a0 0 0
2 -90 a1 0 1
3 0 0 d2 2
a0 a1
Z0
X0
Y0
Z3
X2
Y1
X1
Y2
d2
Z1
X33O
2O1O0O
Z2
Joint 1
Joint 2
Joint 3
35City College of New York
ExampleJoint i
i ai di i
1 0 a0 0 0
2 -90 a1 0 1
3 0 0 d2 2
1000
0100
0cosθsinθ
0sinθcosθ
00
00
000
00
1 sin
cos
a
a
T
1000
000
sinθ
cosθ
1
1
1 1
sincos0
cossin0
111
111
2
a
a
T
))()(( 2103213
0 TTTT
1000
0sinθcosθ 22
22
223
100
00cossin
0
dT
1000
01iii
iiiiiii
iiiiiii
ii dCS
SaCSCCS
CaSSSCC
T
38City College of New York
Link Coordinate Parameters
Joint i i i ai(mm) di(mm)
1 1 -90 0 0
2 2 0 431.8 149.09
3 3 90 -20.32 0
4 4 -90 0 433.07
5 5 90 0 0
6 6 0 0 56.25
PUMA 560 robot arm link coordinate parameters
41City College of New York
Inverse Kinematics• Given a desired position (P)
& orientation (R) of the end-effector
• Find the joint variables which can bring the robot the desired configuration
),,( 21 nqqqq
x
y
z
42City College of New York
Inverse Kinematics
• More difficult– Systematic closed-form
solution in general is not available
– Solution not unique• Redundant robot• Elbow-up/elbow-down
configuration
– Robot dependent
(x , y)l2
l1
l2
l1
43City College of New York
Inverse Kinematics
6
5
4
3
2
1
65
54
43
32
21
10
1000
TTTTTTpasn
pasn
pasn
Tzzzz
yyyy
xxxx
• Transformation Matrix
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)