Kinematika - mif.vu.ltmeglinskas/r/Kinematika_2015.pdf · Kaip paskaičiuoti judėjimą tokiam daiktui ? jeigu įjungsime kairį vaikliuką 2 apsisukimai per sek. greičiu, o dešinį

Kinematika

Kinematika nagrinėja kaip elgiasi mechaninės sistemos

Kaip paskaičiuoti judėjimą tokiam daiktui ?

jeigu įjungsime kairį vaikliuką 2 apsisukimai per sek. greičiu, o dešinį 4 apsisukimai per sek.?

O tokiam ?jeigu įjungsime pirmą varikliuką 2 aps./sek. greičiu, antrą 4 aps./sek. greičiu, trečią 6 aps./sek.

Roboto pozicija:

= [ x, y, q]

Nagrinėsime judėjimą visiškai plokščiame paviršiuje

{XI,YI} – globali atskaitos sistema

{XR,YR} – lokali atskaitos sistema

100

0cossin

0sincos

)(

R

judėjimo koordinačių perkėlimas tarp atskaitos sistemų (iš globalios į lokalią)

{XI,YI} -> {XR,YR}

Pavyzdys:

kampas = p / 2

100

001

010

)2

(p

R

x

y

y

x

100

001

010

Du valdomi ratai

l – rato atstumas nuo centro

r – rato diametras

- rato sukimosi greitis

),,,,( 21

rlfy

x

I

21,

Roboto judėjimo pirmyn greitis

xr1=(1/2) 1r

Roboto sukimosi greitis

l

r

2

11

l

r

2

22

Kinematikos modelis robotui su

dviem valdomais ratais

l

r

l

r

rr

RI

22

022

)(

21

21

1

100

0cossin

0sincos

)( 1

R

Atvirkštinė matricą

p/2

r = 1

l = 1

1 4

2 2

1

3

0

1

0

3

100

001

010

y

x

100

0cossin

0sincos

)( 1

R

l

r

l

r

rr

RI

22

022

)(

21

21

1

- rato sukimosi greitis

l – rato atstumas nuo centro

r – rato diametras

O dabar panašų dalyką pati pabandysime

padaryti sudėtingu būdu

Prielaidos

• Robotas juda visiškai lygiu paviršiumi

• Judant ratai nepraslysti

• Judant robotas neslysta į šalį

• Ratai nesideformuoja

• Ratų padėtis neturi laisvumo

Penki ratų tipai ir lygtys jiems

• Fiksuotas ratas

• Valdomas ratas

• Paslinktas ratas

• Švediškas ratas

• Sferinis ratas

Fiksuotas ratas

0)(cos)();cos();sin( rRl I

Sukimosi apribojimas:

Judėjimo lygtis - apribojimas

x y

R dalis yra performuoti koordinates į roboto atskaitos sistemą,

nes (,,) yra skaičiuojamos roboto atskaitos sistemos

koordinatėmis

Mūsų judėjimas visgi turi būti lygus rato pasisukimui

Slydimo lygtis - apribojimas

0)(sin);sin();cos( IRl

Ir visai apribojame judėjimą ortogonalia kryptimi

Lygties pavyzdys

0, 0, 0

0)(sin);sin();cos( IRl

0001

100

010

001

001

y

x

y

x

Valdomas ratas

Lygtys

0)(cos)()cos()sin( rRl

0)(sin)sin()cos( IRl

Tačiau – jau nebe fiksuotas, o valdomas dydis

Paslinktas ratas (castor wheel)

Lygtys

0)(cos)()cos()sin( rRl

0)(sin)sin()cos( dRld I

Švediškas ratas

Lygtys

0cos)()cos()();cos();sin( rRl

0sin)()sin();sin();cos( swswI rrRl

Elgesena labai priklauso nuo

Jeigu = 0 , tai gauname standartinį ratą

Roboto kinematiniai apribojimai

• Turime viso N ratų, iš kurių Nf fiksuotų ir Nv

valdomų

• s(t) pažymime valdomų ratų kampus

• f pažymime fiksuotų ratų kampus

• Askiriame valdomų ir nevaldomų ratų greičius

)(tf

)(ts

)(

)(

t

t

s

f

0)()( 21 JRJ Is

Roboto riedėjimo ribojimai

J2 yra matrica sudaryta iš ratų spindulių - r (NxN)

yra matrica su visų ratų riedėjimais pagal jų

individualias plokštumas, kuri yra funkcija nuo s

)(1 sJ

)1

1

1 ()(

ss

f

s J

JJ

0)(cos)();cos();sin( rRl I

yra konstantų matrica, visoms ratų

projekcijoms jos dydis yra , kurių

kiekviena eilutė sudaryta iš trijų parametrų

paimtų iš pradinės lygties.

)()(

)1

