Szeromotor pályköetést meglósító irányítás. A gykorlt célj Szeromotor pozíciószbályozásánk megoldás előírt pály mentén. Időben optimális pály és pályköetést meglósító irányítási lgoritmus implementálás.. Elméleti beezető A robotirányítási lgoritmus elelős zért, hogy robot égberendezése eljusson célpontb, gy előre deiniált pályát járjon be. Amennyiben robot csk PTP (ponttól pontig) mozgást égez, robot irányításár egyszerű PID típusú irányítási lgoritmusokt hsználhtunk. Pályköetés esetében igyelembe kell, hogy együk robot dinmikus modelljét is és így nemlineáris lgoritmusokhoz jutunk. Pályterezés: robotok térbeli mozgásánk megterezése, z előírt pály meghtározás. Kétéle mozgást különböztetünk meg: - Pont-pont mozgás (PTP point to point) - Pályköetés A PTP mozgás esetén cél robot eljutttás egy dott égpontb. Fontos, hogy éges idő ltt tegye meg robot z utt kezdőponttól égpontig. A két pont közti szksz nem ontos. Pályköetés esetén cél ugynz, eljutni egy kezdőpontból égpontb, iszont itt kezdőpont és égpont között minden időpillntbn előírjuk robot ktuális helyzetét. A robotirányítási lgoritmusbn z előírt érték minden időpillntbn más és más lesz, mi pontosbb irányításhoz ezet.. Polinomiális pályterezés Adott 0 kezdeti és égpozíció ismeretében terezünk egy pályát mi, mi rendszerünk prmétereinek (imális gyorsulás sys _, imális sebesség sys _ ) megelelően, időben optimális mozgást eredményez. Az első szkszbn nöeljük sebességet ( gyorsulás sys _ ) míg elérjük sys _ sebességet. A második szkszbn mozgást imális sebességgel olyttom. A hrmdik szksz hossz megegyezik z első szkszél. Itt sebességet -ról lecsökkentjük nullár (gyorsulás ). sys _ sys _ A megelelő sebesség és gyorsulás irányok biztosításár beezetjük köetkező áltozókt:
sign sign 0 sys _ 0 sys _ () Az 0, közti táolság üggényében két illete három szkszból álló pályákt különböztetünk meg. H táolság nem elég rr, hogy elérjük imális sebességet és után sebességet nullár csökkentsük, kkor csk két szksz lesz pályánk (egy gyorsulási és egy lssulási szksz). Ábr: Három szksz Ábr: Két szksz Első eset (három szksz): sys _ sys _ 0 - első szksz (gyorsulás): t t t t t 0 hol t. - második szksz (mozgás imális sebességgel): t t t 0 t t () (3)
hol: t ( t ) ( t ) (4) (5) t 0 t t 0 (6) - hrmdik szksz (lssítás): t t t t t t t (7) Második eset (két szksz): sys _ sys _ 0 - első szksz (gyorsulás): hol: - második szksz (lssítás): t t t 0 0. t t t (8) hol: t t t t t t t t (9) t t 0 (0) () t t
. A motor modellezése z irányítás terezéséhez Induljunk ki motor mechnikus és elektromos egyenleteiből: d J ci dt di L i R c u () dt hol szögsebesség, i rotoron olyó árm, u bemeneti eszöltség. A prméterek J terhelés inerciáj, R rotor ellenállás, L rotor induktiitás, c nyomtékállndó, c sebességállndó. Eloszt z ellenállás értékéel és eltételeze, hogy z R/L elektromos időállndój kis értékű, kpjuk: L di c i u R dt R R 0 c i u (3) R R Visszhelyettesíte mechniki egyenletbe, kpjuk z irányítás terezéséhez lklmzhtó modellt: Vezessük be z lábbi jelöléseket: d c cc J u dt R R d c cc u dt J R J R (4) K u c J R cc K (5) J R A jelölésekkel kpjuk: d K u u K (6) dt H -el jelöljük pozíciót: K K u (7) u
3. A mérés menete 3. Az irányítási lgoritmus terezése A pályköetési eldt megoldásához eltételezzük, hogy dott z előírt pály (előírt pozíció, sebesség, gyorsulás) lmint motor (7) dinmikus egyenlete. Deiniáljuk pályköetési hibmetrikát: S (8) hol >0 súlyzótényező. Terezzük úgy z irányítást, hogy S értéke nulláb trtson. Htározzuk meg S időbeli áltozását: Behelyettesíte (7) dinmikát: Válsszuk betkozó jelet: S (9) K K u S (0) u u K u K K S s () A betkozó jellel kpjuk: S K S () H K s 0 -r álsztjuk, kkor () egy stbil elsőokú homogén dierenciálegyenlet ( S K S 0), tehát S értéke szimptotikusn nulláb og konergálni. s s
3 Ábr: Az S hibmetrik konergenciáj A olymt prméterek kiszámításhoz elhsználjuk motor ktlógusdtit: R 7. 3 c 6.6 c 38.0 J.890 3 4 3 38.0 38 8.35 K u 8. 35, K 4. 08.890 7.3.897.3 6.6 A szbályozóprméter megálsztásánál zt esszük igyelembe, hogy (8) hibmetrikábn pozícióhib ngyobb súllyl kell megjelenjen, mint sebességhib. Miel sebességhib súlyzój, ezért értékét egységnél ngyobbnk kell álsztni. Robotiki lklmzásoknál tipikusn 0-nek álsztjuk. 3. A pályterezés és szbályozás implementálás A eldt megoldásához elhsználjuk pozíciószbályozásánál és sebességszbályozásnál lklmzott méréseket és betkozó jel kiküldését. Szükségünk n sebesség, pozíció és mintételi periódus mérésére. A sebesség mértékegysége ok/mp pozíció mértékegysége ok mintételi periódus mértékegysége mp (másodperc) kell, hogy legyen. A betkozó jel kiküldésénél szükségünk n súrlódás mitt megjelenő holtsá kompenzálásár, betkozó jel korlátozásár (+/- 5000 mv) és hrdernek speciikus kiküldésre: betkozó jel bszolút értékét z nlóg kimeneten, előjelét digitális kimeneten küldjük ki. A pályterezés és pályköetést meglósító szbályozás kkor kell égrehjtódjon, mikor MotorControl lklmzásbn TRACKING CONTROL gomb n lenyom. Természetesen ezt is z OnTimer üggényben implementáljuk. A pályterezéshez először z időlpot állítjuk elő egy sttikus áltozóbn. A progrm indításkor ennek áltozónk z értéke null, és szbályozás indításkor értékét minden mintételi periódusbn nöeljük. A pályát számítás: =5000; u=3000; =3000; t=u/; =u /; =-; t=t+(-)/u; t3=t+t; t=t+tperiod; // t-inicilizálni 0-r z OnInitDilog-bn i (t<t) =; u= t; =/ t ;
else i(t<t) =0; u=u; =u(t-t)+; else else i(t<t3) =; u=u-(t-t); =-u / +u(t-t)+(t-t) ;/; else =0; u=0; =; A betkozó jel kiszámításánál KS áltozó értékét KPSlider csúszkáról olssuk le. A betkozó eszültség számítás: l=0; Ku=8.35; K=.08; gettetbovl("kp_trckbr", out temppid); K_S = (double)temppid; s=(-)+l(-); u=(+ l(-)+k +Ks s)/ku; A progrm utttásánál izsgáljuk szbályozás minőségét KS=0 -r mjd más erősítésértékekre is. A TRACE segítségéel izsgáljuk pályköetési pontosságot. 4. Kérdések és eldtok. Módosítsuk úgy progrm pályterező részét, hogy 0 0.. Bőítsük ki progrmot úgy, hogy pályterező rész trtlmzz két szkszos pályát is. 3. Módosítsuk mintételi periódust 5 ezredmásodpercről 60 ezredmásodpercre SetTimer üggényben. Milyen htássl n pályköetés pontosságár?