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

Platforma mobilna klasy (1,1), nazywana również samochodem kinematycznym, jest przykładem platformy z ograniczoną mobilnością. Figure Figure 2: Schemat kinematyczny platformy mobilnej klasy (1,1).

6.1  Kinematyka

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

Na początek wybieramy wektor zmiennych stanu

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

(72)

gdzie:

• (x,y) - położenie początku układu lokalnego stowarzyszonego z robotem w układzie bazowym,
• θ - orientacja platformy,
• β - kąt skrętu kierownicy,
• φ_i - kąt obrotu i-tego koła.

W przypadku platformy mobilnej klasy (1,1) wprowadzamy do modelu zarówno ograniczenia na brak poślizgów wzdłużnych - jak i poprzecznych - wszystkich kół. Ograniczenia te dla poszczególnych kół, zgodnie z oznaczeniami na rysunku, dane są odpowiednio wzorami:

1. Ograniczenia na brak poślizgów poprzeczych kolejnych kół
   
   

   ⋅ x   sin(θ+β) − ⋅ y   cos(θ+β)=0, ⋅ x   2  sinθ− ⋅ y   2  cosθ = 0, ⋅ x   3  sinθ− ⋅ y   3  cosθ = 0,

   (73)
2. Ograniczenia na brak poślizgów wzdłużnych kolejnych kół
   
   

   ⋅ x   cos(θ+β) + ⋅ y   sin(θ+β) − Rφ1=0, ⋅ x   2  cosθ+ ⋅ y   2  sinθ− Rφ2=0, ⋅ x   3  cosθ+ ⋅ y   3  sinθ− Rφ3=0.

   (74)

Następnym krokiem jest wyrażenie równań (74) i (75) za pomocą zmiennych stanu. Do tego celu wykorzystujemy widoczne na rysunku zależności

x1=x−lcosθ,               y1=y−lsinθ, x2=x1+bsinθ,               y2=y1−bcosθ, x3=x1−bsinθ,               y3=y1+bcosθ.

(75)

Różniczkując (76) po czasie otrzymujemy:

⋅ x   1  = ⋅ x   +l ⋅ θ   sinθ,                ⋅ y   1  = ⋅ y   −l ⋅ θ   cosθ, ⋅ x   2  = ⋅ x   1  +b ⋅ θ   cosθ,                ⋅ y   2  = ⋅ y   1  +b ⋅ θ   sinθ, ⋅ x   3  = ⋅ x   1  −b ⋅ θ   cosθ,                ⋅ y   3  = ⋅ y   1  −b ⋅ θ   sinθ.

(76)

Podstawiając powyższe do (74), (75), otrzymujemy ograniczenia fazowe w postaci Pfaffa:

⎧ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎩ ⋅ x   sin(θ+β) − ⋅ y   cos(θ+β)=0, ⋅ x   cos(θ+β) + ⋅ y   sin(θ+β) − Rφ1=0, ⋅ x   sinθ− ⋅ y   cosθ+l ⋅ θ   =0, ⋅ x   cosθ+ ⋅ y   sinθ+b ⋅ θ   − Rφ2=0, ⋅ x   cosθ+ ⋅ y   sinθ− b ⋅ θ   − Rφ3=0,

(77)

Ograniczenia te, dla podanej na początku kolejności zmiennych w wektorze stanu, zapisujemy w formie macierzowej (5), gdzie A(q) przybiera postać

A(q) = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ sin(θ+β) −cos(θ+β) 0 0 0 0 0 cos(θ+β) sin(θ+β) 0 0 −R 0 0 sinθ −cosθ l 0 0 0 0 cosθ sinθ b 0 0 −R 0 cosθ sinθ −b 0 0 0 −R ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ .

(78)

6.1.2  Kinematyka we współrzędnych pomocniczych

