kinematika - mif.vu.ltmeglinskas/r/kinematika_2015.pdf · kaip paskaičiuoti judėjimą tokiam...
TRANSCRIPT
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