3  Przykład 1. Platforma mobilna klasy (2,0)

Platforma (2,0) posiada dwa niezależnie napędzane sztywne koła ustalone. Przy wyprowadzaniu modelu platformy (2,0) przyjęto założenie o tocznym, bezpoślizgowym ruchu kół. Przyjęcie takiego założenia powoduje pojawienie się ograniczeń nieholonomicznych. Figure Figure 1: Schemat kinematyki platformy (2,0).

3.1  Kinematyka we współrzędnych uogólnionych

Jako współrzędne platformy zostały wybrane następujące zmienne

q = ⎛ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎝ x y θ ϕ1 ϕ2 ⎞ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎠ .

(30)

Współrzędne (x,y) opisują położenie środka masy platformy mobilnej (początek lokalnego układu współrzędnych 0X_pY_p) względem nieruchomego układu odniesienia 0X₀Y₀; współrzędne (x_A,y_A) oraz (x_B,y_B) opisują położenie środków odpowiednich kół w tym samym nieruchomym układzie bazowym; θ stanowi orientację platformy mobilnej (kąt pomiędzy osią 0X_p lokalnego układu współrzędnych związanego z platformą, a osią 0X₀ układu bazowego); kąty ϕ₁ oraz ϕ₂ są kątami obrotu kół platformy odpowiednio względem osi 0X_A, 0X_B i ich lokalnych układów współrzędnych. Dla kół ustalonych i sterowanych warunek na brak poślizgu wzdłużnego wyrażony jest za pomocą wzoru

⋅ x   cosθ+ ⋅ y   sinθ−R ⋅ ϕ   =0,

(31)

natomiast brak poślizgu bocznego zapewnia spełnienie warunku

⋅ x   sinθ− ⋅ y   cosθ = 0,

(32)

gdzie :
(x,y) - współrzędne punktu kontaktu koła z podłożem,
θ - orientacja koła,
kąt ϕ - kąt obrotu koła,
R - promień koła.
Zgodnie ze wzorem (32), przy oznaczeniach takich jak na rys. 1, dla obu kół platformy można zapisać warunek na brak poślizgu wzdłużnego jako

⋅ x   A  cosθ+ ⋅ y   A  sinθ−R ⋅ ϕ1   =0, ⋅ x   B  cosθ+ ⋅ y   B  sinθ−R ⋅ ϕ2   =0.

(33)

Współrzędne środków kół należy wyrazić za pomocą współrzędnych środka masy platformy, będącego - w tym przypadku - również środkiem osi obrotu obu kół. Odległość środków kół od środka masy platformy wynosi L. Z zależności geometrycznych z rysunku 1 wynika, że

xA=x−x1, xB=x+x1, yA=y+y1, yB=y−y1.

(34)

Stosując funkcje trygonometryczne można wyznaczyć następujace równania:

x1 L =sinθ⇒ x1=Lsinθ, y1 L =cosθ⇒ y1=Lcosθ.

(35)

Z równań (35) i (36) wyznaczono współrzędne środków kół jako

xA=x−Lsinθ, xB=x+Lsinθ, yA=y+Lcosθ, yB=y−Lcosθ.

(36)

Pochodne po czasie są równe

⋅ x   A  = ⋅ x   −Lcosθ· ⋅ θ   , ⋅ x   B  = ⋅ x   +Lcosθ· ⋅ θ   , ⋅ y   A  = ⋅ y   −Lsinθ· ⋅ θ   , ⋅ y   B  = ⋅ y   +Lsinθ· ⋅ θ   .

(37)

Podstawiając otrzymane wyrażenia do równiania (34), otrzymano ostateczne wzory na brak poślizgu wzdłużnego koła A i B.
Ograniczenie na brak poślizgu wzdłużnego koła A przyjmuje postać

⋅ x   ·cosθ+ ⋅ y   sinθ−L· ⋅ θ   −R·ϕ1=0,

