the lie group se(3) - penn engineeringmeam620/slides/kinematicsi.pdf · rigid body kinematics...
Post on 09-Mar-2018
222 Views
Preview:
TRANSCRIPT
Rigid Body Kinematics
University of Pennsylvania 1
The Lie group SE(3)
( )⎭⎬⎫
⎩⎨⎧
===∈∈⎥⎦
⎤⎢⎣
⎡== ×
×
1,,,,1
3 333
31
RIRRRRrR0
rRAA TTRRSE
http://www.seas.upenn.edu/~meam620/notes/RigidBodyMotion3.pdf
Rigid Body Kinematics
University of Pennsylvania 2
Homogeneous Transformation MatrixCoordinate transformation from {B} to {A}
Homogeneous transformation matrix
OAPBB
APA ′′ += rrRr
x
y
z
ArP
O
BrP
ArO’
z'
y'
x'
{A}
O'A
BP
⎥⎦
⎤⎢⎣
⎡⎥⎦
⎤⎢⎣
⎡=⎥
⎦
⎤⎢⎣
⎡
×
′
111 31
PBOAB
APA r0
rRr
⎥⎦
⎤⎢⎣
⎡=
× 131
'
0rRA
OAB
A
BA
.131
13 ⎥⎦
⎤⎢⎣
⎡=
×
×
00RA B
A
BA
.131
33 ⎥⎦
⎤⎢⎣
⎡=
×
′×
0rIA
OAB
A
Special cases
1.
2.
Rigid Body Kinematics
University of Pennsylvania 3
Homogeneous CoordinatesDescription of a point
(x, y, z)Description of a plane
(t, u, v, s)
Equation of a circlex2 + y2 + z2 = a2
Homogeneous coordinatesDescription of a point
(x, y, z, w)Equation of a plane
tx + uy + vz + sw = 0Equation of a sphere
x2 + y2 + z2 = a2 w2
tx + uy + vz + s = 0
(t, u, v)
Central ideasEquivalence classProjective space P3, and not Euclidean space R3
Mathematical and practical advantages
Rigid Body Kinematics
University of Pennsylvania 4
Example: TranslationTranslation along the z-axis through h
( )⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
=
1000100
00100001
,h
hzTrans
x
y
z
y'
z'⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
=
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
1'''
1000100
00100001
1zyx
hzyx
x
y
z
h
Transformation Displacement
h
Rigid Body Kinematics
University of Pennsylvania 5
Example: RotationRotation about the x-axis through θ
( )⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
θθθ−θ
=θ
10000cossin00sincos00001
,xRot
x
y
z
θ
y'z'
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
θθθ−θ
=
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
1'''
10000cossin00sincos00001
1zyx
zyx
x
y
zθ
Transformation Displacement
Rigid Body Kinematics
University of Pennsylvania 6
Example: RotationRotation about the y-axis through θ
y
z
x
θ
z'x'
( )⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
θθ−
θθ
=θ
10000cos0sin00100sin0cos
,yRot
Rotation about the z-axis through θ
( )⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡θθθ−θ
=θ
1000010000cossin00sincos
,zRot
y
z
x
θ
y'
x'
Rigid Body Kinematics
University of Pennsylvania 7
Mobile Robots
x
y
θ
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡θθθ−θ
=100
cossinsincos
yx
gWRW
R
WAR
Rigid Body Kinematics
University of Pennsylvania 8
Example: Displacement of PointsDisplace (7, 3, 2) through a sequence of:1. Rot(z, 90)2. Rot(y, 90) 3. Trans(4, -3, 7) in the frame F: (x, y, z):
Trans(4, -3, 7) Rot(y, 90) Rot(z, 90)
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡ −
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
−⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡−
1000010000010010
1000000100100100
100071003010
4001
P1
P2
(6, 4, 10, 1)P3
x
y
z
P0 (7, 3, 2, 1)
Rigid Body Kinematics
University of Pennsylvania 9
Example: Transformation of PointsSuccessive transformations of (7, 3, 2):1. Trans(4, -3, 7) 2. Rot(y, 90) 3. Rot(z, 90)in a body fixed frame
Trans(4, -3, 7) Rot(y, 90) Rot(z, 90)x'
y'
z'
x’’
y’’ (7, 3, 2, 1)
(6, 4, 10, 1)
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡ −
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
−⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡−
1000010000010010
1000000100100100
100071003010
4001
x
y
z
(7, 3, 2, 1)
Rigid Body Kinematics
University of Pennsylvania 10
SE(3) is a Lie groupSE(3) satisfies the four axioms that must be satisfied by the elements of an algebraic group:
The set is closed under the binary operation. In other words, if A and B are any two matrices in SE(3), AB ∈ SE(3). The binary operation is associative. In other words, if A, B, and C are any three matrices ∈ SE(3), then (AB) C = A (BC). For every element A ∈ SE(3), there is an identity element given by the 4×4 identity matrix, I∈ SE(3), such that AI = A.For every element A ∈ SE(3), there is an identity inverse, A-1 ∈ SE(3), such that A A-1 = I.
SE(3) is a continuous group.the binary operation above is a continuous operation ⎯ the product of any two elements in SE(3) is a continuous function of the two elementsthe inverse of any element in SE(3) is a continuous function of that element.
In other words, SE(3) is a differentiable manifold. A group that is a differentiable manifold is called a Lie group[ Sophus Lie (1842-1899)].
Rigid Body Kinematics
University of Pennsylvania 11
Composition of DisplacementsDisplacement from {A} to {B}
Displacement from {B} to {C}
Displacement from {A} to {C}x
y
z
O
z'
y'
x'
{A}
O'
z''
x''y''
O''
{B}
POSITION 1
POSITION 2
POSITION 3
{C}
,131
'
⎥⎦
⎤⎢⎣
⎡=
×0rRA
OAB
A
BA
,131
⎥⎦
⎤⎢⎣
⎡=
×
′′
0rRA
OBC
B
CB
⎥⎦
⎤⎢⎣
⎡ +××=
⎥⎦
⎤⎢⎣
⎡×⎥
⎦
⎤⎢⎣
⎡=
⎥⎦
⎤⎢⎣
⎡=
′′′
′′′
′′
1
11
1
0rrRRR
0rR
0rR
0rRA
OAOBB
AC
BB
A
OBC
BOAB
A
OAC
A
CA
Note XAY describes the displacement of the body-fixed frame from {X} to {Y} in reference frame {X}
Rigid Body Kinematics
University of Pennsylvania 12
Composition (continued)
x
y
z
O
z'
y'
x'
{A}
O'
z''
x''y''
O''
{B}
POSITION 1
POSITION 2
POSITION 3
{C}
Note XAY describes the displacement of the body-fixed frame from {X} to {Y} in reference frame {X}
Composition of displacementsDisplacements are generally described in a body-fixed frameExample: BAC is the displacement of a rigid body from B to C relative to theaxes of the “first frame” B.
Composition of transformationsSame basic ideaAAC = AAB
BAC
Note that our description of transformations (e.g., BAC) is relativeto the “first frame” (B, the frame with the leading superscript).
Rigid Body Kinematics
University of Pennsylvania 13
Transformations associated with the Lie group SE(3)
Rigid Body Kinematics
University of Pennsylvania 14
Differentiable Manifold
DefinitionA manifold of dimension n is a set M which is locally homeomorphic* to Rn.
R2
dim 2manifold
Homeomorphism:A map f from M to N and its inverse, f-1 are both continuous.
Smooth mapA map f from U ⊂ Rm to V ⊂ Rn is smooth if all partial derivatives of f, of all orders, exist and are continuous.
DiffeomorphismA smooth map f from U ⊂ Rn to V ⊂ Rn is a diffeomorphism if all partial derivatives of f-1, of all orders, exist and are continuous.
Rigid Body Kinematics
University of Pennsylvania 15
Examples
[ )
0 1
( ) 21)(,1,1:
xxxfRf
−=→−
P
Rigid Body Kinematics
University of Pennsylvania 16
Smooth Manifold
Differentiable manifold is locally homeomorphic to Rn
Parametrize the manifold using a set of local coordinate charts(U, φ ), (V, Ψ ), …
Require compatibility on overlapsC∞ related
Collection of charts covering M
Rn
dim nmanifold
φ
U V
Ψ
Rn
Rigid Body Kinematics
University of Pennsylvania 17
Actions of SE(3)M any smooth manifold
Think R3, SE(3), subgroups of SE(3)
A left action of SE(3) on M is a smooth map, Φ: SE(3) × M → M, such that
Φ(I, x) = x, x∈ MΦ(A, Φ(B, x) ) = Φ(AB, x), A, B ∈ SE(3), x ∈ M
Rigid Body Kinematics
University of Pennsylvania 18
Actions of SE(3)
1. Action of SE(3) on R3
Displacement of points, p → Ap
2. Action of SE(3) on itselfΦ: SE(3) × SE(3) → SE(3) ΦQ: A → Q A Q-1
A denotes a displacement of a rigid bodyΦQ transforms the displacement A
Actions on the Lie algebra (later)
Rigid Body Kinematics
University of Pennsylvania 19
What happens when you want to describe the displacement of the body-fixed frame from {A} to {B} in reference frame {F}?
Displacement is described in {A} by the homogeneous transform, AAB.
Want to describe the same displacement in {F}. The position and orientation of {A} relative to {F} is given by the homogeneous transform, FAA.
The same displacement which moves a body-fixed frame from {A} to {B}, will move another body-fixed frame from {F} to {G}:
FAG = FAA AAB
BAG
x
y
z
O
z'
y'
x'
{A} O'
{B}
POSITION 1
POSITION 2
{F}
{G}
X
Y
Z
X’
Y’
Z’
D
D
T
T
FAG = FAA AAB (FAA)-1
Rigid Body Kinematics
University of Pennsylvania 20
What happens when you want to describe the displacement of the body-fixed frame from {A} to {B} in reference frame {F}?
Displacement is described in {A} by the homogeneous transform, AAB.
Want to describe the same displacement in {F}. The position and orientation of {A} relative to {F} is given by the homogeneous transform, FAA.
The same displacement which moves a body-fixed frame from {A} to {B}, will move another body-fixed frame from {F} to {G}:
FAG = FAA AAB
BAG
x
y
z
O
z'
y'
x'
{A} O'
{B}
POSITION 1
POSITION 2
{F}
{G}
X
Y
Z
X’
Y’
Z’
D
D
T
T
FAG = FAA AAB (FAA)-1
Rigid Body Kinematics
University of Pennsylvania 21
What happens when you want to describe the displacement of the body-fixed frame from {A} to {B} in reference frame {F}?
Displacement is described in {A} by the homogeneous transform, AAB.
Want to describe the same displacement in {F}. The position and orientation of {A} relative to {F} is given by the homogeneous transform, FAA.
The same displacement which moves a body-fixed frame from {A} to {B}, will move another body-fixed frame from {F} to {G}:
FAG = FAA AAB
BAG
FAG = FAA AAB (FAA)-1
x
y
z
O
z'
y'
x'
{A} O'
{B}
POSITION 1
POSITION 2
{F}
{G}
X
Y
Z
X’
Y’
Z’
D
D
T
T
Rigid Body Kinematics
University of Pennsylvania 22
Rotational motion in R3
Specialize to SO(3)
Rigid Body Kinematics
University of Pennsylvania 23
Euler’s Theorem for Rotations
Any displacement of a rigid body such that a point on the rigid body, say O, remains fixed, is equivalent to a rotation about a fixed axis through the point O.
The most general rigid body displacement can be produced by a translation along a line followed (or preceded) by a rotation about that line.
Later: Chasles’ Theorem for Rotations
Rigid Body Kinematics
University of Pennsylvania 24
Proof of Euler’s Theorem for Spherical Displacements
Displacement from {F} to {M}
P = Rp
Solve the eigenvalue problem (find the vector that is unaffected by R):
Rp = λ p
φφ
v2Rv1
v1
Rv2
u
axis of rotation
eigenspace spanned by x and x
_
( )( )
( ) ( ) 0][
2112221131133311
23323322
33221123
=−+−+−λ−
++λ+λ−
RRRRRRRRRRRR
RRR( ) ( )( ) 0111 332211
2 =+−++λ−λ−λ RRR
0=λ− IR
xp
xp
up
==λ
==λ
==λ
φ−
φ
23
22
11
,
,
,1
i
i
e
e
Three eigenvalues and eigenvectors are:
( )121cos 332211 −++=φ RRR
where
Rigid Body Kinematics
University of Pennsylvania 25
The Axis/Angle for a Rotation Matrix
φφ
v2Rv1
v1
Rv2
u
axis of rotation
eigenspace spanned by x and x
_
( ) ( )
( ) ( )2121
21
21,
21
2,
21
vvxvvx
xxvxxv
ii
i
+=−=
−=+=
( ) ( )
( )φ+φ=
+=
λ+λ=+=
φ−φ
sincos21
21
21
21
321
vv
xx
xxxRRxRv
ii ee
( ) ( )
( )φ+φ−=
−=
λ−λ=−=
φ−φ
cossin2
22
21
322
vv
xx
xxxRRxRv
ii eei
ii
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡φφφ−φ
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡=
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
1000cossin0sincos
2121 uvvuvvR
Q ΛQ
Rigid Body Kinematics
University of Pennsylvania 26
R= Q Λ QT
Define
and look at the displacement in the new reference frame, {F’}.
φφ
v2Rv1
v1
Rv2
u
axis of rotation
eigenspace spanned by x and x
_
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡==′ uvvQR 21F
F
FAG = FAA AAB (FAA)-1
[ ]TFF
MF
FF
MF RRRR ′′
′′ =
QT QRΛ
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡φφφ−φ
=Λ1000cossin0sincos
Recall
Any rotation is a left action of SO(3) on a canonical (?) element, Λ.
top related