Konkurenční výhody použití optických mechanických spojek FIBRLOK
Kinematika = studium pohybu mechanických těles bez uvážení...
Transcript of Kinematika = studium pohybu mechanických těles bez uvážení...
Kinematika = studium pohybu mechanických těles
bez uvážení sil
Kinematické řetězce
Kinematické dvojice
Kinematické řetězce
Kinematické dvojice
Kinematické řetězce
Kinematické dvojice
Kinematické řetězce
Kinematické dvojice
Kinematické řetězce
Illustation of a 3R manipulator
Kinematické řetězce
Illustration of a spherical wrist kinematics
Stanford arm
Stanford arm
Pohyb tělesa
Geometrické vektory
Aritmetické vektory
11
1
1
1
00
0
0
0
z
y
x
z
y
x
q
q
q
q
q
q
Posuvný pohyb
000 rdp
Posuvný pohyb
(rychlosti)
000 rdp
Rotační pohyb
Rotační pohyb
1z11y11x10z00y00x0 kjikji pppppp
1z11y11x1z0
1z11y11x10z00y00x0
1z11y11x1y0
1z11y11x10z00y00x0
1z11y11x1x0
1z11y11x10z00y00x0
kkjkik
kjikkjik
kjjjij
kjijkjij
kijiii
kjiikjii
000
00
000
00
000
00
)()(
)()(
)()(
pppp
pppppp
pppp
pppppp
pppp
pppppp
Rotační pohyb
1
1
1
010101
010101
010101
0
0
0
z
y
x
z
y
x
p
p
p
p
p
p
kkkjki
jkjjji
ikijii
1010 pRp
1z11y11x1z0
1z11y11x1y0
1z11y11x1x0
kkjkik
kjjjij
kijiii
000
000
000
pppp
pppp
pppp
Rotační pohyb
0101 pRp
0
0
0
101010
101010
101010
1
1
1
z
y
x
z
y
x
p
p
p
p
p
p
kkkjki
jkjjji
ikijii
Rotační pohyb(význam řádků a sloupců)
1
1
1
010101
010101
010101
0
0
0
z
y
x
z
y
x
p
p
p
p
p
p
kkkjki
jkjjji
ikijii
1010 pRp
i10 j10 k10
i01
j01
k01
Rotační pohyb
0101 pRp 1010 pRp
TRR 1
T
T
10011
10
01101
01
0110101101
1001010010
RRR
RRR
ERRpRRp
ERRpRRp
Orthogonální matice
Rotační pohyb
1)det(
1)det()det()det()det()det()det(
1)det()det(
22
R
ERRRRRR
RRRR
ERRRR
TTT
TT
TT
1)det( R Orthogonální matice
SO(3)
Rotační pohyb
1
1
1
010101
010101
010101
0
0
0
z
y
x
z
y
x
p
p
p
p
p
p
kkkjki
jkjjji
ikijii
PříkladRotační pohyb
cossinsincos 001001 jijjii
Příklad
0
cos
sin
0
0
000
0sincos
0cossin
0
0
100
0cossin
0sincos
)()( 1011010
0
l
l
ll
dt
d
tdt
d
dt
dt
pRpRp
v
Rotační pohyb
1iv lω
Sférický pohyb
Euler
Sférický pohyb
nutace
rotace
precese
Roll Pitch Yaw
Sférický pohyb
Sférický pohyb
Existuje 12 kombinací nezávislých trojic rotací kolem souřadných os
1. XYZ
2. XZY
3. YXZ
4. YZX
5. ZXY
6. ZYX
Asymetrické
1. XYX
2. XZX
3. YXY
4. YZY
5. ZXZ
6. ZYZ
Symetrické
Rotace jsou možné kolem lokálních os a kolem globálních os
Celkem je tak možných 24 kombinací
Rotace kolem globálních os
Postupná rotace kvádru kolem os Z (30o) X(30o) Y(90o)
B
o
Z
o
X
o
YG pRRRp )30()30()90(
pozor na pořadí násobení
Transformace z prostoru Eulerových úhlů do prostoru SO(3)
není one to one
)()()()()()( zyzzyz RRRRRR
např. pro symetrickou rotaci ZYZ platí
pro asymetrickou rotaci ZYX platí
)()()()()()( zyzxyz RRRRRR
)()()()()()( zyzxyz RRRRRR
a)
b)
c)
Transformace z prostoru Eulerových úhlů do prostoru SO(3)
není one to one
100
0)cos()sin(
0)sin()cos(
)()()(
zyz RRR
např. pro symetrickou rotaci ZYZ platí při
pro asymetrickou rotaci ZYX platí při
0
2/
001
)sin()cos(0
)cos()sin(0
)()()(
xyz RRR
Obecný pohyb
kompozice posuvného a
sférického pohybu
Homogenní transformace
nástroj kinematiky, (počítačové grafiky,
počítačového vidění ..)
Denavit - Hartenberg
Euler
Y3
Euler
Roll Pitch Yaw
Roll Pitch Yaw
žádaná poloha a
orientace nástroje
žádaná poloha a
orientace
koncového členu
manipulátoru
Řešení inverzní úlohy dekompozicí rameno-zápěstí
pro Eulerovo a RPY zápěstí
10001000
0
0
0
1000
6363
z
y
x
r
z
y
x
r
p
p
p
p
p
p
RRR
z
y
x
z
y
x
p
p
p
qqqp
qqqp
qqqp
),,(
),,(
),,(
321
321
321
Kloubové souřadnice
ramene
11*10*1
1*
11
6666
0
nR
L00E
pLRp0ER
0
LE
0
pR rrrr
10001000
0
0
1000
66
z
yr
x
z
yr
x
n
n
n
lp
p
p
RER
111
66
0
nR
0
LE
0
pR rr
1
0
0
1
11
11
111
66
66
6
6
1
66
0
RR
0
nLRR
0
LE
0
nR
0
LEE
0
nR
0
LE
0
nR
0
pR
z
y
x
rr
rr
r
TTr
rr
n
n
n
l
10001000
0
0
0
1000
6363
z
y
x
r
z
y
x
r
p
p
p
p
p
p
RRR
)
),tan(
),tan(
2223
222
1
zyx
zyx
yx
pppq
pppAq
ppAq
)
),tan(
),tan(
2223
222
1
zyx
zyx
yx
pppq
pppAq
ppAq
zzz
yyy
xxx
aon
aon
aon
csscs
ssccscsscccs
sccssccssccc
36R
00 ayax
))1,Atan(
nebo
))1,Atan(
2
2
zz
zz
aa
aa
0sin
0sin
zzz
yyy
xxx
aon
aon
aon
csscs
ssccscsscccs
sccssccssccc
36R
pozn. Atan(x,y)……Atan(cos,sin)
Atan2(y,x)…..Atan2(sin,cos)
0sin
0sin
),Atan(
),Atan(
zz
yx
on
aa
),Atan(
),Atan(
zz
yx
on
aa
zzz
yyy
xxx
aon
aon
aon
csscs
ssccscsscccs
sccssccssccc
36R
00 ayax
1
0
0
00
0)()(
0)()(
00
zz
yy
xx
on
on
on
c
cs
sc
c
ssccscsscccs
sccssccssccc
o180;0;1cos;0sin
zzz
yyy
xxx
aon
aon
aon
csscs
ssccscsscccs
sccssccssccc
36R
1
0
0
100
0)()(
0)()(
00
0
0
zz
yy
xx
on
on
on
cs
sc
c
ccscsccccs
cssccssccc
),-Atan( yx nn
-1
Orientace Yaw Pitch Roll
ΦfiiΘthΨpsi
Rz=[cos(psi) -sin(psi) 0
sin(psi) cos(psi) 0
0 0 1]
Orientace Yaw Pitch Roll
Ry=[cos(th) 0 sin(th)
0 1 0
-sin(th) 0 cos(th)]
Rx=[1 0 0
0 cos(fii) -sin(fii)
0 sin(fii) cos(fii)]
Ror=Rz*Ry*Rx
=
[c(psi)*c(th), -s(psi)*c(fii)+c(psi)*s(th)*s(fii), s(psi)*s(fii)+c(psi)*s(th)*c(fii)]
[ s(psi)*c(th), c(psi)*c(fii)+s(psi)*s(th)*s(fii), -c(psi)*s(fii)+s(psi)*s(th)*c(fii)]
[ -s(th), c(th)*s(fii), c(th)*c(fii) ]
ΦfiiΘthΨpsi
Orientace Yaw Pitch Roll
0.9254 0.0180 0.3785
Ror = 0.1632 0.8826 -0.4410
-0.3420 0.4698 0.8138
inverze s kladným znaménkem cos(Θ)
th=atan2(-Ror(3,1),sqrt(1-Ror(3,1)^2)) = 20o
psi=atan2(Ror(2,1),Ror(1,1)) = 10o
fii=atan2(Ror(3,2),Ror(3,3)) = 30oa)
ΦfiiΘthΨpsi
Orientace Yaw Pitch Roll
inverze se záporným znaménkem cos(Θ)
th=atan2(-Ror(3,1),-sqrt(1-Ror(3,1)^2)) = 160o
psi=atan2(-Ror(2,1),-Ror(1,1)) = -170o
fii=atan2(-Ror(3,2),-Ror(3,3)) = -150o
0.9254 0.0180 0.3785
Ror = 0.1632 0.8826 -0.4410
-0.3420 0.4698 0.8138
c)
ΦfiiΘthΨpsi
Orientace Yaw Pitch Roll
0.9254 0.0180 0.3785
Ror = 0.1632 0.8826 -0.4410
-0.3420 0.4698 0.8138
b)
Numerický výpočet případu b)
psi=190/180*pi;
the=160/180*pi;
fii=-150/180*pi;
R321 =
0.9254 0.0180 0.3785
0.1632 0.8826 -0.4410
-0.3420 0.4698 0.8138
R1z=[cos(psi) -sin(psi) 0;
sin(psi) cos(psi) 0;
0 0 1];
R2y=[cos(the) 0 sin(the);
0 1 0;
-sin(the) 0 cos(the)];
R3x=[1 0 0; 0 cos(fii) -sin(fii) ;
0 sin(fii) cos(fii)];
R321=R1z*R2y*R3x
Kinematika úhlových rychlostí
0z0y0x qqq kjiq
0
0
0
)(0
xy
xz
yz
qS
)()( 00 qSS q
polosymetrická
matice
Kinematika úhlových rychlostí
bac
bac )(S
Vyjádření
vektorového součinu
pomocí PSM
111
000
)(
)(
baSc
baSc
pozor na bázi
Kinematika úhlových rychlostí
001101
1101
101
000
)(
)(
)(
bRaSR
baSR
cR
baSc
T
101001
0001
001
111
)(
)(
)(
bRaSR
baSR
cR
baSc
T
T
T
010011
011010
)()(
)()(
RaSRaS
RaSRaS
T
T
Kinematika úhlových rychlostí
Předpokládejme nyní, že soustava 1 koná vzhledem k soustavě 0 sférický
pohyb charakterizovaný vektorem otáčivé rychlosti ω. Je-li v soustavě 1
pevný bod jehož poloha je charakterizována vektorem p je posuvná rychlost
tohoto bodu dána vektorovým součinem pωv
1010
1010000 )()( pRp
pRωSpωSvdt
d
dt
d
)(
)()(
101
010110101001
ωSR
RRωSRRωSR
T
dt
d
Kinematika úhlových rychlostí
Předpokládejme nyní, že máme 3 soustavy očíslované indexy 0,1,2.
Soustava 0 je pevná. Soustava 1 koná vzhledem k soustavě 0 sférický
pohyb charakterizovaný vektorem otáčivé rychlosti ω1. Soustava 2 koná
vzhledem k soustavě 1 sférický pohyb charakterizovaný vektorem
otáčivé rychlosti ω2. Zajímá nás vyjádření otáčivé rychlosti ωc celkového
sférického pohybu který vykonává soustava 2 vzhledem k soustavě 0.
120102 RRR
dt
d
dt
d
dt
d 120112
0102 RRR
RR
Kinematika úhlových rychlostí
120102 RRR
dt
d
dt
d
dt
d 120112
0102 RRR
RR
02200210
1201200210
12010121010210
122101120110020
)()(
)()(
)()(
)()()(
RωSRωS
RRωSRωS
RRRωSRRωS
RωSRRRωSRωS
T
c
)()()( 20100 ωSωSωS c
20100 ωωω c
Kinematika úhlových rychlostí
)()()( 20100 ωSωSωS c
20100 ωωω c
1,1,032022101100
0,3020100
...
...
nnnc
nc
ωRωRωRωω
ωωωωω
umožňuje efektivní výpočet kinetické
energie soustavy
Eulerovy kinematické rovnice YPR(úhlové rychlosti v základní soustavě)
ri a0 jk
00000 ra ijkω
0
0
1
)()(
0
1
0
)(
1
0
0
0 yzz RRRω
0
0
1
)()(
0
1
0
)(
1
0
0
0 yzz RRRω
psid*[0 0 1]'+thd*Rz*[0 1 0]'+fiid*Rz*Ry*[1 0 0] '
ans =
[ -thd*sin(psi)+fiid*cos(psi)*cos(th)]
[ thd*cos(psi)+fiid*sin(psi)*cos(th)]
[ psid-fiid*sin(th)]
[ wx0]
[ wy0]
[ wz0]
Eulerovy kinematické rovnice YPR(úhlové rychlosti v základní soustavě)
[wx0]=[ -thd*sin(psi)+fiid*cos(psi)*cos(th)]
[wy0]=[ thd*cos(psi)+fiid*sin(psi)*cos(th)]
[wz0]=[ psid-fiid*sin(th) ]
Eulerovy kinematické rovnice YPR(úhlové rychlosti v základní soustavě)
)sin(01
)cos()sin()cos(0
)cos()cos()sin(0
0
0
0
z
y
x
Jacobian
Eulerovy kinematické rovnice YPR(úhlové rychlosti v základní soustavě)
3
)sin(01
)cos()sin()cos(0
)cos()cos()sin(0
)(
rankJrank
opro 90
2
)sin(01
0)cos(0
0)sin(0
)(
rankJrank
opro 90
!!!!!!
Eulerovy kinematické rovnice YPR(úhlové rychlosti v základní soustavě)
syms wx0 wy0 wz0 psi th fii real
%vypocet rychlosti uhlu RPY z uhlovych rychlosti otaceni v zemske soustave
W0=[wx0 wy0 wz0]'
J=[0 -sin(psi) cos(psi)*cos(th);
0 cos(psi) sin(psi)*cos(th);
1 0 -sin(th)]
EU=inv(J)*W0 %eulerovy uhly [psi th fii]
% ans =
[ (cos(psi)*sin(th)*wx0+sin(psi)*sin(th)*wy0+wz0*cos(th))/cos(th)]
[ -sin(psi)*wx0+cos(psi)*wy0 ]
[ (cos(psi)*wx0+sin(psi)*wy0)/cos(th) ]
Nechtˇ je letadlo v poloze psi=fii=0 a th≈90o. Nyní otáčíme letadlo pouze kolem
zemské osy X0 rychlostí wx0. Pak se Euler. úhly psii a fii mění velmi rychle.
ri a0 jk
rrarrr ijkω 0
0
0
1
0
1
0
)(
1
0
0
)()(11
xxyr RRRω
Eulerovy kinematické rovnice YPR(úhlové rychlosti v soustavě robota)
0
0
1
0
1
0
)(
1
0
0
)()(11
xxyr RRRω
psid*inv(Ry*Rx)*[0 0 1]'+thd*inv(Rx)*[0 1 0]'+fiid*[1 0 0]'
ans =
[ -psid*sin(th)+fiid]
[ psid*cos(th)*sin(fii)+thd*cos(fii)]
[ -thd*sin(fii)+psid*cos(th)*cos(fii)]
Eulerovy kinematické rovnice YPR(úhlové rychlosti v soustavě robota)
[wxr]=[ -psid*sin(th)+fiid ]
[wyr]=[ psid*cos(th)*sin(fii)+thd*cos(fii) ]
[wzr]=[ -thd*sin(fii)+psid*cos(th)*cos(fii) ]
Eulerovy kinematické rovnice YPR(úhlové rychlosti v soustavě robota)
Eulerovy kinematické rovnice YPR(výpočet rychlosti Euler úhlů z údajů „gyroskopů“)
bAx
A=[-sin(th) 0 1;
cos(th)*sin(fii) cos(fii) 0;
cos(th)*cos(fii) -sin(fii) 0]
x = [psid thd fiid]’
b = [wx wy wz]'
x = inv(A)*b
x =
[ (sin(fii)*wy+wz*cos(fii))/cos(th)]
[ cos(fii)*wy-sin(fii)*wz]
[ (wx*cos(th)+sin(th)*sin(fii)*wy+sin(th)*cos(fii)*wz)/cos(th)]
rychlost E
úhlů
údaje
gyroskopů
viz předchozí
slide
Eulerovy kinematické rovnice YPR(výpočet rychlosti Euler úhlů z údajů „gyroskopů“)
x = [psid thd fiid]’
x =
[ (sin(fii)*wy+wz*cos(fii))/cos(th) ]
[ cos(fii)*wy-sin(fii)*wz ]
[ (wx*cos(th)+sin(th)*sin(fii)*wy+sin(th)*cos(fii)*wz)/cos(th)]
cos
cossinsinsincos
sincos
cos
cossin
zyx
zy
zy
Gimbal lock
Gimbal lock