(38)

natomiast koła B

⋅ x   ·cosθ+ ⋅ y   sinθ+L· ⋅ θ   −R·ϕ2=0.

(39)

Drugie ograniczenie fazowe wynika z warunku na brak poślizgu bocznego wyrażonego przez równanie (33). Warunek na brak poślizgu bocznego dla kół A i B wyraża się odpowiednio wzorami

⋅ x   A  sinθ− ⋅ y   A  cosθ = 0,

(40)

⋅ x   B  sinθ− ⋅ y   B  cosθ = 0.

(41)

Po podstawieniu wyrażeń (38) dla obu kół otrzymano taki sam wynik

⋅ x   sinθ− ⋅ y   cosθ = 0.

(42)

Koła A i B znajdują się na sztywnej osi w równych odległościach od puntku (x,y). Można zatem przyjąć, iż warunek braku poślizgu bocznego dla obu kół jest identyczny i równoważny warunkowi braku poślizgu dla punktu leżącego w punkcie (x,y).
Rówania (39), (40) oraz (43) określają wszystkie zależności na brak poślizgu w przypadku modelowanej platformy. Zależnosci te zapisane w postaci układu równań

⎧ ⎪ ⎪ ⎨ ⎪ ⎪ ⎩ ⋅ x   sinθ− ⋅ y   cosθ = 0 ⋅ x   cosθ+ ⋅ y   sinθ−L· ⋅ θ   −R·ϕ1=0 ⋅ x   cosθ+ ⋅ y   sinθ+L· ⋅ θ   −R·ϕ2=0 ,

(43)

określającego ograniczenia fazowe można zapisać w postaci Pfaffa wyrażonej wzorem (5), otrzymując

⎡ ⎢ ⎢ ⎢ ⎣ sinθ   −cosθ 0 0 0 cosθ   sinθ −L −R 0 cosθ   sinθ L 0 −R ⎤ ⎥ ⎥ ⎥ ⎦ · ⎛ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎝ ⋅ x   ⋅ y   ⋅ θ   ⋅ ϕ1   ⋅ ϕ2   ⎞ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎠ = ⎛ ⎜ ⎜ ⎜ ⎝ 0 0 0 ⎞ ⎟ ⎟ ⎟ ⎠ .

(44)

Macierz A(q) jest zatem postaci

⎡ ⎢ ⎢ ⎢ ⎣ sinθ   −cosθ 0 0 0 cosθ   sinθ −L −R 0 cosθ   sinθ L 0 −R ⎤ ⎥ ⎥ ⎥ ⎦ .

(45)

3.2  Kinematyka we współrzędnych pomocniczych

We współrzędnych pomocniczych równania kinematyki przyjmują postać tzw. bezdryfowego układu sterowania postaci (9). Korzystając z macierzy (46) wyznaczona zostaje macierz

G(q) = ⎛ ⎜ ⎜ ⎝ g1 g2 ⎞ ⎟ ⎟ ⎠ ,

(46)

gdzie wektory q₁(q) i g₂(q) są liniowo nieżależne i pozwalają spełnić równanie bezdryfowego układu sterowania. Warunki te spełniają wektory

g1(q) = ⎛ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎝ cosθ sinθ 1 L 0 2 r ⎞ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎠ ,

(47)

g2(q) = ⎛ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎝ cosθ sinθ − 1 L 2 r 0 ⎞ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎠ .

(48)

Bezdryfowy układ sterowania przyjmuje zatem postać

⎛ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎝ ⋅ x   ⋅ y   ⋅ θ   ⋅ ϕ   1  ⋅ ϕ   2  ⎞ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎠ = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ cosθ   cosθ sinθ   sinθ 1 L   −1 L 0   2 r 2 r 0   0 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎛ ⎜ ⎜ ⎝ η1 η2 ⎞ ⎟ ⎟ ⎠ ,

(49)

