9  Przykład 4. Manipulator typu podwójne wahadło zamontowany na monocyklu

Omawiany manipulator mobilny złożony jest z manipulatora typu 2R oraz platformy mobilnej klasy (2,0). Zgodnie z podziałem manipulatorów mobilnych przedstawionym we wstępie jest typu (nh,h), gdyż manipulator jest układem holonomicznym, który posiada pewne ograniczenia konfiguracyjne, uniemożliwiające osiągnięcie każdej orientacji. Natomiast platforma mobilna jest układem nieholonomicznym, którego ograniczenia nie zmiejszają osiągalności konfiguracji, jedynie utrudnieniu może ulec sposób osiągania pewnych konfiguracji [].

9.1  Kinematyka we współrzędnych przegubowych

Manipulator jest opisywany współrzędnymi przegubowymi:

x = ⎛ ⎜ ⎜ ⎝ x1 x2.  ⎞ ⎟ ⎟ ⎠ .

(133)

Omawiany manipulator posiada tylko 2 przeguby obrotowe, zatem posiada 2 stopnie swobody (Rysunek ).

Manipulator został umieszczony na wysięgniku o długości l₀ i wysunięty na odległość d do przodu względem środka platformy (Rysunek ).
Figure Figure 5: Manipulator typu podwójne wachadło zamontowany na monocyklu
Kinematykę manipulatora pokładowego wyznaczamy na podstawie algorytmu Deneavita-Hartenberga podanego w rodziale 1.1. Postępując zgonie z algorytmem DH z każdym układem wiążemy lokalny układ współrzędnych i na jego podstawie określamy transformacje z jednego układu do drugiego. Figure Figure 6: Schemat kinematyczny manipulatora

Poszczególne transformacje z lokalnych układów współrzędnych wyznaczone na podstawie rysunku 6.

A₀¹(x) = Trans(Z,l₀)Rot(X, π/2), A₁²(x) = Rot(Z,x₁)Trans(X,l₁), A₂³(x) = Rot(Z,x₂)Trans(X,l₂),
Kinematyka manipulatora jest wyznaczana poprzez złożenie transformacji.

K_m(x) = A₀¹(x) ·A₁²(x) ·A₂³(x) = Trans(Z,l₀)Rot(X, π/2)Rot(Z,x₁)Trans(X,l₁)Rot(Z,x₂)Trans(X,l₂)

A01(q) = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ 1 0 0 0 0 1 0 0 0 0 1 l0 0 0 0 1 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ · ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ 1 0 0 0 0 0 −1 0 0 1 0 0 0 0 0 1 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ 1 0 0 0 0 0 −1 0 0 1 0 l0 0 0 0 1 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦

(134)

A12(q) = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ cos(x1)   −sin(x1) 0 0 sin(x1)   cos(x1) 0 0 0 0 1 0 0 0 0 1 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ · ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ 1 0 0 l1 0 1 0 0 0 0 1 0 0 0 0 1 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ cos(x1)   −sin(x1) 0   cos(x1) ·l1 sin(x1)  cos(x1) 0   sin(x1) ·l1 0 0 1 0 0 0 0 1 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦

(135)

A23(q) = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ cos(x2)   −sin(x2) 0 0 sin(x2)   cos(x2) 0 0 0 0 1 0 0 0 0 1 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ · ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ 1 0 0 l2 0 1 0 0 0 0 1 0 0 0 0 1 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ cos(x2) −sin(x2) 0   cos(x2) ·l2 sin(x2) cos(x2) 0   sin(x2) ·l2 0 0 1 0 0 0 0 1 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦

(136)

Km(q) = A01(q) ·A12(q) ·A23(q) = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ cos(x1 + x2)   −sin(x1 + x2) 0   cos(x1 + x2) ·l2 + cos(x1) ·l1 0 0 −1 0 sin(x1 + x2)   cos(x1 + x2) 0   sin(x1 + x2) ·l2 + sin(x1) ·l1+ l0 0 0 0 1 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦

