7  Przykład 3. Platforma mobilna sterowana różnicowo

Figure Figure 3: Schemat kinematyki robota mobilnego sterowanego różnicowo.

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

Tworzenie modelu kinematyki należy rozpocząć od zdefiniowania wektora stanu q, który w rozpatrywanym przykładzie przybiera postać

q = ⎛ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎝ x y θ φ1 φ2 φ3 φ4 ⎞ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎠ ,

(97)

gdzie:

• x,y - współrzędne położenia,
• θ - orientacja,
• φ₁, φ₂, φ₃, φ₄ - kąty obrotu poszczególnych kół.

Kolejnym krokiem jest wybranie ograniczeń, które ma spełniać opisywany model. Oczywistym jest, że przy zmianie orientacji platformy, któreś z kół będzie wpadać w poślizg poprzeczny, tak więc tego ograniczenia nie można narzucić. Sensowne natomiast jest wprowadzenie ograniczeń o braku poślizgu wzdłużnego kół, które w formie ogólnej przyjmuje postać

v − ⋅ φ   R = 0.

(98)

W takim wypadku kąty obrotu kół 1 i 3 muszą być sobie równe, podobnie jak kąty obrotu kół 2 i 4, co modyfikuje zaproponowany wektor q do postaci

q = ⎛ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎝ x y θ φL φP ⎞ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎠ ,

(99)

gdzie φ_L, φ_P to kąty obrotu odpowiednio lewych (koło 2 i 4) i prawych (koło 1 i 3) kół.
Zakładając, że v₁, v₂, v₃, v₄ to prędkości poszczególnych kół, oraz że promienie kół R₁ = R₂ = R₃ = R₄ = R, to postać równań ograniczenia braku poślizgu wzdłużnego dla tego modelu jest następująca

v1 − ⋅ φ   P  R = 0, v2 − ⋅ φ   L  R = 0, v3 − ⋅ φ   P  R = 0, v4 − ⋅ φ   L  R = 0.

(100)

Uwzględniając zależności trygonometryczne widoczne na rysunku prędkości v₁, v₂, v₃ oraz v₄ zapisywane są w następujący sposób

v1 = ⋅ x   1  cosθ+ ⋅ y   1  sinθ, v2 = ⋅ x   2  cosθ+ ⋅ y   2  sinθ, v3 = ⋅ x   3  cosθ+ ⋅ y   3  sinθ, v4 = ⋅ x   4  cosθ+ ⋅ y   4  sinθ.

(101)

Zgodnie z zależnościami trygonometrycznymi widocznymi na wykresie wartości · x₁ − · x₄, · y₁ − · y₄ można zapisać jako

x1 = x − l cosθ+ d sinθ, ⋅ x   1  = ⋅ x   + ⋅ θ   l sinθ+ ⋅ θ   d cosθ, y1 = y − l sinθ− d cosθ, ⋅ y   1  = ⋅ y   − ⋅ θ   l cosθ+ ⋅ θ   d sinθ, x2 = x − l cosθ− d sinθ, ⋅ x   2  = ⋅ x   + ⋅ θ   l sinθ− ⋅ θ   d cosθ, y2 = y − l sinθ+ d cosθ, ⋅ y   2  = ⋅ y   − ⋅ θ   l cosθ− ⋅ θ   d sinθ, x3 = x + l cosθ+ d sinθ, ⋅ x   3  = ⋅ x   − ⋅ θ   l sinθ+ ⋅ θ   d cosθ, y3 = y + l sinθ− d cosθ, ⋅ y   3  = ⋅ y   + ⋅ θ   l cosθ+ ⋅ θ   d sinθ, x4 = x + l cosθ− d sinθ, ⋅ x   4  = ⋅ x   − ⋅ θ   l sinθ− ⋅ θ   d cosθ, y4 = y + l sinθ+ d cosθ, ⋅ y   4  = ⋅ y   + ⋅ θ   l cosθ− ⋅ θ   d sinθ.

(102)

Używając tych wyrażeń można ograniczenia fazowe (101) wyrazić w następujący sposób

⋅ x   cosθ+ ⋅ y   sinθ+ d ⋅ θ   − ⋅ φ   P  R = 0, ⋅ x   cosθ+ ⋅ y   sinθ− d ⋅ θ   − ⋅ φ   L  R = 0.

(103)

Zgodnie z postacią (104) poszukiwana macierz A(q) przybiera postać

A(q) = ⎡ ⎢ ⎢ ⎣ cosθ sinθ −d −R 0 cosθ sinθ d 0 −R ⎤ ⎥ ⎥ ⎦ .

(104)

7.2  Kinematyka we współrzędnych pomocniczych

W tym przypadku równania kinematyki przyjmują postać

⋅ q   =G(q)η.

(105)

Jest to bezdryfowy układ sterowania, spełniający pewne warunki:

• A(q) g_i = 0,
• g_i ≠ 0,
• i ∈ {1,2,...,m}, przy czym m = n − l, gdzie:
  • n - wymiar przestrzeni (liczba współrzędnych wektora q),
  • l - liczba ograniczeń (liczba wektorów macierzy A(q)).