przy czym prędkości pomocnicze η₁,η₁, spełniające rolę sterowań, mają sens przeskalowanych prędkości obrotu poszczególnych kół platformy, co opisuje 4 i 5 wiersz równania (50).

4  Przykład 2. Platforma klasy (2,0)

4.1  Model dynamiki

Punktem wyjścia do obliczeń dynamiki platformy klasy (2,0) jest funkcja Lagrange'a. Funkcja ta zgodnie z [] przyjmuje postać:

L(q, ⋅ q   )=Ek(q, ⋅ q   )−Ep(q),

(50)

gdzie: E_k(q,· q) - energia kinetyczna układu swobodnego, E_p(q) - energia potencjalna układu swobodnego. Ponieważ środek masy platformy nie zmienia swojej odległości od powierzchni ziemi, nie ma ona wpływu na ruch rozpatrywanej platformy, energia potencjalna układu jest zatem stała.

4.1.1  Energia kinetyczna platformy

Energia kinetyczna platformy mobilnej klasy (2,0) składa się z energii kinetycznej nadwozia oraz energii kinetycznej dwóch kół.

Ek = Ekp + Ekk1 + Ekk2,

(51)

gdzie:
E_kp - energia kinetyczna nadwozia,
E_kki - energia kinetyczna i-tego koła. Pozycja [] zawiera szczegółowe sposoby wyliczania energii kinetycznej i potencjalnej dla robota mobilnego. Korzystając z tej rozprawy można wyrazić energię kinetyczną platformy (bez kół) jako

Ekp = 1 2 ·Iz · ⋅ θ   2   + 1 2 ·Mp ·( ⋅ x   2   + ⋅ y   2   ),

(52)

gdzie: I_z - moment bezwładności nadwozia względem pionowej osi, przecinającej oś kół w połowie, M_p - masa platformy bez kół.

4.2  Energia kinetyczna kół

Aby wyliczyć energię kinetyczną kół należy dokonać transformacji z układu współrzędnych (X₀,Y₀) przedstawionego na rysunku 1, do układów współrzędnych każdego z kół. W pierwszej kolejności należy wykonać transformację A^P₀ do układu platformy³, a następnie transformacje do układów każdego z kół: (X_A,Y_A), (X_B,Y_B). Transformacja do układu platformy przybiera następującą postać:

AP0 = T(X,x) ·T(Y,y) ·R(Z,θ),

(53)

gdzie:
T(a,b) - macierz elementarnego przesunięcia układu wzdłuż osi a o wektor długości b,
R(c,d) - macierz elementarnego obrotu układu wokół osi c o kąt d.
Transformacja do układu koła nr 1 zgodnie z oznaczeniami z rysunku 1 wyraża się jako

AK10 = A0P ·R(Z, π 2 ) ·T(X, l 2 ) ·R(X,ϕ1) ,

(54)

natomiast transformacja do układu koła nr 2

AK20 = A0P ·R(Z, −π 2 ) ·T(X, l 2 ) ·R(X,ϕ2).

(55)

Znając powyższe transformacje i korzystając ze wzoru na energię kinetyczną koła, wyprowadzonego w rozprawie [] (str. 44 wzór 3.27) obliczono energię kinetyczną pierwszego koła jako

Ek1 = 1 2 ·IXX1 ·ϕ12 + 1 2 IZZ1 · ⋅ θ   2   + 1 2 Mki · ⎡ ⎢ ⎣ ⋅ x   2   + ⋅ y   2   + l2 ⋅ θ   2   2 − l ⋅ θ   ( ⋅ y   sinθ+ ⋅ x   cosθ) ⎤ ⎥ ⎦ ,

(56)

gdzie:
I_XXi - moment bezwładności koła i-tego wokół osi X_i,
I_ZZi - moment bezwładności koła i-tego wokół osi Z_i,
M_ki - masa i-tego koła.
Analogicznie możliwe jest wyrażenie energii kinetycznej drugiego koła jako