1

1

ss

f

s J

JJ

3fN

fJ1

Roboto sukimosi ribojimai (2)

Roboto slydimo ribojimai

0)()(1 Is RC

)()(

1

1

1

ss

f

sC

CC

0)(sin);sin();cos( IRl

Roboto kinematiniai apribojimai

0)(

)(

)( 2

1

1

JR

C

JI

s

s

0)()( 21 JRJ Is

)()(

1

1

1

ss

f

sC

CC

)1

1

1 ()(

ss

f

s J

JJ

0)()(1 Is RC

Diferencinis robotas

Ratukas yra nevaldomas, todėl į jį

neatsižvelgiame. Abu ratai

nesukiojami, tad priklausomybių nuo

neturėsim

Direfencinio roboto geometrija

ir ?

p/2

p

p/2

0

l=1

r=1

kryptis turi sutapti su judėjimo kryptimi

p/2

p

p/2

0

0

)(

010

01

012

J

Rl

l

I

0)(

)(

)( 2

1

1

JR

C

JI

s

s

l

r

l

r

rr

RI

22

022

)(

21

21

1

Robotas neturi valdomų ratų, tad J1(s) susiprastina iki J1f.

Mums pasisekė - švediškų ratų = 0

0cos)()cos()()cos()sin( rRl

0)(cos))(cos()sin( rRl I

0)()( 21 JRJ Is

Reikia apskaičiuoti ,,

• = p / 3 , p , p / 3

• = 0 visiems ratams

0)(cos))(cos()sin( rRl I

l=1, r = 1, =0 Jeigu ratų greičiai bus 1=4, 2=1, 3=2,

tai kur judės robotas ?

KALMAN FILTRAS

Modelis

𝑥𝑘 = 𝐹𝑘 𝑥𝑘−1+ 𝐵𝑘 𝑢𝑘 + 𝑤𝑘

𝑤𝑘 = N(0, 𝑄𝑘)

𝑧𝑘 = 𝐻𝑘 𝑥𝑘+ 𝑣𝑘

Būsena:

Stebėjimas:

𝑣𝑘 = N(0, 𝑅𝑘)

Skaičiavimai

𝑥𝑘− = 𝐴 𝑥𝑘−1+ 𝐵𝑢𝑘

Būsenos prognozavimas:

Koregavimas pagal duomenis:

𝑃−𝑘 = 𝐴𝑃𝑘−1𝐴

𝑇 + 𝑄

𝐾𝑘 = 𝑃−𝑘𝐻

𝑇(H 𝑃−𝑘𝐻

𝑇 + R)−1

𝑥𝑘 = 𝑥𝑘− + 𝐾𝑘 (𝑧𝑘 − 𝐻 𝑥𝑘

−)

Kalman gain

Patikslinam pagal

matavimus

𝑃 𝑘 = (1 − 𝐾𝑘 H)𝑃−𝑘 Atnaujinam

kovariacinę matricą

Realus atvejis, įtampos

stebėjimas

𝑥𝑘 = 𝐹𝑘 𝑥𝑘−1+ 𝐵𝑘 𝑢𝑘 + 𝑤𝑘 = 𝒙𝒌−𝟏+ 𝒘𝒌

𝑤𝑘 = N(0, 𝑄𝑘)

𝑧𝑘 = 𝐻𝑘 𝑥𝑘+ 𝑣𝑘 = 𝒙𝒌+ 𝒗𝒌

Būsena:

Stebėjimas:

𝑣𝑘 = N(0, 𝑅𝑘)

𝑥𝑘− = 𝐴 𝑥𝑘−1+ 𝐵𝑢𝑘 = 𝑥𝑘−1

Būsenos prognozavimas:

Koregavimas pagal duomenis:

𝑃−𝑘 = 𝑃𝑘−1 + 𝑄

𝐾𝑘 = 𝑃−𝑘𝐻

𝑇(H 𝑃−𝑘𝐻

𝑇 + R)−1= 𝑃−𝑘( 𝑃−

𝑘 + R)−1

𝑥𝑘 = 𝑥𝑘− + 𝐾𝑘 (𝑧𝑘 − 𝐻 𝑥𝑘

−) = 𝑥𝑘− + 𝐾𝑘 (𝑧𝑘 − 𝑥𝑘

−)

𝑃 𝑘 = (1 − 𝐾𝑘 H)𝑃−𝑘

Realus atvejis, įtampos

stebėjimas

Realus atvejis, įtampos

stebėjimas

z = -0.377

Nukrypis nuo matavimų paprastai būna 0.1V

R= (0.1)2

Q = 0.0001

P = 1

x = 0

R = 0.1

R = 1

R = 0.0001