J.Szrek, Control algorithms of mobile manipulators, MSs thesis
Transcript of J.Szrek, Control algorithms of mobile manipulators, MSs thesis
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