Znalezione w pierwszym podrozdziale ograniczenia, teraz zapisujemy w postaci bezdryfowego układu sterowania (9). Wpierw określamy liczbę sterowań. Jest ona równa ilości zmiennych stanu pomniejszonej o liczbę nałożonych na układ ograniczeń nieholonomicznych i w tym przypadku wynosi 2. Wektory g₁ i g₂ dobieramy w taki sposób by sterowania miały sens fizyczny

g1= ⎛ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎝ 0 0 0 1 0 0 0 ⎞ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎠ ,       g2= ⎛ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎝ cos(θ+β) sin(θ+β) sinβ l 0 1 R cosβ+ bsinβ l R cosβ− bsinβ l R ⎞ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎠ .

(79)

Zatem:

⋅ q   =G(q)η = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ 0 cos(θ+β) 0 sin(θ+β) 0 sinβ l 1 0 0 1 R 0 cosβ+ bsinβ l R 0 cosβ− bsinβ l R ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ . ⎛ ⎜ ⎜ ⎝ u1 u2 ⎞ ⎟ ⎟ ⎠ ,

(80)

przy czym u₁ steruje bezpośrednio kątem skrętu kierownicy β, a u₂ ma sens prędkości liniowej platformy.

⎧ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎩ ⋅ x   =cos(θ+β)u2 ⋅ y   =sin(θ+β)u2 ⋅ θ   = sinβ l u2 ⋅ β   =u1 ⋅ φ   1  = 1 R u2 ⋅ φ   2  = cosβ+ bsinβ l R u2 ⋅ φ   3  = cosβ− bsinβ l R u2.

(81)

6.2  Dynamika

Na rysunku 2 zaznaczono lokalny układ współrzędnych stowarzyszony z platformą mobilną (x_p,y_p). Jest on zaczepiony na środku przodu robota (poza środkiem masy). Przyjmujemy, że lokalne układy związane z każdym z obracających się kół znajdują się w środku danego koła, a ich osie są głównymi osiami bezwładności.

6.2.1  Energia kinetyczna platformy

Przy założeniu, że grubośc platformy jest pomijlnie mała, współrzędne środka masy w lokalnym układzie odniesienia (17) wynoszą odpowiednio

P1 = −Mp l 2 , P2 = 0. (82)

Moment bezwładności platformy względem osi z_p (lokalnego układu odniesienia) wynosi I_z=[1/3]M_p((b−[(ω)/2])²+l²). Powyższe należy podstawić do zależności opisującej energię kinetyczą platformy w układzie bazowym (16).

Ekp= 1 2 Iz ⋅ θ   + 1 2 Mp( ⋅ x   2   + ⋅ y   2   )+(−Mp l 2 )( ⋅ y   cosθ− ⋅ x   sinθ) ⋅ θ   .

(83)

Ostatecznie z (18)

K(ξ) = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ Mp 0 Mp l 2 sinθ 0 Mp −Mp l 2 cosθ Mp l 2 sinθ −Mp l 2 cosθ Iz ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ .

(84)

6.2.2  Energia kinetyczna kół

Zgodnie z rysunkiem, parmatery przekształcenia (20) wynoszą odpowiednio:

α1=0 l1=0 β1=β d1=0 γ1=0 φ1=φ1 ∆1=β+θ ,        α2=0 l2=−l β2=− Π 2 d2=b γ2=0 φ2=φ2 ∆2=θ− Π 2 ,        α3=0 l3=−l β3= Π 2 d3=b γ3=0 φ3=φ3 ∆3=θ+ Π 2 .

(85)

Podstawienie powyższego do wzoru () dla kolejnych kół daje następujące zależności na energię kinetyczną