(137)

9.2  Kinematyka we współrzędnych zadaniowych

Położenie i orientacja platformy mobilnej klasy (2,0), czyli monocyklu, jest opisywana współrzędnymi uogólnionymi:

q = ⎛ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎝ q1 q2 q3 φ1 φ2 ⎞ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎠ .

(138)

Współrzędnymi niezbędnymi do wyznaczania położenia i orientacji są q₁, q₂, q₃, natomiast φ₁ i φ₂ są potrzebne do sterowania platformą mobilną.

Kinematyka manipulatora mobilnego jest wyznaczana poprzez złożenie transformacji związanych z kinematyką manipulatora pokładowego oraz kinematyką platformy.

K(q,x) = K_p(q)K_m(x)

Transformacje wyznaczające położenie platformy mobilnej względem bazowego układu współrzędnych zgodnie z rysunkiem :

K_p(q) = Trabs(X,q₁)Trans(Y,q₂)Rot(Z,q₃)Trans(X,d).

Figure Figure 7: Monocykl

Kp(q) = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ 1 0 0 q1 0 1 0 0 0 0 1 0 0 0 0 1 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ · ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ 1 0 0 0 0 1 0 q2 0 0 1 0 0 0 0 1 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ · ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ cos(q3)   −sin(q3) 0 0 sin(q3)   cos(q3) 0 0 0 0 1 0 0 0 0 1 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ · ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ 1 0 0 d 0 1 0 0 0 0 1 0 0 0 0 1 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ =

= ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ cos(q3)   −sin(q3) 0   cos(q3) ·d+q1 sin(q3)  cos(q3) 0   sin(q3) ·d+q2 0 0 1 0 0 0 0 1 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦

(139)

Kinamatyka manipulatora mobilnego wyznaczama przy pomocy wcześniej wyznaczonych macierzy 137 i 139 :

K(q) = K_p(q) ·K_m(q) =

= ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ cos(q3) cos(x1+x2)   −cos(q3) sin(x1 + x2)  sin(q3)   cos(q3) (cos(x1 + x2) ·l2 + cos(x1) ·l1)+cos(q3) d+q1 sin(q3) cos(x1+x2)   −sin(q3) sin(x1 + x2)  −cos(q3)   sin(q3) (cos(x1 + x2) ·l2 + cos(x1) ·l1)+sin(q3) d+q2 sin(x1 + x2)   cos(x1 + x2) 0   sin(x1 + x2) ·l2 + sin(x1) ·l1 + l0 0 0 0 1 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦

(140)

Funkcja wyjścia opisująca współrzędne kartezjańskie efektora w przestrzeni zadaniowej.

y = k(q,x) = ⎛ ⎜ ⎜ ⎜ ⎝ cos(q3) (cos(x1 + x2) ·l2 + cos(x1) ·l1)+cos(q3) d+q1 sin(q3) (cos(x1 + x2) ·l2 + cos(x1) ·l1)+sin(q3) d+q2 sin(x1 + x2) ·l2 + sin(x1) ·l1 + l0 ⎞ ⎟ ⎟ ⎟ ⎠

(141)

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

Kinematyka platformy mobilnej zostanie opisana za pomocą trzech niezależnych ograniczeń nieholonomicznych. Zadaniem platformy będzie poruszanie się w pewnym otoczeniu, jednakże na ruch platformy nałożone zostaną ograniczenia fazowe: brak poślizgu poprzecznego tj. brak ruchu prostopadłego do płaszczyzny kół oraz brak poślizgu wzdłużnego, tj. brak buksowania kół.

Współrzędne uogólnione:

q = ⎛ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎝ q1 q2 q3 φ1 φ2 ⎞ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎠ .

(142)

Brak poślizgu poprzecznego:
· q₁ ·sin(q₃) − · q₁ ·cos(q₃) = 0

Brak poślizgu wzdłużnego:

