Politechnika WrocªawskaWydziaª Elektroniki

Kierunek: Automatyka i Robotyka (AiR)Specjalno±¢: Robotyka (ARR)

PRACA DYPLOMOWAMAGISTERSKA

Algorytmy sterowaniamanipulatorów mobilnych

Autor:Jarosªaw Szrek

Opiekun pracy:Prof. dr hab. in». Krzysztof Tcho«

Prowadz¡cy prac¦:Prof. dr hab. in». Krzysztof Tcho«

Ocena pracy:

Wrocªaw 2004

Panuprof. dr hab. in». Krzysztofowi Tchoniowipodzi¦kowania za pomoc i ukierunkowaniew sposobie opisu bada« przyczyniaj¡cesi¦ do podniesienia merytorycznej warto±ci pracy.Równocze±nie dzi¦kuj¦Panu dr in». Markowi Wnukowiza po±wi¦cony czas i pomoc przy budowie ukªadusterowania modelu manipulatora mobilnego.

1

Spis tre±ci

1 Kinematyka manipulatora mobilnego 71.1 Endogeniczna przestrze« kon�guracujna . . . . . . . . . . . . . . . . 71.2 Kinematyka . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71.3 Wyprowadzenie równa« kinematyki platformy . . . . . . . . . . . . . 81.4 Funkcja wyj±cia . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

2 Algorytm kinematyki odwrotnej 132.1 Jakobian pseudoodwrotny . . . . . . . . . . . . . . . . . . . . . . . . 132.2 Algorytm sterowania . . . . . . . . . . . . . . . . . . . . . . . . . . . 142.3 Dyskretna posta¢ algorytmu . . . . . . . . . . . . . . . . . . . . . . . 14

3 Konstrukcja manipulatora mobilnego 173.1 Ogólna charakterystyka . . . . . . . . . . . . . . . . . . . . . . . . . . 173.2 Silniki robota . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 173.3 Nap¦d i skr¦t platformy . . . . . . . . . . . . . . . . . . . . . . . . . 183.4 Manipulator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 183.5 Chwytak . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 213.6 Sterownik . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 223.7 Jednostka centralna . . . . . . . . . . . . . . . . . . . . . . . . . . . . 233.8 Sensoryka . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

3.8.1 Pomiar poªo»enia . . . . . . . . . . . . . . . . . . . . . . . . . 233.8.2 Pozycjonowanie pocz¡tkowe . . . . . . . . . . . . . . . . . . . 23

3.9 Ukªad wykonawczy dla silników . . . . . . . . . . . . . . . . . . . . . 253.9.1 Cyfrowy ukªad sterowania . . . . . . . . . . . . . . . . . . . . 253.9.2 Ukªad wyzwalania H-mostka . . . . . . . . . . . . . . . . . . . 263.9.3 Mostek typu H . . . . . . . . . . . . . . . . . . . . . . . . . . 27

3.10 Zasilanie robota . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 293.11 Dane techniczne robota . . . . . . . . . . . . . . . . . . . . . . . . . . 293.12 Opis poª¡cze« moduªów elektronicznych . . . . . . . . . . . . . . . . 303.13 Opis funkcji peªnionych przez kanaªy TPU

i porty mikrokontrolera . . . . . . . . . . . . . . . . . . . . . . . . . . 32

4 Badania symulacyjne 344.1 Wyniki dziaªania algorytmu . . . . . . . . . . . . . . . . . . . . . . . 35

2

5 Badania eksperymentalne 395.1 Algorytm ±ledzenia . . . . . . . . . . . . . . . . . . . . . . . . . . . . 395.2 Programowa idea dziaªania algorytmu ±ledzenia poªo»enia . . . . . . . 395.3 Wst¦pne testy na robocie . . . . . . . . . . . . . . . . . . . . . . . . . 405.4 Eksperymentalne sprawdzenie algorytmu

kinematyki odwrotnej . . . . . . . . . . . . . . . . . . . . . . . . . . . 40

3

Spis rysunków

1 Manipulator typu 2R na platformie mobilnej . . . . . . . . . . . . . . 5

1.1 Warunek braku bocznego po±lizgu kóª tylnych . . . . . . . . . . . . . 91.2 Warunek braku bocznego po±lizgu koªa przedniego . . . . . . . . . . . 91.3 Platforma mobilna w zewn¦trznym ukªadzie wspóªrz¦dnych . . . . . . 111.4 Widok manipulatora . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

3.1 Koªo przednie z silnikiem nap¦dowym i skr¦caj¡cym -widok z boku . 193.2 Silnik steruj¡cy skr¦tem -widok z góry . . . . . . . . . . . . . . . . . 193.3 Przekªadnia dla dolnego ramienia manipulatora . . . . . . . . . . . . 203.4 Przekªadnia dla górnego ramienia manipulatora . . . . . . . . . . . . 203.5 Chwytak dwupalczasty . . . . . . . . . . . . . . . . . . . . . . . . . . 213.6 Schemat blokowy ukªadu sterowania robota . . . . . . . . . . . . . . 223.7 Schemat ideowy kodera . . . . . . . . . . . . . . . . . . . . . . . . . . 243.8 Zasada pomiaru za pomoc¡ kodera . . . . . . . . . . . . . . . . . . . 243.9 Schemat blokowy ukªadu sterowania silnikiem . . . . . . . . . . . . . 263.10 Schamat ideowy cyfrowej cz¦±ci ukªadu sterowania mostkiem . . . . . 273.11 Schamat ideowy ukªadu sterowania mostkiem . . . . . . . . . . . . . 283.12 Schemat blokowy poª¡cze« moduªów robota . . . . . . . . . . . . . . 33

4.1 Rozmieszczenie punktów startowych w ukªadzie wspóªrz¦dnych . . . . 344.2 Wynik dziaªania algorytmu dla punktu startowego A . . . . . . . . . 354.3 Wynik dziaªania algorytmu dla punktu startowego B . . . . . . . . . 364.4 Wynik dziaªania algorytmu dla punktu startowego C . . . . . . . . . 364.5 Wynik dziaªania algorytmu dla punktu startowego D . . . . . . . . . 374.6 Wynik dziaªania algorytmu dla punktu startowego E . . . . . . . . . 374.7 Wynik dziaªania algorytmu dla punktu startowego F . . . . . . . . . 384.8 Wynik dziaªania algorytmu dla punktu startowego H . . . . . . . . . 38

5.1 K¡t skr¦tu przedniego koªa w caªym horyzoncie czasowym . . . . . . 415.2 K¡t skr¦tu przedniego koªa w pocz¡tkowej fazie ruchu . . . . . . . . . 415.3 Droga przebyta przez robota w caªym horyzoncie czasowym . . . . . 425.4 Droga przebyta przez robota w pocz¡tkowej fazie ruchu . . . . . . . . 425.5 Widok skonstruowanego manipulatora mobilnego . . . . . . . . . . . 43

4

Wprowadzenie

Post¦p techniczny w wielu dziedzinach nauki, dynamicznie rozwijaj¡ce si¦ bada-nia nad robotami oraz wykorzystanie techniki mikroprocesorowej umo»liwiaj¡ bu-dow¦ coraz lepszych urz¡dze« robotycznych. Podwy»szanie jako±ci i niezawodno±cisterowania, pozwalaj¡ce wykonywa¢ zaprogramowane ruchy znalazªo zastosowanierobotów w ró»nych dziedzinach »ycia. Roboty pomagaj¡ czªowiekowi w ci¦»kich iniebezpiecznych dla zdrowia pracach jak malowanie, spawanie oraz przy wykonywa-niu precyzyjnych czynno±ci np. w medycynie.

Mo»liwo±¢ samodzielnego przemieszczania si¦ robota daje mu szersze mo»liwo±cizastosowa«. Urz¡dzenie skªadaj¡ce si¦ z manipulatora zamontowanego na platformiemobilnej pozwala wykonywa¢ bardziej wyszukane zadania. Manipulatory mobilnemog¡ pomóc czªowiekowi w transporcie, usªugach, opiece nad osobami starszymi lubniepeªnosprawnymi.

Rysunek 1: Manipulator typu 2R na platformie mobilnej

Stosowanie manipulatorów mobilnych generuje potrzeby opracowywania wielutypów algorytmów sterowania. Algorytm sterowania manipulatorem mobilnym lub

5

inaczej algorytm planowania ruchu robota ma na celu wyznaczenie pewnych od-dziaªywa« przeprowadzaj¡cych ukªad z kon�guracji pocz¡tkowej do zadanej kon-�guracji ko«cowej. Zakªadaj¡c nieholonomiczne podej±cie do ukªadu robotycznegozadnie sterowania robotem mo»na podzieli¢ na dwa etapy. Pierwszy etap polegana planowaniu trajektorii ruchu robota, a drugi etap dotyczy ±ledzenia zadanej tra-jektorii realizowane przez ukªad sterowania [2]. �ledzenie trajektorii wyznaczonejw pierwszym etapie powinno doprowadzi¢ robota do celu.

Celem bada« niniejszej pracy jest analiza algorytmu kinematyki odwrotnej orazeksperymentalne sprawdzenie mo»liwo±ci jego implementacji na rzeczywistym mo-delu robota. Obiektem bada« jest manipulator mobilny widoczny na rysunku (1).Robot skªada si¦ z trzykoªowej platformy mobilnej klasy (1,1) i dwuramiennegomanipulatora o dwóch przegubach rotacyjnych. W pracy wykonano teoretycznewyliczenia kinematyki rozwa»anego robota. Przedstawiono algorytm kinematykiodwrotnej i przetestowano algorytm symulacyjnie. Dodatkowo skonstruowano �-zyczny model manipulatora mobilnego. Wykonany robot umo»liwiª przeprowadzeniena nim zamierzonych bada« eksperymentalnych. Wcze±niejsze badania algorytmówkinematycznych [3] i numerycznych metod planowania ruchu robota [4] pozwoliªyna odpowiednie dobranie elementów algorytmu, aby umo»liwi¢ implementacje nasterowniku robota.

Ukªad pracy jest nast¦puj¡cy: W rozdziale pierwszym zawarto teoretyczne pod-stawy dotycz¡ce kinematyki robota w przestrzeni endogeniczniej [1] oraz wypro-wadzono równania kinematyki rozwa»anego manipulatora mobilnego. Nast¦pnie,w rozdziale drugim, opisano badany algorytm kinematyki odwrotnej, a w rozdzialeczwartym przedstawiono wyniki bada« symulacyjnych tego algorytmu. Rozdziaªtrzeci po±wi¦cono opisowi budowy mechanicznej i elektronicznej skonstruowanegomodelu manipulatora mobilnego. Eksperymentalne wyniki dziaªania algorytmu narzeczywistym obiekcie przedstawiono w rozdziale pi¡tym.