Ek2 = 1 2 ·IXX2 ·ϕ22 + 1 2 IZZ2 · ⋅ θ   2   + 1 2 Mki · ⎡ ⎢ ⎣ ⋅ x   2   + ⋅ y   2   + l2 ⋅ θ   2   2 + l ⋅ θ   ( ⋅ y   sinθ+ ⋅ x   cosθ) ⎤ ⎥ ⎦ .

(57)

4.2.1  Energia kinetyczna całego układu

Zakładając identyczność obu kół (równe masy: M_k=M_k1=M_k2 oraz taki sam moment bezwładności: I_XX=I_XX1=I_XX2) otrzymano zależność na energię kinetyczną robota mobilnego klasy (2,0):

EK = 1 2 IZ · ⋅ θ   2   + 1 2 Mp( ⋅ x   2   + ⋅ y   2   ) + 1 2 IXX( ⋅ ϕ   2   + ⋅ ϕ   2   )+2 · 1 2 IZZ · ⋅ θ   2   + 2 · 1 2 Mk · ⎛ ⎝ ⋅ x   2   + ⋅ y   2   ⎞ ⎠ +Mk · L2 θ2 4

(58)

Grupując wyrazy według współrzędnych wektora · q otrzymano wzór zawierający szukaną macierz bezwładności M(q)

Ek = 1 2 qT M (x,y,θ,ϕ1, ϕ2) · ⋅ q   = 1 2 ⋅ x   2   (Mp + 2Mk)+ 1 2 ⋅ y   2   (Mp + 2Mk) + 1 2 ⋅ θ   2   (2Izz + 1 2 Mk L2) + 1 2 Ixx ⋅ ϕ1   2   + 1 2 Ixx ⋅ ϕ2   2   .

(59)

Możliwe jest zatem zapisanie energii kinetycznej układu w postaci macierzowej

Ek = 1 2 ( ⋅ x   , ⋅ y   , ⋅ θ   , ⋅ ϕ1   , ⋅ ϕ2   ) · ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ Mc 0 0 0 0 0 Mc 0 0 0 0 0 Ip 0 0 0 0 0 Ixx 0 0 0 0 0 Ixx ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ · ⎛ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎝ ⋅ x   ⋅ y   ⋅ θ   ⋅ ϕ1   ⋅ ϕ2   ⎞ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎠ ,

(60)

gdzie:
M_c - masa całkowita robota (równa masie kół i nadwozia),
I_p - moment bezwładności robota (wraz z kołami) wokół osi Z.

4.2.2  Równanie dynamiki we współrzędnych uogólnionych

Model dynamiki oparty jest na wzorze uzyskanym z równania Lagrange'a oraz zasady d'Alemberta.

M(q) ⋅⋅ q   + C(q, ⋅ q   ) ⋅ q   +D(q)=AT(q)λ+Bu,

(61)

gdzie: q,· q,·· q, - wektory zawierające odpowiednio położenie, prędkości oraz przyspieszenia (zmienne uogólnione), M(q) - macierz bezwładności, C(q,· q) - macierz sił odśrodkowych i sił Coriolisa, D(q) - macierz grawitacji, λ - wektor mnożników Lagrange'a, B - macierz zmiennych, u - sterowania.
Ponieważ przestrzeń ruchu robota składa się z dwóch wymiarów i nie ma w niej wysokości, przez co można przyjąć brak zmian energii potencjalnej, macierz grawitacji D(q)=0.
Macierz bezwładności M(q) wyliczono na podstawie energii całego układu (61), przyjmuje ona postać

M(q)= ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ Mc 0 0 0 0 0 Mc 0 0 0 0 0 Ip 0 0 0 0 0 Ixx 0 0 0 0 0 Ixx ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦

(62)

W rozpatrywanym przykładzie wszystkie elementy macierzy M(q) są stałe zatem macierz C(q,· q) wynosi 0.

4.2.3  Dynamika we współrzędnych pomocniczych