W rozpatrywanym przypadku m = 5− 2 = 3.
Należy również pamiętać, żeby wybierane wektory były jak najprostsze.

Zgodnie z powyższymi kryteriami dobrano trzy wektory g macierzy G(q).

g1 = ⎛ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎝ sinθ −cosθ 0 0 0 ⎞ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎠ ,   g2 = ⎛ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎝ 0 0 R l −l ⎞ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎠ ,   g3 = ⎛ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎝ R cosθ R sinθ 0 1 1 ⎞ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎠ .

(106)

Uwzględniając wektory z (106) macierz G(q) przyjmuje postać

G(q) = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ sinθ 0 R cosθ −cosθ 0 R sinθ 0 R 0 0 l 1 0 −l 1 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ .

(107)

8  Przykład 3. Platforma mobilna sterowana różnicowo

8.1  Model dynamiki

Figure Figure 4: Układy współrzędnych stowarzyszone z kołami robota mobilnego sterowanego różnicowo Wektor stanu jest równy

q = ⎛ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎝ x y θ φL φP ⎞ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎠ .

(108)

Aby wyprowadzić model dynamiki należy zdefiniować funkcję Lagrange'a, zwaną lagranżjanem, postaci

L(q, ⋅ q   ) = EK(q, ⋅ q   ) − EP(q),

(109)

gdzie:

• E_K - energia kinetyczna robota,
• E_P - energia potencjalna robota.

Ponieważ rozpatrywana platforma mobilna porusza się jedynie po płaszczyźnie, to jej energia potencjalna jest stała (E_P(q) = const).

8.1.1  Energia kinetyczna platformy

Energia kinetyczna pojazdu jest równa sumie energii kinetycznej platformy i wszystkich kół.

Energia kinetyczna platformy wyrażana jest wzorem (pozycja [])

EKP = 1 2 Iz ⋅ θ   2   + 1 2 MP( ⋅ x   2   + ⋅ y   2   ),

(110)

gdzie:

• I_z - moment bezwładności platformy liczony względem lokalnej osi Z,
• M_P - masa platformy.

8.1.2  Energia kinetyczna kół

W dalszych obliczeniach będą obowiązywać następujące założenia:

• M_K1 = M_K2 = M_K3 = M_K4 = M_K - masa koła,
• I_xx1 = I_xx2 = I_xx3 = I_xx4 = I_xx - moment bezwładności pojedynczego koła względem lokalnej osi X,
• I_zz1 = I_zz2 = I_zz3 = I_zz4 = I_zz - moment bezwładności pojedynczego koła względem lokalnej osi Z.

Transformację układu współrzędnych od układu bazowego X₀,Y₀ do układu i-tego koła wyprowadza się korzystając ze wzoru

A0Ki = A0P(x,y,θ) ·Rot(Z,αi) ·Trans(X,li) ·Rot(Z,βi) ·Trans(X,di) ·Rot(Z,γi) ·Rot(X,ϕ),

(111)

gdzie A₀^P(x,y,θ) - transformacja bazowego układu współrzędnych do układu platformy X_P,Y_P.

Zgodnie z rysunkiem 4, wyprowadzono transformacje opisujące układy poszczególnych kół względem układu bazowego.

Transformacja układu platformy względem bazowego układu współrzędnych

A0P = Trans(X,x) ·Trans(Y,y) ·Rot(Z,θ).

(112)

Transformacja układu 1 koła względem układu bazowego

A0K1 = A0P ·Trans(X,−l) ·Rot(Z,− π 2 ) ·Trans(X,d) ·Rot(X,φP).

(113)

Transformacja układu 2 koła względem układu bazowego

A0K2 = A0P ·Trans(X,−l) ·Rot(Z, π 2 ) ·Trans(X,d) ·Rot(X,φL).

(114)

Transformacja układu 3 koła względem układu bazowego

A0K3 = A0P ·Trans(X,l) ·Rot(Z,− π 2 ) ·Trans(X,d) ·Rot(X,φP).

(115)

Transformacja układu 4 koła względem układu bazowego

A0K4 = A0P ·Trans(X,l) ·Rot(Z, π 2 ) ·Trans(X,d) ·Rot(X,φL).

(116)

Zgodnie z (22) oraz obowiązującymi założeniami, energie kinetyczne kół wynoszą

EKK1 = 1 2 Ixx ⋅ ϕ   2 P  + 1 2 Izz ⋅ θ   2   + 1 2 MK ⎧ ⎨ ⎩ ⋅ x   2   + ⋅ y   2   + d2 ⋅ θ   2   + l2 ⋅ θ   2   + 2 d ⋅ θ   ⎡ ⎣ ⋅ y   sinθ+ ⋅ x   cosθ ⎤ ⎦ − 2 l ⋅ θ   ⎡ ⎣ ⋅ y   cosθ− ⋅ x   sinθ ⎤ ⎦ ⎫ ⎬ ⎭ ,