6

Rozdziaª 1

Kinematyka manipulatora mobilnego

1.1 Endogeniczna przestrze« kon�guracujnaPrzyjmijmy, »e manipulator mobilny skªada si¦ z nieholonomicznej platformy mobil-nej oraz z zamontowanego na niej stacjonarnego manipulatora. Platforma posiadan uogólnionych wspóªrz¦dnych stanu oraz m wej±¢ steruj¡cych. Oznaczmy stanplatformy jako q ∈ Rn, wektor wej±¢ jako u ∈ Rm, wektor poªo»e« przegubów ma-nipulatora jako x ∈ Rp. Dodatkowo oznaczmy wektor wspóªrz¦dnych zadaniowychprzez y ∈ Rr. Przy tak przyj¦tych oznaczeniach kinematyka manipulatora mobil-nego mo»e by¢ reprezentowana przez ukªad sterowania z wyj±ciem

{q = G(q)uy = k(q, x).

(1.1)

Przez analogi¦ do stacjonarnych manipulatorów, gdzie kon�guracja zwi¡zana jestz zadawanym poªo»eniem przegubów, w manipulatorch mobilnych kon�guracja b¦-dzie zwi¡zana ze zmiennymi steruj¡cymi.

Endogeniczn¡ przestrzeni¡ kon�guracyjn¡ b¦dzie nazywana przestrze« okre±lonaprzez X = L2

m[0, T ] × Rp, gdzie zbiór funkcji steruj¡cych platform¡ L2m[0, T ] jest

przestrzeni¡ funkcji caªkowalnych z kwadratem na pewnym przedziale czasu [0, T ],natomiast Rp oznacza poªo»enie przegubów manipulatora. Endogeniczna przestrze«kon�guracyjna jest przestrzeni¡ Hilberta z norm¡

‖(u(·), x)‖RW =

(∫ T

OuT (t)R(t)u(t)dt+ xTWx

)1/2

,

dla pewnych dodatnio okre±lonych i symetrycznych macierzy R(t) = RT (t) > 0,W =W T > 0.

1.2 KinematykaUstalaj¡c poªo»enie pocz¡tkowe plaformy q0 ∈ Rn i wybieraj¡c kon�guracj¦ endo-geniczn¡ (u(·), x) ∈ X, wyliczamy trajektori¦ q(t) = ϕq0,t(u(·)) platformy oraz tra-jektori¦ w przestrzeni zadaniowej y(t) = k(q(t), x). Zakªadamy, »e obie trajektorie

7

s¡ zde�niowane dla ka»dego t ∈ [0, T ]. Wyj±cie systemu (1.1) (poªo»enie i orientacjaefektora) w chwili T

Kq0,T : X −→ Rr,

jest okre±lone przez transformacj¦ kon�guracji endogenicznej we wspóªrz¦dne zada-niowe

Kq0,T (u(·), x) = y(T ) = k(ϕq0,T (u(·)), x). (1.2)Tak zde�niowana funkcja b¦dzie nazywana kinematyk¡ manipulatora mobilnego.Odpowiednikiem jakobianu analitycznego manipulatora mobilnego jest pochodnakinematyki,

Jq0,T (u(·), x)(v(·), w) =d

dα|α=0 Kq0,T (u(·) + αv(·), x+ αw) =

C(T, x)∫ T

0Φ(T, s)B(s)v(s)ds+D(T, x)w, (1.3)

której warto±¢ jest wyj±ciem η(T ) ukªadu wariacyjnego{ξ = A(t)ξ +B(t)v,η = C(t, x) +D(t, x)w

(1.4)

stowarzyszonego z ukªadem (1.1), obliczona dla ξ(0) = 0, gdzie

B(t) = G(q(t)), A(t) = ∂∂q

(G(q(t))u(t)),

C(t, x) = ∂k(q(t),x)∂q

, D(t, x) = ∂k(q(t),x)∂x

.

Wyst¦puj¡ca w równaniu(1.3) macierz fundamentalna Φ(t, s) speªnia równanie∂∂t

Φ(t, s) = A(t)Φ(t, s), oraz posiada wªasno±¢ Φ(s, s) = In.

1.3 Wyprowadzenie równa« kinematyki platformyRozwa»my platform¦ mobiln¡ klasy (1,1) [6], posiadaj¡c¡ jedno koªo przednie i dwatylne. Koªa tylne zamocowane s¡ po bokach platformy, natomiast koªo przedniezapewniaj¡ce nap¦d i skr¦t robota znajduje si¦ w poªowie szeroko±ci nadwozia.Platforma jest opisana wektorem zmiennych stanu q = (x, y, ϕ, θ). Parametry x, yokre±laj¡ wspóªrz¦dne ±rodka tylnej osi, a ϕ i θ s¡ odpowiednio orientacj¡ robotai k¡tem skr¦tu przedniego koªa. Na rysunku (1.1) przedstawiony jest schematycznywidok platformy z oznaczonymi parametrami. Odlegªo±¢ przedniego koªa od tylnejosi jest oznaczona przez l. Formalnie, kinematyk¦ robota mobilnego charakteryzujesi¦ przez ukªad sterowania postaci (1.1) gdzie q ∈ Rn, u ∈ Rm,m = n− s. Parametrs oznacza ilo±¢ niezale»nych ogranicze« nieholonomicznych. Wyznaczenie mode-lu kinematyki wymaga zaªo»enia o braku po±lizgu poprzecznego kóª tylnych orazkoªa przedniego. Warunek ten, w przypadku kóª tylnych, mo»na zapisa¢ jednymrównaniem, które odpowiada brakowi po±lizgu bocznego punktu o wspóªrz¦dnych

8

Xo

Yo

yy

ll

ϕϕ

xx

ϕϕ

ϕϕ

θθηη

ξξ

ycosϕϕ··

··xsinϕϕ

xx

yy

··

··

Rysunek 1.1: Warunek braku bocznego po±lizgu kóª tylnych

Xo

Yo

ll

ξ=x+lcosϕϕ

ηη==y+lsinϕϕηη=cos(ϕ+θ)

((ϕ+θ)ξ=sin(ϕ+θ)

ϕϕ

θθ

··

ηη

ξξ

··

··

··((ϕ+θ)

Rysunek 1.2: Warunek braku bocznego po±lizgu koªa przedniego

9

(x,y), b¦d¡cego ±rodkiem tylnej osi. Oznacza to, »e skªadowe wektora pr¦dko±ciprostopadªe do powierzchni kóª musz¡ mie¢ warto±¢ równ¡ zero,

x sinϕ− y cosϕ = 0. (1.5)

Analogicznie, warunek na brak polizgu bocznego koªa przedniego mo»na zapisa¢jako (zob. rys 1.2)

ξ sin(ϕ+ θ)− η cos(ϕ+ θ) = 0, (1.6)gdzie

ξ = x+ l cosϕ, ξ = x− lϕ sinϕ, η = y + l sinϕ, η = y + lϕ cosϕ.

Ostatecznie, po podstawieniu otrzymujemy:

(x− lϕ sinϕ sin(ϕ+ θ)− (y + lϕ cosϕ cos(ϕ+ θ) = 0.

Po kolejnych przeksztaªceniach mamy

x cosϕ sin θ + y sinϕ sin θ − lϕ cos θ = 0. (1.7)

Ograniczenia (1.5) oraz (1.7) mo»na zapisa¢ macierzowo w postaci Pfa�a

A(q)q =[

sinϕ − cosϕ 0 0cosϕ sin θ sinϕ sin θ −l cos θ 0

]

xyϕθ

. (1.8)

Aby otrzyma¢ pierwsze równanie ukªadu (1.1) nale»y znale¹¢ pola wektorowe g1(q),g2(q) nale»¡ce do j¡dra macierzy A(q). �atwo sprawdzi¢, »e takie warunki speªniaj¡pola

g1(q) =

l cosϕ cos θl sinϕ cos θ

sin θ0

, g2(q) =

0001

.

Ostatecznie, ukªad sterowania reprezentuj¡cy kinematyk¦ platformy przyjmujeposta¢

x = u1l cosϕ cos θy = u1l sinϕ cos θϕ = u1 sin θ

θ = u2.

(1.9)

1.4 Funkcja wyj±ciaOkre±lenie poªo»enia i orientacji efektora w ukªadzie globalnym w zale»no±ci odkon�guracji przegubów manipulatora mo»na wyliczy¢ przy pomocy zªo»enia dwóchtransformacji. Pierwsza

Ap0 = Trans(X, x)Trans(Y, y)Rot(Z,ϕ)Trans(X, d)

10

Xo

Zo

Yo

yy

xx

dd

ll

ϕϕ

θθ

Rysunek 1.3: Platforma mobilna w zewn¦trznym ukªadzie wspóªrz¦dnych

lo

x1

x2

l1

l2

Rysunek 1.4: Widok manipulatora

11

okre±la poªo»enie oraz orientacj¦ punktu platformy, w którym jest zamontowanapodstawa manipulatora (Rysunek 1.3). Druga transformacja

Aep = Trans(Z, l0)Rot(X,π

2)Rot(Z, x1)Trans(X, l1)Rot(Z, x2)Trans(X, l2)

wyznacza poªo»enie i orientacj¦ efektora wzgl¦dem punktu zamontowania manipu-latora na platformie (Rysunek 1.4). Iloczyn tych dwóch przeksztaªce« wyznaczapoªo»enie i orientacj¦ efektora wzgl¦dem ukªadu globalnego

Ae0 =

cϕc12 −cϕs12 sϕ x+ cϕ(d+ l1c1 + l2c12)sϕc12 −sϕs12 −cϕ y + sϕ(d+ l1c1 + l2c12)s12 c12 0 l0 + l1s1 + l2s12

0 0 0 1

,

gdzie s12 = sin(x1+x2), c12 = cos(x1+x2). Przy zaªo»eniu, »e interesuje nas poªo»enieefektora, funkcja wyj±cia jest ostatni¡ kolumn¡ macierzy Ae0 i ostatecznie przyjmujepostac

y = k(q, x) =

x+ cϕ(d+ l1c1 + l2c12)y + sϕ(d+ l1c1 + l2c12)

l0 + l1s1 + l2s12

. (1.10)

Poª¡czenie wyra»e« (1.9) oraz (1.10) reprezentuje kinematyk¦ manipulatora mobil-nego w postaci ukªadu sterowania z wyj±ciem.

12

Rozdziaª 2

Algorytm kinematyki odwrotnej

2.1 Jakobian pseudoodwrotnyNiech Jq0,T (u(·), x)(v(·), w) oznacza wcze±niej zde�niowany jakobian analityczny (1.3).Przy ustalonej kon�guracji endogenicznej (u(·), x) rozwa»my równanie

Jq0,T (u(·), x)(v(·), w) = η (2.1)dla danego η ∈ Rr. Rozwi¡zanie tego równania uzyskamy metod¡ najmniejszychkwadratów. Jest to równowa»ne minimalizacji kwadratu normy

min(v(·),w)

‖(v(·), w)‖2RW ,

z uwzgl¦dnieniem warunku równo±ciowego (2.1). W tym celu zde�niujemy lagran»ian

L(v(·), w, λ) =

(∫ T

0vT (t)R(t)v(t)dt+ wTWw

)+

λT(C(T, x)

∫ T

0Φ(T, t)B(t)v(t)dt+D(T, x)w − η

),

gdzie λ = (λ1, . . . , λr) jest wektorem mno»ników Lagrange'a. Po wyznaczeniu ekstre-mum lagran»ianu i eliminacji mno»ników Lagrange'a uzyskujemy rozwi¡zanie rów-nania (2.1) w postaci(v(t)w

)=(J#q0,T

(u(·), x)η)(t) =

[R−1(t)BT (t)ΦT (T, t)CT (T, x)

W−1DT (T, x)

]D−1q0,T

(u(·), x)η,

(2.2)gdzie

Dq0,T (u(·), x) = D(T, x)W−1DT (T, x) +

C(T, x)∫ T

0Φ(T, s)B(s)R−1(s)BT (s)ΦT (T, s)dsCT (T, x).

Równanie (2.2) okre±la prawostronn¡ pseudoodwrotno±¢ (odwrotno±¢ Moore'a -Pen-rose'a) jakobianu, wi¦c prawdziwa jest równo±¢

Jq0,T (u(·), x)J#q0,T

(u(·), x) = Ir. (2.3)Nietrudno sprawdzi¢, »e równanie (2.1) b¦dzie speªnione, je»eli do rozwi¡zania (2.2)zostanie dodany dowolny element ξ ∈ KerJq0,T (u(·), x).

13

2.2 Algorytm sterowaniaOdwrotne zadanie kinematyki dla robota mobilnego polega na znalezieniu takiejkon�guracji endogenicznej (u(·), x), aby przeprowadzi¢ ukªad z poªo»enia pocz¡tko-wego q0, x0 do zadanego punktu docelowego yd w zadanym horyzoncie czasowym T.Zadanie b¦dzie uwa»ane za rozwi¡zane, gdy

Kq0,T (u(·), x) = yd.

Przyjmijmy, »e zadanie odwrotne posiada rozwi¡zanie. Aby je znale¹¢, nale»ywybra¢ pewn¡ krzyw¡ (uθ(·), x(θ)), θ ∈ R, w endogenicznej przestrzeni kon�gu-racyjnej, startuj¡c z pewnej pocz¡tkowej kon�guracji (u0(·), x0).Wzdªu» tej krzywejobliczamy bª¡d

e(θ) = Kq0,T (uθ(·), x(θ))− yd. (2.4)Gdy warto±¢ pocz¡tkowa bª¦du e(0) 6= 0, b¦dziemy chcieli wybra¢ tak¡ krzyw¡,aby wzdªu» krzywej bª¡d zmniejszaª si¦ eksponencjalnie do zera. Eksponencjaln¡zbie»no±¢ bª¦du chcemy osi¡gn¡¢ dla pewnego wspóªczynnika γ > 0, takiego »e

d

dθe(θ) = −γe(θ). (2.5)

Po niezb¦dnych przeksztaªceniach i uwzgl¦dnieniu de�nicji jakobianu analitycznego(1.3), z równania (2.5) otrzymujemy nast¦puj¡ce równanie ró»niczkowe

Jq0,T (uθ(·), x(θ))d

dθ

(uθ(·)x(θ)

)= −γe(θ). (2.6)

Przy zaªo»eniu, »e jakobian robota mobilnego Jq0,T (u(·), x) jest suriekcj¡ wzdªu»krzywej (uθ(·), x(θ)), mo»na zapisa¢ algorytm kinematyki odwrotnej z wykorzys-taniem jakobianu pseudoodwrotnego

d

dθ

(uθ(t)x(θ)

)= −γ

(J#q0,T

(uθ(·), x(θ))e(θ))(t) =

−γ[R−1(t)BT

θ (t)ΦTθ (T, t)CT

θ (T, x)W−1DT

θ (T, x)

]D−1q0,T

(uθ(·), x(θ))e(θ). (2.7)

Rozwi¡zanie odwrotnego zadania kinematyki przedstawi¢ mo»na jako granic¦ w +∞,trajektorii b¦d¡cej rozwi¡zaniem równania (2.7)

(ud(t)xd

)= lim

θ→+∞

(uθ(t)x(θ)

). (2.8)

2.3 Dyskretna posta¢ algorytmuKinematyka manipulatora mobilnego jest okre±lona w postaci ukªadu sterowania(1.1). Przyjmujemy kon�guracj¦ pocz¡tkow¡ (u0(·), x0), punkt docelowy yd oraz

14

interwaª sterowania [0, T ]. Maj¡c dany jakobian analityczny, mo»na obliczy¢ stero-wanie pozwalaj¡ce osi¡gn¡¢ yd w zadanym czasie T wedªug formuªy

(ui+1(·)xi+1(·)

)=(uixi

)− γJ#

q0,T(ui(·), xi)ei, (2.9)

któr¡ uzyskano w wyniku dyskretyzacji (2.7). Je±li punkt docelowy yd zostaª osi¡-gni¦ty w kroku i-tym, to Kq0,T (ui(·), xi) − yd = 0 i odwrotne zadanie kinematykimo»na uwa»a¢ za rozwi¡zane. W przeciwnym przypadku sterowanie ui(·) jest mody-�kowane zgodnie ze wzorem (2.9). Ze wzgl¦dów obliczeniowych, a tak»e z uwagi na�zyczne wªasno±ci nap¦dów robota przyjmuje si¦ sterowanie w postaci sko«czonegoszeregu Fouriera

ui(t) = λi0 +si∑

k=1

λi2k−1 sin kωt+ λi2k cos kωt,

gdzie i = 1, 2, . . . ,m, ω = 2π/T. Powy»sze równanie mo»na zapisa¢ w postaci

u(t) = P (t)λ,

gdzie macierz P (t) =

1 sinωt . . . cos s1ωt 0 0 . . . 0 0 0 . . . 00 0 . . . 0 1 sinωt . . . cos s2ωt 0 0 . . . 0... ...0 0 · · · 0 0 · · · 0 0 1 sinωt · · · cos smωt

tworzy baz¦ przestrzeni funkcyjnej, a λ = (λ1, λ2, · · · , λm) jest wektorem wspóªczyn-ników rozwini¦cia. Ostatecznie dyskretn¡ posta¢ algorytmu sterowaniao ograniczo-nym pa±mie cz¦stotliwo±ci mo»na zapisa¢ jako

(λk+1

xk+1

)=(λkxk

)− γJ#

q0,T(λk, xk)ek. (2.10)

Wystepuj¡ca w (2.10) macierz J#q0,T

stanowi pseudoodwrotno±¢ jakobianu analitycz-nego

Jq0,T (λk, xk) = [Ck(T, xk)∫ T0 Φk(T, s)Bk(s)P (s)ds Dk(T, xk) ] , (2.11)

która jest liczona zgodnie z formuª¡A# = AT (AAT )−1.Wyst¦puj¡ce w (2.7) macierzeR i W wybieramy jako staªe i jednostkowe. Podczas dziaªania algorytmu wektor λb¦dzie aktualizowany, a» do osi¡gni¦cia punktu yd. Punkt docelowy mo»na uzna¢ zaosi¡gni¦ty, gdy bª¡d de�niowany jako ró»nica poªo»enia efektora i punktu zadanego,ek = k(qk(T ), xk) − yd b¦dzie mniejsza od pewnej bliskiej zeru warto±ci. Za szy-bko±¢ zbie»no±ci algorytmu odpowiada parametr γ > 0. Przed rozpocz¦ciem dzia-ªania algorytmu ustalane s¡ warto±ci pocz¡tkowe. Do zmiennych zadeklarowanychw programie wpisywana jest kon�guracja pocz¡tkowa q0, punkt docelowy yd, czassterowania T oraz pocz¡tkowe warto±ci wspóªczynników sterowania λ. Nast¦pnie

15

wykonywane s¡ obliczenia stanu ukªadu w zadanym horyzoncie czasowym, przezwykonanie numerycznego caªkowania równa« (1.9). Do caªkowania zastosujemymetod¦ Eulera ze staªym krokiem. Potrzebne jest te» wyliczenie macierzy prze-j±cia stanu Φ(T, s) wyst¦puj¡cej w jakobianie analitycznym [4]. Macierz Φ(t, s) jestrozwi¡zaniem równania

∂

∂tΦ(t, s) = A(t)Φ(t, s).

Przybli»one rozwi¡zanie powy»szego równania mo»na uzyska¢ korzystaj¡c z wªa-sno±ci macierzy Φ(t, s). Ró»niczkuj¡c wyra»enie Φ(t, s)Φ(s, t) = In po zmiennejs i uproszczeniu otrzymujemy

∂

∂sΦ(t, s) = −Φ(t, s)A(s),

a nast¦pnie∂

∂tΦ(T, t) = −Φ(T, t)A(t).

Przybli»aj¡c pochodn¡ ilorazem ró»nicowym

Φ(T, tk)− Φ(T, tk−1)

∆= −Φ(T, tk)A(tk),

ostatecznie otrzymujemy formuª¦ aproksymuj¡c¡ macierz przej±cia

Φ(T, tk−1) = Φ(T, tk)(In + A(tk)∆),

gdzie ∆ jest odlegªo±ci¡ pomi¦dzy kolejnymi chwilami czasu, a A(t) jest podaneanalitycznie. Dla macierzy przej±cia prawdziwe jest Φ(T, T ) = In.

Je»eli punkt docelowy zostaª osi¡gni¦ty z zadan¡ dokªadno±ci¡, to algorytmko«czy dziaªanie. W przeciwnym razie nast¦puje mody�kacja parametrów sterowa-nia wedªug (2.10) i wykonanie oblicze« od pocz¡tku. W implementacji algorytmubaza przestrzeni funkcyjnej zostaªa ograniczona do skªadowych o cz¦stotliwo±ci pod-stawowej i przyjmuje posta¢

P (t) =[

1 sinωt cosωt 0 0 00 0 0 1 sinωt cosωt

].

Liczba mody�kacji parametrów sterowa« zostaªa ograniczona do 500. Pozostaªeelementy potrzebne w algorytmie s¡ wyliczane funkcjami, które generuj¡ odpowied-nie macierze w zale»no±ci od parametrów wej±ciowych. Symulacje oraz algorytmwykonano przy u»yciu pakietu Matlab.

16

Rozdziaª 3

Konstrukcja manipulatora mobilnego

3.1 Ogólna charakterystykaBaz¦ robota stanowi aluminiowa pªyta w ksztaªcie prostok¡ta, w której wykonanowyci¦cia na koªa. Robot jest klasy (1,1) i posiada trzy koªa: jedno przednie i dwatylne. Koªo przednie, umieszczone w poªowie szeroko±ci robota, realizuje nap¦di skr¦t. Koªa tylne zostaªy zamontowane na bokach platformy. Koªa zaadaptowanoz wózków dzieci¦cych i przerobiono stosownie do potrzeb projektu (centrowanieotworów i dopasowanie sworzni stanowi¡cych ªo»yska ±lizgowe kóª). Przednie zawie-szenie wykonano z pªaskowników stalowych, które zapewniaj¡ odpowiedni¡ wytrzy-maªo±¢. K¡t skr¦tu koªa przedniego jest ograniczony �zycznie i wynosi ±π

2.

W centralnym, punkcie platformy osadzono podstaw¦ manipulatora. Manipu-latorem jest dwuwahadªo, na ko«cu którego znajduje si¦ efektor. Ramiona mani-pulatora zostaªy wykonane z ksztaªtowników aluminiowych. Wysoko±¢ postumentumierzona od powierzchni platformy wynosi 20cm. Dªugo±ci ka»dego z ramion ma-nipulatora wynosi 40cm. Platforma ma 30cm szeroko±ci i 45cm dªugo±ci. Elementykonstrukcyjne i nap¦dowe zaadaptowano z urz¡dze« codziennego u»ytku, co poz-woliªo na znaczne obni»enie koszów robota.

3.2 Silniki robotaRobot posiada cztery silniki elektryczne pr¡du staªego sprz¦»one z przekªadniamiplanetarnymi. Napi¦cie zasilania silników pochodz¡cych od wiertarko-wkr¦tarekwynosi 14.4V. U»yte zespoªy nap¦dowe posiadaj¡ odpowiedni¡ moc, a ich kosztjest niewielki w porównaniu z silnikami profesjonalnymi. Typowe dla wiertarko-wkr¦tarek nap¦dy przed zamontowaniem do robota nale»aªo odpowiednio zmody-�kowa¢. Z waªków wyj±ciowych przekªadni planetarnych usuni¦to sprz¦gªa ciernei osadzono na nich z¦batki wspóªpracuj¡ce z przekªadniami. Nast¦pnie z ksztaªto-wników stalowych wykonano uchwyty umo»liwiaj¡ce zamocowanie silników do ro-bota. Dodatkowo na osiach silników nale»aªo zamontowa¢ czujniki poªo»enia. Abyumo»liwi¢ zamontowanie czujników, do osi silników przymocowano wykonane w tymcelu sprz¦gªa o takiej ±rednicy jak sprz¦gªo sensora. W poni»szej tabeli zestawiono

17

parametry silnika nap¦dowego z przekªadni¡ planetarn¡.

napi¦cie zasilania [V] 14.4pobór pr¡du (bez obci¡»enia slinika ) [A] 1.6

pr¡d rozruchu [A] 12ilo±¢ obrotów silnika [min−1] 19800

przeªo»enie przekªadni planetarnej silnika 36:1

3.3 Nap¦d i skr¦t platformySkr¦t jest realizowany przez dwie sprz¦»one ze sob¡ przekªadnie. Pierwsza z nichobni»a obroty, a druga, k¡towa zapewnia dodatkowo mo»liwo±¢ prostopadªego zamo-cowania silnika na osi skr¦tnej koªa (zob. Rysunek 3.1). Elementy do pierwszejprzekªadni pochodz¡ od urz¡dzenia gospodarstwa domowego. Obie przekªadnieumieszczone jedna za drug¡ zapewniaj¡ obni»enie obrotów silnika (z przekªadni¡planetarn¡) okoªo sze±¢ razy. Pozwala to na dokªadniejsze pozycjonowanie skr¦tukoªa platformy. Dodatkowo, zastosowanie przekªadni k¡towej pozwoliªo na obni»eniewysoko±ci kolumny skr¦caj¡cej, co wyeliminowaªo kolizj¦ z manipulatorem. Na wyj-±ciowym waªku przekªadni k¡towej umieszczone s¡ wideªki, do których zamontowanokoªo oraz nap¦dzaj¡cy je silnik. Nap¦d przedniego koªa odbywa si¦ za po±rednictwempaska z¦batego. �rednica koªo z¦batego na osi silnika nap¦dowego jest mniejsza od±rednicy koªa nap¦dzaj¡cego robota, co pozwala na dodatkowe obni»enie obrotów(zob. Rysunek 3.1). Kolumna przedniego koªa jest przymontowana do platformy zapo±rednictwem pªaskownika stalowego wypro�lowanego w ªuk, który stanowi zawie-szenie ukªadu odpowiedzialnego za nap¦d i skr¦t.

3.4 ManipulatorManipulator robota jest typu 2R. Skªada si¦ on z dwóch ramion o dªugo±ci 40cm osa-dzonych na podstawie. Do nap¦du ramion wahadªa u»yto takich samych silników jakw platformie. Siªy potrzebne do wykonania ruchu ramionami manipulatora s¡ niecowi¦ksze od siª potrzebnych do wykonania ruchu platformy, dlatego wykonano do-datkowe przekªadnie. Czterostopniowe przekªadnie z¦bate obni»aj¡ pr¦dko±¢ obro-tow¡ silników obu ramion manipulatora, jednocze±nie zwi¦kszaj¡c moment siªy nawyj±ciu ukªadu nap¦dowego ramion. Przekªadnia nap¦dzaj¡ca pierwsze rami¦ ma-nipulatora zostaªa wykonana tak, aby mo»liwe byªo umieszczenie jej wewn¡trz pod-stawy. Na ostatniej osi przekªadni, znajduj¡cej si¦ przy szczycie podstawy, zamon-towano pierwsze rami¦ manipulatora. Elementem no±nym pierwszego ramienia s¡dwa równolegle poª¡czone ksztaªtowniki. Pomi¦dzy ksztaªtownikami zamontowanokoªa z¦bate przekªadni dla drugiego ramienia (rys. 3.4). Takie rozwi¡zanie zapro-ponowano, aby silnik byª zamontowany mo»liwie blisko przegubu ª¡cz¡cego pierwszerami¦ z postaw¡. Dzi¦ki takiemu rozwi¡zaniu mo»na sterowa¢ drugim ramieniemmanipulatora, niezale»ne od pierwszego. Ponadto powoduje to przesuni¦cie ±rodkamasy bli»ej podstawy, co ma znaczenie ze wzgl¦du na siª¦ (moment) potrzebn¡ do

18

silnik napedowy

silnik skretny

pasek zebaty

przekladnia katowa

Rysunek 3.1: Koªo przednie z silnikiem nap¦dowym i skr¦caj¡cym -widok z bokuprzekladnia katowa

Rysunek 3.2: Silnik steruj¡cy skr¦tem -widok z góry

19

Rysunek 3.3: Przekªadnia dla dolnego ramienia manipulatora

Rysunek 3.4: Przekªadnia dla górnego ramienia manipulatora20

zapewnienia ruchu ramienia. Do ostatniej osi przekªadni znajduj¡cej si¦ na ko«cupierwszego ramienia zamocowano uchwyt dla drugiego ramienia manipulatora, którejest wykonane z ksztaªtownika aluminiowego. Na ko«cu drugiego ramienia przygo-towano miejsce do zamontowania chwytaka. Koªa z¦bate obu przekªadni pochodz¡z urz¡dze« AGD, które odpowiednio przerobiono stosownie do potrzeb (zmniejszonoszeroko±¢ kóª, wytulejowano otwory).

3.5 ChwytakPodstawowym zadaniem chwytaka jest uchwycenie obiektu i uwolnienie go, po prze-niesieniu przez robota do miejsca docelowego. Z wielu mo»liwych typów chwytakówwybrano jedno z najprostszych rozwi¡za« i wykonano chwytak dwupalczasty. Ele-mentem wykonawczym jest elektromagnes. Pojawienie si¦ napi¦cia na uzwoje-niach cewki powoduje wci¡gni¦cie rdzenia elektromagnesu poª¡czonego z palcamichwytaka i zamkni¦cie go. Za otwarcie chwytaka odpowiedzialna jest spr¦»yna, która

Rysunek 3.5: Chwytak dwupalczasty

po wyª¡czeniu napi¦cia zasilania rozwiera palce chwytaka. Takie rozwi¡zanie umo»li-wia uchwycenie przedmiotów o szeroko±ci okoªo 2.5cm. Po zmianie punktu zamo-cowania ª¡cznika rdzenia elektromagnesu z d¹wigni¡ palców chwytaka, mo»liwe jestrównie» uchwytanie szerszych przedmiotów. Dla odpowiedniego dziaªania chwytaka,po zwi¦kszeniu rozwarcia palców nale»y równie» dokona¢ regulacji spr¦»yny powrot-nej. Poza mo»liwo±ci¡ regulacji szeroko±ci rozwarcia palców chwytaka istnieje mo»li-wo±¢ zmiany ustawienia pªaszczyzny dziaªania chwytaka. Jest to mo»liwe dzi¦kiwykonaniu interfejsu umieszczonego prostopadle do drugiego ramienia manipula-tora. W zale»no±ci od rodzaju obiektu, który chcemy uchwyci¢ mo»na dowolniedopasowa¢ k¡t chwytaka wzgl¦dem pªaszczyzny pionowej. Przez r¦czn¡ regulacj¦i zablokowanie ±rub¡ mo»na przekon�gurowa¢ chwytak tak, aby palce otwieraªy si¦

21

na boki lub w gór¦ i w dóª. Sterowanie otwarciem chwytaka odbywa si¦ jednymz bitów portu mikrokontrolera, za po±rednictwem ukªadu klucza tranzystorowego.

3.6 SterownikZa wszystkie dziaªania i ruchy robota jest odpowiedzialny ukªad sterowania. Wyko-nuje on niezb¦dne pomiary parametrów ruchu, takie jak poªo»enie, pr¦dko±¢. Po-nadto ukªad sterowania komunikuje si¦ z komputerem nadrz¦dnym w celu pobraniaparametrów do realizowanego zadania. Przekazuje on do komputera nadrz¦dnegoaktualne parametry. Na podstawie zadanych i zmierzonych parametrów generuje

Jednostka centralna

TPU SCI

koder N koder S

M1 M2

Uklad wykonawczy

koder W1 koder W2

M3 M4

Uklad wykonawczy

Uklad wykonawczy

Uklad wykonawczy

zasilanie

komputernadrzedny

platforma manipulator

robot

Rysunek 3.6: Schemat blokowy ukªadu sterowania robota

takie sterowania silników, aby doprowadzi¢ robota do punktu docelowego. Mocobliczeniowa ukªadu sterowania powinna by¢ taka, aby zapewni¢ wykonywanie tychwszystkich czynno±ci autonomicznie i w mo»liwie krótkim czasie. Na rysunku 3.9przedstawiono schemat blokowy ukªadu sterownika.

22

3.7 Jednostka centralnaDo sterowania robotem wykorzystano 32-bitowy mikrokontroler MC68332, który po-siada du»e mo»liwo±ci obliczeniowe i funkcjonalne. W robocie wykorzystano autono-miczny okªad czasowy TPU (Time Processor Unit) i komunikacj¦ szeregow¡ z kom-puterem nadrz¦dnym SCI (Serial Communication Interface). Moduª TPU umo»li-wia realizowanie zaprogramowanych w mikrokodzie funkcji czasowych. W robo-cie moduª TPU wykorzystano do generowania sygnaªu PWM oraz do dekodowaniasygnaªów pochodz¡cych z czujników poªo»e«. Obie operacje wykonywane s¡ przyu»yciu standardowych funkcji o nazwie PWM i QDEC. Do pomiaru pr¦dko±ci sil-ników wykorzystano napisan¡ do potrzeb projektu wózka MK funkcj¦ QDVEL [5].Programowanie odbywa si¦ za po±rednictwem interfejsu ª¡cz¡cego mikrokontrolerz komputerem PC. Mikrokontroler po podª¡czeniu do komputera pracuje w specjal-nym trybie BDM (Background Debug Mode). Prac¦ w tym trybie równie» u»ywasi¦ do debagowania programu i podgl¡du zawarto±ci rejestrów. Jest to mo»liwedzi¦ki u»yciu odpowiedniego oprogramowania dost¦pnego w Laboratorium robotyki.Ukªad mikrokontrolera wraz z pami¦ci¡ RAM, EEPROM, interfejsem szeregowymMAX232 oraz niezb¦dnymi zª¡czami, wyprowadzaj¡cymi sygnaªy z ukªadu oraz do-prowadzaj¡cymi do niego, tworzy niezale»ny moduª.

3.8 Sensoryka3.8.1 Pomiar poªo»eniaAby skutecznie sterowa¢ robotem, niezb¦dne jest wykonywanie pomiarów poªo»e-nia i pr¦dko±ci. Uwarunkowania �nansowe wymusiªy u»ycie koderów przyrostowychpochodz¡cych z myszek do komputera. Przed zastosowaniem do robota nale»aªozmieni¢ ukªad elektroniczny zgodnie ze schematem widocznym na rysunku (3.7).Po mody�kacji mechanicznej kodery zamocowano na osiach silników za po±red-nictwem elastycznego ª¡cznika, który eliminuje ewentualne braki wspóªosiowo±ci.Podczas ruchu silnika fototranzystory kodera generuj¡ prostok¡tne sygnaªy prze-suni¦te w fazie o 90o (zob. Rysunek 3.8). Po przej±ciu przez ukªad 7414 (w celuuzyskania stromych zboczy narastaj¡cych i opadaj¡cych), sygnaªy te s¡ doprowa-dzane do kanaªów TPU jednostki centralnej, gdzie s¡ zliczane programowo. Na pod-stawie liczby impulsów mo»na okre±li¢ poªo»enie i obliczy¢ pr¦dko±¢ robota (plat-formy i przegubów manipulatora). Zastosowane kodery generuj¡ 12 impulsów najeden obrót osi silnika. Dekodowanie sygnaªu w module TPU uwzgl¦dnia wszys-tkie zbocza sygnaªu, wi¦c rozdzielczo±¢ pomiaru poªo»enia wzrasta do 48 próbekprzypadaj¡cych na jeden obrót osi silnika. Zastosowanie przekªadni obni»aj¡cychpr¦dko±¢, dodatkowo zwi¦ksza dokªadno±¢ pomiaru.

3.8.2 Pozycjonowanie pocz¡tkoweBezpo±rednio po wª¡czeniu zasilania robota, ukªad sterowania nie zna ani dokªadnejpozycji ramion, ani k¡ta skr¦tu przedniego koªa. Dlatego do kalibracji poªo»enia za-

23

Rysunek 3.7: Schemat ideowy kodera

SENSOR A

SENSOR B

7414

TPU

KANAL X

KANAL Y

Rysunek 3.8: Zasada pomiaru za pomoc¡ kodera

24

montowano czujniki kra«cowe. Przed wykonywaniem jakichkolwiek ruchów nale»yuruchomi¢ funkcj¦ zerowania pozycji robota. Robot wykona wtedy z niewielk¡ pr¦d-ko±ci¡ ruch ramion manipulatora i koªa skr¦caj¡cego oraz zatrzyma silniki steruj¡cepo osi¡gni¦ciu poªo»enia kra«cowego. W ten sposób mo»na uzyska¢ informacj¦ o po-zycji przegubów i koªa odpowiedzialnego za skr¦t. Od tego momentu wszystkie para-metry ruchu b¦d¡ ustawiane na podstawie liczby zliczonych impulsów pochodz¡cychz koderów.

3.9 Ukªad wykonawczy dla silnikówZadaniem ukªadu wykonawczego jest sterowanie silnikami robota w zale»no±ci odsygnaªów pochodz¡cych z jednostki centralnej. W opisywanej konstrukcji robotasterowanie odbywa si¦ przez regulacj¦ napi¦cia zasilania silników, które jest zale»neod wspóªczynnika wypeªnienia sygnaªu PWM (Pulse Width Modulation). Wybranesygnaªem DIR (Direction) odpowiednie tranzystory mostka typu H s¡ kluczowanesygnaªem PWM. Przebieg o odpowiednim wypeªnieniu generowany jest w moduleTPU jednostki centralnej, a DIR jest ustawiany jako jeden z bitów portu mikrokon-trolera. Ukªad wykonawczy zostaª zaprojektowany w taki sposób, aby wysterowa¢silniki o napi¦ciu 14.4V i pr¡dzie o warto±ci 15A. Ukªad wykonawczy mo»na podzieli¢na trzy moduªy:

• cyfrowy ukªad sterowania

• ukªad wyzwalania H-mostka

• mostek typu H.

3.9.1 Cyfrowy ukªad sterowaniaZadanie tego moduªu polega na wygenerowaniu sygnaªów do wª¡czenia odpowied-nich tranzystorów na podstawie stanu linii DIR i PWM. Dla linii PWM stanemaktywnym jest stan wysoki. Wysoki stan logiczny linii DIR oraz stan wysoki PWMpowoduje zaª¡czenie kluczy K1 i K4 (obrót w prawo silnika) (rys. 3.9). Zmiana stanulinii DIR z wysokiego na niski powoduje otwarcie kluczy K1, K4 oraz zamkni¦cieK2 i K3 (obrót w lewo). Zatrzymanie silnika nast¦puje przy zerowym wypeªnieniusygnaªu PWM. Dla takiej sytuacji klucze K1 i K2 s¡ otwarte. Zmiana poziomu syg-naªu DIR powoduje przeª¡czanie tranzystorów kierunku, lecz dopiero po ustawieniustanu wysokiego na linii PWM silnik zacznie si¦ obraca¢. Podczas przeª¡czaniakluczy tranzystorowych w mostku typu H istnieje niebezpiecze«stwo jednoczesnegowª¡czenia kluczy tego samego póªramienia. Jest to zwi¡zane z opó¹nieniami cza-sowymi w ukªadach przeª¡czaj¡cych i elementach wykonawczych (klucze jednegoramienia mostka wª¡cz¡ si¦ przed wyª¡czeniem kluczy dla przeciwlego ramienia).Aby zabezpieczy¢ si¦ przed tego typu zjawiskiem zastosowano ukªad opó¹niaj¡cy,który dopiero po upªywie czasu τ = RC = 2µs wª¡cza tranzystory odpowiedzialneza wybór kierunku obrotów. Wª¡czenie tranzystora kierunku nast¡pi w momenciepojawienia si¦ na obu wej±ciach bramki NAND (U3A lub U3B) stanu wysokiego. Na

25

MM

uklad wyzwalania

uklad wyzwalania

uklad wyzwalania

uklad wyzwalania

modul cyfrowy

DIRPWM

+Uz

K1 K2

K3 K4

Rysunek 3.9: Schemat blokowy ukªadu sterowania silnikiem

jednym wej±ciu sygnaª pojawi si¦ od razu, a na drugim po naªadowaniu przez rezys-tor (R10 lub R12) kondensatora (C2 lub C1), daj¡c szans¦ caªkowitego wyª¡czeniakluczy dla poprzedniej kon�guracji (rys. 3.10). W przypadku odª¡czenia przewoduª¡cz¡cego jednostk¦ centraln¡ z ukªadem wykonawczym, nast¦puje wyª¡czenie sil-ników robota. Dzieje si¦ to za spraw¡ rezystora R1 podª¡czonego do masy, który nalinii PWM wymusza stan niski.

3.9.2 Ukªad wyzwalania H-mostkaZ ukªadu cyfrowego ukªadu sterowania mostka do ukªadu doprowadzone s¡ czterysygnaªy, wygenerowane na podstawie sygnaªów DIR i PWM. Nast¦pnie sygnaªy te s¡doprowadzane przez rezystor do emitera tranzystora pracuj¡cego ukªadzie wspólnejbazy. Kolektor tego tranzystora poª¡czony jest z bazami dwóch tranzystorów steru-j¡cych bezpo±rednio bramk¡ tranzystora mocy. Jeden z nich (npn) zwiera bramk¦tranzystora mocy z dodatnim biegunem zasilania, a drugi (pnp) zwiera bramk¦z mas¡. Zwarcie z mas¡ bramki tranzystora z kanaªem typu P powoduje jego otwar-cie, a zwarcie z potencjaªem dodatnim - zatkanie. W tranzystorze z kanaªem typuN sytuacja jest odwrotna.

Rozwa»my sterowanie tranzystorem T1 (zob.rys 3.11). Tranzystor T5 jest otwar-

26

Rysunek 3.10: Schamat ideowy cyfrowej cz¦±ci ukªadu sterowania mostkiem

ty przez rezystor R7 co powoduje wymuszenie stanu wysokiego na bramce T1 i zam-kni¦cie go (T1 dodatkowo jest zamykany przez rezystor R1). Sygnaª PWMT1w stanie niskim przez rezystor R6 otwiera tranzystor T9, powoduj¡c pojawieniesi¦ stanu niskiego na bazach tranzystorów T5 i T6. Stan niski jednocze±nie zamykatranzystor T5 i otwiera T6. Na bramce T1 pojawia si¦ stan niski wprowadzaj¡ctranzystor typu P w stan przewodzenia. Ukªad oparty na tranzystorach pozwala naszybkie odprowadzenie ªadunku zgromadzonego w pojemno±ci bramki tranzystoraunipolarnego, czyli odpowiedni¡ szybko±¢ przeª¡czenia. Ukªad pracy tranzystoraT9 umo»liwia szybk¡ reakcj¦ na zmian¦ stanu na emiterze i umo»liwia sterowaniesygnaªami o poziomach TTL.

3.9.3 Mostek typu HDo budowy mostka u»yto tranzystorów mocy typu MOSFET. Mostek skªada si¦z dwóch tranzystorów z kanaªem typu P oraz dwóch z kanaªem typu N. Pierwszez nich s¡ kluczowane sygnaªem PWM, a drugie sªu»¡ do wyboru kierunku obrotówsilnika. Tranzystory pracuj¡ce jako klucze podaj¡ napi¦cie zasilania na silniki za-pewniaj¡c obrót z zadan¡ pr¦dko±ci¡ i w odpowiednim kierunku. Pr¡dy pªyn¡ceprzez tranzystory podczas pracy powoduj¡ wydzielanie si¦ znacznej ilo±ci ciepªa,dlatego elementy wykonawcze umieszczono na radiatorze.

27

Rysunek 3.11: Schamat ideowy ukªadu sterowania mostkiem

28

3.10 Zasilanie robotaDo zasilania robota u»yto akumulatorów pochodz¡cych od wiertarko-wkr¦tarek,z których wykorzystano silniki z przekªadniami. Bateria akumulatorów skªada si¦z poª¡czonych szeregowo 1.2 voltowych ogniw niklowo-kadmowych. Napi¦cie zasila-nia silników robota wynosi 14.4V. Napi¦cie pracy ukªadów elektronicznych robotawynosi 5V i jest czerpane z akumulatorów przez stabilizator napi¦cia.

3.11 Dane techniczne robota• Przekªadnie

W tabeli umieszczono warto±ci przeªo»e« liczonych od osi wirnika silnika dowaªka wyj±ciowego ostatniego stopnia przekªadni. Poszczególne liczby skªada-j¡ce si¦ na wynikowe przeªo»enie oznaczaj¡ przeªo»enie nast¦puj¡cych po sobieprzekªadni (z uwzgl¦dnieniem przekªadni planetarnych silników).

przeªo»enie nap¦du 36 x 2 = 72przeªo»enie kolumny skr¦tnej 36 x 2.92 x 3.6 = 378.43

przeªo»enie przekªadni ramion manipulatora 36 x (2.92)4 = 2617.18

• Platforma

szeroko±¢ platformy [cm] 37.5dªugo±¢ platformy [cm] 45

odlegªo±¢ przedniego koªa od tylnej osi [cm] 35±rednica przedniego koªa [cm] 10.5

±rednia tylnych kóª [cm] 9.7rozstaw tylnych kóª [cm] 30.5

prze±wit pod platform¡ [cm] 4

• Manipulator

odlegªo±¢ podstawy manipulatora od ±rodka tylnej osi [cm] 17wysoko±¢ podstawy [cm] 20

dªugo±ci ramion manipulatora [cm] 40

29

3.12 Opis poª¡cze« moduªów elektronicznychBazowym punktem ukªadu sterowania robota jest jednostka centralna, do której do-prowadzane s¡ wszystkie sygnaªy pochodz¡ce z sensorów i wyprowadzane sygnaªysteruj¡ce robotem. Poª¡czenia elektryczne pomi¦dzy moduªami robota wykonanoprzewodami zako«czonymi standardowymi zª¡czami. Takie rozwi¡zanie pozwala naªatwe i szybkie ª¡czanie moduªów, zapewniaj¡c jednocze±nie niezawodno±¢ poª¡czenia.Sposób poª¡cze« pomi¦dzy poszczególnymi moduªami i opis zª¡czek widoczny jest narysunku (3.12). Opis sygnaªów poszczególnych zª¡czek przedstawiono w poni»szychtabelach.

• Zª¡cze PDJ NS (PDW NS) - sygnaªy PWM i DIR dla silnika nap¦dowegoi skr¦caj¡cego:

sygnaª pin pin sygnaªDIR N nap¦d 1 2 PWM N nap¦d

GND 3 4 +5 VGND 5 6 +5 V

DIR S skr¦t 7 8 PWM S skr¦t

• Zª¡cze PDJ W1W2 (PDW W1W2) - sygnaªy PWM i DIR dla silników ramionmanipulatora:

sygnaª pin pin sygnaªDIR W1 rami¦ pierwsze 1 2 PWM W1 rami¦ pierwsze

GND 3 4 +5 VGND 5 6 +5 V

DIR W2 rami¦ drugie 7 8 PWM W2 rami¦ drugie

• Zª¡cze QD W1W2 (QDS W1W2) - kwadraturowe sygnaªy QDECA i QDECBpochodz¡ce z przyrostowych czujników poªo»enia manipulatora:

sygnaª pin pin sygnaªQDECA W1 rami¦ pierwsze 1 2 QDECB W1 rami¦ pierwsze

GND 3 4 +5 VGND 5 6 +5 V

QDECA W2 rami¦ drugie 7 8 QDECB W2 rami¦ drugie

• Zª¡cze SNR - sygnaªy pochodz¡ce z sensorów pozycjonowania pocz¡tkowego.W przypadku zadziaªania sensora nast¦puje zwarcie przewodu sensora z mas¡:

sygnaª pin pin sygnaªsensor skr¦tu 1 2 GND

sensor ramienia pierwszego 3 4 GNDsensor ramienia drugiego 5 6 GND

NC 7 8 NC

30

• Zª¡cze CHW - sygnaª steruj¡cy chwytakiem:

sygnaª pin pin sygnaªchwytak + 1 2 GND

• Zª¡cze MS (MN, MW1, MW2) - sterowanie silnikami wykonawczymi skr¦tu(nap¦du, ramienia pierwszego, ramienia drugiego):

sygnaª pin pin sygnaª+/- Vcc 1 2 -/+ Vcc

• Zª¡cze V15 - napi¦cie zasilaj¡ce elektromagnes chwytaka:

sygnaª pin pin sygnaª+15 V 1 2 GND

• Zª¡cze V5 - napi¦cie zasilaj¡ce jednostk¦ centraln¡ i cyfrow¡ cz¦±¢ ukªaduwykonawczego:

sygnaª pin pin sygnaª+5 V 1 2 GND

• Zª¡cze szeregowe RS232:

sygnaª pin pin sygnaªNC 1 2 RxDTxD 3 4 DTRGND 5 6 NCNC 7 8 NCNC 9 10 NC

• Zª¡cze BDM:

sygnaª pin pin sygnaªGND 1 2 BKPT / DSCLKGND 3 4 FREEZE / QOUT

RESET 5 6 IFETCH / DSIVcc 7 8 IPIPE / DSONC 9 10 NC

31

3.13 Opis funkcji peªnionych przez kanaªy TPUi porty mikrokontrolera

• Funkcie poszczególnych kanaªów TPU:

opis funkcja nr kanaªulinia A czujnika poªo»enia nap¦du QDEC 0linia B czujnika poªo»enia nap¦du QDEC 1

wypeªnienie sygnaªu steruj¡cego nap¦dem PWM 12linia A czujnika poªo»enia skr¦tu QDEC 2linia B czujnika poªo»enia skr¦tu QDEC 3

wypeªnienie sygnaªu steruj¡cego skr¦tem PWM 13linia A czujnika poªo»enia ramienia pierwszego QDEC 4linia B czujnika poªo»enia ramienia pierwszego QDEC 5

wypeªnienie sygnaªu steruj¡cego ramieniem pierwszym PWM 14linia A czujnika poªo»enia ramienia drugiego QDEC 6linia B czujnika poªo»enia ramienia drugiego QDEC 7

wypeªnienie sygnaªu steruj¡cego ramieniem drugim PWM 15

• Porty mikrokontrolera:

opis nr pinu portuwybór kierunku nap¦du PE1wybór kierunku skr¦tu PE2

wybór kierunku ramienia pierwszego PE3wybór kierunku ramienia drugiego PE4

pozycjonowanie skr¦tu PF5pozycjonowanie ramienia pierwszego PF6pozycjonowanie ramienia drugiego PF7

sterowanie chwytakiem PE0

32

BDMRS232

P1 P2

PDW NSPDW W1W2

QDS W1W2QDS NS

MS

MN

MW2

MW1

koder N koder S

M1 M2

koder W1 koder W2

M3 M4

PDJ NSPDJ W1W2

QD NSQD W1W2

komputernadrzedny

CHW

V15

V5

MC68332

74147414

SNR

Rysunek 3.12: Schemat blokowy poª¡cze« moduªów robota

33

Rozdziaª 4

Badania symulacyjne

Badania symulacyjne maj¡ na celu sprawdzenie dziaªania zaproponowanego algo-rytmu kinematyki odwrotnej w zale»no±ci od kon�guracji pocz¡tkowych robotai parametrów algorytmu. Manipulator mobilny reprezentowany jest równaniami

AA

BB

CCDD

EE

FFGG

HH

rr

Rysunek 4.1: Rozmieszczenie punktów startowych w ukªadzie wspóªrz¦dnych

kinematyki (1.9) i (1.10). Kon�guracja pocz¡tkowa ma posta¢ wektorów q0 =(x0, y0, ϕ0, θ0) i x0 = (x01, x02). Jako poªo»enie zadane podawane s¡ wspóªrz¦dneyd = (xd, yd, zd) punktu, w którym ma si¦ znale¹¢ chwytak. Sterowanie ma posta¢uk(t) = λ0k + λ1k sinωt + λ2k cosωt, k=1,2. Horyzont czasowy symulacji wynosiT = 8. Podczas symulacji na staªe przyj¦to orientacj¦ pocz¡tkow¡ robota ϕ0 = 0i wspóªrz¦dne docelowe chwytaka yd = (0, 0, 0.8). Algorytm poddawano analizieprzy ró»nych ustawieniach k¡ta skr¦tu przedniego koªa θ0 i ró»nych warto±ciachwspóªczynnika odpowiedzialnego za szybko±¢ zbie»no±ci algorytmu γ. Odlegªo±¢ dopunktu docelowego ustawiono na staª¡ warto±¢ r = 3m.

Wyniki dziaªania algorytmu przedstawiono na wykresach, po cztery dla ka»dejsymulacji. Na pierwszym z nich znajduje si¦ trajektoria platformy i efektora, drugi

34

przedstawia trajektori¦ stanu platformy q, trzeci trajektori¦ chwytaka, a czwartyprzebieg czasowy sterowania.

4.1 Wyniki dziaªania algorytmuPunkty startowe rozmieszczono na obwodzie koªa o promieniu r = 3 od punktudocelowego (0, 0) (Rysunek 4.1). Pocz¡tkow¡ ustawienie ramion wahadªa wynosi(x01, x02) = (π

3,−π

2), a pocz¡tkowa warto±¢ skr¦tu koªa θ0 = π

6. Wspóªczynnik szyb-

ko±ci zbie»no±ci algorytmu ustawiono na wartó±¢ γ = 0.1. Pocz¡tkowe warto±ciparametrów sterowania wynosz¡ λ01 = 0.01, λ02 = −0.01 oraz λ1k, λ2k = 0. Dla takprzyj¦tej kon�guracji pocz¡tkowej punkt yd zostaª osi¡gni¦ty ze wszystkich zapro-ponowanych punktów startowych, za wyj¡tkiem punktu G. W wi¦kszo±ci przypad-ków osi¡gni¦to punkt zadany bez du»ej ilo±ci skr¦tów i zwrotów.

0 0.5 1 1.5 2 2.5 3 3.5 4−0.4

−0.2

0

0.2

0.4

0.6

0.8

1

q1 y1

q2

y2

yq

0 1 2 3 4 5 6 7 8

0

1

2

3

4

t

q1

q2

q3

q4

q1q2q3q4

0 1 2 3 4 5 6 7 80

1

2

3

t

y1

y2

y3

y1y2y3

0 1 2 3 4 5 6 7 8

−2

0

2

t

u1

u2

u1u2

Rysunek 4.2: Punkt startowy A, pocz¡tkowe parametry symulacji: (x01, x02) =(π

3,−π

2), θ0 = π

6, γ = 0.1, λ1

01 = 0.01, λ202 = −0.01, T = 8,

Najbardziej skomplikowan¡ trajektori¦ do punktu zadanego osi¡gni¦to z punktuA, D i H, gdzie nast¡piª ponad caªy obrót koªa przedniego wokóª osi skr¦tu. Jestto szczególne niepo»¡dane, gdy» rzeczywisty model nie ma mo»liwo±ci skr¦tu o k¡twi¦kszy ni» ±π

2. Z punktu startowego F trajektoria doj±cia do punktu zadanego jest

najlepsza z uzyskanych. Dla tego przypadku maksymalny k¡t skr¦tu koªa zawiera si¦pomi¦dzy π

6i -π

6i robot bez wykonywana zb¦dnych ruchów osi¡ga punkt docelowy.

Dªugo±¢ drogi przebytej przez robota do punktu zadanego ma wpªyw na warto±¢napi¦cia sterowania silników. Jest to zgodne z intuicj¡, gdy» w przypadku dªu»szejdrogi aby robot mógª osi¡gn¡¢ cel w zadanym czasie to musi porusza¢ si¦ z wi¦ksz¡pr¦dko±ci¡. Porównuj¡c przebiegi sterowa« dla przypadku A (Rysunek 4.2) orazF (Rysunek 4.7) ªatwo zauwa»y¢, »e poziom sterowa« dla drugiego przypadku jestni»szy.

35

−0.5 0 0.5 1 1.5 2 2.5 3−0.5

0

0.5

1

1.5

2

2.5

q1 y1

q2

y2

yq

0 1 2 3 4 5 6 7 8

−2

0

2

t

q1

q2

q3

q4

q1q2q3q4

0 1 2 3 4 5 6 7 80

1

2

t

y1

y2

y3

y1y2y3

0 1 2 3 4 5 6 7 8

−2

−1

0

tu

1 u

2

u1u2

Rysunek 4.3: Punkt startowy B, pocz¡tkowe parametry symulacji: (x01, x02) =(π

3,−π

2), θ0 = π

6, γ = 0.1, λ1

01 = 0.01, λ202 = −0.01, T = 8,

−0.2 0 0.2 0.4 0.6 0.8 1 1.2−0.5

0

0.5

1

1.5

2

2.5

3

3.5

q1 y1

q2

y2

yq

0 1 2 3 4 5 6 7 8

−2

0

2

t

q1

q2

q3

q4

q1q2q3q4

0 1 2 3 4 5 6 7 80

1

2

3

t

y1

y2

y3

y1y2y3

0 1 2 3 4 5 6 7 8

0

1

2

t

u1

u2

u1u2

Rysunek 4.4: Punkt startowy C, pocz¡tkowe parametry symulacji: (x01, x02) =(π

3,−π

2), θ0 = π

6, γ = 0.1, λ1

01 = 0.01, λ202 = −0.01, T = 8,

36

−2.5 −2 −1.5 −1 −0.5 0 0.5−0.5

0

0.5

1

1.5

2

2.5

q1 y1

q2

y2

yq

0 1 2 3 4 5 6 7 8

−2

0

2

t

q1

q2

q3

q4

q1q2q3q4

0 1 2 3 4 5 6 7 8

−1

0

1

2

t

y1

y2

y3

y1y2y3

0 1 2 3 4 5 6 7 8−2

−1

0

1

t

u1

u2

u1u2

Rysunek 4.5: Punkt startowy D, pocz¡tkowe parametry symulacji: (x01, x02) =(π

3,−π

2), θ0 = π

6, γ = 0.1, λ1

01 = 0.01, λ202 = −0.01, T = 8,

−3 −2.5 −2 −1.5 −1 −0.5 0 0.5−0.05

0

0.05

0.1

0.15

0.2

0.25

0.3

0.35

0.4

0.45

q1 y1

q2

y2

yq

0 1 2 3 4 5 6 7 8−3

−2

−1

0

t

q1

q2

q3

q4

q1q2q3q4

0 1 2 3 4 5 6 7 8

−2

−1

0

t

y1

y2

y3

y1y2y3

0 1 2 3 4 5 6 7 8

0

0.5

1

t

u1

u2

u1u2

Rysunek 4.6: Punkt startowy E, pocz¡tkowe parametry symulacji: (x01, x02) =(π

3,−π

2), θ0 = π

6, γ = 0.1, λ1

01 = 0.01, λ202 = −0.01, T = 8,

37

−2.5 −2 −1.5 −1 −0.5 0 0.5−2.5

−2

−1.5

−1

−0.5

0

0.5

q1 y1

q2

y2

yq

0 1 2 3 4 5 6 7 8−2

−1

0

1

t

q1

q2

q3

q4

q1q2q3q4

0 1 2 3 4 5 6 7 8−2

−1

0

t

y1

y2

y3

y1y2y3

0 1 2 3 4 5 6 7 8

0

0.5

1

tu

1 u

2

u1u2

Rysunek 4.7: Punkt startowy F, pocz¡tkowe parametry symulacji: (x01, x02) =(π

3,−π

2), θ0 = π

6, γ = 0.1, λ1

01 = 0.01, λ202 = −0.01, T = 8,

−0.5 0 0.5 1 1.5 2 2.5 3 3.5−2.5

−2

−1.5

−1

−0.5

0

0.5

q1 y1

q2

y2

yq

0 1 2 3 4 5 6 7 8−2

0

2

t

q1

q2

q3

q4

q1q2q3q4

0 1 2 3 4 5 6 7 8−2

0

2

t

y1

y2

y3

y1y2y3

0 1 2 3 4 5 6 7 8

−2

−1

0

1

2

t

u1

u2

u1u2

Rysunek 4.8: Punkt startowy H, pocz¡tkowe parametry symulacji: (x01, x02) =(π

3,−π

2), θ0 = π

6, γ = 0.1, λ1

01 = 0.01, λ202 = −0.01, T = 8,

38

Rozdziaª 5

Badania eksperymentalne

5.1 Algorytm ±ledzeniaPo zako«czeniu oblicze« algorytmu kinematyki odwrotnej i osi¡gni¦ciu punktu do-celowego otrzymujemy dwa wektory. Pierwszy, x = (x1, x2) okre±la ustawienie k¡tówposzczególnych ramion manipulatora, a drugi λ = (λ1, . . . , λ6) wyznacza wspóªczyn-niki przy sterowaniu podawanym na silniki robota. Sterowanie generowane przezalgorytm ma sens pr¦dko±ci liniowej platformy i pr¦dko±ci k¡towej skr¦tu przed-niego koªa. Osi¡gni¦cie zadanej pr¦dko±ci w poszczególnych chwilach czasu powinnodoprowadzi¢ robota do punktu docelowego. Jest to przykªad typowego algorytmu±ledzenia pr¦dko±ci. Aby poprawnie ±ledzi¢ pr¦dko±¢, nale»y mie¢ informacje o przy-spieszeniu i mie¢ mo»liwo±¢ zadawania momentu nap¦dowego na silniki.

W skonstruowanym modelu manipulatora mobilnego istnieje bezpo±redni pomiarpoªo»enia osi silników, a sterowanie odbywa si¦ za pomoc¡ zmiany pr¦dko±ci sil-nika. Dlatego zadane ±ledzenia pr¦dko±ci przetransformowano do zadania ±ledzeniapoªo»enia. Wymagaªo to mody�kacji sterowania generowanego przez algorytm kine-matyki odwrotnej. Mody�kacja polegaªa na caªkowaniu zadanych pr¦dko±ci w celuuzyskania funkcji poªo»e« w czasie. Nast¦pnie funkcje poªo»enia platformy i warto±cik¡ta skr¦tu koªa s¡ podawane do algorytmu ±ledzenia. �ledzenie poªo»enia jest re-alizowane standardowym algorytmem PD.

5.2 Programowa idea dziaªania algorytmu ±ledzeniapoªo»enia

Wygenerowane przez algorytm parametry sterowania i k¡ty ustawienia ramion ma-nipulatora s¡ przekazywane do ukªadu sterowania, który generuje tablic¦ poªo»e«osi silników, przez wyliczanie warto±ci funkcji w poszczególnych chwilach czasu. Takprzygotowana tablica jest podawana na regulator PD, który jako parametr zadanybierze kolejne elementy przygotowanej tablicy poªo»e«. Przyrost czasu jest pro-gramowo okre±lony przez liczb¦ przerwa« procesora w ci¡gu sekundy. Wspóªczynnikiwzmocnienia regulatora PD dobrano eksperymentalnie, a bie»¡ce bª¦dy poszczegól-nych parametrów ruchu s¡ wyliczane na podstawie odczytu poªo»e« osi silników.

39

5.3 Wst¦pne testy na robociePo napisaniu oprogramowania dla robota przetestowano go i stwierdzono prawi-dªowe dziaªanie funkcji odpowiedzialnych za zadawanie napi¦cia zasilana silnikówi funkcji zerowania poªo»enia. Do poprawnego okre±lania parametrów sterowaniarobota niezb¦dne jest zerowanie poªo»enia po jego uruchomieniu. Nast¦pnie ekspery-mentalnie sprawdzono obliczone warto±ci wspóªczynników przeªo»e« nap¦dów, abyzadawane warto±ci poªo»e« silników odpowiadaªy parametrom wygenerowanym przezalgorytm.

5.4 Eksperymentalne sprawdzenie algorytmukinematyki odwrotnej

Do eksperymentalnego przetestowania trajektorii wygenerowanej przez algorytmmo»na wybra¢ tylko te trajektorie, dla których k¡t skr¦tu przedniego koªa nieprzekracza ±π

2. Na podstawie wcze±niej wykonanych symulacji do eksperymentu

wybrano przypadek, którego wynik dziaªania widoczny jest na rysunku (4.7). Abysprawdzi¢ osi¡gni¦cie przez chwytak robota poªo»enia docelowego, na pªaskim pod-ªo»u wytyczono punkt startowy i punkt docelowy. Nast¦pnie ustawiono robotaw punkcie startowym i uruchomiono go. Po wykonaniu kalibracji ukªadów po-miarowych, wyliczeniu tablicy poªo»e« silników w poszczególnych chwilach czasu,robot ruszyª. W zadanym czasie robot zrealizowaª trajektori¦ podobn¡ do trajek-torii wcze±niej zasymulowanej. Ze wzgl¦du na brak mo»liwo±ci sprz¦towej realizacjioblicze« zmiennoprzecinkowych, wyliczenie tablicy poªo»e« silników zajmuje pewienczas (okoªo 30s), zale»ny od horyzontu czasowego i liczby przerwa« procesora.Podczas wykonywania zadania przez robota zapisano aktualne i zadane warto±cik¡ta skr¦tu przedniego koªa i dªugo±¢ drogi przebytej przez robota. Zapami¦tanedane odczytano z robota i na ich podstawie wykonano wykresy. Rysunek (5.1)przedstawia rzeczywiste i zadane poªo»enie k¡ta skr¦tu przedniego koªa w caªymhoryzoncie czasowym. Zadanie ±ledzenia trajektorii w caªym horyzoncie czasowymprzebiega poprawnie. Jedynie na pocz¡tku ruchu robota poªo»enie rzeczywiste od-biega nieznacznie od warto±ci zadanej. Dokªadniej jest to widoczne na wykresieo zwi¦kszonej skali (rys. 5.2). Podobna sytuacja ma miejsce w przypadku ±ledzeniazadanego poªo»enia robota (rys. 5.3). Na pocz¡tku algorytm ±ledzenia dziaªaz pewnym opó¹nieniem, ale po upªywie okoªo 0.25s niedokªadno±¢ ±ledzenia pozycjizadanej zmniejsza si¦ i ustala na maªej warto±ci (rys. 5.4). Bª¡d ±ledzenia poªo»e-nia platformy w pocz¡tkowej fazie ruchu jest spowodowany gwaªtownym skokiemwarto±ci zadanej, co ma wpªyw na bª¡d osi¡gni¦cia celu. Niedokªadno±¢ osi¡gni¦ciapunktu zadanego zale»y równie» od pocz¡tkowej orientacji robota. Wykonane próbywykazaªy, »e dla niewielkiej zmiany orientacji pocz¡tkowej robota, ko«cowe poªo»e-nie chwytaka mo»e ró»ni¢ si¦ od poprzedniej nawet o 15 cm. Jest to nieuniknioneze wzgl¦du na odlegªo±¢ jak¡ robot pokonuje. W przypadku zwi¦kszenia odlegªo±cipomi¦dzy punktem pocz¡tkowym i docelowym wra»liwo±¢ na pocz¡tkowe ustawieniarobota b¦dzie rosn¡¢.

40

0 1 2 3 4 5 6 7 8−0.4

−0.3

−0.2

−0.1

0

0.1

0.2

0.3

0.4

0.5

0.6

t

q4

zadane rzeczywiste

Rysunek 5.1: Zadana i rzeczywista warto±¢ k¡ta skr¦tu przedniego koªa w caªymhoryzoncie czasowym

0 0.02 0.04 0.06 0.08 0.1 0.12 0.14 0.16 0.18 0.20.49

0.495

0.5

0.505

0.51

0.515

0.52

0.525

0.53

t

q4

zadane rzeczywiste

Rysunek 5.2: Zadana i rzeczywista warto±¢ k¡ta skr¦tu przedniego koªaw pocz¡tkowej fazie ruchu

41

0 1 2 3 4 5 6 7 8−0.5

0

0.5

1

1.5

2

2.5

3

t

drog

a [m

]

zadane rzeczywiste

Rysunek 5.3: Zadana i rzeczywista warto±¢ drogi przebytej przez robota w caªymhoryzoncie czasowym

0 0.05 0.1 0.15 0.2 0.25 0.3−0.05

0

0.05

0.1

t

drog

a [m

]

zadane rzeczywiste

Rysunek 5.4: Zadana i rzeczywista warto±¢ drogi przebytej przez robotaw pocz¡tkowej fazie ruchu

42

Zako«czenie

Celem pracy byªo przeanalizowanie algorytmu kinematyki odwrotnej oraz ekspery-mentalne sprawdzenie mo»liwo±ci jego implementacji na rzeczywistym modelu ro-bota. W pracy zawarto peªny opis operacji, jakie nale»aªo wykona¢, aby w peªniwypeªni¢ postawione zadanie. Cykl prac rozpocz¦to od wykonania niezb¦dnych

Rysunek 5.5: Widok skonstruowanego manipulatora mobilnego

oblicze« kinematyki badanego obiektu. Nast¦pnie przedstawiono i przeanalizowanosymulacyjnie algorytm kinematyki odwrotnej. Zaprojektowano i wykonano modelmanipulatora mobilnego, na którym sprawdzono eksperymentalnie dziaªanie algo-

43

rytmu sterowania. Widok skonstruowanego w domowych warunkach manipulatoramobilnego jest widoczny na rysunku (5.5).

Zaproponowany algorytm sterowania dla manipulatora mobilnego nale»y do gru-py algorytmów kinematycznych i nie uwzgl¦dnia dynamiki. Pomimo pomini¦ciadynamiki w obliczeniach, robot zrealizowaª zadan¡ trajektori¦ ruchu. Trajekto-ria wygenerowana przez algorytm nie zawsze jest mo»liwa do realizacji na wyko-nanym modelu robota. Gªównym powodem tego jest �zyczne ograniczenie k¡taskr¦tu przedniego koªa robota. Aby algorytm znalazª trajektori¦ realizowaln¡ na mo-delu robota nale»aªo niekiedy uruchomi¢ obliczenia wielokrotnie, dla ró»nych usta-wie« warunków pocz¡tkowych algorytmu. Stwierdzono, »e nawet niewielka zmianaparametrów sterowania czy pocz¡tkowej warto±ci k¡ta skr¦tu przedniego koªa mo»espowodowa¢ wygenerowanie trajektorii o zupeªnie innym ksztaªcie.

Wykonany model manipulatora mobilnego i zaimplementowany program sterowa-nia nap¦dami, ±ledz¡cy zadane poªo»enia silników umo»liwia testowanie równie» in-nych algorytmów sterowania robotów mobilnych lub manipulatorów. Robot mo»estanowi¢ baz¦ dla nowych projektów badawczych lub da¢ mo»liwo±¢ eksperymental-nego sprawdzenia ju» istniej¡cych.

Mo»liwo±¢ wykonywania bada« eksperymentalnych pozwala na pozyskanie infor-macji mog¡cych wyznaczy¢ nowe kierunki bada«. W przypadku badanego algorytmunowe kierunki bada« mog¡ i±¢ w stron¦ uwzgl¦dnienia ogranicze« na k¡t skr¦tuprzedniego koªa oraz wymuszenia zerowej warto±ci pr¦dko±ci w pocz¡tkowej chwiliruchu, która mo»e prowadzi¢ do gwaªtownego szarpni¦cia robotem i spowodowa¢bª¦dy osi¡gni¦cia punktu docelowego.

Dodanie nowych moduªów programowych czy wyposa»enie robota w dodatkowesensory mo»e zwi¦kszy¢ autonomiczno±¢ modelu i umo»liwi¢ robotowi reakcj¦ nabod¹ce zewn¦trzne. Interakcja z otoczeniem zwi¦kszy uniwersalno±¢ robota i zwi¦k-szy zakres jego stosowania.

44

Bibliogra�a

[1] K. Tcho«: Kinematics of mobile manipulators: An endogenous con�gurationspace approach. Proc. 9th MMAR Conference, Mi¦dzyzdroje 2003, Poland, ss.721-732.

[2] K. Tcho«, A. Mazur, I. Dul¦ba, R. Hossa, R. Muszy«ski: Manipulatory i robotymobilne. Akademicka O�cyna Wydawnicza Warszawa 2000.

[3] K. Tcho«, J. Jakubiak: Endogenous con�guration space approach to mobilemanipulators: a derivation and performance assessment of Jacobian inversekinematics algorithms. Int. J. Control 2003, vol. 76, no. 14, 1387-1419.

[4] J. Jakubiak: Numeryczne aspekty metody Newtona planowania ruchu robotównieholonomicznych. Praca magisterska pod kierunkiem dr. in». I. Dul¦by, PWr1999.

[5] M. Kabaªa, M. Wnuk: Konstrukcja i oprogramowanie dwukoªowego robotamobolnego. Raport SPR 20/2002, Instytut Cybernetyki Technicznej PWr 2002.

[6] C. C. de Vit, B. Siciliano, G. Bastin: Theory of Robot Control. Springer-VerlagLondon 1996.

[7] M. J. Griendel, Z. Hendzel, W. �ylski: Modelowanie i sterowania mobilnychrobotów koªowych. Wydawnictwo Naukowe PWN, 2002.

45