J.Szrek, Control algorithms of mobile manipulators, MSs thesis

J.Szrek, Control algorithms of mobile manipulators, MSs thesis

Politechnika Wrocªawska
Wydziaª Elektroniki
Kierunek: Automatyka i Robotyka (AiR)
Specjalno±¢: Robotyka (ARR)
PRACA DYPLOMOWA
MAGISTERSKA
Algorytmy sterowania
manipulatoró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
Panu
prof. dr hab. in». Krzysztofowi Tchoniowi
podzi¦kowania za pomoc i ukierunkowanie
w sposobie opisu bada« przyczyniaj¡ce
si¦ do podniesienia merytorycznej warto±ci pracy.
Równocze±nie dzi¦kuj¦
Panu dr in». Markowi Wnukowi
za po±wi¦cony czas i pomoc przy budowie ukªadu
sterowania modelu manipulatora mobilnego.
1
Spis tre±ci
1 Kinematyka manipulatora mobilnego
1.1
1.2
1.3
1.4
Endogeniczna przestrze« konguracujna . . .
Kinematyka . . . . . . . . . . . . . . . . . . .
Wyprowadzenie równa« kinematyki platformy
Funkcja wyj±cia . . . . . . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
7
. 7
. 7
. 8
. 10
2 Algorytm kinematyki odwrotnej
13
3 Konstrukcja manipulatora mobilnego
17
2.1
2.2
2.3
3.1
3.2
3.3
3.4
3.5
3.6
3.7
3.8
3.9
3.10
3.11
3.12
3.13
Jakobian pseudoodwrotny . . . . . . . . . . . . . . . . . . . . . . . . 13
Algorytm sterowania . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
Dyskretna posta¢ algorytmu . . . . . . . . . . . . . . . . . . . . . . . 14
Ogólna charakterystyka . . . . . . . . . . .
Silniki robota . . . . . . . . . . . . . . . .
Nap¦d i skr¦t platformy . . . . . . . . . .
Manipulator . . . . . . . . . . . . . . . . .
Chwytak . . . . . . . . . . . . . . . . . . .
Sterownik . . . . . . . . . . . . . . . . . .
Jednostka centralna . . . . . . . . . . . . .
Sensoryka . . . . . . . . . . . . . . . . . .
3.8.1 Pomiar poªo»enia . . . . . . . . . .
3.8.2 Pozycjonowanie pocz¡tkowe . . . .
Ukªad wykonawczy dla silników . . . . . .
3.9.1 Cyfrowy ukªad sterowania . . . . .
3.9.2 Ukªad wyzwalania H-mostka . . . .
3.9.3 Mostek typu H . . . . . . . . . . .
Zasilanie robota . . . . . . . . . . . . . . .
Dane techniczne robota . . . . . . . . . . .
Opis poª¡cze« moduªów elektronicznych .
Opis funkcji peªnionych przez kanaªy TPU
i porty mikrokontrolera . . . . . . . . . . .
4 Badania symulacyjne
4.1
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
17
17
18
18
21
22
23
23
23
23
25
25
26
27
29
29
30
. . . . . . . . . . . . . . . 32
34
Wyniki dziaªania algorytmu . . . . . . . . . . . . . . . . . . . . . . . 35
2
5 Badania eksperymentalne
5.1
5.2
5.3
5.4
Algorytm ±ledzenia . . . . . . . . . . . . . . . . . . . . .
Programowa idea dziaªania algorytmu ±ledzenia poªo»enia
Wst¦pne testy na robocie . . . . . . . . . . . . . . . . . .
Eksperymentalne sprawdzenie algorytmu
kinematyki odwrotnej . . . . . . . . . . . . . . . . . . . .
3
39
. . . . . . . 39
. . . . . . . 39
. . . . . . . 40
. . . . . . . 40
Spis rysunków
1
Manipulator typu 2R na platformie mobilnej . . . . . . . . . . . . . .
1.1
1.2
1.3
1.4
Warunek braku bocznego po±lizgu kóª tylnych . . . . . . .
Warunek braku bocznego po±lizgu koªa przedniego . . . . .
Platforma mobilna w zewn¦trznym ukªadzie wspóªrz¦dnych
Widok manipulatora . . . . . . . . . . . . . . . . . . . . .
3.1
3.2
3.3
3.4
3.5
3.6
3.7
3.8
3.9
3.10
3.11
3.12
Koªo przednie z silnikiem nap¦dowym i skr¦caj¡cym -widok z boku
Silnik steruj¡cy skr¦tem -widok z góry . . . . . . . . . . . . . . . .
Przekªadnia dla dolnego ramienia manipulatora . . . . . . . . . . .
Przekªadnia dla górnego ramienia manipulatora . . . . . . . . . . .
Chwytak dwupalczasty . . . . . . . . . . . . . . . . . . . . . . . . .
Schemat blokowy ukªadu sterowania robota . . . . . . . . . . . . .
Schemat ideowy kodera . . . . . . . . . . . . . . . . . . . . . . . . .
Zasada pomiaru za pomoc¡ kodera . . . . . . . . . . . . . . . . . .
Schemat blokowy ukªadu sterowania silnikiem . . . . . . . . . . . .
Schamat ideowy cyfrowej cz¦±ci ukªadu sterowania mostkiem . . . .
Schamat ideowy ukªadu sterowania mostkiem . . . . . . . . . . . .
Schemat blokowy poª¡cze« moduªów robota . . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
19
19
20
20
21
22
24
24
26
27
28
33
4.1
4.2
4.3
4.4
4.5
4.6
4.7
4.8
Rozmieszczenie punktów startowych w ukªadzie wspóªrz¦dnych
Wynik dziaªania algorytmu dla punktu startowego A . . . . .
Wynik dziaªania algorytmu dla punktu startowego B . . . . .
Wynik dziaªania algorytmu dla punktu startowego C . . . . .
Wynik dziaªania algorytmu dla punktu startowego D . . . . .
Wynik dziaªania algorytmu dla punktu startowego E . . . . .
Wynik dziaªania algorytmu dla punktu startowego F . . . . .
Wynik dziaªania algorytmu dla punktu startowego H . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
34
35
36
36
37
37
38
38
5.1
5.2
5.3
5.4
5.5
K¡t skr¦tu przedniego koªa w caªym horyzoncie czasowym .
K¡t skr¦tu przedniego koªa w pocz¡tkowej fazie ruchu . . . .
Droga przebyta przez robota w caªym horyzoncie czasowym
Droga przebyta przez robota w pocz¡tkowej fazie ruchu . . .
Widok skonstruowanego manipulatora mobilnego . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
41
41
42
42
43
4
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
5
. 9
. 9
. 11
. 11
Wprowadzenie
Post¦p techniczny w wielu dziedzinach nauki, dynamicznie rozwijaj¡ce si¦ badania nad robotami oraz wykorzystanie techniki mikroprocesorowej umo»liwiaj¡ budow¦ coraz lepszych urz¡dze« robotycznych. Podwy»szanie jako±ci i niezawodno±ci
sterowania, pozwalaj¡ce wykonywa¢ zaprogramowane ruchy znalazªo zastosowanie
robotów w ró»nych dziedzinach »ycia. Roboty pomagaj¡ czªowiekowi w ci¦»kich i
niebezpiecznych dla zdrowia pracach jak malowanie, spawanie oraz przy wykonywaniu precyzyjnych czynno±ci np. w medycynie.
Mo»liwo±¢ samodzielnego przemieszczania si¦ robota daje mu szersze mo»liwo±ci
zastosowa«. Urz¡dzenie skªadaj¡ce si¦ z manipulatora zamontowanego na platformie
mobilnej pozwala wykonywa¢ bardziej wyszukane zadania. Manipulatory mobilne
mog¡ pomóc czªowiekowi w transporcie, usªugach, opiece nad osobami starszymi lub
niepeªnosprawnymi.
Rysunek 1: Manipulator typu 2R na platformie mobilnej
Stosowanie manipulatorów mobilnych generuje potrzeby opracowywania wielu
typów algorytmów sterowania. Algorytm sterowania manipulatorem mobilnym lub
5
inaczej algorytm planowania ruchu robota ma na celu wyznaczenie pewnych oddziaªywa« przeprowadzaj¡cych ukªad z konguracji pocz¡tkowej do zadanej konguracji ko«cowej. Zakªadaj¡c nieholonomiczne podej±cie do ukªadu robotycznego
zadnie sterowania robotem mo»na podzieli¢ na dwa etapy. Pierwszy etap polega
na planowaniu trajektorii ruchu robota, a drugi etap dotyczy ±ledzenia zadanej trajektorii realizowane przez ukªad sterowania [2]. ‘ledzenie trajektorii wyznaczonej
w pierwszym etapie powinno doprowadzi¢ robota do celu.
Celem bada« niniejszej pracy jest analiza algorytmu kinematyki odwrotnej oraz
eksperymentalne sprawdzenie mo»liwo±ci jego implementacji na rzeczywistym modelu robota. Obiektem bada« jest manipulator mobilny widoczny na rysunku (1).
Robot skªada si¦ z trzykoªowej platformy mobilnej klasy (1,1) i dwuramiennego
manipulatora o dwóch przegubach rotacyjnych. W pracy wykonano teoretyczne
wyliczenia kinematyki rozwa»anego robota. Przedstawiono algorytm kinematyki
odwrotnej i przetestowano algorytm symulacyjnie. Dodatkowo skonstruowano zyczny model manipulatora mobilnego. Wykonany robot umo»liwiª przeprowadzenie
na nim zamierzonych bada« eksperymentalnych. Wcze±niejsze badania algorytmów
kinematycznych [3] i numerycznych metod planowania ruchu robota [4] pozwoliªy
na odpowiednie dobranie elementów algorytmu, aby umo»liwi¢ implementacje na
sterowniku robota.
Ukªad pracy jest nast¦puj¡cy: W rozdziale pierwszym zawarto teoretyczne podstawy dotycz¡ce kinematyki robota w przestrzeni endogeniczniej [1] oraz wyprowadzono równania kinematyki rozwa»anego manipulatora mobilnego. Nast¦pnie,
w rozdziale drugim, opisano badany algorytm kinematyki odwrotnej, a w rozdziale
czwartym przedstawiono wyniki bada« symulacyjnych tego algorytmu. Rozdziaª
trzeci po±wi¦cono opisowi budowy mechanicznej i elektronicznej skonstruowanego
modelu manipulatora mobilnego. Eksperymentalne wyniki dziaªania algorytmu na
rzeczywistym obiekcie przedstawiono w rozdziale pi¡tym.
6
Rozdziaª 1
Kinematyka manipulatora mobilnego
1.1 Endogeniczna przestrze« konguracujna
Przyjmijmy, »e manipulator mobilny skªada si¦ z nieholonomicznej platformy mobilnej oraz z zamontowanego na niej stacjonarnego manipulatora. Platforma posiada
n uogólnionych wspóªrz¦dnych stanu oraz m wej±¢ steruj¡cych. Oznaczmy stan
platformy jako q ∈ Rn , wektor wej±¢ jako u ∈ Rm , wektor poªo»e« przegubów manipulatora jako x ∈ Rp . Dodatkowo oznaczmy wektor wspóªrz¦dnych zadaniowych
przez y ∈ Rr . Przy tak przyj¦tych oznaczeniach kinematyka manipulatora mobilnego mo»e by¢ reprezentowana przez ukªad sterowania z wyj±ciem
(
q̇ = G(q)u
y = k(q, x).
(1.1)
Przez analogi¦ do stacjonarnych manipulatorów, gdzie konguracja zwi¡zana jest
z zadawanym poªo»eniem przegubów, w manipulatorch mobilnych konguracja b¦dzie zwi¡zana ze zmiennymi steruj¡cymi.
Endogeniczn¡ przestrzeni¡ konguracyjn¡ b¦dzie nazywana przestrze« okre±lona
przez X = L2m [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«
konguracyjna jest przestrzeni¡ Hilberta z norm¡
k(u(·), x)kRW =
Z T
O
!1/2
T
T
u (t)R(t)u(t)dt + x W x
,
dla pewnych dodatnio okre±lonych i symetrycznych macierzy R(t) = RT (t) > 0, W =
W T > 0.
1.2 Kinematyka
Ustalaj¡c poªo»enie pocz¡tkowe plaformy q0 ∈ Rn i wybieraj¡c konguracj¦ endogeniczn¡ (u(·), x) ∈ X , wyliczamy trajektori¦ q(t) = ϕq0 ,t (u(·)) platformy oraz trajektori¦ w przestrzeni zadaniowej y(t) = k(q(t), x). Zakªadamy, »e obie trajektorie
7
s¡ zdeniowane dla ka»dego t ∈ [0, T ]. Wyj±cie systemu (1.1) (poªo»enie i orientacja
efektora) w chwili T
Kq0 ,T : X −→ Rr ,
jest okre±lone przez transformacj¦ konguracji endogenicznej we wspóªrz¦dne zadaniowe
Kq0 ,T (u(·), x) = y(T ) = k(ϕq0 ,T (u(·)), x).
(1.2)
Tak zdeniowana funkcja b¦dzie nazywana kinematyk¡ manipulatora mobilnego.
Odpowiednikiem jakobianu analitycznego manipulatora mobilnego jest pochodna
kinematyki,
Jq0 ,T (u(·), x)(v(·), w) =
C(T, x)
Z T
0
d
|α=0 Kq0 ,T (u(·) + αv(·), x + αw) =
dα
Φ(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) =
C(t, x) =
∂k(q(t),x)
,
∂q
∂
(G(q(t))u(t)),
∂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, s) = A(t)Φ(t, s), oraz posiada wªasno±¢ Φ(s, s) = In .
∂t
1.3 Wyprowadzenie równa« kinematyki platformy
Rozwa»my platform¦ mobiln¡ klasy (1,1) [6], posiadaj¡c¡ jedno koªo przednie i dwa
tylne. Koªa tylne zamocowane s¡ po bokach platformy, natomiast koªo przednie
zapewniaj¡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, y
okre±laj¡ wspóªrz¦dne ±rodka tylnej osi, a ϕ i θ s¡ odpowiednio orientacj¡ robota
i k¡tem skr¦tu przedniego koªa. Na rysunku (1.1) przedstawiony jest schematyczny
widok platformy z oznaczonymi parametrami. Odlegªo±¢ przedniego koªa od tylnej
osi jest oznaczona przez l. Formalnie, kinematyk¦ robota mobilnego charakteryzuje
si¦ przez ukªad sterowania postaci (1.1) gdzie q ∈ Rn , u ∈ Rm , m = n − s. Parametr
s oznacza ilo±¢ niezale»nych ogranicze« nieholonomicznych. Wyznaczenie modelu kinematyki wymaga zaªo»enia o braku po±lizgu poprzecznego kóª tylnych oraz
koªa przedniego. Warunek ten, w przypadku kóª tylnych, mo»na zapisa¢ jednym
równaniem, które odpowiada brakowi po±lizgu bocznego punktu o wspóªrz¦dnych
8
Yo
θ
η
y·
ϕ
· ϕ
ycosϕ
l
ϕ
y
ϕ
x·
· ϕ
xsinϕ
Xo
ξ
x
Rysunek 1.1: Warunek braku bocznego po±lizgu kóª tylnych
·
η
Yo
θ
η=y+lsinϕ
ϕ
η· =cos(ϕ+θ)
·
ξ
(ϕ+θ)
l
·
(ϕ+θ)
ϕ
ξ=x+lcosϕ
ξ=sin(ϕ+θ)
Xo
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±ci
prostopadªe do powierzchni kóª musz¡ mie¢ warto±¢ równ¡ zero,
(1.5)
ẋ sin ϕ − ẏ cos ϕ = 0.
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 ϕ, ξ˙ = ẋ − lϕ̇ sin ϕ, η = y + l sin ϕ, η̇ = ẏ + lϕ̇ cos ϕ.
Ostatecznie, po podstawieniu otrzymujemy:
(ẋ − lϕ̇ sin ϕ sin(ϕ + θ) − (ẏ + lϕ̇ cos ϕ cos(ϕ + θ) = 0.
Po kolejnych przeksztaªceniach mamy
(1.7)
ẋ cos ϕ sin θ + ẏ sin ϕ sin θ − lϕ̇ cos θ = 0.
Ograniczenia (1.5) oraz (1.7) mo»na zapisa¢ macierzowo w postaci Pfaa

A(q)q̇ =
sin ϕ
cos ϕ sin θ
− cos ϕ
sin ϕ sin θ
0
−l cos θ

ẋ
 
0  ẏ 
 .
0  ϕ̇ 
θ̇
(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


 
l cos ϕ cos θ
0
 l sin ϕ cos θ 
0

 
g1 (q) = 

, g2 (q) =  .


0
sin θ
0
1
Ostatecznie, ukªad sterowania reprezentuj¡cy kinematyk¦ platformy przyjmuje
posta¢

ẋ = u1 l cos ϕ cos θ



 ẏ = u l sin ϕ cos θ
1
 ϕ̇ = u1 sin θ



(1.9)
θ̇ = u2 .
1.4 Funkcja wyj±cia
Okre±lenie poªo»enia i orientacji efektora w ukªadzie globalnym w zale»no±ci od
konguracji przegubów manipulatora mo»na wyliczy¢ przy pomocy zªo»enia dwóch
transformacji. Pierwsza
Ap0 = T rans(X, x)T rans(Y, y)Rot(Z, ϕ)T rans(X, d)
10
Zo
θ
Yo
l
d
y
ϕ
Xo
x
Rysunek 1.3: Platforma mobilna w zewn¦trznym ukªadzie wspóªrz¦dnych
l2
x2
l1
x1
lo
Rysunek 1.4: Widok manipulatora
11
okre±la poªo»enie oraz orientacj¦ punktu platformy, w którym jest zamontowana
podstawa manipulatora (Rysunek 1.3). Druga transformacja
π
Aep = T rans(Z, l0 )Rot(X, )Rot(Z, x1 )T rans(X, l1 )Rot(Z, x2 )T rans(X, l2 )
2
wyznacza poªo»enie i orientacj¦ efektora wzgl¦dem punktu zamontowania manipulatora na platformie (Rysunek 1.4). Iloczyn tych dwóch przeksztaªce« wyznacza
poªo»enie i orientacj¦ efektora wzgl¦dem ukªadu globalnego

cϕ c12
s c

Ae0 =  ϕ 12
 s12
0
−cϕ s12
−sϕ s12
c12
0
sϕ
−cϕ
0
0

x + cϕ (d + l1 c1 + l2 c12 )
y + sϕ (d + l1 c1 + l2 c12 ) 

,

l0 + l1 s1 + l2 s12
1
gdzie s12 = sin(x1 +x2 ), c12 = cos(x1 +x2 ). Przy zaªo»eniu, »e interesuje nas poªo»enie
efektora, funkcja wyj±cia jest ostatni¡ kolumn¡ macierzy Ae0 i ostatecznie przyjmuje
postac


x + cϕ (d + l1 c1 + l2 c12 )


y = k(q, x) =  y + sϕ (d + l1 c1 + l2 c12 ) .
(1.10)
l0 + l1 s1 + l2 s12
Poª¡czenie wyra»e« (1.9) oraz (1.10) reprezentuje kinematyk¦ manipulatora mobilnego w postaci ukªadu sterowania z wyj±ciem.
12
Rozdziaª 2
Algorytm kinematyki odwrotnej
2.1 Jakobian pseudoodwrotny
Niech Jq0 ,T (u(·), x)(v(·), w) oznacza wcze±niej zdeniowany jakobian analityczny (1.3).
Przy ustalonej konguracji endogenicznej (u(·), x) rozwa»my równanie
(2.1)
Jq0 ,T (u(·), x)(v(·), w) = η
r
dla danego η ∈ R . Rozwi¡zanie tego równania uzyskamy metod¡ najmniejszych
kwadratów. Jest to równowa»ne minimalizacji kwadratu normy
min k(v(·), w)k2RW ,
(v(·),w)
z uwzgl¦dnieniem warunku równo±ciowego (2.1). W tym celu zdeniujemy lagran»ian
Z T
L(v(·), w, λ) =
T
λ
C(T, x)
Z T
0
0
!
T
T
v (t)R(t)v(t)dt + w W w +
!
Φ(T, t)B(t)v(t)dt + D(T, x)w − η ,
gdzie λ = (λ1 , . . . , λr ) jest wektorem mno»ników Lagrange'a. Po wyznaczeniu ekstremum lagran»ianu i eliminacji mno»ników Lagrange'a uzyskujemy rozwi¡zanie równania (2.1) w postaci
v(t)
w
= Jq#0 ,T (u(·), x)η (t) =
gdzie
R−1 (t)B T (t)ΦT (T, t)C T (T, x)
Dq−1
(u(·), x)η,
0 ,T
W −1 DT (T, x)
(2.2)
Dq0 ,T (u(·), x) = D(T, x)W −1 DT (T, x) +
C(T, x)
Z T
0
Φ(T, s)B(s)R−1 (s)B T (s)ΦT (T, s)dsC T (T, x).
Równanie (2.2) okre±la prawostronn¡ pseudoodwrotno±¢ (odwrotno±¢ Moore'a -Penrose'a) jakobianu, wi¦c prawdziwa jest równo±¢
Jq0 ,T (u(·), x)Jq#0 ,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 sterowania
Odwrotne zadanie kinematyki dla robota mobilnego polega na znalezieniu takiej
konguracji endogenicznej (u(·), x), aby przeprowadzi¢ ukªad z poªo»enia pocz¡tkowego 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»y
wybra¢ pewn¡ krzyw¡ (uθ (·), x(θ)), θ ∈ R, w endogenicznej przestrzeni konguracyjnej, startuj¡c z pewnej pocz¡tkowej konguracji (u0 (·), x0 ). Wzdªu» tej krzywej
obliczamy 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
e(θ) = −γe(θ).
dθ
(2.5)
Po niezb¦dnych przeksztaªceniach i uwzgl¦dnieniu denicji jakobianu analitycznego
(1.3), z równania (2.5) otrzymujemy nast¦puj¡ce równanie ró»niczkowe
d
Jq0 ,T (uθ (·), x(θ))
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 wykorzystaniem jakobianu pseudoodwrotnego
d
dθ
uθ (t)
x(θ)
= −γ Jq#0 ,T (uθ (·), x(θ))e(θ) (t) =
R−1 (t)BθT (t)ΦTθ (T, t)CθT (T, x)
−γ
Dq−1
(uθ (·), x(θ))e(θ).
0 ,T
W −1 DθT (T, x)
(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¢ algorytmu
Kinematyka manipulatora mobilnego jest okre±lona w postaci ukªadu sterowania
(1.1). Przyjmujemy konguracj¦ pocz¡tkow¡ (u0 (·), x0 ), punkt docelowy yd oraz
14
interwaª sterowania [0, T ]. Maj¡c dany jakobian analityczny, mo»na obliczy¢ sterowanie pozwalaj¡ce osi¡gn¡¢ yd w zadanym czasie T wedªug formuªy
ui+1 (·)
xi+1 (·)
=
ui
xi
− γJq#0 ,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 kinematyki
mo»na uwa»a¢ za rozwi¡zane. W przeciwnym przypadku sterowanie ui (·) jest modykowane 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«czonego
szeregu Fouriera
ui (t) = λi0 +
si
X
λi2k−1 sin kωt + λi2k cos kωt,
k=1
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
...
0
0

0
.
.
.
0
1
sin
ωt
.
.
.
cos
s
ωt
0
0
.
.
.
0
2


 ..

.
..
.

0
0
···
0
0
···
0
0
1 sin ωt · · · cos sm ωt
tworzy baz¦ przestrzeni funkcyjnej, a λ = (λ1 , λ2 , · · · , λm ) jest wektorem wspóªczynników rozwini¦cia. Ostatecznie dyskretn¡ posta¢ algorytmu sterowaniao ograniczonym pa±mie cz¦stotliwo±ci mo»na zapisa¢ jako
λk+1
xk+1
=
λk
xk
− γ J˜q#0 ,T (λk , xk )ek .
(2.10)
Wystepuj¡ca w (2.10) macierz J˜q#0 ,T stanowi pseudoodwrotno±¢ jakobianu analitycznego
Jq0 ,T (λk , xk ) = [ Ck (T, xk )
RT
0
Φ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) macierze
R 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¢ za
osi¡gni¦ty, gdy bª¡d deniowany 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 szybko±¢ zbie»no±ci algorytmu odpowiada parametr γ > 0. Przed rozpocz¦ciem dziaªania algorytmu ustalane s¡ warto±ci pocz¡tkowe. Do zmiennych zadeklarowanych
w programie wpisywana jest konguracja pocz¡tkowa q0 , punkt docelowy yd , czas
sterowania T oraz pocz¡tkowe warto±ci wspóªczynników sterowania λ. Nast¦pnie
15
wykonywane s¡ obliczenia stanu ukªadu w zadanym horyzoncie czasowym, przez
wykonanie numerycznego caªkowania równa« (1.9). Do caªkowania zastosujemy
metod¦ Eulera ze staªym krokiem. Potrzebne jest te» wyliczenie macierzy przej±cia stanu Φ(T, s) wyst¦puj¡cej w jakobianie analitycznym [4]. Macierz Φ(t, s) jest
rozwi¡zaniem równania
∂
Φ(t, s) = A(t)Φ(t, s).
∂t
Przybli»one rozwi¡zanie powy»szego równania mo»na uzyska¢ korzystaj¡c z wªasno±ci macierzy Φ(t, s). Ró»niczkuj¡c wyra»enie Φ(t, s)Φ(s, t) = In po zmiennej
s i uproszczeniu otrzymujemy
∂
Φ(t, s) = −Φ(t, s)A(s),
∂s
a nast¦pnie
∂
Φ(T, t) = −Φ(T, t)A(t).
∂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 podane
analitycznie. Dla macierzy przej±cia prawdziwe jest Φ(T, T ) = In .
Je»eli punkt docelowy zostaª osi¡gni¦ty z zadan¡ dokªadno±ci¡, to algorytm
ko«czy dziaªanie. W przeciwnym razie nast¦puje modykacja parametrów sterowania wedªug (2.10) i wykonanie oblicze« od pocz¡tku. W implementacji algorytmu
baza przestrzeni funkcyjnej zostaªa ograniczona do skªadowych o cz¦stotliwo±ci podstawowej i przyjmuje posta¢
P (t) =
1 sin ωt cos ωt 0
0
0
.
0
0
0
1 sin ωt cos ωt
Liczba modykacji parametrów sterowa« zostaªa ograniczona do 500. Pozostaªe
elementy potrzebne w algorytmie s¡ wyliczane funkcjami, które generuj¡ odpowiednie macierze w zale»no±ci od parametrów wej±ciowych. Symulacje oraz algorytm
wykonano przy u»yciu pakietu Matlab.
16
Rozdziaª 3
Konstrukcja manipulatora mobilnego
3.1 Ogólna charakterystyka
Baz¦ robota stanowi aluminiowa pªyta w ksztaªcie prostok¡ta, w której wykonano
wyci¦cia na koªa. Robot jest klasy (1,1) i posiada trzy koªa: jedno przednie i dwa
tylne. Koªo przednie, umieszczone w poªowie szeroko±ci robota, realizuje nap¦d
i skr¦t. Koªa tylne zostaªy zamontowane na bokach platformy. Koªa zaadaptowano
z wózków dzieci¦cych i przerobiono stosownie do potrzeb projektu (centrowanie
otworów i dopasowanie sworzni stanowi¡cych ªo»yska ±lizgowe kóª). Przednie zawieszenie wykonano z pªaskowników stalowych, które zapewniaj¡ odpowiedni¡ wytrzymaªo±¢. K¡t skr¦tu koªa przedniego jest ograniczony zycznie i wynosi ± π2 .
W centralnym, punkcie platformy osadzono podstaw¦ manipulatora. Manipulatorem jest dwuwahadªo, na ko«cu którego znajduje si¦ efektor. Ramiona manipulatora zostaªy wykonane z ksztaªtowników aluminiowych. Wysoko±¢ postumentu
mierzona od powierzchni platformy wynosi 20cm. Dªugo±ci ka»dego z ramion manipulatora wynosi 40cm. Platforma ma 30cm szeroko±ci i 45cm dªugo±ci. Elementy
konstrukcyjne i nap¦dowe zaadaptowano z urz¡dze« codziennego u»ytku, co pozwoliªo na znaczne obni»enie koszów robota.
3.2 Silniki robota
Robot posiada cztery silniki elektryczne pr¡du staªego sprz¦»one z przekªadniami
planetarnymi. Napi¦cie zasilania silników pochodz¡cych od wiertarko-wkr¦tarek
wynosi 14.4V. U»yte zespoªy nap¦dowe posiadaj¡ odpowiedni¡ moc, a ich koszt
jest niewielki w porównaniu z silnikami profesjonalnymi. Typowe dla wiertarkowkr¦tarek nap¦dy przed zamontowaniem do robota nale»aªo odpowiednio zmodykowa¢. Z waªków wyj±ciowych przekªadni planetarnych usuni¦to sprz¦gªa cierne
i osadzono na nich z¦batki wspóªpracuj¡ce z przekªadniami. Nast¦pnie z ksztaªtowników stalowych wykonano uchwyty umo»liwiaj¡ce zamocowanie silników do robota. Dodatkowo na osiach silników nale»aªo zamontowa¢ czujniki poªo»enia. Aby
umo»liwi¢ zamontowanie czujników, do osi silników przymocowano wykonane w tym
celu 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]
pobór pr¡du (bez obci¡»enia slinika ) [A]
pr¡d rozruchu [A]
ilo±¢ obrotów silnika [min−1 ]
przeªo»enie przekªadni planetarnej silnika
14.4
1.6
12
19800
36:1
3.3 Nap¦d i skr¦t platformy
Skr¦t jest realizowany przez dwie sprz¦»one ze sob¡ przekªadnie. Pierwsza z nich
obni»a obroty, a druga, k¡towa zapewnia dodatkowo mo»liwo±¢ prostopadªego zamocowania silnika na osi skr¦tnej koªa (zob. Rysunek 3.1). Elementy do pierwszej
przekªadni pochodz¡ od urz¡dzenia gospodarstwa domowego. Obie przekªadnie
umieszczone jedna za drug¡ zapewniaj¡ obni»enie obrotów silnika (z przekªadni¡
planetarn¡) okoªo sze±¢ razy. Pozwala to na dokªadniejsze pozycjonowanie skr¦tu
koªa platformy. Dodatkowo, zastosowanie przekªadni k¡towej pozwoliªo na obni»enie
wysoko±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 zamontowano
koªo oraz nap¦dzaj¡cy je silnik. Nap¦d przedniego koªa odbywa si¦ za po±rednictwem
paska 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 za
po±rednictwem pªaskownika stalowego wyprolowanego w ªuk, który stanowi zawieszenie ukªadu odpowiedzialnego za nap¦d i skr¦t.
3.4 Manipulator
Manipulator robota jest typu 2R. Skªada si¦ on z dwóch ramion o dªugo±ci 40cm osadzonych na podstawie. Do nap¦du ramion wahadªa u»yto takich samych silników jak
w platformie. Siªy potrzebne do wykonania ruchu ramionami manipulatora s¡ nieco
wi¦ksze od siª potrzebnych do wykonania ruchu platformy, dlatego wykonano dodatkowe przekªadnie. Czterostopniowe przekªadnie z¦bate obni»aj¡ pr¦dko±¢ obrotow¡ silników obu ramion manipulatora, jednocze±nie zwi¦kszaj¡c moment siªy na
wyj±ciu ukªadu nap¦dowego ramion. Przekªadnia nap¦dzaj¡ca pierwsze rami¦ manipulatora zostaªa wykonana tak, aby mo»liwe byªo umieszczenie jej wewn¡trz podstawy. Na ostatniej osi przekªadni, znajduj¡cej si¦ przy szczycie podstawy, zamontowano pierwsze rami¦ manipulatora. Elementem no±nym pierwszego ramienia s¡
dwa równolegle poª¡czone ksztaªtowniki. Pomi¦dzy ksztaªtownikami zamontowano
koªa z¦bate przekªadni dla drugiego ramienia (rys. 3.4). Takie rozwi¡zanie zaproponowano, aby silnik byª zamontowany mo»liwie blisko przegubu ª¡cz¡cego pierwsze
rami¦ z postaw¡. Dzi¦ki takiemu rozwi¡zaniu mo»na sterowa¢ drugim ramieniem
manipulatora, niezale»ne od pierwszego. Ponadto powoduje to przesuni¦cie ±rodka
masy bli»ej podstawy, co ma znaczenie ze wzgl¦du na siª¦ (moment) potrzebn¡ do
18
przekladnia katowa
silnik skretny
silnik napedowy
pasek zebaty
Rysunek 3.1: Koªo przednie z silnikiem nap¦dowym i skr¦caj¡cym -widok z boku
przekladnia 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 manipulatora
20
zapewnienia ruchu ramienia. Do ostatniej osi przekªadni znajduj¡cej si¦ na ko«cu
pierwszego ramienia zamocowano uchwyt dla drugiego ramienia manipulatora, które
jest wykonane z ksztaªtownika aluminiowego. Na ko«cu drugiego ramienia przygotowano miejsce do zamontowania chwytaka. Koªa z¦bate obu przekªadni pochodz¡
z urz¡dze« AGD, które odpowiednio przerobiono stosownie do potrzeb (zmniejszono
szeroko±¢ kóª, wytulejowano otwory).
3.5 Chwytak
Podstawowym zadaniem chwytaka jest uchwycenie obiektu i uwolnienie go, po przeniesieniu przez robota do miejsca docelowego. Z wielu mo»liwych typów chwytaków
wybrano jedno z najprostszych rozwi¡za« i wykonano chwytak dwupalczasty. Elementem wykonawczym jest elektromagnes. Pojawienie si¦ napi¦cia na uzwojeniach cewki powoduje wci¡gni¦cie rdzenia elektromagnesu poª¡czonego z palcami
chwytaka 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»liwia uchwycenie przedmiotów o szeroko±ci okoªo 2.5cm. Po zmianie punktu zamocowania ª¡cznika rdzenia elektromagnesu z d¹wigni¡ palców chwytaka, mo»liwe jest
równie» uchwytanie szerszych przedmiotów. Dla odpowiedniego dziaªania chwytaka,
po zwi¦kszeniu rozwarcia palców nale»y równie» dokona¢ regulacji spr¦»yny powrotnej. Poza mo»liwo±ci¡ regulacji szeroko±ci rozwarcia palców chwytaka istnieje mo»liwo±¢ zmiany ustawienia pªaszczyzny dziaªania chwytaka. Jest to mo»liwe dzi¦ki
wykonaniu interfejsu umieszczonego prostopadle do drugiego ramienia manipulatora. W zale»no±ci od rodzaju obiektu, który chcemy uchwyci¢ mo»na dowolnie
dopasowa¢ k¡t chwytaka wzgl¦dem pªaszczyzny pionowej. Przez r¦czn¡ regulacj¦
i zablokowanie ±rub¡ mo»na przekongurowa¢ chwytak tak, aby palce otwieraªy si¦
21
na boki lub w gór¦ i w dóª. Sterowanie otwarciem chwytaka odbywa si¦ jednym
z bitów portu mikrokontrolera, za po±rednictwem ukªadu klucza tranzystorowego.
3.6 Sterownik
Za wszystkie dziaªania i ruchy robota jest odpowiedzialny ukªad sterowania. Wykonuje on niezb¦dne pomiary parametrów ruchu, takie jak poªo»enie, pr¦dko±¢. Ponadto ukªad sterowania komunikuje si¦ z komputerem nadrz¦dnym w celu pobrania
parametrów do realizowanego zadania. Przekazuje on do komputera nadrz¦dnego
aktualne parametry. Na podstawie zadanych i zmierzonych parametrów generuje
manipulator
platforma
koder N
koder S
koder W1
koder W2
M1
M2
M3
M4
Uklad
wykonawczy
Uklad
wykonawczy
komputer
nadrzedny
Uklad
Uklad
wykonawczy wykonawczy
TPU
SCI
Jednostka centralna
zasilanie
robot
Rysunek 3.6: Schemat blokowy ukªadu sterowania robota
takie sterowania silników, aby doprowadzi¢ robota do punktu docelowego. Moc
obliczeniowa ukªadu sterowania powinna by¢ taka, aby zapewni¢ wykonywanie tych
wszystkich czynno±ci autonomicznie i w mo»liwie krótkim czasie. Na rysunku 3.9
przedstawiono schemat blokowy ukªadu sterownika.
22
3.7 Jednostka centralna
Do sterowania robotem wykorzystano 32-bitowy mikrokontroler MC68332, który posiada du»e mo»liwo±ci obliczeniowe i funkcjonalne. W robocie wykorzystano autonomiczny okªad czasowy TPU (Time Processor Unit) i komunikacj¦ szeregow¡ z komputerem nadrz¦dnym SCI (Serial Communication Interface). Moduª TPU umo»liwia realizowanie zaprogramowanych w mikrokodzie funkcji czasowych. W robocie moduª TPU wykorzystano do generowania sygnaªu PWM oraz do dekodowania
sygnaªów pochodz¡cych z czujników poªo»e«. Obie operacje wykonywane s¡ przy
u»yciu standardowych funkcji o nazwie PWM i QDEC. Do pomiaru pr¦dko±ci silników wykorzystano napisan¡ do potrzeb projektu wózka MK funkcj¦ QDVEL [5].
Programowanie odbywa si¦ za po±rednictwem interfejsu ª¡cz¡cego mikrokontroler
z komputerem PC. Mikrokontroler po podª¡czeniu do komputera pracuje w specjalnym trybie BDM (Background Debug Mode). Prac¦ w tym trybie równie» u»ywa
si¦ do debagowania programu i podgl¡du zawarto±ci rejestrów. Jest to mo»liwe
dzi¦ki u»yciu odpowiedniego oprogramowania dost¦pnego w Laboratorium robotyki.
Ukªad mikrokontrolera wraz z pami¦ci¡ RAM, EEPROM, interfejsem szeregowym
MAX232 oraz niezb¦dnymi zª¡czami, wyprowadzaj¡cymi sygnaªy z ukªadu oraz doprowadzaj¡cymi do niego, tworzy niezale»ny moduª.
3.8 Sensoryka
3.8.1 Pomiar poªo»enia
Aby skutecznie sterowa¢ robotem, niezb¦dne jest wykonywanie pomiarów poªo»enia i pr¦dko±ci. Uwarunkowania nansowe wymusiªy u»ycie koderów przyrostowych
pochodz¡cych z myszek do komputera. Przed zastosowaniem do robota nale»aªo
zmieni¢ ukªad elektroniczny zgodnie ze schematem widocznym na rysunku (3.7).
Po modykacji mechanicznej kodery zamocowano na osiach silników za po±rednictwem elastycznego ª¡cznika, który eliminuje ewentualne braki wspóªosiowo±ci.
Podczas ruchu silnika fototranzystory kodera generuj¡ prostok¡tne sygnaªy przesuni¦te w fazie o 90o (zob. Rysunek 3.8). Po przej±ciu przez ukªad 7414 (w celu
uzyskania stromych zboczy narastaj¡cych i opadaj¡cych), sygnaªy te s¡ doprowadzane do kanaªów TPU jednostki centralnej, gdzie s¡ zliczane programowo. Na podstawie liczby impulsów mo»na okre±li¢ poªo»enie i obliczy¢ pr¦dko±¢ robota (platformy i przegubów manipulatora). Zastosowane kodery generuj¡ 12 impulsów na
jeden obrót osi silnika. Dekodowanie sygnaªu w module TPU uwzgl¦dnia wszystkie zbocza sygnaªu, wi¦c rozdzielczo±¢ pomiaru poªo»enia wzrasta do 48 próbek
przypadaj¡cych na jeden obrót osi silnika. Zastosowanie przekªadni obni»aj¡cych
pr¦dko±¢, dodatkowo zwi¦ksza dokªadno±¢ pomiaru.
3.8.2 Pozycjonowanie pocz¡tkowe
Bezpo±rednio po wª¡czeniu zasilania robota, ukªad sterowania nie zna ani dokªadnej
pozycji 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
TPU
KANAL X
7414
KANAL Y
SENSOR B
Rysunek 3.8: Zasada pomiaru za pomoc¡ kodera
24
montowano czujniki kra«cowe. Przed wykonywaniem jakichkolwiek ruchów nale»y
uruchomi¢ funkcj¦ zerowania pozycji robota. Robot wykona wtedy z niewielk¡ pr¦dko±ci¡ ruch ramion manipulatora i koªa skr¦caj¡cego oraz zatrzyma silniki steruj¡ce
po osi¡gni¦ciu poªo»enia kra«cowego. W ten sposób mo»na uzyska¢ informacj¦ o pozycji przegubów i koªa odpowiedzialnego za skr¦t. Od tego momentu wszystkie parametry ruchu b¦d¡ ustawiane na podstawie liczby zliczonych impulsów pochodz¡cych
z koderów.
3.9 Ukªad wykonawczy dla silników
Zadaniem ukªadu wykonawczego jest sterowanie silnikami robota w zale»no±ci od
sygnaªów pochodz¡cych z jednostki centralnej. W opisywanej konstrukcji robota
sterowanie odbywa si¦ przez regulacj¦ napi¦cia zasilania silników, które jest zale»ne
od wspóªczynnika wypeªnienia sygnaªu PWM (Pulse Width Modulation). Wybrane
sygnaªem DIR (Direction) odpowiednie tranzystory mostka typu H s¡ kluczowane
sygnaªem PWM. Przebieg o odpowiednim wypeªnieniu generowany jest w module
TPU jednostki centralnej, a DIR jest ustawiany jako jeden z bitów portu mikrokontrolera. 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 sterowania
Zadanie tego moduªu polega na wygenerowaniu sygnaªów do wª¡czenia odpowiednich tranzystorów na podstawie stanu linii DIR i PWM. Dla linii PWM stanem
aktywnym jest stan wysoki. Wysoki stan logiczny linii DIR oraz stan wysoki PWM
powoduje zaª¡czenie kluczy K1 i K4 (obrót w prawo silnika) (rys. 3.9). Zmiana stanu
linii DIR z wysokiego na niski powoduje otwarcie kluczy K1, K4 oraz zamkni¦cie
K2 i K3 (obrót w lewo). Zatrzymanie silnika nast¦puje przy zerowym wypeªnieniu
sygnaªu PWM. Dla takiej sytuacji klucze K1 i K2 s¡ otwarte. Zmiana poziomu sygnaªu DIR powoduje przeª¡czanie tranzystorów kierunku, lecz dopiero po ustawieniu
stanu wysokiego na linii PWM silnik zacznie si¦ obraca¢. Podczas przeª¡czania
kluczy tranzystorowych w mostku typu H istnieje niebezpiecze«stwo jednoczesnego
wª¡czenia kluczy tego samego póªramienia. Jest to zwi¡zane z opó¹nieniami czasowymi w ukªadach przeª¡czaj¡cych i elementach wykonawczych (klucze jednego
ramienia 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 odpowiedzialne
za wybór kierunku obrotów. Wª¡czenie tranzystora kierunku nast¡pi w momencie
pojawienia si¦ na obu wej±ciach bramki NAND (U3A lub U3B) stanu wysokiego. Na
25
+Uz
uklad
wyzwalania
K2
K1
uklad
wyzwalania
M
uklad
wyzwalania
K3
K4
uklad
wyzwalania
modul cyfrowy
PWM
DIR
Rysunek 3.9: Schemat blokowy ukªadu sterowania silnikiem
jednym wej±ciu sygnaª pojawi si¦ od razu, a na drugim po naªadowaniu przez rezystor (R10 lub R12) kondensatora (C2 lub C1), daj¡c szans¦ caªkowitego wyª¡czenia
kluczy dla poprzedniej konguracji (rys. 3.10). W przypadku odª¡czenia przewodu
ª¡cz¡cego jednostk¦ centraln¡ z ukªadem wykonawczym, nast¦puje wyª¡czenie silników robota. Dzieje si¦ to za spraw¡ rezystora R1 podª¡czonego do masy, który na
linii PWM wymusza stan niski.
3.9.2 Ukªad wyzwalania H-mostka
Z ukªadu cyfrowego ukªadu sterowania mostka do ukªadu doprowadzone s¡ cztery
sygnaª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ólnej
bazy. Kolektor tego tranzystora poª¡czony jest z bazami dwóch tranzystorów steruj¡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 otwarcie, a zwarcie z potencjaªem dodatnim - zatkanie. W tranzystorze z kanaªem typu
N 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 zamkni¦cie go (T1 dodatkowo jest zamykany przez rezystor R1). Sygnaª PWMT1
w stanie niskim przez rezystor R6 otwiera tranzystor T9, powoduj¡c pojawienie
si¦ stanu niskiego na bazach tranzystorów T5 i T6. Stan niski jednocze±nie zamyka
tranzystor T5 i otwiera T6. Na bramce T1 pojawia si¦ stan niski wprowadzaj¡c
tranzystor typu P w stan przewodzenia. Ukªad oparty na tranzystorach pozwala na
szybkie odprowadzenie ªadunku zgromadzonego w pojemno±ci bramki tranzystora
unipolarnego, czyli odpowiedni¡ szybko±¢ przeª¡czenia. Ukªad pracy tranzystora
T9 umo»liwia szybk¡ reakcj¦ na zmian¦ stanu na emiterze i umo»liwia sterowanie
sygnaªami o poziomach TTL.
3.9.3 Mostek typu H
Do 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. Pierwsze
z nich s¡ kluczowane sygnaªem PWM, a drugie sªu»¡ do wyboru kierunku obrotów
silnika. Tranzystory pracuj¡ce jako klucze podaj¡ napi¦cie zasilania na silniki zapewniaj¡c obrót z zadan¡ pr¦dko±ci¡ i w odpowiednim kierunku. Pr¡dy pªyn¡ce
przez 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 robota
Do 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 zasilania silników robota wynosi 14.4V. Napi¦cie pracy ukªadów elektronicznych robota
wynosi 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 do
waªka wyj±ciowego ostatniego stopnia przekªadni. Poszczególne liczby skªadaj¡ce si¦ na wynikowe przeªo»enie oznaczaj¡ przeªo»enie nast¦puj¡cych po sobie
przekªadni (z uwzgl¦dnieniem przekªadni planetarnych silników).
przeªo»enie nap¦du
przeªo»enie kolumny skr¦tnej
przeªo»enie przekªadni ramion manipulatora
36 x 2 = 72
36 x 2.92 x 3.6 = 378.43
36 x (2.92)4 = 2617.18
• Platforma
szeroko±¢ platformy [cm]
dªugo±¢ platformy [cm]
odlegªo±¢ przedniego koªa od tylnej osi [cm]
±rednica przedniego koªa [cm]
±rednia tylnych kóª [cm]
rozstaw tylnych kóª [cm]
prze±wit pod platform¡ [cm]
37.5
45
35
10.5
9.7
30.5
4
• Manipulator
odlegªo±¢ podstawy manipulatora od ±rodka tylnej osi [cm]
wysoko±¢ podstawy [cm]
dªugo±ci ramion manipulatora [cm]
29
17
20
40
3.12 Opis poª¡cze« moduªów elektronicznych
Bazowym punktem ukªadu sterowania robota jest jednostka centralna, do której doprowadzane s¡ wszystkie sygnaªy pochodz¡ce z sensorów i wyprowadzane sygnaªy
steruj¡ce robotem. Poª¡czenia elektryczne pomi¦dzy moduªami robota wykonano
przewodami 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 na
rysunku (3.12). Opis sygnaªów poszczególnych zª¡czek przedstawiono w poni»szych
tabelach.
• Zª¡cze PDJ NS (PDW NS) - sygnaªy PWM i DIR dla silnika nap¦dowego
i skr¦caj¡cego:
sygnaª
DIR N nap¦d
GND
GND
DIR S skr¦t
pin
1
3
5
7
pin
2
4
6
8
sygnaª
PWM N nap¦d
+5 V
+5 V
PWM S skr¦t
• Zª¡cze PDJ W1W2 (PDW W1W2) - sygnaªy PWM i DIR dla silników ramion
manipulatora:
sygnaª
DIR W1 rami¦ pierwsze
GND
GND
DIR W2 rami¦ drugie
pin
1
3
5
7
pin
2
4
6
8
sygnaª
PWM W1 rami¦ pierwsze
+5 V
+5 V
PWM W2 rami¦ drugie
• Zª¡cze QD W1W2 (QDS W1W2) - kwadraturowe sygnaªy QDECA i QDECB
pochodz¡ce z przyrostowych czujników poªo»enia manipulatora:
sygnaª
QDECA W1 rami¦ pierwsze
GND
GND
QDECA W2 rami¦ drugie
pin
1
3
5
7
pin
2
4
6
8
sygnaª
QDECB W1 rami¦ pierwsze
+5 V
+5 V
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ª
sensor skr¦tu
sensor ramienia pierwszego
sensor ramienia drugiego
NC
30
pin
1
3
5
7
pin
2
4
6
8
sygnaª
GND
GND
GND
NC
• Zª¡cze CHW - sygnaª steruj¡cy chwytakiem:
sygnaª
chwytak +
pin
1
pin
2
sygnaª
GND
• Zª¡cze MS (MN, MW1, MW2) - sterowanie silnikami wykonawczymi skr¦tu
(nap¦du, ramienia pierwszego, ramienia drugiego):
sygnaª
+/- Vcc
pin
1
pin
2
sygnaª
-/+ Vcc
• Zª¡cze V15 - napi¦cie zasilaj¡ce elektromagnes chwytaka:
sygnaª
+15 V
pin
1
pin
2
sygnaª
GND
• Zª¡cze V5 - napi¦cie zasilaj¡ce jednostk¦ centraln¡ i cyfrow¡ cz¦±¢ ukªadu
wykonawczego:
sygnaª
+5 V
pin
1
pin
2
sygnaª
GND
sygnaª
NC
TxD
GND
NC
NC
pin
1
3
5
7
9
pin
2
4
6
8
10
sygnaª
RxD
DTR
NC
NC
NC
• Zª¡cze szeregowe RS232:
• Zª¡cze BDM:
sygnaª
GND
GND
RESET
Vcc
NC
pin
1
3
5
7
9
pin
2
4
6
8
10
31
sygnaª
BKP T / DSCLK
FREEZE / QOUT
IF ET CH / DSI
IP IP E / DSO
NC
3.13 Opis funkcji peªnionych przez kanaªy TPU
i porty mikrokontrolera
• Funkcie poszczególnych kanaªów TPU:
opis
linia A czujnika poªo»enia nap¦du
linia B czujnika poªo»enia nap¦du
wypeªnienie sygnaªu steruj¡cego nap¦dem
linia A czujnika poªo»enia skr¦tu
linia B czujnika poªo»enia skr¦tu
wypeªnienie sygnaªu steruj¡cego skr¦tem
linia A czujnika poªo»enia ramienia pierwszego
linia B czujnika poªo»enia ramienia pierwszego
wypeªnienie sygnaªu steruj¡cego ramieniem pierwszym
linia A czujnika poªo»enia ramienia drugiego
linia B czujnika poªo»enia ramienia drugiego
wypeªnienie sygnaªu steruj¡cego ramieniem drugim
funkcja
QDEC
QDEC
PWM
QDEC
QDEC
PWM
QDEC
QDEC
PWM
QDEC
QDEC
PWM
• Porty mikrokontrolera:
opis
wybór kierunku nap¦du
wybór kierunku skr¦tu
wybór kierunku ramienia pierwszego
wybór kierunku ramienia drugiego
pozycjonowanie skr¦tu
pozycjonowanie ramienia pierwszego
pozycjonowanie ramienia drugiego
sterowanie chwytakiem
32
nr pinu portu
PE1
PE2
PE3
PE4
PF5
PF6
PF7
PE0
nr kanaªu
0
1
12
2
3
13
4
5
14
6
7
15
MS
M1
M2
koder N
MN
koder S
MW1
M3
M4
koder W1
koder W2
MW2
PDW NS
PDW W1W2
7414
7414
QDS NS
QDS W1W2
RS232
BDM
P1
CHW
MC68332
P2
V15
SNR
PDJ W1W2 PDJ NS
V5
QD W1W2
QD NS
komputer
nadrzedny
Rysunek 3.12: Schemat blokowy poª¡cze« moduªów robota
33
Rozdziaª 4
Badania symulacyjne
Badania symulacyjne maj¡ na celu sprawdzenie dziaªania zaproponowanego algorytmu kinematyki odwrotnej w zale»no±ci od konguracji pocz¡tkowych robota
i parametrów algorytmu. Manipulator mobilny reprezentowany jest równaniami
C
D
B
r
E
A
H
F
G
Rysunek 4.1: Rozmieszczenie punktów startowych w ukªadzie wspóªrz¦dnych
kinematyki (1.9) i (1.10). Konguracja pocz¡tkowa ma posta¢ wektorów q0 =
(x0 , y0 , ϕ0 , θ0 ) i x0 = (x01 , x02 ). Jako poªo»enie zadane podawane s¡ wspóªrz¦dne
yd = (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 wynosi
T = 8. Podczas symulacji na staªe przyj¦to orientacj¦ pocz¡tkow¡ robota ϕ0 = 0
i wspóªrz¦dne docelowe chwytaka yd = (0, 0, 0.8). Algorytm poddawano analizie
przy ró»nych ustawieniach k¡ta skr¦tu przedniego koªa θ0 i ró»nych warto±ciach
wspóªczynnika odpowiedzialnego za szybko±¢ zbie»no±ci algorytmu γ. Odlegªo±¢ do
punktu docelowego ustawiono na staª¡ warto±¢ r = 3m.
Wyniki dziaªania algorytmu przedstawiono na wykresach, po cztery dla ka»dej
symulacji. Na pierwszym z nich znajduje si¦ trajektoria platformy i efektora, drugi
34
przedstawia trajektori¦ stanu platformy q , trzeci trajektori¦ chwytaka, a czwarty
przebieg czasowy sterowania.
4.1 Wyniki dziaªania algorytmu
Punkty startowe rozmieszczono na obwodzie koªa o promieniu r = 3 od punktu
docelowego (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 szybko±ci zbie»no±ci algorytmu ustawiono na wartó±¢ γ = 0.1. Pocz¡tkowe warto±ci
parametrów sterowania wynosz¡ λ01 = 0.01, λ02 = −0.01 oraz λ1k , λ2k = 0. Dla tak
przyj¦tej konguracji pocz¡tkowej punkt yd zostaª osi¡gni¦ty ze wszystkich zaproponowanych punktów startowych, za wyj¡tkiem punktu G. W wi¦kszo±ci przypadków osi¡gni¦to punkt zadany bez du»ej ilo±ci skr¦tów i zwrotów.
1
4
q1 q2 q3 q4
y
q
0.8
q1
q2
q3
q4
3
2
1
0
0.6
0
1
2
3
4
t
5
6
7
y1
y2
y3
3
q2 y2
y1 y2 y3
0.4
0.2
8
2
1
0
0
0
1
2
3
4
t
5
6
7
u1 u2
2
−0.2
8
u1
u2
0
−2
−0.4
0
0.5
1
1.5
2
q1 y1
2.5
3
3.5
4
0
1
2
3
4
t
5
6
7
8
Rysunek 4.2: Punkt startowy A, pocz¡tkowe parametry symulacji: (x01 , x02 ) =
( π3 , − π2 ), θ0 = π6 , γ = 0.1, λ101 = 0.01, λ202 = −0.01, T = 8,
Najbardziej skomplikowan¡ trajektori¦ do punktu zadanego osi¡gni¦to z punktu
A, D i H, gdzie nast¡piª ponad caªy obrót koªa przedniego wokóª osi skr¦tu. Jest
to szczególne niepo»¡dane, gdy» rzeczywisty model nie ma mo»liwo±ci skr¦tu o k¡t
wi¦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 π6 i - π6 i 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»szej
drogi 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) oraz
F (Rysunek 4.7) ªatwo zauwa»y¢, »e poziom sterowa« dla drugiego przypadku jest
ni»szy.
35
2.5
2
q1
q2
q3
q4
2
q1 q2 q3 q4
y
q
0
−2
0
1
2
3
y1 y2 y3
q2 y2
1.5
1
4
t
5
6
7
y1
y2
y3
2
1
0
0.5
0
1
2
3
4
t
5
6
7
u1 u2
8
u1
u2
0
0
8
−1
−2
−0.5
−0.5
0
0.5
1
1.5
2
2.5
3
0
1
2
3
q1 y1
4
t
5
6
7
8
Rysunek 4.3: Punkt startowy B, pocz¡tkowe parametry symulacji: (x01 , x02 ) =
( π3 , − π2 ), θ0 = π6 , γ = 0.1, λ101 = 0.01, λ202 = −0.01, T = 8,
3.5
3
q1
q2
q3
q4
2
q1 q2 q3 q4
y
q
0
2.5
−2
2
3
y1 y2 y3
q2 y2
0
1.5
1
2
3
4
t
5
6
7
8
y1
y2
y3
2
1
0
1
0
0.5
1
2
3
4
t
5
6
7
u1
u2
u1 u2
2
0
8
1
0
−0.5
−0.2
0
0.2
0.4
0.6
0.8
1
1.2
q1 y1
0
1
2
3
4
t
5
6
7
8
Rysunek 4.4: Punkt startowy C, pocz¡tkowe parametry symulacji: (x01 , x02 ) =
( π3 , − π2 ), θ0 = π6 , γ = 0.1, λ101 = 0.01, λ202 = −0.01, T = 8,
36
2.5
2
q1 q2 q3 q4
y
q
2
q1
q2
q3
q4
0
−2
0
1
2
3
1.5
4
t
5
6
7
1
8
y1
y2
y3
1
y1 y2 y3
q2 y2
2
0
−1
0
0.5
1
2
3
4
t
5
6
7
u1
u2
1
u1 u2
0
8
0
−1
−0.5
−2.5
−2
−1.5
−1
q1 y1
−0.5
0
−2
0.5
0
1
2
3
4
t
5
6
7
8
Rysunek 4.5: Punkt startowy D, pocz¡tkowe parametry symulacji: (x01 , x02 ) =
( π3 , − π2 ), θ0 = π6 , γ = 0.1, λ101 = 0.01, λ202 = −0.01, T = 8,
0.45
y
q
q1
q2
q3
q4
q1 q2 q3 q4
0
0.4
0.35
−1
−2
−3
0.3
0
1
2
3
4
t
5
6
7
y1
y2
y3
y1 y2 y3
q2 y2
0.25
0.2
0.15
8
0
−1
−2
0
1
2
3
0.1
4
t
5
6
7
1
u1
u2
u1 u2
0.05
0
8
0.5
0
−0.05
−3
−2.5
−2
−1.5
−1
−0.5
0
0.5
q1 y1
0
1
2
3
4
t
5
6
7
8
Rysunek 4.6: Punkt startowy E, pocz¡tkowe parametry symulacji: (x01 , x02 ) =
( π3 , − π2 ), θ0 = π6 , γ = 0.1, λ101 = 0.01, λ202 = −0.01, T = 8,
37
0.5
1
q1 q2 q3 q4
y
q
0
q1
q2
q3
q4
0
−1
−2
0
1
2
3
y1 y2 y3
q2 y2
−0.5
−1
4
t
5
6
7
8
y1
y2
y3
0
−1
−2
0
−1.5
1
2
3
4
t
5
6
7
8
1
u1
u2
u1 u2
−2
0.5
0
−2.5
−2.5
−2
−1.5
−1
q1 y1
−0.5
0
0.5
0
1
2
3
4
t
5
6
7
8
Rysunek 4.7: Punkt startowy F, pocz¡tkowe parametry symulacji: (x01 , x02 ) =
( π3 , − π2 ), θ0 = π6 , γ = 0.1, λ101 = 0.01, λ202 = −0.01, T = 8,
0.5
q1 q2 q3 q4
y
q
0
q1
q2
q3
q4
2
0
−2
0
1
2
3
−0.5
4
t
5
6
7
y1
y2
y3
y1 y2 y3
q2 y2
2
−1
8
0
−2
0
−1.5
1
2
3
4
t
5
6
7
2
u1
u2
1
u1 u2
−2
8
0
−1
−2
−2.5
−0.5
0
0.5
1
1.5
q1 y1
2
2.5
3
3.5
0
1
2
3
4
t
5
6
7
8
Rysunek 4.8: Punkt startowy H, pocz¡tkowe parametry symulacji: (x01 , x02 ) =
( π3 , − π2 ), θ0 = π6 , γ = 0.1, λ101 = 0.01, λ202 = −0.01, T = 8,
38
Rozdziaª 5
Badania eksperymentalne
5.1 Algorytm ±ledzenia
Po zako«czeniu oblicze« algorytmu kinematyki odwrotnej i osi¡gni¦ciu punktu docelowego otrzymujemy dwa wektory. Pierwszy, x = (x1 , x2 ) okre±la ustawienie k¡tów
poszczególnych ramion manipulatora, a drugi λ = (λ1 , . . . , λ6 ) wyznacza wspóªczynniki przy sterowaniu podawanym na silniki robota. Sterowanie generowane przez
algorytm ma sens pr¦dko±ci liniowej platformy i pr¦dko±ci k¡towej skr¦tu przedniego koªa. Osi¡gni¦cie zadanej pr¦dko±ci w poszczególnych chwilach czasu powinno
doprowadzi¢ robota do punktu docelowego. Jest to przykªad typowego algorytmu
±ledzenia pr¦dko±ci. Aby poprawnie ±ledzi¢ pr¦dko±¢, nale»y mie¢ informacje o przyspieszeniu i mie¢ mo»liwo±¢ zadawania momentu nap¦dowego na silniki.
W skonstruowanym modelu manipulatora mobilnego istnieje bezpo±redni pomiar
poªo»enia osi silników, a sterowanie odbywa si¦ za pomoc¡ zmiany pr¦dko±ci silnika. Dlatego zadane ±ledzenia pr¦dko±ci przetransformowano do zadania ±ledzenia
poªo»enia. Wymagaªo to modykacji sterowania generowanego przez algorytm kinematyki odwrotnej. Modykacja polegaªa na caªkowaniu zadanych pr¦dko±ci w celu
uzyskania funkcji poªo»e« w czasie. Nast¦pnie funkcje poªo»enia platformy i warto±ci
k¡ta skr¦tu koªa s¡ podawane do algorytmu ±ledzenia. ‘ledzenie poªo»enia jest realizowane standardowym algorytmem PD.
5.2 Programowa idea dziaªania algorytmu ±ledzenia
poªo»enia
Wygenerowane przez algorytm parametry sterowania i k¡ty ustawienia ramion manipulatora 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. Tak
przygotowana tablica jest podawana na regulator PD, który jako parametr zadany
bierze kolejne elementy przygotowanej tablicy poªo»e«. Przyrost czasu jest programowo okre±lony przez liczb¦ przerwa« procesora w ci¡gu sekundy. Wspóªczynniki
wzmocnienia regulatora PD dobrano eksperymentalnie, a bie»¡ce bª¦dy poszczególnych parametrów ruchu s¡ wyliczane na podstawie odczytu poªo»e« osi silników.
39
5.3 Wst¦pne testy na robocie
Po napisaniu oprogramowania dla robota przetestowano go i stwierdzono prawidªowe dziaªanie funkcji odpowiedzialnych za zadawanie napi¦cia zasilana silników
i funkcji zerowania poªo»enia. Do poprawnego okre±lania parametrów sterowania
robota niezb¦dne jest zerowanie poªo»enia po jego uruchomieniu. Nast¦pnie eksperymentalnie sprawdzono obliczone warto±ci wspóªczynników przeªo»e« nap¦dów, aby
zadawane warto±ci poªo»e« silników odpowiadaªy parametrom wygenerowanym przez
algorytm.
5.4 Eksperymentalne sprawdzenie algorytmu
kinematyki odwrotnej
Do eksperymentalnego przetestowania trajektorii wygenerowanej przez algorytm
mo»na wybra¢ tylko te trajektorie, dla których k¡t skr¦tu przedniego koªa nie
przekracza ± π2 . Na podstawie wcze±niej wykonanych symulacji do eksperymentu
wybrano przypadek, którego wynik dziaªania widoczny jest na rysunku (4.7). Aby
sprawdzi¢ 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 robota
w punkcie startowym i uruchomiono go. Po wykonaniu kalibracji ukªadów pomiarowych, wyliczeniu tablicy poªo»e« silników w poszczególnych chwilach czasu,
robot ruszyª. W zadanym czasie robot zrealizowaª trajektori¦ podobn¡ do trajektorii wcze±niej zasymulowanej. Ze wzgl¦du na brak mo»liwo±ci sprz¦towej realizacji
oblicze« zmiennoprzecinkowych, wyliczenie tablicy poªo»e« silników zajmuje pewien
czas (okoªo 30s), zale»ny od horyzontu czasowego i liczby przerwa« procesora.
Podczas wykonywania zadania przez robota zapisano aktualne i zadane warto±ci
k¡ta skr¦tu przedniego koªa i dªugo±¢ drogi przebytej przez robota. Zapami¦tane
dane 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ªym
horyzoncie czasowym. Zadanie ±ledzenia trajektorii w caªym horyzoncie czasowym
przebiega poprawnie. Jedynie na pocz¡tku ruchu robota poªo»enie rzeczywiste odbiega nieznacznie od warto±ci zadanej. Dokªadniej jest to widoczne na wykresie
o zwi¦kszonej skali (rys. 5.2). Podobna sytuacja ma miejsce w przypadku ±ledzenia
zadanego poªo»enia robota (rys. 5.3). Na pocz¡tku algorytm ±ledzenia dziaªa
z pewnym opó¹nieniem, ale po upªywie okoªo 0.25s niedokªadno±¢ ±ledzenia pozycji
zadanej zmniejsza si¦ i ustala na maªej warto±ci (rys. 5.4). Bª¡d ±ledzenia poªo»enia platformy w pocz¡tkowej fazie ruchu jest spowodowany gwaªtownym skokiem
warto±ci zadanej, co ma wpªyw na bª¡d osi¡gni¦cia celu. Niedokªadno±¢ osi¡gni¦cia
punktu zadanego zale»y równie» od pocz¡tkowej orientacji robota. Wykonane próby
wykazaªy, »e dla niewielkiej zmiany orientacji pocz¡tkowej robota, ko«cowe poªo»enie chwytaka mo»e ró»ni¢ si¦ od poprzedniej nawet o 15 cm. Jest to nieuniknione
ze wzgl¦du na odlegªo±¢ jak¡ robot pokonuje. W przypadku zwi¦kszenia odlegªo±ci
pomi¦dzy punktem pocz¡tkowym i docelowym wra»liwo±¢ na pocz¡tkowe ustawienia
robota b¦dzie rosn¡¢.
40
0.6
zadane
rzeczywiste
0.5
0.4
0.3
q4
0.2
0.1
0
−0.1
−0.2
−0.3
−0.4
0
1
2
3
4
t
5
6
7
8
Rysunek 5.1: Zadana i rzeczywista warto±¢ k¡ta skr¦tu przedniego koªa w caªym
horyzoncie czasowym
0.53
zadane
rzeczywiste
0.525
0.52
q4
0.515
0.51
0.505
0.5
0.495
0.49
0
0.02
0.04
0.06
0.08
0.1
t
0.12
0.14
0.16
0.18
0.2
Rysunek 5.2: Zadana i rzeczywista warto±¢ k¡ta skr¦tu przedniego koªa
w pocz¡tkowej fazie ruchu
41
3
zadane
rzeczywiste
2.5
droga [m]
2
1.5
1
0.5
0
−0.5
0
1
2
3
4
t
5
6
7
8
Rysunek 5.3: Zadana i rzeczywista warto±¢ drogi przebytej przez robota w caªym
horyzoncie czasowym
0.1
zadane
rzeczywiste
droga [m]
0.05
0
−0.05
0
0.05
0.1
0.15
t
0.2
0.25
0.3
Rysunek 5.4: Zadana i rzeczywista warto±¢ drogi przebytej przez robota
w pocz¡tkowej fazie ruchu
42
Zako«czenie
Celem pracy byªo przeanalizowanie algorytmu kinematyki odwrotnej oraz eksperymentalne sprawdzenie mo»liwo±ci jego implementacji na rzeczywistym modelu robota. W pracy zawarto peªny opis operacji, jakie nale»aªo wykona¢, aby w peªni
wypeª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 przeanalizowano
symulacyjnie algorytm kinematyki odwrotnej. Zaprojektowano i wykonano model
manipulatora mobilnego, na którym sprawdzono eksperymentalnie dziaªanie algo-
43
rytmu sterowania. Widok skonstruowanego w domowych warunkach manipulatora
mobilnego jest widoczny na rysunku (5.5).
Zaproponowany algorytm sterowania dla manipulatora mobilnego nale»y do grupy algorytmów kinematycznych i nie uwzgl¦dnia dynamiki. Pomimo pomini¦cia
dynamiki w obliczeniach, robot zrealizowaª zadan¡ trajektori¦ ruchu. Trajektoria wygenerowana przez algorytm nie zawsze jest mo»liwa do realizacji na wykonanym modelu robota. Gªównym powodem tego jest zyczne ograniczenie k¡ta
skr¦tu przedniego koªa robota. Aby algorytm znalazª trajektori¦ realizowaln¡ na modelu robota nale»aªo niekiedy uruchomi¢ obliczenia wielokrotnie, dla ró»nych ustawie« warunków pocz¡tkowych algorytmu. Stwierdzono, »e nawet niewielka zmiana
parametrów sterowania czy pocz¡tkowej warto±ci k¡ta skr¦tu przedniego koªa mo»e
spowodowa¢ wygenerowanie trajektorii o zupeªnie innym ksztaªcie.
Wykonany model manipulatora mobilnego i zaimplementowany program sterowania nap¦dami, ±ledz¡cy zadane poªo»enia silników umo»liwia testowanie równie» innych algorytmów sterowania robotów mobilnych lub manipulatorów. Robot mo»e
stanowi¢ baz¦ dla nowych projektów badawczych lub da¢ mo»liwo±¢ eksperymentalnego sprawdzenia ju» istniej¡cych.
Mo»liwo±¢ wykonywania bada« eksperymentalnych pozwala na pozyskanie informacji mog¡cych wyznaczy¢ nowe kierunki bada«. W przypadku badanego algorytmu
nowe kierunki bada« mog¡ i±¢ w stron¦ uwzgl¦dnienia ogranicze« na k¡t skr¦tu
przedniego koªa oraz wymuszenia zerowej warto±ci pr¦dko±ci w pocz¡tkowej chwili
ruchu, 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 dodatkowe
sensory mo»e zwi¦kszy¢ autonomiczno±¢ modelu i umo»liwi¢ robotowi reakcj¦ na
bod¹ce zewn¦trzne. Interakcja z otoczeniem zwi¦kszy uniwersalno±¢ robota i zwi¦kszy zakres jego stosowania.
44
Bibliograa
[1] K. Tcho«: Kinematics of mobile manipulators: An endogenous conguration
space 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 roboty
mobilne. Akademicka Ocyna Wydawnicza Warszawa 2000.
[3] K. Tcho«, J. Jakubiak: Endogenous conguration space approach to mobile
manipulators: a derivation and performance assessment of Jacobian inverse
kinematics algorithms. Int. J. Control 2003, vol. 76, no. 14, 1387-1419.
[4] J. Jakubiak: Numeryczne aspekty metody Newtona planowania ruchu robotów
nieholonomicznych. Praca magisterska pod kierunkiem dr. in». I. Dul¦by, PWr
1999.
[5] M. Kabaªa, M. Wnuk: Konstrukcja i oprogramowanie dwukoªowego robota
mobolnego. Raport SPR 20/2002, Instytut Cybernetyki Technicznej PWr 2002.
[6] C. C. de Vit, B. Siciliano, G. Bastin: Theory of Robot Control. Springer-Verlag
London 1996.
[7] M. J. Griendel, Z. Hendzel, W. ›ylski: Modelowanie i sterowania mobilnych
robotów koªowych. Wydawnictwo Naukowe PWN, 2002.
45