(117)

EKK2 = 1 2 Ixx ⋅ ϕ   2 L  + 1 2 Izz ⋅ θ   2   + 1 2 MK ⎧ ⎨ ⎩ ⋅ x   2   + ⋅ y   2   + d2 ⋅ θ   2   + l2 ⋅ θ   2   + 2 d ⋅ θ   ⎡ ⎣ − ⋅ y   sinθ− ⋅ x   cosθ ⎤ ⎦ − 2 l ⋅ θ   ⎡ ⎣ ⋅ y   cosθ− ⋅ x   sinθ ⎤ ⎦ ⎫ ⎬ ⎭ ,

(118)

EKK3 = 1 2 Ixx ⋅ ϕ   2 P  + 1 2 Izz ⋅ θ   2   + 1 2 MK ⎧ ⎨ ⎩ ⋅ x   2   + ⋅ y   2   + d2 ⋅ θ   2   + l2 ⋅ θ   2   + 2 d ⋅ θ   ⎡ ⎣ ⋅ y   sinθ+ ⋅ x   cosθ ⎤ ⎦ + 2 l ⋅ θ   ⎡ ⎣ ⋅ y   cosθ− ⋅ x   sinθ ⎤ ⎦ ⎫ ⎬ ⎭ ,

(119)

EKK4 = 1 2 Ixx ⋅ ϕ   2 L  + 1 2 Izz ⋅ θ   2   + 1 2 MK ⎧ ⎨ ⎩ ⋅ x   2   + ⋅ y   2   + d2 ⋅ θ   2   + l2 ⋅ θ   2   + 2 d ⋅ θ   ⎡ ⎣ − ⋅ y   sinθ− ⋅ x   cosθ ⎤ ⎦ + 2 l ⋅ θ   ⎡ ⎣ ⋅ y   cosθ− ⋅ x   sinθ ⎤ ⎦ ⎫ ⎬ ⎭ .

(120)

8.1.3  Energia kinetyczna całego układu

Po zsumowaniu wyliczonych energii składowych energia kinetyczna pojazdu jest równa

EK(q, ⋅ q   ) = 1 2 ⎧ ⎨ ⎩ (MP+4MK) MC  ⋅ x   2   + (MP+4MK) MC  ⋅ y   2   + [Iz + 4Izz + 4MK(d2 + l2)] IP  ⋅ θ   + 2Ixx ⋅ ϕ   2 L  + 2Ixx ⋅ ϕ   2 P  ⎫ ⎬ ⎭ ,

(121)

gdzie:

• M_C - całkowita masa robota,
• I_P - moment bezwładności robota.

8.1.4  Równania dynamiki we współrzędnych uogólnionych

Korzystając z () widać, że macierz bezwładności wygląda następująco

M(q) = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ MC 0 0 0 0 0 MC 0 0 0 0 0 IP 0 0 0 0 0 2Ixx 0 0 0 0 0 2Ixx ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ .

(122)

Elementy macierzy sił Coriolisa można wyliczyć używając symboli Cristoffela, jednak w rozpatrywanym przykładzie (ponieważ wszystkie składniki macierzy M(q) są niezależne) wszystkie z nich są zerowe, więc

C(q, ⋅ q   ) = 0.

(123)

Wektor sił grawitacji można wyliczyć z zależności

D(q) = ∂EP(q) ∂q .

(124)

W rozpatrywanym przykładzie E_P(q) = const, więc D(q) = 0.

8.1.5  Dynamika we współrzędnych pomocniczych

Aby otrzymać model dynamiki we współrzędnych pomocniczych wykorzystuje się wyprowadzony wcześniej bezdryfowy układ sterowania

⋅ q   =G(q)η.

(125)

Należy dokonać podstawienia równania (125) do modelu (23)

M(q) · ⎛ ⎝ ⋅ G   (q)η+ G(q) ⋅ η   ⎞ ⎠ = B ·u + AT(q) ·λ.

(126)

W celu otrzymania macierzy kwadratowej całość mnoży się lewostronnie przez G^T(q)

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

(127)

Zgodnie z powyższym, model dynamiki we współrzędnych pomocniczych dla modelu pojazdu sterowanego różniocowo przyjmuje postać

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

(128)

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

M* = ⎡ ⎢ ⎢ ⎢ ⎣ MC 0 0 0 R2IP + 4l2Ixx 0 0 0 MCR2 + 4Ixx ⎤ ⎥ ⎥ ⎥ ⎦ .

(129)

Macierz sił Coriolisa przybiera postać

C* = ⎡ ⎢ ⎢ ⎢ ⎣ 0 0 −MCR 0 0 0 MCR 0 0 ⎤ ⎥ ⎥ ⎥ ⎦ .

(130)

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

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

(131)

B* = ⎡ ⎢ ⎢ ⎢ ⎣ 0 0 l −l 1 1 ⎤ ⎥ ⎥ ⎥ ⎦ ,

(132)

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