Wykorzystując wyprowadzony wcześniej bezdryfowy układ sterowania 50 możliwe jest zapisanie modelu dynamiki we współrzędnych pomocniczych. Należy dokonać podstawienia równania do modelu (tu odwołanie do wzoru ze wstępu teoretycznego)

GT(q)M(q)G(q) M(q)*  ⋅ η   + GT(q) M(q) ⋅ G   (q) C(q,· q)*   η = GT(q)B(q) B(q)*  u

(63)

Zgodnie z powyższym model dynamiki we współrzędnych pomocniczych dla platformy (2,0) przyjmuje postać

M*(q) ⋅ η   + C*(q, ⋅ q   )η = B*(q) u.

(64)

Nowa macierz bezwładności wygląda następująco

M* = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ Mc+ 1 l2 Ip+ 4 R2 Ixx MC− 1 l2 Ip Mc− 1 l2 Ip Mc+ 1 l2 Ip+ 4 R2 Ixx ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ .

(65)

Macierz sterowań zmienia postać z B na B^*

B = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ 0 0 0 0 0 0 1 0 0 1 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ,

(66)

B* = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ 2 R 0 0 2 R ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ .

(67)

W przypadku platformy klasy(2,0) we współrzędnych pomocniczych wyzerowaniu ulega macierz sił Coriolisa

C*=0.

(68)

5  Miary jakosci manipulatorów nieholonomicznych

Miary jakości manipulatorów mobilnych dzielą się na miary lokalne i globalne. Miary lokalne opisują zachowanie manipulatorów mobilnych w określonej konfiguracji, natomiast miary globalne opisują całkowite zachowanie się manipulatorów mobilnych. Miary jakości manipulatorów mobilnych służą do ustalenia optymalnej konfuguracji manipulatora mobilnego. Wyznaczenie odpowiednich zmiennych przegubowych (x₁;x₂) oraz funkcji sterujących platformą (u(·)) w taki sposób, aby zmaksymalizaować lokalną zręczność kinematyczną, pozwala na ustalenie optymalnej konfiguracji [].

5.1  Wyznaczanie macierzy zrecznosci kinematycznej.

Macierz zręczności kinematycznej [].

Dq0,T(u(·),x) = D(T, x)DT(T, x) + C(T, x) Mq0,T(u(·))CT(T, x)

(69)

W skład macierzy zręczności kinematyczej wchodzi macierz mobilności kinematycznej platformy [].

⋅ M   q0,T  (u(·)) = B(t)BT + A(t)Mq0,T(u(·))+Mq0,T(u(·))A(t)

(70)

Macierze A, B, C, D są to macierze przybliżenia liniowego bezdryfowego układu sterowania (wzór 13).

5.2  Wyznaczanie miary lokalnej zrecznosci kinematycznej.

Lokalna zręczność kinematyczna jest pierwiastkiem z wyznacznika macierzy zręczności kinematycznej.

mq0,T(u(·), x = √   detDq0,T(u(·),x)

(71)

Zręczność w konfiguracji osobliwej manipulatora mobilnego wynosi 0. Lokalna zręcznośc kinematyczna jest własnością konfiguracji endogenicznej manipulatora mobilnego, opisująca możliwości ruchu efektora manipulatora w odpowiedzi na lokalne zmiany konfiguracji endogenicznej. Z punktu widzenia teorii sterowania, lokalna zręczność kinematyczna stanowi miarę sterowalności wyjścia przybliżenia liniowego (13) układu sterowania (12) reprezentującego kinematykę manipulatora mobilnego. Można pokazać, że umieszczenie manipulatora stacjonarnego na platformie mobilnej przyczynia się do wzrostu jego zręczności [].

Autorzy : Mirella Frontkiewcz, Weronika Matlakiewcz, Sławomir Paszko, Zuzanna Pietrowska, Monika Puchalska, Łukasz Żygadło