Mobilis robotok irányítása 1. A gyakorlat célja Mobilis robotok kinematikai modellezése Matlab/Simulink környezetben. Mobilis robotok Ponttól Pontig (PTP) irányításának tervezése és megvalósítása.
2. Elméleti bevezet Mobilis robotok – helyváltoztatásra képes irányított mechanikai rendszerek. Kerekeken guruló mobilis robot – kerekekkel felszerelt járm , amely autonóm mozgásra képes. A kerekeket beavatkozókkal (motorokkal) hajtjuk, a robot irányítására célszámítógépet használunk. Irányítási lehet ségek: - Teleoperáció – távvezérlés – a felhasználó vizuális jelek alapján irányítja a robot mozgását. Ebben az esetben vagy a robot vagy a környezek képalkotó/továbbító eszközökkel van felszerelve. Ebben az esetben nem beszélhetünk autonóm robotról, hisz a robot nyílt hurokban van vezérelve. - El re megadott pálya követése – A környezetben a robot el re megadott síkbeli pontokon kell áthaladjon. Ez megoldható Ponttól Pontig szabályozással, vagy a pontokra pályát fektethetünk, és pályakövetést valósíthatunk meg. Az ilyen feladatoknál a robotot akadálydetektáló érzékel kkel (csápos, ultrahangos érzékel k) szereljük fel az el re nem tervezett akadályok elkerülésére. - Dinamikus pályatervezésen alapuló irányítás – Adott a cél koordinátája. A kiindulóponttól a célig való eljutás során a robot az érzékel jelek alapján „feltérképezi” a terepet (kamera-képfeldolgozás, ultrahangos, lézeres távolságmér , digitális irányt , GPS) és építi fel haladás közben a pályáját. 2.1. Mobilis robot kinematikai modellje Feltételezzük, hogy a robot merev test, a kerekek szintén merevek, síkban mozog, a kerekek nem csúsznak meg. x B Oy B bázis koordináta rendszer x R Oy R robothoz rendelt koordináta rendszer A robot síkbeli pozíciója: ξ = (x y θ )T
A két koordinátarendszerben felírt sebességek között egy z körüli elfordulás van θ szöggel. cos θ sin θ 0
R (θ ) = − sin θ 0
cos θ
0
0
1
ξ R = R(θ ) ⋅ ξ
1. Ábra: mobilis robot kinematikai modellje
p – a robot középpontja ω =θ vx = v vy = 0
A robot sebessége x R Oy R -ben ξ R
v = 0
ω
ξ = R(θ ) ⋅ ξ R −1
cos θ = sin θ 0
− sin θ cos θ 0
0 v 0 ⋅ 0 1 ω
x = cos θ ⋅ v y = sin θ ⋅ v θ =ω
Kihasználtuk, hogy R −1 (θ ) = R T (θ ) Kerék szögsebességek és a robot sebessége közti összefüggés: Legyen - a kerekek sugara – r - A féltengely távolság –l - A kerekek szögsebessége ϕ1 , ϕ 2 - A kerekek lineáris sebessége x r1 = r ⋅ ϕ1 , x r2 = r ⋅ ϕ 2 Ha a 2. kerék rögzített és az 1. forog:
(1)
2. Ábra: Robot sebességek –kerék sebességek
x r r ⋅ϕ = 2 2 v r ⋅ϕ ω= = l 2l v=
Ha mind a két kerék forog, a sebességek/szögsebességek összeadódnak. A szögsebességnél figyelembe kell venni, hogy a két kerék ellentétes irányba történ forgást generál. Tehát: r ⋅ ϕ1 r ⋅ ϕ 2 + 2 2 r ⋅ ϕ1 r ⋅ ϕ 2 ω= − 2l 2l v +ω ϕ1 = r 1 ⋅ 1+ 2 l v −ω ϕ2 = r 1 ⋅ 1+ 2 l v=
(2)
(3)
2.2 Irányítás kinematikai modell alapján
Megkülönböztetünk ponttól pontig és pályakövet irányítást. Ponttól pontig irányítás esetében adott a célpozíció és az orientáció a célpozícióba. Pályakövetés esetén adott a robot pályájának id függvénye: x ref = x ref (t ) y ref = y ref (t ) θ ref = θ ref (t )
Az irányítás során a robot sebességét és szögsebességét határozzuk meg (ezek lesznek a beavatkozó jelek). A v, w alapján meghatározzuk a kerekek szögsebességét. Ezek lesznek az alapjelek a kerekek szögsebességét biztosító sebességszabályozónak.
3. ábra: Mobilis robot irányítási rendszere
Seb.szab. – sebesség szabályozó u1 , u 2 – a kerekeknek kiszámított feszültség (beavatkozó jel) (*) transzf. – (3) összefüggés Ponttól pontig irányítás
4. ábra: Ponttól pontig irányítás
Adva van x ref , y ref , θ ref el írt pozíció és orientáció, konstans értékek. Vegyük fel egy ennek megfelel x D Oy D koordinátarendszert a mozgás síkjában úgy, hogy Ox B és Ox D közötti szög θ D , t ρ egyenes, amely áthalad Pref –en és P-n ρ = d (Pref , P ) távolság
α = (x R , t ρ ) szög ∧
β = (x D , t ρ ) szög ∧
Feltételezzük, hogy refenciában:
x ref = y ref = θ ref = 0 .
Linearizáljuk a kinematikai modellt a
x = cos θ ⋅ v y = sin θ ⋅ v
cos θ = 1
θ =0
sin θ = 0
θ =ω x
Az állapotok: x = y θ
A linearizált modell (x = Ax + Bu ) x
0 0 0
x
1 0
y = 0 0 0 ⋅ y + 0 0 ⋅
θ
0 0 0
θ
0 1
A
v
ω
B
A rendszer megfigyelhet ségi mátrixa: M C = [B AB A 2 B ] 1 0 0 0 0 0
rank (M C ) = 2 < 3
MC = 0 0 0 0 0 0 0 1 0 0 0 0
A rendszer lineáris állapotvisszacsatolással nem irányítható. Nemlineáris állapotvisszacsatolás tervezése: Fogalmazzuk át az irányítási feladatot: Keressük úgy a v, ω értékeket, hogy ρ → 0 , α → 0 , β → 0 , ha t → ∞ . ρ = ∆x 2 + ∆y 2 α = −θ + a tan
∆y ∆x
β = −θ − α + θ ref ∆x = x ref − x; ∆y = y ref − y
π π
Az új állapotok változásai, ha α ∈ − ,
2 2
, vagyis a v sebességvektor a „cél felé néz”.
(4)
5. Ábra: Az α szög meghatározása
Az új állapotok dinamikájának meghatározása: d dt
1. ρ 2 = ∆x 2 + ∆y 2
ρ ⋅ ρ = 2∆x (x ref − x ) + 2∆y (y ref − y ) ∆x
ρ=− ∆x
ρ ∆y
ρ
ρ
x−
∆y
ρ
y
= cos(θ + α ) = sin (θ + α )
x = v ⋅ cos(θ )
y = v ⋅ sin (θ ) ρ = − cos θ ⋅ cos α ⋅ v ⋅ cos θ + sin θ ⋅ sin α ⋅ v ⋅ cos θ − sin α ⋅ cos θ ⋅ v ⋅ sin θ − cos α ⋅ sin θ ⋅ v ⋅ sin θ
(
)
ρ = − cos 2 θ + sin 2 θ ⋅ v ⋅ cos θ ρ = −v ⋅ cos θ v
2. α = −θ + θ ρ = −ω + ⋅ sin α ρ
v
v
ρ
ρ
3. β = −θ − α + θ ref = −ω + ω − ⋅ sin α = − ⋅ sin α Tehát: − cos α ρ sin α α = ρ β sin α − ρ
Ha α ∈ − π ,− v := −v
π 2
∪
π 2
,π
0 −1 ⋅
v
ω
0
, akkor a sebességvektor a céllal ellentétes irányba néz és a
helyettesítést kell elvégezzünk.
Válasszuk az irányítási algoritmust:
v = Kρ ⋅ ρ
(5)
ω = Kα ⋅ α + K β ⋅ β
Behelyettesítve kapjuk a zárt rendszert: ρ = − cos α ⋅ K ρ ⋅ ρ α = sin α ⋅ K ρ − K α ⋅ α − K β ⋅ β β = − sin α ⋅ K ρ
Linearizáljuk α = 0 körül
cos α = 1 sin α = α
A linearizált zárt rendszer: ρ = −K ρ ⋅ ρ
α = (K ρ − K α )⋅ α − K β ⋅ β β = −K ρ ⋅α ρ − Kρ α = 0
0
− Kρ
− Kβ 0
(K ρ − K α )
0
β
0
ρ ⋅ α β
A
A rendszermátrix karakterisztikus polinomja:
(
)(
(
)
det (λI − A) = λ + K ρ ⋅ λ 2 − K ρ − K α ⋅ λ − K β ⋅ K ρ
)
Ahhoz, hogy a szabályozás stabil legyen, az összes sajátérték valós része negatív kell, hogy legyen Re(λ i ) < 0, i = 1,2,3 Ennek feltétele, hogy a szabályozó paramétereit az alábbi
3. A mérés menete Feladat: Modellezzük a mobilis robotot S-függvények technikájával. Modellezzük a ponttól pontig irányítást S-függvényként. Készítsük el a szabályozási kört és vizsgáljuk a szabályozás min ségét. A robotot leíró modellt és a szabályozót s-function formájában építjük fel. Az s függvény általános formája: function [sys, x0]= model(t, x, u, flag) if (sys==0) %Initialization sys = [ , ,
% number of continuous states % number of discrete states
x0
, , 0, ]; = [];
if (flag==1) sys= elseif (flag==2) sys= elseif (flag==3) sys= else sys=[]; end
% % % %
number of outputs number of inputs reserved must be zero direct feedthrough flag % continous states % output
equation
% model constants
Egy s függvény bemenetei az id (t) a rendszer állapotai (x), a rendszer bemenetei (u) és egy kapcsoló (flag) amely az s függvény állapotát adja meg. A visszaterítési érték (sys) a kapcsoló értékét l függ. Ha a kapcsoló értéke 0 akkor a rendszer dimenzióit és kezd állapotait (x0) adjuk meg. Az sys utolsó paramétere 0, ha a bemenet hatása egyenes úton jelentkezik a kimeneten (statikus elemek is vannak a rendszerben) Ha a kapcsoló értéke 1, akkor a rendszer folytonos állapotainak változását kell visszatéríteni (dx/dt) Ha a kapcsoló értéke 2, akkor a rendszer diszkrét állapotainak változását kell visszatéríteni (xk+1) Ha a kapcsoló értéke 3, akkor a rendszer kimeneteit kell visszatéríteni (y) A robot modellt az (1) összefüggésben megadott kinematikai modell alapján építjük fel. A v, w sebességeket az (2) összefüggésben megadott kerék transzformáció alapján határozzuk meg. A kerekek szögsebességei az s-függvény bemenetei. A robotmodellnek két bemenete, három kimenete és három folytonos állapota van (pozíció X koordináta, pozíció y koordináta és a szögelfordulás). A kimenetek megegyeznek az állapotokkal. A rendszernek nincs diszkrét állapota. A kezd állapotok legyenek nullák. A robot paraméterei legyenek: - kerék átmer : r=2.6E-2; - tengely távolság fele: l=5.75E-2; A szabályzónak hat bemenete van: az el írt pozíció koordináták (X és Y) és az el írt orientáció és a mobilis robot kimenetei (pozíció koordináták és az orientáció). Válasszunk a modellnek négy kimenetet (kerekek szögsebességei valamint a robot lineáris sebessége és szögsebesség). Az utóbbi kett t csak monitorizálási célokra alkalmazzuk. Mivel a szabályozó statikus, az s-függvény folytonos és diszkrét állapotait nullának választhatjuk, ugyanakkor beállítjuk, hogy a kimenete hatása egyenes tón érz dik a bemeneten. A kerekek szögsebességeinek kiszámítása: - Meghatározzuk a ρ, α, β paramétereket a (4) összefüggés alapján.
- Kiszámítjuk a robot lineáris sebességét és szögsebességét az (5) összefüggés alapján. Válasszuk a szabályozóparamétereket az alábbi módon: Kρ=1, Kα=2, Kβ=-2. - A kerék szögsebességek meghatározására elvégezzük az (3) transzformációt
6 Ábra: A mobilis robot szabályozási rendszere
Építsük fel az 6 Ábrán látható szabályozási rendszert. A referencia pozíció koordinátáit illetve az orientációt Const blokkokkal, konstansként adjuk meg. A pozíció koordintátákat és orientáció illetve a beavatkozó jeleket (sebesség, szögsebesség) Scope blokkokkal vizsgáljuk. A pozíció koordinátákat vizsgáljuk még XY Graph ablakelemen, aminek segítségével kirajzolhatjuk a robot útját, trajektóriáját (lásd 7 Ábra).
7 Ábra: A robot síkbeli mozgása
Módosítsuk a szabályozó három paraméterét egyenként. Milyen hatással van a három paraméternek a szabályozási id re és a túllövésre?
4. Kérdések és feladatok 1. Módosítsuk a programot úgy, hogy se a modell se a szabályozó ne tartalmazza a kerék transzformációt. 2. Vizsgáljuk meg, mi történik a rendszer kimeneteivel, ha a szabályozó paraméterei nem teljesítik a stabilitásra vonatkozó feltételeket? 3. Keresünk az Interneten négykerek mobilis robot mozgását leíró modellt illetve a négykerek (gépjárm szer ) mobilis robot ponttól pontig szabályozását megoldó algoritmust.