• koło A
  · x_A ·cos(q₃) + · y_A ·sin(q₃) − r ·· φ₁ = 0 ,
  
  x_A = q₁− l sin(q₃),     · x_A = · q₁− l · q₃cos(q₃),
  y_A = q₂+ l cos(q₃),     · y_A = · q₂− l · q₃sin(q₃),
  
  (· q₁ − l · q₃cos(q₃))cos(q₃) + (· q₂ − l · q₃sin(q₃))sin(q₃) − r ·· φ₁ = 0,
  · q₁ cos(q₃)+· q₂ sin(q₃) − l · q₃ − r ·· φ₁ = 0.
  
• koło B
  · x_B ·cos(q₃) + · y_B ·sin(q₃) − r ·· φ₂ = 0 ,
  
  x_B = q₁+ l sin(q₃),     · x_B = · q₁+ l · q₃cos(q₃),
  y_B = q₂− l cos(q₃),     · y_B = · q₂+ l · q₃sin(q₃),
  
  (· q₁ − l · q₃cos(q₃))cos(q₃) + (· q₂ + l · q₃sin(q₃))sin(q₃) − r ·· φ₂ = 0,
  · q₁ cos(q₃)+· q₂ sin(q₃) + l · q₃ − r ·· φ₂ = 0.
  

Trzy niezależne ograniczenia nieholonomiczne:
· q₁ ·sin(q₃) − · q₁ ·cos(q₃) = 0 , · q₁ cos(q₃)+· q₂ sin(q₃) − l · q₃ − r ·· φ₁ = 0,
· q₁ cos(q₃)+· q₂ sin(q₃) + l · q₃ − r ·· φ₂ = 0.


Ograniczenia są stałe w czasie, dlatego można je wyrazić w tzw. postaci Pfaffa A(q) · q = 0 .

A(q) · ⋅ q   = ⎡ ⎢ ⎢ ⎢ ⎣ sin(q3) −cos(q3) 0 0 0 cos(q3) sin(q3) −l − r 0 cos(q3) sin(q3) l 0 − r ⎤ ⎥ ⎥ ⎥ ⎦ · ⎛ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎝ ⋅ q1   ⋅ q2   ⋅ q3   ⋅ φ1   ⋅ φ2   ⎞ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎠

(143)

9.4  Kinematyka we współrzędnych pomocniczych

Wyznaczenie bezdryfowowego układu sterowania (9) zgodnie z własnościami podanymi w rozdziale 1.4
m = n − l = 5 − 3 = 2
n - wymiar przestrzeni sterowań, l - liczba ograniczeń niehonolomicznych
Należy znaleźć takie g_i , że A ·g_i = 0

g1 = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ l ·cos(q3) l ·sin(q3) −1 2l r 0 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦                g2 = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ l ·cos(q3) l ·sin(q3) 1 0 2l r ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦

(144)

G(q)u = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ l ·cos(q3) l ·cos(q3) l ·sin(q3) l ·sin(q3) −1 1 2l r 0 0 2l r ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ · ⎡ ⎢ ⎢ ⎣ u1 u2 ⎤ ⎥ ⎥ ⎦

(145)

⎧ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎩ ⋅ q1   = l ·cos(q3) ·u1 + l ·cos(q3) ·u2 ⋅ q2   = r ·sin(q3) ·u1 + r ·sin(q3) ·u1 ⋅ q2   =u2 − u1 ⋅ φ1   = 2l r u1 ⋅ φ2   = 2l r u2

Sterowanie u₁ jest to sterowanie prędkością koła A, natomiast u₂ prędkością koła B.

9.5  Przybliżenie liniowe bezdryfowego układu sterowania

Wreszcie kinematykę manipulatora mobilnego można zapisać w postaci bezdryfowego układu sterowania z funkcją wyjścia (12).

l (q_3) l (q_3) l (q_3) l (q_3) 2l 2l

---

File translated from T_EX by T_TH, version 3.87.
On 23 May 2010, 16:23.

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