Upload
leliem
View
235
Download
0
Embed Size (px)
Citation preview
Introduction to Robotics (Fag 3480)Vår 2011
Robert Wood (Harward Engineeering and Applied Sciences-Basis)Ole Jakob Elle, PhD (Modified for IFI/UIO)
Førsteamanuensis II, Institutt for InformatikkUniversitetet i Oslo
Seksjonsleder Teknologi, Intervensjonssenteret, Oslo Universitetssykehus (Rikshospitalet)
Fag 3480 - Introduction to Roboticsside 2
Ch. 3: Forward and Inverse Kinematics
Fag 3480 - Introduction to Roboticsside 3
Industrial robotsHigh precision and repetitive tasks
Pick and place, painting, etc
Hazardous environments
Fag 3480 - Introduction to Roboticsside 4
Common configurations: elbow manipulatorAnthropomorphic arm: ABB IRB1400 or KUKA
Very similar to the lab arm NACHI (RRR)
Fag 3480 - Introduction to Roboticsside 5
Simple example: control of a 2DOF planar manipulator
Move from ‘home’ position and follow the path AB with a constant contact force F all using visual feedback
Fag 3480 - Introduction to Roboticsside 6
Coordinate frames & forward kinematicsThree coordinate frames:
Positions:
Orientation of the tool frame:0 1
2
( )( )
=
11
11
1
1
sincos
θθ
aa
yx
( ) ( )( ) ( ) ty
xaaaa
yx
≡
++++
=
21211
21211
2
2
sinsincoscos
θθθθθθ
0 1 2
+++−+
=
⋅⋅⋅⋅
=)cos()sin()sin()cos(
ˆˆˆˆˆˆˆˆ
2121
2121
0202
020202 θθθθ
θθθθyyyxxyxx
R
=
=
10ˆ
01ˆ 00 yx ,
++−
=
++
=)cos()sin(ˆ
)sin()cos(ˆ
21
212
21
212 θθ
θθθθθθ
yx ,
Fag 3480 - Introduction to Roboticsside 7
Ch. 2: Rigid Body Motions and Homogeneous Transforms
Fag 3480 - Introduction to Roboticsside 8
Rotation matrices as projections
Projecting the axes of from o1 onto the axes of frame o0
Alternate approach
⋅⋅
=
⋅⋅
=01
0101
01
0101 ˆˆ
ˆˆˆˆˆˆ
yyxy
yyxxx
x ,
−=
−
+
=
⋅⋅⋅⋅
=
θθθθ
θθπ
πθθ
cossinsincos
cosˆˆ2
cosˆˆ
2cosˆˆcosˆˆ
ˆˆˆˆˆˆˆˆ
0101
0101
0101
010101
yyyx
xyxx
yyyxxyxx
R
Fag 3480 - Introduction to Roboticsside 9
Properties of rotation matricesSummary:
Columns (rows) of R are mutually orthogonal
Each column (row) of R is a unit vector
The set of all n x n matrices that have these properties are called the Special Orthogonal group of order n
( ) 1det
1
== −
RRRT
( )nSOR∈
Fag 3480 - Introduction to Roboticsside 10
3D rotations
( )3ˆˆˆˆˆˆˆˆˆˆˆˆˆˆˆˆˆˆ
010101
010101
01010101 SO
zzzyzxyzyyyxxzxyxx
R ∈
⋅⋅⋅⋅⋅⋅⋅⋅⋅
=
General 3D rotation:
Special cases
Basic rotation matrices
−=
−=
θθ
θθ
θθθθ
θ
θ
cos0sin010
sin0cos
cossin0sincos0001
,
,
y
x
R
R
−=
1000cossin0sincos
, θθθθ
θzR
Fag 3480 - Introduction to Roboticsside 11
Properties of rotation matrices (cont’d)
SO(3) is a group under multiplication
Closure:
Identity:
Inverse:
Associativity:
In general, members of SO(3) do not commute
( ) ( )33 2121 SORRSORR ∈⇒∈ , if
( )3100010001
SOI ∈
=
1−= RRT
( ) ( )321321 RRRRRR = Allows us to combine rotations:bcabac RRR =
1221 RRRR ≠
Fag 3480 - Introduction to Roboticsside 12
Rotating a vectorAnother interpretation of a rotation matrix:
Rotating a vector about an axis in a fixed frame
Ex: rotate v0 about y0 by π/2
=
110
0v
=
−=
−=
=
=
011
110
001010100
110
cos0sin010
sin0cos
2/
02/,
1
πθ
π
θθ
θθ
vRv y
Fag 3480 - Introduction to Roboticsside 13
Rotation matrix summaryThree interpretations for the role of rotation matrix:
Representing the coordinates of a point in two different frames
Orientation of a transformed coordinate frame with respect to a fixed frame
Rotating vectors in the same coordinate frame
Fag 3480 - Introduction to Roboticsside 14
Compositions of rotationsw/ respect to the current frame
Ex: three frames o0, o1, o2
This defines the composition law for successive rotations about the currentreference frame: post-multiplication
202
0
212
1
101
0
pRp
pRp
pRp
=
=
=21
201
0 pRRp = 12
01
02 RRR =
Fag 3480 - Introduction to Roboticsside 15
Compositions of rotationsEx: R represents rotation about the current y-axis by φ followed by θ about
the current z-axis
−
−=
−
−=
=
φθφθφθθ
φθφθφθθθθ
φφ
φφθφ
cossinsincossin0cossin
sinsincoscoscos
1000cossin0sincos
cos0sin010
sin0cos,, zy RRR
Fag 3480 - Introduction to Roboticsside 16
Compositions of rotationsw/ respect to a fixed reference frame (o0)
Let the rotation between two frames o0 and o1 be defined by R10
Let R be a desired rotation w/ respect to the fixed frame o0
Using the definition of a similarity transform, we have:
This defines the composition law for successive rotations about a fixed reference frame: pre-multiplication
( )[ ] 01
01
101
01
02 RRRRRRR ==
−
Fag 3480 - Introduction to Roboticsside 17
Compositions of rotationsEx: we want a rotation matrix R that is a composition of φ about y0 (Ry,φ) and then θ about z0
(Rz,θ)
the second rotation needs to be projected back to the initial fixed frame
Now the combination of the two rotations is:
( )θθθ
θθθ
,,,
,,1
,02
yzy
yzy
RRRRRRR
−
−
=
=
[ ] φθφθφφ ,,,,,, yzyzyy RRRRRRR == −
Fag 3480 - Introduction to Roboticsside 18
Compositions of rotationsSummary:
Consecutive rotations w/ respect to the current reference frame:
Post-multiplying by successive rotation matrices
w/ respect to a fixed reference frame (o0)
Pre-multiplying by successive rotation matrices
We can also have hybrid compositions of rotations with respect to the current and a fixed frame using these same rules
Fag 3480 - Introduction to Roboticsside 19
Parameterizing rotationsThere are three parameters that need to be specified to create
arbitrary rigid body rotations
We will describe three such parameterizations:
Euler angles
Roll, Pitch, Yaw angles
Axis/Angle
Fag 3480 - Introduction to Roboticsside 20
Parameterizing rotationsEuler angles
Rotation by φ about the z-axis, followed by θ about the current y-axis, then ψ about the current z-axis
−+−+−−−
=
−
−
−==
θψθψθ
θφψφψθφψφψθφ
θφψφψθφψφψθφ
ψψ
ψψ
θθ
θθ
φφ
φφ
ψθφ
csscsssccscsscccssccssccssccc
cssc
cs
sccssc
RRRR zyzZYZ
10000
0010
0
10000
,,,
Fag 3480 - Introduction to Roboticsside 21
Parameterizing rotationsRoll, Pitch, Yaw angles
Three consecutive rotations about the fixed principal axes:
Yaw (x0) ψ, pitch (y0) θ, roll (z0) φ
−+−+++−
=
−
−
−=
=
ψθψθθ
ψθφψφψθφψφθφ
ψθφψφψθφψφθφ
ψψ
ψψ
θθ
θθ
φφ
φφ
ψθφ
ccscscssscssscccs
cscssssccscc
cssc
cs
sccssc
RRRR xyzXYZ
00
001
0010
0
10000
,,,
Fag 3480 - Introduction to Roboticsside 22
Parameterizing rotationsAxis/Angle representation
Any rotation matrix in SO(3) can be represented as a single rotation about a suitable axis through a set angle
For example, assume that we have a unit vector:
Given θ, we want to derive Rk,θ:
Intermediate step: project the z-axis onto k:
Where the rotation R is given by:
=
z
y
x
kkk
k̂
1,,
−= RRRR zk θθ
αβθβαθ
βα
−−=⇒
=
,,,,,,
,,
zyzyzk
yz
RRRRRRRRR
Fag 3480 - Introduction to Roboticsside 23
Parameterizing rotationsAxis/Angle representation
This is given by:
Inverse problem:
Given arbitrary R, find k and θ
++−−+++−+
=
θθθθθθ
θθθθθθ
θθθθθθ
θ
cvkskvkkskvkkskvkkcvkskvkkskvkkskvkkcvk
R
zxzyyzx
xzyyzyx
yzxzyxx
k2
2
2
,
( )
−−−
=
−
= −
1221
3113
2332
1
sin21ˆ
21cos
rrrrrr
k
RTr
θ
θ
Fag 3480 - Introduction to Roboticsside 24
Rigid motionsRigid motion is a combination of rotation and translation
Defined by a rotation matrix (R) and a displacement vector (d)
the group of all rigid motions (d,R) is known as the Special Euclidean group, SE(3)
Consider three frames, o0, o1, and o2 and corresponding rotation matrices R21, and
R10
Let d21 be the vector from the origin o1 to o2, d1
0 from o0 to o1
For a point p2 attached to o2, we can represent this vector in frames o0 and o1:
( )3
3R∈
∈
dSOR
( ) ( )33 3 SOSE ×= R
( )01
12
01
212
01
01
12
212
01
01
101
0
12
212
1
ddRpRR
ddpRR
dpRp
dpRp
++=
++=
+=
+=
Fag 3480 - Introduction to Roboticsside 25
Homogeneous transformsWe can represent rigid motions (rotations and translations) as matrix
multiplication
Define:
Now the point p2 can be represented in frame o0:
Where the P0 and P2 are:
=
=
10
1012
121
2
01
010
1
dRH
dRH
212
01
0 PHHP =
=
=
1 ,
1
22
00 p
Pp
P
Fag 3480 - Introduction to Roboticsside 26
Homogeneous transformsThe matrix multiplication H is known as a homogeneous
transform and we note that
Inverse transforms:
( )3SEH ∈
−=−
101 dRRH
TT
Fag 3480 - Introduction to Roboticsside 27
Homogeneous transformsBasic transforms:
Three pure translation, three pure rotation
=
=
=
1000100
00100001
10000100
0100001
100001000010
001
,
,
,
c
b
a
cz
by
ax
Trans
Trans
Trans
−
=
−=
−
=
100001000000
100000001000
100000000001
,
,
,
γγ
γγ
γ
ββ
ββ
β
αα
ααα
cssc
cs
sc
cssc
z
y
x
Rot
Rot
Rot
Fag 3480 - Introduction to Roboticsside 28
Ch. 3: Forward and Inverse Kinematics
Fag 3480 - Introduction to Roboticsside 29
Recap: rigid motionsRigid motion is a combination of rotation and translation
Defined by a rotation matrix (R) and a displacement vector (d)
the group of all rigid motions (d,R) is known as the Special Euclidean group, SE(3)
We can represent rigid motions (rotations and translations) as matrix multiplication
The matrix multiplication H is known as a homogeneous transform and we note that
Inverse transforms:
−=−
101 dRRH
TT
=
10dR
H
Fag 3480 - Introduction to Roboticsside 30
Recap: homogeneous transforms
Basic transforms:
Three pure translation, three pure rotation
=
=
=
1000100
00100001
10000100
0100001
100001000010
001
,
,
,
c
b
a
cz
by
ax
Trans
Trans
Trans
−
=
−=
−
=
100001000000
100000001000
100000000001
,
,
,
γγ
γγ
γ
ββ
ββ
β
αα
ααα
cssc
cs
sc
cssc
z
y
x
Rot
Rot
Rot
Fag 3480 - Introduction to Roboticsside 31
ExampleEuler angles: we have only discussed ZYZ Euler
angles. What is the set of all possible Euler angles that can be used to represent any rotation matrix?
Fag 3480 - Introduction to Roboticsside 32
Answer - Euler
XYZ, YZX, ZXY, XYX, YZY, ZXZ, XZY, YXZ, ZYX, XZX, YXY, ZYZ
ZZY cannot be used to describe any arbitrary rotation matrix since two consecutive rotations about the Z axis can be composed into one rotation
Fag 3480 - Introduction to Roboticsside 33
ExampleCompute the homogeneous transformation
representing a translation of 3 units along the x-axis followed by a rotation of π/2 about the current z-axis followed by a translation of 1 unit along the fixed y-axis
Fag 3480 - Introduction to Roboticsside 34
Answer – Homogeneous Transforms
−
=
−
=
=
1000010010013010
1000010000010010
1000010000103001
1000010010100001
2/,3,1, πzxy TTTT
Fag 3480 - Introduction to Roboticsside 35
Forward kinematics introductionChallenge: given all the joint parameters of a manipulator, determine the position
and orientation of the tool frame
Tool frame: coordinate frame attached to the most distal link of the manipulator
Inertial (base) frame: fixed (immobile) coordinate system fixed to the most proximal link of a manipulator
Therefore, we want a mapping between the tool frame and the inertial frame
This will be a function of all joint parameters and the physical geometry of the manipulator
Purely geometric: we do not worry about joint torques or dynamics
(yet!)
Fag 3480 - Introduction to Roboticsside 36
Convention
A n-DOF manipulator will have n joints (either revolute or prismatic) and n+1 links (since each joint connects two links)
We assume that each joint only has one DOF. Although this may seem like it does not include things like spherical or universal joints, we can think of multi-DOF joints as a combination of 1DOF joints with zero length between them
The o0 frame is the inertial frame (or base frame)
on is the tool frame
Joint i connects links i-1 and i
The oi is connected to link i
Joint variables, qi
=prismatic is joint ifrevolute is joint if
idi
qi
ii
θ
Fag 3480 - Introduction to Roboticsside 37
Convention
We said that a homogeneous transformation allowed us to express the position and orientation of oj with respect to oi
what we want is the position and orientation of the tool frame with respect to the inertial frame
An intermediate step is to determine the transformation matrix that gives position and orientation of oi with respect to oi-1: Ai
Now we can define the transformation oj to oi as:
( ) ijjiji
TI
AAAAT
ji
jijiiij
>=<
=
−
−++
if if if
1
21 ...
Fag 3480 - Introduction to Roboticsside 38
Convention
Finally, the position and orientation of the tool frame with respect to the inertial frame is given by one homogeneous transformation matrix:
For a n-DOF manipulator
Thus, to fully define the forward kinematics for any serial manipulator, all we need to do is create the Ai transformations and perform matrix multiplication
But there are shortcuts…
( ) ( ) ( )nnnnn qAqAqAToRH ⋅⋅⋅==
= 2211
000
10
Fag 3480 - Introduction to Roboticsside 39
The Denavit-Hartenberg (DH) Convention
Representing each individual homogeneous transformation as the product of four basic transformations:
−
−
=
−
−
=
=
10000
100000000001
100001000010
001
1000100
00100001
100001000000
,,,,
i
i
i
i
i
xaxdzzi
dcssascccscasscsc
cssc
a
dcssc
A
ii
iiiiii
iiiiii
ii
iiii
ii
iiii
αα
θαθαθθ
θαθαθθ
αα
ααθθ
θθ
αθ RotTransTransRot
Fag 3480 - Introduction to Roboticsside 40
The Denavit-Hartenberg (DH) Convention
Four DH parameters:
ai: link length
αi: link twist
di: link offset
θi: joint angle
Since each Ai is a function of only one variable, three of these will be constant for each link
di will be variable for prismatic joints and θi will be variable for revolute joints
But we said any rigid body needs 6 parameters to describe its position and orientation
Three angles (Euler angles, for example) and a 3x1 position vector
So how can there be just 4 DH parameters?...
Fag 3480 - Introduction to Roboticsside 41
Existence and uniquenessWhen can we represent a homogeneous transformation using the 4 DH parameters?
For example, consider two coordinate frames o0 and o1
There is a unique homogeneous transformation between these two frames
Now assume that the following holds:
DH1: perpendicular ->
DH2: intersects ->
If these hold, we claim that there
exists a unique transformation A:
01 ˆˆ zx ⊥
01 ˆˆ zx ∩
=
=
10
01
01
,,,,
oR
A xaxdzz αθ RotTransTransRot
Fag 3480 - Introduction to Roboticsside 42
Existence and uniquenessProof:
We assume that R10 has the form:
Use DH1 to verify the form of R10
Since the rows and columns of R10 must be unit vectors:
The remainder of R10 follows from the properties of rotation matrices
Therefore our assumption that there exists a unique θ and α that will give us R10
is correct given DH1
0100
0ˆˆ
31
31
21
11
00
0101
==
⇒
=⋅⇒⊥
rrrr
zxzxT
αθ ,,01 xz RRR =
=
3332
232221
13121101
0 rrrrrrrr
R
1
12
332
32
221
211
=+
=+
rr
rr
Fag 3480 - Introduction to Roboticsside 43
Existence and uniquenessProof:
Use DH2 to determine the form of o10
Since the two axes intersect, we can represent the line between the two frames as a linear combination of the two axes (within the plane formed by x1 and z0)
=
+
=⇒
+=⇒∩
dasac
sc
ado
axdzozx
θ
θ
θ
θ
0100
ˆˆ
01
01
00
0101
Fag 3480 - Introduction to Roboticsside 44
Physical basis for DH parameters
ai: link length, distance between the z0 and z1 (along x1)
αi: link twist, angle between z0 and z1 (measured around x1)
di: link offset, distance between o0 and intersection of z0 and x1 (along z0)
θi: joint angle, angle between x0 and x1 (measured around z0)
positive convention:
Fag 3480 - Introduction to Roboticsside 45
Assigning coordinate framesFor any n-link manipulator, we can always choose coordinate frames such that DH1 and DH2
are satisfied
The choice is not unique, but the end result will always be the same
Choose zi as axis of rotation for joint i+1
z0 is axis of rotation for joint 1, z1 is axis of rotation for joint 2, etc
If joint i+1 is revolute, zi is the axis of rotation of joint i+1
If joint i+1 is prismatic, zi is the axis of translation for joint i+1
Fag 3480 - Introduction to Roboticsside 46
Assigning coordinate framesAssign base frame
Can be any point along z0
Chose x0, y0 to follow the right-handed convention
Now start an iterative process to define frame i with respect to frame i-1
Consider three cases for the relationship of zi-1 and zi:
zi-1 and zi are non-coplanar
zi-1 and zi intersect
zi-1 and zi are parallelzi-1 and zi are coplanar
Fag 3480 - Introduction to Roboticsside 47
Assigning coordinate frameszi-1 and zi are non-coplanar
There is a unique shortest distance between the two axes
Choose this line segment to be xi
oi is at the intersection of zi and xi
Choose yi by right-handed convention
Fag 3480 - Introduction to Roboticsside 48
Assigning coordinate frames
zi-1 and zi intersect
Choose xi to be normal to the plane defined by ziand zi-1
oi is at the intersection of zi and xi
Choose yi by right-handed convention
Fag 3480 - Introduction to Roboticsside 49
Assigning coordinate frameszi-1 and zi are parallel
Infinitely many normals of equal length between ziand zi-1
Free to choose oi anywhere along zi, however if we choose xi to be along the normal that intersects at oi-1, the resulting di will be zero
Choose yi by right-handed convention
Fag 3480 - Introduction to Roboticsside 50
Assigning tool frameThe previous assignments are valid up to frame n-1
The tool frame assignment is most often defined by the axes n, s, a:
a is the approach direction
s is the ‘sliding’ direction (direction along which the grippers open/close)
n is the normal direction to a and s
Fag 3480 - Introduction to Roboticsside 51
Example 1: two-link planar manipulator
2DOF: need to assign three coordinate frames
Choose z0 axis (axis of rotation for joint 1, base frame)
Choose z1 axis (axis of rotation for joint 2)
Choose z2 axis (tool frame)
This is arbitrary for this case since we have described no wrist/gripper
Instead, define z2 as parallel to z1 and z0 (for consistency)
Choose xi axes
All zi’s are parallel
Therefore choose xi to intersect oi-1
Fag 3480 - Introduction to Roboticsside 52
Example 1: two-link planar manipulator
Now define DH parameters
First, define the constant parameters ai, αi
Second, define the variable parameters θi, di
The αi terms are 0 because all zi are parallel
Therefore only θi are variable
link ai αi di θi
1 a1 0 0 θ1
2 a2 0 0 θ2
−
=
−
=
10000100
00
10000100
00
2222
2222
21111
1111
1sacscasc
Asacscasc
A ,
++−
==
=
10000100
00
122111212
122111212
210
2
10
1
sasacscacasc
AAT
AT
Fag 3480 - Introduction to Roboticsside 53
Example 2: three-link cylindrical robot
3DOF: need to assign four coordinate frames
Choose z0 axis (axis of rotation for joint 1, base frame)
Choose z1 axis (axis of translation for joint 2)
Choose z2 axis (axis of translation for joint 3)
Choose z3 axis (tool frame)
This is again arbitrary for this case since we have described no wrist/gripper
Instead, define z3 as parallel to z2
Fag 3480 - Introduction to Roboticsside 54
Example 2: three-link cylindrical robot
Now define DH parameters
First, define the constant parameters ai, αi
Second, define the variable parameters θi, di
link ai αi di θi
1 0 0 d1 θ1
2 0 -90 d2 0
3 0 0 d3 0
=
−=
−
=
1000100
00100001
1000010
01000001
1000100
0000
33
22
1
11
11
1 dA
dA
dcssc
A , ,
+−
−−
==
1000010
00
21
3111
3111
3210
3 dddccsdssc
AAAT
Fag 3480 - Introduction to Roboticsside 55
Example 3: spherical wrist3DOF: need to assign four coordinate frames
yaw, pitch, roll (θ4, θ5, θ6) all intersecting at one point o (wrist center)
Fag 3480 - Introduction to Roboticsside 56
Example 3: spherical wristlink ai αi di θi
4 0 -90 0 θ4
5 0 90 0 θ5
6 0 0 d6 θ6
Now define DH parameters
First, define the constant parameters ai, αi
Second, define the variable parameters θi, di
−
=
−
−
=
−
−
=
1000100
0000
100000100000
100000100000
6
66
66
355
55
244
44
1 dcssc
Acssc
Acssc
A , ,
−+−+−−−
==
10006556565
654546465464654
654546465464654
6543
6 dcccscsdssssccscsscccsdscsccssccssccc
AAAT
Fag 3480 - Introduction to Roboticsside 57
Next class…More examples for common configurations
Link to movie that explains how to set-up the Denavit-Hartenberg parameters :
http://en.wikipedia.org/wiki/File:Denavit-Hartenberg_Tutorial_Video.ogv#file