Ek1 = 1 2 Izz1( ⋅ θ   + ⋅ β   )2 + 1 2 M( ⋅ x   2   + ⋅ y   2   )+ 1 2 Ixx1 ⋅ φ   2 1  , Ek2 = 1 2 Ixx2 ⋅ φ   2 2  + 1 2 Izz2 ⋅ θ   2   + 1 2 M( b2 ⋅ θ   2   +l2 ⋅ θ   2   + ⋅ x   2   + ⋅ y   2   +2b ⋅ θ   ( ⋅ y   cos(θ− Π 2 )− ⋅ x   sin(θ− Π 2 )) + 2(−l) ⋅ θ   ( ⋅ y   cosθ− ⋅ x   sinθ)), (86) Ek3 = 1 2 Ixx3 ⋅ φ   2 3  + 1 2 Izz3 ⋅ θ   2   + 1 2 M( b2 ⋅ θ   2   +l2 ⋅ θ   2   + ⋅ x   2   + ⋅ y   2   +2b ⋅ θ   ( ⋅ y   cos(θ+ Π 2 )− ⋅ x   sin(θ+ Π 2 )) + 2(−l) ⋅ θ   ( ⋅ y   cosθ− ⋅ x   sinθ)),

gdzie I_xx1=I_xx2=I_xx3 = I_xx oraz I_zz1=I_zz2=I_zz3=I_zz, zgodnie z przyjętym we wstępie założeniem o jednakowej masie oraz promieniu każdego z kół.

6.2.3  Energia kinetyczna całego układu

Energia kinetyczna całego układu jest to suma energii kinetycznych wszystkich jego elementów składowych.

Ek=Ekp+Ek1+Ek2+Ek3.

(87)

Ek= ⋅ x   2   1 2 Mc+ ⋅ y   2   1 2 Mc+ ⋅ θ   2   1 2 Ip + ⋅ β   2   1 2 Izz+ ⋅ φ1   2   1 2 Ixx+ ⋅ φ2   2   1 2 Ixx + ⋅ φ3   2   1 2 Ixx+ ⋅ x   ⋅ θ   A+ ⋅ y   ⋅ θ   B+ ⋅ β   ⋅ θ   Izz,

(88)

gdzie:
M_c=M_p+3M,
I_p=I_z+3I_zz+2Mb²+2Ml²,
A=M_p[l/2]sinθ+2Mlsinθ,
B=−M_p[l/2]cosθ−2Mlcosθ.
Przekształcając równanie (89) do postaci macierzowej (15), otrzymano następującą macierz formy

M(q) = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ Mc 0 A 0 0 0 0 0 Mc B 0 0 0 0 A B Ip Izz 0 0 0 0 0 Izz Izz 0 0 0 0 0 0 0 Ixx 0 0 0 0 0 0 0 Ixx 0 0 0 0 0 0 0 Ixx ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ .

(89)

Energia potencjalna układu jest stała, zgodnie z założeniem o niedeformowalności kół, a więc nie wpływa na równania ruchu. Funkcja Lagrange'a jest zatem równa energii kinetycznej układu L(q,· q) = E_k(q,· q).

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

Najpierw należy wyliczyć wyrazy potrzebne do skorzystania ze wzoru (26), czyli

d dt ∂L ∂ ⋅ x   = ⋅⋅ x   Mc+ ⋅⋅ θ   A+l ⋅ θ   2   cosθ( Mp 2 +2M), d dt ∂L ∂ ⋅ y   = ⋅⋅ y   Mc+ ⋅⋅ θ   B+l ⋅ θ   2   sinθ( Mp 2 +2M), d dt ∂L ∂ ⋅ θ   = ⋅⋅ θ   Ip+ ⋅⋅ x   A+ ⋅⋅ y   B+ ⋅⋅ β   Izz+l ⋅ x   ⋅ θ   cosθ( Mp 2 +2M)+l ⋅ y   ⋅ θ   sinθ( Mp 2 +2M), d dt ∂L ∂ ⋅ β   = ⋅⋅ β   Izz+ ⋅⋅ θ   Izz, d dt ∂L ∂ ⋅ φ1   = ⋅⋅ φ1   Ixx, (90) d dt ∂L ∂ ⋅ φ2   = ⋅⋅ φ2   Ixx, d dt ∂L ∂ ⋅ φ3   = ⋅⋅ φ3   Ixx, ∂L ∂x = ∂L ∂y = ∂L ∂β = ∂L ∂φ1 =0= ∂L ∂φ2 = ∂L ∂φ3 =0, ∂L ∂θ = ⋅ x   ⋅ θ   Mp l 2 cosθ+ ⋅ x   ⋅ θ   2Mlcosθ+ ⋅ y   ⋅ θ   Mp l 2 sinθ+ ⋅ y   ⋅ θ   2Mlsinθ.

