Kinematika = studium pohybu mechanických těles bez uvážení silrobotika/prednasky/2014_Kinematika.pdf · Kinematika = studium pohybu mechanických těles bez uvážení sil. Kinematické

Kinematika = studium pohybu mechanických těles

bez uvážení sil

Kinematické řetězce

Kinematické dvojice








Illustation of a 3R manipulator


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

qq

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]


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


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


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


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

qq

qq

qq

qS

)()( 00 qSS q

polosymetrická

matice


bac

bac )(S

Vyjádření

vektorového součinu

pomocí PSM

111

000

)(

)(

baSc

baSc

pozor na bázi


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


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


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


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


)()()( 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]


[wx0]=[ -thd*sin(psi)+fiid*cos(psi)*cos(th)]

[wy0]=[ thd*cos(psi)+fiid*sin(psi)*cos(th)]

[wz0]=[ psid-fiid*sin(th) ]


)sin(01

)cos()sin()cos(0

)cos()cos()sin(0

0

0

0

z

y

x

Jacobian


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

!!!!!!


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)]


[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(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

Kinematika = studium pohybu mechanických těles bez uvážení silrobotika/prednasky/2014_Kinematika.pdf · Kinematika = studium pohybu mechanických těles bez uvážení sil. Kinematické

Documents