Kinematika = studium pohybu mechanických těles bez uvážení...

transcript

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

Pohyb tělesa

Geometrické vektory

Aritmetické vektory

Posuvný pohyb

000 rdp

Posuvný pohyb

(rychlosti)

000 rdp

Rotační pohyb

1z11y11x10z00y00x0 kjikji pppppp

1z11y11x1z0

1z11y11x10z00y00x0

1z11y11x1y0

1z11y11x10z00y00x0

1z11y11x1x0

1z11y11x10z00y00x0

kkjkik

kjikkjik

kjjjij

kjijkjij

kijiii

kjiikjii

pppppp

Rotační pohyb

010101

kkkjki

jkjjji

ikijii

1010 pRp

1z11y11x1z0

1z11y11x1y0

1z11y11x1x0

kkjkik

kjjjij

kijiii

Rotační pohyb

0101 pRp

101010

kkkjki

jkjjji

ikijii

Rotační pohyb(význam řádků a sloupců)

010101

kkkjki

jkjjji

ikijii

1010 pRp

i10 j10 k10

Rotační pohyb

0101 pRp 1010 pRp

0110101101

1001010010

ERRpRRp

Orthogonální matice

Rotační pohyb

1)det(

1)det()det()det()det()det()det(

1)det()det(

ERRRRRR

1)det( R Orthogonální matice

Rotační pohyb

010101

kkkjki

jkjjji

ikijii

PříkladRotační pohyb

cossinsincos 001001 jijjii

Příklad

0sincos

0cossin

0sincos

)()( 1011010

Rotační pohyb

1iv lω

Sférický pohyb

nutace

rotace

precese

Roll Pitch Yaw

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)

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

Transformace z prostoru Eulerových úhlů do prostoru SO(3)

není one to one

0)cos()sin(

0)sin()cos(

)()()(

zyz RRR

např. pro symetrickou rotaci ZYZ platí při

pro asymetrickou rotaci ZYX platí při

)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

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

Kloubové souřadnice

ramene

11*10*1

pLRp0ER

pR rrrr

10001000

),tan(

ssccscsscccs

sccssccssccc

00 ayax

))1,Atan(

ssccscsscccs

sccssccssccc

pozn. Atan(x,y)……Atan(cos,sin)

Atan2(y,x)…..Atan2(sin,cos)

),Atan(

ssccscsscccs

sccssccssccc

00 ayax

ssccscsscccs

sccssccssccc

o180;0;1cos;0sin

ssccscsscccs

sccssccssccc

ccscsccccs

cssccssccc

),-Atan( yx nn

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)

-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

ΦfiiΘthΨpsi

0.9254 0.0180 0.3785

Ror = 0.1632 0.8826 -0.4410

-0.3420 0.4698 0.8138

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

)()( 00 qSS q

polosymetrická

matice

bac )(S

Vyjádření

vektorového součinu

pomocí PSM

pozor na bázi

001101

101001

010011

011010

RaSRaS

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

1010000 )()( pRp

pRωSpωSvdt

010110101001

RRωSRRωSR

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

d 120112

0102 RRR

120102 RRR

d 120112

0102 RRR

02200210

1201200210

12010121010210

122101120110020

)()()(

RωSRωS

RRωSRωS

RRRωSRRωS

RωSRRRωSRωS

)()()( 20100 ωSωSωS c

20100 ωωω c

)()()( 20100 ωSωSωS c

20100 ωωω c

1,1,032022101100

0,3020100

ω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 yzz RRRω

psid*[0 0 1]'+thd*Rz*[0 1 0]'+fiid*Rz*Ry*[1 0 0] '

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

Jacobian

)sin(01

)cos()sin()cos(0

)cos()cos()sin(0

rankJrank

opro 90

)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

)()(11

xxyr RRRω

Eulerovy kinematické rovnice YPR(úhlové rychlosti v soustavě robota)

)()(11

xxyr RRRω

psid*inv(Ry*Rx)*[0 0 1]'+thd*inv(Rx)*[0 1 0]'+fiid*[1 0 0]'

[ -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ů“)

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

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

Eulerovy kinematické rovnice YPR(výpočet rychlosti Euler úhlů z údajů „gyroskopů“)

x = [psid thd fiid]’

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

cossinsinsincos

sincos

cossin

Gimbal lock

Kinematika = studium pohybu mechanických těles bez uvážení...

Documents