Równania Lagrange'a dla układu swobodnego przyjmują więc postać

⎧ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎩ ⋅⋅ x   Mc+ ⋅⋅ θ   A+l ⋅ θ   2   cosθ( Mp 2 +2M)=0, ⋅⋅ y   Mc+ ⋅⋅ θ   B+l ⋅ θ   2   sinθ( Mp 2 +2M)=0, ⋅⋅ θ   Ip+ ⋅⋅ x   A+ ⋅⋅ y   B+ ⋅⋅ β   Izz=0, ⋅⋅ β   Izz+ ⋅⋅ θ   Izz=0, ⋅⋅ φ1   Ixx=0, ⋅⋅ φ2   Ixx=0, ⋅⋅ φ3   Ixx=0.

(91)

Model dynamiki platformy mobilnej klasy (1,1) przedstawiamy w uporządkowanej postaci (27), gdzie wektor grawiacji D(q) ≡ 0. Macierz inercji układu tożsama jest wcześniej obliczonej macierzy formy M(q). Macierz Coriolisa wyznaczamy natomiast ze wzoru (69).

C(q, ⋅ q   ) = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ 0 0 l ⋅ θ   cosθ( Mp 2 +2M) 0 0 0 0 0 0 l ⋅ θ   sinθ( Mp 2 +2M) 0 0 0 0 1 2 l ⋅ θ   cosθ( Mp 2 +2M) 1 2 l ⋅ θ   sinθ( Mp 2 +2M) 1 2 l( Mp 2 +2M)( ⋅ x   cosθ+ ⋅ y   sinθ) 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ .

(92)

Na rozpatrywany układ działa wektor uogólnionych sil zewnętrznych u=(u_a, u_b)^T. Macierz B występująca w równaniach dynamiki, informuje nas, na które współrzędne dana siła działa bezpośrednio

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

(93)

6.2.5  Równania dynamiki we współrzędnych pomocniczych

Znając opis kinematyki oraz dynamiki układu we współrzędnych uogólnionych oraz postać bezdryfowego układu sterowania (81), można równania dynamiki wyrazić we współrzędnych pomocniczych (30). Dokonując stosownych podstawień otrzymuje się D^*(q)=0 oraz

M* = GT(q)M(q)G(q) = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ Izz Izz sinβ l Izz sinβ l M1,1 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ .

(94)

C* = GT(q)(C(q,G(q)η)G(q)+M(q) ⋅ G   (q,G(q)η)] = ⎡ ⎢ ⎢ ⎢ ⎣ 0 Izz− 1 l u1cosβ 0 C1,1 ⎤ ⎥ ⎥ ⎥ ⎦ .

(95)

B* = GT(q)B(q) = ⎡ ⎢ ⎢ ⎢ ⎣ 1 0 0 1 R ⎤ ⎥ ⎥ ⎥ ⎦ .

(96)

gdzie:
M_1,1=M_c+2sin²β([(M_p)/2]+2M)+[(sin²β)/l]I_p+I_xx[1/(R²)](1+cos²β+b²sin²β),
C_1,1=cosβ[(sin²β)/l]u₂([(M_p)/2]+2M)+[1/2][(sinβ)/l]I_pu₁cosβ−u₁sinβ([(2cosβ)/(R²)]I_xx+[(2bsinβ)/(R²l)]).

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